feat:support AVIA and Mid-70

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,14 +181,12 @@ 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               |
+| 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               |
 | extrinsic_parameter_source | Int     | Whether to enable extrinsic parameter automatic compensation<br>0 -- Disable automatic compensation of LiDAR external reference<br>1 -- Automatic compensation of LiDAR external reference | 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/common/FastCRC/FastCRC.h

@@ -49,32 +49,32 @@
 // ================= 16-BIT CRC ===================
 
 class FastCRC16 {
-public:
+ public:
   FastCRC16(uint16_t seed);
 
   // change function name from mcrf4xx_upd to mcrf4xx
-  uint16_t
+  uint16_t mcrf4xx_calc(
-  mcrf4xx_calc(const uint8_t *data,
+      const uint8_t *data,
-               const uint16_t datalen); // Equivalent to _crc_ccitt_update() in
+      const uint16_t datalen);  // Equivalent to _crc_ccitt_update() in
-                                        // crc16.h from avr_libc
+                                // crc16.h from avr_libc
 
-private:
+ private:
   uint16_t seed_;
 };
 
 // ================= 32-BIT CRC ===================
 
 class FastCRC32 {
-public:
+ public:
   FastCRC32(uint32_t seed);
 
   // change function name from crc32_upd to crc32
   uint32_t crc32_calc(
       const uint8_t *data,
-      uint16_t len); // Call for subsequent calculations with previous seed
+      uint16_t len);  // Call for subsequent calculations with previous seed
 
-private:
+ private:
   uint32_t seed_;
 };
 
-#endif // FASTCRC_FASTCRC_H_
+#endif  // FASTCRC_FASTCRC_H_

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) {
@@ -90,11 +89,11 @@ FastCRC32::FastCRC32(uint32_t seed) { seed_ = seed; }
   crc = (table[(crc & 0xff) + 0x300]) ^ (table[((crc >> 8) & 0xff) + 0x200]) ^ \
         (table[((crc >> 16) & 0xff) + 0x100]) ^ (table[(crc >> 24) & 0xff]);
 
-#define crcsm_n4d(crc, data, table)                                            \
+#define crcsm_n4d(crc, data, table)       \
-  crc ^= data;                                                                 \
+  crc ^= data;                            \
-  crc = (crc >> 8) ^ (table[crc & 0xff]);                                      \
+  crc = (crc >> 8) ^ (table[crc & 0xff]); \
-  crc = (crc >> 8) ^ (table[crc & 0xff]);                                      \
+  crc = (crc >> 8) ^ (table[crc & 0xff]); \
-  crc = (crc >> 8) ^ (table[crc & 0xff]);                                      \
+  crc = (crc >> 8) ^ (table[crc & 0xff]); \
   crc = (crc >> 8) ^ (table[crc & 0xff]);
 
 /** CRC32
@@ -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_device.h

@@ -67,5 +67,5 @@ typedef struct {
   } config;
 } CommDevConfig;
 
-} // namespace livox_ros
+}  // namespace livox_ros
-#endif // COMM_COMM_DEVICE_H_
+#endif  // COMM_COMM_DEVICE_H_

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 {
 
@@ -116,19 +116,19 @@ int32_t CommProtocol::ParseCommStream(CommPacket *o_pack) {
   while ((GetValidDataSize() > protocol_->GetPreambleLen()) &&
          (GetValidDataSize() > offset_to_read_index_)) {
     switch (fsm_parse_step_) {
-    case kSearchPacketPreamble: {
+      case kSearchPacketPreamble: {
-      FsmSearchPacketPreamble();
+        FsmSearchPacketPreamble();
-      break;
+        break;
-    }
+      }
-    case kFindPacketLength: {
+      case kFindPacketLength: {
-      FsmFindPacketLength();
+        FsmFindPacketLength();
-      break;
+        break;
-    }
+      }
-    case kGetPacketData: {
+      case kGetPacketData: {
-      ret = FsmGetPacketData(o_pack);
+        ret = FsmGetPacketData(o_pack);
-      break;
+        break;
-    }
+      }
-    default: { FsmParserStateTransfer(kSearchPacketPreamble); }
+      default: { FsmParserStateTransfer(kSearchPacketPreamble); }
     }
 
     /* Must exit when in the below case */
@@ -221,4 +221,4 @@ int32_t CommProtocol::FsmGetPacketData(CommPacket *o_pack) {
   return ret;
 }
 
-} // namespace livox_ros
+}  // 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;
@@ -50,7 +50,7 @@ typedef struct {
 } CommCache;
 
 class CommProtocol {
-public:
+ public:
   CommProtocol(ProtocolConfig &config);
   ~CommProtocol();
 
@@ -67,7 +67,7 @@ public:
 
   void ResetParser();
 
-private:
+ private:
   uint32_t GetCacheTailSize();
   uint32_t GetValidDataSize();
   void UpdateCache(void);
@@ -101,5 +101,5 @@ private:
   }
 };
 
-} // namespace livox_ros
+}  // namespace livox_ros
-#endif // COMM_COMM_PROTOCOL_H_
+#endif  // COMM_COMM_PROTOCOL_H_

livox_ros_driver/common/comm/gps_protocol.cpp

@@ -49,7 +49,7 @@ int32_t GpsProtocol::ParsePacket(const uint8_t *i_buf, uint32_t i_len,
   // GpsPacket *gps_packet = (GpsPacket *)i_buf;
 
   if (i_len < GetPacketWrapperLen()) {
-    return -1; // packet length error
+    return -1;  // packet length error
   }
   memset((void *)o_packet, 0, sizeof(CommPacket));
   o_packet->protocol = kGps;
@@ -114,14 +114,12 @@ 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)
+  if (h > 9) h -= 7;
-    h -= 7;
 
   uint8_t l = toupper(TwoChar[1]) - 0x30;
-  if (l > 9)
+  if (l > 9) l -= 7;
-    l -= 7;
 
   return h * 16 + l;
 }
 
-} // namespace livox_ros
+}  // namespace livox_ros

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 {
 
@@ -47,7 +47,7 @@ typedef struct {
 uint8_t AscciiToHex(const uint8_t *TwoChar);
 
 class GpsProtocol : public Protocol {
-public:
+ public:
   GpsProtocol();
   ~GpsProtocol() = default;
 
@@ -69,11 +69,11 @@ public:
 
   int32_t CheckPacket(const uint8_t *buf) override;
 
-private:
+ private:
   uint32_t found_length_;
 
   uint8_t CalcGpsPacketChecksum(const uint8_t *buf, uint32_t length);
 };
 
-} // namespace livox_ros
+}  // namespace livox_ros
-#endif // LIVOX_GPS_PROTOCOL_H_
+#endif  // LIVOX_GPS_PROTOCOL_H_

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 {
+typedef struct { void *data; } GpsProtocolConfig;
-  void *data;
-} GpsProtocolConfig;
 
 typedef struct {
   uint8_t type;
@@ -85,7 +80,7 @@ typedef struct {
 } ProtocolConfig;
 
 class Protocol {
-public:
+ public:
   virtual ~Protocol() = default;
 
   virtual int32_t ParsePacket(const uint8_t *i_buf, uint32_t i_len,
@@ -107,5 +102,5 @@ public:
   virtual int32_t CheckPacket(const uint8_t *buf) = 0;
 };
 
-} // namespace livox_ros
+}  // namespace livox_ros
-#endif // COMM_PROTOCOL_H_
+#endif  // COMM_PROTOCOL_H_

livox_ros_driver/common/comm/sdk_protocol.cpp

@@ -29,8 +29,8 @@
 
 namespace livox_ros {
 const uint8_t kSdkProtocolSof = 0xAA;
-const uint32_t kSdkPacketCrcSize = 4;         // crc32
+const uint32_t kSdkPacketCrcSize = 4;          // crc32
-const uint32_t kSdkPacketPreambleCrcSize = 2; // crc16
+const uint32_t kSdkPacketPreambleCrcSize = 2;  // crc16
 
 SdkProtocol::SdkProtocol(uint16_t seed16, uint32_t seed32)
     : crc16_(seed16), crc32_(seed32) {}
@@ -77,7 +77,7 @@ int32_t SdkProtocol::ParsePacket(const uint8_t *i_buf, uint32_t i_len,
   SdkPacket *sdk_packet = (SdkPacket *)i_buf;
 
   if (i_len < GetPacketWrapperLen()) {
-    return -1; // packet lenth error
+    return -1;  // packet lenth error
   }
 
   memset((void *)o_packet, 0, sizeof(CommPacket));
@@ -131,4 +131,4 @@ int32_t SdkProtocol::CheckPacket(const uint8_t *buf) {
     return -1;
   }
 }
-} // namespace livox_ros
+}  // namespace livox_ros

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;
@@ -58,7 +58,7 @@ typedef struct {
 #pragma pack()
 
 class SdkProtocol : public Protocol {
-public:
+ public:
   SdkProtocol(uint16_t seed16, uint32_t seed32);
   ~SdkProtocol() = default;
 
@@ -78,9 +78,9 @@ public:
 
   int32_t CheckPacket(const uint8_t *buf) override;
 
-private:
+ private:
   FastCRC16 crc16_;
   FastCRC32 crc32_;
 };
-} // namespace livox_ros
+}  // namespace livox_ros
-#endif // LIVOX_SDK_PROTOCOL_H_
+#endif  // LIVOX_SDK_PROTOCOL_H_

livox_ros_driver/common/rapidjson/allocators.h

@@ -78,13 +78,13 @@ allocator may not book-keep this, explicitly pass to it can save memory.)
     \note implements Allocator concept
 */
 class CrtAllocator {
-public:
+ public:
   static const bool kNeedFree = true;
   void *Malloc(size_t size) {
-    if (size) //  behavior of malloc(0) is implementation defined.
+    if (size)  //  behavior of malloc(0) is implementation defined.
       return std::malloc(size);
     else
-      return NULL; // standardize to returning NULL.
+      return NULL;  // standardize to returning NULL.
   }
   void *Realloc(void *originalPtr, size_t originalSize, size_t newSize) {
     (void)originalSize;
@@ -118,11 +118,12 @@ 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>
-public:
+class MemoryPoolAllocator {
+ public:
   static const bool kNeedFree =
-      false; //!< Tell users that no need to call Free() with this allocator.
+      false;  //!< Tell users that no need to call Free() with this allocator.
-             //!< (concept Allocator)
+              //!< (concept Allocator)
 
   //! Constructor with chunkSize.
   /*! \param chunkSize The size of memory chunk. The default is
@@ -131,8 +132,11 @@ public:
   */
   MemoryPoolAllocator(size_t chunkSize = kDefaultChunkCapacity,
                       BaseAllocator *baseAllocator = 0)
-      : chunkHead_(0), chunk_capacity_(chunkSize), userBuffer_(0),
+      : chunkHead_(0),
-        baseAllocator_(baseAllocator), ownBaseAllocator_(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),
+      : chunkHead_(0),
-        baseAllocator_(baseAllocator), ownBaseAllocator_(0) {
+        chunk_capacity_(chunkSize),
+        userBuffer_(buffer),
+        baseAllocator_(baseAllocator),
+        ownBaseAllocator_(0) {
     RAPIDJSON_ASSERT(buffer != 0);
     RAPIDJSON_ASSERT(size > sizeof(ChunkHeader));
     chunkHead_ = reinterpret_cast<ChunkHeader *>(buffer);
@@ -175,7 +182,7 @@ public:
       chunkHead_ = next;
     }
     if (chunkHead_ && chunkHead_ == userBuffer_)
-      chunkHead_->size = 0; // Clear user buffer
+      chunkHead_->size = 0;  // Clear user buffer
   }
 
   //! Computes the total capacity of allocated memory chunks.
@@ -193,15 +200,13 @@ public:
    */
   size_t Size() const {
     size_t size = 0;
-    for (ChunkHeader *c = chunkHead_; c != 0; c = c->next)
+    for (ChunkHeader *c = chunkHead_; c != 0; c = c->next) size += c->size;
-      size += c->size;
     return size;
   }
 
   //! Allocates a memory block. (concept Allocator)
   void *Malloc(size_t size) {
-    if (!size)
+    if (!size) return NULL;
-      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)
+    if (originalPtr == 0) return Malloc(newSize);
-      return Malloc(newSize);
 
-    if (newSize == 0)
+    if (newSize == 0) return NULL;
-      return NULL;
 
     originalSize = RAPIDJSON_ALIGN(originalSize);
     newSize = RAPIDJSON_ALIGN(newSize);
 
     // Do not shrink if new size is smaller than original
-    if (originalSize >= newSize)
+    if (originalSize >= newSize) return originalPtr;
-      return originalPtr;
 
     // Simply expand it if it is the last allocation and there is sufficient
     // space
-    if (originalPtr == reinterpret_cast<char *>(chunkHead_) +
+    if (originalPtr ==
-                           RAPIDJSON_ALIGN(sizeof(ChunkHeader)) +
+        reinterpret_cast<char *>(chunkHead_) +
-                           chunkHead_->size - originalSize) {
+            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,17 +246,16 @@ public:
 
     // Realloc process: allocate and copy memory, do not free original buffer.
     if (void *newBuffer = Malloc(newSize)) {
-      if (originalSize)
+      if (originalSize) std::memcpy(newBuffer, originalPtr, originalSize);
-        std::memcpy(newBuffer, originalPtr, originalSize);
       return newBuffer;
     } else
       return NULL;
   }
 
   //! Frees a memory block (concept Allocator)
-  static void Free(void *ptr) { (void)ptr; } // Do nothing
+  static void Free(void *ptr) { (void)ptr; }  // Do nothing
 
-private:
+ private:
   //! Copy constructor is not permitted.
   MemoryPoolAllocator(const MemoryPoolAllocator &rhs) /* = delete */;
   //! Copy assignment operator is not permitted.
@@ -279,28 +281,28 @@ private:
   }
 
   static const int kDefaultChunkCapacity =
-      RAPIDJSON_ALLOCATOR_DEFAULT_CHUNK_CAPACITY; //!< Default chunk capacity.
+      RAPIDJSON_ALLOCATOR_DEFAULT_CHUNK_CAPACITY;  //!< Default chunk capacity.
 
   //! Chunk header for perpending to each chunk.
   /*! Chunks are stored as a singly linked list.
    */
   struct ChunkHeader {
-    size_t capacity;   //!< Capacity of the chunk in bytes (excluding the header
+    size_t capacity;  //!< Capacity of the chunk in bytes (excluding the header
-                       //!< itself).
+                      //!< itself).
-    size_t size;       //!< Current size of allocated memory in bytes.
+    size_t size;      //!< Current size of allocated memory in bytes.
-    ChunkHeader *next; //!< Next chunk in the linked list.
+    ChunkHeader *next;  //!< Next chunk in the linked list.
   };
 
-  ChunkHeader *chunkHead_; //!< Head of the chunk linked-list. Only the head
+  ChunkHeader *chunkHead_;  //!< Head of the chunk linked-list. Only the head
-                           //!< chunk serves allocation.
+                            //!< chunk serves allocation.
-  size_t chunk_capacity_;  //!< The minimum capacity of chunk when they are
+  size_t chunk_capacity_;   //!< The minimum capacity of chunk when they are
-                           //!< allocated.
+                            //!< allocated.
-  void *userBuffer_;       //!< User supplied buffer.
+  void *userBuffer_;        //!< User supplied buffer.
   BaseAllocator
-      *baseAllocator_; //!< base allocator for allocating memory chunks.
+      *baseAllocator_;  //!< base allocator for allocating memory chunks.
-  BaseAllocator *ownBaseAllocator_; //!< base allocator created by this object.
+  BaseAllocator *ownBaseAllocator_;  //!< base allocator created by this object.
 };
 
 RAPIDJSON_NAMESPACE_END
 
-#endif // RAPIDJSON_ENCODINGS_H_
+#endif  // RAPIDJSON_ENCODINGS_H_

livox_ros_driver/common/rapidjson/cursorstreamwrapper.h

@@ -28,8 +28,8 @@ RAPIDJSON_DIAG_OFF(effc++)
 
 #if defined(_MSC_VER) && _MSC_VER <= 1800
 RAPIDJSON_DIAG_PUSH
-RAPIDJSON_DIAG_OFF(4702) // unreachable code
+RAPIDJSON_DIAG_OFF(4702)  // unreachable code
-RAPIDJSON_DIAG_OFF(4512) // assignment operator could not be generated
+RAPIDJSON_DIAG_OFF(4512)  // assignment operator could not be generated
 #endif
 
 RAPIDJSON_NAMESPACE_BEGIN
@@ -40,7 +40,7 @@ RAPIDJSON_NAMESPACE_BEGIN
 */
 template <typename InputStream, typename Encoding = UTF8<>>
 class CursorStreamWrapper : public GenericStreamWrapper<InputStream, Encoding> {
-public:
+ public:
   typedef typename Encoding::Ch Ch;
 
   CursorStreamWrapper(InputStream &is)
@@ -63,9 +63,9 @@ public:
   //! Get the error column number, if error exists.
   size_t GetColumn() const { return col_; }
 
-private:
+ private:
-  size_t line_; //!< Current Line
+  size_t line_;  //!< Current Line
-  size_t col_;  //!< Current Column
+  size_t col_;   //!< Current Column
 };
 
 #if defined(_MSC_VER) && _MSC_VER <= 1800
@@ -78,4 +78,4 @@ RAPIDJSON_DIAG_POP
 
 RAPIDJSON_NAMESPACE_END
 
-#endif // RAPIDJSON_CURSORSTREAMWRAPPER_H_
+#endif  // RAPIDJSON_CURSORSTREAMWRAPPER_H_

livox_ros_driver/common/rapidjson/encodedstream.h

@@ -44,7 +44,7 @@ template <typename Encoding, typename InputByteStream>
 class EncodedInputStream {
   RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
 
-public:
+ public:
   typedef typename Encoding::Ch Ch;
 
   EncodedInputStream(InputByteStream &is) : is_(is) {
@@ -71,7 +71,7 @@ public:
     return 0;
   }
 
-private:
+ private:
   EncodedInputStream(const EncodedInputStream &);
   EncodedInputStream &operator=(const EncodedInputStream &);
 
@@ -80,17 +80,15 @@ private:
 };
 
 //! Specialized for UTF8 MemoryStream.
-template <> class EncodedInputStream<UTF8<>, MemoryStream> {
+template <>
-public:
+class EncodedInputStream<UTF8<>, MemoryStream> {
+ public:
   typedef UTF8<>::Ch Ch;
 
   EncodedInputStream(MemoryStream &is) : is_(is) {
-    if (static_cast<unsigned char>(is_.Peek()) == 0xEFu)
+    if (static_cast<unsigned char>(is_.Peek()) == 0xEFu) is_.Take();
-      is_.Take();
+    if (static_cast<unsigned char>(is_.Peek()) == 0xBBu) is_.Take();
-    if (static_cast<unsigned char>(is_.Peek()) == 0xBBu)
+    if (static_cast<unsigned char>(is_.Peek()) == 0xBFu) is_.Take();
-      is_.Take();
-    if (static_cast<unsigned char>(is_.Peek()) == 0xBFu)
-      is_.Take();
   }
   Ch Peek() const { return is_.Peek(); }
   Ch Take() { return is_.Take(); }
@@ -104,7 +102,7 @@ public:
 
   MemoryStream &is_;
 
-private:
+ private:
   EncodedInputStream(const EncodedInputStream &);
   EncodedInputStream &operator=(const EncodedInputStream &);
 };
@@ -119,12 +117,11 @@ template <typename Encoding, typename OutputByteStream>
 class EncodedOutputStream {
   RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
 
-public:
+ public:
   typedef typename Encoding::Ch Ch;
 
   EncodedOutputStream(OutputByteStream &os, bool putBOM = true) : os_(os) {
-    if (putBOM)
+    if (putBOM) Encoding::PutBOM(os_);
-      Encoding::PutBOM(os_);
   }
 
   void Put(Ch c) { Encoding::Put(os_, c); }
@@ -152,14 +149,14 @@ public:
     return 0;
   }
 
-private:
+ private:
   EncodedOutputStream(const EncodedOutputStream &);
   EncodedOutputStream &operator=(const EncodedOutputStream &);
 
   OutputByteStream &os_;
 };
 
-#define RAPIDJSON_ENCODINGS_FUNC(x)                                            \
+#define RAPIDJSON_ENCODINGS_FUNC(x) \
   UTF8<Ch>::x, UTF16LE<Ch>::x, UTF16BE<Ch>::x, UTF32LE<Ch>::x, UTF32BE<Ch>::x
 
 //! Input stream wrapper with dynamically bound encoding and automatic encoding
@@ -172,7 +169,7 @@ template <typename CharType, typename InputByteStream>
 class AutoUTFInputStream {
   RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
 
-public:
+ public:
   typedef CharType Ch;
 
   //! Constructor.
@@ -212,7 +209,7 @@ public:
     return 0;
   }
 
-private:
+ private:
   AutoUTFInputStream(const AutoUTFInputStream &);
   AutoUTFInputStream &operator=(const AutoUTFInputStream &);
 
@@ -227,8 +224,7 @@ private:
 
     const unsigned char *c =
         reinterpret_cast<const unsigned char *>(is_->Peek4());
-    if (!c)
+    if (!c) return;
-      return;
 
     unsigned bom =
         static_cast<unsigned>(c[0] | (c[1] << 8) | (c[2] << 16) | (c[3] << 24));
@@ -280,23 +276,23 @@ private:
       int pattern =
           (c[0] ? 1 : 0) | (c[1] ? 2 : 0) | (c[2] ? 4 : 0) | (c[3] ? 8 : 0);
       switch (pattern) {
-      case 0x08:
+        case 0x08:
-        type_ = kUTF32BE;
+          type_ = kUTF32BE;
-        break;
+          break;
-      case 0x0A:
+        case 0x0A:
-        type_ = kUTF16BE;
+          type_ = kUTF16BE;
-        break;
+          break;
-      case 0x01:
+        case 0x01:
-        type_ = kUTF32LE;
+          type_ = kUTF32LE;
-        break;
+          break;
-      case 0x05:
+        case 0x05:
-        type_ = kUTF16LE;
+          type_ = kUTF16LE;
-        break;
+          break;
-      case 0x0F:
+        case 0x0F:
-        type_ = kUTF8;
+          type_ = kUTF8;
-        break;
+          break;
-      default:
+        default:
-        break; // Use type defined by user.
+          break;  // Use type defined by user.
       }
     }
 
@@ -326,7 +322,7 @@ template <typename CharType, typename OutputByteStream>
 class AutoUTFOutputStream {
   RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
 
-public:
+ public:
   typedef CharType Ch;
 
   //! Constructor.
@@ -349,8 +345,7 @@ public:
     static const PutFunc f[] = {RAPIDJSON_ENCODINGS_FUNC(Put)};
     putFunc_ = f[type_];
 
-    if (putBOM)
+    if (putBOM) PutBOM();
-      PutBOM();
   }
 
   UTFType GetType() const { return type_; }
@@ -380,7 +375,7 @@ public:
     return 0;
   }
 
-private:
+ private:
   AutoUTFOutputStream(const AutoUTFOutputStream &);
   AutoUTFOutputStream &operator=(const AutoUTFOutputStream &);
 
@@ -409,4 +404,4 @@ RAPIDJSON_DIAG_POP
 RAPIDJSON_DIAG_POP
 #endif
 
-#endif // RAPIDJSON_FILESTREAM_H_
+#endif  // RAPIDJSON_FILESTREAM_H_

livox_ros_driver/common/rapidjson/encodings.h

@@ -24,8 +24,8 @@
 #if defined(_MSC_VER) && !defined(__clang__)
 RAPIDJSON_DIAG_PUSH
 RAPIDJSON_DIAG_OFF(
-    4244) // conversion from 'type1' to 'type2', possible loss of data
+    4244)  // conversion from 'type1' to 'type2', possible loss of data
-RAPIDJSON_DIAG_OFF(4702) // unreachable code
+RAPIDJSON_DIAG_OFF(4702)  // unreachable code
 #elif defined(__GNUC__)
 RAPIDJSON_DIAG_PUSH
 RAPIDJSON_DIAG_OFF(effc++)
@@ -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 };
@@ -145,13 +146,13 @@ template <typename CharType = char> struct UTF8 {
 
   template <typename InputStream>
   static bool Decode(InputStream &is, unsigned *codepoint) {
-#define RAPIDJSON_COPY()                                                       \
+#define RAPIDJSON_COPY() \
-  c = is.Take();                                                               \
+  c = is.Take();         \
   *codepoint = (*codepoint << 6) | (static_cast<unsigned char>(c) & 0x3Fu)
-#define RAPIDJSON_TRANS(mask)                                                  \
+#define RAPIDJSON_TRANS(mask) \
   result &= ((GetRange(static_cast<unsigned char>(c)) & mask) != 0)
-#define RAPIDJSON_TAIL()                                                       \
+#define RAPIDJSON_TAIL() \
-  RAPIDJSON_COPY();                                                            \
+  RAPIDJSON_COPY();      \
   RAPIDJSON_TRANS(0x70)
     typename InputStream::Ch c = is.Take();
     if (!(c & 0x80)) {
@@ -167,42 +168,42 @@ template <typename CharType = char> struct UTF8 {
     }
     bool result = true;
     switch (type) {
-    case 2:
+      case 2:
-      RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
-      return result;
+        return result;
-    case 3:
+      case 3:
-      RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
-      RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
-      return result;
+        return result;
-    case 4:
+      case 4:
-      RAPIDJSON_COPY();
+        RAPIDJSON_COPY();
-      RAPIDJSON_TRANS(0x50);
+        RAPIDJSON_TRANS(0x50);
-      RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
-      return result;
+        return result;
-    case 5:
+      case 5:
-      RAPIDJSON_COPY();
+        RAPIDJSON_COPY();
-      RAPIDJSON_TRANS(0x10);
+        RAPIDJSON_TRANS(0x10);
-      RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
-      RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
-      return result;
+        return result;
-    case 6:
+      case 6:
-      RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
-      RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
-      RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
-      return result;
+        return result;
-    case 10:
+      case 10:
-      RAPIDJSON_COPY();
+        RAPIDJSON_COPY();
-      RAPIDJSON_TRANS(0x20);
+        RAPIDJSON_TRANS(0x20);
-      RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
-      return result;
+        return result;
-    case 11:
+      case 11:
-      RAPIDJSON_COPY();
+        RAPIDJSON_COPY();
-      RAPIDJSON_TRANS(0x60);
+        RAPIDJSON_TRANS(0x60);
-      RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
-      RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
-      return result;
+        return result;
-    default:
+      default:
-      return false;
+        return false;
     }
 #undef RAPIDJSON_COPY
 #undef RAPIDJSON_TRANS
@@ -212,54 +213,53 @@ template <typename CharType = char> struct UTF8 {
   template <typename InputStream, typename OutputStream>
   static bool Validate(InputStream &is, OutputStream &os) {
 #define RAPIDJSON_COPY() os.Put(c = is.Take())
-#define RAPIDJSON_TRANS(mask)                                                  \
+#define RAPIDJSON_TRANS(mask) \
   result &= ((GetRange(static_cast<unsigned char>(c)) & mask) != 0)
-#define RAPIDJSON_TAIL()                                                       \
+#define RAPIDJSON_TAIL() \
-  RAPIDJSON_COPY();                                                            \
+  RAPIDJSON_COPY();      \
   RAPIDJSON_TRANS(0x70)
     Ch c;
     RAPIDJSON_COPY();
-    if (!(c & 0x80))
+    if (!(c & 0x80)) return true;
-      return true;
 
     bool result = true;
     switch (GetRange(static_cast<unsigned char>(c))) {
-    case 2:
+      case 2:
-      RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
-      return result;
+        return result;
-    case 3:
+      case 3:
-      RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
-      RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
-      return result;
+        return result;
-    case 4:
+      case 4:
-      RAPIDJSON_COPY();
+        RAPIDJSON_COPY();
-      RAPIDJSON_TRANS(0x50);
+        RAPIDJSON_TRANS(0x50);
-      RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
-      return result;
+        return result;
-    case 5:
+      case 5:
-      RAPIDJSON_COPY();
+        RAPIDJSON_COPY();
-      RAPIDJSON_TRANS(0x10);
+        RAPIDJSON_TRANS(0x10);
-      RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
-      RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
-      return result;
+        return result;
-    case 6:
+      case 6:
-      RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
-      RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
-      RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
-      return result;
+        return result;
-    case 10:
+      case 10:
-      RAPIDJSON_COPY();
+        RAPIDJSON_COPY();
-      RAPIDJSON_TRANS(0x20);
+        RAPIDJSON_TRANS(0x20);
-      RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
-      return result;
+        return result;
-    case 11:
+      case 11:
-      RAPIDJSON_COPY();
+        RAPIDJSON_COPY();
-      RAPIDJSON_TRANS(0x60);
+        RAPIDJSON_TRANS(0x60);
-      RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
-      RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
-      return result;
+        return result;
-    default:
+      default:
-      return false;
+        return false;
     }
 #undef RAPIDJSON_COPY
 #undef RAPIDJSON_TRANS
@@ -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)
+    if (static_cast<unsigned char>(c) != 0xEFu) return c;
-      return c;
     c = is.Take();
-    if (static_cast<unsigned char>(c) != 0xBBu)
+    if (static_cast<unsigned char>(c) != 0xBBu) return c;
-      return c;
     c = is.Take();
-    if (static_cast<unsigned char>(c) != 0xBFu)
+    if (static_cast<unsigned char>(c) != 0xBFu) return c;
-      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);
 
@@ -358,7 +357,7 @@ template <typename CharType = wchar_t> struct UTF16 {
     if (codepoint <= 0xFFFF) {
       RAPIDJSON_ASSERT(
           codepoint < 0xD800 ||
-          codepoint > 0xDFFF); // Code point itself cannot be surrogate pair
+          codepoint > 0xDFFF);  // Code point itself cannot be surrogate pair
       os.Put(static_cast<typename OutputStream::Ch>(codepoint));
     } else {
       RAPIDJSON_ASSERT(codepoint <= 0x10FFFF);
@@ -374,7 +373,7 @@ template <typename CharType = wchar_t> struct UTF16 {
     if (codepoint <= 0xFFFF) {
       RAPIDJSON_ASSERT(
           codepoint < 0xD800 ||
-          codepoint > 0xDFFF); // Code point itself cannot be surrogate pair
+          codepoint > 0xDFFF);  // Code point itself cannot be surrogate pair
       PutUnsafe(os, static_cast<typename OutputStream::Ch>(codepoint));
     } else {
       RAPIDJSON_ASSERT(codepoint <= 0x10FFFF);
@@ -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());
   }
@@ -680,11 +686,11 @@ template <typename CharType = char> struct ASCII {
 
 //! Runtime-specified UTF encoding type of a stream.
 enum UTFType {
-  kUTF8 = 0,    //!< UTF-8.
+  kUTF8 = 0,     //!< UTF-8.
-  kUTF16LE = 1, //!< UTF-16 little endian.
+  kUTF16LE = 1,  //!< UTF-16 little endian.
-  kUTF16BE = 2, //!< UTF-16 big endian.
+  kUTF16BE = 2,  //!< UTF-16 big endian.
-  kUTF32LE = 3, //!< UTF-32 little endian.
+  kUTF32LE = 3,  //!< UTF-32 little endian.
-  kUTF32BE = 4  //!< UTF-32 big endian.
+  kUTF32BE = 4   //!< UTF-32 big endian.
 };
 
 //! Dynamically select encoding according to stream's runtime-specified UTF
@@ -692,12 +698,13 @@ 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 };
 
-#define RAPIDJSON_ENCODINGS_FUNC(x)                                            \
+#define RAPIDJSON_ENCODINGS_FUNC(x) \
   UTF8<Ch>::x, UTF16LE<Ch>::x, UTF16BE<Ch>::x, UTF32LE<Ch>::x, UTF32BE<Ch>::x
 
   template <typename OutputStream>
@@ -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))
+    if (!SourceEncoding::Decode(is, &codepoint)) return false;
-      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))
+    if (!SourceEncoding::Decode(is, &codepoint)) return false;
-      return false;
     TargetEncoding::EncodeUnsafe(os, codepoint);
     return true;
   }
