feat:support Tele

increase frequency of polling the receive queue

README.md

@@ -268,7 +268,13 @@ livox_ros_driver supports the conversion of lvx pointcloud data files to rosbag
 
 After replacing "/home/livox/test.lvx" in the above command with the local lvx data file path, you can simply run it; if the conversion is successful, a rosbag format file with the same name will be generated under the above path.
 
-## 8. Support
+## 8. Application Documents
+
+* [How to use lvx file in ros](https://github.com/Livox-SDK/Livox-SDK/wiki/How-to-use-lvx-file-under-ros)
+* [Set publish frequency](https://github.com/Livox-SDK/Livox-SDK/wiki/Set-publish-frequency)
+* [外参标定与点云显示](https://github.com/Livox-SDK/Livox-SDK/wiki/Calibrate-extrinsic-and-display-under-ros-cn)
+
+## 9. Support
 
 You can get support from Livox with the following methods :
 

README_CN.md

@@ -278,7 +278,13 @@ uint8 line              # laser number in lidar
 
 替换如上命令中的 "/home/livox/test.lvx"  为本地 lvx 数据文件路径后，直接运行即可；如果转换成功，将会在上述路径下产生同名 rosbag 格式点云数据文件。
 
-## 8. 支持
+## 8. 应用文档
+
+* [How to use lvx file in ros](https://github.com/Livox-SDK/Livox-SDK/wiki/How-to-use-lvx-file-under-ros)
+* [Set publish frequency](https://github.com/Livox-SDK/Livox-SDK/wiki/Set-publish-frequency)
+* [外参标定与点云显示](https://github.com/Livox-SDK/Livox-SDK/wiki/Calibrate-extrinsic-and-display-under-ros-cn)
+
+## 9. 支持
 
 你可以通过以下方式获取 Livox 的技术支持 :
 

livox_ros_driver/config/display_hub_points.rviz

@@ -9,8 +9,8 @@ Panels:
         - /Grid1
         - /PointCloud1/Autocompute Value Bounds1
         - /PointCloud21
-      Splitter Ratio: 0.500694990158081
+      Splitter Ratio: 0.500695
-    Tree Height: 728
+    Tree Height: 680
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -19,7 +19,7 @@ Panels:
       - /2D Nav Goal1
       - /Publish Point1
     Name: Tool Properties
-    Splitter Ratio: 0.5886790156364441
+    Splitter Ratio: 0.588679
   - Class: rviz/Views
     Expanded:
       - /Current View1
@@ -30,10 +30,6 @@ Panels:
     Name: Time
     SyncMode: 0
     SyncSource: PointCloud2
-Preferences:
-  PromptSaveOnExit: true
-Toolbars:
-  toolButtonStyle: 2
 Visualization Manager:
   Class: ""
   Displays:
@@ -43,7 +39,7 @@ Visualization Manager:
       Color: 160; 160; 164
       Enabled: true
       Line Style:
-        Line Width: 0.029999999329447746
+        Line Width: 0.03
         Value: Lines
       Name: Grid
       Normal Cell Count: 0
@@ -58,8 +54,8 @@ Visualization Manager:
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
-        Max Value: 0.8560000061988831
+        Max Value: 0.856
-        Min Value: -0.7350000143051147
+        Min Value: -0.735
         Value: true
       Axis: Z
       Channel Name: x
@@ -70,15 +66,15 @@ Visualization Manager:
       Enabled: false
       Invert Rainbow: false
       Max Color: 255; 255; 255
-      Max Intensity: -0.08799999952316284
+      Max Intensity: -0.088
       Min Color: 0; 0; 0
-      Min Intensity: -1.9509999752044678
+      Min Intensity: -1.951
       Name: PointCloud
       Position Transformer: XYZ
       Queue Size: 1000
       Selectable: true
       Size (Pixels): 2
-      Size (m): 0.004999999888241291
+      Size (m): 0.005
       Style: Flat Squares
       Topic: /livox/lidar
       Unreliable: false
@@ -88,8 +84,8 @@ Visualization Manager:
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
-        Max Value: 0.8159999847412109
+        Max Value: 0.816
-        Min Value: -0.6740000247955322
+        Min Value: -0.674
         Value: true
       Axis: Z
       Channel Name: intensity
@@ -108,7 +104,7 @@ Visualization Manager:
       Queue Size: 10
       Selectable: true
       Size (Pixels): 2
-      Size (m): 0.004999999888241291
+      Size (m): 0.005
       Style: Points
       Topic: /livox/lidar
       Unreliable: false
@@ -118,7 +114,6 @@ Visualization Manager:
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
-    Default Light: true
     Fixed Frame: livox_frame
     Frame Rate: 50
   Name: root
@@ -130,10 +125,7 @@ Visualization Manager:
     - Class: rviz/FocusCamera
     - Class: rviz/Measure
     - Class: rviz/SetInitialPose
-      Theta std deviation: 0.2617993950843811
       Topic: /initialpose
-      X std deviation: 0.5
-      Y std deviation: 0.5
     - Class: rviz/SetGoal
       Topic: /move_base_simple/goal
     - Class: rviz/PublishPoint
@@ -143,30 +135,27 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 29.202434539794922
+      Distance: 5.292
       Enable Stereo Rendering:
-        Stereo Eye Separation: 0.05999999865889549
+        Stereo Eye Separation: 0.06
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 0.2672550082206726
+        X: 0.267255
-        Y: 0.061853598803281784
+        Y: 0.0618536
-        Z: 0.15087400376796722
+        Z: 0.150874
-      Focal Shape Fixed Size: true
-      Focal Shape Size: 0.05000000074505806
-      Invert Z Axis: false
       Name: Current View
-      Near Clip Distance: 0.009999999776482582
+      Near Clip Distance: 0.01
-      Pitch: 0.679796040058136
+      Pitch: 0.209796
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 3.0174102783203125
+      Yaw: 3.21241
     Saved:
       - Class: rviz/Orbit
         Distance: 10
         Enable Stereo Rendering:
-          Stereo Eye Separation: 0.05999999865889549
+          Stereo Eye Separation: 0.06
           Stereo Focal Distance: 1
           Swap Stereo Eyes: false
           Value: false
@@ -174,22 +163,19 @@ Visualization Manager:
           X: 0
           Y: 0
           Z: 0
-        Focal Shape Fixed Size: true
-        Focal Shape Size: 0.05000000074505806
-        Invert Z Axis: false
         Name: Orbit
-        Near Clip Distance: 0.009999999776482582
+        Near Clip Distance: 0.01
-        Pitch: 1.1103999614715576
+        Pitch: 1.1104
         Target Frame: <Fixed Frame>
         Value: Orbit (rviz)
-        Yaw: 0.5703970193862915
+        Yaw: 0.570397
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1025
+  Height: 961
   Hide Left Dock: false
   Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd0000000400000000000001a900000363fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000363000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000058e0000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000001a900000337fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000337000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002f600fffffffb0000000800540069006d006501000000000000045000000000000000000000058e0000033700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -199,5 +185,5 @@ Window Geometry:
   Views:
     collapsed: true
   Width: 1853
-  X: 67
+  X: 124
-  Y: 27
+  Y: 81

livox_ros_driver/config/livox_lidar_config.json

@@ -6,8 +6,9 @@
             "enable_fan": true,
             "return_mode": 0,
             "coordinate": 0,
-            "imu_rate": 1,
+            "imu_rate": 0,
-            "extrinsic_parameter_source": 0
+            "extrinsic_parameter_source": 0,
+            "enable_high_sensitivity": false
         },
         {
             "broadcast_code": "0TFDG3U99101431",
@@ -15,8 +16,9 @@
             "enable_fan": true,
             "return_mode": 0,
             "coordinate": 0,
-            "imu_rate": 1,
+            "imu_rate": 0,
-            "extrinsic_parameter_source": 0
+            "extrinsic_parameter_source": 0,
+            "enable_high_sensitivity": false
         }
     ],
 	

livox_ros_driver/launch/livox_hub.launch

@@ -10,6 +10,7 @@
 	<arg name="rviz_enable" default="false"/>
 	<arg name="rosbag_enable" default="false"/>
 	<arg name="cmdline_arg" default="$(arg bd_list)"/>
+	<arg name="msg_frame_id" default="livox_frame"/>
 
 	<param name="xfer_format" value="$(arg xfer_format)"/>
 	<param name="multi_topic" value="$(arg multi_topic)"/>
@@ -19,6 +20,7 @@
 	<param name="cmdline_str" type="string" value="$(arg bd_list)"/>
 	<param name="cmdline_file_path" type="string" value="$(arg lvx_file_path)"/>
 	<param name="user_config_path" type="string" value="$(find livox_ros_driver)/config/livox_hub_config.json"/>
+	<param name="frame_id" type="string" value="$(arg msg_frame_id)"/>
 
 	<node name="livox_lidar_publisher" pkg="livox_ros_driver"
 	      type="livox_ros_driver_node" required="true"

livox_ros_driver/launch/livox_hub_msg.launch

@@ -10,6 +10,7 @@
 	<arg name="rviz_enable" default="false"/>
 	<arg name="rosbag_enable" default="false"/>
 	<arg name="cmdline_arg" default="$(arg bd_list)"/>
+	<arg name="msg_frame_id" default="livox_frame"/>
 
 	<param name="xfer_format" value="$(arg xfer_format)"/>
 	<param name="multi_topic" value="$(arg multi_topic)"/>
@@ -19,6 +20,7 @@
 	<param name="cmdline_str" type="string" value="$(arg bd_list)"/>
 	<param name="cmdline_file_path" type="string" value="$(arg lvx_file_path)"/>
 	<param name="user_config_path" type="string" value="$(find livox_ros_driver)/config/livox_hub_config.json"/>
+	<param name="frame_id" type="string" value="$(arg msg_frame_id)"/>
 
 	<node name="livox_lidar_publisher" pkg="livox_ros_driver"
 	      type="livox_ros_driver_node" required="true"

livox_ros_driver/launch/livox_hub_rviz.launch

@@ -10,6 +10,7 @@
 	<arg name="rviz_enable" default="true"/>
 	<arg name="rosbag_enable" default="false"/>
 	<arg name="cmdline_arg" default="$(arg bd_list)"/>
+	<arg name="msg_frame_id" default="livox_frame"/>
 
 	<param name="xfer_format" value="$(arg xfer_format)"/>
 	<param name="multi_topic" value="$(arg multi_topic)"/>
@@ -19,6 +20,7 @@
 	<param name="cmdline_str" type="string" value="$(arg bd_list)"/>
 	<param name="cmdline_file_path" type="string" value="$(arg lvx_file_path)"/>
 	<param name="user_config_path" type="string" value="$(find livox_ros_driver)/config/livox_hub_config.json"/>
+    <param name="frame_id" type="string" value="$(arg msg_frame_id)"/>
 
 	<node name="livox_lidar_publisher" pkg="livox_ros_driver"
 	      type="livox_ros_driver_node" required="true"

livox_ros_driver/launch/livox_lidar.launch

@@ -10,6 +10,7 @@
 	<arg name="rviz_enable" default="false"/>
 	<arg name="rosbag_enable" default="false"/>
 	<arg name="cmdline_arg" default="$(arg bd_list)"/>
+	<arg name="msg_frame_id" default="livox_frame"/>
 
 	<param name="xfer_format" value="$(arg xfer_format)"/>
 	<param name="multi_topic" value="$(arg multi_topic)"/>
@@ -19,6 +20,7 @@
 	<param name="cmdline_str" type="string" value="$(arg bd_list)"/>
 	<param name="cmdline_file_path" type="string" value="$(arg lvx_file_path)"/>
 	<param name="user_config_path" type="string" value="$(find livox_ros_driver)/config/livox_lidar_config.json"/>
+	<param name="frame_id" type="string" value="$(arg msg_frame_id)"/>
 	
 	<node name="livox_lidar_publisher" pkg="livox_ros_driver"
 	      type="livox_ros_driver_node" required="true"

livox_ros_driver/launch/livox_lidar_msg.launch

@@ -10,6 +10,7 @@
 	<arg name="rviz_enable" default="false"/>
 	<arg name="rosbag_enable" default="false"/>
 	<arg name="cmdline_arg" default="$(arg bd_list)"/>
+	<arg name="msg_frame_id" default="livox_frame"/>
 
 	<param name="xfer_format" value="$(arg xfer_format)"/>
 	<param name="multi_topic" value="$(arg multi_topic)"/>
@@ -19,6 +20,7 @@
 	<param name="cmdline_str" type="string" value="$(arg bd_list)"/>
 	<param name="cmdline_file_path" type="string" value="$(arg lvx_file_path)"/>
 	<param name="user_config_path" type="string" value="$(find livox_ros_driver)/config/livox_lidar_config.json"/>
+	<param name="frame_id" type="string" value="$(arg msg_frame_id)"/>
 	
 	<node name="livox_lidar_publisher" pkg="livox_ros_driver"
 	      type="livox_ros_driver_node" required="true"

livox_ros_driver/launch/livox_lidar_rviz.launch

@@ -10,6 +10,7 @@
 	<arg name="rviz_enable" default="true"/>
 	<arg name="rosbag_enable" default="false"/>
 	<arg name="cmdline_arg" default="$(arg bd_list)"/>
+	<arg name="msg_frame_id" default="livox_frame"/>
 
 	<param name="xfer_format" value="$(arg xfer_format)"/>
 	<param name="multi_topic" value="$(arg multi_topic)"/>
@@ -19,6 +20,7 @@
 	<param name="cmdline_str" type="string" value="$(arg bd_list)"/>
 	<param name="cmdline_file_path" type="string" value="$(arg lvx_file_path)"/>
 	<param name="user_config_path" type="string" value="$(find livox_ros_driver)/config/livox_lidar_config.json"/>	
+	<param name="frame_id" type="string" value="$(arg msg_frame_id)"/>
 	
 	<node name="livox_lidar_publisher" pkg="livox_ros_driver"
 	      type="livox_ros_driver_node" required="true"

livox_ros_driver/launch/livox_template.launch

@@ -10,6 +10,7 @@
 	<arg name="rviz_enable" default="false"/>
 	<arg name="rosbag_enable" default="false"/>
 	<arg name="cmdline_arg" default="$(arg bd_list)"/>
+	<arg name="msg_frame_id" default="livox_frame"/>
 
 	<param name="xfer_format" value="$(arg xfer_format)"/>
 	<param name="multi_topic" value="$(arg multi_topic)"/>
@@ -19,8 +20,9 @@
 	<param name="cmdline_str" type="string" value="$(arg bd_list)"/>
 	<param name="cmdline_file_path" type="string" value="$(arg lvx_file_path)"/>
 	<param name="user_config_path" type="string" value="$(find livox_ros_driver)/config/livox_lidar_config.json"/>
+	<param name="frame_id" type="string" value="$(arg msg_frame_id)"/>
 
-	<node name="livox_lidar_publisher" pkg="livox_coat"
+	<node name="livox_lidar_publisher" pkg="livox_ros_driver"
 	      type="livox_ros_driver_node" required="true"
 	      output="screen" args="$(arg cmdline_arg)"/>
 

livox_ros_driver/launch/lvx_to_rosbag.launch

@@ -10,6 +10,7 @@
 	<arg name="rviz_enable" default="false"/>
 	<arg name="rosbag_enable" default="false"/>
 	<arg name="cmdline_arg" default="$(arg bd_list)"/>
+	<arg name="msg_frame_id" default="livox_frame"/>
 
 	<param name="xfer_format" value="$(arg xfer_format)"/>
 	<param name="multi_topic" value="$(arg multi_topic)"/>
@@ -18,6 +19,8 @@
 	<param name="output_data_type" value="$(arg output_type)"/>
 	<param name="cmdline_str" type="string" value="$(arg bd_list)"/>
 	<param name="cmdline_file_path" type="string" value="$(arg lvx_file_path)"/>
+	<param name="user_config_path" type="string" value="$(find livox_ros_driver)/config/livox_hub_config.json"/>
+	<param name="frame_id" type="string" value="$(arg msg_frame_id)"/>
 	
 	<node name="livox_lidar_publisher" pkg="livox_ros_driver"
 	      type="livox_ros_driver_node" required="true"

livox_ros_driver/launch/lvx_to_rosbag_rviz.launch

@@ -10,6 +10,7 @@
 	<arg name="rviz_enable" default="true"/>
 	<arg name="rosbag_enable" default="true"/>
 	<arg name="cmdline_arg" default="$(arg bd_list)"/>
+	<arg name="msg_frame_id" default="livox_frame"/>
 
 	<param name="xfer_format" value="$(arg xfer_format)"/>
 	<param name="multi_topic" value="$(arg multi_topic)"/>
@@ -18,6 +19,8 @@
 	<param name="output_data_type" value="$(arg output_type)"/>
 	<param name="cmdline_str" type="string" value="$(arg bd_list)"/>
 	<param name="cmdline_file_path" type="string" value="$(arg lvx_file_path)"/>
+	<param name="user_config_path" type="string" value="$(find livox_ros_driver)/config/livox_hub_config.json"/>
+	<param name="frame_id" type="string" value="$(arg msg_frame_id)"/>
 	
 	<node name="livox_lidar_publisher" pkg="livox_ros_driver"
 	      type="livox_ros_driver_node" required="true"

livox_ros_driver/livox_ros_driver/include/livox_ros_driver.h

@@ -26,8 +26,8 @@
 #define LIVOX_ROS_DRIVER_INClUDE_LIVOX_ROS_DRIVER_H_
 
 #define LIVOX_ROS_DRIVER_VER_MAJOR 2
-#define LIVOX_ROS_DRIVER_VER_MINOR 0
+#define LIVOX_ROS_DRIVER_VER_MINOR 5
-#define LIVOX_ROS_DRIVER_VER_PATCH 1
+#define LIVOX_ROS_DRIVER_VER_PATCH 0
 
 #define GET_STRING(n) GET_STRING_DIRECT(n)
 #define GET_STRING_DIRECT(n) #n

livox_ros_driver/livox_ros_driver/lddc.cpp

@@ -43,14 +43,13 @@ namespace livox_ros {
 
 typedef pcl::PointCloud<pcl::PointXYZI> PointCloud;
 
-/** Lidar Data Distribute Control
+/** Lidar Data Distribute Control--------------------------------------------*/
- * ----------------------------------------------------------------*/
 Lddc::Lddc(int format, int multi_topic, int data_src, int output_type,
-           double frq)
+           double frq, std::string &frame_id)
     : transfer_format_(format), use_multi_topic_(multi_topic),
-      data_src_(data_src), output_type_(output_type), publish_frq_(frq) {
+      data_src_(data_src), output_type_(output_type), publish_frq_(frq), \
-
+      frame_id_(frame_id) {
-  publish_interval_ms_ = 1000 / publish_frq_;
+  publish_period_ns_ = kNsPerSecond / publish_frq_;
   lds_ = nullptr;
   memset(private_pub_, 0, sizeof(private_pub_));
   memset(private_imu_pub_, 0, sizeof(private_imu_pub_));
@@ -62,7 +61,6 @@ Lddc::Lddc(int format, int multi_topic, int data_src, int output_type,
 
 Lddc::~Lddc() {
 
-  printf("lddc exit\n\n\n\n");
   if (global_pub_) {
     delete global_pub_;
   }
@@ -88,17 +86,64 @@ Lddc::~Lddc() {
   }
 }
 
+int32_t Lddc::GetPublishStartTime(LidarDevice *lidar, LidarDataQueue *queue,
+    uint64_t *start_time,StoragePacket *storage_packet) {
+  QueueProPop(queue, storage_packet);
+  uint64_t timestamp = GetStoragePacketTimestamp(storage_packet, 
+                                                 lidar->data_src);
+  uint32_t remaining_time = timestamp % publish_period_ns_;
+  uint32_t diff_time = publish_period_ns_ - remaining_time;
+  /** Get start time, down to the period boundary */
+  if (diff_time > (publish_period_ns_ / 4)) {
+    //ROS_INFO("0 : %u", diff_time);
+    *start_time = timestamp - remaining_time;
+    return 0;
+  } else if (diff_time <= lidar->packet_interval_max) {
+    *start_time = timestamp;
+    return 0;
+  } else {
+    /** Skip some packets up to the period boundary*/ 
+    //ROS_INFO("2 : %u", diff_time);
+    do {
+      if (QueueIsEmpty(queue)) {
+        break;
+      }
+      QueuePopUpdate(queue); /* skip packet */      
+      QueueProPop(queue, storage_packet);
+      uint32_t last_remaning_time = remaining_time;
+      timestamp = GetStoragePacketTimestamp(storage_packet, 
+                                            lidar->data_src);
+      remaining_time = timestamp % publish_period_ns_;
+      /** Flip to another period */
+      if (last_remaning_time > remaining_time) {
+        //ROS_INFO("Flip to another period, exit");
+        break;
+      }
+      diff_time = publish_period_ns_ - remaining_time;
+    } while (diff_time > lidar->packet_interval);
+      
+    /* the remaning packets in queue maybe not enough after skip */
+    return -1;
+  }
+}
+
 uint32_t Lddc::PublishPointcloud2(LidarDataQueue *queue, uint32_t packet_num,
                                   uint8_t handle) {
   uint64_t timestamp = 0;
   uint64_t last_timestamp = 0;
   uint32_t published_packet = 0;
+
+  StoragePacket storage_packet;
+  LidarDevice *lidar  = &lds_->lidars_[handle];
+  if (GetPublishStartTime(lidar, queue, &last_timestamp, &storage_packet)) {
+    /* the remaning packets in queue maybe not enough after skip */
+    return 0;
+  }
+  
   sensor_msgs::PointCloud2 cloud;
-
+  cloud.header.frame_id.assign(frame_id_);
-  cloud.header.frame_id = "livox_frame";
   cloud.height = 1;
-  cloud.width = 0;
+  cloud.width  = 0;
-
   cloud.fields.resize(6);
   cloud.fields[0].offset = 0;
   cloud.fields[0].name = "x";
@@ -129,57 +174,48 @@ uint32_t Lddc::PublishPointcloud2(LidarDataQueue *queue, uint32_t packet_num,
                     sizeof(LivoxPointXyzrtl));
   cloud.point_step = sizeof(LivoxPointXyzrtl);
   uint8_t *point_base = cloud.data.data();
-  uint8_t data_source = lds_->lidars_[handle].data_src;
+  uint8_t data_source = lidar->data_src;
-  StoragePacket storage_packet;
+  while ((published_packet < packet_num) && !QueueIsEmpty(queue)) {
-  while (published_packet < packet_num) {
     QueueProPop(queue, &storage_packet);
     LivoxEthPacket *raw_packet =
         reinterpret_cast<LivoxEthPacket *>(storage_packet.raw_data);
-
-    uint32_t packet_interval = GetPacketInterval(raw_packet->data_type);
-    int64_t packet_loss_threshold_lower = packet_interval + packet_interval / 2;
     timestamp = GetStoragePacketTimestamp(&storage_packet, data_source);
     int64_t packet_gap = timestamp - last_timestamp;
-    if (published_packet && (packet_gap > packet_loss_threshold_lower) &&
+    if ((packet_gap > lidar->packet_interval_max) && 
-        lds_->lidars_[handle].data_is_pubulished) {
+        lidar->data_is_pubulished) {
-      ROS_INFO("Lidar[%d] packet loss, interval is %ldus", handle, packet_gap);
+      //ROS_INFO("Lidar[%d] packet time interval is %ldns", handle, packet_gap);
       if (kSourceLvxFile != data_source) {
-        // ROS_INFO("Lidar[%d] packet loss %ld %d %d", handle,
-        // packet_loss_threshold_lower, packet_interval, raw_packet->data_type);
-        int64_t packet_loss_threshold_upper = packet_interval * packet_num;
-        if (packet_gap >
-            packet_loss_threshold_upper) { // skip when gap is too large
-          break;
-        }
         point_base = FillZeroPointXyzrtl(point_base, storage_packet.point_num);
         cloud.width += storage_packet.point_num;
-        last_timestamp = last_timestamp + packet_interval;
+        last_timestamp = last_timestamp + lidar->packet_interval;
+        if (!published_packet) { 
+          cloud.header.stamp = ros::Time(last_timestamp / 1000000000.0);
+        }
         ++published_packet;
         continue;
       }
     }
-
+    /** Use the first packet timestamp as pointcloud2 msg timestamp */
-    if (!published_packet) { // use the first packet timestamp as pointcloud2
+    if (!published_packet) { 
-                             // msg timestamp
       cloud.header.stamp = ros::Time(timestamp / 1000000000.0);
     }
     cloud.width += storage_packet.point_num;
 
     if (kSourceLvxFile != data_source) {
       PointConvertHandler pf_point_convert =
-          GetConvertHandler(raw_packet->data_type);
+          GetConvertHandler(lidar->raw_data_type);
       if (pf_point_convert) {
         point_base = pf_point_convert(
-            point_base, raw_packet, lds_->lidars_[handle].extrinsic_parameter);
+            point_base, raw_packet, lidar->extrinsic_parameter);
       } else {
-        /* Skip the packet */
+        /** Skip the packet */
         ROS_INFO("Lidar[%d] unkown packet type[%d]", handle,
                  raw_packet->data_type);
         break;
       }
     } else {
       point_base = LivoxPointToPxyzrtl(
-          point_base, raw_packet, lds_->lidars_[handle].extrinsic_parameter);
+          point_base, raw_packet, lidar->extrinsic_parameter);
     }
 
     QueuePopUpdate(queue);
@@ -190,7 +226,7 @@ uint32_t Lddc::PublishPointcloud2(LidarDataQueue *queue, uint32_t packet_num,
   cloud.row_step = cloud.width * cloud.point_step;
   cloud.is_bigendian = false;
   cloud.is_dense = true;
-  cloud.data.resize(cloud.row_step); // adjust to the real size
+  cloud.data.resize(cloud.row_step); /** Adjust to the real size */
 
   ros::Publisher *p_publisher = Lddc::GetCurrentPublisher(handle);
   if (kOutputToRos == output_type_) {
@@ -202,8 +238,8 @@ uint32_t Lddc::PublishPointcloud2(LidarDataQueue *queue, uint32_t packet_num,
     }
   }
 
-  if (!lds_->lidars_[handle].data_is_pubulished) {
+  if (!lidar->data_is_pubulished) {
-    lds_->lidars_[handle].data_is_pubulished = true;
+    lidar->data_is_pubulished = true;
   }
 
   return published_packet;
@@ -216,37 +252,38 @@ uint32_t Lddc::PublishPointcloudData(LidarDataQueue *queue, uint32_t packet_num,
   uint64_t last_timestamp = 0;
   uint32_t published_packet = 0;
 
+  StoragePacket storage_packet;
+  LidarDevice *lidar  = &lds_->lidars_[handle];
+  if (GetPublishStartTime(lidar, queue, &last_timestamp, &storage_packet)) {
+    /* the remaning packets in queue maybe not enough after skip */
+    return 0;
+  }
+
   /* init point cloud data struct */
   PointCloud::Ptr cloud(new PointCloud);
-  cloud->header.frame_id = "livox_frame";
+  cloud->header.frame_id.assign(frame_id_);
-  // cloud->header.stamp = ros::Time::now();
+  /* cloud->header.stamp = ros::Time::now(); */
   cloud->height = 1;
-  cloud->width = 0;
+  cloud->width  = 0;
 
-  uint8_t data_source = lds_->lidars_[handle].data_src;
+  uint8_t data_source = lidar->data_src;
-  StoragePacket storage_packet;
+  while ((published_packet < packet_num) && !QueueIsEmpty(queue)) {
-  while (published_packet < packet_num) {
     QueueProPop(queue, &storage_packet);
     LivoxEthPacket *raw_packet =
         reinterpret_cast<LivoxEthPacket *>(storage_packet.raw_data);
-
-    uint32_t packet_interval = GetPacketInterval(raw_packet->data_type);
-    int64_t packet_loss_threshold_lower = packet_interval + packet_interval / 2;
     timestamp = GetStoragePacketTimestamp(&storage_packet, data_source);
     int64_t packet_gap = timestamp - last_timestamp;
-    if ((packet_gap > packet_loss_threshold_lower) && published_packet &&
+    if ((packet_gap > lidar->packet_interval_max) &&
-        lds_->lidars_[handle].data_is_pubulished) {
+        lidar->data_is_pubulished) {
-      ROS_INFO("Lidar[%d] packet loss, interval is %ldus", handle, packet_gap);
+      //ROS_INFO("Lidar[%d] packet time interval is %ldns", handle, packet_gap);
-      int64_t packet_loss_threshold_upper = packet_interval * packet_num;
+      pcl::PointXYZI point = {0}; /* fill zero points */
-      if (packet_gap >
-          packet_loss_threshold_upper) { // skip when gap is too large
-        break;
-      }
-      pcl::PointXYZI point = {0}; // fill zero points
       for (uint32_t i = 0; i < storage_packet.point_num; i++) {
         cloud->points.push_back(point);
       }
-      last_timestamp = last_timestamp + packet_interval;
+      last_timestamp = last_timestamp + lidar->packet_interval;
+      if (!published_packet) { 
+          cloud->header.stamp = last_timestamp / 1000.0; // to pcl ros time stamp
+      }
       ++published_packet;
       continue;
     }
@@ -258,10 +295,10 @@ uint32_t Lddc::PublishPointcloudData(LidarDataQueue *queue, uint32_t packet_num,
     uint8_t point_buf[2048];
     if (kSourceLvxFile != data_source) {
       PointConvertHandler pf_point_convert =
-          GetConvertHandler(raw_packet->data_type);
+          GetConvertHandler(lidar->raw_data_type);
       if (pf_point_convert) {
         pf_point_convert(point_buf, raw_packet,
-                         lds_->lidars_[handle].extrinsic_parameter);
+                         lidar->extrinsic_parameter);
       } else {
         /* Skip the packet */
         ROS_INFO("Lidar[%d] unkown packet type[%d]", handle,
@@ -270,7 +307,7 @@ uint32_t Lddc::PublishPointcloudData(LidarDataQueue *queue, uint32_t packet_num,
       }
     } else {
       LivoxPointToPxyzrtl(point_buf, raw_packet,
-                          lds_->lidars_[handle].extrinsic_parameter);
+                          lidar->extrinsic_parameter);
     }
 
     LivoxPointXyzrtl *dst_point = (LivoxPointXyzrtl *)point_buf;
@@ -299,8 +336,8 @@ uint32_t Lddc::PublishPointcloudData(LidarDataQueue *queue, uint32_t packet_num,
     }
   }
 
-  if (!lds_->lidars_[handle].data_is_pubulished) {
+  if (!lidar->data_is_pubulished) {
-    lds_->lidars_[handle].data_is_pubulished = true;
+    lidar->data_is_pubulished = true;
   }
 
   return published_packet;
@@ -316,7 +353,9 @@ uint32_t Lddc::PublishCustomPointcloud(LidarDataQueue *queue,
 
   livox_ros_driver::CustomMsg livox_msg;
 
-  livox_msg.header.frame_id = "livox_frame";
+  //livox_msg.header.frame_id = "livox_frame";
+  livox_msg.header.frame_id.assign(frame_id_);
+  
   livox_msg.header.seq = msg_seq;
   ++msg_seq;
   // livox_msg.header.stamp = ros::Time::now();
@@ -324,13 +363,14 @@ uint32_t Lddc::PublishCustomPointcloud(LidarDataQueue *queue,
   livox_msg.point_num = 0;
   livox_msg.lidar_id = handle;
 
+  LidarDevice *lidar  = &lds_->lidars_[handle];
   uint8_t data_source = lds_->lidars_[handle].data_src;
   StoragePacket storage_packet;
   while (published_packet < packet_num) {
     QueueProPop(queue, &storage_packet);
     LivoxEthPacket *raw_packet =
         reinterpret_cast<LivoxEthPacket *>(storage_packet.raw_data);
-    uint32_t point_interval = GetPointInterval(raw_packet->data_type);
+    uint32_t point_interval = GetPointInterval(lidar->raw_data_type);
     uint32_t dual_point = 0;
     if ((raw_packet->data_type == kDualExtendCartesian) ||
         (raw_packet->data_type == kDualExtendSpherical)) {
@@ -339,7 +379,7 @@ uint32_t Lddc::PublishCustomPointcloud(LidarDataQueue *queue,
 
     timestamp = GetStoragePacketTimestamp(&storage_packet, data_source);
     if (((timestamp - last_timestamp) > kDeviceDisconnectThreshold) &&
-        published_packet && lds_->lidars_[handle].data_is_pubulished) {
+        published_packet && lidar->data_is_pubulished) {
       ROS_INFO("Lidar[%d] packet loss", handle);
       break;
     }
@@ -357,19 +397,19 @@ uint32_t Lddc::PublishCustomPointcloud(LidarDataQueue *queue,
     uint8_t point_buf[2048];
     if (kSourceLvxFile != data_source) {
       PointConvertHandler pf_point_convert =
-          GetConvertHandler(raw_packet->data_type);
+          GetConvertHandler(lidar->raw_data_type);
       if (pf_point_convert) {
         pf_point_convert(point_buf, raw_packet,
-                         lds_->lidars_[handle].extrinsic_parameter);
+                         lidar->extrinsic_parameter);
       } else {
         /* Skip the packet */
         ROS_INFO("Lidar[%d] unkown packet type[%d]", handle,
-                 raw_packet->data_type);
+                 lidar->raw_data_type);
         break;
       }
     } else {
       LivoxPointToPxyzrtl(point_buf, raw_packet,
-                          lds_->lidars_[handle].extrinsic_parameter);
+                          lidar->extrinsic_parameter);
     }
 
     LivoxPointXyzrtl *dst_point = (LivoxPointXyzrtl *)point_buf;
@@ -405,8 +445,8 @@ uint32_t Lddc::PublishCustomPointcloud(LidarDataQueue *queue,
     }
   }
 
-  if (!lds_->lidars_[handle].data_is_pubulished) {
+  if (!lidar->data_is_pubulished) {
-    lds_->lidars_[handle].data_is_pubulished = true;
+    lidar->data_is_pubulished = true;
   }
 
   return published_packet;
@@ -476,7 +516,7 @@ void Lddc::PollingLidarPointCloudData(uint8_t handle, LidarDevice *lidar) {
   while (!QueueIsEmpty(p_queue)) {
     uint32_t used_size = QueueUsedSize(p_queue);
     uint32_t onetime_publish_packets =
-        GetPacketNumPerSec(lidar->info.type) / publish_frq_;
+        GetPacketNumPerSec(lidar->raw_data_type) / publish_frq_;
     if (used_size < onetime_publish_packets) {
       break;
     }

livox_ros_driver/livox_ros_driver/lddc.h

@@ -41,7 +41,8 @@ typedef enum {
 
 class Lddc {
 public:
-  Lddc(int format, int multi_topic, int data_src, int output_type, double frq);
+  Lddc(int format, int multi_topic, int data_src, int output_type, double frq, \
+       std::string &frame_id);
   ~Lddc();
 
   int RegisterLds(Lds *lds);
@@ -59,6 +60,8 @@ public:
   Lds *lds_;
 
 private:
+  int32_t GetPublishStartTime(LidarDevice *lidar, LidarDataQueue *queue,
+      uint64_t *start_time, StoragePacket *storage_packet);
   uint32_t PublishPointcloud2(LidarDataQueue *queue, uint32_t packet_num,
                               uint8_t handle);
   uint32_t PublishPointcloudData(LidarDataQueue *queue, uint32_t packet_num,
@@ -78,7 +81,8 @@ private:
   uint8_t data_src_;
   uint8_t output_type_;
   double publish_frq_;
-  int32_t publish_interval_ms_;
+  uint32_t publish_period_ns_;
+  std::string frame_id_;
   ros::Publisher *private_pub_[kMaxSourceLidar];
   ros::Publisher *global_pub_;
   ros::Publisher *private_imu_pub_[kMaxSourceLidar];

livox_ros_driver/livox_ros_driver/lds.cpp

@@ -441,6 +441,7 @@ void Lds::ResetLidar(LidarDevice *lidar, uint8_t data_src) {
   lidar->data_src = data_src;
   lidar->data_is_pubulished = false;
   lidar->connect_state = kConnectStateOff;
+  lidar->raw_data_type = 0xFF;
 }
 
 void Lds::SetLidarDataSrc(LidarDevice *lidar, uint8_t data_src) {

livox_ros_driver/livox_ros_driver/lds.h

@@ -95,11 +95,12 @@ typedef enum {
 typedef enum { kCoordinateCartesian = 0, kCoordinateSpherical } CoordinateType;
 
 typedef enum {
-  kConfigFan = 1,
+  kConfigFan = 1 << 0,
-  kConfigReturnMode = 2,
+  kConfigReturnMode = 1 << 1,
-  kConfigCoordinate = 4,
+  kConfigCoordinate = 1 << 2,
-  kConfigImuRate = 8,
+  kConfigImuRate    = 1 << 3,
-  kConfigGetExtrinsicParameter = 16,
+  kConfigGetExtrinsicParameter = 1 << 4,
+  kConfigSetHighSensitivity    = 1 << 5,  
   kConfigUndef
 } LidarConfigCodeBit;
 
@@ -139,6 +140,7 @@ typedef struct {
   uint32_t coordinate;
   uint32_t imu_rate;
   uint32_t extrinsic_parameter_source;
+  bool enable_high_sensitivity;
 } UserRawConfig;
 
 typedef struct {
@@ -147,6 +149,7 @@ typedef struct {
   uint32_t coordinate;
   uint32_t imu_rate;
   uint32_t extrinsic_parameter_source;
+  bool enable_high_sensitivity;
   volatile uint32_t set_bits;
   volatile uint32_t get_bits;
 } UserConfig;
@@ -166,8 +169,13 @@ typedef struct {
 typedef struct {
   uint8_t handle;          /**< Lidar access handle. */
   uint8_t data_src;        /**< From raw lidar or livox file. */
+  uint8_t raw_data_type;   /**< The data type in eth packaet */
   bool data_is_pubulished; /**< Indicate the data of lidar whether is
-                              pubulished. */
+                                pubulished. */
+  volatile uint32_t packet_interval;/**< The time interval between packets 
+                                       of current lidar, unit:ns */
+  volatile uint32_t packet_interval_max; /**< If more than it,
+                                            have packet loss */                            
   volatile LidarConnectState connect_state;
   DeviceInfo info;
   LidarPacketStatistic statistic_info;
@@ -220,7 +228,7 @@ const PacketInfoPair packet_info_pair_table[kMaxPointDataType] = {
     {100, 100000, 10000, 1000000, 1318}, {100, 100000, 10000, 1000000, 918},
     {96, 240000, 4167, 400000, 1362},    {96, 240000, 4167, 400000, 978},
     {96, 480000, 4167, 400000, 1362},    {48, 480000, 4167, 400000, 978},
-    {1, 100, 10000000, 10000000, 42}};
+    {1, 200, 10000000, 10000000, 42}};
 
 /**
  * Global function for general use.

livox_ros_driver/livox_ros_driver/lds_hub.cpp

@@ -162,6 +162,13 @@ void LdsHub::OnHubDataCb(uint8_t hub_handle, LivoxEthPacket *data,
          sizeof(cur_timestamp));
 
   if (kImu != eth_packet->data_type) {
+    if (p_lidar->raw_data_type != eth_packet->data_type) {
+      p_lidar->raw_data_type = eth_packet->data_type;
+      p_lidar->packet_interval = GetPacketInterval(eth_packet->data_type);
+      p_lidar->packet_interval_max = p_lidar->packet_interval * 1.8f;
+      
+    }
+    
     if (eth_packet->timestamp_type == kTimestampTypePps) {
       /** Whether a new sync frame */
       if ((cur_timestamp.stamp < packet_statistic->last_timestamp) &&
@@ -210,7 +217,7 @@ void LdsHub::OnHubDataCb(uint8_t hub_handle, LivoxEthPacket *data,
              p_lidar->info.broadcast_code, queue_size, p_queue->size);
     }
 
-    if (!QueueIsFull(p_queue) && (cur_timestamp.stamp >= 0)) {
+    if (!QueueIsFull(p_queue)) {
       QueuePushAny(p_queue, (uint8_t *)eth_packet,
                    GetEthPacketLen(eth_packet->data_type),
                    packet_statistic->imu_timebase,

livox_ros_driver/livox_ros_driver/lds_lidar.cpp

@@ -24,15 +24,18 @@
 
 #include "lds_lidar.h"
 
-#include <memory>
 #include <stdio.h>
 #include <string.h>
+#include <memory>
 #include <thread>
+#include <mutex>
 
 #include "rapidjson/document.h"
 #include "rapidjson/filereadstream.h"
 #include "rapidjson/stringbuffer.h"
 
+using namespace std;
+
 namespace livox_ros {
 
 /** Const varible
@@ -175,6 +178,12 @@ void LdsLidar::OnLidarDataCb(uint8_t handle, LivoxEthPacket *data,
          sizeof(cur_timestamp));
 
   if (kImu != eth_packet->data_type) {
+    if (p_lidar->raw_data_type != eth_packet->data_type) {
+      p_lidar->raw_data_type = eth_packet->data_type;
+      p_lidar->packet_interval = GetPacketInterval(eth_packet->data_type);
+      p_lidar->packet_interval_max = p_lidar->packet_interval * 1.8f;
+    }
+    
     if (eth_packet->timestamp_type == kTimestampTypePps) {
       if ((cur_timestamp.stamp < packet_statistic->last_timestamp) &&
           (cur_timestamp.stamp < kPacketTimeGap)) { // whether a new sync frame
@@ -225,7 +234,7 @@ void LdsLidar::OnLidarDataCb(uint8_t handle, LivoxEthPacket *data,
              p_lidar->info.broadcast_code, queue_size, p_queue->size);
     }
 
-    if (!QueueIsFull(p_queue) && (cur_timestamp.stamp >= 0)) {
+    if (!QueueIsFull(p_queue)) {
       QueuePushAny(p_queue, (uint8_t *)eth_packet,
                    GetEthPacketLen(eth_packet->data_type),
                    packet_statistic->timebase,
@@ -274,6 +283,7 @@ void LdsLidar::OnDeviceBroadcast(const BroadcastDeviceInfo *info) {
       config.coordinate = kCoordinateCartesian;
       config.imu_rate = kImuFreq200Hz;
       config.extrinsic_parameter_source = kNoneExtrinsicParameter;
+      config.enable_high_sensitivity = false;
     }
 
     p_lidar->config.enable_fan = config.enable_fan;
@@ -282,6 +292,8 @@ void LdsLidar::OnDeviceBroadcast(const BroadcastDeviceInfo *info) {
     p_lidar->config.imu_rate = config.imu_rate;
     p_lidar->config.extrinsic_parameter_source =
         config.extrinsic_parameter_source;
+    p_lidar->config.enable_high_sensitivity =
+        config.enable_high_sensitivity;
   } else {
     printf("Add lidar to connect is failed : %d %d \n", result, handle);
   }
@@ -321,6 +333,9 @@ void LdsLidar::OnDeviceChange(const DeviceInfo *info, DeviceEvent type) {
 
     /** Config lidar parameter */
     if (p_lidar->info.state == kLidarStateNormal) {
+      /** Ensure the thread safety for set_bits and connect_state */
+      lock_guard<mutex> lock(g_lds_ldiar->config_mutex_);
+      
       if (p_lidar->config.coordinate != 0) {
         SetSphericalCoordinate(handle, SetCoordinateCb, g_lds_ldiar);
       } else {
@@ -345,6 +360,20 @@ void LdsLidar::OnDeviceChange(const DeviceInfo *info, DeviceEvent type) {
           kExtrinsicParameterFromLidar) {
         LidarGetExtrinsicParameter(handle, GetLidarExtrinsicParameterCb,
                                    g_lds_ldiar);
+        p_lidar->config.set_bits |= kConfigGetExtrinsicParameter;
+      }
+
+      if (kDeviceTypeLidarTele == info->type) {
+        if (p_lidar->config.enable_high_sensitivity) {
+          LidarEnableHighSensitivity(handle, SetHighSensitivityCb,
+                                     g_lds_ldiar);
+          printf("Enable high sensitivity\n");
+        } else {
+          LidarDisableHighSensitivity(handle, SetHighSensitivityCb,
+                                      g_lds_ldiar);
+          printf("Disable high sensitivity\n");
+        }
+        p_lidar->config.set_bits |= kConfigSetHighSensitivity;
       }
 
       p_lidar->connect_state = kConnectStateConfig;
@@ -403,14 +432,14 @@ void LdsLidar::SetPointCloudReturnModeCb(livox_status status, uint8_t handle,
   LidarDevice *p_lidar = &(lds_lidar->lidars_[handle]);
 
   if (status == kStatusSuccess) {
-    p_lidar->config.set_bits &= ~((uint32_t)(kConfigReturnMode));
     printf("Set return mode success!\n");
 
+    lock_guard<mutex> lock(lds_lidar->config_mutex_);
+    p_lidar->config.set_bits &= ~((uint32_t)(kConfigReturnMode));
     if (!p_lidar->config.set_bits) {
       LidarStartSampling(handle, StartSampleCb, lds_lidar);
       p_lidar->connect_state = kConnectStateSampling;
     }
-
   } else {
     LidarSetPointCloudReturnMode(
         handle, (PointCloudReturnMode)(p_lidar->config.return_mode),
@@ -429,9 +458,10 @@ void LdsLidar::SetCoordinateCb(livox_status status, uint8_t handle,
   LidarDevice *p_lidar = &(lds_lidar->lidars_[handle]);
 
   if (status == kStatusSuccess) {
-    p_lidar->config.set_bits &= ~((uint32_t)(kConfigCoordinate));
     printf("Set coordinate success!\n");
 
+    lock_guard<mutex> lock(lds_lidar->config_mutex_);
+    p_lidar->config.set_bits &= ~((uint32_t)(kConfigCoordinate));
     if (!p_lidar->config.set_bits) {
       LidarStartSampling(handle, StartSampleCb, lds_lidar);
       p_lidar->connect_state = kConnectStateSampling;
@@ -457,14 +487,14 @@ void LdsLidar::SetImuRatePushFrequencyCb(livox_status status, uint8_t handle,
   LidarDevice *p_lidar = &(lds_lidar->lidars_[handle]);
 
   if (status == kStatusSuccess) {
-    p_lidar->config.set_bits &= ~((uint32_t)(kConfigImuRate));
     printf("Set imu rate success!\n");
 
+    lock_guard<mutex> lock(lds_lidar->config_mutex_);
+    p_lidar->config.set_bits &= ~((uint32_t)(kConfigImuRate));
     if (!p_lidar->config.set_bits) {
       LidarStartSampling(handle, StartSampleCb, lds_lidar);
       p_lidar->connect_state = kConnectStateSampling;
     }
-
   } else {
     LidarSetImuPushFrequency(handle, (ImuFreq)(p_lidar->config.imu_rate),
                              SetImuRatePushFrequencyCb, g_lds_ldiar);
@@ -497,9 +527,10 @@ void LdsLidar::GetLidarExtrinsicParameterCb(
       if (p_lidar->config.extrinsic_parameter_source) {
         p_extrinsic->enable = true;
       }
-      p_lidar->config.set_bits &= ~((uint32_t)(kConfigGetExtrinsicParameter));
       printf("Lidar[%d] get ExtrinsicParameter success!\n", handle);
 
+      lock_guard<mutex> lock(lds_lidar->config_mutex_);
+      p_lidar->config.set_bits &= ~((uint32_t)(kConfigGetExtrinsicParameter));
       if (!p_lidar->config.set_bits) {
         LidarStartSampling(handle, StartSampleCb, lds_lidar);
         p_lidar->connect_state = kConnectStateSampling;
@@ -512,6 +543,36 @@ void LdsLidar::GetLidarExtrinsicParameterCb(
   }
 }
 
+void LdsLidar::SetHighSensitivityCb(livox_status status, uint8_t handle,
+    DeviceParameterResponse *response, void *clent_data) {
+  LdsLidar *lds_lidar = static_cast<LdsLidar *>(clent_data);
+
+  if (handle >= kMaxLidarCount) {
+    return;
+  }
+  LidarDevice *p_lidar = &(lds_lidar->lidars_[handle]);
+
+  if (status == kStatusSuccess) {
+    p_lidar->config.set_bits &= ~((uint32_t)(kConfigSetHighSensitivity));
+    printf("Set high sensitivity success!\n");
+
+    lock_guard<mutex> lock(lds_lidar->config_mutex_);
+    if (!p_lidar->config.set_bits) {
+      LidarStartSampling(handle, StartSampleCb, lds_lidar);
+      p_lidar->connect_state = kConnectStateSampling;
+    };
+  } else {
+    if (p_lidar->config.enable_high_sensitivity) {
+      LidarEnableHighSensitivity(handle, SetHighSensitivityCb,
+                                 g_lds_ldiar);
+    } else {
+      LidarDisableHighSensitivity(handle, SetHighSensitivityCb,
+                                  g_lds_ldiar);
+    }
+    printf("Set high sensitivity fail, try again!\n");
+  }
+}
+
 /** Callback function of starting sampling. */
 void LdsLidar::StartSampleCb(livox_status status, uint8_t handle,
                              uint8_t response, void *clent_data) {
@@ -676,7 +737,7 @@ int LdsLidar::ParseConfigFile(const char *pathname) {
       for (size_t i = 0; i < len; i++) {
         const rapidjson::Value &object = array[i];
         if (object.IsObject()) {
-          UserRawConfig config;
+          UserRawConfig config = {0};
           memset(&config, 0, sizeof(config));
           if (object.HasMember("broadcast_code") &&
               object["broadcast_code"].IsString()) {
@@ -710,6 +771,11 @@ int LdsLidar::ParseConfigFile(const char *pathname) {
             config.extrinsic_parameter_source =
                 object["extrinsic_parameter_source"].GetInt();
           }
+          if (object.HasMember("enable_high_sensitivity") &&
+              object["enable_high_sensitivity"].GetBool()) {
+            config.enable_high_sensitivity =
+                object["enable_high_sensitivity"].GetBool();
+          }
 
           printf("broadcast code[%s] : %d %d %d %d %d %d\n",
                  config.broadcast_code, config.enable_connect,
@@ -780,6 +846,7 @@ int LdsLidar::GetRawConfig(const char *broadcast_code, UserRawConfig &config) {
       config.coordinate = ite_config.coordinate;
       config.imu_rate = ite_config.imu_rate;
       config.extrinsic_parameter_source = ite_config.extrinsic_parameter_source;
+      config.enable_high_sensitivity = ite_config.enable_high_sensitivity;
       return 0;
     }
   }

livox_ros_driver/livox_ros_driver/lds_lidar.h

@@ -29,6 +29,7 @@
 
 #include <memory>
 #include <vector>
+#include <mutex>
 
 #include "lds.h"
 #include "livox_sdk.h"
@@ -87,6 +88,8 @@ private:
   GetLidarExtrinsicParameterCb(livox_status status, uint8_t handle,
                                LidarGetExtrinsicParameterResponse *response,
                                void *clent_data);
+  static void SetHighSensitivityCb(livox_status status, uint8_t handle,
+      DeviceParameterResponse *response, void *clent_data);
 
   void ResetLdsLidar(void);
   int AddBroadcastCodeToWhitelist(const char *broadcast_code);
@@ -110,6 +113,7 @@ private:
   bool enable_timesync_;
   TimeSync *timesync_;
   TimeSyncConfig timesync_config_;
+  std::mutex config_mutex_;
 };
 
 } // namespace livox_ros

livox_ros_driver/livox_ros_driver/lds_lvx.cpp

@@ -161,11 +161,18 @@ void LdsLvx::ReadLvxFile() {
           eth_packet = (LivoxEthPacket *)(&detail_packet->version);
           handle = detail_packet->device_index;
         }
+
         data_type = eth_packet->data_type;
-        data_offset +=
+        /** Packet length + device index */
-            (GetEthPacketLen(data_type) + 1); /* packet length + device index */
+        data_offset += (GetEthPacketLen(data_type) + 1); 
         if (data_type != kImu) {
+          LidarDevice *p_lidar = &lidars_[handle];
           LidarDataQueue *p_queue = &lidars_[handle].data;
+          if (p_lidar->raw_data_type != eth_packet->data_type) {
+            p_lidar->raw_data_type = eth_packet->data_type;
+            p_lidar->packet_interval = GetPacketInterval(eth_packet->data_type);
+            p_lidar->packet_interval_max = p_lidar->packet_interval * 1.8f;
+          }          
           if ((p_queue != nullptr) && (handle < lidar_count_)) {
             while (QueueIsFull(p_queue)) {
               std::this_thread::sleep_for(std::chrono::milliseconds(1));

livox_ros_driver/livox_ros_driver/livox_ros_driver.cpp

@@ -63,18 +63,27 @@ int main(int argc, char **argv) {
   int xfer_format = kPointCloud2Msg;
   int multi_topic = 0;
   int data_src = kSourceRawLidar;
-  double publish_freq = 20.0; /* Hz */
+  double publish_freq = 10.0; /* Hz */
   int output_type = kOutputToRos;
+  std::string frame_id = "livox_frame";
 
   livox_node.getParam("xfer_format", xfer_format);
   livox_node.getParam("multi_topic", multi_topic);
   livox_node.getParam("data_src", data_src);
   livox_node.getParam("publish_freq", publish_freq);
   livox_node.getParam("output_data_type", output_type);
+  livox_node.getParam("frame_id", frame_id);
+  if (publish_freq > 100.0) {
+    publish_freq = 100.0;
+  } else if (publish_freq < 1.0) {
+    publish_freq = 1.0;
+  } else {
+    publish_freq = publish_freq;
+  }
 
   /** Lidar data distribute control and lidar data source set */
-  Lddc *lddc =
+  Lddc *lddc = new Lddc(xfer_format, multi_topic, data_src, output_type, \
-      new Lddc(xfer_format, multi_topic, data_src, output_type, publish_freq);
+                        publish_freq, frame_id);
   lddc->SetRosNode(&livox_node);
 
   int ret = 0;
@@ -150,7 +159,7 @@ int main(int argc, char **argv) {
   }
 
   ros::Time::init();
-  double poll_freq = publish_freq;
+  double poll_freq = publish_freq * 4;
   if (data_src == kSourceLvxFile) {
     poll_freq = 2000;
   }