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_

360
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

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

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

62
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,38 +104,35 @@ void CommProtocol::UpdateCache(void) {
} }
} }
int32_t CommProtocol::Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len, \ int32_t CommProtocol::Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len,
const CommPacket &i_packet) { 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: {
FsmSearchPacketPreamble(); FsmSearchPacketPreamble();
break; break;
} }
case kFindPacketLength: { case kFindPacketLength: {
FsmFindPacketLength(); FsmFindPacketLength();
break; break;
} }
case kGetPacketData: { case kGetPacketData: {
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

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

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

@@ -34,44 +34,40 @@ const uint8_t kGpsProtocolSof = '$';
const uint8_t kGpsProtocolEof = '*'; const uint8_t kGpsProtocolEof = '*';
const uint32_t kPacketLengthLmit = 200; 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
} }
memset((void *)o_packet, 0, sizeof(CommPacket)); memset((void *)o_packet, 0, sizeof(CommPacket));
o_packet->protocol = kGps; o_packet->protocol = kGps;
o_packet->data = (uint8_t *)i_buf; o_packet->data = (uint8_t *)i_buf;
o_packet->data_len = i_len; o_packet->data_len = i_len;
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

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

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

@@ -40,7 +40,11 @@ typedef enum { kNoNeed, kNeedAck, kDelayAck } NeedAckType;
typedef enum { kParseSuccess, kParseFail, kParseNeedMoreData } ParseResult; typedef enum { 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_

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

@@ -29,12 +29,13 @@
namespace livox_ros { namespace livox_ros {
const uint8_t kSdkProtocolSof = 0xAA; 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,11 +72,12 @@ 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()) {
return -1; // packet lenth error return -1; // packet lenth error
} }
memset((void *)o_packet, 0, sizeof(CommPacket)); memset((void *)o_packet, 0, sizeof(CommPacket));
@@ -92,9 +95,7 @@ int32_t SdkProtocol::ParsePacket(const uint8_t *i_buf, uint32_t i_len, CommPacke
return 0; 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,10 +120,10 @@ 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) {
return 0; return 0;
@@ -129,4 +131,4 @@ int32_t SdkProtocol::CheckPacket(const uint8_t *buf) {
return -1; return -1;
} }
} }
}// namespace livox } // namespace livox_ros

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

@@ -25,9 +25,9 @@
#ifndef LIVOX_SDK_PROTOCOL_H_ #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_

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available. // 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.
// //
// Licensed under the MIT License (the "License"); you may not use this file except // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// in compliance with the License. You may obtain a copy of the License at // rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
// //
// http://opensource.org/licenses/MIT // 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_
@@ -24,15 +28,16 @@ RAPIDJSON_NAMESPACE_BEGIN
/*! \class rapidjson::Allocator /*! \class rapidjson::Allocator
\brief Concept for allocating, resizing and freeing memory block. \brief Concept for allocating, resizing and freeing memory block.
Note that Malloc() and Realloc() are non-static but Free() is static. Note that Malloc() and Realloc() are non-static but Free() is static.
So if an allocator need to support Free(), it needs to put its pointer in So if an allocator need to support Free(), it needs to put its pointer in
the header of memory block. the header of memory block.
\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
@@ -74,209 +79,226 @@ 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)originalSize;
if (newSize == 0) {
std::free(originalPtr);
return NULL;
} }
void* Realloc(void* originalPtr, size_t originalSize, size_t newSize) { return std::realloc(originalPtr, newSize);
(void)originalSize; }
if (newSize == 0) { static void Free(void *ptr) { std::free(ptr); }
std::free(originalPtr);
return NULL;
}
return std::realloc(originalPtr, newSize);
}
static void Free(void *ptr) { std::free(ptr); }
}; };
/////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////////
// MemoryPoolAllocator // MemoryPoolAllocator
//! Default memory allocator used by the parser and DOM. //! Default memory allocator used by the parser and DOM.
/*! This allocator allocate memory blocks from pre-allocated memory chunks. /*! This allocator allocate memory blocks from pre-allocated memory chunks.
It does not free memory blocks. And Realloc() only allocate new memory. 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) : */
chunkHead_(0), chunk_capacity_(chunkSize), userBuffer_(0), baseAllocator_(baseAllocator), ownBaseAllocator_(0) MemoryPoolAllocator(size_t chunkSize = kDefaultChunkCapacity,
{ BaseAllocator *baseAllocator = 0)
: chunkHead_(0), chunk_capacity_(chunkSize), userBuffer_(0),
baseAllocator_(baseAllocator), ownBaseAllocator_(0) {}
//! Constructor with user-supplied buffer.
/*! The user buffer will be used firstly. When it is full, memory pool
allocates new chunk with chunk size.
The user buffer will not be deallocated when this allocator is destructed.
\param buffer User supplied buffer.
\param size Size of the buffer in bytes. It must at least larger than
sizeof(ChunkHeader). \param chunkSize The size of memory chunk. The default
is kDefaultChunkSize. \param baseAllocator The allocator for allocating
memory chunks.
*/
MemoryPoolAllocator(void *buffer, size_t size,
size_t chunkSize = kDefaultChunkCapacity,
BaseAllocator *baseAllocator = 0)
: chunkHead_(0), chunk_capacity_(chunkSize), userBuffer_(buffer),
baseAllocator_(baseAllocator), ownBaseAllocator_(0) {
RAPIDJSON_ASSERT(buffer != 0);
RAPIDJSON_ASSERT(size > sizeof(ChunkHeader));
chunkHead_ = reinterpret_cast<ChunkHeader *>(buffer);
chunkHead_->capacity = size - sizeof(ChunkHeader);
chunkHead_->size = 0;
chunkHead_->next = 0;
}
//! Destructor.
/*! This deallocates all memory chunks, excluding the user-supplied buffer.
*/
~MemoryPoolAllocator() {
Clear();
RAPIDJSON_DELETE(ownBaseAllocator_);
}
//! Deallocates all memory chunks, excluding the user-supplied buffer.
void Clear() {
while (chunkHead_ && chunkHead_ != userBuffer_) {
ChunkHeader *next = chunkHead_->next;
baseAllocator_->Free(chunkHead_);
chunkHead_ = next;
}
if (chunkHead_ && chunkHead_ == userBuffer_)
chunkHead_->size = 0; // Clear user buffer
}
//! Computes the total capacity of allocated memory chunks.
/*! \return total capacity in bytes.
*/
size_t Capacity() const {
size_t capacity = 0;
for (ChunkHeader *c = chunkHead_; c != 0; c = c->next)
capacity += c->capacity;
return capacity;
}
//! Computes the memory blocks allocated.
/*! \return total used bytes.
*/
size_t Size() const {
size_t size = 0;
for (ChunkHeader *c = chunkHead_; c != 0; c = c->next)
size += c->size;
return size;
}
//! Allocates a memory block. (concept Allocator)
void *Malloc(size_t size) {
if (!size)
return NULL;
size = RAPIDJSON_ALIGN(size);
if (chunkHead_ == 0 || chunkHead_->size + size > chunkHead_->capacity)
if (!AddChunk(chunk_capacity_ > size ? chunk_capacity_ : size))
return NULL;
void *buffer = reinterpret_cast<char *>(chunkHead_) +
RAPIDJSON_ALIGN(sizeof(ChunkHeader)) + chunkHead_->size;
chunkHead_->size += size;
return buffer;
}
//! Resizes a memory block (concept Allocator)
void *Realloc(void *originalPtr, size_t originalSize, size_t newSize) {
if (originalPtr == 0)
return Malloc(newSize);
if (newSize == 0)
return NULL;
originalSize = RAPIDJSON_ALIGN(originalSize);
newSize = RAPIDJSON_ALIGN(newSize);
// Do not shrink if new size is smaller than original
if (originalSize >= newSize)
return originalPtr;
// Simply expand it if it is the last allocation and there is sufficient
// space
if (originalPtr == reinterpret_cast<char *>(chunkHead_) +
RAPIDJSON_ALIGN(sizeof(ChunkHeader)) +
chunkHead_->size - originalSize) {
size_t increment = static_cast<size_t>(newSize - originalSize);
if (chunkHead_->size + increment <= chunkHead_->capacity) {
chunkHead_->size += increment;
return originalPtr;
}
} }
//! Constructor with user-supplied buffer. // Realloc process: allocate and copy memory, do not free original buffer.
/*! The user buffer will be used firstly. When it is full, memory pool allocates new chunk with chunk size. if (void *newBuffer = Malloc(newSize)) {
if (originalSize)
std::memcpy(newBuffer, originalPtr, originalSize);
return newBuffer;
} else
return NULL;
}
The user buffer will not be deallocated when this allocator is destructed. //! Frees a memory block (concept Allocator)
static void Free(void *ptr) { (void)ptr; } // Do nothing
\param buffer User supplied buffer.
\param size Size of the buffer in bytes. It must at least larger than sizeof(ChunkHeader).
\param chunkSize The size of memory chunk. The default is kDefaultChunkSize.
\param baseAllocator The allocator for allocating memory chunks.
*/
MemoryPoolAllocator(void *buffer, size_t size, size_t chunkSize = kDefaultChunkCapacity, BaseAllocator* baseAllocator = 0) :
chunkHead_(0), chunk_capacity_(chunkSize), userBuffer_(buffer), baseAllocator_(baseAllocator), ownBaseAllocator_(0)
{
RAPIDJSON_ASSERT(buffer != 0);
RAPIDJSON_ASSERT(size > sizeof(ChunkHeader));
chunkHead_ = reinterpret_cast<ChunkHeader*>(buffer);
chunkHead_->capacity = size - sizeof(ChunkHeader);
chunkHead_->size = 0;
chunkHead_->next = 0;
}
//! Destructor.
/*! This deallocates all memory chunks, excluding the user-supplied buffer.
*/
~MemoryPoolAllocator() {
Clear();
RAPIDJSON_DELETE(ownBaseAllocator_);
}
//! Deallocates all memory chunks, excluding the user-supplied buffer.
void Clear() {
while (chunkHead_ && chunkHead_ != userBuffer_) {
ChunkHeader* next = chunkHead_->next;
baseAllocator_->Free(chunkHead_);
chunkHead_ = next;
}
if (chunkHead_ && chunkHead_ == userBuffer_)
chunkHead_->size = 0; // Clear user buffer
}
//! Computes the total capacity of allocated memory chunks.
/*! \return total capacity in bytes.
*/
size_t Capacity() const {
size_t capacity = 0;
for (ChunkHeader* c = chunkHead_; c != 0; c = c->next)
capacity += c->capacity;
return capacity;
}
//! Computes the memory blocks allocated.
/*! \return total used bytes.
*/
size_t Size() const {
size_t size = 0;
for (ChunkHeader* c = chunkHead_; c != 0; c = c->next)
size += c->size;
return size;
}
//! Allocates a memory block. (concept Allocator)
void* Malloc(size_t size) {
if (!size)
return NULL;
size = RAPIDJSON_ALIGN(size);
if (chunkHead_ == 0 || chunkHead_->size + size > chunkHead_->capacity)
if (!AddChunk(chunk_capacity_ > size ? chunk_capacity_ : size))
return NULL;
void *buffer = reinterpret_cast<char *>(chunkHead_) + RAPIDJSON_ALIGN(sizeof(ChunkHeader)) + chunkHead_->size;
chunkHead_->size += size;
return buffer;
}
//! Resizes a memory block (concept Allocator)
void* Realloc(void* originalPtr, size_t originalSize, size_t newSize) {
if (originalPtr == 0)
return Malloc(newSize);
if (newSize == 0)
return NULL;
originalSize = RAPIDJSON_ALIGN(originalSize);
newSize = RAPIDJSON_ALIGN(newSize);
// Do not shrink if new size is smaller than original
if (originalSize >= newSize)
return originalPtr;
// Simply expand it if it is the last allocation and there is sufficient space
if (originalPtr == reinterpret_cast<char *>(chunkHead_) + RAPIDJSON_ALIGN(sizeof(ChunkHeader)) + chunkHead_->size - originalSize) {
size_t increment = static_cast<size_t>(newSize - originalSize);
if (chunkHead_->size + increment <= chunkHead_->capacity) {
chunkHead_->size += increment;
return originalPtr;
}
}
// Realloc process: allocate and copy memory, do not free original buffer.
if (void* newBuffer = Malloc(newSize)) {
if (originalSize)
std::memcpy(newBuffer, originalPtr, originalSize);
return newBuffer;
}
else
return NULL;
}
//! Frees a memory block (concept Allocator)
static void Free(void *ptr) { (void)ptr; } // Do nothing
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.
\return true if success. \return true if success.
*/ */
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 =
chunk->capacity = capacity; reinterpret_cast<ChunkHeader *>(baseAllocator_->Malloc(
chunk->size = 0; RAPIDJSON_ALIGN(sizeof(ChunkHeader)) + capacity))) {
chunk->next = chunkHead_; chunk->capacity = capacity;
chunkHead_ = chunk; chunk->size = 0;
return true; chunk->next = chunkHead_;
} chunkHead_ = chunk;
else return true;
return false; } else
} return false;
}
static const int kDefaultChunkCapacity = RAPIDJSON_ALLOCATOR_DEFAULT_CHUNK_CAPACITY; //!< Default chunk capacity. static const int kDefaultChunkCapacity =
RAPIDJSON_ALLOCATOR_DEFAULT_CHUNK_CAPACITY; //!< Default chunk capacity.
//! Chunk header for perpending to each chunk. //! 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
size_t size; //!< Current size of allocated memory in bytes. //!< itself).
ChunkHeader *next; //!< Next chunk in the linked list. size_t size; //!< Current size of allocated memory in bytes.
}; ChunkHeader *next; //!< Next chunk in the linked list.
};
ChunkHeader *chunkHead_; //!< Head of the chunk linked-list. Only the head chunk serves allocation. 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.
void *userBuffer_; //!< User supplied buffer. size_t chunk_capacity_; //!< The minimum capacity of chunk when they are
BaseAllocator* baseAllocator_; //!< base allocator for allocating memory chunks. //!< allocated.
BaseAllocator* ownBaseAllocator_; //!< base allocator created by this object. void *userBuffer_; //!< User supplied buffer.
BaseAllocator
*baseAllocator_; //!< base allocator for allocating memory chunks.
BaseAllocator *ownBaseAllocator_; //!< base allocator created by this object.
}; };
RAPIDJSON_NAMESPACE_END RAPIDJSON_NAMESPACE_END

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available. // Tencent is pleased to support the open source community by making RapidJSON
// available.
// //
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved. // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
// //
// Licensed under the MIT License (the "License"); you may not use this file except // 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_
@@ -24,45 +28,44 @@ RAPIDJSON_DIAG_OFF(effc++)
#if defined(_MSC_VER) && _MSC_VER <= 1800 #if defined(_MSC_VER) && _MSC_VER <= 1800
RAPIDJSON_DIAG_PUSH RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(4702) // unreachable code 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
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;
}
//! Get the error line number, if error exists. //! Get the error line number, if error exists.
size_t GetLine() const { return line_; } size_t GetLine() const { return line_; }
//! Get the error column number, if error exists. //! Get the error column number, if error exists.
size_t GetColumn() const { return col_; } size_t GetColumn() const { return col_; }
private: private:
size_t line_; //!< Current Line size_t line_; //!< Current Line
size_t col_; //!< Current Column size_t col_; //!< Current Column
}; };
#if defined(_MSC_VER) && _MSC_VER <= 1800 #if defined(_MSC_VER) && _MSC_VER <= 1800

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

File diff suppressed because it is too large Load Diff

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