@@ -767,7 +773,7 @@ template <typename SourceEncoding, typename TargetEncoding> struct Transcoder {
   static RAPIDJSON_FORCEINLINE bool Validate(InputStream &is,
                                              OutputStream &os) {
     return Transcode(
-        is, os); // Since source/target encoding is different, must transcode.
+        is, os);  // Since source/target encoding is different, must transcode.
   }
 };
 
@@ -776,27 +782,28 @@ 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) {
-    os.Put(is.Take()); // Just copy one code unit. This semantic is different
+    os.Put(is.Take());  // Just copy one code unit. This semantic is different
-                       // from primary template class.
+                        // from primary template class.
     return true;
   }
 
   template <typename InputStream, typename OutputStream>
   static RAPIDJSON_FORCEINLINE bool TranscodeUnsafe(InputStream &is,
                                                     OutputStream &os) {
-    PutUnsafe(os, is.Take()); // Just copy one code unit. This semantic is
+    PutUnsafe(os, is.Take());  // Just copy one code unit. This semantic is
-                              // different from primary template class.
+                               // different from primary template class.
     return true;
   }
 
   template <typename InputStream, typename OutputStream>
   static RAPIDJSON_FORCEINLINE bool Validate(InputStream &is,
                                              OutputStream &os) {
-    return Encoding::Validate(is, os); // source/target encoding are the same
+    return Encoding::Validate(is, os);  // source/target encoding are the same
   }
 };
 
@@ -806,4 +813,4 @@ RAPIDJSON_NAMESPACE_END
 RAPIDJSON_DIAG_POP
 #endif
 
-#endif // RAPIDJSON_ENCODINGS_H_
+#endif  // RAPIDJSON_ENCODINGS_H_

livox_ros_driver/common/rapidjson/error/en.h

@@ -37,61 +37,61 @@ 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 *
+inline const RAPIDJSON_ERROR_CHARTYPE* GetParseError_En(
-GetParseError_En(ParseErrorCode parseErrorCode) {
+    ParseErrorCode parseErrorCode) {
   switch (parseErrorCode) {
-  case kParseErrorNone:
+    case kParseErrorNone:
-    return RAPIDJSON_ERROR_STRING("No error.");
+      return RAPIDJSON_ERROR_STRING("No error.");
 
-  case kParseErrorDocumentEmpty:
+    case kParseErrorDocumentEmpty:
-    return RAPIDJSON_ERROR_STRING("The document is empty.");
+      return RAPIDJSON_ERROR_STRING("The document is empty.");
-  case kParseErrorDocumentRootNotSingular:
+    case kParseErrorDocumentRootNotSingular:
-    return RAPIDJSON_ERROR_STRING(
+      return RAPIDJSON_ERROR_STRING(
-        "The document root must not be followed by other values.");
+          "The document root must not be followed by other values.");
 
-  case kParseErrorValueInvalid:
+    case kParseErrorValueInvalid:
-    return RAPIDJSON_ERROR_STRING("Invalid value.");
+      return RAPIDJSON_ERROR_STRING("Invalid value.");
 
-  case kParseErrorObjectMissName:
+    case kParseErrorObjectMissName:
-    return RAPIDJSON_ERROR_STRING("Missing a name for object member.");
+      return RAPIDJSON_ERROR_STRING("Missing a name for object member.");
-  case kParseErrorObjectMissColon:
+    case kParseErrorObjectMissColon:
-    return RAPIDJSON_ERROR_STRING(
+      return RAPIDJSON_ERROR_STRING(
-        "Missing a colon after a name of object member.");
+          "Missing a colon after a name of object member.");
-  case kParseErrorObjectMissCommaOrCurlyBracket:
+    case kParseErrorObjectMissCommaOrCurlyBracket:
-    return RAPIDJSON_ERROR_STRING(
+      return RAPIDJSON_ERROR_STRING(
-        "Missing a comma or '}' after an object member.");
+          "Missing a comma or '}' after an object member.");
 
-  case kParseErrorArrayMissCommaOrSquareBracket:
+    case kParseErrorArrayMissCommaOrSquareBracket:
-    return RAPIDJSON_ERROR_STRING(
+      return RAPIDJSON_ERROR_STRING(
-        "Missing a comma or ']' after an array element.");
+          "Missing a comma or ']' after an array element.");
 
-  case kParseErrorStringUnicodeEscapeInvalidHex:
+    case kParseErrorStringUnicodeEscapeInvalidHex:
-    return RAPIDJSON_ERROR_STRING(
+      return RAPIDJSON_ERROR_STRING(
-        "Incorrect hex digit after \\u escape in string.");
+          "Incorrect hex digit after \\u escape in string.");
-  case kParseErrorStringUnicodeSurrogateInvalid:
+    case kParseErrorStringUnicodeSurrogateInvalid:
-    return RAPIDJSON_ERROR_STRING("The surrogate pair in string is invalid.");
+      return RAPIDJSON_ERROR_STRING("The surrogate pair in string is invalid.");
-  case kParseErrorStringEscapeInvalid:
+    case kParseErrorStringEscapeInvalid:
-    return RAPIDJSON_ERROR_STRING("Invalid escape character in string.");
+      return RAPIDJSON_ERROR_STRING("Invalid escape character in string.");
-  case kParseErrorStringMissQuotationMark:
+    case kParseErrorStringMissQuotationMark:
-    return RAPIDJSON_ERROR_STRING(
+      return RAPIDJSON_ERROR_STRING(
-        "Missing a closing quotation mark in string.");
+          "Missing a closing quotation mark in string.");
-  case kParseErrorStringInvalidEncoding:
+    case kParseErrorStringInvalidEncoding:
-    return RAPIDJSON_ERROR_STRING("Invalid encoding in string.");
+      return RAPIDJSON_ERROR_STRING("Invalid encoding in string.");
 
-  case kParseErrorNumberTooBig:
+    case kParseErrorNumberTooBig:
-    return RAPIDJSON_ERROR_STRING("Number too big to be stored in double.");
+      return RAPIDJSON_ERROR_STRING("Number too big to be stored in double.");
-  case kParseErrorNumberMissFraction:
+    case kParseErrorNumberMissFraction:
-    return RAPIDJSON_ERROR_STRING("Miss fraction part in number.");
+      return RAPIDJSON_ERROR_STRING("Miss fraction part in number.");
-  case kParseErrorNumberMissExponent:
+    case kParseErrorNumberMissExponent:
-    return RAPIDJSON_ERROR_STRING("Miss exponent in number.");
+      return RAPIDJSON_ERROR_STRING("Miss exponent in number.");
 
-  case kParseErrorTermination:
+    case kParseErrorTermination:
-    return RAPIDJSON_ERROR_STRING("Terminate parsing due to Handler error.");
+      return RAPIDJSON_ERROR_STRING("Terminate parsing due to Handler error.");
-  case kParseErrorUnspecificSyntaxError:
+    case kParseErrorUnspecificSyntaxError:
-    return RAPIDJSON_ERROR_STRING("Unspecific syntax error.");
+      return RAPIDJSON_ERROR_STRING("Unspecific syntax error.");
 
-  default:
+    default:
-    return RAPIDJSON_ERROR_STRING("Unknown error.");
+      return RAPIDJSON_ERROR_STRING("Unknown error.");
   }
 }
 
@@ -101,4 +101,4 @@ RAPIDJSON_NAMESPACE_END
 RAPIDJSON_DIAG_POP
 #endif
 
-#endif // RAPIDJSON_ERROR_EN_H_
+#endif  // RAPIDJSON_ERROR_EN_H_

livox_ros_driver/common/rapidjson/error/error.h

@@ -66,38 +66,41 @@ RAPIDJSON_NAMESPACE_BEGIN
     \see GenericReader::Parse, GenericReader::GetParseErrorCode
 */
 enum ParseErrorCode {
-  kParseErrorNone = 0, //!< No error.
+  kParseErrorNone = 0,  //!< No error.
 
-  kParseErrorDocumentEmpty,           //!< The document is empty.
+  kParseErrorDocumentEmpty,            //!< The document is empty.
-  kParseErrorDocumentRootNotSingular, //!< The document root must not follow by
+  kParseErrorDocumentRootNotSingular,  //!< The document root must not follow by
-                                      //!< other values.
+                                       //!< other values.
 
-  kParseErrorValueInvalid, //!< Invalid value.
+  kParseErrorValueInvalid,  //!< Invalid value.
 
-  kParseErrorObjectMissName,  //!< Missing a name for object member.
+  kParseErrorObjectMissName,   //!< Missing a name for object member.
-  kParseErrorObjectMissColon, //!< Missing a colon after a name of object
+  kParseErrorObjectMissColon,  //!< Missing a colon after a name of object
-                              //!< member.
+                               //!< member.
-  kParseErrorObjectMissCommaOrCurlyBracket, //!< Missing a comma or '}' after an
+  kParseErrorObjectMissCommaOrCurlyBracket,  //!< Missing a comma or '}' after
-                                            //!< object member.
+                                             //!an
+                                             //!< object member.
 
-  kParseErrorArrayMissCommaOrSquareBracket, //!< Missing a comma or ']' after an
+  kParseErrorArrayMissCommaOrSquareBracket,  //!< Missing a comma or ']' after
-                                            //!< array element.
+                                             //!an
+                                             //!< array element.
 
-  kParseErrorStringUnicodeEscapeInvalidHex, //!< Incorrect hex digit after \\u
+  kParseErrorStringUnicodeEscapeInvalidHex,  //!< Incorrect hex digit after \\u
-                                            //!< escape in string.
+                                             //!< escape in string.
-  kParseErrorStringUnicodeSurrogateInvalid, //!< The surrogate pair in string is
+  kParseErrorStringUnicodeSurrogateInvalid,  //!< The surrogate pair in string
-                                            //!< invalid.
+                                             //!is
-  kParseErrorStringEscapeInvalid,     //!< Invalid escape character in string.
+                                             //!< invalid.
-  kParseErrorStringMissQuotationMark, //!< Missing a closing quotation mark in
+  kParseErrorStringEscapeInvalid,      //!< Invalid escape character in string.
-                                      //!< string.
+  kParseErrorStringMissQuotationMark,  //!< Missing a closing quotation mark in
-  kParseErrorStringInvalidEncoding,   //!< Invalid encoding in string.
+                                       //!< string.
+  kParseErrorStringInvalidEncoding,    //!< Invalid encoding in string.
 
-  kParseErrorNumberTooBig,       //!< Number too big to be stored in double.
+  kParseErrorNumberTooBig,        //!< Number too big to be stored in double.
-  kParseErrorNumberMissFraction, //!< Miss fraction part in number.
+  kParseErrorNumberMissFraction,  //!< Miss fraction part in number.
-  kParseErrorNumberMissExponent, //!< Miss exponent in number.
+  kParseErrorNumberMissExponent,  //!< Miss exponent in number.
 
-  kParseErrorTermination,          //!< Parsing was terminated.
+  kParseErrorTermination,           //!< Parsing was terminated.
-  kParseErrorUnspecificSyntaxError //!< Unspecific syntax error.
+  kParseErrorUnspecificSyntaxError  //!< Unspecific syntax error.
 };
 
 //! Result of parsing (wraps ParseErrorCode)
@@ -118,7 +121,7 @@ struct ParseResult {
   //!! Unspecified boolean type
   typedef bool (ParseResult::*BooleanType)() const;
 
-public:
+ public:
   //! Default constructor, no error.
   ParseResult() : code_(kParseErrorNone), offset_(0) {}
   //! Constructor to set an error.
@@ -157,7 +160,7 @@ public:
     offset_ = offset;
   }
 
-private:
+ private:
   ParseErrorCode code_;
   size_t offset_;
 };
@@ -180,4 +183,4 @@ RAPIDJSON_NAMESPACE_END
 RAPIDJSON_DIAG_POP
 #endif
 
-#endif // RAPIDJSON_ERROR_ERROR_H_
+#endif  // RAPIDJSON_ERROR_ERROR_H_

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
@@ -36,8 +36,8 @@ RAPIDJSON_NAMESPACE_BEGIN
     \note implements Stream concept
 */
 class FileReadStream {
-public:
+ public:
-  typedef char Ch; //!< Character type (byte).
+  typedef char Ch;  //!< Character type (byte).
 
   //! Constructor.
   /*!
@@ -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),
+      : fp_(fp),
-        current_(buffer_), readCount_(0), count_(0), eof_(false) {
+        buffer_(buffer),
+        bufferSize_(bufferSize),
+        bufferLast_(0),
+        current_(buffer_),
+        readCount_(0),
+        count_(0),
+        eof_(false) {
     RAPIDJSON_ASSERT(fp_ != 0);
     RAPIDJSON_ASSERT(bufferSize >= 4);
     Read();
@@ -80,7 +86,7 @@ public:
     return (current_ + 4 - !eof_ <= bufferLast_) ? current_ : 0;
   }
 
-private:
+ private:
   void Read() {
     if (current_ < bufferLast_)
       ++current_;
@@ -104,7 +110,7 @@ private:
   Ch *bufferLast_;
   Ch *current_;
   size_t readCount_;
-  size_t count_; //!< Number of characters read
+  size_t count_;  //!< Number of characters read
   bool eof_;
 };
 
@@ -114,4 +120,4 @@ RAPIDJSON_NAMESPACE_END
 RAPIDJSON_DIAG_POP
 #endif
 
-#endif // RAPIDJSON_FILESTREAM_H_
+#endif  // RAPIDJSON_FILESTREAM_H_

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
@@ -34,18 +34,19 @@ RAPIDJSON_NAMESPACE_BEGIN
     \note implements Stream concept
 */
 class FileWriteStream {
-public:
+ public:
-  typedef char Ch; //!< Character type. Only support char.
+  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_)
+    if (current_ >= bufferEnd_) Flush();
-      Flush();
 
     *current_++ = c;
   }
@@ -100,7 +101,7 @@ public:
     return 0;
   }
 
-private:
+ private:
   // Prohibit copy constructor & assignment operator.
   FileWriteStream(const FileWriteStream &);
   FileWriteStream &operator=(const FileWriteStream &);
@@ -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);
 }
 
@@ -123,4 +125,4 @@ RAPIDJSON_NAMESPACE_END
 RAPIDJSON_DIAG_POP
 #endif
 
-#endif // RAPIDJSON_FILESTREAM_H_
+#endif  // RAPIDJSON_FILESTREAM_H_

livox_ros_driver/common/rapidjson/fwd.h

@@ -25,37 +25,51 @@ RAPIDJSON_NAMESPACE_BEGIN
 
 // encodings.h
 
-template <typename CharType> struct UTF8;
+template <typename CharType>
-template <typename CharType> struct UTF16;
+struct UTF8;
-template <typename CharType> struct UTF16BE;
+template <typename CharType>
-template <typename CharType> struct UTF16LE;
+struct UTF16;
-template <typename CharType> struct UTF32;
+template <typename CharType>
-template <typename CharType> struct UTF32BE;
+struct UTF16BE;
-template <typename CharType> struct UTF32LE;
+template <typename CharType>
-template <typename CharType> struct ASCII;
+struct UTF16LE;
-template <typename CharType> struct AutoUTF;
+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>
@@ -146,4 +167,4 @@ typedef GenericSchemaValidator<
 
 RAPIDJSON_NAMESPACE_END
 
-#endif // RAPIDJSON_RAPIDJSONFWD_H_
+#endif  // RAPIDJSON_RAPIDJSONFWD_H_

livox_ros_driver/common/rapidjson/internal/biginteger.h

@@ -22,7 +22,7 @@
 #include "../rapidjson.h"
 
 #if defined(_MSC_VER) && !__INTEL_COMPILER && defined(_M_AMD64)
-#include <intrin.h> // for _umul128
+#include <intrin.h>  // for _umul128
 #pragma intrinsic(_umul128)
 #endif
 
@@ -30,7 +30,7 @@ RAPIDJSON_NAMESPACE_BEGIN
 namespace internal {
 
 class BigInteger {
-public:
+ public:
   typedef uint64_t Type;
 
   BigInteger(const BigInteger &rhs) : count_(rhs.count_) {
@@ -44,15 +44,14 @@ public:
     digits_[0] = 0;
     size_t i = 0;
     const size_t kMaxDigitPerIteration =
-        19; // 2^64 = 18446744073709551616 > 10^19
+        19;  // 2^64 = 18446744073709551616 > 10^19
     while (length >= kMaxDigitPerIteration) {
       AppendDecimal64(decimals + i, decimals + i + kMaxDigitPerIteration);
       length -= kMaxDigitPerIteration;
       i += kMaxDigitPerIteration;
     }
 
-    if (length > 0)
+    if (length > 0) AppendDecimal64(decimals + i, decimals + i + length);
-      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)
+      if (digits_[i] >= backup) return *this;  // no carry
-        return *this; // no carry
       backup = digits_[i + 1];
       digits_[i + 1] += 1;
     }
 
     // Last carry
-    if (digits_[count_ - 1] < backup)
+    if (digits_[count_ - 1] < backup) PushBack(1);
-      PushBack(1);
 
     return *this;
   }
 
   BigInteger &operator*=(uint64_t u) {
-    if (u == 0)
+    if (u == 0) return *this = 0;
-      return *this = 0;
+    if (u == 1) return *this;
-    if (u == 1)
+    if (*this == 1) return *this = u;
-      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)
+    if (k > 0) PushBack(k);
-      PushBack(k);
 
     return *this;
   }
 
   BigInteger &operator*=(uint32_t u) {
-    if (u == 0)
+    if (u == 0) return *this = 0;
-      return *this = 0;
+    if (u == 1) return *this;
-    if (u == 1)
+    if (*this == 1) return *this = u;
-      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)
+    if (k > 0) PushBack(k);
-      PushBack(k);
 
     return *this;
   }
 
   BigInteger &operator<<=(size_t shift) {
-    if (IsZero() || shift == 0)
+    if (IsZero() || shift == 0) return *this;
-      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_])
+      if (digits_[count_]) 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)
+    if (exp == 0) return *this;
-      return *this;
     for (; exp >= 27; exp -= 27)
-      *this *= RAPIDJSON_UINT64_C2(0X6765C793, 0XFA10079D); // 5^27
+      *this *= RAPIDJSON_UINT64_C2(0X6765C793, 0XFA10079D);  // 5^27
     for (; exp >= 13; exp -= 13)
-      *this *= static_cast<uint32_t>(1220703125u); // 5^13
+      *this *= static_cast<uint32_t>(1220703125u);  // 5^13
-    if (exp > 0)
+    if (exp > 0) *this *= kPow5[exp - 1];
-      *this *= kPow5[exp - 1];
     return *this;
   }
 
@@ -199,7 +184,7 @@ public:
   bool Difference(const BigInteger &rhs, BigInteger *out) const {
     int cmp = Compare(rhs);
     RAPIDJSON_ASSERT(cmp != 0);
-    const BigInteger *a, *b; // Makes a > b
+    const BigInteger *a, *b;  // Makes a > b
     bool ret;
     if (cmp < 0) {
       a = &rhs;
@@ -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_)
+      if (i < b->count_) d -= b->digits_[i];
-        d -= b->digits_[i];
       borrow = (d > a->digits_[i]) ? 1 : 0;
       out->digits_[i] = d;
-      if (d != 0)
+      if (d != 0) out->count_ = i + 1;
-        out->count_ = i + 1;
     }
 
     return ret;
   }
 
   int Compare(const BigInteger &rhs) const {
-    if (count_ != rhs.count_)
+    if (count_ != rhs.count_) return count_ < rhs.count_ ? -1 : 1;
-      return count_ < rhs.count_ ? -1 : 1;
 
     for (size_t i = count_; i-- > 0;)
       if (digits_[i] != rhs.digits_[i])
@@ -243,14 +225,14 @@ public:
   }
   bool IsZero() const { return count_ == 1 && digits_[0] == 0; }
 
-private:
+ private:
   void AppendDecimal64(const char *begin, const char *end) {
     uint64_t u = ParseUint64(begin, end);
     if (IsZero())
       *this = u;
     else {
       unsigned exp = static_cast<unsigned>(end - begin);
-      (MultiplyPow5(exp) <<= exp) += u; // *this = *this * 10^exp + u
+      (MultiplyPow5(exp) <<= exp) += u;  // *this = *this * 10^exp + u
     }
   }
 
@@ -273,10 +255,9 @@ private:
                            uint64_t *outHigh) {
 #if defined(_MSC_VER) && defined(_M_AMD64)
     uint64_t low = _umul128(a, b, outHigh) + k;
-    if (low < k)
+    if (low < k) (*outHigh)++;
-      (*outHigh)++;
     return low;
-#elif (__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 6)) &&              \
+#elif (__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 6)) && \
     defined(__x86_64__)
     __extension__ typedef unsigned __int128 uint128;
     uint128 p = static_cast<uint128>(a) * static_cast<uint128>(b);
@@ -287,22 +268,20 @@ private:
     const uint64_t a0 = a & 0xFFFFFFFF, a1 = a >> 32, b0 = b & 0xFFFFFFFF,
                    b1 = b >> 32;
     uint64_t x0 = a0 * b0, x1 = a0 * b1, x2 = a1 * b0, x3 = a1 * b1;
-    x1 += (x0 >> 32); // can't give carry
+    x1 += (x0 >> 32);  // can't give carry
     x1 += x2;
-    if (x1 < x2)
+    if (x1 < x2) x3 += (static_cast<uint64_t>(1) << 32);
-      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)
+    if (lo < k) hi++;
-      hi++;
     *outHigh = hi;
     return lo;
 #endif
   }
 
-  static const size_t kBitCount = 3328; // 64bit * 54 > 10^1000
+  static const size_t kBitCount = 3328;  // 64bit * 54 > 10^1000
   static const size_t kCapacity = kBitCount / sizeof(Type);
   static const size_t kTypeBit = sizeof(Type) * 8;
 
@@ -310,7 +289,7 @@ private:
   size_t count_;
 };
 
-} // namespace internal
+}  // namespace internal
 RAPIDJSON_NAMESPACE_END
 
-#endif // RAPIDJSON_BIGINTEGER_H_
+#endif  // RAPIDJSON_BIGINTEGER_H_

livox_ros_driver/common/rapidjson/internal/clzll.h

@@ -33,7 +33,7 @@
 RAPIDJSON_NAMESPACE_BEGIN
 namespace internal {
 
-#if (defined(__GNUC__) && __GNUC__ >= 4) ||                                    \
+#if (defined(__GNUC__) && __GNUC__ >= 4) || \
     RAPIDJSON_HAS_BUILTIN(__builtin_clzll)
 #define RAPIDJSON_CLZLL __builtin_clzll
 #else
@@ -49,12 +49,11 @@ 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)))
+  if (_BitScanReverse(&r, static_cast<uint32_t>(x >> 32))) return 63 - (r + 32);
-    return 63 - (r + 32);
 
   // Scan the low 32 bits.
   _BitScanReverse(&r, static_cast<uint32_t>(x & 0xFFFFFFFF));
-#endif // _WIN64
+#endif  // _WIN64
 
   return 63 - r;
 #else
@@ -65,14 +64,14 @@ inline uint32_t clzll(uint64_t x) {
   }
 
   return r;
-#endif // _MSC_VER
+#endif  // _MSC_VER
 }
 
 #define RAPIDJSON_CLZLL RAPIDJSON_NAMESPACE::internal::clzll
-#endif // (defined(__GNUC__) && __GNUC__ >= 4) ||
+#endif  // (defined(__GNUC__) && __GNUC__ >= 4) ||
-       // RAPIDJSON_HAS_BUILTIN(__builtin_clzll)
+        // RAPIDJSON_HAS_BUILTIN(__builtin_clzll)
 
-} // namespace internal
+}  // namespace internal
 RAPIDJSON_NAMESPACE_END
 
-#endif // RAPIDJSON_CLZLL_H_
+#endif  // RAPIDJSON_CLZLL_H_

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>
@@ -74,16 +74,16 @@ struct DiyFp {
 #if defined(_MSC_VER) && defined(_M_AMD64)
     uint64_t h;
     uint64_t l = _umul128(f, rhs.f, &h);
-    if (l & (uint64_t(1) << 63)) // rounding
+    if (l & (uint64_t(1) << 63))  // rounding
       h++;
     return DiyFp(h, e + rhs.e + 64);
-#elif (__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 6)) &&              \
+#elif (__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 6)) && \
     defined(__x86_64__)
     __extension__ typedef unsigned __int128 uint128;
     uint128 p = static_cast<uint128>(f) * static_cast<uint128>(rhs.f);
     uint64_t h = static_cast<uint64_t>(p >> 64);
     uint64_t l = static_cast<uint64_t>(p);
-    if (l & (uint64_t(1) << 63)) // rounding
+    if (l & (uint64_t(1) << 63))  // rounding
       h++;
     return DiyFp(h, e + rhs.e + 64);
 #else
@@ -97,7 +97,7 @@ struct DiyFp {
     const uint64_t ad = a * d;
     const uint64_t bd = b * d;
     uint64_t tmp = (bd >> 32) + (ad & M32) + (bc & M32);
-    tmp += 1U << 31; /// mult_round
+    tmp += 1U << 31;  /// mult_round
     return DiyFp(ac + (ad >> 32) + (bc >> 32) + (tmp >> 32), e + rhs.e + 64);
 #endif
   }
@@ -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
+              347;  // dk must be positive, so can do ceiling in positive
   int k = static_cast<int>(dk);
-  if (dk - k > 0.0)
+  if (dk - k > 0.0) k++;
-    k++;
 
   unsigned index = static_cast<unsigned>((k >> 3) + 1);
-  *K = -(-348 +
+  *K = -(-348 + static_cast<int>(
-         static_cast<int>(index << 3)); // decimal exponent no need lookup table
+                    index << 3));  // decimal exponent no need lookup table
 
   return GetCachedPowerByIndex(index);
 }
@@ -301,7 +299,7 @@ RAPIDJSON_DIAG_POP
 RAPIDJSON_DIAG_OFF(padded)
 #endif
 
-} // namespace internal
+}  // namespace internal
 RAPIDJSON_NAMESPACE_END
 
-#endif // RAPIDJSON_DIYFP_H_
+#endif  // RAPIDJSON_DIYFP_H_

livox_ros_driver/common/rapidjson/internal/dtoa.h

@@ -25,7 +25,7 @@
 
 #include "diyfp.h"
 #include "ieee754.h"
-#include "itoa.h" // GetDigitsLut()
+#include "itoa.h"  // GetDigitsLut()
 
 RAPIDJSON_NAMESPACE_BEGIN
 namespace internal {
@@ -33,15 +33,14 @@ namespace internal {
 #ifdef __GNUC__
 RAPIDJSON_DIAG_PUSH
 RAPIDJSON_DIAG_OFF(effc++)
-RAPIDJSON_DIAG_OFF(array -
+RAPIDJSON_DIAG_OFF(array - bounds)  // some gcc versions generate wrong warnings
-                   bounds) // some gcc versions generate wrong warnings
+// https://gcc.gnu.org/bugzilla/show_bug.cgi?id=59124
-                           // https://gcc.gnu.org/bugzilla/show_bug.cgi?id=59124
 #endif
 
 inline void GrisuRound(char *buffer, int len, uint64_t delta, uint64_t rest,
                        uint64_t ten_kappa, uint64_t wp_w) {
   while (rest < wp_w && delta - rest >= ten_kappa &&
-         (rest + ten_kappa < wp_w || /// closer
+         (rest + ten_kappa < wp_w ||  /// closer
           wp_w - rest > rest + ten_kappa - wp_w)) {
     buffer[len - 1]--;
     rest += ten_kappa;
@@ -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)
+  if (n < 10) return 1;
-    return 1;
+  if (n < 100) return 2;
-  if (n < 100)
+  if (n < 1000) return 3;
-    return 2;
+  if (n < 10000) return 4;
-  if (n < 1000)
+  if (n < 100000) return 5;
-    return 3;
+  if (n < 1000000) return 6;
-  if (n < 10000)
+  if (n < 10000000) return 7;
-    return 4;
+  if (n < 100000000) return 8;
-  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;
@@ -82,49 +73,49 @@ inline void DigitGen(const DiyFp &W, const DiyFp &Mp, uint64_t delta,
   const DiyFp wp_w = Mp - W;
   uint32_t p1 = static_cast<uint32_t>(Mp.f >> -one.e);
   uint64_t p2 = Mp.f & (one.f - 1);
-  int kappa = CountDecimalDigit32(p1); // kappa in [0, 9]
+  int kappa = CountDecimalDigit32(p1);  // kappa in [0, 9]
   *len = 0;
 
   while (kappa > 0) {
     uint32_t d = 0;
     switch (kappa) {
-    case 9:
+      case 9:
-      d = p1 / 100000000;
+        d = p1 / 100000000;
-      p1 %= 100000000;
+        p1 %= 100000000;
-      break;
+        break;
-    case 8:
+      case 8:
-      d = p1 / 10000000;
+        d = p1 / 10000000;
-      p1 %= 10000000;
+        p1 %= 10000000;
-      break;
+        break;
-    case 7:
+      case 7:
-      d = p1 / 1000000;
+        d = p1 / 1000000;
-      p1 %= 1000000;
+        p1 %= 1000000;
-      break;
+        break;
-    case 6:
+      case 6:
-      d = p1 / 100000;
+        d = p1 / 100000;
-      p1 %= 100000;
+        p1 %= 100000;
-      break;
+        break;
-    case 5:
+      case 5:
-      d = p1 / 10000;
+        d = p1 / 10000;
-      p1 %= 10000;
+        p1 %= 10000;
-      break;
+        break;
-    case 4:
+      case 4:
-      d = p1 / 1000;
+        d = p1 / 1000;
-      p1 %= 1000;
+        p1 %= 1000;
-      break;
+        break;
-    case 3:
+      case 3:
-      d = p1 / 100;
+        d = p1 / 100;
-      p1 %= 100;
+        p1 %= 100;
-      break;
+        break;
-    case 2:
+      case 2:
-      d = p1 / 10;
+        d = p1 / 10;
-      p1 %= 10;
+        p1 %= 10;
-      break;
+        break;
-    case 1:
+      case 1:
-      d = p1;
+        d = p1;
-      p1 = 0;
+        p1 = 0;
-      break;
+        break;
-    default:;
+      default:;
     }
     if (d || *len)
       buffer[(*len)++] = static_cast<char>('0' + static_cast<char>(d));
@@ -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)
+    if (d || *len) buffer[(*len)++] = static_cast<char>('0' + d);
-      buffer[(*len)++] = static_cast<char>('0' + d);
     p2 &= one.f - 1;
     kappa--;
     if (p2 < delta) {
@@ -194,12 +184,11 @@ inline char *WriteExponent(int K, char *buffer) {
 }
 
 inline char *Prettify(char *buffer, int length, int k, int maxDecimalPlaces) {
-  const int kk = length + k; // 10^(kk-1) <= v < 10^kk
+  const int kk = length + k;  // 10^(kk-1) <= v < 10^kk
 
   if (0 <= k && kk <= 21) {
     // 1234e7 -> 12340000000
-    for (int i = length; i < kk; i++)
+    for (int i = length; i < kk; i++) buffer[i] = '0';
-      buffer[i] = '0';
     buffer[kk] = '.';
     buffer[kk + 1] = '0';
     return &buffer[kk + 2];
@@ -212,9 +201,8 @@ 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')
+        if (buffer[i] != '0') return &buffer[i + 1];
-          return &buffer[i + 1];
+      return &buffer[kk + 2];  // Reserve one zero
-      return &buffer[kk + 2]; // Reserve one zero
     } else
       return &buffer[length + 1];
   } else if (-6 < kk && kk <= 0) {
@@ -223,15 +211,13 @@ 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++)
+    for (int i = 2; i < offset; i++) buffer[i] = '0';
-      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')
+        if (buffer[i] != '0') return &buffer[i + 1];
-          return &buffer[i + 1];
+      return &buffer[3];  // Reserve one zero
-      return &buffer[3]; // Reserve one zero
     } else
       return &buffer[length + offset];
   } else if (kk < -maxDecimalPlaces) {
@@ -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())
+    if (d.Sign()) *buffer++ = '-';  // -0.0, Issue #289
-      *buffer++ = '-'; // -0.0, Issue #289
     buffer[0] = '0';
     buffer[1] = '.';
     buffer[2] = '0';
@@ -278,7 +263,7 @@ inline char *dtoa(double value, char *buffer, int maxDecimalPlaces = 324) {
 RAPIDJSON_DIAG_POP
 #endif
 
-} // namespace internal
+}  // namespace internal
 RAPIDJSON_NAMESPACE_END
 
-#endif // RAPIDJSON_DTOA_
+#endif  // RAPIDJSON_DTOA_

livox_ros_driver/common/rapidjson/internal/ieee754.h

@@ -25,7 +25,7 @@ RAPIDJSON_NAMESPACE_BEGIN
 namespace internal {
 
 class Double {
-public:
+ public:
   Double() {}
   Double(double d) : d_(d) {}
   Double(uint64_t u) : u_(u) {}
@@ -76,7 +76,7 @@ public:
       return order + 1074;
   }
 
-private:
+ private:
   static const int kSignificandSize = 52;
   static const int kExponentBias = 0x3FF;
   static const int kDenormalExponent = 1 - kExponentBias;
@@ -94,7 +94,7 @@ private:
   };
 };
 
-} // namespace internal
+}  // namespace internal
 RAPIDJSON_NAMESPACE_END
 
