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 # precompile macro and compile option
#--------------------------------------------------------------------------------------- #---------------------------------------------------------------------------------------
target_compile_options(${PROJECT_NAME}_node 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. * SOFTWARE.
*/ */
// Teensy 3.0, Teensy 3.1: // Teensy 3.0, Teensy 3.1:
// See K20P64M72SF1RM.pdf (Kinetis), Pages 638 - 641 for documentation of CRC Device // See K20P64M72SF1RM.pdf (Kinetis), Pages 638 - 641 for documentation of CRC
// See KINETIS_4N30D.pdf for Errata (Errata ID 2776) // Device See KINETIS_4N30D.pdf for Errata (Errata ID 2776)
// //
// So, ALL HW-calculations are done as 32 bit. // So, ALL HW-calculations are done as 32 bit.
// //
@@ -47,7 +46,6 @@
#include <stdint.h> #include <stdint.h>
// ================= 16-BIT CRC =================== // ================= 16-BIT CRC ===================
class FastCRC16 { class FastCRC16 {
@@ -55,11 +53,13 @@ public:
FastCRC16(uint16_t seed); FastCRC16(uint16_t seed);
// change function name from mcrf4xx_upd to mcrf4xx // change function name from mcrf4xx_upd to mcrf4xx
uint16_t mcrf4xx_calc(const uint8_t *data,const uint16_t datalen); // Equivalent to _crc_ccitt_update() in crc16.h from avr_libc uint16_t
mcrf4xx_calc(const uint8_t *data,
const uint16_t datalen); // Equivalent to _crc_ccitt_update() in
// crc16.h from avr_libc
private: private:
uint16_t seed_; uint16_t seed_;
}; };
// ================= 32-BIT CRC =================== // ================= 32-BIT CRC ===================
@@ -69,11 +69,12 @@ public:
FastCRC32(uint32_t seed); FastCRC32(uint32_t seed);
// change function name from crc32_upd to crc32 // 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: private:
uint32_t seed_; uint32_t seed_;
}; };
#endif // FASTCRC_FASTCRC_H_ #endif // FASTCRC_FASTCRC_H_

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

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

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

@@ -37,19 +37,15 @@
#include "FastCRC.h" #include "FastCRC.h"
#include "FastCRC_tables.hpp" #include "FastCRC_tables.hpp"
// ================= 16-BIT CRC =================== // ================= 16-BIT CRC ===================
/** Constructor /** Constructor
*/ */
FastCRC16::FastCRC16(uint16_t seed) { FastCRC16::FastCRC16(uint16_t seed) { seed_ = seed; }
seed_ = seed;
}
#define crc_n4(crc, data, table) crc ^= data; \ #define crc_n4(crc, data, table) \
crc = (table[(crc & 0xff) + 0x300]) ^ \ crc ^= data; \
(table[((crc >> 8) & 0xff) + 0x200]) ^ \ crc = (table[(crc & 0xff) + 0x300]) ^ (table[((crc >> 8) & 0xff) + 0x200]) ^ \
(table[((data >> 16) & 0xff) + 0x100]) ^ \ (table[((data >> 16) & 0xff) + 0x100]) ^ (table[data >> 24]);
(table[data >> 24]);
/** MCRF4XX /** MCRF4XX
* equivalent to _crc_ccitt_update() in crc16.h from avr_libc * equivalent to _crc_ccitt_update() in crc16.h from avr_libc
@@ -80,25 +76,22 @@ uint16_t FastCRC16::mcrf4xx_calc(const uint8_t *data, uint16_t len) {
crc = (crc >> 8) ^ crc_table_mcrf4xx[(crc & 0xff) ^ *data++]; crc = (crc >> 8) ^ crc_table_mcrf4xx[(crc & 0xff) ^ *data++];
} }
//seed = crc; // seed = crc;
return crc; return crc;
} }
// ================= 32-BIT CRC =================== // ================= 32-BIT CRC ===================
/** Constructor /** Constructor
*/ */
FastCRC32::FastCRC32(uint32_t seed) { FastCRC32::FastCRC32(uint32_t seed) { seed_ = seed; }
seed_ = seed;
}
#define crc_n4d(crc, data, table) crc ^= data; \ #define crc_n4d(crc, data, table) \
crc = (table[(crc & 0xff) + 0x300]) ^ \ crc ^= data; \
(table[((crc >> 8) & 0xff) + 0x200]) ^ \ crc = (table[(crc & 0xff) + 0x300]) ^ (table[((crc >> 8) & 0xff) + 0x200]) ^ \
(table[((crc >> 16) & 0xff) + 0x100]) ^ \ (table[((crc >> 16) & 0xff) + 0x100]) ^ (table[(crc >> 24) & 0xff]);
(table[(crc >> 24) & 0xff]);
#define crcsm_n4d(crc, data, table) crc ^= data; \ #define crcsm_n4d(crc, data, table) \
crc ^= data; \
crc = (crc >> 8) ^ (table[crc & 0xff]); \ crc = (crc >> 8) ^ (table[crc & 0xff]); \
crc = (crc >> 8) ^ (table[crc & 0xff]); \ crc = (crc >> 8) ^ (table[crc & 0xff]); \
crc = (crc >> 8) ^ (table[crc & 0xff]); \ crc = (crc >> 8) ^ (table[crc & 0xff]); \
@@ -118,7 +111,7 @@ FastCRC32::FastCRC32(uint32_t seed) {
uint32_t FastCRC32::crc32_calc(const uint8_t *data, uint16_t len) { 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) { while (((uintptr_t)data & 3) && len) {
crc = (crc >> 8) ^ CRC_TABLE_CRC32[(crc & 0xff) ^ *data++]; crc = (crc >> 8) ^ CRC_TABLE_CRC32[(crc & 0xff) ^ *data++];
@@ -127,17 +120,17 @@ uint32_t FastCRC32::crc32_calc(const uint8_t *data, uint16_t len) {
while (len >= 16) { while (len >= 16) {
len -= 16; len -= 16;
#if CRC_BIGTABLES #if CRC_BIGTABLES
crc_n4d(crc, ((uint32_t *)data)[0], CRC_TABLE_CRC32); 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)[1], CRC_TABLE_CRC32);
crc_n4d(crc, ((uint32_t *)data)[2], CRC_TABLE_CRC32); crc_n4d(crc, ((uint32_t *)data)[2], CRC_TABLE_CRC32);
crc_n4d(crc, ((uint32_t *)data)[3], CRC_TABLE_CRC32); crc_n4d(crc, ((uint32_t *)data)[3], CRC_TABLE_CRC32);
#else #else
crcsm_n4d(crc, ((uint32_t *)data)[0], CRC_TABLE_CRC32); 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)[1], CRC_TABLE_CRC32);
crcsm_n4d(crc, ((uint32_t *)data)[2], CRC_TABLE_CRC32); crcsm_n4d(crc, ((uint32_t *)data)[2], CRC_TABLE_CRC32);
crcsm_n4d(crc, ((uint32_t *)data)[3], CRC_TABLE_CRC32); crcsm_n4d(crc, ((uint32_t *)data)[3], CRC_TABLE_CRC32);
#endif #endif
data += 16; data += 16;
} }
@@ -145,10 +138,8 @@ uint32_t FastCRC32::crc32_calc(const uint8_t *data, uint16_t len) {
crc = (crc >> 8) ^ CRC_TABLE_CRC32[(crc & 0xff) ^ *data++]; crc = (crc >> 8) ^ CRC_TABLE_CRC32[(crc & 0xff) ^ *data++];
} }
//seed = crc; // seed = crc;
crc ^= 0xffffffff; crc ^= 0xffffffff;
return crc; return crc;
} }

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

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

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

@@ -23,17 +23,17 @@
// //
#include "comm_protocol.h" #include "comm_protocol.h"
#include <iostream>
#include <stdio.h> #include <stdio.h>
#include <string.h> #include <string.h>
#include <iostream>
namespace livox_ros { namespace livox_ros {
CommProtocol::CommProtocol(ProtocolConfig& config) : config_(config) { CommProtocol::CommProtocol(ProtocolConfig &config) : config_(config) {
ResetParser(); ResetParser();
ResetCache(); ResetCache();
offset_to_read_index_= 0; offset_to_read_index_ = 0;
packet_length_ = 0; packet_length_ = 0;
if (kGps == config.type) { if (kGps == config.type) {
@@ -89,7 +89,8 @@ uint32_t CommProtocol::GetValidDataSize() {
} }
void CommProtocol::UpdateCache(void) { 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(); uint32_t valid_data_size = GetValidDataSize();
if (valid_data_size) { if (valid_data_size) {
@@ -103,18 +104,16 @@ void CommProtocol::UpdateCache(void) {
} }
} }
int32_t CommProtocol::Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len, \ int32_t CommProtocol::Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len,
const CommPacket &i_packet) { const CommPacket &i_packet) {
return protocol_->Pack(o_buf, o_buf_size, o_len, i_packet); return protocol_->Pack(o_buf, o_buf_size, o_len, i_packet);
} }
void CommProtocol::ResetParser() { void CommProtocol::ResetParser() { fsm_parse_step_ = kSearchPacketPreamble; }
fsm_parse_step_ = kSearchPacketPreamble;
}
int32_t CommProtocol::ParseCommStream(CommPacket *o_pack) { int32_t CommProtocol::ParseCommStream(CommPacket *o_pack) {
int32_t ret = kParseFail; int32_t ret = kParseFail;
while ((GetValidDataSize() > protocol_->GetPreambleLen()) && \ while ((GetValidDataSize() > protocol_->GetPreambleLen()) &&
(GetValidDataSize() > offset_to_read_index_)) { (GetValidDataSize() > offset_to_read_index_)) {
switch (fsm_parse_step_) { switch (fsm_parse_step_) {
case kSearchPacketPreamble: { case kSearchPacketPreamble: {
@@ -129,12 +128,11 @@ int32_t CommProtocol::ParseCommStream(CommPacket *o_pack) {
ret = FsmGetPacketData(o_pack); ret = FsmGetPacketData(o_pack);
break; break;
} }
default: { default: { FsmParserStateTransfer(kSearchPacketPreamble); }
FsmParserStateTransfer(kSearchPacketPreamble);
}
} }
if (ret == kParseNeedMoreData) break; /* */ if (ret == kParseNeedMoreData)
break; /* */
} }
return ret; return ret;
@@ -158,16 +156,18 @@ int32_t CommProtocol::FsmSearchPacketPreamble() {
break; break;
} else { } else {
packet_length_ = protocol_->GetPacketLen(GetCacheReadPos()); packet_length_ = protocol_->GetPacketLen(GetCacheReadPos());
if ((packet_length_ < cache_.size) && \ if ((packet_length_ < cache_.size) &&
(packet_length_ > protocol_->GetPacketWrapperLen())) { /* check the legality of length */ (packet_length_ >
protocol_
->GetPacketWrapperLen())) { /* check the legality of length */
FsmParserStateTransfer(kGetPacketData); FsmParserStateTransfer(kGetPacketData);
break; break;
} }
} }
} }
//printf("|%2x", cache_.buf[cache_.rd_idx]); // printf("|%2x", cache_.buf[cache_.rd_idx]);
++cache_.rd_idx; /* skip one byte */ ++cache_.rd_idx; /* skip one byte */
} while(0); } while (0);
return 0; return 0;
} }
@@ -216,9 +216,9 @@ int32_t CommProtocol::FsmGetPacketData(CommPacket *o_pack) {
cache_.rd_idx += protocol_->GetPacketLen(GetCacheReadPos()); cache_.rd_idx += protocol_->GetPacketLen(GetCacheReadPos());
FsmParserStateTransfer(kSearchPacketPreamble); FsmParserStateTransfer(kSearchPacketPreamble);
return kParseSuccess; return kParseSuccess;
} while(0); } while (0);
return ret; return ret;
} }
} // namespace livox } // namespace livox_ros

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

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

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

@@ -36,19 +36,17 @@ const uint32_t kPacketLengthLmit = 200;
const uint32_t kPreambleLen = 1; 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() { int32_t GpsProtocol::Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len,
found_length_ = 0;
}
int32_t GpsProtocol::Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len, \
const CommPacket &i_packet) { const CommPacket &i_packet) {
//GpsPacket* gps_packet = (GpsPacket*)o_buf; // GpsPacket* gps_packet = (GpsPacket*)o_buf;
return 0; return 0;
} }
int32_t GpsProtocol::ParsePacket(const uint8_t *i_buf, uint32_t i_len, CommPacket *o_packet) { int32_t GpsProtocol::ParsePacket(const uint8_t *i_buf, uint32_t i_len,
//GpsPacket *gps_packet = (GpsPacket *)i_buf; CommPacket *o_packet) {
// GpsPacket *gps_packet = (GpsPacket *)i_buf;
if (i_len < GetPacketWrapperLen()) { if (i_len < GetPacketWrapperLen()) {
return -1; // packet length error return -1; // packet length error
@@ -61,17 +59,15 @@ int32_t GpsProtocol::ParsePacket(const uint8_t *i_buf, uint32_t i_len, CommPacke
return 0; return 0;
} }
uint32_t GpsProtocol::GetPreambleLen() { uint32_t GpsProtocol::GetPreambleLen() { return kPreambleLen; /** '$' */ }
return kPreambleLen; /** '$' */
}
uint32_t GpsProtocol::GetPacketWrapperLen() { uint32_t GpsProtocol::GetPacketWrapperLen() {
return kWrapperLen; /** '$' + '*' + '2 checksum bytes' */ return kWrapperLen; /** '$' + '*' + '2 checksum bytes' */
} }
uint32_t GpsProtocol::FindPacketLen(const uint8_t *buf, uint32_t buf_length) { uint32_t GpsProtocol::FindPacketLen(const uint8_t *buf, uint32_t buf_length) {
uint32_t i=0; uint32_t i = 0;
for (; (i<buf_length) && (i<kPacketLengthLmit); i++) { for (; (i < buf_length) && (i < kPacketLengthLmit); i++) {
if ((buf[i] == kGpsProtocolEof) && (buf[0] == kGpsProtocolSof)) { if ((buf[i] == kGpsProtocolEof) && (buf[0] == kGpsProtocolSof)) {
found_length_ = i + 1 + 2; /* num = index + 1 + two bytes checksum */ found_length_ = i + 1 + 2; /* num = index + 1 + two bytes checksum */
return kFindLengthSuccess; 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) { uint32_t GpsProtocol::GetPacketLen(const uint8_t *buf) { return found_length_; }
return found_length_;
}
int32_t GpsProtocol::CheckPreamble(const uint8_t *buf) { int32_t GpsProtocol::CheckPreamble(const uint8_t *buf) {
GpsPreamble *preamble = (GpsPreamble *)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) { 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]); uint8_t raw_checksum = AscciiToHex(&buf[found_length_ - 2]);
if (checksum == raw_checksum) { if (checksum == raw_checksum) {
return 0; 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]; uint8_t result = buf[0];
for (uint32_t i = 1; i < length; i++) { for (uint32_t i = 1; i < length; i++) {
result ^= buf[i]; result ^= buf[i];
@@ -116,15 +112,16 @@ uint8_t GpsProtocol::CalcGpsPacketChecksum(const uint8_t *buf, uint32_t length)
return result; return result;
} }
uint8_t AscciiToHex(const uint8_t *TwoChar) { uint8_t AscciiToHex(const uint8_t *TwoChar) {
uint8_t h = toupper(TwoChar[0]) - 0x30; uint8_t h = toupper(TwoChar[0]) - 0x30;
if (h > 9) h -= 7; if (h > 9)
h -= 7;
uint8_t l = toupper(TwoChar[1]) - 0x30; 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

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

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

17
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 { kParseSuccess, kParseFail, kParseNeedMoreData } ParseResult;
typedef enum { kFindLengthSuccess, kFindLengthContinue, kFindLengthError } FindLengthResult; typedef enum {
kFindLengthSuccess,
kFindLengthContinue,
kFindLengthError
} FindLengthResult;
typedef struct CommPacket { typedef struct CommPacket {
uint8_t packet_type; uint8_t packet_type;
@@ -69,7 +73,7 @@ typedef struct {
/** NAME-0183 Protocol info config for gps */ /** NAME-0183 Protocol info config for gps */
typedef struct { typedef struct {
void* data; void *data;
} GpsProtocolConfig; } GpsProtocolConfig;
typedef struct { typedef struct {
@@ -81,12 +85,13 @@ typedef struct {
} ProtocolConfig; } ProtocolConfig;
class Protocol { class Protocol {
public: public:
virtual ~Protocol() = default; 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; const CommPacket &i_packet) = 0;
virtual uint32_t GetPreambleLen() = 0; virtual uint32_t GetPreambleLen() = 0;
@@ -102,5 +107,5 @@ class Protocol {
virtual int32_t CheckPacket(const uint8_t *buf) = 0; virtual int32_t CheckPacket(const uint8_t *buf) = 0;
}; };
} // namespace livox } // namespace livox_ros
#endif // COMM_PROTOCOL_H_ #endif // COMM_PROTOCOL_H_

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

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

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

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

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available. // Tencent is pleased to support the open source community by making RapidJSON
// available.
// //
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved. // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
// //
// Licensed under the MIT License (the "License"); you may not use this file except // Licensed under the MIT License (the "License"); you may not use this file
// in compliance with the License. You may obtain a copy of the License at // except in compliance with the License. You may obtain a copy of the License
// at
// //
// http://opensource.org/licenses/MIT // http://opensource.org/licenses/MIT
// //
// Unless required by applicable law or agreed to in writing, software distributed // Unless required by applicable law or agreed to in writing, software
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// CONDITIONS OF ANY KIND, either express or implied. See the License for the // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// specific language governing permissions and limitations under the License. // License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_ALLOCATORS_H_ #ifndef RAPIDJSON_ALLOCATORS_H_
#define RAPIDJSON_ALLOCATORS_H_ #define RAPIDJSON_ALLOCATORS_H_
@@ -32,7 +36,8 @@ RAPIDJSON_NAMESPACE_BEGIN
\code \code
concept Allocator { 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. // Allocate a memory block.
// \param size of the memory block in bytes. // \param size of the memory block in bytes.
@@ -40,8 +45,10 @@ concept Allocator {
void* Malloc(size_t size); void* Malloc(size_t size);
// Resize a memory block. // Resize a memory block.
// \param originalPtr The pointer to current memory block. Null pointer is permitted. // \param originalPtr The pointer to current memory block. Null pointer is
// \param originalSize The current size in bytes. (Design issue: since some allocator may not book-keep this, explicitly pass to it can save memory.) 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. // \param newSize the new size in bytes.
void* Realloc(void* originalPtr, size_t originalSize, size_t newSize); void* Realloc(void* originalPtr, size_t originalSize, size_t newSize);
@@ -52,7 +59,6 @@ concept Allocator {
\endcode \endcode
*/ */
/*! \def RAPIDJSON_ALLOCATOR_DEFAULT_CHUNK_CAPACITY /*! \def RAPIDJSON_ALLOCATOR_DEFAULT_CHUNK_CAPACITY
\ingroup RAPIDJSON_CONFIG \ingroup RAPIDJSON_CONFIG
\brief User-defined kDefaultChunkCapacity definition. \brief User-defined kDefaultChunkCapacity definition.
@@ -64,7 +70,6 @@ concept Allocator {
#define RAPIDJSON_ALLOCATOR_DEFAULT_CHUNK_CAPACITY (64 * 1024) #define RAPIDJSON_ALLOCATOR_DEFAULT_CHUNK_CAPACITY (64 * 1024)
#endif #endif
/////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////////
// CrtAllocator // CrtAllocator
@@ -75,13 +80,13 @@ concept Allocator {
class CrtAllocator { class CrtAllocator {
public: public:
static const bool kNeedFree = true; static const bool kNeedFree = true;
void* Malloc(size_t size) { void *Malloc(size_t size) {
if (size) // behavior of malloc(0) is implementation defined. if (size) // behavior of malloc(0) is implementation defined.
return std::malloc(size); return std::malloc(size);
else else
return NULL; // standardize to returning NULL. return NULL; // standardize to returning NULL.
} }
void* Realloc(void* originalPtr, size_t originalSize, size_t newSize) { void *Realloc(void *originalPtr, size_t originalSize, size_t newSize) {
(void)originalSize; (void)originalSize;
if (newSize == 0) { if (newSize == 0) {
std::free(originalPtr); std::free(originalPtr);
@@ -100,47 +105,55 @@ public:
It does not free memory blocks. And Realloc() only allocate new memory. 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. 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. The user-buffer is not deallocated by this allocator.
\tparam BaseAllocator the allocator type for allocating memory chunks. Default is CrtAllocator. \tparam BaseAllocator the allocator type for allocating memory chunks.
\note implements Allocator concept Default is CrtAllocator. \note implements Allocator concept
*/ */
template <typename BaseAllocator = CrtAllocator> template <typename BaseAllocator = CrtAllocator> class MemoryPoolAllocator {
class MemoryPoolAllocator {
public: 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. //! Constructor with chunkSize.
/*! \param chunkSize The size of memory chunk. The default is kDefaultChunkSize. /*! \param chunkSize The size of memory chunk. The default is
\param baseAllocator The allocator for allocating memory chunks. kDefaultChunkSize. \param baseAllocator The allocator for allocating memory
chunks.
*/ */
MemoryPoolAllocator(size_t chunkSize = kDefaultChunkCapacity, BaseAllocator* baseAllocator = 0) : MemoryPoolAllocator(size_t chunkSize = kDefaultChunkCapacity,
chunkHead_(0), chunk_capacity_(chunkSize), userBuffer_(0), baseAllocator_(baseAllocator), ownBaseAllocator_(0) BaseAllocator *baseAllocator = 0)
{ : chunkHead_(0), chunk_capacity_(chunkSize), userBuffer_(0),
} baseAllocator_(baseAllocator), ownBaseAllocator_(0) {}
//! Constructor with user-supplied buffer. //! Constructor with user-supplied buffer.
/*! The user buffer will be used firstly. When it is full, memory pool allocates new chunk with chunk size. /*! The user buffer will be used firstly. When it is full, memory pool
allocates new chunk with chunk size.
The user buffer will not be deallocated when this allocator is destructed. The user buffer will not be deallocated when this allocator is destructed.
\param buffer User supplied buffer. \param buffer User supplied buffer.
\param size Size of the buffer in bytes. It must at least larger than sizeof(ChunkHeader). \param size Size of the buffer in bytes. It must at least larger than
\param chunkSize The size of memory chunk. The default is kDefaultChunkSize. sizeof(ChunkHeader). \param chunkSize The size of memory chunk. The default
\param baseAllocator The allocator for allocating memory chunks. is kDefaultChunkSize. \param baseAllocator The allocator for allocating
memory chunks.
*/ */
MemoryPoolAllocator(void *buffer, size_t size, size_t chunkSize = kDefaultChunkCapacity, BaseAllocator* baseAllocator = 0) : MemoryPoolAllocator(void *buffer, size_t size,
chunkHead_(0), chunk_capacity_(chunkSize), userBuffer_(buffer), baseAllocator_(baseAllocator), ownBaseAllocator_(0) size_t chunkSize = kDefaultChunkCapacity,
{ BaseAllocator *baseAllocator = 0)
: chunkHead_(0), chunk_capacity_(chunkSize), userBuffer_(buffer),
baseAllocator_(baseAllocator), ownBaseAllocator_(0) {
RAPIDJSON_ASSERT(buffer != 0); RAPIDJSON_ASSERT(buffer != 0);
RAPIDJSON_ASSERT(size > sizeof(ChunkHeader)); RAPIDJSON_ASSERT(size > sizeof(ChunkHeader));
chunkHead_ = reinterpret_cast<ChunkHeader*>(buffer); chunkHead_ = reinterpret_cast<ChunkHeader *>(buffer);
chunkHead_->capacity = size - sizeof(ChunkHeader); chunkHead_->capacity = size - sizeof(ChunkHeader);
chunkHead_->size = 0; chunkHead_->size = 0;
chunkHead_->next = 0; chunkHead_->next = 0;
@@ -157,7 +170,7 @@ public:
//! Deallocates all memory chunks, excluding the user-supplied buffer. //! Deallocates all memory chunks, excluding the user-supplied buffer.
void Clear() { void Clear() {
while (chunkHead_ && chunkHead_ != userBuffer_) { while (chunkHead_ && chunkHead_ != userBuffer_) {
ChunkHeader* next = chunkHead_->next; ChunkHeader *next = chunkHead_->next;
baseAllocator_->Free(chunkHead_); baseAllocator_->Free(chunkHead_);
chunkHead_ = next; chunkHead_ = next;
} }
@@ -170,7 +183,7 @@ public:
*/ */
size_t Capacity() const { size_t Capacity() const {
size_t capacity = 0; size_t capacity = 0;
for (ChunkHeader* c = chunkHead_; c != 0; c = c->next) for (ChunkHeader *c = chunkHead_; c != 0; c = c->next)
capacity += c->capacity; capacity += c->capacity;
return capacity; return capacity;
} }
@@ -180,13 +193,13 @@ public:
*/ */
size_t Size() const { size_t Size() const {
size_t size = 0; size_t size = 0;
for (ChunkHeader* c = chunkHead_; c != 0; c = c->next) for (ChunkHeader *c = chunkHead_; c != 0; c = c->next)
size += c->size; size += c->size;
return size; return size;
} }
//! Allocates a memory block. (concept Allocator) //! Allocates a memory block. (concept Allocator)
void* Malloc(size_t size) { void *Malloc(size_t size) {
if (!size) if (!size)
return NULL; return NULL;
@@ -195,13 +208,14 @@ public:
if (!AddChunk(chunk_capacity_ > size ? chunk_capacity_ : size)) if (!AddChunk(chunk_capacity_ > size ? chunk_capacity_ : size))
return NULL; return NULL;
void *buffer = reinterpret_cast<char *>(chunkHead_) + RAPIDJSON_ALIGN(sizeof(ChunkHeader)) + chunkHead_->size; void *buffer = reinterpret_cast<char *>(chunkHead_) +
RAPIDJSON_ALIGN(sizeof(ChunkHeader)) + chunkHead_->size;
chunkHead_->size += size; chunkHead_->size += size;
return buffer; return buffer;
} }
//! Resizes a memory block (concept Allocator) //! Resizes a memory block (concept Allocator)
void* Realloc(void* originalPtr, size_t originalSize, size_t newSize) { void *Realloc(void *originalPtr, size_t originalSize, size_t newSize) {
if (originalPtr == 0) if (originalPtr == 0)
return Malloc(newSize); return Malloc(newSize);
@@ -215,8 +229,11 @@ public:
if (originalSize >= newSize) if (originalSize >= newSize)
return originalPtr; return originalPtr;
// Simply expand it if it is the last allocation and there is sufficient space // Simply expand it if it is the last allocation and there is sufficient
if (originalPtr == reinterpret_cast<char *>(chunkHead_) + RAPIDJSON_ALIGN(sizeof(ChunkHeader)) + chunkHead_->size - originalSize) { // space
if (originalPtr == reinterpret_cast<char *>(chunkHead_) +
RAPIDJSON_ALIGN(sizeof(ChunkHeader)) +
chunkHead_->size - originalSize) {
size_t increment = static_cast<size_t>(newSize - originalSize); size_t increment = static_cast<size_t>(newSize - originalSize);
if (chunkHead_->size + increment <= chunkHead_->capacity) { if (chunkHead_->size + increment <= chunkHead_->capacity) {
chunkHead_->size += increment; chunkHead_->size += increment;
@@ -225,12 +242,11 @@ public:
} }
// Realloc process: allocate and copy memory, do not free original buffer. // Realloc process: allocate and copy memory, do not free original buffer.
if (void* newBuffer = Malloc(newSize)) { if (void *newBuffer = Malloc(newSize)) {
if (originalSize) if (originalSize)
std::memcpy(newBuffer, originalPtr, originalSize); std::memcpy(newBuffer, originalPtr, originalSize);
return newBuffer; return newBuffer;
} } else
else
return NULL; return NULL;
} }
@@ -239,9 +255,9 @@ public:
private: private:
//! Copy constructor is not permitted. //! Copy constructor is not permitted.
MemoryPoolAllocator(const MemoryPoolAllocator& rhs) /* = delete */; MemoryPoolAllocator(const MemoryPoolAllocator &rhs) /* = delete */;
//! Copy assignment operator is not permitted. //! Copy assignment operator is not permitted.
MemoryPoolAllocator& operator=(const MemoryPoolAllocator& rhs) /* = delete */; MemoryPoolAllocator &operator=(const MemoryPoolAllocator &rhs) /* = delete */;
//! Creates a new chunk. //! Creates a new chunk.
/*! \param capacity Capacity of the chunk in bytes. /*! \param capacity Capacity of the chunk in bytes.
@@ -250,33 +266,39 @@ private:
bool AddChunk(size_t capacity) { bool AddChunk(size_t capacity) {
if (!baseAllocator_) if (!baseAllocator_)
ownBaseAllocator_ = baseAllocator_ = RAPIDJSON_NEW(BaseAllocator)(); ownBaseAllocator_ = baseAllocator_ = RAPIDJSON_NEW(BaseAllocator)();
if (ChunkHeader* chunk = reinterpret_cast<ChunkHeader*>(baseAllocator_->Malloc(RAPIDJSON_ALIGN(sizeof(ChunkHeader)) + capacity))) { if (ChunkHeader *chunk =
reinterpret_cast<ChunkHeader *>(baseAllocator_->Malloc(
RAPIDJSON_ALIGN(sizeof(ChunkHeader)) + capacity))) {
chunk->capacity = capacity; chunk->capacity = capacity;
chunk->size = 0; chunk->size = 0;
chunk->next = chunkHead_; chunk->next = chunkHead_;
chunkHead_ = chunk; chunkHead_ = chunk;
return true; return true;
} } else
else
return false; 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. //! Chunk header for perpending to each chunk.
/*! Chunks are stored as a singly linked list. /*! Chunks are stored as a singly linked list.
*/ */
struct ChunkHeader { struct ChunkHeader {
size_t capacity; //!< Capacity of the chunk in bytes (excluding the header itself). size_t capacity; //!< Capacity of the chunk in bytes (excluding the header
//!< itself).
size_t size; //!< Current size of allocated memory in bytes. size_t size; //!< Current size of allocated memory in bytes.
ChunkHeader *next; //!< Next chunk in the linked list. ChunkHeader *next; //!< Next chunk in the linked list.
}; };
ChunkHeader *chunkHead_; //!< Head of the chunk linked-list. Only the head chunk serves allocation. ChunkHeader *chunkHead_; //!< Head of the chunk linked-list. Only the head
size_t chunk_capacity_; //!< The minimum capacity of chunk when they are allocated. //!< chunk serves allocation.
size_t chunk_capacity_; //!< The minimum capacity of chunk when they are
//!< allocated.
void *userBuffer_; //!< User supplied buffer. void *userBuffer_; //!< User supplied buffer.
BaseAllocator* baseAllocator_; //!< base allocator for allocating memory chunks. BaseAllocator
BaseAllocator* ownBaseAllocator_; //!< base allocator created by this object. *baseAllocator_; //!< base allocator for allocating memory chunks.
BaseAllocator *ownBaseAllocator_; //!< base allocator created by this object.
}; };
RAPIDJSON_NAMESPACE_END RAPIDJSON_NAMESPACE_END

33
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 // Licensed under the MIT License (the "License"); you may not use this file
// in compliance with the License. You may obtain a copy of the License at // except in compliance with the License. You may obtain a copy of the License
// at
// //
// http://opensource.org/licenses/MIT // http://opensource.org/licenses/MIT
// //
// Unless required by applicable law or agreed to in writing, software distributed // Unless required by applicable law or agreed to in writing, software
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// CONDITIONS OF ANY KIND, either express or implied. See the License for the // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// specific language governing permissions and limitations under the License. // License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_CURSORSTREAMWRAPPER_H_ #ifndef RAPIDJSON_CURSORSTREAMWRAPPER_H_
#define RAPIDJSON_CURSORSTREAMWRAPPER_H_ #define RAPIDJSON_CURSORSTREAMWRAPPER_H_
@@ -30,27 +34,26 @@ RAPIDJSON_DIAG_OFF(4512) // assignment operator could not be generated
RAPIDJSON_NAMESPACE_BEGIN RAPIDJSON_NAMESPACE_BEGIN
//! Cursor stream wrapper for counting line and column number if error exists. //! Cursor stream wrapper for counting line and column number if error exists.
/*! /*!
\tparam InputStream Any stream that implements Stream Concept \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> { class CursorStreamWrapper : public GenericStreamWrapper<InputStream, Encoding> {
public: public:
typedef typename Encoding::Ch Ch; typedef typename Encoding::Ch Ch;
CursorStreamWrapper(InputStream& is): CursorStreamWrapper(InputStream &is)
GenericStreamWrapper<InputStream, Encoding>(is), line_(1), col_(0) {} : GenericStreamWrapper<InputStream, Encoding>(is), line_(1), col_(0) {}
// counting line and column number // counting line and column number
Ch Take() { Ch Take() {
Ch ch = this->is_.Take(); Ch ch = this->is_.Take();
if(ch == '\n') { if (ch == '\n') {
line_ ++; line_++;
col_ = 0; col_ = 0;
} else { } else {
col_ ++; col_++;
} }
return ch; return ch;
} }

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

File diff suppressed because it is too large Load Diff

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

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

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available. // Tencent is pleased to support the open source community by making RapidJSON
// available.
// //
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved. // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
// //
// Licensed under the MIT License (the "License"); you may not use this file except // Licensed under the MIT License (the "License"); you may not use this file
// in compliance with the License. You may obtain a copy of the License at // except in compliance with the License. You may obtain a copy of the License
// at
// //
// http://opensource.org/licenses/MIT // http://opensource.org/licenses/MIT
// //
// Unless required by applicable law or agreed to in writing, software distributed // Unless required by applicable law or agreed to in writing, software
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// CONDITIONS OF ANY KIND, either express or implied. See the License for the // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// specific language governing permissions and limitations under the License. // License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_ENCODINGS_H_ #ifndef RAPIDJSON_ENCODINGS_H_
#define RAPIDJSON_ENCODINGS_H_ #define RAPIDJSON_ENCODINGS_H_
@@ -19,7 +23,8 @@
#if defined(_MSC_VER) && !defined(__clang__) #if defined(_MSC_VER) && !defined(__clang__)
RAPIDJSON_DIAG_PUSH RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(4244) // conversion from 'type1' to 'type2', possible loss of data RAPIDJSON_DIAG_OFF(
4244) // conversion from 'type1' to 'type2', possible loss of data
RAPIDJSON_DIAG_OFF(4702) // unreachable code RAPIDJSON_DIAG_OFF(4702) // unreachable code
#elif defined(__GNUC__) #elif defined(__GNUC__)
RAPIDJSON_DIAG_PUSH RAPIDJSON_DIAG_PUSH
@@ -37,15 +42,16 @@ RAPIDJSON_NAMESPACE_BEGIN
\code \code
concept Encoding { concept Encoding {
typename Ch; //! Type of character. A "character" is actually a code unit in unicode's definition. typename Ch; //! Type of character. A "character" is actually a code unit
in unicode's definition.
enum { supportUnicode = 1 }; // or 0 if not supporting unicode enum { supportUnicode = 1 }; // or 0 if not supporting unicode
//! \brief Encode a Unicode codepoint to an output stream. //! \brief Encode a Unicode codepoint to an output stream.
//! \param os Output stream. //! \param os Output stream.
//! \param codepoint An unicode codepoint, ranging from 0x0 to 0x10FFFF inclusively. //! \param codepoint An unicode codepoint, ranging from 0x0 to 0x10FFFF
template<typename OutputStream> inclusively. template<typename OutputStream> static void Encode(OutputStream&
static void Encode(OutputStream& os, unsigned codepoint); os, unsigned codepoint);
//! \brief Decode a Unicode codepoint from an input stream. //! \brief Decode a Unicode codepoint from an input stream.
//! \param is Input stream. //! \param is Input stream.
@@ -58,8 +64,8 @@ concept Encoding {
//! \param is Input stream to obtain codepoint. //! \param is Input stream to obtain codepoint.
//! \param os Output for copying one codepoint. //! \param os Output for copying one codepoint.
//! \return true if it is valid. //! \return true if it is valid.
//! \note This function just validating and copying the codepoint without actually decode it. //! \note This function just validating and copying the codepoint without
template <typename InputStream, typename OutputStream> actually decode it. template <typename InputStream, typename OutputStream>
static bool Validate(InputStream& is, OutputStream& os); static bool Validate(InputStream& is, OutputStream& os);
// The following functions are deal with byte streams. // The following functions are deal with byte streams.
@@ -92,26 +98,23 @@ concept Encoding {
\tparam CharType Code unit for storing 8-bit UTF-8 data. Default is char. \tparam CharType Code unit for storing 8-bit UTF-8 data. Default is char.
\note implements Encoding concept \note implements Encoding concept
*/ */
template<typename CharType = char> template <typename CharType = char> struct UTF8 {
struct UTF8 {
typedef CharType Ch; typedef CharType Ch;
enum { supportUnicode = 1 }; enum { supportUnicode = 1 };
template<typename OutputStream> template <typename OutputStream>
static void Encode(OutputStream& os, unsigned codepoint) { static void Encode(OutputStream &os, unsigned codepoint) {
if (codepoint <= 0x7F) if (codepoint <= 0x7F)
os.Put(static_cast<Ch>(codepoint & 0xFF)); os.Put(static_cast<Ch>(codepoint & 0xFF));
else if (codepoint <= 0x7FF) { else if (codepoint <= 0x7FF) {
os.Put(static_cast<Ch>(0xC0 | ((codepoint >> 6) & 0xFF))); os.Put(static_cast<Ch>(0xC0 | ((codepoint >> 6) & 0xFF)));
os.Put(static_cast<Ch>(0x80 | ((codepoint & 0x3F)))); os.Put(static_cast<Ch>(0x80 | ((codepoint & 0x3F))));
} } else if (codepoint <= 0xFFFF) {
else if (codepoint <= 0xFFFF) {
os.Put(static_cast<Ch>(0xE0 | ((codepoint >> 12) & 0xFF))); os.Put(static_cast<Ch>(0xE0 | ((codepoint >> 12) & 0xFF)));
os.Put(static_cast<Ch>(0x80 | ((codepoint >> 6) & 0x3F))); os.Put(static_cast<Ch>(0x80 | ((codepoint >> 6) & 0x3F)));
os.Put(static_cast<Ch>(0x80 | (codepoint & 0x3F))); os.Put(static_cast<Ch>(0x80 | (codepoint & 0x3F)));
} } else {
else {
RAPIDJSON_ASSERT(codepoint <= 0x10FFFF); RAPIDJSON_ASSERT(codepoint <= 0x10FFFF);
os.Put(static_cast<Ch>(0xF0 | ((codepoint >> 18) & 0xFF))); os.Put(static_cast<Ch>(0xF0 | ((codepoint >> 18) & 0xFF)));
os.Put(static_cast<Ch>(0x80 | ((codepoint >> 12) & 0x3F))); os.Put(static_cast<Ch>(0x80 | ((codepoint >> 12) & 0x3F)));
@@ -120,20 +123,18 @@ struct UTF8 {
} }
} }
template<typename OutputStream> template <typename OutputStream>
static void EncodeUnsafe(OutputStream& os, unsigned codepoint) { static void EncodeUnsafe(OutputStream &os, unsigned codepoint) {
if (codepoint <= 0x7F) if (codepoint <= 0x7F)
PutUnsafe(os, static_cast<Ch>(codepoint & 0xFF)); PutUnsafe(os, static_cast<Ch>(codepoint & 0xFF));
else if (codepoint <= 0x7FF) { else if (codepoint <= 0x7FF) {
PutUnsafe(os, static_cast<Ch>(0xC0 | ((codepoint >> 6) & 0xFF))); PutUnsafe(os, static_cast<Ch>(0xC0 | ((codepoint >> 6) & 0xFF)));
PutUnsafe(os, static_cast<Ch>(0x80 | ((codepoint & 0x3F)))); PutUnsafe(os, static_cast<Ch>(0x80 | ((codepoint & 0x3F))));
} } else if (codepoint <= 0xFFFF) {
else if (codepoint <= 0xFFFF) {
PutUnsafe(os, static_cast<Ch>(0xE0 | ((codepoint >> 12) & 0xFF))); PutUnsafe(os, static_cast<Ch>(0xE0 | ((codepoint >> 12) & 0xFF)));
PutUnsafe(os, static_cast<Ch>(0x80 | ((codepoint >> 6) & 0x3F))); PutUnsafe(os, static_cast<Ch>(0x80 | ((codepoint >> 6) & 0x3F)));
PutUnsafe(os, static_cast<Ch>(0x80 | (codepoint & 0x3F))); PutUnsafe(os, static_cast<Ch>(0x80 | (codepoint & 0x3F)));
} } else {
else {
RAPIDJSON_ASSERT(codepoint <= 0x10FFFF); RAPIDJSON_ASSERT(codepoint <= 0x10FFFF);
PutUnsafe(os, static_cast<Ch>(0xF0 | ((codepoint >> 18) & 0xFF))); PutUnsafe(os, static_cast<Ch>(0xF0 | ((codepoint >> 18) & 0xFF)));
PutUnsafe(os, static_cast<Ch>(0x80 | ((codepoint >> 12) & 0x3F))); PutUnsafe(os, static_cast<Ch>(0x80 | ((codepoint >> 12) & 0x3F)));
@@ -143,10 +144,15 @@ struct UTF8 {
} }
template <typename InputStream> template <typename InputStream>
static bool Decode(InputStream& is, unsigned* codepoint) { static bool Decode(InputStream &is, unsigned *codepoint) {
#define RAPIDJSON_COPY() c = is.Take(); *codepoint = (*codepoint << 6) | (static_cast<unsigned char>(c) & 0x3Fu) #define RAPIDJSON_COPY() \
#define RAPIDJSON_TRANS(mask) result &= ((GetRange(static_cast<unsigned char>(c)) & mask) != 0) c = is.Take(); \
#define RAPIDJSON_TAIL() RAPIDJSON_COPY(); RAPIDJSON_TRANS(0x70) *codepoint = (*codepoint << 6) | (static_cast<unsigned char>(c) & 0x3Fu)
#define RAPIDJSON_TRANS(mask) \
result &= ((GetRange(static_cast<unsigned char>(c)) & mask) != 0)
#define RAPIDJSON_TAIL() \
RAPIDJSON_COPY(); \
RAPIDJSON_TRANS(0x70)
typename InputStream::Ch c = is.Take(); typename InputStream::Ch c = is.Take();
if (!(c & 0x80)) { if (!(c & 0x80)) {
*codepoint = static_cast<unsigned char>(c); *codepoint = static_cast<unsigned char>(c);
@@ -161,14 +167,42 @@ struct UTF8 {
} }
bool result = true; bool result = true;
switch (type) { switch (type) {
case 2: RAPIDJSON_TAIL(); return result; case 2:
case 3: RAPIDJSON_TAIL(); RAPIDJSON_TAIL(); return result; RAPIDJSON_TAIL();
case 4: RAPIDJSON_COPY(); RAPIDJSON_TRANS(0x50); RAPIDJSON_TAIL(); return result; return result;
case 5: RAPIDJSON_COPY(); RAPIDJSON_TRANS(0x10); RAPIDJSON_TAIL(); RAPIDJSON_TAIL(); return result; case 3:
case 6: RAPIDJSON_TAIL(); RAPIDJSON_TAIL(); RAPIDJSON_TAIL(); return result; RAPIDJSON_TAIL();
case 10: RAPIDJSON_COPY(); RAPIDJSON_TRANS(0x20); RAPIDJSON_TAIL(); return result; RAPIDJSON_TAIL();
case 11: RAPIDJSON_COPY(); RAPIDJSON_TRANS(0x60); RAPIDJSON_TAIL(); RAPIDJSON_TAIL(); return result; return result;
default: return false; case 4:
RAPIDJSON_COPY();
RAPIDJSON_TRANS(0x50);
RAPIDJSON_TAIL();
return result;
case 5:
RAPIDJSON_COPY();
RAPIDJSON_TRANS(0x10);
RAPIDJSON_TAIL();
RAPIDJSON_TAIL();
return result;
case 6:
RAPIDJSON_TAIL();
RAPIDJSON_TAIL();
RAPIDJSON_TAIL();
return result;
case 10:
RAPIDJSON_COPY();
RAPIDJSON_TRANS(0x20);
RAPIDJSON_TAIL();
return result;
case 11:
RAPIDJSON_COPY();
RAPIDJSON_TRANS(0x60);
RAPIDJSON_TAIL();
RAPIDJSON_TAIL();
return result;
default:
return false;
} }
#undef RAPIDJSON_COPY #undef RAPIDJSON_COPY
#undef RAPIDJSON_TRANS #undef RAPIDJSON_TRANS
@@ -176,10 +210,13 @@ struct UTF8 {
} }
template <typename InputStream, typename OutputStream> template <typename InputStream, typename OutputStream>
static bool Validate(InputStream& is, OutputStream& os) { static bool Validate(InputStream &is, OutputStream &os) {
#define RAPIDJSON_COPY() os.Put(c = is.Take()) #define RAPIDJSON_COPY() os.Put(c = is.Take())
#define RAPIDJSON_TRANS(mask) result &= ((GetRange(static_cast<unsigned char>(c)) & mask) != 0) #define RAPIDJSON_TRANS(mask) \
#define RAPIDJSON_TAIL() RAPIDJSON_COPY(); RAPIDJSON_TRANS(0x70) result &= ((GetRange(static_cast<unsigned char>(c)) & mask) != 0)
#define RAPIDJSON_TAIL() \
RAPIDJSON_COPY(); \
RAPIDJSON_TRANS(0x70)
Ch c; Ch c;
RAPIDJSON_COPY(); RAPIDJSON_COPY();
if (!(c & 0x80)) if (!(c & 0x80))
@@ -187,14 +224,42 @@ struct UTF8 {
bool result = true; bool result = true;
switch (GetRange(static_cast<unsigned char>(c))) { switch (GetRange(static_cast<unsigned char>(c))) {
case 2: RAPIDJSON_TAIL(); return result; case 2:
case 3: RAPIDJSON_TAIL(); RAPIDJSON_TAIL(); return result; RAPIDJSON_TAIL();
case 4: RAPIDJSON_COPY(); RAPIDJSON_TRANS(0x50); RAPIDJSON_TAIL(); return result; return result;
case 5: RAPIDJSON_COPY(); RAPIDJSON_TRANS(0x10); RAPIDJSON_TAIL(); RAPIDJSON_TAIL(); return result; case 3:
case 6: RAPIDJSON_TAIL(); RAPIDJSON_TAIL(); RAPIDJSON_TAIL(); return result; RAPIDJSON_TAIL();
case 10: RAPIDJSON_COPY(); RAPIDJSON_TRANS(0x20); RAPIDJSON_TAIL(); return result; RAPIDJSON_TAIL();
case 11: RAPIDJSON_COPY(); RAPIDJSON_TRANS(0x60); RAPIDJSON_TAIL(); RAPIDJSON_TAIL(); return result; return result;
default: return false; case 4:
RAPIDJSON_COPY();
RAPIDJSON_TRANS(0x50);
RAPIDJSON_TAIL();
return result;
case 5:
RAPIDJSON_COPY();
RAPIDJSON_TRANS(0x10);
RAPIDJSON_TAIL();
RAPIDJSON_TAIL();
return result;
case 6:
RAPIDJSON_TAIL();
RAPIDJSON_TAIL();
RAPIDJSON_TAIL();
return result;
case 10:
RAPIDJSON_COPY();
RAPIDJSON_TRANS(0x20);
RAPIDJSON_TAIL();
return result;
case 11:
RAPIDJSON_COPY();
RAPIDJSON_TRANS(0x60);
RAPIDJSON_TAIL();
RAPIDJSON_TAIL();
return result;
default:
return false;
} }
#undef RAPIDJSON_COPY #undef RAPIDJSON_COPY
#undef RAPIDJSON_TRANS #undef RAPIDJSON_TRANS
@@ -203,43 +268,58 @@ struct UTF8 {
static unsigned char GetRange(unsigned char c) { static unsigned char GetRange(unsigned char c) {
// Referring to DFA of http://bjoern.hoehrmann.de/utf-8/decoder/dfa/ // Referring to DFA of http://bjoern.hoehrmann.de/utf-8/decoder/dfa/
// With new mapping 1 -> 0x10, 7 -> 0x20, 9 -> 0x40, such that AND operation can test multiple types. // With new mapping 1 -> 0x10, 7 -> 0x20, 9 -> 0x40, such that AND operation
// can test multiple types.
static const unsigned char type[] = { static const unsigned char type[] = {
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x10, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
8,8,2,2,2,2,2,2,2,2,2,2,2,2,2,2, 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
10,3,3,3,3,3,3,3,3,3,3,3,3,4,3,3, 11,6,6,6,5,8,8,8,8,8,8,8,8,8,8,8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0x10, 0x10, 0x10, 0x10,
0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10,
0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40,
0x40, 0x40, 0x40, 0x40, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
8, 8, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2,
2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2,
2, 2, 2, 2, 2, 2, 2, 2, 10, 3, 3, 3,
3, 3, 3, 3, 3, 3, 3, 3, 3, 4, 3, 3,
11, 6, 6, 6, 5, 8, 8, 8, 8, 8, 8, 8,
8, 8, 8, 8,
}; };
return type[c]; return type[c];
} }
template <typename InputByteStream> template <typename InputByteStream>
static CharType TakeBOM(InputByteStream& is) { static CharType TakeBOM(InputByteStream &is) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1); RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
typename InputByteStream::Ch c = Take(is); typename InputByteStream::Ch c = Take(is);
if (static_cast<unsigned char>(c) != 0xEFu) return c; if (static_cast<unsigned char>(c) != 0xEFu)
return c;
c = is.Take(); c = is.Take();
if (static_cast<unsigned char>(c) != 0xBBu) return c; if (static_cast<unsigned char>(c) != 0xBBu)
return c;
c = is.Take(); c = is.Take();
if (static_cast<unsigned char>(c) != 0xBFu) return c; if (static_cast<unsigned char>(c) != 0xBFu)
return c;
c = is.Take(); c = is.Take();
return c; return c;
} }
template <typename InputByteStream> template <typename InputByteStream> static Ch Take(InputByteStream &is) {
static Ch Take(InputByteStream& is) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1); RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
return static_cast<Ch>(is.Take()); return static_cast<Ch>(is.Take());
} }
template <typename OutputByteStream> template <typename OutputByteStream>
static void PutBOM(OutputByteStream& os) { static void PutBOM(OutputByteStream &os) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1); RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
os.Put(static_cast<typename OutputByteStream::Ch>(0xEFu)); os.Put(static_cast<typename OutputByteStream::Ch>(0xEFu));
os.Put(static_cast<typename OutputByteStream::Ch>(0xBBu)); os.Put(static_cast<typename OutputByteStream::Ch>(0xBBu));
@@ -247,7 +327,7 @@ struct UTF8 {
} }
template <typename OutputByteStream> template <typename OutputByteStream>
static void Put(OutputByteStream& os, Ch c) { static void Put(OutputByteStream &os, Ch c) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1); RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
os.Put(static_cast<typename OutputByteStream::Ch>(c)); os.Put(static_cast<typename OutputByteStream::Ch>(c));
} }
@@ -259,27 +339,28 @@ struct UTF8 {
//! UTF-16 encoding. //! UTF-16 encoding.
/*! http://en.wikipedia.org/wiki/UTF-16 /*! http://en.wikipedia.org/wiki/UTF-16
http://tools.ietf.org/html/rfc2781 http://tools.ietf.org/html/rfc2781
\tparam CharType Type for storing 16-bit UTF-16 data. Default is wchar_t. C++11 may use char16_t instead. \tparam CharType Type for storing 16-bit UTF-16 data. Default is wchar_t.
\note implements Encoding concept C++11 may use char16_t instead. \note implements Encoding concept
\note For in-memory access, no need to concern endianness. The code units and code points are represented by CPU's endianness. \note For in-memory access, no need to concern endianness. The code units
For streaming, use UTF16LE and UTF16BE, which handle endianness. and code points are represented by CPU's endianness. For streaming, use
UTF16LE and UTF16BE, which handle endianness.
*/ */
template<typename CharType = wchar_t> template <typename CharType = wchar_t> struct UTF16 {
struct UTF16 {
typedef CharType Ch; typedef CharType Ch;
RAPIDJSON_STATIC_ASSERT(sizeof(Ch) >= 2); RAPIDJSON_STATIC_ASSERT(sizeof(Ch) >= 2);
enum { supportUnicode = 1 }; enum { supportUnicode = 1 };
template<typename OutputStream> template <typename OutputStream>
static void Encode(OutputStream& os, unsigned codepoint) { static void Encode(OutputStream &os, unsigned codepoint) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputStream::Ch) >= 2); RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputStream::Ch) >= 2);
if (codepoint <= 0xFFFF) { if (codepoint <= 0xFFFF) {
RAPIDJSON_ASSERT(codepoint < 0xD800 || codepoint > 0xDFFF); // Code point itself cannot be surrogate pair RAPIDJSON_ASSERT(
codepoint < 0xD800 ||
codepoint > 0xDFFF); // Code point itself cannot be surrogate pair
os.Put(static_cast<typename OutputStream::Ch>(codepoint)); os.Put(static_cast<typename OutputStream::Ch>(codepoint));
} } else {
else {
RAPIDJSON_ASSERT(codepoint <= 0x10FFFF); RAPIDJSON_ASSERT(codepoint <= 0x10FFFF);
unsigned v = codepoint - 0x10000; unsigned v = codepoint - 0x10000;
os.Put(static_cast<typename OutputStream::Ch>((v >> 10) | 0xD800)); os.Put(static_cast<typename OutputStream::Ch>((v >> 10) | 0xD800));
@@ -287,31 +368,31 @@ struct UTF16 {
} }
} }
template <typename OutputStream>
template<typename OutputStream> static void EncodeUnsafe(OutputStream &os, unsigned codepoint) {
static void EncodeUnsafe(OutputStream& os, unsigned codepoint) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputStream::Ch) >= 2); RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputStream::Ch) >= 2);
if (codepoint <= 0xFFFF) { if (codepoint <= 0xFFFF) {
RAPIDJSON_ASSERT(codepoint < 0xD800 || codepoint > 0xDFFF); // Code point itself cannot be surrogate pair RAPIDJSON_ASSERT(
codepoint < 0xD800 ||
codepoint > 0xDFFF); // Code point itself cannot be surrogate pair
PutUnsafe(os, static_cast<typename OutputStream::Ch>(codepoint)); PutUnsafe(os, static_cast<typename OutputStream::Ch>(codepoint));
} } else {
else {
RAPIDJSON_ASSERT(codepoint <= 0x10FFFF); RAPIDJSON_ASSERT(codepoint <= 0x10FFFF);
unsigned v = codepoint - 0x10000; unsigned v = codepoint - 0x10000;
PutUnsafe(os, static_cast<typename OutputStream::Ch>((v >> 10) | 0xD800)); PutUnsafe(os, static_cast<typename OutputStream::Ch>((v >> 10) | 0xD800));
PutUnsafe(os, static_cast<typename OutputStream::Ch>((v & 0x3FF) | 0xDC00)); PutUnsafe(os,
static_cast<typename OutputStream::Ch>((v & 0x3FF) | 0xDC00));
} }
} }
template <typename InputStream> template <typename InputStream>
static bool Decode(InputStream& is, unsigned* codepoint) { static bool Decode(InputStream &is, unsigned *codepoint) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputStream::Ch) >= 2); RAPIDJSON_STATIC_ASSERT(sizeof(typename InputStream::Ch) >= 2);
typename InputStream::Ch c = is.Take(); typename InputStream::Ch c = is.Take();
if (c < 0xD800 || c > 0xDFFF) { if (c < 0xD800 || c > 0xDFFF) {
*codepoint = static_cast<unsigned>(c); *codepoint = static_cast<unsigned>(c);
return true; return true;
} } else if (c <= 0xDBFF) {
else if (c <= 0xDBFF) {
*codepoint = (static_cast<unsigned>(c) & 0x3FF) << 10; *codepoint = (static_cast<unsigned>(c) & 0x3FF) << 10;
c = is.Take(); c = is.Take();
*codepoint |= (static_cast<unsigned>(c) & 0x3FF); *codepoint |= (static_cast<unsigned>(c) & 0x3FF);
@@ -322,7 +403,7 @@ struct UTF16 {
} }
template <typename InputStream, typename OutputStream> template <typename InputStream, typename OutputStream>
static bool Validate(InputStream& is, OutputStream& os) { static bool Validate(InputStream &is, OutputStream &os) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputStream::Ch) >= 2); RAPIDJSON_STATIC_ASSERT(sizeof(typename InputStream::Ch) >= 2);
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputStream::Ch) >= 2); RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputStream::Ch) >= 2);
typename InputStream::Ch c; typename InputStream::Ch c;
@@ -338,17 +419,16 @@ struct UTF16 {
}; };
//! UTF-16 little endian encoding. //! UTF-16 little endian encoding.
template<typename CharType = wchar_t> template <typename CharType = wchar_t> struct UTF16LE : UTF16<CharType> {
struct UTF16LE : UTF16<CharType> {
template <typename InputByteStream> template <typename InputByteStream>
static CharType TakeBOM(InputByteStream& is) { static CharType TakeBOM(InputByteStream &is) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1); RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
CharType c = Take(is); CharType c = Take(is);
return static_cast<uint16_t>(c) == 0xFEFFu ? Take(is) : c; return static_cast<uint16_t>(c) == 0xFEFFu ? Take(is) : c;
} }
template <typename InputByteStream> template <typename InputByteStream>
static CharType Take(InputByteStream& is) { static CharType Take(InputByteStream &is) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1); RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
unsigned c = static_cast<uint8_t>(is.Take()); unsigned c = static_cast<uint8_t>(is.Take());
c |= static_cast<unsigned>(static_cast<uint8_t>(is.Take())) << 8; c |= static_cast<unsigned>(static_cast<uint8_t>(is.Take())) << 8;
@@ -356,32 +436,33 @@ struct UTF16LE : UTF16<CharType> {
} }
template <typename OutputByteStream> template <typename OutputByteStream>
static void PutBOM(OutputByteStream& os) { static void PutBOM(OutputByteStream &os) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1); RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
os.Put(static_cast<typename OutputByteStream::Ch>(0xFFu)); os.Put(static_cast<typename OutputByteStream::Ch>(0xFFu));
os.Put(static_cast<typename OutputByteStream::Ch>(0xFEu)); os.Put(static_cast<typename OutputByteStream::Ch>(0xFEu));
} }
template <typename OutputByteStream> template <typename OutputByteStream>
static void Put(OutputByteStream& os, CharType c) { static void Put(OutputByteStream &os, CharType c) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1); RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
os.Put(static_cast<typename OutputByteStream::Ch>(static_cast<unsigned>(c) & 0xFFu)); os.Put(static_cast<typename OutputByteStream::Ch>(static_cast<unsigned>(c) &
os.Put(static_cast<typename OutputByteStream::Ch>((static_cast<unsigned>(c) >> 8) & 0xFFu)); 0xFFu));
os.Put(static_cast<typename OutputByteStream::Ch>(
(static_cast<unsigned>(c) >> 8) & 0xFFu));
} }
}; };
//! UTF-16 big endian encoding. //! UTF-16 big endian encoding.
template<typename CharType = wchar_t> template <typename CharType = wchar_t> struct UTF16BE : UTF16<CharType> {
struct UTF16BE : UTF16<CharType> {
template <typename InputByteStream> template <typename InputByteStream>
static CharType TakeBOM(InputByteStream& is) { static CharType TakeBOM(InputByteStream &is) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1); RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
CharType c = Take(is); CharType c = Take(is);
return static_cast<uint16_t>(c) == 0xFEFFu ? Take(is) : c; return static_cast<uint16_t>(c) == 0xFEFFu ? Take(is) : c;
} }
template <typename InputByteStream> template <typename InputByteStream>
static CharType Take(InputByteStream& is) { static CharType Take(InputByteStream &is) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1); RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
unsigned c = static_cast<unsigned>(static_cast<uint8_t>(is.Take())) << 8; unsigned c = static_cast<unsigned>(static_cast<uint8_t>(is.Take())) << 8;
c |= static_cast<unsigned>(static_cast<uint8_t>(is.Take())); c |= static_cast<unsigned>(static_cast<uint8_t>(is.Take()));
@@ -389,17 +470,19 @@ struct UTF16BE : UTF16<CharType> {
} }
template <typename OutputByteStream> template <typename OutputByteStream>
static void PutBOM(OutputByteStream& os) { static void PutBOM(OutputByteStream &os) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1); RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
os.Put(static_cast<typename OutputByteStream::Ch>(0xFEu)); os.Put(static_cast<typename OutputByteStream::Ch>(0xFEu));
os.Put(static_cast<typename OutputByteStream::Ch>(0xFFu)); os.Put(static_cast<typename OutputByteStream::Ch>(0xFFu));
} }
template <typename OutputByteStream> template <typename OutputByteStream>
static void Put(OutputByteStream& os, CharType c) { static void Put(OutputByteStream &os, CharType c) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1); RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
os.Put(static_cast<typename OutputByteStream::Ch>((static_cast<unsigned>(c) >> 8) & 0xFFu)); os.Put(static_cast<typename OutputByteStream::Ch>(
os.Put(static_cast<typename OutputByteStream::Ch>(static_cast<unsigned>(c) & 0xFFu)); (static_cast<unsigned>(c) >> 8) & 0xFFu));
os.Put(static_cast<typename OutputByteStream::Ch>(static_cast<unsigned>(c) &
0xFFu));
} }
}; };
@@ -408,35 +491,35 @@ struct UTF16BE : UTF16<CharType> {
//! UTF-32 encoding. //! UTF-32 encoding.
/*! http://en.wikipedia.org/wiki/UTF-32 /*! http://en.wikipedia.org/wiki/UTF-32
\tparam CharType Type for storing 32-bit UTF-32 data. Default is unsigned. C++11 may use char32_t instead. \tparam CharType Type for storing 32-bit UTF-32 data. Default is unsigned.
\note implements Encoding concept C++11 may use char32_t instead. \note implements Encoding concept
\note For in-memory access, no need to concern endianness. The code units and code points are represented by CPU's endianness. \note For in-memory access, no need to concern endianness. The code units
For streaming, use UTF32LE and UTF32BE, which handle endianness. and code points are represented by CPU's endianness. For streaming, use
UTF32LE and UTF32BE, which handle endianness.
*/ */
template<typename CharType = unsigned> template <typename CharType = unsigned> struct UTF32 {
struct UTF32 {
typedef CharType Ch; typedef CharType Ch;
RAPIDJSON_STATIC_ASSERT(sizeof(Ch) >= 4); RAPIDJSON_STATIC_ASSERT(sizeof(Ch) >= 4);
enum { supportUnicode = 1 }; enum { supportUnicode = 1 };
template<typename OutputStream> template <typename OutputStream>
static void Encode(OutputStream& os, unsigned codepoint) { static void Encode(OutputStream &os, unsigned codepoint) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputStream::Ch) >= 4); RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputStream::Ch) >= 4);
RAPIDJSON_ASSERT(codepoint <= 0x10FFFF); RAPIDJSON_ASSERT(codepoint <= 0x10FFFF);
os.Put(codepoint); os.Put(codepoint);
} }
template<typename OutputStream> template <typename OutputStream>
static void EncodeUnsafe(OutputStream& os, unsigned codepoint) { static void EncodeUnsafe(OutputStream &os, unsigned codepoint) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputStream::Ch) >= 4); RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputStream::Ch) >= 4);
RAPIDJSON_ASSERT(codepoint <= 0x10FFFF); RAPIDJSON_ASSERT(codepoint <= 0x10FFFF);
PutUnsafe(os, codepoint); PutUnsafe(os, codepoint);
} }
template <typename InputStream> template <typename InputStream>
static bool Decode(InputStream& is, unsigned* codepoint) { static bool Decode(InputStream &is, unsigned *codepoint) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputStream::Ch) >= 4); RAPIDJSON_STATIC_ASSERT(sizeof(typename InputStream::Ch) >= 4);
Ch c = is.Take(); Ch c = is.Take();
*codepoint = c; *codepoint = c;
@@ -444,7 +527,7 @@ struct UTF32 {
} }
template <typename InputStream, typename OutputStream> template <typename InputStream, typename OutputStream>
static bool Validate(InputStream& is, OutputStream& os) { static bool Validate(InputStream &is, OutputStream &os) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputStream::Ch) >= 4); RAPIDJSON_STATIC_ASSERT(sizeof(typename InputStream::Ch) >= 4);
Ch c; Ch c;
os.Put(c = is.Take()); os.Put(c = is.Take());
@@ -453,17 +536,16 @@ struct UTF32 {
}; };
//! UTF-32 little endian enocoding. //! UTF-32 little endian enocoding.
template<typename CharType = unsigned> template <typename CharType = unsigned> struct UTF32LE : UTF32<CharType> {
struct UTF32LE : UTF32<CharType> {
template <typename InputByteStream> template <typename InputByteStream>
static CharType TakeBOM(InputByteStream& is) { static CharType TakeBOM(InputByteStream &is) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1); RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
CharType c = Take(is); CharType c = Take(is);
return static_cast<uint32_t>(c) == 0x0000FEFFu ? Take(is) : c; return static_cast<uint32_t>(c) == 0x0000FEFFu ? Take(is) : c;
} }
template <typename InputByteStream> template <typename InputByteStream>
static CharType Take(InputByteStream& is) { static CharType Take(InputByteStream &is) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1); RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
unsigned c = static_cast<uint8_t>(is.Take()); unsigned c = static_cast<uint8_t>(is.Take());
c |= static_cast<unsigned>(static_cast<uint8_t>(is.Take())) << 8; c |= static_cast<unsigned>(static_cast<uint8_t>(is.Take())) << 8;
@@ -473,7 +555,7 @@ struct UTF32LE : UTF32<CharType> {
} }
template <typename OutputByteStream> template <typename OutputByteStream>
static void PutBOM(OutputByteStream& os) { static void PutBOM(OutputByteStream &os) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1); RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
os.Put(static_cast<typename OutputByteStream::Ch>(0xFFu)); os.Put(static_cast<typename OutputByteStream::Ch>(0xFFu));
os.Put(static_cast<typename OutputByteStream::Ch>(0xFEu)); os.Put(static_cast<typename OutputByteStream::Ch>(0xFEu));
@@ -482,7 +564,7 @@ struct UTF32LE : UTF32<CharType> {
} }
template <typename OutputByteStream> template <typename OutputByteStream>
static void Put(OutputByteStream& os, CharType c) { static void Put(OutputByteStream &os, CharType c) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1); RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
os.Put(static_cast<typename OutputByteStream::Ch>(c & 0xFFu)); os.Put(static_cast<typename OutputByteStream::Ch>(c & 0xFFu));
os.Put(static_cast<typename OutputByteStream::Ch>((c >> 8) & 0xFFu)); os.Put(static_cast<typename OutputByteStream::Ch>((c >> 8) & 0xFFu));
@@ -492,17 +574,16 @@ struct UTF32LE : UTF32<CharType> {
}; };
//! UTF-32 big endian encoding. //! UTF-32 big endian encoding.
template<typename CharType = unsigned> template <typename CharType = unsigned> struct UTF32BE : UTF32<CharType> {
struct UTF32BE : UTF32<CharType> {
template <typename InputByteStream> template <typename InputByteStream>
static CharType TakeBOM(InputByteStream& is) { static CharType TakeBOM(InputByteStream &is) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1); RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
CharType c = Take(is); CharType c = Take(is);
return static_cast<uint32_t>(c) == 0x0000FEFFu ? Take(is) : c; return static_cast<uint32_t>(c) == 0x0000FEFFu ? Take(is) : c;
} }
template <typename InputByteStream> template <typename InputByteStream>
static CharType Take(InputByteStream& is) { static CharType Take(InputByteStream &is) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1); RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
unsigned c = static_cast<unsigned>(static_cast<uint8_t>(is.Take())) << 24; unsigned c = static_cast<unsigned>(static_cast<uint8_t>(is.Take())) << 24;
c |= static_cast<unsigned>(static_cast<uint8_t>(is.Take())) << 16; c |= static_cast<unsigned>(static_cast<uint8_t>(is.Take())) << 16;
@@ -512,7 +593,7 @@ struct UTF32BE : UTF32<CharType> {
} }
template <typename OutputByteStream> template <typename OutputByteStream>
static void PutBOM(OutputByteStream& os) { static void PutBOM(OutputByteStream &os) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1); RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
os.Put(static_cast<typename OutputByteStream::Ch>(0x00u)); os.Put(static_cast<typename OutputByteStream::Ch>(0x00u));
os.Put(static_cast<typename OutputByteStream::Ch>(0x00u)); os.Put(static_cast<typename OutputByteStream::Ch>(0x00u));
@@ -521,7 +602,7 @@ struct UTF32BE : UTF32<CharType> {
} }
template <typename OutputByteStream> template <typename OutputByteStream>
static void Put(OutputByteStream& os, CharType c) { static void Put(OutputByteStream &os, CharType c) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1); RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
os.Put(static_cast<typename OutputByteStream::Ch>((c >> 24) & 0xFFu)); os.Put(static_cast<typename OutputByteStream::Ch>((c >> 24) & 0xFFu));
os.Put(static_cast<typename OutputByteStream::Ch>((c >> 16) & 0xFFu)); os.Put(static_cast<typename OutputByteStream::Ch>((c >> 16) & 0xFFu));
@@ -538,59 +619,57 @@ struct UTF32BE : UTF32<CharType> {
\tparam CharType Code unit for storing 7-bit ASCII data. Default is char. \tparam CharType Code unit for storing 7-bit ASCII data. Default is char.
\note implements Encoding concept \note implements Encoding concept
*/ */
template<typename CharType = char> template <typename CharType = char> struct ASCII {
struct ASCII {
typedef CharType Ch; typedef CharType Ch;
enum { supportUnicode = 0 }; enum { supportUnicode = 0 };
template<typename OutputStream> template <typename OutputStream>
static void Encode(OutputStream& os, unsigned codepoint) { static void Encode(OutputStream &os, unsigned codepoint) {
RAPIDJSON_ASSERT(codepoint <= 0x7F); RAPIDJSON_ASSERT(codepoint <= 0x7F);
os.Put(static_cast<Ch>(codepoint & 0xFF)); os.Put(static_cast<Ch>(codepoint & 0xFF));
} }
template<typename OutputStream> template <typename OutputStream>
static void EncodeUnsafe(OutputStream& os, unsigned codepoint) { static void EncodeUnsafe(OutputStream &os, unsigned codepoint) {
RAPIDJSON_ASSERT(codepoint <= 0x7F); RAPIDJSON_ASSERT(codepoint <= 0x7F);
PutUnsafe(os, static_cast<Ch>(codepoint & 0xFF)); PutUnsafe(os, static_cast<Ch>(codepoint & 0xFF));
} }
template <typename InputStream> template <typename InputStream>
static bool Decode(InputStream& is, unsigned* codepoint) { static bool Decode(InputStream &is, unsigned *codepoint) {
uint8_t c = static_cast<uint8_t>(is.Take()); uint8_t c = static_cast<uint8_t>(is.Take());
*codepoint = c; *codepoint = c;
return c <= 0X7F; return c <= 0X7F;
} }
template <typename InputStream, typename OutputStream> template <typename InputStream, typename OutputStream>
static bool Validate(InputStream& is, OutputStream& os) { static bool Validate(InputStream &is, OutputStream &os) {
uint8_t c = static_cast<uint8_t>(is.Take()); uint8_t c = static_cast<uint8_t>(is.Take());
os.Put(static_cast<typename OutputStream::Ch>(c)); os.Put(static_cast<typename OutputStream::Ch>(c));
return c <= 0x7F; return c <= 0x7F;
} }
template <typename InputByteStream> template <typename InputByteStream>
static CharType TakeBOM(InputByteStream& is) { static CharType TakeBOM(InputByteStream &is) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1); RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
uint8_t c = static_cast<uint8_t>(Take(is)); uint8_t c = static_cast<uint8_t>(Take(is));
return static_cast<Ch>(c); return static_cast<Ch>(c);
} }
template <typename InputByteStream> template <typename InputByteStream> static Ch Take(InputByteStream &is) {
static Ch Take(InputByteStream& is) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1); RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
return static_cast<Ch>(is.Take()); return static_cast<Ch>(is.Take());
} }
template <typename OutputByteStream> template <typename OutputByteStream>
static void PutBOM(OutputByteStream& os) { static void PutBOM(OutputByteStream &os) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1); RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
(void)os; (void)os;
} }
template <typename OutputByteStream> template <typename OutputByteStream>
static void Put(OutputByteStream& os, Ch c) { static void Put(OutputByteStream &os, Ch c) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1); RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
os.Put(static_cast<typename OutputByteStream::Ch>(c)); os.Put(static_cast<typename OutputByteStream::Ch>(c));
} }
@@ -608,42 +687,48 @@ enum UTFType {
kUTF32BE = 4 //!< UTF-32 big endian. kUTF32BE = 4 //!< UTF-32 big endian.
}; };
//! Dynamically select encoding according to stream's runtime-specified UTF encoding type. //! Dynamically select encoding according to stream's runtime-specified UTF
/*! \note This class can be used with AutoUTFInputtStream and AutoUTFOutputStream, which provides GetType(). //! encoding type.
*/ /*! \note This class can be used with AutoUTFInputtStream and
template<typename CharType> * AutoUTFOutputStream, which provides GetType().
struct AutoUTF { */
template <typename CharType> struct AutoUTF {
typedef CharType Ch; typedef CharType Ch;
enum { supportUnicode = 1 }; enum { supportUnicode = 1 };
#define RAPIDJSON_ENCODINGS_FUNC(x) UTF8<Ch>::x, UTF16LE<Ch>::x, UTF16BE<Ch>::x, UTF32LE<Ch>::x, UTF32BE<Ch>::x #define RAPIDJSON_ENCODINGS_FUNC(x) \
UTF8<Ch>::x, UTF16LE<Ch>::x, UTF16BE<Ch>::x, UTF32LE<Ch>::x, UTF32BE<Ch>::x
template<typename OutputStream> template <typename OutputStream>
static RAPIDJSON_FORCEINLINE void Encode(OutputStream& os, unsigned codepoint) { static RAPIDJSON_FORCEINLINE void Encode(OutputStream &os,
typedef void (*EncodeFunc)(OutputStream&, unsigned); unsigned codepoint) {
static const EncodeFunc f[] = { RAPIDJSON_ENCODINGS_FUNC(Encode) }; typedef void (*EncodeFunc)(OutputStream &, unsigned);
static const EncodeFunc f[] = {RAPIDJSON_ENCODINGS_FUNC(Encode)};
(*f[os.GetType()])(os, codepoint); (*f[os.GetType()])(os, codepoint);
} }
template<typename OutputStream> template <typename OutputStream>
static RAPIDJSON_FORCEINLINE void EncodeUnsafe(OutputStream& os, unsigned codepoint) { static RAPIDJSON_FORCEINLINE void EncodeUnsafe(OutputStream &os,
typedef void (*EncodeFunc)(OutputStream&, unsigned); unsigned codepoint) {
static const EncodeFunc f[] = { RAPIDJSON_ENCODINGS_FUNC(EncodeUnsafe) }; typedef void (*EncodeFunc)(OutputStream &, unsigned);
static const EncodeFunc f[] = {RAPIDJSON_ENCODINGS_FUNC(EncodeUnsafe)};
(*f[os.GetType()])(os, codepoint); (*f[os.GetType()])(os, codepoint);
} }
template <typename InputStream> template <typename InputStream>
static RAPIDJSON_FORCEINLINE bool Decode(InputStream& is, unsigned* codepoint) { static RAPIDJSON_FORCEINLINE bool Decode(InputStream &is,
typedef bool (*DecodeFunc)(InputStream&, unsigned*); unsigned *codepoint) {
static const DecodeFunc f[] = { RAPIDJSON_ENCODINGS_FUNC(Decode) }; typedef bool (*DecodeFunc)(InputStream &, unsigned *);
static const DecodeFunc f[] = {RAPIDJSON_ENCODINGS_FUNC(Decode)};
return (*f[is.GetType()])(is, codepoint); return (*f[is.GetType()])(is, codepoint);
} }
template <typename InputStream, typename OutputStream> template <typename InputStream, typename OutputStream>
static RAPIDJSON_FORCEINLINE bool Validate(InputStream& is, OutputStream& os) { static RAPIDJSON_FORCEINLINE bool Validate(InputStream &is,
typedef bool (*ValidateFunc)(InputStream&, OutputStream&); OutputStream &os) {
static const ValidateFunc f[] = { RAPIDJSON_ENCODINGS_FUNC(Validate) }; typedef bool (*ValidateFunc)(InputStream &, OutputStream &);
static const ValidateFunc f[] = {RAPIDJSON_ENCODINGS_FUNC(Validate)};
return (*f[is.GetType()])(is, os); return (*f[is.GetType()])(is, os);
} }
@@ -654,11 +739,12 @@ struct AutoUTF {
// Transcoder // Transcoder
//! Encoding conversion. //! Encoding conversion.
template<typename SourceEncoding, typename TargetEncoding> template <typename SourceEncoding, typename TargetEncoding> struct Transcoder {
struct Transcoder { //! Take one Unicode codepoint from source encoding, convert it to target
//! Take one Unicode codepoint from source encoding, convert it to target encoding and put it to the output stream. //! encoding and put it to the output stream.
template<typename InputStream, typename OutputStream> template <typename InputStream, typename OutputStream>
static RAPIDJSON_FORCEINLINE bool Transcode(InputStream& is, OutputStream& os) { static RAPIDJSON_FORCEINLINE bool Transcode(InputStream &is,
OutputStream &os) {
unsigned codepoint; unsigned codepoint;
if (!SourceEncoding::Decode(is, &codepoint)) if (!SourceEncoding::Decode(is, &codepoint))
return false; return false;
@@ -666,8 +752,9 @@ struct Transcoder {
return true; return true;
} }
template<typename InputStream, typename OutputStream> template <typename InputStream, typename OutputStream>
static RAPIDJSON_FORCEINLINE bool TranscodeUnsafe(InputStream& is, OutputStream& os) { static RAPIDJSON_FORCEINLINE bool TranscodeUnsafe(InputStream &is,
OutputStream &os) {
unsigned codepoint; unsigned codepoint;
if (!SourceEncoding::Decode(is, &codepoint)) if (!SourceEncoding::Decode(is, &codepoint))
return false; return false;
@@ -676,33 +763,39 @@ struct Transcoder {
} }
//! Validate one Unicode codepoint from an encoded stream. //! Validate one Unicode codepoint from an encoded stream.
template<typename InputStream, typename OutputStream> template <typename InputStream, typename OutputStream>
static RAPIDJSON_FORCEINLINE bool Validate(InputStream& is, OutputStream& os) { static RAPIDJSON_FORCEINLINE bool Validate(InputStream &is,
return Transcode(is, os); // Since source/target encoding is different, must transcode. OutputStream &os) {
return Transcode(
is, os); // Since source/target encoding is different, must transcode.
} }
}; };
// Forward declaration. // Forward declaration.
template<typename Stream> template <typename Stream>
inline void PutUnsafe(Stream& stream, typename Stream::Ch c); inline void PutUnsafe(Stream &stream, typename Stream::Ch c);
//! Specialization of Transcoder with same source and target encoding. //! Specialization of Transcoder with same source and target encoding.
template<typename Encoding> template <typename Encoding> struct Transcoder<Encoding, Encoding> {
struct Transcoder<Encoding, Encoding> { template <typename InputStream, typename OutputStream>
template<typename InputStream, typename OutputStream> static RAPIDJSON_FORCEINLINE bool Transcode(InputStream &is,
static RAPIDJSON_FORCEINLINE bool Transcode(InputStream& is, OutputStream& os) { OutputStream &os) {
os.Put(is.Take()); // Just copy one code unit. This semantic is different from primary template class. os.Put(is.Take()); // Just copy one code unit. This semantic is different
// from primary template class.
return true; return true;
} }
template<typename InputStream, typename OutputStream> template <typename InputStream, typename OutputStream>
static RAPIDJSON_FORCEINLINE bool TranscodeUnsafe(InputStream& is, OutputStream& os) { static RAPIDJSON_FORCEINLINE bool TranscodeUnsafe(InputStream &is,
PutUnsafe(os, is.Take()); // Just copy one code unit. This semantic is different from primary template class. OutputStream &os) {
PutUnsafe(os, is.Take()); // Just copy one code unit. This semantic is
// different from primary template class.
return true; return true;
} }
template<typename InputStream, typename OutputStream> template <typename InputStream, typename OutputStream>
static RAPIDJSON_FORCEINLINE bool Validate(InputStream& is, OutputStream& os) { static RAPIDJSON_FORCEINLINE bool Validate(InputStream &is,
OutputStream &os) {
return Encoding::Validate(is, os); // source/target encoding are the same return Encoding::Validate(is, os); // source/target encoding are the same
} }
}; };

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available. // Tencent is pleased to support the open source community by making RapidJSON
// available.
// //
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved. // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
// //
// Licensed under the MIT License (the "License"); you may not use this file except // Licensed under the MIT License (the "License"); you may not use this file
// in compliance with the License. You may obtain a copy of the License at // except in compliance with the License. You may obtain a copy of the License
// at
// //
// http://opensource.org/licenses/MIT // http://opensource.org/licenses/MIT
// //
// Unless required by applicable law or agreed to in writing, software distributed // Unless required by applicable law or agreed to in writing, software
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// CONDITIONS OF ANY KIND, either express or implied. See the License for the // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// specific language governing permissions and limitations under the License. // License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_ERROR_EN_H_ #ifndef RAPIDJSON_ERROR_EN_H_
#define RAPIDJSON_ERROR_EN_H_ #define RAPIDJSON_ERROR_EN_H_
@@ -19,8 +23,8 @@
#ifdef __clang__ #ifdef __clang__
RAPIDJSON_DIAG_PUSH RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(switch-enum) RAPIDJSON_DIAG_OFF(switch - enum)
RAPIDJSON_DIAG_OFF(covered-switch-default) RAPIDJSON_DIAG_OFF(covered - switch - default)
#endif #endif
RAPIDJSON_NAMESPACE_BEGIN RAPIDJSON_NAMESPACE_BEGIN
@@ -33,35 +37,61 @@ RAPIDJSON_NAMESPACE_BEGIN
\note User can make a copy of this function for localization. \note User can make a copy of this function for localization.
Using switch-case is safer for future modification of error codes. Using switch-case is safer for future modification of error codes.
*/ */
inline const RAPIDJSON_ERROR_CHARTYPE* GetParseError_En(ParseErrorCode parseErrorCode) { inline const RAPIDJSON_ERROR_CHARTYPE *
GetParseError_En(ParseErrorCode parseErrorCode) {
switch (parseErrorCode) { switch (parseErrorCode) {
case kParseErrorNone: return RAPIDJSON_ERROR_STRING("No error."); case kParseErrorNone:
return RAPIDJSON_ERROR_STRING("No error.");
case kParseErrorDocumentEmpty: return RAPIDJSON_ERROR_STRING("The document is empty."); case kParseErrorDocumentEmpty:
case kParseErrorDocumentRootNotSingular: return RAPIDJSON_ERROR_STRING("The document root must not be followed by other values."); return RAPIDJSON_ERROR_STRING("The document is empty.");
case kParseErrorDocumentRootNotSingular:
return RAPIDJSON_ERROR_STRING(
"The document root must not be followed by other values.");
case kParseErrorValueInvalid: return RAPIDJSON_ERROR_STRING("Invalid value."); case kParseErrorValueInvalid:
return RAPIDJSON_ERROR_STRING("Invalid value.");
case kParseErrorObjectMissName: return RAPIDJSON_ERROR_STRING("Missing a name for object member."); case kParseErrorObjectMissName:
case kParseErrorObjectMissColon: return RAPIDJSON_ERROR_STRING("Missing a colon after a name of object member."); return RAPIDJSON_ERROR_STRING("Missing a name for object member.");
case kParseErrorObjectMissCommaOrCurlyBracket: return RAPIDJSON_ERROR_STRING("Missing a comma or '}' after an object member."); case kParseErrorObjectMissColon:
return RAPIDJSON_ERROR_STRING(
"Missing a colon after a name of object member.");
case kParseErrorObjectMissCommaOrCurlyBracket:
return RAPIDJSON_ERROR_STRING(
"Missing a comma or '}' after an object member.");
case kParseErrorArrayMissCommaOrSquareBracket: return RAPIDJSON_ERROR_STRING("Missing a comma or ']' after an array element."); case kParseErrorArrayMissCommaOrSquareBracket:
return RAPIDJSON_ERROR_STRING(
"Missing a comma or ']' after an array element.");
case kParseErrorStringUnicodeEscapeInvalidHex: return RAPIDJSON_ERROR_STRING("Incorrect hex digit after \\u escape in string."); case kParseErrorStringUnicodeEscapeInvalidHex:
case kParseErrorStringUnicodeSurrogateInvalid: return RAPIDJSON_ERROR_STRING("The surrogate pair in string is invalid."); return RAPIDJSON_ERROR_STRING(
case kParseErrorStringEscapeInvalid: return RAPIDJSON_ERROR_STRING("Invalid escape character in string."); "Incorrect hex digit after \\u escape in string.");
case kParseErrorStringMissQuotationMark: return RAPIDJSON_ERROR_STRING("Missing a closing quotation mark in string."); case kParseErrorStringUnicodeSurrogateInvalid:
case kParseErrorStringInvalidEncoding: return RAPIDJSON_ERROR_STRING("Invalid encoding in string."); 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 kParseErrorNumberTooBig:
case kParseErrorNumberMissFraction: return RAPIDJSON_ERROR_STRING("Miss fraction part in number."); return RAPIDJSON_ERROR_STRING("Number too big to be stored in double.");
case kParseErrorNumberMissExponent: return RAPIDJSON_ERROR_STRING("Miss exponent in number."); 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 kParseErrorTermination:
case kParseErrorUnspecificSyntaxError: return RAPIDJSON_ERROR_STRING("Unspecific syntax error."); return RAPIDJSON_ERROR_STRING("Terminate parsing due to Handler error.");
case kParseErrorUnspecificSyntaxError:
return RAPIDJSON_ERROR_STRING("Unspecific syntax error.");
default: return RAPIDJSON_ERROR_STRING("Unknown error."); default:
return RAPIDJSON_ERROR_STRING("Unknown error.");
} }
} }

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

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

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

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

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

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

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available. // Tencent is pleased to support the open source community by making RapidJSON
// available.
// //
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved. // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
// //
// Licensed under the MIT License (the "License"); you may not use this file except // Licensed under the MIT License (the "License"); you may not use this file
// in compliance with the License. You may obtain a copy of the License at // except in compliance with the License. You may obtain a copy of the License
// at
// //
// http://opensource.org/licenses/MIT // http://opensource.org/licenses/MIT
// //
// Unless required by applicable law or agreed to in writing, software distributed // Unless required by applicable law or agreed to in writing, software
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// CONDITIONS OF ANY KIND, either express or implied. See the License for the // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// specific language governing permissions and limitations under the License. // License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_FWD_H_ #ifndef RAPIDJSON_FWD_H_
#define RAPIDJSON_FWD_H_ #define RAPIDJSON_FWD_H_
@@ -21,42 +25,37 @@ RAPIDJSON_NAMESPACE_BEGIN
// encodings.h // encodings.h
template<typename CharType> struct UTF8; template <typename CharType> struct UTF8;
template<typename CharType> struct UTF16; template <typename CharType> struct UTF16;
template<typename CharType> struct UTF16BE; template <typename CharType> struct UTF16BE;
template<typename CharType> struct UTF16LE; template <typename CharType> struct UTF16LE;
template<typename CharType> struct UTF32; template <typename CharType> struct UTF32;
template<typename CharType> struct UTF32BE; template <typename CharType> struct UTF32BE;
template<typename CharType> struct UTF32LE; template <typename CharType> struct UTF32LE;
template<typename CharType> struct ASCII; template <typename CharType> struct ASCII;
template<typename CharType> struct AutoUTF; template <typename CharType> struct AutoUTF;
template<typename SourceEncoding, typename TargetEncoding> template <typename SourceEncoding, typename TargetEncoding> struct Transcoder;
struct Transcoder;
// allocators.h // allocators.h
class CrtAllocator; class CrtAllocator;
template <typename BaseAllocator> template <typename BaseAllocator> class MemoryPoolAllocator;
class MemoryPoolAllocator;
// stream.h // stream.h
template <typename Encoding> template <typename Encoding> struct GenericStringStream;
struct GenericStringStream;
typedef GenericStringStream<UTF8<char> > StringStream; typedef GenericStringStream<UTF8<char>> StringStream;
template <typename Encoding> template <typename Encoding> struct GenericInsituStringStream;
struct GenericInsituStringStream;
typedef GenericInsituStringStream<UTF8<char> > InsituStringStream; typedef GenericInsituStringStream<UTF8<char>> InsituStringStream;
// stringbuffer.h // stringbuffer.h
template <typename Encoding, typename Allocator> template <typename Encoding, typename Allocator> class GenericStringBuffer;
class GenericStringBuffer;
typedef GenericStringBuffer<UTF8<char>, CrtAllocator> StringBuffer; typedef GenericStringBuffer<UTF8<char>, CrtAllocator> StringBuffer;
@@ -70,8 +69,7 @@ class FileWriteStream;
// memorybuffer.h // memorybuffer.h
template <typename Allocator> template <typename Allocator> struct GenericMemoryBuffer;
struct GenericMemoryBuffer;
typedef GenericMemoryBuffer<CrtAllocator> MemoryBuffer; typedef GenericMemoryBuffer<CrtAllocator> MemoryBuffer;
@@ -81,49 +79,49 @@ struct MemoryStream;
// reader.h // reader.h
template<typename Encoding, typename Derived> template <typename Encoding, typename Derived> struct BaseReaderHandler;
struct BaseReaderHandler;
template <typename SourceEncoding, typename TargetEncoding, typename StackAllocator> template <typename SourceEncoding, typename TargetEncoding,
typename StackAllocator>
class GenericReader; class GenericReader;
typedef GenericReader<UTF8<char>, UTF8<char>, CrtAllocator> Reader; typedef GenericReader<UTF8<char>, UTF8<char>, CrtAllocator> Reader;
// writer.h // 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; class Writer;
// prettywriter.h // 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; class PrettyWriter;
// document.h // document.h
template <typename Encoding, typename Allocator> template <typename Encoding, typename Allocator> class GenericMember;
class GenericMember;
template <bool Const, typename Encoding, typename Allocator> template <bool Const, typename Encoding, typename Allocator>
class GenericMemberIterator; class GenericMemberIterator;
template<typename CharType> template <typename CharType> struct GenericStringRef;
struct GenericStringRef;
template <typename Encoding, typename Allocator> template <typename Encoding, typename Allocator> class GenericValue;
class GenericValue;
typedef GenericValue<UTF8<char>, MemoryPoolAllocator<CrtAllocator> > Value; typedef GenericValue<UTF8<char>, MemoryPoolAllocator<CrtAllocator>> Value;
template <typename Encoding, typename Allocator, typename StackAllocator> template <typename Encoding, typename Allocator, typename StackAllocator>
class GenericDocument; class GenericDocument;
typedef GenericDocument<UTF8<char>, MemoryPoolAllocator<CrtAllocator>, CrtAllocator> Document; typedef GenericDocument<UTF8<char>, MemoryPoolAllocator<CrtAllocator>,
CrtAllocator>
Document;
// pointer.h // pointer.h
template <typename ValueType, typename Allocator> template <typename ValueType, typename Allocator> class GenericPointer;
class GenericPointer;
typedef GenericPointer<Value, CrtAllocator> Pointer; typedef GenericPointer<Value, CrtAllocator> Pointer;
@@ -132,19 +130,19 @@ typedef GenericPointer<Value, CrtAllocator> Pointer;
template <typename SchemaDocumentType> template <typename SchemaDocumentType>
class IGenericRemoteSchemaDocumentProvider; class IGenericRemoteSchemaDocumentProvider;
template <typename ValueT, typename Allocator> template <typename ValueT, typename Allocator> class GenericSchemaDocument;
class GenericSchemaDocument;
typedef GenericSchemaDocument<Value, CrtAllocator> SchemaDocument; typedef GenericSchemaDocument<Value, CrtAllocator> SchemaDocument;
typedef IGenericRemoteSchemaDocumentProvider<SchemaDocument> IRemoteSchemaDocumentProvider; typedef IGenericRemoteSchemaDocumentProvider<SchemaDocument>
IRemoteSchemaDocumentProvider;
template < template <typename SchemaDocumentType, typename OutputHandler,
typename SchemaDocumentType,
typename OutputHandler,
typename StateAllocator> typename StateAllocator>
class GenericSchemaValidator; class GenericSchemaValidator;
typedef GenericSchemaValidator<SchemaDocument, BaseReaderHandler<UTF8<char>, void>, CrtAllocator> SchemaValidator; typedef GenericSchemaValidator<
SchemaDocument, BaseReaderHandler<UTF8<char>, void>, CrtAllocator>
SchemaValidator;
RAPIDJSON_NAMESPACE_END RAPIDJSON_NAMESPACE_END

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available. // Tencent is pleased to support the open source community by making RapidJSON
// available.
// //
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved. // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
// //
// Licensed under the MIT License (the "License"); you may not use this file except // Licensed under the MIT License (the "License"); you may not use this file
// in compliance with the License. You may obtain a copy of the License at // except in compliance with the License. You may obtain a copy of the License
// at
// //
// http://opensource.org/licenses/MIT // http://opensource.org/licenses/MIT
// //
// Unless required by applicable law or agreed to in writing, software distributed // Unless required by applicable law or agreed to in writing, software
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// CONDITIONS OF ANY KIND, either express or implied. See the License for the // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// specific language governing permissions and limitations under the License. // License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_BIGINTEGER_H_ #ifndef RAPIDJSON_BIGINTEGER_H_
#define RAPIDJSON_BIGINTEGER_H_ #define RAPIDJSON_BIGINTEGER_H_
@@ -29,19 +33,18 @@ class BigInteger {
public: public:
typedef uint64_t Type; typedef uint64_t Type;
BigInteger(const BigInteger& rhs) : count_(rhs.count_) { BigInteger(const BigInteger &rhs) : count_(rhs.count_) {
std::memcpy(digits_, rhs.digits_, count_ * sizeof(Type)); std::memcpy(digits_, rhs.digits_, count_ * sizeof(Type));
} }
explicit BigInteger(uint64_t u) : count_(1) { explicit BigInteger(uint64_t u) : count_(1) { digits_[0] = u; }
digits_[0] = u;
}
BigInteger(const char* decimals, size_t length) : count_(1) { BigInteger(const char *decimals, size_t length) : count_(1) {
RAPIDJSON_ASSERT(length > 0); RAPIDJSON_ASSERT(length > 0);
digits_[0] = 0; digits_[0] = 0;
size_t i = 0; size_t i = 0;
const size_t kMaxDigitPerIteration = 19; // 2^64 = 18446744073709551616 > 10^19 const size_t kMaxDigitPerIteration =
19; // 2^64 = 18446744073709551616 > 10^19
while (length >= kMaxDigitPerIteration) { while (length >= kMaxDigitPerIteration) {
AppendDecimal64(decimals + i, decimals + i + kMaxDigitPerIteration); AppendDecimal64(decimals + i, decimals + i + kMaxDigitPerIteration);
length -= kMaxDigitPerIteration; length -= kMaxDigitPerIteration;
@@ -52,8 +55,7 @@ public:
AppendDecimal64(decimals + i, decimals + i + length); AppendDecimal64(decimals + i, decimals + i + length);
} }
BigInteger& operator=(const BigInteger &rhs) BigInteger &operator=(const BigInteger &rhs) {
{
if (this != &rhs) { if (this != &rhs) {
count_ = rhs.count_; count_ = rhs.count_;
std::memcpy(digits_, rhs.digits_, count_ * sizeof(Type)); std::memcpy(digits_, rhs.digits_, count_ * sizeof(Type));
@@ -61,13 +63,13 @@ public:
return *this; return *this;
} }
BigInteger& operator=(uint64_t u) { BigInteger &operator=(uint64_t u) {
digits_[0] = u; digits_[0] = u;
count_ = 1; count_ = 1;
return *this; return *this;
} }
BigInteger& operator+=(uint64_t u) { BigInteger &operator+=(uint64_t u) {
Type backup = digits_[0]; Type backup = digits_[0];
digits_[0] += u; digits_[0] += u;
for (size_t i = 0; i < count_ - 1; i++) { for (size_t i = 0; i < count_ - 1; i++) {
@@ -84,10 +86,13 @@ public:
return *this; return *this;
} }
BigInteger& operator*=(uint64_t u) { BigInteger &operator*=(uint64_t u) {
if (u == 0) return *this = 0; if (u == 0)
if (u == 1) return *this; return *this = 0;
if (*this == 1) return *this = u; if (u == 1)
return *this;
if (*this == 1)
return *this = u;
uint64_t k = 0; uint64_t k = 0;
for (size_t i = 0; i < count_; i++) { for (size_t i = 0; i < count_; i++) {
@@ -102,10 +107,13 @@ public:
return *this; return *this;
} }
BigInteger& operator*=(uint32_t u) { BigInteger &operator*=(uint32_t u) {
if (u == 0) return *this = 0; if (u == 0)
if (u == 1) return *this; return *this = 0;
if (*this == 1) return *this = u; if (u == 1)
return *this;
if (*this == 1)
return *this = u;
uint64_t k = 0; uint64_t k = 0;
for (size_t i = 0; i < count_; i++) { for (size_t i = 0; i < count_; i++) {
@@ -125,8 +133,9 @@ public:
return *this; return *this;
} }
BigInteger& operator<<=(size_t shift) { BigInteger &operator<<=(size_t shift) {
if (IsZero() || shift == 0) return *this; if (IsZero() || shift == 0)
return *this;
size_t offset = shift / kTypeBit; size_t offset = shift / kTypeBit;
size_t interShift = shift % kTypeBit; size_t interShift = shift % kTypeBit;
@@ -135,11 +144,11 @@ public:
if (interShift == 0) { if (interShift == 0) {
std::memmove(digits_ + offset, digits_, count_ * sizeof(Type)); std::memmove(digits_ + offset, digits_, count_ * sizeof(Type));
count_ += offset; count_ += offset;
} } else {
else {
digits_[count_] = 0; digits_[count_] = 0;
for (size_t i = count_; i > 0; i--) for (size_t i = count_; i > 0; i--)
digits_[i + offset] = (digits_[i] << interShift) | (digits_[i - 1] >> (kTypeBit - interShift)); digits_[i + offset] = (digits_[i] << interShift) |
(digits_[i - 1] >> (kTypeBit - interShift));
digits_[offset] = digits_[0] << interShift; digits_[offset] = digits_[0] << interShift;
count_ += offset; count_ += offset;
if (digits_[count_]) if (digits_[count_])
@@ -151,15 +160,16 @@ public:
return *this; return *this;
} }
bool operator==(const BigInteger& rhs) const { bool operator==(const BigInteger &rhs) const {
return count_ == rhs.count_ && std::memcmp(digits_, rhs.digits_, count_ * sizeof(Type)) == 0; return count_ == rhs.count_ &&
std::memcmp(digits_, rhs.digits_, count_ * sizeof(Type)) == 0;
} }
bool operator==(const Type rhs) const { bool operator==(const Type rhs) const {
return count_ == 1 && digits_[0] == rhs; return count_ == 1 && digits_[0] == rhs;
} }
BigInteger& MultiplyPow5(unsigned exp) { BigInteger &MultiplyPow5(unsigned exp) {
static const uint32_t kPow5[12] = { static const uint32_t kPow5[12] = {
5, 5,
5 * 5, 5 * 5,
@@ -172,24 +182,34 @@ public:
5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5, 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5,
5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5, 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5,
5 * 5 * 5 * 5 * 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)
if (exp == 0) return *this; return *this;
for (; exp >= 27; exp -= 27) *this *= RAPIDJSON_UINT64_C2(0X6765C793, 0XFA10079D); // 5^27 for (; exp >= 27; exp -= 27)
for (; exp >= 13; exp -= 13) *this *= static_cast<uint32_t>(1220703125u); // 5^13 *this *= RAPIDJSON_UINT64_C2(0X6765C793, 0XFA10079D); // 5^27
if (exp > 0) *this *= kPow5[exp - 1]; for (; exp >= 13; exp -= 13)
*this *= static_cast<uint32_t>(1220703125u); // 5^13
if (exp > 0)
*this *= kPow5[exp - 1];
return *this; return *this;
} }
// Compute absolute difference of this and rhs. // Compute absolute difference of this and rhs.
// Assume this != rhs // Assume this != rhs
bool Difference(const BigInteger& rhs, BigInteger* out) const { bool Difference(const BigInteger &rhs, BigInteger *out) const {
int cmp = Compare(rhs); int cmp = Compare(rhs);
RAPIDJSON_ASSERT(cmp != 0); RAPIDJSON_ASSERT(cmp != 0);
const BigInteger *a, *b; // Makes a > b const BigInteger *a, *b; // Makes a > b
bool ret; bool ret;
if (cmp < 0) { a = &rhs; b = this; ret = true; } if (cmp < 0) {
else { a = this; b = &rhs; ret = false; } a = &rhs;
b = this;
ret = true;
} else {
a = this;
b = &rhs;
ret = false;
}
Type borrow = 0; Type borrow = 0;
for (size_t i = 0; i < a->count_; i++) { for (size_t i = 0; i < a->count_; i++) {
@@ -205,7 +225,7 @@ public:
return ret; return ret;
} }
int Compare(const BigInteger& rhs) const { int Compare(const BigInteger &rhs) const {
if (count_ != rhs.count_) if (count_ != rhs.count_)
return count_ < rhs.count_ ? -1 : 1; return count_ < rhs.count_ ? -1 : 1;
@@ -217,11 +237,14 @@ public:
} }
size_t GetCount() const { return count_; } size_t GetCount() const { return count_; }
Type GetDigit(size_t index) const { RAPIDJSON_ASSERT(index < count_); return digits_[index]; } Type GetDigit(size_t index) const {
RAPIDJSON_ASSERT(index < count_);
return digits_[index];
}
bool IsZero() const { return count_ == 1 && digits_[0] == 0; } bool IsZero() const { return count_ == 1 && digits_[0] == 0; }
private: private:
void AppendDecimal64(const char* begin, const char* end) { void AppendDecimal64(const char *begin, const char *end) {
uint64_t u = ParseUint64(begin, end); uint64_t u = ParseUint64(begin, end);
if (IsZero()) if (IsZero())
*this = u; *this = u;
@@ -236,9 +259,9 @@ private:
digits_[count_++] = digit; digits_[count_++] = digit;
} }
static uint64_t ParseUint64(const char* begin, const char* end) { static uint64_t ParseUint64(const char *begin, const char *end) {
uint64_t r = 0; uint64_t r = 0;
for (const char* p = begin; p != end; ++p) { for (const char *p = begin; p != end; ++p) {
RAPIDJSON_ASSERT(*p >= '0' && *p <= '9'); RAPIDJSON_ASSERT(*p >= '0' && *p <= '9');
r = r * 10u + static_cast<unsigned>(*p - '0'); r = r * 10u + static_cast<unsigned>(*p - '0');
} }
@@ -246,20 +269,23 @@ private:
} }
// Assume a * b + k < 2^128 // Assume a * b + k < 2^128
static uint64_t MulAdd64(uint64_t a, uint64_t b, uint64_t k, uint64_t* outHigh) { static uint64_t MulAdd64(uint64_t a, uint64_t b, uint64_t k,
uint64_t *outHigh) {
#if defined(_MSC_VER) && defined(_M_AMD64) #if defined(_MSC_VER) && defined(_M_AMD64)
uint64_t low = _umul128(a, b, outHigh) + k; uint64_t low = _umul128(a, b, outHigh) + k;
if (low < k) if (low < k)
(*outHigh)++; (*outHigh)++;
return low; return low;
#elif (__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 6)) && defined(__x86_64__) #elif (__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 6)) && \
defined(__x86_64__)
__extension__ typedef unsigned __int128 uint128; __extension__ typedef unsigned __int128 uint128;
uint128 p = static_cast<uint128>(a) * static_cast<uint128>(b); uint128 p = static_cast<uint128>(a) * static_cast<uint128>(b);
p += k; p += k;
*outHigh = static_cast<uint64_t>(p >> 64); *outHigh = static_cast<uint64_t>(p >> 64);
return static_cast<uint64_t>(p); return static_cast<uint64_t>(p);
#else #else
const uint64_t a0 = a & 0xFFFFFFFF, a1 = a >> 32, b0 = b & 0xFFFFFFFF, b1 = b >> 32; const uint64_t a0 = a & 0xFFFFFFFF, a1 = a >> 32, b0 = b & 0xFFFFFFFF,
b1 = b >> 32;
uint64_t x0 = a0 * b0, x1 = a0 * b1, x2 = a1 * b0, x3 = a1 * b1; uint64_t x0 = a0 * b0, x1 = a0 * b1, x2 = a1 * b0, x3 = a1 * b1;
x1 += (x0 >> 32); // can't give carry x1 += (x0 >> 32); // can't give carry
x1 += x2; x1 += x2;

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

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

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

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

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

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

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

83
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 // Licensed under the MIT License (the "License"); you may not use this file
// in compliance with the License. You may obtain a copy of the License at // except in compliance with the License. You may obtain a copy of the License
// at
// //
// http://opensource.org/licenses/MIT // http://opensource.org/licenses/MIT
// //
// Unless required by applicable law or agreed to in writing, software distributed // Unless required by applicable law or agreed to in writing, software
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// CONDITIONS OF ANY KIND, either express or implied. See the License for the // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// specific language governing permissions and limitations under the License. // License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_ITOA_ #ifndef RAPIDJSON_ITOA_
#define RAPIDJSON_ITOA_ #define RAPIDJSON_ITOA_
@@ -20,26 +24,29 @@
RAPIDJSON_NAMESPACE_BEGIN RAPIDJSON_NAMESPACE_BEGIN
namespace internal { namespace internal {
inline const char* GetDigitsLut() { inline const char *GetDigitsLut() {
static const char cDigitsLut[200] = { 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', '0', '0', '0', '1', '0', '2', '0', '3', '0', '4', '0', '5', '0', '6', '0',
'1','0','1','1','1','2','1','3','1','4','1','5','1','6','1','7','1','8','1','9', '7', '0', '8', '0', '9', '1', '0', '1', '1', '1', '2', '1', '3', '1', '4',
'2','0','2','1','2','2','2','3','2','4','2','5','2','6','2','7','2','8','2','9', '1', '5', '1', '6', '1', '7', '1', '8', '1', '9', '2', '0', '2', '1', '2',
'3','0','3','1','3','2','3','3','3','4','3','5','3','6','3','7','3','8','3','9', '2', '2', '3', '2', '4', '2', '5', '2', '6', '2', '7', '2', '8', '2', '9',
'4','0','4','1','4','2','4','3','4','4','4','5','4','6','4','7','4','8','4','9', '3', '0', '3', '1', '3', '2', '3', '3', '3', '4', '3', '5', '3', '6', '3',
'5','0','5','1','5','2','5','3','5','4','5','5','5','6','5','7','5','8','5','9', '7', '3', '8', '3', '9', '4', '0', '4', '1', '4', '2', '4', '3', '4', '4',
'6','0','6','1','6','2','6','3','6','4','6','5','6','6','6','7','6','8','6','9', '4', '5', '4', '6', '4', '7', '4', '8', '4', '9', '5', '0', '5', '1', '5',
'7','0','7','1','7','2','7','3','7','4','7','5','7','6','7','7','7','8','7','9', '2', '5', '3', '5', '4', '5', '5', '5', '6', '5', '7', '5', '8', '5', '9',
'8','0','8','1','8','2','8','3','8','4','8','5','8','6','8','7','8','8','8','9', '6', '0', '6', '1', '6', '2', '6', '3', '6', '4', '6', '5', '6', '6', '6',
'9','0','9','1','9','2','9','3','9','4','9','5','9','6','9','7','9','8','9','9' '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; return cDigitsLut;
} }
inline char* u32toa(uint32_t value, char* buffer) { inline char *u32toa(uint32_t value, char *buffer) {
RAPIDJSON_ASSERT(buffer != 0); RAPIDJSON_ASSERT(buffer != 0);
const char* cDigitsLut = GetDigitsLut(); const char *cDigitsLut = GetDigitsLut();
if (value < 10000) { if (value < 10000) {
const uint32_t d1 = (value / 100) << 1; const uint32_t d1 = (value / 100) << 1;
@@ -52,8 +59,7 @@ inline char* u32toa(uint32_t value, char* buffer) {
if (value >= 10) if (value >= 10)
*buffer++ = cDigitsLut[d2]; *buffer++ = cDigitsLut[d2];
*buffer++ = cDigitsLut[d2 + 1]; *buffer++ = cDigitsLut[d2 + 1];
} } else if (value < 100000000) {
else if (value < 100000000) {
// value = bbbbcccc // value = bbbbcccc
const uint32_t b = value / 10000; const uint32_t b = value / 10000;
const uint32_t c = value % 10000; const uint32_t c = value % 10000;
@@ -76,8 +82,7 @@ inline char* u32toa(uint32_t value, char* buffer) {
*buffer++ = cDigitsLut[d3 + 1]; *buffer++ = cDigitsLut[d3 + 1];
*buffer++ = cDigitsLut[d4]; *buffer++ = cDigitsLut[d4];
*buffer++ = cDigitsLut[d4 + 1]; *buffer++ = cDigitsLut[d4 + 1];
} } else {
else {
// value = aabbbbcccc in decimal // value = aabbbbcccc in decimal
const uint32_t a = value / 100000000; // 1 to 42 const uint32_t a = value / 100000000; // 1 to 42
@@ -87,8 +92,7 @@ inline char* u32toa(uint32_t value, char* buffer) {
const unsigned i = a << 1; const unsigned i = a << 1;
*buffer++ = cDigitsLut[i]; *buffer++ = cDigitsLut[i];
*buffer++ = cDigitsLut[i + 1]; *buffer++ = cDigitsLut[i + 1];
} } else
else
*buffer++ = static_cast<char>('0' + static_cast<char>(a)); *buffer++ = static_cast<char>('0' + static_cast<char>(a));
const uint32_t b = value / 10000; // 0 to 9999 const uint32_t b = value / 10000; // 0 to 9999
@@ -112,7 +116,7 @@ inline char* u32toa(uint32_t value, char* buffer) {
return buffer; return buffer;
} }
inline char* i32toa(int32_t value, char* buffer) { inline char *i32toa(int32_t value, char *buffer) {
RAPIDJSON_ASSERT(buffer != 0); RAPIDJSON_ASSERT(buffer != 0);
uint32_t u = static_cast<uint32_t>(value); uint32_t u = static_cast<uint32_t>(value);
if (value < 0) { if (value < 0) {
@@ -123,9 +127,9 @@ inline char* i32toa(int32_t value, char* buffer) {
return u32toa(u, buffer); return u32toa(u, buffer);
} }
inline char* u64toa(uint64_t value, char* buffer) { inline char *u64toa(uint64_t value, char *buffer) {
RAPIDJSON_ASSERT(buffer != 0); RAPIDJSON_ASSERT(buffer != 0);
const char* cDigitsLut = GetDigitsLut(); const char *cDigitsLut = GetDigitsLut();
const uint64_t kTen8 = 100000000; const uint64_t kTen8 = 100000000;
const uint64_t kTen9 = kTen8 * 10; const uint64_t kTen9 = kTen8 * 10;
const uint64_t kTen10 = kTen8 * 100; const uint64_t kTen10 = kTen8 * 100;
@@ -149,8 +153,7 @@ inline char* u64toa(uint64_t value, char* buffer) {
if (v >= 10) if (v >= 10)
*buffer++ = cDigitsLut[d2]; *buffer++ = cDigitsLut[d2];
*buffer++ = cDigitsLut[d2 + 1]; *buffer++ = cDigitsLut[d2 + 1];
} } else {
else {
// value = bbbbcccc // value = bbbbcccc
const uint32_t b = v / 10000; const uint32_t b = v / 10000;
const uint32_t c = v % 10000; const uint32_t c = v % 10000;
@@ -174,8 +177,7 @@ inline char* u64toa(uint64_t value, char* buffer) {
*buffer++ = cDigitsLut[d4]; *buffer++ = cDigitsLut[d4];
*buffer++ = cDigitsLut[d4 + 1]; *buffer++ = cDigitsLut[d4 + 1];
} }
} } else if (value < kTen16) {
else if (value < kTen16) {
const uint32_t v0 = static_cast<uint32_t>(value / kTen8); const uint32_t v0 = static_cast<uint32_t>(value / kTen8);
const uint32_t v1 = static_cast<uint32_t>(value % kTen8); const uint32_t v1 = static_cast<uint32_t>(value % kTen8);
@@ -221,8 +223,7 @@ inline char* u64toa(uint64_t value, char* buffer) {
*buffer++ = cDigitsLut[d7 + 1]; *buffer++ = cDigitsLut[d7 + 1];
*buffer++ = cDigitsLut[d8]; *buffer++ = cDigitsLut[d8];
*buffer++ = cDigitsLut[d8 + 1]; *buffer++ = cDigitsLut[d8 + 1];
} } else {
else {
const uint32_t a = static_cast<uint32_t>(value / kTen16); // 1 to 1844 const uint32_t a = static_cast<uint32_t>(value / kTen16); // 1 to 1844
value %= kTen16; value %= kTen16;
@@ -232,15 +233,13 @@ inline char* u64toa(uint64_t value, char* buffer) {
const uint32_t i = a << 1; const uint32_t i = a << 1;
*buffer++ = cDigitsLut[i]; *buffer++ = cDigitsLut[i];
*buffer++ = cDigitsLut[i + 1]; *buffer++ = cDigitsLut[i + 1];
} } else if (a < 1000) {
else if (a < 1000) {
*buffer++ = static_cast<char>('0' + static_cast<char>(a / 100)); *buffer++ = static_cast<char>('0' + static_cast<char>(a / 100));
const uint32_t i = (a % 100) << 1; const uint32_t i = (a % 100) << 1;
*buffer++ = cDigitsLut[i]; *buffer++ = cDigitsLut[i];
*buffer++ = cDigitsLut[i + 1]; *buffer++ = cDigitsLut[i + 1];
} } else {
else {
const uint32_t i = (a / 100) << 1; const uint32_t i = (a / 100) << 1;
const uint32_t j = (a % 100) << 1; const uint32_t j = (a % 100) << 1;
*buffer++ = cDigitsLut[i]; *buffer++ = cDigitsLut[i];
@@ -291,7 +290,7 @@ inline char* u64toa(uint64_t value, char* buffer) {
return buffer; return buffer;
} }
inline char* i64toa(int64_t value, char* buffer) { inline char *i64toa(int64_t value, char *buffer) {
RAPIDJSON_ASSERT(buffer != 0); RAPIDJSON_ASSERT(buffer != 0);
uint64_t u = static_cast<uint64_t>(value); uint64_t u = static_cast<uint64_t>(value);
if (value < 0) { if (value < 0) {

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

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

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available. // Tencent is pleased to support the open source community by making RapidJSON
// available.
// //
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved. // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
// //
// Licensed under the MIT License (the "License"); you may not use this file except // Licensed under the MIT License (the "License"); you may not use this file
// in compliance with the License. You may obtain a copy of the License at // except in compliance with the License. You may obtain a copy of the License
// at
// //
// http://opensource.org/licenses/MIT // http://opensource.org/licenses/MIT
// //
// Unless required by applicable law or agreed to in writing, software distributed // Unless required by applicable law or agreed to in writing, software
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// CONDITIONS OF ANY KIND, either express or implied. See the License for the // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// specific language governing permissions and limitations under the License. // License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_POW10_ #ifndef RAPIDJSON_POW10_
#define RAPIDJSON_POW10_ #define RAPIDJSON_POW10_
@@ -26,25 +30,43 @@ namespace internal {
\return 10.0^n \return 10.0^n
*/ */
inline double Pow10(int n) { inline double Pow10(int n) {
static const double e[] = { // 1e-0...1e308: 309 * 8 bytes = 2472 bytes static const double e[] = {
1e+0, // 1e-0...1e308: 309 * 8 bytes = 2472 bytes
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+0, 1e+1, 1e+2, 1e+3, 1e+4, 1e+5, 1e+6, 1e+7, 1e+8,
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+9, 1e+10, 1e+11, 1e+12, 1e+13, 1e+14, 1e+15, 1e+16, 1e+17,
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+18, 1e+19, 1e+20, 1e+21, 1e+22, 1e+23, 1e+24, 1e+25, 1e+26,
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+27, 1e+28, 1e+29, 1e+30, 1e+31, 1e+32, 1e+33, 1e+34, 1e+35,
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+36, 1e+37, 1e+38, 1e+39, 1e+40, 1e+41, 1e+42, 1e+43, 1e+44,
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+45, 1e+46, 1e+47, 1e+48, 1e+49, 1e+50, 1e+51, 1e+52, 1e+53,
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+54, 1e+55, 1e+56, 1e+57, 1e+58, 1e+59, 1e+60, 1e+61, 1e+62,
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+63, 1e+64, 1e+65, 1e+66, 1e+67, 1e+68, 1e+69, 1e+70, 1e+71,
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+72, 1e+73, 1e+74, 1e+75, 1e+76, 1e+77, 1e+78, 1e+79, 1e+80,
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+81, 1e+82, 1e+83, 1e+84, 1e+85, 1e+86, 1e+87, 1e+88, 1e+89,
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+90, 1e+91, 1e+92, 1e+93, 1e+94, 1e+95, 1e+96, 1e+97, 1e+98,
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+99, 1e+100, 1e+101, 1e+102, 1e+103, 1e+104, 1e+105, 1e+106, 1e+107,
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+108, 1e+109, 1e+110, 1e+111, 1e+112, 1e+113, 1e+114, 1e+115, 1e+116,
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+117, 1e+118, 1e+119, 1e+120, 1e+121, 1e+122, 1e+123, 1e+124, 1e+125,
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+126, 1e+127, 1e+128, 1e+129, 1e+130, 1e+131, 1e+132, 1e+133, 1e+134,
1e+301,1e+302,1e+303,1e+304,1e+305,1e+306,1e+307,1e+308 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); RAPIDJSON_ASSERT(n >= 0 && n <= 308);
return e[n]; return e[n];
} }

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

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

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available. // Tencent is pleased to support the open source community by making RapidJSON
// available.
// //
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved. // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
// //
// Licensed under the MIT License (the "License"); you may not use this file except // Licensed under the MIT License (the "License"); you may not use this file
// in compliance with the License. You may obtain a copy of the License at // except in compliance with the License. You may obtain a copy of the License
// at
// //
// http://opensource.org/licenses/MIT // http://opensource.org/licenses/MIT
// //
// Unless required by applicable law or agreed to in writing, software distributed // Unless required by applicable law or agreed to in writing, software
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// CONDITIONS OF ANY KIND, either express or implied. See the License for the // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// specific language governing permissions and limitations under the License. // License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_INTERNAL_STACK_H_ #ifndef RAPIDJSON_INTERNAL_STACK_H_
#define RAPIDJSON_INTERNAL_STACK_H_ #define RAPIDJSON_INTERNAL_STACK_H_
@@ -21,7 +25,7 @@
#if defined(__clang__) #if defined(__clang__)
RAPIDJSON_DIAG_PUSH RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(c++98-compat) RAPIDJSON_DIAG_OFF(c++ 98 - compat)
#endif #endif
RAPIDJSON_NAMESPACE_BEGIN RAPIDJSON_NAMESPACE_BEGIN
@@ -32,24 +36,20 @@ namespace internal {
//! A type-unsafe stack for storing different types of data. //! A type-unsafe stack for storing different types of data.
/*! \tparam Allocator Allocator for allocating stack memory. /*! \tparam Allocator Allocator for allocating stack memory.
*/ */
template <typename Allocator> template <typename Allocator> class Stack {
class Stack {
public: public:
// Optimization note: Do not allocate memory for stack_ in constructor. // Optimization note: Do not allocate memory for stack_ in constructor.
// Do it lazily when first Push() -> Expand() -> Resize(). // Do it lazily when first Push() -> Expand() -> Resize().
Stack(Allocator* allocator, size_t stackCapacity) : allocator_(allocator), ownAllocator_(0), stack_(0), stackTop_(0), stackEnd_(0), initialCapacity_(stackCapacity) { Stack(Allocator *allocator, size_t stackCapacity)
} : allocator_(allocator), ownAllocator_(0), stack_(0), stackTop_(0),
stackEnd_(0), initialCapacity_(stackCapacity) {}
#if RAPIDJSON_HAS_CXX11_RVALUE_REFS #if RAPIDJSON_HAS_CXX11_RVALUE_REFS
Stack(Stack&& rhs) Stack(Stack &&rhs)
: allocator_(rhs.allocator_), : allocator_(rhs.allocator_), ownAllocator_(rhs.ownAllocator_),
ownAllocator_(rhs.ownAllocator_), stack_(rhs.stack_), stackTop_(rhs.stackTop_), stackEnd_(rhs.stackEnd_),
stack_(rhs.stack_), initialCapacity_(rhs.initialCapacity_) {
stackTop_(rhs.stackTop_),
stackEnd_(rhs.stackEnd_),
initialCapacity_(rhs.initialCapacity_)
{
rhs.allocator_ = 0; rhs.allocator_ = 0;
rhs.ownAllocator_ = 0; rhs.ownAllocator_ = 0;
rhs.stack_ = 0; rhs.stack_ = 0;
@@ -59,14 +59,11 @@ public:
} }
#endif #endif
~Stack() { ~Stack() { Destroy(); }
Destroy();
}
#if RAPIDJSON_HAS_CXX11_RVALUE_REFS #if RAPIDJSON_HAS_CXX11_RVALUE_REFS
Stack& operator=(Stack&& rhs) { Stack &operator=(Stack &&rhs) {
if (&rhs != this) if (&rhs != this) {
{
Destroy(); Destroy();
allocator_ = rhs.allocator_; allocator_ = rhs.allocator_;
@@ -87,7 +84,7 @@ public:
} }
#endif #endif
void Swap(Stack& rhs) RAPIDJSON_NOEXCEPT { void Swap(Stack &rhs) RAPIDJSON_NOEXCEPT {
internal::Swap(allocator_, rhs.allocator_); internal::Swap(allocator_, rhs.allocator_);
internal::Swap(ownAllocator_, rhs.ownAllocator_); internal::Swap(ownAllocator_, rhs.ownAllocator_);
internal::Swap(stack_, rhs.stack_); internal::Swap(stack_, rhs.stack_);
@@ -105,71 +102,65 @@ public:
stack_ = 0; stack_ = 0;
stackTop_ = 0; stackTop_ = 0;
stackEnd_ = 0; stackEnd_ = 0;
} } else
else
Resize(GetSize()); Resize(GetSize());
} }
// Optimization note: try to minimize the size of this function for force inline. // Optimization note: try to minimize the size of this function for force
// Expansion is run very infrequently, so it is moved to another (probably non-inline) function. // inline. Expansion is run very infrequently, so it is moved to another
template<typename T> // (probably non-inline) function.
RAPIDJSON_FORCEINLINE void Reserve(size_t count = 1) { template <typename T> RAPIDJSON_FORCEINLINE void Reserve(size_t count = 1) {
// Expand the stack if needed // Expand the stack if needed
if (RAPIDJSON_UNLIKELY(static_cast<std::ptrdiff_t>(sizeof(T) * count) > (stackEnd_ - stackTop_))) if (RAPIDJSON_UNLIKELY(static_cast<std::ptrdiff_t>(sizeof(T) * count) >
(stackEnd_ - stackTop_)))
Expand<T>(count); Expand<T>(count);
} }
template<typename T> template <typename T> RAPIDJSON_FORCEINLINE T *Push(size_t count = 1) {
RAPIDJSON_FORCEINLINE T* Push(size_t count = 1) {
Reserve<T>(count); Reserve<T>(count);
return PushUnsafe<T>(count); return PushUnsafe<T>(count);
} }
template<typename T> template <typename T> RAPIDJSON_FORCEINLINE T *PushUnsafe(size_t count = 1) {
RAPIDJSON_FORCEINLINE T* PushUnsafe(size_t count = 1) {
RAPIDJSON_ASSERT(stackTop_); RAPIDJSON_ASSERT(stackTop_);
RAPIDJSON_ASSERT(static_cast<std::ptrdiff_t>(sizeof(T) * count) <= (stackEnd_ - stackTop_)); RAPIDJSON_ASSERT(static_cast<std::ptrdiff_t>(sizeof(T) * count) <=
T* ret = reinterpret_cast<T*>(stackTop_); (stackEnd_ - stackTop_));
T *ret = reinterpret_cast<T *>(stackTop_);
stackTop_ += sizeof(T) * count; stackTop_ += sizeof(T) * count;
return ret; return ret;
} }
template<typename T> template <typename T> T *Pop(size_t count) {
T* Pop(size_t count) {
RAPIDJSON_ASSERT(GetSize() >= count * sizeof(T)); RAPIDJSON_ASSERT(GetSize() >= count * sizeof(T));
stackTop_ -= count * sizeof(T); stackTop_ -= count * sizeof(T);
return reinterpret_cast<T*>(stackTop_); return reinterpret_cast<T *>(stackTop_);
} }
template<typename T> template <typename T> T *Top() {
T* Top() {
RAPIDJSON_ASSERT(GetSize() >= sizeof(T)); RAPIDJSON_ASSERT(GetSize() >= sizeof(T));
return reinterpret_cast<T*>(stackTop_ - sizeof(T)); return reinterpret_cast<T *>(stackTop_ - sizeof(T));
} }
template<typename T> template <typename T> const T *Top() const {
const T* Top() const {
RAPIDJSON_ASSERT(GetSize() >= sizeof(T)); RAPIDJSON_ASSERT(GetSize() >= sizeof(T));
return reinterpret_cast<T*>(stackTop_ - sizeof(T)); return reinterpret_cast<T *>(stackTop_ - sizeof(T));
} }
template<typename T> template <typename T> T *End() { return reinterpret_cast<T *>(stackTop_); }
T* End() { return reinterpret_cast<T*>(stackTop_); }
template<typename T> template <typename T> const T *End() const {
const T* End() const { return reinterpret_cast<T*>(stackTop_); } return reinterpret_cast<T *>(stackTop_);
template<typename T>
T* Bottom() { return reinterpret_cast<T*>(stack_); }
template<typename T>
const T* Bottom() const { return reinterpret_cast<T*>(stack_); }
bool HasAllocator() const {
return allocator_ != 0;
} }
Allocator& GetAllocator() { template <typename T> T *Bottom() { return reinterpret_cast<T *>(stack_); }
template <typename T> const T *Bottom() const {
return reinterpret_cast<T *>(stack_);
}
bool HasAllocator() const { return allocator_ != 0; }
Allocator &GetAllocator() {
RAPIDJSON_ASSERT(allocator_); RAPIDJSON_ASSERT(allocator_);
return *allocator_; return *allocator_;
} }
@@ -179,9 +170,9 @@ public:
size_t GetCapacity() const { return static_cast<size_t>(stackEnd_ - stack_); } size_t GetCapacity() const { return static_cast<size_t>(stackEnd_ - stack_); }
private: private:
template<typename T> template <typename T> void Expand(size_t count) {
void Expand(size_t count) { // Only expand the capacity if the current stack exists. Otherwise just
// Only expand the capacity if the current stack exists. Otherwise just create a stack with initial capacity. // create a stack with initial capacity.
size_t newCapacity; size_t newCapacity;
if (stack_ == 0) { if (stack_ == 0) {
if (!allocator_) if (!allocator_)
@@ -200,7 +191,8 @@ private:
void Resize(size_t newCapacity) { void Resize(size_t newCapacity) {
const size_t size = GetSize(); // Backup the current size const size_t size = GetSize(); // Backup the current size
stack_ = static_cast<char*>(allocator_->Realloc(stack_, GetCapacity(), newCapacity)); stack_ = static_cast<char *>(
allocator_->Realloc(stack_, GetCapacity(), newCapacity));
stackTop_ = stack_ + size; stackTop_ = stack_ + size;
stackEnd_ = stack_ + newCapacity; stackEnd_ = stack_ + newCapacity;
} }
@@ -211,11 +203,11 @@ private:
} }
// Prohibit copy constructor & assignment operator. // Prohibit copy constructor & assignment operator.
Stack(const Stack&); Stack(const Stack &);
Stack& operator=(const Stack&); Stack &operator=(const Stack &);
Allocator* allocator_; Allocator *allocator_;
Allocator* ownAllocator_; Allocator *ownAllocator_;
char *stack_; char *stack_;
char *stackTop_; char *stackTop_;
char *stackEnd_; char *stackEnd_;

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

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

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

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

29
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 // Licensed under the MIT License (the "License"); you may not use this file
// in compliance with the License. You may obtain a copy of the License at // except in compliance with the License. You may obtain a copy of the License
// at
// //
// http://opensource.org/licenses/MIT // http://opensource.org/licenses/MIT
// //
// Unless required by applicable law or agreed to in writing, software distributed // Unless required by applicable law or agreed to in writing, software
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// CONDITIONS OF ANY KIND, either express or implied. See the License for the // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// specific language governing permissions and limitations under the License. // License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_INTERNAL_SWAP_H_ #ifndef RAPIDJSON_INTERNAL_SWAP_H_
#define RAPIDJSON_INTERNAL_SWAP_H_ #define RAPIDJSON_INTERNAL_SWAP_H_
@@ -19,18 +23,17 @@
#if defined(__clang__) #if defined(__clang__)
RAPIDJSON_DIAG_PUSH RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(c++98-compat) RAPIDJSON_DIAG_OFF(c++ 98 - compat)
#endif #endif
RAPIDJSON_NAMESPACE_BEGIN RAPIDJSON_NAMESPACE_BEGIN
namespace internal { namespace internal {
//! Custom swap() to avoid dependency on C++ <algorithm> header //! 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. /*! \tparam T Type of the arguments to swap, should be instantiated with
\note This has the same semantics as std::swap(). primitive C++ types only. \note This has the same semantics as std::swap().
*/ */
template <typename T> template <typename T> inline void Swap(T &a, T &b) RAPIDJSON_NOEXCEPT {
inline void Swap(T& a, T& b) RAPIDJSON_NOEXCEPT {
T tmp = a; T tmp = a;
a = b; a = b;
b = tmp; b = tmp;

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

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

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

@@ -1,55 +1,61 @@
// 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 // Licensed under the MIT License (the "License"); you may not use this file
// in compliance with the License. You may obtain a copy of the License at // except in compliance with the License. You may obtain a copy of the License
// at
// //
// http://opensource.org/licenses/MIT // http://opensource.org/licenses/MIT
// //
// Unless required by applicable law or agreed to in writing, software distributed // Unless required by applicable law or agreed to in writing, software
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// CONDITIONS OF ANY KIND, either express or implied. See the License for the // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// specific language governing permissions and limitations under the License. // License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_MEMORYBUFFER_H_ #ifndef RAPIDJSON_MEMORYBUFFER_H_
#define RAPIDJSON_MEMORYBUFFER_H_ #define RAPIDJSON_MEMORYBUFFER_H_
#include "stream.h"
#include "internal/stack.h" #include "internal/stack.h"
#include "stream.h"
RAPIDJSON_NAMESPACE_BEGIN RAPIDJSON_NAMESPACE_BEGIN
//! Represents an in-memory output byte stream. //! 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: Differences between MemoryBuffer and StringBuffer:
1. StringBuffer has Encoding but MemoryBuffer is only a byte buffer. 1. StringBuffer has Encoding but MemoryBuffer is only a byte buffer.
2. StringBuffer::GetString() returns a null-terminated string. MemoryBuffer::GetBuffer() returns a buffer without terminator. 2. StringBuffer::GetString() returns a null-terminated string.
MemoryBuffer::GetBuffer() returns a buffer without terminator.
\tparam Allocator type for allocating memory buffer. \tparam Allocator type for allocating memory buffer.
\note implements Stream concept \note implements Stream concept
*/ */
template <typename Allocator = CrtAllocator> template <typename Allocator = CrtAllocator> struct GenericMemoryBuffer {
struct GenericMemoryBuffer {
typedef char Ch; // byte 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 Put(Ch c) { *stack_.template Push<Ch>() = c; }
void Flush() {} void Flush() {}
void Clear() { stack_.Clear(); } void Clear() { stack_.Clear(); }
void ShrinkToFit() { stack_.ShrinkToFit(); } void ShrinkToFit() { stack_.ShrinkToFit(); }
Ch* Push(size_t count) { return stack_.template Push<Ch>(count); } Ch *Push(size_t count) { return stack_.template Push<Ch>(count); }
void Pop(size_t count) { stack_.template Pop<Ch>(count); } void Pop(size_t count) { stack_.template Pop<Ch>(count); }
const Ch* GetBuffer() const { const Ch *GetBuffer() const { return stack_.template Bottom<Ch>(); }
return stack_.template Bottom<Ch>();
}
size_t GetSize() const { return stack_.GetSize(); } size_t GetSize() const { return stack_.GetSize(); }
@@ -59,9 +65,9 @@ struct GenericMemoryBuffer {
typedef GenericMemoryBuffer<> MemoryBuffer; typedef GenericMemoryBuffer<> MemoryBuffer;
//! Implement specialized version of PutN() with memset() for better performance. //! Implement specialized version of PutN() with memset() for better
template<> //! performance.
inline void PutN(MemoryBuffer& memoryBuffer, char c, size_t n) { template <> inline void PutN(MemoryBuffer &memoryBuffer, char c, size_t n) {
std::memset(memoryBuffer.stack_.Push<char>(n), c, n * sizeof(c)); std::memset(memoryBuffer.stack_.Push<char>(n), c, n * sizeof(c));
} }

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

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

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

@@ -61,7 +61,8 @@ typedef struct {
// 7.8.1 Macros for format specifiers // 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: // The fprintf macros for signed integers are:
#define PRId8 "d" #define PRId8 "d"
@@ -194,11 +195,11 @@ typedef struct {
#define SCNiMAX "I64i" #define SCNiMAX "I64i"
#ifdef _WIN64 // [ #ifdef _WIN64 // [
# define SCNdPTR "I64d" #define SCNdPTR "I64d"
# define SCNiPTR "I64i" #define SCNiPTR "I64i"
#else // _WIN64 ][ #else // _WIN64 ][
# define SCNdPTR "ld" #define SCNdPTR "ld"
# define SCNiPTR "li" #define SCNiPTR "li"
#endif // _WIN64 ] #endif // _WIN64 ]
// The fscanf macros for unsigned integers are: // The fscanf macros for unsigned integers are:
@@ -260,15 +261,15 @@ typedef struct {
#define SCNXMAX "I64X" #define SCNXMAX "I64X"
#ifdef _WIN64 // [ #ifdef _WIN64 // [
# define SCNoPTR "I64o" #define SCNoPTR "I64o"
# define SCNuPTR "I64u" #define SCNuPTR "I64u"
# define SCNxPTR "I64x" #define SCNxPTR "I64x"
# define SCNXPTR "I64X" #define SCNXPTR "I64X"
#else // _WIN64 ][ #else // _WIN64 ][
# define SCNoPTR "lo" #define SCNoPTR "lo"
# define SCNuPTR "lu" #define SCNuPTR "lu"
# define SCNxPTR "lx" #define SCNxPTR "lx"
# define SCNXPTR "lX" #define SCNXPTR "lX"
#endif // _WIN64 ] #endif // _WIN64 ]
#endif // __STDC_FORMAT_MACROS ] #endif // __STDC_FORMAT_MACROS ]
@@ -287,8 +288,7 @@ static
#else // STATIC_IMAXDIV ][ #else // STATIC_IMAXDIV ][
_inline _inline
#endif // STATIC_IMAXDIV ] #endif // STATIC_IMAXDIV ]
imaxdiv_t __cdecl imaxdiv(intmax_t numer, intmax_t denom) imaxdiv_t __cdecl imaxdiv(intmax_t numer, intmax_t denom) {
{
imaxdiv_t result; imaxdiv_t result;
result.quot = numer / denom; result.quot = numer / denom;

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

@@ -45,11 +45,13 @@
#pragma once #pragma once
#endif #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 // [ #if _MSC_VER >= 1600 // [
#include <stdint.h> #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 INT8_C
#undef INT16_C #undef INT16_C
@@ -76,10 +78,10 @@
// These #ifndef's are needed to prevent collisions with <boost/cstdint.hpp>. // These #ifndef's are needed to prevent collisions with <boost/cstdint.hpp>.
// Check out Issue 9 for the details. // Check out Issue 9 for the details.
#ifndef INTMAX_C // [ #ifndef INTMAX_C // [
# define INTMAX_C INT64_C #define INTMAX_C INT64_C
#endif // INTMAX_C ] #endif // INTMAX_C ]
#ifndef UINTMAX_C // [ #ifndef UINTMAX_C // [
# define UINTMAX_C UINT64_C #define UINTMAX_C UINT64_C
#endif // UINTMAX_C ] #endif // UINTMAX_C ]
#endif // __STDC_CONSTANT_MACROS ] #endif // __STDC_CONSTANT_MACROS ]
@@ -95,20 +97,19 @@
#if defined(__cplusplus) && !defined(_M_ARM) #if defined(__cplusplus) && !defined(_M_ARM)
extern "C" { extern "C" {
#endif #endif
# include <wchar.h> #include <wchar.h>
#if defined(__cplusplus) && !defined(_M_ARM) #if defined(__cplusplus) && !defined(_M_ARM)
} }
#endif #endif
// Define _W64 macros to mark types changing their size, like intptr_t. // Define _W64 macros to mark types changing their size, like intptr_t.
#ifndef _W64 #ifndef _W64
# if !defined(__midl) && (defined(_X86_) || defined(_M_IX86)) && _MSC_VER >= 1300 #if !defined(__midl) && (defined(_X86_) || defined(_M_IX86)) && _MSC_VER >= 1300
# define _W64 __w64 #define _W64 __w64
# else #else
# define _W64 #define _W64
# endif #endif
#endif #endif
// 7.18.1 Integer types // 7.18.1 Integer types
@@ -118,24 +119,23 @@ extern "C" {
// realize that, e.g. char has the same size as __int8 // realize that, e.g. char has the same size as __int8
// so we give up on __intX for them. // so we give up on __intX for them.
#if (_MSC_VER < 1300) #if (_MSC_VER < 1300)
typedef signed char int8_t; typedef signed char int8_t;
typedef signed short int16_t; typedef signed short int16_t;
typedef signed int int32_t; typedef signed int int32_t;
typedef unsigned char uint8_t; typedef unsigned char uint8_t;
typedef unsigned short uint16_t; typedef unsigned short uint16_t;
typedef unsigned int uint32_t; typedef unsigned int uint32_t;
#else #else
typedef signed __int8 int8_t; typedef signed __int8 int8_t;
typedef signed __int16 int16_t; typedef signed __int16 int16_t;
typedef signed __int32 int32_t; typedef signed __int32 int32_t;
typedef unsigned __int8 uint8_t; typedef unsigned __int8 uint8_t;
typedef unsigned __int16 uint16_t; typedef unsigned __int16 uint16_t;
typedef unsigned __int32 uint32_t; typedef unsigned __int32 uint32_t;
#endif #endif
typedef signed __int64 int64_t; typedef signed __int64 int64_t;
typedef unsigned __int64 uint64_t; typedef unsigned __int64 uint64_t;
// 7.18.1.2 Minimum-width integer types // 7.18.1.2 Minimum-width integer types
typedef int8_t int_least8_t; typedef int8_t int_least8_t;
typedef int16_t int_least16_t; typedef int16_t int_least16_t;
@@ -158,21 +158,22 @@ typedef uint64_t uint_fast64_t;
// 7.18.1.4 Integer types capable of holding object pointers // 7.18.1.4 Integer types capable of holding object pointers
#ifdef _WIN64 // [ #ifdef _WIN64 // [
typedef signed __int64 intptr_t; typedef signed __int64 intptr_t;
typedef unsigned __int64 uintptr_t; typedef unsigned __int64 uintptr_t;
#else // _WIN64 ][ #else // _WIN64 ][
typedef _W64 signed int intptr_t; typedef _W64 signed int intptr_t;
typedef _W64 unsigned int uintptr_t; typedef _W64 unsigned int uintptr_t;
#endif // _WIN64 ] #endif // _WIN64 ]
// 7.18.1.5 Greatest-width integer types // 7.18.1.5 Greatest-width integer types
typedef int64_t intmax_t; typedef int64_t intmax_t;
typedef uint64_t uintmax_t; typedef uint64_t uintmax_t;
// 7.18.2 Limits of specified-width integer types // 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 // 7.18.2.1 Limits of exact-width integer types
#define INT8_MIN ((int8_t)_I8_MIN) #define INT8_MIN ((int8_t)_I8_MIN)
@@ -218,13 +219,13 @@ typedef uint64_t uintmax_t;
// 7.18.2.4 Limits of integer types capable of holding object pointers // 7.18.2.4 Limits of integer types capable of holding object pointers
#ifdef _WIN64 // [ #ifdef _WIN64 // [
# define INTPTR_MIN INT64_MIN #define INTPTR_MIN INT64_MIN
# define INTPTR_MAX INT64_MAX #define INTPTR_MAX INT64_MAX
# define UINTPTR_MAX UINT64_MAX #define UINTPTR_MAX UINT64_MAX
#else // _WIN64 ][ #else // _WIN64 ][
# define INTPTR_MIN INT32_MIN #define INTPTR_MIN INT32_MIN
# define INTPTR_MAX INT32_MAX #define INTPTR_MAX INT32_MAX
# define UINTPTR_MAX UINT32_MAX #define UINTPTR_MAX UINT32_MAX
#endif // _WIN64 ] #endif // _WIN64 ]
// 7.18.2.5 Limits of greatest-width integer types // 7.18.2.5 Limits of greatest-width integer types
@@ -235,30 +236,30 @@ typedef uint64_t uintmax_t;
// 7.18.3 Limits of other integer types // 7.18.3 Limits of other integer types
#ifdef _WIN64 // [ #ifdef _WIN64 // [
# define PTRDIFF_MIN _I64_MIN #define PTRDIFF_MIN _I64_MIN
# define PTRDIFF_MAX _I64_MAX #define PTRDIFF_MAX _I64_MAX
#else // _WIN64 ][ #else // _WIN64 ][
# define PTRDIFF_MIN _I32_MIN #define PTRDIFF_MIN _I32_MIN
# define PTRDIFF_MAX _I32_MAX #define PTRDIFF_MAX _I32_MAX
#endif // _WIN64 ] #endif // _WIN64 ]
#define SIG_ATOMIC_MIN INT_MIN #define SIG_ATOMIC_MIN INT_MIN
#define SIG_ATOMIC_MAX INT_MAX #define SIG_ATOMIC_MAX INT_MAX
#ifndef SIZE_MAX // [ #ifndef SIZE_MAX // [
# ifdef _WIN64 // [ #ifdef _WIN64 // [
# define SIZE_MAX _UI64_MAX #define SIZE_MAX _UI64_MAX
# else // _WIN64 ][ #else // _WIN64 ][
# define SIZE_MAX _UI32_MAX #define SIZE_MAX _UI32_MAX
# endif // _WIN64 ] #endif // _WIN64 ]
#endif // SIZE_MAX ] #endif // SIZE_MAX ]
// WCHAR_MIN and WCHAR_MAX are also defined in <wchar.h> // WCHAR_MIN and WCHAR_MAX are also defined in <wchar.h>
#ifndef WCHAR_MIN // [ #ifndef WCHAR_MIN // [
# define WCHAR_MIN 0 #define WCHAR_MIN 0
#endif // WCHAR_MIN ] #endif // WCHAR_MIN ]
#ifndef WCHAR_MAX // [ #ifndef WCHAR_MAX // [
# define WCHAR_MAX _UI16_MAX #define WCHAR_MAX _UI16_MAX
#endif // WCHAR_MAX ] #endif // WCHAR_MAX ]
#define WINT_MIN 0 #define WINT_MIN 0
@@ -266,10 +267,10 @@ typedef uint64_t uintmax_t;
#endif // __STDC_LIMIT_MACROS ] #endif // __STDC_LIMIT_MACROS ]
// 7.18.4 Limits of other integer types // 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 // 7.18.4.1 Macros for minimum-width integer constants
@@ -287,10 +288,10 @@ typedef uint64_t uintmax_t;
// These #ifndef's are needed to prevent collisions with <boost/cstdint.hpp>. // These #ifndef's are needed to prevent collisions with <boost/cstdint.hpp>.
// Check out Issue 9 for the details. // Check out Issue 9 for the details.
#ifndef INTMAX_C // [ #ifndef INTMAX_C // [
# define INTMAX_C INT64_C #define INTMAX_C INT64_C
#endif // INTMAX_C ] #endif // INTMAX_C ]
#ifndef UINTMAX_C // [ #ifndef UINTMAX_C // [
# define UINTMAX_C UINT64_C #define UINTMAX_C UINT64_C
#endif // UINTMAX_C ] #endif // UINTMAX_C ]
#endif // __STDC_CONSTANT_MACROS ] #endif // __STDC_CONSTANT_MACROS ]

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

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

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

File diff suppressed because it is too large Load Diff

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

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

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available. // Tencent is pleased to support the open source community by making RapidJSON
// available.
// //
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved. // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
// //
// Licensed under the MIT License (the "License"); you may not use this file except // Licensed under the MIT License (the "License"); you may not use this file
// in compliance with the License. You may obtain a copy of the License at // except in compliance with the License. You may obtain a copy of the License
// at
// //
// http://opensource.org/licenses/MIT // http://opensource.org/licenses/MIT
// //
// Unless required by applicable law or agreed to in writing, software distributed // Unless required by applicable law or agreed to in writing, software
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// CONDITIONS OF ANY KIND, either express or implied. See the License for the // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// specific language governing permissions and limitations under the License. // License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_RAPIDJSON_H_ #ifndef RAPIDJSON_RAPIDJSON_H_
#define RAPIDJSON_RAPIDJSON_H_ #define RAPIDJSON_RAPIDJSON_H_
@@ -29,7 +33,8 @@
features can be configured in terms of overridden or predefined features can be configured in terms of overridden or predefined
preprocessor macros at compile-time. 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 \note These macros should be given on the compiler command-line
(where applicable) to avoid inconsistent values when compiling (where applicable) to avoid inconsistent values when compiling
@@ -42,7 +47,8 @@
/////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_VERSION_STRING // 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 //!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN
@@ -76,7 +82,8 @@
#define RAPIDJSON_MINOR_VERSION 1 #define RAPIDJSON_MINOR_VERSION 1
#define RAPIDJSON_PATCH_VERSION 0 #define RAPIDJSON_PATCH_VERSION 0
#define RAPIDJSON_VERSION_STRING \ #define RAPIDJSON_VERSION_STRING \
RAPIDJSON_STRINGIFY(RAPIDJSON_MAJOR_VERSION.RAPIDJSON_MINOR_VERSION.RAPIDJSON_PATCH_VERSION) RAPIDJSON_STRINGIFY( \
RAPIDJSON_MAJOR_VERSION.RAPIDJSON_MINOR_VERSION.RAPIDJSON_PATCH_VERSION)
/////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_NAMESPACE_(BEGIN|END) // RAPIDJSON_NAMESPACE_(BEGIN|END)
@@ -137,9 +144,9 @@
\ingroup RAPIDJSON_CONFIG \ingroup RAPIDJSON_CONFIG
\brief Enable RapidJSON support for \c std::string \brief Enable RapidJSON support for \c std::string
By defining this preprocessor symbol to \c 1, several convenience functions for using By defining this preprocessor symbol to \c 1, several convenience functions
\ref rapidjson::GenericValue with \c std::string are enabled, especially for using \ref rapidjson::GenericValue with \c std::string are enabled,
for construction and comparison. especially for construction and comparison.
\hideinitializer \hideinitializer
*/ */
@@ -156,8 +163,8 @@
\ingroup RAPIDJSON_CONFIG \ingroup RAPIDJSON_CONFIG
\brief Use external 64-bit integer types. \brief Use external 64-bit integer types.
RapidJSON requires the 64-bit integer types \c int64_t and \c uint64_t types RapidJSON requires the 64-bit integer types \c int64_t and \c uint64_t
to be available at global scope. types to be available at global scope.
If users have their own definition, define RAPIDJSON_NO_INT64DEFINE to If users have their own definition, define RAPIDJSON_NO_INT64DEFINE to
prevent RapidJSON from defining its own types. prevent RapidJSON from defining its own types.
@@ -165,12 +172,12 @@
#ifndef RAPIDJSON_NO_INT64DEFINE #ifndef RAPIDJSON_NO_INT64DEFINE
//!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN //!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN
#if defined(_MSC_VER) && (_MSC_VER < 1800) // Visual Studio 2013 #if defined(_MSC_VER) && (_MSC_VER < 1800) // Visual Studio 2013
#include "msinttypes/stdint.h"
#include "msinttypes/inttypes.h" #include "msinttypes/inttypes.h"
#include "msinttypes/stdint.h"
#else #else
// Other compilers should have this. // Other compilers should have this.
#include <stdint.h>
#include <inttypes.h> #include <inttypes.h>
#include <stdint.h>
#endif #endif
//!@endcond //!@endcond
#ifdef RAPIDJSON_DOXYGEN_RUNNING #ifdef RAPIDJSON_DOXYGEN_RUNNING
@@ -203,51 +210,59 @@
\def RAPIDJSON_ENDIAN \def RAPIDJSON_ENDIAN
\ingroup RAPIDJSON_CONFIG \ingroup RAPIDJSON_CONFIG
GCC 4.6 provided macro for detecting endianness of the target machine. But other GCC 4.6 provided macro for detecting endianness of the target machine. But
compilers may not have this. User can define RAPIDJSON_ENDIAN to either other compilers may not have this. User can define RAPIDJSON_ENDIAN to either
\ref RAPIDJSON_LITTLEENDIAN or \ref RAPIDJSON_BIGENDIAN. \ref RAPIDJSON_LITTLEENDIAN or \ref RAPIDJSON_BIGENDIAN.
Default detection implemented with reference to 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 \li http://www.boost.org/doc/libs/1_42_0/boost/detail/endian.hpp
*/ */
#ifndef RAPIDJSON_ENDIAN #ifndef RAPIDJSON_ENDIAN
// Detect with GCC 4.6's macro // Detect with GCC 4.6's macro
# ifdef __BYTE_ORDER__ #ifdef __BYTE_ORDER__
# if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__ #if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__
# define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN #define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN
# elif __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__ #elif __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__
# define RAPIDJSON_ENDIAN RAPIDJSON_BIGENDIAN #define RAPIDJSON_ENDIAN RAPIDJSON_BIGENDIAN
# else #else
# error Unknown machine endianness detected. User needs to define RAPIDJSON_ENDIAN. # error Unknown machine endianness detected. User needs to define RAPIDJSON_ENDIAN.
# endif // __BYTE_ORDER__ #endif // __BYTE_ORDER__
// Detect with GLIBC's endian.h // Detect with GLIBC's endian.h
# elif defined(__GLIBC__) #elif defined(__GLIBC__)
# include <endian.h> #include <endian.h>
# if (__BYTE_ORDER == __LITTLE_ENDIAN) #if (__BYTE_ORDER == __LITTLE_ENDIAN)
# define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN #define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN
# elif (__BYTE_ORDER == __BIG_ENDIAN) #elif (__BYTE_ORDER == __BIG_ENDIAN)
# define RAPIDJSON_ENDIAN RAPIDJSON_BIGENDIAN #define RAPIDJSON_ENDIAN RAPIDJSON_BIGENDIAN
# else #else
# error Unknown machine endianness detected. User needs to define RAPIDJSON_ENDIAN. # error Unknown machine endianness detected. User needs to define RAPIDJSON_ENDIAN.
# endif // __GLIBC__ #endif // __GLIBC__
// Detect with _LITTLE_ENDIAN and _BIG_ENDIAN macro // Detect with _LITTLE_ENDIAN and _BIG_ENDIAN macro
# elif defined(_LITTLE_ENDIAN) && !defined(_BIG_ENDIAN) #elif defined(_LITTLE_ENDIAN) && !defined(_BIG_ENDIAN)
# define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN #define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN
# elif defined(_BIG_ENDIAN) && !defined(_LITTLE_ENDIAN) #elif defined(_BIG_ENDIAN) && !defined(_LITTLE_ENDIAN)
# define RAPIDJSON_ENDIAN RAPIDJSON_BIGENDIAN #define RAPIDJSON_ENDIAN RAPIDJSON_BIGENDIAN
// Detect with architecture macros // Detect with architecture macros
# elif defined(__sparc) || defined(__sparc__) || defined(_POWER) || defined(__powerpc__) || defined(__ppc__) || defined(__hpux) || defined(__hppa) || defined(_MIPSEB) || defined(_POWER) || defined(__s390__) #elif defined(__sparc) || defined(__sparc__) || defined(_POWER) || \
# define RAPIDJSON_ENDIAN RAPIDJSON_BIGENDIAN defined(__powerpc__) || defined(__ppc__) || defined(__hpux) || \
# 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__) defined(__hppa) || defined(_MIPSEB) || defined(_POWER) || \
# define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN defined(__s390__)
# elif defined(_MSC_VER) && (defined(_M_ARM) || defined(_M_ARM64)) #define RAPIDJSON_ENDIAN RAPIDJSON_BIGENDIAN
# define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN #elif defined(__i386__) || defined(__alpha__) || defined(__ia64) || \
# elif defined(RAPIDJSON_DOXYGEN_RUNNING) defined(__ia64__) || defined(_M_IX86) || defined(_M_IA64) || \
# define RAPIDJSON_ENDIAN defined(_M_ALPHA) || defined(__amd64) || defined(__amd64__) || \
# else 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. # error Unknown machine endianness detected. User needs to define RAPIDJSON_ENDIAN.
# endif #endif
#endif // RAPIDJSON_ENDIAN #endif // RAPIDJSON_ENDIAN
/////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////////
@@ -255,7 +270,8 @@
//! Whether using 64-bit architecture //! Whether using 64-bit architecture
#ifndef RAPIDJSON_64BIT #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 #define RAPIDJSON_64BIT 1
#else #else
#define RAPIDJSON_64BIT 0 #define RAPIDJSON_64BIT 0
@@ -273,7 +289,8 @@
User can customize by defining the RAPIDJSON_ALIGN function macro. User can customize by defining the RAPIDJSON_ALIGN function macro.
*/ */
#ifndef RAPIDJSON_ALIGN #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 #endif
/////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////////
@@ -286,7 +303,8 @@
Use this macro to define 64-bit constants by a pair of 32-bit integer. Use this macro to define 64-bit constants by a pair of 32-bit integer.
*/ */
#ifndef RAPIDJSON_UINT64_C2 #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 #endif
/////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////////
@@ -296,12 +314,14 @@
/*! /*!
\ingroup RAPIDJSON_CONFIG \ingroup RAPIDJSON_CONFIG
This optimization uses the fact that current X86-64 architecture only implement lower 48-bit virtual address. This optimization uses the fact that current X86-64 architecture only
The higher 16-bit can be used for storing other data. implement lower 48-bit virtual address. The higher 16-bit can be used for
\c GenericValue uses this optimization to reduce its size form 24 bytes to 16 bytes in 64-bit architecture. 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 #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 #define RAPIDJSON_48BITPOINTER_OPTIMIZATION 1
#else #else
#define RAPIDJSON_48BITPOINTER_OPTIMIZATION 0 #define RAPIDJSON_48BITPOINTER_OPTIMIZATION 0
@@ -312,8 +332,15 @@
#if RAPIDJSON_64BIT != 1 #if RAPIDJSON_64BIT != 1
#error RAPIDJSON_48BITPOINTER_OPTIMIZATION can only be set to 1 when RAPIDJSON_64BIT=1 #error RAPIDJSON_48BITPOINTER_OPTIMIZATION can only be set to 1 when RAPIDJSON_64BIT=1
#endif #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_SETPOINTER(type, p, x) \
#define RAPIDJSON_GETPOINTER(type, p) (reinterpret_cast<type *>(reinterpret_cast<uintptr_t>(p) & static_cast<uintptr_t>(RAPIDJSON_UINT64_C2(0x0000FFFF, 0xFFFFFFFF)))) (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 #else
#define RAPIDJSON_SETPOINTER(type, p, x) (p = (x)) #define RAPIDJSON_SETPOINTER(type, p, x) (p = (x))
#define RAPIDJSON_GETPOINTER(type, p) (p) #define RAPIDJSON_GETPOINTER(type, p) (p)
@@ -348,8 +375,8 @@
If any of these symbols is defined, RapidJSON defines the macro If any of these symbols is defined, RapidJSON defines the macro
\c RAPIDJSON_SIMD to indicate the availability of the optimized code. \c RAPIDJSON_SIMD to indicate the availability of the optimized code.
*/ */
#if defined(RAPIDJSON_SSE2) || defined(RAPIDJSON_SSE42) \ #if defined(RAPIDJSON_SSE2) || defined(RAPIDJSON_SSE42) || \
|| defined(RAPIDJSON_NEON) || defined(RAPIDJSON_DOXYGEN_RUNNING) defined(RAPIDJSON_NEON) || defined(RAPIDJSON_DOXYGEN_RUNNING)
#define RAPIDJSON_SIMD #define RAPIDJSON_SIMD
#endif #endif
@@ -411,9 +438,8 @@ RAPIDJSON_NAMESPACE_END
// Prefer C++11 static_assert, if available // Prefer C++11 static_assert, if available
#ifndef RAPIDJSON_STATIC_ASSERT #ifndef RAPIDJSON_STATIC_ASSERT
#if __cplusplus >= 201103L || ( defined(_MSC_VER) && _MSC_VER >= 1800 ) #if __cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1800)
#define RAPIDJSON_STATIC_ASSERT(x) \ #define RAPIDJSON_STATIC_ASSERT(x) static_assert(x, RAPIDJSON_STRINGIFY(x))
static_assert(x, RAPIDJSON_STRINGIFY(x))
#endif // C++11 #endif // C++11
#endif // RAPIDJSON_STATIC_ASSERT #endif // RAPIDJSON_STATIC_ASSERT
@@ -424,7 +450,9 @@ RAPIDJSON_NAMESPACE_END
#endif #endif
RAPIDJSON_NAMESPACE_BEGIN RAPIDJSON_NAMESPACE_BEGIN
template <bool x> struct STATIC_ASSERTION_FAILURE; 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 {}; template <size_t x> struct StaticAssertTest {};
RAPIDJSON_NAMESPACE_END RAPIDJSON_NAMESPACE_END
@@ -443,9 +471,10 @@ RAPIDJSON_NAMESPACE_END
\hideinitializer \hideinitializer
*/ */
#define RAPIDJSON_STATIC_ASSERT(x) \ #define RAPIDJSON_STATIC_ASSERT(x) \
typedef ::RAPIDJSON_NAMESPACE::StaticAssertTest< \ typedef ::RAPIDJSON_NAMESPACE::StaticAssertTest<sizeof( \
sizeof(::RAPIDJSON_NAMESPACE::STATIC_ASSERTION_FAILURE<bool(x) >)> \ ::RAPIDJSON_NAMESPACE::STATIC_ASSERTION_FAILURE<bool(x)>)> \
RAPIDJSON_JOIN(StaticAssertTypedef, __LINE__) RAPIDJSON_STATIC_ASSERT_UNUSED_ATTRIBUTE RAPIDJSON_JOIN(StaticAssertTypedef, __LINE__) \
RAPIDJSON_STATIC_ASSERT_UNUSED_ATTRIBUTE
#endif // RAPIDJSON_STATIC_ASSERT #endif // RAPIDJSON_STATIC_ASSERT
/////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////////
@@ -484,11 +513,11 @@ RAPIDJSON_NAMESPACE_END
#define RAPIDJSON_MULTILINEMACRO_BEGIN do { #define RAPIDJSON_MULTILINEMACRO_BEGIN do {
#define RAPIDJSON_MULTILINEMACRO_END \ #define RAPIDJSON_MULTILINEMACRO_END \
} while((void)0, 0) } \
while ((void)0, 0)
// adopted from Boost // adopted from Boost
#define RAPIDJSON_VERSION_CODE(x,y,z) \ #define RAPIDJSON_VERSION_CODE(x, y, z) (((x)*100000) + ((y)*100) + (z))
(((x)*100000) + ((y)*100) + (z))
#if defined(__has_builtin) #if defined(__has_builtin)
#define RAPIDJSON_HAS_BUILTIN(x) __has_builtin(x) #define RAPIDJSON_HAS_BUILTIN(x) __has_builtin(x)
@@ -501,18 +530,20 @@ RAPIDJSON_NAMESPACE_END
#if defined(__GNUC__) #if defined(__GNUC__)
#define RAPIDJSON_GNUC \ #define RAPIDJSON_GNUC \
RAPIDJSON_VERSION_CODE(__GNUC__,__GNUC_MINOR__,__GNUC_PATCHLEVEL__) RAPIDJSON_VERSION_CODE(__GNUC__, __GNUC_MINOR__, __GNUC_PATCHLEVEL__)
#endif #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_PRAGMA(x) _Pragma(RAPIDJSON_STRINGIFY(x))
#define RAPIDJSON_DIAG_PRAGMA(x) RAPIDJSON_PRAGMA(GCC diagnostic x) #define RAPIDJSON_DIAG_PRAGMA(x) RAPIDJSON_PRAGMA(GCC diagnostic x)
#define RAPIDJSON_DIAG_OFF(x) \ #define RAPIDJSON_DIAG_OFF(x) \
RAPIDJSON_DIAG_PRAGMA(ignored RAPIDJSON_STRINGIFY(RAPIDJSON_JOIN(-W,x))) RAPIDJSON_DIAG_PRAGMA(ignored RAPIDJSON_STRINGIFY(RAPIDJSON_JOIN(-W, x)))
// push/pop support in Clang and GCC>=4.6 // 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_PUSH RAPIDJSON_DIAG_PRAGMA(push)
#define RAPIDJSON_DIAG_POP RAPIDJSON_DIAG_PRAGMA(pop) #define RAPIDJSON_DIAG_POP RAPIDJSON_DIAG_PRAGMA(pop)
#else // GCC >= 4.2, < 4.6 #else // GCC >= 4.2, < 4.6
@@ -526,7 +557,7 @@ RAPIDJSON_NAMESPACE_END
#define RAPIDJSON_PRAGMA(x) __pragma(x) #define RAPIDJSON_PRAGMA(x) __pragma(x)
#define RAPIDJSON_DIAG_PRAGMA(x) RAPIDJSON_PRAGMA(warning(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_PUSH RAPIDJSON_DIAG_PRAGMA(push)
#define RAPIDJSON_DIAG_POP RAPIDJSON_DIAG_PRAGMA(pop) #define RAPIDJSON_DIAG_POP RAPIDJSON_DIAG_PRAGMA(pop)
@@ -544,14 +575,18 @@ RAPIDJSON_NAMESPACE_END
#ifndef RAPIDJSON_HAS_CXX11_RVALUE_REFS #ifndef RAPIDJSON_HAS_CXX11_RVALUE_REFS
#if defined(__clang__) #if defined(__clang__)
#if __has_feature(cxx_rvalue_references) && \ #if __has_feature(cxx_rvalue_references) && \
(defined(_MSC_VER) || defined(_LIBCPP_VERSION) || defined(__GLIBCXX__) && __GLIBCXX__ >= 20080306) (defined(_MSC_VER) || defined(_LIBCPP_VERSION) || \
defined(__GLIBCXX__) && __GLIBCXX__ >= 20080306)
#define RAPIDJSON_HAS_CXX11_RVALUE_REFS 1 #define RAPIDJSON_HAS_CXX11_RVALUE_REFS 1
#else #else
#define RAPIDJSON_HAS_CXX11_RVALUE_REFS 0 #define RAPIDJSON_HAS_CXX11_RVALUE_REFS 0
#endif #endif
#elif (defined(RAPIDJSON_GNUC) && (RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4,3,0)) && defined(__GXX_EXPERIMENTAL_CXX0X__)) || \ #elif (defined(RAPIDJSON_GNUC) && \
(RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4, 3, 0)) && \
defined(__GXX_EXPERIMENTAL_CXX0X__)) || \
(defined(_MSC_VER) && _MSC_VER >= 1600) || \ (defined(_MSC_VER) && _MSC_VER >= 1600) || \
(defined(__SUNPRO_CC) && __SUNPRO_CC >= 0x5140 && defined(__GXX_EXPERIMENTAL_CXX0X__)) (defined(__SUNPRO_CC) && __SUNPRO_CC >= 0x5140 && \
defined(__GXX_EXPERIMENTAL_CXX0X__))
#define RAPIDJSON_HAS_CXX11_RVALUE_REFS 1 #define RAPIDJSON_HAS_CXX11_RVALUE_REFS 1
#else #else
@@ -562,9 +597,12 @@ RAPIDJSON_NAMESPACE_END
#ifndef RAPIDJSON_HAS_CXX11_NOEXCEPT #ifndef RAPIDJSON_HAS_CXX11_NOEXCEPT
#if defined(__clang__) #if defined(__clang__)
#define RAPIDJSON_HAS_CXX11_NOEXCEPT __has_feature(cxx_noexcept) #define RAPIDJSON_HAS_CXX11_NOEXCEPT __has_feature(cxx_noexcept)
#elif (defined(RAPIDJSON_GNUC) && (RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4,6,0)) && defined(__GXX_EXPERIMENTAL_CXX0X__)) || \ #elif (defined(RAPIDJSON_GNUC) && \
(RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4, 6, 0)) && \
defined(__GXX_EXPERIMENTAL_CXX0X__)) || \
(defined(_MSC_VER) && _MSC_VER >= 1900) || \ (defined(_MSC_VER) && _MSC_VER >= 1900) || \
(defined(__SUNPRO_CC) && __SUNPRO_CC >= 0x5140 && defined(__GXX_EXPERIMENTAL_CXX0X__)) (defined(__SUNPRO_CC) && __SUNPRO_CC >= 0x5140 && \
defined(__GXX_EXPERIMENTAL_CXX0X__))
#define RAPIDJSON_HAS_CXX11_NOEXCEPT 1 #define RAPIDJSON_HAS_CXX11_NOEXCEPT 1
#else #else
#define RAPIDJSON_HAS_CXX11_NOEXCEPT 0 #define RAPIDJSON_HAS_CXX11_NOEXCEPT 0
@@ -588,9 +626,12 @@ RAPIDJSON_NAMESPACE_END
#ifndef RAPIDJSON_HAS_CXX11_RANGE_FOR #ifndef RAPIDJSON_HAS_CXX11_RANGE_FOR
#if defined(__clang__) #if defined(__clang__)
#define RAPIDJSON_HAS_CXX11_RANGE_FOR __has_feature(cxx_range_for) #define RAPIDJSON_HAS_CXX11_RANGE_FOR __has_feature(cxx_range_for)
#elif (defined(RAPIDJSON_GNUC) && (RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4,6,0)) && defined(__GXX_EXPERIMENTAL_CXX0X__)) || \ #elif (defined(RAPIDJSON_GNUC) && \
(RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4, 6, 0)) && \
defined(__GXX_EXPERIMENTAL_CXX0X__)) || \
(defined(_MSC_VER) && _MSC_VER >= 1700) || \ (defined(_MSC_VER) && _MSC_VER >= 1700) || \
(defined(__SUNPRO_CC) && __SUNPRO_CC >= 0x5140 && defined(__GXX_EXPERIMENTAL_CXX0X__)) (defined(__SUNPRO_CC) && __SUNPRO_CC >= 0x5140 && \
defined(__GXX_EXPERIMENTAL_CXX0X__))
#define RAPIDJSON_HAS_CXX11_RANGE_FOR 1 #define RAPIDJSON_HAS_CXX11_RANGE_FOR 1
#else #else
#define RAPIDJSON_HAS_CXX11_RANGE_FOR 0 #define RAPIDJSON_HAS_CXX11_RANGE_FOR 0
@@ -601,19 +642,19 @@ RAPIDJSON_NAMESPACE_END
// C++17 features // C++17 features
#if defined(__has_cpp_attribute) #if defined(__has_cpp_attribute)
# if __has_cpp_attribute(fallthrough) #if __has_cpp_attribute(fallthrough)
# define RAPIDJSON_DELIBERATE_FALLTHROUGH [[fallthrough]] #define RAPIDJSON_DELIBERATE_FALLTHROUGH [[fallthrough]]
# else
# define RAPIDJSON_DELIBERATE_FALLTHROUGH
# endif
#else #else
# define RAPIDJSON_DELIBERATE_FALLTHROUGH #define RAPIDJSON_DELIBERATE_FALLTHROUGH
#endif
#else
#define RAPIDJSON_DELIBERATE_FALLTHROUGH
#endif #endif
//!@endcond //!@endcond
//! Assertion (in non-throwing contexts). //! Assertion (in non-throwing contexts).
/*! \ingroup RAPIDJSON_CONFIG /*! \ingroup RAPIDJSON_CONFIG
Some functions provide a \c noexcept guarantee, if the compiler supports it. Some functions provide a \c noexcept guarantee, if the compiler supports it.
In these cases, the \ref RAPIDJSON_ASSERT macro cannot be overridden to In these cases, the \ref RAPIDJSON_ASSERT macro cannot be overridden to
throw an exception. This macro adds a separate customization point for throw an exception. This macro adds a separate customization point for
@@ -621,7 +662,7 @@ RAPIDJSON_NAMESPACE_END
Defaults to C \c assert() (as \ref RAPIDJSON_ASSERT), if \c noexcept is Defaults to C \c assert() (as \ref RAPIDJSON_ASSERT), if \c noexcept is
supported, and to \ref RAPIDJSON_ASSERT otherwise. supported, and to \ref RAPIDJSON_ASSERT otherwise.
*/ */
/////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_NOEXCEPT_ASSERT // RAPIDJSON_NOEXCEPT_ASSERT

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

File diff suppressed because it is too large Load Diff

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

File diff suppressed because it is too large Load Diff

115
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 // Licensed under the MIT License (the "License"); you may not use this file
// in compliance with the License. You may obtain a copy of the License at // except in compliance with the License. You may obtain a copy of the License
// at
// //
// http://opensource.org/licenses/MIT // http://opensource.org/licenses/MIT
// //
// Unless required by applicable law or agreed to in writing, software distributed // Unless required by applicable law or agreed to in writing, software
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// CONDITIONS OF ANY KIND, either express or implied. See the License for the // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// specific language governing permissions and limitations under the License. // License for the specific language governing permissions and limitations under
// the License.
#include "rapidjson.h" #include "rapidjson.h"
@@ -27,7 +31,8 @@ RAPIDJSON_NAMESPACE_BEGIN
/*! \class rapidjson::Stream /*! \class rapidjson::Stream
\brief Concept for reading and writing characters. \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(). 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. //! Read the current character from stream without moving the read cursor.
Ch Peek() const; Ch Peek() const;
//! Read the current character from stream and moving the read cursor to next character. //! Read the current character from stream and moving the read cursor to
Ch Take(); next character. Ch Take();
//! Get the current read cursor. //! Get the current read cursor.
//! \return Number of characters read from start. //! \return Number of characters read from start.
@@ -65,36 +70,37 @@ concept Stream {
//! Provides additional information for stream. //! Provides additional information for stream.
/*! /*!
By using traits pattern, this type provides a default configuration for stream. By using traits pattern, this type provides a default configuration for
For custom stream, this type can be specialized for other configuration. stream. For custom stream, this type can be specialized for other
See TEST(Reader, CustomStringStream) in readertest.cpp for example. configuration. See TEST(Reader, CustomStringStream) in readertest.cpp for
example.
*/ */
template<typename Stream> template <typename Stream> struct StreamTraits {
struct StreamTraits {
//! Whether to make local copy of stream for optimization during parsing. //! Whether to make local copy of stream for optimization during parsing.
/*! /*!
By default, for safety, streams do not use local copy optimization. By default, for safety, streams do not use local copy optimization.
Stream that can be copied fast should specialize this, like StreamTraits<StringStream>. Stream that can be copied fast should specialize this, like
StreamTraits<StringStream>.
*/ */
enum { copyOptimization = 0 }; enum { copyOptimization = 0 };
}; };
//! Reserve n characters for writing to a stream. //! Reserve n characters for writing to a stream.
template<typename Stream> template <typename Stream>
inline void PutReserve(Stream& stream, size_t count) { inline void PutReserve(Stream &stream, size_t count) {
(void)stream; (void)stream;
(void)count; (void)count;
} }
//! Write character to a stream, presuming buffer is reserved. //! Write character to a stream, presuming buffer is reserved.
template<typename Stream> template <typename Stream>
inline void PutUnsafe(Stream& stream, typename Stream::Ch c) { inline void PutUnsafe(Stream &stream, typename Stream::Ch c) {
stream.Put(c); stream.Put(c);
} }
//! Put N copies of a character to a stream. //! Put N copies of a character to a stream.
template<typename Stream, typename Ch> template <typename Stream, typename Ch>
inline void PutN(Stream& stream, Ch c, size_t n) { inline void PutN(Stream &stream, Ch c, size_t n) {
PutReserve(stream, n); PutReserve(stream, n);
for (size_t i = 0; i < n; i++) for (size_t i = 0; i < n; i++)
PutUnsafe(stream, c); PutUnsafe(stream, c);
@@ -115,29 +121,29 @@ RAPIDJSON_DIAG_OFF(4702) // unreachable code
RAPIDJSON_DIAG_OFF(4512) // assignment operator could not be generated RAPIDJSON_DIAG_OFF(4512) // assignment operator could not be generated
#endif #endif
template <typename InputStream, typename Encoding = UTF8<> > template <typename InputStream, typename Encoding = UTF8<>>
class GenericStreamWrapper { class GenericStreamWrapper {
public: public:
typedef typename Encoding::Ch Ch; typedef typename Encoding::Ch Ch;
GenericStreamWrapper(InputStream& is): is_(is) {} GenericStreamWrapper(InputStream &is) : is_(is) {}
Ch Peek() const { return is_.Peek(); } Ch Peek() const { return is_.Peek(); }
Ch Take() { return is_.Take(); } Ch Take() { return is_.Take(); }
size_t Tell() { return is_.Tell(); } size_t Tell() { return is_.Tell(); }
Ch* PutBegin() { return is_.PutBegin(); } Ch *PutBegin() { return is_.PutBegin(); }
void Put(Ch ch) { is_.Put(ch); } void Put(Ch ch) { is_.Put(ch); }
void Flush() { is_.Flush(); } void Flush() { is_.Flush(); }
size_t PutEnd(Ch* ch) { return is_.PutEnd(ch); } size_t PutEnd(Ch *ch) { return is_.PutEnd(ch); }
// wrapper for MemoryStream // wrapper for MemoryStream
const Ch* Peek4() const { return is_.Peek4(); } const Ch *Peek4() const { return is_.Peek4(); }
// wrapper for AutoUTFInputStream // wrapper for AutoUTFInputStream
UTFType GetType() const { return is_.GetType(); } UTFType GetType() const { return is_.GetType(); }
bool HasBOM() const { return is_.HasBOM(); } bool HasBOM() const { return is_.HasBOM(); }
protected: protected:
InputStream& is_; InputStream &is_;
}; };
#if defined(_MSC_VER) && _MSC_VER <= 1800 #if defined(_MSC_VER) && _MSC_VER <= 1800
@@ -149,9 +155,8 @@ RAPIDJSON_DIAG_POP
//! Read-only string stream. //! Read-only string stream.
/*! \note implements Stream concept /*! \note implements Stream concept
*/ */
template <typename Encoding> template <typename Encoding> struct GenericStringStream {
struct GenericStringStream {
typedef typename Encoding::Ch Ch; typedef typename Encoding::Ch Ch;
GenericStringStream(const Ch *src) : src_(src), head_(src) {} GenericStringStream(const Ch *src) : src_(src), head_(src) {}
@@ -160,22 +165,28 @@ struct GenericStringStream {
Ch Take() { return *src_++; } Ch Take() { return *src_++; }
size_t Tell() const { return static_cast<size_t>(src_ - head_); } size_t Tell() const { return static_cast<size_t>(src_ - head_); }
Ch* PutBegin() { RAPIDJSON_ASSERT(false); return 0; } Ch *PutBegin() {
RAPIDJSON_ASSERT(false);
return 0;
}
void Put(Ch) { RAPIDJSON_ASSERT(false); } void Put(Ch) { RAPIDJSON_ASSERT(false); }
void Flush() { RAPIDJSON_ASSERT(false); } void Flush() { RAPIDJSON_ASSERT(false); }
size_t PutEnd(Ch*) { RAPIDJSON_ASSERT(false); return 0; } size_t PutEnd(Ch *) {
RAPIDJSON_ASSERT(false);
return 0;
}
const Ch* src_; //!< Current read position. const Ch *src_; //!< Current read position.
const Ch* head_; //!< Original head of the string. const Ch *head_; //!< Original head of the string.
}; };
template <typename Encoding> template <typename Encoding>
struct StreamTraits<GenericStringStream<Encoding> > { struct StreamTraits<GenericStringStream<Encoding>> {
enum { copyOptimization = 1 }; enum { copyOptimization = 1 };
}; };
//! String stream with UTF8 encoding. //! String stream with UTF8 encoding.
typedef GenericStringStream<UTF8<> > StringStream; typedef GenericStringStream<UTF8<>> StringStream;
/////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////////
// InsituStringStream // InsituStringStream
@@ -184,8 +195,7 @@ typedef GenericStringStream<UTF8<> > StringStream;
/*! This string stream is particularly designed for in-situ parsing. /*! This string stream is particularly designed for in-situ parsing.
\note implements Stream concept \note implements Stream concept
*/ */
template <typename Encoding> template <typename Encoding> struct GenericInsituStringStream {
struct GenericInsituStringStream {
typedef typename Encoding::Ch Ch; typedef typename Encoding::Ch Ch;
GenericInsituStringStream(Ch *src) : src_(src), dst_(0), head_(src) {} GenericInsituStringStream(Ch *src) : src_(src), dst_(0), head_(src) {}
@@ -196,27 +206,34 @@ struct GenericInsituStringStream {
size_t Tell() { return static_cast<size_t>(src_ - head_); } size_t Tell() { return static_cast<size_t>(src_ - head_); }
// Write // Write
void Put(Ch c) { RAPIDJSON_ASSERT(dst_ != 0); *dst_++ = c; } void Put(Ch c) {
RAPIDJSON_ASSERT(dst_ != 0);
*dst_++ = c;
}
Ch* PutBegin() { return dst_ = src_; } Ch *PutBegin() { return dst_ = src_; }
size_t PutEnd(Ch* begin) { return static_cast<size_t>(dst_ - begin); } size_t PutEnd(Ch *begin) { return static_cast<size_t>(dst_ - begin); }
void Flush() {} void Flush() {}
Ch* Push(size_t count) { Ch* begin = dst_; dst_ += count; return begin; } Ch *Push(size_t count) {
Ch *begin = dst_;
dst_ += count;
return begin;
}
void Pop(size_t count) { dst_ -= count; } void Pop(size_t count) { dst_ -= count; }
Ch* src_; Ch *src_;
Ch* dst_; Ch *dst_;
Ch* head_; Ch *head_;
}; };
template <typename Encoding> template <typename Encoding>
struct StreamTraits<GenericInsituStringStream<Encoding> > { struct StreamTraits<GenericInsituStringStream<Encoding>> {
enum { copyOptimization = 1 }; enum { copyOptimization = 1 };
}; };
//! Insitu string stream with UTF8 encoding. //! Insitu string stream with UTF8 encoding.
typedef GenericInsituStringStream<UTF8<> > InsituStringStream; typedef GenericInsituStringStream<UTF8<>> InsituStringStream;
RAPIDJSON_NAMESPACE_END RAPIDJSON_NAMESPACE_END

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

@@ -1,22 +1,26 @@
// Tencent is pleased to support the open source community by making RapidJSON available. // Tencent is pleased to support the open source community by making RapidJSON
// available.
// //
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved. // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
// //
// Licensed under the MIT License (the "License"); you may not use this file except // Licensed under the MIT License (the "License"); you may not use this file
// in compliance with the License. You may obtain a copy of the License at // except in compliance with the License. You may obtain a copy of the License
// at
// //
// http://opensource.org/licenses/MIT // http://opensource.org/licenses/MIT
// //
// Unless required by applicable law or agreed to in writing, software distributed // Unless required by applicable law or agreed to in writing, software
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// CONDITIONS OF ANY KIND, either express or implied. See the License for the // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// specific language governing permissions and limitations under the License. // License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_STRINGBUFFER_H_ #ifndef RAPIDJSON_STRINGBUFFER_H_
#define RAPIDJSON_STRINGBUFFER_H_ #define RAPIDJSON_STRINGBUFFER_H_
#include "stream.h"
#include "internal/stack.h" #include "internal/stack.h"
#include "stream.h"
#if RAPIDJSON_HAS_CXX11_RVALUE_REFS #if RAPIDJSON_HAS_CXX11_RVALUE_REFS
#include <utility> // std::move #include <utility> // std::move
@@ -26,7 +30,7 @@
#if defined(__clang__) #if defined(__clang__)
RAPIDJSON_DIAG_PUSH RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(c++98-compat) RAPIDJSON_DIAG_OFF(c++ 98 - compat)
#endif #endif
RAPIDJSON_NAMESPACE_BEGIN RAPIDJSON_NAMESPACE_BEGIN
@@ -42,11 +46,14 @@ class GenericStringBuffer {
public: 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 #if RAPIDJSON_HAS_CXX11_RVALUE_REFS
GenericStringBuffer(GenericStringBuffer&& rhs) : stack_(std::move(rhs.stack_)) {} GenericStringBuffer(GenericStringBuffer &&rhs)
GenericStringBuffer& operator=(GenericStringBuffer&& rhs) { : stack_(std::move(rhs.stack_)) {}
GenericStringBuffer &operator=(GenericStringBuffer &&rhs) {
if (&rhs != this) if (&rhs != this)
stack_ = std::move(rhs.stack_); stack_ = std::move(rhs.stack_);
return *this; return *this;
@@ -66,11 +73,11 @@ public:
} }
void Reserve(size_t count) { stack_.template Reserve<Ch>(count); } void Reserve(size_t count) { stack_.template Reserve<Ch>(count); }
Ch* Push(size_t count) { return stack_.template Push<Ch>(count); } Ch *Push(size_t count) { return stack_.template Push<Ch>(count); }
Ch* PushUnsafe(size_t count) { return stack_.template PushUnsafe<Ch>(count); } Ch *PushUnsafe(size_t count) { return stack_.template PushUnsafe<Ch>(count); }
void Pop(size_t count) { stack_.template Pop<Ch>(count); } void Pop(size_t count) { stack_.template Pop<Ch>(count); }
const Ch* GetString() const { const Ch *GetString() const {
// Push and pop a null terminator. This is safe. // Push and pop a null terminator. This is safe.
*stack_.template Push<Ch>() = '\0'; *stack_.template Push<Ch>() = '\0';
stack_.template Pop<Ch>(1); stack_.template Pop<Ch>(1);
@@ -89,26 +96,29 @@ public:
private: private:
// Prohibit copy constructor & assignment operator. // Prohibit copy constructor & assignment operator.
GenericStringBuffer(const GenericStringBuffer&); GenericStringBuffer(const GenericStringBuffer &);
GenericStringBuffer& operator=(const GenericStringBuffer&); GenericStringBuffer &operator=(const GenericStringBuffer &);
}; };
//! String buffer with UTF8 encoding //! String buffer with UTF8 encoding
typedef GenericStringBuffer<UTF8<> > StringBuffer; typedef GenericStringBuffer<UTF8<>> StringBuffer;
template<typename Encoding, typename Allocator> template <typename Encoding, typename Allocator>
inline void PutReserve(GenericStringBuffer<Encoding, Allocator>& stream, size_t count) { inline void PutReserve(GenericStringBuffer<Encoding, Allocator> &stream,
size_t count) {
stream.Reserve(count); stream.Reserve(count);
} }
template<typename Encoding, typename Allocator> template <typename Encoding, typename Allocator>
inline void PutUnsafe(GenericStringBuffer<Encoding, Allocator>& stream, typename Encoding::Ch c) { inline void PutUnsafe(GenericStringBuffer<Encoding, Allocator> &stream,
typename Encoding::Ch c) {
stream.PutUnsafe(c); stream.PutUnsafe(c);
} }
//! Implement specialized version of PutN() with memset() for better performance. //! Implement specialized version of PutN() with memset() for better
template<> //! performance.
inline void PutN(GenericStringBuffer<UTF8<> >& stream, char c, size_t n) { template <>
inline void PutN(GenericStringBuffer<UTF8<>> &stream, char c, size_t n) {
std::memset(stream.stack_.Push<char>(n), c, n * sizeof(c)); std::memset(stream.stack_.Push<char>(n), c, n * sizeof(c));
} }

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

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

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

File diff suppressed because it is too large Load Diff

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

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

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

@@ -10,66 +10,80 @@
// Only include streams if not disabled // Only include streams if not disabled
#ifndef RAPIDXML_NO_STREAMS #ifndef RAPIDXML_NO_STREAMS
#include <ostream> #include <iterator>
#include <iterator> #include <ostream>
#endif #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 //! \cond internal
namespace internal namespace internal {
{
/////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////
// Internal character operations // Internal character operations
// Copy characters from given range to given output iterator // Copy characters from given range to given output iterator
template<class OutIt, class Ch> template <class OutIt, class Ch>
inline OutIt copy_chars(const Ch *begin, const Ch *end, OutIt out) inline OutIt copy_chars(const Ch *begin, const Ch *end, OutIt out) {
{
while (begin != end) while (begin != end)
*out++ = *begin++; *out++ = *begin++;
return out; return out;
} }
// Copy characters from given range to given output iterator and expand // Copy characters from given range to given output iterator and expand
// characters into references (&lt; &gt; &apos; &quot; &amp;) // characters into references (&lt; &gt; &apos; &quot; &amp;)
template<class OutIt, class Ch> template <class OutIt, class Ch>
inline OutIt copy_and_expand_chars(const Ch *begin, const Ch *end, Ch noexpand, OutIt out) inline OutIt copy_and_expand_chars(const Ch *begin, const Ch *end, Ch noexpand,
{ OutIt out) {
while (begin != end) while (begin != end) {
{ if (*begin == noexpand) {
if (*begin == noexpand)
{
*out++ = *begin; // No expansion, copy character *out++ = *begin; // No expansion, copy character
} } else {
else switch (*begin) {
{
switch (*begin)
{
case Ch('<'): case Ch('<'):
*out++ = Ch('&'); *out++ = Ch('l'); *out++ = Ch('t'); *out++ = Ch(';'); *out++ = Ch('&');
*out++ = Ch('l');
*out++ = Ch('t');
*out++ = Ch(';');
break; break;
case Ch('>'): case Ch('>'):
*out++ = Ch('&'); *out++ = Ch('g'); *out++ = Ch('t'); *out++ = Ch(';'); *out++ = Ch('&');
*out++ = Ch('g');
*out++ = Ch('t');
*out++ = Ch(';');
break; break;
case Ch('\''): case Ch('\''):
*out++ = Ch('&'); *out++ = Ch('a'); *out++ = Ch('p'); *out++ = Ch('o'); *out++ = Ch('s'); *out++ = Ch(';'); *out++ = Ch('&');
*out++ = Ch('a');
*out++ = Ch('p');
*out++ = Ch('o');
*out++ = Ch('s');
*out++ = Ch(';');
break; break;
case Ch('"'): case Ch('"'):
*out++ = Ch('&'); *out++ = Ch('q'); *out++ = Ch('u'); *out++ = Ch('o'); *out++ = Ch('t'); *out++ = Ch(';'); *out++ = Ch('&');
*out++ = Ch('q');
*out++ = Ch('u');
*out++ = Ch('o');
*out++ = Ch('t');
*out++ = Ch(';');
break; break;
case Ch('&'): case Ch('&'):
*out++ = Ch('&'); *out++ = Ch('a'); *out++ = Ch('m'); *out++ = Ch('p'); *out++ = Ch(';'); *out++ = Ch('&');
*out++ = Ch('a');
*out++ = Ch('m');
*out++ = Ch('p');
*out++ = Ch(';');
break; break;
default: default:
*out++ = *begin; // No expansion, copy character *out++ = *begin; // No expansion, copy character
@@ -78,37 +92,34 @@ namespace rapidxml
++begin; // Step to next character ++begin; // Step to next character
} }
return out; return out;
} }
// Fill given output iterator with repetitions of the same character // Fill given output iterator with repetitions of the same character
template<class OutIt, class Ch> template <class OutIt, class Ch>
inline OutIt fill_chars(OutIt out, int n, Ch ch) inline OutIt fill_chars(OutIt out, int n, Ch ch) {
{
for (int i = 0; i < n; ++i) for (int i = 0; i < n; ++i)
*out++ = ch; *out++ = ch;
return out; return out;
} }
// Find character // Find character
template<class Ch, Ch ch> template <class Ch, Ch ch>
inline bool find_char(const Ch *begin, const Ch *end) inline bool find_char(const Ch *begin, const Ch *end) {
{
while (begin != end) while (begin != end)
if (*begin++ == ch) if (*begin++ == ch)
return true; return true;
return false; return false;
} }
/////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////
// Internal printing operations // Internal printing operations
// Print node // Print node
template<class OutIt, class Ch> template <class OutIt, class Ch>
inline OutIt print_node(OutIt out, const xml_node<Ch> *node, int flags, int indent) inline OutIt print_node(OutIt out, const xml_node<Ch> *node, int flags,
{ int indent) {
// Print proper node type // Print proper node type
switch (node->type()) switch (node->type()) {
{
// Document // Document
case node_document: case node_document:
@@ -162,85 +173,101 @@ namespace rapidxml
// Return modified iterator // Return modified iterator
return out; return out;
} }
// Print children of the node // Print children of the node
template<class OutIt, class Ch> template <class OutIt, class Ch>
inline OutIt print_children(OutIt out, const xml_node<Ch> *node, int flags, int indent) 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()) for (xml_node<Ch> *child = node->first_node(); child;
child = child->next_sibling())
out = print_node(out, child, flags, indent); out = print_node(out, child, flags, indent);
return out; return out;
} }
// Print attributes of the node // Print attributes of the node
template<class OutIt, class Ch> template <class OutIt, class Ch>
inline OutIt print_attributes(OutIt out, const xml_node<Ch> *node, int flags) inline OutIt print_attributes(OutIt out, const xml_node<Ch> *node, int flags) {
{ for (xml_attribute<Ch> *attribute = node->first_attribute(); attribute;
for (xml_attribute<Ch> *attribute = node->first_attribute(); attribute; attribute = attribute->next_attribute()) attribute = attribute->next_attribute()) {
{ if (attribute->name() && attribute->value()) {
if (attribute->name() && attribute->value())
{
// Print attribute name // Print attribute name
*out = Ch(' '), ++out; *out = Ch(' '), ++out;
out = copy_chars(attribute->name(), attribute->name() + attribute->name_size(), out); out = copy_chars(attribute->name(),
attribute->name() + attribute->name_size(), out);
*out = Ch('='), ++out; *out = Ch('='), ++out;
// Print attribute value using appropriate quote type // Print attribute value using appropriate quote type
if (find_char<Ch, Ch('"')>(attribute->value(), attribute->value() + attribute->value_size())) if (find_char<Ch, Ch('"')>(attribute->value(),
{ attribute->value() +
attribute->value_size())) {
*out = Ch('\''), ++out; *out = Ch('\''), ++out;
out = copy_and_expand_chars(attribute->value(), attribute->value() + attribute->value_size(), Ch('"'), out); out = copy_and_expand_chars(
attribute->value(), attribute->value() + attribute->value_size(),
Ch('"'), out);
*out = Ch('\''), ++out; *out = Ch('\''), ++out;
} } else {
else
{
*out = Ch('"'), ++out; *out = Ch('"'), ++out;
out = copy_and_expand_chars(attribute->value(), attribute->value() + attribute->value_size(), Ch('\''), out); out = copy_and_expand_chars(
attribute->value(), attribute->value() + attribute->value_size(),
Ch('\''), out);
*out = Ch('"'), ++out; *out = Ch('"'), ++out;
} }
} }
} }
return out; return out;
} }
// Print data node // Print data node
template<class OutIt, class Ch> template <class OutIt, class Ch>
inline OutIt print_data_node(OutIt out, const xml_node<Ch> *node, int flags, int indent) inline OutIt print_data_node(OutIt out, const xml_node<Ch> *node, int flags,
{ int indent) {
assert(node->type() == node_data); assert(node->type() == node_data);
if (!(flags & print_no_indenting)) if (!(flags & print_no_indenting))
out = fill_chars(out, indent, Ch('\t')); out = fill_chars(out, indent, Ch('\t'));
out = copy_and_expand_chars(node->value(), node->value() + node->value_size(), Ch(0), out); out = copy_and_expand_chars(node->value(), node->value() + node->value_size(),
Ch(0), out);
return out; return out;
} }
// Print data node // Print data node
template<class OutIt, class Ch> template <class OutIt, class Ch>
inline OutIt print_cdata_node(OutIt out, const xml_node<Ch> *node, int flags, int indent) inline OutIt print_cdata_node(OutIt out, const xml_node<Ch> *node, int flags,
{ int indent) {
assert(node->type() == node_cdata); assert(node->type() == node_cdata);
if (!(flags & print_no_indenting)) if (!(flags & print_no_indenting))
out = fill_chars(out, indent, Ch('\t')); out = fill_chars(out, indent, Ch('\t'));
*out = Ch('<'); ++out; *out = Ch('<');
*out = Ch('!'); ++out; ++out;
*out = Ch('['); ++out; *out = Ch('!');
*out = Ch('C'); ++out; ++out;
*out = Ch('D'); ++out; *out = Ch('[');
*out = Ch('A'); ++out; ++out;
*out = Ch('T'); ++out; *out = Ch('C');
*out = Ch('A'); ++out; ++out;
*out = Ch('['); ++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 = copy_chars(node->value(), node->value() + node->value_size(), out);
*out = Ch(']'); ++out; *out = Ch(']');
*out = Ch(']'); ++out; ++out;
*out = Ch('>'); ++out; *out = Ch(']');
++out;
*out = Ch('>');
++out;
return out; return out;
} }
// Print element node // Print element node
template<class OutIt, class Ch> template <class OutIt, class Ch>
inline OutIt print_element_node(OutIt out, const xml_node<Ch> *node, int flags, int indent) inline OutIt print_element_node(OutIt out, const xml_node<Ch> *node, int flags,
{ int indent) {
assert(node->type() == node_element); assert(node->type() == node_element);
// Print element name and attributes, if any // Print element name and attributes, if any
@@ -251,31 +278,25 @@ namespace rapidxml
out = print_attributes(out, node, flags); out = print_attributes(out, node, flags);
// If node is childless // If node is childless
if (node->value_size() == 0 && !node->first_node()) if (node->value_size() == 0 && !node->first_node()) {
{
// Print childless node tag ending // Print childless node tag ending
*out = Ch('/'), ++out; *out = Ch('/'), ++out;
*out = Ch('>'), ++out; *out = Ch('>'), ++out;
} } else {
else
{
// Print normal node tag ending // Print normal node tag ending
*out = Ch('>'), ++out; *out = Ch('>'), ++out;
// Test if node contains a single data node only (and no other nodes) // Test if node contains a single data node only (and no other nodes)
xml_node<Ch> *child = node->first_node(); xml_node<Ch> *child = node->first_node();
if (!child) if (!child) {
{
// If node has no children, only print its value without indenting // 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); 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) } else if (child->next_sibling() == 0 && child->type() == node_data) {
{
// If node has a sole data child, only print its value without indenting // 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); out = copy_and_expand_chars(
} child->value(), child->value() + child->value_size(), Ch(0), out);
else } else {
{
// Print all children with full indenting // Print all children with full indenting
if (!(flags & print_no_indenting)) if (!(flags & print_no_indenting))
*out = Ch('\n'), ++out; *out = Ch('\n'), ++out;
@@ -291,12 +312,12 @@ namespace rapidxml
*out = Ch('>'), ++out; *out = Ch('>'), ++out;
} }
return out; return out;
} }
// Print declaration node // Print declaration node
template<class OutIt, class Ch> template <class OutIt, class Ch>
inline OutIt print_declaration_node(OutIt out, const xml_node<Ch> *node, int flags, int indent) inline OutIt print_declaration_node(OutIt out, const xml_node<Ch> *node,
{ int flags, int indent) {
// Print declaration start // Print declaration start
if (!(flags & print_no_indenting)) if (!(flags & print_no_indenting))
out = fill_chars(out, indent, Ch('\t')); out = fill_chars(out, indent, Ch('\t'));
@@ -314,12 +335,12 @@ namespace rapidxml
*out = Ch('>'), ++out; *out = Ch('>'), ++out;
return out; return out;
} }
// Print comment node // Print comment node
template<class OutIt, class Ch> template <class OutIt, class Ch>
inline OutIt print_comment_node(OutIt out, const xml_node<Ch> *node, int flags, int indent) inline OutIt print_comment_node(OutIt out, const xml_node<Ch> *node, int flags,
{ int indent) {
assert(node->type() == node_comment); assert(node->type() == node_comment);
if (!(flags & print_no_indenting)) if (!(flags & print_no_indenting))
out = fill_chars(out, indent, Ch('\t')); out = fill_chars(out, indent, Ch('\t'));
@@ -332,12 +353,12 @@ namespace rapidxml
*out = Ch('-'), ++out; *out = Ch('-'), ++out;
*out = Ch('>'), ++out; *out = Ch('>'), ++out;
return out; return out;
} }
// Print doctype node // Print doctype node
template<class OutIt, class Ch> template <class OutIt, class Ch>
inline OutIt print_doctype_node(OutIt out, const xml_node<Ch> *node, int flags, int indent) inline OutIt print_doctype_node(OutIt out, const xml_node<Ch> *node, int flags,
{ int indent) {
assert(node->type() == node_doctype); assert(node->type() == node_doctype);
if (!(flags & print_no_indenting)) if (!(flags & print_no_indenting))
out = fill_chars(out, indent, Ch('\t')); out = fill_chars(out, indent, Ch('\t'));
@@ -354,12 +375,12 @@ namespace rapidxml
out = copy_chars(node->value(), node->value() + node->value_size(), out); out = copy_chars(node->value(), node->value() + node->value_size(), out);
*out = Ch('>'), ++out; *out = Ch('>'), ++out;
return out; return out;
} }
// Print pi node // Print pi node
template<class OutIt, class Ch> template <class OutIt, class Ch>
inline OutIt print_pi_node(OutIt out, const xml_node<Ch> *node, int flags, int indent) inline OutIt print_pi_node(OutIt out, const xml_node<Ch> *node, int flags,
{ int indent) {
assert(node->type() == node_pi); assert(node->type() == node_pi);
if (!(flags & print_no_indenting)) if (!(flags & print_no_indenting))
out = fill_chars(out, indent, Ch('\t')); out = fill_chars(out, indent, Ch('\t'));
@@ -371,51 +392,50 @@ namespace rapidxml
*out = Ch('?'), ++out; *out = Ch('?'), ++out;
*out = Ch('>'), ++out; *out = Ch('>'), ++out;
return out; return out;
} }
} } // namespace internal
//! \endcond //! \endcond
/////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////
// Printing // Printing
//! Prints XML to given output iterator. //! Prints XML to given output iterator.
//! \param out Output iterator to print to. //! \param out Output iterator to print to.
//! \param node Node to be printed. Pass xml_document to print entire document. //! \param node Node to be printed. Pass xml_document to print entire document.
//! \param flags Flags controlling how XML is printed. //! \param flags Flags controlling how XML is printed.
//! \return Output iterator pointing to position immediately after last character of printed text. //! \return Output iterator pointing to position immediately after last
template<class OutIt, class Ch> //! character of printed text.
inline OutIt print(OutIt out, const xml_node<Ch> &node, int flags = 0) 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); return internal::print_node(out, &node, flags, 0);
} }
#ifndef RAPIDXML_NO_STREAMS #ifndef RAPIDXML_NO_STREAMS
//! Prints XML to given output stream. //! Prints XML to given output stream.
//! \param out Output stream to print to. //! \param out Output stream to print to.
//! \param node Node to be printed. Pass xml_document to print entire document. //! \param node Node to be printed. Pass xml_document to print entire document.
//! \param flags Flags controlling how XML is printed. //! \param flags Flags controlling how XML is printed.
//! \return Output stream. //! \return Output stream.
template<class Ch> template <class Ch>
inline std::basic_ostream<Ch> &print(std::basic_ostream<Ch> &out, const xml_node<Ch> &node, int flags = 0) inline std::basic_ostream<Ch> &print(std::basic_ostream<Ch> &out,
{ const xml_node<Ch> &node, int flags = 0) {
print(std::ostream_iterator<Ch>(out), node, flags); print(std::ostream_iterator<Ch>(out), node, flags);
return out; return out;
} }
//! Prints formatted XML to given output stream. Uses default printing flags. Use print() function to customize printing process. //! Prints formatted XML to given output stream. Uses default printing flags.
//! \param out Output stream to print to. //! Use print() function to customize printing process. \param out Output stream
//! \param node Node to be printed. //! to print to. \param node Node to be printed. \return Output stream.
//! \return Output stream. template <class Ch>
template<class Ch> inline std::basic_ostream<Ch> &operator<<(std::basic_ostream<Ch> &out,
inline std::basic_ostream<Ch> &operator <<(std::basic_ostream<Ch> &out, const xml_node<Ch> &node) const xml_node<Ch> &node) {
{
return print(out, node); return print(out, node);
}
#endif
} }
#endif #endif
} // namespace rapidxml
#endif

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

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

2
livox_ros_driver/launch/lvx_to_rosbag.launch
View File

@@ -5,7 +5,7 @@
<arg name="xfer_format" default="0"/> <arg name="xfer_format" default="0"/>
<arg name="multi_topic" default="0"/> <arg name="multi_topic" default="0"/>
<arg name="data_src" default="2"/> <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="output_type" default="1"/>
<arg name="rviz_enable" default="false"/> <arg name="rviz_enable" default="false"/>
<arg name="rosbag_enable" default="false"/> <arg name="rosbag_enable" default="false"/>

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

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

291
livox_ros_driver/livox_ros_driver/lddc.cpp
View File

@@ -24,30 +24,33 @@
#include "lddc.h" #include "lddc.h"
#include <stdint.h>
#include <inttypes.h> #include <inttypes.h>
#include <math.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 <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 { namespace livox_ros {
typedef pcl::PointCloud<pcl::PointXYZI> PointCloud; typedef pcl::PointCloud<pcl::PointXYZI> PointCloud;
/** Lidar Data Distribute Control ----------------------------------------------------------------*/ /** Lidar Data Distribute Control
Lddc::Lddc(int format, int multi_topic, int data_src, int output_type, double frq) : \ * ----------------------------------------------------------------*/
transfer_format_(format), use_multi_topic_(multi_topic),\ 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) { 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; lds_ = nullptr;
memset(private_pub_, 0, sizeof(private_pub_)); memset(private_pub_, 0, sizeof(private_pub_));
memset(private_imu_pub_, 0, sizeof(private_imu_pub_)); memset(private_imu_pub_, 0, sizeof(private_imu_pub_));
@@ -72,20 +75,21 @@ Lddc::~Lddc() {
lds_->PrepareExit(); lds_->PrepareExit();
} }
for (uint32_t i=0; i<kMaxSourceLidar; i++) { for (uint32_t i = 0; i < kMaxSourceLidar; i++) {
if (private_pub_[i]) { if (private_pub_[i]) {
delete 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]) { if (private_imu_pub_[i]) {
delete 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 timestamp = 0;
uint64_t last_timestamp = 0; uint64_t last_timestamp = 0;
uint32_t published_packet = 0; uint32_t published_packet = 0;
@@ -121,42 +125,61 @@ uint32_t Lddc::PublishPointcloud2(LidarDataQueue* queue, uint32_t packet_num, ui
cloud.fields[5].count = 1; cloud.fields[5].count = 1;
cloud.fields[5].datatype = sensor_msgs::PointField::UINT8; cloud.fields[5].datatype = sensor_msgs::PointField::UINT8;
cloud.data.resize(packet_num * kMaxPointPerEthPacket * sizeof(LivoxPointXyzrtl)); cloud.data.resize(packet_num * kMaxPointPerEthPacket *
sizeof(LivoxPointXyzrtl));
cloud.point_step = sizeof(LivoxPointXyzrtl); cloud.point_step = sizeof(LivoxPointXyzrtl);
uint8_t *point_base = cloud.data.data(); uint8_t *point_base = cloud.data.data();
uint8_t data_source = lds_->lidars_[handle].data_src; uint8_t data_source = lds_->lidars_[handle].data_src;
StoragePacket storage_packet; StoragePacket storage_packet;
while (published_packet < packet_num) { while (published_packet < packet_num) {
QueueProPop(queue, &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);
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); timestamp = GetStoragePacketTimestamp(&storage_packet, data_source);
if (published_packet && \ int64_t packet_gap = timestamp - last_timestamp;
((timestamp - last_timestamp) > kMaxPacketTimeGap)) { 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) { if (kSourceLvxFile != data_source) {
ROS_INFO("Lidar[%d] packet loss", handle); // ROS_INFO("Lidar[%d] packet loss %ld %d %d", handle,
// packet_loss_threshold_lower, packet_interval, raw_packet->data_type);
int64_t packet_loss_threshold_upper = packet_interval * packet_num;
if (packet_gap >
packet_loss_threshold_upper) { // skip when gap is too large
break; 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) { if (!published_packet) { // use the first packet timestamp as pointcloud2
cloud.header.stamp = ros::Time(timestamp/1000000000.0); // to ros time stamp // msg timestamp
cloud.header.stamp = ros::Time(timestamp / 1000000000.0);
} }
cloud.width += storage_packet.point_num; cloud.width += storage_packet.point_num;
if (kSourceLvxFile != data_source) { 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) { if (pf_point_convert) {
point_base = pf_point_convert(point_base, raw_packet, \ point_base = pf_point_convert(
lds_->lidars_[handle].extrinsic_parameter); point_base, raw_packet, lds_->lidars_[handle].extrinsic_parameter);
} else { } else {
/* Skip the packet */ /* 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; break;
} }
} else { } else {
point_base = LivoxPointToPxyzrtl(point_base, raw_packet, \ point_base = LivoxPointToPxyzrtl(
lds_->lidars_[handle].extrinsic_parameter); point_base, raw_packet, lds_->lidars_[handle].extrinsic_parameter);
} }
QueuePopUpdate(queue); QueuePopUpdate(queue);
@@ -169,28 +192,34 @@ uint32_t Lddc::PublishPointcloud2(LidarDataQueue* queue, uint32_t packet_num, ui
cloud.is_dense = true; cloud.is_dense = true;
cloud.data.resize(cloud.row_step); // adjust to the real size cloud.data.resize(cloud.row_step); // adjust to the real size
ros::Publisher* p_publisher = Lddc::GetCurrentPublisher(handle); ros::Publisher *p_publisher = Lddc::GetCurrentPublisher(handle);
if (kOutputToRos == output_type_) { if (kOutputToRos == output_type_) {
p_publisher->publish(cloud); p_publisher->publish(cloud);
} else { } else {
if (bag_) { 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; return published_packet;
} }
/* for pcl::pxyzi */ /* for pcl::pxyzi */
uint32_t Lddc::PublishPointcloudData(LidarDataQueue* queue, uint32_t packet_num, uint8_t handle) { uint32_t Lddc::PublishPointcloudData(LidarDataQueue *queue, uint32_t packet_num,
uint8_t handle) {
uint64_t timestamp = 0; uint64_t timestamp = 0;
uint64_t last_timestamp = 0; uint64_t last_timestamp = 0;
uint32_t published_packet = 0; uint32_t published_packet = 0;
/* init point cloud data struct */ /* init point cloud data struct */
PointCloud::Ptr cloud (new PointCloud); PointCloud::Ptr cloud(new PointCloud);
cloud->header.frame_id = "livox_frame"; cloud->header.frame_id = "livox_frame";
//cloud->header.stamp = ros::Time::now(); // cloud->header.stamp = ros::Time::now();
cloud->height = 1; cloud->height = 1;
cloud->width = 0; cloud->width = 0;
@@ -198,35 +227,53 @@ uint32_t Lddc::PublishPointcloudData(LidarDataQueue* queue, uint32_t packet_num,
StoragePacket storage_packet; StoragePacket storage_packet;
while (published_packet < packet_num) { while (published_packet < packet_num) {
QueueProPop(queue, &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);
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); timestamp = GetStoragePacketTimestamp(&storage_packet, data_source);
if (published_packet && \ int64_t packet_gap = timestamp - last_timestamp;
((timestamp - last_timestamp) > kMaxPacketTimeGap)) { if ((packet_gap > packet_loss_threshold_lower) && published_packet &&
ROS_INFO("Lidar[%d] packet loss", handle); 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; 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) { 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; cloud->width += storage_packet.point_num;
uint8_t point_buf[2048]; uint8_t point_buf[2048];
if (kSourceLvxFile != data_source) { 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) { if (pf_point_convert) {
pf_point_convert(point_buf, raw_packet, \ pf_point_convert(point_buf, raw_packet,
lds_->lidars_[handle].extrinsic_parameter); lds_->lidars_[handle].extrinsic_parameter);
} else { } else {
/* Skip the packet */ /* 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; break;
} }
} else { } else {
LivoxPointToPxyzrtl(point_buf, raw_packet, \ LivoxPointToPxyzrtl(point_buf, raw_packet,
lds_->lidars_[handle].extrinsic_parameter); 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++) { for (uint32_t i = 0; i < storage_packet.point_num; i++) {
pcl::PointXYZI point; pcl::PointXYZI point;
point.x = dst_point->x; point.x = dst_point->x;
@@ -242,20 +289,25 @@ uint32_t Lddc::PublishPointcloudData(LidarDataQueue* queue, uint32_t packet_num,
++published_packet; ++published_packet;
} }
ros::Publisher* p_publisher = Lddc::GetCurrentPublisher(handle); ros::Publisher *p_publisher = Lddc::GetCurrentPublisher(handle);
if (kOutputToRos == output_type_) { if (kOutputToRos == output_type_) {
p_publisher->publish(cloud); p_publisher->publish(cloud);
} else { } else {
if (bag_) { 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; return published_packet;
} }
uint32_t Lddc::PublishCustomPointcloud(LidarDataQueue* queue, uint32_t packet_num, \ uint32_t Lddc::PublishCustomPointcloud(LidarDataQueue *queue,
uint8_t handle) { uint32_t packet_num, uint8_t handle) {
static uint32_t msg_seq = 0; static uint32_t msg_seq = 0;
uint64_t timestamp = 0; uint64_t timestamp = 0;
uint64_t last_timestamp = 0; uint64_t last_timestamp = 0;
@@ -267,7 +319,7 @@ uint32_t Lddc::PublishCustomPointcloud(LidarDataQueue* queue, uint32_t packet_nu
livox_msg.header.frame_id = "livox_frame"; livox_msg.header.frame_id = "livox_frame";
livox_msg.header.seq = msg_seq; livox_msg.header.seq = msg_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.timebase = 0;
livox_msg.point_num = 0; livox_msg.point_num = 0;
livox_msg.lidar_id = handle; livox_msg.lidar_id = handle;
@@ -276,25 +328,27 @@ uint32_t Lddc::PublishCustomPointcloud(LidarDataQueue* queue, uint32_t packet_nu
StoragePacket storage_packet; StoragePacket storage_packet;
while (published_packet < packet_num) { while (published_packet < packet_num) {
QueueProPop(queue, &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);
uint32_t point_interval = GetPointInterval(raw_packet->data_type); uint32_t point_interval = GetPointInterval(raw_packet->data_type);
uint32_t dual_point = 0; uint32_t dual_point = 0;
if ((raw_packet->data_type == kDualExtendCartesian) || \ if ((raw_packet->data_type == kDualExtendCartesian) ||
(raw_packet->data_type == kDualExtendSpherical)) { (raw_packet->data_type == kDualExtendSpherical)) {
dual_point = 1; dual_point = 1;
} }
timestamp = GetStoragePacketTimestamp(&storage_packet, data_source); timestamp = GetStoragePacketTimestamp(&storage_packet, data_source);
if (published_packet && \ if (((timestamp - last_timestamp) > kDeviceDisconnectThreshold) &&
((timestamp - last_timestamp) > kDeviceDisconnectThreshold)) { published_packet && lds_->lidars_[handle].data_is_pubulished) {
ROS_INFO("Lidar[%d] packet loss", handle); ROS_INFO("Lidar[%d] packet loss", handle);
break; break;
} }
if (!published_packet) { if (!published_packet) {
livox_msg.timebase = timestamp; // to us livox_msg.timebase = timestamp; // to us
packet_offset_time = 0; // first packet packet_offset_time = 0; // first packet
livox_msg.header.stamp = ros::Time(timestamp/1000000000.0); // to ros time stamp livox_msg.header.stamp =
//ROS_DEBUG("[%d]:%ld %d", handle, livox_msg.timebase, point_interval); ros::Time(timestamp / 1000000000.0); // to ros time stamp
// ROS_DEBUG("[%d]:%ld %d", handle, livox_msg.timebase, point_interval);
} else { } else {
packet_offset_time = (uint32_t)(timestamp - livox_msg.timebase); packet_offset_time = (uint32_t)(timestamp - livox_msg.timebase);
} }
@@ -302,27 +356,29 @@ uint32_t Lddc::PublishCustomPointcloud(LidarDataQueue* queue, uint32_t packet_nu
uint8_t point_buf[2048]; uint8_t point_buf[2048];
if (kSourceLvxFile != data_source) { 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) { if (pf_point_convert) {
pf_point_convert(point_buf, raw_packet, \ pf_point_convert(point_buf, raw_packet,
lds_->lidars_[handle].extrinsic_parameter); lds_->lidars_[handle].extrinsic_parameter);
} else { } else {
/* Skip the packet */ /* 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; break;
} }
} else { } else {
LivoxPointToPxyzrtl(point_buf, raw_packet, \ LivoxPointToPxyzrtl(point_buf, raw_packet,
lds_->lidars_[handle].extrinsic_parameter); 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++) { for (uint32_t i = 0; i < storage_packet.point_num; i++) {
livox_ros_driver::CustomPoint point; livox_ros_driver::CustomPoint point;
if (!dual_point) { /** dual return mode */ 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 { } 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.x = dst_point->x;
point.y = dst_point->y; point.y = dst_point->y;
@@ -339,19 +395,24 @@ uint32_t Lddc::PublishCustomPointcloud(LidarDataQueue* queue, uint32_t packet_nu
++published_packet; ++published_packet;
} }
ros::Publisher* p_publisher = Lddc::GetCurrentPublisher(handle); ros::Publisher *p_publisher = Lddc::GetCurrentPublisher(handle);
if (kOutputToRos == output_type_) { if (kOutputToRos == output_type_) {
p_publisher->publish(livox_msg); p_publisher->publish(livox_msg);
} else { } else {
if (bag_) { 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; 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) { uint8_t handle) {
uint64_t timestamp = 0; uint64_t timestamp = 0;
uint32_t published_packet = 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; uint8_t data_source = lds_->lidars_[handle].data_src;
StoragePacket storage_packet; StoragePacket storage_packet;
QueueProPop(queue, &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); 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]; uint8_t point_buf[2048];
LivoxImuDataProcess(point_buf, raw_packet); 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.x = imu->gyro_x;
imu_data.angular_velocity.y = imu->gyro_y; imu_data.angular_velocity.y = imu->gyro_y;
imu_data.angular_velocity.z = imu->gyro_z; imu_data.angular_velocity.z = imu->gyro_z;
@@ -380,20 +445,20 @@ uint32_t Lddc::PublishImuData(LidarDataQueue* queue, uint32_t packet_num,
QueuePopUpdate(queue); QueuePopUpdate(queue);
++published_packet; ++published_packet;
ros::Publisher* p_publisher = Lddc::GetCurrentImuPublisher(handle); ros::Publisher *p_publisher = Lddc::GetCurrentImuPublisher(handle);
if (kOutputToRos == output_type_) { if (kOutputToRos == output_type_) {
p_publisher->publish(imu_data); p_publisher->publish(imu_data);
} else { } else {
if (bag_) { 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; return published_packet;
} }
int Lddc::RegisterLds(Lds *lds) {
int Lddc::RegisterLds(Lds* lds) {
if (lds_ == nullptr) { if (lds_ == nullptr) {
lds_ = lds; lds_ = lds;
return 0; return 0;
@@ -402,41 +467,32 @@ int Lddc::RegisterLds(Lds* lds) {
} }
} }
void Lddc::PollingLidarPointCloudData(uint8_t handle, LidarDevice* lidar) { void Lddc::PollingLidarPointCloudData(uint8_t handle, LidarDevice *lidar) {
LidarDataQueue* p_queue = &lidar->data; LidarDataQueue *p_queue = &lidar->data;
if (p_queue == nullptr) { if (p_queue == nullptr) {
return; return;
} }
while (!QueueIsEmpty(p_queue)) { while (!QueueIsEmpty(p_queue)) {
uint32_t used_size = QueueUsedSize(p_queue); uint32_t used_size = QueueUsedSize(p_queue);
uint32_t publish_packet_upper_limit = GetPacketNumPerSec(lidar->info.type); uint32_t onetime_publish_packets =
uint32_t publish_packet_lower_limit = publish_packet_upper_limit / 2 / ((uint32_t)publish_frq_); GetPacketNumPerSec(lidar->info.type) / publish_frq_;
/** increase margin */ if (used_size < onetime_publish_packets) {
publish_packet_upper_limit = publish_packet_upper_limit + publish_packet_upper_limit / 10;
if (used_size < publish_packet_lower_limit) {
break; break;
} }
if (used_size > publish_packet_upper_limit) {
used_size = publish_packet_upper_limit;
}
if (kPointCloud2Msg == transfer_format_) { 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_) { } 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_) { } 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) { void Lddc::PollingLidarImuData(uint8_t handle, LidarDevice *lidar) {
LidarDataQueue* p_queue = &lidar->imu_data; LidarDataQueue *p_queue = &lidar->imu_data;
if (p_queue == nullptr) { if (p_queue == nullptr) {
return; return;
} }
@@ -453,9 +509,10 @@ void Lddc::DistributeLidarData(void) {
for (uint32_t i = 0; i < lds_->lidar_count_; i++) { for (uint32_t i = 0; i < lds_->lidar_count_; i++) {
uint32_t lidar_id = i; uint32_t lidar_id = i;
LidarDevice* lidar = &lds_->lidars_[lidar_id]; LidarDevice *lidar = &lds_->lidars_[lidar_id];
LidarDataQueue* p_queue = &lidar->data; LidarDataQueue *p_queue = &lidar->data;
if ((kConnectStateSampling!= lidar->connect_state) || (p_queue == nullptr)) { if ((kConnectStateSampling != lidar->connect_state) ||
(p_queue == nullptr)) {
continue; continue;
} }
@@ -468,22 +525,22 @@ void Lddc::DistributeLidarData(void) {
} }
} }
ros::Publisher* Lddc::GetCurrentPublisher(uint8_t handle) { ros::Publisher *Lddc::GetCurrentPublisher(uint8_t handle) {
ros::Publisher** pub = nullptr; ros::Publisher **pub = nullptr;
uint32_t queue_size = kMinEthPacketQueueSize * 4; uint32_t queue_size = kMinEthPacketQueueSize;
if (use_multi_topic_) { if (use_multi_topic_) {
pub = &private_pub_[handle]; pub = &private_pub_[handle];
} else { } else {
pub = &global_pub_; pub = &global_pub_;
queue_size = queue_size * 32; queue_size = queue_size * 8;
} }
if (*pub == nullptr) { if (*pub == nullptr) {
char name_str[48]; char name_str[48];
memset(name_str, 0, sizeof(name_str)); memset(name_str, 0, sizeof(name_str));
if (use_multi_topic_) { 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); lds_->lidars_[handle].info.broadcast_code);
ROS_INFO("Support multi topics."); ROS_INFO("Support multi topics.");
} else { } else {
@@ -493,19 +550,21 @@ ros::Publisher* Lddc::GetCurrentPublisher(uint8_t handle) {
*pub = new ros::Publisher; *pub = new ros::Publisher;
if (kPointCloud2Msg == transfer_format_) { if (kPointCloud2Msg == transfer_format_) {
**pub = cur_node_->advertise<sensor_msgs::PointCloud2>(name_str,\ **pub =
queue_size); cur_node_->advertise<sensor_msgs::PointCloud2>(name_str, queue_size);
ROS_INFO("%s publish use PointCloud2 format, publisher queue size [%d]", \ ROS_INFO(
"%s publish use PointCloud2 format, set ROS publisher queue size %d",
name_str, queue_size); name_str, queue_size);
} else if (kLivoxCustomMsg == transfer_format_) { } 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); queue_size);
ROS_INFO("%s publish use livox custom format, publisher queue size [%d]", \ ROS_INFO(
"%s publish use livox custom format, set ROS publisher queue size %d",
name_str, queue_size); name_str, queue_size);
} else if (kPclPxyziMsg == transfer_format_) { } else if (kPclPxyziMsg == transfer_format_) {
**pub = cur_node_->advertise<PointCloud>(name_str,\ **pub = cur_node_->advertise<PointCloud>(name_str, queue_size);
queue_size); ROS_INFO("%s publish use pcl PointXYZI format, set ROS publisher queue "
ROS_INFO("%s publish use pcl PointXYZI format, publisher queue size [%d]", \ "size %d",
name_str, queue_size); name_str, queue_size);
} }
} }
@@ -513,15 +572,15 @@ ros::Publisher* Lddc::GetCurrentPublisher(uint8_t handle) {
return *pub; return *pub;
} }
ros::Publisher* Lddc::GetCurrentImuPublisher(uint8_t handle) { ros::Publisher *Lddc::GetCurrentImuPublisher(uint8_t handle) {
ros::Publisher** pub = nullptr; ros::Publisher **pub = nullptr;
uint32_t queue_size = kMinEthPacketQueueSize * 4; uint32_t queue_size = kMinEthPacketQueueSize;
if (use_multi_topic_) { if (use_multi_topic_) {
pub = &private_imu_pub_[handle]; pub = &private_imu_pub_[handle];
} else { } else {
pub = &global_imu_pub_; pub = &global_imu_pub_;
queue_size = queue_size * 32; queue_size = queue_size * 4;
} }
if (*pub == nullptr) { if (*pub == nullptr) {
@@ -529,7 +588,7 @@ ros::Publisher* Lddc::GetCurrentImuPublisher(uint8_t handle) {
memset(name_str, 0, sizeof(name_str)); memset(name_str, 0, sizeof(name_str));
if (use_multi_topic_) { if (use_multi_topic_) {
ROS_INFO("Support multi topics."); 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); lds_->lidars_[handle].info.broadcast_code);
} else { } else {
ROS_INFO("Support only one topic."); ROS_INFO("Support only one topic.");
@@ -538,14 +597,14 @@ ros::Publisher* Lddc::GetCurrentImuPublisher(uint8_t handle) {
*pub = new ros::Publisher; *pub = new ros::Publisher;
**pub = cur_node_->advertise<sensor_msgs::Imu>(name_str, queue_size); **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; return *pub;
} }
void Lddc::CreateBagFile(const std::string &file_name) {
void Lddc::CreateBagFile(const std::string& file_name) {
if (!bag_) { if (!bag_) {
bag_ = new rosbag::Bag; bag_ = new rosbag::Bag;
bag_->open(file_name, rosbag::bagmode::Write); 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_ #ifndef LIVOX_ROS_DRIVER_LDDC_H_
#define LIVOX_ROS_DRIVER_LDDC_H_ #define LIVOX_ROS_DRIVER_LDDC_H_
#include "livox_sdk.h"
#include "lds.h" #include "lds.h"
#include "livox_sdk.h"
#include <ros/ros.h> #include <ros/ros.h>
#include <rosbag/bag.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 format, int multi_topic, int data_src, int output_type, double frq);
~Lddc(); ~Lddc();
int RegisterLds(Lds* lds); int RegisterLds(Lds *lds);
void DistributeLidarData(void); void DistributeLidarData(void);
void CreateBagFile(const std::string& file_name); void CreateBagFile(const std::string &file_name);
void PrepareExit(void); void PrepareExit(void);
uint8_t GetTransferFormat(void) { return transfer_format_; } uint8_t GetTransferFormat(void) { return transfer_format_; }
uint8_t IsMultiTopic(void) { return use_multi_topic_; } 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; } void SetPublishFrq(uint32_t frq) { publish_frq_ = frq; }
Lds* lds_; Lds *lds_;
private: private:
uint32_t PublishPointcloud2(LidarDataQueue* queue, uint32_t packet_num, \ uint32_t PublishPointcloud2(LidarDataQueue *queue, uint32_t packet_num,
uint8_t handle); uint8_t handle);
uint32_t PublishPointcloudData(LidarDataQueue* queue, uint32_t packet_num, \ uint32_t PublishPointcloudData(LidarDataQueue *queue, uint32_t packet_num,
uint8_t handle); uint8_t handle);
uint32_t PublishCustomPointcloud(LidarDataQueue* queue, uint32_t packet_num,\ uint32_t PublishCustomPointcloud(LidarDataQueue *queue, uint32_t packet_num,
uint8_t handle); uint8_t handle);
uint32_t PublishImuData(LidarDataQueue* queue, uint32_t packet_num,\ uint32_t PublishImuData(LidarDataQueue *queue, uint32_t packet_num,
uint8_t handle); uint8_t handle);
ros::Publisher* GetCurrentPublisher(uint8_t handle); ros::Publisher *GetCurrentPublisher(uint8_t handle);
ros::Publisher* GetCurrentImuPublisher(uint8_t handle); ros::Publisher *GetCurrentImuPublisher(uint8_t handle);
void PollingLidarPointCloudData(uint8_t handle, LidarDevice* lidar); void PollingLidarPointCloudData(uint8_t handle, LidarDevice *lidar);
void PollingLidarImuData(uint8_t handle, LidarDevice* lidar); void PollingLidarImuData(uint8_t handle, LidarDevice *lidar);
uint8_t transfer_format_; uint8_t transfer_format_;
uint8_t use_multi_topic_; uint8_t use_multi_topic_;
@@ -79,14 +79,14 @@ private:
uint8_t output_type_; uint8_t output_type_;
double publish_frq_; double publish_frq_;
int32_t publish_interval_ms_; int32_t publish_interval_ms_;
ros::Publisher* private_pub_[kMaxSourceLidar]; ros::Publisher *private_pub_[kMaxSourceLidar];
ros::Publisher* global_pub_; ros::Publisher *global_pub_;
ros::Publisher* private_imu_pub_[kMaxSourceLidar]; ros::Publisher *private_imu_pub_[kMaxSourceLidar];
ros::Publisher* global_imu_pub_; ros::Publisher *global_imu_pub_;
ros::NodeHandle* cur_node_; ros::NodeHandle *cur_node_;
rosbag::Bag* bag_; rosbag::Bag *bag_;
}; };
} } // namespace livox_ros
#endif #endif

42
livox_ros_driver/livox_ros_driver/ldq.cpp
View File

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

31
livox_ros_driver/livox_ros_driver/ldq.h
View File

@@ -33,7 +33,6 @@ namespace livox_ros {
const uint32_t KEthPacketMaxLength = 1500; const uint32_t KEthPacketMaxLength = 1500;
#pragma pack(1) #pragma pack(1)
typedef struct { typedef struct {
@@ -58,9 +57,9 @@ inline static bool IsPowerOf2(uint32_t size) {
inline static uint32_t RoundupPowerOf2(uint32_t size) { inline static uint32_t RoundupPowerOf2(uint32_t size) {
uint32_t power2_val = 0; 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; power2_val = ((uint32_t)1) << i;
if(size <= power2_val) { if (size <= power2_val) {
break; break;
} }
} }
@@ -69,19 +68,19 @@ inline static uint32_t RoundupPowerOf2(uint32_t size) {
} }
/** queue operate function */ /** queue operate function */
int InitQueue(LidarDataQueue* queue, uint32_t queue_size); int InitQueue(LidarDataQueue *queue, uint32_t queue_size);
int DeInitQueue(LidarDataQueue* queue); int DeInitQueue(LidarDataQueue *queue);
void ResetQueue(LidarDataQueue* queue); void ResetQueue(LidarDataQueue *queue);
void QueueProPop(LidarDataQueue* queue, StoragePacket* storage_packet); void QueueProPop(LidarDataQueue *queue, StoragePacket *storage_packet);
void QueuePopUpdate(LidarDataQueue* queue); void QueuePopUpdate(LidarDataQueue *queue);
uint32_t QueuePop(LidarDataQueue* queue, StoragePacket* storage_packet); uint32_t QueuePop(LidarDataQueue *queue, StoragePacket *storage_packet);
uint32_t QueueUsedSize(LidarDataQueue* queue); uint32_t QueueUsedSize(LidarDataQueue *queue);
uint32_t QueueUnusedSize(LidarDataQueue* queue); uint32_t QueueUnusedSize(LidarDataQueue *queue);
uint32_t QueueIsFull(LidarDataQueue* queue); uint32_t QueueIsFull(LidarDataQueue *queue);
uint32_t QueueIsEmpty(LidarDataQueue* queue); uint32_t QueueIsEmpty(LidarDataQueue *queue);
uint32_t QueuePush(LidarDataQueue* queue, StoragePacket* storage_packet); uint32_t QueuePush(LidarDataQueue *queue, StoragePacket *storage_packet);
uint32_t QueuePushAny(LidarDataQueue* queue, uint8_t *data, uint32_t length, \ uint32_t QueuePushAny(LidarDataQueue *queue, uint8_t *data, uint32_t length,
uint64_t time_rcv, uint32_t point_num); uint64_t time_rcv, uint32_t point_num);
} } // namespace livox_ros
#endif #endif

238
livox_ros_driver/livox_ros_driver/lds.cpp
View File

@@ -24,16 +24,17 @@
#include "lds.h" #include "lds.h"
#include <chrono>
#include <math.h> #include <math.h>
#include <stdio.h> #include <stdio.h>
#include <string.h> #include <string.h>
#include <time.h> #include <time.h>
#include <chrono>
namespace livox_ros { namespace livox_ros {
/** Common function ------ ----------------------------------------------------------------------- */ /** Common function ------
bool IsFilePathValid(const char* path_str) { * ----------------------------------------------------------------------- */
bool IsFilePathValid(const char *path_str) {
int str_len = strlen(path_str); int str_len = strlen(path_str);
if ((str_len > kPathStrMinSize) && (str_len < kPathStrMaxSize)) { 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; LdsStamp timestamp;
memcpy(timestamp.stamp_bytes, raw_packet->timestamp, sizeof(timestamp)); memcpy(timestamp.stamp_bytes, raw_packet->timestamp, sizeof(timestamp));
@@ -92,17 +94,17 @@ uint32_t CalculatePacketQueueSize(uint32_t interval_ms, uint32_t data_type) {
return queue_size; return queue_size;
} }
void ParseCommandlineInputBdCode(const char* cammandline_str, void ParseCommandlineInputBdCode(const char *cammandline_str,
std::vector<std::string>& bd_code_list) { std::vector<std::string> &bd_code_list) {
char* strs = new char[strlen(cammandline_str) + 1]; char *strs = new char[strlen(cammandline_str) + 1];
strcpy(strs, cammandline_str); strcpy(strs, cammandline_str);
std::string pattern = "&"; std::string pattern = "&";
char* bd_str = strtok(strs, pattern.c_str()); char *bd_str = strtok(strs, pattern.c_str());
std::string invalid_bd = "000000000"; std::string invalid_bd = "000000000";
while (bd_str != NULL) { while (bd_str != NULL) {
printf("Commandline input bd:%s\n", bd_str); 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()))) { (NULL == strstr(bd_str, invalid_bd.c_str()))) {
bd_code_list.push_back(bd_str); bd_code_list.push_back(bd_str);
} else { } else {
@@ -111,7 +113,7 @@ void ParseCommandlineInputBdCode(const char* cammandline_str,
bd_str = strtok(NULL, pattern.c_str()); bd_str = strtok(NULL, pattern.c_str());
} }
delete [] strs; delete[] strs;
} }
void EulerAnglesToRotationMatrix(EulerAngle euler, RotationMatrix matrix) { void EulerAnglesToRotationMatrix(EulerAngle euler, RotationMatrix matrix) {
@@ -134,56 +136,55 @@ void EulerAnglesToRotationMatrix(EulerAngle euler, RotationMatrix matrix) {
matrix[2][1] = sin_roll * cos_pitch; matrix[2][1] = sin_roll * cos_pitch;
matrix[2][2] = cos_roll * cos_pitch; matrix[2][2] = cos_roll * cos_pitch;
/* /*
float rotate[3][3] = { float rotate[3][3] = {
{ {
std::cos(info.pitch) * std::cos(info.yaw), 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::sin(info.roll) * std::sin(info.pitch) * std::cos(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.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::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::sin(info.roll) * std::sin(info.pitch) * std::sin(info.yaw) +
std::cos(info.roll) * std::sin(info.pitch) * std::sin(info.yaw) - std::sin(info.roll) * std::cos(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.pitch),
std::sin(info.roll) * std::cos(info.pitch), std::sin(info.roll) * std::cos(info.pitch),
std::cos(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) {
void PointExtrisincCompensation(PointXyz* dst_point, const PointXyz& src_point, \ dst_point->x = src_point.x * extrinsic.rotation[0][0] +
ExtrinsicParameter& extrinsic) { src_point.y * extrinsic.rotation[0][1] +
dst_point->x = src_point.x * extrinsic.rotation[0][0] + \ src_point.z * extrinsic.rotation[0][2] + extrinsic.trans[0];
src_point.y * extrinsic.rotation[0][1] + \ dst_point->y = src_point.x * extrinsic.rotation[1][0] +
src_point.z * extrinsic.rotation[0][2] + \ src_point.y * extrinsic.rotation[1][1] +
extrinsic.trans[0]; src_point.z * extrinsic.rotation[1][2] + extrinsic.trans[1];
dst_point->y = src_point.x * extrinsic.rotation[1][0] + \ dst_point->z = src_point.x * extrinsic.rotation[2][0] +
src_point.y * extrinsic.rotation[1][1] + \ src_point.y * extrinsic.rotation[2][1] +
src_point.z * extrinsic.rotation[1][2] + \ src_point.z * extrinsic.rotation[2][2] + extrinsic.trans[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
/** Livox point procees for different raw data format --------------------------------------------*/ * --------------------------------------------*/
uint8_t* LivoxPointToPxyzrtl(uint8_t* point_buf, LivoxEthPacket* eth_packet, \ uint8_t *LivoxPointToPxyzrtl(uint8_t *point_buf, LivoxEthPacket *eth_packet,
ExtrinsicParameter& extrinsic) { ExtrinsicParameter &extrinsic) {
LivoxPointXyzrtl* dst_point = (LivoxPointXyzrtl *)point_buf; LivoxPointXyzrtl *dst_point = (LivoxPointXyzrtl *)point_buf;
uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type); uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type);
LivoxPoint* raw_point = reinterpret_cast<LivoxPoint *>(eth_packet->data); LivoxPoint *raw_point = reinterpret_cast<LivoxPoint *>(eth_packet->data);
while(points_per_packet) { while (points_per_packet) {
RawPointConvert((LivoxPointXyzr*)dst_point, raw_point); RawPointConvert((LivoxPointXyzr *)dst_point, raw_point);
if (extrinsic.enable) { if (extrinsic.enable) {
PointXyz src_point = *((PointXyz*)dst_point); PointXyz src_point = *((PointXyz *)dst_point);
PointExtrisincCompensation((PointXyz*)dst_point, src_point, extrinsic); PointExtrisincCompensation((PointXyz *)dst_point, src_point, extrinsic);
} }
dst_point->tag = 0; dst_point->tag = 0;
dst_point->line = 0; dst_point->line = 0;
@@ -195,17 +196,18 @@ uint8_t* LivoxPointToPxyzrtl(uint8_t* point_buf, LivoxEthPacket* eth_packet, \
return (uint8_t *)dst_point; return (uint8_t *)dst_point;
} }
uint8_t* LivoxRawPointToPxyzrtl(uint8_t* point_buf, LivoxEthPacket* eth_packet, \ uint8_t *LivoxRawPointToPxyzrtl(uint8_t *point_buf, LivoxEthPacket *eth_packet,
ExtrinsicParameter& extrinsic) { ExtrinsicParameter &extrinsic) {
LivoxPointXyzrtl* dst_point = (LivoxPointXyzrtl *)point_buf; LivoxPointXyzrtl *dst_point = (LivoxPointXyzrtl *)point_buf;
uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type); uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type);
LivoxRawPoint* raw_point = reinterpret_cast<LivoxRawPoint *>(eth_packet->data); LivoxRawPoint *raw_point =
reinterpret_cast<LivoxRawPoint *>(eth_packet->data);
while(points_per_packet) { while (points_per_packet) {
RawPointConvert((LivoxPointXyzr*)dst_point, raw_point); RawPointConvert((LivoxPointXyzr *)dst_point, raw_point);
if (extrinsic.enable) { if (extrinsic.enable) {
PointXyz src_point = *((PointXyz*)dst_point); PointXyz src_point = *((PointXyz *)dst_point);
PointExtrisincCompensation((PointXyz*)dst_point, src_point, extrinsic); PointExtrisincCompensation((PointXyz *)dst_point, src_point, extrinsic);
} }
dst_point->tag = 0; dst_point->tag = 0;
dst_point->line = 0; dst_point->line = 0;
@@ -217,17 +219,19 @@ uint8_t* LivoxRawPointToPxyzrtl(uint8_t* point_buf, LivoxEthPacket* eth_packet,
return (uint8_t *)dst_point; return (uint8_t *)dst_point;
} }
uint8_t* LivoxSpherPointToPxyzrtl(uint8_t* point_buf, LivoxEthPacket* eth_packet, \ uint8_t *LivoxSpherPointToPxyzrtl(uint8_t *point_buf,
ExtrinsicParameter& extrinsic) { LivoxEthPacket *eth_packet,
LivoxPointXyzrtl* dst_point = (LivoxPointXyzrtl *)point_buf; ExtrinsicParameter &extrinsic) {
LivoxPointXyzrtl *dst_point = (LivoxPointXyzrtl *)point_buf;
uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type); uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type);
LivoxSpherPoint* raw_point = reinterpret_cast<LivoxSpherPoint *>(eth_packet->data); LivoxSpherPoint *raw_point =
reinterpret_cast<LivoxSpherPoint *>(eth_packet->data);
while(points_per_packet) { while (points_per_packet) {
RawPointConvert((LivoxPointXyzr*)dst_point, raw_point); RawPointConvert((LivoxPointXyzr *)dst_point, raw_point);
if (extrinsic.enable) { if (extrinsic.enable) {
PointXyz src_point = *((PointXyz*)dst_point); PointXyz src_point = *((PointXyz *)dst_point);
PointExtrisincCompensation((PointXyz*)dst_point, src_point, extrinsic); PointExtrisincCompensation((PointXyz *)dst_point, src_point, extrinsic);
} }
dst_point->tag = 0; dst_point->tag = 0;
dst_point->line = 0; dst_point->line = 0;
@@ -239,19 +243,20 @@ uint8_t* LivoxSpherPointToPxyzrtl(uint8_t* point_buf, LivoxEthPacket* eth_packet
return (uint8_t *)dst_point; return (uint8_t *)dst_point;
} }
uint8_t* LivoxExtendRawPointToPxyzrtl(uint8_t* point_buf, LivoxEthPacket* eth_packet, \ uint8_t *LivoxExtendRawPointToPxyzrtl(uint8_t *point_buf,
ExtrinsicParameter& extrinsic) { LivoxEthPacket *eth_packet,
LivoxPointXyzrtl* dst_point = (LivoxPointXyzrtl *)point_buf; ExtrinsicParameter &extrinsic) {
LivoxPointXyzrtl *dst_point = (LivoxPointXyzrtl *)point_buf;
uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type); uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type);
LivoxExtendRawPoint* raw_point = \ LivoxExtendRawPoint *raw_point =
reinterpret_cast<LivoxExtendRawPoint *>(eth_packet->data); reinterpret_cast<LivoxExtendRawPoint *>(eth_packet->data);
uint8_t line_id = 0; uint8_t line_id = 0;
while(points_per_packet) { while (points_per_packet) {
RawPointConvert((LivoxPointXyzr*)dst_point, (LivoxRawPoint *)raw_point); RawPointConvert((LivoxPointXyzr *)dst_point, (LivoxRawPoint *)raw_point);
if (extrinsic.enable) { if (extrinsic.enable) {
PointXyz src_point = *((PointXyz*)dst_point); PointXyz src_point = *((PointXyz *)dst_point);
PointExtrisincCompensation((PointXyz*)dst_point, src_point, extrinsic); 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 = line_id;
@@ -265,18 +270,20 @@ uint8_t* LivoxExtendRawPointToPxyzrtl(uint8_t* point_buf, LivoxEthPacket* eth_p
return (uint8_t *)dst_point; return (uint8_t *)dst_point;
} }
uint8_t* LivoxExtendSpherPointToPxyzrtl(uint8_t* point_buf, LivoxEthPacket* eth_packet, \ uint8_t *LivoxExtendSpherPointToPxyzrtl(uint8_t *point_buf,
ExtrinsicParameter& extrinsic) { LivoxEthPacket *eth_packet,
LivoxPointXyzrtl* dst_point = (LivoxPointXyzrtl *)point_buf; ExtrinsicParameter &extrinsic) {
LivoxPointXyzrtl *dst_point = (LivoxPointXyzrtl *)point_buf;
uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type); uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type);
LivoxExtendSpherPoint* raw_point = reinterpret_cast<LivoxExtendSpherPoint *>(eth_packet->data); LivoxExtendSpherPoint *raw_point =
reinterpret_cast<LivoxExtendSpherPoint *>(eth_packet->data);
uint8_t line_id = 0; uint8_t line_id = 0;
while(points_per_packet) { while (points_per_packet) {
RawPointConvert((LivoxPointXyzr*)dst_point, (LivoxSpherPoint *)raw_point); RawPointConvert((LivoxPointXyzr *)dst_point, (LivoxSpherPoint *)raw_point);
if (extrinsic.enable) { if (extrinsic.enable) {
PointXyz src_point = *((PointXyz*)dst_point); PointXyz src_point = *((PointXyz *)dst_point);
PointExtrisincCompensation((PointXyz*)dst_point, src_point, extrinsic); 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 = line_id;
@@ -290,22 +297,24 @@ uint8_t* LivoxExtendSpherPointToPxyzrtl(uint8_t* point_buf, LivoxEthPacket* eth
return (uint8_t *)dst_point; return (uint8_t *)dst_point;
} }
uint8_t* LivoxDualExtendRawPointToPxyzrtl(uint8_t* point_buf, LivoxEthPacket* eth_packet, \ uint8_t *LivoxDualExtendRawPointToPxyzrtl(uint8_t *point_buf,
ExtrinsicParameter& extrinsic) { LivoxEthPacket *eth_packet,
LivoxPointXyzrtl* dst_point = (LivoxPointXyzrtl *)point_buf; ExtrinsicParameter &extrinsic) {
LivoxPointXyzrtl *dst_point = (LivoxPointXyzrtl *)point_buf;
uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type); uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type);
LivoxExtendRawPoint* raw_point = \ LivoxExtendRawPoint *raw_point =
reinterpret_cast<LivoxExtendRawPoint *>(eth_packet->data); reinterpret_cast<LivoxExtendRawPoint *>(eth_packet->data);
uint8_t line_id = 0; uint8_t line_id = 0;
while(points_per_packet) { while (points_per_packet) {
RawPointConvert((LivoxPointXyzr*)dst_point, (LivoxRawPoint *)raw_point); RawPointConvert((LivoxPointXyzr *)dst_point, (LivoxRawPoint *)raw_point);
if (extrinsic.enable) { if (extrinsic.enable) {
PointXyz src_point = *((PointXyz*)dst_point); PointXyz src_point = *((PointXyz *)dst_point);
PointExtrisincCompensation((PointXyz*)dst_point, src_point, extrinsic); PointExtrisincCompensation((PointXyz *)dst_point, src_point, extrinsic);
} }
dst_point->tag = raw_point->tag; dst_point->tag = raw_point->tag;
dst_point->line = line_id / 2; /* LivoxDualExtendRawPoint = 2*LivoxExtendRawPoint */ dst_point->line =
line_id / 2; /* LivoxDualExtendRawPoint = 2*LivoxExtendRawPoint */
dst_point->line = dst_point->line % 6; dst_point->line = dst_point->line % 6;
++raw_point; ++raw_point;
++dst_point; ++dst_point;
@@ -316,20 +325,22 @@ uint8_t* LivoxDualExtendRawPointToPxyzrtl(uint8_t* point_buf, LivoxEthPacket* et
return (uint8_t *)dst_point; return (uint8_t *)dst_point;
} }
uint8_t* LivoxDualExtendSpherPointToPxyzrtl(uint8_t* point_buf, LivoxEthPacket* eth_packet, \ uint8_t *LivoxDualExtendSpherPointToPxyzrtl(uint8_t *point_buf,
ExtrinsicParameter& extrinsic) { LivoxEthPacket *eth_packet,
LivoxPointXyzrtl* dst_point = (LivoxPointXyzrtl *)point_buf; ExtrinsicParameter &extrinsic) {
LivoxPointXyzrtl *dst_point = (LivoxPointXyzrtl *)point_buf;
uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type); uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type);
LivoxDualExtendSpherPoint* raw_point = \ LivoxDualExtendSpherPoint *raw_point =
reinterpret_cast<LivoxDualExtendSpherPoint *>(eth_packet->data); reinterpret_cast<LivoxDualExtendSpherPoint *>(eth_packet->data);
uint8_t line_id = 0; uint8_t line_id = 0;
while(points_per_packet) { while (points_per_packet) {
RawPointConvert((LivoxPointXyzr*)dst_point, (LivoxPointXyzr *)(dst_point + 1), \ RawPointConvert((LivoxPointXyzr *)dst_point,
(LivoxPointXyzr *)(dst_point + 1),
(LivoxDualExtendSpherPoint *)raw_point); (LivoxDualExtendSpherPoint *)raw_point);
if (extrinsic.enable) { if (extrinsic.enable) {
PointXyz src_point = *((PointXyz*)dst_point); PointXyz src_point = *((PointXyz *)dst_point);
PointExtrisincCompensation((PointXyz*)dst_point, src_point, extrinsic); 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 = line_id;
@@ -337,8 +348,8 @@ uint8_t* LivoxDualExtendSpherPointToPxyzrtl(uint8_t* point_buf, LivoxEthPacket*
++dst_point; ++dst_point;
if (extrinsic.enable) { if (extrinsic.enable) {
PointXyz src_point = *((PointXyz*)dst_point); PointXyz src_point = *((PointXyz *)dst_point);
PointExtrisincCompensation((PointXyz*)dst_point, src_point, extrinsic); 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 = line_id;
@@ -353,7 +364,7 @@ uint8_t* LivoxDualExtendSpherPointToPxyzrtl(uint8_t* point_buf, LivoxEthPacket*
return (uint8_t *)dst_point; 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)); memcpy(point_buf, eth_packet->data, sizeof(LivoxImuPoint));
return point_buf; return point_buf;
} }
@@ -365,8 +376,7 @@ const PointConvertHandler to_pxyzi_handler_table[kMaxPointDataType] = {
LivoxExtendSpherPointToPxyzrtl, LivoxExtendSpherPointToPxyzrtl,
LivoxDualExtendRawPointToPxyzrtl, LivoxDualExtendRawPointToPxyzrtl,
LivoxDualExtendSpherPointToPxyzrtl, LivoxDualExtendSpherPointToPxyzrtl,
nullptr nullptr};
};
PointConvertHandler GetConvertHandler(uint8_t data_type) { PointConvertHandler GetConvertHandler(uint8_t data_type) {
if (data_type < kMaxPointDataType) if (data_type < kMaxPointDataType)
@@ -375,7 +385,23 @@ PointConvertHandler GetConvertHandler(uint8_t data_type) {
return nullptr; 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 #if 0
@@ -388,7 +414,8 @@ static void PointCloudConvert(LivoxPoint *p_dpoint, LivoxRawPoint *p_raw_point)
#endif #endif
/* Member function ------ ----------------------------------------------------------------------- */ /* Member function ------
* ----------------------------------------------------------------------- */
Lds::Lds(uint32_t buffer_time_ms, uint8_t data_src) Lds::Lds(uint32_t buffer_time_ms, uint8_t data_src)
: buffer_time_ms_(buffer_time_ms), data_src_(data_src) { : buffer_time_ms_(buffer_time_ms), data_src_(data_src) {
@@ -402,7 +429,7 @@ Lds::~Lds() {
ResetLds(0); 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->data);
DeInitQueue(&lidar->imu_data); DeInitQueue(&lidar->imu_data);
@@ -410,17 +437,18 @@ void Lds::ResetLidar(LidarDevice* lidar, uint8_t data_src) {
/** unallocated state */ /** unallocated state */
lidar->handle = kMaxSourceLidar; lidar->handle = kMaxSourceLidar;
lidar->connect_state = kConnectStateOff;
lidar->data_src = data_src; 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; lidar->data_src = data_src;
} }
void Lds::ResetLds(uint8_t data_src) { void Lds::ResetLds(uint8_t data_src) {
lidar_count_ = kMaxSourceLidar; lidar_count_ = kMaxSourceLidar;
for (uint32_t i=0; i<kMaxSourceLidar; i++) { for (uint32_t i = 0; i < kMaxSourceLidar; i++) {
ResetLidar(&lidars_[i], data_src); 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
}

101
livox_ros_driver/livox_ros_driver/lds.h
View File

@@ -27,12 +27,12 @@
#ifndef LIVOX_ROS_DRIVER_LDS_H_ #ifndef LIVOX_ROS_DRIVER_LDS_H_
#define LIVOX_ROS_DRIVER_LDS_H_ #define LIVOX_ROS_DRIVER_LDS_H_
#include <math.h>
#include <stdint.h> #include <stdint.h>
#include <stdio.h> #include <stdio.h>
#include <stdlib.h> #include <stdlib.h>
#include <math.h>
#include <vector>
#include <string> #include <string>
#include <vector>
#include "ldq.h" #include "ldq.h"
@@ -50,13 +50,15 @@ const uint32_t kMaxEthPacketQueueSize = 8192; /**< must be 2^n */
const uint32_t kImuEthPacketQueueSize = 256; const uint32_t kImuEthPacketQueueSize = 256;
const uint32_t KEthPacketHeaderLength = 18; /**< (sizeof(LivoxEthPacket) - 1) */ const uint32_t KEthPacketHeaderLength = 18; /**< (sizeof(LivoxEthPacket) - 1) */
//const uint32_t KEthPacketMaxLength = 1500; // const uint32_t KEthPacketMaxLength = 1500;
const uint32_t KCartesianPointSize = 13; const uint32_t KCartesianPointSize = 13;
const uint32_t KSphericalPointSzie = 9; const uint32_t KSphericalPointSzie = 9;
const int64_t kPacketTimeGap = 1000000; /**< 1ms = 1000000ns */ const int64_t kPacketTimeGap = 1000000; /**< 1ms = 1000000ns */
const int64_t kMaxPacketTimeGap = 1700000; /**< the threshold of packet continuous */ const int64_t kMaxPacketTimeGap =
const int64_t kDeviceDisconnectThreshold = 1000000000; /**< the threshold of device disconect */ 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 kNsPerSecond = 1000000000; /**< 1s = 1000000000ns */
const int kPathStrMinSize = 4; /**< Must more than 4 char */ const int kPathStrMinSize = 4; /**< Must more than 4 char */
@@ -90,10 +92,7 @@ typedef enum {
kOutputToRosBagFile = 1, kOutputToRosBagFile = 1,
} LidarDataOutputType; } LidarDataOutputType;
typedef enum { typedef enum { kCoordinateCartesian = 0, kCoordinateSpherical } CoordinateType;
kCoordinateCartesian = 0,
kCoordinateSpherical
} CoordinateType;
typedef enum { typedef enum {
kConfigFan = 1, kConfigFan = 1,
@@ -167,6 +166,8 @@ typedef struct {
typedef struct { typedef struct {
uint8_t handle; /**< Lidar access handle. */ uint8_t handle; /**< Lidar access handle. */
uint8_t data_src; /**< From raw lidar or livox file. */ uint8_t data_src; /**< From raw lidar or livox file. */
bool data_is_pubulished; /**< Indicate the data of lidar whether is
pubulished. */
volatile LidarConnectState connect_state; volatile LidarConnectState connect_state;
DeviceInfo info; DeviceInfo info;
LidarPacketStatistic statistic_info; LidarPacketStatistic statistic_info;
@@ -179,27 +180,28 @@ typedef struct {
typedef struct { typedef struct {
uint32_t points_per_packet; uint32_t points_per_packet;
uint32_t points_per_second; /**< unit:ns */ uint32_t points_per_second;
uint32_t point_interval; uint32_t point_interval; /**< unit:ns */
uint32_t packet_interval; /**< unit:ns */
uint32_t packet_length; uint32_t packet_length;
} PacketInfoPair; } PacketInfoPair;
#pragma pack(1) #pragma pack(1)
typedef struct{ typedef struct {
float x; /**< X axis, Unit:m */ float x; /**< X axis, Unit:m */
float y; /**< Y axis, Unit:m */ float y; /**< Y axis, Unit:m */
float z; /**< Z axis, Unit:m */ float z; /**< Z axis, Unit:m */
} PointXyz; } PointXyz;
typedef struct{ typedef struct {
float x; /**< X axis, Unit:m */ float x; /**< X axis, Unit:m */
float y; /**< Y axis, Unit:m */ float y; /**< Y axis, Unit:m */
float z; /**< Z axis, Unit:m */ float z; /**< Z axis, Unit:m */
float reflectivity; /**< Reflectivity */ float reflectivity; /**< Reflectivity */
} LivoxPointXyzr; } LivoxPointXyzr;
typedef struct{ typedef struct {
float x; /**< X axis, Unit:m */ float x; /**< X axis, Unit:m */
float y; /**< Y axis, Unit:m */ float y; /**< Y axis, Unit:m */
float z; /**< Z axis, Unit:m */ float z; /**< Z axis, Unit:m */
@@ -210,67 +212,71 @@ typedef struct{
#pragma pack() #pragma pack()
typedef uint8_t *(*PointConvertHandler)(uint8_t *point_buf,
typedef uint8_t* (* PointConvertHandler)(uint8_t* point_buf, LivoxEthPacket* eth_packet, \ LivoxEthPacket *eth_packet,
ExtrinsicParameter& extrinsic); ExtrinsicParameter &extrinsic);
const PacketInfoPair packet_info_pair_table[kMaxPointDataType] = { const PacketInfoPair packet_info_pair_table[kMaxPointDataType] = {
{100, 100000, 10000 , 1318}, {100, 100000, 10000, 1000000, 1318}, {100, 100000, 10000, 1000000, 918},
{100, 100000, 10000 , 918}, {96, 240000, 4167, 400000, 1362}, {96, 240000, 4167, 400000, 978},
{96, 240000, 4167 , 1362}, {96, 480000, 4167, 400000, 1362}, {48, 480000, 4167, 400000, 978},
{96, 240000, 4167 , 978}, {1, 100, 10000000, 10000000, 42}};
{96, 480000, 4167 , 1362},
{48, 480000, 4167 , 978},
{1, 100, 10000000, 42}
};
/** /**
* Global function for general use. * Global function for general use.
*/ */
bool IsFilePathValid(const char* path_str); bool IsFilePathValid(const char *path_str);
uint64_t GetStoragePacketTimestamp(StoragePacket* packet, uint8_t data_src_); uint64_t GetStoragePacketTimestamp(StoragePacket *packet, uint8_t data_src_);
uint32_t CalculatePacketQueueSize(uint32_t interval_ms, uint32_t data_type); uint32_t CalculatePacketQueueSize(uint32_t interval_ms, uint32_t data_type);
void ParseCommandlineInputBdCode(const char* cammandline_str, void ParseCommandlineInputBdCode(const char *cammandline_str,
std::vector<std::string>& bd_code_list); std::vector<std::string> &bd_code_list);
PointConvertHandler GetConvertHandler(uint8_t data_type); PointConvertHandler GetConvertHandler(uint8_t data_type);
uint8_t* LivoxPointToPxyzrtl(uint8_t* point_buf, LivoxEthPacket* eth_packet, \ uint8_t *LivoxPointToPxyzrtl(uint8_t *point_buf, LivoxEthPacket *eth_packet,
ExtrinsicParameter& extrinsic); ExtrinsicParameter &extrinsic);
uint8_t* LivoxImuDataProcess(uint8_t* point_buf, LivoxEthPacket* eth_packet); 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 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) { inline uint32_t GetPointInterval(uint32_t data_type) {
return packet_info_pair_table[data_type].point_interval; return packet_info_pair_table[data_type].point_interval;
} }
inline uint32_t GetPacketNumPerSec(uint32_t data_type) { inline uint32_t GetPacketNumPerSec(uint32_t data_type) {
return packet_info_pair_table[data_type].points_per_second / 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) { inline uint32_t GetPointsPerPacket(uint32_t data_type) {
return packet_info_pair_table[data_type].points_per_packet; 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) { inline uint32_t GetEthPacketLen(uint32_t data_type) {
return packet_info_pair_table[data_type].packet_length; 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->x = raw_point->x;
dst_point->y = raw_point->y; dst_point->y = raw_point->y;
dst_point->z = raw_point->z; dst_point->z = raw_point->z;
dst_point->reflectivity = (float)raw_point->reflectivity; dst_point->reflectivity = (float)raw_point->reflectivity;
} }
inline void RawPointConvert(LivoxPointXyzr* dst_point, LivoxRawPoint* raw_point) { inline void RawPointConvert(LivoxPointXyzr *dst_point,
dst_point->x = raw_point->x/1000.0f; LivoxRawPoint *raw_point) {
dst_point->y = raw_point->y/1000.0f; dst_point->x = raw_point->x / 1000.0f;
dst_point->z = raw_point->z/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; 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 radius = raw_point->depth / 1000.0;
double theta = raw_point->theta / 100.0 / 180 * PI; double theta = raw_point->theta / 100.0 / 180 * PI;
double phi = raw_point->phi / 100.0 / 180 * PI; double phi = raw_point->phi / 100.0 / 180 * PI;
@@ -280,9 +286,9 @@ inline void RawPointConvert(LivoxPointXyzr* dst_point, LivoxSpherPoint* raw_poin
dst_point->reflectivity = (float)raw_point->reflectivity; dst_point->reflectivity = (float)raw_point->reflectivity;
} }
inline void RawPointConvert(LivoxPointXyzr *dst_point1,
inline void RawPointConvert(LivoxPointXyzr* dst_point1, LivoxPointXyzr* dst_point2, \ LivoxPointXyzr *dst_point2,
LivoxDualExtendSpherPoint* raw_point) { LivoxDualExtendSpherPoint *raw_point) {
double radius1 = raw_point->depth1 / 1000.0; double radius1 = raw_point->depth1 / 1000.0;
double radius2 = raw_point->depth2 / 1000.0; double radius2 = raw_point->depth2 / 1000.0;
double theta = raw_point->theta / 100.0 / 180 * PI; double theta = raw_point->theta / 100.0 / 180 * PI;
@@ -307,8 +313,8 @@ public:
virtual ~Lds(); virtual ~Lds();
uint8_t GetDeviceType(uint8_t handle); uint8_t GetDeviceType(uint8_t handle);
static void ResetLidar(LidarDevice* lidar, uint8_t data_src); static void ResetLidar(LidarDevice *lidar, uint8_t data_src);
static void SetLidarDataSrc(LidarDevice* lidar, uint8_t data_src); static void SetLidarDataSrc(LidarDevice *lidar, uint8_t data_src);
void ResetLds(uint8_t data_src); void ResetLds(uint8_t data_src);
void RequestExit() { request_exit_ = true; } void RequestExit() { request_exit_ = true; }
void CleanRequestExit() { request_exit_ = false; } void CleanRequestExit() { request_exit_ = false; }
@@ -321,9 +327,10 @@ public:
protected: protected:
uint32_t buffer_time_ms_; /**< Buffer time before data in queue is read */ uint32_t buffer_time_ms_; /**< Buffer time before data in queue is read */
uint8_t data_src_; uint8_t data_src_;
private: private:
volatile bool request_exit_; volatile bool request_exit_;
}; };
} } // namespace livox_ros
#endif #endif

303
livox_ros_driver/livox_ros_driver/lds_hub.cpp
View File

@@ -24,26 +24,29 @@
#include "lds_hub.h" #include "lds_hub.h"
#include <memory>
#include <stdio.h> #include <stdio.h>
#include <string.h> #include <string.h>
#include <thread> #include <thread>
#include <memory>
#include "rapidjson/document.h" #include "rapidjson/document.h"
#include "rapidjson/stringbuffer.h"
#include "rapidjson/filereadstream.h" #include "rapidjson/filereadstream.h"
#include "rapidjson/stringbuffer.h"
namespace livox_ros { namespace livox_ros {
/** Const varible ------------------------------------------------------------------------------- */ /** Const varible
* -------------------------------------------------------------------------------
*/
/** For callback use only */ /** 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) { LdsHub::LdsHub(uint32_t interval_ms) : Lds(interval_ms, kSourceRawHub) {
auto_connect_mode_ = true; auto_connect_mode_ = true;
whitelist_count_ = 0; whitelist_count_ = 0;
@@ -55,15 +58,14 @@ LdsHub::LdsHub(uint32_t interval_ms) : Lds(interval_ms, kSourceRawHub) {
ResetLdsHub(); ResetLdsHub();
} }
LdsHub::~LdsHub() { LdsHub::~LdsHub() {}
}
void LdsHub::ResetLdsHub(void) { void LdsHub::ResetLdsHub(void) {
ResetLds(kSourceRawHub); ResetLds(kSourceRawHub);
ResetLidar(&hub_, kSourceRawHub); ResetLidar(&hub_, kSourceRawHub);
} }
int LdsHub::InitLdsHub(std::vector<std::string>& broadcast_code_strs,\ int LdsHub::InitLdsHub(std::vector<std::string> &broadcast_code_strs,
const char *user_config_path) { const char *user_config_path) {
if (is_initialized_) { if (is_initialized_) {
@@ -79,7 +81,8 @@ int LdsHub::InitLdsHub(std::vector<std::string>& broadcast_code_strs,\
LivoxSdkVersion _sdkversion; LivoxSdkVersion _sdkversion;
GetLivoxSdkVersion(&_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); SetBroadcastCallback(LdsHub::OnDeviceBroadcast);
SetDeviceStateUpdateCallback(LdsHub::OnDeviceChange); SetDeviceStateUpdateCallback(LdsHub::OnDeviceChange);
@@ -96,12 +99,13 @@ int LdsHub::InitLdsHub(std::vector<std::string>& broadcast_code_strs,\
printf("Disable auto connect mode!\n"); printf("Disable auto connect mode!\n");
printf("List all broadcast code in whiltelist:\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]); printf("%s\n", broadcast_code_whitelist_[i]);
} }
} else { } else {
EnableAutoConnectMode(); 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 */ /** 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) { if (g_lds_hub == nullptr) {
g_lds_hub = this; g_lds_hub = this;
} }
is_initialized_= true; is_initialized_ = true;
printf("Livox-SDK init success!\n"); printf("Livox-SDK init success!\n");
return 0; return 0;
@@ -134,15 +138,13 @@ int LdsHub::DeInitLdsHub(void) {
return 0; return 0;
} }
void LdsHub::PrepareExit(void) { void LdsHub::PrepareExit(void) { DeInitLdsHub(); }
DeInitLdsHub();
}
/** Static function in LdsLidar for callback */ /** Static function in LdsLidar for callback */
void LdsHub::OnHubDataCb(uint8_t hub_handle, LivoxEthPacket *data, void LdsHub::OnHubDataCb(uint8_t hub_handle, LivoxEthPacket *data,
uint32_t data_num, void *client_data) { uint32_t data_num, void *client_data) {
LdsHub* lds_hub = static_cast<LdsHub *>(client_data); LdsHub *lds_hub = static_cast<LdsHub *>(client_data);
LivoxEthPacket* eth_packet = data; LivoxEthPacket *eth_packet = data;
if (!data || !data_num) { if (!data || !data_num) {
return; return;
@@ -153,15 +155,16 @@ void LdsHub::OnHubDataCb(uint8_t hub_handle, LivoxEthPacket *data,
return; return;
} }
LidarDevice* p_lidar = &lds_hub->lidars_[handle]; LidarDevice *p_lidar = &lds_hub->lidars_[handle];
LidarPacketStatistic *packet_statistic = &p_lidar->statistic_info; LidarPacketStatistic *packet_statistic = &p_lidar->statistic_info;
LdsStamp cur_timestamp; 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 (kImu != eth_packet->data_type) {
if (eth_packet->timestamp_type == kTimestampTypePps) { if (eth_packet->timestamp_type == kTimestampTypePps) {
/** Whether a new sync frame */ /** 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)) { (cur_timestamp.stamp < kPacketTimeGap)) {
auto cur_time = std::chrono::high_resolution_clock::now(); auto cur_time = std::chrono::high_resolution_clock::now();
int64_t sync_time = cur_time.time_since_epoch().count(); 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; LidarDataQueue *p_queue = &p_lidar->data;
if (nullptr == p_queue->storage_packet) { 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); eth_packet->data_type);
queue_size = queue_size * 16; /* 16 multiple the min size */ queue_size = queue_size * 16; /* 16 multiple the min size */
InitQueue(p_queue, queue_size); InitQueue(p_queue, queue_size);
printf("Lidar%02d[%s] queue size : %d %d\n", p_lidar->handle, p_lidar->info.broadcast_code, \ printf("Lidar%02d[%s] queue size : %d %d\n", p_lidar->handle,
queue_size, p_queue->size); p_lidar->info.broadcast_code, queue_size, p_queue->size);
} }
if (!QueueIsFull(p_queue)) { if (!QueueIsFull(p_queue)) {
QueuePushAny(p_queue, (uint8_t *)eth_packet,\ QueuePushAny(p_queue, (uint8_t *)eth_packet,
GetEthPacketLen(eth_packet->data_type), packet_statistic->timebase, \ GetEthPacketLen(eth_packet->data_type),
packet_statistic->timebase,
GetPointsPerPacket(eth_packet->data_type)); GetPointsPerPacket(eth_packet->data_type));
} }
} else { } else {
if (eth_packet->timestamp_type == kTimestampTypePps) { if (eth_packet->timestamp_type == kTimestampTypePps) {
/** Whether a new sync frame */ /** 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)) { (cur_timestamp.stamp < kPacketTimeGap)) {
auto cur_time = std::chrono::high_resolution_clock::now(); auto cur_time = std::chrono::high_resolution_clock::now();
int64_t sync_time = cur_time.time_since_epoch().count(); int64_t sync_time = cur_time.time_since_epoch().count();
@@ -202,13 +206,14 @@ void LdsHub::OnHubDataCb(uint8_t hub_handle, LivoxEthPacket *data,
if (nullptr == p_queue->storage_packet) { if (nullptr == p_queue->storage_packet) {
uint32_t queue_size = 256; uint32_t queue_size = 256;
InitQueue(p_queue, queue_size); 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); p_lidar->info.broadcast_code, queue_size, p_queue->size);
} }
if (!QueueIsFull(p_queue) && (cur_timestamp.stamp >= 0)) { if (!QueueIsFull(p_queue) && (cur_timestamp.stamp >= 0)) {
QueuePushAny(p_queue, (uint8_t *)eth_packet,\ QueuePushAny(p_queue, (uint8_t *)eth_packet,
GetEthPacketLen(eth_packet->data_type), packet_statistic->imu_timebase, \ GetEthPacketLen(eth_packet->data_type),
packet_statistic->imu_timebase,
GetPointsPerPacket(eth_packet->data_type)); GetPointsPerPacket(eth_packet->data_type));
} }
} }
@@ -225,16 +230,17 @@ void LdsHub::OnDeviceBroadcast(const BroadcastDeviceInfo *info) {
} }
if (g_lds_hub->IsAutoConnectMode()) { 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 { } else {
if (!g_lds_hub->IsBroadcastCodeExistInWhitelist(info->broadcast_code)) { 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); info->broadcast_code);
return; return;
} }
} }
LidarDevice* p_hub = &g_lds_hub->hub_; LidarDevice *p_hub = &g_lds_hub->hub_;
if (p_hub->connect_state == kConnectStateOff) { if (p_hub->connect_state == kConnectStateOff) {
bool result = false; bool result = false;
uint8_t handle = 0; uint8_t handle = 0;
@@ -247,7 +253,8 @@ void LdsHub::OnDeviceBroadcast(const BroadcastDeviceInfo *info) {
printf("add to connect\n"); printf("add to connect\n");
UserRawConfig config; UserRawConfig config;
if (strncmp(info->broadcast_code, g_lds_hub->hub_raw_config_.broadcast_code, \ if (strncmp(info->broadcast_code,
g_lds_hub->hub_raw_config_.broadcast_code,
sizeof(info->broadcast_code)) != 0) { sizeof(info->broadcast_code)) != 0) {
printf("Could not find hub raw config, set config to default!\n"); printf("Could not find hub raw config, set config to default!\n");
config.enable_fan = 1; config.enable_fan = 1;
@@ -278,7 +285,7 @@ void LdsHub::OnDeviceChange(const DeviceInfo *info, DeviceEvent type) {
return; return;
} }
LidarDevice* p_hub = &g_lds_hub->hub_; LidarDevice *p_hub = &g_lds_hub->hub_;
if (type == kEventHubConnectionChange) { if (type == kEventHubConnectionChange) {
if (p_hub->connect_state == kConnectStateOff) { if (p_hub->connect_state == kConnectStateOff) {
p_hub->connect_state = kConnectStateOn; p_hub->connect_state = kConnectStateOn;
@@ -294,10 +301,9 @@ void LdsHub::OnDeviceChange(const DeviceInfo *info, DeviceEvent type) {
} }
if (p_hub->connect_state == kConnectStateOn) { if (p_hub->connect_state == kConnectStateOn) {
printf("Hub[%s] status_code[%d] working state[%d] feature[%d]\n", \ printf("Hub[%s] status_code[%d] working state[%d] feature[%d]\n",
p_hub->info.broadcast_code,\ p_hub->info.broadcast_code,
p_hub->info.status.status_code.error_code,\ p_hub->info.status.status_code.error_code, p_hub->info.state,
p_hub->info.state,\
p_hub->info.feature); p_hub->info.feature);
SetErrorMessageCallback(p_hub->handle, LdsHub::HubErrorStatusCb); SetErrorMessageCallback(p_hub->handle, LdsHub::HubErrorStatusCb);
if (p_hub->info.state == kLidarStateNormal) { if (p_hub->info.state == kLidarStateNormal) {
@@ -306,10 +312,10 @@ void LdsHub::OnDeviceChange(const DeviceInfo *info, DeviceEvent type) {
} }
} }
void LdsHub::HubQueryLidarInfoCb(livox_status status, uint8_t handle, \ void LdsHub::HubQueryLidarInfoCb(livox_status status, uint8_t handle,
HubQueryLidarInformationResponse *response,\ HubQueryLidarInformationResponse *response,
void *client_data) { void *client_data) {
LdsHub* lds_hub = static_cast<LdsHub *>(client_data); LdsHub *lds_hub = static_cast<LdsHub *>(client_data);
if ((handle >= kMaxLidarCount) || !response) { if ((handle >= kMaxLidarCount) || !response) {
return; return;
} }
@@ -318,25 +324,24 @@ void LdsHub::HubQueryLidarInfoCb(livox_status status, uint8_t handle, \
if (response->count) { if (response->count) {
printf("Hub have %d lidars:\n", response->count); printf("Hub have %d lidars:\n", response->count);
for (int i = 0; i < response->count; i++) { 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); response->device_info_list[i].id);
if (index < kMaxLidarCount) { if (index < kMaxLidarCount) {
LidarDevice* p_lidar = &lds_hub->lidars_[index]; LidarDevice *p_lidar = &lds_hub->lidars_[index];
p_lidar->handle = index; p_lidar->handle = index;
p_lidar->info.handle = index; p_lidar->info.handle = index;
p_lidar->info.slot = response->device_info_list[i].slot; 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->info.type = response->device_info_list[i].dev_type;
p_lidar->connect_state = kConnectStateSampling; p_lidar->connect_state = kConnectStateSampling;
strncpy(p_lidar->info.broadcast_code, \ strncpy(p_lidar->info.broadcast_code,
response->device_info_list[i].broadcast_code, \ response->device_info_list[i].broadcast_code,
sizeof(p_lidar->info.broadcast_code)); sizeof(p_lidar->info.broadcast_code));
printf("[%d]%s DeviceType[%d] Slot[%d] Ver[%d.%d.%d.%d]\n", index, \ printf("[%d]%s DeviceType[%d] Slot[%d] Ver[%d.%d.%d.%d]\n", index,
p_lidar->info.broadcast_code,\ p_lidar->info.broadcast_code, p_lidar->info.type,
p_lidar->info.type, p_lidar->info.slot,\ p_lidar->info.slot, response->device_info_list[i].version[0],
response->device_info_list[i].version[0],\ response->device_info_list[i].version[1],
response->device_info_list[i].version[1],\ response->device_info_list[i].version[2],
response->device_info_list[i].version[2],\
response->device_info_list[i].version[3]); 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. */ /** 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; static uint32_t error_message_count = 0;
if (message != NULL) { if (message != NULL) {
++error_message_count; ++error_message_count;
@@ -361,23 +367,21 @@ void LdsHub::HubErrorStatusCb(livox_status status, uint8_t handle, ErrorMessage
printf("sync_status : %u\n", message->hub_error_code.sync_status); printf("sync_status : %u\n", message->hub_error_code.sync_status);
printf("temp_status : %u\n", message->hub_error_code.temp_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_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("firmware_err : %u\n", message->hub_error_code.firmware_err);
printf("system_status : %u\n", message->hub_error_code.system_status); printf("system_status : %u\n", message->hub_error_code.system_status);
} }
} }
} }
void LdsHub::ControlFanCb(livox_status status, uint8_t handle, \ void LdsHub::ControlFanCb(livox_status status, uint8_t handle, uint8_t response,
uint8_t response, void *clent_data) { void *clent_data) {}
void LdsHub::HubSetPointCloudReturnModeCb(
} livox_status status, uint8_t handle,
HubSetPointCloudReturnModeResponse *response, void *clent_data) {
void LdsHub::HubSetPointCloudReturnModeCb(livox_status status, uint8_t handle, \ LdsHub *lds_hub = static_cast<LdsHub *>(clent_data);
HubSetPointCloudReturnModeResponse* response,\
void *clent_data) {
LdsHub* lds_hub = static_cast<LdsHub *>(clent_data);
if ((handle >= kMaxLidarCount) || !response) { if ((handle >= kMaxLidarCount) || !response) {
return; 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) { 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) { if (handle >= kMaxLidarCount) {
return; return;
} }
LidarDevice* p_hub = &(lds_hub->hub_); LidarDevice *p_hub = &(lds_hub->hub_);
if (status == kStatusSuccess) { if (status == kStatusSuccess) {
p_hub->config.set_bits &= ~((uint32_t)(kConfigCoordinate)); p_hub->config.set_bits &= ~((uint32_t)(kConfigCoordinate));
printf("Set coordinate success!\n"); 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, \ void LdsHub::HubSetImuRatePushFrequencyCb(
HubSetImuPushFrequencyResponse* response,\ livox_status status, uint8_t handle,
void *clent_data) { HubSetImuPushFrequencyResponse *response, void *clent_data) {
LdsHub* lds_hub = static_cast<LdsHub *>(clent_data); LdsHub *lds_hub = static_cast<LdsHub *>(clent_data);
if ((handle >= kMaxLidarCount) || !response) { if ((handle >= kMaxLidarCount) || !response) {
return; return;
} }
@@ -444,21 +448,21 @@ void LdsHub::HubSetImuRatePushFrequencyCb(livox_status status, uint8_t handle, \
} }
/** Callback function of starting sampling. */ /** 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) { 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) { if (handle >= kMaxLidarCount) {
return; return;
} }
LidarDevice* p_hub = &lds_hub->hub_; LidarDevice *p_hub = &lds_hub->hub_;
if ((status != kStatusSuccess) || (response != 0)) { if ((status != kStatusSuccess) || (response != 0)) {
p_hub->connect_state = kConnectStateOn; p_hub->connect_state = kConnectStateOn;
printf("Hub start sample fail : state[%d] handle[%d] res[%d]\n", \ printf("Hub start sample fail : state[%d] handle[%d] res[%d]\n", status,
status, handle, response); handle, response);
for (int i = 0; i < kMaxLidarCount; i++) { 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) { if (p_lidar->connect_state == kConnectStateSampling) {
p_lidar->connect_state = kConnectStateOn; p_lidar->connect_state = kConnectStateOn;
} }
@@ -469,18 +473,18 @@ void LdsHub::StartSampleCb(livox_status status, uint8_t handle, \
} }
/** Callback function of stopping sampling. */ /** Callback function of stopping sampling. */
void LdsHub::StopSampleCb(livox_status status, uint8_t handle, \ void LdsHub::StopSampleCb(livox_status status, uint8_t handle, uint8_t response,
uint8_t response, void *clent_data) { void *clent_data) {}
}
void LdsHub::ConfigPointCloudReturnMode(LdsHub* lds_hub) { void LdsHub::ConfigPointCloudReturnMode(LdsHub *lds_hub) {
uint8_t req_buf[1024]; uint8_t req_buf[1024];
HubSetPointCloudReturnModeRequest* req = (HubSetPointCloudReturnModeRequest *)req_buf; HubSetPointCloudReturnModeRequest *req =
(HubSetPointCloudReturnModeRequest *)req_buf;
req->count = 0; req->count = 0;
for (int i = 0; i < kMaxLidarCount; i++) { for (int i = 0; i < kMaxLidarCount; i++) {
LidarDevice* p_lidar = &(lds_hub->lidars_[i]); LidarDevice *p_lidar = &(lds_hub->lidars_[i]);
if ((p_lidar->info.type != kDeviceTypeLidarMid40) && \ if ((p_lidar->info.type != kDeviceTypeLidarMid40) &&
(p_lidar->connect_state == kConnectStateSampling)) { (p_lidar->connect_state == kConnectStateSampling)) {
UserRawConfig config; UserRawConfig config;
if (lds_hub->GetRawConfig(p_lidar->info.broadcast_code, config)) { if (lds_hub->GetRawConfig(p_lidar->info.broadcast_code, config)) {
@@ -490,8 +494,8 @@ void LdsHub::ConfigPointCloudReturnMode(LdsHub* lds_hub) {
config.coordinate = kCoordinateCartesian; config.coordinate = kCoordinateCartesian;
config.imu_rate = kImuFreq200Hz; config.imu_rate = kImuFreq200Hz;
} }
strncpy(req->lidar_cfg_list[req->count].broadcast_code, \ strncpy(req->lidar_cfg_list[req->count].broadcast_code,
p_lidar->info.broadcast_code, \ p_lidar->info.broadcast_code,
sizeof(req->lidar_cfg_list[req->count].broadcast_code)); sizeof(req->lidar_cfg_list[req->count].broadcast_code));
req->lidar_cfg_list[req->count].mode = config.return_mode; req->lidar_cfg_list[req->count].mode = config.return_mode;
req->count++; req->count++;
@@ -499,21 +503,22 @@ void LdsHub::ConfigPointCloudReturnMode(LdsHub* lds_hub) {
} }
if (req->count) { if (req->count) {
uint32_t length = 1 + sizeof(SetPointCloudReturnModeRequestItem) * req->count; uint32_t length =
HubSetPointCloudReturnMode(req, length,\ 1 + sizeof(SetPointCloudReturnModeRequestItem) * req->count;
HubSetPointCloudReturnMode(req, length,
LdsHub::HubSetPointCloudReturnModeCb, lds_hub); LdsHub::HubSetPointCloudReturnModeCb, lds_hub);
lds_hub->hub_.config.set_bits |= kConfigReturnMode; lds_hub->hub_.config.set_bits |= kConfigReturnMode;
} }
} }
void LdsHub::ConfigImuPushFrequency(LdsHub* lds_hub) { void LdsHub::ConfigImuPushFrequency(LdsHub *lds_hub) {
uint8_t req_buf[1024]; uint8_t req_buf[1024];
HubSetImuPushFrequencyRequest* req = (HubSetImuPushFrequencyRequest *)req_buf; HubSetImuPushFrequencyRequest *req = (HubSetImuPushFrequencyRequest *)req_buf;
req->count = 0; req->count = 0;
for (int i = 0; i < kMaxLidarCount; i++) { for (int i = 0; i < kMaxLidarCount; i++) {
LidarDevice* p_lidar = &(lds_hub->lidars_[i]); LidarDevice *p_lidar = &(lds_hub->lidars_[i]);
if ((p_lidar->info.type != kDeviceTypeLidarMid40) && \ if ((p_lidar->info.type != kDeviceTypeLidarMid40) &&
(p_lidar->connect_state == kConnectStateSampling)) { (p_lidar->connect_state == kConnectStateSampling)) {
UserRawConfig config; UserRawConfig config;
if (lds_hub->GetRawConfig(p_lidar->info.broadcast_code, config)) { if (lds_hub->GetRawConfig(p_lidar->info.broadcast_code, config)) {
@@ -523,8 +528,8 @@ void LdsHub::ConfigImuPushFrequency(LdsHub* lds_hub) {
config.coordinate = kCoordinateCartesian; config.coordinate = kCoordinateCartesian;
config.imu_rate = kImuFreq200Hz; config.imu_rate = kImuFreq200Hz;
} }
strncpy(req->lidar_cfg_list[req->count].broadcast_code, \ strncpy(req->lidar_cfg_list[req->count].broadcast_code,
p_lidar->info.broadcast_code, \ p_lidar->info.broadcast_code,
sizeof(req->lidar_cfg_list[req->count].broadcast_code)); sizeof(req->lidar_cfg_list[req->count].broadcast_code));
req->lidar_cfg_list[req->count].freq = config.imu_rate; req->lidar_cfg_list[req->count].freq = config.imu_rate;
req->count++; req->count++;
@@ -533,22 +538,24 @@ void LdsHub::ConfigImuPushFrequency(LdsHub* lds_hub) {
if (req->count) { if (req->count) {
uint32_t length = 1 + sizeof(SetImuPushFrequencyRequestItem) * req->count; uint32_t length = 1 + sizeof(SetImuPushFrequencyRequestItem) * req->count;
HubSetImuPushFrequency(req, length,\ HubSetImuPushFrequency(req, length, LdsHub::HubSetImuRatePushFrequencyCb,
LdsHub::HubSetImuRatePushFrequencyCb, lds_hub); lds_hub);
lds_hub->hub_.config.set_bits |= kConfigImuRate; lds_hub->hub_.config.set_bits |= kConfigImuRate;
} }
} }
void LdsHub::ConfigLidarsOfHub(LdsHub* lds_hub) { void LdsHub::ConfigLidarsOfHub(LdsHub *lds_hub) {
ConfigPointCloudReturnMode(lds_hub); ConfigPointCloudReturnMode(lds_hub);
ConfigImuPushFrequency(lds_hub); ConfigImuPushFrequency(lds_hub);
if (lds_hub->hub_.config.coordinate != 0) { 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"); printf("Hub set coordinate spherical\n");
} else { } else {
printf("Hub set coordinate cartesian\n"); 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; lds_hub->hub_.config.set_bits |= kConfigCoordinate;
@@ -556,8 +563,8 @@ void LdsHub::ConfigLidarsOfHub(LdsHub* lds_hub) {
} }
/** Add broadcast code to whitelist */ /** Add broadcast code to whitelist */
int LdsHub::AddBroadcastCodeToWhitelist(const char* broadcast_code) { int LdsHub::AddBroadcastCodeToWhitelist(const char *broadcast_code) {
if (!broadcast_code || (strlen(broadcast_code) > kBroadcastCodeSize) || \ if (!broadcast_code || (strlen(broadcast_code) > kBroadcastCodeSize) ||
(whitelist_count_ >= kMaxLidarCount)) { (whitelist_count_ >= kMaxLidarCount)) {
return -1; return -1;
} }
@@ -573,13 +580,14 @@ int LdsHub::AddBroadcastCodeToWhitelist(const char* broadcast_code) {
return 0; return 0;
} }
bool LdsHub::IsBroadcastCodeExistInWhitelist(const char* broadcast_code) { bool LdsHub::IsBroadcastCodeExistInWhitelist(const char *broadcast_code) {
if (!broadcast_code) { if (!broadcast_code) {
return false; return false;
} }
for (uint32_t i=0; i<whitelist_count_; i++) { for (uint32_t i = 0; i < whitelist_count_; i++) {
if (strncmp(broadcast_code, broadcast_code_whitelist_[i], kBroadcastCodeSize) == 0) { if (strncmp(broadcast_code, broadcast_code_whitelist_[i],
kBroadcastCodeSize) == 0) {
return true; return true;
} }
} }
@@ -589,7 +597,8 @@ bool LdsHub::IsBroadcastCodeExistInWhitelist(const char* broadcast_code) {
/** Get and update LiDAR info */ /** Get and update LiDAR info */
void LdsHub::UpdateHubLidarinfo(void) { void LdsHub::UpdateHubLidarinfo(void) {
DeviceInfo *_lidars = (DeviceInfo *) malloc(sizeof(DeviceInfo) * kMaxLidarCount); DeviceInfo *_lidars =
(DeviceInfo *)malloc(sizeof(DeviceInfo) * kMaxLidarCount);
uint8_t count = kMaxLidarCount; uint8_t count = kMaxLidarCount;
uint8_t status = GetConnectedDevices(_lidars, &count); uint8_t status = GetConnectedDevices(_lidars, &count);
@@ -612,48 +621,51 @@ void LdsHub::UpdateHubLidarinfo(void) {
} }
/** Config file process */ /** Config file process */
int LdsHub::ParseConfigFile(const char* pathname) { int LdsHub::ParseConfigFile(const char *pathname) {
FILE* raw_file = std::fopen(pathname, "rb"); FILE *raw_file = std::fopen(pathname, "rb");
if(!raw_file) { if (!raw_file) {
printf("Open json config file fail!\n"); printf("Open json config file fail!\n");
return -1; return -1;
} }
char read_buffer[32768]; 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; rapidjson::Document doc;
if(!doc.ParseStream(config_file).HasParseError()) { if (!doc.ParseStream(config_file).HasParseError()) {
if(doc.HasMember("hub_config") && doc["hub_config"].IsObject()) { if (doc.HasMember("hub_config") && doc["hub_config"].IsObject()) {
const rapidjson::Value& object = doc["hub_config"]; const rapidjson::Value &object = doc["hub_config"];
UserRawConfig hub_config; UserRawConfig hub_config;
memset(&hub_config, 0, sizeof(hub_config)); memset(&hub_config, 0, sizeof(hub_config));
if(object.HasMember("broadcast_code") && \ if (object.HasMember("broadcast_code") &&
object["broadcast_code"].IsString()) { object["broadcast_code"].IsString()) {
std::string broadcast_code = object["broadcast_code"].GetString(); std::string broadcast_code = object["broadcast_code"].GetString();
std::memcpy(hub_config.broadcast_code, broadcast_code.c_str(),\ std::memcpy(hub_config.broadcast_code, broadcast_code.c_str(),
sizeof(hub_config.broadcast_code)); sizeof(hub_config.broadcast_code));
if(object.HasMember("enable_connect") && \ if (object.HasMember("enable_connect") &&
object["enable_connect"].IsBool()) { object["enable_connect"].IsBool()) {
hub_config.enable_connect = object["enable_connect"].GetBool(); 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(); hub_config.coordinate = object["coordinate"].GetInt();
} }
printf("Hub[%s] : %d %d %d %d %d\n", hub_config.broadcast_code, \ 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.enable_connect, hub_config.enable_fan,
hub_config.coordinate, hub_config.imu_rate); hub_config.return_mode, hub_config.coordinate,
hub_config.imu_rate);
if (hub_config.enable_connect) { if (hub_config.enable_connect) {
if (!AddBroadcastCodeToWhitelist(hub_config.broadcast_code)) { if (!AddBroadcastCodeToWhitelist(hub_config.broadcast_code)) {
hub_raw_config_ = hub_config; hub_raw_config_ = hub_config;
printf("Add hub[%s] [%d] to whitelist\n", hub_raw_config_.broadcast_code, \ printf("Add hub[%s] [%d] to whitelist\n",
hub_raw_config_.coordinate); hub_raw_config_.broadcast_code, hub_raw_config_.coordinate);
} else { } else {
memset(&hub_raw_config_, 0, sizeof(hub_raw_config_)); 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 { } else {
@@ -661,32 +673,32 @@ int LdsHub::ParseConfigFile(const char* pathname) {
} }
} }
if(doc.HasMember("lidar_config") && doc["lidar_config"].IsArray()) { if (doc.HasMember("lidar_config") && doc["lidar_config"].IsArray()) {
const rapidjson::Value& array = doc["lidar_config"]; const rapidjson::Value &array = doc["lidar_config"];
size_t len = array.Size(); size_t len = array.Size();
for(size_t i=0; i<len; i++) { for (size_t i = 0; i < len; i++) {
const rapidjson::Value& object = array[i]; const rapidjson::Value &object = array[i];
if(object.IsObject()) { if (object.IsObject()) {
UserRawConfig config; UserRawConfig config;
memset(&config, 0, sizeof(config)); memset(&config, 0, sizeof(config));
if(object.HasMember("broadcast_code") && \ if (object.HasMember("broadcast_code") &&
object["broadcast_code"].IsString()) { object["broadcast_code"].IsString()) {
std::string broadcast_code = object["broadcast_code"].GetString(); std::string broadcast_code = object["broadcast_code"].GetString();
std::memcpy(config.broadcast_code, broadcast_code.c_str(),\ std::memcpy(config.broadcast_code, broadcast_code.c_str(),
sizeof(config.broadcast_code)); sizeof(config.broadcast_code));
} else { } else {
printf("User config file parse error\n"); printf("User config file parse error\n");
continue; continue;
} }
if(object.HasMember("enable_fan") && \ if (object.HasMember("enable_fan") && object["enable_fan"].IsBool()) {
object["enable_fan"].IsBool()) {
config.enable_fan = object["enable_fan"].GetBool(); 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(); 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(); config.imu_rate = object["imu_rate"].GetInt();
} }
if (hub_raw_config_.enable_connect) { if (hub_raw_config_.enable_connect) {
@@ -694,18 +706,20 @@ int LdsHub::ParseConfigFile(const char* pathname) {
} else { } else {
config.coordinate = 0; config.coordinate = 0;
} }
printf("Lidar[%s] : %d %d %d %d %d\n", config.broadcast_code, \ printf("Lidar[%s] : %d %d %d %d %d\n", config.broadcast_code,
config.enable_connect, config.enable_fan, config.return_mode,\ config.enable_connect, config.enable_fan, config.return_mode,
config.coordinate, config.imu_rate); config.coordinate, config.imu_rate);
if (AddRawUserConfig(config)) { 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 { } 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); std::fclose(raw_file);
@@ -713,7 +727,7 @@ int LdsHub::ParseConfigFile(const char* pathname) {
return 0; return 0;
} }
int LdsHub::AddRawUserConfig(UserRawConfig& config) { int LdsHub::AddRawUserConfig(UserRawConfig &config) {
if (IsExistInRawConfig(config.broadcast_code)) { if (IsExistInRawConfig(config.broadcast_code)) {
return -1; return -1;
} }
@@ -724,13 +738,14 @@ int LdsHub::AddRawUserConfig(UserRawConfig& config) {
return 0; return 0;
} }
bool LdsHub::IsExistInRawConfig(const char* broadcast_code) { bool LdsHub::IsExistInRawConfig(const char *broadcast_code) {
if (broadcast_code == nullptr) { if (broadcast_code == nullptr) {
return false; return false;
} }
for (auto ite_config : lidar_raw_config_) { 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; return true;
} }
} }
@@ -738,14 +753,14 @@ bool LdsHub::IsExistInRawConfig(const char* broadcast_code) {
return false; return false;
} }
int LdsHub::GetRawConfig(const char *broadcast_code, UserRawConfig &config) {
int LdsHub::GetRawConfig(const char* broadcast_code, UserRawConfig& config) {
if (broadcast_code == nullptr) { if (broadcast_code == nullptr) {
return -1; return -1;
} }
for (auto ite_config : lidar_raw_config_) { for (auto ite_config : lidar_raw_config_) {
if (strncmp(ite_config.broadcast_code, broadcast_code, kBroadcastCodeSize) == 0) { if (strncmp(ite_config.broadcast_code, broadcast_code,
kBroadcastCodeSize) == 0) {
config.enable_fan = ite_config.enable_fan; config.enable_fan = ite_config.enable_fan;
config.return_mode = ite_config.return_mode; config.return_mode = ite_config.return_mode;
config.coordinate = ite_config.coordinate; config.coordinate = ite_config.coordinate;
@@ -757,4 +772,4 @@ int LdsHub::GetRawConfig(const char* broadcast_code, UserRawConfig& config) {
return -1; return -1;
} }
} } // namespace livox_ros

67
livox_ros_driver/livox_ros_driver/lds_hub.h
View File

@@ -39,60 +39,67 @@ namespace livox_ros {
* LiDAR data source, data from hub. * LiDAR data source, data from hub.
*/ */
class LdsHub : public Lds { class LdsHub : public Lds {
public: public:
static LdsHub* GetInstance(uint32_t interval_ms) { static LdsHub *GetInstance(uint32_t interval_ms) {
static LdsHub lds_hub(interval_ms); static LdsHub lds_hub(interval_ms);
return &lds_hub; 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); int DeInitLdsHub(void);
private: private:
LdsHub(uint32_t interval_ms); LdsHub(uint32_t interval_ms);
LdsHub(const LdsHub&) = delete; LdsHub(const LdsHub &) = delete;
~LdsHub(); ~LdsHub();
LdsHub& operator=(const LdsHub&) = delete; LdsHub &operator=(const LdsHub &) = delete;
virtual void PrepareExit(void); virtual void PrepareExit(void);
static void OnHubDataCb(uint8_t hub_handle, LivoxEthPacket *data,\ static void OnHubDataCb(uint8_t hub_handle, LivoxEthPacket *data,
uint32_t data_num, void *client_data); uint32_t data_num, void *client_data);
static void OnDeviceBroadcast(const BroadcastDeviceInfo *info); static void OnDeviceBroadcast(const BroadcastDeviceInfo *info);
static void OnDeviceChange(const DeviceInfo *info, DeviceEvent type); 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 StartSampleCb(livox_status status, uint8_t handle,
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, \
uint8_t response, void *clent_data); uint8_t response, void *clent_data);
static void HubSetPointCloudReturnModeCb(livox_status status, uint8_t handle, \ static void StopSampleCb(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); uint8_t response, void *clent_data);
static void HubSetImuRatePushFrequencyCb(livox_status status, uint8_t handle, \ static void HubQueryLidarInfoCb(livox_status status, uint8_t handle,
HubSetImuPushFrequencyResponse* response,\ 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); void *clent_data);
static void HubErrorStatusCb(livox_status status, uint8_t handle, ErrorMessage *message); static void SetCoordinateCb(livox_status status, uint8_t handle,
static void ConfigPointCloudReturnMode(LdsHub* lds_hub); uint8_t response, void *clent_data);
static void ConfigImuPushFrequency(LdsHub* lds_hub); static void
static void ConfigLidarsOfHub(LdsHub* lds_hub); 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 ResetLdsHub(void);
void StateReset(void); void StateReset(void);
int AddBroadcastCodeToWhitelist(const char* broadcast_code); int AddBroadcastCodeToWhitelist(const char *broadcast_code);
bool IsBroadcastCodeExistInWhitelist(const char* broadcast_code); bool IsBroadcastCodeExistInWhitelist(const char *broadcast_code);
void UpdateHubLidarinfo(void); void UpdateHubLidarinfo(void);
void EnableAutoConnectMode(void) { auto_connect_mode_ = true; } void EnableAutoConnectMode(void) { auto_connect_mode_ = true; }
void DisableAutoConnectMode(void) { auto_connect_mode_ = false; } void DisableAutoConnectMode(void) { auto_connect_mode_ = false; }
bool IsAutoConnectMode(void) { return auto_connect_mode_; } bool IsAutoConnectMode(void) { return auto_connect_mode_; }
int ParseConfigFile(const char* pathname); int ParseConfigFile(const char *pathname);
int AddRawUserConfig(UserRawConfig& config); int AddRawUserConfig(UserRawConfig &config);
bool IsExistInRawConfig(const char* broadcast_code); bool IsExistInRawConfig(const char *broadcast_code);
int GetRawConfig(const char* broadcast_code, UserRawConfig& config); int GetRawConfig(const char *broadcast_code, UserRawConfig &config);
bool IsAllLidarSetBitsClear() { bool IsAllLidarSetBitsClear() {
for(int i=0; i<kMaxLidarCount; i++) { for (int i = 0; i < kMaxLidarCount; i++) {
if (lidars_[i].config.set_bits) { if (lidars_[i].config.set_bits) {
return false; return false;
} }
@@ -111,5 +118,5 @@ class LdsHub : public Lds {
UserRawConfig hub_raw_config_; UserRawConfig hub_raw_config_;
}; };
} } // namespace livox_ros
#endif #endif

337
livox_ros_driver/livox_ros_driver/lds_lidar.cpp
View File

@@ -24,25 +24,28 @@
#include "lds_lidar.h" #include "lds_lidar.h"
#include <memory>
#include <stdio.h> #include <stdio.h>
#include <string.h> #include <string.h>
#include <thread> #include <thread>
#include <memory>
#include "rapidjson/document.h" #include "rapidjson/document.h"
#include "rapidjson/stringbuffer.h"
#include "rapidjson/filereadstream.h" #include "rapidjson/filereadstream.h"
#include "rapidjson/stringbuffer.h"
namespace livox_ros { namespace livox_ros {
/** Const varible ------------------------------------------------------------------------------- */ /** Const varible
* -------------------------------------------------------------------------------
*/
/** For callback use only */ /** 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) { LdsLidar::LdsLidar(uint32_t interval_ms) : Lds(interval_ms, kSourceRawLidar) {
auto_connect_mode_ = true; auto_connect_mode_ = true;
is_initialized_ = false; is_initialized_ = false;
@@ -53,15 +56,11 @@ LdsLidar::LdsLidar(uint32_t interval_ms) : Lds(interval_ms, kSourceRawLidar) {
ResetLdsLidar(); ResetLdsLidar();
} }
LdsLidar::~LdsLidar() { LdsLidar::~LdsLidar() {}
} void LdsLidar::ResetLdsLidar(void) { ResetLds(kSourceRawLidar); }
void LdsLidar::ResetLdsLidar(void) { int LdsLidar::InitLdsLidar(std::vector<std::string> &broadcast_code_strs,
ResetLds(kSourceRawLidar);
}
int LdsLidar::InitLdsLidar(std::vector<std::string>& broadcast_code_strs,\
const char *user_config_path) { const char *user_config_path) {
if (is_initialized_) { if (is_initialized_) {
printf("LiDAR data source is already inited!\n"); printf("LiDAR data source is already inited!\n");
@@ -76,7 +75,8 @@ int LdsLidar::InitLdsLidar(std::vector<std::string>& broadcast_code_strs,\
LivoxSdkVersion _sdkversion; LivoxSdkVersion _sdkversion;
GetLivoxSdkVersion(&_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); SetBroadcastCallback(OnDeviceBroadcast);
SetDeviceStateUpdateCallback(OnDeviceChange); SetDeviceStateUpdateCallback(OnDeviceChange);
@@ -93,12 +93,13 @@ int LdsLidar::InitLdsLidar(std::vector<std::string>& broadcast_code_strs,\
printf("Disable auto connect mode!\n"); printf("Disable auto connect mode!\n");
printf("List all broadcast code in whiltelist:\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]); printf("%s\n", broadcast_code_whitelist_[i]);
} }
} else { } else {
EnableAutoConnectMode(); 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_) { if (enable_timesync_) {
@@ -127,7 +128,7 @@ int LdsLidar::InitLdsLidar(std::vector<std::string>& broadcast_code_strs,\
if (g_lds_ldiar == nullptr) { if (g_lds_ldiar == nullptr) {
g_lds_ldiar = this; g_lds_ldiar = this;
} }
is_initialized_= true; is_initialized_ = true;
printf("Livox-SDK init success!\n"); printf("Livox-SDK init success!\n");
return 0; return 0;
@@ -150,33 +151,32 @@ int LdsLidar::DeInitLdsLidar(void) {
return 0; return 0;
} }
void LdsLidar::PrepareExit(void) { void LdsLidar::PrepareExit(void) { DeInitLdsLidar(); }
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. */ /** Receiving point cloud data from Livox LiDAR. */
void LdsLidar::OnLidarDataCb(uint8_t handle, LivoxEthPacket *data, void LdsLidar::OnLidarDataCb(uint8_t handle, LivoxEthPacket *data,
uint32_t data_num, void *client_data) { uint32_t data_num, void *client_data) {
using namespace std; using namespace std;
LdsLidar* lds_lidar = static_cast<LdsLidar *>(client_data); LdsLidar *lds_lidar = static_cast<LdsLidar *>(client_data);
LivoxEthPacket* eth_packet = data; LivoxEthPacket *eth_packet = data;
if (!data || !data_num || (handle >= kMaxLidarCount)) { if (!data || !data_num || (handle >= kMaxLidarCount)) {
return; return;
} }
LidarDevice* p_lidar = &lds_lidar->lidars_[handle]; LidarDevice *p_lidar = &lds_lidar->lidars_[handle];
LidarPacketStatistic *packet_statistic = &p_lidar->statistic_info; LidarPacketStatistic *packet_statistic = &p_lidar->statistic_info;
LdsStamp cur_timestamp; 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 (kImu != eth_packet->data_type) {
if (eth_packet->timestamp_type == kTimestampTypePps) { 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 (cur_timestamp.stamp < kPacketTimeGap)) { // whether a new sync frame
auto cur_time = std::chrono::high_resolution_clock::now(); auto cur_time = std::chrono::high_resolution_clock::now();
@@ -189,28 +189,30 @@ void LdsLidar::OnLidarDataCb(uint8_t handle, LivoxEthPacket *data,
LidarDataQueue *p_queue = &p_lidar->data; LidarDataQueue *p_queue = &p_lidar->data;
if (nullptr == p_queue->storage_packet) { 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); eth_packet->data_type);
queue_size = queue_size * 16; /* 16 multiple the min size */ queue_size = queue_size * 16; /* 16 multiple the min size */
InitQueue(p_queue, queue_size); InitQueue(p_queue, queue_size);
printf("Lidar%02d[%s] queue size : %d %d\n", p_lidar->handle, p_lidar->info.broadcast_code, \ printf("Lidar%02d[%s] queue size : %d %d\n", p_lidar->handle,
queue_size, p_queue->size); p_lidar->info.broadcast_code, queue_size, p_queue->size);
} }
if (!QueueIsFull(p_queue)) { if (!QueueIsFull(p_queue)) {
QueuePushAny(p_queue, (uint8_t *)eth_packet,\ QueuePushAny(p_queue, (uint8_t *)eth_packet,
GetEthPacketLen(eth_packet->data_type), packet_statistic->timebase, \ GetEthPacketLen(eth_packet->data_type),
packet_statistic->timebase,
GetPointsPerPacket(eth_packet->data_type)); GetPointsPerPacket(eth_packet->data_type));
} }
} else { } else {
if (eth_packet->timestamp_type == kTimestampTypePps) { 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 (cur_timestamp.stamp < kPacketTimeGap)) { // whether a new sync frame
auto cur_time = std::chrono::high_resolution_clock::now(); auto cur_time = std::chrono::high_resolution_clock::now();
int64_t sync_time = cur_time.time_since_epoch().count(); 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; 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) { if (nullptr == p_queue->storage_packet) {
uint32_t queue_size = 256; uint32_t queue_size = 256;
InitQueue(p_queue, queue_size); 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); p_lidar->info.broadcast_code, queue_size, p_queue->size);
} }
if (!QueueIsFull(p_queue) && (cur_timestamp.stamp >= 0)) { if (!QueueIsFull(p_queue) && (cur_timestamp.stamp >= 0)) {
QueuePushAny(p_queue, (uint8_t *)eth_packet,\ QueuePushAny(p_queue, (uint8_t *)eth_packet,
GetEthPacketLen(eth_packet->data_type), packet_statistic->timebase, \ GetEthPacketLen(eth_packet->data_type),
packet_statistic->timebase,
GetPointsPerPacket(eth_packet->data_type)); GetPointsPerPacket(eth_packet->data_type));
} }
} }
@@ -237,15 +240,17 @@ void LdsLidar::OnDeviceBroadcast(const BroadcastDeviceInfo *info) {
} }
if (info->dev_type == kDeviceTypeHub) { 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; return;
} }
if (g_lds_ldiar->IsAutoConnectMode()) { 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 { } else {
if (!g_lds_ldiar->IsBroadcastCodeExistInWhitelist(info->broadcast_code)) { 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); info->broadcast_code);
return; return;
} }
@@ -257,7 +262,7 @@ void LdsLidar::OnDeviceBroadcast(const BroadcastDeviceInfo *info) {
if (result == kStatusSuccess && handle < kMaxLidarCount) { if (result == kStatusSuccess && handle < kMaxLidarCount) {
SetDataCallback(handle, OnLidarDataCb, (void *)g_lds_ldiar); 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->handle = handle;
p_lidar->connect_state = kConnectStateOff; p_lidar->connect_state = kConnectStateOff;
@@ -275,7 +280,8 @@ void LdsLidar::OnDeviceBroadcast(const BroadcastDeviceInfo *info) {
p_lidar->config.return_mode = config.return_mode; 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.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 { } else {
printf("Add lidar to connect is failed : %d %d \n", result, handle); 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; return;
} }
LidarDevice* p_lidar = &(g_lds_ldiar->lidars_[handle]); LidarDevice *p_lidar = &(g_lds_ldiar->lidars_[handle]);
if (type == kEventConnect) { if (type == kEventConnect) {
QueryDeviceInformation(handle, DeviceInformationCb, g_lds_ldiar); QueryDeviceInformation(handle, DeviceInformationCb, g_lds_ldiar);
if (p_lidar->connect_state == kConnectStateOff) { if (p_lidar->connect_state == kConnectStateOff) {
@@ -307,10 +313,9 @@ void LdsLidar::OnDeviceChange(const DeviceInfo *info, DeviceEvent type) {
} }
if (p_lidar->connect_state == kConnectStateOn) { if (p_lidar->connect_state == kConnectStateOn) {
printf("Lidar[%s] status_code[%d] working state[%d] feature[%d]\n", \ printf("Lidar[%s] status_code[%d] working state[%d] feature[%d]\n",
p_lidar->info.broadcast_code,\ p_lidar->info.broadcast_code,
p_lidar->info.status.status_code.error_code,\ p_lidar->info.status.status_code.error_code, p_lidar->info.state,
p_lidar->info.state,\
p_lidar->info.feature); p_lidar->info.feature);
SetErrorMessageCallback(handle, LidarErrorStatusCb); SetErrorMessageCallback(handle, LidarErrorStatusCb);
@@ -324,19 +329,22 @@ void LdsLidar::OnDeviceChange(const DeviceInfo *info, DeviceEvent type) {
p_lidar->config.set_bits |= kConfigCoordinate; p_lidar->config.set_bits |= kConfigCoordinate;
if (kDeviceTypeLidarMid40 != info->type) { if (kDeviceTypeLidarMid40 != info->type) {
LidarSetPointCloudReturnMode(handle, (PointCloudReturnMode)(p_lidar->config.return_mode),\ LidarSetPointCloudReturnMode(
handle, (PointCloudReturnMode)(p_lidar->config.return_mode),
SetPointCloudReturnModeCb, g_lds_ldiar); SetPointCloudReturnModeCb, g_lds_ldiar);
p_lidar->config.set_bits |= kConfigReturnMode; p_lidar->config.set_bits |= kConfigReturnMode;
} }
if (kDeviceTypeLidarMid40 != info->type) { if (kDeviceTypeLidarMid40 != info->type) {
LidarSetImuPushFrequency(handle, (ImuFreq)(p_lidar->config.imu_rate),\ LidarSetImuPushFrequency(handle, (ImuFreq)(p_lidar->config.imu_rate),
SetImuRatePushFrequencyCb, g_lds_ldiar); SetImuRatePushFrequencyCb, g_lds_ldiar);
p_lidar->config.set_bits |= kConfigImuRate; p_lidar->config.set_bits |= kConfigImuRate;
} }
if (p_lidar->config.extrinsic_parameter_source == kExtrinsicParameterFromLidar) { if (p_lidar->config.extrinsic_parameter_source ==
LidarGetExtrinsicParameter(handle, GetLidarExtrinsicParameterCb, g_lds_ldiar); kExtrinsicParameterFromLidar) {
LidarGetExtrinsicParameter(handle, GetLidarExtrinsicParameterCb,
g_lds_ldiar);
} }
p_lidar->connect_state = kConnectStateConfig; p_lidar->connect_state = kConnectStateConfig;
@@ -345,22 +353,22 @@ void LdsLidar::OnDeviceChange(const DeviceInfo *info, DeviceEvent type) {
} }
/** Query the firmware version of Livox LiDAR. */ /** Query the firmware version of Livox LiDAR. */
void LdsLidar::DeviceInformationCb(livox_status status, uint8_t handle, \ void LdsLidar::DeviceInformationCb(livox_status status, uint8_t handle,
DeviceInformationResponse *ack, void *clent_data) { DeviceInformationResponse *ack,
void *clent_data) {
if (status != kStatusSuccess) { if (status != kStatusSuccess) {
printf("Device Query Informations Failed : %d\n", status); printf("Device Query Informations Failed : %d\n", status);
} }
if (ack) { if (ack) {
printf("firmware version: %d.%d.%d.%d\n", printf("firmware version: %d.%d.%d.%d\n", ack->firmware_version[0],
ack->firmware_version[0], ack->firmware_version[1], ack->firmware_version[2],
ack->firmware_version[1],
ack->firmware_version[2],
ack->firmware_version[3]); ack->firmware_version[3]);
} }
} }
/** Callback function of Lidar error message. */ /** 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; static uint32_t error_message_count = 0;
if (message != NULL) { if (message != NULL) {
++error_message_count; ++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("fan_status : %u\n", message->lidar_error_code.fan_status);
printf("self_heating : %u\n", message->lidar_error_code.self_heating); printf("self_heating : %u\n", message->lidar_error_code.self_heating);
printf("ptp_status : %u\n", message->lidar_error_code.ptp_status); 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); printf("system_status : %u\n", message->lidar_error_code.system_status);
} }
} }
} }
void LdsLidar::ControlFanCb(livox_status status, uint8_t handle, \ void LdsLidar::ControlFanCb(livox_status status, uint8_t handle,
uint8_t response, void *clent_data) { 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) { 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) { if (handle >= kMaxLidarCount) {
return; return;
} }
LidarDevice* p_lidar = &(lds_lidar->lidars_[handle]); LidarDevice *p_lidar = &(lds_lidar->lidars_[handle]);
if (status == kStatusSuccess) { if (status == kStatusSuccess) {
p_lidar->config.set_bits &= ~((uint32_t)(kConfigReturnMode)); p_lidar->config.set_bits &= ~((uint32_t)(kConfigReturnMode));
@@ -405,20 +412,21 @@ void LdsLidar::SetPointCloudReturnModeCb(livox_status status, uint8_t handle, \
} }
} else { } else {
LidarSetPointCloudReturnMode(handle, (PointCloudReturnMode)(p_lidar->config.return_mode),\ LidarSetPointCloudReturnMode(
handle, (PointCloudReturnMode)(p_lidar->config.return_mode),
SetPointCloudReturnModeCb, lds_lidar); SetPointCloudReturnModeCb, lds_lidar);
printf("Set return mode fail, try again!\n"); 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) { 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) { if (handle >= kMaxLidarCount) {
return; return;
} }
LidarDevice* p_lidar = &(lds_lidar->lidars_[handle]); LidarDevice *p_lidar = &(lds_lidar->lidars_[handle]);
if (status == kStatusSuccess) { if (status == kStatusSuccess) {
p_lidar->config.set_bits &= ~((uint32_t)(kConfigCoordinate)); p_lidar->config.set_bits &= ~((uint32_t)(kConfigCoordinate));
@@ -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) { 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) { if (handle >= kMaxLidarCount) {
return; return;
} }
LidarDevice* p_lidar = &(lds_lidar->lidars_[handle]); LidarDevice *p_lidar = &(lds_lidar->lidars_[handle]);
if (status == kStatusSuccess) { if (status == kStatusSuccess) {
p_lidar->config.set_bits &= ~((uint32_t)(kConfigImuRate)); p_lidar->config.set_bits &= ~((uint32_t)(kConfigImuRate));
@@ -458,28 +466,28 @@ void LdsLidar::SetImuRatePushFrequencyCb(livox_status status, uint8_t handle, \
} }
} else { } else {
LidarSetImuPushFrequency(handle, (ImuFreq)(p_lidar->config.imu_rate),\ LidarSetImuPushFrequency(handle, (ImuFreq)(p_lidar->config.imu_rate),
SetImuRatePushFrequencyCb, g_lds_ldiar); SetImuRatePushFrequencyCb, g_lds_ldiar);
printf("Set imu rate fail, try again!\n"); printf("Set imu rate fail, try again!\n");
} }
} }
/** Callback function of get LiDARs' extrinsic parameter. */ /** 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) { LidarGetExtrinsicParameterResponse *response, void *clent_data) {
LdsLidar* lds_lidar = static_cast<LdsLidar *>(clent_data); LdsLidar *lds_lidar = static_cast<LdsLidar *>(clent_data);
if (handle >= kMaxLidarCount) { if (handle >= kMaxLidarCount) {
return; return;
} }
if (status == kStatusSuccess) { if (status == kStatusSuccess) {
if (response != nullptr) { 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); handle, status, response->ret_code);
LidarDevice* p_lidar = &(lds_lidar->lidars_[handle]); LidarDevice *p_lidar = &(lds_lidar->lidars_[handle]);
ExtrinsicParameter* p_extrinsic = &p_lidar->extrinsic_parameter; ExtrinsicParameter *p_extrinsic = &p_lidar->extrinsic_parameter;
p_extrinsic->euler[0] = static_cast<float>(response->roll* PI / 180.0); 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[1] = static_cast<float>(response->pitch * PI / 180.0);
p_extrinsic->euler[2] = static_cast<float>(response->yaw * 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); p_extrinsic->trans[0] = static_cast<float>(response->x / 1000.0);
@@ -504,65 +512,65 @@ void LdsLidar::GetLidarExtrinsicParameterCb(livox_status status, uint8_t handle,
} }
} }
/** Callback function of starting sampling. */ /** 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) { 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) { if (handle >= kMaxLidarCount) {
return; return;
} }
LidarDevice* p_lidar = &(lds_lidar->lidars_[handle]); LidarDevice *p_lidar = &(lds_lidar->lidars_[handle]);
if (status == kStatusSuccess) { if (status == kStatusSuccess) {
if (response != 0) { if (response != 0) {
p_lidar->connect_state = kConnectStateOn; p_lidar->connect_state = kConnectStateOn;
printf("Lidar start sample fail : state[%d] handle[%d] res[%d]\n", \ printf("Lidar start sample fail : state[%d] handle[%d] res[%d]\n", status,
status, handle, response); handle, response);
} else { } else {
printf("Lidar start sample success\n"); printf("Lidar start sample success\n");
} }
} else if (status == kStatusTimeout) { } else if (status == kStatusTimeout) {
p_lidar->connect_state = kConnectStateOn; 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); status, handle, response);
} }
} }
/** Callback function of stopping sampling. */ /** Callback function of stopping sampling. */
void LdsLidar::StopSampleCb(livox_status status, uint8_t handle, \ void LdsLidar::StopSampleCb(livox_status status, uint8_t handle,
uint8_t response, void *clent_data) { uint8_t response, void *clent_data) {}
}
void LdsLidar::SetRmcSyncTimeCb(livox_status status, uint8_t handle, \ void LdsLidar::SetRmcSyncTimeCb(livox_status status, uint8_t handle,
uint8_t response, void* client_data) { uint8_t response, void *client_data) {
if (handle >= kMaxLidarCount) { if (handle >= kMaxLidarCount) {
return; return;
} }
printf("Set lidar[%d] sync time status[%d] response[%d]\n", handle, status, response); printf("Set lidar[%d] sync time status[%d] response[%d]\n", handle, status,
response);
} }
void LdsLidar::ReceiveSyncTimeCallback(const char* rmc, uint32_t rmc_length, void* client_data) { void LdsLidar::ReceiveSyncTimeCallback(const char *rmc, uint32_t rmc_length,
LdsLidar* lds_lidar = static_cast<LdsLidar *>(client_data); void *client_data) {
//std::unique_lock<std::mutex> lock(mtx); LdsLidar *lds_lidar = static_cast<LdsLidar *>(client_data);
LidarDevice* p_lidar = nullptr; // std::unique_lock<std::mutex> lock(mtx);
LidarDevice *p_lidar = nullptr;
for (uint8_t handle = 0; handle < kMaxLidarCount; handle++) { for (uint8_t handle = 0; handle < kMaxLidarCount; handle++) {
p_lidar = &(lds_lidar->lidars_[handle]); p_lidar = &(lds_lidar->lidars_[handle]);
if (p_lidar->connect_state == kConnectStateSampling && \ if (p_lidar->connect_state == kConnectStateSampling &&
p_lidar->info.state == kLidarStateNormal) { p_lidar->info.state == kLidarStateNormal) {
livox_status status = LidarSetRmcSyncTime(handle, rmc, rmc_length, \ livox_status status = LidarSetRmcSyncTime(handle, rmc, rmc_length,
SetRmcSyncTimeCb, lds_lidar); SetRmcSyncTimeCb, lds_lidar);
if (status != kStatusSuccess) { 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 */ /** Add broadcast code to whitelist */
int LdsLidar::AddBroadcastCodeToWhitelist(const char* broadcast_code) { int LdsLidar::AddBroadcastCodeToWhitelist(const char *broadcast_code) {
if (!broadcast_code || (strlen(broadcast_code) > kBroadcastCodeSize) || \ if (!broadcast_code || (strlen(broadcast_code) > kBroadcastCodeSize) ||
(whitelist_count_ >= kMaxLidarCount)) { (whitelist_count_ >= kMaxLidarCount)) {
return -1; return -1;
} }
@@ -578,13 +586,14 @@ int LdsLidar::AddBroadcastCodeToWhitelist(const char* broadcast_code) {
return 0; return 0;
} }
bool LdsLidar::IsBroadcastCodeExistInWhitelist(const char* broadcast_code) { bool LdsLidar::IsBroadcastCodeExistInWhitelist(const char *broadcast_code) {
if (!broadcast_code) { if (!broadcast_code) {
return false; return false;
} }
for (uint32_t i=0; i<whitelist_count_; i++) { for (uint32_t i = 0; i < whitelist_count_; i++) {
if (strncmp(broadcast_code, broadcast_code_whitelist_[i], kBroadcastCodeSize) == 0) { if (strncmp(broadcast_code, broadcast_code_whitelist_[i],
kBroadcastCodeSize) == 0) {
return true; return true;
} }
} }
@@ -592,106 +601,125 @@ bool LdsLidar::IsBroadcastCodeExistInWhitelist(const char* broadcast_code) {
return false; return false;
} }
int LdsLidar::ParseTimesyncConfig(rapidjson::Document& doc) { int LdsLidar::ParseTimesyncConfig(rapidjson::Document &doc) {
do { 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"]; const rapidjson::Value &object = doc["timesync_config"];
if (!object.IsObject()) break; if (!object.IsObject())
break;
if(!object.HasMember("enable_timesync") || !object["enable_timesync"].IsBool()) break; if (!object.HasMember("enable_timesync") ||
!object["enable_timesync"].IsBool())
break;
enable_timesync_ = object["enable_timesync"].GetBool(); 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::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)); 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(); timesync_config_.dev_config.type = object["comm_device_type"].GetInt();
if (timesync_config_.dev_config.type == kCommDevUart) { if (timesync_config_.dev_config.type == kCommDevUart) {
if(!object.HasMember("baudrate_index") || !object["baudrate_index"].IsInt()) break; if (!object.HasMember("baudrate_index") ||
timesync_config_.dev_config.config.uart.baudrate = object["baudrate_index"].GetInt(); !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; if (!object.HasMember("parity_index") || !object["parity_index"].IsInt())
timesync_config_.dev_config.config.uart.parity = object["parity_index"].GetInt(); break;
timesync_config_.dev_config.config.uart.parity =
object["parity_index"].GetInt();
} }
if (enable_timesync_) { if (enable_timesync_) {
printf("Enable timesync : \n"); printf("Enable timesync : \n");
if (timesync_config_.dev_config.type == kCommDevUart) { if (timesync_config_.dev_config.type == kCommDevUart) {
printf("Uart[%s],baudrate index[%d],parity index[%d]\n", timesync_config_.dev_config.name,\ printf("Uart[%s],baudrate index[%d],parity index[%d]\n",
timesync_config_.dev_config.config.uart.baudrate,\ timesync_config_.dev_config.name,
timesync_config_.dev_config.config.uart.baudrate,
timesync_config_.dev_config.config.uart.parity); timesync_config_.dev_config.config.uart.parity);
} }
} else { } else {
printf("Disable timesync\n"); printf("Disable timesync\n");
} }
return 0; return 0;
}while(0); } while (0);
return -1; return -1;
} }
/** Config file process */ /** Config file process */
int LdsLidar::ParseConfigFile(const char* pathname) { int LdsLidar::ParseConfigFile(const char *pathname) {
FILE* raw_file = std::fopen(pathname, "rb"); FILE *raw_file = std::fopen(pathname, "rb");
if(!raw_file) { if (!raw_file) {
printf("Open json config file fail!\n"); printf("Open json config file fail!\n");
return -1; return -1;
} }
char read_buffer[32768]; 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; rapidjson::Document doc;
if(!doc.ParseStream(config_file).HasParseError()) { if (!doc.ParseStream(config_file).HasParseError()) {
if(doc.HasMember("lidar_config") && doc["lidar_config"].IsArray()) { if (doc.HasMember("lidar_config") && doc["lidar_config"].IsArray()) {
const rapidjson::Value& array = doc["lidar_config"]; const rapidjson::Value &array = doc["lidar_config"];
size_t len = array.Size(); size_t len = array.Size();
for(size_t i=0; i<len; i++) { for (size_t i = 0; i < len; i++) {
const rapidjson::Value& object = array[i]; const rapidjson::Value &object = array[i];
if(object.IsObject()) { if (object.IsObject()) {
UserRawConfig config; UserRawConfig config;
memset(&config, 0, sizeof(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::string broadcast_code = object["broadcast_code"].GetString();
std::strncpy(config.broadcast_code, broadcast_code.c_str(),\ std::strncpy(config.broadcast_code, broadcast_code.c_str(),
sizeof(config.broadcast_code)); sizeof(config.broadcast_code));
} else { } else {
printf("User config file parse error\n"); printf("User config file parse error\n");
continue; 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(); 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(); 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(); 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(); 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(); config.imu_rate = object["imu_rate"].GetInt();
} }
if(object.HasMember("extrinsic_parameter_source") && \ if (object.HasMember("extrinsic_parameter_source") &&
object["extrinsic_parameter_source"].IsInt()) { object["extrinsic_parameter_source"].IsInt()) {
config.extrinsic_parameter_source = object["extrinsic_parameter_source"].GetInt(); config.extrinsic_parameter_source =
object["extrinsic_parameter_source"].GetInt();
} }
printf("broadcast code[%s] : %d %d %d %d %d %d\n", config.broadcast_code, \ printf("broadcast code[%s] : %d %d %d %d %d %d\n",
config.enable_connect, config.enable_fan, config.return_mode,\ config.broadcast_code, config.enable_connect,
config.coordinate, config.imu_rate, config.extrinsic_parameter_source); config.enable_fan, config.return_mode, config.coordinate,
config.imu_rate, config.extrinsic_parameter_source);
if (config.enable_connect) { if (config.enable_connect) {
if (!AddBroadcastCodeToWhitelist(config.broadcast_code)) { if (!AddBroadcastCodeToWhitelist(config.broadcast_code)) {
if (AddRawUserConfig(config)) { 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; enable_timesync_ = false;
} }
} else { } 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); std::fclose(raw_file);
@@ -712,7 +741,7 @@ int LdsLidar::ParseConfigFile(const char* pathname) {
return 0; return 0;
} }
int LdsLidar::AddRawUserConfig(UserRawConfig& config) { int LdsLidar::AddRawUserConfig(UserRawConfig &config) {
if (IsExistInRawConfig(config.broadcast_code)) { if (IsExistInRawConfig(config.broadcast_code)) {
return -1; return -1;
} }
@@ -723,13 +752,14 @@ int LdsLidar::AddRawUserConfig(UserRawConfig& config) {
return 0; return 0;
} }
bool LdsLidar::IsExistInRawConfig(const char* broadcast_code) { bool LdsLidar::IsExistInRawConfig(const char *broadcast_code) {
if (broadcast_code == nullptr) { if (broadcast_code == nullptr) {
return false; return false;
} }
for (auto ite_config : raw_config_) { for (auto ite_config : raw_config_) {
if (strncmp(ite_config.broadcast_code, broadcast_code, kBroadcastCodeSize) == 0) { if (strncmp(ite_config.broadcast_code, broadcast_code,
kBroadcastCodeSize) == 0) {
return true; return true;
} }
} }
@@ -737,13 +767,14 @@ bool LdsLidar::IsExistInRawConfig(const char* broadcast_code) {
return false; return false;
} }
int LdsLidar::GetRawConfig(const char* broadcast_code, UserRawConfig& config) { int LdsLidar::GetRawConfig(const char *broadcast_code, UserRawConfig &config) {
if (broadcast_code == nullptr) { if (broadcast_code == nullptr) {
return -1; return -1;
} }
for (auto ite_config : raw_config_) { for (auto ite_config : raw_config_) {
if (strncmp(ite_config.broadcast_code, broadcast_code, kBroadcastCodeSize) == 0) { if (strncmp(ite_config.broadcast_code, broadcast_code,
kBroadcastCodeSize) == 0) {
config.enable_fan = ite_config.enable_fan; config.enable_fan = ite_config.enable_fan;
config.return_mode = ite_config.return_mode; config.return_mode = ite_config.return_mode;
config.coordinate = ite_config.coordinate; config.coordinate = ite_config.coordinate;
@@ -756,4 +787,4 @@ int LdsLidar::GetRawConfig(const char* broadcast_code, UserRawConfig& config) {
return -1; 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 "lds.h"
#include "livox_sdk.h" #include "livox_sdk.h"
#include "timesync.h"
#include "rapidjson/document.h" #include "rapidjson/document.h"
#include "timesync.h"
namespace livox_ros { namespace livox_ros {
@@ -42,57 +41,65 @@ namespace livox_ros {
* LiDAR data source, data from dependent lidar. * LiDAR data source, data from dependent lidar.
*/ */
class LdsLidar : public Lds { class LdsLidar : public Lds {
public: public:
static LdsLidar* GetInstance(uint32_t interval_ms) { static LdsLidar *GetInstance(uint32_t interval_ms) {
static LdsLidar lds_lidar(interval_ms); static LdsLidar lds_lidar(interval_ms);
return &lds_lidar; 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); int DeInitLdsLidar(void);
private: private:
LdsLidar(uint32_t interval_ms); LdsLidar(uint32_t interval_ms);
LdsLidar(const LdsLidar&) = delete; LdsLidar(const LdsLidar &) = delete;
~LdsLidar(); ~LdsLidar();
LdsLidar& operator=(const LdsLidar&) = delete; LdsLidar &operator=(const LdsLidar &) = delete;
virtual void PrepareExit(void); 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); uint32_t data_num, void *client_data);
static void OnDeviceBroadcast(const BroadcastDeviceInfo *info); static void OnDeviceBroadcast(const BroadcastDeviceInfo *info);
static void OnDeviceChange(const DeviceInfo *info, DeviceEvent type); 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 StartSampleCb(livox_status status, uint8_t handle,
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, \
uint8_t response, void *clent_data); uint8_t response, void *clent_data);
static void SetPointCloudReturnModeCb(livox_status status, uint8_t handle, \ static void StopSampleCb(livox_status status, uint8_t handle,
uint8_t response, void *clent_data); uint8_t response, void *clent_data);
static void SetCoordinateCb(livox_status status, uint8_t handle, \ static void DeviceInformationCb(livox_status status, uint8_t handle,
DeviceInformationResponse *ack,
void *clent_data);
static void LidarErrorStatusCb(livox_status status, uint8_t handle,
ErrorMessage *message);
static void ControlFanCb(livox_status status, uint8_t handle,
uint8_t response, void *clent_data); uint8_t response, void *clent_data);
static void SetImuRatePushFrequencyCb(livox_status status, uint8_t handle, \ static void SetPointCloudReturnModeCb(livox_status status, uint8_t handle,
uint8_t response, void *clent_data); uint8_t response, void *clent_data);
static void SetRmcSyncTimeCb(livox_status status, uint8_t handle, \ static void SetCoordinateCb(livox_status status, uint8_t handle,
uint8_t response, void* client_data); uint8_t response, void *clent_data);
static void ReceiveSyncTimeCallback(const char* rmc, uint32_t rmc_length, void* client_data); static void SetImuRatePushFrequencyCb(livox_status status, uint8_t handle,
static void GetLidarExtrinsicParameterCb(livox_status status, uint8_t handle, \ uint8_t response, void *clent_data);
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); void ResetLdsLidar(void);
int AddBroadcastCodeToWhitelist(const char* broadcast_code); int AddBroadcastCodeToWhitelist(const char *broadcast_code);
bool IsBroadcastCodeExistInWhitelist(const char* broadcast_code); bool IsBroadcastCodeExistInWhitelist(const char *broadcast_code);
void EnableAutoConnectMode(void) { auto_connect_mode_ = true; } void EnableAutoConnectMode(void) { auto_connect_mode_ = true; }
void DisableAutoConnectMode(void) { auto_connect_mode_ = false; } void DisableAutoConnectMode(void) { auto_connect_mode_ = false; }
bool IsAutoConnectMode(void) { return auto_connect_mode_; } bool IsAutoConnectMode(void) { return auto_connect_mode_; }
int ParseTimesyncConfig(rapidjson::Document& doc); int ParseTimesyncConfig(rapidjson::Document &doc);
int ParseConfigFile(const char* pathname); int ParseConfigFile(const char *pathname);
int AddRawUserConfig(UserRawConfig& config); int AddRawUserConfig(UserRawConfig &config);
bool IsExistInRawConfig(const char* broadcast_code); bool IsExistInRawConfig(const char *broadcast_code);
int GetRawConfig(const char* broadcast_code, UserRawConfig& config); int GetRawConfig(const char *broadcast_code, UserRawConfig &config);
bool auto_connect_mode_; bool auto_connect_mode_;
uint32_t whitelist_count_; uint32_t whitelist_count_;
@@ -105,5 +112,5 @@ class LdsLidar : public Lds {
TimeSyncConfig timesync_config_; TimeSyncConfig timesync_config_;
}; };
} } // namespace livox_ros
#endif #endif

99
livox_ros_driver/livox_ros_driver/lds_lvx.cpp
View File

@@ -24,31 +24,36 @@
#include "lds_lvx.h" #include "lds_lvx.h"
#include <functional>
#include <memory>
#include <stdio.h> #include <stdio.h>
#include <string.h> #include <string.h>
#include <thread> #include <thread>
#include <memory>
#include <functional>
#include "lvx_file.h" #include "lvx_file.h"
namespace livox_ros { namespace livox_ros {
/** Const varible -------------------------------------------------------------------------------- */ /** Const varible
* --------------------------------------------------------------------------------
*/
const uint32_t kMaxPacketsNumOfFrame = 8192; const uint32_t kMaxPacketsNumOfFrame = 8192;
/** For device connect use ---------------------------------------------------------------------- */ /** For device connect use
* ---------------------------------------------------------------------- */
LdsLvx::LdsLvx(uint32_t interval_ms) : Lds(interval_ms, kSourceLvxFile) { LdsLvx::LdsLvx(uint32_t interval_ms) : Lds(interval_ms, kSourceLvxFile) {
start_read_lvx_ = false; start_read_lvx_ = false;
is_initialized_ = false; is_initialized_ = false;
lvx_file_ = std::make_shared<LvxFileHandle>(); lvx_file_ = std::make_shared<LvxFileHandle>();
packets_of_frame_.buffer_capacity = kMaxPacketsNumOfFrame * sizeof(LvxFilePacket); packets_of_frame_.buffer_capacity =
packets_of_frame_.packet = new uint8_t[kMaxPacketsNumOfFrame * sizeof(LvxFilePacket)]; kMaxPacketsNumOfFrame * sizeof(LvxFilePacket);
packets_of_frame_.packet =
new uint8_t[kMaxPacketsNumOfFrame * sizeof(LvxFilePacket)];
} }
LdsLvx::~LdsLvx() { LdsLvx::~LdsLvx() {
if (packets_of_frame_.packet != nullptr) { 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"); 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_) { if (is_initialized_) {
printf("Livox file data source is already inited!\n"); printf("Livox file data source is already inited!\n");
return -1; return -1;
@@ -84,14 +88,14 @@ int LdsLvx::InitLdsLvx(const char* lvx_path) {
} }
printf("LvxFile[%s] have %d lidars\n", lvx_path, lidar_count_); 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; LvxFileDeviceInfo lvx_dev_info;
lvx_file_->GetDeviceInfo(i, &lvx_dev_info); lvx_file_->GetDeviceInfo(i, &lvx_dev_info);
lidars_[i].handle = i; lidars_[i].handle = i;
lidars_[i].connect_state = kConnectStateSampling; lidars_[i].connect_state = kConnectStateSampling;
lidars_[i].info.handle = i; lidars_[i].info.handle = i;
lidars_[i].info.type = lvx_dev_info.device_type; lidars_[i].info.type = lvx_dev_info.device_type;
memcpy(lidars_[i].info.broadcast_code, lvx_dev_info.lidar_broadcast_code,\ memcpy(lidars_[i].info.broadcast_code, lvx_dev_info.lidar_broadcast_code,
sizeof(lidars_[i].info.broadcast_code)); sizeof(lidars_[i].info.broadcast_code));
if (lvx_file_->GetFileVersion() == kLvxFileV1) { if (lvx_file_->GetFileVersion() == kLvxFileV1) {
@@ -100,8 +104,8 @@ int LdsLvx::InitLdsLvx(const char* lvx_path) {
lidars_[i].data_src = kSourceLvxFile; lidars_[i].data_src = kSourceLvxFile;
} }
ExtrinsicParameter* p_extrinsic = &lidars_[i].extrinsic_parameter; ExtrinsicParameter *p_extrinsic = &lidars_[i].extrinsic_parameter;
p_extrinsic->euler[0] = lvx_dev_info.roll* PI / 180.0; 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[1] = lvx_dev_info.pitch * PI / 180.0;
p_extrinsic->euler[2] = lvx_dev_info.yaw * PI / 180.0; p_extrinsic->euler[2] = lvx_dev_info.yaw * PI / 180.0;
p_extrinsic->trans[0] = lvx_dev_info.x; 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); 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; is_initialized_ = true;
StartRead(); StartRead();
@@ -126,7 +131,8 @@ int LdsLvx::InitLdsLvx(const char* lvx_path) {
/** Global function in LdsLvx for callback */ /** Global function in LdsLvx for callback */
void LdsLvx::ReadLvxFile() { void LdsLvx::ReadLvxFile() {
while (!start_read_lvx_); while (!start_read_lvx_)
;
printf("Start to read lvx file.\n"); printf("Start to read lvx file.\n");
int file_state = kLvxFileOk; int file_state = kLvxFileOk;
@@ -135,40 +141,45 @@ void LdsLvx::ReadLvxFile() {
file_state = lvx_file_->GetPacketsOfFrame(&packets_of_frame_); file_state = lvx_file_->GetPacketsOfFrame(&packets_of_frame_);
if (!file_state) { if (!file_state) {
uint32_t data_size = packets_of_frame_.data_size; uint32_t data_size = packets_of_frame_.data_size;
uint8_t* packet_base = packets_of_frame_.packet; uint8_t *packet_base = packets_of_frame_.packet;
uint32_t data_offset = 0; uint32_t data_offset = 0;
while (data_offset < data_size) { while (data_offset < data_size) {
LivoxEthPacket* eth_packet; LivoxEthPacket *eth_packet;
int32_t handle; int32_t handle;
uint8_t data_type; uint8_t data_type;
if (lvx_file_->GetFileVersion()) { if (lvx_file_->GetFileVersion()) {
LvxFilePacket* detail_packet = (LvxFilePacket*)&packet_base[data_offset]; LvxFilePacket *detail_packet =
eth_packet = (LivoxEthPacket*)(&detail_packet->version); (LvxFilePacket *)&packet_base[data_offset];
eth_packet = (LivoxEthPacket *)(&detail_packet->version);
handle = detail_packet->device_index; handle = detail_packet->device_index;
} else { } else {
LvxFilePacketV0* detail_packet = (LvxFilePacketV0*)&packet_base[data_offset]; LvxFilePacketV0 *detail_packet =
eth_packet = (LivoxEthPacket*)(&detail_packet->version); (LvxFilePacketV0 *)&packet_base[data_offset];
eth_packet = (LivoxEthPacket *)(&detail_packet->version);
handle = detail_packet->device_index; handle = detail_packet->device_index;
} }
data_type = eth_packet->data_type; data_type = eth_packet->data_type;
data_offset += (GetEthPacketLen(data_type) + 1); /* packet length + device index */ data_offset +=
(GetEthPacketLen(data_type) + 1); /* packet length + device index */
if (data_type != kImu) { if (data_type != kImu) {
LidarDataQueue* p_queue = &lidars_[handle].data; LidarDataQueue *p_queue = &lidars_[handle].data;
if ((p_queue != nullptr) && (handle < lidar_count_)) { if ((p_queue != nullptr) && (handle < lidar_count_)) {
while(QueueIsFull(p_queue)) { while (QueueIsFull(p_queue)) {
std::this_thread::sleep_for(std::chrono::milliseconds(1)); std::this_thread::sleep_for(std::chrono::milliseconds(1));
} }
QueuePushAny(p_queue, (uint8_t *)eth_packet, GetEthPacketLen(data_type), QueuePushAny(p_queue, (uint8_t *)eth_packet,
0, GetPointsPerPacket(data_type)); GetEthPacketLen(data_type), 0,
GetPointsPerPacket(data_type));
} }
} else { } else {
LidarDataQueue* p_queue = &lidars_[handle].imu_data; LidarDataQueue *p_queue = &lidars_[handle].imu_data;
if ((p_queue != nullptr) && (handle < lidar_count_)) { if ((p_queue != nullptr) && (handle < lidar_count_)) {
while(QueueIsFull(p_queue)) { while (QueueIsFull(p_queue)) {
std::this_thread::sleep_for(std::chrono::milliseconds(1)); std::this_thread::sleep_for(std::chrono::milliseconds(1));
} }
QueuePushAny(p_queue, (uint8_t *)eth_packet, GetEthPacketLen(data_type), QueuePushAny(p_queue, (uint8_t *)eth_packet,
0, GetPointsPerPacket(data_type)); GetEthPacketLen(data_type), 0,
GetPointsPerPacket(data_type));
} }
} }
} }
@@ -187,15 +198,22 @@ void LdsLvx::ReadLvxFile() {
} }
} }
while(IsAllQueueEmpty()) { int32_t wait_cnt = 10;
std::this_thread::sleep_for(std::chrono::milliseconds(1)); while (!IsAllQueueEmpty()) {
std::this_thread::sleep_for(std::chrono::milliseconds(50));
if (IsAllQueueReadStop()) {
--wait_cnt;
if (wait_cnt <= 0) {
break;
}
}
} }
RequestExit(); RequestExit();
} }
bool LdsLvx::IsAllQueueEmpty() { bool LdsLvx::IsAllQueueEmpty() {
for (int i=0; i<lidar_count_; i++) { for (int i = 0; i < lidar_count_; i++) {
LidarDevice* p_lidar = &lidars_[i]; LidarDevice *p_lidar = &lidars_[i];
if (!QueueIsEmpty(&p_lidar->data)) { if (!QueueIsEmpty(&p_lidar->data)) {
return false; return false;
} }
@@ -204,4 +222,17 @@ bool LdsLvx::IsAllQueueEmpty() {
return true; 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

15
livox_ros_driver/livox_ros_driver/lds_lvx.h
View File

@@ -39,21 +39,21 @@ namespace livox_ros {
* Lidar data source abstract. * Lidar data source abstract.
*/ */
class LdsLvx : public Lds { class LdsLvx : public Lds {
public: public:
static LdsLvx* GetInstance(uint32_t interval_ms) { static LdsLvx *GetInstance(uint32_t interval_ms) {
static LdsLvx lds_lvx(interval_ms); static LdsLvx lds_lvx(interval_ms);
return &lds_lvx; return &lds_lvx;
} }
int InitLdsLvx(const char* lvx_path); int InitLdsLvx(const char *lvx_path);
int DeInitLdsLvx(void); int DeInitLdsLvx(void);
void PrepareExit(void); void PrepareExit(void);
private: private:
LdsLvx(uint32_t interval_ms); LdsLvx(uint32_t interval_ms);
LdsLvx(const LdsLvx&) = delete; LdsLvx(const LdsLvx &) = delete;
~LdsLvx(); ~LdsLvx();
LdsLvx& operator=(const LdsLvx&) = delete; LdsLvx &operator=(const LdsLvx &) = delete;
void StartRead() { start_read_lvx_ = true; } void StartRead() { start_read_lvx_ = true; }
void StopRead() { start_read_lvx_ = false; } void StopRead() { start_read_lvx_ = false; }
@@ -61,6 +61,7 @@ class LdsLvx : public Lds {
void ReadLvxFile(); void ReadLvxFile();
bool IsAllQueueEmpty(); bool IsAllQueueEmpty();
bool IsAllQueueReadStop();
volatile bool is_initialized_; volatile bool is_initialized_;
OutPacketBuffer packets_of_frame_; OutPacketBuffer packets_of_frame_;
@@ -69,5 +70,5 @@ class LdsLvx : public Lds {
volatile bool start_read_lvx_; volatile bool start_read_lvx_;
}; };
} } // namespace livox_ros
#endif #endif

44
livox_ros_driver/livox_ros_driver/livox_ros_driver.cpp
View File

@@ -22,18 +22,18 @@
// SOFTWARE. // SOFTWARE.
// //
#include <vector> #include "include/livox_ros_driver.h"
#include <chrono>
#include <chrono>
#include <vector>
#include "lddc.h"
#include "lds_hub.h"
#include "lds_lidar.h"
#include "lds_lvx.h"
#include "livox_sdk.h" #include "livox_sdk.h"
#include <ros/ros.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; using namespace livox_ros;
const int32_t kSdkVersionMajorLimit = 2; 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_INFO("Livox Ros Driver Version: %s", LIVOX_ROS_DRIVER_VERSION_STRING);
/** Ros related */ /** 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::console::notifyLoggerLevelsChanged();
} }
ros::init(argc, argv, "livox_lidar_publisher"); ros::init(argc, argv, "livox_lidar_publisher");
@@ -53,8 +54,8 @@ int main(int argc, char **argv) {
LivoxSdkVersion _sdkversion; LivoxSdkVersion _sdkversion;
GetLivoxSdkVersion(&_sdkversion); GetLivoxSdkVersion(&_sdkversion);
if (_sdkversion.major < kSdkVersionMajorLimit) { if (_sdkversion.major < kSdkVersionMajorLimit) {
ROS_INFO("The SDK version[%d.%d.%d] is too low", \ ROS_INFO("The SDK version[%d.%d.%d] is too low", _sdkversion.major,
_sdkversion.major, _sdkversion.minor, _sdkversion.patch); _sdkversion.minor, _sdkversion.patch);
return 0; return 0;
} }
@@ -62,7 +63,7 @@ int main(int argc, char **argv) {
int xfer_format = kPointCloud2Msg; int xfer_format = kPointCloud2Msg;
int multi_topic = 0; int multi_topic = 0;
int data_src = kSourceRawLidar; int data_src = kSourceRawLidar;
double publish_freq = 50.0; /* Hz */ double publish_freq = 20.0; /* Hz */
int output_type = kOutputToRos; int output_type = kOutputToRos;
livox_node.getParam("xfer_format", xfer_format); 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); livox_node.getParam("output_data_type", output_type);
/** Lidar data distribute control and lidar data source set */ /** 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); lddc->SetRosNode(&livox_node);
int ret = 0; int ret = 0;
@@ -89,7 +91,7 @@ int main(int argc, char **argv) {
std::vector<std::string> bd_code_list; std::vector<std::string> bd_code_list;
ParseCommandlineInputBdCode(cmdline_bd_code.c_str(), 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)); lddc->RegisterLds(static_cast<Lds *>(read_lidar));
ret = read_lidar->InitLdsLidar(bd_code_list, user_config_path.c_str()); ret = read_lidar->InitLdsLidar(bd_code_list, user_config_path.c_str());
if (!ret) { if (!ret) {
@@ -110,7 +112,7 @@ int main(int argc, char **argv) {
std::vector<std::string> bd_code_list; std::vector<std::string> bd_code_list;
ParseCommandlineInputBdCode(cmdline_bd_code.c_str(), 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)); lddc->RegisterLds(static_cast<Lds *>(read_hub));
ret = read_hub->InitLdsHub(bd_code_list, user_config_path.c_str()); ret = read_hub->InitLdsHub(bd_code_list, user_config_path.c_str());
if (!ret) { 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 = cmdline_file_path.substr(0, path_end_pos);
rosbag_file_path += ".bag"; 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->RegisterLds(static_cast<Lds *>(read_lvx));
lddc->CreateBagFile(rosbag_file_path); lddc->CreateBagFile(rosbag_file_path);
int ret = read_lvx->InitLdsLvx(cmdline_file_path.c_str()); int ret = read_lvx->InitLdsLvx(cmdline_file_path.c_str());
@@ -144,11 +146,15 @@ int main(int argc, char **argv) {
} else { } else {
ROS_ERROR("Init lds lvx file fail!"); ROS_ERROR("Init lds lvx file fail!");
} }
} while(0); } while (0);
} }
ros::Time::init(); 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()) { while (ros::ok()) {
lddc->DistributeLidarData(); lddc->DistributeLidarData();
r.sleep(); r.sleep();
@@ -156,5 +162,3 @@ int main(int argc, char **argv) {
return 0; return 0;
} }

136
livox_ros_driver/livox_ros_driver/lvx_file.cpp
View File

@@ -23,23 +23,24 @@
// //
#include "lvx_file.h" #include "lvx_file.h"
#include <time.h>
#include <string.h>
#include <cmath> #include <cmath>
#include <string.h>
#include <time.h>
#include "lds.h"
#include "rapidxml/rapidxml.hpp" #include "rapidxml/rapidxml.hpp"
#include "rapidxml/rapidxml_utils.hpp" #include "rapidxml/rapidxml_utils.hpp"
#include "lds.h"
namespace livox_ros { namespace livox_ros {
#define M_PI 3.14159265358979323846 #define M_PI 3.14159265358979323846
const uint32_t kMaxLvxFileHeaderLength = 16 * 1024; const uint32_t kMaxLvxFileHeaderLength = 16 * 1024;
const char* kLvxHeaderSigStr = "livox_tech"; const char *kLvxHeaderSigStr = "livox_tech";
const uint32_t kLvxHeaderMagicCode = 0xac0ea767; const uint32_t kLvxHeaderMagicCode = 0xac0ea767;
LvxFileHandle::LvxFileHandle() : file_ver_(kLvxFileV1), device_count_(0), cur_frame_index_(0), LvxFileHandle::LvxFileHandle()
: file_ver_(kLvxFileV1), device_count_(0), cur_frame_index_(0),
cur_offset_(0), data_start_offset_(0), size_(0), mode_(0), state_(0) { cur_offset_(0), data_start_offset_(0), size_(0), mode_(0), state_(0) {
memset((void *)&public_header_, 0, sizeof(public_header_)); memset((void *)&public_header_, 0, sizeof(public_header_));
memset((void *)&private_header_, 0, sizeof(private_header_)); memset((void *)&private_header_, 0, sizeof(private_header_));
@@ -60,8 +61,9 @@ bool LvxFileHandle::ReadAndCheckHeader() {
} }
*/ */
if (public_header_.version[1] > kLvxFileV1) { if (public_header_.version[1] > kLvxFileV1) {
printf("Unkown lvx file version[%d.%d.%d.%d]\n", public_header_.version[0], \ 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]); public_header_.version[1], public_header_.version[2],
public_header_.version[3]);
return false; return false;
} }
@@ -73,11 +75,13 @@ bool LvxFileHandle::ReadAndCheckHeader() {
uint64_t LvxFileHandle::MiniFileSize() { uint64_t LvxFileHandle::MiniFileSize() {
if (file_ver_ == kLvxFileV1) { if (file_ver_ == kLvxFileV1) {
return (sizeof(LvxFilePublicHeader) + sizeof(LvxFilePrivateHeader) + \ return (sizeof(LvxFilePublicHeader) + sizeof(LvxFilePrivateHeader) +
sizeof(LvxFileDeviceInfo) + sizeof(FrameHeader) + sizeof(LvxFilePacket)); sizeof(LvxFileDeviceInfo) + sizeof(FrameHeader) +
sizeof(LvxFilePacket));
} else { } else {
return (sizeof(LvxFilePublicHeader) + sizeof(LvxFilePrivateHeaderV0) + \ return (sizeof(LvxFilePublicHeader) + sizeof(LvxFilePrivateHeaderV0) +
sizeof(LvxFileDeviceInfoV0) + sizeof(FrameHeaderV0) + sizeof(LvxFilePacketV0)); sizeof(LvxFileDeviceInfoV0) + sizeof(FrameHeaderV0) +
sizeof(LvxFilePacketV0));
} }
} }
@@ -87,16 +91,16 @@ uint64_t LvxFileHandle::PrivateHeaderOffset() {
uint64_t LvxFileHandle::DataStartOffset() { uint64_t LvxFileHandle::DataStartOffset() {
if (file_ver_ == kLvxFileV1) { if (file_ver_ == kLvxFileV1) {
return (sizeof(LvxFilePublicHeader) + sizeof(LvxFilePrivateHeader) + \ return (sizeof(LvxFilePublicHeader) + sizeof(LvxFilePrivateHeader) +
sizeof(LvxFileDeviceInfo) * private_header_.device_count); sizeof(LvxFileDeviceInfo) * private_header_.device_count);
} else { } else {
return (sizeof(LvxFilePublicHeader) + sizeof(LvxFilePrivateHeaderV0) + \ return (sizeof(LvxFilePublicHeader) + sizeof(LvxFilePrivateHeaderV0) +
sizeof(LvxFileDeviceInfoV0) * private_header_v0_.device_count); sizeof(LvxFileDeviceInfoV0) * private_header_v0_.device_count);
} }
} }
bool LvxFileHandle::AddAndCheckDeviceInfo() { bool LvxFileHandle::AddAndCheckDeviceInfo() {
lvx_file_.seekg (PrivateHeaderOffset(), std::ios::beg); lvx_file_.seekg(PrivateHeaderOffset(), std::ios::beg);
if (file_ver_ == kLvxFileV1) { if (file_ver_ == kLvxFileV1) {
lvx_file_.read((char *)(&private_header_), sizeof(private_header_)); lvx_file_.read((char *)(&private_header_), sizeof(private_header_));
@@ -110,16 +114,16 @@ bool LvxFileHandle::AddAndCheckDeviceInfo() {
return false; return false;
} }
for (int i=0; i< device_count_; i++) { for (int i = 0; i < device_count_; i++) {
LvxFileDeviceInfo device_info; LvxFileDeviceInfo device_info;
if (file_ver_ == kLvxFileV1) { if (file_ver_ == kLvxFileV1) {
lvx_file_.read((char *)(&device_info), sizeof(LvxFileDeviceInfo)); lvx_file_.read((char *)(&device_info), sizeof(LvxFileDeviceInfo));
} else { /* device info v0 to v1 */ } else { /* device info v0 to v1 */
LvxFileDeviceInfoV0 device_info_v0; LvxFileDeviceInfoV0 device_info_v0;
lvx_file_.read((char *)(&device_info_v0), sizeof(LvxFileDeviceInfoV0)); lvx_file_.read((char *)(&device_info_v0), sizeof(LvxFileDeviceInfoV0));
memcpy((void *)&device_info, (void *)&device_info_v0, \ memcpy((void *)&device_info, (void *)&device_info_v0,
&device_info.extrinsic_enable - device_info.lidar_broadcast_code); &device_info.extrinsic_enable - device_info.lidar_broadcast_code);
memcpy((void *)&device_info.roll, (void *)&device_info_v0.roll, \ memcpy((void *)&device_info.roll, (void *)&device_info_v0.roll,
sizeof(float) * 6); sizeof(float) * 6);
device_info.extrinsic_enable = 0; device_info.extrinsic_enable = 0;
} }
@@ -130,23 +134,23 @@ bool LvxFileHandle::AddAndCheckDeviceInfo() {
} }
bool LvxFileHandle::PrepareDataRead() { bool LvxFileHandle::PrepareDataRead() {
lvx_file_.seekg (DataStartOffset(), std::ios::beg); lvx_file_.seekg(DataStartOffset(), std::ios::beg);
FrameHeader frame_header; /* v0&v1 compatible */ FrameHeader frame_header; /* v0&v1 compatible */
lvx_file_.read((char *)(&frame_header), sizeof(frame_header)); 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)) { (frame_header.frame_index != 0)) {
return false; return false;
} }
/** reset the read position to the start offset of data erea */ /** 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; 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) { if ((mode & std::ios::in) == std::ios::in) {
state_ = kLvxFileOk; state_ = kLvxFileOk;
@@ -158,7 +162,8 @@ int LvxFileHandle::Open(const char* filename,std::ios_base::openmode mode) {
} }
size_ = lvx_file_.tellg(); 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()) { if (size_ < MiniFileSize()) {
state_ = kLvxFileSizeFault; state_ = kLvxFileSizeFault;
@@ -192,15 +197,13 @@ int LvxFileHandle::Open(const char* filename,std::ios_base::openmode mode) {
return state_; return state_;
} }
bool LvxFileHandle::Eof() { bool LvxFileHandle::Eof() { return lvx_file_.eof(); }
return lvx_file_.eof();
}
int LvxFileHandle::InitLvxFile() { int LvxFileHandle::InitLvxFile() {
time_t curtime = time(nullptr); 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); strftime(filename, sizeof(filename), "%Y%m%d%H%M%S", local_time);
return Open(filename, std::ios::out | std::ios::binary); return Open(filename, std::ios::out | std::ios::binary);
@@ -217,33 +220,41 @@ void LvxFileHandle::InitLvxFileHeader() {
public_header_.version[2] = 0; public_header_.version[2] = 0;
public_header_.version[3] = 0; public_header_.version[3] = 0;
public_header_.magic_code = kLvxHeaderMagicCode; 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_); cur_offset_ += sizeof(public_header_);
if (file_ver_ == kLvxFileV1) { 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_; private_header_.frame_duration = frame_duration_;
device_count_ = private_header_.device_count; 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_); cur_offset_ += sizeof(private_header_);
} else { } 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; 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_); cur_offset_ += sizeof(private_header_v0_);
} }
for (int i = 0; i < device_count_; i++) { for (int i = 0; i < device_count_; i++) {
if (file_ver_ == kLvxFileV1) { 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); cur_offset_ += sizeof(LvxFileDeviceInfo);
} else { } else {
LvxFileDeviceInfoV0 device_info_v0; LvxFileDeviceInfoV0 device_info_v0;
memcpy((void *)&device_info_v0, (void *)&device_info_list_[i], \ memcpy((void *)&device_info_v0, (void *)&device_info_list_[i],
&device_info_list_[i].extrinsic_enable - device_info_list_[i].lidar_broadcast_code); &device_info_list_[i].extrinsic_enable -
memcpy((void *)&device_info_v0.roll, (void *)&device_info_list_[i].roll, \ device_info_list_[i].lidar_broadcast_code);
memcpy((void *)&device_info_v0.roll, (void *)&device_info_list_[i].roll,
sizeof(float) * 6); sizeof(float) * 6);
memcpy(&write_buffer[cur_offset_], (void *)&device_info_v0, sizeof(device_info_v0)); memcpy(&write_buffer[cur_offset_], (void *)&device_info_v0,
sizeof(device_info_v0));
cur_offset_ += sizeof(device_info_v0); cur_offset_ += sizeof(device_info_v0);
} }
} }
@@ -251,9 +262,10 @@ void LvxFileHandle::InitLvxFileHeader() {
lvx_file_.write(&write_buffer[cur_offset_], cur_offset_); 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; uint64_t cur_pos = 0;
FrameHeader frame_header = { 0 }; FrameHeader frame_header = {0};
std::unique_ptr<char[]> write_buffer(new char[kMaxFrameSize]); std::unique_ptr<char[]> write_buffer(new char[kMaxFrameSize]);
frame_header.current_offset = cur_offset_; 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_; 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); cur_pos += sizeof(FrameHeader);
for (auto iter : point_packet_list_temp) { for (auto iter : point_packet_list_temp) {
if (cur_pos + iter.pack_size >= kMaxFrameSize) { 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; 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; cur_pos += iter.pack_size;
} else { } 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; 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_offset_ = frame_header.next_offset;
cur_frame_index_++; cur_frame_index_++;
@@ -289,11 +302,12 @@ void LvxFileHandle::CloseLvxFile() {
lvx_file_.close(); 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)); 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()) { if (idx < device_info_list_.size()) {
*info = device_info_list_[idx]; *info = device_info_list_[idx];
return 0; return 0;
@@ -302,12 +316,19 @@ int LvxFileHandle::GetDeviceInfo(uint8_t idx, LvxFileDeviceInfo* info) {
return -1; return -1;
} }
int LvxFileHandle::GetPacketsOfFrame(OutPacketBuffer* packets_of_frame) { int LvxFileHandle::GetPacketsOfFrame(OutPacketBuffer *packets_of_frame) {
if (!lvx_file_ || lvx_file_.eof()) { if (!lvx_file_ || lvx_file_.eof()) {
state_ = kLvxFileAtEnd; state_ = kLvxFileAtEnd;
return 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; FrameHeader frame_header;
FrameHeaderV0 frame_header_v0; FrameHeaderV0 frame_header_v0;
uint64_t read_length; uint64_t read_length;
@@ -316,7 +337,7 @@ int LvxFileHandle::GetPacketsOfFrame(OutPacketBuffer* packets_of_frame) {
if (!lvx_file_) { if (!lvx_file_) {
return kLvxFileReadFail; return kLvxFileReadFail;
} }
if ((size_ < frame_header.current_offset) || \ if ((size_ < frame_header.current_offset) ||
(frame_header.next_offset < frame_header.current_offset)) { (frame_header.next_offset < frame_header.current_offset)) {
return kLvxFileFrameHeaderError; return kLvxFileFrameHeaderError;
} }
@@ -327,14 +348,13 @@ int LvxFileHandle::GetPacketsOfFrame(OutPacketBuffer* packets_of_frame) {
if (!lvx_file_) { if (!lvx_file_) {
return kLvxFileReadFail; 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)) { (frame_header_v0.next_offset < frame_header_v0.current_offset)) {
return kLvxFileFrameHeaderError; return kLvxFileFrameHeaderError;
} }
packets_of_frame->data_size = DataSizeOfFrame(frame_header_v0); packets_of_frame->data_size = DataSizeOfFrame(frame_header_v0);
read_length = packets_of_frame->data_size; read_length = packets_of_frame->data_size;
} }
lvx_file_.read((char *)(packets_of_frame->packet), read_length); lvx_file_.read((char *)(packets_of_frame->packet), read_length);
if (lvx_file_) { if (lvx_file_) {
return kLvxFileOk; return kLvxFileOk;
@@ -359,18 +379,19 @@ void ParseExtrinsicXml(DeviceItem &item, LvxFileDeviceInfo &info) {
rapidxml::file<> extrinsic_param("extrinsic.xml"); rapidxml::file<> extrinsic_param("extrinsic.xml");
rapidxml::xml_document<> doc; rapidxml::xml_document<> doc;
doc.parse<0>(extrinsic_param.data()); 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()) { if ("Livox" == (std::string)root->name()) {
for (rapidxml::xml_node<>* device = root->first_node(); device; \ for (rapidxml::xml_node<> *device = root->first_node(); device;
device = device->next_sibling()) { device = device->next_sibling()) {
if ("Device" == (std::string)device->name() && \ if ("Device" == (std::string)device->name() &&
(strncmp(item.info.broadcast_code, device->value(), kBroadcastCodeSize) == 0)) { (strncmp(item.info.broadcast_code, device->value(),
kBroadcastCodeSize) == 0)) {
memcpy(info.lidar_broadcast_code, device->value(), kBroadcastCodeSize); memcpy(info.lidar_broadcast_code, device->value(), kBroadcastCodeSize);
memset(info.hub_broadcast_code, 0, kBroadcastCodeSize); memset(info.hub_broadcast_code, 0, kBroadcastCodeSize);
info.device_type = item.info.type; info.device_type = item.info.type;
info.device_index = item.handle; info.device_index = item.handle;
for (rapidxml::xml_attribute<>* param = device->first_attribute(); param; \ for (rapidxml::xml_attribute<> *param = device->first_attribute();
param = param->next_attribute()) { param; param = param->next_attribute()) {
if ("roll" == (std::string)param->name()) { if ("roll" == (std::string)param->name()) {
info.roll = static_cast<float>(atof(param->value())); 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_ #ifndef LIVOX_FILE_H_
#define LIVOX_FILE_H_ #define LIVOX_FILE_H_
#include <memory>
#include <ios>
#include <fstream>
#include <list>
#include <vector>
#include <mutex>
#include "livox_sdk.h" #include "livox_sdk.h"
#include <fstream>
#include <ios>
#include <list>
#include <memory>
#include <mutex>
#include <vector>
namespace livox_ros { namespace livox_ros {
#define kMaxPointSize 1500 #define kMaxPointSize 1500
#define kDefaultFrameDurationTime 50 #define kDefaultFrameDurationTime 50
const uint32_t kMaxFrameSize = 2048*1024; const uint32_t kMaxFrameSize = 2048 * 1024;
typedef enum { typedef enum {
kDeviceStateDisconnect = 0, kDeviceStateDisconnect = 0,
@@ -172,29 +172,31 @@ typedef struct {
#pragma pack() #pragma pack()
class LvxFileHandle { class LvxFileHandle {
public: public:
LvxFileHandle(); LvxFileHandle();
~LvxFileHandle() = default; ~LvxFileHandle() = default;
int Open(const char* filename, std::ios_base::openmode mode); int Open(const char *filename, std::ios_base::openmode mode);
bool Eof(); bool Eof();
int InitLvxFile(); int InitLvxFile();
void InitLvxFileHeader(); void InitLvxFileHeader();
void SaveFrameToLvxFile(std::list<LvxFilePacket>& point_packet_list_temp); void SaveFrameToLvxFile(std::list<LvxFilePacket> &point_packet_list_temp);
void BasePointsHandle(LivoxEthPacket* data, LvxFilePacket& packet); void BasePointsHandle(LivoxEthPacket *data, LvxFilePacket &packet);
void CloseLvxFile(); 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 GetDeviceInfoListSize() { return device_info_list_.size(); }
int GetDeviceCount() { return device_count_; } 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 GetFileState(void) { return state_; };
int GetPacketsOfFrame(OutPacketBuffer* PacketsOfFrame); int GetPacketsOfFrame(OutPacketBuffer *PacketsOfFrame);
int GetLvxFileReadProgress(); int GetLvxFileReadProgress();
int GetFileVersion() { return file_ver_; } int GetFileVersion() { return file_ver_; }
private: private:
std::fstream lvx_file_; std::fstream lvx_file_;
std::vector<LvxFileDeviceInfo> device_info_list_; std::vector<LvxFileDeviceInfo> device_info_list_;
uint8_t file_ver_; uint8_t file_ver_;
@@ -219,16 +221,16 @@ class LvxFileHandle {
bool AddAndCheckDeviceInfo(); bool AddAndCheckDeviceInfo();
bool PrepareDataRead(); bool PrepareDataRead();
uint64_t DataSizeOfFrame(FrameHeader& frame_header) { uint64_t DataSizeOfFrame(FrameHeader &frame_header) {
return (frame_header.next_offset - frame_header.current_offset - sizeof(frame_header)); return (frame_header.next_offset - frame_header.current_offset -
sizeof(frame_header));
} }
uint64_t DataSizeOfFrame(FrameHeaderV0& frame_header_v0) { uint64_t DataSizeOfFrame(FrameHeaderV0 &frame_header_v0) {
return (frame_header_v0.next_offset - frame_header_v0.current_offset - \ return (frame_header_v0.next_offset - frame_header_v0.current_offset -
sizeof(frame_header_v0)); sizeof(frame_header_v0));
} }
}; };
} } // namespace livox_ros
#endif #endif

41
livox_ros_driver/timesync/timesync.cpp
View File

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

37
livox_ros_driver/timesync/timesync.h
View File

@@ -25,22 +25,17 @@
#ifndef TIMESYNC_TIMESYNC_H_ #ifndef TIMESYNC_TIMESYNC_H_
#define TIMESYNC_TIMESYNC_H_ #define TIMESYNC_TIMESYNC_H_
#include <thread>
#include "comm_protocol.h"
#include "comm_device.h" #include "comm_device.h"
#include "comm_protocol.h"
#include "user_uart.h" #include "user_uart.h"
#include <thread>
namespace livox_ros { 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 { typedef struct {
CommDevConfig dev_config; CommDevConfig dev_config;
@@ -48,21 +43,21 @@ typedef struct {
} TimeSyncConfig; } TimeSyncConfig;
class TimeSync { class TimeSync {
public: public:
static TimeSync* GetInstance() { static TimeSync *GetInstance() {
static TimeSync time_sync; static TimeSync time_sync;
return &time_sync; return &time_sync;
} }
int32_t InitTimeSync(const TimeSyncConfig& config); int32_t InitTimeSync(const TimeSyncConfig &config);
int32_t DeInitTimeSync(); int32_t DeInitTimeSync();
void StartTimesync() { void StartTimesync() {
start_poll_state_ = true; 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)) { if ((cb != nullptr) || (data != nullptr)) {
fn_cb_ = cb; fn_cb_ = cb;
client_data_ = data; client_data_ = data;
@@ -72,11 +67,11 @@ class TimeSync {
} }
} }
private: private:
TimeSync(); TimeSync();
~TimeSync(); ~TimeSync();
TimeSync(const TimeSync&) = delete; TimeSync(const TimeSync &) = delete;
TimeSync& operator=(const TimeSync&) = delete; TimeSync &operator=(const TimeSync &) = delete;
void PollStateLoop(); void PollStateLoop();
void PollDataLoop(); void PollDataLoop();
@@ -91,12 +86,12 @@ class TimeSync {
volatile bool start_poll_data_; volatile bool start_poll_data_;
TimeSyncConfig config_; TimeSyncConfig config_;
UserUart* uart_; UserUart *uart_;
CommProtocol* comm_; CommProtocol *comm_;
volatile uint32_t rx_bytes_; volatile uint32_t rx_bytes_;
FnReceiveSyncTimeCb fn_cb_; FnReceiveSyncTimeCb fn_cb_;
void* client_data_; void *client_data_;
volatile uint8_t fsm_state_; volatile uint8_t fsm_state_;
std::chrono::steady_clock::time_point transfer_time_; std::chrono::steady_clock::time_point transfer_time_;
@@ -106,5 +101,5 @@ class TimeSync {
void FsmCheckDevState(); void FsmCheckDevState();
}; };
} } // namespace livox_ros
#endif #endif

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

@@ -22,20 +22,19 @@
// SOFTWARE. // SOFTWARE.
// //
#include "user_uart.h" #include "user_uart.h"
#include <stdio.h> #include <stdio.h>
#include <string.h> #include <string.h>
#include <unistd.h>
#include <termios.h>
#include <sys/types.h>
#include <sys/stat.h> #include <sys/stat.h>
#include <sys/types.h>
#include <termios.h>
#include <unistd.h>
namespace livox_ros { namespace livox_ros {
UserUart::UserUart(uint8_t baudrate_index, uint8_t parity): UserUart::UserUart(uint8_t baudrate_index, uint8_t parity)
baudrate_(baudrate_index), parity_(parity) { : baudrate_(baudrate_index), parity_(parity) {
fd_ = 0; fd_ = 0;
is_open_ = false; is_open_ = false;
} }
@@ -44,14 +43,14 @@ UserUart::~UserUart() {
is_open_ = false; is_open_ = false;
if (fd_ > 0) { if (fd_ > 0) {
/** first we flush the port */ /** first we flush the port */
tcflush(fd_,TCOFLUSH); tcflush(fd_, TCOFLUSH);
tcflush(fd_,TCIFLUSH); tcflush(fd_, TCIFLUSH);
close(fd_); close(fd_);
} }
} }
int UserUart::Open(const char* filename) { int UserUart::Open(const char *filename) {
fd_ = open(filename, O_RDWR | O_NOCTTY); //| O_NDELAY fd_ = open(filename, O_RDWR | O_NOCTTY); //| O_NDELAY
if (fd_ < 0) { if (fd_ < 0) {
printf("Open %s fail!\n", filename); printf("Open %s fail!\n", filename);
@@ -77,8 +76,8 @@ int UserUart::Close() {
is_open_ = false; is_open_ = false;
if (fd_ > 0) { if (fd_ > 0) {
/** first we flush the port */ /** first we flush the port */
tcflush(fd_,TCOFLUSH); tcflush(fd_, TCOFLUSH);
tcflush(fd_,TCIFLUSH); tcflush(fd_, TCIFLUSH);
return close(fd_); return close(fd_);
} }
@@ -87,11 +86,10 @@ int UserUart::Close() {
/** sets up the port parameters */ /** sets up the port parameters */
int UserUart::Setup(uint8_t baudrate_index, uint8_t parity) { int UserUart::Setup(uint8_t baudrate_index, uint8_t parity) {
static uint32_t baud_map[19] = {\ static uint32_t baud_map[19] = {
B2400, B4800, B9600, B19200, B38400, B57600,B115200, B230400,\ B2400, B4800, B9600, B19200, B38400, B57600, B115200,
B460800, B500000, B576000,B921600,B1152000, B1500000, B2000000,\ B230400, B460800, B500000, B576000, B921600, B1152000, B1500000,
B2500000, B3000000, B3500000, B4000000\ B2000000, B2500000, B3000000, B3500000, B4000000};
};
tcflag_t baudrate; tcflag_t baudrate;
struct termios options; struct termios options;
@@ -110,12 +108,12 @@ int UserUart::Setup(uint8_t baudrate_index, uint8_t parity) {
options.c_cflag |= (CLOCAL | CREAD); options.c_cflag |= (CLOCAL | CREAD);
/** Disable hardware flow */ /** Disable hardware flow */
//options.c_cflag &= ~CRTSCTS; // options.c_cflag &= ~CRTSCTS;
/** Disable software flow */ /** 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 */ /** set boadrate */
options.c_cflag &= ~CBAUD; options.c_cflag &= ~CBAUD;
@@ -161,7 +159,7 @@ int UserUart::Setup(uint8_t baudrate_index, uint8_t parity) {
options.c_iflag &= ~INPCK; options.c_iflag &= ~INPCK;
/** Enable non-canonical */ /** Enable non-canonical */
//options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); // options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
/** Time to wait for data */ /** Time to wait for data */
options.c_cc[VTIME] = 1; options.c_cc[VTIME] = 1;
@@ -194,5 +192,4 @@ ssize_t UserUart::Read(char *buffer, size_t size) {
} }
} }
} } // namespace livox_ros

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

@@ -26,9 +26,9 @@
#define USER_UART_H_ #define USER_UART_H_
#include <stdint.h> #include <stdint.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/fcntl.h> #include <sys/fcntl.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <termios.h> #include <termios.h>
@@ -67,7 +67,7 @@ enum BaudRate {
class UserUart { class UserUart {
public: public:
UserUart(uint8_t baudrate_index, uint8_t parity); UserUart(uint8_t baudrate_index, uint8_t parity);
~UserUart(); ~UserUart();
@@ -75,10 +75,10 @@ class UserUart {
ssize_t Write(const char *buffer, size_t size); ssize_t Write(const char *buffer, size_t size);
ssize_t Read(char *buffer, size_t size); ssize_t Read(char *buffer, size_t size);
int Close(); int Close();
int Open(const char* filename); int Open(const char *filename);
bool IsOpen() { return is_open_; }; bool IsOpen() { return is_open_; };
private: private:
int fd_; int fd_;
volatile bool is_open_; volatile bool is_open_;
@@ -86,7 +86,6 @@ class UserUart {
uint8_t parity_; uint8_t parity_;
}; };
} } // namespace livox_ros
#endif #endif