@@ -1,22 +1,26 @@
// Tencent is pleased to support the open source community by making RapidJSON available. // 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.
// //
// Licensed under the MIT License (the "License"); you may not use this file except // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// in compliance with the License. You may obtain a copy of the License at // rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
// //
// http://opensource.org/licenses/MIT // 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,256 +36,365 @@ 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() {
size_t Tell() const { return is_.Tell(); } Ch c = current_;
current_ = Encoding::Take(is_);
return c;
}
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();
Ch Peek() const { return is_.Peek(); } if (static_cast<unsigned char>(is_.Peek()) == 0xBFu)
Ch Take() { return is_.Take(); } is_.Take();
size_t Tell() const { return is_.Tell(); } }
Ch Peek() const { return is_.Peek(); }
Ch Take() { return is_.Take(); }
size_t Tell() const { return is_.Tell(); }
// 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_);
} }
void Put(Ch c) { Encoding::Put(os_, c); } void Put(Ch c) { Encoding::Put(os_, c); }
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.
*/ */
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;
//! Constructor. //! Constructor.
/*! /*!
\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)
RAPIDJSON_ASSERT(type >= kUTF8 && type <= kUTF32BE); : is_(&is), type_(type), hasBOM_(false) {
DetectType(); RAPIDJSON_ASSERT(type >= kUTF8 && type <= kUTF32BE);
static const TakeFunc f[] = { RAPIDJSON_ENCODINGS_FUNC(Take) }; DetectType();
takeFunc_ = f[type_]; static const TakeFunc f[] = {RAPIDJSON_ENCODINGS_FUNC(Take)};
current_ = takeFunc_(*is_); takeFunc_ = f[type_];
} current_ = takeFunc_(*is_);
}
UTFType GetType() const { return type_; } UTFType GetType() const { return type_; }
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() {
size_t Tell() const { return is_->Tell(); } Ch c = current_;
current_ = takeFunc_(*is_);
return c;
}
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() {
// BOM (Byte Order Mark): // BOM (Byte Order Mark):
// 00 00 FE FF UTF-32BE // 00 00 FE FF UTF-32BE
// FF FE 00 00 UTF-32LE // FF FE 00 00 UTF-32LE
// FE FF UTF-16BE // FE FF UTF-16BE
// 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 =
if (!c) reinterpret_cast<const unsigned char *>(is_->Peek4());
return; if (!c)
return;
unsigned bom = static_cast<unsigned>(c[0] | (c[1] << 8) | (c[2] << 16) | (c[3] << 24)); unsigned bom =
hasBOM_ = false; static_cast<unsigned>(c[0] | (c[1] << 8) | (c[2] << 16) | (c[3] << 24));
if (bom == 0xFFFE0000) { type_ = kUTF32BE; hasBOM_ = true; is_->Take(); is_->Take(); is_->Take(); is_->Take(); } hasBOM_ = false;
else if (bom == 0x0000FEFF) { type_ = kUTF32LE; hasBOM_ = true; is_->Take(); is_->Take(); is_->Take(); is_->Take(); } if (bom == 0xFFFE0000) {
else if ((bom & 0xFFFF) == 0xFFFE) { type_ = kUTF16BE; hasBOM_ = true; is_->Take(); is_->Take(); } type_ = kUTF32BE;
else if ((bom & 0xFFFF) == 0xFEFF) { type_ = kUTF16LE; hasBOM_ = true; is_->Take(); is_->Take(); } hasBOM_ = true;
else if ((bom & 0xFFFFFF) == 0xBFBBEF) { type_ = kUTF8; hasBOM_ = true; is_->Take(); is_->Take(); is_->Take(); } is_->Take();
is_->Take();
// RFC 4627: Section 3 is_->Take();
// "Since the first two characters of a JSON text will always be ASCII is_->Take();
// characters [RFC0020], it is possible to determine whether an octet } else if (bom == 0x0000FEFF) {
// stream is UTF-8, UTF-16 (BE or LE), or UTF-32 (BE or LE) by looking type_ = kUTF32LE;
// at the pattern of nulls in the first four octets." hasBOM_ = true;
// 00 00 00 xx UTF-32BE is_->Take();
// 00 xx 00 xx UTF-16BE is_->Take();
// xx 00 00 00 UTF-32LE is_->Take();
// xx 00 xx 00 UTF-16LE is_->Take();
// xx xx xx xx UTF-8 } else if ((bom & 0xFFFF) == 0xFFFE) {
type_ = kUTF16BE;
if (!hasBOM_) { hasBOM_ = true;
int pattern = (c[0] ? 1 : 0) | (c[1] ? 2 : 0) | (c[2] ? 4 : 0) | (c[3] ? 8 : 0); is_->Take();
switch (pattern) { is_->Take();
case 0x08: type_ = kUTF32BE; break; } else if ((bom & 0xFFFF) == 0xFEFF) {
case 0x0A: type_ = kUTF16BE; break; type_ = kUTF16LE;
case 0x01: type_ = kUTF32LE; break; hasBOM_ = true;
case 0x05: type_ = kUTF16LE; break; is_->Take();
case 0x0F: type_ = kUTF8; break; is_->Take();
default: break; // Use type defined by user. } else if ((bom & 0xFFFFFF) == 0xBFBBEF) {
} type_ = kUTF8;
} hasBOM_ = true;
is_->Take();
// Runtime check whether the size of character type is sufficient. It only perform checks with assertion. is_->Take();
if (type_ == kUTF16LE || type_ == kUTF16BE) RAPIDJSON_ASSERT(sizeof(Ch) >= 2); is_->Take();
if (type_ == kUTF32LE || type_ == kUTF32BE) RAPIDJSON_ASSERT(sizeof(Ch) >= 4);
} }
typedef Ch (*TakeFunc)(InputByteStream& is); // RFC 4627: Section 3
InputByteStream* is_; // "Since the first two characters of a JSON text will always be ASCII
UTFType type_; // characters [RFC0020], it is possible to determine whether an octet
Ch current_; // stream is UTF-8, UTF-16 (BE or LE), or UTF-32 (BE or LE) by looking
TakeFunc takeFunc_; // at the pattern of nulls in the first four octets."
bool hasBOM_; // 00 00 00 xx UTF-32BE
// 00 xx 00 xx UTF-16BE
// xx 00 00 00 UTF-32LE
// xx 00 xx 00 UTF-16LE
// xx xx xx xx UTF-8
if (!hasBOM_) {
int pattern =
(c[0] ? 1 : 0) | (c[1] ? 2 : 0) | (c[2] ? 4 : 0) | (c[3] ? 8 : 0);
switch (pattern) {
case 0x08:
type_ = kUTF32BE;
break;
case 0x0A:
type_ = kUTF16BE;
break;
case 0x01:
type_ = kUTF32LE;
break;
case 0x05:
type_ = kUTF16LE;
break;
case 0x0F:
type_ = kUTF8;
break;
default:
break; // Use type defined by user.
}
}
// Runtime check whether the size of character type is sufficient. It only
// perform checks with assertion.
if (type_ == kUTF16LE || type_ == kUTF16BE)
RAPIDJSON_ASSERT(sizeof(Ch) >= 2);
if (type_ == kUTF32LE || type_ == kUTF32BE)
RAPIDJSON_ASSERT(sizeof(Ch) >= 4);
}
typedef Ch (*TakeFunc)(InputByteStream &is);
InputByteStream *is_;
UTFType type_;
Ch current_;
TakeFunc takeFunc_;
bool hasBOM_;
}; };
//! Output stream wrapper with dynamically bound encoding and automatic encoding detection. //! Output stream wrapper with dynamically bound encoding and automatic encoding
//! detection.
/*! /*!
\tparam CharType Type of character for writing. \tparam 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.
*/ */
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;
//! Constructor. //! Constructor.
/*! /*!
\param os output stream to be wrapped. \param os output stream to be wrapped.
\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)
RAPIDJSON_ASSERT(type >= kUTF8 && type <= kUTF32BE); : os_(&os), type_(type) {
RAPIDJSON_ASSERT(type >= kUTF8 && type <= kUTF32BE);
// Runtime check whether the size of character type is sufficient. It only perform checks with assertion. // 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)
PutBOM(); PutBOM();
} }
UTFType GetType() const { return type_; } UTFType GetType() const { return type_; }
void Put(Ch c) { putFunc_(*os_, c); } void Put(Ch c) { putFunc_(*os_, c); }
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_;
}; };
#undef RAPIDJSON_ENCODINGS_FUNC #undef RAPIDJSON_ENCODINGS_FUNC

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

File diff suppressed because it is too large Load Diff

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available. // 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.
// //
// Licensed under the MIT License (the "License"); you may not use this file except // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// in compliance with the License. You may obtain a copy of the License at // rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
// //
// http://opensource.org/licenses/MIT // 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,36 +37,62 @@ 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 *
switch (parseErrorCode) { GetParseError_En(ParseErrorCode parseErrorCode) {
case kParseErrorNone: return RAPIDJSON_ERROR_STRING("No error."); switch (parseErrorCode) {
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:
case kParseErrorValueInvalid: return RAPIDJSON_ERROR_STRING("Invalid value."); return RAPIDJSON_ERROR_STRING(
"The document root must not be followed by other values.");
case kParseErrorObjectMissName: return RAPIDJSON_ERROR_STRING("Missing a name for object member.");
case kParseErrorObjectMissColon: return RAPIDJSON_ERROR_STRING("Missing a colon after a name of object member.");
case kParseErrorObjectMissCommaOrCurlyBracket: return RAPIDJSON_ERROR_STRING("Missing a comma or '}' after an object member.");
case kParseErrorArrayMissCommaOrSquareBracket: return RAPIDJSON_ERROR_STRING("Missing a comma or ']' after an array element.");
case kParseErrorStringUnicodeEscapeInvalidHex: return RAPIDJSON_ERROR_STRING("Incorrect hex digit after \\u escape in string."); case kParseErrorValueInvalid:
case kParseErrorStringUnicodeSurrogateInvalid: return RAPIDJSON_ERROR_STRING("The surrogate pair in string is invalid."); return RAPIDJSON_ERROR_STRING("Invalid value.");
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 kParseErrorObjectMissName:
case kParseErrorNumberMissFraction: return RAPIDJSON_ERROR_STRING("Miss fraction part in number."); return RAPIDJSON_ERROR_STRING("Missing a name for object member.");
case kParseErrorNumberMissExponent: return RAPIDJSON_ERROR_STRING("Miss exponent in number."); case kParseErrorObjectMissColon:
return RAPIDJSON_ERROR_STRING(
"Missing a colon after a name of object member.");
case kParseErrorObjectMissCommaOrCurlyBracket:
return RAPIDJSON_ERROR_STRING(
"Missing a comma or '}' after an object member.");
case kParseErrorTermination: return RAPIDJSON_ERROR_STRING("Terminate parsing due to Handler error."); case kParseErrorArrayMissCommaOrSquareBracket:
case kParseErrorUnspecificSyntaxError: return RAPIDJSON_ERROR_STRING("Unspecific syntax error."); return RAPIDJSON_ERROR_STRING(
"Missing a comma or ']' after an array element.");
default: return RAPIDJSON_ERROR_STRING("Unknown error."); case kParseErrorStringUnicodeEscapeInvalidHex:
} return RAPIDJSON_ERROR_STRING(
"Incorrect hex digit after \\u escape in string.");
case kParseErrorStringUnicodeSurrogateInvalid:
return RAPIDJSON_ERROR_STRING("The surrogate pair in string is invalid.");
case kParseErrorStringEscapeInvalid:
return RAPIDJSON_ERROR_STRING("Invalid escape character in string.");
case kParseErrorStringMissQuotationMark:
return RAPIDJSON_ERROR_STRING(
"Missing a closing quotation mark in string.");
case kParseErrorStringInvalidEncoding:
return RAPIDJSON_ERROR_STRING("Invalid encoding in string.");
case kParseErrorNumberTooBig:
return RAPIDJSON_ERROR_STRING("Number too big to be stored in double.");
case kParseErrorNumberMissFraction:
return RAPIDJSON_ERROR_STRING("Miss fraction part in number.");
case kParseErrorNumberMissExponent:
return RAPIDJSON_ERROR_STRING("Miss exponent in number.");
case kParseErrorTermination:
return RAPIDJSON_ERROR_STRING("Terminate parsing due to Handler error.");
case kParseErrorUnspecificSyntaxError:
return RAPIDJSON_ERROR_STRING("Unspecific syntax error.");
default:
return RAPIDJSON_ERROR_STRING("Unknown error.");
}
} }
RAPIDJSON_NAMESPACE_END RAPIDJSON_NAMESPACE_END

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available. // 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.
// //
// Licensed under the MIT License (the "License"); you may not use this file except // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// in compliance with the License. You may obtain a copy of the License at // rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
// //
// http://opensource.org/licenses/MIT // 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_
@@ -62,31 +66,38 @@ RAPIDJSON_NAMESPACE_BEGIN
\see GenericReader::Parse, GenericReader::GetParseErrorCode \see GenericReader::Parse, GenericReader::GetParseErrorCode
*/ */
enum ParseErrorCode { 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.
kParseErrorStringEscapeInvalid, //!< Invalid escape character in string. kParseErrorStringUnicodeSurrogateInvalid, //!< The surrogate pair in string is
kParseErrorStringMissQuotationMark, //!< Missing a closing quotation mark in string. //!< invalid.
kParseErrorStringInvalidEncoding, //!< Invalid encoding in string. kParseErrorStringEscapeInvalid, //!< Invalid escape character in string.
kParseErrorStringMissQuotationMark, //!< Missing a closing quotation mark in
//!< string.
kParseErrorStringInvalidEncoding, //!< Invalid encoding in string.
kParseErrorNumberTooBig, //!< Number too big to be stored in double. kParseErrorNumberTooBig, //!< Number too big to be stored in double.
kParseErrorNumberMissFraction, //!< Miss fraction part in number. kParseErrorNumberMissFraction, //!< Miss fraction part in number.
kParseErrorNumberMissExponent, //!< Miss exponent in number. kParseErrorNumberMissExponent, //!< Miss exponent in number.
kParseErrorTermination, //!< Parsing was terminated. kParseErrorTermination, //!< Parsing was terminated.
kParseErrorUnspecificSyntaxError //!< Unspecific syntax error. kParseErrorUnspecificSyntaxError //!< Unspecific syntax error.
}; };
//! Result of parsing (wraps ParseErrorCode) //! Result of parsing (wraps ParseErrorCode)
@@ -104,40 +115,51 @@ enum ParseErrorCode {
\see GenericReader::Parse, GenericDocument::Parse \see GenericReader::Parse, GenericDocument::Parse
*/ */
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_; }
//! Get the error offset, if \ref IsError(), 0 otherwise. //! Get the error offset, if \ref IsError(), 0 otherwise.
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 {
//! Whether the result is an error. return !IsError() ? &ParseResult::IsError : NULL;
bool IsError() const { return code_ != kParseErrorNone; } }
//! Whether the result is an error.
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_;
size_t offset_; size_t offset_;
}; };
//! Function pointer type of GetParseError(). //! Function pointer type of GetParseError().
@@ -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

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available. // 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.
// //
// Licensed under the MIT License (the "License"); you may not use this file except // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// in compliance with the License. You may obtain a copy of the License at // rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
// //
// http://opensource.org/licenses/MIT // 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
@@ -33,61 +37,75 @@ RAPIDJSON_NAMESPACE_BEGIN
*/ */
class FileReadStream { class FileReadStream {
public: public:
typedef char Ch; //!< Character type (byte). typedef char Ch; //!< Character type (byte).
//! Constructor. //! Constructor.
/*! /*!
\param fp File pointer opened for read. \param fp File pointer opened for read.
\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)
RAPIDJSON_ASSERT(fp_ != 0); : fp_(fp), buffer_(buffer), bufferSize_(bufferSize), bufferLast_(0),
RAPIDJSON_ASSERT(bufferSize >= 4); current_(buffer_), readCount_(0), count_(0), eof_(false) {
Read(); RAPIDJSON_ASSERT(fp_ != 0);
} RAPIDJSON_ASSERT(bufferSize >= 4);
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:
void Read() { void Read() {
if (current_ < bufferLast_) if (current_ < bufferLast_)
++current_; ++current_;
else if (!eof_) { else if (!eof_) {
count_ += readCount_; count_ += readCount_;
readCount_ = std::fread(buffer_, 1, bufferSize_, fp_); readCount_ = std::fread(buffer_, 1, bufferSize_, fp_);
bufferLast_ = buffer_ + readCount_ - 1; bufferLast_ = buffer_ + readCount_ - 1;
current_ = buffer_; current_ = buffer_;
if (readCount_ < bufferSize_) { if (readCount_ < bufferSize_) {
buffer_[readCount_] = '\0'; buffer_[readCount_] = '\0';
++bufferLast_; ++bufferLast_;
eof_ = true; eof_ = true;
} }
}
} }
}
std::FILE* fp_; std::FILE *fp_;
Ch *buffer_; Ch *buffer_;
size_t bufferSize_; size_t bufferSize_;
Ch *bufferLast_; Ch *bufferLast_;
Ch *current_; Ch *current_;
size_t readCount_; size_t readCount_;
size_t count_; //!< Number of characters read size_t count_; //!< Number of characters read
bool eof_; bool eof_;
}; };
RAPIDJSON_NAMESPACE_END RAPIDJSON_NAMESPACE_END

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available. // 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.
// //
// Licensed under the MIT License (the "License"); you may not use this file except // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// in compliance with the License. You may obtain a copy of the License at // rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
// //
// http://opensource.org/licenses/MIT // 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
@@ -31,68 +35,86 @@ RAPIDJSON_NAMESPACE_BEGIN
*/ */
class FileWriteStream { 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)
RAPIDJSON_ASSERT(fp_ != 0); : fp_(fp), buffer_(buffer), bufferEnd_(buffer + bufferSize),
current_(buffer_) {
RAPIDJSON_ASSERT(fp_ != 0);
}
void Put(char c) {
if (current_ >= bufferEnd_)
Flush();
*current_++ = c;
}
void PutN(char c, size_t n) {
size_t avail = static_cast<size_t>(bufferEnd_ - current_);
while (n > avail) {
std::memset(current_, c, avail);
current_ += avail;
Flush();
n -= avail;
avail = static_cast<size_t>(bufferEnd_ - current_);
} }
void Put(char c) { if (n > 0) {
if (current_ >= bufferEnd_) std::memset(current_, c, n);
Flush(); current_ += n;
*current_++ = c;
} }
}
void PutN(char c, size_t n) { void Flush() {
size_t avail = static_cast<size_t>(bufferEnd_ - current_); if (current_ != buffer_) {
while (n > avail) { size_t result =
std::memset(current_, c, avail); std::fwrite(buffer_, 1, static_cast<size_t>(current_ - buffer_), fp_);
current_ += avail; if (result < static_cast<size_t>(current_ - buffer_)) {
Flush(); // failure deliberately ignored at this time
n -= avail; // added to avoid warn_unused_result build errors
avail = static_cast<size_t>(bufferEnd_ - current_); }
} current_ = buffer_;
if (n > 0) {
std::memset(current_, c, n);
current_ += n;
}
} }
}
void Flush() { // Not implemented
if (current_ != buffer_) { char Peek() const {
size_t result = std::fwrite(buffer_, 1, static_cast<size_t>(current_ - buffer_), fp_); RAPIDJSON_ASSERT(false);
if (result < static_cast<size_t>(current_ - buffer_)) { return 0;
// failure deliberately ignored at this time }
// added to avoid warn_unused_result build errors char Take() {
} RAPIDJSON_ASSERT(false);
current_ = buffer_; return 0;
} }
} size_t Tell() const {
RAPIDJSON_ASSERT(false);
// Not implemented return 0;
char Peek() const { RAPIDJSON_ASSERT(false); return 0; } }
char Take() { RAPIDJSON_ASSERT(false); return 0; } char *PutBegin() {
size_t Tell() const { RAPIDJSON_ASSERT(false); return 0; } RAPIDJSON_ASSERT(false);
char* PutBegin() { RAPIDJSON_ASSERT(false); return 0; } return 0;
size_t PutEnd(char*) { 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);
} }
RAPIDJSON_NAMESPACE_END RAPIDJSON_NAMESPACE_END

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available. // 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.
// //
// Licensed under the MIT License (the "License"); you may not use this file except // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// in compliance with the License. You may obtain a copy of the License at // rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
// //
// http://opensource.org/licenses/MIT // 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 StateAllocator>
typename OutputHandler,
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

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available. // 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.
// //
// Licensed under the MIT License (the "License"); you may not use this file except // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// in compliance with the License. You may obtain a copy of the License at // rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
// //
// http://opensource.org/licenses/MIT // 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_
@@ -27,261 +31,283 @@ namespace internal {
class BigInteger { 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) { digits_[0] = u; }
BigInteger(const char *decimals, size_t length) : count_(1) {
RAPIDJSON_ASSERT(length > 0);
digits_[0] = 0;
size_t i = 0;
const size_t kMaxDigitPerIteration =
19; // 2^64 = 18446744073709551616 > 10^19
while (length >= kMaxDigitPerIteration) {
AppendDecimal64(decimals + i, decimals + i + kMaxDigitPerIteration);
length -= kMaxDigitPerIteration;
i += kMaxDigitPerIteration;
} }
explicit BigInteger(uint64_t u) : count_(1) { if (length > 0)
digits_[0] = u; AppendDecimal64(decimals + i, decimals + i + length);
}
BigInteger &operator=(const BigInteger &rhs) {
if (this != &rhs) {
count_ = rhs.count_;
std::memcpy(digits_, rhs.digits_, count_ * sizeof(Type));
}
return *this;
}
BigInteger &operator=(uint64_t u) {
digits_[0] = u;
count_ = 1;
return *this;
}
BigInteger &operator+=(uint64_t u) {
Type backup = digits_[0];
digits_[0] += u;
for (size_t i = 0; i < count_ - 1; i++) {
if (digits_[i] >= backup)
return *this; // no carry
backup = digits_[i + 1];
digits_[i + 1] += 1;
} }
BigInteger(const char* decimals, size_t length) : count_(1) { // Last carry
RAPIDJSON_ASSERT(length > 0); if (digits_[count_ - 1] < backup)
digits_[0] = 0; PushBack(1);
size_t i = 0;
const size_t kMaxDigitPerIteration = 19; // 2^64 = 18446744073709551616 > 10^19
while (length >= kMaxDigitPerIteration) {
AppendDecimal64(decimals + i, decimals + i + kMaxDigitPerIteration);
length -= kMaxDigitPerIteration;
i += kMaxDigitPerIteration;
}
if (length > 0) return *this;
AppendDecimal64(decimals + i, decimals + i + length); }
}
BigInteger &operator*=(uint64_t u) {
BigInteger& operator=(const BigInteger &rhs) if (u == 0)
{ return *this = 0;
if (this != &rhs) { if (u == 1)
count_ = rhs.count_; return *this;
std::memcpy(digits_, rhs.digits_, count_ * sizeof(Type)); if (*this == 1)
} return *this = u;
return *this;
} uint64_t k = 0;
for (size_t i = 0; i < count_; i++) {
BigInteger& operator=(uint64_t u) { uint64_t hi;
digits_[0] = u; digits_[i] = MulAdd64(digits_[i], u, k, &hi);
count_ = 1; k = hi;
return *this;
} }
BigInteger& operator+=(uint64_t u) { if (k > 0)
Type backup = digits_[0]; PushBack(k);
digits_[0] += u;
for (size_t i = 0; i < count_ - 1; i++) {
if (digits_[i] >= backup)
return *this; // no carry
backup = digits_[i + 1];
digits_[i + 1] += 1;
}
// Last carry return *this;
if (digits_[count_ - 1] < backup) }
PushBack(1);
return *this; BigInteger &operator*=(uint32_t u) {
if (u == 0)
return *this = 0;
if (u == 1)
return *this;
if (*this == 1)
return *this = u;
uint64_t k = 0;
for (size_t i = 0; i < count_; i++) {
const uint64_t c = digits_[i] >> 32;
const uint64_t d = digits_[i] & 0xFFFFFFFF;
const uint64_t uc = u * c;
const uint64_t ud = u * d;
const uint64_t p0 = ud + k;
const uint64_t p1 = uc + (p0 >> 32);
digits_[i] = (p0 & 0xFFFFFFFF) | (p1 << 32);
k = p1 >> 32;
} }
BigInteger& operator*=(uint64_t u) { if (k > 0)
if (u == 0) return *this = 0; PushBack(k);
if (u == 1) return *this;
if (*this == 1) return *this = u;
uint64_t k = 0; return *this;
for (size_t i = 0; i < count_; i++) { }
uint64_t hi;
digits_[i] = MulAdd64(digits_[i], u, k, &hi);
k = hi;
}
if (k > 0)
PushBack(k);
return *this; BigInteger &operator<<=(size_t shift) {
if (IsZero() || shift == 0)
return *this;
size_t offset = shift / kTypeBit;
size_t interShift = shift % kTypeBit;
RAPIDJSON_ASSERT(count_ + offset <= kCapacity);
if (interShift == 0) {
std::memmove(digits_ + offset, digits_, count_ * sizeof(Type));
count_ += offset;
} else {
digits_[count_] = 0;
for (size_t i = count_; i > 0; i--)
digits_[i + offset] = (digits_[i] << interShift) |
(digits_[i - 1] >> (kTypeBit - interShift));
digits_[offset] = digits_[0] << interShift;
count_ += offset;
if (digits_[count_])
count_++;
} }
BigInteger& operator*=(uint32_t u) { std::memset(digits_, 0, offset * sizeof(Type));
if (u == 0) return *this = 0;
if (u == 1) return *this;
if (*this == 1) return *this = u;
uint64_t k = 0; return *this;
for (size_t i = 0; i < count_; i++) { }
const uint64_t c = digits_[i] >> 32;
const uint64_t d = digits_[i] & 0xFFFFFFFF;
const uint64_t uc = u * c;
const uint64_t ud = u * d;
const uint64_t p0 = ud + k;
const uint64_t p1 = uc + (p0 >> 32);
digits_[i] = (p0 & 0xFFFFFFFF) | (p1 << 32);
k = p1 >> 32;
}
if (k > 0)
PushBack(k);
return *this; bool operator==(const BigInteger &rhs) const {
return count_ == rhs.count_ &&
std::memcmp(digits_, rhs.digits_, count_ * sizeof(Type)) == 0;
}
bool operator==(const Type rhs) const {
return count_ == 1 && digits_[0] == rhs;
}
BigInteger &MultiplyPow5(unsigned exp) {
static const uint32_t kPow5[12] = {
5,
5 * 5,
5 * 5 * 5,
5 * 5 * 5 * 5,
5 * 5 * 5 * 5 * 5,
5 * 5 * 5 * 5 * 5 * 5,
5 * 5 * 5 * 5 * 5 * 5 * 5,
5 * 5 * 5 * 5 * 5 * 5 * 5 * 5,
5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5,
5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5,
5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5,
5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5};
if (exp == 0)
return *this;
for (; exp >= 27; exp -= 27)
*this *= RAPIDJSON_UINT64_C2(0X6765C793, 0XFA10079D); // 5^27
for (; exp >= 13; exp -= 13)
*this *= static_cast<uint32_t>(1220703125u); // 5^13
if (exp > 0)
*this *= kPow5[exp - 1];
return *this;
}
// Compute absolute difference of this and rhs.
// Assume this != rhs
bool Difference(const BigInteger &rhs, BigInteger *out) const {
int cmp = Compare(rhs);
RAPIDJSON_ASSERT(cmp != 0);
const BigInteger *a, *b; // Makes a > b
bool ret;
if (cmp < 0) {
a = &rhs;
b = this;
ret = true;
} else {
a = this;
b = &rhs;
ret = false;
} }
BigInteger& operator<<=(size_t shift) { Type borrow = 0;
if (IsZero() || shift == 0) return *this; for (size_t i = 0; i < a->count_; i++) {
Type d = a->digits_[i] - borrow;
size_t offset = shift / kTypeBit; if (i < b->count_)
size_t interShift = shift % kTypeBit; d -= b->digits_[i];
RAPIDJSON_ASSERT(count_ + offset <= kCapacity); borrow = (d > a->digits_[i]) ? 1 : 0;
out->digits_[i] = d;
if (interShift == 0) { if (d != 0)
std::memmove(digits_ + offset, digits_, count_ * sizeof(Type)); out->count_ = i + 1;
count_ += offset;
}
else {
digits_[count_] = 0;
for (size_t i = count_; i > 0; i--)
digits_[i + offset] = (digits_[i] << interShift) | (digits_[i - 1] >> (kTypeBit - interShift));
digits_[offset] = digits_[0] << interShift;
count_ += offset;
if (digits_[count_])
count_++;
}
std::memset(digits_, 0, offset * sizeof(Type));
return *this;
} }
bool operator==(const BigInteger& rhs) const { return ret;
return count_ == rhs.count_ && std::memcmp(digits_, rhs.digits_, count_ * sizeof(Type)) == 0; }
}
bool operator==(const Type rhs) const { int Compare(const BigInteger &rhs) const {
return count_ == 1 && digits_[0] == rhs; if (count_ != rhs.count_)
} return count_ < rhs.count_ ? -1 : 1;
BigInteger& MultiplyPow5(unsigned exp) { for (size_t i = count_; i-- > 0;)
static const uint32_t kPow5[12] = { if (digits_[i] != rhs.digits_[i])
5, return digits_[i] < rhs.digits_[i] ? -1 : 1;
5 * 5,
5 * 5 * 5,
5 * 5 * 5 * 5,
5 * 5 * 5 * 5 * 5,
5 * 5 * 5 * 5 * 5 * 5,
5 * 5 * 5 * 5 * 5 * 5 * 5,
5 * 5 * 5 * 5 * 5 * 5 * 5 * 5,
5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5,
5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5,
5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5,
5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5
};
if (exp == 0) return *this;
for (; exp >= 27; exp -= 27) *this *= RAPIDJSON_UINT64_C2(0X6765C793, 0XFA10079D); // 5^27
for (; exp >= 13; exp -= 13) *this *= static_cast<uint32_t>(1220703125u); // 5^13
if (exp > 0) *this *= kPow5[exp - 1];
return *this;
}
// Compute absolute difference of this and rhs. return 0;
// Assume this != rhs }
bool Difference(const BigInteger& rhs, BigInteger* out) const {
int cmp = Compare(rhs);
RAPIDJSON_ASSERT(cmp != 0);
const BigInteger *a, *b; // Makes a > b
bool ret;
if (cmp < 0) { a = &rhs; b = this; ret = true; }
else { a = this; b = &rhs; ret = false; }
Type borrow = 0; size_t GetCount() const { return count_; }
for (size_t i = 0; i < a->count_; i++) { Type GetDigit(size_t index) const {
Type d = a->digits_[i] - borrow; RAPIDJSON_ASSERT(index < count_);
if (i < b->count_) return digits_[index];
d -= b->digits_[i]; }
borrow = (d > a->digits_[i]) ? 1 : 0; bool IsZero() const { return count_ == 1 && digits_[0] == 0; }
out->digits_[i] = d;
if (d != 0)
out->count_ = i + 1;
}
return ret;
}
int Compare(const BigInteger& rhs) const {
if (count_ != rhs.count_)
return count_ < rhs.count_ ? -1 : 1;
for (size_t i = count_; i-- > 0;)
if (digits_[i] != rhs.digits_[i])
return digits_[i] < rhs.digits_[i] ? -1 : 1;
return 0;
}
size_t GetCount() const { return count_; }
Type GetDigit(size_t index) const { RAPIDJSON_ASSERT(index < count_); return digits_[index]; }
bool IsZero() const { return count_ == 1 && digits_[0] == 0; }
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;
else { else {
unsigned exp = static_cast<unsigned>(end - begin); unsigned exp = static_cast<unsigned>(end - begin);
(MultiplyPow5(exp) <<= exp) += u; // *this = *this * 10^exp + u (MultiplyPow5(exp) <<= exp) += u; // *this = *this * 10^exp + u
}
} }
}
void PushBack(Type digit) { void PushBack(Type digit) {
RAPIDJSON_ASSERT(count_ < kCapacity); RAPIDJSON_ASSERT(count_ < kCapacity);
digits_[count_++] = digit; digits_[count_++] = digit;
}
static uint64_t ParseUint64(const char *begin, const char *end) {
uint64_t r = 0;
for (const char *p = begin; p != end; ++p) {
RAPIDJSON_ASSERT(*p >= '0' && *p <= '9');
r = r * 10u + static_cast<unsigned>(*p - '0');
} }
return r;
}
static uint64_t ParseUint64(const char* begin, const char* end) { // Assume a * b + k < 2^128
uint64_t r = 0; static uint64_t MulAdd64(uint64_t a, uint64_t b, uint64_t k,
for (const char* p = begin; p != end; ++p) { uint64_t *outHigh) {
RAPIDJSON_ASSERT(*p >= '0' && *p <= '9');
r = r * 10u + static_cast<unsigned>(*p - '0');
}
return r;
}
// Assume a * b + k < 2^128
static uint64_t MulAdd64(uint64_t a, uint64_t b, uint64_t k, uint64_t* outHigh) {
#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)) && \
__extension__ typedef unsigned __int128 uint128; defined(__x86_64__)
uint128 p = static_cast<uint128>(a) * static_cast<uint128>(b); __extension__ typedef unsigned __int128 uint128;
p += k; uint128 p = static_cast<uint128>(a) * static_cast<uint128>(b);
*outHigh = static_cast<uint64_t>(p >> 64); p += k;
return static_cast<uint64_t>(p); *outHigh = static_cast<uint64_t>(p >> 64);
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,
uint64_t x0 = a0 * b0, x1 = a0 * b1, x2 = a1 * b0, x3 = a1 * b1; b1 = b >> 32;
x1 += (x0 >> 32); // can't give carry uint64_t x0 = a0 * b0, x1 = a0 * b1, x2 = a1 * b0, x3 = a1 * b1;
x1 += x2; x1 += (x0 >> 32); // can't give carry
if (x1 < x2) x1 += x2;
x3 += (static_cast<uint64_t>(1) << 32); if (x1 < x2)
uint64_t lo = (x1 << 32) + (x0 & 0xFFFFFFFF); x3 += (static_cast<uint64_t>(1) << 32);
uint64_t hi = x3 + (x1 >> 32); uint64_t lo = (x1 << 32) + (x0 & 0xFFFFFFFF);
uint64_t hi = x3 + (x1 >> 32);
lo += k; lo += k;
if (lo < k) if (lo < k)
hi++; hi++;
*outHigh = hi; *outHigh = hi;
return lo; return lo;
#endif #endif
} }
static const size_t kBitCount = 3328; // 64bit * 54 > 10^1000 static const size_t kBitCount = 3328; // 64bit * 54 > 10^1000
static const size_t kCapacity = kBitCount / sizeof(Type); static const size_t kCapacity = kBitCount / sizeof(Type);
static const size_t kTypeBit = sizeof(Type) * 8; static const size_t kTypeBit = sizeof(Type) * 8;
Type digits_[kCapacity]; Type digits_[kCapacity];
size_t count_; size_t count_;
}; };
} // namespace internal } // namespace internal

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available. // Tencent is pleased to support the open source community by making RapidJSON
// available.
// //
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved. // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
// //
// Licensed under the MIT License (the "License"); you may not use this file except // 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,42 +33,44 @@
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
inline uint32_t clzll(uint64_t x) { inline uint32_t clzll(uint64_t x) {
// Passing 0 to __builtin_clzll is UB in GCC and results in an // Passing 0 to __builtin_clzll is UB in GCC and results in an
// infinite loop in the software implementation. // infinite loop in the software implementation.
RAPIDJSON_ASSERT(x != 0); RAPIDJSON_ASSERT(x != 0);
#if defined(_MSC_VER) #if defined(_MSC_VER)
unsigned long r = 0; unsigned long r = 0;
#if defined(_WIN64) #if defined(_WIN64)
_BitScanReverse64(&r, x); _BitScanReverse64(&r, x);
#else #else
// Scan the high 32 bits. // Scan the high 32 bits.
if (_BitScanReverse(&r, static_cast<uint32_t>(x >> 32))) if (_BitScanReverse(&r, static_cast<uint32_t>(x >> 32)))
return 63 - (r + 32); return 63 - (r + 32);
// Scan the low 32 bits. // Scan the low 32 bits.
_BitScanReverse(&r, static_cast<uint32_t>(x & 0xFFFFFFFF)); _BitScanReverse(&r, static_cast<uint32_t>(x & 0xFFFFFFFF));
#endif // _WIN64 #endif // _WIN64
return 63 - r; return 63 - r;
#else #else
uint32_t r; uint32_t r;
while (!(x & (static_cast<uint64_t>(1) << 63))) { while (!(x & (static_cast<uint64_t>(1) << 63))) {
x <<= 1; x <<= 1;
++r; ++r;
} }
return r; return r;
#endif // _MSC_VER #endif // _MSC_VER
} }
#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

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