-#endif // RAPIDJSON_IEEE754_
+#endif  // RAPIDJSON_IEEE754_

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)
+    if (value >= 1000) *buffer++ = cDigitsLut[d1];
-      *buffer++ = cDigitsLut[d1];
+    if (value >= 100) *buffer++ = cDigitsLut[d1 + 1];
-    if (value >= 100)
+    if (value >= 10) *buffer++ = cDigitsLut[d2];
-      *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)
+    if (value >= 10000000) *buffer++ = cDigitsLut[d1];
-      *buffer++ = cDigitsLut[d1];
+    if (value >= 1000000) *buffer++ = cDigitsLut[d1 + 1];
-    if (value >= 1000000)
+    if (value >= 100000) *buffer++ = cDigitsLut[d2];
-      *buffer++ = cDigitsLut[d1 + 1];
-    if (value >= 100000)
-      *buffer++ = cDigitsLut[d2];
     *buffer++ = cDigitsLut[d2 + 1];
 
     *buffer++ = cDigitsLut[d3];
@@ -85,7 +79,7 @@ inline char *u32toa(uint32_t value, char *buffer) {
   } else {
     // value = aabbbbcccc in decimal
 
-    const uint32_t a = value / 100000000; // 1 to 42
+    const uint32_t a = value / 100000000;  // 1 to 42
     value %= 100000000;
 
     if (a >= 10) {
@@ -95,8 +89,8 @@ inline char *u32toa(uint32_t value, char *buffer) {
     } else
       *buffer++ = static_cast<char>('0' + static_cast<char>(a));
 
-    const uint32_t b = value / 10000; // 0 to 9999
+    const uint32_t b = value / 10000;  // 0 to 9999
-    const uint32_t c = value % 10000; // 0 to 9999
+    const uint32_t c = value % 10000;  // 0 to 9999
 
     const uint32_t d1 = (b / 100) << 1;
     const uint32_t d2 = (b % 100) << 1;
@@ -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)
+      if (v >= 1000) *buffer++ = cDigitsLut[d1];
-        *buffer++ = cDigitsLut[d1];
+      if (v >= 100) *buffer++ = cDigitsLut[d1 + 1];
-      if (v >= 100)
+      if (v >= 10) *buffer++ = cDigitsLut[d2];
-        *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)
+      if (value >= 10000000) *buffer++ = cDigitsLut[d1];
-        *buffer++ = cDigitsLut[d1];
+      if (value >= 1000000) *buffer++ = cDigitsLut[d1 + 1];
-      if (value >= 1000000)
+      if (value >= 100000) *buffer++ = cDigitsLut[d2];
-        *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)
+    if (value >= kTen15) *buffer++ = cDigitsLut[d1];
-      *buffer++ = cDigitsLut[d1];
+    if (value >= kTen14) *buffer++ = cDigitsLut[d1 + 1];
-    if (value >= kTen14)
+    if (value >= kTen13) *buffer++ = cDigitsLut[d2];
-      *buffer++ = cDigitsLut[d1 + 1];
+    if (value >= kTen12) *buffer++ = cDigitsLut[d2 + 1];
-    if (value >= kTen13)
+    if (value >= kTen11) *buffer++ = cDigitsLut[d3];
-      *buffer++ = cDigitsLut[d2];
+    if (value >= kTen10) *buffer++ = cDigitsLut[d3 + 1];
-    if (value >= kTen12)
+    if (value >= kTen9) *buffer++ = cDigitsLut[d4];
-      *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];
@@ -224,7 +205,7 @@ inline char *u64toa(uint64_t value, char *buffer) {
     *buffer++ = cDigitsLut[d8];
     *buffer++ = cDigitsLut[d8 + 1];
   } else {
-    const uint32_t a = static_cast<uint32_t>(value / kTen16); // 1 to 1844
+    const uint32_t a = static_cast<uint32_t>(value / kTen16);  // 1 to 1844
     value %= kTen16;
 
     if (a < 10)
@@ -301,7 +282,7 @@ inline char *i64toa(int64_t value, char *buffer) {
   return u64toa(u, buffer);
 }
 
-} // namespace internal
+}  // namespace internal
 RAPIDJSON_NAMESPACE_END
 
-#endif // RAPIDJSON_ITOA_
+#endif  // RAPIDJSON_ITOA_

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 <bool C>
-  template <typename T1, typename T2> struct Apply { typedef T1 Type; };
+struct SelectIfImpl {
+  template <typename T1, typename T2>
+  struct Apply {
+    typedef T1 Type;
+  };
 };
-template <> struct SelectIfImpl<false> {
+template <>
-  template <typename T1, typename T2> struct Apply { typedef T2 Type; };
+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 <bool Cond1, bool Cond2>
-template <> struct AndExprCond<true, true> : TrueType {};
+struct AndExprCond : FalseType {};
-template <bool Cond1, bool Cond2> struct OrExprCond : TrueType {};
+template <>
-template <> struct OrExprCond<false, false> : FalseType {};
+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>
-template <typename T> struct RemoveConst<const T> { typedef T Type; };
+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, typename U>
-template <typename T> struct IsSame<T, T> : TrueType {};
+struct IsSame : FalseType {};
+template <typename T>
+struct IsSame<T, T> : TrueType {};
 
-template <typename T> struct IsConst : FalseType {};
+template <typename T>
-template <typename T> struct IsConst<const T> : TrueType {};
+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>
-template <typename T> struct IsPointer<T *> : TrueType {};
+struct IsPointer : FalseType {};
+template <typename T>
+struct IsPointer<T *> : TrueType {};
 
 ///////////////////////////////////////////////////////////////////////////////
 // IsBaseOf
@@ -116,16 +147,18 @@ template <typename T> struct IsPointer<T *> : TrueType {};
 template <typename B, typename D>
 struct IsBaseOf : BoolType<::std::is_base_of<B, D>::value> {};
 
-#else // simplified version adopted from Boost
+#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 {
@@ -139,21 +172,25 @@ template <typename B, typename D> struct IsBaseOfImpl {
 template <typename B, typename D>
 struct IsBaseOf : OrExpr<IsSame<B, D>, BoolExpr<IsBaseOfImpl<B, D>>>::Type {};
 
-#endif // RAPIDJSON_HAS_CXX11_TYPETRAITS
+#endif  // RAPIDJSON_HAS_CXX11_TYPETRAITS
 
 //////////////////////////////////////////////////////////////////////////
 // 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,32 +201,34 @@ struct DisableIf : DisableIfCond<Condition::Value, T> {};
 
 // SFINAE helpers
 struct SfinaeTag {};
-template <typename T> struct RemoveSfinaeTag;
+template <typename T>
-template <typename T> struct RemoveSfinaeTag<SfinaeTag &(*)(T)> {
+struct RemoveSfinaeTag;
+template <typename T>
+struct RemoveSfinaeTag<SfinaeTag &(*)(T)> {
   typedef T Type;
 };
 
-#define RAPIDJSON_REMOVEFPTR_(type)                                            \
+#define RAPIDJSON_REMOVEFPTR_(type)                          \
-  typename ::RAPIDJSON_NAMESPACE::internal::RemoveSfinaeTag<                   \
+  typename ::RAPIDJSON_NAMESPACE::internal::RemoveSfinaeTag< \
       ::RAPIDJSON_NAMESPACE::internal::SfinaeTag &(*)type>::Type
 
-#define RAPIDJSON_ENABLEIF(cond)                                               \
+#define RAPIDJSON_ENABLEIF(cond)                                            \
-  typename ::RAPIDJSON_NAMESPACE::internal::EnableIf<RAPIDJSON_REMOVEFPTR_(    \
+  typename ::RAPIDJSON_NAMESPACE::internal::EnableIf<RAPIDJSON_REMOVEFPTR_( \
       cond)>::Type * = NULL
 
-#define RAPIDJSON_DISABLEIF(cond)                                              \
+#define RAPIDJSON_DISABLEIF(cond)                                            \
-  typename ::RAPIDJSON_NAMESPACE::internal::DisableIf<RAPIDJSON_REMOVEFPTR_(   \
+  typename ::RAPIDJSON_NAMESPACE::internal::DisableIf<RAPIDJSON_REMOVEFPTR_( \
       cond)>::Type * = NULL
 
-#define RAPIDJSON_ENABLEIF_RETURN(cond, returntype)                            \
+#define RAPIDJSON_ENABLEIF_RETURN(cond, returntype)   \
-  typename ::RAPIDJSON_NAMESPACE::internal::EnableIf<                          \
+  typename ::RAPIDJSON_NAMESPACE::internal::EnableIf< \
       RAPIDJSON_REMOVEFPTR_(cond), RAPIDJSON_REMOVEFPTR_(returntype)>::Type
 
-#define RAPIDJSON_DISABLEIF_RETURN(cond, returntype)                           \
+#define RAPIDJSON_DISABLEIF_RETURN(cond, returntype)   \
-  typename ::RAPIDJSON_NAMESPACE::internal::DisableIf<                         \
+  typename ::RAPIDJSON_NAMESPACE::internal::DisableIf< \
       RAPIDJSON_REMOVEFPTR_(cond), RAPIDJSON_REMOVEFPTR_(returntype)>::Type
 
-} // namespace internal
+}  // namespace internal
 RAPIDJSON_NAMESPACE_END
 //@endcond
 
@@ -201,4 +240,4 @@ RAPIDJSON_DIAG_POP
 RAPIDJSON_DIAG_POP
 #endif
 
-#endif // RAPIDJSON_INTERNAL_META_H_
+#endif  // RAPIDJSON_INTERNAL_META_H_

livox_ros_driver/common/rapidjson/internal/pow10.h

@@ -71,7 +71,7 @@ inline double Pow10(int n) {
   return e[n];
 }
 
-} // namespace internal
+}  // namespace internal
 RAPIDJSON_NAMESPACE_END
 
-#endif // RAPIDJSON_POW10_
+#endif  // RAPIDJSON_POW10_

livox_ros_driver/common/rapidjson/internal/regex.h

@@ -29,7 +29,7 @@ RAPIDJSON_DIAG_OFF(padded)
 RAPIDJSON_DIAG_OFF(switch - enum)
 #elif defined(_MSC_VER)
 RAPIDJSON_DIAG_PUSH
-RAPIDJSON_DIAG_OFF(4512) // assignment operator could not be generated
+RAPIDJSON_DIAG_OFF(4512)  // assignment operator could not be generated
 #endif
 
 #ifdef __GNUC__
@@ -47,21 +47,21 @@ namespace internal {
 ///////////////////////////////////////////////////////////////////////////////
 // DecodedStream
 
-template <typename SourceStream, typename Encoding> class DecodedStream {
+template <typename SourceStream, typename Encoding>
-public:
+class DecodedStream {
+ public:
   DecodedStream(SourceStream &ss) : ss_(ss), codepoint_() { Decode(); }
   unsigned Peek() { return codepoint_; }
   unsigned Take() {
     unsigned c = codepoint_;
-    if (c) // No further decoding when '\0'
+    if (c)  // No further decoding when '\0'
       Decode();
     return c;
   }
 
-private:
+ private:
   void Decode() {
-    if (!Encoding::Decode(ss_, &codepoint_))
+    if (!Encoding::Decode(ss_, &codepoint_)) codepoint_ = 0;
-      codepoint_ = 0;
   }
 
   SourceStream &ss_;
@@ -72,10 +72,11 @@ private:
 // GenericRegex
 
 static const SizeType kRegexInvalidState = ~SizeType(
-    0); //!< Represents an invalid index in GenericRegex::State::out, out1
+    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.
 /*!
@@ -112,16 +113,21 @@ template <typename Encoding, typename Allocator> class GenericRegexSearch;
 */
 template <typename Encoding, typename Allocator = CrtAllocator>
 class GenericRegex {
-public:
+ 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),
+        states_(allocator_, 256),
-        root_(kRegexInvalidState), stateCount_(), rangeCount_(), anchorBegin_(),
+        ranges_(allocator_, 256),
+        root_(kRegexInvalidState),
+        stateCount_(),
+        rangeCount_(),
+        anchorBegin_(),
         anchorEnd_() {
     GenericStringStream<Encoding> ss(source);
     DecodedStream<GenericStringStream<Encoding>, Encoding> ds(ss);
@@ -132,7 +138,7 @@ public:
 
   bool IsValid() const { return root_ != kRegexInvalidState; }
 
-private:
+ private:
   enum Operator {
     kZeroOrOne,
     kZeroOrMore,
@@ -142,19 +148,19 @@ private:
     kLeftParenthesis
   };
 
-  static const unsigned kAnyCharacterClass = 0xFFFFFFFF; //!< For '.'
+  static const unsigned kAnyCharacterClass = 0xFFFFFFFF;  //!< For '.'
   static const unsigned kRangeCharacterClass = 0xFFFFFFFE;
   static const unsigned kRangeNegationFlag = 0x80000000;
 
   struct Range {
-    unsigned start; //
+    unsigned start;  //
     unsigned end;
     SizeType next;
   };
 
   struct State {
-    SizeType out;  //!< Equals to kInvalid for matching state
+    SizeType out;   //!< Equals to kInvalid for matching state
-    SizeType out1; //!< Equals to non-kInvalid for split
+    SizeType out1;  //!< Equals to non-kInvalid for split
     SizeType rangeStart;
     unsigned codepoint;
   };
@@ -162,7 +168,7 @@ private:
   struct Frag {
     Frag(SizeType s, SizeType o, SizeType m) : start(s), out(o), minIndex(m) {}
     SizeType start;
-    SizeType out; //!< link-list of all output states
+    SizeType out;  //!< link-list of all output states
     SizeType minIndex;
   };
 
@@ -188,116 +194,108 @@ private:
 
   template <typename InputStream>
   void Parse(DecodedStream<InputStream, Encoding> &ds) {
-    Stack<Allocator> operandStack(allocator_, 256);  // Frag
+    Stack<Allocator> operandStack(allocator_, 256);   // Frag
-    Stack<Allocator> operatorStack(allocator_, 256); // Operator
+    Stack<Allocator> operatorStack(allocator_, 256);  // Operator
     Stack<Allocator> atomCountStack(allocator_,
-                                    256); // unsigned (Atom per parenthesis)
+                                    256);  // unsigned (Atom per parenthesis)
 
     *atomCountStack.template Push<unsigned>() = 0;
 
     unsigned codepoint;
     while (ds.Peek() != 0) {
       switch (codepoint = ds.Take()) {
-      case '^':
+        case '^':
-        anchorBegin_ = true;
+          anchorBegin_ = true;
-        break;
+          break;
 
-      case '$':
+        case '$':
-        anchorEnd_ = true;
+          anchorEnd_ = true;
-        break;
+          break;
 
-      case '|':
+        case '|':
-        while (!operatorStack.Empty() &&
+          while (!operatorStack.Empty() &&
-               *operatorStack.template Top<Operator>() < kAlternation)
+                 *operatorStack.template Top<Operator>() < kAlternation)
-          if (!Eval(operandStack, *operatorStack.template Pop<Operator>(1)))
+            if (!Eval(operandStack, *operatorStack.template Pop<Operator>(1)))
-            return;
+              return;
-        *operatorStack.template Push<Operator>() = kAlternation;
+          *operatorStack.template Push<Operator>() = kAlternation;
-        *atomCountStack.template Top<unsigned>() = 0;
+          *atomCountStack.template Top<unsigned>() = 0;
-        break;
+          break;
 
-      case '(':
+        case '(':
-        *operatorStack.template Push<Operator>() = kLeftParenthesis;
+          *operatorStack.template Push<Operator>() = kLeftParenthesis;
-        *atomCountStack.template Push<unsigned>() = 0;
+          *atomCountStack.template Push<unsigned>() = 0;
-        break;
+          break;
 
-      case ')':
+        case ')':
-        while (!operatorStack.Empty() &&
+          while (!operatorStack.Empty() &&
-               *operatorStack.template Top<Operator>() != kLeftParenthesis)
+                 *operatorStack.template Top<Operator>() != kLeftParenthesis)
-          if (!Eval(operandStack, *operatorStack.template Pop<Operator>(1)))
+            if (!Eval(operandStack, *operatorStack.template Pop<Operator>(1)))
-            return;
+              return;
-        if (operatorStack.Empty())
+          if (operatorStack.Empty()) return;
-          return;
+          operatorStack.template Pop<Operator>(1);
-        operatorStack.template Pop<Operator>(1);
+          atomCountStack.template Pop<unsigned>(1);
-        atomCountStack.template Pop<unsigned>(1);
+          ImplicitConcatenation(atomCountStack, operatorStack);
-        ImplicitConcatenation(atomCountStack, operatorStack);
+          break;
-        break;
 
-      case '?':
+        case '?':
-        if (!Eval(operandStack, kZeroOrOne))
+          if (!Eval(operandStack, kZeroOrOne)) return;
-          return;
+          break;
-        break;
 
-      case '*':
+        case '*':
-        if (!Eval(operandStack, kZeroOrMore))
+          if (!Eval(operandStack, kZeroOrMore)) return;
-          return;
+          break;
-        break;
 
-      case '+':
+        case '+':
-        if (!Eval(operandStack, kOneOrMore))
+          if (!Eval(operandStack, kOneOrMore)) return;
-          return;
+          break;
-        break;
 
-      case '{': {
+        case '{': {
-        unsigned n, m;
+          unsigned n, m;
-        if (!ParseUnsigned(ds, &n))
+          if (!ParseUnsigned(ds, &n)) return;
-          return;
 
-        if (ds.Peek() == ',') {
+          if (ds.Peek() == ',') {
+            ds.Take();
+            if (ds.Peek() == '}')
+              m = kInfinityQuantifier;
+            else if (!ParseUnsigned(ds, &m) || m < n)
+              return;
+          } else
+            m = n;
+
+          if (!EvalQuantifier(operandStack, n, m) || ds.Peek() != '}') return;
           ds.Take();
-          if (ds.Peek() == '}')
+        } break;
-            m = kInfinityQuantifier;
-          else if (!ParseUnsigned(ds, &m) || m < n)
-            return;
-        } else
-          m = n;
 
-        if (!EvalQuantifier(operandStack, n, m) || ds.Peek() != '}')
+        case '.':
-          return;
+          PushOperand(operandStack, kAnyCharacterClass);
-        ds.Take();
+          ImplicitConcatenation(atomCountStack, operatorStack);
-      } break;
+          break;
 
-      case '.':
+        case '[': {
-        PushOperand(operandStack, kAnyCharacterClass);
+          SizeType range;
-        ImplicitConcatenation(atomCountStack, operatorStack);
+          if (!ParseRange(ds, &range)) return;
-        break;
+          SizeType s = NewState(kRegexInvalidState, kRegexInvalidState,
+                                kRangeCharacterClass);
+          GetState(s).rangeStart = range;
+          *operandStack.template Push<Frag>() = Frag(s, s, s);
+        }
+          ImplicitConcatenation(atomCountStack, operatorStack);
+          break;
 
-      case '[': {
+        case '\\':  // Escape character
-        SizeType range;
+          if (!CharacterEscape(ds, &codepoint))
-        if (!ParseRange(ds, &range))
+            return;  // Unsupported escape character
-          return;
+          // fall through to default
-        SizeType s = NewState(kRegexInvalidState, kRegexInvalidState,
+          RAPIDJSON_DELIBERATE_FALLTHROUGH;
-                              kRangeCharacterClass);
-        GetState(s).rangeStart = range;
-        *operandStack.template Push<Frag>() = Frag(s, s, s);
-      }
-        ImplicitConcatenation(atomCountStack, operatorStack);
-        break;
 
-      case '\\': // Escape character
+        default:  // Pattern character
-        if (!CharacterEscape(ds, &codepoint))
+          PushOperand(operandStack, codepoint);
-          return; // Unsupported escape character
+          ImplicitConcatenation(atomCountStack, operatorStack);
-        // fall through to default
-        RAPIDJSON_DELIBERATE_FALLTHROUGH;
-
-      default: // Pattern character
-        PushOperand(operandStack, codepoint);
-        ImplicitConcatenation(atomCountStack, operatorStack);
       }
     }
 
     while (!operatorStack.Empty())
-      if (!Eval(operandStack, *operatorStack.template Pop<Operator>(1)))
+      if (!Eval(operandStack, *operatorStack.template Pop<Operator>(1))) return;
-        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)
+    while (GetState(l1).out != kRegexInvalidState) l1 = GetState(l1).out;
-      l1 = GetState(l1).out;
     GetState(l1).out = l2;
     return old;
   }
@@ -355,61 +352,61 @@ private:
 
   bool Eval(Stack<Allocator> &operandStack, Operator op) {
     switch (op) {
-    case kConcatenation:
+      case kConcatenation:
-      RAPIDJSON_ASSERT(operandStack.GetSize() >= sizeof(Frag) * 2);
+        RAPIDJSON_ASSERT(operandStack.GetSize() >= sizeof(Frag) * 2);
-      {
+        {
-        Frag e2 = *operandStack.template Pop<Frag>(1);
+          Frag e2 = *operandStack.template Pop<Frag>(1);
-        Frag e1 = *operandStack.template Pop<Frag>(1);
+          Frag e1 = *operandStack.template Pop<Frag>(1);
-        Patch(e1.out, e2.start);
+          Patch(e1.out, e2.start);
-        *operandStack.template Push<Frag>() =
+          *operandStack.template Push<Frag>() =
-            Frag(e1.start, e2.out, Min(e1.minIndex, e2.minIndex));
+              Frag(e1.start, e2.out, Min(e1.minIndex, e2.minIndex));
-      }
+        }
-      return true;
-
-    case kAlternation:
-      if (operandStack.GetSize() >= sizeof(Frag) * 2) {
-        Frag e2 = *operandStack.template Pop<Frag>(1);
-        Frag e1 = *operandStack.template Pop<Frag>(1);
-        SizeType s = NewState(e1.start, e2.start, 0);
-        *operandStack.template Push<Frag>() =
-            Frag(s, Append(e1.out, e2.out), Min(e1.minIndex, e2.minIndex));
         return true;
-      }
-      return false;
 
-    case kZeroOrOne:
+      case kAlternation:
-      if (operandStack.GetSize() >= sizeof(Frag)) {
+        if (operandStack.GetSize() >= sizeof(Frag) * 2) {
-        Frag e = *operandStack.template Pop<Frag>(1);
+          Frag e2 = *operandStack.template Pop<Frag>(1);
-        SizeType s = NewState(kRegexInvalidState, e.start, 0);
+          Frag e1 = *operandStack.template Pop<Frag>(1);
-        *operandStack.template Push<Frag>() =
+          SizeType s = NewState(e1.start, e2.start, 0);
-            Frag(s, Append(e.out, s), e.minIndex);
+          *operandStack.template Push<Frag>() =
-        return true;
+              Frag(s, Append(e1.out, e2.out), Min(e1.minIndex, e2.minIndex));
-      }
+          return true;
-      return false;
+        }
+        return false;
 
-    case kZeroOrMore:
+      case kZeroOrOne:
-      if (operandStack.GetSize() >= sizeof(Frag)) {
+        if (operandStack.GetSize() >= sizeof(Frag)) {
-        Frag e = *operandStack.template Pop<Frag>(1);
+          Frag e = *operandStack.template Pop<Frag>(1);
-        SizeType s = NewState(kRegexInvalidState, e.start, 0);
+          SizeType s = NewState(kRegexInvalidState, e.start, 0);
-        Patch(e.out, s);
+          *operandStack.template Push<Frag>() =
-        *operandStack.template Push<Frag>() = Frag(s, s, e.minIndex);
+              Frag(s, Append(e.out, s), e.minIndex);
-        return true;
+          return true;
-      }
+        }
-      return false;
+        return false;
 
-    case kOneOrMore:
+      case kZeroOrMore:
-      if (operandStack.GetSize() >= sizeof(Frag)) {
+        if (operandStack.GetSize() >= sizeof(Frag)) {
-        Frag e = *operandStack.template Pop<Frag>(1);
+          Frag e = *operandStack.template Pop<Frag>(1);
-        SizeType s = NewState(kRegexInvalidState, e.start, 0);
+          SizeType s = NewState(kRegexInvalidState, e.start, 0);
-        Patch(e.out, s);
+          Patch(e.out, s);
-        *operandStack.template Push<Frag>() = Frag(e.start, s, e.minIndex);
+          *operandStack.template Push<Frag>() = Frag(s, s, e.minIndex);
-        return true;
+          return true;
-      }
+        }
-      return false;
+        return false;
 
-    default:
+      case kOneOrMore:
-      // syntax error (e.g. unclosed kLeftParenthesis)
+        if (operandStack.GetSize() >= sizeof(Frag)) {
-      return false;
+          Frag e = *operandStack.template Pop<Frag>(1);
+          SizeType s = NewState(kRegexInvalidState, e.start, 0);
+          Patch(e.out, s);
+          *operandStack.template Push<Frag>() = Frag(e.start, s, e.minIndex);
+          return true;
+        }
+        return false;
+
+      default:
+        // syntax error (e.g. unclosed kLeftParenthesis)
+        return false;
     }
   }
 
@@ -418,37 +415,37 @@ private:
     RAPIDJSON_ASSERT(operandStack.GetSize() >= sizeof(Frag));
 
     if (n == 0) {
-      if (m == 0) // a{0} not support
+      if (m == 0)  // a{0} not support
         return false;
       else if (m == kInfinityQuantifier)
-        Eval(operandStack, kZeroOrMore); // a{0,} -> a*
+        Eval(operandStack, kZeroOrMore);  // a{0,} -> a*
       else {
-        Eval(operandStack, kZeroOrOne); // a{0,5} -> a?
+        Eval(operandStack, kZeroOrOne);  // a{0,5} -> a?
         for (unsigned i = 0; i < m - 1; i++)
-          CloneTopOperand(operandStack); // a{0,5} -> a? a? a? a? a?
+          CloneTopOperand(operandStack);  // a{0,5} -> a? a? a? a? a?
         for (unsigned i = 0; i < m - 1; i++)
-          Eval(operandStack, kConcatenation); // a{0,5} -> a?a?a?a?a?
+          Eval(operandStack, kConcatenation);  // a{0,5} -> a?a?a?a?a?
       }
       return true;
     }
 
-    for (unsigned i = 0; i < n - 1; i++) // a{3} -> a a a
+    for (unsigned i = 0; i < n - 1; i++)  // a{3} -> a a a
       CloneTopOperand(operandStack);
 
     if (m == kInfinityQuantifier)
-      Eval(operandStack, kOneOrMore); // a{3,} -> a a a+
+      Eval(operandStack, kOneOrMore);  // a{3,} -> a a a+
     else if (m > n) {
-      CloneTopOperand(operandStack);  // a{3,5} -> a a a a
+      CloneTopOperand(operandStack);   // a{3,5} -> a a a a
-      Eval(operandStack, kZeroOrOne); // a{3,5} -> a a a a?
+      Eval(operandStack, kZeroOrOne);  // a{3,5} -> a a a a?
       for (unsigned i = n; i < m - 1; i++)
-        CloneTopOperand(operandStack); // a{3,5} -> a a a a? a?
+        CloneTopOperand(operandStack);  // a{3,5} -> a a a a? a?
       for (unsigned i = n; i < m; i++)
-        Eval(operandStack, kConcatenation); // a{3,5} -> a a aa?a?
+        Eval(operandStack, kConcatenation);  // a{3,5} -> a a aa?a?
     }
 
     for (unsigned i = 0; i < n - 1; i++)
       Eval(operandStack,
-           kConcatenation); // a{3} -> aaa, a{3,} -> aaa+, a{3.5} -> aaaa?a?
+           kConcatenation);  // a{3} -> aaa, a{3,} -> aaa+, a{3.5} -> aaaa?a?
 
     return true;
   }
@@ -458,17 +455,15 @@ private:
   void CloneTopOperand(Stack<Allocator> &operandStack) {
     const Frag src =
         *operandStack
-             .template Top<Frag>(); // Copy constructor to prevent invalidation
+             .template Top<Frag>();  // Copy constructor to prevent invalidation
     SizeType count =
-        stateCount_ - src.minIndex; // Assumes top operand contains states in
+        stateCount_ - src.minIndex;  // Assumes top operand contains states in
-                                    // [src->minIndex, stateCount_)
+                                     // [src->minIndex, stateCount_)
     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)
+      if (s[j].out != kRegexInvalidState) s[j].out += count;
-        s[j].out += count;
+      if (s[j].out1 != kRegexInvalidState) s[j].out1 += 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,11 +473,10 @@ private:
   template <typename InputStream>
   bool ParseUnsigned(DecodedStream<InputStream, Encoding> &ds, unsigned *u) {
     unsigned r = 0;
-    if (ds.Peek() < '0' || ds.Peek() > '9')
+    if (ds.Peek() < '0' || ds.Peek() > '9') return false;
-      return false;
     while (ds.Peek() >= '0' && ds.Peek() <= '9') {
-      if (r >= 429496729 && ds.Peek() > '5') // 2^32 - 1 = 4294967295
+      if (r >= 429496729 && ds.Peek() > '5')  // 2^32 - 1 = 4294967295
-        return false;                        // overflow
+        return false;                         // overflow
       r = r * 10 + (ds.Take() - '0');
     }
     *u = r;
@@ -507,54 +501,51 @@ private:
       }
 
       switch (codepoint) {
-      case ']':
+        case ']':
-        if (start == kRegexInvalidRange)
+          if (start == kRegexInvalidRange)
-          return false;  // Error: nothing inside []
+            return false;   // Error: nothing inside []
-        if (step == 2) { // Add trailing '-'
+          if (step == 2) {  // Add trailing '-'
-          SizeType r = NewRange('-');
+            SizeType r = NewRange('-');
-          RAPIDJSON_ASSERT(current != kRegexInvalidRange);
+            RAPIDJSON_ASSERT(current != kRegexInvalidRange);
-          GetRange(current).next = r;
+            GetRange(current).next = r;
-        }
-        if (negate)
-          GetRange(start).start |= kRangeNegationFlag;
-        *range = start;
-        return true;
-
-      case '\\':
-        if (ds.Peek() == 'b') {
-          ds.Take();
-          codepoint = 0x0008; // Escape backspace character
-        } else if (!CharacterEscape(ds, &codepoint))
-          return false;
-        // fall through to default
-        RAPIDJSON_DELIBERATE_FALLTHROUGH;
-
-      default:
-        switch (step) {
-        case 1:
-          if (codepoint == '-') {
-            step++;
-            break;
           }
-          // fall through to step 0 for other characters
+          if (negate) GetRange(start).start |= kRangeNegationFlag;
+          *range = start;
+          return true;
+
+        case '\\':
+          if (ds.Peek() == 'b') {
+            ds.Take();
+            codepoint = 0x0008;  // Escape backspace character
+          } else if (!CharacterEscape(ds, &codepoint))
+            return false;
+          // fall through to default
           RAPIDJSON_DELIBERATE_FALLTHROUGH;
 
-        case 0: {
-          SizeType r = NewRange(codepoint);
-          if (current != kRegexInvalidRange)
-            GetRange(current).next = r;
-          if (start == kRegexInvalidRange)
-            start = r;
-          current = r;
-        }
-          step = 1;
-          break;
-
         default:
-          RAPIDJSON_ASSERT(step == 2);
+          switch (step) {
-          GetRange(current).end = codepoint;
+            case 1:
-          step = 0;
+              if (codepoint == '-') {
-        }
+                step++;
+                break;
+              }
+              // fall through to step 0 for other characters
+              RAPIDJSON_DELIBERATE_FALLTHROUGH;
+
+            case 0: {
+              SizeType r = NewRange(codepoint);
+              if (current != kRegexInvalidRange) GetRange(current).next = r;
+              if (start == kRegexInvalidRange) start = r;
+              current = r;
+            }
+              step = 1;
+              break;
+
+            default:
+              RAPIDJSON_ASSERT(step == 2);
+              GetRange(current).end = codepoint;
+              step = 0;
+          }
       }
     }
     return false;
@@ -572,39 +563,39 @@ private:
                        unsigned *escapedCodepoint) {
     unsigned codepoint;
     switch (codepoint = ds.Take()) {
-    case '^':
+      case '^':
-    case '$':
+      case '$':
-    case '|':
+      case '|':
-    case '(':
+      case '(':
-    case ')':
+      case ')':
-    case '?':
+      case '?':
-    case '*':
+      case '*':
-    case '+':
+      case '+':
-    case '.':
+      case '.':
-    case '[':
+      case '[':
-    case ']':
+      case ']':
-    case '{':
+      case '{':
-    case '}':
+      case '}':
-    case '\\':
+      case '\\':
-      *escapedCodepoint = codepoint;
+        *escapedCodepoint = codepoint;
-      return true;
+        return true;
-    case 'f':
+      case 'f':
-      *escapedCodepoint = 0x000C;
+        *escapedCodepoint = 0x000C;
-      return true;
+        return true;
-    case 'n':
+      case 'n':
-      *escapedCodepoint = 0x000A;
+        *escapedCodepoint = 0x000A;
-      return true;
+        return true;
-    case 'r':
+      case 'r':
-      *escapedCodepoint = 0x000D;
+        *escapedCodepoint = 0x000D;
-      return true;
+        return true;
-    case 't':
+      case 't':
-      *escapedCodepoint = 0x0009;
+        *escapedCodepoint = 0x0009;
-      return true;
+        return true;
-    case 'v':
+      case 'v':
-      *escapedCodepoint = 0x000B;
+        *escapedCodepoint = 0x000B;
-      return true;
+        return true;
-    default:
+      default:
-      return false; // Unsupported escape character
+        return false;  // Unsupported escape character
     }
   }
 
