feat:support AVIA and Mid-70

Fix double free of global_pub_

README.md

@@ -2,6 +2,16 @@
 
 livox_ros_driver is a new ROS package, specially used to connect LiDAR products produced by Livox. The driver can be run under ubuntu 14.04/16.04/18.04 operating system with ROS environment (indigo, kinetic, melodic) installed. Tested hardware platforms that can run livox_ros_driver include: Intel x86 cpu platforms, and some ARM64 hardware platforms (such as nvida TX2 / Xavier, etc.).
 
+## 0. Version and Release History
+
+### 0.1 Current Version
+
+[v2.6.0](https://github.com/Livox-SDK/livox_ros_driver/releases)
+
+### 0.2 Release History
+
+[Release History](https://github.com/Livox-SDK/livox_ros_driver/releases)
+
 ## 1. Install dependencies
 
 Before running livox_ros_driver, ROS and Livox-SDK must be installed.
@@ -171,12 +181,10 @@ In the "ws_livox/src/livox_ros_driver/launch" path, there are two json files, li
     The parameter attributes in the above json file are described in the following table :
 
 <center>LiDAR configuration parameter</center>
-
 | Parameter                  | Type    | Description                                                  | Default         |
 | :------------------------- | ------- | ------------------------------------------------------------ | --------------- |
 | broadcast_code             | String  | LiDAR broadcast code, 15 characters, consisting of a 14-character length serial number plus a character-length additional code | 0TFDG3B006H2Z11 |
 | enable_connect             | Boolean | Whether to connect to this LiDAR<br>true -- Connect this LiDAR<br>false --Do not connect this LiDAR | false           |
-| enable_fan                 | Boolean | Whether to automatically control the fan of this LiDAR<br>true -- Automatically control the fan of this LiDAR<br>false -- Does not automatically control the fan of this LiDAR | true            |
 | return_mode                | Int     | return mode<br>0 -- First single return mode<br>1 -- Strongest single return mode<br>2 -- Dual return mode | 0               |
 | coordinate                 | Int     | Coordinate<br>0 -- Cartesian<br>1 -- Spherical               | 0               |
 | imu_rate                   | Int     | Push frequency of IMU sensor data<br>0 -- stop push<br>1 -- 200 Hz<br>Others -- undefined, it will cause unpredictable behavior<br>Currently only Horizon supports this, MID serials do not support it | 0               |
@@ -198,7 +206,6 @@ In the "ws_livox/src/livox_ros_driver/launch" path, there are two json files, li
    "lidar_config": [
       {
          "broadcast_code": "0TFDG3B006H2Z11",
-         "enable_fan": true,
          "return_mode": 0,
          "imu_rate": 1
       }
@@ -209,7 +216,6 @@ In the "ws_livox/src/livox_ros_driver/launch" path, there are two json files, li
 &ensp;&ensp;&ensp;&ensp;The main difference between the content of Hub json configuration file and the content of the LiDAR json configuration file is that the Hub configuration item "hub_config" is added, and the related configuration content of the Hub is shown in the following table :
 
 <center>HUB configuration parameter</center>
-
 | Parameter      | Type    | Description                                                  | Default         |
 | -------------- | ------- | ------------------------------------------------------------ | --------------- |
 | broadcast_code | String  | HUB broadcast code, 15 characters, consisting of a 14-character length serial number plus a character-length additional code | 13UUG1R00400170 |
@@ -251,7 +257,6 @@ $GPRMC,190430,A,4812.3038,S,07330.7690,W,3.7,3.8,090210,13.7,E,D*26
 livox_ros_driver only supports the timestamp synchronization function when connected to LiDAR. The timestamp related configuration item timesync_config is in the livox_lidar_config.json file. The detailed configuration content is shown in the table below :
 
 <center>Timestamp synchronization function configuration instructions</center>
-
 | Parameter        | Type     | Description                                                  | Default        |
 | ---------------- | -------- | ------------------------------------------------------------ | -------------- |
 | enable_timesync  | Boolean  | Whether to enable the timestamp synchronization <br>true -- Enable timestamp synchronization<br>false -- Disable timestamp synchronization | false          |

README_CN.md

@@ -3,6 +3,16 @@
 览沃ROS驱动程序是一个全新的 ROS 包，专门用于连接览沃生产的 LiDAR 产品。该驱动程序可以在安装了
 ROS 环境（ indigo,kinetic,melodic ）的 ubuntu14.04/16.04/18.04 操作系统下运行。经测试可以运行览沃 ROS 驱动程序的硬件平台包括：intel x86 主流 cpu 平台，部分 ARM64 硬件平台（如，nvida TX2/Xavier 等）。
 
+## 0. 版本和发布记录
+
+### 0.1 当前版本
+
+v2.6.0
+
+### 0.2 发布记录
+
+[发布记录](https://github.com/Livox-SDK/livox_ros_driver/releases)
+
 ## 1. 安装依赖
 
 运行览沃 ROS 驱动程序之前，必须安装 ROS 和 Livox-SDK。
@@ -165,7 +175,6 @@ uint8 line              # laser number in lidar
          {
             "broadcast_code": "0TFDG3B006H2Z11",
             "enable_connect": true,
-            "enable_fan": true,
             "return_mode": 0,
             "coordinate": 0,
             "imu_rate": 1,
@@ -178,12 +187,10 @@ uint8 line              # laser number in lidar
     上面 json 文件中各参数属性说明如下表：
 
 <center>LiDAR 配置参数说明</center>
-
 | 属性                       | 类型   | 描述                                                         | 默认值          |
 | :------------------------- | ------ | ------------------------------------------------------------ | --------------- |
 | broadcast_code             | 字符串 | LiDAR 广播码，15位字符，由14位字符长度序列号加一个字符长度附加码组成 | 0TFDG3B006H2Z11 |
 | enable_connect             | 布尔值 | 是否连接此 LiDAR<br>true -- 连接此 LiDAR<br>false -- 禁止连接此 LiDAR | false           |
-| enable_fan                 | 布尔值 | 是否自动控制此 LiDAR 风扇<br>true -- 自动控制 LiDAR 风扇<br>false -- 禁止自动控制此 LiDAR 风扇 | true            |
 | return_mode                | 整型   | 回波模式<br>0 -- 第一个回波模式<br>1 -- 最强回波模式<br>2 -- 双回波模式 | 0               |
 | coordinate                 | 整型   | 原始点云数据的坐标轴类型<br>0 -- 直角坐标系<br>1 -- 球坐标系 | 0               |
 | imu_rate                   | 整型   | IMU 传感器数据的推送频率<br>0 -- 关闭 IMU 传感器数据推送<br>1 --  以 200Hz 频率推送 IMU 传感器数据<br>其他值 -- 未定义，会导致不可预测的行为发生<br>目前只有 Horizon/Tele 支持此选项，MID 序列不支持 | 0               |
@@ -206,7 +213,6 @@ uint8 line              # laser number in lidar
       "lidar_config": [
          {
             "broadcast_code": "0TFDG3B006H2Z11",
-            "enable_fan": true,
             "return_mode": 0,
             "imu_rate": 1
          }
@@ -217,7 +223,6 @@ uint8 line              # laser number in lidar
 &ensp;&ensp;&ensp;&ensp;中心板 json 配置文件内容与 LiDAR 配置文件的主要区别在于，增加了中心板配置项 hub_config ，中心板相关的具体配置内容见下表：
 
 <center>HUB 配置参数说明</center>
-
 | 属性           | 类型   | 描述                                                         | 默认值          |
 | -------------- | ------ | ------------------------------------------------------------ | --------------- |
 | broadcast_code | 字符串 | HUB 广播码，15位字符，由14位字符长度的序列号加一个字符长度的附加码组成 | 13UUG1R00400170 |
@@ -261,7 +266,6 @@ uint8 line              # laser number in lidar
 览沃 ROS 驱动程序只有在与 LiDAR 连接的时候才支持时间戳同步功能，时间戳相关的配置项 timesync_config 位于 livox_lidar_config.json 文件中，详细的配置内容见下表：
 
 <center>时间戳同步功能配置说明</center>
-
 | 属性             | 类型   | 描述                                                         | 默认值         |
 | ---------------- | ------ | ------------------------------------------------------------ | -------------- |
 | enable_timesync  | 布尔值 | 是否使能时间戳同步功能<br>true -- 使能时间戳同步功能<br>false -- 禁止时间戳同步功能 | false          |

livox_ros_driver/CMakeLists.txt

@@ -212,6 +212,10 @@ install(TARGETS ${PROJECT_NAME}_node
 	RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 	)
 
+install(DIRECTORY launch/
+    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+	)
+
 #---------------------------------------------------------------------------------------
 # end of CMakeList.txt
 #---------------------------------------------------------------------------------------

livox_ros_driver/common/FastCRC/FastCRC.h

@@ -53,8 +53,8 @@ public:
   FastCRC16(uint16_t seed);
 
   // change function name from mcrf4xx_upd to mcrf4xx
-  uint16_t
-  mcrf4xx_calc(const uint8_t *data,
+  uint16_t mcrf4xx_calc(
+      const uint8_t *data,
       const uint16_t datalen);  // Equivalent to _crc_ccitt_update() in
                                 // crc16.h from avr_libc
 

livox_ros_driver/common/FastCRC/FastCRCsw.cpp

@@ -55,7 +55,6 @@ FastCRC16::FastCRC16(uint16_t seed) { seed_ = seed; }
  */
 
 uint16_t FastCRC16::mcrf4xx_calc(const uint8_t *data, uint16_t len) {
-
   uint16_t crc = seed_;
 
   while (((uintptr_t)data & 3) && len) {
@@ -110,7 +109,6 @@ FastCRC32::FastCRC32(uint32_t seed) { seed_ = seed; }
 #endif
 
 uint32_t FastCRC32::crc32_calc(const uint8_t *data, uint16_t len) {
-
   uint32_t crc = seed_ ^ 0xffffffff;
 
   while (((uintptr_t)data & 3) && len) {

livox_ros_driver/common/comm/comm_protocol.cpp

@@ -23,9 +23,9 @@
 //
 
 #include "comm_protocol.h"
-#include <iostream>
 #include <stdio.h>
 #include <string.h>
+#include <iostream>
 
 namespace livox_ros {
 

livox_ros_driver/common/comm/comm_protocol.h

@@ -25,10 +25,10 @@
 #ifndef COMM_COMM_PROTOCOL_H_
 #define COMM_COMM_PROTOCOL_H_
 
+#include <stdint.h>
 #include "gps_protocol.h"
 #include "protocol.h"
 #include "sdk_protocol.h"
-#include <stdint.h>
 
 namespace livox_ros {
 const uint32_t kCacheSize = 8192;

livox_ros_driver/common/comm/gps_protocol.cpp

@@ -114,12 +114,10 @@ uint8_t GpsProtocol::CalcGpsPacketChecksum(const uint8_t *buf,
 
 uint8_t AscciiToHex(const uint8_t *TwoChar) {
   uint8_t h = toupper(TwoChar[0]) - 0x30;
-  if (h > 9)
-    h -= 7;
+  if (h > 9) h -= 7;
 
   uint8_t l = toupper(TwoChar[1]) - 0x30;
-  if (l > 9)
-    l -= 7;
+  if (l > 9) l -= 7;
 
   return h * 16 + l;
 }

livox_ros_driver/common/comm/gps_protocol.h

@@ -25,8 +25,8 @@
 #ifndef LIVOX_GPS_PROTOCOL_H_
 #define LIVOX_GPS_PROTOCOL_H_
 
-#include "protocol.h"
 #include <stdint.h>
+#include "protocol.h"
 
 namespace livox_ros {
 

livox_ros_driver/common/comm/protocol.h

@@ -60,9 +60,6 @@ typedef struct CommPacket {
   uint8_t *data;
   uint16_t data_len;
   uint32_t padding;
-  //  RequestPackCb *ack_request_cb;
-  //  uint32_t retry_times;
-  //  uint32_t timeout;
 } CommPacket;
 
 /** SDK Protocol info config */
@@ -72,9 +69,7 @@ typedef struct {
 } SdkProtocolConfig;
 
 /** NAME-0183 Protocol info config for gps */
-typedef struct {
-  void *data;
-} GpsProtocolConfig;
+typedef struct { void *data; } GpsProtocolConfig;
 
 typedef struct {
   uint8_t type;

livox_ros_driver/common/comm/sdk_protocol.h

@@ -25,9 +25,9 @@
 #ifndef LIVOX_SDK_PROTOCOL_H_
 #define LIVOX_SDK_PROTOCOL_H_
 
+#include <stdint.h>
 #include "FastCRC/FastCRC.h"
 #include "protocol.h"
-#include <stdint.h>
 
 namespace livox_ros {
 typedef enum { kSdkVerNone, kSdkVer0, kSdkVer1 } SdkVersion;

livox_ros_driver/common/rapidjson/allocators.h

@@ -118,7 +118,8 @@ public:
     \tparam BaseAllocator the allocator type for allocating memory chunks.
    Default is CrtAllocator. \note implements Allocator concept
 */
-template <typename BaseAllocator = CrtAllocator> class MemoryPoolAllocator {
+template <typename BaseAllocator = CrtAllocator>
+class MemoryPoolAllocator {
  public:
   static const bool kNeedFree =
       false;  //!< Tell users that no need to call Free() with this allocator.
@@ -131,8 +132,11 @@ public:
   */
   MemoryPoolAllocator(size_t chunkSize = kDefaultChunkCapacity,
                       BaseAllocator *baseAllocator = 0)
-      : chunkHead_(0), chunk_capacity_(chunkSize), userBuffer_(0),
-        baseAllocator_(baseAllocator), ownBaseAllocator_(0) {}
+      : chunkHead_(0),
+        chunk_capacity_(chunkSize),
+        userBuffer_(0),
+        baseAllocator_(baseAllocator),
+        ownBaseAllocator_(0) {}
 
   //! Constructor with user-supplied buffer.
   /*! The user buffer will be used firstly. When it is full, memory pool
@@ -149,8 +153,11 @@ public:
   MemoryPoolAllocator(void *buffer, size_t size,
                       size_t chunkSize = kDefaultChunkCapacity,
                       BaseAllocator *baseAllocator = 0)
-      : chunkHead_(0), chunk_capacity_(chunkSize), userBuffer_(buffer),
-        baseAllocator_(baseAllocator), ownBaseAllocator_(0) {
+      : chunkHead_(0),
+        chunk_capacity_(chunkSize),
+        userBuffer_(buffer),
+        baseAllocator_(baseAllocator),
+        ownBaseAllocator_(0) {
     RAPIDJSON_ASSERT(buffer != 0);
     RAPIDJSON_ASSERT(size > sizeof(ChunkHeader));
     chunkHead_ = reinterpret_cast<ChunkHeader *>(buffer);
@@ -193,15 +200,13 @@ public:
    */
   size_t Size() const {
     size_t size = 0;
-    for (ChunkHeader *c = chunkHead_; c != 0; c = c->next)
-      size += c->size;
+    for (ChunkHeader *c = chunkHead_; c != 0; c = c->next) size += c->size;
     return size;
   }
 
   //! Allocates a memory block. (concept Allocator)
   void *Malloc(size_t size) {
-    if (!size)
-      return NULL;
+    if (!size) return NULL;
 
     size = RAPIDJSON_ALIGN(size);
     if (chunkHead_ == 0 || chunkHead_->size + size > chunkHead_->capacity)
@@ -216,24 +221,22 @@ public:
 
   //! Resizes a memory block (concept Allocator)
   void *Realloc(void *originalPtr, size_t originalSize, size_t newSize) {
-    if (originalPtr == 0)
-      return Malloc(newSize);
+    if (originalPtr == 0) return Malloc(newSize);
 
-    if (newSize == 0)
-      return NULL;
+    if (newSize == 0) return NULL;
 
     originalSize = RAPIDJSON_ALIGN(originalSize);
     newSize = RAPIDJSON_ALIGN(newSize);
 
     // Do not shrink if new size is smaller than original
-    if (originalSize >= newSize)
-      return originalPtr;
+    if (originalSize >= newSize) return originalPtr;
 
     // Simply expand it if it is the last allocation and there is sufficient
     // space
-    if (originalPtr == reinterpret_cast<char *>(chunkHead_) +
-                           RAPIDJSON_ALIGN(sizeof(ChunkHeader)) +
-                           chunkHead_->size - originalSize) {
+    if (originalPtr ==
+        reinterpret_cast<char *>(chunkHead_) +
+            RAPIDJSON_ALIGN(sizeof(ChunkHeader)) + chunkHead_->size -
+            originalSize) {
       size_t increment = static_cast<size_t>(newSize - originalSize);
       if (chunkHead_->size + increment <= chunkHead_->capacity) {
         chunkHead_->size += increment;
@@ -243,8 +246,7 @@ public:
 
     // Realloc process: allocate and copy memory, do not free original buffer.
     if (void *newBuffer = Malloc(newSize)) {
-      if (originalSize)
-        std::memcpy(newBuffer, originalPtr, originalSize);
+      if (originalSize) std::memcpy(newBuffer, originalPtr, originalSize);
       return newBuffer;
     } else
       return NULL;

livox_ros_driver/common/rapidjson/document.h

@@ -21,13 +21,13 @@
 
 /*! \file document.h */
 
+#include <limits>
+#include <new>  // placement new
 #include "encodedstream.h"
 #include "internal/meta.h"
 #include "internal/strfunc.h"
 #include "memorystream.h"
 #include "reader.h"
-#include <limits>
-#include <new> // placement new
 
 RAPIDJSON_DIAG_PUSH
 #ifdef __clang__
@@ -55,7 +55,8 @@ RAPIDJSON_DIAG_OFF(effc++)
 RAPIDJSON_NAMESPACE_BEGIN
 
 // Forward declaration.
-template <typename Encoding, typename Allocator> class GenericValue;
+template <typename Encoding, typename Allocator>
+class GenericValue;
 
 template <typename Encoding, typename Allocator, typename StackAllocator>
 class GenericDocument;
@@ -67,9 +68,11 @@ class GenericDocument;
    that so it moved as a namespace scope struct.
     https://code.google.com/p/rapidjson/issues/detail?id=64
 */
-template <typename Encoding, typename Allocator> class GenericMember {
+template <typename Encoding, typename Allocator>
+class GenericMember {
  public:
-  GenericValue<Encoding, Allocator> name; //!< name of member (must be a string)
+  GenericValue<Encoding, Allocator>
+      name;  //!< name of member (must be a string)
   GenericValue<Encoding, Allocator> value;  //!< value of member.
 
 #if RAPIDJSON_HAS_CXX11_RVALUE_REFS
@@ -137,9 +140,9 @@ private:
  */
 template <bool Const, typename Encoding, typename Allocator>
 class GenericMemberIterator {
-
   friend class GenericValue<Encoding, Allocator>;
-  template <bool, typename, typename> friend class GenericMemberIterator;
+  template <bool, typename, typename>
+  friend class GenericMemberIterator;
 
   typedef GenericMember<Encoding, Allocator> PlainType;
   typedef typename internal::MaybeAddConst<Const, PlainType>::Type ValueType;
@@ -315,7 +318,8 @@ class GenericMemberIterator<true, Encoding, Allocator> {
 
     \see StringRef, GenericValue::SetString
 */
-template <typename CharType> struct GenericStringRef {
+template <typename CharType>
+struct GenericStringRef {
   typedef CharType Ch;  //!< character type of the string
 
 //! Create string reference from \c const character array
@@ -394,8 +398,8 @@ template <typename CharType> struct GenericStringRef {
   operator const Ch *() const { return s; }
 
   const Ch *const s;      //!< plain CharType pointer
-  const SizeType
-      length; //!< length of the string (excluding the trailing NULL terminator)
+  const SizeType length;  //!< length of the string (excluding the trailing NULL
+                          //!terminator)
 
  private:
   SizeType NotNullStrLen(const CharType *str) {
@@ -407,7 +411,8 @@ private:
   static const Ch emptyString[];
 
   //! Disallow construction from non-const array
-  template <SizeType N> GenericStringRef(CharType (&str)[N]) /* = delete */;
+  template <SizeType N>
+  GenericStringRef(CharType (&str)[N]) /* = delete */;
   //! Copy assignment operator not permitted - immutable type
   GenericStringRef &operator=(const GenericStringRef &rhs) /* = delete */;
 };
@@ -470,8 +475,8 @@ inline GenericStringRef<CharType> StringRef(const CharType *str,
    preprocessor symbol \ref RAPIDJSON_HAS_STDSTRING.
 */
 template <typename CharType>
-inline GenericStringRef<CharType>
-StringRef(const std::basic_string<CharType> &str) {
+inline GenericStringRef<CharType> StringRef(
+    const std::basic_string<CharType> &str) {
   return GenericStringRef<CharType>(str.data(), SizeType(str.size()));
 }
 #endif
@@ -493,7 +498,8 @@ struct IsGenericValueImpl<T, typename Void<typename T::EncodingType>::Type,
 
 // helper to match arbitrary GenericValue instantiations, including derived
 // classes
-template <typename T> struct IsGenericValue : IsGenericValueImpl<T>::Type {};
+template <typename T>
+struct IsGenericValue : IsGenericValueImpl<T>::Type {};
 
 }  // namespace internal
 
@@ -502,9 +508,11 @@ template <typename T> struct IsGenericValue : IsGenericValueImpl<T>::Type {};
 
 namespace internal {
 
-template <typename ValueType, typename T> struct TypeHelper {};
+template <typename ValueType, typename T>
+struct TypeHelper {};
 
-template <typename ValueType> struct TypeHelper<ValueType, bool> {
+template <typename ValueType>
+struct TypeHelper<ValueType, bool> {
   static bool Is(const ValueType &v) { return v.IsBool(); }
   static bool Get(const ValueType &v) { return v.GetBool(); }
   static ValueType &Set(ValueType &v, bool data) { return v.SetBool(data); }
@@ -514,7 +522,8 @@ template <typename ValueType> struct TypeHelper<ValueType, bool> {
   }
 };
 
-template <typename ValueType> struct TypeHelper<ValueType, int> {
+template <typename ValueType>
+struct TypeHelper<ValueType, int> {
   static bool Is(const ValueType &v) { return v.IsInt(); }
   static int Get(const ValueType &v) { return v.GetInt(); }
   static ValueType &Set(ValueType &v, int data) { return v.SetInt(data); }
@@ -524,7 +533,8 @@ template <typename ValueType> struct TypeHelper<ValueType, int> {
   }
 };
 
-template <typename ValueType> struct TypeHelper<ValueType, unsigned> {
+template <typename ValueType>
+struct TypeHelper<ValueType, unsigned> {
   static bool Is(const ValueType &v) { return v.IsUint(); }
   static unsigned Get(const ValueType &v) { return v.GetUint(); }
   static ValueType &Set(ValueType &v, unsigned data) { return v.SetUint(data); }
@@ -536,7 +546,8 @@ template <typename ValueType> struct TypeHelper<ValueType, unsigned> {
 
 #ifdef _MSC_VER
 RAPIDJSON_STATIC_ASSERT(sizeof(long) == sizeof(int));
-template <typename ValueType> struct TypeHelper<ValueType, long> {
+template <typename ValueType>
+struct TypeHelper<ValueType, long> {
   static bool Is(const ValueType &v) { return v.IsInt(); }
   static long Get(const ValueType &v) { return v.GetInt(); }
   static ValueType &Set(ValueType &v, long data) { return v.SetInt(data); }
@@ -547,7 +558,8 @@ template <typename ValueType> struct TypeHelper<ValueType, long> {
 };
 
 RAPIDJSON_STATIC_ASSERT(sizeof(unsigned long) == sizeof(unsigned));
-template <typename ValueType> struct TypeHelper<ValueType, unsigned long> {
+template <typename ValueType>
+struct TypeHelper<ValueType, unsigned long> {
   static bool Is(const ValueType &v) { return v.IsUint(); }
   static unsigned long Get(const ValueType &v) { return v.GetUint(); }
   static ValueType &Set(ValueType &v, unsigned long data) {
@@ -560,7 +572,8 @@ template <typename ValueType> struct TypeHelper<ValueType, unsigned long> {
 };
 #endif
 
-template <typename ValueType> struct TypeHelper<ValueType, int64_t> {
+template <typename ValueType>
+struct TypeHelper<ValueType, int64_t> {
   static bool Is(const ValueType &v) { return v.IsInt64(); }
   static int64_t Get(const ValueType &v) { return v.GetInt64(); }
   static ValueType &Set(ValueType &v, int64_t data) { return v.SetInt64(data); }
@@ -570,7 +583,8 @@ template <typename ValueType> struct TypeHelper<ValueType, int64_t> {
   }
 };
 
-template <typename ValueType> struct TypeHelper<ValueType, uint64_t> {
+template <typename ValueType>
+struct TypeHelper<ValueType, uint64_t> {
   static bool Is(const ValueType &v) { return v.IsUint64(); }
   static uint64_t Get(const ValueType &v) { return v.GetUint64(); }
   static ValueType &Set(ValueType &v, uint64_t data) {
@@ -582,7 +596,8 @@ template <typename ValueType> struct TypeHelper<ValueType, uint64_t> {
   }
 };
 
-template <typename ValueType> struct TypeHelper<ValueType, double> {
+template <typename ValueType>
+struct TypeHelper<ValueType, double> {
   static bool Is(const ValueType &v) { return v.IsDouble(); }
   static double Get(const ValueType &v) { return v.GetDouble(); }
   static ValueType &Set(ValueType &v, double data) { return v.SetDouble(data); }
@@ -592,7 +607,8 @@ template <typename ValueType> struct TypeHelper<ValueType, double> {
   }
 };
 
-template <typename ValueType> struct TypeHelper<ValueType, float> {
+template <typename ValueType>
+struct TypeHelper<ValueType, float> {
   static bool Is(const ValueType &v) { return v.IsFloat(); }
   static float Get(const ValueType &v) { return v.GetFloat(); }
   static ValueType &Set(ValueType &v, float data) { return v.SetFloat(data); }
@@ -672,8 +688,10 @@ struct TypeHelper<ValueType, typename ValueType::ConstObject> {
 }  // namespace internal
 
 // Forward declarations
-template <bool, typename> class GenericArray;
-template <bool, typename> class GenericObject;
+template <bool, typename>
+class GenericArray;
+template <bool, typename>
+class GenericObject;
 
 ///////////////////////////////////////////////////////////////////////////////
 // GenericValue
@@ -739,8 +757,8 @@ private:
 
   //! Move assignment from a GenericDocument is not permitted.
   template <typename StackAllocator>
-  GenericValue &
-  operator=(GenericDocument<Encoding, Allocator, StackAllocator> &&rhs);
+  GenericValue &operator=(
+      GenericDocument<Encoding, Allocator, StackAllocator> &&rhs);
 #endif
 
  public:
@@ -757,8 +775,7 @@ public:
     data_.f.flags = defaultFlags[type];
 
     // Use ShortString to store empty string.
-    if (type == kStringType)
-      data_.ss.SetLength(0);
+    if (type == kStringType) data_.ss.SetLength(0);
   }
 
   //! Explicit copy constructor (with allocator)
@@ -776,12 +793,13 @@ public:
     switch (rhs.GetType()) {
       case kObjectType: {
         SizeType count = rhs.data_.o.size;
-      Member *lm =
-          reinterpret_cast<Member *>(allocator.Malloc(count * sizeof(Member)));
+        Member *lm = reinterpret_cast<Member *>(
+            allocator.Malloc(count * sizeof(Member)));
         const typename GenericValue<Encoding, SourceAllocator>::Member *rm =
             rhs.GetMembersPointer();
         for (SizeType i = 0; i < count; i++) {
-        new (&lm[i].name) GenericValue(rm[i].name, allocator, copyConstStrings);
+          new (&lm[i].name)
+              GenericValue(rm[i].name, allocator, copyConstStrings);
           new (&lm[i].value)
               GenericValue(rm[i].value, allocator, copyConstStrings);
         }
@@ -1091,13 +1109,11 @@ public:
   template <typename SourceAllocator>
   bool operator==(const GenericValue<Encoding, SourceAllocator> &rhs) const {
     typedef GenericValue<Encoding, SourceAllocator> RhsType;
-    if (GetType() != rhs.GetType())
-      return false;
+    if (GetType() != rhs.GetType()) return false;
 
     switch (GetType()) {
       case kObjectType:  // Warning: O(n^2) inner-loop
-      if (data_.o.size != rhs.data_.o.size)
-        return false;
+        if (data_.o.size != rhs.data_.o.size) return false;
         for (ConstMemberIterator lhsMemberItr = MemberBegin();
              lhsMemberItr != MemberEnd(); ++lhsMemberItr) {
           typename RhsType::ConstMemberIterator rhsMemberItr =
@@ -1109,11 +1125,9 @@ public:
         return true;
 
       case kArrayType:
-      if (data_.a.size != rhs.data_.a.size)
-        return false;
+        if (data_.a.size != rhs.data_.a.size) return false;
         for (SizeType i = 0; i < data_.a.size; i++)
-        if ((*this)[i] != rhs[i])
-          return false;
+          if ((*this)[i] != rhs[i]) return false;
         return true;
 
       case kStringType:
@@ -1218,8 +1232,7 @@ public:
 
   // Checks whether a number can be losslessly converted to a double.
   bool IsLosslessDouble() const {
-    if (!IsNumber())
-      return false;
+    if (!IsNumber()) return false;
     if (IsUint64()) {
       uint64_t u = GetUint64();
       volatile double d = static_cast<double>(u);
@@ -1241,15 +1254,13 @@ public:
 
   // Checks whether a number is a float (possible lossy).
   bool IsFloat() const {
-    if ((data_.f.flags & kDoubleFlag) == 0)
-      return false;
+    if ((data_.f.flags & kDoubleFlag) == 0) return false;
     double d = GetDouble();
     return d >= -3.4028234e38 && d <= 3.4028234e38;
   }
   // Checks whether a number can be losslessly converted to a float.
   bool IsLosslessFloat() const {
-    if (!IsNumber())
-      return false;
+    if (!IsNumber()) return false;
     double a = GetDouble();
     if (a < static_cast<double>(-(std::numeric_limits<float>::max)()) ||
         a > static_cast<double>((std::numeric_limits<float>::max)()))
@@ -1355,8 +1366,8 @@ public:
       \note Linear time complexity.
   */
   template <typename SourceAllocator>
-  GenericValue &
-  operator[](const GenericValue<Encoding, SourceAllocator> &name) {
+  GenericValue &operator[](
+      const GenericValue<Encoding, SourceAllocator> &name) {
     MemberIterator member = FindMember(name);
     if (member != MemberEnd())
       return member->value;
@@ -1373,8 +1384,8 @@ public:
     }
   }
   template <typename SourceAllocator>
-  const GenericValue &
-  operator[](const GenericValue<Encoding, SourceAllocator> &name) const {
+  const GenericValue &operator[](
+      const GenericValue<Encoding, SourceAllocator> &name) const {
     return const_cast<GenericValue &>(*this)[name];
   }
 
@@ -1505,19 +1516,18 @@ public:
       \note Linear time complexity.
   */
   template <typename SourceAllocator>
-  MemberIterator
-  FindMember(const GenericValue<Encoding, SourceAllocator> &name) {
+  MemberIterator FindMember(
+      const GenericValue<Encoding, SourceAllocator> &name) {
     RAPIDJSON_ASSERT(IsObject());
     RAPIDJSON_ASSERT(name.IsString());
     MemberIterator member = MemberBegin();
     for (; member != MemberEnd(); ++member)
-      if (name.StringEqual(member->name))
-        break;
+      if (name.StringEqual(member->name)) break;
     return member;
   }
   template <typename SourceAllocator>
-  ConstMemberIterator
-  FindMember(const GenericValue<Encoding, SourceAllocator> &name) const {
+  ConstMemberIterator FindMember(
+      const GenericValue<Encoding, SourceAllocator> &name) const {
     return const_cast<GenericValue &>(*this).FindMember(name);
   }
 
@@ -1707,8 +1717,7 @@ public:
   */
   void RemoveAllMembers() {
     RAPIDJSON_ASSERT(IsObject());
-    for (MemberIterator m = MemberBegin(); m != MemberEnd(); ++m)
-      m->~Member();
+    for (MemberIterator m = MemberBegin(); m != MemberEnd(); ++m) m->~Member();
     data_.o.size = 0;
   }
 
@@ -1796,8 +1805,7 @@ public:
     RAPIDJSON_ASSERT(last <= MemberEnd());
 
     MemberIterator pos = MemberBegin() + (first - MemberBegin());
-    for (MemberIterator itr = pos; itr != last; ++itr)
-      itr->~Member();
+    for (MemberIterator itr = pos; itr != last; ++itr) itr->~Member();
     std::memmove(static_cast<void *>(&*pos), &*last,
                  static_cast<size_t>(MemberEnd() - last) * sizeof(Member));
     data_.o.size -= static_cast<SizeType>(last - first);
@@ -1877,8 +1885,7 @@ public:
   void Clear() {
     RAPIDJSON_ASSERT(IsArray());
     GenericValue *e = GetElementsPointer();
-    for (GenericValue *v = e; v != e + data_.a.size; ++v)
-      v->~GenericValue();
+    for (GenericValue *v = e; v != e + data_.a.size; ++v) v->~GenericValue();
     data_.a.size = 0;
   }
 
@@ -2040,8 +2047,7 @@ public:
     RAPIDJSON_ASSERT(first <= last);
     RAPIDJSON_ASSERT(last <= End());
     ValueIterator pos = Begin() + (first - Begin());
-    for (ValueIterator itr = pos; itr != last; ++itr)
-      itr->~GenericValue();
+    for (ValueIterator itr = pos; itr != last; ++itr) itr->~GenericValue();
     std::memmove(static_cast<void *>(pos), last,
                  static_cast<size_t>(End() - last) * sizeof(GenericValue));
     data_.a.size -= static_cast<SizeType>(last - first);
@@ -2087,8 +2093,7 @@ public:
     RAPIDJSON_ASSERT(IsNumber());
     if ((data_.f.flags & kDoubleFlag) != 0)
       return data_.n.d;  // exact type, no conversion.
-    if ((data_.f.flags & kIntFlag) != 0)
-      return data_.n.i.i; // int -> double
+    if ((data_.f.flags & kIntFlag) != 0) return data_.n.i.i;  // int -> double
     if ((data_.f.flags & kUintFlag) != 0)
       return data_.n.u.u;  // unsigned -> double
     if ((data_.f.flags & kInt64Flag) != 0)
@@ -2243,19 +2248,23 @@ public:
       \tparam T Either \c bool, \c int, \c unsigned, \c int64_t, \c uint64_t, \c
      double, \c float, \c const \c char*, \c std::basic_string<Ch>
   */
-  template <typename T> bool Is() const {
+  template <typename T>
+  bool Is() const {
     return internal::TypeHelper<ValueType, T>::Is(*this);
   }
 
-  template <typename T> T Get() const {
+  template <typename T>
+  T Get() const {
     return internal::TypeHelper<ValueType, T>::Get(*this);
   }
 
-  template <typename T> T Get() {
+  template <typename T>
+  T Get() {
     return internal::TypeHelper<ValueType, T>::Get(*this);
   }
 
-  template <typename T> ValueType &Set(const T &data) {
+  template <typename T>
+  ValueType &Set(const T &data) {
     return internal::TypeHelper<ValueType, T>::Set(*this, data);
   }
 
@@ -2273,7 +2282,8 @@ public:
      GenericDocument, which is also a Handler. \tparam Handler type of handler.
       \param handler An object implementing concept Handler.
   */
-  template <typename Handler> bool Accept(Handler &handler) const {
+  template <typename Handler>
+  bool Accept(Handler &handler) const {
     switch (GetType()) {
       case kNullType:
         return handler.Null();
@@ -2283,8 +2293,7 @@ public:
         return handler.Bool(true);
 
       case kObjectType:
-      if (RAPIDJSON_UNLIKELY(!handler.StartObject()))
-        return false;
+        if (RAPIDJSON_UNLIKELY(!handler.StartObject())) return false;
         for (ConstMemberIterator m = MemberBegin(); m != MemberEnd(); ++m) {
           RAPIDJSON_ASSERT(m->name.IsString());  // User may change the type of
                                                  // name by MemberIterator.
@@ -2292,17 +2301,14 @@ public:
                   !handler.Key(m->name.GetString(), m->name.GetStringLength(),
                                (m->name.data_.f.flags & kCopyFlag) != 0)))
             return false;
-        if (RAPIDJSON_UNLIKELY(!m->value.Accept(handler)))
-          return false;
+          if (RAPIDJSON_UNLIKELY(!m->value.Accept(handler))) return false;
         }
         return handler.EndObject(data_.o.size);
 
       case kArrayType:
-      if (RAPIDJSON_UNLIKELY(!handler.StartArray()))
-        return false;
+        if (RAPIDJSON_UNLIKELY(!handler.StartArray())) return false;
         for (const GenericValue *v = Begin(); v != End(); ++v)
-        if (RAPIDJSON_UNLIKELY(!v->Accept(handler)))
-          return false;
+          if (RAPIDJSON_UNLIKELY(!v->Accept(handler))) return false;
         return handler.EndArray(data_.a.size);
 
       case kStringType:
@@ -2325,8 +2331,10 @@ public:
   }
 
  private:
-  template <typename, typename> friend class GenericValue;
-  template <typename, typename, typename> friend class GenericDocument;
+  template <typename, typename>
+  friend class GenericValue;
+  template <typename, typename, typename>
+  friend class GenericDocument;
 
   enum {
     kBoolFlag = 0x0008,
@@ -2469,8 +2477,8 @@ private:
   RAPIDJSON_FORCEINLINE GenericValue *GetElementsPointer() const {
     return RAPIDJSON_GETPOINTER(GenericValue, data_.a.elements);
   }
-  RAPIDJSON_FORCEINLINE GenericValue *
-  SetElementsPointer(GenericValue *elements) {
+  RAPIDJSON_FORCEINLINE GenericValue *SetElementsPointer(
+      GenericValue *elements) {
     return RAPIDJSON_SETPOINTER(GenericValue, data_.a.elements, elements);
   }
   RAPIDJSON_FORCEINLINE Member *GetMembersPointer() const {
@@ -2600,11 +2608,12 @@ public:
   explicit GenericDocument(Type type, Allocator *allocator = 0,
                            size_t stackCapacity = kDefaultStackCapacity,
                            StackAllocator *stackAllocator = 0)
-      : GenericValue<Encoding, Allocator>(type), allocator_(allocator),
-        ownAllocator_(0), stack_(stackAllocator, stackCapacity),
+      : GenericValue<Encoding, Allocator>(type),
+        allocator_(allocator),
+        ownAllocator_(0),
+        stack_(stackAllocator, stackCapacity),
         parseResult_() {
-    if (!allocator_)
-      ownAllocator_ = allocator_ = RAPIDJSON_NEW(Allocator)();
+    if (!allocator_) ownAllocator_ = allocator_ = RAPIDJSON_NEW(Allocator)();
   }
 
   //! Constructor
@@ -2617,10 +2626,11 @@ public:
   GenericDocument(Allocator *allocator = 0,
                   size_t stackCapacity = kDefaultStackCapacity,
                   StackAllocator *stackAllocator = 0)
-      : allocator_(allocator), ownAllocator_(0),
-        stack_(stackAllocator, stackCapacity), parseResult_() {
-    if (!allocator_)
-      ownAllocator_ = allocator_ = RAPIDJSON_NEW(Allocator)();
+      : allocator_(allocator),
+        ownAllocator_(0),
+        stack_(stackAllocator, stackCapacity),
+        parseResult_() {
+    if (!allocator_) ownAllocator_ = allocator_ = RAPIDJSON_NEW(Allocator)();
   }
 
 #if RAPIDJSON_HAS_CXX11_RVALUE_REFS
@@ -2702,7 +2712,8 @@ public:
       \param g Generator functor which sends SAX events to the parameter.
       \return The document itself for fluent API.
   */
-  template <typename Generator> GenericDocument &Populate(Generator &g) {
+  template <typename Generator>
+  GenericDocument &Populate(Generator &g) {
     ClearStackOnExit scope(*this);
     if (g(*this)) {
       RAPIDJSON_ASSERT(stack_.GetSize() ==
@@ -2768,7 +2779,8 @@ public:
       \param str Mutable zero-terminated string to be parsed.
       \return The document itself for fluent API.
   */
-  template <unsigned parseFlags> GenericDocument &ParseInsitu(Ch *str) {
+  template <unsigned parseFlags>
+  GenericDocument &ParseInsitu(Ch *str) {
     GenericInsituStringStream<Encoding> s(str);
     return ParseStream<parseFlags | kParseInsituFlag>(s);
   }
@@ -2802,7 +2814,8 @@ public:
      kParseInsituFlag). \param str Read-only zero-terminated string to be
      parsed.
   */
-  template <unsigned parseFlags> GenericDocument &Parse(const Ch *str) {
+  template <unsigned parseFlags>
+  GenericDocument &Parse(const Ch *str) {
     return Parse<parseFlags, Encoding>(str);
   }
 
@@ -2835,8 +2848,8 @@ public:
 
 #if RAPIDJSON_HAS_STDSTRING
   template <unsigned parseFlags, typename SourceEncoding>
-  GenericDocument &
-  Parse(const std::basic_string<typename SourceEncoding::Ch> &str) {
+  GenericDocument &Parse(
+      const std::basic_string<typename SourceEncoding::Ch> &str) {
     // c_str() is constant complexity according to standard. Should be faster
     // than Parse(const char*, size_t)
     return Parse<parseFlags, SourceEncoding>(str.c_str());
@@ -2905,7 +2918,8 @@ private:
   // callers of the following private Handler functions
   // template <typename,typename,typename> friend class GenericReader; // for
   // parsing
-  template <typename, typename> friend class GenericValue; // for deep copying
+  template <typename, typename>
+  friend class GenericValue;  // for deep copying
 
  public:
   // Implementation of Handler
@@ -3020,7 +3034,8 @@ typedef GenericDocument<UTF8<>> Document;
     In addition to all APIs for array type, it provides range-based for loop if
    \c RAPIDJSON_HAS_CXX11_RANGE_FOR=1.
 */
-template <bool Const, typename ValueT> class GenericArray {
+template <bool Const, typename ValueT>
+class GenericArray {
  public:
   typedef GenericArray<true, ValueT> ConstArray;
   typedef GenericArray<false, ValueT> Array;
@@ -3031,7 +3046,8 @@ public:
   typedef typename ValueType::AllocatorType AllocatorType;
   typedef typename ValueType::StringRefType StringRefType;
 
-  template <typename, typename> friend class GenericValue;
+  template <typename, typename>
+  friend class GenericValue;
 
   GenericArray(const GenericArray &rhs) : value_(rhs.value_) {}
   GenericArray &operator=(const GenericArray &rhs) {
@@ -3101,7 +3117,8 @@ private:
     In addition to all APIs for array type, it provides range-based for loop if
    \c RAPIDJSON_HAS_CXX11_RANGE_FOR=1.
 */
-template <bool Const, typename ValueT> class GenericObject {
+template <bool Const, typename ValueT>
+class GenericObject {
  public:
   typedef GenericObject<true, ValueT> ConstObject;
   typedef GenericObject<false, ValueT> Object;
@@ -3118,7 +3135,8 @@ public:
   typedef typename ValueType::EncodingType EncodingType;
   typedef typename ValueType::Ch Ch;
 
-  template <typename, typename> friend class GenericValue;
+  template <typename, typename>
+  friend class GenericValue;
 
   GenericObject(const GenericObject &rhs) : value_(rhs.value_) {}
   GenericObject &operator=(const GenericObject &rhs) {
@@ -3130,12 +3148,13 @@ public:
   SizeType MemberCount() const { return value_.MemberCount(); }
   SizeType MemberCapacity() const { return value_.MemberCapacity(); }
   bool ObjectEmpty() const { return value_.ObjectEmpty(); }
-  template <typename T> ValueType &operator[](T *name) const {
+  template <typename T>
+  ValueType &operator[](T *name) const {
     return value_[name];
   }
   template <typename SourceAllocator>
-  ValueType &
-  operator[](const GenericValue<EncodingType, SourceAllocator> &name) const {
+  ValueType &operator[](
+      const GenericValue<EncodingType, SourceAllocator> &name) const {
     return value_[name];
   }
 #if RAPIDJSON_HAS_STDSTRING
@@ -3157,16 +3176,16 @@ public:
   }
 #endif
   template <typename SourceAllocator>
-  bool
-  HasMember(const GenericValue<EncodingType, SourceAllocator> &name) const {
+  bool HasMember(
+      const GenericValue<EncodingType, SourceAllocator> &name) const {
     return value_.HasMember(name);
   }
   MemberIterator FindMember(const Ch *name) const {
     return value_.FindMember(name);
   }
   template <typename SourceAllocator>
-  MemberIterator
-  FindMember(const GenericValue<EncodingType, SourceAllocator> &name) const {
+  MemberIterator FindMember(
+      const GenericValue<EncodingType, SourceAllocator> &name) const {
     return value_.FindMember(name);
   }
 #if RAPIDJSON_HAS_STDSTRING
@@ -3247,8 +3266,8 @@ public:
   }
 #endif
   template <typename SourceAllocator>
-  bool
-  RemoveMember(const GenericValue<EncodingType, SourceAllocator> &name) const {
+  bool RemoveMember(
+      const GenericValue<EncodingType, SourceAllocator> &name) const {
     return value_.RemoveMember(name);
   }
   MemberIterator RemoveMember(MemberIterator m) const {
@@ -3268,8 +3287,8 @@ public:
   }
 #endif
   template <typename SourceAllocator>
-  bool
-  EraseMember(const GenericValue<EncodingType, SourceAllocator> &name) const {
+  bool EraseMember(
+      const GenericValue<EncodingType, SourceAllocator> &name) const {
     return value_.EraseMember(name);
   }
 

livox_ros_driver/common/rapidjson/encodedstream.h

@@ -80,17 +80,15 @@ private:
 };
 
 //! Specialized for UTF8 MemoryStream.
-template <> class EncodedInputStream<UTF8<>, MemoryStream> {
+template <>
+class EncodedInputStream<UTF8<>, MemoryStream> {
  public:
   typedef UTF8<>::Ch Ch;
 
   EncodedInputStream(MemoryStream &is) : is_(is) {
-    if (static_cast<unsigned char>(is_.Peek()) == 0xEFu)
-      is_.Take();
-    if (static_cast<unsigned char>(is_.Peek()) == 0xBBu)
-      is_.Take();
-    if (static_cast<unsigned char>(is_.Peek()) == 0xBFu)
-      is_.Take();
+    if (static_cast<unsigned char>(is_.Peek()) == 0xEFu) is_.Take();
+    if (static_cast<unsigned char>(is_.Peek()) == 0xBBu) is_.Take();
+    if (static_cast<unsigned char>(is_.Peek()) == 0xBFu) is_.Take();
   }
   Ch Peek() const { return is_.Peek(); }
   Ch Take() { return is_.Take(); }
@@ -123,8 +121,7 @@ public:
   typedef typename Encoding::Ch Ch;
 
   EncodedOutputStream(OutputByteStream &os, bool putBOM = true) : os_(os) {
-    if (putBOM)
-      Encoding::PutBOM(os_);
+    if (putBOM) Encoding::PutBOM(os_);
   }
 
   void Put(Ch c) { Encoding::Put(os_, c); }
@@ -227,8 +224,7 @@ private:
 
     const unsigned char *c =
         reinterpret_cast<const unsigned char *>(is_->Peek4());
-    if (!c)
-      return;
+    if (!c) return;
 
     unsigned bom =
         static_cast<unsigned>(c[0] | (c[1] << 8) | (c[2] << 16) | (c[3] << 24));
@@ -349,8 +345,7 @@ public:
     static const PutFunc f[] = {RAPIDJSON_ENCODINGS_FUNC(Put)};
     putFunc_ = f[type_];
 
-    if (putBOM)
-      PutBOM();
+    if (putBOM) PutBOM();
   }
 
   UTFType GetType() const { return type_; }

livox_ros_driver/common/rapidjson/encodings.h

@@ -98,7 +98,8 @@ actually decode it. template <typename InputStream, typename OutputStream>
     \tparam CharType Code unit for storing 8-bit UTF-8 data. Default is char.
     \note implements Encoding concept
 */
-template <typename CharType = char> struct UTF8 {
+template <typename CharType = char>
+struct UTF8 {
   typedef CharType Ch;
 
   enum { supportUnicode = 1 };
@@ -219,8 +220,7 @@ template <typename CharType = char> struct UTF8 {
   RAPIDJSON_TRANS(0x70)
     Ch c;
     RAPIDJSON_COPY();
-    if (!(c & 0x80))
-      return true;
+    if (!(c & 0x80)) return true;
 
     bool result = true;
     switch (GetRange(static_cast<unsigned char>(c))) {
@@ -301,19 +301,17 @@ template <typename CharType = char> struct UTF8 {
   static CharType TakeBOM(InputByteStream &is) {
     RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
     typename InputByteStream::Ch c = Take(is);
-    if (static_cast<unsigned char>(c) != 0xEFu)
-      return c;
+    if (static_cast<unsigned char>(c) != 0xEFu) return c;
     c = is.Take();
-    if (static_cast<unsigned char>(c) != 0xBBu)
-      return c;
+    if (static_cast<unsigned char>(c) != 0xBBu) return c;
     c = is.Take();
-    if (static_cast<unsigned char>(c) != 0xBFu)
-      return c;
+    if (static_cast<unsigned char>(c) != 0xBFu) return c;
     c = is.Take();
     return c;
   }
 
-  template <typename InputByteStream> static Ch Take(InputByteStream &is) {
+  template <typename InputByteStream>
+  static Ch Take(InputByteStream &is) {
     RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
     return static_cast<Ch>(is.Take());
   }
@@ -346,7 +344,8 @@ template <typename CharType = char> struct UTF8 {
    and code points are represented by CPU's endianness. For streaming, use
    UTF16LE and UTF16BE, which handle endianness.
 */
-template <typename CharType = wchar_t> struct UTF16 {
+template <typename CharType = wchar_t>
+struct UTF16 {
   typedef CharType Ch;
   RAPIDJSON_STATIC_ASSERT(sizeof(Ch) >= 2);
 
@@ -419,7 +418,8 @@ template <typename CharType = wchar_t> struct UTF16 {
 };
 
 //! UTF-16 little endian encoding.
-template <typename CharType = wchar_t> struct UTF16LE : UTF16<CharType> {
+template <typename CharType = wchar_t>
+struct UTF16LE : UTF16<CharType> {
   template <typename InputByteStream>
   static CharType TakeBOM(InputByteStream &is) {
     RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
@@ -453,7 +453,8 @@ template <typename CharType = wchar_t> struct UTF16LE : UTF16<CharType> {
 };
 
 //! UTF-16 big endian encoding.
-template <typename CharType = wchar_t> struct UTF16BE : UTF16<CharType> {
+template <typename CharType = wchar_t>
+struct UTF16BE : UTF16<CharType> {
   template <typename InputByteStream>
   static CharType TakeBOM(InputByteStream &is) {
     RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
@@ -498,7 +499,8 @@ template <typename CharType = wchar_t> struct UTF16BE : UTF16<CharType> {
    and code points are represented by CPU's endianness. For streaming, use
    UTF32LE and UTF32BE, which handle endianness.
 */
-template <typename CharType = unsigned> struct UTF32 {
+template <typename CharType = unsigned>
+struct UTF32 {
   typedef CharType Ch;
   RAPIDJSON_STATIC_ASSERT(sizeof(Ch) >= 4);
 
@@ -536,7 +538,8 @@ template <typename CharType = unsigned> struct UTF32 {
 };
 
 //! UTF-32 little endian enocoding.
-template <typename CharType = unsigned> struct UTF32LE : UTF32<CharType> {
+template <typename CharType = unsigned>
+struct UTF32LE : UTF32<CharType> {
   template <typename InputByteStream>
   static CharType TakeBOM(InputByteStream &is) {
     RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
@@ -574,7 +577,8 @@ template <typename CharType = unsigned> struct UTF32LE : UTF32<CharType> {
 };
 
 //! UTF-32 big endian encoding.
-template <typename CharType = unsigned> struct UTF32BE : UTF32<CharType> {
+template <typename CharType = unsigned>
+struct UTF32BE : UTF32<CharType> {
   template <typename InputByteStream>
   static CharType TakeBOM(InputByteStream &is) {
     RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
@@ -619,7 +623,8 @@ template <typename CharType = unsigned> struct UTF32BE : UTF32<CharType> {
     \tparam CharType Code unit for storing 7-bit ASCII data. Default is char.
     \note implements Encoding concept
 */
-template <typename CharType = char> struct ASCII {
+template <typename CharType = char>
+struct ASCII {
   typedef CharType Ch;
 
   enum { supportUnicode = 0 };
@@ -657,7 +662,8 @@ template <typename CharType = char> struct ASCII {
     return static_cast<Ch>(c);
   }
 
-  template <typename InputByteStream> static Ch Take(InputByteStream &is) {
+  template <typename InputByteStream>
+  static Ch Take(InputByteStream &is) {
     RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
     return static_cast<Ch>(is.Take());
   }
@@ -692,7 +698,8 @@ enum UTFType {
 /*! \note This class can be used with AutoUTFInputtStream and
  * AutoUTFOutputStream, which provides GetType().
  */
-template <typename CharType> struct AutoUTF {
+template <typename CharType>
+struct AutoUTF {
   typedef CharType Ch;
 
   enum { supportUnicode = 1 };
@@ -739,15 +746,15 @@ template <typename CharType> struct AutoUTF {
 // Transcoder
 
 //! Encoding conversion.
-template <typename SourceEncoding, typename TargetEncoding> struct Transcoder {
+template <typename SourceEncoding, typename TargetEncoding>
+struct Transcoder {
   //! Take one Unicode codepoint from source encoding, convert it to target
   //! encoding and put it to the output stream.
   template <typename InputStream, typename OutputStream>
   static RAPIDJSON_FORCEINLINE bool Transcode(InputStream &is,
                                               OutputStream &os) {
     unsigned codepoint;
-    if (!SourceEncoding::Decode(is, &codepoint))
-      return false;
+    if (!SourceEncoding::Decode(is, &codepoint)) return false;
     TargetEncoding::Encode(os, codepoint);
     return true;
   }
@@ -756,8 +763,7 @@ template <typename SourceEncoding, typename TargetEncoding> struct Transcoder {
   static RAPIDJSON_FORCEINLINE bool TranscodeUnsafe(InputStream &is,
                                                     OutputStream &os) {
     unsigned codepoint;
-    if (!SourceEncoding::Decode(is, &codepoint))
-      return false;
+    if (!SourceEncoding::Decode(is, &codepoint)) return false;
     TargetEncoding::EncodeUnsafe(os, codepoint);
     return true;
   }
@@ -776,7 +782,8 @@ template <typename Stream>
 inline void PutUnsafe(Stream &stream, typename Stream::Ch c);
 
 //! Specialization of Transcoder with same source and target encoding.
-template <typename Encoding> struct Transcoder<Encoding, Encoding> {
+template <typename Encoding>
+struct Transcoder<Encoding, Encoding> {
   template <typename InputStream, typename OutputStream>
   static RAPIDJSON_FORCEINLINE bool Transcode(InputStream &is,
                                               OutputStream &os) {

livox_ros_driver/common/rapidjson/error/en.h

@@ -37,8 +37,8 @@ RAPIDJSON_NAMESPACE_BEGIN
     \note User can make a copy of this function for localization.
         Using switch-case is safer for future modification of error codes.
 */
-inline const RAPIDJSON_ERROR_CHARTYPE *
-GetParseError_En(ParseErrorCode parseErrorCode) {
+inline const RAPIDJSON_ERROR_CHARTYPE* GetParseError_En(
+    ParseErrorCode parseErrorCode) {
   switch (parseErrorCode) {
     case kParseErrorNone:
       return RAPIDJSON_ERROR_STRING("No error.");

livox_ros_driver/common/rapidjson/error/error.h

@@ -77,15 +77,18 @@ enum ParseErrorCode {
   kParseErrorObjectMissName,   //!< Missing a name for object member.
   kParseErrorObjectMissColon,  //!< Missing a colon after a name of object
                                //!< member.
-  kParseErrorObjectMissCommaOrCurlyBracket, //!< Missing a comma or '}' after an
+  kParseErrorObjectMissCommaOrCurlyBracket,  //!< Missing a comma or '}' after
+                                             //!an
                                              //!< object member.
 
-  kParseErrorArrayMissCommaOrSquareBracket, //!< Missing a comma or ']' after an
+  kParseErrorArrayMissCommaOrSquareBracket,  //!< Missing a comma or ']' after
+                                             //!an
                                              //!< array element.
 
   kParseErrorStringUnicodeEscapeInvalidHex,  //!< Incorrect hex digit after \\u
                                              //!< escape in string.
-  kParseErrorStringUnicodeSurrogateInvalid, //!< The surrogate pair in string is
+  kParseErrorStringUnicodeSurrogateInvalid,  //!< The surrogate pair in string
+                                             //!is
                                              //!< invalid.
   kParseErrorStringEscapeInvalid,      //!< Invalid escape character in string.
   kParseErrorStringMissQuotationMark,  //!< Missing a closing quotation mark in

livox_ros_driver/common/rapidjson/filereadstream.h

@@ -19,8 +19,8 @@
 #ifndef RAPIDJSON_FILEREADSTREAM_H_
 #define RAPIDJSON_FILEREADSTREAM_H_
 
-#include "stream.h"
 #include <cstdio>
+#include "stream.h"
 
 #ifdef __clang__
 RAPIDJSON_DIAG_PUSH
@@ -46,8 +46,14 @@ public:
       \param bufferSize size of buffer in bytes. Must >=4 bytes.
   */
   FileReadStream(std::FILE *fp, char *buffer, size_t bufferSize)
-      : fp_(fp), buffer_(buffer), bufferSize_(bufferSize), bufferLast_(0),
-        current_(buffer_), readCount_(0), count_(0), eof_(false) {
+      : fp_(fp),
+        buffer_(buffer),
+        bufferSize_(bufferSize),
+        bufferLast_(0),
+        current_(buffer_),
+        readCount_(0),
+        count_(0),
+        eof_(false) {
     RAPIDJSON_ASSERT(fp_ != 0);
     RAPIDJSON_ASSERT(bufferSize >= 4);
     Read();

livox_ros_driver/common/rapidjson/filewritestream.h

@@ -19,8 +19,8 @@
 #ifndef RAPIDJSON_FILEWRITESTREAM_H_
 #define RAPIDJSON_FILEWRITESTREAM_H_
 
-#include "stream.h"
 #include <cstdio>
+#include "stream.h"
 
 #ifdef __clang__
 RAPIDJSON_DIAG_PUSH
@@ -38,14 +38,15 @@ public:
   typedef char Ch;  //!< Character type. Only support char.
 
   FileWriteStream(std::FILE *fp, char *buffer, size_t bufferSize)
-      : fp_(fp), buffer_(buffer), bufferEnd_(buffer + bufferSize),
+      : fp_(fp),
+        buffer_(buffer),
+        bufferEnd_(buffer + bufferSize),
         current_(buffer_) {
     RAPIDJSON_ASSERT(fp_ != 0);
   }
 
   void Put(char c) {
-    if (current_ >= bufferEnd_)
-      Flush();
+    if (current_ >= bufferEnd_) Flush();
 
     *current_++ = c;
   }
@@ -113,7 +114,8 @@ private:
 
 //! Implement specialized version of PutN() with memset() for better
 //! performance.
-template <> inline void PutN(FileWriteStream &stream, char c, size_t n) {
+template <>
+inline void PutN(FileWriteStream &stream, char c, size_t n) {
   stream.PutN(c, n);
 }
 

livox_ros_driver/common/rapidjson/fwd.h

@@ -25,37 +25,51 @@ RAPIDJSON_NAMESPACE_BEGIN
 
 // encodings.h
 
-template <typename CharType> struct UTF8;
-template <typename CharType> struct UTF16;
-template <typename CharType> struct UTF16BE;
-template <typename CharType> struct UTF16LE;
-template <typename CharType> struct UTF32;
-template <typename CharType> struct UTF32BE;
-template <typename CharType> struct UTF32LE;
-template <typename CharType> struct ASCII;
-template <typename CharType> struct AutoUTF;
+template <typename CharType>
+struct UTF8;
+template <typename CharType>
+struct UTF16;
+template <typename CharType>
+struct UTF16BE;
+template <typename CharType>
+struct UTF16LE;
+template <typename CharType>
+struct UTF32;
+template <typename CharType>
+struct UTF32BE;
+template <typename CharType>
+struct UTF32LE;
+template <typename CharType>
+struct ASCII;
+template <typename CharType>
+struct AutoUTF;
 
-template <typename SourceEncoding, typename TargetEncoding> struct Transcoder;
+template <typename SourceEncoding, typename TargetEncoding>
+struct Transcoder;
 
 // allocators.h
 
 class CrtAllocator;
 
-template <typename BaseAllocator> class MemoryPoolAllocator;
+template <typename BaseAllocator>
+class MemoryPoolAllocator;
 
 // stream.h
 
-template <typename Encoding> struct GenericStringStream;
+template <typename Encoding>
+struct GenericStringStream;
 
 typedef GenericStringStream<UTF8<char>> StringStream;
 
-template <typename Encoding> struct GenericInsituStringStream;
+template <typename Encoding>
+struct GenericInsituStringStream;
 
 typedef GenericInsituStringStream<UTF8<char>> InsituStringStream;
 
 // stringbuffer.h
 
-template <typename Encoding, typename Allocator> class GenericStringBuffer;
+template <typename Encoding, typename Allocator>
+class GenericStringBuffer;
 
 typedef GenericStringBuffer<UTF8<char>, CrtAllocator> StringBuffer;
 
@@ -69,7 +83,8 @@ class FileWriteStream;
 
 // memorybuffer.h
 
-template <typename Allocator> struct GenericMemoryBuffer;
+template <typename Allocator>
+struct GenericMemoryBuffer;
 
 typedef GenericMemoryBuffer<CrtAllocator> MemoryBuffer;
 
@@ -79,7 +94,8 @@ struct MemoryStream;
 
 // reader.h
 
-template <typename Encoding, typename Derived> struct BaseReaderHandler;
+template <typename Encoding, typename Derived>
+struct BaseReaderHandler;
 
 template <typename SourceEncoding, typename TargetEncoding,
           typename StackAllocator>
@@ -101,14 +117,17 @@ class PrettyWriter;
 
 // document.h
 
-template <typename Encoding, typename Allocator> class GenericMember;
+template <typename Encoding, typename Allocator>
+class GenericMember;
 
 template <bool Const, typename Encoding, typename Allocator>
 class GenericMemberIterator;
 
-template <typename CharType> struct GenericStringRef;
+template <typename CharType>
+struct GenericStringRef;
 
-template <typename Encoding, typename Allocator> class GenericValue;
+template <typename Encoding, typename Allocator>
+class GenericValue;
 
 typedef GenericValue<UTF8<char>, MemoryPoolAllocator<CrtAllocator>> Value;
 
@@ -121,7 +140,8 @@ typedef GenericDocument<UTF8<char>, MemoryPoolAllocator<CrtAllocator>,
 
 // pointer.h
 
-template <typename ValueType, typename Allocator> class GenericPointer;
+template <typename ValueType, typename Allocator>
+class GenericPointer;
 
 typedef GenericPointer<Value, CrtAllocator> Pointer;
 
@@ -130,7 +150,8 @@ typedef GenericPointer<Value, CrtAllocator> Pointer;
 template <typename SchemaDocumentType>
 class IGenericRemoteSchemaDocumentProvider;
 
-template <typename ValueT, typename Allocator> class GenericSchemaDocument;
+template <typename ValueT, typename Allocator>
+class GenericSchemaDocument;
 
 typedef GenericSchemaDocument<Value, CrtAllocator> SchemaDocument;
 typedef IGenericRemoteSchemaDocumentProvider<SchemaDocument>

livox_ros_driver/common/rapidjson/internal/biginteger.h

@@ -51,8 +51,7 @@ public:
       i += kMaxDigitPerIteration;
     }
 
-    if (length > 0)
-      AppendDecimal64(decimals + i, decimals + i + length);
+    if (length > 0) AppendDecimal64(decimals + i, decimals + i + length);
   }
 
   BigInteger &operator=(const BigInteger &rhs) {
@@ -73,26 +72,21 @@ public:
     Type backup = digits_[0];
     digits_[0] += u;
     for (size_t i = 0; i < count_ - 1; i++) {
-      if (digits_[i] >= backup)
-        return *this; // no carry
+      if (digits_[i] >= backup) return *this;  // no carry
       backup = digits_[i + 1];
       digits_[i + 1] += 1;
     }
 
     // Last carry
-    if (digits_[count_ - 1] < backup)
-      PushBack(1);
+    if (digits_[count_ - 1] < backup) PushBack(1);
 
     return *this;
   }
 
   BigInteger &operator*=(uint64_t u) {
-    if (u == 0)
-      return *this = 0;
-    if (u == 1)
-      return *this;
-    if (*this == 1)
-      return *this = u;
+    if (u == 0) return *this = 0;
+    if (u == 1) return *this;
+    if (*this == 1) return *this = u;
 
     uint64_t k = 0;
     for (size_t i = 0; i < count_; i++) {
@@ -101,19 +95,15 @@ public:
       k = hi;
     }
 
-    if (k > 0)
-      PushBack(k);
+    if (k > 0) PushBack(k);
 
     return *this;
   }
 
   BigInteger &operator*=(uint32_t u) {
-    if (u == 0)
-      return *this = 0;
-    if (u == 1)
-      return *this;
-    if (*this == 1)
-      return *this = u;
+    if (u == 0) return *this = 0;
+    if (u == 1) return *this;
+    if (*this == 1) return *this = u;
 
     uint64_t k = 0;
     for (size_t i = 0; i < count_; i++) {
@@ -127,15 +117,13 @@ public:
       k = p1 >> 32;
     }
 
-    if (k > 0)
-      PushBack(k);
+    if (k > 0) PushBack(k);
 
     return *this;
   }
 
   BigInteger &operator<<=(size_t shift) {
-    if (IsZero() || shift == 0)
-      return *this;
+    if (IsZero() || shift == 0) return *this;
 
     size_t offset = shift / kTypeBit;
     size_t interShift = shift % kTypeBit;
@@ -151,8 +139,7 @@ public:
                               (digits_[i - 1] >> (kTypeBit - interShift));
       digits_[offset] = digits_[0] << interShift;
       count_ += offset;
-      if (digits_[count_])
-        count_++;
+      if (digits_[count_]) count_++;
     }
 
     std::memset(digits_, 0, offset * sizeof(Type));
@@ -183,14 +170,12 @@ public:
         5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5,
         5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5,
         5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5};
-    if (exp == 0)
-      return *this;
+    if (exp == 0) return *this;
     for (; exp >= 27; exp -= 27)
       *this *= RAPIDJSON_UINT64_C2(0X6765C793, 0XFA10079D);  // 5^27
     for (; exp >= 13; exp -= 13)
       *this *= static_cast<uint32_t>(1220703125u);  // 5^13
-    if (exp > 0)
-      *this *= kPow5[exp - 1];
+    if (exp > 0) *this *= kPow5[exp - 1];
     return *this;
   }
 
@@ -214,20 +199,17 @@ public:
     Type borrow = 0;
     for (size_t i = 0; i < a->count_; i++) {
       Type d = a->digits_[i] - borrow;
-      if (i < b->count_)
-        d -= b->digits_[i];
+      if (i < b->count_) d -= b->digits_[i];
       borrow = (d > a->digits_[i]) ? 1 : 0;
       out->digits_[i] = d;
-      if (d != 0)
-        out->count_ = i + 1;
+      if (d != 0) out->count_ = i + 1;
     }
 
     return ret;
   }
 
   int Compare(const BigInteger &rhs) const {
-    if (count_ != rhs.count_)
-      return count_ < rhs.count_ ? -1 : 1;
+    if (count_ != rhs.count_) return count_ < rhs.count_ ? -1 : 1;
 
     for (size_t i = count_; i-- > 0;)
       if (digits_[i] != rhs.digits_[i])
@@ -273,8 +255,7 @@ private:
                            uint64_t *outHigh) {
 #if defined(_MSC_VER) && defined(_M_AMD64)
     uint64_t low = _umul128(a, b, outHigh) + k;
-    if (low < k)
-      (*outHigh)++;
+    if (low < k) (*outHigh)++;
     return low;
 #elif (__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 6)) && \
     defined(__x86_64__)
@@ -289,14 +270,12 @@ private:
     uint64_t x0 = a0 * b0, x1 = a0 * b1, x2 = a1 * b0, x3 = a1 * b1;
     x1 += (x0 >> 32);  // can't give carry
     x1 += x2;
-    if (x1 < x2)
-      x3 += (static_cast<uint64_t>(1) << 32);
+    if (x1 < x2) x3 += (static_cast<uint64_t>(1) << 32);
     uint64_t lo = (x1 << 32) + (x0 & 0xFFFFFFFF);
     uint64_t hi = x3 + (x1 >> 32);
 
     lo += k;
-    if (lo < k)
-      hi++;
+    if (lo < k) hi++;
     *outHigh = hi;
     return lo;
 #endif

livox_ros_driver/common/rapidjson/internal/clzll.h

@@ -49,8 +49,7 @@ inline uint32_t clzll(uint64_t x) {
   _BitScanReverse64(&r, x);
 #else
   // Scan the high 32 bits.
-  if (_BitScanReverse(&r, static_cast<uint32_t>(x >> 32)))
-    return 63 - (r + 32);
+  if (_BitScanReverse(&r, static_cast<uint32_t>(x >> 32))) return 63 - (r + 32);
 
   // Scan the low 32 bits.
   _BitScanReverse(&r, static_cast<uint32_t>(x & 0xFFFFFFFF));

livox_ros_driver/common/rapidjson/internal/diyfp.h

@@ -23,9 +23,9 @@
 #ifndef RAPIDJSON_DIYFP_H_
 #define RAPIDJSON_DIYFP_H_
 
+#include <limits>
 #include "../rapidjson.h"
 #include "clzll.h"
-#include <limits>
 
 #if defined(_MSC_VER) && defined(_M_AMD64) && !defined(__INTEL_COMPILER)
 #include <intrin.h>
@@ -270,17 +270,15 @@ inline DiyFp GetCachedPowerByIndex(size_t index) {
 }
 
 inline DiyFp GetCachedPower(int e, int *K) {
-
   // int k = static_cast<int>(ceil((-61 - e) * 0.30102999566398114)) + 374;
   double dk = (-61 - e) * 0.30102999566398114 +
               347;  // dk must be positive, so can do ceiling in positive
   int k = static_cast<int>(dk);
-  if (dk - k > 0.0)
-    k++;
+  if (dk - k > 0.0) k++;
 
   unsigned index = static_cast<unsigned>((k >> 3) + 1);
-  *K = -(-348 +
-         static_cast<int>(index << 3)); // decimal exponent no need lookup table
+  *K = -(-348 + static_cast<int>(
+                    index << 3));  // decimal exponent no need lookup table
 
   return GetCachedPowerByIndex(index);
 }

livox_ros_driver/common/rapidjson/internal/dtoa.h

@@ -33,8 +33,7 @@ namespace internal {
 #ifdef __GNUC__
 RAPIDJSON_DIAG_PUSH
 RAPIDJSON_DIAG_OFF(effc++)
-RAPIDJSON_DIAG_OFF(array -
-                   bounds) // some gcc versions generate wrong warnings
+RAPIDJSON_DIAG_OFF(array - bounds)  // some gcc versions generate wrong warnings
 // https://gcc.gnu.org/bugzilla/show_bug.cgi?id=59124
 #endif
 
@@ -51,22 +50,14 @@ inline void GrisuRound(char *buffer, int len, uint64_t delta, uint64_t rest,
 inline int CountDecimalDigit32(uint32_t n) {
   // Simple pure C++ implementation was faster than __builtin_clz version in
   // this situation.
-  if (n < 10)
-    return 1;
-  if (n < 100)
-    return 2;
-  if (n < 1000)
-    return 3;
-  if (n < 10000)
-    return 4;
-  if (n < 100000)
-    return 5;
-  if (n < 1000000)
-    return 6;
-  if (n < 10000000)
-    return 7;
-  if (n < 100000000)
-    return 8;
+  if (n < 10) return 1;
+  if (n < 100) return 2;
+  if (n < 1000) return 3;
+  if (n < 10000) return 4;
+  if (n < 100000) return 5;
+  if (n < 1000000) return 6;
+  if (n < 10000000) return 7;
+  if (n < 100000000) return 8;
   // Will not reach 10 digits in DigitGen()
   // if (n < 1000000000) return 9;
   // return 10;
@@ -143,8 +134,7 @@ inline void DigitGen(const DiyFp &W, const DiyFp &Mp, uint64_t delta,
     p2 *= 10;
     delta *= 10;
     char d = static_cast<char>(p2 >> -one.e);
-    if (d || *len)
-      buffer[(*len)++] = static_cast<char>('0' + d);
+    if (d || *len) buffer[(*len)++] = static_cast<char>('0' + d);
     p2 &= one.f - 1;
     kappa--;
     if (p2 < delta) {
@@ -198,8 +188,7 @@ inline char *Prettify(char *buffer, int length, int k, int maxDecimalPlaces) {
 
   if (0 <= k && kk <= 21) {
     // 1234e7 -> 12340000000
-    for (int i = length; i < kk; i++)
-      buffer[i] = '0';
+    for (int i = length; i < kk; i++) buffer[i] = '0';
     buffer[kk] = '.';
     buffer[kk + 1] = '0';
     return &buffer[kk + 2];
@@ -212,8 +201,7 @@ inline char *Prettify(char *buffer, int length, int k, int maxDecimalPlaces) {
       // When maxDecimalPlaces = 2, 1.2345 -> 1.23, 1.102 -> 1.1
       // Remove extra trailing zeros (at least one) after truncation.
       for (int i = kk + maxDecimalPlaces; i > kk + 1; i--)
-        if (buffer[i] != '0')
-          return &buffer[i + 1];
+        if (buffer[i] != '0') return &buffer[i + 1];
       return &buffer[kk + 2];  // Reserve one zero
     } else
       return &buffer[length + 1];
@@ -223,14 +211,12 @@ inline char *Prettify(char *buffer, int length, int k, int maxDecimalPlaces) {
     std::memmove(&buffer[offset], &buffer[0], static_cast<size_t>(length));
     buffer[0] = '0';
     buffer[1] = '.';
-    for (int i = 2; i < offset; i++)
-      buffer[i] = '0';
+    for (int i = 2; i < offset; i++) buffer[i] = '0';
     if (length - kk > maxDecimalPlaces) {
       // When maxDecimalPlaces = 2, 0.123 -> 0.12, 0.102 -> 0.1
       // Remove extra trailing zeros (at least one) after truncation.
       for (int i = maxDecimalPlaces + 1; i > 2; i--)
-        if (buffer[i] != '0')
-          return &buffer[i + 1];
+        if (buffer[i] != '0') return &buffer[i + 1];
       return &buffer[3];  // Reserve one zero
     } else
       return &buffer[length + offset];
@@ -257,8 +243,7 @@ inline char *dtoa(double value, char *buffer, int maxDecimalPlaces = 324) {
   RAPIDJSON_ASSERT(maxDecimalPlaces >= 1);
   Double d(value);
   if (d.IsZero()) {
-    if (d.Sign())
-      *buffer++ = '-'; // -0.0, Issue #289
+    if (d.Sign()) *buffer++ = '-';  // -0.0, Issue #289
     buffer[0] = '0';
     buffer[1] = '.';
     buffer[2] = '0';

livox_ros_driver/common/rapidjson/internal/itoa.h

@@ -52,12 +52,9 @@ inline char *u32toa(uint32_t value, char *buffer) {
     const uint32_t d1 = (value / 100) << 1;
     const uint32_t d2 = (value % 100) << 1;
 
-    if (value >= 1000)
-      *buffer++ = cDigitsLut[d1];
-    if (value >= 100)
-      *buffer++ = cDigitsLut[d1 + 1];
-    if (value >= 10)
-      *buffer++ = cDigitsLut[d2];
+    if (value >= 1000) *buffer++ = cDigitsLut[d1];
+    if (value >= 100) *buffer++ = cDigitsLut[d1 + 1];
+    if (value >= 10) *buffer++ = cDigitsLut[d2];
     *buffer++ = cDigitsLut[d2 + 1];
   } else if (value < 100000000) {
     // value = bbbbcccc
@@ -70,12 +67,9 @@ inline char *u32toa(uint32_t value, char *buffer) {
     const uint32_t d3 = (c / 100) << 1;
     const uint32_t d4 = (c % 100) << 1;
 
-    if (value >= 10000000)
-      *buffer++ = cDigitsLut[d1];
-    if (value >= 1000000)
-      *buffer++ = cDigitsLut[d1 + 1];
-    if (value >= 100000)
-      *buffer++ = cDigitsLut[d2];
+    if (value >= 10000000) *buffer++ = cDigitsLut[d1];
+    if (value >= 1000000) *buffer++ = cDigitsLut[d1 + 1];
+    if (value >= 100000) *buffer++ = cDigitsLut[d2];
     *buffer++ = cDigitsLut[d2 + 1];
 
     *buffer++ = cDigitsLut[d3];
@@ -146,12 +140,9 @@ inline char *u64toa(uint64_t value, char *buffer) {
       const uint32_t d1 = (v / 100) << 1;
       const uint32_t d2 = (v % 100) << 1;
 
-      if (v >= 1000)
-        *buffer++ = cDigitsLut[d1];
-      if (v >= 100)
-        *buffer++ = cDigitsLut[d1 + 1];
-      if (v >= 10)
-        *buffer++ = cDigitsLut[d2];
+      if (v >= 1000) *buffer++ = cDigitsLut[d1];
+      if (v >= 100) *buffer++ = cDigitsLut[d1 + 1];
+      if (v >= 10) *buffer++ = cDigitsLut[d2];
       *buffer++ = cDigitsLut[d2 + 1];
     } else {
       // value = bbbbcccc
@@ -164,12 +155,9 @@ inline char *u64toa(uint64_t value, char *buffer) {
       const uint32_t d3 = (c / 100) << 1;
       const uint32_t d4 = (c % 100) << 1;
 
-      if (value >= 10000000)
-        *buffer++ = cDigitsLut[d1];
-      if (value >= 1000000)
-        *buffer++ = cDigitsLut[d1 + 1];
-      if (value >= 100000)
-        *buffer++ = cDigitsLut[d2];
+      if (value >= 10000000) *buffer++ = cDigitsLut[d1];
+      if (value >= 1000000) *buffer++ = cDigitsLut[d1 + 1];
+      if (value >= 100000) *buffer++ = cDigitsLut[d2];
       *buffer++ = cDigitsLut[d2 + 1];
 
       *buffer++ = cDigitsLut[d3];
@@ -199,20 +187,13 @@ inline char *u64toa(uint64_t value, char *buffer) {
     const uint32_t d7 = (c1 / 100) << 1;
     const uint32_t d8 = (c1 % 100) << 1;
 
-    if (value >= kTen15)
-      *buffer++ = cDigitsLut[d1];
-    if (value >= kTen14)
-      *buffer++ = cDigitsLut[d1 + 1];
-    if (value >= kTen13)
-      *buffer++ = cDigitsLut[d2];
-    if (value >= kTen12)
-      *buffer++ = cDigitsLut[d2 + 1];
-    if (value >= kTen11)
-      *buffer++ = cDigitsLut[d3];
-    if (value >= kTen10)
-      *buffer++ = cDigitsLut[d3 + 1];
-    if (value >= kTen9)
-      *buffer++ = cDigitsLut[d4];
+    if (value >= kTen15) *buffer++ = cDigitsLut[d1];
+    if (value >= kTen14) *buffer++ = cDigitsLut[d1 + 1];
+    if (value >= kTen13) *buffer++ = cDigitsLut[d2];
+    if (value >= kTen12) *buffer++ = cDigitsLut[d2 + 1];
+    if (value >= kTen11) *buffer++ = cDigitsLut[d3];
+    if (value >= kTen10) *buffer++ = cDigitsLut[d3 + 1];
+    if (value >= kTen9) *buffer++ = cDigitsLut[d4];
 
     *buffer++ = cDigitsLut[d4 + 1];
     *buffer++ = cDigitsLut[d5];

livox_ros_driver/common/rapidjson/internal/meta.h

@@ -41,12 +41,16 @@ namespace internal {
 
 // Helper to wrap/convert arbitrary types to void, useful for arbitrary type
 // matching
-template <typename T> struct Void { typedef void Type; };
+template <typename T>
+struct Void {
+  typedef void Type;
+};
 
 ///////////////////////////////////////////////////////////////////////////////
 // BoolType, TrueType, FalseType
 //
-template <bool Cond> struct BoolType {
+template <bool Cond>
+struct BoolType {
   static const bool Value = Cond;
   typedef BoolType Type;
 };
@@ -57,21 +61,33 @@ typedef BoolType<false> FalseType;
 // SelectIf, BoolExpr, NotExpr, AndExpr, OrExpr
 //
 
-template <bool C> struct SelectIfImpl {
-  template <typename T1, typename T2> struct Apply { typedef T1 Type; };
+template <bool C>
+struct SelectIfImpl {
+  template <typename T1, typename T2>
+  struct Apply {
+    typedef T1 Type;
+  };
+};
+template <>
+struct SelectIfImpl<false> {
+  template <typename T1, typename T2>
+  struct Apply {
+    typedef T2 Type;
   };
-template <> struct SelectIfImpl<false> {
-  template <typename T1, typename T2> struct Apply { typedef T2 Type; };
 };
 template <bool C, typename T1, typename T2>
 struct SelectIfCond : SelectIfImpl<C>::template Apply<T1, T2> {};
 template <typename C, typename T1, typename T2>
 struct SelectIf : SelectIfCond<C::Value, T1, T2> {};
 
-template <bool Cond1, bool Cond2> struct AndExprCond : FalseType {};
-template <> struct AndExprCond<true, true> : TrueType {};
-template <bool Cond1, bool Cond2> struct OrExprCond : TrueType {};
-template <> struct OrExprCond<false, false> : FalseType {};
+template <bool Cond1, bool Cond2>
+struct AndExprCond : FalseType {};
+template <>
+struct AndExprCond<true, true> : TrueType {};
+template <bool Cond1, bool Cond2>
+struct OrExprCond : TrueType {};
+template <>
+struct OrExprCond<false, false> : FalseType {};
 
 template <typename C>
 struct BoolExpr : SelectIf<C, TrueType, FalseType>::Type {};
@@ -84,20 +100,33 @@ struct OrExpr : OrExprCond<C1::Value, C2::Value>::Type {};
 
 ///////////////////////////////////////////////////////////////////////////////
 // AddConst, MaybeAddConst, RemoveConst
-template <typename T> struct AddConst { typedef const T Type; };
+template <typename T>
+struct AddConst {
+  typedef const T Type;
+};
 template <bool Constify, typename T>
 struct MaybeAddConst : SelectIfCond<Constify, const T, T> {};
-template <typename T> struct RemoveConst { typedef T Type; };
-template <typename T> struct RemoveConst<const T> { typedef T Type; };
+template <typename T>
+struct RemoveConst {
+  typedef T Type;
+};
+template <typename T>
+struct RemoveConst<const T> {
+  typedef T Type;
+};
 
 ///////////////////////////////////////////////////////////////////////////////
 // IsSame, IsConst, IsMoreConst, IsPointer
 //
-template <typename T, typename U> struct IsSame : FalseType {};
-template <typename T> struct IsSame<T, T> : TrueType {};
+template <typename T, typename U>
+struct IsSame : FalseType {};
+template <typename T>
+struct IsSame<T, T> : TrueType {};
 
-template <typename T> struct IsConst : FalseType {};
-template <typename T> struct IsConst<const T> : TrueType {};
+template <typename T>
+struct IsConst : FalseType {};
+template <typename T>
+struct IsConst<const T> : TrueType {};
 
 template <typename CT, typename T>
 struct IsMoreConst
@@ -105,8 +134,10 @@ struct IsMoreConst
           IsSame<typename RemoveConst<CT>::Type, typename RemoveConst<T>::Type>,
           BoolType<IsConst<CT>::Value >= IsConst<T>::Value>>::Type {};
 
-template <typename T> struct IsPointer : FalseType {};
-template <typename T> struct IsPointer<T *> : TrueType {};
+template <typename T>
+struct IsPointer : FalseType {};
+template <typename T>
+struct IsPointer<T *> : TrueType {};
 
 ///////////////////////////////////////////////////////////////////////////////
 // IsBaseOf
@@ -118,14 +149,16 @@ struct IsBaseOf : BoolType<::std::is_base_of<B, D>::value> {};
 
 #else  // simplified version adopted from Boost
 
-template <typename B, typename D> struct IsBaseOfImpl {
+template <typename B, typename D>
+struct IsBaseOfImpl {
   RAPIDJSON_STATIC_ASSERT(sizeof(B) != 0);
   RAPIDJSON_STATIC_ASSERT(sizeof(D) != 0);
 
   typedef char (&Yes)[1];
   typedef char (&No)[2];
 
-  template <typename T> static Yes Check(const D *, T);
+  template <typename T>
+  static Yes Check(const D *, T);
   static No Check(const B *, int);
 
   struct Host {
@@ -144,16 +177,20 @@ struct IsBaseOf : OrExpr<IsSame<B, D>, BoolExpr<IsBaseOfImpl<B, D>>>::Type {};
 //////////////////////////////////////////////////////////////////////////
 // EnableIf / DisableIf
 //
-template <bool Condition, typename T = void> struct EnableIfCond {
+template <bool Condition, typename T = void>
+struct EnableIfCond {
   typedef T Type;
 };
-template <typename T> struct EnableIfCond<false, T> { /* empty */
+template <typename T>
+struct EnableIfCond<false, T> { /* empty */
 };
 
-template <bool Condition, typename T = void> struct DisableIfCond {
+template <bool Condition, typename T = void>
+struct DisableIfCond {
   typedef T Type;
 };
-template <typename T> struct DisableIfCond<true, T> { /* empty */
+template <typename T>
+struct DisableIfCond<true, T> { /* empty */
 };
 
 template <typename Condition, typename T = void>
@@ -164,8 +201,10 @@ struct DisableIf : DisableIfCond<Condition::Value, T> {};
 
 // SFINAE helpers
 struct SfinaeTag {};
-template <typename T> struct RemoveSfinaeTag;
-template <typename T> struct RemoveSfinaeTag<SfinaeTag &(*)(T)> {
+template <typename T>
+struct RemoveSfinaeTag;
+template <typename T>
+struct RemoveSfinaeTag<SfinaeTag &(*)(T)> {
   typedef T Type;
 };
 

livox_ros_driver/common/rapidjson/internal/regex.h

@@ -47,7 +47,8 @@ namespace internal {
 ///////////////////////////////////////////////////////////////////////////////
 // DecodedStream
 
-template <typename SourceStream, typename Encoding> class DecodedStream {
+template <typename SourceStream, typename Encoding>
+class DecodedStream {
  public:
   DecodedStream(SourceStream &ss) : ss_(ss), codepoint_() { Decode(); }
   unsigned Peek() { return codepoint_; }
@@ -60,8 +61,7 @@ public:
 
  private:
   void Decode() {
-    if (!Encoding::Decode(ss_, &codepoint_))
-      codepoint_ = 0;
+    if (!Encoding::Decode(ss_, &codepoint_)) codepoint_ = 0;
   }
 
   SourceStream &ss_;
@@ -75,7 +75,8 @@ static const SizeType kRegexInvalidState = ~SizeType(
     0);  //!< Represents an invalid index in GenericRegex::State::out, out1
 static const SizeType kRegexInvalidRange = ~SizeType(0);
 
-template <typename Encoding, typename Allocator> class GenericRegexSearch;
+template <typename Encoding, typename Allocator>
+class GenericRegexSearch;
 
 //! Regular expression engine with subset of ECMAscript grammar.
 /*!
@@ -115,13 +116,18 @@ class GenericRegex {
  public:
   typedef Encoding EncodingType;
   typedef typename Encoding::Ch Ch;
-  template <typename, typename> friend class GenericRegexSearch;
+  template <typename, typename>
+  friend class GenericRegexSearch;
 
   GenericRegex(const Ch *source, Allocator *allocator = 0)
       : ownAllocator_(allocator ? 0 : RAPIDJSON_NEW(Allocator)()),
         allocator_(allocator ? allocator : ownAllocator_),
-        states_(allocator_, 256), ranges_(allocator_, 256),
-        root_(kRegexInvalidState), stateCount_(), rangeCount_(), anchorBegin_(),
+        states_(allocator_, 256),
+        ranges_(allocator_, 256),
+        root_(kRegexInvalidState),
+        stateCount_(),
+        rangeCount_(),
+        anchorBegin_(),
         anchorEnd_() {
     GenericStringStream<Encoding> ss(source);
     DecodedStream<GenericStringStream<Encoding>, Encoding> ds(ss);
@@ -225,32 +231,27 @@ private:
                  *operatorStack.template Top<Operator>() != kLeftParenthesis)
             if (!Eval(operandStack, *operatorStack.template Pop<Operator>(1)))
               return;
-        if (operatorStack.Empty())
-          return;
+          if (operatorStack.Empty()) return;
           operatorStack.template Pop<Operator>(1);
           atomCountStack.template Pop<unsigned>(1);
           ImplicitConcatenation(atomCountStack, operatorStack);
           break;
 
         case '?':
-        if (!Eval(operandStack, kZeroOrOne))
-          return;
+          if (!Eval(operandStack, kZeroOrOne)) return;
           break;
 
         case '*':
-        if (!Eval(operandStack, kZeroOrMore))
-          return;
+          if (!Eval(operandStack, kZeroOrMore)) return;
           break;
 
         case '+':
-        if (!Eval(operandStack, kOneOrMore))
-          return;
+          if (!Eval(operandStack, kOneOrMore)) return;
           break;
 
         case '{': {
           unsigned n, m;
-        if (!ParseUnsigned(ds, &n))
-          return;
+          if (!ParseUnsigned(ds, &n)) return;
 
           if (ds.Peek() == ',') {
             ds.Take();
@@ -261,8 +262,7 @@ private:
           } else
             m = n;
 
-        if (!EvalQuantifier(operandStack, n, m) || ds.Peek() != '}')
-          return;
+          if (!EvalQuantifier(operandStack, n, m) || ds.Peek() != '}') return;
           ds.Take();
         } break;
 
@@ -273,8 +273,7 @@ private:
 
         case '[': {
           SizeType range;
-        if (!ParseRange(ds, &range))
-          return;
+          if (!ParseRange(ds, &range)) return;
           SizeType s = NewState(kRegexInvalidState, kRegexInvalidState,
                                 kRangeCharacterClass);
           GetState(s).rangeStart = range;
@@ -296,8 +295,7 @@ private:
     }
 
     while (!operatorStack.Empty())
-      if (!Eval(operandStack, *operatorStack.template Pop<Operator>(1)))
-        return;
+      if (!Eval(operandStack, *operatorStack.template Pop<Operator>(1))) return;
 
     // Link the operand to matching state.
     if (operandStack.GetSize() == sizeof(Frag)) {
@@ -340,8 +338,7 @@ private:
 
   SizeType Append(SizeType l1, SizeType l2) {
     SizeType old = l1;
-    while (GetState(l1).out != kRegexInvalidState)
-      l1 = GetState(l1).out;
+    while (GetState(l1).out != kRegexInvalidState) l1 = GetState(l1).out;
     GetState(l1).out = l2;
     return old;
   }
@@ -465,10 +462,8 @@ private:
     State *s = states_.template Push<State>(count);
     memcpy(s, &GetState(src.minIndex), count * sizeof(State));
     for (SizeType j = 0; j < count; j++) {
-      if (s[j].out != kRegexInvalidState)
-        s[j].out += count;
-      if (s[j].out1 != kRegexInvalidState)
-        s[j].out1 += count;
+      if (s[j].out != kRegexInvalidState) s[j].out += count;
+      if (s[j].out1 != kRegexInvalidState) s[j].out1 += count;
     }
     *operandStack.template Push<Frag>() =
         Frag(src.start + count, src.out + count, src.minIndex + count);
@@ -478,8 +473,7 @@ private:
   template <typename InputStream>
   bool ParseUnsigned(DecodedStream<InputStream, Encoding> &ds, unsigned *u) {
     unsigned r = 0;
-    if (ds.Peek() < '0' || ds.Peek() > '9')
-      return false;
+    if (ds.Peek() < '0' || ds.Peek() > '9') return false;
     while (ds.Peek() >= '0' && ds.Peek() <= '9') {
       if (r >= 429496729 && ds.Peek() > '5')  // 2^32 - 1 = 4294967295
         return false;                         // overflow
@@ -515,8 +509,7 @@ private:
             RAPIDJSON_ASSERT(current != kRegexInvalidRange);
             GetRange(current).next = r;
           }
-        if (negate)
-          GetRange(start).start |= kRangeNegationFlag;
+          if (negate) GetRange(start).start |= kRangeNegationFlag;
           *range = start;
           return true;
 
@@ -541,10 +534,8 @@ private:
 
             case 0: {
               SizeType r = NewRange(codepoint);
-          if (current != kRegexInvalidRange)
-            GetRange(current).next = r;
-          if (start == kRegexInvalidRange)
-            start = r;
+              if (current != kRegexInvalidRange) GetRange(current).next = r;
+              if (start == kRegexInvalidRange) start = r;
               current = r;
             }
               step = 1;
@@ -630,11 +621,14 @@ public:
   typedef typename Encoding::Ch Ch;
 
   GenericRegexSearch(const RegexType &regex, Allocator *allocator = 0)
-      : regex_(regex), allocator_(allocator), ownAllocator_(0),
-        state0_(allocator, 0), state1_(allocator, 0), stateSet_() {
+      : regex_(regex),
+        allocator_(allocator),
+        ownAllocator_(0),
+        state0_(allocator, 0),
+        state1_(allocator, 0),
+        stateSet_() {
     RAPIDJSON_ASSERT(regex_.IsValid());
-    if (!allocator_)
-      ownAllocator_ = allocator_ = RAPIDJSON_NEW(Allocator)();
+    if (!allocator_) ownAllocator_ = allocator_ = RAPIDJSON_NEW(Allocator)();
     stateSet_ = static_cast<unsigned *>(allocator_->Malloc(GetStateSetSize()));
     state0_.template Reserve<SizeType>(regex_.stateCount_);
     state1_.template Reserve<SizeType>(regex_.stateCount_);
@@ -645,7 +639,8 @@ public:
     RAPIDJSON_DELETE(ownAllocator_);
   }
 
-  template <typename InputStream> bool Match(InputStream &is) {
+  template <typename InputStream>
+  bool Match(InputStream &is) {
     return SearchWithAnchoring(is, true, true);
   }
 
@@ -654,7 +649,8 @@ public:
     return Match(is);
   }
 
-  template <typename InputStream> bool Search(InputStream &is) {
+  template <typename InputStream>
+  bool Search(InputStream &is) {
     return SearchWithAnchoring(is, regex_.anchorBegin_, regex_.anchorEnd_);
   }
 
@@ -690,11 +686,9 @@ private:
             (sr.codepoint == RegexType::kRangeCharacterClass &&
              MatchRange(sr.rangeStart, codepoint))) {
           matched = AddState(*next, sr.out) || matched;
-          if (!anchorEnd && matched)
-            return true;
+          if (!anchorEnd && matched) return true;
         }
-        if (!anchorBegin)
-          AddState(*next, regex_.root_);
+        if (!anchorBegin) AddState(*next, regex_.root_);
       }
       internal::Swap(current, next);
     }

livox_ros_driver/common/rapidjson/internal/stack.h

@@ -19,9 +19,9 @@
 #ifndef RAPIDJSON_INTERNAL_STACK_H_
 #define RAPIDJSON_INTERNAL_STACK_H_
 
+#include <cstddef>
 #include "../allocators.h"
 #include "swap.h"
-#include <cstddef>
 
 #if defined(__clang__)
 RAPIDJSON_DIAG_PUSH
@@ -37,18 +37,26 @@ namespace internal {
 //! A type-unsafe stack for storing different types of data.
 /*! \tparam Allocator Allocator for allocating stack memory.
  */
-template <typename Allocator> class Stack {
+template <typename Allocator>
+class Stack {
  public:
   // Optimization note: Do not allocate memory for stack_ in constructor.
   // Do it lazily when first Push() -> Expand() -> Resize().
   Stack(Allocator *allocator, size_t stackCapacity)
-      : allocator_(allocator), ownAllocator_(0), stack_(0), stackTop_(0),
-        stackEnd_(0), initialCapacity_(stackCapacity) {}
+      : allocator_(allocator),
+        ownAllocator_(0),
+        stack_(0),
+        stackTop_(0),
+        stackEnd_(0),
+        initialCapacity_(stackCapacity) {}
 
 #if RAPIDJSON_HAS_CXX11_RVALUE_REFS
   Stack(Stack &&rhs)
-      : allocator_(rhs.allocator_), ownAllocator_(rhs.ownAllocator_),
-        stack_(rhs.stack_), stackTop_(rhs.stackTop_), stackEnd_(rhs.stackEnd_),
+      : allocator_(rhs.allocator_),
+        ownAllocator_(rhs.ownAllocator_),
+        stack_(rhs.stack_),
+        stackTop_(rhs.stackTop_),
+        stackEnd_(rhs.stackEnd_),
         initialCapacity_(rhs.initialCapacity_) {
     rhs.allocator_ = 0;
     rhs.ownAllocator_ = 0;
@@ -109,19 +117,22 @@ public:
   // Optimization note: try to minimize the size of this function for force
   // inline. Expansion is run very infrequently, so it is moved to another
   // (probably non-inline) function.
-  template <typename T> RAPIDJSON_FORCEINLINE void Reserve(size_t count = 1) {
+  template <typename T>
+  RAPIDJSON_FORCEINLINE void Reserve(size_t count = 1) {
     // Expand the stack if needed
     if (RAPIDJSON_UNLIKELY(static_cast<std::ptrdiff_t>(sizeof(T) * count) >
                            (stackEnd_ - stackTop_)))
       Expand<T>(count);
   }
 
-  template <typename T> RAPIDJSON_FORCEINLINE T *Push(size_t count = 1) {
+  template <typename T>
+  RAPIDJSON_FORCEINLINE T *Push(size_t count = 1) {
     Reserve<T>(count);
     return PushUnsafe<T>(count);
   }
 
-  template <typename T> RAPIDJSON_FORCEINLINE T *PushUnsafe(size_t count = 1) {
+  template <typename T>
+  RAPIDJSON_FORCEINLINE T *PushUnsafe(size_t count = 1) {
     RAPIDJSON_ASSERT(stackTop_);
     RAPIDJSON_ASSERT(static_cast<std::ptrdiff_t>(sizeof(T) * count) <=
                      (stackEnd_ - stackTop_));
@@ -130,31 +141,42 @@ public:
     return ret;
   }
 
-  template <typename T> T *Pop(size_t count) {
+  template <typename T>
+  T *Pop(size_t count) {
     RAPIDJSON_ASSERT(GetSize() >= count * sizeof(T));
     stackTop_ -= count * sizeof(T);
     return reinterpret_cast<T *>(stackTop_);
   }
 
-  template <typename T> T *Top() {
+  template <typename T>
+  T *Top() {
     RAPIDJSON_ASSERT(GetSize() >= sizeof(T));
     return reinterpret_cast<T *>(stackTop_ - sizeof(T));
   }
 
-  template <typename T> const T *Top() const {
+  template <typename T>
+  const T *Top() const {
     RAPIDJSON_ASSERT(GetSize() >= sizeof(T));
     return reinterpret_cast<T *>(stackTop_ - sizeof(T));
   }
 
-  template <typename T> T *End() { return reinterpret_cast<T *>(stackTop_); }
-
-  template <typename T> const T *End() const {
+  template <typename T>
+  T *End() {
     return reinterpret_cast<T *>(stackTop_);
   }
 
-  template <typename T> T *Bottom() { return reinterpret_cast<T *>(stack_); }
+  template <typename T>
+  const T *End() const {
+    return reinterpret_cast<T *>(stackTop_);
+  }
 
-  template <typename T> const T *Bottom() const {
+  template <typename T>
+  T *Bottom() {
+    return reinterpret_cast<T *>(stack_);
+  }
+
+  template <typename T>
+  const T *Bottom() const {
     return reinterpret_cast<T *>(stack_);
   }
 
@@ -170,21 +192,20 @@ public:
   size_t GetCapacity() const { return static_cast<size_t>(stackEnd_ - stack_); }
 
  private:
-  template <typename T> void Expand(size_t count) {
+  template <typename T>
+  void Expand(size_t count) {
     // Only expand the capacity if the current stack exists. Otherwise just
     // create a stack with initial capacity.
     size_t newCapacity;
     if (stack_ == 0) {
-      if (!allocator_)
-        ownAllocator_ = allocator_ = RAPIDJSON_NEW(Allocator)();
+      if (!allocator_) ownAllocator_ = allocator_ = RAPIDJSON_NEW(Allocator)();
       newCapacity = initialCapacity_;
     } else {
       newCapacity = GetCapacity();
       newCapacity += (newCapacity + 1) / 2;
     }
     size_t newSize = GetSize() + sizeof(T) * count;
-    if (newCapacity < newSize)
-      newCapacity = newSize;
+    if (newCapacity < newSize) newCapacity = newSize;
 
     Resize(newCapacity);
   }

livox_ros_driver/common/rapidjson/internal/strfunc.h

@@ -19,8 +19,8 @@
 #ifndef RAPIDJSON_INTERNAL_STRFUNC_H_
 #define RAPIDJSON_INTERNAL_STRFUNC_H_
 
-#include "../stream.h"
 #include <cwchar>
+#include "../stream.h"
 
 RAPIDJSON_NAMESPACE_BEGIN
 namespace internal {
@@ -32,19 +32,21 @@ namespace internal {
     \note This has the same semantics as strlen(), the return value is not
    number of Unicode codepoints.
 */
-template <typename Ch> inline SizeType StrLen(const Ch *s) {
+template <typename Ch>
+inline SizeType StrLen(const Ch *s) {
   RAPIDJSON_ASSERT(s != 0);
   const Ch *p = s;
-  while (*p)
-    ++p;
+  while (*p) ++p;
   return SizeType(p - s);
 }
 
-template <> inline SizeType StrLen(const char *s) {
+template <>
+inline SizeType StrLen(const char *s) {
   return SizeType(std::strlen(s));
 }
 
-template <> inline SizeType StrLen(const wchar_t *s) {
+template <>
+inline SizeType StrLen(const wchar_t *s) {
   return SizeType(std::wcslen(s));
 }
 
@@ -59,8 +61,7 @@ bool CountStringCodePoint(const typename Encoding::Ch *s, SizeType length,
   SizeType count = 0;
   while (is.src_ < end) {
     unsigned codepoint;
-    if (!Encoding::Decode(is, &codepoint))
-      return false;
+    if (!Encoding::Decode(is, &codepoint)) return false;
     count++;
   }
   *outCount = count;

livox_ros_driver/common/rapidjson/internal/strtod.h

@@ -19,12 +19,12 @@
 #ifndef RAPIDJSON_STRTOD_
 #define RAPIDJSON_STRTOD_
 
+#include <climits>
+#include <limits>
 #include "biginteger.h"
 #include "diyfp.h"
 #include "ieee754.h"
 #include "pow10.h"
-#include <climits>
-#include <limits>
 
 RAPIDJSON_NAMESPACE_BEGIN
 namespace internal {
@@ -48,12 +48,11 @@ inline double StrtodNormalPrecision(double d, int p) {
   return d;
 }
 
-template <typename T> inline T Min3(T a, T b, T c) {
+template <typename T>
+inline T Min3(T a, T b, T c) {
   T m = a;
-  if (m > b)
-    m = b;
-  if (m > c)
-    m = c;
+  if (m > b) m = b;
+  if (m > c) m = c;
   return m;
 }
 
@@ -177,7 +176,8 @@ inline bool StrtodDiyFp(const char *decimals, int dLen, int dExp,
     int adjustment = dExp - actualExp;
     RAPIDJSON_ASSERT(adjustment >= 1 && adjustment < 8);
     v = v * kPow10[adjustment - 1];
-    if (dLen + adjustment > 19) // has more digits than decimal digits in 64-bit
+    if (dLen + adjustment >
+        19)  // has more digits than decimal digits in 64-bit
       error += kUlp / 2;
   }
 
@@ -244,8 +244,7 @@ inline double StrtodFullPrecision(double d, int p, const char *decimals,
   RAPIDJSON_ASSERT(length >= 1);
 
   double result = 0.0;
-  if (StrtodFast(d, p, &result))
-    return result;
+  if (StrtodFast(d, p, &result)) return result;
 
   RAPIDJSON_ASSERT(length <= INT_MAX);
   int dLen = static_cast<int>(length);
@@ -285,16 +284,13 @@ inline double StrtodFullPrecision(double d, int p, const char *decimals,
 
   // If too small, underflow to zero.
   // Any x <= 10^-324 is interpreted as zero.
-  if (dLen + dExp <= -324)
-    return 0.0;
+  if (dLen + dExp <= -324) return 0.0;
 
   // If too large, overflow to infinity.
   // Any x >= 10^309 is interpreted as +infinity.
-  if (dLen + dExp > 309)
-    return std::numeric_limits<double>::infinity();
+  if (dLen + dExp > 309) return std::numeric_limits<double>::infinity();
 
-  if (StrtodDiyFp(decimals, dLen, dExp, &result))
-    return result;
+  if (StrtodDiyFp(decimals, dLen, dExp, &result)) return result;
 
   // Use approximation from StrtodDiyFp and make adjustment with BigInteger
   // comparison

livox_ros_driver/common/rapidjson/internal/swap.h

@@ -33,7 +33,8 @@ namespace internal {
 /*! \tparam T Type of the arguments to swap, should be instantiated with
    primitive C++ types only. \note This has the same semantics as std::swap().
 */
-template <typename T> inline void Swap(T &a, T &b) RAPIDJSON_NOEXCEPT {
+template <typename T>
+inline void Swap(T &a, T &b) RAPIDJSON_NOEXCEPT {
   T tmp = a;
   a = b;
   b = tmp;

livox_ros_driver/common/rapidjson/istreamwrapper.h

@@ -19,17 +19,17 @@
 #ifndef RAPIDJSON_ISTREAMWRAPPER_H_
 #define RAPIDJSON_ISTREAMWRAPPER_H_
 
-#include "stream.h"
 #include <ios>
 #include <iosfwd>
+#include "stream.h"
 
 #ifdef __clang__
 RAPIDJSON_DIAG_PUSH
 RAPIDJSON_DIAG_OFF(padded)
 #elif defined(_MSC_VER)
 RAPIDJSON_DIAG_PUSH
-RAPIDJSON_DIAG_OFF(
-    4351) // new behavior: elements of array 'array' will be default initialized
+RAPIDJSON_DIAG_OFF(4351)  // new behavior: elements of array 'array' will be
+                          // default initialized
 #endif
 
 RAPIDJSON_NAMESPACE_BEGIN
@@ -50,7 +50,8 @@ RAPIDJSON_NAMESPACE_BEGIN
     \tparam StreamType Class derived from \c std::basic_istream.
 */
 
-template <typename StreamType> class BasicIStreamWrapper {
+template <typename StreamType>
+class BasicIStreamWrapper {
  public:
   typedef typename StreamType::char_type Ch;
 
@@ -59,8 +60,14 @@ public:
       \param stream stream opened for read.
   */
   BasicIStreamWrapper(StreamType &stream)
-      : stream_(stream), buffer_(peekBuffer_), bufferSize_(4), bufferLast_(0),
-        current_(buffer_), readCount_(0), count_(0), eof_(false) {
+      : stream_(stream),
+        buffer_(peekBuffer_),
+        bufferSize_(4),
+        bufferLast_(0),
+        current_(buffer_),
+        readCount_(0),
+        count_(0),
+        eof_(false) {
     Read();
   }
 
@@ -71,8 +78,13 @@ public:
       \param bufferSize size of buffer in bytes. Must >=4 bytes.
   */
   BasicIStreamWrapper(StreamType &stream, char *buffer, size_t bufferSize)
-      : stream_(stream), buffer_(buffer), bufferSize_(bufferSize),
-        bufferLast_(0), current_(buffer_), readCount_(0), count_(0),
+      : stream_(stream),
+        buffer_(buffer),
+        bufferSize_(bufferSize),
+        bufferLast_(0),
+        current_(buffer_),
+        readCount_(0),
+        count_(0),
         eof_(false) {
     RAPIDJSON_ASSERT(bufferSize >= 4);
     Read();

livox_ros_driver/common/rapidjson/memorybuffer.h

@@ -40,7 +40,8 @@ RAPIDJSON_NAMESPACE_BEGIN
     \tparam Allocator type for allocating memory buffer.
     \note implements Stream concept
 */
-template <typename Allocator = CrtAllocator> struct GenericMemoryBuffer {
+template <typename Allocator = CrtAllocator>
+struct GenericMemoryBuffer {
   typedef char Ch;  // byte
 
   GenericMemoryBuffer(Allocator *allocator = 0,
@@ -67,7 +68,8 @@ typedef GenericMemoryBuffer<> MemoryBuffer;
 
 //! Implement specialized version of PutN() with memset() for better
 //! performance.
-template <> inline void PutN(MemoryBuffer &memoryBuffer, char c, size_t n) {
+template <>
+inline void PutN(MemoryBuffer &memoryBuffer, char c, size_t n) {
   std::memset(memoryBuffer.stack_.Push<char>(n), c, n * sizeof(c));
 }
 

livox_ros_driver/common/rapidjson/ostreamwrapper.h

@@ -19,8 +19,8 @@
 #ifndef RAPIDJSON_OSTREAMWRAPPER_H_
 #define RAPIDJSON_OSTREAMWRAPPER_H_
 
-#include "stream.h"
 #include <iosfwd>
+#include "stream.h"
 
 #ifdef __clang__
 RAPIDJSON_DIAG_PUSH
@@ -45,7 +45,8 @@ RAPIDJSON_NAMESPACE_BEGIN
     \tparam StreamType Class derived from \c std::basic_ostream.
 */
 
-template <typename StreamType> class BasicOStreamWrapper {
+template <typename StreamType>
+class BasicOStreamWrapper {
  public:
   typedef typename StreamType::char_type Ch;
   BasicOStreamWrapper(StreamType &stream) : stream_(stream) {}

livox_ros_driver/common/rapidjson/pointer.h

@@ -45,7 +45,8 @@ enum PointerParseErrorCode {
   kPointerParseErrorTokenMustBeginWithSolidus,  //!< A token must begin with a
                                                 //!< '/'
   kPointerParseErrorInvalidEscape,              //!< Invalid escape
-  kPointerParseErrorInvalidPercentEncoding, //!< Invalid percent encoding in URI
+  kPointerParseErrorInvalidPercentEncoding,     //!< Invalid percent encoding in
+                                                //!URI
                                                 //!< fragment
   kPointerParseErrorCharacterMustPercentEncode  //!< A character must percent
                                                 //!< encoded in URI fragment
@@ -107,7 +108,8 @@ public:
      and allocation, using a special constructor.
   */
   struct Token {
-    const Ch *name; //!< Name of the token. It has null character at the end but
+    const Ch
+        *name;  //!< Name of the token. It has null character at the end but
                 //!< it can contain null character.
     SizeType length;  //!< Length of the name.
     SizeType index;   //!< A valid array index, if it is not equal to
@@ -119,8 +121,12 @@ public:
 
   //! Default constructor.
   GenericPointer(Allocator *allocator = 0)
-      : allocator_(allocator), ownAllocator_(), nameBuffer_(), tokens_(),
-        tokenCount_(), parseErrorOffset_(),
+      : allocator_(allocator),
+        ownAllocator_(),
+        nameBuffer_(),
+        tokens_(),
+        tokenCount_(),
+        parseErrorOffset_(),
         parseErrorCode_(kPointerParseErrorNone) {}
 
   //! Constructor that parses a string or URI fragment representation.
@@ -130,8 +136,12 @@ public:
      no allocator is provided, it creates a self-owned one.
   */
   explicit GenericPointer(const Ch *source, Allocator *allocator = 0)
-      : allocator_(allocator), ownAllocator_(), nameBuffer_(), tokens_(),
-        tokenCount_(), parseErrorOffset_(),
+      : allocator_(allocator),
+        ownAllocator_(),
+        nameBuffer_(),
+        tokens_(),
+        tokenCount_(),
+        parseErrorOffset_(),
         parseErrorCode_(kPointerParseErrorNone) {
     Parse(source, internal::StrLen(source));
   }
@@ -146,8 +156,12 @@ public:
   */
   explicit GenericPointer(const std::basic_string<Ch> &source,
                           Allocator *allocator = 0)
-      : allocator_(allocator), ownAllocator_(), nameBuffer_(), tokens_(),
-        tokenCount_(), parseErrorOffset_(),
+      : allocator_(allocator),
+        ownAllocator_(),
+        nameBuffer_(),
+        tokens_(),
+        tokenCount_(),
+        parseErrorOffset_(),
         parseErrorCode_(kPointerParseErrorNone) {
     Parse(source.c_str(), source.size());
   }
@@ -163,8 +177,12 @@ public:
      overload without length.
   */
   GenericPointer(const Ch *source, size_t length, Allocator *allocator = 0)
-      : allocator_(allocator), ownAllocator_(), nameBuffer_(), tokens_(),
-        tokenCount_(), parseErrorOffset_(),
+      : allocator_(allocator),
+        ownAllocator_(),
+        nameBuffer_(),
+        tokens_(),
+        tokenCount_(),
+        parseErrorOffset_(),
         parseErrorCode_(kPointerParseErrorNone) {
     Parse(source, length);
   }
@@ -192,29 +210,42 @@ public:
       \endcode
   */
   GenericPointer(const Token *tokens, size_t tokenCount)
-      : allocator_(), ownAllocator_(), nameBuffer_(),
-        tokens_(const_cast<Token *>(tokens)), tokenCount_(tokenCount),
-        parseErrorOffset_(), parseErrorCode_(kPointerParseErrorNone) {}
+      : allocator_(),
+        ownAllocator_(),
+        nameBuffer_(),
+        tokens_(const_cast<Token *>(tokens)),
+        tokenCount_(tokenCount),
+        parseErrorOffset_(),
+        parseErrorCode_(kPointerParseErrorNone) {}
 
   //! Copy constructor.
   GenericPointer(const GenericPointer &rhs)
-      : allocator_(rhs.allocator_), ownAllocator_(), nameBuffer_(), tokens_(),
-        tokenCount_(), parseErrorOffset_(),
+      : allocator_(rhs.allocator_),
+        ownAllocator_(),
+        nameBuffer_(),
+        tokens_(),
+        tokenCount_(),
+        parseErrorOffset_(),
         parseErrorCode_(kPointerParseErrorNone) {
     *this = rhs;
   }
 
   //! Copy constructor.
   GenericPointer(const GenericPointer &rhs, Allocator *allocator)
-      : allocator_(allocator), ownAllocator_(), nameBuffer_(), tokens_(),
-        tokenCount_(), parseErrorOffset_(),
+      : allocator_(allocator),
+        ownAllocator_(),
+        nameBuffer_(),
+        tokens_(),
+        tokenCount_(),
+        parseErrorOffset_(),
         parseErrorCode_(kPointerParseErrorNone) {
     *this = rhs;
   }
 
   //! Destructor.
   ~GenericPointer() {
-    if (nameBuffer_) // If user-supplied tokens constructor is used, nameBuffer_
+    if (nameBuffer_)  // If user-supplied tokens constructor is used,
+                      // nameBuffer_
                       // is nullptr and tokens_ are not deallocated.
       Allocator::Free(tokens_);
     RAPIDJSON_DELETE(ownAllocator_);
@@ -224,8 +255,7 @@ public:
   GenericPointer &operator=(const GenericPointer &rhs) {
     if (this != &rhs) {
       // Do not delete ownAllcator
-      if (nameBuffer_)
-        Allocator::Free(tokens_);
+      if (nameBuffer_) Allocator::Free(tokens_);
 
       tokenCount_ = rhs.tokenCount_;
       parseErrorOffset_ = rhs.parseErrorOffset_;
@@ -353,8 +383,7 @@ public:
       return Append(token, allocator);
     } else {
       Ch name[21];
-      for (size_t i = 0; i <= length; i++)
-        name[i] = static_cast<Ch>(buffer[i]);
+      for (size_t i = 0; i <= length; i++) name[i] = static_cast<Ch>(buffer[i]);
       Token token = {name, length, index};
       return Append(token, allocator);
     }
@@ -440,13 +469,10 @@ public:
       \note Invalid pointers are always greater than valid ones.
   */
   bool operator<(const GenericPointer &rhs) const {
-    if (!IsValid())
-      return false;
-    if (!rhs.IsValid())
-      return true;
+    if (!IsValid()) return false;
+    if (!rhs.IsValid()) return true;
 
-    if (tokenCount_ != rhs.tokenCount_)
-      return tokenCount_ < rhs.tokenCount_;
+    if (tokenCount_ != rhs.tokenCount_) return tokenCount_ < rhs.tokenCount_;
 
     for (size_t i = 0; i < tokenCount_; i++) {
       if (tokens_[i].index != rhs.tokens_[i].index)
@@ -473,7 +499,8 @@ public:
       \tparam OutputStream Type of output stream.
       \param os The output stream.
   */
-  template <typename OutputStream> bool Stringify(OutputStream &os) const {
+  template <typename OutputStream>
+  bool Stringify(OutputStream &os) const {
     return Stringify<false, OutputStream>(os);
   }
 
@@ -522,8 +549,7 @@ public:
         exist = false;
       } else {
         if (t->index == kPointerInvalidIndex) {  // must be object name
-          if (!v->IsObject())
-            v->SetObject(); // Change to Object
+          if (!v->IsObject()) v->SetObject();    // Change to Object
         } else {                                 // object name or array index
           if (!v->IsArray() && !v->IsObject())
             v->SetArray();  // Change to Array
@@ -553,8 +579,7 @@ public:
       }
     }
 
-    if (alreadyExist)
-      *alreadyExist = exist;
+    if (alreadyExist) *alreadyExist = exist;
 
     return *v;
   }
@@ -566,8 +591,8 @@ public:
      already exist. \return The resolved newly created, or already exists value.
   */
   template <typename stackAllocator>
-  ValueType &
-  Create(GenericDocument<EncodingType, typename ValueType::AllocatorType,
+  ValueType &Create(
+      GenericDocument<EncodingType, typename ValueType::AllocatorType,
                       stackAllocator> &document,
       bool *alreadyExist = 0) const {
     return Create(document, document.GetAllocator(), alreadyExist);
@@ -603,14 +628,12 @@ public:
           typename ValueType::MemberIterator m =
               v->FindMember(GenericValue<EncodingType>(
                   GenericStringRef<Ch>(t->name, t->length)));
-        if (m == v->MemberEnd())
-          break;
+          if (m == v->MemberEnd()) break;
           v = &m->value;
         }
           continue;
         case kArrayType:
-        if (t->index == kPointerInvalidIndex || t->index >= v->Size())
-          break;
+          if (t->index == kPointerInvalidIndex || t->index >= v->Size()) break;
           v = &((*v)[t->index]);
           continue;
         default:
@@ -652,8 +675,8 @@ public:
      the values if the specified value or its parents are not exist. \see
      Create()
   */
-  ValueType &
-  GetWithDefault(ValueType &root, const ValueType &defaultValue,
+  ValueType &GetWithDefault(
+      ValueType &root, const ValueType &defaultValue,
       typename ValueType::AllocatorType &allocator) const {
     bool alreadyExist;
     ValueType &v = Create(root, allocator, &alreadyExist);
@@ -661,8 +684,8 @@ public:
   }
 
   //! Query a value in a subtree with default null-terminated string.
-  ValueType &
-  GetWithDefault(ValueType &root, const Ch *defaultValue,
+  ValueType &GetWithDefault(
+      ValueType &root, const Ch *defaultValue,
       typename ValueType::AllocatorType &allocator) const {
     bool alreadyExist;
     ValueType &v = Create(root, allocator, &alreadyExist);
@@ -671,8 +694,8 @@ public:
 
 #if RAPIDJSON_HAS_STDSTRING
   //! Query a value in a subtree with default std::basic_string.
-  ValueType &
-  GetWithDefault(ValueType &root, const std::basic_string<Ch> &defaultValue,
+  ValueType &GetWithDefault(
+      ValueType &root, const std::basic_string<Ch> &defaultValue,
       typename ValueType::AllocatorType &allocator) const {
     bool alreadyExist;
     ValueType &v = Create(root, allocator, &alreadyExist);
@@ -796,8 +819,8 @@ public:
 
   //! Set a value in a document, with move semantics.
   template <typename stackAllocator>
-  ValueType &
-  Set(GenericDocument<EncodingType, typename ValueType::AllocatorType,
+  ValueType &Set(
+      GenericDocument<EncodingType, typename ValueType::AllocatorType,
                       stackAllocator> &document,
       ValueType &value) const {
     return Create(document) = value;
@@ -805,8 +828,8 @@ public:
 
   //! Set a value in a document, with copy semantics.
   template <typename stackAllocator>
-  ValueType &
-  Set(GenericDocument<EncodingType, typename ValueType::AllocatorType,
+  ValueType &Set(
+      GenericDocument<EncodingType, typename ValueType::AllocatorType,
                       stackAllocator> &document,
       const ValueType &value) const {
     return Create(document).CopyFrom(value, document.GetAllocator());
@@ -814,8 +837,8 @@ public:
 
   //! Set a null-terminated string in a document.
   template <typename stackAllocator>
-  ValueType &
-  Set(GenericDocument<EncodingType, typename ValueType::AllocatorType,
+  ValueType &Set(
+      GenericDocument<EncodingType, typename ValueType::AllocatorType,
                       stackAllocator> &document,
       const Ch *value) const {
     return Create(document) = ValueType(value, document.GetAllocator()).Move();
@@ -824,8 +847,8 @@ public:
 #if RAPIDJSON_HAS_STDSTRING
   //! Sets a std::basic_string in a document.
   template <typename stackAllocator>
-  ValueType &
-  Set(GenericDocument<EncodingType, typename ValueType::AllocatorType,
+  ValueType &Set(
+      GenericDocument<EncodingType, typename ValueType::AllocatorType,
                       stackAllocator> &document,
       const std::basic_string<Ch> &value) const {
     return Create(document) = ValueType(value, document.GetAllocator()).Move();
@@ -870,8 +893,8 @@ public:
 
   //! Swap a value with a value in a document.
   template <typename stackAllocator>
-  ValueType &
-  Swap(GenericDocument<EncodingType, typename ValueType::AllocatorType,
+  ValueType &Swap(
+      GenericDocument<EncodingType, typename ValueType::AllocatorType,
                       stackAllocator> &document,
       ValueType &value) const {
     return Create(document).Swap(value);
@@ -901,8 +924,7 @@ public:
           typename ValueType::MemberIterator m =
               v->FindMember(GenericValue<EncodingType>(
                   GenericStringRef<Ch>(t->name, t->length)));
-        if (m == v->MemberEnd())
-          return false;
+          if (m == v->MemberEnd()) return false;
           v = &m->value;
         } break;
         case kArrayType:
@@ -981,8 +1003,10 @@ private:
 #ifndef __clang__  // -Wdocumentation
                    /*!
                        \param source Either a JSON Pointer string, or its URI fragment
-                     representation. Not need to be null terminated.                 \param length Length of the
-                     source string.                 \note Source cannot be JSON String Representation of JSON
+                      representation. Not need to be null terminated.                 \param length
+                      Length of the
+                      source string.                 \note Source cannot be JSON String
+                      Representation of JSON
                       Pointer, e.g. In "/\u0000", \u0000 will not be unescaped.
                    */
 #endif
@@ -992,14 +1016,12 @@ private:
     RAPIDJSON_ASSERT(tokens_ == 0);
 
     // Create own allocator if user did not supply.
-    if (!allocator_)
-      ownAllocator_ = allocator_ = RAPIDJSON_NEW(Allocator)();
+    if (!allocator_) ownAllocator_ = allocator_ = RAPIDJSON_NEW(Allocator)();
 
     // Count number of '/' as tokenCount
     tokenCount_ = 0;
     for (const Ch *s = source; s != source + length; s++)
-      if (*s == '/')
-        tokenCount_++;
+      if (*s == '/') tokenCount_++;
 
     Token *token = tokens_ = static_cast<Token *>(
         allocator_->Malloc(tokenCount_ * sizeof(Token) + length * sizeof(Ch)));
@@ -1076,14 +1098,12 @@ private:
         }
 
         // First check for index: all of characters are digit
-        if (c < '0' || c > '9')
-          isNumber = false;
+        if (c < '0' || c > '9') isNumber = false;
 
         *name++ = c;
       }
       token->length = static_cast<SizeType>(name - token->name);
-      if (token->length == 0)
-        isNumber = false;
+      if (token->length == 0) isNumber = false;
       *name++ = '\0';  // Null terminator
 
       // Second check for index: more than one digit cannot have leading zero
@@ -1131,8 +1151,7 @@ private:
   bool Stringify(OutputStream &os) const {
     RAPIDJSON_ASSERT(IsValid());
 
-    if (uriFragment)
-      os.Put('#');
+    if (uriFragment) os.Put('#');
 
     for (Token *t = tokens_; t != tokens_ + tokenCount_; ++t) {
       os.Put('/');
@@ -1214,7 +1233,8 @@ private:
 
   //! A helper stream to encode character (UTF-8 code unit) into percent-encoded
   //! sequence.
-  template <typename OutputStream> class PercentEncodeStream {
+  template <typename OutputStream>
+  class PercentEncodeStream {
    public:
     PercentEncodeStream(OutputStream &os) : os_(os) {}
     void Put(char c) {  // UTF-8 must be byte
@@ -1250,9 +1270,8 @@ typedef GenericPointer<Value> Pointer;
 //////////////////////////////////////////////////////////////////////////////
 
 template <typename T>
-typename T::ValueType &
-CreateValueByPointer(T &root,
-                     const GenericPointer<typename T::ValueType> &pointer,
+typename T::ValueType &CreateValueByPointer(
+    T &root, const GenericPointer<typename T::ValueType> &pointer,
     typename T::AllocatorType &a) {
   return pointer.Create(root, a);
 }
@@ -1274,8 +1293,8 @@ typename DocumentType::ValueType &CreateValueByPointer(
 }
 
 template <typename DocumentType, typename CharType, size_t N>
-typename DocumentType::ValueType &
-CreateValueByPointer(DocumentType &document, const CharType (&source)[N]) {
+typename DocumentType::ValueType &CreateValueByPointer(
+    DocumentType &document, const CharType (&source)[N]) {
   return GenericPointer<typename DocumentType::ValueType>(source, N - 1)
       .Create(document);
 }
@@ -1283,16 +1302,15 @@ CreateValueByPointer(DocumentType &document, const CharType (&source)[N]) {
 //////////////////////////////////////////////////////////////////////////////
 
 template <typename T>
-typename T::ValueType *
-GetValueByPointer(T &root, const GenericPointer<typename T::ValueType> &pointer,
+typename T::ValueType *GetValueByPointer(
+    T &root, const GenericPointer<typename T::ValueType> &pointer,
     size_t *unresolvedTokenIndex = 0) {
   return pointer.Get(root, unresolvedTokenIndex);
 }
 
 template <typename T>
-const typename T::ValueType *
-GetValueByPointer(const T &root,
-                  const GenericPointer<typename T::ValueType> &pointer,
+const typename T::ValueType *GetValueByPointer(
+    const T &root, const GenericPointer<typename T::ValueType> &pointer,
     size_t *unresolvedTokenIndex = 0) {
   return pointer.Get(root, unresolvedTokenIndex);
 }
@@ -1305,8 +1323,8 @@ typename T::ValueType *GetValueByPointer(T &root, const CharType (&source)[N],
 }
 
 template <typename T, typename CharType, size_t N>
-const typename T::ValueType *
-GetValueByPointer(const T &root, const CharType (&source)[N],
+const typename T::ValueType *GetValueByPointer(
+    const T &root, const CharType (&source)[N],
     size_t *unresolvedTokenIndex = 0) {
   return GenericPointer<typename T::ValueType>(source, N - 1)
       .Get(root, unresolvedTokenIndex);
@@ -1349,18 +1367,16 @@ GetValueByPointerWithDefault(
 }
 
 template <typename T, typename CharType, size_t N>
-typename T::ValueType &
-GetValueByPointerWithDefault(T &root, const CharType (&source)[N],
-                             const typename T::ValueType &defaultValue,
-                             typename T::AllocatorType &a) {
+typename T::ValueType &GetValueByPointerWithDefault(
+    T &root, const CharType (&source)[N],
+    const typename T::ValueType &defaultValue, typename T::AllocatorType &a) {
   return GenericPointer<typename T::ValueType>(source, N - 1)
       .GetWithDefault(root, defaultValue, a);
 }
 
 template <typename T, typename CharType, size_t N>
-typename T::ValueType &
-GetValueByPointerWithDefault(T &root, const CharType (&source)[N],
-                             const typename T::Ch *defaultValue,
+typename T::ValueType &GetValueByPointerWithDefault(
+    T &root, const CharType (&source)[N], const typename T::Ch *defaultValue,
     typename T::AllocatorType &a) {
   return GenericPointer<typename T::ValueType>(source, N - 1)
       .GetWithDefault(root, defaultValue, a);
@@ -1435,9 +1451,8 @@ typename DocumentType::ValueType &GetValueByPointerWithDefault(
 }
 
 template <typename DocumentType, typename CharType, size_t N>
-typename DocumentType::ValueType &
-GetValueByPointerWithDefault(DocumentType &document,
-                             const CharType (&source)[N],
+typename DocumentType::ValueType &GetValueByPointerWithDefault(
+    DocumentType &document, const CharType (&source)[N],
     const typename DocumentType::Ch *defaultValue) {
   return GenericPointer<typename DocumentType::ValueType>(source, N - 1)
       .GetWithDefault(document, defaultValue);
@@ -1466,31 +1481,30 @@ GetValueByPointerWithDefault(DocumentType &document,
 //////////////////////////////////////////////////////////////////////////////
 
 template <typename T>
-typename T::ValueType &
-SetValueByPointer(T &root, const GenericPointer<typename T::ValueType> &pointer,
+typename T::ValueType &SetValueByPointer(
+    T &root, const GenericPointer<typename T::ValueType> &pointer,
     typename T::ValueType &value, typename T::AllocatorType &a) {
   return pointer.Set(root, value, a);
 }
 
 template <typename T>
-typename T::ValueType &
-SetValueByPointer(T &root, const GenericPointer<typename T::ValueType> &pointer,
-                  const typename T::ValueType &value,
-                  typename T::AllocatorType &a) {
+typename T::ValueType &SetValueByPointer(
+    T &root, const GenericPointer<typename T::ValueType> &pointer,
+    const typename T::ValueType &value, typename T::AllocatorType &a) {
   return pointer.Set(root, value, a);
 }
 
 template <typename T>
-typename T::ValueType &
-SetValueByPointer(T &root, const GenericPointer<typename T::ValueType> &pointer,
+typename T::ValueType &SetValueByPointer(
+    T &root, const GenericPointer<typename T::ValueType> &pointer,
     const typename T::Ch *value, typename T::AllocatorType &a) {
   return pointer.Set(root, value, a);
 }
 
 #if RAPIDJSON_HAS_STDSTRING
 template <typename T>
-typename T::ValueType &
-SetValueByPointer(T &root, const GenericPointer<typename T::ValueType> &pointer,
+typename T::ValueType &SetValueByPointer(
+    T &root, const GenericPointer<typename T::ValueType> &pointer,
     const std::basic_string<typename T::Ch> &value,
     typename T::AllocatorType &a) {
   return pointer.Set(root, value, a);
@@ -1532,8 +1546,8 @@ typename T::ValueType &SetValueByPointer(T &root, const CharType (&source)[N],
 
 #if RAPIDJSON_HAS_STDSTRING
 template <typename T, typename CharType, size_t N>
-typename T::ValueType &
-SetValueByPointer(T &root, const CharType (&source)[N],
+typename T::ValueType &SetValueByPointer(
+    T &root, const CharType (&source)[N],
     const std::basic_string<typename T::Ch> &value,
     typename T::AllocatorType &a) {
   return GenericPointer<typename T::ValueType>(source, N - 1)
@@ -1598,24 +1612,24 @@ SetValueByPointer(
 }
 
 template <typename DocumentType, typename CharType, size_t N>
-typename DocumentType::ValueType &
-SetValueByPointer(DocumentType &document, const CharType (&source)[N],
+typename DocumentType::ValueType &SetValueByPointer(
+    DocumentType &document, const CharType (&source)[N],
     typename DocumentType::ValueType &value) {
   return GenericPointer<typename DocumentType::ValueType>(source, N - 1)
       .Set(document, value);
 }
 
 template <typename DocumentType, typename CharType, size_t N>
-typename DocumentType::ValueType &
-SetValueByPointer(DocumentType &document, const CharType (&source)[N],
+typename DocumentType::ValueType &SetValueByPointer(
+    DocumentType &document, const CharType (&source)[N],
     const typename DocumentType::ValueType &value) {
   return GenericPointer<typename DocumentType::ValueType>(source, N - 1)
       .Set(document, value);
 }
 
 template <typename DocumentType, typename CharType, size_t N>
-typename DocumentType::ValueType &
-SetValueByPointer(DocumentType &document, const CharType (&source)[N],
+typename DocumentType::ValueType &SetValueByPointer(
+    DocumentType &document, const CharType (&source)[N],
     const typename DocumentType::Ch *value) {
   return GenericPointer<typename DocumentType::ValueType>(source, N - 1)
       .Set(document, value);
@@ -1623,8 +1637,8 @@ SetValueByPointer(DocumentType &document, const CharType (&source)[N],
 
 #if RAPIDJSON_HAS_STDSTRING
 template <typename DocumentType, typename CharType, size_t N>
-typename DocumentType::ValueType &
-SetValueByPointer(DocumentType &document, const CharType (&source)[N],
+typename DocumentType::ValueType &SetValueByPointer(
+    DocumentType &document, const CharType (&source)[N],
     const std::basic_string<typename DocumentType::Ch> &value) {
   return GenericPointer<typename DocumentType::ValueType>(source, N - 1)
       .Set(document, value);
@@ -1644,9 +1658,8 @@ SetValueByPointer(DocumentType &document, const CharType (&source)[N],
 //////////////////////////////////////////////////////////////////////////////
 
 template <typename T>
-typename T::ValueType &
-SwapValueByPointer(T &root,
-                   const GenericPointer<typename T::ValueType> &pointer,
+typename T::ValueType &SwapValueByPointer(
+    T &root, const GenericPointer<typename T::ValueType> &pointer,
     typename T::ValueType &value, typename T::AllocatorType &a) {
   return pointer.Swap(root, value, a);
 }
@@ -1668,8 +1681,8 @@ typename DocumentType::ValueType &SwapValueByPointer(
 }
 
 template <typename DocumentType, typename CharType, size_t N>
-typename DocumentType::ValueType &
-SwapValueByPointer(DocumentType &document, const CharType (&source)[N],
+typename DocumentType::ValueType &SwapValueByPointer(
+    DocumentType &document, const CharType (&source)[N],
     typename DocumentType::ValueType &value) {
   return GenericPointer<typename DocumentType::ValueType>(source, N - 1)
       .Swap(document, value);

livox_ros_driver/common/rapidjson/prettywriter.h

@@ -67,7 +67,9 @@ public:
   */
   explicit PrettyWriter(OutputStream &os, StackAllocator *allocator = 0,
                         size_t levelDepth = Base::kDefaultLevelDepth)
-      : Base(os, allocator, levelDepth), indentChar_(' '), indentCharCount_(4),
+      : Base(os, allocator, levelDepth),
+        indentChar_(' '),
+        indentCharCount_(4),
         formatOptions_(kFormatDefault) {}
 
   explicit PrettyWriter(StackAllocator *allocator = 0,
@@ -76,7 +78,8 @@ public:
 
 #if RAPIDJSON_HAS_CXX11_RVALUE_REFS
   PrettyWriter(PrettyWriter &&rhs)
-      : Base(std::forward<PrettyWriter>(rhs)), indentChar_(rhs.indentChar_),
+      : Base(std::forward<PrettyWriter>(rhs)),
+        indentChar_(rhs.indentChar_),
         indentCharCount_(rhs.indentCharCount_),
         formatOptions_(rhs.formatOptions_) {}
 #endif
@@ -267,8 +270,7 @@ protected:
         if (level->valueCount > 0) {
           Base::os_->Put(
               ',');  // add comma if it is not the first element in array
-          if (formatOptions_ & kFormatSingleLineArray)
-            Base::os_->Put(' ');
+          if (formatOptions_ & kFormatSingleLineArray) Base::os_->Put(' ');
         }
 
         if (!(formatOptions_ & kFormatSingleLineArray)) {
@@ -287,8 +289,7 @@ protected:
         } else
           Base::os_->Put('\n');
 
-        if (level->valueCount % 2 == 0)
-          WriteIndent();
+        if (level->valueCount % 2 == 0) WriteIndent();
       }
       if (!level->inArray && level->valueCount % 2 == 0)
         RAPIDJSON_ASSERT(type == kStringType);  // if it's in object, then even

livox_ros_driver/common/rapidjson/rapidjson.h

@@ -449,11 +449,14 @@ RAPIDJSON_NAMESPACE_END
 //!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN
 #endif
 RAPIDJSON_NAMESPACE_BEGIN
-template <bool x> struct STATIC_ASSERTION_FAILURE;
-template <> struct STATIC_ASSERTION_FAILURE<true> {
+template <bool x>
+struct STATIC_ASSERTION_FAILURE;
+template <>
+struct STATIC_ASSERTION_FAILURE<true> {
   enum { value = 1 };
 };
-template <size_t x> struct StaticAssertTest {};
+template <size_t x>
+struct StaticAssertTest {};
 RAPIDJSON_NAMESPACE_END
 
 #if defined(__GNUC__) || defined(__clang__)

livox_ros_driver/common/rapidjson/reader.h

@@ -21,6 +21,7 @@
 
 /*! \file reader.h */
 
+#include <limits>
 #include "allocators.h"
 #include "encodedstream.h"
 #include "internal/clzll.h"
@@ -28,7 +29,6 @@
 #include "internal/stack.h"
 #include "internal/strtod.h"
 #include "stream.h"
-#include <limits>
 
 #if defined(RAPIDJSON_SIMD) && defined(_MSC_VER)
 #include <intrin.h>
@@ -89,8 +89,8 @@ RAPIDJSON_DIAG_OFF(effc++)
     #define RAPIDJSON_PARSE_ERROR_NORETURN(parseErrorCode,offset) \
        throw ParseException(parseErrorCode, #parseErrorCode, offset)
 
-    #include "rapidjson/error/error.h" // rapidjson::ParseResult
     #include <stdexcept>               // std::runtime_error
+    #include "rapidjson/error/error.h" // rapidjson::ParseResult
 
     struct ParseException : std::runtime_error, rapidjson::ParseResult {
       ParseException(rapidjson::ParseErrorCode code, const char* msg, size_t
@@ -168,8 +168,8 @@ enum ParseFlag {
       64,  //!< Parse all numbers (ints/doubles) as strings.
   kParseTrailingCommasFlag =
       128,  //!< Allow trailing commas at the end of objects and arrays.
-  kParseNanAndInfFlag =
-      256, //!< Allow parsing NaN, Inf, Infinity, -Inf and -Infinity as doubles.
+  kParseNanAndInfFlag = 256,  //!< Allow parsing NaN, Inf, Infinity, -Inf and
+                              //!-Infinity as doubles.
   kParseDefaultFlags =
       RAPIDJSON_PARSE_DEFAULT_FLAGS  //!< Default parse flags. Can be customized
                                      //!< by defining
@@ -251,7 +251,8 @@ template <typename Stream, int = StreamTraits<Stream>::copyOptimization>
 class StreamLocalCopy;
 
 //! Do copy optimization.
-template <typename Stream> class StreamLocalCopy<Stream, 1> {
+template <typename Stream>
+class StreamLocalCopy<Stream, 1> {
  public:
   StreamLocalCopy(Stream &original) : s(original), original_(original) {}
   ~StreamLocalCopy() { original_ = s; }
@@ -265,7 +266,8 @@ private:
 };
 
 //! Keep reference.
-template <typename Stream> class StreamLocalCopy<Stream, 0> {
+template <typename Stream>
+class StreamLocalCopy<Stream, 0> {
  public:
   StreamLocalCopy(Stream &original) : s(original) {}
 
@@ -284,18 +286,17 @@ private:
 /*! \param is A input stream for skipping white spaces.
     \note This function has SSE2/SSE4.2 specialization.
 */
-template <typename InputStream> void SkipWhitespace(InputStream &is) {
+template <typename InputStream>
+void SkipWhitespace(InputStream &is) {
   internal::StreamLocalCopy<InputStream> copy(is);
   InputStream &s(copy.s);
 
   typename InputStream::Ch c;
-  while ((c = s.Peek()) == ' ' || c == '\n' || c == '\r' || c == '\t')
-    s.Take();
+  while ((c = s.Peek()) == ' ' || c == '\n' || c == '\r' || c == '\t') s.Take();
 }
 
 inline const char *SkipWhitespace(const char *p, const char *end) {
-  while (p != end && (*p == ' ' || *p == '\n' || *p == '\r' || *p == '\t'))
-    ++p;
+  while (p != end && (*p == ' ' || *p == '\n' || *p == '\r' || *p == '\t')) ++p;
   return p;
 }
 
@@ -325,10 +326,9 @@ inline const char *SkipWhitespace_SIMD(const char *p) {
 
   for (;; p += 16) {
     const __m128i s = _mm_load_si128(reinterpret_cast<const __m128i *>(p));
-    const int r =
-        _mm_cmpistri(w, s,
-                     _SIDD_UBYTE_OPS | _SIDD_CMP_EQUAL_ANY |
-                         _SIDD_LEAST_SIGNIFICANT | _SIDD_NEGATIVE_POLARITY);
+    const int r = _mm_cmpistri(w, s, _SIDD_UBYTE_OPS | _SIDD_CMP_EQUAL_ANY |
+                                         _SIDD_LEAST_SIGNIFICANT |
+                                         _SIDD_NEGATIVE_POLARITY);
     if (r != 16)  // some of characters is non-whitespace
       return p + r;
   }
@@ -348,10 +348,9 @@ inline const char *SkipWhitespace_SIMD(const char *p, const char *end) {
 
   for (; p <= end - 16; p += 16) {
     const __m128i s = _mm_loadu_si128(reinterpret_cast<const __m128i *>(p));
-    const int r =
-        _mm_cmpistri(w, s,
-                     _SIDD_UBYTE_OPS | _SIDD_CMP_EQUAL_ANY |
-                         _SIDD_LEAST_SIGNIFICANT | _SIDD_NEGATIVE_POLARITY);
+    const int r = _mm_cmpistri(w, s, _SIDD_UBYTE_OPS | _SIDD_CMP_EQUAL_ANY |
+                                         _SIDD_LEAST_SIGNIFICANT |
+                                         _SIDD_NEGATIVE_POLARITY);
     if (r != 16)  // some of characters is non-whitespace
       return p + r;
   }
@@ -549,12 +548,14 @@ inline const char *SkipWhitespace_SIMD(const char *p, const char *end) {
 
 #ifdef RAPIDJSON_SIMD
 //! Template function specialization for InsituStringStream
-template <> inline void SkipWhitespace(InsituStringStream &is) {
+template <>
+inline void SkipWhitespace(InsituStringStream &is) {
   is.src_ = const_cast<char *>(SkipWhitespace_SIMD(is.src_));
 }
 
 //! Template function specialization for StringStream
-template <> inline void SkipWhitespace(StringStream &is) {
+template <>
+inline void SkipWhitespace(StringStream &is) {
   is.src_ = SkipWhitespace_SIMD(is.src_);
 }
 
@@ -598,7 +599,8 @@ public:
   */
   GenericReader(StackAllocator *stackAllocator = 0,
                 size_t stackCapacity = kDefaultStackCapacity)
-      : stack_(stackAllocator, stackCapacity), parseResult_(),
+      : stack_(stackAllocator, stackCapacity),
+        parseResult_(),
         state_(IterativeParsingStartState) {}
 
   //! Parse JSON text.
@@ -711,8 +713,7 @@ public:
 
       // If we parsed anything other than a delimiter, we invoked the handler,
       // so we can return true now.
-      if (!IsIterativeParsingDelimiterState(n))
-        return true;
+      if (!IsIterativeParsingDelimiterState(n)) return true;
     }
 
     // We reached the end of file.
@@ -777,8 +778,7 @@ private:
               RAPIDJSON_PARSE_ERROR(kParseErrorUnspecificSyntaxError,
                                     is.Tell());
             else if (Consume(is, '*')) {
-              if (Consume(is, '/'))
-                break;
+              if (Consume(is, '/')) break;
             } else
               is.Take();
           }
@@ -986,7 +986,8 @@ private:
     return codepoint;
   }
 
-  template <typename CharType> class StackStream {
+  template <typename CharType>
+  class StackStream {
    public:
     typedef CharType Ch;
 
@@ -1135,8 +1136,8 @@ private:
 
 #if defined(RAPIDJSON_SSE2) || defined(RAPIDJSON_SSE42)
   // StringStream -> StackStream<char>
-  static RAPIDJSON_FORCEINLINE void
-  ScanCopyUnescapedString(StringStream &is, StackStream<char> &os) {
+  static RAPIDJSON_FORCEINLINE void ScanCopyUnescapedString(
+      StringStream &is, StackStream<char> &os) {
     const char *p = is.src_;
 
     // Scan one by one until alignment (unaligned load may cross page boundary
@@ -1187,8 +1188,7 @@ private:
 #endif
         if (length != 0) {
           char *q = reinterpret_cast<char *>(os.Push(length));
-          for (size_t i = 0; i < length; i++)
-            q[i] = p[i];
+          for (size_t i = 0; i < length; i++) q[i] = p[i];
 
           p += length;
         }
@@ -1201,8 +1201,8 @@ private:
   }
 
   // InsituStringStream -> InsituStringStream
-  static RAPIDJSON_FORCEINLINE void
-  ScanCopyUnescapedString(InsituStringStream &is, InsituStringStream &os) {
+  static RAPIDJSON_FORCEINLINE void ScanCopyUnescapedString(
+      InsituStringStream &is, InsituStringStream &os) {
     RAPIDJSON_ASSERT(&is == &os);
     (void)os;
 
@@ -1261,8 +1261,7 @@ private:
 #else
         length = static_cast<size_t>(__builtin_ffs(r) - 1);
 #endif
-        for (const char *pend = p + length; p != pend;)
-          *q++ = *p++;
+        for (const char *pend = p + length; p != pend;) *q++ = *p++;
         break;
       }
       _mm_storeu_si128(reinterpret_cast<__m128i *>(q), s);
@@ -1274,8 +1273,8 @@ private:
 
   // When read/write pointers are the same for insitu stream, just skip
   // unescaped characters
-  static RAPIDJSON_FORCEINLINE void
-  SkipUnescapedString(InsituStringStream &is) {
+  static RAPIDJSON_FORCEINLINE void SkipUnescapedString(
+      InsituStringStream &is) {
     RAPIDJSON_ASSERT(is.src_ == is.dst_);
     char *p = is.src_;
 
@@ -1333,8 +1332,8 @@ private:
   }
 #elif defined(RAPIDJSON_NEON)
   // StringStream -> StackStream<char>
-  static RAPIDJSON_FORCEINLINE void
-  ScanCopyUnescapedString(StringStream &is, StackStream<char> &os) {
+  static RAPIDJSON_FORCEINLINE void ScanCopyUnescapedString(
+      StringStream &is, StackStream<char> &os) {
     const char *p = is.src_;
 
     // Scan one by one until alignment (unaligned load may cross page boundary
@@ -1382,8 +1381,7 @@ private:
       if (RAPIDJSON_UNLIKELY(escaped)) {  // some of characters is escaped
         if (length != 0) {
           char *q = reinterpret_cast<char *>(os.Push(length));
-          for (size_t i = 0; i < length; i++)
-            q[i] = p[i];
+          for (size_t i = 0; i < length; i++) q[i] = p[i];
 
           p += length;
         }
@@ -1396,8 +1394,8 @@ private:
   }
 
   // InsituStringStream -> InsituStringStream
-  static RAPIDJSON_FORCEINLINE void
-  ScanCopyUnescapedString(InsituStringStream &is, InsituStringStream &os) {
+  static RAPIDJSON_FORCEINLINE void ScanCopyUnescapedString(
+      InsituStringStream &is, InsituStringStream &os) {
     RAPIDJSON_ASSERT(&is == &os);
     (void)os;
 
@@ -1467,8 +1465,8 @@ private:
 
   // When read/write pointers are the same for insitu stream, just skip
   // unescaped characters
-  static RAPIDJSON_FORCEINLINE void
-  SkipUnescapedString(InsituStringStream &is) {
+  static RAPIDJSON_FORCEINLINE void SkipUnescapedString(
+      InsituStringStream &is) {
     RAPIDJSON_ASSERT(is.src_ == is.dst_);
     char *p = is.src_;
 
@@ -1721,8 +1719,7 @@ private:
       if (!useDouble) {
 #if RAPIDJSON_64BIT
         // Use i64 to store significand in 64-bit architecture
-        if (!use64bit)
-          i64 = i;
+        if (!use64bit) i64 = i;
 
         while (RAPIDJSON_LIKELY(s.Peek() >= '0' && s.Peek() <= '9')) {
           if (i64 > RAPIDJSON_UINT64_C2(0x1FFFFF,
@@ -1731,8 +1728,7 @@ private:
           else {
             i64 = i64 * 10 + static_cast<unsigned>(s.TakePush() - '0');
             --expFrac;
-            if (i64 != 0)
-              significandDigit++;
+            if (i64 != 0) significandDigit++;
           }
         }
 
@@ -1748,8 +1744,7 @@ private:
         if (significandDigit < 17) {
           d = d * 10.0 + (s.TakePush() - '0');
           --expFrac;
-          if (RAPIDJSON_LIKELY(d > 0.0))
-            significandDigit++;
+          if (RAPIDJSON_LIKELY(d > 0.0)) significandDigit++;
         } else
           s.TakePush();
       }
@@ -1802,8 +1797,7 @@ private:
       } else
         RAPIDJSON_PARSE_ERROR(kParseErrorNumberMissExponent, s.Tell());
 
-      if (expMinus)
-        exp = -exp;
+      if (expMinus) exp = -exp;
     }
 
     // Finish parsing, call event according to the type of number.
@@ -1952,7 +1946,6 @@ private:
   };
 
   RAPIDJSON_FORCEINLINE Token Tokenize(Ch c) const {
-
 //!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN
 #define N NumberToken
 #define N16 N, N, N, N, N, N, N, N, N, N, N, N, N, N, N, N
@@ -2274,7 +2267,8 @@ private:
         return dst;
 
       case IterativeParsingObjectFinishState: {
-      // Transit from delimiter is only allowed when trailing commas are enabled
+        // Transit from delimiter is only allowed when trailing commas are
+        // enabled
         if (!(parseFlags & kParseTrailingCommasFlag) &&
             src == IterativeParsingMemberDelimiterState) {
           RAPIDJSON_PARSE_ERROR_NORETURN(kParseErrorObjectMissName, is.Tell());
@@ -2283,14 +2277,12 @@ private:
         // Get member count.
         SizeType c = *stack_.template Pop<SizeType>(1);
         // If the object is not empty, count the last member.
-      if (src == IterativeParsingMemberValueState)
-        ++c;
+        if (src == IterativeParsingMemberValueState) ++c;
         // Restore the state.
-      IterativeParsingState n =
-          static_cast<IterativeParsingState>(*stack_.template Pop<SizeType>(1));
+        IterativeParsingState n = static_cast<IterativeParsingState>(
+            *stack_.template Pop<SizeType>(1));
         // Transit to Finish state if this is the topmost scope.
-      if (n == IterativeParsingStartState)
-        n = IterativeParsingFinishState;
+        if (n == IterativeParsingStartState) n = IterativeParsingFinishState;
         // Call handler
         bool hr = handler.EndObject(c);
         // On handler short circuits the parsing.
@@ -2304,7 +2296,8 @@ private:
       }
 
       case IterativeParsingArrayFinishState: {
-      // Transit from delimiter is only allowed when trailing commas are enabled
+        // Transit from delimiter is only allowed when trailing commas are
+        // enabled
         if (!(parseFlags & kParseTrailingCommasFlag) &&
             src == IterativeParsingElementDelimiterState) {
           RAPIDJSON_PARSE_ERROR_NORETURN(kParseErrorValueInvalid, is.Tell());
@@ -2313,14 +2306,12 @@ private:
         // Get element count.
         SizeType c = *stack_.template Pop<SizeType>(1);
         // If the array is not empty, count the last element.
-      if (src == IterativeParsingElementState)
-        ++c;
+        if (src == IterativeParsingElementState) ++c;
         // Restore the state.
-      IterativeParsingState n =
-          static_cast<IterativeParsingState>(*stack_.template Pop<SizeType>(1));
+        IterativeParsingState n = static_cast<IterativeParsingState>(
+            *stack_.template Pop<SizeType>(1));
         // Transit to Finish state if this is the topmost scope.
-      if (n == IterativeParsingStartState)
-        n = IterativeParsingFinishState;
+        if (n == IterativeParsingStartState) n = IterativeParsingFinishState;
         // Call handler
         bool hr = handler.EndArray(c);
         // On handler short circuits the parsing.
@@ -2396,13 +2387,13 @@ private:
     }
   }
 
-  RAPIDJSON_FORCEINLINE bool
-  IsIterativeParsingDelimiterState(IterativeParsingState s) const {
+  RAPIDJSON_FORCEINLINE bool IsIterativeParsingDelimiterState(
+      IterativeParsingState s) const {
     return s >= IterativeParsingElementDelimiterState;
   }
 
-  RAPIDJSON_FORCEINLINE bool
-  IsIterativeParsingCompleteState(IterativeParsingState s) const {
+  RAPIDJSON_FORCEINLINE bool IsIterativeParsingCompleteState(
+      IterativeParsingState s) const {
     return s <= IterativeParsingErrorState;
   }
 
@@ -2436,8 +2427,7 @@ private:
     }
 
     // Handle the end of file.
-    if (state != IterativeParsingFinishState)
-      HandleError(state, is);
+    if (state != IterativeParsingFinishState) HandleError(state, is);
 
     return parseResult_;
   }

livox_ros_driver/common/rapidjson/schema.h

@@ -19,10 +19,10 @@
 #ifndef RAPIDJSON_SCHEMA_H_
 #define RAPIDJSON_SCHEMA_H_
 
+#include <cmath>  // abs, floor
 #include "document.h"
 #include "pointer.h"
 #include "stringbuffer.h"
-#include <cmath> // abs, floor
 
 #if !defined(RAPIDJSON_SCHEMA_USE_INTERNALREGEX)
 #define RAPIDJSON_SCHEMA_USE_INTERNALREGEX 1
@@ -133,11 +133,13 @@ inline void PrintValidatorPointers(unsigned depth, const wchar_t *s,
 ///////////////////////////////////////////////////////////////////////////////
 // Forward declarations
 
-template <typename ValueType, typename Allocator> class GenericSchemaDocument;
+template <typename ValueType, typename Allocator>
+class GenericSchemaDocument;
 
 namespace internal {
 
-template <typename SchemaDocumentType> class Schema;
+template <typename SchemaDocumentType>
+class Schema;
 
 ///////////////////////////////////////////////////////////////////////////////
 // ISchemaValidator
@@ -151,7 +153,8 @@ public:
 ///////////////////////////////////////////////////////////////////////////////
 // ISchemaStateFactory
 
-template <typename SchemaType> class ISchemaStateFactory {
+template <typename SchemaType>
+class ISchemaStateFactory {
  public:
   virtual ~ISchemaStateFactory() {}
   virtual ISchemaValidator *CreateSchemaValidator(const SchemaType &) = 0;
@@ -166,7 +169,8 @@ public:
 ///////////////////////////////////////////////////////////////////////////////
 // IValidationErrorHandler
 
-template <typename SchemaType> class IValidationErrorHandler {
+template <typename SchemaType>
+class IValidationErrorHandler {
  public:
   typedef typename SchemaType::Ch Ch;
   typedef typename SchemaType::SValue SValue;
@@ -219,10 +223,10 @@ public:
 
   virtual void DisallowedValue() = 0;
   virtual void StartDisallowedType() = 0;
-  virtual void
-  AddExpectedType(const typename SchemaType::ValueType &expectedType) = 0;
-  virtual void
-  EndDisallowedType(const typename SchemaType::ValueType &actualType) = 0;
+  virtual void AddExpectedType(
+      const typename SchemaType::ValueType &expectedType) = 0;
+  virtual void EndDisallowedType(
+      const typename SchemaType::ValueType &actualType) = 0;
   virtual void NotAllOf(ISchemaValidator **subvalidators, SizeType count) = 0;
   virtual void NoneOf(ISchemaValidator **subvalidators, SizeType count) = 0;
   virtual void NotOneOf(ISchemaValidator **subvalidators, SizeType count) = 0;
@@ -233,7 +237,8 @@ public:
 // Hasher
 
 // For comparison of compound value
-template <typename Encoding, typename Allocator> class Hasher {
+template <typename Encoding, typename Allocator>
+class Hasher {
  public:
   typedef typename Encoding::Ch Ch;
 
@@ -337,8 +342,7 @@ private:
     // FNV-1a from http://isthe.com/chongo/tech/comp/fnv/
     uint64_t h = Hash(RAPIDJSON_UINT64_C2(0x84222325, 0xcbf29ce4), type);
     const unsigned char *d = static_cast<const unsigned char *>(data);
-    for (size_t i = 0; i < len; i++)
-      h = Hash(h, d[i]);
+    for (size_t i = 0; i < len; i++) h = Hash(h, d[i]);
     *stack_.template Push<uint64_t>() = h;
     return true;
   }
@@ -356,7 +360,8 @@ private:
 ///////////////////////////////////////////////////////////////////////////////
 // SchemaValidationContext
 
-template <typename SchemaDocumentType> struct SchemaValidationContext {
+template <typename SchemaDocumentType>
+struct SchemaValidationContext {
   typedef Schema<SchemaDocumentType> SchemaType;
   typedef ISchemaStateFactory<SchemaType> SchemaValidatorFactoryType;
   typedef IValidationErrorHandler<SchemaType> ErrorHandlerType;
@@ -371,17 +376,27 @@ template <typename SchemaDocumentType> struct SchemaValidationContext {
 
   SchemaValidationContext(SchemaValidatorFactoryType &f, ErrorHandlerType &eh,
                           const SchemaType *s)
-      : factory(f), error_handler(eh), schema(s), valueSchema(),
-        invalidKeyword(), hasher(), arrayElementHashCodes(), validators(),
-        validatorCount(), patternPropertiesValidators(),
-        patternPropertiesValidatorCount(), patternPropertiesSchemas(),
+      : factory(f),
+        error_handler(eh),
+        schema(s),
+        valueSchema(),
+        invalidKeyword(),
+        hasher(),
+        arrayElementHashCodes(),
+        validators(),
+        validatorCount(),
+        patternPropertiesValidators(),
+        patternPropertiesValidatorCount(),
+        patternPropertiesSchemas(),
         patternPropertiesSchemaCount(),
-        valuePatternValidatorType(kPatternValidatorOnly), propertyExist(),
-        inArray(false), valueUniqueness(false), arrayUniqueness(false) {}
+        valuePatternValidatorType(kPatternValidatorOnly),
+        propertyExist(),
+        inArray(false),
+        valueUniqueness(false),
+        arrayUniqueness(false) {}
 
   ~SchemaValidationContext() {
-    if (hasher)
-      factory.DestroryHasher(hasher);
+    if (hasher) factory.DestroryHasher(hasher);
     if (validators) {
       for (SizeType i = 0; i < validatorCount; i++)
         factory.DestroySchemaValidator(validators[i]);
@@ -392,10 +407,8 @@ template <typename SchemaDocumentType> struct SchemaValidationContext {
         factory.DestroySchemaValidator(patternPropertiesValidators[i]);
       factory.FreeState(patternPropertiesValidators);
     }
-    if (patternPropertiesSchemas)
-      factory.FreeState(patternPropertiesSchemas);
-    if (propertyExist)
-      factory.FreeState(propertyExist);
+    if (patternPropertiesSchemas) factory.FreeState(patternPropertiesSchemas);
+    if (propertyExist) factory.FreeState(propertyExist);
   }
 
   SchemaValidatorFactoryType &factory;
@@ -423,7 +436,8 @@ template <typename SchemaDocumentType> struct SchemaValidationContext {
 ///////////////////////////////////////////////////////////////////////////////
 // Schema
 
-template <typename SchemaDocumentType> class Schema {
+template <typename SchemaDocumentType>
+class Schema {
  public:
   typedef typename SchemaDocumentType::ValueType ValueType;
   typedef typename SchemaDocumentType::AllocatorType AllocatorType;
@@ -439,26 +453,46 @@ public:
   Schema(SchemaDocumentType *schemaDocument, const PointerType &p,
          const ValueType &value, const ValueType &document,
          AllocatorType *allocator)
-      : allocator_(allocator), uri_(schemaDocument->GetURI(), *allocator),
-        pointer_(p, allocator), typeless_(schemaDocument->GetTypeless()),
-        enum_(), enumCount_(), not_(),
+      : allocator_(allocator),
+        uri_(schemaDocument->GetURI(), *allocator),
+        pointer_(p, allocator),
+        typeless_(schemaDocument->GetTypeless()),
+        enum_(),
+        enumCount_(),
+        not_(),
         type_((1 << kTotalSchemaType) - 1),  // typeless
-        validatorCount_(), notValidatorIndex_(), properties_(),
-        additionalPropertiesSchema_(), patternProperties_(),
-        patternPropertyCount_(), propertyCount_(), minProperties_(),
-        maxProperties_(SizeType(~0)), additionalProperties_(true),
-        hasDependencies_(), hasRequired_(), hasSchemaDependencies_(),
-        additionalItemsSchema_(), itemsList_(), itemsTuple_(),
-        itemsTupleCount_(), minItems_(), maxItems_(SizeType(~0)),
-        additionalItems_(true), uniqueItems_(false), pattern_(), minLength_(0),
-        maxLength_(~SizeType(0)), exclusiveMinimum_(false),
-        exclusiveMaximum_(false), defaultValueLength_(0) {
+        validatorCount_(),
+        notValidatorIndex_(),
+        properties_(),
+        additionalPropertiesSchema_(),
+        patternProperties_(),
+        patternPropertyCount_(),
+        propertyCount_(),
+        minProperties_(),
+        maxProperties_(SizeType(~0)),
+        additionalProperties_(true),
+        hasDependencies_(),
+        hasRequired_(),
+        hasSchemaDependencies_(),
+        additionalItemsSchema_(),
+        itemsList_(),
+        itemsTuple_(),
+        itemsTupleCount_(),
+        minItems_(),
+        maxItems_(SizeType(~0)),
+        additionalItems_(true),
+        uniqueItems_(false),
+        pattern_(),
+        minLength_(0),
+        maxLength_(~SizeType(0)),
+        exclusiveMinimum_(false),
+        exclusiveMaximum_(false),
+        defaultValueLength_(0) {
     typedef typename SchemaDocumentType::ValueType ValueType;
     typedef typename ValueType::ConstValueIterator ConstValueIterator;
     typedef typename ValueType::ConstMemberIterator ConstMemberIterator;
 
-    if (!value.IsObject())
-      return;
+    if (!value.IsObject()) return;
 
     if (const ValueType *v = GetMember(value, GetTypeString())) {
       type_ = 0;
@@ -516,8 +550,7 @@ public:
       if (required && required->IsArray())
         for (ConstValueIterator itr = required->Begin(); itr != required->End();
              ++itr)
-          if (itr->IsString())
-            AddUniqueElement(allProperties, *itr);
+          if (itr->IsString()) AddUniqueElement(allProperties, *itr);
 
       if (dependencies && dependencies->IsObject())
         for (ConstMemberIterator itr = dependencies->MemberBegin();
@@ -526,8 +559,7 @@ public:
           if (itr->value.IsArray())
             for (ConstValueIterator i = itr->value.Begin();
                  i != itr->value.End(); ++i)
-              if (i->IsString())
-                AddUniqueElement(allProperties, *i);
+              if (i->IsString()) AddUniqueElement(allProperties, *i);
         }
 
       if (allProperties.Size() > 0) {
@@ -668,12 +700,10 @@ public:
 
     // Number
     if (const ValueType *v = GetMember(value, GetMinimumString()))
-      if (v->IsNumber())
-        minimum_.CopyFrom(*v, *allocator_);
+      if (v->IsNumber()) minimum_.CopyFrom(*v, *allocator_);
 
     if (const ValueType *v = GetMember(value, GetMaximumString()))
-      if (v->IsNumber())
-        maximum_.CopyFrom(*v, *allocator_);
+      if (v->IsNumber()) maximum_.CopyFrom(*v, *allocator_);
 
     AssignIfExist(exclusiveMinimum_, value, GetExclusiveMinimumString());
     AssignIfExist(exclusiveMaximum_, value, GetExclusiveMaximumString());
@@ -684,15 +714,13 @@ public:
 
     // Default
     if (const ValueType *v = GetMember(value, GetDefaultValueString()))
-      if (v->IsString())
-        defaultValueLength_ = v->GetStringLength();
+      if (v->IsString()) defaultValueLength_ = v->GetStringLength();
   }
 
   ~Schema() {
     AllocatorType::Free(enum_);
     if (properties_) {
-      for (SizeType i = 0; i < propertyCount_; i++)
-        properties_[i].~Property();
+      for (SizeType i = 0; i < propertyCount_; i++) properties_[i].~Property();
       AllocatorType::Free(properties_);
     }
     if (patternProperties_) {
@@ -715,8 +743,7 @@ public:
 
   bool BeginValue(Context &context) const {
     if (context.inArray) {
-      if (uniqueItems_)
-        context.valueUniqueness = true;
+      if (uniqueItems_) context.valueUniqueness = true;
 
       if (itemsList_)
         context.valueSchema = itemsList_;
@@ -778,8 +805,7 @@ public:
     if (enum_) {
       const uint64_t h = context.factory.GetHashCode(context.hasher);
       for (SizeType i = 0; i < enumCount_; i++)
-        if (enum_[i] == h)
-          goto foundEnum;
+        if (enum_[i] == h) goto foundEnum;
       context.error_handler.DisallowedValue();
       RAPIDJSON_INVALID_KEYWORD_RETURN(GetEnumString());
     foundEnum:;
@@ -795,8 +821,7 @@ public:
 
     if (anyOf_.schemas) {
       for (SizeType i = anyOf_.begin; i < anyOf_.begin + anyOf_.count; i++)
-        if (context.validators[i]->IsValid())
-          goto foundAny;
+        if (context.validators[i]->IsValid()) goto foundAny;
       context.error_handler.NoneOf(&context.validators[anyOf_.begin],
                                    anyOf_.count);
       RAPIDJSON_INVALID_KEYWORD_RETURN(GetAnyOfString());
@@ -846,26 +871,22 @@ public:
   }
 
   bool Int(Context &context, int i) const {
-    if (!CheckInt(context, i))
-      return false;
+    if (!CheckInt(context, i)) return false;
     return CreateParallelValidator(context);
   }
 
   bool Uint(Context &context, unsigned u) const {
-    if (!CheckUint(context, u))
-      return false;
+    if (!CheckUint(context, u)) return false;
     return CreateParallelValidator(context);
   }
 
   bool Int64(Context &context, int64_t i) const {
-    if (!CheckInt(context, i))
-      return false;
+    if (!CheckInt(context, i)) return false;
     return CreateParallelValidator(context);
   }
 
   bool Uint64(Context &context, uint64_t u) const {
-    if (!CheckUint(context, u))
-      return false;
+    if (!CheckUint(context, u)) return false;
     return CreateParallelValidator(context);
   }
 
@@ -875,11 +896,9 @@ public:
       RAPIDJSON_INVALID_KEYWORD_RETURN(GetTypeString());
     }
 
-    if (!minimum_.IsNull() && !CheckDoubleMinimum(context, d))
-      return false;
+    if (!minimum_.IsNull() && !CheckDoubleMinimum(context, d)) return false;
 
-    if (!maximum_.IsNull() && !CheckDoubleMaximum(context, d))
-      return false;
+    if (!maximum_.IsNull() && !CheckDoubleMaximum(context, d)) return false;
 
     if (!multipleOf_.IsNull() && !CheckDoubleMultipleOf(context, d))
       return false;
@@ -965,8 +984,7 @@ public:
       } else
         context.valueSchema = properties_[index].schema;
 
-      if (context.propertyExist)
-        context.propertyExist[index] = true;
+      if (context.propertyExist) context.propertyExist[index] = true;
 
       return true;
     }
@@ -1166,8 +1184,7 @@ private:
   template <typename V1, typename V2>
   void AddUniqueElement(V1 &a, const V2 &v) {
     for (typename V1::ConstValueIterator itr = a.Begin(); itr != a.End(); ++itr)
-      if (*itr == v)
-        return;
+      if (*itr == v) return;
     V1 c(v, *allocator_);
     a.PushBack(c, *allocator_);
   }
@@ -1181,8 +1198,7 @@ private:
   static void AssignIfExist(bool &out, const ValueType &value,
                             const ValueType &name) {
     if (const ValueType *v = GetMember(value, name))
-      if (v->IsBool())
-        out = v->GetBool();
+      if (v->IsBool()) out = v->GetBool();
   }
 
   static void AssignIfExist(SizeType &out, const ValueType &value,
@@ -1255,7 +1271,8 @@ private:
     return std::regex_search(str, str + length, r, *pattern);
   }
 #else
-  template <typename ValueType> RegexType *CreatePattern(const ValueType &) {
+  template <typename ValueType>
+  RegexType *CreatePattern(const ValueType &) {
     return 0;
   }
 
@@ -1292,14 +1309,11 @@ private:
               sizeof(ISchemaValidator *) * validatorCount_));
       context.validatorCount = validatorCount_;
 
-      if (allOf_.schemas)
-        CreateSchemaValidators(context, allOf_);
+      if (allOf_.schemas) CreateSchemaValidators(context, allOf_);
 
-      if (anyOf_.schemas)
-        CreateSchemaValidators(context, anyOf_);
+      if (anyOf_.schemas) CreateSchemaValidators(context, anyOf_);
 
-      if (oneOf_.schemas)
-        CreateSchemaValidators(context, oneOf_);
+      if (oneOf_.schemas) CreateSchemaValidators(context, oneOf_);
 
       if (not_)
         context.validators[notValidatorIndex_] =
@@ -1467,16 +1481,12 @@ private:
     ErrorHandler &eh = context.error_handler;
     eh.StartDisallowedType();
 
-    if (type_ & (1 << kNullSchemaType))
-      eh.AddExpectedType(GetNullString());
+    if (type_ & (1 << kNullSchemaType)) eh.AddExpectedType(GetNullString());
     if (type_ & (1 << kBooleanSchemaType))
       eh.AddExpectedType(GetBooleanString());
-    if (type_ & (1 << kObjectSchemaType))
-      eh.AddExpectedType(GetObjectString());
-    if (type_ & (1 << kArraySchemaType))
-      eh.AddExpectedType(GetArrayString());
-    if (type_ & (1 << kStringSchemaType))
-      eh.AddExpectedType(GetStringString());
+    if (type_ & (1 << kObjectSchemaType)) eh.AddExpectedType(GetObjectString());
+    if (type_ & (1 << kArraySchemaType)) eh.AddExpectedType(GetArrayString());
+    if (type_ & (1 << kStringSchemaType)) eh.AddExpectedType(GetStringString());
 
     if (type_ & (1 << kNumberSchemaType))
       eh.AddExpectedType(GetNumberString());
@@ -1488,8 +1498,11 @@ private:
 
   struct Property {
     Property()
-        : schema(), dependenciesSchema(), dependenciesValidatorIndex(),
-          dependencies(), required(false) {}
+        : schema(),
+          dependenciesSchema(),
+          dependenciesValidatorIndex(),
+          dependencies(),
+          required(false) {}
     ~Property() { AllocatorType::Free(dependencies); }
     SValue name;
     const SchemaType *schema;
@@ -1559,7 +1572,8 @@ private:
   SizeType defaultValueLength_;
 };
 
-template <typename Stack, typename Ch> struct TokenHelper {
+template <typename Stack, typename Ch>
+struct TokenHelper {
   RAPIDJSON_FORCEINLINE static void AppendIndexToken(Stack &documentStack,
                                                      SizeType index) {
     *documentStack.template Push<Ch>() = '/';
@@ -1574,7 +1588,8 @@ template <typename Stack, typename Ch> struct TokenHelper {
 };
 
 // Partial specialized version for char to prevent buffer copying.
-template <typename Stack> struct TokenHelper<Stack, char> {
+template <typename Stack>
+struct TokenHelper<Stack, char> {
   RAPIDJSON_FORCEINLINE static void AppendIndexToken(Stack &documentStack,
                                                      SizeType index) {
     if (sizeof(SizeType) == 4) {
@@ -1634,7 +1649,8 @@ public:
   typedef GenericPointer<ValueType, Allocator> PointerType;
   typedef GenericValue<EncodingType, Allocator> URIType;
   friend class internal::Schema<GenericSchemaDocument>;
-  template <typename, typename, typename> friend class GenericSchemaValidator;
+  template <typename, typename, typename>
+  friend class GenericSchemaValidator;
 
   //! Constructor.
   /*!
@@ -1651,11 +1667,14 @@ public:
       const ValueType &document, const Ch *uri = 0, SizeType uriLength = 0,
       IRemoteSchemaDocumentProviderType *remoteProvider = 0,
       Allocator *allocator = 0)
-      : remoteProvider_(remoteProvider), allocator_(allocator), ownAllocator_(),
-        root_(), typeless_(), schemaMap_(allocator, kInitialSchemaMapSize),
+      : remoteProvider_(remoteProvider),
+        allocator_(allocator),
+        ownAllocator_(),
+        root_(),
+        typeless_(),
+        schemaMap_(allocator, kInitialSchemaMapSize),
         schemaRef_(allocator, kInitialSchemaRefSize) {
-    if (!allocator_)
-      ownAllocator_ = allocator_ = RAPIDJSON_NEW(Allocator)();
+    if (!allocator_) ownAllocator_ = allocator_ = RAPIDJSON_NEW(Allocator)();
 
     Ch noUri[1] = {0};
     uri_.SetString(uri ? uri : noUri, uriLength, *allocator_);
@@ -1674,8 +1693,7 @@ public:
     while (!schemaRef_.Empty()) {
       SchemaRefEntry *refEntry = schemaRef_.template Pop<SchemaRefEntry>(1);
       if (const SchemaType *s = GetSchema(refEntry->target)) {
-        if (refEntry->schema)
-          *refEntry->schema = s;
+        if (refEntry->schema) *refEntry->schema = s;
 
         // Create entry in map if not exist
         if (!GetSchema(refEntry->source)) {
@@ -1762,13 +1780,11 @@ private:
   void CreateSchemaRecursive(const SchemaType **schema,
                              const PointerType &pointer, const ValueType &v,
                              const ValueType &document) {
-    if (schema)
-      *schema = typeless_;
+    if (schema) *schema = typeless_;
 
     if (v.GetType() == kObjectType) {
       const SchemaType *s = GetSchema(pointer);
-      if (!s)
-        CreateSchema(schema, pointer, v, document);
+      if (!s) CreateSchema(schema, pointer, v, document);
 
       for (typename ValueType::ConstMemberIterator itr = v.MemberBegin();
            itr != v.MemberEnd(); ++itr)
@@ -1788,8 +1804,7 @@ private:
             SchemaType(this, pointer, v, document, allocator_);
         new (schemaMap_.template Push<SchemaEntry>())
             SchemaEntry(pointer, s, true, allocator_);
-        if (schema)
-          *schema = s;
+        if (schema) *schema = s;
       }
     }
   }
@@ -1800,8 +1815,7 @@ private:
     static const ValueType kRefValue(kRefString, 4);
 
     typename ValueType::ConstMemberIterator itr = v.FindMember(kRefValue);
-    if (itr == v.MemberEnd())
-      return false;
+    if (itr == v.MemberEnd()) return false;
 
     if (itr->value.IsString()) {
       SizeType len = itr->value.GetStringLength();
@@ -1818,8 +1832,7 @@ private:
               PointerType pointer(&s[i], len - i, allocator_);
               if (pointer.IsValid()) {
                 if (const SchemaType *sc = remoteDocument->GetSchema(pointer)) {
-                  if (schema)
-                    *schema = sc;
+                  if (schema) *schema = sc;
                   new (schemaMap_.template Push<SchemaEntry>()) SchemaEntry(
                       source, const_cast<SchemaType *>(sc), false, allocator_);
                   return true;
@@ -1831,8 +1844,7 @@ private:
           PointerType pointer(&s[i], len - i, allocator_);
           if (pointer.IsValid()) {
             if (const ValueType *nv = pointer.Get(document))
-              if (HandleRefSchema(source, schema, *nv, document))
-                return true;
+              if (HandleRefSchema(source, schema, *nv, document)) return true;
 
             new (schemaRef_.template Push<SchemaRefEntry>())
                 SchemaRefEntry(source, pointer, schema, allocator_);
@@ -1847,16 +1859,14 @@ private:
   const SchemaType *GetSchema(const PointerType &pointer) const {
     for (const SchemaEntry *target = schemaMap_.template Bottom<SchemaEntry>();
          target != schemaMap_.template End<SchemaEntry>(); ++target)
-      if (pointer == target->pointer)
-        return target->schema;
+      if (pointer == target->pointer) return target->schema;
     return 0;
   }
 
   PointerType GetPointer(const SchemaType *schema) const {
     for (const SchemaEntry *target = schemaMap_.template Bottom<SchemaEntry>();
          target != schemaMap_.template End<SchemaEntry>(); ++target)
-      if (schema == target->schema)
-        return target->pointer;
+      if (schema == target->schema) return target->pointer;
     return PointerType();
   }
 
@@ -1927,11 +1937,17 @@ public:
       const SchemaDocumentType &schemaDocument, StateAllocator *allocator = 0,
       size_t schemaStackCapacity = kDefaultSchemaStackCapacity,
       size_t documentStackCapacity = kDefaultDocumentStackCapacity)
-      : schemaDocument_(&schemaDocument), root_(schemaDocument.GetRoot()),
-        stateAllocator_(allocator), ownStateAllocator_(0),
+      : schemaDocument_(&schemaDocument),
+        root_(schemaDocument.GetRoot()),
+        stateAllocator_(allocator),
+        ownStateAllocator_(0),
         schemaStack_(allocator, schemaStackCapacity),
-        documentStack_(allocator, documentStackCapacity), outputHandler_(0),
-        error_(kObjectType), currentError_(), missingDependents_(), valid_(true)
+        documentStack_(allocator, documentStackCapacity),
+        outputHandler_(0),
+        error_(kObjectType),
+        currentError_(),
+        missingDependents_(),
+        valid_(true)
 #if RAPIDJSON_SCHEMA_VERBOSE
         ,
         depth_(0)
@@ -1952,12 +1968,17 @@ public:
       StateAllocator *allocator = 0,
       size_t schemaStackCapacity = kDefaultSchemaStackCapacity,
       size_t documentStackCapacity = kDefaultDocumentStackCapacity)
-      : schemaDocument_(&schemaDocument), root_(schemaDocument.GetRoot()),
-        stateAllocator_(allocator), ownStateAllocator_(0),
+      : schemaDocument_(&schemaDocument),
+        root_(schemaDocument.GetRoot()),
+        stateAllocator_(allocator),
+        ownStateAllocator_(0),
         schemaStack_(allocator, schemaStackCapacity),
         documentStack_(allocator, documentStackCapacity),
-        outputHandler_(&outputHandler), error_(kObjectType), currentError_(),
-        missingDependents_(), valid_(true)
+        outputHandler_(&outputHandler),
+        error_(kObjectType),
+        currentError_(),
+        missingDependents_(),
+        valid_(true)
 #if RAPIDJSON_SCHEMA_VERBOSE
         ,
         depth_(0)
@@ -1973,8 +1994,7 @@ public:
 
   //! Reset the internal states.
   void Reset() {
-    while (!schemaStack_.Empty())
-      PopSchema();
+    while (!schemaStack_.Empty()) PopSchema();
     documentStack_.Clear();
     error_.SetObject();
     currentError_.SetNull();
@@ -2109,8 +2129,7 @@ public:
                            GetStateAllocator());
   }
   bool EndMissingProperties() {
-    if (currentError_.Empty())
-      return false;
+    if (currentError_.Empty()) return false;
     ValueType error(kObjectType);
     error.AddMember(GetMissingString(), currentError_, GetStateAllocator());
     currentError_ = error;
@@ -2149,8 +2168,7 @@ public:
         GetStateAllocator());
   }
   bool EndDependencyErrors() {
-    if (currentError_.ObjectEmpty())
-      return false;
+    if (currentError_.ObjectEmpty()) return false;
     ValueType error(kObjectType);
     error.AddMember(GetErrorsString(), currentError_, GetStateAllocator());
     currentError_ = error;
@@ -2227,8 +2245,7 @@ public:
 #endif
 
 #define RAPIDJSON_SCHEMA_HANDLE_BEGIN_(method, arg1)   \
-  if (!valid_)                                                                 \
-    return false;                                                              \
+  if (!valid_) return false;                           \
   if (!BeginValue() || !CurrentSchema().method arg1) { \
     RAPIDJSON_SCHEMA_HANDLE_BEGIN_VERBOSE_();          \
     return valid_ = false;                             \
@@ -2294,8 +2311,7 @@ public:
   }
 
   bool Key(const Ch *str, SizeType len, bool copy) {
-    if (!valid_)
-      return false;
+    if (!valid_) return false;
     AppendToken(str, len);
     if (!CurrentSchema().Key(CurrentContext(), str, len, copy))
       return valid_ = false;
@@ -2304,8 +2320,7 @@ public:
   }
 
   bool EndObject(SizeType memberCount) {
-    if (!valid_)
-      return false;
+    if (!valid_) return false;
     RAPIDJSON_SCHEMA_HANDLE_PARALLEL_(EndObject, (memberCount));
     if (!CurrentSchema().EndObject(CurrentContext(), memberCount))
       return valid_ = false;
@@ -2319,8 +2334,7 @@ public:
   }
 
   bool EndArray(SizeType elementCount) {
-    if (!valid_)
-      return false;
+    if (!valid_) return false;
     RAPIDJSON_SCHEMA_HANDLE_PARALLEL_(EndArray, (elementCount));
     if (!CurrentSchema().EndArray(CurrentContext(), elementCount))
       return valid_ = false;
@@ -2386,11 +2400,17 @@ private:
       StateAllocator *allocator = 0,
       size_t schemaStackCapacity = kDefaultSchemaStackCapacity,
       size_t documentStackCapacity = kDefaultDocumentStackCapacity)
-      : schemaDocument_(&schemaDocument), root_(root),
-        stateAllocator_(allocator), ownStateAllocator_(0),
+      : schemaDocument_(&schemaDocument),
+        root_(root),
+        stateAllocator_(allocator),
+        ownStateAllocator_(0),
         schemaStack_(allocator, schemaStackCapacity),
-        documentStack_(allocator, documentStackCapacity), outputHandler_(0),
-        error_(kObjectType), currentError_(), missingDependents_(), valid_(true)
+        documentStack_(allocator, documentStackCapacity),
+        outputHandler_(0),
+        error_(kObjectType),
+        currentError_(),
+        missingDependents_(),
+        valid_(true)
 #if RAPIDJSON_SCHEMA_VERBOSE
         ,
         depth_(depth)
@@ -2417,8 +2437,7 @@ private:
                                                     CurrentContext()
                                                         .arrayElementIndex);
 
-      if (!CurrentSchema().BeginValue(CurrentContext()))
-        return false;
+      if (!CurrentSchema().BeginValue(CurrentContext())) return false;
 
       SizeType count = CurrentContext().patternPropertiesSchemaCount;
       const SchemaType **sa = CurrentContext().patternPropertiesSchemas;
@@ -2445,8 +2464,7 @@ private:
   }
 
   bool EndValue() {
-    if (!CurrentSchema().EndValue(CurrentContext()))
-      return false;
+    if (!CurrentSchema().EndValue(CurrentContext())) return false;
 
 #if RAPIDJSON_SCHEMA_VERBOSE
     GenericStringBuffer<EncodingType> sb;
@@ -2495,7 +2513,8 @@ private:
 
   void AppendToken(const Ch *str, SizeType len) {
     documentStack_.template Reserve<Ch>(
-        1 + len * 2); // worst case all characters are escaped as two characters
+        1 +
+        len * 2);  // worst case all characters are escaped as two characters
     *documentStack_.template PushUnsafe<Ch>() = '/';
     for (SizeType i = 0; i < len; i++) {
       if (str[i] == '~') {
@@ -2575,9 +2594,9 @@ private:
     }
   }
 
-  void
-  AddNumberError(const typename SchemaType::ValueType &keyword,
-                 ValueType &actual, const SValue &expected,
+  void AddNumberError(
+      const typename SchemaType::ValueType &keyword, ValueType &actual,
+      const SValue &expected,
       const typename SchemaType::ValueType &(*exclusive)() = 0) {
     currentError_.SetObject();
     currentError_.AddMember(GetActualString(), actual, GetStateAllocator());
@@ -2664,10 +2683,14 @@ public:
       \param sd Schema document.
   */
   SchemaValidatingReader(InputStream &is, const SchemaDocumentType &sd)
-      : is_(is), sd_(sd), invalidSchemaKeyword_(), error_(kObjectType),
+      : is_(is),
+        sd_(sd),
+        invalidSchemaKeyword_(),
+        error_(kObjectType),
         isValid_(true) {}
 
-  template <typename Handler> bool operator()(Handler &handler) {
+  template <typename Handler>
+  bool operator()(Handler &handler) {
     GenericReader<SourceEncoding, typename SchemaDocumentType::EncodingType,
                   StackAllocator>
         reader;

livox_ros_driver/common/rapidjson/stream.h

@@ -75,7 +75,8 @@ next character. Ch Take();
    configuration. See TEST(Reader, CustomStringStream) in readertest.cpp for
    example.
 */
-template <typename Stream> struct StreamTraits {
+template <typename Stream>
+struct StreamTraits {
   //! Whether to make local copy of stream for optimization during parsing.
   /*!
       By default, for safety, streams do not use local copy optimization.
@@ -102,8 +103,7 @@ inline void PutUnsafe(Stream &stream, typename Stream::Ch c) {
 template <typename Stream, typename Ch>
 inline void PutN(Stream &stream, Ch c, size_t n) {
   PutReserve(stream, n);
-  for (size_t i = 0; i < n; i++)
-    PutUnsafe(stream, c);
+  for (size_t i = 0; i < n; i++) PutUnsafe(stream, c);
 }
 
 ///////////////////////////////////////////////////////////////////////////////
@@ -156,7 +156,8 @@ RAPIDJSON_DIAG_POP
 //! Read-only string stream.
 /*! \note implements Stream concept
  */
-template <typename Encoding> struct GenericStringStream {
+template <typename Encoding>
+struct GenericStringStream {
   typedef typename Encoding::Ch Ch;
 
   GenericStringStream(const Ch *src) : src_(src), head_(src) {}
@@ -195,7 +196,8 @@ typedef GenericStringStream<UTF8<>> StringStream;
 /*! This string stream is particularly designed for in-situ parsing.
     \note implements Stream concept
 */
-template <typename Encoding> struct GenericInsituStringStream {
+template <typename Encoding>
+struct GenericInsituStringStream {
   typedef typename Encoding::Ch Ch;
 
   GenericInsituStringStream(Ch *src) : src_(src), dst_(0), head_(src) {}

livox_ros_driver/common/rapidjson/stringbuffer.h

@@ -54,8 +54,7 @@ public:
   GenericStringBuffer(GenericStringBuffer &&rhs)
       : stack_(std::move(rhs.stack_)) {}
   GenericStringBuffer &operator=(GenericStringBuffer &&rhs) {
-    if (&rhs != this)
-      stack_ = std::move(rhs.stack_);
+    if (&rhs != this) stack_ = std::move(rhs.stack_);
     return *this;
   }
 #endif

livox_ros_driver/common/rapidjson/writer.h

@@ -19,6 +19,7 @@
 #ifndef RAPIDJSON_WRITER_H_
 #define RAPIDJSON_WRITER_H_
 
+#include <new>  // placement new
 #include "internal/clzll.h"
 #include "internal/dtoa.h"
 #include "internal/itoa.h"
@@ -27,7 +28,6 @@
 #include "internal/strfunc.h"
 #include "stream.h"
 #include "stringbuffer.h"
-#include <new> // placement new
 
 #if defined(RAPIDJSON_SIMD) && defined(_MSC_VER)
 #include <intrin.h>
@@ -112,18 +112,24 @@ public:
   */
   explicit Writer(OutputStream &os, StackAllocator *stackAllocator = 0,
                   size_t levelDepth = kDefaultLevelDepth)
-      : os_(&os), level_stack_(stackAllocator, levelDepth * sizeof(Level)),
-        maxDecimalPlaces_(kDefaultMaxDecimalPlaces), hasRoot_(false) {}
+      : os_(&os),
+        level_stack_(stackAllocator, levelDepth * sizeof(Level)),
+        maxDecimalPlaces_(kDefaultMaxDecimalPlaces),
+        hasRoot_(false) {}
 
   explicit Writer(StackAllocator *allocator = 0,
                   size_t levelDepth = kDefaultLevelDepth)
-      : os_(0), level_stack_(allocator, levelDepth * sizeof(Level)),
-        maxDecimalPlaces_(kDefaultMaxDecimalPlaces), hasRoot_(false) {}
+      : os_(0),
+        level_stack_(allocator, levelDepth * sizeof(Level)),
+        maxDecimalPlaces_(kDefaultMaxDecimalPlaces),
+        hasRoot_(false) {}
 
 #if RAPIDJSON_HAS_CXX11_RVALUE_REFS
   Writer(Writer &&rhs)
-      : os_(rhs.os_), level_stack_(std::move(rhs.level_stack_)),
-        maxDecimalPlaces_(rhs.maxDecimalPlaces_), hasRoot_(rhs.hasRoot_) {
+      : os_(rhs.os_),
+        level_stack_(std::move(rhs.level_stack_)),
+        maxDecimalPlaces_(rhs.maxDecimalPlaces_),
+        hasRoot_(rhs.hasRoot_) {
     rhs.os_ = 0;
   }
 #endif
@@ -268,7 +274,8 @@ public:
                      sizeof(Level));  // not inside an Object
     RAPIDJSON_ASSERT(!level_stack_.template Top<Level>()
                           ->inArray);  // currently inside an Array, not Object
-    RAPIDJSON_ASSERT(0 == level_stack_.template Top<Level>()->valueCount %
+    RAPIDJSON_ASSERT(0 ==
+                     level_stack_.template Top<Level>()->valueCount %
                          2);  // Object has a Key without a Value
     level_stack_.template Pop<Level>(1);
     return EndValue(WriteEndObject());
@@ -395,8 +402,7 @@ protected:
 
   bool WriteDouble(double d) {
     if (internal::Double(d).IsNanOrInf()) {
-      if (!(writeFlags & kWriteNanAndInfFlag))
-        return false;
+      if (!(writeFlags & kWriteNanAndInfFlag)) return false;
       if (internal::Double(d).IsNan()) {
         PutReserve(*os_, 3);
         PutUnsafe(*os_, 'N');
@@ -592,40 +598,44 @@ private:
 
 // Full specialization for StringStream to prevent memory copying
 
-template <> inline bool Writer<StringBuffer>::WriteInt(int i) {
+template <>
+inline bool Writer<StringBuffer>::WriteInt(int i) {
   char *buffer = os_->Push(11);
   const char *end = internal::i32toa(i, buffer);
   os_->Pop(static_cast<size_t>(11 - (end - buffer)));
   return true;
 }
 
-template <> inline bool Writer<StringBuffer>::WriteUint(unsigned u) {
+template <>
+inline bool Writer<StringBuffer>::WriteUint(unsigned u) {
   char *buffer = os_->Push(10);
   const char *end = internal::u32toa(u, buffer);
   os_->Pop(static_cast<size_t>(10 - (end - buffer)));
   return true;
 }
 
-template <> inline bool Writer<StringBuffer>::WriteInt64(int64_t i64) {
+template <>
+inline bool Writer<StringBuffer>::WriteInt64(int64_t i64) {
   char *buffer = os_->Push(21);
   const char *end = internal::i64toa(i64, buffer);
   os_->Pop(static_cast<size_t>(21 - (end - buffer)));
   return true;
 }
 
-template <> inline bool Writer<StringBuffer>::WriteUint64(uint64_t u) {
+template <>
+inline bool Writer<StringBuffer>::WriteUint64(uint64_t u) {
   char *buffer = os_->Push(20);
   const char *end = internal::u64toa(u, buffer);
   os_->Pop(static_cast<size_t>(20 - (end - buffer)));
   return true;
 }
 
-template <> inline bool Writer<StringBuffer>::WriteDouble(double d) {
+template <>
+inline bool Writer<StringBuffer>::WriteDouble(double d) {
   if (internal::Double(d).IsNanOrInf()) {
     // Note: This code path can only be reached if
     // (RAPIDJSON_WRITE_DEFAULT_FLAGS & kWriteNanAndInfFlag).
-    if (!(kWriteDefaultFlags & kWriteNanAndInfFlag))
-      return false;
+    if (!(kWriteDefaultFlags & kWriteNanAndInfFlag)) return false;
     if (internal::Double(d).IsNan()) {
       PutReserve(*os_, 3);
       PutUnsafe(*os_, 'N');
@@ -659,11 +669,9 @@ template <> inline bool Writer<StringBuffer>::WriteDouble(double d) {
 template <>
 inline bool Writer<StringBuffer>::ScanWriteUnescapedString(StringStream &is,
                                                            size_t length) {
-  if (length < 16)
-    return RAPIDJSON_LIKELY(is.Tell() < length);
+  if (length < 16) return RAPIDJSON_LIKELY(is.Tell() < length);
 
-  if (!RAPIDJSON_LIKELY(is.Tell() < length))
-    return false;
+  if (!RAPIDJSON_LIKELY(is.Tell() < length)) return false;
 
   const char *p = is.src_;
   const char *end = is.head_ + length;
@@ -671,8 +679,7 @@ inline bool Writer<StringBuffer>::ScanWriteUnescapedString(StringStream &is,
       (reinterpret_cast<size_t>(p) + 15) & static_cast<size_t>(~15));
   const char *endAligned = reinterpret_cast<const char *>(
       reinterpret_cast<size_t>(end) & static_cast<size_t>(~15));
-  if (nextAligned > end)
-    return true;
+  if (nextAligned > end) return true;
 
   while (p != nextAligned)
     if (*p < 0x20 || *p == '\"' || *p == '\\') {
@@ -716,8 +723,7 @@ inline bool Writer<StringBuffer>::ScanWriteUnescapedString(StringStream &is,
       len = static_cast<SizeType>(__builtin_ffs(r) - 1);
 #endif
       char *q = reinterpret_cast<char *>(os_->PushUnsafe(len));
-      for (size_t i = 0; i < len; i++)
-        q[i] = p[i];
+      for (size_t i = 0; i < len; i++) q[i] = p[i];
 
       p += len;
       break;
@@ -732,11 +738,9 @@ inline bool Writer<StringBuffer>::ScanWriteUnescapedString(StringStream &is,
 template <>
 inline bool Writer<StringBuffer>::ScanWriteUnescapedString(StringStream &is,
                                                            size_t length) {
-  if (length < 16)
-    return RAPIDJSON_LIKELY(is.Tell() < length);
+  if (length < 16) return RAPIDJSON_LIKELY(is.Tell() < length);
 
-  if (!RAPIDJSON_LIKELY(is.Tell() < length))
-    return false;
+  if (!RAPIDJSON_LIKELY(is.Tell() < length)) return false;
 
   const char *p = is.src_;
   const char *end = is.head_ + length;
@@ -744,8 +748,7 @@ inline bool Writer<StringBuffer>::ScanWriteUnescapedString(StringStream &is,
       (reinterpret_cast<size_t>(p) + 15) & static_cast<size_t>(~15));
   const char *endAligned = reinterpret_cast<const char *>(
       reinterpret_cast<size_t>(end) & static_cast<size_t>(~15));
-  if (nextAligned > end)
-    return true;
+  if (nextAligned > end) return true;
 
   while (p != nextAligned)
     if (*p < 0x20 || *p == '\"' || *p == '\\') {
@@ -786,8 +789,7 @@ inline bool Writer<StringBuffer>::ScanWriteUnescapedString(StringStream &is,
     }
     if (RAPIDJSON_UNLIKELY(escaped)) {  // some of characters is escaped
       char *q = reinterpret_cast<char *>(os_->PushUnsafe(len));
-      for (size_t i = 0; i < len; i++)
-        q[i] = p[i];
+      for (size_t i = 0; i < len; i++) q[i] = p[i];
 
       p += len;
       break;

livox_ros_driver/common/rapidxml/rapidxml.hpp

@@ -73,7 +73,6 @@ namespace rapidxml {
 //! <br><br>
 //! This class derives from <code>std::exception</code> class.
 class parse_error : public std::exception {
-
  public:
   //! Constructs parse error
   parse_error(const char *what, void *where) : m_what(what), m_where(where) {}
@@ -86,7 +85,8 @@ public:
   //! Ch should be the same as char type of xml_document that produced the
   //! error. \return Pointer to location within the parsed string where error
   //! occured.
-  template <class Ch> Ch *where() const {
+  template <class Ch>
+  Ch *where() const {
     return reinterpret_cast<Ch *>(m_where);
   }
 
@@ -129,23 +129,29 @@ private:
 
 namespace rapidxml {
 // Forward declarations
-template <class Ch> class xml_node;
-template <class Ch> class xml_attribute;
-template <class Ch> class xml_document;
+template <class Ch>
+class xml_node;
+template <class Ch>
+class xml_attribute;
+template <class Ch>
+class xml_document;
 
 //! Enumeration listing all node types produced by the parser.
 //! Use xml_node::type() function to query node type.
 enum node_type {
   node_document,     //!< A document node. Name and value are empty.
-  node_element, //!< An element node. Name contains element name. Value contains
+  node_element,      //!< An element node. Name contains element name. Value
+                     //!contains
                      //!< text of first data node.
   node_data,         //!< A data node. Name is empty. Value contains data text.
   node_cdata,        //!< A CDATA node. Name is empty. Value contains data text.
-  node_comment, //!< A comment node. Name is empty. Value contains comment text.
+  node_comment,      //!< A comment node. Name is empty. Value contains comment
+                     //!text.
   node_declaration,  //!< A declaration node. Name and value are empty.
                      //!< Declaration parameters (version, encoding and
                      //!< standalone) are in node attributes.
-  node_doctype, //!< A DOCTYPE node. Name is empty. Value contains DOCTYPE text.
+  node_doctype,      //!< A DOCTYPE node. Name is empty. Value contains DOCTYPE
+                     //!text.
   node_pi  //!< A PI node. Name contains target. Value contains instructions.
 };
 
@@ -280,13 +286,15 @@ namespace internal {
 // Struct that contains lookup tables for the parser
 // It must be a template to allow correct linking (because it has static data
 // members, which are defined in a header file).
-template <int Dummy> struct lookup_tables {
+template <int Dummy>
+struct lookup_tables {
   static const unsigned char lookup_whitespace[256];         // Whitespace table
   static const unsigned char lookup_node_name[256];          // Node name table
   static const unsigned char lookup_text[256];               // Text table
   static const unsigned char lookup_text_pure_no_ws[256];    // Text table
   static const unsigned char lookup_text_pure_with_ws[256];  // Text table
-  static const unsigned char lookup_attribute_name[256]; // Attribute name table
+  static const unsigned char
+      lookup_attribute_name[256];  // Attribute name table
   static const unsigned char
       lookup_attribute_data_1[256];  // Attribute data table with single quote
   static const unsigned char
@@ -303,10 +311,10 @@ template <int Dummy> struct lookup_tables {
 };
 
 // Find length of the string
-template <class Ch> inline std::size_t measure(const Ch *p) {
+template <class Ch>
+inline std::size_t measure(const Ch *p) {
   const Ch *tmp = p;
-  while (*tmp)
-    ++tmp;
+  while (*tmp) ++tmp;
   return tmp - p;
 }
 
@@ -314,12 +322,10 @@ template <class Ch> inline std::size_t measure(const Ch *p) {
 template <class Ch>
 inline bool compare(const Ch *p1, std::size_t size1, const Ch *p2,
                     std::size_t size2, bool case_sensitive) {
-  if (size1 != size2)
-    return false;
+  if (size1 != size2) return false;
   if (case_sensitive) {
     for (const Ch *end = p1 + size1; p1 < end; ++p1, ++p2)
-      if (*p1 != *p2)
-        return false;
+      if (*p1 != *p2) return false;
   } else {
     for (const Ch *end = p1 + size1; p1 < end; ++p1, ++p2)
       if (lookup_tables<0>::lookup_upcase[static_cast<unsigned char>(*p1)] !=
@@ -370,8 +376,8 @@ inline bool compare(const Ch *p1, std::size_t size1, const Ch *p2,
 //! to obtain best wasted memory to performance compromise.
 //! To do it, define their values before rapidxml.hpp file is included.
 //! \param Ch Character type of created nodes.
-template <class Ch = char> class memory_pool {
-
+template <class Ch = char>
+class memory_pool {
  public:
   //! \cond internal
   typedef void *(alloc_func)(
@@ -461,13 +467,12 @@ public:
   //! be specified and null terminated. \return Pointer to allocated char array.
   //! This pointer will never be NULL.
   Ch *allocate_string(const Ch *source = 0, std::size_t size = 0) {
-    assert(source || size); // Either source or size (or both) must be specified
-    if (size == 0)
-      size = internal::measure(source) + 1;
+    assert(source ||
+           size);  // Either source or size (or both) must be specified
+    if (size == 0) size = internal::measure(source) + 1;
     Ch *result = static_cast<Ch *>(allocate_aligned(size * sizeof(Ch)));
     if (source)
-      for (std::size_t i = 0; i < size; ++i)
-        result[i] = source[i];
+      for (std::size_t i = 0; i < size; ++i) result[i] = source[i];
     return result;
   }
 
@@ -589,13 +594,12 @@ private:
       // Calculate required pool size (may be bigger than
       // RAPIDXML_DYNAMIC_POOL_SIZE)
       std::size_t pool_size = RAPIDXML_DYNAMIC_POOL_SIZE;
-      if (pool_size < size)
-        pool_size = size;
+      if (pool_size < size) pool_size = size;
 
       // Allocate
-      std::size_t alloc_size =
-          sizeof(header) + (2 * RAPIDXML_ALIGNMENT - 2) +
-          pool_size; // 2 alignments required in worst case: one for header, one
+      std::size_t alloc_size = sizeof(header) + (2 * RAPIDXML_ALIGNMENT - 2) +
+                               pool_size;  // 2 alignments required in worst
+                                           // case: one for header, one
                                            // for actual allocation
       char *raw_memory = allocate_raw(alloc_size);
 
@@ -620,7 +624,8 @@ private:
   char *m_ptr;    // First free byte in current pool
   char *m_end;    // One past last available byte in current pool
   char m_static_memory[RAPIDXML_STATIC_POOL_SIZE];  // Static raw memory
-  alloc_func *m_alloc_func; // Allocator function, or 0 if default is to be used
+  alloc_func
+      *m_alloc_func;       // Allocator function, or 0 if default is to be used
   free_func *m_free_func;  // Free function, or 0 if default is to be used
 };
 
@@ -630,8 +635,8 @@ private:
 //! Base class for xml_node and xml_attribute implementing common functions:
 //! name(), name_size(), value(), value_size() and parent().
 //! \param Ch Character type to use
-template <class Ch = char> class xml_base {
-
+template <class Ch = char>
+class xml_base {
  public:
   ///////////////////////////////////////////////////////////////////////////
   // Construction & destruction
@@ -752,8 +757,8 @@ protected:
 //! parse, both name and value of attribute will point to interior of source
 //! text used for parsing. Thus, this text must persist in memory for the
 //! lifetime of attribute. \param Ch Character type to use.
-template <class Ch = char> class xml_attribute : public xml_base<Ch> {
-
+template <class Ch = char>
+class xml_attribute : public xml_base<Ch> {
   friend class xml_node<Ch>;
 
  public:
@@ -773,8 +778,7 @@ public:
   //! no parent document.
   xml_document<Ch> *document() const {
     if (xml_node<Ch> *node = this->parent()) {
-      while (node->parent())
-        node = node->parent();
+      while (node->parent()) node = node->parent();
       return node->type() == node_document
                  ? static_cast<xml_document<Ch> *>(node)
                  : 0;
@@ -794,8 +798,7 @@ public:
                                         std::size_t name_size = 0,
                                         bool case_sensitive = true) const {
     if (name) {
-      if (name_size == 0)
-        name_size = internal::measure(name);
+      if (name_size == 0) name_size = internal::measure(name);
       for (xml_attribute<Ch> *attribute = m_prev_attribute; attribute;
            attribute = attribute->m_prev_attribute)
         if (internal::compare(attribute->name(), attribute->name_size(), name,
@@ -818,8 +821,7 @@ public:
                                     std::size_t name_size = 0,
                                     bool case_sensitive = true) const {
     if (name) {
-      if (name_size == 0)
-        name_size = internal::measure(name);
+      if (name_size == 0) name_size = internal::measure(name);
       for (xml_attribute<Ch> *attribute = m_next_attribute; attribute;
            attribute = attribute->m_next_attribute)
         if (internal::compare(attribute->name(), attribute->name_size(), name,
@@ -850,8 +852,8 @@ private:
 //! any, will point interior of source text used for parsing. Thus, this text
 //! must persist in the memory for the lifetime of node. \param Ch Character
 //! type to use.
-template <class Ch = char> class xml_node : public xml_base<Ch> {
-
+template <class Ch = char>
+class xml_node : public xml_base<Ch> {
  public:
   ///////////////////////////////////////////////////////////////////////////
   // Construction & destruction
@@ -877,8 +879,7 @@ public:
   //! parent document.
   xml_document<Ch> *document() const {
     xml_node<Ch> *node = const_cast<xml_node<Ch> *>(this);
-    while (node->parent())
-      node = node->parent();
+    while (node->parent()) node = node->parent();
     return node->type() == node_document ? static_cast<xml_document<Ch> *>(node)
                                          : 0;
   }
@@ -894,8 +895,7 @@ public:
   xml_node<Ch> *first_node(const Ch *name = 0, std::size_t name_size = 0,
                            bool case_sensitive = true) const {
     if (name) {
-      if (name_size == 0)
-        name_size = internal::measure(name);
+      if (name_size == 0) name_size = internal::measure(name);
       for (xml_node<Ch> *child = m_first_node; child;
            child = child->next_sibling())
         if (internal::compare(child->name(), child->name_size(), name,
@@ -918,10 +918,10 @@ public:
   //! found.
   xml_node<Ch> *last_node(const Ch *name = 0, std::size_t name_size = 0,
                           bool case_sensitive = true) const {
-    assert(m_first_node); // Cannot query for last child if node has no children
+    assert(
+        m_first_node);  // Cannot query for last child if node has no children
     if (name) {
-      if (name_size == 0)
-        name_size = internal::measure(name);
+      if (name_size == 0) name_size = internal::measure(name);
       for (xml_node<Ch> *child = m_last_node; child;
            child = child->previous_sibling())
         if (internal::compare(child->name(), child->name_size(), name,
@@ -946,8 +946,7 @@ public:
                                  bool case_sensitive = true) const {
     assert(this->m_parent);  // Cannot query for siblings if node has no parent
     if (name) {
-      if (name_size == 0)
-        name_size = internal::measure(name);
+      if (name_size == 0) name_size = internal::measure(name);
       for (xml_node<Ch> *sibling = m_prev_sibling; sibling;
            sibling = sibling->m_prev_sibling)
         if (internal::compare(sibling->name(), sibling->name_size(), name,
@@ -972,8 +971,7 @@ public:
                              bool case_sensitive = true) const {
     assert(this->m_parent);  // Cannot query for siblings if node has no parent
     if (name) {
-      if (name_size == 0)
-        name_size = internal::measure(name);
+      if (name_size == 0) name_size = internal::measure(name);
       for (xml_node<Ch> *sibling = m_next_sibling; sibling;
            sibling = sibling->m_next_sibling)
         if (internal::compare(sibling->name(), sibling->name_size(), name,
@@ -996,8 +994,7 @@ public:
                                      std::size_t name_size = 0,
                                      bool case_sensitive = true) const {
     if (name) {
-      if (name_size == 0)
-        name_size = internal::measure(name);
+      if (name_size == 0) name_size = internal::measure(name);
       for (xml_attribute<Ch> *attribute = m_first_attribute; attribute;
            attribute = attribute->m_next_attribute)
         if (internal::compare(attribute->name(), attribute->name_size(), name,
@@ -1020,8 +1017,7 @@ public:
                                     std::size_t name_size = 0,
                                     bool case_sensitive = true) const {
     if (name) {
-      if (name_size == 0)
-        name_size = internal::measure(name);
+      if (name_size == 0) name_size = internal::measure(name);
       for (xml_attribute<Ch> *attribute = m_last_attribute; attribute;
            attribute = attribute->m_prev_attribute)
         if (internal::compare(attribute->name(), attribute->name_size(), name,
@@ -1281,8 +1277,8 @@ private:
                               // value is only valid if m_first_node is non-zero
   xml_attribute<Ch> *m_first_attribute;  // Pointer to first attribute of node,
                                          // or 0 if none; always valid
-  xml_attribute<Ch> *
-      m_last_attribute; // Pointer to last attribute of node, or 0 if none; this
+  xml_attribute<Ch> *m_last_attribute;  // Pointer to last attribute of node, or
+                                        // 0 if none; this
   // value is only valid if m_first_attribute is non-zero
   xml_node<Ch>
       *m_prev_sibling;  // Pointer to previous sibling of node, or 0 if none;
@@ -1304,7 +1300,6 @@ private:
 //! xml_node. \param Ch Character type to use.
 template <class Ch = char>
 class xml_document : public xml_node<Ch>, public memory_pool<Ch> {
-
  public:
   //! Constructs empty XML document
   xml_document() : xml_node<Ch>(node_document) {}
@@ -1320,7 +1315,8 @@ public:
   //! nodes and attributes (if any), but does not clear memory pool. \param text
   //! XML data to parse; pointer is non-const to denote fact that this data may
   //! be modified by the parser.
-  template <int Flags> void parse(Ch *text) {
+  template <int Flags>
+  void parse(Ch *text) {
     assert(text);
 
     // Remove current contents
@@ -1334,8 +1330,7 @@ public:
     while (1) {
       // Skip whitespace before node
       skip<whitespace_pred, Flags>(text);
-      if (*text == 0)
-        break;
+      if (*text == 0) break;
 
       // Parse and append new child
       if (*text == Ch('<')) {
@@ -1408,7 +1403,8 @@ private:
   };
 
   // Detect attribute value character
-  template <Ch Quote> struct attribute_value_pred {
+  template <Ch Quote>
+  struct attribute_value_pred {
     static unsigned char test(Ch ch) {
       if (Quote == Ch('\''))
         return internal::lookup_tables<
@@ -1421,7 +1417,8 @@ private:
   };
 
   // Detect attribute value character
-  template <Ch Quote> struct attribute_value_pure_pred {
+  template <Ch Quote>
+  struct attribute_value_pure_pred {
     static unsigned char test(Ch ch) {
       if (Quote == Ch('\''))
         return internal::lookup_tables<
@@ -1480,10 +1477,10 @@ private:
   }
 
   // Skip characters until predicate evaluates to true
-  template <class StopPred, int Flags> static void skip(Ch *&text) {
+  template <class StopPred, int Flags>
+  static void skip(Ch *&text) {
     Ch *tmp = text;
-    while (StopPred::test(*tmp))
-      ++tmp;
+    while (StopPred::test(*tmp)) ++tmp;
     text = tmp;
   }
 
@@ -1515,7 +1512,6 @@ private:
         // Test if replacement is needed
         if (src[0] == Ch('&')) {
           switch (src[1]) {
-
             // &amp; &apos;
             case Ch('a'):
               if (src[2] == Ch('m') && src[3] == Ch('p') && src[4] == Ch(';')) {
@@ -1572,8 +1568,7 @@ private:
                 while (1) {
                   unsigned char digit = internal::lookup_tables<
                       0>::lookup_digits[static_cast<unsigned char>(*src)];
-                if (digit == 0xFF)
-                  break;
+                  if (digit == 0xFF) break;
                   code = code * 16 + digit;
                   ++src;
                 }
@@ -1585,8 +1580,7 @@ private:
                 while (1) {
                   unsigned char digit = internal::lookup_tables<
                       0>::lookup_digits[static_cast<unsigned char>(*src)];
-                if (digit == 0xFF)
-                  break;
+                  if (digit == 0xFF) break;
                   code = code * 10 + digit;
                   ++src;
                 }
@@ -1615,8 +1609,7 @@ private:
           ++dest;  // Put single space in dest
           ++src;   // Skip first whitespace char
           // Skip remaining whitespace chars
-          while (whitespace_pred::test(*src))
-            ++src;
+          while (whitespace_pred::test(*src)) ++src;
           continue;
         }
       }
@@ -1634,7 +1627,8 @@ private:
   // Internal parsing functions
 
   // Parse BOM, if any
-  template <int Flags> void parse_bom(Ch *&text) {
+  template <int Flags>
+  void parse_bom(Ch *&text) {
     // UTF-8?
     if (static_cast<unsigned char>(text[0]) == 0xEF &&
         static_cast<unsigned char>(text[1]) == 0xBB &&
@@ -1644,13 +1638,13 @@ private:
   }
 
   // Parse XML declaration (<?xml...)
-  template <int Flags> xml_node<Ch> *parse_xml_declaration(Ch *&text) {
+  template <int Flags>
+  xml_node<Ch> *parse_xml_declaration(Ch *&text) {
     // If parsing of declaration is disabled
     if (!(Flags & parse_declaration_node)) {
       // Skip until end of declaration
       while (text[0] != Ch('?') || text[1] != Ch('>')) {
-        if (!text[0])
-          RAPIDXML_PARSE_ERROR("unexpected end of data", text);
+        if (!text[0]) RAPIDXML_PARSE_ERROR("unexpected end of data", text);
         ++text;
       }
       text += 2;  // Skip '?>'
@@ -1675,13 +1669,13 @@ private:
   }
 
   // Parse XML comment (<!--...)
-  template <int Flags> xml_node<Ch> *parse_comment(Ch *&text) {
+  template <int Flags>
+  xml_node<Ch> *parse_comment(Ch *&text) {
     // If parsing of comments is disabled
     if (!(Flags & parse_comment_nodes)) {
       // Skip until end of comment
       while (text[0] != Ch('-') || text[1] != Ch('-') || text[2] != Ch('>')) {
-        if (!text[0])
-          RAPIDXML_PARSE_ERROR("unexpected end of data", text);
+        if (!text[0]) RAPIDXML_PARSE_ERROR("unexpected end of data", text);
         ++text;
       }
       text += 3;  // Skip '-->'
@@ -1693,8 +1687,7 @@ private:
 
     // Skip until end of comment
     while (text[0] != Ch('-') || text[1] != Ch('-') || text[2] != Ch('>')) {
-      if (!text[0])
-        RAPIDXML_PARSE_ERROR("unexpected end of data", text);
+      if (!text[0]) RAPIDXML_PARSE_ERROR("unexpected end of data", text);
       ++text;
     }
 
@@ -1703,15 +1696,15 @@ private:
     comment->value(value, text - value);
 
     // Place zero terminator after comment value
-    if (!(Flags & parse_no_string_terminators))
-      *text = Ch('\0');
+    if (!(Flags & parse_no_string_terminators)) *text = Ch('\0');
 
     text += 3;  // Skip '-->'
     return comment;
   }
 
   // Parse DOCTYPE
-  template <int Flags> xml_node<Ch> *parse_doctype(Ch *&text) {
+  template <int Flags>
+  xml_node<Ch> *parse_doctype(Ch *&text) {
     // Remember value start
     Ch *value = text;
 
@@ -1719,8 +1712,8 @@ private:
     while (*text != Ch('>')) {
       // Determine character type
       switch (*text) {
-
-      // If '[' encountered, scan for matching ending ']' using naive algorithm
+        // If '[' encountered, scan for matching ending ']' using naive
+        // algorithm
         // with depth This works for all W3C test files except for 2 most wicked
         case Ch('['): {
           ++text;  // Skip '['
@@ -1758,8 +1751,7 @@ private:
       doctype->value(value, text - value);
 
       // Place zero terminator after value
-      if (!(Flags & parse_no_string_terminators))
-        *text = Ch('\0');
+      if (!(Flags & parse_no_string_terminators)) *text = Ch('\0');
 
       text += 1;  // skip '>'
       return doctype;
@@ -1770,7 +1762,8 @@ private:
   }
 
   // Parse PI
-  template <int Flags> xml_node<Ch> *parse_pi(Ch *&text) {
+  template <int Flags>
+  xml_node<Ch> *parse_pi(Ch *&text) {
     // If creation of PI nodes is enabled
     if (Flags & parse_pi_nodes) {
       // Create pi node
@@ -1779,8 +1772,7 @@ private:
       // Extract PI target name
       Ch *name = text;
       skip<node_name_pred, Flags>(text);
-      if (text == name)
-        RAPIDXML_PARSE_ERROR("expected PI target", text);
+      if (text == name) RAPIDXML_PARSE_ERROR("expected PI target", text);
       pi->name(name, text - name);
 
       // Skip whitespace between pi target and pi
@@ -1827,8 +1819,7 @@ private:
   template <int Flags>
   Ch parse_and_append_data(xml_node<Ch> *node, Ch *&text, Ch *contents_start) {
     // Backup to contents start if whitespace trimming is disabled
-    if (!(Flags & parse_trim_whitespace))
-      text = contents_start;
+    if (!(Flags & parse_trim_whitespace)) text = contents_start;
 
     // Skip until end of data
     Ch *value = text, *end;
@@ -1845,12 +1836,10 @@ private:
       if (Flags & parse_normalize_whitespace) {
         // Whitespace is already condensed to single space characters by
         // skipping function, so just trim 1 char off the end
-        if (*(end - 1) == Ch(' '))
-          --end;
+        if (*(end - 1) == Ch(' ')) --end;
       } else {
         // Backup until non-whitespace character is found
-        while (whitespace_pred::test(*(end - 1)))
-          --end;
+        while (whitespace_pred::test(*(end - 1))) --end;
       }
     }
 
@@ -1864,8 +1853,7 @@ private:
 
     // Add data to parent node if no data exists yet
     if (!(Flags & parse_no_element_values))
-      if (*node->value() == Ch('\0'))
-        node->value(value, end - value);
+      if (*node->value() == Ch('\0')) node->value(value, end - value);
 
     // Place zero terminator after value
     if (!(Flags & parse_no_string_terminators)) {
@@ -1880,13 +1868,13 @@ private:
   }
 
   // Parse CDATA
-  template <int Flags> xml_node<Ch> *parse_cdata(Ch *&text) {
+  template <int Flags>
+  xml_node<Ch> *parse_cdata(Ch *&text) {
     // If CDATA is disabled
     if (Flags & parse_no_data_nodes) {
       // Skip until end of cdata
       while (text[0] != Ch(']') || text[1] != Ch(']') || text[2] != Ch('>')) {
-        if (!text[0])
-          RAPIDXML_PARSE_ERROR("unexpected end of data", text);
+        if (!text[0]) RAPIDXML_PARSE_ERROR("unexpected end of data", text);
         ++text;
       }
       text += 3;  // Skip ]]>
@@ -1896,8 +1884,7 @@ private:
     // Skip until end of cdata
     Ch *value = text;
     while (text[0] != Ch(']') || text[1] != Ch(']') || text[2] != Ch('>')) {
-      if (!text[0])
-        RAPIDXML_PARSE_ERROR("unexpected end of data", text);
+      if (!text[0]) RAPIDXML_PARSE_ERROR("unexpected end of data", text);
       ++text;
     }
 
@@ -1906,23 +1893,22 @@ private:
     cdata->value(value, text - value);
 
     // Place zero terminator after value
-    if (!(Flags & parse_no_string_terminators))
-      *text = Ch('\0');
+    if (!(Flags & parse_no_string_terminators)) *text = Ch('\0');
 
     text += 3;  // Skip ]]>
     return cdata;
   }
 
   // Parse element node
-  template <int Flags> xml_node<Ch> *parse_element(Ch *&text) {
+  template <int Flags>
+  xml_node<Ch> *parse_element(Ch *&text) {
     // Create element node
     xml_node<Ch> *element = this->allocate_node(node_element);
 
     // Extract element name
     Ch *name = text;
     skip<node_name_pred, Flags>(text);
-    if (text == name)
-      RAPIDXML_PARSE_ERROR("expected element name", text);
+    if (text == name) RAPIDXML_PARSE_ERROR("expected element name", text);
     element->name(name, text - name);
 
     // Skip whitespace between element name and attributes or >
@@ -1937,8 +1923,7 @@ private:
       parse_node_contents<Flags>(text, element);
     } else if (*text == Ch('/')) {
       ++text;
-      if (*text != Ch('>'))
-        RAPIDXML_PARSE_ERROR("expected >", text);
+      if (*text != Ch('>')) RAPIDXML_PARSE_ERROR("expected >", text);
       ++text;
     } else
       RAPIDXML_PARSE_ERROR("expected >", text);
@@ -1952,10 +1937,10 @@ private:
   }
 
   // Determine node type, and parse it
-  template <int Flags> xml_node<Ch> *parse_node(Ch *&text) {
+  template <int Flags>
+  xml_node<Ch> *parse_node(Ch *&text) {
     // Parse proper node type
     switch (text[0]) {
-
       // <...
       default:
         // Parse and append element node
@@ -1981,7 +1966,6 @@ private:
 
         // Parse proper subset of <! node
         switch (text[1]) {
-
           // <!-
           case Ch('-'):
             if (text[2] == Ch('-')) {
@@ -1993,8 +1977,9 @@ private:
 
           // <![
           case Ch('['):
-        if (text[2] == Ch('C') && text[3] == Ch('D') && text[4] == Ch('A') &&
-            text[5] == Ch('T') && text[6] == Ch('A') && text[7] == Ch('[')) {
+            if (text[2] == Ch('C') && text[3] == Ch('D') &&
+                text[4] == Ch('A') && text[5] == Ch('T') &&
+                text[6] == Ch('A') && text[7] == Ch('[')) {
               // '<![CDATA[' - cdata
               text += 8;  // Skip '![CDATA['
               return parse_cdata<Flags>(text);
@@ -2003,8 +1988,9 @@ private:
 
           // <!D
           case Ch('D'):
-        if (text[2] == Ch('O') && text[3] == Ch('C') && text[4] == Ch('T') &&
-            text[5] == Ch('Y') && text[6] == Ch('P') && text[7] == Ch('E') &&
+            if (text[2] == Ch('O') && text[3] == Ch('C') &&
+                text[4] == Ch('T') && text[5] == Ch('Y') &&
+                text[6] == Ch('P') && text[7] == Ch('E') &&
                 whitespace_pred::test(text[8])) {
               // '<!DOCTYPE ' - doctype
               text += 9;  // skip '!DOCTYPE '
@@ -2016,8 +2002,7 @@ private:
         // Attempt to skip other, unrecognized node types starting with <!
         ++text;  // Skip !
         while (*text != Ch('>')) {
-        if (*text == 0)
-          RAPIDXML_PARSE_ERROR("unexpected end of data", text);
+          if (*text == 0) RAPIDXML_PARSE_ERROR("unexpected end of data", text);
           ++text;
         }
         ++text;    // Skip '>'
@@ -2026,7 +2011,8 @@ private:
   }
 
   // Parse contents of the node - children, data etc.
-  template <int Flags> void parse_node_contents(Ch *&text, xml_node<Ch> *node) {
+  template <int Flags>
+  void parse_node_contents(Ch *&text, xml_node<Ch> *node) {
     // For all children and text
     while (1) {
       // Skip whitespace between > and node contents
@@ -2043,7 +2029,6 @@ private:
 
       // Determine what comes next: node closing, child node, data node, or 0?
       switch (next_char) {
-
         // Node closing or child node
         case Ch('<'):
           if (text[1] == Ch('/')) {
@@ -2062,8 +2047,7 @@ private:
             }
             // Skip remaining whitespace after node name
             skip<whitespace_pred, Flags>(text);
-          if (*text != Ch('>'))
-            RAPIDXML_PARSE_ERROR("expected >", text);
+            if (*text != Ch('>')) RAPIDXML_PARSE_ERROR("expected >", text);
             ++text;  // Skip '>'
             return;  // Node closed, finished parsing contents
           } else {
@@ -2095,8 +2079,7 @@ private:
       Ch *name = text;
       ++text;  // Skip first character of attribute name
       skip<attribute_name_pred, Flags>(text);
-      if (text == name)
-        RAPIDXML_PARSE_ERROR("expected attribute name", name);
+      if (text == name) RAPIDXML_PARSE_ERROR("expected attribute name", name);
 
       // Create new attribute
       xml_attribute<Ch> *attribute = this->allocate_attribute();
@@ -2107,8 +2090,7 @@ private:
       skip<whitespace_pred, Flags>(text);
 
       // Skip =
-      if (*text != Ch('='))
-        RAPIDXML_PARSE_ERROR("expected =", text);
+      if (*text != Ch('=')) RAPIDXML_PARSE_ERROR("expected =", text);
       ++text;
 
       // Add terminating zero after name
@@ -2127,7 +2109,8 @@ private:
       // Extract attribute value and expand char refs in it
       Ch *value = text, *end;
       const int AttFlags =
-          Flags & ~parse_normalize_whitespace; // No whitespace normalization in
+          Flags &
+          ~parse_normalize_whitespace;  // No whitespace normalization in
                                         // attributes
       if (quote == Ch('\''))
         end =
@@ -2143,8 +2126,7 @@ private:
       attribute->value(value, end - value);
 
       // Make sure that end quote is present
-      if (*text != quote)
-        RAPIDXML_PARSE_ERROR("expected ' or \"", text);
+      if (*text != quote) RAPIDXML_PARSE_ERROR("expected ' or \"", text);
       ++text;  // Skip quote
 
       // Add terminating zero after value

livox_ros_driver/common/rapidxml/rapidxml_iterators.hpp

@@ -11,8 +11,8 @@
 namespace rapidxml {
 
 //! Iterator of child nodes of xml_node
-template <class Ch> class node_iterator {
-
+template <class Ch>
+class node_iterator {
  public:
   typedef typename xml_node<Ch> value_type;
   typedef typename xml_node<Ch> &reference;
@@ -67,8 +67,8 @@ private:
 };
 
 //! Iterator of child attributes of xml_node
-template <class Ch> class attribute_iterator {
-
+template <class Ch>
+class attribute_iterator {
  public:
   typedef typename xml_attribute<Ch> value_type;
   typedef typename xml_attribute<Ch> &reference;

livox_ros_driver/common/rapidxml/rapidxml_print.hpp

@@ -19,8 +19,8 @@ namespace rapidxml {
 ///////////////////////////////////////////////////////////////////////
 // Printing flags
 
-const int print_no_indenting =
-    0x1; //!< Printer flag instructing the printer to suppress indenting of XML.
+const int print_no_indenting = 0x1;  //!< Printer flag instructing the printer
+                                     //!to suppress indenting of XML.
                                      //!< See print() function.
 
 ///////////////////////////////////////////////////////////////////////
@@ -35,8 +35,7 @@ namespace internal {
 // Copy characters from given range to given output iterator
 template <class OutIt, class Ch>
 inline OutIt copy_chars(const Ch *begin, const Ch *end, OutIt out) {
-  while (begin != end)
-    *out++ = *begin++;
+  while (begin != end) *out++ = *begin++;
   return out;
 }
 
@@ -97,8 +96,7 @@ inline OutIt copy_and_expand_chars(const Ch *begin, const Ch *end, Ch noexpand,
 // Fill given output iterator with repetitions of the same character
 template <class OutIt, class Ch>
 inline OutIt fill_chars(OutIt out, int n, Ch ch) {
-  for (int i = 0; i < n; ++i)
-    *out++ = ch;
+  for (int i = 0; i < n; ++i) *out++ = ch;
   return out;
 }
 
@@ -106,8 +104,7 @@ inline OutIt fill_chars(OutIt out, int n, Ch ch) {
 template <class Ch, Ch ch>
 inline bool find_char(const Ch *begin, const Ch *end) {
   while (begin != end)
-    if (*begin++ == ch)
-      return true;
+    if (*begin++ == ch) return true;
   return false;
 }
 
@@ -120,7 +117,6 @@ inline OutIt print_node(OutIt out, const xml_node<Ch> *node, int flags,
                         int indent) {
   // Print proper node type
   switch (node->type()) {
-
     // Document
     case node_document:
       out = print_children(out, node, flags, indent);
@@ -168,8 +164,7 @@ inline OutIt print_node(OutIt out, const xml_node<Ch> *node, int flags,
   }
 
   // If indenting not disabled, add line break after node
-  if (!(flags & print_no_indenting))
-    *out = Ch('\n'), ++out;
+  if (!(flags & print_no_indenting)) *out = Ch('\n'), ++out;
 
   // Return modified iterator
   return out;
@@ -197,9 +192,9 @@ inline OutIt print_attributes(OutIt out, const xml_node<Ch> *node, int flags) {
                        attribute->name() + attribute->name_size(), out);
       *out = Ch('='), ++out;
       // Print attribute value using appropriate quote type
-      if (find_char<Ch, Ch('"')>(attribute->value(),
-                                 attribute->value() +
-                                     attribute->value_size())) {
+      if (find_char<Ch, Ch('"')>(
+              attribute->value(),
+              attribute->value() + attribute->value_size())) {
         *out = Ch('\''), ++out;
         out = copy_and_expand_chars(
             attribute->value(), attribute->value() + attribute->value_size(),
@@ -222,8 +217,7 @@ template <class OutIt, class Ch>
 inline OutIt print_data_node(OutIt out, const xml_node<Ch> *node, int flags,
                              int indent) {
   assert(node->type() == node_data);
-  if (!(flags & print_no_indenting))
-    out = fill_chars(out, indent, Ch('\t'));
+  if (!(flags & print_no_indenting)) out = fill_chars(out, indent, Ch('\t'));
   out = copy_and_expand_chars(node->value(), node->value() + node->value_size(),
                               Ch(0), out);
   return out;
@@ -234,8 +228,7 @@ template <class OutIt, class Ch>
 inline OutIt print_cdata_node(OutIt out, const xml_node<Ch> *node, int flags,
                               int indent) {
   assert(node->type() == node_cdata);
-  if (!(flags & print_no_indenting))
-    out = fill_chars(out, indent, Ch('\t'));
+  if (!(flags & print_no_indenting)) out = fill_chars(out, indent, Ch('\t'));
   *out = Ch('<');
   ++out;
   *out = Ch('!');
@@ -271,8 +264,7 @@ inline OutIt print_element_node(OutIt out, const xml_node<Ch> *node, int flags,
   assert(node->type() == node_element);
 
   // Print element name and attributes, if any
-  if (!(flags & print_no_indenting))
-    out = fill_chars(out, indent, Ch('\t'));
+  if (!(flags & print_no_indenting)) out = fill_chars(out, indent, Ch('\t'));
   *out = Ch('<'), ++out;
   out = copy_chars(node->name(), node->name() + node->name_size(), out);
   out = print_attributes(out, node, flags);
@@ -298,8 +290,7 @@ inline OutIt print_element_node(OutIt out, const xml_node<Ch> *node, int flags,
           child->value(), child->value() + child->value_size(), Ch(0), out);
     } else {
       // Print all children with full indenting
-      if (!(flags & print_no_indenting))
-        *out = Ch('\n'), ++out;
+      if (!(flags & print_no_indenting)) *out = Ch('\n'), ++out;
       out = print_children(out, node, flags, indent + 1);
       if (!(flags & print_no_indenting))
         out = fill_chars(out, indent, Ch('\t'));
@@ -319,8 +310,7 @@ template <class OutIt, class Ch>
 inline OutIt print_declaration_node(OutIt out, const xml_node<Ch> *node,
                                     int flags, int indent) {
   // Print declaration start
-  if (!(flags & print_no_indenting))
-    out = fill_chars(out, indent, Ch('\t'));
+  if (!(flags & print_no_indenting)) out = fill_chars(out, indent, Ch('\t'));
   *out = Ch('<'), ++out;
   *out = Ch('?'), ++out;
   *out = Ch('x'), ++out;
@@ -342,8 +332,7 @@ template <class OutIt, class Ch>
 inline OutIt print_comment_node(OutIt out, const xml_node<Ch> *node, int flags,
                                 int indent) {
   assert(node->type() == node_comment);
-  if (!(flags & print_no_indenting))
-    out = fill_chars(out, indent, Ch('\t'));
+  if (!(flags & print_no_indenting)) out = fill_chars(out, indent, Ch('\t'));
   *out = Ch('<'), ++out;
   *out = Ch('!'), ++out;
   *out = Ch('-'), ++out;
@@ -360,8 +349,7 @@ template <class OutIt, class Ch>
 inline OutIt print_doctype_node(OutIt out, const xml_node<Ch> *node, int flags,
                                 int indent) {
   assert(node->type() == node_doctype);
-  if (!(flags & print_no_indenting))
-    out = fill_chars(out, indent, Ch('\t'));
+  if (!(flags & print_no_indenting)) out = fill_chars(out, indent, Ch('\t'));
   *out = Ch('<'), ++out;
   *out = Ch('!'), ++out;
   *out = Ch('D'), ++out;
@@ -382,8 +370,7 @@ template <class OutIt, class Ch>
 inline OutIt print_pi_node(OutIt out, const xml_node<Ch> *node, int flags,
                            int indent) {
   assert(node->type() == node_pi);
-  if (!(flags & print_no_indenting))
-    out = fill_chars(out, indent, Ch('\t'));
+  if (!(flags & print_no_indenting)) out = fill_chars(out, indent, Ch('\t'));
   *out = Ch('<'), ++out;
   *out = Ch('?'), ++out;
   out = copy_chars(node->name(), node->name() + node->name_size(), out);

livox_ros_driver/common/rapidxml/rapidxml_utils.hpp

@@ -8,17 +8,17 @@
 //! that can be useful in certain simple scenarios. They should probably not be
 //! used if maximizing performance is the main objective.
 
-#include "rapidxml.hpp"
 #include <fstream>
 #include <stdexcept>
 #include <string>
 #include <vector>
+#include "rapidxml.hpp"
 
 namespace rapidxml {
 
 //! Represents data loaded from a file
-template <class Ch = char> class file {
-
+template <class Ch = char>
+class file {
  public:
   //! Loads file into the memory. Data will be automatically destroyed by the
   //! destructor. \param filename Filename to load.
@@ -27,8 +27,7 @@ public:
 
     // Open stream
     basic_ifstream<Ch> stream(filename, ios::binary);
-    if (!stream)
-      throw runtime_error(string("cannot open file ") + filename);
+    if (!stream) throw runtime_error(string("cannot open file ") + filename);
     stream.unsetf(ios::skipws);
 
     // Determine stream size
@@ -73,7 +72,8 @@ private:
 
 //! Counts children of node. Time complexity is O(n).
 //! \return Number of children of node
-template <class Ch> inline std::size_t count_children(xml_node<Ch> *node) {
+template <class Ch>
+inline std::size_t count_children(xml_node<Ch> *node) {
   xml_node<Ch> *child = node->first_node();
   std::size_t count = 0;
   while (child) {
@@ -85,7 +85,8 @@ template <class Ch> inline std::size_t count_children(xml_node<Ch> *node) {
 
 //! Counts attributes of node. Time complexity is O(n).
 //! \return Number of attributes of node
-template <class Ch> inline std::size_t count_attributes(xml_node<Ch> *node) {
+template <class Ch>
+inline std::size_t count_attributes(xml_node<Ch> *node) {
   xml_attribute<Ch> *attr = node->first_attribute();
   std::size_t count = 0;
   while (attr) {

livox_ros_driver/config/livox_hub_config.json

@@ -7,27 +7,23 @@
     "lidar_config": [
         {
             "broadcast_code": "0TFDG3B006H2Z11",
-            "enable_fan": true,
             "return_mode": 0,
-            "imu_rate": 1
+            "imu_rate": 0
         },
         {
             "broadcast_code": "0TFDG3U99101291",
-            "enable_fan": true,
             "return_mode": 0,
-            "imu_rate": 1
+            "imu_rate": 0
         },
         {
             "broadcast_code": "1HDDG8M00100191",
-            "enable_fan": true,
             "return_mode": 0,
-            "imu_rate": 1
+            "imu_rate": 0
         },
         {
             "broadcast_code": "1PQDG8E00100321",
-            "enable_fan": true,
             "return_mode": 0,
-            "imu_rate": 1
+            "imu_rate": 0
         }
     ]
 }

livox_ros_driver/config/livox_lidar_config.json

@@ -1,9 +1,8 @@
 {
     "lidar_config": [
         {
-            "broadcast_code": "0T9DFBC00401611",
+            "broadcast_code": "1PQDH5B00100041",
             "enable_connect": false,
-            "enable_fan": true,
             "return_mode": 0,
             "coordinate": 0,
             "imu_rate": 0,
@@ -13,7 +12,6 @@
         {
             "broadcast_code": "0TFDG3U99101431",
             "enable_connect": false,
-            "enable_fan": true,
             "return_mode": 0,
             "coordinate": 0,
             "imu_rate": 0,

livox_ros_driver/livox_ros_driver/include/livox_ros_driver.h

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

livox_ros_driver/livox_ros_driver/lddc.cpp

@@ -34,20 +34,21 @@
 #include <sensor_msgs/Imu.h>
 #include <sensor_msgs/PointCloud2.h>
 
-#include "lds_lidar.h"
-#include "lds_lvx.h"
 #include <livox_ros_driver/CustomMsg.h>
 #include <livox_ros_driver/CustomPoint.h>
+#include "lds_lidar.h"
+#include "lds_lvx.h"
 
 namespace livox_ros {
 
-typedef pcl::PointCloud<pcl::PointXYZI> PointCloud;
-
 /** Lidar Data Distribute Control--------------------------------------------*/
 Lddc::Lddc(int format, int multi_topic, int data_src, int output_type,
            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), \
+    : transfer_format_(format),
+      use_multi_topic_(multi_topic),
+      data_src_(data_src),
+      output_type_(output_type),
+      publish_frq_(frq),
       frame_id_(frame_id) {
   publish_period_ns_ = kNsPerSecond / publish_frq_;
   lds_ = nullptr;
@@ -60,13 +61,12 @@ Lddc::Lddc(int format, int multi_topic, int data_src, int output_type,
 };
 
 Lddc::~Lddc() {
-
   if (global_pub_) {
     delete global_pub_;
   }
 
   if (global_imu_pub_) {
-    delete global_pub_;
+    delete global_imu_pub_;
   }
 
   if (lds_) {
@@ -87,10 +87,11 @@ 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);
+                                  uint64_t *start_time,
+                                  StoragePacket *storage_packet) {
+  QueuePrePop(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 */
@@ -109,10 +110,9 @@ int32_t Lddc::GetPublishStartTime(LidarDevice *lidar, LidarDataQueue *queue,
         break;
       }
       QueuePopUpdate(queue); /* skip packet */
-      QueueProPop(queue, storage_packet);
+      QueuePrePop(queue, storage_packet);
       uint32_t last_remaning_time = remaining_time;
-      timestamp = GetStoragePacketTimestamp(storage_packet, 
-                                            lidar->data_src);
+      timestamp = GetStoragePacketTimestamp(storage_packet, lidar->data_src);
       remaining_time = timestamp % publish_period_ns_;
       /** Flip to another period */
       if (last_remaning_time > remaining_time) {
@@ -127,20 +127,7 @@ int32_t Lddc::GetPublishStartTime(LidarDevice *lidar, LidarDataQueue *queue,
   }
 }
 
-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;
+void Lddc::InitPointcloud2MsgHeader(sensor_msgs::PointCloud2& cloud) {
   cloud.header.frame_id.assign(frame_id_);
   cloud.height = 1;
   cloud.width = 0;
@@ -169,44 +156,61 @@ uint32_t Lddc::PublishPointcloud2(LidarDataQueue *queue, uint32_t packet_num,
   cloud.fields[5].name = "line";
   cloud.fields[5].count = 1;
   cloud.fields[5].datatype = sensor_msgs::PointField::UINT8;
+  cloud.point_step = sizeof(LivoxPointXyzrtl);
+}
 
+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;
+  InitPointcloud2MsgHeader(cloud);
   cloud.data.resize(packet_num * kMaxPointPerEthPacket *
                     sizeof(LivoxPointXyzrtl));
   cloud.point_step = sizeof(LivoxPointXyzrtl);
+
   uint8_t *point_base = cloud.data.data();
   uint8_t data_source = lidar->data_src;
+  uint32_t line_num = GetLaserLineNumber(lidar->info.type);
+  uint32_t echo_num = GetEchoNumPerPoint(lidar->raw_data_type);
+  uint32_t is_zero_packet = 0;
   while ((published_packet < packet_num) && !QueueIsEmpty(queue)) {
-    QueueProPop(queue, &storage_packet);
+    QueuePrePop(queue, &storage_packet);
     LivoxEthPacket *raw_packet =
         reinterpret_cast<LivoxEthPacket *>(storage_packet.raw_data);
     timestamp = GetStoragePacketTimestamp(&storage_packet, data_source);
     int64_t packet_gap = timestamp - last_timestamp;
     if ((packet_gap > lidar->packet_interval_max) &&
         lidar->data_is_pubulished) {
-      //ROS_INFO("Lidar[%d] packet time interval is %ldns", handle, packet_gap);
+      // ROS_INFO("Lidar[%d] packet time interval is %ldns", handle,
+      //     packet_gap);
       if (kSourceLvxFile != data_source) {
-        point_base = FillZeroPointXyzrtl(point_base, storage_packet.point_num);
-        cloud.width += storage_packet.point_num;
-        last_timestamp = last_timestamp + lidar->packet_interval;
-        if (!published_packet) { 
-          cloud.header.stamp = ros::Time(last_timestamp / 1000000000.0);
-        }
-        ++published_packet;
-        continue;
+        timestamp = last_timestamp + lidar->packet_interval;
+        ZeroPointDataOfStoragePacket(&storage_packet);
+        is_zero_packet = 1;
       }
     }
     /** Use the first packet timestamp as pointcloud2 msg timestamp */
     if (!published_packet) {
       cloud.header.stamp = ros::Time(timestamp / 1000000000.0);
     }
-    cloud.width += storage_packet.point_num;
+    uint32_t single_point_num = storage_packet.point_num * echo_num;
 
     if (kSourceLvxFile != data_source) {
       PointConvertHandler pf_point_convert =
           GetConvertHandler(lidar->raw_data_type);
       if (pf_point_convert) {
-        point_base = pf_point_convert(
-            point_base, raw_packet, lidar->extrinsic_parameter);
+        point_base = pf_point_convert(point_base, raw_packet,
+            lidar->extrinsic_parameter, line_num);
       } else {
         /** Skip the packet */
         ROS_INFO("Lidar[%d] unkown packet type[%d]", handle,
@@ -214,20 +218,23 @@ uint32_t Lddc::PublishPointcloud2(LidarDataQueue *queue, uint32_t packet_num,
         break;
       }
     } else {
-      point_base = LivoxPointToPxyzrtl(
-          point_base, raw_packet, lidar->extrinsic_parameter);
+      point_base = LivoxPointToPxyzrtl(point_base, raw_packet,
+          lidar->extrinsic_parameter, line_num);
     }
 
+    if (!is_zero_packet) {
       QueuePopUpdate(queue);
-    last_timestamp = timestamp;
-    ++published_packet;
+    } else {
+      is_zero_packet = 0;
+    }
+    cloud.width += single_point_num;
+    ++published_packet;
+    last_timestamp = timestamp;
   }
-
   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 */
-
   ros::Publisher *p_publisher = Lddc::GetCurrentPublisher(handle);
   if (kOutputToRos == output_type_) {
     p_publisher->publish(cloud);
@@ -237,14 +244,26 @@ uint32_t Lddc::PublishPointcloud2(LidarDataQueue *queue, uint32_t packet_num,
                   cloud);
     }
   }
-
   if (!lidar->data_is_pubulished) {
     lidar->data_is_pubulished = true;
   }
-
   return published_packet;
 }
 
+void Lddc::FillPointsToPclMsg(PointCloud::Ptr& pcl_msg, \
+    LivoxPointXyzrtl* src_point, uint32_t num) {
+  LivoxPointXyzrtl* point_xyzrtl = (LivoxPointXyzrtl*)src_point;
+  for (uint32_t i = 0; i < num; i++) {
+    pcl::PointXYZI point;
+    point.x = point_xyzrtl->x;
+    point.y = point_xyzrtl->y;
+    point.z = point_xyzrtl->z;
+    point.intensity = point_xyzrtl->reflectivity;
+    ++point_xyzrtl;
+    pcl_msg->points.push_back(point);
+  }
+}
+
 /* for pcl::pxyzi */
 uint32_t Lddc::PublishPointcloudData(LidarDataQueue *queue, uint32_t packet_num,
                                      uint8_t handle) {
@@ -259,16 +278,18 @@ uint32_t Lddc::PublishPointcloudData(LidarDataQueue *queue, uint32_t packet_num,
     return 0;
   }
 
-  /* init point cloud data struct */
   PointCloud::Ptr cloud(new PointCloud);
   cloud->header.frame_id.assign(frame_id_);
-  /* cloud->header.stamp = ros::Time::now(); */
   cloud->height = 1;
   cloud->width = 0;
 
+  uint8_t point_buf[2048];
+  uint32_t is_zero_packet = 0;
   uint8_t data_source = lidar->data_src;
+  uint32_t line_num = GetLaserLineNumber(lidar->info.type);
+  uint32_t echo_num = GetEchoNumPerPoint(lidar->raw_data_type);
   while ((published_packet < packet_num) && !QueueIsEmpty(queue)) {
-    QueueProPop(queue, &storage_packet);
+    QueuePrePop(queue, &storage_packet);
     LivoxEthPacket *raw_packet =
         reinterpret_cast<LivoxEthPacket *>(storage_packet.raw_data);
     timestamp = GetStoragePacketTimestamp(&storage_packet, data_source);
@@ -276,29 +297,23 @@ uint32_t Lddc::PublishPointcloudData(LidarDataQueue *queue, uint32_t packet_num,
     if ((packet_gap > lidar->packet_interval_max) &&
         lidar->data_is_pubulished) {
       //ROS_INFO("Lidar[%d] packet time interval is %ldns", handle, packet_gap);
-      pcl::PointXYZI point = {0}; /* fill zero points */
-      for (uint32_t i = 0; i < storage_packet.point_num; i++) {
-        cloud->points.push_back(point);
+      if (kSourceLvxFile != data_source) {
+        timestamp = last_timestamp + lidar->packet_interval;
+        ZeroPointDataOfStoragePacket(&storage_packet);
+        is_zero_packet = 1;
       }
-      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;
     }
     if (!published_packet) {
       cloud->header.stamp = timestamp / 1000.0;  // to pcl ros time stamp
     }
-    cloud->width += storage_packet.point_num;
+    uint32_t single_point_num = storage_packet.point_num * echo_num;
 
-    uint8_t point_buf[2048];
     if (kSourceLvxFile != data_source) {
       PointConvertHandler pf_point_convert =
           GetConvertHandler(lidar->raw_data_type);
       if (pf_point_convert) {
-        pf_point_convert(point_buf, raw_packet,
-                         lidar->extrinsic_parameter);
+        pf_point_convert(point_buf, raw_packet, lidar->extrinsic_parameter, \
+            line_num);
       } else {
         /* Skip the packet */
         ROS_INFO("Lidar[%d] unkown packet type[%d]", handle,
@@ -306,24 +321,19 @@ uint32_t Lddc::PublishPointcloudData(LidarDataQueue *queue, uint32_t packet_num,
         break;
       }
     } else {
-      LivoxPointToPxyzrtl(point_buf, raw_packet,
-                          lidar->extrinsic_parameter);
+      LivoxPointToPxyzrtl(point_buf, raw_packet, lidar->extrinsic_parameter, \
+          line_num);
     }
-
     LivoxPointXyzrtl *dst_point = (LivoxPointXyzrtl *)point_buf;
-    for (uint32_t i = 0; i < storage_packet.point_num; i++) {
-      pcl::PointXYZI point;
-      point.x = dst_point->x;
-      point.y = dst_point->y;
-      point.z = dst_point->z;
-      point.intensity = dst_point->reflectivity;
-      ++dst_point;
-      cloud->points.push_back(point);
-    }
-
+    FillPointsToPclMsg(cloud, dst_point, single_point_num);
+    if (!is_zero_packet) {
       QueuePopUpdate(queue);
-    last_timestamp = timestamp;
+    } else {
+      is_zero_packet = 0;
+    }
+    cloud->width += single_point_num;
     ++published_packet;
+    last_timestamp = timestamp;
   }
 
   ros::Publisher *p_publisher = Lddc::GetCurrentPublisher(handle);
@@ -335,72 +345,96 @@ uint32_t Lddc::PublishPointcloudData(LidarDataQueue *queue, uint32_t packet_num,
                   cloud);
     }
   }
-
   if (!lidar->data_is_pubulished) {
     lidar->data_is_pubulished = true;
   }
-
   return published_packet;
 }
 
+void Lddc::FillPointsToCustomMsg(livox_ros_driver::CustomMsg& livox_msg, \
+    LivoxPointXyzrtl* src_point, uint32_t num, uint32_t offset_time, \
+    uint32_t point_interval, uint32_t echo_num) {
+  LivoxPointXyzrtl* point_xyzrtl = (LivoxPointXyzrtl*)src_point;
+  for (uint32_t i = 0; i < num; i++) {
+    livox_ros_driver::CustomPoint point;
+    if (echo_num > 1) { /** dual return mode */
+      point.offset_time = offset_time + (i / echo_num) * point_interval;
+    } else {
+      point.offset_time = offset_time + i * point_interval;
+    }
+    point.x = point_xyzrtl->x;
+    point.y = point_xyzrtl->y;
+    point.z = point_xyzrtl->z;
+    point.reflectivity = point_xyzrtl->reflectivity;
+    point.tag = point_xyzrtl->tag;
+    point.line = point_xyzrtl->line;
+    ++point_xyzrtl;
+    livox_msg.points.push_back(point);
+  }
+}
+
 uint32_t Lddc::PublishCustomPointcloud(LidarDataQueue *queue,
                                        uint32_t packet_num, uint8_t handle) {
   static uint32_t msg_seq = 0;
   uint64_t timestamp = 0;
   uint64_t last_timestamp = 0;
-  uint32_t published_packet = 0;
-  uint32_t packet_offset_time = 0; // ns
+
+  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;
+  }
 
   livox_ros_driver::CustomMsg livox_msg;
-
-  //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();
   livox_msg.timebase = 0;
   livox_msg.point_num = 0;
   livox_msg.lidar_id = handle;
 
-  LidarDevice *lidar  = &lds_->lidars_[handle];
+  uint8_t point_buf[2048];
   uint8_t data_source = lds_->lidars_[handle].data_src;
-  StoragePacket storage_packet;
+  uint32_t line_num = GetLaserLineNumber(lidar->info.type);
+  uint32_t echo_num = GetEchoNumPerPoint(lidar->raw_data_type);
+  uint32_t point_interval = GetPointInterval(lidar->info.type);
+  uint32_t published_packet = 0;
+  uint32_t packet_offset_time = 0;  /** uint:ns */
+  uint32_t is_zero_packet = 0;
   while (published_packet < packet_num) {
-    QueueProPop(queue, &storage_packet);
+    QueuePrePop(queue, &storage_packet);
     LivoxEthPacket *raw_packet =
         reinterpret_cast<LivoxEthPacket *>(storage_packet.raw_data);
-    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)) {
-      dual_point = 1;
-    }
-
     timestamp = GetStoragePacketTimestamp(&storage_packet, data_source);
-    if (((timestamp - last_timestamp) > kDeviceDisconnectThreshold) &&
-        published_packet && lidar->data_is_pubulished) {
-      ROS_INFO("Lidar[%d] packet loss", handle);
-      break;
+    int64_t packet_gap = timestamp - last_timestamp;
+    if ((packet_gap > lidar->packet_interval_max) &&
+        lidar->data_is_pubulished) {
+      // ROS_INFO("Lidar[%d] packet time interval is %ldns", handle,
+      // packet_gap);
+      if (kSourceLvxFile != data_source) {
+        timestamp = last_timestamp + lidar->packet_interval;
+        ZeroPointDataOfStoragePacket(&storage_packet);
+        is_zero_packet = 1;
       }
+    }
+    /** first packet */
     if (!published_packet) {
-      livox_msg.timebase = timestamp; // to us
-      packet_offset_time = 0;         // first packet
-      livox_msg.header.stamp =
-          ros::Time(timestamp / 1000000000.0); // to ros time stamp
-      // ROS_DEBUG("[%d]:%ld %d", handle, livox_msg.timebase, point_interval);
+      livox_msg.timebase = timestamp;
+      packet_offset_time = 0;
+      /** convert to ros time stamp */
+      livox_msg.header.stamp = ros::Time(timestamp / 1000000000.0);
     } else {
       packet_offset_time = (uint32_t)(timestamp - livox_msg.timebase);
     }
-    livox_msg.point_num += storage_packet.point_num;
+    uint32_t single_point_num = storage_packet.point_num * echo_num;
 
-    uint8_t point_buf[2048];
     if (kSourceLvxFile != data_source) {
       PointConvertHandler pf_point_convert =
           GetConvertHandler(lidar->raw_data_type);
       if (pf_point_convert) {
-        pf_point_convert(point_buf, raw_packet,
-                         lidar->extrinsic_parameter);
+        pf_point_convert(point_buf, raw_packet, lidar->extrinsic_parameter, \
+            line_num);
       } else {
         /* Skip the packet */
         ROS_INFO("Lidar[%d] unkown packet type[%d]", handle,
@@ -408,29 +442,20 @@ uint32_t Lddc::PublishCustomPointcloud(LidarDataQueue *queue,
         break;
       }
     } else {
-      LivoxPointToPxyzrtl(point_buf, raw_packet,
-                          lidar->extrinsic_parameter);
+      LivoxPointToPxyzrtl(point_buf, raw_packet, lidar->extrinsic_parameter, \
+          line_num);
     }
-
     LivoxPointXyzrtl *dst_point = (LivoxPointXyzrtl *)point_buf;
-    for (uint32_t i = 0; i < storage_packet.point_num; i++) {
-      livox_ros_driver::CustomPoint point;
-      if (!dual_point) { /** dual return mode */
-        point.offset_time = packet_offset_time + i * point_interval;
+    FillPointsToCustomMsg(livox_msg, dst_point, single_point_num, \
+        packet_offset_time, point_interval, echo_num);
+
+    if (!is_zero_packet) {
+      QueuePopUpdate(queue);
     } else {
-        point.offset_time = packet_offset_time + (i / 2) * point_interval;
-      }
-      point.x = dst_point->x;
-      point.y = dst_point->y;
-      point.z = dst_point->z;
-      point.reflectivity = dst_point->reflectivity;
-      point.tag = dst_point->tag;
-      point.line = dst_point->line;
-      ++dst_point;
-      livox_msg.points.push_back(point);
+      is_zero_packet = 0;
     }
 
-    QueuePopUpdate(queue);
+    livox_msg.point_num += single_point_num;
     last_timestamp = timestamp;
     ++published_packet;
   }
@@ -448,7 +473,6 @@ uint32_t Lddc::PublishCustomPointcloud(LidarDataQueue *queue,
   if (!lidar->data_is_pubulished) {
     lidar->data_is_pubulished = true;
   }
-
   return published_packet;
 }
 
@@ -462,7 +486,7 @@ uint32_t Lddc::PublishImuData(LidarDataQueue *queue, uint32_t packet_num,
 
   uint8_t data_source = lds_->lidars_[handle].data_src;
   StoragePacket storage_packet;
-  QueueProPop(queue, &storage_packet);
+  QueuePrePop(queue, &storage_packet);
   LivoxEthPacket *raw_packet =
       reinterpret_cast<LivoxEthPacket *>(storage_packet.raw_data);
   timestamp = GetStoragePacketTimestamp(&storage_packet, data_source);
@@ -494,7 +518,6 @@ uint32_t Lddc::PublishImuData(LidarDataQueue *queue, uint32_t packet_num,
                   imu_data);
     }
   }
-
   return published_packet;
 }
 
@@ -509,14 +532,13 @@ int Lddc::RegisterLds(Lds *lds) {
 
 void Lddc::PollingLidarPointCloudData(uint8_t handle, LidarDevice *lidar) {
   LidarDataQueue *p_queue = &lidar->data;
-  if (p_queue == nullptr) {
+  if (p_queue->storage_packet == nullptr) {
     return;
   }
 
   while (!QueueIsEmpty(p_queue)) {
     uint32_t used_size = QueueUsedSize(p_queue);
-    uint32_t onetime_publish_packets =
-        GetPacketNumPerSec(lidar->raw_data_type) / publish_frq_;
+    uint32_t onetime_publish_packets = lidar->onetime_publish_packets;
     if (used_size < onetime_publish_packets) {
       break;
     }
@@ -533,7 +555,7 @@ void Lddc::PollingLidarPointCloudData(uint8_t handle, LidarDevice *lidar) {
 
 void Lddc::PollingLidarImuData(uint8_t handle, LidarDevice *lidar) {
   LidarDataQueue *p_queue = &lidar->imu_data;
-  if (p_queue == nullptr) {
+  if (p_queue->storage_packet == nullptr) {
     return;
   }
 
@@ -546,7 +568,7 @@ void Lddc::DistributeLidarData(void) {
   if (lds_ == nullptr) {
     return;
   }
-
+  lds_->semaphore_.Wait();
   for (uint32_t i = 0; i < lds_->lidar_count_; i++) {
     uint32_t lidar_id = i;
     LidarDevice *lidar = &lds_->lidars_[lidar_id];
@@ -555,7 +577,6 @@ void Lddc::DistributeLidarData(void) {
         (p_queue == nullptr)) {
       continue;
     }
-
     PollingLidarPointCloudData(lidar_id, lidar);
     PollingLidarImuData(lidar_id, lidar);
   }
@@ -603,7 +624,8 @@ ros::Publisher *Lddc::GetCurrentPublisher(uint8_t handle) {
           name_str, queue_size);
     } else if (kPclPxyziMsg == transfer_format_) {
       **pub = cur_node_->advertise<PointCloud>(name_str, queue_size);
-      ROS_INFO("%s publish use pcl PointXYZI format, set ROS publisher queue "
+      ROS_INFO(
+          "%s publish use pcl PointXYZI format, set ROS publisher queue "
           "size %d",
           name_str, queue_size);
     }
@@ -654,9 +676,9 @@ void Lddc::CreateBagFile(const std::string &file_name) {
 
 void Lddc::PrepareExit(void) {
   if (bag_) {
+    ROS_INFO("Waiting to save the bag file!");
     bag_->close();
-
-    ROS_INFO("Press [Ctrl+C] to exit!\n");
+    ROS_INFO("Save the bag file successfully!");
     bag_ = nullptr;
   }
   if (lds_) {

livox_ros_driver/livox_ros_driver/lddc.h

@@ -29,9 +29,14 @@
 
 #include <ros/ros.h>
 #include <rosbag/bag.h>
+#include <pcl_ros/point_cloud.h>
+#include <livox_ros_driver/CustomMsg.h>
+#include <livox_ros_driver/CustomPoint.h>
 
 namespace livox_ros {
 
+typedef pcl::PointCloud<pcl::PointXYZI> PointCloud;
+
 /** Lidar data distribute control */
 typedef enum {
   kPointCloud2Msg = 0,
@@ -41,7 +46,7 @@ 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();
 
@@ -61,7 +66,8 @@ public:
 
  private:
   int32_t GetPublishStartTime(LidarDevice *lidar, LidarDataQueue *queue,
-      uint64_t *start_time, StoragePacket *storage_packet);
+                              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,
@@ -75,7 +81,12 @@ private:
   ros::Publisher *GetCurrentImuPublisher(uint8_t handle);
   void PollingLidarPointCloudData(uint8_t handle, LidarDevice *lidar);
   void PollingLidarImuData(uint8_t handle, LidarDevice *lidar);
-
+  void InitPointcloud2MsgHeader(sensor_msgs::PointCloud2& cloud);
+  void FillPointsToPclMsg(PointCloud::Ptr& pcl_msg, \
+      LivoxPointXyzrtl* src_point, uint32_t num);
+  void FillPointsToCustomMsg(livox_ros_driver::CustomMsg& livox_msg, \
+      LivoxPointXyzrtl* src_point, uint32_t num, uint32_t offset_time, \
+      uint32_t point_interval, uint32_t echo_num);
   uint8_t transfer_format_;
   uint8_t use_multi_topic_;
   uint8_t data_src_;

livox_ros_driver/livox_ros_driver/ldq.cpp

@@ -31,7 +31,6 @@ namespace livox_ros {
 
 /* for pointcloud queue process */
 int InitQueue(LidarDataQueue *queue, uint32_t queue_size) {
-
   if (queue == nullptr) {
     return 1;
   }
@@ -54,7 +53,6 @@ int InitQueue(LidarDataQueue *queue, uint32_t queue_size) {
 }
 
 int DeInitQueue(LidarDataQueue *queue) {
-
   if (queue == nullptr) {
     return 1;
   }
@@ -76,7 +74,7 @@ void ResetQueue(LidarDataQueue *queue) {
   queue->wr_idx = 0;
 }
 
-void QueueProPop(LidarDataQueue *queue, StoragePacket *storage_packet) {
+void QueuePrePop(LidarDataQueue *queue, StoragePacket *storage_packet) {
   uint32_t rd_idx = queue->rd_idx & queue->mask;
 
   memcpy(storage_packet, &(queue->storage_packet[rd_idx]),
@@ -86,7 +84,7 @@ void QueueProPop(LidarDataQueue *queue, StoragePacket *storage_packet) {
 void QueuePopUpdate(LidarDataQueue *queue) { queue->rd_idx++; }
 
 uint32_t QueuePop(LidarDataQueue *queue, StoragePacket *storage_packet) {
-  QueueProPop(queue, storage_packet);
+  QueuePrePop(queue, storage_packet);
   QueuePopUpdate(queue);
 
   return 1;

livox_ros_driver/livox_ros_driver/ldq.h

@@ -71,7 +71,7 @@ inline static uint32_t RoundupPowerOf2(uint32_t size) {
 int InitQueue(LidarDataQueue *queue, uint32_t queue_size);
 int DeInitQueue(LidarDataQueue *queue);
 void ResetQueue(LidarDataQueue *queue);
-void QueueProPop(LidarDataQueue *queue, StoragePacket *storage_packet);
+void QueuePrePop(LidarDataQueue *queue, StoragePacket *storage_packet);
 void QueuePopUpdate(LidarDataQueue *queue);
 uint32_t QueuePop(LidarDataQueue *queue, StoragePacket *storage_packet);
 uint32_t QueueUsedSize(LidarDataQueue *queue);

livox_ros_driver/livox_ros_driver/lds.cpp

@@ -24,16 +24,15 @@
 
 #include "lds.h"
 
-#include <chrono>
 #include <math.h>
 #include <stdio.h>
 #include <string.h>
 #include <time.h>
+#include <chrono>
 
 namespace livox_ros {
 
-/** Common function ------
- * ----------------------------------------------------------------------- */
+/** Common function --------------------------------------------------------- */
 bool IsFilePathValid(const char *path_str) {
   int str_len = strlen(path_str);
 
@@ -44,15 +43,14 @@ bool IsFilePathValid(const char *path_str) {
   }
 }
 
-uint64_t GetStoragePacketTimestamp(StoragePacket *packet, uint8_t data_src_) {
-
+uint64_t GetStoragePacketTimestamp(StoragePacket *packet, uint8_t data_src) {
   LivoxEthPacket *raw_packet =
       reinterpret_cast<LivoxEthPacket *>(packet->raw_data);
   LdsStamp timestamp;
   memcpy(timestamp.stamp_bytes, raw_packet->timestamp, sizeof(timestamp));
 
   if (raw_packet->timestamp_type == kTimestampTypePps) {
-    if (data_src_ != kSourceLvxFile) {
+    if (data_src != kSourceLvxFile) {
       return (timestamp.stamp + packet->time_rcv);
     } else {
       return timestamp.stamp;
@@ -83,8 +81,10 @@ uint64_t GetStoragePacketTimestamp(StoragePacket *packet, uint8_t data_src_) {
   }
 }
 
-uint32_t CalculatePacketQueueSize(uint32_t interval_ms, uint32_t data_type) {
-  uint32_t queue_size = (interval_ms * GetPacketNumPerSec(data_type)) / 1000;
+uint32_t CalculatePacketQueueSize(uint32_t interval_ms, uint8_t product_type,
+                                  uint8_t data_type) {
+  uint32_t queue_size =
+      (interval_ms * GetPacketNumPerSec(product_type, data_type)) / 1000;
 
   if (queue_size < kMinEthPacketQueueSize) {
     queue_size = kMinEthPacketQueueSize;
@@ -176,14 +176,15 @@ void PointExtrisincCompensation(PointXyz *dst_point, const PointXyz &src_point,
 /** Livox point procees for different raw data format
  * --------------------------------------------*/
 uint8_t *LivoxPointToPxyzrtl(uint8_t *point_buf, LivoxEthPacket *eth_packet,
-                             ExtrinsicParameter &extrinsic) {
+    ExtrinsicParameter &extrinsic, uint32_t line_num) {
   LivoxPointXyzrtl *dst_point = (LivoxPointXyzrtl *)point_buf;
   uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type);
   LivoxPoint *raw_point = reinterpret_cast<LivoxPoint *>(eth_packet->data);
 
   while (points_per_packet) {
     RawPointConvert((LivoxPointXyzr *)dst_point, raw_point);
-    if (extrinsic.enable) {
+    if (extrinsic.enable && IsTripleFloatNoneZero(raw_point->x,
+        raw_point->y, raw_point->z)) {
       PointXyz src_point = *((PointXyz *)dst_point);
       PointExtrisincCompensation((PointXyz *)dst_point, src_point, extrinsic);
     }
@@ -197,8 +198,8 @@ uint8_t *LivoxPointToPxyzrtl(uint8_t *point_buf, LivoxEthPacket *eth_packet,
   return (uint8_t *)dst_point;
 }
 
-uint8_t *LivoxRawPointToPxyzrtl(uint8_t *point_buf, LivoxEthPacket *eth_packet,
-                                ExtrinsicParameter &extrinsic) {
+static uint8_t *LivoxRawPointToPxyzrtl(uint8_t *point_buf, LivoxEthPacket *eth_packet,
+    ExtrinsicParameter &extrinsic, uint32_t line_num) {
   LivoxPointXyzrtl *dst_point = (LivoxPointXyzrtl *)point_buf;
   uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type);
   LivoxRawPoint *raw_point =
@@ -206,7 +207,8 @@ uint8_t *LivoxRawPointToPxyzrtl(uint8_t *point_buf, LivoxEthPacket *eth_packet,
 
   while (points_per_packet) {
     RawPointConvert((LivoxPointXyzr *)dst_point, raw_point);
-    if (extrinsic.enable) {
+    if (extrinsic.enable && IsTripleIntNoneZero(raw_point->x,
+        raw_point->y, raw_point->z)) {
       PointXyz src_point = *((PointXyz *)dst_point);
       PointExtrisincCompensation((PointXyz *)dst_point, src_point, extrinsic);
     }
@@ -220,9 +222,9 @@ uint8_t *LivoxRawPointToPxyzrtl(uint8_t *point_buf, LivoxEthPacket *eth_packet,
   return (uint8_t *)dst_point;
 }
 
-uint8_t *LivoxSpherPointToPxyzrtl(uint8_t *point_buf,
-                                  LivoxEthPacket *eth_packet,
-                                  ExtrinsicParameter &extrinsic) {
+static uint8_t *LivoxSpherPointToPxyzrtl(uint8_t *point_buf, \
+    LivoxEthPacket *eth_packet, ExtrinsicParameter &extrinsic, \
+    uint32_t line_num) {
   LivoxPointXyzrtl *dst_point = (LivoxPointXyzrtl *)point_buf;
   uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type);
   LivoxSpherPoint *raw_point =
@@ -230,7 +232,7 @@ uint8_t *LivoxSpherPointToPxyzrtl(uint8_t *point_buf,
 
   while (points_per_packet) {
     RawPointConvert((LivoxPointXyzr *)dst_point, raw_point);
-    if (extrinsic.enable) {
+    if (extrinsic.enable && raw_point->depth) {
       PointXyz src_point = *((PointXyz *)dst_point);
       PointExtrisincCompensation((PointXyz *)dst_point, src_point, extrinsic);
     }
@@ -244,9 +246,9 @@ uint8_t *LivoxSpherPointToPxyzrtl(uint8_t *point_buf,
   return (uint8_t *)dst_point;
 }
 
-uint8_t *LivoxExtendRawPointToPxyzrtl(uint8_t *point_buf,
-                                      LivoxEthPacket *eth_packet,
-                                      ExtrinsicParameter &extrinsic) {
+static uint8_t *LivoxExtendRawPointToPxyzrtl(uint8_t *point_buf, \
+    LivoxEthPacket *eth_packet, ExtrinsicParameter &extrinsic, \
+    uint32_t line_num) {
   LivoxPointXyzrtl *dst_point = (LivoxPointXyzrtl *)point_buf;
   uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type);
   LivoxExtendRawPoint *raw_point =
@@ -255,13 +257,17 @@ uint8_t *LivoxExtendRawPointToPxyzrtl(uint8_t *point_buf,
   uint8_t line_id = 0;
   while (points_per_packet) {
     RawPointConvert((LivoxPointXyzr *)dst_point, (LivoxRawPoint *)raw_point);
-    if (extrinsic.enable) {
+    if (extrinsic.enable && IsTripleIntNoneZero(raw_point->x,
+        raw_point->y, raw_point->z)) {
       PointXyz src_point = *((PointXyz *)dst_point);
       PointExtrisincCompensation((PointXyz *)dst_point, src_point, extrinsic);
     }
     dst_point->tag = raw_point->tag;
-    dst_point->line = line_id;
-    dst_point->line = dst_point->line % 6;
+    if (line_num > 1) {
+      dst_point->line = line_id % line_num;
+    } else {
+      dst_point->line = 0;
+    }
     ++raw_point;
     ++dst_point;
     ++line_id;
@@ -271,9 +277,9 @@ uint8_t *LivoxExtendRawPointToPxyzrtl(uint8_t *point_buf,
   return (uint8_t *)dst_point;
 }
 
-uint8_t *LivoxExtendSpherPointToPxyzrtl(uint8_t *point_buf,
-                                        LivoxEthPacket *eth_packet,
-                                        ExtrinsicParameter &extrinsic) {
+static uint8_t *LivoxExtendSpherPointToPxyzrtl(uint8_t *point_buf, \
+    LivoxEthPacket *eth_packet, ExtrinsicParameter &extrinsic, \
+    uint32_t line_num) {
   LivoxPointXyzrtl *dst_point = (LivoxPointXyzrtl *)point_buf;
   uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type);
   LivoxExtendSpherPoint *raw_point =
@@ -282,13 +288,16 @@ uint8_t *LivoxExtendSpherPointToPxyzrtl(uint8_t *point_buf,
   uint8_t line_id = 0;
   while (points_per_packet) {
     RawPointConvert((LivoxPointXyzr *)dst_point, (LivoxSpherPoint *)raw_point);
-    if (extrinsic.enable) {
+    if (extrinsic.enable && raw_point->depth) {
       PointXyz src_point = *((PointXyz *)dst_point);
       PointExtrisincCompensation((PointXyz *)dst_point, src_point, extrinsic);
     }
     dst_point->tag = raw_point->tag;
-    dst_point->line = line_id;
-    dst_point->line = dst_point->line % 6;
+    if (line_num > 1) {
+      dst_point->line = line_id % line_num;
+    } else {
+      dst_point->line = 0;
+    }
     ++raw_point;
     ++dst_point;
     ++line_id;
@@ -298,25 +307,30 @@ uint8_t *LivoxExtendSpherPointToPxyzrtl(uint8_t *point_buf,
   return (uint8_t *)dst_point;
 }
 
-uint8_t *LivoxDualExtendRawPointToPxyzrtl(uint8_t *point_buf,
-                                          LivoxEthPacket *eth_packet,
-                                          ExtrinsicParameter &extrinsic) {
+static uint8_t *LivoxDualExtendRawPointToPxyzrtl(uint8_t *point_buf, \
+    LivoxEthPacket *eth_packet, ExtrinsicParameter &extrinsic, \
+    uint32_t line_num) {
   LivoxPointXyzrtl *dst_point = (LivoxPointXyzrtl *)point_buf;
   uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type);
   LivoxExtendRawPoint *raw_point =
       reinterpret_cast<LivoxExtendRawPoint *>(eth_packet->data);
 
+  /* LivoxDualExtendRawPoint = 2*LivoxExtendRawPoint */
+  points_per_packet = points_per_packet * 2;
   uint8_t line_id = 0;
   while (points_per_packet) {
     RawPointConvert((LivoxPointXyzr *)dst_point, (LivoxRawPoint *)raw_point);
-    if (extrinsic.enable) {
+    if (extrinsic.enable && IsTripleIntNoneZero(raw_point->x,
+        raw_point->y, raw_point->z)) {
       PointXyz src_point = *((PointXyz *)dst_point);
       PointExtrisincCompensation((PointXyz *)dst_point, src_point, extrinsic);
     }
     dst_point->tag = raw_point->tag;
-    dst_point->line =
-        line_id / 2; /* LivoxDualExtendRawPoint = 2*LivoxExtendRawPoint */
-    dst_point->line = dst_point->line % 6;
+    if (line_num > 1) {
+      dst_point->line = (line_id / 2) % line_num;
+    } else {
+      dst_point->line = 0;
+    }
     ++raw_point;
     ++dst_point;
     ++line_id;
@@ -326,9 +340,9 @@ uint8_t *LivoxDualExtendRawPointToPxyzrtl(uint8_t *point_buf,
   return (uint8_t *)dst_point;
 }
 
-uint8_t *LivoxDualExtendSpherPointToPxyzrtl(uint8_t *point_buf,
-                                            LivoxEthPacket *eth_packet,
-                                            ExtrinsicParameter &extrinsic) {
+static uint8_t *LivoxDualExtendSpherPointToPxyzrtl(uint8_t *point_buf, \
+    LivoxEthPacket *eth_packet, ExtrinsicParameter &extrinsic, \
+    uint32_t line_num) {
   LivoxPointXyzrtl *dst_point = (LivoxPointXyzrtl *)point_buf;
   uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type);
   LivoxDualExtendSpherPoint *raw_point =
@@ -339,22 +353,119 @@ uint8_t *LivoxDualExtendSpherPointToPxyzrtl(uint8_t *point_buf,
     RawPointConvert((LivoxPointXyzr *)dst_point,
                     (LivoxPointXyzr *)(dst_point + 1),
                     (LivoxDualExtendSpherPoint *)raw_point);
-    if (extrinsic.enable) {
+    if (extrinsic.enable && raw_point->depth1) {
       PointXyz src_point = *((PointXyz *)dst_point);
       PointExtrisincCompensation((PointXyz *)dst_point, src_point, extrinsic);
     }
     dst_point->tag = raw_point->tag1;
-    dst_point->line = line_id;
-    dst_point->line = dst_point->line % 6;
+    if (line_num > 1) {
+      dst_point->line = line_id % line_num;
+    } else {
+      dst_point->line = 0;
+    }
     ++dst_point;
 
-    if (extrinsic.enable) {
+    if (extrinsic.enable && raw_point->depth2) {
       PointXyz src_point = *((PointXyz *)dst_point);
       PointExtrisincCompensation((PointXyz *)dst_point, src_point, extrinsic);
     }
     dst_point->tag = raw_point->tag2;
-    dst_point->line = line_id;
-    dst_point->line = dst_point->line % 6;
+    if (line_num > 1) {
+      dst_point->line = line_id % line_num;
+    } else {
+      dst_point->line = 0;
+    }
+    ++dst_point;
+
+    ++raw_point; /* only increase one */
+    ++line_id;
+    --points_per_packet;
+  }
+
+  return (uint8_t *)dst_point;
+}
+
+static uint8_t *LivoxTripleExtendRawPointToPxyzrtl(uint8_t *point_buf, \
+    LivoxEthPacket *eth_packet, ExtrinsicParameter &extrinsic, \
+    uint32_t line_num) {
+  LivoxPointXyzrtl *dst_point = (LivoxPointXyzrtl *)point_buf;
+  uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type);
+  LivoxExtendRawPoint *raw_point =
+      reinterpret_cast<LivoxExtendRawPoint *>(eth_packet->data);
+
+  /* LivoxTripleExtendRawPoint = 3*LivoxExtendRawPoint, echo_num */
+  points_per_packet = points_per_packet * 3;
+  uint8_t line_id = 0;
+  while (points_per_packet) {
+    RawPointConvert((LivoxPointXyzr *)dst_point, (LivoxRawPoint *)raw_point);
+    if (extrinsic.enable && IsTripleIntNoneZero(raw_point->x,
+        raw_point->y, raw_point->z)) {
+      PointXyz src_point = *((PointXyz *)dst_point);
+      PointExtrisincCompensation((PointXyz *)dst_point, src_point, extrinsic);
+    }
+    dst_point->tag = raw_point->tag;
+    if (line_num > 1) {
+      dst_point->line = (line_id / 3) % line_num;
+    } else {
+      dst_point->line = 0;
+    }
+    ++raw_point;
+    ++dst_point;
+    ++line_id;
+    --points_per_packet;
+  }
+
+  return (uint8_t *)dst_point;
+}
+
+static uint8_t *LivoxTripleExtendSpherPointToPxyzrtl(uint8_t *point_buf, \
+    LivoxEthPacket *eth_packet, ExtrinsicParameter &extrinsic, \
+    uint32_t line_num) {
+  LivoxPointXyzrtl *dst_point = (LivoxPointXyzrtl *)point_buf;
+  uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type);
+  LivoxTripleExtendSpherPoint *raw_point =
+      reinterpret_cast<LivoxTripleExtendSpherPoint *>(eth_packet->data);
+
+  uint8_t line_id = 0;
+  while (points_per_packet) {
+    RawPointConvert((LivoxPointXyzr *)dst_point,
+                    (LivoxPointXyzr *)(dst_point + 1),
+                    (LivoxPointXyzr *)(dst_point + 2),
+                    (LivoxTripleExtendSpherPoint *)raw_point);
+    if (extrinsic.enable && raw_point->depth1) {
+      PointXyz src_point = *((PointXyz *)dst_point);
+      PointExtrisincCompensation((PointXyz *)dst_point, src_point, extrinsic);
+    }
+    dst_point->tag = raw_point->tag1;
+    if (line_num > 1) {
+      dst_point->line = line_id % line_num;
+    } else {
+      dst_point->line = 0;
+    }
+    ++dst_point;
+
+    if (extrinsic.enable && raw_point->depth2) {
+      PointXyz src_point = *((PointXyz *)dst_point);
+      PointExtrisincCompensation((PointXyz *)dst_point, src_point, extrinsic);
+    }
+    dst_point->tag = raw_point->tag2;
+    if (line_num > 1) {
+      dst_point->line = line_id % line_num;
+    } else {
+      dst_point->line = 0;
+    }
+    ++dst_point;
+
+    if (extrinsic.enable && raw_point->depth3) {
+      PointXyz src_point = *((PointXyz *)dst_point);
+      PointExtrisincCompensation((PointXyz *)dst_point, src_point, extrinsic);
+    }
+    dst_point->tag = raw_point->tag3;
+    if (line_num > 1) {
+      dst_point->line = line_id % line_num;
+    } else {
+      dst_point->line = 0;
+    }
     ++dst_point;
 
     ++raw_point; /* only increase one */
@@ -377,7 +488,10 @@ const PointConvertHandler to_pxyzi_handler_table[kMaxPointDataType] = {
     LivoxExtendSpherPointToPxyzrtl,
     LivoxDualExtendRawPointToPxyzrtl,
     LivoxDualExtendSpherPointToPxyzrtl,
-    nullptr};
+    nullptr,
+    LivoxTripleExtendRawPointToPxyzrtl,
+    LivoxTripleExtendSpherPointToPxyzrtl
+    };
 
 PointConvertHandler GetConvertHandler(uint8_t data_type) {
   if (data_type < kMaxPointDataType)
@@ -386,22 +500,11 @@ PointConvertHandler GetConvertHandler(uint8_t data_type) {
     return nullptr;
 }
 
-uint8_t *FillZeroPointXyzrtl(uint8_t *point_buf, uint32_t num) {
-  LivoxPointXyzrtl *dst_point = (LivoxPointXyzrtl *)point_buf;
-  uint32_t points_per_packet = num;
-
-  while (points_per_packet) {
-    dst_point->x = 0;
-    dst_point->y = 0;
-    dst_point->z = 0;
-    dst_point->reflectivity = 0;
-    dst_point->tag = 0;
-    dst_point->line = 0;
-    ++dst_point;
-    --points_per_packet;
-  }
-
-  return (uint8_t *)dst_point;
+void ZeroPointDataOfStoragePacket(StoragePacket* storage_packet) {
+  LivoxEthPacket *raw_packet =
+      reinterpret_cast<LivoxEthPacket *>(storage_packet->raw_data);
+  uint32_t point_length = GetPointLen(raw_packet->data_type);
+  memset(raw_packet->data, 0, point_length * storage_packet->point_num);
 }
 
 #if 0
@@ -415,13 +518,10 @@ static void PointCloudConvert(LivoxPoint *p_dpoint, LivoxRawPoint *p_raw_point)
 
 #endif
 
-/* Member function ------
- * ----------------------------------------------------------------------- */
-
-Lds::Lds(uint32_t buffer_time_ms, uint8_t data_src)
-    : buffer_time_ms_(buffer_time_ms), data_src_(data_src) {
-  lidar_count_ = kMaxSourceLidar;
-  request_exit_ = false;
+/* Member function --------------------------------------------------------- */
+Lds::Lds(uint32_t buffer_time_ms, uint8_t data_src) : \
+    lidar_count_(kMaxSourceLidar), semaphore_(0), \
+    buffer_time_ms_(buffer_time_ms), data_src_(data_src), request_exit_(false) {
     ResetLds(data_src_);
 };
 
@@ -455,6 +555,31 @@ void Lds::ResetLds(uint8_t data_src) {
   }
 }
 
+void Lds::RequestExit() {
+  request_exit_ = true;
+}
+
+bool Lds::IsAllQueueEmpty() {
+  for (int i = 0; i < lidar_count_; i++) {
+    if (!QueueIsEmpty(&lidars_[i].data)) {
+      return false;
+    }
+  }
+
+  return true;
+}
+
+bool Lds::IsAllQueueReadStop() {
+  for (int i = 0; i < lidar_count_; i++) {
+    uint32_t data_size = QueueUsedSize(&lidars_[i].data);
+    if (data_size && (data_size > lidars_[i].onetime_publish_packets)) {
+      return false;
+    }
+  }
+
+  return true;
+}
+
 uint8_t Lds::GetDeviceType(uint8_t handle) {
   if (handle < kMaxSourceLidar) {
     return lidars_[handle].info.type;
@@ -463,6 +588,92 @@ uint8_t Lds::GetDeviceType(uint8_t handle) {
   }
 }
 
+void Lds::UpdateLidarInfoByEthPacket(LidarDevice *p_lidar, \
+    LivoxEthPacket* eth_packet) {
+  if (p_lidar->raw_data_type != eth_packet->data_type) {
+    p_lidar->raw_data_type = eth_packet->data_type;
+    p_lidar->packet_interval = GetPacketInterval(p_lidar->info.type, \
+        eth_packet->data_type);
+    p_lidar->packet_interval_max = p_lidar->packet_interval * 1.8f;
+    p_lidar->onetime_publish_packets = \
+        GetPacketNumPerSec(p_lidar->info.type, \
+        p_lidar->raw_data_type) * buffer_time_ms_ / 1000;
+    printf("DataType[%d] PacketInterval[%d] PublishPackets[%d]\n", \
+        p_lidar->raw_data_type, p_lidar->packet_interval, \
+        p_lidar->onetime_publish_packets);
+  }
+}
+
+void Lds::StorageRawPacket(uint8_t handle, LivoxEthPacket* eth_packet) {
+  LidarDevice *p_lidar = &lidars_[handle];
+  LidarPacketStatistic *packet_statistic = &p_lidar->statistic_info;
+  LdsStamp cur_timestamp;
+  memcpy(cur_timestamp.stamp_bytes, eth_packet->timestamp,
+         sizeof(cur_timestamp));
+
+  if (kImu != eth_packet->data_type) {
+    UpdateLidarInfoByEthPacket(p_lidar, eth_packet);
+    if (eth_packet->timestamp_type == kTimestampTypePps) {
+      /** Whether a new sync frame */
+      if ((cur_timestamp.stamp < packet_statistic->last_timestamp) &&
+          (cur_timestamp.stamp < kPacketTimeGap)) {
+        auto cur_time = std::chrono::high_resolution_clock::now();
+        int64_t sync_time = cur_time.time_since_epoch().count();
+        /** used receive time as timebase */
+        packet_statistic->timebase = sync_time;
+      }
+    }
+    packet_statistic->last_timestamp = cur_timestamp.stamp;
+
+    LidarDataQueue *p_queue = &p_lidar->data;
+    if (nullptr == p_queue->storage_packet) {
+      uint32_t queue_size = CalculatePacketQueueSize(
+          buffer_time_ms_, p_lidar->info.type, eth_packet->data_type);
+      queue_size = queue_size * 8; /* 8 multiple the min size */
+      InitQueue(p_queue, queue_size);
+      printf("Lidar%02d[%s] queue size : %d %d\n", p_lidar->handle,
+             p_lidar->info.broadcast_code, queue_size, p_queue->size);
+    }
+    if (!QueueIsFull(p_queue)) {
+      QueuePushAny(p_queue, (uint8_t *)eth_packet, \
+          GetEthPacketLen(eth_packet->data_type), \
+          packet_statistic->timebase, \
+          GetPointsPerPacket(eth_packet->data_type));
+      if (QueueUsedSize(p_queue) > p_lidar->onetime_publish_packets) {
+        if (semaphore_.GetCount() <= 0) {
+          semaphore_.Signal();
+        }
+      }
+    }
+  } else {
+    if (eth_packet->timestamp_type == kTimestampTypePps) {
+      /** Whether a new sync frame */
+      if ((cur_timestamp.stamp < packet_statistic->last_imu_timestamp) &&
+          (cur_timestamp.stamp < kPacketTimeGap)) {
+        auto cur_time = std::chrono::high_resolution_clock::now();
+        int64_t sync_time = cur_time.time_since_epoch().count();
+        /** used receive time as timebase */
+        packet_statistic->imu_timebase = sync_time;
+      }
+    }
+    packet_statistic->last_imu_timestamp = cur_timestamp.stamp;
+
+    LidarDataQueue *p_queue = &p_lidar->imu_data;
+    if (nullptr == p_queue->storage_packet) {
+      uint32_t queue_size = 256;  /* fixed imu data queue size */
+      InitQueue(p_queue, queue_size);
+      printf("Lidar%02d[%s] imu queue size : %d %d\n", p_lidar->handle,
+             p_lidar->info.broadcast_code, queue_size, p_queue->size);
+    }
+    if (!QueueIsFull(p_queue)) {
+      QueuePushAny(p_queue, (uint8_t *)eth_packet, \
+          GetEthPacketLen(eth_packet->data_type),\
+          packet_statistic->imu_timebase, \
+          GetPointsPerPacket(eth_packet->data_type));
+    }
+  }
+}
+
 void Lds::PrepareExit(void) {}
 
 }  // namespace livox_ros

livox_ros_driver/livox_ros_driver/lds.h

@@ -33,6 +33,8 @@
 #include <stdlib.h>
 #include <string>
 #include <vector>
+#include <mutex>
+#include <condition_variable>
 
 #include "ldq.h"
 
@@ -55,10 +57,10 @@ const uint32_t KCartesianPointSize = 13;
 const uint32_t KSphericalPointSzie = 9;
 
 const int64_t kPacketTimeGap = 1000000; /**< 1ms = 1000000ns */
-const int64_t kMaxPacketTimeGap =
-    1700000; /**< the threshold of packet continuous */
-const int64_t kDeviceDisconnectThreshold =
-    1000000000; /**< the threshold of device disconect */
+/**< the threshold of packet continuous */
+const int64_t kMaxPacketTimeGap = 1700000;
+/**< the threshold of device disconect */
+const int64_t kDeviceDisconnectThreshold = 1000000000;
 const int64_t kNsPerSecond = 1000000000; /**< 1s  = 1000000000ns */
 
 const int kPathStrMinSize = 4;   /**< Must more than 4 char */
@@ -176,6 +178,8 @@ typedef struct {
                                         of current lidar, unit:ns */
   volatile uint32_t packet_interval_max; /**< If more than it,
                                             have packet loss */
+  /**< packet num that onetime published */
+  volatile uint32_t onetime_publish_packets;
   volatile LidarConnectState connect_state;
   DeviceInfo info;
   LidarPacketStatistic statistic_info;
@@ -187,12 +191,17 @@ typedef struct {
 } LidarDevice;
 
 typedef struct {
-  uint32_t points_per_packet;
-  uint32_t points_per_second;
+  uint32_t points_per_packet; /**< number of points every packet */
+  uint32_t packet_length;     /**< length of raw ethenet packet unit:bytes */
+  uint32_t raw_point_length;  /**< length of point uint:bytes */
+  uint32_t echo_num;          /**< echo number of current data */
+} DataTypePointInfoPair;
+
+typedef struct {
+  uint32_t points_per_second; /**< number of points per second */
   uint32_t point_interval;    /**< unit:ns */
-  uint32_t packet_interval; /**< unit:ns */
-  uint32_t packet_length;
-} PacketInfoPair;
+  uint32_t line_num;          /**< laser line number */
+} ProductTypePointInfoPair;
 
 #pragma pack(1)
 
@@ -220,52 +229,90 @@ typedef struct {
 
 #pragma pack()
 
-typedef uint8_t *(*PointConvertHandler)(uint8_t *point_buf,
-                                        LivoxEthPacket *eth_packet,
-                                        ExtrinsicParameter &extrinsic);
+typedef uint8_t *(*PointConvertHandler)(uint8_t *point_buf, \
+    LivoxEthPacket *eth_packet, ExtrinsicParameter &extrinsic, \
+    uint32_t line_num);
 
-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, 200, 10000000, 10000000, 42}};
+const DataTypePointInfoPair data_type_info_pair_table[kMaxPointDataType] = {
+    {100, 1318, sizeof(LivoxRawPoint), 1},
+    {100, 918,  9,  1},
+    {96,  1362, 14, 1},
+    {96,  978,  9,  1},
+    {48,  1362, sizeof(LivoxDualExtendRawPoint),   2},
+    {48,  786,  sizeof(LivoxDualExtendSpherPoint), 2},
+    {1,   42,   sizeof(LivoxImuPoint), 1},
+    {30,  1278, sizeof(LivoxTripleExtendRawPoint),   3},
+    {30,  678,  sizeof(LivoxTripleExtendSpherPoint), 3}};
+
+const uint32_t kMaxProductType = 9;
+const uint32_t kDeviceTypeLidarMid70 = 6;
+const ProductTypePointInfoPair product_type_info_pair_table[kMaxProductType] = {
+    {100000, 10000, 1},
+    {100000, 10000, 1},
+    {240000, 4167 , 6}, /**< tele */
+    {240000, 4167 , 6},
+    {100000, 10000, 1},
+    {100000, 10000, 1},
+    {100000, 10000, 1}, /**< mid70 */
+    {240000, 4167,  6},
+    {240000, 4167,  6},
+};
 
 /**
  * Global function for general use.
  */
 bool IsFilePathValid(const char *path_str);
-uint64_t GetStoragePacketTimestamp(StoragePacket *packet, uint8_t data_src_);
-uint32_t CalculatePacketQueueSize(uint32_t interval_ms, uint32_t data_type);
+uint64_t GetStoragePacketTimestamp(StoragePacket *packet, uint8_t data_src);
+uint32_t CalculatePacketQueueSize(uint32_t interval_ms, uint8_t product_type,
+                                  uint8_t data_type);
 void ParseCommandlineInputBdCode(const char *cammandline_str,
                                  std::vector<std::string> &bd_code_list);
 PointConvertHandler GetConvertHandler(uint8_t data_type);
 uint8_t *LivoxPointToPxyzrtl(uint8_t *point_buf, LivoxEthPacket *eth_packet,
-                             ExtrinsicParameter &extrinsic);
-uint8_t *FillZeroPointXyzrtl(uint8_t *point_buf, uint32_t num);
+    ExtrinsicParameter &extrinsic, uint32_t line_num);
+void ZeroPointDataOfStoragePacket(StoragePacket* storage_packet);
 uint8_t *LivoxImuDataProcess(uint8_t *point_buf, LivoxEthPacket *eth_packet);
 void EulerAnglesToRotationMatrix(EulerAngle euler, RotationMatrix matrix);
 void PointExtrisincCompensation(PointXyz *dst_point,
                                 ExtrinsicParameter &extrinsic);
 
-inline uint32_t GetPointInterval(uint32_t data_type) {
-  return packet_info_pair_table[data_type].point_interval;
+inline uint32_t GetPointInterval(uint32_t product_type) {
+  return product_type_info_pair_table[product_type].point_interval;
 }
 
-inline uint32_t GetPacketNumPerSec(uint32_t data_type) {
-  return packet_info_pair_table[data_type].points_per_second /
-         packet_info_pair_table[data_type].points_per_packet;
+// inline uint32_t GetPacketNumPerSec(uint32_t data_type) {
+//  return packet_info_pair_table[data_type].points_per_second /
+//         packet_info_pair_table[data_type].points_per_packet;
+//}
+
+inline uint32_t GetPacketNumPerSec(uint32_t product_type, uint32_t data_type) {
+  return product_type_info_pair_table[product_type].points_per_second /
+      data_type_info_pair_table[data_type].points_per_packet;
+}
+
+inline uint32_t GetPacketInterval(uint32_t product_type, uint32_t data_type) {
+  return product_type_info_pair_table[product_type].point_interval *
+      data_type_info_pair_table[data_type].points_per_packet;
+}
+
+inline uint32_t GetLaserLineNumber(uint32_t product_type) {
+  return product_type_info_pair_table[product_type].line_num;
 }
 
 inline uint32_t GetPointsPerPacket(uint32_t data_type) {
-  return packet_info_pair_table[data_type].points_per_packet;
-}
-
-inline uint32_t GetPacketInterval(uint32_t data_type) {
-  return packet_info_pair_table[data_type].packet_interval;
+  return data_type_info_pair_table[data_type].points_per_packet;
 }
 
 inline uint32_t GetEthPacketLen(uint32_t data_type) {
-  return packet_info_pair_table[data_type].packet_length;
+  return data_type_info_pair_table[data_type].packet_length;
+}
+
+inline uint32_t GetPointLen(uint32_t data_type) {
+  return data_type_info_pair_table[data_type].raw_point_length;
+}
+
+inline uint32_t GetEchoNumPerPoint(uint32_t data_type) {
+  return data_type_info_pair_table[data_type].echo_num;
 }
 
 inline void RawPointConvert(LivoxPointXyzr *dst_point, LivoxPoint *raw_point) {
@@ -306,12 +353,72 @@ inline void RawPointConvert(LivoxPointXyzr *dst_point1,
   dst_point1->z = radius1 * cos(theta);
   dst_point1->reflectivity = (float)raw_point->reflectivity1;
 
-  (dst_point2 + 1)->x = radius2 * sin(theta) * cos(phi);
-  (dst_point2 + 1)->y = radius2 * sin(theta) * sin(phi);
-  (dst_point2 + 1)->z = radius2 * cos(theta);
-  (dst_point2 + 1)->reflectivity = (float)raw_point->reflectivity2;
+  dst_point2->x = radius2 * sin(theta) * cos(phi);
+  dst_point2->y = radius2 * sin(theta) * sin(phi);
+  dst_point2->z = radius2 * cos(theta);
+  dst_point2->reflectivity = (float)raw_point->reflectivity2;
 }
 
+inline void RawPointConvert(LivoxPointXyzr *dst_point1,
+                            LivoxPointXyzr *dst_point2,
+                            LivoxPointXyzr *dst_point3,
+                            LivoxTripleExtendSpherPoint *raw_point) {
+  double radius1 = raw_point->depth1 / 1000.0;
+  double radius2 = raw_point->depth2 / 1000.0;
+  double radius3 = raw_point->depth3 / 1000.0;
+  double theta = raw_point->theta / 100.0 / 180 * PI;
+  double phi = raw_point->phi / 100.0 / 180 * PI;
+  dst_point1->x = radius1 * sin(theta) * cos(phi);
+  dst_point1->y = radius1 * sin(theta) * sin(phi);
+  dst_point1->z = radius1 * cos(theta);
+  dst_point1->reflectivity = (float)raw_point->reflectivity1;
+
+  dst_point2->x = radius2 * sin(theta) * cos(phi);
+  dst_point2->y = radius2 * sin(theta) * sin(phi);
+  dst_point2->z = radius2 * cos(theta);
+  dst_point2->reflectivity = (float)raw_point->reflectivity2;
+
+  dst_point3->x = radius3 * sin(theta) * cos(phi);
+  dst_point3->y = radius3 * sin(theta) * sin(phi);
+  dst_point3->z = radius3 * cos(theta);
+  dst_point3->reflectivity = (float)raw_point->reflectivity3;
+}
+
+inline bool IsTripleIntNoneZero(int32_t x, int32_t y, int32_t z) {
+    return (x | y | z);
+}
+
+inline bool IsTripleFloatNoneZero(float x, float y, float z) {
+    return ((x != 0.0f) || (y != 0.0f) || (z != 0.0f));
+}
+
+class Semaphore {
+ public:
+  explicit Semaphore(int count = 0) : count_(count) {
+  }
+
+  void Signal() {
+    std::unique_lock<std::mutex> lock(mutex_);
+    ++count_;
+    cv_.notify_one();
+  }
+
+  void Wait() {
+    std::unique_lock<std::mutex> lock(mutex_);
+    cv_.wait(lock, [=] { return count_ > 0; });
+    --count_;
+  }
+
+  int GetCount() {
+    return count_;
+  }
+
+ private:
+  std::mutex mutex_;
+  std::condition_variable cv_;
+  volatile int count_;
+};
+
 /**
  * Lidar data source abstract.
  */
@@ -320,17 +427,22 @@ public:
   Lds(uint32_t buffer_time_ms, uint8_t data_src);
   virtual ~Lds();
 
+  void StorageRawPacket(uint8_t handle, LivoxEthPacket* eth_packet);
   uint8_t GetDeviceType(uint8_t handle);
   static void ResetLidar(LidarDevice *lidar, uint8_t data_src);
   static void SetLidarDataSrc(LidarDevice *lidar, uint8_t data_src);
   void ResetLds(uint8_t data_src);
-  void RequestExit() { request_exit_ = true; }
+  void RequestExit();
+  bool IsAllQueueEmpty();
+  bool IsAllQueueReadStop();
   void CleanRequestExit() { request_exit_ = false; }
   bool IsRequestExit() { return request_exit_; }
   virtual void PrepareExit(void);
-
+  void UpdateLidarInfoByEthPacket(LidarDevice *p_lidar, \
+      LivoxEthPacket* eth_packet);
   uint8_t lidar_count_;                 /**< Lidar access handle. */
   LidarDevice lidars_[kMaxSourceLidar]; /**< The index is the handle */
+  Semaphore semaphore_;
 
  protected:
   uint32_t buffer_time_ms_; /**< Buffer time before data in queue is read */

livox_ros_driver/livox_ros_driver/lds_hub.cpp

@@ -24,9 +24,9 @@
 
 #include "lds_hub.h"
 
-#include <memory>
 #include <stdio.h>
 #include <string.h>
+#include <memory>
 #include <thread>
 
 #include "rapidjson/document.h"
@@ -35,18 +35,14 @@
 
 namespace livox_ros {
 
-/** Const varible
- * -------------------------------------------------------------------------------
- */
+/** Const varible ------------------------------------------------------------*/
 
 /** For callback use only */
 static LdsHub *g_lds_hub = nullptr;
 
-/** Global function for common use
- * ---------------------------------------------------------------*/
+/** Global function for common use -------------------------------------------*/
 
-/** Lds hub function
- * -----------------------------------------------------------------------------*/
+/** Lds hub function ---------------------------------------------------------*/
 LdsHub::LdsHub(uint32_t interval_ms) : Lds(interval_ms, kSourceRawHub) {
   auto_connect_mode_ = true;
   whitelist_count_ = 0;
@@ -67,7 +63,6 @@ void LdsHub::ResetLdsHub(void) {
 
 int LdsHub::InitLdsHub(std::vector<std::string> &broadcast_code_strs,
                        const char *user_config_path) {
-
   if (is_initialized_) {
     printf("LiDAR data source is already inited!\n");
     return -1;
@@ -104,7 +99,8 @@ int LdsHub::InitLdsHub(std::vector<std::string> &broadcast_code_strs,
     }
   } else {
     EnableAutoConnectMode();
-    printf("No broadcast code was added to whitelist, swith to automatic "
+    printf(
+        "No broadcast code was added to whitelist, swith to automatic "
         "connection mode!\n");
   }
 
@@ -126,7 +122,6 @@ int LdsHub::InitLdsHub(std::vector<std::string> &broadcast_code_strs,
 }
 
 int LdsHub::DeInitLdsHub(void) {
-
   if (!is_initialized_) {
     printf("LiDAR data source is not exit");
     return -1;
@@ -149,81 +144,14 @@ void LdsHub::OnHubDataCb(uint8_t hub_handle, LivoxEthPacket *data,
   if (!data || !data_num) {
     return;
   }
+
   /** Caculate which lidar the eth packet data belong to */
   uint8_t handle = HubGetLidarHandle(eth_packet->slot, eth_packet->id);
   if (handle >= kMaxLidarCount) {
     return;
   }
 
-  LidarDevice *p_lidar = &lds_hub->lidars_[handle];
-  LidarPacketStatistic *packet_statistic = &p_lidar->statistic_info;
-  LdsStamp cur_timestamp;
-  memcpy(cur_timestamp.stamp_bytes, eth_packet->timestamp,
-         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) &&
-          (cur_timestamp.stamp < kPacketTimeGap)) {
-        auto cur_time = std::chrono::high_resolution_clock::now();
-        int64_t sync_time = cur_time.time_since_epoch().count();
-        /** used receive time as timebase */
-        packet_statistic->timebase = sync_time;
-      }
-    }
-    packet_statistic->last_timestamp = cur_timestamp.stamp;
-
-    LidarDataQueue *p_queue = &p_lidar->data;
-    if (nullptr == p_queue->storage_packet) {
-      uint32_t queue_size = CalculatePacketQueueSize(lds_hub->buffer_time_ms_,
-                                                     eth_packet->data_type);
-      queue_size = queue_size * 16; /* 16 multiple the min size */
-      InitQueue(p_queue, queue_size);
-      printf("Lidar%02d[%s] queue size : %d %d\n", p_lidar->handle,
-             p_lidar->info.broadcast_code, queue_size, p_queue->size);
-    }
-    if (!QueueIsFull(p_queue)) {
-      QueuePushAny(p_queue, (uint8_t *)eth_packet,
-                   GetEthPacketLen(eth_packet->data_type),
-                   packet_statistic->timebase,
-                   GetPointsPerPacket(eth_packet->data_type));
-    }
-  } else {
-    if (eth_packet->timestamp_type == kTimestampTypePps) {
-      /** Whether a new sync frame */
-      if ((cur_timestamp.stamp < packet_statistic->last_imu_timestamp) &&
-          (cur_timestamp.stamp < kPacketTimeGap)) {
-        auto cur_time = std::chrono::high_resolution_clock::now();
-        int64_t sync_time = cur_time.time_since_epoch().count();
-        /** used receive time as timebase */
-        packet_statistic->imu_timebase = sync_time;
-      }
-    }
-    packet_statistic->last_imu_timestamp = cur_timestamp.stamp;
-
-    LidarDataQueue *p_queue = &p_lidar->imu_data;
-    if (nullptr == p_queue->storage_packet) {
-      uint32_t queue_size = 256;
-      InitQueue(p_queue, queue_size);
-      printf("Lidar%02d[%s] imu queue size : %d %d\n", p_lidar->handle,
-             p_lidar->info.broadcast_code, queue_size, p_queue->size);
-    }
-
-    if (!QueueIsFull(p_queue)) {
-      QueuePushAny(p_queue, (uint8_t *)eth_packet,
-                   GetEthPacketLen(eth_packet->data_type),
-                   packet_statistic->imu_timebase,
-                   GetPointsPerPacket(eth_packet->data_type));
-    }
-  }
+  lds_hub->StorageRawPacket(handle, eth_packet);
 }
 
 void LdsHub::OnDeviceBroadcast(const BroadcastDeviceInfo *info) {
@@ -300,7 +228,8 @@ void LdsHub::OnDeviceChange(const DeviceInfo *info, DeviceEvent type) {
       printf("Hub[%s] connect on\n", p_hub->info.broadcast_code);
     }
   } else if (type == kEventDisconnect) {
-    p_hub->connect_state = kConnectStateOff;
+    g_lds_hub->ResetLds(0);
+    g_lds_hub->ResetLidar(p_hub, 0);
     printf("Hub[%s] disconnect!\n", info->broadcast_code);
   } else if (type == kEventStateChange) {
     p_hub->info = *info;
@@ -526,6 +455,7 @@ void LdsHub::ConfigImuPushFrequency(LdsHub *lds_hub) {
     LidarDevice *p_lidar = &(lds_hub->lidars_[i]);
 
     if ((p_lidar->info.type != kDeviceTypeLidarMid40) &&
+        (p_lidar->info.type != kDeviceTypeLidarMid70) &&
         (p_lidar->connect_state == kConnectStateSampling)) {
       UserRawConfig config;
       if (lds_hub->GetRawConfig(p_lidar->info.broadcast_code, config)) {

livox_ros_driver/livox_ros_driver/lds_hub.h

@@ -69,16 +69,14 @@ private:
                                   void *client_data);
   static void ControlFanCb(livox_status status, uint8_t handle,
                            uint8_t response, void *clent_data);
-  static void
-  HubSetPointCloudReturnModeCb(livox_status status, uint8_t handle,
-                               HubSetPointCloudReturnModeResponse *response,
-                               void *clent_data);
+  static void HubSetPointCloudReturnModeCb(
+      livox_status status, uint8_t handle,
+      HubSetPointCloudReturnModeResponse *response, void *clent_data);
   static void SetCoordinateCb(livox_status status, uint8_t handle,
                               uint8_t response, void *clent_data);
-  static void
-  HubSetImuRatePushFrequencyCb(livox_status status, uint8_t handle,
-                               HubSetImuPushFrequencyResponse *response,
-                               void *clent_data);
+  static void HubSetImuRatePushFrequencyCb(
+      livox_status status, uint8_t handle,
+      HubSetImuPushFrequencyResponse *response, void *clent_data);
   static void HubErrorStatusCb(livox_status status, uint8_t handle,
                                ErrorMessage *message);
   static void ConfigPointCloudReturnMode(LdsHub *lds_hub);

livox_ros_driver/livox_ros_driver/lds_lidar.cpp

@@ -27,8 +27,8 @@
 #include <stdio.h>
 #include <string.h>
 #include <memory>
-#include <thread>
 #include <mutex>
+#include <thread>
 
 #include "rapidjson/document.h"
 #include "rapidjson/filereadstream.h"
@@ -38,17 +38,13 @@ using namespace std;
 
 namespace livox_ros {
 
-/** Const varible
- * -------------------------------------------------------------------------------
- */
+/** Const varible ------------------------------------------------------------*/
 /** For callback use only */
 LdsLidar *g_lds_ldiar = nullptr;
 
-/** Global function for common use
- * ---------------------------------------------------------------*/
+/** Global function for common use -------------------------------------------*/
 
-/** Lds lidar function
- * ---------------------------------------------------------------------------*/
+/** Lds lidar function -------------------------------------------------------*/
 LdsLidar::LdsLidar(uint32_t interval_ms) : Lds(interval_ms, kSourceRawLidar) {
   auto_connect_mode_ = true;
   is_initialized_ = false;
@@ -101,7 +97,8 @@ int LdsLidar::InitLdsLidar(std::vector<std::string> &broadcast_code_strs,
     }
   } else {
     EnableAutoConnectMode();
-    printf("No broadcast code was added to whitelist, swith to automatic "
+    printf(
+        "No broadcast code was added to whitelist, swith to automatic "
         "connection mode!\n");
   }
 
@@ -138,7 +135,6 @@ int LdsLidar::InitLdsLidar(std::vector<std::string> &broadcast_code_strs,
 }
 
 int LdsLidar::DeInitLdsLidar(void) {
-
   if (!is_initialized_) {
     printf("LiDAR data source is not exit");
     return -1;
@@ -156,8 +152,7 @@ int LdsLidar::DeInitLdsLidar(void) {
 
 void LdsLidar::PrepareExit(void) { DeInitLdsLidar(); }
 
-/** Static function in LdsLidar for callback or event process
- * ------------------------------------*/
+/** Static function in LdsLidar for callback or event process ----------------*/
 
 /** Receiving point cloud data from Livox LiDAR. */
 void LdsLidar::OnLidarDataCb(uint8_t handle, LivoxEthPacket *data,
@@ -171,76 +166,7 @@ void LdsLidar::OnLidarDataCb(uint8_t handle, LivoxEthPacket *data,
     return;
   }
 
-  LidarDevice *p_lidar = &lds_lidar->lidars_[handle];
-  LidarPacketStatistic *packet_statistic = &p_lidar->statistic_info;
-  LdsStamp cur_timestamp;
-  memcpy(cur_timestamp.stamp_bytes, eth_packet->timestamp,
-         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
-
-        auto cur_time = std::chrono::high_resolution_clock::now();
-        int64_t sync_time = cur_time.time_since_epoch().count();
-
-        packet_statistic->timebase = sync_time; // used receive time as timebase
-      }
-    }
-    packet_statistic->last_timestamp = cur_timestamp.stamp;
-
-    LidarDataQueue *p_queue = &p_lidar->data;
-    if (nullptr == p_queue->storage_packet) {
-      uint32_t queue_size = CalculatePacketQueueSize(lds_lidar->buffer_time_ms_,
-                                                     eth_packet->data_type);
-      queue_size = queue_size * 16; /* 16 multiple the min size */
-      InitQueue(p_queue, queue_size);
-      printf("Lidar%02d[%s] queue size : %d %d\n", p_lidar->handle,
-             p_lidar->info.broadcast_code, queue_size, p_queue->size);
-    }
-
-    if (!QueueIsFull(p_queue)) {
-      QueuePushAny(p_queue, (uint8_t *)eth_packet,
-                   GetEthPacketLen(eth_packet->data_type),
-                   packet_statistic->timebase,
-                   GetPointsPerPacket(eth_packet->data_type));
-    }
-  } else {
-    if (eth_packet->timestamp_type == kTimestampTypePps) {
-      if ((cur_timestamp.stamp < packet_statistic->last_imu_timestamp) &&
-          (cur_timestamp.stamp < kPacketTimeGap)) { // whether a new sync frame
-
-        auto cur_time = std::chrono::high_resolution_clock::now();
-        int64_t sync_time = cur_time.time_since_epoch().count();
-
-        packet_statistic->imu_timebase =
-            sync_time; // used receive time as timebase
-      }
-    }
-    packet_statistic->last_imu_timestamp = cur_timestamp.stamp;
-
-    LidarDataQueue *p_queue = &p_lidar->imu_data;
-    if (nullptr == p_queue->storage_packet) {
-      uint32_t queue_size = 256;
-      InitQueue(p_queue, queue_size);
-      printf("Lidar%02d[%s] imu queue size : %d %d\n", p_lidar->handle,
-             p_lidar->info.broadcast_code, queue_size, p_queue->size);
-    }
-
-    if (!QueueIsFull(p_queue)) {
-      QueuePushAny(p_queue, (uint8_t *)eth_packet,
-                   GetEthPacketLen(eth_packet->data_type),
-                   packet_statistic->timebase,
-                   GetPointsPerPacket(eth_packet->data_type));
-    }
-  }
+  lds_lidar->StorageRawPacket(handle, eth_packet);
 }
 
 void LdsLidar::OnDeviceBroadcast(const BroadcastDeviceInfo *info) {
@@ -292,8 +218,7 @@ 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;
+    p_lidar->config.enable_high_sensitivity = config.enable_high_sensitivity;
   } else {
     printf("Add lidar to connect is failed : %d %d \n", result, handle);
   }
@@ -350,7 +275,8 @@ void LdsLidar::OnDeviceChange(const DeviceInfo *info, DeviceEvent type) {
         p_lidar->config.set_bits |= kConfigReturnMode;
       }
 
-      if (kDeviceTypeLidarMid40 != info->type) {
+      if ((kDeviceTypeLidarMid70 != info->type) &&
+          (kDeviceTypeLidarMid40 != info->type)) {
         LidarSetImuPushFrequency(handle, (ImuFreq)(p_lidar->config.imu_rate),
                                  SetImuRatePushFrequencyCb, g_lds_ldiar);
         p_lidar->config.set_bits |= kConfigImuRate;
@@ -365,8 +291,7 @@ void LdsLidar::OnDeviceChange(const DeviceInfo *info, DeviceEvent type) {
 
       if (kDeviceTypeLidarTele == info->type) {
         if (p_lidar->config.enable_high_sensitivity) {
-          LidarEnableHighSensitivity(handle, SetHighSensitivityCb,
-                                     g_lds_ldiar);
+          LidarEnableHighSensitivity(handle, SetHighSensitivityCb, g_lds_ldiar);
           printf("Enable high sensitivity\n");
         } else {
           LidarDisableHighSensitivity(handle, SetHighSensitivityCb,
@@ -544,7 +469,8 @@ void LdsLidar::GetLidarExtrinsicParameterCb(
 }
 
 void LdsLidar::SetHighSensitivityCb(livox_status status, uint8_t handle,
-    DeviceParameterResponse *response, void *clent_data) {
+                                    DeviceParameterResponse *response,
+                                    void *clent_data) {
   LdsLidar *lds_lidar = static_cast<LdsLidar *>(clent_data);
 
   if (handle >= kMaxLidarCount) {
@@ -563,11 +489,9 @@ void LdsLidar::SetHighSensitivityCb(livox_status status, uint8_t handle,
     };
   } else {
     if (p_lidar->config.enable_high_sensitivity) {
-      LidarEnableHighSensitivity(handle, SetHighSensitivityCb,
-                                 g_lds_ldiar);
+      LidarEnableHighSensitivity(handle, SetHighSensitivityCb, g_lds_ldiar);
     } else {
-      LidarDisableHighSensitivity(handle, SetHighSensitivityCb,
-                                  g_lds_ldiar);
+      LidarDisableHighSensitivity(handle, SetHighSensitivityCb, g_lds_ldiar);
     }
     printf("Set high sensitivity fail, try again!\n");
   }
@@ -668,8 +592,7 @@ int LdsLidar::ParseTimesyncConfig(rapidjson::Document &doc) {
       break;
 
     const rapidjson::Value &object = doc["timesync_config"];
-    if (!object.IsObject())
-      break;
+    if (!object.IsObject()) break;
 
     if (!object.HasMember("enable_timesync") ||
         !object["enable_timesync"].IsBool())

livox_ros_driver/livox_ros_driver/lds_lidar.h

@@ -28,8 +28,8 @@
 #define LIVOX_ROS_DRIVER_LDS_LIDAR_H_
 
 #include <memory>
-#include <vector>
 #include <mutex>
+#include <vector>
 
 #include "lds.h"
 #include "livox_sdk.h"
@@ -84,12 +84,12 @@ private:
                                uint8_t response, void *client_data);
   static void ReceiveSyncTimeCallback(const char *rmc, uint32_t rmc_length,
                                       void *client_data);
-  static void
-  GetLidarExtrinsicParameterCb(livox_status status, uint8_t handle,
-                               LidarGetExtrinsicParameterResponse *response,
-                               void *clent_data);
+  static void 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);
+                                   DeviceParameterResponse *response,
+                                   void *clent_data);
 
   void ResetLdsLidar(void);
   int AddBroadcastCodeToWhitelist(const char *broadcast_code);

livox_ros_driver/livox_ros_driver/lds_lvx.cpp

@@ -24,23 +24,21 @@
 
 #include "lds_lvx.h"
 
+#include <stdio.h>
+#include <unistd.h>
+#include <string.h>
 #include <functional>
 #include <memory>
-#include <stdio.h>
-#include <string.h>
 #include <thread>
 
 #include "lvx_file.h"
 
 namespace livox_ros {
 
-/** Const varible
- * --------------------------------------------------------------------------------
- */
+/** Const varible ------------------------------------------------------------*/
 const uint32_t kMaxPacketsNumOfFrame = 8192;
 
-/** For device connect use
- * ---------------------------------------------------------------------- */
+/** For device connect use ---------------------------------------------------*/
 LdsLvx::LdsLvx(uint32_t interval_ms) : Lds(interval_ms, kSourceLvxFile) {
   start_read_lvx_ = false;
   is_initialized_ = false;
@@ -62,7 +60,7 @@ LdsLvx::~LdsLvx() {
 
 void LdsLvx::PrepareExit(void) {
   lvx_file_->CloseLvxFile();
-  printf("Lvx to rosbag convert complete and exit!\n");
+  printf("Convert complete, Press [Ctrl+C] to exit!\n");
 }
 
 int LdsLvx::InitLdsLvx(const char *lvx_path) {
@@ -83,31 +81,37 @@ int LdsLvx::InitLdsLvx(const char *lvx_path) {
     ResetLds(kSourceLvxFile);
   }
 
-  lidar_count_ = lvx_file_->GetDeviceCount();
-  if (!lidar_count_ || (lidar_count_ >= kMaxSourceLidar)) {
+  uint32_t valid_lidar_count_ = lvx_file_->GetDeviceCount();
+  if (!valid_lidar_count_ || (valid_lidar_count_ >= kMaxSourceLidar)) {
     lvx_file_->CloseLvxFile();
-    printf("Lidar count error in %s : %d\n", lvx_path, lidar_count_);
+    printf("Lidar count error in %s : %d\n", lvx_path, valid_lidar_count_);
     return -1;
   }
-  printf("LvxFile[%s] have %d lidars\n", lvx_path, lidar_count_);
+  printf("LvxFile[%s] have %d lidars\n", lvx_path, valid_lidar_count_);
 
-  for (int i = 0; i < lidar_count_; i++) {
+  for (uint32_t i = 0; i < valid_lidar_count_; i++) {
     LvxFileDeviceInfo lvx_dev_info;
     lvx_file_->GetDeviceInfo(i, &lvx_dev_info);
-    lidars_[i].handle = i;
-    lidars_[i].connect_state = kConnectStateSampling;
-    lidars_[i].info.handle = i;
-    lidars_[i].info.type = lvx_dev_info.device_type;
-    memcpy(lidars_[i].info.broadcast_code, lvx_dev_info.lidar_broadcast_code,
-           sizeof(lidars_[i].info.broadcast_code));
+    uint8_t handle = lvx_dev_info.device_index;
+    if (handle >= kMaxSourceLidar) {
+      printf("Invalid hanle from lvx file!\n");
+      continue;
+    }
+    lidars_[handle].handle        = handle;
+    lidars_[handle].connect_state = kConnectStateSampling;
+    lidars_[handle].info.handle = handle;
+    lidars_[handle].info.type   = lvx_dev_info.device_type;
+    memcpy(lidars_[handle].info.broadcast_code, \
+           lvx_dev_info.lidar_broadcast_code, \
+           sizeof(lidars_[handle].info.broadcast_code));
 
     if (lvx_file_->GetFileVersion() == kLvxFileV1) {
-      lidars_[i].data_src = kSourceRawLidar;
+      lidars_[handle].data_src = kSourceRawLidar;
     } else {
-      lidars_[i].data_src = kSourceLvxFile;
+      lidars_[handle].data_src = kSourceLvxFile;
     }
 
-    ExtrinsicParameter *p_extrinsic = &lidars_[i].extrinsic_parameter;
+    ExtrinsicParameter *p_extrinsic = &lidars_[handle].extrinsic_parameter;
     p_extrinsic->euler[0] = lvx_dev_info.roll * PI / 180.0;
     p_extrinsic->euler[1] = lvx_dev_info.pitch * PI / 180.0;
     p_extrinsic->euler[2] = lvx_dev_info.yaw * PI / 180.0;
@@ -118,9 +122,9 @@ int LdsLvx::InitLdsLvx(const char *lvx_path) {
     p_extrinsic->enable = lvx_dev_info.extrinsic_enable;
 
     uint32_t queue_size = kMaxEthPacketQueueSize * 16;
-    InitQueue(&lidars_[i].data, queue_size);
+    InitQueue(&lidars_[handle].data, queue_size);
     queue_size = kMaxEthPacketQueueSize;
-    InitQueue(&lidars_[i].imu_data, queue_size);
+    InitQueue(&lidars_[handle].imu_data, queue_size);
   }
 
   t_read_lvx_ =
@@ -134,8 +138,7 @@ int LdsLvx::InitLdsLvx(const char *lvx_path) {
 
 /** Global function in LdsLvx for callback */
 void LdsLvx::ReadLvxFile() {
-  while (!start_read_lvx_)
-    ;
+  while (!start_read_lvx_);
   printf("Start to read lvx file.\n");
 
   int file_state = kLvxFileOk;
@@ -165,32 +168,19 @@ void LdsLvx::ReadLvxFile() {
         data_type = eth_packet->data_type;
         /** Packet length + device index */
         data_offset += (GetEthPacketLen(data_type) + 1);
-        if (data_type != kImu) {
-          LidarDevice *p_lidar = &lidars_[handle];
+        StorageRawPacket(handle, eth_packet);
+
         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_)) {
+        if (p_queue != nullptr) {
           while (QueueIsFull(p_queue)) {
             std::this_thread::sleep_for(std::chrono::milliseconds(1));
           }
-            QueuePushAny(p_queue, (uint8_t *)eth_packet,
-                         GetEthPacketLen(data_type), 0,
-                         GetPointsPerPacket(data_type));
         }
-        } else {
-          LidarDataQueue *p_queue = &lidars_[handle].imu_data;
-          if ((p_queue != nullptr) && (handle < lidar_count_)) {
+        p_queue = &lidars_[handle].imu_data;
+        if (p_queue != nullptr) {
           while (QueueIsFull(p_queue)) {
             std::this_thread::sleep_for(std::chrono::milliseconds(1));
           }
-            QueuePushAny(p_queue, (uint8_t *)eth_packet,
-                         GetEthPacketLen(data_type), 0,
-                         GetPointsPerPacket(data_type));
-          }
         }
       }
     } else {
@@ -207,10 +197,13 @@ void LdsLvx::ReadLvxFile() {
       printf("Read progress : %d \n", progress);
     }
   }
-
-  int32_t wait_cnt = 10;
+  printf("Wait for file conversion to complete!\n");
+  int32_t wait_cnt = 5;
   while (!IsAllQueueEmpty()) {
-    std::this_thread::sleep_for(std::chrono::milliseconds(50));
+    std::this_thread::sleep_for(std::chrono::milliseconds(40));
+    if (semaphore_.GetCount() <= 0) {
+      semaphore_.Signal();
+    }
     if (IsAllQueueReadStop()) {
       --wait_cnt;
       if (wait_cnt <= 0) {
@@ -218,31 +211,12 @@ void LdsLvx::ReadLvxFile() {
       }
     }
   }
+
   RequestExit();
+  while(semaphore_.GetCount() > 0) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(10));
   }
-
-bool LdsLvx::IsAllQueueEmpty() {
-  for (int i = 0; i < lidar_count_; i++) {
-    LidarDevice *p_lidar = &lidars_[i];
-    if (!QueueIsEmpty(&p_lidar->data)) {
-      return false;
-    }
-  }
-
-  return true;
-}
-
-bool LdsLvx::IsAllQueueReadStop() {
-  static uint32_t remain_size[kMaxSourceLidar];
-  for (int i = 0; i < lidar_count_; i++) {
-    LidarDevice *p_lidar = &lidars_[i];
-    if (remain_size[i] != QueueIsEmpty(&p_lidar->data)) {
-      remain_size[i] = QueueIsEmpty(&p_lidar->data);
-      return false;
-    }
-  }
-
-  return true;
+  semaphore_.Signal();
 }
 
 }  // namespace livox_ros

livox_ros_driver/livox_ros_driver/lds_lvx.h

@@ -60,8 +60,6 @@ private:
   bool IsStarted() { return start_read_lvx_; }
 
   void ReadLvxFile();
-  bool IsAllQueueEmpty();
-  bool IsAllQueueReadStop();
 
   volatile bool is_initialized_;
   OutPacketBuffer packets_of_frame_;

livox_ros_driver/livox_ros_driver/livox_ros_driver.cpp

@@ -27,19 +27,18 @@
 #include <chrono>
 #include <vector>
 
+#include <ros/ros.h>
 #include "lddc.h"
 #include "lds_hub.h"
 #include "lds_lidar.h"
 #include "lds_lvx.h"
 #include "livox_sdk.h"
-#include <ros/ros.h>
 
 using namespace livox_ros;
 
 const int32_t kSdkVersionMajorLimit = 2;
 
 int main(int argc, char **argv) {
-
   ROS_INFO("Livox Ros Driver Version: %s", LIVOX_ROS_DRIVER_VERSION_STRING);
 
   /** Ros related */
@@ -59,7 +58,7 @@ int main(int argc, char **argv) {
     return 0;
   }
 
-  /** Init defualt system parameter */
+  /** Init default system parameter */
   int xfer_format = kPointCloud2Msg;
   int multi_topic = 0;
   int data_src = kSourceRawLidar;
@@ -82,7 +81,7 @@ int main(int argc, char **argv) {
   }
 
   /** Lidar data distribute control and lidar data source set */
-  Lddc *lddc = new Lddc(xfer_format, multi_topic, data_src, output_type, \
+  Lddc *lddc = new Lddc(xfer_format, multi_topic, data_src, output_type,
                         publish_freq, frame_id);
   lddc->SetRosNode(&livox_node);
 
@@ -159,14 +158,8 @@ int main(int argc, char **argv) {
   }
 
   ros::Time::init();
-  double poll_freq = publish_freq * 4;
-  if (data_src == kSourceLvxFile) {
-    poll_freq = 2000;
-  }
-  ros::Rate r(poll_freq);
   while (ros::ok()) {
     lddc->DistributeLidarData();
-    r.sleep();
   }
 
   return 0;

livox_ros_driver/livox_ros_driver/lvx_file.cpp

@@ -23,9 +23,9 @@
 //
 
 #include "lvx_file.h"
-#include <cmath>
 #include <string.h>
 #include <time.h>
+#include <cmath>
 
 #include "lds.h"
 #include "rapidxml/rapidxml.hpp"
@@ -40,8 +40,14 @@ const char *kLvxHeaderSigStr = "livox_tech";
 const uint32_t kLvxHeaderMagicCode = 0xac0ea767;
 
 LvxFileHandle::LvxFileHandle()
-    : file_ver_(kLvxFileV1), device_count_(0), cur_frame_index_(0),
-      cur_offset_(0), data_start_offset_(0), size_(0), mode_(0), state_(0) {
+    : file_ver_(kLvxFileV1),
+      device_count_(0),
+      cur_frame_index_(0),
+      cur_offset_(0),
+      data_start_offset_(0),
+      size_(0),
+      mode_(0),
+      state_(0) {
   memset((void *)&public_header_, 0, sizeof(public_header_));
   memset((void *)&private_header_, 0, sizeof(private_header_));
   memset((void *)&private_header_v0_, 0, sizeof(private_header_v0_));
@@ -151,7 +157,6 @@ bool LvxFileHandle::PrepareDataRead() {
 }
 
 int LvxFileHandle::Open(const char *filename, std::ios_base::openmode mode) {
-
   if ((mode & std::ios::in) == std::ios::in) {
     state_ = kLvxFileOk;
     lvx_file_.open(filename, mode | std::ios_base::binary | std::ios_base::ate);
@@ -298,8 +303,7 @@ void LvxFileHandle::SaveFrameToLvxFile(
 }
 
 void LvxFileHandle::CloseLvxFile() {
-  if (lvx_file_ && lvx_file_.is_open())
-    lvx_file_.close();
+  if (lvx_file_ && lvx_file_.is_open()) lvx_file_.close();
 }
 
 void LvxFileHandle::BasePointsHandle(LivoxEthPacket *data,

livox_ros_driver/livox_ros_driver/lvx_file.h

@@ -24,13 +24,13 @@
 #ifndef LIVOX_FILE_H_
 #define LIVOX_FILE_H_
 
-#include "livox_sdk.h"
 #include <fstream>
 #include <ios>
 #include <list>
 #include <memory>
 #include <mutex>
 #include <vector>
+#include "livox_sdk.h"
 
 namespace livox_ros {
 
@@ -121,9 +121,7 @@ typedef struct {
   LvxFilePacket *packet;
 } LvxFileFrame;
 
-typedef struct {
-  uint8_t device_count;
-} LvxFilePrivateHeaderV0;
+typedef struct { uint8_t device_count; } LvxFilePrivateHeaderV0;
 
 typedef struct {
   uint8_t lidar_broadcast_code[16];

livox_ros_driver/timesync/timesync.cpp

@@ -24,17 +24,19 @@
 
 #include "timesync.h"
 
-#include <chrono>
-#include <functional>
 #include <stdint.h>
 #include <string.h>
+#include <chrono>
+#include <functional>
 #include <thread>
 
 namespace livox_ros {
 using namespace std;
 
 TimeSync::TimeSync()
-    : exit_poll_state_(false), start_poll_state_(false), exit_poll_data_(false),
+    : exit_poll_state_(false),
+      start_poll_state_(false),
+      exit_poll_data_(false),
       start_poll_data_(false) {
   fsm_state_ = kOpenDev;
   uart_ = nullptr;
@@ -78,10 +80,8 @@ int32_t TimeSync::InitTimeSync(const TimeSyncConfig &config) {
 int32_t TimeSync::DeInitTimeSync() {
   StopTimesync();
 
-  if (uart_)
-    delete uart_;
-  if (comm_)
-    delete comm_;
+  if (uart_) delete uart_;
+  if (comm_) delete comm_;
 
   fn_cb_ = nullptr;
   client_data_ = nullptr;

livox_ros_driver/timesync/timesync.h

@@ -25,10 +25,10 @@
 #ifndef TIMESYNC_TIMESYNC_H_
 #define TIMESYNC_TIMESYNC_H_
 
+#include <thread>
 #include "comm_device.h"
 #include "comm_protocol.h"
 #include "user_uart.h"
-#include <thread>
 
 namespace livox_ros {
 

livox_ros_driver/timesync/user_uart/user_uart.cpp

@@ -72,7 +72,6 @@ int UserUart::Open(const char *filename) {
 }
 
 int UserUart::Close() {
-
   is_open_ = false;
   if (fd_ > 0) {
     /** first we flush the port */

livox_ros_driver/timesync/user_uart/user_uart.h

@@ -66,7 +66,6 @@ enum BaudRate {
 };
 
 class UserUart {
-
  public:
   UserUart(uint8_t baudrate_index, uint8_t parity);
   ~UserUart();