@@ -1,20 +1,24 @@
// Tencent is pleased to support the open source community by making RapidJSON available. // Tencent is pleased to support the open source community by making RapidJSON
// available.
// //
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved. // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
// //
// Licensed under the MIT License (the "License"); you may not use this file except // 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_
@@ -42,204 +46,250 @@ RAPIDJSON_DIAG_OFF(padded)
#endif #endif
struct DiyFp { struct DiyFp {
DiyFp() : f(), e() {} DiyFp() : f(), e() {}
DiyFp(uint64_t fp, int exp) : f(fp), e(exp) {} DiyFp(uint64_t fp, int exp) : f(fp), e(exp) {}
explicit DiyFp(double d) { explicit DiyFp(double d) {
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 =
uint64_t significand = (u.u64 & kDpSignificandMask); static_cast<int>((u.u64 & kDpExponentMask) >> kDpSignificandSize);
if (biased_e != 0) { uint64_t significand = (u.u64 & kDpSignificandMask);
f = significand + kDpHiddenBit; if (biased_e != 0) {
e = biased_e - kDpExponentBias; f = significand + kDpHiddenBit;
} 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)) && \
__extension__ typedef unsigned __int128 uint128; defined(__x86_64__)
uint128 p = static_cast<uint128>(f) * static_cast<uint128>(rhs.f); __extension__ typedef unsigned __int128 uint128;
uint64_t h = static_cast<uint64_t>(p >> 64); uint128 p = static_cast<uint128>(f) * static_cast<uint128>(rhs.f);
uint64_t l = static_cast<uint64_t>(p); uint64_t h = static_cast<uint64_t>(p >> 64);
if (l & (uint64_t(1) << 63)) // rounding uint64_t l = static_cast<uint64_t>(p);
h++; if (l & (uint64_t(1) << 63)) // rounding
return DiyFp(h, e + rhs.e + 64); h++;
return DiyFp(h, e + rhs.e + 64);
#else #else
const uint64_t M32 = 0xFFFFFFFF; const uint64_t M32 = 0xFFFFFFFF;
const uint64_t a = f >> 32; const uint64_t a = f >> 32;
const uint64_t b = f & M32; const uint64_t b = f & M32;
const uint64_t c = rhs.f >> 32; const uint64_t c = rhs.f >> 32;
const uint64_t d = rhs.f & M32; const uint64_t d = rhs.f & M32;
const uint64_t ac = a * c; const uint64_t ac = a * c;
const uint64_t bc = b * c; const uint64_t bc = b * c;
const uint64_t ad = a * d; const uint64_t ad = a * d;
const uint64_t bd = b * d; const uint64_t bd = b * d;
uint64_t tmp = (bd >> 32) + (ad & M32) + (bc & M32); uint64_t tmp = (bd >> 32) + (ad & M32) + (bc & M32);
tmp += 1U << 31; /// mult_round tmp += 1U << 31; /// mult_round
return DiyFp(ac + (ad >> 32) + (bc >> 32) + (tmp >> 32), e + rhs.e + 64); return DiyFp(ac + (ad >> 32) + (bc >> 32) + (tmp >> 32), e + rhs.e + 64);
#endif #endif
}
DiyFp Normalize() const {
int s = static_cast<int>(RAPIDJSON_CLZLL(f));
return DiyFp(f << s, e - s);
}
DiyFp NormalizeBoundary() const {
DiyFp res = *this;
while (!(res.f & (kDpHiddenBit << 1))) {
res.f <<= 1;
res.e--;
} }
res.f <<= (kDiySignificandSize - kDpSignificandSize - 2);
res.e = res.e - (kDiySignificandSize - kDpSignificandSize - 2);
return res;
}
DiyFp Normalize() const { void NormalizedBoundaries(DiyFp *minus, DiyFp *plus) const {
int s = static_cast<int>(RAPIDJSON_CLZLL(f)); DiyFp pl = DiyFp((f << 1) + 1, e - 1).NormalizeBoundary();
return DiyFp(f << s, e - s); DiyFp mi = (f == kDpHiddenBit) ? DiyFp((f << 2) - 1, e - 2)
: DiyFp((f << 1) - 1, e - 1);
mi.f <<= mi.e - pl.e;
mi.e = pl.e;
*plus = pl;
*minus = mi;
}
double ToDouble() const {
union {
double d;
uint64_t u64;
} u;
RAPIDJSON_ASSERT(f <= kDpHiddenBit + kDpSignificandMask);
if (e < kDpDenormalExponent) {
// Underflow.
return 0.0;
} }
if (e >= kDpMaxExponent) {
DiyFp NormalizeBoundary() const { // Overflow.
DiyFp res = *this; return std::numeric_limits<double>::infinity();
while (!(res.f & (kDpHiddenBit << 1))) {
res.f <<= 1;
res.e--;
}
res.f <<= (kDiySignificandSize - kDpSignificandSize - 2);
res.e = res.e - (kDiySignificandSize - kDpSignificandSize - 2);
return res;
} }
const uint64_t be = (e == kDpDenormalExponent && (f & kDpHiddenBit) == 0)
? 0
: static_cast<uint64_t>(e + kDpExponentBias);
u.u64 = (f & kDpSignificandMask) | (be << kDpSignificandSize);
return u.d;
}
void NormalizedBoundaries(DiyFp* minus, DiyFp* plus) const { static const int kDiySignificandSize = 64;
DiyFp pl = DiyFp((f << 1) + 1, e - 1).NormalizeBoundary(); static const int kDpSignificandSize = 52;
DiyFp mi = (f == kDpHiddenBit) ? DiyFp((f << 2) - 1, e - 2) : DiyFp((f << 1) - 1, e - 1); static const int kDpExponentBias = 0x3FF + kDpSignificandSize;
mi.f <<= mi.e - pl.e; static const int kDpMaxExponent = 0x7FF - kDpExponentBias;
mi.e = pl.e; static const int kDpMinExponent = -kDpExponentBias;
*plus = pl; static const int kDpDenormalExponent = -kDpExponentBias + 1;
*minus = mi; static const uint64_t kDpExponentMask =
} RAPIDJSON_UINT64_C2(0x7FF00000, 0x00000000);
static const uint64_t kDpSignificandMask =
RAPIDJSON_UINT64_C2(0x000FFFFF, 0xFFFFFFFF);
static const uint64_t kDpHiddenBit =
RAPIDJSON_UINT64_C2(0x00100000, 0x00000000);
double ToDouble() const { uint64_t f;
union { int e;
double d;
uint64_t u64;
}u;
RAPIDJSON_ASSERT(f <= kDpHiddenBit + kDpSignificandMask);
if (e < kDpDenormalExponent) {
// Underflow.
return 0.0;
}
if (e >= kDpMaxExponent) {
// Overflow.
return std::numeric_limits<double>::infinity();
}
const uint64_t be = (e == kDpDenormalExponent && (f & kDpHiddenBit) == 0) ? 0 :
static_cast<uint64_t>(e + kDpExponentBias);
u.u64 = (f & kDpSignificandMask) | (be << kDpSignificandSize);
return u.d;
}
static const int kDiySignificandSize = 64;
static const int kDpSignificandSize = 52;
static const int kDpExponentBias = 0x3FF + kDpSignificandSize;
static const int kDpMaxExponent = 0x7FF - kDpExponentBias;
static const int kDpMinExponent = -kDpExponentBias;
static const int kDpDenormalExponent = -kDpExponentBias + 1;
static const uint64_t kDpExponentMask = RAPIDJSON_UINT64_C2(0x7FF00000, 0x00000000);
static const uint64_t kDpSignificandMask = RAPIDJSON_UINT64_C2(0x000FFFFF, 0xFFFFFFFF);
static const uint64_t kDpHiddenBit = RAPIDJSON_UINT64_C2(0x00100000, 0x00000000);
uint64_t f;
int e;
}; };
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),
static const int16_t kCachedPowers_E[] = { RAPIDJSON_UINT64_C2(0xe8d4a510, 0x00000000),
-1220, -1193, -1166, -1140, -1113, -1087, -1060, -1034, -1007, -980, RAPIDJSON_UINT64_C2(0xad78ebc5, 0xac620000),
-954, -927, -901, -874, -847, -821, -794, -768, -741, -715, RAPIDJSON_UINT64_C2(0x813f3978, 0xf8940984),
-688, -661, -635, -608, -582, -555, -529, -502, -475, -449, RAPIDJSON_UINT64_C2(0xc097ce7b, 0xc90715b3),
-422, -396, -369, -343, -316, -289, -263, -236, -210, -183, RAPIDJSON_UINT64_C2(0x8f7e32ce, 0x7bea5c70),
-157, -130, -103, -77, -50, -24, 3, 30, 56, 83, RAPIDJSON_UINT64_C2(0xd5d238a4, 0xabe98068),
109, 136, 162, 189, 216, 242, 269, 295, 322, 348, RAPIDJSON_UINT64_C2(0x9f4f2726, 0x179a2245),
375, 402, 428, 455, 481, 508, 534, 561, 588, 614, RAPIDJSON_UINT64_C2(0xed63a231, 0xd4c4fb27),
641, 667, 694, 720, 747, 774, 800, 827, 853, 880, RAPIDJSON_UINT64_C2(0xb0de6538, 0x8cc8ada8),
907, 933, 960, 986, 1013, 1039, 1066 RAPIDJSON_UINT64_C2(0x83c7088e, 0x1aab65db),
}; RAPIDJSON_UINT64_C2(0xc45d1df9, 0x42711d9a),
RAPIDJSON_ASSERT(index < 87); RAPIDJSON_UINT64_C2(0x924d692c, 0xa61be758),
return DiyFp(kCachedPowers_F[index], kCachedPowers_E[index]); RAPIDJSON_UINT64_C2(0xda01ee64, 0x1a708dea),
RAPIDJSON_UINT64_C2(0xa26da399, 0x9aef774a),
RAPIDJSON_UINT64_C2(0xf209787b, 0xb47d6b85),
RAPIDJSON_UINT64_C2(0xb454e4a1, 0x79dd1877),
RAPIDJSON_UINT64_C2(0x865b8692, 0x5b9bc5c2),
RAPIDJSON_UINT64_C2(0xc83553c5, 0xc8965d3d),
RAPIDJSON_UINT64_C2(0x952ab45c, 0xfa97a0b3),
RAPIDJSON_UINT64_C2(0xde469fbd, 0x99a05fe3),
RAPIDJSON_UINT64_C2(0xa59bc234, 0xdb398c25),
RAPIDJSON_UINT64_C2(0xf6c69a72, 0xa3989f5c),
RAPIDJSON_UINT64_C2(0xb7dcbf53, 0x54e9bece),
RAPIDJSON_UINT64_C2(0x88fcf317, 0xf22241e2),
RAPIDJSON_UINT64_C2(0xcc20ce9b, 0xd35c78a5),
RAPIDJSON_UINT64_C2(0x98165af3, 0x7b2153df),
RAPIDJSON_UINT64_C2(0xe2a0b5dc, 0x971f303a),
RAPIDJSON_UINT64_C2(0xa8d9d153, 0x5ce3b396),
RAPIDJSON_UINT64_C2(0xfb9b7cd9, 0xa4a7443c),
RAPIDJSON_UINT64_C2(0xbb764c4c, 0xa7a44410),
RAPIDJSON_UINT64_C2(0x8bab8eef, 0xb6409c1a),
RAPIDJSON_UINT64_C2(0xd01fef10, 0xa657842c),
RAPIDJSON_UINT64_C2(0x9b10a4e5, 0xe9913129),
RAPIDJSON_UINT64_C2(0xe7109bfb, 0xa19c0c9d),
RAPIDJSON_UINT64_C2(0xac2820d9, 0x623bf429),
RAPIDJSON_UINT64_C2(0x80444b5e, 0x7aa7cf85),
RAPIDJSON_UINT64_C2(0xbf21e440, 0x03acdd2d),
RAPIDJSON_UINT64_C2(0x8e679c2f, 0x5e44ff8f),
RAPIDJSON_UINT64_C2(0xd433179d, 0x9c8cb841),
RAPIDJSON_UINT64_C2(0x9e19db92, 0xb4e31ba9),
RAPIDJSON_UINT64_C2(0xeb96bf6e, 0xbadf77d9),
RAPIDJSON_UINT64_C2(0xaf87023b, 0x9bf0ee6b)};
static const int16_t kCachedPowers_E[] = {
-1220, -1193, -1166, -1140, -1113, -1087, -1060, -1034, -1007, -980, -954,
-927, -901, -874, -847, -821, -794, -768, -741, -715, -688, -661,
-635, -608, -582, -555, -529, -502, -475, -449, -422, -396, -369,
-343, -316, -289, -263, -236, -210, -183, -157, -130, -103, -77,
-50, -24, 3, 30, 56, 83, 109, 136, 162, 189, 216,
242, 269, 295, 322, 348, 375, 402, 428, 455, 481, 508,
534, 561, 588, 614, 641, 667, 694, 720, 747, 774, 800,
827, 853, 880, 907, 933, 960, 986, 1013, 1039, 1066};
RAPIDJSON_ASSERT(index < 87);
return DiyFp(kCachedPowers_F[index], kCachedPowers_E[index]);
} }
inline DiyFp GetCachedPower(int e, int* K) { inline DiyFp GetCachedPower(int e, int *K) {
//int k = static_cast<int>(ceil((-61 - e) * 0.30102999566398114)) + 374; // 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 +
int k = static_cast<int>(dk); 347; // dk must be positive, so can do ceiling in positive
if (dk - k > 0.0) int k = static_cast<int>(dk);
k++; if (dk - k > 0.0)
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);
} }
inline DiyFp GetCachedPower10(int exp, int *outExp) { inline DiyFp GetCachedPower10(int exp, int *outExp) {
RAPIDJSON_ASSERT(exp >= -348); RAPIDJSON_ASSERT(exp >= -348);
unsigned index = static_cast<unsigned>(exp + 348) / 8u; unsigned index = static_cast<unsigned>(exp + 348) / 8u;
*outExp = -348 + static_cast<int>(index) * 8; *outExp = -348 + static_cast<int>(index) * 8;
return GetCachedPowerByIndex(index); return GetCachedPowerByIndex(index);
} }
#ifdef __GNUC__ #ifdef __GNUC__

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