@@ -625,16 +616,19 @@ private:
 
 template <typename RegexType, typename Allocator = CrtAllocator>
 class GenericRegexSearch {
-public:
+ public:
   typedef typename RegexType::EncodingType Encoding;
   typedef typename Encoding::Ch Ch;
 
   GenericRegexSearch(const RegexType &regex, Allocator *allocator = 0)
-      : regex_(regex), allocator_(allocator), ownAllocator_(0),
+      : regex_(regex),
-        state0_(allocator, 0), state1_(allocator, 0), stateSet_() {
+        allocator_(allocator),
+        ownAllocator_(0),
+        state0_(allocator, 0),
+        state1_(allocator, 0),
+        stateSet_() {
     RAPIDJSON_ASSERT(regex_.IsValid());
-    if (!allocator_)
+    if (!allocator_) ownAllocator_ = allocator_ = RAPIDJSON_NEW(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_);
   }
 
@@ -663,7 +659,7 @@ public:
     return Search(is);
   }
 
-private:
+ private:
   typedef typename RegexType::State State;
   typedef typename RegexType::Range Range;
 
@@ -690,11 +686,9 @@ private:
             (sr.codepoint == RegexType::kRangeCharacterClass &&
              MatchRange(sr.rangeStart, codepoint))) {
           matched = AddState(*next, sr.out) || matched;
-          if (!anchorEnd && matched)
+          if (!anchorEnd && matched) return true;
-            return true;
         }
-        if (!anchorBegin)
+        if (!anchorBegin) AddState(*next, regex_.root_);
-          AddState(*next, regex_.root_);
       }
       internal::Swap(current, next);
     }
@@ -709,7 +703,7 @@ private:
     RAPIDJSON_ASSERT(index != kRegexInvalidState);
 
     const State &s = regex_.GetState(index);
-    if (s.out1 != kRegexInvalidState) { // Split
+    if (s.out1 != kRegexInvalidState) {  // Split
       bool matched = AddState(l, s.out);
       return AddState(l, s.out1) || matched;
     } else if (!(stateSet_[index >> 5] & (1u << (index & 31)))) {
@@ -717,8 +711,8 @@ private:
       *l.template PushUnsafe<SizeType>() = index;
     }
     return s.out ==
-           kRegexInvalidState; // by using PushUnsafe() above, we can ensure s
+           kRegexInvalidState;  // by using PushUnsafe() above, we can ensure s
-                               // is not validated due to reallocation.
+                                // is not validated due to reallocation.
   }
 
   bool MatchRange(SizeType rangeIndex, unsigned codepoint) const {
@@ -745,7 +739,7 @@ private:
 typedef GenericRegex<UTF8<>> Regex;
 typedef GenericRegexSearch<Regex> RegexSearch;
 
-} // namespace internal
+}  // namespace internal
 RAPIDJSON_NAMESPACE_END
 
 #ifdef __GNUC__
@@ -756,4 +750,4 @@ RAPIDJSON_DIAG_POP
 RAPIDJSON_DIAG_POP
 #endif
 
-#endif // RAPIDJSON_INTERNAL_REGEX_H_
+#endif  // RAPIDJSON_INTERNAL_REGEX_H_

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>
-public:
+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),
+      : allocator_(allocator),
-        stackEnd_(0), initialCapacity_(stackCapacity) {}
+        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_),
+      : allocator_(rhs.allocator_),
-        stack_(rhs.stack_), stackTop_(rhs.stackTop_), stackEnd_(rhs.stackEnd_),
+        ownAllocator_(rhs.ownAllocator_),
+        stack_(rhs.stack_),
+        stackTop_(rhs.stackTop_),
+        stackEnd_(rhs.stackEnd_),
         initialCapacity_(rhs.initialCapacity_) {
     rhs.allocator_ = 0;
     rhs.ownAllocator_ = 0;
@@ -98,7 +106,7 @@ public:
   void ShrinkToFit() {
     if (Empty()) {
       // If the stack is empty, completely deallocate the memory.
-      Allocator::Free(stack_); // NOLINT (+clang-analyzer-unix.Malloc)
+      Allocator::Free(stack_);  // NOLINT (+clang-analyzer-unix.Malloc)
       stack_ = 0;
       stackTop_ = 0;
       stackEnd_ = 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>
-
+  T *End() {
-  template <typename T> const T *End() const {
     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_);
   }
 
@@ -169,28 +191,27 @@ public:
   size_t GetSize() const { return static_cast<size_t>(stackTop_ - stack_); }
   size_t GetCapacity() const { return static_cast<size_t>(stackEnd_ - stack_); }
 
-private:
+ 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_)
+      if (!allocator_) ownAllocator_ = allocator_ = RAPIDJSON_NEW(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)
+    if (newCapacity < newSize) newCapacity = newSize;
-      newCapacity = newSize;
 
     Resize(newCapacity);
   }
 
   void Resize(size_t newCapacity) {
-    const size_t size = GetSize(); // Backup the current size
+    const size_t size = GetSize();  // Backup the current size
     stack_ = static_cast<char *>(
         allocator_->Realloc(stack_, GetCapacity(), newCapacity));
     stackTop_ = stack_ + size;
@@ -199,7 +220,7 @@ private:
 
   void Destroy() {
     Allocator::Free(stack_);
-    RAPIDJSON_DELETE(ownAllocator_); // Only delete if it is owned by the stack
+    RAPIDJSON_DELETE(ownAllocator_);  // Only delete if it is owned by the stack
   }
 
   // Prohibit copy constructor & assignment operator.
@@ -214,11 +235,11 @@ private:
   size_t initialCapacity_;
 };
 
-} // namespace internal
+}  // namespace internal
 RAPIDJSON_NAMESPACE_END
 
 #if defined(__clang__)
 RAPIDJSON_DIAG_POP
 #endif
 
-#endif // RAPIDJSON_STACK_H_
+#endif  // RAPIDJSON_STACK_H_

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)
+  while (*p) ++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,15 +61,14 @@ bool CountStringCodePoint(const typename Encoding::Ch *s, SizeType length,
   SizeType count = 0;
   while (is.src_ < end) {
     unsigned codepoint;
-    if (!Encoding::Decode(is, &codepoint))
+    if (!Encoding::Decode(is, &codepoint)) return false;
-      return false;
     count++;
   }
   *outCount = count;
   return true;
 }
 
-} // namespace internal
+}  // namespace internal
 RAPIDJSON_NAMESPACE_END
 
-#endif // RAPIDJSON_INTERNAL_STRFUNC_H_
+#endif  // RAPIDJSON_INTERNAL_STRFUNC_H_

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)
+  if (m > b) m = b;
-    m = b;
+  if (m > c) m = c;
-  if (m > c)
-    m = c;
   return m;
 }
 
@@ -127,7 +126,7 @@ inline bool StrtodFast(double d, int p, double *result) {
     p = 22;
   }
 
-  if (p >= -22 && p <= 22 && d <= 9007199254740991.0) { // 2^53 - 1
+  if (p >= -22 && p <= 22 && d <= 9007199254740991.0) {  // 2^53 - 1
     *result = FastPath(d, p);
     return true;
   } else
@@ -138,8 +137,8 @@ inline bool StrtodFast(double d, int p, double *result) {
 inline bool StrtodDiyFp(const char *decimals, int dLen, int dExp,
                         double *result) {
   uint64_t significand = 0;
-  int i = 0; // 2^64 - 1 = 18446744073709551615, 1844674407370955161 =
+  int i = 0;  // 2^64 - 1 = 18446744073709551615, 1844674407370955161 =
-             // 0x1999999999999999
+              // 0x1999999999999999
   for (; i < dLen; i++) {
     if (significand > RAPIDJSON_UINT64_C2(0x19999999, 0x99999999) ||
         (significand == RAPIDJSON_UINT64_C2(0x19999999, 0x99999999) &&
@@ -148,7 +147,7 @@ inline bool StrtodDiyFp(const char *decimals, int dLen, int dExp,
     significand = significand * 10u + static_cast<unsigned>(decimals[i] - '0');
   }
 
-  if (i < dLen && decimals[i] >= '5') // Rounding
+  if (i < dLen && decimals[i] >= '5')  // Rounding
     significand++;
 
   int remaining = dLen - i;
@@ -166,18 +165,19 @@ inline bool StrtodDiyFp(const char *decimals, int dLen, int dExp,
   DiyFp cachedPower = GetCachedPower10(dExp, &actualExp);
   if (actualExp != dExp) {
     static const DiyFp kPow10[] = {
-        DiyFp(RAPIDJSON_UINT64_C2(0xa0000000, 0x00000000), -60), // 10^1
+        DiyFp(RAPIDJSON_UINT64_C2(0xa0000000, 0x00000000), -60),  // 10^1
-        DiyFp(RAPIDJSON_UINT64_C2(0xc8000000, 0x00000000), -57), // 10^2
+        DiyFp(RAPIDJSON_UINT64_C2(0xc8000000, 0x00000000), -57),  // 10^2
-        DiyFp(RAPIDJSON_UINT64_C2(0xfa000000, 0x00000000), -54), // 10^3
+        DiyFp(RAPIDJSON_UINT64_C2(0xfa000000, 0x00000000), -54),  // 10^3
-        DiyFp(RAPIDJSON_UINT64_C2(0x9c400000, 0x00000000), -50), // 10^4
+        DiyFp(RAPIDJSON_UINT64_C2(0x9c400000, 0x00000000), -50),  // 10^4
-        DiyFp(RAPIDJSON_UINT64_C2(0xc3500000, 0x00000000), -47), // 10^5
+        DiyFp(RAPIDJSON_UINT64_C2(0xc3500000, 0x00000000), -47),  // 10^5
-        DiyFp(RAPIDJSON_UINT64_C2(0xf4240000, 0x00000000), -44), // 10^6
+        DiyFp(RAPIDJSON_UINT64_C2(0xf4240000, 0x00000000), -44),  // 10^6
-        DiyFp(RAPIDJSON_UINT64_C2(0x98968000, 0x00000000), -40)  // 10^7
+        DiyFp(RAPIDJSON_UINT64_C2(0x98968000, 0x00000000), -40)   // 10^7
     };
     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;
   }
 
@@ -207,7 +207,7 @@ inline bool StrtodDiyFp(const char *decimals, int dLen, int dExp,
   if (precisionBits >= halfWay + static_cast<unsigned>(error)) {
     rounded.f++;
     if (rounded.f & (DiyFp::kDpHiddenBit
-                     << 1)) { // rounding overflows mantissa (issue #340)
+                     << 1)) {  // rounding overflows mantissa (issue #340)
       rounded.f >>= 1;
       rounded.e++;
     }
@@ -226,14 +226,14 @@ inline double StrtodBigInteger(double approx, const char *decimals, int dLen,
   Double a(approx);
   int cmp = CheckWithinHalfULP(a.Value(), dInt, dExp);
   if (cmp < 0)
-    return a.Value(); // within half ULP
+    return a.Value();  // within half ULP
   else if (cmp == 0) {
     // Round towards even
     if (a.Significand() & 1)
       return a.NextPositiveDouble();
     else
       return a.Value();
-  } else // adjustment
+  } else  // adjustment
     return a.NextPositiveDouble();
 }
 
@@ -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))
+  if (StrtodFast(d, p, &result)) return result;
-    return result;
 
   RAPIDJSON_ASSERT(length <= INT_MAX);
   int dLen = static_cast<int>(length);
@@ -272,7 +271,7 @@ inline double StrtodFullPrecision(double d, int p, const char *decimals,
     dExp++;
   }
 
-  if (dLen == 0) { // Buffer only contains zeros.
+  if (dLen == 0) {  // Buffer only contains zeros.
     return 0.0;
   }
 
@@ -285,23 +284,20 @@ 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)
+  if (dLen + dExp <= -324) return 0.0;
-    return 0.0;
 
   // If too large, overflow to infinity.
   // Any x >= 10^309 is interpreted as +infinity.
-  if (dLen + dExp > 309)
+  if (dLen + dExp > 309) return std::numeric_limits<double>::infinity();
-    return std::numeric_limits<double>::infinity();
 
-  if (StrtodDiyFp(decimals, dLen, dExp, &result))
+  if (StrtodDiyFp(decimals, dLen, dExp, &result)) return result;
-    return result;
 
   // Use approximation from StrtodDiyFp and make adjustment with BigInteger
   // comparison
   return StrtodBigInteger(result, decimals, dLen, dExp);
 }
 
-} // namespace internal
+}  // namespace internal
 RAPIDJSON_NAMESPACE_END
 
-#endif // RAPIDJSON_STRTOD_
+#endif  // RAPIDJSON_STRTOD_

livox_ros_driver/common/rapidjson/internal/swap.h

@@ -33,17 +33,18 @@ 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;
 }
 
-} // namespace internal
+}  // namespace internal
 RAPIDJSON_NAMESPACE_END
 
 #if defined(__clang__)
 RAPIDJSON_DIAG_POP
 #endif
 
-#endif // RAPIDJSON_INTERNAL_SWAP_H_
+#endif  // RAPIDJSON_INTERNAL_SWAP_H_

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(
+RAPIDJSON_DIAG_OFF(4351)  // new behavior: elements of array 'array' will be
-    4351) // new behavior: elements of array 'array' will be default initialized
+                          // default initialized
 #endif
 
 RAPIDJSON_NAMESPACE_BEGIN
@@ -50,8 +50,9 @@ RAPIDJSON_NAMESPACE_BEGIN
     \tparam StreamType Class derived from \c std::basic_istream.
 */
 
-template <typename StreamType> class BasicIStreamWrapper {
+template <typename StreamType>
-public:
+class BasicIStreamWrapper {
+ public:
   typedef typename StreamType::char_type Ch;
 
   //! Constructor.
@@ -59,8 +60,14 @@ public:
       \param stream stream opened for read.
   */
   BasicIStreamWrapper(StreamType &stream)
-      : stream_(stream), buffer_(peekBuffer_), bufferSize_(4), bufferLast_(0),
+      : stream_(stream),
-        current_(buffer_), readCount_(0), count_(0), eof_(false) {
+        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),
+      : stream_(stream),
-        bufferLast_(0), current_(buffer_), readCount_(0), count_(0),
+        buffer_(buffer),
+        bufferSize_(bufferSize),
+        bufferLast_(0),
+        current_(buffer_),
+        readCount_(0),
+        count_(0),
         eof_(false) {
     RAPIDJSON_ASSERT(bufferSize >= 4);
     Read();
@@ -105,7 +117,7 @@ public:
     return (current_ + 4 - !eof_ <= bufferLast_) ? current_ : 0;
   }
 
-private:
+ private:
   BasicIStreamWrapper();
   BasicIStreamWrapper(const BasicIStreamWrapper &);
   BasicIStreamWrapper &operator=(const BasicIStreamWrapper &);
@@ -133,7 +145,7 @@ private:
   Ch *bufferLast_;
   Ch *current_;
   size_t readCount_;
-  size_t count_; //!< Number of characters read
+  size_t count_;  //!< Number of characters read
   bool eof_;
 };
 
@@ -146,4 +158,4 @@ RAPIDJSON_DIAG_POP
 
 RAPIDJSON_NAMESPACE_END
 
-#endif // RAPIDJSON_ISTREAMWRAPPER_H_
+#endif  // RAPIDJSON_ISTREAMWRAPPER_H_

livox_ros_driver/common/rapidjson/memorybuffer.h

@@ -40,8 +40,9 @@ RAPIDJSON_NAMESPACE_BEGIN
     \tparam Allocator type for allocating memory buffer.
     \note implements Stream concept
 */
-template <typename Allocator = CrtAllocator> struct GenericMemoryBuffer {
+template <typename Allocator = CrtAllocator>
-  typedef char Ch; // byte
+struct GenericMemoryBuffer {
+  typedef char Ch;  // byte
 
   GenericMemoryBuffer(Allocator *allocator = 0,
                       size_t capacity = kDefaultCapacity)
@@ -67,10 +68,11 @@ 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));
 }
 
 RAPIDJSON_NAMESPACE_END
 
-#endif // RAPIDJSON_MEMORYBUFFER_H_
+#endif  // RAPIDJSON_MEMORYBUFFER_H_

livox_ros_driver/common/rapidjson/memorystream.h

