mirrors/livox_ros_driver
1
0
Fork 0
mirror of https://github.com/Livox-SDK/livox_ros_driver.git synced 2023-04-06 15:49:55 +08:00
Code Issues Packages Projects Releases Wiki Activity

feat:support AVIA and Mid-70

Browse Source
This commit is contained in:
Livox-SDK
2020-10-26 21:10:14 +08:00
parent 2c68829af6
commit fb5669b90a
75 changed files with 4301 additions and 4070 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
README.md
View File

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

16
README_CN.md
View File

@@ -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
&ensp;&ensp;&ensp;&ensp;上面 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 |

20
livox_ros_driver/common/FastCRC/FastCRC.h
View File

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

12
livox_ros_driver/common/FastCRC/FastCRCsw.cpp
View File

@@ -55,7 +55,6 @@ FastCRC16::FastCRC16(uint16_t seed) { seed_ = seed; }
*/
uint16_t FastCRC16::mcrf4xx_calc(const uint8_t *data, uint16_t len) {
uint16_t crc = seed_;
while (((uintptr_t)data & 3) && len) {
@@ -90,11 +89,11 @@ FastCRC32::FastCRC32(uint32_t seed) { seed_ = seed; }
crc = (table[(crc & 0xff) + 0x300]) ^ (table[((crc >> 8) & 0xff) + 0x200]) ^ \
(table[((crc >> 16) & 0xff) + 0x100]) ^ (table[(crc >> 24) & 0xff]);
#define crcsm_n4d(crc, data, table) \
crc ^= data; \
crc = (crc >> 8) ^ (table[crc & 0xff]); \
crc = (crc >> 8) ^ (table[crc & 0xff]); \
crc = (crc >> 8) ^ (table[crc & 0xff]); \
#define crcsm_n4d(crc, data, table) \
crc ^= data; \
crc = (crc >> 8) ^ (table[crc & 0xff]); \
crc = (crc >> 8) ^ (table[crc & 0xff]); \
crc = (crc >> 8) ^ (table[crc & 0xff]); \
crc = (crc >> 8) ^ (table[crc & 0xff]);
/** CRC32
@@ -110,7 +109,6 @@ FastCRC32::FastCRC32(uint32_t seed) { seed_ = seed; }
#endif
uint32_t FastCRC32::crc32_calc(const uint8_t *data, uint16_t len) {
uint32_t crc = seed_ ^ 0xffffffff;
while (((uintptr_t)data & 3) && len) {

4
livox_ros_driver/common/comm/comm_device.h
View File

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

30
livox_ros_driver/common/comm/comm_protocol.cpp
View File

@@ -23,9 +23,9 @@
//
#include "comm_protocol.h"
#include <iostream>
#include <stdio.h>
#include <string.h>
#include <iostream>
namespace livox_ros {
@@ -116,19 +116,19 @@ int32_t CommProtocol::ParseCommStream(CommPacket *o_pack) {
while ((GetValidDataSize() > protocol_->GetPreambleLen()) &&
(GetValidDataSize() > offset_to_read_index_)) {
switch (fsm_parse_step_) {
case kSearchPacketPreamble: {
FsmSearchPacketPreamble();
break;
}
case kFindPacketLength: {
FsmFindPacketLength();
break;
}
case kGetPacketData: {
ret = FsmGetPacketData(o_pack);
break;
}
default: { FsmParserStateTransfer(kSearchPacketPreamble); }
case kSearchPacketPreamble: {
FsmSearchPacketPreamble();
break;
}
case kFindPacketLength: {
FsmFindPacketLength();
break;
}
case kGetPacketData: {
ret = FsmGetPacketData(o_pack);
break;
}
default: { FsmParserStateTransfer(kSearchPacketPreamble); }
}
/* Must exit when in the below case */
@@ -221,4 +221,4 @@ int32_t CommProtocol::FsmGetPacketData(CommPacket *o_pack) {
return ret;
}
} // namespace livox_ros
} // namespace livox_ros

10
livox_ros_driver/common/comm/comm_protocol.h
View File

@@ -25,10 +25,10 @@
#ifndef COMM_COMM_PROTOCOL_H_
#define COMM_COMM_PROTOCOL_H_
#include <stdint.h>
#include "gps_protocol.h"
#include "protocol.h"
#include "sdk_protocol.h"
#include <stdint.h>
namespace livox_ros {
const uint32_t kCacheSize = 8192;
@@ -50,7 +50,7 @@ typedef struct {
} CommCache;
class CommProtocol {
public:
public:
CommProtocol(ProtocolConfig &config);
~CommProtocol();
@@ -67,7 +67,7 @@ public:
void ResetParser();
private:
private:
uint32_t GetCacheTailSize();
uint32_t GetValidDataSize();
void UpdateCache(void);
@@ -101,5 +101,5 @@ private:
}
};
} // namespace livox_ros
#endif // COMM_COMM_PROTOCOL_H_
} // namespace livox_ros
#endif // COMM_COMM_PROTOCOL_H_

10
livox_ros_driver/common/comm/gps_protocol.cpp
View File

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

10
livox_ros_driver/common/comm/gps_protocol.h
View File

@@ -25,8 +25,8 @@
#ifndef LIVOX_GPS_PROTOCOL_H_
#define LIVOX_GPS_PROTOCOL_H_
#include "protocol.h"
#include <stdint.h>
#include "protocol.h"
namespace livox_ros {
@@ -47,7 +47,7 @@ typedef struct {
uint8_t AscciiToHex(const uint8_t *TwoChar);
class GpsProtocol : public Protocol {
public:
public:
GpsProtocol();
~GpsProtocol() = default;
@@ -69,11 +69,11 @@ public:
int32_t CheckPacket(const uint8_t *buf) override;
private:
private:
uint32_t found_length_;
uint8_t CalcGpsPacketChecksum(const uint8_t *buf, uint32_t length);
};
} // namespace livox_ros
#endif // LIVOX_GPS_PROTOCOL_H_
} // namespace livox_ros
#endif // LIVOX_GPS_PROTOCOL_H_

13
livox_ros_driver/common/comm/protocol.h
View File

@@ -60,9 +60,6 @@ typedef struct CommPacket {
uint8_t *data;
uint16_t data_len;
uint32_t padding;
// RequestPackCb *ack_request_cb;
// uint32_t retry_times;
// uint32_t timeout;
} CommPacket;
/** SDK Protocol info config */
@@ -72,9 +69,7 @@ typedef struct {
} SdkProtocolConfig;
/** NAME-0183 Protocol info config for gps */
typedef struct {
void *data;
} GpsProtocolConfig;
typedef struct { void *data; } GpsProtocolConfig;
typedef struct {
uint8_t type;
@@ -85,7 +80,7 @@ typedef struct {
} ProtocolConfig;
class Protocol {
public:
public:
virtual ~Protocol() = default;
virtual int32_t ParsePacket(const uint8_t *i_buf, uint32_t i_len,
@@ -107,5 +102,5 @@ public:
virtual int32_t CheckPacket(const uint8_t *buf) = 0;
};
} // namespace livox_ros
#endif // COMM_PROTOCOL_H_
} // namespace livox_ros
#endif // COMM_PROTOCOL_H_

8
livox_ros_driver/common/comm/sdk_protocol.cpp
View File

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

10
livox_ros_driver/common/comm/sdk_protocol.h
View File

@@ -25,9 +25,9 @@
#ifndef LIVOX_SDK_PROTOCOL_H_
#define LIVOX_SDK_PROTOCOL_H_
#include <stdint.h>
#include "FastCRC/FastCRC.h"
#include "protocol.h"
#include <stdint.h>
namespace livox_ros {
typedef enum { kSdkVerNone, kSdkVer0, kSdkVer1 } SdkVersion;
@@ -58,7 +58,7 @@ typedef struct {
#pragma pack()
class SdkProtocol : public Protocol {
public:
public:
SdkProtocol(uint16_t seed16, uint32_t seed32);
~SdkProtocol() = default;
@@ -78,9 +78,9 @@ public:
int32_t CheckPacket(const uint8_t *buf) override;
private:
private:
FastCRC16 crc16_;
FastCRC32 crc32_;
};
} // namespace livox_ros
#endif // LIVOX_SDK_PROTOCOL_H_
} // namespace livox_ros
#endif // LIVOX_SDK_PROTOCOL_H_

