feat:support AVIA and Mid-70

Fix double free of global_pub_

README.md

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

README_CN.md

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

livox_ros_driver/CMakeLists.txt

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

livox_ros_driver/common/FastCRC/FastCRC.h

@@ -49,23 +49,23 @@
 // ================= 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
                                 // 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
@@ -73,7 +73,7 @@ public:
       const uint8_t *data,
       uint16_t len);  // Call for subsequent calculations with previous seed
 
-private:
+ private:
   uint32_t seed_;
 };
 

livox_ros_driver/common/FastCRC/FastCRCsw.cpp

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

livox_ros_driver/common/comm/comm_protocol.cpp

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

livox_ros_driver/common/comm/comm_protocol.h

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

livox_ros_driver/common/comm/gps_protocol.cpp

@@ -114,12 +114,10 @@ uint8_t GpsProtocol::CalcGpsPacketChecksum(const uint8_t *buf,
 
 uint8_t AscciiToHex(const uint8_t *TwoChar) {
   uint8_t h = toupper(TwoChar[0]) - 0x30;
-  if (h > 9)
+  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;
 }

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,7 +69,7 @@ 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);

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,

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,7 +78,7 @@ public:
 
   int32_t CheckPacket(const uint8_t *buf) override;
 
-private:
+ private:
   FastCRC16 crc16_;
   FastCRC32 crc32_;
 };

livox_ros_driver/common/rapidjson/allocators.h

@@ -78,7 +78,7 @@ 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.
@@ -118,8 +118,9 @@ 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.
               //!< (concept Allocator)
@@ -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);
@@ -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,8 +246,7 @@ 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;
@@ -253,7 +255,7 @@ public:
   //! Frees a memory block (concept Allocator)
   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.

livox_ros_driver/common/rapidjson/cursorstreamwrapper.h

@@ -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,7 +63,7 @@ public:
   //! Get the error column number, if error exists.
   size_t GetColumn() const { return col_; }
 
-private:
+ private:
   size_t line_;  //!< Current Line
   size_t col_;   //!< Current Column
 };

livox_ros_driver/common/rapidjson/document.h

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

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,7 +149,7 @@ public:
     return 0;
   }
 
-private:
+ private:
   EncodedOutputStream(const EncodedOutputStream &);
   EncodedOutputStream &operator=(const EncodedOutputStream &);
 
@@ -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));
@@ -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 &);
 

livox_ros_driver/common/rapidjson/encodings.h

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

livox_ros_driver/common/rapidjson/error/en.h

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

livox_ros_driver/common/rapidjson/error/error.h

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

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,7 +36,7 @@ RAPIDJSON_NAMESPACE_BEGIN
     \note implements Stream concept
 */
 class FileReadStream {
-public:
+ public:
   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_;

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.
 
   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);
 }
 

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>

livox_ros_driver/common/rapidjson/internal/biginteger.h