@@ -46,7 +46,7 @@ RAPIDJSON_NAMESPACE_BEGIN
    Stream concept
 */
 struct MemoryStream {
-  typedef char Ch; // byte
+  typedef char Ch;  // byte
 
   MemoryStream(const Ch *src, size_t size)
       : src_(src), begin_(src), end_(src + size), size_(size) {}
@@ -69,10 +69,10 @@ struct MemoryStream {
   // For encoding detection only.
   const Ch *Peek4() const { return Tell() + 4 <= size_ ? src_ : 0; }
 
-  const Ch *src_;   //!< Current read position.
+  const Ch *src_;    //!< Current read position.
-  const Ch *begin_; //!< Original head of the string.
+  const Ch *begin_;  //!< Original head of the string.
-  const Ch *end_;   //!< End of stream.
+  const Ch *end_;    //!< End of stream.
-  size_t size_;     //!< Size of the stream.
+  size_t size_;      //!< Size of the stream.
 };
 
 RAPIDJSON_NAMESPACE_END
@@ -81,4 +81,4 @@ RAPIDJSON_NAMESPACE_END
 RAPIDJSON_DIAG_POP
 #endif
 
-#endif // RAPIDJSON_MEMORYBUFFER_H_
+#endif  // RAPIDJSON_MEMORYBUFFER_H_

livox_ros_driver/common/rapidjson/msinttypes/inttypes.h

@@ -34,11 +34,11 @@
 // THL A29 Limited ("Tencent Modifications").
 // All Tencent Modifications are Copyright (C) 2015 THL A29 Limited.
 
-#ifndef _MSC_VER // [
+#ifndef _MSC_VER  // [
 #error "Use this header only with Microsoft Visual C++ compilers!"
-#endif // _MSC_VER ]
+#endif  // _MSC_VER ]
 
-#ifndef _MSC_INTTYPES_H_ // [
+#ifndef _MSC_INTTYPES_H_  // [
 #define _MSC_INTTYPES_H_
 
 #if _MSC_VER > 1000
@@ -61,8 +61,8 @@ typedef struct {
 
 // 7.8.1 Macros for format specifiers
 
-#if !defined(__cplusplus) ||                                                   \
+#if !defined(__cplusplus) || \
-    defined(__STDC_FORMAT_MACROS) // [   See footnote 185 at page 198
+    defined(__STDC_FORMAT_MACROS)  // [   See footnote 185 at page 198
 
 // The fprintf macros for signed integers are:
 #define PRId8 "d"
@@ -194,13 +194,13 @@ typedef struct {
 #define SCNdMAX "I64d"
 #define SCNiMAX "I64i"
 
-#ifdef _WIN64 // [
+#ifdef _WIN64  // [
 #define SCNdPTR "I64d"
 #define SCNiPTR "I64i"
-#else // _WIN64 ][
+#else  // _WIN64 ][
 #define SCNdPTR "ld"
 #define SCNiPTR "li"
-#endif // _WIN64 ]
+#endif  // _WIN64 ]
 
 // The fscanf macros for unsigned integers are:
 #define SCNo8 "o"
@@ -260,19 +260,19 @@ typedef struct {
 #define SCNxMAX "I64x"
 #define SCNXMAX "I64X"
 
-#ifdef _WIN64 // [
+#ifdef _WIN64  // [
 #define SCNoPTR "I64o"
 #define SCNuPTR "I64u"
 #define SCNxPTR "I64x"
 #define SCNXPTR "I64X"
-#else // _WIN64 ][
+#else  // _WIN64 ][
 #define SCNoPTR "lo"
 #define SCNuPTR "lu"
 #define SCNxPTR "lx"
 #define SCNXPTR "lX"
-#endif // _WIN64 ]
+#endif  // _WIN64 ]
 
-#endif // __STDC_FORMAT_MACROS ]
+#endif  // __STDC_FORMAT_MACROS ]
 
 // 7.8.2 Functions for greatest-width integer types
 
@@ -283,11 +283,11 @@ typedef struct {
 
 // This is modified version of div() function from Microsoft's div.c found
 // in %MSVC.NET%\crt\src\div.c
-#ifdef STATIC_IMAXDIV // [
+#ifdef STATIC_IMAXDIV  // [
 static
-#else                 // STATIC_IMAXDIV ][
+#else                  // STATIC_IMAXDIV ][
 _inline
-#endif                // STATIC_IMAXDIV ]
+#endif                 // STATIC_IMAXDIV ]
     imaxdiv_t __cdecl imaxdiv(intmax_t numer, intmax_t denom) {
   imaxdiv_t result;
 
@@ -311,6 +311,6 @@ _inline
 #define wcstoimax _wcstoi64
 #define wcstoumax _wcstoui64
 
-#endif // _MSC_VER >= 1800
+#endif  // _MSC_VER >= 1800
 
-#endif // _MSC_INTTYPES_H_ ]
+#endif  // _MSC_INTTYPES_H_ ]

livox_ros_driver/common/rapidjson/msinttypes/stdint.h

@@ -34,11 +34,11 @@
 // THL A29 Limited ("Tencent Modifications").
 // All Tencent Modifications are Copyright (C) 2015 THL A29 Limited.
 
-#ifndef _MSC_VER // [
+#ifndef _MSC_VER  // [
 #error "Use this header only with Microsoft Visual C++ compilers!"
-#endif // _MSC_VER ]
+#endif  // _MSC_VER ]
 
-#ifndef _MSC_STDINT_H_ // [
+#ifndef _MSC_STDINT_H_  // [
 #define _MSC_STDINT_H_
 
 #if _MSC_VER > 1000
@@ -47,11 +47,11 @@
 
 // miloyip: Originally Visual Studio 2010 uses its own stdint.h. However it
 // generates warning with INT64_C(), so change to use this file for vs2010.
-#if _MSC_VER >= 1600 // [
+#if _MSC_VER >= 1600  // [
 #include <stdint.h>
 
-#if !defined(__cplusplus) ||                                                   \
+#if !defined(__cplusplus) || \
-    defined(__STDC_CONSTANT_MACROS) // [   See footnote 224 at page 260
+    defined(__STDC_CONSTANT_MACROS)  // [   See footnote 224 at page 260
 
 #undef INT8_C
 #undef INT16_C
@@ -77,16 +77,16 @@
 // 7.18.4.2 Macros for greatest-width integer constants
 // These #ifndef's are needed to prevent collisions with <boost/cstdint.hpp>.
 // Check out Issue 9 for the details.
-#ifndef INTMAX_C //   [
+#ifndef INTMAX_C  //   [
 #define INTMAX_C INT64_C
-#endif            // INTMAX_C    ]
+#endif             // INTMAX_C    ]
-#ifndef UINTMAX_C //  [
+#ifndef UINTMAX_C  //  [
 #define UINTMAX_C UINT64_C
-#endif // UINTMAX_C   ]
+#endif  // UINTMAX_C   ]
 
-#endif // __STDC_CONSTANT_MACROS ]
+#endif  // __STDC_CONSTANT_MACROS ]
 
-#else // ] _MSC_VER >= 1700 [
+#else  // ] _MSC_VER >= 1700 [
 
 #include <limits.h>
 
@@ -157,13 +157,13 @@ typedef uint32_t uint_fast32_t;
 typedef uint64_t uint_fast64_t;
 
 // 7.18.1.4 Integer types capable of holding object pointers
-#ifdef _WIN64 // [
+#ifdef _WIN64  // [
 typedef signed __int64 intptr_t;
 typedef unsigned __int64 uintptr_t;
-#else         // _WIN64 ][
+#else          // _WIN64 ][
 typedef _W64 signed int intptr_t;
 typedef _W64 unsigned int uintptr_t;
-#endif        // _WIN64 ]
+#endif  // _WIN64 ]
 
 // 7.18.1.5 Greatest-width integer types
 typedef int64_t intmax_t;
@@ -171,9 +171,9 @@ typedef uint64_t uintmax_t;
 
 // 7.18.2 Limits of specified-width integer types
 
-#if !defined(__cplusplus) ||                                                   \
+#if !defined(__cplusplus) || \
-    defined(__STDC_LIMIT_MACROS) // [   See footnote 220 at page 257 and
+    defined(__STDC_LIMIT_MACROS)  // [   See footnote 220 at page 257 and
-                                 // footnote 221 at page 259
+// footnote 221 at page 259
 
 // 7.18.2.1 Limits of exact-width integer types
 #define INT8_MIN ((int8_t)_I8_MIN)
@@ -218,15 +218,15 @@ typedef uint64_t uintmax_t;
 #define UINT_FAST64_MAX UINT64_MAX
 
 // 7.18.2.4 Limits of integer types capable of holding object pointers
-#ifdef _WIN64 // [
+#ifdef _WIN64  // [
 #define INTPTR_MIN INT64_MIN
 #define INTPTR_MAX INT64_MAX
 #define UINTPTR_MAX UINT64_MAX
-#else // _WIN64 ][
+#else  // _WIN64 ][
 #define INTPTR_MIN INT32_MIN
 #define INTPTR_MAX INT32_MAX
 #define UINTPTR_MAX UINT32_MAX
-#endif // _WIN64 ]
+#endif  // _WIN64 ]
 
 // 7.18.2.5 Limits of greatest-width integer types
 #define INTMAX_MIN INT64_MIN
@@ -235,42 +235,42 @@ typedef uint64_t uintmax_t;
 
 // 7.18.3 Limits of other integer types
 
-#ifdef _WIN64 // [
+#ifdef _WIN64  // [
 #define PTRDIFF_MIN _I64_MIN
 #define PTRDIFF_MAX _I64_MAX
-#else // _WIN64 ][
+#else  // _WIN64 ][
 #define PTRDIFF_MIN _I32_MIN
 #define PTRDIFF_MAX _I32_MAX
-#endif // _WIN64 ]
+#endif  // _WIN64 ]
 
 #define SIG_ATOMIC_MIN INT_MIN
 #define SIG_ATOMIC_MAX INT_MAX
 
-#ifndef SIZE_MAX // [
+#ifndef SIZE_MAX  // [
-#ifdef _WIN64    // [
+#ifdef _WIN64     // [
 #define SIZE_MAX _UI64_MAX
-#else // _WIN64 ][
+#else  // _WIN64 ][
 #define SIZE_MAX _UI32_MAX
-#endif            // _WIN64 ]
+#endif  // _WIN64 ]
-#endif            // SIZE_MAX ]
+#endif  // SIZE_MAX ]
 
 // WCHAR_MIN and WCHAR_MAX are also defined in <wchar.h>
-#ifndef WCHAR_MIN // [
+#ifndef WCHAR_MIN  // [
 #define WCHAR_MIN 0
-#endif            // WCHAR_MIN ]
+#endif             // WCHAR_MIN ]
-#ifndef WCHAR_MAX // [
+#ifndef WCHAR_MAX  // [
 #define WCHAR_MAX _UI16_MAX
-#endif // WCHAR_MAX ]
+#endif  // WCHAR_MAX ]
 
 #define WINT_MIN 0
 #define WINT_MAX _UI16_MAX
 
-#endif // __STDC_LIMIT_MACROS ]
+#endif  // __STDC_LIMIT_MACROS ]
 
 // 7.18.4 Limits of other integer types
 
-#if !defined(__cplusplus) ||                                                   \
+#if !defined(__cplusplus) || \
-    defined(__STDC_CONSTANT_MACROS) // [   See footnote 224 at page 260
+    defined(__STDC_CONSTANT_MACROS)  // [   See footnote 224 at page 260
 
 // 7.18.4.1 Macros for minimum-width integer constants
 
@@ -287,15 +287,15 @@ typedef uint64_t uintmax_t;
 // 7.18.4.2 Macros for greatest-width integer constants
 // These #ifndef's are needed to prevent collisions with <boost/cstdint.hpp>.
 // Check out Issue 9 for the details.
-#ifndef INTMAX_C //   [
+#ifndef INTMAX_C  //   [
 #define INTMAX_C INT64_C
-#endif            // INTMAX_C    ]
+#endif             // INTMAX_C    ]
-#ifndef UINTMAX_C //  [
+#ifndef UINTMAX_C  //  [
 #define UINTMAX_C UINT64_C
-#endif // UINTMAX_C   ]
+#endif  // UINTMAX_C   ]
 
-#endif // __STDC_CONSTANT_MACROS ]
+#endif  // __STDC_CONSTANT_MACROS ]
 
-#endif // _MSC_VER >= 1600 ]
+#endif  // _MSC_VER >= 1600 ]
 
-#endif // _MSC_STDINT_H_ ]
+#endif  // _MSC_STDINT_H_ ]

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,8 +45,9 @@ RAPIDJSON_NAMESPACE_BEGIN
     \tparam StreamType Class derived from \c std::basic_ostream.
 */
 
-template <typename StreamType> class BasicOStreamWrapper {
+template <typename StreamType>
-public:
+class BasicOStreamWrapper {
+ public:
   typedef typename StreamType::char_type Ch;
   BasicOStreamWrapper(StreamType &stream) : stream_(stream) {}
 
@@ -76,7 +77,7 @@ public:
     return 0;
   }
 
-private:
+ private:
   BasicOStreamWrapper(const BasicOStreamWrapper &);
   BasicOStreamWrapper &operator=(const BasicOStreamWrapper &);
 
@@ -92,4 +93,4 @@ RAPIDJSON_DIAG_POP
 
 RAPIDJSON_NAMESPACE_END
 
-#endif // RAPIDJSON_OSTREAMWRAPPER_H_
+#endif  // RAPIDJSON_OSTREAMWRAPPER_H_

livox_ros_driver/common/rapidjson/pointer.h

@@ -27,28 +27,29 @@ RAPIDJSON_DIAG_PUSH
 RAPIDJSON_DIAG_OFF(switch - enum)
 #elif defined(_MSC_VER)
 RAPIDJSON_DIAG_PUSH
-RAPIDJSON_DIAG_OFF(4512) // assignment operator could not be generated
+RAPIDJSON_DIAG_OFF(4512)  // assignment operator could not be generated
 #endif
 
 RAPIDJSON_NAMESPACE_BEGIN
 
 static const SizeType kPointerInvalidIndex =
-    ~SizeType(0); //!< Represents an invalid index in GenericPointer::Token
+    ~SizeType(0);  //!< Represents an invalid index in GenericPointer::Token
 
 //! Error code of parsing.
 /*! \ingroup RAPIDJSON_ERRORS
     \see GenericPointer::GenericPointer, GenericPointer::GetParseErrorCode
 */
 enum PointerParseErrorCode {
-  kPointerParseErrorNone = 0, //!< The parse is successful
+  kPointerParseErrorNone = 0,  //!< The parse is successful
 
-  kPointerParseErrorTokenMustBeginWithSolidus, //!< A token must begin with a
+  kPointerParseErrorTokenMustBeginWithSolidus,  //!< A token must begin with a
-                                               //!< '/'
+                                                //!< '/'
-  kPointerParseErrorInvalidEscape,             //!< Invalid escape
+  kPointerParseErrorInvalidEscape,              //!< Invalid escape
-  kPointerParseErrorInvalidPercentEncoding, //!< Invalid percent encoding in URI
+  kPointerParseErrorInvalidPercentEncoding,     //!< Invalid percent encoding in
-                                            //!< fragment
+                                                //!URI
-  kPointerParseErrorCharacterMustPercentEncode //!< A character must percent
+                                                //!< fragment
-                                               //!< encoded in URI fragment
+  kPointerParseErrorCharacterMustPercentEncode  //!< A character must percent
+                                                //!< encoded in URI fragment
 };
 
 ///////////////////////////////////////////////////////////////////////////////
@@ -87,10 +88,10 @@ enum PointerParseErrorCode {
 */
 template <typename ValueType, typename Allocator = CrtAllocator>
 class GenericPointer {
-public:
+ public:
   typedef typename ValueType::EncodingType
-      EncodingType;                  //!< Encoding type from Value
+      EncodingType;                   //!< Encoding type from Value
-  typedef typename ValueType::Ch Ch; //!< Character type from Value
+  typedef typename ValueType::Ch Ch;  //!< Character type from Value
 
   //! A token is the basic units of internal representation.
   /*!
@@ -107,11 +108,12 @@ 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
-                    //!< it can contain null character.
+        *name;  //!< Name of the token. It has null character at the end but
-    SizeType length; //!< Length of the name.
+                //!< it can contain null character.
-    SizeType index;  //!< A valid array index, if it is not equal to
+    SizeType length;  //!< Length of the name.
-                     //!< kPointerInvalidIndex.
+    SizeType index;   //!< A valid array index, if it is not equal to
+                      //!< kPointerInvalidIndex.
   };
 
   //!@name Constructors and destructor.
@@ -119,8 +121,12 @@ public:
 
   //! Default constructor.
   GenericPointer(Allocator *allocator = 0)
-      : allocator_(allocator), ownAllocator_(), nameBuffer_(), tokens_(),
+      : allocator_(allocator),
-        tokenCount_(), parseErrorOffset_(),
+        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_(),
+      : allocator_(allocator),
-        tokenCount_(), parseErrorOffset_(),
+        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_(),
+      : allocator_(allocator),
-        tokenCount_(), parseErrorOffset_(),
+        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_(),
+      : allocator_(allocator),
-        tokenCount_(), parseErrorOffset_(),
+        ownAllocator_(),
+        nameBuffer_(),
+        tokens_(),
+        tokenCount_(),
+        parseErrorOffset_(),
         parseErrorCode_(kPointerParseErrorNone) {
     Parse(source, length);
   }
@@ -192,30 +210,43 @@ public:
       \endcode
   */
   GenericPointer(const Token *tokens, size_t tokenCount)
-      : allocator_(), ownAllocator_(), nameBuffer_(),
+      : allocator_(),
-        tokens_(const_cast<Token *>(tokens)), tokenCount_(tokenCount),
+        ownAllocator_(),
-        parseErrorOffset_(), parseErrorCode_(kPointerParseErrorNone) {}
+        nameBuffer_(),
+        tokens_(const_cast<Token *>(tokens)),
+        tokenCount_(tokenCount),
+        parseErrorOffset_(),
+        parseErrorCode_(kPointerParseErrorNone) {}
 
   //! Copy constructor.
   GenericPointer(const GenericPointer &rhs)
-      : allocator_(rhs.allocator_), ownAllocator_(), nameBuffer_(), tokens_(),
+      : allocator_(rhs.allocator_),
-        tokenCount_(), parseErrorOffset_(),
+        ownAllocator_(),
+        nameBuffer_(),
+        tokens_(),
+        tokenCount_(),
+        parseErrorOffset_(),
         parseErrorCode_(kPointerParseErrorNone) {
     *this = rhs;
   }
 
   //! Copy constructor.
   GenericPointer(const GenericPointer &rhs, Allocator *allocator)
-      : allocator_(allocator), ownAllocator_(), nameBuffer_(), tokens_(),
+      : allocator_(allocator),
-        tokenCount_(), parseErrorOffset_(),
+        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,
-                     // is nullptr and tokens_ are not deallocated.
+                      // nameBuffer_
+                      // is nullptr and tokens_ are not deallocated.
       Allocator::Free(tokens_);
     RAPIDJSON_DELETE(ownAllocator_);
   }
@@ -224,17 +255,16 @@ public:
   GenericPointer &operator=(const GenericPointer &rhs) {
     if (this != &rhs) {
       // Do not delete ownAllcator
-      if (nameBuffer_)
+      if (nameBuffer_) Allocator::Free(tokens_);
-        Allocator::Free(tokens_);
 
       tokenCount_ = rhs.tokenCount_;
       parseErrorOffset_ = rhs.parseErrorOffset_;
       parseErrorCode_ = rhs.parseErrorCode_;
 
       if (rhs.nameBuffer_)
-        CopyFromRaw(rhs); // Normally parsed tokens.
+        CopyFromRaw(rhs);  // Normally parsed tokens.
       else {
-        tokens_ = rhs.tokens_; // User supplied const tokens.
+        tokens_ = rhs.tokens_;  // User supplied const tokens.
         nameBuffer_ = 0;
       }
     }
@@ -353,8 +383,7 @@ public:
       return Append(token, allocator);
     } else {
       Ch name[21];
-      for (size_t i = 0; i <= length; i++)
+      for (size_t i = 0; i <= length; i++) name[i] = static_cast<Ch>(buffer[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())
+    if (!IsValid()) return false;
-      return false;
+    if (!rhs.IsValid()) return true;
-    if (!rhs.IsValid())
-      return true;
 
-    if (tokenCount_ != rhs.tokenCount_)
+    if (tokenCount_ != rhs.tokenCount_) return 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);
   }
 
@@ -521,12 +548,11 @@ public:
         v = &((*v)[v->Size() - 1]);
         exist = false;
       } else {
-        if (t->index == kPointerInvalidIndex) { // must be object name
+        if (t->index == kPointerInvalidIndex) {  // must be object name
-          if (!v->IsObject())
+          if (!v->IsObject()) v->SetObject();    // Change to Object
-            v->SetObject(); // Change to Object
+        } else {                                 // object name or array index
-        } else {            // object name or array index
           if (!v->IsArray() && !v->IsObject())
-            v->SetArray(); // Change to Array
+            v->SetArray();  // Change to Array
         }
 
         if (v->IsArray()) {
@@ -545,7 +571,7 @@ public:
             v->AddMember(ValueType(t->name, t->length, allocator).Move(),
                          ValueType().Move(), allocator);
             m = v->MemberEnd();
-            v = &(--m)->value; // Assumes AddMember() appends at the end
+            v = &(--m)->value;  // Assumes AddMember() appends at the end
             exist = false;
           } else
             v = &m->value;
@@ -553,8 +579,7 @@ public:
       }
     }
 
-    if (alreadyExist)
+    if (alreadyExist) *alreadyExist = exist;
-      *alreadyExist = exist;
 
     return *v;
   }
@@ -566,10 +591,10 @@ public:
      already exist. \return The resolved newly created, or already exists value.
   */
   template <typename stackAllocator>
-  ValueType &
+  ValueType &Create(
-  Create(GenericDocument<EncodingType, typename ValueType::AllocatorType,
+      GenericDocument<EncodingType, typename ValueType::AllocatorType,
-                         stackAllocator> &document,
+                      stackAllocator> &document,
-         bool *alreadyExist = 0) const {
+      bool *alreadyExist = 0) const {
     return Create(document, document.GetAllocator(), alreadyExist);
   }
 
@@ -599,22 +624,20 @@ public:
     ValueType *v = &root;
     for (const Token *t = tokens_; t != tokens_ + tokenCount_; ++t) {
       switch (v->GetType()) {
-      case kObjectType: {
+        case kObjectType: {
-        typename ValueType::MemberIterator m =
+          typename ValueType::MemberIterator m =
-            v->FindMember(GenericValue<EncodingType>(
+              v->FindMember(GenericValue<EncodingType>(
-                GenericStringRef<Ch>(t->name, t->length)));
+                  GenericStringRef<Ch>(t->name, t->length)));
-        if (m == v->MemberEnd())
+          if (m == v->MemberEnd()) break;
+          v = &m->value;
+        }
+          continue;
+        case kArrayType:
+          if (t->index == kPointerInvalidIndex || t->index >= v->Size()) break;
+          v = &((*v)[t->index]);
+          continue;
+        default:
           break;
-        v = &m->value;
-      }
-        continue;
-      case kArrayType:
-        if (t->index == kPointerInvalidIndex || t->index >= v->Size())
-          break;
-        v = &((*v)[t->index]);
-        continue;
-      default:
-        break;
       }
 
       // Error: unresolved token
@@ -652,18 +675,18 @@ public:
      the values if the specified value or its parents are not exist. \see
      Create()
   */
-  ValueType &
+  ValueType &GetWithDefault(
-  GetWithDefault(ValueType &root, const ValueType &defaultValue,
+      ValueType &root, const ValueType &defaultValue,
-                 typename ValueType::AllocatorType &allocator) const {
+      typename ValueType::AllocatorType &allocator) const {
     bool alreadyExist;
     ValueType &v = Create(root, allocator, &alreadyExist);
     return alreadyExist ? v : v.CopyFrom(defaultValue, allocator);
   }
 
   //! Query a value in a subtree with default null-terminated string.
-  ValueType &
+  ValueType &GetWithDefault(
-  GetWithDefault(ValueType &root, const Ch *defaultValue,
+      ValueType &root, const Ch *defaultValue,
-                 typename ValueType::AllocatorType &allocator) const {
+      typename ValueType::AllocatorType &allocator) const {
     bool alreadyExist;
     ValueType &v = Create(root, allocator, &alreadyExist);
     return alreadyExist ? v : v.SetString(defaultValue, allocator);
@@ -671,9 +694,9 @@ public:
 
 #if RAPIDJSON_HAS_STDSTRING
   //! Query a value in a subtree with default std::basic_string.
-  ValueType &
+  ValueType &GetWithDefault(
-  GetWithDefault(ValueType &root, const std::basic_string<Ch> &defaultValue,
+      ValueType &root, const std::basic_string<Ch> &defaultValue,
-                 typename ValueType::AllocatorType &allocator) const {
+      typename ValueType::AllocatorType &allocator) const {
     bool alreadyExist;
     ValueType &v = Create(root, allocator, &alreadyExist);
     return alreadyExist ? v : v.SetString(defaultValue, allocator);
@@ -796,8 +819,8 @@ public:
 
   //! Set a value in a document, with move semantics.
   template <typename stackAllocator>
-  ValueType &
+  ValueType &Set(
-  Set(GenericDocument<EncodingType, typename ValueType::AllocatorType,
+      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 &
+  ValueType &Set(
-  Set(GenericDocument<EncodingType, typename ValueType::AllocatorType,
+      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 &
+  ValueType &Set(
-  Set(GenericDocument<EncodingType, typename ValueType::AllocatorType,
+      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 &
+  ValueType &Set(
-  Set(GenericDocument<EncodingType, typename ValueType::AllocatorType,
+      GenericDocument<EncodingType, typename ValueType::AllocatorType,
                       stackAllocator> &document,
       const std::basic_string<Ch> &value) const {
     return Create(document) = ValueType(value, document.GetAllocator()).Move();
@@ -870,10 +893,10 @@ public:
 
   //! Swap a value with a value in a document.
   template <typename stackAllocator>
-  ValueType &
+  ValueType &Swap(
-  Swap(GenericDocument<EncodingType, typename ValueType::AllocatorType,
+      GenericDocument<EncodingType, typename ValueType::AllocatorType,
-                       stackAllocator> &document,
+                      stackAllocator> &document,
-       ValueType &value) const {
+      ValueType &value) const {
     return Create(document).Swap(value);
   }
 
@@ -890,45 +913,44 @@ public:
   */
   bool Erase(ValueType &root) const {
     RAPIDJSON_ASSERT(IsValid());
-    if (tokenCount_ == 0) // Cannot erase the root
+    if (tokenCount_ == 0)  // Cannot erase the root
       return false;
 
     ValueType *v = &root;
     const Token *last = tokens_ + (tokenCount_ - 1);
     for (const Token *t = tokens_; t != last; ++t) {
       switch (v->GetType()) {
-      case kObjectType: {
+        case kObjectType: {
-        typename ValueType::MemberIterator m =
+          typename ValueType::MemberIterator m =
-            v->FindMember(GenericValue<EncodingType>(
+              v->FindMember(GenericValue<EncodingType>(
-                GenericStringRef<Ch>(t->name, t->length)));
+                  GenericStringRef<Ch>(t->name, t->length)));
-        if (m == v->MemberEnd())
+          if (m == v->MemberEnd()) return false;
+          v = &m->value;
+        } break;
+        case kArrayType:
+          if (t->index == kPointerInvalidIndex || t->index >= v->Size())
+            return false;
+          v = &((*v)[t->index]);
+          break;
+        default:
           return false;
-        v = &m->value;
-      } break;
-      case kArrayType:
-        if (t->index == kPointerInvalidIndex || t->index >= v->Size())
-          return false;
-        v = &((*v)[t->index]);
-        break;
-      default:
-        return false;
       }
     }
 
     switch (v->GetType()) {
-    case kObjectType:
+      case kObjectType:
-      return v->EraseMember(GenericStringRef<Ch>(last->name, last->length));
+        return v->EraseMember(GenericStringRef<Ch>(last->name, last->length));
-    case kArrayType:
+      case kArrayType:
-      if (last->index == kPointerInvalidIndex || last->index >= v->Size())
+        if (last->index == kPointerInvalidIndex || last->index >= v->Size())
+          return false;
+        v->Erase(v->Begin() + last->index);
+        return true;
+      default:
         return false;
-      v->Erase(v->Begin() + last->index);
-      return true;
-    default:
-      return false;
     }
   }
 
-private:
+ private:
   //! Clone the content from rhs to this.
   /*!
       \param rhs Source pointer.
@@ -939,10 +961,10 @@ private:
   */
   Ch *CopyFromRaw(const GenericPointer &rhs, size_t extraToken = 0,
                   size_t extraNameBufferSize = 0) {
-    if (!allocator_) // allocator is independently owned.
+    if (!allocator_)  // allocator is independently owned.
       ownAllocator_ = allocator_ = RAPIDJSON_NEW(Allocator)();
 
-    size_t nameBufferSize = rhs.tokenCount_; // null terminators for tokens
+    size_t nameBufferSize = rhs.tokenCount_;  // null terminators for tokens
     for (Token *t = rhs.tokens_; t != rhs.tokens_ + rhs.tokenCount_; ++t)
       nameBufferSize += t->length;
 
@@ -977,14 +999,16 @@ private:
              c == '~');
   }
 
-  //! Parse a JSON String or its URI fragment representation into tokens.
+//! Parse a JSON String or its URI fragment representation into tokens.
-#ifndef __clang__ // -Wdocumentation
+#ifndef __clang__  // -Wdocumentation
-                  /*!
+                   /*!
-                      \param source Either a JSON Pointer string, or its URI fragment
+                       \param source Either a JSON Pointer string, or its URI fragment
-                     representation. Not need to be null terminated.                 \param length Length of the
+                      representation. Not need to be null terminated.                 \param length
-                     source string.                 \note Source cannot be JSON String Representation of JSON
+                      Length of the
-                     Pointer, e.g. In "/\u0000", \u0000 will not be unescaped.
+                      source string.                 \note Source cannot be JSON String
-                  */
+                      Representation of JSON
+                      Pointer, e.g. In "/\u0000", \u0000 will not be unescaped.
+                   */
 #endif
   void Parse(const Ch *source, size_t length) {
     RAPIDJSON_ASSERT(source != NULL);
@@ -992,14 +1016,12 @@ private:
     RAPIDJSON_ASSERT(tokens_ == 0);
 
     // Create own allocator if user did not supply.
-    if (!allocator_)
+    if (!allocator_) ownAllocator_ = allocator_ = RAPIDJSON_NEW(Allocator)();
-      ownAllocator_ = allocator_ = RAPIDJSON_NEW(Allocator)();
 
     // Count number of '/' as tokenCount
     tokenCount_ = 0;
     for (const Ch *s = source; s != source + length; s++)
-      if (*s == '/')
+      if (*s == '/') tokenCount_++;
-        tokenCount_++;
 
     Token *token = tokens_ = static_cast<Token *>(
         allocator_->Malloc(tokenCount_ * sizeof(Token) + length * sizeof(Ch)));
@@ -1020,7 +1042,7 @@ private:
 
     while (i < length) {
       RAPIDJSON_ASSERT(source[i] == '/');
-      i++; // consumes '/'
+      i++;  // consumes '/'
 
       token->name = name;
       bool isNumber = true;
@@ -1076,15 +1098,13 @@ private:
         }
 
         // First check for index: all of characters are digit
-        if (c < '0' || c > '9')
+        if (c < '0' || c > '9') isNumber = false;
-          isNumber = false;
 
         *name++ = c;
       }
       token->length = static_cast<SizeType>(name - token->name);
-      if (token->length == 0)
+      if (token->length == 0) isNumber = false;
-        isNumber = false;
+      *name++ = '\0';  // Null terminator
-      *name++ = '\0'; // Null terminator
 
       // Second check for index: more than one digit cannot have leading zero
       if (isNumber && token->length > 1 && token->name[0] == '0')
@@ -1095,7 +1115,7 @@ private:
       if (isNumber) {
         for (size_t j = 0; j < token->length; j++) {
           SizeType m = n * 10 + static_cast<SizeType>(token->name[j] - '0');
-          if (m < n) { // overflow detection
+          if (m < n) {  // overflow detection
             isNumber = false;
             break;
           }
@@ -1108,7 +1128,7 @@ private:
     }
 
     RAPIDJSON_ASSERT(name <=
-                     nameBuffer_ + length); // Should not overflow buffer
+                     nameBuffer_ + length);  // Should not overflow buffer
     parseErrorCode_ = kPointerParseErrorNone;
     return;
 
@@ -1131,8 +1151,7 @@ private:
   bool Stringify(OutputStream &os) const {
     RAPIDJSON_ASSERT(IsValid());
 
-    if (uriFragment)
+    if (uriFragment) os.Put('#');
-      os.Put('#');
 
     for (Token *t = tokens_; t != tokens_ + tokenCount_; ++t) {
       os.Put('/');
@@ -1166,7 +1185,7 @@ private:
       mark invalid, and to be checked by IsValid().
   */
   class PercentDecodeStream {
-  public:
+   public:
     typedef typename ValueType::Ch Ch;
 
     //! Constructor
@@ -1178,7 +1197,7 @@ private:
         : src_(source), head_(source), end_(end), valid_(true) {}
 
     Ch Take() {
-      if (*src_ != '%' || src_ + 3 > end_) { // %XY triplet
+      if (*src_ != '%' || src_ + 3 > end_) {  // %XY triplet
         valid_ = false;
         return 0;
       }
@@ -1205,19 +1224,20 @@ private:
     size_t Tell() const { return static_cast<size_t>(src_ - head_); }
     bool IsValid() const { return valid_; }
 
-  private:
+   private:
-    const Ch *src_;  //!< Current read position.
+    const Ch *src_;   //!< Current read position.
-    const Ch *head_; //!< Original head of the string.
+    const Ch *head_;  //!< Original head of the string.
-    const Ch *end_;  //!< Past-the-end position.
+    const Ch *end_;   //!< Past-the-end position.
-    bool valid_;     //!< Whether the parsing is valid.
+    bool valid_;      //!< Whether the parsing is valid.
   };
 
   //! A helper stream to encode character (UTF-8 code unit) into percent-encoded
   //! sequence.
-  template <typename OutputStream> class PercentEncodeStream {
+  template <typename OutputStream>
-  public:
+  class PercentEncodeStream {
+   public:
     PercentEncodeStream(OutputStream &os) : os_(os) {}
-    void Put(char c) { // UTF-8 must be byte
+    void Put(char c) {  // UTF-8 must be byte
       unsigned char u = static_cast<unsigned char>(c);
       static const char hexDigits[16] = {'0', '1', '2', '3', '4', '5',
                                          '6', '7', '8', '9', 'A', 'B',
@@ -1227,18 +1247,18 @@ private:
       os_.Put(static_cast<typename OutputStream::Ch>(hexDigits[u & 15]));
     }
 
-  private:
+   private:
     OutputStream &os_;
   };
 
-  Allocator *allocator_; //!< The current allocator. It is either user-supplied
+  Allocator *allocator_;  //!< The current allocator. It is either user-supplied
-                         //!< or equal to ownAllocator_.
+                          //!< or equal to ownAllocator_.
-  Allocator *ownAllocator_; //!< Allocator owned by this Pointer.
+  Allocator *ownAllocator_;  //!< Allocator owned by this Pointer.
-  Ch *nameBuffer_;          //!< A buffer containing all names in tokens.
+  Ch *nameBuffer_;           //!< A buffer containing all names in tokens.
-  Token *tokens_;           //!< A list of tokens.
+  Token *tokens_;            //!< A list of tokens.
-  size_t tokenCount_;       //!< Number of tokens in tokens_.
+  size_t tokenCount_;        //!< Number of tokens in tokens_.
-  size_t parseErrorOffset_; //!< Offset in code unit when parsing fail.
+  size_t parseErrorOffset_;  //!< Offset in code unit when parsing fail.
-  PointerParseErrorCode parseErrorCode_; //!< Parsing error code.
+  PointerParseErrorCode parseErrorCode_;  //!< Parsing error code.
 };
 
 //! GenericPointer for Value (UTF-8, default allocator).
@@ -1250,10 +1270,9 @@ typedef GenericPointer<Value> Pointer;
 //////////////////////////////////////////////////////////////////////////////
 
 template <typename T>
-typename T::ValueType &
+typename T::ValueType &CreateValueByPointer(
-CreateValueByPointer(T &root,
+    T &root, const GenericPointer<typename T::ValueType> &pointer,
-                     const GenericPointer<typename T::ValueType> &pointer,
+    typename T::AllocatorType &a) {
-                     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 &
+typename DocumentType::ValueType &CreateValueByPointer(
-CreateValueByPointer(DocumentType &document, const CharType (&source)[N]) {
+    DocumentType &document, const CharType (&source)[N]) {
   return GenericPointer<typename DocumentType::ValueType>(source, N - 1)
       .Create(document);
 }
@@ -1283,17 +1302,16 @@ CreateValueByPointer(DocumentType &document, const CharType (&source)[N]) {
 //////////////////////////////////////////////////////////////////////////////
 
 template <typename T>
-typename T::ValueType *
+typename T::ValueType *GetValueByPointer(
-GetValueByPointer(T &root, const GenericPointer<typename T::ValueType> &pointer,
+    T &root, const GenericPointer<typename T::ValueType> &pointer,
-                  size_t *unresolvedTokenIndex = 0) {
+    size_t *unresolvedTokenIndex = 0) {
   return pointer.Get(root, unresolvedTokenIndex);
 }
 
 template <typename T>
-const typename T::ValueType *
+const typename T::ValueType *GetValueByPointer(
-GetValueByPointer(const T &root,
+    const T &root, const GenericPointer<typename T::ValueType> &pointer,
-                  const GenericPointer<typename T::ValueType> &pointer,
+    size_t *unresolvedTokenIndex = 0) {
-                  size_t *unresolvedTokenIndex = 0) {
   return pointer.Get(root, unresolvedTokenIndex);
 }
 
@@ -1305,9 +1323,9 @@ typename T::ValueType *GetValueByPointer(T &root, const CharType (&source)[N],
 }
 
 template <typename T, typename CharType, size_t N>
-const typename T::ValueType *
+const typename T::ValueType *GetValueByPointer(
-GetValueByPointer(const T &root, const CharType (&source)[N],
+    const T &root, const CharType (&source)[N],
-                  size_t *unresolvedTokenIndex = 0) {
+    size_t *unresolvedTokenIndex = 0) {
   return GenericPointer<typename T::ValueType>(source, N - 1)
       .Get(root, unresolvedTokenIndex);
 }
@@ -1349,19 +1367,17 @@ GetValueByPointerWithDefault(
 }
 
 template <typename T, typename CharType, size_t N>
-typename T::ValueType &
+typename T::ValueType &GetValueByPointerWithDefault(
-GetValueByPointerWithDefault(T &root, const CharType (&source)[N],
+    T &root, const CharType (&source)[N],
-                             const typename T::ValueType &defaultValue,
+    const typename T::ValueType &defaultValue, typename T::AllocatorType &a) {
-                             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 &
+typename T::ValueType &GetValueByPointerWithDefault(
-GetValueByPointerWithDefault(T &root, const CharType (&source)[N],
+    T &root, const CharType (&source)[N], const typename T::Ch *defaultValue,
-                             const typename T::Ch *defaultValue,
+    typename T::AllocatorType &a) {
-                             typename T::AllocatorType &a) {
   return GenericPointer<typename T::ValueType>(source, N - 1)
       .GetWithDefault(root, defaultValue, a);
 }
@@ -1435,10 +1451,9 @@ typename DocumentType::ValueType &GetValueByPointerWithDefault(
 }
 
 template <typename DocumentType, typename CharType, size_t N>
-typename DocumentType::ValueType &
+typename DocumentType::ValueType &GetValueByPointerWithDefault(
-GetValueByPointerWithDefault(DocumentType &document,
+    DocumentType &document, const CharType (&source)[N],
-                             const CharType (&source)[N],
+    const typename DocumentType::Ch *defaultValue) {
-                             const typename DocumentType::Ch *defaultValue) {
   return GenericPointer<typename DocumentType::ValueType>(source, N - 1)
       .GetWithDefault(document, defaultValue);
 }
@@ -1466,33 +1481,32 @@ GetValueByPointerWithDefault(DocumentType &document,
 //////////////////////////////////////////////////////////////////////////////
 
 template <typename T>
-typename T::ValueType &
+typename T::ValueType &SetValueByPointer(
-SetValueByPointer(T &root, const GenericPointer<typename T::ValueType> &pointer,
+    T &root, const GenericPointer<typename T::ValueType> &pointer,
-                  typename T::ValueType &value, typename T::AllocatorType &a) {
+    typename T::ValueType &value, typename T::AllocatorType &a) {
   return pointer.Set(root, value, a);
 }
 
 template <typename T>
-typename T::ValueType &
+typename T::ValueType &SetValueByPointer(
-SetValueByPointer(T &root, const GenericPointer<typename T::ValueType> &pointer,
+    T &root, const GenericPointer<typename T::ValueType> &pointer,
-                  const typename T::ValueType &value,
+    const typename T::ValueType &value, typename T::AllocatorType &a) {
-                  typename T::AllocatorType &a) {
   return pointer.Set(root, value, a);
 }
 
 template <typename T>
-typename T::ValueType &
+typename T::ValueType &SetValueByPointer(
-SetValueByPointer(T &root, const GenericPointer<typename T::ValueType> &pointer,
+    T &root, const GenericPointer<typename T::ValueType> &pointer,
-                  const typename T::Ch *value, typename T::AllocatorType &a) {
+    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 &
+typename T::ValueType &SetValueByPointer(
-SetValueByPointer(T &root, const GenericPointer<typename T::ValueType> &pointer,
+    T &root, const GenericPointer<typename T::ValueType> &pointer,
-                  const std::basic_string<typename T::Ch> &value,
+    const std::basic_string<typename T::Ch> &value,
-                  typename T::AllocatorType &a) {
+    typename T::AllocatorType &a) {
   return pointer.Set(root, value, a);
 }
 #endif
@@ -1532,10 +1546,10 @@ 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 &
+typename T::ValueType &SetValueByPointer(
-SetValueByPointer(T &root, const CharType (&source)[N],
+    T &root, const CharType (&source)[N],
-                  const std::basic_string<typename T::Ch> &value,
+    const std::basic_string<typename T::Ch> &value,
-                  typename T::AllocatorType &a) {
+    typename T::AllocatorType &a) {
   return GenericPointer<typename T::ValueType>(source, N - 1)
       .Set(root, value, a);
 }
@@ -1598,34 +1612,34 @@ SetValueByPointer(
 }
 
 template <typename DocumentType, typename CharType, size_t N>
-typename DocumentType::ValueType &
+typename DocumentType::ValueType &SetValueByPointer(
-SetValueByPointer(DocumentType &document, const CharType (&source)[N],
+    DocumentType &document, const CharType (&source)[N],
-                  typename DocumentType::ValueType &value) {
+    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 &
+typename DocumentType::ValueType &SetValueByPointer(
-SetValueByPointer(DocumentType &document, const CharType (&source)[N],
+    DocumentType &document, const CharType (&source)[N],
-                  const typename DocumentType::ValueType &value) {
+    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 &
+typename DocumentType::ValueType &SetValueByPointer(
-SetValueByPointer(DocumentType &document, const CharType (&source)[N],
+    DocumentType &document, const CharType (&source)[N],
-                  const typename DocumentType::Ch *value) {
+    const typename DocumentType::Ch *value) {
   return GenericPointer<typename DocumentType::ValueType>(source, N - 1)
       .Set(document, value);
 }
 
 #if RAPIDJSON_HAS_STDSTRING
 template <typename DocumentType, typename CharType, size_t N>
-typename DocumentType::ValueType &
+typename DocumentType::ValueType &SetValueByPointer(
-SetValueByPointer(DocumentType &document, const CharType (&source)[N],
+    DocumentType &document, const CharType (&source)[N],
-                  const std::basic_string<typename DocumentType::Ch> &value) {
+    const std::basic_string<typename DocumentType::Ch> &value) {
   return GenericPointer<typename DocumentType::ValueType>(source, N - 1)
       .Set(document, value);
 }
@@ -1644,10 +1658,9 @@ SetValueByPointer(DocumentType &document, const CharType (&source)[N],
 //////////////////////////////////////////////////////////////////////////////
 
 template <typename T>
-typename T::ValueType &
+typename T::ValueType &SwapValueByPointer(
-SwapValueByPointer(T &root,
+    T &root, const GenericPointer<typename T::ValueType> &pointer,
-                   const GenericPointer<typename T::ValueType> &pointer,
+    typename T::ValueType &value, typename T::AllocatorType &a) {
-                   typename T::ValueType &value, typename T::AllocatorType &a) {
   return pointer.Swap(root, value, a);
 }
 
@@ -1668,9 +1681,9 @@ typename DocumentType::ValueType &SwapValueByPointer(
 }
 
 template <typename DocumentType, typename CharType, size_t N>
-typename DocumentType::ValueType &
+typename DocumentType::ValueType &SwapValueByPointer(
-SwapValueByPointer(DocumentType &document, const CharType (&source)[N],
+    DocumentType &document, const CharType (&source)[N],
-                   typename DocumentType::ValueType &value) {
+    typename DocumentType::ValueType &value) {
   return GenericPointer<typename DocumentType::ValueType>(source, N - 1)
       .Swap(document, value);
 }
@@ -1696,4 +1709,4 @@ RAPIDJSON_NAMESPACE_END
 RAPIDJSON_DIAG_POP
 #endif
 
-#endif // RAPIDJSON_POINTER_H_
+#endif  // RAPIDJSON_POINTER_H_

livox_ros_driver/common/rapidjson/prettywriter.h

@@ -37,8 +37,8 @@ RAPIDJSON_NAMESPACE_BEGIN
 /*! \see PrettyWriter::SetFormatOptions
  */
 enum PrettyFormatOptions {
-  kFormatDefault = 0,        //!< Default pretty formatting.
+  kFormatDefault = 0,         //!< Default pretty formatting.
-  kFormatSingleLineArray = 1 //!< Format arrays on a single line.
+  kFormatSingleLineArray = 1  //!< Format arrays on a single line.
 };
 
 //! Writer with indentation and spacing.
@@ -54,7 +54,7 @@ template <typename OutputStream, typename SourceEncoding = UTF8<>,
           unsigned writeFlags = kWriteDefaultFlags>
 class PrettyWriter : public Writer<OutputStream, SourceEncoding, TargetEncoding,
                                    StackAllocator, writeFlags> {
-public:
+ public:
   typedef Writer<OutputStream, SourceEncoding, TargetEncoding, StackAllocator,
                  writeFlags>
       Base;
@@ -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
@@ -177,13 +180,13 @@ public:
   bool EndObject(SizeType memberCount = 0) {
     (void)memberCount;
     RAPIDJSON_ASSERT(Base::level_stack_.GetSize() >=
-                     sizeof(typename Base::Level)); // not inside an Object
+                     sizeof(typename Base::Level));  // not inside an Object
     RAPIDJSON_ASSERT(!Base::level_stack_.template Top<typename Base::Level>()
-                          ->inArray); // currently inside an Array, not Object
+                          ->inArray);  // currently inside an Array, not Object
     RAPIDJSON_ASSERT(
         0 ==
         Base::level_stack_.template Top<typename Base::Level>()->valueCount %
-            2); // Object has a Key without a Value
+            2);  // Object has a Key without a Value
 
     bool empty =
         Base::level_stack_.template Pop<typename Base::Level>(1)->valueCount ==
@@ -196,7 +199,7 @@ public:
     bool ret = Base::EndValue(Base::WriteEndObject());
     (void)ret;
     RAPIDJSON_ASSERT(ret == true);
-    if (Base::level_stack_.Empty()) // end of json text
+    if (Base::level_stack_.Empty())  // end of json text
       Base::Flush();
     return true;
   }
@@ -225,7 +228,7 @@ public:
     bool ret = Base::EndValue(Base::WriteEndArray());
     (void)ret;
     RAPIDJSON_ASSERT(ret == true);
-    if (Base::level_stack_.Empty()) // end of json text
+    if (Base::level_stack_.Empty())  // end of json text
       Base::Flush();
     return true;
   }
@@ -256,26 +259,25 @@ public:
     return Base::EndValue(Base::WriteRawValue(json, length));
   }
 
-protected:
+ protected:
   void PrettyPrefix(Type type) {
     (void)type;
-    if (Base::level_stack_.GetSize() != 0) { // this value is not at root
+    if (Base::level_stack_.GetSize() != 0) {  // this value is not at root
       typename Base::Level *level =
           Base::level_stack_.template Top<typename Base::Level>();
 
       if (level->inArray) {
         if (level->valueCount > 0) {
           Base::os_->Put(
-              ','); // add comma if it is not the first element in array
+              ',');  // add comma if it is not the first element in array
-          if (formatOptions_ & kFormatSingleLineArray)
+          if (formatOptions_ & kFormatSingleLineArray) Base::os_->Put(' ');
-            Base::os_->Put(' ');
         }
 
         if (!(formatOptions_ & kFormatSingleLineArray)) {
           Base::os_->Put('\n');
           WriteIndent();
         }
-      } else { // in object
+      } else {  // in object
         if (level->valueCount > 0) {
           if (level->valueCount % 2 == 0) {
             Base::os_->Put(',');
@@ -287,16 +289,15 @@ protected:
         } else
           Base::os_->Put('\n');
 
-        if (level->valueCount % 2 == 0)
+        if (level->valueCount % 2 == 0) WriteIndent();
-          WriteIndent();
       }
       if (!level->inArray && level->valueCount % 2 == 0)
-        RAPIDJSON_ASSERT(type == kStringType); // if it's in object, then even
+        RAPIDJSON_ASSERT(type == kStringType);  // if it's in object, then even
-                                               // number should be a name
+                                                // number should be a name
       level->valueCount++;
     } else {
       RAPIDJSON_ASSERT(
-          !Base::hasRoot_); // Should only has one and only one root.
+          !Base::hasRoot_);  // Should only has one and only one root.
       Base::hasRoot_ = true;
     }
   }
@@ -313,7 +314,7 @@ protected:
   unsigned indentCharCount_;
   PrettyFormatOptions formatOptions_;
 
-private:
+ private:
   // Prohibit copy constructor & assignment operator.
   PrettyWriter(const PrettyWriter &);
   PrettyWriter &operator=(const PrettyWriter &);
@@ -329,4 +330,4 @@ RAPIDJSON_DIAG_POP
 RAPIDJSON_DIAG_POP
 #endif
 
-#endif // RAPIDJSON_RAPIDJSON_H_
+#endif  // RAPIDJSON_RAPIDJSON_H_

livox_ros_driver/common/rapidjson/rapidjson.h

@@ -41,8 +41,8 @@
           different translation units of a single application.
  */
 
-#include <cstdlib> // malloc(), realloc(), free(), size_t
+#include <cstdlib>  // malloc(), realloc(), free(), size_t
-#include <cstring> // memset(), memcpy(), memmove(), memcmp()
+#include <cstring>  // memset(), memcpy(), memmove(), memcmp()
 
 ///////////////////////////////////////////////////////////////////////////////
 // RAPIDJSON_VERSION_STRING
@@ -81,8 +81,8 @@
 #define RAPIDJSON_MAJOR_VERSION 1
 #define RAPIDJSON_MINOR_VERSION 1
 #define RAPIDJSON_PATCH_VERSION 0
-#define RAPIDJSON_VERSION_STRING                                               \
+#define RAPIDJSON_VERSION_STRING \
-  RAPIDJSON_STRINGIFY(                                                         \
+  RAPIDJSON_STRINGIFY(           \
       RAPIDJSON_MAJOR_VERSION.RAPIDJSON_MINOR_VERSION.RAPIDJSON_PATCH_VERSION)
 
 ///////////////////////////////////////////////////////////////////////////////
@@ -136,9 +136,9 @@
 
 #ifndef RAPIDJSON_HAS_STDSTRING
 #ifdef RAPIDJSON_DOXYGEN_RUNNING
-#define RAPIDJSON_HAS_STDSTRING 1 // force generation of documentation
+#define RAPIDJSON_HAS_STDSTRING 1  // force generation of documentation
 #else
-#define RAPIDJSON_HAS_STDSTRING 0 // no std::string support by default
+#define RAPIDJSON_HAS_STDSTRING 0  // no std::string support by default
 #endif
 /*! \def RAPIDJSON_HAS_STDSTRING
     \ingroup RAPIDJSON_CONFIG
@@ -150,11 +150,11 @@
 
     \hideinitializer
 */
-#endif // !defined(RAPIDJSON_HAS_STDSTRING)
+#endif  // !defined(RAPIDJSON_HAS_STDSTRING)
 
 #if RAPIDJSON_HAS_STDSTRING
 #include <string>
-#endif // RAPIDJSON_HAS_STDSTRING
+#endif  // RAPIDJSON_HAS_STDSTRING
 
 ///////////////////////////////////////////////////////////////////////////////
 // RAPIDJSON_NO_INT64DEFINE
@@ -171,7 +171,7 @@
 */
 #ifndef RAPIDJSON_NO_INT64DEFINE
 //!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN
-#if defined(_MSC_VER) && (_MSC_VER < 1800) // Visual Studio 2013
+#if defined(_MSC_VER) && (_MSC_VER < 1800)  // Visual Studio 2013
 #include "msinttypes/inttypes.h"
 #include "msinttypes/stdint.h"
 #else
@@ -183,7 +183,7 @@
 #ifdef RAPIDJSON_DOXYGEN_RUNNING
 #define RAPIDJSON_NO_INT64DEFINE
 #endif
-#endif // RAPIDJSON_NO_INT64TYPEDEF
+#endif  // RAPIDJSON_NO_INT64TYPEDEF
 
 ///////////////////////////////////////////////////////////////////////////////
 // RAPIDJSON_FORCEINLINE
@@ -198,12 +198,12 @@
 #define RAPIDJSON_FORCEINLINE
 #endif
 //!@endcond
-#endif // RAPIDJSON_FORCEINLINE
+#endif  // RAPIDJSON_FORCEINLINE
 
 ///////////////////////////////////////////////////////////////////////////////
 // RAPIDJSON_ENDIAN
-#define RAPIDJSON_LITTLEENDIAN 0 //!< Little endian machine
+#define RAPIDJSON_LITTLEENDIAN 0  //!< Little endian machine
-#define RAPIDJSON_BIGENDIAN 1    //!< Big endian machine
+#define RAPIDJSON_BIGENDIAN 1     //!< Big endian machine
 
 //! Endianness of the machine.
 /*!
@@ -228,7 +228,7 @@
 #define RAPIDJSON_ENDIAN RAPIDJSON_BIGENDIAN
 #else
 #      error Unknown machine endianness detected. User needs to define RAPIDJSON_ENDIAN.
-#endif // __BYTE_ORDER__
+#endif  // __BYTE_ORDER__
 // Detect with GLIBC's endian.h
 #elif defined(__GLIBC__)
 #include <endian.h>
@@ -238,22 +238,22 @@
 #define RAPIDJSON_ENDIAN RAPIDJSON_BIGENDIAN
 #else
 #      error Unknown machine endianness detected. User needs to define RAPIDJSON_ENDIAN.
-#endif // __GLIBC__
+#endif  // __GLIBC__
 // Detect with _LITTLE_ENDIAN and _BIG_ENDIAN macro
 #elif defined(_LITTLE_ENDIAN) && !defined(_BIG_ENDIAN)
 #define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN
 #elif defined(_BIG_ENDIAN) && !defined(_LITTLE_ENDIAN)
 #define RAPIDJSON_ENDIAN RAPIDJSON_BIGENDIAN
 // Detect with architecture macros
-#elif defined(__sparc) || defined(__sparc__) || defined(_POWER) ||             \
+#elif defined(__sparc) || defined(__sparc__) || defined(_POWER) || \
-    defined(__powerpc__) || defined(__ppc__) || defined(__hpux) ||             \
+    defined(__powerpc__) || defined(__ppc__) || defined(__hpux) || \
-    defined(__hppa) || defined(_MIPSEB) || defined(_POWER) ||                  \
+    defined(__hppa) || defined(_MIPSEB) || defined(_POWER) ||      \
     defined(__s390__)
 #define RAPIDJSON_ENDIAN RAPIDJSON_BIGENDIAN
-#elif defined(__i386__) || defined(__alpha__) || defined(__ia64) ||            \
+#elif defined(__i386__) || defined(__alpha__) || defined(__ia64) ||  \
-    defined(__ia64__) || defined(_M_IX86) || defined(_M_IA64) ||               \
+    defined(__ia64__) || defined(_M_IX86) || defined(_M_IA64) ||     \
-    defined(_M_ALPHA) || defined(__amd64) || defined(__amd64__) ||             \
+    defined(_M_ALPHA) || defined(__amd64) || defined(__amd64__) ||   \
-    defined(_M_AMD64) || defined(__x86_64) || defined(__x86_64__) ||           \
+    defined(_M_AMD64) || defined(__x86_64) || defined(__x86_64__) || \
     defined(_M_X64) || defined(__bfin__)
 #define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN
 #elif defined(_MSC_VER) && (defined(_M_ARM) || defined(_M_ARM64))
@@ -263,20 +263,20 @@
 #else
 #    error Unknown machine endianness detected. User needs to define RAPIDJSON_ENDIAN.
 #endif
-#endif // RAPIDJSON_ENDIAN
+#endif  // RAPIDJSON_ENDIAN
 
 ///////////////////////////////////////////////////////////////////////////////
 // RAPIDJSON_64BIT
 
 //! Whether using 64-bit architecture
 #ifndef RAPIDJSON_64BIT
-#if defined(__LP64__) || (defined(__x86_64__) && defined(__ILP32__)) ||        \
+#if defined(__LP64__) || (defined(__x86_64__) && defined(__ILP32__)) || \
     defined(_WIN64) || defined(__EMSCRIPTEN__)
 #define RAPIDJSON_64BIT 1
 #else
 #define RAPIDJSON_64BIT 0
 #endif
-#endif // RAPIDJSON_64BIT
+#endif  // RAPIDJSON_64BIT
 
 ///////////////////////////////////////////////////////////////////////////////
 // RAPIDJSON_ALIGN
@@ -289,7 +289,7 @@
     User can customize by defining the RAPIDJSON_ALIGN function macro.
 */
 #ifndef RAPIDJSON_ALIGN
-#define RAPIDJSON_ALIGN(x)                                                     \
+#define RAPIDJSON_ALIGN(x) \
   (((x) + static_cast<size_t>(7u)) & ~static_cast<size_t>(7u))
 #endif
 
@@ -303,7 +303,7 @@
     Use this macro to define 64-bit constants by a pair of 32-bit integer.
 */
 #ifndef RAPIDJSON_UINT64_C2
-#define RAPIDJSON_UINT64_C2(high32, low32)                                     \
+#define RAPIDJSON_UINT64_C2(high32, low32) \
   ((static_cast<uint64_t>(high32) << 32) | static_cast<uint64_t>(low32))
 #endif
 
@@ -320,13 +320,13 @@
    form 24 bytes to 16 bytes in 64-bit architecture.
 */
 #ifndef RAPIDJSON_48BITPOINTER_OPTIMIZATION
-#if defined(__amd64__) || defined(__amd64) || defined(__x86_64__) ||           \
+#if defined(__amd64__) || defined(__amd64) || defined(__x86_64__) || \
     defined(__x86_64) || defined(_M_X64) || defined(_M_AMD64)
 #define RAPIDJSON_48BITPOINTER_OPTIMIZATION 1
 #else
 #define RAPIDJSON_48BITPOINTER_OPTIMIZATION 0
 #endif
-#endif // RAPIDJSON_48BITPOINTER_OPTIMIZATION
+#endif  // RAPIDJSON_48BITPOINTER_OPTIMIZATION
 
 #if RAPIDJSON_48BITPOINTER_OPTIMIZATION == 1
 #if RAPIDJSON_64BIT != 1
@@ -337,9 +337,9 @@
        (reinterpret_cast<uintptr_t>(p) &                                       \
         static_cast<uintptr_t>(RAPIDJSON_UINT64_C2(0xFFFF0000, 0x00000000))) | \
        reinterpret_cast<uintptr_t>(reinterpret_cast<const void *>(x))))
-#define RAPIDJSON_GETPOINTER(type, p)                                          \
+#define RAPIDJSON_GETPOINTER(type, p)  \
-  (reinterpret_cast<type *>(                                                   \
+  (reinterpret_cast<type *>(           \
-      reinterpret_cast<uintptr_t>(p) &                                         \
+      reinterpret_cast<uintptr_t>(p) & \
       static_cast<uintptr_t>(RAPIDJSON_UINT64_C2(0x0000FFFF, 0xFFFFFFFF))))
 #else
 #define RAPIDJSON_SETPOINTER(type, p, x) (p = (x))
@@ -375,7 +375,7 @@
     If any of these symbols is defined, RapidJSON defines the macro
     \c RAPIDJSON_SIMD to indicate the availability of the optimized code.
 */
-#if defined(RAPIDJSON_SSE2) || defined(RAPIDJSON_SSE42) ||                     \
+#if defined(RAPIDJSON_SSE2) || defined(RAPIDJSON_SSE42) || \
     defined(RAPIDJSON_NEON) || defined(RAPIDJSON_DOXYGEN_RUNNING)
 #define RAPIDJSON_SIMD
 #endif
@@ -431,7 +431,7 @@ RAPIDJSON_NAMESPACE_END
 #ifndef RAPIDJSON_ASSERT
 #include <cassert>
 #define RAPIDJSON_ASSERT(x) assert(x)
-#endif // RAPIDJSON_ASSERT
+#endif  // RAPIDJSON_ASSERT
 
 ///////////////////////////////////////////////////////////////////////////////
 // RAPIDJSON_STATIC_ASSERT
@@ -440,8 +440,8 @@ RAPIDJSON_NAMESPACE_END
 #ifndef RAPIDJSON_STATIC_ASSERT
 #if __cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1800)
 #define RAPIDJSON_STATIC_ASSERT(x) static_assert(x, RAPIDJSON_STRINGIFY(x))
-#endif // C++11
+#endif  // C++11
-#endif // RAPIDJSON_STATIC_ASSERT
+#endif  // RAPIDJSON_STATIC_ASSERT
 
 // Adopt C++03 implementation from boost
 #ifndef RAPIDJSON_STATIC_ASSERT
@@ -449,11 +449,14 @@ RAPIDJSON_NAMESPACE_END
 //!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN
 #endif
 RAPIDJSON_NAMESPACE_BEGIN
-template <bool x> struct STATIC_ASSERTION_FAILURE;
+template <bool x>
-template <> struct STATIC_ASSERTION_FAILURE<true> {
+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__)
@@ -470,12 +473,12 @@ RAPIDJSON_NAMESPACE_END
     \param x compile-time condition
     \hideinitializer
  */
-#define RAPIDJSON_STATIC_ASSERT(x)                                             \
+#define RAPIDJSON_STATIC_ASSERT(x)                               \
-  typedef ::RAPIDJSON_NAMESPACE::StaticAssertTest<sizeof(                      \
+  typedef ::RAPIDJSON_NAMESPACE::StaticAssertTest<sizeof(        \
-      ::RAPIDJSON_NAMESPACE::STATIC_ASSERTION_FAILURE<bool(x)>)>               \
+      ::RAPIDJSON_NAMESPACE::STATIC_ASSERTION_FAILURE<bool(x)>)> \
-      RAPIDJSON_JOIN(StaticAssertTypedef, __LINE__)                            \
+      RAPIDJSON_JOIN(StaticAssertTypedef, __LINE__)              \
           RAPIDJSON_STATIC_ASSERT_UNUSED_ATTRIBUTE
-#endif // RAPIDJSON_STATIC_ASSERT
+#endif  // RAPIDJSON_STATIC_ASSERT
 
 ///////////////////////////////////////////////////////////////////////////////
 // RAPIDJSON_LIKELY, RAPIDJSON_UNLIKELY
@@ -512,8 +515,8 @@ RAPIDJSON_NAMESPACE_END
 //!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN
 
 #define RAPIDJSON_MULTILINEMACRO_BEGIN do {
-#define RAPIDJSON_MULTILINEMACRO_END                                           \
+#define RAPIDJSON_MULTILINEMACRO_END \
-  }                                                                            \
+  }                                  \
   while ((void)0, 0)
 
 // adopted from Boost
@@ -529,20 +532,20 @@ RAPIDJSON_NAMESPACE_END
 // RAPIDJSON_DIAG_PUSH/POP, RAPIDJSON_DIAG_OFF
 
 #if defined(__GNUC__)
-#define RAPIDJSON_GNUC                                                         \
+#define RAPIDJSON_GNUC \
   RAPIDJSON_VERSION_CODE(__GNUC__, __GNUC_MINOR__, __GNUC_PATCHLEVEL__)
 #endif
 
-#if defined(__clang__) || (defined(RAPIDJSON_GNUC) &&                          \
+#if defined(__clang__) || (defined(RAPIDJSON_GNUC) && \
                            RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4, 2, 0))
 
 #define RAPIDJSON_PRAGMA(x) _Pragma(RAPIDJSON_STRINGIFY(x))
 #define RAPIDJSON_DIAG_PRAGMA(x) RAPIDJSON_PRAGMA(GCC diagnostic x)
-#define RAPIDJSON_DIAG_OFF(x)                                                  \
+#define RAPIDJSON_DIAG_OFF(x) \
   RAPIDJSON_DIAG_PRAGMA(ignored RAPIDJSON_STRINGIFY(RAPIDJSON_JOIN(-W, x)))
 
 // push/pop support in Clang and GCC>=4.6
-#if defined(__clang__) || (defined(RAPIDJSON_GNUC) &&                          \
+#if defined(__clang__) || (defined(RAPIDJSON_GNUC) && \
                            RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4, 6, 0))
 #define RAPIDJSON_DIAG_PUSH RAPIDJSON_DIAG_PRAGMA(push)
 #define RAPIDJSON_DIAG_POP RAPIDJSON_DIAG_PRAGMA(pop)
@@ -567,41 +570,41 @@ RAPIDJSON_NAMESPACE_END
 #define RAPIDJSON_DIAG_PUSH   /* ignored */
 #define RAPIDJSON_DIAG_POP    /* ignored */
 
-#endif // RAPIDJSON_DIAG_*
+#endif  // RAPIDJSON_DIAG_*
 
 ///////////////////////////////////////////////////////////////////////////////
 // C++11 features
 
 #ifndef RAPIDJSON_HAS_CXX11_RVALUE_REFS
 #if defined(__clang__)
-#if __has_feature(cxx_rvalue_references) &&                                    \
+#if __has_feature(cxx_rvalue_references) &&           \
-    (defined(_MSC_VER) || defined(_LIBCPP_VERSION) ||                          \
+    (defined(_MSC_VER) || defined(_LIBCPP_VERSION) || \
      defined(__GLIBCXX__) && __GLIBCXX__ >= 20080306)
 #define RAPIDJSON_HAS_CXX11_RVALUE_REFS 1
 #else
 #define RAPIDJSON_HAS_CXX11_RVALUE_REFS 0
 #endif
-#elif (defined(RAPIDJSON_GNUC) &&                                              \
+#elif (defined(RAPIDJSON_GNUC) &&                             \
-       (RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4, 3, 0)) &&                  \
+       (RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4, 3, 0)) && \
-       defined(__GXX_EXPERIMENTAL_CXX0X__)) ||                                 \
+       defined(__GXX_EXPERIMENTAL_CXX0X__)) ||                \
-    (defined(_MSC_VER) && _MSC_VER >= 1600) ||                                 \
+    (defined(_MSC_VER) && _MSC_VER >= 1600) ||                \
-    (defined(__SUNPRO_CC) && __SUNPRO_CC >= 0x5140 &&                          \
+    (defined(__SUNPRO_CC) && __SUNPRO_CC >= 0x5140 &&         \
      defined(__GXX_EXPERIMENTAL_CXX0X__))
 
 #define RAPIDJSON_HAS_CXX11_RVALUE_REFS 1
 #else
 #define RAPIDJSON_HAS_CXX11_RVALUE_REFS 0
 #endif
-#endif // RAPIDJSON_HAS_CXX11_RVALUE_REFS
+#endif  // RAPIDJSON_HAS_CXX11_RVALUE_REFS
 
 #ifndef RAPIDJSON_HAS_CXX11_NOEXCEPT
 #if defined(__clang__)
 #define RAPIDJSON_HAS_CXX11_NOEXCEPT __has_feature(cxx_noexcept)
-#elif (defined(RAPIDJSON_GNUC) &&                                              \
+#elif (defined(RAPIDJSON_GNUC) &&                             \
-       (RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4, 6, 0)) &&                  \
+       (RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4, 6, 0)) && \
-       defined(__GXX_EXPERIMENTAL_CXX0X__)) ||                                 \
+       defined(__GXX_EXPERIMENTAL_CXX0X__)) ||                \
-    (defined(_MSC_VER) && _MSC_VER >= 1900) ||                                 \
+    (defined(_MSC_VER) && _MSC_VER >= 1900) ||                \
-    (defined(__SUNPRO_CC) && __SUNPRO_CC >= 0x5140 &&                          \
+    (defined(__SUNPRO_CC) && __SUNPRO_CC >= 0x5140 &&         \
      defined(__GXX_EXPERIMENTAL_CXX0X__))
 #define RAPIDJSON_HAS_CXX11_NOEXCEPT 1
 #else
@@ -626,17 +629,17 @@ RAPIDJSON_NAMESPACE_END
 #ifndef RAPIDJSON_HAS_CXX11_RANGE_FOR
 #if defined(__clang__)
 #define RAPIDJSON_HAS_CXX11_RANGE_FOR __has_feature(cxx_range_for)
-#elif (defined(RAPIDJSON_GNUC) &&                                              \
+#elif (defined(RAPIDJSON_GNUC) &&                             \
-       (RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4, 6, 0)) &&                  \
+       (RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4, 6, 0)) && \
-       defined(__GXX_EXPERIMENTAL_CXX0X__)) ||                                 \
+       defined(__GXX_EXPERIMENTAL_CXX0X__)) ||                \
-    (defined(_MSC_VER) && _MSC_VER >= 1700) ||                                 \
+    (defined(_MSC_VER) && _MSC_VER >= 1700) ||                \
-    (defined(__SUNPRO_CC) && __SUNPRO_CC >= 0x5140 &&                          \
+    (defined(__SUNPRO_CC) && __SUNPRO_CC >= 0x5140 &&         \
      defined(__GXX_EXPERIMENTAL_CXX0X__))
 #define RAPIDJSON_HAS_CXX11_RANGE_FOR 1
 #else
 #define RAPIDJSON_HAS_CXX11_RANGE_FOR 0
 #endif
-#endif // RAPIDJSON_HAS_CXX11_RANGE_FOR
+#endif  // RAPIDJSON_HAS_CXX11_RANGE_FOR
 
 ///////////////////////////////////////////////////////////////////////////////
 // C++17 features
@@ -673,11 +676,11 @@ RAPIDJSON_NAMESPACE_END
 #define RAPIDJSON_NOEXCEPT_ASSERT(x)
 #else
 #define RAPIDJSON_NOEXCEPT_ASSERT(x) RAPIDJSON_ASSERT(x)
-#endif // RAPIDJSON_HAS_CXX11_NOEXCEPT
+#endif  // RAPIDJSON_HAS_CXX11_NOEXCEPT
 #else
 #define RAPIDJSON_NOEXCEPT_ASSERT(x) RAPIDJSON_ASSERT(x)
-#endif // RAPIDJSON_ASSERT_THROWS
+#endif  // RAPIDJSON_ASSERT_THROWS
-#endif // RAPIDJSON_NOEXCEPT_ASSERT
+#endif  // RAPIDJSON_NOEXCEPT_ASSERT
 
 ///////////////////////////////////////////////////////////////////////////////
 // new/delete
@@ -702,15 +705,15 @@ RAPIDJSON_NAMESPACE_BEGIN
 
 //! Type of JSON value
 enum Type {
-  kNullType = 0,   //!< null
+  kNullType = 0,    //!< null
-  kFalseType = 1,  //!< false
+  kFalseType = 1,   //!< false
-  kTrueType = 2,   //!< true
+  kTrueType = 2,    //!< true
-  kObjectType = 3, //!< object
+  kObjectType = 3,  //!< object
-  kArrayType = 4,  //!< array
+  kArrayType = 4,   //!< array
-  kStringType = 5, //!< string
+  kStringType = 5,  //!< string
-  kNumberType = 6  //!< number
+  kNumberType = 6   //!< number
 };
 
 RAPIDJSON_NAMESPACE_END
 
-#endif // RAPIDJSON_RAPIDJSON_H_
+#endif  // RAPIDJSON_RAPIDJSON_H_

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++)
+  for (size_t i = 0; i < n; i++) PutUnsafe(stream, c);
-    PutUnsafe(stream, c);
 }
 
 ///////////////////////////////////////////////////////////////////////////////
@@ -117,13 +117,13 @@ inline void PutN(Stream &stream, Ch c, size_t n) {
 
 #if defined(_MSC_VER) && _MSC_VER <= 1800
 RAPIDJSON_DIAG_PUSH
-RAPIDJSON_DIAG_OFF(4702) // unreachable code
+RAPIDJSON_DIAG_OFF(4702)  // unreachable code
-RAPIDJSON_DIAG_OFF(4512) // assignment operator could not be generated
+RAPIDJSON_DIAG_OFF(4512)  // assignment operator could not be generated
 #endif
 
 template <typename InputStream, typename Encoding = UTF8<>>
 class GenericStreamWrapper {
-public:
+ public:
   typedef typename Encoding::Ch Ch;
   GenericStreamWrapper(InputStream &is) : is_(is) {}
 
@@ -142,7 +142,7 @@ public:
   UTFType GetType() const { return is_.GetType(); }
   bool HasBOM() const { return is_.HasBOM(); }
 
-protected:
+ protected:
   InputStream &is_;
 };
 
@@ -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) {}
@@ -176,8 +177,8 @@ template <typename Encoding> struct GenericStringStream {
     return 0;
   }
 
-  const Ch *src_;  //!< Current read position.
+  const Ch *src_;   //!< Current read position.
-  const Ch *head_; //!< Original head of the string.
+  const Ch *head_;  //!< Original head of the string.
 };
 
 template <typename Encoding>
@@ -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) {}
@@ -237,4 +239,4 @@ typedef GenericInsituStringStream<UTF8<>> InsituStringStream;
 
 RAPIDJSON_NAMESPACE_END
 
-#endif // RAPIDJSON_STREAM_H_
+#endif  // RAPIDJSON_STREAM_H_

livox_ros_driver/common/rapidjson/stringbuffer.h

@@ -23,7 +23,7 @@
 #include "stream.h"
 
 #if RAPIDJSON_HAS_CXX11_RVALUE_REFS
-#include <utility> // std::move
+#include <utility>  // std::move
 #endif
 
 #include "internal/stack.h"
@@ -43,7 +43,7 @@ RAPIDJSON_NAMESPACE_BEGIN
 */
 template <typename Encoding, typename Allocator = CrtAllocator>
 class GenericStringBuffer {
-public:
+ public:
   typedef typename Encoding::Ch Ch;
 
   GenericStringBuffer(Allocator *allocator = 0,
@@ -54,8 +54,7 @@ public:
   GenericStringBuffer(GenericStringBuffer &&rhs)
       : stack_(std::move(rhs.stack_)) {}
   GenericStringBuffer &operator=(GenericStringBuffer &&rhs) {
-    if (&rhs != this)
+    if (&rhs != this) stack_ = std::move(rhs.stack_);
-      stack_ = std::move(rhs.stack_);
     return *this;
   }
 #endif
@@ -94,7 +93,7 @@ public:
   static const size_t kDefaultCapacity = 256;
   mutable internal::Stack<Allocator> stack_;
 
-private:
+ private:
   // Prohibit copy constructor & assignment operator.
   GenericStringBuffer(const GenericStringBuffer &);
   GenericStringBuffer &operator=(const GenericStringBuffer &);
@@ -128,4 +127,4 @@ RAPIDJSON_NAMESPACE_END
 RAPIDJSON_DIAG_POP
 #endif
 
-#endif // RAPIDJSON_STRINGBUFFER_H_
+#endif  // RAPIDJSON_STRINGBUFFER_H_

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>
@@ -48,7 +48,7 @@ RAPIDJSON_DIAG_OFF(unreachable - code)
 RAPIDJSON_DIAG_OFF(c++ 98 - compat)
 #elif defined(_MSC_VER)
 RAPIDJSON_DIAG_PUSH
-RAPIDJSON_DIAG_OFF(4127) // conditional expression is constant
+RAPIDJSON_DIAG_OFF(4127)  // conditional expression is constant
 #endif
 
 RAPIDJSON_NAMESPACE_BEGIN
@@ -68,13 +68,13 @@ RAPIDJSON_NAMESPACE_BEGIN
 
 //! Combination of writeFlags
 enum WriteFlag {
-  kWriteNoFlags = 0,              //!< No flags are set.
+  kWriteNoFlags = 0,               //!< No flags are set.
-  kWriteValidateEncodingFlag = 1, //!< Validate encoding of JSON strings.
+  kWriteValidateEncodingFlag = 1,  //!< Validate encoding of JSON strings.
-  kWriteNanAndInfFlag = 2, //!< Allow writing of Infinity, -Infinity and NaN.
+  kWriteNanAndInfFlag = 2,  //!< Allow writing of Infinity, -Infinity and NaN.
   kWriteDefaultFlags =
-      RAPIDJSON_WRITE_DEFAULT_FLAGS //!< Default write flags. Can be customized
+      RAPIDJSON_WRITE_DEFAULT_FLAGS  //!< Default write flags. Can be customized
-                                    //!< by defining
+                                     //!< by defining
-                                    //!< RAPIDJSON_WRITE_DEFAULT_FLAGS
+                                     //!< RAPIDJSON_WRITE_DEFAULT_FLAGS
 };
 
 //! JSON writer
@@ -100,7 +100,7 @@ template <typename OutputStream, typename SourceEncoding = UTF8<>,
           typename StackAllocator = CrtAllocator,
           unsigned writeFlags = kWriteDefaultFlags>
 class Writer {
-public:
+ public:
   typedef typename SourceEncoding::Ch Ch;
 
   static const int kDefaultMaxDecimalPlaces = 324;
@@ -112,18 +112,24 @@ public:
   */
   explicit Writer(OutputStream &os, StackAllocator *stackAllocator = 0,
                   size_t levelDepth = kDefaultLevelDepth)
-      : os_(&os), level_stack_(stackAllocator, levelDepth * sizeof(Level)),
+      : os_(&os),
-        maxDecimalPlaces_(kDefaultMaxDecimalPlaces), hasRoot_(false) {}
+        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)),
+      : os_(0),
-        maxDecimalPlaces_(kDefaultMaxDecimalPlaces), hasRoot_(false) {}
+        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_)),
+      : os_(rhs.os_),
-        maxDecimalPlaces_(rhs.maxDecimalPlaces_), hasRoot_(rhs.hasRoot_) {
+        level_stack_(std::move(rhs.level_stack_)),
+        maxDecimalPlaces_(rhs.maxDecimalPlaces_),
+        hasRoot_(rhs.hasRoot_) {
     rhs.os_ = 0;
   }
 #endif
@@ -265,11 +271,12 @@ public:
   bool EndObject(SizeType memberCount = 0) {
     (void)memberCount;
     RAPIDJSON_ASSERT(level_stack_.GetSize() >=
-                     sizeof(Level)); // not inside an Object
+                     sizeof(Level));  // not inside an Object
     RAPIDJSON_ASSERT(!level_stack_.template Top<Level>()
-                          ->inArray); // currently inside an Array, not Object
+                          ->inArray);  // currently inside an Array, not Object
-    RAPIDJSON_ASSERT(0 == level_stack_.template Top<Level>()->valueCount %
+    RAPIDJSON_ASSERT(0 ==
-                              2); // Object has a Key without a Value
+                     level_stack_.template Top<Level>()->valueCount %
+                         2);  // Object has a Key without a Value
     level_stack_.template Pop<Level>(1);
     return EndValue(WriteEndObject());
   }
@@ -320,12 +327,12 @@ public:
    */
   void Flush() { os_->Flush(); }
 
-protected:
+ protected:
   //! Information for each nested level
   struct Level {
     Level(bool inArray_) : valueCount(0), inArray(inArray_) {}
-    size_t valueCount; //!< number of values in this level
+    size_t valueCount;  //!< number of values in this level
-    bool inArray;      //!< true if in array, otherwise in object
+    bool inArray;       //!< true if in array, otherwise in object
   };
 
   static const size_t kDefaultLevelDepth = 32;
@@ -395,8 +402,7 @@ protected:
 
   bool WriteDouble(double d) {
     if (internal::Double(d).IsNanOrInf()) {
-      if (!(writeFlags & kWriteNanAndInfFlag))
+      if (!(writeFlags & kWriteNanAndInfFlag)) return false;
-        return false;
       if (internal::Double(d).IsNan()) {
         PutReserve(*os_, 3);
         PutUnsafe(*os_, 'N');
@@ -437,22 +443,22 @@ protected:
         // 0    1    2    3    4    5    6    7    8    9    A    B    C    D E
         // F
         'u', 'u', 'u',  'u', 'u', 'u', 'u', 'u', 'b', 't',
-        'n', 'u', 'f',  'r', 'u', 'u', // 00
+        'n', 'u', 'f',  'r', 'u', 'u',  // 00
         'u', 'u', 'u',  'u', 'u', 'u', 'u', 'u', 'u', 'u',
-        'u', 'u', 'u',  'u', 'u', 'u', // 10
+        'u', 'u', 'u',  'u', 'u', 'u',  // 10
         0,   0,   '"',  0,   0,   0,   0,   0,   0,   0,
-        0,   0,   0,    0,   0,   0, // 20
+        0,   0,   0,    0,   0,   0,  // 20
-        Z16, Z16,                    // 30~4F
+        Z16, Z16,                     // 30~4F
         0,   0,   0,    0,   0,   0,   0,   0,   0,   0,
-        0,   0,   '\\', 0,   0,   0,                      // 50
+        0,   0,   '\\', 0,   0,   0,                       // 50
-        Z16, Z16, Z16,  Z16, Z16, Z16, Z16, Z16, Z16, Z16 // 60~FF
+        Z16, Z16, Z16,  Z16, Z16, Z16, Z16, Z16, Z16, Z16  // 60~FF
 #undef Z16
     };
 
     if (TargetEncoding::supportUnicode)
-      PutReserve(*os_, 2 + length * 6); // "\uxxxx..."
+      PutReserve(*os_, 2 + length * 6);  // "\uxxxx..."
     else
-      PutReserve(*os_, 2 + length * 12); // "\uxxxx\uyyyy..."
+      PutReserve(*os_, 2 + length * 12);  // "\uxxxx\uyyyy..."
 
     PutUnsafe(*os_, '\"');
     GenericStringStream<SourceEncoding> is(str);
@@ -554,27 +560,27 @@ protected:
   void Prefix(Type type) {
     (void)type;
     if (RAPIDJSON_LIKELY(level_stack_.GetSize() !=
-                         0)) { // this value is not at root
+                         0)) {  // this value is not at root
       Level *level = level_stack_.template Top<Level>();
       if (level->valueCount > 0) {
         if (level->inArray)
-          os_->Put(','); // add comma if it is not the first element in array
+          os_->Put(',');  // add comma if it is not the first element in array
-        else             // in object
+        else              // in object
           os_->Put((level->valueCount % 2 == 0) ? ',' : ':');
       }
       if (!level->inArray && level->valueCount % 2 == 0)
-        RAPIDJSON_ASSERT(type == kStringType); // if it's in object, then even
+        RAPIDJSON_ASSERT(type == kStringType);  // if it's in object, then even
-                                               // number should be a name
+                                                // number should be a name
       level->valueCount++;
     } else {
-      RAPIDJSON_ASSERT(!hasRoot_); // Should only has one and only one root.
+      RAPIDJSON_ASSERT(!hasRoot_);  // Should only has one and only one root.
       hasRoot_ = true;
     }
   }
 
   // Flush the value if it is the top level one.
   bool EndValue(bool ret) {
-    if (RAPIDJSON_UNLIKELY(level_stack_.Empty())) // end of json text
+    if (RAPIDJSON_UNLIKELY(level_stack_.Empty()))  // end of json text
       Flush();
     return ret;
   }
@@ -584,7 +590,7 @@ protected:
   int maxDecimalPlaces_;
   bool hasRoot_;
 
-private:
+ private:
   // Prohibit copy constructor & assignment operator.
   Writer(const Writer &);
   Writer &operator=(const Writer &);
@@ -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))
+    if (!(kWriteDefaultFlags & kWriteNanAndInfFlag)) return false;
-      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)
+  if (length < 16) return RAPIDJSON_LIKELY(is.Tell() < length);
-    return RAPIDJSON_LIKELY(is.Tell() < length);
 
-  if (!RAPIDJSON_LIKELY(is.Tell() < length))
+  if (!RAPIDJSON_LIKELY(is.Tell() < length)) return false;
-    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)
+  if (nextAligned > end) return true;
-    return true;
 
   while (p != nextAligned)
     if (*p < 0x20 || *p == '\"' || *p == '\\') {
@@ -703,12 +710,12 @@ inline bool Writer<StringBuffer>::ScanWriteUnescapedString(StringStream &is,
     const __m128i t1 = _mm_cmpeq_epi8(s, dq);
     const __m128i t2 = _mm_cmpeq_epi8(s, bs);
     const __m128i t3 = _mm_cmpeq_epi8(_mm_max_epu8(s, sp),
-                                      sp); // s < 0x20 <=> max(s, 0x1F) == 0x1F
+                                      sp);  // s < 0x20 <=> max(s, 0x1F) == 0x1F
     const __m128i x = _mm_or_si128(_mm_or_si128(t1, t2), t3);
     unsigned short r = static_cast<unsigned short>(_mm_movemask_epi8(x));
-    if (RAPIDJSON_UNLIKELY(r != 0)) { // some of characters is escaped
+    if (RAPIDJSON_UNLIKELY(r != 0)) {  // some of characters is escaped
       SizeType len;
-#ifdef _MSC_VER // Find the index of first escaped
+#ifdef _MSC_VER  // Find the index of first escaped
       unsigned long offset;
       _BitScanForward(&offset, r);
       len = offset;
@@ -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++)
+      for (size_t i = 0; i < len; i++) q[i] = p[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)
+  if (length < 16) return RAPIDJSON_LIKELY(is.Tell() < length);
-    return RAPIDJSON_LIKELY(is.Tell() < length);
 
-  if (!RAPIDJSON_LIKELY(is.Tell() < length))
+  if (!RAPIDJSON_LIKELY(is.Tell() < length)) return false;
-    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)
+  if (nextAligned > end) return true;
-    return true;
 
   while (p != nextAligned)
     if (*p < 0x20 || *p == '\"' || *p == '\\') {
@@ -767,9 +770,9 @@ inline bool Writer<StringBuffer>::ScanWriteUnescapedString(StringStream &is,
     x = vorrq_u8(x, vceqq_u8(s, s2));
     x = vorrq_u8(x, vcltq_u8(s, s3));
 
-    x = vrev64q_u8(x);                                          // Rev in 64
+    x = vrev64q_u8(x);                                           // Rev in 64
-    uint64_t low = vgetq_lane_u64(vreinterpretq_u64_u8(x), 0);  // extract
+    uint64_t low = vgetq_lane_u64(vreinterpretq_u64_u8(x), 0);   // extract
-    uint64_t high = vgetq_lane_u64(vreinterpretq_u64_u8(x), 1); // extract
+    uint64_t high = vgetq_lane_u64(vreinterpretq_u64_u8(x), 1);  // extract
 
     SizeType len = 0;
     bool escaped = false;
@@ -784,10 +787,9 @@ inline bool Writer<StringBuffer>::ScanWriteUnescapedString(StringStream &is,
       len = lz >> 3;
       escaped = true;
     }
-    if (RAPIDJSON_UNLIKELY(escaped)) { // some of characters is escaped
+    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++)
+      for (size_t i = 0; i < len; i++) q[i] = p[i];
-        q[i] = p[i];
 
       p += len;
       break;
@@ -798,7 +800,7 @@ inline bool Writer<StringBuffer>::ScanWriteUnescapedString(StringStream &is,
   is.src_ = p;
   return RAPIDJSON_LIKELY(is.Tell() < length);
 }
-#endif // RAPIDJSON_NEON
+#endif  // RAPIDJSON_NEON
 
 RAPIDJSON_NAMESPACE_END
 
@@ -806,4 +808,4 @@ RAPIDJSON_NAMESPACE_END
 RAPIDJSON_DIAG_POP
 #endif
 
-#endif // RAPIDJSON_RAPIDJSON_H_
+#endif  // RAPIDJSON_RAPIDJSON_H_

livox_ros_driver/common/rapidxml/rapidxml_iterators.hpp

@@ -11,9 +11,9 @@
 namespace rapidxml {
 
 //! Iterator of child nodes of xml_node
-template <class Ch> class node_iterator {
+template <class Ch>
-
+class node_iterator {
-public:
+ public:
   typedef typename xml_node<Ch> value_type;
   typedef typename xml_node<Ch> &reference;
   typedef typename xml_node<Ch> *pointer;
@@ -62,14 +62,14 @@ public:
 
   bool operator!=(const node_iterator<Ch> &rhs) { return m_node != rhs.m_node; }
 
-private:
+ private:
   xml_node<Ch> *m_node;
 };
 
 //! Iterator of child attributes of xml_node
-template <class Ch> class attribute_iterator {
+template <class Ch>
-
+class attribute_iterator {
-public:
+ public:
   typedef typename xml_attribute<Ch> value_type;
   typedef typename xml_attribute<Ch> &reference;
   typedef typename xml_attribute<Ch> *pointer;
@@ -123,10 +123,10 @@ public:
     return m_attribute != rhs.m_attribute;
   }
 
-private:
+ private:
   xml_attribute<Ch> *m_attribute;
 };
 
-} // namespace rapidxml
+}  // namespace rapidxml
 
 #endif

livox_ros_driver/common/rapidxml/rapidxml_print.hpp

@@ -19,9 +19,9 @@ namespace rapidxml {
 ///////////////////////////////////////////////////////////////////////
 // Printing flags
 
-const int print_no_indenting =
+const int print_no_indenting = 0x1;  //!< Printer flag instructing the printer
-    0x1; //!< Printer flag instructing the printer to suppress indenting of XML.
+                                     //!to suppress indenting of XML.
-         //!< See print() function.
+                                     //!< See print() function.
 
 ///////////////////////////////////////////////////////////////////////
 // Internal
@@ -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)
+  while (begin != end) *out++ = *begin++;
-    *out++ = *begin++;
   return out;
 }
 
@@ -47,49 +46,49 @@ inline OutIt copy_and_expand_chars(const Ch *begin, const Ch *end, Ch noexpand,
                                    OutIt out) {
   while (begin != end) {
     if (*begin == noexpand) {
-      *out++ = *begin; // No expansion, copy character
+      *out++ = *begin;  // No expansion, copy character
     } else {
       switch (*begin) {
-      case Ch('<'):
+        case Ch('<'):
-        *out++ = Ch('&');
+          *out++ = Ch('&');
-        *out++ = Ch('l');
+          *out++ = Ch('l');
-        *out++ = Ch('t');
+          *out++ = Ch('t');
-        *out++ = Ch(';');
+          *out++ = Ch(';');
-        break;
+          break;
-      case Ch('>'):
+        case Ch('>'):
-        *out++ = Ch('&');
+          *out++ = Ch('&');
-        *out++ = Ch('g');
+          *out++ = Ch('g');
-        *out++ = Ch('t');
+          *out++ = Ch('t');
-        *out++ = Ch(';');
+          *out++ = Ch(';');
-        break;
+          break;
-      case Ch('\''):
+        case Ch('\''):
-        *out++ = Ch('&');
+          *out++ = Ch('&');
-        *out++ = Ch('a');
+          *out++ = Ch('a');
-        *out++ = Ch('p');
+          *out++ = Ch('p');
-        *out++ = Ch('o');
+          *out++ = Ch('o');
-        *out++ = Ch('s');
+          *out++ = Ch('s');
-        *out++ = Ch(';');
+          *out++ = Ch(';');
-        break;
+          break;
-      case Ch('"'):
+        case Ch('"'):
-        *out++ = Ch('&');
+          *out++ = Ch('&');
-        *out++ = Ch('q');
+          *out++ = Ch('q');
-        *out++ = Ch('u');
+          *out++ = Ch('u');
-        *out++ = Ch('o');
+          *out++ = Ch('o');
-        *out++ = Ch('t');
+          *out++ = Ch('t');
-        *out++ = Ch(';');
+          *out++ = Ch(';');
-        break;
+          break;
-      case Ch('&'):
+        case Ch('&'):
-        *out++ = Ch('&');
+          *out++ = Ch('&');
-        *out++ = Ch('a');
+          *out++ = Ch('a');
-        *out++ = Ch('m');
+          *out++ = Ch('m');
-        *out++ = Ch('p');
+          *out++ = Ch('p');
-        *out++ = Ch(';');
+          *out++ = Ch(';');
-        break;
+          break;
-      default:
+        default:
-        *out++ = *begin; // No expansion, copy character
+          *out++ = *begin;  // No expansion, copy character
       }
     }
-    ++begin; // Step to next character
+    ++begin;  // Step to next character
   }
   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)
+  for (int i = 0; i < n; ++i) *out++ = ch;
-    *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)
+    if (*begin++ == ch) return true;
-      return true;
   return false;
 }
 
@@ -120,56 +117,54 @@ 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);
+      break;
 
-  // Document
+    // Element
-  case node_document:
+    case node_element:
-    out = print_children(out, node, flags, indent);
+      out = print_element_node(out, node, flags, indent);
-    break;
+      break;
 
-  // Element
+    // Data
-  case node_element:
+    case node_data:
-    out = print_element_node(out, node, flags, indent);
+      out = print_data_node(out, node, flags, indent);
-    break;
+      break;
 
-  // Data
+    // CDATA
-  case node_data:
+    case node_cdata:
-    out = print_data_node(out, node, flags, indent);
+      out = print_cdata_node(out, node, flags, indent);
-    break;
+      break;
 
-  // CDATA
+    // Declaration
-  case node_cdata:
+    case node_declaration:
-    out = print_cdata_node(out, node, flags, indent);
+      out = print_declaration_node(out, node, flags, indent);
-    break;
+      break;
 
-  // Declaration
+    // Comment
-  case node_declaration:
+    case node_comment:
-    out = print_declaration_node(out, node, flags, indent);
+      out = print_comment_node(out, node, flags, indent);
-    break;
+      break;
 
-  // Comment
+    // Doctype
-  case node_comment:
+    case node_doctype:
-    out = print_comment_node(out, node, flags, indent);
+      out = print_doctype_node(out, node, flags, indent);
-    break;
+      break;
 
-  // Doctype
+    // Pi
-  case node_doctype:
+    case node_pi:
-    out = print_doctype_node(out, node, flags, indent);
+      out = print_pi_node(out, node, flags, indent);
-    break;
+      break;
-
-  // Pi
-  case node_pi:
-    out = print_pi_node(out, node, flags, indent);
-    break;
 
     // Unknown
-  default:
+    default:
-    assert(0);
+      assert(0);
-    break;
+      break;
   }
 
   // If indenting not disabled, add line break after node
-  if (!(flags & print_no_indenting))
+  if (!(flags & print_no_indenting)) *out = Ch('\n'), ++out;
-    *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(),
+      if (find_char<Ch, Ch('"')>(
-                                 attribute->value() +
+              attribute->value(),
-                                     attribute->value_size())) {
+              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))
+  if (!(flags & print_no_indenting)) out = fill_chars(out, indent, Ch('\t'));
-    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))
+  if (!(flags & print_no_indenting)) out = fill_chars(out, indent, Ch('\t'));
-    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))
+  if (!(flags & print_no_indenting)) out = fill_chars(out, indent, Ch('\t'));
-    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))
+      if (!(flags & print_no_indenting)) *out = Ch('\n'), ++out;
-        *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))
+  if (!(flags & print_no_indenting)) out = fill_chars(out, indent, Ch('\t'));
-    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))
+  if (!(flags & print_no_indenting)) out = fill_chars(out, indent, Ch('\t'));
-    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))
+  if (!(flags & print_no_indenting)) out = fill_chars(out, indent, Ch('\t'));
-    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))
+  if (!(flags & print_no_indenting)) out = fill_chars(out, indent, Ch('\t'));
-    out = fill_chars(out, indent, Ch('\t'));
   *out = Ch('<'), ++out;
   *out = Ch('?'), ++out;
   out = copy_chars(node->name(), node->name() + node->name_size(), out);
@@ -394,7 +381,7 @@ inline OutIt print_pi_node(OutIt out, const xml_node<Ch> *node, int flags,
   return out;
 }
 
-} // namespace internal
+}  // namespace internal
 //! \endcond
 
 ///////////////////////////////////////////////////////////////////////////
@@ -436,6 +423,6 @@ inline std::basic_ostream<Ch> &operator<<(std::basic_ostream<Ch> &out,
 
 #endif
 
-} // namespace rapidxml
+}  // namespace rapidxml
 
 #endif

livox_ros_driver/common/rapidxml/rapidxml_utils.hpp

@@ -8,18 +8,18 @@
 //! 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:
+ public:
   //! Loads file into the memory. Data will be automatically destroyed by the
   //! destructor. \param filename Filename to load.
   file(const char *filename) {
@@ -27,8 +27,7 @@ public:
 
     // Open stream
     basic_ifstream<Ch> stream(filename, ios::binary);
-    if (!stream)
+    if (!stream) throw runtime_error(string("cannot open file ") + filename);
-      throw runtime_error(string("cannot open file ") + filename);
     stream.unsetf(ios::skipws);
 
     // Determine stream size
@@ -67,13 +66,14 @@ public:
   //! \return Size of file data, in characters.
   std::size_t size() const { return m_data.size(); }
 
-private:
+ private:
-  std::vector<Ch> m_data; // File data
+  std::vector<Ch> m_data;  // File data
 };
 
 //! 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) {
@@ -95,6 +96,6 @@ template <class Ch> inline std::size_t count_attributes(xml_node<Ch> *node) {
   return count;
 }
 
-} // namespace rapidxml
+}  // namespace rapidxml
 
 #endif

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,15 +26,15 @@
 #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)
 #define GET_STRING_DIRECT(n) #n
 
-#define LIVOX_ROS_DRIVER_VERSION_STRING                                        \
+#define LIVOX_ROS_DRIVER_VERSION_STRING                      \
-  GET_STRING(LIVOX_ROS_DRIVER_VER_MAJOR)                                       \
+  GET_STRING(LIVOX_ROS_DRIVER_VER_MAJOR)                     \
-  "." GET_STRING(LIVOX_ROS_DRIVER_VER_MINOR) "." GET_STRING(                   \
+  "." GET_STRING(LIVOX_ROS_DRIVER_VER_MINOR) "." GET_STRING( \
       LIVOX_ROS_DRIVER_VER_PATCH)
 
 #endif

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),
+    : transfer_format_(format),
-      data_src_(data_src), output_type_(output_type), publish_frq_(frq), \
+      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,7 +61,6 @@ Lddc::Lddc(int format, int multi_topic, int data_src, int output_type,
 };
 
 Lddc::~Lddc() {
-
   if (global_pub_) {
     delete global_pub_;
   }
@@ -87,15 +87,16 @@ Lddc::~Lddc() {
 }
 
 int32_t Lddc::GetPublishStartTime(LidarDevice *lidar, LidarDataQueue *queue,
-    uint64_t *start_time,StoragePacket *storage_packet) {
+                                  uint64_t *start_time,
-  QueueProPop(queue, storage_packet);
+                                  StoragePacket *storage_packet) {
-  uint64_t timestamp = GetStoragePacketTimestamp(storage_packet, 
+  QueuePrePop(queue, storage_packet);
-                                                 lidar->data_src);
+  uint64_t timestamp =
+      GetStoragePacketTimestamp(storage_packet, lidar->data_src);
   uint32_t remaining_time = timestamp % publish_period_ns_;
   uint32_t diff_time = publish_period_ns_ - remaining_time;
   /** Get start time, down to the period boundary */
   if (diff_time > (publish_period_ns_ / 4)) {
-    //ROS_INFO("0 : %u", diff_time);
+    // ROS_INFO("0 : %u", diff_time);
     *start_time = timestamp - remaining_time;
     return 0;
   } else if (diff_time <= lidar->packet_interval_max) {
@@ -103,20 +104,19 @@ int32_t Lddc::GetPublishStartTime(LidarDevice *lidar, LidarDataQueue *queue,
     return 0;
   } else {
     /** Skip some packets up to the period boundary*/
-    //ROS_INFO("2 : %u", diff_time);
+    // ROS_INFO("2 : %u", diff_time);
     do {
       if (QueueIsEmpty(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, 
+      timestamp = GetStoragePacketTimestamp(storage_packet, lidar->data_src);
-                                            lidar->data_src);
       remaining_time = timestamp % publish_period_ns_;
       /** Flip to another period */
       if (last_remaning_time > remaining_time) {
-        //ROS_INFO("Flip to another period, exit");
+        // ROS_INFO("Flip to another period, exit");
         break;
       }
       diff_time = publish_period_ns_ - remaining_time;
@@ -127,23 +127,10 @@ int32_t Lddc::GetPublishStartTime(LidarDevice *lidar, LidarDataQueue *queue,
   }
 }
 
-uint32_t Lddc::PublishPointcloud2(LidarDataQueue *queue, uint32_t packet_num,
+void Lddc::InitPointcloud2MsgHeader(sensor_msgs::PointCloud2& cloud) {
-                                  uint8_t handle) {
-  uint64_t timestamp = 0;
-  uint64_t last_timestamp = 0;
-  uint32_t published_packet = 0;
-
-  StoragePacket storage_packet;
-  LidarDevice *lidar  = &lds_->lidars_[handle];
-  if (GetPublishStartTime(lidar, queue, &last_timestamp, &storage_packet)) {
-    /* the remaning packets in queue maybe not enough after skip */
-    return 0;
-  }
-  
-  sensor_msgs::PointCloud2 cloud;
   cloud.header.frame_id.assign(frame_id_);
   cloud.height = 1;
-  cloud.width  = 0;
+  cloud.width = 0;
   cloud.fields.resize(6);
   cloud.fields[0].offset = 0;
   cloud.fields[0].name = "x";
@@ -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);
+        timestamp = last_timestamp + lidar->packet_interval;
-        cloud.width += storage_packet.point_num;
+        ZeroPointDataOfStoragePacket(&storage_packet);
-        last_timestamp = last_timestamp + lidar->packet_interval;
+        is_zero_packet = 1;
-        if (!published_packet) { 
-          cloud.header.stamp = ros::Time(last_timestamp / 1000000000.0);
-        }
-        ++published_packet;
-        continue;
       }
     }
     /** Use the first packet timestamp as pointcloud2 msg timestamp */
     if (!published_packet) {
       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 = pf_point_convert(point_base, raw_packet,
-            point_base, raw_packet, lidar->extrinsic_parameter);
+            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 = LivoxPointToPxyzrtl(point_base, raw_packet,
-          point_base, raw_packet, lidar->extrinsic_parameter);
+          lidar->extrinsic_parameter, line_num);
     }
 
-    QueuePopUpdate(queue);
+    if (!is_zero_packet) {
-    last_timestamp = timestamp;
+      QueuePopUpdate(queue);
+    } else {
+      is_zero_packet = 0;
+    }
+    cloud.width += single_point_num;
     ++published_packet;
+    last_timestamp = timestamp;
   }
-
+  cloud.row_step     = cloud.width * cloud.point_step;
-  cloud.row_step = cloud.width * cloud.point_step;
   cloud.is_bigendian = false;
-  cloud.is_dense = true;
+  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) {
@@ -253,22 +272,24 @@ uint32_t Lddc::PublishPointcloudData(LidarDataQueue *queue, uint32_t packet_num,
   uint32_t published_packet = 0;
 
   StoragePacket storage_packet;
-  LidarDevice *lidar  = &lds_->lidars_[handle];
+  LidarDevice *lidar = &lds_->lidars_[handle];
   if (GetPublishStartTime(lidar, queue, &last_timestamp, &storage_packet)) {
     /* the remaning packets in queue maybe not enough after skip */
     return 0;
   }
 
-  /* init point cloud data struct */
   PointCloud::Ptr cloud(new PointCloud);
   cloud->header.frame_id.assign(frame_id_);
-  /* cloud->header.stamp = ros::Time::now(); */
   cloud->height = 1;
-  cloud->width  = 0;
+  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 */
+      if (kSourceLvxFile != data_source) {
-      for (uint32_t i = 0; i < storage_packet.point_num; i++) {
+        timestamp = last_timestamp + lidar->packet_interval;
-        cloud->points.push_back(point);
+        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->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,
+        pf_point_convert(point_buf, raw_packet, lidar->extrinsic_parameter, \
-                         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,
+      LivoxPointToPxyzrtl(point_buf, raw_packet, lidar->extrinsic_parameter, \
-                          lidar->extrinsic_parameter);
+          line_num);
     }
-
     LivoxPointXyzrtl *dst_point = (LivoxPointXyzrtl *)point_buf;
-    for (uint32_t i = 0; i < storage_packet.point_num; i++) {
+    FillPointsToPclMsg(cloud, dst_point, single_point_num);
-      pcl::PointXYZI point;
+    if (!is_zero_packet) {
-      point.x = dst_point->x;
+      QueuePopUpdate(queue);
-      point.y = dst_point->y;
+    } else {
-      point.z = dst_point->z;
+      is_zero_packet = 0;
-      point.intensity = dst_point->reflectivity;
-      ++dst_point;
-      cloud->points.push_back(point);
     }
-
+    cloud->width += single_point_num;
-    QueuePopUpdate(queue);
-    last_timestamp = timestamp;
     ++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) &&
+    int64_t packet_gap = timestamp - last_timestamp;
-        published_packet && lidar->data_is_pubulished) {
+    if ((packet_gap > lidar->packet_interval_max) &&
-      ROS_INFO("Lidar[%d] packet loss", handle);
+        lidar->data_is_pubulished) {
-      break;
+      // 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
+      livox_msg.timebase = timestamp;
-      packet_offset_time = 0;         // first packet
+      packet_offset_time = 0;
-      livox_msg.header.stamp =
+      /** convert to ros time stamp */
-          ros::Time(timestamp / 1000000000.0); // to ros time stamp
+      livox_msg.header.stamp = ros::Time(timestamp / 1000000000.0);
-      // ROS_DEBUG("[%d]:%ld %d", handle, livox_msg.timebase, point_interval);
     } else {
       packet_offset_time = (uint32_t)(timestamp - livox_msg.timebase);
     }
-    livox_msg.point_num += storage_packet.point_num;
+    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,
+        pf_point_convert(point_buf, raw_packet, lidar->extrinsic_parameter, \
-                         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,
+      LivoxPointToPxyzrtl(point_buf, raw_packet, lidar->extrinsic_parameter, \
-                          lidar->extrinsic_parameter);
+          line_num);
     }
-
     LivoxPointXyzrtl *dst_point = (LivoxPointXyzrtl *)point_buf;
-    for (uint32_t i = 0; i < storage_packet.point_num; i++) {
+    FillPointsToCustomMsg(livox_msg, dst_point, single_point_num, \
-      livox_ros_driver::CustomPoint point;
+        packet_offset_time, point_interval, echo_num);
-      if (!dual_point) { /** dual return mode */
+
-        point.offset_time = packet_offset_time + i * point_interval;
+    if (!is_zero_packet) {
-      } else {
+      QueuePopUpdate(queue);
-        point.offset_time = packet_offset_time + (i / 2) * point_interval;
+    } else {
-      }
+      is_zero_packet = 0;
-      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);
     }
 
-    QueuePopUpdate(queue);
+    livox_msg.point_num += single_point_num;
     last_timestamp = timestamp;
     ++published_packet;
   }
@@ -441,14 +466,13 @@ uint32_t Lddc::PublishCustomPointcloud(LidarDataQueue *queue,
   } else {
     if (bag_) {
       bag_->write(p_publisher->getTopic(), ros::Time(timestamp / 1000000000.0),
-                  livox_msg);
+          livox_msg);
     }
   }
 
   if (!lidar->data_is_pubulished) {
     lidar->data_is_pubulished = true;
   }
-
   return published_packet;
 }
 
@@ -462,13 +486,13 @@ 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);
   if (timestamp >= 0) {
     imu_data.header.stamp =
-        ros::Time(timestamp / 1000000000.0); // to ros time stamp
+        ros::Time(timestamp / 1000000000.0);  // to ros time stamp
   }
 
   uint8_t point_buf[2048];
@@ -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 =
+    uint32_t onetime_publish_packets = lidar->onetime_publish_packets;
-        GetPacketNumPerSec(lidar->raw_data_type) / publish_frq_;
     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,9 +624,10 @@ 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(
-               "size %d",
+          "%s publish use pcl PointXYZI format, set ROS publisher queue "
-               name_str, queue_size);
+          "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("Save the bag file successfully!");
-    ROS_INFO("Press [Ctrl+C] to exit!\n");
     bag_ = nullptr;
   }
   if (lds_) {
@@ -665,4 +687,4 @@ void Lddc::PrepareExit(void) {
   }
 }
 
-} // namespace livox_ros
+}  // namespace livox_ros

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,
@@ -40,8 +45,8 @@ typedef enum {
 } TransferType;
 
 class Lddc {
-public:
+ 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();
 
@@ -59,9 +64,10 @@ public:
 
   Lds *lds_;
 
-private:
+ 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_;
@@ -92,5 +103,5 @@ private:
   rosbag::Bag *bag_;
 };
 
-} // namespace livox_ros
+}  // namespace livox_ros
 #endif

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;
@@ -132,4 +130,4 @@ uint32_t QueuePushAny(LidarDataQueue *queue, uint8_t *data, uint32_t length,
   return 1;
 }
 
-} // namespace livox_ros
+}  // namespace livox_ros

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);
@@ -82,5 +82,5 @@ uint32_t QueuePush(LidarDataQueue *queue, StoragePacket *storage_packet);
 uint32_t QueuePushAny(LidarDataQueue *queue, uint8_t *data, uint32_t length,
                       uint64_t time_rcv, uint32_t point_num);
 
