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

Merge pull request #19 from Livox-SDK/draft_pr

Browse Source 
Draft pr
This commit is contained in:
livox
2020-03-20 16:03:16 +08:00
committed by GitHub
parent a4b1afc024 e1ae882b39
commit 1bd671effb
74 changed files with 18658 additions and 16496 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
livox_ros_driver/CMakeLists.txt
View File

@@ -143,7 +143,7 @@ add_executable(${PROJECT_NAME}_node
# precompile macro and compile option
#---------------------------------------------------------------------------------------
target_compile_options(${PROJECT_NAME}_node
PRIVATE $<$<CXX_COMPILER_ID:GNU>:-Wall -Werror>
PRIVATE $<$<CXX_COMPILER_ID:GNU>:-Wall>
)
#---------------------------------------------------------------------------------------

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

@@ -22,10 +22,9 @@
* SOFTWARE.
*/
// Teensy 3.0, Teensy 3.1:
// See K20P64M72SF1RM.pdf (Kinetis), Pages 638 - 641 for documentation of CRC Device
// See KINETIS_4N30D.pdf for Errata (Errata ID 2776)
// See K20P64M72SF1RM.pdf (Kinetis), Pages 638 - 641 for documentation of CRC
// Device See KINETIS_4N30D.pdf for Errata (Errata ID 2776)
//
// So, ALL HW-calculations are done as 32 bit.
//
@@ -47,7 +46,6 @@
#include <stdint.h>
// ================= 16-BIT CRC ===================
class FastCRC16 {
@@ -55,11 +53,13 @@ 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:
uint16_t seed_;
};
// ================= 32-BIT CRC ===================
@@ -69,11 +69,12 @@ 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
uint32_t crc32_calc(
const uint8_t *data,
uint16_t len); // Call for subsequent calculations with previous seed
private:
uint32_t seed_;
};
#endif // FASTCRC_FASTCRC_H_

354
livox_ros_driver/common/FastCRC/FastCRC_tables.hpp
View File

@@ -30,7 +30,6 @@
// modify from FastCRC library @ 2018/11/20
//
#ifndef FASTCRC_FASTCRC_TABLES_H_
#define FASTCRC_FASTCRC_TABLES_H_
@@ -38,204 +37,165 @@
// crc16 table
const uint16_t crc_table_mcrf4xx[1024] = {
0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf,
0x8c48, 0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, 0xe97e, 0xf8f7,
0x1081, 0x0108, 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e,
0x9cc9, 0x8d40, 0xbfdb, 0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876,
0x2102, 0x308b, 0x0210, 0x1399, 0x6726, 0x76af, 0x4434, 0x55bd,
0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e, 0xfae7, 0xc87c, 0xd9f5,
0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e, 0x54b5, 0x453c,
0xbdcb, 0xac42, 0x9ed9, 0x8f50, 0xfbef, 0xea66, 0xd8fd, 0xc974,
0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb,
0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3,
0x5285, 0x430c, 0x7197, 0x601e, 0x14a1, 0x0528, 0x37b3, 0x263a,
0xdecd, 0xcf44, 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72,
0x6306, 0x728f, 0x4014, 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9,
0xef4e, 0xfec7, 0xcc5c, 0xddd5, 0xa96a, 0xb8e3, 0x8a78, 0x9bf1,
0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, 0x242a, 0x16b1, 0x0738,
0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, 0x9af9, 0x8b70,
0x8408, 0x9581, 0xa71a, 0xb693, 0xc22c, 0xd3a5, 0xe13e, 0xf0b7,
0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff,
0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036,
0x18c1, 0x0948, 0x3bd3, 0x2a5a, 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e,
0xa50a, 0xb483, 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5,
0x2942, 0x38cb, 0x0a50, 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd,
0xb58b, 0xa402, 0x9699, 0x8710, 0xf3af, 0xe226, 0xd0bd, 0xc134,
0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, 0x6e6e, 0x5cf5, 0x4d7c,
0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1, 0xa33a, 0xb2b3,
0x4a44, 0x5bcd, 0x6956, 0x78df, 0x0c60, 0x1de9, 0x2f72, 0x3efb,
0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232,
0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a,
0xe70e, 0xf687, 0xc41c, 0xd595, 0xa12a, 0xb0a3, 0x8238, 0x93b1,
0x6b46, 0x7acf, 0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9,
0xf78f, 0xe606, 0xd49d, 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330,
0x7bc7, 0x6a4e, 0x58d5, 0x495c, 0x3de3, 0x2c6a, 0x1ef1, 0x0f78,
0x0000, 0x19d8, 0x33b0, 0x2a68, 0x6760, 0x7eb8, 0x54d0, 0x4d08,
0xcec0, 0xd718, 0xfd70, 0xe4a8, 0xa9a0, 0xb078, 0x9a10, 0x83c8,
0x9591, 0x8c49, 0xa621, 0xbff9, 0xf2f1, 0xeb29, 0xc141, 0xd899,
0x5b51, 0x4289, 0x68e1, 0x7139, 0x3c31, 0x25e9, 0x0f81, 0x1659,
0x2333, 0x3aeb, 0x1083, 0x095b, 0x4453, 0x5d8b, 0x77e3, 0x6e3b,
0xedf3, 0xf42b, 0xde43, 0xc79b, 0x8a93, 0x934b, 0xb923, 0xa0fb,
0xb6a2, 0xaf7a, 0x8512, 0x9cca, 0xd1c2, 0xc81a, 0xe272, 0xfbaa,
0x7862, 0x61ba, 0x4bd2, 0x520a, 0x1f02, 0x06da, 0x2cb2, 0x356a,
0x4666, 0x5fbe, 0x75d6, 0x6c0e, 0x2106, 0x38de, 0x12b6, 0x0b6e,
0x88a6, 0x917e, 0xbb16, 0xa2ce, 0xefc6, 0xf61e, 0xdc76, 0xc5ae,
0xd3f7, 0xca2f, 0xe047, 0xf99f, 0xb497, 0xad4f, 0x8727, 0x9eff,
0x1d37, 0x04ef, 0x2e87, 0x375f, 0x7a57, 0x638f, 0x49e7, 0x503f,
0x6555, 0x7c8d, 0x56e5, 0x4f3d, 0x0235, 0x1bed, 0x3185, 0x285d,
0xab95, 0xb24d, 0x9825, 0x81fd, 0xccf5, 0xd52d, 0xff45, 0xe69d,
0xf0c4, 0xe91c, 0xc374, 0xdaac, 0x97a4, 0x8e7c, 0xa414, 0xbdcc,
0x3e04, 0x27dc, 0x0db4, 0x146c, 0x5964, 0x40bc, 0x6ad4, 0x730c,
0x8ccc, 0x9514, 0xbf7c, 0xa6a4, 0xebac, 0xf274, 0xd81c, 0xc1c4,
0x420c, 0x5bd4, 0x71bc, 0x6864, 0x256c, 0x3cb4, 0x16dc, 0x0f04,
0x195d, 0x0085, 0x2aed, 0x3335, 0x7e3d, 0x67e5, 0x4d8d, 0x5455,
0xd79d, 0xce45, 0xe42d, 0xfdf5, 0xb0fd, 0xa925, 0x834d, 0x9a95,
0xafff, 0xb627, 0x9c4f, 0x8597, 0xc89f, 0xd147, 0xfb2f, 0xe2f7,
0x613f, 0x78e7, 0x528f, 0x4b57, 0x065f, 0x1f87, 0x35ef, 0x2c37,
0x3a6e, 0x23b6, 0x09de, 0x1006, 0x5d0e, 0x44d6, 0x6ebe, 0x7766,
0xf4ae, 0xed76, 0xc71e, 0xdec6, 0x93ce, 0x8a16, 0xa07e, 0xb9a6,
0xcaaa, 0xd372, 0xf91a, 0xe0c2, 0xadca, 0xb412, 0x9e7a, 0x87a2,
0x046a, 0x1db2, 0x37da, 0x2e02, 0x630a, 0x7ad2, 0x50ba, 0x4962,
0x5f3b, 0x46e3, 0x6c8b, 0x7553, 0x385b, 0x2183, 0x0beb, 0x1233,
0x91fb, 0x8823, 0xa24b, 0xbb93, 0xf69b, 0xef43, 0xc52b, 0xdcf3,
0xe999, 0xf041, 0xda29, 0xc3f1, 0x8ef9, 0x9721, 0xbd49, 0xa491,
0x2759, 0x3e81, 0x14e9, 0x0d31, 0x4039, 0x59e1, 0x7389, 0x6a51,
0x7c08, 0x65d0, 0x4fb8, 0x5660, 0x1b68, 0x02b0, 0x28d8, 0x3100,
0xb2c8, 0xab10, 0x8178, 0x98a0, 0xd5a8, 0xcc70, 0xe618, 0xffc0,
0x0000, 0x5adc, 0xb5b8, 0xef64, 0x6361, 0x39bd, 0xd6d9, 0x8c05,
0xc6c2, 0x9c1e, 0x737a, 0x29a6, 0xa5a3, 0xff7f, 0x101b, 0x4ac7,
0x8595, 0xdf49, 0x302d, 0x6af1, 0xe6f4, 0xbc28, 0x534c, 0x0990,
0x4357, 0x198b, 0xf6ef, 0xac33, 0x2036, 0x7aea, 0x958e, 0xcf52,
0x033b, 0x59e7, 0xb683, 0xec5f, 0x605a, 0x3a86, 0xd5e2, 0x8f3e,
0xc5f9, 0x9f25, 0x7041, 0x2a9d, 0xa698, 0xfc44, 0x1320, 0x49fc,
0x86ae, 0xdc72, 0x3316, 0x69ca, 0xe5cf, 0xbf13, 0x5077, 0x0aab,
0x406c, 0x1ab0, 0xf5d4, 0xaf08, 0x230d, 0x79d1, 0x96b5, 0xcc69,
0x0676, 0x5caa, 0xb3ce, 0xe912, 0x6517, 0x3fcb, 0xd0af, 0x8a73,
0xc0b4, 0x9a68, 0x750c, 0x2fd0, 0xa3d5, 0xf909, 0x166d, 0x4cb1,
0x83e3, 0xd93f, 0x365b, 0x6c87, 0xe082, 0xba5e, 0x553a, 0x0fe6,
0x4521, 0x1ffd, 0xf099, 0xaa45, 0x2640, 0x7c9c, 0x93f8, 0xc924,
0x054d, 0x5f91, 0xb0f5, 0xea29, 0x662c, 0x3cf0, 0xd394, 0x8948,
0xc38f, 0x9953, 0x7637, 0x2ceb, 0xa0ee, 0xfa32, 0x1556, 0x4f8a,
0x80d8, 0xda04, 0x3560, 0x6fbc, 0xe3b9, 0xb965, 0x5601, 0x0cdd,
0x461a, 0x1cc6, 0xf3a2, 0xa97e, 0x257b, 0x7fa7, 0x90c3, 0xca1f,
0x0cec, 0x5630, 0xb954, 0xe388, 0x6f8d, 0x3551, 0xda35, 0x80e9,
0xca2e, 0x90f2, 0x7f96, 0x254a, 0xa94f, 0xf393, 0x1cf7, 0x462b,
0x8979, 0xd3a5, 0x3cc1, 0x661d, 0xea18, 0xb0c4, 0x5fa0, 0x057c,
0x4fbb, 0x1567, 0xfa03, 0xa0df, 0x2cda, 0x7606, 0x9962, 0xc3be,
0x0fd7, 0x550b, 0xba6f, 0xe0b3, 0x6cb6, 0x366a, 0xd90e, 0x83d2,
0xc915, 0x93c9, 0x7cad, 0x2671, 0xaa74, 0xf0a8, 0x1fcc, 0x4510,
0x8a42, 0xd09e, 0x3ffa, 0x6526, 0xe923, 0xb3ff, 0x5c9b, 0x0647,
0x4c80, 0x165c, 0xf938, 0xa3e4, 0x2fe1, 0x753d, 0x9a59, 0xc085,
0x0a9a, 0x5046, 0xbf22, 0xe5fe, 0x69fb, 0x3327, 0xdc43, 0x869f,
0xcc58, 0x9684, 0x79e0, 0x233c, 0xaf39, 0xf5e5, 0x1a81, 0x405d,
0x8f0f, 0xd5d3, 0x3ab7, 0x606b, 0xec6e, 0xb6b2, 0x59d6, 0x030a,
0x49cd, 0x1311, 0xfc75, 0xa6a9, 0x2aac, 0x7070, 0x9f14, 0xc5c8,
0x09a1, 0x537d, 0xbc19, 0xe6c5, 0x6ac0, 0x301c, 0xdf78, 0x85a4,
0xcf63, 0x95bf, 0x7adb, 0x2007, 0xac02, 0xf6de, 0x19ba, 0x4366,
0x8c34, 0xd6e8, 0x398c, 0x6350, 0xef55, 0xb589, 0x5aed, 0x0031,
0x4af6, 0x102a, 0xff4e, 0xa592, 0x2997, 0x734b, 0x9c2f, 0xc6f3,
0x0000, 0x1cbb, 0x3976, 0x25cd, 0x72ec, 0x6e57, 0x4b9a, 0x5721,
0xe5d8, 0xf963, 0xdcae, 0xc015, 0x9734, 0x8b8f, 0xae42, 0xb2f9,
0xc3a1, 0xdf1a, 0xfad7, 0xe66c, 0xb14d, 0xadf6, 0x883b, 0x9480,
0x2679, 0x3ac2, 0x1f0f, 0x03b4, 0x5495, 0x482e, 0x6de3, 0x7158,
0x8f53, 0x93e8, 0xb625, 0xaa9e, 0xfdbf, 0xe104, 0xc4c9, 0xd872,
0x6a8b, 0x7630, 0x53fd, 0x4f46, 0x1867, 0x04dc, 0x2111, 0x3daa,
0x4cf2, 0x5049, 0x7584, 0x693f, 0x3e1e, 0x22a5, 0x0768, 0x1bd3,
0xa92a, 0xb591, 0x905c, 0x8ce7, 0xdbc6, 0xc77d, 0xe2b0, 0xfe0b,
0x16b7, 0x0a0c, 0x2fc1, 0x337a, 0x645b, 0x78e0, 0x5d2d, 0x4196,
0xf36f, 0xefd4, 0xca19, 0xd6a2, 0x8183, 0x9d38, 0xb8f5, 0xa44e,
0xd516, 0xc9ad, 0xec60, 0xf0db, 0xa7fa, 0xbb41, 0x9e8c, 0x8237,
0x30ce, 0x2c75, 0x09b8, 0x1503, 0x4222, 0x5e99, 0x7b54, 0x67ef,
0x99e4, 0x855f, 0xa092, 0xbc29, 0xeb08, 0xf7b3, 0xd27e, 0xcec5,
0x7c3c, 0x6087, 0x454a, 0x59f1, 0x0ed0, 0x126b, 0x37a6, 0x2b1d,
0x5a45, 0x46fe, 0x6333, 0x7f88, 0x28a9, 0x3412, 0x11df, 0x0d64,
0xbf9d, 0xa326, 0x86eb, 0x9a50, 0xcd71, 0xd1ca, 0xf407, 0xe8bc,
0x2d6e, 0x31d5, 0x1418, 0x08a3, 0x5f82, 0x4339, 0x66f4, 0x7a4f,
0xc8b6, 0xd40d, 0xf1c0, 0xed7b, 0xba5a, 0xa6e1, 0x832c, 0x9f97,
0xeecf, 0xf274, 0xd7b9, 0xcb02, 0x9c23, 0x8098, 0xa555, 0xb9ee,
0x0b17, 0x17ac, 0x3261, 0x2eda, 0x79fb, 0x6540, 0x408d, 0x5c36,
0xa23d, 0xbe86, 0x9b4b, 0x87f0, 0xd0d1, 0xcc6a, 0xe9a7, 0xf51c,
0x47e5, 0x5b5e, 0x7e93, 0x6228, 0x3509, 0x29b2, 0x0c7f, 0x10c4,
0x619c, 0x7d27, 0x58ea, 0x4451, 0x1370, 0x0fcb, 0x2a06, 0x36bd,
0x8444, 0x98ff, 0xbd32, 0xa189, 0xf6a8, 0xea13, 0xcfde, 0xd365,
0x3bd9, 0x2762, 0x02af, 0x1e14, 0x4935, 0x558e, 0x7043, 0x6cf8,
0xde01, 0xc2ba, 0xe777, 0xfbcc, 0xaced, 0xb056, 0x959b, 0x8920,
0xf878, 0xe4c3, 0xc10e, 0xddb5, 0x8a94, 0x962f, 0xb3e2, 0xaf59,
0x1da0, 0x011b, 0x24d6, 0x386d, 0x6f4c, 0x73f7, 0x563a, 0x4a81,
0xb48a, 0xa831, 0x8dfc, 0x9147, 0xc666, 0xdadd, 0xff10, 0xe3ab,
0x5152, 0x4de9, 0x6824, 0x749f, 0x23be, 0x3f05, 0x1ac8, 0x0673,
0x772b, 0x6b90, 0x4e5d, 0x52e6, 0x05c7, 0x197c, 0x3cb1, 0x200a,
0x92f3, 0x8e48, 0xab85, 0xb73e, 0xe01f, 0xfca4, 0xd969, 0xc5d2
};
0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, 0x8c48,
0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, 0xe97e, 0xf8f7, 0x1081, 0x0108,
0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e, 0x9cc9, 0x8d40, 0xbfdb,
0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876, 0x2102, 0x308b, 0x0210, 0x1399,
0x6726, 0x76af, 0x4434, 0x55bd, 0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e,
0xfae7, 0xc87c, 0xd9f5, 0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e,
0x54b5, 0x453c, 0xbdcb, 0xac42, 0x9ed9, 0x8f50, 0xfbef, 0xea66, 0xd8fd,
0xc974, 0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb,
0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3, 0x5285,
0x430c, 0x7197, 0x601e, 0x14a1, 0x0528, 0x37b3, 0x263a, 0xdecd, 0xcf44,
0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72, 0x6306, 0x728f, 0x4014,
0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9, 0xef4e, 0xfec7, 0xcc5c, 0xddd5,
0xa96a, 0xb8e3, 0x8a78, 0x9bf1, 0x7387, 0x620e, 0x5095, 0x411c, 0x35a3,
0x242a, 0x16b1, 0x0738, 0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862,
0x9af9, 0x8b70, 0x8408, 0x9581, 0xa71a, 0xb693, 0xc22c, 0xd3a5, 0xe13e,
0xf0b7, 0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff,
0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036, 0x18c1,
0x0948, 0x3bd3, 0x2a5a, 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e, 0xa50a, 0xb483,
0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5, 0x2942, 0x38cb, 0x0a50,
0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd, 0xb58b, 0xa402, 0x9699, 0x8710,
0xf3af, 0xe226, 0xd0bd, 0xc134, 0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7,
0x6e6e, 0x5cf5, 0x4d7c, 0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1,
0xa33a, 0xb2b3, 0x4a44, 0x5bcd, 0x6956, 0x78df, 0x0c60, 0x1de9, 0x2f72,
0x3efb, 0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232,
0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a, 0xe70e,
0xf687, 0xc41c, 0xd595, 0xa12a, 0xb0a3, 0x8238, 0x93b1, 0x6b46, 0x7acf,
0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9, 0xf78f, 0xe606, 0xd49d,
0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330, 0x7bc7, 0x6a4e, 0x58d5, 0x495c,
0x3de3, 0x2c6a, 0x1ef1, 0x0f78, 0x0000, 0x19d8, 0x33b0, 0x2a68, 0x6760,
0x7eb8, 0x54d0, 0x4d08, 0xcec0, 0xd718, 0xfd70, 0xe4a8, 0xa9a0, 0xb078,
0x9a10, 0x83c8, 0x9591, 0x8c49, 0xa621, 0xbff9, 0xf2f1, 0xeb29, 0xc141,
0xd899, 0x5b51, 0x4289, 0x68e1, 0x7139, 0x3c31, 0x25e9, 0x0f81, 0x1659,
0x2333, 0x3aeb, 0x1083, 0x095b, 0x4453, 0x5d8b, 0x77e3, 0x6e3b, 0xedf3,
0xf42b, 0xde43, 0xc79b, 0x8a93, 0x934b, 0xb923, 0xa0fb, 0xb6a2, 0xaf7a,
0x8512, 0x9cca, 0xd1c2, 0xc81a, 0xe272, 0xfbaa, 0x7862, 0x61ba, 0x4bd2,
0x520a, 0x1f02, 0x06da, 0x2cb2, 0x356a, 0x4666, 0x5fbe, 0x75d6, 0x6c0e,
0x2106, 0x38de, 0x12b6, 0x0b6e, 0x88a6, 0x917e, 0xbb16, 0xa2ce, 0xefc6,
0xf61e, 0xdc76, 0xc5ae, 0xd3f7, 0xca2f, 0xe047, 0xf99f, 0xb497, 0xad4f,
0x8727, 0x9eff, 0x1d37, 0x04ef, 0x2e87, 0x375f, 0x7a57, 0x638f, 0x49e7,
0x503f, 0x6555, 0x7c8d, 0x56e5, 0x4f3d, 0x0235, 0x1bed, 0x3185, 0x285d,
0xab95, 0xb24d, 0x9825, 0x81fd, 0xccf5, 0xd52d, 0xff45, 0xe69d, 0xf0c4,
0xe91c, 0xc374, 0xdaac, 0x97a4, 0x8e7c, 0xa414, 0xbdcc, 0x3e04, 0x27dc,
0x0db4, 0x146c, 0x5964, 0x40bc, 0x6ad4, 0x730c, 0x8ccc, 0x9514, 0xbf7c,
0xa6a4, 0xebac, 0xf274, 0xd81c, 0xc1c4, 0x420c, 0x5bd4, 0x71bc, 0x6864,
0x256c, 0x3cb4, 0x16dc, 0x0f04, 0x195d, 0x0085, 0x2aed, 0x3335, 0x7e3d,
0x67e5, 0x4d8d, 0x5455, 0xd79d, 0xce45, 0xe42d, 0xfdf5, 0xb0fd, 0xa925,
0x834d, 0x9a95, 0xafff, 0xb627, 0x9c4f, 0x8597, 0xc89f, 0xd147, 0xfb2f,
0xe2f7, 0x613f, 0x78e7, 0x528f, 0x4b57, 0x065f, 0x1f87, 0x35ef, 0x2c37,
0x3a6e, 0x23b6, 0x09de, 0x1006, 0x5d0e, 0x44d6, 0x6ebe, 0x7766, 0xf4ae,
0xed76, 0xc71e, 0xdec6, 0x93ce, 0x8a16, 0xa07e, 0xb9a6, 0xcaaa, 0xd372,
0xf91a, 0xe0c2, 0xadca, 0xb412, 0x9e7a, 0x87a2, 0x046a, 0x1db2, 0x37da,
0x2e02, 0x630a, 0x7ad2, 0x50ba, 0x4962, 0x5f3b, 0x46e3, 0x6c8b, 0x7553,
0x385b, 0x2183, 0x0beb, 0x1233, 0x91fb, 0x8823, 0xa24b, 0xbb93, 0xf69b,
0xef43, 0xc52b, 0xdcf3, 0xe999, 0xf041, 0xda29, 0xc3f1, 0x8ef9, 0x9721,
0xbd49, 0xa491, 0x2759, 0x3e81, 0x14e9, 0x0d31, 0x4039, 0x59e1, 0x7389,
0x6a51, 0x7c08, 0x65d0, 0x4fb8, 0x5660, 0x1b68, 0x02b0, 0x28d8, 0x3100,
0xb2c8, 0xab10, 0x8178, 0x98a0, 0xd5a8, 0xcc70, 0xe618, 0xffc0, 0x0000,
0x5adc, 0xb5b8, 0xef64, 0x6361, 0x39bd, 0xd6d9, 0x8c05, 0xc6c2, 0x9c1e,
0x737a, 0x29a6, 0xa5a3, 0xff7f, 0x101b, 0x4ac7, 0x8595, 0xdf49, 0x302d,
0x6af1, 0xe6f4, 0xbc28, 0x534c, 0x0990, 0x4357, 0x198b, 0xf6ef, 0xac33,
0x2036, 0x7aea, 0x958e, 0xcf52, 0x033b, 0x59e7, 0xb683, 0xec5f, 0x605a,
0x3a86, 0xd5e2, 0x8f3e, 0xc5f9, 0x9f25, 0x7041, 0x2a9d, 0xa698, 0xfc44,
0x1320, 0x49fc, 0x86ae, 0xdc72, 0x3316, 0x69ca, 0xe5cf, 0xbf13, 0x5077,
0x0aab, 0x406c, 0x1ab0, 0xf5d4, 0xaf08, 0x230d, 0x79d1, 0x96b5, 0xcc69,
0x0676, 0x5caa, 0xb3ce, 0xe912, 0x6517, 0x3fcb, 0xd0af, 0x8a73, 0xc0b4,
0x9a68, 0x750c, 0x2fd0, 0xa3d5, 0xf909, 0x166d, 0x4cb1, 0x83e3, 0xd93f,
0x365b, 0x6c87, 0xe082, 0xba5e, 0x553a, 0x0fe6, 0x4521, 0x1ffd, 0xf099,
0xaa45, 0x2640, 0x7c9c, 0x93f8, 0xc924, 0x054d, 0x5f91, 0xb0f5, 0xea29,
0x662c, 0x3cf0, 0xd394, 0x8948, 0xc38f, 0x9953, 0x7637, 0x2ceb, 0xa0ee,
0xfa32, 0x1556, 0x4f8a, 0x80d8, 0xda04, 0x3560, 0x6fbc, 0xe3b9, 0xb965,
0x5601, 0x0cdd, 0x461a, 0x1cc6, 0xf3a2, 0xa97e, 0x257b, 0x7fa7, 0x90c3,
0xca1f, 0x0cec, 0x5630, 0xb954, 0xe388, 0x6f8d, 0x3551, 0xda35, 0x80e9,
0xca2e, 0x90f2, 0x7f96, 0x254a, 0xa94f, 0xf393, 0x1cf7, 0x462b, 0x8979,
0xd3a5, 0x3cc1, 0x661d, 0xea18, 0xb0c4, 0x5fa0, 0x057c, 0x4fbb, 0x1567,
0xfa03, 0xa0df, 0x2cda, 0x7606, 0x9962, 0xc3be, 0x0fd7, 0x550b, 0xba6f,
0xe0b3, 0x6cb6, 0x366a, 0xd90e, 0x83d2, 0xc915, 0x93c9, 0x7cad, 0x2671,
0xaa74, 0xf0a8, 0x1fcc, 0x4510, 0x8a42, 0xd09e, 0x3ffa, 0x6526, 0xe923,
0xb3ff, 0x5c9b, 0x0647, 0x4c80, 0x165c, 0xf938, 0xa3e4, 0x2fe1, 0x753d,
0x9a59, 0xc085, 0x0a9a, 0x5046, 0xbf22, 0xe5fe, 0x69fb, 0x3327, 0xdc43,
0x869f, 0xcc58, 0x9684, 0x79e0, 0x233c, 0xaf39, 0xf5e5, 0x1a81, 0x405d,
0x8f0f, 0xd5d3, 0x3ab7, 0x606b, 0xec6e, 0xb6b2, 0x59d6, 0x030a, 0x49cd,
0x1311, 0xfc75, 0xa6a9, 0x2aac, 0x7070, 0x9f14, 0xc5c8, 0x09a1, 0x537d,
0xbc19, 0xe6c5, 0x6ac0, 0x301c, 0xdf78, 0x85a4, 0xcf63, 0x95bf, 0x7adb,
0x2007, 0xac02, 0xf6de, 0x19ba, 0x4366, 0x8c34, 0xd6e8, 0x398c, 0x6350,
0xef55, 0xb589, 0x5aed, 0x0031, 0x4af6, 0x102a, 0xff4e, 0xa592, 0x2997,
0x734b, 0x9c2f, 0xc6f3, 0x0000, 0x1cbb, 0x3976, 0x25cd, 0x72ec, 0x6e57,
0x4b9a, 0x5721, 0xe5d8, 0xf963, 0xdcae, 0xc015, 0x9734, 0x8b8f, 0xae42,
0xb2f9, 0xc3a1, 0xdf1a, 0xfad7, 0xe66c, 0xb14d, 0xadf6, 0x883b, 0x9480,
0x2679, 0x3ac2, 0x1f0f, 0x03b4, 0x5495, 0x482e, 0x6de3, 0x7158, 0x8f53,
0x93e8, 0xb625, 0xaa9e, 0xfdbf, 0xe104, 0xc4c9, 0xd872, 0x6a8b, 0x7630,
0x53fd, 0x4f46, 0x1867, 0x04dc, 0x2111, 0x3daa, 0x4cf2, 0x5049, 0x7584,
0x693f, 0x3e1e, 0x22a5, 0x0768, 0x1bd3, 0xa92a, 0xb591, 0x905c, 0x8ce7,
0xdbc6, 0xc77d, 0xe2b0, 0xfe0b, 0x16b7, 0x0a0c, 0x2fc1, 0x337a, 0x645b,
0x78e0, 0x5d2d, 0x4196, 0xf36f, 0xefd4, 0xca19, 0xd6a2, 0x8183, 0x9d38,
0xb8f5, 0xa44e, 0xd516, 0xc9ad, 0xec60, 0xf0db, 0xa7fa, 0xbb41, 0x9e8c,
0x8237, 0x30ce, 0x2c75, 0x09b8, 0x1503, 0x4222, 0x5e99, 0x7b54, 0x67ef,
0x99e4, 0x855f, 0xa092, 0xbc29, 0xeb08, 0xf7b3, 0xd27e, 0xcec5, 0x7c3c,
0x6087, 0x454a, 0x59f1, 0x0ed0, 0x126b, 0x37a6, 0x2b1d, 0x5a45, 0x46fe,
0x6333, 0x7f88, 0x28a9, 0x3412, 0x11df, 0x0d64, 0xbf9d, 0xa326, 0x86eb,
0x9a50, 0xcd71, 0xd1ca, 0xf407, 0xe8bc, 0x2d6e, 0x31d5, 0x1418, 0x08a3,
0x5f82, 0x4339, 0x66f4, 0x7a4f, 0xc8b6, 0xd40d, 0xf1c0, 0xed7b, 0xba5a,
0xa6e1, 0x832c, 0x9f97, 0xeecf, 0xf274, 0xd7b9, 0xcb02, 0x9c23, 0x8098,
0xa555, 0xb9ee, 0x0b17, 0x17ac, 0x3261, 0x2eda, 0x79fb, 0x6540, 0x408d,
0x5c36, 0xa23d, 0xbe86, 0x9b4b, 0x87f0, 0xd0d1, 0xcc6a, 0xe9a7, 0xf51c,
0x47e5, 0x5b5e, 0x7e93, 0x6228, 0x3509, 0x29b2, 0x0c7f, 0x10c4, 0x619c,
0x7d27, 0x58ea, 0x4451, 0x1370, 0x0fcb, 0x2a06, 0x36bd, 0x8444, 0x98ff,
0xbd32, 0xa189, 0xf6a8, 0xea13, 0xcfde, 0xd365, 0x3bd9, 0x2762, 0x02af,
0x1e14, 0x4935, 0x558e, 0x7043, 0x6cf8, 0xde01, 0xc2ba, 0xe777, 0xfbcc,
0xaced, 0xb056, 0x959b, 0x8920, 0xf878, 0xe4c3, 0xc10e, 0xddb5, 0x8a94,
0x962f, 0xb3e2, 0xaf59, 0x1da0, 0x011b, 0x24d6, 0x386d, 0x6f4c, 0x73f7,
0x563a, 0x4a81, 0xb48a, 0xa831, 0x8dfc, 0x9147, 0xc666, 0xdadd, 0xff10,
0xe3ab, 0x5152, 0x4de9, 0x6824, 0x749f, 0x23be, 0x3f05, 0x1ac8, 0x0673,
0x772b, 0x6b90, 0x4e5d, 0x52e6, 0x05c7, 0x197c, 0x3cb1, 0x200a, 0x92f3,
0x8e48, 0xab85, 0xb73e, 0xe01f, 0xfca4, 0xd969, 0xc5d2};
// crc32 table
const uint32_t crc_table_crc32[256] = {
0x00000000, 0x77073096, 0xee0e612c, 0x990951ba,
0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3,
0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988,
0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91,
0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de,
0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7,
0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec,
0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5,
0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172,
0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b,
0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940,
0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59,
0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116,
0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f,
0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924,
0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d,
0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a,
0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433,
0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818,
0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01,
0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e,
0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457,
0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c,
0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65,
0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2,
0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb,
0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0,
0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9,
0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086,
0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f,
0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4,
0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad,
0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a,
0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683,
0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8,
0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1,
0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe,
0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7,
0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc,
0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5,
0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252,
0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b,
0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60,
0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79,
0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236,
0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f,
0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04,
0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d,
0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a,
0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713,
0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38,
0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21,
0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e,
0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777,
0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c,
0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45,
0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2,
0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db,
0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0,
0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9,
0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6,
0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf,
0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94,
0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d
};
0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f,
0xe963a535, 0x9e6495a3, 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988,
0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, 0x1db71064, 0x6ab020f2,
0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7,
0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9,
0xfa0f3d63, 0x8d080df5, 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172,
0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, 0x35b5a8fa, 0x42b2986c,
0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59,
0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423,
0xcfba9599, 0xb8bda50f, 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924,
0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, 0x76dc4190, 0x01db7106,
0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433,
0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d,
0x91646c97, 0xe6635c01, 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e,
0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, 0x65b0d9c6, 0x12b7e950,
0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65,
0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7,
0xa4d1c46d, 0xd3d6f4fb, 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0,
0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, 0x5005713c, 0x270241aa,
0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f,
0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81,
0xb7bd5c3b, 0xc0ba6cad, 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a,
0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, 0xe3630b12, 0x94643b84,
0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1,
0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb,
0x196c3671, 0x6e6b06e7, 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc,
0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, 0xd6d6a3e8, 0xa1d1937e,
0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b,
0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55,
0x316e8eef, 0x4669be79, 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236,
0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, 0xc5ba3bbe, 0xb2bd0b28,
0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d,
0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f,
0x72076785, 0x05005713, 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38,
0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, 0x86d3d2d4, 0xf1d4e242,
0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777,
0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69,
0x616bffd3, 0x166ccf45, 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2,
0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, 0xaed16a4a, 0xd9d65adc,
0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9,
0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693,
0x54de5729, 0x23d967bf, 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94,
0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d};
#endif

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

@@ -37,19 +37,15 @@
#include "FastCRC.h"
#include "FastCRC_tables.hpp"
// ================= 16-BIT CRC ===================
/** Constructor
*/
FastCRC16::FastCRC16(uint16_t seed) {
seed_ = seed;
}
FastCRC16::FastCRC16(uint16_t seed) { seed_ = seed; }
#define crc_n4(crc, data, table) crc ^= data; \
crc = (table[(crc & 0xff) + 0x300]) ^ \
(table[((crc >> 8) & 0xff) + 0x200]) ^ \
(table[((data >> 16) & 0xff) + 0x100]) ^ \
(table[data >> 24]);
#define crc_n4(crc, data, table) \
crc ^= data; \
crc = (table[(crc & 0xff) + 0x300]) ^ (table[((crc >> 8) & 0xff) + 0x200]) ^ \
(table[((data >> 16) & 0xff) + 0x100]) ^ (table[data >> 24]);
/** MCRF4XX
* equivalent to _crc_ccitt_update() in crc16.h from avr_libc
@@ -84,21 +80,18 @@ uint16_t FastCRC16::mcrf4xx_calc(const uint8_t *data, uint16_t len) {
return crc;
}
// ================= 32-BIT CRC ===================
/** Constructor
*/
FastCRC32::FastCRC32(uint32_t seed) {
seed_ = seed;
}
FastCRC32::FastCRC32(uint32_t seed) { seed_ = seed; }
#define crc_n4d(crc, data, table) crc ^= data; \
crc = (table[(crc & 0xff) + 0x300]) ^ \
(table[((crc >> 8) & 0xff) + 0x200]) ^ \
(table[((crc >> 16) & 0xff) + 0x100]) ^ \
(table[(crc >> 24) & 0xff]);
#define crc_n4d(crc, data, table) \
crc ^= data; \
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; \
#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]); \
@@ -150,5 +143,3 @@ uint32_t FastCRC32::crc32_calc(const uint8_t *data, uint16_t len) {
return crc;
}

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

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

28
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 {
@@ -89,7 +89,8 @@ uint32_t CommProtocol::GetValidDataSize() {
}
void CommProtocol::UpdateCache(void) {
if (GetCacheTailSize() < kMoveCacheLimit) { /* move unused data to cache head */
if (GetCacheTailSize() <
kMoveCacheLimit) { /* move unused data to cache head */
uint32_t valid_data_size = GetValidDataSize();
if (valid_data_size) {
@@ -103,18 +104,16 @@ void CommProtocol::UpdateCache(void) {
}
}
int32_t CommProtocol::Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len, \
int32_t CommProtocol::Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len,
const CommPacket &i_packet) {
return protocol_->Pack(o_buf, o_buf_size, o_len, i_packet);
}
void CommProtocol::ResetParser() {
fsm_parse_step_ = kSearchPacketPreamble;
}
void CommProtocol::ResetParser() { fsm_parse_step_ = kSearchPacketPreamble; }
int32_t CommProtocol::ParseCommStream(CommPacket *o_pack) {
int32_t ret = kParseFail;
while ((GetValidDataSize() > protocol_->GetPreambleLen()) && \
while ((GetValidDataSize() > protocol_->GetPreambleLen()) &&
(GetValidDataSize() > offset_to_read_index_)) {
switch (fsm_parse_step_) {
case kSearchPacketPreamble: {
@@ -129,12 +128,11 @@ int32_t CommProtocol::ParseCommStream(CommPacket *o_pack) {
ret = FsmGetPacketData(o_pack);
break;
}
default: {
FsmParserStateTransfer(kSearchPacketPreamble);
}
default: { FsmParserStateTransfer(kSearchPacketPreamble); }
}
if (ret == kParseNeedMoreData) break; /* */
if (ret == kParseNeedMoreData)
break; /* */
}
return ret;
@@ -158,8 +156,10 @@ int32_t CommProtocol::FsmSearchPacketPreamble() {
break;
} else {
packet_length_ = protocol_->GetPacketLen(GetCacheReadPos());
if ((packet_length_ < cache_.size) && \
(packet_length_ > protocol_->GetPacketWrapperLen())) { /* check the legality of length */
if ((packet_length_ < cache_.size) &&
(packet_length_ >
protocol_
->GetPacketWrapperLen())) { /* check the legality of length */
FsmParserStateTransfer(kGetPacketData);
break;
}
@@ -221,4 +221,4 @@ int32_t CommProtocol::FsmGetPacketData(CommPacket *o_pack) {
return ret;
}
} // namespace livox
} // namespace livox_ros

9
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 "gps_protocol.h"
#include <stdint.h>
namespace livox_ros {
const uint32_t kCacheSize = 8192;
@@ -54,7 +54,8 @@ class CommProtocol {
CommProtocol(ProtocolConfig &config);
~CommProtocol();
int32_t Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len, const CommPacket &i_packet);
int32_t Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len,
const CommPacket &i_packet);
int32_t ParseCommStream(CommPacket *o_pack);
@@ -100,5 +101,5 @@ class CommProtocol {
}
};
} // namespace livox
} // namespace livox_ros
#endif // COMM_COMM_PROTOCOL_H_

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

@@ -36,18 +36,16 @@ const uint32_t kPacketLengthLmit = 200;
const uint32_t kPreambleLen = 1;
const uint32_t kWrapperLen = 4; /** '$' + '*' + '2 checksum byte' */
GpsProtocol::GpsProtocol() { found_length_ = 0; }
GpsProtocol::GpsProtocol() {
found_length_ = 0;
}
int32_t GpsProtocol::Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len, \
int32_t GpsProtocol::Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len,
const CommPacket &i_packet) {
// GpsPacket* gps_packet = (GpsPacket*)o_buf;
return 0;
}
int32_t GpsProtocol::ParsePacket(const uint8_t *i_buf, uint32_t i_len, CommPacket *o_packet) {
int32_t GpsProtocol::ParsePacket(const uint8_t *i_buf, uint32_t i_len,
CommPacket *o_packet) {
// GpsPacket *gps_packet = (GpsPacket *)i_buf;
if (i_len < GetPacketWrapperLen()) {
@@ -61,9 +59,7 @@ int32_t GpsProtocol::ParsePacket(const uint8_t *i_buf, uint32_t i_len, CommPacke
return 0;
}
uint32_t GpsProtocol::GetPreambleLen() {
return kPreambleLen; /** '$' */
}
uint32_t GpsProtocol::GetPreambleLen() { return kPreambleLen; /** '$' */ }
uint32_t GpsProtocol::GetPacketWrapperLen() {
return kWrapperLen; /** '$' + '*' + '2 checksum bytes' */
@@ -84,9 +80,7 @@ uint32_t GpsProtocol::FindPacketLen(const uint8_t *buf, uint32_t buf_length) {
}
}
uint32_t GpsProtocol::GetPacketLen(const uint8_t *buf) {
return found_length_;
}
uint32_t GpsProtocol::GetPacketLen(const uint8_t *buf) { return found_length_; }
int32_t GpsProtocol::CheckPreamble(const uint8_t *buf) {
GpsPreamble *preamble = (GpsPreamble *)buf;
@@ -99,7 +93,8 @@ int32_t GpsProtocol::CheckPreamble(const uint8_t *buf) {
}
int32_t GpsProtocol::CheckPacket(const uint8_t *buf) {
uint8_t checksum = CalcGpsPacketChecksum(&buf[1], found_length_ - kWrapperLen);
uint8_t checksum =
CalcGpsPacketChecksum(&buf[1], found_length_ - kWrapperLen);
uint8_t raw_checksum = AscciiToHex(&buf[found_length_ - 2]);
if (checksum == raw_checksum) {
return 0;
@@ -108,7 +103,8 @@ int32_t GpsProtocol::CheckPacket(const uint8_t *buf) {
}
}
uint8_t GpsProtocol::CalcGpsPacketChecksum(const uint8_t *buf, uint32_t length) {
uint8_t GpsProtocol::CalcGpsPacketChecksum(const uint8_t *buf,
uint32_t length) {
uint8_t result = buf[0];
for (uint32_t i = 1; i < length; i++) {
result ^= buf[i];
@@ -116,15 +112,16 @@ uint8_t GpsProtocol::CalcGpsPacketChecksum(const uint8_t *buf, uint32_t length)
return result;
}
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
} // 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 <stdint.h>
#include "protocol.h"
#include <stdint.h>
namespace livox_ros {
@@ -51,9 +51,10 @@ class GpsProtocol : public Protocol {
GpsProtocol();
~GpsProtocol() = default;
int32_t ParsePacket(const uint8_t *i_buf, uint32_t i_len, CommPacket *o_packet) override;
int32_t ParsePacket(const uint8_t *i_buf, uint32_t i_len,
CommPacket *o_packet) override;
int32_t Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len, \
int32_t Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len,
const CommPacket &i_packet) override;
uint32_t GetPreambleLen() override;
@@ -72,8 +73,7 @@ class GpsProtocol : public Protocol {
uint32_t found_length_;
uint8_t CalcGpsPacketChecksum(const uint8_t *buf, uint32_t length);
};
} // namespace livox
} // namespace livox_ros
#endif // LIVOX_GPS_PROTOCOL_H_

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

@@ -40,7 +40,11 @@ typedef enum { kNoNeed, kNeedAck, kDelayAck } NeedAckType;
typedef enum { kParseSuccess, kParseFail, kParseNeedMoreData } ParseResult;
typedef enum { kFindLengthSuccess, kFindLengthContinue, kFindLengthError } FindLengthResult;
typedef enum {
kFindLengthSuccess,
kFindLengthContinue,
kFindLengthError
} FindLengthResult;
typedef struct CommPacket {
uint8_t packet_type;
@@ -84,9 +88,10 @@ class Protocol {
public:
virtual ~Protocol() = default;
virtual int32_t ParsePacket(const uint8_t *i_buf, uint32_t i_len, CommPacket *o_packet) = 0;
virtual int32_t ParsePacket(const uint8_t *i_buf, uint32_t i_len,
CommPacket *o_packet) = 0;
virtual int32_t Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len, \
virtual int32_t Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len,
const CommPacket &i_packet) = 0;
virtual uint32_t GetPreambleLen() = 0;
@@ -102,5 +107,5 @@ class Protocol {
virtual int32_t CheckPacket(const uint8_t *buf) = 0;
};
} // namespace livox
} // namespace livox_ros
#endif // COMM_PROTOCOL_H_

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

@@ -32,9 +32,10 @@ const uint8_t kSdkProtocolSof = 0xAA;
const uint32_t kSdkPacketCrcSize = 4; // crc32
const uint32_t kSdkPacketPreambleCrcSize = 2; // crc16
SdkProtocol::SdkProtocol(uint16_t seed16, uint32_t seed32) : crc16_(seed16), crc32_(seed32) {}
SdkProtocol::SdkProtocol(uint16_t seed16, uint32_t seed32)
: crc16_(seed16), crc32_(seed32) {}
int32_t SdkProtocol::Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len,\
int32_t SdkProtocol::Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len,
const CommPacket &i_packet) {
SdkPacket *sdk_packet = (SdkPacket *)o_buf;
@@ -52,14 +53,15 @@ int32_t SdkProtocol::Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len,\
sdk_packet->version = kSdkVer0;
sdk_packet->packet_type = i_packet.packet_type;
sdk_packet->seq_num = i_packet.seq_num & 0xFFFF;
sdk_packet->preamble_crc = crc16_.mcrf4xx_calc(o_buf, GetPreambleLen() - \
kSdkPacketPreambleCrcSize);
sdk_packet->preamble_crc =
crc16_.mcrf4xx_calc(o_buf, GetPreambleLen() - kSdkPacketPreambleCrcSize);
sdk_packet->cmd_set = i_packet.cmd_set;
sdk_packet->cmd_id = i_packet.cmd_code;
memcpy(sdk_packet->data, i_packet.data, i_packet.data_len);
uint32_t crc = crc32_.crc32_calc(o_buf, sdk_packet->length - kSdkPacketCrcSize);
uint32_t crc =
crc32_.crc32_calc(o_buf, sdk_packet->length - kSdkPacketCrcSize);
o_buf[sdk_packet->length - 4] = crc & 0xFF;
o_buf[sdk_packet->length - 3] = (crc >> 8) & 0xFF;
o_buf[sdk_packet->length - 2] = (crc >> 16) & 0xFF;
@@ -70,7 +72,8 @@ int32_t SdkProtocol::Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len,\
return 0;
}
int32_t SdkProtocol::ParsePacket(const uint8_t *i_buf, uint32_t i_len, CommPacket *o_packet) {
int32_t SdkProtocol::ParsePacket(const uint8_t *i_buf, uint32_t i_len,
CommPacket *o_packet) {
SdkPacket *sdk_packet = (SdkPacket *)i_buf;
if (i_len < GetPacketWrapperLen()) {
@@ -92,9 +95,7 @@ int32_t SdkProtocol::ParsePacket(const uint8_t *i_buf, uint32_t i_len, CommPacke
return 0;
}
uint32_t SdkProtocol::GetPreambleLen() {
return sizeof(SdkPreamble);
}
uint32_t SdkProtocol::GetPreambleLen() { return sizeof(SdkPreamble); }
uint32_t SdkProtocol::GetPacketWrapperLen() {
return sizeof(SdkPacket) - 1 + kSdkPacketCrcSize;
@@ -108,7 +109,8 @@ uint32_t SdkProtocol::GetPacketLen(const uint8_t *buf) {
int32_t SdkProtocol::CheckPreamble(const uint8_t *buf) {
SdkPreamble *preamble = (SdkPreamble *)buf;
if ((preamble->sof == kSdkProtocolSof) && (crc16_.mcrf4xx_calc(buf, GetPreambleLen()) == 0)) {
if ((preamble->sof == kSdkProtocolSof) &&
(crc16_.mcrf4xx_calc(buf, GetPreambleLen()) == 0)) {
return 0;
} else {
return -1;
@@ -118,9 +120,9 @@ int32_t SdkProtocol::CheckPreamble(const uint8_t *buf) {
int32_t SdkProtocol::CheckPacket(const uint8_t *buf) {
SdkPacket *sdk_packet = (SdkPacket *)buf;
uint32_t crc = ((uint32_t)(buf[sdk_packet->length - 4])) | \
(((uint32_t)(buf[sdk_packet->length - 3])) << 8) | \
(((uint32_t)(buf[sdk_packet->length - 2])) << 16) | \
uint32_t crc = ((uint32_t)(buf[sdk_packet->length - 4])) |
(((uint32_t)(buf[sdk_packet->length - 3])) << 8) |
(((uint32_t)(buf[sdk_packet->length - 2])) << 16) |
(((uint32_t)(buf[sdk_packet->length - 1])) << 24);
if (crc32_.crc32_calc(buf, sdk_packet->length - kSdkPacketCrcSize) == crc) {
@@ -129,4 +131,4 @@ int32_t SdkProtocol::CheckPacket(const uint8_t *buf) {
return -1;
}
}
}// namespace livox
} // namespace livox_ros

11
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 "protocol.h"
#include "FastCRC/FastCRC.h"
#include "protocol.h"
#include <stdint.h>
namespace livox_ros {
typedef enum { kSdkVerNone, kSdkVer0, kSdkVer1 } SdkVersion;
@@ -62,9 +62,10 @@ class SdkProtocol : public Protocol {
SdkProtocol(uint16_t seed16, uint32_t seed32);
~SdkProtocol() = default;
int32_t ParsePacket(const uint8_t *i_buf, uint32_t i_len, CommPacket *o_packet) override;
int32_t ParsePacket(const uint8_t *i_buf, uint32_t i_len,
CommPacket *o_packet) override;
int32_t Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len, \
int32_t Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len,
const CommPacket &i_packet) override;
uint32_t GetPreambleLen() override;
@@ -81,5 +82,5 @@ class SdkProtocol : public Protocol {
FastCRC16 crc16_;
FastCRC32 crc32_;
};
} // namespace livox
} // namespace livox_ros
#endif // LIVOX_SDK_PROTOCOL_H_

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_ALLOCATORS_H_
#define RAPIDJSON_ALLOCATORS_H_
@@ -32,7 +36,8 @@ RAPIDJSON_NAMESPACE_BEGIN
\code
concept Allocator {
static const bool kNeedFree; //!< Whether this allocator needs to call Free().
static const bool kNeedFree; //!< Whether this allocator needs to call
Free().
// Allocate a memory block.
// \param size of the memory block in bytes.
@@ -40,8 +45,10 @@ concept Allocator {
void* Malloc(size_t size);
// Resize a memory block.
// \param originalPtr The pointer to current memory block. Null pointer is permitted.
// \param originalSize The current size in bytes. (Design issue: since some allocator may not book-keep this, explicitly pass to it can save memory.)
// \param originalPtr The pointer to current memory block. Null pointer is
permitted.
// \param originalSize The current size in bytes. (Design issue: since some
allocator may not book-keep this, explicitly pass to it can save memory.)
// \param newSize the new size in bytes.
void* Realloc(void* originalPtr, size_t originalSize, size_t newSize);
@@ -52,7 +59,6 @@ concept Allocator {
\endcode
*/
/*! \def RAPIDJSON_ALLOCATOR_DEFAULT_CHUNK_CAPACITY
\ingroup RAPIDJSON_CONFIG
\brief User-defined kDefaultChunkCapacity definition.
@@ -64,7 +70,6 @@ concept Allocator {
#define RAPIDJSON_ALLOCATOR_DEFAULT_CHUNK_CAPACITY (64 * 1024)
#endif
///////////////////////////////////////////////////////////////////////////////
// CrtAllocator
@@ -100,44 +105,52 @@ public:
It does not free memory blocks. And Realloc() only allocate new memory.
The memory chunks are allocated by BaseAllocator, which is CrtAllocator by default.
The memory chunks are allocated by BaseAllocator, which is CrtAllocator by
default.
User may also supply a buffer as the first chunk.
If the user-buffer is full then additional chunks are allocated by BaseAllocator.
If the user-buffer is full then additional chunks are allocated by
BaseAllocator.
The user-buffer is not deallocated by this allocator.
\tparam BaseAllocator the allocator type for allocating memory chunks. Default is CrtAllocator.
\note implements Allocator concept
\tparam BaseAllocator the allocator type for allocating memory chunks.
Default is CrtAllocator. \note implements Allocator concept
*/
template <typename BaseAllocator = CrtAllocator>
class MemoryPoolAllocator {
template <typename BaseAllocator = CrtAllocator> class MemoryPoolAllocator {
public:
static const bool kNeedFree = false; //!< Tell users that no need to call Free() with this allocator. (concept Allocator)
static const bool kNeedFree =
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 kDefaultChunkSize.
\param baseAllocator The allocator for allocating memory chunks.
/*! \param chunkSize The size of memory chunk. The default is
kDefaultChunkSize. \param baseAllocator The allocator for allocating memory
chunks.
*/
MemoryPoolAllocator(size_t chunkSize = kDefaultChunkCapacity, BaseAllocator* baseAllocator = 0) :
chunkHead_(0), chunk_capacity_(chunkSize), userBuffer_(0), baseAllocator_(baseAllocator), ownBaseAllocator_(0)
{
}
MemoryPoolAllocator(size_t chunkSize = kDefaultChunkCapacity,
BaseAllocator *baseAllocator = 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 allocates new chunk with chunk size.
/*! The user buffer will be used firstly. When it is full, memory pool
allocates new chunk with chunk size.
The user buffer will not be deallocated when this allocator is destructed.
\param buffer User supplied buffer.
\param size Size of the buffer in bytes. It must at least larger than sizeof(ChunkHeader).
\param chunkSize The size of memory chunk. The default is kDefaultChunkSize.
\param baseAllocator The allocator for allocating memory chunks.
\param size Size of the buffer in bytes. It must at least larger than
sizeof(ChunkHeader). \param chunkSize The size of memory chunk. The default
is kDefaultChunkSize. \param baseAllocator The allocator for allocating
memory chunks.
*/
MemoryPoolAllocator(void *buffer, size_t size, size_t chunkSize = kDefaultChunkCapacity, BaseAllocator* baseAllocator = 0) :
chunkHead_(0), chunk_capacity_(chunkSize), userBuffer_(buffer), baseAllocator_(baseAllocator), ownBaseAllocator_(0)
{
MemoryPoolAllocator(void *buffer, size_t size,
size_t chunkSize = kDefaultChunkCapacity,
BaseAllocator *baseAllocator = 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);
@@ -195,7 +208,8 @@ public:
if (!AddChunk(chunk_capacity_ > size ? chunk_capacity_ : size))
return NULL;
void *buffer = reinterpret_cast<char *>(chunkHead_) + RAPIDJSON_ALIGN(sizeof(ChunkHeader)) + chunkHead_->size;
void *buffer = reinterpret_cast<char *>(chunkHead_) +
RAPIDJSON_ALIGN(sizeof(ChunkHeader)) + chunkHead_->size;
chunkHead_->size += size;
return buffer;
}
@@ -215,8 +229,11 @@ public:
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) {
// 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) {
size_t increment = static_cast<size_t>(newSize - originalSize);
if (chunkHead_->size + increment <= chunkHead_->capacity) {
chunkHead_->size += increment;
@@ -229,8 +246,7 @@ public:
if (originalSize)
std::memcpy(newBuffer, originalPtr, originalSize);
return newBuffer;
}
else
} else
return NULL;
}
@@ -250,32 +266,38 @@ private:
bool AddChunk(size_t capacity) {
if (!baseAllocator_)
ownBaseAllocator_ = baseAllocator_ = RAPIDJSON_NEW(BaseAllocator)();
if (ChunkHeader* chunk = reinterpret_cast<ChunkHeader*>(baseAllocator_->Malloc(RAPIDJSON_ALIGN(sizeof(ChunkHeader)) + capacity))) {
if (ChunkHeader *chunk =
reinterpret_cast<ChunkHeader *>(baseAllocator_->Malloc(
RAPIDJSON_ALIGN(sizeof(ChunkHeader)) + capacity))) {
chunk->capacity = capacity;
chunk->size = 0;
chunk->next = chunkHead_;
chunkHead_ = chunk;
return true;
}
else
} else
return false;
}
static const int kDefaultChunkCapacity = RAPIDJSON_ALLOCATOR_DEFAULT_CHUNK_CAPACITY; //!< Default chunk capacity.
static const int kDefaultChunkCapacity =
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 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.
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
*baseAllocator_; //!< base allocator for allocating memory chunks.
BaseAllocator *ownBaseAllocator_; //!< base allocator created by this object.
};

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_CURSORSTREAMWRAPPER_H_
#define RAPIDJSON_CURSORSTREAMWRAPPER_H_
@@ -30,7 +34,6 @@ RAPIDJSON_DIAG_OFF(4512) // assignment operator could not be generated
RAPIDJSON_NAMESPACE_BEGIN
//! Cursor stream wrapper for counting line and column number if error exists.
/*!
\tparam InputStream Any stream that implements Stream Concept
@@ -40,8 +43,8 @@ class CursorStreamWrapper : public GenericStreamWrapper<InputStream, Encoding> {
public:
typedef typename Encoding::Ch Ch;
CursorStreamWrapper(InputStream& is):
GenericStreamWrapper<InputStream, Encoding>(is), line_(1), col_(0) {}
CursorStreamWrapper(InputStream &is)
: GenericStreamWrapper<InputStream, Encoding>(is), line_(1), col_(0) {}
// counting line and column number
Ch Take() {

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

File diff suppressed because it is too large Load Diff

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

@@ -1,22 +1,26 @@
// Tencent is pleased to support the open source community by making RapidJSON available.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_ENCODEDSTREAM_H_
#define RAPIDJSON_ENCODEDSTREAM_H_
#include "stream.h"
#include "memorystream.h"
#include "stream.h"
#ifdef __GNUC__
RAPIDJSON_DIAG_PUSH
@@ -32,12 +36,14 @@ RAPIDJSON_NAMESPACE_BEGIN
//! Input byte stream wrapper with a statically bound encoding.
/*!
\tparam Encoding The interpretation of encoding of the stream. Either UTF8, UTF16LE, UTF16BE, UTF32LE, UTF32BE.
\tparam InputByteStream Type of input byte stream. For example, FileReadStream.
\tparam Encoding The interpretation of encoding of the stream. Either UTF8,
UTF16LE, UTF16BE, UTF32LE, UTF32BE. \tparam InputByteStream Type of input
byte stream. For example, FileReadStream.
*/
template <typename Encoding, typename InputByteStream>
class EncodedInputStream {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
public:
typedef typename Encoding::Ch Ch;
@@ -46,14 +52,24 @@ public:
}
Ch Peek() const { return current_; }
Ch Take() { Ch c = current_; current_ = Encoding::Take(is_); return c; }
Ch Take() {
Ch c = current_;
current_ = Encoding::Take(is_);
return c;
}
size_t Tell() const { return is_.Tell(); }
// Not implemented
void Put(Ch) { RAPIDJSON_ASSERT(false); }
void Flush() { RAPIDJSON_ASSERT(false); }
Ch* PutBegin() { RAPIDJSON_ASSERT(false); return 0; }
size_t PutEnd(Ch*) { RAPIDJSON_ASSERT(false); return 0; }
Ch *PutBegin() {
RAPIDJSON_ASSERT(false);
return 0;
}
size_t PutEnd(Ch *) {
RAPIDJSON_ASSERT(false);
return 0;
}
private:
EncodedInputStream(const EncodedInputStream &);
@@ -64,15 +80,17 @@ private:
};
//! Specialized for UTF8 MemoryStream.
template <>
class EncodedInputStream<UTF8<>, MemoryStream> {
template <> class EncodedInputStream<UTF8<>, MemoryStream> {
public:
typedef UTF8<>::Ch Ch;
EncodedInputStream(MemoryStream &is) : is_(is) {
if (static_cast<unsigned char>(is_.Peek()) == 0xEFu) is_.Take();
if (static_cast<unsigned char>(is_.Peek()) == 0xBBu) is_.Take();
if (static_cast<unsigned char>(is_.Peek()) == 0xBFu) is_.Take();
if (static_cast<unsigned char>(is_.Peek()) == 0xEFu)
is_.Take();
if (static_cast<unsigned char>(is_.Peek()) == 0xBBu)
is_.Take();
if (static_cast<unsigned char>(is_.Peek()) == 0xBFu)
is_.Take();
}
Ch Peek() const { return is_.Peek(); }
Ch Take() { return is_.Take(); }
@@ -93,12 +111,14 @@ private:
//! Output byte stream wrapper with statically bound encoding.
/*!
\tparam Encoding The interpretation of encoding of the stream. Either UTF8, UTF16LE, UTF16BE, UTF32LE, UTF32BE.
\tparam OutputByteStream Type of input byte stream. For example, FileWriteStream.
\tparam Encoding The interpretation of encoding of the stream. Either UTF8,
UTF16LE, UTF16BE, UTF32LE, UTF32BE. \tparam OutputByteStream Type of input
byte stream. For example, FileWriteStream.
*/
template <typename Encoding, typename OutputByteStream>
class EncodedOutputStream {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
public:
typedef typename Encoding::Ch Ch;
@@ -111,11 +131,26 @@ public:
void Flush() { os_.Flush(); }
// Not implemented
Ch Peek() const { RAPIDJSON_ASSERT(false); return 0;}
Ch Take() { RAPIDJSON_ASSERT(false); return 0;}
size_t Tell() const { RAPIDJSON_ASSERT(false); return 0; }
Ch* PutBegin() { RAPIDJSON_ASSERT(false); return 0; }
size_t PutEnd(Ch*) { RAPIDJSON_ASSERT(false); return 0; }
Ch Peek() const {
RAPIDJSON_ASSERT(false);
return 0;
}
Ch Take() {
RAPIDJSON_ASSERT(false);
return 0;
}
size_t Tell() const {
RAPIDJSON_ASSERT(false);
return 0;
}
Ch *PutBegin() {
RAPIDJSON_ASSERT(false);
return 0;
}
size_t PutEnd(Ch *) {
RAPIDJSON_ASSERT(false);
return 0;
}
private:
EncodedOutputStream(const EncodedOutputStream &);
@@ -124,9 +159,11 @@ private:
OutputByteStream &os_;
};
#define RAPIDJSON_ENCODINGS_FUNC(x) UTF8<Ch>::x, UTF16LE<Ch>::x, UTF16BE<Ch>::x, UTF32LE<Ch>::x, UTF32BE<Ch>::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 detection.
//! Input stream wrapper with dynamically bound encoding and automatic encoding
//! detection.
/*!
\tparam CharType Type of character for reading.
\tparam InputByteStream type of input byte stream to be wrapped.
@@ -134,6 +171,7 @@ private:
template <typename CharType, typename InputByteStream>
class AutoUTFInputStream {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
public:
typedef CharType Ch;
@@ -142,7 +180,8 @@ public:
\param is input stream to be wrapped.
\param type UTF encoding type if it is not detected from the stream.
*/
AutoUTFInputStream(InputByteStream& is, UTFType type = kUTF8) : is_(&is), type_(type), hasBOM_(false) {
AutoUTFInputStream(InputByteStream &is, UTFType type = kUTF8)
: is_(&is), type_(type), hasBOM_(false) {
RAPIDJSON_ASSERT(type >= kUTF8 && type <= kUTF32BE);
DetectType();
static const TakeFunc f[] = {RAPIDJSON_ENCODINGS_FUNC(Take)};
@@ -154,14 +193,24 @@ public:
bool HasBOM() const { return hasBOM_; }
Ch Peek() const { return current_; }
Ch Take() { Ch c = current_; current_ = takeFunc_(*is_); return c; }
Ch Take() {
Ch c = current_;
current_ = takeFunc_(*is_);
return c;
}
size_t Tell() const { return is_->Tell(); }
// Not implemented
void Put(Ch) { RAPIDJSON_ASSERT(false); }
void Flush() { RAPIDJSON_ASSERT(false); }
Ch* PutBegin() { RAPIDJSON_ASSERT(false); return 0; }
size_t PutEnd(Ch*) { RAPIDJSON_ASSERT(false); return 0; }
Ch *PutBegin() {
RAPIDJSON_ASSERT(false);
return 0;
}
size_t PutEnd(Ch *) {
RAPIDJSON_ASSERT(false);
return 0;
}
private:
AutoUTFInputStream(const AutoUTFInputStream &);
@@ -176,17 +225,45 @@ private:
// FF FE UTF-16LE
// EF BB BF UTF-8
const unsigned char* c = reinterpret_cast<const unsigned char *>(is_->Peek4());
const unsigned char *c =
reinterpret_cast<const unsigned char *>(is_->Peek4());
if (!c)
return;
unsigned bom = static_cast<unsigned>(c[0] | (c[1] << 8) | (c[2] << 16) | (c[3] << 24));
unsigned bom =
static_cast<unsigned>(c[0] | (c[1] << 8) | (c[2] << 16) | (c[3] << 24));
hasBOM_ = false;
if (bom == 0xFFFE0000) { type_ = kUTF32BE; hasBOM_ = true; is_->Take(); is_->Take(); is_->Take(); is_->Take(); }
else if (bom == 0x0000FEFF) { type_ = kUTF32LE; hasBOM_ = true; is_->Take(); is_->Take(); is_->Take(); is_->Take(); }
else if ((bom & 0xFFFF) == 0xFFFE) { type_ = kUTF16BE; hasBOM_ = true; is_->Take(); is_->Take(); }
else if ((bom & 0xFFFF) == 0xFEFF) { type_ = kUTF16LE; hasBOM_ = true; is_->Take(); is_->Take(); }
else if ((bom & 0xFFFFFF) == 0xBFBBEF) { type_ = kUTF8; hasBOM_ = true; is_->Take(); is_->Take(); is_->Take(); }
if (bom == 0xFFFE0000) {
type_ = kUTF32BE;
hasBOM_ = true;
is_->Take();
is_->Take();
is_->Take();
is_->Take();
} else if (bom == 0x0000FEFF) {
type_ = kUTF32LE;
hasBOM_ = true;
is_->Take();
is_->Take();
is_->Take();
is_->Take();
} else if ((bom & 0xFFFF) == 0xFFFE) {
type_ = kUTF16BE;
hasBOM_ = true;
is_->Take();
is_->Take();
} else if ((bom & 0xFFFF) == 0xFEFF) {
type_ = kUTF16LE;
hasBOM_ = true;
is_->Take();
is_->Take();
} else if ((bom & 0xFFFFFF) == 0xBFBBEF) {
type_ = kUTF8;
hasBOM_ = true;
is_->Take();
is_->Take();
is_->Take();
}
// RFC 4627: Section 3
// "Since the first two characters of a JSON text will always be ASCII
@@ -200,20 +277,35 @@ private:
// xx xx xx xx UTF-8
if (!hasBOM_) {
int pattern = (c[0] ? 1 : 0) | (c[1] ? 2 : 0) | (c[2] ? 4 : 0) | (c[3] ? 8 : 0);
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.
}
}
// Runtime check whether the size of character type is sufficient. It only perform checks with assertion.
if (type_ == kUTF16LE || type_ == kUTF16BE) RAPIDJSON_ASSERT(sizeof(Ch) >= 2);
if (type_ == kUTF32LE || type_ == kUTF32BE) RAPIDJSON_ASSERT(sizeof(Ch) >= 4);
// Runtime check whether the size of character type is sufficient. It only
// perform checks with assertion.
if (type_ == kUTF16LE || type_ == kUTF16BE)
RAPIDJSON_ASSERT(sizeof(Ch) >= 2);
if (type_ == kUTF32LE || type_ == kUTF32BE)
RAPIDJSON_ASSERT(sizeof(Ch) >= 4);
}
typedef Ch (*TakeFunc)(InputByteStream &is);
@@ -224,7 +316,8 @@ private:
bool hasBOM_;
};
//! Output stream wrapper with dynamically bound encoding and automatic encoding detection.
//! Output stream wrapper with dynamically bound encoding and automatic encoding
//! detection.
/*!
\tparam CharType Type of character for writing.
\tparam OutputByteStream type of output byte stream to be wrapped.
@@ -232,6 +325,7 @@ private:
template <typename CharType, typename OutputByteStream>
class AutoUTFOutputStream {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
public:
typedef CharType Ch;
@@ -241,12 +335,16 @@ public:
\param type UTF encoding type.
\param putBOM Whether to write BOM at the beginning of the stream.
*/
AutoUTFOutputStream(OutputByteStream& os, UTFType type, bool putBOM) : os_(&os), type_(type) {
AutoUTFOutputStream(OutputByteStream &os, UTFType type, bool putBOM)
: os_(&os), type_(type) {
RAPIDJSON_ASSERT(type >= kUTF8 && type <= kUTF32BE);
// Runtime check whether the size of character type is sufficient. It only perform checks with assertion.
if (type_ == kUTF16LE || type_ == kUTF16BE) RAPIDJSON_ASSERT(sizeof(Ch) >= 2);
if (type_ == kUTF32LE || type_ == kUTF32BE) RAPIDJSON_ASSERT(sizeof(Ch) >= 4);
// Runtime check whether the size of character type is sufficient. It only
// perform checks with assertion.
if (type_ == kUTF16LE || type_ == kUTF16BE)
RAPIDJSON_ASSERT(sizeof(Ch) >= 2);
if (type_ == kUTF32LE || type_ == kUTF32BE)
RAPIDJSON_ASSERT(sizeof(Ch) >= 4);
static const PutFunc f[] = {RAPIDJSON_ENCODINGS_FUNC(Put)};
putFunc_ = f[type_];
@@ -261,11 +359,26 @@ public:
void Flush() { os_->Flush(); }
// Not implemented
Ch Peek() const { RAPIDJSON_ASSERT(false); return 0;}
Ch Take() { RAPIDJSON_ASSERT(false); return 0;}
size_t Tell() const { RAPIDJSON_ASSERT(false); return 0; }
Ch* PutBegin() { RAPIDJSON_ASSERT(false); return 0; }
size_t PutEnd(Ch*) { RAPIDJSON_ASSERT(false); return 0; }
Ch Peek() const {
RAPIDJSON_ASSERT(false);
return 0;
}
Ch Take() {
RAPIDJSON_ASSERT(false);
return 0;
}
size_t Tell() const {
RAPIDJSON_ASSERT(false);
return 0;
}
Ch *PutBegin() {
RAPIDJSON_ASSERT(false);
return 0;
}
size_t PutEnd(Ch *) {
RAPIDJSON_ASSERT(false);
return 0;
}
private:
AutoUTFOutputStream(const AutoUTFOutputStream &);

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_ENCODINGS_H_
#define RAPIDJSON_ENCODINGS_H_
@@ -19,7 +23,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(
4244) // conversion from 'type1' to 'type2', possible loss of data
RAPIDJSON_DIAG_OFF(4702) // unreachable code
#elif defined(__GNUC__)
RAPIDJSON_DIAG_PUSH
@@ -37,15 +42,16 @@ RAPIDJSON_NAMESPACE_BEGIN
\code
concept Encoding {
typename Ch; //! Type of character. A "character" is actually a code unit in unicode's definition.
typename Ch; //! Type of character. A "character" is actually a code unit
in unicode's definition.
enum { supportUnicode = 1 }; // or 0 if not supporting unicode
//! \brief Encode a Unicode codepoint to an output stream.
//! \param os Output stream.
//! \param codepoint An unicode codepoint, ranging from 0x0 to 0x10FFFF inclusively.
template<typename OutputStream>
static void Encode(OutputStream& os, unsigned codepoint);
//! \param codepoint An unicode codepoint, ranging from 0x0 to 0x10FFFF
inclusively. template<typename OutputStream> static void Encode(OutputStream&
os, unsigned codepoint);
//! \brief Decode a Unicode codepoint from an input stream.
//! \param is Input stream.
@@ -58,8 +64,8 @@ concept Encoding {
//! \param is Input stream to obtain codepoint.
//! \param os Output for copying one codepoint.
//! \return true if it is valid.
//! \note This function just validating and copying the codepoint without actually decode it.
template <typename InputStream, typename OutputStream>
//! \note This function just validating and copying the codepoint without
actually decode it. template <typename InputStream, typename OutputStream>
static bool Validate(InputStream& is, OutputStream& os);
// The following functions are deal with byte streams.
@@ -92,8 +98,7 @@ concept Encoding {
\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 };
@@ -105,13 +110,11 @@ struct UTF8 {
else if (codepoint <= 0x7FF) {
os.Put(static_cast<Ch>(0xC0 | ((codepoint >> 6) & 0xFF)));
os.Put(static_cast<Ch>(0x80 | ((codepoint & 0x3F))));
}
else if (codepoint <= 0xFFFF) {
} else if (codepoint <= 0xFFFF) {
os.Put(static_cast<Ch>(0xE0 | ((codepoint >> 12) & 0xFF)));
os.Put(static_cast<Ch>(0x80 | ((codepoint >> 6) & 0x3F)));
os.Put(static_cast<Ch>(0x80 | (codepoint & 0x3F)));
}
else {
} else {
RAPIDJSON_ASSERT(codepoint <= 0x10FFFF);
os.Put(static_cast<Ch>(0xF0 | ((codepoint >> 18) & 0xFF)));
os.Put(static_cast<Ch>(0x80 | ((codepoint >> 12) & 0x3F)));
@@ -127,13 +130,11 @@ struct UTF8 {
else if (codepoint <= 0x7FF) {
PutUnsafe(os, static_cast<Ch>(0xC0 | ((codepoint >> 6) & 0xFF)));
PutUnsafe(os, static_cast<Ch>(0x80 | ((codepoint & 0x3F))));
}
else if (codepoint <= 0xFFFF) {
} else if (codepoint <= 0xFFFF) {
PutUnsafe(os, static_cast<Ch>(0xE0 | ((codepoint >> 12) & 0xFF)));
PutUnsafe(os, static_cast<Ch>(0x80 | ((codepoint >> 6) & 0x3F)));
PutUnsafe(os, static_cast<Ch>(0x80 | (codepoint & 0x3F)));
}
else {
} else {
RAPIDJSON_ASSERT(codepoint <= 0x10FFFF);
PutUnsafe(os, static_cast<Ch>(0xF0 | ((codepoint >> 18) & 0xFF)));
PutUnsafe(os, static_cast<Ch>(0x80 | ((codepoint >> 12) & 0x3F)));
@@ -144,9 +145,14 @@ struct UTF8 {
template <typename InputStream>
static bool Decode(InputStream &is, unsigned *codepoint) {
#define RAPIDJSON_COPY() c = is.Take(); *codepoint = (*codepoint << 6) | (static_cast<unsigned char>(c) & 0x3Fu)
#define RAPIDJSON_TRANS(mask) result &= ((GetRange(static_cast<unsigned char>(c)) & mask) != 0)
#define RAPIDJSON_TAIL() RAPIDJSON_COPY(); RAPIDJSON_TRANS(0x70)
#define RAPIDJSON_COPY() \
c = is.Take(); \
*codepoint = (*codepoint << 6) | (static_cast<unsigned char>(c) & 0x3Fu)
#define RAPIDJSON_TRANS(mask) \
result &= ((GetRange(static_cast<unsigned char>(c)) & mask) != 0)
#define RAPIDJSON_TAIL() \
RAPIDJSON_COPY(); \
RAPIDJSON_TRANS(0x70)
typename InputStream::Ch c = is.Take();
if (!(c & 0x80)) {
*codepoint = static_cast<unsigned char>(c);
@@ -161,14 +167,42 @@ 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
@@ -178,8 +212,11 @@ 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) result &= ((GetRange(static_cast<unsigned char>(c)) & mask) != 0)
#define RAPIDJSON_TAIL() RAPIDJSON_COPY(); RAPIDJSON_TRANS(0x70)
#define RAPIDJSON_TRANS(mask) \
result &= ((GetRange(static_cast<unsigned char>(c)) & mask) != 0)
#define RAPIDJSON_TAIL() \
RAPIDJSON_COPY(); \
RAPIDJSON_TRANS(0x70)
Ch c;
RAPIDJSON_COPY();
if (!(c & 0x80))
@@ -187,14 +224,42 @@ struct UTF8 {
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
@@ -203,18 +268,31 @@ struct UTF8 {
static unsigned char GetRange(unsigned char c) {
// Referring to DFA of http://bjoern.hoehrmann.de/utf-8/decoder/dfa/
// With new mapping 1 -> 0x10, 7 -> 0x20, 9 -> 0x40, such that AND operation can test multiple types.
// With new mapping 1 -> 0x10, 7 -> 0x20, 9 -> 0x40, such that AND operation
// can test multiple types.
static const unsigned char type[] = {
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x10,
0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,
0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,
0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,
8,8,2,2,2,2,2,2,2,2,2,2,2,2,2,2, 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,
10,3,3,3,3,3,3,3,3,3,3,3,3,4,3,3, 11,6,6,6,5,8,8,8,8,8,8,8,8,8,8,8,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0x10, 0x10, 0x10, 0x10,
0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10,
0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40,
0x40, 0x40, 0x40, 0x40, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
8, 8, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2,
2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2,
2, 2, 2, 2, 2, 2, 2, 2, 10, 3, 3, 3,
3, 3, 3, 3, 3, 3, 3, 3, 3, 4, 3, 3,
11, 6, 6, 6, 5, 8, 8, 8, 8, 8, 8, 8,
8, 8, 8, 8,
};
return type[c];
}
@@ -223,17 +301,19 @@ 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());
}
@@ -259,14 +339,14 @@ struct UTF8 {
//! UTF-16 encoding.
/*! http://en.wikipedia.org/wiki/UTF-16
http://tools.ietf.org/html/rfc2781
\tparam CharType Type for storing 16-bit UTF-16 data. Default is wchar_t. C++11 may use char16_t instead.
\note implements Encoding concept
\tparam CharType Type for storing 16-bit UTF-16 data. Default is wchar_t.
C++11 may use char16_t instead. \note implements Encoding concept
\note For in-memory access, no need to concern endianness. The code units and code points are represented by CPU's endianness.
For streaming, use UTF16LE and UTF16BE, which handle endianness.
\note For in-memory access, no need to concern endianness. The code units
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);
@@ -276,10 +356,11 @@ struct UTF16 {
static void Encode(OutputStream &os, unsigned codepoint) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputStream::Ch) >= 2);
if (codepoint <= 0xFFFF) {
RAPIDJSON_ASSERT(codepoint < 0xD800 || codepoint > 0xDFFF); // Code point itself cannot be surrogate pair
RAPIDJSON_ASSERT(
codepoint < 0xD800 ||
codepoint > 0xDFFF); // Code point itself cannot be surrogate pair
os.Put(static_cast<typename OutputStream::Ch>(codepoint));
}
else {
} else {
RAPIDJSON_ASSERT(codepoint <= 0x10FFFF);
unsigned v = codepoint - 0x10000;
os.Put(static_cast<typename OutputStream::Ch>((v >> 10) | 0xD800));
@@ -287,19 +368,20 @@ struct UTF16 {
}
}
template <typename OutputStream>
static void EncodeUnsafe(OutputStream &os, unsigned codepoint) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputStream::Ch) >= 2);
if (codepoint <= 0xFFFF) {
RAPIDJSON_ASSERT(codepoint < 0xD800 || codepoint > 0xDFFF); // Code point itself cannot be surrogate pair
RAPIDJSON_ASSERT(
codepoint < 0xD800 ||
codepoint > 0xDFFF); // Code point itself cannot be surrogate pair
PutUnsafe(os, static_cast<typename OutputStream::Ch>(codepoint));
}
else {
} else {
RAPIDJSON_ASSERT(codepoint <= 0x10FFFF);
unsigned v = codepoint - 0x10000;
PutUnsafe(os, static_cast<typename OutputStream::Ch>((v >> 10) | 0xD800));
PutUnsafe(os, static_cast<typename OutputStream::Ch>((v & 0x3FF) | 0xDC00));
PutUnsafe(os,
static_cast<typename OutputStream::Ch>((v & 0x3FF) | 0xDC00));
}
}
@@ -310,8 +392,7 @@ struct UTF16 {
if (c < 0xD800 || c > 0xDFFF) {
*codepoint = static_cast<unsigned>(c);
return true;
}
else if (c <= 0xDBFF) {
} else if (c <= 0xDBFF) {
*codepoint = (static_cast<unsigned>(c) & 0x3FF) << 10;
c = is.Take();
*codepoint |= (static_cast<unsigned>(c) & 0x3FF);
@@ -338,8 +419,7 @@ 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);
@@ -365,14 +445,15 @@ struct UTF16LE : UTF16<CharType> {
template <typename OutputByteStream>
static void Put(OutputByteStream &os, CharType c) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
os.Put(static_cast<typename OutputByteStream::Ch>(static_cast<unsigned>(c) & 0xFFu));
os.Put(static_cast<typename OutputByteStream::Ch>((static_cast<unsigned>(c) >> 8) & 0xFFu));
os.Put(static_cast<typename OutputByteStream::Ch>(static_cast<unsigned>(c) &
0xFFu));
os.Put(static_cast<typename OutputByteStream::Ch>(
(static_cast<unsigned>(c) >> 8) & 0xFFu));
}
};
//! 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);
@@ -398,8 +479,10 @@ struct UTF16BE : UTF16<CharType> {
template <typename OutputByteStream>
static void Put(OutputByteStream &os, CharType c) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
os.Put(static_cast<typename OutputByteStream::Ch>((static_cast<unsigned>(c) >> 8) & 0xFFu));
os.Put(static_cast<typename OutputByteStream::Ch>(static_cast<unsigned>(c) & 0xFFu));
os.Put(static_cast<typename OutputByteStream::Ch>(
(static_cast<unsigned>(c) >> 8) & 0xFFu));
os.Put(static_cast<typename OutputByteStream::Ch>(static_cast<unsigned>(c) &
0xFFu));
}
};
@@ -408,14 +491,14 @@ struct UTF16BE : UTF16<CharType> {
//! UTF-32 encoding.
/*! http://en.wikipedia.org/wiki/UTF-32
\tparam CharType Type for storing 32-bit UTF-32 data. Default is unsigned. C++11 may use char32_t instead.
\note implements Encoding concept
\tparam CharType Type for storing 32-bit UTF-32 data. Default is unsigned.
C++11 may use char32_t instead. \note implements Encoding concept
\note For in-memory access, no need to concern endianness. The code units and code points are represented by CPU's endianness.
For streaming, use UTF32LE and UTF32BE, which handle endianness.
\note For in-memory access, no need to concern endianness. The code units
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);
@@ -453,8 +536,7 @@ 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);
@@ -492,8 +574,7 @@ 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);
@@ -538,8 +619,7 @@ 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 };
@@ -577,8 +657,7 @@ 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());
}
@@ -608,40 +687,46 @@ enum UTFType {
kUTF32BE = 4 //!< UTF-32 big endian.
};
//! Dynamically select encoding according to stream's runtime-specified UTF encoding type.
/*! \note This class can be used with AutoUTFInputtStream and AutoUTFOutputStream, which provides GetType().
//! Dynamically select encoding according to stream's runtime-specified UTF
//! encoding type.
/*! \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) UTF8<Ch>::x, UTF16LE<Ch>::x, UTF16BE<Ch>::x, UTF32LE<Ch>::x, UTF32BE<Ch>::x
#define RAPIDJSON_ENCODINGS_FUNC(x) \
UTF8<Ch>::x, UTF16LE<Ch>::x, UTF16BE<Ch>::x, UTF32LE<Ch>::x, UTF32BE<Ch>::x
template <typename OutputStream>
static RAPIDJSON_FORCEINLINE void Encode(OutputStream& os, unsigned codepoint) {
static RAPIDJSON_FORCEINLINE void Encode(OutputStream &os,
unsigned codepoint) {
typedef void (*EncodeFunc)(OutputStream &, unsigned);
static const EncodeFunc f[] = {RAPIDJSON_ENCODINGS_FUNC(Encode)};
(*f[os.GetType()])(os, codepoint);
}
template <typename OutputStream>
static RAPIDJSON_FORCEINLINE void EncodeUnsafe(OutputStream& os, unsigned codepoint) {
static RAPIDJSON_FORCEINLINE void EncodeUnsafe(OutputStream &os,
unsigned codepoint) {
typedef void (*EncodeFunc)(OutputStream &, unsigned);
static const EncodeFunc f[] = {RAPIDJSON_ENCODINGS_FUNC(EncodeUnsafe)};
(*f[os.GetType()])(os, codepoint);
}
template <typename InputStream>
static RAPIDJSON_FORCEINLINE bool Decode(InputStream& is, unsigned* codepoint) {
static RAPIDJSON_FORCEINLINE bool Decode(InputStream &is,
unsigned *codepoint) {
typedef bool (*DecodeFunc)(InputStream &, unsigned *);
static const DecodeFunc f[] = {RAPIDJSON_ENCODINGS_FUNC(Decode)};
return (*f[is.GetType()])(is, codepoint);
}
template <typename InputStream, typename OutputStream>
static RAPIDJSON_FORCEINLINE bool Validate(InputStream& is, OutputStream& os) {
static RAPIDJSON_FORCEINLINE bool Validate(InputStream &is,
OutputStream &os) {
typedef bool (*ValidateFunc)(InputStream &, OutputStream &);
static const ValidateFunc f[] = {RAPIDJSON_ENCODINGS_FUNC(Validate)};
return (*f[is.GetType()])(is, os);
@@ -654,11 +739,12 @@ struct AutoUTF {
// Transcoder
//! Encoding conversion.
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 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) {
static RAPIDJSON_FORCEINLINE bool Transcode(InputStream &is,
OutputStream &os) {
unsigned codepoint;
if (!SourceEncoding::Decode(is, &codepoint))
return false;
@@ -667,7 +753,8 @@ struct Transcoder {
}
template <typename InputStream, typename OutputStream>
static RAPIDJSON_FORCEINLINE bool TranscodeUnsafe(InputStream& is, OutputStream& os) {
static RAPIDJSON_FORCEINLINE bool TranscodeUnsafe(InputStream &is,
OutputStream &os) {
unsigned codepoint;
if (!SourceEncoding::Decode(is, &codepoint))
return false;
@@ -677,8 +764,10 @@ struct Transcoder {
//! Validate one Unicode codepoint from an encoded stream.
template <typename InputStream, typename OutputStream>
static RAPIDJSON_FORCEINLINE bool Validate(InputStream& is, OutputStream& os) {
return Transcode(is, os); // Since source/target encoding is different, must transcode.
static RAPIDJSON_FORCEINLINE bool Validate(InputStream &is,
OutputStream &os) {
return Transcode(
is, os); // Since source/target encoding is different, must transcode.
}
};
@@ -687,22 +776,26 @@ 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.
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.
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.
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.
return true;
}
template <typename InputStream, typename OutputStream>
static RAPIDJSON_FORCEINLINE bool Validate(InputStream& is, OutputStream& os) {
static RAPIDJSON_FORCEINLINE bool Validate(InputStream &is,
OutputStream &os) {
return Encoding::Validate(is, os); // source/target encoding are the same
}
};

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_ERROR_EN_H_
#define RAPIDJSON_ERROR_EN_H_
@@ -33,35 +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.");
}
}

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_ERROR_ERROR_H_
#define RAPIDJSON_ERROR_ERROR_H_
@@ -65,20 +69,27 @@ enum ParseErrorCode {
kParseErrorNone = 0, //!< No error.
kParseErrorDocumentEmpty, //!< The document is empty.
kParseErrorDocumentRootNotSingular, //!< The document root must not follow by other values.
kParseErrorDocumentRootNotSingular, //!< The document root must not follow by
//!< other values.
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.
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.
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.
kParseErrorStringMissQuotationMark, //!< Missing a closing quotation mark in
//!< string.
kParseErrorStringInvalidEncoding, //!< Invalid encoding in string.
kParseErrorNumberTooBig, //!< Number too big to be stored in double.
@@ -106,11 +117,13 @@ enum ParseErrorCode {
struct ParseResult {
//!! Unspecified boolean type
typedef bool (ParseResult::*BooleanType)() const;
public:
//! Default constructor, no error.
ParseResult() : code_(kParseErrorNone), offset_(0) {}
//! Constructor to set an error.
ParseResult(ParseErrorCode code, size_t offset) : code_(code), offset_(offset) {}
ParseResult(ParseErrorCode code, size_t offset)
: code_(code), offset_(offset) {}
//! Get the error code.
ParseErrorCode Code() const { return code_; }
@@ -118,22 +131,31 @@ public:
size_t Offset() const { return offset_; }
//! Explicit conversion to \c bool, returns \c true, iff !\ref IsError().
operator BooleanType() const { return !IsError() ? &ParseResult::IsError : NULL; }
operator BooleanType() const {
return !IsError() ? &ParseResult::IsError : NULL;
}
//! Whether the result is an error.
bool IsError() const { return code_ != kParseErrorNone; }
bool operator==(const ParseResult &that) const { return code_ == that.code_; }
bool operator==(ParseErrorCode code) const { return code_ == code; }
friend bool operator==(ParseErrorCode code, const ParseResult & err) { return code == err.code_; }
friend bool operator==(ParseErrorCode code, const ParseResult &err) {
return code == err.code_;
}
bool operator!=(const ParseResult &that) const { return !(*this == that); }
bool operator!=(ParseErrorCode code) const { return !(*this == code); }
friend bool operator!=(ParseErrorCode code, const ParseResult & err) { return err != code; }
friend bool operator!=(ParseErrorCode code, const ParseResult &err) {
return err != code;
}
//! Reset error code.
void Clear() { Set(kParseErrorNone); }
//! Update error code and offset.
void Set(ParseErrorCode code, size_t offset = 0) { code_ = code; offset_ = offset; }
void Set(ParseErrorCode code, size_t offset = 0) {
code_ = code;
offset_ = offset;
}
private:
ParseErrorCode code_;
@@ -147,8 +169,8 @@ private:
User can dynamically change locale in runtime, e.g.:
\code
GetParseErrorFunc GetParseError = GetParseError_En; // or whatever
const RAPIDJSON_ERROR_CHARTYPE* s = GetParseError(document.GetParseErrorCode());
\endcode
const RAPIDJSON_ERROR_CHARTYPE* s =
GetParseError(document.GetParseErrorCode()); \endcode
*/
typedef const RAPIDJSON_ERROR_CHARTYPE *(*GetParseErrorFunc)(ParseErrorCode);

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_FILEREADSTREAM_H_
#define RAPIDJSON_FILEREADSTREAM_H_
@@ -41,21 +45,35 @@ public:
\param buffer user-supplied buffer.
\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) {
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) {
RAPIDJSON_ASSERT(fp_ != 0);
RAPIDJSON_ASSERT(bufferSize >= 4);
Read();
}
Ch Peek() const { return *current_; }
Ch Take() { Ch c = *current_; Read(); return c; }
size_t Tell() const { return count_ + static_cast<size_t>(current_ - buffer_); }
Ch Take() {
Ch c = *current_;
Read();
return c;
}
size_t Tell() const {
return count_ + static_cast<size_t>(current_ - buffer_);
}
// Not implemented
void Put(Ch) { RAPIDJSON_ASSERT(false); }
void Flush() { RAPIDJSON_ASSERT(false); }
Ch* PutBegin() { RAPIDJSON_ASSERT(false); return 0; }
size_t PutEnd(Ch*) { RAPIDJSON_ASSERT(false); return 0; }
Ch *PutBegin() {
RAPIDJSON_ASSERT(false);
return 0;
}
size_t PutEnd(Ch *) {
RAPIDJSON_ASSERT(false);
return 0;
}
// For encoding detection only.
const Ch *Peek4() const {

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_FILEWRITESTREAM_H_
#define RAPIDJSON_FILEWRITESTREAM_H_
@@ -33,7 +37,9 @@ class FileWriteStream {
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), current_(buffer_) {
FileWriteStream(std::FILE *fp, char *buffer, size_t bufferSize)
: fp_(fp), buffer_(buffer), bufferEnd_(buffer + bufferSize),
current_(buffer_) {
RAPIDJSON_ASSERT(fp_ != 0);
}
@@ -62,7 +68,8 @@ public:
void Flush() {
if (current_ != buffer_) {
size_t result = std::fwrite(buffer_, 1, static_cast<size_t>(current_ - buffer_), fp_);
size_t result =
std::fwrite(buffer_, 1, static_cast<size_t>(current_ - buffer_), fp_);
if (result < static_cast<size_t>(current_ - buffer_)) {
// failure deliberately ignored at this time
// added to avoid warn_unused_result build errors
@@ -72,11 +79,26 @@ public:
}
// Not implemented
char Peek() const { RAPIDJSON_ASSERT(false); return 0; }
char Take() { RAPIDJSON_ASSERT(false); return 0; }
size_t Tell() const { RAPIDJSON_ASSERT(false); return 0; }
char* PutBegin() { RAPIDJSON_ASSERT(false); return 0; }
size_t PutEnd(char*) { RAPIDJSON_ASSERT(false); return 0; }
char Peek() const {
RAPIDJSON_ASSERT(false);
return 0;
}
char Take() {
RAPIDJSON_ASSERT(false);
return 0;
}
size_t Tell() const {
RAPIDJSON_ASSERT(false);
return 0;
}
char *PutBegin() {
RAPIDJSON_ASSERT(false);
return 0;
}
size_t PutEnd(char *) {
RAPIDJSON_ASSERT(false);
return 0;
}
private:
// Prohibit copy constructor & assignment operator.
@@ -89,9 +111,9 @@ private:
char *current_;
};
//! Implement specialized version of PutN() with memset() for better performance.
template<>
inline void PutN(FileWriteStream& stream, char c, size_t n) {
//! Implement specialized version of PutN() with memset() for better
//! performance.
template <> inline void PutN(FileWriteStream &stream, char c, size_t n) {
stream.PutN(c, n);
}

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_FWD_H_
#define RAPIDJSON_FWD_H_
@@ -31,32 +35,27 @@ 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;
@@ -70,8 +69,7 @@ class FileWriteStream;
// memorybuffer.h
template <typename Allocator>
struct GenericMemoryBuffer;
template <typename Allocator> struct GenericMemoryBuffer;
typedef GenericMemoryBuffer<CrtAllocator> MemoryBuffer;
@@ -81,49 +79,49 @@ 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>
template <typename SourceEncoding, typename TargetEncoding,
typename StackAllocator>
class GenericReader;
typedef GenericReader<UTF8<char>, UTF8<char>, CrtAllocator> Reader;
// writer.h
template<typename OutputStream, typename SourceEncoding, typename TargetEncoding, typename StackAllocator, unsigned writeFlags>
template <typename OutputStream, typename SourceEncoding,
typename TargetEncoding, typename StackAllocator, unsigned writeFlags>
class Writer;
// prettywriter.h
template<typename OutputStream, typename SourceEncoding, typename TargetEncoding, typename StackAllocator, unsigned writeFlags>
template <typename OutputStream, typename SourceEncoding,
typename TargetEncoding, typename StackAllocator, unsigned writeFlags>
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;
template <typename Encoding, typename Allocator, typename StackAllocator>
class GenericDocument;
typedef GenericDocument<UTF8<char>, MemoryPoolAllocator<CrtAllocator>, CrtAllocator> Document;
typedef GenericDocument<UTF8<char>, MemoryPoolAllocator<CrtAllocator>,
CrtAllocator>
Document;
// pointer.h
template <typename ValueType, typename Allocator>
class GenericPointer;
template <typename ValueType, typename Allocator> class GenericPointer;
typedef GenericPointer<Value, CrtAllocator> Pointer;
@@ -132,19 +130,19 @@ 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> IRemoteSchemaDocumentProvider;
typedef IGenericRemoteSchemaDocumentProvider<SchemaDocument>
IRemoteSchemaDocumentProvider;
template <
typename SchemaDocumentType,
typename OutputHandler,
template <typename SchemaDocumentType, typename OutputHandler,
typename StateAllocator>
class GenericSchemaValidator;
typedef GenericSchemaValidator<SchemaDocument, BaseReaderHandler<UTF8<char>, void>, CrtAllocator> SchemaValidator;
typedef GenericSchemaValidator<
SchemaDocument, BaseReaderHandler<UTF8<char>, void>, CrtAllocator>
SchemaValidator;
RAPIDJSON_NAMESPACE_END

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_BIGINTEGER_H_
#define RAPIDJSON_BIGINTEGER_H_
@@ -33,15 +37,14 @@ public:
std::memcpy(digits_, rhs.digits_, count_ * sizeof(Type));
}
explicit BigInteger(uint64_t u) : count_(1) {
digits_[0] = u;
}
explicit BigInteger(uint64_t u) : count_(1) { digits_[0] = u; }
BigInteger(const char *decimals, size_t length) : count_(1) {
RAPIDJSON_ASSERT(length > 0);
digits_[0] = 0;
size_t i = 0;
const size_t kMaxDigitPerIteration = 19; // 2^64 = 18446744073709551616 > 10^19
const size_t kMaxDigitPerIteration =
19; // 2^64 = 18446744073709551616 > 10^19
while (length >= kMaxDigitPerIteration) {
AppendDecimal64(decimals + i, decimals + i + kMaxDigitPerIteration);
length -= kMaxDigitPerIteration;
@@ -52,8 +55,7 @@ public:
AppendDecimal64(decimals + i, decimals + i + length);
}
BigInteger& operator=(const BigInteger &rhs)
{
BigInteger &operator=(const BigInteger &rhs) {
if (this != &rhs) {
count_ = rhs.count_;
std::memcpy(digits_, rhs.digits_, count_ * sizeof(Type));
@@ -85,9 +87,12 @@ public:
}
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++) {
@@ -103,9 +108,12 @@ public:
}
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++) {
@@ -126,7 +134,8 @@ public:
}
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;
@@ -135,11 +144,11 @@ public:
if (interShift == 0) {
std::memmove(digits_ + offset, digits_, count_ * sizeof(Type));
count_ += offset;
}
else {
} else {
digits_[count_] = 0;
for (size_t i = count_; i > 0; i--)
digits_[i + offset] = (digits_[i] << interShift) | (digits_[i - 1] >> (kTypeBit - interShift));
digits_[i + offset] = (digits_[i] << interShift) |
(digits_[i - 1] >> (kTypeBit - interShift));
digits_[offset] = digits_[0] << interShift;
count_ += offset;
if (digits_[count_])
@@ -152,7 +161,8 @@ public:
}
bool operator==(const BigInteger &rhs) const {
return count_ == rhs.count_ && std::memcmp(digits_, rhs.digits_, count_ * sizeof(Type)) == 0;
return count_ == rhs.count_ &&
std::memcmp(digits_, rhs.digits_, count_ * sizeof(Type)) == 0;
}
bool operator==(const Type rhs) const {
@@ -172,12 +182,15 @@ 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 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5
};
if (exp == 0) return *this;
for (; exp >= 27; exp -= 27) *this *= RAPIDJSON_UINT64_C2(0X6765C793, 0XFA10079D); // 5^27
for (; exp >= 13; exp -= 13) *this *= static_cast<uint32_t>(1220703125u); // 5^13
if (exp > 0) *this *= kPow5[exp - 1];
5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5};
if (exp == 0)
return *this;
for (; exp >= 27; exp -= 27)
*this *= RAPIDJSON_UINT64_C2(0X6765C793, 0XFA10079D); // 5^27
for (; exp >= 13; exp -= 13)
*this *= static_cast<uint32_t>(1220703125u); // 5^13
if (exp > 0)
*this *= kPow5[exp - 1];
return *this;
}
@@ -188,8 +201,15 @@ public:
RAPIDJSON_ASSERT(cmp != 0);
const BigInteger *a, *b; // Makes a > b
bool ret;
if (cmp < 0) { a = &rhs; b = this; ret = true; }
else { a = this; b = &rhs; ret = false; }
if (cmp < 0) {
a = &rhs;
b = this;
ret = true;
} else {
a = this;
b = &rhs;
ret = false;
}
Type borrow = 0;
for (size_t i = 0; i < a->count_; i++) {
@@ -217,7 +237,10 @@ public:
}
size_t GetCount() const { return count_; }
Type GetDigit(size_t index) const { RAPIDJSON_ASSERT(index < count_); return digits_[index]; }
Type GetDigit(size_t index) const {
RAPIDJSON_ASSERT(index < count_);
return digits_[index];
}
bool IsZero() const { return count_ == 1 && digits_[0] == 0; }
private:
@@ -246,20 +269,23 @@ private:
}
// Assume a * b + k < 2^128
static uint64_t MulAdd64(uint64_t a, uint64_t b, uint64_t k, uint64_t* outHigh) {
static uint64_t MulAdd64(uint64_t a, uint64_t b, uint64_t k,
uint64_t *outHigh) {
#if defined(_MSC_VER) && defined(_M_AMD64)
uint64_t low = _umul128(a, b, outHigh) + k;
if (low < k)
(*outHigh)++;
return low;
#elif (__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 6)) && defined(__x86_64__)
#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);
p += k;
*outHigh = static_cast<uint64_t>(p >> 64);
return static_cast<uint64_t>(p);
#else
const uint64_t a0 = a & 0xFFFFFFFF, a1 = a >> 32, b0 = b & 0xFFFFFFFF, b1 = b >> 32;
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 += x2;

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_CLZLL_H_
#define RAPIDJSON_CLZLL_H_
@@ -29,7 +33,8 @@
RAPIDJSON_NAMESPACE_BEGIN
namespace internal {
#if (defined(__GNUC__) && __GNUC__ >= 4) || RAPIDJSON_HAS_BUILTIN(__builtin_clzll)
#if (defined(__GNUC__) && __GNUC__ >= 4) || \
RAPIDJSON_HAS_BUILTIN(__builtin_clzll)
#define RAPIDJSON_CLZLL __builtin_clzll
#else
@@ -64,7 +69,8 @@ inline uint32_t clzll(uint64_t x) {
}
#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
RAPIDJSON_NAMESPACE_END

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

@@ -1,20 +1,24 @@
// Tencent is pleased to support the open source community by making RapidJSON available.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
// This is a C++ header-only implementation of Grisu2 algorithm from the publication:
// Loitsch, Florian. "Printing floating-point numbers quickly and accurately with
// integers." ACM Sigplan Notices 45.6 (2010): 233-243.
// This is a C++ header-only implementation of Grisu2 algorithm from the
// publication: Loitsch, Florian. "Printing floating-point numbers quickly and
// accurately with integers." ACM Sigplan Notices 45.6 (2010): 233-243.
#ifndef RAPIDJSON_DIYFP_H_
#define RAPIDJSON_DIYFP_H_
@@ -52,21 +56,19 @@ struct DiyFp {
uint64_t u64;
} u = {d};
int biased_e = static_cast<int>((u.u64 & kDpExponentMask) >> kDpSignificandSize);
int biased_e =
static_cast<int>((u.u64 & kDpExponentMask) >> kDpSignificandSize);
uint64_t significand = (u.u64 & kDpSignificandMask);
if (biased_e != 0) {
f = significand + kDpHiddenBit;
e = biased_e - kDpExponentBias;
}
else {
} else {
f = significand;
e = kDpMinExponent + 1;
}
}
DiyFp operator-(const DiyFp& rhs) const {
return DiyFp(f - rhs.f, e);
}
DiyFp operator-(const DiyFp &rhs) const { return DiyFp(f - rhs.f, e); }
DiyFp operator*(const DiyFp &rhs) const {
#if defined(_MSC_VER) && defined(_M_AMD64)
@@ -75,7 +77,8 @@ struct DiyFp {
if (l & (uint64_t(1) << 63)) // rounding
h++;
return DiyFp(h, e + rhs.e + 64);
#elif (__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 6)) && defined(__x86_64__)
#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);
@@ -117,7 +120,8 @@ struct DiyFp {
void NormalizedBoundaries(DiyFp *minus, DiyFp *plus) const {
DiyFp pl = DiyFp((f << 1) + 1, e - 1).NormalizeBoundary();
DiyFp mi = (f == kDpHiddenBit) ? DiyFp((f << 2) - 1, e - 2) : DiyFp((f << 1) - 1, e - 1);
DiyFp mi = (f == kDpHiddenBit) ? DiyFp((f << 2) - 1, e - 2)
: DiyFp((f << 1) - 1, e - 1);
mi.f <<= mi.e - pl.e;
mi.e = pl.e;
*plus = pl;
@@ -138,8 +142,9 @@ struct DiyFp {
// Overflow.
return std::numeric_limits<double>::infinity();
}
const uint64_t be = (e == kDpDenormalExponent && (f & kDpHiddenBit) == 0) ? 0 :
static_cast<uint64_t>(e + kDpExponentBias);
const uint64_t be = (e == kDpDenormalExponent && (f & kDpHiddenBit) == 0)
? 0
: static_cast<uint64_t>(e + kDpExponentBias);
u.u64 = (f & kDpSignificandMask) | (be << kDpSignificandSize);
return u.d;
}
@@ -150,9 +155,12 @@ struct DiyFp {
static const int kDpMaxExponent = 0x7FF - kDpExponentBias;
static const int kDpMinExponent = -kDpExponentBias;
static const int kDpDenormalExponent = -kDpExponentBias + 1;
static const uint64_t kDpExponentMask = RAPIDJSON_UINT64_C2(0x7FF00000, 0x00000000);
static const uint64_t kDpSignificandMask = RAPIDJSON_UINT64_C2(0x000FFFFF, 0xFFFFFFFF);
static const uint64_t kDpHiddenBit = RAPIDJSON_UINT64_C2(0x00100000, 0x00000000);
static const uint64_t kDpExponentMask =
RAPIDJSON_UINT64_C2(0x7FF00000, 0x00000000);
static const uint64_t kDpSignificandMask =
RAPIDJSON_UINT64_C2(0x000FFFFF, 0xFFFFFFFF);
static const uint64_t kDpHiddenBit =
RAPIDJSON_UINT64_C2(0x00100000, 0x00000000);
uint64_t f;
int e;
@@ -161,62 +169,102 @@ struct DiyFp {
inline DiyFp GetCachedPowerByIndex(size_t index) {
// 10^-348, 10^-340, ..., 10^340
static const uint64_t kCachedPowers_F[] = {
RAPIDJSON_UINT64_C2(0xfa8fd5a0, 0x081c0288), RAPIDJSON_UINT64_C2(0xbaaee17f, 0xa23ebf76),
RAPIDJSON_UINT64_C2(0x8b16fb20, 0x3055ac76), RAPIDJSON_UINT64_C2(0xcf42894a, 0x5dce35ea),
RAPIDJSON_UINT64_C2(0x9a6bb0aa, 0x55653b2d), RAPIDJSON_UINT64_C2(0xe61acf03, 0x3d1a45df),
RAPIDJSON_UINT64_C2(0xab70fe17, 0xc79ac6ca), RAPIDJSON_UINT64_C2(0xff77b1fc, 0xbebcdc4f),
RAPIDJSON_UINT64_C2(0xbe5691ef, 0x416bd60c), RAPIDJSON_UINT64_C2(0x8dd01fad, 0x907ffc3c),
RAPIDJSON_UINT64_C2(0xd3515c28, 0x31559a83), RAPIDJSON_UINT64_C2(0x9d71ac8f, 0xada6c9b5),
RAPIDJSON_UINT64_C2(0xea9c2277, 0x23ee8bcb), RAPIDJSON_UINT64_C2(0xaecc4991, 0x4078536d),
RAPIDJSON_UINT64_C2(0x823c1279, 0x5db6ce57), RAPIDJSON_UINT64_C2(0xc2109436, 0x4dfb5637),
RAPIDJSON_UINT64_C2(0x9096ea6f, 0x3848984f), RAPIDJSON_UINT64_C2(0xd77485cb, 0x25823ac7),
RAPIDJSON_UINT64_C2(0xa086cfcd, 0x97bf97f4), RAPIDJSON_UINT64_C2(0xef340a98, 0x172aace5),
RAPIDJSON_UINT64_C2(0xb23867fb, 0x2a35b28e), RAPIDJSON_UINT64_C2(0x84c8d4df, 0xd2c63f3b),
RAPIDJSON_UINT64_C2(0xc5dd4427, 0x1ad3cdba), RAPIDJSON_UINT64_C2(0x936b9fce, 0xbb25c996),
RAPIDJSON_UINT64_C2(0xdbac6c24, 0x7d62a584), RAPIDJSON_UINT64_C2(0xa3ab6658, 0x0d5fdaf6),
RAPIDJSON_UINT64_C2(0xf3e2f893, 0xdec3f126), RAPIDJSON_UINT64_C2(0xb5b5ada8, 0xaaff80b8),
RAPIDJSON_UINT64_C2(0x87625f05, 0x6c7c4a8b), RAPIDJSON_UINT64_C2(0xc9bcff60, 0x34c13053),
RAPIDJSON_UINT64_C2(0x964e858c, 0x91ba2655), RAPIDJSON_UINT64_C2(0xdff97724, 0x70297ebd),
RAPIDJSON_UINT64_C2(0xa6dfbd9f, 0xb8e5b88f), RAPIDJSON_UINT64_C2(0xf8a95fcf, 0x88747d94),
RAPIDJSON_UINT64_C2(0xb9447093, 0x8fa89bcf), RAPIDJSON_UINT64_C2(0x8a08f0f8, 0xbf0f156b),
RAPIDJSON_UINT64_C2(0xcdb02555, 0x653131b6), RAPIDJSON_UINT64_C2(0x993fe2c6, 0xd07b7fac),
RAPIDJSON_UINT64_C2(0xe45c10c4, 0x2a2b3b06), RAPIDJSON_UINT64_C2(0xaa242499, 0x697392d3),
RAPIDJSON_UINT64_C2(0xfd87b5f2, 0x8300ca0e), RAPIDJSON_UINT64_C2(0xbce50864, 0x92111aeb),
RAPIDJSON_UINT64_C2(0x8cbccc09, 0x6f5088cc), RAPIDJSON_UINT64_C2(0xd1b71758, 0xe219652c),
RAPIDJSON_UINT64_C2(0x9c400000, 0x00000000), RAPIDJSON_UINT64_C2(0xe8d4a510, 0x00000000),
RAPIDJSON_UINT64_C2(0xad78ebc5, 0xac620000), RAPIDJSON_UINT64_C2(0x813f3978, 0xf8940984),
RAPIDJSON_UINT64_C2(0xc097ce7b, 0xc90715b3), RAPIDJSON_UINT64_C2(0x8f7e32ce, 0x7bea5c70),
RAPIDJSON_UINT64_C2(0xd5d238a4, 0xabe98068), RAPIDJSON_UINT64_C2(0x9f4f2726, 0x179a2245),
RAPIDJSON_UINT64_C2(0xed63a231, 0xd4c4fb27), RAPIDJSON_UINT64_C2(0xb0de6538, 0x8cc8ada8),
RAPIDJSON_UINT64_C2(0x83c7088e, 0x1aab65db), RAPIDJSON_UINT64_C2(0xc45d1df9, 0x42711d9a),
RAPIDJSON_UINT64_C2(0x924d692c, 0xa61be758), RAPIDJSON_UINT64_C2(0xda01ee64, 0x1a708dea),
RAPIDJSON_UINT64_C2(0xa26da399, 0x9aef774a), RAPIDJSON_UINT64_C2(0xf209787b, 0xb47d6b85),
RAPIDJSON_UINT64_C2(0xb454e4a1, 0x79dd1877), RAPIDJSON_UINT64_C2(0x865b8692, 0x5b9bc5c2),
RAPIDJSON_UINT64_C2(0xc83553c5, 0xc8965d3d), RAPIDJSON_UINT64_C2(0x952ab45c, 0xfa97a0b3),
RAPIDJSON_UINT64_C2(0xde469fbd, 0x99a05fe3), RAPIDJSON_UINT64_C2(0xa59bc234, 0xdb398c25),
RAPIDJSON_UINT64_C2(0xf6c69a72, 0xa3989f5c), RAPIDJSON_UINT64_C2(0xb7dcbf53, 0x54e9bece),
RAPIDJSON_UINT64_C2(0x88fcf317, 0xf22241e2), RAPIDJSON_UINT64_C2(0xcc20ce9b, 0xd35c78a5),
RAPIDJSON_UINT64_C2(0x98165af3, 0x7b2153df), RAPIDJSON_UINT64_C2(0xe2a0b5dc, 0x971f303a),
RAPIDJSON_UINT64_C2(0xa8d9d153, 0x5ce3b396), RAPIDJSON_UINT64_C2(0xfb9b7cd9, 0xa4a7443c),
RAPIDJSON_UINT64_C2(0xbb764c4c, 0xa7a44410), RAPIDJSON_UINT64_C2(0x8bab8eef, 0xb6409c1a),
RAPIDJSON_UINT64_C2(0xd01fef10, 0xa657842c), RAPIDJSON_UINT64_C2(0x9b10a4e5, 0xe9913129),
RAPIDJSON_UINT64_C2(0xe7109bfb, 0xa19c0c9d), RAPIDJSON_UINT64_C2(0xac2820d9, 0x623bf429),
RAPIDJSON_UINT64_C2(0x80444b5e, 0x7aa7cf85), RAPIDJSON_UINT64_C2(0xbf21e440, 0x03acdd2d),
RAPIDJSON_UINT64_C2(0x8e679c2f, 0x5e44ff8f), RAPIDJSON_UINT64_C2(0xd433179d, 0x9c8cb841),
RAPIDJSON_UINT64_C2(0x9e19db92, 0xb4e31ba9), RAPIDJSON_UINT64_C2(0xeb96bf6e, 0xbadf77d9),
RAPIDJSON_UINT64_C2(0xaf87023b, 0x9bf0ee6b)
};
RAPIDJSON_UINT64_C2(0xfa8fd5a0, 0x081c0288),
RAPIDJSON_UINT64_C2(0xbaaee17f, 0xa23ebf76),
RAPIDJSON_UINT64_C2(0x8b16fb20, 0x3055ac76),
RAPIDJSON_UINT64_C2(0xcf42894a, 0x5dce35ea),
RAPIDJSON_UINT64_C2(0x9a6bb0aa, 0x55653b2d),
RAPIDJSON_UINT64_C2(0xe61acf03, 0x3d1a45df),
RAPIDJSON_UINT64_C2(0xab70fe17, 0xc79ac6ca),
RAPIDJSON_UINT64_C2(0xff77b1fc, 0xbebcdc4f),
RAPIDJSON_UINT64_C2(0xbe5691ef, 0x416bd60c),
RAPIDJSON_UINT64_C2(0x8dd01fad, 0x907ffc3c),
RAPIDJSON_UINT64_C2(0xd3515c28, 0x31559a83),
RAPIDJSON_UINT64_C2(0x9d71ac8f, 0xada6c9b5),
RAPIDJSON_UINT64_C2(0xea9c2277, 0x23ee8bcb),
RAPIDJSON_UINT64_C2(0xaecc4991, 0x4078536d),
RAPIDJSON_UINT64_C2(0x823c1279, 0x5db6ce57),
RAPIDJSON_UINT64_C2(0xc2109436, 0x4dfb5637),
RAPIDJSON_UINT64_C2(0x9096ea6f, 0x3848984f),
RAPIDJSON_UINT64_C2(0xd77485cb, 0x25823ac7),
RAPIDJSON_UINT64_C2(0xa086cfcd, 0x97bf97f4),
RAPIDJSON_UINT64_C2(0xef340a98, 0x172aace5),
RAPIDJSON_UINT64_C2(0xb23867fb, 0x2a35b28e),
RAPIDJSON_UINT64_C2(0x84c8d4df, 0xd2c63f3b),
RAPIDJSON_UINT64_C2(0xc5dd4427, 0x1ad3cdba),
RAPIDJSON_UINT64_C2(0x936b9fce, 0xbb25c996),
RAPIDJSON_UINT64_C2(0xdbac6c24, 0x7d62a584),
RAPIDJSON_UINT64_C2(0xa3ab6658, 0x0d5fdaf6),
RAPIDJSON_UINT64_C2(0xf3e2f893, 0xdec3f126),
RAPIDJSON_UINT64_C2(0xb5b5ada8, 0xaaff80b8),
RAPIDJSON_UINT64_C2(0x87625f05, 0x6c7c4a8b),
RAPIDJSON_UINT64_C2(0xc9bcff60, 0x34c13053),
RAPIDJSON_UINT64_C2(0x964e858c, 0x91ba2655),
RAPIDJSON_UINT64_C2(0xdff97724, 0x70297ebd),
RAPIDJSON_UINT64_C2(0xa6dfbd9f, 0xb8e5b88f),
RAPIDJSON_UINT64_C2(0xf8a95fcf, 0x88747d94),
RAPIDJSON_UINT64_C2(0xb9447093, 0x8fa89bcf),
RAPIDJSON_UINT64_C2(0x8a08f0f8, 0xbf0f156b),
RAPIDJSON_UINT64_C2(0xcdb02555, 0x653131b6),
RAPIDJSON_UINT64_C2(0x993fe2c6, 0xd07b7fac),
RAPIDJSON_UINT64_C2(0xe45c10c4, 0x2a2b3b06),
RAPIDJSON_UINT64_C2(0xaa242499, 0x697392d3),
RAPIDJSON_UINT64_C2(0xfd87b5f2, 0x8300ca0e),
RAPIDJSON_UINT64_C2(0xbce50864, 0x92111aeb),
RAPIDJSON_UINT64_C2(0x8cbccc09, 0x6f5088cc),
RAPIDJSON_UINT64_C2(0xd1b71758, 0xe219652c),
RAPIDJSON_UINT64_C2(0x9c400000, 0x00000000),
RAPIDJSON_UINT64_C2(0xe8d4a510, 0x00000000),
RAPIDJSON_UINT64_C2(0xad78ebc5, 0xac620000),
RAPIDJSON_UINT64_C2(0x813f3978, 0xf8940984),
RAPIDJSON_UINT64_C2(0xc097ce7b, 0xc90715b3),
RAPIDJSON_UINT64_C2(0x8f7e32ce, 0x7bea5c70),
RAPIDJSON_UINT64_C2(0xd5d238a4, 0xabe98068),
RAPIDJSON_UINT64_C2(0x9f4f2726, 0x179a2245),
RAPIDJSON_UINT64_C2(0xed63a231, 0xd4c4fb27),
RAPIDJSON_UINT64_C2(0xb0de6538, 0x8cc8ada8),
RAPIDJSON_UINT64_C2(0x83c7088e, 0x1aab65db),
RAPIDJSON_UINT64_C2(0xc45d1df9, 0x42711d9a),
RAPIDJSON_UINT64_C2(0x924d692c, 0xa61be758),
RAPIDJSON_UINT64_C2(0xda01ee64, 0x1a708dea),
RAPIDJSON_UINT64_C2(0xa26da399, 0x9aef774a),
RAPIDJSON_UINT64_C2(0xf209787b, 0xb47d6b85),
RAPIDJSON_UINT64_C2(0xb454e4a1, 0x79dd1877),
RAPIDJSON_UINT64_C2(0x865b8692, 0x5b9bc5c2),
RAPIDJSON_UINT64_C2(0xc83553c5, 0xc8965d3d),
RAPIDJSON_UINT64_C2(0x952ab45c, 0xfa97a0b3),
RAPIDJSON_UINT64_C2(0xde469fbd, 0x99a05fe3),
RAPIDJSON_UINT64_C2(0xa59bc234, 0xdb398c25),
RAPIDJSON_UINT64_C2(0xf6c69a72, 0xa3989f5c),
RAPIDJSON_UINT64_C2(0xb7dcbf53, 0x54e9bece),
RAPIDJSON_UINT64_C2(0x88fcf317, 0xf22241e2),
RAPIDJSON_UINT64_C2(0xcc20ce9b, 0xd35c78a5),
RAPIDJSON_UINT64_C2(0x98165af3, 0x7b2153df),
RAPIDJSON_UINT64_C2(0xe2a0b5dc, 0x971f303a),
RAPIDJSON_UINT64_C2(0xa8d9d153, 0x5ce3b396),
RAPIDJSON_UINT64_C2(0xfb9b7cd9, 0xa4a7443c),
RAPIDJSON_UINT64_C2(0xbb764c4c, 0xa7a44410),
RAPIDJSON_UINT64_C2(0x8bab8eef, 0xb6409c1a),
RAPIDJSON_UINT64_C2(0xd01fef10, 0xa657842c),
RAPIDJSON_UINT64_C2(0x9b10a4e5, 0xe9913129),
RAPIDJSON_UINT64_C2(0xe7109bfb, 0xa19c0c9d),
RAPIDJSON_UINT64_C2(0xac2820d9, 0x623bf429),
RAPIDJSON_UINT64_C2(0x80444b5e, 0x7aa7cf85),
RAPIDJSON_UINT64_C2(0xbf21e440, 0x03acdd2d),
RAPIDJSON_UINT64_C2(0x8e679c2f, 0x5e44ff8f),
RAPIDJSON_UINT64_C2(0xd433179d, 0x9c8cb841),
RAPIDJSON_UINT64_C2(0x9e19db92, 0xb4e31ba9),
RAPIDJSON_UINT64_C2(0xeb96bf6e, 0xbadf77d9),
RAPIDJSON_UINT64_C2(0xaf87023b, 0x9bf0ee6b)};
static const int16_t kCachedPowers_E[] = {
-1220, -1193, -1166, -1140, -1113, -1087, -1060, -1034, -1007, -980,
-954, -927, -901, -874, -847, -821, -794, -768, -741, -715,
-688, -661, -635, -608, -582, -555, -529, -502, -475, -449,
-422, -396, -369, -343, -316, -289, -263, -236, -210, -183,
-157, -130, -103, -77, -50, -24, 3, 30, 56, 83,
109, 136, 162, 189, 216, 242, 269, 295, 322, 348,
375, 402, 428, 455, 481, 508, 534, 561, 588, 614,
641, 667, 694, 720, 747, 774, 800, 827, 853, 880,
907, 933, 960, 986, 1013, 1039, 1066
};
-1220, -1193, -1166, -1140, -1113, -1087, -1060, -1034, -1007, -980, -954,
-927, -901, -874, -847, -821, -794, -768, -741, -715, -688, -661,
-635, -608, -582, -555, -529, -502, -475, -449, -422, -396, -369,
-343, -316, -289, -263, -236, -210, -183, -157, -130, -103, -77,
-50, -24, 3, 30, 56, 83, 109, 136, 162, 189, 216,
242, 269, 295, 322, 348, 375, 402, 428, 455, 481, 508,
534, 561, 588, 614, 641, 667, 694, 720, 747, 774, 800,
827, 853, 880, 907, 933, 960, 986, 1013, 1039, 1066};
RAPIDJSON_ASSERT(index < 87);
return DiyFp(kCachedPowers_F[index], kCachedPowers_E[index]);
}
@@ -224,13 +272,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
double dk = (-61 - e) * 0.30102999566398114 +
347; // dk must be positive, so can do ceiling in positive
int k = static_cast<int>(dk);
if (dk - k > 0.0)
k++;
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);
}

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

@@ -1,27 +1,31 @@
// Tencent is pleased to support the open source community by making RapidJSON available.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
// This is a C++ header-only implementation of Grisu2 algorithm from the publication:
// Loitsch, Florian. "Printing floating-point numbers quickly and accurately with
// integers." ACM Sigplan Notices 45.6 (2010): 233-243.
// This is a C++ header-only implementation of Grisu2 algorithm from the
// publication: Loitsch, Florian. "Printing floating-point numbers quickly and
// accurately with integers." ACM Sigplan Notices 45.6 (2010): 233-243.
#ifndef RAPIDJSON_DTOA_
#define RAPIDJSON_DTOA_
#include "itoa.h" // GetDigitsLut()
#include "diyfp.h"
#include "ieee754.h"
#include "itoa.h" // GetDigitsLut()
RAPIDJSON_NAMESPACE_BEGIN
namespace internal {
@@ -29,10 +33,13 @@ 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) {
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
wp_w - rest > rest + ten_kappa - wp_w)) {
@@ -42,23 +49,35 @@ inline void GrisuRound(char* buffer, int len, uint64_t delta, uint64_t rest, uin
}
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;
// 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;
// Will not reach 10 digits in DigitGen()
// if (n < 1000000000) return 9;
// return 10;
return 9;
}
inline void DigitGen(const DiyFp& W, const DiyFp& Mp, uint64_t delta, char* buffer, int* len, int* K) {
static const uint32_t kPow10[] = { 1, 10, 100, 1000, 10000, 100000, 1000000, 10000000, 100000000, 1000000000 };
inline void DigitGen(const DiyFp &W, const DiyFp &Mp, uint64_t delta,
char *buffer, int *len, int *K) {
static const uint32_t kPow10[] = {1, 10, 100, 1000,
10000, 100000, 1000000, 10000000,
100000000, 1000000000};
const DiyFp one(uint64_t(1) << -Mp.e, Mp.e);
const DiyFp wp_w = Mp - W;
uint32_t p1 = static_cast<uint32_t>(Mp.f >> -one.e);
@@ -69,15 +88,42 @@ inline void DigitGen(const DiyFp& W, const DiyFp& Mp, uint64_t delta, char* buff
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;
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)
@@ -86,7 +132,8 @@ inline void DigitGen(const DiyFp& W, const DiyFp& Mp, uint64_t delta, char* buff
uint64_t tmp = (static_cast<uint64_t>(p1) << -one.e) + p2;
if (tmp <= delta) {
*K += kappa;
GrisuRound(buffer, *len, delta, tmp, static_cast<uint64_t>(kPow10[kappa]) << -one.e, wp_w.f);
GrisuRound(buffer, *len, delta, tmp,
static_cast<uint64_t>(kPow10[kappa]) << -one.e, wp_w.f);
return;
}
}
@@ -103,7 +150,8 @@ inline void DigitGen(const DiyFp& W, const DiyFp& Mp, uint64_t delta, char* buff
if (p2 < delta) {
*K += kappa;
int index = -kappa;
GrisuRound(buffer, *len, delta, p2, one.f, wp_w.f * (index < 9 ? kPow10[index] : 0));
GrisuRound(buffer, *len, delta, p2, one.f,
wp_w.f * (index < 9 ? kPow10[index] : 0));
return;
}
}
@@ -135,13 +183,11 @@ inline char* WriteExponent(int K, char* buffer) {
const char *d = GetDigitsLut() + K * 2;
*buffer++ = d[0];
*buffer++ = d[1];
}
else if (K >= 10) {
} else if (K >= 10) {
const char *d = GetDigitsLut() + K * 2;
*buffer++ = d[0];
*buffer++ = d[1];
}
else
} else
*buffer++ = static_cast<char>('0' + static_cast<char>(K));
return buffer;
@@ -157,10 +203,10 @@ inline char* Prettify(char* buffer, int length, int k, int maxDecimalPlaces) {
buffer[kk] = '.';
buffer[kk + 1] = '0';
return &buffer[kk + 2];
}
else if (0 < kk && kk <= 21) {
} else if (0 < kk && kk <= 21) {
// 1234e-2 -> 12.34
std::memmove(&buffer[kk + 1], &buffer[kk], static_cast<size_t>(length - kk));
std::memmove(&buffer[kk + 1], &buffer[kk],
static_cast<size_t>(length - kk));
buffer[kk] = '.';
if (0 > k + maxDecimalPlaces) {
// When maxDecimalPlaces = 2, 1.2345 -> 1.23, 1.102 -> 1.1
@@ -169,11 +215,9 @@ inline char* Prettify(char* buffer, int length, int k, int maxDecimalPlaces) {
if (buffer[i] != '0')
return &buffer[i + 1];
return &buffer[kk + 2]; // Reserve one zero
}
else
} else
return &buffer[length + 1];
}
else if (-6 < kk && kk <= 0) {
} else if (-6 < kk && kk <= 0) {
// 1234e-6 -> 0.001234
const int offset = 2 - kk;
std::memmove(&buffer[offset], &buffer[0], static_cast<size_t>(length));
@@ -188,23 +232,19 @@ inline char* Prettify(char* buffer, int length, int k, int maxDecimalPlaces) {
if (buffer[i] != '0')
return &buffer[i + 1];
return &buffer[3]; // Reserve one zero
}
else
} else
return &buffer[length + offset];
}
else if (kk < -maxDecimalPlaces) {
} else if (kk < -maxDecimalPlaces) {
// Truncate to zero
buffer[0] = '0';
buffer[1] = '.';
buffer[2] = '0';
return &buffer[3];
}
else if (length == 1) {
} else if (length == 1) {
// 1e30
buffer[1] = 'e';
return WriteExponent(kk - 1, &buffer[2]);
}
else {
} else {
// 1234e30 -> 1.234e33
std::memmove(&buffer[2], &buffer[1], static_cast<size_t>(length - 1));
buffer[1] = '.';
@@ -223,8 +263,7 @@ inline char* dtoa(double value, char* buffer, int maxDecimalPlaces = 324) {
buffer[1] = '.';
buffer[2] = '0';
return &buffer[3];
}
else {
} else {
if (value < 0) {
*buffer++ = '-';
value = -value;

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_IEEE754_
#define RAPIDJSON_IEEE754_
@@ -36,17 +40,32 @@ public:
bool Sign() const { return (u_ & kSignMask) != 0; }
uint64_t Significand() const { return u_ & kSignificandMask; }
int Exponent() const { return static_cast<int>(((u_ & kExponentMask) >> kSignificandSize) - kExponentBias); }
int Exponent() const {
return static_cast<int>(((u_ & kExponentMask) >> kSignificandSize) -
kExponentBias);
}
bool IsNan() const { return (u_ & kExponentMask) == kExponentMask && Significand() != 0; }
bool IsInf() const { return (u_ & kExponentMask) == kExponentMask && Significand() == 0; }
bool IsNan() const {
return (u_ & kExponentMask) == kExponentMask && Significand() != 0;
}
bool IsInf() const {
return (u_ & kExponentMask) == kExponentMask && Significand() == 0;
}
bool IsNanOrInf() const { return (u_ & kExponentMask) == kExponentMask; }
bool IsNormal() const { return (u_ & kExponentMask) != 0 || Significand() == 0; }
bool IsNormal() const {
return (u_ & kExponentMask) != 0 || Significand() == 0;
}
bool IsZero() const { return (u_ & (kExponentMask | kSignificandMask)) == 0; }
uint64_t IntegerSignificand() const { return IsNormal() ? Significand() | kHiddenBit : Significand(); }
int IntegerExponent() const { return (IsNormal() ? Exponent() : kDenormalExponent) - kSignificandSize; }
uint64_t ToBias() const { return (u_ & kSignMask) ? ~u_ + 1 : u_ | kSignMask; }
uint64_t IntegerSignificand() const {
return IsNormal() ? Significand() | kHiddenBit : Significand();
}
int IntegerExponent() const {
return (IsNormal() ? Exponent() : kDenormalExponent) - kSignificandSize;
}
uint64_t ToBias() const {
return (u_ & kSignMask) ? ~u_ + 1 : u_ | kSignMask;
}
static int EffectiveSignificandSize(int order) {
if (order >= -1021)
@@ -62,9 +81,12 @@ private:
static const int kExponentBias = 0x3FF;
static const int kDenormalExponent = 1 - kExponentBias;
static const uint64_t kSignMask = RAPIDJSON_UINT64_C2(0x80000000, 0x00000000);
static const uint64_t kExponentMask = RAPIDJSON_UINT64_C2(0x7FF00000, 0x00000000);
static const uint64_t kSignificandMask = RAPIDJSON_UINT64_C2(0x000FFFFF, 0xFFFFFFFF);
static const uint64_t kHiddenBit = RAPIDJSON_UINT64_C2(0x00100000, 0x00000000);
static const uint64_t kExponentMask =
RAPIDJSON_UINT64_C2(0x7FF00000, 0x00000000);
static const uint64_t kSignificandMask =
RAPIDJSON_UINT64_C2(0x000FFFFF, 0xFFFFFFFF);
static const uint64_t kHiddenBit =
RAPIDJSON_UINT64_C2(0x00100000, 0x00000000);
union {
double d_;

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_ITOA_
#define RAPIDJSON_ITOA_
@@ -22,17 +26,20 @@ namespace internal {
inline const char *GetDigitsLut() {
static const char cDigitsLut[200] = {
'0','0','0','1','0','2','0','3','0','4','0','5','0','6','0','7','0','8','0','9',
'1','0','1','1','1','2','1','3','1','4','1','5','1','6','1','7','1','8','1','9',
'2','0','2','1','2','2','2','3','2','4','2','5','2','6','2','7','2','8','2','9',
'3','0','3','1','3','2','3','3','3','4','3','5','3','6','3','7','3','8','3','9',
'4','0','4','1','4','2','4','3','4','4','4','5','4','6','4','7','4','8','4','9',
'5','0','5','1','5','2','5','3','5','4','5','5','5','6','5','7','5','8','5','9',
'6','0','6','1','6','2','6','3','6','4','6','5','6','6','6','7','6','8','6','9',
'7','0','7','1','7','2','7','3','7','4','7','5','7','6','7','7','7','8','7','9',
'8','0','8','1','8','2','8','3','8','4','8','5','8','6','8','7','8','8','8','9',
'9','0','9','1','9','2','9','3','9','4','9','5','9','6','9','7','9','8','9','9'
};
'0', '0', '0', '1', '0', '2', '0', '3', '0', '4', '0', '5', '0', '6', '0',
'7', '0', '8', '0', '9', '1', '0', '1', '1', '1', '2', '1', '3', '1', '4',
'1', '5', '1', '6', '1', '7', '1', '8', '1', '9', '2', '0', '2', '1', '2',
'2', '2', '3', '2', '4', '2', '5', '2', '6', '2', '7', '2', '8', '2', '9',
'3', '0', '3', '1', '3', '2', '3', '3', '3', '4', '3', '5', '3', '6', '3',
'7', '3', '8', '3', '9', '4', '0', '4', '1', '4', '2', '4', '3', '4', '4',
'4', '5', '4', '6', '4', '7', '4', '8', '4', '9', '5', '0', '5', '1', '5',
'2', '5', '3', '5', '4', '5', '5', '5', '6', '5', '7', '5', '8', '5', '9',
'6', '0', '6', '1', '6', '2', '6', '3', '6', '4', '6', '5', '6', '6', '6',
'7', '6', '8', '6', '9', '7', '0', '7', '1', '7', '2', '7', '3', '7', '4',
'7', '5', '7', '6', '7', '7', '7', '8', '7', '9', '8', '0', '8', '1', '8',
'2', '8', '3', '8', '4', '8', '5', '8', '6', '8', '7', '8', '8', '8', '9',
'9', '0', '9', '1', '9', '2', '9', '3', '9', '4', '9', '5', '9', '6', '9',
'7', '9', '8', '9', '9'};
return cDigitsLut;
}
@@ -52,8 +59,7 @@ inline char* u32toa(uint32_t value, char* buffer) {
if (value >= 10)
*buffer++ = cDigitsLut[d2];
*buffer++ = cDigitsLut[d2 + 1];
}
else if (value < 100000000) {
} else if (value < 100000000) {
// value = bbbbcccc
const uint32_t b = value / 10000;
const uint32_t c = value % 10000;
@@ -76,8 +82,7 @@ inline char* u32toa(uint32_t value, char* buffer) {
*buffer++ = cDigitsLut[d3 + 1];
*buffer++ = cDigitsLut[d4];
*buffer++ = cDigitsLut[d4 + 1];
}
else {
} else {
// value = aabbbbcccc in decimal
const uint32_t a = value / 100000000; // 1 to 42
@@ -87,8 +92,7 @@ inline char* u32toa(uint32_t value, char* buffer) {
const unsigned i = a << 1;
*buffer++ = cDigitsLut[i];
*buffer++ = cDigitsLut[i + 1];
}
else
} else
*buffer++ = static_cast<char>('0' + static_cast<char>(a));
const uint32_t b = value / 10000; // 0 to 9999
@@ -149,8 +153,7 @@ inline char* u64toa(uint64_t value, char* buffer) {
if (v >= 10)
*buffer++ = cDigitsLut[d2];
*buffer++ = cDigitsLut[d2 + 1];
}
else {
} else {
// value = bbbbcccc
const uint32_t b = v / 10000;
const uint32_t c = v % 10000;
@@ -174,8 +177,7 @@ inline char* u64toa(uint64_t value, char* buffer) {
*buffer++ = cDigitsLut[d4];
*buffer++ = cDigitsLut[d4 + 1];
}
}
else if (value < kTen16) {
} else if (value < kTen16) {
const uint32_t v0 = static_cast<uint32_t>(value / kTen8);
const uint32_t v1 = static_cast<uint32_t>(value % kTen8);
@@ -221,8 +223,7 @@ inline char* u64toa(uint64_t value, char* buffer) {
*buffer++ = cDigitsLut[d7 + 1];
*buffer++ = cDigitsLut[d8];
*buffer++ = cDigitsLut[d8 + 1];
}
else {
} else {
const uint32_t a = static_cast<uint32_t>(value / kTen16); // 1 to 1844
value %= kTen16;
@@ -232,15 +233,13 @@ inline char* u64toa(uint64_t value, char* buffer) {
const uint32_t i = a << 1;
*buffer++ = cDigitsLut[i];
*buffer++ = cDigitsLut[i + 1];
}
else if (a < 1000) {
} else if (a < 1000) {
*buffer++ = static_cast<char>('0' + static_cast<char>(a / 100));
const uint32_t i = (a % 100) << 1;
*buffer++ = cDigitsLut[i];
*buffer++ = cDigitsLut[i + 1];
}
else {
} else {
const uint32_t i = (a / 100) << 1;
const uint32_t j = (a % 100) << 1;
*buffer++ = cDigitsLut[i];

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_INTERNAL_META_H_
#define RAPIDJSON_INTERNAL_META_H_
@@ -35,7 +39,8 @@ RAPIDJSON_DIAG_OFF(6334)
RAPIDJSON_NAMESPACE_BEGIN
namespace internal {
// Helper to wrap/convert arbitrary types to void, useful for arbitrary type matching
// Helper to wrap/convert arbitrary types to void, useful for arbitrary type
// matching
template <typename T> struct Void { typedef void Type; };
///////////////////////////////////////////////////////////////////////////////
@@ -48,35 +53,43 @@ template <bool Cond> struct BoolType {
typedef BoolType<true> TrueType;
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 <> 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 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 <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 <typename C> struct BoolExpr : SelectIf<C,TrueType,FalseType>::Type {};
template <typename C> struct NotExpr : SelectIf<C,FalseType,TrueType>::Type {};
template <typename C1, typename C2> struct AndExpr : AndExprCond<C1::Value, C2::Value>::Type {};
template <typename C1, typename C2> struct OrExpr : OrExprCond<C1::Value, C2::Value>::Type {};
template <typename C>
struct BoolExpr : SelectIf<C, TrueType, FalseType>::Type {};
template <typename C>
struct NotExpr : SelectIf<C, FalseType, TrueType>::Type {};
template <typename C1, typename C2>
struct AndExpr : AndExprCond<C1::Value, C2::Value>::Type {};
template <typename C1, typename C2>
struct OrExpr : OrExprCond<C1::Value, C2::Value>::Type {};
///////////////////////////////////////////////////////////////////////////////
// AddConst, MaybeAddConst, RemoveConst
template <typename T> struct AddConst { typedef const T Type; };
template <bool Constify, typename T> struct MaybeAddConst : SelectIfCond<Constify, const T, T> {};
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; };
///////////////////////////////////////////////////////////////////////////////
// IsSame, IsConst, IsMoreConst, IsPointer
//
@@ -88,7 +101,8 @@ template <typename T> struct IsConst<const T> : TrueType {};
template <typename CT, typename T>
struct IsMoreConst
: AndExpr<IsSame<typename RemoveConst<CT>::Type, typename RemoveConst<T>::Type>,
: AndExpr<
IsSame<typename RemoveConst<CT>::Type, typename RemoveConst<T>::Type>,
BoolType<IsConst<CT>::Value >= IsConst<T>::Value>>::Type {};
template <typename T> struct IsPointer : FalseType {};
@@ -99,8 +113,8 @@ template <typename T> struct IsPointer<T*> : TrueType {};
//
#if RAPIDJSON_HAS_CXX11_TYPETRAITS
template <typename B, typename D> struct IsBaseOf
: BoolType< ::std::is_base_of<B,D>::value> {};
template <typename B, typename D>
struct IsBaseOf : BoolType<::std::is_base_of<B, D>::value> {};
#else // simplified version adopted from Boost
@@ -111,8 +125,7 @@ template<typename B, typename D> struct IsBaseOfImpl {
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 {
@@ -123,20 +136,25 @@ template<typename B, typename D> struct IsBaseOfImpl {
enum { Value = (sizeof(Check(Host(), 0)) == sizeof(Yes)) };
};
template <typename B, typename D> struct IsBaseOf
: OrExpr<IsSame<B, D>, BoolExpr<IsBaseOfImpl<B, D> > >::Type {};
template <typename B, typename D>
struct IsBaseOf : OrExpr<IsSame<B, D>, BoolExpr<IsBaseOfImpl<B, D>>>::Type {};
#endif // RAPIDJSON_HAS_CXX11_TYPETRAITS
//////////////////////////////////////////////////////////////////////////
// EnableIf / DisableIf
//
template <bool Condition, typename T = void> struct EnableIfCond { typedef T Type; };
template <typename T> struct EnableIfCond<false, T> { /* empty */ };
template <bool Condition, typename T = void> struct EnableIfCond {
typedef T Type;
};
template <typename T> struct EnableIfCond<false, T> { /* empty */
};
template <bool Condition, typename T = void> struct DisableIfCond { typedef T Type; };
template <typename T> struct DisableIfCond<true, T> { /* empty */ };
template <bool Condition, typename T = void> struct DisableIfCond {
typedef T Type;
};
template <typename T> struct DisableIfCond<true, T> { /* empty */
};
template <typename Condition, typename T = void>
struct EnableIf : EnableIfCond<Condition::Value, T> {};
@@ -147,29 +165,29 @@ struct DisableIf : DisableIfCond<Condition::Value, T> {};
// SFINAE helpers
struct SfinaeTag {};
template <typename T> struct RemoveSfinaeTag;
template <typename T> struct RemoveSfinaeTag<SfinaeTag&(*)(T)> { typedef T Type; };
template <typename T> struct RemoveSfinaeTag<SfinaeTag &(*)(T)> {
typedef T Type;
};
#define RAPIDJSON_REMOVEFPTR_(type) \
typename ::RAPIDJSON_NAMESPACE::internal::RemoveSfinaeTag \
< ::RAPIDJSON_NAMESPACE::internal::SfinaeTag&(*) type>::Type
typename ::RAPIDJSON_NAMESPACE::internal::RemoveSfinaeTag< \
::RAPIDJSON_NAMESPACE::internal::SfinaeTag &(*)type>::Type
#define RAPIDJSON_ENABLEIF(cond) \
typename ::RAPIDJSON_NAMESPACE::internal::EnableIf \
<RAPIDJSON_REMOVEFPTR_(cond)>::Type * = NULL
typename ::RAPIDJSON_NAMESPACE::internal::EnableIf<RAPIDJSON_REMOVEFPTR_( \
cond)>::Type * = NULL
#define RAPIDJSON_DISABLEIF(cond) \
typename ::RAPIDJSON_NAMESPACE::internal::DisableIf \
<RAPIDJSON_REMOVEFPTR_(cond)>::Type * = NULL
typename ::RAPIDJSON_NAMESPACE::internal::DisableIf<RAPIDJSON_REMOVEFPTR_( \
cond)>::Type * = NULL
#define RAPIDJSON_ENABLEIF_RETURN(cond, returntype) \
typename ::RAPIDJSON_NAMESPACE::internal::EnableIf \
<RAPIDJSON_REMOVEFPTR_(cond), \
RAPIDJSON_REMOVEFPTR_(returntype)>::Type
typename ::RAPIDJSON_NAMESPACE::internal::EnableIf< \
RAPIDJSON_REMOVEFPTR_(cond), RAPIDJSON_REMOVEFPTR_(returntype)>::Type
#define RAPIDJSON_DISABLEIF_RETURN(cond, returntype) \
typename ::RAPIDJSON_NAMESPACE::internal::DisableIf \
<RAPIDJSON_REMOVEFPTR_(cond), \
RAPIDJSON_REMOVEFPTR_(returntype)>::Type
typename ::RAPIDJSON_NAMESPACE::internal::DisableIf< \
RAPIDJSON_REMOVEFPTR_(cond), RAPIDJSON_REMOVEFPTR_(returntype)>::Type
} // namespace internal
RAPIDJSON_NAMESPACE_END

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_POW10_
#define RAPIDJSON_POW10_
@@ -26,25 +30,43 @@ namespace internal {
\return 10.0^n
*/
inline double Pow10(int n) {
static const double e[] = { // 1e-0...1e308: 309 * 8 bytes = 2472 bytes
1e+0,
1e+1, 1e+2, 1e+3, 1e+4, 1e+5, 1e+6, 1e+7, 1e+8, 1e+9, 1e+10, 1e+11, 1e+12, 1e+13, 1e+14, 1e+15, 1e+16, 1e+17, 1e+18, 1e+19, 1e+20,
1e+21, 1e+22, 1e+23, 1e+24, 1e+25, 1e+26, 1e+27, 1e+28, 1e+29, 1e+30, 1e+31, 1e+32, 1e+33, 1e+34, 1e+35, 1e+36, 1e+37, 1e+38, 1e+39, 1e+40,
1e+41, 1e+42, 1e+43, 1e+44, 1e+45, 1e+46, 1e+47, 1e+48, 1e+49, 1e+50, 1e+51, 1e+52, 1e+53, 1e+54, 1e+55, 1e+56, 1e+57, 1e+58, 1e+59, 1e+60,
1e+61, 1e+62, 1e+63, 1e+64, 1e+65, 1e+66, 1e+67, 1e+68, 1e+69, 1e+70, 1e+71, 1e+72, 1e+73, 1e+74, 1e+75, 1e+76, 1e+77, 1e+78, 1e+79, 1e+80,
1e+81, 1e+82, 1e+83, 1e+84, 1e+85, 1e+86, 1e+87, 1e+88, 1e+89, 1e+90, 1e+91, 1e+92, 1e+93, 1e+94, 1e+95, 1e+96, 1e+97, 1e+98, 1e+99, 1e+100,
1e+101,1e+102,1e+103,1e+104,1e+105,1e+106,1e+107,1e+108,1e+109,1e+110,1e+111,1e+112,1e+113,1e+114,1e+115,1e+116,1e+117,1e+118,1e+119,1e+120,
1e+121,1e+122,1e+123,1e+124,1e+125,1e+126,1e+127,1e+128,1e+129,1e+130,1e+131,1e+132,1e+133,1e+134,1e+135,1e+136,1e+137,1e+138,1e+139,1e+140,
1e+141,1e+142,1e+143,1e+144,1e+145,1e+146,1e+147,1e+148,1e+149,1e+150,1e+151,1e+152,1e+153,1e+154,1e+155,1e+156,1e+157,1e+158,1e+159,1e+160,
1e+161,1e+162,1e+163,1e+164,1e+165,1e+166,1e+167,1e+168,1e+169,1e+170,1e+171,1e+172,1e+173,1e+174,1e+175,1e+176,1e+177,1e+178,1e+179,1e+180,
1e+181,1e+182,1e+183,1e+184,1e+185,1e+186,1e+187,1e+188,1e+189,1e+190,1e+191,1e+192,1e+193,1e+194,1e+195,1e+196,1e+197,1e+198,1e+199,1e+200,
1e+201,1e+202,1e+203,1e+204,1e+205,1e+206,1e+207,1e+208,1e+209,1e+210,1e+211,1e+212,1e+213,1e+214,1e+215,1e+216,1e+217,1e+218,1e+219,1e+220,
1e+221,1e+222,1e+223,1e+224,1e+225,1e+226,1e+227,1e+228,1e+229,1e+230,1e+231,1e+232,1e+233,1e+234,1e+235,1e+236,1e+237,1e+238,1e+239,1e+240,
1e+241,1e+242,1e+243,1e+244,1e+245,1e+246,1e+247,1e+248,1e+249,1e+250,1e+251,1e+252,1e+253,1e+254,1e+255,1e+256,1e+257,1e+258,1e+259,1e+260,
1e+261,1e+262,1e+263,1e+264,1e+265,1e+266,1e+267,1e+268,1e+269,1e+270,1e+271,1e+272,1e+273,1e+274,1e+275,1e+276,1e+277,1e+278,1e+279,1e+280,
1e+281,1e+282,1e+283,1e+284,1e+285,1e+286,1e+287,1e+288,1e+289,1e+290,1e+291,1e+292,1e+293,1e+294,1e+295,1e+296,1e+297,1e+298,1e+299,1e+300,
1e+301,1e+302,1e+303,1e+304,1e+305,1e+306,1e+307,1e+308
};
static const double e[] = {
// 1e-0...1e308: 309 * 8 bytes = 2472 bytes
1e+0, 1e+1, 1e+2, 1e+3, 1e+4, 1e+5, 1e+6, 1e+7, 1e+8,
1e+9, 1e+10, 1e+11, 1e+12, 1e+13, 1e+14, 1e+15, 1e+16, 1e+17,
1e+18, 1e+19, 1e+20, 1e+21, 1e+22, 1e+23, 1e+24, 1e+25, 1e+26,
1e+27, 1e+28, 1e+29, 1e+30, 1e+31, 1e+32, 1e+33, 1e+34, 1e+35,
1e+36, 1e+37, 1e+38, 1e+39, 1e+40, 1e+41, 1e+42, 1e+43, 1e+44,
1e+45, 1e+46, 1e+47, 1e+48, 1e+49, 1e+50, 1e+51, 1e+52, 1e+53,
1e+54, 1e+55, 1e+56, 1e+57, 1e+58, 1e+59, 1e+60, 1e+61, 1e+62,
1e+63, 1e+64, 1e+65, 1e+66, 1e+67, 1e+68, 1e+69, 1e+70, 1e+71,
1e+72, 1e+73, 1e+74, 1e+75, 1e+76, 1e+77, 1e+78, 1e+79, 1e+80,
1e+81, 1e+82, 1e+83, 1e+84, 1e+85, 1e+86, 1e+87, 1e+88, 1e+89,
1e+90, 1e+91, 1e+92, 1e+93, 1e+94, 1e+95, 1e+96, 1e+97, 1e+98,
1e+99, 1e+100, 1e+101, 1e+102, 1e+103, 1e+104, 1e+105, 1e+106, 1e+107,
1e+108, 1e+109, 1e+110, 1e+111, 1e+112, 1e+113, 1e+114, 1e+115, 1e+116,
1e+117, 1e+118, 1e+119, 1e+120, 1e+121, 1e+122, 1e+123, 1e+124, 1e+125,
1e+126, 1e+127, 1e+128, 1e+129, 1e+130, 1e+131, 1e+132, 1e+133, 1e+134,
1e+135, 1e+136, 1e+137, 1e+138, 1e+139, 1e+140, 1e+141, 1e+142, 1e+143,
1e+144, 1e+145, 1e+146, 1e+147, 1e+148, 1e+149, 1e+150, 1e+151, 1e+152,
1e+153, 1e+154, 1e+155, 1e+156, 1e+157, 1e+158, 1e+159, 1e+160, 1e+161,
1e+162, 1e+163, 1e+164, 1e+165, 1e+166, 1e+167, 1e+168, 1e+169, 1e+170,
1e+171, 1e+172, 1e+173, 1e+174, 1e+175, 1e+176, 1e+177, 1e+178, 1e+179,
1e+180, 1e+181, 1e+182, 1e+183, 1e+184, 1e+185, 1e+186, 1e+187, 1e+188,
1e+189, 1e+190, 1e+191, 1e+192, 1e+193, 1e+194, 1e+195, 1e+196, 1e+197,
1e+198, 1e+199, 1e+200, 1e+201, 1e+202, 1e+203, 1e+204, 1e+205, 1e+206,
1e+207, 1e+208, 1e+209, 1e+210, 1e+211, 1e+212, 1e+213, 1e+214, 1e+215,
1e+216, 1e+217, 1e+218, 1e+219, 1e+220, 1e+221, 1e+222, 1e+223, 1e+224,
1e+225, 1e+226, 1e+227, 1e+228, 1e+229, 1e+230, 1e+231, 1e+232, 1e+233,
1e+234, 1e+235, 1e+236, 1e+237, 1e+238, 1e+239, 1e+240, 1e+241, 1e+242,
1e+243, 1e+244, 1e+245, 1e+246, 1e+247, 1e+248, 1e+249, 1e+250, 1e+251,
1e+252, 1e+253, 1e+254, 1e+255, 1e+256, 1e+257, 1e+258, 1e+259, 1e+260,
1e+261, 1e+262, 1e+263, 1e+264, 1e+265, 1e+266, 1e+267, 1e+268, 1e+269,
1e+270, 1e+271, 1e+272, 1e+273, 1e+274, 1e+275, 1e+276, 1e+277, 1e+278,
1e+279, 1e+280, 1e+281, 1e+282, 1e+283, 1e+284, 1e+285, 1e+286, 1e+287,
1e+288, 1e+289, 1e+290, 1e+291, 1e+292, 1e+293, 1e+294, 1e+295, 1e+296,
1e+297, 1e+298, 1e+299, 1e+300, 1e+301, 1e+302, 1e+303, 1e+304, 1e+305,
1e+306, 1e+307, 1e+308};
RAPIDJSON_ASSERT(n >= 0 && n <= 308);
return e[n];
}

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_INTERNAL_REGEX_H_
#define RAPIDJSON_INTERNAL_REGEX_H_
@@ -43,8 +47,7 @@ namespace internal {
///////////////////////////////////////////////////////////////////////////////
// DecodedStream
template <typename SourceStream, typename Encoding>
class DecodedStream {
template <typename SourceStream, typename Encoding> class DecodedStream {
public:
DecodedStream(SourceStream &ss) : ss_(ss), codepoint_() { Decode(); }
unsigned Peek() { return codepoint_; }
@@ -68,11 +71,11 @@ private:
///////////////////////////////////////////////////////////////////////////////
// GenericRegex
static const SizeType kRegexInvalidState = ~SizeType(0); //!< Represents an invalid index in GenericRegex::State::out, out1
static const SizeType kRegexInvalidState = ~SizeType(
0); //!< Represents an invalid index in GenericRegex::State::out, out1
static const SizeType kRegexInvalidRange = ~SizeType(0);
template <typename Encoding, typename Allocator>
class GenericRegexSearch;
template <typename Encoding, typename Allocator> class GenericRegexSearch;
//! Regular expression engine with subset of ECMAscript grammar.
/*!
@@ -103,7 +106,8 @@ class GenericRegexSearch;
- \c \\v Vertical tab (U+000B)
\note This is a Thompson NFA engine, implemented with reference to
Cox, Russ. "Regular Expression Matching Can Be Simple And Fast (but is slow in Java, Perl, PHP, Python, Ruby,...).",
Cox, Russ. "Regular Expression Matching Can Be Simple And Fast (but is
slow in Java, Perl, PHP, Python, Ruby,...).",
https://swtch.com/~rsc/regexp/regexp1.html
*/
template <typename Encoding, typename Allocator = CrtAllocator>
@@ -113,24 +117,20 @@ public:
typedef typename Encoding::Ch Ch;
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_(), anchorEnd_()
{
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_(),
anchorEnd_() {
GenericStringStream<Encoding> ss(source);
DecodedStream<GenericStringStream<Encoding>, Encoding> ds(ss);
Parse(ds);
}
~GenericRegex()
{
RAPIDJSON_DELETE(ownAllocator_);
}
~GenericRegex() { RAPIDJSON_DELETE(ownAllocator_); }
bool IsValid() const {
return root_ != kRegexInvalidState;
}
bool IsValid() const { return root_ != kRegexInvalidState; }
private:
enum Operator {
@@ -190,7 +190,8 @@ private:
void Parse(DecodedStream<InputStream, Encoding> &ds) {
Stack<Allocator> operandStack(allocator_, 256); // Frag
Stack<Allocator> operatorStack(allocator_, 256); // Operator
Stack<Allocator> atomCountStack(allocator_, 256); // unsigned (Atom per parenthesis)
Stack<Allocator> atomCountStack(allocator_,
256); // unsigned (Atom per parenthesis)
*atomCountStack.template Push<unsigned>() = 0;
@@ -206,7 +207,8 @@ private:
break;
case '|':
while (!operatorStack.Empty() && *operatorStack.template Top<Operator>() < kAlternation)
while (!operatorStack.Empty() &&
*operatorStack.template Top<Operator>() < kAlternation)
if (!Eval(operandStack, *operatorStack.template Pop<Operator>(1)))
return;
*operatorStack.template Push<Operator>() = kAlternation;
@@ -219,7 +221,8 @@ private:
break;
case ')':
while (!operatorStack.Empty() && *operatorStack.template Top<Operator>() != kLeftParenthesis)
while (!operatorStack.Empty() &&
*operatorStack.template Top<Operator>() != kLeftParenthesis)
if (!Eval(operandStack, *operatorStack.template Pop<Operator>(1)))
return;
if (operatorStack.Empty())
@@ -244,8 +247,7 @@ private:
return;
break;
case '{':
{
case '{': {
unsigned n, m;
if (!ParseUnsigned(ds, &n))
return;
@@ -256,27 +258,25 @@ private:
m = kInfinityQuantifier;
else if (!ParseUnsigned(ds, &m) || m < n)
return;
}
else
} else
m = n;
if (!EvalQuantifier(operandStack, n, m) || ds.Peek() != '}')
return;
ds.Take();
}
break;
} break;
case '.':
PushOperand(operandStack, kAnyCharacterClass);
ImplicitConcatenation(atomCountStack, operatorStack);
break;
case '[':
{
case '[': {
SizeType range;
if (!ParseRange(ds, &range))
return;
SizeType s = NewState(kRegexInvalidState, kRegexInvalidState, kRangeCharacterClass);
SizeType s = NewState(kRegexInvalidState, kRegexInvalidState,
kRangeCharacterClass);
GetState(s).rangeStart = range;
*operandStack.template Push<Frag>() = Frag(s, s, s);
}
@@ -309,7 +309,8 @@ private:
printf("root: %d\n", root_);
for (SizeType i = 0; i < stateCount_; i++) {
State &s = GetState(i);
printf("[%2d] out: %2d out1: %2d c: '%c'\n", i, s.out, s.out1, (char)s.codepoint);
printf("[%2d] out: %2d out1: %2d c: '%c'\n", i, s.out, s.out1,
(char)s.codepoint);
}
printf("\n");
#endif
@@ -330,7 +331,8 @@ private:
*operandStack.template Push<Frag>() = Frag(s, s, s);
}
void ImplicitConcatenation(Stack<Allocator>& atomCountStack, Stack<Allocator>& operatorStack) {
void ImplicitConcatenation(Stack<Allocator> &atomCountStack,
Stack<Allocator> &operatorStack) {
if (*atomCountStack.template Top<unsigned>())
*operatorStack.template Push<Operator>() = kConcatenation;
(*atomCountStack.template Top<unsigned>())++;
@@ -359,7 +361,8 @@ private:
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));
*operandStack.template Push<Frag>() =
Frag(e1.start, e2.out, Min(e1.minIndex, e2.minIndex));
}
return true;
@@ -368,7 +371,8 @@ private:
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));
*operandStack.template Push<Frag>() =
Frag(s, Append(e1.out, e2.out), Min(e1.minIndex, e2.minIndex));
return true;
}
return false;
@@ -377,7 +381,8 @@ private:
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);
*operandStack.template Push<Frag>() =
Frag(s, Append(e.out, s), e.minIndex);
return true;
}
return false;
@@ -442,7 +447,8 @@ private:
}
for (unsigned i = 0; i < n - 1; i++)
Eval(operandStack, kConcatenation); // a{3} -> aaa, a{3,} -> aaa+, a{3.5} -> aaaa?a?
Eval(operandStack,
kConcatenation); // a{3} -> aaa, a{3,} -> aaa+, a{3.5} -> aaaa?a?
return true;
}
@@ -450,8 +456,12 @@ private:
static SizeType Min(SizeType a, SizeType b) { return a < b ? a : b; }
void CloneTopOperand(Stack<Allocator> &operandStack) {
const Frag src = *operandStack.template Top<Frag>(); // Copy constructor to prevent invalidation
SizeType count = stateCount_ - src.minIndex; // Assumes top operand contains states in [src->minIndex, stateCount_)
const Frag src =
*operandStack
.template Top<Frag>(); // Copy constructor to prevent invalidation
SizeType count =
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++) {
@@ -460,7 +470,8 @@ private:
if (s[j].out1 != kRegexInvalidState)
s[j].out1 += count;
}
*operandStack.template Push<Frag>() = Frag(src.start + count, src.out + count, src.minIndex + count);
*operandStack.template Push<Frag>() =
Frag(src.start + count, src.out + count, src.minIndex + count);
stateCount_ += count;
}
@@ -513,8 +524,7 @@ private:
if (ds.Peek() == 'b') {
ds.Take();
codepoint = 0x0008; // Escape backspace character
}
else if (!CharacterEscape(ds, &codepoint))
} else if (!CharacterEscape(ds, &codepoint))
return false;
// fall through to default
RAPIDJSON_DELIBERATE_FALLTHROUGH;
@@ -529,8 +539,7 @@ private:
// fall through to step 0 for other characters
RAPIDJSON_DELIBERATE_FALLTHROUGH;
case 0:
{
case 0: {
SizeType r = NewRange(codepoint);
if (current != kRegexInvalidRange)
GetRange(current).next = r;
@@ -559,7 +568,8 @@ private:
}
template <typename InputStream>
bool CharacterEscape(DecodedStream<InputStream, Encoding>& ds, unsigned* escapedCodepoint) {
bool CharacterEscape(DecodedStream<InputStream, Encoding> &ds,
unsigned *escapedCodepoint) {
unsigned codepoint;
switch (codepoint = ds.Take()) {
case '^':
@@ -576,12 +586,23 @@ private:
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;
*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
}
@@ -608,10 +629,9 @@ 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_()
{
GenericRegexSearch(const RegexType &regex, Allocator *allocator = 0)
: regex_(regex), allocator_(allocator), ownAllocator_(0),
state0_(allocator, 0), state1_(allocator, 0), stateSet_() {
RAPIDJSON_ASSERT(regex_.IsValid());
if (!allocator_)
ownAllocator_ = allocator_ = RAPIDJSON_NEW(Allocator)();
@@ -625,8 +645,7 @@ public:
RAPIDJSON_DELETE(ownAllocator_);
}
template <typename InputStream>
bool Match(InputStream& is) {
template <typename InputStream> bool Match(InputStream &is) {
return SearchWithAnchoring(is, true, true);
}
@@ -635,8 +654,7 @@ 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_);
}
@@ -664,12 +682,13 @@ private:
std::memset(stateSet_, 0, stateSetSize);
next->Clear();
matched = false;
for (const SizeType* s = current->template Bottom<SizeType>(); s != current->template End<SizeType>(); ++s) {
for (const SizeType *s = current->template Bottom<SizeType>();
s != current->template End<SizeType>(); ++s) {
const State &sr = regex_.GetState(*s);
if (sr.codepoint == codepoint ||
sr.codepoint == RegexType::kAnyCharacterClass ||
(sr.codepoint == RegexType::kRangeCharacterClass && MatchRange(sr.rangeStart, codepoint)))
{
(sr.codepoint == RegexType::kRangeCharacterClass &&
MatchRange(sr.rangeStart, codepoint))) {
matched = AddState(*next, sr.out) || matched;
if (!anchorEnd && matched)
return true;
@@ -683,9 +702,7 @@ private:
return matched;
}
size_t GetStateSetSize() const {
return (regex_.stateCount_ + 31) / 32 * 4;
}
size_t GetStateSetSize() const { return (regex_.stateCount_ + 31) / 32 * 4; }
// Return whether the added states is a match state
bool AddState(Stack<Allocator> &l, SizeType index) {
@@ -695,19 +712,22 @@ private:
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)))) {
} else if (!(stateSet_[index >> 5] & (1u << (index & 31)))) {
stateSet_[index >> 5] |= (1u << (index & 31));
*l.template PushUnsafe<SizeType>() = index;
}
return s.out == kRegexInvalidState; // by using PushUnsafe() above, we can ensure s is not validated due to reallocation.
return s.out ==
kRegexInvalidState; // by using PushUnsafe() above, we can ensure s
// is not validated due to reallocation.
}
bool MatchRange(SizeType rangeIndex, unsigned codepoint) const {
bool yes = (regex_.GetRange(rangeIndex).start & RegexType::kRangeNegationFlag) == 0;
bool yes = (regex_.GetRange(rangeIndex).start &
RegexType::kRangeNegationFlag) == 0;
while (rangeIndex != kRegexInvalidRange) {
const Range &r = regex_.GetRange(rangeIndex);
if (codepoint >= (r.start & ~RegexType::kRangeNegationFlag) && codepoint <= r.end)
if (codepoint >= (r.start & ~RegexType::kRangeNegationFlag) &&
codepoint <= r.end)
return yes;
rangeIndex = r.next;
}

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_INTERNAL_STACK_H_
#define RAPIDJSON_INTERNAL_STACK_H_
@@ -33,23 +37,19 @@ namespace internal {
//! A type-unsafe stack for storing different types of data.
/*! \tparam Allocator Allocator for allocating stack memory.
*/
template <typename Allocator>
class Stack {
template <typename Allocator> class Stack {
public:
// Optimization note: Do not allocate memory for stack_ in constructor.
// Do it lazily when first Push() -> Expand() -> Resize().
Stack(Allocator* allocator, size_t stackCapacity) : allocator_(allocator), ownAllocator_(0), stack_(0), stackTop_(0), stackEnd_(0), initialCapacity_(stackCapacity) {
}
Stack(Allocator *allocator, size_t 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_),
initialCapacity_(rhs.initialCapacity_)
{
: allocator_(rhs.allocator_), ownAllocator_(rhs.ownAllocator_),
stack_(rhs.stack_), stackTop_(rhs.stackTop_), stackEnd_(rhs.stackEnd_),
initialCapacity_(rhs.initialCapacity_) {
rhs.allocator_ = 0;
rhs.ownAllocator_ = 0;
rhs.stack_ = 0;
@@ -59,14 +59,11 @@ public:
}
#endif
~Stack() {
Destroy();
}
~Stack() { Destroy(); }
#if RAPIDJSON_HAS_CXX11_RVALUE_REFS
Stack &operator=(Stack &&rhs) {
if (&rhs != this)
{
if (&rhs != this) {
Destroy();
allocator_ = rhs.allocator_;
@@ -105,70 +102,64 @@ public:
stack_ = 0;
stackTop_ = 0;
stackEnd_ = 0;
}
else
} else
Resize(GetSize());
}
// 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) {
// 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) {
// Expand the stack if needed
if (RAPIDJSON_UNLIKELY(static_cast<std::ptrdiff_t>(sizeof(T) * count) > (stackEnd_ - stackTop_)))
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_));
RAPIDJSON_ASSERT(static_cast<std::ptrdiff_t>(sizeof(T) * count) <=
(stackEnd_ - stackTop_));
T *ret = reinterpret_cast<T *>(stackTop_);
stackTop_ += sizeof(T) * count;
return ret;
}
template<typename T>
T* Pop(size_t count) {
template <typename T> T *Pop(size_t count) {
RAPIDJSON_ASSERT(GetSize() >= count * sizeof(T));
stackTop_ -= count * sizeof(T);
return reinterpret_cast<T *>(stackTop_);
}
template<typename T>
T* Top() {
template <typename T> T *Top() {
RAPIDJSON_ASSERT(GetSize() >= sizeof(T));
return reinterpret_cast<T *>(stackTop_ - sizeof(T));
}
template<typename T>
const T* Top() const {
template <typename T> const T *Top() const {
RAPIDJSON_ASSERT(GetSize() >= sizeof(T));
return reinterpret_cast<T *>(stackTop_ - sizeof(T));
}
template<typename T>
T* End() { return reinterpret_cast<T*>(stackTop_); }
template <typename T> T *End() { return reinterpret_cast<T *>(stackTop_); }
template<typename T>
const T* End() const { return reinterpret_cast<T*>(stackTop_); }
template<typename T>
T* Bottom() { return reinterpret_cast<T*>(stack_); }
template<typename T>
const T* Bottom() const { return reinterpret_cast<T*>(stack_); }
bool HasAllocator() const {
return allocator_ != 0;
template <typename T> const T *End() const {
return reinterpret_cast<T *>(stackTop_);
}
template <typename T> T *Bottom() { return reinterpret_cast<T *>(stack_); }
template <typename T> const T *Bottom() const {
return reinterpret_cast<T *>(stack_);
}
bool HasAllocator() const { return allocator_ != 0; }
Allocator &GetAllocator() {
RAPIDJSON_ASSERT(allocator_);
return *allocator_;
@@ -179,9 +170,9 @@ public:
size_t GetCapacity() const { return static_cast<size_t>(stackEnd_ - stack_); }
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.
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_)
@@ -200,7 +191,8 @@ private:
void Resize(size_t newCapacity) {
const size_t size = GetSize(); // Backup the current size
stack_ = static_cast<char*>(allocator_->Realloc(stack_, GetCapacity(), newCapacity));
stack_ = static_cast<char *>(
allocator_->Realloc(stack_, GetCapacity(), newCapacity));
stackTop_ = stack_ + size;
stackEnd_ = stack_ + newCapacity;
}

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_INTERNAL_STRFUNC_H_
#define RAPIDJSON_INTERNAL_STRFUNC_H_
@@ -25,29 +29,29 @@ namespace internal {
/*! \tparam Ch Character type (e.g. char, wchar_t, short)
\param s Null-terminated input string.
\return Number of characters in the string.
\note This has the same semantics as strlen(), the return value is not number of Unicode codepoints.
\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));
}
//! Returns number of code points in a encoded string.
template <typename Encoding>
bool CountStringCodePoint(const typename Encoding::Ch* s, SizeType length, SizeType* outCount) {
bool CountStringCodePoint(const typename Encoding::Ch *s, SizeType length,
SizeType *outCount) {
RAPIDJSON_ASSERT(s != 0);
RAPIDJSON_ASSERT(outCount != 0);
GenericStringStream<Encoding> is(s);

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

@@ -1,23 +1,27 @@
// Tencent is pleased to support the open source community by making RapidJSON available.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_STRTOD_
#define RAPIDJSON_STRTOD_
#include "ieee754.h"
#include "biginteger.h"
#include "diyfp.h"
#include "ieee754.h"
#include "pow10.h"
#include <climits>
#include <limits>
@@ -39,17 +43,17 @@ inline double StrtodNormalPrecision(double d, int p) {
// Prevent expSum < -308, making Pow10(p) = 0
d = FastPath(d, -308);
d = FastPath(d, p + 308);
}
else
} else
d = FastPath(d, 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;
}
@@ -59,14 +63,14 @@ inline int CheckWithinHalfULP(double b, const BigInteger& d, int dExp) {
const int bExp = db.IntegerExponent();
const int hExp = bExp - 1;
int dS_Exp2 = 0, dS_Exp5 = 0, bS_Exp2 = 0, bS_Exp5 = 0, hS_Exp2 = 0, hS_Exp5 = 0;
int dS_Exp2 = 0, dS_Exp5 = 0, bS_Exp2 = 0, bS_Exp5 = 0, hS_Exp2 = 0,
hS_Exp5 = 0;
// Adjust for decimal exponent
if (dExp >= 0) {
dS_Exp2 += dExp;
dS_Exp5 += dExp;
}
else {
} else {
bS_Exp2 -= dExp;
bS_Exp5 -= dExp;
hS_Exp2 -= dExp;
@@ -96,13 +100,16 @@ inline int CheckWithinHalfULP(double b, const BigInteger& d, int dExp) {
hS_Exp2 -= common_Exp2;
BigInteger dS = d;
dS.MultiplyPow5(static_cast<unsigned>(dS_Exp5)) <<= static_cast<unsigned>(dS_Exp2);
dS.MultiplyPow5(static_cast<unsigned>(dS_Exp5)) <<=
static_cast<unsigned>(dS_Exp2);
BigInteger bS(bInt);
bS.MultiplyPow5(static_cast<unsigned>(bS_Exp5)) <<= static_cast<unsigned>(bS_Exp2);
bS.MultiplyPow5(static_cast<unsigned>(bS_Exp5)) <<=
static_cast<unsigned>(bS_Exp2);
BigInteger hS(1);
hS.MultiplyPow5(static_cast<unsigned>(hS_Exp5)) <<= static_cast<unsigned>(hS_Exp2);
hS.MultiplyPow5(static_cast<unsigned>(hS_Exp5)) <<=
static_cast<unsigned>(hS_Exp2);
BigInteger delta(0);
dS.Difference(bS, &delta);
@@ -112,7 +119,8 @@ inline int CheckWithinHalfULP(double b, const BigInteger& d, int dExp) {
inline bool StrtodFast(double d, int p, double *result) {
// Use fast path for string-to-double conversion if possible
// see http://www.exploringbinary.com/fast-path-decimal-to-floating-point-conversion/
// see
// http://www.exploringbinary.com/fast-path-decimal-to-floating-point-conversion/
if (p > 22 && p < 22 + 16) {
// Fast Path Cases In Disguise
d *= internal::Pow10(p - 22);
@@ -122,18 +130,20 @@ inline bool StrtodFast(double d, int p, double* result) {
if (p >= -22 && p <= 22 && d <= 9007199254740991.0) { // 2^53 - 1
*result = FastPath(d, p);
return true;
}
else
} else
return false;
}
// Compute an approximation and see if it is within 1/2 ULP
inline bool StrtodDiyFp(const char* decimals, int dLen, int dExp, 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) && decimals[i] > '5'))
(significand == RAPIDJSON_UINT64_C2(0x19999999, 0x99999999) &&
decimals[i] > '5'))
break;
significand = significand * 10u + static_cast<unsigned>(decimals[i] - '0');
}
@@ -179,7 +189,8 @@ inline bool StrtodDiyFp(const char* decimals, int dLen, int dExp, double* result
v = v.Normalize();
error <<= oldExp - v.e;
const int effectiveSignificandSize = Double::EffectiveSignificandSize(64 + v.e);
const int effectiveSignificandSize =
Double::EffectiveSignificandSize(64 + v.e);
int precisionSize = 64 - effectiveSignificandSize;
if (precisionSize + kUlpShift >= 64) {
int scaleExp = (precisionSize + kUlpShift) - 63;
@@ -190,11 +201,13 @@ inline bool StrtodDiyFp(const char* decimals, int dLen, int dExp, double* result
}
DiyFp rounded(v.f >> precisionSize, v.e + precisionSize);
const uint64_t precisionBits = (v.f & ((uint64_t(1) << precisionSize) - 1)) * kUlp;
const uint64_t precisionBits =
(v.f & ((uint64_t(1) << precisionSize) - 1)) * kUlp;
const uint64_t halfWay = (uint64_t(1) << (precisionSize - 1)) * kUlp;
if (precisionBits >= halfWay + static_cast<unsigned>(error)) {
rounded.f++;
if (rounded.f & (DiyFp::kDpHiddenBit << 1)) { // rounding overflows mantissa (issue #340)
if (rounded.f & (DiyFp::kDpHiddenBit
<< 1)) { // rounding overflows mantissa (issue #340)
rounded.f >>= 1;
rounded.e++;
}
@@ -202,10 +215,12 @@ inline bool StrtodDiyFp(const char* decimals, int dLen, int dExp, double* result
*result = rounded.ToDouble();
return halfWay - static_cast<unsigned>(error) >= precisionBits || precisionBits >= halfWay + static_cast<unsigned>(error);
return halfWay - static_cast<unsigned>(error) >= precisionBits ||
precisionBits >= halfWay + static_cast<unsigned>(error);
}
inline double StrtodBigInteger(double approx, const char* decimals, int dLen, int dExp) {
inline double StrtodBigInteger(double approx, const char *decimals, int dLen,
int dExp) {
RAPIDJSON_ASSERT(dLen >= 0);
const BigInteger dInt(decimals, static_cast<unsigned>(dLen));
Double a(approx);
@@ -218,12 +233,13 @@ inline double StrtodBigInteger(double approx, const char* decimals, int dLen, in
return a.NextPositiveDouble();
else
return a.Value();
}
else // adjustment
} else // adjustment
return a.NextPositiveDouble();
}
inline double StrtodFullPrecision(double d, int p, const char* decimals, size_t length, size_t decimalPosition, int exp) {
inline double StrtodFullPrecision(double d, int p, const char *decimals,
size_t length, size_t decimalPosition,
int exp) {
RAPIDJSON_ASSERT(d >= 0.0);
RAPIDJSON_ASSERT(length >= 1);
@@ -280,7 +296,8 @@ inline double StrtodFullPrecision(double d, int p, const char* decimals, size_t
if (StrtodDiyFp(decimals, dLen, dExp, &result))
return result;
// Use approximation from StrtodDiyFp and make adjustment with BigInteger comparison
// Use approximation from StrtodDiyFp and make adjustment with BigInteger
// comparison
return StrtodBigInteger(result, decimals, dLen, dExp);
}

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_INTERNAL_SWAP_H_
#define RAPIDJSON_INTERNAL_SWAP_H_
@@ -26,11 +30,10 @@ RAPIDJSON_NAMESPACE_BEGIN
namespace internal {
//! Custom swap() to avoid dependency on C++ <algorithm> header
/*! \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().
/*! \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;

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

@@ -1,30 +1,35 @@
// Tencent is pleased to support the open source community by making RapidJSON available.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_ISTREAMWRAPPER_H_
#define RAPIDJSON_ISTREAMWRAPPER_H_
#include "stream.h"
#include <iosfwd>
#include <ios>
#include <iosfwd>
#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
@@ -45,8 +50,7 @@ RAPIDJSON_NAMESPACE_BEGIN
\tparam StreamType Class derived from \c std::basic_istream.
*/
template <typename StreamType>
class BasicIStreamWrapper {
template <typename StreamType> class BasicIStreamWrapper {
public:
typedef typename StreamType::char_type Ch;
@@ -54,7 +58,9 @@ 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) {
BasicIStreamWrapper(StreamType &stream)
: stream_(stream), buffer_(peekBuffer_), bufferSize_(4), bufferLast_(0),
current_(buffer_), readCount_(0), count_(0), eof_(false) {
Read();
}
@@ -64,20 +70,35 @@ public:
\param buffer user-supplied buffer.
\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), eof_(false) {
BasicIStreamWrapper(StreamType &stream, char *buffer, size_t bufferSize)
: stream_(stream), buffer_(buffer), bufferSize_(bufferSize),
bufferLast_(0), current_(buffer_), readCount_(0), count_(0),
eof_(false) {
RAPIDJSON_ASSERT(bufferSize >= 4);
Read();
}
Ch Peek() const { return *current_; }
Ch Take() { Ch c = *current_; Read(); return c; }
size_t Tell() const { return count_ + static_cast<size_t>(current_ - buffer_); }
Ch Take() {
Ch c = *current_;
Read();
return c;
}
size_t Tell() const {
return count_ + static_cast<size_t>(current_ - buffer_);
}
// Not implemented
void Put(Ch) { RAPIDJSON_ASSERT(false); }
void Flush() { RAPIDJSON_ASSERT(false); }
Ch* PutBegin() { RAPIDJSON_ASSERT(false); return 0; }
size_t PutEnd(Ch*) { RAPIDJSON_ASSERT(false); return 0; }
Ch *PutBegin() {
RAPIDJSON_ASSERT(false);
return 0;
}
size_t PutEnd(Ch *) {
RAPIDJSON_ASSERT(false);
return 0;
}
// For encoding detection only.
const Ch *Peek4() const {

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

@@ -1,43 +1,51 @@
// Tencent is pleased to support the open source community by making RapidJSON available.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_MEMORYBUFFER_H_
#define RAPIDJSON_MEMORYBUFFER_H_
#include "stream.h"
#include "internal/stack.h"
#include "stream.h"
RAPIDJSON_NAMESPACE_BEGIN
//! Represents an in-memory output byte stream.
/*!
This class is mainly for being wrapped by EncodedOutputStream or AutoUTFOutputStream.
This class is mainly for being wrapped by EncodedOutputStream or
AutoUTFOutputStream.
It is similar to FileWriteBuffer but the destination is an in-memory buffer instead of a file.
It is similar to FileWriteBuffer but the destination is an in-memory buffer
instead of a file.
Differences between MemoryBuffer and StringBuffer:
1. StringBuffer has Encoding but MemoryBuffer is only a byte buffer.
2. StringBuffer::GetString() returns a null-terminated string. MemoryBuffer::GetBuffer() returns a buffer without terminator.
2. StringBuffer::GetString() returns a null-terminated string.
MemoryBuffer::GetBuffer() returns a buffer without terminator.
\tparam Allocator type for allocating memory buffer.
\note implements Stream concept
*/
template <typename Allocator = CrtAllocator>
struct GenericMemoryBuffer {
template <typename Allocator = CrtAllocator> struct GenericMemoryBuffer {
typedef char Ch; // byte
GenericMemoryBuffer(Allocator* allocator = 0, size_t capacity = kDefaultCapacity) : stack_(allocator, capacity) {}
GenericMemoryBuffer(Allocator *allocator = 0,
size_t capacity = kDefaultCapacity)
: stack_(allocator, capacity) {}
void Put(Ch c) { *stack_.template Push<Ch>() = c; }
void Flush() {}
@@ -47,9 +55,7 @@ struct GenericMemoryBuffer {
Ch *Push(size_t count) { return stack_.template Push<Ch>(count); }
void Pop(size_t count) { stack_.template Pop<Ch>(count); }
const Ch* GetBuffer() const {
return stack_.template Bottom<Ch>();
}
const Ch *GetBuffer() const { return stack_.template Bottom<Ch>(); }
size_t GetSize() const { return stack_.GetSize(); }
@@ -59,9 +65,9 @@ struct GenericMemoryBuffer {
typedef GenericMemoryBuffer<> MemoryBuffer;
//! Implement specialized version of PutN() with memset() for better performance.
template<>
inline void PutN(MemoryBuffer& memoryBuffer, char c, size_t n) {
//! Implement specialized version of PutN() with memset() for better
//! performance.
template <> inline void PutN(MemoryBuffer &memoryBuffer, char c, size_t n) {
std::memset(memoryBuffer.stack_.Push<char>(n), c, n * sizeof(c));
}

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_MEMORYSTREAM_H_
#define RAPIDJSON_MEMORYSTREAM_H_
@@ -27,34 +31,43 @@ RAPIDJSON_NAMESPACE_BEGIN
//! Represents an in-memory input byte stream.
/*!
This class is mainly for being wrapped by EncodedInputStream or AutoUTFInputStream.
This class is mainly for being wrapped by EncodedInputStream or
AutoUTFInputStream.
It is similar to FileReadBuffer but the source is an in-memory buffer instead of a file.
It is similar to FileReadBuffer but the source is an in-memory buffer
instead of a file.
Differences between MemoryStream and StringStream:
1. StringStream has encoding but MemoryStream is a byte stream.
2. MemoryStream needs size of the source buffer and the buffer don't need to be null terminated. StringStream assume null-terminated string as source.
3. MemoryStream supports Peek4() for encoding detection. StringStream is specified with an encoding so it should not have Peek4().
\note implements Stream concept
2. MemoryStream needs size of the source buffer and the buffer don't need to
be null terminated. StringStream assume null-terminated string as source.
3. MemoryStream supports Peek4() for encoding detection. StringStream is
specified with an encoding so it should not have Peek4(). \note implements
Stream concept
*/
struct MemoryStream {
typedef char Ch; // byte
MemoryStream(const Ch *src, size_t size) : src_(src), begin_(src), end_(src + size), size_(size) {}
MemoryStream(const Ch *src, size_t size)
: src_(src), begin_(src), end_(src + size), size_(size) {}
Ch Peek() const { return RAPIDJSON_UNLIKELY(src_ == end_) ? '\0' : *src_; }
Ch Take() { return RAPIDJSON_UNLIKELY(src_ == end_) ? '\0' : *src_++; }
size_t Tell() const { return static_cast<size_t>(src_ - begin_); }
Ch* PutBegin() { RAPIDJSON_ASSERT(false); return 0; }
Ch *PutBegin() {
RAPIDJSON_ASSERT(false);
return 0;
}
void Put(Ch) { RAPIDJSON_ASSERT(false); }
void Flush() { RAPIDJSON_ASSERT(false); }
size_t PutEnd(Ch*) { RAPIDJSON_ASSERT(false); return 0; }
size_t PutEnd(Ch *) {
RAPIDJSON_ASSERT(false);
return 0;
}
// For encoding detection only.
const Ch* Peek4() const {
return Tell() + 4 <= size_ ? src_ : 0;
}
const Ch *Peek4() const { return Tell() + 4 <= size_ ? src_ : 0; }
const Ch *src_; //!< Current read position.
const Ch *begin_; //!< Original head of the string.

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

@@ -61,7 +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"
@@ -287,8 +288,7 @@ static
#else // STATIC_IMAXDIV ][
_inline
#endif // STATIC_IMAXDIV ]
imaxdiv_t __cdecl imaxdiv(intmax_t numer, intmax_t denom)
{
imaxdiv_t __cdecl imaxdiv(intmax_t numer, intmax_t denom) {
imaxdiv_t result;
result.quot = numer / denom;

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

@@ -45,11 +45,13 @@
#pragma once
#endif
// 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.
// 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 // [
#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
@@ -109,7 +111,6 @@ extern "C" {
#endif
#endif
// 7.18.1 Integer types
// 7.18.1.1 Exact-width integer types
@@ -135,7 +136,6 @@ extern "C" {
typedef signed __int64 int64_t;
typedef unsigned __int64 uint64_t;
// 7.18.1.2 Minimum-width integer types
typedef int8_t int_least8_t;
typedef int16_t int_least16_t;
@@ -169,10 +169,11 @@ typedef uint64_t uint_fast64_t;
typedef int64_t intmax_t;
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)
@@ -266,10 +267,10 @@ typedef uint64_t uintmax_t;
#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

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_OSTREAMWRAPPER_H_
#define RAPIDJSON_OSTREAMWRAPPER_H_
@@ -41,26 +45,36 @@ RAPIDJSON_NAMESPACE_BEGIN
\tparam StreamType Class derived from \c std::basic_ostream.
*/
template <typename StreamType>
class BasicOStreamWrapper {
template <typename StreamType> class BasicOStreamWrapper {
public:
typedef typename StreamType::char_type Ch;
BasicOStreamWrapper(StreamType &stream) : stream_(stream) {}
void Put(Ch c) {
stream_.put(c);
}
void Put(Ch c) { stream_.put(c); }
void Flush() {
stream_.flush();
}
void Flush() { stream_.flush(); }
// Not implemented
char Peek() const { RAPIDJSON_ASSERT(false); return 0; }
char Take() { RAPIDJSON_ASSERT(false); return 0; }
size_t Tell() const { RAPIDJSON_ASSERT(false); return 0; }
char* PutBegin() { RAPIDJSON_ASSERT(false); return 0; }
size_t PutEnd(char*) { RAPIDJSON_ASSERT(false); return 0; }
char Peek() const {
RAPIDJSON_ASSERT(false);
return 0;
}
char Take() {
RAPIDJSON_ASSERT(false);
return 0;
}
size_t Tell() const {
RAPIDJSON_ASSERT(false);
return 0;
}
char *PutBegin() {
RAPIDJSON_ASSERT(false);
return 0;
}
size_t PutEnd(char *) {
RAPIDJSON_ASSERT(false);
return 0;
}
private:
BasicOStreamWrapper(const BasicOStreamWrapper &);

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

File diff suppressed because it is too large Load Diff

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_PRETTYWRITER_H_
#define RAPIDJSON_PRETTYWRITER_H_
@@ -44,36 +48,48 @@ enum PrettyFormatOptions {
\tparam TargetEncoding Encoding of output stream.
\tparam StackAllocator Type of allocator for allocating memory of stack.
*/
template<typename OutputStream, typename SourceEncoding = UTF8<>, typename TargetEncoding = UTF8<>, typename StackAllocator = CrtAllocator, unsigned writeFlags = kWriteDefaultFlags>
class PrettyWriter : public Writer<OutputStream, SourceEncoding, TargetEncoding, StackAllocator, writeFlags> {
template <typename OutputStream, typename SourceEncoding = UTF8<>,
typename TargetEncoding = UTF8<>,
typename StackAllocator = CrtAllocator,
unsigned writeFlags = kWriteDefaultFlags>
class PrettyWriter : public Writer<OutputStream, SourceEncoding, TargetEncoding,
StackAllocator, writeFlags> {
public:
typedef Writer<OutputStream, SourceEncoding, TargetEncoding, StackAllocator, writeFlags> Base;
typedef Writer<OutputStream, SourceEncoding, TargetEncoding, StackAllocator,
writeFlags>
Base;
typedef typename Base::Ch Ch;
//! Constructor
/*! \param os Output stream.
\param allocator User supplied allocator. If it is null, it will create a private one.
\param levelDepth Initial capacity of stack.
\param allocator User supplied allocator. If it is null, it will create a
private one. \param levelDepth Initial capacity of stack.
*/
explicit PrettyWriter(OutputStream& os, StackAllocator* allocator = 0, size_t levelDepth = Base::kDefaultLevelDepth) :
Base(os, allocator, levelDepth), indentChar_(' '), indentCharCount_(4), formatOptions_(kFormatDefault) {}
explicit PrettyWriter(OutputStream &os, StackAllocator *allocator = 0,
size_t levelDepth = Base::kDefaultLevelDepth)
: Base(os, allocator, levelDepth), indentChar_(' '), indentCharCount_(4),
formatOptions_(kFormatDefault) {}
explicit PrettyWriter(StackAllocator* allocator = 0, size_t levelDepth = Base::kDefaultLevelDepth) :
Base(allocator, levelDepth), indentChar_(' '), indentCharCount_(4) {}
explicit PrettyWriter(StackAllocator *allocator = 0,
size_t levelDepth = Base::kDefaultLevelDepth)
: Base(allocator, levelDepth), indentChar_(' '), indentCharCount_(4) {}
#if RAPIDJSON_HAS_CXX11_RVALUE_REFS
PrettyWriter(PrettyWriter&& rhs) :
Base(std::forward<PrettyWriter>(rhs)), indentChar_(rhs.indentChar_), indentCharCount_(rhs.indentCharCount_), formatOptions_(rhs.formatOptions_) {}
PrettyWriter(PrettyWriter &&rhs)
: Base(std::forward<PrettyWriter>(rhs)), indentChar_(rhs.indentChar_),
indentCharCount_(rhs.indentCharCount_),
formatOptions_(rhs.formatOptions_) {}
#endif
//! Set custom indentation.
/*! \param indentChar Character for indentation. Must be whitespace character (' ', '\\t', '\\n', '\\r').
\param indentCharCount Number of indent characters for each indentation level.
\note The default indentation is 4 spaces.
/*! \param indentChar Character for indentation. Must be whitespace
character (' ', '\\t', '\\n', '\\r'). \param indentCharCount Number of
indent characters for each indentation level. \note The default indentation
is 4 spaces.
*/
PrettyWriter &SetIndent(Ch indentChar, unsigned indentCharCount) {
RAPIDJSON_ASSERT(indentChar == ' ' || indentChar == '\t' || indentChar == '\n' || indentChar == '\r');
RAPIDJSON_ASSERT(indentChar == ' ' || indentChar == '\t' ||
indentChar == '\n' || indentChar == '\r');
indentChar_ = indentChar;
indentCharCount_ = indentCharCount;
return *this;
@@ -92,13 +108,34 @@ public:
*/
//@{
bool Null() { PrettyPrefix(kNullType); return Base::EndValue(Base::WriteNull()); }
bool Bool(bool b) { PrettyPrefix(b ? kTrueType : kFalseType); return Base::EndValue(Base::WriteBool(b)); }
bool Int(int i) { PrettyPrefix(kNumberType); return Base::EndValue(Base::WriteInt(i)); }
bool Uint(unsigned u) { PrettyPrefix(kNumberType); return Base::EndValue(Base::WriteUint(u)); }
bool Int64(int64_t i64) { PrettyPrefix(kNumberType); return Base::EndValue(Base::WriteInt64(i64)); }
bool Uint64(uint64_t u64) { PrettyPrefix(kNumberType); return Base::EndValue(Base::WriteUint64(u64)); }
bool Double(double d) { PrettyPrefix(kNumberType); return Base::EndValue(Base::WriteDouble(d)); }
bool Null() {
PrettyPrefix(kNullType);
return Base::EndValue(Base::WriteNull());
}
bool Bool(bool b) {
PrettyPrefix(b ? kTrueType : kFalseType);
return Base::EndValue(Base::WriteBool(b));
}
bool Int(int i) {
PrettyPrefix(kNumberType);
return Base::EndValue(Base::WriteInt(i));
}
bool Uint(unsigned u) {
PrettyPrefix(kNumberType);
return Base::EndValue(Base::WriteUint(u));
}
bool Int64(int64_t i64) {
PrettyPrefix(kNumberType);
return Base::EndValue(Base::WriteInt64(i64));
}
bool Uint64(uint64_t u64) {
PrettyPrefix(kNumberType);
return Base::EndValue(Base::WriteUint64(u64));
}
bool Double(double d) {
PrettyPrefix(kNumberType);
return Base::EndValue(Base::WriteDouble(d));
}
bool RawNumber(const Ch *str, SizeType length, bool copy = false) {
RAPIDJSON_ASSERT(str != 0);
@@ -122,11 +159,14 @@ public:
bool StartObject() {
PrettyPrefix(kObjectType);
new (Base::level_stack_.template Push<typename Base::Level>()) typename Base::Level(false);
new (Base::level_stack_.template Push<typename Base::Level>())
typename Base::Level(false);
return Base::WriteStartObject();
}
bool Key(const Ch* str, SizeType length, bool copy = false) { return String(str, length, copy); }
bool Key(const Ch *str, SizeType length, bool copy = false) {
return String(str, length, copy);
}
#if RAPIDJSON_HAS_STDSTRING
bool Key(const std::basic_string<Ch> &str) {
@@ -136,11 +176,18 @@ public:
bool EndObject(SizeType memberCount = 0) {
(void)memberCount;
RAPIDJSON_ASSERT(Base::level_stack_.GetSize() >= 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
RAPIDJSON_ASSERT(0 == Base::level_stack_.template Top<typename Base::Level>()->valueCount % 2); // Object has a Key without a Value
RAPIDJSON_ASSERT(Base::level_stack_.GetSize() >=
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
RAPIDJSON_ASSERT(
0 ==
Base::level_stack_.template Top<typename Base::Level>()->valueCount %
2); // Object has a Key without a Value
bool empty = Base::level_stack_.template Pop<typename Base::Level>(1)->valueCount == 0;
bool empty =
Base::level_stack_.template Pop<typename Base::Level>(1)->valueCount ==
0;
if (!empty) {
Base::os_->Put('\n');
@@ -156,15 +203,20 @@ public:
bool StartArray() {
PrettyPrefix(kArrayType);
new (Base::level_stack_.template Push<typename Base::Level>()) typename Base::Level(true);
new (Base::level_stack_.template Push<typename Base::Level>())
typename Base::Level(true);
return Base::WriteStartArray();
}
bool EndArray(SizeType memberCount = 0) {
(void)memberCount;
RAPIDJSON_ASSERT(Base::level_stack_.GetSize() >= sizeof(typename Base::Level));
RAPIDJSON_ASSERT(Base::level_stack_.template Top<typename Base::Level>()->inArray);
bool empty = Base::level_stack_.template Pop<typename Base::Level>(1)->valueCount == 0;
RAPIDJSON_ASSERT(Base::level_stack_.GetSize() >=
sizeof(typename Base::Level));
RAPIDJSON_ASSERT(
Base::level_stack_.template Top<typename Base::Level>()->inArray);
bool empty =
Base::level_stack_.template Pop<typename Base::Level>(1)->valueCount ==
0;
if (!empty && !(formatOptions_ & kFormatSingleLineArray)) {
Base::os_->Put('\n');
@@ -193,10 +245,10 @@ public:
/*!
For user to write a stringified JSON as a value.
\param json A well-formed JSON value. It should not contain null character within [0, length - 1] range.
\param length Length of the json.
\param type Type of the root of json.
\note When using PrettyWriter::RawValue(), the result json may not be indented correctly.
\param json A well-formed JSON value. It should not contain null character
within [0, length - 1] range. \param length Length of the json. \param type
Type of the root of json. \note When using PrettyWriter::RawValue(), the
result json may not be indented correctly.
*/
bool RawValue(const Ch *json, size_t length, Type type) {
RAPIDJSON_ASSERT(json != 0);
@@ -208,11 +260,13 @@ protected:
void PrettyPrefix(Type type) {
(void)type;
if (Base::level_stack_.GetSize() != 0) { // this value is not at root
typename Base::Level* level = Base::level_stack_.template Top<typename Base::Level>();
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
Base::os_->Put(
','); // add comma if it is not the first element in array
if (formatOptions_ & kFormatSingleLineArray)
Base::os_->Put(' ');
}
@@ -221,37 +275,38 @@ protected:
Base::os_->Put('\n');
WriteIndent();
}
}
else { // in object
} else { // in object
if (level->valueCount > 0) {
if (level->valueCount % 2 == 0) {
Base::os_->Put(',');
Base::os_->Put('\n');
}
else {
} else {
Base::os_->Put(':');
Base::os_->Put(' ');
}
}
else
} else
Base::os_->Put('\n');
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.
} else {
RAPIDJSON_ASSERT(
!Base::hasRoot_); // Should only has one and only one root.
Base::hasRoot_ = true;
}
}
void WriteIndent() {
size_t count = (Base::level_stack_.GetSize() / sizeof(typename Base::Level)) * indentCharCount_;
PutN(*Base::os_, static_cast<typename OutputStream::Ch>(indentChar_), count);
size_t count =
(Base::level_stack_.GetSize() / sizeof(typename Base::Level)) *
indentCharCount_;
PutN(*Base::os_, static_cast<typename OutputStream::Ch>(indentChar_),
count);
}
Ch indentChar_;

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_RAPIDJSON_H_
#define RAPIDJSON_RAPIDJSON_H_
@@ -29,7 +33,8 @@
features can be configured in terms of overridden or predefined
preprocessor macros at compile-time.
Some additional customization is available in the \ref RAPIDJSON_ERRORS APIs.
Some additional customization is available in the \ref RAPIDJSON_ERRORS
APIs.
\note These macros should be given on the compiler command-line
(where applicable) to avoid inconsistent values when compiling
@@ -42,7 +47,8 @@
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_VERSION_STRING
//
// ALWAYS synchronize the following 3 macros with corresponding variables in /CMakeLists.txt.
// ALWAYS synchronize the following 3 macros with corresponding variables in
// /CMakeLists.txt.
//
//!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN
@@ -76,7 +82,8 @@
#define RAPIDJSON_MINOR_VERSION 1
#define RAPIDJSON_PATCH_VERSION 0
#define RAPIDJSON_VERSION_STRING \
RAPIDJSON_STRINGIFY(RAPIDJSON_MAJOR_VERSION.RAPIDJSON_MINOR_VERSION.RAPIDJSON_PATCH_VERSION)
RAPIDJSON_STRINGIFY( \
RAPIDJSON_MAJOR_VERSION.RAPIDJSON_MINOR_VERSION.RAPIDJSON_PATCH_VERSION)
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_NAMESPACE_(BEGIN|END)
@@ -137,9 +144,9 @@
\ingroup RAPIDJSON_CONFIG
\brief Enable RapidJSON support for \c std::string
By defining this preprocessor symbol to \c 1, several convenience functions for using
\ref rapidjson::GenericValue with \c std::string are enabled, especially
for construction and comparison.
By defining this preprocessor symbol to \c 1, several convenience functions
for using \ref rapidjson::GenericValue with \c std::string are enabled,
especially for construction and comparison.
\hideinitializer
*/
@@ -156,8 +163,8 @@
\ingroup RAPIDJSON_CONFIG
\brief Use external 64-bit integer types.
RapidJSON requires the 64-bit integer types \c int64_t and \c uint64_t types
to be available at global scope.
RapidJSON requires the 64-bit integer types \c int64_t and \c uint64_t
types to be available at global scope.
If users have their own definition, define RAPIDJSON_NO_INT64DEFINE to
prevent RapidJSON from defining its own types.
@@ -165,12 +172,12 @@
#ifndef RAPIDJSON_NO_INT64DEFINE
//!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN
#if defined(_MSC_VER) && (_MSC_VER < 1800) // Visual Studio 2013
#include "msinttypes/stdint.h"
#include "msinttypes/inttypes.h"
#include "msinttypes/stdint.h"
#else
// Other compilers should have this.
#include <stdint.h>
#include <inttypes.h>
#include <stdint.h>
#endif
//!@endcond
#ifdef RAPIDJSON_DOXYGEN_RUNNING
@@ -203,12 +210,13 @@
\def RAPIDJSON_ENDIAN
\ingroup RAPIDJSON_CONFIG
GCC 4.6 provided macro for detecting endianness of the target machine. But other
compilers may not have this. User can define RAPIDJSON_ENDIAN to either
GCC 4.6 provided macro for detecting endianness of the target machine. But
other compilers may not have this. User can define RAPIDJSON_ENDIAN to either
\ref RAPIDJSON_LITTLEENDIAN or \ref RAPIDJSON_BIGENDIAN.
Default detection implemented with reference to
\li https://gcc.gnu.org/onlinedocs/gcc-4.6.0/cpp/Common-Predefined-Macros.html
\li
https://gcc.gnu.org/onlinedocs/gcc-4.6.0/cpp/Common-Predefined-Macros.html
\li http://www.boost.org/doc/libs/1_42_0/boost/detail/endian.hpp
*/
#ifndef RAPIDJSON_ENDIAN
@@ -237,9 +245,16 @@
#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) || defined(__s390__)
#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__) || defined(_M_X64) || defined(__bfin__)
#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))
#define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN
@@ -255,7 +270,8 @@
//! Whether using 64-bit architecture
#ifndef RAPIDJSON_64BIT
#if defined(__LP64__) || (defined(__x86_64__) && defined(__ILP32__)) || defined(_WIN64) || defined(__EMSCRIPTEN__)
#if defined(__LP64__) || (defined(__x86_64__) && defined(__ILP32__)) || \
defined(_WIN64) || defined(__EMSCRIPTEN__)
#define RAPIDJSON_64BIT 1
#else
#define RAPIDJSON_64BIT 0
@@ -273,7 +289,8 @@
User can customize by defining the RAPIDJSON_ALIGN function macro.
*/
#ifndef RAPIDJSON_ALIGN
#define RAPIDJSON_ALIGN(x) (((x) + static_cast<size_t>(7u)) & ~static_cast<size_t>(7u))
#define RAPIDJSON_ALIGN(x) \
(((x) + static_cast<size_t>(7u)) & ~static_cast<size_t>(7u))
#endif
///////////////////////////////////////////////////////////////////////////////
@@ -286,7 +303,8 @@
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) ((static_cast<uint64_t>(high32) << 32) | static_cast<uint64_t>(low32))
#define RAPIDJSON_UINT64_C2(high32, low32) \
((static_cast<uint64_t>(high32) << 32) | static_cast<uint64_t>(low32))
#endif
///////////////////////////////////////////////////////////////////////////////
@@ -296,12 +314,14 @@
/*!
\ingroup RAPIDJSON_CONFIG
This optimization uses the fact that current X86-64 architecture only implement lower 48-bit virtual address.
The higher 16-bit can be used for storing other data.
\c GenericValue uses this optimization to reduce its size form 24 bytes to 16 bytes in 64-bit architecture.
This optimization uses the fact that current X86-64 architecture only
implement lower 48-bit virtual address. The higher 16-bit can be used for
storing other data. \c GenericValue uses this optimization to reduce its size
form 24 bytes to 16 bytes in 64-bit architecture.
*/
#ifndef RAPIDJSON_48BITPOINTER_OPTIMIZATION
#if defined(__amd64__) || defined(__amd64) || defined(__x86_64__) || defined(__x86_64) || defined(_M_X64) || defined(_M_AMD64)
#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
@@ -312,8 +332,15 @@
#if RAPIDJSON_64BIT != 1
#error RAPIDJSON_48BITPOINTER_OPTIMIZATION can only be set to 1 when RAPIDJSON_64BIT=1
#endif
#define RAPIDJSON_SETPOINTER(type, p, x) (p = reinterpret_cast<type *>((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) & static_cast<uintptr_t>(RAPIDJSON_UINT64_C2(0x0000FFFF, 0xFFFFFFFF))))
#define RAPIDJSON_SETPOINTER(type, p, x) \
(p = reinterpret_cast<type *>( \
(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) & \
static_cast<uintptr_t>(RAPIDJSON_UINT64_C2(0x0000FFFF, 0xFFFFFFFF))))
#else
#define RAPIDJSON_SETPOINTER(type, p, x) (p = (x))
#define RAPIDJSON_GETPOINTER(type, p) (p)
@@ -348,8 +375,8 @@
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) \
|| defined(RAPIDJSON_NEON) || defined(RAPIDJSON_DOXYGEN_RUNNING)
#if defined(RAPIDJSON_SSE2) || defined(RAPIDJSON_SSE42) || \
defined(RAPIDJSON_NEON) || defined(RAPIDJSON_DOXYGEN_RUNNING)
#define RAPIDJSON_SIMD
#endif
@@ -412,8 +439,7 @@ RAPIDJSON_NAMESPACE_END
// Prefer C++11 static_assert, if available
#ifndef RAPIDJSON_STATIC_ASSERT
#if __cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1800)
#define RAPIDJSON_STATIC_ASSERT(x) \
static_assert(x, RAPIDJSON_STRINGIFY(x))
#define RAPIDJSON_STATIC_ASSERT(x) static_assert(x, RAPIDJSON_STRINGIFY(x))
#endif // C++11
#endif // RAPIDJSON_STATIC_ASSERT
@@ -424,7 +450,9 @@ RAPIDJSON_NAMESPACE_END
#endif
RAPIDJSON_NAMESPACE_BEGIN
template <bool x> struct STATIC_ASSERTION_FAILURE;
template <> struct STATIC_ASSERTION_FAILURE<true> { enum { value = 1 }; };
template <> struct STATIC_ASSERTION_FAILURE<true> {
enum { value = 1 };
};
template <size_t x> struct StaticAssertTest {};
RAPIDJSON_NAMESPACE_END
@@ -443,9 +471,10 @@ RAPIDJSON_NAMESPACE_END
\hideinitializer
*/
#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
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
///////////////////////////////////////////////////////////////////////////////
@@ -484,11 +513,11 @@ RAPIDJSON_NAMESPACE_END
#define RAPIDJSON_MULTILINEMACRO_BEGIN do {
#define RAPIDJSON_MULTILINEMACRO_END \
} while((void)0, 0)
} \
while ((void)0, 0)
// adopted from Boost
#define RAPIDJSON_VERSION_CODE(x,y,z) \
(((x)*100000) + ((y)*100) + (z))
#define RAPIDJSON_VERSION_CODE(x, y, z) (((x)*100000) + ((y)*100) + (z))
#if defined(__has_builtin)
#define RAPIDJSON_HAS_BUILTIN(x) __has_builtin(x)
@@ -504,7 +533,8 @@ RAPIDJSON_NAMESPACE_END
RAPIDJSON_VERSION_CODE(__GNUC__, __GNUC_MINOR__, __GNUC_PATCHLEVEL__)
#endif
#if defined(__clang__) || (defined(RAPIDJSON_GNUC) && RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4,2,0))
#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)
@@ -512,7 +542,8 @@ RAPIDJSON_NAMESPACE_END
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) && RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4,6,0))
#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)
#else // GCC >= 4.2, < 4.6
@@ -544,14 +575,18 @@ RAPIDJSON_NAMESPACE_END
#ifndef RAPIDJSON_HAS_CXX11_RVALUE_REFS
#if defined(__clang__)
#if __has_feature(cxx_rvalue_references) && \
(defined(_MSC_VER) || defined(_LIBCPP_VERSION) || defined(__GLIBCXX__) && __GLIBCXX__ >= 20080306)
(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__)) || \
#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__))
(defined(__SUNPRO_CC) && __SUNPRO_CC >= 0x5140 && \
defined(__GXX_EXPERIMENTAL_CXX0X__))
#define RAPIDJSON_HAS_CXX11_RVALUE_REFS 1
#else
@@ -562,9 +597,12 @@ RAPIDJSON_NAMESPACE_END
#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__)) || \
#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__))
(defined(__SUNPRO_CC) && __SUNPRO_CC >= 0x5140 && \
defined(__GXX_EXPERIMENTAL_CXX0X__))
#define RAPIDJSON_HAS_CXX11_NOEXCEPT 1
#else
#define RAPIDJSON_HAS_CXX11_NOEXCEPT 0
@@ -588,9 +626,12 @@ 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__)) || \
#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__))
(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

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

File diff suppressed because it is too large Load Diff

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

File diff suppressed because it is too large Load Diff

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#include "rapidjson.h"
@@ -27,7 +31,8 @@ RAPIDJSON_NAMESPACE_BEGIN
/*! \class rapidjson::Stream
\brief Concept for reading and writing characters.
For read-only stream, no need to implement PutBegin(), Put(), Flush() and PutEnd().
For read-only stream, no need to implement PutBegin(), Put(), Flush() and
PutEnd().
For write-only stream, only need to implement Put() and Flush().
@@ -38,8 +43,8 @@ concept Stream {
//! Read the current character from stream without moving the read cursor.
Ch Peek() const;
//! Read the current character from stream and moving the read cursor to next character.
Ch Take();
//! Read the current character from stream and moving the read cursor to
next character. Ch Take();
//! Get the current read cursor.
//! \return Number of characters read from start.
@@ -65,16 +70,17 @@ concept Stream {
//! Provides additional information for stream.
/*!
By using traits pattern, this type provides a default configuration for stream.
For custom stream, this type can be specialized for other configuration.
See TEST(Reader, CustomStringStream) in readertest.cpp for example.
By using traits pattern, this type provides a default configuration for
stream. For custom stream, this type can be specialized for other
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.
Stream that can be copied fast should specialize this, like StreamTraits<StringStream>.
Stream that can be copied fast should specialize this, like
StreamTraits<StringStream>.
*/
enum { copyOptimization = 0 };
};
@@ -150,8 +156,7 @@ 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) {}
@@ -160,10 +165,16 @@ struct GenericStringStream {
Ch Take() { return *src_++; }
size_t Tell() const { return static_cast<size_t>(src_ - head_); }
Ch* PutBegin() { RAPIDJSON_ASSERT(false); return 0; }
Ch *PutBegin() {
RAPIDJSON_ASSERT(false);
return 0;
}
void Put(Ch) { RAPIDJSON_ASSERT(false); }
void Flush() { RAPIDJSON_ASSERT(false); }
size_t PutEnd(Ch*) { RAPIDJSON_ASSERT(false); return 0; }
size_t PutEnd(Ch *) {
RAPIDJSON_ASSERT(false);
return 0;
}
const Ch *src_; //!< Current read position.
const Ch *head_; //!< Original head of the string.
@@ -184,8 +195,7 @@ 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) {}
@@ -196,13 +206,20 @@ struct GenericInsituStringStream {
size_t Tell() { return static_cast<size_t>(src_ - head_); }
// Write
void Put(Ch c) { RAPIDJSON_ASSERT(dst_ != 0); *dst_++ = c; }
void Put(Ch c) {
RAPIDJSON_ASSERT(dst_ != 0);
*dst_++ = c;
}
Ch *PutBegin() { return dst_ = src_; }
size_t PutEnd(Ch *begin) { return static_cast<size_t>(dst_ - begin); }
void Flush() {}
Ch* Push(size_t count) { Ch* begin = dst_; dst_ += count; return begin; }
Ch *Push(size_t count) {
Ch *begin = dst_;
dst_ += count;
return begin;
}
void Pop(size_t count) { dst_ -= count; }
Ch *src_;

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

@@ -1,22 +1,26 @@
// Tencent is pleased to support the open source community by making RapidJSON available.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_STRINGBUFFER_H_
#define RAPIDJSON_STRINGBUFFER_H_
#include "stream.h"
#include "internal/stack.h"
#include "stream.h"
#if RAPIDJSON_HAS_CXX11_RVALUE_REFS
#include <utility> // std::move
@@ -42,10 +46,13 @@ class GenericStringBuffer {
public:
typedef typename Encoding::Ch Ch;
GenericStringBuffer(Allocator* allocator = 0, size_t capacity = kDefaultCapacity) : stack_(allocator, capacity) {}
GenericStringBuffer(Allocator *allocator = 0,
size_t capacity = kDefaultCapacity)
: stack_(allocator, capacity) {}
#if RAPIDJSON_HAS_CXX11_RVALUE_REFS
GenericStringBuffer(GenericStringBuffer&& rhs) : stack_(std::move(rhs.stack_)) {}
GenericStringBuffer(GenericStringBuffer &&rhs)
: stack_(std::move(rhs.stack_)) {}
GenericStringBuffer &operator=(GenericStringBuffer &&rhs) {
if (&rhs != this)
stack_ = std::move(rhs.stack_);
@@ -97,16 +104,19 @@ private:
typedef GenericStringBuffer<UTF8<>> StringBuffer;
template <typename Encoding, typename Allocator>
inline void PutReserve(GenericStringBuffer<Encoding, Allocator>& stream, size_t count) {
inline void PutReserve(GenericStringBuffer<Encoding, Allocator> &stream,
size_t count) {
stream.Reserve(count);
}
template <typename Encoding, typename Allocator>
inline void PutUnsafe(GenericStringBuffer<Encoding, Allocator>& stream, typename Encoding::Ch c) {
inline void PutUnsafe(GenericStringBuffer<Encoding, Allocator> &stream,
typename Encoding::Ch c) {
stream.PutUnsafe(c);
}
//! Implement specialized version of PutN() with memset() for better performance.
//! Implement specialized version of PutN() with memset() for better
//! performance.
template <>
inline void PutN(GenericStringBuffer<UTF8<>> &stream, char c, size_t n) {
std::memset(stream.stack_.Push<char>(n), c, n * sizeof(c));

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

@@ -1,27 +1,31 @@
// Tencent is pleased to support the open source community by making RapidJSON available.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_WRITER_H_
#define RAPIDJSON_WRITER_H_
#include "stream.h"
#include "internal/clzll.h"
#include "internal/dtoa.h"
#include "internal/itoa.h"
#include "internal/meta.h"
#include "internal/stack.h"
#include "internal/strfunc.h"
#include "internal/dtoa.h"
#include "internal/itoa.h"
#include "stream.h"
#include "stringbuffer.h"
#include <new> // placement new
@@ -67,16 +71,21 @@ enum WriteFlag {
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
kWriteDefaultFlags =
RAPIDJSON_WRITE_DEFAULT_FLAGS //!< Default write flags. Can be customized
//!< by defining
//!< RAPIDJSON_WRITE_DEFAULT_FLAGS
};
//! JSON writer
/*! Writer implements the concept Handler.
It generates JSON text by events to an output os.
User may programmatically calls the functions of a writer to generate JSON text.
User may programmatically calls the functions of a writer to generate JSON
text.
On the other side, a writer can also be passed to objects that generates events,
On the other side, a writer can also be passed to objects that generates
events,
for example Reader::Parse() and Document::Accept().
@@ -86,7 +95,10 @@ enum WriteFlag {
\tparam StackAllocator Type of allocator for allocating memory of stack.
\note implements Handler concept
*/
template<typename OutputStream, typename SourceEncoding = UTF8<>, typename TargetEncoding = UTF8<>, typename StackAllocator = CrtAllocator, unsigned writeFlags = kWriteDefaultFlags>
template <typename OutputStream, typename SourceEncoding = UTF8<>,
typename TargetEncoding = UTF8<>,
typename StackAllocator = CrtAllocator,
unsigned writeFlags = kWriteDefaultFlags>
class Writer {
public:
typedef typename SourceEncoding::Ch Ch;
@@ -95,20 +107,23 @@ public:
//! Constructor
/*! \param os Output stream.
\param stackAllocator User supplied allocator. If it is null, it will create a private one.
\param levelDepth Initial capacity of stack.
\param stackAllocator User supplied allocator. If it is null, it will
create a private one. \param levelDepth Initial capacity of stack.
*/
explicit
Writer(OutputStream& os, StackAllocator* stackAllocator = 0, size_t levelDepth = kDefaultLevelDepth) :
os_(&os), level_stack_(stackAllocator, levelDepth * sizeof(Level)), maxDecimalPlaces_(kDefaultMaxDecimalPlaces), hasRoot_(false) {}
explicit Writer(OutputStream &os, StackAllocator *stackAllocator = 0,
size_t levelDepth = kDefaultLevelDepth)
: 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) {}
explicit Writer(StackAllocator *allocator = 0,
size_t levelDepth = kDefaultLevelDepth)
: 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_) {
Writer(Writer &&rhs)
: os_(rhs.os_), level_stack_(std::move(rhs.level_stack_)),
maxDecimalPlaces_(rhs.maxDecimalPlaces_), hasRoot_(rhs.hasRoot_) {
rhs.os_ = 0;
}
#endif
@@ -141,13 +156,9 @@ public:
/*!
A complete JSON has a complete root object or array.
*/
bool IsComplete() const {
return hasRoot_ && level_stack_.Empty();
}
bool IsComplete() const { return hasRoot_ && level_stack_.Empty(); }
int GetMaxDecimalPlaces() const {
return maxDecimalPlaces_;
}
int GetMaxDecimalPlaces() const { return maxDecimalPlaces_; }
//! Sets the maximum number of decimal places for double output.
/*!
@@ -160,13 +171,14 @@ public:
writer.StartArray();
writer.Double(0.12345); // "0.123"
writer.Double(0.0001); // "0.0"
writer.Double(1.234567890123456e30); // "1.234567890123456e30" (do not truncate significand for positive exponent)
writer.Double(1.23e-4); // "0.0" (do truncate significand for negative exponent)
writer.Double(1.234567890123456e30); // "1.234567890123456e30" (do not
truncate significand for positive exponent) writer.Double(1.23e-4); //
"0.0" (do truncate significand for negative exponent)
writer.EndArray();
\endcode
The default setting does not truncate any decimal places. You can restore to this setting by calling
\code
The default setting does not truncate any decimal places. You can restore
to this setting by calling \code
writer.SetMaxDecimalPlaces(Writer::kDefaultMaxDecimalPlaces);
\endcode
*/
@@ -179,19 +191,40 @@ public:
*/
//@{
bool Null() { Prefix(kNullType); return EndValue(WriteNull()); }
bool Bool(bool b) { Prefix(b ? kTrueType : kFalseType); return EndValue(WriteBool(b)); }
bool Int(int i) { Prefix(kNumberType); return EndValue(WriteInt(i)); }
bool Uint(unsigned u) { Prefix(kNumberType); return EndValue(WriteUint(u)); }
bool Int64(int64_t i64) { Prefix(kNumberType); return EndValue(WriteInt64(i64)); }
bool Uint64(uint64_t u64) { Prefix(kNumberType); return EndValue(WriteUint64(u64)); }
bool Null() {
Prefix(kNullType);
return EndValue(WriteNull());
}
bool Bool(bool b) {
Prefix(b ? kTrueType : kFalseType);
return EndValue(WriteBool(b));
}
bool Int(int i) {
Prefix(kNumberType);
return EndValue(WriteInt(i));
}
bool Uint(unsigned u) {
Prefix(kNumberType);
return EndValue(WriteUint(u));
}
bool Int64(int64_t i64) {
Prefix(kNumberType);
return EndValue(WriteInt64(i64));
}
bool Uint64(uint64_t u64) {
Prefix(kNumberType);
return EndValue(WriteUint64(u64));
}
//! Writes the given \c double value to the stream
/*!
\param d The value to be written.
\return Whether it is succeed.
*/
bool Double(double d) { Prefix(kNumberType); return EndValue(WriteDouble(d)); }
bool Double(double d) {
Prefix(kNumberType);
return EndValue(WriteDouble(d));
}
bool RawNumber(const Ch *str, SizeType length, bool copy = false) {
RAPIDJSON_ASSERT(str != 0);
@@ -219,20 +252,24 @@ public:
return WriteStartObject();
}
bool Key(const Ch* str, SizeType length, bool copy = false) { return String(str, length, copy); }
bool Key(const Ch *str, SizeType length, bool copy = false) {
return String(str, length, copy);
}
#if RAPIDJSON_HAS_STDSTRING
bool Key(const std::basic_string<Ch>& str)
{
bool Key(const std::basic_string<Ch> &str) {
return Key(str.data(), SizeType(str.size()));
}
#endif
bool EndObject(SizeType memberCount = 0) {
(void)memberCount;
RAPIDJSON_ASSERT(level_stack_.GetSize() >= 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
RAPIDJSON_ASSERT(level_stack_.GetSize() >=
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
level_stack_.template Pop<Level>(1);
return EndValue(WriteEndObject());
}
@@ -256,7 +293,9 @@ public:
//@{
//! Simpler but slower overload.
bool String(const Ch* const& str) { return String(str, internal::StrLen(str)); }
bool String(const Ch *const &str) {
return String(str, internal::StrLen(str));
}
bool Key(const Ch *const &str) { return Key(str, internal::StrLen(str)); }
//@}
@@ -265,9 +304,9 @@ public:
/*!
For user to write a stringified JSON as a value.
\param json A well-formed JSON value. It should not contain null character within [0, length - 1] range.
\param length Length of the json.
\param type Type of the root of json.
\param json A well-formed JSON value. It should not contain null character
within [0, length - 1] range. \param length Length of the json. \param type
Type of the root of json.
*/
bool RawValue(const Ch *json, size_t length, Type type) {
RAPIDJSON_ASSERT(json != 0);
@@ -279,9 +318,7 @@ public:
/*!
Allows the user to flush the output stream immediately.
*/
void Flush() {
os_->Flush();
}
void Flush() { os_->Flush(); }
protected:
//! Information for each nested level
@@ -295,17 +332,27 @@ protected:
bool WriteNull() {
PutReserve(*os_, 4);
PutUnsafe(*os_, 'n'); PutUnsafe(*os_, 'u'); PutUnsafe(*os_, 'l'); PutUnsafe(*os_, 'l'); return true;
PutUnsafe(*os_, 'n');
PutUnsafe(*os_, 'u');
PutUnsafe(*os_, 'l');
PutUnsafe(*os_, 'l');
return true;
}
bool WriteBool(bool b) {
if (b) {
PutReserve(*os_, 4);
PutUnsafe(*os_, 't'); PutUnsafe(*os_, 'r'); PutUnsafe(*os_, 'u'); PutUnsafe(*os_, 'e');
}
else {
PutUnsafe(*os_, 't');
PutUnsafe(*os_, 'r');
PutUnsafe(*os_, 'u');
PutUnsafe(*os_, 'e');
} else {
PutReserve(*os_, 5);
PutUnsafe(*os_, 'f'); PutUnsafe(*os_, 'a'); PutUnsafe(*os_, 'l'); PutUnsafe(*os_, 's'); PutUnsafe(*os_, 'e');
PutUnsafe(*os_, 'f');
PutUnsafe(*os_, 'a');
PutUnsafe(*os_, 'l');
PutUnsafe(*os_, 's');
PutUnsafe(*os_, 'e');
}
return true;
}
@@ -352,17 +399,24 @@ protected:
return false;
if (internal::Double(d).IsNan()) {
PutReserve(*os_, 3);
PutUnsafe(*os_, 'N'); PutUnsafe(*os_, 'a'); PutUnsafe(*os_, 'N');
PutUnsafe(*os_, 'N');
PutUnsafe(*os_, 'a');
PutUnsafe(*os_, 'N');
return true;
}
if (internal::Double(d).Sign()) {
PutReserve(*os_, 9);
PutUnsafe(*os_, '-');
}
else
} else
PutReserve(*os_, 8);
PutUnsafe(*os_, 'I'); PutUnsafe(*os_, 'n'); PutUnsafe(*os_, 'f');
PutUnsafe(*os_, 'i'); PutUnsafe(*os_, 'n'); PutUnsafe(*os_, 'i'); PutUnsafe(*os_, 't'); PutUnsafe(*os_, 'y');
PutUnsafe(*os_, 'I');
PutUnsafe(*os_, 'n');
PutUnsafe(*os_, 'f');
PutUnsafe(*os_, 'i');
PutUnsafe(*os_, 'n');
PutUnsafe(*os_, 'i');
PutUnsafe(*os_, 't');
PutUnsafe(*os_, 'y');
return true;
}
@@ -375,15 +429,22 @@ protected:
}
bool WriteString(const Ch *str, SizeType length) {
static const typename OutputStream::Ch hexDigits[16] = { '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F' };
static const typename OutputStream::Ch hexDigits[16] = {
'0', '1', '2', '3', '4', '5', '6', '7',
'8', '9', 'A', 'B', 'C', 'D', 'E', 'F'};
static const char escape[256] = {
#define Z16 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
//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
'u', 'u', 'u', 'u', 'u', 'u', 'u', 'u', 'u', 'u', 'u', 'u', 'u', 'u', 'u', 'u', // 10
0, 0, '"', 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // 20
// 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
'u', 'u', 'u', 'u', 'u', 'u', 'u', 'u', 'u', 'u',
'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, 0, 0, 0, 0, 0, 0,'\\', 0, 0, 0, // 50
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
#undef Z16
};
@@ -404,13 +465,13 @@ protected:
return false;
PutUnsafe(*os_, '\\');
PutUnsafe(*os_, 'u');
if (codepoint <= 0xD7FF || (codepoint >= 0xE000 && codepoint <= 0xFFFF)) {
if (codepoint <= 0xD7FF ||
(codepoint >= 0xE000 && codepoint <= 0xFFFF)) {
PutUnsafe(*os_, hexDigits[(codepoint >> 12) & 15]);
PutUnsafe(*os_, hexDigits[(codepoint >> 8) & 15]);
PutUnsafe(*os_, hexDigits[(codepoint >> 4) & 15]);
PutUnsafe(*os_, hexDigits[(codepoint)&15]);
}
else {
} else {
RAPIDJSON_ASSERT(codepoint >= 0x010000 && codepoint <= 0x10FFFF);
// Surrogate pair
unsigned s = codepoint - 0x010000;
@@ -427,44 +488,64 @@ protected:
PutUnsafe(*os_, hexDigits[(trail >> 4) & 15]);
PutUnsafe(*os_, hexDigits[(trail)&15]);
}
}
else if ((sizeof(Ch) == 1 || static_cast<unsigned>(c) < 256) && RAPIDJSON_UNLIKELY(escape[static_cast<unsigned char>(c)])) {
} else if ((sizeof(Ch) == 1 || static_cast<unsigned>(c) < 256) &&
RAPIDJSON_UNLIKELY(escape[static_cast<unsigned char>(c)])) {
is.Take();
PutUnsafe(*os_, '\\');
PutUnsafe(*os_, static_cast<typename OutputStream::Ch>(escape[static_cast<unsigned char>(c)]));
PutUnsafe(*os_, static_cast<typename OutputStream::Ch>(
escape[static_cast<unsigned char>(c)]));
if (escape[static_cast<unsigned char>(c)] == 'u') {
PutUnsafe(*os_, '0');
PutUnsafe(*os_, '0');
PutUnsafe(*os_, hexDigits[static_cast<unsigned char>(c) >> 4]);
PutUnsafe(*os_, hexDigits[static_cast<unsigned char>(c) & 0xF]);
}
}
else if (RAPIDJSON_UNLIKELY(!(writeFlags & kWriteValidateEncodingFlag ?
Transcoder<SourceEncoding, TargetEncoding>::Validate(is, *os_) :
Transcoder<SourceEncoding, TargetEncoding>::TranscodeUnsafe(is, *os_))))
} else if (RAPIDJSON_UNLIKELY(!(
writeFlags & kWriteValidateEncodingFlag
? Transcoder<SourceEncoding, TargetEncoding>::Validate(
is, *os_)
: Transcoder<SourceEncoding,
TargetEncoding>::TranscodeUnsafe(is,
*os_))))
return false;
}
PutUnsafe(*os_, '\"');
return true;
}
bool ScanWriteUnescapedString(GenericStringStream<SourceEncoding>& is, size_t length) {
bool ScanWriteUnescapedString(GenericStringStream<SourceEncoding> &is,
size_t length) {
return RAPIDJSON_LIKELY(is.Tell() < length);
}
bool WriteStartObject() { os_->Put('{'); return true; }
bool WriteEndObject() { os_->Put('}'); return true; }
bool WriteStartArray() { os_->Put('['); return true; }
bool WriteEndArray() { os_->Put(']'); return true; }
bool WriteStartObject() {
os_->Put('{');
return true;
}
bool WriteEndObject() {
os_->Put('}');
return true;
}
bool WriteStartArray() {
os_->Put('[');
return true;
}
bool WriteEndArray() {
os_->Put(']');
return true;
}
bool WriteRawValue(const Ch *json, size_t length) {
PutReserve(*os_, length);
GenericStringStream<SourceEncoding> is(json);
while (RAPIDJSON_LIKELY(is.Tell() < length)) {
RAPIDJSON_ASSERT(is.Peek() != '\0');
if (RAPIDJSON_UNLIKELY(!(writeFlags & kWriteValidateEncodingFlag ?
Transcoder<SourceEncoding, TargetEncoding>::Validate(is, *os_) :
Transcoder<SourceEncoding, TargetEncoding>::TranscodeUnsafe(is, *os_))))
if (RAPIDJSON_UNLIKELY(!(
writeFlags & kWriteValidateEncodingFlag
? Transcoder<SourceEncoding, TargetEncoding>::Validate(is,
*os_)
: Transcoder<SourceEncoding, TargetEncoding>::TranscodeUnsafe(
is, *os_))))
return false;
}
return true;
@@ -472,7 +553,8 @@ protected:
void Prefix(Type type) {
(void)type;
if (RAPIDJSON_LIKELY(level_stack_.GetSize() != 0)) { // this value is not at root
if (RAPIDJSON_LIKELY(level_stack_.GetSize() !=
0)) { // this value is not at root
Level *level = level_stack_.template Top<Level>();
if (level->valueCount > 0) {
if (level->inArray)
@@ -481,10 +563,10 @@ protected:
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 {
} else {
RAPIDJSON_ASSERT(!hasRoot_); // Should only has one and only one root.
hasRoot_ = true;
}
@@ -510,57 +592,60 @@ 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).
// Note: This code path can only be reached if
// (RAPIDJSON_WRITE_DEFAULT_FLAGS & kWriteNanAndInfFlag).
if (!(kWriteDefaultFlags & kWriteNanAndInfFlag))
return false;
if (internal::Double(d).IsNan()) {
PutReserve(*os_, 3);
PutUnsafe(*os_, 'N'); PutUnsafe(*os_, 'a'); PutUnsafe(*os_, 'N');
PutUnsafe(*os_, 'N');
PutUnsafe(*os_, 'a');
PutUnsafe(*os_, 'N');
return true;
}
if (internal::Double(d).Sign()) {
PutReserve(*os_, 9);
PutUnsafe(*os_, '-');
}
else
} else
PutReserve(*os_, 8);
PutUnsafe(*os_, 'I'); PutUnsafe(*os_, 'n'); PutUnsafe(*os_, 'f');
PutUnsafe(*os_, 'i'); PutUnsafe(*os_, 'n'); PutUnsafe(*os_, 'i'); PutUnsafe(*os_, 't'); PutUnsafe(*os_, 'y');
PutUnsafe(*os_, 'I');
PutUnsafe(*os_, 'n');
PutUnsafe(*os_, 'f');
PutUnsafe(*os_, 'i');
PutUnsafe(*os_, 'n');
PutUnsafe(*os_, 'i');
PutUnsafe(*os_, 't');
PutUnsafe(*os_, 'y');
return true;
}
@@ -572,7 +657,8 @@ inline bool Writer<StringBuffer>::WriteDouble(double d) {
#if defined(RAPIDJSON_SSE2) || defined(RAPIDJSON_SSE42)
template <>
inline bool Writer<StringBuffer>::ScanWriteUnescapedString(StringStream& is, size_t length) {
inline bool Writer<StringBuffer>::ScanWriteUnescapedString(StringStream &is,
size_t length) {
if (length < 16)
return RAPIDJSON_LIKELY(is.Tell() < length);
@@ -581,8 +667,10 @@ inline bool Writer<StringBuffer>::ScanWriteUnescapedString(StringStream& is, siz
const char *p = is.src_;
const char *end = is.head_ + length;
const char* nextAligned = reinterpret_cast<const char*>((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));
const char *nextAligned = reinterpret_cast<const char *>(
(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;
@@ -590,23 +678,32 @@ inline bool Writer<StringBuffer>::ScanWriteUnescapedString(StringStream& is, siz
if (*p < 0x20 || *p == '\"' || *p == '\\') {
is.src_ = p;
return RAPIDJSON_LIKELY(is.Tell() < length);
}
else
} else
os_->PutUnsafe(*p++);
// The rest of string using SIMD
static const char dquote[16] = { '\"', '\"', '\"', '\"', '\"', '\"', '\"', '\"', '\"', '\"', '\"', '\"', '\"', '\"', '\"', '\"' };
static const char bslash[16] = { '\\', '\\', '\\', '\\', '\\', '\\', '\\', '\\', '\\', '\\', '\\', '\\', '\\', '\\', '\\', '\\' };
static const char space[16] = { 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F };
const __m128i dq = _mm_loadu_si128(reinterpret_cast<const __m128i *>(&dquote[0]));
const __m128i bs = _mm_loadu_si128(reinterpret_cast<const __m128i *>(&bslash[0]));
const __m128i sp = _mm_loadu_si128(reinterpret_cast<const __m128i *>(&space[0]));
static const char dquote[16] = {'\"', '\"', '\"', '\"', '\"', '\"',
'\"', '\"', '\"', '\"', '\"', '\"',
'\"', '\"', '\"', '\"'};
static const char bslash[16] = {'\\', '\\', '\\', '\\', '\\', '\\',
'\\', '\\', '\\', '\\', '\\', '\\',
'\\', '\\', '\\', '\\'};
static const char space[16] = {0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F,
0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F,
0x1F, 0x1F, 0x1F, 0x1F};
const __m128i dq =
_mm_loadu_si128(reinterpret_cast<const __m128i *>(&dquote[0]));
const __m128i bs =
_mm_loadu_si128(reinterpret_cast<const __m128i *>(&bslash[0]));
const __m128i sp =
_mm_loadu_si128(reinterpret_cast<const __m128i *>(&space[0]));
for (; p != endAligned; p += 16) {
const __m128i s = _mm_load_si128(reinterpret_cast<const __m128i *>(p));
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
const __m128i t3 = _mm_cmpeq_epi8(_mm_max_epu8(s, sp),
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
@@ -633,7 +730,8 @@ inline bool Writer<StringBuffer>::ScanWriteUnescapedString(StringStream& is, siz
}
#elif defined(RAPIDJSON_NEON)
template <>
inline bool Writer<StringBuffer>::ScanWriteUnescapedString(StringStream& is, size_t length) {
inline bool Writer<StringBuffer>::ScanWriteUnescapedString(StringStream &is,
size_t length) {
if (length < 16)
return RAPIDJSON_LIKELY(is.Tell() < length);
@@ -642,8 +740,10 @@ inline bool Writer<StringBuffer>::ScanWriteUnescapedString(StringStream& is, siz
const char *p = is.src_;
const char *end = is.head_ + length;
const char* nextAligned = reinterpret_cast<const char*>((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));
const char *nextAligned = reinterpret_cast<const char *>(
(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;
@@ -651,8 +751,7 @@ inline bool Writer<StringBuffer>::ScanWriteUnescapedString(StringStream& is, siz
if (*p < 0x20 || *p == '\"' || *p == '\\') {
is.src_ = p;
return RAPIDJSON_LIKELY(is.Tell() < length);
}
else
} else
os_->PutUnsafe(*p++);
// The rest of string using SIMD

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

File diff suppressed because it is too large Load Diff

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

@@ -8,167 +8,125 @@
#include "rapidxml.hpp"
namespace rapidxml
{
namespace rapidxml {
//! Iterator of child nodes of xml_node
template<class Ch>
class node_iterator
{
template <class Ch> class node_iterator {
public:
typedef typename xml_node<Ch> value_type;
typedef typename xml_node<Ch> &reference;
typedef typename xml_node<Ch> *pointer;
typedef std::ptrdiff_t difference_type;
typedef std::bidirectional_iterator_tag iterator_category;
node_iterator()
: m_node(0)
{
}
node_iterator() : m_node(0) {}
node_iterator(xml_node<Ch> *node)
: m_node(node->first_node())
{
}
node_iterator(xml_node<Ch> *node) : m_node(node->first_node()) {}
reference operator *() const
{
reference operator*() const {
assert(m_node);
return *m_node;
}
pointer operator->() const
{
pointer operator->() const {
assert(m_node);
return m_node;
}
node_iterator& operator++()
{
node_iterator &operator++() {
assert(m_node);
m_node = m_node->next_sibling();
return *this;
}
node_iterator operator++(int)
{
node_iterator operator++(int) {
node_iterator tmp = *this;
++this;
return tmp;
}
node_iterator& operator--()
{
node_iterator &operator--() {
assert(m_node && m_node->previous_sibling());
m_node = m_node->previous_sibling();
return *this;
}
node_iterator operator--(int)
{
node_iterator operator--(int) {
node_iterator tmp = *this;
++this;
return tmp;
}
bool operator ==(const node_iterator<Ch> &rhs)
{
return m_node == rhs.m_node;
}
bool operator==(const node_iterator<Ch> &rhs) { return m_node == rhs.m_node; }
bool operator !=(const node_iterator<Ch> &rhs)
{
return m_node != rhs.m_node;
}
bool operator!=(const node_iterator<Ch> &rhs) { return m_node != rhs.m_node; }
private:
xml_node<Ch> *m_node;
};
//! Iterator of child attributes of xml_node
template<class Ch>
class attribute_iterator
{
template <class Ch> class attribute_iterator {
public:
typedef typename xml_attribute<Ch> value_type;
typedef typename xml_attribute<Ch> &reference;
typedef typename xml_attribute<Ch> *pointer;
typedef std::ptrdiff_t difference_type;
typedef std::bidirectional_iterator_tag iterator_category;
attribute_iterator()
: m_attribute(0)
{
}
attribute_iterator() : m_attribute(0) {}
attribute_iterator(xml_node<Ch> *node)
: m_attribute(node->first_attribute())
{
}
: m_attribute(node->first_attribute()) {}
reference operator *() const
{
reference operator*() const {
assert(m_attribute);
return *m_attribute;
}
pointer operator->() const
{
pointer operator->() const {
assert(m_attribute);
return m_attribute;
}
attribute_iterator& operator++()
{
attribute_iterator &operator++() {
assert(m_attribute);
m_attribute = m_attribute->next_attribute();
return *this;
}
attribute_iterator operator++(int)
{
attribute_iterator operator++(int) {
attribute_iterator tmp = *this;
++this;
return tmp;
}
attribute_iterator& operator--()
{
attribute_iterator &operator--() {
assert(m_attribute && m_attribute->previous_attribute());
m_attribute = m_attribute->previous_attribute();
return *this;
}
attribute_iterator operator--(int)
{
attribute_iterator operator--(int) {
attribute_iterator tmp = *this;
++this;
return tmp;
}
bool operator ==(const attribute_iterator<Ch> &rhs)
{
bool operator==(const attribute_iterator<Ch> &rhs) {
return m_attribute == rhs.m_attribute;
}
bool operator !=(const attribute_iterator<Ch> &rhs)
{
bool operator!=(const attribute_iterator<Ch> &rhs) {
return m_attribute != rhs.m_attribute;
}
private:
xml_attribute<Ch> *m_attribute;
};
}
} // namespace rapidxml
#endif

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

@@ -10,32 +10,31 @@
// Only include streams if not disabled
#ifndef RAPIDXML_NO_STREAMS
#include <ostream>
#include <iterator>
#include <ostream>
#endif
namespace rapidxml
{
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
//! \cond internal
namespace internal
{
namespace internal {
///////////////////////////////////////////////////////////////////////////
// Internal character operations
// 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)
{
inline OutIt copy_chars(const Ch *begin, const Ch *end, OutIt out) {
while (begin != end)
*out++ = *begin++;
return out;
@@ -44,32 +43,47 @@ namespace rapidxml
// Copy characters from given range to given output iterator and expand
// characters into references (&lt; &gt; &apos; &quot; &amp;)
template <class OutIt, class Ch>
inline OutIt copy_and_expand_chars(const Ch *begin, const Ch *end, Ch noexpand, OutIt out)
{
while (begin != end)
{
if (*begin == noexpand)
{
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
}
else
{
switch (*begin)
{
} else {
switch (*begin) {
case Ch('<'):
*out++ = Ch('&'); *out++ = Ch('l'); *out++ = Ch('t'); *out++ = Ch(';');
*out++ = Ch('&');
*out++ = Ch('l');
*out++ = Ch('t');
*out++ = Ch(';');
break;
case Ch('>'):
*out++ = Ch('&'); *out++ = Ch('g'); *out++ = Ch('t'); *out++ = 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(';');
*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(';');
*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(';');
*out++ = Ch('&');
*out++ = Ch('a');
*out++ = Ch('m');
*out++ = Ch('p');
*out++ = Ch(';');
break;
default:
*out++ = *begin; // No expansion, copy character
@@ -82,8 +96,7 @@ namespace rapidxml
// 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)
{
inline OutIt fill_chars(OutIt out, int n, Ch ch) {
for (int i = 0; i < n; ++i)
*out++ = ch;
return out;
@@ -91,8 +104,7 @@ namespace rapidxml
// Find character
template <class Ch, Ch ch>
inline bool find_char(const Ch *begin, const Ch *end)
{
inline bool find_char(const Ch *begin, const Ch *end) {
while (begin != end)
if (*begin++ == ch)
return true;
@@ -104,11 +116,10 @@ namespace rapidxml
// Print node
template <class OutIt, class Ch>
inline OutIt print_node(OutIt out, const xml_node<Ch> *node, int flags, int indent)
{
inline OutIt print_node(OutIt out, const xml_node<Ch> *node, int flags,
int indent) {
// Print proper node type
switch (node->type())
{
switch (node->type()) {
// Document
case node_document:
@@ -166,36 +177,39 @@ namespace rapidxml
// Print children of the node
template <class OutIt, class Ch>
inline OutIt print_children(OutIt out, const xml_node<Ch> *node, int flags, int indent)
{
for (xml_node<Ch> *child = node->first_node(); child; child = child->next_sibling())
inline OutIt print_children(OutIt out, const xml_node<Ch> *node, int flags,
int indent) {
for (xml_node<Ch> *child = node->first_node(); child;
child = child->next_sibling())
out = print_node(out, child, flags, indent);
return out;
}
// Print attributes of the node
template <class OutIt, class Ch>
inline OutIt print_attributes(OutIt out, const xml_node<Ch> *node, int flags)
{
for (xml_attribute<Ch> *attribute = node->first_attribute(); attribute; attribute = attribute->next_attribute())
{
if (attribute->name() && attribute->value())
{
inline OutIt print_attributes(OutIt out, const xml_node<Ch> *node, int flags) {
for (xml_attribute<Ch> *attribute = node->first_attribute(); attribute;
attribute = attribute->next_attribute()) {
if (attribute->name() && attribute->value()) {
// Print attribute name
*out = Ch(' '), ++out;
out = copy_chars(attribute->name(), attribute->name() + attribute->name_size(), out);
out = copy_chars(attribute->name(),
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(), Ch('"'), out);
out = copy_and_expand_chars(
attribute->value(), attribute->value() + attribute->value_size(),
Ch('"'), out);
*out = Ch('\''), ++out;
}
else
{
} else {
*out = Ch('"'), ++out;
out = copy_and_expand_chars(attribute->value(), attribute->value() + attribute->value_size(), Ch('\''), out);
out = copy_and_expand_chars(
attribute->value(), attribute->value() + attribute->value_size(),
Ch('\''), out);
*out = Ch('"'), ++out;
}
}
@@ -205,42 +219,55 @@ namespace rapidxml
// Print data node
template <class OutIt, class Ch>
inline OutIt print_data_node(OutIt out, const xml_node<Ch> *node, int flags, int indent)
{
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'));
out = copy_and_expand_chars(node->value(), node->value() + node->value_size(), Ch(0), out);
out = copy_and_expand_chars(node->value(), node->value() + node->value_size(),
Ch(0), out);
return out;
}
// Print data node
template <class OutIt, class Ch>
inline OutIt print_cdata_node(OutIt out, const xml_node<Ch> *node, int flags, int indent)
{
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'));
*out = Ch('<'); ++out;
*out = Ch('!'); ++out;
*out = Ch('['); ++out;
*out = Ch('C'); ++out;
*out = Ch('D'); ++out;
*out = Ch('A'); ++out;
*out = Ch('T'); ++out;
*out = Ch('A'); ++out;
*out = Ch('['); ++out;
*out = Ch('<');
++out;
*out = Ch('!');
++out;
*out = Ch('[');
++out;
*out = Ch('C');
++out;
*out = Ch('D');
++out;
*out = Ch('A');
++out;
*out = Ch('T');
++out;
*out = Ch('A');
++out;
*out = Ch('[');
++out;
out = copy_chars(node->value(), node->value() + node->value_size(), out);
*out = Ch(']'); ++out;
*out = Ch(']'); ++out;
*out = Ch('>'); ++out;
*out = Ch(']');
++out;
*out = Ch(']');
++out;
*out = Ch('>');
++out;
return out;
}
// Print element node
template <class OutIt, class Ch>
inline OutIt print_element_node(OutIt out, const xml_node<Ch> *node, int flags, int indent)
{
inline OutIt print_element_node(OutIt out, const xml_node<Ch> *node, int flags,
int indent) {
assert(node->type() == node_element);
// Print element name and attributes, if any
@@ -251,31 +278,25 @@ namespace rapidxml
out = print_attributes(out, node, flags);
// If node is childless
if (node->value_size() == 0 && !node->first_node())
{
if (node->value_size() == 0 && !node->first_node()) {
// Print childless node tag ending
*out = Ch('/'), ++out;
*out = Ch('>'), ++out;
}
else
{
} else {
// Print normal node tag ending
*out = Ch('>'), ++out;
// Test if node contains a single data node only (and no other nodes)
xml_node<Ch> *child = node->first_node();
if (!child)
{
if (!child) {
// If node has no children, only print its value without indenting
out = copy_and_expand_chars(node->value(), node->value() + node->value_size(), Ch(0), out);
}
else if (child->next_sibling() == 0 && child->type() == node_data)
{
out = copy_and_expand_chars(
node->value(), node->value() + node->value_size(), Ch(0), out);
} else if (child->next_sibling() == 0 && child->type() == node_data) {
// If node has a sole data child, only print its value without indenting
out = copy_and_expand_chars(child->value(), child->value() + child->value_size(), Ch(0), out);
}
else
{
out = copy_and_expand_chars(
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;
@@ -295,8 +316,8 @@ namespace rapidxml
// Print declaration node
template <class OutIt, class Ch>
inline OutIt print_declaration_node(OutIt out, const xml_node<Ch> *node, int flags, int indent)
{
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'));
@@ -318,8 +339,8 @@ namespace rapidxml
// Print comment node
template <class OutIt, class Ch>
inline OutIt print_comment_node(OutIt out, const xml_node<Ch> *node, int flags, int indent)
{
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'));
@@ -336,8 +357,8 @@ namespace rapidxml
// Print doctype node
template <class OutIt, class Ch>
inline OutIt print_doctype_node(OutIt out, const xml_node<Ch> *node, int flags, int indent)
{
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'));
@@ -358,8 +379,8 @@ namespace rapidxml
// Print pi node
template <class OutIt, class Ch>
inline OutIt print_pi_node(OutIt out, const xml_node<Ch> *node, int flags, int indent)
{
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'));
@@ -373,7 +394,7 @@ namespace rapidxml
return out;
}
}
} // namespace internal
//! \endcond
///////////////////////////////////////////////////////////////////////////
@@ -383,10 +404,10 @@ namespace rapidxml
//! \param out Output iterator to print to.
//! \param node Node to be printed. Pass xml_document to print entire document.
//! \param flags Flags controlling how XML is printed.
//! \return Output iterator pointing to position immediately after last character of printed text.
//! \return Output iterator pointing to position immediately after last
//! character of printed text.
template <class OutIt, class Ch>
inline OutIt print(OutIt out, const xml_node<Ch> &node, int flags = 0)
{
inline OutIt print(OutIt out, const xml_node<Ch> &node, int flags = 0) {
return internal::print_node(out, &node, flags, 0);
}
@@ -398,24 +419,23 @@ namespace rapidxml
//! \param flags Flags controlling how XML is printed.
//! \return Output stream.
template <class Ch>
inline std::basic_ostream<Ch> &print(std::basic_ostream<Ch> &out, const xml_node<Ch> &node, int flags = 0)
{
inline std::basic_ostream<Ch> &print(std::basic_ostream<Ch> &out,
const xml_node<Ch> &node, int flags = 0) {
print(std::ostream_iterator<Ch>(out), node, flags);
return out;
}
//! Prints formatted XML to given output stream. Uses default printing flags. Use print() function to customize printing process.
//! \param out Output stream to print to.
//! \param node Node to be printed.
//! \return Output stream.
//! Prints formatted XML to given output stream. Uses default printing flags.
//! Use print() function to customize printing process. \param out Output stream
//! to print to. \param node Node to be printed. \return Output stream.
template <class Ch>
inline std::basic_ostream<Ch> &operator <<(std::basic_ostream<Ch> &out, const xml_node<Ch> &node)
{
inline std::basic_ostream<Ch> &operator<<(std::basic_ostream<Ch> &out,
const xml_node<Ch> &node) {
return print(out, node);
}
#endif
}
} // namespace rapidxml
#endif

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

@@ -4,29 +4,25 @@
// Copyright (C) 2006, 2009 Marcin Kalicinski
// Version 1.13
// Revision $DateTime: 2009/05/13 01:46:17 $
//! \file rapidxml_utils.hpp This file contains high-level rapidxml utilities that can be useful
//! in certain simple scenarios. They should probably not be used if maximizing performance is the main objective.
//! \file rapidxml_utils.hpp This file contains high-level rapidxml utilities
//! 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 <vector>
#include <string>
#include <fstream>
#include <stdexcept>
#include <string>
#include <vector>
namespace rapidxml
{
namespace rapidxml {
//! Represents data loaded from a file
template<class Ch = char>
class file
{
template <class Ch = char> class file {
public:
//! Loads file into the memory. Data will be automatically destroyed by the destructor.
//! \param filename Filename to load.
file(const char *filename)
{
//! Loads file into the memory. Data will be automatically destroyed by the
//! destructor. \param filename Filename to load.
file(const char *filename) {
using namespace std;
// Open stream
@@ -46,10 +42,9 @@ namespace rapidxml
m_data[size] = 0;
}
//! Loads file into the memory. Data will be automatically destroyed by the destructor
//! \param stream Stream to load from
file(std::basic_istream<Ch> &stream)
{
//! Loads file into the memory. Data will be automatically destroyed by the
//! destructor \param stream Stream to load from
file(std::basic_istream<Ch> &stream) {
using namespace std;
// Load data and add terminating 0
@@ -62,40 +57,26 @@ namespace rapidxml
//! Gets file data.
//! \return Pointer to data of file.
Ch *data()
{
return &m_data.front();
}
Ch *data() { return &m_data.front(); }
//! Gets file data.
//! \return Pointer to data of file.
const Ch *data() const
{
return &m_data.front();
}
const Ch *data() const { return &m_data.front(); }
//! Gets file data size.
//! \return Size of file data, in characters.
std::size_t size() const
{
return m_data.size();
}
std::size_t size() const { return m_data.size(); }
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)
{
while (child) {
++count;
child = child->next_sibling();
}
@@ -104,19 +85,16 @@ namespace rapidxml
//! 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)
{
while (attr) {
++count;
attr = attr->next_attribute();
}
return count;
}
}
} // namespace rapidxml
#endif

2
livox_ros_driver/launch/lvx_to_rosbag.launch
View File

@@ -5,7 +5,7 @@
<arg name="xfer_format" default="0"/>
<arg name="multi_topic" default="0"/>
<arg name="data_src" default="2"/>
<arg name="publish_freq" default="2000.0"/>
<arg name="publish_freq" default="20.0"/>
<arg name="output_type" default="1"/>
<arg name="rviz_enable" default="false"/>
<arg name="rosbag_enable" default="false"/>

10
livox_ros_driver/livox_ros_driver/include/livox_ros_driver.h
View File

@@ -27,16 +27,14 @@
#define LIVOX_ROS_DRIVER_VER_MAJOR 2
#define LIVOX_ROS_DRIVER_VER_MINOR 0
#define LIVOX_ROS_DRIVER_VER_PATCH 0
#define LIVOX_ROS_DRIVER_VER_PATCH 1
#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(LIVOX_ROS_DRIVER_VER_PATCH)
GET_STRING(LIVOX_ROS_DRIVER_VER_MAJOR) \
"." GET_STRING(LIVOX_ROS_DRIVER_VER_MINOR) "." GET_STRING( \
LIVOX_ROS_DRIVER_VER_PATCH)
#endif

231
livox_ros_driver/livox_ros_driver/lddc.cpp
View File

@@ -24,27 +24,30 @@
#include "lddc.h"
#include <stdint.h>
#include <inttypes.h>
#include <math.h>
#include <stdint.h>
#include "lds_lvx.h"
#include "lds_lidar.h"
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Imu.h>
#include <rosbag/bag.h>
#include <livox_ros_driver/CustomPoint.h>
#include <livox_ros_driver/CustomMsg.h>
#include <pcl_ros/point_cloud.h>
#include <ros/ros.h>
#include <rosbag/bag.h>
#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>
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) : \
transfer_format_(format), use_multi_topic_(multi_topic),\
/** Lidar Data Distribute Control
* ----------------------------------------------------------------*/
Lddc::Lddc(int format, int multi_topic, int data_src, int output_type,
double frq)
: transfer_format_(format), use_multi_topic_(multi_topic),
data_src_(data_src), output_type_(output_type), publish_frq_(frq) {
publish_interval_ms_ = 1000 / publish_frq_;
@@ -85,7 +88,8 @@ Lddc::~Lddc() {
}
}
uint32_t Lddc::PublishPointcloud2(LidarDataQueue* queue, uint32_t packet_num, uint8_t handle) {
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;
@@ -121,42 +125,61 @@ uint32_t Lddc::PublishPointcloud2(LidarDataQueue* queue, uint32_t packet_num, ui
cloud.fields[5].count = 1;
cloud.fields[5].datatype = sensor_msgs::PointField::UINT8;
cloud.data.resize(packet_num * kMaxPointPerEthPacket * sizeof(LivoxPointXyzrtl));
cloud.data.resize(packet_num * kMaxPointPerEthPacket *
sizeof(LivoxPointXyzrtl));
cloud.point_step = sizeof(LivoxPointXyzrtl);
uint8_t *point_base = cloud.data.data();
uint8_t data_source = lds_->lidars_[handle].data_src;
StoragePacket storage_packet;
while (published_packet < packet_num) {
QueueProPop(queue, &storage_packet);
LivoxEthPacket* raw_packet = reinterpret_cast<LivoxEthPacket *>(storage_packet.raw_data);
LivoxEthPacket *raw_packet =
reinterpret_cast<LivoxEthPacket *>(storage_packet.raw_data);
uint32_t packet_interval = GetPacketInterval(raw_packet->data_type);
int64_t packet_loss_threshold_lower = packet_interval + packet_interval / 2;
timestamp = GetStoragePacketTimestamp(&storage_packet, data_source);
if (published_packet && \
((timestamp - last_timestamp) > kMaxPacketTimeGap)) {
int64_t packet_gap = timestamp - last_timestamp;
if (published_packet && (packet_gap > packet_loss_threshold_lower) &&
lds_->lidars_[handle].data_is_pubulished) {
ROS_INFO("Lidar[%d] packet loss, interval is %ldus", handle, packet_gap);
if (kSourceLvxFile != data_source) {
ROS_INFO("Lidar[%d] packet loss", handle);
// ROS_INFO("Lidar[%d] packet loss %ld %d %d", handle,
// packet_loss_threshold_lower, packet_interval, raw_packet->data_type);
int64_t packet_loss_threshold_upper = packet_interval * packet_num;
if (packet_gap >
packet_loss_threshold_upper) { // skip when gap is too large
break;
}
point_base = FillZeroPointXyzrtl(point_base, storage_packet.point_num);
cloud.width += storage_packet.point_num;
last_timestamp = last_timestamp + packet_interval;
++published_packet;
continue;
}
}
if (!published_packet) {
cloud.header.stamp = ros::Time(timestamp/1000000000.0); // to ros time stamp
if (!published_packet) { // use the first packet timestamp as pointcloud2
// msg timestamp
cloud.header.stamp = ros::Time(timestamp / 1000000000.0);
}
cloud.width += storage_packet.point_num;
if (kSourceLvxFile != data_source) {
PointConvertHandler pf_point_convert = GetConvertHandler(raw_packet->data_type);
PointConvertHandler pf_point_convert =
GetConvertHandler(raw_packet->data_type);
if (pf_point_convert) {
point_base = pf_point_convert(point_base, raw_packet, \
lds_->lidars_[handle].extrinsic_parameter);
point_base = pf_point_convert(
point_base, raw_packet, lds_->lidars_[handle].extrinsic_parameter);
} else {
/* Skip the packet */
ROS_INFO("Lidar[%d] unkown packet type[%d]", handle, raw_packet->data_type);
ROS_INFO("Lidar[%d] unkown packet type[%d]", handle,
raw_packet->data_type);
break;
}
} else {
point_base = LivoxPointToPxyzrtl(point_base, raw_packet, \
lds_->lidars_[handle].extrinsic_parameter);
point_base = LivoxPointToPxyzrtl(
point_base, raw_packet, lds_->lidars_[handle].extrinsic_parameter);
}
QueuePopUpdate(queue);
@@ -174,15 +197,21 @@ uint32_t Lddc::PublishPointcloud2(LidarDataQueue* queue, uint32_t packet_num, ui
p_publisher->publish(cloud);
} else {
if (bag_) {
bag_->write(p_publisher->getTopic(), ros::Time(timestamp/1000000000.0), cloud);
bag_->write(p_publisher->getTopic(), ros::Time(timestamp / 1000000000.0),
cloud);
}
}
if (!lds_->lidars_[handle].data_is_pubulished) {
lds_->lidars_[handle].data_is_pubulished = true;
}
return published_packet;
}
/* for pcl::pxyzi */
uint32_t Lddc::PublishPointcloudData(LidarDataQueue* queue, uint32_t packet_num, uint8_t handle) {
uint32_t Lddc::PublishPointcloudData(LidarDataQueue *queue, uint32_t packet_num,
uint8_t handle) {
uint64_t timestamp = 0;
uint64_t last_timestamp = 0;
uint32_t published_packet = 0;
@@ -198,13 +227,29 @@ uint32_t Lddc::PublishPointcloudData(LidarDataQueue* queue, uint32_t packet_num,
StoragePacket storage_packet;
while (published_packet < packet_num) {
QueueProPop(queue, &storage_packet);
LivoxEthPacket* raw_packet = reinterpret_cast<LivoxEthPacket *>(storage_packet.raw_data);
LivoxEthPacket *raw_packet =
reinterpret_cast<LivoxEthPacket *>(storage_packet.raw_data);
uint32_t packet_interval = GetPacketInterval(raw_packet->data_type);
int64_t packet_loss_threshold_lower = packet_interval + packet_interval / 2;
timestamp = GetStoragePacketTimestamp(&storage_packet, data_source);
if (published_packet && \
((timestamp - last_timestamp) > kMaxPacketTimeGap)) {
ROS_INFO("Lidar[%d] packet loss", handle);
int64_t packet_gap = timestamp - last_timestamp;
if ((packet_gap > packet_loss_threshold_lower) && published_packet &&
lds_->lidars_[handle].data_is_pubulished) {
ROS_INFO("Lidar[%d] packet loss, interval is %ldus", handle, packet_gap);
int64_t packet_loss_threshold_upper = packet_interval * packet_num;
if (packet_gap >
packet_loss_threshold_upper) { // skip when gap is too large
break;
}
pcl::PointXYZI point = {0}; // fill zero points
for (uint32_t i = 0; i < storage_packet.point_num; i++) {
cloud->points.push_back(point);
}
last_timestamp = last_timestamp + packet_interval;
++published_packet;
continue;
}
if (!published_packet) {
cloud->header.stamp = timestamp / 1000.0; // to pcl ros time stamp
}
@@ -212,17 +257,19 @@ uint32_t Lddc::PublishPointcloudData(LidarDataQueue* queue, uint32_t packet_num,
uint8_t point_buf[2048];
if (kSourceLvxFile != data_source) {
PointConvertHandler pf_point_convert = GetConvertHandler(raw_packet->data_type);
PointConvertHandler pf_point_convert =
GetConvertHandler(raw_packet->data_type);
if (pf_point_convert) {
pf_point_convert(point_buf, raw_packet, \
pf_point_convert(point_buf, raw_packet,
lds_->lidars_[handle].extrinsic_parameter);
} else {
/* Skip the packet */
ROS_INFO("Lidar[%d] unkown packet type[%d]", handle, raw_packet->data_type);
ROS_INFO("Lidar[%d] unkown packet type[%d]", handle,
raw_packet->data_type);
break;
}
} else {
LivoxPointToPxyzrtl(point_buf, raw_packet, \
LivoxPointToPxyzrtl(point_buf, raw_packet,
lds_->lidars_[handle].extrinsic_parameter);
}
@@ -247,15 +294,20 @@ uint32_t Lddc::PublishPointcloudData(LidarDataQueue* queue, uint32_t packet_num,
p_publisher->publish(cloud);
} else {
if (bag_) {
bag_->write(p_publisher->getTopic(), ros::Time(timestamp/1000000000.0), cloud);
bag_->write(p_publisher->getTopic(), ros::Time(timestamp / 1000000000.0),
cloud);
}
}
if (!lds_->lidars_[handle].data_is_pubulished) {
lds_->lidars_[handle].data_is_pubulished = true;
}
return published_packet;
}
uint32_t Lddc::PublishCustomPointcloud(LidarDataQueue* queue, uint32_t packet_num, \
uint8_t handle) {
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;
@@ -276,24 +328,26 @@ uint32_t Lddc::PublishCustomPointcloud(LidarDataQueue* queue, uint32_t packet_nu
StoragePacket storage_packet;
while (published_packet < packet_num) {
QueueProPop(queue, &storage_packet);
LivoxEthPacket* raw_packet = reinterpret_cast<LivoxEthPacket *>(storage_packet.raw_data);
LivoxEthPacket *raw_packet =
reinterpret_cast<LivoxEthPacket *>(storage_packet.raw_data);
uint32_t point_interval = GetPointInterval(raw_packet->data_type);
uint32_t dual_point = 0;
if ((raw_packet->data_type == kDualExtendCartesian) || \
if ((raw_packet->data_type == kDualExtendCartesian) ||
(raw_packet->data_type == kDualExtendSpherical)) {
dual_point = 1;
}
timestamp = GetStoragePacketTimestamp(&storage_packet, data_source);
if (published_packet && \
((timestamp - last_timestamp) > kDeviceDisconnectThreshold)) {
if (((timestamp - last_timestamp) > kDeviceDisconnectThreshold) &&
published_packet && lds_->lidars_[handle].data_is_pubulished) {
ROS_INFO("Lidar[%d] packet loss", handle);
break;
}
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
livox_msg.header.stamp =
ros::Time(timestamp / 1000000000.0); // to ros time stamp
// ROS_DEBUG("[%d]:%ld %d", handle, livox_msg.timebase, point_interval);
} else {
packet_offset_time = (uint32_t)(timestamp - livox_msg.timebase);
@@ -302,17 +356,19 @@ uint32_t Lddc::PublishCustomPointcloud(LidarDataQueue* queue, uint32_t packet_nu
uint8_t point_buf[2048];
if (kSourceLvxFile != data_source) {
PointConvertHandler pf_point_convert = GetConvertHandler(raw_packet->data_type);
PointConvertHandler pf_point_convert =
GetConvertHandler(raw_packet->data_type);
if (pf_point_convert) {
pf_point_convert(point_buf, raw_packet, \
pf_point_convert(point_buf, raw_packet,
lds_->lidars_[handle].extrinsic_parameter);
} else {
/* Skip the packet */
ROS_INFO("Lidar[%d] unkown packet type[%d]", handle, raw_packet->data_type);
ROS_INFO("Lidar[%d] unkown packet type[%d]", handle,
raw_packet->data_type);
break;
}
} else {
LivoxPointToPxyzrtl(point_buf, raw_packet, \
LivoxPointToPxyzrtl(point_buf, raw_packet,
lds_->lidars_[handle].extrinsic_parameter);
}
@@ -344,10 +400,15 @@ uint32_t Lddc::PublishCustomPointcloud(LidarDataQueue* queue, uint32_t packet_nu
p_publisher->publish(livox_msg);
} else {
if (bag_) {
bag_->write(p_publisher->getTopic(), ros::Time(timestamp/1000000000.0), livox_msg);
bag_->write(p_publisher->getTopic(), ros::Time(timestamp / 1000000000.0),
livox_msg);
}
}
if (!lds_->lidars_[handle].data_is_pubulished) {
lds_->lidars_[handle].data_is_pubulished = true;
}
return published_packet;
}
@@ -362,9 +423,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);
LivoxEthPacket* raw_packet = reinterpret_cast<LivoxEthPacket *>(storage_packet.raw_data);
LivoxEthPacket *raw_packet =
reinterpret_cast<LivoxEthPacket *>(storage_packet.raw_data);
timestamp = GetStoragePacketTimestamp(&storage_packet, data_source);
imu_data.header.stamp = ros::Time(timestamp/1000000000.0); // to ros time stamp
if (timestamp >= 0) {
imu_data.header.stamp =
ros::Time(timestamp / 1000000000.0); // to ros time stamp
}
uint8_t point_buf[2048];
LivoxImuDataProcess(point_buf, raw_packet);
@@ -385,14 +450,14 @@ uint32_t Lddc::PublishImuData(LidarDataQueue* queue, uint32_t packet_num,
p_publisher->publish(imu_data);
} else {
if (bag_) {
bag_->write(p_publisher->getTopic(), ros::Time(timestamp/1000000000.0), imu_data);
bag_->write(p_publisher->getTopic(), ros::Time(timestamp / 1000000000.0),
imu_data);
}
}
return published_packet;
}
int Lddc::RegisterLds(Lds *lds) {
if (lds_ == nullptr) {
lds_ = lds;
@@ -410,27 +475,18 @@ void Lddc::PollingLidarPointCloudData(uint8_t handle, LidarDevice* lidar) {
while (!QueueIsEmpty(p_queue)) {
uint32_t used_size = QueueUsedSize(p_queue);
uint32_t publish_packet_upper_limit = GetPacketNumPerSec(lidar->info.type);
uint32_t publish_packet_lower_limit = publish_packet_upper_limit / 2 / ((uint32_t)publish_frq_);
/** increase margin */
publish_packet_upper_limit = publish_packet_upper_limit + publish_packet_upper_limit / 10;
if (used_size < publish_packet_lower_limit) {
uint32_t onetime_publish_packets =
GetPacketNumPerSec(lidar->info.type) / publish_frq_;
if (used_size < onetime_publish_packets) {
break;
}
if (used_size > publish_packet_upper_limit) {
used_size = publish_packet_upper_limit;
}
if (kPointCloud2Msg == transfer_format_) {
if (used_size == PublishPointcloud2(p_queue, used_size, handle)) {
}
PublishPointcloud2(p_queue, onetime_publish_packets, handle);
} else if (kLivoxCustomMsg == transfer_format_) {
if (used_size == PublishCustomPointcloud(p_queue, used_size, handle)) {
}
PublishCustomPointcloud(p_queue, onetime_publish_packets, handle);
} else if (kPclPxyziMsg == transfer_format_) {
if (used_size == PublishPointcloudData(p_queue, used_size, handle)) {
}
PublishPointcloudData(p_queue, onetime_publish_packets, handle);
}
}
}
@@ -455,7 +511,8 @@ void Lddc::DistributeLidarData(void) {
uint32_t lidar_id = i;
LidarDevice *lidar = &lds_->lidars_[lidar_id];
LidarDataQueue *p_queue = &lidar->data;
if ((kConnectStateSampling!= lidar->connect_state) || (p_queue == nullptr)) {
if ((kConnectStateSampling != lidar->connect_state) ||
(p_queue == nullptr)) {
continue;
}
@@ -470,20 +527,20 @@ void Lddc::DistributeLidarData(void) {
ros::Publisher *Lddc::GetCurrentPublisher(uint8_t handle) {
ros::Publisher **pub = nullptr;
uint32_t queue_size = kMinEthPacketQueueSize * 4;
uint32_t queue_size = kMinEthPacketQueueSize;
if (use_multi_topic_) {
pub = &private_pub_[handle];
} else {
pub = &global_pub_;
queue_size = queue_size * 32;
queue_size = queue_size * 8;
}
if (*pub == nullptr) {
char name_str[48];
memset(name_str, 0, sizeof(name_str));
if (use_multi_topic_) {
snprintf(name_str, sizeof(name_str), "livox/lidar_%s", \
snprintf(name_str, sizeof(name_str), "livox/lidar_%s",
lds_->lidars_[handle].info.broadcast_code);
ROS_INFO("Support multi topics.");
} else {
@@ -493,19 +550,21 @@ ros::Publisher* Lddc::GetCurrentPublisher(uint8_t handle) {
*pub = new ros::Publisher;
if (kPointCloud2Msg == transfer_format_) {
**pub = cur_node_->advertise<sensor_msgs::PointCloud2>(name_str,\
queue_size);
ROS_INFO("%s publish use PointCloud2 format, publisher queue size [%d]", \
**pub =
cur_node_->advertise<sensor_msgs::PointCloud2>(name_str, queue_size);
ROS_INFO(
"%s publish use PointCloud2 format, set ROS publisher queue size %d",
name_str, queue_size);
} else if (kLivoxCustomMsg == transfer_format_) {
**pub = cur_node_->advertise<livox_ros_driver::CustomMsg>(name_str,\
**pub = cur_node_->advertise<livox_ros_driver::CustomMsg>(name_str,
queue_size);
ROS_INFO("%s publish use livox custom format, publisher queue size [%d]", \
ROS_INFO(
"%s publish use livox custom format, set ROS publisher queue size %d",
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, publisher queue size [%d]", \
**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);
}
}
@@ -515,13 +574,13 @@ ros::Publisher* Lddc::GetCurrentPublisher(uint8_t handle) {
ros::Publisher *Lddc::GetCurrentImuPublisher(uint8_t handle) {
ros::Publisher **pub = nullptr;
uint32_t queue_size = kMinEthPacketQueueSize * 4;
uint32_t queue_size = kMinEthPacketQueueSize;
if (use_multi_topic_) {
pub = &private_imu_pub_[handle];
} else {
pub = &global_imu_pub_;
queue_size = queue_size * 32;
queue_size = queue_size * 4;
}
if (*pub == nullptr) {
@@ -529,7 +588,7 @@ ros::Publisher* Lddc::GetCurrentImuPublisher(uint8_t handle) {
memset(name_str, 0, sizeof(name_str));
if (use_multi_topic_) {
ROS_INFO("Support multi topics.");
snprintf(name_str, sizeof(name_str), "livox/imu_%s", \
snprintf(name_str, sizeof(name_str), "livox/imu_%s",
lds_->lidars_[handle].info.broadcast_code);
} else {
ROS_INFO("Support only one topic.");
@@ -538,13 +597,13 @@ ros::Publisher* Lddc::GetCurrentImuPublisher(uint8_t handle) {
*pub = new ros::Publisher;
**pub = cur_node_->advertise<sensor_msgs::Imu>(name_str, queue_size);
ROS_INFO("%s publish imu data, Publisher QueueSize[%d]", name_str, queue_size);
ROS_INFO("%s publish imu data, set ROS publisher queue size %d", name_str,
queue_size);
}
return *pub;
}
void Lddc::CreateBagFile(const std::string &file_name) {
if (!bag_) {
bag_ = new rosbag::Bag;
@@ -566,4 +625,4 @@ void Lddc::PrepareExit(void) {
}
}
}
} // namespace livox_ros

12
livox_ros_driver/livox_ros_driver/lddc.h
View File

@@ -24,8 +24,8 @@
#ifndef LIVOX_ROS_DRIVER_LDDC_H_
#define LIVOX_ROS_DRIVER_LDDC_H_
#include "livox_sdk.h"
#include "lds.h"
#include "livox_sdk.h"
#include <ros/ros.h>
#include <rosbag/bag.h>
@@ -59,13 +59,13 @@ public:
Lds *lds_;
private:
uint32_t PublishPointcloud2(LidarDataQueue* queue, uint32_t packet_num, \
uint32_t PublishPointcloud2(LidarDataQueue *queue, uint32_t packet_num,
uint8_t handle);
uint32_t PublishPointcloudData(LidarDataQueue* queue, uint32_t packet_num, \
uint32_t PublishPointcloudData(LidarDataQueue *queue, uint32_t packet_num,
uint8_t handle);
uint32_t PublishCustomPointcloud(LidarDataQueue* queue, uint32_t packet_num,\
uint32_t PublishCustomPointcloud(LidarDataQueue *queue, uint32_t packet_num,
uint8_t handle);
uint32_t PublishImuData(LidarDataQueue* queue, uint32_t packet_num,\
uint32_t PublishImuData(LidarDataQueue *queue, uint32_t packet_num,
uint8_t handle);
ros::Publisher *GetCurrentPublisher(uint8_t handle);
@@ -88,5 +88,5 @@ private:
rosbag::Bag *bag_;
};
}
} // namespace livox_ros
#endif

16
livox_ros_driver/livox_ros_driver/ldq.cpp
View File

@@ -24,8 +24,8 @@
#include "ldq.h"
#include <string.h>
#include <stdio.h>
#include <string.h>
namespace livox_ros {
@@ -79,12 +79,11 @@ void ResetQueue(LidarDataQueue* queue) {
void QueueProPop(LidarDataQueue *queue, StoragePacket *storage_packet) {
uint32_t rd_idx = queue->rd_idx & queue->mask;
memcpy(storage_packet, &(queue->storage_packet[rd_idx]), sizeof(StoragePacket));
memcpy(storage_packet, &(queue->storage_packet[rd_idx]),
sizeof(StoragePacket));
}
void QueuePopUpdate(LidarDataQueue* queue) {
queue->rd_idx++;
}
void QueuePopUpdate(LidarDataQueue *queue) { queue->rd_idx++; }
uint32_t QueuePop(LidarDataQueue *queue, StoragePacket *storage_packet) {
QueueProPop(queue, storage_packet);
@@ -112,8 +111,7 @@ uint32_t QueueIsEmpty(LidarDataQueue* queue) {
uint32_t QueuePush(LidarDataQueue *queue, StoragePacket *storage_packet) {
uint32_t wr_idx = queue->wr_idx & queue->mask;
memcpy((void *)(&(queue->storage_packet[wr_idx])), \
(void *)(storage_packet), \
memcpy((void *)(&(queue->storage_packet[wr_idx])), (void *)(storage_packet),
sizeof(StoragePacket));
queue->wr_idx++;
@@ -121,7 +119,7 @@ uint32_t QueuePush(LidarDataQueue* queue, StoragePacket* storage_packet) {
return 1;
}
uint32_t QueuePushAny(LidarDataQueue* queue, uint8_t *data, uint32_t length, \
uint32_t QueuePushAny(LidarDataQueue *queue, uint8_t *data, uint32_t length,
uint64_t time_rcv, uint32_t point_num) {
uint32_t wr_idx = queue->wr_idx & queue->mask;
@@ -134,4 +132,4 @@ uint32_t QueuePushAny(LidarDataQueue* queue, uint8_t *data, uint32_t length, \
return 1;
}
}
} // namespace livox_ros

5
livox_ros_driver/livox_ros_driver/ldq.h
View File

@@ -33,7 +33,6 @@ namespace livox_ros {
const uint32_t KEthPacketMaxLength = 1500;
#pragma pack(1)
typedef struct {
@@ -80,8 +79,8 @@ uint32_t QueueUnusedSize(LidarDataQueue* queue);
uint32_t QueueIsFull(LidarDataQueue *queue);
uint32_t QueueIsEmpty(LidarDataQueue *queue);
uint32_t QueuePush(LidarDataQueue *queue, StoragePacket *storage_packet);
uint32_t QueuePushAny(LidarDataQueue* queue, uint8_t *data, uint32_t length, \
uint32_t QueuePushAny(LidarDataQueue *queue, uint8_t *data, uint32_t length,
uint64_t time_rcv, uint32_t point_num);
}
} // namespace livox_ros
#endif

122
livox_ros_driver/livox_ros_driver/lds.cpp
View File

@@ -24,15 +24,16 @@
#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);
@@ -45,7 +46,8 @@ bool IsFilePathValid(const char* path_str) {
uint64_t GetStoragePacketTimestamp(StoragePacket *packet, uint8_t data_src_) {
LivoxEthPacket* raw_packet = reinterpret_cast<LivoxEthPacket *>(packet->raw_data);
LivoxEthPacket *raw_packet =
reinterpret_cast<LivoxEthPacket *>(packet->raw_data);
LdsStamp timestamp;
memcpy(timestamp.stamp_bytes, raw_packet->timestamp, sizeof(timestamp));
@@ -102,7 +104,7 @@ void ParseCommandlineInputBdCode(const char* cammandline_str,
std::string invalid_bd = "000000000";
while (bd_str != NULL) {
printf("Commandline input bd:%s\n", bd_str);
if ((kBdCodeSize == strlen(bd_str)) && \
if ((kBdCodeSize == strlen(bd_str)) &&
(NULL == strstr(bd_str, invalid_bd.c_str()))) {
bd_code_list.push_back(bd_str);
} else {
@@ -138,12 +140,16 @@ void EulerAnglesToRotationMatrix(EulerAngle euler, RotationMatrix matrix) {
float rotate[3][3] = {
{
std::cos(info.pitch) * std::cos(info.yaw),
std::sin(info.roll) * std::sin(info.pitch) * std::cos(info.yaw) - std::cos(info.roll) * std::sin(info.yaw),
std::cos(info.roll) * std::sin(info.pitch) * std::cos(info.yaw) + std::sin(info.roll) * std::sin(info.yaw) },
std::sin(info.roll) * std::sin(info.pitch) * std::cos(info.yaw) -
std::cos(info.roll) * std::sin(info.yaw), std::cos(info.roll) *
std::sin(info.pitch) * std::cos(info.yaw) + std::sin(info.roll) *
std::sin(info.yaw) },
{
std::cos(info.pitch) * std::sin(info.yaw),
std::sin(info.roll) * std::sin(info.pitch) * std::sin(info.yaw) + std::cos(info.roll) * std::cos(info.yaw),
std::cos(info.roll) * std::sin(info.pitch) * std::sin(info.yaw) - std::sin(info.roll) * std::cos(info.yaw) },
std::sin(info.roll) * std::sin(info.pitch) * std::sin(info.yaw) +
std::cos(info.roll) * std::cos(info.yaw), std::cos(info.roll) *
std::sin(info.pitch) * std::sin(info.yaw) - std::sin(info.roll) *
std::cos(info.yaw) },
{
-std::sin(info.pitch),
std::sin(info.roll) * std::cos(info.pitch),
@@ -153,27 +159,22 @@ void EulerAnglesToRotationMatrix(EulerAngle euler, RotationMatrix matrix) {
*/
}
void PointExtrisincCompensation(PointXyz* dst_point, const PointXyz& src_point, \
void PointExtrisincCompensation(PointXyz *dst_point, const PointXyz &src_point,
ExtrinsicParameter &extrinsic) {
dst_point->x = src_point.x * extrinsic.rotation[0][0] + \
src_point.y * extrinsic.rotation[0][1] + \
src_point.z * extrinsic.rotation[0][2] + \
extrinsic.trans[0];
dst_point->y = src_point.x * extrinsic.rotation[1][0] + \
src_point.y * extrinsic.rotation[1][1] + \
src_point.z * extrinsic.rotation[1][2] + \
extrinsic.trans[1];
dst_point->z = src_point.x * extrinsic.rotation[2][0] + \
src_point.y * extrinsic.rotation[2][1] + \
src_point.z * extrinsic.rotation[2][2] +
extrinsic.trans[2];
dst_point->x = src_point.x * extrinsic.rotation[0][0] +
src_point.y * extrinsic.rotation[0][1] +
src_point.z * extrinsic.rotation[0][2] + extrinsic.trans[0];
dst_point->y = src_point.x * extrinsic.rotation[1][0] +
src_point.y * extrinsic.rotation[1][1] +
src_point.z * extrinsic.rotation[1][2] + extrinsic.trans[1];
dst_point->z = src_point.x * extrinsic.rotation[2][0] +
src_point.y * extrinsic.rotation[2][1] +
src_point.z * extrinsic.rotation[2][2] + extrinsic.trans[2];
}
/** Livox point procees for different raw data format --------------------------------------------*/
uint8_t* LivoxPointToPxyzrtl(uint8_t* point_buf, LivoxEthPacket* eth_packet, \
/** Livox point procees for different raw data format
* --------------------------------------------*/
uint8_t *LivoxPointToPxyzrtl(uint8_t *point_buf, LivoxEthPacket *eth_packet,
ExtrinsicParameter &extrinsic) {
LivoxPointXyzrtl *dst_point = (LivoxPointXyzrtl *)point_buf;
uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type);
@@ -195,11 +196,12 @@ 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, \
uint8_t *LivoxRawPointToPxyzrtl(uint8_t *point_buf, LivoxEthPacket *eth_packet,
ExtrinsicParameter &extrinsic) {
LivoxPointXyzrtl *dst_point = (LivoxPointXyzrtl *)point_buf;
uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type);
LivoxRawPoint* raw_point = reinterpret_cast<LivoxRawPoint *>(eth_packet->data);
LivoxRawPoint *raw_point =
reinterpret_cast<LivoxRawPoint *>(eth_packet->data);
while (points_per_packet) {
RawPointConvert((LivoxPointXyzr *)dst_point, raw_point);
@@ -217,11 +219,13 @@ 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, \
uint8_t *LivoxSpherPointToPxyzrtl(uint8_t *point_buf,
LivoxEthPacket *eth_packet,
ExtrinsicParameter &extrinsic) {
LivoxPointXyzrtl *dst_point = (LivoxPointXyzrtl *)point_buf;
uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type);
LivoxSpherPoint* raw_point = reinterpret_cast<LivoxSpherPoint *>(eth_packet->data);
LivoxSpherPoint *raw_point =
reinterpret_cast<LivoxSpherPoint *>(eth_packet->data);
while (points_per_packet) {
RawPointConvert((LivoxPointXyzr *)dst_point, raw_point);
@@ -239,11 +243,12 @@ uint8_t* LivoxSpherPointToPxyzrtl(uint8_t* point_buf, LivoxEthPacket* eth_packet
return (uint8_t *)dst_point;
}
uint8_t* LivoxExtendRawPointToPxyzrtl(uint8_t* point_buf, LivoxEthPacket* eth_packet, \
uint8_t *LivoxExtendRawPointToPxyzrtl(uint8_t *point_buf,
LivoxEthPacket *eth_packet,
ExtrinsicParameter &extrinsic) {
LivoxPointXyzrtl *dst_point = (LivoxPointXyzrtl *)point_buf;
uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type);
LivoxExtendRawPoint* raw_point = \
LivoxExtendRawPoint *raw_point =
reinterpret_cast<LivoxExtendRawPoint *>(eth_packet->data);
uint8_t line_id = 0;
@@ -265,11 +270,13 @@ uint8_t* LivoxExtendRawPointToPxyzrtl(uint8_t* point_buf, LivoxEthPacket* eth_p
return (uint8_t *)dst_point;
}
uint8_t* LivoxExtendSpherPointToPxyzrtl(uint8_t* point_buf, LivoxEthPacket* eth_packet, \
uint8_t *LivoxExtendSpherPointToPxyzrtl(uint8_t *point_buf,
LivoxEthPacket *eth_packet,
ExtrinsicParameter &extrinsic) {
LivoxPointXyzrtl *dst_point = (LivoxPointXyzrtl *)point_buf;
uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type);
LivoxExtendSpherPoint* raw_point = reinterpret_cast<LivoxExtendSpherPoint *>(eth_packet->data);
LivoxExtendSpherPoint *raw_point =
reinterpret_cast<LivoxExtendSpherPoint *>(eth_packet->data);
uint8_t line_id = 0;
while (points_per_packet) {
@@ -290,11 +297,12 @@ uint8_t* LivoxExtendSpherPointToPxyzrtl(uint8_t* point_buf, LivoxEthPacket* eth
return (uint8_t *)dst_point;
}
uint8_t* LivoxDualExtendRawPointToPxyzrtl(uint8_t* point_buf, LivoxEthPacket* eth_packet, \
uint8_t *LivoxDualExtendRawPointToPxyzrtl(uint8_t *point_buf,
LivoxEthPacket *eth_packet,
ExtrinsicParameter &extrinsic) {
LivoxPointXyzrtl *dst_point = (LivoxPointXyzrtl *)point_buf;
uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type);
LivoxExtendRawPoint* raw_point = \
LivoxExtendRawPoint *raw_point =
reinterpret_cast<LivoxExtendRawPoint *>(eth_packet->data);
uint8_t line_id = 0;
@@ -305,7 +313,8 @@ uint8_t* LivoxDualExtendRawPointToPxyzrtl(uint8_t* point_buf, LivoxEthPacket* et
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 =
line_id / 2; /* LivoxDualExtendRawPoint = 2*LivoxExtendRawPoint */
dst_point->line = dst_point->line % 6;
++raw_point;
++dst_point;
@@ -316,16 +325,18 @@ uint8_t* LivoxDualExtendRawPointToPxyzrtl(uint8_t* point_buf, LivoxEthPacket* et
return (uint8_t *)dst_point;
}
uint8_t* LivoxDualExtendSpherPointToPxyzrtl(uint8_t* point_buf, LivoxEthPacket* eth_packet, \
uint8_t *LivoxDualExtendSpherPointToPxyzrtl(uint8_t *point_buf,
LivoxEthPacket *eth_packet,
ExtrinsicParameter &extrinsic) {
LivoxPointXyzrtl *dst_point = (LivoxPointXyzrtl *)point_buf;
uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type);
LivoxDualExtendSpherPoint* raw_point = \
LivoxDualExtendSpherPoint *raw_point =
reinterpret_cast<LivoxDualExtendSpherPoint *>(eth_packet->data);
uint8_t line_id = 0;
while (points_per_packet) {
RawPointConvert((LivoxPointXyzr*)dst_point, (LivoxPointXyzr *)(dst_point + 1), \
RawPointConvert((LivoxPointXyzr *)dst_point,
(LivoxPointXyzr *)(dst_point + 1),
(LivoxDualExtendSpherPoint *)raw_point);
if (extrinsic.enable) {
PointXyz src_point = *((PointXyz *)dst_point);
@@ -365,8 +376,7 @@ const PointConvertHandler to_pxyzi_handler_table[kMaxPointDataType] = {
LivoxExtendSpherPointToPxyzrtl,
LivoxDualExtendRawPointToPxyzrtl,
LivoxDualExtendSpherPointToPxyzrtl,
nullptr
};
nullptr};
PointConvertHandler GetConvertHandler(uint8_t data_type) {
if (data_type < kMaxPointDataType)
@@ -375,7 +385,23 @@ 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;
}
#if 0
@@ -388,7 +414,8 @@ static void PointCloudConvert(LivoxPoint *p_dpoint, LivoxRawPoint *p_raw_point)
#endif
/* Member function ------ ----------------------------------------------------------------------- */
/* Member function ------
* ----------------------------------------------------------------------- */
Lds::Lds(uint32_t buffer_time_ms, uint8_t data_src)
: buffer_time_ms_(buffer_time_ms), data_src_(data_src) {
@@ -410,8 +437,9 @@ void Lds::ResetLidar(LidarDevice* lidar, uint8_t data_src) {
/** unallocated state */
lidar->handle = kMaxSourceLidar;
lidar->connect_state = kConnectStateOff;
lidar->data_src = data_src;
lidar->data_is_pubulished = false;
lidar->connect_state = kConnectStateOff;
}
void Lds::SetLidarDataSrc(LidarDevice *lidar, uint8_t data_src) {
@@ -433,8 +461,6 @@ uint8_t Lds::GetDeviceType(uint8_t handle) {
}
}
void Lds::PrepareExit(void) {
void Lds::PrepareExit(void) {}
}
}
} // namespace livox_ros

65
livox_ros_driver/livox_ros_driver/lds.h
View File

@@ -27,12 +27,12 @@
#ifndef LIVOX_ROS_DRIVER_LDS_H_
#define LIVOX_ROS_DRIVER_LDS_H_
#include <math.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <vector>
#include <string>
#include <vector>
#include "ldq.h"
@@ -55,8 +55,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 */
const int64_t kMaxPacketTimeGap =
1700000; /**< the threshold of packet continuous */
const int64_t kDeviceDisconnectThreshold =
1000000000; /**< the threshold of device disconect */
const int64_t kNsPerSecond = 1000000000; /**< 1s = 1000000000ns */
const int kPathStrMinSize = 4; /**< Must more than 4 char */
@@ -90,10 +92,7 @@ typedef enum {
kOutputToRosBagFile = 1,
} LidarDataOutputType;
typedef enum {
kCoordinateCartesian = 0,
kCoordinateSpherical
} CoordinateType;
typedef enum { kCoordinateCartesian = 0, kCoordinateSpherical } CoordinateType;
typedef enum {
kConfigFan = 1,
@@ -167,6 +166,8 @@ typedef struct {
typedef struct {
uint8_t handle; /**< Lidar access handle. */
uint8_t data_src; /**< From raw lidar or livox file. */
bool data_is_pubulished; /**< Indicate the data of lidar whether is
pubulished. */
volatile LidarConnectState connect_state;
DeviceInfo info;
LidarPacketStatistic statistic_info;
@@ -179,8 +180,9 @@ typedef struct {
typedef struct {
uint32_t points_per_packet;
uint32_t points_per_second; /**< unit:ns */
uint32_t point_interval;
uint32_t points_per_second;
uint32_t point_interval; /**< unit:ns */
uint32_t packet_interval; /**< unit:ns */
uint32_t packet_length;
} PacketInfoPair;
@@ -210,19 +212,15 @@ typedef struct{
#pragma pack()
typedef uint8_t* (* PointConvertHandler)(uint8_t* point_buf, LivoxEthPacket* eth_packet, \
typedef uint8_t *(*PointConvertHandler)(uint8_t *point_buf,
LivoxEthPacket *eth_packet,
ExtrinsicParameter &extrinsic);
const PacketInfoPair packet_info_pair_table[kMaxPointDataType] = {
{100, 100000, 10000 , 1318},
{100, 100000, 10000 , 918},
{96, 240000, 4167 , 1362},
{96, 240000, 4167 , 978},
{96, 480000, 4167 , 1362},
{48, 480000, 4167 , 978},
{1, 100, 10000000, 42}
};
{100, 100000, 10000, 1000000, 1318}, {100, 100000, 10000, 1000000, 918},
{96, 240000, 4167, 400000, 1362}, {96, 240000, 4167, 400000, 978},
{96, 480000, 4167, 400000, 1362}, {48, 480000, 4167, 400000, 978},
{1, 100, 10000000, 10000000, 42}};
/**
* Global function for general use.
@@ -233,25 +231,31 @@ uint32_t CalculatePacketQueueSize(uint32_t interval_ms, uint32_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, \
uint8_t *LivoxPointToPxyzrtl(uint8_t *point_buf, LivoxEthPacket *eth_packet,
ExtrinsicParameter &extrinsic);
uint8_t *FillZeroPointXyzrtl(uint8_t *point_buf, uint32_t num);
uint8_t *LivoxImuDataProcess(uint8_t *point_buf, LivoxEthPacket *eth_packet);
void EulerAnglesToRotationMatrix(EulerAngle euler, RotationMatrix matrix);
void PointExtrisincCompensation(PointXyz* dst_point, ExtrinsicParameter& extrinsic);
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 GetPacketNumPerSec(uint32_t data_type) {
return packet_info_pair_table[data_type].points_per_second / packet_info_pair_table[data_type].points_per_packet;
return packet_info_pair_table[data_type].points_per_second /
packet_info_pair_table[data_type].points_per_packet;
}
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;
}
inline uint32_t GetEthPacketLen(uint32_t data_type) {
return packet_info_pair_table[data_type].packet_length;
}
@@ -263,14 +267,16 @@ inline void RawPointConvert(LivoxPointXyzr* dst_point, LivoxPoint* raw_point) {
dst_point->reflectivity = (float)raw_point->reflectivity;
}
inline void RawPointConvert(LivoxPointXyzr* dst_point, LivoxRawPoint* raw_point) {
inline void RawPointConvert(LivoxPointXyzr *dst_point,
LivoxRawPoint *raw_point) {
dst_point->x = raw_point->x / 1000.0f;
dst_point->y = raw_point->y / 1000.0f;
dst_point->z = raw_point->z / 1000.0f;
dst_point->reflectivity = (float)raw_point->reflectivity;
}
inline void RawPointConvert(LivoxPointXyzr* dst_point, LivoxSpherPoint* raw_point) {
inline void RawPointConvert(LivoxPointXyzr *dst_point,
LivoxSpherPoint *raw_point) {
double radius = raw_point->depth / 1000.0;
double theta = raw_point->theta / 100.0 / 180 * PI;
double phi = raw_point->phi / 100.0 / 180 * PI;
@@ -280,8 +286,8 @@ inline void RawPointConvert(LivoxPointXyzr* dst_point, LivoxSpherPoint* raw_poin
dst_point->reflectivity = (float)raw_point->reflectivity;
}
inline void RawPointConvert(LivoxPointXyzr* dst_point1, LivoxPointXyzr* dst_point2, \
inline void RawPointConvert(LivoxPointXyzr *dst_point1,
LivoxPointXyzr *dst_point2,
LivoxDualExtendSpherPoint *raw_point) {
double radius1 = raw_point->depth1 / 1000.0;
double radius2 = raw_point->depth2 / 1000.0;
@@ -321,9 +327,10 @@ public:
protected:
uint32_t buffer_time_ms_; /**< Buffer time before data in queue is read */
uint8_t data_src_;
private:
volatile bool request_exit_;
};
}
} // namespace livox_ros
#endif

219
livox_ros_driver/livox_ros_driver/lds_hub.cpp
View File

@@ -24,26 +24,29 @@
#include "lds_hub.h"
#include <memory>
#include <stdio.h>
#include <string.h>
#include <thread>
#include <memory>
#include "rapidjson/document.h"
#include "rapidjson/stringbuffer.h"
#include "rapidjson/filereadstream.h"
#include "rapidjson/stringbuffer.h"
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;
@@ -55,15 +58,14 @@ LdsHub::LdsHub(uint32_t interval_ms) : Lds(interval_ms, kSourceRawHub) {
ResetLdsHub();
}
LdsHub::~LdsHub() {
}
LdsHub::~LdsHub() {}
void LdsHub::ResetLdsHub(void) {
ResetLds(kSourceRawHub);
ResetLidar(&hub_, kSourceRawHub);
}
int LdsHub::InitLdsHub(std::vector<std::string>& broadcast_code_strs,\
int LdsHub::InitLdsHub(std::vector<std::string> &broadcast_code_strs,
const char *user_config_path) {
if (is_initialized_) {
@@ -79,7 +81,8 @@ int LdsHub::InitLdsHub(std::vector<std::string>& broadcast_code_strs,\
LivoxSdkVersion _sdkversion;
GetLivoxSdkVersion(&_sdkversion);
printf("Livox SDK version %d.%d.%d\n", _sdkversion.major, _sdkversion.minor, _sdkversion.patch);
printf("Livox SDK version %d.%d.%d\n", _sdkversion.major, _sdkversion.minor,
_sdkversion.patch);
SetBroadcastCallback(LdsHub::OnDeviceBroadcast);
SetDeviceStateUpdateCallback(LdsHub::OnDeviceChange);
@@ -101,7 +104,8 @@ 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 */
@@ -134,9 +138,7 @@ int LdsHub::DeInitLdsHub(void) {
return 0;
}
void LdsHub::PrepareExit(void) {
DeInitLdsHub();
}
void LdsHub::PrepareExit(void) { DeInitLdsHub(); }
/** Static function in LdsLidar for callback */
void LdsHub::OnHubDataCb(uint8_t hub_handle, LivoxEthPacket *data,
@@ -156,12 +158,13 @@ void LdsHub::OnHubDataCb(uint8_t hub_handle, LivoxEthPacket *data,
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));
memcpy(cur_timestamp.stamp_bytes, eth_packet->timestamp,
sizeof(cur_timestamp));
if (kImu != eth_packet->data_type) {
if (eth_packet->timestamp_type == kTimestampTypePps) {
/** Whether a new sync frame */
if ((cur_timestamp.stamp < packet_statistic->last_timestamp) && \
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();
@@ -173,22 +176,23 @@ void LdsHub::OnHubDataCb(uint8_t hub_handle, LivoxEthPacket *data,
LidarDataQueue *p_queue = &p_lidar->data;
if (nullptr == p_queue->storage_packet) {
uint32_t queue_size = CalculatePacketQueueSize(lds_hub->buffer_time_ms_, \
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);
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, \
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) && \
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();
@@ -202,13 +206,14 @@ void LdsHub::OnHubDataCb(uint8_t hub_handle, LivoxEthPacket *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,\
printf("Lidar%02d[%s] imu queue size : %d %d\n", p_lidar->handle,
p_lidar->info.broadcast_code, queue_size, p_queue->size);
}
if (!QueueIsFull(p_queue) && (cur_timestamp.stamp >= 0)) {
QueuePushAny(p_queue, (uint8_t *)eth_packet,\
GetEthPacketLen(eth_packet->data_type), packet_statistic->imu_timebase, \
QueuePushAny(p_queue, (uint8_t *)eth_packet,
GetEthPacketLen(eth_packet->data_type),
packet_statistic->imu_timebase,
GetPointsPerPacket(eth_packet->data_type));
}
}
@@ -225,10 +230,11 @@ void LdsHub::OnDeviceBroadcast(const BroadcastDeviceInfo *info) {
}
if (g_lds_hub->IsAutoConnectMode()) {
printf("In automatic connection mode, will connect %s\n", info->broadcast_code);
printf("In automatic connection mode, will connect %s\n",
info->broadcast_code);
} else {
if (!g_lds_hub->IsBroadcastCodeExistInWhitelist(info->broadcast_code)) {
printf("Not in the whitelist, please add %s to if want to connect!\n",\
printf("Not in the whitelist, please add %s to if want to connect!\n",
info->broadcast_code);
return;
}
@@ -247,7 +253,8 @@ void LdsHub::OnDeviceBroadcast(const BroadcastDeviceInfo *info) {
printf("add to connect\n");
UserRawConfig config;
if (strncmp(info->broadcast_code, g_lds_hub->hub_raw_config_.broadcast_code, \
if (strncmp(info->broadcast_code,
g_lds_hub->hub_raw_config_.broadcast_code,
sizeof(info->broadcast_code)) != 0) {
printf("Could not find hub raw config, set config to default!\n");
config.enable_fan = 1;
@@ -294,10 +301,9 @@ void LdsHub::OnDeviceChange(const DeviceInfo *info, DeviceEvent type) {
}
if (p_hub->connect_state == kConnectStateOn) {
printf("Hub[%s] status_code[%d] working state[%d] feature[%d]\n", \
p_hub->info.broadcast_code,\
p_hub->info.status.status_code.error_code,\
p_hub->info.state,\
printf("Hub[%s] status_code[%d] working state[%d] feature[%d]\n",
p_hub->info.broadcast_code,
p_hub->info.status.status_code.error_code, p_hub->info.state,
p_hub->info.feature);
SetErrorMessageCallback(p_hub->handle, LdsHub::HubErrorStatusCb);
if (p_hub->info.state == kLidarStateNormal) {
@@ -306,8 +312,8 @@ void LdsHub::OnDeviceChange(const DeviceInfo *info, DeviceEvent type) {
}
}
void LdsHub::HubQueryLidarInfoCb(livox_status status, uint8_t handle, \
HubQueryLidarInformationResponse *response,\
void LdsHub::HubQueryLidarInfoCb(livox_status status, uint8_t handle,
HubQueryLidarInformationResponse *response,
void *client_data) {
LdsHub *lds_hub = static_cast<LdsHub *>(client_data);
if ((handle >= kMaxLidarCount) || !response) {
@@ -318,7 +324,7 @@ void LdsHub::HubQueryLidarInfoCb(livox_status status, uint8_t handle, \
if (response->count) {
printf("Hub have %d lidars:\n", response->count);
for (int i = 0; i < response->count; i++) {
uint32_t index = HubGetLidarHandle(response->device_info_list[i].slot,\
uint32_t index = HubGetLidarHandle(response->device_info_list[i].slot,
response->device_info_list[i].id);
if (index < kMaxLidarCount) {
LidarDevice *p_lidar = &lds_hub->lidars_[index];
@@ -328,15 +334,14 @@ void LdsHub::HubQueryLidarInfoCb(livox_status status, uint8_t handle, \
p_lidar->info.id = response->device_info_list[i].id;
p_lidar->info.type = response->device_info_list[i].dev_type;
p_lidar->connect_state = kConnectStateSampling;
strncpy(p_lidar->info.broadcast_code, \
response->device_info_list[i].broadcast_code, \
strncpy(p_lidar->info.broadcast_code,
response->device_info_list[i].broadcast_code,
sizeof(p_lidar->info.broadcast_code));
printf("[%d]%s DeviceType[%d] Slot[%d] Ver[%d.%d.%d.%d]\n", index, \
p_lidar->info.broadcast_code,\
p_lidar->info.type, p_lidar->info.slot,\
response->device_info_list[i].version[0],\
response->device_info_list[i].version[1],\
response->device_info_list[i].version[2],\
printf("[%d]%s DeviceType[%d] Slot[%d] Ver[%d.%d.%d.%d]\n", index,
p_lidar->info.broadcast_code, p_lidar->info.type,
p_lidar->info.slot, response->device_info_list[i].version[0],
response->device_info_list[i].version[1],
response->device_info_list[i].version[2],
response->device_info_list[i].version[3]);
}
}
@@ -352,7 +357,8 @@ void LdsHub::HubQueryLidarInfoCb(livox_status status, uint8_t handle, \
}
/** Callback function of hub error message. */
void LdsHub::HubErrorStatusCb(livox_status status, uint8_t handle, ErrorMessage *message) {
void LdsHub::HubErrorStatusCb(livox_status status, uint8_t handle,
ErrorMessage *message) {
static uint32_t error_message_count = 0;
if (message != NULL) {
++error_message_count;
@@ -361,22 +367,20 @@ void LdsHub::HubErrorStatusCb(livox_status status, uint8_t handle, ErrorMessage
printf("sync_status : %u\n", message->hub_error_code.sync_status);
printf("temp_status : %u\n", message->hub_error_code.temp_status);
printf("lidar_status :%u\n", message->hub_error_code.lidar_status);
printf("lidar_link_status : %u\n", message->hub_error_code.lidar_link_status);
printf("lidar_link_status : %u\n",
message->hub_error_code.lidar_link_status);
printf("firmware_err : %u\n", message->hub_error_code.firmware_err);
printf("system_status : %u\n", message->hub_error_code.system_status);
}
}
}
void LdsHub::ControlFanCb(livox_status status, uint8_t handle, \
uint8_t response, void *clent_data) {
void LdsHub::ControlFanCb(livox_status status, uint8_t handle, uint8_t response,
void *clent_data) {}
}
void LdsHub::HubSetPointCloudReturnModeCb(livox_status status, uint8_t handle, \
HubSetPointCloudReturnModeResponse* response,\
void *clent_data) {
void LdsHub::HubSetPointCloudReturnModeCb(
livox_status status, uint8_t handle,
HubSetPointCloudReturnModeResponse *response, void *clent_data) {
LdsHub *lds_hub = static_cast<LdsHub *>(clent_data);
if ((handle >= kMaxLidarCount) || !response) {
return;
@@ -395,7 +399,7 @@ void LdsHub::HubSetPointCloudReturnModeCb(livox_status status, uint8_t handle, \
}
}
void LdsHub::SetCoordinateCb(livox_status status, uint8_t handle, \
void LdsHub::SetCoordinateCb(livox_status status, uint8_t handle,
uint8_t response, void *clent_data) {
LdsHub *lds_hub = static_cast<LdsHub *>(clent_data);
@@ -422,9 +426,9 @@ void LdsHub::SetCoordinateCb(livox_status status, uint8_t handle, \
}
}
void LdsHub::HubSetImuRatePushFrequencyCb(livox_status status, uint8_t handle, \
HubSetImuPushFrequencyResponse* response,\
void *clent_data) {
void LdsHub::HubSetImuRatePushFrequencyCb(
livox_status status, uint8_t handle,
HubSetImuPushFrequencyResponse *response, void *clent_data) {
LdsHub *lds_hub = static_cast<LdsHub *>(clent_data);
if ((handle >= kMaxLidarCount) || !response) {
return;
@@ -444,7 +448,7 @@ void LdsHub::HubSetImuRatePushFrequencyCb(livox_status status, uint8_t handle, \
}
/** Callback function of starting sampling. */
void LdsHub::StartSampleCb(livox_status status, uint8_t handle, \
void LdsHub::StartSampleCb(livox_status status, uint8_t handle,
uint8_t response, void *clent_data) {
LdsHub *lds_hub = static_cast<LdsHub *>(clent_data);
if (handle >= kMaxLidarCount) {
@@ -454,8 +458,8 @@ void LdsHub::StartSampleCb(livox_status status, uint8_t handle, \
LidarDevice *p_hub = &lds_hub->hub_;
if ((status != kStatusSuccess) || (response != 0)) {
p_hub->connect_state = kConnectStateOn;
printf("Hub start sample fail : state[%d] handle[%d] res[%d]\n", \
status, handle, response);
printf("Hub start sample fail : state[%d] handle[%d] res[%d]\n", status,
handle, response);
for (int i = 0; i < kMaxLidarCount; i++) {
LidarDevice *p_lidar = &(lds_hub->lidars_[i]);
@@ -469,18 +473,18 @@ void LdsHub::StartSampleCb(livox_status status, uint8_t handle, \
}
/** Callback function of stopping sampling. */
void LdsHub::StopSampleCb(livox_status status, uint8_t handle, \
uint8_t response, void *clent_data) {
}
void LdsHub::StopSampleCb(livox_status status, uint8_t handle, uint8_t response,
void *clent_data) {}
void LdsHub::ConfigPointCloudReturnMode(LdsHub *lds_hub) {
uint8_t req_buf[1024];
HubSetPointCloudReturnModeRequest* req = (HubSetPointCloudReturnModeRequest *)req_buf;
HubSetPointCloudReturnModeRequest *req =
(HubSetPointCloudReturnModeRequest *)req_buf;
req->count = 0;
for (int i = 0; i < kMaxLidarCount; i++) {
LidarDevice *p_lidar = &(lds_hub->lidars_[i]);
if ((p_lidar->info.type != kDeviceTypeLidarMid40) && \
if ((p_lidar->info.type != kDeviceTypeLidarMid40) &&
(p_lidar->connect_state == kConnectStateSampling)) {
UserRawConfig config;
if (lds_hub->GetRawConfig(p_lidar->info.broadcast_code, config)) {
@@ -490,8 +494,8 @@ void LdsHub::ConfigPointCloudReturnMode(LdsHub* lds_hub) {
config.coordinate = kCoordinateCartesian;
config.imu_rate = kImuFreq200Hz;
}
strncpy(req->lidar_cfg_list[req->count].broadcast_code, \
p_lidar->info.broadcast_code, \
strncpy(req->lidar_cfg_list[req->count].broadcast_code,
p_lidar->info.broadcast_code,
sizeof(req->lidar_cfg_list[req->count].broadcast_code));
req->lidar_cfg_list[req->count].mode = config.return_mode;
req->count++;
@@ -499,8 +503,9 @@ void LdsHub::ConfigPointCloudReturnMode(LdsHub* lds_hub) {
}
if (req->count) {
uint32_t length = 1 + sizeof(SetPointCloudReturnModeRequestItem) * req->count;
HubSetPointCloudReturnMode(req, length,\
uint32_t length =
1 + sizeof(SetPointCloudReturnModeRequestItem) * req->count;
HubSetPointCloudReturnMode(req, length,
LdsHub::HubSetPointCloudReturnModeCb, lds_hub);
lds_hub->hub_.config.set_bits |= kConfigReturnMode;
}
@@ -513,7 +518,7 @@ void LdsHub::ConfigImuPushFrequency(LdsHub* lds_hub) {
for (int i = 0; i < kMaxLidarCount; i++) {
LidarDevice *p_lidar = &(lds_hub->lidars_[i]);
if ((p_lidar->info.type != kDeviceTypeLidarMid40) && \
if ((p_lidar->info.type != kDeviceTypeLidarMid40) &&
(p_lidar->connect_state == kConnectStateSampling)) {
UserRawConfig config;
if (lds_hub->GetRawConfig(p_lidar->info.broadcast_code, config)) {
@@ -523,8 +528,8 @@ void LdsHub::ConfigImuPushFrequency(LdsHub* lds_hub) {
config.coordinate = kCoordinateCartesian;
config.imu_rate = kImuFreq200Hz;
}
strncpy(req->lidar_cfg_list[req->count].broadcast_code, \
p_lidar->info.broadcast_code, \
strncpy(req->lidar_cfg_list[req->count].broadcast_code,
p_lidar->info.broadcast_code,
sizeof(req->lidar_cfg_list[req->count].broadcast_code));
req->lidar_cfg_list[req->count].freq = config.imu_rate;
req->count++;
@@ -533,8 +538,8 @@ void LdsHub::ConfigImuPushFrequency(LdsHub* lds_hub) {
if (req->count) {
uint32_t length = 1 + sizeof(SetImuPushFrequencyRequestItem) * req->count;
HubSetImuPushFrequency(req, length,\
LdsHub::HubSetImuRatePushFrequencyCb, lds_hub);
HubSetImuPushFrequency(req, length, LdsHub::HubSetImuRatePushFrequencyCb,
lds_hub);
lds_hub->hub_.config.set_bits |= kConfigImuRate;
}
}
@@ -544,11 +549,13 @@ void LdsHub::ConfigLidarsOfHub(LdsHub* lds_hub) {
ConfigImuPushFrequency(lds_hub);
if (lds_hub->hub_.config.coordinate != 0) {
SetSphericalCoordinate(lds_hub->hub_.handle, LdsHub::SetCoordinateCb, lds_hub);
SetSphericalCoordinate(lds_hub->hub_.handle, LdsHub::SetCoordinateCb,
lds_hub);
printf("Hub set coordinate spherical\n");
} else {
printf("Hub set coordinate cartesian\n");
SetCartesianCoordinate(lds_hub->hub_.handle, LdsHub::SetCoordinateCb, lds_hub);
SetCartesianCoordinate(lds_hub->hub_.handle, LdsHub::SetCoordinateCb,
lds_hub);
}
lds_hub->hub_.config.set_bits |= kConfigCoordinate;
@@ -557,7 +564,7 @@ void LdsHub::ConfigLidarsOfHub(LdsHub* lds_hub) {
/** Add broadcast code to whitelist */
int LdsHub::AddBroadcastCodeToWhitelist(const char *broadcast_code) {
if (!broadcast_code || (strlen(broadcast_code) > kBroadcastCodeSize) || \
if (!broadcast_code || (strlen(broadcast_code) > kBroadcastCodeSize) ||
(whitelist_count_ >= kMaxLidarCount)) {
return -1;
}
@@ -579,7 +586,8 @@ bool LdsHub::IsBroadcastCodeExistInWhitelist(const char* broadcast_code) {
}
for (uint32_t i = 0; i < whitelist_count_; i++) {
if (strncmp(broadcast_code, broadcast_code_whitelist_[i], kBroadcastCodeSize) == 0) {
if (strncmp(broadcast_code, broadcast_code_whitelist_[i],
kBroadcastCodeSize) == 0) {
return true;
}
}
@@ -589,7 +597,8 @@ bool LdsHub::IsBroadcastCodeExistInWhitelist(const char* broadcast_code) {
/** Get and update LiDAR info */
void LdsHub::UpdateHubLidarinfo(void) {
DeviceInfo *_lidars = (DeviceInfo *) malloc(sizeof(DeviceInfo) * kMaxLidarCount);
DeviceInfo *_lidars =
(DeviceInfo *)malloc(sizeof(DeviceInfo) * kMaxLidarCount);
uint8_t count = kMaxLidarCount;
uint8_t status = GetConnectedDevices(_lidars, &count);
@@ -620,7 +629,8 @@ int LdsHub::ParseConfigFile(const char* pathname) {
}
char read_buffer[32768];
rapidjson::FileReadStream config_file(raw_file, read_buffer, sizeof(read_buffer));
rapidjson::FileReadStream config_file(raw_file, read_buffer,
sizeof(read_buffer));
rapidjson::Document doc;
if (!doc.ParseStream(config_file).HasParseError()) {
@@ -628,13 +638,13 @@ int LdsHub::ParseConfigFile(const char* pathname) {
const rapidjson::Value &object = doc["hub_config"];
UserRawConfig hub_config;
memset(&hub_config, 0, sizeof(hub_config));
if(object.HasMember("broadcast_code") && \
if (object.HasMember("broadcast_code") &&
object["broadcast_code"].IsString()) {
std::string broadcast_code = object["broadcast_code"].GetString();
std::memcpy(hub_config.broadcast_code, broadcast_code.c_str(),\
std::memcpy(hub_config.broadcast_code, broadcast_code.c_str(),
sizeof(hub_config.broadcast_code));
if(object.HasMember("enable_connect") && \
if (object.HasMember("enable_connect") &&
object["enable_connect"].IsBool()) {
hub_config.enable_connect = object["enable_connect"].GetBool();
}
@@ -642,18 +652,20 @@ int LdsHub::ParseConfigFile(const char* pathname) {
hub_config.coordinate = object["coordinate"].GetInt();
}
printf("Hub[%s] : %d %d %d %d %d\n", hub_config.broadcast_code, \
hub_config.enable_connect, hub_config.enable_fan, hub_config.return_mode,\
hub_config.coordinate, hub_config.imu_rate);
printf("Hub[%s] : %d %d %d %d %d\n", hub_config.broadcast_code,
hub_config.enable_connect, hub_config.enable_fan,
hub_config.return_mode, hub_config.coordinate,
hub_config.imu_rate);
if (hub_config.enable_connect) {
if (!AddBroadcastCodeToWhitelist(hub_config.broadcast_code)) {
hub_raw_config_ = hub_config;
printf("Add hub[%s] [%d] to whitelist\n", hub_raw_config_.broadcast_code, \
hub_raw_config_.coordinate);
printf("Add hub[%s] [%d] to whitelist\n",
hub_raw_config_.broadcast_code, hub_raw_config_.coordinate);
} else {
memset(&hub_raw_config_, 0, sizeof(hub_raw_config_));
printf("Add hub[%s] to whitelist fail\n", hub_raw_config_.broadcast_code);
printf("Add hub[%s] to whitelist fail\n",
hub_raw_config_.broadcast_code);
}
}
} else {
@@ -669,21 +681,21 @@ int LdsHub::ParseConfigFile(const char* pathname) {
if (object.IsObject()) {
UserRawConfig config;
memset(&config, 0, sizeof(config));
if(object.HasMember("broadcast_code") && \
if (object.HasMember("broadcast_code") &&
object["broadcast_code"].IsString()) {
std::string broadcast_code = object["broadcast_code"].GetString();
std::memcpy(config.broadcast_code, broadcast_code.c_str(),\
std::memcpy(config.broadcast_code, broadcast_code.c_str(),
sizeof(config.broadcast_code));
} else {
printf("User config file parse error\n");
continue;
}
if(object.HasMember("enable_fan") && \
object["enable_fan"].IsBool()) {
if (object.HasMember("enable_fan") && object["enable_fan"].IsBool()) {
config.enable_fan = object["enable_fan"].GetBool();
}
if(object.HasMember("return_mode") && object["return_mode"].IsInt()) {
if (object.HasMember("return_mode") &&
object["return_mode"].IsInt()) {
config.return_mode = object["return_mode"].GetInt();
}
if (object.HasMember("imu_rate") && object["imu_rate"].IsInt()) {
@@ -694,18 +706,20 @@ int LdsHub::ParseConfigFile(const char* pathname) {
} else {
config.coordinate = 0;
}
printf("Lidar[%s] : %d %d %d %d %d\n", config.broadcast_code, \
config.enable_connect, config.enable_fan, config.return_mode,\
printf("Lidar[%s] : %d %d %d %d %d\n", config.broadcast_code,
config.enable_connect, config.enable_fan, config.return_mode,
config.coordinate, config.imu_rate);
if (AddRawUserConfig(config)) {
printf("Lidar Raw config is already exist : %s \n", config.broadcast_code);
printf("Lidar Raw config is already exist : %s \n",
config.broadcast_code);
}
}
}
}
} else {
printf("User config file parse error[%d]\n", doc.ParseStream(config_file).HasParseError());
printf("User config file parse error[%d]\n",
doc.ParseStream(config_file).HasParseError());
}
std::fclose(raw_file);
@@ -730,7 +744,8 @@ bool LdsHub::IsExistInRawConfig(const char* broadcast_code) {
}
for (auto ite_config : lidar_raw_config_) {
if (strncmp(ite_config.broadcast_code, broadcast_code, kBroadcastCodeSize) == 0) {
if (strncmp(ite_config.broadcast_code, broadcast_code,
kBroadcastCodeSize) == 0) {
return true;
}
}
@@ -738,14 +753,14 @@ bool LdsHub::IsExistInRawConfig(const char* broadcast_code) {
return false;
}
int LdsHub::GetRawConfig(const char *broadcast_code, UserRawConfig &config) {
if (broadcast_code == nullptr) {
return -1;
}
for (auto ite_config : lidar_raw_config_) {
if (strncmp(ite_config.broadcast_code, broadcast_code, kBroadcastCodeSize) == 0) {
if (strncmp(ite_config.broadcast_code, broadcast_code,
kBroadcastCodeSize) == 0) {
config.enable_fan = ite_config.enable_fan;
config.return_mode = ite_config.return_mode;
config.coordinate = ite_config.coordinate;
@@ -757,4 +772,4 @@ int LdsHub::GetRawConfig(const char* broadcast_code, UserRawConfig& config) {
return -1;
}
}
} // namespace livox_ros

37
livox_ros_driver/livox_ros_driver/lds_hub.h
View File

@@ -45,7 +45,8 @@ class LdsHub : public Lds {
return &lds_hub;
}
int InitLdsHub(std::vector<std::string>& broadcast_code_strs, const char *user_config_path);
int InitLdsHub(std::vector<std::string> &broadcast_code_strs,
const char *user_config_path);
int DeInitLdsHub(void);
private:
@@ -55,25 +56,31 @@ class LdsHub : public Lds {
LdsHub &operator=(const LdsHub &) = delete;
virtual void PrepareExit(void);
static void OnHubDataCb(uint8_t hub_handle, LivoxEthPacket *data,\
static void OnHubDataCb(uint8_t hub_handle, LivoxEthPacket *data,
uint32_t data_num, void *client_data);
static void OnDeviceBroadcast(const BroadcastDeviceInfo *info);
static void OnDeviceChange(const DeviceInfo *info, DeviceEvent type);
static void StartSampleCb(livox_status status, uint8_t handle, uint8_t response, void *clent_data);
static void StopSampleCb(livox_status status, uint8_t handle, uint8_t response, void *clent_data);
static void HubQueryLidarInfoCb(livox_status status, uint8_t handle, \
HubQueryLidarInformationResponse *response, void *client_data);
static void ControlFanCb(livox_status status, uint8_t handle, \
static void StartSampleCb(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 SetCoordinateCb(livox_status status, uint8_t handle, \
static void StopSampleCb(livox_status status, uint8_t handle,
uint8_t response, void *clent_data);
static void HubSetImuRatePushFrequencyCb(livox_status status, uint8_t handle, \
HubSetImuPushFrequencyResponse* response,\
static void HubQueryLidarInfoCb(livox_status status, uint8_t handle,
HubQueryLidarInformationResponse *response,
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 HubErrorStatusCb(livox_status status, uint8_t handle, ErrorMessage *message);
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 HubErrorStatusCb(livox_status status, uint8_t handle,
ErrorMessage *message);
static void ConfigPointCloudReturnMode(LdsHub *lds_hub);
static void ConfigImuPushFrequency(LdsHub *lds_hub);
static void ConfigLidarsOfHub(LdsHub *lds_hub);
@@ -111,5 +118,5 @@ class LdsHub : public Lds {
UserRawConfig hub_raw_config_;
};
}
} // namespace livox_ros
#endif

245
livox_ros_driver/livox_ros_driver/lds_lidar.cpp
View File

@@ -24,25 +24,28 @@
#include "lds_lidar.h"
#include <memory>
#include <stdio.h>
#include <string.h>
#include <thread>
#include <memory>
#include "rapidjson/document.h"
#include "rapidjson/stringbuffer.h"
#include "rapidjson/filereadstream.h"
#include "rapidjson/stringbuffer.h"
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;
@@ -53,15 +56,11 @@ LdsLidar::LdsLidar(uint32_t interval_ms) : Lds(interval_ms, kSourceRawLidar) {
ResetLdsLidar();
}
LdsLidar::~LdsLidar() {
LdsLidar::~LdsLidar() {}
}
void LdsLidar::ResetLdsLidar(void) { ResetLds(kSourceRawLidar); }
void LdsLidar::ResetLdsLidar(void) {
ResetLds(kSourceRawLidar);
}
int LdsLidar::InitLdsLidar(std::vector<std::string>& broadcast_code_strs,\
int LdsLidar::InitLdsLidar(std::vector<std::string> &broadcast_code_strs,
const char *user_config_path) {
if (is_initialized_) {
printf("LiDAR data source is already inited!\n");
@@ -76,7 +75,8 @@ int LdsLidar::InitLdsLidar(std::vector<std::string>& broadcast_code_strs,\
LivoxSdkVersion _sdkversion;
GetLivoxSdkVersion(&_sdkversion);
printf("Livox SDK version %d.%d.%d\n", _sdkversion.major, _sdkversion.minor, _sdkversion.patch);
printf("Livox SDK version %d.%d.%d\n", _sdkversion.major, _sdkversion.minor,
_sdkversion.patch);
SetBroadcastCallback(OnDeviceBroadcast);
SetDeviceStateUpdateCallback(OnDeviceChange);
@@ -98,7 +98,8 @@ 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_) {
@@ -150,11 +151,10 @@ int LdsLidar::DeInitLdsLidar(void) {
return 0;
}
void LdsLidar::PrepareExit(void) {
DeInitLdsLidar();
}
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,12 +171,12 @@ void LdsLidar::OnLidarDataCb(uint8_t handle, LivoxEthPacket *data,
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));
memcpy(cur_timestamp.stamp_bytes, eth_packet->timestamp,
sizeof(cur_timestamp));
if (kImu != eth_packet->data_type) {
if (eth_packet->timestamp_type == kTimestampTypePps) {
if ((cur_timestamp.stamp< packet_statistic->last_timestamp) && \
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();
@@ -189,28 +189,30 @@ void LdsLidar::OnLidarDataCb(uint8_t handle, LivoxEthPacket *data,
LidarDataQueue *p_queue = &p_lidar->data;
if (nullptr == p_queue->storage_packet) {
uint32_t queue_size = CalculatePacketQueueSize(lds_lidar->buffer_time_ms_, \
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);
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, \
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) && \
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->imu_timebase =
sync_time; // used receive time as timebase
}
}
packet_statistic->last_imu_timestamp = cur_timestamp.stamp;
@@ -219,13 +221,14 @@ void LdsLidar::OnLidarDataCb(uint8_t handle, LivoxEthPacket *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, \
printf("Lidar%02d[%s] imu queue size : %d %d\n", p_lidar->handle,
p_lidar->info.broadcast_code, queue_size, p_queue->size);
}
if (!QueueIsFull(p_queue) && (cur_timestamp.stamp >= 0)) {
QueuePushAny(p_queue, (uint8_t *)eth_packet,\
GetEthPacketLen(eth_packet->data_type), packet_statistic->timebase, \
QueuePushAny(p_queue, (uint8_t *)eth_packet,
GetEthPacketLen(eth_packet->data_type),
packet_statistic->timebase,
GetPointsPerPacket(eth_packet->data_type));
}
}
@@ -237,15 +240,17 @@ void LdsLidar::OnDeviceBroadcast(const BroadcastDeviceInfo *info) {
}
if (info->dev_type == kDeviceTypeHub) {
printf("In lidar mode, couldn't connect a hub : %s\n", info->broadcast_code);
printf("In lidar mode, couldn't connect a hub : %s\n",
info->broadcast_code);
return;
}
if (g_lds_ldiar->IsAutoConnectMode()) {
printf("In automatic connection mode, will connect %s\n", info->broadcast_code);
printf("In automatic connection mode, will connect %s\n",
info->broadcast_code);
} else {
if (!g_lds_ldiar->IsBroadcastCodeExistInWhitelist(info->broadcast_code)) {
printf("Not in the whitelist, please add %s to if want to connect!\n",\
printf("Not in the whitelist, please add %s to if want to connect!\n",
info->broadcast_code);
return;
}
@@ -275,7 +280,8 @@ void LdsLidar::OnDeviceBroadcast(const BroadcastDeviceInfo *info) {
p_lidar->config.return_mode = config.return_mode;
p_lidar->config.coordinate = config.coordinate;
p_lidar->config.imu_rate = config.imu_rate;
p_lidar->config.extrinsic_parameter_source = config.extrinsic_parameter_source;
p_lidar->config.extrinsic_parameter_source =
config.extrinsic_parameter_source;
} else {
printf("Add lidar to connect is failed : %d %d \n", result, handle);
}
@@ -307,10 +313,9 @@ void LdsLidar::OnDeviceChange(const DeviceInfo *info, DeviceEvent type) {
}
if (p_lidar->connect_state == kConnectStateOn) {
printf("Lidar[%s] status_code[%d] working state[%d] feature[%d]\n", \
p_lidar->info.broadcast_code,\
p_lidar->info.status.status_code.error_code,\
p_lidar->info.state,\
printf("Lidar[%s] status_code[%d] working state[%d] feature[%d]\n",
p_lidar->info.broadcast_code,
p_lidar->info.status.status_code.error_code, p_lidar->info.state,
p_lidar->info.feature);
SetErrorMessageCallback(handle, LidarErrorStatusCb);
@@ -324,19 +329,22 @@ void LdsLidar::OnDeviceChange(const DeviceInfo *info, DeviceEvent type) {
p_lidar->config.set_bits |= kConfigCoordinate;
if (kDeviceTypeLidarMid40 != info->type) {
LidarSetPointCloudReturnMode(handle, (PointCloudReturnMode)(p_lidar->config.return_mode),\
LidarSetPointCloudReturnMode(
handle, (PointCloudReturnMode)(p_lidar->config.return_mode),
SetPointCloudReturnModeCb, g_lds_ldiar);
p_lidar->config.set_bits |= kConfigReturnMode;
}
if (kDeviceTypeLidarMid40 != info->type) {
LidarSetImuPushFrequency(handle, (ImuFreq)(p_lidar->config.imu_rate),\
LidarSetImuPushFrequency(handle, (ImuFreq)(p_lidar->config.imu_rate),
SetImuRatePushFrequencyCb, g_lds_ldiar);
p_lidar->config.set_bits |= kConfigImuRate;
}
if (p_lidar->config.extrinsic_parameter_source == kExtrinsicParameterFromLidar) {
LidarGetExtrinsicParameter(handle, GetLidarExtrinsicParameterCb, g_lds_ldiar);
if (p_lidar->config.extrinsic_parameter_source ==
kExtrinsicParameterFromLidar) {
LidarGetExtrinsicParameter(handle, GetLidarExtrinsicParameterCb,
g_lds_ldiar);
}
p_lidar->connect_state = kConnectStateConfig;
@@ -345,22 +353,22 @@ void LdsLidar::OnDeviceChange(const DeviceInfo *info, DeviceEvent type) {
}
/** Query the firmware version of Livox LiDAR. */
void LdsLidar::DeviceInformationCb(livox_status status, uint8_t handle, \
DeviceInformationResponse *ack, void *clent_data) {
void LdsLidar::DeviceInformationCb(livox_status status, uint8_t handle,
DeviceInformationResponse *ack,
void *clent_data) {
if (status != kStatusSuccess) {
printf("Device Query Informations Failed : %d\n", status);
}
if (ack) {
printf("firmware version: %d.%d.%d.%d\n",
ack->firmware_version[0],
ack->firmware_version[1],
ack->firmware_version[2],
printf("firmware version: %d.%d.%d.%d\n", ack->firmware_version[0],
ack->firmware_version[1], ack->firmware_version[2],
ack->firmware_version[3]);
}
}
/** Callback function of Lidar error message. */
void LdsLidar::LidarErrorStatusCb(livox_status status, uint8_t handle, ErrorMessage *message) {
void LdsLidar::LidarErrorStatusCb(livox_status status, uint8_t handle,
ErrorMessage *message) {
static uint32_t error_message_count = 0;
if (message != NULL) {
++error_message_count;
@@ -375,18 +383,17 @@ void LdsLidar::LidarErrorStatusCb(livox_status status, uint8_t handle, ErrorMess
printf("fan_status : %u\n", message->lidar_error_code.fan_status);
printf("self_heating : %u\n", message->lidar_error_code.self_heating);
printf("ptp_status : %u\n", message->lidar_error_code.ptp_status);
printf("time_sync_status : %u\n", message->lidar_error_code.time_sync_status);
printf("time_sync_status : %u\n",
message->lidar_error_code.time_sync_status);
printf("system_status : %u\n", message->lidar_error_code.system_status);
}
}
}
void LdsLidar::ControlFanCb(livox_status status, uint8_t handle, \
uint8_t response, void *clent_data) {
void LdsLidar::ControlFanCb(livox_status status, uint8_t handle,
uint8_t response, void *clent_data) {}
}
void LdsLidar::SetPointCloudReturnModeCb(livox_status status, uint8_t handle, \
void LdsLidar::SetPointCloudReturnModeCb(livox_status status, uint8_t handle,
uint8_t response, void *clent_data) {
LdsLidar *lds_lidar = static_cast<LdsLidar *>(clent_data);
@@ -405,13 +412,14 @@ void LdsLidar::SetPointCloudReturnModeCb(livox_status status, uint8_t handle, \
}
} else {
LidarSetPointCloudReturnMode(handle, (PointCloudReturnMode)(p_lidar->config.return_mode),\
LidarSetPointCloudReturnMode(
handle, (PointCloudReturnMode)(p_lidar->config.return_mode),
SetPointCloudReturnModeCb, lds_lidar);
printf("Set return mode fail, try again!\n");
}
}
void LdsLidar::SetCoordinateCb(livox_status status, uint8_t handle, \
void LdsLidar::SetCoordinateCb(livox_status status, uint8_t handle,
uint8_t response, void *clent_data) {
LdsLidar *lds_lidar = static_cast<LdsLidar *>(clent_data);
@@ -439,7 +447,7 @@ void LdsLidar::SetCoordinateCb(livox_status status, uint8_t handle, \
}
}
void LdsLidar::SetImuRatePushFrequencyCb(livox_status status, uint8_t handle, \
void LdsLidar::SetImuRatePushFrequencyCb(livox_status status, uint8_t handle,
uint8_t response, void *clent_data) {
LdsLidar *lds_lidar = static_cast<LdsLidar *>(clent_data);
@@ -458,15 +466,15 @@ void LdsLidar::SetImuRatePushFrequencyCb(livox_status status, uint8_t handle, \
}
} else {
LidarSetImuPushFrequency(handle, (ImuFreq)(p_lidar->config.imu_rate),\
LidarSetImuPushFrequency(handle, (ImuFreq)(p_lidar->config.imu_rate),
SetImuRatePushFrequencyCb, g_lds_ldiar);
printf("Set imu rate fail, try again!\n");
}
}
/** Callback function of get LiDARs' extrinsic parameter. */
void LdsLidar::GetLidarExtrinsicParameterCb(livox_status status, uint8_t handle, \
void LdsLidar::GetLidarExtrinsicParameterCb(
livox_status status, uint8_t handle,
LidarGetExtrinsicParameterResponse *response, void *clent_data) {
LdsLidar *lds_lidar = static_cast<LdsLidar *>(clent_data);
if (handle >= kMaxLidarCount) {
@@ -475,7 +483,7 @@ void LdsLidar::GetLidarExtrinsicParameterCb(livox_status status, uint8_t handle,
if (status == kStatusSuccess) {
if (response != nullptr) {
printf("Lidar[%d] get ExtrinsicParameter status[%d] response[%d]\n", \
printf("Lidar[%d] get ExtrinsicParameter status[%d] response[%d]\n",
handle, status, response->ret_code);
LidarDevice *p_lidar = &(lds_lidar->lidars_[handle]);
ExtrinsicParameter *p_extrinsic = &p_lidar->extrinsic_parameter;
@@ -504,9 +512,8 @@ void LdsLidar::GetLidarExtrinsicParameterCb(livox_status status, uint8_t handle,
}
}
/** Callback function of starting sampling. */
void LdsLidar::StartSampleCb(livox_status status, uint8_t handle, \
void LdsLidar::StartSampleCb(livox_status status, uint8_t handle,
uint8_t response, void *clent_data) {
LdsLidar *lds_lidar = static_cast<LdsLidar *>(clent_data);
@@ -518,40 +525,41 @@ void LdsLidar::StartSampleCb(livox_status status, uint8_t handle, \
if (status == kStatusSuccess) {
if (response != 0) {
p_lidar->connect_state = kConnectStateOn;
printf("Lidar start sample fail : state[%d] handle[%d] res[%d]\n", \
status, handle, response);
printf("Lidar start sample fail : state[%d] handle[%d] res[%d]\n", status,
handle, response);
} else {
printf("Lidar start sample success\n");
}
} else if (status == kStatusTimeout) {
p_lidar->connect_state = kConnectStateOn;
printf("Lidar start sample timeout : state[%d] handle[%d] res[%d]\n", \
printf("Lidar start sample timeout : state[%d] handle[%d] res[%d]\n",
status, handle, response);
}
}
/** Callback function of stopping sampling. */
void LdsLidar::StopSampleCb(livox_status status, uint8_t handle, \
uint8_t response, void *clent_data) {
}
void LdsLidar::StopSampleCb(livox_status status, uint8_t handle,
uint8_t response, void *clent_data) {}
void LdsLidar::SetRmcSyncTimeCb(livox_status status, uint8_t handle, \
void LdsLidar::SetRmcSyncTimeCb(livox_status status, uint8_t handle,
uint8_t response, void *client_data) {
if (handle >= kMaxLidarCount) {
return;
}
printf("Set lidar[%d] sync time status[%d] response[%d]\n", handle, status, response);
printf("Set lidar[%d] sync time status[%d] response[%d]\n", handle, status,
response);
}
void LdsLidar::ReceiveSyncTimeCallback(const char* rmc, uint32_t rmc_length, void* client_data) {
void LdsLidar::ReceiveSyncTimeCallback(const char *rmc, uint32_t rmc_length,
void *client_data) {
LdsLidar *lds_lidar = static_cast<LdsLidar *>(client_data);
// std::unique_lock<std::mutex> lock(mtx);
LidarDevice *p_lidar = nullptr;
for (uint8_t handle = 0; handle < kMaxLidarCount; handle++) {
p_lidar = &(lds_lidar->lidars_[handle]);
if (p_lidar->connect_state == kConnectStateSampling && \
if (p_lidar->connect_state == kConnectStateSampling &&
p_lidar->info.state == kLidarStateNormal) {
livox_status status = LidarSetRmcSyncTime(handle, rmc, rmc_length, \
livox_status status = LidarSetRmcSyncTime(handle, rmc, rmc_length,
SetRmcSyncTimeCb, lds_lidar);
if (status != kStatusSuccess) {
printf("Set GPRMC synchronization time error code: %d.\n", status);
@@ -562,7 +570,7 @@ void LdsLidar::ReceiveSyncTimeCallback(const char* rmc, uint32_t rmc_length, voi
/** Add broadcast code to whitelist */
int LdsLidar::AddBroadcastCodeToWhitelist(const char *broadcast_code) {
if (!broadcast_code || (strlen(broadcast_code) > kBroadcastCodeSize) || \
if (!broadcast_code || (strlen(broadcast_code) > kBroadcastCodeSize) ||
(whitelist_count_ >= kMaxLidarCount)) {
return -1;
}
@@ -584,7 +592,8 @@ bool LdsLidar::IsBroadcastCodeExistInWhitelist(const char* broadcast_code) {
}
for (uint32_t i = 0; i < whitelist_count_; i++) {
if (strncmp(broadcast_code, broadcast_code_whitelist_[i], kBroadcastCodeSize) == 0) {
if (strncmp(broadcast_code, broadcast_code_whitelist_[i],
kBroadcastCodeSize) == 0) {
return true;
}
}
@@ -594,35 +603,48 @@ bool LdsLidar::IsBroadcastCodeExistInWhitelist(const char* broadcast_code) {
int LdsLidar::ParseTimesyncConfig(rapidjson::Document &doc) {
do {
if (!doc.HasMember("timesync_config") || !doc["timesync_config"].IsObject()) break;
if (!doc.HasMember("timesync_config") || !doc["timesync_config"].IsObject())
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()) break;
if (!object.HasMember("enable_timesync") ||
!object["enable_timesync"].IsBool())
break;
enable_timesync_ = object["enable_timesync"].GetBool();
if(!object.HasMember("device_name") || !object["device_name"].IsString()) break;
if (!object.HasMember("device_name") || !object["device_name"].IsString())
break;
std::string device_name = object["device_name"].GetString();
std::strncpy(timesync_config_.dev_config.name, device_name.c_str(),\
std::strncpy(timesync_config_.dev_config.name, device_name.c_str(),
sizeof(timesync_config_.dev_config.name));
if(!object.HasMember("comm_device_type") || !object["comm_device_type"].IsInt()) break;
if (!object.HasMember("comm_device_type") ||
!object["comm_device_type"].IsInt())
break;
timesync_config_.dev_config.type = object["comm_device_type"].GetInt();
if (timesync_config_.dev_config.type == kCommDevUart) {
if(!object.HasMember("baudrate_index") || !object["baudrate_index"].IsInt()) break;
timesync_config_.dev_config.config.uart.baudrate = object["baudrate_index"].GetInt();
if (!object.HasMember("baudrate_index") ||
!object["baudrate_index"].IsInt())
break;
timesync_config_.dev_config.config.uart.baudrate =
object["baudrate_index"].GetInt();
if(!object.HasMember("parity_index") || !object["parity_index"].IsInt()) break;
timesync_config_.dev_config.config.uart.parity = object["parity_index"].GetInt();
if (!object.HasMember("parity_index") || !object["parity_index"].IsInt())
break;
timesync_config_.dev_config.config.uart.parity =
object["parity_index"].GetInt();
}
if (enable_timesync_) {
printf("Enable timesync : \n");
if (timesync_config_.dev_config.type == kCommDevUart) {
printf("Uart[%s],baudrate index[%d],parity index[%d]\n", timesync_config_.dev_config.name,\
timesync_config_.dev_config.config.uart.baudrate,\
printf("Uart[%s],baudrate index[%d],parity index[%d]\n",
timesync_config_.dev_config.name,
timesync_config_.dev_config.config.uart.baudrate,
timesync_config_.dev_config.config.uart.parity);
}
} else {
@@ -634,7 +656,6 @@ int LdsLidar::ParseTimesyncConfig(rapidjson::Document& doc) {
return -1;
}
/** Config file process */
int LdsLidar::ParseConfigFile(const char *pathname) {
FILE *raw_file = std::fopen(pathname, "rb");
@@ -644,7 +665,8 @@ int LdsLidar::ParseConfigFile(const char* pathname) {
}
char read_buffer[32768];
rapidjson::FileReadStream config_file(raw_file, read_buffer, sizeof(read_buffer));
rapidjson::FileReadStream config_file(raw_file, read_buffer,
sizeof(read_buffer));
rapidjson::Document doc;
if (!doc.ParseStream(config_file).HasParseError()) {
@@ -656,22 +678,25 @@ int LdsLidar::ParseConfigFile(const char* pathname) {
if (object.IsObject()) {
UserRawConfig config;
memset(&config, 0, sizeof(config));
if(object.HasMember("broadcast_code") && object["broadcast_code"].IsString()) {
if (object.HasMember("broadcast_code") &&
object["broadcast_code"].IsString()) {
std::string broadcast_code = object["broadcast_code"].GetString();
std::strncpy(config.broadcast_code, broadcast_code.c_str(),\
std::strncpy(config.broadcast_code, broadcast_code.c_str(),
sizeof(config.broadcast_code));
} else {
printf("User config file parse error\n");
continue;
}
if(object.HasMember("enable_connect") && object["enable_connect"].IsBool()) {
if (object.HasMember("enable_connect") &&
object["enable_connect"].IsBool()) {
config.enable_connect = object["enable_connect"].GetBool();
}
if (object.HasMember("enable_fan") && object["enable_fan"].IsBool()) {
config.enable_fan = object["enable_fan"].GetBool();
}
if(object.HasMember("return_mode") && object["return_mode"].IsInt()) {
if (object.HasMember("return_mode") &&
object["return_mode"].IsInt()) {
config.return_mode = object["return_mode"].GetInt();
}
if (object.HasMember("coordinate") && object["coordinate"].IsInt()) {
@@ -680,18 +705,21 @@ int LdsLidar::ParseConfigFile(const char* pathname) {
if (object.HasMember("imu_rate") && object["imu_rate"].IsInt()) {
config.imu_rate = object["imu_rate"].GetInt();
}
if(object.HasMember("extrinsic_parameter_source") && \
if (object.HasMember("extrinsic_parameter_source") &&
object["extrinsic_parameter_source"].IsInt()) {
config.extrinsic_parameter_source = object["extrinsic_parameter_source"].GetInt();
config.extrinsic_parameter_source =
object["extrinsic_parameter_source"].GetInt();
}
printf("broadcast code[%s] : %d %d %d %d %d %d\n", config.broadcast_code, \
config.enable_connect, config.enable_fan, config.return_mode,\
config.coordinate, config.imu_rate, config.extrinsic_parameter_source);
printf("broadcast code[%s] : %d %d %d %d %d %d\n",
config.broadcast_code, config.enable_connect,
config.enable_fan, config.return_mode, config.coordinate,
config.imu_rate, config.extrinsic_parameter_source);
if (config.enable_connect) {
if (!AddBroadcastCodeToWhitelist(config.broadcast_code)) {
if (AddRawUserConfig(config)) {
printf("Raw config is already exist : %s \n", config.broadcast_code);
printf("Raw config is already exist : %s \n",
config.broadcast_code);
}
}
}
@@ -704,7 +732,8 @@ int LdsLidar::ParseConfigFile(const char* pathname) {
enable_timesync_ = false;
}
} else {
printf("User config file parse error[%d]\n", doc.ParseStream(config_file).HasParseError());
printf("User config file parse error[%d]\n",
doc.ParseStream(config_file).HasParseError());
}
std::fclose(raw_file);
@@ -729,7 +758,8 @@ bool LdsLidar::IsExistInRawConfig(const char* broadcast_code) {
}
for (auto ite_config : raw_config_) {
if (strncmp(ite_config.broadcast_code, broadcast_code, kBroadcastCodeSize) == 0) {
if (strncmp(ite_config.broadcast_code, broadcast_code,
kBroadcastCodeSize) == 0) {
return true;
}
}
@@ -743,7 +773,8 @@ int LdsLidar::GetRawConfig(const char* broadcast_code, UserRawConfig& config) {
}
for (auto ite_config : raw_config_) {
if (strncmp(ite_config.broadcast_code, broadcast_code, kBroadcastCodeSize) == 0) {
if (strncmp(ite_config.broadcast_code, broadcast_code,
kBroadcastCodeSize) == 0) {
config.enable_fan = ite_config.enable_fan;
config.return_mode = ite_config.return_mode;
config.coordinate = ite_config.coordinate;
@@ -756,4 +787,4 @@ int LdsLidar::GetRawConfig(const char* broadcast_code, UserRawConfig& config) {
return -1;
}
}
} // namespace livox_ros

43
livox_ros_driver/livox_ros_driver/lds_lidar.h
View File

@@ -32,9 +32,8 @@
#include "lds.h"
#include "livox_sdk.h"
#include "timesync.h"
#include "rapidjson/document.h"
#include "timesync.h"
namespace livox_ros {
@@ -48,7 +47,8 @@ class LdsLidar : public Lds {
return &lds_lidar;
}
int InitLdsLidar(std::vector<std::string>& broadcast_code_strs, const char *user_config_path);
int InitLdsLidar(std::vector<std::string> &broadcast_code_strs,
const char *user_config_path);
int DeInitLdsLidar(void);
private:
@@ -58,28 +58,35 @@ class LdsLidar : public Lds {
LdsLidar &operator=(const LdsLidar &) = delete;
virtual void PrepareExit(void);
static void OnLidarDataCb(uint8_t handle, LivoxEthPacket *data,\
static void OnLidarDataCb(uint8_t handle, LivoxEthPacket *data,
uint32_t data_num, void *client_data);
static void OnDeviceBroadcast(const BroadcastDeviceInfo *info);
static void OnDeviceChange(const DeviceInfo *info, DeviceEvent type);
static void StartSampleCb(livox_status status, uint8_t handle, uint8_t response, void *clent_data);
static void StopSampleCb(livox_status status, uint8_t handle, uint8_t response, void *clent_data);
static void DeviceInformationCb(livox_status status, uint8_t handle, \
DeviceInformationResponse *ack, void *clent_data);
static void LidarErrorStatusCb(livox_status status, uint8_t handle, ErrorMessage *message);
static void ControlFanCb(livox_status status, uint8_t handle, \
static void StartSampleCb(livox_status status, uint8_t handle,
uint8_t response, void *clent_data);
static void SetPointCloudReturnModeCb(livox_status status, uint8_t handle, \
static void StopSampleCb(livox_status status, uint8_t handle,
uint8_t response, void *clent_data);
static void SetCoordinateCb(livox_status status, uint8_t handle, \
static void DeviceInformationCb(livox_status status, uint8_t handle,
DeviceInformationResponse *ack,
void *clent_data);
static void LidarErrorStatusCb(livox_status status, uint8_t handle,
ErrorMessage *message);
static void ControlFanCb(livox_status status, uint8_t handle,
uint8_t response, void *clent_data);
static void SetImuRatePushFrequencyCb(livox_status status, uint8_t handle, \
static void SetPointCloudReturnModeCb(livox_status status, uint8_t handle,
uint8_t response, void *clent_data);
static void SetRmcSyncTimeCb(livox_status status, uint8_t handle, \
static void SetCoordinateCb(livox_status status, uint8_t handle,
uint8_t response, void *clent_data);
static void SetImuRatePushFrequencyCb(livox_status status, uint8_t handle,
uint8_t response, void *clent_data);
static void SetRmcSyncTimeCb(livox_status status, uint8_t handle,
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 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);
void ResetLdsLidar(void);
int AddBroadcastCodeToWhitelist(const char *broadcast_code);
@@ -105,5 +112,5 @@ class LdsLidar : public Lds {
TimeSyncConfig timesync_config_;
};
}
} // namespace livox_ros
#endif

69
livox_ros_driver/livox_ros_driver/lds_lvx.cpp
View File

@@ -24,26 +24,31 @@
#include "lds_lvx.h"
#include <functional>
#include <memory>
#include <stdio.h>
#include <string.h>
#include <thread>
#include <memory>
#include <functional>
#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;
lvx_file_ = std::make_shared<LvxFileHandle>();
packets_of_frame_.buffer_capacity = kMaxPacketsNumOfFrame * sizeof(LvxFilePacket);
packets_of_frame_.packet = new uint8_t[kMaxPacketsNumOfFrame * sizeof(LvxFilePacket)];
packets_of_frame_.buffer_capacity =
kMaxPacketsNumOfFrame * sizeof(LvxFilePacket);
packets_of_frame_.packet =
new uint8_t[kMaxPacketsNumOfFrame * sizeof(LvxFilePacket)];
}
LdsLvx::~LdsLvx() {
@@ -57,7 +62,6 @@ void LdsLvx::PrepareExit(void) {
printf("Lvx to rosbag convert complete and exit!\n");
}
int LdsLvx::InitLdsLvx(const char *lvx_path) {
if (is_initialized_) {
printf("Livox file data source is already inited!\n");
@@ -91,7 +95,7 @@ int LdsLvx::InitLdsLvx(const char* lvx_path) {
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,\
memcpy(lidars_[i].info.broadcast_code, lvx_dev_info.lidar_broadcast_code,
sizeof(lidars_[i].info.broadcast_code));
if (lvx_file_->GetFileVersion() == kLvxFileV1) {
@@ -116,7 +120,8 @@ int LdsLvx::InitLdsLvx(const char* lvx_path) {
InitQueue(&lidars_[i].imu_data, queue_size);
}
t_read_lvx_ = std::make_shared<std::thread>(std::bind(&LdsLvx::ReadLvxFile, this));
t_read_lvx_ =
std::make_shared<std::thread>(std::bind(&LdsLvx::ReadLvxFile, this));
is_initialized_ = true;
StartRead();
@@ -126,7 +131,8 @@ 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;
@@ -142,24 +148,28 @@ void LdsLvx::ReadLvxFile() {
int32_t handle;
uint8_t data_type;
if (lvx_file_->GetFileVersion()) {
LvxFilePacket* detail_packet = (LvxFilePacket*)&packet_base[data_offset];
LvxFilePacket *detail_packet =
(LvxFilePacket *)&packet_base[data_offset];
eth_packet = (LivoxEthPacket *)(&detail_packet->version);
handle = detail_packet->device_index;
} else {
LvxFilePacketV0* detail_packet = (LvxFilePacketV0*)&packet_base[data_offset];
LvxFilePacketV0 *detail_packet =
(LvxFilePacketV0 *)&packet_base[data_offset];
eth_packet = (LivoxEthPacket *)(&detail_packet->version);
handle = detail_packet->device_index;
}
data_type = eth_packet->data_type;
data_offset += (GetEthPacketLen(data_type) + 1); /* packet length + device index */
data_offset +=
(GetEthPacketLen(data_type) + 1); /* packet length + device index */
if (data_type != kImu) {
LidarDataQueue *p_queue = &lidars_[handle].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));
QueuePushAny(p_queue, (uint8_t *)eth_packet,
GetEthPacketLen(data_type), 0,
GetPointsPerPacket(data_type));
}
} else {
LidarDataQueue *p_queue = &lidars_[handle].imu_data;
@@ -167,8 +177,9 @@ void LdsLvx::ReadLvxFile() {
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));
QueuePushAny(p_queue, (uint8_t *)eth_packet,
GetEthPacketLen(data_type), 0,
GetPointsPerPacket(data_type));
}
}
}
@@ -187,8 +198,15 @@ void LdsLvx::ReadLvxFile() {
}
}
while(IsAllQueueEmpty()) {
std::this_thread::sleep_for(std::chrono::milliseconds(1));
int32_t wait_cnt = 10;
while (!IsAllQueueEmpty()) {
std::this_thread::sleep_for(std::chrono::milliseconds(50));
if (IsAllQueueReadStop()) {
--wait_cnt;
if (wait_cnt <= 0) {
break;
}
}
}
RequestExit();
}
@@ -204,4 +222,17 @@ bool LdsLvx::IsAllQueueEmpty() {
return true;
}
bool LdsLvx::IsAllQueueReadStop() {
static uint32_t remain_size[kMaxSourceLidar];
for (int i = 0; i < lidar_count_; i++) {
LidarDevice *p_lidar = &lidars_[i];
if (remain_size[i] != QueueIsEmpty(&p_lidar->data)) {
remain_size[i] = QueueIsEmpty(&p_lidar->data);
return false;
}
}
return true;
}
} // namespace livox_ros

3
livox_ros_driver/livox_ros_driver/lds_lvx.h
View File

@@ -61,6 +61,7 @@ class LdsLvx : public Lds {
void ReadLvxFile();
bool IsAllQueueEmpty();
bool IsAllQueueReadStop();
volatile bool is_initialized_;
OutPacketBuffer packets_of_frame_;
@@ -69,5 +70,5 @@ class LdsLvx : public Lds {
volatile bool start_read_lvx_;
};
}
} // namespace livox_ros
#endif

36
livox_ros_driver/livox_ros_driver/livox_ros_driver.cpp
View File

@@ -22,18 +22,18 @@
// SOFTWARE.
//
#include <vector>
#include <chrono>
#include "include/livox_ros_driver.h"
#include <chrono>
#include <vector>
#include "lddc.h"
#include "lds_hub.h"
#include "lds_lidar.h"
#include "lds_lvx.h"
#include "livox_sdk.h"
#include <ros/ros.h>
#include "lds_lvx.h"
#include "lds_lidar.h"
#include "lds_hub.h"
#include "lddc.h"
#include "include/livox_ros_driver.h"
using namespace livox_ros;
const int32_t kSdkVersionMajorLimit = 2;
@@ -43,7 +43,8 @@ int main(int argc, char **argv) {
ROS_INFO("Livox Ros Driver Version: %s", LIVOX_ROS_DRIVER_VERSION_STRING);
/** Ros related */
if(ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)) {
if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME,
ros::console::levels::Debug)) {
ros::console::notifyLoggerLevelsChanged();
}
ros::init(argc, argv, "livox_lidar_publisher");
@@ -53,8 +54,8 @@ int main(int argc, char **argv) {
LivoxSdkVersion _sdkversion;
GetLivoxSdkVersion(&_sdkversion);
if (_sdkversion.major < kSdkVersionMajorLimit) {
ROS_INFO("The SDK version[%d.%d.%d] is too low", \
_sdkversion.major, _sdkversion.minor, _sdkversion.patch);
ROS_INFO("The SDK version[%d.%d.%d] is too low", _sdkversion.major,
_sdkversion.minor, _sdkversion.patch);
return 0;
}
@@ -62,7 +63,7 @@ int main(int argc, char **argv) {
int xfer_format = kPointCloud2Msg;
int multi_topic = 0;
int data_src = kSourceRawLidar;
double publish_freq = 50.0; /* Hz */
double publish_freq = 20.0; /* Hz */
int output_type = kOutputToRos;
livox_node.getParam("xfer_format", xfer_format);
@@ -72,7 +73,8 @@ int main(int argc, char **argv) {
livox_node.getParam("output_data_type", output_type);
/** Lidar data distribute control and lidar data source set */
Lddc* lddc = new Lddc(xfer_format, multi_topic, data_src, output_type, publish_freq);
Lddc *lddc =
new Lddc(xfer_format, multi_topic, data_src, output_type, publish_freq);
lddc->SetRosNode(&livox_node);
int ret = 0;
@@ -148,7 +150,11 @@ int main(int argc, char **argv) {
}
ros::Time::init();
ros::Rate r(publish_freq);
double poll_freq = publish_freq;
if (data_src == kSourceLvxFile) {
poll_freq = 2000;
}
ros::Rate r(poll_freq);
while (ros::ok()) {
lddc->DistributeLidarData();
r.sleep();
@@ -156,5 +162,3 @@ int main(int argc, char **argv) {
return 0;
}

102
livox_ros_driver/livox_ros_driver/lvx_file.cpp
View File

@@ -23,13 +23,13 @@
//
#include "lvx_file.h"
#include <time.h>
#include <string.h>
#include <cmath>
#include <string.h>
#include <time.h>
#include "lds.h"
#include "rapidxml/rapidxml.hpp"
#include "rapidxml/rapidxml_utils.hpp"
#include "lds.h"
namespace livox_ros {
@@ -39,7 +39,8 @@ const uint32_t kMaxLvxFileHeaderLength = 16 * 1024;
const char *kLvxHeaderSigStr = "livox_tech";
const uint32_t kLvxHeaderMagicCode = 0xac0ea767;
LvxFileHandle::LvxFileHandle() : file_ver_(kLvxFileV1), device_count_(0), cur_frame_index_(0),
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) {
memset((void *)&public_header_, 0, sizeof(public_header_));
memset((void *)&private_header_, 0, sizeof(private_header_));
@@ -60,8 +61,9 @@ bool LvxFileHandle::ReadAndCheckHeader() {
}
*/
if (public_header_.version[1] > kLvxFileV1) {
printf("Unkown lvx file version[%d.%d.%d.%d]\n", public_header_.version[0], \
public_header_.version[1], public_header_.version[2], public_header_.version[3]);
printf("Unkown lvx file version[%d.%d.%d.%d]\n", public_header_.version[0],
public_header_.version[1], public_header_.version[2],
public_header_.version[3]);
return false;
}
@@ -73,11 +75,13 @@ bool LvxFileHandle::ReadAndCheckHeader() {
uint64_t LvxFileHandle::MiniFileSize() {
if (file_ver_ == kLvxFileV1) {
return (sizeof(LvxFilePublicHeader) + sizeof(LvxFilePrivateHeader) + \
sizeof(LvxFileDeviceInfo) + sizeof(FrameHeader) + sizeof(LvxFilePacket));
return (sizeof(LvxFilePublicHeader) + sizeof(LvxFilePrivateHeader) +
sizeof(LvxFileDeviceInfo) + sizeof(FrameHeader) +
sizeof(LvxFilePacket));
} else {
return (sizeof(LvxFilePublicHeader) + sizeof(LvxFilePrivateHeaderV0) + \
sizeof(LvxFileDeviceInfoV0) + sizeof(FrameHeaderV0) + sizeof(LvxFilePacketV0));
return (sizeof(LvxFilePublicHeader) + sizeof(LvxFilePrivateHeaderV0) +
sizeof(LvxFileDeviceInfoV0) + sizeof(FrameHeaderV0) +
sizeof(LvxFilePacketV0));
}
}
@@ -87,10 +91,10 @@ uint64_t LvxFileHandle::PrivateHeaderOffset() {
uint64_t LvxFileHandle::DataStartOffset() {
if (file_ver_ == kLvxFileV1) {
return (sizeof(LvxFilePublicHeader) + sizeof(LvxFilePrivateHeader) + \
return (sizeof(LvxFilePublicHeader) + sizeof(LvxFilePrivateHeader) +
sizeof(LvxFileDeviceInfo) * private_header_.device_count);
} else {
return (sizeof(LvxFilePublicHeader) + sizeof(LvxFilePrivateHeaderV0) + \
return (sizeof(LvxFilePublicHeader) + sizeof(LvxFilePrivateHeaderV0) +
sizeof(LvxFileDeviceInfoV0) * private_header_v0_.device_count);
}
}
@@ -117,9 +121,9 @@ bool LvxFileHandle::AddAndCheckDeviceInfo() {
} else { /* device info v0 to v1 */
LvxFileDeviceInfoV0 device_info_v0;
lvx_file_.read((char *)(&device_info_v0), sizeof(LvxFileDeviceInfoV0));
memcpy((void *)&device_info, (void *)&device_info_v0, \
memcpy((void *)&device_info, (void *)&device_info_v0,
&device_info.extrinsic_enable - device_info.lidar_broadcast_code);
memcpy((void *)&device_info.roll, (void *)&device_info_v0.roll, \
memcpy((void *)&device_info.roll, (void *)&device_info_v0.roll,
sizeof(float) * 6);
device_info.extrinsic_enable = 0;
}
@@ -135,7 +139,7 @@ bool LvxFileHandle::PrepareDataRead() {
FrameHeader frame_header; /* v0&v1 compatible */
lvx_file_.read((char *)(&frame_header), sizeof(frame_header));
if ((frame_header.current_offset != DataStartOffset()) ||\
if ((frame_header.current_offset != DataStartOffset()) ||
(frame_header.frame_index != 0)) {
return false;
}
@@ -159,6 +163,7 @@ int LvxFileHandle::Open(const char* filename,std::ios_base::openmode mode) {
size_ = lvx_file_.tellg();
lvx_file_.seekg(0, std::ios::beg);
printf("Filesize %lu\n", size_);
if (size_ < MiniFileSize()) {
state_ = kLvxFileSizeFault;
@@ -192,9 +197,7 @@ int LvxFileHandle::Open(const char* filename,std::ios_base::openmode mode) {
return state_;
}
bool LvxFileHandle::Eof() {
return lvx_file_.eof();
}
bool LvxFileHandle::Eof() { return lvx_file_.eof(); }
int LvxFileHandle::InitLvxFile() {
time_t curtime = time(nullptr);
@@ -217,33 +220,41 @@ void LvxFileHandle::InitLvxFileHeader() {
public_header_.version[2] = 0;
public_header_.version[3] = 0;
public_header_.magic_code = kLvxHeaderMagicCode;
memcpy(&write_buffer[cur_offset_], (void *)&public_header_, sizeof(public_header_));
memcpy(&write_buffer[cur_offset_], (void *)&public_header_,
sizeof(public_header_));
cur_offset_ += sizeof(public_header_);
if (file_ver_ == kLvxFileV1) {
private_header_.device_count = static_cast<uint8_t>(device_info_list_.size());
private_header_.device_count =
static_cast<uint8_t>(device_info_list_.size());
private_header_.frame_duration = frame_duration_;
device_count_ = private_header_.device_count;
memcpy(&write_buffer[cur_offset_], (void *)&private_header_, sizeof(private_header_));
memcpy(&write_buffer[cur_offset_], (void *)&private_header_,
sizeof(private_header_));
cur_offset_ += sizeof(private_header_);
} else {
private_header_v0_.device_count = static_cast<uint8_t>(device_info_list_.size());
private_header_v0_.device_count =
static_cast<uint8_t>(device_info_list_.size());
device_count_ = private_header_v0_.device_count;
memcpy(&write_buffer[cur_offset_], (void *)&private_header_v0_, sizeof(private_header_v0_));
memcpy(&write_buffer[cur_offset_], (void *)&private_header_v0_,
sizeof(private_header_v0_));
cur_offset_ += sizeof(private_header_v0_);
}
for (int i = 0; i < device_count_; i++) {
if (file_ver_ == kLvxFileV1) {
memcpy(&write_buffer[cur_offset_], (void *)&device_info_list_[i], sizeof(LvxFileDeviceInfo));
memcpy(&write_buffer[cur_offset_], (void *)&device_info_list_[i],
sizeof(LvxFileDeviceInfo));
cur_offset_ += sizeof(LvxFileDeviceInfo);
} else {
LvxFileDeviceInfoV0 device_info_v0;
memcpy((void *)&device_info_v0, (void *)&device_info_list_[i], \
&device_info_list_[i].extrinsic_enable - device_info_list_[i].lidar_broadcast_code);
memcpy((void *)&device_info_v0.roll, (void *)&device_info_list_[i].roll, \
memcpy((void *)&device_info_v0, (void *)&device_info_list_[i],
&device_info_list_[i].extrinsic_enable -
device_info_list_[i].lidar_broadcast_code);
memcpy((void *)&device_info_v0.roll, (void *)&device_info_list_[i].roll,
sizeof(float) * 6);
memcpy(&write_buffer[cur_offset_], (void *)&device_info_v0, sizeof(device_info_v0));
memcpy(&write_buffer[cur_offset_], (void *)&device_info_v0,
sizeof(device_info_v0));
cur_offset_ += sizeof(device_info_v0);
}
}
@@ -251,7 +262,8 @@ void LvxFileHandle::InitLvxFileHeader() {
lvx_file_.write(&write_buffer[cur_offset_], cur_offset_);
}
void LvxFileHandle::SaveFrameToLvxFile(std::list<LvxFilePacket> &point_packet_list_temp) {
void LvxFileHandle::SaveFrameToLvxFile(
std::list<LvxFilePacket> &point_packet_list_temp) {
uint64_t cur_pos = 0;
FrameHeader frame_header = {0};
std::unique_ptr<char[]> write_buffer(new char[kMaxFrameSize]);
@@ -264,7 +276,8 @@ void LvxFileHandle::SaveFrameToLvxFile(std::list<LvxFilePacket> &point_packet_li
frame_header.frame_index = cur_frame_index_;
memcpy(write_buffer.get() + cur_pos, (void*)&frame_header, sizeof(FrameHeader));
memcpy(write_buffer.get() + cur_pos, (void *)&frame_header,
sizeof(FrameHeader));
cur_pos += sizeof(FrameHeader);
for (auto iter : point_packet_list_temp) {
@@ -289,7 +302,8 @@ void LvxFileHandle::CloseLvxFile() {
lvx_file_.close();
}
void LvxFileHandle::BasePointsHandle(LivoxEthPacket *data, LvxFilePacket &packet) {
void LvxFileHandle::BasePointsHandle(LivoxEthPacket *data,
LvxFilePacket &packet) {
memcpy((void *)&packet, (void *)data, GetEthPacketLen(data->data_type));
}
@@ -308,6 +322,13 @@ int LvxFileHandle::GetPacketsOfFrame(OutPacketBuffer* packets_of_frame) {
return kLvxFileAtEnd;
}
uint64_t tmp_size = lvx_file_.tellg();
if (tmp_size >= size_) {
printf("At the file end %lu\n", tmp_size);
state_ = kLvxFileAtEnd;
return kLvxFileAtEnd;
}
FrameHeader frame_header;
FrameHeaderV0 frame_header_v0;
uint64_t read_length;
@@ -316,7 +337,7 @@ int LvxFileHandle::GetPacketsOfFrame(OutPacketBuffer* packets_of_frame) {
if (!lvx_file_) {
return kLvxFileReadFail;
}
if ((size_ < frame_header.current_offset) || \
if ((size_ < frame_header.current_offset) ||
(frame_header.next_offset < frame_header.current_offset)) {
return kLvxFileFrameHeaderError;
}
@@ -327,14 +348,13 @@ int LvxFileHandle::GetPacketsOfFrame(OutPacketBuffer* packets_of_frame) {
if (!lvx_file_) {
return kLvxFileReadFail;
}
if ((size_ < frame_header_v0.current_offset) || \
if ((size_ < frame_header_v0.current_offset) ||
(frame_header_v0.next_offset < frame_header_v0.current_offset)) {
return kLvxFileFrameHeaderError;
}
packets_of_frame->data_size = DataSizeOfFrame(frame_header_v0);
read_length = packets_of_frame->data_size;
}
lvx_file_.read((char *)(packets_of_frame->packet), read_length);
if (lvx_file_) {
return kLvxFileOk;
@@ -361,16 +381,17 @@ void ParseExtrinsicXml(DeviceItem &item, LvxFileDeviceInfo &info) {
doc.parse<0>(extrinsic_param.data());
rapidxml::xml_node<> *root = doc.first_node();
if ("Livox" == (std::string)root->name()) {
for (rapidxml::xml_node<>* device = root->first_node(); device; \
for (rapidxml::xml_node<> *device = root->first_node(); device;
device = device->next_sibling()) {
if ("Device" == (std::string)device->name() && \
(strncmp(item.info.broadcast_code, device->value(), kBroadcastCodeSize) == 0)) {
if ("Device" == (std::string)device->name() &&
(strncmp(item.info.broadcast_code, device->value(),
kBroadcastCodeSize) == 0)) {
memcpy(info.lidar_broadcast_code, device->value(), kBroadcastCodeSize);
memset(info.hub_broadcast_code, 0, kBroadcastCodeSize);
info.device_type = item.info.type;
info.device_index = item.handle;
for (rapidxml::xml_attribute<>* param = device->first_attribute(); param; \
param = param->next_attribute()) {
for (rapidxml::xml_attribute<> *param = device->first_attribute();
param; param = param->next_attribute()) {
if ("roll" == (std::string)param->name()) {
info.roll = static_cast<float>(atof(param->value()));
}
@@ -395,5 +416,4 @@ void ParseExtrinsicXml(DeviceItem &item, LvxFileDeviceInfo &info) {
}
}
}
} // namespace livox_ros

24
livox_ros_driver/livox_ros_driver/lvx_file.h
View File

@@ -24,13 +24,13 @@
#ifndef LIVOX_FILE_H_
#define LIVOX_FILE_H_
#include <memory>
#include <ios>
#include <fstream>
#include <list>
#include <vector>
#include <mutex>
#include "livox_sdk.h"
#include <fstream>
#include <ios>
#include <list>
#include <memory>
#include <mutex>
#include <vector>
namespace livox_ros {
@@ -185,7 +185,9 @@ class LvxFileHandle {
void BasePointsHandle(LivoxEthPacket *data, LvxFilePacket &packet);
void CloseLvxFile();
void AddDeviceInfo(LvxFileDeviceInfo& info) { device_info_list_.push_back(info); }
void AddDeviceInfo(LvxFileDeviceInfo &info) {
device_info_list_.push_back(info);
}
int GetDeviceInfoListSize() { return device_info_list_.size(); }
int GetDeviceCount() { return device_count_; }
int GetDeviceInfo(uint8_t idx, LvxFileDeviceInfo *info);
@@ -220,15 +222,15 @@ class LvxFileHandle {
bool PrepareDataRead();
uint64_t DataSizeOfFrame(FrameHeader &frame_header) {
return (frame_header.next_offset - frame_header.current_offset - sizeof(frame_header));
return (frame_header.next_offset - frame_header.current_offset -
sizeof(frame_header));
}
uint64_t DataSizeOfFrame(FrameHeaderV0 &frame_header_v0) {
return (frame_header_v0.next_offset - frame_header_v0.current_offset - \
return (frame_header_v0.next_offset - frame_header_v0.current_offset -
sizeof(frame_header_v0));
}
};
}
} // namespace livox_ros
#endif

33
livox_ros_driver/timesync/timesync.cpp
View File

@@ -24,17 +24,18 @@
#include "timesync.h"
#include <chrono>
#include <functional>
#include <stdint.h>
#include <string.h>
#include <thread>
#include <chrono>
#include <functional>
namespace livox_ros {
using namespace std;
TimeSync::TimeSync() : exit_poll_state_(false), start_poll_state_(false),
exit_poll_data_(false), start_poll_data_(false) {
TimeSync::TimeSync()
: exit_poll_state_(false), start_poll_state_(false), exit_poll_data_(false),
start_poll_data_(false) {
fsm_state_ = kOpenDev;
uart_ = nullptr;
comm_ = nullptr;
@@ -43,9 +44,7 @@ TimeSync::TimeSync() : exit_poll_state_(false), start_poll_state_(false),
rx_bytes_ = 0;
}
TimeSync::~TimeSync() {
DeInitTimeSync();
}
TimeSync::~TimeSync() { DeInitTimeSync(); }
int32_t TimeSync::InitTimeSync(const TimeSyncConfig &config) {
config_ = config;
@@ -68,8 +67,10 @@ int32_t TimeSync::InitTimeSync(const TimeSyncConfig& config) {
config_.protocol_config.type = kGps;
comm_ = new CommProtocol(config_.protocol_config);
t_poll_state_ = std::make_shared<std::thread>(std::bind(&TimeSync::PollStateLoop, this));
t_poll_data_ = std::make_shared<std::thread>(std::bind(&TimeSync::PollDataLoop, this));
t_poll_state_ =
std::make_shared<std::thread>(std::bind(&TimeSync::PollStateLoop, this));
t_poll_data_ =
std::make_shared<std::thread>(std::bind(&TimeSync::PollDataLoop, this));
return 0;
}
@@ -77,8 +78,10 @@ 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;
@@ -167,7 +170,8 @@ void TimeSync::FsmOpenDev() {
void TimeSync::FsmPrepareDev() {
chrono::steady_clock::time_point t = chrono::steady_clock::now();
chrono::milliseconds time_gap = chrono::duration_cast<chrono::milliseconds>(t-transfer_time_);
chrono::milliseconds time_gap =
chrono::duration_cast<chrono::milliseconds>(t - transfer_time_);
/** delay some time when device is opened, 4s */
if (time_gap.count() > 3000) {
FsmTransferState(kCheckDevState);
@@ -179,7 +183,8 @@ void TimeSync::FsmCheckDevState() {
static chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::milliseconds time_gap = chrono::duration_cast<chrono::milliseconds>(t2 - t1);
chrono::milliseconds time_gap =
chrono::duration_cast<chrono::milliseconds>(t2 - t1);
if (time_gap.count() > 2000) { /* period : 2.5s */
if (last_rx_bytes == rx_bytes_) {
@@ -192,4 +197,4 @@ void TimeSync::FsmCheckDevState() {
}
}
}
} // namespace livox_ros

17
livox_ros_driver/timesync/timesync.h
View File

@@ -25,22 +25,17 @@
#ifndef TIMESYNC_TIMESYNC_H_
#define TIMESYNC_TIMESYNC_H_
#include <thread>
#include "comm_protocol.h"
#include "comm_device.h"
#include "comm_protocol.h"
#include "user_uart.h"
#include <thread>
namespace livox_ros {
typedef void (*FnReceiveSyncTimeCb)(const char* rmc, uint32_t rmc_length, void* client_data);
typedef void (*FnReceiveSyncTimeCb)(const char *rmc, uint32_t rmc_length,
void *client_data);
enum FsmPollState {
kOpenDev,
kPrepareDev,
kCheckDevState,
kFsmDevUndef
};
enum FsmPollState { kOpenDev, kPrepareDev, kCheckDevState, kFsmDevUndef };
typedef struct {
CommDevConfig dev_config;
@@ -106,5 +101,5 @@ class TimeSync {
void FsmCheckDevState();
};
}
} // namespace livox_ros
#endif

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

@@ -22,20 +22,19 @@
// SOFTWARE.
//
#include "user_uart.h"
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <termios.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <termios.h>
#include <unistd.h>
namespace livox_ros {
UserUart::UserUart(uint8_t baudrate_index, uint8_t parity):
baudrate_(baudrate_index), parity_(parity) {
UserUart::UserUart(uint8_t baudrate_index, uint8_t parity)
: baudrate_(baudrate_index), parity_(parity) {
fd_ = 0;
is_open_ = false;
}
@@ -87,11 +86,10 @@ int UserUart::Close() {
/** sets up the port parameters */
int UserUart::Setup(uint8_t baudrate_index, uint8_t parity) {
static uint32_t baud_map[19] = {\
B2400, B4800, B9600, B19200, B38400, B57600,B115200, B230400,\
B460800, B500000, B576000,B921600,B1152000, B1500000, B2000000,\
B2500000, B3000000, B3500000, B4000000\
};
static uint32_t baud_map[19] = {
B2400, B4800, B9600, B19200, B38400, B57600, B115200,
B230400, B460800, B500000, B576000, B921600, B1152000, B1500000,
B2000000, B2500000, B3000000, B3500000, B4000000};
tcflag_t baudrate;
struct termios options;
@@ -194,5 +192,4 @@ ssize_t UserUart::Read(char *buffer, size_t size) {
}
}
}
} // namespace livox_ros

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

@@ -26,9 +26,9 @@
#define USER_UART_H_
#include <stdint.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/fcntl.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <termios.h>
@@ -86,7 +86,6 @@ class UserUart {
uint8_t parity_;
};
}
} // namespace livox_ros
#endif