├── .gitignore ├── LICENSE.txt ├── README.md ├── README_CN.md ├── images └── broadcast_code.png └── livox_ros_driver ├── CMakeLists.txt ├── cmake └── version.cmake ├── common ├── FastCRC │ ├── FastCRC.h │ ├── FastCRC_tables.hpp │ ├── FastCRCsw.cpp │ ├── LICENSE.md │ └── README.md ├── comm │ ├── comm_device.h │ ├── comm_protocol.cpp │ ├── comm_protocol.h │ ├── gps_protocol.cpp │ ├── gps_protocol.h │ ├── protocol.h │ ├── sdk_protocol.cpp │ └── sdk_protocol.h ├── rapidjson │ ├── allocators.h │ ├── cursorstreamwrapper.h │ ├── document.h │ ├── encodedstream.h │ ├── encodings.h │ ├── error │ │ ├── en.h │ │ └── error.h │ ├── filereadstream.h │ ├── filewritestream.h │ ├── fwd.h │ ├── internal │ │ ├── biginteger.h │ │ ├── clzll.h │ │ ├── diyfp.h │ │ ├── dtoa.h │ │ ├── ieee754.h │ │ ├── itoa.h │ │ ├── meta.h │ │ ├── pow10.h │ │ ├── regex.h │ │ ├── stack.h │ │ ├── strfunc.h │ │ ├── strtod.h │ │ └── swap.h │ ├── istreamwrapper.h │ ├── memorybuffer.h │ ├── memorystream.h │ ├── msinttypes │ │ ├── inttypes.h │ │ └── stdint.h │ ├── ostreamwrapper.h │ ├── pointer.h │ ├── prettywriter.h │ ├── rapidjson.h │ ├── reader.h │ ├── schema.h │ ├── stream.h │ ├── stringbuffer.h │ └── writer.h └── rapidxml │ ├── rapidxml.hpp │ ├── rapidxml_iterators.hpp │ ├── rapidxml_print.hpp │ └── rapidxml_utils.hpp ├── config ├── display_hub_points.rviz ├── display_lidar_points.rviz ├── livox_hub_config.json └── livox_lidar_config.json ├── launch ├── livox_hub.launch ├── livox_hub_msg.launch ├── livox_hub_rviz.launch ├── livox_lidar.launch ├── livox_lidar_msg.launch ├── livox_lidar_rviz.launch ├── livox_template.launch ├── lvx_to_rosbag.launch └── lvx_to_rosbag_rviz.launch ├── livox_ros_driver ├── include │ └── livox_ros_driver.h ├── lddc.cpp ├── lddc.h ├── ldq.cpp ├── ldq.h ├── lds.cpp ├── lds.h ├── lds_hub.cpp ├── lds_hub.h ├── lds_lidar.cpp ├── lds_lidar.h ├── lds_lvx.cpp ├── lds_lvx.h ├── livox_ros_driver.cpp ├── lvx_file.cpp └── lvx_file.h ├── msg ├── CustomMsg.msg └── CustomPoint.msg ├── package.xml └── timesync ├── timesync.cpp ├── timesync.h └── user_uart ├── user_uart.cpp └── user_uart.h /.gitignore: -------------------------------------------------------------------------------- 1 | 2 | # ignore dir or file 3 | CMakeLists.txt 4 | livox_ros_driver/Livox-SDK 5 | #whitelist 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /LICENSE.txt: -------------------------------------------------------------------------------- 1 | The following portions of the LIVOX’s Livox SDK (“Software” referred to in the terms below) are made available to you under the terms of the MIT License provided below. 2 | 3 | livox_ros_driver 4 | ├── LICENSE.txt 5 | ├── livox_ros_driver 6 | │   ├── CMakeLists.txt 7 | │   ├── config 8 | │   │   ├── display_hub_points.rviz 9 | │   │   └── display_lidar_points.rviz 10 | │   ├── launch 11 | │   │   ├── livox_hub.launch 12 | │   │   ├── livox_hub_msg.launch 13 | │   │   ├── livox_hub_rviz.launch 14 | │   │   ├── livox_lidar.launch 15 | │   │   ├── livox_lidar_msg.launch 16 | │   │   └── livox_lidar_rviz.launch 17 | │   ├── livox_hub.cpp 18 | │   ├── livox_lidar.cpp 19 | │   ├── msg 20 | │   │   ├── CustomMsg.msg 21 | │   │   └── CustomPoint.msg 22 | │   └── package.xml 23 | └── README.md 24 | 25 | 26 | ------------------------------------------------------------- 27 | The MIT License (MIT) 28 | 29 | Copyright (c) 2019 Livox. All rights reserved. 30 | 31 | Permission is hereby granted, free of charge, to any person obtaining a copy 32 | of this software and associated documentation files (the "Software"), to deal 33 | in the Software without restriction, including without limitation the rights 34 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 35 | copies of the Software, and to permit persons to whom the Software is 36 | furnished to do so, subject to the following conditions: 37 | 38 | The above copyright notice and this permission notice shall be included in 39 | all copies or substantial portions of the Software. 40 | 41 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 42 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 43 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 44 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 45 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 46 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 47 | SOFTWARE. 48 | =============================================================== 49 | LIVOX’s Livox SDK uses unmodified libraries of Boost (https://www.boost.org), which is licensed under the Boost Software license. A copy of the Boost Software license is provided below and is also available at https://www.boost.org/LICENSE_1_0.txt. 50 | 51 | ------------------------------------------------------------- 52 | Boost Software License - Version 1.0 - August 17th, 2003 53 | 54 | Permission is hereby granted, free of charge, to any person or organization 55 | obtaining a copy of the software and accompanying documentation covered by 56 | this license (the "Software") to use, reproduce, display, distribute, 57 | execute, and transmit the Software, and to prepare derivative works of the 58 | Software, and to permit third-parties to whom the Software is furnished to 59 | do so, all subject to the following: 60 | 61 | The copyright notices in the Software and this entire statement, including 62 | the above license grant, this restriction and the following disclaimer, 63 | must be included in all copies of the Software, in whole or in part, and 64 | all derivative works of the Software, unless such copies or derivative 65 | works are solely in the form of machine-executable object code generated by 66 | a source language processor. 67 | 68 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 69 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 70 | FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT 71 | SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE 72 | FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE, 73 | ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 74 | DEALINGS IN THE SOFTWARE. 75 | =============================================================== 76 | FastCRC 77 | └── sdk_core 78 | ├── include 79 | │   └── third_party 80 | │   └── FastCRC 81 | │   └── FastCRC.h 82 | └── src 83 | └── third_party 84 | └── FastCRC 85 | ├── FastCRCsw.cpp 86 | ├── FastCRC_tables.hpp 87 | ├── LICENSE.md 88 | └── README.md 89 | 90 | LIVOX’s Livox SDK uses modified libraries of FastCRC (https://github.com/FrankBoesing/FastCRC), which is licensed under MIT license. A copy of the MIT license is provided below and is also available at https://raw.githubusercontent.com/FrankBoesing/FastCRC/master/LICENCE.md. 91 | 92 | The MIT License (MIT) 93 | 94 | Copyright (c) 2016 Frank 95 | 96 | Permission is hereby granted, free of charge, to any person obtaining a copy 97 | of this software and associated documentation files (the "Software"), to deal 98 | in the Software without restriction, including without limitation the rights 99 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 100 | copies of the Software, and to permit persons to whom the Software is 101 | furnished to do so, subject to the following conditions: 102 | 103 | The above copyright notice and this permission notice shall be included in all 104 | copies or substantial portions of the Software. 105 | 106 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 107 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 108 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 109 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 110 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 111 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 112 | SOFTWARE. 113 | =============================================================== 114 | LIVOX’s Livox SDK uses unmodified libraries of Ros (https://github.com/ros/ros), which is licensed under BSD license. A copy of the BSD license is provided below and is also available at https://opensource.org/licenses/BSD-3-Clause. 115 | 116 | ------------------------------------------------------------- 117 | Copyright (c) 2001 - 2009, The Board of Trustees of the University of Illinois. 118 | All rights reserved. 119 | 120 | Redistribution and use in source and binary forms, with or without 121 | modification, are permitted provided that the following conditions are 122 | met: 123 | 124 | * Redistributions of source code must retain the above 125 | copyright notice, this list of conditions and the 126 | following disclaimer. 127 | 128 | * Redistributions in binary form must reproduce the 129 | above copyright notice, this list of conditions 130 | and the following disclaimer in the documentation 131 | and/or other materials provided with the distribution. 132 | 133 | * Neither the name of the University of Illinois 134 | nor the names of its contributors may be used to 135 | endorse or promote products derived from this 136 | software without specific prior written permission. 137 | 138 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 139 | IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 140 | THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 141 | PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR 142 | CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 143 | EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 144 | PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 145 | PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 146 | LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 147 | NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 148 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 149 | 150 | -------------------------------------------------------------------------------- /images/broadcast_code.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Livox-SDK/livox_ros_driver/3d240d5666129e1a3052e78ee8487a04b08fdda3/images/broadcast_code.png -------------------------------------------------------------------------------- /livox_ros_driver/cmake/version.cmake: -------------------------------------------------------------------------------- 1 | #--------------------------------------------------------------------------------------- 2 | # Get livox_ros_driver version from include/livox_ros_driver.h 3 | #--------------------------------------------------------------------------------------- 4 | file(READ "${CMAKE_CURRENT_LIST_DIR}/../livox_ros_driver/include/livox_ros_driver.h" LIVOX_ROS_DRIVER_VERSION_FILE) 5 | string(REGEX MATCH "LIVOX_ROS_DRIVER_VER_MAJOR ([0-9]+)" _ "${LIVOX_ROS_DRIVER_VERSION_FILE}") 6 | set(ver_major ${CMAKE_MATCH_1}) 7 | 8 | string(REGEX MATCH "LIVOX_ROS_DRIVER_VER_MINOR ([0-9]+)" _ "${LIVOX_ROS_DRIVER_VERSION_FILE}") 9 | set(ver_minor ${CMAKE_MATCH_1}) 10 | string(REGEX MATCH "LIVOX_ROS_DRIVER_VER_PATCH ([0-9]+)" _ "${LIVOX_ROS_DRIVER_VERSION_FILE}") 11 | set(ver_patch ${CMAKE_MATCH_1}) 12 | 13 | if (NOT DEFINED ver_major OR NOT DEFINED ver_minor OR NOT DEFINED ver_patch) 14 | message(FATAL_ERROR "Could not extract valid version from include/livox_ros_driver.h") 15 | endif() 16 | set (LIVOX_ROS_DRIVER_VERSION "${ver_major}.${ver_minor}.${ver_patch}") 17 | -------------------------------------------------------------------------------- /livox_ros_driver/common/FastCRC/FastCRC.h: -------------------------------------------------------------------------------- 1 | /* FastCRC library code is placed under the MIT license 2 | * Copyright (c) 2014,2015 Frank Bosing 3 | * 4 | * Permission is hereby granted, free of charge, to any person obtaining 5 | * a copy of this software and associated documentation files (the 6 | * "Software"), to deal in the Software without restriction, including 7 | * without limitation the rights to use, copy, modify, merge, publish, 8 | * distribute, sublicense, and/or sell copies of the Software, and to 9 | * permit persons to whom the Software is furnished to do so, subject to 10 | * the following conditions: 11 | * 12 | * The above copyright notice and this permission notice shall be 13 | * included in all copies or substantial portions of the Software. 14 | * 15 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 16 | * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 17 | * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 18 | * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS 19 | * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN 20 | * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | * SOFTWARE. 23 | */ 24 | 25 | // Teensy 3.0, Teensy 3.1: 26 | // See K20P64M72SF1RM.pdf (Kinetis), Pages 638 - 641 for documentation of CRC 27 | // Device See KINETIS_4N30D.pdf for Errata (Errata ID 2776) 28 | // 29 | // So, ALL HW-calculations are done as 32 bit. 30 | // 31 | // 32 | // 33 | // Thanks to: 34 | // - Catalogue of parametrised CRC algorithms, CRC RevEng 35 | // http://reveng.sourceforge.net/crc-catalogue/ 36 | // 37 | // - Danjel McGougan (CRC-Table-Generator) 38 | // 39 | 40 | // 41 | // modify from FastCRC library @ 2018/11/20 42 | // 43 | 44 | #ifndef FASTCRC_FASTCRC_H_ 45 | #define FASTCRC_FASTCRC_H_ 46 | 47 | #include 48 | 49 | // ================= 16-BIT CRC =================== 50 | 51 | class FastCRC16 { 52 | public: 53 | FastCRC16(uint16_t seed); 54 | 55 | // change function name from mcrf4xx_upd to mcrf4xx 56 | uint16_t mcrf4xx_calc( 57 | const uint8_t *data, 58 | const uint16_t datalen); // Equivalent to _crc_ccitt_update() in 59 | // crc16.h from avr_libc 60 | 61 | private: 62 | uint16_t seed_; 63 | }; 64 | 65 | // ================= 32-BIT CRC =================== 66 | 67 | class FastCRC32 { 68 | public: 69 | FastCRC32(uint32_t seed); 70 | 71 | // change function name from crc32_upd to crc32 72 | uint32_t crc32_calc( 73 | const uint8_t *data, 74 | uint16_t len); // Call for subsequent calculations with previous seed 75 | 76 | private: 77 | uint32_t seed_; 78 | }; 79 | 80 | #endif // FASTCRC_FASTCRC_H_ 81 | -------------------------------------------------------------------------------- /livox_ros_driver/common/FastCRC/FastCRCsw.cpp: -------------------------------------------------------------------------------- 1 | /* FastCRC library code is placed under the MIT license 2 | * Copyright (c) 2014,2015,2016 Frank Bosing 3 | * 4 | * Permission is hereby granted, free of charge, to any person obtaining 5 | * a copy of this software and associated documentation files (the 6 | * "Software"), to deal in the Software without restriction, including 7 | * without limitation the rights to use, copy, modify, merge, publish, 8 | * distribute, sublicense, and/or sell copies of the Software, and to 9 | * permit persons to whom the Software is furnished to do so, subject to 10 | * the following conditions: 11 | * 12 | * The above copyright notice and this permission notice shall be 13 | * included in all copies or substantial portions of the Software. 14 | * 15 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 16 | * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 17 | * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 18 | * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS 19 | * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN 20 | * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | * SOFTWARE. 23 | */ 24 | 25 | // 26 | // Thanks to: 27 | // - Catalogue of parametrised CRC algorithms, CRC RevEng 28 | // http://reveng.sourceforge.net/crc-catalogue/ 29 | // 30 | // - Danjel McGougan (CRC-Table-Generator) 31 | // 32 | 33 | // 34 | // modify from FastCRC library @ 2018/11/20 35 | // 36 | 37 | #include "FastCRC.h" 38 | #include "FastCRC_tables.hpp" 39 | 40 | // ================= 16-BIT CRC =================== 41 | /** Constructor 42 | */ 43 | FastCRC16::FastCRC16(uint16_t seed) { seed_ = seed; } 44 | 45 | #define crc_n4(crc, data, table) \ 46 | crc ^= data; \ 47 | crc = (table[(crc & 0xff) + 0x300]) ^ (table[((crc >> 8) & 0xff) + 0x200]) ^ \ 48 | (table[((data >> 16) & 0xff) + 0x100]) ^ (table[data >> 24]); 49 | 50 | /** MCRF4XX 51 | * equivalent to _crc_ccitt_update() in crc16.h from avr_libc 52 | * @param data Pointer to Data 53 | * @param datalen Length of Data 54 | * @return CRC value 55 | */ 56 | 57 | uint16_t FastCRC16::mcrf4xx_calc(const uint8_t *data, uint16_t len) { 58 | uint16_t crc = seed_; 59 | 60 | while (((uintptr_t)data & 3) && len) { 61 | crc = (crc >> 8) ^ crc_table_mcrf4xx[(crc & 0xff) ^ *data++]; 62 | len--; 63 | } 64 | 65 | while (len >= 16) { 66 | len -= 16; 67 | crc_n4(crc, ((uint32_t *)data)[0], crc_table_mcrf4xx); 68 | crc_n4(crc, ((uint32_t *)data)[1], crc_table_mcrf4xx); 69 | crc_n4(crc, ((uint32_t *)data)[2], crc_table_mcrf4xx); 70 | crc_n4(crc, ((uint32_t *)data)[3], crc_table_mcrf4xx); 71 | data += 16; 72 | } 73 | 74 | while (len--) { 75 | crc = (crc >> 8) ^ crc_table_mcrf4xx[(crc & 0xff) ^ *data++]; 76 | } 77 | 78 | // seed = crc; 79 | return crc; 80 | } 81 | 82 | // ================= 32-BIT CRC =================== 83 | /** Constructor 84 | */ 85 | FastCRC32::FastCRC32(uint32_t seed) { seed_ = seed; } 86 | 87 | #define crc_n4d(crc, data, table) \ 88 | crc ^= data; \ 89 | crc = (table[(crc & 0xff) + 0x300]) ^ (table[((crc >> 8) & 0xff) + 0x200]) ^ \ 90 | (table[((crc >> 16) & 0xff) + 0x100]) ^ (table[(crc >> 24) & 0xff]); 91 | 92 | #define crcsm_n4d(crc, data, table) \ 93 | crc ^= data; \ 94 | crc = (crc >> 8) ^ (table[crc & 0xff]); \ 95 | crc = (crc >> 8) ^ (table[crc & 0xff]); \ 96 | crc = (crc >> 8) ^ (table[crc & 0xff]); \ 97 | crc = (crc >> 8) ^ (table[crc & 0xff]); 98 | 99 | /** CRC32 100 | * Alias CRC-32/ADCCP, PKZIP, Ethernet, 802.3 101 | * @param data Pointer to Data 102 | * @param datalen Length of Data 103 | * @return CRC value 104 | */ 105 | #if CRC_BIGTABLES 106 | #define CRC_TABLE_CRC32 crc_table_crc32_big 107 | #else 108 | #define CRC_TABLE_CRC32 crc_table_crc32 109 | #endif 110 | 111 | uint32_t FastCRC32::crc32_calc(const uint8_t *data, uint16_t len) { 112 | uint32_t crc = seed_ ^ 0xffffffff; 113 | 114 | while (((uintptr_t)data & 3) && len) { 115 | crc = (crc >> 8) ^ CRC_TABLE_CRC32[(crc & 0xff) ^ *data++]; 116 | len--; 117 | } 118 | 119 | while (len >= 16) { 120 | len -= 16; 121 | #if CRC_BIGTABLES 122 | crc_n4d(crc, ((uint32_t *)data)[0], CRC_TABLE_CRC32); 123 | crc_n4d(crc, ((uint32_t *)data)[1], CRC_TABLE_CRC32); 124 | crc_n4d(crc, ((uint32_t *)data)[2], CRC_TABLE_CRC32); 125 | crc_n4d(crc, ((uint32_t *)data)[3], CRC_TABLE_CRC32); 126 | #else 127 | crcsm_n4d(crc, ((uint32_t *)data)[0], CRC_TABLE_CRC32); 128 | crcsm_n4d(crc, ((uint32_t *)data)[1], CRC_TABLE_CRC32); 129 | crcsm_n4d(crc, ((uint32_t *)data)[2], CRC_TABLE_CRC32); 130 | crcsm_n4d(crc, ((uint32_t *)data)[3], CRC_TABLE_CRC32); 131 | #endif 132 | data += 16; 133 | } 134 | 135 | while (len--) { 136 | crc = (crc >> 8) ^ CRC_TABLE_CRC32[(crc & 0xff) ^ *data++]; 137 | } 138 | 139 | // seed = crc; 140 | crc ^= 0xffffffff; 141 | 142 | return crc; 143 | } 144 | -------------------------------------------------------------------------------- /livox_ros_driver/common/FastCRC/LICENSE.md: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2016 Frank 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /livox_ros_driver/common/FastCRC/README.md: -------------------------------------------------------------------------------- 1 | FastCRC 2 | ======= 3 | 4 | Fast CRC Arduino library 5 | Up to 30 times faster than crc16.h (_avr_libc) 6 | 7 | - uses the on-chip hardware for Teensy 3.0 / 3.1 / 3.2 / 3.5 / 3.6 8 | - uses fast table-algorithms for other chips 9 | 10 | List of supported CRC calculations: 11 | - 12 | 7 BIT: 13 | 14 | CRC7 15 | (poly=0x09 init=0x00 refin=false refout=false xorout=0x00 check=0x75) 16 | MultiMediaCard interface 17 | 18 | 19 | 8 BIT: 20 | 21 | SMBUS 22 | (poly=0x07 init=0x00 refin=false refout=false xorout=0x00 check=0xf4) 23 | 24 | MAXIM 25 | (poly=0x31 init=0x00 refin=true refout=true xorout=0x00 check=0xa1) 26 | 27 | 28 | 16 BIT: 29 | 30 | KERMIT (Alias CRC-16/CCITT, CRC-16/CCITT-TRUE, CRC-CCITT) 31 | (poly=0x1021 init=0x0000 refin=true refout=true xorout=0x0000 check=0x2189 32 | Attention: sometimes you'll find byteswapped presentation of result in other implementations) 33 | 34 | CCITT-FALSE 35 | (poly=0x1021 init=0xffff refin=false refout=false xorout=0x0000 check=0x29b1) 36 | 37 | MCRF4XX 38 | (poly=0x1021 init=0xffff refin=true refout=true xorout=0x0000 check=0x6f91) 39 | 40 | MODBUS 41 | (poly=0x8005 init=0xffff refin=true refout=true xorout=0x0000 check=0x4b37) 42 | 43 | XMODEM (Alias ZMODEM, CRC-16/ACORN) 44 | (poly=0x1021 init=0x0000 refin=false refout=false xorout=0x0000 check=0x31c3) 45 | 46 | X25 (Alias CRC-16/IBM-SDLC, CRC-16/ISO-HDLC, CRC-B) 47 | (poly=0x1021 init=0xffff refin=true refout=true xorout=0xffff check=0x906e) 48 | 49 | 50 | 32 BIT: 51 | 52 | CRC32, CRC-32/ADCCP, PKZIP, ETHERNET, 802.3 53 | (poly=0x04c11db7 init=0xffffffff refin=true refout=true xorout=0xffffffff check=0xcbf43926) 54 | 55 | CKSUM, CRC-32/POSIX 56 | (poly=0x04c11db7 init=0x00000000 refin=false refout=false xorout=0xffffffff check=0x765e7680) 57 | -------------------------------------------------------------------------------- /livox_ros_driver/common/comm/comm_device.h: -------------------------------------------------------------------------------- 1 | // 2 | // The MIT License (MIT) 3 | // 4 | // Copyright (c) 2019 Livox. All rights reserved. 5 | // 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | // 13 | // The above copyright notice and this permission notice shall be included in 14 | // all copies or substantial portions of the Software. 15 | // 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | // 24 | 25 | #ifndef COMM_COMM_DEVICE_H_ 26 | #define COMM_COMM_DEVICE_H_ 27 | 28 | #include 29 | 30 | namespace livox_ros { 31 | 32 | const uint32_t kDevNameLengthMax = 256; 33 | 34 | /** Communication device type define */ 35 | enum CommDeviceType { 36 | kCommDevUart, 37 | kCommDevUsb, 38 | kCommDevCan, 39 | kCommDevEthernet, 40 | kCommDevUndef 41 | }; 42 | 43 | /** Communication device uart config */ 44 | struct CommDevUartConfig { 45 | uint8_t baudrate; 46 | uint8_t parity; 47 | }; 48 | 49 | /** Communication device usb config */ 50 | struct CommDevUsbConfig { 51 | void *data; 52 | }; 53 | 54 | /** Communication device can config */ 55 | struct CommDevCanConfig { 56 | void *data; 57 | }; 58 | 59 | /** Communication device config */ 60 | typedef struct { 61 | uint8_t type; 62 | char name[kDevNameLengthMax]; 63 | union { 64 | CommDevUartConfig uart; 65 | CommDevUsbConfig usb; 66 | CommDevCanConfig can; 67 | } config; 68 | } CommDevConfig; 69 | 70 | } // namespace livox_ros 71 | #endif // COMM_COMM_DEVICE_H_ 72 | -------------------------------------------------------------------------------- /livox_ros_driver/common/comm/comm_protocol.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // The MIT License (MIT) 3 | // 4 | // Copyright (c) 2019 Livox. All rights reserved. 5 | // 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | // 13 | // The above copyright notice and this permission notice shall be included in 14 | // all copies or substantial portions of the Software. 15 | // 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | // 24 | 25 | #include "comm_protocol.h" 26 | #include 27 | #include 28 | #include 29 | 30 | namespace livox_ros { 31 | 32 | CommProtocol::CommProtocol(ProtocolConfig &config) : config_(config) { 33 | ResetParser(); 34 | ResetCache(); 35 | 36 | offset_to_read_index_ = 0; 37 | packet_length_ = 0; 38 | 39 | if (kGps == config.type) { 40 | is_length_known = false; 41 | protocol_ = new GpsProtocol(); 42 | } else { 43 | is_length_known = true; 44 | protocol_ = nullptr; 45 | } 46 | } 47 | 48 | CommProtocol::~CommProtocol() { 49 | if (protocol_) { 50 | delete protocol_; 51 | } 52 | } 53 | 54 | uint8_t *CommProtocol::FetchCacheFreeSpace(uint32_t *o_len) { 55 | UpdateCache(); 56 | 57 | if (cache_.wr_idx < cache_.size) { 58 | *o_len = cache_.size - cache_.wr_idx; 59 | return &cache_.buf[cache_.wr_idx]; 60 | } else { 61 | *o_len = 0; 62 | return nullptr; 63 | } 64 | } 65 | 66 | int32_t CommProtocol::UpdateCacheWrIdx(uint32_t used_size) { 67 | if ((cache_.wr_idx + used_size) < cache_.size) { 68 | cache_.wr_idx += used_size; 69 | return 0; 70 | } else { 71 | return -1; 72 | } 73 | } 74 | 75 | uint32_t CommProtocol::GetCacheTailSize() { 76 | if (cache_.wr_idx < cache_.size) { 77 | return cache_.size - cache_.wr_idx; 78 | } else { 79 | return 0; 80 | } 81 | } 82 | 83 | uint32_t CommProtocol::GetValidDataSize() { 84 | if (cache_.wr_idx > cache_.rd_idx) { 85 | return cache_.wr_idx - cache_.rd_idx; 86 | } else { 87 | return 0; 88 | } 89 | } 90 | 91 | void CommProtocol::UpdateCache(void) { 92 | if (GetCacheTailSize() < 93 | kMoveCacheLimit) { /* move unused data to cache head */ 94 | uint32_t valid_data_size = GetValidDataSize(); 95 | 96 | if (valid_data_size) { 97 | memmove(cache_.buf, &cache_.buf[cache_.rd_idx], valid_data_size); 98 | cache_.rd_idx = 0; 99 | cache_.wr_idx = valid_data_size; 100 | } else if (cache_.rd_idx) { 101 | cache_.rd_idx = 0; 102 | cache_.wr_idx = 0; 103 | } 104 | } 105 | } 106 | 107 | int32_t CommProtocol::Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len, 108 | const CommPacket &i_packet) { 109 | return protocol_->Pack(o_buf, o_buf_size, o_len, i_packet); 110 | } 111 | 112 | void CommProtocol::ResetParser() { fsm_parse_step_ = kSearchPacketPreamble; } 113 | 114 | int32_t CommProtocol::ParseCommStream(CommPacket *o_pack) { 115 | int32_t ret = kParseFail; 116 | while ((GetValidDataSize() > protocol_->GetPreambleLen()) && 117 | (GetValidDataSize() > offset_to_read_index_)) { 118 | switch (fsm_parse_step_) { 119 | case kSearchPacketPreamble: { 120 | FsmSearchPacketPreamble(); 121 | break; 122 | } 123 | case kFindPacketLength: { 124 | FsmFindPacketLength(); 125 | break; 126 | } 127 | case kGetPacketData: { 128 | ret = FsmGetPacketData(o_pack); 129 | break; 130 | } 131 | default: { FsmParserStateTransfer(kSearchPacketPreamble); } 132 | } 133 | 134 | /* Must exit when in the below case */ 135 | if ((ret == kParseSuccess) || (ret == kParseNeedMoreData)) break; 136 | } 137 | 138 | return ret; 139 | } 140 | 141 | uint16_t CommProtocol::GetAndUpdateSeqNum() { 142 | /* add lock here for thread safe */ 143 | uint16_t seq = seq_num_; 144 | seq_num_++; 145 | 146 | return seq; 147 | } 148 | 149 | int32_t CommProtocol::FsmSearchPacketPreamble() { 150 | do { 151 | if (!protocol_->CheckPreamble(GetCacheReadPos())) { 152 | if (!is_length_known) { 153 | offset_to_read_index_ = 0; 154 | packet_length_ = 0; 155 | FsmParserStateTransfer(kFindPacketLength); 156 | break; 157 | } else { 158 | packet_length_ = protocol_->GetPacketLen(GetCacheReadPos()); 159 | if ((packet_length_ < cache_.size) && 160 | (packet_length_ > 161 | protocol_ 162 | ->GetPacketWrapperLen())) { /* check the legality of length */ 163 | FsmParserStateTransfer(kGetPacketData); 164 | break; 165 | } 166 | } 167 | } 168 | // printf("|%2x", cache_.buf[cache_.rd_idx]); 169 | ++cache_.rd_idx; /* skip one byte */ 170 | } while (0); 171 | 172 | return 0; 173 | } 174 | 175 | int32_t CommProtocol::FsmFindPacketLength() { 176 | uint32_t valid_data_size = GetValidDataSize(); 177 | uint32_t ret = protocol_->FindPacketLen(GetCacheReadPos(), valid_data_size); 178 | if (ret == kFindLengthSuccess) { 179 | packet_length_ = protocol_->GetPacketLen(GetCacheReadPos()); 180 | FsmParserStateTransfer(kGetPacketData); 181 | offset_to_read_index_ = 0; 182 | } else if (ret == kFindLengthContinue) { /* continue to find next time */ 183 | offset_to_read_index_ = valid_data_size; 184 | } else { /* find length error */ 185 | offset_to_read_index_ = 0; 186 | cache_.rd_idx += valid_data_size; 187 | FsmParserStateTransfer(kSearchPacketPreamble); 188 | printf("Continue to find length error\n"); 189 | } 190 | 191 | return 0; 192 | } 193 | 194 | int32_t CommProtocol::FsmGetPacketData(CommPacket *o_pack) { 195 | int32_t ret = kParseFail; 196 | do { 197 | if (GetValidDataSize() < packet_length_) { 198 | ret = kParseNeedMoreData; 199 | break; 200 | } 201 | 202 | if (protocol_->CheckPacket(GetCacheReadPos())) { 203 | cache_.rd_idx += protocol_->GetPreambleLen(); 204 | FsmParserStateTransfer(kSearchPacketPreamble); 205 | printf("Check packet error\n"); 206 | break; 207 | } 208 | 209 | if (protocol_->ParsePacket(GetCacheReadPos(), packet_length_, o_pack)) { 210 | cache_.rd_idx += protocol_->GetPacketLen(GetCacheReadPos()); 211 | FsmParserStateTransfer(kSearchPacketPreamble); 212 | printf("Parse packet error\n"); 213 | break; 214 | } 215 | 216 | cache_.rd_idx += protocol_->GetPacketLen(GetCacheReadPos()); 217 | FsmParserStateTransfer(kSearchPacketPreamble); 218 | return kParseSuccess; 219 | } while (0); 220 | 221 | return ret; 222 | } 223 | 224 | } // namespace livox_ros 225 | -------------------------------------------------------------------------------- /livox_ros_driver/common/comm/comm_protocol.h: -------------------------------------------------------------------------------- 1 | // 2 | // The MIT License (MIT) 3 | // 4 | // Copyright (c) 2019 Livox. All rights reserved. 5 | // 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | // 13 | // The above copyright notice and this permission notice shall be included in 14 | // all copies or substantial portions of the Software. 15 | // 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | // 24 | 25 | #ifndef COMM_COMM_PROTOCOL_H_ 26 | #define COMM_COMM_PROTOCOL_H_ 27 | 28 | #include 29 | #include "gps_protocol.h" 30 | #include "protocol.h" 31 | #include "sdk_protocol.h" 32 | 33 | namespace livox_ros { 34 | const uint32_t kCacheSize = 8192; 35 | const uint32_t kMoveCacheLimit = 1536; 36 | 37 | enum FsmParseState { 38 | kSearchPacketPreamble = 0, 39 | kFindPacketLength = 1, 40 | kGetPacketData = 2, 41 | kParseStepUndef 42 | }; 43 | 44 | /** Communication data cache define */ 45 | typedef struct { 46 | uint8_t buf[kCacheSize]; 47 | uint32_t rd_idx; 48 | uint32_t wr_idx; 49 | uint32_t size; 50 | } CommCache; 51 | 52 | class CommProtocol { 53 | public: 54 | CommProtocol(ProtocolConfig &config); 55 | ~CommProtocol(); 56 | 57 | int32_t Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len, 58 | const CommPacket &i_packet); 59 | 60 | int32_t ParseCommStream(CommPacket *o_pack); 61 | 62 | uint8_t *FetchCacheFreeSpace(uint32_t *o_len); 63 | 64 | int32_t UpdateCacheWrIdx(uint32_t used_size); 65 | 66 | uint16_t GetAndUpdateSeqNum(); 67 | 68 | void ResetParser(); 69 | 70 | private: 71 | uint32_t GetCacheTailSize(); 72 | uint32_t GetValidDataSize(); 73 | void UpdateCache(void); 74 | uint8_t *GetCacheReadPos() { return &cache_.buf[cache_.rd_idx]; } 75 | void ResetCache() { 76 | cache_.wr_idx = 0; 77 | cache_.rd_idx = 0; 78 | cache_.size = kCacheSize; 79 | } 80 | 81 | ProtocolConfig config_; 82 | Protocol *protocol_; 83 | CommCache cache_; 84 | uint16_t seq_num_; 85 | 86 | bool is_length_known; 87 | bool IsLengthKnown() { return is_length_known; } 88 | 89 | volatile uint32_t offset_to_read_index_; 90 | uint32_t packet_length_; 91 | volatile uint32_t fsm_parse_step_; 92 | int32_t FsmSearchPacketPreamble(); 93 | int32_t FsmFindPacketLength(); 94 | int32_t FsmGetPacketData(CommPacket *o_pack); 95 | void FsmParserStateTransfer(uint32_t new_state) { 96 | if (new_state < kParseStepUndef) { 97 | fsm_parse_step_ = new_state; 98 | } else { 99 | fsm_parse_step_ = kSearchPacketPreamble; 100 | } 101 | } 102 | }; 103 | 104 | } // namespace livox_ros 105 | #endif // COMM_COMM_PROTOCOL_H_ 106 | -------------------------------------------------------------------------------- /livox_ros_driver/common/comm/gps_protocol.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // The MIT License (MIT) 3 | // 4 | // Copyright (c) 2019 Livox. All rights reserved. 5 | // 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | // 13 | // The above copyright notice and this permission notice shall be included in 14 | // all copies or substantial portions of the Software. 15 | // 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | // 24 | 25 | #include "gps_protocol.h" 26 | 27 | #include 28 | #include 29 | #include 30 | 31 | namespace livox_ros { 32 | 33 | const uint8_t kGpsProtocolSof = '$'; 34 | const uint8_t kGpsProtocolEof = '*'; 35 | const uint32_t kPacketLengthLmit = 200; 36 | const uint32_t kPreambleLen = 1; 37 | const uint32_t kWrapperLen = 4; /** '$' + '*' + '2 checksum byte' */ 38 | 39 | GpsProtocol::GpsProtocol() { found_length_ = 0; } 40 | 41 | int32_t GpsProtocol::Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len, 42 | const CommPacket &i_packet) { 43 | // GpsPacket* gps_packet = (GpsPacket*)o_buf; 44 | return 0; 45 | } 46 | 47 | int32_t GpsProtocol::ParsePacket(const uint8_t *i_buf, uint32_t i_len, 48 | CommPacket *o_packet) { 49 | // GpsPacket *gps_packet = (GpsPacket *)i_buf; 50 | 51 | if (i_len < GetPacketWrapperLen()) { 52 | return -1; // packet length error 53 | } 54 | memset((void *)o_packet, 0, sizeof(CommPacket)); 55 | o_packet->protocol = kGps; 56 | o_packet->data = (uint8_t *)i_buf; 57 | o_packet->data_len = i_len; 58 | 59 | return 0; 60 | } 61 | 62 | uint32_t GpsProtocol::GetPreambleLen() { return kPreambleLen; /** '$' */ } 63 | 64 | uint32_t GpsProtocol::GetPacketWrapperLen() { 65 | return kWrapperLen; /** '$' + '*' + '2 checksum bytes' */ 66 | } 67 | 68 | uint32_t GpsProtocol::FindPacketLen(const uint8_t *buf, uint32_t buf_length) { 69 | uint32_t i = 0; 70 | for (; (i < buf_length) && (i < kPacketLengthLmit); i++) { 71 | if ((buf[i] == kGpsProtocolEof) && (buf[0] == kGpsProtocolSof)) { 72 | found_length_ = i + 1 + 2; /* num = index + 1 + two bytes checksum */ 73 | return kFindLengthSuccess; 74 | } 75 | } 76 | if (i < kPacketLengthLmit) { 77 | return kFindLengthContinue; 78 | } else { 79 | return kFindLengthError; 80 | } 81 | } 82 | 83 | uint32_t GpsProtocol::GetPacketLen(const uint8_t *buf) { return found_length_; } 84 | 85 | int32_t GpsProtocol::CheckPreamble(const uint8_t *buf) { 86 | GpsPreamble *preamble = (GpsPreamble *)buf; 87 | 88 | if (preamble->sof == kGpsProtocolSof) { 89 | return 0; 90 | } else { 91 | return -1; 92 | } 93 | } 94 | 95 | int32_t GpsProtocol::CheckPacket(const uint8_t *buf) { 96 | uint8_t checksum = 97 | CalcGpsPacketChecksum(&buf[1], found_length_ - kWrapperLen); 98 | uint8_t raw_checksum = AscciiToHex(&buf[found_length_ - 2]); 99 | if (checksum == raw_checksum) { 100 | return 0; 101 | } else { 102 | return -1; 103 | } 104 | } 105 | 106 | uint8_t GpsProtocol::CalcGpsPacketChecksum(const uint8_t *buf, 107 | uint32_t length) { 108 | uint8_t result = buf[0]; 109 | for (uint32_t i = 1; i < length; i++) { 110 | result ^= buf[i]; 111 | } 112 | return result; 113 | } 114 | 115 | uint8_t AscciiToHex(const uint8_t *TwoChar) { 116 | uint8_t h = toupper(TwoChar[0]) - 0x30; 117 | if (h > 9) h -= 7; 118 | 119 | uint8_t l = toupper(TwoChar[1]) - 0x30; 120 | if (l > 9) l -= 7; 121 | 122 | return h * 16 + l; 123 | } 124 | 125 | } // namespace livox_ros 126 | -------------------------------------------------------------------------------- /livox_ros_driver/common/comm/gps_protocol.h: -------------------------------------------------------------------------------- 1 | // 2 | // The MIT License (MIT) 3 | // 4 | // Copyright (c) 2019 Livox. All rights reserved. 5 | // 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | // 13 | // The above copyright notice and this permission notice shall be included in 14 | // all copies or substantial portions of the Software. 15 | // 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | // 24 | 25 | #ifndef LIVOX_GPS_PROTOCOL_H_ 26 | #define LIVOX_GPS_PROTOCOL_H_ 27 | 28 | #include 29 | #include "protocol.h" 30 | 31 | namespace livox_ros { 32 | 33 | #pragma pack(1) 34 | 35 | typedef struct { 36 | uint8_t sof; 37 | uint8_t cmd_str[1]; 38 | } GpsPreamble; 39 | 40 | typedef struct { 41 | uint8_t sof; 42 | uint8_t data[1]; 43 | } GpsPacket; 44 | 45 | #pragma pack() 46 | 47 | uint8_t AscciiToHex(const uint8_t *TwoChar); 48 | 49 | class GpsProtocol : public Protocol { 50 | public: 51 | GpsProtocol(); 52 | ~GpsProtocol() = default; 53 | 54 | int32_t ParsePacket(const uint8_t *i_buf, uint32_t i_len, 55 | CommPacket *o_packet) override; 56 | 57 | int32_t Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len, 58 | const CommPacket &i_packet) override; 59 | 60 | uint32_t GetPreambleLen() override; 61 | 62 | uint32_t GetPacketWrapperLen() override; 63 | 64 | uint32_t FindPacketLen(const uint8_t *buf, uint32_t buf_length) override; 65 | 66 | uint32_t GetPacketLen(const uint8_t *buf) override; 67 | 68 | int32_t CheckPreamble(const uint8_t *buf) override; 69 | 70 | int32_t CheckPacket(const uint8_t *buf) override; 71 | 72 | private: 73 | uint32_t found_length_; 74 | 75 | uint8_t CalcGpsPacketChecksum(const uint8_t *buf, uint32_t length); 76 | }; 77 | 78 | } // namespace livox_ros 79 | #endif // LIVOX_GPS_PROTOCOL_H_ 80 | -------------------------------------------------------------------------------- /livox_ros_driver/common/comm/protocol.h: -------------------------------------------------------------------------------- 1 | // 2 | // The MIT License (MIT) 3 | // 4 | // Copyright (c) 2019 Livox. All rights reserved. 5 | // 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | // 13 | // The above copyright notice and this permission notice shall be included in 14 | // all copies or substantial portions of the Software. 15 | // 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | // 24 | 25 | #ifndef COMM_PROTOCOL_H_ 26 | #define COMM_PROTOCOL_H_ 27 | 28 | #include 29 | 30 | namespace livox_ros { 31 | typedef struct CommPacket CommPacket; 32 | 33 | typedef int (*RequestPackCb)(CommPacket *packet); 34 | 35 | typedef enum { kRequestPack, kAckPack, kMsgPack } PacketType; 36 | 37 | typedef enum { kLidarSdk, kRsvd1, kGps, kProtocolUndef } ProtocolType; 38 | 39 | typedef enum { kNoNeed, kNeedAck, kDelayAck } NeedAckType; 40 | 41 | typedef enum { kParseSuccess, kParseFail, kParseNeedMoreData } ParseResult; 42 | 43 | typedef enum { 44 | kFindLengthSuccess, 45 | kFindLengthContinue, 46 | kFindLengthError 47 | } FindLengthResult; 48 | 49 | typedef struct CommPacket { 50 | uint8_t packet_type; 51 | uint8_t protocol; 52 | uint8_t protocol_version; 53 | uint8_t cmd_set; 54 | uint32_t cmd_code; 55 | uint32_t sender; 56 | uint32_t sub_sender; 57 | uint32_t receiver; 58 | uint32_t sub_receiver; 59 | uint32_t seq_num; 60 | uint8_t *data; 61 | uint16_t data_len; 62 | uint32_t padding; 63 | } CommPacket; 64 | 65 | /** SDK Protocol info config */ 66 | typedef struct { 67 | uint16_t seed16; 68 | uint16_t seed32; 69 | } SdkProtocolConfig; 70 | 71 | /** NAME-0183 Protocol info config for gps */ 72 | typedef struct { void *data; } GpsProtocolConfig; 73 | 74 | typedef struct { 75 | uint8_t type; 76 | union { 77 | SdkProtocolConfig sdk; 78 | GpsProtocolConfig gps; 79 | } config; 80 | } ProtocolConfig; 81 | 82 | class Protocol { 83 | public: 84 | virtual ~Protocol() = default; 85 | 86 | virtual int32_t ParsePacket(const uint8_t *i_buf, uint32_t i_len, 87 | CommPacket *o_packet) = 0; 88 | 89 | virtual int32_t Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len, 90 | const CommPacket &i_packet) = 0; 91 | 92 | virtual uint32_t GetPreambleLen() = 0; 93 | 94 | virtual uint32_t GetPacketWrapperLen() = 0; 95 | 96 | virtual uint32_t FindPacketLen(const uint8_t *buf, uint32_t buf_length) = 0; 97 | 98 | virtual uint32_t GetPacketLen(const uint8_t *buf) = 0; 99 | 100 | virtual int32_t CheckPreamble(const uint8_t *buf) = 0; 101 | 102 | virtual int32_t CheckPacket(const uint8_t *buf) = 0; 103 | }; 104 | 105 | } // namespace livox_ros 106 | #endif // COMM_PROTOCOL_H_ 107 | -------------------------------------------------------------------------------- /livox_ros_driver/common/comm/sdk_protocol.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // The MIT License (MIT) 3 | // 4 | // Copyright (c) 2019 Livox. All rights reserved. 5 | // 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | // 13 | // The above copyright notice and this permission notice shall be included in 14 | // all copies or substantial portions of the Software. 15 | // 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | // 24 | 25 | #include "sdk_protocol.h" 26 | 27 | #include 28 | #include 29 | 30 | namespace livox_ros { 31 | const uint8_t kSdkProtocolSof = 0xAA; 32 | const uint32_t kSdkPacketCrcSize = 4; // crc32 33 | const uint32_t kSdkPacketPreambleCrcSize = 2; // crc16 34 | 35 | SdkProtocol::SdkProtocol(uint16_t seed16, uint32_t seed32) 36 | : crc16_(seed16), crc32_(seed32) {} 37 | 38 | int32_t SdkProtocol::Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len, 39 | const CommPacket &i_packet) { 40 | SdkPacket *sdk_packet = (SdkPacket *)o_buf; 41 | 42 | if (kLidarSdk != i_packet.protocol) { 43 | return -1; 44 | } 45 | 46 | sdk_packet->sof = kSdkProtocolSof; 47 | sdk_packet->length = i_packet.data_len + GetPacketWrapperLen(); 48 | 49 | if (sdk_packet->length > o_buf_size) { 50 | return -1; 51 | } 52 | 53 | sdk_packet->version = kSdkVer0; 54 | sdk_packet->packet_type = i_packet.packet_type; 55 | sdk_packet->seq_num = i_packet.seq_num & 0xFFFF; 56 | sdk_packet->preamble_crc = 57 | crc16_.mcrf4xx_calc(o_buf, GetPreambleLen() - kSdkPacketPreambleCrcSize); 58 | sdk_packet->cmd_set = i_packet.cmd_set; 59 | sdk_packet->cmd_id = i_packet.cmd_code; 60 | 61 | memcpy(sdk_packet->data, i_packet.data, i_packet.data_len); 62 | 63 | uint32_t crc = 64 | crc32_.crc32_calc(o_buf, sdk_packet->length - kSdkPacketCrcSize); 65 | o_buf[sdk_packet->length - 4] = crc & 0xFF; 66 | o_buf[sdk_packet->length - 3] = (crc >> 8) & 0xFF; 67 | o_buf[sdk_packet->length - 2] = (crc >> 16) & 0xFF; 68 | o_buf[sdk_packet->length - 1] = (crc >> 24) & 0xFF; 69 | 70 | *o_len = sdk_packet->length; 71 | 72 | return 0; 73 | } 74 | 75 | int32_t SdkProtocol::ParsePacket(const uint8_t *i_buf, uint32_t i_len, 76 | CommPacket *o_packet) { 77 | SdkPacket *sdk_packet = (SdkPacket *)i_buf; 78 | 79 | if (i_len < GetPacketWrapperLen()) { 80 | return -1; // packet lenth error 81 | } 82 | 83 | memset((void *)o_packet, 0, sizeof(CommPacket)); 84 | 85 | o_packet->packet_type = sdk_packet->packet_type; 86 | o_packet->protocol = kLidarSdk; 87 | o_packet->protocol_version = sdk_packet->version; 88 | 89 | o_packet->seq_num = sdk_packet->seq_num; 90 | o_packet->cmd_set = sdk_packet->cmd_set; 91 | o_packet->cmd_code = sdk_packet->cmd_id; 92 | o_packet->data_len = sdk_packet->length - GetPacketWrapperLen(); 93 | o_packet->data = sdk_packet->data; 94 | 95 | return 0; 96 | } 97 | 98 | uint32_t SdkProtocol::GetPreambleLen() { return sizeof(SdkPreamble); } 99 | 100 | uint32_t SdkProtocol::GetPacketWrapperLen() { 101 | return sizeof(SdkPacket) - 1 + kSdkPacketCrcSize; 102 | } 103 | 104 | uint32_t SdkProtocol::GetPacketLen(const uint8_t *buf) { 105 | SdkPreamble *preamble = (SdkPreamble *)buf; 106 | return preamble->length; 107 | } 108 | 109 | int32_t SdkProtocol::CheckPreamble(const uint8_t *buf) { 110 | SdkPreamble *preamble = (SdkPreamble *)buf; 111 | 112 | if ((preamble->sof == kSdkProtocolSof) && 113 | (crc16_.mcrf4xx_calc(buf, GetPreambleLen()) == 0)) { 114 | return 0; 115 | } else { 116 | return -1; 117 | } 118 | } 119 | 120 | int32_t SdkProtocol::CheckPacket(const uint8_t *buf) { 121 | SdkPacket *sdk_packet = (SdkPacket *)buf; 122 | 123 | uint32_t crc = ((uint32_t)(buf[sdk_packet->length - 4])) | 124 | (((uint32_t)(buf[sdk_packet->length - 3])) << 8) | 125 | (((uint32_t)(buf[sdk_packet->length - 2])) << 16) | 126 | (((uint32_t)(buf[sdk_packet->length - 1])) << 24); 127 | 128 | if (crc32_.crc32_calc(buf, sdk_packet->length - kSdkPacketCrcSize) == crc) { 129 | return 0; 130 | } else { 131 | return -1; 132 | } 133 | } 134 | } // namespace livox_ros 135 | -------------------------------------------------------------------------------- /livox_ros_driver/common/comm/sdk_protocol.h: -------------------------------------------------------------------------------- 1 | // 2 | // The MIT License (MIT) 3 | // 4 | // Copyright (c) 2019 Livox. All rights reserved. 5 | // 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | // 13 | // The above copyright notice and this permission notice shall be included in 14 | // all copies or substantial portions of the Software. 15 | // 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | // 24 | 25 | #ifndef LIVOX_SDK_PROTOCOL_H_ 26 | #define LIVOX_SDK_PROTOCOL_H_ 27 | 28 | #include 29 | #include "FastCRC/FastCRC.h" 30 | #include "protocol.h" 31 | 32 | namespace livox_ros { 33 | typedef enum { kSdkVerNone, kSdkVer0, kSdkVer1 } SdkVersion; 34 | 35 | #pragma pack(1) 36 | 37 | typedef struct { 38 | uint8_t sof; 39 | uint8_t version; 40 | uint16_t length; 41 | uint8_t packet_type; 42 | uint16_t seq_num; 43 | uint16_t preamble_crc; 44 | } SdkPreamble; 45 | 46 | typedef struct { 47 | uint8_t sof; 48 | uint8_t version; 49 | uint16_t length; 50 | uint8_t packet_type; 51 | uint16_t seq_num; 52 | uint16_t preamble_crc; 53 | uint8_t cmd_set; 54 | uint8_t cmd_id; 55 | uint8_t data[1]; 56 | } SdkPacket; 57 | 58 | #pragma pack() 59 | 60 | class SdkProtocol : public Protocol { 61 | public: 62 | SdkProtocol(uint16_t seed16, uint32_t seed32); 63 | ~SdkProtocol() = default; 64 | 65 | int32_t ParsePacket(const uint8_t *i_buf, uint32_t i_len, 66 | CommPacket *o_packet) override; 67 | 68 | int32_t Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len, 69 | const CommPacket &i_packet) override; 70 | 71 | uint32_t GetPreambleLen() override; 72 | 73 | uint32_t GetPacketWrapperLen() override; 74 | 75 | uint32_t GetPacketLen(const uint8_t *buf) override; 76 | 77 | int32_t CheckPreamble(const uint8_t *buf) override; 78 | 79 | int32_t CheckPacket(const uint8_t *buf) override; 80 | 81 | private: 82 | FastCRC16 crc16_; 83 | FastCRC32 crc32_; 84 | }; 85 | } // namespace livox_ros 86 | #endif // LIVOX_SDK_PROTOCOL_H_ 87 | -------------------------------------------------------------------------------- /livox_ros_driver/common/rapidjson/cursorstreamwrapper.h: -------------------------------------------------------------------------------- 1 | // Tencent is pleased to support the open source community by making RapidJSON 2 | // available. 3 | // 4 | // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All 5 | // rights reserved. 6 | // 7 | // Licensed under the MIT License (the "License"); you may not use this file 8 | // except in compliance with the License. You may obtain a copy of the License 9 | // at 10 | // 11 | // http://opensource.org/licenses/MIT 12 | // 13 | // Unless required by applicable law or agreed to in writing, software 14 | // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT 15 | // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the 16 | // License for the specific language governing permissions and limitations under 17 | // the License. 18 | 19 | #ifndef RAPIDJSON_CURSORSTREAMWRAPPER_H_ 20 | #define RAPIDJSON_CURSORSTREAMWRAPPER_H_ 21 | 22 | #include "stream.h" 23 | 24 | #if defined(__GNUC__) 25 | RAPIDJSON_DIAG_PUSH 26 | RAPIDJSON_DIAG_OFF(effc++) 27 | #endif 28 | 29 | #if defined(_MSC_VER) && _MSC_VER <= 1800 30 | RAPIDJSON_DIAG_PUSH 31 | RAPIDJSON_DIAG_OFF(4702) // unreachable code 32 | RAPIDJSON_DIAG_OFF(4512) // assignment operator could not be generated 33 | #endif 34 | 35 | RAPIDJSON_NAMESPACE_BEGIN 36 | 37 | //! Cursor stream wrapper for counting line and column number if error exists. 38 | /*! 39 | \tparam InputStream Any stream that implements Stream Concept 40 | */ 41 | template > 42 | class CursorStreamWrapper : public GenericStreamWrapper { 43 | public: 44 | typedef typename Encoding::Ch Ch; 45 | 46 | CursorStreamWrapper(InputStream &is) 47 | : GenericStreamWrapper(is), line_(1), col_(0) {} 48 | 49 | // counting line and column number 50 | Ch Take() { 51 | Ch ch = this->is_.Take(); 52 | if (ch == '\n') { 53 | line_++; 54 | col_ = 0; 55 | } else { 56 | col_++; 57 | } 58 | return ch; 59 | } 60 | 61 | //! Get the error line number, if error exists. 62 | size_t GetLine() const { return line_; } 63 | //! Get the error column number, if error exists. 64 | size_t GetColumn() const { return col_; } 65 | 66 | private: 67 | size_t line_; //!< Current Line 68 | size_t col_; //!< Current Column 69 | }; 70 | 71 | #if defined(_MSC_VER) && _MSC_VER <= 1800 72 | RAPIDJSON_DIAG_POP 73 | #endif 74 | 75 | #if defined(__GNUC__) 76 | RAPIDJSON_DIAG_POP 77 | #endif 78 | 79 | RAPIDJSON_NAMESPACE_END 80 | 81 | #endif // RAPIDJSON_CURSORSTREAMWRAPPER_H_ 82 | -------------------------------------------------------------------------------- /livox_ros_driver/common/rapidjson/error/en.h: -------------------------------------------------------------------------------- 1 | // Tencent is pleased to support the open source community by making RapidJSON 2 | // available. 3 | // 4 | // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All 5 | // rights reserved. 6 | // 7 | // Licensed under the MIT License (the "License"); you may not use this file 8 | // except in compliance with the License. You may obtain a copy of the License 9 | // at 10 | // 11 | // http://opensource.org/licenses/MIT 12 | // 13 | // Unless required by applicable law or agreed to in writing, software 14 | // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT 15 | // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the 16 | // License for the specific language governing permissions and limitations under 17 | // the License. 18 | 19 | #ifndef RAPIDJSON_ERROR_EN_H_ 20 | #define RAPIDJSON_ERROR_EN_H_ 21 | 22 | #include "error.h" 23 | 24 | #ifdef __clang__ 25 | RAPIDJSON_DIAG_PUSH 26 | RAPIDJSON_DIAG_OFF(switch - enum) 27 | RAPIDJSON_DIAG_OFF(covered - switch - default) 28 | #endif 29 | 30 | RAPIDJSON_NAMESPACE_BEGIN 31 | 32 | //! Maps error code of parsing into error message. 33 | /*! 34 | \ingroup RAPIDJSON_ERRORS 35 | \param parseErrorCode Error code obtained in parsing. 36 | \return the error message. 37 | \note User can make a copy of this function for localization. 38 | Using switch-case is safer for future modification of error codes. 39 | */ 40 | inline const RAPIDJSON_ERROR_CHARTYPE* GetParseError_En( 41 | ParseErrorCode parseErrorCode) { 42 | switch (parseErrorCode) { 43 | case kParseErrorNone: 44 | return RAPIDJSON_ERROR_STRING("No error."); 45 | 46 | case kParseErrorDocumentEmpty: 47 | return RAPIDJSON_ERROR_STRING("The document is empty."); 48 | case kParseErrorDocumentRootNotSingular: 49 | return RAPIDJSON_ERROR_STRING( 50 | "The document root must not be followed by other values."); 51 | 52 | case kParseErrorValueInvalid: 53 | return RAPIDJSON_ERROR_STRING("Invalid value."); 54 | 55 | case kParseErrorObjectMissName: 56 | return RAPIDJSON_ERROR_STRING("Missing a name for object member."); 57 | case kParseErrorObjectMissColon: 58 | return RAPIDJSON_ERROR_STRING( 59 | "Missing a colon after a name of object member."); 60 | case kParseErrorObjectMissCommaOrCurlyBracket: 61 | return RAPIDJSON_ERROR_STRING( 62 | "Missing a comma or '}' after an object member."); 63 | 64 | case kParseErrorArrayMissCommaOrSquareBracket: 65 | return RAPIDJSON_ERROR_STRING( 66 | "Missing a comma or ']' after an array element."); 67 | 68 | case kParseErrorStringUnicodeEscapeInvalidHex: 69 | return RAPIDJSON_ERROR_STRING( 70 | "Incorrect hex digit after \\u escape in string."); 71 | case kParseErrorStringUnicodeSurrogateInvalid: 72 | return RAPIDJSON_ERROR_STRING("The surrogate pair in string is invalid."); 73 | case kParseErrorStringEscapeInvalid: 74 | return RAPIDJSON_ERROR_STRING("Invalid escape character in string."); 75 | case kParseErrorStringMissQuotationMark: 76 | return RAPIDJSON_ERROR_STRING( 77 | "Missing a closing quotation mark in string."); 78 | case kParseErrorStringInvalidEncoding: 79 | return RAPIDJSON_ERROR_STRING("Invalid encoding in string."); 80 | 81 | case kParseErrorNumberTooBig: 82 | return RAPIDJSON_ERROR_STRING("Number too big to be stored in double."); 83 | case kParseErrorNumberMissFraction: 84 | return RAPIDJSON_ERROR_STRING("Miss fraction part in number."); 85 | case kParseErrorNumberMissExponent: 86 | return RAPIDJSON_ERROR_STRING("Miss exponent in number."); 87 | 88 | case kParseErrorTermination: 89 | return RAPIDJSON_ERROR_STRING("Terminate parsing due to Handler error."); 90 | case kParseErrorUnspecificSyntaxError: 91 | return RAPIDJSON_ERROR_STRING("Unspecific syntax error."); 92 | 93 | default: 94 | return RAPIDJSON_ERROR_STRING("Unknown error."); 95 | } 96 | } 97 | 98 | RAPIDJSON_NAMESPACE_END 99 | 100 | #ifdef __clang__ 101 | RAPIDJSON_DIAG_POP 102 | #endif 103 | 104 | #endif // RAPIDJSON_ERROR_EN_H_ 105 | -------------------------------------------------------------------------------- /livox_ros_driver/common/rapidjson/error/error.h: -------------------------------------------------------------------------------- 1 | // Tencent is pleased to support the open source community by making RapidJSON 2 | // available. 3 | // 4 | // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All 5 | // rights reserved. 6 | // 7 | // Licensed under the MIT License (the "License"); you may not use this file 8 | // except in compliance with the License. You may obtain a copy of the License 9 | // at 10 | // 11 | // http://opensource.org/licenses/MIT 12 | // 13 | // Unless required by applicable law or agreed to in writing, software 14 | // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT 15 | // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the 16 | // License for the specific language governing permissions and limitations under 17 | // the License. 18 | 19 | #ifndef RAPIDJSON_ERROR_ERROR_H_ 20 | #define RAPIDJSON_ERROR_ERROR_H_ 21 | 22 | #include "../rapidjson.h" 23 | 24 | #ifdef __clang__ 25 | RAPIDJSON_DIAG_PUSH 26 | RAPIDJSON_DIAG_OFF(padded) 27 | #endif 28 | 29 | /*! \file error.h */ 30 | 31 | /*! \defgroup RAPIDJSON_ERRORS RapidJSON error handling */ 32 | 33 | /////////////////////////////////////////////////////////////////////////////// 34 | // RAPIDJSON_ERROR_CHARTYPE 35 | 36 | //! Character type of error messages. 37 | /*! \ingroup RAPIDJSON_ERRORS 38 | The default character type is \c char. 39 | On Windows, user can define this macro as \c TCHAR for supporting both 40 | unicode/non-unicode settings. 41 | */ 42 | #ifndef RAPIDJSON_ERROR_CHARTYPE 43 | #define RAPIDJSON_ERROR_CHARTYPE char 44 | #endif 45 | 46 | /////////////////////////////////////////////////////////////////////////////// 47 | // RAPIDJSON_ERROR_STRING 48 | 49 | //! Macro for converting string literial to \ref RAPIDJSON_ERROR_CHARTYPE[]. 50 | /*! \ingroup RAPIDJSON_ERRORS 51 | By default this conversion macro does nothing. 52 | On Windows, user can define this macro as \c _T(x) for supporting both 53 | unicode/non-unicode settings. 54 | */ 55 | #ifndef RAPIDJSON_ERROR_STRING 56 | #define RAPIDJSON_ERROR_STRING(x) x 57 | #endif 58 | 59 | RAPIDJSON_NAMESPACE_BEGIN 60 | 61 | /////////////////////////////////////////////////////////////////////////////// 62 | // ParseErrorCode 63 | 64 | //! Error code of parsing. 65 | /*! \ingroup RAPIDJSON_ERRORS 66 | \see GenericReader::Parse, GenericReader::GetParseErrorCode 67 | */ 68 | enum ParseErrorCode { 69 | kParseErrorNone = 0, //!< No error. 70 | 71 | kParseErrorDocumentEmpty, //!< The document is empty. 72 | kParseErrorDocumentRootNotSingular, //!< The document root must not follow by 73 | //!< other values. 74 | 75 | kParseErrorValueInvalid, //!< Invalid value. 76 | 77 | kParseErrorObjectMissName, //!< Missing a name for object member. 78 | kParseErrorObjectMissColon, //!< Missing a colon after a name of object 79 | //!< member. 80 | kParseErrorObjectMissCommaOrCurlyBracket, //!< Missing a comma or '}' after 81 | //!an 82 | //!< object member. 83 | 84 | kParseErrorArrayMissCommaOrSquareBracket, //!< Missing a comma or ']' after 85 | //!an 86 | //!< array element. 87 | 88 | kParseErrorStringUnicodeEscapeInvalidHex, //!< Incorrect hex digit after \\u 89 | //!< escape in string. 90 | kParseErrorStringUnicodeSurrogateInvalid, //!< The surrogate pair in string 91 | //!is 92 | //!< invalid. 93 | kParseErrorStringEscapeInvalid, //!< Invalid escape character in string. 94 | kParseErrorStringMissQuotationMark, //!< Missing a closing quotation mark in 95 | //!< string. 96 | kParseErrorStringInvalidEncoding, //!< Invalid encoding in string. 97 | 98 | kParseErrorNumberTooBig, //!< Number too big to be stored in double. 99 | kParseErrorNumberMissFraction, //!< Miss fraction part in number. 100 | kParseErrorNumberMissExponent, //!< Miss exponent in number. 101 | 102 | kParseErrorTermination, //!< Parsing was terminated. 103 | kParseErrorUnspecificSyntaxError //!< Unspecific syntax error. 104 | }; 105 | 106 | //! Result of parsing (wraps ParseErrorCode) 107 | /*! 108 | \ingroup RAPIDJSON_ERRORS 109 | \code 110 | Document doc; 111 | ParseResult ok = doc.Parse("[42]"); 112 | if (!ok) { 113 | fprintf(stderr, "JSON parse error: %s (%u)", 114 | GetParseError_En(ok.Code()), ok.Offset()); 115 | exit(EXIT_FAILURE); 116 | } 117 | \endcode 118 | \see GenericReader::Parse, GenericDocument::Parse 119 | */ 120 | struct ParseResult { 121 | //!! Unspecified boolean type 122 | typedef bool (ParseResult::*BooleanType)() const; 123 | 124 | public: 125 | //! Default constructor, no error. 126 | ParseResult() : code_(kParseErrorNone), offset_(0) {} 127 | //! Constructor to set an error. 128 | ParseResult(ParseErrorCode code, size_t offset) 129 | : code_(code), offset_(offset) {} 130 | 131 | //! Get the error code. 132 | ParseErrorCode Code() const { return code_; } 133 | //! Get the error offset, if \ref IsError(), 0 otherwise. 134 | size_t Offset() const { return offset_; } 135 | 136 | //! Explicit conversion to \c bool, returns \c true, iff !\ref IsError(). 137 | operator BooleanType() const { 138 | return !IsError() ? &ParseResult::IsError : NULL; 139 | } 140 | //! Whether the result is an error. 141 | bool IsError() const { return code_ != kParseErrorNone; } 142 | 143 | bool operator==(const ParseResult &that) const { return code_ == that.code_; } 144 | bool operator==(ParseErrorCode code) const { return code_ == code; } 145 | friend bool operator==(ParseErrorCode code, const ParseResult &err) { 146 | return code == err.code_; 147 | } 148 | 149 | bool operator!=(const ParseResult &that) const { return !(*this == that); } 150 | bool operator!=(ParseErrorCode code) const { return !(*this == code); } 151 | friend bool operator!=(ParseErrorCode code, const ParseResult &err) { 152 | return err != code; 153 | } 154 | 155 | //! Reset error code. 156 | void Clear() { Set(kParseErrorNone); } 157 | //! Update error code and offset. 158 | void Set(ParseErrorCode code, size_t offset = 0) { 159 | code_ = code; 160 | offset_ = offset; 161 | } 162 | 163 | private: 164 | ParseErrorCode code_; 165 | size_t offset_; 166 | }; 167 | 168 | //! Function pointer type of GetParseError(). 169 | /*! \ingroup RAPIDJSON_ERRORS 170 | 171 | This is the prototype for \c GetParseError_X(), where \c X is a locale. 172 | User can dynamically change locale in runtime, e.g.: 173 | \code 174 | GetParseErrorFunc GetParseError = GetParseError_En; // or whatever 175 | const RAPIDJSON_ERROR_CHARTYPE* s = 176 | GetParseError(document.GetParseErrorCode()); \endcode 177 | */ 178 | typedef const RAPIDJSON_ERROR_CHARTYPE *(*GetParseErrorFunc)(ParseErrorCode); 179 | 180 | RAPIDJSON_NAMESPACE_END 181 | 182 | #ifdef __clang__ 183 | RAPIDJSON_DIAG_POP 184 | #endif 185 | 186 | #endif // RAPIDJSON_ERROR_ERROR_H_ 187 | -------------------------------------------------------------------------------- /livox_ros_driver/common/rapidjson/filereadstream.h: -------------------------------------------------------------------------------- 1 | // Tencent is pleased to support the open source community by making RapidJSON 2 | // available. 3 | // 4 | // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All 5 | // rights reserved. 6 | // 7 | // Licensed under the MIT License (the "License"); you may not use this file 8 | // except in compliance with the License. You may obtain a copy of the License 9 | // at 10 | // 11 | // http://opensource.org/licenses/MIT 12 | // 13 | // Unless required by applicable law or agreed to in writing, software 14 | // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT 15 | // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the 16 | // License for the specific language governing permissions and limitations under 17 | // the License. 18 | 19 | #ifndef RAPIDJSON_FILEREADSTREAM_H_ 20 | #define RAPIDJSON_FILEREADSTREAM_H_ 21 | 22 | #include 23 | #include "stream.h" 24 | 25 | #ifdef __clang__ 26 | RAPIDJSON_DIAG_PUSH 27 | RAPIDJSON_DIAG_OFF(padded) 28 | RAPIDJSON_DIAG_OFF(unreachable - code) 29 | RAPIDJSON_DIAG_OFF(missing - noreturn) 30 | #endif 31 | 32 | RAPIDJSON_NAMESPACE_BEGIN 33 | 34 | //! File byte stream for input using fread(). 35 | /*! 36 | \note implements Stream concept 37 | */ 38 | class FileReadStream { 39 | public: 40 | typedef char Ch; //!< Character type (byte). 41 | 42 | //! Constructor. 43 | /*! 44 | \param fp File pointer opened for read. 45 | \param buffer user-supplied buffer. 46 | \param bufferSize size of buffer in bytes. Must >=4 bytes. 47 | */ 48 | FileReadStream(std::FILE *fp, char *buffer, size_t bufferSize) 49 | : fp_(fp), 50 | buffer_(buffer), 51 | bufferSize_(bufferSize), 52 | bufferLast_(0), 53 | current_(buffer_), 54 | readCount_(0), 55 | count_(0), 56 | eof_(false) { 57 | RAPIDJSON_ASSERT(fp_ != 0); 58 | RAPIDJSON_ASSERT(bufferSize >= 4); 59 | Read(); 60 | } 61 | 62 | Ch Peek() const { return *current_; } 63 | Ch Take() { 64 | Ch c = *current_; 65 | Read(); 66 | return c; 67 | } 68 | size_t Tell() const { 69 | return count_ + static_cast(current_ - buffer_); 70 | } 71 | 72 | // Not implemented 73 | void Put(Ch) { RAPIDJSON_ASSERT(false); } 74 | void Flush() { RAPIDJSON_ASSERT(false); } 75 | Ch *PutBegin() { 76 | RAPIDJSON_ASSERT(false); 77 | return 0; 78 | } 79 | size_t PutEnd(Ch *) { 80 | RAPIDJSON_ASSERT(false); 81 | return 0; 82 | } 83 | 84 | // For encoding detection only. 85 | const Ch *Peek4() const { 86 | return (current_ + 4 - !eof_ <= bufferLast_) ? current_ : 0; 87 | } 88 | 89 | private: 90 | void Read() { 91 | if (current_ < bufferLast_) 92 | ++current_; 93 | else if (!eof_) { 94 | count_ += readCount_; 95 | readCount_ = std::fread(buffer_, 1, bufferSize_, fp_); 96 | bufferLast_ = buffer_ + readCount_ - 1; 97 | current_ = buffer_; 98 | 99 | if (readCount_ < bufferSize_) { 100 | buffer_[readCount_] = '\0'; 101 | ++bufferLast_; 102 | eof_ = true; 103 | } 104 | } 105 | } 106 | 107 | std::FILE *fp_; 108 | Ch *buffer_; 109 | size_t bufferSize_; 110 | Ch *bufferLast_; 111 | Ch *current_; 112 | size_t readCount_; 113 | size_t count_; //!< Number of characters read 114 | bool eof_; 115 | }; 116 | 117 | RAPIDJSON_NAMESPACE_END 118 | 119 | #ifdef __clang__ 120 | RAPIDJSON_DIAG_POP 121 | #endif 122 | 123 | #endif // RAPIDJSON_FILESTREAM_H_ 124 | -------------------------------------------------------------------------------- /livox_ros_driver/common/rapidjson/filewritestream.h: -------------------------------------------------------------------------------- 1 | // Tencent is pleased to support the open source community by making RapidJSON 2 | // available. 3 | // 4 | // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All 5 | // rights reserved. 6 | // 7 | // Licensed under the MIT License (the "License"); you may not use this file 8 | // except in compliance with the License. You may obtain a copy of the License 9 | // at 10 | // 11 | // http://opensource.org/licenses/MIT 12 | // 13 | // Unless required by applicable law or agreed to in writing, software 14 | // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT 15 | // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the 16 | // License for the specific language governing permissions and limitations under 17 | // the License. 18 | 19 | #ifndef RAPIDJSON_FILEWRITESTREAM_H_ 20 | #define RAPIDJSON_FILEWRITESTREAM_H_ 21 | 22 | #include 23 | #include "stream.h" 24 | 25 | #ifdef __clang__ 26 | RAPIDJSON_DIAG_PUSH 27 | RAPIDJSON_DIAG_OFF(unreachable - code) 28 | #endif 29 | 30 | RAPIDJSON_NAMESPACE_BEGIN 31 | 32 | //! Wrapper of C file stream for output using fwrite(). 33 | /*! 34 | \note implements Stream concept 35 | */ 36 | class FileWriteStream { 37 | public: 38 | typedef char Ch; //!< Character type. Only support char. 39 | 40 | FileWriteStream(std::FILE *fp, char *buffer, size_t bufferSize) 41 | : fp_(fp), 42 | buffer_(buffer), 43 | bufferEnd_(buffer + bufferSize), 44 | current_(buffer_) { 45 | RAPIDJSON_ASSERT(fp_ != 0); 46 | } 47 | 48 | void Put(char c) { 49 | if (current_ >= bufferEnd_) Flush(); 50 | 51 | *current_++ = c; 52 | } 53 | 54 | void PutN(char c, size_t n) { 55 | size_t avail = static_cast(bufferEnd_ - current_); 56 | while (n > avail) { 57 | std::memset(current_, c, avail); 58 | current_ += avail; 59 | Flush(); 60 | n -= avail; 61 | avail = static_cast(bufferEnd_ - current_); 62 | } 63 | 64 | if (n > 0) { 65 | std::memset(current_, c, n); 66 | current_ += n; 67 | } 68 | } 69 | 70 | void Flush() { 71 | if (current_ != buffer_) { 72 | size_t result = 73 | std::fwrite(buffer_, 1, static_cast(current_ - buffer_), fp_); 74 | if (result < static_cast(current_ - buffer_)) { 75 | // failure deliberately ignored at this time 76 | // added to avoid warn_unused_result build errors 77 | } 78 | current_ = buffer_; 79 | } 80 | } 81 | 82 | // Not implemented 83 | char Peek() const { 84 | RAPIDJSON_ASSERT(false); 85 | return 0; 86 | } 87 | char Take() { 88 | RAPIDJSON_ASSERT(false); 89 | return 0; 90 | } 91 | size_t Tell() const { 92 | RAPIDJSON_ASSERT(false); 93 | return 0; 94 | } 95 | char *PutBegin() { 96 | RAPIDJSON_ASSERT(false); 97 | return 0; 98 | } 99 | size_t PutEnd(char *) { 100 | RAPIDJSON_ASSERT(false); 101 | return 0; 102 | } 103 | 104 | private: 105 | // Prohibit copy constructor & assignment operator. 106 | FileWriteStream(const FileWriteStream &); 107 | FileWriteStream &operator=(const FileWriteStream &); 108 | 109 | std::FILE *fp_; 110 | char *buffer_; 111 | char *bufferEnd_; 112 | char *current_; 113 | }; 114 | 115 | //! Implement specialized version of PutN() with memset() for better 116 | //! performance. 117 | template <> 118 | inline void PutN(FileWriteStream &stream, char c, size_t n) { 119 | stream.PutN(c, n); 120 | } 121 | 122 | RAPIDJSON_NAMESPACE_END 123 | 124 | #ifdef __clang__ 125 | RAPIDJSON_DIAG_POP 126 | #endif 127 | 128 | #endif // RAPIDJSON_FILESTREAM_H_ 129 | -------------------------------------------------------------------------------- /livox_ros_driver/common/rapidjson/fwd.h: -------------------------------------------------------------------------------- 1 | // Tencent is pleased to support the open source community by making RapidJSON 2 | // available. 3 | // 4 | // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All 5 | // rights reserved. 6 | // 7 | // Licensed under the MIT License (the "License"); you may not use this file 8 | // except in compliance with the License. You may obtain a copy of the License 9 | // at 10 | // 11 | // http://opensource.org/licenses/MIT 12 | // 13 | // Unless required by applicable law or agreed to in writing, software 14 | // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT 15 | // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the 16 | // License for the specific language governing permissions and limitations under 17 | // the License. 18 | 19 | #ifndef RAPIDJSON_FWD_H_ 20 | #define RAPIDJSON_FWD_H_ 21 | 22 | #include "rapidjson.h" 23 | 24 | RAPIDJSON_NAMESPACE_BEGIN 25 | 26 | // encodings.h 27 | 28 | template 29 | struct UTF8; 30 | template 31 | struct UTF16; 32 | template 33 | struct UTF16BE; 34 | template 35 | struct UTF16LE; 36 | template 37 | struct UTF32; 38 | template 39 | struct UTF32BE; 40 | template 41 | struct UTF32LE; 42 | template 43 | struct ASCII; 44 | template 45 | struct AutoUTF; 46 | 47 | template 48 | struct Transcoder; 49 | 50 | // allocators.h 51 | 52 | class CrtAllocator; 53 | 54 | template 55 | class MemoryPoolAllocator; 56 | 57 | // stream.h 58 | 59 | template 60 | struct GenericStringStream; 61 | 62 | typedef GenericStringStream> StringStream; 63 | 64 | template 65 | struct GenericInsituStringStream; 66 | 67 | typedef GenericInsituStringStream> InsituStringStream; 68 | 69 | // stringbuffer.h 70 | 71 | template 72 | class GenericStringBuffer; 73 | 74 | typedef GenericStringBuffer, CrtAllocator> StringBuffer; 75 | 76 | // filereadstream.h 77 | 78 | class FileReadStream; 79 | 80 | // filewritestream.h 81 | 82 | class FileWriteStream; 83 | 84 | // memorybuffer.h 85 | 86 | template 87 | struct GenericMemoryBuffer; 88 | 89 | typedef GenericMemoryBuffer MemoryBuffer; 90 | 91 | // memorystream.h 92 | 93 | struct MemoryStream; 94 | 95 | // reader.h 96 | 97 | template 98 | struct BaseReaderHandler; 99 | 100 | template 102 | class GenericReader; 103 | 104 | typedef GenericReader, UTF8, CrtAllocator> Reader; 105 | 106 | // writer.h 107 | 108 | template 110 | class Writer; 111 | 112 | // prettywriter.h 113 | 114 | template 116 | class PrettyWriter; 117 | 118 | // document.h 119 | 120 | template 121 | class GenericMember; 122 | 123 | template 124 | class GenericMemberIterator; 125 | 126 | template 127 | struct GenericStringRef; 128 | 129 | template 130 | class GenericValue; 131 | 132 | typedef GenericValue, MemoryPoolAllocator> Value; 133 | 134 | template 135 | class GenericDocument; 136 | 137 | typedef GenericDocument, MemoryPoolAllocator, 138 | CrtAllocator> 139 | Document; 140 | 141 | // pointer.h 142 | 143 | template 144 | class GenericPointer; 145 | 146 | typedef GenericPointer Pointer; 147 | 148 | // schema.h 149 | 150 | template 151 | class IGenericRemoteSchemaDocumentProvider; 152 | 153 | template 154 | class GenericSchemaDocument; 155 | 156 | typedef GenericSchemaDocument SchemaDocument; 157 | typedef IGenericRemoteSchemaDocumentProvider 158 | IRemoteSchemaDocumentProvider; 159 | 160 | template 162 | class GenericSchemaValidator; 163 | 164 | typedef GenericSchemaValidator< 165 | SchemaDocument, BaseReaderHandler, void>, CrtAllocator> 166 | SchemaValidator; 167 | 168 | RAPIDJSON_NAMESPACE_END 169 | 170 | #endif // RAPIDJSON_RAPIDJSONFWD_H_ 171 | -------------------------------------------------------------------------------- /livox_ros_driver/common/rapidjson/internal/clzll.h: -------------------------------------------------------------------------------- 1 | // Tencent is pleased to support the open source community by making RapidJSON 2 | // available. 3 | // 4 | // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All 5 | // rights reserved. 6 | // 7 | // Licensed under the MIT License (the "License"); you may not use this file 8 | // except in compliance with the License. You may obtain a copy of the License 9 | // at 10 | // 11 | // http://opensource.org/licenses/MIT 12 | // 13 | // Unless required by applicable law or agreed to in writing, software 14 | // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT 15 | // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the 16 | // License for the specific language governing permissions and limitations under 17 | // the License. 18 | 19 | #ifndef RAPIDJSON_CLZLL_H_ 20 | #define RAPIDJSON_CLZLL_H_ 21 | 22 | #include "../rapidjson.h" 23 | 24 | #if defined(_MSC_VER) 25 | #include 26 | #if defined(_WIN64) 27 | #pragma intrinsic(_BitScanReverse64) 28 | #else 29 | #pragma intrinsic(_BitScanReverse) 30 | #endif 31 | #endif 32 | 33 | RAPIDJSON_NAMESPACE_BEGIN 34 | namespace internal { 35 | 36 | #if (defined(__GNUC__) && __GNUC__ >= 4) || \ 37 | RAPIDJSON_HAS_BUILTIN(__builtin_clzll) 38 | #define RAPIDJSON_CLZLL __builtin_clzll 39 | #else 40 | 41 | inline uint32_t clzll(uint64_t x) { 42 | // Passing 0 to __builtin_clzll is UB in GCC and results in an 43 | // infinite loop in the software implementation. 44 | RAPIDJSON_ASSERT(x != 0); 45 | 46 | #if defined(_MSC_VER) 47 | unsigned long r = 0; 48 | #if defined(_WIN64) 49 | _BitScanReverse64(&r, x); 50 | #else 51 | // Scan the high 32 bits. 52 | if (_BitScanReverse(&r, static_cast(x >> 32))) return 63 - (r + 32); 53 | 54 | // Scan the low 32 bits. 55 | _BitScanReverse(&r, static_cast(x & 0xFFFFFFFF)); 56 | #endif // _WIN64 57 | 58 | return 63 - r; 59 | #else 60 | uint32_t r; 61 | while (!(x & (static_cast(1) << 63))) { 62 | x <<= 1; 63 | ++r; 64 | } 65 | 66 | return r; 67 | #endif // _MSC_VER 68 | } 69 | 70 | #define RAPIDJSON_CLZLL RAPIDJSON_NAMESPACE::internal::clzll 71 | #endif // (defined(__GNUC__) && __GNUC__ >= 4) || 72 | // RAPIDJSON_HAS_BUILTIN(__builtin_clzll) 73 | 74 | } // namespace internal 75 | RAPIDJSON_NAMESPACE_END 76 | 77 | #endif // RAPIDJSON_CLZLL_H_ -------------------------------------------------------------------------------- /livox_ros_driver/common/rapidjson/internal/ieee754.h: -------------------------------------------------------------------------------- 1 | // Tencent is pleased to support the open source community by making RapidJSON 2 | // available. 3 | // 4 | // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All 5 | // rights reserved. 6 | // 7 | // Licensed under the MIT License (the "License"); you may not use this file 8 | // except in compliance with the License. You may obtain a copy of the License 9 | // at 10 | // 11 | // http://opensource.org/licenses/MIT 12 | // 13 | // Unless required by applicable law or agreed to in writing, software 14 | // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT 15 | // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the 16 | // License for the specific language governing permissions and limitations under 17 | // the License. 18 | 19 | #ifndef RAPIDJSON_IEEE754_ 20 | #define RAPIDJSON_IEEE754_ 21 | 22 | #include "../rapidjson.h" 23 | 24 | RAPIDJSON_NAMESPACE_BEGIN 25 | namespace internal { 26 | 27 | class Double { 28 | public: 29 | Double() {} 30 | Double(double d) : d_(d) {} 31 | Double(uint64_t u) : u_(u) {} 32 | 33 | double Value() const { return d_; } 34 | uint64_t Uint64Value() const { return u_; } 35 | 36 | double NextPositiveDouble() const { 37 | RAPIDJSON_ASSERT(!Sign()); 38 | return Double(u_ + 1).Value(); 39 | } 40 | 41 | bool Sign() const { return (u_ & kSignMask) != 0; } 42 | uint64_t Significand() const { return u_ & kSignificandMask; } 43 | int Exponent() const { 44 | return static_cast(((u_ & kExponentMask) >> kSignificandSize) - 45 | kExponentBias); 46 | } 47 | 48 | bool IsNan() const { 49 | return (u_ & kExponentMask) == kExponentMask && Significand() != 0; 50 | } 51 | bool IsInf() const { 52 | return (u_ & kExponentMask) == kExponentMask && Significand() == 0; 53 | } 54 | bool IsNanOrInf() const { return (u_ & kExponentMask) == kExponentMask; } 55 | bool IsNormal() const { 56 | return (u_ & kExponentMask) != 0 || Significand() == 0; 57 | } 58 | bool IsZero() const { return (u_ & (kExponentMask | kSignificandMask)) == 0; } 59 | 60 | uint64_t IntegerSignificand() const { 61 | return IsNormal() ? Significand() | kHiddenBit : Significand(); 62 | } 63 | int IntegerExponent() const { 64 | return (IsNormal() ? Exponent() : kDenormalExponent) - kSignificandSize; 65 | } 66 | uint64_t ToBias() const { 67 | return (u_ & kSignMask) ? ~u_ + 1 : u_ | kSignMask; 68 | } 69 | 70 | static int EffectiveSignificandSize(int order) { 71 | if (order >= -1021) 72 | return 53; 73 | else if (order <= -1074) 74 | return 0; 75 | else 76 | return order + 1074; 77 | } 78 | 79 | private: 80 | static const int kSignificandSize = 52; 81 | static const int kExponentBias = 0x3FF; 82 | static const int kDenormalExponent = 1 - kExponentBias; 83 | static const uint64_t kSignMask = RAPIDJSON_UINT64_C2(0x80000000, 0x00000000); 84 | static const uint64_t kExponentMask = 85 | RAPIDJSON_UINT64_C2(0x7FF00000, 0x00000000); 86 | static const uint64_t kSignificandMask = 87 | RAPIDJSON_UINT64_C2(0x000FFFFF, 0xFFFFFFFF); 88 | static const uint64_t kHiddenBit = 89 | RAPIDJSON_UINT64_C2(0x00100000, 0x00000000); 90 | 91 | union { 92 | double d_; 93 | uint64_t u_; 94 | }; 95 | }; 96 | 97 | } // namespace internal 98 | RAPIDJSON_NAMESPACE_END 99 | 100 | #endif // RAPIDJSON_IEEE754_ 101 | -------------------------------------------------------------------------------- /livox_ros_driver/common/rapidjson/internal/pow10.h: -------------------------------------------------------------------------------- 1 | // Tencent is pleased to support the open source community by making RapidJSON 2 | // available. 3 | // 4 | // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All 5 | // rights reserved. 6 | // 7 | // Licensed under the MIT License (the "License"); you may not use this file 8 | // except in compliance with the License. You may obtain a copy of the License 9 | // at 10 | // 11 | // http://opensource.org/licenses/MIT 12 | // 13 | // Unless required by applicable law or agreed to in writing, software 14 | // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT 15 | // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the 16 | // License for the specific language governing permissions and limitations under 17 | // the License. 18 | 19 | #ifndef RAPIDJSON_POW10_ 20 | #define RAPIDJSON_POW10_ 21 | 22 | #include "../rapidjson.h" 23 | 24 | RAPIDJSON_NAMESPACE_BEGIN 25 | namespace internal { 26 | 27 | //! Computes integer powers of 10 in double (10.0^n). 28 | /*! This function uses lookup table for fast and accurate results. 29 | \param n non-negative exponent. Must <= 308. 30 | \return 10.0^n 31 | */ 32 | inline double Pow10(int n) { 33 | static const double e[] = { 34 | // 1e-0...1e308: 309 * 8 bytes = 2472 bytes 35 | 1e+0, 1e+1, 1e+2, 1e+3, 1e+4, 1e+5, 1e+6, 1e+7, 1e+8, 36 | 1e+9, 1e+10, 1e+11, 1e+12, 1e+13, 1e+14, 1e+15, 1e+16, 1e+17, 37 | 1e+18, 1e+19, 1e+20, 1e+21, 1e+22, 1e+23, 1e+24, 1e+25, 1e+26, 38 | 1e+27, 1e+28, 1e+29, 1e+30, 1e+31, 1e+32, 1e+33, 1e+34, 1e+35, 39 | 1e+36, 1e+37, 1e+38, 1e+39, 1e+40, 1e+41, 1e+42, 1e+43, 1e+44, 40 | 1e+45, 1e+46, 1e+47, 1e+48, 1e+49, 1e+50, 1e+51, 1e+52, 1e+53, 41 | 1e+54, 1e+55, 1e+56, 1e+57, 1e+58, 1e+59, 1e+60, 1e+61, 1e+62, 42 | 1e+63, 1e+64, 1e+65, 1e+66, 1e+67, 1e+68, 1e+69, 1e+70, 1e+71, 43 | 1e+72, 1e+73, 1e+74, 1e+75, 1e+76, 1e+77, 1e+78, 1e+79, 1e+80, 44 | 1e+81, 1e+82, 1e+83, 1e+84, 1e+85, 1e+86, 1e+87, 1e+88, 1e+89, 45 | 1e+90, 1e+91, 1e+92, 1e+93, 1e+94, 1e+95, 1e+96, 1e+97, 1e+98, 46 | 1e+99, 1e+100, 1e+101, 1e+102, 1e+103, 1e+104, 1e+105, 1e+106, 1e+107, 47 | 1e+108, 1e+109, 1e+110, 1e+111, 1e+112, 1e+113, 1e+114, 1e+115, 1e+116, 48 | 1e+117, 1e+118, 1e+119, 1e+120, 1e+121, 1e+122, 1e+123, 1e+124, 1e+125, 49 | 1e+126, 1e+127, 1e+128, 1e+129, 1e+130, 1e+131, 1e+132, 1e+133, 1e+134, 50 | 1e+135, 1e+136, 1e+137, 1e+138, 1e+139, 1e+140, 1e+141, 1e+142, 1e+143, 51 | 1e+144, 1e+145, 1e+146, 1e+147, 1e+148, 1e+149, 1e+150, 1e+151, 1e+152, 52 | 1e+153, 1e+154, 1e+155, 1e+156, 1e+157, 1e+158, 1e+159, 1e+160, 1e+161, 53 | 1e+162, 1e+163, 1e+164, 1e+165, 1e+166, 1e+167, 1e+168, 1e+169, 1e+170, 54 | 1e+171, 1e+172, 1e+173, 1e+174, 1e+175, 1e+176, 1e+177, 1e+178, 1e+179, 55 | 1e+180, 1e+181, 1e+182, 1e+183, 1e+184, 1e+185, 1e+186, 1e+187, 1e+188, 56 | 1e+189, 1e+190, 1e+191, 1e+192, 1e+193, 1e+194, 1e+195, 1e+196, 1e+197, 57 | 1e+198, 1e+199, 1e+200, 1e+201, 1e+202, 1e+203, 1e+204, 1e+205, 1e+206, 58 | 1e+207, 1e+208, 1e+209, 1e+210, 1e+211, 1e+212, 1e+213, 1e+214, 1e+215, 59 | 1e+216, 1e+217, 1e+218, 1e+219, 1e+220, 1e+221, 1e+222, 1e+223, 1e+224, 60 | 1e+225, 1e+226, 1e+227, 1e+228, 1e+229, 1e+230, 1e+231, 1e+232, 1e+233, 61 | 1e+234, 1e+235, 1e+236, 1e+237, 1e+238, 1e+239, 1e+240, 1e+241, 1e+242, 62 | 1e+243, 1e+244, 1e+245, 1e+246, 1e+247, 1e+248, 1e+249, 1e+250, 1e+251, 63 | 1e+252, 1e+253, 1e+254, 1e+255, 1e+256, 1e+257, 1e+258, 1e+259, 1e+260, 64 | 1e+261, 1e+262, 1e+263, 1e+264, 1e+265, 1e+266, 1e+267, 1e+268, 1e+269, 65 | 1e+270, 1e+271, 1e+272, 1e+273, 1e+274, 1e+275, 1e+276, 1e+277, 1e+278, 66 | 1e+279, 1e+280, 1e+281, 1e+282, 1e+283, 1e+284, 1e+285, 1e+286, 1e+287, 67 | 1e+288, 1e+289, 1e+290, 1e+291, 1e+292, 1e+293, 1e+294, 1e+295, 1e+296, 68 | 1e+297, 1e+298, 1e+299, 1e+300, 1e+301, 1e+302, 1e+303, 1e+304, 1e+305, 69 | 1e+306, 1e+307, 1e+308}; 70 | RAPIDJSON_ASSERT(n >= 0 && n <= 308); 71 | return e[n]; 72 | } 73 | 74 | } // namespace internal 75 | RAPIDJSON_NAMESPACE_END 76 | 77 | #endif // RAPIDJSON_POW10_ 78 | -------------------------------------------------------------------------------- /livox_ros_driver/common/rapidjson/internal/strfunc.h: -------------------------------------------------------------------------------- 1 | // Tencent is pleased to support the open source community by making RapidJSON 2 | // available. 3 | // 4 | // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All 5 | // rights reserved. 6 | // 7 | // Licensed under the MIT License (the "License"); you may not use this file 8 | // except in compliance with the License. You may obtain a copy of the License 9 | // at 10 | // 11 | // http://opensource.org/licenses/MIT 12 | // 13 | // Unless required by applicable law or agreed to in writing, software 14 | // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT 15 | // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the 16 | // License for the specific language governing permissions and limitations under 17 | // the License. 18 | 19 | #ifndef RAPIDJSON_INTERNAL_STRFUNC_H_ 20 | #define RAPIDJSON_INTERNAL_STRFUNC_H_ 21 | 22 | #include 23 | #include "../stream.h" 24 | 25 | RAPIDJSON_NAMESPACE_BEGIN 26 | namespace internal { 27 | 28 | //! Custom strlen() which works on different character types. 29 | /*! \tparam Ch Character type (e.g. char, wchar_t, short) 30 | \param s Null-terminated input string. 31 | \return Number of characters in the string. 32 | \note This has the same semantics as strlen(), the return value is not 33 | number of Unicode codepoints. 34 | */ 35 | template 36 | inline SizeType StrLen(const Ch *s) { 37 | RAPIDJSON_ASSERT(s != 0); 38 | const Ch *p = s; 39 | while (*p) ++p; 40 | return SizeType(p - s); 41 | } 42 | 43 | template <> 44 | inline SizeType StrLen(const char *s) { 45 | return SizeType(std::strlen(s)); 46 | } 47 | 48 | template <> 49 | inline SizeType StrLen(const wchar_t *s) { 50 | return SizeType(std::wcslen(s)); 51 | } 52 | 53 | //! Returns number of code points in a encoded string. 54 | template 55 | bool CountStringCodePoint(const typename Encoding::Ch *s, SizeType length, 56 | SizeType *outCount) { 57 | RAPIDJSON_ASSERT(s != 0); 58 | RAPIDJSON_ASSERT(outCount != 0); 59 | GenericStringStream is(s); 60 | const typename Encoding::Ch *end = s + length; 61 | SizeType count = 0; 62 | while (is.src_ < end) { 63 | unsigned codepoint; 64 | if (!Encoding::Decode(is, &codepoint)) return false; 65 | count++; 66 | } 67 | *outCount = count; 68 | return true; 69 | } 70 | 71 | } // namespace internal 72 | RAPIDJSON_NAMESPACE_END 73 | 74 | #endif // RAPIDJSON_INTERNAL_STRFUNC_H_ 75 | -------------------------------------------------------------------------------- /livox_ros_driver/common/rapidjson/internal/swap.h: -------------------------------------------------------------------------------- 1 | // Tencent is pleased to support the open source community by making RapidJSON 2 | // available. 3 | // 4 | // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All 5 | // rights reserved. 6 | // 7 | // Licensed under the MIT License (the "License"); you may not use this file 8 | // except in compliance with the License. You may obtain a copy of the License 9 | // at 10 | // 11 | // http://opensource.org/licenses/MIT 12 | // 13 | // Unless required by applicable law or agreed to in writing, software 14 | // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT 15 | // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the 16 | // License for the specific language governing permissions and limitations under 17 | // the License. 18 | 19 | #ifndef RAPIDJSON_INTERNAL_SWAP_H_ 20 | #define RAPIDJSON_INTERNAL_SWAP_H_ 21 | 22 | #include "../rapidjson.h" 23 | 24 | #if defined(__clang__) 25 | RAPIDJSON_DIAG_PUSH 26 | RAPIDJSON_DIAG_OFF(c++ 98 - compat) 27 | #endif 28 | 29 | RAPIDJSON_NAMESPACE_BEGIN 30 | namespace internal { 31 | 32 | //! Custom swap() to avoid dependency on C++ header 33 | /*! \tparam T Type of the arguments to swap, should be instantiated with 34 | primitive C++ types only. \note This has the same semantics as std::swap(). 35 | */ 36 | template 37 | inline void Swap(T &a, T &b) RAPIDJSON_NOEXCEPT { 38 | T tmp = a; 39 | a = b; 40 | b = tmp; 41 | } 42 | 43 | } // namespace internal 44 | RAPIDJSON_NAMESPACE_END 45 | 46 | #if defined(__clang__) 47 | RAPIDJSON_DIAG_POP 48 | #endif 49 | 50 | #endif // RAPIDJSON_INTERNAL_SWAP_H_ 51 | -------------------------------------------------------------------------------- /livox_ros_driver/common/rapidjson/istreamwrapper.h: -------------------------------------------------------------------------------- 1 | // Tencent is pleased to support the open source community by making RapidJSON 2 | // available. 3 | // 4 | // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All 5 | // rights reserved. 6 | // 7 | // Licensed under the MIT License (the "License"); you may not use this file 8 | // except in compliance with the License. You may obtain a copy of the License 9 | // at 10 | // 11 | // http://opensource.org/licenses/MIT 12 | // 13 | // Unless required by applicable law or agreed to in writing, software 14 | // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT 15 | // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the 16 | // License for the specific language governing permissions and limitations under 17 | // the License. 18 | 19 | #ifndef RAPIDJSON_ISTREAMWRAPPER_H_ 20 | #define RAPIDJSON_ISTREAMWRAPPER_H_ 21 | 22 | #include 23 | #include 24 | #include "stream.h" 25 | 26 | #ifdef __clang__ 27 | RAPIDJSON_DIAG_PUSH 28 | RAPIDJSON_DIAG_OFF(padded) 29 | #elif defined(_MSC_VER) 30 | RAPIDJSON_DIAG_PUSH 31 | RAPIDJSON_DIAG_OFF(4351) // new behavior: elements of array 'array' will be 32 | // default initialized 33 | #endif 34 | 35 | RAPIDJSON_NAMESPACE_BEGIN 36 | 37 | //! Wrapper of \c std::basic_istream into RapidJSON's Stream concept. 38 | /*! 39 | The classes can be wrapped including but not limited to: 40 | 41 | - \c std::istringstream 42 | - \c std::stringstream 43 | - \c std::wistringstream 44 | - \c std::wstringstream 45 | - \c std::ifstream 46 | - \c std::fstream 47 | - \c std::wifstream 48 | - \c std::wfstream 49 | 50 | \tparam StreamType Class derived from \c std::basic_istream. 51 | */ 52 | 53 | template 54 | class BasicIStreamWrapper { 55 | public: 56 | typedef typename StreamType::char_type Ch; 57 | 58 | //! Constructor. 59 | /*! 60 | \param stream stream opened for read. 61 | */ 62 | BasicIStreamWrapper(StreamType &stream) 63 | : stream_(stream), 64 | buffer_(peekBuffer_), 65 | bufferSize_(4), 66 | bufferLast_(0), 67 | current_(buffer_), 68 | readCount_(0), 69 | count_(0), 70 | eof_(false) { 71 | Read(); 72 | } 73 | 74 | //! Constructor. 75 | /*! 76 | \param stream stream opened for read. 77 | \param buffer user-supplied buffer. 78 | \param bufferSize size of buffer in bytes. Must >=4 bytes. 79 | */ 80 | BasicIStreamWrapper(StreamType &stream, char *buffer, size_t bufferSize) 81 | : stream_(stream), 82 | buffer_(buffer), 83 | bufferSize_(bufferSize), 84 | bufferLast_(0), 85 | current_(buffer_), 86 | readCount_(0), 87 | count_(0), 88 | eof_(false) { 89 | RAPIDJSON_ASSERT(bufferSize >= 4); 90 | Read(); 91 | } 92 | 93 | Ch Peek() const { return *current_; } 94 | Ch Take() { 95 | Ch c = *current_; 96 | Read(); 97 | return c; 98 | } 99 | size_t Tell() const { 100 | return count_ + static_cast(current_ - buffer_); 101 | } 102 | 103 | // Not implemented 104 | void Put(Ch) { RAPIDJSON_ASSERT(false); } 105 | void Flush() { RAPIDJSON_ASSERT(false); } 106 | Ch *PutBegin() { 107 | RAPIDJSON_ASSERT(false); 108 | return 0; 109 | } 110 | size_t PutEnd(Ch *) { 111 | RAPIDJSON_ASSERT(false); 112 | return 0; 113 | } 114 | 115 | // For encoding detection only. 116 | const Ch *Peek4() const { 117 | return (current_ + 4 - !eof_ <= bufferLast_) ? current_ : 0; 118 | } 119 | 120 | private: 121 | BasicIStreamWrapper(); 122 | BasicIStreamWrapper(const BasicIStreamWrapper &); 123 | BasicIStreamWrapper &operator=(const BasicIStreamWrapper &); 124 | 125 | void Read() { 126 | if (current_ < bufferLast_) 127 | ++current_; 128 | else if (!eof_) { 129 | count_ += readCount_; 130 | readCount_ = bufferSize_; 131 | bufferLast_ = buffer_ + readCount_ - 1; 132 | current_ = buffer_; 133 | 134 | if (!stream_.read(buffer_, static_cast(bufferSize_))) { 135 | readCount_ = static_cast(stream_.gcount()); 136 | *(bufferLast_ = buffer_ + readCount_) = '\0'; 137 | eof_ = true; 138 | } 139 | } 140 | } 141 | 142 | StreamType &stream_; 143 | Ch peekBuffer_[4], *buffer_; 144 | size_t bufferSize_; 145 | Ch *bufferLast_; 146 | Ch *current_; 147 | size_t readCount_; 148 | size_t count_; //!< Number of characters read 149 | bool eof_; 150 | }; 151 | 152 | typedef BasicIStreamWrapper IStreamWrapper; 153 | typedef BasicIStreamWrapper WIStreamWrapper; 154 | 155 | #if defined(__clang__) || defined(_MSC_VER) 156 | RAPIDJSON_DIAG_POP 157 | #endif 158 | 159 | RAPIDJSON_NAMESPACE_END 160 | 161 | #endif // RAPIDJSON_ISTREAMWRAPPER_H_ 162 | -------------------------------------------------------------------------------- /livox_ros_driver/common/rapidjson/memorybuffer.h: -------------------------------------------------------------------------------- 1 | // Tencent is pleased to support the open source community by making RapidJSON 2 | // available. 3 | // 4 | // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All 5 | // rights reserved. 6 | // 7 | // Licensed under the MIT License (the "License"); you may not use this file 8 | // except in compliance with the License. You may obtain a copy of the License 9 | // at 10 | // 11 | // http://opensource.org/licenses/MIT 12 | // 13 | // Unless required by applicable law or agreed to in writing, software 14 | // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT 15 | // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the 16 | // License for the specific language governing permissions and limitations under 17 | // the License. 18 | 19 | #ifndef RAPIDJSON_MEMORYBUFFER_H_ 20 | #define RAPIDJSON_MEMORYBUFFER_H_ 21 | 22 | #include "internal/stack.h" 23 | #include "stream.h" 24 | 25 | RAPIDJSON_NAMESPACE_BEGIN 26 | 27 | //! Represents an in-memory output byte stream. 28 | /*! 29 | This class is mainly for being wrapped by EncodedOutputStream or 30 | AutoUTFOutputStream. 31 | 32 | It is similar to FileWriteBuffer but the destination is an in-memory buffer 33 | instead of a file. 34 | 35 | Differences between MemoryBuffer and StringBuffer: 36 | 1. StringBuffer has Encoding but MemoryBuffer is only a byte buffer. 37 | 2. StringBuffer::GetString() returns a null-terminated string. 38 | MemoryBuffer::GetBuffer() returns a buffer without terminator. 39 | 40 | \tparam Allocator type for allocating memory buffer. 41 | \note implements Stream concept 42 | */ 43 | template 44 | struct GenericMemoryBuffer { 45 | typedef char Ch; // byte 46 | 47 | GenericMemoryBuffer(Allocator *allocator = 0, 48 | size_t capacity = kDefaultCapacity) 49 | : stack_(allocator, capacity) {} 50 | 51 | void Put(Ch c) { *stack_.template Push() = c; } 52 | void Flush() {} 53 | 54 | void Clear() { stack_.Clear(); } 55 | void ShrinkToFit() { stack_.ShrinkToFit(); } 56 | Ch *Push(size_t count) { return stack_.template Push(count); } 57 | void Pop(size_t count) { stack_.template Pop(count); } 58 | 59 | const Ch *GetBuffer() const { return stack_.template Bottom(); } 60 | 61 | size_t GetSize() const { return stack_.GetSize(); } 62 | 63 | static const size_t kDefaultCapacity = 256; 64 | mutable internal::Stack stack_; 65 | }; 66 | 67 | typedef GenericMemoryBuffer<> MemoryBuffer; 68 | 69 | //! Implement specialized version of PutN() with memset() for better 70 | //! performance. 71 | template <> 72 | inline void PutN(MemoryBuffer &memoryBuffer, char c, size_t n) { 73 | std::memset(memoryBuffer.stack_.Push(n), c, n * sizeof(c)); 74 | } 75 | 76 | RAPIDJSON_NAMESPACE_END 77 | 78 | #endif // RAPIDJSON_MEMORYBUFFER_H_ 79 | -------------------------------------------------------------------------------- /livox_ros_driver/common/rapidjson/memorystream.h: -------------------------------------------------------------------------------- 1 | // Tencent is pleased to support the open source community by making RapidJSON 2 | // available. 3 | // 4 | // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All 5 | // rights reserved. 6 | // 7 | // Licensed under the MIT License (the "License"); you may not use this file 8 | // except in compliance with the License. You may obtain a copy of the License 9 | // at 10 | // 11 | // http://opensource.org/licenses/MIT 12 | // 13 | // Unless required by applicable law or agreed to in writing, software 14 | // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT 15 | // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the 16 | // License for the specific language governing permissions and limitations under 17 | // the License. 18 | 19 | #ifndef RAPIDJSON_MEMORYSTREAM_H_ 20 | #define RAPIDJSON_MEMORYSTREAM_H_ 21 | 22 | #include "stream.h" 23 | 24 | #ifdef __clang__ 25 | RAPIDJSON_DIAG_PUSH 26 | RAPIDJSON_DIAG_OFF(unreachable - code) 27 | RAPIDJSON_DIAG_OFF(missing - noreturn) 28 | #endif 29 | 30 | RAPIDJSON_NAMESPACE_BEGIN 31 | 32 | //! Represents an in-memory input byte stream. 33 | /*! 34 | This class is mainly for being wrapped by EncodedInputStream or 35 | AutoUTFInputStream. 36 | 37 | It is similar to FileReadBuffer but the source is an in-memory buffer 38 | instead of a file. 39 | 40 | Differences between MemoryStream and StringStream: 41 | 1. StringStream has encoding but MemoryStream is a byte stream. 42 | 2. MemoryStream needs size of the source buffer and the buffer don't need to 43 | be null terminated. StringStream assume null-terminated string as source. 44 | 3. MemoryStream supports Peek4() for encoding detection. StringStream is 45 | specified with an encoding so it should not have Peek4(). \note implements 46 | Stream concept 47 | */ 48 | struct MemoryStream { 49 | typedef char Ch; // byte 50 | 51 | MemoryStream(const Ch *src, size_t size) 52 | : src_(src), begin_(src), end_(src + size), size_(size) {} 53 | 54 | Ch Peek() const { return RAPIDJSON_UNLIKELY(src_ == end_) ? '\0' : *src_; } 55 | Ch Take() { return RAPIDJSON_UNLIKELY(src_ == end_) ? '\0' : *src_++; } 56 | size_t Tell() const { return static_cast(src_ - begin_); } 57 | 58 | Ch *PutBegin() { 59 | RAPIDJSON_ASSERT(false); 60 | return 0; 61 | } 62 | void Put(Ch) { RAPIDJSON_ASSERT(false); } 63 | void Flush() { RAPIDJSON_ASSERT(false); } 64 | size_t PutEnd(Ch *) { 65 | RAPIDJSON_ASSERT(false); 66 | return 0; 67 | } 68 | 69 | // For encoding detection only. 70 | const Ch *Peek4() const { return Tell() + 4 <= size_ ? src_ : 0; } 71 | 72 | const Ch *src_; //!< Current read position. 73 | const Ch *begin_; //!< Original head of the string. 74 | const Ch *end_; //!< End of stream. 75 | size_t size_; //!< Size of the stream. 76 | }; 77 | 78 | RAPIDJSON_NAMESPACE_END 79 | 80 | #ifdef __clang__ 81 | RAPIDJSON_DIAG_POP 82 | #endif 83 | 84 | #endif // RAPIDJSON_MEMORYBUFFER_H_ 85 | -------------------------------------------------------------------------------- /livox_ros_driver/common/rapidjson/ostreamwrapper.h: -------------------------------------------------------------------------------- 1 | // Tencent is pleased to support the open source community by making RapidJSON 2 | // available. 3 | // 4 | // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All 5 | // rights reserved. 6 | // 7 | // Licensed under the MIT License (the "License"); you may not use this file 8 | // except in compliance with the License. You may obtain a copy of the License 9 | // at 10 | // 11 | // http://opensource.org/licenses/MIT 12 | // 13 | // Unless required by applicable law or agreed to in writing, software 14 | // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT 15 | // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the 16 | // License for the specific language governing permissions and limitations under 17 | // the License. 18 | 19 | #ifndef RAPIDJSON_OSTREAMWRAPPER_H_ 20 | #define RAPIDJSON_OSTREAMWRAPPER_H_ 21 | 22 | #include 23 | #include "stream.h" 24 | 25 | #ifdef __clang__ 26 | RAPIDJSON_DIAG_PUSH 27 | RAPIDJSON_DIAG_OFF(padded) 28 | #endif 29 | 30 | RAPIDJSON_NAMESPACE_BEGIN 31 | 32 | //! Wrapper of \c std::basic_ostream into RapidJSON's Stream concept. 33 | /*! 34 | The classes can be wrapped including but not limited to: 35 | 36 | - \c std::ostringstream 37 | - \c std::stringstream 38 | - \c std::wpstringstream 39 | - \c std::wstringstream 40 | - \c std::ifstream 41 | - \c std::fstream 42 | - \c std::wofstream 43 | - \c std::wfstream 44 | 45 | \tparam StreamType Class derived from \c std::basic_ostream. 46 | */ 47 | 48 | template 49 | class BasicOStreamWrapper { 50 | public: 51 | typedef typename StreamType::char_type Ch; 52 | BasicOStreamWrapper(StreamType &stream) : stream_(stream) {} 53 | 54 | void Put(Ch c) { stream_.put(c); } 55 | 56 | void Flush() { stream_.flush(); } 57 | 58 | // Not implemented 59 | char Peek() const { 60 | RAPIDJSON_ASSERT(false); 61 | return 0; 62 | } 63 | char Take() { 64 | RAPIDJSON_ASSERT(false); 65 | return 0; 66 | } 67 | size_t Tell() const { 68 | RAPIDJSON_ASSERT(false); 69 | return 0; 70 | } 71 | char *PutBegin() { 72 | RAPIDJSON_ASSERT(false); 73 | return 0; 74 | } 75 | size_t PutEnd(char *) { 76 | RAPIDJSON_ASSERT(false); 77 | return 0; 78 | } 79 | 80 | private: 81 | BasicOStreamWrapper(const BasicOStreamWrapper &); 82 | BasicOStreamWrapper &operator=(const BasicOStreamWrapper &); 83 | 84 | StreamType &stream_; 85 | }; 86 | 87 | typedef BasicOStreamWrapper OStreamWrapper; 88 | typedef BasicOStreamWrapper WOStreamWrapper; 89 | 90 | #ifdef __clang__ 91 | RAPIDJSON_DIAG_POP 92 | #endif 93 | 94 | RAPIDJSON_NAMESPACE_END 95 | 96 | #endif // RAPIDJSON_OSTREAMWRAPPER_H_ 97 | -------------------------------------------------------------------------------- /livox_ros_driver/common/rapidjson/stream.h: -------------------------------------------------------------------------------- 1 | // Tencent is pleased to support the open source community by making RapidJSON 2 | // available. 3 | // 4 | // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All 5 | // rights reserved. 6 | // 7 | // Licensed under the MIT License (the "License"); you may not use this file 8 | // except in compliance with the License. You may obtain a copy of the License 9 | // at 10 | // 11 | // http://opensource.org/licenses/MIT 12 | // 13 | // Unless required by applicable law or agreed to in writing, software 14 | // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT 15 | // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the 16 | // License for the specific language governing permissions and limitations under 17 | // the License. 18 | 19 | #include "rapidjson.h" 20 | 21 | #ifndef RAPIDJSON_STREAM_H_ 22 | #define RAPIDJSON_STREAM_H_ 23 | 24 | #include "encodings.h" 25 | 26 | RAPIDJSON_NAMESPACE_BEGIN 27 | 28 | /////////////////////////////////////////////////////////////////////////////// 29 | // Stream 30 | 31 | /*! \class rapidjson::Stream 32 | \brief Concept for reading and writing characters. 33 | 34 | For read-only stream, no need to implement PutBegin(), Put(), Flush() and 35 | PutEnd(). 36 | 37 | For write-only stream, only need to implement Put() and Flush(). 38 | 39 | \code 40 | concept Stream { 41 | typename Ch; //!< Character type of the stream. 42 | 43 | //! Read the current character from stream without moving the read cursor. 44 | Ch Peek() const; 45 | 46 | //! Read the current character from stream and moving the read cursor to 47 | next character. Ch Take(); 48 | 49 | //! Get the current read cursor. 50 | //! \return Number of characters read from start. 51 | size_t Tell(); 52 | 53 | //! Begin writing operation at the current read pointer. 54 | //! \return The begin writer pointer. 55 | Ch* PutBegin(); 56 | 57 | //! Write a character. 58 | void Put(Ch c); 59 | 60 | //! Flush the buffer. 61 | void Flush(); 62 | 63 | //! End the writing operation. 64 | //! \param begin The begin write pointer returned by PutBegin(). 65 | //! \return Number of characters written. 66 | size_t PutEnd(Ch* begin); 67 | } 68 | \endcode 69 | */ 70 | 71 | //! Provides additional information for stream. 72 | /*! 73 | By using traits pattern, this type provides a default configuration for 74 | stream. For custom stream, this type can be specialized for other 75 | configuration. See TEST(Reader, CustomStringStream) in readertest.cpp for 76 | example. 77 | */ 78 | template 79 | struct StreamTraits { 80 | //! Whether to make local copy of stream for optimization during parsing. 81 | /*! 82 | By default, for safety, streams do not use local copy optimization. 83 | Stream that can be copied fast should specialize this, like 84 | StreamTraits. 85 | */ 86 | enum { copyOptimization = 0 }; 87 | }; 88 | 89 | //! Reserve n characters for writing to a stream. 90 | template 91 | inline void PutReserve(Stream &stream, size_t count) { 92 | (void)stream; 93 | (void)count; 94 | } 95 | 96 | //! Write character to a stream, presuming buffer is reserved. 97 | template 98 | inline void PutUnsafe(Stream &stream, typename Stream::Ch c) { 99 | stream.Put(c); 100 | } 101 | 102 | //! Put N copies of a character to a stream. 103 | template 104 | inline void PutN(Stream &stream, Ch c, size_t n) { 105 | PutReserve(stream, n); 106 | for (size_t i = 0; i < n; i++) PutUnsafe(stream, c); 107 | } 108 | 109 | /////////////////////////////////////////////////////////////////////////////// 110 | // GenericStreamWrapper 111 | 112 | //! A Stream Wrapper 113 | /*! \tThis string stream is a wrapper for any stream by just forwarding any 114 | \treceived message to the origin stream. 115 | \note implements Stream concept 116 | */ 117 | 118 | #if defined(_MSC_VER) && _MSC_VER <= 1800 119 | RAPIDJSON_DIAG_PUSH 120 | RAPIDJSON_DIAG_OFF(4702) // unreachable code 121 | RAPIDJSON_DIAG_OFF(4512) // assignment operator could not be generated 122 | #endif 123 | 124 | template > 125 | class GenericStreamWrapper { 126 | public: 127 | typedef typename Encoding::Ch Ch; 128 | GenericStreamWrapper(InputStream &is) : is_(is) {} 129 | 130 | Ch Peek() const { return is_.Peek(); } 131 | Ch Take() { return is_.Take(); } 132 | size_t Tell() { return is_.Tell(); } 133 | Ch *PutBegin() { return is_.PutBegin(); } 134 | void Put(Ch ch) { is_.Put(ch); } 135 | void Flush() { is_.Flush(); } 136 | size_t PutEnd(Ch *ch) { return is_.PutEnd(ch); } 137 | 138 | // wrapper for MemoryStream 139 | const Ch *Peek4() const { return is_.Peek4(); } 140 | 141 | // wrapper for AutoUTFInputStream 142 | UTFType GetType() const { return is_.GetType(); } 143 | bool HasBOM() const { return is_.HasBOM(); } 144 | 145 | protected: 146 | InputStream &is_; 147 | }; 148 | 149 | #if defined(_MSC_VER) && _MSC_VER <= 1800 150 | RAPIDJSON_DIAG_POP 151 | #endif 152 | 153 | /////////////////////////////////////////////////////////////////////////////// 154 | // StringStream 155 | 156 | //! Read-only string stream. 157 | /*! \note implements Stream concept 158 | */ 159 | template 160 | struct GenericStringStream { 161 | typedef typename Encoding::Ch Ch; 162 | 163 | GenericStringStream(const Ch *src) : src_(src), head_(src) {} 164 | 165 | Ch Peek() const { return *src_; } 166 | Ch Take() { return *src_++; } 167 | size_t Tell() const { return static_cast(src_ - head_); } 168 | 169 | Ch *PutBegin() { 170 | RAPIDJSON_ASSERT(false); 171 | return 0; 172 | } 173 | void Put(Ch) { RAPIDJSON_ASSERT(false); } 174 | void Flush() { RAPIDJSON_ASSERT(false); } 175 | size_t PutEnd(Ch *) { 176 | RAPIDJSON_ASSERT(false); 177 | return 0; 178 | } 179 | 180 | const Ch *src_; //!< Current read position. 181 | const Ch *head_; //!< Original head of the string. 182 | }; 183 | 184 | template 185 | struct StreamTraits> { 186 | enum { copyOptimization = 1 }; 187 | }; 188 | 189 | //! String stream with UTF8 encoding. 190 | typedef GenericStringStream> StringStream; 191 | 192 | /////////////////////////////////////////////////////////////////////////////// 193 | // InsituStringStream 194 | 195 | //! A read-write string stream. 196 | /*! This string stream is particularly designed for in-situ parsing. 197 | \note implements Stream concept 198 | */ 199 | template 200 | struct GenericInsituStringStream { 201 | typedef typename Encoding::Ch Ch; 202 | 203 | GenericInsituStringStream(Ch *src) : src_(src), dst_(0), head_(src) {} 204 | 205 | // Read 206 | Ch Peek() { return *src_; } 207 | Ch Take() { return *src_++; } 208 | size_t Tell() { return static_cast(src_ - head_); } 209 | 210 | // Write 211 | void Put(Ch c) { 212 | RAPIDJSON_ASSERT(dst_ != 0); 213 | *dst_++ = c; 214 | } 215 | 216 | Ch *PutBegin() { return dst_ = src_; } 217 | size_t PutEnd(Ch *begin) { return static_cast(dst_ - begin); } 218 | void Flush() {} 219 | 220 | Ch *Push(size_t count) { 221 | Ch *begin = dst_; 222 | dst_ += count; 223 | return begin; 224 | } 225 | void Pop(size_t count) { dst_ -= count; } 226 | 227 | Ch *src_; 228 | Ch *dst_; 229 | Ch *head_; 230 | }; 231 | 232 | template 233 | struct StreamTraits> { 234 | enum { copyOptimization = 1 }; 235 | }; 236 | 237 | //! Insitu string stream with UTF8 encoding. 238 | typedef GenericInsituStringStream> InsituStringStream; 239 | 240 | RAPIDJSON_NAMESPACE_END 241 | 242 | #endif // RAPIDJSON_STREAM_H_ 243 | -------------------------------------------------------------------------------- /livox_ros_driver/common/rapidjson/stringbuffer.h: -------------------------------------------------------------------------------- 1 | // Tencent is pleased to support the open source community by making RapidJSON 2 | // available. 3 | // 4 | // Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All 5 | // rights reserved. 6 | // 7 | // Licensed under the MIT License (the "License"); you may not use this file 8 | // except in compliance with the License. You may obtain a copy of the License 9 | // at 10 | // 11 | // http://opensource.org/licenses/MIT 12 | // 13 | // Unless required by applicable law or agreed to in writing, software 14 | // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT 15 | // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the 16 | // License for the specific language governing permissions and limitations under 17 | // the License. 18 | 19 | #ifndef RAPIDJSON_STRINGBUFFER_H_ 20 | #define RAPIDJSON_STRINGBUFFER_H_ 21 | 22 | #include "internal/stack.h" 23 | #include "stream.h" 24 | 25 | #if RAPIDJSON_HAS_CXX11_RVALUE_REFS 26 | #include // std::move 27 | #endif 28 | 29 | #include "internal/stack.h" 30 | 31 | #if defined(__clang__) 32 | RAPIDJSON_DIAG_PUSH 33 | RAPIDJSON_DIAG_OFF(c++ 98 - compat) 34 | #endif 35 | 36 | RAPIDJSON_NAMESPACE_BEGIN 37 | 38 | //! Represents an in-memory output stream. 39 | /*! 40 | \tparam Encoding Encoding of the stream. 41 | \tparam Allocator type for allocating memory buffer. 42 | \note implements Stream concept 43 | */ 44 | template 45 | class GenericStringBuffer { 46 | public: 47 | typedef typename Encoding::Ch Ch; 48 | 49 | GenericStringBuffer(Allocator *allocator = 0, 50 | size_t capacity = kDefaultCapacity) 51 | : stack_(allocator, capacity) {} 52 | 53 | #if RAPIDJSON_HAS_CXX11_RVALUE_REFS 54 | GenericStringBuffer(GenericStringBuffer &&rhs) 55 | : stack_(std::move(rhs.stack_)) {} 56 | GenericStringBuffer &operator=(GenericStringBuffer &&rhs) { 57 | if (&rhs != this) stack_ = std::move(rhs.stack_); 58 | return *this; 59 | } 60 | #endif 61 | 62 | void Put(Ch c) { *stack_.template Push() = c; } 63 | void PutUnsafe(Ch c) { *stack_.template PushUnsafe() = c; } 64 | void Flush() {} 65 | 66 | void Clear() { stack_.Clear(); } 67 | void ShrinkToFit() { 68 | // Push and pop a null terminator. This is safe. 69 | *stack_.template Push() = '\0'; 70 | stack_.ShrinkToFit(); 71 | stack_.template Pop(1); 72 | } 73 | 74 | void Reserve(size_t count) { stack_.template Reserve(count); } 75 | Ch *Push(size_t count) { return stack_.template Push(count); } 76 | Ch *PushUnsafe(size_t count) { return stack_.template PushUnsafe(count); } 77 | void Pop(size_t count) { stack_.template Pop(count); } 78 | 79 | const Ch *GetString() const { 80 | // Push and pop a null terminator. This is safe. 81 | *stack_.template Push() = '\0'; 82 | stack_.template Pop(1); 83 | 84 | return stack_.template Bottom(); 85 | } 86 | 87 | //! Get the size of string in bytes in the string buffer. 88 | size_t GetSize() const { return stack_.GetSize(); } 89 | 90 | //! Get the length of string in Ch in the string buffer. 91 | size_t GetLength() const { return stack_.GetSize() / sizeof(Ch); } 92 | 93 | static const size_t kDefaultCapacity = 256; 94 | mutable internal::Stack stack_; 95 | 96 | private: 97 | // Prohibit copy constructor & assignment operator. 98 | GenericStringBuffer(const GenericStringBuffer &); 99 | GenericStringBuffer &operator=(const GenericStringBuffer &); 100 | }; 101 | 102 | //! String buffer with UTF8 encoding 103 | typedef GenericStringBuffer> StringBuffer; 104 | 105 | template 106 | inline void PutReserve(GenericStringBuffer &stream, 107 | size_t count) { 108 | stream.Reserve(count); 109 | } 110 | 111 | template 112 | inline void PutUnsafe(GenericStringBuffer &stream, 113 | typename Encoding::Ch c) { 114 | stream.PutUnsafe(c); 115 | } 116 | 117 | //! Implement specialized version of PutN() with memset() for better 118 | //! performance. 119 | template <> 120 | inline void PutN(GenericStringBuffer> &stream, char c, size_t n) { 121 | std::memset(stream.stack_.Push(n), c, n * sizeof(c)); 122 | } 123 | 124 | RAPIDJSON_NAMESPACE_END 125 | 126 | #if defined(__clang__) 127 | RAPIDJSON_DIAG_POP 128 | #endif 129 | 130 | #endif // RAPIDJSON_STRINGBUFFER_H_ 131 | -------------------------------------------------------------------------------- /livox_ros_driver/common/rapidxml/rapidxml_iterators.hpp: -------------------------------------------------------------------------------- 1 | #ifndef RAPIDXML_ITERATORS_HPP_INCLUDED 2 | #define RAPIDXML_ITERATORS_HPP_INCLUDED 3 | 4 | // Copyright (C) 2006, 2009 Marcin Kalicinski 5 | // Version 1.13 6 | // Revision $DateTime: 2009/05/13 01:46:17 $ 7 | //! \file rapidxml_iterators.hpp This file contains rapidxml iterators 8 | 9 | #include "rapidxml.hpp" 10 | 11 | namespace rapidxml { 12 | 13 | //! Iterator of child nodes of xml_node 14 | template 15 | class node_iterator { 16 | public: 17 | typedef typename xml_node value_type; 18 | typedef typename xml_node &reference; 19 | typedef typename xml_node *pointer; 20 | typedef std::ptrdiff_t difference_type; 21 | typedef std::bidirectional_iterator_tag iterator_category; 22 | 23 | node_iterator() : m_node(0) {} 24 | 25 | node_iterator(xml_node *node) : m_node(node->first_node()) {} 26 | 27 | reference operator*() const { 28 | assert(m_node); 29 | return *m_node; 30 | } 31 | 32 | pointer operator->() const { 33 | assert(m_node); 34 | return m_node; 35 | } 36 | 37 | node_iterator &operator++() { 38 | assert(m_node); 39 | m_node = m_node->next_sibling(); 40 | return *this; 41 | } 42 | 43 | node_iterator operator++(int) { 44 | node_iterator tmp = *this; 45 | ++this; 46 | return tmp; 47 | } 48 | 49 | node_iterator &operator--() { 50 | assert(m_node && m_node->previous_sibling()); 51 | m_node = m_node->previous_sibling(); 52 | return *this; 53 | } 54 | 55 | node_iterator operator--(int) { 56 | node_iterator tmp = *this; 57 | ++this; 58 | return tmp; 59 | } 60 | 61 | bool operator==(const node_iterator &rhs) { return m_node == rhs.m_node; } 62 | 63 | bool operator!=(const node_iterator &rhs) { return m_node != rhs.m_node; } 64 | 65 | private: 66 | xml_node *m_node; 67 | }; 68 | 69 | //! Iterator of child attributes of xml_node 70 | template 71 | class attribute_iterator { 72 | public: 73 | typedef typename xml_attribute value_type; 74 | typedef typename xml_attribute &reference; 75 | typedef typename xml_attribute *pointer; 76 | typedef std::ptrdiff_t difference_type; 77 | typedef std::bidirectional_iterator_tag iterator_category; 78 | 79 | attribute_iterator() : m_attribute(0) {} 80 | 81 | attribute_iterator(xml_node *node) 82 | : m_attribute(node->first_attribute()) {} 83 | 84 | reference operator*() const { 85 | assert(m_attribute); 86 | return *m_attribute; 87 | } 88 | 89 | pointer operator->() const { 90 | assert(m_attribute); 91 | return m_attribute; 92 | } 93 | 94 | attribute_iterator &operator++() { 95 | assert(m_attribute); 96 | m_attribute = m_attribute->next_attribute(); 97 | return *this; 98 | } 99 | 100 | attribute_iterator operator++(int) { 101 | attribute_iterator tmp = *this; 102 | ++this; 103 | return tmp; 104 | } 105 | 106 | attribute_iterator &operator--() { 107 | assert(m_attribute && m_attribute->previous_attribute()); 108 | m_attribute = m_attribute->previous_attribute(); 109 | return *this; 110 | } 111 | 112 | attribute_iterator operator--(int) { 113 | attribute_iterator tmp = *this; 114 | ++this; 115 | return tmp; 116 | } 117 | 118 | bool operator==(const attribute_iterator &rhs) { 119 | return m_attribute == rhs.m_attribute; 120 | } 121 | 122 | bool operator!=(const attribute_iterator &rhs) { 123 | return m_attribute != rhs.m_attribute; 124 | } 125 | 126 | private: 127 | xml_attribute *m_attribute; 128 | }; 129 | 130 | } // namespace rapidxml 131 | 132 | #endif 133 | -------------------------------------------------------------------------------- /livox_ros_driver/common/rapidxml/rapidxml_utils.hpp: -------------------------------------------------------------------------------- 1 | #ifndef RAPIDXML_UTILS_HPP_INCLUDED 2 | #define RAPIDXML_UTILS_HPP_INCLUDED 3 | 4 | // Copyright (C) 2006, 2009 Marcin Kalicinski 5 | // Version 1.13 6 | // Revision $DateTime: 2009/05/13 01:46:17 $ 7 | //! \file rapidxml_utils.hpp This file contains high-level rapidxml utilities 8 | //! that can be useful in certain simple scenarios. They should probably not be 9 | //! used if maximizing performance is the main objective. 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include "rapidxml.hpp" 16 | 17 | namespace rapidxml { 18 | 19 | //! Represents data loaded from a file 20 | template 21 | class file { 22 | public: 23 | //! Loads file into the memory. Data will be automatically destroyed by the 24 | //! destructor. \param filename Filename to load. 25 | file(const char *filename) { 26 | using namespace std; 27 | 28 | // Open stream 29 | basic_ifstream stream(filename, ios::binary); 30 | if (!stream) throw runtime_error(string("cannot open file ") + filename); 31 | stream.unsetf(ios::skipws); 32 | 33 | // Determine stream size 34 | stream.seekg(0, ios::end); 35 | size_t size = stream.tellg(); 36 | stream.seekg(0); 37 | 38 | // Load data and add terminating 0 39 | m_data.resize(size + 1); 40 | stream.read(&m_data.front(), static_cast(size)); 41 | m_data[size] = 0; 42 | } 43 | 44 | //! Loads file into the memory. Data will be automatically destroyed by the 45 | //! destructor \param stream Stream to load from 46 | file(std::basic_istream &stream) { 47 | using namespace std; 48 | 49 | // Load data and add terminating 0 50 | stream.unsetf(ios::skipws); 51 | m_data.assign(istreambuf_iterator(stream), istreambuf_iterator()); 52 | if (stream.fail() || stream.bad()) 53 | throw runtime_error("error reading stream"); 54 | m_data.push_back(0); 55 | } 56 | 57 | //! Gets file data. 58 | //! \return Pointer to data of file. 59 | Ch *data() { return &m_data.front(); } 60 | 61 | //! Gets file data. 62 | //! \return Pointer to data of file. 63 | const Ch *data() const { return &m_data.front(); } 64 | 65 | //! Gets file data size. 66 | //! \return Size of file data, in characters. 67 | std::size_t size() const { return m_data.size(); } 68 | 69 | private: 70 | std::vector m_data; // File data 71 | }; 72 | 73 | //! Counts children of node. Time complexity is O(n). 74 | //! \return Number of children of node 75 | template 76 | inline std::size_t count_children(xml_node *node) { 77 | xml_node *child = node->first_node(); 78 | std::size_t count = 0; 79 | while (child) { 80 | ++count; 81 | child = child->next_sibling(); 82 | } 83 | return count; 84 | } 85 | 86 | //! Counts attributes of node. Time complexity is O(n). 87 | //! \return Number of attributes of node 88 | template 89 | inline std::size_t count_attributes(xml_node *node) { 90 | xml_attribute *attr = node->first_attribute(); 91 | std::size_t count = 0; 92 | while (attr) { 93 | ++count; 94 | attr = attr->next_attribute(); 95 | } 96 | return count; 97 | } 98 | 99 | } // namespace rapidxml 100 | 101 | #endif 102 | -------------------------------------------------------------------------------- /livox_ros_driver/config/display_hub_points.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Grid1 10 | - /PointCloud1/Autocompute Value Bounds1 11 | - /PointCloud21 12 | Splitter Ratio: 0.500695 13 | Tree Height: 680 14 | - Class: rviz/Selection 15 | Name: Selection 16 | - Class: rviz/Tool Properties 17 | Expanded: 18 | - /2D Pose Estimate1 19 | - /2D Nav Goal1 20 | - /Publish Point1 21 | Name: Tool Properties 22 | Splitter Ratio: 0.588679 23 | - Class: rviz/Views 24 | Expanded: 25 | - /Current View1 26 | Name: Views 27 | Splitter Ratio: 0.5 28 | - Class: rviz/Time 29 | Experimental: false 30 | Name: Time 31 | SyncMode: 0 32 | SyncSource: PointCloud2 33 | Visualization Manager: 34 | Class: "" 35 | Displays: 36 | - Alpha: 0.5 37 | Cell Size: 1 38 | Class: rviz/Grid 39 | Color: 160; 160; 164 40 | Enabled: true 41 | Line Style: 42 | Line Width: 0.03 43 | Value: Lines 44 | Name: Grid 45 | Normal Cell Count: 0 46 | Offset: 47 | X: 0 48 | Y: 0 49 | Z: 0 50 | Plane: XY 51 | Plane Cell Count: 10 52 | Reference Frame: 53 | Value: true 54 | - Alpha: 1 55 | Autocompute Intensity Bounds: true 56 | Autocompute Value Bounds: 57 | Max Value: 0.856 58 | Min Value: -0.735 59 | Value: true 60 | Axis: Z 61 | Channel Name: x 62 | Class: rviz/PointCloud 63 | Color: 255; 255; 255 64 | Color Transformer: Intensity 65 | Decay Time: 1 66 | Enabled: false 67 | Invert Rainbow: false 68 | Max Color: 255; 255; 255 69 | Max Intensity: -0.088 70 | Min Color: 0; 0; 0 71 | Min Intensity: -1.951 72 | Name: PointCloud 73 | Position Transformer: XYZ 74 | Queue Size: 1000 75 | Selectable: true 76 | Size (Pixels): 2 77 | Size (m): 0.005 78 | Style: Flat Squares 79 | Topic: /livox/lidar 80 | Unreliable: false 81 | Use Fixed Frame: true 82 | Use rainbow: true 83 | Value: false 84 | - Alpha: 1 85 | Autocompute Intensity Bounds: true 86 | Autocompute Value Bounds: 87 | Max Value: 0.816 88 | Min Value: -0.674 89 | Value: true 90 | Axis: Z 91 | Channel Name: intensity 92 | Class: rviz/PointCloud2 93 | Color: 255; 255; 255 94 | Color Transformer: Intensity 95 | Decay Time: 1 96 | Enabled: true 97 | Invert Rainbow: true 98 | Max Color: 255; 255; 255 99 | Max Intensity: 152 100 | Min Color: 0; 0; 0 101 | Min Intensity: 0 102 | Name: PointCloud2 103 | Position Transformer: XYZ 104 | Queue Size: 10 105 | Selectable: true 106 | Size (Pixels): 2 107 | Size (m): 0.005 108 | Style: Points 109 | Topic: /livox/lidar 110 | Unreliable: false 111 | Use Fixed Frame: true 112 | Use rainbow: true 113 | Value: true 114 | Enabled: true 115 | Global Options: 116 | Background Color: 48; 48; 48 117 | Fixed Frame: livox_frame 118 | Frame Rate: 50 119 | Name: root 120 | Tools: 121 | - Class: rviz/Interact 122 | Hide Inactive Objects: true 123 | - Class: rviz/MoveCamera 124 | - Class: rviz/Select 125 | - Class: rviz/FocusCamera 126 | - Class: rviz/Measure 127 | - Class: rviz/SetInitialPose 128 | Topic: /initialpose 129 | - Class: rviz/SetGoal 130 | Topic: /move_base_simple/goal 131 | - Class: rviz/PublishPoint 132 | Single click: true 133 | Topic: /clicked_point 134 | Value: true 135 | Views: 136 | Current: 137 | Class: rviz/Orbit 138 | Distance: 5.292 139 | Enable Stereo Rendering: 140 | Stereo Eye Separation: 0.06 141 | Stereo Focal Distance: 1 142 | Swap Stereo Eyes: false 143 | Value: false 144 | Focal Point: 145 | X: 0.267255 146 | Y: 0.0618536 147 | Z: 0.150874 148 | Name: Current View 149 | Near Clip Distance: 0.01 150 | Pitch: 0.209796 151 | Target Frame: 152 | Value: Orbit (rviz) 153 | Yaw: 3.21241 154 | Saved: 155 | - Class: rviz/Orbit 156 | Distance: 10 157 | Enable Stereo Rendering: 158 | Stereo Eye Separation: 0.06 159 | Stereo Focal Distance: 1 160 | Swap Stereo Eyes: false 161 | Value: false 162 | Focal Point: 163 | X: 0 164 | Y: 0 165 | Z: 0 166 | Name: Orbit 167 | Near Clip Distance: 0.01 168 | Pitch: 1.1104 169 | Target Frame: 170 | Value: Orbit (rviz) 171 | Yaw: 0.570397 172 | Window Geometry: 173 | Displays: 174 | collapsed: false 175 | Height: 961 176 | Hide Left Dock: false 177 | Hide Right Dock: true 178 | QMainWindow State: 000000ff00000000fd0000000400000000000001a900000337fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000337000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002f600fffffffb0000000800540069006d006501000000000000045000000000000000000000058e0000033700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 179 | Selection: 180 | collapsed: false 181 | Time: 182 | collapsed: false 183 | Tool Properties: 184 | collapsed: false 185 | Views: 186 | collapsed: true 187 | Width: 1853 188 | X: 124 189 | Y: 81 190 | -------------------------------------------------------------------------------- /livox_ros_driver/config/display_lidar_points.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Grid1 10 | - /PointCloud21 11 | Splitter Ratio: 0.500694990158081 12 | Tree Height: 728 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.5886790156364441 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: PointCloud2 32 | Preferences: 33 | PromptSaveOnExit: true 34 | Toolbars: 35 | toolButtonStyle: 2 36 | Visualization Manager: 37 | Class: "" 38 | Displays: 39 | - Alpha: 0.5 40 | Cell Size: 1 41 | Class: rviz/Grid 42 | Color: 160; 160; 164 43 | Enabled: true 44 | Line Style: 45 | Line Width: 0.029999999329447746 46 | Value: Lines 47 | Name: Grid 48 | Normal Cell Count: 0 49 | Offset: 50 | X: 0 51 | Y: 0 52 | Z: 0 53 | Plane: XY 54 | Plane Cell Count: 10 55 | Reference Frame: 56 | Value: true 57 | - Alpha: 1 58 | Autocompute Intensity Bounds: true 59 | Autocompute Value Bounds: 60 | Max Value: 0.4569999873638153 61 | Min Value: -0.367000013589859 62 | Value: true 63 | Axis: Z 64 | Channel Name: intensity 65 | Class: rviz/PointCloud2 66 | Color: 255; 255; 255 67 | Color Transformer: Intensity 68 | Decay Time: 1 69 | Enabled: true 70 | Invert Rainbow: false 71 | Max Color: 255; 255; 255 72 | Max Intensity: 255 73 | Min Color: 0; 0; 0 74 | Min Intensity: 0 75 | Name: PointCloud2 76 | Position Transformer: XYZ 77 | Queue Size: 10 78 | Selectable: true 79 | Size (Pixels): 2 80 | Size (m): 0.004999999888241291 81 | Style: Points 82 | Topic: /livox/lidar 83 | Unreliable: false 84 | Use Fixed Frame: true 85 | Use rainbow: true 86 | Value: true 87 | Enabled: true 88 | Global Options: 89 | Background Color: 48; 48; 48 90 | Default Light: true 91 | Fixed Frame: livox_frame 92 | Frame Rate: 40 93 | Name: root 94 | Tools: 95 | - Class: rviz/Interact 96 | Hide Inactive Objects: true 97 | - Class: rviz/MoveCamera 98 | - Class: rviz/Select 99 | - Class: rviz/FocusCamera 100 | - Class: rviz/Measure 101 | - Class: rviz/SetInitialPose 102 | Theta std deviation: 0.2617993950843811 103 | Topic: /initialpose 104 | X std deviation: 0.5 105 | Y std deviation: 0.5 106 | - Class: rviz/SetGoal 107 | Topic: /move_base_simple/goal 108 | - Class: rviz/PublishPoint 109 | Single click: true 110 | Topic: /clicked_point 111 | Value: true 112 | Views: 113 | Current: 114 | Class: rviz/Orbit 115 | Distance: 25.80008888244629 116 | Enable Stereo Rendering: 117 | Stereo Eye Separation: 0.05999999865889549 118 | Stereo Focal Distance: 1 119 | Swap Stereo Eyes: false 120 | Value: false 121 | Focal Point: 122 | X: 0.2672550082206726 123 | Y: 0.061853598803281784 124 | Z: 0.15087400376796722 125 | Focal Shape Fixed Size: true 126 | Focal Shape Size: 0.05000000074505806 127 | Invert Z Axis: false 128 | Name: Current View 129 | Near Clip Distance: 0.009999999776482582 130 | Pitch: 0.5597995519638062 131 | Target Frame: 132 | Value: Orbit (rviz) 133 | Yaw: 3.065610408782959 134 | Saved: 135 | - Class: rviz/Orbit 136 | Distance: 10 137 | Enable Stereo Rendering: 138 | Stereo Eye Separation: 0.05999999865889549 139 | Stereo Focal Distance: 1 140 | Swap Stereo Eyes: false 141 | Value: false 142 | Focal Point: 143 | X: 0 144 | Y: 0 145 | Z: 0 146 | Focal Shape Fixed Size: true 147 | Focal Shape Size: 0.05000000074505806 148 | Invert Z Axis: false 149 | Name: Orbit 150 | Near Clip Distance: 0.009999999776482582 151 | Pitch: 1.1103999614715576 152 | Target Frame: 153 | Value: Orbit (rviz) 154 | Yaw: 0.5703970193862915 155 | Window Geometry: 156 | Displays: 157 | collapsed: false 158 | Height: 1025 159 | Hide Left Dock: false 160 | Hide Right Dock: true 161 | QMainWindow State: 000000ff00000000fd0000000400000000000001c400000363fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000363000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005730000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 162 | Selection: 163 | collapsed: false 164 | Time: 165 | collapsed: false 166 | Tool Properties: 167 | collapsed: false 168 | Views: 169 | collapsed: true 170 | Width: 1853 171 | X: 67 172 | Y: 27 173 | -------------------------------------------------------------------------------- /livox_ros_driver/config/livox_hub_config.json: -------------------------------------------------------------------------------- 1 | { 2 | "hub_config": { 3 | "broadcast_code": "13UUG1R00400170", 4 | "enable_connect": false, 5 | "coordinate": 0 6 | }, 7 | "lidar_config": [ 8 | { 9 | "broadcast_code": "0TFDG3B006H2Z11", 10 | "return_mode": 0, 11 | "imu_rate": 0 12 | }, 13 | { 14 | "broadcast_code": "0TFDG3U99101291", 15 | "return_mode": 0, 16 | "imu_rate": 0 17 | }, 18 | { 19 | "broadcast_code": "1HDDG8M00100191", 20 | "return_mode": 0, 21 | "imu_rate": 0 22 | }, 23 | { 24 | "broadcast_code": "1PQDG8E00100321", 25 | "return_mode": 0, 26 | "imu_rate": 0 27 | } 28 | ] 29 | } 30 | -------------------------------------------------------------------------------- /livox_ros_driver/config/livox_lidar_config.json: -------------------------------------------------------------------------------- 1 | { 2 | "lidar_config": [ 3 | { 4 | "broadcast_code": "1PQDH5B00100041", 5 | "enable_connect": false, 6 | "return_mode": 0, 7 | "coordinate": 0, 8 | "imu_rate": 0, 9 | "extrinsic_parameter_source": 0, 10 | "enable_high_sensitivity": false 11 | }, 12 | { 13 | "broadcast_code": "0TFDG3U99101431", 14 | "enable_connect": false, 15 | "return_mode": 0, 16 | "coordinate": 0, 17 | "imu_rate": 0, 18 | "extrinsic_parameter_source": 0, 19 | "enable_high_sensitivity": false 20 | } 21 | ], 22 | 23 | "timesync_config": { 24 | "enable_timesync": false, 25 | "device_name": "/dev/ttyUSB0", 26 | "comm_device_type": 0, 27 | "baudrate_index": 2, 28 | "parity_index": 0 29 | } 30 | } 31 | -------------------------------------------------------------------------------- /livox_ros_driver/launch/livox_hub.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 32 | 33 | 34 | 36 | 37 | 38 | 39 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /livox_ros_driver/launch/livox_hub_msg.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 32 | 33 | 34 | 36 | 37 | 38 | 39 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /livox_ros_driver/launch/livox_hub_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 32 | 33 | 34 | 36 | 37 | 38 | 39 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /livox_ros_driver/launch/livox_lidar.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 32 | 33 | 34 | 36 | 37 | 38 | 39 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /livox_ros_driver/launch/livox_lidar_msg.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 32 | 33 | 34 | 36 | 37 | 38 | 39 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /livox_ros_driver/launch/livox_lidar_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 32 | 33 | 34 | 36 | 37 | 38 | 39 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /livox_ros_driver/launch/livox_template.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 32 | 33 | 34 | 36 | 37 | 38 | 39 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /livox_ros_driver/launch/lvx_to_rosbag.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 32 | 33 | 34 | 36 | 37 | 38 | 39 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /livox_ros_driver/launch/lvx_to_rosbag_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 32 | 33 | 34 | 36 | 37 | 38 | 39 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /livox_ros_driver/livox_ros_driver/include/livox_ros_driver.h: -------------------------------------------------------------------------------- 1 | // 2 | // The MIT License (MIT) 3 | // 4 | // Copyright (c) 2019 Livox. All rights reserved. 5 | // 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | // 13 | // The above copyright notice and this permission notice shall be included in 14 | // all copies or substantial portions of the Software. 15 | // 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | // 24 | 25 | #ifndef LIVOX_ROS_DRIVER_INClUDE_LIVOX_ROS_DRIVER_H_ 26 | #define LIVOX_ROS_DRIVER_INClUDE_LIVOX_ROS_DRIVER_H_ 27 | 28 | #define LIVOX_ROS_DRIVER_VER_MAJOR 2 29 | #define LIVOX_ROS_DRIVER_VER_MINOR 6 30 | #define LIVOX_ROS_DRIVER_VER_PATCH 0 31 | 32 | #define GET_STRING(n) GET_STRING_DIRECT(n) 33 | #define GET_STRING_DIRECT(n) #n 34 | 35 | #define LIVOX_ROS_DRIVER_VERSION_STRING \ 36 | GET_STRING(LIVOX_ROS_DRIVER_VER_MAJOR) \ 37 | "." GET_STRING(LIVOX_ROS_DRIVER_VER_MINOR) "." GET_STRING( \ 38 | LIVOX_ROS_DRIVER_VER_PATCH) 39 | 40 | #endif 41 | -------------------------------------------------------------------------------- /livox_ros_driver/livox_ros_driver/lddc.h: -------------------------------------------------------------------------------- 1 | // 2 | // The MIT License (MIT) 3 | // 4 | // Copyright (c) 2019 Livox. All rights reserved. 5 | // 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | // 13 | // The above copyright notice and this permission notice shall be included in 14 | // all copies or substantial portions of the Software. 15 | // 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | // 24 | #ifndef LIVOX_ROS_DRIVER_LDDC_H_ 25 | #define LIVOX_ROS_DRIVER_LDDC_H_ 26 | 27 | #include "lds.h" 28 | #include "livox_sdk.h" 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | namespace livox_ros { 37 | 38 | typedef pcl::PointCloud PointCloud; 39 | 40 | /** Lidar data distribute control */ 41 | typedef enum { 42 | kPointCloud2Msg = 0, 43 | kLivoxCustomMsg = 1, 44 | kPclPxyziMsg 45 | } TransferType; 46 | 47 | class Lddc { 48 | public: 49 | Lddc(int format, int multi_topic, int data_src, int output_type, double frq, 50 | std::string &frame_id, bool lidar_bag, bool imu_bag); 51 | ~Lddc(); 52 | 53 | int RegisterLds(Lds *lds); 54 | void DistributeLidarData(void); 55 | void CreateBagFile(const std::string &file_name); 56 | void PrepareExit(void); 57 | 58 | uint8_t GetTransferFormat(void) { return transfer_format_; } 59 | uint8_t IsMultiTopic(void) { return use_multi_topic_; } 60 | void SetRosNode(ros::NodeHandle *node) { cur_node_ = node; } 61 | 62 | void SetRosPub(ros::Publisher *pub) { global_pub_ = pub; }; 63 | void SetPublishFrq(uint32_t frq) { publish_frq_ = frq; } 64 | 65 | Lds *lds_; 66 | 67 | private: 68 | int32_t GetPublishStartTime(LidarDevice *lidar, LidarDataQueue *queue, 69 | uint64_t *start_time, 70 | StoragePacket *storage_packet); 71 | uint32_t PublishPointcloud2(LidarDataQueue *queue, uint32_t packet_num, 72 | uint8_t handle); 73 | uint32_t PublishPointcloudData(LidarDataQueue *queue, uint32_t packet_num, 74 | uint8_t handle); 75 | uint32_t PublishCustomPointcloud(LidarDataQueue *queue, uint32_t packet_num, 76 | uint8_t handle); 77 | uint32_t PublishImuData(LidarDataQueue *queue, uint32_t packet_num, 78 | uint8_t handle); 79 | 80 | ros::Publisher *GetCurrentPublisher(uint8_t handle); 81 | ros::Publisher *GetCurrentImuPublisher(uint8_t handle); 82 | void PollingLidarPointCloudData(uint8_t handle, LidarDevice *lidar); 83 | void PollingLidarImuData(uint8_t handle, LidarDevice *lidar); 84 | void InitPointcloud2MsgHeader(sensor_msgs::PointCloud2& cloud); 85 | void FillPointsToPclMsg(PointCloud::Ptr& pcl_msg, \ 86 | LivoxPointXyzrtl* src_point, uint32_t num); 87 | void FillPointsToCustomMsg(livox_ros_driver::CustomMsg& livox_msg, \ 88 | LivoxPointXyzrtl* src_point, uint32_t num, uint32_t offset_time, \ 89 | uint32_t point_interval, uint32_t echo_num); 90 | uint8_t transfer_format_; 91 | uint8_t use_multi_topic_; 92 | uint8_t data_src_; 93 | uint8_t output_type_; 94 | double publish_frq_; 95 | uint32_t publish_period_ns_; 96 | std::string frame_id_; 97 | bool enable_lidar_bag_; 98 | bool enable_imu_bag_; 99 | ros::Publisher *private_pub_[kMaxSourceLidar]; 100 | ros::Publisher *global_pub_; 101 | ros::Publisher *private_imu_pub_[kMaxSourceLidar]; 102 | ros::Publisher *global_imu_pub_; 103 | 104 | ros::NodeHandle *cur_node_; 105 | rosbag::Bag *bag_; 106 | }; 107 | 108 | } // namespace livox_ros 109 | #endif 110 | -------------------------------------------------------------------------------- /livox_ros_driver/livox_ros_driver/ldq.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // The MIT License (MIT) 3 | // 4 | // Copyright (c) 2019 Livox. All rights reserved. 5 | // 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | // 13 | // The above copyright notice and this permission notice shall be included in 14 | // all copies or substantial portions of the Software. 15 | // 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | // 24 | 25 | #include "ldq.h" 26 | 27 | #include 28 | #include 29 | 30 | namespace livox_ros { 31 | 32 | /* for pointcloud queue process */ 33 | int InitQueue(LidarDataQueue *queue, uint32_t queue_size) { 34 | if (queue == nullptr) { 35 | return 1; 36 | } 37 | 38 | if (IsPowerOf2(queue_size) != true) { 39 | queue_size = RoundupPowerOf2(queue_size); 40 | } 41 | 42 | queue->storage_packet = new StoragePacket[queue_size]; 43 | if (queue->storage_packet == nullptr) { 44 | return 1; 45 | } 46 | 47 | queue->rd_idx = 0; 48 | queue->wr_idx = 0; 49 | queue->size = queue_size; 50 | queue->mask = queue_size - 1; 51 | 52 | return 0; 53 | } 54 | 55 | int DeInitQueue(LidarDataQueue *queue) { 56 | if (queue == nullptr) { 57 | return 1; 58 | } 59 | 60 | if (queue->storage_packet) { 61 | delete[] queue->storage_packet; 62 | } 63 | 64 | queue->rd_idx = 0; 65 | queue->wr_idx = 0; 66 | queue->size = 0; 67 | queue->mask = 0; 68 | 69 | return 0; 70 | } 71 | 72 | void ResetQueue(LidarDataQueue *queue) { 73 | queue->rd_idx = 0; 74 | queue->wr_idx = 0; 75 | } 76 | 77 | void QueuePrePop(LidarDataQueue *queue, StoragePacket *storage_packet) { 78 | uint32_t rd_idx = queue->rd_idx & queue->mask; 79 | 80 | memcpy(storage_packet, &(queue->storage_packet[rd_idx]), 81 | sizeof(StoragePacket)); 82 | } 83 | 84 | void QueuePopUpdate(LidarDataQueue *queue) { queue->rd_idx++; } 85 | 86 | uint32_t QueuePop(LidarDataQueue *queue, StoragePacket *storage_packet) { 87 | QueuePrePop(queue, storage_packet); 88 | QueuePopUpdate(queue); 89 | 90 | return 1; 91 | } 92 | 93 | uint32_t QueueUsedSize(LidarDataQueue *queue) { 94 | return queue->wr_idx - queue->rd_idx; 95 | } 96 | 97 | uint32_t QueueUnusedSize(LidarDataQueue *queue) { 98 | return (queue->size - (queue->wr_idx - queue->rd_idx)); 99 | } 100 | 101 | uint32_t QueueIsFull(LidarDataQueue *queue) { 102 | return ((queue->wr_idx - queue->rd_idx) > queue->mask); 103 | } 104 | 105 | uint32_t QueueIsEmpty(LidarDataQueue *queue) { 106 | return (queue->rd_idx == queue->wr_idx); 107 | } 108 | 109 | uint32_t QueuePush(LidarDataQueue *queue, StoragePacket *storage_packet) { 110 | uint32_t wr_idx = queue->wr_idx & queue->mask; 111 | 112 | memcpy((void *)(&(queue->storage_packet[wr_idx])), (void *)(storage_packet), 113 | sizeof(StoragePacket)); 114 | 115 | queue->wr_idx++; 116 | 117 | return 1; 118 | } 119 | 120 | uint32_t QueuePushAny(LidarDataQueue *queue, uint8_t *data, uint32_t length, 121 | uint64_t time_rcv, uint32_t point_num) { 122 | uint32_t wr_idx = queue->wr_idx & queue->mask; 123 | 124 | queue->storage_packet[wr_idx].time_rcv = time_rcv; 125 | queue->storage_packet[wr_idx].point_num = point_num; 126 | memcpy(queue->storage_packet[wr_idx].raw_data, data, length); 127 | 128 | queue->wr_idx++; 129 | 130 | return 1; 131 | } 132 | 133 | } // namespace livox_ros 134 | -------------------------------------------------------------------------------- /livox_ros_driver/livox_ros_driver/ldq.h: -------------------------------------------------------------------------------- 1 | // 2 | // The MIT License (MIT) 3 | // 4 | // Copyright (c) 2019 Livox. All rights reserved. 5 | // 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | // 13 | // The above copyright notice and this permission notice shall be included in 14 | // all copies or substantial portions of the Software. 15 | // 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | // 24 | 25 | // livox lidar data queue 26 | 27 | #ifndef LIVOX_ROS_DRIVER_LDQ_H_ 28 | #define LIVOX_ROS_DRIVER_LDQ_H_ 29 | 30 | #include 31 | 32 | namespace livox_ros { 33 | 34 | const uint32_t KEthPacketMaxLength = 1500; 35 | 36 | #pragma pack(1) 37 | 38 | typedef struct { 39 | uint64_t time_rcv; /**< receive time when data arrive */ 40 | uint32_t point_num; 41 | uint8_t raw_data[KEthPacketMaxLength]; 42 | } StoragePacket; 43 | 44 | #pragma pack() 45 | 46 | typedef struct { 47 | StoragePacket *storage_packet; 48 | volatile uint32_t rd_idx; 49 | volatile uint32_t wr_idx; 50 | uint32_t mask; 51 | uint32_t size; /**< must be power of 2. */ 52 | } LidarDataQueue; 53 | 54 | inline static bool IsPowerOf2(uint32_t size) { 55 | return (size != 0) && ((size & (size - 1)) == 0); 56 | } 57 | 58 | inline static uint32_t RoundupPowerOf2(uint32_t size) { 59 | uint32_t power2_val = 0; 60 | for (int i = 0; i < 32; i++) { 61 | power2_val = ((uint32_t)1) << i; 62 | if (size <= power2_val) { 63 | break; 64 | } 65 | } 66 | 67 | return power2_val; 68 | } 69 | 70 | /** queue operate function */ 71 | int InitQueue(LidarDataQueue *queue, uint32_t queue_size); 72 | int DeInitQueue(LidarDataQueue *queue); 73 | void ResetQueue(LidarDataQueue *queue); 74 | void QueuePrePop(LidarDataQueue *queue, StoragePacket *storage_packet); 75 | void QueuePopUpdate(LidarDataQueue *queue); 76 | uint32_t QueuePop(LidarDataQueue *queue, StoragePacket *storage_packet); 77 | uint32_t QueueUsedSize(LidarDataQueue *queue); 78 | uint32_t QueueUnusedSize(LidarDataQueue *queue); 79 | uint32_t QueueIsFull(LidarDataQueue *queue); 80 | uint32_t QueueIsEmpty(LidarDataQueue *queue); 81 | uint32_t QueuePush(LidarDataQueue *queue, StoragePacket *storage_packet); 82 | uint32_t QueuePushAny(LidarDataQueue *queue, uint8_t *data, uint32_t length, 83 | uint64_t time_rcv, uint32_t point_num); 84 | 85 | } // namespace livox_ros 86 | #endif 87 | -------------------------------------------------------------------------------- /livox_ros_driver/livox_ros_driver/lds_hub.h: -------------------------------------------------------------------------------- 1 | // 2 | // The MIT License (MIT) 3 | // 4 | // Copyright (c) 2019 Livox. All rights reserved. 5 | // 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | // 13 | // The above copyright notice and this permission notice shall be included in 14 | // all copies or substantial portions of the Software. 15 | // 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | // 24 | 25 | /** Livox LiDAR data source, data from hub */ 26 | 27 | #ifndef LIVOX_ROS_DRIVER_LDS_HUB_H_ 28 | #define LIVOX_ROS_DRIVER_LDS_HUB_H_ 29 | 30 | #include 31 | #include 32 | 33 | #include "lds.h" 34 | #include "livox_sdk.h" 35 | 36 | namespace livox_ros { 37 | 38 | /** 39 | * LiDAR data source, data from hub. 40 | */ 41 | class LdsHub : public Lds { 42 | public: 43 | static LdsHub *GetInstance(uint32_t interval_ms) { 44 | static LdsHub lds_hub(interval_ms); 45 | return &lds_hub; 46 | } 47 | 48 | int InitLdsHub(std::vector &broadcast_code_strs, 49 | const char *user_config_path); 50 | int DeInitLdsHub(void); 51 | 52 | private: 53 | LdsHub(uint32_t interval_ms); 54 | LdsHub(const LdsHub &) = delete; 55 | ~LdsHub(); 56 | LdsHub &operator=(const LdsHub &) = delete; 57 | virtual void PrepareExit(void); 58 | 59 | static void OnHubDataCb(uint8_t hub_handle, LivoxEthPacket *data, 60 | uint32_t data_num, void *client_data); 61 | static void OnDeviceBroadcast(const BroadcastDeviceInfo *info); 62 | static void OnDeviceChange(const DeviceInfo *info, DeviceEvent type); 63 | static void StartSampleCb(livox_status status, uint8_t handle, 64 | uint8_t response, void *clent_data); 65 | static void StopSampleCb(livox_status status, uint8_t handle, 66 | uint8_t response, void *clent_data); 67 | static void HubQueryLidarInfoCb(livox_status status, uint8_t handle, 68 | HubQueryLidarInformationResponse *response, 69 | void *client_data); 70 | static void ControlFanCb(livox_status status, uint8_t handle, 71 | uint8_t response, void *clent_data); 72 | static void HubSetPointCloudReturnModeCb( 73 | livox_status status, uint8_t handle, 74 | HubSetPointCloudReturnModeResponse *response, void *clent_data); 75 | static void SetCoordinateCb(livox_status status, uint8_t handle, 76 | uint8_t response, void *clent_data); 77 | static void HubSetImuRatePushFrequencyCb( 78 | livox_status status, uint8_t handle, 79 | HubSetImuPushFrequencyResponse *response, void *clent_data); 80 | static void HubErrorStatusCb(livox_status status, uint8_t handle, 81 | ErrorMessage *message); 82 | static void ConfigPointCloudReturnMode(LdsHub *lds_hub); 83 | static void ConfigImuPushFrequency(LdsHub *lds_hub); 84 | static void ConfigLidarsOfHub(LdsHub *lds_hub); 85 | 86 | void ResetLdsHub(void); 87 | void StateReset(void); 88 | int AddBroadcastCodeToWhitelist(const char *broadcast_code); 89 | bool IsBroadcastCodeExistInWhitelist(const char *broadcast_code); 90 | void UpdateHubLidarinfo(void); 91 | 92 | void EnableAutoConnectMode(void) { auto_connect_mode_ = true; } 93 | void DisableAutoConnectMode(void) { auto_connect_mode_ = false; } 94 | bool IsAutoConnectMode(void) { return auto_connect_mode_; } 95 | int ParseConfigFile(const char *pathname); 96 | int AddRawUserConfig(UserRawConfig &config); 97 | bool IsExistInRawConfig(const char *broadcast_code); 98 | int GetRawConfig(const char *broadcast_code, UserRawConfig &config); 99 | bool IsAllLidarSetBitsClear() { 100 | for (int i = 0; i < kMaxLidarCount; i++) { 101 | if (lidars_[i].config.set_bits) { 102 | return false; 103 | } 104 | } 105 | return true; 106 | } 107 | 108 | bool auto_connect_mode_; 109 | uint32_t whitelist_count_; 110 | volatile bool is_initialized_; 111 | char broadcast_code_whitelist_[kMaxLidarCount][kBroadcastCodeSize]; 112 | 113 | LidarDevice hub_; 114 | 115 | std::vector lidar_raw_config_; 116 | UserRawConfig hub_raw_config_; 117 | }; 118 | 119 | } // namespace livox_ros 120 | #endif 121 | -------------------------------------------------------------------------------- /livox_ros_driver/livox_ros_driver/lds_lidar.h: -------------------------------------------------------------------------------- 1 | // 2 | // The MIT License (MIT) 3 | // 4 | // Copyright (c) 2019 Livox. All rights reserved. 5 | // 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | // 13 | // The above copyright notice and this permission notice shall be included in 14 | // all copies or substantial portions of the Software. 15 | // 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | // 24 | 25 | /** Livox LiDAR data source, data from dependent lidar */ 26 | 27 | #ifndef LIVOX_ROS_DRIVER_LDS_LIDAR_H_ 28 | #define LIVOX_ROS_DRIVER_LDS_LIDAR_H_ 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | #include "lds.h" 35 | #include "livox_sdk.h" 36 | #include "rapidjson/document.h" 37 | #include "timesync.h" 38 | 39 | namespace livox_ros { 40 | 41 | /** 42 | * LiDAR data source, data from dependent lidar. 43 | */ 44 | class LdsLidar : public Lds { 45 | public: 46 | static LdsLidar *GetInstance(uint32_t interval_ms) { 47 | static LdsLidar lds_lidar(interval_ms); 48 | return &lds_lidar; 49 | } 50 | 51 | int InitLdsLidar(std::vector &broadcast_code_strs, 52 | const char *user_config_path); 53 | int DeInitLdsLidar(void); 54 | 55 | private: 56 | LdsLidar(uint32_t interval_ms); 57 | LdsLidar(const LdsLidar &) = delete; 58 | ~LdsLidar(); 59 | LdsLidar &operator=(const LdsLidar &) = delete; 60 | virtual void PrepareExit(void); 61 | 62 | static void OnLidarDataCb(uint8_t handle, LivoxEthPacket *data, 63 | uint32_t data_num, void *client_data); 64 | static void OnDeviceBroadcast(const BroadcastDeviceInfo *info); 65 | static void OnDeviceChange(const DeviceInfo *info, DeviceEvent type); 66 | static void StartSampleCb(livox_status status, uint8_t handle, 67 | uint8_t response, void *clent_data); 68 | static void StopSampleCb(livox_status status, uint8_t handle, 69 | uint8_t response, void *clent_data); 70 | static void DeviceInformationCb(livox_status status, uint8_t handle, 71 | DeviceInformationResponse *ack, 72 | void *clent_data); 73 | static void LidarErrorStatusCb(livox_status status, uint8_t handle, 74 | ErrorMessage *message); 75 | static void ControlFanCb(livox_status status, uint8_t handle, 76 | uint8_t response, void *clent_data); 77 | static void SetPointCloudReturnModeCb(livox_status status, uint8_t handle, 78 | uint8_t response, void *clent_data); 79 | static void SetCoordinateCb(livox_status status, uint8_t handle, 80 | uint8_t response, void *clent_data); 81 | static void SetImuRatePushFrequencyCb(livox_status status, uint8_t handle, 82 | uint8_t response, void *clent_data); 83 | static void SetRmcSyncTimeCb(livox_status status, uint8_t handle, 84 | uint8_t response, void *client_data); 85 | static void ReceiveSyncTimeCallback(const char *rmc, uint32_t rmc_length, 86 | void *client_data); 87 | static void GetLidarExtrinsicParameterCb( 88 | livox_status status, uint8_t handle, 89 | LidarGetExtrinsicParameterResponse *response, void *clent_data); 90 | static void SetHighSensitivityCb(livox_status status, uint8_t handle, 91 | DeviceParameterResponse *response, 92 | void *clent_data); 93 | 94 | void ResetLdsLidar(void); 95 | int AddBroadcastCodeToWhitelist(const char *broadcast_code); 96 | bool IsBroadcastCodeExistInWhitelist(const char *broadcast_code); 97 | 98 | void EnableAutoConnectMode(void) { auto_connect_mode_ = true; } 99 | void DisableAutoConnectMode(void) { auto_connect_mode_ = false; } 100 | bool IsAutoConnectMode(void) { return auto_connect_mode_; } 101 | int ParseTimesyncConfig(rapidjson::Document &doc); 102 | int ParseConfigFile(const char *pathname); 103 | int AddRawUserConfig(UserRawConfig &config); 104 | bool IsExistInRawConfig(const char *broadcast_code); 105 | int GetRawConfig(const char *broadcast_code, UserRawConfig &config); 106 | 107 | bool auto_connect_mode_; 108 | uint32_t whitelist_count_; 109 | volatile bool is_initialized_; 110 | char broadcast_code_whitelist_[kMaxLidarCount][kBroadcastCodeSize]; 111 | std::vector raw_config_; 112 | 113 | bool enable_timesync_; 114 | TimeSync *timesync_; 115 | TimeSyncConfig timesync_config_; 116 | std::mutex config_mutex_; 117 | }; 118 | 119 | } // namespace livox_ros 120 | #endif 121 | -------------------------------------------------------------------------------- /livox_ros_driver/livox_ros_driver/lds_lvx.h: -------------------------------------------------------------------------------- 1 | // 2 | // The MIT License (MIT) 3 | // 4 | // Copyright (c) 2019 Livox. All rights reserved. 5 | // 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | // 13 | // The above copyright notice and this permission notice shall be included in 14 | // all copies or substantial portions of the Software. 15 | // 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | // 24 | 25 | // livox lidar lvx data source 26 | 27 | #ifndef LIVOX_ROS_DRIVER_LDS_LVX_H_ 28 | #define LIVOX_ROS_DRIVER_LDS_LVX_H_ 29 | 30 | #include 31 | #include 32 | 33 | #include "lds.h" 34 | #include "lvx_file.h" 35 | 36 | namespace livox_ros { 37 | 38 | /** 39 | * Lidar data source abstract. 40 | */ 41 | class LdsLvx : public Lds { 42 | public: 43 | static LdsLvx *GetInstance(uint32_t interval_ms) { 44 | static LdsLvx lds_lvx(interval_ms); 45 | return &lds_lvx; 46 | } 47 | 48 | int InitLdsLvx(const char *lvx_path); 49 | int DeInitLdsLvx(void); 50 | void PrepareExit(void); 51 | 52 | private: 53 | LdsLvx(uint32_t interval_ms); 54 | LdsLvx(const LdsLvx &) = delete; 55 | ~LdsLvx(); 56 | LdsLvx &operator=(const LdsLvx &) = delete; 57 | 58 | void StartRead() { start_read_lvx_ = true; } 59 | void StopRead() { start_read_lvx_ = false; } 60 | bool IsStarted() { return start_read_lvx_; } 61 | 62 | void ReadLvxFile(); 63 | 64 | volatile bool is_initialized_; 65 | OutPacketBuffer packets_of_frame_; 66 | std::shared_ptr lvx_file_; 67 | std::shared_ptr t_read_lvx_; 68 | volatile bool start_read_lvx_; 69 | }; 70 | 71 | } // namespace livox_ros 72 | #endif 73 | -------------------------------------------------------------------------------- /livox_ros_driver/livox_ros_driver/livox_ros_driver.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // The MIT License (MIT) 3 | // 4 | // Copyright (c) 2019 Livox. All rights reserved. 5 | // 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | // 13 | // The above copyright notice and this permission notice shall be included in 14 | // all copies or substantial portions of the Software. 15 | // 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | // 24 | 25 | #include "include/livox_ros_driver.h" 26 | 27 | #include 28 | #include 29 | #include 30 | 31 | #include 32 | #include "lddc.h" 33 | #include "lds_hub.h" 34 | #include "lds_lidar.h" 35 | #include "lds_lvx.h" 36 | #include "livox_sdk.h" 37 | 38 | using namespace livox_ros; 39 | 40 | const int32_t kSdkVersionMajorLimit = 2; 41 | 42 | inline void SignalHandler(int signum) { 43 | printf("livox ros driver will exit\r\n"); 44 | ros::shutdown(); 45 | exit(signum); 46 | } 47 | 48 | int main(int argc, char **argv) { 49 | /** Ros related */ 50 | if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, 51 | ros::console::levels::Debug)) { 52 | ros::console::notifyLoggerLevelsChanged(); 53 | } 54 | ros::init(argc, argv, "livox_lidar_publisher"); 55 | ros::NodeHandle livox_node; 56 | 57 | ROS_INFO("Livox Ros Driver Version: %s", LIVOX_ROS_DRIVER_VERSION_STRING); 58 | signal(SIGINT, SignalHandler); 59 | /** Check sdk version */ 60 | LivoxSdkVersion _sdkversion; 61 | GetLivoxSdkVersion(&_sdkversion); 62 | if (_sdkversion.major < kSdkVersionMajorLimit) { 63 | ROS_INFO("The SDK version[%d.%d.%d] is too low", _sdkversion.major, 64 | _sdkversion.minor, _sdkversion.patch); 65 | return 0; 66 | } 67 | 68 | /** Init default system parameter */ 69 | int xfer_format = kPointCloud2Msg; 70 | int multi_topic = 0; 71 | int data_src = kSourceRawLidar; 72 | double publish_freq = 10.0; /* Hz */ 73 | int output_type = kOutputToRos; 74 | std::string frame_id = "livox_frame"; 75 | bool lidar_bag = true; 76 | bool imu_bag = false; 77 | 78 | livox_node.getParam("xfer_format", xfer_format); 79 | livox_node.getParam("multi_topic", multi_topic); 80 | livox_node.getParam("data_src", data_src); 81 | livox_node.getParam("publish_freq", publish_freq); 82 | livox_node.getParam("output_data_type", output_type); 83 | livox_node.getParam("frame_id", frame_id); 84 | livox_node.getParam("enable_lidar_bag", lidar_bag); 85 | livox_node.getParam("enable_imu_bag", imu_bag); 86 | if (publish_freq > 100.0) { 87 | publish_freq = 100.0; 88 | } else if (publish_freq < 0.1) { 89 | publish_freq = 0.1; 90 | } else { 91 | publish_freq = publish_freq; 92 | } 93 | 94 | /** Lidar data distribute control and lidar data source set */ 95 | Lddc *lddc = new Lddc(xfer_format, multi_topic, data_src, output_type, 96 | publish_freq, frame_id, lidar_bag, imu_bag); 97 | lddc->SetRosNode(&livox_node); 98 | 99 | int ret = 0; 100 | if (data_src == kSourceRawLidar) { 101 | ROS_INFO("Data Source is raw lidar."); 102 | 103 | std::string user_config_path; 104 | livox_node.getParam("user_config_path", user_config_path); 105 | ROS_INFO("Config file : %s", user_config_path.c_str()); 106 | 107 | std::string cmdline_bd_code; 108 | livox_node.getParam("cmdline_str", cmdline_bd_code); 109 | 110 | std::vector bd_code_list; 111 | ParseCommandlineInputBdCode(cmdline_bd_code.c_str(), bd_code_list); 112 | 113 | LdsLidar *read_lidar = LdsLidar::GetInstance(1000 / publish_freq); 114 | lddc->RegisterLds(static_cast(read_lidar)); 115 | ret = read_lidar->InitLdsLidar(bd_code_list, user_config_path.c_str()); 116 | if (!ret) { 117 | ROS_INFO("Init lds lidar success!"); 118 | } else { 119 | ROS_ERROR("Init lds lidar fail!"); 120 | } 121 | } else if (data_src == kSourceRawHub) { 122 | ROS_INFO("Data Source is hub."); 123 | 124 | std::string user_config_path; 125 | livox_node.getParam("user_config_path", user_config_path); 126 | ROS_INFO("Config file : %s", user_config_path.c_str()); 127 | 128 | std::string cmdline_bd_code; 129 | livox_node.getParam("cmdline_str", cmdline_bd_code); 130 | 131 | std::vector bd_code_list; 132 | ParseCommandlineInputBdCode(cmdline_bd_code.c_str(), bd_code_list); 133 | 134 | LdsHub *read_hub = LdsHub::GetInstance(1000 / publish_freq); 135 | lddc->RegisterLds(static_cast(read_hub)); 136 | ret = read_hub->InitLdsHub(bd_code_list, user_config_path.c_str()); 137 | if (!ret) { 138 | ROS_INFO("Init lds hub success!"); 139 | } else { 140 | ROS_ERROR("Init lds hub fail!"); 141 | } 142 | } else { 143 | ROS_INFO("Data Source is lvx file."); 144 | 145 | std::string cmdline_file_path; 146 | livox_node.getParam("cmdline_file_path", cmdline_file_path); 147 | 148 | do { 149 | if (!IsFilePathValid(cmdline_file_path.c_str())) { 150 | ROS_ERROR("File path invalid : %s !", cmdline_file_path.c_str()); 151 | break; 152 | } 153 | 154 | std::string rosbag_file_path; 155 | int path_end_pos = cmdline_file_path.find_last_of('.'); 156 | rosbag_file_path = cmdline_file_path.substr(0, path_end_pos); 157 | rosbag_file_path += ".bag"; 158 | 159 | LdsLvx *read_lvx = LdsLvx::GetInstance(1000 / publish_freq); 160 | lddc->RegisterLds(static_cast(read_lvx)); 161 | lddc->CreateBagFile(rosbag_file_path); 162 | int ret = read_lvx->InitLdsLvx(cmdline_file_path.c_str()); 163 | if (!ret) { 164 | ROS_INFO("Init lds lvx file success!"); 165 | } else { 166 | ROS_ERROR("Init lds lvx file fail!"); 167 | } 168 | } while (0); 169 | } 170 | 171 | ros::Time::init(); 172 | while (ros::ok()) { 173 | lddc->DistributeLidarData(); 174 | } 175 | 176 | return 0; 177 | } 178 | -------------------------------------------------------------------------------- /livox_ros_driver/livox_ros_driver/lvx_file.h: -------------------------------------------------------------------------------- 1 | // 2 | // The MIT License (MIT) 3 | // 4 | // Copyright (c) 2019 Livox. All rights reserved. 5 | // 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | // 13 | // The above copyright notice and this permission notice shall be included in 14 | // all copies or substantial portions of the Software. 15 | // 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | // 24 | #ifndef LIVOX_FILE_H_ 25 | #define LIVOX_FILE_H_ 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include "livox_sdk.h" 34 | 35 | namespace livox_ros { 36 | 37 | #define kMaxPointSize 1500 38 | #define kDefaultFrameDurationTime 50 39 | const uint32_t kMaxFrameSize = 2048 * 1024; 40 | 41 | typedef enum { 42 | kDeviceStateDisconnect = 0, 43 | kDeviceStateConnect = 1, 44 | kDeviceStateSampling = 2, 45 | } DeviceState; 46 | 47 | typedef enum { 48 | kLvxFileOk = 0, 49 | kLvxFileNotExist, 50 | kLvxFileSizeFault, 51 | kLvxFileHeaderFault, 52 | kLvxFileDeviceInfoFault, 53 | kLvxFileDataInfoFault, 54 | kLvxFileAtEnd, 55 | kLvxFileReadFail, 56 | kLvxFileFrameHeaderError, 57 | kLvxFileUndefFault 58 | } LvxFileState; 59 | 60 | typedef enum { 61 | kLvxFileV0 = 0, 62 | kLvxFileV1 = 1, 63 | kLvxFileVersionUndef = 2, 64 | } LvxFileVersion; 65 | 66 | typedef struct { 67 | uint8_t handle; 68 | DeviceState device_state; 69 | DeviceInfo info; 70 | } DeviceItem; 71 | 72 | #pragma pack(1) 73 | 74 | typedef struct { 75 | uint8_t signature[16]; 76 | uint8_t version[4]; 77 | uint32_t magic_code; 78 | } LvxFilePublicHeader; 79 | 80 | typedef struct { 81 | uint32_t frame_duration; 82 | uint8_t device_count; 83 | } LvxFilePrivateHeader; 84 | 85 | typedef struct { 86 | uint8_t lidar_broadcast_code[16]; 87 | uint8_t hub_broadcast_code[16]; 88 | uint8_t device_index; 89 | uint8_t device_type; 90 | uint8_t extrinsic_enable; 91 | float roll; 92 | float pitch; 93 | float yaw; 94 | float x; 95 | float y; 96 | float z; 97 | } LvxFileDeviceInfo; 98 | 99 | typedef struct { 100 | uint8_t device_index; 101 | uint8_t version; 102 | uint8_t port_id; 103 | uint8_t lidar_index; 104 | uint8_t rsvd; 105 | uint32_t error_code; 106 | uint8_t timestamp_type; 107 | uint8_t data_type; 108 | uint8_t timestamp[8]; 109 | uint8_t raw_point[kMaxPointSize]; 110 | uint32_t pack_size; 111 | } LvxFilePacket; 112 | 113 | typedef struct { 114 | uint64_t current_offset; 115 | uint64_t next_offset; 116 | uint64_t frame_index; 117 | } FrameHeader; 118 | 119 | typedef struct { 120 | FrameHeader header; 121 | LvxFilePacket *packet; 122 | } LvxFileFrame; 123 | 124 | typedef struct { uint8_t device_count; } LvxFilePrivateHeaderV0; 125 | 126 | typedef struct { 127 | uint8_t lidar_broadcast_code[16]; 128 | uint8_t hub_broadcast_code[16]; 129 | uint8_t device_index; 130 | uint8_t device_type; 131 | float roll; 132 | float pitch; 133 | float yaw; 134 | float x; 135 | float y; 136 | float z; 137 | } LvxFileDeviceInfoV0; 138 | 139 | typedef struct { 140 | uint8_t device_index; 141 | uint8_t version; 142 | uint8_t port_id; 143 | uint8_t lidar_index; 144 | uint8_t rsvd; 145 | uint32_t error_code; 146 | uint8_t timestamp_type; 147 | uint8_t data_type; 148 | uint8_t timestamp[8]; 149 | LivoxPoint raw_point[100]; 150 | } LvxFilePacketV0; 151 | 152 | typedef struct { 153 | uint64_t current_offset; 154 | uint64_t next_offset; 155 | uint64_t frame_index; 156 | uint64_t packet_count; 157 | } FrameHeaderV0; 158 | 159 | typedef struct { 160 | FrameHeaderV0 header; 161 | LvxFilePacketV0 *packet; 162 | } LvxFileFrameV0; 163 | 164 | typedef struct { 165 | uint32_t buffer_capacity; /* max buffer size */ 166 | uint32_t data_size; /* frame data erea size */ 167 | uint8_t *packet; /* packet data erea */ 168 | } OutPacketBuffer; 169 | 170 | #pragma pack() 171 | 172 | class LvxFileHandle { 173 | public: 174 | LvxFileHandle(); 175 | ~LvxFileHandle() = default; 176 | 177 | int Open(const char *filename, std::ios_base::openmode mode); 178 | bool Eof(); 179 | 180 | int InitLvxFile(); 181 | void InitLvxFileHeader(); 182 | void SaveFrameToLvxFile(std::list &point_packet_list_temp); 183 | void BasePointsHandle(LivoxEthPacket *data, LvxFilePacket &packet); 184 | void CloseLvxFile(); 185 | 186 | void AddDeviceInfo(LvxFileDeviceInfo &info) { 187 | device_info_list_.push_back(info); 188 | } 189 | int GetDeviceInfoListSize() { return device_info_list_.size(); } 190 | int GetDeviceCount() { return device_count_; } 191 | int GetDeviceInfo(uint8_t idx, LvxFileDeviceInfo *info); 192 | int GetFileState(void) { return state_; }; 193 | int GetPacketsOfFrame(OutPacketBuffer *PacketsOfFrame); 194 | int GetLvxFileReadProgress(); 195 | int GetFileVersion() { return file_ver_; } 196 | 197 | private: 198 | std::fstream lvx_file_; 199 | std::vector device_info_list_; 200 | uint8_t file_ver_; 201 | uint8_t device_count_; 202 | LvxFilePublicHeader public_header_; 203 | LvxFilePrivateHeader private_header_; 204 | LvxFilePrivateHeaderV0 private_header_v0_; 205 | uint32_t cur_frame_index_; 206 | uint64_t cur_offset_; 207 | uint32_t frame_duration_; 208 | uint64_t data_start_offset_; 209 | uint64_t size_; 210 | int mode_; 211 | int state_; 212 | 213 | uint64_t MiniFileSize(); 214 | uint64_t PrivateHeaderOffset(); 215 | uint64_t DataStartOffset(); 216 | uint32_t PacketNumOfFrame(); 217 | 218 | bool ReadAndCheckHeader(); 219 | bool AddAndCheckDeviceInfo(); 220 | bool PrepareDataRead(); 221 | 222 | uint64_t DataSizeOfFrame(FrameHeader &frame_header) { 223 | return (frame_header.next_offset - frame_header.current_offset - 224 | sizeof(frame_header)); 225 | } 226 | 227 | uint64_t DataSizeOfFrame(FrameHeaderV0 &frame_header_v0) { 228 | return (frame_header_v0.next_offset - frame_header_v0.current_offset - 229 | sizeof(frame_header_v0)); 230 | } 231 | }; 232 | 233 | } // namespace livox_ros 234 | #endif 235 | -------------------------------------------------------------------------------- /livox_ros_driver/msg/CustomMsg.msg: -------------------------------------------------------------------------------- 1 | # Livox publish pointcloud msg format. 2 | 3 | Header header # ROS standard message header 4 | uint64 timebase # The time of first point 5 | uint32 point_num # Total number of pointclouds 6 | uint8 lidar_id # Lidar device id number 7 | uint8[3] rsvd # Reserved use 8 | CustomPoint[] points # Pointcloud data 9 | 10 | -------------------------------------------------------------------------------- /livox_ros_driver/msg/CustomPoint.msg: -------------------------------------------------------------------------------- 1 | # Livox costom pointcloud format. 2 | 3 | uint32 offset_time # offset time relative to the base time 4 | float32 x # X axis, unit:m 5 | float32 y # Y axis, unit:m 6 | float32 z # Z axis, unit:m 7 | uint8 reflectivity # reflectivity, 0~255 8 | uint8 tag # livox tag 9 | uint8 line # laser number in lidar 10 | 11 | -------------------------------------------------------------------------------- /livox_ros_driver/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | livox_ros_driver 4 | 2.0.0 5 | The ROS device driver for Livox 3D LiDARs and Livox Hub 6 | 7 | 8 | 9 | 10 | Livox Dev Team 11 | 12 | 13 | 14 | 15 | 16 | MIT 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | Livox Dev Team 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | 53 | roscpp 54 | rospy 55 | std_msgs 56 | message_generation 57 | rosbag 58 | pcl_ros 59 | 60 | roscpp 61 | rospy 62 | std_msgs 63 | rosbag 64 | pcl_ros 65 | 66 | roscpp 67 | rospy 68 | std_msgs 69 | message_runtime 70 | rosbag 71 | pcl_ros 72 | 73 | sensor_msgs 74 | git 75 | apr 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | -------------------------------------------------------------------------------- /livox_ros_driver/timesync/timesync.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // The MIT License (MIT) 3 | // 4 | // Copyright (c) 2019 Livox. All rights reserved. 5 | // 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | // 13 | // The above copyright notice and this permission notice shall be included in 14 | // all copies or substantial portions of the Software. 15 | // 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | // 24 | 25 | #include "timesync.h" 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | namespace livox_ros { 35 | using namespace std; 36 | 37 | TimeSync::TimeSync() 38 | : exit_poll_state_(false), 39 | start_poll_state_(false), 40 | exit_poll_data_(false), 41 | start_poll_data_(false) { 42 | fsm_state_ = kOpenDev; 43 | uart_ = nullptr; 44 | comm_ = nullptr; 45 | fn_cb_ = nullptr; 46 | client_data_ = nullptr; 47 | rx_bytes_ = 0; 48 | } 49 | 50 | TimeSync::~TimeSync() { DeInitTimeSync(); } 51 | 52 | int32_t TimeSync::InitTimeSync(const TimeSyncConfig &config) { 53 | config_ = config; 54 | 55 | if (config_.dev_config.type == kCommDevUart) { 56 | uint8_t baudrate_index = config_.dev_config.config.uart.baudrate; 57 | uint8_t parity_index = config_.dev_config.config.uart.parity; 58 | 59 | if ((baudrate_index < BRUnkown) && (parity_index < ParityUnkown)) { 60 | uart_ = new UserUart(baudrate_index, parity_index); 61 | } else { 62 | printf("Uart parameter error, please check the configuration file!\n"); 63 | return -1; 64 | } 65 | } else { 66 | printf("Device type not supported, now only uart is supported!\n"); 67 | return -1; 68 | } 69 | 70 | config_.protocol_config.type = kGps; 71 | comm_ = new CommProtocol(config_.protocol_config); 72 | 73 | t_poll_state_ = 74 | std::make_shared(std::bind(&TimeSync::PollStateLoop, this)); 75 | t_poll_data_ = 76 | std::make_shared(std::bind(&TimeSync::PollDataLoop, this)); 77 | 78 | return 0; 79 | } 80 | 81 | int32_t TimeSync::DeInitTimeSync() { 82 | StopTimesync(); 83 | 84 | if (uart_) delete uart_; 85 | if (comm_) delete comm_; 86 | 87 | fn_cb_ = nullptr; 88 | client_data_ = nullptr; 89 | return 0; 90 | } 91 | 92 | void TimeSync::StopTimesync() { 93 | start_poll_state_ = false; 94 | start_poll_data_ = false; 95 | exit_poll_state_ = true; 96 | exit_poll_data_ = true; 97 | if (t_poll_state_) { 98 | t_poll_state_->join(); 99 | t_poll_state_ = nullptr; 100 | } 101 | 102 | if (t_poll_data_) { 103 | t_poll_data_->join(); 104 | t_poll_data_ = nullptr; 105 | } 106 | } 107 | 108 | void TimeSync::PollStateLoop() { 109 | while (!start_poll_state_) { 110 | /* waiting to start */ 111 | } 112 | 113 | while (!exit_poll_state_) { 114 | if (fsm_state_ == kOpenDev) { 115 | FsmOpenDev(); 116 | } else if (fsm_state_ == kPrepareDev) { 117 | FsmPrepareDev(); 118 | } else if (fsm_state_ == kCheckDevState) { 119 | FsmCheckDevState(); 120 | } 121 | std::this_thread::sleep_for(std::chrono::milliseconds(50)); 122 | } 123 | } 124 | 125 | void TimeSync::PollDataLoop() { 126 | while (!start_poll_data_) { 127 | /* waiting to start */ 128 | } 129 | 130 | while (!exit_poll_data_) { 131 | if (uart_->IsOpen()) { 132 | uint32_t get_buf_size; 133 | uint8_t *cache_buf = comm_->FetchCacheFreeSpace(&get_buf_size); 134 | if (get_buf_size) { 135 | uint32_t read_data_size; 136 | read_data_size = uart_->Read((char *)cache_buf, get_buf_size); 137 | if (read_data_size) { 138 | comm_->UpdateCacheWrIdx(read_data_size); 139 | rx_bytes_ += read_data_size; 140 | CommPacket packet; 141 | memset(&packet, 0, sizeof(packet)); 142 | while ((kParseSuccess == comm_->ParseCommStream(&packet))) { 143 | if (((fn_cb_ != nullptr) || (client_data_ != nullptr))) { 144 | if ((strstr((const char *)packet.data, "$GPRMC")) || 145 | (strstr((const char *)packet.data , "$GNRMC"))){ 146 | fn_cb_((const char *)packet.data, packet.data_len, client_data_); 147 | printf("RMC data parse success!.\n"); 148 | } 149 | } 150 | } 151 | } 152 | } 153 | } else { 154 | std::this_thread::sleep_for(std::chrono::milliseconds(50)); 155 | } 156 | } 157 | } 158 | 159 | void TimeSync::FsmTransferState(uint8_t new_state) { 160 | if (new_state < kFsmDevUndef) { 161 | fsm_state_ = new_state; 162 | } 163 | transfer_time_ = chrono::steady_clock::now(); 164 | } 165 | 166 | void TimeSync::FsmOpenDev() { 167 | if (!uart_->IsOpen()) { 168 | if (!uart_->Open(config_.dev_config.name)) { 169 | FsmTransferState(kPrepareDev); 170 | } 171 | } else { 172 | FsmTransferState(kPrepareDev); 173 | } 174 | } 175 | 176 | void TimeSync::FsmPrepareDev() { 177 | chrono::steady_clock::time_point t = chrono::steady_clock::now(); 178 | chrono::milliseconds time_gap = 179 | chrono::duration_cast(t - transfer_time_); 180 | /** delay some time when device is opened, 4s */ 181 | if (time_gap.count() > 3000) { 182 | FsmTransferState(kCheckDevState); 183 | } 184 | } 185 | 186 | void TimeSync::FsmCheckDevState() { 187 | static uint32_t last_rx_bytes = 0; 188 | static chrono::steady_clock::time_point t1 = chrono::steady_clock::now(); 189 | 190 | chrono::steady_clock::time_point t2 = chrono::steady_clock::now(); 191 | chrono::milliseconds time_gap = 192 | chrono::duration_cast(t2 - t1); 193 | 194 | if (time_gap.count() > 2000) { /* period : 2.5s */ 195 | if (last_rx_bytes == rx_bytes_) { 196 | uart_->Close(); 197 | FsmTransferState(kOpenDev); 198 | printf("Uart is disconnected, close it\n"); 199 | } 200 | last_rx_bytes = rx_bytes_; 201 | t1 = t2; 202 | } 203 | } 204 | 205 | } // namespace livox_ros 206 | -------------------------------------------------------------------------------- /livox_ros_driver/timesync/timesync.h: -------------------------------------------------------------------------------- 1 | // 2 | // The MIT License (MIT) 3 | // 4 | // Copyright (c) 2019 Livox. All rights reserved. 5 | // 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | // 13 | // The above copyright notice and this permission notice shall be included in 14 | // all copies or substantial portions of the Software. 15 | // 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | // 24 | 25 | #ifndef TIMESYNC_TIMESYNC_H_ 26 | #define TIMESYNC_TIMESYNC_H_ 27 | 28 | #include 29 | #include "comm_device.h" 30 | #include "comm_protocol.h" 31 | #include "user_uart.h" 32 | 33 | namespace livox_ros { 34 | 35 | typedef void (*FnReceiveSyncTimeCb)(const char *rmc, uint32_t rmc_length, 36 | void *client_data); 37 | 38 | enum FsmPollState { kOpenDev, kPrepareDev, kCheckDevState, kFsmDevUndef }; 39 | 40 | typedef struct { 41 | CommDevConfig dev_config; 42 | ProtocolConfig protocol_config; 43 | } TimeSyncConfig; 44 | 45 | class TimeSync { 46 | public: 47 | static TimeSync *GetInstance() { 48 | static TimeSync time_sync; 49 | 50 | return &time_sync; 51 | } 52 | 53 | int32_t InitTimeSync(const TimeSyncConfig &config); 54 | int32_t DeInitTimeSync(); 55 | void StartTimesync() { 56 | start_poll_state_ = true; 57 | start_poll_data_ = true; 58 | } 59 | 60 | int32_t SetReceiveSyncTimeCb(FnReceiveSyncTimeCb cb, void *data) { 61 | if ((cb != nullptr) || (data != nullptr)) { 62 | fn_cb_ = cb; 63 | client_data_ = data; 64 | return 0; 65 | } else { 66 | return -1; 67 | } 68 | } 69 | 70 | private: 71 | TimeSync(); 72 | ~TimeSync(); 73 | TimeSync(const TimeSync &) = delete; 74 | TimeSync &operator=(const TimeSync &) = delete; 75 | 76 | void PollStateLoop(); 77 | void PollDataLoop(); 78 | void StopTimesync(); 79 | 80 | std::shared_ptr t_poll_state_; 81 | volatile bool exit_poll_state_; 82 | volatile bool start_poll_state_; 83 | 84 | std::shared_ptr t_poll_data_; 85 | volatile bool exit_poll_data_; 86 | volatile bool start_poll_data_; 87 | 88 | TimeSyncConfig config_; 89 | UserUart *uart_; 90 | CommProtocol *comm_; 91 | volatile uint32_t rx_bytes_; 92 | 93 | FnReceiveSyncTimeCb fn_cb_; 94 | void *client_data_; 95 | 96 | volatile uint8_t fsm_state_; 97 | std::chrono::steady_clock::time_point transfer_time_; 98 | void FsmTransferState(uint8_t new_state); 99 | void FsmOpenDev(); 100 | void FsmPrepareDev(); 101 | void FsmCheckDevState(); 102 | }; 103 | 104 | } // namespace livox_ros 105 | #endif 106 | -------------------------------------------------------------------------------- /livox_ros_driver/timesync/user_uart/user_uart.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // The MIT License (MIT) 3 | // 4 | // Copyright (c) 2019 Livox. All rights reserved. 5 | // 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | // 13 | // The above copyright notice and this permission notice shall be included in 14 | // all copies or substantial portions of the Software. 15 | // 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | // 24 | 25 | #include "user_uart.h" 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | namespace livox_ros { 35 | 36 | UserUart::UserUart(uint8_t baudrate_index, uint8_t parity) 37 | : baudrate_(baudrate_index), parity_(parity) { 38 | fd_ = 0; 39 | is_open_ = false; 40 | } 41 | 42 | UserUart::~UserUart() { 43 | is_open_ = false; 44 | if (fd_ > 0) { 45 | /** first we flush the port */ 46 | tcflush(fd_, TCOFLUSH); 47 | tcflush(fd_, TCIFLUSH); 48 | 49 | close(fd_); 50 | } 51 | } 52 | 53 | int UserUart::Open(const char *filename) { 54 | fd_ = open(filename, O_RDWR | O_NOCTTY); //| O_NDELAY 55 | if (fd_ < 0) { 56 | printf("Open %s fail!\n", filename); 57 | return -1; 58 | } else { 59 | chmod(filename, S_IRWXU | S_IRWXG | S_IRWXO); /* need add here */ 60 | printf("Open %s success!\n", filename); 61 | } 62 | 63 | if (fd_ > 0) { 64 | /** set baudrate and parity,etc. */ 65 | if (Setup(baudrate_, parity_)) { 66 | return -1; 67 | } 68 | } 69 | 70 | is_open_ = true; 71 | return 0; 72 | } 73 | 74 | int UserUart::Close() { 75 | is_open_ = false; 76 | if (fd_ > 0) { 77 | /** first we flush the port */ 78 | tcflush(fd_, TCOFLUSH); 79 | tcflush(fd_, TCIFLUSH); 80 | return close(fd_); 81 | } 82 | 83 | return -1; 84 | } 85 | 86 | /** sets up the port parameters */ 87 | int UserUart::Setup(uint8_t baudrate_index, uint8_t parity) { 88 | static uint32_t baud_map[19] = { 89 | B2400, B4800, B9600, B19200, B38400, B57600, B115200, 90 | B230400, B460800, B500000, B576000, B921600, B1152000, B1500000, 91 | B2000000, B2500000, B3000000, B3500000, B4000000}; 92 | tcflag_t baudrate; 93 | struct termios options; 94 | 95 | if ((baudrate_index > BR4000000) || (parity > P_7S1)) { 96 | return -1; 97 | } 98 | 99 | /** clear old setting completely,must add here for CDC serial */ 100 | tcgetattr(fd_, &options); 101 | memset(&options, 0, sizeof(options)); 102 | tcflush(fd_, TCIOFLUSH); 103 | tcsetattr(fd_, TCSANOW, &options); 104 | usleep(10000); 105 | 106 | /** Enable the receiver and set local mode... */ 107 | options.c_cflag |= (CLOCAL | CREAD); 108 | 109 | /** Disable hardware flow */ 110 | // options.c_cflag &= ~CRTSCTS; 111 | 112 | /** Disable software flow */ 113 | // options.c_iflag &= ~(IXON | IXOFF | IXANY); 114 | 115 | // options.c_oflag &= ~OPOST; 116 | 117 | /** set boadrate */ 118 | options.c_cflag &= ~CBAUD; 119 | baudrate = baud_map[baudrate_index]; 120 | options.c_cflag |= baudrate; 121 | 122 | switch (parity) { 123 | case P_8N1: 124 | /** No parity (8N1) */ 125 | options.c_cflag &= ~PARENB; 126 | options.c_cflag &= ~CSTOPB; 127 | options.c_cflag &= ~CSIZE; 128 | options.c_cflag |= CS8; 129 | break; 130 | case P_7E1: 131 | /** Even parity (7E1) */ 132 | options.c_cflag |= PARENB; 133 | options.c_cflag &= ~PARODD; 134 | options.c_cflag &= ~CSTOPB; 135 | options.c_cflag &= ~CSIZE; 136 | options.c_cflag |= CS7; 137 | break; 138 | case P_7O1: 139 | /** Odd parity (7O1) */ 140 | options.c_cflag |= PARENB; 141 | options.c_cflag |= PARODD; 142 | options.c_cflag &= ~CSTOPB; 143 | options.c_cflag &= ~CSIZE; 144 | options.c_cflag |= CS7; 145 | break; 146 | case P_7S1: 147 | /** Space parity is setup the same as no parity (7S1) */ 148 | options.c_cflag &= ~PARENB; 149 | options.c_cflag &= ~CSTOPB; 150 | options.c_cflag &= ~CSIZE; 151 | options.c_cflag |= CS8; 152 | break; 153 | default: 154 | return -1; 155 | } 156 | 157 | /** now we setup the values in port's termios */ 158 | options.c_iflag &= ~INPCK; 159 | 160 | /** Enable non-canonical */ 161 | // options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); 162 | 163 | /** Time to wait for data */ 164 | options.c_cc[VTIME] = 1; 165 | 166 | /** Minimum number of characters to read */ 167 | options.c_cc[VMIN] = 1; 168 | 169 | /** flush the port */ 170 | tcflush(fd_, TCIOFLUSH); 171 | 172 | /** send new config to the port */ 173 | tcsetattr(fd_, TCSANOW, &options); 174 | 175 | return 0; 176 | } 177 | 178 | ssize_t UserUart::Write(const char *buffer, size_t size) { 179 | if (fd_ > 0) { 180 | return write(fd_, buffer, size); 181 | } else { 182 | return 0; 183 | } 184 | } 185 | 186 | ssize_t UserUart::Read(char *buffer, size_t size) { 187 | if (fd_ > 0) { 188 | return read(fd_, buffer, size); 189 | } else { 190 | return 0; 191 | } 192 | } 193 | 194 | } // namespace livox_ros 195 | -------------------------------------------------------------------------------- /livox_ros_driver/timesync/user_uart/user_uart.h: -------------------------------------------------------------------------------- 1 | // 2 | // The MIT License (MIT) 3 | // 4 | // Copyright (c) 2019 Livox. All rights reserved. 5 | // 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | // 13 | // The above copyright notice and this permission notice shall be included in 14 | // all copies or substantial portions of the Software. 15 | // 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | // 24 | 25 | #ifndef USER_UART_H_ 26 | #define USER_UART_H_ 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | #include 34 | 35 | namespace livox_ros { 36 | 37 | enum Parity { 38 | P_8N1, /* No parity (8N1) */ 39 | P_7E1, /* Even parity (7E1)*/ 40 | P_7O1, /* Odd parity (7O1) */ 41 | P_7S1, /* Space parity is setup the same as no parity (7S1) */ 42 | ParityUnkown 43 | }; 44 | 45 | enum BaudRate { 46 | BR2400, 47 | BR4800, 48 | BR9600, 49 | BR19200, 50 | BR38400, 51 | BR57600, 52 | BR115200, 53 | BR230400, 54 | BR460800, 55 | BR500000, 56 | BR576000, 57 | BR921600, 58 | BR1152000, 59 | BR1500000, 60 | BR2000000, 61 | BR2500000, 62 | BR3000000, 63 | BR3500000, 64 | BR4000000, 65 | BRUnkown, 66 | }; 67 | 68 | class UserUart { 69 | public: 70 | UserUart(uint8_t baudrate_index, uint8_t parity); 71 | ~UserUart(); 72 | 73 | int Setup(uint8_t baudrate_index, uint8_t parity); 74 | ssize_t Write(const char *buffer, size_t size); 75 | ssize_t Read(char *buffer, size_t size); 76 | int Close(); 77 | int Open(const char *filename); 78 | bool IsOpen() { return is_open_; }; 79 | 80 | private: 81 | int fd_; 82 | volatile bool is_open_; 83 | 84 | uint8_t baudrate_; 85 | uint8_t parity_; 86 | }; 87 | 88 | } // namespace livox_ros 89 | 90 | #endif 91 | --------------------------------------------------------------------------------