-} // namespace livox_ros
+}  // namespace livox_ros
 #endif

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;
@@ -64,17 +62,17 @@ uint64_t GetStoragePacketTimestamp(StoragePacket *packet, uint8_t data_src_) {
   } else if (raw_packet->timestamp_type == kTimestampTypePpsGps) {
     struct tm time_utc;
     time_utc.tm_isdst = 0;
-    time_utc.tm_year = raw_packet->timestamp[0] + 100; // map 2000 to 1990
+    time_utc.tm_year = raw_packet->timestamp[0] + 100;  // map 2000 to 1990
-    time_utc.tm_mon  = raw_packet->timestamp[1] - 1;   // map 1~12 to 0~11
+    time_utc.tm_mon = raw_packet->timestamp[1] - 1;     // map 1~12 to 0~11
     time_utc.tm_mday = raw_packet->timestamp[2];
     time_utc.tm_hour = raw_packet->timestamp[3];
     time_utc.tm_min = 0;
     time_utc.tm_sec = 0;
 
-    //uint64_t time_epoch = mktime(&time_utc);
+    // uint64_t time_epoch = mktime(&time_utc);
-    uint64_t time_epoch = timegm(&time_utc); // no timezone
+    uint64_t time_epoch = timegm(&time_utc);  // no timezone
-    time_epoch = time_epoch * 1000000 + timestamp.stamp_word.high; // to us
+    time_epoch = time_epoch * 1000000 + timestamp.stamp_word.high;  // to us
-    time_epoch = time_epoch * 1000;                                // to ns
+    time_epoch = time_epoch * 1000;                                 // to ns
 
     return time_epoch;
   } else {
@@ -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 CalculatePacketQueueSize(uint32_t interval_ms, uint8_t product_type,
-  uint32_t queue_size = (interval_ms * GetPacketNumPerSec(data_type)) / 1000;
+                                  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,
+static uint8_t *LivoxRawPointToPxyzrtl(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);
   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,
+static uint8_t *LivoxSpherPointToPxyzrtl(uint8_t *point_buf, \
-                                  LivoxEthPacket *eth_packet,
+    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);
   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,
+static uint8_t *LivoxExtendRawPointToPxyzrtl(uint8_t *point_buf, \
-                                      LivoxEthPacket *eth_packet,
+    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);
   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;
+    if (line_num > 1) {
-    dst_point->line = dst_point->line % 6;
+      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,
+static uint8_t *LivoxExtendSpherPointToPxyzrtl(uint8_t *point_buf, \
-                                        LivoxEthPacket *eth_packet,
+    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);
   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;
+    if (line_num > 1) {
-    dst_point->line = dst_point->line % 6;
+      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,
+static uint8_t *LivoxDualExtendRawPointToPxyzrtl(uint8_t *point_buf, \
-                                          LivoxEthPacket *eth_packet,
+    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);
   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 =
+    if (line_num > 1) {
-        line_id / 2; /* LivoxDualExtendRawPoint = 2*LivoxExtendRawPoint */
+      dst_point->line = (line_id / 2) % line_num;
-    dst_point->line = dst_point->line % 6;
+    } 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,
+static uint8_t *LivoxDualExtendSpherPointToPxyzrtl(uint8_t *point_buf, \
-                                            LivoxEthPacket *eth_packet,
+    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);
   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;
+    if (line_num > 1) {
-    dst_point->line = dst_point->line % 6;
+      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;
+    if (line_num > 1) {
-    dst_point->line = dst_point->line % 6;
+      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) {
+void ZeroPointDataOfStoragePacket(StoragePacket* storage_packet) {
-  LivoxPointXyzrtl *dst_point = (LivoxPointXyzrtl *)point_buf;
+  LivoxEthPacket *raw_packet =
-  uint32_t points_per_packet = num;
+      reinterpret_cast<LivoxEthPacket *>(storage_packet->raw_data);
-
+  uint32_t point_length = GetPointLen(raw_packet->data_type);
-  while (points_per_packet) {
+  memset(raw_packet->data, 0, point_length * storage_packet->point_num);
-    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;
 }
 
 #if 0
@@ -415,14 +518,11 @@ static void PointCloudConvert(LivoxPoint *p_dpoint, LivoxRawPoint *p_raw_point)
 
 #endif
 
-/* Member function ------
+/* Member function --------------------------------------------------------- */
- * ----------------------------------------------------------------------- */
+Lds::Lds(uint32_t buffer_time_ms, uint8_t data_src) : \
-
+    lidar_count_(kMaxSourceLidar), semaphore_(0), \
-Lds::Lds(uint32_t buffer_time_ms, uint8_t data_src)
+    buffer_time_ms_(buffer_time_ms), data_src_(data_src), request_exit_(false) {
-    : buffer_time_ms_(buffer_time_ms), data_src_(data_src) {
+    ResetLds(data_src_);
-  lidar_count_ = kMaxSourceLidar;
-  request_exit_ = false;
-  ResetLds(data_src_);
 };
 
 Lds::~Lds() {
@@ -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
+}  // 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 =
+/**< the threshold of packet continuous */
-    1700000; /**< the threshold of packet continuous */
+const int64_t kMaxPacketTimeGap = 1700000;
-const int64_t kDeviceDisconnectThreshold =
+/**< the threshold of device disconect */
-    1000000000; /**< 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 */
@@ -98,9 +100,9 @@ typedef enum {
   kConfigFan = 1 << 0,
   kConfigReturnMode = 1 << 1,
   kConfigCoordinate = 1 << 2,
-  kConfigImuRate    = 1 << 3,
+  kConfigImuRate = 1 << 3,
   kConfigGetExtrinsicParameter = 1 << 4,
-  kConfigSetHighSensitivity    = 1 << 5,  
+  kConfigSetHighSensitivity = 1 << 5,
   kConfigUndef
 } LidarConfigCodeBit;
 
@@ -167,15 +169,17 @@ typedef struct {
 
 /** Lidar data source info abstract */
 typedef struct {
-  uint8_t handle;          /**< Lidar access handle. */
+  uint8_t handle;                    /**< Lidar access handle. */
-  uint8_t data_src;        /**< From raw lidar or livox file. */
+  uint8_t data_src;                  /**< From raw lidar or livox file. */
-  uint8_t raw_data_type;   /**< The data type in eth packaet */
+  uint8_t raw_data_type;             /**< The data type in eth packaet */
-  bool data_is_pubulished; /**< Indicate the data of lidar whether is
+  bool data_is_pubulished;           /**< Indicate the data of lidar whether is
-                                pubulished. */
+                                          pubulished. */
-  volatile uint32_t packet_interval;/**< The time interval between packets 
+  volatile uint32_t packet_interval; /**< The time interval between packets
-                                       of current lidar, unit:ns */
+                                        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_packet; /**< number of points every packet */
-  uint32_t points_per_second;
+  uint32_t packet_length;     /**< length of raw ethenet packet unit:bytes */
-  uint32_t point_interval;  /**< unit:ns */
+  uint32_t raw_point_length;  /**< length of point uint:bytes */
-  uint32_t packet_interval; /**< unit:ns */
+  uint32_t echo_num;          /**< echo number of current data */
-  uint32_t packet_length;
+} DataTypePointInfoPair;
-} PacketInfoPair;
+
+typedef struct {
+  uint32_t points_per_second; /**< number of points per second */
+  uint32_t point_interval;    /**< unit:ns */
+  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,
+typedef uint8_t *(*PointConvertHandler)(uint8_t *point_buf, \
-                                        LivoxEthPacket *eth_packet,
+    LivoxEthPacket *eth_packet, ExtrinsicParameter &extrinsic, \
-                                        ExtrinsicParameter &extrinsic);
+    uint32_t line_num);
 
-const PacketInfoPair packet_info_pair_table[kMaxPointDataType] = {
+const DataTypePointInfoPair data_type_info_pair_table[kMaxPointDataType] = {
-    {100, 100000, 10000, 1000000, 1318}, {100, 100000, 10000, 1000000, 918},
+    {100, 1318, sizeof(LivoxRawPoint), 1},
-    {96, 240000, 4167, 400000, 1362},    {96, 240000, 4167, 400000, 978},
+    {100, 918,  9,  1},
-    {96, 480000, 4167, 400000, 1362},    {48, 480000, 4167, 400000, 978},
+    {96,  1362, 14, 1},
-    {1, 200, 10000000, 10000000, 42}};
+    {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_);
+uint64_t GetStoragePacketTimestamp(StoragePacket *packet, uint8_t data_src);
-uint32_t CalculatePacketQueueSize(uint32_t interval_ms, uint32_t data_type);
+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);
+    ExtrinsicParameter &extrinsic, uint32_t line_num);
-uint8_t *FillZeroPointXyzrtl(uint8_t *point_buf, uint32_t 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) {
+inline uint32_t GetPointInterval(uint32_t product_type) {
-  return packet_info_pair_table[data_type].point_interval;
+  return product_type_info_pair_table[product_type].point_interval;
 }
 
-inline uint32_t GetPacketNumPerSec(uint32_t data_type) {
+// inline uint32_t GetPacketNumPerSec(uint32_t data_type) {
-  return packet_info_pair_table[data_type].points_per_second /
+//  return packet_info_pair_table[data_type].points_per_second /
-         packet_info_pair_table[data_type].points_per_packet;
+//         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;
+  return data_type_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;
 }
 
 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,39 +353,104 @@ 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->x = radius2 * sin(theta) * cos(phi);
-  (dst_point2 + 1)->y = radius2 * sin(theta) * sin(phi);
+  dst_point2->y = radius2 * sin(theta) * sin(phi);
-  (dst_point2 + 1)->z = radius2 * cos(theta);
+  dst_point2->z = radius2 * cos(theta);
-  (dst_point2 + 1)->reflectivity = (float)raw_point->reflectivity2;
+  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.
  */
 class Lds {
-public:
+ 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:
+ protected:
   uint32_t buffer_time_ms_; /**< Buffer time before data in queue is read */
   uint8_t data_src_;
 
-private:
+ private:
   volatile bool request_exit_;
 };
 
-} // namespace livox_ros
+}  // namespace livox_ros
 #endif

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,8 +99,9 @@ int LdsHub::InitLdsHub(std::vector<std::string> &broadcast_code_strs,
     }
   } else {
     EnableAutoConnectMode();
-    printf("No broadcast code was added to whitelist, swith to automatic "
+    printf(
-           "connection mode!\n");
+        "No broadcast code was added to whitelist, swith to automatic "
+        "connection mode!\n");
   }
 
   /** Start livox sdk to receive lidar data */
@@ -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];
+  lds_hub->StorageRawPacket(handle, eth_packet);
-  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));
-    }
-  }
 }
 
 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)) {
@@ -779,4 +709,4 @@ int LdsHub::GetRawConfig(const char *broadcast_code, UserRawConfig &config) {
   return -1;
 }
 
-} // namespace livox_ros
+}  // namespace livox_ros