86
livox_ros_driver/common/rapidjson/allocators.h
View File

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

14
livox_ros_driver/common/rapidjson/cursorstreamwrapper.h
View File

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

751
livox_ros_driver/common/rapidjson/document.h
View File

File diff suppressed because it is too large Load Diff

79
livox_ros_driver/common/rapidjson/encodedstream.h
View File

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

251
livox_ros_driver/common/rapidjson/encodings.h
View File

@@ -24,8 +24,8 @@
#if defined(_MSC_VER) && !defined(__clang__)
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(
4244) // conversion from 'type1' to 'type2', possible loss of data
RAPIDJSON_DIAG_OFF(4702) // unreachable code
4244) // conversion from 'type1' to 'type2', possible loss of data
RAPIDJSON_DIAG_OFF(4702) // unreachable code
#elif defined(__GNUC__)
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(effc++)
@@ -98,7 +98,8 @@ actually decode it. template <typename InputStream, typename OutputStream>
\tparam CharType Code unit for storing 8-bit UTF-8 data. Default is char.
\note implements Encoding concept
*/
template <typename CharType = char> struct UTF8 {
template <typename CharType = char>
struct UTF8 {
typedef CharType Ch;
enum { supportUnicode = 1 };
@@ -145,13 +146,13 @@ template <typename CharType = char> struct UTF8 {
template <typename InputStream>
static bool Decode(InputStream &is, unsigned *codepoint) {
#define RAPIDJSON_COPY() \
c = is.Take(); \
#define RAPIDJSON_COPY() \
c = is.Take(); \
*codepoint = (*codepoint << 6) | (static_cast<unsigned char>(c) & 0x3Fu)
#define RAPIDJSON_TRANS(mask) \
#define RAPIDJSON_TRANS(mask) \
result &= ((GetRange(static_cast<unsigned char>(c)) & mask) != 0)
#define RAPIDJSON_TAIL() \
RAPIDJSON_COPY(); \
#define RAPIDJSON_TAIL() \
RAPIDJSON_COPY(); \
RAPIDJSON_TRANS(0x70)
typename InputStream::Ch c = is.Take();
if (!(c & 0x80)) {
@@ -167,42 +168,42 @@ template <typename CharType = char> struct UTF8 {
}
bool result = true;
switch (type) {
case 2:
RAPIDJSON_TAIL();
return result;
case 3:
RAPIDJSON_TAIL();
RAPIDJSON_TAIL();
return result;
case 4:
RAPIDJSON_COPY();
RAPIDJSON_TRANS(0x50);
RAPIDJSON_TAIL();
return result;
case 5:
RAPIDJSON_COPY();
RAPIDJSON_TRANS(0x10);
RAPIDJSON_TAIL();
RAPIDJSON_TAIL();
return result;
case 6:
RAPIDJSON_TAIL();
RAPIDJSON_TAIL();
RAPIDJSON_TAIL();
return result;
case 10:
RAPIDJSON_COPY();
RAPIDJSON_TRANS(0x20);
RAPIDJSON_TAIL();
return result;
case 11:
RAPIDJSON_COPY();
RAPIDJSON_TRANS(0x60);
RAPIDJSON_TAIL();
RAPIDJSON_TAIL();
return result;
default:
return false;
case 2:
RAPIDJSON_TAIL();
return result;
case 3:
RAPIDJSON_TAIL();
RAPIDJSON_TAIL();
return result;
case 4:
RAPIDJSON_COPY();
RAPIDJSON_TRANS(0x50);
RAPIDJSON_TAIL();
return result;
case 5:
RAPIDJSON_COPY();
RAPIDJSON_TRANS(0x10);
RAPIDJSON_TAIL();
RAPIDJSON_TAIL();
return result;
case 6:
RAPIDJSON_TAIL();
RAPIDJSON_TAIL();
RAPIDJSON_TAIL();
return result;
case 10:
RAPIDJSON_COPY();
RAPIDJSON_TRANS(0x20);
RAPIDJSON_TAIL();
return result;
case 11:
RAPIDJSON_COPY();
RAPIDJSON_TRANS(0x60);
RAPIDJSON_TAIL();
RAPIDJSON_TAIL();
return result;
default:
return false;
}
#undef RAPIDJSON_COPY
#undef RAPIDJSON_TRANS
@@ -212,54 +213,53 @@ template <typename CharType = char> struct UTF8 {
template <typename InputStream, typename OutputStream>
static bool Validate(InputStream &is, OutputStream &os) {
#define RAPIDJSON_COPY() os.Put(c = is.Take())
#define RAPIDJSON_TRANS(mask) \
#define RAPIDJSON_TRANS(mask) \
result &= ((GetRange(static_cast<unsigned char>(c)) & mask) != 0)
#define RAPIDJSON_TAIL() \
RAPIDJSON_COPY(); \
#define RAPIDJSON_TAIL() \
RAPIDJSON_COPY(); \
RAPIDJSON_TRANS(0x70)
Ch c;
RAPIDJSON_COPY();
if (!(c & 0x80))
return true;
if (!(c & 0x80)) return true;
bool result = true;
switch (GetRange(static_cast<unsigned char>(c))) {
case 2:
RAPIDJSON_TAIL();
return result;
case 3:
RAPIDJSON_TAIL();
RAPIDJSON_TAIL();
return result;
case 4:
RAPIDJSON_COPY();
RAPIDJSON_TRANS(0x50);
RAPIDJSON_TAIL();
return result;
case 5:
RAPIDJSON_COPY();
RAPIDJSON_TRANS(0x10);
RAPIDJSON_TAIL();
RAPIDJSON_TAIL();
return result;
case 6:
RAPIDJSON_TAIL();
RAPIDJSON_TAIL();
RAPIDJSON_TAIL();
return result;
case 10:
RAPIDJSON_COPY();
RAPIDJSON_TRANS(0x20);
RAPIDJSON_TAIL();
return result;
case 11:
RAPIDJSON_COPY();
RAPIDJSON_TRANS(0x60);
RAPIDJSON_TAIL();
RAPIDJSON_TAIL();
return result;
default:
return false;
case 2:
RAPIDJSON_TAIL();
return result;
case 3:
RAPIDJSON_TAIL();
RAPIDJSON_TAIL();
return result;
case 4:
RAPIDJSON_COPY();
RAPIDJSON_TRANS(0x50);
RAPIDJSON_TAIL();
return result;
case 5:
RAPIDJSON_COPY();
RAPIDJSON_TRANS(0x10);
RAPIDJSON_TAIL();
RAPIDJSON_TAIL();
return result;
case 6:
RAPIDJSON_TAIL();
RAPIDJSON_TAIL();
RAPIDJSON_TAIL();
return result;
case 10:
RAPIDJSON_COPY();
RAPIDJSON_TRANS(0x20);
RAPIDJSON_TAIL();
return result;
case 11:
RAPIDJSON_COPY();
RAPIDJSON_TRANS(0x60);
RAPIDJSON_TAIL();
RAPIDJSON_TAIL();
return result;
default:
return false;
}
#undef RAPIDJSON_COPY
#undef RAPIDJSON_TRANS
@@ -301,19 +301,17 @@ template <typename CharType = char> struct UTF8 {
static CharType TakeBOM(InputByteStream &is) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
typename InputByteStream::Ch c = Take(is);
if (static_cast<unsigned char>(c) != 0xEFu)
return c;
if (static_cast<unsigned char>(c) != 0xEFu) return c;
c = is.Take();
if (static_cast<unsigned char>(c) != 0xBBu)
return c;
if (static_cast<unsigned char>(c) != 0xBBu) return c;
c = is.Take();
if (static_cast<unsigned char>(c) != 0xBFu)
return c;
if (static_cast<unsigned char>(c) != 0xBFu) return c;
c = is.Take();
return c;
}
template <typename InputByteStream> static Ch Take(InputByteStream &is) {
template <typename InputByteStream>
static Ch Take(InputByteStream &is) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
return static_cast<Ch>(is.Take());
}
@@ -346,7 +344,8 @@ template <typename CharType = char> struct UTF8 {
and code points are represented by CPU's endianness. For streaming, use
UTF16LE and UTF16BE, which handle endianness.
*/
template <typename CharType = wchar_t> struct UTF16 {
template <typename CharType = wchar_t>
struct UTF16 {
typedef CharType Ch;
RAPIDJSON_STATIC_ASSERT(sizeof(Ch) >= 2);
@@ -358,7 +357,7 @@ template <typename CharType = wchar_t> struct UTF16 {
if (codepoint <= 0xFFFF) {
RAPIDJSON_ASSERT(
codepoint < 0xD800 ||
codepoint > 0xDFFF); // Code point itself cannot be surrogate pair
codepoint > 0xDFFF); // Code point itself cannot be surrogate pair
os.Put(static_cast<typename OutputStream::Ch>(codepoint));
} else {
RAPIDJSON_ASSERT(codepoint <= 0x10FFFF);
@@ -374,7 +373,7 @@ template <typename CharType = wchar_t> struct UTF16 {
if (codepoint <= 0xFFFF) {
RAPIDJSON_ASSERT(
codepoint < 0xD800 ||
codepoint > 0xDFFF); // Code point itself cannot be surrogate pair
codepoint > 0xDFFF); // Code point itself cannot be surrogate pair
PutUnsafe(os, static_cast<typename OutputStream::Ch>(codepoint));
} else {
RAPIDJSON_ASSERT(codepoint <= 0x10FFFF);
@@ -419,7 +418,8 @@ template <typename CharType = wchar_t> struct UTF16 {
};
//! UTF-16 little endian encoding.
template <typename CharType = wchar_t> struct UTF16LE : UTF16<CharType> {
template <typename CharType = wchar_t>
struct UTF16LE : UTF16<CharType> {
template <typename InputByteStream>
static CharType TakeBOM(InputByteStream &is) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
@@ -453,7 +453,8 @@ template <typename CharType = wchar_t> struct UTF16LE : UTF16<CharType> {
};
//! UTF-16 big endian encoding.
template <typename CharType = wchar_t> struct UTF16BE : UTF16<CharType> {
template <typename CharType = wchar_t>
struct UTF16BE : UTF16<CharType> {
template <typename InputByteStream>
static CharType TakeBOM(InputByteStream &is) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
@@ -498,7 +499,8 @@ template <typename CharType = wchar_t> struct UTF16BE : UTF16<CharType> {
and code points are represented by CPU's endianness. For streaming, use
UTF32LE and UTF32BE, which handle endianness.
*/
template <typename CharType = unsigned> struct UTF32 {
template <typename CharType = unsigned>
struct UTF32 {
typedef CharType Ch;
RAPIDJSON_STATIC_ASSERT(sizeof(Ch) >= 4);
@@ -536,7 +538,8 @@ template <typename CharType = unsigned> struct UTF32 {
};
//! UTF-32 little endian enocoding.
template <typename CharType = unsigned> struct UTF32LE : UTF32<CharType> {
template <typename CharType = unsigned>
struct UTF32LE : UTF32<CharType> {
template <typename InputByteStream>
static CharType TakeBOM(InputByteStream &is) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
@@ -574,7 +577,8 @@ template <typename CharType = unsigned> struct UTF32LE : UTF32<CharType> {
};
//! UTF-32 big endian encoding.
template <typename CharType = unsigned> struct UTF32BE : UTF32<CharType> {
template <typename CharType = unsigned>
struct UTF32BE : UTF32<CharType> {
template <typename InputByteStream>
static CharType TakeBOM(InputByteStream &is) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
@@ -619,7 +623,8 @@ template <typename CharType = unsigned> struct UTF32BE : UTF32<CharType> {
\tparam CharType Code unit for storing 7-bit ASCII data. Default is char.
\note implements Encoding concept
*/
template <typename CharType = char> struct ASCII {
template <typename CharType = char>
struct ASCII {
typedef CharType Ch;
enum { supportUnicode = 0 };
@@ -657,7 +662,8 @@ template <typename CharType = char> struct ASCII {
return static_cast<Ch>(c);
}
template <typename InputByteStream> static Ch Take(InputByteStream &is) {
template <typename InputByteStream>
static Ch Take(InputByteStream &is) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
return static_cast<Ch>(is.Take());
}
@@ -680,11 +686,11 @@ template <typename CharType = char> struct ASCII {
//! Runtime-specified UTF encoding type of a stream.
enum UTFType {
kUTF8 = 0, //!< UTF-8.
kUTF16LE = 1, //!< UTF-16 little endian.
kUTF16BE = 2, //!< UTF-16 big endian.
kUTF32LE = 3, //!< UTF-32 little endian.
kUTF32BE = 4 //!< UTF-32 big endian.
kUTF8 = 0, //!< UTF-8.
kUTF16LE = 1, //!< UTF-16 little endian.
kUTF16BE = 2, //!< UTF-16 big endian.
kUTF32LE = 3, //!< UTF-32 little endian.
kUTF32BE = 4 //!< UTF-32 big endian.
};
//! Dynamically select encoding according to stream's runtime-specified UTF
@@ -692,12 +698,13 @@ enum UTFType {
/*! \note This class can be used with AutoUTFInputtStream and
* AutoUTFOutputStream, which provides GetType().
*/
template <typename CharType> struct AutoUTF {
template <typename CharType>
struct AutoUTF {
typedef CharType Ch;
enum { supportUnicode = 1 };
#define RAPIDJSON_ENCODINGS_FUNC(x) \
#define RAPIDJSON_ENCODINGS_FUNC(x) \
UTF8<Ch>::x, UTF16LE<Ch>::x, UTF16BE<Ch>::x, UTF32LE<Ch>::x, UTF32BE<Ch>::x
template <typename OutputStream>
@@ -739,15 +746,15 @@ template <typename CharType> struct AutoUTF {
// Transcoder
//! Encoding conversion.
template <typename SourceEncoding, typename TargetEncoding> struct Transcoder {
template <typename SourceEncoding, typename TargetEncoding>
struct Transcoder {
//! Take one Unicode codepoint from source encoding, convert it to target
//! encoding and put it to the output stream.
template <typename InputStream, typename OutputStream>
static RAPIDJSON_FORCEINLINE bool Transcode(InputStream &is,
OutputStream &os) {
unsigned codepoint;
if (!SourceEncoding::Decode(is, &codepoint))
return false;
if (!SourceEncoding::Decode(is, &codepoint)) return false;
TargetEncoding::Encode(os, codepoint);
return true;
}
@@ -756,8 +763,7 @@ template <typename SourceEncoding, typename TargetEncoding> struct Transcoder {
static RAPIDJSON_FORCEINLINE bool TranscodeUnsafe(InputStream &is,
OutputStream &os) {
unsigned codepoint;
if (!SourceEncoding::Decode(is, &codepoint))
return false;
if (!SourceEncoding::Decode(is, &codepoint)) return false;
TargetEncoding::EncodeUnsafe(os, codepoint);
return true;
}
@@ -767,7 +773,7 @@ template <typename SourceEncoding, typename TargetEncoding> struct Transcoder {
static RAPIDJSON_FORCEINLINE bool Validate(InputStream &is,
OutputStream &os) {
return Transcode(
is, os); // Since source/target encoding is different, must transcode.
is, os); // Since source/target encoding is different, must transcode.
}
};
@@ -776,27 +782,28 @@ template <typename Stream>
inline void PutUnsafe(Stream &stream, typename Stream::Ch c);
//! Specialization of Transcoder with same source and target encoding.
template <typename Encoding> struct Transcoder<Encoding, Encoding> {
template <typename Encoding>
struct Transcoder<Encoding, Encoding> {
template <typename InputStream, typename OutputStream>
static RAPIDJSON_FORCEINLINE bool Transcode(InputStream &is,
OutputStream &os) {
os.Put(is.Take()); // Just copy one code unit. This semantic is different
// from primary template class.
os.Put(is.Take()); // Just copy one code unit. This semantic is different
// from primary template class.
return true;
}
template <typename InputStream, typename OutputStream>
static RAPIDJSON_FORCEINLINE bool TranscodeUnsafe(InputStream &is,
OutputStream &os) {
PutUnsafe(os, is.Take()); // Just copy one code unit. This semantic is
// different from primary template class.
PutUnsafe(os, is.Take()); // Just copy one code unit. This semantic is
// different from primary template class.
return true;
}
template <typename InputStream, typename OutputStream>
static RAPIDJSON_FORCEINLINE bool Validate(InputStream &is,
OutputStream &os) {
return Encoding::Validate(is, os); // source/target encoding are the same
return Encoding::Validate(is, os); // source/target encoding are the same
}
};
@@ -806,4 +813,4 @@ RAPIDJSON_NAMESPACE_END
RAPIDJSON_DIAG_POP
#endif
#endif // RAPIDJSON_ENCODINGS_H_
#endif // RAPIDJSON_ENCODINGS_H_

94
livox_ros_driver/common/rapidjson/error/en.h
View File

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

59
livox_ros_driver/common/rapidjson/error/error.h
View File

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

22
livox_ros_driver/common/rapidjson/filereadstream.h
View File

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

20
livox_ros_driver/common/rapidjson/filewritestream.h
View File

@@ -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:
typedef char Ch; //!< Character type. Only support char.
public:
typedef char Ch; //!< Character type. Only support char.
FileWriteStream(std::FILE *fp, char *buffer, size_t bufferSize)
: fp_(fp), buffer_(buffer), bufferEnd_(buffer + bufferSize),
: fp_(fp),
buffer_(buffer),
bufferEnd_(buffer + bufferSize),
current_(buffer_) {
RAPIDJSON_ASSERT(fp_ != 0);
}
void Put(char c) {
if (current_ >= bufferEnd_)
Flush();
if (current_ >= bufferEnd_) Flush();
*current_++ = c;
}
@@ -100,7 +101,7 @@ public:
return 0;
}
private:
private:
// Prohibit copy constructor & assignment operator.
FileWriteStream(const FileWriteStream &);
FileWriteStream &operator=(const FileWriteStream &);
@@ -113,7 +114,8 @@ private:
//! Implement specialized version of PutN() with memset() for better
//! performance.
template <> inline void PutN(FileWriteStream &stream, char c, size_t n) {
template <>
inline void PutN(FileWriteStream &stream, char c, size_t n) {
stream.PutN(c, n);
}
@@ -123,4 +125,4 @@ RAPIDJSON_NAMESPACE_END
RAPIDJSON_DIAG_POP
#endif
#endif // RAPIDJSON_FILESTREAM_H_
#endif // RAPIDJSON_FILESTREAM_H_

65
livox_ros_driver/common/rapidjson/fwd.h
View File

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

89
livox_ros_driver/common/rapidjson/internal/biginteger.h
View File

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

17
livox_ros_driver/common/rapidjson/internal/clzll.h
View File

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

24
livox_ros_driver/common/rapidjson/internal/diyfp.h
View File

@@ -23,9 +23,9 @@
#ifndef RAPIDJSON_DIYFP_H_
#define RAPIDJSON_DIYFP_H_
#include <limits>
#include "../rapidjson.h"
#include "clzll.h"
#include <limits>
#if defined(_MSC_VER) && defined(_M_AMD64) && !defined(__INTEL_COMPILER)
#include <intrin.h>
@@ -74,16 +74,16 @@ struct DiyFp {
#if defined(_MSC_VER) && defined(_M_AMD64)
uint64_t h;
uint64_t l = _umul128(f, rhs.f, &h);
if (l & (uint64_t(1) << 63)) // rounding
if (l & (uint64_t(1) << 63)) // rounding
h++;
return DiyFp(h, e + rhs.e + 64);
#elif (__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 6)) && \
#elif (__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 6)) && \
defined(__x86_64__)
__extension__ typedef unsigned __int128 uint128;
uint128 p = static_cast<uint128>(f) * static_cast<uint128>(rhs.f);
uint64_t h = static_cast<uint64_t>(p >> 64);
uint64_t l = static_cast<uint64_t>(p);
if (l & (uint64_t(1) << 63)) // rounding
if (l & (uint64_t(1) << 63)) // rounding
h++;
return DiyFp(h, e + rhs.e + 64);
#else
@@ -97,7 +97,7 @@ struct DiyFp {
const uint64_t ad = a * d;
const uint64_t bd = b * d;
uint64_t tmp = (bd >> 32) + (ad & M32) + (bc & M32);
tmp += 1U << 31; /// mult_round
tmp += 1U << 31; /// mult_round
return DiyFp(ac + (ad >> 32) + (bc >> 32) + (tmp >> 32), e + rhs.e + 64);
#endif
}
@@ -270,17 +270,15 @@ inline DiyFp GetCachedPowerByIndex(size_t index) {
}
inline DiyFp GetCachedPower(int e, int *K) {
// int k = static_cast<int>(ceil((-61 - e) * 0.30102999566398114)) + 374;
double dk = (-61 - e) * 0.30102999566398114 +
347; // dk must be positive, so can do ceiling in positive
347; // dk must be positive, so can do ceiling in positive
int k = static_cast<int>(dk);
if (dk - k > 0.0)
k++;
if (dk - k > 0.0) k++;
unsigned index = static_cast<unsigned>((k >> 3) + 1);
*K = -(-348 +
static_cast<int>(index << 3)); // decimal exponent no need lookup table
*K = -(-348 + static_cast<int>(
index << 3)); // decimal exponent no need lookup table
return GetCachedPowerByIndex(index);
}
@@ -301,7 +299,7 @@ RAPIDJSON_DIAG_POP
RAPIDJSON_DIAG_OFF(padded)
#endif
} // namespace internal
} // namespace internal
RAPIDJSON_NAMESPACE_END
#endif // RAPIDJSON_DIYFP_H_
#endif // RAPIDJSON_DIYFP_H_

137
livox_ros_driver/common/rapidjson/internal/dtoa.h
View File

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

8
livox_ros_driver/common/rapidjson/internal/ieee754.h
View File

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

69
livox_ros_driver/common/rapidjson/internal/itoa.h
View File

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

121
livox_ros_driver/common/rapidjson/internal/meta.h
View File

@@ -41,12 +41,16 @@ namespace internal {
// Helper to wrap/convert arbitrary types to void, useful for arbitrary type
// matching
template <typename T> struct Void { typedef void Type; };
template <typename T>
struct Void {
typedef void Type;
};
///////////////////////////////////////////////////////////////////////////////
// BoolType, TrueType, FalseType
//
template <bool Cond> struct BoolType {
template <bool Cond>
struct BoolType {
static const bool Value = Cond;
typedef BoolType Type;
};
@@ -57,21 +61,33 @@ typedef BoolType<false> FalseType;
// SelectIf, BoolExpr, NotExpr, AndExpr, OrExpr
//
template <bool C> struct SelectIfImpl {
template <typename T1, typename T2> struct Apply { typedef T1 Type; };
template <bool C>
struct SelectIfImpl {
template <typename T1, typename T2>
struct Apply {
typedef T1 Type;
};
};
template <> struct SelectIfImpl<false> {
template <typename T1, typename T2> struct Apply { typedef T2 Type; };
template <>
struct SelectIfImpl<false> {
template <typename T1, typename T2>
struct Apply {
typedef T2 Type;
};
};
template <bool C, typename T1, typename T2>
struct SelectIfCond : SelectIfImpl<C>::template Apply<T1, T2> {};
template <typename C, typename T1, typename T2>
struct SelectIf : SelectIfCond<C::Value, T1, T2> {};
template <bool Cond1, bool Cond2> struct AndExprCond : FalseType {};
template <> struct AndExprCond<true, true> : TrueType {};
template <bool Cond1, bool Cond2> struct OrExprCond : TrueType {};
template <> struct OrExprCond<false, false> : FalseType {};
template <bool Cond1, bool Cond2>
struct AndExprCond : FalseType {};
template <>
struct AndExprCond<true, true> : TrueType {};
template <bool Cond1, bool Cond2>
struct OrExprCond : TrueType {};
template <>
struct OrExprCond<false, false> : FalseType {};
template <typename C>
struct BoolExpr : SelectIf<C, TrueType, FalseType>::Type {};
@@ -84,20 +100,33 @@ struct OrExpr : OrExprCond<C1::Value, C2::Value>::Type {};
///////////////////////////////////////////////////////////////////////////////
// AddConst, MaybeAddConst, RemoveConst
template <typename T> struct AddConst { typedef const T Type; };
template <typename T>
struct AddConst {
typedef const T Type;
};
template <bool Constify, typename T>
struct MaybeAddConst : SelectIfCond<Constify, const T, T> {};
template <typename T> struct RemoveConst { typedef T Type; };
template <typename T> struct RemoveConst<const T> { typedef T Type; };
template <typename T>
struct RemoveConst {
typedef T Type;
};
template <typename T>
struct RemoveConst<const T> {
typedef T Type;
};
///////////////////////////////////////////////////////////////////////////////
// IsSame, IsConst, IsMoreConst, IsPointer
//
template <typename T, typename U> struct IsSame : FalseType {};
template <typename T> struct IsSame<T, T> : TrueType {};
template <typename T, typename U>
struct IsSame : FalseType {};
template <typename T>
struct IsSame<T, T> : TrueType {};
template <typename T> struct IsConst : FalseType {};
template <typename T> struct IsConst<const T> : TrueType {};
template <typename T>
struct IsConst : FalseType {};
template <typename T>
struct IsConst<const T> : TrueType {};
template <typename CT, typename T>
struct IsMoreConst
@@ -105,8 +134,10 @@ struct IsMoreConst
IsSame<typename RemoveConst<CT>::Type, typename RemoveConst<T>::Type>,
BoolType<IsConst<CT>::Value >= IsConst<T>::Value>>::Type {};
template <typename T> struct IsPointer : FalseType {};
template <typename T> struct IsPointer<T *> : TrueType {};
template <typename T>
struct IsPointer : FalseType {};
template <typename T>
struct IsPointer<T *> : TrueType {};
///////////////////////////////////////////////////////////////////////////////
// IsBaseOf
@@ -116,16 +147,18 @@ template <typename T> struct IsPointer<T *> : TrueType {};
template <typename B, typename D>
struct IsBaseOf : BoolType<::std::is_base_of<B, D>::value> {};
#else // simplified version adopted from Boost
#else // simplified version adopted from Boost
template <typename B, typename D> struct IsBaseOfImpl {
template <typename B, typename D>
struct IsBaseOfImpl {
RAPIDJSON_STATIC_ASSERT(sizeof(B) != 0);
RAPIDJSON_STATIC_ASSERT(sizeof(D) != 0);
typedef char (&Yes)[1];
typedef char (&No)[2];
template <typename T> static Yes Check(const D *, T);
template <typename T>
static Yes Check(const D *, T);
static No Check(const B *, int);
struct Host {
@@ -139,21 +172,25 @@ template <typename B, typename D> struct IsBaseOfImpl {
template <typename B, typename D>
struct IsBaseOf : OrExpr<IsSame<B, D>, BoolExpr<IsBaseOfImpl<B, D>>>::Type {};
#endif // RAPIDJSON_HAS_CXX11_TYPETRAITS
#endif // RAPIDJSON_HAS_CXX11_TYPETRAITS
//////////////////////////////////////////////////////////////////////////
// EnableIf / DisableIf
//
template <bool Condition, typename T = void> struct EnableIfCond {
template <bool Condition, typename T = void>
struct EnableIfCond {
typedef T Type;
};
template <typename T> struct EnableIfCond<false, T> { /* empty */
template <typename T>
struct EnableIfCond<false, T> { /* empty */
};
template <bool Condition, typename T = void> struct DisableIfCond {
template <bool Condition, typename T = void>
struct DisableIfCond {
typedef T Type;
};
template <typename T> struct DisableIfCond<true, T> { /* empty */
template <typename T>
struct DisableIfCond<true, T> { /* empty */
};
template <typename Condition, typename T = void>
@@ -164,32 +201,34 @@ struct DisableIf : DisableIfCond<Condition::Value, T> {};
// SFINAE helpers
struct SfinaeTag {};
template <typename T> struct RemoveSfinaeTag;
template <typename T> struct RemoveSfinaeTag<SfinaeTag &(*)(T)> {
template <typename T>
struct RemoveSfinaeTag;
template <typename T>
struct RemoveSfinaeTag<SfinaeTag &(*)(T)> {
typedef T Type;
};
#define RAPIDJSON_REMOVEFPTR_(type) \
typename ::RAPIDJSON_NAMESPACE::internal::RemoveSfinaeTag< \
#define RAPIDJSON_REMOVEFPTR_(type) \
typename ::RAPIDJSON_NAMESPACE::internal::RemoveSfinaeTag< \
::RAPIDJSON_NAMESPACE::internal::SfinaeTag &(*)type>::Type
#define RAPIDJSON_ENABLEIF(cond) \
typename ::RAPIDJSON_NAMESPACE::internal::EnableIf<RAPIDJSON_REMOVEFPTR_( \
#define RAPIDJSON_ENABLEIF(cond) \
typename ::RAPIDJSON_NAMESPACE::internal::EnableIf<RAPIDJSON_REMOVEFPTR_( \
cond)>::Type * = NULL
#define RAPIDJSON_DISABLEIF(cond) \
typename ::RAPIDJSON_NAMESPACE::internal::DisableIf<RAPIDJSON_REMOVEFPTR_( \
#define RAPIDJSON_DISABLEIF(cond) \
typename ::RAPIDJSON_NAMESPACE::internal::DisableIf<RAPIDJSON_REMOVEFPTR_( \
cond)>::Type * = NULL
#define RAPIDJSON_ENABLEIF_RETURN(cond, returntype) \
typename ::RAPIDJSON_NAMESPACE::internal::EnableIf< \
#define RAPIDJSON_ENABLEIF_RETURN(cond, returntype) \
typename ::RAPIDJSON_NAMESPACE::internal::EnableIf< \
RAPIDJSON_REMOVEFPTR_(cond), RAPIDJSON_REMOVEFPTR_(returntype)>::Type
#define RAPIDJSON_DISABLEIF_RETURN(cond, returntype) \
typename ::RAPIDJSON_NAMESPACE::internal::DisableIf< \
#define RAPIDJSON_DISABLEIF_RETURN(cond, returntype) \
typename ::RAPIDJSON_NAMESPACE::internal::DisableIf< \
RAPIDJSON_REMOVEFPTR_(cond), RAPIDJSON_REMOVEFPTR_(returntype)>::Type
} // namespace internal
} // namespace internal
RAPIDJSON_NAMESPACE_END
//@endcond
@@ -201,4 +240,4 @@ RAPIDJSON_DIAG_POP
RAPIDJSON_DIAG_POP
#endif
#endif // RAPIDJSON_INTERNAL_META_H_
#endif // RAPIDJSON_INTERNAL_META_H_

4
livox_ros_driver/common/rapidjson/internal/pow10.h
View File

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

540
livox_ros_driver/common/rapidjson/internal/regex.h
View File

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

79
livox_ros_driver/common/rapidjson/internal/stack.h
View File

@@ -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 {
public:
template <typename Allocator>
class Stack {
public:
// Optimization note: Do not allocate memory for stack_ in constructor.
// Do it lazily when first Push() -> Expand() -> Resize().
Stack(Allocator *allocator, size_t stackCapacity)
: allocator_(allocator), ownAllocator_(0), stack_(0), stackTop_(0),
stackEnd_(0), initialCapacity_(stackCapacity) {}
: allocator_(allocator),
ownAllocator_(0),
stack_(0),
stackTop_(0),
stackEnd_(0),
initialCapacity_(stackCapacity) {}
#if RAPIDJSON_HAS_CXX11_RVALUE_REFS
Stack(Stack &&rhs)
: allocator_(rhs.allocator_), ownAllocator_(rhs.ownAllocator_),
stack_(rhs.stack_), stackTop_(rhs.stackTop_), stackEnd_(rhs.stackEnd_),
: allocator_(rhs.allocator_),
ownAllocator_(rhs.ownAllocator_),
stack_(rhs.stack_),
stackTop_(rhs.stackTop_),
stackEnd_(rhs.stackEnd_),
initialCapacity_(rhs.initialCapacity_) {
rhs.allocator_ = 0;
rhs.ownAllocator_ = 0;
@@ -98,7 +106,7 @@ public:
void ShrinkToFit() {
if (Empty()) {
// If the stack is empty, completely deallocate the memory.
Allocator::Free(stack_); // NOLINT (+clang-analyzer-unix.Malloc)
Allocator::Free(stack_); // NOLINT (+clang-analyzer-unix.Malloc)
stack_ = 0;
stackTop_ = 0;
stackEnd_ = 0;
@@ -109,19 +117,22 @@ public:
// Optimization note: try to minimize the size of this function for force
// inline. Expansion is run very infrequently, so it is moved to another
// (probably non-inline) function.
template <typename T> RAPIDJSON_FORCEINLINE void Reserve(size_t count = 1) {
template <typename T>
RAPIDJSON_FORCEINLINE void Reserve(size_t count = 1) {
// Expand the stack if needed
if (RAPIDJSON_UNLIKELY(static_cast<std::ptrdiff_t>(sizeof(T) * count) >
(stackEnd_ - stackTop_)))
Expand<T>(count);
}
template <typename T> RAPIDJSON_FORCEINLINE T *Push(size_t count = 1) {
template <typename T>
RAPIDJSON_FORCEINLINE T *Push(size_t count = 1) {
Reserve<T>(count);
return PushUnsafe<T>(count);
}
template <typename T> RAPIDJSON_FORCEINLINE T *PushUnsafe(size_t count = 1) {
template <typename T>
RAPIDJSON_FORCEINLINE T *PushUnsafe(size_t count = 1) {
RAPIDJSON_ASSERT(stackTop_);
RAPIDJSON_ASSERT(static_cast<std::ptrdiff_t>(sizeof(T) * count) <=
(stackEnd_ - stackTop_));
@@ -130,31 +141,42 @@ public:
return ret;
}
template <typename T> T *Pop(size_t count) {
template <typename T>
T *Pop(size_t count) {
RAPIDJSON_ASSERT(GetSize() >= count * sizeof(T));
stackTop_ -= count * sizeof(T);
return reinterpret_cast<T *>(stackTop_);
}
template <typename T> T *Top() {
template <typename T>
T *Top() {
RAPIDJSON_ASSERT(GetSize() >= sizeof(T));
return reinterpret_cast<T *>(stackTop_ - sizeof(T));
}
template <typename T> const T *Top() const {
template <typename T>
const T *Top() const {
RAPIDJSON_ASSERT(GetSize() >= sizeof(T));
return reinterpret_cast<T *>(stackTop_ - sizeof(T));
}
template <typename T> T *End() { return reinterpret_cast<T *>(stackTop_); }
template <typename T> const T *End() const {
template <typename T>
T *End() {
return reinterpret_cast<T *>(stackTop_);
}
template <typename T> T *Bottom() { return reinterpret_cast<T *>(stack_); }
template <typename T>
const T *End() const {
return reinterpret_cast<T *>(stackTop_);
}
template <typename T> const T *Bottom() const {
template <typename T>
T *Bottom() {
return reinterpret_cast<T *>(stack_);
}
template <typename T>
const T *Bottom() const {
return reinterpret_cast<T *>(stack_);
}
@@ -169,28 +191,27 @@ public:
size_t GetSize() const { return static_cast<size_t>(stackTop_ - stack_); }
size_t GetCapacity() const { return static_cast<size_t>(stackEnd_ - stack_); }
private:
template <typename T> void Expand(size_t count) {
private:
template <typename T>
void Expand(size_t count) {
// Only expand the capacity if the current stack exists. Otherwise just
// create a stack with initial capacity.
size_t newCapacity;
if (stack_ == 0) {
if (!allocator_)
ownAllocator_ = allocator_ = RAPIDJSON_NEW(Allocator)();
if (!allocator_) ownAllocator_ = allocator_ = RAPIDJSON_NEW(Allocator)();
newCapacity = initialCapacity_;
} else {
newCapacity = GetCapacity();
newCapacity += (newCapacity + 1) / 2;
}
size_t newSize = GetSize() + sizeof(T) * count;
if (newCapacity < newSize)
newCapacity = newSize;
if (newCapacity < newSize) newCapacity = newSize;
Resize(newCapacity);
}
void Resize(size_t newCapacity) {
const size_t size = GetSize(); // Backup the current size
const size_t size = GetSize(); // Backup the current size
stack_ = static_cast<char *>(
allocator_->Realloc(stack_, GetCapacity(), newCapacity));
stackTop_ = stack_ + size;
@@ -199,7 +220,7 @@ private:
void Destroy() {
Allocator::Free(stack_);
RAPIDJSON_DELETE(ownAllocator_); // Only delete if it is owned by the stack
RAPIDJSON_DELETE(ownAllocator_); // Only delete if it is owned by the stack
}
// Prohibit copy constructor & assignment operator.
@@ -214,11 +235,11 @@ private:
size_t initialCapacity_;
};
} // namespace internal
} // namespace internal
RAPIDJSON_NAMESPACE_END
#if defined(__clang__)
RAPIDJSON_DIAG_POP
#endif
#endif // RAPIDJSON_STACK_H_
#endif // RAPIDJSON_STACK_H_

21
livox_ros_driver/common/rapidjson/internal/strfunc.h
View File

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

62
livox_ros_driver/common/rapidjson/internal/strtod.h
View File

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

7
livox_ros_driver/common/rapidjson/internal/swap.h
View File

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

36
livox_ros_driver/common/rapidjson/istreamwrapper.h
View File

@@ -19,17 +19,17 @@
#ifndef RAPIDJSON_ISTREAMWRAPPER_H_
#define RAPIDJSON_ISTREAMWRAPPER_H_
#include "stream.h"
#include <ios>
#include <iosfwd>
#include "stream.h"
#ifdef __clang__
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(padded)
#elif defined(_MSC_VER)
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(
4351) // new behavior: elements of array 'array' will be default initialized
RAPIDJSON_DIAG_OFF(4351) // new behavior: elements of array 'array' will be
// default initialized
#endif
RAPIDJSON_NAMESPACE_BEGIN
@@ -50,8 +50,9 @@ RAPIDJSON_NAMESPACE_BEGIN
\tparam StreamType Class derived from \c std::basic_istream.
*/
template <typename StreamType> class BasicIStreamWrapper {
public:
template <typename StreamType>
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),
current_(buffer_), readCount_(0), count_(0), eof_(false) {
: stream_(stream),
buffer_(peekBuffer_),
bufferSize_(4),
bufferLast_(0),
current_(buffer_),
readCount_(0),
count_(0),
eof_(false) {
Read();
}
@@ -71,8 +78,13 @@ public:
\param bufferSize size of buffer in bytes. Must >=4 bytes.
*/
BasicIStreamWrapper(StreamType &stream, char *buffer, size_t bufferSize)
: stream_(stream), buffer_(buffer), bufferSize_(bufferSize),
bufferLast_(0), current_(buffer_), readCount_(0), count_(0),
: stream_(stream),
buffer_(buffer),
bufferSize_(bufferSize),
bufferLast_(0),
current_(buffer_),
readCount_(0),
count_(0),
eof_(false) {
RAPIDJSON_ASSERT(bufferSize >= 4);
Read();
@@ -105,7 +117,7 @@ public:
return (current_ + 4 - !eof_ <= bufferLast_) ? current_ : 0;
}
private:
private:
BasicIStreamWrapper();
BasicIStreamWrapper(const BasicIStreamWrapper &);
BasicIStreamWrapper &operator=(const BasicIStreamWrapper &);
@@ -133,7 +145,7 @@ private:
Ch *bufferLast_;
Ch *current_;
size_t readCount_;
size_t count_; //!< Number of characters read
size_t count_; //!< Number of characters read
bool eof_;
};
@@ -146,4 +158,4 @@ RAPIDJSON_DIAG_POP
RAPIDJSON_NAMESPACE_END
#endif // RAPIDJSON_ISTREAMWRAPPER_H_
#endif // RAPIDJSON_ISTREAMWRAPPER_H_

10
livox_ros_driver/common/rapidjson/memorybuffer.h
View File

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

12
livox_ros_driver/common/rapidjson/memorystream.h
View File

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

34
livox_ros_driver/common/rapidjson/msinttypes/inttypes.h
View File

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

86
livox_ros_driver/common/rapidjson/msinttypes/stdint.h
View File

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

11
livox_ros_driver/common/rapidjson/ostreamwrapper.h
View File

@@ -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 {
public:
template <typename StreamType>
class BasicOStreamWrapper {
public:
typedef typename StreamType::char_type Ch;
BasicOStreamWrapper(StreamType &stream) : stream_(stream) {}
@@ -76,7 +77,7 @@ public:
return 0;
}
private:
private:
BasicOStreamWrapper(const BasicOStreamWrapper &);
BasicOStreamWrapper &operator=(const BasicOStreamWrapper &);
@@ -92,4 +93,4 @@ RAPIDJSON_DIAG_POP
RAPIDJSON_NAMESPACE_END
#endif // RAPIDJSON_OSTREAMWRAPPER_H_
#endif // RAPIDJSON_OSTREAMWRAPPER_H_

471
livox_ros_driver/common/rapidjson/pointer.h
View File

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

47
livox_ros_driver/common/rapidjson/prettywriter.h
View File

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

165
livox_ros_driver/common/rapidjson/rapidjson.h
View File

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

978
livox_ros_driver/common/rapidjson/reader.h
View File

File diff suppressed because it is too large Load Diff

533
livox_ros_driver/common/rapidjson/schema.h
View File

File diff suppressed because it is too large Load Diff

26
livox_ros_driver/common/rapidjson/stream.h
View File

@@ -75,7 +75,8 @@ next character. Ch Take();
configuration. See TEST(Reader, CustomStringStream) in readertest.cpp for
example.
*/
template <typename Stream> struct StreamTraits {
template <typename Stream>
struct StreamTraits {
//! Whether to make local copy of stream for optimization during parsing.
/*!
By default, for safety, streams do not use local copy optimization.
@@ -102,8 +103,7 @@ inline void PutUnsafe(Stream &stream, typename Stream::Ch c) {
template <typename Stream, typename Ch>
inline void PutN(Stream &stream, Ch c, size_t n) {
PutReserve(stream, n);
for (size_t i = 0; i < n; i++)
PutUnsafe(stream, c);
for (size_t i = 0; i < n; i++) PutUnsafe(stream, c);
}
///////////////////////////////////////////////////////////////////////////////
@@ -117,13 +117,13 @@ inline void PutN(Stream &stream, Ch c, size_t n) {
#if defined(_MSC_VER) && _MSC_VER <= 1800
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(4702) // unreachable code
RAPIDJSON_DIAG_OFF(4512) // assignment operator could not be generated
RAPIDJSON_DIAG_OFF(4702) // unreachable code
RAPIDJSON_DIAG_OFF(4512) // assignment operator could not be generated
#endif
template <typename InputStream, typename Encoding = UTF8<>>
class GenericStreamWrapper {
public:
public:
typedef typename Encoding::Ch Ch;
GenericStreamWrapper(InputStream &is) : is_(is) {}
@@ -142,7 +142,7 @@ public:
UTFType GetType() const { return is_.GetType(); }
bool HasBOM() const { return is_.HasBOM(); }
protected:
protected:
InputStream &is_;
};
@@ -156,7 +156,8 @@ RAPIDJSON_DIAG_POP
//! Read-only string stream.
/*! \note implements Stream concept
*/
template <typename Encoding> struct GenericStringStream {
template <typename Encoding>
struct GenericStringStream {
typedef typename Encoding::Ch Ch;
GenericStringStream(const Ch *src) : src_(src), head_(src) {}
@@ -176,8 +177,8 @@ template <typename Encoding> struct GenericStringStream {
return 0;
}
const Ch *src_; //!< Current read position.
const Ch *head_; //!< Original head of the string.
const Ch *src_; //!< Current read position.
const Ch *head_; //!< Original head of the string.
};
template <typename Encoding>
@@ -195,7 +196,8 @@ typedef GenericStringStream<UTF8<>> StringStream;
/*! This string stream is particularly designed for in-situ parsing.
\note implements Stream concept
*/
template <typename Encoding> struct GenericInsituStringStream {
template <typename Encoding>
struct GenericInsituStringStream {
typedef typename Encoding::Ch Ch;
GenericInsituStringStream(Ch *src) : src_(src), dst_(0), head_(src) {}
@@ -237,4 +239,4 @@ typedef GenericInsituStringStream<UTF8<>> InsituStringStream;
RAPIDJSON_NAMESPACE_END
#endif // RAPIDJSON_STREAM_H_
#endif // RAPIDJSON_STREAM_H_

11
livox_ros_driver/common/rapidjson/stringbuffer.h
View File

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

146
livox_ros_driver/common/rapidjson/writer.h
View File

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

1246
livox_ros_driver/common/rapidxml/rapidxml.hpp
View File

File diff suppressed because it is too large Load Diff

18
livox_ros_driver/common/rapidxml/rapidxml_iterators.hpp
View File

@@ -11,9 +11,9 @@
namespace rapidxml {
//! Iterator of child nodes of xml_node
template <class Ch> class node_iterator {
public:
template <class Ch>
class node_iterator {
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 {
public:
template <class Ch>
class attribute_iterator {
public:
typedef typename xml_attribute<Ch> value_type;
typedef typename xml_attribute<Ch> &reference;
typedef typename xml_attribute<Ch> *pointer;
@@ -123,10 +123,10 @@ public:
return m_attribute != rhs.m_attribute;
}
private:
private:
xml_attribute<Ch> *m_attribute;
};
} // namespace rapidxml
} // namespace rapidxml
#endif

201
livox_ros_driver/common/rapidxml/rapidxml_print.hpp
View File

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

23
livox_ros_driver/common/rapidxml/rapidxml_utils.hpp
View File

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

12
livox_ros_driver/config/livox_hub_config.json
View File

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

4
livox_ros_driver/config/livox_lidar_config.json
View File

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

8
livox_ros_driver/livox_ros_driver/include/livox_ros_driver.h
View File

@@ -26,15 +26,15 @@
#define LIVOX_ROS_DRIVER_INClUDE_LIVOX_ROS_DRIVER_H_
#define LIVOX_ROS_DRIVER_VER_MAJOR 2
#define LIVOX_ROS_DRIVER_VER_MINOR 5
#define LIVOX_ROS_DRIVER_VER_MINOR 6
#define LIVOX_ROS_DRIVER_VER_PATCH 0
#define GET_STRING(n) GET_STRING_DIRECT(n)
#define GET_STRING_DIRECT(n) #n
#define LIVOX_ROS_DRIVER_VERSION_STRING \
GET_STRING(LIVOX_ROS_DRIVER_VER_MAJOR) \
"." GET_STRING(LIVOX_ROS_DRIVER_VER_MINOR) "." GET_STRING( \
#define LIVOX_ROS_DRIVER_VERSION_STRING \
GET_STRING(LIVOX_ROS_DRIVER_VER_MAJOR) \
"." GET_STRING(LIVOX_ROS_DRIVER_VER_MINOR) "." GET_STRING( \
LIVOX_ROS_DRIVER_VER_PATCH)
#endif

332
livox_ros_driver/livox_ros_driver/lddc.cpp
View File

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

23
livox_ros_driver/livox_ros_driver/lddc.h
View File

@@ -29,9 +29,14 @@
#include <ros/ros.h>
#include <rosbag/bag.h>
#include <pcl_ros/point_cloud.h>
#include <livox_ros_driver/CustomMsg.h>
#include <livox_ros_driver/CustomPoint.h>
namespace livox_ros {
typedef pcl::PointCloud<pcl::PointXYZI> PointCloud;
/** Lidar data distribute control */
typedef enum {
kPointCloud2Msg = 0,
@@ -40,8 +45,8 @@ typedef enum {
} TransferType;
class Lddc {
public:
Lddc(int format, int multi_topic, int data_src, int output_type, double frq, \
public:
Lddc(int format, int multi_topic, int data_src, int output_type, double frq,
std::string &frame_id);
~Lddc();
@@ -59,9 +64,10 @@ public:
Lds *lds_;
private:
private:
int32_t GetPublishStartTime(LidarDevice *lidar, LidarDataQueue *queue,
uint64_t *start_time, StoragePacket *storage_packet);
uint64_t *start_time,
StoragePacket *storage_packet);
uint32_t PublishPointcloud2(LidarDataQueue *queue, uint32_t packet_num,
uint8_t handle);
uint32_t PublishPointcloudData(LidarDataQueue *queue, uint32_t packet_num,
@@ -75,7 +81,12 @@ private:
ros::Publisher *GetCurrentImuPublisher(uint8_t handle);
void PollingLidarPointCloudData(uint8_t handle, LidarDevice *lidar);
void PollingLidarImuData(uint8_t handle, LidarDevice *lidar);
void InitPointcloud2MsgHeader(sensor_msgs::PointCloud2& cloud);
void FillPointsToPclMsg(PointCloud::Ptr& pcl_msg, \
LivoxPointXyzrtl* src_point, uint32_t num);
void FillPointsToCustomMsg(livox_ros_driver::CustomMsg& livox_msg, \
LivoxPointXyzrtl* src_point, uint32_t num, uint32_t offset_time, \
uint32_t point_interval, uint32_t echo_num);
uint8_t transfer_format_;
uint8_t use_multi_topic_;
uint8_t data_src_;
@@ -92,5 +103,5 @@ private:
rosbag::Bag *bag_;
};
} // namespace livox_ros
} // namespace livox_ros
#endif

8
livox_ros_driver/livox_ros_driver/ldq.cpp
View File

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

4
livox_ros_driver/livox_ros_driver/ldq.h
View File

@@ -71,7 +71,7 @@ inline static uint32_t RoundupPowerOf2(uint32_t size) {
int InitQueue(LidarDataQueue *queue, uint32_t queue_size);
int DeInitQueue(LidarDataQueue *queue);
void ResetQueue(LidarDataQueue *queue);
void QueueProPop(LidarDataQueue *queue, StoragePacket *storage_packet);
void QueuePrePop(LidarDataQueue *queue, StoragePacket *storage_packet);
void QueuePopUpdate(LidarDataQueue *queue);
uint32_t QueuePop(LidarDataQueue *queue, StoragePacket *storage_packet);
uint32_t QueueUsedSize(LidarDataQueue *queue);
@@ -82,5 +82,5 @@ uint32_t QueuePush(LidarDataQueue *queue, StoragePacket *storage_packet);
uint32_t QueuePushAny(LidarDataQueue *queue, uint8_t *data, uint32_t length,
uint64_t time_rcv, uint32_t point_num);
} // namespace livox_ros
} // namespace livox_ros
#endif

365
livox_ros_driver/livox_ros_driver/lds.cpp
View File

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

216
livox_ros_driver/livox_ros_driver/lds.h
View File

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

96
livox_ros_driver/livox_ros_driver/lds_hub.cpp
View File

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

20
livox_ros_driver/livox_ros_driver/lds_hub.h
View File

@@ -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
HubSetPointCloudReturnModeCb(livox_status status, uint8_t handle,
HubSetPointCloudReturnModeResponse *response,
void *clent_data);
static void HubSetPointCloudReturnModeCb(
livox_status status, uint8_t handle,
HubSetPointCloudReturnModeResponse *response, void *clent_data);
static void SetCoordinateCb(livox_status status, uint8_t handle,
uint8_t response, void *clent_data);
static void
HubSetImuRatePushFrequencyCb(livox_status status, uint8_t handle,
HubSetImuPushFrequencyResponse *response,
void *clent_data);
static void HubSetImuRatePushFrequencyCb(
livox_status status, uint8_t handle,
HubSetImuPushFrequencyResponse *response, void *clent_data);
static void HubErrorStatusCb(livox_status status, uint8_t handle,
ErrorMessage *message);
static void ConfigPointCloudReturnMode(LdsHub *lds_hub);
@@ -118,5 +116,5 @@ private:
UserRawConfig hub_raw_config_;
};
} // namespace livox_ros
} // namespace livox_ros
#endif

115
livox_ros_driver/livox_ros_driver/lds_lidar.cpp
View File

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

18
livox_ros_driver/livox_ros_driver/lds_lidar.h
View File

@@ -28,8 +28,8 @@
#define LIVOX_ROS_DRIVER_LDS_LIDAR_H_
#include <memory>
#include <vector>
#include <mutex>
#include <vector>
#include "lds.h"
#include "livox_sdk.h"
@@ -42,7 +42,7 @@ namespace livox_ros {
* LiDAR data source, data from dependent lidar.
*/
class LdsLidar : public Lds {
public:
public:
static LdsLidar *GetInstance(uint32_t interval_ms) {
static LdsLidar lds_lidar(interval_ms);
return &lds_lidar;
@@ -52,7 +52,7 @@ public:
const char *user_config_path);
int DeInitLdsLidar(void);
private:
private:
LdsLidar(uint32_t interval_ms);
LdsLidar(const LdsLidar &) = delete;
~LdsLidar();
@@ -84,12 +84,12 @@ private:
uint8_t response, void *client_data);
static void ReceiveSyncTimeCallback(const char *rmc, uint32_t rmc_length,
void *client_data);
static void
GetLidarExtrinsicParameterCb(livox_status status, uint8_t handle,
LidarGetExtrinsicParameterResponse *response,
void *clent_data);
static void GetLidarExtrinsicParameterCb(
livox_status status, uint8_t handle,
LidarGetExtrinsicParameterResponse *response, void *clent_data);
static void SetHighSensitivityCb(livox_status status, uint8_t handle,
DeviceParameterResponse *response, void *clent_data);
DeviceParameterResponse *response,
void *clent_data);
void ResetLdsLidar(void);
int AddBroadcastCodeToWhitelist(const char *broadcast_code);
@@ -116,5 +116,5 @@ private:
std::mutex config_mutex_;
};
} // namespace livox_ros
} // namespace livox_ros
#endif

128
livox_ros_driver/livox_ros_driver/lds_lvx.cpp
View File

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

8
livox_ros_driver/livox_ros_driver/lds_lvx.h
View File

@@ -39,7 +39,7 @@ namespace livox_ros {
* Lidar data source abstract.
*/
class LdsLvx : public Lds {
public:
public:
static LdsLvx *GetInstance(uint32_t interval_ms) {
static LdsLvx lds_lvx(interval_ms);
return &lds_lvx;
@@ -49,7 +49,7 @@ public:
int DeInitLdsLvx(void);
void PrepareExit(void);
private:
private:
LdsLvx(uint32_t interval_ms);
LdsLvx(const LdsLvx &) = delete;
~LdsLvx();
@@ -60,8 +60,6 @@ private:
bool IsStarted() { return start_read_lvx_; }
void ReadLvxFile();
bool IsAllQueueEmpty();
bool IsAllQueueReadStop();
volatile bool is_initialized_;
OutPacketBuffer packets_of_frame_;
@@ -70,5 +68,5 @@ private:
volatile bool start_read_lvx_;
};
} // namespace livox_ros
} // namespace livox_ros
#endif

13
livox_ros_driver/livox_ros_driver/livox_ros_driver.cpp
View File

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

18
livox_ros_driver/livox_ros_driver/lvx_file.cpp
View File

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

12
livox_ros_driver/livox_ros_driver/lvx_file.h
View File

@@ -24,13 +24,13 @@
#ifndef LIVOX_FILE_H_
#define LIVOX_FILE_H_
#include "livox_sdk.h"
#include <fstream>
#include <ios>
#include <list>
#include <memory>
#include <mutex>
#include <vector>
#include "livox_sdk.h"
namespace livox_ros {
@@ -121,9 +121,7 @@ typedef struct {
LvxFilePacket *packet;
} LvxFileFrame;
typedef struct {
uint8_t device_count;
} LvxFilePrivateHeaderV0;
typedef struct { uint8_t device_count; } LvxFilePrivateHeaderV0;
typedef struct {
uint8_t lidar_broadcast_code[16];
@@ -172,7 +170,7 @@ typedef struct {
#pragma pack()
class LvxFileHandle {
public:
public:
LvxFileHandle();
~LvxFileHandle() = default;
@@ -196,7 +194,7 @@ public:
int GetLvxFileReadProgress();
int GetFileVersion() { return file_ver_; }
private:
private:
std::fstream lvx_file_;
std::vector<LvxFileDeviceInfo> device_info_list_;
uint8_t file_ver_;
@@ -232,5 +230,5 @@ private:
}
};
} // namespace livox_ros
} // namespace livox_ros
#endif

16
livox_ros_driver/timesync/timesync.cpp
View File

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

8
livox_ros_driver/timesync/timesync.h
View File

@@ -25,10 +25,10 @@
#ifndef TIMESYNC_TIMESYNC_H_
#define TIMESYNC_TIMESYNC_H_
#include <thread>
#include "comm_device.h"
#include "comm_protocol.h"
#include "user_uart.h"
#include <thread>
namespace livox_ros {
@@ -43,7 +43,7 @@ typedef struct {
} TimeSyncConfig;
class TimeSync {
public:
public:
static TimeSync *GetInstance() {
static TimeSync time_sync;
@@ -67,7 +67,7 @@ public:
}
}
private:
private:
TimeSync();
~TimeSync();
TimeSync(const TimeSync &) = delete;
@@ -101,5 +101,5 @@ private:
void FsmCheckDevState();
};
} // namespace livox_ros
} // namespace livox_ros
#endif

69
livox_ros_driver/timesync/user_uart/user_uart.cpp
View File

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

7
livox_ros_driver/timesync/user_uart/user_uart.h
View File

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