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_

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

@@ -22,15 +22,14 @@
* SOFTWARE.
*/
/*
Tables generated with universal_crc by Danjel McGougan
*/
/*
Tables generated with universal_crc by Danjel McGougan
*/
//
// 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

129
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
@@ -60,49 +56,46 @@ FastCRC16::FastCRC16(uint16_t seed) {
uint16_t FastCRC16::mcrf4xx_calc(const uint8_t *data, uint16_t len) {
uint16_t crc = seed_;
uint16_t crc = seed_;
while (((uintptr_t)data & 3) && len) {
crc = (crc >> 8) ^ crc_table_mcrf4xx[(crc & 0xff) ^ *data++];
len--;
}
while (((uintptr_t)data & 3) && len) {
crc = (crc >> 8) ^ crc_table_mcrf4xx[(crc & 0xff) ^ *data++];
len--;
}
while (len >= 16) {
len -= 16;
crc_n4(crc, ((uint32_t *)data)[0], crc_table_mcrf4xx);
crc_n4(crc, ((uint32_t *)data)[1], crc_table_mcrf4xx);
crc_n4(crc, ((uint32_t *)data)[2], crc_table_mcrf4xx);
crc_n4(crc, ((uint32_t *)data)[3], crc_table_mcrf4xx);
data += 16;
}
while (len >= 16) {
len -= 16;
crc_n4(crc, ((uint32_t *)data)[0], crc_table_mcrf4xx);
crc_n4(crc, ((uint32_t *)data)[1], crc_table_mcrf4xx);
crc_n4(crc, ((uint32_t *)data)[2], crc_table_mcrf4xx);
crc_n4(crc, ((uint32_t *)data)[3], crc_table_mcrf4xx);
data += 16;
}
while (len--) {
crc = (crc >> 8) ^ crc_table_mcrf4xx[(crc & 0xff) ^ *data++];
}
while (len--) {
crc = (crc >> 8) ^ crc_table_mcrf4xx[(crc & 0xff) ^ *data++];
}
//seed = crc;
return crc;
// seed = crc;
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; \
crc = (crc >> 8) ^ (table[crc & 0xff]); \
crc = (crc >> 8) ^ (table[crc & 0xff]); \
crc = (crc >> 8) ^ (table[crc & 0xff]); \
crc = (crc >> 8) ^ (table[crc & 0xff]);
#define crcsm_n4d(crc, data, table) \
crc ^= data; \
crc = (crc >> 8) ^ (table[crc & 0xff]); \
crc = (crc >> 8) ^ (table[crc & 0xff]); \
crc = (crc >> 8) ^ (table[crc & 0xff]); \
crc = (crc >> 8) ^ (table[crc & 0xff]);
/** CRC32
* Alias CRC-32/ADCCP, PKZIP, Ethernet, 802.3
@@ -118,37 +111,35 @@ FastCRC32::FastCRC32(uint32_t seed) {
uint32_t FastCRC32::crc32_calc(const uint8_t *data, uint16_t len) {
uint32_t crc = seed_^0xffffffff;
uint32_t crc = seed_ ^ 0xffffffff;
while (((uintptr_t)data & 3) && len) {
crc = (crc >> 8) ^ CRC_TABLE_CRC32[(crc & 0xff) ^ *data++];
len--;
}
while (((uintptr_t)data & 3) && len) {
crc = (crc >> 8) ^ CRC_TABLE_CRC32[(crc & 0xff) ^ *data++];
len--;
}
while (len >= 16) {
len -= 16;
#if CRC_BIGTABLES
crc_n4d(crc, ((uint32_t *)data)[0], CRC_TABLE_CRC32);
crc_n4d(crc, ((uint32_t *)data)[1], CRC_TABLE_CRC32);
crc_n4d(crc, ((uint32_t *)data)[2], CRC_TABLE_CRC32);
crc_n4d(crc, ((uint32_t *)data)[3], CRC_TABLE_CRC32);
#else
crcsm_n4d(crc, ((uint32_t *)data)[0], CRC_TABLE_CRC32);
crcsm_n4d(crc, ((uint32_t *)data)[1], CRC_TABLE_CRC32);
crcsm_n4d(crc, ((uint32_t *)data)[2], CRC_TABLE_CRC32);
crcsm_n4d(crc, ((uint32_t *)data)[3], CRC_TABLE_CRC32);
#endif
data += 16;
}
while (len >= 16) {
len -= 16;
#if CRC_BIGTABLES
crc_n4d(crc, ((uint32_t *)data)[0], CRC_TABLE_CRC32);
crc_n4d(crc, ((uint32_t *)data)[1], CRC_TABLE_CRC32);
crc_n4d(crc, ((uint32_t *)data)[2], CRC_TABLE_CRC32);
crc_n4d(crc, ((uint32_t *)data)[3], CRC_TABLE_CRC32);
#else
crcsm_n4d(crc, ((uint32_t *)data)[0], CRC_TABLE_CRC32);
crcsm_n4d(crc, ((uint32_t *)data)[1], CRC_TABLE_CRC32);
crcsm_n4d(crc, ((uint32_t *)data)[2], CRC_TABLE_CRC32);
crcsm_n4d(crc, ((uint32_t *)data)[3], CRC_TABLE_CRC32);
#endif
data += 16;
}
while (len--) {
crc = (crc >> 8) ^ CRC_TABLE_CRC32[(crc & 0xff) ^ *data++];
}
while (len--) {
crc = (crc >> 8) ^ CRC_TABLE_CRC32[(crc & 0xff) ^ *data++];
}
//seed = crc;
crc ^= 0xffffffff;
// seed = crc;
crc ^= 0xffffffff;
return crc;
return crc;
}

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

@@ -48,12 +48,12 @@ struct CommDevUartConfig {
/** Communication device usb config */
struct CommDevUsbConfig {
void* data;
void *data;
};
/** Communication device can config */
struct CommDevCanConfig {
void* data;
void *data;
};
/** Communication device config */
@@ -67,5 +67,5 @@ typedef struct {
} config;
} CommDevConfig;
} // namespace livox
#endif // COMM_COMM_DEVICE_H_
} // namespace livox_ros
#endif // COMM_COMM_DEVICE_H_

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

@@ -23,17 +23,17 @@
//
#include "comm_protocol.h"
#include <iostream>
#include <stdio.h>
#include <string.h>
#include <iostream>
namespace livox_ros {
CommProtocol::CommProtocol(ProtocolConfig& config) : config_(config) {
CommProtocol::CommProtocol(ProtocolConfig &config) : config_(config) {
ResetParser();
ResetCache();
offset_to_read_index_= 0;
offset_to_read_index_ = 0;
packet_length_ = 0;
if (kGps == config.type) {
@@ -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,38 +104,35 @@ 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: {
FsmSearchPacketPreamble();
break;
}
case kFindPacketLength: {
FsmFindPacketLength();
break;
}
case kGetPacketData: {
ret = FsmGetPacketData(o_pack);
break;
}
default: {
FsmParserStateTransfer(kSearchPacketPreamble);
}
case kSearchPacketPreamble: {
FsmSearchPacketPreamble();
break;
}
case kFindPacketLength: {
FsmFindPacketLength();
break;
}
case kGetPacketData: {
ret = FsmGetPacketData(o_pack);
break;
}
default: { FsmParserStateTransfer(kSearchPacketPreamble); }
}
if (ret == kParseNeedMoreData) break; /* */
if (ret == kParseNeedMoreData)
break; /* */
}
return ret;
@@ -158,16 +156,18 @@ 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;
}
}
}
//printf("|%2x", cache_.buf[cache_.rd_idx]);
// printf("|%2x", cache_.buf[cache_.rd_idx]);
++cache_.rd_idx; /* skip one byte */
} while(0);
} while (0);
return 0;
}
@@ -216,9 +216,9 @@ int32_t CommProtocol::FsmGetPacketData(CommPacket *o_pack) {
cache_.rd_idx += protocol_->GetPacketLen(GetCacheReadPos());
FsmParserStateTransfer(kSearchPacketPreamble);
return kParseSuccess;
} while(0);
} while (0);
return ret;
}
} // namespace livox
} // namespace livox_ros

23
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;
@@ -50,11 +50,12 @@ typedef struct {
} CommCache;
class CommProtocol {
public:
CommProtocol(ProtocolConfig& config);
public:
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);
@@ -66,11 +67,11 @@ class CommProtocol {
void ResetParser();
private:
private:
uint32_t GetCacheTailSize();
uint32_t GetValidDataSize();
void UpdateCache(void);
uint8_t* GetCacheReadPos() { return &cache_.buf[cache_.rd_idx]; }
uint8_t *GetCacheReadPos() { return &cache_.buf[cache_.rd_idx]; }
void ResetCache() {
cache_.wr_idx = 0;
cache_.rd_idx = 0;
@@ -78,7 +79,7 @@ class CommProtocol {
}
ProtocolConfig config_;
Protocol* protocol_;
Protocol *protocol_;
CommCache cache_;
uint16_t seq_num_;
@@ -92,7 +93,7 @@ class CommProtocol {
int32_t FsmFindPacketLength();
int32_t FsmGetPacketData(CommPacket *o_pack);
void FsmParserStateTransfer(uint32_t new_state) {
if(new_state < kParseStepUndef) {
if (new_state < kParseStepUndef) {
fsm_parse_step_ = new_state;
} else {
fsm_parse_step_ = kSearchPacketPreamble;
@@ -100,5 +101,5 @@ class CommProtocol {
}
};
} // namespace livox
#endif // COMM_COMM_PROTOCOL_H_
} // namespace livox_ros
#endif // COMM_COMM_PROTOCOL_H_

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

@@ -34,44 +34,40 @@ const uint8_t kGpsProtocolSof = '$';
const uint8_t kGpsProtocolEof = '*';
const uint32_t kPacketLengthLmit = 200;
const uint32_t kPreambleLen = 1;
const uint32_t kWrapperLen = 4; /** '$' + '*' + '2 checksum byte' */
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;
// GpsPacket* gps_packet = (GpsPacket*)o_buf;
return 0;
}
int32_t GpsProtocol::ParsePacket(const uint8_t *i_buf, uint32_t i_len, CommPacket *o_packet) {
//GpsPacket *gps_packet = (GpsPacket *)i_buf;
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()) {
return -1; // packet length error
return -1; // packet length error
}
memset((void *)o_packet, 0, sizeof(CommPacket));
o_packet->protocol = kGps;
o_packet->data = (uint8_t *)i_buf;
o_packet->data = (uint8_t *)i_buf;
o_packet->data_len = i_len;
return 0;
}
uint32_t GpsProtocol::GetPreambleLen() {
return kPreambleLen; /** '$' */
}
uint32_t GpsProtocol::GetPreambleLen() { return kPreambleLen; /** '$' */ }
uint32_t GpsProtocol::GetPacketWrapperLen() {
return kWrapperLen; /** '$' + '*' + '2 checksum bytes' */
}
uint32_t GpsProtocol::FindPacketLen(const uint8_t *buf, uint32_t buf_length) {
uint32_t i=0;
for (; (i<buf_length) && (i<kPacketLengthLmit); i++) {
uint32_t i = 0;
for (; (i < buf_length) && (i < kPacketLengthLmit); i++) {
if ((buf[i] == kGpsProtocolEof) && (buf[0] == kGpsProtocolSof)) {
found_length_ = i + 1 + 2; /* num = index + 1 + two bytes checksum */
return kFindLengthSuccess;
@@ -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;
return h * 16 + l;
}
}// namespace livox
} // namespace livox_ros

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

19
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;
@@ -69,7 +73,7 @@ typedef struct {
/** NAME-0183 Protocol info config for gps */
typedef struct {
void* data;
void *data;
} GpsProtocolConfig;
typedef struct {
@@ -81,12 +85,13 @@ typedef struct {
} ProtocolConfig;
class Protocol {
public:
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
#endif // COMM_PROTOCOL_H_
} // namespace livox_ros
#endif // COMM_PROTOCOL_H_

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

@@ -29,12 +29,13 @@
namespace livox_ros {
const uint8_t kSdkProtocolSof = 0xAA;
const uint32_t kSdkPacketCrcSize = 4; // crc32
const uint32_t kSdkPacketPreambleCrcSize = 2; // crc16
const uint32_t kSdkPacketCrcSize = 4; // crc32
const uint32_t kSdkPacketPreambleCrcSize = 2; // crc16
SdkProtocol::SdkProtocol(uint16_t seed16, uint32_t seed32) : crc16_(seed16), crc32_(seed32) {}
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,11 +72,12 @@ 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()) {
return -1; // packet lenth error
return -1; // packet lenth error
}
memset((void *)o_packet, 0, sizeof(CommPacket));
@@ -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,10 +120,10 @@ 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)(buf[sdk_packet->length - 1])) << 24);
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) {
return 0;
@@ -129,4 +131,4 @@ int32_t SdkProtocol::CheckPacket(const uint8_t *buf) {
return -1;
}
}
}// namespace livox
} // namespace livox_ros

17
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;
@@ -58,13 +58,14 @@ typedef struct {
#pragma pack()
class SdkProtocol : public Protocol {
public:
public:
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;
@@ -77,9 +78,9 @@ class SdkProtocol : public Protocol {
int32_t CheckPacket(const uint8_t *buf) override;
private:
private:
FastCRC16 crc16_;
FastCRC32 crc32_;
};
} // namespace livox
#endif // LIVOX_SDK_PROTOCOL_H_
} // namespace livox_ros
#endif // LIVOX_SDK_PROTOCOL_H_

406
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.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// 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
// 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
//
// 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_
@@ -24,15 +28,16 @@ RAPIDJSON_NAMESPACE_BEGIN
/*! \class rapidjson::Allocator
\brief Concept for allocating, resizing and freeing memory block.
Note that Malloc() and Realloc() are non-static but Free() is static.
So if an allocator need to support Free(), it needs to put its pointer in
So if an allocator need to support Free(), it needs to put its pointer in
the header of memory block.
\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
@@ -74,209 +79,226 @@ concept Allocator {
*/
class CrtAllocator {
public:
static const bool kNeedFree = true;
void* Malloc(size_t size) {
if (size) // behavior of malloc(0) is implementation defined.
return std::malloc(size);
else
return NULL; // standardize to returning NULL.
static const bool kNeedFree = true;
void *Malloc(size_t size) {
if (size) // behavior of malloc(0) is implementation defined.
return std::malloc(size);
else
return NULL; // standardize to returning NULL.
}
void *Realloc(void *originalPtr, size_t originalSize, size_t newSize) {
(void)originalSize;
if (newSize == 0) {
std::free(originalPtr);
return NULL;
}
void* Realloc(void* originalPtr, size_t originalSize, size_t newSize) {
(void)originalSize;
if (newSize == 0) {
std::free(originalPtr);
return NULL;
}
return std::realloc(originalPtr, newSize);
}
static void Free(void *ptr) { std::free(ptr); }
return std::realloc(originalPtr, newSize);
}
static void Free(void *ptr) { std::free(ptr); }
};
///////////////////////////////////////////////////////////////////////////////
// MemoryPoolAllocator
//! Default memory allocator used by the parser and DOM.
/*! This allocator allocate memory blocks from pre-allocated memory chunks.
/*! This allocator allocate memory blocks from pre-allocated memory chunks.
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.
*/
MemoryPoolAllocator(size_t chunkSize = kDefaultChunkCapacity, BaseAllocator* baseAllocator = 0) :
chunkHead_(0), chunk_capacity_(chunkSize), userBuffer_(0), baseAllocator_(baseAllocator), ownBaseAllocator_(0)
{
//! Constructor with chunkSize.
/*! \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) {}
//! 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 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.
*/
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);
chunkHead_->capacity = size - sizeof(ChunkHeader);
chunkHead_->size = 0;
chunkHead_->next = 0;
}
//! Destructor.
/*! This deallocates all memory chunks, excluding the user-supplied buffer.
*/
~MemoryPoolAllocator() {
Clear();
RAPIDJSON_DELETE(ownBaseAllocator_);
}
//! Deallocates all memory chunks, excluding the user-supplied buffer.
void Clear() {
while (chunkHead_ && chunkHead_ != userBuffer_) {
ChunkHeader *next = chunkHead_->next;
baseAllocator_->Free(chunkHead_);
chunkHead_ = next;
}
if (chunkHead_ && chunkHead_ == userBuffer_)
chunkHead_->size = 0; // Clear user buffer
}
//! Computes the total capacity of allocated memory chunks.
/*! \return total capacity in bytes.
*/
size_t Capacity() const {
size_t capacity = 0;
for (ChunkHeader *c = chunkHead_; c != 0; c = c->next)
capacity += c->capacity;
return capacity;
}
//! Computes the memory blocks allocated.
/*! \return total used bytes.
*/
size_t Size() const {
size_t size = 0;
for (ChunkHeader *c = chunkHead_; c != 0; c = c->next)
size += c->size;
return size;
}
//! Allocates a memory block. (concept Allocator)
void *Malloc(size_t size) {
if (!size)
return NULL;
size = RAPIDJSON_ALIGN(size);
if (chunkHead_ == 0 || chunkHead_->size + size > chunkHead_->capacity)
if (!AddChunk(chunk_capacity_ > size ? chunk_capacity_ : size))
return NULL;
void *buffer = reinterpret_cast<char *>(chunkHead_) +
RAPIDJSON_ALIGN(sizeof(ChunkHeader)) + chunkHead_->size;
chunkHead_->size += size;
return buffer;
}
//! Resizes a memory block (concept Allocator)
void *Realloc(void *originalPtr, size_t originalSize, size_t newSize) {
if (originalPtr == 0)
return Malloc(newSize);
if (newSize == 0)
return NULL;
originalSize = RAPIDJSON_ALIGN(originalSize);
newSize = RAPIDJSON_ALIGN(newSize);
// Do not shrink if new size is smaller than original
if (originalSize >= newSize)
return originalPtr;
// 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;
return originalPtr;
}
}
//! Constructor with user-supplied buffer.
/*! The user buffer will be used firstly. When it is full, memory pool allocates new chunk with chunk size.
// Realloc process: allocate and copy memory, do not free original buffer.
if (void *newBuffer = Malloc(newSize)) {
if (originalSize)
std::memcpy(newBuffer, originalPtr, originalSize);
return newBuffer;
} else
return NULL;
}
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.
*/
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);
chunkHead_->capacity = size - sizeof(ChunkHeader);
chunkHead_->size = 0;
chunkHead_->next = 0;
}
//! Destructor.
/*! This deallocates all memory chunks, excluding the user-supplied buffer.
*/
~MemoryPoolAllocator() {
Clear();
RAPIDJSON_DELETE(ownBaseAllocator_);
}
//! Deallocates all memory chunks, excluding the user-supplied buffer.
void Clear() {
while (chunkHead_ && chunkHead_ != userBuffer_) {
ChunkHeader* next = chunkHead_->next;
baseAllocator_->Free(chunkHead_);
chunkHead_ = next;
}
if (chunkHead_ && chunkHead_ == userBuffer_)
chunkHead_->size = 0; // Clear user buffer
}
//! Computes the total capacity of allocated memory chunks.
/*! \return total capacity in bytes.
*/
size_t Capacity() const {
size_t capacity = 0;
for (ChunkHeader* c = chunkHead_; c != 0; c = c->next)
capacity += c->capacity;
return capacity;
}
//! Computes the memory blocks allocated.
/*! \return total used bytes.
*/
size_t Size() const {
size_t size = 0;
for (ChunkHeader* c = chunkHead_; c != 0; c = c->next)
size += c->size;
return size;
}
//! Allocates a memory block. (concept Allocator)
void* Malloc(size_t size) {
if (!size)
return NULL;
size = RAPIDJSON_ALIGN(size);
if (chunkHead_ == 0 || chunkHead_->size + size > chunkHead_->capacity)
if (!AddChunk(chunk_capacity_ > size ? chunk_capacity_ : size))
return NULL;
void *buffer = reinterpret_cast<char *>(chunkHead_) + RAPIDJSON_ALIGN(sizeof(ChunkHeader)) + chunkHead_->size;
chunkHead_->size += size;
return buffer;
}
//! Resizes a memory block (concept Allocator)
void* Realloc(void* originalPtr, size_t originalSize, size_t newSize) {
if (originalPtr == 0)
return Malloc(newSize);
if (newSize == 0)
return NULL;
originalSize = RAPIDJSON_ALIGN(originalSize);
newSize = RAPIDJSON_ALIGN(newSize);
// Do not shrink if new size is smaller than original
if (originalSize >= newSize)
return originalPtr;
// 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;
return originalPtr;
}
}
// Realloc process: allocate and copy memory, do not free original buffer.
if (void* newBuffer = Malloc(newSize)) {
if (originalSize)
std::memcpy(newBuffer, originalPtr, originalSize);
return newBuffer;
}
else
return NULL;
}
//! Frees a memory block (concept Allocator)
static void Free(void *ptr) { (void)ptr; } // Do nothing
//! Frees a memory block (concept Allocator)
static void Free(void *ptr) { (void)ptr; } // Do nothing
private:
//! Copy constructor is not permitted.
MemoryPoolAllocator(const MemoryPoolAllocator& rhs) /* = delete */;
//! Copy assignment operator is not permitted.
MemoryPoolAllocator& operator=(const MemoryPoolAllocator& rhs) /* = delete */;
//! Copy constructor is not permitted.
MemoryPoolAllocator(const MemoryPoolAllocator &rhs) /* = delete */;
//! Copy assignment operator is not permitted.
MemoryPoolAllocator &operator=(const MemoryPoolAllocator &rhs) /* = delete */;
//! Creates a new chunk.
/*! \param capacity Capacity of the chunk in bytes.
\return true if success.
*/
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))) {
chunk->capacity = capacity;
chunk->size = 0;
chunk->next = chunkHead_;
chunkHead_ = chunk;
return true;
}
else
return false;
}
//! Creates a new chunk.
/*! \param capacity Capacity of the chunk in bytes.
\return true if success.
*/
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))) {
chunk->capacity = capacity;
chunk->size = 0;
chunk->next = chunkHead_;
chunkHead_ = chunk;
return true;
} 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 size; //!< Current size of allocated memory in bytes.
ChunkHeader *next; //!< Next chunk in the linked list.
};
//! Chunk header for perpending to each chunk.
/*! Chunks are stored as a singly linked list.
*/
struct ChunkHeader {
size_t capacity; //!< Capacity of the chunk in bytes (excluding the header
//!< itself).
size_t size; //!< Current size of allocated memory in bytes.
ChunkHeader *next; //!< Next chunk in the linked list.
};
ChunkHeader *chunkHead_; //!< Head of the chunk linked-list. Only the head chunk serves allocation.
size_t chunk_capacity_; //!< The minimum capacity of chunk when they are allocated.
void *userBuffer_; //!< User supplied buffer.
BaseAllocator* baseAllocator_; //!< base allocator for allocating memory chunks.
BaseAllocator* ownBaseAllocator_; //!< base allocator created by this object.
ChunkHeader *chunkHead_; //!< Head of the chunk linked-list. Only the head
//!< chunk serves allocation.
size_t chunk_capacity_; //!< The minimum capacity of chunk when they are
//!< allocated.
void *userBuffer_; //!< User supplied buffer.
BaseAllocator
*baseAllocator_; //!< base allocator for allocating memory chunks.
BaseAllocator *ownBaseAllocator_; //!< base allocator created by this object.
};
RAPIDJSON_NAMESPACE_END

65
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_
@@ -24,45 +28,44 @@ RAPIDJSON_DIAG_OFF(effc++)
#if defined(_MSC_VER) && _MSC_VER <= 1800
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(4702) // unreachable code
RAPIDJSON_DIAG_OFF(4512) // assignment operator could not be generated
RAPIDJSON_DIAG_OFF(4702) // unreachable code
RAPIDJSON_DIAG_OFF(4512) // assignment operator could not be generated
#endif
RAPIDJSON_NAMESPACE_BEGIN
//! Cursor stream wrapper for counting line and column number if error exists.
/*!
\tparam InputStream Any stream that implements Stream Concept
*/
template <typename InputStream, typename Encoding = UTF8<> >
template <typename InputStream, typename Encoding = UTF8<>>
class CursorStreamWrapper : public GenericStreamWrapper<InputStream, Encoding> {
public:
typedef typename Encoding::Ch Ch;
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() {
Ch ch = this->is_.Take();
if(ch == '\n') {
line_ ++;
col_ = 0;
} else {
col_ ++;
}
return ch;
// counting line and column number
Ch Take() {
Ch ch = this->is_.Take();
if (ch == '\n') {
line_++;
col_ = 0;
} else {
col_++;
}
return ch;
}
//! Get the error line number, if error exists.
size_t GetLine() const { return line_; }
//! Get the error column number, if error exists.
size_t GetColumn() const { return col_; }
//! Get the error line number, if error exists.
size_t GetLine() const { return line_; }
//! Get the error column number, if error exists.
size_t GetColumn() const { return col_; }
private:
size_t line_; //!< Current Line
size_t col_; //!< Current Column
size_t line_; //!< Current Line
size_t col_; //!< Current Column
};
#if defined(_MSC_VER) && _MSC_VER <= 1800

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

File diff suppressed because it is too large Load Diff

483
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.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// 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
// 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
//
// 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,256 +36,365 @@ 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);
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
public:
typedef typename Encoding::Ch Ch;
typedef typename Encoding::Ch Ch;
EncodedInputStream(InputByteStream& is) : is_(is) {
current_ = Encoding::TakeBOM(is_);
}
EncodedInputStream(InputByteStream &is) : is_(is) {
current_ = Encoding::TakeBOM(is_);
}
Ch Peek() const { return current_; }
Ch Take() { Ch c = current_; current_ = Encoding::Take(is_); return c; }
size_t Tell() const { return is_.Tell(); }
Ch Peek() const { return current_; }
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; }
// 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;
}
private:
EncodedInputStream(const EncodedInputStream&);
EncodedInputStream& operator=(const EncodedInputStream&);
EncodedInputStream(const EncodedInputStream &);
EncodedInputStream &operator=(const EncodedInputStream &);
InputByteStream& is_;
Ch current_;
InputByteStream &is_;
Ch current_;
};
//! Specialized for UTF8 MemoryStream.
template <>
class EncodedInputStream<UTF8<>, MemoryStream> {
template <> class EncodedInputStream<UTF8<>, MemoryStream> {
public:
typedef UTF8<>::Ch Ch;
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();
}
Ch Peek() const { return is_.Peek(); }
Ch Take() { return is_.Take(); }
size_t Tell() const { return is_.Tell(); }
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();
}
Ch Peek() const { return is_.Peek(); }
Ch Take() { return is_.Take(); }
size_t Tell() const { return is_.Tell(); }
// Not implemented
void Put(Ch) {}
void Flush() {}
Ch* PutBegin() { return 0; }
size_t PutEnd(Ch*) { return 0; }
// Not implemented
void Put(Ch) {}
void Flush() {}
Ch *PutBegin() { return 0; }
size_t PutEnd(Ch *) { return 0; }
MemoryStream& is_;
MemoryStream &is_;
private:
EncodedInputStream(const EncodedInputStream&);
EncodedInputStream& operator=(const EncodedInputStream&);
EncodedInputStream(const EncodedInputStream &);
EncodedInputStream &operator=(const EncodedInputStream &);
};
//! 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);
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
public:
typedef typename Encoding::Ch Ch;
typedef typename Encoding::Ch Ch;
EncodedOutputStream(OutputByteStream& os, bool putBOM = true) : os_(os) {
if (putBOM)
Encoding::PutBOM(os_);
}
EncodedOutputStream(OutputByteStream &os, bool putBOM = true) : os_(os) {
if (putBOM)
Encoding::PutBOM(os_);
}
void Put(Ch c) { Encoding::Put(os_, c); }
void Flush() { os_.Flush(); }
void Put(Ch c) { Encoding::Put(os_, c); }
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; }
// 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;
}
private:
EncodedOutputStream(const EncodedOutputStream&);
EncodedOutputStream& operator=(const EncodedOutputStream&);
EncodedOutputStream(const EncodedOutputStream &);
EncodedOutputStream &operator=(const EncodedOutputStream &);
OutputByteStream& os_;
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.
*/
template <typename CharType, typename InputByteStream>
class AutoUTFInputStream {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
public:
typedef CharType Ch;
typedef CharType Ch;
//! Constructor.
/*!
\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) {
RAPIDJSON_ASSERT(type >= kUTF8 && type <= kUTF32BE);
DetectType();
static const TakeFunc f[] = { RAPIDJSON_ENCODINGS_FUNC(Take) };
takeFunc_ = f[type_];
current_ = takeFunc_(*is_);
}
//! Constructor.
/*!
\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) {
RAPIDJSON_ASSERT(type >= kUTF8 && type <= kUTF32BE);
DetectType();
static const TakeFunc f[] = {RAPIDJSON_ENCODINGS_FUNC(Take)};
takeFunc_ = f[type_];
current_ = takeFunc_(*is_);
}
UTFType GetType() const { return type_; }
bool HasBOM() const { return hasBOM_; }
UTFType GetType() const { return type_; }
bool HasBOM() const { return hasBOM_; }
Ch Peek() const { return current_; }
Ch Take() { Ch c = current_; current_ = takeFunc_(*is_); return c; }
size_t Tell() const { return is_->Tell(); }
Ch Peek() const { return current_; }
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; }
// 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;
}
private:
AutoUTFInputStream(const AutoUTFInputStream&);
AutoUTFInputStream& operator=(const AutoUTFInputStream&);
AutoUTFInputStream(const AutoUTFInputStream &);
AutoUTFInputStream &operator=(const AutoUTFInputStream &);
// Detect encoding type with BOM or RFC 4627
void DetectType() {
// BOM (Byte Order Mark):
// 00 00 FE FF UTF-32BE
// FF FE 00 00 UTF-32LE
// FE FF UTF-16BE
// FF FE UTF-16LE
// EF BB BF UTF-8
// Detect encoding type with BOM or RFC 4627
void DetectType() {
// BOM (Byte Order Mark):
// 00 00 FE FF UTF-32BE
// FF FE 00 00 UTF-32LE
// FE FF UTF-16BE
// FF FE UTF-16LE
// EF BB BF UTF-8
const unsigned char* c = reinterpret_cast<const unsigned char *>(is_->Peek4());
if (!c)
return;
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));
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(); }
// RFC 4627: Section 3
// "Since the first two characters of a JSON text will always be ASCII
// characters [RFC0020], it is possible to determine whether an octet
// stream is UTF-8, UTF-16 (BE or LE), or UTF-32 (BE or LE) by looking
// at the pattern of nulls in the first four octets."
// 00 00 00 xx UTF-32BE
// 00 xx 00 xx UTF-16BE
// xx 00 00 00 UTF-32LE
// xx 00 xx 00 UTF-16LE
// 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);
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.
}
}
// 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);
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();
}
typedef Ch (*TakeFunc)(InputByteStream& is);
InputByteStream* is_;
UTFType type_;
Ch current_;
TakeFunc takeFunc_;
bool hasBOM_;
// RFC 4627: Section 3
// "Since the first two characters of a JSON text will always be ASCII
// characters [RFC0020], it is possible to determine whether an octet
// stream is UTF-8, UTF-16 (BE or LE), or UTF-32 (BE or LE) by looking
// at the pattern of nulls in the first four octets."
// 00 00 00 xx UTF-32BE
// 00 xx 00 xx UTF-16BE
// xx 00 00 00 UTF-32LE
// xx 00 xx 00 UTF-16LE
// 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);
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.
}
}
// 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);
InputByteStream *is_;
UTFType type_;
Ch current_;
TakeFunc takeFunc_;
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.
*/
template <typename CharType, typename OutputByteStream>
class AutoUTFOutputStream {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
public:
typedef CharType Ch;
typedef CharType Ch;
//! Constructor.
/*!
\param os output stream to be wrapped.
\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) {
RAPIDJSON_ASSERT(type >= kUTF8 && type <= kUTF32BE);
//! Constructor.
/*!
\param os output stream to be wrapped.
\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) {
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_];
static const PutFunc f[] = {RAPIDJSON_ENCODINGS_FUNC(Put)};
putFunc_ = f[type_];
if (putBOM)
PutBOM();
}
if (putBOM)
PutBOM();
}
UTFType GetType() const { return type_; }
UTFType GetType() const { return type_; }
void Put(Ch c) { putFunc_(*os_, c); }
void Flush() { os_->Flush(); }
void Put(Ch c) { putFunc_(*os_, c); }
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; }
// 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;
}
private:
AutoUTFOutputStream(const AutoUTFOutputStream&);
AutoUTFOutputStream& operator=(const AutoUTFOutputStream&);
AutoUTFOutputStream(const AutoUTFOutputStream &);
AutoUTFOutputStream &operator=(const AutoUTFOutputStream &);
void PutBOM() {
typedef void (*PutBOMFunc)(OutputByteStream&);
static const PutBOMFunc f[] = { RAPIDJSON_ENCODINGS_FUNC(PutBOM) };
f[type_](*os_);
}
void PutBOM() {
typedef void (*PutBOMFunc)(OutputByteStream &);
static const PutBOMFunc f[] = {RAPIDJSON_ENCODINGS_FUNC(PutBOM)};
f[type_](*os_);
}
typedef void (*PutFunc)(OutputByteStream&, Ch);
typedef void (*PutFunc)(OutputByteStream &, Ch);
OutputByteStream* os_;
UTFType type_;
PutFunc putFunc_;
OutputByteStream *os_;
UTFType type_;
PutFunc putFunc_;
};
#undef RAPIDJSON_ENCODINGS_FUNC

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

File diff suppressed because it is too large Load Diff

102
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.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// 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
// 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
//
// 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_
@@ -19,8 +23,8 @@
#ifdef __clang__
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(switch-enum)
RAPIDJSON_DIAG_OFF(covered-switch-default)
RAPIDJSON_DIAG_OFF(switch - enum)
RAPIDJSON_DIAG_OFF(covered - switch - default)
#endif
RAPIDJSON_NAMESPACE_BEGIN
@@ -33,36 +37,62 @@ 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) {
switch (parseErrorCode) {
case kParseErrorNone: return RAPIDJSON_ERROR_STRING("No error.");
inline const RAPIDJSON_ERROR_CHARTYPE *
GetParseError_En(ParseErrorCode parseErrorCode) {
switch (parseErrorCode) {
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 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 kParseErrorArrayMissCommaOrSquareBracket: return RAPIDJSON_ERROR_STRING("Missing a comma or ']' after an array element.");
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 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 kParseErrorValueInvalid:
return RAPIDJSON_ERROR_STRING("Invalid value.");
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 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 kParseErrorTermination: return RAPIDJSON_ERROR_STRING("Terminate parsing due to Handler error.");
case kParseErrorUnspecificSyntaxError: return RAPIDJSON_ERROR_STRING("Unspecific syntax error.");
case kParseErrorArrayMissCommaOrSquareBracket:
return RAPIDJSON_ERROR_STRING(
"Missing a comma or ']' after an array element.");
default: return RAPIDJSON_ERROR_STRING("Unknown error.");
}
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 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.");
}
}
RAPIDJSON_NAMESPACE_END

134
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.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// 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
// 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
//
// 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_
@@ -62,31 +66,38 @@ RAPIDJSON_NAMESPACE_BEGIN
\see GenericReader::Parse, GenericReader::GetParseErrorCode
*/
enum ParseErrorCode {
kParseErrorNone = 0, //!< No error.
kParseErrorNone = 0, //!< No error.
kParseErrorDocumentEmpty, //!< The document is empty.
kParseErrorDocumentRootNotSingular, //!< The document root must not follow by other values.
kParseErrorDocumentEmpty, //!< The document is empty.
kParseErrorDocumentRootNotSingular, //!< The document root must not follow by
//!< other values.
kParseErrorValueInvalid, //!< Invalid value.
kParseErrorValueInvalid, //!< Invalid value.
kParseErrorObjectMissName, //!< Missing a name for object member.
kParseErrorObjectMissColon, //!< Missing a colon after a name of object member.
kParseErrorObjectMissCommaOrCurlyBracket, //!< Missing a comma or '}' after an object member.
kParseErrorObjectMissName, //!< Missing a name for object member.
kParseErrorObjectMissColon, //!< Missing a colon after a name of object
//!< member.
kParseErrorObjectMissCommaOrCurlyBracket, //!< Missing a comma or '}' after an
//!< object member.
kParseErrorArrayMissCommaOrSquareBracket, //!< Missing a comma or ']' after an array element.
kParseErrorArrayMissCommaOrSquareBracket, //!< Missing a comma or ']' after an
//!< array element.
kParseErrorStringUnicodeEscapeInvalidHex, //!< Incorrect hex digit after \\u escape in string.
kParseErrorStringUnicodeSurrogateInvalid, //!< The surrogate pair in string is invalid.
kParseErrorStringEscapeInvalid, //!< Invalid escape character in string.
kParseErrorStringMissQuotationMark, //!< Missing a closing quotation mark in string.
kParseErrorStringInvalidEncoding, //!< Invalid encoding in string.
kParseErrorStringUnicodeEscapeInvalidHex, //!< Incorrect hex digit after \\u
//!< escape in string.
kParseErrorStringUnicodeSurrogateInvalid, //!< The surrogate pair in string is
//!< invalid.
kParseErrorStringEscapeInvalid, //!< Invalid escape character in string.
kParseErrorStringMissQuotationMark, //!< Missing a closing quotation mark in
//!< string.
kParseErrorStringInvalidEncoding, //!< Invalid encoding in string.
kParseErrorNumberTooBig, //!< Number too big to be stored in double.
kParseErrorNumberMissFraction, //!< Miss fraction part in number.
kParseErrorNumberMissExponent, //!< Miss exponent in number.
kParseErrorNumberTooBig, //!< Number too big to be stored in double.
kParseErrorNumberMissFraction, //!< Miss fraction part in number.
kParseErrorNumberMissExponent, //!< Miss exponent in number.
kParseErrorTermination, //!< Parsing was terminated.
kParseErrorUnspecificSyntaxError //!< Unspecific syntax error.
kParseErrorTermination, //!< Parsing was terminated.
kParseErrorUnspecificSyntaxError //!< Unspecific syntax error.
};
//! Result of parsing (wraps ParseErrorCode)
@@ -104,40 +115,51 @@ enum ParseErrorCode {
\see GenericReader::Parse, GenericDocument::Parse
*/
struct ParseResult {
//!! Unspecified boolean type
typedef bool (ParseResult::*BooleanType)() const;
//!! 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) {}
//! Default constructor, no error.
ParseResult() : code_(kParseErrorNone), offset_(0) {}
//! Constructor to set an error.
ParseResult(ParseErrorCode code, size_t offset)
: code_(code), offset_(offset) {}
//! Get the error code.
ParseErrorCode Code() const { return code_; }
//! Get the error offset, if \ref IsError(), 0 otherwise.
size_t Offset() const { return offset_; }
//! Get the error code.
ParseErrorCode Code() const { return code_; }
//! Get the error offset, if \ref IsError(), 0 otherwise.
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; }
//! Whether the result is an error.
bool IsError() const { return code_ != kParseErrorNone; }
//! Explicit conversion to \c bool, returns \c true, iff !\ref IsError().
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_; }
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_;
}
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; }
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;
}
//! Reset error code.
void Clear() { Set(kParseErrorNone); }
//! Update error code and offset.
void Set(ParseErrorCode code, size_t offset = 0) { code_ = code; offset_ = offset; }
//! Reset error code.
void Clear() { Set(kParseErrorNone); }
//! Update error code and offset.
void Set(ParseErrorCode code, size_t offset = 0) {
code_ = code;
offset_ = offset;
}
private:
ParseErrorCode code_;
size_t offset_;
ParseErrorCode code_;
size_t offset_;
};
//! Function pointer type of GetParseError().
@@ -147,10 +169,10 @@ 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);
typedef const RAPIDJSON_ERROR_CHARTYPE *(*GetParseErrorFunc)(ParseErrorCode);
RAPIDJSON_NAMESPACE_END

132
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.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// 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
// 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
//
// 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_
@@ -21,8 +25,8 @@
#ifdef __clang__
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(padded)
RAPIDJSON_DIAG_OFF(unreachable-code)
RAPIDJSON_DIAG_OFF(missing-noreturn)
RAPIDJSON_DIAG_OFF(unreachable - code)
RAPIDJSON_DIAG_OFF(missing - noreturn)
#endif
RAPIDJSON_NAMESPACE_BEGIN
@@ -33,61 +37,75 @@ RAPIDJSON_NAMESPACE_BEGIN
*/
class FileReadStream {
public:
typedef char Ch; //!< Character type (byte).
typedef char Ch; //!< Character type (byte).
//! Constructor.
/*!
\param fp File pointer opened for read.
\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) {
RAPIDJSON_ASSERT(fp_ != 0);
RAPIDJSON_ASSERT(bufferSize >= 4);
Read();
}
//! Constructor.
/*!
\param fp File pointer opened for read.
\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) {
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 Peek() const { return *current_; }
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; }
// 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;
}
// For encoding detection only.
const Ch* Peek4() const {
return (current_ + 4 - !eof_ <= bufferLast_) ? current_ : 0;
}
// For encoding detection only.
const Ch *Peek4() const {
return (current_ + 4 - !eof_ <= bufferLast_) ? current_ : 0;
}
private:
void Read() {
if (current_ < bufferLast_)
++current_;
else if (!eof_) {
count_ += readCount_;
readCount_ = std::fread(buffer_, 1, bufferSize_, fp_);
bufferLast_ = buffer_ + readCount_ - 1;
current_ = buffer_;
void Read() {
if (current_ < bufferLast_)
++current_;
else if (!eof_) {
count_ += readCount_;
readCount_ = std::fread(buffer_, 1, bufferSize_, fp_);
bufferLast_ = buffer_ + readCount_ - 1;
current_ = buffer_;
if (readCount_ < bufferSize_) {
buffer_[readCount_] = '\0';
++bufferLast_;
eof_ = true;
}
}
if (readCount_ < bufferSize_) {
buffer_[readCount_] = '\0';
++bufferLast_;
eof_ = true;
}
}
}
std::FILE* fp_;
Ch *buffer_;
size_t bufferSize_;
Ch *bufferLast_;
Ch *current_;
size_t readCount_;
size_t count_; //!< Number of characters read
bool eof_;
std::FILE *fp_;
Ch *buffer_;
size_t bufferSize_;
Ch *bufferLast_;
Ch *current_;
size_t readCount_;
size_t count_; //!< Number of characters read
bool eof_;
};
RAPIDJSON_NAMESPACE_END

142
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.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// 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
// 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
//
// 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_
@@ -20,7 +24,7 @@
#ifdef __clang__
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(unreachable-code)
RAPIDJSON_DIAG_OFF(unreachable - code)
#endif
RAPIDJSON_NAMESPACE_BEGIN
@@ -31,68 +35,86 @@ RAPIDJSON_NAMESPACE_BEGIN
*/
class FileWriteStream {
public:
typedef char Ch; //!< Character type. Only support char.
typedef char Ch; //!< Character type. Only support char.
FileWriteStream(std::FILE* fp, char* buffer, size_t bufferSize) : fp_(fp), buffer_(buffer), bufferEnd_(buffer + bufferSize), current_(buffer_) {
RAPIDJSON_ASSERT(fp_ != 0);
FileWriteStream(std::FILE *fp, char *buffer, size_t bufferSize)
: fp_(fp), buffer_(buffer), bufferEnd_(buffer + bufferSize),
current_(buffer_) {
RAPIDJSON_ASSERT(fp_ != 0);
}
void Put(char c) {
if (current_ >= bufferEnd_)
Flush();
*current_++ = c;
}
void PutN(char c, size_t n) {
size_t avail = static_cast<size_t>(bufferEnd_ - current_);
while (n > avail) {
std::memset(current_, c, avail);
current_ += avail;
Flush();
n -= avail;
avail = static_cast<size_t>(bufferEnd_ - current_);
}
void Put(char c) {
if (current_ >= bufferEnd_)
Flush();
*current_++ = c;
if (n > 0) {
std::memset(current_, c, n);
current_ += n;
}
}
void PutN(char c, size_t n) {
size_t avail = static_cast<size_t>(bufferEnd_ - current_);
while (n > avail) {
std::memset(current_, c, avail);
current_ += avail;
Flush();
n -= avail;
avail = static_cast<size_t>(bufferEnd_ - current_);
}
if (n > 0) {
std::memset(current_, c, n);
current_ += n;
}
void Flush() {
if (current_ != buffer_) {
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
}
current_ = buffer_;
}
}
void Flush() {
if (current_ != buffer_) {
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
}
current_ = buffer_;
}
}
// 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; }
// 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;
}
private:
// Prohibit copy constructor & assignment operator.
FileWriteStream(const FileWriteStream&);
FileWriteStream& operator=(const FileWriteStream&);
// Prohibit copy constructor & assignment operator.
FileWriteStream(const FileWriteStream &);
FileWriteStream &operator=(const FileWriteStream &);
std::FILE* fp_;
char *buffer_;
char *bufferEnd_;
char *current_;
std::FILE *fp_;
char *buffer_;
char *bufferEnd_;
char *current_;
};
//! 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);
//! 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);
}
RAPIDJSON_NAMESPACE_END

108
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.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// 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
// 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
//
// 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_
@@ -21,42 +25,37 @@ RAPIDJSON_NAMESPACE_BEGIN
// encodings.h
template<typename CharType> struct UTF8;
template<typename CharType> struct UTF16;
template<typename CharType> struct UTF16BE;
template<typename CharType> struct UTF16LE;
template<typename CharType> struct UTF32;
template<typename CharType> struct UTF32BE;
template<typename CharType> struct UTF32LE;
template<typename CharType> struct ASCII;
template<typename CharType> struct AutoUTF;
template <typename CharType> struct UTF8;
template <typename CharType> struct UTF16;
template <typename CharType> struct UTF16BE;
template <typename CharType> struct UTF16LE;
template <typename CharType> struct UTF32;
template <typename CharType> struct UTF32BE;
template <typename CharType> struct UTF32LE;
template <typename CharType> struct ASCII;
template <typename CharType> struct AutoUTF;
template<typename SourceEncoding, typename TargetEncoding>
struct Transcoder;
template <typename SourceEncoding, typename TargetEncoding> struct Transcoder;
// allocators.h
class CrtAllocator;
template <typename BaseAllocator>
class MemoryPoolAllocator;
template <typename BaseAllocator> class MemoryPoolAllocator;
// stream.h
template <typename Encoding>
struct GenericStringStream;
template <typename Encoding> struct GenericStringStream;
typedef GenericStringStream<UTF8<char> > StringStream;
typedef GenericStringStream<UTF8<char>> StringStream;
template <typename Encoding>
struct GenericInsituStringStream;
template <typename Encoding> struct GenericInsituStringStream;
typedef GenericInsituStringStream<UTF8<char> > InsituStringStream;
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;
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,
typename StateAllocator>
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

478
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.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// 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
// 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
//
// 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_
@@ -27,261 +31,283 @@ namespace internal {
class BigInteger {
public:
typedef uint64_t Type;
typedef uint64_t Type;
BigInteger(const BigInteger& rhs) : count_(rhs.count_) {
std::memcpy(digits_, rhs.digits_, count_ * sizeof(Type));
BigInteger(const BigInteger &rhs) : count_(rhs.count_) {
std::memcpy(digits_, rhs.digits_, count_ * sizeof(Type));
}
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
while (length >= kMaxDigitPerIteration) {
AppendDecimal64(decimals + i, decimals + i + kMaxDigitPerIteration);
length -= kMaxDigitPerIteration;
i += kMaxDigitPerIteration;
}
explicit BigInteger(uint64_t u) : count_(1) {
digits_[0] = u;
if (length > 0)
AppendDecimal64(decimals + i, decimals + i + length);
}
BigInteger &operator=(const BigInteger &rhs) {
if (this != &rhs) {
count_ = rhs.count_;
std::memcpy(digits_, rhs.digits_, count_ * sizeof(Type));
}
return *this;
}
BigInteger &operator=(uint64_t u) {
digits_[0] = u;
count_ = 1;
return *this;
}
BigInteger &operator+=(uint64_t u) {
Type backup = digits_[0];
digits_[0] += u;
for (size_t i = 0; i < count_ - 1; i++) {
if (digits_[i] >= backup)
return *this; // no carry
backup = digits_[i + 1];
digits_[i + 1] += 1;
}
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
while (length >= kMaxDigitPerIteration) {
AppendDecimal64(decimals + i, decimals + i + kMaxDigitPerIteration);
length -= kMaxDigitPerIteration;
i += kMaxDigitPerIteration;
}
// Last carry
if (digits_[count_ - 1] < backup)
PushBack(1);
if (length > 0)
AppendDecimal64(decimals + i, decimals + i + length);
}
BigInteger& operator=(const BigInteger &rhs)
{
if (this != &rhs) {
count_ = rhs.count_;
std::memcpy(digits_, rhs.digits_, count_ * sizeof(Type));
}
return *this;
}
BigInteger& operator=(uint64_t u) {
digits_[0] = u;
count_ = 1;
return *this;
return *this;
}
BigInteger &operator*=(uint64_t 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++) {
uint64_t hi;
digits_[i] = MulAdd64(digits_[i], u, k, &hi);
k = hi;
}
BigInteger& operator+=(uint64_t u) {
Type backup = digits_[0];
digits_[0] += u;
for (size_t i = 0; i < count_ - 1; i++) {
if (digits_[i] >= backup)
return *this; // no carry
backup = digits_[i + 1];
digits_[i + 1] += 1;
}
if (k > 0)
PushBack(k);
// Last carry
if (digits_[count_ - 1] < backup)
PushBack(1);
return *this;
}
return *this;
BigInteger &operator*=(uint32_t 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++) {
const uint64_t c = digits_[i] >> 32;
const uint64_t d = digits_[i] & 0xFFFFFFFF;
const uint64_t uc = u * c;
const uint64_t ud = u * d;
const uint64_t p0 = ud + k;
const uint64_t p1 = uc + (p0 >> 32);
digits_[i] = (p0 & 0xFFFFFFFF) | (p1 << 32);
k = p1 >> 32;
}
BigInteger& operator*=(uint64_t u) {
if (u == 0) return *this = 0;
if (u == 1) return *this;
if (*this == 1) return *this = u;
if (k > 0)
PushBack(k);
uint64_t k = 0;
for (size_t i = 0; i < count_; i++) {
uint64_t hi;
digits_[i] = MulAdd64(digits_[i], u, k, &hi);
k = hi;
}
if (k > 0)
PushBack(k);
return *this;
}
return *this;
BigInteger &operator<<=(size_t shift) {
if (IsZero() || shift == 0)
return *this;
size_t offset = shift / kTypeBit;
size_t interShift = shift % kTypeBit;
RAPIDJSON_ASSERT(count_ + offset <= kCapacity);
if (interShift == 0) {
std::memmove(digits_ + offset, digits_, count_ * sizeof(Type));
count_ += offset;
} else {
digits_[count_] = 0;
for (size_t i = count_; i > 0; i--)
digits_[i + offset] = (digits_[i] << interShift) |
(digits_[i - 1] >> (kTypeBit - interShift));
digits_[offset] = digits_[0] << interShift;
count_ += offset;
if (digits_[count_])
count_++;
}
BigInteger& operator*=(uint32_t u) {
if (u == 0) return *this = 0;
if (u == 1) return *this;
if (*this == 1) return *this = u;
std::memset(digits_, 0, offset * sizeof(Type));
uint64_t k = 0;
for (size_t i = 0; i < count_; i++) {
const uint64_t c = digits_[i] >> 32;
const uint64_t d = digits_[i] & 0xFFFFFFFF;
const uint64_t uc = u * c;
const uint64_t ud = u * d;
const uint64_t p0 = ud + k;
const uint64_t p1 = uc + (p0 >> 32);
digits_[i] = (p0 & 0xFFFFFFFF) | (p1 << 32);
k = p1 >> 32;
}
if (k > 0)
PushBack(k);
return *this;
}
return *this;
bool operator==(const BigInteger &rhs) const {
return count_ == rhs.count_ &&
std::memcmp(digits_, rhs.digits_, count_ * sizeof(Type)) == 0;
}
bool operator==(const Type rhs) const {
return count_ == 1 && digits_[0] == rhs;
}
BigInteger &MultiplyPow5(unsigned exp) {
static const uint32_t kPow5[12] = {
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 * 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];
return *this;
}
// Compute absolute difference of this and rhs.
// Assume this != rhs
bool Difference(const BigInteger &rhs, BigInteger *out) const {
int cmp = Compare(rhs);
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;
}
BigInteger& operator<<=(size_t shift) {
if (IsZero() || shift == 0) return *this;
size_t offset = shift / kTypeBit;
size_t interShift = shift % kTypeBit;
RAPIDJSON_ASSERT(count_ + offset <= kCapacity);
if (interShift == 0) {
std::memmove(digits_ + offset, digits_, count_ * sizeof(Type));
count_ += offset;
}
else {
digits_[count_] = 0;
for (size_t i = count_; i > 0; i--)
digits_[i + offset] = (digits_[i] << interShift) | (digits_[i - 1] >> (kTypeBit - interShift));
digits_[offset] = digits_[0] << interShift;
count_ += offset;
if (digits_[count_])
count_++;
}
std::memset(digits_, 0, offset * sizeof(Type));
return *this;
Type borrow = 0;
for (size_t i = 0; i < a->count_; i++) {
Type d = a->digits_[i] - borrow;
if (i < b->count_)
d -= b->digits_[i];
borrow = (d > a->digits_[i]) ? 1 : 0;
out->digits_[i] = d;
if (d != 0)
out->count_ = i + 1;
}
bool operator==(const BigInteger& rhs) const {
return count_ == rhs.count_ && std::memcmp(digits_, rhs.digits_, count_ * sizeof(Type)) == 0;
}
return ret;
}
bool operator==(const Type rhs) const {
return count_ == 1 && digits_[0] == rhs;
}
int Compare(const BigInteger &rhs) const {
if (count_ != rhs.count_)
return count_ < rhs.count_ ? -1 : 1;
BigInteger& MultiplyPow5(unsigned exp) {
static const uint32_t kPow5[12] = {
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 * 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];
return *this;
}
for (size_t i = count_; i-- > 0;)
if (digits_[i] != rhs.digits_[i])
return digits_[i] < rhs.digits_[i] ? -1 : 1;
// Compute absolute difference of this and rhs.
// Assume this != rhs
bool Difference(const BigInteger& rhs, BigInteger* out) const {
int cmp = Compare(rhs);
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; }
return 0;
}
Type borrow = 0;
for (size_t i = 0; i < a->count_; i++) {
Type d = a->digits_[i] - borrow;
if (i < b->count_)
d -= b->digits_[i];
borrow = (d > a->digits_[i]) ? 1 : 0;
out->digits_[i] = d;
if (d != 0)
out->count_ = i + 1;
}
return ret;
}
int Compare(const BigInteger& rhs) const {
if (count_ != rhs.count_)
return count_ < rhs.count_ ? -1 : 1;
for (size_t i = count_; i-- > 0;)
if (digits_[i] != rhs.digits_[i])
return digits_[i] < rhs.digits_[i] ? -1 : 1;
return 0;
}
size_t GetCount() const { return count_; }
Type GetDigit(size_t index) const { RAPIDJSON_ASSERT(index < count_); return digits_[index]; }
bool IsZero() const { return count_ == 1 && digits_[0] == 0; }
size_t GetCount() const { return count_; }
Type GetDigit(size_t index) const {
RAPIDJSON_ASSERT(index < count_);
return digits_[index];
}
bool IsZero() const { return count_ == 1 && digits_[0] == 0; }
private:
void AppendDecimal64(const char* begin, const char* end) {
uint64_t u = ParseUint64(begin, end);
if (IsZero())
*this = u;
else {
unsigned exp = static_cast<unsigned>(end - begin);
(MultiplyPow5(exp) <<= exp) += u; // *this = *this * 10^exp + u
}
void AppendDecimal64(const char *begin, const char *end) {
uint64_t u = ParseUint64(begin, end);
if (IsZero())
*this = u;
else {
unsigned exp = static_cast<unsigned>(end - begin);
(MultiplyPow5(exp) <<= exp) += u; // *this = *this * 10^exp + u
}
}
void PushBack(Type digit) {
RAPIDJSON_ASSERT(count_ < kCapacity);
digits_[count_++] = digit;
void PushBack(Type digit) {
RAPIDJSON_ASSERT(count_ < kCapacity);
digits_[count_++] = digit;
}
static uint64_t ParseUint64(const char *begin, const char *end) {
uint64_t r = 0;
for (const char *p = begin; p != end; ++p) {
RAPIDJSON_ASSERT(*p >= '0' && *p <= '9');
r = r * 10u + static_cast<unsigned>(*p - '0');
}
return r;
}
static uint64_t ParseUint64(const char* begin, const char* end) {
uint64_t r = 0;
for (const char* p = begin; p != end; ++p) {
RAPIDJSON_ASSERT(*p >= '0' && *p <= '9');
r = r * 10u + static_cast<unsigned>(*p - '0');
}
return r;
}
// Assume a * b + k < 2^128
static uint64_t MulAdd64(uint64_t a, uint64_t b, uint64_t k, uint64_t* outHigh) {
// Assume a * b + k < 2^128
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__)
__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);
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__)
__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;
uint64_t x0 = a0 * b0, x1 = a0 * b1, x2 = a1 * b0, x3 = a1 * b1;
x1 += (x0 >> 32); // can't give carry
x1 += x2;
if (x1 < x2)
x3 += (static_cast<uint64_t>(1) << 32);
uint64_t lo = (x1 << 32) + (x0 & 0xFFFFFFFF);
uint64_t hi = x3 + (x1 >> 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;
if (x1 < x2)
x3 += (static_cast<uint64_t>(1) << 32);
uint64_t lo = (x1 << 32) + (x0 & 0xFFFFFFFF);
uint64_t hi = x3 + (x1 >> 32);
lo += k;
if (lo < k)
hi++;
*outHigh = hi;
return lo;
lo += k;
if (lo < k)
hi++;
*outHigh = hi;
return lo;
#endif
}
}
static const size_t kBitCount = 3328; // 64bit * 54 > 10^1000
static const size_t kCapacity = kBitCount / sizeof(Type);
static const size_t kTypeBit = sizeof(Type) * 8;
static const size_t kBitCount = 3328; // 64bit * 54 > 10^1000
static const size_t kCapacity = kBitCount / sizeof(Type);
static const size_t kTypeBit = sizeof(Type) * 8;
Type digits_[kCapacity];
size_t count_;
Type digits_[kCapacity];
size_t count_;
};
} // namespace internal

60
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,42 +33,44 @@
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
inline uint32_t clzll(uint64_t x) {
// Passing 0 to __builtin_clzll is UB in GCC and results in an
// infinite loop in the software implementation.
RAPIDJSON_ASSERT(x != 0);
// Passing 0 to __builtin_clzll is UB in GCC and results in an
// infinite loop in the software implementation.
RAPIDJSON_ASSERT(x != 0);
#if defined(_MSC_VER)
unsigned long r = 0;
unsigned long r = 0;
#if defined(_WIN64)
_BitScanReverse64(&r, x);
_BitScanReverse64(&r, x);
#else
// Scan the high 32 bits.
if (_BitScanReverse(&r, static_cast<uint32_t>(x >> 32)))
return 63 - (r + 32);
// Scan the high 32 bits.
if (_BitScanReverse(&r, static_cast<uint32_t>(x >> 32)))
return 63 - (r + 32);
// Scan the low 32 bits.
_BitScanReverse(&r, static_cast<uint32_t>(x & 0xFFFFFFFF));
// Scan the low 32 bits.
_BitScanReverse(&r, static_cast<uint32_t>(x & 0xFFFFFFFF));
#endif // _WIN64
return 63 - r;
return 63 - r;
#else
uint32_t r;
while (!(x & (static_cast<uint64_t>(1) << 63))) {
x <<= 1;
++r;
}
uint32_t r;
while (!(x & (static_cast<uint64_t>(1) << 63))) {
x <<= 1;
++r;
}
return r;
return r;
#endif // _MSC_VER
}
#define RAPIDJSON_CLZLL RAPIDJSON_NAMESPACE::internal::clzll
#endif // (defined(__GNUC__) && __GNUC__ >= 4) || RAPIDJSON_HAS_BUILTIN(__builtin_clzll)
#endif // (defined(__GNUC__) && __GNUC__ >= 4) ||
// RAPIDJSON_HAS_BUILTIN(__builtin_clzll)
} // namespace internal
RAPIDJSON_NAMESPACE_END

416
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_
@@ -42,204 +46,250 @@ RAPIDJSON_DIAG_OFF(padded)
#endif
struct DiyFp {
DiyFp() : f(), e() {}
DiyFp() : f(), e() {}
DiyFp(uint64_t fp, int exp) : f(fp), e(exp) {}
DiyFp(uint64_t fp, int exp) : f(fp), e(exp) {}
explicit DiyFp(double d) {
union {
double d;
uint64_t u64;
} u = { d };
explicit DiyFp(double d) {
union {
double d;
uint64_t u64;
} u = {d};
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 {
f = significand;
e = kDpMinExponent + 1;
}
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 {
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 {
DiyFp operator*(const DiyFp &rhs) const {
#if defined(_MSC_VER) && defined(_M_AMD64)
uint64_t h;
uint64_t l = _umul128(f, rhs.f, &h);
if (l & (uint64_t(1) << 63)) // rounding
h++;
return DiyFp(h, e + rhs.e + 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);
uint64_t l = static_cast<uint64_t>(p);
if (l & (uint64_t(1) << 63)) // rounding
h++;
return DiyFp(h, e + rhs.e + 64);
uint64_t h;
uint64_t l = _umul128(f, rhs.f, &h);
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__)
__extension__ typedef unsigned __int128 uint128;
uint128 p = static_cast<uint128>(f) * static_cast<uint128>(rhs.f);
uint64_t h = static_cast<uint64_t>(p >> 64);
uint64_t l = static_cast<uint64_t>(p);
if (l & (uint64_t(1) << 63)) // rounding
h++;
return DiyFp(h, e + rhs.e + 64);
#else
const uint64_t M32 = 0xFFFFFFFF;
const uint64_t a = f >> 32;
const uint64_t b = f & M32;
const uint64_t c = rhs.f >> 32;
const uint64_t d = rhs.f & M32;
const uint64_t ac = a * c;
const uint64_t bc = b * c;
const uint64_t ad = a * d;
const uint64_t bd = b * d;
uint64_t tmp = (bd >> 32) + (ad & M32) + (bc & M32);
tmp += 1U << 31; /// mult_round
return DiyFp(ac + (ad >> 32) + (bc >> 32) + (tmp >> 32), e + rhs.e + 64);
const uint64_t M32 = 0xFFFFFFFF;
const uint64_t a = f >> 32;
const uint64_t b = f & M32;
const uint64_t c = rhs.f >> 32;
const uint64_t d = rhs.f & M32;
const uint64_t ac = a * c;
const uint64_t bc = b * c;
const uint64_t ad = a * d;
const uint64_t bd = b * d;
uint64_t tmp = (bd >> 32) + (ad & M32) + (bc & M32);
tmp += 1U << 31; /// mult_round
return DiyFp(ac + (ad >> 32) + (bc >> 32) + (tmp >> 32), e + rhs.e + 64);
#endif
}
DiyFp Normalize() const {
int s = static_cast<int>(RAPIDJSON_CLZLL(f));
return DiyFp(f << s, e - s);
}
DiyFp NormalizeBoundary() const {
DiyFp res = *this;
while (!(res.f & (kDpHiddenBit << 1))) {
res.f <<= 1;
res.e--;
}
res.f <<= (kDiySignificandSize - kDpSignificandSize - 2);
res.e = res.e - (kDiySignificandSize - kDpSignificandSize - 2);
return res;
}
DiyFp Normalize() const {
int s = static_cast<int>(RAPIDJSON_CLZLL(f));
return DiyFp(f << s, e - s);
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);
mi.f <<= mi.e - pl.e;
mi.e = pl.e;
*plus = pl;
*minus = mi;
}
double ToDouble() const {
union {
double d;
uint64_t u64;
} u;
RAPIDJSON_ASSERT(f <= kDpHiddenBit + kDpSignificandMask);
if (e < kDpDenormalExponent) {
// Underflow.
return 0.0;
}
DiyFp NormalizeBoundary() const {
DiyFp res = *this;
while (!(res.f & (kDpHiddenBit << 1))) {
res.f <<= 1;
res.e--;
}
res.f <<= (kDiySignificandSize - kDpSignificandSize - 2);
res.e = res.e - (kDiySignificandSize - kDpSignificandSize - 2);
return res;
if (e >= kDpMaxExponent) {
// Overflow.
return std::numeric_limits<double>::infinity();
}
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;
}
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);
mi.f <<= mi.e - pl.e;
mi.e = pl.e;
*plus = pl;
*minus = mi;
}
static const int kDiySignificandSize = 64;
static const int kDpSignificandSize = 52;
static const int kDpExponentBias = 0x3FF + kDpSignificandSize;
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);
double ToDouble() const {
union {
double d;
uint64_t u64;
}u;
RAPIDJSON_ASSERT(f <= kDpHiddenBit + kDpSignificandMask);
if (e < kDpDenormalExponent) {
// Underflow.
return 0.0;
}
if (e >= kDpMaxExponent) {
// Overflow.
return std::numeric_limits<double>::infinity();
}
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;
}
static const int kDiySignificandSize = 64;
static const int kDpSignificandSize = 52;
static const int kDpExponentBias = 0x3FF + kDpSignificandSize;
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);
uint64_t f;
int e;
uint64_t f;
int e;
};
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)
};
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
};
RAPIDJSON_ASSERT(index < 87);
return DiyFp(kCachedPowers_F[index], kCachedPowers_E[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)};
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};
RAPIDJSON_ASSERT(index < 87);
return DiyFp(kCachedPowers_F[index], kCachedPowers_E[index]);
}
inline DiyFp GetCachedPower(int e, int* K) {
inline DiyFp GetCachedPower(int e, int *K) {
//int k = static_cast<int>(ceil((-61 - e) * 0.30102999566398114)) + 374;
double dk = (-61 - e) * 0.30102999566398114 + 347; // dk must be positive, so can do ceiling in positive
int k = static_cast<int>(dk);
if (dk - k > 0.0)
k++;
// int k = static_cast<int>(ceil((-61 - e) * 0.30102999566398114)) + 374;
double dk = (-61 - e) * 0.30102999566398114 +
347; // dk must be positive, so can do ceiling in positive
int k = static_cast<int>(dk);
if (dk - k > 0.0)
k++;
unsigned index = static_cast<unsigned>((k >> 3) + 1);
*K = -(-348 + static_cast<int>(index << 3)); // decimal exponent no need lookup table
unsigned index = static_cast<unsigned>((k >> 3) + 1);
*K = -(-348 +
static_cast<int>(index << 3)); // decimal exponent no need lookup table
return GetCachedPowerByIndex(index);
return GetCachedPowerByIndex(index);
}
inline DiyFp GetCachedPower10(int exp, int *outExp) {
RAPIDJSON_ASSERT(exp >= -348);
unsigned index = static_cast<unsigned>(exp + 348) / 8u;
*outExp = -348 + static_cast<int>(index) * 8;
return GetCachedPowerByIndex(index);
RAPIDJSON_ASSERT(exp >= -348);
unsigned index = static_cast<unsigned>(exp + 348) / 8u;
*outExp = -348 + static_cast<int>(index) * 8;
return GetCachedPowerByIndex(index);
}
#ifdef __GNUC__

425
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.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// 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
// 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
//
// 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,210 +33,245 @@ namespace internal {
#ifdef __GNUC__
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(effc++)
RAPIDJSON_DIAG_OFF(array-bounds) // some gcc versions generate wrong warnings https://gcc.gnu.org/bugzilla/show_bug.cgi?id=59124
RAPIDJSON_DIAG_OFF(array -
bounds) // some gcc versions generate wrong warnings
// https://gcc.gnu.org/bugzilla/show_bug.cgi?id=59124
#endif
inline void GrisuRound(char* buffer, int len, uint64_t delta, uint64_t rest, uint64_t ten_kappa, uint64_t wp_w) {
while (rest < wp_w && delta - rest >= ten_kappa &&
(rest + ten_kappa < wp_w || /// closer
wp_w - rest > rest + ten_kappa - wp_w)) {
buffer[len - 1]--;
rest += ten_kappa;
}
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)) {
buffer[len - 1]--;
rest += ten_kappa;
}
}
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;
// Will not reach 10 digits in DigitGen()
//if (n < 1000000000) return 9;
//return 10;
return 9;
// 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 };
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);
uint64_t p2 = Mp.f & (one.f - 1);
int kappa = CountDecimalDigit32(p1); // kappa in [0, 9]
*len = 0;
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);
uint64_t p2 = Mp.f & (one.f - 1);
int kappa = CountDecimalDigit32(p1); // kappa in [0, 9]
*len = 0;
while (kappa > 0) {
uint32_t d = 0;
switch (kappa) {
case 9: d = p1 / 100000000; p1 %= 100000000; break;
case 8: d = p1 / 10000000; p1 %= 10000000; break;
case 7: d = p1 / 1000000; p1 %= 1000000; break;
case 6: d = p1 / 100000; p1 %= 100000; break;
case 5: d = p1 / 10000; p1 %= 10000; break;
case 4: d = p1 / 1000; p1 %= 1000; break;
case 3: d = p1 / 100; p1 %= 100; break;
case 2: d = p1 / 10; p1 %= 10; break;
case 1: d = p1; p1 = 0; break;
default:;
}
if (d || *len)
buffer[(*len)++] = static_cast<char>('0' + static_cast<char>(d));
kappa--;
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);
return;
}
while (kappa > 0) {
uint32_t d = 0;
switch (kappa) {
case 9:
d = p1 / 100000000;
p1 %= 100000000;
break;
case 8:
d = p1 / 10000000;
p1 %= 10000000;
break;
case 7:
d = p1 / 1000000;
p1 %= 1000000;
break;
case 6:
d = p1 / 100000;
p1 %= 100000;
break;
case 5:
d = p1 / 10000;
p1 %= 10000;
break;
case 4:
d = p1 / 1000;
p1 %= 1000;
break;
case 3:
d = p1 / 100;
p1 %= 100;
break;
case 2:
d = p1 / 10;
p1 %= 10;
break;
case 1:
d = p1;
p1 = 0;
break;
default:;
}
if (d || *len)
buffer[(*len)++] = static_cast<char>('0' + static_cast<char>(d));
kappa--;
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);
return;
}
}
// kappa = 0
for (;;) {
p2 *= 10;
delta *= 10;
char d = static_cast<char>(p2 >> -one.e);
if (d || *len)
buffer[(*len)++] = static_cast<char>('0' + d);
p2 &= one.f - 1;
kappa--;
if (p2 < delta) {
*K += kappa;
int index = -kappa;
GrisuRound(buffer, *len, delta, p2, one.f, wp_w.f * (index < 9 ? kPow10[index] : 0));
return;
}
// kappa = 0
for (;;) {
p2 *= 10;
delta *= 10;
char d = static_cast<char>(p2 >> -one.e);
if (d || *len)
buffer[(*len)++] = static_cast<char>('0' + d);
p2 &= one.f - 1;
kappa--;
if (p2 < delta) {
*K += kappa;
int index = -kappa;
GrisuRound(buffer, *len, delta, p2, one.f,
wp_w.f * (index < 9 ? kPow10[index] : 0));
return;
}
}
}
inline void Grisu2(double value, char* buffer, int* length, int* K) {
const DiyFp v(value);
DiyFp w_m, w_p;
v.NormalizedBoundaries(&w_m, &w_p);
inline void Grisu2(double value, char *buffer, int *length, int *K) {
const DiyFp v(value);
DiyFp w_m, w_p;
v.NormalizedBoundaries(&w_m, &w_p);
const DiyFp c_mk = GetCachedPower(w_p.e, K);
const DiyFp W = v.Normalize() * c_mk;
DiyFp Wp = w_p * c_mk;
DiyFp Wm = w_m * c_mk;
Wm.f++;
Wp.f--;
DigitGen(W, Wp, Wp.f - Wm.f, buffer, length, K);
const DiyFp c_mk = GetCachedPower(w_p.e, K);
const DiyFp W = v.Normalize() * c_mk;
DiyFp Wp = w_p * c_mk;
DiyFp Wm = w_m * c_mk;
Wm.f++;
Wp.f--;
DigitGen(W, Wp, Wp.f - Wm.f, buffer, length, K);
}
inline char* WriteExponent(int K, char* buffer) {
if (K < 0) {
*buffer++ = '-';
K = -K;
}
inline char *WriteExponent(int K, char *buffer) {
if (K < 0) {
*buffer++ = '-';
K = -K;
}
if (K >= 100) {
*buffer++ = static_cast<char>('0' + static_cast<char>(K / 100));
K %= 100;
const char* d = GetDigitsLut() + K * 2;
*buffer++ = d[0];
*buffer++ = d[1];
}
else if (K >= 10) {
const char* d = GetDigitsLut() + K * 2;
*buffer++ = d[0];
*buffer++ = d[1];
}
else
*buffer++ = static_cast<char>('0' + static_cast<char>(K));
if (K >= 100) {
*buffer++ = static_cast<char>('0' + static_cast<char>(K / 100));
K %= 100;
const char *d = GetDigitsLut() + K * 2;
*buffer++ = d[0];
*buffer++ = d[1];
} else if (K >= 10) {
const char *d = GetDigitsLut() + K * 2;
*buffer++ = d[0];
*buffer++ = d[1];
} else
*buffer++ = static_cast<char>('0' + static_cast<char>(K));
return buffer;
return buffer;
}
inline char* Prettify(char* buffer, int length, int k, int maxDecimalPlaces) {
const int kk = length + k; // 10^(kk-1) <= v < 10^kk
inline char *Prettify(char *buffer, int length, int k, int maxDecimalPlaces) {
const int kk = length + k; // 10^(kk-1) <= v < 10^kk
if (0 <= k && kk <= 21) {
// 1234e7 -> 12340000000
for (int i = length; i < kk; i++)
buffer[i] = '0';
buffer[kk] = '.';
buffer[kk + 1] = '0';
return &buffer[kk + 2];
}
else if (0 < kk && kk <= 21) {
// 1234e-2 -> 12.34
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
// Remove extra trailing zeros (at least one) after truncation.
for (int i = kk + maxDecimalPlaces; i > kk + 1; i--)
if (buffer[i] != '0')
return &buffer[i + 1];
return &buffer[kk + 2]; // Reserve one zero
}
else
return &buffer[length + 1];
}
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));
buffer[0] = '0';
buffer[1] = '.';
for (int i = 2; i < offset; i++)
buffer[i] = '0';
if (length - kk > maxDecimalPlaces) {
// When maxDecimalPlaces = 2, 0.123 -> 0.12, 0.102 -> 0.1
// Remove extra trailing zeros (at least one) after truncation.
for (int i = maxDecimalPlaces + 1; i > 2; i--)
if (buffer[i] != '0')
return &buffer[i + 1];
return &buffer[3]; // Reserve one zero
}
else
return &buffer[length + offset];
}
else if (kk < -maxDecimalPlaces) {
// Truncate to zero
buffer[0] = '0';
buffer[1] = '.';
buffer[2] = '0';
return &buffer[3];
}
else if (length == 1) {
// 1e30
buffer[1] = 'e';
return WriteExponent(kk - 1, &buffer[2]);
}
else {
// 1234e30 -> 1.234e33
std::memmove(&buffer[2], &buffer[1], static_cast<size_t>(length - 1));
buffer[1] = '.';
buffer[length + 1] = 'e';
return WriteExponent(kk - 1, &buffer[0 + length + 2]);
}
if (0 <= k && kk <= 21) {
// 1234e7 -> 12340000000
for (int i = length; i < kk; i++)
buffer[i] = '0';
buffer[kk] = '.';
buffer[kk + 1] = '0';
return &buffer[kk + 2];
} else if (0 < kk && kk <= 21) {
// 1234e-2 -> 12.34
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
// Remove extra trailing zeros (at least one) after truncation.
for (int i = kk + maxDecimalPlaces; i > kk + 1; i--)
if (buffer[i] != '0')
return &buffer[i + 1];
return &buffer[kk + 2]; // Reserve one zero
} else
return &buffer[length + 1];
} 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));
buffer[0] = '0';
buffer[1] = '.';
for (int i = 2; i < offset; i++)
buffer[i] = '0';
if (length - kk > maxDecimalPlaces) {
// When maxDecimalPlaces = 2, 0.123 -> 0.12, 0.102 -> 0.1
// Remove extra trailing zeros (at least one) after truncation.
for (int i = maxDecimalPlaces + 1; i > 2; i--)
if (buffer[i] != '0')
return &buffer[i + 1];
return &buffer[3]; // Reserve one zero
} else
return &buffer[length + offset];
} else if (kk < -maxDecimalPlaces) {
// Truncate to zero
buffer[0] = '0';
buffer[1] = '.';
buffer[2] = '0';
return &buffer[3];
} else if (length == 1) {
// 1e30
buffer[1] = 'e';
return WriteExponent(kk - 1, &buffer[2]);
} else {
// 1234e30 -> 1.234e33
std::memmove(&buffer[2], &buffer[1], static_cast<size_t>(length - 1));
buffer[1] = '.';
buffer[length + 1] = 'e';
return WriteExponent(kk - 1, &buffer[0 + length + 2]);
}
}
inline char* dtoa(double value, char* buffer, int maxDecimalPlaces = 324) {
RAPIDJSON_ASSERT(maxDecimalPlaces >= 1);
Double d(value);
if (d.IsZero()) {
if (d.Sign())
*buffer++ = '-'; // -0.0, Issue #289
buffer[0] = '0';
buffer[1] = '.';
buffer[2] = '0';
return &buffer[3];
}
else {
if (value < 0) {
*buffer++ = '-';
value = -value;
}
int length, K;
Grisu2(value, buffer, &length, &K);
return Prettify(buffer, length, K, maxDecimalPlaces);
inline char *dtoa(double value, char *buffer, int maxDecimalPlaces = 324) {
RAPIDJSON_ASSERT(maxDecimalPlaces >= 1);
Double d(value);
if (d.IsZero()) {
if (d.Sign())
*buffer++ = '-'; // -0.0, Issue #289
buffer[0] = '0';
buffer[1] = '.';
buffer[2] = '0';
return &buffer[3];
} else {
if (value < 0) {
*buffer++ = '-';
value = -value;
}
int length, K;
Grisu2(value, buffer, &length, &K);
return Prettify(buffer, length, K, maxDecimalPlaces);
}
}
#ifdef __GNUC__

118
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.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// 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
// 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
//
// 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_
@@ -22,54 +26,72 @@ namespace internal {
class Double {
public:
Double() {}
Double(double d) : d_(d) {}
Double(uint64_t u) : u_(u) {}
Double() {}
Double(double d) : d_(d) {}
Double(uint64_t u) : u_(u) {}
double Value() const { return d_; }
uint64_t Uint64Value() const { return u_; }
double Value() const { return d_; }
uint64_t Uint64Value() const { return u_; }
double NextPositiveDouble() const {
RAPIDJSON_ASSERT(!Sign());
return Double(u_ + 1).Value();
}
double NextPositiveDouble() const {
RAPIDJSON_ASSERT(!Sign());
return Double(u_ + 1).Value();
}
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); }
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);
}
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 IsZero() const { return (u_ & (kExponentMask | kSignificandMask)) == 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 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)
return 53;
else if (order <= -1074)
return 0;
else
return order + 1074;
}
static int EffectiveSignificandSize(int order) {
if (order >= -1021)
return 53;
else if (order <= -1074)
return 0;
else
return order + 1074;
}
private:
static const int kSignificandSize = 52;
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 int kSignificandSize = 52;
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);
union {
double d_;
uint64_t u_;
};
union {
double d_;
uint64_t u_;
};
};
} // namespace internal

517
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_
@@ -20,286 +24,281 @@
RAPIDJSON_NAMESPACE_BEGIN
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'
};
return cDigitsLut;
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'};
return cDigitsLut;
}
inline char* u32toa(uint32_t value, char* buffer) {
RAPIDJSON_ASSERT(buffer != 0);
inline char *u32toa(uint32_t value, char *buffer) {
RAPIDJSON_ASSERT(buffer != 0);
const char* cDigitsLut = GetDigitsLut();
const char *cDigitsLut = GetDigitsLut();
if (value < 10000) {
const uint32_t d1 = (value / 100) << 1;
const uint32_t d2 = (value % 100) << 1;
if (value < 10000) {
const uint32_t d1 = (value / 100) << 1;
const uint32_t d2 = (value % 100) << 1;
if (value >= 1000)
*buffer++ = cDigitsLut[d1];
if (value >= 100)
*buffer++ = cDigitsLut[d1 + 1];
if (value >= 10)
*buffer++ = cDigitsLut[d2];
*buffer++ = cDigitsLut[d2 + 1];
}
else if (value < 100000000) {
// value = bbbbcccc
const uint32_t b = value / 10000;
const uint32_t c = value % 10000;
if (value >= 1000)
*buffer++ = cDigitsLut[d1];
if (value >= 100)
*buffer++ = cDigitsLut[d1 + 1];
if (value >= 10)
*buffer++ = cDigitsLut[d2];
*buffer++ = cDigitsLut[d2 + 1];
} else if (value < 100000000) {
// value = bbbbcccc
const uint32_t b = value / 10000;
const uint32_t c = value % 10000;
const uint32_t d1 = (b / 100) << 1;
const uint32_t d2 = (b % 100) << 1;
const uint32_t d1 = (b / 100) << 1;
const uint32_t d2 = (b % 100) << 1;
const uint32_t d3 = (c / 100) << 1;
const uint32_t d4 = (c % 100) << 1;
const uint32_t d3 = (c / 100) << 1;
const uint32_t d4 = (c % 100) << 1;
if (value >= 10000000)
*buffer++ = cDigitsLut[d1];
if (value >= 1000000)
*buffer++ = cDigitsLut[d1 + 1];
if (value >= 100000)
*buffer++ = cDigitsLut[d2];
*buffer++ = cDigitsLut[d2 + 1];
if (value >= 10000000)
*buffer++ = cDigitsLut[d1];
if (value >= 1000000)
*buffer++ = cDigitsLut[d1 + 1];
if (value >= 100000)
*buffer++ = cDigitsLut[d2];
*buffer++ = cDigitsLut[d2 + 1];
*buffer++ = cDigitsLut[d3];
*buffer++ = cDigitsLut[d3 + 1];
*buffer++ = cDigitsLut[d4];
*buffer++ = cDigitsLut[d4 + 1];
}
else {
// value = aabbbbcccc in decimal
*buffer++ = cDigitsLut[d3];
*buffer++ = cDigitsLut[d3 + 1];
*buffer++ = cDigitsLut[d4];
*buffer++ = cDigitsLut[d4 + 1];
} else {
// value = aabbbbcccc in decimal
const uint32_t a = value / 100000000; // 1 to 42
value %= 100000000;
const uint32_t a = value / 100000000; // 1 to 42
value %= 100000000;
if (a >= 10) {
const unsigned i = a << 1;
*buffer++ = cDigitsLut[i];
*buffer++ = cDigitsLut[i + 1];
}
else
*buffer++ = static_cast<char>('0' + static_cast<char>(a));
if (a >= 10) {
const unsigned i = a << 1;
*buffer++ = cDigitsLut[i];
*buffer++ = cDigitsLut[i + 1];
} else
*buffer++ = static_cast<char>('0' + static_cast<char>(a));
const uint32_t b = value / 10000; // 0 to 9999
const uint32_t c = value % 10000; // 0 to 9999
const uint32_t b = value / 10000; // 0 to 9999
const uint32_t c = value % 10000; // 0 to 9999
const uint32_t d1 = (b / 100) << 1;
const uint32_t d2 = (b % 100) << 1;
const uint32_t d1 = (b / 100) << 1;
const uint32_t d2 = (b % 100) << 1;
const uint32_t d3 = (c / 100) << 1;
const uint32_t d4 = (c % 100) << 1;
const uint32_t d3 = (c / 100) << 1;
const uint32_t d4 = (c % 100) << 1;
*buffer++ = cDigitsLut[d1];
*buffer++ = cDigitsLut[d1 + 1];
*buffer++ = cDigitsLut[d2];
*buffer++ = cDigitsLut[d2 + 1];
*buffer++ = cDigitsLut[d3];
*buffer++ = cDigitsLut[d3 + 1];
*buffer++ = cDigitsLut[d4];
*buffer++ = cDigitsLut[d4 + 1];
}
return buffer;
}
inline char *i32toa(int32_t value, char *buffer) {
RAPIDJSON_ASSERT(buffer != 0);
uint32_t u = static_cast<uint32_t>(value);
if (value < 0) {
*buffer++ = '-';
u = ~u + 1;
}
return u32toa(u, buffer);
}
inline char *u64toa(uint64_t value, char *buffer) {
RAPIDJSON_ASSERT(buffer != 0);
const char *cDigitsLut = GetDigitsLut();
const uint64_t kTen8 = 100000000;
const uint64_t kTen9 = kTen8 * 10;
const uint64_t kTen10 = kTen8 * 100;
const uint64_t kTen11 = kTen8 * 1000;
const uint64_t kTen12 = kTen8 * 10000;
const uint64_t kTen13 = kTen8 * 100000;
const uint64_t kTen14 = kTen8 * 1000000;
const uint64_t kTen15 = kTen8 * 10000000;
const uint64_t kTen16 = kTen8 * kTen8;
if (value < kTen8) {
uint32_t v = static_cast<uint32_t>(value);
if (v < 10000) {
const uint32_t d1 = (v / 100) << 1;
const uint32_t d2 = (v % 100) << 1;
if (v >= 1000)
*buffer++ = cDigitsLut[d1];
if (v >= 100)
*buffer++ = cDigitsLut[d1 + 1];
if (v >= 10)
*buffer++ = cDigitsLut[d2];
*buffer++ = cDigitsLut[d2 + 1];
*buffer++ = cDigitsLut[d3];
*buffer++ = cDigitsLut[d3 + 1];
*buffer++ = cDigitsLut[d4];
*buffer++ = cDigitsLut[d4 + 1];
}
return buffer;
}
*buffer++ = cDigitsLut[d2 + 1];
} else {
// value = bbbbcccc
const uint32_t b = v / 10000;
const uint32_t c = v % 10000;
inline char* i32toa(int32_t value, char* buffer) {
RAPIDJSON_ASSERT(buffer != 0);
uint32_t u = static_cast<uint32_t>(value);
if (value < 0) {
*buffer++ = '-';
u = ~u + 1;
}
const uint32_t d1 = (b / 100) << 1;
const uint32_t d2 = (b % 100) << 1;
return u32toa(u, buffer);
}
inline char* u64toa(uint64_t value, char* buffer) {
RAPIDJSON_ASSERT(buffer != 0);
const char* cDigitsLut = GetDigitsLut();
const uint64_t kTen8 = 100000000;
const uint64_t kTen9 = kTen8 * 10;
const uint64_t kTen10 = kTen8 * 100;
const uint64_t kTen11 = kTen8 * 1000;
const uint64_t kTen12 = kTen8 * 10000;
const uint64_t kTen13 = kTen8 * 100000;
const uint64_t kTen14 = kTen8 * 1000000;
const uint64_t kTen15 = kTen8 * 10000000;
const uint64_t kTen16 = kTen8 * kTen8;
if (value < kTen8) {
uint32_t v = static_cast<uint32_t>(value);
if (v < 10000) {
const uint32_t d1 = (v / 100) << 1;
const uint32_t d2 = (v % 100) << 1;
if (v >= 1000)
*buffer++ = cDigitsLut[d1];
if (v >= 100)
*buffer++ = cDigitsLut[d1 + 1];
if (v >= 10)
*buffer++ = cDigitsLut[d2];
*buffer++ = cDigitsLut[d2 + 1];
}
else {
// value = bbbbcccc
const uint32_t b = v / 10000;
const uint32_t c = v % 10000;
const uint32_t d1 = (b / 100) << 1;
const uint32_t d2 = (b % 100) << 1;
const uint32_t d3 = (c / 100) << 1;
const uint32_t d4 = (c % 100) << 1;
if (value >= 10000000)
*buffer++ = cDigitsLut[d1];
if (value >= 1000000)
*buffer++ = cDigitsLut[d1 + 1];
if (value >= 100000)
*buffer++ = cDigitsLut[d2];
*buffer++ = cDigitsLut[d2 + 1];
*buffer++ = cDigitsLut[d3];
*buffer++ = cDigitsLut[d3 + 1];
*buffer++ = cDigitsLut[d4];
*buffer++ = cDigitsLut[d4 + 1];
}
}
else if (value < kTen16) {
const uint32_t v0 = static_cast<uint32_t>(value / kTen8);
const uint32_t v1 = static_cast<uint32_t>(value % kTen8);
const uint32_t b0 = v0 / 10000;
const uint32_t c0 = v0 % 10000;
const uint32_t d1 = (b0 / 100) << 1;
const uint32_t d2 = (b0 % 100) << 1;
const uint32_t d3 = (c0 / 100) << 1;
const uint32_t d4 = (c0 % 100) << 1;
const uint32_t b1 = v1 / 10000;
const uint32_t c1 = v1 % 10000;
const uint32_t d5 = (b1 / 100) << 1;
const uint32_t d6 = (b1 % 100) << 1;
const uint32_t d7 = (c1 / 100) << 1;
const uint32_t d8 = (c1 % 100) << 1;
if (value >= kTen15)
*buffer++ = cDigitsLut[d1];
if (value >= kTen14)
*buffer++ = cDigitsLut[d1 + 1];
if (value >= kTen13)
*buffer++ = cDigitsLut[d2];
if (value >= kTen12)
*buffer++ = cDigitsLut[d2 + 1];
if (value >= kTen11)
*buffer++ = cDigitsLut[d3];
if (value >= kTen10)
*buffer++ = cDigitsLut[d3 + 1];
if (value >= kTen9)
*buffer++ = cDigitsLut[d4];
*buffer++ = cDigitsLut[d4 + 1];
*buffer++ = cDigitsLut[d5];
*buffer++ = cDigitsLut[d5 + 1];
*buffer++ = cDigitsLut[d6];
*buffer++ = cDigitsLut[d6 + 1];
*buffer++ = cDigitsLut[d7];
*buffer++ = cDigitsLut[d7 + 1];
*buffer++ = cDigitsLut[d8];
*buffer++ = cDigitsLut[d8 + 1];
}
else {
const uint32_t a = static_cast<uint32_t>(value / kTen16); // 1 to 1844
value %= kTen16;
if (a < 10)
*buffer++ = static_cast<char>('0' + static_cast<char>(a));
else if (a < 100) {
const uint32_t i = a << 1;
*buffer++ = cDigitsLut[i];
*buffer++ = cDigitsLut[i + 1];
}
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 {
const uint32_t i = (a / 100) << 1;
const uint32_t j = (a % 100) << 1;
*buffer++ = cDigitsLut[i];
*buffer++ = cDigitsLut[i + 1];
*buffer++ = cDigitsLut[j];
*buffer++ = cDigitsLut[j + 1];
}
const uint32_t v0 = static_cast<uint32_t>(value / kTen8);
const uint32_t v1 = static_cast<uint32_t>(value % kTen8);
const uint32_t b0 = v0 / 10000;
const uint32_t c0 = v0 % 10000;
const uint32_t d1 = (b0 / 100) << 1;
const uint32_t d2 = (b0 % 100) << 1;
const uint32_t d3 = (c0 / 100) << 1;
const uint32_t d4 = (c0 % 100) << 1;
const uint32_t b1 = v1 / 10000;
const uint32_t c1 = v1 % 10000;
const uint32_t d5 = (b1 / 100) << 1;
const uint32_t d6 = (b1 % 100) << 1;
const uint32_t d7 = (c1 / 100) << 1;
const uint32_t d8 = (c1 % 100) << 1;
const uint32_t d3 = (c / 100) << 1;
const uint32_t d4 = (c % 100) << 1;
if (value >= 10000000)
*buffer++ = cDigitsLut[d1];
if (value >= 1000000)
*buffer++ = cDigitsLut[d1 + 1];
if (value >= 100000)
*buffer++ = cDigitsLut[d2];
*buffer++ = cDigitsLut[d2 + 1];
*buffer++ = cDigitsLut[d3];
*buffer++ = cDigitsLut[d3 + 1];
*buffer++ = cDigitsLut[d4];
*buffer++ = cDigitsLut[d4 + 1];
*buffer++ = cDigitsLut[d5];
*buffer++ = cDigitsLut[d5 + 1];
*buffer++ = cDigitsLut[d6];
*buffer++ = cDigitsLut[d6 + 1];
*buffer++ = cDigitsLut[d7];
*buffer++ = cDigitsLut[d7 + 1];
*buffer++ = cDigitsLut[d8];
*buffer++ = cDigitsLut[d8 + 1];
*buffer++ = cDigitsLut[d2 + 1];
*buffer++ = cDigitsLut[d3];
*buffer++ = cDigitsLut[d3 + 1];
*buffer++ = cDigitsLut[d4];
*buffer++ = cDigitsLut[d4 + 1];
}
} else if (value < kTen16) {
const uint32_t v0 = static_cast<uint32_t>(value / kTen8);
const uint32_t v1 = static_cast<uint32_t>(value % kTen8);
const uint32_t b0 = v0 / 10000;
const uint32_t c0 = v0 % 10000;
const uint32_t d1 = (b0 / 100) << 1;
const uint32_t d2 = (b0 % 100) << 1;
const uint32_t d3 = (c0 / 100) << 1;
const uint32_t d4 = (c0 % 100) << 1;
const uint32_t b1 = v1 / 10000;
const uint32_t c1 = v1 % 10000;
const uint32_t d5 = (b1 / 100) << 1;
const uint32_t d6 = (b1 % 100) << 1;
const uint32_t d7 = (c1 / 100) << 1;
const uint32_t d8 = (c1 % 100) << 1;
if (value >= kTen15)
*buffer++ = cDigitsLut[d1];
if (value >= kTen14)
*buffer++ = cDigitsLut[d1 + 1];
if (value >= kTen13)
*buffer++ = cDigitsLut[d2];
if (value >= kTen12)
*buffer++ = cDigitsLut[d2 + 1];
if (value >= kTen11)
*buffer++ = cDigitsLut[d3];
if (value >= kTen10)
*buffer++ = cDigitsLut[d3 + 1];
if (value >= kTen9)
*buffer++ = cDigitsLut[d4];
*buffer++ = cDigitsLut[d4 + 1];
*buffer++ = cDigitsLut[d5];
*buffer++ = cDigitsLut[d5 + 1];
*buffer++ = cDigitsLut[d6];
*buffer++ = cDigitsLut[d6 + 1];
*buffer++ = cDigitsLut[d7];
*buffer++ = cDigitsLut[d7 + 1];
*buffer++ = cDigitsLut[d8];
*buffer++ = cDigitsLut[d8 + 1];
} else {
const uint32_t a = static_cast<uint32_t>(value / kTen16); // 1 to 1844
value %= kTen16;
if (a < 10)
*buffer++ = static_cast<char>('0' + static_cast<char>(a));
else if (a < 100) {
const uint32_t i = a << 1;
*buffer++ = cDigitsLut[i];
*buffer++ = cDigitsLut[i + 1];
} 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 {
const uint32_t i = (a / 100) << 1;
const uint32_t j = (a % 100) << 1;
*buffer++ = cDigitsLut[i];
*buffer++ = cDigitsLut[i + 1];
*buffer++ = cDigitsLut[j];
*buffer++ = cDigitsLut[j + 1];
}
return buffer;
const uint32_t v0 = static_cast<uint32_t>(value / kTen8);
const uint32_t v1 = static_cast<uint32_t>(value % kTen8);
const uint32_t b0 = v0 / 10000;
const uint32_t c0 = v0 % 10000;
const uint32_t d1 = (b0 / 100) << 1;
const uint32_t d2 = (b0 % 100) << 1;
const uint32_t d3 = (c0 / 100) << 1;
const uint32_t d4 = (c0 % 100) << 1;
const uint32_t b1 = v1 / 10000;
const uint32_t c1 = v1 % 10000;
const uint32_t d5 = (b1 / 100) << 1;
const uint32_t d6 = (b1 % 100) << 1;
const uint32_t d7 = (c1 / 100) << 1;
const uint32_t d8 = (c1 % 100) << 1;
*buffer++ = cDigitsLut[d1];
*buffer++ = cDigitsLut[d1 + 1];
*buffer++ = cDigitsLut[d2];
*buffer++ = cDigitsLut[d2 + 1];
*buffer++ = cDigitsLut[d3];
*buffer++ = cDigitsLut[d3 + 1];
*buffer++ = cDigitsLut[d4];
*buffer++ = cDigitsLut[d4 + 1];
*buffer++ = cDigitsLut[d5];
*buffer++ = cDigitsLut[d5 + 1];
*buffer++ = cDigitsLut[d6];
*buffer++ = cDigitsLut[d6 + 1];
*buffer++ = cDigitsLut[d7];
*buffer++ = cDigitsLut[d7 + 1];
*buffer++ = cDigitsLut[d8];
*buffer++ = cDigitsLut[d8 + 1];
}
return buffer;
}
inline char* i64toa(int64_t value, char* buffer) {
RAPIDJSON_ASSERT(buffer != 0);
uint64_t u = static_cast<uint64_t>(value);
if (value < 0) {
*buffer++ = '-';
u = ~u + 1;
}
inline char *i64toa(int64_t value, char *buffer) {
RAPIDJSON_ASSERT(buffer != 0);
uint64_t u = static_cast<uint64_t>(value);
if (value < 0) {
*buffer++ = '-';
u = ~u + 1;
}
return u64toa(u, buffer);
return u64toa(u, buffer);
}
} // namespace internal

152
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.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// 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
// 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
//
// 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,48 +39,57 @@ 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; };
///////////////////////////////////////////////////////////////////////////////
// BoolType, TrueType, FalseType
//
template <bool Cond> struct BoolType {
static const bool Value = Cond;
typedef BoolType Type;
static const bool Value = Cond;
typedef BoolType Type;
};
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,55 +101,60 @@ 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>,
BoolType<IsConst<CT>::Value >= IsConst<T>::Value> >::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 {};
template <typename T> struct IsPointer<T*> : TrueType {};
template <typename T> struct IsPointer<T *> : TrueType {};
///////////////////////////////////////////////////////////////////////////////
// IsBaseOf
//
#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
template<typename B, typename D> struct IsBaseOfImpl {
RAPIDJSON_STATIC_ASSERT(sizeof(B) != 0);
RAPIDJSON_STATIC_ASSERT(sizeof(D) != 0);
template <typename B, typename D> struct IsBaseOfImpl {
RAPIDJSON_STATIC_ASSERT(sizeof(B) != 0);
RAPIDJSON_STATIC_ASSERT(sizeof(D) != 0);
typedef char (&Yes)[1];
typedef char (&No) [2];
typedef char (&Yes)[1];
typedef char (&No)[2];
template <typename T>
static Yes Check(const D*, T);
static No Check(const B*, int);
template <typename T> static Yes Check(const D *, T);
static No Check(const B *, int);
struct Host {
operator const B*() const;
operator const D*();
};
struct Host {
operator const B *() const;
operator const D *();
};
enum { Value = (sizeof(Check(Host(), 0)) == sizeof(Yes)) };
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
#define RAPIDJSON_REMOVEFPTR_(type) \
typename ::RAPIDJSON_NAMESPACE::internal::RemoveSfinaeTag< \
::RAPIDJSON_NAMESPACE::internal::SfinaeTag &(*)type>::Type
#define RAPIDJSON_ENABLEIF(cond) \
typename ::RAPIDJSON_NAMESPACE::internal::EnableIf \
<RAPIDJSON_REMOVEFPTR_(cond)>::Type * = NULL
#define RAPIDJSON_ENABLEIF(cond) \
typename ::RAPIDJSON_NAMESPACE::internal::EnableIf<RAPIDJSON_REMOVEFPTR_( \
cond)>::Type * = NULL
#define RAPIDJSON_DISABLEIF(cond) \
typename ::RAPIDJSON_NAMESPACE::internal::DisableIf \
<RAPIDJSON_REMOVEFPTR_(cond)>::Type * = NULL
#define RAPIDJSON_DISABLEIF(cond) \
typename ::RAPIDJSON_NAMESPACE::internal::DisableIf<RAPIDJSON_REMOVEFPTR_( \
cond)>::Type * = NULL
#define RAPIDJSON_ENABLEIF_RETURN(cond,returntype) \
typename ::RAPIDJSON_NAMESPACE::internal::EnableIf \
<RAPIDJSON_REMOVEFPTR_(cond), \
RAPIDJSON_REMOVEFPTR_(returntype)>::Type
#define RAPIDJSON_ENABLEIF_RETURN(cond, returntype) \
typename ::RAPIDJSON_NAMESPACE::internal::EnableIf< \
RAPIDJSON_REMOVEFPTR_(cond), RAPIDJSON_REMOVEFPTR_(returntype)>::Type
#define RAPIDJSON_DISABLEIF_RETURN(cond,returntype) \
typename ::RAPIDJSON_NAMESPACE::internal::DisableIf \
<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
} // namespace internal
RAPIDJSON_NAMESPACE_END

82
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.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// 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
// 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
//
// 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,27 +30,45 @@ 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
};
RAPIDJSON_ASSERT(n >= 0 && n <= 308);
return e[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};
RAPIDJSON_ASSERT(n >= 0 && n <= 308);
return e[n];
}
} // namespace internal

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

File diff suppressed because it is too large Load Diff

320
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.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// 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
// 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
//
// 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_
@@ -21,7 +25,7 @@
#if defined(__clang__)
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(c++98-compat)
RAPIDJSON_DIAG_OFF(c++ 98 - compat)
#endif
RAPIDJSON_NAMESPACE_BEGIN
@@ -32,194 +36,182 @@ 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) {
}
// 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) {}
#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_)
{
rhs.allocator_ = 0;
rhs.ownAllocator_ = 0;
rhs.stack_ = 0;
rhs.stackTop_ = 0;
rhs.stackEnd_ = 0;
rhs.initialCapacity_ = 0;
}
Stack(Stack &&rhs)
: 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;
rhs.stackTop_ = 0;
rhs.stackEnd_ = 0;
rhs.initialCapacity_ = 0;
}
#endif
~Stack() {
Destroy();
}
~Stack() { Destroy(); }
#if RAPIDJSON_HAS_CXX11_RVALUE_REFS
Stack& operator=(Stack&& rhs) {
if (&rhs != this)
{
Destroy();
Stack &operator=(Stack &&rhs) {
if (&rhs != this) {
Destroy();
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;
rhs.stackTop_ = 0;
rhs.stackEnd_ = 0;
rhs.initialCapacity_ = 0;
}
return *this;
rhs.allocator_ = 0;
rhs.ownAllocator_ = 0;
rhs.stack_ = 0;
rhs.stackTop_ = 0;
rhs.stackEnd_ = 0;
rhs.initialCapacity_ = 0;
}
return *this;
}
#endif
void Swap(Stack& rhs) RAPIDJSON_NOEXCEPT {
internal::Swap(allocator_, rhs.allocator_);
internal::Swap(ownAllocator_, rhs.ownAllocator_);
internal::Swap(stack_, rhs.stack_);
internal::Swap(stackTop_, rhs.stackTop_);
internal::Swap(stackEnd_, rhs.stackEnd_);
internal::Swap(initialCapacity_, rhs.initialCapacity_);
}
void Swap(Stack &rhs) RAPIDJSON_NOEXCEPT {
internal::Swap(allocator_, rhs.allocator_);
internal::Swap(ownAllocator_, rhs.ownAllocator_);
internal::Swap(stack_, rhs.stack_);
internal::Swap(stackTop_, rhs.stackTop_);
internal::Swap(stackEnd_, rhs.stackEnd_);
internal::Swap(initialCapacity_, rhs.initialCapacity_);
}
void Clear() { stackTop_ = stack_; }
void Clear() { stackTop_ = stack_; }
void ShrinkToFit() {
if (Empty()) {
// If the stack is empty, completely deallocate the memory.
Allocator::Free(stack_); // NOLINT (+clang-analyzer-unix.Malloc)
stack_ = 0;
stackTop_ = 0;
stackEnd_ = 0;
}
else
Resize(GetSize());
}
void ShrinkToFit() {
if (Empty()) {
// If the stack is empty, completely deallocate the memory.
Allocator::Free(stack_); // NOLINT (+clang-analyzer-unix.Malloc)
stack_ = 0;
stackTop_ = 0;
stackEnd_ = 0;
} 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) {
// Expand the stack if needed
if (RAPIDJSON_UNLIKELY(static_cast<std::ptrdiff_t>(sizeof(T) * count) > (stackEnd_ - stackTop_)))
Expand<T>(count);
}
// 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_)))
Expand<T>(count);
}
template<typename T>
RAPIDJSON_FORCEINLINE T* Push(size_t count = 1) {
Reserve<T>(count);
return PushUnsafe<T>(count);
}
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) {
RAPIDJSON_ASSERT(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> RAPIDJSON_FORCEINLINE T *PushUnsafe(size_t count = 1) {
RAPIDJSON_ASSERT(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) {
RAPIDJSON_ASSERT(GetSize() >= count * sizeof(T));
stackTop_ -= count * sizeof(T);
return reinterpret_cast<T*>(stackTop_);
}
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() {
RAPIDJSON_ASSERT(GetSize() >= sizeof(T));
return reinterpret_cast<T*>(stackTop_ - sizeof(T));
}
template <typename T> T *Top() {
RAPIDJSON_ASSERT(GetSize() >= sizeof(T));
return reinterpret_cast<T *>(stackTop_ - sizeof(T));
}
template<typename T>
const T* Top() const {
RAPIDJSON_ASSERT(GetSize() >= sizeof(T));
return reinterpret_cast<T*>(stackTop_ - sizeof(T));
}
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> const T *End() const {
return reinterpret_cast<T *>(stackTop_);
}
template<typename T>
T* Bottom() { return reinterpret_cast<T*>(stack_); }
template <typename T> T *Bottom() { return reinterpret_cast<T *>(stack_); }
template<typename T>
const T* Bottom() const { return reinterpret_cast<T*>(stack_); }
template <typename T> const T *Bottom() const {
return reinterpret_cast<T *>(stack_);
}
bool HasAllocator() const {
return allocator_ != 0;
}
bool HasAllocator() const { return allocator_ != 0; }
Allocator& GetAllocator() {
RAPIDJSON_ASSERT(allocator_);
return *allocator_;
}
Allocator &GetAllocator() {
RAPIDJSON_ASSERT(allocator_);
return *allocator_;
}
bool Empty() const { return stackTop_ == stack_; }
size_t GetSize() const { return static_cast<size_t>(stackTop_ - stack_); }
size_t GetCapacity() const { return static_cast<size_t>(stackEnd_ - stack_); }
bool Empty() const { return stackTop_ == stack_; }
size_t GetSize() const { return static_cast<size_t>(stackTop_ - stack_); }
size_t GetCapacity() const { return static_cast<size_t>(stackEnd_ - stack_); }
private:
template<typename T>
void Expand(size_t count) {
// Only expand the capacity if the current stack exists. Otherwise just create a stack with initial capacity.
size_t newCapacity;
if (stack_ == 0) {
if (!allocator_)
ownAllocator_ = allocator_ = RAPIDJSON_NEW(Allocator)();
newCapacity = initialCapacity_;
} else {
newCapacity = GetCapacity();
newCapacity += (newCapacity + 1) / 2;
}
size_t newSize = GetSize() + sizeof(T) * count;
if (newCapacity < newSize)
newCapacity = newSize;
Resize(newCapacity);
template <typename T> void Expand(size_t count) {
// Only expand the capacity if the current stack exists. Otherwise just
// create a stack with initial capacity.
size_t newCapacity;
if (stack_ == 0) {
if (!allocator_)
ownAllocator_ = allocator_ = RAPIDJSON_NEW(Allocator)();
newCapacity = initialCapacity_;
} else {
newCapacity = GetCapacity();
newCapacity += (newCapacity + 1) / 2;
}
size_t newSize = GetSize() + sizeof(T) * count;
if (newCapacity < newSize)
newCapacity = newSize;
void Resize(size_t newCapacity) {
const size_t size = GetSize(); // Backup the current size
stack_ = static_cast<char*>(allocator_->Realloc(stack_, GetCapacity(), newCapacity));
stackTop_ = stack_ + size;
stackEnd_ = stack_ + newCapacity;
}
Resize(newCapacity);
}
void Destroy() {
Allocator::Free(stack_);
RAPIDJSON_DELETE(ownAllocator_); // Only delete if it is owned by the stack
}
void Resize(size_t newCapacity) {
const size_t size = GetSize(); // Backup the current size
stack_ = static_cast<char *>(
allocator_->Realloc(stack_, GetCapacity(), newCapacity));
stackTop_ = stack_ + size;
stackEnd_ = stack_ + newCapacity;
}
// Prohibit copy constructor & assignment operator.
Stack(const Stack&);
Stack& operator=(const Stack&);
void Destroy() {
Allocator::Free(stack_);
RAPIDJSON_DELETE(ownAllocator_); // Only delete if it is owned by the stack
}
Allocator* allocator_;
Allocator* ownAllocator_;
char *stack_;
char *stackTop_;
char *stackEnd_;
size_t initialCapacity_;
// Prohibit copy constructor & assignment operator.
Stack(const Stack &);
Stack &operator=(const Stack &);
Allocator *allocator_;
Allocator *ownAllocator_;
char *stack_;
char *stackTop_;
char *stackEnd_;
size_t initialCapacity_;
};
} // namespace internal

80
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.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// 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
// 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
//
// 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_
@@ -24,43 +28,43 @@ namespace internal {
//! Custom strlen() which works on different character types.
/*! \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.
\return Number of characters in the string.
\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) {
RAPIDJSON_ASSERT(s != 0);
const Ch* p = s;
while (*p) ++p;
return SizeType(p - s);
template <typename Ch> inline SizeType StrLen(const Ch *s) {
RAPIDJSON_ASSERT(s != 0);
const Ch *p = s;
while (*p)
++p;
return SizeType(p - s);
}
template <>
inline SizeType StrLen(const char* s) {
return SizeType(std::strlen(s));
template <> inline SizeType StrLen(const char *s) {
return SizeType(std::strlen(s));
}
template <>
inline SizeType StrLen(const wchar_t* s) {
return SizeType(std::wcslen(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) {
RAPIDJSON_ASSERT(s != 0);
RAPIDJSON_ASSERT(outCount != 0);
GenericStringStream<Encoding> is(s);
const typename Encoding::Ch* end = s + length;
SizeType count = 0;
while (is.src_ < end) {
unsigned codepoint;
if (!Encoding::Decode(is, &codepoint))
return false;
count++;
}
*outCount = count;
return true;
template <typename Encoding>
bool CountStringCodePoint(const typename Encoding::Ch *s, SizeType length,
SizeType *outCount) {
RAPIDJSON_ASSERT(s != 0);
RAPIDJSON_ASSERT(outCount != 0);
GenericStringStream<Encoding> is(s);
const typename Encoding::Ch *end = s + length;
SizeType count = 0;
while (is.src_ < end) {
unsigned codepoint;
if (!Encoding::Decode(is, &codepoint))
return false;
count++;
}
*outCount = count;
return true;
}
} // namespace internal

463
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.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// 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
// 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
//
// 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>
@@ -26,262 +30,275 @@ RAPIDJSON_NAMESPACE_BEGIN
namespace internal {
inline double FastPath(double significand, int exp) {
if (exp < -308)
return 0.0;
else if (exp >= 0)
return significand * internal::Pow10(exp);
else
return significand / internal::Pow10(-exp);
if (exp < -308)
return 0.0;
else if (exp >= 0)
return significand * internal::Pow10(exp);
else
return significand / internal::Pow10(-exp);
}
inline double StrtodNormalPrecision(double d, int p) {
if (p < -308) {
// Prevent expSum < -308, making Pow10(p) = 0
d = FastPath(d, -308);
d = FastPath(d, p + 308);
}
else
d = FastPath(d, p);
return d;
if (p < -308) {
// Prevent expSum < -308, making Pow10(p) = 0
d = FastPath(d, -308);
d = FastPath(d, p + 308);
} else
d = FastPath(d, p);
return d;
}
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;
return m;
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;
return m;
}
inline int CheckWithinHalfULP(double b, const BigInteger& d, int dExp) {
const Double db(b);
const uint64_t bInt = db.IntegerSignificand();
const int bExp = db.IntegerExponent();
const int hExp = bExp - 1;
inline int CheckWithinHalfULP(double b, const BigInteger &d, int dExp) {
const Double db(b);
const uint64_t bInt = db.IntegerSignificand();
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 {
bS_Exp2 -= dExp;
bS_Exp5 -= dExp;
hS_Exp2 -= dExp;
hS_Exp5 -= dExp;
}
// Adjust for decimal exponent
if (dExp >= 0) {
dS_Exp2 += dExp;
dS_Exp5 += dExp;
} else {
bS_Exp2 -= dExp;
bS_Exp5 -= dExp;
hS_Exp2 -= dExp;
hS_Exp5 -= dExp;
}
// Adjust for binary exponent
if (bExp >= 0)
bS_Exp2 += bExp;
else {
dS_Exp2 -= bExp;
hS_Exp2 -= bExp;
}
// Adjust for binary exponent
if (bExp >= 0)
bS_Exp2 += bExp;
else {
dS_Exp2 -= bExp;
hS_Exp2 -= bExp;
}
// Adjust for half ulp exponent
if (hExp >= 0)
hS_Exp2 += hExp;
else {
dS_Exp2 -= hExp;
bS_Exp2 -= hExp;
}
// Adjust for half ulp exponent
if (hExp >= 0)
hS_Exp2 += hExp;
else {
dS_Exp2 -= hExp;
bS_Exp2 -= hExp;
}
// Remove common power of two factor from all three scaled values
int common_Exp2 = Min3(dS_Exp2, bS_Exp2, hS_Exp2);
dS_Exp2 -= common_Exp2;
bS_Exp2 -= common_Exp2;
hS_Exp2 -= common_Exp2;
// Remove common power of two factor from all three scaled values
int common_Exp2 = Min3(dS_Exp2, bS_Exp2, hS_Exp2);
dS_Exp2 -= common_Exp2;
bS_Exp2 -= common_Exp2;
hS_Exp2 -= common_Exp2;
BigInteger dS = d;
dS.MultiplyPow5(static_cast<unsigned>(dS_Exp5)) <<= static_cast<unsigned>(dS_Exp2);
BigInteger dS = d;
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);
BigInteger bS(bInt);
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);
BigInteger hS(1);
hS.MultiplyPow5(static_cast<unsigned>(hS_Exp5)) <<=
static_cast<unsigned>(hS_Exp2);
BigInteger delta(0);
dS.Difference(bS, &delta);
BigInteger delta(0);
dS.Difference(bS, &delta);
return delta.Compare(hS);
return delta.Compare(hS);
}
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/
if (p > 22 && p < 22 + 16) {
// Fast Path Cases In Disguise
d *= internal::Pow10(p - 22);
p = 22;
}
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/
if (p > 22 && p < 22 + 16) {
// Fast Path Cases In Disguise
d *= internal::Pow10(p - 22);
p = 22;
}
if (p >= -22 && p <= 22 && d <= 9007199254740991.0) { // 2^53 - 1
*result = FastPath(d, p);
return true;
}
else
return false;
if (p >= -22 && p <= 22 && d <= 9007199254740991.0) { // 2^53 - 1
*result = FastPath(d, p);
return true;
} 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) {
uint64_t significand = 0;
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'))
break;
significand = significand * 10u + static_cast<unsigned>(decimals[i] - '0');
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
for (; i < dLen; i++) {
if (significand > RAPIDJSON_UINT64_C2(0x19999999, 0x99999999) ||
(significand == RAPIDJSON_UINT64_C2(0x19999999, 0x99999999) &&
decimals[i] > '5'))
break;
significand = significand * 10u + static_cast<unsigned>(decimals[i] - '0');
}
if (i < dLen && decimals[i] >= '5') // Rounding
significand++;
int remaining = dLen - i;
const int kUlpShift = 3;
const int kUlp = 1 << kUlpShift;
int64_t error = (remaining == 0) ? 0 : kUlp / 2;
DiyFp v(significand, 0);
v = v.Normalize();
error <<= -v.e;
dExp += remaining;
int actualExp;
DiyFp cachedPower = GetCachedPower10(dExp, &actualExp);
if (actualExp != dExp) {
static const DiyFp kPow10[] = {
DiyFp(RAPIDJSON_UINT64_C2(0xa0000000, 0x00000000), -60), // 10^1
DiyFp(RAPIDJSON_UINT64_C2(0xc8000000, 0x00000000), -57), // 10^2
DiyFp(RAPIDJSON_UINT64_C2(0xfa000000, 0x00000000), -54), // 10^3
DiyFp(RAPIDJSON_UINT64_C2(0x9c400000, 0x00000000), -50), // 10^4
DiyFp(RAPIDJSON_UINT64_C2(0xc3500000, 0x00000000), -47), // 10^5
DiyFp(RAPIDJSON_UINT64_C2(0xf4240000, 0x00000000), -44), // 10^6
DiyFp(RAPIDJSON_UINT64_C2(0x98968000, 0x00000000), -40) // 10^7
};
int adjustment = dExp - actualExp;
RAPIDJSON_ASSERT(adjustment >= 1 && adjustment < 8);
v = v * kPow10[adjustment - 1];
if (dLen + adjustment > 19) // has more digits than decimal digits in 64-bit
error += kUlp / 2;
}
v = v * cachedPower;
error += kUlp + (error == 0 ? 0 : 1);
const int oldExp = v.e;
v = v.Normalize();
error <<= oldExp - v.e;
const int effectiveSignificandSize =
Double::EffectiveSignificandSize(64 + v.e);
int precisionSize = 64 - effectiveSignificandSize;
if (precisionSize + kUlpShift >= 64) {
int scaleExp = (precisionSize + kUlpShift) - 63;
v.f >>= scaleExp;
v.e += scaleExp;
error = (error >> scaleExp) + 1 + kUlp;
precisionSize -= scaleExp;
}
DiyFp rounded(v.f >> precisionSize, v.e + precisionSize);
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)
rounded.f >>= 1;
rounded.e++;
}
if (i < dLen && decimals[i] >= '5') // Rounding
significand++;
}
int remaining = dLen - i;
const int kUlpShift = 3;
const int kUlp = 1 << kUlpShift;
int64_t error = (remaining == 0) ? 0 : kUlp / 2;
*result = rounded.ToDouble();
DiyFp v(significand, 0);
v = v.Normalize();
error <<= -v.e;
dExp += remaining;
int actualExp;
DiyFp cachedPower = GetCachedPower10(dExp, &actualExp);
if (actualExp != dExp) {
static const DiyFp kPow10[] = {
DiyFp(RAPIDJSON_UINT64_C2(0xa0000000, 0x00000000), -60), // 10^1
DiyFp(RAPIDJSON_UINT64_C2(0xc8000000, 0x00000000), -57), // 10^2
DiyFp(RAPIDJSON_UINT64_C2(0xfa000000, 0x00000000), -54), // 10^3
DiyFp(RAPIDJSON_UINT64_C2(0x9c400000, 0x00000000), -50), // 10^4
DiyFp(RAPIDJSON_UINT64_C2(0xc3500000, 0x00000000), -47), // 10^5
DiyFp(RAPIDJSON_UINT64_C2(0xf4240000, 0x00000000), -44), // 10^6
DiyFp(RAPIDJSON_UINT64_C2(0x98968000, 0x00000000), -40) // 10^7
};
int adjustment = dExp - actualExp;
RAPIDJSON_ASSERT(adjustment >= 1 && adjustment < 8);
v = v * kPow10[adjustment - 1];
if (dLen + adjustment > 19) // has more digits than decimal digits in 64-bit
error += kUlp / 2;
}
v = v * cachedPower;
error += kUlp + (error == 0 ? 0 : 1);
const int oldExp = v.e;
v = v.Normalize();
error <<= oldExp - v.e;
const int effectiveSignificandSize = Double::EffectiveSignificandSize(64 + v.e);
int precisionSize = 64 - effectiveSignificandSize;
if (precisionSize + kUlpShift >= 64) {
int scaleExp = (precisionSize + kUlpShift) - 63;
v.f >>= scaleExp;
v.e += scaleExp;
error = (error >> scaleExp) + 1 + kUlp;
precisionSize -= scaleExp;
}
DiyFp rounded(v.f >> precisionSize, v.e + precisionSize);
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)
rounded.f >>= 1;
rounded.e++;
}
}
*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) {
RAPIDJSON_ASSERT(dLen >= 0);
const BigInteger dInt(decimals, static_cast<unsigned>(dLen));
Double a(approx);
int cmp = CheckWithinHalfULP(a.Value(), dInt, dExp);
if (cmp < 0)
return a.Value(); // within half ULP
else if (cmp == 0) {
// Round towards even
if (a.Significand() & 1)
return a.NextPositiveDouble();
else
return a.Value();
}
else // adjustment
return a.NextPositiveDouble();
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);
int cmp = CheckWithinHalfULP(a.Value(), dInt, dExp);
if (cmp < 0)
return a.Value(); // within half ULP
else if (cmp == 0) {
// Round towards even
if (a.Significand() & 1)
return a.NextPositiveDouble();
else
return a.Value();
} else // adjustment
return a.NextPositiveDouble();
}
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);
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);
double result = 0.0;
if (StrtodFast(d, p, &result))
return result;
double result = 0.0;
if (StrtodFast(d, p, &result))
return result;
RAPIDJSON_ASSERT(length <= INT_MAX);
int dLen = static_cast<int>(length);
RAPIDJSON_ASSERT(length <= INT_MAX);
int dLen = static_cast<int>(length);
RAPIDJSON_ASSERT(length >= decimalPosition);
RAPIDJSON_ASSERT(length - decimalPosition <= INT_MAX);
int dExpAdjust = static_cast<int>(length - decimalPosition);
RAPIDJSON_ASSERT(length >= decimalPosition);
RAPIDJSON_ASSERT(length - decimalPosition <= INT_MAX);
int dExpAdjust = static_cast<int>(length - decimalPosition);
RAPIDJSON_ASSERT(exp >= INT_MIN + dExpAdjust);
int dExp = exp - dExpAdjust;
RAPIDJSON_ASSERT(exp >= INT_MIN + dExpAdjust);
int dExp = exp - dExpAdjust;
// Make sure length+dExp does not overflow
RAPIDJSON_ASSERT(dExp <= INT_MAX - dLen);
// Make sure length+dExp does not overflow
RAPIDJSON_ASSERT(dExp <= INT_MAX - dLen);
// Trim leading zeros
while (dLen > 0 && *decimals == '0') {
dLen--;
decimals++;
}
// Trim leading zeros
while (dLen > 0 && *decimals == '0') {
dLen--;
decimals++;
}
// Trim trailing zeros
while (dLen > 0 && decimals[dLen - 1] == '0') {
dLen--;
dExp++;
}
// Trim trailing zeros
while (dLen > 0 && decimals[dLen - 1] == '0') {
dLen--;
dExp++;
}
if (dLen == 0) { // Buffer only contains zeros.
return 0.0;
}
if (dLen == 0) { // Buffer only contains zeros.
return 0.0;
}
// Trim right-most digits
const int kMaxDecimalDigit = 767 + 1;
if (dLen > kMaxDecimalDigit) {
dExp += dLen - kMaxDecimalDigit;
dLen = kMaxDecimalDigit;
}
// Trim right-most digits
const int kMaxDecimalDigit = 767 + 1;
if (dLen > kMaxDecimalDigit) {
dExp += dLen - kMaxDecimalDigit;
dLen = kMaxDecimalDigit;
}
// If too small, underflow to zero.
// Any x <= 10^-324 is interpreted as zero.
if (dLen + dExp <= -324)
return 0.0;
// If too small, underflow to zero.
// Any x <= 10^-324 is interpreted as zero.
if (dLen + dExp <= -324)
return 0.0;
// If too large, overflow to infinity.
// Any x >= 10^309 is interpreted as +infinity.
if (dLen + dExp > 309)
return std::numeric_limits<double>::infinity();
// If too large, overflow to infinity.
// Any x >= 10^309 is interpreted as +infinity.
if (dLen + dExp > 309)
return std::numeric_limits<double>::infinity();
if (StrtodDiyFp(decimals, dLen, dExp, &result))
return result;
if (StrtodDiyFp(decimals, dLen, dExp, &result))
return result;
// Use approximation from StrtodDiyFp and make adjustment with BigInteger comparison
return StrtodBigInteger(result, decimals, dLen, dExp);
// Use approximation from StrtodDiyFp and make adjustment with BigInteger
// comparison
return StrtodBigInteger(result, decimals, dLen, dExp);
}
} // namespace internal

35
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_
@@ -19,21 +23,20 @@
#if defined(__clang__)
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(c++98-compat)
RAPIDJSON_DIAG_OFF(c++ 98 - compat)
#endif
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 {
T tmp = a;
a = b;
b = tmp;
template <typename T> inline void Swap(T &a, T &b) RAPIDJSON_NOEXCEPT {
T tmp = a;
a = b;
b = tmp;
}
} // namespace internal

159
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.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// 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
// 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
//
// 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
@@ -44,76 +49,92 @@ 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;
typedef typename StreamType::char_type Ch;
//! Constructor.
/*!
\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) {
Read();
}
//! Constructor.
/*!
\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) {
Read();
}
//! Constructor.
/*!
\param stream stream opened for read.
\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) {
RAPIDJSON_ASSERT(bufferSize >= 4);
Read();
}
//! Constructor.
/*!
\param stream stream opened for read.
\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) {
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 Peek() const { return *current_; }
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; }
// 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;
}
// For encoding detection only.
const Ch* Peek4() const {
return (current_ + 4 - !eof_ <= bufferLast_) ? current_ : 0;
}
// For encoding detection only.
const Ch *Peek4() const {
return (current_ + 4 - !eof_ <= bufferLast_) ? current_ : 0;
}
private:
BasicIStreamWrapper();
BasicIStreamWrapper(const BasicIStreamWrapper&);
BasicIStreamWrapper& operator=(const BasicIStreamWrapper&);
BasicIStreamWrapper();
BasicIStreamWrapper(const BasicIStreamWrapper &);
BasicIStreamWrapper &operator=(const BasicIStreamWrapper &);
void Read() {
if (current_ < bufferLast_)
++current_;
else if (!eof_) {
count_ += readCount_;
readCount_ = bufferSize_;
bufferLast_ = buffer_ + readCount_ - 1;
current_ = buffer_;
void Read() {
if (current_ < bufferLast_)
++current_;
else if (!eof_) {
count_ += readCount_;
readCount_ = bufferSize_;
bufferLast_ = buffer_ + readCount_ - 1;
current_ = buffer_;
if (!stream_.read(buffer_, static_cast<std::streamsize>(bufferSize_))) {
readCount_ = static_cast<size_t>(stream_.gcount());
*(bufferLast_ = buffer_ + readCount_) = '\0';
eof_ = true;
}
}
if (!stream_.read(buffer_, static_cast<std::streamsize>(bufferSize_))) {
readCount_ = static_cast<size_t>(stream_.gcount());
*(bufferLast_ = buffer_ + readCount_) = '\0';
eof_ = true;
}
}
}
StreamType &stream_;
Ch peekBuffer_[4], *buffer_;
size_t bufferSize_;
Ch *bufferLast_;
Ch *current_;
size_t readCount_;
size_t count_; //!< Number of characters read
bool eof_;
StreamType &stream_;
Ch peekBuffer_[4], *buffer_;
size_t bufferSize_;
Ch *bufferLast_;
Ch *current_;
size_t readCount_;
size_t count_; //!< Number of characters read
bool eof_;
};
typedef BasicIStreamWrapper<std::istream> IStreamWrapper;

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

@@ -1,68 +1,74 @@
// 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.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// 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
// 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
//
// 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.
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.
\tparam Allocator type for allocating memory buffer.
\note implements Stream concept
*/
template <typename Allocator = CrtAllocator>
struct GenericMemoryBuffer {
typedef char Ch; // byte
template <typename Allocator = CrtAllocator> struct GenericMemoryBuffer {
typedef char Ch; // byte
GenericMemoryBuffer(Allocator* allocator = 0, size_t capacity = kDefaultCapacity) : 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() {}
void Put(Ch c) { *stack_.template Push<Ch>() = c; }
void Flush() {}
void Clear() { stack_.Clear(); }
void ShrinkToFit() { stack_.ShrinkToFit(); }
Ch* Push(size_t count) { return stack_.template Push<Ch>(count); }
void Pop(size_t count) { stack_.template Pop<Ch>(count); }
void Clear() { stack_.Clear(); }
void ShrinkToFit() { stack_.ShrinkToFit(); }
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(); }
size_t GetSize() const { return stack_.GetSize(); }
static const size_t kDefaultCapacity = 256;
mutable internal::Stack<Allocator> stack_;
static const size_t kDefaultCapacity = 256;
mutable internal::Stack<Allocator> stack_;
};
typedef GenericMemoryBuffer<> MemoryBuffer;
//! 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));
//! 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));
}
RAPIDJSON_NAMESPACE_END

79
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.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// 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
// 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
//
// 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_
@@ -19,47 +23,56 @@
#ifdef __clang__
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(unreachable-code)
RAPIDJSON_DIAG_OFF(missing-noreturn)
RAPIDJSON_DIAG_OFF(unreachable - code)
RAPIDJSON_DIAG_OFF(missing - noreturn)
#endif
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
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 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; }
void Put(Ch) { RAPIDJSON_ASSERT(false); }
void Flush() { RAPIDJSON_ASSERT(false); }
size_t PutEnd(Ch*) { 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;
}
// For encoding detection only.
const Ch* Peek4() const {
return Tell() + 4 <= size_ ? src_ : 0;
}
// For encoding detection only.
const Ch *Peek4() const { return Tell() + 4 <= size_ ? src_ : 0; }
const Ch* src_; //!< Current read position.
const Ch* begin_; //!< Original head of the string.
const Ch* end_; //!< End of stream.
size_t size_; //!< Size of the stream.
const Ch *src_; //!< Current read position.
const Ch *begin_; //!< Original head of the string.
const Ch *end_; //!< End of stream.
size_t size_; //!< Size of the stream.
};
RAPIDJSON_NAMESPACE_END

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

@@ -1,37 +1,37 @@
// ISO C9x compliant inttypes.h for Microsoft Visual Studio
// Based on ISO/IEC 9899:TC2 Committee draft (May 6, 2005) WG14/N1124
//
// Based on ISO/IEC 9899:TC2 Committee draft (May 6, 2005) WG14/N1124
//
// Copyright (c) 2006-2013 Alexander Chemeris
//
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
//
// 1. Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
//
// 3. Neither the name of the product nor the names of its contributors may
// be used to endorse or promote products derived from this software
// without specific prior written permission.
//
//
// THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
// WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO
// EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
// OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
// OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
// OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
// ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
//
///////////////////////////////////////////////////////////////////////////////
// The above software in this distribution may have been modified by
// THL A29 Limited ("Tencent Modifications").
// The above software in this distribution may have been modified by
// THL A29 Limited ("Tencent Modifications").
// All Tencent Modifications are Copyright (C) 2015 THL A29 Limited.
#ifndef _MSC_VER // [
@@ -55,221 +55,222 @@
// 7.8 Format conversion of integer types
typedef struct {
intmax_t quot;
intmax_t rem;
intmax_t quot;
intmax_t rem;
} imaxdiv_t;
// 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"
#define PRIi8 "i"
#define PRIdLEAST8 "d"
#define PRIiLEAST8 "i"
#define PRIdFAST8 "d"
#define PRIiFAST8 "i"
#define PRId8 "d"
#define PRIi8 "i"
#define PRIdLEAST8 "d"
#define PRIiLEAST8 "i"
#define PRIdFAST8 "d"
#define PRIiFAST8 "i"
#define PRId16 "hd"
#define PRIi16 "hi"
#define PRIdLEAST16 "hd"
#define PRIiLEAST16 "hi"
#define PRIdFAST16 "hd"
#define PRIiFAST16 "hi"
#define PRId16 "hd"
#define PRIi16 "hi"
#define PRIdLEAST16 "hd"
#define PRIiLEAST16 "hi"
#define PRIdFAST16 "hd"
#define PRIiFAST16 "hi"
#define PRId32 "I32d"
#define PRIi32 "I32i"
#define PRIdLEAST32 "I32d"
#define PRIiLEAST32 "I32i"
#define PRIdFAST32 "I32d"
#define PRIiFAST32 "I32i"
#define PRId32 "I32d"
#define PRIi32 "I32i"
#define PRIdLEAST32 "I32d"
#define PRIiLEAST32 "I32i"
#define PRIdFAST32 "I32d"
#define PRIiFAST32 "I32i"
#define PRId64 "I64d"
#define PRIi64 "I64i"
#define PRIdLEAST64 "I64d"
#define PRIiLEAST64 "I64i"
#define PRIdFAST64 "I64d"
#define PRIiFAST64 "I64i"
#define PRId64 "I64d"
#define PRIi64 "I64i"
#define PRIdLEAST64 "I64d"
#define PRIiLEAST64 "I64i"
#define PRIdFAST64 "I64d"
#define PRIiFAST64 "I64i"
#define PRIdMAX "I64d"
#define PRIiMAX "I64i"
#define PRIdMAX "I64d"
#define PRIiMAX "I64i"
#define PRIdPTR "Id"
#define PRIiPTR "Ii"
#define PRIdPTR "Id"
#define PRIiPTR "Ii"
// The fprintf macros for unsigned integers are:
#define PRIo8 "o"
#define PRIu8 "u"
#define PRIx8 "x"
#define PRIX8 "X"
#define PRIoLEAST8 "o"
#define PRIuLEAST8 "u"
#define PRIxLEAST8 "x"
#define PRIXLEAST8 "X"
#define PRIoFAST8 "o"
#define PRIuFAST8 "u"
#define PRIxFAST8 "x"
#define PRIXFAST8 "X"
#define PRIo8 "o"
#define PRIu8 "u"
#define PRIx8 "x"
#define PRIX8 "X"
#define PRIoLEAST8 "o"
#define PRIuLEAST8 "u"
#define PRIxLEAST8 "x"
#define PRIXLEAST8 "X"
#define PRIoFAST8 "o"
#define PRIuFAST8 "u"
#define PRIxFAST8 "x"
#define PRIXFAST8 "X"
#define PRIo16 "ho"
#define PRIu16 "hu"
#define PRIx16 "hx"
#define PRIX16 "hX"
#define PRIoLEAST16 "ho"
#define PRIuLEAST16 "hu"
#define PRIxLEAST16 "hx"
#define PRIXLEAST16 "hX"
#define PRIoFAST16 "ho"
#define PRIuFAST16 "hu"
#define PRIxFAST16 "hx"
#define PRIXFAST16 "hX"
#define PRIo16 "ho"
#define PRIu16 "hu"
#define PRIx16 "hx"
#define PRIX16 "hX"
#define PRIoLEAST16 "ho"
#define PRIuLEAST16 "hu"
#define PRIxLEAST16 "hx"
#define PRIXLEAST16 "hX"
#define PRIoFAST16 "ho"
#define PRIuFAST16 "hu"
#define PRIxFAST16 "hx"
#define PRIXFAST16 "hX"
#define PRIo32 "I32o"
#define PRIu32 "I32u"
#define PRIx32 "I32x"
#define PRIX32 "I32X"
#define PRIoLEAST32 "I32o"
#define PRIuLEAST32 "I32u"
#define PRIxLEAST32 "I32x"
#define PRIXLEAST32 "I32X"
#define PRIoFAST32 "I32o"
#define PRIuFAST32 "I32u"
#define PRIxFAST32 "I32x"
#define PRIXFAST32 "I32X"
#define PRIo32 "I32o"
#define PRIu32 "I32u"
#define PRIx32 "I32x"
#define PRIX32 "I32X"
#define PRIoLEAST32 "I32o"
#define PRIuLEAST32 "I32u"
#define PRIxLEAST32 "I32x"
#define PRIXLEAST32 "I32X"
#define PRIoFAST32 "I32o"
#define PRIuFAST32 "I32u"
#define PRIxFAST32 "I32x"
#define PRIXFAST32 "I32X"
#define PRIo64 "I64o"
#define PRIu64 "I64u"
#define PRIx64 "I64x"
#define PRIX64 "I64X"
#define PRIoLEAST64 "I64o"
#define PRIuLEAST64 "I64u"
#define PRIxLEAST64 "I64x"
#define PRIXLEAST64 "I64X"
#define PRIoFAST64 "I64o"
#define PRIuFAST64 "I64u"
#define PRIxFAST64 "I64x"
#define PRIXFAST64 "I64X"
#define PRIo64 "I64o"
#define PRIu64 "I64u"
#define PRIx64 "I64x"
#define PRIX64 "I64X"
#define PRIoLEAST64 "I64o"
#define PRIuLEAST64 "I64u"
#define PRIxLEAST64 "I64x"
#define PRIXLEAST64 "I64X"
#define PRIoFAST64 "I64o"
#define PRIuFAST64 "I64u"
#define PRIxFAST64 "I64x"
#define PRIXFAST64 "I64X"
#define PRIoMAX "I64o"
#define PRIuMAX "I64u"
#define PRIxMAX "I64x"
#define PRIXMAX "I64X"
#define PRIoMAX "I64o"
#define PRIuMAX "I64u"
#define PRIxMAX "I64x"
#define PRIXMAX "I64X"
#define PRIoPTR "Io"
#define PRIuPTR "Iu"
#define PRIxPTR "Ix"
#define PRIXPTR "IX"
#define PRIoPTR "Io"
#define PRIuPTR "Iu"
#define PRIxPTR "Ix"
#define PRIXPTR "IX"
// The fscanf macros for signed integers are:
#define SCNd8 "d"
#define SCNi8 "i"
#define SCNdLEAST8 "d"
#define SCNiLEAST8 "i"
#define SCNdFAST8 "d"
#define SCNiFAST8 "i"
#define SCNd8 "d"
#define SCNi8 "i"
#define SCNdLEAST8 "d"
#define SCNiLEAST8 "i"
#define SCNdFAST8 "d"
#define SCNiFAST8 "i"
#define SCNd16 "hd"
#define SCNi16 "hi"
#define SCNdLEAST16 "hd"
#define SCNiLEAST16 "hi"
#define SCNdFAST16 "hd"
#define SCNiFAST16 "hi"
#define SCNd16 "hd"
#define SCNi16 "hi"
#define SCNdLEAST16 "hd"
#define SCNiLEAST16 "hi"
#define SCNdFAST16 "hd"
#define SCNiFAST16 "hi"
#define SCNd32 "ld"
#define SCNi32 "li"
#define SCNdLEAST32 "ld"
#define SCNiLEAST32 "li"
#define SCNdFAST32 "ld"
#define SCNiFAST32 "li"
#define SCNd32 "ld"
#define SCNi32 "li"
#define SCNdLEAST32 "ld"
#define SCNiLEAST32 "li"
#define SCNdFAST32 "ld"
#define SCNiFAST32 "li"
#define SCNd64 "I64d"
#define SCNi64 "I64i"
#define SCNdLEAST64 "I64d"
#define SCNiLEAST64 "I64i"
#define SCNdFAST64 "I64d"
#define SCNiFAST64 "I64i"
#define SCNd64 "I64d"
#define SCNi64 "I64i"
#define SCNdLEAST64 "I64d"
#define SCNiLEAST64 "I64i"
#define SCNdFAST64 "I64d"
#define SCNiFAST64 "I64i"
#define SCNdMAX "I64d"
#define SCNiMAX "I64i"
#define SCNdMAX "I64d"
#define SCNiMAX "I64i"
#ifdef _WIN64 // [
# define SCNdPTR "I64d"
# define SCNiPTR "I64i"
#else // _WIN64 ][
# define SCNdPTR "ld"
# define SCNiPTR "li"
#endif // _WIN64 ]
#define SCNdPTR "I64d"
#define SCNiPTR "I64i"
#else // _WIN64 ][
#define SCNdPTR "ld"
#define SCNiPTR "li"
#endif // _WIN64 ]
// The fscanf macros for unsigned integers are:
#define SCNo8 "o"
#define SCNu8 "u"
#define SCNx8 "x"
#define SCNX8 "X"
#define SCNoLEAST8 "o"
#define SCNuLEAST8 "u"
#define SCNxLEAST8 "x"
#define SCNXLEAST8 "X"
#define SCNoFAST8 "o"
#define SCNuFAST8 "u"
#define SCNxFAST8 "x"
#define SCNXFAST8 "X"
#define SCNo8 "o"
#define SCNu8 "u"
#define SCNx8 "x"
#define SCNX8 "X"
#define SCNoLEAST8 "o"
#define SCNuLEAST8 "u"
#define SCNxLEAST8 "x"
#define SCNXLEAST8 "X"
#define SCNoFAST8 "o"
#define SCNuFAST8 "u"
#define SCNxFAST8 "x"
#define SCNXFAST8 "X"
#define SCNo16 "ho"
#define SCNu16 "hu"
#define SCNx16 "hx"
#define SCNX16 "hX"
#define SCNoLEAST16 "ho"
#define SCNuLEAST16 "hu"
#define SCNxLEAST16 "hx"
#define SCNXLEAST16 "hX"
#define SCNoFAST16 "ho"
#define SCNuFAST16 "hu"
#define SCNxFAST16 "hx"
#define SCNXFAST16 "hX"
#define SCNo16 "ho"
#define SCNu16 "hu"
#define SCNx16 "hx"
#define SCNX16 "hX"
#define SCNoLEAST16 "ho"
#define SCNuLEAST16 "hu"
#define SCNxLEAST16 "hx"
#define SCNXLEAST16 "hX"
#define SCNoFAST16 "ho"
#define SCNuFAST16 "hu"
#define SCNxFAST16 "hx"
#define SCNXFAST16 "hX"
#define SCNo32 "lo"
#define SCNu32 "lu"
#define SCNx32 "lx"
#define SCNX32 "lX"
#define SCNoLEAST32 "lo"
#define SCNuLEAST32 "lu"
#define SCNxLEAST32 "lx"
#define SCNXLEAST32 "lX"
#define SCNoFAST32 "lo"
#define SCNuFAST32 "lu"
#define SCNxFAST32 "lx"
#define SCNXFAST32 "lX"
#define SCNo32 "lo"
#define SCNu32 "lu"
#define SCNx32 "lx"
#define SCNX32 "lX"
#define SCNoLEAST32 "lo"
#define SCNuLEAST32 "lu"
#define SCNxLEAST32 "lx"
#define SCNXLEAST32 "lX"
#define SCNoFAST32 "lo"
#define SCNuFAST32 "lu"
#define SCNxFAST32 "lx"
#define SCNXFAST32 "lX"
#define SCNo64 "I64o"
#define SCNu64 "I64u"
#define SCNx64 "I64x"
#define SCNX64 "I64X"
#define SCNoLEAST64 "I64o"
#define SCNuLEAST64 "I64u"
#define SCNxLEAST64 "I64x"
#define SCNXLEAST64 "I64X"
#define SCNoFAST64 "I64o"
#define SCNuFAST64 "I64u"
#define SCNxFAST64 "I64x"
#define SCNXFAST64 "I64X"
#define SCNo64 "I64o"
#define SCNu64 "I64u"
#define SCNx64 "I64x"
#define SCNX64 "I64X"
#define SCNoLEAST64 "I64o"
#define SCNuLEAST64 "I64u"
#define SCNxLEAST64 "I64x"
#define SCNXLEAST64 "I64X"
#define SCNoFAST64 "I64o"
#define SCNuFAST64 "I64u"
#define SCNxFAST64 "I64x"
#define SCNXFAST64 "I64X"
#define SCNoMAX "I64o"
#define SCNuMAX "I64u"
#define SCNxMAX "I64x"
#define SCNXMAX "I64X"
#define SCNoMAX "I64o"
#define SCNuMAX "I64u"
#define SCNxMAX "I64x"
#define SCNXMAX "I64X"
#ifdef _WIN64 // [
# define SCNoPTR "I64o"
# define SCNuPTR "I64u"
# define SCNxPTR "I64x"
# define SCNXPTR "I64X"
#else // _WIN64 ][
# define SCNoPTR "lo"
# define SCNuPTR "lu"
# define SCNxPTR "lx"
# define SCNXPTR "lX"
#endif // _WIN64 ]
#define SCNoPTR "I64o"
#define SCNuPTR "I64u"
#define SCNxPTR "I64x"
#define SCNXPTR "I64X"
#else // _WIN64 ][
#define SCNoPTR "lo"
#define SCNuPTR "lu"
#define SCNxPTR "lx"
#define SCNXPTR "lX"
#endif // _WIN64 ]
#endif // __STDC_FORMAT_MACROS ]
@@ -284,23 +285,22 @@ typedef struct {
// in %MSVC.NET%\crt\src\div.c
#ifdef STATIC_IMAXDIV // [
static
#else // STATIC_IMAXDIV ][
#else // STATIC_IMAXDIV ][
_inline
#endif // STATIC_IMAXDIV ]
imaxdiv_t __cdecl imaxdiv(intmax_t numer, intmax_t denom)
{
imaxdiv_t result;
#endif // STATIC_IMAXDIV ]
imaxdiv_t __cdecl imaxdiv(intmax_t numer, intmax_t denom) {
imaxdiv_t result;
result.quot = numer / denom;
result.rem = numer % denom;
result.quot = numer / denom;
result.rem = numer % denom;
if (numer < 0 && result.rem > 0) {
// did division wrong; must fix up
++result.quot;
result.rem -= denom;
}
if (numer < 0 && result.rem > 0) {
// did division wrong; must fix up
++result.quot;
result.rem -= denom;
}
return result;
return result;
}
// 7.8.2.3 The strtoimax and strtoumax functions

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

@@ -1,37 +1,37 @@
// ISO C9x compliant stdint.h for Microsoft Visual Studio
// Based on ISO/IEC 9899:TC2 Committee draft (May 6, 2005) WG14/N1124
//
// Based on ISO/IEC 9899:TC2 Committee draft (May 6, 2005) WG14/N1124
//
// Copyright (c) 2006-2013 Alexander Chemeris
//
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
//
// 1. Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
//
// 3. Neither the name of the product nor the names of its contributors may
// be used to endorse or promote products derived from this software
// without specific prior written permission.
//
//
// THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
// WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO
// EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
// OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
// OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
// OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
// ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
//
///////////////////////////////////////////////////////////////////////////////
// The above software in this distribution may have been modified by
// THL A29 Limited ("Tencent Modifications").
// The above software in this distribution may have been modified by
// THL A29 Limited ("Tencent Modifications").
// All Tencent Modifications are Copyright (C) 2015 THL A29 Limited.
#ifndef _MSC_VER // [
@@ -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
@@ -62,12 +64,12 @@
// 7.18.4.1 Macros for minimum-width integer constants
#define INT8_C(val) val##i8
#define INT8_C(val) val##i8
#define INT16_C(val) val##i16
#define INT32_C(val) val##i32
#define INT64_C(val) val##i64
#define UINT8_C(val) val##ui8
#define UINT8_C(val) val##ui8
#define UINT16_C(val) val##ui16
#define UINT32_C(val) val##ui32
#define UINT64_C(val) val##ui64
@@ -76,10 +78,10 @@
// These #ifndef's are needed to prevent collisions with <boost/cstdint.hpp>.
// Check out Issue 9 for the details.
#ifndef INTMAX_C // [
# define INTMAX_C INT64_C
#endif // INTMAX_C ]
#define INTMAX_C INT64_C
#endif // INTMAX_C ]
#ifndef UINTMAX_C // [
# define UINTMAX_C UINT64_C
#define UINTMAX_C UINT64_C
#endif // UINTMAX_C ]
#endif // __STDC_CONSTANT_MACROS ]
@@ -95,20 +97,19 @@
#if defined(__cplusplus) && !defined(_M_ARM)
extern "C" {
#endif
# include <wchar.h>
#include <wchar.h>
#if defined(__cplusplus) && !defined(_M_ARM)
}
#endif
// Define _W64 macros to mark types changing their size, like intptr_t.
#ifndef _W64
# if !defined(__midl) && (defined(_X86_) || defined(_M_IX86)) && _MSC_VER >= 1300
# define _W64 __w64
# else
# define _W64
# endif
#if !defined(__midl) && (defined(_X86_) || defined(_M_IX86)) && _MSC_VER >= 1300
#define _W64 __w64
#else
#define _W64
#endif
#endif
// 7.18.1 Integer types
@@ -118,167 +119,167 @@ extern "C" {
// realize that, e.g. char has the same size as __int8
// so we give up on __intX for them.
#if (_MSC_VER < 1300)
typedef signed char int8_t;
typedef signed short int16_t;
typedef signed int int32_t;
typedef unsigned char uint8_t;
typedef unsigned short uint16_t;
typedef unsigned int uint32_t;
typedef signed char int8_t;
typedef signed short int16_t;
typedef signed int int32_t;
typedef unsigned char uint8_t;
typedef unsigned short uint16_t;
typedef unsigned int uint32_t;
#else
typedef signed __int8 int8_t;
typedef signed __int16 int16_t;
typedef signed __int32 int32_t;
typedef unsigned __int8 uint8_t;
typedef unsigned __int16 uint16_t;
typedef unsigned __int32 uint32_t;
typedef signed __int8 int8_t;
typedef signed __int16 int16_t;
typedef signed __int32 int32_t;
typedef unsigned __int8 uint8_t;
typedef unsigned __int16 uint16_t;
typedef unsigned __int32 uint32_t;
#endif
typedef signed __int64 int64_t;
typedef unsigned __int64 uint64_t;
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;
typedef int32_t int_least32_t;
typedef int64_t int_least64_t;
typedef uint8_t uint_least8_t;
typedef uint16_t uint_least16_t;
typedef uint32_t uint_least32_t;
typedef uint64_t uint_least64_t;
typedef int8_t int_least8_t;
typedef int16_t int_least16_t;
typedef int32_t int_least32_t;
typedef int64_t int_least64_t;
typedef uint8_t uint_least8_t;
typedef uint16_t uint_least16_t;
typedef uint32_t uint_least32_t;
typedef uint64_t uint_least64_t;
// 7.18.1.3 Fastest minimum-width integer types
typedef int8_t int_fast8_t;
typedef int16_t int_fast16_t;
typedef int32_t int_fast32_t;
typedef int64_t int_fast64_t;
typedef uint8_t uint_fast8_t;
typedef uint16_t uint_fast16_t;
typedef uint32_t uint_fast32_t;
typedef uint64_t uint_fast64_t;
typedef int8_t int_fast8_t;
typedef int16_t int_fast16_t;
typedef int32_t int_fast32_t;
typedef int64_t int_fast64_t;
typedef uint8_t uint_fast8_t;
typedef uint16_t uint_fast16_t;
typedef uint32_t uint_fast32_t;
typedef uint64_t uint_fast64_t;
// 7.18.1.4 Integer types capable of holding object pointers
#ifdef _WIN64 // [
typedef signed __int64 intptr_t;
typedef unsigned __int64 uintptr_t;
#else // _WIN64 ][
typedef _W64 signed int intptr_t;
typedef _W64 unsigned int uintptr_t;
#endif // _WIN64 ]
typedef signed __int64 intptr_t;
typedef unsigned __int64 uintptr_t;
#else // _WIN64 ][
typedef _W64 signed int intptr_t;
typedef _W64 unsigned int uintptr_t;
#endif // _WIN64 ]
// 7.18.1.5 Greatest-width integer types
typedef int64_t intmax_t;
typedef uint64_t uintmax_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)
#define INT8_MAX _I8_MAX
#define INT16_MIN ((int16_t)_I16_MIN)
#define INT16_MAX _I16_MAX
#define INT32_MIN ((int32_t)_I32_MIN)
#define INT32_MAX _I32_MAX
#define INT64_MIN ((int64_t)_I64_MIN)
#define INT64_MAX _I64_MAX
#define UINT8_MAX _UI8_MAX
#define UINT16_MAX _UI16_MAX
#define UINT32_MAX _UI32_MAX
#define UINT64_MAX _UI64_MAX
#define INT8_MIN ((int8_t)_I8_MIN)
#define INT8_MAX _I8_MAX
#define INT16_MIN ((int16_t)_I16_MIN)
#define INT16_MAX _I16_MAX
#define INT32_MIN ((int32_t)_I32_MIN)
#define INT32_MAX _I32_MAX
#define INT64_MIN ((int64_t)_I64_MIN)
#define INT64_MAX _I64_MAX
#define UINT8_MAX _UI8_MAX
#define UINT16_MAX _UI16_MAX
#define UINT32_MAX _UI32_MAX
#define UINT64_MAX _UI64_MAX
// 7.18.2.2 Limits of minimum-width integer types
#define INT_LEAST8_MIN INT8_MIN
#define INT_LEAST8_MAX INT8_MAX
#define INT_LEAST16_MIN INT16_MIN
#define INT_LEAST16_MAX INT16_MAX
#define INT_LEAST32_MIN INT32_MIN
#define INT_LEAST32_MAX INT32_MAX
#define INT_LEAST64_MIN INT64_MIN
#define INT_LEAST64_MAX INT64_MAX
#define UINT_LEAST8_MAX UINT8_MAX
#define UINT_LEAST16_MAX UINT16_MAX
#define UINT_LEAST32_MAX UINT32_MAX
#define UINT_LEAST64_MAX UINT64_MAX
#define INT_LEAST8_MIN INT8_MIN
#define INT_LEAST8_MAX INT8_MAX
#define INT_LEAST16_MIN INT16_MIN
#define INT_LEAST16_MAX INT16_MAX
#define INT_LEAST32_MIN INT32_MIN
#define INT_LEAST32_MAX INT32_MAX
#define INT_LEAST64_MIN INT64_MIN
#define INT_LEAST64_MAX INT64_MAX
#define UINT_LEAST8_MAX UINT8_MAX
#define UINT_LEAST16_MAX UINT16_MAX
#define UINT_LEAST32_MAX UINT32_MAX
#define UINT_LEAST64_MAX UINT64_MAX
// 7.18.2.3 Limits of fastest minimum-width integer types
#define INT_FAST8_MIN INT8_MIN
#define INT_FAST8_MAX INT8_MAX
#define INT_FAST16_MIN INT16_MIN
#define INT_FAST16_MAX INT16_MAX
#define INT_FAST32_MIN INT32_MIN
#define INT_FAST32_MAX INT32_MAX
#define INT_FAST64_MIN INT64_MIN
#define INT_FAST64_MAX INT64_MAX
#define UINT_FAST8_MAX UINT8_MAX
#define UINT_FAST16_MAX UINT16_MAX
#define UINT_FAST32_MAX UINT32_MAX
#define UINT_FAST64_MAX UINT64_MAX
#define INT_FAST8_MIN INT8_MIN
#define INT_FAST8_MAX INT8_MAX
#define INT_FAST16_MIN INT16_MIN
#define INT_FAST16_MAX INT16_MAX
#define INT_FAST32_MIN INT32_MIN
#define INT_FAST32_MAX INT32_MAX
#define INT_FAST64_MIN INT64_MIN
#define INT_FAST64_MAX INT64_MAX
#define UINT_FAST8_MAX UINT8_MAX
#define UINT_FAST16_MAX UINT16_MAX
#define UINT_FAST32_MAX UINT32_MAX
#define UINT_FAST64_MAX UINT64_MAX
// 7.18.2.4 Limits of integer types capable of holding object pointers
#ifdef _WIN64 // [
# define INTPTR_MIN INT64_MIN
# define INTPTR_MAX INT64_MAX
# define UINTPTR_MAX UINT64_MAX
#define INTPTR_MIN INT64_MIN
#define INTPTR_MAX INT64_MAX
#define UINTPTR_MAX UINT64_MAX
#else // _WIN64 ][
# define INTPTR_MIN INT32_MIN
# define INTPTR_MAX INT32_MAX
# define UINTPTR_MAX UINT32_MAX
#define INTPTR_MIN INT32_MIN
#define INTPTR_MAX INT32_MAX
#define UINTPTR_MAX UINT32_MAX
#endif // _WIN64 ]
// 7.18.2.5 Limits of greatest-width integer types
#define INTMAX_MIN INT64_MIN
#define INTMAX_MAX INT64_MAX
#define UINTMAX_MAX UINT64_MAX
#define INTMAX_MIN INT64_MIN
#define INTMAX_MAX INT64_MAX
#define UINTMAX_MAX UINT64_MAX
// 7.18.3 Limits of other integer types
#ifdef _WIN64 // [
# define PTRDIFF_MIN _I64_MIN
# define PTRDIFF_MAX _I64_MAX
#else // _WIN64 ][
# define PTRDIFF_MIN _I32_MIN
# define PTRDIFF_MAX _I32_MAX
#endif // _WIN64 ]
#define PTRDIFF_MIN _I64_MIN
#define PTRDIFF_MAX _I64_MAX
#else // _WIN64 ][
#define PTRDIFF_MIN _I32_MIN
#define PTRDIFF_MAX _I32_MAX
#endif // _WIN64 ]
#define SIG_ATOMIC_MIN INT_MIN
#define SIG_ATOMIC_MAX INT_MAX
#define SIG_ATOMIC_MIN INT_MIN
#define SIG_ATOMIC_MAX INT_MAX
#ifndef SIZE_MAX // [
# ifdef _WIN64 // [
# define SIZE_MAX _UI64_MAX
# else // _WIN64 ][
# define SIZE_MAX _UI32_MAX
# endif // _WIN64 ]
#endif // SIZE_MAX ]
#ifdef _WIN64 // [
#define SIZE_MAX _UI64_MAX
#else // _WIN64 ][
#define SIZE_MAX _UI32_MAX
#endif // _WIN64 ]
#endif // SIZE_MAX ]
// WCHAR_MIN and WCHAR_MAX are also defined in <wchar.h>
#ifndef WCHAR_MIN // [
# define WCHAR_MIN 0
#endif // WCHAR_MIN ]
#define WCHAR_MIN 0
#endif // WCHAR_MIN ]
#ifndef WCHAR_MAX // [
# define WCHAR_MAX _UI16_MAX
#endif // WCHAR_MAX ]
#define WCHAR_MAX _UI16_MAX
#endif // WCHAR_MAX ]
#define WINT_MIN 0
#define WINT_MAX _UI16_MAX
#define WINT_MIN 0
#define WINT_MAX _UI16_MAX
#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
#define INT8_C(val) val##i8
#define INT8_C(val) val##i8
#define INT16_C(val) val##i16
#define INT32_C(val) val##i32
#define INT64_C(val) val##i64
#define UINT8_C(val) val##ui8
#define UINT8_C(val) val##ui8
#define UINT16_C(val) val##ui16
#define UINT32_C(val) val##ui32
#define UINT64_C(val) val##ui64
@@ -287,10 +288,10 @@ typedef uint64_t uintmax_t;
// These #ifndef's are needed to prevent collisions with <boost/cstdint.hpp>.
// Check out Issue 9 for the details.
#ifndef INTMAX_C // [
# define INTMAX_C INT64_C
#endif // INTMAX_C ]
#define INTMAX_C INT64_C
#endif // INTMAX_C ]
#ifndef UINTMAX_C // [
# define UINTMAX_C UINT64_C
#define UINTMAX_C UINT64_C
#endif // UINTMAX_C ]
#endif // __STDC_CONSTANT_MACROS ]

72
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.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// 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
// 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
//
// 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_
@@ -40,33 +44,43 @@ 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) {}
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; }
// 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;
}
private:
BasicOStreamWrapper(const BasicOStreamWrapper&);
BasicOStreamWrapper& operator=(const BasicOStreamWrapper&);
BasicOStreamWrapper(const BasicOStreamWrapper &);
BasicOStreamWrapper &operator=(const BasicOStreamWrapper &);
StreamType& stream_;
StreamType &stream_;
};
typedef BasicOStreamWrapper<std::ostream> OStreamWrapper;

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

File diff suppressed because it is too large Load Diff

435
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.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// 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
// 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
//
// 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_
@@ -24,7 +28,7 @@ RAPIDJSON_DIAG_OFF(effc++)
#if defined(__clang__)
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(c++98-compat)
RAPIDJSON_DIAG_OFF(c++ 98 - compat)
#endif
RAPIDJSON_NAMESPACE_BEGIN
@@ -33,8 +37,8 @@ RAPIDJSON_NAMESPACE_BEGIN
/*! \see PrettyWriter::SetFormatOptions
*/
enum PrettyFormatOptions {
kFormatDefault = 0, //!< Default pretty formatting.
kFormatSingleLineArray = 1 //!< Format arrays on a single line.
kFormatDefault = 0, //!< Default pretty formatting.
kFormatSingleLineArray = 1 //!< Format arrays on a single line.
};
//! Writer with indentation and spacing.
@@ -44,224 +48,275 @@ 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 typename Base::Ch Ch;
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.
*/
explicit PrettyWriter(OutputStream& os, StackAllocator* allocator = 0, size_t levelDepth = Base::kDefaultLevelDepth) :
Base(os, allocator, levelDepth), indentChar_(' '), indentCharCount_(4), formatOptions_(kFormatDefault) {}
//! 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.
*/
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.
*/
PrettyWriter& SetIndent(Ch indentChar, unsigned indentCharCount) {
RAPIDJSON_ASSERT(indentChar == ' ' || indentChar == '\t' || indentChar == '\n' || indentChar == '\r');
indentChar_ = indentChar;
indentCharCount_ = indentCharCount;
return *this;
}
//! 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.
*/
PrettyWriter &SetIndent(Ch indentChar, unsigned indentCharCount) {
RAPIDJSON_ASSERT(indentChar == ' ' || indentChar == '\t' ||
indentChar == '\n' || indentChar == '\r');
indentChar_ = indentChar;
indentCharCount_ = indentCharCount;
return *this;
}
//! Set pretty writer formatting options.
/*! \param options Formatting options.
*/
PrettyWriter& SetFormatOptions(PrettyFormatOptions options) {
formatOptions_ = options;
return *this;
}
//! Set pretty writer formatting options.
/*! \param options Formatting options.
*/
PrettyWriter &SetFormatOptions(PrettyFormatOptions options) {
formatOptions_ = options;
return *this;
}
/*! @name Implementation of Handler
\see Handler
*/
//@{
/*! @name Implementation of Handler
\see Handler
*/
//@{
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);
(void)copy;
PrettyPrefix(kNumberType);
return Base::EndValue(Base::WriteString(str, length));
}
bool RawNumber(const Ch *str, SizeType length, bool copy = false) {
RAPIDJSON_ASSERT(str != 0);
(void)copy;
PrettyPrefix(kNumberType);
return Base::EndValue(Base::WriteString(str, length));
}
bool String(const Ch* str, SizeType length, bool copy = false) {
RAPIDJSON_ASSERT(str != 0);
(void)copy;
PrettyPrefix(kStringType);
return Base::EndValue(Base::WriteString(str, length));
}
bool String(const Ch *str, SizeType length, bool copy = false) {
RAPIDJSON_ASSERT(str != 0);
(void)copy;
PrettyPrefix(kStringType);
return Base::EndValue(Base::WriteString(str, length));
}
#if RAPIDJSON_HAS_STDSTRING
bool String(const std::basic_string<Ch>& str) {
return String(str.data(), SizeType(str.size()));
}
bool String(const std::basic_string<Ch> &str) {
return String(str.data(), SizeType(str.size()));
}
#endif
bool StartObject() {
PrettyPrefix(kObjectType);
new (Base::level_stack_.template Push<typename Base::Level>()) typename Base::Level(false);
return Base::WriteStartObject();
}
bool StartObject() {
PrettyPrefix(kObjectType);
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) {
return Key(str.data(), SizeType(str.size()));
}
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(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;
if (!empty) {
Base::os_->Put('\n');
WriteIndent();
}
bool ret = Base::EndValue(Base::WriteEndObject());
(void)ret;
RAPIDJSON_ASSERT(ret == true);
if (Base::level_stack_.Empty()) // end of json text
Base::Flush();
return true;
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
bool empty =
Base::level_stack_.template Pop<typename Base::Level>(1)->valueCount ==
0;
if (!empty) {
Base::os_->Put('\n');
WriteIndent();
}
bool ret = Base::EndValue(Base::WriteEndObject());
(void)ret;
RAPIDJSON_ASSERT(ret == true);
if (Base::level_stack_.Empty()) // end of json text
Base::Flush();
return true;
}
bool StartArray() {
PrettyPrefix(kArrayType);
new (Base::level_stack_.template Push<typename Base::Level>()) typename Base::Level(true);
return Base::WriteStartArray();
bool StartArray() {
PrettyPrefix(kArrayType);
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;
if (!empty && !(formatOptions_ & kFormatSingleLineArray)) {
Base::os_->Put('\n');
WriteIndent();
}
bool ret = Base::EndValue(Base::WriteEndArray());
(void)ret;
RAPIDJSON_ASSERT(ret == true);
if (Base::level_stack_.Empty()) // end of json text
Base::Flush();
return true;
}
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;
//@}
if (!empty && !(formatOptions_ & kFormatSingleLineArray)) {
Base::os_->Put('\n');
WriteIndent();
}
bool ret = Base::EndValue(Base::WriteEndArray());
(void)ret;
RAPIDJSON_ASSERT(ret == true);
if (Base::level_stack_.Empty()) // end of json text
Base::Flush();
return true;
}
/*! @name Convenience extensions */
//@{
//@}
//! Simpler but slower overload.
bool String(const Ch *str) { return String(str, internal::StrLen(str)); }
bool Key(const Ch *str) { return Key(str, internal::StrLen(str)); }
/*! @name Convenience extensions */
//@{
//@}
//! Simpler but slower overload.
bool String(const Ch* str) { return String(str, internal::StrLen(str)); }
bool Key(const Ch* str) { return Key(str, internal::StrLen(str)); }
//! Write a raw JSON value.
/*!
For user to write a stringified JSON as a value.
//@}
//! Write a raw JSON value.
/*!
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.
*/
bool RawValue(const Ch* json, size_t length, Type type) {
RAPIDJSON_ASSERT(json != 0);
PrettyPrefix(type);
return Base::EndValue(Base::WriteRawValue(json, length));
}
\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);
PrettyPrefix(type);
return Base::EndValue(Base::WriteRawValue(json, length));
}
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>();
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>();
if (level->inArray) {
if (level->valueCount > 0) {
Base::os_->Put(','); // add comma if it is not the first element in array
if (formatOptions_ & kFormatSingleLineArray)
Base::os_->Put(' ');
}
if (!(formatOptions_ & kFormatSingleLineArray)) {
Base::os_->Put('\n');
WriteIndent();
}
}
else { // in object
if (level->valueCount > 0) {
if (level->valueCount % 2 == 0) {
Base::os_->Put(',');
Base::os_->Put('\n');
}
else {
Base::os_->Put(':');
Base::os_->Put(' ');
}
}
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
level->valueCount++;
if (level->inArray) {
if (level->valueCount > 0) {
Base::os_->Put(
','); // add comma if it is not the first element in array
if (formatOptions_ & kFormatSingleLineArray)
Base::os_->Put(' ');
}
else {
RAPIDJSON_ASSERT(!Base::hasRoot_); // Should only has one and only one root.
Base::hasRoot_ = true;
if (!(formatOptions_ & kFormatSingleLineArray)) {
Base::os_->Put('\n');
WriteIndent();
}
}
} else { // in object
if (level->valueCount > 0) {
if (level->valueCount % 2 == 0) {
Base::os_->Put(',');
Base::os_->Put('\n');
} else {
Base::os_->Put(':');
Base::os_->Put(' ');
}
} else
Base::os_->Put('\n');
void WriteIndent() {
size_t count = (Base::level_stack_.GetSize() / sizeof(typename Base::Level)) * indentCharCount_;
PutN(*Base::os_, static_cast<typename OutputStream::Ch>(indentChar_), count);
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
level->valueCount++;
} else {
RAPIDJSON_ASSERT(
!Base::hasRoot_); // Should only has one and only one root.
Base::hasRoot_ = true;
}
}
Ch indentChar_;
unsigned indentCharCount_;
PrettyFormatOptions formatOptions_;
void WriteIndent() {
size_t count =
(Base::level_stack_.GetSize() / sizeof(typename Base::Level)) *
indentCharCount_;
PutN(*Base::os_, static_cast<typename OutputStream::Ch>(indentChar_),
count);
}
Ch indentChar_;
unsigned indentCharCount_;
PrettyFormatOptions formatOptions_;
private:
// Prohibit copy constructor & assignment operator.
PrettyWriter(const PrettyWriter&);
PrettyWriter& operator=(const PrettyWriter&);
// Prohibit copy constructor & assignment operator.
PrettyWriter(const PrettyWriter &);
PrettyWriter &operator=(const PrettyWriter &);
};
RAPIDJSON_NAMESPACE_END

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

@@ -1,23 +1,27 @@
// 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.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// 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
// 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
//
// 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_
/*!\file rapidjson.h
\brief common definitions and configuration
\see RAPIDJSON_CONFIG
*/
@@ -29,20 +33,22 @@
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
different translation units of a single application.
*/
#include <cstdlib> // malloc(), realloc(), free(), size_t
#include <cstring> // memset(), memcpy(), memmove(), memcmp()
#include <cstdlib> // malloc(), realloc(), free(), size_t
#include <cstring> // memset(), memcpy(), memmove(), memcmp()
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_VERSION_STRING
//
// 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
@@ -75,8 +81,9 @@
#define RAPIDJSON_MAJOR_VERSION 1
#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)
#define RAPIDJSON_VERSION_STRING \
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,21 +163,21 @@
\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.
*/
#ifndef RAPIDJSON_NO_INT64DEFINE
//!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN
#if defined(_MSC_VER) && (_MSC_VER < 1800) // Visual Studio 2013
#include "msinttypes/stdint.h"
#if defined(_MSC_VER) && (_MSC_VER < 1800) // Visual Studio 2013
#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
@@ -195,59 +202,67 @@
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_ENDIAN
#define RAPIDJSON_LITTLEENDIAN 0 //!< Little endian machine
#define RAPIDJSON_BIGENDIAN 1 //!< Big endian machine
#define RAPIDJSON_LITTLEENDIAN 0 //!< Little endian machine
#define RAPIDJSON_BIGENDIAN 1 //!< Big endian machine
//! Endianness of the machine.
/*!
\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
// Detect with GCC 4.6's macro
# ifdef __BYTE_ORDER__
# if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__
# define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN
# elif __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__
# define RAPIDJSON_ENDIAN RAPIDJSON_BIGENDIAN
# else
#ifdef __BYTE_ORDER__
#if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__
#define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN
#elif __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__
#define RAPIDJSON_ENDIAN RAPIDJSON_BIGENDIAN
#else
# error Unknown machine endianness detected. User needs to define RAPIDJSON_ENDIAN.
# endif // __BYTE_ORDER__
#endif // __BYTE_ORDER__
// Detect with GLIBC's endian.h
# elif defined(__GLIBC__)
# include <endian.h>
# if (__BYTE_ORDER == __LITTLE_ENDIAN)
# define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN
# elif (__BYTE_ORDER == __BIG_ENDIAN)
# define RAPIDJSON_ENDIAN RAPIDJSON_BIGENDIAN
# else
#elif defined(__GLIBC__)
#include <endian.h>
#if (__BYTE_ORDER == __LITTLE_ENDIAN)
#define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN
#elif (__BYTE_ORDER == __BIG_ENDIAN)
#define RAPIDJSON_ENDIAN RAPIDJSON_BIGENDIAN
#else
# error Unknown machine endianness detected. User needs to define RAPIDJSON_ENDIAN.
# endif // __GLIBC__
#endif // __GLIBC__
// Detect with _LITTLE_ENDIAN and _BIG_ENDIAN macro
# elif defined(_LITTLE_ENDIAN) && !defined(_BIG_ENDIAN)
# define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN
# elif defined(_BIG_ENDIAN) && !defined(_LITTLE_ENDIAN)
# define RAPIDJSON_ENDIAN RAPIDJSON_BIGENDIAN
#elif defined(_LITTLE_ENDIAN) && !defined(_BIG_ENDIAN)
#define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN
#elif defined(_BIG_ENDIAN) && !defined(_LITTLE_ENDIAN)
#define RAPIDJSON_ENDIAN RAPIDJSON_BIGENDIAN
// Detect with architecture macros
# elif defined(__sparc) || defined(__sparc__) || defined(_POWER) || defined(__powerpc__) || defined(__ppc__) || defined(__hpux) || defined(__hppa) || defined(_MIPSEB) || defined(_POWER) || 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__)
# define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN
# elif defined(_MSC_VER) && (defined(_M_ARM) || defined(_M_ARM64))
# define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN
# elif defined(RAPIDJSON_DOXYGEN_RUNNING)
# define RAPIDJSON_ENDIAN
# else
# error Unknown machine endianness detected. User needs to define RAPIDJSON_ENDIAN.
# endif
#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__)
#define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN
#elif defined(_MSC_VER) && (defined(_M_ARM) || defined(_M_ARM64))
#define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN
#elif defined(RAPIDJSON_DOXYGEN_RUNNING)
#define RAPIDJSON_ENDIAN
#else
# error Unknown machine endianness detected. User needs to define RAPIDJSON_ENDIAN.
#endif
#endif // RAPIDJSON_ENDIAN
///////////////////////////////////////////////////////////////////////////////
@@ -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
@@ -411,9 +438,8 @@ 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))
#if __cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1800)
#define RAPIDJSON_STATIC_ASSERT(x) static_assert(x, RAPIDJSON_STRINGIFY(x))
#endif // C++11
#endif // RAPIDJSON_STATIC_ASSERT
@@ -424,14 +450,16 @@ 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
#if defined(__GNUC__) || defined(__clang__)
#define RAPIDJSON_STATIC_ASSERT_UNUSED_ATTRIBUTE __attribute__((unused))
#else
#define RAPIDJSON_STATIC_ASSERT_UNUSED_ATTRIBUTE
#define RAPIDJSON_STATIC_ASSERT_UNUSED_ATTRIBUTE
#endif
#ifndef __clang__
//!@endcond
@@ -442,10 +470,11 @@ RAPIDJSON_NAMESPACE_END
\param x compile-time condition
\hideinitializer
*/
#define RAPIDJSON_STATIC_ASSERT(x) \
typedef ::RAPIDJSON_NAMESPACE::StaticAssertTest< \
sizeof(::RAPIDJSON_NAMESPACE::STATIC_ASSERTION_FAILURE<bool(x) >)> \
RAPIDJSON_JOIN(StaticAssertTypedef, __LINE__) RAPIDJSON_STATIC_ASSERT_UNUSED_ATTRIBUTE
#define RAPIDJSON_STATIC_ASSERT(x) \
typedef ::RAPIDJSON_NAMESPACE::StaticAssertTest<sizeof( \
::RAPIDJSON_NAMESPACE::STATIC_ASSERTION_FAILURE<bool(x)>)> \
RAPIDJSON_JOIN(StaticAssertTypedef, __LINE__) \
RAPIDJSON_STATIC_ASSERT_UNUSED_ATTRIBUTE
#endif // RAPIDJSON_STATIC_ASSERT
///////////////////////////////////////////////////////////////////////////////
@@ -482,13 +511,13 @@ RAPIDJSON_NAMESPACE_END
//!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN
#define RAPIDJSON_MULTILINEMACRO_BEGIN do {
#define RAPIDJSON_MULTILINEMACRO_END \
} while((void)0, 0)
#define RAPIDJSON_MULTILINEMACRO_BEGIN do {
#define RAPIDJSON_MULTILINEMACRO_END \
} \
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)
@@ -500,24 +529,26 @@ RAPIDJSON_NAMESPACE_END
// RAPIDJSON_DIAG_PUSH/POP, RAPIDJSON_DIAG_OFF
#if defined(__GNUC__)
#define RAPIDJSON_GNUC \
RAPIDJSON_VERSION_CODE(__GNUC__,__GNUC_MINOR__,__GNUC_PATCHLEVEL__)
#define RAPIDJSON_GNUC \
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)
#define RAPIDJSON_DIAG_OFF(x) \
RAPIDJSON_DIAG_PRAGMA(ignored RAPIDJSON_STRINGIFY(RAPIDJSON_JOIN(-W,x)))
#define RAPIDJSON_DIAG_OFF(x) \
RAPIDJSON_DIAG_PRAGMA(ignored RAPIDJSON_STRINGIFY(RAPIDJSON_JOIN(-W, x)))
// push/pop support in Clang and GCC>=4.6
#if defined(__clang__) || (defined(RAPIDJSON_GNUC) && 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
#define RAPIDJSON_DIAG_POP RAPIDJSON_DIAG_PRAGMA(pop)
#else // GCC >= 4.2, < 4.6
#define RAPIDJSON_DIAG_PUSH /* ignored */
#define RAPIDJSON_DIAG_POP /* ignored */
#define RAPIDJSON_DIAG_POP /* ignored */
#endif
#elif defined(_MSC_VER)
@@ -526,9 +557,9 @@ RAPIDJSON_NAMESPACE_END
#define RAPIDJSON_PRAGMA(x) __pragma(x)
#define RAPIDJSON_DIAG_PRAGMA(x) RAPIDJSON_PRAGMA(warning(x))
#define RAPIDJSON_DIAG_OFF(x) RAPIDJSON_DIAG_PRAGMA(disable: x)
#define RAPIDJSON_DIAG_OFF(x) RAPIDJSON_DIAG_PRAGMA(disable : x)
#define RAPIDJSON_DIAG_PUSH RAPIDJSON_DIAG_PRAGMA(push)
#define RAPIDJSON_DIAG_POP RAPIDJSON_DIAG_PRAGMA(pop)
#define RAPIDJSON_DIAG_POP RAPIDJSON_DIAG_PRAGMA(pop)
#else
@@ -543,15 +574,19 @@ 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)
#if __has_feature(cxx_rvalue_references) && \
(defined(_MSC_VER) || defined(_LIBCPP_VERSION) || \
defined(__GLIBCXX__) && __GLIBCXX__ >= 20080306)
#define RAPIDJSON_HAS_CXX11_RVALUE_REFS 1
#else
#define RAPIDJSON_HAS_CXX11_RVALUE_REFS 0
#endif
#elif (defined(RAPIDJSON_GNUC) && (RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4,3,0)) && defined(__GXX_EXPERIMENTAL_CXX0X__)) || \
(defined(_MSC_VER) && _MSC_VER >= 1600) || \
(defined(__SUNPRO_CC) && __SUNPRO_CC >= 0x5140 && 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__))
#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__)) || \
(defined(_MSC_VER) && _MSC_VER >= 1900) || \
(defined(__SUNPRO_CC) && __SUNPRO_CC >= 0x5140 && 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__))
#define RAPIDJSON_HAS_CXX11_NOEXCEPT 1
#else
#define RAPIDJSON_HAS_CXX11_NOEXCEPT 0
@@ -574,7 +612,7 @@ RAPIDJSON_NAMESPACE_END
#define RAPIDJSON_NOEXCEPT noexcept
#else
#define RAPIDJSON_NOEXCEPT /* noexcept */
#endif // RAPIDJSON_HAS_CXX11_NOEXCEPT
#endif // RAPIDJSON_HAS_CXX11_NOEXCEPT
// no automatic detection, yet
#ifndef RAPIDJSON_HAS_CXX11_TYPETRAITS
@@ -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__)) || \
(defined(_MSC_VER) && _MSC_VER >= 1700) || \
(defined(__SUNPRO_CC) && __SUNPRO_CC >= 0x5140 && 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__))
#define RAPIDJSON_HAS_CXX11_RANGE_FOR 1
#else
#define RAPIDJSON_HAS_CXX11_RANGE_FOR 0
@@ -601,27 +642,27 @@ RAPIDJSON_NAMESPACE_END
// C++17 features
#if defined(__has_cpp_attribute)
# if __has_cpp_attribute(fallthrough)
# define RAPIDJSON_DELIBERATE_FALLTHROUGH [[fallthrough]]
# else
# define RAPIDJSON_DELIBERATE_FALLTHROUGH
# endif
#if __has_cpp_attribute(fallthrough)
#define RAPIDJSON_DELIBERATE_FALLTHROUGH [[fallthrough]]
#else
# define RAPIDJSON_DELIBERATE_FALLTHROUGH
#define RAPIDJSON_DELIBERATE_FALLTHROUGH
#endif
#else
#define RAPIDJSON_DELIBERATE_FALLTHROUGH
#endif
//!@endcond
//! Assertion (in non-throwing contexts).
/*! \ingroup RAPIDJSON_CONFIG
Some functions provide a \c noexcept guarantee, if the compiler supports it.
In these cases, the \ref RAPIDJSON_ASSERT macro cannot be overridden to
throw an exception. This macro adds a separate customization point for
such cases.
/*! \ingroup RAPIDJSON_CONFIG
Some functions provide a \c noexcept guarantee, if the compiler supports it.
In these cases, the \ref RAPIDJSON_ASSERT macro cannot be overridden to
throw an exception. This macro adds a separate customization point for
such cases.
Defaults to C \c assert() (as \ref RAPIDJSON_ASSERT), if \c noexcept is
supported, and to \ref RAPIDJSON_ASSERT otherwise.
*/
Defaults to C \c assert() (as \ref RAPIDJSON_ASSERT), if \c noexcept is
supported, and to \ref RAPIDJSON_ASSERT otherwise.
*/
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_NOEXCEPT_ASSERT
@@ -661,13 +702,13 @@ RAPIDJSON_NAMESPACE_BEGIN
//! Type of JSON value
enum Type {
kNullType = 0, //!< null
kFalseType = 1, //!< false
kTrueType = 2, //!< true
kObjectType = 3, //!< object
kArrayType = 4, //!< array
kStringType = 5, //!< string
kNumberType = 6 //!< number
kNullType = 0, //!< null
kFalseType = 1, //!< false
kTrueType = 2, //!< true
kObjectType = 3, //!< object
kArrayType = 4, //!< array
kStringType = 5, //!< string
kNumberType = 6 //!< number
};
RAPIDJSON_NAMESPACE_END

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

File diff suppressed because it is too large Load Diff

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

File diff suppressed because it is too large Load Diff

197
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,39 +70,40 @@ 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 {
//! 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>.
*/
enum { copyOptimization = 0 };
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>.
*/
enum { copyOptimization = 0 };
};
//! Reserve n characters for writing to a stream.
template<typename Stream>
inline void PutReserve(Stream& stream, size_t count) {
(void)stream;
(void)count;
template <typename Stream>
inline void PutReserve(Stream &stream, size_t count) {
(void)stream;
(void)count;
}
//! Write character to a stream, presuming buffer is reserved.
template<typename Stream>
inline void PutUnsafe(Stream& stream, typename Stream::Ch c) {
stream.Put(c);
template <typename Stream>
inline void PutUnsafe(Stream &stream, typename Stream::Ch c) {
stream.Put(c);
}
//! Put N copies of a character to a stream.
template<typename Stream, typename Ch>
inline void PutN(Stream& stream, Ch c, size_t n) {
PutReserve(stream, n);
for (size_t i = 0; i < n; i++)
PutUnsafe(stream, c);
template <typename Stream, typename Ch>
inline void PutN(Stream &stream, Ch c, size_t n) {
PutReserve(stream, n);
for (size_t i = 0; i < n; i++)
PutUnsafe(stream, c);
}
///////////////////////////////////////////////////////////////////////////////
@@ -111,33 +117,33 @@ inline void PutN(Stream& stream, Ch c, size_t n) {
#if defined(_MSC_VER) && _MSC_VER <= 1800
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(4702) // unreachable code
RAPIDJSON_DIAG_OFF(4512) // assignment operator could not be generated
RAPIDJSON_DIAG_OFF(4702) // unreachable code
RAPIDJSON_DIAG_OFF(4512) // assignment operator could not be generated
#endif
template <typename InputStream, typename Encoding = UTF8<> >
template <typename InputStream, typename Encoding = UTF8<>>
class GenericStreamWrapper {
public:
typedef typename Encoding::Ch Ch;
GenericStreamWrapper(InputStream& is): is_(is) {}
typedef typename Encoding::Ch Ch;
GenericStreamWrapper(InputStream &is) : is_(is) {}
Ch Peek() const { return is_.Peek(); }
Ch Take() { return is_.Take(); }
size_t Tell() { return is_.Tell(); }
Ch* PutBegin() { return is_.PutBegin(); }
void Put(Ch ch) { is_.Put(ch); }
void Flush() { is_.Flush(); }
size_t PutEnd(Ch* ch) { return is_.PutEnd(ch); }
Ch Peek() const { return is_.Peek(); }
Ch Take() { return is_.Take(); }
size_t Tell() { return is_.Tell(); }
Ch *PutBegin() { return is_.PutBegin(); }
void Put(Ch ch) { is_.Put(ch); }
void Flush() { is_.Flush(); }
size_t PutEnd(Ch *ch) { return is_.PutEnd(ch); }
// wrapper for MemoryStream
const Ch* Peek4() const { return is_.Peek4(); }
// wrapper for MemoryStream
const Ch *Peek4() const { return is_.Peek4(); }
// wrapper for AutoUTFInputStream
UTFType GetType() const { return is_.GetType(); }
bool HasBOM() const { return is_.HasBOM(); }
// wrapper for AutoUTFInputStream
UTFType GetType() const { return is_.GetType(); }
bool HasBOM() const { return is_.HasBOM(); }
protected:
InputStream& is_;
InputStream &is_;
};
#if defined(_MSC_VER) && _MSC_VER <= 1800
@@ -149,33 +155,38 @@ RAPIDJSON_DIAG_POP
//! Read-only string stream.
/*! \note implements Stream concept
*/
template <typename Encoding>
struct GenericStringStream {
typedef typename Encoding::Ch Ch;
*/
template <typename Encoding> struct GenericStringStream {
typedef typename Encoding::Ch Ch;
GenericStringStream(const Ch *src) : src_(src), head_(src) {}
GenericStringStream(const Ch *src) : src_(src), head_(src) {}
Ch Peek() const { return *src_; }
Ch Take() { return *src_++; }
size_t Tell() const { return static_cast<size_t>(src_ - head_); }
Ch Peek() const { return *src_; }
Ch Take() { return *src_++; }
size_t Tell() const { return static_cast<size_t>(src_ - head_); }
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; }
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;
}
const Ch* src_; //!< Current read position.
const Ch* head_; //!< Original head of the string.
const Ch *src_; //!< Current read position.
const Ch *head_; //!< Original head of the string.
};
template <typename Encoding>
struct StreamTraits<GenericStringStream<Encoding> > {
enum { copyOptimization = 1 };
struct StreamTraits<GenericStringStream<Encoding>> {
enum { copyOptimization = 1 };
};
//! String stream with UTF8 encoding.
typedef GenericStringStream<UTF8<> > StringStream;
typedef GenericStringStream<UTF8<>> StringStream;
///////////////////////////////////////////////////////////////////////////////
// InsituStringStream
@@ -184,39 +195,45 @@ typedef GenericStringStream<UTF8<> > StringStream;
/*! This string stream is particularly designed for in-situ parsing.
\note implements Stream concept
*/
template <typename Encoding>
struct GenericInsituStringStream {
typedef typename Encoding::Ch Ch;
template <typename Encoding> struct GenericInsituStringStream {
typedef typename Encoding::Ch Ch;
GenericInsituStringStream(Ch *src) : src_(src), dst_(0), head_(src) {}
GenericInsituStringStream(Ch *src) : src_(src), dst_(0), head_(src) {}
// Read
Ch Peek() { return *src_; }
Ch Take() { return *src_++; }
size_t Tell() { return static_cast<size_t>(src_ - head_); }
// Read
Ch Peek() { return *src_; }
Ch Take() { return *src_++; }
size_t Tell() { return static_cast<size_t>(src_ - head_); }
// Write
void Put(Ch c) { RAPIDJSON_ASSERT(dst_ != 0); *dst_++ = c; }
// Write
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 *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; }
void Pop(size_t count) { dst_ -= count; }
Ch *Push(size_t count) {
Ch *begin = dst_;
dst_ += count;
return begin;
}
void Pop(size_t count) { dst_ -= count; }
Ch* src_;
Ch* dst_;
Ch* head_;
Ch *src_;
Ch *dst_;
Ch *head_;
};
template <typename Encoding>
struct StreamTraits<GenericInsituStringStream<Encoding> > {
enum { copyOptimization = 1 };
struct StreamTraits<GenericInsituStringStream<Encoding>> {
enum { copyOptimization = 1 };
};
//! Insitu string stream with UTF8 encoding.
typedef GenericInsituStringStream<UTF8<> > InsituStringStream;
typedef GenericInsituStringStream<UTF8<>> InsituStringStream;
RAPIDJSON_NAMESPACE_END

128
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.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// 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
// 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
//
// 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
@@ -26,7 +30,7 @@
#if defined(__clang__)
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(c++98-compat)
RAPIDJSON_DIAG_OFF(c++ 98 - compat)
#endif
RAPIDJSON_NAMESPACE_BEGIN
@@ -40,76 +44,82 @@ RAPIDJSON_NAMESPACE_BEGIN
template <typename Encoding, typename Allocator = CrtAllocator>
class GenericStringBuffer {
public:
typedef typename Encoding::Ch Ch;
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& operator=(GenericStringBuffer&& rhs) {
if (&rhs != this)
stack_ = std::move(rhs.stack_);
return *this;
}
GenericStringBuffer(GenericStringBuffer &&rhs)
: stack_(std::move(rhs.stack_)) {}
GenericStringBuffer &operator=(GenericStringBuffer &&rhs) {
if (&rhs != this)
stack_ = std::move(rhs.stack_);
return *this;
}
#endif
void Put(Ch c) { *stack_.template Push<Ch>() = c; }
void PutUnsafe(Ch c) { *stack_.template PushUnsafe<Ch>() = c; }
void Flush() {}
void Put(Ch c) { *stack_.template Push<Ch>() = c; }
void PutUnsafe(Ch c) { *stack_.template PushUnsafe<Ch>() = c; }
void Flush() {}
void Clear() { stack_.Clear(); }
void ShrinkToFit() {
// Push and pop a null terminator. This is safe.
*stack_.template Push<Ch>() = '\0';
stack_.ShrinkToFit();
stack_.template Pop<Ch>(1);
}
void Clear() { stack_.Clear(); }
void ShrinkToFit() {
// Push and pop a null terminator. This is safe.
*stack_.template Push<Ch>() = '\0';
stack_.ShrinkToFit();
stack_.template Pop<Ch>(1);
}
void Reserve(size_t count) { stack_.template Reserve<Ch>(count); }
Ch* Push(size_t count) { return stack_.template Push<Ch>(count); }
Ch* PushUnsafe(size_t count) { return stack_.template PushUnsafe<Ch>(count); }
void Pop(size_t count) { stack_.template Pop<Ch>(count); }
void Reserve(size_t count) { stack_.template Reserve<Ch>(count); }
Ch *Push(size_t count) { return stack_.template Push<Ch>(count); }
Ch *PushUnsafe(size_t count) { return stack_.template PushUnsafe<Ch>(count); }
void Pop(size_t count) { stack_.template Pop<Ch>(count); }
const Ch* GetString() const {
// Push and pop a null terminator. This is safe.
*stack_.template Push<Ch>() = '\0';
stack_.template Pop<Ch>(1);
const Ch *GetString() const {
// Push and pop a null terminator. This is safe.
*stack_.template Push<Ch>() = '\0';
stack_.template Pop<Ch>(1);
return stack_.template Bottom<Ch>();
}
return stack_.template Bottom<Ch>();
}
//! Get the size of string in bytes in the string buffer.
size_t GetSize() const { return stack_.GetSize(); }
//! Get the size of string in bytes in the string buffer.
size_t GetSize() const { return stack_.GetSize(); }
//! Get the length of string in Ch in the string buffer.
size_t GetLength() const { return stack_.GetSize() / sizeof(Ch); }
//! Get the length of string in Ch in the string buffer.
size_t GetLength() const { return stack_.GetSize() / sizeof(Ch); }
static const size_t kDefaultCapacity = 256;
mutable internal::Stack<Allocator> stack_;
static const size_t kDefaultCapacity = 256;
mutable internal::Stack<Allocator> stack_;
private:
// Prohibit copy constructor & assignment operator.
GenericStringBuffer(const GenericStringBuffer&);
GenericStringBuffer& operator=(const GenericStringBuffer&);
// Prohibit copy constructor & assignment operator.
GenericStringBuffer(const GenericStringBuffer &);
GenericStringBuffer &operator=(const GenericStringBuffer &);
};
//! String buffer with UTF8 encoding
typedef GenericStringBuffer<UTF8<> > StringBuffer;
typedef GenericStringBuffer<UTF8<>> StringBuffer;
template<typename Encoding, typename Allocator>
inline void PutReserve(GenericStringBuffer<Encoding, Allocator>& stream, size_t count) {
stream.Reserve(count);
template <typename Encoding, typename Allocator>
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) {
stream.PutUnsafe(c);
template <typename Encoding, typename Allocator>
inline void PutUnsafe(GenericStringBuffer<Encoding, Allocator> &stream,
typename Encoding::Ch c) {
stream.PutUnsafe(c);
}
//! 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));
//! 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));
}
RAPIDJSON_NAMESPACE_END

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

File diff suppressed because it is too large Load Diff

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

File diff suppressed because it is too large Load Diff

228
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
{
public:
//! Iterator of child nodes of xml_node
template <class Ch> class node_iterator {
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)
{
}
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(xml_node<Ch> *node)
: m_node(node->first_node())
{
}
reference operator *() const
{
assert(m_node);
return *m_node;
}
node_iterator() : m_node(0) {}
pointer operator->() const
{
assert(m_node);
return m_node;
}
node_iterator(xml_node<Ch> *node) : m_node(node->first_node()) {}
node_iterator& operator++()
{
assert(m_node);
m_node = m_node->next_sibling();
return *this;
}
reference operator*() const {
assert(m_node);
return *m_node;
}
node_iterator operator++(int)
{
node_iterator tmp = *this;
++this;
return tmp;
}
pointer operator->() const {
assert(m_node);
return m_node;
}
node_iterator& operator--()
{
assert(m_node && m_node->previous_sibling());
m_node = m_node->previous_sibling();
return *this;
}
node_iterator &operator++() {
assert(m_node);
m_node = m_node->next_sibling();
return *this;
}
node_iterator operator--(int)
{
node_iterator tmp = *this;
++this;
return tmp;
}
node_iterator operator++(int) {
node_iterator tmp = *this;
++this;
return tmp;
}
bool operator ==(const node_iterator<Ch> &rhs)
{
return m_node == rhs.m_node;
}
node_iterator &operator--() {
assert(m_node && m_node->previous_sibling());
m_node = m_node->previous_sibling();
return *this;
}
bool operator !=(const node_iterator<Ch> &rhs)
{
return m_node != rhs.m_node;
}
node_iterator operator--(int) {
node_iterator tmp = *this;
++this;
return tmp;
}
private:
bool operator==(const node_iterator<Ch> &rhs) { return m_node == rhs.m_node; }
xml_node<Ch> *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
{
public:
//! Iterator of child attributes of xml_node
template <class Ch> class attribute_iterator {
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)
{
}
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(xml_node<Ch> *node)
: m_attribute(node->first_attribute())
{
}
reference operator *() const
{
assert(m_attribute);
return *m_attribute;
}
attribute_iterator() : m_attribute(0) {}
pointer operator->() const
{
assert(m_attribute);
return m_attribute;
}
attribute_iterator(xml_node<Ch> *node)
: m_attribute(node->first_attribute()) {}
attribute_iterator& operator++()
{
assert(m_attribute);
m_attribute = m_attribute->next_attribute();
return *this;
}
reference operator*() const {
assert(m_attribute);
return *m_attribute;
}
attribute_iterator operator++(int)
{
attribute_iterator tmp = *this;
++this;
return tmp;
}
pointer operator->() const {
assert(m_attribute);
return m_attribute;
}
attribute_iterator& operator--()
{
assert(m_attribute && m_attribute->previous_attribute());
m_attribute = m_attribute->previous_attribute();
return *this;
}
attribute_iterator &operator++() {
assert(m_attribute);
m_attribute = m_attribute->next_attribute();
return *this;
}
attribute_iterator operator--(int)
{
attribute_iterator tmp = *this;
++this;
return tmp;
}
attribute_iterator operator++(int) {
attribute_iterator tmp = *this;
++this;
return tmp;
}
bool operator ==(const attribute_iterator<Ch> &rhs)
{
return m_attribute == rhs.m_attribute;
}
attribute_iterator &operator--() {
assert(m_attribute && m_attribute->previous_attribute());
m_attribute = m_attribute->previous_attribute();
return *this;
}
bool operator !=(const attribute_iterator<Ch> &rhs)
{
return m_attribute != rhs.m_attribute;
}
attribute_iterator operator--(int) {
attribute_iterator tmp = *this;
++this;
return tmp;
}
private:
bool operator==(const attribute_iterator<Ch> &rhs) {
return m_attribute == rhs.m_attribute;
}
xml_attribute<Ch> *m_attribute;
bool operator!=(const attribute_iterator<Ch> &rhs) {
return m_attribute != rhs.m_attribute;
}
};
private:
xml_attribute<Ch> *m_attribute;
};
}
} // namespace rapidxml
#endif

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

@@ -10,412 +10,432 @@
// Only include streams if not disabled
#ifndef RAPIDXML_NO_STREAMS
#include <ostream>
#include <iterator>
#include <iterator>
#include <ostream>
#endif
namespace rapidxml
{
namespace rapidxml {
///////////////////////////////////////////////////////////////////////
// Printing flags
///////////////////////////////////////////////////////////////////////
// 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
///////////////////////////////////////////////////////////////////////
// Internal
//! \cond 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)
{
while (begin != end)
*out++ = *begin++;
return out;
}
// 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)
{
*out++ = *begin; // No expansion, copy character
}
else
{
switch (*begin)
{
case Ch('<'):
*out++ = Ch('&'); *out++ = Ch('l'); *out++ = Ch('t'); *out++ = Ch(';');
break;
case Ch('>'):
*out++ = Ch('&'); *out++ = Ch('g'); *out++ = Ch('t'); *out++ = Ch(';');
break;
case Ch('\''):
*out++ = Ch('&'); *out++ = Ch('a'); *out++ = Ch('p'); *out++ = Ch('o'); *out++ = Ch('s'); *out++ = Ch(';');
break;
case Ch('"'):
*out++ = Ch('&'); *out++ = Ch('q'); *out++ = Ch('u'); *out++ = Ch('o'); *out++ = Ch('t'); *out++ = Ch(';');
break;
case Ch('&'):
*out++ = Ch('&'); *out++ = Ch('a'); *out++ = Ch('m'); *out++ = Ch('p'); *out++ = Ch(';');
break;
default:
*out++ = *begin; // No expansion, copy character
}
}
++begin; // Step to next character
}
return out;
}
//! \cond internal
namespace internal {
// Fill given output iterator with repetitions of the same character
template<class OutIt, class Ch>
inline OutIt fill_chars(OutIt out, int n, Ch ch)
{
for (int i = 0; i < n; ++i)
*out++ = ch;
return out;
}
///////////////////////////////////////////////////////////////////////////
// Internal character operations
// Find character
template<class Ch, Ch ch>
inline bool find_char(const Ch *begin, const Ch *end)
{
while (begin != end)
if (*begin++ == ch)
return true;
return false;
}
///////////////////////////////////////////////////////////////////////////
// Internal printing operations
// Print node
template<class OutIt, class Ch>
inline OutIt print_node(OutIt out, const xml_node<Ch> *node, int flags, int indent)
{
// Print proper node type
switch (node->type())
{
// Document
case node_document:
out = print_children(out, node, flags, indent);
break;
// Element
case node_element:
out = print_element_node(out, node, flags, indent);
break;
// Data
case node_data:
out = print_data_node(out, node, flags, indent);
break;
// CDATA
case node_cdata:
out = print_cdata_node(out, node, flags, indent);
break;
// Declaration
case node_declaration:
out = print_declaration_node(out, node, flags, indent);
break;
// Comment
case node_comment:
out = print_comment_node(out, node, flags, indent);
break;
// Doctype
case node_doctype:
out = print_doctype_node(out, node, flags, indent);
break;
// Pi
case node_pi:
out = print_pi_node(out, node, flags, indent);
break;
// Unknown
default:
assert(0);
break;
}
// If indenting not disabled, add line break after node
if (!(flags & print_no_indenting))
*out = Ch('\n'), ++out;
// Return modified iterator
return out;
}
// 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())
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())
{
// Print attribute name
*out = Ch(' '), ++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()))
{
*out = Ch('\''), ++out;
out = copy_and_expand_chars(attribute->value(), attribute->value() + attribute->value_size(), Ch('"'), out);
*out = Ch('\''), ++out;
}
else
{
*out = Ch('"'), ++out;
out = copy_and_expand_chars(attribute->value(), attribute->value() + attribute->value_size(), Ch('\''), out);
*out = Ch('"'), ++out;
}
}
}
return out;
}
// Print data node
template<class OutIt, class Ch>
inline OutIt print_data_node(OutIt out, const xml_node<Ch> *node, int flags, int indent)
{
assert(node->type() == node_data);
if (!(flags & print_no_indenting))
out = fill_chars(out, indent, Ch('\t'));
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)
{
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 = copy_chars(node->value(), node->value() + node->value_size(), 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)
{
assert(node->type() == node_element);
// Print element name and attributes, if any
if (!(flags & print_no_indenting))
out = fill_chars(out, indent, Ch('\t'));
*out = Ch('<'), ++out;
out = copy_chars(node->name(), node->name() + node->name_size(), out);
out = print_attributes(out, node, flags);
// If node is childless
if (node->value_size() == 0 && !node->first_node())
{
// Print childless node tag ending
*out = Ch('/'), ++out;
*out = Ch('>'), ++out;
}
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 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)
{
// 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
{
// Print all children with full indenting
if (!(flags & print_no_indenting))
*out = Ch('\n'), ++out;
out = print_children(out, node, flags, indent + 1);
if (!(flags & print_no_indenting))
out = fill_chars(out, indent, Ch('\t'));
}
// Print node end
*out = Ch('<'), ++out;
*out = Ch('/'), ++out;
out = copy_chars(node->name(), node->name() + node->name_size(), out);
*out = Ch('>'), ++out;
}
return out;
}
// Print declaration node
template<class OutIt, class Ch>
inline OutIt print_declaration_node(OutIt out, const xml_node<Ch> *node, int flags, int indent)
{
// Print declaration start
if (!(flags & print_no_indenting))
out = fill_chars(out, indent, Ch('\t'));
*out = Ch('<'), ++out;
*out = Ch('?'), ++out;
*out = Ch('x'), ++out;
*out = Ch('m'), ++out;
*out = Ch('l'), ++out;
// Print attributes
out = print_attributes(out, node, flags);
// Print declaration end
*out = Ch('?'), ++out;
*out = Ch('>'), ++out;
return out;
}
// Print comment node
template<class OutIt, class Ch>
inline OutIt print_comment_node(OutIt out, const xml_node<Ch> *node, int flags, int indent)
{
assert(node->type() == node_comment);
if (!(flags & print_no_indenting))
out = fill_chars(out, indent, Ch('\t'));
*out = Ch('<'), ++out;
*out = Ch('!'), ++out;
*out = Ch('-'), ++out;
*out = Ch('-'), ++out;
out = copy_chars(node->value(), node->value() + node->value_size(), out);
*out = Ch('-'), ++out;
*out = Ch('-'), ++out;
*out = Ch('>'), ++out;
return out;
}
// Print doctype node
template<class OutIt, class Ch>
inline OutIt print_doctype_node(OutIt out, const xml_node<Ch> *node, int flags, int indent)
{
assert(node->type() == node_doctype);
if (!(flags & print_no_indenting))
out = fill_chars(out, indent, Ch('\t'));
*out = Ch('<'), ++out;
*out = Ch('!'), ++out;
*out = Ch('D'), ++out;
*out = Ch('O'), ++out;
*out = Ch('C'), ++out;
*out = Ch('T'), ++out;
*out = Ch('Y'), ++out;
*out = Ch('P'), ++out;
*out = Ch('E'), ++out;
*out = Ch(' '), ++out;
out = copy_chars(node->value(), node->value() + node->value_size(), out);
*out = Ch('>'), ++out;
return out;
}
// Print pi node
template<class OutIt, class Ch>
inline OutIt print_pi_node(OutIt out, const xml_node<Ch> *node, int flags, int indent)
{
assert(node->type() == node_pi);
if (!(flags & print_no_indenting))
out = fill_chars(out, indent, Ch('\t'));
*out = Ch('<'), ++out;
*out = Ch('?'), ++out;
out = copy_chars(node->name(), node->name() + node->name_size(), out);
*out = Ch(' '), ++out;
out = copy_chars(node->value(), node->value() + node->value_size(), out);
*out = Ch('?'), ++out;
*out = Ch('>'), ++out;
return out;
}
// Copy characters from given range to given output iterator
template <class OutIt, class Ch>
inline OutIt copy_chars(const Ch *begin, const Ch *end, OutIt out) {
while (begin != end)
*out++ = *begin++;
return out;
}
// 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) {
*out++ = *begin; // No expansion, copy character
} else {
switch (*begin) {
case Ch('<'):
*out++ = Ch('&');
*out++ = Ch('l');
*out++ = Ch('t');
*out++ = Ch(';');
break;
case Ch('>'):
*out++ = Ch('&');
*out++ = Ch('g');
*out++ = Ch('t');
*out++ = Ch(';');
break;
case Ch('\''):
*out++ = Ch('&');
*out++ = Ch('a');
*out++ = Ch('p');
*out++ = Ch('o');
*out++ = Ch('s');
*out++ = Ch(';');
break;
case Ch('"'):
*out++ = Ch('&');
*out++ = Ch('q');
*out++ = Ch('u');
*out++ = Ch('o');
*out++ = Ch('t');
*out++ = Ch(';');
break;
case Ch('&'):
*out++ = Ch('&');
*out++ = Ch('a');
*out++ = Ch('m');
*out++ = Ch('p');
*out++ = Ch(';');
break;
default:
*out++ = *begin; // No expansion, copy character
}
}
//! \endcond
++begin; // Step to next character
}
return out;
}
///////////////////////////////////////////////////////////////////////////
// Printing
// Fill given output iterator with repetitions of the same character
template <class OutIt, class Ch>
inline OutIt fill_chars(OutIt out, int n, Ch ch) {
for (int i = 0; i < n; ++i)
*out++ = ch;
return out;
}
//! Prints XML to given output iterator.
//! \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.
template<class OutIt, class Ch>
inline OutIt print(OutIt out, const xml_node<Ch> &node, int flags = 0)
{
return internal::print_node(out, &node, flags, 0);
// Find character
template <class Ch, Ch ch>
inline bool find_char(const Ch *begin, const Ch *end) {
while (begin != end)
if (*begin++ == ch)
return true;
return false;
}
///////////////////////////////////////////////////////////////////////////
// Internal printing operations
// Print node
template <class OutIt, class Ch>
inline OutIt print_node(OutIt out, const xml_node<Ch> *node, int flags,
int indent) {
// Print proper node type
switch (node->type()) {
// Document
case node_document:
out = print_children(out, node, flags, indent);
break;
// Element
case node_element:
out = print_element_node(out, node, flags, indent);
break;
// Data
case node_data:
out = print_data_node(out, node, flags, indent);
break;
// CDATA
case node_cdata:
out = print_cdata_node(out, node, flags, indent);
break;
// Declaration
case node_declaration:
out = print_declaration_node(out, node, flags, indent);
break;
// Comment
case node_comment:
out = print_comment_node(out, node, flags, indent);
break;
// Doctype
case node_doctype:
out = print_doctype_node(out, node, flags, indent);
break;
// Pi
case node_pi:
out = print_pi_node(out, node, flags, indent);
break;
// Unknown
default:
assert(0);
break;
}
// If indenting not disabled, add line break after node
if (!(flags & print_no_indenting))
*out = Ch('\n'), ++out;
// Return modified iterator
return out;
}
// 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())
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()) {
// Print attribute name
*out = Ch(' '), ++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())) {
*out = Ch('\''), ++out;
out = copy_and_expand_chars(
attribute->value(), attribute->value() + attribute->value_size(),
Ch('"'), out);
*out = Ch('\''), ++out;
} else {
*out = Ch('"'), ++out;
out = copy_and_expand_chars(
attribute->value(), attribute->value() + attribute->value_size(),
Ch('\''), out);
*out = Ch('"'), ++out;
}
}
}
return out;
}
// Print data node
template <class OutIt, class Ch>
inline OutIt print_data_node(OutIt out, const xml_node<Ch> *node, int flags,
int indent) {
assert(node->type() == node_data);
if (!(flags & print_no_indenting))
out = fill_chars(out, indent, Ch('\t'));
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) {
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 = copy_chars(node->value(), node->value() + node->value_size(), 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) {
assert(node->type() == node_element);
// Print element name and attributes, if any
if (!(flags & print_no_indenting))
out = fill_chars(out, indent, Ch('\t'));
*out = Ch('<'), ++out;
out = copy_chars(node->name(), node->name() + node->name_size(), out);
out = print_attributes(out, node, flags);
// If node is childless
if (node->value_size() == 0 && !node->first_node()) {
// Print childless node tag ending
*out = Ch('/'), ++out;
*out = Ch('>'), ++out;
} 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 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) {
// 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 {
// Print all children with full indenting
if (!(flags & print_no_indenting))
*out = Ch('\n'), ++out;
out = print_children(out, node, flags, indent + 1);
if (!(flags & print_no_indenting))
out = fill_chars(out, indent, Ch('\t'));
}
// Print node end
*out = Ch('<'), ++out;
*out = Ch('/'), ++out;
out = copy_chars(node->name(), node->name() + node->name_size(), out);
*out = Ch('>'), ++out;
}
return out;
}
// Print declaration node
template <class OutIt, class Ch>
inline OutIt print_declaration_node(OutIt out, const xml_node<Ch> *node,
int flags, int indent) {
// Print declaration start
if (!(flags & print_no_indenting))
out = fill_chars(out, indent, Ch('\t'));
*out = Ch('<'), ++out;
*out = Ch('?'), ++out;
*out = Ch('x'), ++out;
*out = Ch('m'), ++out;
*out = Ch('l'), ++out;
// Print attributes
out = print_attributes(out, node, flags);
// Print declaration end
*out = Ch('?'), ++out;
*out = Ch('>'), ++out;
return out;
}
// Print comment node
template <class OutIt, class Ch>
inline OutIt print_comment_node(OutIt out, const xml_node<Ch> *node, int flags,
int indent) {
assert(node->type() == node_comment);
if (!(flags & print_no_indenting))
out = fill_chars(out, indent, Ch('\t'));
*out = Ch('<'), ++out;
*out = Ch('!'), ++out;
*out = Ch('-'), ++out;
*out = Ch('-'), ++out;
out = copy_chars(node->value(), node->value() + node->value_size(), out);
*out = Ch('-'), ++out;
*out = Ch('-'), ++out;
*out = Ch('>'), ++out;
return out;
}
// Print doctype node
template <class OutIt, class Ch>
inline OutIt print_doctype_node(OutIt out, const xml_node<Ch> *node, int flags,
int indent) {
assert(node->type() == node_doctype);
if (!(flags & print_no_indenting))
out = fill_chars(out, indent, Ch('\t'));
*out = Ch('<'), ++out;
*out = Ch('!'), ++out;
*out = Ch('D'), ++out;
*out = Ch('O'), ++out;
*out = Ch('C'), ++out;
*out = Ch('T'), ++out;
*out = Ch('Y'), ++out;
*out = Ch('P'), ++out;
*out = Ch('E'), ++out;
*out = Ch(' '), ++out;
out = copy_chars(node->value(), node->value() + node->value_size(), out);
*out = Ch('>'), ++out;
return out;
}
// Print pi node
template <class OutIt, class Ch>
inline OutIt print_pi_node(OutIt out, const xml_node<Ch> *node, int flags,
int indent) {
assert(node->type() == node_pi);
if (!(flags & print_no_indenting))
out = fill_chars(out, indent, Ch('\t'));
*out = Ch('<'), ++out;
*out = Ch('?'), ++out;
out = copy_chars(node->name(), node->name() + node->name_size(), out);
*out = Ch(' '), ++out;
out = copy_chars(node->value(), node->value() + node->value_size(), out);
*out = Ch('?'), ++out;
*out = Ch('>'), ++out;
return out;
}
} // namespace internal
//! \endcond
///////////////////////////////////////////////////////////////////////////
// Printing
//! Prints XML to given output iterator.
//! \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.
template <class OutIt, class Ch>
inline OutIt print(OutIt out, const xml_node<Ch> &node, int flags = 0) {
return internal::print_node(out, &node, flags, 0);
}
#ifndef RAPIDXML_NO_STREAMS
//! Prints XML to given output stream.
//! \param out Output stream 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 stream.
template<class Ch>
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.
template<class Ch>
inline std::basic_ostream<Ch> &operator <<(std::basic_ostream<Ch> &out, const xml_node<Ch> &node)
{
return print(out, node);
}
#endif
//! Prints XML to given output stream.
//! \param out Output stream 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 stream.
template <class Ch>
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.
template <class Ch>
inline std::basic_ostream<Ch> &operator<<(std::basic_ostream<Ch> &out,
const xml_node<Ch> &node) {
return print(out, node);
}
#endif
} // namespace rapidxml
#endif

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

@@ -4,119 +4,97 @@
// 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
{
public:
//! 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;
//! Represents data loaded from a file
template <class Ch = char> class file {
// Open stream
basic_ifstream<Ch> stream(filename, ios::binary);
if (!stream)
throw runtime_error(string("cannot open file ") + filename);
stream.unsetf(ios::skipws);
// Determine stream size
stream.seekg(0, ios::end);
size_t size = stream.tellg();
stream.seekg(0);
// Load data and add terminating 0
m_data.resize(size + 1);
stream.read(&m_data.front(), static_cast<streamsize>(size));
m_data[size] = 0;
}
public:
//! 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;
//! 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;
// Open stream
basic_ifstream<Ch> stream(filename, ios::binary);
if (!stream)
throw runtime_error(string("cannot open file ") + filename);
stream.unsetf(ios::skipws);
// Load data and add terminating 0
stream.unsetf(ios::skipws);
m_data.assign(istreambuf_iterator<Ch>(stream), istreambuf_iterator<Ch>());
if (stream.fail() || stream.bad())
throw runtime_error("error reading stream");
m_data.push_back(0);
}
//! Gets file data.
//! \return Pointer to data of file.
Ch *data()
{
return &m_data.front();
}
// Determine stream size
stream.seekg(0, ios::end);
size_t size = stream.tellg();
stream.seekg(0);
//! Gets file data.
//! \return Pointer to data of file.
const Ch *data() const
{
return &m_data.front();
}
// Load data and add terminating 0
m_data.resize(size + 1);
stream.read(&m_data.front(), static_cast<streamsize>(size));
m_data[size] = 0;
}
//! Gets file data size.
//! \return Size of file data, in characters.
std::size_t size() const
{
return m_data.size();
}
//! 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;
private:
// Load data and add terminating 0
stream.unsetf(ios::skipws);
m_data.assign(istreambuf_iterator<Ch>(stream), istreambuf_iterator<Ch>());
if (stream.fail() || stream.bad())
throw runtime_error("error reading stream");
m_data.push_back(0);
}
std::vector<Ch> m_data; // File data
//! Gets file data.
//! \return Pointer to data of file.
Ch *data() { return &m_data.front(); }
};
//! Gets file data.
//! \return Pointer to data of file.
const Ch *data() const { return &m_data.front(); }
//! 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)
{
xml_node<Ch> *child = node->first_node();
std::size_t count = 0;
while (child)
{
++count;
child = child->next_sibling();
}
return count;
}
//! Gets file data size.
//! \return Size of file data, in characters.
std::size_t size() const { return m_data.size(); }
//! 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)
{
xml_attribute<Ch> *attr = node->first_attribute();
std::size_t count = 0;
while (attr)
{
++count;
attr = attr->next_attribute();
}
return count;
}
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) {
xml_node<Ch> *child = node->first_node();
std::size_t count = 0;
while (child) {
++count;
child = child->next_sibling();
}
return count;
}
//! 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) {
xml_attribute<Ch> *attr = node->first_attribute();
std::size_t count = 0;
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"/>

12
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)
#define LIVOX_ROS_DRIVER_VERSION_STRING \
GET_STRING(LIVOX_ROS_DRIVER_VER_MAJOR) \
"." GET_STRING(LIVOX_ROS_DRIVER_VER_MINOR) "." GET_STRING( \
LIVOX_ROS_DRIVER_VER_PATCH)
#endif

359
livox_ros_driver/livox_ros_driver/lddc.cpp
View File

@@ -24,37 +24,40 @@
#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),\
data_src_(data_src), output_type_(output_type), publish_frq_(frq) {
/** 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_;
publish_interval_ms_ = 1000 / publish_frq_;
lds_ = nullptr;
memset(private_pub_, 0, sizeof(private_pub_));
memset(private_imu_pub_, 0, sizeof(private_imu_pub_));
global_pub_ = nullptr;
global_imu_pub_ = nullptr;
cur_node_ = nullptr;
bag_ = nullptr;
cur_node_ = nullptr;
bag_ = nullptr;
};
Lddc::~Lddc() {
@@ -72,20 +75,21 @@ Lddc::~Lddc() {
lds_->PrepareExit();
}
for (uint32_t i=0; i<kMaxSourceLidar; i++) {
for (uint32_t i = 0; i < kMaxSourceLidar; i++) {
if (private_pub_[i]) {
delete private_pub_[i];
}
}
for (uint32_t i=0; i<kMaxSourceLidar; i++) {
for (uint32_t i = 0; i < kMaxSourceLidar; i++) {
if (private_imu_pub_[i]) {
delete private_imu_pub_[i];
}
}
}
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;
@@ -93,70 +97,89 @@ uint32_t Lddc::PublishPointcloud2(LidarDataQueue* queue, uint32_t packet_num, ui
cloud.header.frame_id = "livox_frame";
cloud.height = 1;
cloud.width = 0;
cloud.width = 0;
cloud.fields.resize(6);
cloud.fields[0].offset = 0;
cloud.fields[0].name = "x";
cloud.fields[0].count = 1;
cloud.fields[0].name = "x";
cloud.fields[0].count = 1;
cloud.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
cloud.fields[1].offset = 4;
cloud.fields[1].name = "y";
cloud.fields[1].count = 1;
cloud.fields[1].name = "y";
cloud.fields[1].count = 1;
cloud.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
cloud.fields[2].offset = 8;
cloud.fields[2].name = "z";
cloud.fields[2].count = 1;
cloud.fields[2].name = "z";
cloud.fields[2].count = 1;
cloud.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
cloud.fields[3].offset = 12;
cloud.fields[3].name = "intensity";
cloud.fields[3].count = 1;
cloud.fields[3].name = "intensity";
cloud.fields[3].count = 1;
cloud.fields[3].datatype = sensor_msgs::PointField::FLOAT32;
cloud.fields[4].offset = 16;
cloud.fields[4].name = "tag";
cloud.fields[4].count = 1;
cloud.fields[4].name = "tag";
cloud.fields[4].count = 1;
cloud.fields[4].datatype = sensor_msgs::PointField::UINT8;
cloud.fields[5].offset = 17;
cloud.fields[5].name = "line";
cloud.fields[5].count = 1;
cloud.fields[5].name = "line";
cloud.fields[5].count = 1;
cloud.fields[5].datatype = sensor_msgs::PointField::UINT8;
cloud.data.resize(packet_num * kMaxPointPerEthPacket * sizeof(LivoxPointXyzrtl));
cloud.point_step = 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) {
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)) {
if (kSourceLvxFile != data_source) {
ROS_INFO("Lidar[%d] packet loss", handle);
break;
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 %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);
@@ -166,67 +189,91 @@ uint32_t Lddc::PublishPointcloud2(LidarDataQueue* queue, uint32_t packet_num, ui
cloud.row_step = cloud.width * cloud.point_step;
cloud.is_bigendian = false;
cloud.is_dense = true;
cloud.data.resize(cloud.row_step); // adjust to the real size
cloud.is_dense = true;
cloud.data.resize(cloud.row_step); // adjust to the real size
ros::Publisher* p_publisher = Lddc::GetCurrentPublisher(handle);
ros::Publisher *p_publisher = Lddc::GetCurrentPublisher(handle);
if (kOutputToRos == output_type_) {
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) {
uint64_t timestamp = 0;
uint64_t last_timestamp = 0;
uint32_t published_packet = 0;
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;
/* init point cloud data struct */
PointCloud::Ptr cloud (new PointCloud);
PointCloud::Ptr cloud(new PointCloud);
cloud->header.frame_id = "livox_frame";
//cloud->header.stamp = ros::Time::now();
// cloud->header.stamp = ros::Time::now();
cloud->height = 1;
cloud->width = 0;
uint8_t data_source = lds_->lidars_[handle].data_src;
StoragePacket storage_packet;
while (published_packet < packet_num) {
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);
break;
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
cloud->header.stamp = timestamp / 1000.0; // to pcl ros time stamp
}
cloud->width += storage_packet.point_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);
}
LivoxPointXyzrtl* dst_point = (LivoxPointXyzrtl*)point_buf;
LivoxPointXyzrtl *dst_point = (LivoxPointXyzrtl *)point_buf;
for (uint32_t i = 0; i < storage_packet.point_num; i++) {
pcl::PointXYZI point;
point.x = dst_point->x;
@@ -242,20 +289,25 @@ uint32_t Lddc::PublishPointcloudData(LidarDataQueue* queue, uint32_t packet_num,
++published_packet;
}
ros::Publisher* p_publisher = Lddc::GetCurrentPublisher(handle);
ros::Publisher *p_publisher = Lddc::GetCurrentPublisher(handle);
if (kOutputToRos == output_type_) {
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;
@@ -267,34 +319,36 @@ uint32_t Lddc::PublishCustomPointcloud(LidarDataQueue* queue, uint32_t packet_nu
livox_msg.header.frame_id = "livox_frame";
livox_msg.header.seq = msg_seq;
++msg_seq;
//livox_msg.header.stamp = ros::Time::now();
// livox_msg.header.stamp = ros::Time::now();
livox_msg.timebase = 0;
livox_msg.point_num = 0;
livox_msg.lidar_id = handle;
livox_msg.lidar_id = handle;
uint8_t data_source = lds_->lidars_[handle].data_src;
StoragePacket storage_packet;
while (published_packet < packet_num) {
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
//ROS_DEBUG("[%d]:%ld %d", handle, livox_msg.timebase, point_interval);
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,33 +356,35 @@ 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);
}
LivoxPointXyzrtl* dst_point = (LivoxPointXyzrtl*)point_buf;
LivoxPointXyzrtl *dst_point = (LivoxPointXyzrtl *)point_buf;
for (uint32_t i = 0; i < storage_packet.point_num; i++) {
livox_ros_driver::CustomPoint point;
if (!dual_point) { /** dual return mode */
point.offset_time = packet_offset_time + i*point_interval;
point.offset_time = packet_offset_time + i * point_interval;
} else {
point.offset_time = packet_offset_time + (i/2)*point_interval;
point.offset_time = packet_offset_time + (i / 2) * point_interval;
}
point.x = dst_point->x;
point.y = dst_point->y;
point.z = dst_point->z;
point.reflectivity = dst_point->reflectivity;
point.tag = dst_point->tag;
point.tag = dst_point->tag;
point.line = dst_point->line;
++dst_point;
livox_msg.points.push_back(point);
@@ -339,19 +395,24 @@ uint32_t Lddc::PublishCustomPointcloud(LidarDataQueue* queue, uint32_t packet_nu
++published_packet;
}
ros::Publisher* p_publisher = Lddc::GetCurrentPublisher(handle);
ros::Publisher *p_publisher = Lddc::GetCurrentPublisher(handle);
if (kOutputToRos == output_type_) {
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;
}
uint32_t Lddc::PublishImuData(LidarDataQueue* queue, uint32_t packet_num,
uint32_t Lddc::PublishImuData(LidarDataQueue *queue, uint32_t packet_num,
uint8_t handle) {
uint64_t timestamp = 0;
uint32_t published_packet = 0;
@@ -362,14 +423,18 @@ 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);
LivoxImuPoint* imu = (LivoxImuPoint*)point_buf;
LivoxImuPoint *imu = (LivoxImuPoint *)point_buf;
imu_data.angular_velocity.x = imu->gyro_x;
imu_data.angular_velocity.y = imu->gyro_y;
imu_data.angular_velocity.z = imu->gyro_z;
@@ -380,20 +445,20 @@ uint32_t Lddc::PublishImuData(LidarDataQueue* queue, uint32_t packet_num,
QueuePopUpdate(queue);
++published_packet;
ros::Publisher* p_publisher = Lddc::GetCurrentImuPublisher(handle);
ros::Publisher *p_publisher = Lddc::GetCurrentImuPublisher(handle);
if (kOutputToRos == output_type_) {
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) {
int Lddc::RegisterLds(Lds *lds) {
if (lds_ == nullptr) {
lds_ = lds;
return 0;
@@ -402,42 +467,33 @@ int Lddc::RegisterLds(Lds* lds) {
}
}
void Lddc::PollingLidarPointCloudData(uint8_t handle, LidarDevice* lidar) {
LidarDataQueue* p_queue = &lidar->data;
if (p_queue == nullptr) {
void Lddc::PollingLidarPointCloudData(uint8_t handle, LidarDevice *lidar) {
LidarDataQueue *p_queue = &lidar->data;
if (p_queue == nullptr) {
return;
}
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);
}
}
}
void Lddc::PollingLidarImuData(uint8_t handle, LidarDevice* lidar) {
LidarDataQueue* p_queue = &lidar->imu_data;
if (p_queue == nullptr) {
void Lddc::PollingLidarImuData(uint8_t handle, LidarDevice *lidar) {
LidarDataQueue *p_queue = &lidar->imu_data;
if (p_queue == nullptr) {
return;
}
@@ -453,9 +509,10 @@ void Lddc::DistributeLidarData(void) {
for (uint32_t i = 0; i < lds_->lidar_count_; i++) {
uint32_t lidar_id = i;
LidarDevice* lidar = &lds_->lidars_[lidar_id];
LidarDataQueue* p_queue = &lidar->data;
if ((kConnectStateSampling!= lidar->connect_state) || (p_queue == nullptr)) {
LidarDevice *lidar = &lds_->lidars_[lidar_id];
LidarDataQueue *p_queue = &lidar->data;
if ((kConnectStateSampling != lidar->connect_state) ||
(p_queue == nullptr)) {
continue;
}
@@ -468,22 +525,22 @@ void Lddc::DistributeLidarData(void) {
}
}
ros::Publisher* Lddc::GetCurrentPublisher(uint8_t handle) {
ros::Publisher** pub = nullptr;
uint32_t queue_size = kMinEthPacketQueueSize * 4;
ros::Publisher *Lddc::GetCurrentPublisher(uint8_t handle) {
ros::Publisher **pub = nullptr;
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]", \
name_str, queue_size);
**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]", \
name_str, queue_size);
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);
}
}
@@ -513,15 +572,15 @@ ros::Publisher* Lddc::GetCurrentPublisher(uint8_t handle) {
return *pub;
}
ros::Publisher* Lddc::GetCurrentImuPublisher(uint8_t handle) {
ros::Publisher** pub = nullptr;
uint32_t queue_size = kMinEthPacketQueueSize * 4;
ros::Publisher *Lddc::GetCurrentImuPublisher(uint8_t handle) {
ros::Publisher **pub = nullptr;
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,14 +597,14 @@ 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) {
void Lddc::CreateBagFile(const std::string &file_name) {
if (!bag_) {
bag_ = new rosbag::Bag;
bag_->open(file_name, rosbag::bagmode::Write);
@@ -566,4 +625,4 @@ void Lddc::PrepareExit(void) {
}
}
}
} // namespace livox_ros

42
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>
@@ -44,34 +44,34 @@ public:
Lddc(int format, int multi_topic, int data_src, int output_type, double frq);
~Lddc();
int RegisterLds(Lds* lds);
int RegisterLds(Lds *lds);
void DistributeLidarData(void);
void CreateBagFile(const std::string& file_name);
void CreateBagFile(const std::string &file_name);
void PrepareExit(void);
uint8_t GetTransferFormat(void) { return transfer_format_; }
uint8_t IsMultiTopic(void) { return use_multi_topic_; }
void SetRosNode(ros::NodeHandle* node) { cur_node_ = node; }
void SetRosNode(ros::NodeHandle *node) { cur_node_ = node; }
void SetRosPub(ros::Publisher* pub) { global_pub_ = pub; };
void SetRosPub(ros::Publisher *pub) { global_pub_ = pub; };
void SetPublishFrq(uint32_t frq) { publish_frq_ = frq; }
Lds* lds_;
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);
ros::Publisher* GetCurrentImuPublisher(uint8_t handle);
void PollingLidarPointCloudData(uint8_t handle, LidarDevice* lidar);
void PollingLidarImuData(uint8_t handle, LidarDevice* lidar);
ros::Publisher *GetCurrentPublisher(uint8_t handle);
ros::Publisher *GetCurrentImuPublisher(uint8_t handle);
void PollingLidarPointCloudData(uint8_t handle, LidarDevice *lidar);
void PollingLidarImuData(uint8_t handle, LidarDevice *lidar);
uint8_t transfer_format_;
uint8_t use_multi_topic_;
@@ -79,14 +79,14 @@ private:
uint8_t output_type_;
double publish_frq_;
int32_t publish_interval_ms_;
ros::Publisher* private_pub_[kMaxSourceLidar];
ros::Publisher* global_pub_;
ros::Publisher* private_imu_pub_[kMaxSourceLidar];
ros::Publisher* global_imu_pub_;
ros::Publisher *private_pub_[kMaxSourceLidar];
ros::Publisher *global_pub_;
ros::Publisher *private_imu_pub_[kMaxSourceLidar];
ros::Publisher *global_imu_pub_;
ros::NodeHandle* cur_node_;
rosbag::Bag* bag_;
ros::NodeHandle *cur_node_;
rosbag::Bag *bag_;
};
}
} // namespace livox_ros
#endif

46
livox_ros_driver/livox_ros_driver/ldq.cpp
View File

@@ -24,24 +24,24 @@
#include "ldq.h"
#include <string.h>
#include <stdio.h>
#include <string.h>
namespace livox_ros {
/* for pointcloud queue process */
int InitQueue(LidarDataQueue* queue, uint32_t queue_size) {
int InitQueue(LidarDataQueue *queue, uint32_t queue_size) {
if (queue == nullptr) {
return 1;
}
if(IsPowerOf2(queue_size) != true) {
if (IsPowerOf2(queue_size) != true) {
queue_size = RoundupPowerOf2(queue_size);
}
queue->storage_packet = new StoragePacket[queue_size];
if(queue->storage_packet == nullptr) {
if (queue->storage_packet == nullptr) {
return 1;
}
@@ -53,14 +53,14 @@ int InitQueue(LidarDataQueue* queue, uint32_t queue_size) {
return 0;
}
int DeInitQueue(LidarDataQueue* queue) {
int DeInitQueue(LidarDataQueue *queue) {
if (queue == nullptr) {
return 1;
}
if (queue->storage_packet) {
delete [] queue->storage_packet;
delete[] queue->storage_packet;
}
queue->rd_idx = 0;
@@ -71,49 +71,47 @@ int DeInitQueue(LidarDataQueue* queue) {
return 0;
}
void ResetQueue(LidarDataQueue* queue) {
void ResetQueue(LidarDataQueue *queue) {
queue->rd_idx = 0;
queue->wr_idx = 0;
}
void QueueProPop(LidarDataQueue* queue, StoragePacket* storage_packet) {
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) {
uint32_t QueuePop(LidarDataQueue *queue, StoragePacket *storage_packet) {
QueueProPop(queue, storage_packet);
QueuePopUpdate(queue);
return 1;
}
uint32_t QueueUsedSize(LidarDataQueue* queue) {
uint32_t QueueUsedSize(LidarDataQueue *queue) {
return queue->wr_idx - queue->rd_idx;
}
uint32_t QueueUnusedSize(LidarDataQueue* queue) {
uint32_t QueueUnusedSize(LidarDataQueue *queue) {
return (queue->size - (queue->wr_idx - queue->rd_idx));
}
uint32_t QueueIsFull(LidarDataQueue* queue) {
uint32_t QueueIsFull(LidarDataQueue *queue) {
return ((queue->wr_idx - queue->rd_idx) > queue->mask);
}
uint32_t QueueIsEmpty(LidarDataQueue* queue) {
uint32_t QueueIsEmpty(LidarDataQueue *queue) {
return (queue->rd_idx == queue->wr_idx);
}
uint32_t QueuePush(LidarDataQueue* queue, StoragePacket* storage_packet) {
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,11 +119,11 @@ uint32_t QueuePush(LidarDataQueue* queue, StoragePacket* storage_packet) {
return 1;
}
uint32_t QueuePushAny(LidarDataQueue* queue, uint8_t *data, uint32_t length, \
uint64_t time_rcv, uint32_t point_num) {
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;
queue->storage_packet[wr_idx].time_rcv = time_rcv;
queue->storage_packet[wr_idx].time_rcv = time_rcv;
queue->storage_packet[wr_idx].point_num = point_num;
memcpy(queue->storage_packet[wr_idx].raw_data, data, length);
@@ -134,4 +132,4 @@ uint32_t QueuePushAny(LidarDataQueue* queue, uint8_t *data, uint32_t length, \
return 1;
}
}
} // namespace livox_ros

39
livox_ros_driver/livox_ros_driver/ldq.h
View File

@@ -31,15 +31,14 @@
namespace livox_ros {
const uint32_t KEthPacketMaxLength = 1500;
const uint32_t KEthPacketMaxLength = 1500;
#pragma pack(1)
typedef struct {
uint64_t time_rcv; /**< receive time when data arrive */
uint64_t time_rcv; /**< receive time when data arrive */
uint32_t point_num;
uint8_t raw_data[KEthPacketMaxLength];
uint8_t raw_data[KEthPacketMaxLength];
} StoragePacket;
#pragma pack()
@@ -49,7 +48,7 @@ typedef struct {
volatile uint32_t rd_idx;
volatile uint32_t wr_idx;
uint32_t mask;
uint32_t size; /**< must be power of 2. */
uint32_t size; /**< must be power of 2. */
} LidarDataQueue;
inline static bool IsPowerOf2(uint32_t size) {
@@ -58,9 +57,9 @@ inline static bool IsPowerOf2(uint32_t size) {
inline static uint32_t RoundupPowerOf2(uint32_t size) {
uint32_t power2_val = 0;
for(int i = 0; i < 32; i++) {
for (int i = 0; i < 32; i++) {
power2_val = ((uint32_t)1) << i;
if(size <= power2_val) {
if (size <= power2_val) {
break;
}
}
@@ -69,19 +68,19 @@ inline static uint32_t RoundupPowerOf2(uint32_t size) {
}
/** queue operate function */
int InitQueue(LidarDataQueue* queue, uint32_t queue_size);
int DeInitQueue(LidarDataQueue* queue);
void ResetQueue(LidarDataQueue* queue);
void QueueProPop(LidarDataQueue* queue, StoragePacket* storage_packet);
void QueuePopUpdate(LidarDataQueue* queue);
uint32_t QueuePop(LidarDataQueue* queue, StoragePacket* storage_packet);
uint32_t QueueUsedSize(LidarDataQueue* queue);
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, \
int InitQueue(LidarDataQueue *queue, uint32_t queue_size);
int DeInitQueue(LidarDataQueue *queue);
void ResetQueue(LidarDataQueue *queue);
void QueueProPop(LidarDataQueue *queue, StoragePacket *storage_packet);
void QueuePopUpdate(LidarDataQueue *queue);
uint32_t QueuePop(LidarDataQueue *queue, StoragePacket *storage_packet);
uint32_t QueueUsedSize(LidarDataQueue *queue);
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,
uint64_t time_rcv, uint32_t point_num);
}
} // namespace livox_ros
#endif

342
livox_ros_driver/livox_ros_driver/lds.cpp
View File

@@ -24,16 +24,17 @@
#include "lds.h"
#include <chrono>
#include <math.h>
#include <stdio.h>
#include <string.h>
#include <time.h>
#include <chrono>
namespace livox_ros {
/** Common function ------ ----------------------------------------------------------------------- */
bool IsFilePathValid(const char* path_str) {
/** Common function ------
* ----------------------------------------------------------------------- */
bool IsFilePathValid(const char *path_str) {
int str_len = strlen(path_str);
if ((str_len > kPathStrMinSize) && (str_len < kPathStrMaxSize)) {
@@ -43,9 +44,10 @@ bool IsFilePathValid(const char* path_str) {
}
}
uint64_t GetStoragePacketTimestamp(StoragePacket* packet, uint8_t data_src_) {
uint64_t GetStoragePacketTimestamp(StoragePacket *packet, uint8_t data_src_) {
LivoxEthPacket* raw_packet = reinterpret_cast<LivoxEthPacket *>(packet->raw_data);
LivoxEthPacket *raw_packet =
reinterpret_cast<LivoxEthPacket *>(packet->raw_data);
LdsStamp timestamp;
memcpy(timestamp.stamp_bytes, raw_packet->timestamp, sizeof(timestamp));
@@ -62,16 +64,16 @@ uint64_t GetStoragePacketTimestamp(StoragePacket* packet, uint8_t data_src_) {
} else if (raw_packet->timestamp_type == kTimestampTypePpsGps) {
struct tm time_utc;
time_utc.tm_isdst = 0;
time_utc.tm_year = raw_packet->timestamp[0] + 100; // map 2000 to 1990
time_utc.tm_mon = raw_packet->timestamp[1];
time_utc.tm_mday = raw_packet->timestamp[2];
time_utc.tm_hour = raw_packet->timestamp[3];
time_utc.tm_min = 0;
time_utc.tm_sec = 0;
time_utc.tm_year = raw_packet->timestamp[0] + 100; // map 2000 to 1990
time_utc.tm_mon = raw_packet->timestamp[1];
time_utc.tm_mday = raw_packet->timestamp[2];
time_utc.tm_hour = raw_packet->timestamp[3];
time_utc.tm_min = 0;
time_utc.tm_sec = 0;
uint64_t time_epoch = mktime(&time_utc);
time_epoch = time_epoch * 1000000 + timestamp.stamp_word.high; // to us
time_epoch = time_epoch * 1000; // to ns
time_epoch = time_epoch * 1000; // to ns
return time_epoch;
} else {
@@ -92,17 +94,17 @@ uint32_t CalculatePacketQueueSize(uint32_t interval_ms, uint32_t data_type) {
return queue_size;
}
void ParseCommandlineInputBdCode(const char* cammandline_str,
std::vector<std::string>& bd_code_list) {
char* strs = new char[strlen(cammandline_str) + 1];
void ParseCommandlineInputBdCode(const char *cammandline_str,
std::vector<std::string> &bd_code_list) {
char *strs = new char[strlen(cammandline_str) + 1];
strcpy(strs, cammandline_str);
std::string pattern = "&";
char* bd_str = strtok(strs, pattern.c_str());
char *bd_str = strtok(strs, pattern.c_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 {
@@ -111,16 +113,16 @@ void ParseCommandlineInputBdCode(const char* cammandline_str,
bd_str = strtok(NULL, pattern.c_str());
}
delete [] strs;
delete[] strs;
}
void EulerAnglesToRotationMatrix(EulerAngle euler, RotationMatrix matrix) {
double cos_roll = cos(static_cast<double>(euler[0]));
double cos_roll = cos(static_cast<double>(euler[0]));
double cos_pitch = cos(static_cast<double>(euler[1]));
double cos_yaw = cos(static_cast<double>(euler[2]));
double sin_roll = sin(static_cast<double>(euler[0]));
double cos_yaw = cos(static_cast<double>(euler[2]));
double sin_roll = sin(static_cast<double>(euler[0]));
double sin_pitch = sin(static_cast<double>(euler[1]));
double sin_yaw = sin(static_cast<double>(euler[2]));
double sin_yaw = sin(static_cast<double>(euler[2]));
matrix[0][0] = cos_pitch * cos_yaw;
matrix[0][1] = sin_roll * sin_pitch * cos_yaw - cos_roll * sin_yaw;
@@ -134,58 +136,57 @@ void EulerAnglesToRotationMatrix(EulerAngle euler, RotationMatrix matrix) {
matrix[2][1] = sin_roll * cos_pitch;
matrix[2][2] = cos_roll * cos_pitch;
/*
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::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.pitch),
std::sin(info.roll) * std::cos(info.pitch),
std::cos(info.roll) * std::cos(info.pitch)
}
};
*/
}
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];
}
/** 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);
LivoxPoint* raw_point = reinterpret_cast<LivoxPoint *>(eth_packet->data);
while(points_per_packet) {
RawPointConvert((LivoxPointXyzr*)dst_point, raw_point);
if (extrinsic.enable) {
PointXyz src_point = *((PointXyz*)dst_point);
PointExtrisincCompensation((PointXyz*)dst_point, src_point, extrinsic);
/*
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::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.pitch),
std::sin(info.roll) * std::cos(info.pitch),
std::cos(info.roll) * std::cos(info.pitch)
}
dst_point->tag = 0;
};
*/
}
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];
}
/** 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);
LivoxPoint *raw_point = reinterpret_cast<LivoxPoint *>(eth_packet->data);
while (points_per_packet) {
RawPointConvert((LivoxPointXyzr *)dst_point, raw_point);
if (extrinsic.enable) {
PointXyz src_point = *((PointXyz *)dst_point);
PointExtrisincCompensation((PointXyz *)dst_point, src_point, extrinsic);
}
dst_point->tag = 0;
dst_point->line = 0;
++raw_point;
++dst_point;
@@ -195,19 +196,20 @@ uint8_t* LivoxPointToPxyzrtl(uint8_t* point_buf, LivoxEthPacket* eth_packet, \
return (uint8_t *)dst_point;
}
uint8_t* LivoxRawPointToPxyzrtl(uint8_t* point_buf, LivoxEthPacket* eth_packet, \
ExtrinsicParameter& extrinsic) {
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);
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);
while(points_per_packet) {
RawPointConvert((LivoxPointXyzr*)dst_point, raw_point);
while (points_per_packet) {
RawPointConvert((LivoxPointXyzr *)dst_point, raw_point);
if (extrinsic.enable) {
PointXyz src_point = *((PointXyz*)dst_point);
PointExtrisincCompensation((PointXyz*)dst_point, src_point, extrinsic);
PointXyz src_point = *((PointXyz *)dst_point);
PointExtrisincCompensation((PointXyz *)dst_point, src_point, extrinsic);
}
dst_point->tag = 0;
dst_point->tag = 0;
dst_point->line = 0;
++raw_point;
++dst_point;
@@ -217,19 +219,21 @@ uint8_t* LivoxRawPointToPxyzrtl(uint8_t* point_buf, LivoxEthPacket* eth_packet,
return (uint8_t *)dst_point;
}
uint8_t* LivoxSpherPointToPxyzrtl(uint8_t* point_buf, LivoxEthPacket* eth_packet, \
ExtrinsicParameter& extrinsic) {
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);
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);
while(points_per_packet) {
RawPointConvert((LivoxPointXyzr*)dst_point, raw_point);
while (points_per_packet) {
RawPointConvert((LivoxPointXyzr *)dst_point, raw_point);
if (extrinsic.enable) {
PointXyz src_point = *((PointXyz*)dst_point);
PointExtrisincCompensation((PointXyz*)dst_point, src_point, extrinsic);
PointXyz src_point = *((PointXyz *)dst_point);
PointExtrisincCompensation((PointXyz *)dst_point, src_point, extrinsic);
}
dst_point->tag = 0;
dst_point->tag = 0;
dst_point->line = 0;
++raw_point;
++dst_point;
@@ -239,21 +243,22 @@ 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, \
ExtrinsicParameter& extrinsic) {
LivoxPointXyzrtl* dst_point = (LivoxPointXyzrtl *)point_buf;
uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type);
LivoxExtendRawPoint* raw_point = \
reinterpret_cast<LivoxExtendRawPoint *>(eth_packet->data);
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 =
reinterpret_cast<LivoxExtendRawPoint *>(eth_packet->data);
uint8_t line_id = 0;
while(points_per_packet) {
RawPointConvert((LivoxPointXyzr*)dst_point, (LivoxRawPoint *)raw_point);
while (points_per_packet) {
RawPointConvert((LivoxPointXyzr *)dst_point, (LivoxRawPoint *)raw_point);
if (extrinsic.enable) {
PointXyz src_point = *((PointXyz*)dst_point);
PointExtrisincCompensation((PointXyz*)dst_point, src_point, extrinsic);
PointXyz src_point = *((PointXyz *)dst_point);
PointExtrisincCompensation((PointXyz *)dst_point, src_point, extrinsic);
}
dst_point->tag = raw_point->tag;
dst_point->tag = raw_point->tag;
dst_point->line = line_id;
dst_point->line = dst_point->line % 6;
++raw_point;
@@ -265,20 +270,22 @@ 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, \
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);
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);
uint8_t line_id = 0;
while(points_per_packet) {
RawPointConvert((LivoxPointXyzr*)dst_point, (LivoxSpherPoint *)raw_point);
while (points_per_packet) {
RawPointConvert((LivoxPointXyzr *)dst_point, (LivoxSpherPoint *)raw_point);
if (extrinsic.enable) {
PointXyz src_point = *((PointXyz*)dst_point);
PointExtrisincCompensation((PointXyz*)dst_point, src_point, extrinsic);
PointXyz src_point = *((PointXyz *)dst_point);
PointExtrisincCompensation((PointXyz *)dst_point, src_point, extrinsic);
}
dst_point->tag = raw_point->tag;
dst_point->tag = raw_point->tag;
dst_point->line = line_id;
dst_point->line = dst_point->line % 6;
++raw_point;
@@ -290,22 +297,24 @@ uint8_t* LivoxExtendSpherPointToPxyzrtl(uint8_t* point_buf, LivoxEthPacket* eth
return (uint8_t *)dst_point;
}
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 = \
reinterpret_cast<LivoxExtendRawPoint *>(eth_packet->data);
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 =
reinterpret_cast<LivoxExtendRawPoint *>(eth_packet->data);
uint8_t line_id = 0;
while(points_per_packet) {
RawPointConvert((LivoxPointXyzr*)dst_point, (LivoxRawPoint *)raw_point);
while (points_per_packet) {
RawPointConvert((LivoxPointXyzr *)dst_point, (LivoxRawPoint *)raw_point);
if (extrinsic.enable) {
PointXyz src_point = *((PointXyz*)dst_point);
PointExtrisincCompensation((PointXyz*)dst_point, src_point, extrinsic);
PointXyz src_point = *((PointXyz *)dst_point);
PointExtrisincCompensation((PointXyz *)dst_point, src_point, extrinsic);
}
dst_point->tag = raw_point->tag;
dst_point->line = line_id / 2; /* LivoxDualExtendRawPoint = 2*LivoxExtendRawPoint */
dst_point->tag = raw_point->tag;
dst_point->line =
line_id / 2; /* LivoxDualExtendRawPoint = 2*LivoxExtendRawPoint */
dst_point->line = dst_point->line % 6;
++raw_point;
++dst_point;
@@ -316,31 +325,33 @@ uint8_t* LivoxDualExtendRawPointToPxyzrtl(uint8_t* point_buf, LivoxEthPacket* et
return (uint8_t *)dst_point;
}
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 = \
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 =
reinterpret_cast<LivoxDualExtendSpherPoint *>(eth_packet->data);
uint8_t line_id = 0;
while(points_per_packet) {
RawPointConvert((LivoxPointXyzr*)dst_point, (LivoxPointXyzr *)(dst_point + 1), \
while (points_per_packet) {
RawPointConvert((LivoxPointXyzr *)dst_point,
(LivoxPointXyzr *)(dst_point + 1),
(LivoxDualExtendSpherPoint *)raw_point);
if (extrinsic.enable) {
PointXyz src_point = *((PointXyz*)dst_point);
PointExtrisincCompensation((PointXyz*)dst_point, src_point, extrinsic);
PointXyz src_point = *((PointXyz *)dst_point);
PointExtrisincCompensation((PointXyz *)dst_point, src_point, extrinsic);
}
dst_point->tag = raw_point->tag1;
dst_point->tag = raw_point->tag1;
dst_point->line = line_id;
dst_point->line = dst_point->line % 6;
++dst_point;
if (extrinsic.enable) {
PointXyz src_point = *((PointXyz*)dst_point);
PointExtrisincCompensation((PointXyz*)dst_point, src_point, extrinsic);
PointXyz src_point = *((PointXyz *)dst_point);
PointExtrisincCompensation((PointXyz *)dst_point, src_point, extrinsic);
}
dst_point->tag = raw_point->tag2;
dst_point->tag = raw_point->tag2;
dst_point->line = line_id;
dst_point->line = dst_point->line % 6;
++dst_point;
@@ -353,20 +364,19 @@ uint8_t* LivoxDualExtendSpherPointToPxyzrtl(uint8_t* point_buf, LivoxEthPacket*
return (uint8_t *)dst_point;
}
uint8_t* LivoxImuDataProcess(uint8_t* point_buf, LivoxEthPacket* eth_packet) {
uint8_t *LivoxImuDataProcess(uint8_t *point_buf, LivoxEthPacket *eth_packet) {
memcpy(point_buf, eth_packet->data, sizeof(LivoxImuPoint));
return point_buf;
}
const PointConvertHandler to_pxyzi_handler_table[kMaxPointDataType] = {
LivoxRawPointToPxyzrtl,
LivoxSpherPointToPxyzrtl,
LivoxExtendRawPointToPxyzrtl,
LivoxExtendSpherPointToPxyzrtl,
LivoxDualExtendRawPointToPxyzrtl,
LivoxDualExtendSpherPointToPxyzrtl,
nullptr
};
LivoxRawPointToPxyzrtl,
LivoxSpherPointToPxyzrtl,
LivoxExtendRawPointToPxyzrtl,
LivoxExtendSpherPointToPxyzrtl,
LivoxDualExtendRawPointToPxyzrtl,
LivoxDualExtendSpherPointToPxyzrtl,
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,11 +414,12 @@ 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) {
lidar_count_ = kMaxSourceLidar;
lidar_count_ = kMaxSourceLidar;
request_exit_ = false;
ResetLds(data_src_);
};
@@ -402,7 +429,7 @@ Lds::~Lds() {
ResetLds(0);
};
void Lds::ResetLidar(LidarDevice* lidar, uint8_t data_src) {
void Lds::ResetLidar(LidarDevice *lidar, uint8_t data_src) {
DeInitQueue(&lidar->data);
DeInitQueue(&lidar->imu_data);
@@ -410,17 +437,18 @@ 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) {
void Lds::SetLidarDataSrc(LidarDevice *lidar, uint8_t data_src) {
lidar->data_src = data_src;
}
void Lds::ResetLds(uint8_t data_src) {
lidar_count_ = kMaxSourceLidar;
for (uint32_t i=0; i<kMaxSourceLidar; i++) {
for (uint32_t i = 0; i < kMaxSourceLidar; i++) {
ResetLidar(&lidars_[i], data_src);
}
}
@@ -433,8 +461,6 @@ uint8_t Lds::GetDeviceType(uint8_t handle) {
}
}
void Lds::PrepareExit(void) {
void Lds::PrepareExit(void) {}
}
}
} // namespace livox_ros

187
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"
@@ -44,27 +44,29 @@ namespace livox_ros {
const uint32_t kMaxSourceLidar = 32;
/** Eth packet relative info parama */
const uint32_t kMaxPointPerEthPacket = 100;
const uint32_t kMinEthPacketQueueSize = 32; /**< must be 2^n */
const uint32_t kMaxEthPacketQueueSize = 8192; /**< must be 2^n */
const uint32_t kImuEthPacketQueueSize = 256;
const uint32_t kMaxPointPerEthPacket = 100;
const uint32_t kMinEthPacketQueueSize = 32; /**< must be 2^n */
const uint32_t kMaxEthPacketQueueSize = 8192; /**< must be 2^n */
const uint32_t kImuEthPacketQueueSize = 256;
const uint32_t KEthPacketHeaderLength = 18; /**< (sizeof(LivoxEthPacket) - 1) */
//const uint32_t KEthPacketMaxLength = 1500;
const uint32_t KCartesianPointSize = 13;
const uint32_t KSphericalPointSzie = 9;
const uint32_t KEthPacketHeaderLength = 18; /**< (sizeof(LivoxEthPacket) - 1) */
// const uint32_t KEthPacketMaxLength = 1500;
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 kNsPerSecond = 1000000000; /**< 1s = 1000000000ns */
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 kNsPerSecond = 1000000000; /**< 1s = 1000000000ns */
const int kPathStrMinSize = 4; /**< Must more than 4 char */
const int kPathStrMaxSize = 256; /**< Must less than 256 char */
const int kBdCodeSize = 15;
const int kPathStrMinSize = 4; /**< Must more than 4 char */
const int kPathStrMaxSize = 256; /**< Must less than 256 char */
const int kBdCodeSize = 15;
const uint32_t kPointXYZRSize = 16;
const uint32_t kPointXYZRTRSize = 18;
const uint32_t kPointXYZRSize = 16;
const uint32_t kPointXYZRTRSize = 18;
const double PI = 3.14159265358979323846;
@@ -78,9 +80,9 @@ typedef enum {
/** Device data source type */
typedef enum {
kSourceRawLidar = 0, /**< Data from raw lidar. */
kSourceRawHub = 1, /**< Data from lidar hub. */
kSourceLvxFile, /**< Data from parse lvx file. */
kSourceRawLidar = 0, /**< Data from raw lidar. */
kSourceRawHub = 1, /**< Data from lidar hub. */
kSourceLvxFile, /**< Data from parse lvx file. */
kSourceUndef,
} LidarDataSourceType;
@@ -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,
@@ -114,9 +113,9 @@ typedef struct {
uint32_t receive_packet_count;
uint32_t loss_packet_count;
int64_t last_timestamp;
int64_t timebase; /**< unit:ns */
int64_t timebase; /**< unit:ns */
int64_t last_imu_timestamp;
int64_t imu_timebase; /**< unit:ns */
int64_t imu_timebase; /**< unit:ns */
uint32_t timebase_state;
} LidarPacketStatistic;
@@ -152,8 +151,8 @@ typedef struct {
volatile uint32_t get_bits;
} UserConfig;
typedef float EulerAngle[3]; /**< Roll, Pitch, Yaw, unit:radian. */
typedef float TranslationVector[3]; /**< x, y, z translation, unit: m. */
typedef float EulerAngle[3]; /**< Roll, Pitch, Yaw, unit:radian. */
typedef float TranslationVector[3]; /**< x, y, z translation, unit: m. */
typedef float RotationMatrix[3][3];
typedef struct {
@@ -165,128 +164,135 @@ typedef struct {
/** Lidar data source info abstract */
typedef struct {
uint8_t handle; /**< Lidar access handle. */
uint8_t data_src; /**< From raw lidar or livox file. */
uint8_t 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;
LidarDataQueue data;
LidarDataQueue imu_data;
uint32_t firmware_ver; /**< Firmware version of lidar */
uint32_t firmware_ver; /**< Firmware version of lidar */
UserConfig config;
ExtrinsicParameter extrinsic_parameter;
} LidarDevice;
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;
#pragma pack(1)
typedef struct{
float x; /**< X axis, Unit:m */
float y; /**< Y axis, Unit:m */
float z; /**< Z axis, Unit:m */
typedef struct {
float x; /**< X axis, Unit:m */
float y; /**< Y axis, Unit:m */
float z; /**< Z axis, Unit:m */
} PointXyz;
typedef struct{
float x; /**< X axis, Unit:m */
float y; /**< Y axis, Unit:m */
float z; /**< Z axis, Unit:m */
float reflectivity; /**< Reflectivity */
typedef struct {
float x; /**< X axis, Unit:m */
float y; /**< Y axis, Unit:m */
float z; /**< Z axis, Unit:m */
float reflectivity; /**< Reflectivity */
} LivoxPointXyzr;
typedef struct{
float x; /**< X axis, Unit:m */
float y; /**< Y axis, Unit:m */
float z; /**< Z axis, Unit:m */
float reflectivity; /**< Reflectivity */
uint8_t tag; /**< Livox point tag */
uint8_t line; /**< Laser line id */
typedef struct {
float x; /**< X axis, Unit:m */
float y; /**< Y axis, Unit:m */
float z; /**< Z axis, Unit:m */
float reflectivity; /**< Reflectivity */
uint8_t tag; /**< Livox point tag */
uint8_t line; /**< Laser line id */
} LivoxPointXyzrtl;
#pragma pack()
typedef uint8_t* (* PointConvertHandler)(uint8_t* point_buf, LivoxEthPacket* eth_packet, \
ExtrinsicParameter& extrinsic);
typedef uint8_t *(*PointConvertHandler)(uint8_t *point_buf,
LivoxEthPacket *eth_packet,
ExtrinsicParameter &extrinsic);
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.
*/
bool IsFilePathValid(const char* path_str);
uint64_t GetStoragePacketTimestamp(StoragePacket* packet, uint8_t data_src_);
bool IsFilePathValid(const char *path_str);
uint64_t GetStoragePacketTimestamp(StoragePacket *packet, uint8_t data_src_);
uint32_t CalculatePacketQueueSize(uint32_t interval_ms, uint32_t data_type);
void ParseCommandlineInputBdCode(const char* cammandline_str,
std::vector<std::string>& bd_code_list);
void ParseCommandlineInputBdCode(const char *cammandline_str,
std::vector<std::string> &bd_code_list);
PointConvertHandler GetConvertHandler(uint8_t data_type);
uint8_t* LivoxPointToPxyzrtl(uint8_t* point_buf, LivoxEthPacket* eth_packet, \
ExtrinsicParameter& extrinsic);
uint8_t* LivoxImuDataProcess(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;
}
inline void RawPointConvert(LivoxPointXyzr* dst_point, LivoxPoint* raw_point) {
inline void RawPointConvert(LivoxPointXyzr *dst_point, LivoxPoint *raw_point) {
dst_point->x = raw_point->x;
dst_point->y = raw_point->y;
dst_point->z = raw_point->z;
dst_point->reflectivity = (float)raw_point->reflectivity;
}
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;
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;
double theta = raw_point->theta / 100.0 / 180 * PI;
double phi = raw_point->phi / 100.0 / 180 * PI;
dst_point->x = radius * sin(theta) * cos(phi);
dst_point->y = radius * sin(theta) * sin(phi);
dst_point->z = radius * cos(theta);
dst_point->reflectivity = (float)raw_point->reflectivity;
}
inline void RawPointConvert(LivoxPointXyzr* dst_point1, LivoxPointXyzr* dst_point2, \
LivoxDualExtendSpherPoint* raw_point) {
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;
double theta = raw_point->theta / 100.0 / 180 * PI;
double phi = raw_point->phi / 100.0 / 180 * PI;
double theta = raw_point->theta / 100.0 / 180 * PI;
double phi = raw_point->phi / 100.0 / 180 * PI;
dst_point1->x = radius1 * sin(theta) * cos(phi);
dst_point1->y = radius1 * sin(theta) * sin(phi);
dst_point1->z = radius1 * cos(theta);
@@ -307,8 +313,8 @@ public:
virtual ~Lds();
uint8_t GetDeviceType(uint8_t handle);
static void ResetLidar(LidarDevice* lidar, uint8_t data_src);
static void SetLidarDataSrc(LidarDevice* lidar, uint8_t data_src);
static void ResetLidar(LidarDevice *lidar, uint8_t data_src);
static void SetLidarDataSrc(LidarDevice *lidar, uint8_t data_src);
void ResetLds(uint8_t data_src);
void RequestExit() { request_exit_ = true; }
void CleanRequestExit() { request_exit_ = false; }
@@ -319,11 +325,12 @@ public:
LidarDevice lidars_[kMaxSourceLidar]; /**< The index is the handle */
protected:
uint32_t buffer_time_ms_; /**< Buffer time before data in queue is read */
uint8_t data_src_;
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

431
livox_ros_driver/livox_ros_driver/lds_hub.cpp
View File

@@ -24,30 +24,33 @@
#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;
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;
is_initialized_ = false;
auto_connect_mode_ = true;
whitelist_count_ = 0;
is_initialized_ = false;
whitelist_count_ = 0;
memset(broadcast_code_whitelist_, 0, sizeof(broadcast_code_whitelist_));
@@ -55,16 +58,15 @@ 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,\
const char *user_config_path) {
int LdsHub::InitLdsHub(std::vector<std::string> &broadcast_code_strs,
const char *user_config_path) {
if (is_initialized_) {
printf("LiDAR data source is already inited!\n");
@@ -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);
@@ -96,12 +99,13 @@ int LdsHub::InitLdsHub(std::vector<std::string>& broadcast_code_strs,\
printf("Disable auto connect mode!\n");
printf("List all broadcast code in whiltelist:\n");
for (uint32_t i=0; i<whitelist_count_; i++) {
for (uint32_t i = 0; i < whitelist_count_; i++) {
printf("%s\n", broadcast_code_whitelist_[i]);
}
} 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 */
@@ -115,7 +119,7 @@ int LdsHub::InitLdsHub(std::vector<std::string>& broadcast_code_strs,\
if (g_lds_hub == nullptr) {
g_lds_hub = this;
}
is_initialized_= true;
is_initialized_ = true;
printf("Livox-SDK init success!\n");
return 0;
@@ -134,15 +138,13 @@ 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,
uint32_t data_num, void *client_data) {
LdsHub* lds_hub = static_cast<LdsHub *>(client_data);
LivoxEthPacket* eth_packet = data;
LdsHub *lds_hub = static_cast<LdsHub *>(client_data);
LivoxEthPacket *eth_packet = data;
if (!data || !data_num) {
return;
@@ -153,15 +155,16 @@ void LdsHub::OnHubDataCb(uint8_t hub_handle, LivoxEthPacket *data,
return;
}
LidarDevice* p_lidar = &lds_hub->lidars_[handle];
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,15 +206,16 @@ 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, \
if (!QueueIsFull(p_queue) && (cur_timestamp.stamp >= 0)) {
QueuePushAny(p_queue, (uint8_t *)eth_packet,
GetEthPacketLen(eth_packet->data_type),
packet_statistic->imu_timebase,
GetPointsPerPacket(eth_packet->data_type));
}
}
}
}
@@ -225,16 +230,17 @@ 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;
}
}
LidarDevice* p_hub = &g_lds_hub->hub_;
LidarDevice *p_hub = &g_lds_hub->hub_;
if (p_hub->connect_state == kConnectStateOff) {
bool result = false;
uint8_t handle = 0;
@@ -247,20 +253,21 @@ 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, \
sizeof(info->broadcast_code)) != 0) {
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;
config.return_mode = kFirstReturn;
config.coordinate = kCoordinateCartesian;
config.imu_rate = kImuFreq200Hz;
config.enable_fan = 1;
config.return_mode = kFirstReturn;
config.coordinate = kCoordinateCartesian;
config.imu_rate = kImuFreq200Hz;
} else {
config = g_lds_hub->hub_raw_config_;
}
p_hub->config.enable_fan = config.enable_fan;
p_hub->config.enable_fan = config.enable_fan;
p_hub->config.return_mode = config.return_mode;
p_hub->config.coordinate = config.coordinate;
p_hub->config.coordinate = config.coordinate;
p_hub->config.imu_rate = config.imu_rate;
} else {
printf("Add Hub to connect is failed : %d %d \n", result, handle);
@@ -278,7 +285,7 @@ void LdsHub::OnDeviceChange(const DeviceInfo *info, DeviceEvent type) {
return;
}
LidarDevice* p_hub = &g_lds_hub->hub_;
LidarDevice *p_hub = &g_lds_hub->hub_;
if (type == kEventHubConnectionChange) {
if (p_hub->connect_state == kConnectStateOff) {
p_hub->connect_state = kConnectStateOn;
@@ -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,11 +312,11 @@ 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) {
LdsHub *lds_hub = static_cast<LdsHub *>(client_data);
if ((handle >= kMaxLidarCount) || !response) {
return;
}
@@ -318,25 +324,24 @@ 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];
LidarDevice *p_lidar = &lds_hub->lidars_[index];
p_lidar->handle = index;
p_lidar->info.handle = index;
p_lidar->info.slot = response->device_info_list[i].slot;
p_lidar->info.id = response->device_info_list[i].id;
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,24 +367,22 @@ 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) {
LdsHub* lds_hub = static_cast<LdsHub *>(clent_data);
if ((handle >= kMaxLidarCount) || !response) {
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,15 +399,15 @@ 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);
LdsHub *lds_hub = static_cast<LdsHub *>(clent_data);
if (handle >= kMaxLidarCount) {
return;
}
LidarDevice* p_hub = &(lds_hub->hub_);
LidarDevice *p_hub = &(lds_hub->hub_);
if (status == kStatusSuccess) {
p_hub->config.set_bits &= ~((uint32_t)(kConfigCoordinate));
printf("Set coordinate success!\n");
@@ -422,10 +426,10 @@ void LdsHub::SetCoordinateCb(livox_status status, uint8_t handle, \
}
}
void LdsHub::HubSetImuRatePushFrequencyCb(livox_status status, uint8_t handle, \
HubSetImuPushFrequencyResponse* response,\
void *clent_data) {
LdsHub* lds_hub = static_cast<LdsHub *>(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,21 +448,21 @@ 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);
LdsHub *lds_hub = static_cast<LdsHub *>(clent_data);
if (handle >= kMaxLidarCount) {
return;
}
LidarDevice* p_hub = &lds_hub->hub_;
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]);
LidarDevice *p_lidar = &(lds_hub->lidars_[i]);
if (p_lidar->connect_state == kConnectStateSampling) {
p_lidar->connect_state = kConnectStateOn;
}
@@ -469,86 +473,89 @@ 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;
req->count = 0;
for (int i = 0; i < kMaxLidarCount; i++) {
LidarDevice* p_lidar = &(lds_hub->lidars_[i]);
void LdsHub::ConfigPointCloudReturnMode(LdsHub *lds_hub) {
uint8_t req_buf[1024];
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) && \
(p_lidar->connect_state == kConnectStateSampling)) {
UserRawConfig config;
if (lds_hub->GetRawConfig(p_lidar->info.broadcast_code, config)) {
printf("Could not find raw config, set config to default!\n");
config.enable_fan = 1;
config.return_mode = kFirstReturn;
config.coordinate = kCoordinateCartesian;
config.imu_rate = kImuFreq200Hz;
}
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++;
}
}
if ((p_lidar->info.type != kDeviceTypeLidarMid40) &&
(p_lidar->connect_state == kConnectStateSampling)) {
UserRawConfig config;
if (lds_hub->GetRawConfig(p_lidar->info.broadcast_code, config)) {
printf("Could not find raw config, set config to default!\n");
config.enable_fan = 1;
config.return_mode = kFirstReturn;
config.coordinate = kCoordinateCartesian;
config.imu_rate = kImuFreq200Hz;
}
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++;
}
}
if (req->count) {
uint32_t length = 1 + sizeof(SetPointCloudReturnModeRequestItem) * req->count;
HubSetPointCloudReturnMode(req, length,\
LdsHub::HubSetPointCloudReturnModeCb, lds_hub);
lds_hub->hub_.config.set_bits |= kConfigReturnMode;
if (req->count) {
uint32_t length =
1 + sizeof(SetPointCloudReturnModeRequestItem) * req->count;
HubSetPointCloudReturnMode(req, length,
LdsHub::HubSetPointCloudReturnModeCb, lds_hub);
lds_hub->hub_.config.set_bits |= kConfigReturnMode;
}
}
void LdsHub::ConfigImuPushFrequency(LdsHub* lds_hub) {
uint8_t req_buf[1024];
HubSetImuPushFrequencyRequest* req = (HubSetImuPushFrequencyRequest *)req_buf;
req->count = 0;
for (int i = 0; i < kMaxLidarCount; i++) {
LidarDevice* p_lidar = &(lds_hub->lidars_[i]);
void LdsHub::ConfigImuPushFrequency(LdsHub *lds_hub) {
uint8_t req_buf[1024];
HubSetImuPushFrequencyRequest *req = (HubSetImuPushFrequencyRequest *)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) && \
(p_lidar->connect_state == kConnectStateSampling)) {
UserRawConfig config;
if (lds_hub->GetRawConfig(p_lidar->info.broadcast_code, config)) {
printf("Could not find raw config, set config to default!\n");
config.enable_fan = 1;
config.return_mode = kFirstReturn;
config.coordinate = kCoordinateCartesian;
config.imu_rate = kImuFreq200Hz;
}
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++;
}
}
if ((p_lidar->info.type != kDeviceTypeLidarMid40) &&
(p_lidar->connect_state == kConnectStateSampling)) {
UserRawConfig config;
if (lds_hub->GetRawConfig(p_lidar->info.broadcast_code, config)) {
printf("Could not find raw config, set config to default!\n");
config.enable_fan = 1;
config.return_mode = kFirstReturn;
config.coordinate = kCoordinateCartesian;
config.imu_rate = kImuFreq200Hz;
}
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++;
}
}
if (req->count) {
uint32_t length = 1 + sizeof(SetImuPushFrequencyRequestItem) * req->count;
HubSetImuPushFrequency(req, length,\
LdsHub::HubSetImuRatePushFrequencyCb, lds_hub);
lds_hub->hub_.config.set_bits |= kConfigImuRate;
if (req->count) {
uint32_t length = 1 + sizeof(SetImuPushFrequencyRequestItem) * req->count;
HubSetImuPushFrequency(req, length, LdsHub::HubSetImuRatePushFrequencyCb,
lds_hub);
lds_hub->hub_.config.set_bits |= kConfigImuRate;
}
}
void LdsHub::ConfigLidarsOfHub(LdsHub* lds_hub) {
void LdsHub::ConfigLidarsOfHub(LdsHub *lds_hub) {
ConfigPointCloudReturnMode(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;
@@ -556,8 +563,8 @@ 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) || \
int LdsHub::AddBroadcastCodeToWhitelist(const char *broadcast_code) {
if (!broadcast_code || (strlen(broadcast_code) > kBroadcastCodeSize) ||
(whitelist_count_ >= kMaxLidarCount)) {
return -1;
}
@@ -573,13 +580,14 @@ int LdsHub::AddBroadcastCodeToWhitelist(const char* broadcast_code) {
return 0;
}
bool LdsHub::IsBroadcastCodeExistInWhitelist(const char* broadcast_code) {
bool LdsHub::IsBroadcastCodeExistInWhitelist(const char *broadcast_code) {
if (!broadcast_code) {
return false;
}
for (uint32_t i=0; i<whitelist_count_; i++) {
if (strncmp(broadcast_code, broadcast_code_whitelist_[i], kBroadcastCodeSize) == 0) {
for (uint32_t i = 0; i < whitelist_count_; i++) {
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);
@@ -612,48 +621,51 @@ void LdsHub::UpdateHubLidarinfo(void) {
}
/** Config file process */
int LdsHub::ParseConfigFile(const char* pathname) {
FILE* raw_file = std::fopen(pathname, "rb");
if(!raw_file) {
int LdsHub::ParseConfigFile(const char *pathname) {
FILE *raw_file = std::fopen(pathname, "rb");
if (!raw_file) {
printf("Open json config file fail!\n");
return -1;
}
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()) {
if(doc.HasMember("hub_config") && doc["hub_config"].IsObject()) {
const rapidjson::Value& object = doc["hub_config"];
if (!doc.ParseStream(config_file).HasParseError()) {
if (doc.HasMember("hub_config") && doc["hub_config"].IsObject()) {
const rapidjson::Value &object = doc["hub_config"];
UserRawConfig hub_config;
memset(&hub_config, 0, sizeof(hub_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::memcpy(hub_config.broadcast_code, broadcast_code.c_str(),\
sizeof(hub_config.broadcast_code));
std::memcpy(hub_config.broadcast_code, broadcast_code.c_str(),
sizeof(hub_config.broadcast_code));
if(object.HasMember("enable_connect") && \
object["enable_connect"].IsBool()) {
if (object.HasMember("enable_connect") &&
object["enable_connect"].IsBool()) {
hub_config.enable_connect = object["enable_connect"].GetBool();
}
if(object.HasMember("coordinate") && object["coordinate"].IsInt()) {
if (object.HasMember("coordinate") && object["coordinate"].IsInt()) {
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 {
@@ -661,32 +673,32 @@ int LdsHub::ParseConfigFile(const char* pathname) {
}
}
if(doc.HasMember("lidar_config") && doc["lidar_config"].IsArray()) {
const rapidjson::Value& array = doc["lidar_config"];
if (doc.HasMember("lidar_config") && doc["lidar_config"].IsArray()) {
const rapidjson::Value &array = doc["lidar_config"];
size_t len = array.Size();
for(size_t i=0; i<len; i++) {
const rapidjson::Value& object = array[i];
if(object.IsObject()) {
for (size_t i = 0; i < len; i++) {
const rapidjson::Value &object = array[i];
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::memcpy(config.broadcast_code, broadcast_code.c_str(),\
sizeof(config.broadcast_code));
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()) {
if (object.HasMember("imu_rate") && object["imu_rate"].IsInt()) {
config.imu_rate = object["imu_rate"].GetInt();
}
if (hub_raw_config_.enable_connect) {
@@ -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);
@@ -713,7 +727,7 @@ int LdsHub::ParseConfigFile(const char* pathname) {
return 0;
}
int LdsHub::AddRawUserConfig(UserRawConfig& config) {
int LdsHub::AddRawUserConfig(UserRawConfig &config) {
if (IsExistInRawConfig(config.broadcast_code)) {
return -1;
}
@@ -724,13 +738,14 @@ int LdsHub::AddRawUserConfig(UserRawConfig& config) {
return 0;
}
bool LdsHub::IsExistInRawConfig(const char* broadcast_code) {
bool LdsHub::IsExistInRawConfig(const char *broadcast_code) {
if (broadcast_code == nullptr) {
return false;
}
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,18 +753,18 @@ bool LdsHub::IsExistInRawConfig(const char* broadcast_code) {
return false;
}
int LdsHub::GetRawConfig(const char* broadcast_code, UserRawConfig& config) {
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) {
config.enable_fan = ite_config.enable_fan;
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;
config.imu_rate = ite_config.imu_rate;
config.coordinate = ite_config.coordinate;
config.imu_rate = ite_config.imu_rate;
return 0;
}
}
@@ -757,4 +772,4 @@ int LdsHub::GetRawConfig(const char* broadcast_code, UserRawConfig& config) {
return -1;
}
}
} // namespace livox_ros

71
livox_ros_driver/livox_ros_driver/lds_hub.h
View File

@@ -39,60 +39,67 @@ namespace livox_ros {
* LiDAR data source, data from hub.
*/
class LdsHub : public Lds {
public:
static LdsHub* GetInstance(uint32_t interval_ms) {
public:
static LdsHub *GetInstance(uint32_t interval_ms) {
static LdsHub lds_hub(interval_ms);
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:
private:
LdsHub(uint32_t interval_ms);
LdsHub(const LdsHub&) = delete;
LdsHub(const LdsHub &) = delete;
~LdsHub();
LdsHub& operator=(const LdsHub&) = delete;
LdsHub &operator=(const LdsHub &) = delete;
virtual void PrepareExit(void);
static void OnHubDataCb(uint8_t hub_handle, LivoxEthPacket *data,\
uint32_t data_num, void *client_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 StopSampleCb(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 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 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);
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);
void ResetLdsHub(void);
void StateReset(void);
int AddBroadcastCodeToWhitelist(const char* broadcast_code);
bool IsBroadcastCodeExistInWhitelist(const char* broadcast_code);
int AddBroadcastCodeToWhitelist(const char *broadcast_code);
bool IsBroadcastCodeExistInWhitelist(const char *broadcast_code);
void UpdateHubLidarinfo(void);
void EnableAutoConnectMode(void) { auto_connect_mode_ = true; }
void DisableAutoConnectMode(void) { auto_connect_mode_ = false; }
bool IsAutoConnectMode(void) { return auto_connect_mode_; }
int ParseConfigFile(const char* pathname);
int AddRawUserConfig(UserRawConfig& config);
bool IsExistInRawConfig(const char* broadcast_code);
int GetRawConfig(const char* broadcast_code, UserRawConfig& config);
int ParseConfigFile(const char *pathname);
int AddRawUserConfig(UserRawConfig &config);
bool IsExistInRawConfig(const char *broadcast_code);
int GetRawConfig(const char *broadcast_code, UserRawConfig &config);
bool IsAllLidarSetBitsClear() {
for(int i=0; i<kMaxLidarCount; i++) {
for (int i = 0; i < kMaxLidarCount; i++) {
if (lidars_[i].config.set_bits) {
return false;
}
@@ -111,5 +118,5 @@ class LdsHub : public Lds {
UserRawConfig hub_raw_config_;
};
}
} // namespace livox_ros
#endif

377
livox_ros_driver/livox_ros_driver/lds_lidar.cpp
View File

@@ -24,28 +24,31 @@
#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;
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;
is_initialized_ = false;
whitelist_count_ = 0;
memset(broadcast_code_whitelist_, 0, sizeof(broadcast_code_whitelist_));
@@ -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);
@@ -93,12 +93,13 @@ int LdsLidar::InitLdsLidar(std::vector<std::string>& broadcast_code_strs,\
printf("Disable auto connect mode!\n");
printf("List all broadcast code in whiltelist:\n");
for (uint32_t i=0; i<whitelist_count_; i++) {
for (uint32_t i = 0; i < whitelist_count_; i++) {
printf("%s\n", broadcast_code_whitelist_[i]);
}
} 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_) {
@@ -127,7 +128,7 @@ int LdsLidar::InitLdsLidar(std::vector<std::string>& broadcast_code_strs,\
if (g_lds_ldiar == nullptr) {
g_lds_ldiar = this;
}
is_initialized_= true;
is_initialized_ = true;
printf("Livox-SDK init success!\n");
return 0;
@@ -150,67 +151,68 @@ 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,
uint32_t data_num, void *client_data) {
using namespace std;
LdsLidar* lds_lidar = static_cast<LdsLidar *>(client_data);
LivoxEthPacket* eth_packet = data;
LdsLidar *lds_lidar = static_cast<LdsLidar *>(client_data);
LivoxEthPacket *eth_packet = data;
if (!data || !data_num || (handle >= kMaxLidarCount)) {
return;
}
LidarDevice* p_lidar = &lds_lidar->lidars_[handle];
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();
int64_t sync_time = cur_time.time_since_epoch().count();
packet_statistic->timebase = sync_time; // used receive time as timebase
packet_statistic->timebase = sync_time; // used receive time as timebase
}
}
packet_statistic->last_timestamp = cur_timestamp.stamp;
LidarDataQueue *p_queue = &p_lidar->data;
if (nullptr == p_queue->storage_packet) {
uint32_t queue_size = CalculatePacketQueueSize(lds_lidar->buffer_time_ms_, \
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, \
if (!QueueIsFull(p_queue) && (cur_timestamp.stamp >= 0)) {
QueuePushAny(p_queue, (uint8_t *)eth_packet,
GetEthPacketLen(eth_packet->data_type),
packet_statistic->timebase,
GetPointsPerPacket(eth_packet->data_type));
}
}
@@ -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;
}
@@ -257,25 +262,26 @@ void LdsLidar::OnDeviceBroadcast(const BroadcastDeviceInfo *info) {
if (result == kStatusSuccess && handle < kMaxLidarCount) {
SetDataCallback(handle, OnLidarDataCb, (void *)g_lds_ldiar);
LidarDevice* p_lidar = &(g_lds_ldiar->lidars_[handle]);
LidarDevice *p_lidar = &(g_lds_ldiar->lidars_[handle]);
p_lidar->handle = handle;
p_lidar->connect_state = kConnectStateOff;
UserRawConfig config;
if (g_lds_ldiar->GetRawConfig(info->broadcast_code, config)) {
printf("Could not find raw config, set config to default!\n");
config.enable_fan = 1;
config.return_mode = kFirstReturn;
config.coordinate = kCoordinateCartesian;
config.imu_rate = kImuFreq200Hz;
config.enable_fan = 1;
config.return_mode = kFirstReturn;
config.coordinate = kCoordinateCartesian;
config.imu_rate = kImuFreq200Hz;
config.extrinsic_parameter_source = kNoneExtrinsicParameter;
}
p_lidar->config.enable_fan = config.enable_fan;
p_lidar->config.enable_fan = config.enable_fan;
p_lidar->config.return_mode = config.return_mode;
p_lidar->config.coordinate = config.coordinate;
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);
}
@@ -292,7 +298,7 @@ void LdsLidar::OnDeviceChange(const DeviceInfo *info, DeviceEvent type) {
return;
}
LidarDevice* p_lidar = &(g_lds_ldiar->lidars_[handle]);
LidarDevice *p_lidar = &(g_lds_ldiar->lidars_[handle]);
if (type == kEventConnect) {
QueryDeviceInformation(handle, DeviceInformationCb, g_lds_ldiar);
if (p_lidar->connect_state == kConnectStateOff) {
@@ -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),\
SetPointCloudReturnModeCb, g_lds_ldiar);
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,25 +383,24 @@ 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);
LdsLidar *lds_lidar = static_cast<LdsLidar *>(clent_data);
if (handle >= kMaxLidarCount) {
return;
}
LidarDevice* p_lidar = &(lds_lidar->lidars_[handle]);
LidarDevice *p_lidar = &(lds_lidar->lidars_[handle]);
if (status == kStatusSuccess) {
p_lidar->config.set_bits &= ~((uint32_t)(kConfigReturnMode));
@@ -405,28 +412,29 @@ void LdsLidar::SetPointCloudReturnModeCb(livox_status status, uint8_t handle, \
}
} else {
LidarSetPointCloudReturnMode(handle, (PointCloudReturnMode)(p_lidar->config.return_mode),\
SetPointCloudReturnModeCb, lds_lidar);
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);
LdsLidar *lds_lidar = static_cast<LdsLidar *>(clent_data);
if (handle >= kMaxLidarCount) {
return;
}
LidarDevice* p_lidar = &(lds_lidar->lidars_[handle]);
LidarDevice *p_lidar = &(lds_lidar->lidars_[handle]);
if (status == kStatusSuccess) {
p_lidar->config.set_bits &= ~((uint32_t)(kConfigCoordinate));
printf("Set coordinate success!\n");
if (!p_lidar->config.set_bits) {
LidarStartSampling(handle, StartSampleCb, lds_lidar);
p_lidar->connect_state = kConnectStateSampling;
LidarStartSampling(handle, StartSampleCb, lds_lidar);
p_lidar->connect_state = kConnectStateSampling;
}
} else {
if (p_lidar->config.coordinate != 0) {
@@ -439,14 +447,14 @@ 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);
LdsLidar *lds_lidar = static_cast<LdsLidar *>(clent_data);
if (handle >= kMaxLidarCount) {
return;
}
LidarDevice* p_lidar = &(lds_lidar->lidars_[handle]);
LidarDevice *p_lidar = &(lds_lidar->lidars_[handle]);
if (status == kStatusSuccess) {
p_lidar->config.set_bits &= ~((uint32_t)(kConfigImuRate));
@@ -458,28 +466,28 @@ 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);
LdsLidar *lds_lidar = static_cast<LdsLidar *>(clent_data);
if (handle >= kMaxLidarCount) {
return;
}
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;
p_extrinsic->euler[0] = static_cast<float>(response->roll* PI / 180.0);
LidarDevice *p_lidar = &(lds_lidar->lidars_[handle]);
ExtrinsicParameter *p_extrinsic = &p_lidar->extrinsic_parameter;
p_extrinsic->euler[0] = static_cast<float>(response->roll * PI / 180.0);
p_extrinsic->euler[1] = static_cast<float>(response->pitch * PI / 180.0);
p_extrinsic->euler[2] = static_cast<float>(response->yaw * PI / 180.0);
p_extrinsic->trans[0] = static_cast<float>(response->x / 1000.0);
@@ -496,7 +504,7 @@ void LdsLidar::GetLidarExtrinsicParameterCb(livox_status status, uint8_t handle,
LidarStartSampling(handle, StartSampleCb, lds_lidar);
p_lidar->connect_state = kConnectStateSampling;
}
} else {
} else {
printf("Lidar[%d] get ExtrinsicParameter fail!\n", handle);
}
} else if (status == kStatusTimeout) {
@@ -504,65 +512,65 @@ 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);
LdsLidar *lds_lidar = static_cast<LdsLidar *>(clent_data);
if (handle >= kMaxLidarCount) {
return;
}
LidarDevice* p_lidar = &(lds_lidar->lidars_[handle]);
LidarDevice *p_lidar = &(lds_lidar->lidars_[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, \
uint8_t response, void* client_data) {
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) {
LdsLidar* lds_lidar = static_cast<LdsLidar *>(client_data);
//std::unique_lock<std::mutex> lock(mtx);
LidarDevice* p_lidar = nullptr;
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);
printf("Set GPRMC synchronization time error code: %d.\n", status);
}
}
}
}
/** Add broadcast code to whitelist */
int LdsLidar::AddBroadcastCodeToWhitelist(const char* broadcast_code) {
if (!broadcast_code || (strlen(broadcast_code) > kBroadcastCodeSize) || \
int LdsLidar::AddBroadcastCodeToWhitelist(const char *broadcast_code) {
if (!broadcast_code || (strlen(broadcast_code) > kBroadcastCodeSize) ||
(whitelist_count_ >= kMaxLidarCount)) {
return -1;
}
@@ -578,13 +586,14 @@ int LdsLidar::AddBroadcastCodeToWhitelist(const char* broadcast_code) {
return 0;
}
bool LdsLidar::IsBroadcastCodeExistInWhitelist(const char* broadcast_code) {
bool LdsLidar::IsBroadcastCodeExistInWhitelist(const char *broadcast_code) {
if (!broadcast_code) {
return false;
}
for (uint32_t i=0; i<whitelist_count_; i++) {
if (strncmp(broadcast_code, broadcast_code_whitelist_[i], kBroadcastCodeSize) == 0) {
for (uint32_t i = 0; i < whitelist_count_; i++) {
if (strncmp(broadcast_code, broadcast_code_whitelist_[i],
kBroadcastCodeSize) == 0) {
return true;
}
}
@@ -592,106 +601,125 @@ bool LdsLidar::IsBroadcastCodeExistInWhitelist(const char* broadcast_code) {
return false;
}
int LdsLidar::ParseTimesyncConfig(rapidjson::Document& doc) {
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;
const rapidjson::Value &object = doc["timesync_config"];
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 {
printf("Disable timesync\n");
}
return 0;
}while(0);
} while (0);
return -1;
}
/** Config file process */
int LdsLidar::ParseConfigFile(const char* pathname) {
FILE* raw_file = std::fopen(pathname, "rb");
if(!raw_file) {
int LdsLidar::ParseConfigFile(const char *pathname) {
FILE *raw_file = std::fopen(pathname, "rb");
if (!raw_file) {
printf("Open json config file fail!\n");
return -1;
}
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()) {
if(doc.HasMember("lidar_config") && doc["lidar_config"].IsArray()) {
const rapidjson::Value& array = doc["lidar_config"];
if (!doc.ParseStream(config_file).HasParseError()) {
if (doc.HasMember("lidar_config") && doc["lidar_config"].IsArray()) {
const rapidjson::Value &array = doc["lidar_config"];
size_t len = array.Size();
for(size_t i=0; i<len; i++) {
const rapidjson::Value& object = array[i];
if(object.IsObject()) {
for (size_t i = 0; i < len; i++) {
const rapidjson::Value &object = array[i];
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(),\
sizeof(config.broadcast_code));
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()) {
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()) {
if (object.HasMember("coordinate") && object["coordinate"].IsInt()) {
config.coordinate = object["coordinate"].GetInt();
}
if(object.HasMember("imu_rate") && object["imu_rate"].IsInt()) {
if (object.HasMember("imu_rate") && object["imu_rate"].IsInt()) {
config.imu_rate = object["imu_rate"].GetInt();
}
if(object.HasMember("extrinsic_parameter_source") && \
object["extrinsic_parameter_source"].IsInt()) {
config.extrinsic_parameter_source = object["extrinsic_parameter_source"].GetInt();
if (object.HasMember("extrinsic_parameter_source") &&
object["extrinsic_parameter_source"].IsInt()) {
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);
@@ -712,7 +741,7 @@ int LdsLidar::ParseConfigFile(const char* pathname) {
return 0;
}
int LdsLidar::AddRawUserConfig(UserRawConfig& config) {
int LdsLidar::AddRawUserConfig(UserRawConfig &config) {
if (IsExistInRawConfig(config.broadcast_code)) {
return -1;
}
@@ -723,13 +752,14 @@ int LdsLidar::AddRawUserConfig(UserRawConfig& config) {
return 0;
}
bool LdsLidar::IsExistInRawConfig(const char* broadcast_code) {
bool LdsLidar::IsExistInRawConfig(const char *broadcast_code) {
if (broadcast_code == nullptr) {
return false;
}
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;
}
}
@@ -737,17 +767,18 @@ bool LdsLidar::IsExistInRawConfig(const char* broadcast_code) {
return false;
}
int LdsLidar::GetRawConfig(const char* broadcast_code, UserRawConfig& config) {
int LdsLidar::GetRawConfig(const char *broadcast_code, UserRawConfig &config) {
if (broadcast_code == nullptr) {
return -1;
}
for (auto ite_config : raw_config_) {
if (strncmp(ite_config.broadcast_code, broadcast_code, kBroadcastCodeSize) == 0) {
config.enable_fan = ite_config.enable_fan;
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;
config.imu_rate = ite_config.imu_rate;
config.coordinate = ite_config.coordinate;
config.imu_rate = ite_config.imu_rate;
config.extrinsic_parameter_source = ite_config.extrinsic_parameter_source;
return 0;
}
@@ -756,4 +787,4 @@ int LdsLidar::GetRawConfig(const char* broadcast_code, UserRawConfig& config) {
return -1;
}
}
} // namespace livox_ros

69
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 {
@@ -42,57 +41,65 @@ namespace livox_ros {
* LiDAR data source, data from dependent lidar.
*/
class LdsLidar : public Lds {
public:
static LdsLidar* GetInstance(uint32_t interval_ms) {
public:
static LdsLidar *GetInstance(uint32_t interval_ms) {
static LdsLidar lds_lidar(interval_ms);
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:
private:
LdsLidar(uint32_t interval_ms);
LdsLidar(const LdsLidar&) = delete;
LdsLidar(const LdsLidar &) = delete;
~LdsLidar();
LdsLidar& operator=(const LdsLidar&) = delete;
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 StopSampleCb(livox_status status, uint8_t handle,
uint8_t response, void *clent_data);
static void SetPointCloudReturnModeCb(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 SetPointCloudReturnModeCb(livox_status status, uint8_t handle,
uint8_t response, void *clent_data);
static void SetCoordinateCb(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, \
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 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);
void ResetLdsLidar(void);
int AddBroadcastCodeToWhitelist(const char* broadcast_code);
bool IsBroadcastCodeExistInWhitelist(const char* broadcast_code);
int AddBroadcastCodeToWhitelist(const char *broadcast_code);
bool IsBroadcastCodeExistInWhitelist(const char *broadcast_code);
void EnableAutoConnectMode(void) { auto_connect_mode_ = true; }
void DisableAutoConnectMode(void) { auto_connect_mode_ = false; }
bool IsAutoConnectMode(void) { return auto_connect_mode_; }
int ParseTimesyncConfig(rapidjson::Document& doc);
int ParseConfigFile(const char* pathname);
int AddRawUserConfig(UserRawConfig& config);
bool IsExistInRawConfig(const char* broadcast_code);
int GetRawConfig(const char* broadcast_code, UserRawConfig& config);
int ParseTimesyncConfig(rapidjson::Document &doc);
int ParseConfigFile(const char *pathname);
int AddRawUserConfig(UserRawConfig &config);
bool IsExistInRawConfig(const char *broadcast_code);
int GetRawConfig(const char *broadcast_code, UserRawConfig &config);
bool auto_connect_mode_;
uint32_t whitelist_count_;
@@ -105,5 +112,5 @@ class LdsLidar : public Lds {
TimeSyncConfig timesync_config_;
};
}
} // namespace livox_ros
#endif

111
livox_ros_driver/livox_ros_driver/lds_lvx.cpp
View File

@@ -24,31 +24,36 @@
#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() {
if (packets_of_frame_.packet != nullptr) {
delete [] packets_of_frame_.packet;
delete[] packets_of_frame_.packet;
}
}
@@ -57,8 +62,7 @@ void LdsLvx::PrepareExit(void) {
printf("Lvx to rosbag convert complete and exit!\n");
}
int LdsLvx::InitLdsLvx(const char* lvx_path) {
int LdsLvx::InitLdsLvx(const char *lvx_path) {
if (is_initialized_) {
printf("Livox file data source is already inited!\n");
return -1;
@@ -84,14 +88,14 @@ int LdsLvx::InitLdsLvx(const char* lvx_path) {
}
printf("LvxFile[%s] have %d lidars\n", lvx_path, lidar_count_);
for (int i=0; i<lidar_count_; i++) {
for (int i = 0; i < lidar_count_; i++) {
LvxFileDeviceInfo lvx_dev_info;
lvx_file_->GetDeviceInfo(i, &lvx_dev_info);
lidars_[i].handle = i;
lidars_[i].connect_state = kConnectStateSampling;
lidars_[i].info.handle = i;
lidars_[i].info.type = lvx_dev_info.device_type;
memcpy(lidars_[i].info.broadcast_code, lvx_dev_info.lidar_broadcast_code,\
lidars_[i].info.type = lvx_dev_info.device_type;
memcpy(lidars_[i].info.broadcast_code, lvx_dev_info.lidar_broadcast_code,
sizeof(lidars_[i].info.broadcast_code));
if (lvx_file_->GetFileVersion() == kLvxFileV1) {
@@ -100,8 +104,8 @@ int LdsLvx::InitLdsLvx(const char* lvx_path) {
lidars_[i].data_src = kSourceLvxFile;
}
ExtrinsicParameter* p_extrinsic = &lidars_[i].extrinsic_parameter;
p_extrinsic->euler[0] = lvx_dev_info.roll* PI / 180.0;
ExtrinsicParameter *p_extrinsic = &lidars_[i].extrinsic_parameter;
p_extrinsic->euler[0] = lvx_dev_info.roll * PI / 180.0;
p_extrinsic->euler[1] = lvx_dev_info.pitch * PI / 180.0;
p_extrinsic->euler[2] = lvx_dev_info.yaw * PI / 180.0;
p_extrinsic->trans[0] = lvx_dev_info.x;
@@ -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,49 +131,55 @@ 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;
int progress = 0;
int progress = 0;
while (start_read_lvx_) {
file_state = lvx_file_->GetPacketsOfFrame(&packets_of_frame_);
if (!file_state) {
uint32_t data_size = packets_of_frame_.data_size;
uint8_t* packet_base = packets_of_frame_.packet;
uint32_t data_size = packets_of_frame_.data_size;
uint8_t *packet_base = packets_of_frame_.packet;
uint32_t data_offset = 0;
while (data_offset < data_size) {
LivoxEthPacket* eth_packet;
LivoxEthPacket *eth_packet;
int32_t handle;
uint8_t data_type;
if (lvx_file_->GetFileVersion()) {
LvxFilePacket* detail_packet = (LvxFilePacket*)&packet_base[data_offset];
eth_packet = (LivoxEthPacket*)(&detail_packet->version);
handle = detail_packet->device_index;
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];
eth_packet = (LivoxEthPacket*)(&detail_packet->version);
handle = detail_packet->device_index;
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_type = eth_packet->data_type;
data_offset +=
(GetEthPacketLen(data_type) + 1); /* packet length + device index */
if (data_type != kImu) {
LidarDataQueue* p_queue = &lidars_[handle].data;
LidarDataQueue *p_queue = &lidars_[handle].data;
if ((p_queue != nullptr) && (handle < lidar_count_)) {
while(QueueIsFull(p_queue)) {
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;
LidarDataQueue *p_queue = &lidars_[handle].imu_data;
if ((p_queue != nullptr) && (handle < lidar_count_)) {
while(QueueIsFull(p_queue)) {
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,15 +198,22 @@ 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();
}
bool LdsLvx::IsAllQueueEmpty() {
for (int i=0; i<lidar_count_; i++) {
LidarDevice* p_lidar = &lidars_[i];
for (int i = 0; i < lidar_count_; i++) {
LidarDevice *p_lidar = &lidars_[i];
if (!QueueIsEmpty(&p_lidar->data)) {
return false;
}
@@ -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

17
livox_ros_driver/livox_ros_driver/lds_lvx.h
View File

@@ -39,28 +39,29 @@ namespace livox_ros {
* Lidar data source abstract.
*/
class LdsLvx : public Lds {
public:
static LdsLvx* GetInstance(uint32_t interval_ms) {
public:
static LdsLvx *GetInstance(uint32_t interval_ms) {
static LdsLvx lds_lvx(interval_ms);
return &lds_lvx;
}
int InitLdsLvx(const char* lvx_path);
int InitLdsLvx(const char *lvx_path);
int DeInitLdsLvx(void);
void PrepareExit(void);
private:
private:
LdsLvx(uint32_t interval_ms);
LdsLvx(const LdsLvx&) = delete;
LdsLvx(const LdsLvx &) = delete;
~LdsLvx();
LdsLvx& operator=(const LdsLvx&) = delete;
LdsLvx &operator=(const LdsLvx &) = delete;
void StartRead() { start_read_lvx_ = true; }
void StopRead() { start_read_lvx_ = false; }
void StopRead() { start_read_lvx_ = false; }
bool IsStarted() { return start_read_lvx_; }
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

46
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,16 +54,16 @@ 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;
}
/** Init defualt system parameter */
int xfer_format = kPointCloud2Msg;
int multi_topic = 0;
int data_src = kSourceRawLidar;
double publish_freq = 50.0; /* Hz */
int data_src = kSourceRawLidar;
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;
@@ -89,7 +91,7 @@ int main(int argc, char **argv) {
std::vector<std::string> bd_code_list;
ParseCommandlineInputBdCode(cmdline_bd_code.c_str(), bd_code_list);
LdsLidar* read_lidar = LdsLidar::GetInstance(1000/publish_freq);
LdsLidar *read_lidar = LdsLidar::GetInstance(1000 / publish_freq);
lddc->RegisterLds(static_cast<Lds *>(read_lidar));
ret = read_lidar->InitLdsLidar(bd_code_list, user_config_path.c_str());
if (!ret) {
@@ -110,7 +112,7 @@ int main(int argc, char **argv) {
std::vector<std::string> bd_code_list;
ParseCommandlineInputBdCode(cmdline_bd_code.c_str(), bd_code_list);
LdsHub* read_hub = LdsHub::GetInstance(1000/publish_freq);
LdsHub *read_hub = LdsHub::GetInstance(1000 / publish_freq);
lddc->RegisterLds(static_cast<Lds *>(read_hub));
ret = read_hub->InitLdsHub(bd_code_list, user_config_path.c_str());
if (!ret) {
@@ -135,7 +137,7 @@ int main(int argc, char **argv) {
rosbag_file_path = cmdline_file_path.substr(0, path_end_pos);
rosbag_file_path += ".bag";
LdsLvx* read_lvx = LdsLvx::GetInstance(1000/publish_freq);
LdsLvx *read_lvx = LdsLvx::GetInstance(1000 / publish_freq);
lddc->RegisterLds(static_cast<Lds *>(read_lvx));
lddc->CreateBagFile(rosbag_file_path);
int ret = read_lvx->InitLdsLvx(cmdline_file_path.c_str());
@@ -144,11 +146,15 @@ int main(int argc, char **argv) {
} else {
ROS_ERROR("Init lds lvx file fail!");
}
} while(0);
} while (0);
}
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;
}

156
livox_ros_driver/livox_ros_driver/lvx_file.cpp
View File

@@ -23,24 +23,25 @@
//
#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 {
#define M_PI 3.14159265358979323846
#define M_PI 3.14159265358979323846
const uint32_t kMaxLvxFileHeaderLength = 16 * 1024;
const char* kLvxHeaderSigStr = "livox_tech";
const uint32_t kLvxHeaderMagicCode = 0xac0ea767;
const char *kLvxHeaderSigStr = "livox_tech";
const uint32_t kLvxHeaderMagicCode = 0xac0ea767;
LvxFileHandle::LvxFileHandle() : file_ver_(kLvxFileV1), device_count_(0), cur_frame_index_(0),
cur_offset_(0), data_start_offset_(0), size_(0), mode_(0), state_(0) {
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_));
memset((void *)&private_header_v0_, 0, sizeof(private_header_v0_));
@@ -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,16 +91,16 @@ uint64_t LvxFileHandle::PrivateHeaderOffset() {
uint64_t LvxFileHandle::DataStartOffset() {
if (file_ver_ == kLvxFileV1) {
return (sizeof(LvxFilePublicHeader) + sizeof(LvxFilePrivateHeader) + \
sizeof(LvxFileDeviceInfo) * private_header_.device_count);
return (sizeof(LvxFilePublicHeader) + sizeof(LvxFilePrivateHeader) +
sizeof(LvxFileDeviceInfo) * private_header_.device_count);
} else {
return (sizeof(LvxFilePublicHeader) + sizeof(LvxFilePrivateHeaderV0) + \
sizeof(LvxFileDeviceInfoV0) * private_header_v0_.device_count);
return (sizeof(LvxFilePublicHeader) + sizeof(LvxFilePrivateHeaderV0) +
sizeof(LvxFileDeviceInfoV0) * private_header_v0_.device_count);
}
}
bool LvxFileHandle::AddAndCheckDeviceInfo() {
lvx_file_.seekg (PrivateHeaderOffset(), std::ios::beg);
lvx_file_.seekg(PrivateHeaderOffset(), std::ios::beg);
if (file_ver_ == kLvxFileV1) {
lvx_file_.read((char *)(&private_header_), sizeof(private_header_));
@@ -110,17 +114,17 @@ bool LvxFileHandle::AddAndCheckDeviceInfo() {
return false;
}
for (int i=0; i< device_count_; i++) {
for (int i = 0; i < device_count_; i++) {
LvxFileDeviceInfo device_info;
if (file_ver_ == kLvxFileV1) {
lvx_file_.read((char *)(&device_info), sizeof(LvxFileDeviceInfo));
} 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, \
&device_info.extrinsic_enable - device_info.lidar_broadcast_code);
memcpy((void *)&device_info.roll, (void *)&device_info_v0.roll, \
sizeof(float) * 6);
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,
sizeof(float) * 6);
device_info.extrinsic_enable = 0;
}
AddDeviceInfo(device_info);
@@ -130,23 +134,23 @@ bool LvxFileHandle::AddAndCheckDeviceInfo() {
}
bool LvxFileHandle::PrepareDataRead() {
lvx_file_.seekg (DataStartOffset(), std::ios::beg);
lvx_file_.seekg(DataStartOffset(), std::ios::beg);
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;
}
/** reset the read position to the start offset of data erea */
lvx_file_.seekg (DataStartOffset(), std::ios::beg);
lvx_file_.seekg(DataStartOffset(), std::ios::beg);
return true;
}
int LvxFileHandle::Open(const char* filename,std::ios_base::openmode mode) {
int LvxFileHandle::Open(const char *filename, std::ios_base::openmode mode) {
if ((mode & std::ios::in) == std::ios::in) {
state_ = kLvxFileOk;
@@ -158,7 +162,8 @@ int LvxFileHandle::Open(const char* filename,std::ios_base::openmode mode) {
}
size_ = lvx_file_.tellg();
lvx_file_.seekg (0, std::ios::beg);
lvx_file_.seekg(0, std::ios::beg);
printf("Filesize %lu\n", size_);
if (size_ < MiniFileSize()) {
state_ = kLvxFileSizeFault;
@@ -192,15 +197,13 @@ 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);
char filename[30] = { 0 };
char filename[30] = {0};
tm* local_time = localtime(&curtime);
tm *local_time = localtime(&curtime);
strftime(filename, sizeof(filename), "%Y%m%d%H%M%S", local_time);
return Open(filename, std::ios::out | std::ios::binary);
@@ -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, \
sizeof(float) * 6);
memcpy(&write_buffer[cur_offset_], (void *)&device_info_v0, sizeof(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,
sizeof(float) * 6);
memcpy(&write_buffer[cur_offset_], (void *)&device_info_v0,
sizeof(device_info_v0));
cur_offset_ += sizeof(device_info_v0);
}
}
@@ -251,9 +262,10 @@ 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 };
FrameHeader frame_header = {0};
std::unique_ptr<char[]> write_buffer(new char[kMaxFrameSize]);
frame_header.current_offset = cur_offset_;
@@ -264,21 +276,22 @@ 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) {
if (cur_pos + iter.pack_size >= kMaxFrameSize) {
lvx_file_.write((char*)write_buffer.get(), cur_pos);
lvx_file_.write((char *)write_buffer.get(), cur_pos);
cur_pos = 0;
memcpy(write_buffer.get() + cur_pos, (void*)&(iter), iter.pack_size);
memcpy(write_buffer.get() + cur_pos, (void *)&(iter), iter.pack_size);
cur_pos += iter.pack_size;
} else {
memcpy(write_buffer.get() + cur_pos, (void*)&(iter), iter.pack_size);
memcpy(write_buffer.get() + cur_pos, (void *)&(iter), iter.pack_size);
cur_pos += iter.pack_size;
}
}
lvx_file_.write((char*)write_buffer.get(), cur_pos);
lvx_file_.write((char *)write_buffer.get(), cur_pos);
cur_offset_ = frame_header.next_offset;
cur_frame_index_++;
@@ -289,11 +302,12 @@ 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));
}
int LvxFileHandle::GetDeviceInfo(uint8_t idx, LvxFileDeviceInfo* info) {
int LvxFileHandle::GetDeviceInfo(uint8_t idx, LvxFileDeviceInfo *info) {
if (idx < device_info_list_.size()) {
*info = device_info_list_[idx];
return 0;
@@ -302,12 +316,19 @@ int LvxFileHandle::GetDeviceInfo(uint8_t idx, LvxFileDeviceInfo* info) {
return -1;
}
int LvxFileHandle::GetPacketsOfFrame(OutPacketBuffer* packets_of_frame) {
int LvxFileHandle::GetPacketsOfFrame(OutPacketBuffer *packets_of_frame) {
if (!lvx_file_ || lvx_file_.eof()) {
state_ = kLvxFileAtEnd;
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;
@@ -315,8 +336,8 @@ int LvxFileHandle::GetPacketsOfFrame(OutPacketBuffer* packets_of_frame) {
lvx_file_.read((char *)&frame_header, sizeof(frame_header));
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;
@@ -359,18 +379,19 @@ void ParseExtrinsicXml(DeviceItem &item, LvxFileDeviceInfo &info) {
rapidxml::file<> extrinsic_param("extrinsic.xml");
rapidxml::xml_document<> doc;
doc.parse<0>(extrinsic_param.data());
rapidxml::xml_node<>* root = doc.first_node();
rapidxml::xml_node<> *root = doc.first_node();
if ("Livox" == (std::string)root->name()) {
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)) {
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)) {
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

44
livox_ros_driver/livox_ros_driver/lvx_file.h
View File

@@ -24,19 +24,19 @@
#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 {
#define kMaxPointSize 1500
#define kDefaultFrameDurationTime 50
const uint32_t kMaxFrameSize = 2048*1024;
const uint32_t kMaxFrameSize = 2048 * 1024;
typedef enum {
kDeviceStateDisconnect = 0,
@@ -172,29 +172,31 @@ typedef struct {
#pragma pack()
class LvxFileHandle {
public:
public:
LvxFileHandle();
~LvxFileHandle() = default;
int Open(const char* filename, std::ios_base::openmode mode);
int Open(const char *filename, std::ios_base::openmode mode);
bool Eof();
int InitLvxFile();
void InitLvxFileHeader();
void SaveFrameToLvxFile(std::list<LvxFilePacket>& point_packet_list_temp);
void BasePointsHandle(LivoxEthPacket* data, LvxFilePacket& packet);
void SaveFrameToLvxFile(std::list<LvxFilePacket> &point_packet_list_temp);
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);
int GetDeviceInfo(uint8_t idx, LvxFileDeviceInfo *info);
int GetFileState(void) { return state_; };
int GetPacketsOfFrame(OutPacketBuffer* PacketsOfFrame);
int GetPacketsOfFrame(OutPacketBuffer *PacketsOfFrame);
int GetLvxFileReadProgress();
int GetFileVersion() { return file_ver_; }
private:
private:
std::fstream lvx_file_;
std::vector<LvxFileDeviceInfo> device_info_list_;
uint8_t file_ver_;
@@ -219,16 +221,16 @@ class LvxFileHandle {
bool AddAndCheckDeviceInfo();
bool PrepareDataRead();
uint64_t DataSizeOfFrame(FrameHeader& frame_header) {
return (frame_header.next_offset - frame_header.current_offset - sizeof(frame_header));
uint64_t DataSizeOfFrame(FrameHeader &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 - \
uint64_t DataSizeOfFrame(FrameHeaderV0 &frame_header_v0) {
return (frame_header_v0.next_offset - frame_header_v0.current_offset -
sizeof(frame_header_v0));
}
};
}
} // namespace livox_ros
#endif

55
livox_ros_driver/timesync/timesync.cpp
View File

@@ -24,30 +24,29 @@
#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;
uart_ = nullptr;
comm_ = nullptr;
fn_cb_ = nullptr;
client_data_ = nullptr;
rx_bytes_ = 0;
}
TimeSync::~TimeSync() {
DeInitTimeSync();
}
TimeSync::~TimeSync() { DeInitTimeSync(); }
int32_t TimeSync::InitTimeSync(const TimeSyncConfig& config) {
int32_t TimeSync::InitTimeSync(const TimeSyncConfig &config) {
config_ = config;
if (config_.dev_config.type == kCommDevUart) {
@@ -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;
@@ -87,9 +90,9 @@ int32_t TimeSync::DeInitTimeSync() {
void TimeSync::StopTimesync() {
start_poll_state_ = false;
start_poll_data_ = false;
exit_poll_state_ = true;
exit_poll_data_ = true;
start_poll_data_ = false;
exit_poll_state_ = true;
exit_poll_data_ = true;
if (t_poll_state_) {
t_poll_state_->join();
t_poll_state_ = nullptr;
@@ -97,7 +100,7 @@ void TimeSync::StopTimesync() {
if (t_poll_state_) {
t_poll_data_->join();
t_poll_data_ = nullptr;
t_poll_data_ = nullptr;
}
}
@@ -107,7 +110,7 @@ void TimeSync::PollStateLoop() {
}
while (!exit_poll_state_) {
if(fsm_state_ == kOpenDev) {
if (fsm_state_ == kOpenDev) {
FsmOpenDev();
} else if (fsm_state_ == kPrepareDev) {
FsmPrepareDev();
@@ -137,7 +140,7 @@ void TimeSync::PollDataLoop() {
memset(&packet, 0, sizeof(packet));
while ((kParseSuccess == comm_->ParseCommStream(&packet))) {
if ((fn_cb_ != nullptr) || (client_data_ != nullptr)) {
fn_cb_((const char*)packet.data, packet.data_len, client_data_);
fn_cb_((const char *)packet.data, packet.data_len, client_data_);
}
}
}
@@ -149,8 +152,8 @@ void TimeSync::PollDataLoop() {
}
void TimeSync::FsmTransferState(uint8_t new_state) {
if(new_state < kFsmDevUndef) {
fsm_state_ = new_state;
if (new_state < kFsmDevUndef) {
fsm_state_ = new_state;
}
transfer_time_ = chrono::steady_clock::now();
}
@@ -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

41
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;
@@ -48,21 +43,21 @@ typedef struct {
} TimeSyncConfig;
class TimeSync {
public:
static TimeSync* GetInstance() {
public:
static TimeSync *GetInstance() {
static TimeSync time_sync;
return &time_sync;
}
int32_t InitTimeSync(const TimeSyncConfig& config);
int32_t InitTimeSync(const TimeSyncConfig &config);
int32_t DeInitTimeSync();
void StartTimesync() {
start_poll_state_ = true;
start_poll_data_ = true;
start_poll_data_ = true;
}
int32_t SetReceiveSyncTimeCb(FnReceiveSyncTimeCb cb, void* data) {
int32_t SetReceiveSyncTimeCb(FnReceiveSyncTimeCb cb, void *data) {
if ((cb != nullptr) || (data != nullptr)) {
fn_cb_ = cb;
client_data_ = data;
@@ -72,11 +67,11 @@ class TimeSync {
}
}
private:
private:
TimeSync();
~TimeSync();
TimeSync(const TimeSync&) = delete;
TimeSync& operator=(const TimeSync&) = delete;
TimeSync(const TimeSync &) = delete;
TimeSync &operator=(const TimeSync &) = delete;
void PollStateLoop();
void PollDataLoop();
@@ -91,20 +86,20 @@ class TimeSync {
volatile bool start_poll_data_;
TimeSyncConfig config_;
UserUart* uart_;
CommProtocol* comm_;
UserUart *uart_;
CommProtocol *comm_;
volatile uint32_t rx_bytes_;
FnReceiveSyncTimeCb fn_cb_;
void* client_data_;
void *client_data_;
volatile uint8_t fsm_state_;
std::chrono::steady_clock::time_point transfer_time_;
std::chrono::steady_clock::time_point transfer_time_;
void FsmTransferState(uint8_t new_state);
void FsmOpenDev();
void FsmPrepareDev();
void FsmCheckDevState();
};
}
} // namespace livox_ros
#endif

97
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;
}
@@ -44,14 +43,14 @@ UserUart::~UserUart() {
is_open_ = false;
if (fd_ > 0) {
/** first we flush the port */
tcflush(fd_,TCOFLUSH);
tcflush(fd_,TCIFLUSH);
tcflush(fd_, TCOFLUSH);
tcflush(fd_, TCIFLUSH);
close(fd_);
}
}
int UserUart::Open(const char* filename) {
int UserUart::Open(const char *filename) {
fd_ = open(filename, O_RDWR | O_NOCTTY); //| O_NDELAY
if (fd_ < 0) {
printf("Open %s fail!\n", filename);
@@ -77,8 +76,8 @@ int UserUart::Close() {
is_open_ = false;
if (fd_ > 0) {
/** first we flush the port */
tcflush(fd_,TCOFLUSH);
tcflush(fd_,TCIFLUSH);
tcflush(fd_, TCOFLUSH);
tcflush(fd_, TCIFLUSH);
return close(fd_);
}
@@ -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;
@@ -110,12 +108,12 @@ int UserUart::Setup(uint8_t baudrate_index, uint8_t parity) {
options.c_cflag |= (CLOCAL | CREAD);
/** Disable hardware flow */
//options.c_cflag &= ~CRTSCTS;
// options.c_cflag &= ~CRTSCTS;
/** Disable software flow */
//options.c_iflag &= ~(IXON | IXOFF | IXANY);
// options.c_iflag &= ~(IXON | IXOFF | IXANY);
//options.c_oflag &= ~OPOST;
// options.c_oflag &= ~OPOST;
/** set boadrate */
options.c_cflag &= ~CBAUD;
@@ -123,45 +121,45 @@ int UserUart::Setup(uint8_t baudrate_index, uint8_t parity) {
options.c_cflag |= baudrate;
switch (parity) {
case P_8N1:
/** No parity (8N1) */
options.c_cflag &= ~PARENB;
options.c_cflag &= ~CSTOPB;
options.c_cflag &= ~CSIZE;
options.c_cflag |= CS8;
case P_8N1:
/** No parity (8N1) */
options.c_cflag &= ~PARENB;
options.c_cflag &= ~CSTOPB;
options.c_cflag &= ~CSIZE;
options.c_cflag |= CS8;
break;
case P_7E1:
/** Even parity (7E1) */
options.c_cflag |= PARENB;
options.c_cflag &= ~PARODD;
options.c_cflag &= ~CSTOPB;
options.c_cflag &= ~CSIZE;
options.c_cflag |= CS7;
case P_7E1:
/** Even parity (7E1) */
options.c_cflag |= PARENB;
options.c_cflag &= ~PARODD;
options.c_cflag &= ~CSTOPB;
options.c_cflag &= ~CSIZE;
options.c_cflag |= CS7;
break;
case P_7O1:
/** Odd parity (7O1) */
options.c_cflag |= PARENB;
options.c_cflag |= PARODD;
options.c_cflag &= ~CSTOPB;
options.c_cflag &= ~CSIZE;
options.c_cflag |= CS7;
case P_7O1:
/** Odd parity (7O1) */
options.c_cflag |= PARENB;
options.c_cflag |= PARODD;
options.c_cflag &= ~CSTOPB;
options.c_cflag &= ~CSIZE;
options.c_cflag |= CS7;
break;
case P_7S1:
/** Space parity is setup the same as no parity (7S1) */
options.c_cflag &= ~PARENB;
options.c_cflag &= ~CSTOPB;
options.c_cflag &= ~CSIZE;
options.c_cflag |= CS8;
case P_7S1:
/** Space parity is setup the same as no parity (7S1) */
options.c_cflag &= ~PARENB;
options.c_cflag &= ~CSTOPB;
options.c_cflag &= ~CSIZE;
options.c_cflag |= CS8;
break;
default:
return -1;
default:
return -1;
}
/** now we setup the values in port's termios */
options.c_iflag &= ~INPCK;
/** Enable non-canonical */
//options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
// options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
/** Time to wait for data */
options.c_cc[VTIME] = 1;
@@ -194,5 +192,4 @@ ssize_t UserUart::Read(char *buffer, size_t size) {
}
}
}
} // namespace livox_ros

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

@@ -26,19 +26,19 @@
#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>
namespace livox_ros {
enum Parity {
P_8N1, /* No parity (8N1) */
P_7E1, /* Even parity (7E1)*/
P_7O1, /* Odd parity (7O1) */
P_7S1, /* Space parity is setup the same as no parity (7S1) */
P_8N1, /* No parity (8N1) */
P_7E1, /* Even parity (7E1)*/
P_7O1, /* Odd parity (7O1) */
P_7S1, /* Space parity is setup the same as no parity (7S1) */
ParityUnkown
};
@@ -67,7 +67,7 @@ enum BaudRate {
class UserUart {
public:
public:
UserUart(uint8_t baudrate_index, uint8_t parity);
~UserUart();
@@ -75,10 +75,10 @@ class UserUart {
ssize_t Write(const char *buffer, size_t size);
ssize_t Read(char *buffer, size_t size);
int Close();
int Open(const char* filename);
int Open(const char *filename);
bool IsOpen() { return is_open_; };
private:
private:
int fd_;
volatile bool is_open_;
@@ -86,7 +86,6 @@ class UserUart {
uint8_t parity_;
};
}
} // namespace livox_ros
#endif