livox_ros_driver/livox_ros_driver/lds_hub.h

@@ -39,7 +39,7 @@ namespace livox_ros {
  * LiDAR data source, data from hub.
  */
 class LdsHub : public Lds {
-public:
+ public:
   static LdsHub *GetInstance(uint32_t interval_ms) {
     static LdsHub lds_hub(interval_ms);
     return &lds_hub;
@@ -49,7 +49,7 @@ public:
                  const char *user_config_path);
   int DeInitLdsHub(void);
 
-private:
+ private:
   LdsHub(uint32_t interval_ms);
   LdsHub(const LdsHub &) = delete;
   ~LdsHub();
@@ -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
+  static void HubSetPointCloudReturnModeCb(
-  HubSetPointCloudReturnModeCb(livox_status status, uint8_t handle,
+      livox_status status, uint8_t handle,
-                               HubSetPointCloudReturnModeResponse *response,
+      HubSetPointCloudReturnModeResponse *response, void *clent_data);
-                               void *clent_data);
   static void SetCoordinateCb(livox_status status, uint8_t handle,
                               uint8_t response, void *clent_data);
-  static void
+  static void HubSetImuRatePushFrequencyCb(
-  HubSetImuRatePushFrequencyCb(livox_status status, uint8_t handle,
+      livox_status status, uint8_t handle,
-                               HubSetImuPushFrequencyResponse *response,
+      HubSetImuPushFrequencyResponse *response, void *clent_data);
-                               void *clent_data);
   static void HubErrorStatusCb(livox_status status, uint8_t handle,
                                ErrorMessage *message);
   static void ConfigPointCloudReturnMode(LdsHub *lds_hub);
@@ -118,5 +116,5 @@ private:
   UserRawConfig hub_raw_config_;
 };
 
-} // namespace livox_ros
+}  // namespace livox_ros
 #endif

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,8 +97,9 @@ int LdsLidar::InitLdsLidar(std::vector<std::string> &broadcast_code_strs,
     }
   } else {
     EnableAutoConnectMode();
-    printf("No broadcast code was added to whitelist, swith to automatic "
+    printf(
-           "connection mode!\n");
+        "No broadcast code was added to whitelist, swith to automatic "
+        "connection mode!\n");
   }
 
   if (enable_timesync_) {
@@ -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];
+  lds_lidar->StorageRawPacket(handle, eth_packet);
-  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));
-    }
-  }
 }
 
 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 =