@@ -30,7 +30,7 @@ RAPIDJSON_NAMESPACE_BEGIN
 namespace internal {
 
 class BigInteger {
-public:
+ public:
   typedef uint64_t Type;
 
   BigInteger(const BigInteger &rhs) : count_(rhs.count_) {
@@ -51,8 +51,7 @@ public:
       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
     for (; exp >= 13; exp -= 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;
   }
 
@@ -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,7 +225,7 @@ 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())
@@ -273,8 +255,7 @@ private:
                            uint64_t *outHigh) {
 #if defined(_MSC_VER) && defined(_M_AMD64)
     uint64_t low = _umul128(a, b, outHigh) + k;
-    if (low < k)
+    if (low < k) (*outHigh)++;
-      (*outHigh)++;
     return low;
 #elif (__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 6)) && \
     defined(__x86_64__)
@@ -289,14 +270,12 @@ private:
     uint64_t x0 = a0 * b0, x1 = a0 * b1, x2 = a1 * b0, x3 = a1 * b1;
     x1 += (x0 >> 32);  // can't give carry
     x1 += x2;
-    if (x1 < x2)
+    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

livox_ros_driver/common/rapidjson/internal/clzll.h

@@ -49,8 +49,7 @@ inline uint32_t clzll(uint64_t x) {
   _BitScanReverse64(&r, x);
 #else
   // Scan the high 32 bits.
-  if (_BitScanReverse(&r, static_cast<uint32_t>(x >> 32)))
+  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));

livox_ros_driver/common/rapidjson/internal/diyfp.h

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

livox_ros_driver/common/rapidjson/internal/dtoa.h

@@ -33,9 +33,8 @@ 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,
@@ -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;
@@ -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) {
@@ -198,8 +188,7 @@ inline char *Prettify(char *buffer, int length, int k, int maxDecimalPlaces) {
 
   if (0 <= k && kk <= 21) {
     // 1234e7 -> 12340000000
-    for (int i = length; i < kk; i++)
+    for (int i = length; i < kk; i++) buffer[i] = '0';
-      buffer[i] = '0';
     buffer[kk] = '.';
     buffer[kk + 1] = '0';
     return &buffer[kk + 2];
@@ -212,8 +201,7 @@ inline char *Prettify(char *buffer, int length, int k, int maxDecimalPlaces) {
       // When maxDecimalPlaces = 2, 1.2345 -> 1.23, 1.102 -> 1.1
       // Remove extra trailing zeros (at least one) after truncation.
       for (int i = kk + maxDecimalPlaces; i > kk + 1; i--)
-        if (buffer[i] != '0')
+        if (buffer[i] != '0') return &buffer[i + 1];
-          return &buffer[i + 1];
       return &buffer[kk + 2];  // Reserve one zero
     } else
       return &buffer[length + 1];
@@ -223,14 +211,12 @@ inline char *Prettify(char *buffer, int length, int k, int maxDecimalPlaces) {
     std::memmove(&buffer[offset], &buffer[0], static_cast<size_t>(length));
     buffer[0] = '0';
     buffer[1] = '.';
-    for (int i = 2; i < offset; i++)
+    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
     } else
       return &buffer[length + offset];
@@ -257,8 +243,7 @@ inline char *dtoa(double value, char *buffer, int maxDecimalPlaces = 324) {
   RAPIDJSON_ASSERT(maxDecimalPlaces >= 1);
   Double d(value);
   if (d.IsZero()) {
-    if (d.Sign())
+    if (d.Sign()) *buffer++ = '-';  // -0.0, Issue #289
-      *buffer++ = '-'; // -0.0, Issue #289
     buffer[0] = '0';
     buffer[1] = '.';
     buffer[2] = '0';

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;

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];
@@ -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];

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
@@ -118,14 +149,16 @@ struct IsBaseOf : BoolType<::std::is_base_of<B, D>::value> {};
 
 #else  // simplified version adopted from Boost
 
-template <typename B, typename D> struct IsBaseOfImpl {
+template <typename B, typename D>
+struct IsBaseOfImpl {
   RAPIDJSON_STATIC_ASSERT(sizeof(B) != 0);
   RAPIDJSON_STATIC_ASSERT(sizeof(D) != 0);
 
   typedef char (&Yes)[1];
   typedef char (&No)[2];
 
-  template <typename T> static Yes Check(const D *, T);
+  template <typename T>
+  static Yes Check(const D *, T);
   static No Check(const B *, int);
 
   struct Host {
@@ -144,16 +177,20 @@ struct IsBaseOf : OrExpr<IsSame<B, D>, BoolExpr<IsBaseOfImpl<B, D>>>::Type {};
 //////////////////////////////////////////////////////////////////////////
 // EnableIf / DisableIf
 //
-template <bool Condition, typename T = void> struct EnableIfCond {
+template <bool Condition, typename T = void>
+struct EnableIfCond {
   typedef T Type;
 };
-template <typename T> struct EnableIfCond<false, T> { /* empty */
+template <typename T>
+struct EnableIfCond<false, T> { /* empty */
 };
 
-template <bool Condition, typename T = void> struct DisableIfCond {
+template <bool Condition, typename T = void>
+struct DisableIfCond {
   typedef T Type;
 };
-template <typename T> struct DisableIfCond<true, T> { /* empty */
+template <typename T>
+struct DisableIfCond<true, T> { /* empty */
 };
 
 template <typename Condition, typename T = void>
@@ -164,8 +201,10 @@ struct DisableIf : DisableIfCond<Condition::Value, T> {};
 
 // SFINAE helpers
 struct SfinaeTag {};
-template <typename T> struct RemoveSfinaeTag;
+template <typename T>
-template <typename T> struct RemoveSfinaeTag<SfinaeTag &(*)(T)> {
+struct RemoveSfinaeTag;
+template <typename T>
+struct RemoveSfinaeTag<SfinaeTag &(*)(T)> {
   typedef T Type;
 };
 

livox_ros_driver/common/rapidjson/internal/regex.h

@@ -47,8 +47,9 @@ 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() {
@@ -58,10 +59,9 @@ public:
     return c;
   }
 
-private:
+ private:
   void Decode() {
-    if (!Encoding::Decode(ss_, &codepoint_))
+    if (!Encoding::Decode(ss_, &codepoint_)) codepoint_ = 0;
-      codepoint_ = 0;
   }
 
   SourceStream &ss_;
@@ -75,7 +75,8 @@ static const SizeType kRegexInvalidState = ~SizeType(
     0);  //!< Represents an invalid index in GenericRegex::State::out, out1
 static const SizeType kRegexInvalidRange = ~SizeType(0);
 
-template <typename Encoding, typename Allocator> class GenericRegexSearch;
+template <typename Encoding, typename Allocator>
+class GenericRegexSearch;
 
 //! Regular expression engine with subset of ECMAscript grammar.
 /*!
@@ -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,
@@ -225,32 +231,27 @@ private:
                  *operatorStack.template Top<Operator>() != kLeftParenthesis)
             if (!Eval(operandStack, *operatorStack.template Pop<Operator>(1)))
               return;
-        if (operatorStack.Empty())
+          if (operatorStack.Empty()) return;
-          return;
           operatorStack.template Pop<Operator>(1);
           atomCountStack.template Pop<unsigned>(1);
           ImplicitConcatenation(atomCountStack, operatorStack);
           break;
 
         case '?':
-        if (!Eval(operandStack, kZeroOrOne))
+          if (!Eval(operandStack, kZeroOrOne)) return;
-          return;
           break;
 
         case '*':
-        if (!Eval(operandStack, kZeroOrMore))
+          if (!Eval(operandStack, kZeroOrMore)) return;
-          return;
           break;
 
         case '+':
-        if (!Eval(operandStack, kOneOrMore))
+          if (!Eval(operandStack, kOneOrMore)) return;
-          return;
           break;
 
         case '{': {
           unsigned n, m;
-        if (!ParseUnsigned(ds, &n))
+          if (!ParseUnsigned(ds, &n)) return;
-          return;
 
           if (ds.Peek() == ',') {
             ds.Take();
@@ -261,8 +262,7 @@ private:
           } else
             m = n;
 
-        if (!EvalQuantifier(operandStack, n, m) || ds.Peek() != '}')
+          if (!EvalQuantifier(operandStack, n, m) || ds.Peek() != '}') return;
-          return;
           ds.Take();
         } break;
 
@@ -273,8 +273,7 @@ private:
 
         case '[': {
           SizeType range;
-        if (!ParseRange(ds, &range))
+          if (!ParseRange(ds, &range)) return;
-          return;
           SizeType s = NewState(kRegexInvalidState, kRegexInvalidState,
                                 kRangeCharacterClass);
           GetState(s).rangeStart = range;
@@ -296,8 +295,7 @@ private:
     }
 
     while (!operatorStack.Empty())
-      if (!Eval(operandStack, *operatorStack.template Pop<Operator>(1)))
+      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;
   }
@@ -465,10 +462,8 @@ private:
     State *s = states_.template Push<State>(count);
     memcpy(s, &GetState(src.minIndex), count * sizeof(State));
     for (SizeType j = 0; j < count; j++) {
-      if (s[j].out != kRegexInvalidState)
+      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,8 +473,7 @@ private:
   template <typename InputStream>
   bool ParseUnsigned(DecodedStream<InputStream, Encoding> &ds, unsigned *u) {
     unsigned r = 0;
-    if (ds.Peek() < '0' || ds.Peek() > '9')
+    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
         return false;                         // overflow
@@ -515,8 +509,7 @@ private:
             RAPIDJSON_ASSERT(current != kRegexInvalidRange);
             GetRange(current).next = r;
           }
-        if (negate)
+          if (negate) GetRange(start).start |= kRangeNegationFlag;
-          GetRange(start).start |= kRangeNegationFlag;
           *range = start;
           return true;
 
@@ -541,10 +534,8 @@ private:
 
             case 0: {
               SizeType r = NewRange(codepoint);
-          if (current != kRegexInvalidRange)
+              if (current != kRegexInvalidRange) GetRange(current).next = r;
-            GetRange(current).next = r;
+              if (start == kRegexInvalidRange) start = r;
-          if (start == kRegexInvalidRange)
-            start = r;
               current = r;
             }
               step = 1;
@@ -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);
     }

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;
@@ -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,22 +191,21 @@ 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);
   }

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,8 +61,7 @@ bool CountStringCodePoint(const typename Encoding::Ch *s, SizeType length,
   SizeType count = 0;
   while (is.src_ < end) {
     unsigned codepoint;
-    if (!Encoding::Decode(is, &codepoint))
+    if (!Encoding::Decode(is, &codepoint)) return false;
-      return false;
     count++;
   }
   *outCount = count;

livox_ros_driver/common/rapidjson/internal/strtod.h

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

livox_ros_driver/common/rapidjson/internal/swap.h

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

livox_ros_driver/common/rapidjson/istreamwrapper.h

@@ -19,17 +19,17 @@
 #ifndef RAPIDJSON_ISTREAMWRAPPER_H_
 #define RAPIDJSON_ISTREAMWRAPPER_H_
 
-#include "stream.h"
 #include <ios>
 #include <iosfwd>
+#include "stream.h"
 
 #ifdef __clang__
 RAPIDJSON_DIAG_PUSH
 RAPIDJSON_DIAG_OFF(padded)
 #elif defined(_MSC_VER)
 RAPIDJSON_DIAG_PUSH
-RAPIDJSON_DIAG_OFF(
+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 &);

livox_ros_driver/common/rapidjson/memorybuffer.h

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

livox_ros_driver/common/rapidjson/msinttypes/stdint.h

@@ -173,7 +173,7 @@ typedef uint64_t uintmax_t;
 
 #if !defined(__cplusplus) || \
     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)

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 &);
 

livox_ros_driver/common/rapidjson/pointer.h

@@ -45,7 +45,8 @@ enum PointerParseErrorCode {
   kPointerParseErrorTokenMustBeginWithSolidus,  //!< A token must begin with a
                                                 //!< '/'
   kPointerParseErrorInvalidEscape,              //!< Invalid escape
-  kPointerParseErrorInvalidPercentEncoding, //!< Invalid percent encoding in URI
+  kPointerParseErrorInvalidPercentEncoding,     //!< Invalid percent encoding in
+                                                //!URI
                                                 //!< fragment
   kPointerParseErrorCharacterMustPercentEncode  //!< A character must percent
                                                 //!< encoded in URI fragment
@@ -87,7 +88,7 @@ enum PointerParseErrorCode {
 */
 template <typename ValueType, typename Allocator = CrtAllocator>
 class GenericPointer {
-public:
+ public:
   typedef typename ValueType::EncodingType
       EncodingType;                   //!< Encoding type from Value
   typedef typename ValueType::Ch Ch;  //!< Character type from Value
@@ -107,7 +108,8 @@ public:
      and allocation, using a special constructor.
   */
   struct Token {
-    const Ch *name; //!< Name of the token. It has null character at the end but
+    const Ch
+        *name;  //!< Name of the token. It has null character at the end but
                 //!< it can contain null character.
     SizeType length;  //!< Length of the name.
     SizeType index;   //!< A valid array index, if it is not equal to
@@ -119,8 +121,12 @@ public:
 
   //! Default constructor.
   GenericPointer(Allocator *allocator = 0)
-      : allocator_(allocator), ownAllocator_(), nameBuffer_(), tokens_(),
+      : 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,29 +210,42 @@ 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,
+                      // nameBuffer_
                       // is nullptr and tokens_ are not deallocated.
       Allocator::Free(tokens_);
     RAPIDJSON_DELETE(ownAllocator_);
@@ -224,8 +255,7 @@ public:
   GenericPointer &operator=(const GenericPointer &rhs) {
     if (this != &rhs) {
       // Do not delete ownAllcator
-      if (nameBuffer_)
+      if (nameBuffer_) Allocator::Free(tokens_);
-        Allocator::Free(tokens_);
 
       tokenCount_ = rhs.tokenCount_;
       parseErrorOffset_ = rhs.parseErrorOffset_;
@@ -353,8 +383,7 @@ public:
       return Append(token, allocator);
     } else {
       Ch name[21];
-      for (size_t i = 0; i <= length; i++)
+      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);
   }
 
@@ -522,8 +549,7 @@ public:
         exist = false;
       } else {
         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
           if (!v->IsArray() && !v->IsObject())
             v->SetArray();  // Change to Array
@@ -553,8 +579,7 @@ public:
       }
     }
 
-    if (alreadyExist)
+    if (alreadyExist) *alreadyExist = exist;
-      *alreadyExist = exist;
 
     return *v;
   }
@@ -566,8 +591,8 @@ 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,
       bool *alreadyExist = 0) const {
     return Create(document, document.GetAllocator(), alreadyExist);
@@ -603,14 +628,12 @@ public:
           typename ValueType::MemberIterator m =
               v->FindMember(GenericValue<EncodingType>(
                   GenericStringRef<Ch>(t->name, t->length)));
-        if (m == v->MemberEnd())
+          if (m == v->MemberEnd()) break;
-          break;
           v = &m->value;
         }
           continue;
         case kArrayType:
-        if (t->index == kPointerInvalidIndex || t->index >= v->Size())
+          if (t->index == kPointerInvalidIndex || t->index >= v->Size()) break;
-          break;
           v = &((*v)[t->index]);
           continue;
         default:
@@ -652,8 +675,8 @@ public:
      the values if the specified value or its parents are not exist. \see
      Create()
   */
-  ValueType &
+  ValueType &GetWithDefault(
-  GetWithDefault(ValueType &root, const ValueType &defaultValue,
+      ValueType &root, const ValueType &defaultValue,
       typename ValueType::AllocatorType &allocator) const {
     bool alreadyExist;
     ValueType &v = Create(root, allocator, &alreadyExist);
@@ -661,8 +684,8 @@ public:
   }
 
   //! Query a value in a subtree with default null-terminated string.
-  ValueType &
+  ValueType &GetWithDefault(
-  GetWithDefault(ValueType &root, const Ch *defaultValue,
+      ValueType &root, const Ch *defaultValue,
       typename ValueType::AllocatorType &allocator) const {
     bool alreadyExist;
     ValueType &v = Create(root, allocator, &alreadyExist);
@@ -671,8 +694,8 @@ public:
 
 #if RAPIDJSON_HAS_STDSTRING
   //! Query a value in a subtree with default std::basic_string.
-  ValueType &
+  ValueType &GetWithDefault(
-  GetWithDefault(ValueType &root, const std::basic_string<Ch> &defaultValue,
+      ValueType &root, const std::basic_string<Ch> &defaultValue,
       typename ValueType::AllocatorType &allocator) const {
     bool alreadyExist;
     ValueType &v = Create(root, allocator, &alreadyExist);
@@ -796,8 +819,8 @@ public:
 
   //! Set a value in a document, with move semantics.
   template <typename stackAllocator>
-  ValueType &
+  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,8 +893,8 @@ 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,
       ValueType &value) const {
     return Create(document).Swap(value);
@@ -901,8 +924,7 @@ public:
           typename ValueType::MemberIterator m =
               v->FindMember(GenericValue<EncodingType>(
                   GenericStringRef<Ch>(t->name, t->length)));
-        if (m == v->MemberEnd())
+          if (m == v->MemberEnd()) return false;
-          return false;
           v = &m->value;
         } break;
         case kArrayType:
@@ -928,7 +950,7 @@ public:
     }
   }
 
-private:
+ private:
   //! Clone the content from rhs to this.
   /*!
       \param rhs Source pointer.
@@ -977,12 +999,14 @@ 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
                    /*!
                        \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
+                      source string.                 \note Source cannot be JSON String
+                      Representation of JSON
                       Pointer, e.g. In "/\u0000", \u0000 will not be unescaped.
                    */
 #endif
@@ -992,14 +1016,12 @@ private:
     RAPIDJSON_ASSERT(tokens_ == 0);
 
     // Create own allocator if user did not supply.
-    if (!allocator_)
+    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)));
@@ -1076,14 +1098,12 @@ 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
 
       // Second check for index: more than one digit cannot have leading zero
@@ -1131,8 +1151,7 @@ private:
   bool Stringify(OutputStream &os) const {
     RAPIDJSON_ASSERT(IsValid());
 
-    if (uriFragment)
+    if (uriFragment) os.Put('#');
-      os.Put('#');
 
     for (Token *t = tokens_; t != tokens_ + tokenCount_; ++t) {
       os.Put('/');
@@ -1214,7 +1233,8 @@ private:
 
   //! A helper stream to encode character (UTF-8 code unit) into percent-encoded
   //! sequence.
-  template <typename OutputStream> class PercentEncodeStream {
+  template <typename OutputStream>
+  class PercentEncodeStream {
    public:
     PercentEncodeStream(OutputStream &os) : os_(os) {}
     void Put(char c) {  // UTF-8 must be byte
@@ -1250,9 +1270,8 @@ typedef GenericPointer<Value> Pointer;
 //////////////////////////////////////////////////////////////////////////////
 
 template <typename T>
-typename T::ValueType &
+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) {
   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,16 +1302,15 @@ 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) {
   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) {
   return pointer.Get(root, unresolvedTokenIndex);
 }
@@ -1305,8 +1323,8 @@ typename T::ValueType *GetValueByPointer(T &root, const CharType (&source)[N],
 }
 
 template <typename T, typename CharType, size_t N>
-const typename T::ValueType *
+const typename T::ValueType *GetValueByPointer(
-GetValueByPointer(const T &root, const CharType (&source)[N],
+    const T &root, const CharType (&source)[N],
     size_t *unresolvedTokenIndex = 0) {
   return GenericPointer<typename T::ValueType>(source, N - 1)
       .Get(root, unresolvedTokenIndex);
@@ -1349,18 +1367,16 @@ GetValueByPointerWithDefault(
 }
 
 template <typename T, typename CharType, size_t N>
-typename T::ValueType &
+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) {
   return GenericPointer<typename T::ValueType>(source, N - 1)
       .GetWithDefault(root, defaultValue, a);
@@ -1435,9 +1451,8 @@ typename DocumentType::ValueType &GetValueByPointerWithDefault(
 }
 
 template <typename DocumentType, typename CharType, size_t N>
-typename DocumentType::ValueType &
+typename DocumentType::ValueType &GetValueByPointerWithDefault(
-GetValueByPointerWithDefault(DocumentType &document,
+    DocumentType &document, const CharType (&source)[N],
-                             const CharType (&source)[N],
     const typename DocumentType::Ch *defaultValue) {
   return GenericPointer<typename DocumentType::ValueType>(source, N - 1)
       .GetWithDefault(document, defaultValue);
@@ -1466,31 +1481,30 @@ GetValueByPointerWithDefault(DocumentType &document,
 //////////////////////////////////////////////////////////////////////////////
 
 template <typename T>
-typename T::ValueType &
+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) {
   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) {
   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,
     typename T::AllocatorType &a) {
   return pointer.Set(root, value, a);
@@ -1532,8 +1546,8 @@ typename T::ValueType &SetValueByPointer(T &root, const CharType (&source)[N],
 
 #if RAPIDJSON_HAS_STDSTRING
 template <typename T, typename CharType, size_t N>
-typename T::ValueType &
+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,
     typename T::AllocatorType &a) {
   return GenericPointer<typename T::ValueType>(source, N - 1)
@@ -1598,24 +1612,24 @@ SetValueByPointer(
 }
 
 template <typename DocumentType, typename CharType, size_t N>
-typename DocumentType::ValueType &
+typename DocumentType::ValueType &SetValueByPointer(
-SetValueByPointer(DocumentType &document, const CharType (&source)[N],
+    DocumentType &document, const CharType (&source)[N],
     typename DocumentType::ValueType &value) {
   return GenericPointer<typename DocumentType::ValueType>(source, N - 1)
       .Set(document, value);
 }
 
 template <typename DocumentType, typename CharType, size_t N>
-typename DocumentType::ValueType &
+typename DocumentType::ValueType &SetValueByPointer(
-SetValueByPointer(DocumentType &document, const CharType (&source)[N],
+    DocumentType &document, const CharType (&source)[N],
     const typename DocumentType::ValueType &value) {
   return GenericPointer<typename DocumentType::ValueType>(source, N - 1)
       .Set(document, value);
 }
 
 template <typename DocumentType, typename CharType, size_t N>
-typename DocumentType::ValueType &
+typename DocumentType::ValueType &SetValueByPointer(
-SetValueByPointer(DocumentType &document, const CharType (&source)[N],
+    DocumentType &document, const CharType (&source)[N],
     const typename DocumentType::Ch *value) {
   return GenericPointer<typename DocumentType::ValueType>(source, N - 1)
       .Set(document, value);
@@ -1623,8 +1637,8 @@ SetValueByPointer(DocumentType &document, const CharType (&source)[N],
 
 #if RAPIDJSON_HAS_STDSTRING
 template <typename DocumentType, typename CharType, size_t N>
-typename DocumentType::ValueType &
+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) {
   return GenericPointer<typename DocumentType::ValueType>(source, N - 1)
       .Set(document, value);
@@ -1644,9 +1658,8 @@ SetValueByPointer(DocumentType &document, const CharType (&source)[N],
 //////////////////////////////////////////////////////////////////////////////
 
 template <typename T>
-typename T::ValueType &
+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) {
   return pointer.Swap(root, value, a);
 }
@@ -1668,8 +1681,8 @@ typename DocumentType::ValueType &SwapValueByPointer(
 }
 
 template <typename DocumentType, typename CharType, size_t N>
-typename DocumentType::ValueType &
+typename DocumentType::ValueType &SwapValueByPointer(
-SwapValueByPointer(DocumentType &document, const CharType (&source)[N],
+    DocumentType &document, const CharType (&source)[N],
     typename DocumentType::ValueType &value) {
   return GenericPointer<typename DocumentType::ValueType>(source, N - 1)
       .Swap(document, value);

livox_ros_driver/common/rapidjson/prettywriter.h

@@ -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
@@ -256,7 +259,7 @@ 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
@@ -267,8 +270,7 @@ protected:
         if (level->valueCount > 0) {
           Base::os_->Put(
               ',');  // add comma if it is not the first element in array
-          if (formatOptions_ & kFormatSingleLineArray)
+          if (formatOptions_ & kFormatSingleLineArray) Base::os_->Put(' ');
-            Base::os_->Put(' ');
         }
 
         if (!(formatOptions_ & kFormatSingleLineArray)) {
@@ -287,8 +289,7 @@ 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
@@ -313,7 +314,7 @@ protected:
   unsigned indentCharCount_;
   PrettyFormatOptions formatOptions_;
 
-private:
+ private:
   // Prohibit copy constructor & assignment operator.
   PrettyWriter(const PrettyWriter &);
   PrettyWriter &operator=(const PrettyWriter &);

livox_ros_driver/common/rapidjson/rapidjson.h

@@ -449,11 +449,14 @@ RAPIDJSON_NAMESPACE_END
 //!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN
 #endif
 RAPIDJSON_NAMESPACE_BEGIN
-template <bool x> struct STATIC_ASSERTION_FAILURE;
+template <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__)

livox_ros_driver/common/rapidjson/reader.h

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

livox_ros_driver/common/rapidjson/schema.h

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

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);
 }
 
 ///////////////////////////////////////////////////////////////////////////////
@@ -123,7 +123,7 @@ RAPIDJSON_DIAG_OFF(4512) // assignment operator could not be generated
 
 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) {}
@@ -195,7 +196,8 @@ typedef GenericStringStream<UTF8<>> StringStream;
 /*! This string stream is particularly designed for in-situ parsing.
     \note implements Stream concept
 */
-template <typename Encoding> struct GenericInsituStringStream {
+template <typename Encoding>
+struct GenericInsituStringStream {
   typedef typename Encoding::Ch Ch;
 
   GenericInsituStringStream(Ch *src) : src_(src), dst_(0), head_(src) {}

livox_ros_driver/common/rapidjson/stringbuffer.h

@@ -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 &);

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>
@@ -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
@@ -268,7 +274,8 @@ public:
                      sizeof(Level));  // not inside an Object
     RAPIDJSON_ASSERT(!level_stack_.template Top<Level>()
                           ->inArray);  // currently inside an Array, not Object
-    RAPIDJSON_ASSERT(0 == level_stack_.template Top<Level>()->valueCount %
+    RAPIDJSON_ASSERT(0 ==
+                     level_stack_.template Top<Level>()->valueCount %
                          2);  // Object has a Key without a Value
     level_stack_.template Pop<Level>(1);
     return EndValue(WriteEndObject());
@@ -320,7 +327,7 @@ public:
    */
   void Flush() { os_->Flush(); }
 
-protected:
+ protected:
   //! Information for each nested level
   struct Level {
     Level(bool inArray_) : valueCount(0), inArray(inArray_) {}
@@ -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');
@@ -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 == '\\') {
@@ -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 == '\\') {
@@ -786,8 +789,7 @@ inline bool Writer<StringBuffer>::ScanWriteUnescapedString(StringStream &is,
     }
     if (RAPIDJSON_UNLIKELY(escaped)) {  // some of characters is escaped
       char *q = reinterpret_cast<char *>(os_->PushUnsafe(len));
-      for (size_t i = 0; i < len; i++)
+      for (size_t i = 0; i < len; i++) q[i] = p[i];
-        q[i] = p[i];
 
       p += len;
       break;

livox_ros_driver/common/rapidxml/rapidxml.hpp

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

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,7 +123,7 @@ public:
     return m_attribute != rhs.m_attribute;
   }
 
-private:
+ private:
   xml_attribute<Ch> *m_attribute;
 };
 

livox_ros_driver/common/rapidxml/rapidxml_print.hpp

@@ -19,8 +19,8 @@ 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.
 
 ///////////////////////////////////////////////////////////////////////
@@ -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;
 }
 
@@ -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,7 +117,6 @@ inline OutIt print_node(OutIt out, const xml_node<Ch> *node, int flags,
                         int indent) {
   // Print proper node type
   switch (node->type()) {
-
     // Document
     case node_document:
       out = print_children(out, node, flags, indent);
@@ -168,8 +164,7 @@ inline OutIt print_node(OutIt out, const xml_node<Ch> *node, int flags,
   }
 
   // If indenting not disabled, add line break after node
-  if (!(flags & print_no_indenting))
+  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);

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
 };
 
 //! Counts children of node. Time complexity is O(n).
 //! \return Number of children of node
-template <class Ch> inline std::size_t count_children(xml_node<Ch> *node) {
+template <class Ch>
+inline std::size_t count_children(xml_node<Ch> *node) {
   xml_node<Ch> *child = node->first_node();
   std::size_t count = 0;
   while (child) {
@@ -85,7 +85,8 @@ template <class Ch> inline std::size_t count_children(xml_node<Ch> *node) {
 
 //! Counts attributes of node. Time complexity is O(n).
 //! \return Number of attributes of node
-template <class Ch> inline std::size_t count_attributes(xml_node<Ch> *node) {
+template <class Ch>
+inline std::size_t count_attributes(xml_node<Ch> *node) {
   xml_attribute<Ch> *attr = node->first_attribute();
   std::size_t count = 0;
   while (attr) {

livox_ros_driver/config/display_hub_points.rviz

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

livox_ros_driver/config/livox_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,22 +1,22 @@
 {
     "lidar_config": [
         {
-            "broadcast_code": "0T9DFBC00401611",
+            "broadcast_code": "1PQDH5B00100041",
             "enable_connect": false,
-            "enable_fan": true,
             "return_mode": 0,
             "coordinate": 0,
-            "imu_rate": 1,
+            "imu_rate": 0,
-            "extrinsic_parameter_source": 0
+            "extrinsic_parameter_source": 0,
+            "enable_high_sensitivity": false
         },
         {
             "broadcast_code": "0TFDG3U99101431",
             "enable_connect": false,
-            "enable_fan": true,
             "return_mode": 0,
             "coordinate": 0,
-            "imu_rate": 1,
+            "imu_rate": 0,
-            "extrinsic_parameter_source": 0
+            "extrinsic_parameter_source": 0,
+            "enable_high_sensitivity": false
         }
     ],
 	

livox_ros_driver/launch/livox_hub.launch

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

livox_ros_driver/launch/livox_hub_msg.launch

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

livox_ros_driver/launch/livox_hub_rviz.launch

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

livox_ros_driver/launch/livox_lidar.launch

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

livox_ros_driver/launch/livox_lidar_msg.launch

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

livox_ros_driver/launch/livox_lidar_rviz.launch

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

livox_ros_driver/launch/livox_template.launch

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

livox_ros_driver/launch/lvx_to_rosbag.launch

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

livox_ros_driver/launch/lvx_to_rosbag_rviz.launch

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

livox_ros_driver/livox_ros_driver/include/livox_ros_driver.h

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

livox_ros_driver/livox_ros_driver/lddc.cpp

@@ -34,23 +34,23 @@
 #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--------------------------------------------*/
-
-/** Lidar Data Distribute Control
- * ----------------------------------------------------------------*/
 Lddc::Lddc(int format, int multi_topic, int data_src, int output_type,
-           double frq)
+           double frq, std::string &frame_id)
-    : transfer_format_(format), use_multi_topic_(multi_topic),
+    : transfer_format_(format),
-      data_src_(data_src), output_type_(output_type), publish_frq_(frq) {
+      use_multi_topic_(multi_topic),
-
+      data_src_(data_src),
-  publish_interval_ms_ = 1000 / publish_frq_;
+      output_type_(output_type),
+      publish_frq_(frq),
+      frame_id_(frame_id) {
+  publish_period_ns_ = kNsPerSecond / publish_frq_;
   lds_ = nullptr;
   memset(private_pub_, 0, sizeof(private_pub_));
   memset(private_imu_pub_, 0, sizeof(private_imu_pub_));
@@ -61,14 +61,12 @@ Lddc::Lddc(int format, int multi_topic, int data_src, int output_type,
 };
 
 Lddc::~Lddc() {
-
-  printf("lddc exit\n\n\n\n");
   if (global_pub_) {
     delete global_pub_;
   }
 
   if (global_imu_pub_) {
-    delete global_pub_;
+    delete global_imu_pub_;
   }
 
   if (lds_) {
@@ -88,17 +86,51 @@ Lddc::~Lddc() {
   }
 }
 
-uint32_t Lddc::PublishPointcloud2(LidarDataQueue *queue, uint32_t packet_num,
+int32_t Lddc::GetPublishStartTime(LidarDevice *lidar, LidarDataQueue *queue,
-                                  uint8_t handle) {
+                                  uint64_t *start_time,
-  uint64_t timestamp = 0;
+                                  StoragePacket *storage_packet) {
-  uint64_t last_timestamp = 0;
+  QueuePrePop(queue, storage_packet);
-  uint32_t published_packet = 0;
+  uint64_t timestamp =
-  sensor_msgs::PointCloud2 cloud;
+      GetStoragePacketTimestamp(storage_packet, lidar->data_src);
+  uint32_t remaining_time = timestamp % publish_period_ns_;
+  uint32_t diff_time = publish_period_ns_ - remaining_time;
+  /** Get start time, down to the period boundary */
+  if (diff_time > (publish_period_ns_ / 4)) {
+    // ROS_INFO("0 : %u", diff_time);
+    *start_time = timestamp - remaining_time;
+    return 0;
+  } else if (diff_time <= lidar->packet_interval_max) {
+    *start_time = timestamp;
+    return 0;
+  } else {
+    /** Skip some packets up to the period boundary*/
+    // ROS_INFO("2 : %u", diff_time);
+    do {
+      if (QueueIsEmpty(queue)) {
+        break;
+      }
+      QueuePopUpdate(queue); /* skip packet */
+      QueuePrePop(queue, storage_packet);
+      uint32_t last_remaning_time = remaining_time;
+      timestamp = GetStoragePacketTimestamp(storage_packet, lidar->data_src);
+      remaining_time = timestamp % publish_period_ns_;
+      /** Flip to another period */
+      if (last_remaning_time > remaining_time) {
+        // ROS_INFO("Flip to another period, exit");
+        break;
+      }
+      diff_time = publish_period_ns_ - remaining_time;
+    } while (diff_time > lidar->packet_interval);
 
-  cloud.header.frame_id = "livox_frame";
+    /* the remaning packets in queue maybe not enough after skip */
+    return -1;
+  }
+}
+
+void Lddc::InitPointcloud2MsgHeader(sensor_msgs::PointCloud2& cloud) {
+  cloud.header.frame_id.assign(frame_id_);
   cloud.height = 1;
   cloud.width = 0;
-
   cloud.fields.resize(6);
   cloud.fields[0].offset = 0;
   cloud.fields[0].name = "x";
@@ -124,74 +156,85 @@ 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 = lds_->lidars_[handle].data_src;
+  uint8_t data_source = lidar->data_src;
-  StoragePacket storage_packet;
+  uint32_t line_num = GetLaserLineNumber(lidar->info.type);
-  while (published_packet < packet_num) {
+  uint32_t echo_num = GetEchoNumPerPoint(lidar->raw_data_type);
-    QueueProPop(queue, &storage_packet);
+  uint32_t is_zero_packet = 0;
+  while ((published_packet < packet_num) && !QueueIsEmpty(queue)) {
+    QueuePrePop(queue, &storage_packet);
     LivoxEthPacket *raw_packet =
         reinterpret_cast<LivoxEthPacket *>(storage_packet.raw_data);
-
-    uint32_t packet_interval = GetPacketInterval(raw_packet->data_type);
-    int64_t packet_loss_threshold_lower = packet_interval + packet_interval / 2;
     timestamp = GetStoragePacketTimestamp(&storage_packet, data_source);
     int64_t packet_gap = timestamp - last_timestamp;
-    if (published_packet && (packet_gap > packet_loss_threshold_lower) &&
+    if ((packet_gap > lidar->packet_interval_max) &&
-        lds_->lidars_[handle].data_is_pubulished) {
+        lidar->data_is_pubulished) {
-      ROS_INFO("Lidar[%d] packet loss, interval is %ldus", handle, packet_gap);
+      // ROS_INFO("Lidar[%d] packet time interval is %ldns", handle,
+      //     packet_gap);
       if (kSourceLvxFile != data_source) {
-        // ROS_INFO("Lidar[%d] packet loss %ld %d %d", handle,
+        timestamp = last_timestamp + lidar->packet_interval;
-        // packet_loss_threshold_lower, packet_interval, raw_packet->data_type);
+        ZeroPointDataOfStoragePacket(&storage_packet);
-        int64_t packet_loss_threshold_upper = packet_interval * packet_num;
+        is_zero_packet = 1;
-        if (packet_gap >
-            packet_loss_threshold_upper) { // skip when gap is too large
-          break;
-        }
-        point_base = FillZeroPointXyzrtl(point_base, storage_packet.point_num);
-        cloud.width += storage_packet.point_num;
-        last_timestamp = last_timestamp + packet_interval;
-        ++published_packet;
-        continue;
       }
     }
-
+    /** Use the first packet timestamp as pointcloud2 msg timestamp */
-    if (!published_packet) { // use the first packet timestamp as pointcloud2
+    if (!published_packet) {
-                             // msg timestamp
       cloud.header.stamp = ros::Time(timestamp / 1000000000.0);
     }
-    cloud.width += storage_packet.point_num;
+    uint32_t single_point_num = storage_packet.point_num * echo_num;
 
     if (kSourceLvxFile != data_source) {
       PointConvertHandler pf_point_convert =
-          GetConvertHandler(raw_packet->data_type);
+          GetConvertHandler(lidar->raw_data_type);
       if (pf_point_convert) {
-        point_base = pf_point_convert(
+        point_base = pf_point_convert(point_base, raw_packet,
-            point_base, raw_packet, lds_->lidars_[handle].extrinsic_parameter);
+            lidar->extrinsic_parameter, line_num);
       } else {
-        /* Skip the packet */
+        /** Skip the packet */
         ROS_INFO("Lidar[%d] unkown packet type[%d]", handle,
                  raw_packet->data_type);
         break;
       }
     } else {
-      point_base = LivoxPointToPxyzrtl(
+      point_base = LivoxPointToPxyzrtl(point_base, raw_packet,
-          point_base, raw_packet, lds_->lidars_[handle].extrinsic_parameter);
+          lidar->extrinsic_parameter, line_num);
     }
 
+    if (!is_zero_packet) {
       QueuePopUpdate(queue);
-    last_timestamp = timestamp;
+    } else {
-    ++published_packet;
+      is_zero_packet = 0;
+    }
+    cloud.width += single_point_num;
+    ++published_packet;
+    last_timestamp = timestamp;
   }
-
   cloud.row_step     = cloud.width * cloud.point_step;
   cloud.is_bigendian = false;
   cloud.is_dense     = true;
-  cloud.data.resize(cloud.row_step); // adjust to the real size
+  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);
@@ -201,14 +244,26 @@ uint32_t Lddc::PublishPointcloud2(LidarDataQueue *queue, uint32_t packet_num,
                   cloud);
     }
   }
-
+  if (!lidar->data_is_pubulished) {
-  if (!lds_->lidars_[handle].data_is_pubulished) {
+    lidar->data_is_pubulished = true;
-    lds_->lidars_[handle].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) {
@@ -216,52 +271,49 @@ uint32_t Lddc::PublishPointcloudData(LidarDataQueue *queue, uint32_t packet_num,
   uint64_t last_timestamp = 0;
   uint32_t published_packet = 0;
 
-  /* init point cloud data struct */
+  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;
+  }
+
   PointCloud::Ptr cloud(new PointCloud);
-  cloud->header.frame_id = "livox_frame";
+  cloud->header.frame_id.assign(frame_id_);
-  // cloud->header.stamp = ros::Time::now();
   cloud->height = 1;
   cloud->width = 0;
 
-  uint8_t data_source = lds_->lidars_[handle].data_src;
+  uint8_t point_buf[2048];
-  StoragePacket storage_packet;
+  uint32_t is_zero_packet = 0;
-  while (published_packet < packet_num) {
+  uint8_t data_source = lidar->data_src;
-    QueueProPop(queue, &storage_packet);
+  uint32_t line_num = GetLaserLineNumber(lidar->info.type);
+  uint32_t echo_num = GetEchoNumPerPoint(lidar->raw_data_type);
+  while ((published_packet < packet_num) && !QueueIsEmpty(queue)) {
+    QueuePrePop(queue, &storage_packet);
     LivoxEthPacket *raw_packet =
         reinterpret_cast<LivoxEthPacket *>(storage_packet.raw_data);
-
-    uint32_t packet_interval = GetPacketInterval(raw_packet->data_type);
-    int64_t packet_loss_threshold_lower = packet_interval + packet_interval / 2;
     timestamp = GetStoragePacketTimestamp(&storage_packet, data_source);
     int64_t packet_gap = timestamp - last_timestamp;
-    if ((packet_gap > packet_loss_threshold_lower) && published_packet &&
+    if ((packet_gap > lidar->packet_interval_max) &&
-        lds_->lidars_[handle].data_is_pubulished) {
+        lidar->data_is_pubulished) {
-      ROS_INFO("Lidar[%d] packet loss, interval is %ldus", handle, packet_gap);
+      //ROS_INFO("Lidar[%d] packet time interval is %ldns", handle, packet_gap);
-      int64_t packet_loss_threshold_upper = packet_interval * packet_num;
+      if (kSourceLvxFile != data_source) {
-      if (packet_gap >
+        timestamp = last_timestamp + lidar->packet_interval;
-          packet_loss_threshold_upper) { // skip when gap is too large
+        ZeroPointDataOfStoragePacket(&storage_packet);
-        break;
+        is_zero_packet = 1;
       }
-      pcl::PointXYZI point = {0}; // fill zero points
-      for (uint32_t i = 0; i < storage_packet.point_num; i++) {
-        cloud->points.push_back(point);
-      }
-      last_timestamp = last_timestamp + packet_interval;
-      ++published_packet;
-      continue;
     }
     if (!published_packet) {
       cloud->header.stamp = timestamp / 1000.0;  // to pcl ros time stamp
     }
-    cloud->width += storage_packet.point_num;
+    uint32_t single_point_num = storage_packet.point_num * echo_num;
 
-    uint8_t point_buf[2048];
     if (kSourceLvxFile != data_source) {
       PointConvertHandler pf_point_convert =
-          GetConvertHandler(raw_packet->data_type);
+          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, \
-                         lds_->lidars_[handle].extrinsic_parameter);
+            line_num);
       } else {
         /* Skip the packet */
         ROS_INFO("Lidar[%d] unkown packet type[%d]", handle,
@@ -269,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, \
-                          lds_->lidars_[handle].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;
-      point.y = dst_point->y;
-      point.z = dst_point->z;
-      point.intensity = dst_point->reflectivity;
-      ++dst_point;
-      cloud->points.push_back(point);
-    }
-
       QueuePopUpdate(queue);
-    last_timestamp = timestamp;
+    } else {
+      is_zero_packet = 0;
+    }
+    cloud->width += single_point_num;
     ++published_packet;
+    last_timestamp = timestamp;
   }
 
   ros::Publisher *p_publisher = Lddc::GetCurrentPublisher(handle);
@@ -298,99 +345,117 @@ uint32_t Lddc::PublishPointcloudData(LidarDataQueue *queue, uint32_t packet_num,
                   cloud);
     }
   }
-
+  if (!lidar->data_is_pubulished) {
-  if (!lds_->lidars_[handle].data_is_pubulished) {
+    lidar->data_is_pubulished = true;
-    lds_->lidars_[handle].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.assign(frame_id_);
-  livox_msg.header.frame_id = "livox_frame";
   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;
 
+  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(raw_packet->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 && lds_->lidars_[handle].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(raw_packet->data_type);
+          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, \
-                         lds_->lidars_[handle].extrinsic_parameter);
+            line_num);
       } else {
         /* Skip the packet */
         ROS_INFO("Lidar[%d] unkown packet type[%d]", handle,
-                 raw_packet->data_type);
+                 lidar->raw_data_type);
         break;
       }
     } else {
-      LivoxPointToPxyzrtl(point_buf, raw_packet,
+      LivoxPointToPxyzrtl(point_buf, raw_packet, lidar->extrinsic_parameter, \
-                          lds_->lidars_[handle].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) {
+      QueuePopUpdate(queue);
     } else {
-        point.offset_time = packet_offset_time + (i / 2) * point_interval;
+      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;
   }
@@ -405,10 +470,9 @@ uint32_t Lddc::PublishCustomPointcloud(LidarDataQueue *queue,
     }
   }
 
-  if (!lds_->lidars_[handle].data_is_pubulished) {
+  if (!lidar->data_is_pubulished) {
-    lds_->lidars_[handle].data_is_pubulished = true;
+    lidar->data_is_pubulished = true;
   }
-
   return published_packet;
 }
 
@@ -422,7 +486,7 @@ uint32_t Lddc::PublishImuData(LidarDataQueue *queue, uint32_t packet_num,
 
   uint8_t data_source = lds_->lidars_[handle].data_src;
   StoragePacket storage_packet;
-  QueueProPop(queue, &storage_packet);
+  QueuePrePop(queue, &storage_packet);
   LivoxEthPacket *raw_packet =
       reinterpret_cast<LivoxEthPacket *>(storage_packet.raw_data);
   timestamp = GetStoragePacketTimestamp(&storage_packet, data_source);
@@ -454,7 +518,6 @@ uint32_t Lddc::PublishImuData(LidarDataQueue *queue, uint32_t packet_num,
                   imu_data);
     }
   }
-
   return published_packet;
 }
 
@@ -469,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->info.type) / publish_frq_;
     if (used_size < onetime_publish_packets) {
       break;
     }
@@ -493,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;
   }
 
@@ -506,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];
@@ -515,7 +577,6 @@ void Lddc::DistributeLidarData(void) {
         (p_queue == nullptr)) {
       continue;
     }
-
     PollingLidarPointCloudData(lidar_id, lidar);
     PollingLidarImuData(lidar_id, lidar);
   }
@@ -563,7 +624,8 @@ ros::Publisher *Lddc::GetCurrentPublisher(uint8_t handle) {
           name_str, queue_size);
     } else if (kPclPxyziMsg == transfer_format_) {
       **pub = cur_node_->advertise<PointCloud>(name_str, queue_size);
-      ROS_INFO("%s publish use pcl PointXYZI format, set ROS publisher queue "
+      ROS_INFO(
+          "%s publish use pcl PointXYZI format, set ROS publisher queue "
           "size %d",
           name_str, queue_size);
     }
@@ -614,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_) {

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,9 @@ 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();
 
   int RegisterLds(Lds *lds);
@@ -58,7 +64,10 @@ public:
 
   Lds *lds_;
 
-private:
+ private:
+  int32_t GetPublishStartTime(LidarDevice *lidar, LidarDataQueue *queue,
+                              uint64_t *start_time,
+                              StoragePacket *storage_packet);
   uint32_t PublishPointcloud2(LidarDataQueue *queue, uint32_t packet_num,
                               uint8_t handle);
   uint32_t PublishPointcloudData(LidarDataQueue *queue, uint32_t packet_num,
@@ -72,13 +81,19 @@ 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_;
   uint8_t output_type_;
   double publish_frq_;
-  int32_t publish_interval_ms_;
+  uint32_t publish_period_ns_;
+  std::string frame_id_;
   ros::Publisher *private_pub_[kMaxSourceLidar];
   ros::Publisher *global_pub_;
   ros::Publisher *private_imu_pub_[kMaxSourceLidar];

livox_ros_driver/livox_ros_driver/ldq.cpp

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

livox_ros_driver/livox_ros_driver/ldq.h

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

livox_ros_driver/livox_ros_driver/lds.cpp

@@ -24,16 +24,15 @@
 
 #include "lds.h"
 
-#include <chrono>
 #include <math.h>
 #include <stdio.h>
 #include <string.h>
 #include <time.h>
+#include <chrono>
 
 namespace livox_ros {
 
-/** Common function ------
+/** Common function --------------------------------------------------------- */
- * ----------------------------------------------------------------------- */
 bool IsFilePathValid(const char *path_str) {
   int str_len = strlen(path_str);
 
@@ -44,15 +43,14 @@ bool IsFilePathValid(const char *path_str) {
   }
 }
 
-uint64_t GetStoragePacketTimestamp(StoragePacket *packet, uint8_t data_src_) {
+uint64_t GetStoragePacketTimestamp(StoragePacket *packet, uint8_t data_src) {
-
   LivoxEthPacket *raw_packet =
       reinterpret_cast<LivoxEthPacket *>(packet->raw_data);
   LdsStamp timestamp;
   memcpy(timestamp.stamp_bytes, raw_packet->timestamp, sizeof(timestamp));
 
   if (raw_packet->timestamp_type == kTimestampTypePps) {
-    if (data_src_ != kSourceLvxFile) {
+    if (data_src != kSourceLvxFile) {
       return (timestamp.stamp + packet->time_rcv);
     } else {
       return timestamp.stamp;
@@ -71,7 +69,7 @@ uint64_t GetStoragePacketTimestamp(StoragePacket *packet, uint8_t data_src_) {
     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
     time_epoch = time_epoch * 1000000 + timestamp.stamp_word.high;  // to us
     time_epoch = time_epoch * 1000;                                 // to ns
@@ -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,13 +518,10 @@ 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) {
-  lidar_count_ = kMaxSourceLidar;
-  request_exit_ = false;
     ResetLds(data_src_);
 };
 
@@ -441,6 +541,7 @@ void Lds::ResetLidar(LidarDevice *lidar, uint8_t data_src) {
   lidar->data_src = data_src;
   lidar->data_is_pubulished = false;
   lidar->connect_state = kConnectStateOff;
+  lidar->raw_data_type = 0xFF;
 }
 
 void Lds::SetLidarDataSrc(LidarDevice *lidar, uint8_t data_src) {
@@ -454,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;
@@ -462,6 +588,92 @@ uint8_t Lds::GetDeviceType(uint8_t handle) {
   }
 }
 
+void Lds::UpdateLidarInfoByEthPacket(LidarDevice *p_lidar, \
+    LivoxEthPacket* eth_packet) {
+  if (p_lidar->raw_data_type != eth_packet->data_type) {
+    p_lidar->raw_data_type = eth_packet->data_type;
+    p_lidar->packet_interval = GetPacketInterval(p_lidar->info.type, \
+        eth_packet->data_type);
+    p_lidar->packet_interval_max = p_lidar->packet_interval * 1.8f;
+    p_lidar->onetime_publish_packets = \
+        GetPacketNumPerSec(p_lidar->info.type, \
+        p_lidar->raw_data_type) * buffer_time_ms_ / 1000;
+    printf("DataType[%d] PacketInterval[%d] PublishPackets[%d]\n", \
+        p_lidar->raw_data_type, p_lidar->packet_interval, \
+        p_lidar->onetime_publish_packets);
+  }
+}
+
+void Lds::StorageRawPacket(uint8_t handle, LivoxEthPacket* eth_packet) {
+  LidarDevice *p_lidar = &lidars_[handle];
+  LidarPacketStatistic *packet_statistic = &p_lidar->statistic_info;
+  LdsStamp cur_timestamp;
+  memcpy(cur_timestamp.stamp_bytes, eth_packet->timestamp,
+         sizeof(cur_timestamp));
+
+  if (kImu != eth_packet->data_type) {
+    UpdateLidarInfoByEthPacket(p_lidar, eth_packet);
+    if (eth_packet->timestamp_type == kTimestampTypePps) {
+      /** Whether a new sync frame */
+      if ((cur_timestamp.stamp < packet_statistic->last_timestamp) &&
+          (cur_timestamp.stamp < kPacketTimeGap)) {
+        auto cur_time = std::chrono::high_resolution_clock::now();
+        int64_t sync_time = cur_time.time_since_epoch().count();
+        /** used receive time as timebase */
+        packet_statistic->timebase = sync_time;
+      }
+    }
+    packet_statistic->last_timestamp = cur_timestamp.stamp;
+
+    LidarDataQueue *p_queue = &p_lidar->data;
+    if (nullptr == p_queue->storage_packet) {
+      uint32_t queue_size = CalculatePacketQueueSize(
+          buffer_time_ms_, p_lidar->info.type, eth_packet->data_type);
+      queue_size = queue_size * 8; /* 8 multiple the min size */
+      InitQueue(p_queue, queue_size);
+      printf("Lidar%02d[%s] queue size : %d %d\n", p_lidar->handle,
+             p_lidar->info.broadcast_code, queue_size, p_queue->size);
+    }
+    if (!QueueIsFull(p_queue)) {
+      QueuePushAny(p_queue, (uint8_t *)eth_packet, \
+          GetEthPacketLen(eth_packet->data_type), \
+          packet_statistic->timebase, \
+          GetPointsPerPacket(eth_packet->data_type));
+      if (QueueUsedSize(p_queue) > p_lidar->onetime_publish_packets) {
+        if (semaphore_.GetCount() <= 0) {
+          semaphore_.Signal();
+        }
+      }
+    }
+  } else {
+    if (eth_packet->timestamp_type == kTimestampTypePps) {
+      /** Whether a new sync frame */
+      if ((cur_timestamp.stamp < packet_statistic->last_imu_timestamp) &&
+          (cur_timestamp.stamp < kPacketTimeGap)) {
+        auto cur_time = std::chrono::high_resolution_clock::now();
+        int64_t sync_time = cur_time.time_since_epoch().count();
+        /** used receive time as timebase */
+        packet_statistic->imu_timebase = sync_time;
+      }
+    }
+    packet_statistic->last_imu_timestamp = cur_timestamp.stamp;
+
+    LidarDataQueue *p_queue = &p_lidar->imu_data;
+    if (nullptr == p_queue->storage_packet) {
+      uint32_t queue_size = 256;  /* fixed imu data queue size */
+      InitQueue(p_queue, queue_size);
+      printf("Lidar%02d[%s] imu queue size : %d %d\n", p_lidar->handle,
+             p_lidar->info.broadcast_code, queue_size, p_queue->size);
+    }
+    if (!QueueIsFull(p_queue)) {
+      QueuePushAny(p_queue, (uint8_t *)eth_packet, \
+          GetEthPacketLen(eth_packet->data_type),\
+          packet_statistic->imu_timebase, \
+          GetPointsPerPacket(eth_packet->data_type));
+    }
+  }
+}
+
 void Lds::PrepareExit(void) {}
 
 }  // namespace livox_ros

livox_ros_driver/livox_ros_driver/lds.h

@@ -33,6 +33,8 @@
 #include <stdlib.h>
 #include <string>
 #include <vector>
+#include <mutex>
+#include <condition_variable>
 
 #include "ldq.h"
 
@@ -55,10 +57,10 @@ const uint32_t KCartesianPointSize = 13;
 const uint32_t KSphericalPointSzie = 9;
 
 const int64_t kPacketTimeGap = 1000000; /**< 1ms = 1000000ns */
-const int64_t kMaxPacketTimeGap =
+/**< 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 */
@@ -95,11 +97,12 @@ typedef enum {
 typedef enum { kCoordinateCartesian = 0, kCoordinateSpherical } CoordinateType;
 
 typedef enum {
-  kConfigFan = 1,
+  kConfigFan = 1 << 0,
-  kConfigReturnMode = 2,
+  kConfigReturnMode = 1 << 1,
-  kConfigCoordinate = 4,
+  kConfigCoordinate = 1 << 2,
-  kConfigImuRate = 8,
+  kConfigImuRate = 1 << 3,
-  kConfigGetExtrinsicParameter = 16,
+  kConfigGetExtrinsicParameter = 1 << 4,
+  kConfigSetHighSensitivity = 1 << 5,
   kConfigUndef
 } LidarConfigCodeBit;
 
@@ -139,6 +142,7 @@ typedef struct {
   uint32_t coordinate;
   uint32_t imu_rate;
   uint32_t extrinsic_parameter_source;
+  bool enable_high_sensitivity;
 } UserRawConfig;
 
 typedef struct {
@@ -147,6 +151,7 @@ typedef struct {
   uint32_t coordinate;
   uint32_t imu_rate;
   uint32_t extrinsic_parameter_source;
+  bool enable_high_sensitivity;
   volatile uint32_t set_bits;
   volatile uint32_t get_bits;
 } UserConfig;
@@ -166,8 +171,15 @@ typedef struct {
 typedef struct {
   uint8_t handle;                    /**< Lidar access handle. */
   uint8_t data_src;                  /**< From raw lidar or livox file. */
+  uint8_t raw_data_type;             /**< The data type in eth packaet */
   bool data_is_pubulished;           /**< Indicate the data of lidar whether is
                                           pubulished. */
+  volatile uint32_t packet_interval; /**< The time interval between packets
+                                        of current lidar, unit:ns */
+  volatile uint32_t packet_interval_max; /**< If more than it,
+                                            have packet loss */
+  /**< packet num that onetime published */
+  volatile uint32_t onetime_publish_packets;
   volatile LidarConnectState connect_state;
   DeviceInfo info;
   LidarPacketStatistic statistic_info;
@@ -179,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 raw_point_length;  /**< length of point uint:bytes */
+  uint32_t echo_num;          /**< echo number of current data */
+} DataTypePointInfoPair;
+
+typedef struct {
+  uint32_t points_per_second; /**< number of points per second */
   uint32_t point_interval;    /**< unit:ns */
-  uint32_t packet_interval; /**< unit:ns */
+  uint32_t line_num;          /**< laser line number */
-  uint32_t packet_length;
+} ProductTypePointInfoPair;
-} PacketInfoPair;
 
 #pragma pack(1)
 
@@ -212,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, 100, 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) {
@@ -298,37 +353,102 @@ 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_;
 };
 

livox_ros_driver/livox_ros_driver/lds_hub.cpp

@@ -24,9 +24,9 @@
 
 #include "lds_hub.h"
 
-#include <memory>
 #include <stdio.h>
 #include <string.h>
+#include <memory>
 #include <thread>
 
 #include "rapidjson/document.h"
@@ -35,18 +35,14 @@
 
 namespace livox_ros {
 
-/** Const varible
+/** Const varible ------------------------------------------------------------*/
- * -------------------------------------------------------------------------------
- */
 
 /** For callback use only */
 static LdsHub *g_lds_hub = nullptr;
 
-/** Global function for common use
+/** Global function for common use -------------------------------------------*/
- * ---------------------------------------------------------------*/
 
-/** Lds hub function
+/** Lds hub function ---------------------------------------------------------*/
- * -----------------------------------------------------------------------------*/
 LdsHub::LdsHub(uint32_t interval_ms) : Lds(interval_ms, kSourceRawHub) {
   auto_connect_mode_ = true;
   whitelist_count_ = 0;
@@ -67,7 +63,6 @@ void LdsHub::ResetLdsHub(void) {
 
 int LdsHub::InitLdsHub(std::vector<std::string> &broadcast_code_strs,
                        const char *user_config_path) {
-
   if (is_initialized_) {
     printf("LiDAR data source is already inited!\n");
     return -1;
@@ -104,7 +99,8 @@ int LdsHub::InitLdsHub(std::vector<std::string> &broadcast_code_strs,
     }
   } else {
     EnableAutoConnectMode();
-    printf("No broadcast code was added to whitelist, swith to automatic "
+    printf(
+        "No broadcast code was added to whitelist, swith to automatic "
         "connection mode!\n");
   }
 
@@ -126,7 +122,6 @@ int LdsHub::InitLdsHub(std::vector<std::string> &broadcast_code_strs,
 }
 
 int LdsHub::DeInitLdsHub(void) {
-
   if (!is_initialized_) {
     printf("LiDAR data source is not exit");
     return -1;
@@ -149,74 +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 (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) && (cur_timestamp.stamp >= 0)) {
-      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) {
@@ -293,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;
@@ -519,6 +455,7 @@ void LdsHub::ConfigImuPushFrequency(LdsHub *lds_hub) {
     LidarDevice *p_lidar = &(lds_hub->lidars_[i]);
 
     if ((p_lidar->info.type != kDeviceTypeLidarMid40) &&
+        (p_lidar->info.type != kDeviceTypeLidarMid70) &&
         (p_lidar->connect_state == kConnectStateSampling)) {
       UserRawConfig config;
       if (lds_hub->GetRawConfig(p_lidar->info.broadcast_code, config)) {

livox_ros_driver/livox_ros_driver/lds_hub.h

@@ -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);

livox_ros_driver/livox_ros_driver/lds_lidar.cpp

@@ -24,28 +24,27 @@
 
 #include "lds_lidar.h"
 
-#include <memory>
 #include <stdio.h>
 #include <string.h>
+#include <memory>
+#include <mutex>
 #include <thread>
 
 #include "rapidjson/document.h"
 #include "rapidjson/filereadstream.h"
 #include "rapidjson/stringbuffer.h"
 
+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;
@@ -98,7 +97,8 @@ int LdsLidar::InitLdsLidar(std::vector<std::string> &broadcast_code_strs,
     }
   } else {
     EnableAutoConnectMode();
-    printf("No broadcast code was added to whitelist, swith to automatic "
+    printf(
+        "No broadcast code was added to whitelist, swith to automatic "
         "connection mode!\n");
   }
 
@@ -135,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;
@@ -153,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,
@@ -168,70 +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 (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) && (cur_timestamp.stamp >= 0)) {
-      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) {
@@ -274,6 +209,7 @@ void LdsLidar::OnDeviceBroadcast(const BroadcastDeviceInfo *info) {
       config.coordinate = kCoordinateCartesian;
       config.imu_rate = kImuFreq200Hz;
       config.extrinsic_parameter_source = kNoneExtrinsicParameter;
+      config.enable_high_sensitivity = false;
     }
 
     p_lidar->config.enable_fan = config.enable_fan;
@@ -282,6 +218,7 @@ void LdsLidar::OnDeviceBroadcast(const BroadcastDeviceInfo *info) {
     p_lidar->config.imu_rate = config.imu_rate;
     p_lidar->config.extrinsic_parameter_source =
         config.extrinsic_parameter_source;
+    p_lidar->config.enable_high_sensitivity = config.enable_high_sensitivity;
   } else {
     printf("Add lidar to connect is failed : %d %d \n", result, handle);
   }
@@ -321,6 +258,9 @@ void LdsLidar::OnDeviceChange(const DeviceInfo *info, DeviceEvent type) {
 
     /** Config lidar parameter */
     if (p_lidar->info.state == kLidarStateNormal) {
+      /** Ensure the thread safety for set_bits and connect_state */
+      lock_guard<mutex> lock(g_lds_ldiar->config_mutex_);
+
       if (p_lidar->config.coordinate != 0) {
         SetSphericalCoordinate(handle, SetCoordinateCb, g_lds_ldiar);
       } else {
@@ -335,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;
@@ -345,6 +286,19 @@ void LdsLidar::OnDeviceChange(const DeviceInfo *info, DeviceEvent type) {
           kExtrinsicParameterFromLidar) {
         LidarGetExtrinsicParameter(handle, GetLidarExtrinsicParameterCb,
                                    g_lds_ldiar);
+        p_lidar->config.set_bits |= kConfigGetExtrinsicParameter;
+      }
+
+      if (kDeviceTypeLidarTele == info->type) {
+        if (p_lidar->config.enable_high_sensitivity) {
+          LidarEnableHighSensitivity(handle, SetHighSensitivityCb, g_lds_ldiar);
+          printf("Enable high sensitivity\n");
+        } else {
+          LidarDisableHighSensitivity(handle, SetHighSensitivityCb,
+                                      g_lds_ldiar);
+          printf("Disable high sensitivity\n");
+        }
+        p_lidar->config.set_bits |= kConfigSetHighSensitivity;
       }
 
       p_lidar->connect_state = kConnectStateConfig;
@@ -403,14 +357,14 @@ void LdsLidar::SetPointCloudReturnModeCb(livox_status status, uint8_t handle,
   LidarDevice *p_lidar = &(lds_lidar->lidars_[handle]);
 
   if (status == kStatusSuccess) {
-    p_lidar->config.set_bits &= ~((uint32_t)(kConfigReturnMode));
     printf("Set return mode success!\n");
 
+    lock_guard<mutex> lock(lds_lidar->config_mutex_);
+    p_lidar->config.set_bits &= ~((uint32_t)(kConfigReturnMode));
     if (!p_lidar->config.set_bits) {
       LidarStartSampling(handle, StartSampleCb, lds_lidar);
       p_lidar->connect_state = kConnectStateSampling;
     }
-
   } else {
     LidarSetPointCloudReturnMode(
         handle, (PointCloudReturnMode)(p_lidar->config.return_mode),
@@ -429,9 +383,10 @@ void LdsLidar::SetCoordinateCb(livox_status status, uint8_t handle,
   LidarDevice *p_lidar = &(lds_lidar->lidars_[handle]);
 
   if (status == kStatusSuccess) {
-    p_lidar->config.set_bits &= ~((uint32_t)(kConfigCoordinate));
     printf("Set coordinate success!\n");
 
+    lock_guard<mutex> lock(lds_lidar->config_mutex_);
+    p_lidar->config.set_bits &= ~((uint32_t)(kConfigCoordinate));
     if (!p_lidar->config.set_bits) {
       LidarStartSampling(handle, StartSampleCb, lds_lidar);
       p_lidar->connect_state = kConnectStateSampling;
@@ -457,14 +412,14 @@ void LdsLidar::SetImuRatePushFrequencyCb(livox_status status, uint8_t handle,
   LidarDevice *p_lidar = &(lds_lidar->lidars_[handle]);
 
   if (status == kStatusSuccess) {
-    p_lidar->config.set_bits &= ~((uint32_t)(kConfigImuRate));
     printf("Set imu rate success!\n");
 
+    lock_guard<mutex> lock(lds_lidar->config_mutex_);
+    p_lidar->config.set_bits &= ~((uint32_t)(kConfigImuRate));
     if (!p_lidar->config.set_bits) {
       LidarStartSampling(handle, StartSampleCb, lds_lidar);
       p_lidar->connect_state = kConnectStateSampling;
     }
-
   } else {
     LidarSetImuPushFrequency(handle, (ImuFreq)(p_lidar->config.imu_rate),
                              SetImuRatePushFrequencyCb, g_lds_ldiar);
@@ -497,9 +452,10 @@ void LdsLidar::GetLidarExtrinsicParameterCb(
       if (p_lidar->config.extrinsic_parameter_source) {
         p_extrinsic->enable = true;
       }
-      p_lidar->config.set_bits &= ~((uint32_t)(kConfigGetExtrinsicParameter));
       printf("Lidar[%d] get ExtrinsicParameter success!\n", handle);
 
+      lock_guard<mutex> lock(lds_lidar->config_mutex_);
+      p_lidar->config.set_bits &= ~((uint32_t)(kConfigGetExtrinsicParameter));
       if (!p_lidar->config.set_bits) {
         LidarStartSampling(handle, StartSampleCb, lds_lidar);
         p_lidar->connect_state = kConnectStateSampling;
@@ -512,6 +468,35 @@ void LdsLidar::GetLidarExtrinsicParameterCb(
   }
 }
 
+void LdsLidar::SetHighSensitivityCb(livox_status status, uint8_t handle,
+                                    DeviceParameterResponse *response,
+                                    void *clent_data) {
+  LdsLidar *lds_lidar = static_cast<LdsLidar *>(clent_data);
+
+  if (handle >= kMaxLidarCount) {
+    return;
+  }
+  LidarDevice *p_lidar = &(lds_lidar->lidars_[handle]);
+
+  if (status == kStatusSuccess) {
+    p_lidar->config.set_bits &= ~((uint32_t)(kConfigSetHighSensitivity));
+    printf("Set high sensitivity success!\n");
+
+    lock_guard<mutex> lock(lds_lidar->config_mutex_);
+    if (!p_lidar->config.set_bits) {
+      LidarStartSampling(handle, StartSampleCb, lds_lidar);
+      p_lidar->connect_state = kConnectStateSampling;
+    };
+  } else {
+    if (p_lidar->config.enable_high_sensitivity) {
+      LidarEnableHighSensitivity(handle, SetHighSensitivityCb, g_lds_ldiar);
+    } else {
+      LidarDisableHighSensitivity(handle, SetHighSensitivityCb, g_lds_ldiar);
+    }
+    printf("Set high sensitivity fail, try again!\n");
+  }
+}
+
 /** Callback function of starting sampling. */
 void LdsLidar::StartSampleCb(livox_status status, uint8_t handle,
                              uint8_t response, void *clent_data) {
@@ -607,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())
@@ -676,7 +660,7 @@ int LdsLidar::ParseConfigFile(const char *pathname) {
       for (size_t i = 0; i < len; i++) {
         const rapidjson::Value &object = array[i];
         if (object.IsObject()) {
-          UserRawConfig config;
+          UserRawConfig config = {0};
           memset(&config, 0, sizeof(config));
           if (object.HasMember("broadcast_code") &&
               object["broadcast_code"].IsString()) {
@@ -710,6 +694,11 @@ int LdsLidar::ParseConfigFile(const char *pathname) {
             config.extrinsic_parameter_source =
                 object["extrinsic_parameter_source"].GetInt();
           }
+          if (object.HasMember("enable_high_sensitivity") &&
+              object["enable_high_sensitivity"].GetBool()) {
+            config.enable_high_sensitivity =
+                object["enable_high_sensitivity"].GetBool();
+          }
 
           printf("broadcast code[%s] : %d %d %d %d %d %d\n",
                  config.broadcast_code, config.enable_connect,
@@ -780,6 +769,7 @@ int LdsLidar::GetRawConfig(const char *broadcast_code, UserRawConfig &config) {
       config.coordinate = ite_config.coordinate;
       config.imu_rate = ite_config.imu_rate;
       config.extrinsic_parameter_source = ite_config.extrinsic_parameter_source;
+      config.enable_high_sensitivity = ite_config.enable_high_sensitivity;
       return 0;
     }
   }

livox_ros_driver/livox_ros_driver/lds_lidar.h

@@ -28,6 +28,7 @@
 #define LIVOX_ROS_DRIVER_LDS_LIDAR_H_
 
 #include <memory>
+#include <mutex>
 #include <vector>
 
 #include "lds.h"
@@ -41,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;
@@ -51,7 +52,7 @@ public:
                    const char *user_config_path);
   int DeInitLdsLidar(void);
 
-private:
+ private:
   LdsLidar(uint32_t interval_ms);
   LdsLidar(const LdsLidar &) = delete;
   ~LdsLidar();
@@ -83,9 +84,11 @@ 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);
+  static void SetHighSensitivityCb(livox_status status, uint8_t handle,
+                                   DeviceParameterResponse *response,
                                    void *clent_data);
 
   void ResetLdsLidar(void);
@@ -110,6 +113,7 @@ private:
   bool enable_timesync_;
   TimeSync *timesync_;
   TimeSyncConfig timesync_config_;
+  std::mutex config_mutex_;
 };
 
 }  // namespace livox_ros

livox_ros_driver/livox_ros_driver/lds_lvx.cpp

@@ -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;
@@ -161,29 +164,23 @@ void LdsLvx::ReadLvxFile() {
           eth_packet = (LivoxEthPacket *)(&detail_packet->version);
           handle = detail_packet->device_index;
         }
+
         data_type = eth_packet->data_type;
-        data_offset +=
+        /** Packet length + device index */
-            (GetEthPacketLen(data_type) + 1); /* packet length + device index */
+        data_offset += (GetEthPacketLen(data_type) + 1);
-        if (data_type != kImu) {
+        StorageRawPacket(handle, eth_packet);
+
         LidarDataQueue *p_queue = &lidars_[handle].data;
-          if ((p_queue != nullptr) && (handle < lidar_count_)) {
+        if (p_queue != nullptr) {
           while (QueueIsFull(p_queue)) {
             std::this_thread::sleep_for(std::chrono::milliseconds(1));
           }
-            QueuePushAny(p_queue, (uint8_t *)eth_packet,
-                         GetEthPacketLen(data_type), 0,
-                         GetPointsPerPacket(data_type));
         }
-        } else {
+        p_queue = &lidars_[handle].imu_data;
-          LidarDataQueue *p_queue = &lidars_[handle].imu_data;
+        if (p_queue != nullptr) {
-          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));
-          }
         }
       }
     } else {
@@ -200,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) {
@@ -211,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() {
-  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_;

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,22 +58,31 @@ 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;
-  double publish_freq = 20.0; /* Hz */
+  double publish_freq = 10.0; /* Hz */
   int output_type = kOutputToRos;
+  std::string frame_id = "livox_frame";
 
   livox_node.getParam("xfer_format", xfer_format);
   livox_node.getParam("multi_topic", multi_topic);
   livox_node.getParam("data_src", data_src);
   livox_node.getParam("publish_freq", publish_freq);
   livox_node.getParam("output_data_type", output_type);
+  livox_node.getParam("frame_id", frame_id);
+  if (publish_freq > 100.0) {
+    publish_freq = 100.0;
+  } else if (publish_freq < 1.0) {
+    publish_freq = 1.0;
+  } else {
+    publish_freq = publish_freq;
+  }
 
   /** Lidar data distribute control and lidar data source set */
-  Lddc *lddc =
+  Lddc *lddc = new Lddc(xfer_format, multi_topic, data_src, output_type,
-      new Lddc(xfer_format, multi_topic, data_src, output_type, publish_freq);
+                        publish_freq, frame_id);
   lddc->SetRosNode(&livox_node);
 
   int ret = 0;
@@ -150,14 +158,8 @@ int main(int argc, char **argv) {
   }
 
   ros::Time::init();
-  double poll_freq = publish_freq;
-  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,

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_;

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;

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;

livox_ros_driver/timesync/user_uart/user_uart.cpp

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

livox_ros_driver/timesync/user_uart/user_uart.h

@@ -66,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_;