@@ -1,27 +1,31 @@
// Tencent is pleased to support the open source community by making RapidJSON available. // 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.
// //
// Licensed under the MIT License (the "License"); you may not use this file except // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// in compliance with the License. You may obtain a copy of the License at // rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
// //
// http://opensource.org/licenses/MIT // 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,210 +33,245 @@ 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,
while (rest < wp_w && delta - rest >= ten_kappa && uint64_t ten_kappa, uint64_t wp_w) {
(rest + ten_kappa < wp_w || /// closer while (rest < wp_w && delta - rest >= ten_kappa &&
wp_w - rest > rest + ten_kappa - wp_w)) { (rest + ten_kappa < wp_w || /// closer
buffer[len - 1]--; wp_w - rest > rest + ten_kappa - wp_w)) {
rest += ten_kappa; buffer[len - 1]--;
} rest += ten_kappa;
}
} }
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)
// Will not reach 10 digits in DigitGen() return 4;
//if (n < 1000000000) return 9; if (n < 100000)
//return 10; return 5;
return 9; if (n < 1000000)
return 6;
if (n < 10000000)
return 7;
if (n < 100000000)
return 8;
// Will not reach 10 digits in DigitGen()
// if (n < 1000000000) return 9;
// return 10;
return 9;
} }
inline void DigitGen(const DiyFp& W, const DiyFp& Mp, uint64_t delta, char* buffer, int* len, int* K) { 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) {
const DiyFp one(uint64_t(1) << -Mp.e, Mp.e); static const uint32_t kPow10[] = {1, 10, 100, 1000,
const DiyFp wp_w = Mp - W; 10000, 100000, 1000000, 10000000,
uint32_t p1 = static_cast<uint32_t>(Mp.f >> -one.e); 100000000, 1000000000};
uint64_t p2 = Mp.f & (one.f - 1); const DiyFp one(uint64_t(1) << -Mp.e, Mp.e);
int kappa = CountDecimalDigit32(p1); // kappa in [0, 9] const DiyFp wp_w = Mp - W;
*len = 0; uint32_t p1 = static_cast<uint32_t>(Mp.f >> -one.e);
uint64_t p2 = Mp.f & (one.f - 1);
int kappa = CountDecimalDigit32(p1); // kappa in [0, 9]
*len = 0;
while (kappa > 0) { 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:
default:; d = p1 / 1000000;
} p1 %= 1000000;
if (d || *len) break;
buffer[(*len)++] = static_cast<char>('0' + static_cast<char>(d)); case 6:
kappa--; d = p1 / 100000;
uint64_t tmp = (static_cast<uint64_t>(p1) << -one.e) + p2; p1 %= 100000;
if (tmp <= delta) { break;
*K += kappa; case 5:
GrisuRound(buffer, *len, delta, tmp, static_cast<uint64_t>(kPow10[kappa]) << -one.e, wp_w.f); d = p1 / 10000;
return; p1 %= 10000;
} break;
case 4:
d = p1 / 1000;
p1 %= 1000;
break;
case 3:
d = p1 / 100;
p1 %= 100;
break;
case 2:
d = p1 / 10;
p1 %= 10;
break;
case 1:
d = p1;
p1 = 0;
break;
default:;
} }
if (d || *len)
buffer[(*len)++] = static_cast<char>('0' + static_cast<char>(d));
kappa--;
uint64_t tmp = (static_cast<uint64_t>(p1) << -one.e) + p2;
if (tmp <= delta) {
*K += kappa;
GrisuRound(buffer, *len, delta, tmp,
static_cast<uint64_t>(kPow10[kappa]) << -one.e, wp_w.f);
return;
}
}
// kappa = 0 // kappa = 0
for (;;) { for (;;) {
p2 *= 10; p2 *= 10;
delta *= 10; delta *= 10;
char d = static_cast<char>(p2 >> -one.e); char d = static_cast<char>(p2 >> -one.e);
if (d || *len) if (d || *len)
buffer[(*len)++] = static_cast<char>('0' + d); buffer[(*len)++] = static_cast<char>('0' + d);
p2 &= one.f - 1; p2 &= one.f - 1;
kappa--; kappa--;
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,
return; wp_w.f * (index < 9 ? kPow10[index] : 0));
} 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);
const DiyFp c_mk = GetCachedPower(w_p.e, K); const DiyFp c_mk = GetCachedPower(w_p.e, K);
const DiyFp W = v.Normalize() * c_mk; const DiyFp W = v.Normalize() * c_mk;
DiyFp Wp = w_p * c_mk; DiyFp Wp = w_p * c_mk;
DiyFp Wm = w_m * c_mk; DiyFp Wm = w_m * c_mk;
Wm.f++; Wm.f++;
Wp.f--; Wp.f--;
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;
} }
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
} *buffer++ = static_cast<char>('0' + static_cast<char>(K));
else
*buffer++ = static_cast<char>('0' + static_cast<char>(K));
return buffer; return buffer;
} }
inline char* Prettify(char* buffer, int length, int k, int maxDecimalPlaces) { 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) {
// 1234e7 -> 12340000000 // 1234e7 -> 12340000000
for (int i = length; i < kk; i++) for (int i = length; i < kk; i++)
buffer[i] = '0'; buffer[i] = '0';
buffer[kk] = '.'; buffer[kk] = '.';
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],
std::memmove(&buffer[kk + 1], &buffer[kk], static_cast<size_t>(length - 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
// Remove extra trailing zeros (at least one) after truncation. // Remove extra trailing zeros (at least one) after truncation.
for (int i = kk + maxDecimalPlaces; i > kk + 1; i--) for (int i = kk + maxDecimalPlaces; i > kk + 1; i--)
if (buffer[i] != '0') if (buffer[i] != '0')
return &buffer[i + 1]; return &buffer[i + 1];
return &buffer[kk + 2]; // Reserve one zero return &buffer[kk + 2]; // Reserve one zero
} } else
else return &buffer[length + 1];
return &buffer[length + 1]; } else if (-6 < kk && kk <= 0) {
} // 1234e-6 -> 0.001234
else if (-6 < kk && kk <= 0) { const int offset = 2 - kk;
// 1234e-6 -> 0.001234 std::memmove(&buffer[offset], &buffer[0], static_cast<size_t>(length));
const int offset = 2 - kk; buffer[0] = '0';
std::memmove(&buffer[offset], &buffer[0], static_cast<size_t>(length)); buffer[1] = '.';
buffer[0] = '0'; for (int i = 2; i < offset; i++)
buffer[1] = '.'; buffer[i] = '0';
for (int i = 2; i < offset; i++) if (length - kk > maxDecimalPlaces) {
buffer[i] = '0'; // When maxDecimalPlaces = 2, 0.123 -> 0.12, 0.102 -> 0.1
if (length - kk > maxDecimalPlaces) { // Remove extra trailing zeros (at least one) after truncation.
// When maxDecimalPlaces = 2, 0.123 -> 0.12, 0.102 -> 0.1 for (int i = maxDecimalPlaces + 1; i > 2; i--)
// Remove extra trailing zeros (at least one) after truncation. if (buffer[i] != '0')
for (int i = maxDecimalPlaces + 1; i > 2; i--) return &buffer[i + 1];
if (buffer[i] != '0') return &buffer[3]; // Reserve one zero
return &buffer[i + 1]; } else
return &buffer[3]; // Reserve one zero return &buffer[length + offset];
} } else if (kk < -maxDecimalPlaces) {
else // Truncate to zero
return &buffer[length + offset]; buffer[0] = '0';
} buffer[1] = '.';
else if (kk < -maxDecimalPlaces) { buffer[2] = '0';
// Truncate to zero return &buffer[3];
buffer[0] = '0'; } else if (length == 1) {
buffer[1] = '.'; // 1e30
buffer[2] = '0'; buffer[1] = 'e';
return &buffer[3]; return WriteExponent(kk - 1, &buffer[2]);
} } else {
else if (length == 1) { // 1234e30 -> 1.234e33
// 1e30 std::memmove(&buffer[2], &buffer[1], static_cast<size_t>(length - 1));
buffer[1] = 'e'; buffer[1] = '.';
return WriteExponent(kk - 1, &buffer[2]); buffer[length + 1] = 'e';
} return WriteExponent(kk - 1, &buffer[0 + length + 2]);
else { }
// 1234e30 -> 1.234e33
std::memmove(&buffer[2], &buffer[1], static_cast<size_t>(length - 1));
buffer[1] = '.';
buffer[length + 1] = 'e';
return WriteExponent(kk - 1, &buffer[0 + length + 2]);
}
} }
inline char* dtoa(double value, char* buffer, int maxDecimalPlaces = 324) { 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()) {
if (d.Sign()) if (d.Sign())
*buffer++ = '-'; // -0.0, Issue #289 *buffer++ = '-'; // -0.0, Issue #289
buffer[0] = '0'; buffer[0] = '0';
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;
}
int length, K;
Grisu2(value, buffer, &length, &K);
return Prettify(buffer, length, K, maxDecimalPlaces);
} }
int length, K;
Grisu2(value, buffer, &length, &K);
return Prettify(buffer, length, K, maxDecimalPlaces);
}
} }
#ifdef __GNUC__ #ifdef __GNUC__

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available. // 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.
// //
// Licensed under the MIT License (the "License"); you may not use this file except // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// in compliance with the License. You may obtain a copy of the License at // rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
// //
// http://opensource.org/licenses/MIT // 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_
@@ -22,54 +26,72 @@ namespace internal {
class Double { class Double {
public: public:
Double() {} Double() {}
Double(double d) : d_(d) {} Double(double d) : d_(d) {}
Double(uint64_t u) : u_(u) {} Double(uint64_t u) : u_(u) {}
double Value() const { return d_; } double Value() const { return d_; }
uint64_t Uint64Value() const { return u_; } uint64_t Uint64Value() const { return u_; }
double NextPositiveDouble() const { double NextPositiveDouble() const {
RAPIDJSON_ASSERT(!Sign()); RAPIDJSON_ASSERT(!Sign());
return Double(u_ + 1).Value(); return Double(u_ + 1).Value();
} }
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 IsNanOrInf() const { return (u_ & kExponentMask) == kExponentMask; } }
bool IsNormal() const { return (u_ & kExponentMask) != 0 || Significand() == 0; } bool IsInf() const {
bool IsZero() const { return (u_ & (kExponentMask | kSignificandMask)) == 0; } return (u_ & kExponentMask) == kExponentMask && Significand() == 0;
}
bool IsNanOrInf() const { return (u_ & kExponentMask) == kExponentMask; }
bool IsNormal() const {
return (u_ & kExponentMask) != 0 || Significand() == 0;
}
bool IsZero() const { return (u_ & (kExponentMask | kSignificandMask)) == 0; }
uint64_t IntegerSignificand() const { return IsNormal() ? Significand() | kHiddenBit : Significand(); } 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)
return 53; return 53;
else if (order <= -1074) else if (order <= -1074)
return 0; return 0;
else else
return order + 1074; return order + 1074;
} }
private: private:
static const int kSignificandSize = 52; static const int kSignificandSize = 52;
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_;
uint64_t u_; uint64_t u_;
}; };
}; };
} // namespace internal } // namespace internal

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available. // Tencent is pleased to support the open source community by making RapidJSON
// available.
// //
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved. // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
// //
// Licensed under the MIT License (the "License"); you may not use this file except // 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,286 +24,281 @@
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',
return cDigitsLut; '2', '8', '3', '8', '4', '8', '5', '8', '6', '8', '7', '8', '8', '8', '9',
'9', '0', '9', '1', '9', '2', '9', '3', '9', '4', '9', '5', '9', '6', '9',
'7', '9', '8', '9', '9'};
return cDigitsLut;
} }
inline char* u32toa(uint32_t value, char* buffer) { 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;
const uint32_t d2 = (value % 100) << 1; const uint32_t d2 = (value % 100) << 1;
if (value >= 1000) if (value >= 1000)
*buffer++ = cDigitsLut[d1]; *buffer++ = cDigitsLut[d1];
if (value >= 100) if (value >= 100)
*buffer++ = cDigitsLut[d1 + 1]; *buffer++ = cDigitsLut[d1 + 1];
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;
const uint32_t d1 = (b / 100) << 1; const uint32_t d1 = (b / 100) << 1;
const uint32_t d2 = (b % 100) << 1; const uint32_t d2 = (b % 100) << 1;
const uint32_t d3 = (c / 100) << 1; const uint32_t d3 = (c / 100) << 1;
const uint32_t d4 = (c % 100) << 1; const uint32_t d4 = (c % 100) << 1;
if (value >= 10000000) if (value >= 10000000)
*buffer++ = cDigitsLut[d1]; *buffer++ = cDigitsLut[d1];
if (value >= 1000000) if (value >= 1000000)
*buffer++ = cDigitsLut[d1 + 1]; *buffer++ = cDigitsLut[d1 + 1];
if (value >= 100000) if (value >= 100000)
*buffer++ = cDigitsLut[d2]; *buffer++ = cDigitsLut[d2];
*buffer++ = cDigitsLut[d2 + 1]; *buffer++ = cDigitsLut[d2 + 1];
*buffer++ = cDigitsLut[d3]; *buffer++ = cDigitsLut[d3];
*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
value %= 100000000; value %= 100000000;
if (a >= 10) { if (a >= 10) {
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
const uint32_t c = value % 10000; // 0 to 9999 const uint32_t c = value % 10000; // 0 to 9999
const uint32_t d1 = (b / 100) << 1; const uint32_t d1 = (b / 100) << 1;
const uint32_t d2 = (b % 100) << 1; const uint32_t d2 = (b % 100) << 1;
const uint32_t d3 = (c / 100) << 1; const uint32_t d3 = (c / 100) << 1;
const uint32_t d4 = (c % 100) << 1; const uint32_t d4 = (c % 100) << 1;
*buffer++ = cDigitsLut[d1];
*buffer++ = cDigitsLut[d1 + 1];
*buffer++ = cDigitsLut[d2];
*buffer++ = cDigitsLut[d2 + 1];
*buffer++ = cDigitsLut[d3];
*buffer++ = cDigitsLut[d3 + 1];
*buffer++ = cDigitsLut[d4];
*buffer++ = cDigitsLut[d4 + 1];
}
return buffer;
}
inline char *i32toa(int32_t value, char *buffer) {
RAPIDJSON_ASSERT(buffer != 0);
uint32_t u = static_cast<uint32_t>(value);
if (value < 0) {
*buffer++ = '-';
u = ~u + 1;
}
return u32toa(u, buffer);
}
inline char *u64toa(uint64_t value, char *buffer) {
RAPIDJSON_ASSERT(buffer != 0);
const char *cDigitsLut = GetDigitsLut();
const uint64_t kTen8 = 100000000;
const uint64_t kTen9 = kTen8 * 10;
const uint64_t kTen10 = kTen8 * 100;
const uint64_t kTen11 = kTen8 * 1000;
const uint64_t kTen12 = kTen8 * 10000;
const uint64_t kTen13 = kTen8 * 100000;
const uint64_t kTen14 = kTen8 * 1000000;
const uint64_t kTen15 = kTen8 * 10000000;
const uint64_t kTen16 = kTen8 * kTen8;
if (value < kTen8) {
uint32_t v = static_cast<uint32_t>(value);
if (v < 10000) {
const uint32_t d1 = (v / 100) << 1;
const uint32_t d2 = (v % 100) << 1;
if (v >= 1000)
*buffer++ = cDigitsLut[d1]; *buffer++ = cDigitsLut[d1];
if (v >= 100)
*buffer++ = cDigitsLut[d1 + 1]; *buffer++ = cDigitsLut[d1 + 1];
if (v >= 10)
*buffer++ = cDigitsLut[d2]; *buffer++ = cDigitsLut[d2];
*buffer++ = cDigitsLut[d2 + 1]; *buffer++ = cDigitsLut[d2 + 1];
*buffer++ = cDigitsLut[d3]; } else {
*buffer++ = cDigitsLut[d3 + 1]; // value = bbbbcccc
*buffer++ = cDigitsLut[d4]; const uint32_t b = v / 10000;
*buffer++ = cDigitsLut[d4 + 1]; const uint32_t c = v % 10000;
}
return buffer;
}
inline char* i32toa(int32_t value, char* buffer) { const uint32_t d1 = (b / 100) << 1;
RAPIDJSON_ASSERT(buffer != 0); const uint32_t d2 = (b % 100) << 1;
uint32_t u = static_cast<uint32_t>(value);
if (value < 0) {
*buffer++ = '-';
u = ~u + 1;
}
return u32toa(u, buffer); const uint32_t d3 = (c / 100) << 1;
} const uint32_t d4 = (c % 100) << 1;
inline char* u64toa(uint64_t value, char* buffer) {
RAPIDJSON_ASSERT(buffer != 0);
const char* cDigitsLut = GetDigitsLut();
const uint64_t kTen8 = 100000000;
const uint64_t kTen9 = kTen8 * 10;
const uint64_t kTen10 = kTen8 * 100;
const uint64_t kTen11 = kTen8 * 1000;
const uint64_t kTen12 = kTen8 * 10000;
const uint64_t kTen13 = kTen8 * 100000;
const uint64_t kTen14 = kTen8 * 1000000;
const uint64_t kTen15 = kTen8 * 10000000;
const uint64_t kTen16 = kTen8 * kTen8;
if (value < kTen8) {
uint32_t v = static_cast<uint32_t>(value);
if (v < 10000) {
const uint32_t d1 = (v / 100) << 1;
const uint32_t d2 = (v % 100) << 1;
if (v >= 1000)
*buffer++ = cDigitsLut[d1];
if (v >= 100)
*buffer++ = cDigitsLut[d1 + 1];
if (v >= 10)
*buffer++ = cDigitsLut[d2];
*buffer++ = cDigitsLut[d2 + 1];
}
else {
// value = bbbbcccc
const uint32_t b = v / 10000;
const uint32_t c = v % 10000;
const uint32_t d1 = (b / 100) << 1;
const uint32_t d2 = (b % 100) << 1;
const uint32_t d3 = (c / 100) << 1;
const uint32_t d4 = (c % 100) << 1;
if (value >= 10000000)
*buffer++ = cDigitsLut[d1];
if (value >= 1000000)
*buffer++ = cDigitsLut[d1 + 1];
if (value >= 100000)
*buffer++ = cDigitsLut[d2];
*buffer++ = cDigitsLut[d2 + 1];
*buffer++ = cDigitsLut[d3];
*buffer++ = cDigitsLut[d3 + 1];
*buffer++ = cDigitsLut[d4];
*buffer++ = cDigitsLut[d4 + 1];
}
}
else if (value < kTen16) {
const uint32_t v0 = static_cast<uint32_t>(value / kTen8);
const uint32_t v1 = static_cast<uint32_t>(value % kTen8);
const uint32_t b0 = v0 / 10000;
const uint32_t c0 = v0 % 10000;
const uint32_t d1 = (b0 / 100) << 1;
const uint32_t d2 = (b0 % 100) << 1;
const uint32_t d3 = (c0 / 100) << 1;
const uint32_t d4 = (c0 % 100) << 1;
const uint32_t b1 = v1 / 10000;
const uint32_t c1 = v1 % 10000;
const uint32_t d5 = (b1 / 100) << 1;
const uint32_t d6 = (b1 % 100) << 1;
const uint32_t d7 = (c1 / 100) << 1;
const uint32_t d8 = (c1 % 100) << 1;
if (value >= kTen15)
*buffer++ = cDigitsLut[d1];
if (value >= kTen14)
*buffer++ = cDigitsLut[d1 + 1];
if (value >= kTen13)
*buffer++ = cDigitsLut[d2];
if (value >= kTen12)
*buffer++ = cDigitsLut[d2 + 1];
if (value >= kTen11)
*buffer++ = cDigitsLut[d3];
if (value >= kTen10)
*buffer++ = cDigitsLut[d3 + 1];
if (value >= kTen9)
*buffer++ = cDigitsLut[d4];
*buffer++ = cDigitsLut[d4 + 1];
*buffer++ = cDigitsLut[d5];
*buffer++ = cDigitsLut[d5 + 1];
*buffer++ = cDigitsLut[d6];
*buffer++ = cDigitsLut[d6 + 1];
*buffer++ = cDigitsLut[d7];
*buffer++ = cDigitsLut[d7 + 1];
*buffer++ = cDigitsLut[d8];
*buffer++ = cDigitsLut[d8 + 1];
}
else {
const uint32_t a = static_cast<uint32_t>(value / kTen16); // 1 to 1844
value %= kTen16;
if (a < 10)
*buffer++ = static_cast<char>('0' + static_cast<char>(a));
else if (a < 100) {
const uint32_t i = a << 1;
*buffer++ = cDigitsLut[i];
*buffer++ = cDigitsLut[i + 1];
}
else if (a < 1000) {
*buffer++ = static_cast<char>('0' + static_cast<char>(a / 100));
const uint32_t i = (a % 100) << 1;
*buffer++ = cDigitsLut[i];
*buffer++ = cDigitsLut[i + 1];
}
else {
const uint32_t i = (a / 100) << 1;
const uint32_t j = (a % 100) << 1;
*buffer++ = cDigitsLut[i];
*buffer++ = cDigitsLut[i + 1];
*buffer++ = cDigitsLut[j];
*buffer++ = cDigitsLut[j + 1];
}
const uint32_t v0 = static_cast<uint32_t>(value / kTen8);
const uint32_t v1 = static_cast<uint32_t>(value % kTen8);
const uint32_t b0 = v0 / 10000;
const uint32_t c0 = v0 % 10000;
const uint32_t d1 = (b0 / 100) << 1;
const uint32_t d2 = (b0 % 100) << 1;
const uint32_t d3 = (c0 / 100) << 1;
const uint32_t d4 = (c0 % 100) << 1;
const uint32_t b1 = v1 / 10000;
const uint32_t c1 = v1 % 10000;
const uint32_t d5 = (b1 / 100) << 1;
const uint32_t d6 = (b1 % 100) << 1;
const uint32_t d7 = (c1 / 100) << 1;
const uint32_t d8 = (c1 % 100) << 1;
if (value >= 10000000)
*buffer++ = cDigitsLut[d1]; *buffer++ = cDigitsLut[d1];
if (value >= 1000000)
*buffer++ = cDigitsLut[d1 + 1]; *buffer++ = cDigitsLut[d1 + 1];
if (value >= 100000)
*buffer++ = cDigitsLut[d2]; *buffer++ = cDigitsLut[d2];
*buffer++ = cDigitsLut[d2 + 1]; *buffer++ = cDigitsLut[d2 + 1];
*buffer++ = cDigitsLut[d3];
*buffer++ = cDigitsLut[d3 + 1]; *buffer++ = cDigitsLut[d3];
*buffer++ = cDigitsLut[d4]; *buffer++ = cDigitsLut[d3 + 1];
*buffer++ = cDigitsLut[d4 + 1]; *buffer++ = cDigitsLut[d4];
*buffer++ = cDigitsLut[d5]; *buffer++ = cDigitsLut[d4 + 1];
*buffer++ = cDigitsLut[d5 + 1]; }
*buffer++ = cDigitsLut[d6]; } else if (value < kTen16) {
*buffer++ = cDigitsLut[d6 + 1]; const uint32_t v0 = static_cast<uint32_t>(value / kTen8);
*buffer++ = cDigitsLut[d7]; const uint32_t v1 = static_cast<uint32_t>(value % kTen8);
*buffer++ = cDigitsLut[d7 + 1];
*buffer++ = cDigitsLut[d8]; const uint32_t b0 = v0 / 10000;
*buffer++ = cDigitsLut[d8 + 1]; const uint32_t c0 = v0 % 10000;
const uint32_t d1 = (b0 / 100) << 1;
const uint32_t d2 = (b0 % 100) << 1;
const uint32_t d3 = (c0 / 100) << 1;
const uint32_t d4 = (c0 % 100) << 1;
const uint32_t b1 = v1 / 10000;
const uint32_t c1 = v1 % 10000;
const uint32_t d5 = (b1 / 100) << 1;
const uint32_t d6 = (b1 % 100) << 1;
const uint32_t d7 = (c1 / 100) << 1;
const uint32_t d8 = (c1 % 100) << 1;
if (value >= kTen15)
*buffer++ = cDigitsLut[d1];
if (value >= kTen14)
*buffer++ = cDigitsLut[d1 + 1];
if (value >= kTen13)
*buffer++ = cDigitsLut[d2];
if (value >= kTen12)
*buffer++ = cDigitsLut[d2 + 1];
if (value >= kTen11)
*buffer++ = cDigitsLut[d3];
if (value >= kTen10)
*buffer++ = cDigitsLut[d3 + 1];
if (value >= kTen9)
*buffer++ = cDigitsLut[d4];
*buffer++ = cDigitsLut[d4 + 1];
*buffer++ = cDigitsLut[d5];
*buffer++ = cDigitsLut[d5 + 1];
*buffer++ = cDigitsLut[d6];
*buffer++ = cDigitsLut[d6 + 1];
*buffer++ = cDigitsLut[d7];
*buffer++ = cDigitsLut[d7 + 1];
*buffer++ = cDigitsLut[d8];
*buffer++ = cDigitsLut[d8 + 1];
} else {
const uint32_t a = static_cast<uint32_t>(value / kTen16); // 1 to 1844
value %= kTen16;
if (a < 10)
*buffer++ = static_cast<char>('0' + static_cast<char>(a));
else if (a < 100) {
const uint32_t i = a << 1;
*buffer++ = cDigitsLut[i];
*buffer++ = cDigitsLut[i + 1];
} else if (a < 1000) {
*buffer++ = static_cast<char>('0' + static_cast<char>(a / 100));
const uint32_t i = (a % 100) << 1;
*buffer++ = cDigitsLut[i];
*buffer++ = cDigitsLut[i + 1];
} else {
const uint32_t i = (a / 100) << 1;
const uint32_t j = (a % 100) << 1;
*buffer++ = cDigitsLut[i];
*buffer++ = cDigitsLut[i + 1];
*buffer++ = cDigitsLut[j];
*buffer++ = cDigitsLut[j + 1];
} }
return buffer; const uint32_t v0 = static_cast<uint32_t>(value / kTen8);
const uint32_t v1 = static_cast<uint32_t>(value % kTen8);
const uint32_t b0 = v0 / 10000;
const uint32_t c0 = v0 % 10000;
const uint32_t d1 = (b0 / 100) << 1;
const uint32_t d2 = (b0 % 100) << 1;
const uint32_t d3 = (c0 / 100) << 1;
const uint32_t d4 = (c0 % 100) << 1;
const uint32_t b1 = v1 / 10000;
const uint32_t c1 = v1 % 10000;
const uint32_t d5 = (b1 / 100) << 1;
const uint32_t d6 = (b1 % 100) << 1;
const uint32_t d7 = (c1 / 100) << 1;
const uint32_t d8 = (c1 % 100) << 1;
*buffer++ = cDigitsLut[d1];
*buffer++ = cDigitsLut[d1 + 1];
*buffer++ = cDigitsLut[d2];
*buffer++ = cDigitsLut[d2 + 1];
*buffer++ = cDigitsLut[d3];
*buffer++ = cDigitsLut[d3 + 1];
*buffer++ = cDigitsLut[d4];
*buffer++ = cDigitsLut[d4 + 1];
*buffer++ = cDigitsLut[d5];
*buffer++ = cDigitsLut[d5 + 1];
*buffer++ = cDigitsLut[d6];
*buffer++ = cDigitsLut[d6 + 1];
*buffer++ = cDigitsLut[d7];
*buffer++ = cDigitsLut[d7 + 1];
*buffer++ = cDigitsLut[d8];
*buffer++ = cDigitsLut[d8 + 1];
}
return buffer;
} }
inline char* i64toa(int64_t value, char* buffer) { 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) {
*buffer++ = '-'; *buffer++ = '-';
u = ~u + 1; u = ~u + 1;
} }
return u64toa(u, buffer); return u64toa(u, buffer);
} }
} // namespace internal } // namespace internal

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available. // 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.
// //
// Licensed under the MIT License (the "License"); you may not use this file except // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// in compliance with the License. You may obtain a copy of the License at // rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
// //
// http://opensource.org/licenses/MIT // 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,48 +39,57 @@ 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; };
/////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////////
// BoolType, TrueType, FalseType // BoolType, TrueType, FalseType
// //
template <bool Cond> struct BoolType { template <bool Cond> struct BoolType {
static const bool Value = Cond; static const bool Value = Cond;
typedef BoolType Type; typedef BoolType Type;
}; };
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

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available. // 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.
// //
// Licensed under the MIT License (the "License"); you may not use this file except // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// in compliance with the License. You may obtain a copy of the License at // rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
// //
// http://opensource.org/licenses/MIT // 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,27 +30,45 @@ 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,
RAPIDJSON_ASSERT(n >= 0 && n <= 308); 1e+153, 1e+154, 1e+155, 1e+156, 1e+157, 1e+158, 1e+159, 1e+160, 1e+161,
return e[n]; 1e+162, 1e+163, 1e+164, 1e+165, 1e+166, 1e+167, 1e+168, 1e+169, 1e+170,
1e+171, 1e+172, 1e+173, 1e+174, 1e+175, 1e+176, 1e+177, 1e+178, 1e+179,
1e+180, 1e+181, 1e+182, 1e+183, 1e+184, 1e+185, 1e+186, 1e+187, 1e+188,
1e+189, 1e+190, 1e+191, 1e+192, 1e+193, 1e+194, 1e+195, 1e+196, 1e+197,
1e+198, 1e+199, 1e+200, 1e+201, 1e+202, 1e+203, 1e+204, 1e+205, 1e+206,
1e+207, 1e+208, 1e+209, 1e+210, 1e+211, 1e+212, 1e+213, 1e+214, 1e+215,
1e+216, 1e+217, 1e+218, 1e+219, 1e+220, 1e+221, 1e+222, 1e+223, 1e+224,
1e+225, 1e+226, 1e+227, 1e+228, 1e+229, 1e+230, 1e+231, 1e+232, 1e+233,
1e+234, 1e+235, 1e+236, 1e+237, 1e+238, 1e+239, 1e+240, 1e+241, 1e+242,
1e+243, 1e+244, 1e+245, 1e+246, 1e+247, 1e+248, 1e+249, 1e+250, 1e+251,
1e+252, 1e+253, 1e+254, 1e+255, 1e+256, 1e+257, 1e+258, 1e+259, 1e+260,
1e+261, 1e+262, 1e+263, 1e+264, 1e+265, 1e+266, 1e+267, 1e+268, 1e+269,
1e+270, 1e+271, 1e+272, 1e+273, 1e+274, 1e+275, 1e+276, 1e+277, 1e+278,
1e+279, 1e+280, 1e+281, 1e+282, 1e+283, 1e+284, 1e+285, 1e+286, 1e+287,
1e+288, 1e+289, 1e+290, 1e+291, 1e+292, 1e+293, 1e+294, 1e+295, 1e+296,
1e+297, 1e+298, 1e+299, 1e+300, 1e+301, 1e+302, 1e+303, 1e+304, 1e+305,
1e+306, 1e+307, 1e+308};
RAPIDJSON_ASSERT(n >= 0 && n <= 308);
return e[n];
} }
} // namespace internal } // namespace internal

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