+    p_lidar->config.enable_high_sensitivity = 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,
+          LidarEnableHighSensitivity(handle, SetHighSensitivityCb, g_lds_ldiar);
-                                     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,
+      LidarEnableHighSensitivity(handle, SetHighSensitivityCb, g_lds_ldiar);
-                                 g_lds_ldiar);
     } else {
-      LidarDisableHighSensitivity(handle, SetHighSensitivityCb,
+      LidarDisableHighSensitivity(handle, SetHighSensitivityCb, g_lds_ldiar);
-                                  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())
+    if (!object.IsObject()) break;
-      break;
 
     if (!object.HasMember("enable_timesync") ||
         !object["enable_timesync"].IsBool())
@@ -854,4 +777,4 @@ int LdsLidar::GetRawConfig(const char *broadcast_code, UserRawConfig &config) {
   return -1;
 }
 
-} // namespace livox_ros
+}  // namespace livox_ros

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"
@@ -42,7 +42,7 @@ namespace livox_ros {
  * LiDAR data source, data from dependent lidar.
  */
 class LdsLidar : public Lds {
-public:
+ public:
   static LdsLidar *GetInstance(uint32_t interval_ms) {
     static LdsLidar lds_lidar(interval_ms);
     return &lds_lidar;
@@ -52,7 +52,7 @@ public:
                    const char *user_config_path);
   int DeInitLdsLidar(void);
 
-private:
+ private:
   LdsLidar(uint32_t interval_ms);
   LdsLidar(const LdsLidar &) = delete;
   ~LdsLidar();
@@ -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
+  static void GetLidarExtrinsicParameterCb(
-  GetLidarExtrinsicParameterCb(livox_status status, uint8_t handle,
+      livox_status status, uint8_t handle,
-                               LidarGetExtrinsicParameterResponse *response,
+      LidarGetExtrinsicParameterResponse *response, void *clent_data);
-                               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);
@@ -116,5 +116,5 @@ private:
   std::mutex config_mutex_;
 };
 
-} // namespace livox_ros
+}  // namespace livox_ros
 #endif

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();
+  uint32_t valid_lidar_count_ = lvx_file_->GetDeviceCount();
-  if (!lidar_count_ || (lidar_count_ >= kMaxSourceLidar)) {
+  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;
+    uint8_t handle = lvx_dev_info.device_index;
-    lidars_[i].connect_state = kConnectStateSampling;
+    if (handle >= kMaxSourceLidar) {
-    lidars_[i].info.handle = i;
+      printf("Invalid hanle from lvx file!\n");
-    lidars_[i].info.type = lvx_dev_info.device_type;
+      continue;
-    memcpy(lidars_[i].info.broadcast_code, lvx_dev_info.lidar_broadcast_code,
+    }
-           sizeof(lidars_[i].info.broadcast_code));
+    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,31 +168,18 @@ void LdsLvx::ReadLvxFile() {
         data_type = eth_packet->data_type;
         /** Packet length + device index */
         data_offset += (GetEthPacketLen(data_type) + 1);
-        if (data_type != kImu) {
+        StorageRawPacket(handle, eth_packet);
-          LidarDevice *p_lidar = &lidars_[handle];
+
-          LidarDataQueue *p_queue = &lidars_[handle].data;
+        LidarDataQueue *p_queue = &lidars_[handle].data;
-          if (p_lidar->raw_data_type != eth_packet->data_type) {
+        if (p_queue != nullptr) {
-            p_lidar->raw_data_type = eth_packet->data_type;
+          while (QueueIsFull(p_queue)) {
-            p_lidar->packet_interval = GetPacketInterval(eth_packet->data_type);
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
-            p_lidar->packet_interval_max = p_lidar->packet_interval * 1.8f;
           }
-          if ((p_queue != nullptr) && (handle < lidar_count_)) {
+        }
-            while (QueueIsFull(p_queue)) {
+        p_queue = &lidars_[handle].imu_data;
-              std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        if (p_queue != nullptr) {
-            }
+          while (QueueIsFull(p_queue)) {
-            QueuePushAny(p_queue, (uint8_t *)eth_packet,
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
-                         GetEthPacketLen(data_type), 0,
-                         GetPointsPerPacket(data_type));
-          }
-        } else {
-          LidarDataQueue *p_queue = &lidars_[handle].imu_data;
-          if ((p_queue != nullptr) && (handle < lidar_count_)) {
-            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));
           }
         }
       }
@@ -207,10 +197,13 @@ void LdsLvx::ReadLvxFile() {
       printf("Read progress : %d \n", progress);
     }
   }
-
+  printf("Wait for file conversion to complete!\n");
-  int32_t wait_cnt = 10;
+  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;
-    }
   }
-
+  semaphore_.Signal();
-  return true;
 }
 
-bool LdsLvx::IsAllQueueReadStop() {
+}  // namespace livox_ros
-  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;
-}
-
-} // namespace livox_ros

livox_ros_driver/livox_ros_driver/lds_lvx.h

@@ -39,7 +39,7 @@ namespace livox_ros {
  * Lidar data source abstract.
  */
 class LdsLvx : public Lds {
-public:
+ public:
   static LdsLvx *GetInstance(uint32_t interval_ms) {
     static LdsLvx lds_lvx(interval_ms);
     return &lds_lvx;
@@ -49,7 +49,7 @@ public:
   int DeInitLdsLvx(void);
   void PrepareExit(void);
 
-private:
+ private:
   LdsLvx(uint32_t interval_ms);
   LdsLvx(const LdsLvx &) = delete;
   ~LdsLvx();
@@ -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_;
@@ -70,5 +68,5 @@ private:
   volatile bool start_read_lvx_;
 };
 
-} // namespace livox_ros
+}  // namespace livox_ros
 #endif

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),
+    : file_ver_(kLvxFileV1),
-      cur_offset_(0), data_start_offset_(0), size_(0), mode_(0), state_(0) {
+      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())
+  if (lvx_file_ && lvx_file_.is_open()) lvx_file_.close();
-    lvx_file_.close();
 }
 
 void LvxFileHandle::BasePointsHandle(LivoxEthPacket *data,
@@ -416,4 +420,4 @@ void ParseExtrinsicXml(DeviceItem &item, LvxFileDeviceInfo &info) {
   }
 }
 
-} // namespace livox_ros
+}  // namespace livox_ros

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 {
+typedef struct { uint8_t device_count; } LvxFilePrivateHeaderV0;
-  uint8_t device_count;
-} LvxFilePrivateHeaderV0;
 
 typedef struct {
   uint8_t lidar_broadcast_code[16];
@@ -172,7 +170,7 @@ typedef struct {
 #pragma pack()
 
 class LvxFileHandle {
-public:
+ public:
   LvxFileHandle();
   ~LvxFileHandle() = default;
 
@@ -196,7 +194,7 @@ public:
   int GetLvxFileReadProgress();
   int GetFileVersion() { return file_ver_; }
 
-private:
+ private:
   std::fstream lvx_file_;
   std::vector<LvxFileDeviceInfo> device_info_list_;
   uint8_t file_ver_;
@@ -232,5 +230,5 @@ private:
   }
 };
 
-} // namespace livox_ros
+}  // namespace livox_ros
 #endif

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_)
+  if (uart_) delete uart_;
-    delete uart_;
+  if (comm_) delete comm_;
-  if (comm_)
-    delete comm_;
 
   fn_cb_ = nullptr;
   client_data_ = nullptr;
@@ -197,4 +197,4 @@ void TimeSync::FsmCheckDevState() {
   }
 }
 
-} // namespace livox_ros
+}  // namespace livox_ros

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 {
 
@@ -43,7 +43,7 @@ typedef struct {
 } TimeSyncConfig;
 
 class TimeSync {
-public:
+ public:
   static TimeSync *GetInstance() {
     static TimeSync time_sync;
 
@@ -67,7 +67,7 @@ public:
     }
   }
 
-private:
+ private:
   TimeSync();
   ~TimeSync();
   TimeSync(const TimeSync &) = delete;
@@ -101,5 +101,5 @@ private:
   void FsmCheckDevState();
 };
 
-} // namespace livox_ros
+}  // namespace livox_ros
 #endif

livox_ros_driver/timesync/user_uart/user_uart.cpp

@@ -51,7 +51,7 @@ UserUart::~UserUart() {
 }
 
 int UserUart::Open(const char *filename) {
-  fd_ = open(filename, O_RDWR | O_NOCTTY); //| O_NDELAY
+  fd_ = open(filename, O_RDWR | O_NOCTTY);  //| O_NDELAY
   if (fd_ < 0) {
     printf("Open %s fail!\n", filename);
     return -1;
@@ -72,7 +72,6 @@ int UserUart::Open(const char *filename) {
 }
 
 int UserUart::Close() {
-
   is_open_ = false;
   if (fd_ > 0) {
     /** first we flush the port */
@@ -121,38 +120,38 @@ int UserUart::Setup(uint8_t baudrate_index, uint8_t parity) {
   options.c_cflag |= baudrate;
 
   switch (parity) {
-  case P_8N1:
+    case P_8N1:
-    /** No parity (8N1)  */
+      /** No parity (8N1)  */
-    options.c_cflag &= ~PARENB;
+      options.c_cflag &= ~PARENB;
-    options.c_cflag &= ~CSTOPB;
+      options.c_cflag &= ~CSTOPB;
-    options.c_cflag &= ~CSIZE;
+      options.c_cflag &= ~CSIZE;
-    options.c_cflag |= CS8;
+      options.c_cflag |= CS8;
-    break;
+      break;
-  case P_7E1:
+    case P_7E1:
-    /** Even parity (7E1) */
+      /** Even parity (7E1) */
-    options.c_cflag |= PARENB;
+      options.c_cflag |= PARENB;
-    options.c_cflag &= ~PARODD;
+      options.c_cflag &= ~PARODD;
-    options.c_cflag &= ~CSTOPB;
+      options.c_cflag &= ~CSTOPB;
-    options.c_cflag &= ~CSIZE;
+      options.c_cflag &= ~CSIZE;
-    options.c_cflag |= CS7;
+      options.c_cflag |= CS7;
-    break;
+      break;
-  case P_7O1:
+    case P_7O1:
-    /** Odd parity (7O1) */
+      /** Odd parity (7O1) */
-    options.c_cflag |= PARENB;
+      options.c_cflag |= PARENB;
-    options.c_cflag |= PARODD;
+      options.c_cflag |= PARODD;
-    options.c_cflag &= ~CSTOPB;
+      options.c_cflag &= ~CSTOPB;
-    options.c_cflag &= ~CSIZE;
+      options.c_cflag &= ~CSIZE;
-    options.c_cflag |= CS7;
+      options.c_cflag |= CS7;
-    break;
+      break;
-  case P_7S1:
+    case P_7S1:
-    /** Space parity is setup the same as no parity (7S1)  */
+      /** Space parity is setup the same as no parity (7S1)  */
-    options.c_cflag &= ~PARENB;
+      options.c_cflag &= ~PARENB;
-    options.c_cflag &= ~CSTOPB;
+      options.c_cflag &= ~CSTOPB;
-    options.c_cflag &= ~CSIZE;
+      options.c_cflag &= ~CSIZE;
-    options.c_cflag |= CS8;
+      options.c_cflag |= CS8;
-    break;
+      break;
-  default:
+    default:
-    return -1;
+      return -1;
   }
 
   /** now we setup the values in port's termios */
@@ -192,4 +191,4 @@ ssize_t UserUart::Read(char *buffer, size_t size) {
   }
 }
 
-} // namespace livox_ros
+}  // namespace livox_ros

livox_ros_driver/timesync/user_uart/user_uart.h

@@ -66,8 +66,7 @@ enum BaudRate {
 };
 
 class UserUart {
-
+ public:
-public:
   UserUart(uint8_t baudrate_index, uint8_t parity);
   ~UserUart();
 
@@ -78,7 +77,7 @@ public:
   int Open(const char *filename);
   bool IsOpen() { return is_open_; };
 
-private:
+ private:
   int fd_;
   volatile bool is_open_;
 
@@ -86,6 +85,6 @@ private:
   uint8_t parity_;
 };
 
-} // namespace livox_ros
+}  // namespace livox_ros
 
 #endif