File diff suppressed because it is too large Load Diff

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available. // 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.
// //
// Licensed under the MIT License (the "License"); you may not use this file except // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// in compliance with the License. You may obtain a copy of the License at // rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
// //
// http://opensource.org/licenses/MIT // 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,194 +36,182 @@ 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_), rhs.allocator_ = 0;
stackEnd_(rhs.stackEnd_), rhs.ownAllocator_ = 0;
initialCapacity_(rhs.initialCapacity_) rhs.stack_ = 0;
{ rhs.stackTop_ = 0;
rhs.allocator_ = 0; rhs.stackEnd_ = 0;
rhs.ownAllocator_ = 0; rhs.initialCapacity_ = 0;
rhs.stack_ = 0; }
rhs.stackTop_ = 0;
rhs.stackEnd_ = 0;
rhs.initialCapacity_ = 0;
}
#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_;
ownAllocator_ = rhs.ownAllocator_; ownAllocator_ = rhs.ownAllocator_;
stack_ = rhs.stack_; stack_ = rhs.stack_;
stackTop_ = rhs.stackTop_; stackTop_ = rhs.stackTop_;
stackEnd_ = rhs.stackEnd_; stackEnd_ = rhs.stackEnd_;
initialCapacity_ = rhs.initialCapacity_; initialCapacity_ = rhs.initialCapacity_;
rhs.allocator_ = 0; rhs.allocator_ = 0;
rhs.ownAllocator_ = 0; rhs.ownAllocator_ = 0;
rhs.stack_ = 0; rhs.stack_ = 0;
rhs.stackTop_ = 0; rhs.stackTop_ = 0;
rhs.stackEnd_ = 0; rhs.stackEnd_ = 0;
rhs.initialCapacity_ = 0; rhs.initialCapacity_ = 0;
}
return *this;
} }
return *this;
}
#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_);
internal::Swap(stackTop_, rhs.stackTop_); internal::Swap(stackTop_, rhs.stackTop_);
internal::Swap(stackEnd_, rhs.stackEnd_); internal::Swap(stackEnd_, rhs.stackEnd_);
internal::Swap(initialCapacity_, rhs.initialCapacity_); internal::Swap(initialCapacity_, rhs.initialCapacity_);
} }
void Clear() { stackTop_ = stack_; } void Clear() { stackTop_ = stack_; }
void ShrinkToFit() { void ShrinkToFit() {
if (Empty()) { if (Empty()) {
// If the stack is empty, completely deallocate the memory. // If the stack is empty, completely deallocate the memory.
Allocator::Free(stack_); // NOLINT (+clang-analyzer-unix.Malloc) Allocator::Free(stack_); // NOLINT (+clang-analyzer-unix.Malloc)
stack_ = 0; 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) >
Expand<T>(count); (stackEnd_ - stackTop_)))
} 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) <=
RAPIDJSON_ASSERT(static_cast<std::ptrdiff_t>(sizeof(T) * count) <= (stackEnd_ - stackTop_)); (stackEnd_ - stackTop_));
T* ret = reinterpret_cast<T*>(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> template <typename T> T *Bottom() { return reinterpret_cast<T *>(stack_); }
T* Bottom() { return reinterpret_cast<T*>(stack_); }
template<typename T> template <typename T> const T *Bottom() const {
const T* Bottom() const { return reinterpret_cast<T*>(stack_); } return reinterpret_cast<T *>(stack_);
}
bool HasAllocator() const { bool HasAllocator() const { return allocator_ != 0; }
return allocator_ != 0;
}
Allocator& GetAllocator() { Allocator &GetAllocator() {
RAPIDJSON_ASSERT(allocator_); RAPIDJSON_ASSERT(allocator_);
return *allocator_; return *allocator_;
} }
bool Empty() const { return stackTop_ == stack_; } bool Empty() const { return stackTop_ == stack_; }
size_t GetSize() const { return static_cast<size_t>(stackTop_ - stack_); } size_t GetSize() const { return static_cast<size_t>(stackTop_ - stack_); }
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_)
ownAllocator_ = allocator_ = RAPIDJSON_NEW(Allocator)(); ownAllocator_ = allocator_ = RAPIDJSON_NEW(Allocator)();
newCapacity = initialCapacity_; newCapacity = initialCapacity_;
} else { } else {
newCapacity = GetCapacity(); newCapacity = GetCapacity();
newCapacity += (newCapacity + 1) / 2; newCapacity += (newCapacity + 1) / 2;
}
size_t newSize = GetSize() + sizeof(T) * count;
if (newCapacity < newSize)
newCapacity = newSize;
Resize(newCapacity);
} }
size_t newSize = GetSize() + sizeof(T) * count;
if (newCapacity < newSize)
newCapacity = newSize;
void Resize(size_t newCapacity) { Resize(newCapacity);
const size_t size = GetSize(); // Backup the current size }
stack_ = static_cast<char*>(allocator_->Realloc(stack_, GetCapacity(), newCapacity));
stackTop_ = stack_ + size;
stackEnd_ = stack_ + newCapacity;
}
void Destroy() { void Resize(size_t newCapacity) {
Allocator::Free(stack_); const size_t size = GetSize(); // Backup the current size
RAPIDJSON_DELETE(ownAllocator_); // Only delete if it is owned by the stack stack_ = static_cast<char *>(
} allocator_->Realloc(stack_, GetCapacity(), newCapacity));
stackTop_ = stack_ + size;
stackEnd_ = stack_ + newCapacity;
}
// Prohibit copy constructor & assignment operator. void Destroy() {
Stack(const Stack&); Allocator::Free(stack_);
Stack& operator=(const Stack&); RAPIDJSON_DELETE(ownAllocator_); // Only delete if it is owned by the stack
}
Allocator* allocator_; // Prohibit copy constructor & assignment operator.
Allocator* ownAllocator_; Stack(const Stack &);
char *stack_; Stack &operator=(const Stack &);
char *stackTop_;
char *stackEnd_; Allocator *allocator_;
size_t initialCapacity_; Allocator *ownAllocator_;
char *stack_;
char *stackTop_;
char *stackEnd_;
size_t initialCapacity_;
}; };
} // namespace internal } // namespace internal

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available. // 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.
// //
// Licensed under the MIT License (the "License"); you may not use this file except // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// in compliance with the License. You may obtain a copy of the License at // rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
// //
// http://opensource.org/licenses/MIT // 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_
@@ -24,43 +28,43 @@ namespace internal {
//! Custom strlen() which works on different character types. //! Custom strlen() which works on different character types.
/*! \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)
while (*p) ++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,
RAPIDJSON_ASSERT(s != 0); SizeType *outCount) {
RAPIDJSON_ASSERT(outCount != 0); RAPIDJSON_ASSERT(s != 0);
GenericStringStream<Encoding> is(s); RAPIDJSON_ASSERT(outCount != 0);
const typename Encoding::Ch* end = s + length; GenericStringStream<Encoding> is(s);
SizeType count = 0; const typename Encoding::Ch *end = s + length;
while (is.src_ < end) { SizeType count = 0;
unsigned codepoint; while (is.src_ < end) {
if (!Encoding::Decode(is, &codepoint)) unsigned codepoint;
return false; if (!Encoding::Decode(is, &codepoint))
count++; return false;
} count++;
*outCount = count; }
return true; *outCount = count;
return true;
} }
} // namespace internal } // namespace internal

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

@@ -1,23 +1,27 @@
// Tencent is pleased to support the open source community by making RapidJSON available. // 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.
// //
// Licensed under the MIT License (the "License"); you may not use this file except // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// in compliance with the License. You may obtain a copy of the License at // rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
// //
// http://opensource.org/licenses/MIT // 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>
@@ -26,262 +30,275 @@ RAPIDJSON_NAMESPACE_BEGIN
namespace internal { namespace internal {
inline double FastPath(double significand, int exp) { inline double FastPath(double significand, int exp) {
if (exp < -308) if (exp < -308)
return 0.0; return 0.0;
else if (exp >= 0) else if (exp >= 0)
return significand * internal::Pow10(exp); return significand * internal::Pow10(exp);
else else
return significand / internal::Pow10(-exp); return significand / internal::Pow10(-exp);
} }
inline double StrtodNormalPrecision(double d, int p) { inline double StrtodNormalPrecision(double d, int p) {
if (p < -308) { if (p < -308) {
// 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)
if (m > b) m = b; m = b;
if (m > c) m = c; if (m > c)
return m; m = c;
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; hS_Exp5 -= dExp;
hS_Exp5 -= dExp; }
}
// Adjust for binary exponent // Adjust for binary exponent
if (bExp >= 0) if (bExp >= 0)
bS_Exp2 += bExp; bS_Exp2 += bExp;
else { else {
dS_Exp2 -= bExp; dS_Exp2 -= bExp;
hS_Exp2 -= bExp; hS_Exp2 -= bExp;
} }
// Adjust for half ulp exponent // Adjust for half ulp exponent
if (hExp >= 0) if (hExp >= 0)
hS_Exp2 += hExp; hS_Exp2 += hExp;
else { else {
dS_Exp2 -= hExp; dS_Exp2 -= hExp;
bS_Exp2 -= hExp; bS_Exp2 -= hExp;
} }
// Remove common power of two factor from all three scaled values // Remove common power of two factor from all three scaled values
int common_Exp2 = Min3(dS_Exp2, bS_Exp2, hS_Exp2); int common_Exp2 = Min3(dS_Exp2, bS_Exp2, hS_Exp2);
dS_Exp2 -= common_Exp2; dS_Exp2 -= common_Exp2;
bS_Exp2 -= common_Exp2; bS_Exp2 -= common_Exp2;
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);
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
if (p > 22 && p < 22 + 16) { // http://www.exploringbinary.com/fast-path-decimal-to-floating-point-conversion/
// Fast Path Cases In Disguise if (p > 22 && p < 22 + 16) {
d *= internal::Pow10(p - 22); // Fast Path Cases In Disguise
p = 22; d *= internal::Pow10(p - 22);
} p = 22;
}
if (p >= -22 && p <= 22 && d <= 9007199254740991.0) { // 2^53 - 1 if (p >= -22 && p <= 22 && d <= 9007199254740991.0) { // 2^53 - 1
*result = FastPath(d, p); *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,
uint64_t significand = 0; double *result) {
int i = 0; // 2^64 - 1 = 18446744073709551615, 1844674407370955161 = 0x1999999999999999 uint64_t significand = 0;
for (; i < dLen; i++) { int i = 0; // 2^64 - 1 = 18446744073709551615, 1844674407370955161 =
if (significand > RAPIDJSON_UINT64_C2(0x19999999, 0x99999999) || // 0x1999999999999999
(significand == RAPIDJSON_UINT64_C2(0x19999999, 0x99999999) && decimals[i] > '5')) for (; i < dLen; i++) {
break; if (significand > RAPIDJSON_UINT64_C2(0x19999999, 0x99999999) ||
significand = significand * 10u + static_cast<unsigned>(decimals[i] - '0'); (significand == RAPIDJSON_UINT64_C2(0x19999999, 0x99999999) &&
decimals[i] > '5'))
break;
significand = significand * 10u + static_cast<unsigned>(decimals[i] - '0');
}
if (i < dLen && decimals[i] >= '5') // Rounding
significand++;
int remaining = dLen - i;
const int kUlpShift = 3;
const int kUlp = 1 << kUlpShift;
int64_t error = (remaining == 0) ? 0 : kUlp / 2;
DiyFp v(significand, 0);
v = v.Normalize();
error <<= -v.e;
dExp += remaining;
int actualExp;
DiyFp cachedPower = GetCachedPower10(dExp, &actualExp);
if (actualExp != dExp) {
static const DiyFp kPow10[] = {
DiyFp(RAPIDJSON_UINT64_C2(0xa0000000, 0x00000000), -60), // 10^1
DiyFp(RAPIDJSON_UINT64_C2(0xc8000000, 0x00000000), -57), // 10^2
DiyFp(RAPIDJSON_UINT64_C2(0xfa000000, 0x00000000), -54), // 10^3
DiyFp(RAPIDJSON_UINT64_C2(0x9c400000, 0x00000000), -50), // 10^4
DiyFp(RAPIDJSON_UINT64_C2(0xc3500000, 0x00000000), -47), // 10^5
DiyFp(RAPIDJSON_UINT64_C2(0xf4240000, 0x00000000), -44), // 10^6
DiyFp(RAPIDJSON_UINT64_C2(0x98968000, 0x00000000), -40) // 10^7
};
int adjustment = dExp - actualExp;
RAPIDJSON_ASSERT(adjustment >= 1 && adjustment < 8);
v = v * kPow10[adjustment - 1];
if (dLen + adjustment > 19) // has more digits than decimal digits in 64-bit
error += kUlp / 2;
}
v = v * cachedPower;
error += kUlp + (error == 0 ? 0 : 1);
const int oldExp = v.e;
v = v.Normalize();
error <<= oldExp - v.e;
const int effectiveSignificandSize =
Double::EffectiveSignificandSize(64 + v.e);
int precisionSize = 64 - effectiveSignificandSize;
if (precisionSize + kUlpShift >= 64) {
int scaleExp = (precisionSize + kUlpShift) - 63;
v.f >>= scaleExp;
v.e += scaleExp;
error = (error >> scaleExp) + 1 + kUlp;
precisionSize -= scaleExp;
}
DiyFp rounded(v.f >> precisionSize, v.e + precisionSize);
const uint64_t precisionBits =
(v.f & ((uint64_t(1) << precisionSize) - 1)) * kUlp;
const uint64_t halfWay = (uint64_t(1) << (precisionSize - 1)) * kUlp;
if (precisionBits >= halfWay + static_cast<unsigned>(error)) {
rounded.f++;
if (rounded.f & (DiyFp::kDpHiddenBit
<< 1)) { // rounding overflows mantissa (issue #340)
rounded.f >>= 1;
rounded.e++;
} }
}
if (i < dLen && decimals[i] >= '5') // Rounding
significand++;
int remaining = dLen - i; *result = rounded.ToDouble();
const int kUlpShift = 3;
const int kUlp = 1 << kUlpShift;
int64_t error = (remaining == 0) ? 0 : kUlp / 2;
DiyFp v(significand, 0); return halfWay - static_cast<unsigned>(error) >= precisionBits ||
v = v.Normalize(); precisionBits >= halfWay + static_cast<unsigned>(error);
error <<= -v.e;
dExp += remaining;
int actualExp;
DiyFp cachedPower = GetCachedPower10(dExp, &actualExp);
if (actualExp != dExp) {
static const DiyFp kPow10[] = {
DiyFp(RAPIDJSON_UINT64_C2(0xa0000000, 0x00000000), -60), // 10^1
DiyFp(RAPIDJSON_UINT64_C2(0xc8000000, 0x00000000), -57), // 10^2
DiyFp(RAPIDJSON_UINT64_C2(0xfa000000, 0x00000000), -54), // 10^3
DiyFp(RAPIDJSON_UINT64_C2(0x9c400000, 0x00000000), -50), // 10^4
DiyFp(RAPIDJSON_UINT64_C2(0xc3500000, 0x00000000), -47), // 10^5
DiyFp(RAPIDJSON_UINT64_C2(0xf4240000, 0x00000000), -44), // 10^6
DiyFp(RAPIDJSON_UINT64_C2(0x98968000, 0x00000000), -40) // 10^7
};
int adjustment = dExp - actualExp;
RAPIDJSON_ASSERT(adjustment >= 1 && adjustment < 8);
v = v * kPow10[adjustment - 1];
if (dLen + adjustment > 19) // has more digits than decimal digits in 64-bit
error += kUlp / 2;
}
v = v * cachedPower;
error += kUlp + (error == 0 ? 0 : 1);
const int oldExp = v.e;
v = v.Normalize();
error <<= oldExp - v.e;
const int effectiveSignificandSize = Double::EffectiveSignificandSize(64 + v.e);
int precisionSize = 64 - effectiveSignificandSize;
if (precisionSize + kUlpShift >= 64) {
int scaleExp = (precisionSize + kUlpShift) - 63;
v.f >>= scaleExp;
v.e += scaleExp;
error = (error >> scaleExp) + 1 + kUlp;
precisionSize -= scaleExp;
}
DiyFp rounded(v.f >> precisionSize, v.e + precisionSize);
const uint64_t precisionBits = (v.f & ((uint64_t(1) << precisionSize) - 1)) * kUlp;
const uint64_t halfWay = (uint64_t(1) << (precisionSize - 1)) * kUlp;
if (precisionBits >= halfWay + static_cast<unsigned>(error)) {
rounded.f++;
if (rounded.f & (DiyFp::kDpHiddenBit << 1)) { // rounding overflows mantissa (issue #340)
rounded.f >>= 1;
rounded.e++;
}
}
*result = rounded.ToDouble();
return halfWay - static_cast<unsigned>(error) >= precisionBits || precisionBits >= halfWay + static_cast<unsigned>(error);
} }
inline double StrtodBigInteger(double approx, const char* decimals, int dLen, int dExp) { inline double StrtodBigInteger(double approx, const char *decimals, int dLen,
RAPIDJSON_ASSERT(dLen >= 0); int dExp) {
const BigInteger dInt(decimals, static_cast<unsigned>(dLen)); RAPIDJSON_ASSERT(dLen >= 0);
Double a(approx); const BigInteger dInt(decimals, static_cast<unsigned>(dLen));
int cmp = CheckWithinHalfULP(a.Value(), dInt, dExp); Double a(approx);
if (cmp < 0) int cmp = CheckWithinHalfULP(a.Value(), dInt, dExp);
return a.Value(); // within half ULP if (cmp < 0)
else if (cmp == 0) { return a.Value(); // within half ULP
// Round towards even else if (cmp == 0) {
if (a.Significand() & 1) // Round towards even
return a.NextPositiveDouble(); if (a.Significand() & 1)
else return a.NextPositiveDouble();
return a.Value(); else
} 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,
RAPIDJSON_ASSERT(d >= 0.0); size_t length, size_t decimalPosition,
RAPIDJSON_ASSERT(length >= 1); int exp) {
RAPIDJSON_ASSERT(d >= 0.0);
RAPIDJSON_ASSERT(length >= 1);
double result = 0.0; double result = 0.0;
if (StrtodFast(d, p, &result)) if (StrtodFast(d, p, &result))
return result; return result;
RAPIDJSON_ASSERT(length <= INT_MAX); RAPIDJSON_ASSERT(length <= INT_MAX);
int dLen = static_cast<int>(length); int dLen = static_cast<int>(length);
RAPIDJSON_ASSERT(length >= decimalPosition); RAPIDJSON_ASSERT(length >= decimalPosition);
RAPIDJSON_ASSERT(length - decimalPosition <= INT_MAX); RAPIDJSON_ASSERT(length - decimalPosition <= INT_MAX);
int dExpAdjust = static_cast<int>(length - decimalPosition); int dExpAdjust = static_cast<int>(length - decimalPosition);
RAPIDJSON_ASSERT(exp >= INT_MIN + dExpAdjust); RAPIDJSON_ASSERT(exp >= INT_MIN + dExpAdjust);
int dExp = exp - dExpAdjust; int dExp = exp - dExpAdjust;
// Make sure length+dExp does not overflow // Make sure length+dExp does not overflow
RAPIDJSON_ASSERT(dExp <= INT_MAX - dLen); RAPIDJSON_ASSERT(dExp <= INT_MAX - dLen);
// Trim leading zeros // Trim leading zeros
while (dLen > 0 && *decimals == '0') { while (dLen > 0 && *decimals == '0') {
dLen--; dLen--;
decimals++; decimals++;
} }
// Trim trailing zeros // Trim trailing zeros
while (dLen > 0 && decimals[dLen - 1] == '0') { while (dLen > 0 && decimals[dLen - 1] == '0') {
dLen--; dLen--;
dExp++; dExp++;
} }
if (dLen == 0) { // Buffer only contains zeros. if (dLen == 0) { // Buffer only contains zeros.
return 0.0; return 0.0;
} }
// Trim right-most digits // Trim right-most digits
const int kMaxDecimalDigit = 767 + 1; const int kMaxDecimalDigit = 767 + 1;
if (dLen > kMaxDecimalDigit) { if (dLen > kMaxDecimalDigit) {
dExp += dLen - kMaxDecimalDigit; dExp += dLen - kMaxDecimalDigit;
dLen = kMaxDecimalDigit; dLen = kMaxDecimalDigit;
} }
// If too small, underflow to zero. // If too small, underflow to zero.
// Any x <= 10^-324 is interpreted as zero. // Any x <= 10^-324 is interpreted as zero.
if (dLen + dExp <= -324) if (dLen + dExp <= -324)
return 0.0; return 0.0;
// If too large, overflow to infinity. // If too large, overflow to infinity.
// Any x >= 10^309 is interpreted as +infinity. // Any x >= 10^309 is interpreted as +infinity.
if (dLen + dExp > 309) if (dLen + dExp > 309)
return std::numeric_limits<double>::infinity(); return std::numeric_limits<double>::infinity();
if (StrtodDiyFp(decimals, dLen, dExp, &result)) if (StrtodDiyFp(decimals, dLen, dExp, &result))
return result; return result;
// Use approximation from StrtodDiyFp and make adjustment with BigInteger comparison // Use approximation from StrtodDiyFp and make adjustment with BigInteger
return StrtodBigInteger(result, decimals, dLen, dExp); // comparison
return StrtodBigInteger(result, decimals, dLen, dExp);
} }
} // namespace internal } // namespace internal

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available. // Tencent is pleased to support the open source community by making RapidJSON
// available.
// //
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved. // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
// //
// Licensed under the MIT License (the "License"); you may not use this file except // 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,21 +23,20 @@
#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;
} }
} // namespace internal } // namespace internal

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

@@ -1,30 +1,35 @@
// Tencent is pleased to support the open source community by making RapidJSON available. // 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.
// //
// Licensed under the MIT License (the "License"); you may not use this file except // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// in compliance with the License. You may obtain a copy of the License at // rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
// //
// http://opensource.org/licenses/MIT // 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
@@ -44,76 +49,92 @@ 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;
//! Constructor. //! Constructor.
/*! /*!
\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)
Read(); : stream_(stream), buffer_(peekBuffer_), bufferSize_(4), bufferLast_(0),
} current_(buffer_), readCount_(0), count_(0), eof_(false) {
Read();
}
//! Constructor. //! Constructor.
/*! /*!
\param stream stream opened for read. \param stream stream opened for read.
\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)
RAPIDJSON_ASSERT(bufferSize >= 4); : stream_(stream), buffer_(buffer), bufferSize_(bufferSize),
Read(); bufferLast_(0), current_(buffer_), readCount_(0), count_(0),
} eof_(false) {
RAPIDJSON_ASSERT(bufferSize >= 4);
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_)
++current_; ++current_;
else if (!eof_) { else if (!eof_) {
count_ += readCount_; count_ += readCount_;
readCount_ = bufferSize_; readCount_ = bufferSize_;
bufferLast_ = buffer_ + readCount_ - 1; bufferLast_ = buffer_ + readCount_ - 1;
current_ = buffer_; current_ = buffer_;
if (!stream_.read(buffer_, static_cast<std::streamsize>(bufferSize_))) { if (!stream_.read(buffer_, static_cast<std::streamsize>(bufferSize_))) {
readCount_ = static_cast<size_t>(stream_.gcount()); readCount_ = static_cast<size_t>(stream_.gcount());
*(bufferLast_ = buffer_ + readCount_) = '\0'; *(bufferLast_ = buffer_ + readCount_) = '\0';
eof_ = true; eof_ = true;
} }
}
} }
}
StreamType &stream_; StreamType &stream_;
Ch peekBuffer_[4], *buffer_; Ch peekBuffer_[4], *buffer_;
size_t bufferSize_; size_t bufferSize_;
Ch *bufferLast_; Ch *bufferLast_;
Ch *current_; Ch *current_;
size_t readCount_; size_t readCount_;
size_t count_; //!< Number of characters read size_t count_; //!< Number of characters read
bool eof_; bool eof_;
}; };
typedef BasicIStreamWrapper<std::istream> IStreamWrapper; typedef BasicIStreamWrapper<std::istream> IStreamWrapper;

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

@@ -1,68 +1,74 @@
// Tencent is pleased to support the open source community by making RapidJSON available. // 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.
// //
// Licensed under the MIT License (the "License"); you may not use this file except // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// in compliance with the License. You may obtain a copy of the License at // rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
// //
// http://opensource.org/licenses/MIT // 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(); }
static const size_t kDefaultCapacity = 256; static const size_t kDefaultCapacity = 256;
mutable internal::Stack<Allocator> stack_; mutable internal::Stack<Allocator> stack_;
}; };
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));
} }
RAPIDJSON_NAMESPACE_END RAPIDJSON_NAMESPACE_END

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available. // 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.
// //
// Licensed under the MIT License (the "License"); you may not use this file except // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// in compliance with the License. You may obtain a copy of the License at // rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
// //
// http://opensource.org/licenses/MIT // 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,47 +23,56 @@
#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() {
void Put(Ch) { RAPIDJSON_ASSERT(false); } RAPIDJSON_ASSERT(false);
void Flush() { RAPIDJSON_ASSERT(false); } return 0;
size_t PutEnd(Ch*) { RAPIDJSON_ASSERT(false); return 0; } }
void Put(Ch) { RAPIDJSON_ASSERT(false); }
void Flush() { RAPIDJSON_ASSERT(false); }
size_t PutEnd(Ch *) {
RAPIDJSON_ASSERT(false);
return 0;
}
// For encoding detection only. // For encoding detection only.
const Ch* Peek4() const { const Ch *Peek4() const { return Tell() + 4 <= size_ ? src_ : 0; }
return Tell() + 4 <= size_ ? src_ : 0;
}
const Ch* src_; //!< Current read position. const Ch *src_; //!< Current read position.
const Ch* begin_; //!< Original head of the string. const Ch *begin_; //!< Original head of the string.
const Ch* end_; //!< End of stream. const Ch *end_; //!< End of stream.
size_t size_; //!< Size of the stream. size_t size_; //!< Size of the stream.
}; };
RAPIDJSON_NAMESPACE_END RAPIDJSON_NAMESPACE_END

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

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

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

@@ -1,37 +1,37 @@
// ISO C9x compliant stdint.h for Microsoft Visual Studio // ISO C9x compliant stdint.h for Microsoft Visual Studio
// Based on ISO/IEC 9899:TC2 Committee draft (May 6, 2005) WG14/N1124 // Based on ISO/IEC 9899:TC2 Committee draft (May 6, 2005) WG14/N1124
// //
// Copyright (c) 2006-2013 Alexander Chemeris // Copyright (c) 2006-2013 Alexander Chemeris
// //
// Redistribution and use in source and binary forms, with or without // Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met: // modification, are permitted provided that the following conditions are met:
// //
// 1. Redistributions of source code must retain the above copyright notice, // 1. Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer. // this list of conditions and the following disclaimer.
// //
// 2. Redistributions in binary form must reproduce the above copyright // 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the // notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution. // documentation and/or other materials provided with the distribution.
// //
// 3. Neither the name of the product nor the names of its contributors may // 3. Neither the name of the product nor the names of its contributors may
// be used to endorse or promote products derived from this software // be used to endorse or promote products derived from this software
// without specific prior written permission. // without specific prior written permission.
// //
// THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED // THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
// WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF // WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO // MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO
// EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, // EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; // PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
// OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, // OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR // WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
// OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF // OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
// ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. // ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
// //
/////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////////
// The above software in this distribution may have been modified by // The above software in this distribution may have been modified by
// THL A29 Limited ("Tencent Modifications"). // THL A29 Limited ("Tencent Modifications").
// All Tencent Modifications are Copyright (C) 2015 THL A29 Limited. // All Tencent Modifications are Copyright (C) 2015 THL A29 Limited.
#ifndef _MSC_VER // [ #ifndef _MSC_VER // [
@@ -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
@@ -62,12 +64,12 @@
// 7.18.4.1 Macros for minimum-width integer constants // 7.18.4.1 Macros for minimum-width integer constants
#define INT8_C(val) val##i8 #define INT8_C(val) val##i8
#define INT16_C(val) val##i16 #define INT16_C(val) val##i16
#define INT32_C(val) val##i32 #define INT32_C(val) val##i32
#define INT64_C(val) val##i64 #define INT64_C(val) val##i64
#define UINT8_C(val) val##ui8 #define UINT8_C(val) val##ui8
#define UINT16_C(val) val##ui16 #define UINT16_C(val) val##ui16
#define UINT32_C(val) val##ui32 #define UINT32_C(val) val##ui32
#define UINT64_C(val) val##ui64 #define UINT64_C(val) val##ui64
@@ -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,167 +119,167 @@ 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;
typedef int32_t int_least32_t; typedef int32_t int_least32_t;
typedef int64_t int_least64_t; typedef int64_t int_least64_t;
typedef uint8_t uint_least8_t; typedef uint8_t uint_least8_t;
typedef uint16_t uint_least16_t; typedef uint16_t uint_least16_t;
typedef uint32_t uint_least32_t; typedef uint32_t uint_least32_t;
typedef uint64_t uint_least64_t; typedef uint64_t uint_least64_t;
// 7.18.1.3 Fastest minimum-width integer types // 7.18.1.3 Fastest minimum-width integer types
typedef int8_t int_fast8_t; typedef int8_t int_fast8_t;
typedef int16_t int_fast16_t; typedef int16_t int_fast16_t;
typedef int32_t int_fast32_t; typedef int32_t int_fast32_t;
typedef int64_t int_fast64_t; typedef int64_t int_fast64_t;
typedef uint8_t uint_fast8_t; typedef uint8_t uint_fast8_t;
typedef uint16_t uint_fast16_t; typedef uint16_t uint_fast16_t;
typedef uint32_t uint_fast32_t; typedef uint32_t uint_fast32_t;
typedef uint64_t uint_fast64_t; 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)
#define INT8_MAX _I8_MAX #define INT8_MAX _I8_MAX
#define INT16_MIN ((int16_t)_I16_MIN) #define INT16_MIN ((int16_t)_I16_MIN)
#define INT16_MAX _I16_MAX #define INT16_MAX _I16_MAX
#define INT32_MIN ((int32_t)_I32_MIN) #define INT32_MIN ((int32_t)_I32_MIN)
#define INT32_MAX _I32_MAX #define INT32_MAX _I32_MAX
#define INT64_MIN ((int64_t)_I64_MIN) #define INT64_MIN ((int64_t)_I64_MIN)
#define INT64_MAX _I64_MAX #define INT64_MAX _I64_MAX
#define UINT8_MAX _UI8_MAX #define UINT8_MAX _UI8_MAX
#define UINT16_MAX _UI16_MAX #define UINT16_MAX _UI16_MAX
#define UINT32_MAX _UI32_MAX #define UINT32_MAX _UI32_MAX
#define UINT64_MAX _UI64_MAX #define UINT64_MAX _UI64_MAX
// 7.18.2.2 Limits of minimum-width integer types // 7.18.2.2 Limits of minimum-width integer types
#define INT_LEAST8_MIN INT8_MIN #define INT_LEAST8_MIN INT8_MIN
#define INT_LEAST8_MAX INT8_MAX #define INT_LEAST8_MAX INT8_MAX
#define INT_LEAST16_MIN INT16_MIN #define INT_LEAST16_MIN INT16_MIN
#define INT_LEAST16_MAX INT16_MAX #define INT_LEAST16_MAX INT16_MAX
#define INT_LEAST32_MIN INT32_MIN #define INT_LEAST32_MIN INT32_MIN
#define INT_LEAST32_MAX INT32_MAX #define INT_LEAST32_MAX INT32_MAX
#define INT_LEAST64_MIN INT64_MIN #define INT_LEAST64_MIN INT64_MIN
#define INT_LEAST64_MAX INT64_MAX #define INT_LEAST64_MAX INT64_MAX
#define UINT_LEAST8_MAX UINT8_MAX #define UINT_LEAST8_MAX UINT8_MAX
#define UINT_LEAST16_MAX UINT16_MAX #define UINT_LEAST16_MAX UINT16_MAX
#define UINT_LEAST32_MAX UINT32_MAX #define UINT_LEAST32_MAX UINT32_MAX
#define UINT_LEAST64_MAX UINT64_MAX #define UINT_LEAST64_MAX UINT64_MAX
// 7.18.2.3 Limits of fastest minimum-width integer types // 7.18.2.3 Limits of fastest minimum-width integer types
#define INT_FAST8_MIN INT8_MIN #define INT_FAST8_MIN INT8_MIN
#define INT_FAST8_MAX INT8_MAX #define INT_FAST8_MAX INT8_MAX
#define INT_FAST16_MIN INT16_MIN #define INT_FAST16_MIN INT16_MIN
#define INT_FAST16_MAX INT16_MAX #define INT_FAST16_MAX INT16_MAX
#define INT_FAST32_MIN INT32_MIN #define INT_FAST32_MIN INT32_MIN
#define INT_FAST32_MAX INT32_MAX #define INT_FAST32_MAX INT32_MAX
#define INT_FAST64_MIN INT64_MIN #define INT_FAST64_MIN INT64_MIN
#define INT_FAST64_MAX INT64_MAX #define INT_FAST64_MAX INT64_MAX
#define UINT_FAST8_MAX UINT8_MAX #define UINT_FAST8_MAX UINT8_MAX
#define UINT_FAST16_MAX UINT16_MAX #define UINT_FAST16_MAX UINT16_MAX
#define UINT_FAST32_MAX UINT32_MAX #define UINT_FAST32_MAX UINT32_MAX
#define UINT_FAST64_MAX UINT64_MAX #define UINT_FAST64_MAX UINT64_MAX
// 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
#define INTMAX_MIN INT64_MIN #define INTMAX_MIN INT64_MIN
#define INTMAX_MAX INT64_MAX #define INTMAX_MAX INT64_MAX
#define UINTMAX_MAX UINT64_MAX #define UINTMAX_MAX UINT64_MAX
// 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
#define WINT_MAX _UI16_MAX #define WINT_MAX _UI16_MAX
#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
#define INT8_C(val) val##i8 #define INT8_C(val) val##i8
#define INT16_C(val) val##i16 #define INT16_C(val) val##i16
#define INT32_C(val) val##i32 #define INT32_C(val) val##i32
#define INT64_C(val) val##i64 #define INT64_C(val) val##i64
#define UINT8_C(val) val##ui8 #define UINT8_C(val) val##ui8
#define UINT16_C(val) val##ui16 #define UINT16_C(val) val##ui16
#define UINT32_C(val) val##ui32 #define UINT32_C(val) val##ui32
#define UINT64_C(val) val##ui64 #define UINT64_C(val) val##ui64
@@ -287,10 +288,10 @@ typedef uint64_t uintmax_t;
// These #ifndef's are needed to prevent collisions with <boost/cstdint.hpp>. // 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 ]

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available. // 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.
// //
// Licensed under the MIT License (the "License"); you may not use this file except // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// in compliance with the License. You may obtain a copy of the License at // rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
// //
// http://opensource.org/licenses/MIT // 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_
@@ -40,33 +44,43 @@ 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;

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

File diff suppressed because it is too large Load Diff

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available. // 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.
// //
// Licensed under the MIT License (the "License"); you may not use this file except // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// in compliance with the License. You may obtain a copy of the License at // rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
// //
// http://opensource.org/licenses/MIT // 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
@@ -33,8 +37,8 @@ RAPIDJSON_NAMESPACE_BEGIN
/*! \see PrettyWriter::SetFormatOptions /*! \see PrettyWriter::SetFormatOptions
*/ */
enum PrettyFormatOptions { enum PrettyFormatOptions {
kFormatDefault = 0, //!< Default pretty formatting. kFormatDefault = 0, //!< Default pretty formatting.
kFormatSingleLineArray = 1 //!< Format arrays on a single line. kFormatSingleLineArray = 1 //!< Format arrays on a single line.
}; };
//! Writer with indentation and spacing. //! Writer with indentation and spacing.
@@ -44,224 +48,275 @@ 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,
typedef typename Base::Ch Ch; writeFlags>
Base;
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) { */
RAPIDJSON_ASSERT(indentChar == ' ' || indentChar == '\t' || indentChar == '\n' || indentChar == '\r'); PrettyWriter &SetIndent(Ch indentChar, unsigned indentCharCount) {
indentChar_ = indentChar; RAPIDJSON_ASSERT(indentChar == ' ' || indentChar == '\t' ||
indentCharCount_ = indentCharCount; indentChar == '\n' || indentChar == '\r');
return *this; indentChar_ = indentChar;
} indentCharCount_ = indentCharCount;
return *this;
}
//! 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;
} }
/*! @name Implementation of Handler /*! @name Implementation of Handler
\see Handler \see Handler
*/ */
//@{ //@{
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);
return Base::EndValue(Base::WriteString(str, length)); return Base::EndValue(Base::WriteString(str, length));
} }
#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>())
return Base::WriteStartObject(); typename Base::Level(false);
} return Base::WriteStartObject();
}
bool Key(const Ch* str, SizeType length, bool copy = false) { return String(str, length, copy); } bool Key(const Ch *str, SizeType length, bool copy = false) {
return String(str, length, copy);
}
#if RAPIDJSON_HAS_STDSTRING #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) {
(void)memberCount;
RAPIDJSON_ASSERT(Base::level_stack_.GetSize() >= sizeof(typename Base::Level)); // not inside an Object
RAPIDJSON_ASSERT(!Base::level_stack_.template Top<typename Base::Level>()->inArray); // currently inside an Array, not Object
RAPIDJSON_ASSERT(0 == Base::level_stack_.template Top<typename Base::Level>()->valueCount % 2); // Object has a Key without a Value
bool empty = Base::level_stack_.template Pop<typename Base::Level>(1)->valueCount == 0;
if (!empty) { bool EndObject(SizeType memberCount = 0) {
Base::os_->Put('\n'); (void)memberCount;
WriteIndent(); RAPIDJSON_ASSERT(Base::level_stack_.GetSize() >=
} sizeof(typename Base::Level)); // not inside an Object
bool ret = Base::EndValue(Base::WriteEndObject()); RAPIDJSON_ASSERT(!Base::level_stack_.template Top<typename Base::Level>()
(void)ret; ->inArray); // currently inside an Array, not Object
RAPIDJSON_ASSERT(ret == true); RAPIDJSON_ASSERT(
if (Base::level_stack_.Empty()) // end of json text 0 ==
Base::Flush(); Base::level_stack_.template Top<typename Base::Level>()->valueCount %
return true; 2); // Object has a Key without a Value
bool empty =
Base::level_stack_.template Pop<typename Base::Level>(1)->valueCount ==
0;
if (!empty) {
Base::os_->Put('\n');
WriteIndent();
} }
bool ret = Base::EndValue(Base::WriteEndObject());
(void)ret;
RAPIDJSON_ASSERT(ret == true);
if (Base::level_stack_.Empty()) // end of json text
Base::Flush();
return true;
}
bool StartArray() { 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>())
return Base::WriteStartArray(); typename Base::Level(true);
return Base::WriteStartArray();
}
bool EndArray(SizeType memberCount = 0) {
(void)memberCount;
RAPIDJSON_ASSERT(Base::level_stack_.GetSize() >=
sizeof(typename Base::Level));
RAPIDJSON_ASSERT(
Base::level_stack_.template Top<typename Base::Level>()->inArray);
bool empty =
Base::level_stack_.template Pop<typename Base::Level>(1)->valueCount ==
0;
if (!empty && !(formatOptions_ & kFormatSingleLineArray)) {
Base::os_->Put('\n');
WriteIndent();
} }
bool ret = Base::EndValue(Base::WriteEndArray());
(void)ret;
RAPIDJSON_ASSERT(ret == true);
if (Base::level_stack_.Empty()) // end of json text
Base::Flush();
return true;
}
bool EndArray(SizeType memberCount = 0) { //@}
(void)memberCount;
RAPIDJSON_ASSERT(Base::level_stack_.GetSize() >= sizeof(typename Base::Level));
RAPIDJSON_ASSERT(Base::level_stack_.template Top<typename Base::Level>()->inArray);
bool empty = Base::level_stack_.template Pop<typename Base::Level>(1)->valueCount == 0;
if (!empty && !(formatOptions_ & kFormatSingleLineArray)) { /*! @name Convenience extensions */
Base::os_->Put('\n'); //@{
WriteIndent();
}
bool ret = Base::EndValue(Base::WriteEndArray());
(void)ret;
RAPIDJSON_ASSERT(ret == true);
if (Base::level_stack_.Empty()) // end of json text
Base::Flush();
return true;
}
//@} //! Simpler but slower overload.
bool String(const Ch *str) { return String(str, internal::StrLen(str)); }
bool Key(const Ch *str) { return Key(str, internal::StrLen(str)); }
/*! @name Convenience extensions */ //@}
//@{
//! Simpler but slower overload. //! Write a raw JSON value.
bool String(const Ch* str) { return String(str, internal::StrLen(str)); } /*!
bool Key(const Ch* str) { return Key(str, internal::StrLen(str)); } For user to write a stringified JSON as a value.
//@} \param json A well-formed JSON value. It should not contain null character
within [0, length - 1] range. \param length Length of the json. \param type
//! Write a raw JSON value. Type of the root of json. \note When using PrettyWriter::RawValue(), the
/*! result json may not be indented correctly.
For user to write a stringified JSON as a value. */
bool RawValue(const Ch *json, size_t length, Type type) {
\param json A well-formed JSON value. It should not contain null character within [0, length - 1] range. RAPIDJSON_ASSERT(json != 0);
\param length Length of the json. PrettyPrefix(type);
\param type Type of the root of json. return Base::EndValue(Base::WriteRawValue(json, length));
\note When using PrettyWriter::RawValue(), the result json may not be indented correctly. }
*/
bool RawValue(const Ch* json, size_t length, Type type) {
RAPIDJSON_ASSERT(json != 0);
PrettyPrefix(type);
return Base::EndValue(Base::WriteRawValue(json, length));
}
protected: 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(
if (formatOptions_ & kFormatSingleLineArray) ','); // add comma if it is not the first element in array
Base::os_->Put(' '); if (formatOptions_ & kFormatSingleLineArray)
} Base::os_->Put(' ');
if (!(formatOptions_ & kFormatSingleLineArray)) {
Base::os_->Put('\n');
WriteIndent();
}
}
else { // in object
if (level->valueCount > 0) {
if (level->valueCount % 2 == 0) {
Base::os_->Put(',');
Base::os_->Put('\n');
}
else {
Base::os_->Put(':');
Base::os_->Put(' ');
}
}
else
Base::os_->Put('\n');
if (level->valueCount % 2 == 0)
WriteIndent();
}
if (!level->inArray && level->valueCount % 2 == 0)
RAPIDJSON_ASSERT(type == kStringType); // if it's in object, then even number should be a name
level->valueCount++;
} }
else {
RAPIDJSON_ASSERT(!Base::hasRoot_); // Should only has one and only one root. if (!(formatOptions_ & kFormatSingleLineArray)) {
Base::hasRoot_ = true; Base::os_->Put('\n');
WriteIndent();
} }
} } else { // in object
if (level->valueCount > 0) {
if (level->valueCount % 2 == 0) {
Base::os_->Put(',');
Base::os_->Put('\n');
} else {
Base::os_->Put(':');
Base::os_->Put(' ');
}
} else
Base::os_->Put('\n');
void WriteIndent() { if (level->valueCount % 2 == 0)
size_t count = (Base::level_stack_.GetSize() / sizeof(typename Base::Level)) * indentCharCount_; WriteIndent();
PutN(*Base::os_, static_cast<typename OutputStream::Ch>(indentChar_), count); }
if (!level->inArray && level->valueCount % 2 == 0)
RAPIDJSON_ASSERT(type == kStringType); // if it's in object, then even
// number should be a name
level->valueCount++;
} else {
RAPIDJSON_ASSERT(
!Base::hasRoot_); // Should only has one and only one root.
Base::hasRoot_ = true;
} }
}
Ch indentChar_; void WriteIndent() {
unsigned indentCharCount_; size_t count =
PrettyFormatOptions formatOptions_; (Base::level_stack_.GetSize() / sizeof(typename Base::Level)) *
indentCharCount_;
PutN(*Base::os_, static_cast<typename OutputStream::Ch>(indentChar_),
count);
}
Ch indentChar_;
unsigned indentCharCount_;
PrettyFormatOptions formatOptions_;
private: 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

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

@@ -1,23 +1,27 @@
// Tencent is pleased to support the open source community by making RapidJSON available. // 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.
// //
// Licensed under the MIT License (the "License"); you may not use this file except // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// in compliance with the License. You may obtain a copy of the License at // rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
// //
// http://opensource.org/licenses/MIT // 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_
/*!\file rapidjson.h /*!\file rapidjson.h
\brief common definitions and configuration \brief common definitions and configuration
\see RAPIDJSON_CONFIG \see RAPIDJSON_CONFIG
*/ */
@@ -29,20 +33,22 @@
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
different translation units of a single application. different translation units of a single application.
*/ */
#include <cstdlib> // malloc(), realloc(), free(), size_t #include <cstdlib> // malloc(), realloc(), free(), size_t
#include <cstring> // memset(), memcpy(), memmove(), memcmp() #include <cstring> // memset(), memcpy(), memmove(), memcmp()
/////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////////
// 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
@@ -75,8 +81,9 @@
#define RAPIDJSON_MAJOR_VERSION 1 #define RAPIDJSON_MAJOR_VERSION 1
#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,21 +163,21 @@
\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.
*/ */
#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
@@ -195,59 +202,67 @@
/////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_ENDIAN // RAPIDJSON_ENDIAN
#define RAPIDJSON_LITTLEENDIAN 0 //!< Little endian machine #define RAPIDJSON_LITTLEENDIAN 0 //!< Little endian machine
#define RAPIDJSON_BIGENDIAN 1 //!< Big endian machine #define RAPIDJSON_BIGENDIAN 1 //!< Big endian machine
//! Endianness of the machine. //! Endianness of the machine.
/*! /*!
\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__) || \
# error Unknown machine endianness detected. User needs to define RAPIDJSON_ENDIAN. defined(_M_X64) || defined(__bfin__)
# endif #define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN
#elif defined(_MSC_VER) && (defined(_M_ARM) || defined(_M_ARM64))
#define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN
#elif defined(RAPIDJSON_DOXYGEN_RUNNING)
#define RAPIDJSON_ENDIAN
#else
# error Unknown machine endianness detected. User needs to define RAPIDJSON_ENDIAN.
#endif
#endif // RAPIDJSON_ENDIAN #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,14 +450,16 @@ 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
#if defined(__GNUC__) || defined(__clang__) #if defined(__GNUC__) || defined(__clang__)
#define RAPIDJSON_STATIC_ASSERT_UNUSED_ATTRIBUTE __attribute__((unused)) #define RAPIDJSON_STATIC_ASSERT_UNUSED_ATTRIBUTE __attribute__((unused))
#else #else
#define RAPIDJSON_STATIC_ASSERT_UNUSED_ATTRIBUTE #define RAPIDJSON_STATIC_ASSERT_UNUSED_ATTRIBUTE
#endif #endif
#ifndef __clang__ #ifndef __clang__
//!@endcond //!@endcond
@@ -442,10 +470,11 @@ RAPIDJSON_NAMESPACE_END
\param x compile-time condition \param x compile-time condition
\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
/////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////////
@@ -482,13 +511,13 @@ RAPIDJSON_NAMESPACE_END
//!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN //!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN
#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)
@@ -500,24 +529,26 @@ RAPIDJSON_NAMESPACE_END
// RAPIDJSON_DIAG_PUSH/POP, RAPIDJSON_DIAG_OFF // RAPIDJSON_DIAG_PUSH/POP, RAPIDJSON_DIAG_OFF
#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
#define RAPIDJSON_DIAG_PUSH /* ignored */ #define RAPIDJSON_DIAG_PUSH /* ignored */
#define RAPIDJSON_DIAG_POP /* ignored */ #define RAPIDJSON_DIAG_POP /* ignored */
#endif #endif
#elif defined(_MSC_VER) #elif defined(_MSC_VER)
@@ -526,9 +557,9 @@ 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)
#else #else
@@ -543,15 +574,19 @@ 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) && \
(defined(_MSC_VER) && _MSC_VER >= 1600) || \ (RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4, 3, 0)) && \
(defined(__SUNPRO_CC) && __SUNPRO_CC >= 0x5140 && defined(__GXX_EXPERIMENTAL_CXX0X__)) defined(__GXX_EXPERIMENTAL_CXX0X__)) || \
(defined(_MSC_VER) && _MSC_VER >= 1600) || \
(defined(__SUNPRO_CC) && __SUNPRO_CC >= 0x5140 && \
defined(__GXX_EXPERIMENTAL_CXX0X__))
#define RAPIDJSON_HAS_CXX11_RVALUE_REFS 1 #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) && \
(defined(_MSC_VER) && _MSC_VER >= 1900) || \ (RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4, 6, 0)) && \
(defined(__SUNPRO_CC) && __SUNPRO_CC >= 0x5140 && defined(__GXX_EXPERIMENTAL_CXX0X__)) defined(__GXX_EXPERIMENTAL_CXX0X__)) || \
(defined(_MSC_VER) && _MSC_VER >= 1900) || \
(defined(__SUNPRO_CC) && __SUNPRO_CC >= 0x5140 && \
defined(__GXX_EXPERIMENTAL_CXX0X__))
#define RAPIDJSON_HAS_CXX11_NOEXCEPT 1 #define RAPIDJSON_HAS_CXX11_NOEXCEPT 1
#else #else
#define RAPIDJSON_HAS_CXX11_NOEXCEPT 0 #define RAPIDJSON_HAS_CXX11_NOEXCEPT 0
@@ -574,7 +612,7 @@ RAPIDJSON_NAMESPACE_END
#define RAPIDJSON_NOEXCEPT noexcept #define RAPIDJSON_NOEXCEPT noexcept
#else #else
#define RAPIDJSON_NOEXCEPT /* noexcept */ #define RAPIDJSON_NOEXCEPT /* noexcept */
#endif // RAPIDJSON_HAS_CXX11_NOEXCEPT #endif // RAPIDJSON_HAS_CXX11_NOEXCEPT
// no automatic detection, yet // no automatic detection, yet
#ifndef RAPIDJSON_HAS_CXX11_TYPETRAITS #ifndef RAPIDJSON_HAS_CXX11_TYPETRAITS
@@ -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) && \
(defined(_MSC_VER) && _MSC_VER >= 1700) || \ (RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4, 6, 0)) && \
(defined(__SUNPRO_CC) && __SUNPRO_CC >= 0x5140 && defined(__GXX_EXPERIMENTAL_CXX0X__)) defined(__GXX_EXPERIMENTAL_CXX0X__)) || \
(defined(_MSC_VER) && _MSC_VER >= 1700) || \
(defined(__SUNPRO_CC) && __SUNPRO_CC >= 0x5140 && \
defined(__GXX_EXPERIMENTAL_CXX0X__))
#define RAPIDJSON_HAS_CXX11_RANGE_FOR 1 #define RAPIDJSON_HAS_CXX11_RANGE_FOR 1
#else #else
#define RAPIDJSON_HAS_CXX11_RANGE_FOR 0 #define RAPIDJSON_HAS_CXX11_RANGE_FOR 0
@@ -601,27 +642,27 @@ 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
such cases. such cases.
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
@@ -661,13 +702,13 @@ RAPIDJSON_NAMESPACE_BEGIN
//! Type of JSON value //! Type of JSON value
enum Type { enum Type {
kNullType = 0, //!< null kNullType = 0, //!< null
kFalseType = 1, //!< false kFalseType = 1, //!< false
kTrueType = 2, //!< true kTrueType = 2, //!< true
kObjectType = 3, //!< object kObjectType = 3, //!< object
kArrayType = 4, //!< array kArrayType = 4, //!< array
kStringType = 5, //!< string kStringType = 5, //!< string
kNumberType = 6 //!< number kNumberType = 6 //!< number
}; };
RAPIDJSON_NAMESPACE_END RAPIDJSON_NAMESPACE_END

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

File diff suppressed because it is too large Load Diff

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

File diff suppressed because it is too large Load Diff

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

@@ -1,16 +1,20 @@
// Tencent is pleased to support the open source community by making RapidJSON available. // Tencent is pleased to support the open source community by making RapidJSON
// available.
// //
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved. // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
// //
// Licensed under the MIT License (the "License"); you may not use this file except // 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,39 +70,40 @@ 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
Stream that can be copied fast should specialize this, like StreamTraits<StringStream>. 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);
} }
/////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////////
@@ -111,33 +117,33 @@ inline void PutN(Stream& stream, Ch c, size_t n) {
#if defined(_MSC_VER) && _MSC_VER <= 1800 #if defined(_MSC_VER) && _MSC_VER <= 1800
RAPIDJSON_DIAG_PUSH RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(4702) // unreachable code 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,33 +155,38 @@ 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) {}
Ch Peek() const { return *src_; } Ch Peek() const { return *src_; }
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() {
void Put(Ch) { RAPIDJSON_ASSERT(false); } RAPIDJSON_ASSERT(false);
void Flush() { RAPIDJSON_ASSERT(false); } return 0;
size_t PutEnd(Ch*) { RAPIDJSON_ASSERT(false); return 0; } }
void Put(Ch) { RAPIDJSON_ASSERT(false); }
void Flush() { RAPIDJSON_ASSERT(false); }
size_t PutEnd(Ch *) {
RAPIDJSON_ASSERT(false);
return 0;
}
const Ch* src_; //!< Current read position. const Ch *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,39 +195,45 @@ 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) {}
// Read // Read
Ch Peek() { return *src_; } Ch Peek() { return *src_; }
Ch Take() { return *src_++; } Ch Take() { return *src_++; }
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) {
void Pop(size_t count) { dst_ -= count; } Ch *begin = dst_;
dst_ += count;
return begin;
}
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

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

@@ -1,22 +1,26 @@
// Tencent is pleased to support the open source community by making RapidJSON available. // 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.
// //
// Licensed under the MIT License (the "License"); you may not use this file except // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// in compliance with the License. You may obtain a copy of the License at // rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
// //
// http://opensource.org/licenses/MIT // 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
@@ -40,76 +44,82 @@ RAPIDJSON_NAMESPACE_BEGIN
template <typename Encoding, typename Allocator = CrtAllocator> template <typename Encoding, typename Allocator = CrtAllocator>
class GenericStringBuffer { 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_)) {}
if (&rhs != this) GenericStringBuffer &operator=(GenericStringBuffer &&rhs) {
stack_ = std::move(rhs.stack_); if (&rhs != this)
return *this; stack_ = std::move(rhs.stack_);
} return *this;
}
#endif #endif
void Put(Ch c) { *stack_.template Push<Ch>() = c; } void Put(Ch c) { *stack_.template Push<Ch>() = c; }
void PutUnsafe(Ch c) { *stack_.template PushUnsafe<Ch>() = c; } void PutUnsafe(Ch c) { *stack_.template PushUnsafe<Ch>() = c; }
void Flush() {} void Flush() {}
void Clear() { stack_.Clear(); } void Clear() { stack_.Clear(); }
void ShrinkToFit() { void ShrinkToFit() {
// 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_.ShrinkToFit(); stack_.ShrinkToFit();
stack_.template Pop<Ch>(1); stack_.template Pop<Ch>(1);
} }
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);
return stack_.template Bottom<Ch>(); return stack_.template Bottom<Ch>();
} }
//! Get the size of string in bytes in the string buffer. //! Get the size of string in bytes in the string buffer.
size_t GetSize() const { return stack_.GetSize(); } size_t GetSize() const { return stack_.GetSize(); }
//! Get the length of string in Ch in the string buffer. //! Get the length of string in Ch in the string buffer.
size_t GetLength() const { return stack_.GetSize() / sizeof(Ch); } size_t GetLength() const { return stack_.GetSize() / sizeof(Ch); }
static const size_t kDefaultCapacity = 256; static const size_t kDefaultCapacity = 256;
mutable internal::Stack<Allocator> stack_; mutable internal::Stack<Allocator> stack_;
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,
stream.Reserve(count); size_t 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,
stream.PutUnsafe(c); typename Encoding::Ch c) {
stream.PutUnsafe(c);
} }
//! Implement specialized version of PutN() with memset() for better performance. //! Implement specialized version of PutN() with memset() for better
template<> //! performance.
inline void PutN(GenericStringBuffer<UTF8<> >& stream, char c, size_t n) { template <>
std::memset(stream.stack_.Push<char>(n), c, n * sizeof(c)); inline void PutN(GenericStringBuffer<UTF8<>> &stream, char c, size_t n) {
std::memset(stream.stack_.Push<char>(n), c, n * sizeof(c));
} }
RAPIDJSON_NAMESPACE_END RAPIDJSON_NAMESPACE_END

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

File diff suppressed because it is too large Load Diff

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

File diff suppressed because it is too large Load Diff

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

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

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

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

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

@@ -4,119 +4,97 @@
// Copyright (C) 2006, 2009 Marcin Kalicinski // 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:
//! Loads file into the memory. Data will be automatically destroyed by the destructor.
//! \param filename Filename to load.
file(const char *filename)
{
using namespace std;
// Open stream public:
basic_ifstream<Ch> stream(filename, ios::binary); //! Loads file into the memory. Data will be automatically destroyed by the
if (!stream) //! destructor. \param filename Filename to load.
throw runtime_error(string("cannot open file ") + filename); file(const char *filename) {
stream.unsetf(ios::skipws); using namespace std;
// Determine stream size
stream.seekg(0, ios::end);
size_t size = stream.tellg();
stream.seekg(0);
// Load data and add terminating 0
m_data.resize(size + 1);
stream.read(&m_data.front(), static_cast<streamsize>(size));
m_data[size] = 0;
}
//! Loads file into the memory. Data will be automatically destroyed by the destructor // Open stream
//! \param stream Stream to load from basic_ifstream<Ch> stream(filename, ios::binary);
file(std::basic_istream<Ch> &stream) if (!stream)
{ throw runtime_error(string("cannot open file ") + filename);
using namespace std; stream.unsetf(ios::skipws);
// Load data and add terminating 0 // Determine stream size
stream.unsetf(ios::skipws); stream.seekg(0, ios::end);
m_data.assign(istreambuf_iterator<Ch>(stream), istreambuf_iterator<Ch>()); size_t size = stream.tellg();
if (stream.fail() || stream.bad()) stream.seekg(0);
throw runtime_error("error reading stream");
m_data.push_back(0);
}
//! Gets file data.
//! \return Pointer to data of file.
Ch *data()
{
return &m_data.front();
}
//! Gets file data. // Load data and add terminating 0
//! \return Pointer to data of file. m_data.resize(size + 1);
const Ch *data() const stream.read(&m_data.front(), static_cast<streamsize>(size));
{ m_data[size] = 0;
return &m_data.front(); }
}
//! Gets file data size. //! Loads file into the memory. Data will be automatically destroyed by the
//! \return Size of file data, in characters. //! destructor \param stream Stream to load from
std::size_t size() const file(std::basic_istream<Ch> &stream) {
{ using namespace std;
return m_data.size();
}
private: // Load data and add terminating 0
stream.unsetf(ios::skipws);
m_data.assign(istreambuf_iterator<Ch>(stream), istreambuf_iterator<Ch>());
if (stream.fail() || stream.bad())
throw runtime_error("error reading stream");
m_data.push_back(0);
}
std::vector<Ch> m_data; // File data //! Gets file data.
//! \return Pointer to data of file.
Ch *data() { return &m_data.front(); }
}; //! Gets file data.
//! \return Pointer to data of file.
const Ch *data() const { return &m_data.front(); }
//! Counts children of node. Time complexity is O(n). //! Gets file data size.
//! \return Number of children of node //! \return Size of file data, in characters.
template<class Ch> std::size_t size() const { return m_data.size(); }
inline std::size_t count_children(xml_node<Ch> *node)
{
xml_node<Ch> *child = node->first_node();
std::size_t count = 0;
while (child)
{
++count;
child = child->next_sibling();
}
return count;
}
//! Counts attributes of node. Time complexity is O(n). private:
//! \return Number of attributes of node std::vector<Ch> m_data; // File data
template<class Ch> };
inline std::size_t count_attributes(xml_node<Ch> *node)
{
xml_attribute<Ch> *attr = node->first_attribute();
std::size_t count = 0;
while (attr)
{
++count;
attr = attr->next_attribute();
}
return count;
}
//! Counts children of node. Time complexity is O(n).
//! \return Number of children of node
template <class Ch> inline std::size_t count_children(xml_node<Ch> *node) {
xml_node<Ch> *child = node->first_node();
std::size_t count = 0;
while (child) {
++count;
child = child->next_sibling();
}
return count;
} }
//! Counts attributes of node. Time complexity is O(n).
//! \return Number of attributes of node
template <class Ch> inline std::size_t count_attributes(xml_node<Ch> *node) {
xml_attribute<Ch> *attr = node->first_attribute();
std::size_t count = 0;
while (attr) {
++count;
attr = attr->next_attribute();
}
return count;
}
} // namespace rapidxml
#endif #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"/>

12
livox_ros_driver/livox_ros_driver/include/livox_ros_driver.h
View File

@@ -27,16 +27,14 @@
#define LIVOX_ROS_DRIVER_VER_MAJOR 2 #define LIVOX_ROS_DRIVER_VER_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

359
livox_ros_driver/livox_ros_driver/lddc.cpp
View File

@@ -24,37 +24,40 @@
#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,
data_src_(data_src), output_type_(output_type), publish_frq_(frq) { double frq)
: transfer_format_(format), use_multi_topic_(multi_topic),
data_src_(data_src), output_type_(output_type), publish_frq_(frq) {
publish_interval_ms_ = 1000/publish_frq_; publish_interval_ms_ = 1000 / publish_frq_;
lds_ = nullptr; 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_));
global_pub_ = nullptr; global_pub_ = nullptr;
global_imu_pub_ = nullptr; global_imu_pub_ = nullptr;
cur_node_ = nullptr; cur_node_ = nullptr;
bag_ = nullptr; bag_ = nullptr;
}; };
Lddc::~Lddc() { Lddc::~Lddc() {
@@ -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;
@@ -93,70 +97,89 @@ uint32_t Lddc::PublishPointcloud2(LidarDataQueue* queue, uint32_t packet_num, ui
cloud.header.frame_id = "livox_frame"; cloud.header.frame_id = "livox_frame";
cloud.height = 1; cloud.height = 1;
cloud.width = 0; cloud.width = 0;
cloud.fields.resize(6); cloud.fields.resize(6);
cloud.fields[0].offset = 0; cloud.fields[0].offset = 0;
cloud.fields[0].name = "x"; cloud.fields[0].name = "x";
cloud.fields[0].count = 1; cloud.fields[0].count = 1;
cloud.fields[0].datatype = sensor_msgs::PointField::FLOAT32; cloud.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
cloud.fields[1].offset = 4; cloud.fields[1].offset = 4;
cloud.fields[1].name = "y"; cloud.fields[1].name = "y";
cloud.fields[1].count = 1; cloud.fields[1].count = 1;
cloud.fields[1].datatype = sensor_msgs::PointField::FLOAT32; cloud.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
cloud.fields[2].offset = 8; cloud.fields[2].offset = 8;
cloud.fields[2].name = "z"; cloud.fields[2].name = "z";
cloud.fields[2].count = 1; cloud.fields[2].count = 1;
cloud.fields[2].datatype = sensor_msgs::PointField::FLOAT32; cloud.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
cloud.fields[3].offset = 12; cloud.fields[3].offset = 12;
cloud.fields[3].name = "intensity"; cloud.fields[3].name = "intensity";
cloud.fields[3].count = 1; cloud.fields[3].count = 1;
cloud.fields[3].datatype = sensor_msgs::PointField::FLOAT32; cloud.fields[3].datatype = sensor_msgs::PointField::FLOAT32;
cloud.fields[4].offset = 16; cloud.fields[4].offset = 16;
cloud.fields[4].name = "tag"; cloud.fields[4].name = "tag";
cloud.fields[4].count = 1; cloud.fields[4].count = 1;
cloud.fields[4].datatype = sensor_msgs::PointField::UINT8; cloud.fields[4].datatype = sensor_msgs::PointField::UINT8;
cloud.fields[5].offset = 17; cloud.fields[5].offset = 17;
cloud.fields[5].name = "line"; cloud.fields[5].name = "line";
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 *
cloud.point_step = sizeof(LivoxPointXyzrtl); 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) &&
if (kSourceLvxFile != data_source) { lds_->lidars_[handle].data_is_pubulished) {
ROS_INFO("Lidar[%d] packet loss", handle); ROS_INFO("Lidar[%d] packet loss, interval is %ldus", handle, packet_gap);
break; if (kSourceLvxFile != data_source) {
// ROS_INFO("Lidar[%d] packet loss %ld %d %d", handle,
// packet_loss_threshold_lower, packet_interval, raw_packet->data_type);
int64_t packet_loss_threshold_upper = packet_interval * packet_num;
if (packet_gap >
packet_loss_threshold_upper) { // skip when gap is too large
break;
}
point_base = FillZeroPointXyzrtl(point_base, storage_packet.point_num);
cloud.width += storage_packet.point_num;
last_timestamp = last_timestamp + packet_interval;
++published_packet;
continue;
} }
} }
if (!published_packet) { 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);
@@ -166,67 +189,91 @@ uint32_t Lddc::PublishPointcloud2(LidarDataQueue* queue, uint32_t packet_num, ui
cloud.row_step = cloud.width * cloud.point_step; cloud.row_step = cloud.width * cloud.point_step;
cloud.is_bigendian = false; cloud.is_bigendian = false;
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,
uint64_t timestamp = 0; uint8_t handle) {
uint64_t last_timestamp = 0; uint64_t timestamp = 0;
uint32_t published_packet = 0; uint64_t last_timestamp = 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;
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 ((packet_gap > packet_loss_threshold_lower) && published_packet &&
ROS_INFO("Lidar[%d] packet loss", handle); lds_->lidars_[handle].data_is_pubulished) {
break; ROS_INFO("Lidar[%d] packet loss, interval is %ldus", handle, packet_gap);
int64_t packet_loss_threshold_upper = packet_interval * packet_num;
if (packet_gap >
packet_loss_threshold_upper) { // skip when gap is too large
break;
}
pcl::PointXYZI point = {0}; // fill zero points
for (uint32_t i = 0; i < storage_packet.point_num; i++) {
cloud->points.push_back(point);
}
last_timestamp = last_timestamp + packet_interval;
++published_packet;
continue;
} }
if (!published_packet) { 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,34 +319,36 @@ 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;
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 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,33 +356,35 @@ 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;
point.z = dst_point->z; point.z = dst_point->z;
point.reflectivity = dst_point->reflectivity; point.reflectivity = dst_point->reflectivity;
point.tag = dst_point->tag; point.tag = dst_point->tag;
point.line = dst_point->line; point.line = dst_point->line;
++dst_point; ++dst_point;
livox_msg.points.push_back(point); livox_msg.points.push_back(point);
@@ -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,42 +467,33 @@ 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(
name_str, queue_size); "%s publish use PointCloud2 format, set ROS publisher queue size %d",
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(
name_str, queue_size); "%s publish use livox custom format, set ROS publisher queue size %d",
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

46
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,11 +119,11 @@ 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;
queue->storage_packet[wr_idx].time_rcv = time_rcv; queue->storage_packet[wr_idx].time_rcv = time_rcv;
queue->storage_packet[wr_idx].point_num = point_num; queue->storage_packet[wr_idx].point_num = point_num;
memcpy(queue->storage_packet[wr_idx].raw_data, data, length); memcpy(queue->storage_packet[wr_idx].raw_data, data, length);
@@ -134,4 +132,4 @@ uint32_t QueuePushAny(LidarDataQueue* queue, uint8_t *data, uint32_t length, \
return 1; return 1;
} }
} } // namespace livox_ros

39
livox_ros_driver/livox_ros_driver/ldq.h
View File

@@ -31,15 +31,14 @@
namespace livox_ros { namespace livox_ros {
const uint32_t KEthPacketMaxLength = 1500; const uint32_t KEthPacketMaxLength = 1500;
#pragma pack(1) #pragma pack(1)
typedef struct { typedef struct {
uint64_t time_rcv; /**< receive time when data arrive */ uint64_t time_rcv; /**< receive time when data arrive */
uint32_t point_num; uint32_t point_num;
uint8_t raw_data[KEthPacketMaxLength]; uint8_t raw_data[KEthPacketMaxLength];
} StoragePacket; } StoragePacket;
#pragma pack() #pragma pack()
@@ -49,7 +48,7 @@ typedef struct {
volatile uint32_t rd_idx; volatile uint32_t rd_idx;
volatile uint32_t wr_idx; volatile uint32_t wr_idx;
uint32_t mask; uint32_t mask;
uint32_t size; /**< must be power of 2. */ uint32_t size; /**< must be power of 2. */
} LidarDataQueue; } LidarDataQueue;
inline static bool IsPowerOf2(uint32_t size) { inline static bool IsPowerOf2(uint32_t size) {
@@ -58,9 +57,9 @@ inline static bool IsPowerOf2(uint32_t size) {
inline static uint32_t RoundupPowerOf2(uint32_t size) { 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

342
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));
@@ -62,16 +64,16 @@ uint64_t GetStoragePacketTimestamp(StoragePacket* packet, uint8_t data_src_) {
} else if (raw_packet->timestamp_type == kTimestampTypePpsGps) { } else if (raw_packet->timestamp_type == kTimestampTypePpsGps) {
struct tm time_utc; struct tm time_utc;
time_utc.tm_isdst = 0; time_utc.tm_isdst = 0;
time_utc.tm_year = raw_packet->timestamp[0] + 100; // map 2000 to 1990 time_utc.tm_year = raw_packet->timestamp[0] + 100; // map 2000 to 1990
time_utc.tm_mon = raw_packet->timestamp[1]; time_utc.tm_mon = raw_packet->timestamp[1];
time_utc.tm_mday = raw_packet->timestamp[2]; time_utc.tm_mday = raw_packet->timestamp[2];
time_utc.tm_hour = raw_packet->timestamp[3]; time_utc.tm_hour = raw_packet->timestamp[3];
time_utc.tm_min = 0; time_utc.tm_min = 0;
time_utc.tm_sec = 0; time_utc.tm_sec = 0;
uint64_t time_epoch = mktime(&time_utc); uint64_t time_epoch = mktime(&time_utc);
time_epoch = time_epoch * 1000000 + timestamp.stamp_word.high; // to us time_epoch = time_epoch * 1000000 + timestamp.stamp_word.high; // to us
time_epoch = time_epoch * 1000; // to ns time_epoch = time_epoch * 1000; // to ns
return time_epoch; return time_epoch;
} else { } else {
@@ -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,16 +113,16 @@ 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) {
double cos_roll = cos(static_cast<double>(euler[0])); double cos_roll = cos(static_cast<double>(euler[0]));
double cos_pitch = cos(static_cast<double>(euler[1])); double cos_pitch = cos(static_cast<double>(euler[1]));
double cos_yaw = cos(static_cast<double>(euler[2])); double cos_yaw = cos(static_cast<double>(euler[2]));
double sin_roll = sin(static_cast<double>(euler[0])); double sin_roll = sin(static_cast<double>(euler[0]));
double sin_pitch = sin(static_cast<double>(euler[1])); double sin_pitch = sin(static_cast<double>(euler[1]));
double sin_yaw = sin(static_cast<double>(euler[2])); double sin_yaw = sin(static_cast<double>(euler[2]));
matrix[0][0] = cos_pitch * cos_yaw; matrix[0][0] = cos_pitch * cos_yaw;
matrix[0][1] = sin_roll * sin_pitch * cos_yaw - cos_roll * sin_yaw; matrix[0][1] = sin_roll * sin_pitch * cos_yaw - cos_roll * sin_yaw;
@@ -134,58 +136,57 @@ void EulerAnglesToRotationMatrix(EulerAngle euler, RotationMatrix matrix) {
matrix[2][1] = sin_roll * cos_pitch; matrix[2][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::cos(info.pitch) * std::sin(info.yaw), std::sin(info.yaw) },
std::sin(info.roll) * std::sin(info.pitch) * std::sin(info.yaw) + std::cos(info.roll) * std::cos(info.yaw), {
std::cos(info.roll) * std::sin(info.pitch) * std::sin(info.yaw) - std::sin(info.roll) * std::cos(info.yaw) }, std::cos(info.pitch) * std::sin(info.yaw),
{ std::sin(info.roll) * std::sin(info.pitch) * std::sin(info.yaw) +
-std::sin(info.pitch), std::cos(info.roll) * std::cos(info.yaw), std::cos(info.roll) *
std::sin(info.roll) * std::cos(info.pitch), std::sin(info.pitch) * std::sin(info.yaw) - std::sin(info.roll) *
std::cos(info.roll) * std::cos(info.pitch) std::cos(info.yaw) },
} {
}; -std::sin(info.pitch),
*/ std::sin(info.roll) * std::cos(info.pitch),
} std::cos(info.roll) * std::cos(info.pitch)
void PointExtrisincCompensation(PointXyz* dst_point, const PointXyz& src_point, \
ExtrinsicParameter& extrinsic) {
dst_point->x = src_point.x * extrinsic.rotation[0][0] + \
src_point.y * extrinsic.rotation[0][1] + \
src_point.z * extrinsic.rotation[0][2] + \
extrinsic.trans[0];
dst_point->y = src_point.x * extrinsic.rotation[1][0] + \
src_point.y * extrinsic.rotation[1][1] + \
src_point.z * extrinsic.rotation[1][2] + \
extrinsic.trans[1];
dst_point->z = src_point.x * extrinsic.rotation[2][0] + \
src_point.y * extrinsic.rotation[2][1] + \
src_point.z * extrinsic.rotation[2][2] +
extrinsic.trans[2];
}
/** Livox point procees for different raw data format --------------------------------------------*/
uint8_t* LivoxPointToPxyzrtl(uint8_t* point_buf, LivoxEthPacket* eth_packet, \
ExtrinsicParameter& extrinsic) {
LivoxPointXyzrtl* dst_point = (LivoxPointXyzrtl *)point_buf;
uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type);
LivoxPoint* raw_point = reinterpret_cast<LivoxPoint *>(eth_packet->data);
while(points_per_packet) {
RawPointConvert((LivoxPointXyzr*)dst_point, raw_point);
if (extrinsic.enable) {
PointXyz src_point = *((PointXyz*)dst_point);
PointExtrisincCompensation((PointXyz*)dst_point, src_point, extrinsic);
} }
dst_point->tag = 0; };
*/
}
void PointExtrisincCompensation(PointXyz *dst_point, const PointXyz &src_point,
ExtrinsicParameter &extrinsic) {
dst_point->x = src_point.x * extrinsic.rotation[0][0] +
src_point.y * extrinsic.rotation[0][1] +
src_point.z * extrinsic.rotation[0][2] + extrinsic.trans[0];
dst_point->y = src_point.x * extrinsic.rotation[1][0] +
src_point.y * extrinsic.rotation[1][1] +
src_point.z * extrinsic.rotation[1][2] + extrinsic.trans[1];
dst_point->z = src_point.x * extrinsic.rotation[2][0] +
src_point.y * extrinsic.rotation[2][1] +
src_point.z * extrinsic.rotation[2][2] + extrinsic.trans[2];
}
/** Livox point procees for different raw data format
* --------------------------------------------*/
uint8_t *LivoxPointToPxyzrtl(uint8_t *point_buf, LivoxEthPacket *eth_packet,
ExtrinsicParameter &extrinsic) {
LivoxPointXyzrtl *dst_point = (LivoxPointXyzrtl *)point_buf;
uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type);
LivoxPoint *raw_point = reinterpret_cast<LivoxPoint *>(eth_packet->data);
while (points_per_packet) {
RawPointConvert((LivoxPointXyzr *)dst_point, raw_point);
if (extrinsic.enable) {
PointXyz src_point = *((PointXyz *)dst_point);
PointExtrisincCompensation((PointXyz *)dst_point, src_point, extrinsic);
}
dst_point->tag = 0;
dst_point->line = 0; dst_point->line = 0;
++raw_point; ++raw_point;
++dst_point; ++dst_point;
@@ -195,19 +196,20 @@ 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;
++raw_point; ++raw_point;
++dst_point; ++dst_point;
@@ -217,19 +219,21 @@ 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) {
uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type); LivoxPointXyzrtl *dst_point = (LivoxPointXyzrtl *)point_buf;
LivoxSpherPoint* raw_point = reinterpret_cast<LivoxSpherPoint *>(eth_packet->data); uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type);
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;
++raw_point; ++raw_point;
++dst_point; ++dst_point;
@@ -239,21 +243,22 @@ 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) {
uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type); LivoxPointXyzrtl *dst_point = (LivoxPointXyzrtl *)point_buf;
LivoxExtendRawPoint* raw_point = \ uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type);
reinterpret_cast<LivoxExtendRawPoint *>(eth_packet->data); LivoxExtendRawPoint *raw_point =
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;
dst_point->line = dst_point->line % 6; dst_point->line = dst_point->line % 6;
++raw_point; ++raw_point;
@@ -265,20 +270,22 @@ 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) {
uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type); LivoxPointXyzrtl *dst_point = (LivoxPointXyzrtl *)point_buf;
LivoxExtendSpherPoint* raw_point = reinterpret_cast<LivoxExtendSpherPoint *>(eth_packet->data); uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type);
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;
dst_point->line = dst_point->line % 6; dst_point->line = dst_point->line % 6;
++raw_point; ++raw_point;
@@ -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) {
uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type); LivoxPointXyzrtl *dst_point = (LivoxPointXyzrtl *)point_buf;
LivoxExtendRawPoint* raw_point = \ uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type);
reinterpret_cast<LivoxExtendRawPoint *>(eth_packet->data); LivoxExtendRawPoint *raw_point =
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,31 +325,33 @@ 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) {
uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type); LivoxPointXyzrtl *dst_point = (LivoxPointXyzrtl *)point_buf;
LivoxDualExtendSpherPoint* raw_point = \ uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type);
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;
dst_point->line = dst_point->line % 6; dst_point->line = dst_point->line % 6;
++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;
dst_point->line = dst_point->line % 6; dst_point->line = dst_point->line % 6;
++dst_point; ++dst_point;
@@ -353,20 +364,19 @@ 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;
} }
const PointConvertHandler to_pxyzi_handler_table[kMaxPointDataType] = { const PointConvertHandler to_pxyzi_handler_table[kMaxPointDataType] = {
LivoxRawPointToPxyzrtl, LivoxRawPointToPxyzrtl,
LivoxSpherPointToPxyzrtl, LivoxSpherPointToPxyzrtl,
LivoxExtendRawPointToPxyzrtl, LivoxExtendRawPointToPxyzrtl,
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,11 +414,12 @@ 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) {
lidar_count_ = kMaxSourceLidar; lidar_count_ = kMaxSourceLidar;
request_exit_ = false; request_exit_ = false;
ResetLds(data_src_); ResetLds(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
}

187
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"
@@ -44,27 +44,29 @@ namespace livox_ros {
const uint32_t kMaxSourceLidar = 32; const uint32_t kMaxSourceLidar = 32;
/** Eth packet relative info parama */ /** Eth packet relative info parama */
const uint32_t kMaxPointPerEthPacket = 100; const uint32_t kMaxPointPerEthPacket = 100;
const uint32_t kMinEthPacketQueueSize = 32; /**< must be 2^n */ const uint32_t kMinEthPacketQueueSize = 32; /**< must be 2^n */
const uint32_t kMaxEthPacketQueueSize = 8192; /**< must be 2^n */ 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 kNsPerSecond = 1000000000; /**< 1s = 1000000000ns */ const int64_t kDeviceDisconnectThreshold =
1000000000; /**< the threshold of device disconect */
const int64_t kNsPerSecond = 1000000000; /**< 1s = 1000000000ns */
const int kPathStrMinSize = 4; /**< Must more than 4 char */ const int kPathStrMinSize = 4; /**< Must more than 4 char */
const int kPathStrMaxSize = 256; /**< Must less than 256 char */ const int kPathStrMaxSize = 256; /**< Must less than 256 char */
const int kBdCodeSize = 15; const int kBdCodeSize = 15;
const uint32_t kPointXYZRSize = 16; const uint32_t kPointXYZRSize = 16;
const uint32_t kPointXYZRTRSize = 18; const uint32_t kPointXYZRTRSize = 18;
const double PI = 3.14159265358979323846; const double PI = 3.14159265358979323846;
@@ -78,9 +80,9 @@ typedef enum {
/** Device data source type */ /** Device data source type */
typedef enum { typedef enum {
kSourceRawLidar = 0, /**< Data from raw lidar. */ kSourceRawLidar = 0, /**< Data from raw lidar. */
kSourceRawHub = 1, /**< Data from lidar hub. */ kSourceRawHub = 1, /**< Data from lidar hub. */
kSourceLvxFile, /**< Data from parse lvx file. */ kSourceLvxFile, /**< Data from parse lvx file. */
kSourceUndef, kSourceUndef,
} LidarDataSourceType; } LidarDataSourceType;
@@ -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,
@@ -114,9 +113,9 @@ typedef struct {
uint32_t receive_packet_count; uint32_t receive_packet_count;
uint32_t loss_packet_count; uint32_t loss_packet_count;
int64_t last_timestamp; int64_t last_timestamp;
int64_t timebase; /**< unit:ns */ int64_t timebase; /**< unit:ns */
int64_t last_imu_timestamp; int64_t last_imu_timestamp;
int64_t imu_timebase; /**< unit:ns */ int64_t imu_timebase; /**< unit:ns */
uint32_t timebase_state; uint32_t timebase_state;
} LidarPacketStatistic; } LidarPacketStatistic;
@@ -152,8 +151,8 @@ typedef struct {
volatile uint32_t get_bits; volatile uint32_t get_bits;
} UserConfig; } UserConfig;
typedef float EulerAngle[3]; /**< Roll, Pitch, Yaw, unit:radian. */ typedef float EulerAngle[3]; /**< Roll, Pitch, Yaw, unit:radian. */
typedef float TranslationVector[3]; /**< x, y, z translation, unit: m. */ typedef float TranslationVector[3]; /**< x, y, z translation, unit: m. */
typedef float RotationMatrix[3][3]; typedef float RotationMatrix[3][3];
typedef struct { typedef struct {
@@ -165,128 +164,135 @@ typedef struct {
/** Lidar data source info abstract */ /** Lidar data source info abstract */
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;
LidarDataQueue data; LidarDataQueue data;
LidarDataQueue imu_data; LidarDataQueue imu_data;
uint32_t firmware_ver; /**< Firmware version of lidar */ uint32_t firmware_ver; /**< Firmware version of lidar */
UserConfig config; UserConfig config;
ExtrinsicParameter extrinsic_parameter; ExtrinsicParameter extrinsic_parameter;
} LidarDevice; } LidarDevice;
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 */
float reflectivity; /**< Reflectivity */ float reflectivity; /**< Reflectivity */
uint8_t tag; /**< Livox point tag */ uint8_t tag; /**< Livox point tag */
uint8_t line; /**< Laser line id */ uint8_t line; /**< Laser line id */
} LivoxPointXyzrtl; } LivoxPointXyzrtl;
#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;
dst_point->x = radius * sin(theta) * cos(phi); dst_point->x = radius * sin(theta) * cos(phi);
dst_point->y = radius * sin(theta) * sin(phi); dst_point->y = radius * sin(theta) * sin(phi);
dst_point->z = radius * cos(theta); dst_point->z = radius * cos(theta);
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;
double phi = raw_point->phi / 100.0 / 180 * PI; double phi = raw_point->phi / 100.0 / 180 * PI;
dst_point1->x = radius1 * sin(theta) * cos(phi); dst_point1->x = radius1 * sin(theta) * cos(phi);
dst_point1->y = radius1 * sin(theta) * sin(phi); dst_point1->y = radius1 * sin(theta) * sin(phi);
dst_point1->z = radius1 * cos(theta); dst_point1->z = radius1 * cos(theta);
@@ -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; }
@@ -319,11 +325,12 @@ public:
LidarDevice lidars_[kMaxSourceLidar]; /**< The index is the handle */ LidarDevice lidars_[kMaxSourceLidar]; /**< The index is the handle */
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

431
livox_ros_driver/livox_ros_driver/lds_hub.cpp
View File

@@ -24,30 +24,33 @@
#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;
is_initialized_ = false; is_initialized_ = false;
whitelist_count_ = 0; whitelist_count_ = 0;
memset(broadcast_code_whitelist_, 0, sizeof(broadcast_code_whitelist_)); memset(broadcast_code_whitelist_, 0, sizeof(broadcast_code_whitelist_));
@@ -55,16 +58,15 @@ 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_) {
printf("LiDAR data source is already inited!\n"); printf("LiDAR data source is already inited!\n");
@@ -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,15 +206,16 @@ 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,20 +253,21 @@ 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,
sizeof(info->broadcast_code)) != 0) { g_lds_hub->hub_raw_config_.broadcast_code,
sizeof(info->broadcast_code)) != 0) {
printf("Could not find hub raw config, set config to default!\n"); printf("Could not find hub raw config, set config to default!\n");
config.enable_fan = 1; config.enable_fan = 1;
config.return_mode = kFirstReturn; config.return_mode = kFirstReturn;
config.coordinate = kCoordinateCartesian; config.coordinate = kCoordinateCartesian;
config.imu_rate = kImuFreq200Hz; config.imu_rate = kImuFreq200Hz;
} else { } else {
config = g_lds_hub->hub_raw_config_; config = g_lds_hub->hub_raw_config_;
} }
p_hub->config.enable_fan = config.enable_fan; p_hub->config.enable_fan = config.enable_fan;
p_hub->config.return_mode = config.return_mode; p_hub->config.return_mode = config.return_mode;
p_hub->config.coordinate = config.coordinate; p_hub->config.coordinate = config.coordinate;
p_hub->config.imu_rate = config.imu_rate; p_hub->config.imu_rate = config.imu_rate;
} else { } else {
printf("Add Hub to connect is failed : %d %d \n", result, handle); printf("Add Hub to connect is failed : %d %d \n", result, handle);
@@ -278,7 +285,7 @@ void LdsHub::OnDeviceChange(const DeviceInfo *info, DeviceEvent type) {
return; 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,11 +312,11 @@ 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,24 +367,22 @@ void LdsHub::HubErrorStatusCb(livox_status status, uint8_t handle, ErrorMessage
printf("sync_status : %u\n", message->hub_error_code.sync_status); printf("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,\ if ((handle >= kMaxLidarCount) || !response) {
void *clent_data) {
LdsHub* lds_hub = static_cast<LdsHub *>(clent_data);
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,86 +473,89 @@ 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 =
req->count = 0; (HubSetPointCloudReturnModeRequest *)req_buf;
for (int i = 0; i < kMaxLidarCount; i++) { req->count = 0;
LidarDevice* p_lidar = &(lds_hub->lidars_[i]); for (int i = 0; i < kMaxLidarCount; i++) {
LidarDevice *p_lidar = &(lds_hub->lidars_[i]);
if ((p_lidar->info.type != kDeviceTypeLidarMid40) && \ if ((p_lidar->info.type != kDeviceTypeLidarMid40) &&
(p_lidar->connect_state == kConnectStateSampling)) { (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)) {
printf("Could not find raw config, set config to default!\n"); printf("Could not find raw config, set config to default!\n");
config.enable_fan = 1; config.enable_fan = 1;
config.return_mode = kFirstReturn; config.return_mode = kFirstReturn;
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++;
} }
} }
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;
LdsHub::HubSetPointCloudReturnModeCb, lds_hub); HubSetPointCloudReturnMode(req, length,
lds_hub->hub_.config.set_bits |= kConfigReturnMode; LdsHub::HubSetPointCloudReturnModeCb, lds_hub);
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)) {
printf("Could not find raw config, set config to default!\n"); printf("Could not find raw config, set config to default!\n");
config.enable_fan = 1; config.enable_fan = 1;
config.return_mode = kFirstReturn; config.return_mode = kFirstReturn;
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++;
} }
} }
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,18 +753,18 @@ 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,
config.enable_fan = ite_config.enable_fan; kBroadcastCodeSize) == 0) {
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;
config.imu_rate = ite_config.imu_rate; config.imu_rate = ite_config.imu_rate;
return 0; return 0;
} }
} }
@@ -757,4 +772,4 @@ int LdsHub::GetRawConfig(const char* broadcast_code, UserRawConfig& config) {
return -1; return -1;
} }
} } // namespace livox_ros

71
livox_ros_driver/livox_ros_driver/lds_hub.h
View File

@@ -39,60 +39,67 @@ namespace livox_ros {
* LiDAR data source, data from hub. * 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); uint8_t response, void *clent_data);
static void HubQueryLidarInfoCb(livox_status status, uint8_t handle, \ static void StopSampleCb(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 HubQueryLidarInfoCb(livox_status status, uint8_t handle,
HubSetPointCloudReturnModeResponse* response,\ HubQueryLidarInformationResponse *response,
void *clent_data); void *client_data);
static void SetCoordinateCb(livox_status status, uint8_t handle, \ static void ControlFanCb(livox_status status, uint8_t handle,
uint8_t response, void *clent_data);
static void
HubSetPointCloudReturnModeCb(livox_status status, uint8_t handle,
HubSetPointCloudReturnModeResponse *response,
void *clent_data);
static void SetCoordinateCb(livox_status status, uint8_t handle,
uint8_t response, void *clent_data); uint8_t response, void *clent_data);
static void HubSetImuRatePushFrequencyCb(livox_status status, uint8_t handle, \ static void
HubSetImuPushFrequencyResponse* response,\ HubSetImuRatePushFrequencyCb(livox_status status, uint8_t handle,
void *clent_data); HubSetImuPushFrequencyResponse *response,
static void HubErrorStatusCb(livox_status status, uint8_t handle, ErrorMessage *message); void *clent_data);
static void ConfigPointCloudReturnMode(LdsHub* lds_hub); static void HubErrorStatusCb(livox_status status, uint8_t handle,
static void ConfigImuPushFrequency(LdsHub* lds_hub); ErrorMessage *message);
static void ConfigLidarsOfHub(LdsHub* lds_hub); 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

377
livox_ros_driver/livox_ros_driver/lds_lidar.cpp
View File

@@ -24,28 +24,31 @@
#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;
whitelist_count_ = 0; whitelist_count_ = 0;
memset(broadcast_code_whitelist_, 0, sizeof(broadcast_code_whitelist_)); memset(broadcast_code_whitelist_, 0, sizeof(broadcast_code_whitelist_));
@@ -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,67 +151,68 @@ 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();
int64_t sync_time = cur_time.time_since_epoch().count(); int64_t sync_time = cur_time.time_since_epoch().count();
packet_statistic->timebase = sync_time; // used receive time as timebase packet_statistic->timebase = sync_time; // used receive time as timebase
} }
} }
packet_statistic->last_timestamp = cur_timestamp.stamp; packet_statistic->last_timestamp = cur_timestamp.stamp;
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,25 +262,26 @@ 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;
UserRawConfig config; UserRawConfig config;
if (g_lds_ldiar->GetRawConfig(info->broadcast_code, config)) { if (g_lds_ldiar->GetRawConfig(info->broadcast_code, config)) {
printf("Could not find raw config, set config to default!\n"); printf("Could not find raw config, set config to default!\n");
config.enable_fan = 1; config.enable_fan = 1;
config.return_mode = kFirstReturn; config.return_mode = kFirstReturn;
config.coordinate = kCoordinateCartesian; config.coordinate = kCoordinateCartesian;
config.imu_rate = kImuFreq200Hz; config.imu_rate = kImuFreq200Hz;
config.extrinsic_parameter_source = kNoneExtrinsicParameter; config.extrinsic_parameter_source = kNoneExtrinsicParameter;
} }
p_lidar->config.enable_fan = config.enable_fan; p_lidar->config.enable_fan = config.enable_fan;
p_lidar->config.return_mode = config.return_mode; p_lidar->config.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(
SetPointCloudReturnModeCb, g_lds_ldiar); handle, (PointCloudReturnMode)(p_lidar->config.return_mode),
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,28 +412,29 @@ void LdsLidar::SetPointCloudReturnModeCb(livox_status status, uint8_t handle, \
} }
} else { } else {
LidarSetPointCloudReturnMode(handle, (PointCloudReturnMode)(p_lidar->config.return_mode),\ LidarSetPointCloudReturnMode(
SetPointCloudReturnModeCb, lds_lidar); handle, (PointCloudReturnMode)(p_lidar->config.return_mode),
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));
printf("Set coordinate success!\n"); printf("Set coordinate success!\n");
if (!p_lidar->config.set_bits) { if (!p_lidar->config.set_bits) {
LidarStartSampling(handle, StartSampleCb, lds_lidar); LidarStartSampling(handle, StartSampleCb, lds_lidar);
p_lidar->connect_state = kConnectStateSampling; p_lidar->connect_state = kConnectStateSampling;
} }
} else { } else {
if (p_lidar->config.coordinate != 0) { if (p_lidar->config.coordinate != 0) {
@@ -439,14 +447,14 @@ void LdsLidar::SetCoordinateCb(livox_status status, uint8_t handle, \
} }
} }
void LdsLidar::SetImuRatePushFrequencyCb(livox_status status, uint8_t handle, \ void LdsLidar::SetImuRatePushFrequencyCb(livox_status status, uint8_t handle,
uint8_t response, void *clent_data) { 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);
@@ -496,7 +504,7 @@ void LdsLidar::GetLidarExtrinsicParameterCb(livox_status status, uint8_t handle,
LidarStartSampling(handle, StartSampleCb, lds_lidar); LidarStartSampling(handle, StartSampleCb, lds_lidar);
p_lidar->connect_state = kConnectStateSampling; p_lidar->connect_state = kConnectStateSampling;
} }
} else { } else {
printf("Lidar[%d] get ExtrinsicParameter fail!\n", handle); printf("Lidar[%d] get ExtrinsicParameter fail!\n", handle);
} }
} else if (status == kStatusTimeout) { } else if (status == kStatusTimeout) {
@@ -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,17 +767,18 @@ 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,
config.enable_fan = ite_config.enable_fan; kBroadcastCodeSize) == 0) {
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;
config.imu_rate = ite_config.imu_rate; config.imu_rate = ite_config.imu_rate;
config.extrinsic_parameter_source = ite_config.extrinsic_parameter_source; config.extrinsic_parameter_source = ite_config.extrinsic_parameter_source;
return 0; return 0;
} }
@@ -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); uint8_t response, void *clent_data);
static void DeviceInformationCb(livox_status status, uint8_t handle, \ static void StopSampleCb(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 DeviceInformationCb(livox_status status, uint8_t handle,
DeviceInformationResponse *ack,
void *clent_data);
static void LidarErrorStatusCb(livox_status status, uint8_t handle,
ErrorMessage *message);
static void ControlFanCb(livox_status status, uint8_t handle,
uint8_t response, void *clent_data);
static void SetPointCloudReturnModeCb(livox_status status, uint8_t handle,
uint8_t response, void *clent_data); uint8_t response, void *clent_data);
static void SetCoordinateCb(livox_status status, uint8_t handle, \ static void SetCoordinateCb(livox_status status, uint8_t handle,
uint8_t response, void *clent_data); uint8_t response, void *clent_data);
static void SetImuRatePushFrequencyCb(livox_status status, uint8_t handle, \ static void SetImuRatePushFrequencyCb(livox_status status, uint8_t handle,
uint8_t response, void *clent_data); uint8_t response, void *clent_data);
static void SetRmcSyncTimeCb(livox_status status, uint8_t handle, \ static void SetRmcSyncTimeCb(livox_status status, uint8_t handle,
uint8_t response, void* client_data); uint8_t response, void *client_data);
static void ReceiveSyncTimeCallback(const char* rmc, uint32_t rmc_length, void* client_data); static void ReceiveSyncTimeCallback(const char *rmc, uint32_t rmc_length,
static void GetLidarExtrinsicParameterCb(livox_status status, uint8_t handle, \ void *client_data);
LidarGetExtrinsicParameterResponse *response, void *clent_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

111
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,49 +131,55 @@ 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;
int progress = 0; int progress = 0;
while (start_read_lvx_) { while (start_read_lvx_) {
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];
handle = detail_packet->device_index; eth_packet = (LivoxEthPacket *)(&detail_packet->version);
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];
handle = detail_packet->device_index; eth_packet = (LivoxEthPacket *)(&detail_packet->version);
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

17
livox_ros_driver/livox_ros_driver/lds_lvx.h
View File

@@ -39,28 +39,29 @@ 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; }
bool IsStarted() { return start_read_lvx_; } bool IsStarted() { return start_read_lvx_; }
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

46
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,16 +54,16 @@ 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;
} }
/** Init defualt system parameter */ /** Init defualt system parameter */
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;
} }

156
livox_ros_driver/livox_ros_driver/lvx_file.cpp
View File

@@ -23,24 +23,25 @@
// //
#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()
cur_offset_(0), data_start_offset_(0), size_(0), mode_(0), state_(0) { : file_ver_(kLvxFileV1), device_count_(0), cur_frame_index_(0),
cur_offset_(0), data_start_offset_(0), size_(0), mode_(0), state_(0) {
memset((void *)&public_header_, 0, sizeof(public_header_)); memset((void *)&public_header_, 0, sizeof(public_header_));
memset((void *)&private_header_, 0, sizeof(private_header_)); memset((void *)&private_header_, 0, sizeof(private_header_));
memset((void *)&private_header_v0_, 0, sizeof(private_header_v0_)); memset((void *)&private_header_v0_, 0, sizeof(private_header_v0_));
@@ -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,17 +114,17 @@ 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;
} }
AddDeviceInfo(device_info); AddDeviceInfo(device_info);
@@ -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);
sizeof(float) * 6); memcpy((void *)&device_info_v0.roll, (void *)&device_info_list_[i].roll,
memcpy(&write_buffer[cur_offset_], (void *)&device_info_v0, sizeof(device_info_v0)); sizeof(float) * 6);
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;
@@ -315,8 +336,8 @@ int LvxFileHandle::GetPacketsOfFrame(OutPacketBuffer* packets_of_frame) {
lvx_file_.read((char *)&frame_header, sizeof(frame_header)); lvx_file_.read((char *)&frame_header, sizeof(frame_header));
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

55
livox_ros_driver/timesync/timesync.cpp
View File

@@ -24,30 +24,29 @@
#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;
fn_cb_ = nullptr; fn_cb_ = nullptr;
client_data_ = nullptr; client_data_ = nullptr;
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;
@@ -87,9 +90,9 @@ int32_t TimeSync::DeInitTimeSync() {
void TimeSync::StopTimesync() { void TimeSync::StopTimesync() {
start_poll_state_ = false; start_poll_state_ = false;
start_poll_data_ = false; start_poll_data_ = false;
exit_poll_state_ = true; exit_poll_state_ = true;
exit_poll_data_ = true; exit_poll_data_ = true;
if (t_poll_state_) { if (t_poll_state_) {
t_poll_state_->join(); t_poll_state_->join();
t_poll_state_ = nullptr; t_poll_state_ = nullptr;
@@ -97,7 +100,7 @@ void TimeSync::StopTimesync() {
if (t_poll_state_) { if (t_poll_state_) {
t_poll_data_->join(); t_poll_data_->join();
t_poll_data_ = nullptr; t_poll_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,8 +152,8 @@ 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

41
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,20 +86,20 @@ 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_;
void FsmTransferState(uint8_t new_state); void FsmTransferState(uint8_t new_state);
void FsmOpenDev(); void FsmOpenDev();
void FsmPrepareDev(); void FsmPrepareDev();
void FsmCheckDevState(); void FsmCheckDevState();
}; };
} } // namespace livox_ros
#endif #endif

97
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;
@@ -123,45 +121,45 @@ int UserUart::Setup(uint8_t baudrate_index, uint8_t parity) {
options.c_cflag |= baudrate; options.c_cflag |= baudrate;
switch (parity) { switch (parity) {
case P_8N1: case P_8N1:
/** No parity (8N1) */ /** No parity (8N1) */
options.c_cflag &= ~PARENB; options.c_cflag &= ~PARENB;
options.c_cflag &= ~CSTOPB; options.c_cflag &= ~CSTOPB;
options.c_cflag &= ~CSIZE; options.c_cflag &= ~CSIZE;
options.c_cflag |= CS8; options.c_cflag |= CS8;
break; break;
case P_7E1: case P_7E1:
/** Even parity (7E1) */ /** Even parity (7E1) */
options.c_cflag |= PARENB; options.c_cflag |= PARENB;
options.c_cflag &= ~PARODD; options.c_cflag &= ~PARODD;
options.c_cflag &= ~CSTOPB; options.c_cflag &= ~CSTOPB;
options.c_cflag &= ~CSIZE; options.c_cflag &= ~CSIZE;
options.c_cflag |= CS7; options.c_cflag |= CS7;
break; break;
case P_7O1: case P_7O1:
/** Odd parity (7O1) */ /** Odd parity (7O1) */
options.c_cflag |= PARENB; options.c_cflag |= PARENB;
options.c_cflag |= PARODD; options.c_cflag |= PARODD;
options.c_cflag &= ~CSTOPB; options.c_cflag &= ~CSTOPB;
options.c_cflag &= ~CSIZE; options.c_cflag &= ~CSIZE;
options.c_cflag |= CS7; options.c_cflag |= CS7;
break; break;
case P_7S1: case P_7S1:
/** Space parity is setup the same as no parity (7S1) */ /** Space parity is setup the same as no parity (7S1) */
options.c_cflag &= ~PARENB; options.c_cflag &= ~PARENB;
options.c_cflag &= ~CSTOPB; options.c_cflag &= ~CSTOPB;
options.c_cflag &= ~CSIZE; options.c_cflag &= ~CSIZE;
options.c_cflag |= CS8; options.c_cflag |= CS8;
break; break;
default: default:
return -1; return -1;
} }
/** now we setup the values in port's termios */ /** now we setup the values in port's termios */
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

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

@@ -26,19 +26,19 @@
#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>
namespace livox_ros { namespace livox_ros {
enum Parity { enum Parity {
P_8N1, /* No parity (8N1) */ P_8N1, /* No parity (8N1) */
P_7E1, /* Even parity (7E1)*/ P_7E1, /* Even parity (7E1)*/
P_7O1, /* Odd parity (7O1) */ P_7O1, /* Odd parity (7O1) */
P_7S1, /* Space parity is setup the same as no parity (7S1) */ P_7S1, /* Space parity is setup the same as no parity (7S1) */
ParityUnkown ParityUnkown
}; };
@@ -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