mirrors/livox_ros_driver
1
0
Fork 0
mirror of https://github.com/Livox-SDK/livox_ros_driver.git synced 2023-04-06 15:49:55 +08:00
Code Issues Packages Projects Releases Wiki Activity

feature:support for connencting horizon

Browse Source
This commit is contained in:
Livox-SDK
2019-12-19 15:44:56 +08:00
parent ea1bca22ed
commit 39ae27d450
95 changed files with 27331 additions and 2067 deletions
Download Patch File Download Diff File Expand all files Collapse all files

210
README.md
View File

@@ -2,60 +2,101 @@
livox_ros_driver is a new ros package, which is designed to gradually become the standard driver package for livox devices in the ros environment. livox_ros_driver is a new ros package, which is designed to gradually become the standard driver package for livox devices in the ros environment.
### Compile & Install Livox SDK ## Compile & Install Livox SDK
livox_ros_driver depends on Livox-SDK lib. If you have never installed Livox-SDK lib or it is out of date, you must first install Livox-SDK lib. If you have installed the latest version of Livox-SDK, skip this step and go to the next step. livox_ros_driver depends on Livox-SDK lib. If you have never installed Livox-SDK lib or it is out of date, you must first install Livox-SDK lib. If you have installed the latest version of Livox-SDK, skip this step and go to the next step.
1. Download or clone the [Livox-SDK/Livox-SDK](https://github.com/Livox-SDK/Livox-SDK/) repository on GitHub. 1. Download or clone the [Livox-SDK/Livox-SDK](https://github.com/Livox-SDK/Livox-SDK/) repository on GitHub.
2. Compile and install the Livox-SDK under the ***build*** directory following `README.md` of Livox-SDK/Livox-SDK. 2. Compile and install the Livox-SDK under the ***build*** directory following `README.md` of Livox-SDK/Livox-SDK.
### Clone livox_ros_driver ## Clone livox_ros_driver
1. Clone livox_ros_driver package for github: 1. Clone livox_ros_driver package for github :
`git clone https://github.com/Livox-SDK/livox_ros_driver.git ws_livox/src` `git clone https://github.com/Livox-SDK/livox_ros_driver.git ws_livox/src`
2. Build livox_ros_driver package: 2. Build livox_ros_driver package :
``` ```
cd ws_livox cd ws_livox
catkin_make catkin_make
``` ```
3. Package environment setup: 3. Package environment setup :
`source ./devel/setup.sh` `source ./devel/setup.sh`
### Run livox ros driver ## Run livox_ros_driver
The driver offers users a wealth of options when using different launch file. There is *bd_list* arg in each launch file. All Livox LiDAR units in your LAN will be connected automatically in default. ##### Run livox_ros_driver using launch file
e.g. The command format is :
``` `roslaunch livox_ros_driver [launch file] [param]`
roslaunch livox_ros_driver livox_lidar_rviz.launch
```
If you want to connect to the specific LiDAR uint(s) only, please add the broadcast code into command line. 1. Connect LiDAR uint(s) automatically.
e.g. ​ `roslaunch livox_ros_driver livox_lidar_rviz.launch`
``` 2. Connect to the specific LiDAR uint(s), suppose there are LiDAR(0TFDG3B006H2Z11) and LiDAR(1HDDG8M00100191) .
roslaunch livox_ros_driver livox_lidar_rviz.launch bd_list:="broadcast_code1&broadcast_code2&broadcast_code3"
```
Features of *.launch* files are listed as below: Specify the LiDAR via command line arguments :
```
roslaunch livox_ros_driver livox_lidar_rviz.launch bd_list:="0TFDG3B006H2Z11&1HDDG8M00100191"
```
​ *Specifying the LiDAR via json file is supported, detailed usage will be explained later.*
***NOTE:***
Each Livox LiDAR unit owns a unique Broadcast Code . The broadcast code consists of its serial number and an additional number (1,2, or 3). The serial number can be found on the body of the LiDAR unit (below the QR code).The Broadcast Code may be used when you want to connect to the specific LiDAR unit(s). The detailed format is shown as below :
![Broadcast Code](images/broadcast_code.png)
##### Launch file introduction
The driver offers users a wealth of options when using different launch file. The launch file directory
is "ws_livox/src/livox_ros_driver/launch". All launch files are listed as below :
| launch file | features | | launch file | features |
| ------------------------- | ------------------------------------------------------------ | | ------------------------- | ------------------------------------------------------------ |
| *livox_lidar_rviz.launch* | Connect to Livox LiDAR units<br/>Publish pointcloud2 format point cloud<br/>Automatically load rviz | | livox_lidar_rviz.launch | Connect to Livox LiDAR units<br/>Publish pointcloud2 format point cloud<br/>Automatically load rviz |
| livox_hub_rviz.launch | Connect to Livox Hub units<br/>Publish pointcloud2 format point cloud<br />Automatically load rviz | | livox_hub_rviz.launch | Connect to Livox Hub units<br/>Publish pointcloud2 format point cloud<br />Automatically load rviz |
| livox_lidar.launch | Connect to Livox LiDAR units<br />Publish pointcloud2 format point cloud | | livox_lidar.launch | Connect to Livox LiDAR units<br />Publish pointcloud2 format point cloud |
| livox_hub.launch | Connect to Livox Hub units<br />Publish pointcloud2 format point cloud | | livox_hub.launch | Connect to Livox Hub units<br />Publish pointcloud2 format point cloud |
| livox_lidar_msg.launch | Connect to Livox LiDAR units<br />Publish livox custom format point cloud | | livox_lidar_msg.launch | Connect to Livox LiDAR units<br />Publish livox custom format point cloud |
| livox_hub_msg.launch | Connect to Livox Hub units<br />Publish livox custom format point cloud | | livox_hub_msg.launch | Connect to Livox Hub units<br />Publish livox custom format point cloud |
| lvx_to_rosbag.launch | Covert lvx file to rosbag file<br />Covert lvx file directly to rosbag file |
| lvx_to_rosbag_rviz.launch | Covert lvx file to rosbag file<br />Read data from lvx file and convert it to pointcloud2, then publish using ros topic |
Livox custom msg format： ## Configure livox_ros_driver internal parameter
The livox_ros_driver internal parameters are in the launch file, they are listed as below :
| Parameter name | detail |
| -------------- | ------------------------------------------------------------ |
| publish_freq | Set the frequency of publishing pointcloud data <br/>The data type is float, it can be set to 10.0,20.0,50.0,100.0,200, etc. |
| multi_topic | All lidars share the same topic, or each lidar uses a topic independently<br/>0 -- all lidars share the same topic<br/>1 -- each lidar uses a topic independently |
| xfer_format | Set publish data format<br/>0 -- livox pointcloud2(PointXYZRTL)<br/>1 -- livox custom msg format<br/>2 -- pcl pointcloud2(pcl::PointXYZI) |
***Notes :***
Detailed xfer data format
1.1 The livox point format in pointcloud2 message :
```
float32 x # X axis, unit:m
float32 y # Y axis, unit:m
float32 z # Z axis, unit:m
float intensity # the value is reflectivity, 0~255
uint8 tag # livox tag
uint8 line # laser number in lidar
```
1.2 Livox custom message format :
``` ```
Header header # ROS standard message header Header header # ROS standard message header
@@ -65,12 +106,139 @@ uint8 lidar_id # Lidar device id number
uint8[3] rsvd # Reserved use uint8[3] rsvd # Reserved use
CustomPoint[] points # Pointcloud data CustomPoint[] points # Pointcloud data
``` ```
pointcloud format:
The Custom Point format in custom message :
``` ```
uint32 offset_time # offset time relative to the base time uint32 offset_time # offset time relative to the base time
float32 x # X axis, unit:m float32 x # X axis, unit:m
float32 y # Y axis, unit:m float32 y # Y axis, unit:m
float32 z # Z axis, unit:m float32 z # Z axis, unit:m
uint8 reflectivity # reflectivity, 0~255 uint8 reflectivity # reflectivity, 0~255
uint8 tag # livox tag
uint8 line # laser number in lidar uint8 line # laser number in lidar
``` ```
1.3 PCL pointcloud2(pcl::PointXYZI) format :
​ Please refer the pcl::PointXYZI structure in Point Cloud Library (point_types.hpp) .
## Configure LiDAR parameter
There are two json files in the directory "ws_livox/src/livox_ros_driver/launch", they are livox_hub_config.json and livox_lidar_config.json.
1. When connect with LiDAR only, we can modify LiDAR parameter in livox_lidar_config.json.
The content of the file livox_lidar_config.json is as follows :
```
{
"lidar_config": [
{
"broadcast_code": "0TFDG3B006H2Z11",
"enable_connect": true,
"enable_fan": true,
"return_mode": 0,
"coordinate": 0,
"imu_rate": 1,
"extrinsic_parameter_source": 0
}
]
}
```
If you want to config a new LiDAR by livox_lidar_config.json file. For example , if you want to config a LiDAR (broadcast code : "1HDDG8M00100191") : (1) enable connection；(2) enable fan; (3) select First single return mode; (4) use Cartesian coordinate; (5) use Cartesian coordinate; (6) set imu rate to 200; (7)enable extrinsic parameter. the content of livox_lidar_config.json file should be :
```
{
"lidar_config": [
{
"broadcast_code": "0TFDG3B006H2Z11",
"enable_connect": true,
"enable_fan": true,
"return_mode": 0,
"coordinate": 0,
"imu_rate": 1,
"extrinsic_parameter_source": 1
},
{
"broadcast_code": "1HDDG8M00100191",
"enable_connect": true,
"enable_fan": true,
"return_mode": 0,
"coordinate": 0,
"imu_rate": 1,
"extrinsic_parameter_source": 1
}
]
}
```
2. When connect with Hub, we must modify Hub or LiDAR parameter in livox_hub_config.json.
The content of the livox_hub_config.json is as follows :
```
{
"hub_config": {
"broadcast_code": "13UUG1R00400170",
"enable_connect": true,
"coordinate": 0
},
"lidar_config": [
{
"broadcast_code": "0TFDG3B006H2Z11",
"enable_fan": true,
"return_mode": 0,
"imu_rate": 1
}
]
}
```
If you want to config a new LiDAR by the livox_hub_config.json file, For example , if you want to config a LiDAR (broadcast code : "1HDDG8M00100191") : (1) enable connection；(2) enable fan; (3) select First single return mode; (4) use Cartesian coordinate; (5) use Cartesian coordinate; (6) set imu rate to 200. the content of the livox_hub_config.json file should be :
```
{
"hub_config": {
"broadcast_code": "13UUG1R00400170",
"enable_connect": true,
"coordinate": 0
},
"lidar_config": [
{
"broadcast_code": "0TFDG3B006H2Z11",
"enable_fan": true,
"return_mode": 0,
"imu_rate": 1
},
{
"broadcast_code": "1HDDG8M00100191",
"enable_fan": true,
"return_mode": 0,
"imu_rate": 1
}
]
}
```
***Notes :***
When connect with Hub, the lidar's parameter "enable_connect" and "coordinate" could only be set via hub.
## Convert the lvx data file（v1.0/v1.1） to rosbag file
Livox ros driver support lvx file to rosbag file function, using lvx_to_rosbag.launch file.
Excute the follow command :
`roslaunch livox_ros_driver lvx_to_rosbag.launch lvx_file_path:="/home/livox/test.lvx"`
Replace the path "/home/livox/test.lvx" to your local path when convert the lvx data file.

BIN
images/broadcast_code.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 120 KiB

366
livox_ros_driver/CMakeLists.txt Executable file → Normal file
View File

@@ -1,162 +1,50 @@
cmake_minimum_required(VERSION 2.8.3) # Copyright(c) 2019 livoxtech limited.
project(livox_ros_driver) cmake_minimum_required(VERSION 3.0)
set(PROJECT_LIDAR livox_lidar)
set(PROJECT_HUB livox_hub)
set(PROJECT_LIDAR_SRC ./livox_lidar.cpp)
set(PROJECT_HUB_SRC ./livox_hub.cpp)
## Compile as C++11, supported in ROS Kinetic and newer #---------------------------------------------------------------------------------------
# add_compile_options(-std=c++11) # Start livox_ros_driver project
add_definitions(-std=c++11) #---------------------------------------------------------------------------------------
include(cmake/version.cmake)
project(livox_ros_driver VERSION ${LIVOX_ROS_DRIVER_VERSION} LANGUAGES CXX)
message(STATUS "livox_ros_driver version: ${LIVOX_ROS_DRIVER_VERSION}")
#---------------------------------------------------------------------------------------
# find package and the dependecy
#---------------------------------------------------------------------------------------
find_package(Boost 1.54 REQUIRED COMPONENTS
system
thread
chrono
)
## Find catkin macros and libraries ## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages ## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS find_package(catkin REQUIRED COMPONENTS
roscpp roscpp
rospy rospy
std_msgs sensor_msgs
message_generation std_msgs
) message_generation
rosbag
)
find_package(Boost REQUIRED) ## Find pcl lib
find_package(Boost REQUIRED COMPONENTS thread)
## get pointcloud package
find_package(PCL REQUIRED) find_package(PCL REQUIRED)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a run_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder ## Generate messages in the 'msg' folder
add_message_files(FILES add_message_files(FILES
CustomPoint.msg CustomPoint.msg
CustomMsg.msg CustomMsg.msg
# Message2.msg # Message2.msg
) )
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here ## Generate added messages and services with any dependencies listed here
generate_messages(DEPENDENCIES generate_messages(DEPENDENCIES
std_msgs std_msgs
) )
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a run_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(CATKIN_DEPENDS
roscpp rospy std_msgs message_runtime
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
${catkin_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
)
## Declare a C++ library
# add_library(${PROJECT_NAME}
# src/${PROJECT_NAME}/lidar_sdk.cpp
# )
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
##SET(SRC_DIR
## ./
##)
# traverse all source directories and add include dir
##FOREACH(dir ${SRC_DIR})
## MESSAGE(STATUS"src_dir: "${dir})
## include_directories(./include/${dir})
## include_directories(./${dir})
## AUX_SOURCE_DIRECTORY(${dir} source_list)
##ENDFOREACH()
find_package(PkgConfig) find_package(PkgConfig)
pkg_check_modules(APR apr-1) pkg_check_modules(APR apr-1)
@@ -165,14 +53,32 @@ if (APR_FOUND)
message(${APR_LIBRARIES}) message(${APR_LIBRARIES})
endif (APR_FOUND) endif (APR_FOUND)
include_directories( ###################################
./ ## catkin specific configuration ##
${APR_INCLUDE_DIRS} ###################################
) ## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects als o need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also n eed
catkin_package(CATKIN_DEPENDS
roscpp rospy std_msgs message_runtime
)
## PCL library #---------------------------------------------------------------------------------------
link_directories(${PCL_LIBRARY_DIRS}) # Set default build to release
add_definitions(${PCL_DEFINITIONS}) #---------------------------------------------------------------------------------------
if(NOT CMAKE_BUILD_TYPE)
set(CMAKE_BUILD_TYPE "Release" CACHE STRING "Choose Release or Debug" FORCE)
endif()
#---------------------------------------------------------------------------------------
# Compiler config
#---------------------------------------------------------------------------------------
set(CMAKE_CXX_STANDARD 11)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)
## make sure the livox_sdk_static library is installed ## make sure the livox_sdk_static library is installed
find_library(LIVOX_SDK_LIBRARY liblivox_sdk_static.a /usr/local/lib) find_library(LIVOX_SDK_LIBRARY liblivox_sdk_static.a /usr/local/lib)
@@ -218,113 +124,83 @@ if((NOT LIVOX_SDK_LIBRARY) OR (NOT EXISTS ${LIVOX_SDK_LIBRARY}))
${CMAKE_CURRENT_SOURCE_DIR}/Livox-SDK/build/sdk_core ${CMAKE_CURRENT_SOURCE_DIR}/Livox-SDK/build/sdk_core
) )
## add lidar project here
add_executable(${PROJECT_LIDAR}_node
${PROJECT_LIDAR_SRC}
)
add_dependencies(${PROJECT_LIDAR}_node ${PROJECT_NAME}_generate_messages_cpp)
target_link_libraries(${PROJECT_LIDAR}_node
livox_sdk_static.a
${APR_LIBRARIES}
${catkin_LIBRARIES}
${Boost_LIBRARIES}
-lrt
)
## add hub project here
add_executable(${PROJECT_HUB}_node
${PROJECT_HUB_SRC}
)
add_dependencies(${PROJECT_LIDAR}_node ${PROJECT_NAME}_generate_messages_cpp)
target_link_libraries(${PROJECT_HUB}_node
livox_sdk_static.a
${APR_LIBRARIES}
${catkin_LIBRARIES}
${Boost_LIBRARIES}
-lrt
)
else() else()
message("find livox sdk library success") message("find livox sdk library success")
## add lidar project here
add_executable(${PROJECT_LIDAR}_node
${PROJECT_LIDAR_SRC}
)
add_dependencies(${PROJECT_LIDAR}_node ${PROJECT_NAME}_generate_messages_cpp)
target_link_libraries(${PROJECT_LIDAR}_node
livox_sdk_static.a
${APR_LIBRARIES}
${catkin_LIBRARIES}
${Boost_LIBRARIES}
-lrt
)
## add hub project here
add_executable(${PROJECT_HUB}_node
${PROJECT_HUB_SRC}
)
add_dependencies(${PROJECT_HUB}_node ${PROJECT_NAME}_generate_messages_cpp)
target_link_libraries(${PROJECT_HUB}_node
livox_sdk_static.a
${APR_LIBRARIES}
${catkin_LIBRARIES}
${Boost_LIBRARIES}
-lrt
)
endif() endif()
############# ## PCL library
## Install ## link_directories(${PCL_LIBRARY_DIRS})
############# add_definitions(${PCL_DEFINITIONS})
# all install targets should use catkin DESTINATION variables #---------------------------------------------------------------------------------------
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html # generate excutable and add libraries
#---------------------------------------------------------------------------------------
add_executable(${PROJECT_NAME}_node
""
)
## Mark executable scripts (Python etc.) for installation #---------------------------------------------------------------------------------------
## in contrast to setup.py, you can choose the destination # precompile macro and compile option
# install(PROGRAMS #---------------------------------------------------------------------------------------
# scripts/my_python_script target_compile_options(${PROJECT_NAME}_node
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} PRIVATE $<$<CXX_COMPILER_ID:GNU>:-Wall -Werror>
# ) )
## Mark executables and/or libraries for installation #---------------------------------------------------------------------------------------
# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node # add projects that depend on
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} #---------------------------------------------------------------------------------------
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} add_dependencies(${PROJECT_NAME}_node ${PROJECT_NAME}_generate_messages_cpp)
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark cpp header files for installation #---------------------------------------------------------------------------------------
# install(DIRECTORY include/${PROJECT_NAME}/ # source file
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} #---------------------------------------------------------------------------------------
# FILES_MATCHING PATTERN "*.h" target_sources(${PROJECT_NAME}_node
# PATTERN ".svn" EXCLUDE PRIVATE
# ) livox_ros_driver/lvx_file.cpp
livox_ros_driver/ldq.cpp
livox_ros_driver/lds.cpp
livox_ros_driver/lds_lvx.cpp
livox_ros_driver/lds_lidar.cpp
livox_ros_driver/lds_hub.cpp
livox_ros_driver/lddc.cpp
livox_ros_driver/livox_ros_driver.cpp
timesync/timesync.cpp
timesync/user_uart/user_uart.cpp
common/comm/comm_protocol.cpp
common/comm/sdk_protocol.cpp
common/comm/gps_protocol.cpp
)
## Mark other files for installation (e.g. launch and bag files, etc.) #---------------------------------------------------------------------------------------
# install(FILES # include file
# # myfile1 #---------------------------------------------------------------------------------------
# # myfile2 target_include_directories(${PROJECT_NAME}_node
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} PUBLIC
# ) ${catkin_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
${APR_INCLUDE_DIRS}
common
common/rapidjson
comon/rapdidxml
common/comm
timesync
timesync/user_uart
livox_ros_driver
)
############# #---------------------------------------------------------------------------------------
## Testing ## # link libraries
############# #---------------------------------------------------------------------------------------
target_link_libraries(${PROJECT_NAME}_node
livox_sdk_static.a
${Boost_LIBRARY}
${catkin_LIBRARIES}
${PCL_LIBRARIES}
${APR_LIBRARIES}
)
#---------------------------------------------------------------------------------------
# end of CMakeList.txt
#---------------------------------------------------------------------------------------
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_lidar_sdk.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()

16
livox_ros_driver/cmake/version.cmake Normal file
View File

@@ -0,0 +1,16 @@
#---------------------------------------------------------------------------------------
# Get livox_ros_driver version from include/livox_ros_driver.h
#---------------------------------------------------------------------------------------
file(READ "${CMAKE_CURRENT_LIST_DIR}/../livox_ros_driver/include/livox_ros_driver.h" LIVOX_ROS_DRIVER_VERSION_FILE)
string(REGEX MATCH "LIVOX_ROS_DRIVER_VER_MAJOR ([0-9]+)" _ "${LIVOX_ROS_DRIVER_VERSION_FILE}")
set(ver_major ${CMAKE_MATCH_1})
string(REGEX MATCH "LIVOX_ROS_DRIVER_VER_MINOR ([0-9]+)" _ "${LIVOX_ROS_DRIVER_VERSION_FILE}")
set(ver_minor ${CMAKE_MATCH_1})
string(REGEX MATCH "LIVOX_ROS_DRIVER_VER_PATCH ([0-9]+)" _ "${LIVOX_ROS_DRIVER_VERSION_FILE}")
set(ver_patch ${CMAKE_MATCH_1})
if (NOT DEFINED ver_major OR NOT DEFINED ver_minor OR NOT DEFINED ver_patch)
message(FATAL_ERROR "Could not extract valid version from include/livox_ros_driver.h")
endif()
set (LIVOX_ROS_DRIVER_VERSION "${ver_major}.${ver_minor}.${ver_patch}")

79
livox_ros_driver/common/FastCRC/FastCRC.h Normal file
View File

@@ -0,0 +1,79 @@
/* FastCRC library code is placed under the MIT license
* Copyright (c) 2014,2015 Frank Bosing
*
* Permission is hereby granted, free of charge, to any person obtaining
* a copy of this software and associated documentation files (the
* "Software"), to deal in the Software without restriction, including
* without limitation the rights to use, copy, modify, merge, publish,
* distribute, sublicense, and/or sell copies of the Software, and to
* permit persons to whom the Software is furnished to do so, subject to
* the following conditions:
*
* The above copyright notice and this permission notice shall be
* included in all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
* BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
* ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
// Teensy 3.0, Teensy 3.1:
// See K20P64M72SF1RM.pdf (Kinetis), Pages 638 - 641 for documentation of CRC Device
// See KINETIS_4N30D.pdf for Errata (Errata ID 2776)
//
// So, ALL HW-calculations are done as 32 bit.
//
//
//
// Thanks to:
// - Catalogue of parametrised CRC algorithms, CRC RevEng
// http://reveng.sourceforge.net/crc-catalogue/
//
// - Danjel McGougan (CRC-Table-Generator)
//
//
// modify from FastCRC library @ 2018/11/20
//
#ifndef FASTCRC_FASTCRC_H_
#define FASTCRC_FASTCRC_H_
#include <stdint.h>
// ================= 16-BIT CRC ===================
class FastCRC16 {
public:
FastCRC16(uint16_t seed);
// change function name from mcrf4xx_upd to mcrf4xx
uint16_t mcrf4xx_calc(const uint8_t *data,const uint16_t datalen); // Equivalent to _crc_ccitt_update() in crc16.h from avr_libc
private:
uint16_t seed_;
};
// ================= 32-BIT CRC ===================
class FastCRC32 {
public:
FastCRC32(uint32_t seed);
// change function name from crc32_upd to crc32
uint32_t crc32_calc(const uint8_t *data, uint16_t len); // Call for subsequent calculations with previous seed
private:
uint32_t seed_;
};
#endif // FASTCRC_FASTCRC_H_

241
livox_ros_driver/common/FastCRC/FastCRC_tables.hpp Normal file
View File

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

154
livox_ros_driver/common/FastCRC/FastCRCsw.cpp Normal file
View File

@@ -0,0 +1,154 @@
/* FastCRC library code is placed under the MIT license
* Copyright (c) 2014,2015,2016 Frank Bosing
*
* Permission is hereby granted, free of charge, to any person obtaining
* a copy of this software and associated documentation files (the
* "Software"), to deal in the Software without restriction, including
* without limitation the rights to use, copy, modify, merge, publish,
* distribute, sublicense, and/or sell copies of the Software, and to
* permit persons to whom the Software is furnished to do so, subject to
* the following conditions:
*
* The above copyright notice and this permission notice shall be
* included in all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
* BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
* ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
//
// Thanks to:
// - Catalogue of parametrised CRC algorithms, CRC RevEng
// http://reveng.sourceforge.net/crc-catalogue/
//
// - Danjel McGougan (CRC-Table-Generator)
//
//
// modify from FastCRC library @ 2018/11/20
//
#include "FastCRC.h"
#include "FastCRC_tables.hpp"
// ================= 16-BIT CRC ===================
/** Constructor
*/
FastCRC16::FastCRC16(uint16_t seed) {
seed_ = seed;
}
#define crc_n4(crc, data, table) crc ^= data; \
crc = (table[(crc & 0xff) + 0x300]) ^ \
(table[((crc >> 8) & 0xff) + 0x200]) ^ \
(table[((data >> 16) & 0xff) + 0x100]) ^ \
(table[data >> 24]);
/** MCRF4XX
* equivalent to _crc_ccitt_update() in crc16.h from avr_libc
* @param data Pointer to Data
* @param datalen Length of Data
* @return CRC value
*/
uint16_t FastCRC16::mcrf4xx_calc(const uint8_t *data, uint16_t len) {
uint16_t crc = seed_;
while (((uintptr_t)data & 3) && len) {
crc = (crc >> 8) ^ crc_table_mcrf4xx[(crc & 0xff) ^ *data++];
len--;
}
while (len >= 16) {
len -= 16;
crc_n4(crc, ((uint32_t *)data)[0], crc_table_mcrf4xx);
crc_n4(crc, ((uint32_t *)data)[1], crc_table_mcrf4xx);
crc_n4(crc, ((uint32_t *)data)[2], crc_table_mcrf4xx);
crc_n4(crc, ((uint32_t *)data)[3], crc_table_mcrf4xx);
data += 16;
}
while (len--) {
crc = (crc >> 8) ^ crc_table_mcrf4xx[(crc & 0xff) ^ *data++];
}
//seed = crc;
return crc;
}
// ================= 32-BIT CRC ===================
/** Constructor
*/
FastCRC32::FastCRC32(uint32_t seed) {
seed_ = seed;
}
#define crc_n4d(crc, data, table) crc ^= data; \
crc = (table[(crc & 0xff) + 0x300]) ^ \
(table[((crc >> 8) & 0xff) + 0x200]) ^ \
(table[((crc >> 16) & 0xff) + 0x100]) ^ \
(table[(crc >> 24) & 0xff]);
#define crcsm_n4d(crc, data, table) crc ^= data; \
crc = (crc >> 8) ^ (table[crc & 0xff]); \
crc = (crc >> 8) ^ (table[crc & 0xff]); \
crc = (crc >> 8) ^ (table[crc & 0xff]); \
crc = (crc >> 8) ^ (table[crc & 0xff]);
/** CRC32
* Alias CRC-32/ADCCP, PKZIP, Ethernet, 802.3
* @param data Pointer to Data
* @param datalen Length of Data
* @return CRC value
*/
#if CRC_BIGTABLES
#define CRC_TABLE_CRC32 crc_table_crc32_big
#else
#define CRC_TABLE_CRC32 crc_table_crc32
#endif
uint32_t FastCRC32::crc32_calc(const uint8_t *data, uint16_t len) {
uint32_t crc = seed_^0xffffffff;
while (((uintptr_t)data & 3) && len) {
crc = (crc >> 8) ^ CRC_TABLE_CRC32[(crc & 0xff) ^ *data++];
len--;
}
while (len >= 16) {
len -= 16;
#if CRC_BIGTABLES
crc_n4d(crc, ((uint32_t *)data)[0], CRC_TABLE_CRC32);
crc_n4d(crc, ((uint32_t *)data)[1], CRC_TABLE_CRC32);
crc_n4d(crc, ((uint32_t *)data)[2], CRC_TABLE_CRC32);
crc_n4d(crc, ((uint32_t *)data)[3], CRC_TABLE_CRC32);
#else
crcsm_n4d(crc, ((uint32_t *)data)[0], CRC_TABLE_CRC32);
crcsm_n4d(crc, ((uint32_t *)data)[1], CRC_TABLE_CRC32);
crcsm_n4d(crc, ((uint32_t *)data)[2], CRC_TABLE_CRC32);
crcsm_n4d(crc, ((uint32_t *)data)[3], CRC_TABLE_CRC32);
#endif
data += 16;
}
while (len--) {
crc = (crc >> 8) ^ CRC_TABLE_CRC32[(crc & 0xff) ^ *data++];
}
//seed = crc;
crc ^= 0xffffffff;
return crc;
}

21
livox_ros_driver/common/FastCRC/LICENSE.md Normal file
View File

@@ -0,0 +1,21 @@
The MIT License (MIT)
Copyright (c) 2016 Frank
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.

56
livox_ros_driver/common/FastCRC/README.md Normal file
View File

@@ -0,0 +1,56 @@
FastCRC
=======
Fast CRC Arduino library
Up to 30 times faster than crc16.h (_avr_libc)
- uses the on-chip hardware for Teensy 3.0 / 3.1 / 3.2 / 3.5 / 3.6
- uses fast table-algorithms for other chips
List of supported CRC calculations:
-
7 BIT:
CRC7
(poly=0x09 init=0x00 refin=false refout=false xorout=0x00 check=0x75)
MultiMediaCard interface
8 BIT:
SMBUS
(poly=0x07 init=0x00 refin=false refout=false xorout=0x00 check=0xf4)
MAXIM
(poly=0x31 init=0x00 refin=true refout=true xorout=0x00 check=0xa1)
16 BIT:
KERMIT (Alias CRC-16/CCITT, CRC-16/CCITT-TRUE, CRC-CCITT)
(poly=0x1021 init=0x0000 refin=true refout=true xorout=0x0000 check=0x2189
Attention: sometimes you'll find byteswapped presentation of result in other implementations)
CCITT-FALSE
(poly=0x1021 init=0xffff refin=false refout=false xorout=0x0000 check=0x29b1)
MCRF4XX
(poly=0x1021 init=0xffff refin=true refout=true xorout=0x0000 check=0x6f91)
MODBUS
(poly=0x8005 init=0xffff refin=true refout=true xorout=0x0000 check=0x4b37)
XMODEM (Alias ZMODEM, CRC-16/ACORN)
(poly=0x1021 init=0x0000 refin=false refout=false xorout=0x0000 check=0x31c3)
X25 (Alias CRC-16/IBM-SDLC, CRC-16/ISO-HDLC, CRC-B)
(poly=0x1021 init=0xffff refin=true refout=true xorout=0xffff check=0x906e)
32 BIT:
CRC32, CRC-32/ADCCP, PKZIP, ETHERNET, 802.3
(poly=0x04c11db7 init=0xffffffff refin=true refout=true xorout=0xffffffff check=0xcbf43926)
CKSUM, CRC-32/POSIX
(poly=0x04c11db7 init=0x00000000 refin=false refout=false xorout=0xffffffff check=0x765e7680)

71
livox_ros_driver/common/comm/comm_device.h Normal file
View File

@@ -0,0 +1,71 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2019 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef COMM_COMM_DEVICE_H_
#define COMM_COMM_DEVICE_H_
#include <stdint.h>
namespace livox_ros {
const uint32_t kDevNameLengthMax = 256;
/** Communication device type define */
enum CommDeviceType {
kCommDevUart,
kCommDevUsb,
kCommDevCan,
kCommDevEthernet,
kCommDevUndef
};
/** Communication device uart config */
struct CommDevUartConfig {
uint8_t baudrate;
uint8_t parity;
};
/** Communication device usb config */
struct CommDevUsbConfig {
void* data;
};
/** Communication device can config */
struct CommDevCanConfig {
void* data;
};
/** Communication device config */
typedef struct {
uint8_t type;
char name[kDevNameLengthMax];
union {
CommDevUartConfig uart;
CommDevUsbConfig usb;
CommDevCanConfig can;
} config;
} CommDevConfig;
} // namespace livox
#endif // COMM_COMM_DEVICE_H_

224
livox_ros_driver/common/comm/comm_protocol.cpp Normal file
View File

@@ -0,0 +1,224 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2019 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include "comm_protocol.h"
#include <stdio.h>
#include <string.h>
#include <iostream>
namespace livox_ros {
CommProtocol::CommProtocol(ProtocolConfig& config) : config_(config) {
ResetParser();
ResetCache();
offset_to_read_index_= 0;
packet_length_ = 0;
if (kGps == config.type) {
is_length_known = false;
protocol_ = new GpsProtocol();
} else {
is_length_known = true;
protocol_ = nullptr;
}
}
CommProtocol::~CommProtocol() {
if (protocol_) {
delete protocol_;
}
}
uint8_t *CommProtocol::FetchCacheFreeSpace(uint32_t *o_len) {
UpdateCache();
if (cache_.wr_idx < cache_.size) {
*o_len = cache_.size - cache_.wr_idx;
return &cache_.buf[cache_.wr_idx];
} else {
*o_len = 0;
return nullptr;
}
}
int32_t CommProtocol::UpdateCacheWrIdx(uint32_t used_size) {
if ((cache_.wr_idx + used_size) < cache_.size) {
cache_.wr_idx += used_size;
return 0;
} else {
return -1;
}
}
uint32_t CommProtocol::GetCacheTailSize() {
if (cache_.wr_idx < cache_.size) {
return cache_.size - cache_.wr_idx;
} else {
return 0;
}
}
uint32_t CommProtocol::GetValidDataSize() {
if (cache_.wr_idx > cache_.rd_idx) {
return cache_.wr_idx - cache_.rd_idx;
} else {
return 0;
}
}
void CommProtocol::UpdateCache(void) {
if (GetCacheTailSize() < kMoveCacheLimit) { /* move unused data to cache head */
uint32_t valid_data_size = GetValidDataSize();
if (valid_data_size) {
memmove(cache_.buf, &cache_.buf[cache_.rd_idx], valid_data_size);
cache_.rd_idx = 0;
cache_.wr_idx = valid_data_size;
} else if (cache_.rd_idx) {
cache_.rd_idx = 0;
cache_.wr_idx = 0;
}
}
}
int32_t CommProtocol::Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len, \
const CommPacket &i_packet) {
return protocol_->Pack(o_buf, o_buf_size, o_len, i_packet);
}
void CommProtocol::ResetParser() {
fsm_parse_step_ = kSearchPacketPreamble;
}
int32_t CommProtocol::ParseCommStream(CommPacket *o_pack) {
int32_t ret = kParseFail;
while ((GetValidDataSize() > protocol_->GetPreambleLen()) && \
(GetValidDataSize() > offset_to_read_index_)) {
switch (fsm_parse_step_) {
case kSearchPacketPreamble: {
FsmSearchPacketPreamble();
break;
}
case kFindPacketLength: {
FsmFindPacketLength();
break;
}
case kGetPacketData: {
ret = FsmGetPacketData(o_pack);
break;
}
default: {
FsmParserStateTransfer(kSearchPacketPreamble);
}
}
if (ret == kParseNeedMoreData) break; /* */
}
return ret;
}
uint16_t CommProtocol::GetAndUpdateSeqNum() {
/* add lock here for thread safe */
uint16_t seq = seq_num_;
seq_num_++;
return seq;
}
int32_t CommProtocol::FsmSearchPacketPreamble() {
do {
if (!protocol_->CheckPreamble(GetCacheReadPos())) {
if (!is_length_known) {
offset_to_read_index_ = 0;
packet_length_ = 0;
FsmParserStateTransfer(kFindPacketLength);
break;
} else {
packet_length_ = protocol_->GetPacketLen(GetCacheReadPos());
if ((packet_length_ < cache_.size) && \
(packet_length_ > protocol_->GetPacketWrapperLen())) { /* check the legality of length */
FsmParserStateTransfer(kGetPacketData);
break;
}
}
}
//printf("|%2x", cache_.buf[cache_.rd_idx]);
++cache_.rd_idx; /* skip one byte */
} while(0);
return 0;
}
int32_t CommProtocol::FsmFindPacketLength() {
uint32_t valid_data_size = GetValidDataSize();
uint32_t ret = protocol_->FindPacketLen(GetCacheReadPos(), valid_data_size);
if (ret == kFindLengthSuccess) {
packet_length_ = protocol_->GetPacketLen(GetCacheReadPos());
FsmParserStateTransfer(kGetPacketData);
offset_to_read_index_ = 0;
} else if (ret == kFindLengthContinue) { /* continue to find next time */
offset_to_read_index_ = valid_data_size;
} else { /* find length error */
offset_to_read_index_ = 0;
cache_.rd_idx += valid_data_size;
FsmParserStateTransfer(kSearchPacketPreamble);
printf("Continue to find length error\n");
}
return 0;
}
int32_t CommProtocol::FsmGetPacketData(CommPacket *o_pack) {
int32_t ret = kParseFail;
do {
if (GetValidDataSize() < packet_length_) {
ret = kParseNeedMoreData;
break;
}
if (protocol_->CheckPacket(GetCacheReadPos())) {
cache_.rd_idx += protocol_->GetPreambleLen();
FsmParserStateTransfer(kSearchPacketPreamble);
printf("Check packet error\n");
break;
}
if (protocol_->ParsePacket(GetCacheReadPos(), packet_length_, o_pack)) {
cache_.rd_idx += protocol_->GetPacketLen(GetCacheReadPos());
FsmParserStateTransfer(kSearchPacketPreamble);
printf("Parse packet error\n");
break;
}
cache_.rd_idx += protocol_->GetPacketLen(GetCacheReadPos());
FsmParserStateTransfer(kSearchPacketPreamble);
return kParseSuccess;
} while(0);
return ret;
}
} // namespace livox

104
livox_ros_driver/common/comm/comm_protocol.h Normal file
View File

@@ -0,0 +1,104 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2019 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef COMM_COMM_PROTOCOL_H_
#define COMM_COMM_PROTOCOL_H_
#include <stdint.h>
#include "protocol.h"
#include "sdk_protocol.h"
#include "gps_protocol.h"
namespace livox_ros {
const uint32_t kCacheSize = 8192;
const uint32_t kMoveCacheLimit = 1536;
enum FsmParseState {
kSearchPacketPreamble = 0,
kFindPacketLength = 1,
kGetPacketData = 2,
kParseStepUndef
};
/** Communication data cache define */
typedef struct {
uint8_t buf[kCacheSize];
uint32_t rd_idx;
uint32_t wr_idx;
uint32_t size;
} CommCache;
class CommProtocol {
public:
CommProtocol(ProtocolConfig& config);
~CommProtocol();
int32_t Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len, const CommPacket &i_packet);
int32_t ParseCommStream(CommPacket *o_pack);
uint8_t *FetchCacheFreeSpace(uint32_t *o_len);
int32_t UpdateCacheWrIdx(uint32_t used_size);
uint16_t GetAndUpdateSeqNum();
void ResetParser();
private:
uint32_t GetCacheTailSize();
uint32_t GetValidDataSize();
void UpdateCache(void);
uint8_t* GetCacheReadPos() { return &cache_.buf[cache_.rd_idx]; }
void ResetCache() {
cache_.wr_idx = 0;
cache_.rd_idx = 0;
cache_.size = kCacheSize;
}
ProtocolConfig config_;
Protocol* protocol_;
CommCache cache_;
uint16_t seq_num_;
bool is_length_known;
bool IsLengthKnown() { return is_length_known; }
volatile uint32_t offset_to_read_index_;
uint32_t packet_length_;
volatile uint32_t fsm_parse_step_;
int32_t FsmSearchPacketPreamble();
int32_t FsmFindPacketLength();
int32_t FsmGetPacketData(CommPacket *o_pack);
void FsmParserStateTransfer(uint32_t new_state) {
if(new_state < kParseStepUndef) {
fsm_parse_step_ = new_state;
} else {
fsm_parse_step_ = kSearchPacketPreamble;
}
}
};
} // namespace livox
#endif // COMM_COMM_PROTOCOL_H_

130
livox_ros_driver/common/comm/gps_protocol.cpp Normal file
View File

@@ -0,0 +1,130 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2019 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include "gps_protocol.h"
#include <ctype.h>
#include <stdio.h>
#include <string.h>
namespace livox_ros {
const uint8_t kGpsProtocolSof = '$';
const uint8_t kGpsProtocolEof = '*';
const uint32_t kPacketLengthLmit = 200;
const uint32_t kPreambleLen = 1;
const uint32_t kWrapperLen = 4; /** '$' + '*' + '2 checksum byte' */
GpsProtocol::GpsProtocol() {
found_length_ = 0;
}
int32_t GpsProtocol::Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len, \
const CommPacket &i_packet) {
//GpsPacket* gps_packet = (GpsPacket*)o_buf;
return 0;
}
int32_t GpsProtocol::ParsePacket(const uint8_t *i_buf, uint32_t i_len, CommPacket *o_packet) {
//GpsPacket *gps_packet = (GpsPacket *)i_buf;
if (i_len < GetPacketWrapperLen()) {
return -1; // packet length error
}
memset((void *)o_packet, 0, sizeof(CommPacket));
o_packet->protocol = kGps;
o_packet->data = (uint8_t *)i_buf;
o_packet->data_len = i_len;
return 0;
}
uint32_t GpsProtocol::GetPreambleLen() {
return kPreambleLen; /** '$' */
}
uint32_t GpsProtocol::GetPacketWrapperLen() {
return kWrapperLen; /** '$' + '*' + '2 checksum bytes' */
}
uint32_t GpsProtocol::FindPacketLen(const uint8_t *buf, uint32_t buf_length) {
uint32_t i=0;
for (; (i<buf_length) && (i<kPacketLengthLmit); i++) {
if ((buf[i] == kGpsProtocolEof) && (buf[0] == kGpsProtocolSof)) {
found_length_ = i + 1 + 2; /* num = index + 1 + two bytes checksum */
return kFindLengthSuccess;
}
}
if (i < kPacketLengthLmit) {
return kFindLengthContinue;
} else {
return kFindLengthError;
}
}
uint32_t GpsProtocol::GetPacketLen(const uint8_t *buf) {
return found_length_;
}
int32_t GpsProtocol::CheckPreamble(const uint8_t *buf) {
GpsPreamble *preamble = (GpsPreamble *)buf;
if (preamble->sof == kGpsProtocolSof) {
return 0;
} else {
return -1;
}
}
int32_t GpsProtocol::CheckPacket(const uint8_t *buf) {
uint8_t checksum = CalcGpsPacketChecksum(&buf[1], found_length_ - kWrapperLen);
uint8_t raw_checksum = AscciiToHex(&buf[found_length_ - 2]);
if (checksum == raw_checksum) {
return 0;
} else {
return -1;
}
}
uint8_t GpsProtocol::CalcGpsPacketChecksum(const uint8_t *buf, uint32_t length) {
uint8_t result = buf[0];
for (uint32_t i = 1; i < length; i++) {
result ^= buf[i];
}
return result;
}
uint8_t AscciiToHex(const uint8_t *TwoChar) {
uint8_t h = toupper(TwoChar[0]) - 0x30;
if (h > 9) h -= 7;
uint8_t l = toupper(TwoChar[1]) - 0x30;
if (l > 9) l -= 7;
return h*16 + l;
}
}// namespace livox

79
livox_ros_driver/common/comm/gps_protocol.h Normal file
View File

@@ -0,0 +1,79 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2019 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef LIVOX_GPS_PROTOCOL_H_
#define LIVOX_GPS_PROTOCOL_H_
#include <stdint.h>
#include "protocol.h"
namespace livox_ros {
#pragma pack(1)
typedef struct {
uint8_t sof;
uint8_t cmd_str[1];
} GpsPreamble;
typedef struct {
uint8_t sof;
uint8_t data[1];
} GpsPacket;
#pragma pack()
uint8_t AscciiToHex(const uint8_t *TwoChar);
class GpsProtocol : public Protocol {
public:
GpsProtocol();
~GpsProtocol() = default;
int32_t ParsePacket(const uint8_t *i_buf, uint32_t i_len, CommPacket *o_packet) override;
int32_t Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len, \
const CommPacket &i_packet) override;
uint32_t GetPreambleLen() override;
uint32_t GetPacketWrapperLen() override;
uint32_t FindPacketLen(const uint8_t *buf, uint32_t buf_length) override;
uint32_t GetPacketLen(const uint8_t *buf) override;
int32_t CheckPreamble(const uint8_t *buf) override;
int32_t CheckPacket(const uint8_t *buf) override;
private:
uint32_t found_length_;
uint8_t CalcGpsPacketChecksum(const uint8_t *buf, uint32_t length);
};
} // namespace livox
#endif // LIVOX_GPS_PROTOCOL_H_

106
livox_ros_driver/common/comm/protocol.h Normal file
View File

@@ -0,0 +1,106 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2019 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef COMM_PROTOCOL_H_
#define COMM_PROTOCOL_H_
#include <stdint.h>
namespace livox_ros {
typedef struct CommPacket CommPacket;
typedef int (*RequestPackCb)(CommPacket *packet);
typedef enum { kRequestPack, kAckPack, kMsgPack } PacketType;
typedef enum { kLidarSdk, kRsvd1, kGps, kProtocolUndef } ProtocolType;
typedef enum { kNoNeed, kNeedAck, kDelayAck } NeedAckType;
typedef enum { kParseSuccess, kParseFail, kParseNeedMoreData } ParseResult;
typedef enum { kFindLengthSuccess, kFindLengthContinue, kFindLengthError } FindLengthResult;
typedef struct CommPacket {
uint8_t packet_type;
uint8_t protocol;
uint8_t protocol_version;
uint8_t cmd_set;
uint32_t cmd_code;
uint32_t sender;
uint32_t sub_sender;
uint32_t receiver;
uint32_t sub_receiver;
uint32_t seq_num;
uint8_t *data;
uint16_t data_len;
uint32_t padding;
// RequestPackCb *ack_request_cb;
// uint32_t retry_times;
// uint32_t timeout;
} CommPacket;
/** SDK Protocol info config */
typedef struct {
uint16_t seed16;
uint16_t seed32;
} SdkProtocolConfig;
/** NAME-0183 Protocol info config for gps */
typedef struct {
void* data;
} GpsProtocolConfig;
typedef struct {
uint8_t type;
union {
SdkProtocolConfig sdk;
GpsProtocolConfig gps;
} config;
} ProtocolConfig;
class Protocol {
public:
virtual ~Protocol() = default;
virtual int32_t ParsePacket(const uint8_t *i_buf, uint32_t i_len, CommPacket *o_packet) = 0;
virtual int32_t Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len, \
const CommPacket &i_packet) = 0;
virtual uint32_t GetPreambleLen() = 0;
virtual uint32_t GetPacketWrapperLen() = 0;
virtual uint32_t FindPacketLen(const uint8_t *buf, uint32_t buf_length) = 0;
virtual uint32_t GetPacketLen(const uint8_t *buf) = 0;
virtual int32_t CheckPreamble(const uint8_t *buf) = 0;
virtual int32_t CheckPacket(const uint8_t *buf) = 0;
};
} // namespace livox
#endif // COMM_PROTOCOL_H_

132
livox_ros_driver/common/comm/sdk_protocol.cpp Normal file
View File

@@ -0,0 +1,132 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2019 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include "sdk_protocol.h"
#include <stdio.h>
#include <string.h>
namespace livox_ros {
const uint8_t kSdkProtocolSof = 0xAA;
const uint32_t kSdkPacketCrcSize = 4; // crc32
const uint32_t kSdkPacketPreambleCrcSize = 2; // crc16
SdkProtocol::SdkProtocol(uint16_t seed16, uint32_t seed32) : crc16_(seed16), crc32_(seed32) {}
int32_t SdkProtocol::Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len,\
const CommPacket &i_packet) {
SdkPacket *sdk_packet = (SdkPacket *)o_buf;
if (kLidarSdk != i_packet.protocol) {
return -1;
}
sdk_packet->sof = kSdkProtocolSof;
sdk_packet->length = i_packet.data_len + GetPacketWrapperLen();
if (sdk_packet->length > o_buf_size) {
return -1;
}
sdk_packet->version = kSdkVer0;
sdk_packet->packet_type = i_packet.packet_type;
sdk_packet->seq_num = i_packet.seq_num & 0xFFFF;
sdk_packet->preamble_crc = crc16_.mcrf4xx_calc(o_buf, GetPreambleLen() - \
kSdkPacketPreambleCrcSize);
sdk_packet->cmd_set = i_packet.cmd_set;
sdk_packet->cmd_id = i_packet.cmd_code;
memcpy(sdk_packet->data, i_packet.data, i_packet.data_len);
uint32_t crc = crc32_.crc32_calc(o_buf, sdk_packet->length - kSdkPacketCrcSize);
o_buf[sdk_packet->length - 4] = crc & 0xFF;
o_buf[sdk_packet->length - 3] = (crc >> 8) & 0xFF;
o_buf[sdk_packet->length - 2] = (crc >> 16) & 0xFF;
o_buf[sdk_packet->length - 1] = (crc >> 24) & 0xFF;
*o_len = sdk_packet->length;
return 0;
}
int32_t SdkProtocol::ParsePacket(const uint8_t *i_buf, uint32_t i_len, CommPacket *o_packet) {
SdkPacket *sdk_packet = (SdkPacket *)i_buf;
if (i_len < GetPacketWrapperLen()) {
return -1; // packet lenth error
}
memset((void *)o_packet, 0, sizeof(CommPacket));
o_packet->packet_type = sdk_packet->packet_type;
o_packet->protocol = kLidarSdk;
o_packet->protocol_version = sdk_packet->version;
o_packet->seq_num = sdk_packet->seq_num;
o_packet->cmd_set = sdk_packet->cmd_set;
o_packet->cmd_code = sdk_packet->cmd_id;
o_packet->data_len = sdk_packet->length - GetPacketWrapperLen();
o_packet->data = sdk_packet->data;
return 0;
}
uint32_t SdkProtocol::GetPreambleLen() {
return sizeof(SdkPreamble);
}
uint32_t SdkProtocol::GetPacketWrapperLen() {
return sizeof(SdkPacket) - 1 + kSdkPacketCrcSize;
}
uint32_t SdkProtocol::GetPacketLen(const uint8_t *buf) {
SdkPreamble *preamble = (SdkPreamble *)buf;
return preamble->length;
}
int32_t SdkProtocol::CheckPreamble(const uint8_t *buf) {
SdkPreamble *preamble = (SdkPreamble *)buf;
if ((preamble->sof == kSdkProtocolSof) && (crc16_.mcrf4xx_calc(buf, GetPreambleLen()) == 0)) {
return 0;
} else {
return -1;
}
}
int32_t SdkProtocol::CheckPacket(const uint8_t *buf) {
SdkPacket *sdk_packet = (SdkPacket *)buf;
uint32_t crc = ((uint32_t)(buf[sdk_packet->length - 4])) | \
(((uint32_t)(buf[sdk_packet->length - 3])) << 8) | \
(((uint32_t)(buf[sdk_packet->length - 2])) << 16) | \
(((uint32_t)(buf[sdk_packet->length - 1])) << 24);
if (crc32_.crc32_calc(buf, sdk_packet->length - kSdkPacketCrcSize) == crc) {
return 0;
} else {
return -1;
}
}
}// namespace livox

85
livox_ros_driver/common/comm/sdk_protocol.h Normal file
View File

@@ -0,0 +1,85 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2019 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef LIVOX_SDK_PROTOCOL_H_
#define LIVOX_SDK_PROTOCOL_H_
#include <stdint.h>
#include "protocol.h"
#include "FastCRC/FastCRC.h"
namespace livox_ros {
typedef enum { kSdkVerNone, kSdkVer0, kSdkVer1 } SdkVersion;
#pragma pack(1)
typedef struct {
uint8_t sof;
uint8_t version;
uint16_t length;
uint8_t packet_type;
uint16_t seq_num;
uint16_t preamble_crc;
} SdkPreamble;
typedef struct {
uint8_t sof;
uint8_t version;
uint16_t length;
uint8_t packet_type;
uint16_t seq_num;
uint16_t preamble_crc;
uint8_t cmd_set;
uint8_t cmd_id;
uint8_t data[1];
} SdkPacket;
#pragma pack()
class SdkProtocol : public Protocol {
public:
SdkProtocol(uint16_t seed16, uint32_t seed32);
~SdkProtocol() = default;
int32_t ParsePacket(const uint8_t *i_buf, uint32_t i_len, CommPacket *o_packet) override;
int32_t Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len, \
const CommPacket &i_packet) override;
uint32_t GetPreambleLen() override;
uint32_t GetPacketWrapperLen() override;
uint32_t GetPacketLen(const uint8_t *buf) override;
int32_t CheckPreamble(const uint8_t *buf) override;
int32_t CheckPacket(const uint8_t *buf) override;
private:
FastCRC16 crc16_;
FastCRC32 crc32_;
};
} // namespace livox
#endif // LIVOX_SDK_PROTOCOL_H_

284
livox_ros_driver/common/rapidjson/allocators.h Normal file
View File

@@ -0,0 +1,284 @@
// Tencent is pleased to support the open source community by making RapidJSON available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
#ifndef RAPIDJSON_ALLOCATORS_H_
#define RAPIDJSON_ALLOCATORS_H_
#include "rapidjson.h"
RAPIDJSON_NAMESPACE_BEGIN
///////////////////////////////////////////////////////////////////////////////
// Allocator
/*! \class rapidjson::Allocator
\brief Concept for allocating, resizing and freeing memory block.
Note that Malloc() and Realloc() are non-static but Free() is static.
So if an allocator need to support Free(), it needs to put its pointer in
the header of memory block.
\code
concept Allocator {
static const bool kNeedFree; //!< Whether this allocator needs to call Free().
// Allocate a memory block.
// \param size of the memory block in bytes.
// \returns pointer to the memory block.
void* Malloc(size_t size);
// Resize a memory block.
// \param originalPtr The pointer to current memory block. Null pointer is permitted.
// \param originalSize The current size in bytes. (Design issue: since some allocator may not book-keep this, explicitly pass to it can save memory.)
// \param newSize the new size in bytes.
void* Realloc(void* originalPtr, size_t originalSize, size_t newSize);
// Free a memory block.
// \param pointer to the memory block. Null pointer is permitted.
static void Free(void *ptr);
};
\endcode
*/
/*! \def RAPIDJSON_ALLOCATOR_DEFAULT_CHUNK_CAPACITY
\ingroup RAPIDJSON_CONFIG
\brief User-defined kDefaultChunkCapacity definition.
User can define this as any \c size that is a power of 2.
*/
#ifndef RAPIDJSON_ALLOCATOR_DEFAULT_CHUNK_CAPACITY
#define RAPIDJSON_ALLOCATOR_DEFAULT_CHUNK_CAPACITY (64 * 1024)
#endif
///////////////////////////////////////////////////////////////////////////////
// CrtAllocator
//! C-runtime library allocator.
/*! This class is just wrapper for standard C library memory routines.
\note implements Allocator concept
*/
class CrtAllocator {
public:
static const bool kNeedFree = true;
void* Malloc(size_t size) {
if (size) // behavior of malloc(0) is implementation defined.
return std::malloc(size);
else
return NULL; // standardize to returning NULL.
}
void* Realloc(void* originalPtr, size_t originalSize, size_t newSize) {
(void)originalSize;
if (newSize == 0) {
std::free(originalPtr);
return NULL;
}
return std::realloc(originalPtr, newSize);
}
static void Free(void *ptr) { std::free(ptr); }
};
///////////////////////////////////////////////////////////////////////////////
// MemoryPoolAllocator
//! Default memory allocator used by the parser and DOM.
/*! This allocator allocate memory blocks from pre-allocated memory chunks.
It does not free memory blocks. And Realloc() only allocate new memory.
The memory chunks are allocated by BaseAllocator, which is CrtAllocator by default.
User may also supply a buffer as the first chunk.
If the user-buffer is full then additional chunks are allocated by BaseAllocator.
The user-buffer is not deallocated by this allocator.
\tparam BaseAllocator the allocator type for allocating memory chunks. Default is CrtAllocator.
\note implements Allocator concept
*/
template <typename BaseAllocator = CrtAllocator>
class MemoryPoolAllocator {
public:
static const bool kNeedFree = false; //!< Tell users that no need to call Free() with this allocator. (concept Allocator)
//! Constructor with chunkSize.
/*! \param chunkSize The size of memory chunk. The default is kDefaultChunkSize.
\param baseAllocator The allocator for allocating memory chunks.
*/
MemoryPoolAllocator(size_t chunkSize = kDefaultChunkCapacity, BaseAllocator* baseAllocator = 0) :
chunkHead_(0), chunk_capacity_(chunkSize), userBuffer_(0), baseAllocator_(baseAllocator), ownBaseAllocator_(0)
{
}
//! Constructor with user-supplied buffer.
/*! The user buffer will be used firstly. When it is full, memory pool allocates new chunk with chunk size.
The user buffer will not be deallocated when this allocator is destructed.
\param buffer User supplied buffer.
\param size Size of the buffer in bytes. It must at least larger than sizeof(ChunkHeader).
\param chunkSize The size of memory chunk. The default is kDefaultChunkSize.
\param baseAllocator The allocator for allocating memory chunks.
*/
MemoryPoolAllocator(void *buffer, size_t size, size_t chunkSize = kDefaultChunkCapacity, BaseAllocator* baseAllocator = 0) :
chunkHead_(0), chunk_capacity_(chunkSize), userBuffer_(buffer), baseAllocator_(baseAllocator), ownBaseAllocator_(0)
{
RAPIDJSON_ASSERT(buffer != 0);
RAPIDJSON_ASSERT(size > sizeof(ChunkHeader));
chunkHead_ = reinterpret_cast<ChunkHeader*>(buffer);
chunkHead_->capacity = size - sizeof(ChunkHeader);
chunkHead_->size = 0;
chunkHead_->next = 0;
}
//! Destructor.
/*! This deallocates all memory chunks, excluding the user-supplied buffer.
*/
~MemoryPoolAllocator() {
Clear();
RAPIDJSON_DELETE(ownBaseAllocator_);
}
//! Deallocates all memory chunks, excluding the user-supplied buffer.
void Clear() {
while (chunkHead_ && chunkHead_ != userBuffer_) {
ChunkHeader* next = chunkHead_->next;
baseAllocator_->Free(chunkHead_);
chunkHead_ = next;
}
if (chunkHead_ && chunkHead_ == userBuffer_)
chunkHead_->size = 0; // Clear user buffer
}
//! Computes the total capacity of allocated memory chunks.
/*! \return total capacity in bytes.
*/
size_t Capacity() const {
size_t capacity = 0;
for (ChunkHeader* c = chunkHead_; c != 0; c = c->next)
capacity += c->capacity;
return capacity;
}
//! Computes the memory blocks allocated.
/*! \return total used bytes.
*/
size_t Size() const {
size_t size = 0;
for (ChunkHeader* c = chunkHead_; c != 0; c = c->next)
size += c->size;
return size;
}
//! Allocates a memory block. (concept Allocator)
void* Malloc(size_t size) {
if (!size)
return NULL;
size = RAPIDJSON_ALIGN(size);
if (chunkHead_ == 0 || chunkHead_->size + size > chunkHead_->capacity)
if (!AddChunk(chunk_capacity_ > size ? chunk_capacity_ : size))
return NULL;
void *buffer = reinterpret_cast<char *>(chunkHead_) + RAPIDJSON_ALIGN(sizeof(ChunkHeader)) + chunkHead_->size;
chunkHead_->size += size;
return buffer;
}
//! Resizes a memory block (concept Allocator)
void* Realloc(void* originalPtr, size_t originalSize, size_t newSize) {
if (originalPtr == 0)
return Malloc(newSize);
if (newSize == 0)
return NULL;
originalSize = RAPIDJSON_ALIGN(originalSize);
newSize = RAPIDJSON_ALIGN(newSize);
// Do not shrink if new size is smaller than original
if (originalSize >= newSize)
return originalPtr;
// Simply expand it if it is the last allocation and there is sufficient space
if (originalPtr == reinterpret_cast<char *>(chunkHead_) + RAPIDJSON_ALIGN(sizeof(ChunkHeader)) + chunkHead_->size - originalSize) {
size_t increment = static_cast<size_t>(newSize - originalSize);
if (chunkHead_->size + increment <= chunkHead_->capacity) {
chunkHead_->size += increment;
return originalPtr;
}
}
// Realloc process: allocate and copy memory, do not free original buffer.
if (void* newBuffer = Malloc(newSize)) {
if (originalSize)
std::memcpy(newBuffer, originalPtr, originalSize);
return newBuffer;
}
else
return NULL;
}
//! Frees a memory block (concept Allocator)
static void Free(void *ptr) { (void)ptr; } // Do nothing
private:
//! Copy constructor is not permitted.
MemoryPoolAllocator(const MemoryPoolAllocator& rhs) /* = delete */;
//! Copy assignment operator is not permitted.
MemoryPoolAllocator& operator=(const MemoryPoolAllocator& rhs) /* = delete */;
//! Creates a new chunk.
/*! \param capacity Capacity of the chunk in bytes.
\return true if success.
*/
bool AddChunk(size_t capacity) {
if (!baseAllocator_)
ownBaseAllocator_ = baseAllocator_ = RAPIDJSON_NEW(BaseAllocator)();
if (ChunkHeader* chunk = reinterpret_cast<ChunkHeader*>(baseAllocator_->Malloc(RAPIDJSON_ALIGN(sizeof(ChunkHeader)) + capacity))) {
chunk->capacity = capacity;
chunk->size = 0;
chunk->next = chunkHead_;
chunkHead_ = chunk;
return true;
}
else
return false;
}
static const int kDefaultChunkCapacity = RAPIDJSON_ALLOCATOR_DEFAULT_CHUNK_CAPACITY; //!< Default chunk capacity.
//! Chunk header for perpending to each chunk.
/*! Chunks are stored as a singly linked list.
*/
struct ChunkHeader {
size_t capacity; //!< Capacity of the chunk in bytes (excluding the header itself).
size_t size; //!< Current size of allocated memory in bytes.
ChunkHeader *next; //!< Next chunk in the linked list.
};
ChunkHeader *chunkHead_; //!< Head of the chunk linked-list. Only the head chunk serves allocation.
size_t chunk_capacity_; //!< The minimum capacity of chunk when they are allocated.
void *userBuffer_; //!< User supplied buffer.
BaseAllocator* baseAllocator_; //!< base allocator for allocating memory chunks.
BaseAllocator* ownBaseAllocator_; //!< base allocator created by this object.
};
RAPIDJSON_NAMESPACE_END
#endif // RAPIDJSON_ENCODINGS_H_

78
livox_ros_driver/common/rapidjson/cursorstreamwrapper.h Normal file
View File

@@ -0,0 +1,78 @@
// Tencent is pleased to support the open source community by making RapidJSON available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
#ifndef RAPIDJSON_CURSORSTREAMWRAPPER_H_
#define RAPIDJSON_CURSORSTREAMWRAPPER_H_
#include "stream.h"
#if defined(__GNUC__)
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(effc++)
#endif
#if defined(_MSC_VER) && _MSC_VER <= 1800
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(4702) // unreachable code
RAPIDJSON_DIAG_OFF(4512) // assignment operator could not be generated
#endif
RAPIDJSON_NAMESPACE_BEGIN
//! Cursor stream wrapper for counting line and column number if error exists.
/*!
\tparam InputStream Any stream that implements Stream Concept
*/
template <typename InputStream, typename Encoding = UTF8<> >
class CursorStreamWrapper : public GenericStreamWrapper<InputStream, Encoding> {
public:
typedef typename Encoding::Ch Ch;
CursorStreamWrapper(InputStream& is):
GenericStreamWrapper<InputStream, Encoding>(is), line_(1), col_(0) {}
// counting line and column number
Ch Take() {
Ch ch = this->is_.Take();
if(ch == '\n') {
line_ ++;
col_ = 0;
} else {
col_ ++;
}
return ch;
}
//! Get the error line number, if error exists.
size_t GetLine() const { return line_; }
//! Get the error column number, if error exists.
size_t GetColumn() const { return col_; }
private:
size_t line_; //!< Current Line
size_t col_; //!< Current Column
};
#if defined(_MSC_VER) && _MSC_VER <= 1800
RAPIDJSON_DIAG_POP
#endif
#if defined(__GNUC__)
RAPIDJSON_DIAG_POP
#endif
RAPIDJSON_NAMESPACE_END
#endif // RAPIDJSON_CURSORSTREAMWRAPPER_H_

2682
livox_ros_driver/common/rapidjson/document.h Normal file
View File

File diff suppressed because it is too large Load Diff

299
livox_ros_driver/common/rapidjson/encodedstream.h Normal file
View File

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

716
livox_ros_driver/common/rapidjson/encodings.h Normal file
View File

@@ -0,0 +1,716 @@
// Tencent is pleased to support the open source community by making RapidJSON available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
#ifndef RAPIDJSON_ENCODINGS_H_
#define RAPIDJSON_ENCODINGS_H_
#include "rapidjson.h"
#if defined(_MSC_VER) && !defined(__clang__)
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(4244) // conversion from 'type1' to 'type2', possible loss of data
RAPIDJSON_DIAG_OFF(4702) // unreachable code
#elif defined(__GNUC__)
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(effc++)
RAPIDJSON_DIAG_OFF(overflow)
#endif
RAPIDJSON_NAMESPACE_BEGIN
///////////////////////////////////////////////////////////////////////////////
// Encoding
/*! \class rapidjson::Encoding
\brief Concept for encoding of Unicode characters.
\code
concept Encoding {
typename Ch; //! Type of character. A "character" is actually a code unit in unicode's definition.
enum { supportUnicode = 1 }; // or 0 if not supporting unicode
//! \brief Encode a Unicode codepoint to an output stream.
//! \param os Output stream.
//! \param codepoint An unicode codepoint, ranging from 0x0 to 0x10FFFF inclusively.
template<typename OutputStream>
static void Encode(OutputStream& os, unsigned codepoint);
//! \brief Decode a Unicode codepoint from an input stream.
//! \param is Input stream.
//! \param codepoint Output of the unicode codepoint.
//! \return true if a valid codepoint can be decoded from the stream.
template <typename InputStream>
static bool Decode(InputStream& is, unsigned* codepoint);
//! \brief Validate one Unicode codepoint from an encoded stream.
//! \param is Input stream to obtain codepoint.
//! \param os Output for copying one codepoint.
//! \return true if it is valid.
//! \note This function just validating and copying the codepoint without actually decode it.
template <typename InputStream, typename OutputStream>
static bool Validate(InputStream& is, OutputStream& os);
// The following functions are deal with byte streams.
//! Take a character from input byte stream, skip BOM if exist.
template <typename InputByteStream>
static CharType TakeBOM(InputByteStream& is);
//! Take a character from input byte stream.
template <typename InputByteStream>
static Ch Take(InputByteStream& is);
//! Put BOM to output byte stream.
template <typename OutputByteStream>
static void PutBOM(OutputByteStream& os);
//! Put a character to output byte stream.
template <typename OutputByteStream>
static void Put(OutputByteStream& os, Ch c);
};
\endcode
*/
///////////////////////////////////////////////////////////////////////////////
// UTF8
//! UTF-8 encoding.
/*! http://en.wikipedia.org/wiki/UTF-8
http://tools.ietf.org/html/rfc3629
\tparam CharType Code unit for storing 8-bit UTF-8 data. Default is char.
\note implements Encoding concept
*/
template<typename CharType = char>
struct UTF8 {
typedef CharType Ch;
enum { supportUnicode = 1 };
template<typename OutputStream>
static void Encode(OutputStream& os, unsigned codepoint) {
if (codepoint <= 0x7F)
os.Put(static_cast<Ch>(codepoint & 0xFF));
else if (codepoint <= 0x7FF) {
os.Put(static_cast<Ch>(0xC0 | ((codepoint >> 6) & 0xFF)));
os.Put(static_cast<Ch>(0x80 | ((codepoint & 0x3F))));
}
else if (codepoint <= 0xFFFF) {
os.Put(static_cast<Ch>(0xE0 | ((codepoint >> 12) & 0xFF)));
os.Put(static_cast<Ch>(0x80 | ((codepoint >> 6) & 0x3F)));
os.Put(static_cast<Ch>(0x80 | (codepoint & 0x3F)));
}
else {
RAPIDJSON_ASSERT(codepoint <= 0x10FFFF);
os.Put(static_cast<Ch>(0xF0 | ((codepoint >> 18) & 0xFF)));
os.Put(static_cast<Ch>(0x80 | ((codepoint >> 12) & 0x3F)));
os.Put(static_cast<Ch>(0x80 | ((codepoint >> 6) & 0x3F)));
os.Put(static_cast<Ch>(0x80 | (codepoint & 0x3F)));
}
}
template<typename OutputStream>
static void EncodeUnsafe(OutputStream& os, unsigned codepoint) {
if (codepoint <= 0x7F)
PutUnsafe(os, static_cast<Ch>(codepoint & 0xFF));
else if (codepoint <= 0x7FF) {
PutUnsafe(os, static_cast<Ch>(0xC0 | ((codepoint >> 6) & 0xFF)));
PutUnsafe(os, static_cast<Ch>(0x80 | ((codepoint & 0x3F))));
}
else if (codepoint <= 0xFFFF) {
PutUnsafe(os, static_cast<Ch>(0xE0 | ((codepoint >> 12) & 0xFF)));
PutUnsafe(os, static_cast<Ch>(0x80 | ((codepoint >> 6) & 0x3F)));
PutUnsafe(os, static_cast<Ch>(0x80 | (codepoint & 0x3F)));
}
else {
RAPIDJSON_ASSERT(codepoint <= 0x10FFFF);
PutUnsafe(os, static_cast<Ch>(0xF0 | ((codepoint >> 18) & 0xFF)));
PutUnsafe(os, static_cast<Ch>(0x80 | ((codepoint >> 12) & 0x3F)));
PutUnsafe(os, static_cast<Ch>(0x80 | ((codepoint >> 6) & 0x3F)));
PutUnsafe(os, static_cast<Ch>(0x80 | (codepoint & 0x3F)));
}
}
template <typename InputStream>
static bool Decode(InputStream& is, unsigned* codepoint) {
#define RAPIDJSON_COPY() c = is.Take(); *codepoint = (*codepoint << 6) | (static_cast<unsigned char>(c) & 0x3Fu)
#define RAPIDJSON_TRANS(mask) result &= ((GetRange(static_cast<unsigned char>(c)) & mask) != 0)
#define RAPIDJSON_TAIL() RAPIDJSON_COPY(); RAPIDJSON_TRANS(0x70)
typename InputStream::Ch c = is.Take();
if (!(c & 0x80)) {
*codepoint = static_cast<unsigned char>(c);
return true;
}
unsigned char type = GetRange(static_cast<unsigned char>(c));
if (type >= 32) {
*codepoint = 0;
} else {
*codepoint = (0xFFu >> type) & static_cast<unsigned char>(c);
}
bool result = true;
switch (type) {
case 2: RAPIDJSON_TAIL(); return result;
case 3: RAPIDJSON_TAIL(); RAPIDJSON_TAIL(); return result;
case 4: RAPIDJSON_COPY(); RAPIDJSON_TRANS(0x50); RAPIDJSON_TAIL(); return result;
case 5: RAPIDJSON_COPY(); RAPIDJSON_TRANS(0x10); RAPIDJSON_TAIL(); RAPIDJSON_TAIL(); return result;
case 6: RAPIDJSON_TAIL(); RAPIDJSON_TAIL(); RAPIDJSON_TAIL(); return result;
case 10: RAPIDJSON_COPY(); RAPIDJSON_TRANS(0x20); RAPIDJSON_TAIL(); return result;
case 11: RAPIDJSON_COPY(); RAPIDJSON_TRANS(0x60); RAPIDJSON_TAIL(); RAPIDJSON_TAIL(); return result;
default: return false;
}
#undef RAPIDJSON_COPY
#undef RAPIDJSON_TRANS
#undef RAPIDJSON_TAIL
}
template <typename InputStream, typename OutputStream>
static bool Validate(InputStream& is, OutputStream& os) {
#define RAPIDJSON_COPY() os.Put(c = is.Take())
#define RAPIDJSON_TRANS(mask) result &= ((GetRange(static_cast<unsigned char>(c)) & mask) != 0)
#define RAPIDJSON_TAIL() RAPIDJSON_COPY(); RAPIDJSON_TRANS(0x70)
Ch c;
RAPIDJSON_COPY();
if (!(c & 0x80))
return true;
bool result = true;
switch (GetRange(static_cast<unsigned char>(c))) {
case 2: RAPIDJSON_TAIL(); return result;
case 3: RAPIDJSON_TAIL(); RAPIDJSON_TAIL(); return result;
case 4: RAPIDJSON_COPY(); RAPIDJSON_TRANS(0x50); RAPIDJSON_TAIL(); return result;
case 5: RAPIDJSON_COPY(); RAPIDJSON_TRANS(0x10); RAPIDJSON_TAIL(); RAPIDJSON_TAIL(); return result;
case 6: RAPIDJSON_TAIL(); RAPIDJSON_TAIL(); RAPIDJSON_TAIL(); return result;
case 10: RAPIDJSON_COPY(); RAPIDJSON_TRANS(0x20); RAPIDJSON_TAIL(); return result;
case 11: RAPIDJSON_COPY(); RAPIDJSON_TRANS(0x60); RAPIDJSON_TAIL(); RAPIDJSON_TAIL(); return result;
default: return false;
}
#undef RAPIDJSON_COPY
#undef RAPIDJSON_TRANS
#undef RAPIDJSON_TAIL
}
static unsigned char GetRange(unsigned char c) {
// Referring to DFA of http://bjoern.hoehrmann.de/utf-8/decoder/dfa/
// With new mapping 1 -> 0x10, 7 -> 0x20, 9 -> 0x40, such that AND operation can test multiple types.
static const unsigned char type[] = {
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x10,0x10,
0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,
0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,
0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,
8,8,2,2,2,2,2,2,2,2,2,2,2,2,2,2, 2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,
10,3,3,3,3,3,3,3,3,3,3,3,3,4,3,3, 11,6,6,6,5,8,8,8,8,8,8,8,8,8,8,8,
};
return type[c];
}
template <typename InputByteStream>
static CharType TakeBOM(InputByteStream& is) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
typename InputByteStream::Ch c = Take(is);
if (static_cast<unsigned char>(c) != 0xEFu) return c;
c = is.Take();
if (static_cast<unsigned char>(c) != 0xBBu) return c;
c = is.Take();
if (static_cast<unsigned char>(c) != 0xBFu) return c;
c = is.Take();
return c;
}
template <typename InputByteStream>
static Ch Take(InputByteStream& is) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
return static_cast<Ch>(is.Take());
}
template <typename OutputByteStream>
static void PutBOM(OutputByteStream& os) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
os.Put(static_cast<typename OutputByteStream::Ch>(0xEFu));
os.Put(static_cast<typename OutputByteStream::Ch>(0xBBu));
os.Put(static_cast<typename OutputByteStream::Ch>(0xBFu));
}
template <typename OutputByteStream>
static void Put(OutputByteStream& os, Ch c) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
os.Put(static_cast<typename OutputByteStream::Ch>(c));
}
};
///////////////////////////////////////////////////////////////////////////////
// UTF16
//! UTF-16 encoding.
/*! http://en.wikipedia.org/wiki/UTF-16
http://tools.ietf.org/html/rfc2781
\tparam CharType Type for storing 16-bit UTF-16 data. Default is wchar_t. C++11 may use char16_t instead.
\note implements Encoding concept
\note For in-memory access, no need to concern endianness. The code units and code points are represented by CPU's endianness.
For streaming, use UTF16LE and UTF16BE, which handle endianness.
*/
template<typename CharType = wchar_t>
struct UTF16 {
typedef CharType Ch;
RAPIDJSON_STATIC_ASSERT(sizeof(Ch) >= 2);
enum { supportUnicode = 1 };
template<typename OutputStream>
static void Encode(OutputStream& os, unsigned codepoint) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputStream::Ch) >= 2);
if (codepoint <= 0xFFFF) {
RAPIDJSON_ASSERT(codepoint < 0xD800 || codepoint > 0xDFFF); // Code point itself cannot be surrogate pair
os.Put(static_cast<typename OutputStream::Ch>(codepoint));
}
else {
RAPIDJSON_ASSERT(codepoint <= 0x10FFFF);
unsigned v = codepoint - 0x10000;
os.Put(static_cast<typename OutputStream::Ch>((v >> 10) | 0xD800));
os.Put(static_cast<typename OutputStream::Ch>((v & 0x3FF) | 0xDC00));
}
}
template<typename OutputStream>
static void EncodeUnsafe(OutputStream& os, unsigned codepoint) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputStream::Ch) >= 2);
if (codepoint <= 0xFFFF) {
RAPIDJSON_ASSERT(codepoint < 0xD800 || codepoint > 0xDFFF); // Code point itself cannot be surrogate pair
PutUnsafe(os, static_cast<typename OutputStream::Ch>(codepoint));
}
else {
RAPIDJSON_ASSERT(codepoint <= 0x10FFFF);
unsigned v = codepoint - 0x10000;
PutUnsafe(os, static_cast<typename OutputStream::Ch>((v >> 10) | 0xD800));
PutUnsafe(os, static_cast<typename OutputStream::Ch>((v & 0x3FF) | 0xDC00));
}
}
template <typename InputStream>
static bool Decode(InputStream& is, unsigned* codepoint) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputStream::Ch) >= 2);
typename InputStream::Ch c = is.Take();
if (c < 0xD800 || c > 0xDFFF) {
*codepoint = static_cast<unsigned>(c);
return true;
}
else if (c <= 0xDBFF) {
*codepoint = (static_cast<unsigned>(c) & 0x3FF) << 10;
c = is.Take();
*codepoint |= (static_cast<unsigned>(c) & 0x3FF);
*codepoint += 0x10000;
return c >= 0xDC00 && c <= 0xDFFF;
}
return false;
}
template <typename InputStream, typename OutputStream>
static bool Validate(InputStream& is, OutputStream& os) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputStream::Ch) >= 2);
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputStream::Ch) >= 2);
typename InputStream::Ch c;
os.Put(static_cast<typename OutputStream::Ch>(c = is.Take()));
if (c < 0xD800 || c > 0xDFFF)
return true;
else if (c <= 0xDBFF) {
os.Put(c = is.Take());
return c >= 0xDC00 && c <= 0xDFFF;
}
return false;
}
};
//! UTF-16 little endian encoding.
template<typename CharType = wchar_t>
struct UTF16LE : UTF16<CharType> {
template <typename InputByteStream>
static CharType TakeBOM(InputByteStream& is) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
CharType c = Take(is);
return static_cast<uint16_t>(c) == 0xFEFFu ? Take(is) : c;
}
template <typename InputByteStream>
static CharType Take(InputByteStream& is) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
unsigned c = static_cast<uint8_t>(is.Take());
c |= static_cast<unsigned>(static_cast<uint8_t>(is.Take())) << 8;
return static_cast<CharType>(c);
}
template <typename OutputByteStream>
static void PutBOM(OutputByteStream& os) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
os.Put(static_cast<typename OutputByteStream::Ch>(0xFFu));
os.Put(static_cast<typename OutputByteStream::Ch>(0xFEu));
}
template <typename OutputByteStream>
static void Put(OutputByteStream& os, CharType c) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
os.Put(static_cast<typename OutputByteStream::Ch>(static_cast<unsigned>(c) & 0xFFu));
os.Put(static_cast<typename OutputByteStream::Ch>((static_cast<unsigned>(c) >> 8) & 0xFFu));
}
};
//! UTF-16 big endian encoding.
template<typename CharType = wchar_t>
struct UTF16BE : UTF16<CharType> {
template <typename InputByteStream>
static CharType TakeBOM(InputByteStream& is) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
CharType c = Take(is);
return static_cast<uint16_t>(c) == 0xFEFFu ? Take(is) : c;
}
template <typename InputByteStream>
static CharType Take(InputByteStream& is) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
unsigned c = static_cast<unsigned>(static_cast<uint8_t>(is.Take())) << 8;
c |= static_cast<unsigned>(static_cast<uint8_t>(is.Take()));
return static_cast<CharType>(c);
}
template <typename OutputByteStream>
static void PutBOM(OutputByteStream& os) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
os.Put(static_cast<typename OutputByteStream::Ch>(0xFEu));
os.Put(static_cast<typename OutputByteStream::Ch>(0xFFu));
}
template <typename OutputByteStream>
static void Put(OutputByteStream& os, CharType c) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
os.Put(static_cast<typename OutputByteStream::Ch>((static_cast<unsigned>(c) >> 8) & 0xFFu));
os.Put(static_cast<typename OutputByteStream::Ch>(static_cast<unsigned>(c) & 0xFFu));
}
};
///////////////////////////////////////////////////////////////////////////////
// UTF32
//! UTF-32 encoding.
/*! http://en.wikipedia.org/wiki/UTF-32
\tparam CharType Type for storing 32-bit UTF-32 data. Default is unsigned. C++11 may use char32_t instead.
\note implements Encoding concept
\note For in-memory access, no need to concern endianness. The code units and code points are represented by CPU's endianness.
For streaming, use UTF32LE and UTF32BE, which handle endianness.
*/
template<typename CharType = unsigned>
struct UTF32 {
typedef CharType Ch;
RAPIDJSON_STATIC_ASSERT(sizeof(Ch) >= 4);
enum { supportUnicode = 1 };
template<typename OutputStream>
static void Encode(OutputStream& os, unsigned codepoint) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputStream::Ch) >= 4);
RAPIDJSON_ASSERT(codepoint <= 0x10FFFF);
os.Put(codepoint);
}
template<typename OutputStream>
static void EncodeUnsafe(OutputStream& os, unsigned codepoint) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputStream::Ch) >= 4);
RAPIDJSON_ASSERT(codepoint <= 0x10FFFF);
PutUnsafe(os, codepoint);
}
template <typename InputStream>
static bool Decode(InputStream& is, unsigned* codepoint) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputStream::Ch) >= 4);
Ch c = is.Take();
*codepoint = c;
return c <= 0x10FFFF;
}
template <typename InputStream, typename OutputStream>
static bool Validate(InputStream& is, OutputStream& os) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputStream::Ch) >= 4);
Ch c;
os.Put(c = is.Take());
return c <= 0x10FFFF;
}
};
//! UTF-32 little endian enocoding.
template<typename CharType = unsigned>
struct UTF32LE : UTF32<CharType> {
template <typename InputByteStream>
static CharType TakeBOM(InputByteStream& is) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
CharType c = Take(is);
return static_cast<uint32_t>(c) == 0x0000FEFFu ? Take(is) : c;
}
template <typename InputByteStream>
static CharType Take(InputByteStream& is) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
unsigned c = static_cast<uint8_t>(is.Take());
c |= static_cast<unsigned>(static_cast<uint8_t>(is.Take())) << 8;
c |= static_cast<unsigned>(static_cast<uint8_t>(is.Take())) << 16;
c |= static_cast<unsigned>(static_cast<uint8_t>(is.Take())) << 24;
return static_cast<CharType>(c);
}
template <typename OutputByteStream>
static void PutBOM(OutputByteStream& os) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
os.Put(static_cast<typename OutputByteStream::Ch>(0xFFu));
os.Put(static_cast<typename OutputByteStream::Ch>(0xFEu));
os.Put(static_cast<typename OutputByteStream::Ch>(0x00u));
os.Put(static_cast<typename OutputByteStream::Ch>(0x00u));
}
template <typename OutputByteStream>
static void Put(OutputByteStream& os, CharType c) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
os.Put(static_cast<typename OutputByteStream::Ch>(c & 0xFFu));
os.Put(static_cast<typename OutputByteStream::Ch>((c >> 8) & 0xFFu));
os.Put(static_cast<typename OutputByteStream::Ch>((c >> 16) & 0xFFu));
os.Put(static_cast<typename OutputByteStream::Ch>((c >> 24) & 0xFFu));
}
};
//! UTF-32 big endian encoding.
template<typename CharType = unsigned>
struct UTF32BE : UTF32<CharType> {
template <typename InputByteStream>
static CharType TakeBOM(InputByteStream& is) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
CharType c = Take(is);
return static_cast<uint32_t>(c) == 0x0000FEFFu ? Take(is) : c;
}
template <typename InputByteStream>
static CharType Take(InputByteStream& is) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
unsigned c = static_cast<unsigned>(static_cast<uint8_t>(is.Take())) << 24;
c |= static_cast<unsigned>(static_cast<uint8_t>(is.Take())) << 16;
c |= static_cast<unsigned>(static_cast<uint8_t>(is.Take())) << 8;
c |= static_cast<unsigned>(static_cast<uint8_t>(is.Take()));
return static_cast<CharType>(c);
}
template <typename OutputByteStream>
static void PutBOM(OutputByteStream& os) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
os.Put(static_cast<typename OutputByteStream::Ch>(0x00u));
os.Put(static_cast<typename OutputByteStream::Ch>(0x00u));
os.Put(static_cast<typename OutputByteStream::Ch>(0xFEu));
os.Put(static_cast<typename OutputByteStream::Ch>(0xFFu));
}
template <typename OutputByteStream>
static void Put(OutputByteStream& os, CharType c) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
os.Put(static_cast<typename OutputByteStream::Ch>((c >> 24) & 0xFFu));
os.Put(static_cast<typename OutputByteStream::Ch>((c >> 16) & 0xFFu));
os.Put(static_cast<typename OutputByteStream::Ch>((c >> 8) & 0xFFu));
os.Put(static_cast<typename OutputByteStream::Ch>(c & 0xFFu));
}
};
///////////////////////////////////////////////////////////////////////////////
// ASCII
//! ASCII encoding.
/*! http://en.wikipedia.org/wiki/ASCII
\tparam CharType Code unit for storing 7-bit ASCII data. Default is char.
\note implements Encoding concept
*/
template<typename CharType = char>
struct ASCII {
typedef CharType Ch;
enum { supportUnicode = 0 };
template<typename OutputStream>
static void Encode(OutputStream& os, unsigned codepoint) {
RAPIDJSON_ASSERT(codepoint <= 0x7F);
os.Put(static_cast<Ch>(codepoint & 0xFF));
}
template<typename OutputStream>
static void EncodeUnsafe(OutputStream& os, unsigned codepoint) {
RAPIDJSON_ASSERT(codepoint <= 0x7F);
PutUnsafe(os, static_cast<Ch>(codepoint & 0xFF));
}
template <typename InputStream>
static bool Decode(InputStream& is, unsigned* codepoint) {
uint8_t c = static_cast<uint8_t>(is.Take());
*codepoint = c;
return c <= 0X7F;
}
template <typename InputStream, typename OutputStream>
static bool Validate(InputStream& is, OutputStream& os) {
uint8_t c = static_cast<uint8_t>(is.Take());
os.Put(static_cast<typename OutputStream::Ch>(c));
return c <= 0x7F;
}
template <typename InputByteStream>
static CharType TakeBOM(InputByteStream& is) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
uint8_t c = static_cast<uint8_t>(Take(is));
return static_cast<Ch>(c);
}
template <typename InputByteStream>
static Ch Take(InputByteStream& is) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
return static_cast<Ch>(is.Take());
}
template <typename OutputByteStream>
static void PutBOM(OutputByteStream& os) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
(void)os;
}
template <typename OutputByteStream>
static void Put(OutputByteStream& os, Ch c) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
os.Put(static_cast<typename OutputByteStream::Ch>(c));
}
};
///////////////////////////////////////////////////////////////////////////////
// AutoUTF
//! Runtime-specified UTF encoding type of a stream.
enum UTFType {
kUTF8 = 0, //!< UTF-8.
kUTF16LE = 1, //!< UTF-16 little endian.
kUTF16BE = 2, //!< UTF-16 big endian.
kUTF32LE = 3, //!< UTF-32 little endian.
kUTF32BE = 4 //!< UTF-32 big endian.
};
//! Dynamically select encoding according to stream's runtime-specified UTF encoding type.
/*! \note This class can be used with AutoUTFInputtStream and AutoUTFOutputStream, which provides GetType().
*/
template<typename CharType>
struct AutoUTF {
typedef CharType Ch;
enum { supportUnicode = 1 };
#define RAPIDJSON_ENCODINGS_FUNC(x) UTF8<Ch>::x, UTF16LE<Ch>::x, UTF16BE<Ch>::x, UTF32LE<Ch>::x, UTF32BE<Ch>::x
template<typename OutputStream>
static RAPIDJSON_FORCEINLINE void Encode(OutputStream& os, unsigned codepoint) {
typedef void (*EncodeFunc)(OutputStream&, unsigned);
static const EncodeFunc f[] = { RAPIDJSON_ENCODINGS_FUNC(Encode) };
(*f[os.GetType()])(os, codepoint);
}
template<typename OutputStream>
static RAPIDJSON_FORCEINLINE void EncodeUnsafe(OutputStream& os, unsigned codepoint) {
typedef void (*EncodeFunc)(OutputStream&, unsigned);
static const EncodeFunc f[] = { RAPIDJSON_ENCODINGS_FUNC(EncodeUnsafe) };
(*f[os.GetType()])(os, codepoint);
}
template <typename InputStream>
static RAPIDJSON_FORCEINLINE bool Decode(InputStream& is, unsigned* codepoint) {
typedef bool (*DecodeFunc)(InputStream&, unsigned*);
static const DecodeFunc f[] = { RAPIDJSON_ENCODINGS_FUNC(Decode) };
return (*f[is.GetType()])(is, codepoint);
}
template <typename InputStream, typename OutputStream>
static RAPIDJSON_FORCEINLINE bool Validate(InputStream& is, OutputStream& os) {
typedef bool (*ValidateFunc)(InputStream&, OutputStream&);
static const ValidateFunc f[] = { RAPIDJSON_ENCODINGS_FUNC(Validate) };
return (*f[is.GetType()])(is, os);
}
#undef RAPIDJSON_ENCODINGS_FUNC
};
///////////////////////////////////////////////////////////////////////////////
// Transcoder
//! Encoding conversion.
template<typename SourceEncoding, typename TargetEncoding>
struct Transcoder {
//! Take one Unicode codepoint from source encoding, convert it to target encoding and put it to the output stream.
template<typename InputStream, typename OutputStream>
static RAPIDJSON_FORCEINLINE bool Transcode(InputStream& is, OutputStream& os) {
unsigned codepoint;
if (!SourceEncoding::Decode(is, &codepoint))
return false;
TargetEncoding::Encode(os, codepoint);
return true;
}
template<typename InputStream, typename OutputStream>
static RAPIDJSON_FORCEINLINE bool TranscodeUnsafe(InputStream& is, OutputStream& os) {
unsigned codepoint;
if (!SourceEncoding::Decode(is, &codepoint))
return false;
TargetEncoding::EncodeUnsafe(os, codepoint);
return true;
}
//! Validate one Unicode codepoint from an encoded stream.
template<typename InputStream, typename OutputStream>
static RAPIDJSON_FORCEINLINE bool Validate(InputStream& is, OutputStream& os) {
return Transcode(is, os); // Since source/target encoding is different, must transcode.
}
};
// Forward declaration.
template<typename Stream>
inline void PutUnsafe(Stream& stream, typename Stream::Ch c);
//! Specialization of Transcoder with same source and target encoding.
template<typename Encoding>
struct Transcoder<Encoding, Encoding> {
template<typename InputStream, typename OutputStream>
static RAPIDJSON_FORCEINLINE bool Transcode(InputStream& is, OutputStream& os) {
os.Put(is.Take()); // Just copy one code unit. This semantic is different from primary template class.
return true;
}
template<typename InputStream, typename OutputStream>
static RAPIDJSON_FORCEINLINE bool TranscodeUnsafe(InputStream& is, OutputStream& os) {
PutUnsafe(os, is.Take()); // Just copy one code unit. This semantic is different from primary template class.
return true;
}
template<typename InputStream, typename OutputStream>
static RAPIDJSON_FORCEINLINE bool Validate(InputStream& is, OutputStream& os) {
return Encoding::Validate(is, os); // source/target encoding are the same
}
};
RAPIDJSON_NAMESPACE_END
#if defined(__GNUC__) || (defined(_MSC_VER) && !defined(__clang__))
RAPIDJSON_DIAG_POP
#endif
#endif // RAPIDJSON_ENCODINGS_H_

74
livox_ros_driver/common/rapidjson/error/en.h Normal file
View File

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

161
livox_ros_driver/common/rapidjson/error/error.h Normal file
View File

@@ -0,0 +1,161 @@
// Tencent is pleased to support the open source community by making RapidJSON available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
#ifndef RAPIDJSON_ERROR_ERROR_H_
#define RAPIDJSON_ERROR_ERROR_H_
#include "../rapidjson.h"
#ifdef __clang__
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(padded)
#endif
/*! \file error.h */
/*! \defgroup RAPIDJSON_ERRORS RapidJSON error handling */
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_ERROR_CHARTYPE
//! Character type of error messages.
/*! \ingroup RAPIDJSON_ERRORS
The default character type is \c char.
On Windows, user can define this macro as \c TCHAR for supporting both
unicode/non-unicode settings.
*/
#ifndef RAPIDJSON_ERROR_CHARTYPE
#define RAPIDJSON_ERROR_CHARTYPE char
#endif
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_ERROR_STRING
//! Macro for converting string literial to \ref RAPIDJSON_ERROR_CHARTYPE[].
/*! \ingroup RAPIDJSON_ERRORS
By default this conversion macro does nothing.
On Windows, user can define this macro as \c _T(x) for supporting both
unicode/non-unicode settings.
*/
#ifndef RAPIDJSON_ERROR_STRING
#define RAPIDJSON_ERROR_STRING(x) x
#endif
RAPIDJSON_NAMESPACE_BEGIN
///////////////////////////////////////////////////////////////////////////////
// ParseErrorCode
//! Error code of parsing.
/*! \ingroup RAPIDJSON_ERRORS
\see GenericReader::Parse, GenericReader::GetParseErrorCode
*/
enum ParseErrorCode {
kParseErrorNone = 0, //!< No error.
kParseErrorDocumentEmpty, //!< The document is empty.
kParseErrorDocumentRootNotSingular, //!< The document root must not follow by other values.
kParseErrorValueInvalid, //!< Invalid value.
kParseErrorObjectMissName, //!< Missing a name for object member.
kParseErrorObjectMissColon, //!< Missing a colon after a name of object member.
kParseErrorObjectMissCommaOrCurlyBracket, //!< Missing a comma or '}' after an object member.
kParseErrorArrayMissCommaOrSquareBracket, //!< Missing a comma or ']' after an array element.
kParseErrorStringUnicodeEscapeInvalidHex, //!< Incorrect hex digit after \\u escape in string.
kParseErrorStringUnicodeSurrogateInvalid, //!< The surrogate pair in string is invalid.
kParseErrorStringEscapeInvalid, //!< Invalid escape character in string.
kParseErrorStringMissQuotationMark, //!< Missing a closing quotation mark in string.
kParseErrorStringInvalidEncoding, //!< Invalid encoding in string.
kParseErrorNumberTooBig, //!< Number too big to be stored in double.
kParseErrorNumberMissFraction, //!< Miss fraction part in number.
kParseErrorNumberMissExponent, //!< Miss exponent in number.
kParseErrorTermination, //!< Parsing was terminated.
kParseErrorUnspecificSyntaxError //!< Unspecific syntax error.
};
//! Result of parsing (wraps ParseErrorCode)
/*!
\ingroup RAPIDJSON_ERRORS
\code
Document doc;
ParseResult ok = doc.Parse("[42]");
if (!ok) {
fprintf(stderr, "JSON parse error: %s (%u)",
GetParseError_En(ok.Code()), ok.Offset());
exit(EXIT_FAILURE);
}
\endcode
\see GenericReader::Parse, GenericDocument::Parse
*/
struct ParseResult {
//!! Unspecified boolean type
typedef bool (ParseResult::*BooleanType)() const;
public:
//! Default constructor, no error.
ParseResult() : code_(kParseErrorNone), offset_(0) {}
//! Constructor to set an error.
ParseResult(ParseErrorCode code, size_t offset) : code_(code), offset_(offset) {}
//! Get the error code.
ParseErrorCode Code() const { return code_; }
//! Get the error offset, if \ref IsError(), 0 otherwise.
size_t Offset() const { return offset_; }
//! Explicit conversion to \c bool, returns \c true, iff !\ref IsError().
operator BooleanType() const { return !IsError() ? &ParseResult::IsError : NULL; }
//! Whether the result is an error.
bool IsError() const { return code_ != kParseErrorNone; }
bool operator==(const ParseResult& that) const { return code_ == that.code_; }
bool operator==(ParseErrorCode code) const { return code_ == code; }
friend bool operator==(ParseErrorCode code, const ParseResult & err) { return code == err.code_; }
bool operator!=(const ParseResult& that) const { return !(*this == that); }
bool operator!=(ParseErrorCode code) const { return !(*this == code); }
friend bool operator!=(ParseErrorCode code, const ParseResult & err) { return err != code; }
//! Reset error code.
void Clear() { Set(kParseErrorNone); }
//! Update error code and offset.
void Set(ParseErrorCode code, size_t offset = 0) { code_ = code; offset_ = offset; }
private:
ParseErrorCode code_;
size_t offset_;
};
//! Function pointer type of GetParseError().
/*! \ingroup RAPIDJSON_ERRORS
This is the prototype for \c GetParseError_X(), where \c X is a locale.
User can dynamically change locale in runtime, e.g.:
\code
GetParseErrorFunc GetParseError = GetParseError_En; // or whatever
const RAPIDJSON_ERROR_CHARTYPE* s = GetParseError(document.GetParseErrorCode());
\endcode
*/
typedef const RAPIDJSON_ERROR_CHARTYPE* (*GetParseErrorFunc)(ParseErrorCode);
RAPIDJSON_NAMESPACE_END
#ifdef __clang__
RAPIDJSON_DIAG_POP
#endif
#endif // RAPIDJSON_ERROR_ERROR_H_

99
livox_ros_driver/common/rapidjson/filereadstream.h Normal file
View File

@@ -0,0 +1,99 @@
// Tencent is pleased to support the open source community by making RapidJSON available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
#ifndef RAPIDJSON_FILEREADSTREAM_H_
#define RAPIDJSON_FILEREADSTREAM_H_
#include "stream.h"
#include <cstdio>
#ifdef __clang__
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(padded)
RAPIDJSON_DIAG_OFF(unreachable-code)
RAPIDJSON_DIAG_OFF(missing-noreturn)
#endif
RAPIDJSON_NAMESPACE_BEGIN
//! File byte stream for input using fread().
/*!
\note implements Stream concept
*/
class FileReadStream {
public:
typedef char Ch; //!< Character type (byte).
//! Constructor.
/*!
\param fp File pointer opened for read.
\param buffer user-supplied buffer.
\param bufferSize size of buffer in bytes. Must >=4 bytes.
*/
FileReadStream(std::FILE* fp, char* buffer, size_t bufferSize) : fp_(fp), buffer_(buffer), bufferSize_(bufferSize), bufferLast_(0), current_(buffer_), readCount_(0), count_(0), eof_(false) {
RAPIDJSON_ASSERT(fp_ != 0);
RAPIDJSON_ASSERT(bufferSize >= 4);
Read();
}
Ch Peek() const { return *current_; }
Ch Take() { Ch c = *current_; Read(); return c; }
size_t Tell() const { return count_ + static_cast<size_t>(current_ - buffer_); }
// Not implemented
void Put(Ch) { RAPIDJSON_ASSERT(false); }
void Flush() { RAPIDJSON_ASSERT(false); }
Ch* PutBegin() { RAPIDJSON_ASSERT(false); return 0; }
size_t PutEnd(Ch*) { RAPIDJSON_ASSERT(false); return 0; }
// For encoding detection only.
const Ch* Peek4() const {
return (current_ + 4 - !eof_ <= bufferLast_) ? current_ : 0;
}
private:
void Read() {
if (current_ < bufferLast_)
++current_;
else if (!eof_) {
count_ += readCount_;
readCount_ = std::fread(buffer_, 1, bufferSize_, fp_);
bufferLast_ = buffer_ + readCount_ - 1;
current_ = buffer_;
if (readCount_ < bufferSize_) {
buffer_[readCount_] = '\0';
++bufferLast_;
eof_ = true;
}
}
}
std::FILE* fp_;
Ch *buffer_;
size_t bufferSize_;
Ch *bufferLast_;
Ch *current_;
size_t readCount_;
size_t count_; //!< Number of characters read
bool eof_;
};
RAPIDJSON_NAMESPACE_END
#ifdef __clang__
RAPIDJSON_DIAG_POP
#endif
#endif // RAPIDJSON_FILESTREAM_H_

104
livox_ros_driver/common/rapidjson/filewritestream.h Normal file
View File

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

151
livox_ros_driver/common/rapidjson/fwd.h Normal file
View File

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

290
livox_ros_driver/common/rapidjson/internal/biginteger.h Normal file
View File

@@ -0,0 +1,290 @@
// Tencent is pleased to support the open source community by making RapidJSON available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
#ifndef RAPIDJSON_BIGINTEGER_H_
#define RAPIDJSON_BIGINTEGER_H_
#include "../rapidjson.h"
#if defined(_MSC_VER) && !__INTEL_COMPILER && defined(_M_AMD64)
#include <intrin.h> // for _umul128
#pragma intrinsic(_umul128)
#endif
RAPIDJSON_NAMESPACE_BEGIN
namespace internal {
class BigInteger {
public:
typedef uint64_t Type;
BigInteger(const BigInteger& rhs) : count_(rhs.count_) {
std::memcpy(digits_, rhs.digits_, count_ * sizeof(Type));
}
explicit BigInteger(uint64_t u) : count_(1) {
digits_[0] = u;
}
BigInteger(const char* decimals, size_t length) : count_(1) {
RAPIDJSON_ASSERT(length > 0);
digits_[0] = 0;
size_t i = 0;
const size_t kMaxDigitPerIteration = 19; // 2^64 = 18446744073709551616 > 10^19
while (length >= kMaxDigitPerIteration) {
AppendDecimal64(decimals + i, decimals + i + kMaxDigitPerIteration);
length -= kMaxDigitPerIteration;
i += kMaxDigitPerIteration;
}
if (length > 0)
AppendDecimal64(decimals + i, decimals + i + length);
}
BigInteger& operator=(const BigInteger &rhs)
{
if (this != &rhs) {
count_ = rhs.count_;
std::memcpy(digits_, rhs.digits_, count_ * sizeof(Type));
}
return *this;
}
BigInteger& operator=(uint64_t u) {
digits_[0] = u;
count_ = 1;
return *this;
}
BigInteger& operator+=(uint64_t u) {
Type backup = digits_[0];
digits_[0] += u;
for (size_t i = 0; i < count_ - 1; i++) {
if (digits_[i] >= backup)
return *this; // no carry
backup = digits_[i + 1];
digits_[i + 1] += 1;
}
// Last carry
if (digits_[count_ - 1] < backup)
PushBack(1);
return *this;
}
BigInteger& operator*=(uint64_t u) {
if (u == 0) return *this = 0;
if (u == 1) return *this;
if (*this == 1) return *this = u;
uint64_t k = 0;
for (size_t i = 0; i < count_; i++) {
uint64_t hi;
digits_[i] = MulAdd64(digits_[i], u, k, &hi);
k = hi;
}
if (k > 0)
PushBack(k);
return *this;
}
BigInteger& operator*=(uint32_t u) {
if (u == 0) return *this = 0;
if (u == 1) return *this;
if (*this == 1) return *this = u;
uint64_t k = 0;
for (size_t i = 0; i < count_; i++) {
const uint64_t c = digits_[i] >> 32;
const uint64_t d = digits_[i] & 0xFFFFFFFF;
const uint64_t uc = u * c;
const uint64_t ud = u * d;
const uint64_t p0 = ud + k;
const uint64_t p1 = uc + (p0 >> 32);
digits_[i] = (p0 & 0xFFFFFFFF) | (p1 << 32);
k = p1 >> 32;
}
if (k > 0)
PushBack(k);
return *this;
}
BigInteger& operator<<=(size_t shift) {
if (IsZero() || shift == 0) return *this;
size_t offset = shift / kTypeBit;
size_t interShift = shift % kTypeBit;
RAPIDJSON_ASSERT(count_ + offset <= kCapacity);
if (interShift == 0) {
std::memmove(digits_ + offset, digits_, count_ * sizeof(Type));
count_ += offset;
}
else {
digits_[count_] = 0;
for (size_t i = count_; i > 0; i--)
digits_[i + offset] = (digits_[i] << interShift) | (digits_[i - 1] >> (kTypeBit - interShift));
digits_[offset] = digits_[0] << interShift;
count_ += offset;
if (digits_[count_])
count_++;
}
std::memset(digits_, 0, offset * sizeof(Type));
return *this;
}
bool operator==(const BigInteger& rhs) const {
return count_ == rhs.count_ && std::memcmp(digits_, rhs.digits_, count_ * sizeof(Type)) == 0;
}
bool operator==(const Type rhs) const {
return count_ == 1 && digits_[0] == rhs;
}
BigInteger& MultiplyPow5(unsigned exp) {
static const uint32_t kPow5[12] = {
5,
5 * 5,
5 * 5 * 5,
5 * 5 * 5 * 5,
5 * 5 * 5 * 5 * 5,
5 * 5 * 5 * 5 * 5 * 5,
5 * 5 * 5 * 5 * 5 * 5 * 5,
5 * 5 * 5 * 5 * 5 * 5 * 5 * 5,
5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5,
5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5,
5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5,
5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5
};
if (exp == 0) return *this;
for (; exp >= 27; exp -= 27) *this *= RAPIDJSON_UINT64_C2(0X6765C793, 0XFA10079D); // 5^27
for (; exp >= 13; exp -= 13) *this *= static_cast<uint32_t>(1220703125u); // 5^13
if (exp > 0) *this *= kPow5[exp - 1];
return *this;
}
// Compute absolute difference of this and rhs.
// Assume this != rhs
bool Difference(const BigInteger& rhs, BigInteger* out) const {
int cmp = Compare(rhs);
RAPIDJSON_ASSERT(cmp != 0);
const BigInteger *a, *b; // Makes a > b
bool ret;
if (cmp < 0) { a = &rhs; b = this; ret = true; }
else { a = this; b = &rhs; ret = false; }
Type borrow = 0;
for (size_t i = 0; i < a->count_; i++) {
Type d = a->digits_[i] - borrow;
if (i < b->count_)
d -= b->digits_[i];
borrow = (d > a->digits_[i]) ? 1 : 0;
out->digits_[i] = d;
if (d != 0)
out->count_ = i + 1;
}
return ret;
}
int Compare(const BigInteger& rhs) const {
if (count_ != rhs.count_)
return count_ < rhs.count_ ? -1 : 1;
for (size_t i = count_; i-- > 0;)
if (digits_[i] != rhs.digits_[i])
return digits_[i] < rhs.digits_[i] ? -1 : 1;
return 0;
}
size_t GetCount() const { return count_; }
Type GetDigit(size_t index) const { RAPIDJSON_ASSERT(index < count_); return digits_[index]; }
bool IsZero() const { return count_ == 1 && digits_[0] == 0; }
private:
void AppendDecimal64(const char* begin, const char* end) {
uint64_t u = ParseUint64(begin, end);
if (IsZero())
*this = u;
else {
unsigned exp = static_cast<unsigned>(end - begin);
(MultiplyPow5(exp) <<= exp) += u; // *this = *this * 10^exp + u
}
}
void PushBack(Type digit) {
RAPIDJSON_ASSERT(count_ < kCapacity);
digits_[count_++] = digit;
}
static uint64_t ParseUint64(const char* begin, const char* end) {
uint64_t r = 0;
for (const char* p = begin; p != end; ++p) {
RAPIDJSON_ASSERT(*p >= '0' && *p <= '9');
r = r * 10u + static_cast<unsigned>(*p - '0');
}
return r;
}
// Assume a * b + k < 2^128
static uint64_t MulAdd64(uint64_t a, uint64_t b, uint64_t k, uint64_t* outHigh) {
#if defined(_MSC_VER) && defined(_M_AMD64)
uint64_t low = _umul128(a, b, outHigh) + k;
if (low < k)
(*outHigh)++;
return low;
#elif (__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 6)) && defined(__x86_64__)
__extension__ typedef unsigned __int128 uint128;
uint128 p = static_cast<uint128>(a) * static_cast<uint128>(b);
p += k;
*outHigh = static_cast<uint64_t>(p >> 64);
return static_cast<uint64_t>(p);
#else
const uint64_t a0 = a & 0xFFFFFFFF, a1 = a >> 32, b0 = b & 0xFFFFFFFF, b1 = b >> 32;
uint64_t x0 = a0 * b0, x1 = a0 * b1, x2 = a1 * b0, x3 = a1 * b1;
x1 += (x0 >> 32); // can't give carry
x1 += x2;
if (x1 < x2)
x3 += (static_cast<uint64_t>(1) << 32);
uint64_t lo = (x1 << 32) + (x0 & 0xFFFFFFFF);
uint64_t hi = x3 + (x1 >> 32);
lo += k;
if (lo < k)
hi++;
*outHigh = hi;
return lo;
#endif
}
static const size_t kBitCount = 3328; // 64bit * 54 > 10^1000
static const size_t kCapacity = kBitCount / sizeof(Type);
static const size_t kTypeBit = sizeof(Type) * 8;
Type digits_[kCapacity];
size_t count_;
};
} // namespace internal
RAPIDJSON_NAMESPACE_END
#endif // RAPIDJSON_BIGINTEGER_H_

72
livox_ros_driver/common/rapidjson/internal/clzll.h Normal file
View File

@@ -0,0 +1,72 @@
// Tencent is pleased to support the open source community by making RapidJSON available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
#ifndef RAPIDJSON_CLZLL_H_
#define RAPIDJSON_CLZLL_H_
#include "../rapidjson.h"
#if defined(_MSC_VER)
#include <intrin.h>
#if defined(_WIN64)
#pragma intrinsic(_BitScanReverse64)
#else
#pragma intrinsic(_BitScanReverse)
#endif
#endif
RAPIDJSON_NAMESPACE_BEGIN
namespace internal {
#if (defined(__GNUC__) && __GNUC__ >= 4) || RAPIDJSON_HAS_BUILTIN(__builtin_clzll)
#define RAPIDJSON_CLZLL __builtin_clzll
#else
inline uint32_t clzll(uint64_t x) {
// Passing 0 to __builtin_clzll is UB in GCC and results in an
// infinite loop in the software implementation.
RAPIDJSON_ASSERT(x != 0);
#if defined(_MSC_VER)
unsigned long r = 0;
#if defined(_WIN64)
_BitScanReverse64(&r, x);
#else
// Scan the high 32 bits.
if (_BitScanReverse(&r, static_cast<uint32_t>(x >> 32)))
return 63 - (r + 32);
// Scan the low 32 bits.
_BitScanReverse(&r, static_cast<uint32_t>(x & 0xFFFFFFFF));
#endif // _WIN64
return 63 - r;
#else
uint32_t r;
while (!(x & (static_cast<uint64_t>(1) << 63))) {
x <<= 1;
++r;
}
return r;
#endif // _MSC_VER
}
#define RAPIDJSON_CLZLL RAPIDJSON_NAMESPACE::internal::clzll
#endif // (defined(__GNUC__) && __GNUC__ >= 4) || RAPIDJSON_HAS_BUILTIN(__builtin_clzll)
} // namespace internal
RAPIDJSON_NAMESPACE_END
#endif // RAPIDJSON_CLZLL_H_

257
livox_ros_driver/common/rapidjson/internal/diyfp.h Normal file
View File

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

245
livox_ros_driver/common/rapidjson/internal/dtoa.h Normal file
View File

@@ -0,0 +1,245 @@
// Tencent is pleased to support the open source community by making RapidJSON available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
// This is a C++ header-only implementation of Grisu2 algorithm from the publication:
// Loitsch, Florian. "Printing floating-point numbers quickly and accurately with
// integers." ACM Sigplan Notices 45.6 (2010): 233-243.
#ifndef RAPIDJSON_DTOA_
#define RAPIDJSON_DTOA_
#include "itoa.h" // GetDigitsLut()
#include "diyfp.h"
#include "ieee754.h"
RAPIDJSON_NAMESPACE_BEGIN
namespace internal {
#ifdef __GNUC__
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(effc++)
RAPIDJSON_DIAG_OFF(array-bounds) // some gcc versions generate wrong warnings https://gcc.gnu.org/bugzilla/show_bug.cgi?id=59124
#endif
inline void GrisuRound(char* buffer, int len, uint64_t delta, uint64_t rest, uint64_t ten_kappa, uint64_t wp_w) {
while (rest < wp_w && delta - rest >= ten_kappa &&
(rest + ten_kappa < wp_w || /// closer
wp_w - rest > rest + ten_kappa - wp_w)) {
buffer[len - 1]--;
rest += ten_kappa;
}
}
inline int CountDecimalDigit32(uint32_t n) {
// Simple pure C++ implementation was faster than __builtin_clz version in this situation.
if (n < 10) return 1;
if (n < 100) return 2;
if (n < 1000) return 3;
if (n < 10000) return 4;
if (n < 100000) return 5;
if (n < 1000000) return 6;
if (n < 10000000) return 7;
if (n < 100000000) return 8;
// Will not reach 10 digits in DigitGen()
//if (n < 1000000000) return 9;
//return 10;
return 9;
}
inline void DigitGen(const DiyFp& W, const DiyFp& Mp, uint64_t delta, char* buffer, int* len, int* K) {
static const uint32_t kPow10[] = { 1, 10, 100, 1000, 10000, 100000, 1000000, 10000000, 100000000, 1000000000 };
const DiyFp one(uint64_t(1) << -Mp.e, Mp.e);
const DiyFp wp_w = Mp - W;
uint32_t p1 = static_cast<uint32_t>(Mp.f >> -one.e);
uint64_t p2 = Mp.f & (one.f - 1);
int kappa = CountDecimalDigit32(p1); // kappa in [0, 9]
*len = 0;
while (kappa > 0) {
uint32_t d = 0;
switch (kappa) {
case 9: d = p1 / 100000000; p1 %= 100000000; break;
case 8: d = p1 / 10000000; p1 %= 10000000; break;
case 7: d = p1 / 1000000; p1 %= 1000000; break;
case 6: d = p1 / 100000; p1 %= 100000; break;
case 5: d = p1 / 10000; p1 %= 10000; break;
case 4: d = p1 / 1000; p1 %= 1000; break;
case 3: d = p1 / 100; p1 %= 100; break;
case 2: d = p1 / 10; p1 %= 10; break;
case 1: d = p1; p1 = 0; break;
default:;
}
if (d || *len)
buffer[(*len)++] = static_cast<char>('0' + static_cast<char>(d));
kappa--;
uint64_t tmp = (static_cast<uint64_t>(p1) << -one.e) + p2;
if (tmp <= delta) {
*K += kappa;
GrisuRound(buffer, *len, delta, tmp, static_cast<uint64_t>(kPow10[kappa]) << -one.e, wp_w.f);
return;
}
}
// kappa = 0
for (;;) {
p2 *= 10;
delta *= 10;
char d = static_cast<char>(p2 >> -one.e);
if (d || *len)
buffer[(*len)++] = static_cast<char>('0' + d);
p2 &= one.f - 1;
kappa--;
if (p2 < delta) {
*K += kappa;
int index = -kappa;
GrisuRound(buffer, *len, delta, p2, one.f, wp_w.f * (index < 9 ? kPow10[index] : 0));
return;
}
}
}
inline void Grisu2(double value, char* buffer, int* length, int* K) {
const DiyFp v(value);
DiyFp w_m, w_p;
v.NormalizedBoundaries(&w_m, &w_p);
const DiyFp c_mk = GetCachedPower(w_p.e, K);
const DiyFp W = v.Normalize() * c_mk;
DiyFp Wp = w_p * c_mk;
DiyFp Wm = w_m * c_mk;
Wm.f++;
Wp.f--;
DigitGen(W, Wp, Wp.f - Wm.f, buffer, length, K);
}
inline char* WriteExponent(int K, char* buffer) {
if (K < 0) {
*buffer++ = '-';
K = -K;
}
if (K >= 100) {
*buffer++ = static_cast<char>('0' + static_cast<char>(K / 100));
K %= 100;
const char* d = GetDigitsLut() + K * 2;
*buffer++ = d[0];
*buffer++ = d[1];
}
else if (K >= 10) {
const char* d = GetDigitsLut() + K * 2;
*buffer++ = d[0];
*buffer++ = d[1];
}
else
*buffer++ = static_cast<char>('0' + static_cast<char>(K));
return buffer;
}
inline char* Prettify(char* buffer, int length, int k, int maxDecimalPlaces) {
const int kk = length + k; // 10^(kk-1) <= v < 10^kk
if (0 <= k && kk <= 21) {
// 1234e7 -> 12340000000
for (int i = length; i < kk; i++)
buffer[i] = '0';
buffer[kk] = '.';
buffer[kk + 1] = '0';
return &buffer[kk + 2];
}
else if (0 < kk && kk <= 21) {
// 1234e-2 -> 12.34
std::memmove(&buffer[kk + 1], &buffer[kk], static_cast<size_t>(length - kk));
buffer[kk] = '.';
if (0 > k + maxDecimalPlaces) {
// When maxDecimalPlaces = 2, 1.2345 -> 1.23, 1.102 -> 1.1
// Remove extra trailing zeros (at least one) after truncation.
for (int i = kk + maxDecimalPlaces; i > kk + 1; i--)
if (buffer[i] != '0')
return &buffer[i + 1];
return &buffer[kk + 2]; // Reserve one zero
}
else
return &buffer[length + 1];
}
else if (-6 < kk && kk <= 0) {
// 1234e-6 -> 0.001234
const int offset = 2 - kk;
std::memmove(&buffer[offset], &buffer[0], static_cast<size_t>(length));
buffer[0] = '0';
buffer[1] = '.';
for (int i = 2; i < offset; i++)
buffer[i] = '0';
if (length - kk > maxDecimalPlaces) {
// When maxDecimalPlaces = 2, 0.123 -> 0.12, 0.102 -> 0.1
// Remove extra trailing zeros (at least one) after truncation.
for (int i = maxDecimalPlaces + 1; i > 2; i--)
if (buffer[i] != '0')
return &buffer[i + 1];
return &buffer[3]; // Reserve one zero
}
else
return &buffer[length + offset];
}
else if (kk < -maxDecimalPlaces) {
// Truncate to zero
buffer[0] = '0';
buffer[1] = '.';
buffer[2] = '0';
return &buffer[3];
}
else if (length == 1) {
// 1e30
buffer[1] = 'e';
return WriteExponent(kk - 1, &buffer[2]);
}
else {
// 1234e30 -> 1.234e33
std::memmove(&buffer[2], &buffer[1], static_cast<size_t>(length - 1));
buffer[1] = '.';
buffer[length + 1] = 'e';
return WriteExponent(kk - 1, &buffer[0 + length + 2]);
}
}
inline char* dtoa(double value, char* buffer, int maxDecimalPlaces = 324) {
RAPIDJSON_ASSERT(maxDecimalPlaces >= 1);
Double d(value);
if (d.IsZero()) {
if (d.Sign())
*buffer++ = '-'; // -0.0, Issue #289
buffer[0] = '0';
buffer[1] = '.';
buffer[2] = '0';
return &buffer[3];
}
else {
if (value < 0) {
*buffer++ = '-';
value = -value;
}
int length, K;
Grisu2(value, buffer, &length, &K);
return Prettify(buffer, length, K, maxDecimalPlaces);
}
}
#ifdef __GNUC__
RAPIDJSON_DIAG_POP
#endif
} // namespace internal
RAPIDJSON_NAMESPACE_END
#endif // RAPIDJSON_DTOA_

78
livox_ros_driver/common/rapidjson/internal/ieee754.h Normal file
View File

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

308
livox_ros_driver/common/rapidjson/internal/itoa.h Normal file
View File

@@ -0,0 +1,308 @@
// Tencent is pleased to support the open source community by making RapidJSON available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
#ifndef RAPIDJSON_ITOA_
#define RAPIDJSON_ITOA_
#include "../rapidjson.h"
RAPIDJSON_NAMESPACE_BEGIN
namespace internal {
inline const char* GetDigitsLut() {
static const char cDigitsLut[200] = {
'0','0','0','1','0','2','0','3','0','4','0','5','0','6','0','7','0','8','0','9',
'1','0','1','1','1','2','1','3','1','4','1','5','1','6','1','7','1','8','1','9',
'2','0','2','1','2','2','2','3','2','4','2','5','2','6','2','7','2','8','2','9',
'3','0','3','1','3','2','3','3','3','4','3','5','3','6','3','7','3','8','3','9',
'4','0','4','1','4','2','4','3','4','4','4','5','4','6','4','7','4','8','4','9',
'5','0','5','1','5','2','5','3','5','4','5','5','5','6','5','7','5','8','5','9',
'6','0','6','1','6','2','6','3','6','4','6','5','6','6','6','7','6','8','6','9',
'7','0','7','1','7','2','7','3','7','4','7','5','7','6','7','7','7','8','7','9',
'8','0','8','1','8','2','8','3','8','4','8','5','8','6','8','7','8','8','8','9',
'9','0','9','1','9','2','9','3','9','4','9','5','9','6','9','7','9','8','9','9'
};
return cDigitsLut;
}
inline char* u32toa(uint32_t value, char* buffer) {
RAPIDJSON_ASSERT(buffer != 0);
const char* cDigitsLut = GetDigitsLut();
if (value < 10000) {
const uint32_t d1 = (value / 100) << 1;
const uint32_t d2 = (value % 100) << 1;
if (value >= 1000)
*buffer++ = cDigitsLut[d1];
if (value >= 100)
*buffer++ = cDigitsLut[d1 + 1];
if (value >= 10)
*buffer++ = cDigitsLut[d2];
*buffer++ = cDigitsLut[d2 + 1];
}
else if (value < 100000000) {
// value = bbbbcccc
const uint32_t b = value / 10000;
const uint32_t c = value % 10000;
const uint32_t d1 = (b / 100) << 1;
const uint32_t d2 = (b % 100) << 1;
const uint32_t d3 = (c / 100) << 1;
const uint32_t d4 = (c % 100) << 1;
if (value >= 10000000)
*buffer++ = cDigitsLut[d1];
if (value >= 1000000)
*buffer++ = cDigitsLut[d1 + 1];
if (value >= 100000)
*buffer++ = cDigitsLut[d2];
*buffer++ = cDigitsLut[d2 + 1];
*buffer++ = cDigitsLut[d3];
*buffer++ = cDigitsLut[d3 + 1];
*buffer++ = cDigitsLut[d4];
*buffer++ = cDigitsLut[d4 + 1];
}
else {
// value = aabbbbcccc in decimal
const uint32_t a = value / 100000000; // 1 to 42
value %= 100000000;
if (a >= 10) {
const unsigned i = a << 1;
*buffer++ = cDigitsLut[i];
*buffer++ = cDigitsLut[i + 1];
}
else
*buffer++ = static_cast<char>('0' + static_cast<char>(a));
const uint32_t b = value / 10000; // 0 to 9999
const uint32_t c = value % 10000; // 0 to 9999
const uint32_t d1 = (b / 100) << 1;
const uint32_t d2 = (b % 100) << 1;
const uint32_t d3 = (c / 100) << 1;
const uint32_t d4 = (c % 100) << 1;
*buffer++ = cDigitsLut[d1];
*buffer++ = cDigitsLut[d1 + 1];
*buffer++ = cDigitsLut[d2];
*buffer++ = cDigitsLut[d2 + 1];
*buffer++ = cDigitsLut[d3];
*buffer++ = cDigitsLut[d3 + 1];
*buffer++ = cDigitsLut[d4];
*buffer++ = cDigitsLut[d4 + 1];
}
return buffer;
}
inline char* i32toa(int32_t value, char* buffer) {
RAPIDJSON_ASSERT(buffer != 0);
uint32_t u = static_cast<uint32_t>(value);
if (value < 0) {
*buffer++ = '-';
u = ~u + 1;
}
return u32toa(u, buffer);
}
inline char* u64toa(uint64_t value, char* buffer) {
RAPIDJSON_ASSERT(buffer != 0);
const char* cDigitsLut = GetDigitsLut();
const uint64_t kTen8 = 100000000;
const uint64_t kTen9 = kTen8 * 10;
const uint64_t kTen10 = kTen8 * 100;
const uint64_t kTen11 = kTen8 * 1000;
const uint64_t kTen12 = kTen8 * 10000;
const uint64_t kTen13 = kTen8 * 100000;
const uint64_t kTen14 = kTen8 * 1000000;
const uint64_t kTen15 = kTen8 * 10000000;
const uint64_t kTen16 = kTen8 * kTen8;
if (value < kTen8) {
uint32_t v = static_cast<uint32_t>(value);
if (v < 10000) {
const uint32_t d1 = (v / 100) << 1;
const uint32_t d2 = (v % 100) << 1;
if (v >= 1000)
*buffer++ = cDigitsLut[d1];
if (v >= 100)
*buffer++ = cDigitsLut[d1 + 1];
if (v >= 10)
*buffer++ = cDigitsLut[d2];
*buffer++ = cDigitsLut[d2 + 1];
}
else {
// value = bbbbcccc
const uint32_t b = v / 10000;
const uint32_t c = v % 10000;
const uint32_t d1 = (b / 100) << 1;
const uint32_t d2 = (b % 100) << 1;
const uint32_t d3 = (c / 100) << 1;
const uint32_t d4 = (c % 100) << 1;
if (value >= 10000000)
*buffer++ = cDigitsLut[d1];
if (value >= 1000000)
*buffer++ = cDigitsLut[d1 + 1];
if (value >= 100000)
*buffer++ = cDigitsLut[d2];
*buffer++ = cDigitsLut[d2 + 1];
*buffer++ = cDigitsLut[d3];
*buffer++ = cDigitsLut[d3 + 1];
*buffer++ = cDigitsLut[d4];
*buffer++ = cDigitsLut[d4 + 1];
}
}
else if (value < kTen16) {
const uint32_t v0 = static_cast<uint32_t>(value / kTen8);
const uint32_t v1 = static_cast<uint32_t>(value % kTen8);
const uint32_t b0 = v0 / 10000;
const uint32_t c0 = v0 % 10000;
const uint32_t d1 = (b0 / 100) << 1;
const uint32_t d2 = (b0 % 100) << 1;
const uint32_t d3 = (c0 / 100) << 1;
const uint32_t d4 = (c0 % 100) << 1;
const uint32_t b1 = v1 / 10000;
const uint32_t c1 = v1 % 10000;
const uint32_t d5 = (b1 / 100) << 1;
const uint32_t d6 = (b1 % 100) << 1;
const uint32_t d7 = (c1 / 100) << 1;
const uint32_t d8 = (c1 % 100) << 1;
if (value >= kTen15)
*buffer++ = cDigitsLut[d1];
if (value >= kTen14)
*buffer++ = cDigitsLut[d1 + 1];
if (value >= kTen13)
*buffer++ = cDigitsLut[d2];
if (value >= kTen12)
*buffer++ = cDigitsLut[d2 + 1];
if (value >= kTen11)
*buffer++ = cDigitsLut[d3];
if (value >= kTen10)
*buffer++ = cDigitsLut[d3 + 1];
if (value >= kTen9)
*buffer++ = cDigitsLut[d4];
*buffer++ = cDigitsLut[d4 + 1];
*buffer++ = cDigitsLut[d5];
*buffer++ = cDigitsLut[d5 + 1];
*buffer++ = cDigitsLut[d6];
*buffer++ = cDigitsLut[d6 + 1];
*buffer++ = cDigitsLut[d7];
*buffer++ = cDigitsLut[d7 + 1];
*buffer++ = cDigitsLut[d8];
*buffer++ = cDigitsLut[d8 + 1];
}
else {
const uint32_t a = static_cast<uint32_t>(value / kTen16); // 1 to 1844
value %= kTen16;
if (a < 10)
*buffer++ = static_cast<char>('0' + static_cast<char>(a));
else if (a < 100) {
const uint32_t i = a << 1;
*buffer++ = cDigitsLut[i];
*buffer++ = cDigitsLut[i + 1];
}
else if (a < 1000) {
*buffer++ = static_cast<char>('0' + static_cast<char>(a / 100));
const uint32_t i = (a % 100) << 1;
*buffer++ = cDigitsLut[i];
*buffer++ = cDigitsLut[i + 1];
}
else {
const uint32_t i = (a / 100) << 1;
const uint32_t j = (a % 100) << 1;
*buffer++ = cDigitsLut[i];
*buffer++ = cDigitsLut[i + 1];
*buffer++ = cDigitsLut[j];
*buffer++ = cDigitsLut[j + 1];
}
const uint32_t v0 = static_cast<uint32_t>(value / kTen8);
const uint32_t v1 = static_cast<uint32_t>(value % kTen8);
const uint32_t b0 = v0 / 10000;
const uint32_t c0 = v0 % 10000;
const uint32_t d1 = (b0 / 100) << 1;
const uint32_t d2 = (b0 % 100) << 1;
const uint32_t d3 = (c0 / 100) << 1;
const uint32_t d4 = (c0 % 100) << 1;
const uint32_t b1 = v1 / 10000;
const uint32_t c1 = v1 % 10000;
const uint32_t d5 = (b1 / 100) << 1;
const uint32_t d6 = (b1 % 100) << 1;
const uint32_t d7 = (c1 / 100) << 1;
const uint32_t d8 = (c1 % 100) << 1;
*buffer++ = cDigitsLut[d1];
*buffer++ = cDigitsLut[d1 + 1];
*buffer++ = cDigitsLut[d2];
*buffer++ = cDigitsLut[d2 + 1];
*buffer++ = cDigitsLut[d3];
*buffer++ = cDigitsLut[d3 + 1];
*buffer++ = cDigitsLut[d4];
*buffer++ = cDigitsLut[d4 + 1];
*buffer++ = cDigitsLut[d5];
*buffer++ = cDigitsLut[d5 + 1];
*buffer++ = cDigitsLut[d6];
*buffer++ = cDigitsLut[d6 + 1];
*buffer++ = cDigitsLut[d7];
*buffer++ = cDigitsLut[d7 + 1];
*buffer++ = cDigitsLut[d8];
*buffer++ = cDigitsLut[d8 + 1];
}
return buffer;
}
inline char* i64toa(int64_t value, char* buffer) {
RAPIDJSON_ASSERT(buffer != 0);
uint64_t u = static_cast<uint64_t>(value);
if (value < 0) {
*buffer++ = '-';
u = ~u + 1;
}
return u64toa(u, buffer);
}
} // namespace internal
RAPIDJSON_NAMESPACE_END
#endif // RAPIDJSON_ITOA_

186
livox_ros_driver/common/rapidjson/internal/meta.h Normal file
View File

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

55
livox_ros_driver/common/rapidjson/internal/pow10.h Normal file
View File

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

739
livox_ros_driver/common/rapidjson/internal/regex.h Normal file
View File

@@ -0,0 +1,739 @@
// Tencent is pleased to support the open source community by making RapidJSON available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
#ifndef RAPIDJSON_INTERNAL_REGEX_H_
#define RAPIDJSON_INTERNAL_REGEX_H_
#include "../allocators.h"
#include "../stream.h"
#include "stack.h"
#ifdef __clang__
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(padded)
RAPIDJSON_DIAG_OFF(switch-enum)
#elif defined(_MSC_VER)
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(4512) // assignment operator could not be generated
#endif
#ifdef __GNUC__
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(effc++)
#endif
#ifndef RAPIDJSON_REGEX_VERBOSE
#define RAPIDJSON_REGEX_VERBOSE 0
#endif
RAPIDJSON_NAMESPACE_BEGIN
namespace internal {
///////////////////////////////////////////////////////////////////////////////
// DecodedStream
template <typename SourceStream, typename Encoding>
class DecodedStream {
public:
DecodedStream(SourceStream& ss) : ss_(ss), codepoint_() { Decode(); }
unsigned Peek() { return codepoint_; }
unsigned Take() {
unsigned c = codepoint_;
if (c) // No further decoding when '\0'
Decode();
return c;
}
private:
void Decode() {
if (!Encoding::Decode(ss_, &codepoint_))
codepoint_ = 0;
}
SourceStream& ss_;
unsigned codepoint_;
};
///////////////////////////////////////////////////////////////////////////////
// GenericRegex
static const SizeType kRegexInvalidState = ~SizeType(0); //!< Represents an invalid index in GenericRegex::State::out, out1
static const SizeType kRegexInvalidRange = ~SizeType(0);
template <typename Encoding, typename Allocator>
class GenericRegexSearch;
//! Regular expression engine with subset of ECMAscript grammar.
/*!
Supported regular expression syntax:
- \c ab Concatenation
- \c a|b Alternation
- \c a? Zero or one
- \c a* Zero or more
- \c a+ One or more
- \c a{3} Exactly 3 times
- \c a{3,} At least 3 times
- \c a{3,5} 3 to 5 times
- \c (ab) Grouping
- \c ^a At the beginning
- \c a$ At the end
- \c . Any character
- \c [abc] Character classes
- \c [a-c] Character class range
- \c [a-z0-9_] Character class combination
- \c [^abc] Negated character classes
- \c [^a-c] Negated character class range
- \c [\b] Backspace (U+0008)
- \c \\| \\\\ ... Escape characters
- \c \\f Form feed (U+000C)
- \c \\n Line feed (U+000A)
- \c \\r Carriage return (U+000D)
- \c \\t Tab (U+0009)
- \c \\v Vertical tab (U+000B)
\note This is a Thompson NFA engine, implemented with reference to
Cox, Russ. "Regular Expression Matching Can Be Simple And Fast (but is slow in Java, Perl, PHP, Python, Ruby,...).",
https://swtch.com/~rsc/regexp/regexp1.html
*/
template <typename Encoding, typename Allocator = CrtAllocator>
class GenericRegex {
public:
typedef Encoding EncodingType;
typedef typename Encoding::Ch Ch;
template <typename, typename> friend class GenericRegexSearch;
GenericRegex(const Ch* source, Allocator* allocator = 0) :
ownAllocator_(allocator ? 0 : RAPIDJSON_NEW(Allocator)()), allocator_(allocator ? allocator : ownAllocator_),
states_(allocator_, 256), ranges_(allocator_, 256), root_(kRegexInvalidState), stateCount_(), rangeCount_(),
anchorBegin_(), anchorEnd_()
{
GenericStringStream<Encoding> ss(source);
DecodedStream<GenericStringStream<Encoding>, Encoding> ds(ss);
Parse(ds);
}
~GenericRegex()
{
RAPIDJSON_DELETE(ownAllocator_);
}
bool IsValid() const {
return root_ != kRegexInvalidState;
}
private:
enum Operator {
kZeroOrOne,
kZeroOrMore,
kOneOrMore,
kConcatenation,
kAlternation,
kLeftParenthesis
};
static const unsigned kAnyCharacterClass = 0xFFFFFFFF; //!< For '.'
static const unsigned kRangeCharacterClass = 0xFFFFFFFE;
static const unsigned kRangeNegationFlag = 0x80000000;
struct Range {
unsigned start; //
unsigned end;
SizeType next;
};
struct State {
SizeType out; //!< Equals to kInvalid for matching state
SizeType out1; //!< Equals to non-kInvalid for split
SizeType rangeStart;
unsigned codepoint;
};
struct Frag {
Frag(SizeType s, SizeType o, SizeType m) : start(s), out(o), minIndex(m) {}
SizeType start;
SizeType out; //!< link-list of all output states
SizeType minIndex;
};
State& GetState(SizeType index) {
RAPIDJSON_ASSERT(index < stateCount_);
return states_.template Bottom<State>()[index];
}
const State& GetState(SizeType index) const {
RAPIDJSON_ASSERT(index < stateCount_);
return states_.template Bottom<State>()[index];
}
Range& GetRange(SizeType index) {
RAPIDJSON_ASSERT(index < rangeCount_);
return ranges_.template Bottom<Range>()[index];
}
const Range& GetRange(SizeType index) const {
RAPIDJSON_ASSERT(index < rangeCount_);
return ranges_.template Bottom<Range>()[index];
}
template <typename InputStream>
void Parse(DecodedStream<InputStream, Encoding>& ds) {
Stack<Allocator> operandStack(allocator_, 256); // Frag
Stack<Allocator> operatorStack(allocator_, 256); // Operator
Stack<Allocator> atomCountStack(allocator_, 256); // unsigned (Atom per parenthesis)
*atomCountStack.template Push<unsigned>() = 0;
unsigned codepoint;
while (ds.Peek() != 0) {
switch (codepoint = ds.Take()) {
case '^':
anchorBegin_ = true;
break;
case '$':
anchorEnd_ = true;
break;
case '|':
while (!operatorStack.Empty() && *operatorStack.template Top<Operator>() < kAlternation)
if (!Eval(operandStack, *operatorStack.template Pop<Operator>(1)))
return;
*operatorStack.template Push<Operator>() = kAlternation;
*atomCountStack.template Top<unsigned>() = 0;
break;
case '(':
*operatorStack.template Push<Operator>() = kLeftParenthesis;
*atomCountStack.template Push<unsigned>() = 0;
break;
case ')':
while (!operatorStack.Empty() && *operatorStack.template Top<Operator>() != kLeftParenthesis)
if (!Eval(operandStack, *operatorStack.template Pop<Operator>(1)))
return;
if (operatorStack.Empty())
return;
operatorStack.template Pop<Operator>(1);
atomCountStack.template Pop<unsigned>(1);
ImplicitConcatenation(atomCountStack, operatorStack);
break;
case '?':
if (!Eval(operandStack, kZeroOrOne))
return;
break;
case '*':
if (!Eval(operandStack, kZeroOrMore))
return;
break;
case '+':
if (!Eval(operandStack, kOneOrMore))
return;
break;
case '{':
{
unsigned n, m;
if (!ParseUnsigned(ds, &n))
return;
if (ds.Peek() == ',') {
ds.Take();
if (ds.Peek() == '}')
m = kInfinityQuantifier;
else if (!ParseUnsigned(ds, &m) || m < n)
return;
}
else
m = n;
if (!EvalQuantifier(operandStack, n, m) || ds.Peek() != '}')
return;
ds.Take();
}
break;
case '.':
PushOperand(operandStack, kAnyCharacterClass);
ImplicitConcatenation(atomCountStack, operatorStack);
break;
case '[':
{
SizeType range;
if (!ParseRange(ds, &range))
return;
SizeType s = NewState(kRegexInvalidState, kRegexInvalidState, kRangeCharacterClass);
GetState(s).rangeStart = range;
*operandStack.template Push<Frag>() = Frag(s, s, s);
}
ImplicitConcatenation(atomCountStack, operatorStack);
break;
case '\\': // Escape character
if (!CharacterEscape(ds, &codepoint))
return; // Unsupported escape character
// fall through to default
RAPIDJSON_DELIBERATE_FALLTHROUGH;
default: // Pattern character
PushOperand(operandStack, codepoint);
ImplicitConcatenation(atomCountStack, operatorStack);
}
}
while (!operatorStack.Empty())
if (!Eval(operandStack, *operatorStack.template Pop<Operator>(1)))
return;
// Link the operand to matching state.
if (operandStack.GetSize() == sizeof(Frag)) {
Frag* e = operandStack.template Pop<Frag>(1);
Patch(e->out, NewState(kRegexInvalidState, kRegexInvalidState, 0));
root_ = e->start;
#if RAPIDJSON_REGEX_VERBOSE
printf("root: %d\n", root_);
for (SizeType i = 0; i < stateCount_ ; i++) {
State& s = GetState(i);
printf("[%2d] out: %2d out1: %2d c: '%c'\n", i, s.out, s.out1, (char)s.codepoint);
}
printf("\n");
#endif
}
}
SizeType NewState(SizeType out, SizeType out1, unsigned codepoint) {
State* s = states_.template Push<State>();
s->out = out;
s->out1 = out1;
s->codepoint = codepoint;
s->rangeStart = kRegexInvalidRange;
return stateCount_++;
}
void PushOperand(Stack<Allocator>& operandStack, unsigned codepoint) {
SizeType s = NewState(kRegexInvalidState, kRegexInvalidState, codepoint);
*operandStack.template Push<Frag>() = Frag(s, s, s);
}
void ImplicitConcatenation(Stack<Allocator>& atomCountStack, Stack<Allocator>& operatorStack) {
if (*atomCountStack.template Top<unsigned>())
*operatorStack.template Push<Operator>() = kConcatenation;
(*atomCountStack.template Top<unsigned>())++;
}
SizeType Append(SizeType l1, SizeType l2) {
SizeType old = l1;
while (GetState(l1).out != kRegexInvalidState)
l1 = GetState(l1).out;
GetState(l1).out = l2;
return old;
}
void Patch(SizeType l, SizeType s) {
for (SizeType next; l != kRegexInvalidState; l = next) {
next = GetState(l).out;
GetState(l).out = s;
}
}
bool Eval(Stack<Allocator>& operandStack, Operator op) {
switch (op) {
case kConcatenation:
RAPIDJSON_ASSERT(operandStack.GetSize() >= sizeof(Frag) * 2);
{
Frag e2 = *operandStack.template Pop<Frag>(1);
Frag e1 = *operandStack.template Pop<Frag>(1);
Patch(e1.out, e2.start);
*operandStack.template Push<Frag>() = Frag(e1.start, e2.out, Min(e1.minIndex, e2.minIndex));
}
return true;
case kAlternation:
if (operandStack.GetSize() >= sizeof(Frag) * 2) {
Frag e2 = *operandStack.template Pop<Frag>(1);
Frag e1 = *operandStack.template Pop<Frag>(1);
SizeType s = NewState(e1.start, e2.start, 0);
*operandStack.template Push<Frag>() = Frag(s, Append(e1.out, e2.out), Min(e1.minIndex, e2.minIndex));
return true;
}
return false;
case kZeroOrOne:
if (operandStack.GetSize() >= sizeof(Frag)) {
Frag e = *operandStack.template Pop<Frag>(1);
SizeType s = NewState(kRegexInvalidState, e.start, 0);
*operandStack.template Push<Frag>() = Frag(s, Append(e.out, s), e.minIndex);
return true;
}
return false;
case kZeroOrMore:
if (operandStack.GetSize() >= sizeof(Frag)) {
Frag e = *operandStack.template Pop<Frag>(1);
SizeType s = NewState(kRegexInvalidState, e.start, 0);
Patch(e.out, s);
*operandStack.template Push<Frag>() = Frag(s, s, e.minIndex);
return true;
}
return false;
case kOneOrMore:
if (operandStack.GetSize() >= sizeof(Frag)) {
Frag e = *operandStack.template Pop<Frag>(1);
SizeType s = NewState(kRegexInvalidState, e.start, 0);
Patch(e.out, s);
*operandStack.template Push<Frag>() = Frag(e.start, s, e.minIndex);
return true;
}
return false;
default:
// syntax error (e.g. unclosed kLeftParenthesis)
return false;
}
}
bool EvalQuantifier(Stack<Allocator>& operandStack, unsigned n, unsigned m) {
RAPIDJSON_ASSERT(n <= m);
RAPIDJSON_ASSERT(operandStack.GetSize() >= sizeof(Frag));
if (n == 0) {
if (m == 0) // a{0} not support
return false;
else if (m == kInfinityQuantifier)
Eval(operandStack, kZeroOrMore); // a{0,} -> a*
else {
Eval(operandStack, kZeroOrOne); // a{0,5} -> a?
for (unsigned i = 0; i < m - 1; i++)
CloneTopOperand(operandStack); // a{0,5} -> a? a? a? a? a?
for (unsigned i = 0; i < m - 1; i++)
Eval(operandStack, kConcatenation); // a{0,5} -> a?a?a?a?a?
}
return true;
}
for (unsigned i = 0; i < n - 1; i++) // a{3} -> a a a
CloneTopOperand(operandStack);
if (m == kInfinityQuantifier)
Eval(operandStack, kOneOrMore); // a{3,} -> a a a+
else if (m > n) {
CloneTopOperand(operandStack); // a{3,5} -> a a a a
Eval(operandStack, kZeroOrOne); // a{3,5} -> a a a a?
for (unsigned i = n; i < m - 1; i++)
CloneTopOperand(operandStack); // a{3,5} -> a a a a? a?
for (unsigned i = n; i < m; i++)
Eval(operandStack, kConcatenation); // a{3,5} -> a a aa?a?
}
for (unsigned i = 0; i < n - 1; i++)
Eval(operandStack, kConcatenation); // a{3} -> aaa, a{3,} -> aaa+, a{3.5} -> aaaa?a?
return true;
}
static SizeType Min(SizeType a, SizeType b) { return a < b ? a : b; }
void CloneTopOperand(Stack<Allocator>& operandStack) {
const Frag src = *operandStack.template Top<Frag>(); // Copy constructor to prevent invalidation
SizeType count = stateCount_ - src.minIndex; // Assumes top operand contains states in [src->minIndex, stateCount_)
State* s = states_.template Push<State>(count);
memcpy(s, &GetState(src.minIndex), count * sizeof(State));
for (SizeType j = 0; j < count; j++) {
if (s[j].out != kRegexInvalidState)
s[j].out += count;
if (s[j].out1 != kRegexInvalidState)
s[j].out1 += count;
}
*operandStack.template Push<Frag>() = Frag(src.start + count, src.out + count, src.minIndex + count);
stateCount_ += count;
}
template <typename InputStream>
bool ParseUnsigned(DecodedStream<InputStream, Encoding>& ds, unsigned* u) {
unsigned r = 0;
if (ds.Peek() < '0' || ds.Peek() > '9')
return false;
while (ds.Peek() >= '0' && ds.Peek() <= '9') {
if (r >= 429496729 && ds.Peek() > '5') // 2^32 - 1 = 4294967295
return false; // overflow
r = r * 10 + (ds.Take() - '0');
}
*u = r;
return true;
}
template <typename InputStream>
bool ParseRange(DecodedStream<InputStream, Encoding>& ds, SizeType* range) {
bool isBegin = true;
bool negate = false;
int step = 0;
SizeType start = kRegexInvalidRange;
SizeType current = kRegexInvalidRange;
unsigned codepoint;
while ((codepoint = ds.Take()) != 0) {
if (isBegin) {
isBegin = false;
if (codepoint == '^') {
negate = true;
continue;
}
}
switch (codepoint) {
case ']':
if (start == kRegexInvalidRange)
return false; // Error: nothing inside []
if (step == 2) { // Add trailing '-'
SizeType r = NewRange('-');
RAPIDJSON_ASSERT(current != kRegexInvalidRange);
GetRange(current).next = r;
}
if (negate)
GetRange(start).start |= kRangeNegationFlag;
*range = start;
return true;
case '\\':
if (ds.Peek() == 'b') {
ds.Take();
codepoint = 0x0008; // Escape backspace character
}
else if (!CharacterEscape(ds, &codepoint))
return false;
// fall through to default
RAPIDJSON_DELIBERATE_FALLTHROUGH;
default:
switch (step) {
case 1:
if (codepoint == '-') {
step++;
break;
}
// fall through to step 0 for other characters
RAPIDJSON_DELIBERATE_FALLTHROUGH;
case 0:
{
SizeType r = NewRange(codepoint);
if (current != kRegexInvalidRange)
GetRange(current).next = r;
if (start == kRegexInvalidRange)
start = r;
current = r;
}
step = 1;
break;
default:
RAPIDJSON_ASSERT(step == 2);
GetRange(current).end = codepoint;
step = 0;
}
}
}
return false;
}
SizeType NewRange(unsigned codepoint) {
Range* r = ranges_.template Push<Range>();
r->start = r->end = codepoint;
r->next = kRegexInvalidRange;
return rangeCount_++;
}
template <typename InputStream>
bool CharacterEscape(DecodedStream<InputStream, Encoding>& ds, unsigned* escapedCodepoint) {
unsigned codepoint;
switch (codepoint = ds.Take()) {
case '^':
case '$':
case '|':
case '(':
case ')':
case '?':
case '*':
case '+':
case '.':
case '[':
case ']':
case '{':
case '}':
case '\\':
*escapedCodepoint = codepoint; return true;
case 'f': *escapedCodepoint = 0x000C; return true;
case 'n': *escapedCodepoint = 0x000A; return true;
case 'r': *escapedCodepoint = 0x000D; return true;
case 't': *escapedCodepoint = 0x0009; return true;
case 'v': *escapedCodepoint = 0x000B; return true;
default:
return false; // Unsupported escape character
}
}
Allocator* ownAllocator_;
Allocator* allocator_;
Stack<Allocator> states_;
Stack<Allocator> ranges_;
SizeType root_;
SizeType stateCount_;
SizeType rangeCount_;
static const unsigned kInfinityQuantifier = ~0u;
// For SearchWithAnchoring()
bool anchorBegin_;
bool anchorEnd_;
};
template <typename RegexType, typename Allocator = CrtAllocator>
class GenericRegexSearch {
public:
typedef typename RegexType::EncodingType Encoding;
typedef typename Encoding::Ch Ch;
GenericRegexSearch(const RegexType& regex, Allocator* allocator = 0) :
regex_(regex), allocator_(allocator), ownAllocator_(0),
state0_(allocator, 0), state1_(allocator, 0), stateSet_()
{
RAPIDJSON_ASSERT(regex_.IsValid());
if (!allocator_)
ownAllocator_ = allocator_ = RAPIDJSON_NEW(Allocator)();
stateSet_ = static_cast<unsigned*>(allocator_->Malloc(GetStateSetSize()));
state0_.template Reserve<SizeType>(regex_.stateCount_);
state1_.template Reserve<SizeType>(regex_.stateCount_);
}
~GenericRegexSearch() {
Allocator::Free(stateSet_);
RAPIDJSON_DELETE(ownAllocator_);
}
template <typename InputStream>
bool Match(InputStream& is) {
return SearchWithAnchoring(is, true, true);
}
bool Match(const Ch* s) {
GenericStringStream<Encoding> is(s);
return Match(is);
}
template <typename InputStream>
bool Search(InputStream& is) {
return SearchWithAnchoring(is, regex_.anchorBegin_, regex_.anchorEnd_);
}
bool Search(const Ch* s) {
GenericStringStream<Encoding> is(s);
return Search(is);
}
private:
typedef typename RegexType::State State;
typedef typename RegexType::Range Range;
template <typename InputStream>
bool SearchWithAnchoring(InputStream& is, bool anchorBegin, bool anchorEnd) {
DecodedStream<InputStream, Encoding> ds(is);
state0_.Clear();
Stack<Allocator> *current = &state0_, *next = &state1_;
const size_t stateSetSize = GetStateSetSize();
std::memset(stateSet_, 0, stateSetSize);
bool matched = AddState(*current, regex_.root_);
unsigned codepoint;
while (!current->Empty() && (codepoint = ds.Take()) != 0) {
std::memset(stateSet_, 0, stateSetSize);
next->Clear();
matched = false;
for (const SizeType* s = current->template Bottom<SizeType>(); s != current->template End<SizeType>(); ++s) {
const State& sr = regex_.GetState(*s);
if (sr.codepoint == codepoint ||
sr.codepoint == RegexType::kAnyCharacterClass ||
(sr.codepoint == RegexType::kRangeCharacterClass && MatchRange(sr.rangeStart, codepoint)))
{
matched = AddState(*next, sr.out) || matched;
if (!anchorEnd && matched)
return true;
}
if (!anchorBegin)
AddState(*next, regex_.root_);
}
internal::Swap(current, next);
}
return matched;
}
size_t GetStateSetSize() const {
return (regex_.stateCount_ + 31) / 32 * 4;
}
// Return whether the added states is a match state
bool AddState(Stack<Allocator>& l, SizeType index) {
RAPIDJSON_ASSERT(index != kRegexInvalidState);
const State& s = regex_.GetState(index);
if (s.out1 != kRegexInvalidState) { // Split
bool matched = AddState(l, s.out);
return AddState(l, s.out1) || matched;
}
else if (!(stateSet_[index >> 5] & (1u << (index & 31)))) {
stateSet_[index >> 5] |= (1u << (index & 31));
*l.template PushUnsafe<SizeType>() = index;
}
return s.out == kRegexInvalidState; // by using PushUnsafe() above, we can ensure s is not validated due to reallocation.
}
bool MatchRange(SizeType rangeIndex, unsigned codepoint) const {
bool yes = (regex_.GetRange(rangeIndex).start & RegexType::kRangeNegationFlag) == 0;
while (rangeIndex != kRegexInvalidRange) {
const Range& r = regex_.GetRange(rangeIndex);
if (codepoint >= (r.start & ~RegexType::kRangeNegationFlag) && codepoint <= r.end)
return yes;
rangeIndex = r.next;
}
return !yes;
}
const RegexType& regex_;
Allocator* allocator_;
Allocator* ownAllocator_;
Stack<Allocator> state0_;
Stack<Allocator> state1_;
uint32_t* stateSet_;
};
typedef GenericRegex<UTF8<> > Regex;
typedef GenericRegexSearch<Regex> RegexSearch;
} // namespace internal
RAPIDJSON_NAMESPACE_END
#ifdef __GNUC__
RAPIDJSON_DIAG_POP
#endif
#if defined(__clang__) || defined(_MSC_VER)
RAPIDJSON_DIAG_POP
#endif
#endif // RAPIDJSON_INTERNAL_REGEX_H_

232
livox_ros_driver/common/rapidjson/internal/stack.h Normal file
View File

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

69
livox_ros_driver/common/rapidjson/internal/strfunc.h Normal file
View File

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

290
livox_ros_driver/common/rapidjson/internal/strtod.h Normal file
View File

@@ -0,0 +1,290 @@
// Tencent is pleased to support the open source community by making RapidJSON available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
#ifndef RAPIDJSON_STRTOD_
#define RAPIDJSON_STRTOD_
#include "ieee754.h"
#include "biginteger.h"
#include "diyfp.h"
#include "pow10.h"
#include <climits>
#include <limits>
RAPIDJSON_NAMESPACE_BEGIN
namespace internal {
inline double FastPath(double significand, int exp) {
if (exp < -308)
return 0.0;
else if (exp >= 0)
return significand * internal::Pow10(exp);
else
return significand / internal::Pow10(-exp);
}
inline double StrtodNormalPrecision(double d, int p) {
if (p < -308) {
// Prevent expSum < -308, making Pow10(p) = 0
d = FastPath(d, -308);
d = FastPath(d, p + 308);
}
else
d = FastPath(d, p);
return d;
}
template <typename T>
inline T Min3(T a, T b, T c) {
T m = a;
if (m > b) m = b;
if (m > c) m = c;
return m;
}
inline int CheckWithinHalfULP(double b, const BigInteger& d, int dExp) {
const Double db(b);
const uint64_t bInt = db.IntegerSignificand();
const int bExp = db.IntegerExponent();
const int hExp = bExp - 1;
int dS_Exp2 = 0, dS_Exp5 = 0, bS_Exp2 = 0, bS_Exp5 = 0, hS_Exp2 = 0, hS_Exp5 = 0;
// Adjust for decimal exponent
if (dExp >= 0) {
dS_Exp2 += dExp;
dS_Exp5 += dExp;
}
else {
bS_Exp2 -= dExp;
bS_Exp5 -= dExp;
hS_Exp2 -= dExp;
hS_Exp5 -= dExp;
}
// Adjust for binary exponent
if (bExp >= 0)
bS_Exp2 += bExp;
else {
dS_Exp2 -= bExp;
hS_Exp2 -= bExp;
}
// Adjust for half ulp exponent
if (hExp >= 0)
hS_Exp2 += hExp;
else {
dS_Exp2 -= hExp;
bS_Exp2 -= hExp;
}
// Remove common power of two factor from all three scaled values
int common_Exp2 = Min3(dS_Exp2, bS_Exp2, hS_Exp2);
dS_Exp2 -= common_Exp2;
bS_Exp2 -= common_Exp2;
hS_Exp2 -= common_Exp2;
BigInteger dS = d;
dS.MultiplyPow5(static_cast<unsigned>(dS_Exp5)) <<= static_cast<unsigned>(dS_Exp2);
BigInteger bS(bInt);
bS.MultiplyPow5(static_cast<unsigned>(bS_Exp5)) <<= static_cast<unsigned>(bS_Exp2);
BigInteger hS(1);
hS.MultiplyPow5(static_cast<unsigned>(hS_Exp5)) <<= static_cast<unsigned>(hS_Exp2);
BigInteger delta(0);
dS.Difference(bS, &delta);
return delta.Compare(hS);
}
inline bool StrtodFast(double d, int p, double* result) {
// Use fast path for string-to-double conversion if possible
// see http://www.exploringbinary.com/fast-path-decimal-to-floating-point-conversion/
if (p > 22 && p < 22 + 16) {
// Fast Path Cases In Disguise
d *= internal::Pow10(p - 22);
p = 22;
}
if (p >= -22 && p <= 22 && d <= 9007199254740991.0) { // 2^53 - 1
*result = FastPath(d, p);
return true;
}
else
return false;
}
// Compute an approximation and see if it is within 1/2 ULP
inline bool StrtodDiyFp(const char* decimals, int dLen, int dExp, double* result) {
uint64_t significand = 0;
int i = 0; // 2^64 - 1 = 18446744073709551615, 1844674407370955161 = 0x1999999999999999
for (; i < dLen; i++) {
if (significand > RAPIDJSON_UINT64_C2(0x19999999, 0x99999999) ||
(significand == RAPIDJSON_UINT64_C2(0x19999999, 0x99999999) && decimals[i] > '5'))
break;
significand = significand * 10u + static_cast<unsigned>(decimals[i] - '0');
}
if (i < dLen && decimals[i] >= '5') // Rounding
significand++;
int remaining = dLen - i;
const int kUlpShift = 3;
const int kUlp = 1 << kUlpShift;
int64_t error = (remaining == 0) ? 0 : kUlp / 2;
DiyFp v(significand, 0);
v = v.Normalize();
error <<= -v.e;
dExp += remaining;
int actualExp;
DiyFp cachedPower = GetCachedPower10(dExp, &actualExp);
if (actualExp != dExp) {
static const DiyFp kPow10[] = {
DiyFp(RAPIDJSON_UINT64_C2(0xa0000000, 0x00000000), -60), // 10^1
DiyFp(RAPIDJSON_UINT64_C2(0xc8000000, 0x00000000), -57), // 10^2
DiyFp(RAPIDJSON_UINT64_C2(0xfa000000, 0x00000000), -54), // 10^3
DiyFp(RAPIDJSON_UINT64_C2(0x9c400000, 0x00000000), -50), // 10^4
DiyFp(RAPIDJSON_UINT64_C2(0xc3500000, 0x00000000), -47), // 10^5
DiyFp(RAPIDJSON_UINT64_C2(0xf4240000, 0x00000000), -44), // 10^6
DiyFp(RAPIDJSON_UINT64_C2(0x98968000, 0x00000000), -40) // 10^7
};
int adjustment = dExp - actualExp;
RAPIDJSON_ASSERT(adjustment >= 1 && adjustment < 8);
v = v * kPow10[adjustment - 1];
if (dLen + adjustment > 19) // has more digits than decimal digits in 64-bit
error += kUlp / 2;
}
v = v * cachedPower;
error += kUlp + (error == 0 ? 0 : 1);
const int oldExp = v.e;
v = v.Normalize();
error <<= oldExp - v.e;
const int effectiveSignificandSize = Double::EffectiveSignificandSize(64 + v.e);
int precisionSize = 64 - effectiveSignificandSize;
if (precisionSize + kUlpShift >= 64) {
int scaleExp = (precisionSize + kUlpShift) - 63;
v.f >>= scaleExp;
v.e += scaleExp;
error = (error >> scaleExp) + 1 + kUlp;
precisionSize -= scaleExp;
}
DiyFp rounded(v.f >> precisionSize, v.e + precisionSize);
const uint64_t precisionBits = (v.f & ((uint64_t(1) << precisionSize) - 1)) * kUlp;
const uint64_t halfWay = (uint64_t(1) << (precisionSize - 1)) * kUlp;
if (precisionBits >= halfWay + static_cast<unsigned>(error)) {
rounded.f++;
if (rounded.f & (DiyFp::kDpHiddenBit << 1)) { // rounding overflows mantissa (issue #340)
rounded.f >>= 1;
rounded.e++;
}
}
*result = rounded.ToDouble();
return halfWay - static_cast<unsigned>(error) >= precisionBits || precisionBits >= halfWay + static_cast<unsigned>(error);
}
inline double StrtodBigInteger(double approx, const char* decimals, int dLen, int dExp) {
RAPIDJSON_ASSERT(dLen >= 0);
const BigInteger dInt(decimals, static_cast<unsigned>(dLen));
Double a(approx);
int cmp = CheckWithinHalfULP(a.Value(), dInt, dExp);
if (cmp < 0)
return a.Value(); // within half ULP
else if (cmp == 0) {
// Round towards even
if (a.Significand() & 1)
return a.NextPositiveDouble();
else
return a.Value();
}
else // adjustment
return a.NextPositiveDouble();
}
inline double StrtodFullPrecision(double d, int p, const char* decimals, size_t length, size_t decimalPosition, int exp) {
RAPIDJSON_ASSERT(d >= 0.0);
RAPIDJSON_ASSERT(length >= 1);
double result = 0.0;
if (StrtodFast(d, p, &result))
return result;
RAPIDJSON_ASSERT(length <= INT_MAX);
int dLen = static_cast<int>(length);
RAPIDJSON_ASSERT(length >= decimalPosition);
RAPIDJSON_ASSERT(length - decimalPosition <= INT_MAX);
int dExpAdjust = static_cast<int>(length - decimalPosition);
RAPIDJSON_ASSERT(exp >= INT_MIN + dExpAdjust);
int dExp = exp - dExpAdjust;
// Make sure length+dExp does not overflow
RAPIDJSON_ASSERT(dExp <= INT_MAX - dLen);
// Trim leading zeros
while (dLen > 0 && *decimals == '0') {
dLen--;
decimals++;
}
// Trim trailing zeros
while (dLen > 0 && decimals[dLen - 1] == '0') {
dLen--;
dExp++;
}
if (dLen == 0) { // Buffer only contains zeros.
return 0.0;
}
// Trim right-most digits
const int kMaxDecimalDigit = 767 + 1;
if (dLen > kMaxDecimalDigit) {
dExp += dLen - kMaxDecimalDigit;
dLen = kMaxDecimalDigit;
}
// If too small, underflow to zero.
// Any x <= 10^-324 is interpreted as zero.
if (dLen + dExp <= -324)
return 0.0;
// If too large, overflow to infinity.
// Any x >= 10^309 is interpreted as +infinity.
if (dLen + dExp > 309)
return std::numeric_limits<double>::infinity();
if (StrtodDiyFp(decimals, dLen, dExp, &result))
return result;
// Use approximation from StrtodDiyFp and make adjustment with BigInteger comparison
return StrtodBigInteger(result, decimals, dLen, dExp);
}
} // namespace internal
RAPIDJSON_NAMESPACE_END
#endif // RAPIDJSON_STRTOD_

46
livox_ros_driver/common/rapidjson/internal/swap.h Normal file
View File

@@ -0,0 +1,46 @@
// Tencent is pleased to support the open source community by making RapidJSON available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
#ifndef RAPIDJSON_INTERNAL_SWAP_H_
#define RAPIDJSON_INTERNAL_SWAP_H_
#include "../rapidjson.h"
#if defined(__clang__)
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(c++98-compat)
#endif
RAPIDJSON_NAMESPACE_BEGIN
namespace internal {
//! Custom swap() to avoid dependency on C++ <algorithm> header
/*! \tparam T Type of the arguments to swap, should be instantiated with primitive C++ types only.
\note This has the same semantics as std::swap().
*/
template <typename T>
inline void Swap(T& a, T& b) RAPIDJSON_NOEXCEPT {
T tmp = a;
a = b;
b = tmp;
}
} // namespace internal
RAPIDJSON_NAMESPACE_END
#if defined(__clang__)
RAPIDJSON_DIAG_POP
#endif
#endif // RAPIDJSON_INTERNAL_SWAP_H_

128
livox_ros_driver/common/rapidjson/istreamwrapper.h Normal file
View File

@@ -0,0 +1,128 @@
// Tencent is pleased to support the open source community by making RapidJSON available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
#ifndef RAPIDJSON_ISTREAMWRAPPER_H_
#define RAPIDJSON_ISTREAMWRAPPER_H_
#include "stream.h"
#include <iosfwd>
#include <ios>
#ifdef __clang__
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(padded)
#elif defined(_MSC_VER)
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(4351) // new behavior: elements of array 'array' will be default initialized
#endif
RAPIDJSON_NAMESPACE_BEGIN
//! Wrapper of \c std::basic_istream into RapidJSON's Stream concept.
/*!
The classes can be wrapped including but not limited to:
- \c std::istringstream
- \c std::stringstream
- \c std::wistringstream
- \c std::wstringstream
- \c std::ifstream
- \c std::fstream
- \c std::wifstream
- \c std::wfstream
\tparam StreamType Class derived from \c std::basic_istream.
*/
template <typename StreamType>
class BasicIStreamWrapper {
public:
typedef typename StreamType::char_type Ch;
//! Constructor.
/*!
\param stream stream opened for read.
*/
BasicIStreamWrapper(StreamType &stream) : stream_(stream), buffer_(peekBuffer_), bufferSize_(4), bufferLast_(0), current_(buffer_), readCount_(0), count_(0), eof_(false) {
Read();
}
//! Constructor.
/*!
\param stream stream opened for read.
\param buffer user-supplied buffer.
\param bufferSize size of buffer in bytes. Must >=4 bytes.
*/
BasicIStreamWrapper(StreamType &stream, char* buffer, size_t bufferSize) : stream_(stream), buffer_(buffer), bufferSize_(bufferSize), bufferLast_(0), current_(buffer_), readCount_(0), count_(0), eof_(false) {
RAPIDJSON_ASSERT(bufferSize >= 4);
Read();
}
Ch Peek() const { return *current_; }
Ch Take() { Ch c = *current_; Read(); return c; }
size_t Tell() const { return count_ + static_cast<size_t>(current_ - buffer_); }
// Not implemented
void Put(Ch) { RAPIDJSON_ASSERT(false); }
void Flush() { RAPIDJSON_ASSERT(false); }
Ch* PutBegin() { RAPIDJSON_ASSERT(false); return 0; }
size_t PutEnd(Ch*) { RAPIDJSON_ASSERT(false); return 0; }
// For encoding detection only.
const Ch* Peek4() const {
return (current_ + 4 - !eof_ <= bufferLast_) ? current_ : 0;
}
private:
BasicIStreamWrapper();
BasicIStreamWrapper(const BasicIStreamWrapper&);
BasicIStreamWrapper& operator=(const BasicIStreamWrapper&);
void Read() {
if (current_ < bufferLast_)
++current_;
else if (!eof_) {
count_ += readCount_;
readCount_ = bufferSize_;
bufferLast_ = buffer_ + readCount_ - 1;
current_ = buffer_;
if (!stream_.read(buffer_, static_cast<std::streamsize>(bufferSize_))) {
readCount_ = static_cast<size_t>(stream_.gcount());
*(bufferLast_ = buffer_ + readCount_) = '\0';
eof_ = true;
}
}
}
StreamType &stream_;
Ch peekBuffer_[4], *buffer_;
size_t bufferSize_;
Ch *bufferLast_;
Ch *current_;
size_t readCount_;
size_t count_; //!< Number of characters read
bool eof_;
};
typedef BasicIStreamWrapper<std::istream> IStreamWrapper;
typedef BasicIStreamWrapper<std::wistream> WIStreamWrapper;
#if defined(__clang__) || defined(_MSC_VER)
RAPIDJSON_DIAG_POP
#endif
RAPIDJSON_NAMESPACE_END
#endif // RAPIDJSON_ISTREAMWRAPPER_H_

70
livox_ros_driver/common/rapidjson/memorybuffer.h Normal file
View File

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

71
livox_ros_driver/common/rapidjson/memorystream.h Normal file
View File

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

316
livox_ros_driver/common/rapidjson/msinttypes/inttypes.h Normal file
View File

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

300
livox_ros_driver/common/rapidjson/msinttypes/stdint.h Normal file
View File

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

81
livox_ros_driver/common/rapidjson/ostreamwrapper.h Normal file
View File

@@ -0,0 +1,81 @@
// Tencent is pleased to support the open source community by making RapidJSON available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
#ifndef RAPIDJSON_OSTREAMWRAPPER_H_
#define RAPIDJSON_OSTREAMWRAPPER_H_
#include "stream.h"
#include <iosfwd>
#ifdef __clang__
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(padded)
#endif
RAPIDJSON_NAMESPACE_BEGIN
//! Wrapper of \c std::basic_ostream into RapidJSON's Stream concept.
/*!
The classes can be wrapped including but not limited to:
- \c std::ostringstream
- \c std::stringstream
- \c std::wpstringstream
- \c std::wstringstream
- \c std::ifstream
- \c std::fstream
- \c std::wofstream
- \c std::wfstream
\tparam StreamType Class derived from \c std::basic_ostream.
*/
template <typename StreamType>
class BasicOStreamWrapper {
public:
typedef typename StreamType::char_type Ch;
BasicOStreamWrapper(StreamType& stream) : stream_(stream) {}
void Put(Ch c) {
stream_.put(c);
}
void Flush() {
stream_.flush();
}
// Not implemented
char Peek() const { RAPIDJSON_ASSERT(false); return 0; }
char Take() { RAPIDJSON_ASSERT(false); return 0; }
size_t Tell() const { RAPIDJSON_ASSERT(false); return 0; }
char* PutBegin() { RAPIDJSON_ASSERT(false); return 0; }
size_t PutEnd(char*) { RAPIDJSON_ASSERT(false); return 0; }
private:
BasicOStreamWrapper(const BasicOStreamWrapper&);
BasicOStreamWrapper& operator=(const BasicOStreamWrapper&);
StreamType& stream_;
};
typedef BasicOStreamWrapper<std::ostream> OStreamWrapper;
typedef BasicOStreamWrapper<std::wostream> WOStreamWrapper;
#ifdef __clang__
RAPIDJSON_DIAG_POP
#endif
RAPIDJSON_NAMESPACE_END
#endif // RAPIDJSON_OSTREAMWRAPPER_H_

1415
livox_ros_driver/common/rapidjson/pointer.h Normal file
View File

File diff suppressed because it is too large Load Diff

277
livox_ros_driver/common/rapidjson/prettywriter.h Normal file
View File

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

675
livox_ros_driver/common/rapidjson/rapidjson.h Normal file
View File

@@ -0,0 +1,675 @@
// Tencent is pleased to support the open source community by making RapidJSON available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
#ifndef RAPIDJSON_RAPIDJSON_H_
#define RAPIDJSON_RAPIDJSON_H_
/*!\file rapidjson.h
\brief common definitions and configuration
\see RAPIDJSON_CONFIG
*/
/*! \defgroup RAPIDJSON_CONFIG RapidJSON configuration
\brief Configuration macros for library features
Some RapidJSON features are configurable to adapt the library to a wide
variety of platforms, environments and usage scenarios. Most of the
features can be configured in terms of overridden or predefined
preprocessor macros at compile-time.
Some additional customization is available in the \ref RAPIDJSON_ERRORS APIs.
\note These macros should be given on the compiler command-line
(where applicable) to avoid inconsistent values when compiling
different translation units of a single application.
*/
#include <cstdlib> // malloc(), realloc(), free(), size_t
#include <cstring> // memset(), memcpy(), memmove(), memcmp()
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_VERSION_STRING
//
// ALWAYS synchronize the following 3 macros with corresponding variables in /CMakeLists.txt.
//
//!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN
// token stringification
#define RAPIDJSON_STRINGIFY(x) RAPIDJSON_DO_STRINGIFY(x)
#define RAPIDJSON_DO_STRINGIFY(x) #x
// token concatenation
#define RAPIDJSON_JOIN(X, Y) RAPIDJSON_DO_JOIN(X, Y)
#define RAPIDJSON_DO_JOIN(X, Y) RAPIDJSON_DO_JOIN2(X, Y)
#define RAPIDJSON_DO_JOIN2(X, Y) X##Y
//!@endcond
/*! \def RAPIDJSON_MAJOR_VERSION
\ingroup RAPIDJSON_CONFIG
\brief Major version of RapidJSON in integer.
*/
/*! \def RAPIDJSON_MINOR_VERSION
\ingroup RAPIDJSON_CONFIG
\brief Minor version of RapidJSON in integer.
*/
/*! \def RAPIDJSON_PATCH_VERSION
\ingroup RAPIDJSON_CONFIG
\brief Patch version of RapidJSON in integer.
*/
/*! \def RAPIDJSON_VERSION_STRING
\ingroup RAPIDJSON_CONFIG
\brief Version of RapidJSON in "<major>.<minor>.<patch>" string format.
*/
#define RAPIDJSON_MAJOR_VERSION 1
#define RAPIDJSON_MINOR_VERSION 1
#define RAPIDJSON_PATCH_VERSION 0
#define RAPIDJSON_VERSION_STRING \
RAPIDJSON_STRINGIFY(RAPIDJSON_MAJOR_VERSION.RAPIDJSON_MINOR_VERSION.RAPIDJSON_PATCH_VERSION)
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_NAMESPACE_(BEGIN|END)
/*! \def RAPIDJSON_NAMESPACE
\ingroup RAPIDJSON_CONFIG
\brief provide custom rapidjson namespace
In order to avoid symbol clashes and/or "One Definition Rule" errors
between multiple inclusions of (different versions of) RapidJSON in
a single binary, users can customize the name of the main RapidJSON
namespace.
In case of a single nesting level, defining \c RAPIDJSON_NAMESPACE
to a custom name (e.g. \c MyRapidJSON) is sufficient. If multiple
levels are needed, both \ref RAPIDJSON_NAMESPACE_BEGIN and \ref
RAPIDJSON_NAMESPACE_END need to be defined as well:
\code
// in some .cpp file
#define RAPIDJSON_NAMESPACE my::rapidjson
#define RAPIDJSON_NAMESPACE_BEGIN namespace my { namespace rapidjson {
#define RAPIDJSON_NAMESPACE_END } }
#include "rapidjson/..."
\endcode
\see rapidjson
*/
/*! \def RAPIDJSON_NAMESPACE_BEGIN
\ingroup RAPIDJSON_CONFIG
\brief provide custom rapidjson namespace (opening expression)
\see RAPIDJSON_NAMESPACE
*/
/*! \def RAPIDJSON_NAMESPACE_END
\ingroup RAPIDJSON_CONFIG
\brief provide custom rapidjson namespace (closing expression)
\see RAPIDJSON_NAMESPACE
*/
#ifndef RAPIDJSON_NAMESPACE
#define RAPIDJSON_NAMESPACE rapidjson
#endif
#ifndef RAPIDJSON_NAMESPACE_BEGIN
#define RAPIDJSON_NAMESPACE_BEGIN namespace RAPIDJSON_NAMESPACE {
#endif
#ifndef RAPIDJSON_NAMESPACE_END
#define RAPIDJSON_NAMESPACE_END }
#endif
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_HAS_STDSTRING
#ifndef RAPIDJSON_HAS_STDSTRING
#ifdef RAPIDJSON_DOXYGEN_RUNNING
#define RAPIDJSON_HAS_STDSTRING 1 // force generation of documentation
#else
#define RAPIDJSON_HAS_STDSTRING 0 // no std::string support by default
#endif
/*! \def RAPIDJSON_HAS_STDSTRING
\ingroup RAPIDJSON_CONFIG
\brief Enable RapidJSON support for \c std::string
By defining this preprocessor symbol to \c 1, several convenience functions for using
\ref rapidjson::GenericValue with \c std::string are enabled, especially
for construction and comparison.
\hideinitializer
*/
#endif // !defined(RAPIDJSON_HAS_STDSTRING)
#if RAPIDJSON_HAS_STDSTRING
#include <string>
#endif // RAPIDJSON_HAS_STDSTRING
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_NO_INT64DEFINE
/*! \def RAPIDJSON_NO_INT64DEFINE
\ingroup RAPIDJSON_CONFIG
\brief Use external 64-bit integer types.
RapidJSON requires the 64-bit integer types \c int64_t and \c uint64_t types
to be available at global scope.
If users have their own definition, define RAPIDJSON_NO_INT64DEFINE to
prevent RapidJSON from defining its own types.
*/
#ifndef RAPIDJSON_NO_INT64DEFINE
//!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN
#if defined(_MSC_VER) && (_MSC_VER < 1800) // Visual Studio 2013
#include "msinttypes/stdint.h"
#include "msinttypes/inttypes.h"
#else
// Other compilers should have this.
#include <stdint.h>
#include <inttypes.h>
#endif
//!@endcond
#ifdef RAPIDJSON_DOXYGEN_RUNNING
#define RAPIDJSON_NO_INT64DEFINE
#endif
#endif // RAPIDJSON_NO_INT64TYPEDEF
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_FORCEINLINE
#ifndef RAPIDJSON_FORCEINLINE
//!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN
#if defined(_MSC_VER) && defined(NDEBUG)
#define RAPIDJSON_FORCEINLINE __forceinline
#elif defined(__GNUC__) && __GNUC__ >= 4 && defined(NDEBUG)
#define RAPIDJSON_FORCEINLINE __attribute__((always_inline))
#else
#define RAPIDJSON_FORCEINLINE
#endif
//!@endcond
#endif // RAPIDJSON_FORCEINLINE
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_ENDIAN
#define RAPIDJSON_LITTLEENDIAN 0 //!< Little endian machine
#define RAPIDJSON_BIGENDIAN 1 //!< Big endian machine
//! Endianness of the machine.
/*!
\def RAPIDJSON_ENDIAN
\ingroup RAPIDJSON_CONFIG
GCC 4.6 provided macro for detecting endianness of the target machine. But other
compilers may not have this. User can define RAPIDJSON_ENDIAN to either
\ref RAPIDJSON_LITTLEENDIAN or \ref RAPIDJSON_BIGENDIAN.
Default detection implemented with reference to
\li https://gcc.gnu.org/onlinedocs/gcc-4.6.0/cpp/Common-Predefined-Macros.html
\li http://www.boost.org/doc/libs/1_42_0/boost/detail/endian.hpp
*/
#ifndef RAPIDJSON_ENDIAN
// Detect with GCC 4.6's macro
# ifdef __BYTE_ORDER__
# if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__
# define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN
# elif __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__
# define RAPIDJSON_ENDIAN RAPIDJSON_BIGENDIAN
# else
# error Unknown machine endianness detected. User needs to define RAPIDJSON_ENDIAN.
# endif // __BYTE_ORDER__
// Detect with GLIBC's endian.h
# elif defined(__GLIBC__)
# include <endian.h>
# if (__BYTE_ORDER == __LITTLE_ENDIAN)
# define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN
# elif (__BYTE_ORDER == __BIG_ENDIAN)
# define RAPIDJSON_ENDIAN RAPIDJSON_BIGENDIAN
# else
# error Unknown machine endianness detected. User needs to define RAPIDJSON_ENDIAN.
# endif // __GLIBC__
// Detect with _LITTLE_ENDIAN and _BIG_ENDIAN macro
# elif defined(_LITTLE_ENDIAN) && !defined(_BIG_ENDIAN)
# define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN
# elif defined(_BIG_ENDIAN) && !defined(_LITTLE_ENDIAN)
# define RAPIDJSON_ENDIAN RAPIDJSON_BIGENDIAN
// Detect with architecture macros
# elif defined(__sparc) || defined(__sparc__) || defined(_POWER) || defined(__powerpc__) || defined(__ppc__) || defined(__hpux) || defined(__hppa) || defined(_MIPSEB) || defined(_POWER) || defined(__s390__)
# define RAPIDJSON_ENDIAN RAPIDJSON_BIGENDIAN
# elif defined(__i386__) || defined(__alpha__) || defined(__ia64) || defined(__ia64__) || defined(_M_IX86) || defined(_M_IA64) || defined(_M_ALPHA) || defined(__amd64) || defined(__amd64__) || defined(_M_AMD64) || defined(__x86_64) || defined(__x86_64__) || defined(_M_X64) || defined(__bfin__)
# define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN
# elif defined(_MSC_VER) && (defined(_M_ARM) || defined(_M_ARM64))
# define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN
# elif defined(RAPIDJSON_DOXYGEN_RUNNING)
# define RAPIDJSON_ENDIAN
# else
# error Unknown machine endianness detected. User needs to define RAPIDJSON_ENDIAN.
# endif
#endif // RAPIDJSON_ENDIAN
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_64BIT
//! Whether using 64-bit architecture
#ifndef RAPIDJSON_64BIT
#if defined(__LP64__) || (defined(__x86_64__) && defined(__ILP32__)) || defined(_WIN64) || defined(__EMSCRIPTEN__)
#define RAPIDJSON_64BIT 1
#else
#define RAPIDJSON_64BIT 0
#endif
#endif // RAPIDJSON_64BIT
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_ALIGN
//! Data alignment of the machine.
/*! \ingroup RAPIDJSON_CONFIG
\param x pointer to align
Some machines require strict data alignment. The default is 8 bytes.
User can customize by defining the RAPIDJSON_ALIGN function macro.
*/
#ifndef RAPIDJSON_ALIGN
#define RAPIDJSON_ALIGN(x) (((x) + static_cast<size_t>(7u)) & ~static_cast<size_t>(7u))
#endif
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_UINT64_C2
//! Construct a 64-bit literal by a pair of 32-bit integer.
/*!
64-bit literal with or without ULL suffix is prone to compiler warnings.
UINT64_C() is C macro which cause compilation problems.
Use this macro to define 64-bit constants by a pair of 32-bit integer.
*/
#ifndef RAPIDJSON_UINT64_C2
#define RAPIDJSON_UINT64_C2(high32, low32) ((static_cast<uint64_t>(high32) << 32) | static_cast<uint64_t>(low32))
#endif
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_48BITPOINTER_OPTIMIZATION
//! Use only lower 48-bit address for some pointers.
/*!
\ingroup RAPIDJSON_CONFIG
This optimization uses the fact that current X86-64 architecture only implement lower 48-bit virtual address.
The higher 16-bit can be used for storing other data.
\c GenericValue uses this optimization to reduce its size form 24 bytes to 16 bytes in 64-bit architecture.
*/
#ifndef RAPIDJSON_48BITPOINTER_OPTIMIZATION
#if defined(__amd64__) || defined(__amd64) || defined(__x86_64__) || defined(__x86_64) || defined(_M_X64) || defined(_M_AMD64)
#define RAPIDJSON_48BITPOINTER_OPTIMIZATION 1
#else
#define RAPIDJSON_48BITPOINTER_OPTIMIZATION 0
#endif
#endif // RAPIDJSON_48BITPOINTER_OPTIMIZATION
#if RAPIDJSON_48BITPOINTER_OPTIMIZATION == 1
#if RAPIDJSON_64BIT != 1
#error RAPIDJSON_48BITPOINTER_OPTIMIZATION can only be set to 1 when RAPIDJSON_64BIT=1
#endif
#define RAPIDJSON_SETPOINTER(type, p, x) (p = reinterpret_cast<type *>((reinterpret_cast<uintptr_t>(p) & static_cast<uintptr_t>(RAPIDJSON_UINT64_C2(0xFFFF0000, 0x00000000))) | reinterpret_cast<uintptr_t>(reinterpret_cast<const void*>(x))))
#define RAPIDJSON_GETPOINTER(type, p) (reinterpret_cast<type *>(reinterpret_cast<uintptr_t>(p) & static_cast<uintptr_t>(RAPIDJSON_UINT64_C2(0x0000FFFF, 0xFFFFFFFF))))
#else
#define RAPIDJSON_SETPOINTER(type, p, x) (p = (x))
#define RAPIDJSON_GETPOINTER(type, p) (p)
#endif
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_SSE2/RAPIDJSON_SSE42/RAPIDJSON_NEON/RAPIDJSON_SIMD
/*! \def RAPIDJSON_SIMD
\ingroup RAPIDJSON_CONFIG
\brief Enable SSE2/SSE4.2/Neon optimization.
RapidJSON supports optimized implementations for some parsing operations
based on the SSE2, SSE4.2 or NEon SIMD extensions on modern Intel
or ARM compatible processors.
To enable these optimizations, three different symbols can be defined;
\code
// Enable SSE2 optimization.
#define RAPIDJSON_SSE2
// Enable SSE4.2 optimization.
#define RAPIDJSON_SSE42
\endcode
// Enable ARM Neon optimization.
#define RAPIDJSON_NEON
\endcode
\c RAPIDJSON_SSE42 takes precedence over SSE2, if both are defined.
If any of these symbols is defined, RapidJSON defines the macro
\c RAPIDJSON_SIMD to indicate the availability of the optimized code.
*/
#if defined(RAPIDJSON_SSE2) || defined(RAPIDJSON_SSE42) \
|| defined(RAPIDJSON_NEON) || defined(RAPIDJSON_DOXYGEN_RUNNING)
#define RAPIDJSON_SIMD
#endif
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_NO_SIZETYPEDEFINE
#ifndef RAPIDJSON_NO_SIZETYPEDEFINE
/*! \def RAPIDJSON_NO_SIZETYPEDEFINE
\ingroup RAPIDJSON_CONFIG
\brief User-provided \c SizeType definition.
In order to avoid using 32-bit size types for indexing strings and arrays,
define this preprocessor symbol and provide the type rapidjson::SizeType
before including RapidJSON:
\code
#define RAPIDJSON_NO_SIZETYPEDEFINE
namespace rapidjson { typedef ::std::size_t SizeType; }
#include "rapidjson/..."
\endcode
\see rapidjson::SizeType
*/
#ifdef RAPIDJSON_DOXYGEN_RUNNING
#define RAPIDJSON_NO_SIZETYPEDEFINE
#endif
RAPIDJSON_NAMESPACE_BEGIN
//! Size type (for string lengths, array sizes, etc.)
/*! RapidJSON uses 32-bit array/string indices even on 64-bit platforms,
instead of using \c size_t. Users may override the SizeType by defining
\ref RAPIDJSON_NO_SIZETYPEDEFINE.
*/
typedef unsigned SizeType;
RAPIDJSON_NAMESPACE_END
#endif
// always import std::size_t to rapidjson namespace
RAPIDJSON_NAMESPACE_BEGIN
using std::size_t;
RAPIDJSON_NAMESPACE_END
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_ASSERT
//! Assertion.
/*! \ingroup RAPIDJSON_CONFIG
By default, rapidjson uses C \c assert() for internal assertions.
User can override it by defining RAPIDJSON_ASSERT(x) macro.
\note Parsing errors are handled and can be customized by the
\ref RAPIDJSON_ERRORS APIs.
*/
#ifndef RAPIDJSON_ASSERT
#include <cassert>
#define RAPIDJSON_ASSERT(x) assert(x)
#endif // RAPIDJSON_ASSERT
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_STATIC_ASSERT
// Prefer C++11 static_assert, if available
#ifndef RAPIDJSON_STATIC_ASSERT
#if __cplusplus >= 201103L || ( defined(_MSC_VER) && _MSC_VER >= 1800 )
#define RAPIDJSON_STATIC_ASSERT(x) \
static_assert(x, RAPIDJSON_STRINGIFY(x))
#endif // C++11
#endif // RAPIDJSON_STATIC_ASSERT
// Adopt C++03 implementation from boost
#ifndef RAPIDJSON_STATIC_ASSERT
#ifndef __clang__
//!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN
#endif
RAPIDJSON_NAMESPACE_BEGIN
template <bool x> struct STATIC_ASSERTION_FAILURE;
template <> struct STATIC_ASSERTION_FAILURE<true> { enum { value = 1 }; };
template <size_t x> struct StaticAssertTest {};
RAPIDJSON_NAMESPACE_END
#if defined(__GNUC__) || defined(__clang__)
#define RAPIDJSON_STATIC_ASSERT_UNUSED_ATTRIBUTE __attribute__((unused))
#else
#define RAPIDJSON_STATIC_ASSERT_UNUSED_ATTRIBUTE
#endif
#ifndef __clang__
//!@endcond
#endif
/*! \def RAPIDJSON_STATIC_ASSERT
\brief (Internal) macro to check for conditions at compile-time
\param x compile-time condition
\hideinitializer
*/
#define RAPIDJSON_STATIC_ASSERT(x) \
typedef ::RAPIDJSON_NAMESPACE::StaticAssertTest< \
sizeof(::RAPIDJSON_NAMESPACE::STATIC_ASSERTION_FAILURE<bool(x) >)> \
RAPIDJSON_JOIN(StaticAssertTypedef, __LINE__) RAPIDJSON_STATIC_ASSERT_UNUSED_ATTRIBUTE
#endif // RAPIDJSON_STATIC_ASSERT
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_LIKELY, RAPIDJSON_UNLIKELY
//! Compiler branching hint for expression with high probability to be true.
/*!
\ingroup RAPIDJSON_CONFIG
\param x Boolean expression likely to be true.
*/
#ifndef RAPIDJSON_LIKELY
#if defined(__GNUC__) || defined(__clang__)
#define RAPIDJSON_LIKELY(x) __builtin_expect(!!(x), 1)
#else
#define RAPIDJSON_LIKELY(x) (x)
#endif
#endif
//! Compiler branching hint for expression with low probability to be true.
/*!
\ingroup RAPIDJSON_CONFIG
\param x Boolean expression unlikely to be true.
*/
#ifndef RAPIDJSON_UNLIKELY
#if defined(__GNUC__) || defined(__clang__)
#define RAPIDJSON_UNLIKELY(x) __builtin_expect(!!(x), 0)
#else
#define RAPIDJSON_UNLIKELY(x) (x)
#endif
#endif
///////////////////////////////////////////////////////////////////////////////
// Helpers
//!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN
#define RAPIDJSON_MULTILINEMACRO_BEGIN do {
#define RAPIDJSON_MULTILINEMACRO_END \
} while((void)0, 0)
// adopted from Boost
#define RAPIDJSON_VERSION_CODE(x,y,z) \
(((x)*100000) + ((y)*100) + (z))
#if defined(__has_builtin)
#define RAPIDJSON_HAS_BUILTIN(x) __has_builtin(x)
#else
#define RAPIDJSON_HAS_BUILTIN(x) 0
#endif
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_DIAG_PUSH/POP, RAPIDJSON_DIAG_OFF
#if defined(__GNUC__)
#define RAPIDJSON_GNUC \
RAPIDJSON_VERSION_CODE(__GNUC__,__GNUC_MINOR__,__GNUC_PATCHLEVEL__)
#endif
#if defined(__clang__) || (defined(RAPIDJSON_GNUC) && RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4,2,0))
#define RAPIDJSON_PRAGMA(x) _Pragma(RAPIDJSON_STRINGIFY(x))
#define RAPIDJSON_DIAG_PRAGMA(x) RAPIDJSON_PRAGMA(GCC diagnostic x)
#define RAPIDJSON_DIAG_OFF(x) \
RAPIDJSON_DIAG_PRAGMA(ignored RAPIDJSON_STRINGIFY(RAPIDJSON_JOIN(-W,x)))
// push/pop support in Clang and GCC>=4.6
#if defined(__clang__) || (defined(RAPIDJSON_GNUC) && RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4,6,0))
#define RAPIDJSON_DIAG_PUSH RAPIDJSON_DIAG_PRAGMA(push)
#define RAPIDJSON_DIAG_POP RAPIDJSON_DIAG_PRAGMA(pop)
#else // GCC >= 4.2, < 4.6
#define RAPIDJSON_DIAG_PUSH /* ignored */
#define RAPIDJSON_DIAG_POP /* ignored */
#endif
#elif defined(_MSC_VER)
// pragma (MSVC specific)
#define RAPIDJSON_PRAGMA(x) __pragma(x)
#define RAPIDJSON_DIAG_PRAGMA(x) RAPIDJSON_PRAGMA(warning(x))
#define RAPIDJSON_DIAG_OFF(x) RAPIDJSON_DIAG_PRAGMA(disable: x)
#define RAPIDJSON_DIAG_PUSH RAPIDJSON_DIAG_PRAGMA(push)
#define RAPIDJSON_DIAG_POP RAPIDJSON_DIAG_PRAGMA(pop)
#else
#define RAPIDJSON_DIAG_OFF(x) /* ignored */
#define RAPIDJSON_DIAG_PUSH /* ignored */
#define RAPIDJSON_DIAG_POP /* ignored */
#endif // RAPIDJSON_DIAG_*
///////////////////////////////////////////////////////////////////////////////
// C++11 features
#ifndef RAPIDJSON_HAS_CXX11_RVALUE_REFS
#if defined(__clang__)
#if __has_feature(cxx_rvalue_references) && \
(defined(_MSC_VER) || defined(_LIBCPP_VERSION) || defined(__GLIBCXX__) && __GLIBCXX__ >= 20080306)
#define RAPIDJSON_HAS_CXX11_RVALUE_REFS 1
#else
#define RAPIDJSON_HAS_CXX11_RVALUE_REFS 0
#endif
#elif (defined(RAPIDJSON_GNUC) && (RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4,3,0)) && defined(__GXX_EXPERIMENTAL_CXX0X__)) || \
(defined(_MSC_VER) && _MSC_VER >= 1600) || \
(defined(__SUNPRO_CC) && __SUNPRO_CC >= 0x5140 && defined(__GXX_EXPERIMENTAL_CXX0X__))
#define RAPIDJSON_HAS_CXX11_RVALUE_REFS 1
#else
#define RAPIDJSON_HAS_CXX11_RVALUE_REFS 0
#endif
#endif // RAPIDJSON_HAS_CXX11_RVALUE_REFS
#ifndef RAPIDJSON_HAS_CXX11_NOEXCEPT
#if defined(__clang__)
#define RAPIDJSON_HAS_CXX11_NOEXCEPT __has_feature(cxx_noexcept)
#elif (defined(RAPIDJSON_GNUC) && (RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4,6,0)) && defined(__GXX_EXPERIMENTAL_CXX0X__)) || \
(defined(_MSC_VER) && _MSC_VER >= 1900) || \
(defined(__SUNPRO_CC) && __SUNPRO_CC >= 0x5140 && defined(__GXX_EXPERIMENTAL_CXX0X__))
#define RAPIDJSON_HAS_CXX11_NOEXCEPT 1
#else
#define RAPIDJSON_HAS_CXX11_NOEXCEPT 0
#endif
#endif
#if RAPIDJSON_HAS_CXX11_NOEXCEPT
#define RAPIDJSON_NOEXCEPT noexcept
#else
#define RAPIDJSON_NOEXCEPT /* noexcept */
#endif // RAPIDJSON_HAS_CXX11_NOEXCEPT
// no automatic detection, yet
#ifndef RAPIDJSON_HAS_CXX11_TYPETRAITS
#if (defined(_MSC_VER) && _MSC_VER >= 1700)
#define RAPIDJSON_HAS_CXX11_TYPETRAITS 1
#else
#define RAPIDJSON_HAS_CXX11_TYPETRAITS 0
#endif
#endif
#ifndef RAPIDJSON_HAS_CXX11_RANGE_FOR
#if defined(__clang__)
#define RAPIDJSON_HAS_CXX11_RANGE_FOR __has_feature(cxx_range_for)
#elif (defined(RAPIDJSON_GNUC) && (RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4,6,0)) && defined(__GXX_EXPERIMENTAL_CXX0X__)) || \
(defined(_MSC_VER) && _MSC_VER >= 1700) || \
(defined(__SUNPRO_CC) && __SUNPRO_CC >= 0x5140 && defined(__GXX_EXPERIMENTAL_CXX0X__))
#define RAPIDJSON_HAS_CXX11_RANGE_FOR 1
#else
#define RAPIDJSON_HAS_CXX11_RANGE_FOR 0
#endif
#endif // RAPIDJSON_HAS_CXX11_RANGE_FOR
///////////////////////////////////////////////////////////////////////////////
// C++17 features
#if defined(__has_cpp_attribute)
# if __has_cpp_attribute(fallthrough)
# define RAPIDJSON_DELIBERATE_FALLTHROUGH [[fallthrough]]
# else
# define RAPIDJSON_DELIBERATE_FALLTHROUGH
# endif
#else
# define RAPIDJSON_DELIBERATE_FALLTHROUGH
#endif
//!@endcond
//! Assertion (in non-throwing contexts).
/*! \ingroup RAPIDJSON_CONFIG
Some functions provide a \c noexcept guarantee, if the compiler supports it.
In these cases, the \ref RAPIDJSON_ASSERT macro cannot be overridden to
throw an exception. This macro adds a separate customization point for
such cases.
Defaults to C \c assert() (as \ref RAPIDJSON_ASSERT), if \c noexcept is
supported, and to \ref RAPIDJSON_ASSERT otherwise.
*/
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_NOEXCEPT_ASSERT
#ifndef RAPIDJSON_NOEXCEPT_ASSERT
#ifdef RAPIDJSON_ASSERT_THROWS
#if RAPIDJSON_HAS_CXX11_NOEXCEPT
#define RAPIDJSON_NOEXCEPT_ASSERT(x)
#else
#define RAPIDJSON_NOEXCEPT_ASSERT(x) RAPIDJSON_ASSERT(x)
#endif // RAPIDJSON_HAS_CXX11_NOEXCEPT
#else
#define RAPIDJSON_NOEXCEPT_ASSERT(x) RAPIDJSON_ASSERT(x)
#endif // RAPIDJSON_ASSERT_THROWS
#endif // RAPIDJSON_NOEXCEPT_ASSERT
///////////////////////////////////////////////////////////////////////////////
// new/delete
#ifndef RAPIDJSON_NEW
///! customization point for global \c new
#define RAPIDJSON_NEW(TypeName) new TypeName
#endif
#ifndef RAPIDJSON_DELETE
///! customization point for global \c delete
#define RAPIDJSON_DELETE(x) delete x
#endif
///////////////////////////////////////////////////////////////////////////////
// Type
/*! \namespace rapidjson
\brief main RapidJSON namespace
\see RAPIDJSON_NAMESPACE
*/
RAPIDJSON_NAMESPACE_BEGIN
//! Type of JSON value
enum Type {
kNullType = 0, //!< null
kFalseType = 1, //!< false
kTrueType = 2, //!< true
kObjectType = 3, //!< object
kArrayType = 4, //!< array
kStringType = 5, //!< string
kNumberType = 6 //!< number
};
RAPIDJSON_NAMESPACE_END
#endif // RAPIDJSON_RAPIDJSON_H_

2231
livox_ros_driver/common/rapidjson/reader.h Normal file
View File

File diff suppressed because it is too large Load Diff

2497
livox_ros_driver/common/rapidjson/schema.h Normal file
View File

File diff suppressed because it is too large Load Diff

223
livox_ros_driver/common/rapidjson/stream.h Normal file
View File

@@ -0,0 +1,223 @@
// Tencent is pleased to support the open source community by making RapidJSON available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
#include "rapidjson.h"
#ifndef RAPIDJSON_STREAM_H_
#define RAPIDJSON_STREAM_H_
#include "encodings.h"
RAPIDJSON_NAMESPACE_BEGIN
///////////////////////////////////////////////////////////////////////////////
// Stream
/*! \class rapidjson::Stream
\brief Concept for reading and writing characters.
For read-only stream, no need to implement PutBegin(), Put(), Flush() and PutEnd().
For write-only stream, only need to implement Put() and Flush().
\code
concept Stream {
typename Ch; //!< Character type of the stream.
//! Read the current character from stream without moving the read cursor.
Ch Peek() const;
//! Read the current character from stream and moving the read cursor to next character.
Ch Take();
//! Get the current read cursor.
//! \return Number of characters read from start.
size_t Tell();
//! Begin writing operation at the current read pointer.
//! \return The begin writer pointer.
Ch* PutBegin();
//! Write a character.
void Put(Ch c);
//! Flush the buffer.
void Flush();
//! End the writing operation.
//! \param begin The begin write pointer returned by PutBegin().
//! \return Number of characters written.
size_t PutEnd(Ch* begin);
}
\endcode
*/
//! Provides additional information for stream.
/*!
By using traits pattern, this type provides a default configuration for stream.
For custom stream, this type can be specialized for other configuration.
See TEST(Reader, CustomStringStream) in readertest.cpp for example.
*/
template<typename Stream>
struct StreamTraits {
//! Whether to make local copy of stream for optimization during parsing.
/*!
By default, for safety, streams do not use local copy optimization.
Stream that can be copied fast should specialize this, like StreamTraits<StringStream>.
*/
enum { copyOptimization = 0 };
};
//! Reserve n characters for writing to a stream.
template<typename Stream>
inline void PutReserve(Stream& stream, size_t count) {
(void)stream;
(void)count;
}
//! Write character to a stream, presuming buffer is reserved.
template<typename Stream>
inline void PutUnsafe(Stream& stream, typename Stream::Ch c) {
stream.Put(c);
}
//! Put N copies of a character to a stream.
template<typename Stream, typename Ch>
inline void PutN(Stream& stream, Ch c, size_t n) {
PutReserve(stream, n);
for (size_t i = 0; i < n; i++)
PutUnsafe(stream, c);
}
///////////////////////////////////////////////////////////////////////////////
// GenericStreamWrapper
//! A Stream Wrapper
/*! \tThis string stream is a wrapper for any stream by just forwarding any
\treceived message to the origin stream.
\note implements Stream concept
*/
#if defined(_MSC_VER) && _MSC_VER <= 1800
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(4702) // unreachable code
RAPIDJSON_DIAG_OFF(4512) // assignment operator could not be generated
#endif
template <typename InputStream, typename Encoding = UTF8<> >
class GenericStreamWrapper {
public:
typedef typename Encoding::Ch Ch;
GenericStreamWrapper(InputStream& is): is_(is) {}
Ch Peek() const { return is_.Peek(); }
Ch Take() { return is_.Take(); }
size_t Tell() { return is_.Tell(); }
Ch* PutBegin() { return is_.PutBegin(); }
void Put(Ch ch) { is_.Put(ch); }
void Flush() { is_.Flush(); }
size_t PutEnd(Ch* ch) { return is_.PutEnd(ch); }
// wrapper for MemoryStream
const Ch* Peek4() const { return is_.Peek4(); }
// wrapper for AutoUTFInputStream
UTFType GetType() const { return is_.GetType(); }
bool HasBOM() const { return is_.HasBOM(); }
protected:
InputStream& is_;
};
#if defined(_MSC_VER) && _MSC_VER <= 1800
RAPIDJSON_DIAG_POP
#endif
///////////////////////////////////////////////////////////////////////////////
// StringStream
//! Read-only string stream.
/*! \note implements Stream concept
*/
template <typename Encoding>
struct GenericStringStream {
typedef typename Encoding::Ch Ch;
GenericStringStream(const Ch *src) : src_(src), head_(src) {}
Ch Peek() const { return *src_; }
Ch Take() { return *src_++; }
size_t Tell() const { return static_cast<size_t>(src_ - head_); }
Ch* PutBegin() { RAPIDJSON_ASSERT(false); return 0; }
void Put(Ch) { RAPIDJSON_ASSERT(false); }
void Flush() { RAPIDJSON_ASSERT(false); }
size_t PutEnd(Ch*) { RAPIDJSON_ASSERT(false); return 0; }
const Ch* src_; //!< Current read position.
const Ch* head_; //!< Original head of the string.
};
template <typename Encoding>
struct StreamTraits<GenericStringStream<Encoding> > {
enum { copyOptimization = 1 };
};
//! String stream with UTF8 encoding.
typedef GenericStringStream<UTF8<> > StringStream;
///////////////////////////////////////////////////////////////////////////////
// InsituStringStream
//! A read-write string stream.
/*! This string stream is particularly designed for in-situ parsing.
\note implements Stream concept
*/
template <typename Encoding>
struct GenericInsituStringStream {
typedef typename Encoding::Ch Ch;
GenericInsituStringStream(Ch *src) : src_(src), dst_(0), head_(src) {}
// Read
Ch Peek() { return *src_; }
Ch Take() { return *src_++; }
size_t Tell() { return static_cast<size_t>(src_ - head_); }
// Write
void Put(Ch c) { RAPIDJSON_ASSERT(dst_ != 0); *dst_++ = c; }
Ch* PutBegin() { return dst_ = src_; }
size_t PutEnd(Ch* begin) { return static_cast<size_t>(dst_ - begin); }
void Flush() {}
Ch* Push(size_t count) { Ch* begin = dst_; dst_ += count; return begin; }
void Pop(size_t count) { dst_ -= count; }
Ch* src_;
Ch* dst_;
Ch* head_;
};
template <typename Encoding>
struct StreamTraits<GenericInsituStringStream<Encoding> > {
enum { copyOptimization = 1 };
};
//! Insitu string stream with UTF8 encoding.
typedef GenericInsituStringStream<UTF8<> > InsituStringStream;
RAPIDJSON_NAMESPACE_END
#endif // RAPIDJSON_STREAM_H_

121
livox_ros_driver/common/rapidjson/stringbuffer.h Normal file
View File

@@ -0,0 +1,121 @@
// Tencent is pleased to support the open source community by making RapidJSON available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
#ifndef RAPIDJSON_STRINGBUFFER_H_
#define RAPIDJSON_STRINGBUFFER_H_
#include "stream.h"
#include "internal/stack.h"
#if RAPIDJSON_HAS_CXX11_RVALUE_REFS
#include <utility> // std::move
#endif
#include "internal/stack.h"
#if defined(__clang__)
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(c++98-compat)
#endif
RAPIDJSON_NAMESPACE_BEGIN
//! Represents an in-memory output stream.
/*!
\tparam Encoding Encoding of the stream.
\tparam Allocator type for allocating memory buffer.
\note implements Stream concept
*/
template <typename Encoding, typename Allocator = CrtAllocator>
class GenericStringBuffer {
public:
typedef typename Encoding::Ch Ch;
GenericStringBuffer(Allocator* allocator = 0, size_t capacity = kDefaultCapacity) : stack_(allocator, capacity) {}
#if RAPIDJSON_HAS_CXX11_RVALUE_REFS
GenericStringBuffer(GenericStringBuffer&& rhs) : stack_(std::move(rhs.stack_)) {}
GenericStringBuffer& operator=(GenericStringBuffer&& rhs) {
if (&rhs != this)
stack_ = std::move(rhs.stack_);
return *this;
}
#endif
void Put(Ch c) { *stack_.template Push<Ch>() = c; }
void PutUnsafe(Ch c) { *stack_.template PushUnsafe<Ch>() = c; }
void Flush() {}
void Clear() { stack_.Clear(); }
void ShrinkToFit() {
// Push and pop a null terminator. This is safe.
*stack_.template Push<Ch>() = '\0';
stack_.ShrinkToFit();
stack_.template Pop<Ch>(1);
}
void Reserve(size_t count) { stack_.template Reserve<Ch>(count); }
Ch* Push(size_t count) { return stack_.template Push<Ch>(count); }
Ch* PushUnsafe(size_t count) { return stack_.template PushUnsafe<Ch>(count); }
void Pop(size_t count) { stack_.template Pop<Ch>(count); }
const Ch* GetString() const {
// Push and pop a null terminator. This is safe.
*stack_.template Push<Ch>() = '\0';
stack_.template Pop<Ch>(1);
return stack_.template Bottom<Ch>();
}
//! Get the size of string in bytes in the string buffer.
size_t GetSize() const { return stack_.GetSize(); }
//! Get the length of string in Ch in the string buffer.
size_t GetLength() const { return stack_.GetSize() / sizeof(Ch); }
static const size_t kDefaultCapacity = 256;
mutable internal::Stack<Allocator> stack_;
private:
// Prohibit copy constructor & assignment operator.
GenericStringBuffer(const GenericStringBuffer&);
GenericStringBuffer& operator=(const GenericStringBuffer&);
};
//! String buffer with UTF8 encoding
typedef GenericStringBuffer<UTF8<> > StringBuffer;
template<typename Encoding, typename Allocator>
inline void PutReserve(GenericStringBuffer<Encoding, Allocator>& stream, size_t count) {
stream.Reserve(count);
}
template<typename Encoding, typename Allocator>
inline void PutUnsafe(GenericStringBuffer<Encoding, Allocator>& stream, typename Encoding::Ch c) {
stream.PutUnsafe(c);
}
//! Implement specialized version of PutN() with memset() for better performance.
template<>
inline void PutN(GenericStringBuffer<UTF8<> >& stream, char c, size_t n) {
std::memset(stream.stack_.Push<char>(n), c, n * sizeof(c));
}
RAPIDJSON_NAMESPACE_END
#if defined(__clang__)
RAPIDJSON_DIAG_POP
#endif
#endif // RAPIDJSON_STRINGBUFFER_H_

710
livox_ros_driver/common/rapidjson/writer.h Normal file
View File

@@ -0,0 +1,710 @@
// Tencent is pleased to support the open source community by making RapidJSON available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
#ifndef RAPIDJSON_WRITER_H_
#define RAPIDJSON_WRITER_H_
#include "stream.h"
#include "internal/clzll.h"
#include "internal/meta.h"
#include "internal/stack.h"
#include "internal/strfunc.h"
#include "internal/dtoa.h"
#include "internal/itoa.h"
#include "stringbuffer.h"
#include <new> // placement new
#if defined(RAPIDJSON_SIMD) && defined(_MSC_VER)
#include <intrin.h>
#pragma intrinsic(_BitScanForward)
#endif
#ifdef RAPIDJSON_SSE42
#include <nmmintrin.h>
#elif defined(RAPIDJSON_SSE2)
#include <emmintrin.h>
#elif defined(RAPIDJSON_NEON)
#include <arm_neon.h>
#endif
#ifdef __clang__
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(padded)
RAPIDJSON_DIAG_OFF(unreachable-code)
RAPIDJSON_DIAG_OFF(c++98-compat)
#elif defined(_MSC_VER)
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(4127) // conditional expression is constant
#endif
RAPIDJSON_NAMESPACE_BEGIN
///////////////////////////////////////////////////////////////////////////////
// WriteFlag
/*! \def RAPIDJSON_WRITE_DEFAULT_FLAGS
\ingroup RAPIDJSON_CONFIG
\brief User-defined kWriteDefaultFlags definition.
User can define this as any \c WriteFlag combinations.
*/
#ifndef RAPIDJSON_WRITE_DEFAULT_FLAGS
#define RAPIDJSON_WRITE_DEFAULT_FLAGS kWriteNoFlags
#endif
//! Combination of writeFlags
enum WriteFlag {
kWriteNoFlags = 0, //!< No flags are set.
kWriteValidateEncodingFlag = 1, //!< Validate encoding of JSON strings.
kWriteNanAndInfFlag = 2, //!< Allow writing of Infinity, -Infinity and NaN.
kWriteDefaultFlags = RAPIDJSON_WRITE_DEFAULT_FLAGS //!< Default write flags. Can be customized by defining RAPIDJSON_WRITE_DEFAULT_FLAGS
};
//! JSON writer
/*! Writer implements the concept Handler.
It generates JSON text by events to an output os.
User may programmatically calls the functions of a writer to generate JSON text.
On the other side, a writer can also be passed to objects that generates events,
for example Reader::Parse() and Document::Accept().
\tparam OutputStream Type of output stream.
\tparam SourceEncoding Encoding of source string.
\tparam TargetEncoding Encoding of output stream.
\tparam StackAllocator Type of allocator for allocating memory of stack.
\note implements Handler concept
*/
template<typename OutputStream, typename SourceEncoding = UTF8<>, typename TargetEncoding = UTF8<>, typename StackAllocator = CrtAllocator, unsigned writeFlags = kWriteDefaultFlags>
class Writer {
public:
typedef typename SourceEncoding::Ch Ch;
static const int kDefaultMaxDecimalPlaces = 324;
//! Constructor
/*! \param os Output stream.
\param stackAllocator User supplied allocator. If it is null, it will create a private one.
\param levelDepth Initial capacity of stack.
*/
explicit
Writer(OutputStream& os, StackAllocator* stackAllocator = 0, size_t levelDepth = kDefaultLevelDepth) :
os_(&os), level_stack_(stackAllocator, levelDepth * sizeof(Level)), maxDecimalPlaces_(kDefaultMaxDecimalPlaces), hasRoot_(false) {}
explicit
Writer(StackAllocator* allocator = 0, size_t levelDepth = kDefaultLevelDepth) :
os_(0), level_stack_(allocator, levelDepth * sizeof(Level)), maxDecimalPlaces_(kDefaultMaxDecimalPlaces), hasRoot_(false) {}
#if RAPIDJSON_HAS_CXX11_RVALUE_REFS
Writer(Writer&& rhs) :
os_(rhs.os_), level_stack_(std::move(rhs.level_stack_)), maxDecimalPlaces_(rhs.maxDecimalPlaces_), hasRoot_(rhs.hasRoot_) {
rhs.os_ = 0;
}
#endif
//! Reset the writer with a new stream.
/*!
This function reset the writer with a new stream and default settings,
in order to make a Writer object reusable for output multiple JSONs.
\param os New output stream.
\code
Writer<OutputStream> writer(os1);
writer.StartObject();
// ...
writer.EndObject();
writer.Reset(os2);
writer.StartObject();
// ...
writer.EndObject();
\endcode
*/
void Reset(OutputStream& os) {
os_ = &os;
hasRoot_ = false;
level_stack_.Clear();
}
//! Checks whether the output is a complete JSON.
/*!
A complete JSON has a complete root object or array.
*/
bool IsComplete() const {
return hasRoot_ && level_stack_.Empty();
}
int GetMaxDecimalPlaces() const {
return maxDecimalPlaces_;
}
//! Sets the maximum number of decimal places for double output.
/*!
This setting truncates the output with specified number of decimal places.
For example,
\code
writer.SetMaxDecimalPlaces(3);
writer.StartArray();
writer.Double(0.12345); // "0.123"
writer.Double(0.0001); // "0.0"
writer.Double(1.234567890123456e30); // "1.234567890123456e30" (do not truncate significand for positive exponent)
writer.Double(1.23e-4); // "0.0" (do truncate significand for negative exponent)
writer.EndArray();
\endcode
The default setting does not truncate any decimal places. You can restore to this setting by calling
\code
writer.SetMaxDecimalPlaces(Writer::kDefaultMaxDecimalPlaces);
\endcode
*/
void SetMaxDecimalPlaces(int maxDecimalPlaces) {
maxDecimalPlaces_ = maxDecimalPlaces;
}
/*!@name Implementation of Handler
\see Handler
*/
//@{
bool Null() { Prefix(kNullType); return EndValue(WriteNull()); }
bool Bool(bool b) { Prefix(b ? kTrueType : kFalseType); return EndValue(WriteBool(b)); }
bool Int(int i) { Prefix(kNumberType); return EndValue(WriteInt(i)); }
bool Uint(unsigned u) { Prefix(kNumberType); return EndValue(WriteUint(u)); }
bool Int64(int64_t i64) { Prefix(kNumberType); return EndValue(WriteInt64(i64)); }
bool Uint64(uint64_t u64) { Prefix(kNumberType); return EndValue(WriteUint64(u64)); }
//! Writes the given \c double value to the stream
/*!
\param d The value to be written.
\return Whether it is succeed.
*/
bool Double(double d) { Prefix(kNumberType); return EndValue(WriteDouble(d)); }
bool RawNumber(const Ch* str, SizeType length, bool copy = false) {
RAPIDJSON_ASSERT(str != 0);
(void)copy;
Prefix(kNumberType);
return EndValue(WriteString(str, length));
}
bool String(const Ch* str, SizeType length, bool copy = false) {
RAPIDJSON_ASSERT(str != 0);
(void)copy;
Prefix(kStringType);
return EndValue(WriteString(str, length));
}
#if RAPIDJSON_HAS_STDSTRING
bool String(const std::basic_string<Ch>& str) {
return String(str.data(), SizeType(str.size()));
}
#endif
bool StartObject() {
Prefix(kObjectType);
new (level_stack_.template Push<Level>()) Level(false);
return WriteStartObject();
}
bool Key(const Ch* str, SizeType length, bool copy = false) { return String(str, length, copy); }
#if RAPIDJSON_HAS_STDSTRING
bool Key(const std::basic_string<Ch>& str)
{
return Key(str.data(), SizeType(str.size()));
}
#endif
bool EndObject(SizeType memberCount = 0) {
(void)memberCount;
RAPIDJSON_ASSERT(level_stack_.GetSize() >= sizeof(Level)); // not inside an Object
RAPIDJSON_ASSERT(!level_stack_.template Top<Level>()->inArray); // currently inside an Array, not Object
RAPIDJSON_ASSERT(0 == level_stack_.template Top<Level>()->valueCount % 2); // Object has a Key without a Value
level_stack_.template Pop<Level>(1);
return EndValue(WriteEndObject());
}
bool StartArray() {
Prefix(kArrayType);
new (level_stack_.template Push<Level>()) Level(true);
return WriteStartArray();
}
bool EndArray(SizeType elementCount = 0) {
(void)elementCount;
RAPIDJSON_ASSERT(level_stack_.GetSize() >= sizeof(Level));
RAPIDJSON_ASSERT(level_stack_.template Top<Level>()->inArray);
level_stack_.template Pop<Level>(1);
return EndValue(WriteEndArray());
}
//@}
/*! @name Convenience extensions */
//@{
//! Simpler but slower overload.
bool String(const Ch* const& str) { return String(str, internal::StrLen(str)); }
bool Key(const Ch* const& str) { return Key(str, internal::StrLen(str)); }
//@}
//! Write a raw JSON value.
/*!
For user to write a stringified JSON as a value.
\param json A well-formed JSON value. It should not contain null character within [0, length - 1] range.
\param length Length of the json.
\param type Type of the root of json.
*/
bool RawValue(const Ch* json, size_t length, Type type) {
RAPIDJSON_ASSERT(json != 0);
Prefix(type);
return EndValue(WriteRawValue(json, length));
}
//! Flush the output stream.
/*!
Allows the user to flush the output stream immediately.
*/
void Flush() {
os_->Flush();
}
protected:
//! Information for each nested level
struct Level {
Level(bool inArray_) : valueCount(0), inArray(inArray_) {}
size_t valueCount; //!< number of values in this level
bool inArray; //!< true if in array, otherwise in object
};
static const size_t kDefaultLevelDepth = 32;
bool WriteNull() {
PutReserve(*os_, 4);
PutUnsafe(*os_, 'n'); PutUnsafe(*os_, 'u'); PutUnsafe(*os_, 'l'); PutUnsafe(*os_, 'l'); return true;
}
bool WriteBool(bool b) {
if (b) {
PutReserve(*os_, 4);
PutUnsafe(*os_, 't'); PutUnsafe(*os_, 'r'); PutUnsafe(*os_, 'u'); PutUnsafe(*os_, 'e');
}
else {
PutReserve(*os_, 5);
PutUnsafe(*os_, 'f'); PutUnsafe(*os_, 'a'); PutUnsafe(*os_, 'l'); PutUnsafe(*os_, 's'); PutUnsafe(*os_, 'e');
}
return true;
}
bool WriteInt(int i) {
char buffer[11];
const char* end = internal::i32toa(i, buffer);
PutReserve(*os_, static_cast<size_t>(end - buffer));
for (const char* p = buffer; p != end; ++p)
PutUnsafe(*os_, static_cast<typename OutputStream::Ch>(*p));
return true;
}
bool WriteUint(unsigned u) {
char buffer[10];
const char* end = internal::u32toa(u, buffer);
PutReserve(*os_, static_cast<size_t>(end - buffer));
for (const char* p = buffer; p != end; ++p)
PutUnsafe(*os_, static_cast<typename OutputStream::Ch>(*p));
return true;
}
bool WriteInt64(int64_t i64) {
char buffer[21];
const char* end = internal::i64toa(i64, buffer);
PutReserve(*os_, static_cast<size_t>(end - buffer));
for (const char* p = buffer; p != end; ++p)
PutUnsafe(*os_, static_cast<typename OutputStream::Ch>(*p));
return true;
}
bool WriteUint64(uint64_t u64) {
char buffer[20];
char* end = internal::u64toa(u64, buffer);
PutReserve(*os_, static_cast<size_t>(end - buffer));
for (char* p = buffer; p != end; ++p)
PutUnsafe(*os_, static_cast<typename OutputStream::Ch>(*p));
return true;
}
bool WriteDouble(double d) {
if (internal::Double(d).IsNanOrInf()) {
if (!(writeFlags & kWriteNanAndInfFlag))
return false;
if (internal::Double(d).IsNan()) {
PutReserve(*os_, 3);
PutUnsafe(*os_, 'N'); PutUnsafe(*os_, 'a'); PutUnsafe(*os_, 'N');
return true;
}
if (internal::Double(d).Sign()) {
PutReserve(*os_, 9);
PutUnsafe(*os_, '-');
}
else
PutReserve(*os_, 8);
PutUnsafe(*os_, 'I'); PutUnsafe(*os_, 'n'); PutUnsafe(*os_, 'f');
PutUnsafe(*os_, 'i'); PutUnsafe(*os_, 'n'); PutUnsafe(*os_, 'i'); PutUnsafe(*os_, 't'); PutUnsafe(*os_, 'y');
return true;
}
char buffer[25];
char* end = internal::dtoa(d, buffer, maxDecimalPlaces_);
PutReserve(*os_, static_cast<size_t>(end - buffer));
for (char* p = buffer; p != end; ++p)
PutUnsafe(*os_, static_cast<typename OutputStream::Ch>(*p));
return true;
}
bool WriteString(const Ch* str, SizeType length) {
static const typename OutputStream::Ch hexDigits[16] = { '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F' };
static const char escape[256] = {
#define Z16 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
//0 1 2 3 4 5 6 7 8 9 A B C D E F
'u', 'u', 'u', 'u', 'u', 'u', 'u', 'u', 'b', 't', 'n', 'u', 'f', 'r', 'u', 'u', // 00
'u', 'u', 'u', 'u', 'u', 'u', 'u', 'u', 'u', 'u', 'u', 'u', 'u', 'u', 'u', 'u', // 10
0, 0, '"', 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // 20
Z16, Z16, // 30~4F
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,'\\', 0, 0, 0, // 50
Z16, Z16, Z16, Z16, Z16, Z16, Z16, Z16, Z16, Z16 // 60~FF
#undef Z16
};
if (TargetEncoding::supportUnicode)
PutReserve(*os_, 2 + length * 6); // "\uxxxx..."
else
PutReserve(*os_, 2 + length * 12); // "\uxxxx\uyyyy..."
PutUnsafe(*os_, '\"');
GenericStringStream<SourceEncoding> is(str);
while (ScanWriteUnescapedString(is, length)) {
const Ch c = is.Peek();
if (!TargetEncoding::supportUnicode && static_cast<unsigned>(c) >= 0x80) {
// Unicode escaping
unsigned codepoint;
if (RAPIDJSON_UNLIKELY(!SourceEncoding::Decode(is, &codepoint)))
return false;
PutUnsafe(*os_, '\\');
PutUnsafe(*os_, 'u');
if (codepoint <= 0xD7FF || (codepoint >= 0xE000 && codepoint <= 0xFFFF)) {
PutUnsafe(*os_, hexDigits[(codepoint >> 12) & 15]);
PutUnsafe(*os_, hexDigits[(codepoint >> 8) & 15]);
PutUnsafe(*os_, hexDigits[(codepoint >> 4) & 15]);
PutUnsafe(*os_, hexDigits[(codepoint ) & 15]);
}
else {
RAPIDJSON_ASSERT(codepoint >= 0x010000 && codepoint <= 0x10FFFF);
// Surrogate pair
unsigned s = codepoint - 0x010000;
unsigned lead = (s >> 10) + 0xD800;
unsigned trail = (s & 0x3FF) + 0xDC00;
PutUnsafe(*os_, hexDigits[(lead >> 12) & 15]);
PutUnsafe(*os_, hexDigits[(lead >> 8) & 15]);
PutUnsafe(*os_, hexDigits[(lead >> 4) & 15]);
PutUnsafe(*os_, hexDigits[(lead ) & 15]);
PutUnsafe(*os_, '\\');
PutUnsafe(*os_, 'u');
PutUnsafe(*os_, hexDigits[(trail >> 12) & 15]);
PutUnsafe(*os_, hexDigits[(trail >> 8) & 15]);
PutUnsafe(*os_, hexDigits[(trail >> 4) & 15]);
PutUnsafe(*os_, hexDigits[(trail ) & 15]);
}
}
else if ((sizeof(Ch) == 1 || static_cast<unsigned>(c) < 256) && RAPIDJSON_UNLIKELY(escape[static_cast<unsigned char>(c)])) {
is.Take();
PutUnsafe(*os_, '\\');
PutUnsafe(*os_, static_cast<typename OutputStream::Ch>(escape[static_cast<unsigned char>(c)]));
if (escape[static_cast<unsigned char>(c)] == 'u') {
PutUnsafe(*os_, '0');
PutUnsafe(*os_, '0');
PutUnsafe(*os_, hexDigits[static_cast<unsigned char>(c) >> 4]);
PutUnsafe(*os_, hexDigits[static_cast<unsigned char>(c) & 0xF]);
}
}
else if (RAPIDJSON_UNLIKELY(!(writeFlags & kWriteValidateEncodingFlag ?
Transcoder<SourceEncoding, TargetEncoding>::Validate(is, *os_) :
Transcoder<SourceEncoding, TargetEncoding>::TranscodeUnsafe(is, *os_))))
return false;
}
PutUnsafe(*os_, '\"');
return true;
}
bool ScanWriteUnescapedString(GenericStringStream<SourceEncoding>& is, size_t length) {
return RAPIDJSON_LIKELY(is.Tell() < length);
}
bool WriteStartObject() { os_->Put('{'); return true; }
bool WriteEndObject() { os_->Put('}'); return true; }
bool WriteStartArray() { os_->Put('['); return true; }
bool WriteEndArray() { os_->Put(']'); return true; }
bool WriteRawValue(const Ch* json, size_t length) {
PutReserve(*os_, length);
GenericStringStream<SourceEncoding> is(json);
while (RAPIDJSON_LIKELY(is.Tell() < length)) {
RAPIDJSON_ASSERT(is.Peek() != '\0');
if (RAPIDJSON_UNLIKELY(!(writeFlags & kWriteValidateEncodingFlag ?
Transcoder<SourceEncoding, TargetEncoding>::Validate(is, *os_) :
Transcoder<SourceEncoding, TargetEncoding>::TranscodeUnsafe(is, *os_))))
return false;
}
return true;
}
void Prefix(Type type) {
(void)type;
if (RAPIDJSON_LIKELY(level_stack_.GetSize() != 0)) { // this value is not at root
Level* level = level_stack_.template Top<Level>();
if (level->valueCount > 0) {
if (level->inArray)
os_->Put(','); // add comma if it is not the first element in array
else // in object
os_->Put((level->valueCount % 2 == 0) ? ',' : ':');
}
if (!level->inArray && level->valueCount % 2 == 0)
RAPIDJSON_ASSERT(type == kStringType); // if it's in object, then even number should be a name
level->valueCount++;
}
else {
RAPIDJSON_ASSERT(!hasRoot_); // Should only has one and only one root.
hasRoot_ = true;
}
}
// Flush the value if it is the top level one.
bool EndValue(bool ret) {
if (RAPIDJSON_UNLIKELY(level_stack_.Empty())) // end of json text
Flush();
return ret;
}
OutputStream* os_;
internal::Stack<StackAllocator> level_stack_;
int maxDecimalPlaces_;
bool hasRoot_;
private:
// Prohibit copy constructor & assignment operator.
Writer(const Writer&);
Writer& operator=(const Writer&);
};
// Full specialization for StringStream to prevent memory copying
template<>
inline bool Writer<StringBuffer>::WriteInt(int i) {
char *buffer = os_->Push(11);
const char* end = internal::i32toa(i, buffer);
os_->Pop(static_cast<size_t>(11 - (end - buffer)));
return true;
}
template<>
inline bool Writer<StringBuffer>::WriteUint(unsigned u) {
char *buffer = os_->Push(10);
const char* end = internal::u32toa(u, buffer);
os_->Pop(static_cast<size_t>(10 - (end - buffer)));
return true;
}
template<>
inline bool Writer<StringBuffer>::WriteInt64(int64_t i64) {
char *buffer = os_->Push(21);
const char* end = internal::i64toa(i64, buffer);
os_->Pop(static_cast<size_t>(21 - (end - buffer)));
return true;
}
template<>
inline bool Writer<StringBuffer>::WriteUint64(uint64_t u) {
char *buffer = os_->Push(20);
const char* end = internal::u64toa(u, buffer);
os_->Pop(static_cast<size_t>(20 - (end - buffer)));
return true;
}
template<>
inline bool Writer<StringBuffer>::WriteDouble(double d) {
if (internal::Double(d).IsNanOrInf()) {
// Note: This code path can only be reached if (RAPIDJSON_WRITE_DEFAULT_FLAGS & kWriteNanAndInfFlag).
if (!(kWriteDefaultFlags & kWriteNanAndInfFlag))
return false;
if (internal::Double(d).IsNan()) {
PutReserve(*os_, 3);
PutUnsafe(*os_, 'N'); PutUnsafe(*os_, 'a'); PutUnsafe(*os_, 'N');
return true;
}
if (internal::Double(d).Sign()) {
PutReserve(*os_, 9);
PutUnsafe(*os_, '-');
}
else
PutReserve(*os_, 8);
PutUnsafe(*os_, 'I'); PutUnsafe(*os_, 'n'); PutUnsafe(*os_, 'f');
PutUnsafe(*os_, 'i'); PutUnsafe(*os_, 'n'); PutUnsafe(*os_, 'i'); PutUnsafe(*os_, 't'); PutUnsafe(*os_, 'y');
return true;
}
char *buffer = os_->Push(25);
char* end = internal::dtoa(d, buffer, maxDecimalPlaces_);
os_->Pop(static_cast<size_t>(25 - (end - buffer)));
return true;
}
#if defined(RAPIDJSON_SSE2) || defined(RAPIDJSON_SSE42)
template<>
inline bool Writer<StringBuffer>::ScanWriteUnescapedString(StringStream& is, size_t length) {
if (length < 16)
return RAPIDJSON_LIKELY(is.Tell() < length);
if (!RAPIDJSON_LIKELY(is.Tell() < length))
return false;
const char* p = is.src_;
const char* end = is.head_ + length;
const char* nextAligned = reinterpret_cast<const char*>((reinterpret_cast<size_t>(p) + 15) & static_cast<size_t>(~15));
const char* endAligned = reinterpret_cast<const char*>(reinterpret_cast<size_t>(end) & static_cast<size_t>(~15));
if (nextAligned > end)
return true;
while (p != nextAligned)
if (*p < 0x20 || *p == '\"' || *p == '\\') {
is.src_ = p;
return RAPIDJSON_LIKELY(is.Tell() < length);
}
else
os_->PutUnsafe(*p++);
// The rest of string using SIMD
static const char dquote[16] = { '\"', '\"', '\"', '\"', '\"', '\"', '\"', '\"', '\"', '\"', '\"', '\"', '\"', '\"', '\"', '\"' };
static const char bslash[16] = { '\\', '\\', '\\', '\\', '\\', '\\', '\\', '\\', '\\', '\\', '\\', '\\', '\\', '\\', '\\', '\\' };
static const char space[16] = { 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F };
const __m128i dq = _mm_loadu_si128(reinterpret_cast<const __m128i *>(&dquote[0]));
const __m128i bs = _mm_loadu_si128(reinterpret_cast<const __m128i *>(&bslash[0]));
const __m128i sp = _mm_loadu_si128(reinterpret_cast<const __m128i *>(&space[0]));
for (; p != endAligned; p += 16) {
const __m128i s = _mm_load_si128(reinterpret_cast<const __m128i *>(p));
const __m128i t1 = _mm_cmpeq_epi8(s, dq);
const __m128i t2 = _mm_cmpeq_epi8(s, bs);
const __m128i t3 = _mm_cmpeq_epi8(_mm_max_epu8(s, sp), sp); // s < 0x20 <=> max(s, 0x1F) == 0x1F
const __m128i x = _mm_or_si128(_mm_or_si128(t1, t2), t3);
unsigned short r = static_cast<unsigned short>(_mm_movemask_epi8(x));
if (RAPIDJSON_UNLIKELY(r != 0)) { // some of characters is escaped
SizeType len;
#ifdef _MSC_VER // Find the index of first escaped
unsigned long offset;
_BitScanForward(&offset, r);
len = offset;
#else
len = static_cast<SizeType>(__builtin_ffs(r) - 1);
#endif
char* q = reinterpret_cast<char*>(os_->PushUnsafe(len));
for (size_t i = 0; i < len; i++)
q[i] = p[i];
p += len;
break;
}
_mm_storeu_si128(reinterpret_cast<__m128i *>(os_->PushUnsafe(16)), s);
}
is.src_ = p;
return RAPIDJSON_LIKELY(is.Tell() < length);
}
#elif defined(RAPIDJSON_NEON)
template<>
inline bool Writer<StringBuffer>::ScanWriteUnescapedString(StringStream& is, size_t length) {
if (length < 16)
return RAPIDJSON_LIKELY(is.Tell() < length);
if (!RAPIDJSON_LIKELY(is.Tell() < length))
return false;
const char* p = is.src_;
const char* end = is.head_ + length;
const char* nextAligned = reinterpret_cast<const char*>((reinterpret_cast<size_t>(p) + 15) & static_cast<size_t>(~15));
const char* endAligned = reinterpret_cast<const char*>(reinterpret_cast<size_t>(end) & static_cast<size_t>(~15));
if (nextAligned > end)
return true;
while (p != nextAligned)
if (*p < 0x20 || *p == '\"' || *p == '\\') {
is.src_ = p;
return RAPIDJSON_LIKELY(is.Tell() < length);
}
else
os_->PutUnsafe(*p++);
// The rest of string using SIMD
const uint8x16_t s0 = vmovq_n_u8('"');
const uint8x16_t s1 = vmovq_n_u8('\\');
const uint8x16_t s2 = vmovq_n_u8('\b');
const uint8x16_t s3 = vmovq_n_u8(32);
for (; p != endAligned; p += 16) {
const uint8x16_t s = vld1q_u8(reinterpret_cast<const uint8_t *>(p));
uint8x16_t x = vceqq_u8(s, s0);
x = vorrq_u8(x, vceqq_u8(s, s1));
x = vorrq_u8(x, vceqq_u8(s, s2));
x = vorrq_u8(x, vcltq_u8(s, s3));
x = vrev64q_u8(x); // Rev in 64
uint64_t low = vgetq_lane_u64(vreinterpretq_u64_u8(x), 0); // extract
uint64_t high = vgetq_lane_u64(vreinterpretq_u64_u8(x), 1); // extract
SizeType len = 0;
bool escaped = false;
if (low == 0) {
if (high != 0) {
uint32_t lz = RAPIDJSON_CLZLL(high);
len = 8 + (lz >> 3);
escaped = true;
}
} else {
uint32_t lz = RAPIDJSON_CLZLL(low);
len = lz >> 3;
escaped = true;
}
if (RAPIDJSON_UNLIKELY(escaped)) { // some of characters is escaped
char* q = reinterpret_cast<char*>(os_->PushUnsafe(len));
for (size_t i = 0; i < len; i++)
q[i] = p[i];
p += len;
break;
}
vst1q_u8(reinterpret_cast<uint8_t *>(os_->PushUnsafe(16)), s);
}
is.src_ = p;
return RAPIDJSON_LIKELY(is.Tell() < length);
}
#endif // RAPIDJSON_NEON
RAPIDJSON_NAMESPACE_END
#if defined(_MSC_VER) || defined(__clang__)
RAPIDJSON_DIAG_POP
#endif
#endif // RAPIDJSON_RAPIDJSON_H_

2596
livox_ros_driver/common/rapidxml/rapidxml.hpp Normal file
View File

File diff suppressed because it is too large Load Diff

174
livox_ros_driver/common/rapidxml/rapidxml_iterators.hpp Normal file
View File

@@ -0,0 +1,174 @@
#ifndef RAPIDXML_ITERATORS_HPP_INCLUDED
#define RAPIDXML_ITERATORS_HPP_INCLUDED
// Copyright (C) 2006, 2009 Marcin Kalicinski
// Version 1.13
// Revision $DateTime: 2009/05/13 01:46:17 $
//! \file rapidxml_iterators.hpp This file contains rapidxml iterators
#include "rapidxml.hpp"
namespace rapidxml
{
//! Iterator of child nodes of xml_node
template<class Ch>
class node_iterator
{
public:
typedef typename xml_node<Ch> value_type;
typedef typename xml_node<Ch> &reference;
typedef typename xml_node<Ch> *pointer;
typedef std::ptrdiff_t difference_type;
typedef std::bidirectional_iterator_tag iterator_category;
node_iterator()
: m_node(0)
{
}
node_iterator(xml_node<Ch> *node)
: m_node(node->first_node())
{
}
reference operator *() const
{
assert(m_node);
return *m_node;
}
pointer operator->() const
{
assert(m_node);
return m_node;
}
node_iterator& operator++()
{
assert(m_node);
m_node = m_node->next_sibling();
return *this;
}
node_iterator operator++(int)
{
node_iterator tmp = *this;
++this;
return tmp;
}
node_iterator& operator--()
{
assert(m_node && m_node->previous_sibling());
m_node = m_node->previous_sibling();
return *this;
}
node_iterator operator--(int)
{
node_iterator tmp = *this;
++this;
return tmp;
}
bool operator ==(const node_iterator<Ch> &rhs)
{
return m_node == rhs.m_node;
}
bool operator !=(const node_iterator<Ch> &rhs)
{
return m_node != rhs.m_node;
}
private:
xml_node<Ch> *m_node;
};
//! Iterator of child attributes of xml_node
template<class Ch>
class attribute_iterator
{
public:
typedef typename xml_attribute<Ch> value_type;
typedef typename xml_attribute<Ch> &reference;
typedef typename xml_attribute<Ch> *pointer;
typedef std::ptrdiff_t difference_type;
typedef std::bidirectional_iterator_tag iterator_category;
attribute_iterator()
: m_attribute(0)
{
}
attribute_iterator(xml_node<Ch> *node)
: m_attribute(node->first_attribute())
{
}
reference operator *() const
{
assert(m_attribute);
return *m_attribute;
}
pointer operator->() const
{
assert(m_attribute);
return m_attribute;
}
attribute_iterator& operator++()
{
assert(m_attribute);
m_attribute = m_attribute->next_attribute();
return *this;
}
attribute_iterator operator++(int)
{
attribute_iterator tmp = *this;
++this;
return tmp;
}
attribute_iterator& operator--()
{
assert(m_attribute && m_attribute->previous_attribute());
m_attribute = m_attribute->previous_attribute();
return *this;
}
attribute_iterator operator--(int)
{
attribute_iterator tmp = *this;
++this;
return tmp;
}
bool operator ==(const attribute_iterator<Ch> &rhs)
{
return m_attribute == rhs.m_attribute;
}
bool operator !=(const attribute_iterator<Ch> &rhs)
{
return m_attribute != rhs.m_attribute;
}
private:
xml_attribute<Ch> *m_attribute;
};
}
#endif

421
livox_ros_driver/common/rapidxml/rapidxml_print.hpp Normal file
View File

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

122
livox_ros_driver/common/rapidxml/rapidxml_utils.hpp Normal file
View File

@@ -0,0 +1,122 @@
#ifndef RAPIDXML_UTILS_HPP_INCLUDED
#define RAPIDXML_UTILS_HPP_INCLUDED
// Copyright (C) 2006, 2009 Marcin Kalicinski
// Version 1.13
// Revision $DateTime: 2009/05/13 01:46:17 $
//! \file rapidxml_utils.hpp This file contains high-level rapidxml utilities that can be useful
//! in certain simple scenarios. They should probably not be used if maximizing performance is the main objective.
#include "rapidxml.hpp"
#include <vector>
#include <string>
#include <fstream>
#include <stdexcept>
namespace rapidxml
{
//! Represents data loaded from a file
template<class Ch = char>
class file
{
public:
//! Loads file into the memory. Data will be automatically destroyed by the destructor.
//! \param filename Filename to load.
file(const char *filename)
{
using namespace std;
// Open stream
basic_ifstream<Ch> stream(filename, ios::binary);
if (!stream)
throw runtime_error(string("cannot open file ") + filename);
stream.unsetf(ios::skipws);
// Determine stream size
stream.seekg(0, ios::end);
size_t size = stream.tellg();
stream.seekg(0);
// Load data and add terminating 0
m_data.resize(size + 1);
stream.read(&m_data.front(), static_cast<streamsize>(size));
m_data[size] = 0;
}
//! Loads file into the memory. Data will be automatically destroyed by the destructor
//! \param stream Stream to load from
file(std::basic_istream<Ch> &stream)
{
using namespace std;
// Load data and add terminating 0
stream.unsetf(ios::skipws);
m_data.assign(istreambuf_iterator<Ch>(stream), istreambuf_iterator<Ch>());
if (stream.fail() || stream.bad())
throw runtime_error("error reading stream");
m_data.push_back(0);
}
//! Gets file data.
//! \return Pointer to data of file.
Ch *data()
{
return &m_data.front();
}
//! Gets file data.
//! \return Pointer to data of file.
const Ch *data() const
{
return &m_data.front();
}
//! Gets file data size.
//! \return Size of file data, in characters.
std::size_t size() const
{
return m_data.size();
}
private:
std::vector<Ch> m_data; // File data
};
//! Counts children of node. Time complexity is O(n).
//! \return Number of children of node
template<class Ch>
inline std::size_t count_children(xml_node<Ch> *node)
{
xml_node<Ch> *child = node->first_node();
std::size_t count = 0;
while (child)
{
++count;
child = child->next_sibling();
}
return count;
}
//! Counts attributes of node. Time complexity is O(n).
//! \return Number of attributes of node
template<class Ch>
inline std::size_t count_attributes(xml_node<Ch> *node)
{
xml_attribute<Ch> *attr = node->first_attribute();
std::size_t count = 0;
while (attr)
{
++count;
attr = attr->next_attribute();
}
return count;
}
}
#endif

84
livox_ros_driver/config/display_hub_points.rviz
View File

@@ -9,8 +9,8 @@ Panels:
- /Grid1 - /Grid1
- /PointCloud1/Autocompute Value Bounds1 - /PointCloud1/Autocompute Value Bounds1
- /PointCloud21 - /PointCloud21
Splitter Ratio: 0.500695 Splitter Ratio: 0.500694990158081
Tree Height: 747 Tree Height: 728
- Class: rviz/Selection - Class: rviz/Selection
Name: Selection Name: Selection
- Class: rviz/Tool Properties - Class: rviz/Tool Properties
@@ -19,7 +19,7 @@ Panels:
- /2D Nav Goal1 - /2D Nav Goal1
- /Publish Point1 - /Publish Point1
Name: Tool Properties Name: Tool Properties
Splitter Ratio: 0.588679 Splitter Ratio: 0.5886790156364441
- Class: rviz/Views - Class: rviz/Views
Expanded: Expanded:
- /Current View1 - /Current View1
@@ -30,6 +30,10 @@ Panels:
Name: Time Name: Time
SyncMode: 0 SyncMode: 0
SyncSource: PointCloud2 SyncSource: PointCloud2
Preferences:
PromptSaveOnExit: true
Toolbars:
toolButtonStyle: 2
Visualization Manager: Visualization Manager:
Class: "" Class: ""
Displays: Displays:
@@ -39,7 +43,7 @@ Visualization Manager:
Color: 160; 160; 164 Color: 160; 160; 164
Enabled: true Enabled: true
Line Style: Line Style:
Line Width: 0.03 Line Width: 0.029999999329447746
Value: Lines Value: Lines
Name: Grid Name: Grid
Normal Cell Count: 0 Normal Cell Count: 0
@@ -54,8 +58,8 @@ Visualization Manager:
- Alpha: 1 - Alpha: 1
Autocompute Intensity Bounds: true Autocompute Intensity Bounds: true
Autocompute Value Bounds: Autocompute Value Bounds:
Max Value: 0.856 Max Value: 0.8560000061988831
Min Value: -0.735 Min Value: -0.7350000143051147
Value: true Value: true
Axis: Z Axis: Z
Channel Name: x Channel Name: x
@@ -66,15 +70,15 @@ Visualization Manager:
Enabled: false Enabled: false
Invert Rainbow: false Invert Rainbow: false
Max Color: 255; 255; 255 Max Color: 255; 255; 255
Max Intensity: -0.088 Max Intensity: -0.08799999952316284
Min Color: 0; 0; 0 Min Color: 0; 0; 0
Min Intensity: -1.951 Min Intensity: -1.9509999752044678
Name: PointCloud Name: PointCloud
Position Transformer: XYZ Position Transformer: XYZ
Queue Size: 1000 Queue Size: 1000
Selectable: true Selectable: true
Size (Pixels): 2 Size (Pixels): 2
Size (m): 0.005 Size (m): 0.004999999888241291
Style: Flat Squares Style: Flat Squares
Topic: /livox/lidar Topic: /livox/lidar
Unreliable: false Unreliable: false
@@ -84,8 +88,8 @@ Visualization Manager:
- Alpha: 1 - Alpha: 1
Autocompute Intensity Bounds: true Autocompute Intensity Bounds: true
Autocompute Value Bounds: Autocompute Value Bounds:
Max Value: 0.816 Max Value: 0.8159999847412109
Min Value: -0.674 Min Value: -0.6740000247955322
Value: true Value: true
Axis: Z Axis: Z
Channel Name: intensity Channel Name: intensity
@@ -96,17 +100,17 @@ Visualization Manager:
Enabled: true Enabled: true
Invert Rainbow: true Invert Rainbow: true
Max Color: 255; 255; 255 Max Color: 255; 255; 255
Max Intensity: 151 Max Intensity: 152
Min Color: 0; 0; 0 Min Color: 0; 0; 0
Min Intensity: 0 Min Intensity: 0
Name: PointCloud2 Name: PointCloud2
Position Transformer: XYZ Position Transformer: XYZ
Queue Size: 1000 Queue Size: 10
Selectable: true Selectable: true
Size (Pixels): 3 Size (Pixels): 2
Size (m): 0.005 Size (m): 0.004999999888241291
Style: Flat Squares Style: Points
Topic: /livox/hub Topic: /livox/lidar
Unreliable: false Unreliable: false
Use Fixed Frame: true Use Fixed Frame: true
Use rainbow: true Use rainbow: true
@@ -114,8 +118,9 @@ Visualization Manager:
Enabled: true Enabled: true
Global Options: Global Options:
Background Color: 48; 48; 48 Background Color: 48; 48; 48
Default Light: true
Fixed Frame: livox_frame Fixed Frame: livox_frame
Frame Rate: 20 Frame Rate: 50
Name: root Name: root
Tools: Tools:
- Class: rviz/Interact - Class: rviz/Interact
@@ -125,7 +130,10 @@ Visualization Manager:
- Class: rviz/FocusCamera - Class: rviz/FocusCamera
- Class: rviz/Measure - Class: rviz/Measure
- Class: rviz/SetInitialPose - Class: rviz/SetInitialPose
Theta std deviation: 0.2617993950843811
Topic: /initialpose Topic: /initialpose
X std deviation: 0.5
Y std deviation: 0.5
- Class: rviz/SetGoal - Class: rviz/SetGoal
Topic: /move_base_simple/goal Topic: /move_base_simple/goal
- Class: rviz/PublishPoint - Class: rviz/PublishPoint
@@ -135,27 +143,30 @@ Visualization Manager:
Views: Views:
Current: Current:
Class: rviz/Orbit Class: rviz/Orbit
Distance: 1.61081 Distance: 29.202434539794922
Enable Stereo Rendering: Enable Stereo Rendering:
Stereo Eye Separation: 0.06 Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1 Stereo Focal Distance: 1
Swap Stereo Eyes: false Swap Stereo Eyes: false
Value: false Value: false
Focal Point: Focal Point:
X: 0.267255 X: 0.2672550082206726
Y: 0.0618536 Y: 0.061853598803281784
Z: 0.150874 Z: 0.15087400376796722
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View Name: Current View
Near Clip Distance: 0.01 Near Clip Distance: 0.009999999776482582
Pitch: 0.3048 Pitch: 0.679796040058136
Target Frame: <Fixed Frame> Target Frame: <Fixed Frame>
Value: Orbit (rviz) Value: Orbit (rviz)
Yaw: 3.01742 Yaw: 3.0174102783203125
Saved: Saved:
- Class: rviz/Orbit - Class: rviz/Orbit
Distance: 10 Distance: 10
Enable Stereo Rendering: Enable Stereo Rendering:
Stereo Eye Separation: 0.06 Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1 Stereo Focal Distance: 1
Swap Stereo Eyes: false Swap Stereo Eyes: false
Value: false Value: false
@@ -163,19 +174,22 @@ Visualization Manager:
X: 0 X: 0
Y: 0 Y: 0
Z: 0 Z: 0
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Orbit Name: Orbit
Near Clip Distance: 0.01 Near Clip Distance: 0.009999999776482582
Pitch: 1.1104 Pitch: 1.1103999614715576
Target Frame: <Fixed Frame> Target Frame: <Fixed Frame>
Value: Orbit (rviz) Value: Orbit (rviz)
Yaw: 0.570397 Yaw: 0.5703970193862915
Window Geometry: Window Geometry:
Displays: Displays:
collapsed: false collapsed: false
Height: 1028 Height: 1025
Hide Left Dock: false Hide Left Dock: false
Hide Right Dock: true Hide Right Dock: true
QMainWindow State: 000000ff00000000fd0000000400000000000001830000037afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000037a000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005110000003efc0100000002fb0000000800540069006d0065010000000000000511000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000003880000037a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 QMainWindow State: 000000ff00000000fd0000000400000000000001a900000363fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000363000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000058e0000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection: Selection:
collapsed: false collapsed: false
Time: Time:
@@ -184,6 +198,6 @@ Window Geometry:
collapsed: false collapsed: false
Views: Views:
collapsed: true collapsed: true
Width: 1297 Width: 1853
X: 209 X: 67
Y: 14 Y: 27

105
livox_ros_driver/config/display_lidar_points.rviz
View File

@@ -7,10 +7,9 @@ Panels:
- /Global Options1 - /Global Options1
- /Status1 - /Status1
- /Grid1 - /Grid1
- /PointCloud1/Autocompute Value Bounds1
- /PointCloud21 - /PointCloud21
Splitter Ratio: 0.500695 Splitter Ratio: 0.500694990158081
Tree Height: 747 Tree Height: 728
- Class: rviz/Selection - Class: rviz/Selection
Name: Selection Name: Selection
- Class: rviz/Tool Properties - Class: rviz/Tool Properties
@@ -19,7 +18,7 @@ Panels:
- /2D Nav Goal1 - /2D Nav Goal1
- /Publish Point1 - /Publish Point1
Name: Tool Properties Name: Tool Properties
Splitter Ratio: 0.588679 Splitter Ratio: 0.5886790156364441
- Class: rviz/Views - Class: rviz/Views
Expanded: Expanded:
- /Current View1 - /Current View1
@@ -30,6 +29,10 @@ Panels:
Name: Time Name: Time
SyncMode: 0 SyncMode: 0
SyncSource: PointCloud2 SyncSource: PointCloud2
Preferences:
PromptSaveOnExit: true
Toolbars:
toolButtonStyle: 2
Visualization Manager: Visualization Manager:
Class: "" Class: ""
Displays: Displays:
@@ -39,7 +42,7 @@ Visualization Manager:
Color: 160; 160; 164 Color: 160; 160; 164
Enabled: true Enabled: true
Line Style: Line Style:
Line Width: 0.03 Line Width: 0.029999999329447746
Value: Lines Value: Lines
Name: Grid Name: Grid
Normal Cell Count: 0 Normal Cell Count: 0
@@ -54,58 +57,28 @@ Visualization Manager:
- Alpha: 1 - Alpha: 1
Autocompute Intensity Bounds: true Autocompute Intensity Bounds: true
Autocompute Value Bounds: Autocompute Value Bounds:
Max Value: 0.856 Max Value: 0.4569999873638153
Min Value: -0.735 Min Value: -0.367000013589859
Value: true
Axis: Z
Channel Name: x
Class: rviz/PointCloud
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 1
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: -0.088
Min Color: 0; 0; 0
Min Intensity: -1.951
Name: PointCloud
Position Transformer: XYZ
Queue Size: 1000
Selectable: true
Size (Pixels): 2
Size (m): 0.005
Style: Flat Squares
Topic: /livox/lidar
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 0.653
Min Value: -0.918
Value: true Value: true
Axis: Z Axis: Z
Channel Name: intensity Channel Name: intensity
Class: rviz/PointCloud2 Class: rviz/PointCloud2
Color: 255; 255; 255 Color: 255; 255; 255
Color Transformer: AxisColor Color Transformer: Intensity
Decay Time: 1 Decay Time: 1
Enabled: true Enabled: true
Invert Rainbow: false Invert Rainbow: false
Max Color: 255; 255; 255 Max Color: 255; 255; 255
Max Intensity: 151 Max Intensity: 255
Min Color: 0; 0; 0 Min Color: 0; 0; 0
Min Intensity: 0 Min Intensity: 0
Name: PointCloud2 Name: PointCloud2
Position Transformer: XYZ Position Transformer: XYZ
Queue Size: 100 Queue Size: 10
Selectable: true Selectable: true
Size (Pixels): 3 Size (Pixels): 2
Size (m): 0.005 Size (m): 0.004999999888241291
Style: Flat Squares Style: Points
Topic: /livox/lidar Topic: /livox/lidar
Unreliable: false Unreliable: false
Use Fixed Frame: true Use Fixed Frame: true
@@ -114,8 +87,9 @@ Visualization Manager:
Enabled: true Enabled: true
Global Options: Global Options:
Background Color: 48; 48; 48 Background Color: 48; 48; 48
Default Light: true
Fixed Frame: livox_frame Fixed Frame: livox_frame
Frame Rate: 20 Frame Rate: 40
Name: root Name: root
Tools: Tools:
- Class: rviz/Interact - Class: rviz/Interact
@@ -125,7 +99,10 @@ Visualization Manager:
- Class: rviz/FocusCamera - Class: rviz/FocusCamera
- Class: rviz/Measure - Class: rviz/Measure
- Class: rviz/SetInitialPose - Class: rviz/SetInitialPose
Theta std deviation: 0.2617993950843811
Topic: /initialpose Topic: /initialpose
X std deviation: 0.5
Y std deviation: 0.5
- Class: rviz/SetGoal - Class: rviz/SetGoal
Topic: /move_base_simple/goal Topic: /move_base_simple/goal
- Class: rviz/PublishPoint - Class: rviz/PublishPoint
@@ -135,27 +112,30 @@ Visualization Manager:
Views: Views:
Current: Current:
Class: rviz/Orbit Class: rviz/Orbit
Distance: 0.591995 Distance: 25.80008888244629
Enable Stereo Rendering: Enable Stereo Rendering:
Stereo Eye Separation: 0.06 Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1 Stereo Focal Distance: 1
Swap Stereo Eyes: false Swap Stereo Eyes: false
Value: false Value: false
Focal Point: Focal Point:
X: 0.267255 X: 0.2672550082206726
Y: 0.0618536 Y: 0.061853598803281784
Z: 0.150874 Z: 0.15087400376796722
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View Name: Current View
Near Clip Distance: 0.01 Near Clip Distance: 0.009999999776482582
Pitch: 0.229799 Pitch: 0.5597995519638062
Target Frame: <Fixed Frame> Target Frame: <Fixed Frame>
Value: Orbit (rviz) Value: Orbit (rviz)
Yaw: 2.99561 Yaw: 3.065610408782959
Saved: Saved:
- Class: rviz/Orbit - Class: rviz/Orbit
Distance: 10 Distance: 10
Enable Stereo Rendering: Enable Stereo Rendering:
Stereo Eye Separation: 0.06 Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1 Stereo Focal Distance: 1
Swap Stereo Eyes: false Swap Stereo Eyes: false
Value: false Value: false
@@ -163,19 +143,22 @@ Visualization Manager:
X: 0 X: 0
Y: 0 Y: 0
Z: 0 Z: 0
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Orbit Name: Orbit
Near Clip Distance: 0.01 Near Clip Distance: 0.009999999776482582
Pitch: 1.1104 Pitch: 1.1103999614715576
Target Frame: <Fixed Frame> Target Frame: <Fixed Frame>
Value: Orbit (rviz) Value: Orbit (rviz)
Yaw: 0.570397 Yaw: 0.5703970193862915
Window Geometry: Window Geometry:
Displays: Displays:
collapsed: false collapsed: false
Height: 1028 Height: 1025
Hide Left Dock: false Hide Left Dock: false
Hide Right Dock: true Hide Right Dock: true
QMainWindow State: 000000ff00000000fd0000000400000000000001830000037afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000037a000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005110000003efc0100000002fb0000000800540069006d0065010000000000000511000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000003880000037a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 QMainWindow State: 000000ff00000000fd0000000400000000000001c400000363fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000363000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005730000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection: Selection:
collapsed: false collapsed: false
Time: Time:
@@ -184,6 +167,6 @@ Window Geometry:
collapsed: false collapsed: false
Views: Views:
collapsed: true collapsed: true
Width: 1297 Width: 1853
X: 407 X: 67
Y: 14 Y: 27

33
livox_ros_driver/config/livox_hub_config.json Normal file
View File

@@ -0,0 +1,33 @@
{
"hub_config": {
"broadcast_code": "13UUG1R00400170",
"enable_connect": false,
"coordinate": 0
},
"lidar_config": [
{
"broadcast_code": "0TFDG3B006H2Z11",
"enable_fan": true,
"return_mode": 0,
"imu_rate": 1
},
{
"broadcast_code": "0TFDG3U99101291",
"enable_fan": true,
"return_mode": 0,
"imu_rate": 1
},
{
"broadcast_code": "1HDDG8M00100191",
"enable_fan": true,
"return_mode": 0,
"imu_rate": 1
},
{
"broadcast_code": "1PQDG8E00100321",
"enable_fan": true,
"return_mode": 0,
"imu_rate": 1
}
]
}

30
livox_ros_driver/config/livox_lidar_config.json Normal file
View File

@@ -0,0 +1,30 @@
{
"lidar_config": [
{
"broadcast_code": "0T9DFBC00401611",
"enable_connect": false,
"enable_fan": true,
"return_mode": 0,
"coordinate": 0,
"imu_rate": 1,
"extrinsic_parameter_source": 0
},
{
"broadcast_code": "0TFDG3U99101431",
"enable_connect": false,
"enable_fan": true,
"return_mode": 0,
"coordinate": 0,
"imu_rate": 1,
"extrinsic_parameter_source": 0
}
],
"timesync_config": {
"enable_timesync": false,
"device_name": "/dev/ttyUSB0",
"comm_device_type": 0,
"baudrate_index": 2,
"parity_index": 0
}
}

45
livox_ros_driver/launch/livox_hub.launch
View File

@@ -1,14 +1,37 @@
<launch> <launch>
<arg name="bd_list" default="100000000000000"/>
<param name="livox_msg_type" value="0"/>
<node name="livox_hub_publisher" pkg="livox_ros_driver" <arg name="lvx_file_path" default="livox_test.lvx"/>
type="livox_hub_node" required="true" <arg name="bd_list" default="100000000000000"/>
output="screen" args="$(arg bd_list)"/> <arg name="xfer_format" default="0"/>
<arg name="multi_topic" default="0"/>
<!-- <arg name="data_src" default="1"/>
<node name="rviz" pkg="rviz" type="rviz" respawn="true" <arg name="publish_freq" default="20.0"/>
args="-d $(find livox_ros_driver)/config/display_hub_points.rviz"/> <arg name="output_type" default="0"/>
--> <arg name="rviz_enable" default="false"/>
<arg name="rosbag_enable" default="false"/>
<arg name="cmdline_arg" default="$(arg bd_list)"/>
<param name="xfer_format" value="$(arg xfer_format)"/>
<param name="multi_topic" value="$(arg multi_topic)"/>
<param name="data_src" value="$(arg data_src)"/>
<param name="publish_freq" type="double" value="$(arg publish_freq)"/>
<param name="output_data_type" value="$(arg output_type)"/>
<param name="cmdline_str" type="string" value="$(arg bd_list)"/>
<param name="cmdline_file_path" type="string" value="$(arg lvx_file_path)"/>
<param name="user_config_path" type="string" value="$(find livox_ros_driver)/config/livox_hub_config.json"/>
<node name="livox_lidar_publisher" pkg="livox_ros_driver"
type="livox_ros_driver_node" required="true"
output="screen" args="$(arg cmdline_arg)"/>
<group if="$(arg rviz_enable)">
<node name="rviz" pkg="rviz" type="rviz" respawn="true"
args="-d $(find livox_ros_driver)/config/display_hub_points.rviz"/>
</group>
<group if="$(arg rosbag_enable)">
<node pkg="rosbag" type="record" name="record" output="screen"
args="-a"/>
</group>
</launch> </launch>

45
livox_ros_driver/launch/livox_hub_msg.launch
View File

@@ -1,14 +1,37 @@
<launch> <launch>
<arg name="bd_list" default="100000000000000"/>
<param name="livox_msg_type" value="0"/>
<node name="livox_hub_publisher" pkg="livox_ros_driver" <arg name="lvx_file_path" default="livox_test.lvx"/>
type="livox_hub_node" required="true" <arg name="bd_list" default="100000000000000"/>
output="screen" args="$(arg bd_list)"/> <arg name="xfer_format" default="1"/>
<arg name="multi_topic" default="0"/>
<!-- <arg name="data_src" default="1"/>
<node name="rviz" pkg="rviz" type="rviz" respawn="true" <arg name="publish_freq" default="20.0"/>
args="-d $(find livox_ros_driver)/config/display_hub_points.rviz"/> <arg name="output_type" default="0"/>
--> <arg name="rviz_enable" default="false"/>
<arg name="rosbag_enable" default="false"/>
<arg name="cmdline_arg" default="$(arg bd_list)"/>
<param name="xfer_format" value="$(arg xfer_format)"/>
<param name="multi_topic" value="$(arg multi_topic)"/>
<param name="data_src" value="$(arg data_src)"/>
<param name="publish_freq" type="double" value="$(arg publish_freq)"/>
<param name="output_data_type" value="$(arg output_type)"/>
<param name="cmdline_str" type="string" value="$(arg bd_list)"/>
<param name="cmdline_file_path" type="string" value="$(arg lvx_file_path)"/>
<param name="user_config_path" type="string" value="$(find livox_ros_driver)/config/livox_hub_config.json"/>
<node name="livox_lidar_publisher" pkg="livox_ros_driver"
type="livox_ros_driver_node" required="true"
output="screen" args="$(arg cmdline_arg)"/>
<group if="$(arg rviz_enable)">
<node name="rviz" pkg="rviz" type="rviz" respawn="true"
args="-d $(find livox_ros_driver)/config/display_hub_points.rviz"/>
</group>
<group if="$(arg rosbag_enable)">
<node pkg="rosbag" type="record" name="record" output="screen"
args="-a"/>
</group>
</launch> </launch>

41
livox_ros_driver/launch/livox_hub_rviz.launch
View File

@@ -1,12 +1,37 @@
<launch> <launch>
<arg name="bd_list" default="100000000000000"/>
<param name="livox_msg_type" value="0"/>
<node name="livox_hub_publisher" pkg="livox_ros_driver" <arg name="lvx_file_path" default="livox_test.lvx"/>
type="livox_hub_node" required="true" <arg name="bd_list" default="100000000000000"/>
output="screen" args="$(arg bd_list)"/> <arg name="xfer_format" default="0"/>
<arg name="multi_topic" default="0"/>
<arg name="data_src" default="1"/>
<arg name="publish_freq" default="20.0"/>
<arg name="output_type" default="0"/>
<arg name="rviz_enable" default="true"/>
<arg name="rosbag_enable" default="false"/>
<arg name="cmdline_arg" default="$(arg bd_list)"/>
<param name="xfer_format" value="$(arg xfer_format)"/>
<param name="multi_topic" value="$(arg multi_topic)"/>
<param name="data_src" value="$(arg data_src)"/>
<param name="publish_freq" type="double" value="$(arg publish_freq)"/>
<param name="output_data_type" value="$(arg output_type)"/>
<param name="cmdline_str" type="string" value="$(arg bd_list)"/>
<param name="cmdline_file_path" type="string" value="$(arg lvx_file_path)"/>
<param name="user_config_path" type="string" value="$(find livox_ros_driver)/config/livox_hub_config.json"/>
<node name="livox_lidar_publisher" pkg="livox_ros_driver"
type="livox_ros_driver_node" required="true"
output="screen" args="$(arg cmdline_arg)"/>
<group if="$(arg rviz_enable)">
<node name="rviz" pkg="rviz" type="rviz" respawn="true"
args="-d $(find livox_ros_driver)/config/display_hub_points.rviz"/>
</group>
<group if="$(arg rosbag_enable)">
<node pkg="rosbag" type="record" name="record" output="screen"
args="-a"/>
</group>
<node name="rviz" pkg="rviz" type="rviz" respawn="true"
args="-d $(find livox_ros_driver)/config/display_hub_points.rviz"/>
</launch> </launch>

44
livox_ros_driver/launch/livox_lidar.launch
View File

@@ -1,13 +1,37 @@
<launch> <launch>
<arg name="bd_list" default="100000000000000"/>
<param name="livox_msg_type" value="0"/>
<node name="livox_lidar_publisher" pkg="livox_ros_driver" <arg name="lvx_file_path" default="livox_test.lvx"/>
type="livox_lidar_node" required="true" <arg name="bd_list" default="100000000000000"/>
output="screen" args="$(arg bd_list)"/> <arg name="xfer_format" default="0"/>
<!-- <arg name="multi_topic" default="0"/>
<node name="rviz" pkg="rviz" type="rviz" respawn="true" <arg name="data_src" default="0"/>
args="-d $(find livox_ros_driver)/config/display_lidar_points.rviz"/> <arg name="publish_freq" default="20.0"/>
--> <arg name="output_type" default="0"/>
<arg name="rviz_enable" default="false"/>
<arg name="rosbag_enable" default="false"/>
<arg name="cmdline_arg" default="$(arg bd_list)"/>
<param name="xfer_format" value="$(arg xfer_format)"/>
<param name="multi_topic" value="$(arg multi_topic)"/>
<param name="data_src" value="$(arg data_src)"/>
<param name="publish_freq" type="double" value="$(arg publish_freq)"/>
<param name="output_data_type" value="$(arg output_type)"/>
<param name="cmdline_str" type="string" value="$(arg bd_list)"/>
<param name="cmdline_file_path" type="string" value="$(arg lvx_file_path)"/>
<param name="user_config_path" type="string" value="$(find livox_ros_driver)/config/livox_lidar_config.json"/>
<node name="livox_lidar_publisher" pkg="livox_ros_driver"
type="livox_ros_driver_node" required="true"
output="screen" args="$(arg cmdline_arg)"/>
<group if="$(arg rviz_enable)">
<node name="rviz" pkg="rviz" type="rviz" respawn="true"
args="-d $(find livox_ros_driver)/config/display_lidar_points.rviz"/>
</group>
<group if="$(arg rosbag_enable)">
<node pkg="rosbag" type="record" name="record" output="screen"
args="-a"/>
</group>
</launch> </launch>

44
livox_ros_driver/launch/livox_lidar_msg.launch
View File

@@ -1,13 +1,37 @@
<launch> <launch>
<arg name="bd_list" default="100000000000000"/>
<param name="livox_msg_type" value="1"/>
<node name="livox_lidar_publisher" pkg="livox_ros_driver" <arg name="lvx_file_path" default="livox_test.lvx"/>
type="livox_lidar_node" required="true" <arg name="bd_list" default="100000000000000"/>
output="screen" args="$(arg bd_list)"/> <arg name="xfer_format" default="1"/>
<!-- <arg name="multi_topic" default="0"/>
<node name="rviz" pkg="rviz" type="rviz" respawn="true" <arg name="data_src" default="0"/>
args="-d $(find livox_ros_driver)/config/display_lidar_points.rviz"/> <arg name="publish_freq" default="20.0"/>
--> <arg name="output_type" default="0"/>
<arg name="rviz_enable" default="false"/>
<arg name="rosbag_enable" default="false"/>
<arg name="cmdline_arg" default="$(arg bd_list)"/>
<param name="xfer_format" value="$(arg xfer_format)"/>
<param name="multi_topic" value="$(arg multi_topic)"/>
<param name="data_src" value="$(arg data_src)"/>
<param name="publish_freq" type="double" value="$(arg publish_freq)"/>
<param name="output_data_type" value="$(arg output_type)"/>
<param name="cmdline_str" type="string" value="$(arg bd_list)"/>
<param name="cmdline_file_path" type="string" value="$(arg lvx_file_path)"/>
<param name="user_config_path" type="string" value="$(find livox_ros_driver)/config/livox_lidar_config.json"/>
<node name="livox_lidar_publisher" pkg="livox_ros_driver"
type="livox_ros_driver_node" required="true"
output="screen" args="$(arg cmdline_arg)"/>
<group if="$(arg rviz_enable)">
<node name="rviz" pkg="rviz" type="rviz" respawn="true"
args="-d $(find livox_ros_driver)/config/display_lidar_points.rviz"/>
</group>
<group if="$(arg rosbag_enable)">
<node pkg="rosbag" type="record" name="record" output="screen"
args="-a"/>
</group>
</launch> </launch>

41
livox_ros_driver/launch/livox_lidar_rviz.launch
View File

@@ -1,12 +1,37 @@
<launch> <launch>
<arg name="bd_list" default="100000000000000"/>
<param name="livox_msg_type" value="0"/>
<node name="livox_lidar_publisher" pkg="livox_ros_driver" <arg name="lvx_file_path" default="livox_test.lvx"/>
type="livox_lidar_node" required="true" <arg name="bd_list" default="100000000000000"/>
output="screen" args="$(arg bd_list)"/> <arg name="xfer_format" default="0"/>
<arg name="multi_topic" default="0"/>
<arg name="data_src" default="0"/>
<arg name="publish_freq" default="20.0"/>
<arg name="output_type" default="0"/>
<arg name="rviz_enable" default="true"/>
<arg name="rosbag_enable" default="false"/>
<arg name="cmdline_arg" default="$(arg bd_list)"/>
<param name="xfer_format" value="$(arg xfer_format)"/>
<param name="multi_topic" value="$(arg multi_topic)"/>
<param name="data_src" value="$(arg data_src)"/>
<param name="publish_freq" type="double" value="$(arg publish_freq)"/>
<param name="output_data_type" value="$(arg output_type)"/>
<param name="cmdline_str" type="string" value="$(arg bd_list)"/>
<param name="cmdline_file_path" type="string" value="$(arg lvx_file_path)"/>
<param name="user_config_path" type="string" value="$(find livox_ros_driver)/config/livox_lidar_config.json"/>
<node name="livox_lidar_publisher" pkg="livox_ros_driver"
type="livox_ros_driver_node" required="true"
output="screen" args="$(arg cmdline_arg)"/>
<group if="$(arg rviz_enable)">
<node name="rviz" pkg="rviz" type="rviz" respawn="true"
args="-d $(find livox_ros_driver)/config/display_lidar_points.rviz"/>
</group>
<group if="$(arg rosbag_enable)">
<node pkg="rosbag" type="record" name="record" output="screen"
args="-a"/>
</group>
<node name="rviz" pkg="rviz" type="rviz" respawn="true"
args="-d $(find livox_ros_driver)/config/display_lidar_points.rviz"/>
</launch> </launch>

37
livox_ros_driver/launch/livox_template.launch Normal file
View File

@@ -0,0 +1,37 @@
<launch>
<arg name="lvx_file_path" default="livox_test.lvx"/>
<arg name="bd_list" default="100000000000000"/>
<arg name="xfer_format" default="0"/>
<arg name="multi_topic" default="0"/>
<arg name="data_src" default="0"/>
<arg name="publish_freq" default="20.0"/>
<arg name="output_type" default="0"/>
<arg name="rviz_enable" default="false"/>
<arg name="rosbag_enable" default="false"/>
<arg name="cmdline_arg" default="$(arg bd_list)"/>
<param name="xfer_format" value="$(arg xfer_format)"/>
<param name="multi_topic" value="$(arg multi_topic)"/>
<param name="data_src" value="$(arg data_src)"/>
<param name="publish_freq" type="double" value="$(arg publish_freq)"/>
<param name="output_data_type" value="$(arg output_type)"/>
<param name="cmdline_str" type="string" value="$(arg bd_list)"/>
<param name="cmdline_file_path" type="string" value="$(arg lvx_file_path)"/>
<param name="user_config_path" type="string" value="$(find livox_ros_driver)/config/livox_lidar_config.json"/>
<node name="livox_lidar_publisher" pkg="livox_coat"
type="livox_ros_driver_node" required="true"
output="screen" args="$(arg cmdline_arg)"/>
<group if="$(arg rviz_enable)">
<node name="rviz" pkg="rviz" type="rviz" respawn="true"
args="-d $(find livox_ros_driver)/config/display_lidar_points.rviz"/>
</group>
<group if="$(arg rosbag_enable)">
<node pkg="rosbag" type="record" name="record" output="screen"
args="-a"/>
</group>
</launch>

36
livox_ros_driver/launch/lvx_to_rosbag.launch Normal file
View File

@@ -0,0 +1,36 @@
<launch>
<arg name="lvx_file_path" default="livox_test.lvx"/>
<arg name="bd_list" default="100000000000000"/>
<arg name="xfer_format" default="0"/>
<arg name="multi_topic" default="0"/>
<arg name="data_src" default="2"/>
<arg name="publish_freq" default="2000.0"/>
<arg name="output_type" default="1"/>
<arg name="rviz_enable" default="false"/>
<arg name="rosbag_enable" default="false"/>
<arg name="cmdline_arg" default="$(arg bd_list)"/>
<param name="xfer_format" value="$(arg xfer_format)"/>
<param name="multi_topic" value="$(arg multi_topic)"/>
<param name="data_src" value="$(arg data_src)"/>
<param name="publish_freq" type="double" value="$(arg publish_freq)"/>
<param name="output_data_type" value="$(arg output_type)"/>
<param name="cmdline_str" type="string" value="$(arg bd_list)"/>
<param name="cmdline_file_path" type="string" value="$(arg lvx_file_path)"/>
<node name="livox_lidar_publisher" pkg="livox_ros_driver"
type="livox_ros_driver_node" required="true"
output="screen" args="$(arg cmdline_arg)"/>
<group if="$(arg rviz_enable)">
<node name="rviz" pkg="rviz" type="rviz" respawn="true"
args="-d $(find livox_ros_driver)/config/display_lidar_points.rviz"/>
</group>
<group if="$(arg rosbag_enable)">
<node pkg="rosbag" type="record" name="record" output="screen"
args="-a"/>
</group>
</launch>

36
livox_ros_driver/launch/lvx_to_rosbag_rviz.launch Normal file
View File

@@ -0,0 +1,36 @@
<launch>
<arg name="lvx_file_path" default="livox_test.lvx"/>
<arg name="bd_list" default="100000000000000"/>
<arg name="xfer_format" default="0"/>
<arg name="multi_topic" default="0"/>
<arg name="data_src" default="2"/>
<arg name="publish_freq" default="50.0"/>
<arg name="output_type" default="0"/>
<arg name="rviz_enable" default="true"/>
<arg name="rosbag_enable" default="true"/>
<arg name="cmdline_arg" default="$(arg bd_list)"/>
<param name="xfer_format" value="$(arg xfer_format)"/>
<param name="multi_topic" value="$(arg multi_topic)"/>
<param name="data_src" value="$(arg data_src)"/>
<param name="publish_freq" type="double" value="$(arg publish_freq)"/>
<param name="output_data_type" value="$(arg output_type)"/>
<param name="cmdline_str" type="string" value="$(arg bd_list)"/>
<param name="cmdline_file_path" type="string" value="$(arg lvx_file_path)"/>
<node name="livox_lidar_publisher" pkg="livox_ros_driver"
type="livox_ros_driver_node" required="true"
output="screen" args="$(arg cmdline_arg)"/>
<group if="$(arg rviz_enable)">
<node name="rviz" pkg="rviz" type="rviz" respawn="true"
args="-d $(find livox_ros_driver)/config/display_lidar_points.rviz"/>
</group>
<group if="$(arg rosbag_enable)">
<node pkg="rosbag" type="record" name="record" output="screen"
args="-a"/>
</group>
</launch>

845
livox_ros_driver/livox_hub.cpp
View File

@@ -1,845 +0,0 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2019 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <stdint.h>
#include <stdbool.h>
#include <stddef.h>
#include <math.h>
#include <time.h>
#include <vector>
#include <chrono>
#include "livox_sdk.h"
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <livox_ros_driver/CustomPoint.h>
#include <livox_ros_driver/CustomMsg.h>
#include <pcl_ros/point_cloud.h>
/* const varible -------------------------------------------------------------------------------- */
/* user area */
const uint32_t kPublishIntervalMs = 50; // unit:ms
/* const parama */
const uint32_t kMaxPointPerEthPacket = 100;
const uint32_t kMinEthPacketQueueSize = 128; // must be 2^n
const uint32_t kMaxEthPacketQueueSize = 8192; // must be 2^n
const uint32_t KEthPacketHeaderLength = 18; // (sizeof(LivoxEthPacket) - 1)
const uint32_t KEthPacketMaxLength = 1500;
const uint32_t KCartesianPointSize = 13;
const uint32_t KSphericalPointSzie = 9;
const uint64_t kPacketTimeGap = 1000000; // 1ms = 1000000ns
const uint64_t kMaxPacketTimeGap = 1700000; // the threshold of packet continuous
const uint64_t kDeviceDisconnectThreshold = 1000000000; // the threshold of device disconect
const uint64_t kNsPerSecond = 1000000000; // 1s = 1000000000ns
const uint32_t kPublishIntervalUpperLimitMs = 2000; // limit upper boundary 2s
const uint32_t kPublishIntervalLowerLimitMs = 2; // limit lower boundary to 2ms
const int kBdArgcNum = 4;
const int kBdArgvPos = 1;
const int kCommandlineBdSize = 15;
#pragma pack(1)
typedef struct {
uint64_t time_rcv; // used for pps sync only mode
uint32_t point_num;
uint8_t raw_data[KEthPacketMaxLength];
} StoragePacket;
typedef struct {
uint8_t lidar_id;
uint8_t rsvd[3];
uint32_t point_num;
uint64_t timestamp; // ns
LivoxPoint *point;
} PublishPacket;
#pragma pack()
typedef struct {
StoragePacket *storage_packet;
volatile uint32_t rd_idx;
volatile uint32_t wr_idx;
uint32_t mask;
uint32_t size; // must be 2^n
} StoragePacketQueue;
typedef struct {
uint32_t receive_packet_count;
uint32_t loss_packet_count;
uint64_t last_timestamp;
uint64_t timebase; // unit:nanosecond
uint32_t timebase_state;
} LidarPacketStatistic;
typedef enum {
kCoordinateCartesian = 0,
kCoordinateSpherical
} CoordinateType;
typedef enum {
kPointCloud2Msg = 0,
kLivoxCustomMsg
} LivoxMsgType;
/* for global publisher use */
ros::Publisher cloud_pub;
typedef pcl::PointCloud<pcl::PointXYZI> PointCloud;
/* for device connect use ----------------------------------------------------------------------- */
typedef enum {
kDeviceStateDisconnect = 0,
kDeviceStateConnect = 1,
kDeviceStateSampling = 2,
} DeviceState;
typedef struct {
uint8_t handle;
DeviceState device_state;
DeviceInfo info;
LidarPacketStatistic statistic_info;
StoragePacketQueue packet_queue;
} DeviceItem;
DeviceItem lidars[kMaxLidarCount];
DeviceItem hub;
/* user add hub broadcast code here */
const char* broadcast_code_list[] = {
"000000000000001",
};
#define BROADCAST_CODE_LIST_SIZE (sizeof(broadcast_code_list) / sizeof(intptr_t))
/* total broadcast code, include broadcast_code_list and commandline input */
std::vector<std::string > total_broadcast_code;
/* power of 2 buferr operation */
static bool IsPowerOf2(uint32_t size) {
return (size != 0) && ((size & (size - 1)) == 0);
}
static uint32_t RoundupPowerOf2(uint32_t size) {
uint32_t power2_val = 0;
for(int i = 0; i < 32; i++) {
power2_val = ((uint32_t)1) << i;
if(size <= power2_val) {
break;
}
}
return power2_val;
}
/* for pointcloud queue process */
int InitQueue(StoragePacketQueue* queue, uint32_t queue_size) {
if (queue == NULL) {
return 1;
}
if(IsPowerOf2(queue_size) != true) {
queue_size = RoundupPowerOf2(queue_size);
}
queue->storage_packet = new StoragePacket [queue_size];
if(queue->storage_packet == nullptr) {
return 1;
}
queue->rd_idx = 0;
queue->wr_idx = 0;
queue->size = queue_size;
queue->mask = queue_size - 1;
return 0;
}
void ResetQueue(StoragePacketQueue* queue) {
queue->rd_idx = 0;
queue->wr_idx = 0;
}
static void QueueProPop(StoragePacketQueue* queue, StoragePacket* storage_packet) {
uint32_t mask = queue->mask;
uint32_t rd_idx = queue->rd_idx & mask;
memcpy(storage_packet, &(queue->storage_packet[rd_idx]), sizeof(StoragePacket));
}
static void QueuePopUpdate(StoragePacketQueue* queue) {
queue->rd_idx++;
}
uint32_t QueuePop(StoragePacketQueue* queue, StoragePacket* storage_packet) {
QueueProPop(queue, storage_packet);
QueuePopUpdate(queue);
return 1;
}
uint32_t QueueUsedSize(StoragePacketQueue *queue) {
return (queue->wr_idx - queue->rd_idx) & queue->mask;
}
uint32_t QueueIsFull(StoragePacketQueue *queue) {
return ((queue->wr_idx + 1) == queue->rd_idx);
}
uint32_t QueueIsEmpty(StoragePacketQueue *queue) {
return (queue->rd_idx == queue->wr_idx);
}
static uint32_t GetEthPacketLen(LivoxEthPacket* eth_packet, uint32_t point_num) {
if (kCoordinateCartesian == eth_packet->data_type) {
return (KEthPacketHeaderLength + point_num * KCartesianPointSize);
} else {
return (KEthPacketHeaderLength + point_num * KSphericalPointSzie);
}
}
uint32_t PushEthPacketToStorageQueue(StoragePacketQueue* queue, LivoxEthPacket* eth_packet, \
uint32_t point_num, uint64_t timebase) {
uint32_t mask = queue->mask;
uint32_t wr_idx = queue->wr_idx & mask;
queue->storage_packet[wr_idx].time_rcv = timebase;
queue->storage_packet[wr_idx].point_num = point_num;
memcpy(queue->storage_packet[wr_idx].raw_data, \
reinterpret_cast<char *>(eth_packet), \
GetEthPacketLen(eth_packet, point_num));
queue->wr_idx++;
return 1;
}
static uint64_t GetStoragePacketTimestamp(StoragePacket* packet) {
LivoxEthPacket* raw_packet = reinterpret_cast<LivoxEthPacket *>(packet->raw_data);
uint64_t timestamp = *((uint64_t *)(raw_packet->timestamp));
if (raw_packet->timestamp_type == kTimestampTypePps) {
return (timestamp + packet->time_rcv);
} else if (raw_packet->timestamp_type == kTimestampTypeNoSync) {
return timestamp;
} else if (raw_packet->timestamp_type == kTimestampTypePtp) {
return timestamp;
} else if (raw_packet->timestamp_type == kTimestampTypePpsGps) {
struct tm time_utc;
time_utc.tm_isdst = 0;
time_utc.tm_year = raw_packet->timestamp[0] + 100; // map 2000 to 1990
time_utc.tm_mon = raw_packet->timestamp[1];
time_utc.tm_mday = raw_packet->timestamp[2];
time_utc.tm_hour = raw_packet->timestamp[3];
time_utc.tm_min = 0;
time_utc.tm_sec = 0;
//ROS_INFO("UTC:%d %d %d %d", time_utc.tm_year, time_utc.tm_mon,\
// time_utc.tm_mday, time_utc.tm_hour);
uint64_t time_epoch = mktime(&time_utc);
time_epoch = time_epoch * 1000000 + *((uint32_t *)(&(raw_packet->timestamp[4]))); // to us
time_epoch = time_epoch * 1000; // to ns
return time_epoch;
} else {
ROS_INFO("timestamp type invalid");
return 0;
}
}
static uint32_t GetPointInterval(uint32_t device_type) {
if ((kDeviceTypeLidarTele == device_type) || \
(kDeviceTypeLidarHorizon == device_type)) {
return 4167; // 4167 ns
} else {
return 10000; // ns
}
}
static uint32_t GetPacketNumPerSec(uint32_t device_type) {
if ((kDeviceTypeLidarTele == device_type) || \
(kDeviceTypeLidarHorizon == device_type)) {
return 2400; // 2400 raw packet per second
} else {
return 1000; // 1000 raw packet per second
}
}
static uint32_t CalculatePacketQueueSize(uint32_t interval_ms, uint32_t device_type) {
uint32_t queue_size = (1000000.0 / interval_ms) * GetPacketNumPerSec(device_type);
if (queue_size < kMinEthPacketQueueSize) {
queue_size = kMinEthPacketQueueSize;
} else if (queue_size > kMaxEthPacketQueueSize) {
queue_size = kMaxEthPacketQueueSize;
}
return queue_size;
}
#if 0
/* for pointcloud convert process */
static uint32_t PublishPointcloud2(StoragePacketQueue* queue, uint32_t packet_num, \
uint8_t handle) {
uint64_t timestamp = 0;
uint64_t last_timestamp = 0;
uint32_t published_packet = 0;
uint32_t point_num = 0;
uint32_t kPointXYZISize = 16;
sensor_msgs::PointCloud2 cloud;
if (!packet_num) {
return -1;
}
// init ROS standard message header
cloud.header.frame_id = "livox_frame";
cloud.height = 1;
cloud.width = 0;
// init ROS standard fields
cloud.fields.resize(4);
cloud.fields[0].offset = 0;
cloud.fields[0].name = "x";
cloud.fields[0].count = 1;
cloud.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
cloud.fields[1].offset = 4;
cloud.fields[1].name = "y";
cloud.fields[1].count = 1;
cloud.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
cloud.fields[2].offset = 8;
cloud.fields[2].name = "z";
cloud.fields[2].count = 1;
cloud.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
cloud.fields[3].offset = 12;
cloud.fields[3].name = "intensity";
cloud.fields[3].count = 1;
cloud.fields[3].datatype = sensor_msgs::PointField::FLOAT32;
// add pointcloud
cloud.data.resize(packet_num * kMaxPointPerEthPacket * kPointXYZISize);
cloud.point_step = kPointXYZISize;
uint8_t *point_base = cloud.data.data();
StoragePacket storage_packet;
while (published_packet < packet_num) {
QueueProPop(queue, &storage_packet);
LivoxEthPacket* raw_packet = reinterpret_cast<LivoxEthPacket *>(storage_packet.raw_data);
LivoxRawPoint* raw_points = reinterpret_cast<LivoxRawPoint *>(raw_packet->data);
timestamp = GetStoragePacketTimestamp(&storage_packet);
if (published_packet && \
((timestamp - last_timestamp) > kMaxPacketTimeGap)) {
ROS_INFO("packet loss : %ld", timestamp);
break;
}
if (!cloud.width) {
cloud.header.stamp = ros::Time(timestamp/1000000000.0); // to ros time stamp
ROS_DEBUG("[%d]:%ld us", handle, timestamp);
}
cloud.width += storage_packet.point_num;
for (uint32_t i = 0; i < storage_packet.point_num; i++) {
*((float*)(point_base + 0)) = raw_points->x/1000.0f;
*((float*)(point_base + 4)) = raw_points->y/1000.0f;
*((float*)(point_base + 8)) = raw_points->z/1000.0f;
*((float*)(point_base + 12)) = (float) raw_points->reflectivity;
++raw_points;
++point_num;
point_base += kPointXYZISize;
}
QueuePopUpdate(queue);
last_timestamp = timestamp;
++published_packet;
}
//ROS_INFO("%d", cloud.width);
cloud.row_step = cloud.width * cloud.point_step;
cloud.is_bigendian = false;
cloud.is_dense = true;
// adjust the real size
//cloud.data.resize(cloud.row_step);
//ROS_INFO("%ld", cloud.data.capacity());
cloud_pub.publish(cloud);
return published_packet;
}
#endif
/* for pointcloud convert process */
static uint32_t PublishPointcloud2(StoragePacketQueue* queue, uint32_t packet_num, \
uint8_t handle) {
uint64_t timestamp = 0;
uint64_t last_timestamp = 0;
uint32_t published_packet = 0;
uint32_t point_num = 0;
/* init point cloud data struct */
PointCloud::Ptr cloud (new PointCloud);
cloud->header.frame_id = "livox_frame";
cloud->height = 1;
cloud->width = 0;
// add pointcloud
StoragePacket storage_packet;
while (published_packet < packet_num) {
QueueProPop(queue, &storage_packet);
LivoxEthPacket* raw_packet = reinterpret_cast<LivoxEthPacket *>(storage_packet.raw_data);
LivoxRawPoint* raw_points = reinterpret_cast<LivoxRawPoint *>(raw_packet->data);
timestamp = GetStoragePacketTimestamp(&storage_packet);
if (published_packet && \
((timestamp - last_timestamp) > kMaxPacketTimeGap)) {
ROS_INFO("packet loss : %ld", timestamp);
break;
}
if (!cloud->width) {
//cloud->header.stamp = ros::Time(timestamp/1000000000.0); // to ros time stamp
cloud->header.stamp = timestamp/1000.0; // to ros time stamp
ROS_DEBUG("[%d]:%ld us", handle, timestamp);
}
cloud->width += storage_packet.point_num;
for (uint32_t i = 0; i < storage_packet.point_num; i++) {
pcl::PointXYZI point;
point.x = raw_points->x/1000.0f;
point.y = raw_points->y/1000.0f;
point.z = raw_points->z/1000.0f;
point.intensity = (float)raw_points->reflectivity;
cloud->points.push_back(point);
++raw_points;
++point_num;
}
QueuePopUpdate(queue);
last_timestamp = timestamp;
++published_packet;
}
//ROS_INFO("%ld", ptr_cloud->data.capacity());
cloud_pub.publish(cloud);
return published_packet;
}
/* for pointcloud convert process */
static uint32_t PublishCustomPointcloud(StoragePacketQueue* queue, uint32_t packet_num,\
uint8_t handle) {
static uint32_t msg_seq = 0;
uint64_t timestamp = 0;
uint64_t last_timestamp = 0;
uint32_t published_packet = 0;
uint32_t point_interval = GetPointInterval(lidars[handle].info.type);
uint32_t packet_offset_time = 0; // ns
/* init livox custom msg */
livox_ros_driver::CustomMsg livox_msg;
livox_msg.header.frame_id = "livox_frame";
livox_msg.header.seq = msg_seq;
++msg_seq;
livox_msg.header.stamp = ros::Time::now();
livox_msg.timebase = 0;
livox_msg.point_num = 0;
livox_msg.lidar_id = handle;
StoragePacket storage_packet;
while (published_packet < packet_num) {
QueueProPop(queue, &storage_packet);
LivoxEthPacket* raw_packet = reinterpret_cast<LivoxEthPacket *>(storage_packet.raw_data);
LivoxRawPoint* raw_points = reinterpret_cast<LivoxRawPoint *>(raw_packet->data);
timestamp = GetStoragePacketTimestamp(&storage_packet);
if (published_packet && \
((timestamp - last_timestamp) > kDeviceDisconnectThreshold)) {
ROS_INFO("packet loss : %ld", timestamp);
break;
}
if (!livox_msg.timebase) {
livox_msg.timebase = timestamp; // to us
packet_offset_time = 0; // first packet
ROS_DEBUG("[%d]:%ld %d", handle, livox_msg.timebase, point_interval);
} else {
packet_offset_time = (uint32_t)(timestamp - livox_msg.timebase);
}
livox_msg.point_num += storage_packet.point_num;
for (uint32_t i = 0; i < storage_packet.point_num; i++) {
livox_ros_driver::CustomPoint point;
point.offset_time = packet_offset_time + i*point_interval;
point.x = raw_points->x/1000.0f;
point.y = raw_points->y/1000.0f;
point.z = raw_points->z/1000.0f;
point.reflectivity = raw_points->reflectivity;
point.line = 0;
++raw_points;
livox_msg.points.push_back(point);
}
QueuePopUpdate(queue);
last_timestamp = timestamp;
++published_packet;
}
//ROS_INFO("%d", livox_msg.point_num);
cloud_pub.publish(livox_msg);
return published_packet;
}
static void PointCloudConvert(LivoxPoint *p_dpoint, LivoxRawPoint *p_raw_point) {
p_dpoint->x = p_raw_point->x/1000.0f;
p_dpoint->y = p_raw_point->y/1000.0f;
p_dpoint->z = p_raw_point->z/1000.0f;
p_dpoint->reflectivity = p_raw_point->reflectivity;
}
void GetLidarData(uint8_t hub_handle, LivoxEthPacket *data, uint32_t data_num, void *client_data) {
using namespace std;
LivoxEthPacket *lidar_pack = data;
if (!data || !data_num) {
return;
}
/* caculate which lidar this eth packet data belong to */
uint8_t handle = HubGetLidarHandle(lidar_pack->slot, lidar_pack->id);
if (handle >= kMaxLidarCount) {
return;
}
LidarPacketStatistic *packet_statistic = &lidars[handle].statistic_info;
uint64_t cur_timestamp = *((uint64_t *)(lidar_pack->timestamp));
if (lidar_pack->timestamp_type == kTimestampTypePps) {
if ((cur_timestamp < packet_statistic->last_timestamp) && \
(cur_timestamp < kPacketTimeGap)) { // sync point
auto cur_time = chrono::high_resolution_clock::now();
int64_t sync_time = cur_time.time_since_epoch().count();
packet_statistic->timebase = sync_time;
//ROS_DEBUG("sysnc time : %lu %lu %lu", packet_statistic->timebase, cur_timestamp, \
// packet_statistic->last_timestamp);
}
}
packet_statistic->last_timestamp = cur_timestamp;
StoragePacketQueue *p_queue = &lidars[handle].packet_queue;
if (nullptr == p_queue->storage_packet) {
uint32_t queue_size = CalculatePacketQueueSize(kPublishIntervalMs, lidars[handle].info.type);
InitQueue(p_queue, queue_size);
}
if (!QueueIsFull(p_queue)) {
if (data_num <= kMaxPointPerEthPacket) {
PushEthPacketToStorageQueue(p_queue, lidar_pack, data_num, packet_statistic->timebase);
}
}
}
void PollPointcloudData(int msg_type) {
for (int i = 0; i < kMaxLidarCount; i++) {
StoragePacketQueue *p_queue = &lidars[i].packet_queue;
if (kDeviceStateSampling != lidars[i].device_state) {
continue;
}
while (!QueueIsEmpty(p_queue)) {
//ROS_DEBUG("%d %d %d %d\r\n", i, p_queue->rd_idx, p_queue->wr_idx, QueueUsedSize(p_queue));
uint32_t used_size = QueueUsedSize(p_queue);
if (kPointCloud2Msg == msg_type) {
if (used_size == PublishPointcloud2(p_queue, used_size, i)) {
break;
}
} else {
if (used_size == PublishCustomPointcloud(p_queue, QueueUsedSize(p_queue), i)) {
break;
}
}
}
}
}
/** add bd to total_broadcast_code */
void AddBroadcastCode(const char* bd_str) {
total_broadcast_code.push_back(bd_str);
}
/** add bd in broadcast_code_list to total_broadcast_code */
void AddLocalBroadcastCode(void) {
for (int i = 0; i < BROADCAST_CODE_LIST_SIZE; ++i) {
std::string invalid_bd = "000000000";
ROS_INFO("broadcast code list :%s", broadcast_code_list[i]);
if ((kCommandlineBdSize == strlen(broadcast_code_list[i])) && \
(NULL == strstr(broadcast_code_list[i], invalid_bd.c_str()))) {
AddBroadcastCode(broadcast_code_list[i]);
} else {
ROS_INFO("Invalid bd:%s", broadcast_code_list[i]);
}
}
}
/** add commandline bd to total_broadcast_code */
void AddCommandlineBroadcastCode(const char* cammandline_str) {
char* strs = new char[strlen(cammandline_str) + 1];
strcpy(strs, cammandline_str);
std::string pattern = "&";
char* bd_str = strtok(strs, pattern.c_str());
std::string invalid_bd = "000000000";
while (bd_str != NULL) {
ROS_INFO("commandline input bd:%s", bd_str);
if ((kCommandlineBdSize == strlen(bd_str)) && \
(NULL == strstr(bd_str, invalid_bd.c_str()))) {
AddBroadcastCode(bd_str);
} else {
ROS_INFO("Invalid bd:%s", bd_str);
}
bd_str = strtok(NULL, pattern.c_str());
}
delete [] strs;
}
/* control hub ---------------------------------------------------------------------------------- */
void OnSampleCallback(uint8_t status, uint8_t hub_handle, uint8_t response, void *data) {
ROS_INFO("OnSampleCallback statue %d handle %d response %d \n", status, hub_handle, response);
if (status == kStatusSuccess) {
if (response != 0) {
hub.device_state = kDeviceStateConnect;
}
} else if (status == kStatusTimeout) {
hub.device_state = kDeviceStateConnect;
}
}
/** Callback function of stopping sampling. */
void OnStopSampleCallback(uint8_t status, uint8_t hub_handle, uint8_t response, void *data) {
}
/** Query the firmware version of Livox Hub. */
void OnDeviceInformation(uint8_t status, uint8_t handle, DeviceInformationResponse *ack, void *data) {
if (status != kStatusSuccess) {
ROS_INFO("Device Query Informations Failed %d", status);
}
if (ack) {
ROS_INFO("firm ver: %d.%d.%d.%d",
ack->firmware_version[0],
ack->firmware_version[1],
ack->firmware_version[2],
ack->firmware_version[3]);
}
}
void OnHubLidarInfo(uint8_t status, uint8_t handle, HubQueryLidarInformationResponse *response, void *client_data) {
if (status != kStatusSuccess) {
printf("Device Query Informations Failed %d\n", status);
}
if (response) {
int i = 0;
for (i = 0; i < response->count; ++i) {
printf("Hub Lidar Info broadcast code %s id %d slot %d \n ",
response->device_info_list[i].broadcast_code,
response->device_info_list[i].id,
response->device_info_list[i].slot);
}
}
}
void OnDeviceChange(const DeviceInfo *info, DeviceEvent type) {
if (info == NULL) {
return;
}
ROS_INFO("OnDeviceChange broadcast code %s update type %d", info->broadcast_code, type);
uint8_t hub_handle = info->handle;
if (hub_handle >= kMaxLidarCount) {
return;
}
if (type == kEventConnect) {
DeviceInfo *_lidars = (DeviceInfo *) malloc(sizeof(DeviceInfo) * kMaxLidarCount);
uint8_t count = kMaxLidarCount;
uint8_t status = GetConnectedDevices(_lidars, &count);
if (status == kStatusSuccess) {
int i = 0;
for (i = 0; i < count; ++i) {
uint8_t handle = _lidars[i].handle;
if (handle < kMaxLidarCount) {
lidars[handle].handle = handle;
lidars[handle].info = _lidars[i];
lidars[handle].device_state = kDeviceStateSampling;
ROS_INFO("lidar %d : %s\r\n", _lidars[i].handle, _lidars[i].broadcast_code);
}
}
}
if (_lidars) {
free(_lidars);
}
if (info->type == kDeviceTypeHub) {
HubQueryLidarInformation(OnHubLidarInfo, NULL);
}
if (hub.device_state == kDeviceStateDisconnect) {
hub.device_state = kDeviceStateConnect;
hub.info = *info;
}
} else if (type == kEventDisconnect) {
hub.device_state = kDeviceStateDisconnect;
} else if (type == kEventStateChange) {
hub.info = *info;
}
if (hub.device_state == kDeviceStateConnect) {
ROS_INFO("Device State status_code %d", hub.info.status.status_code);
ROS_INFO("Device State working state %d", hub.info.state);
ROS_INFO("Device feature %d", hub.info.feature);
if (hub.info.state == kLidarStateNormal) {
HubStartSampling(OnSampleCallback, NULL);
hub.device_state = kDeviceStateSampling;
}
}
}
void OnDeviceBroadcast(const BroadcastDeviceInfo *info) {
if (info == NULL) {
return;
}
ROS_INFO("Receive Broadcast Code %s, please add it to broacast_code_list if want to connect!\n",\
info->broadcast_code);
if (total_broadcast_code.size() > 0) {
bool found = false;
for (int i = 0; i < total_broadcast_code.size(); ++i) {
if (strncmp(info->broadcast_code, total_broadcast_code[i].c_str(), kBroadcastCodeSize) == 0) {
found = true;
break;
}
}
if (!found) {
ROS_INFO("Not in the broacast_code_list, please add it to if want to connect!");
return;
}
} else {
ROS_INFO("In automatic connection mode, will connect %s", info->broadcast_code);
}
bool result = false;
uint8_t hub_handle = 0;
result = AddHubToConnect(info->broadcast_code, &hub_handle);
if (result == kStatusSuccess && hub_handle < kMaxLidarCount) {
ROS_INFO("set callback");
SetDataCallback(hub_handle, GetLidarData, NULL);
hub.handle = hub_handle;
hub.device_state = kDeviceStateDisconnect;
}
}
int main(int argc, char **argv) {
if( ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info) ) {
ros::console::notifyLoggerLevelsChanged();
}
ROS_INFO("Livox-SDK ros demo");
if (!Init()) {
ROS_FATAL("Livox-SDK init fail!");
return -1;
}
AddLocalBroadcastCode();
if (argc >= kBdArgcNum) {
ROS_INFO("Commandline input %s", argv[kBdArgvPos]);
AddCommandlineBroadcastCode(argv[kBdArgvPos]);
}
if (total_broadcast_code.size() > 0) {
ROS_INFO("list all valid bd:");
for (int i = 0; i < total_broadcast_code.size(); ++i) {
ROS_INFO("%s", total_broadcast_code[i].c_str());
}
} else {
ROS_INFO("No valid bd input, switch to automatic connection mode!");
}
memset(lidars, 0, sizeof(lidars));
memset(&hub, 0, sizeof(hub));
SetBroadcastCallback(OnDeviceBroadcast);
SetDeviceStateUpdateCallback(OnDeviceChange);
if (!Start()) {
Uninit();
return -1;
}
/* ros related */
ros::init(argc, argv, "livox_hub_publisher");
ros::NodeHandle livox_node;
int msg_type;
livox_node.getParam("livox_msg_type", msg_type);
if (kPointCloud2Msg == msg_type) {
cloud_pub = livox_node.advertise<sensor_msgs::PointCloud2>("livox/hub", kMaxEthPacketQueueSize);
ROS_INFO("Publish PointCloud2");
} else {
cloud_pub = livox_node.advertise<livox_ros_driver::CustomMsg>("livox/hub", \
kMaxEthPacketQueueSize);
ROS_INFO("Publish Livox Custom Msg");
}
ros::Time::init();
ros::Rate r(1000.0 / kPublishIntervalMs); // 1000.0 / x = hz
while (ros::ok()) {
PollPointcloudData(msg_type);
r.sleep();
}
Uninit();
}

800
livox_ros_driver/livox_lidar.cpp
View File

@@ -1,800 +0,0 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2019 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <stdint.h>
#include <stdbool.h>
#include <stddef.h>
#include <math.h>
#include <time.h>
#include <vector>
#include <chrono>
#include "livox_sdk.h"
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <livox_ros_driver/CustomPoint.h>
#include <livox_ros_driver/CustomMsg.h>
#include <pcl_ros/point_cloud.h>
/* const varible -------------------------------------------------------------------------------- */
/* user area */
const uint32_t kPublishIntervalMs = 50; // unit:ms
/* const parama */
const uint32_t kMaxPointPerEthPacket = 100;
const uint32_t kMinEthPacketQueueSize = 128; // must be 2^n
const uint32_t kMaxEthPacketQueueSize = 8192; // must be 2^n
const uint32_t KEthPacketHeaderLength = 18; // (sizeof(LivoxEthPacket) - 1)
const uint32_t KEthPacketMaxLength = 1500;
const uint32_t KCartesianPointSize = 13;
const uint32_t KSphericalPointSzie = 9;
const uint64_t kPacketTimeGap = 1000000; // 1ms = 1000000ns
const uint64_t kMaxPacketTimeGap = 1700000; // the threshold of packet continuous
const uint64_t kDeviceDisconnectThreshold = 1000000000; // the threshold of device disconect
const uint64_t kNsPerSecond = 1000000000; // 1s = 1000000000ns
const uint32_t kPublishIntervalUpperLimitMs = 2000; // limit upper boundary 2s
const uint32_t kPublishIntervalLowerLimitMs = 2; // limit lower boundary to 2ms
const int kBdArgcNum = 4;
const int kBdArgvPos = 1;
const int kCommandlineBdSize = 15;
#pragma pack(1)
typedef struct {
uint64_t time_rcv; // used for pps sync only mode
uint32_t point_num;
uint8_t raw_data[KEthPacketMaxLength];
} StoragePacket;
typedef struct {
uint8_t lidar_id;
uint8_t rsvd[3];
uint32_t point_num;
uint64_t timestamp; // ns
LivoxPoint *point;
} PublishPacket;
#pragma pack()
typedef struct {
StoragePacket *storage_packet;
volatile uint32_t rd_idx;
volatile uint32_t wr_idx;
uint32_t mask;
uint32_t size; // must be 2^n
} StoragePacketQueue;
typedef struct {
uint32_t receive_packet_count;
uint32_t loss_packet_count;
uint64_t last_timestamp;
uint64_t timebase; // unit:nanosecond
uint32_t timebase_state;
} LidarPacketStatistic;
typedef enum {
kCoordinateCartesian = 0,
kCoordinateSpherical
} CoordinateType;
typedef enum {
kPointCloud2Msg = 0,
kLivoxCustomMsg
} LivoxMsgType;
/* for global publisher use */
ros::Publisher cloud_pub;
typedef pcl::PointCloud<pcl::PointXYZI> PointCloud;
/* for device connect use ----------------------------------------------------------------------- */
typedef enum {
kDeviceStateDisconnect = 0,
kDeviceStateConnect = 1,
kDeviceStateSampling = 2,
} DeviceState;
typedef struct {
uint8_t handle;
DeviceState device_state;
DeviceInfo info;
LidarPacketStatistic statistic_info;
StoragePacketQueue packet_queue;
} DeviceItem;
DeviceItem lidars[kMaxLidarCount];
/* user add broadcast code here */
const char* broadcast_code_list[] = {
"000000000000001",
};
#define BROADCAST_CODE_LIST_SIZE (sizeof(broadcast_code_list) / sizeof(intptr_t))
/* total broadcast code, include broadcast_code_list and commandline input */
std::vector<std::string > total_broadcast_code;
/* power of 2 buferr operation */
static bool IsPowerOf2(uint32_t size) {
return (size != 0) && ((size & (size - 1)) == 0);
}
static uint32_t RoundupPowerOf2(uint32_t size) {
uint32_t power2_val = 0;
for(int i = 0; i < 32; i++) {
power2_val = ((uint32_t)1) << i;
if(size <= power2_val) {
break;
}
}
return power2_val;
}
/* for pointcloud queue process */
int InitQueue(StoragePacketQueue* queue, uint32_t queue_size) {
if (queue == NULL) {
return 1;
}
if(IsPowerOf2(queue_size) != true) {
queue_size = RoundupPowerOf2(queue_size);
}
queue->storage_packet = new StoragePacket [queue_size];
if(queue->storage_packet == nullptr) {
return 1;
}
queue->rd_idx = 0;
queue->wr_idx = 0;
queue->size = queue_size;
queue->mask = queue_size - 1;
return 0;
}
void ResetQueue(StoragePacketQueue* queue) {
queue->rd_idx = 0;
queue->wr_idx = 0;
}
static void QueueProPop(StoragePacketQueue* queue, StoragePacket* storage_packet) {
uint32_t mask = queue->mask;
uint32_t rd_idx = queue->rd_idx & mask;
memcpy(storage_packet, &(queue->storage_packet[rd_idx]), sizeof(StoragePacket));
}
static void QueuePopUpdate(StoragePacketQueue* queue) {
queue->rd_idx++;
}
uint32_t QueuePop(StoragePacketQueue* queue, StoragePacket* storage_packet) {
QueueProPop(queue, storage_packet);
QueuePopUpdate(queue);
return 1;
}
uint32_t QueueUsedSize(StoragePacketQueue *queue) {
return (queue->wr_idx - queue->rd_idx) & queue->mask;
}
uint32_t QueueIsFull(StoragePacketQueue *queue) {
return ((queue->wr_idx + 1) == queue->rd_idx);
}
uint32_t QueueIsEmpty(StoragePacketQueue *queue) {
return (queue->rd_idx == queue->wr_idx);
}
static uint32_t GetEthPacketLen(LivoxEthPacket* eth_packet, uint32_t point_num) {
if (kCoordinateCartesian == eth_packet->data_type) {
return (KEthPacketHeaderLength + point_num * KCartesianPointSize);
} else {
return (KEthPacketHeaderLength + point_num * KSphericalPointSzie);
}
}
uint32_t PushEthPacketToStorageQueue(StoragePacketQueue* queue, LivoxEthPacket* eth_packet, \
uint32_t point_num, uint64_t timebase) {
uint32_t mask = queue->mask;
uint32_t wr_idx = queue->wr_idx & mask;
queue->storage_packet[wr_idx].time_rcv = timebase;
queue->storage_packet[wr_idx].point_num = point_num;
memcpy(queue->storage_packet[wr_idx].raw_data, \
reinterpret_cast<char *>(eth_packet), \
GetEthPacketLen(eth_packet, point_num));
queue->wr_idx++;
return 1;
}
static uint64_t GetStoragePacketTimestamp(StoragePacket* packet) {
LivoxEthPacket* raw_packet = reinterpret_cast<LivoxEthPacket *>(packet->raw_data);
uint64_t timestamp = *((uint64_t *)(raw_packet->timestamp));
if (raw_packet->timestamp_type == kTimestampTypePps) {
return (timestamp + packet->time_rcv);
} else if (raw_packet->timestamp_type == kTimestampTypeNoSync) {
return timestamp;
} else if (raw_packet->timestamp_type == kTimestampTypePtp) {
return timestamp;
} else if (raw_packet->timestamp_type == kTimestampTypePpsGps) {
struct tm time_utc;
time_utc.tm_isdst = 0;
time_utc.tm_year = raw_packet->timestamp[0] + 100; // map 2000 to 1990
time_utc.tm_mon = raw_packet->timestamp[1];
time_utc.tm_mday = raw_packet->timestamp[2];
time_utc.tm_hour = raw_packet->timestamp[3];
time_utc.tm_min = 0;
time_utc.tm_sec = 0;
uint64_t time_epoch = mktime(&time_utc);
time_epoch = time_epoch * 1000000 + *((uint32_t *)(&(raw_packet->timestamp[4]))); // to us
time_epoch = time_epoch * 1000; // to ns
return time_epoch;
} else {
ROS_INFO("timestamp type invalid");
return 0;
}
}
static uint32_t GetPointInterval(uint32_t device_type) {
if ((kDeviceTypeLidarTele == device_type) || \
(kDeviceTypeLidarHorizon == device_type)) {
return 4167; // 4167 ns
} else {
return 10000; // ns
}
}
static uint32_t GetPacketNumPerSec(uint32_t device_type) {
if ((kDeviceTypeLidarTele == device_type) || \
(kDeviceTypeLidarHorizon == device_type)) {
return 2400; // 2400 raw packet per second
} else {
return 1000; // 1000 raw packet per second
}
}
static uint32_t CalculatePacketQueueSize(uint32_t interval_ms, uint32_t device_type) {
uint32_t queue_size = (1000000.0 / interval_ms) * GetPacketNumPerSec(device_type);
if (queue_size < kMinEthPacketQueueSize) {
queue_size = kMinEthPacketQueueSize;
} else if (queue_size > kMaxEthPacketQueueSize) {
queue_size = kMaxEthPacketQueueSize;
}
return queue_size;
}
#if 0
/* for pointcloud convert process */
static uint32_t PublishPointcloud2(StoragePacketQueue* queue, uint32_t packet_num, \
uint8_t handle) {
uint64_t timestamp = 0;
uint64_t last_timestamp = 0;
uint32_t published_packet = 0;
uint32_t point_num = 0;
uint32_t kPointXYZISize = 16;
sensor_msgs::PointCloud2 cloud;
if (!packet_num) {
return -1;
}
// init ROS standard message header
cloud.header.frame_id = "livox_frame";
cloud.height = 1;
cloud.width = 0;
// init ROS standard fields
cloud.fields.resize(4);
cloud.fields[0].offset = 0;
cloud.fields[0].name = "x";
cloud.fields[0].count = 1;
cloud.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
cloud.fields[1].offset = 4;
cloud.fields[1].name = "y";
cloud.fields[1].count = 1;
cloud.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
cloud.fields[2].offset = 8;
cloud.fields[2].name = "z";
cloud.fields[2].count = 1;
cloud.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
cloud.fields[3].offset = 12;
cloud.fields[3].name = "intensity";
cloud.fields[3].count = 1;
cloud.fields[3].datatype = sensor_msgs::PointField::FLOAT32;
// add pointcloud
cloud.data.resize(packet_num * kMaxPointPerEthPacket * kPointXYZISize);
cloud.point_step = kPointXYZISize;
uint8_t *point_base = cloud.data.data();
StoragePacket storage_packet;
while (published_packet < packet_num) {
QueueProPop(queue, &storage_packet);
LivoxEthPacket* raw_packet = reinterpret_cast<LivoxEthPacket *>(storage_packet.raw_data);
LivoxRawPoint* raw_points = reinterpret_cast<LivoxRawPoint *>(raw_packet->data);
timestamp = GetStoragePacketTimestamp(&storage_packet);
if (published_packet && \
((timestamp - last_timestamp) > kMaxPacketTimeGap)) {
ROS_INFO("packet loss : %ld", timestamp);
break;
}
if (!cloud.width) {
cloud.header.stamp = ros::Time(timestamp/1000000000.0); // to ros time stamp
ROS_DEBUG("[%d]:%ld us", handle, timestamp);
}
cloud.width += storage_packet.point_num;
for (uint32_t i = 0; i < storage_packet.point_num; i++) {
*((float*)(point_base + 0)) = raw_points->x/1000.0f;
*((float*)(point_base + 4)) = raw_points->y/1000.0f;
*((float*)(point_base + 8)) = raw_points->z/1000.0f;
*((float*)(point_base + 12)) = (float) raw_points->reflectivity;
++raw_points;
++point_num;
point_base += kPointXYZISize;
}
QueuePopUpdate(queue);
last_timestamp = timestamp;
++published_packet;
}
//ROS_INFO("%d", cloud.width);
cloud.row_step = cloud.width * cloud.point_step;
cloud.is_bigendian = false;
cloud.is_dense = true;
// adjust the real size
//cloud.data.resize(cloud.row_step);
//ROS_INFO("%ld", cloud.data.capacity());
cloud_pub.publish(cloud);
return published_packet;
}
#endif
/* for pointcloud convert process */
static uint32_t PublishPointcloud2(StoragePacketQueue* queue, uint32_t packet_num, \
uint8_t handle) {
uint64_t timestamp = 0;
uint64_t last_timestamp = 0;
uint32_t published_packet = 0;
uint32_t point_num = 0;
/* init point cloud data struct */
PointCloud::Ptr cloud (new PointCloud);
cloud->header.frame_id = "livox_frame";
cloud->height = 1;
cloud->width = 0;
// add pointcloud
StoragePacket storage_packet;
while (published_packet < packet_num) {
QueueProPop(queue, &storage_packet);
LivoxEthPacket* raw_packet = reinterpret_cast<LivoxEthPacket *>(storage_packet.raw_data);
LivoxRawPoint* raw_points = reinterpret_cast<LivoxRawPoint *>(raw_packet->data);
timestamp = GetStoragePacketTimestamp(&storage_packet);
if (published_packet && \
((timestamp - last_timestamp) > kMaxPacketTimeGap)) {
ROS_INFO("packet loss : %ld", timestamp);
break;
}
if (!cloud->width) {
//cloud->header.stamp = ros::Time(timestamp/1000000000.0); // to ros time stamp
cloud->header.stamp = timestamp/1000.0; // to ros time stamp
ROS_DEBUG("[%d]:%ld us", handle, timestamp);
}
cloud->width += storage_packet.point_num;
for (uint32_t i = 0; i < storage_packet.point_num; i++) {
pcl::PointXYZI point;
point.x = raw_points->x/1000.0f;
point.y = raw_points->y/1000.0f;
point.z = raw_points->z/1000.0f;
point.intensity = (float)raw_points->reflectivity;
cloud->points.push_back(point);
++raw_points;
++point_num;
}
QueuePopUpdate(queue);
last_timestamp = timestamp;
++published_packet;
}
//ROS_INFO("%ld", ptr_cloud->data.capacity());
cloud_pub.publish(cloud);
return published_packet;
}
/* for pointcloud convert process */
static uint32_t PublishCustomPointcloud(StoragePacketQueue* queue, uint32_t packet_num,\
uint8_t handle) {
static uint32_t msg_seq = 0;
uint64_t timestamp = 0;
uint64_t last_timestamp = 0;
uint32_t published_packet = 0;
uint32_t point_interval = GetPointInterval(lidars[handle].info.type);
uint32_t packet_offset_time = 0; // ns
/* init livox custom msg */
livox_ros_driver::CustomMsg livox_msg;
livox_msg.header.frame_id = "livox_frame";
livox_msg.header.seq = msg_seq;
++msg_seq;
livox_msg.header.stamp = ros::Time::now();
livox_msg.timebase = 0;
livox_msg.point_num = 0;
livox_msg.lidar_id = handle;
StoragePacket storage_packet;
while (published_packet < packet_num) {
QueueProPop(queue, &storage_packet);
LivoxEthPacket* raw_packet = reinterpret_cast<LivoxEthPacket *>(storage_packet.raw_data);
LivoxRawPoint* raw_points = reinterpret_cast<LivoxRawPoint *>(raw_packet->data);
timestamp = GetStoragePacketTimestamp(&storage_packet);
if (published_packet && \
((timestamp - last_timestamp) > kDeviceDisconnectThreshold)) {
ROS_INFO("packet loss : %ld", timestamp);
break;
}
if (!livox_msg.timebase) {
livox_msg.timebase = timestamp; // to us
packet_offset_time = 0; // first packet
ROS_DEBUG("[%d]:%ld %d", handle, livox_msg.timebase, point_interval);
} else {
packet_offset_time = (uint32_t)(timestamp - livox_msg.timebase);
}
livox_msg.point_num += storage_packet.point_num;
for (uint32_t i = 0; i < storage_packet.point_num; i++) {
livox_ros_driver::CustomPoint point;
point.offset_time = packet_offset_time + i*point_interval;
point.x = raw_points->x/1000.0f;
point.y = raw_points->y/1000.0f;
point.z = raw_points->z/1000.0f;
point.reflectivity = raw_points->reflectivity;
point.line = 0;
++raw_points;
livox_msg.points.push_back(point);
}
QueuePopUpdate(queue);
last_timestamp = timestamp;
++published_packet;
}
//ROS_INFO("%d", livox_msg.point_num);
cloud_pub.publish(livox_msg);
return published_packet;
}
static void PointCloudConvert(LivoxPoint *p_dpoint, LivoxRawPoint *p_raw_point) {
p_dpoint->x = p_raw_point->x/1000.0f;
p_dpoint->y = p_raw_point->y/1000.0f;
p_dpoint->z = p_raw_point->z/1000.0f;
p_dpoint->reflectivity = p_raw_point->reflectivity;
}
void GetLidarData(uint8_t handle, LivoxEthPacket *data, uint32_t data_num, void *client_data) {
using namespace std;
LivoxEthPacket* lidar_pack = data;
if (!data || !data_num || (handle >= kMaxLidarCount)) {
return;
}
LidarPacketStatistic *packet_statistic = &lidars[handle].statistic_info;
uint64_t cur_timestamp = *((uint64_t *)(lidar_pack->timestamp));
if (lidar_pack->timestamp_type == kTimestampTypePps) {
if ((cur_timestamp < packet_statistic->last_timestamp) && \
(cur_timestamp < kPacketTimeGap)) { // sync point
auto cur_time = chrono::high_resolution_clock::now();
int64_t sync_time = cur_time.time_since_epoch().count();
packet_statistic->timebase = sync_time;
//ROS_DEBUG("sysnc time : %lu %lu %lu", packet_statistic->timebase, cur_timestamp, \
// packet_statistic->last_timestamp);
}
}
packet_statistic->last_timestamp = cur_timestamp;
StoragePacketQueue *p_queue = &lidars[handle].packet_queue;
if (nullptr == p_queue->storage_packet) {
uint32_t queue_size = CalculatePacketQueueSize(kPublishIntervalMs, lidars[handle].info.type);
InitQueue(p_queue, queue_size);
}
if (!QueueIsFull(p_queue)) {
if (data_num <= kMaxPointPerEthPacket) {
PushEthPacketToStorageQueue(p_queue, lidar_pack, data_num, packet_statistic->timebase);
}
}
}
void PollPointcloudData(int msg_type) {
for (int i = 0; i < kMaxLidarCount; i++) {
StoragePacketQueue *p_queue = &lidars[i].packet_queue;
if (kDeviceStateSampling != lidars[i].device_state) {
continue;
}
while (!QueueIsEmpty(p_queue)) {
//ROS_DEBUG("%d %d %d %d\r\n", i, p_queue->rd_idx, p_queue->wr_idx, QueueUsedSize(p_queue));
uint32_t used_size = QueueUsedSize(p_queue);
if (kPointCloud2Msg == msg_type) {
if (used_size == PublishPointcloud2(p_queue, used_size, i)) {
break;
}
} else {
if (used_size == PublishCustomPointcloud(p_queue, QueueUsedSize(p_queue), i)) {
break;
}
}
}
}
}
/** add bd to total_broadcast_code */
void AddBroadcastCode(const char* bd_str) {
total_broadcast_code.push_back(bd_str);
}
/** add bd in broadcast_code_list to total_broadcast_code */
void AddLocalBroadcastCode(void) {
for (int i = 0; i < BROADCAST_CODE_LIST_SIZE; ++i) {
std::string invalid_bd = "000000000";
ROS_INFO("broadcast code list :%s", broadcast_code_list[i]);
if ((kCommandlineBdSize == strlen(broadcast_code_list[i])) && \
(NULL == strstr(broadcast_code_list[i], invalid_bd.c_str()))) {
AddBroadcastCode(broadcast_code_list[i]);
} else {
ROS_INFO("Invalid bd:%s", broadcast_code_list[i]);
}
}
}
/** add commandline bd to total_broadcast_code */
void AddCommandlineBroadcastCode(const char* cammandline_str) {
char* strs = new char[strlen(cammandline_str) + 1];
strcpy(strs, cammandline_str);
std::string pattern = "&";
char* bd_str = strtok(strs, pattern.c_str());
std::string invalid_bd = "000000000";
while (bd_str != NULL) {
ROS_INFO("commandline input bd:%s", bd_str);
if ((kCommandlineBdSize == strlen(bd_str)) && \
(NULL == strstr(bd_str, invalid_bd.c_str()))) {
AddBroadcastCode(bd_str);
} else {
ROS_INFO("Invalid bd:%s", bd_str);
}
bd_str = strtok(NULL, pattern.c_str());
}
delete [] strs;
}
/** Callback function of starting sampling. */
void OnSampleCallback(uint8_t status, uint8_t handle, uint8_t response, void *data) {
ROS_INFO("OnSampleCallback statue %d handle %d response %d", status, handle, response);
if (status == kStatusSuccess) {
if (response != 0) {
lidars[handle].device_state = kDeviceStateConnect;
}
} else if (status == kStatusTimeout) {
lidars[handle].device_state = kDeviceStateConnect;
}
}
/** Callback function of stopping sampling. */
void OnStopSampleCallback(uint8_t status, uint8_t handle, uint8_t response, void *data) {
}
/** Query the firmware version of Livox LiDAR. */
void OnDeviceInformation(uint8_t status, uint8_t handle, DeviceInformationResponse *ack, void *data) {
if (status != kStatusSuccess) {
ROS_INFO("Device Query Informations Failed %d", status);
}
if (ack) {
ROS_INFO("firm ver: %d.%d.%d.%d",
ack->firmware_version[0],
ack->firmware_version[1],
ack->firmware_version[2],
ack->firmware_version[3]);
}
}
/** Callback function of changing of device state. */
void OnDeviceChange(const DeviceInfo *info, DeviceEvent type) {
if (info == NULL) {
return;
}
ROS_INFO("OnDeviceChange broadcast code %s update type %d", info->broadcast_code, type);
uint8_t handle = info->handle;
if (handle >= kMaxLidarCount) {
return;
}
if (type == kEventConnect) {
QueryDeviceInformation(handle, OnDeviceInformation, NULL);
if (lidars[handle].device_state == kDeviceStateDisconnect) {
lidars[handle].device_state = kDeviceStateConnect;
lidars[handle].info = *info;
}
} else if (type == kEventDisconnect) {
lidars[handle].device_state = kDeviceStateDisconnect;
} else if (type == kEventStateChange) {
lidars[handle].info = *info;
}
if (lidars[handle].device_state == kDeviceStateConnect) {
ROS_INFO("Device State status_code %d", lidars[handle].info.status.status_code);
ROS_INFO("Device State working state %d", lidars[handle].info.state);
ROS_INFO("Device feature %d", lidars[handle].info.feature);
if (lidars[handle].info.state == kLidarStateNormal) {
if (lidars[handle].info.type == kDeviceTypeHub) {
HubStartSampling(OnSampleCallback, NULL);
} else {
LidarStartSampling(handle, OnSampleCallback, NULL);
}
lidars[handle].device_state = kDeviceStateSampling;
}
}
}
void OnDeviceBroadcast(const BroadcastDeviceInfo *info) {
if (info == NULL) {
return;
}
ROS_INFO("Receive Broadcast Code %s, please add it to broacast_code_list if want to connect!\n",\
info->broadcast_code);
if (total_broadcast_code.size() > 0) {
bool found = false;
for (int i = 0; i < total_broadcast_code.size(); ++i) {
if (strncmp(info->broadcast_code, total_broadcast_code[i].c_str(), kBroadcastCodeSize) == 0) {
found = true;
break;
}
}
if (!found) {
ROS_INFO("Not in the broacast_code_list, please add it to if want to connect!");
return;
}
} else {
ROS_INFO("In automatic connection mode, will connect %s", info->broadcast_code);
}
bool result = false;
uint8_t handle = 0;
result = AddLidarToConnect(info->broadcast_code, &handle);
if (result == kStatusSuccess && handle < kMaxLidarCount) {
SetDataCallback(handle, GetLidarData, NULL);
lidars[handle].handle = handle;
lidars[handle].device_state = kDeviceStateDisconnect;
}
}
int main(int argc, char **argv) {
if(ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info)) {
ros::console::notifyLoggerLevelsChanged();
}
ROS_INFO("Livox-SDK ros demo");
if (!Init()) {
ROS_FATAL("Livox-SDK init fail!");
return -1;
}
AddLocalBroadcastCode();
if (argc >= kBdArgcNum) {
ROS_INFO("Commandline input %s", argv[kBdArgvPos]);
AddCommandlineBroadcastCode(argv[kBdArgvPos]);
}
if (total_broadcast_code.size() > 0) {
ROS_INFO("list all valid bd:");
for (int i = 0; i < total_broadcast_code.size(); ++i) {
ROS_INFO("%s", total_broadcast_code[i].c_str());
}
} else {
ROS_INFO("No valid bd input, switch to automatic connection mode!");
}
memset(lidars, 0, sizeof(lidars));
SetBroadcastCallback(OnDeviceBroadcast);
SetDeviceStateUpdateCallback(OnDeviceChange);
if (!Start()) {
Uninit();
return -1;
}
/* ros related */
ros::init(argc, argv, "livox_lidar_publisher");
ros::NodeHandle livox_node;
int msg_type;
livox_node.getParam("livox_msg_type", msg_type);
if (kPointCloud2Msg == msg_type) {
cloud_pub = livox_node.advertise<sensor_msgs::PointCloud2>("livox/lidar", \
kMaxEthPacketQueueSize);
ROS_INFO("Publish PointCloud2");
} else {
cloud_pub = livox_node.advertise<livox_ros_driver::CustomMsg>("livox/lidar", \
kMaxEthPacketQueueSize);
ROS_INFO("Publish Livox Custom Msg");
}
ros::Time::init();
ros::Rate r(1000.0 / kPublishIntervalMs); // 1000.0 / x = hz
while (ros::ok()) {
PollPointcloudData(msg_type);
r.sleep();
}
Uninit();
}

42
livox_ros_driver/livox_ros_driver/include/livox_ros_driver.h Normal file
View File

@@ -0,0 +1,42 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2019 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef LIVOX_ROS_DRIVER_INClUDE_LIVOX_ROS_DRIVER_H_
#define LIVOX_ROS_DRIVER_INClUDE_LIVOX_ROS_DRIVER_H_
#define LIVOX_ROS_DRIVER_VER_MAJOR 2
#define LIVOX_ROS_DRIVER_VER_MINOR 0
#define LIVOX_ROS_DRIVER_VER_PATCH 0
#define GET_STRING(n) GET_STRING_DIRECT(n)
#define GET_STRING_DIRECT(n) #n
#define LIVOX_ROS_DRIVER_VERSION_STRING \
GET_STRING(LIVOX_ROS_DRIVER_VER_MAJOR) "." \
GET_STRING(LIVOX_ROS_DRIVER_VER_MINOR) "." \
GET_STRING(LIVOX_ROS_DRIVER_VER_PATCH)
#endif

569
livox_ros_driver/livox_ros_driver/lddc.cpp Normal file
View File

@@ -0,0 +1,569 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2019 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include "lddc.h"
#include <stdint.h>
#include <inttypes.h>
#include <math.h>
#include "lds_lvx.h"
#include "lds_lidar.h"
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Imu.h>
#include <rosbag/bag.h>
#include <livox_ros_driver/CustomPoint.h>
#include <livox_ros_driver/CustomMsg.h>
#include <pcl_ros/point_cloud.h>
namespace livox_ros {
typedef pcl::PointCloud<pcl::PointXYZI> PointCloud;
/** Lidar Data Distribute Control ----------------------------------------------------------------*/
Lddc::Lddc(int format, int multi_topic, int data_src, int output_type, double frq) : \
transfer_format_(format), use_multi_topic_(multi_topic),\
data_src_(data_src), output_type_(output_type), publish_frq_(frq) {
publish_interval_ms_ = 1000/publish_frq_;
lds_ = nullptr;
memset(private_pub_, 0, sizeof(private_pub_));
memset(private_imu_pub_, 0, sizeof(private_imu_pub_));
global_pub_ = nullptr;
global_imu_pub_ = nullptr;
cur_node_ = nullptr;
bag_ = nullptr;
};
Lddc::~Lddc() {
printf("lddc exit\n\n\n\n");
if (global_pub_) {
delete global_pub_;
}
if (global_imu_pub_) {
delete global_pub_;
}
if (lds_) {
lds_->PrepareExit();
}
for (uint32_t i=0; i<kMaxSourceLidar; i++) {
if (private_pub_[i]) {
delete private_pub_[i];
}
}
for (uint32_t i=0; i<kMaxSourceLidar; i++) {
if (private_imu_pub_[i]) {
delete private_imu_pub_[i];
}
}
}
uint32_t Lddc::PublishPointcloud2(LidarDataQueue* queue, uint32_t packet_num, uint8_t handle) {
uint64_t timestamp = 0;
uint64_t last_timestamp = 0;
uint32_t published_packet = 0;
sensor_msgs::PointCloud2 cloud;
cloud.header.frame_id = "livox_frame";
cloud.height = 1;
cloud.width = 0;
cloud.fields.resize(6);
cloud.fields[0].offset = 0;
cloud.fields[0].name = "x";
cloud.fields[0].count = 1;
cloud.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
cloud.fields[1].offset = 4;
cloud.fields[1].name = "y";
cloud.fields[1].count = 1;
cloud.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
cloud.fields[2].offset = 8;
cloud.fields[2].name = "z";
cloud.fields[2].count = 1;
cloud.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
cloud.fields[3].offset = 12;
cloud.fields[3].name = "intensity";
cloud.fields[3].count = 1;
cloud.fields[3].datatype = sensor_msgs::PointField::FLOAT32;
cloud.fields[4].offset = 16;
cloud.fields[4].name = "tag";
cloud.fields[4].count = 1;
cloud.fields[4].datatype = sensor_msgs::PointField::UINT8;
cloud.fields[5].offset = 17;
cloud.fields[5].name = "line";
cloud.fields[5].count = 1;
cloud.fields[5].datatype = sensor_msgs::PointField::UINT8;
cloud.data.resize(packet_num * kMaxPointPerEthPacket * sizeof(LivoxPointXyzrtl));
cloud.point_step = sizeof(LivoxPointXyzrtl);
uint8_t *point_base = cloud.data.data();
uint8_t data_source = lds_->lidars_[handle].data_src;
StoragePacket storage_packet;
while (published_packet < packet_num) {
QueueProPop(queue, &storage_packet);
LivoxEthPacket* raw_packet = reinterpret_cast<LivoxEthPacket *>(storage_packet.raw_data);
timestamp = GetStoragePacketTimestamp(&storage_packet, data_source);
if (published_packet && \
((timestamp - last_timestamp) > kMaxPacketTimeGap)) {
if (kSourceLvxFile != data_source) {
ROS_INFO("Lidar[%d] packet loss", handle);
break;
}
}
if (!published_packet) {
cloud.header.stamp = ros::Time(timestamp/1000000000.0); // to ros time stamp
}
cloud.width += storage_packet.point_num;
if (kSourceLvxFile != data_source) {
PointConvertHandler pf_point_convert = GetConvertHandler(raw_packet->data_type);
if (pf_point_convert) {
point_base = pf_point_convert(point_base, raw_packet, \
lds_->lidars_[handle].extrinsic_parameter);
} else {
/* Skip the packet */
ROS_INFO("Lidar[%d] unkown packet type[%d]", handle, raw_packet->data_type);
break;
}
} else {
point_base = LivoxPointToPxyzrtl(point_base, raw_packet, \
lds_->lidars_[handle].extrinsic_parameter);
}
QueuePopUpdate(queue);
last_timestamp = timestamp;
++published_packet;
}
cloud.row_step = cloud.width * cloud.point_step;
cloud.is_bigendian = false;
cloud.is_dense = true;
cloud.data.resize(cloud.row_step); // adjust to the real size
ros::Publisher* p_publisher = Lddc::GetCurrentPublisher(handle);
if (kOutputToRos == output_type_) {
p_publisher->publish(cloud);
} else {
if (bag_) {
bag_->write(p_publisher->getTopic(), ros::Time(timestamp/1000000000.0), cloud);
}
}
return published_packet;
}
/* for pcl::pxyzi */
uint32_t Lddc::PublishPointcloudData(LidarDataQueue* queue, uint32_t packet_num, uint8_t handle) {
uint64_t timestamp = 0;
uint64_t last_timestamp = 0;
uint32_t published_packet = 0;
/* init point cloud data struct */
PointCloud::Ptr cloud (new PointCloud);
cloud->header.frame_id = "livox_frame";
//cloud->header.stamp = ros::Time::now();
cloud->height = 1;
cloud->width = 0;
uint8_t data_source = lds_->lidars_[handle].data_src;
StoragePacket storage_packet;
while (published_packet < packet_num) {
QueueProPop(queue, &storage_packet);
LivoxEthPacket* raw_packet = reinterpret_cast<LivoxEthPacket *>(storage_packet.raw_data);
timestamp = GetStoragePacketTimestamp(&storage_packet, data_source);
if (published_packet && \
((timestamp - last_timestamp) > kMaxPacketTimeGap)) {
ROS_INFO("Lidar[%d] packet loss", handle);
break;
}
if (!published_packet) {
cloud->header.stamp = timestamp/1000.0; // to pcl ros time stamp
}
cloud->width += storage_packet.point_num;
uint8_t point_buf[2048];
if (kSourceLvxFile != data_source) {
PointConvertHandler pf_point_convert = GetConvertHandler(raw_packet->data_type);
if (pf_point_convert) {
pf_point_convert(point_buf, raw_packet, \
lds_->lidars_[handle].extrinsic_parameter);
} else {
/* Skip the packet */
ROS_INFO("Lidar[%d] unkown packet type[%d]", handle, raw_packet->data_type);
break;
}
} else {
LivoxPointToPxyzrtl(point_buf, raw_packet, \
lds_->lidars_[handle].extrinsic_parameter);
}
LivoxPointXyzrtl* dst_point = (LivoxPointXyzrtl*)point_buf;
for (uint32_t i = 0; i < storage_packet.point_num; i++) {
pcl::PointXYZI point;
point.x = dst_point->x;
point.y = dst_point->y;
point.z = dst_point->z;
point.intensity = dst_point->reflectivity;
++dst_point;
cloud->points.push_back(point);
}
QueuePopUpdate(queue);
last_timestamp = timestamp;
++published_packet;
}
ros::Publisher* p_publisher = Lddc::GetCurrentPublisher(handle);
if (kOutputToRos == output_type_) {
p_publisher->publish(cloud);
} else {
if (bag_) {
bag_->write(p_publisher->getTopic(), ros::Time(timestamp/1000000000.0), cloud);
}
}
return published_packet;
}
uint32_t Lddc::PublishCustomPointcloud(LidarDataQueue* queue, uint32_t packet_num, \
uint8_t handle) {
static uint32_t msg_seq = 0;
uint64_t timestamp = 0;
uint64_t last_timestamp = 0;
uint32_t published_packet = 0;
uint32_t packet_offset_time = 0; // ns
livox_ros_driver::CustomMsg livox_msg;
livox_msg.header.frame_id = "livox_frame";
livox_msg.header.seq = msg_seq;
++msg_seq;
//livox_msg.header.stamp = ros::Time::now();
livox_msg.timebase = 0;
livox_msg.point_num = 0;
livox_msg.lidar_id = handle;
uint8_t data_source = lds_->lidars_[handle].data_src;
StoragePacket storage_packet;
while (published_packet < packet_num) {
QueueProPop(queue, &storage_packet);
LivoxEthPacket* raw_packet = reinterpret_cast<LivoxEthPacket *>(storage_packet.raw_data);
uint32_t point_interval = GetPointInterval(raw_packet->data_type);
uint32_t dual_point = 0;
if ((raw_packet->data_type == kDualExtendCartesian) || \
(raw_packet->data_type == kDualExtendSpherical)) {
dual_point = 1;
}
timestamp = GetStoragePacketTimestamp(&storage_packet, data_source);
if (published_packet && \
((timestamp - last_timestamp) > kDeviceDisconnectThreshold)) {
ROS_INFO("Lidar[%d] packet loss", handle);
break;
}
if (!published_packet) {
livox_msg.timebase = timestamp; // to us
packet_offset_time = 0; // first packet
livox_msg.header.stamp = ros::Time(timestamp/1000000000.0); // to ros time stamp
//ROS_DEBUG("[%d]:%ld %d", handle, livox_msg.timebase, point_interval);
} else {
packet_offset_time = (uint32_t)(timestamp - livox_msg.timebase);
}
livox_msg.point_num += storage_packet.point_num;
uint8_t point_buf[2048];
if (kSourceLvxFile != data_source) {
PointConvertHandler pf_point_convert = GetConvertHandler(raw_packet->data_type);
if (pf_point_convert) {
pf_point_convert(point_buf, raw_packet, \
lds_->lidars_[handle].extrinsic_parameter);
} else {
/* Skip the packet */
ROS_INFO("Lidar[%d] unkown packet type[%d]", handle, raw_packet->data_type);
break;
}
} else {
LivoxPointToPxyzrtl(point_buf, raw_packet, \
lds_->lidars_[handle].extrinsic_parameter);
}
LivoxPointXyzrtl* dst_point = (LivoxPointXyzrtl*)point_buf;
for (uint32_t i = 0; i < storage_packet.point_num; i++) {
livox_ros_driver::CustomPoint point;
if (!dual_point) { /** dual return mode */
point.offset_time = packet_offset_time + i*point_interval;
} else {
point.offset_time = packet_offset_time + (i/2)*point_interval;
}
point.x = dst_point->x;
point.y = dst_point->y;
point.z = dst_point->z;
point.reflectivity = dst_point->reflectivity;
point.tag = dst_point->tag;
point.line = dst_point->line;
++dst_point;
livox_msg.points.push_back(point);
}
QueuePopUpdate(queue);
last_timestamp = timestamp;
++published_packet;
}
ros::Publisher* p_publisher = Lddc::GetCurrentPublisher(handle);
if (kOutputToRos == output_type_) {
p_publisher->publish(livox_msg);
} else {
if (bag_) {
bag_->write(p_publisher->getTopic(), ros::Time(timestamp/1000000000.0), livox_msg);
}
}
return published_packet;
}
uint32_t Lddc::PublishImuData(LidarDataQueue* queue, uint32_t packet_num,
uint8_t handle) {
uint64_t timestamp = 0;
uint32_t published_packet = 0;
sensor_msgs::Imu imu_data;
imu_data.header.frame_id = "livox_frame";
uint8_t data_source = lds_->lidars_[handle].data_src;
StoragePacket storage_packet;
QueueProPop(queue, &storage_packet);
LivoxEthPacket* raw_packet = reinterpret_cast<LivoxEthPacket *>(storage_packet.raw_data);
timestamp = GetStoragePacketTimestamp(&storage_packet, data_source);
imu_data.header.stamp = ros::Time(timestamp/1000000000.0); // to ros time stamp
uint8_t point_buf[2048];
LivoxImuDataProcess(point_buf, raw_packet);
LivoxImuPoint* imu = (LivoxImuPoint*)point_buf;
imu_data.angular_velocity.x = imu->gyro_x;
imu_data.angular_velocity.y = imu->gyro_y;
imu_data.angular_velocity.z = imu->gyro_z;
imu_data.linear_acceleration.x = imu->acc_x;
imu_data.linear_acceleration.y = imu->acc_y;
imu_data.linear_acceleration.z = imu->acc_z;
QueuePopUpdate(queue);
++published_packet;
ros::Publisher* p_publisher = Lddc::GetCurrentImuPublisher(handle);
if (kOutputToRos == output_type_) {
p_publisher->publish(imu_data);
} else {
if (bag_) {
bag_->write(p_publisher->getTopic(), ros::Time(timestamp/1000000000.0), imu_data);
}
}
return published_packet;
}
int Lddc::RegisterLds(Lds* lds) {
if (lds_ == nullptr) {
lds_ = lds;
return 0;
} else {
return -1;
}
}
void Lddc::PollingLidarPointCloudData(uint8_t handle, LidarDevice* lidar) {
LidarDataQueue* p_queue = &lidar->data;
if (p_queue == nullptr) {
return;
}
while (!QueueIsEmpty(p_queue)) {
uint32_t used_size = QueueUsedSize(p_queue);
uint32_t publish_packet_upper_limit = GetPacketNumPerSec(lidar->info.type);
uint32_t publish_packet_lower_limit = publish_packet_upper_limit / 2 / ((uint32_t)publish_frq_);
/** increase margin */
publish_packet_upper_limit = publish_packet_upper_limit + publish_packet_upper_limit / 10;
if (used_size < publish_packet_lower_limit) {
break;
}
if (used_size > publish_packet_upper_limit) {
used_size = publish_packet_upper_limit;
}
if (kPointCloud2Msg == transfer_format_) {
if (used_size == PublishPointcloud2(p_queue, used_size, handle)) {
}
} else if (kLivoxCustomMsg == transfer_format_) {
if (used_size == PublishCustomPointcloud(p_queue, used_size, handle)) {
}
} else if (kPclPxyziMsg == transfer_format_) {
if (used_size == PublishPointcloudData(p_queue, used_size, handle)) {
}
}
}
}
void Lddc::PollingLidarImuData(uint8_t handle, LidarDevice* lidar) {
LidarDataQueue* p_queue = &lidar->imu_data;
if (p_queue == nullptr) {
return;
}
while (!QueueIsEmpty(p_queue)) {
PublishImuData(p_queue, 1, handle);
}
}
void Lddc::DistributeLidarData(void) {
if (lds_ == nullptr) {
return;
}
for (uint32_t i = 0; i < lds_->lidar_count_; i++) {
uint32_t lidar_id = i;
LidarDevice* lidar = &lds_->lidars_[lidar_id];
LidarDataQueue* p_queue = &lidar->data;
if ((kConnectStateSampling!= lidar->connect_state) || (p_queue == nullptr)) {
continue;
}
PollingLidarPointCloudData(lidar_id, lidar);
PollingLidarImuData(lidar_id, lidar);
}
if (lds_->IsRequestExit()) {
PrepareExit();
}
}
ros::Publisher* Lddc::GetCurrentPublisher(uint8_t handle) {
ros::Publisher** pub = nullptr;
uint32_t queue_size = kMinEthPacketQueueSize * 4;
if (use_multi_topic_) {
pub = &private_pub_[handle];
} else {
pub = &global_pub_;
queue_size = queue_size * 32;
}
if (*pub == nullptr) {
char name_str[48];
memset(name_str, 0, sizeof(name_str));
if (use_multi_topic_) {
snprintf(name_str, sizeof(name_str), "livox/lidar_%s", \
lds_->lidars_[handle].info.broadcast_code);
ROS_INFO("Support multi topics.");
} else {
ROS_INFO("Support only one topic.");
snprintf(name_str, sizeof(name_str), "livox/lidar");
}
*pub = new ros::Publisher;
if (kPointCloud2Msg == transfer_format_) {
**pub = cur_node_->advertise<sensor_msgs::PointCloud2>(name_str,\
queue_size);
ROS_INFO("%s publish use PointCloud2 format, publisher queue size [%d]", \
name_str, queue_size);
} else if (kLivoxCustomMsg == transfer_format_) {
**pub = cur_node_->advertise<livox_ros_driver::CustomMsg>(name_str,\
queue_size);
ROS_INFO("%s publish use livox custom format, publisher queue size [%d]", \
name_str, queue_size);
} else if (kPclPxyziMsg == transfer_format_) {
**pub = cur_node_->advertise<PointCloud>(name_str,\
queue_size);
ROS_INFO("%s publish use pcl PointXYZI format, publisher queue size [%d]", \
name_str, queue_size);
}
}
return *pub;
}
ros::Publisher* Lddc::GetCurrentImuPublisher(uint8_t handle) {
ros::Publisher** pub = nullptr;
uint32_t queue_size = kMinEthPacketQueueSize * 4;
if (use_multi_topic_) {
pub = &private_imu_pub_[handle];
} else {
pub = &global_imu_pub_;
queue_size = queue_size * 32;
}
if (*pub == nullptr) {
char name_str[48];
memset(name_str, 0, sizeof(name_str));
if (use_multi_topic_) {
ROS_INFO("Support multi topics.");
snprintf(name_str, sizeof(name_str), "livox/imu_%s", \
lds_->lidars_[handle].info.broadcast_code);
} else {
ROS_INFO("Support only one topic.");
snprintf(name_str, sizeof(name_str), "livox/imu");
}
*pub = new ros::Publisher;
**pub = cur_node_->advertise<sensor_msgs::Imu>(name_str, queue_size);
ROS_INFO("%s publish imu data, Publisher QueueSize[%d]", name_str, queue_size);
}
return *pub;
}
void Lddc::CreateBagFile(const std::string& file_name) {
if (!bag_) {
bag_ = new rosbag::Bag;
bag_->open(file_name, rosbag::bagmode::Write);
ROS_INFO("Create bag file :%s!", file_name.c_str());
}
}
void Lddc::PrepareExit(void) {
if (bag_) {
bag_->close();
ROS_INFO("Press [Ctrl+C] to exit!\n");
bag_ = nullptr;
}
if (lds_) {
lds_->PrepareExit();
lds_ = nullptr;
}
}
}

92
livox_ros_driver/livox_ros_driver/lddc.h Normal file
View File

@@ -0,0 +1,92 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2019 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef LIVOX_ROS_DRIVER_LDDC_H_
#define LIVOX_ROS_DRIVER_LDDC_H_
#include "livox_sdk.h"
#include "lds.h"
#include <ros/ros.h>
#include <rosbag/bag.h>
namespace livox_ros {
/** Lidar data distribute control */
typedef enum {
kPointCloud2Msg = 0,
kLivoxCustomMsg = 1,
kPclPxyziMsg
} TransferType;
class Lddc {
public:
Lddc(int format, int multi_topic, int data_src, int output_type, double frq);
~Lddc();
int RegisterLds(Lds* lds);
void DistributeLidarData(void);
void CreateBagFile(const std::string& file_name);
void PrepareExit(void);
uint8_t GetTransferFormat(void) { return transfer_format_; }
uint8_t IsMultiTopic(void) { return use_multi_topic_; }
void SetRosNode(ros::NodeHandle* node) { cur_node_ = node; }
void SetRosPub(ros::Publisher* pub) { global_pub_ = pub; };
void SetPublishFrq(uint32_t frq) { publish_frq_ = frq; }
Lds* lds_;
private:
uint32_t PublishPointcloud2(LidarDataQueue* queue, uint32_t packet_num, \
uint8_t handle);
uint32_t PublishPointcloudData(LidarDataQueue* queue, uint32_t packet_num, \
uint8_t handle);
uint32_t PublishCustomPointcloud(LidarDataQueue* queue, uint32_t packet_num,\
uint8_t handle);
uint32_t PublishImuData(LidarDataQueue* queue, uint32_t packet_num,\
uint8_t handle);
ros::Publisher* GetCurrentPublisher(uint8_t handle);
ros::Publisher* GetCurrentImuPublisher(uint8_t handle);
void PollingLidarPointCloudData(uint8_t handle, LidarDevice* lidar);
void PollingLidarImuData(uint8_t handle, LidarDevice* lidar);
uint8_t transfer_format_;
uint8_t use_multi_topic_;
uint8_t data_src_;
uint8_t output_type_;
double publish_frq_;
int32_t publish_interval_ms_;
ros::Publisher* private_pub_[kMaxSourceLidar];
ros::Publisher* global_pub_;
ros::Publisher* private_imu_pub_[kMaxSourceLidar];
ros::Publisher* global_imu_pub_;
ros::NodeHandle* cur_node_;
rosbag::Bag* bag_;
};
}
#endif

137
livox_ros_driver/livox_ros_driver/ldq.cpp Normal file
View File

@@ -0,0 +1,137 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2019 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include "ldq.h"
#include <string.h>
#include <stdio.h>
namespace livox_ros {
/* for pointcloud queue process */
int InitQueue(LidarDataQueue* queue, uint32_t queue_size) {
if (queue == nullptr) {
return 1;
}
if(IsPowerOf2(queue_size) != true) {
queue_size = RoundupPowerOf2(queue_size);
}
queue->storage_packet = new StoragePacket[queue_size];
if(queue->storage_packet == nullptr) {
return 1;
}
queue->rd_idx = 0;
queue->wr_idx = 0;
queue->size = queue_size;
queue->mask = queue_size - 1;
return 0;
}
int DeInitQueue(LidarDataQueue* queue) {
if (queue == nullptr) {
return 1;
}
if (queue->storage_packet) {
delete [] queue->storage_packet;
}
queue->rd_idx = 0;
queue->wr_idx = 0;
queue->size = 0;
queue->mask = 0;
return 0;
}
void ResetQueue(LidarDataQueue* queue) {
queue->rd_idx = 0;
queue->wr_idx = 0;
}
void QueueProPop(LidarDataQueue* queue, StoragePacket* storage_packet) {
uint32_t rd_idx = queue->rd_idx & queue->mask;
memcpy(storage_packet, &(queue->storage_packet[rd_idx]), sizeof(StoragePacket));
}
void QueuePopUpdate(LidarDataQueue* queue) {
queue->rd_idx++;
}
uint32_t QueuePop(LidarDataQueue* queue, StoragePacket* storage_packet) {
QueueProPop(queue, storage_packet);
QueuePopUpdate(queue);
return 1;
}
uint32_t QueueUsedSize(LidarDataQueue* queue) {
return queue->wr_idx - queue->rd_idx;
}
uint32_t QueueUnusedSize(LidarDataQueue* queue) {
return (queue->size - (queue->wr_idx - queue->rd_idx));
}
uint32_t QueueIsFull(LidarDataQueue* queue) {
return ((queue->wr_idx - queue->rd_idx) > queue->mask);
}
uint32_t QueueIsEmpty(LidarDataQueue* queue) {
return (queue->rd_idx == queue->wr_idx);
}
uint32_t QueuePush(LidarDataQueue* queue, StoragePacket* storage_packet) {
uint32_t wr_idx = queue->wr_idx & queue->mask;
memcpy((void *)(&(queue->storage_packet[wr_idx])), \
(void *)(storage_packet), \
sizeof(StoragePacket));
queue->wr_idx++;
return 1;
}
uint32_t QueuePushAny(LidarDataQueue* queue, uint8_t *data, uint32_t length, \
uint64_t time_rcv, uint32_t point_num) {
uint32_t wr_idx = queue->wr_idx & queue->mask;
queue->storage_packet[wr_idx].time_rcv = time_rcv;
queue->storage_packet[wr_idx].point_num = point_num;
memcpy(queue->storage_packet[wr_idx].raw_data, data, length);
queue->wr_idx++;
return 1;
}
}

87
livox_ros_driver/livox_ros_driver/ldq.h Normal file
View File

@@ -0,0 +1,87 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2019 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
// livox lidar data queue
#ifndef LIVOX_ROS_DRIVER_LDQ_H_
#define LIVOX_ROS_DRIVER_LDQ_H_
#include <stdint.h>
namespace livox_ros {
const uint32_t KEthPacketMaxLength = 1500;
#pragma pack(1)
typedef struct {
uint64_t time_rcv; /**< receive time when data arrive */
uint32_t point_num;
uint8_t raw_data[KEthPacketMaxLength];
} StoragePacket;
#pragma pack()
typedef struct {
StoragePacket *storage_packet;
volatile uint32_t rd_idx;
volatile uint32_t wr_idx;
uint32_t mask;
uint32_t size; /**< must be power of 2. */
} LidarDataQueue;
inline static bool IsPowerOf2(uint32_t size) {
return (size != 0) && ((size & (size - 1)) == 0);
}
inline static uint32_t RoundupPowerOf2(uint32_t size) {
uint32_t power2_val = 0;
for(int i = 0; i < 32; i++) {
power2_val = ((uint32_t)1) << i;
if(size <= power2_val) {
break;
}
}
return power2_val;
}
/** queue operate function */
int InitQueue(LidarDataQueue* queue, uint32_t queue_size);
int DeInitQueue(LidarDataQueue* queue);
void ResetQueue(LidarDataQueue* queue);
void QueueProPop(LidarDataQueue* queue, StoragePacket* storage_packet);
void QueuePopUpdate(LidarDataQueue* queue);
uint32_t QueuePop(LidarDataQueue* queue, StoragePacket* storage_packet);
uint32_t QueueUsedSize(LidarDataQueue* queue);
uint32_t QueueUnusedSize(LidarDataQueue* queue);
uint32_t QueueIsFull(LidarDataQueue* queue);
uint32_t QueueIsEmpty(LidarDataQueue* queue);
uint32_t QueuePush(LidarDataQueue* queue, StoragePacket* storage_packet);
uint32_t QueuePushAny(LidarDataQueue* queue, uint8_t *data, uint32_t length, \
uint64_t time_rcv, uint32_t point_num);
}
#endif

440
livox_ros_driver/livox_ros_driver/lds.cpp Normal file
View File

@@ -0,0 +1,440 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2019 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include "lds.h"
#include <math.h>
#include <stdio.h>
#include <string.h>
#include <time.h>
#include <chrono>
namespace livox_ros {
/** Common function ------ ----------------------------------------------------------------------- */
bool IsFilePathValid(const char* path_str) {
int str_len = strlen(path_str);
if ((str_len > kPathStrMinSize) && (str_len < kPathStrMaxSize)) {
return true;
} else {
return false;
}
}
uint64_t GetStoragePacketTimestamp(StoragePacket* packet, uint8_t data_src_) {
LivoxEthPacket* raw_packet = reinterpret_cast<LivoxEthPacket *>(packet->raw_data);
LdsStamp timestamp;
memcpy(timestamp.stamp_bytes, raw_packet->timestamp, sizeof(timestamp));
if (raw_packet->timestamp_type == kTimestampTypePps) {
if (data_src_ != kSourceLvxFile) {
return (timestamp.stamp + packet->time_rcv);
} else {
return timestamp.stamp;
}
} else if (raw_packet->timestamp_type == kTimestampTypeNoSync) {
return timestamp.stamp;
} else if (raw_packet->timestamp_type == kTimestampTypePtp) {
return timestamp.stamp;
} else if (raw_packet->timestamp_type == kTimestampTypePpsGps) {
struct tm time_utc;
time_utc.tm_isdst = 0;
time_utc.tm_year = raw_packet->timestamp[0] + 100; // map 2000 to 1990
time_utc.tm_mon = raw_packet->timestamp[1];
time_utc.tm_mday = raw_packet->timestamp[2];
time_utc.tm_hour = raw_packet->timestamp[3];
time_utc.tm_min = 0;
time_utc.tm_sec = 0;
uint64_t time_epoch = mktime(&time_utc);
time_epoch = time_epoch * 1000000 + timestamp.stamp_word.high; // to us
time_epoch = time_epoch * 1000; // to ns
return time_epoch;
} else {
printf("Timestamp type[%d] invalid.\n", raw_packet->timestamp_type);
return 0;
}
}
uint32_t CalculatePacketQueueSize(uint32_t interval_ms, uint32_t data_type) {
uint32_t queue_size = (interval_ms * GetPacketNumPerSec(data_type)) / 1000;
queue_size = queue_size;
if (queue_size < kMinEthPacketQueueSize) {
queue_size = kMinEthPacketQueueSize;
} else if (queue_size > kMaxEthPacketQueueSize) {
queue_size = kMaxEthPacketQueueSize;
}
return queue_size;
}
void ParseCommandlineInputBdCode(const char* cammandline_str,
std::vector<std::string>& bd_code_list) {
char* strs = new char[strlen(cammandline_str) + 1];
strcpy(strs, cammandline_str);
std::string pattern = "&";
char* bd_str = strtok(strs, pattern.c_str());
std::string invalid_bd = "000000000";
while (bd_str != NULL) {
printf("Commandline input bd:%s\n", bd_str);
if ((kBdCodeSize == strlen(bd_str)) && \
(NULL == strstr(bd_str, invalid_bd.c_str()))) {
bd_code_list.push_back(bd_str);
} else {
printf("Invalid bd:%s!\n", bd_str);
}
bd_str = strtok(NULL, pattern.c_str());
}
delete [] strs;
}
void EulerAnglesToRotationMatrix(EulerAngle euler, RotationMatrix matrix) {
double cos_roll = cos(static_cast<double>(euler[0]));
double cos_pitch = cos(static_cast<double>(euler[1]));
double cos_yaw = cos(static_cast<double>(euler[2]));
double sin_roll = sin(static_cast<double>(euler[0]));
double sin_pitch = sin(static_cast<double>(euler[1]));
double sin_yaw = sin(static_cast<double>(euler[2]));
matrix[0][0] = cos_pitch * cos_yaw;
matrix[0][1] = sin_roll * sin_pitch * cos_yaw - cos_roll * sin_yaw;
matrix[0][2] = cos_roll * sin_pitch * cos_yaw + sin_roll * sin_yaw;
matrix[1][0] = cos_pitch * sin_yaw;
matrix[1][1] = sin_roll * sin_pitch * sin_yaw + cos_roll * cos_yaw;
matrix[1][2] = cos_roll * sin_pitch * sin_yaw - sin_roll * cos_yaw;
matrix[2][0] = -sin_pitch;
matrix[2][1] = sin_roll * cos_pitch;
matrix[2][2] = cos_roll * cos_pitch;
/*
float rotate[3][3] = {
{
std::cos(info.pitch) * std::cos(info.yaw),
std::sin(info.roll) * std::sin(info.pitch) * std::cos(info.yaw) - std::cos(info.roll) * std::sin(info.yaw),
std::cos(info.roll) * std::sin(info.pitch) * std::cos(info.yaw) + std::sin(info.roll) * std::sin(info.yaw) },
{
std::cos(info.pitch) * std::sin(info.yaw),
std::sin(info.roll) * std::sin(info.pitch) * std::sin(info.yaw) + std::cos(info.roll) * std::cos(info.yaw),
std::cos(info.roll) * std::sin(info.pitch) * std::sin(info.yaw) - std::sin(info.roll) * std::cos(info.yaw) },
{
-std::sin(info.pitch),
std::sin(info.roll) * std::cos(info.pitch),
std::cos(info.roll) * std::cos(info.pitch)
}
};
*/
}
void PointExtrisincCompensation(PointXyz* dst_point, const PointXyz& src_point, \
ExtrinsicParameter& extrinsic) {
dst_point->x = src_point.x * extrinsic.rotation[0][0] + \
src_point.y * extrinsic.rotation[0][1] + \
src_point.z * extrinsic.rotation[0][2] + \
extrinsic.trans[0];
dst_point->y = src_point.x * extrinsic.rotation[1][0] + \
src_point.y * extrinsic.rotation[1][1] + \
src_point.z * extrinsic.rotation[1][2] + \
extrinsic.trans[1];
dst_point->z = src_point.x * extrinsic.rotation[2][0] + \
src_point.y * extrinsic.rotation[2][1] + \
src_point.z * extrinsic.rotation[2][2] +
extrinsic.trans[2];
}
/** Livox point procees for different raw data format --------------------------------------------*/
uint8_t* LivoxPointToPxyzrtl(uint8_t* point_buf, LivoxEthPacket* eth_packet, \
ExtrinsicParameter& extrinsic) {
LivoxPointXyzrtl* dst_point = (LivoxPointXyzrtl *)point_buf;
uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type);
LivoxPoint* raw_point = reinterpret_cast<LivoxPoint *>(eth_packet->data);
while(points_per_packet) {
RawPointConvert((LivoxPointXyzr*)dst_point, raw_point);
if (extrinsic.enable) {
PointXyz src_point = *((PointXyz*)dst_point);
PointExtrisincCompensation((PointXyz*)dst_point, src_point, extrinsic);
}
dst_point->tag = 0;
dst_point->line = 0;
++raw_point;
++dst_point;
--points_per_packet;
}
return (uint8_t *)dst_point;
}
uint8_t* LivoxRawPointToPxyzrtl(uint8_t* point_buf, LivoxEthPacket* eth_packet, \
ExtrinsicParameter& extrinsic) {
LivoxPointXyzrtl* dst_point = (LivoxPointXyzrtl *)point_buf;
uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type);
LivoxRawPoint* raw_point = reinterpret_cast<LivoxRawPoint *>(eth_packet->data);
while(points_per_packet) {
RawPointConvert((LivoxPointXyzr*)dst_point, raw_point);
if (extrinsic.enable) {
PointXyz src_point = *((PointXyz*)dst_point);
PointExtrisincCompensation((PointXyz*)dst_point, src_point, extrinsic);
}
dst_point->tag = 0;
dst_point->line = 0;
++raw_point;
++dst_point;
--points_per_packet;
}
return (uint8_t *)dst_point;
}
uint8_t* LivoxSpherPointToPxyzrtl(uint8_t* point_buf, LivoxEthPacket* eth_packet, \
ExtrinsicParameter& extrinsic) {
LivoxPointXyzrtl* dst_point = (LivoxPointXyzrtl *)point_buf;
uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type);
LivoxSpherPoint* raw_point = reinterpret_cast<LivoxSpherPoint *>(eth_packet->data);
while(points_per_packet) {
RawPointConvert((LivoxPointXyzr*)dst_point, raw_point);
if (extrinsic.enable) {
PointXyz src_point = *((PointXyz*)dst_point);
PointExtrisincCompensation((PointXyz*)dst_point, src_point, extrinsic);
}
dst_point->tag = 0;
dst_point->line = 0;
++raw_point;
++dst_point;
--points_per_packet;
}
return (uint8_t *)dst_point;
}
uint8_t* LivoxExtendRawPointToPxyzrtl(uint8_t* point_buf, LivoxEthPacket* eth_packet, \
ExtrinsicParameter& extrinsic) {
LivoxPointXyzrtl* dst_point = (LivoxPointXyzrtl *)point_buf;
uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type);
LivoxExtendRawPoint* raw_point = \
reinterpret_cast<LivoxExtendRawPoint *>(eth_packet->data);
uint8_t line_id = 0;
while(points_per_packet) {
RawPointConvert((LivoxPointXyzr*)dst_point, (LivoxRawPoint *)raw_point);
if (extrinsic.enable) {
PointXyz src_point = *((PointXyz*)dst_point);
PointExtrisincCompensation((PointXyz*)dst_point, src_point, extrinsic);
}
dst_point->tag = raw_point->tag;
dst_point->line = line_id;
dst_point->line = dst_point->line % 6;
++raw_point;
++dst_point;
++line_id;
--points_per_packet;
}
return (uint8_t *)dst_point;
}
uint8_t* LivoxExtendSpherPointToPxyzrtl(uint8_t* point_buf, LivoxEthPacket* eth_packet, \
ExtrinsicParameter& extrinsic) {
LivoxPointXyzrtl* dst_point = (LivoxPointXyzrtl *)point_buf;
uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type);
LivoxExtendSpherPoint* raw_point = reinterpret_cast<LivoxExtendSpherPoint *>(eth_packet->data);
uint8_t line_id = 0;
while(points_per_packet) {
RawPointConvert((LivoxPointXyzr*)dst_point, (LivoxSpherPoint *)raw_point);
if (extrinsic.enable) {
PointXyz src_point = *((PointXyz*)dst_point);
PointExtrisincCompensation((PointXyz*)dst_point, src_point, extrinsic);
}
dst_point->tag = raw_point->tag;
dst_point->line = line_id;
dst_point->line = dst_point->line % 6;
++raw_point;
++dst_point;
++line_id;
--points_per_packet;
}
return (uint8_t *)dst_point;
}
uint8_t* LivoxDualExtendRawPointToPxyzrtl(uint8_t* point_buf, LivoxEthPacket* eth_packet, \
ExtrinsicParameter& extrinsic) {
LivoxPointXyzrtl* dst_point = (LivoxPointXyzrtl *)point_buf;
uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type);
LivoxExtendRawPoint* raw_point = \
reinterpret_cast<LivoxExtendRawPoint *>(eth_packet->data);
uint8_t line_id = 0;
while(points_per_packet) {
RawPointConvert((LivoxPointXyzr*)dst_point, (LivoxRawPoint *)raw_point);
if (extrinsic.enable) {
PointXyz src_point = *((PointXyz*)dst_point);
PointExtrisincCompensation((PointXyz*)dst_point, src_point, extrinsic);
}
dst_point->tag = raw_point->tag;
dst_point->line = line_id / 2; /* LivoxDualExtendRawPoint = 2*LivoxExtendRawPoint */
dst_point->line = dst_point->line % 6;
++raw_point;
++dst_point;
++line_id;
--points_per_packet;
}
return (uint8_t *)dst_point;
}
uint8_t* LivoxDualExtendSpherPointToPxyzrtl(uint8_t* point_buf, LivoxEthPacket* eth_packet, \
ExtrinsicParameter& extrinsic) {
LivoxPointXyzrtl* dst_point = (LivoxPointXyzrtl *)point_buf;
uint32_t points_per_packet = GetPointsPerPacket(eth_packet->data_type);
LivoxDualExtendSpherPoint* raw_point = \
reinterpret_cast<LivoxDualExtendSpherPoint *>(eth_packet->data);
uint8_t line_id = 0;
while(points_per_packet) {
RawPointConvert((LivoxPointXyzr*)dst_point, (LivoxPointXyzr *)(dst_point + 1), \
(LivoxDualExtendSpherPoint *)raw_point);
if (extrinsic.enable) {
PointXyz src_point = *((PointXyz*)dst_point);
PointExtrisincCompensation((PointXyz*)dst_point, src_point, extrinsic);
}
dst_point->tag = raw_point->tag1;
dst_point->line = line_id;
dst_point->line = dst_point->line % 6;
++dst_point;
if (extrinsic.enable) {
PointXyz src_point = *((PointXyz*)dst_point);
PointExtrisincCompensation((PointXyz*)dst_point, src_point, extrinsic);
}
dst_point->tag = raw_point->tag2;
dst_point->line = line_id;
dst_point->line = dst_point->line % 6;
++dst_point;
++raw_point; /* only increase one */
++line_id;
--points_per_packet;
}
return (uint8_t *)dst_point;
}
uint8_t* LivoxImuDataProcess(uint8_t* point_buf, LivoxEthPacket* eth_packet) {
memcpy(point_buf, eth_packet->data, sizeof(LivoxImuPoint));
return point_buf;
}
const PointConvertHandler to_pxyzi_handler_table[kMaxPointDataType] = {
LivoxRawPointToPxyzrtl,
LivoxSpherPointToPxyzrtl,
LivoxExtendRawPointToPxyzrtl,
LivoxExtendSpherPointToPxyzrtl,
LivoxDualExtendRawPointToPxyzrtl,
LivoxDualExtendSpherPointToPxyzrtl,
nullptr
};
PointConvertHandler GetConvertHandler(uint8_t data_type) {
if (data_type < kMaxPointDataType)
return to_pxyzi_handler_table[data_type];
else
return nullptr;
}
#if 0
static void PointCloudConvert(LivoxPoint *p_dpoint, LivoxRawPoint *p_raw_point) {
p_dpoint->x = p_raw_point->x/1000.0f;
p_dpoint->y = p_raw_point->y/1000.0f;
p_dpoint->z = p_raw_point->z/1000.0f;
p_dpoint->reflectivity = p_raw_point->reflectivity;
}
#endif
/* Member function ------ ----------------------------------------------------------------------- */
Lds::Lds(uint32_t buffer_time_ms, uint8_t data_src)
: buffer_time_ms_(buffer_time_ms), data_src_(data_src) {
lidar_count_ = kMaxSourceLidar;
request_exit_ = false;
ResetLds(data_src_);
};
Lds::~Lds() {
lidar_count_ = 0;
ResetLds(0);
};
void Lds::ResetLidar(LidarDevice* lidar, uint8_t data_src) {
DeInitQueue(&lidar->data);
DeInitQueue(&lidar->imu_data);
memset(lidar, 0, sizeof(LidarDevice));
/** unallocated state */
lidar->handle = kMaxSourceLidar;
lidar->connect_state = kConnectStateOff;
lidar->data_src = data_src;
}
void Lds::SetLidarDataSrc(LidarDevice* lidar, uint8_t data_src) {
lidar->data_src = data_src;
}
void Lds::ResetLds(uint8_t data_src) {
lidar_count_ = kMaxSourceLidar;
for (uint32_t i=0; i<kMaxSourceLidar; i++) {
ResetLidar(&lidars_[i], data_src);
}
}
uint8_t Lds::GetDeviceType(uint8_t handle) {
if (handle < kMaxSourceLidar) {
return lidars_[handle].info.type;
} else {
return kDeviceTypeHub;
}
}
void Lds::PrepareExit(void) {
}
}

329
livox_ros_driver/livox_ros_driver/lds.h Normal file
View File

@@ -0,0 +1,329 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2019 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
// livox lidar data source
#ifndef LIVOX_ROS_DRIVER_LDS_H_
#define LIVOX_ROS_DRIVER_LDS_H_
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <vector>
#include <string>
#include "ldq.h"
#include "livox_def.h"
namespace livox_ros {
/** Max lidar data source num */
const uint32_t kMaxSourceLidar = 32;
/** Eth packet relative info parama */
const uint32_t kMaxPointPerEthPacket = 100;
const uint32_t kMinEthPacketQueueSize = 32; /**< must be 2^n */
const uint32_t kMaxEthPacketQueueSize = 8192; /**< must be 2^n */
const uint32_t kImuEthPacketQueueSize = 256;
const uint32_t KEthPacketHeaderLength = 18; /**< (sizeof(LivoxEthPacket) - 1) */
//const uint32_t KEthPacketMaxLength = 1500;
const uint32_t KCartesianPointSize = 13;
const uint32_t KSphericalPointSzie = 9;
const int64_t kPacketTimeGap = 1000000; /**< 1ms = 1000000ns */
const int64_t kMaxPacketTimeGap = 1700000; /**< the threshold of packet continuous */
const int64_t kDeviceDisconnectThreshold = 1000000000; /**< the threshold of device disconect */
const int64_t kNsPerSecond = 1000000000; /**< 1s = 1000000000ns */
const int kPathStrMinSize = 4; /**< Must more than 4 char */
const int kPathStrMaxSize = 256; /**< Must less than 256 char */
const int kBdCodeSize = 15;
const uint32_t kPointXYZRSize = 16;
const uint32_t kPointXYZRTRSize = 18;
const double PI = 3.14159265358979323846;
/** Lidar connect state */
typedef enum {
kConnectStateOff = 0,
kConnectStateOn = 1,
kConnectStateConfig = 2,
kConnectStateSampling = 3,
} LidarConnectState;
/** Device data source type */
typedef enum {
kSourceRawLidar = 0, /**< Data from raw lidar. */
kSourceRawHub = 1, /**< Data from lidar hub. */
kSourceLvxFile, /**< Data from parse lvx file. */
kSourceUndef,
} LidarDataSourceType;
/** Lidar Data output type */
typedef enum {
kOutputToRos = 0,
kOutputToRosBagFile = 1,
} LidarDataOutputType;
typedef enum {
kCoordinateCartesian = 0,
kCoordinateSpherical
} CoordinateType;
typedef enum {
kConfigFan = 1,
kConfigReturnMode = 2,
kConfigCoordinate = 4,
kConfigImuRate = 8,
kConfigGetExtrinsicParameter = 16,
kConfigUndef
} LidarConfigCodeBit;
typedef enum {
kNoneExtrinsicParameter,
kExtrinsicParameterFromLidar,
kExtrinsicParameterFromXml
} ExtrinsicParameterType;
typedef struct {
uint32_t receive_packet_count;
uint32_t loss_packet_count;
int64_t last_timestamp;
int64_t timebase; /**< unit:ns */
int64_t last_imu_timestamp;
int64_t imu_timebase; /**< unit:ns */
uint32_t timebase_state;
} LidarPacketStatistic;
/** 8bytes stamp to uint64_t stamp */
typedef union {
struct {
uint32_t low;
uint32_t high;
} stamp_word;
uint8_t stamp_bytes[8];
int64_t stamp;
} LdsStamp;
/** Configuration in json config file for livox lidar */
typedef struct {
char broadcast_code[16];
bool enable_connect;
bool enable_fan;
uint32_t return_mode;
uint32_t coordinate;
uint32_t imu_rate;
uint32_t extrinsic_parameter_source;
} UserRawConfig;
typedef struct {
bool enable_fan;
uint32_t return_mode;
uint32_t coordinate;
uint32_t imu_rate;
uint32_t extrinsic_parameter_source;
volatile uint32_t set_bits;
volatile uint32_t get_bits;
} UserConfig;
typedef float EulerAngle[3]; /**< Roll, Pitch, Yaw, unit:radian. */
typedef float TranslationVector[3]; /**< x, y, z translation, unit: m. */
typedef float RotationMatrix[3][3];
typedef struct {
EulerAngle euler;
TranslationVector trans;
RotationMatrix rotation;
bool enable;
} ExtrinsicParameter;
/** Lidar data source info abstract */
typedef struct {
uint8_t handle; /**< Lidar access handle. */
uint8_t data_src; /**< From raw lidar or livox file. */
volatile LidarConnectState connect_state;
DeviceInfo info;
LidarPacketStatistic statistic_info;
LidarDataQueue data;
LidarDataQueue imu_data;
uint32_t firmware_ver; /**< Firmware version of lidar */
UserConfig config;
ExtrinsicParameter extrinsic_parameter;
} LidarDevice;
typedef struct {
uint32_t points_per_packet;
uint32_t points_per_second; /**< unit:ns */
uint32_t point_interval;
uint32_t packet_length;
} PacketInfoPair;
#pragma pack(1)
typedef struct{
float x; /**< X axis, Unit:m */
float y; /**< Y axis, Unit:m */
float z; /**< Z axis, Unit:m */
} PointXyz;
typedef struct{
float x; /**< X axis, Unit:m */
float y; /**< Y axis, Unit:m */
float z; /**< Z axis, Unit:m */
float reflectivity; /**< Reflectivity */
} LivoxPointXyzr;
typedef struct{
float x; /**< X axis, Unit:m */
float y; /**< Y axis, Unit:m */
float z; /**< Z axis, Unit:m */
float reflectivity; /**< Reflectivity */
uint8_t tag; /**< Livox point tag */
uint8_t line; /**< Laser line id */
} LivoxPointXyzrtl;
#pragma pack()
typedef uint8_t* (* PointConvertHandler)(uint8_t* point_buf, LivoxEthPacket* eth_packet, \
ExtrinsicParameter& extrinsic);
const PacketInfoPair packet_info_pair_table[kMaxPointDataType] = {
{100, 100000, 10000 , 1318},
{100, 100000, 10000 , 918},
{96, 240000, 4167 , 1362},
{96, 240000, 4167 , 978},
{96, 480000, 4167 , 1362},
{48, 480000, 4167 , 978},
{1, 100, 10000000, 42}
};
/**
* Global function for general use.
*/
bool IsFilePathValid(const char* path_str);
uint64_t GetStoragePacketTimestamp(StoragePacket* packet, uint8_t data_src_);
uint32_t CalculatePacketQueueSize(uint32_t interval_ms, uint32_t data_type);
void ParseCommandlineInputBdCode(const char* cammandline_str,
std::vector<std::string>& bd_code_list);
PointConvertHandler GetConvertHandler(uint8_t data_type);
uint8_t* LivoxPointToPxyzrtl(uint8_t* point_buf, LivoxEthPacket* eth_packet, \
ExtrinsicParameter& extrinsic);
uint8_t* LivoxImuDataProcess(uint8_t* point_buf, LivoxEthPacket* eth_packet);
void EulerAnglesToRotationMatrix(EulerAngle euler, RotationMatrix matrix);
void PointExtrisincCompensation(PointXyz* dst_point, ExtrinsicParameter& extrinsic);
inline uint32_t GetPointInterval(uint32_t data_type) {
return packet_info_pair_table[data_type].point_interval;
}
inline uint32_t GetPacketNumPerSec(uint32_t data_type) {
return packet_info_pair_table[data_type].points_per_second / packet_info_pair_table[data_type].points_per_packet;
}
inline uint32_t GetPointsPerPacket(uint32_t data_type) {
return packet_info_pair_table[data_type].points_per_packet;
}
inline uint32_t GetEthPacketLen(uint32_t data_type) {
return packet_info_pair_table[data_type].packet_length;
}
inline void RawPointConvert(LivoxPointXyzr* dst_point, LivoxPoint* raw_point) {
dst_point->x = raw_point->x;
dst_point->y = raw_point->y;
dst_point->z = raw_point->z;
dst_point->reflectivity = (float)raw_point->reflectivity;
}
inline void RawPointConvert(LivoxPointXyzr* dst_point, LivoxRawPoint* raw_point) {
dst_point->x = raw_point->x/1000.0f;
dst_point->y = raw_point->y/1000.0f;
dst_point->z = raw_point->z/1000.0f;
dst_point->reflectivity = (float)raw_point->reflectivity;
}
inline void RawPointConvert(LivoxPointXyzr* dst_point, LivoxSpherPoint* raw_point) {
double radius = raw_point->depth / 1000.0;
double theta = raw_point->theta / 100.0 / 180 * PI;
double phi = raw_point->phi / 100.0 / 180 * PI;
dst_point->x = radius * sin(theta) * cos(phi);
dst_point->y = radius * sin(theta) * sin(phi);
dst_point->z = radius * cos(theta);
dst_point->reflectivity = (float)raw_point->reflectivity;
}
inline void RawPointConvert(LivoxPointXyzr* dst_point1, LivoxPointXyzr* dst_point2, \
LivoxDualExtendSpherPoint* raw_point) {
double radius1 = raw_point->depth1 / 1000.0;
double radius2 = raw_point->depth2 / 1000.0;
double theta = raw_point->theta / 100.0 / 180 * PI;
double phi = raw_point->phi / 100.0 / 180 * PI;
dst_point1->x = radius1 * sin(theta) * cos(phi);
dst_point1->y = radius1 * sin(theta) * sin(phi);
dst_point1->z = radius1 * cos(theta);
dst_point1->reflectivity = (float)raw_point->reflectivity1;
(dst_point2 + 1)->x = radius2 * sin(theta) * cos(phi);
(dst_point2 + 1)->y = radius2 * sin(theta) * sin(phi);
(dst_point2 + 1)->z = radius2 * cos(theta);
(dst_point2 + 1)->reflectivity = (float)raw_point->reflectivity2;
}
/**
* Lidar data source abstract.
*/
class Lds {
public:
Lds(uint32_t buffer_time_ms, uint8_t data_src);
virtual ~Lds();
uint8_t GetDeviceType(uint8_t handle);
static void ResetLidar(LidarDevice* lidar, uint8_t data_src);
static void SetLidarDataSrc(LidarDevice* lidar, uint8_t data_src);
void ResetLds(uint8_t data_src);
void RequestExit() { request_exit_ = true; }
void CleanRequestExit() { request_exit_ = false; }
bool IsRequestExit() { return request_exit_; }
virtual void PrepareExit(void);
uint8_t lidar_count_; /**< Lidar access handle. */
LidarDevice lidars_[kMaxSourceLidar]; /**< The index is the handle */
protected:
uint32_t buffer_time_ms_; /**< Buffer time before data in queue is read */
uint8_t data_src_;
private:
volatile bool request_exit_;
};
}
#endif

760
livox_ros_driver/livox_ros_driver/lds_hub.cpp Normal file
View File

@@ -0,0 +1,760 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2019 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include "lds_hub.h"
#include <stdio.h>
#include <string.h>
#include <thread>
#include <memory>
#include "rapidjson/document.h"
#include "rapidjson/stringbuffer.h"
#include "rapidjson/filereadstream.h"
namespace livox_ros {
/** Const varible ------------------------------------------------------------------------------- */
/** For callback use only */
static LdsHub* g_lds_hub = nullptr;
/** Global function for common use ---------------------------------------------------------------*/
/** Lds hub function -----------------------------------------------------------------------------*/
LdsHub::LdsHub(uint32_t interval_ms) : Lds(interval_ms, kSourceRawHub) {
auto_connect_mode_ = true;
whitelist_count_ = 0;
is_initialized_ = false;
whitelist_count_ = 0;
memset(broadcast_code_whitelist_, 0, sizeof(broadcast_code_whitelist_));
ResetLdsHub();
}
LdsHub::~LdsHub() {
}
void LdsHub::ResetLdsHub(void) {
ResetLds(kSourceRawHub);
ResetLidar(&hub_, kSourceRawHub);
}
int LdsHub::InitLdsHub(std::vector<std::string>& broadcast_code_strs,\
const char *user_config_path) {
if (is_initialized_) {
printf("LiDAR data source is already inited!\n");
return -1;
}
if (!Init()) {
Uninit();
printf("Livox-SDK init fail!\n");
return -1;
}
LivoxSdkVersion _sdkversion;
GetLivoxSdkVersion(&_sdkversion);
printf("Livox SDK version %d.%d.%d\n", _sdkversion.major, _sdkversion.minor, _sdkversion.patch);
SetBroadcastCallback(LdsHub::OnDeviceBroadcast);
SetDeviceStateUpdateCallback(LdsHub::OnDeviceChange);
for (auto input_str : broadcast_code_strs) {
AddBroadcastCodeToWhitelist(input_str.c_str());
printf("Cmdline input broadcast code : %s\n", input_str.c_str());
}
ParseConfigFile(user_config_path);
if (whitelist_count_) {
DisableAutoConnectMode();
printf("Disable auto connect mode!\n");
printf("List all broadcast code in whiltelist:\n");
for (uint32_t i=0; i<whitelist_count_; i++) {
printf("%s\n", broadcast_code_whitelist_[i]);
}
} else {
EnableAutoConnectMode();
printf("No broadcast code was added to whitelist, swith to automatic connection mode!\n");
}
/** Start livox sdk to receive lidar data */
if (!Start()) {
Uninit();
printf("Livox-SDK init fail!\n");
return -1;
}
/** Add here, only for callback use */
if (g_lds_hub == nullptr) {
g_lds_hub = this;
}
is_initialized_= true;
printf("Livox-SDK init success!\n");
return 0;
}
int LdsHub::DeInitLdsHub(void) {
if (!is_initialized_) {
printf("LiDAR data source is not exit");
return -1;
}
Uninit();
printf("Livox SDK Deinit completely!\n");
return 0;
}
void LdsHub::PrepareExit(void) {
DeInitLdsHub();
}
/** Static function in LdsLidar for callback */
void LdsHub::OnHubDataCb(uint8_t hub_handle, LivoxEthPacket *data,
uint32_t data_num, void *client_data) {
LdsHub* lds_hub = static_cast<LdsHub *>(client_data);
LivoxEthPacket* eth_packet = data;
if (!data || !data_num) {
return;
}
/** Caculate which lidar the eth packet data belong to */
uint8_t handle = HubGetLidarHandle(eth_packet->slot, eth_packet->id);
if (handle >= kMaxLidarCount) {
return;
}
LidarDevice* p_lidar = &lds_hub->lidars_[handle];
LidarPacketStatistic *packet_statistic = &p_lidar->statistic_info;
LdsStamp cur_timestamp;
memcpy(cur_timestamp.stamp_bytes, eth_packet->timestamp, sizeof(cur_timestamp));
if (kImu != eth_packet->data_type) {
if (eth_packet->timestamp_type == kTimestampTypePps) {
/** Whether a new sync frame */
if ((cur_timestamp.stamp < packet_statistic->last_timestamp) && \
(cur_timestamp.stamp < kPacketTimeGap)) {
auto cur_time = std::chrono::high_resolution_clock::now();
int64_t sync_time = cur_time.time_since_epoch().count();
/** used receive time as timebase */
packet_statistic->timebase = sync_time;
}
}
packet_statistic->last_timestamp = cur_timestamp.stamp;
LidarDataQueue *p_queue = &p_lidar->data;
if (nullptr == p_queue->storage_packet) {
uint32_t queue_size = CalculatePacketQueueSize(lds_hub->buffer_time_ms_, \
eth_packet->data_type);
queue_size = queue_size * 16; /* 16 multiple the min size */
InitQueue(p_queue, queue_size);
printf("Lidar%02d[%s] queue size : %d %d\n", p_lidar->handle, p_lidar->info.broadcast_code, \
queue_size, p_queue->size);
}
if (!QueueIsFull(p_queue)) {
QueuePushAny(p_queue, (uint8_t *)eth_packet,\
GetEthPacketLen(eth_packet->data_type), packet_statistic->timebase, \
GetPointsPerPacket(eth_packet->data_type));
}
} else {
if (eth_packet->timestamp_type == kTimestampTypePps) {
/** Whether a new sync frame */
if ((cur_timestamp.stamp < packet_statistic->last_imu_timestamp) && \
(cur_timestamp.stamp < kPacketTimeGap)) {
auto cur_time = std::chrono::high_resolution_clock::now();
int64_t sync_time = cur_time.time_since_epoch().count();
/** used receive time as timebase */
packet_statistic->imu_timebase = sync_time;
}
}
packet_statistic->last_imu_timestamp = cur_timestamp.stamp;
LidarDataQueue *p_queue = &p_lidar->imu_data;
if (nullptr == p_queue->storage_packet) {
uint32_t queue_size = 256;
InitQueue(p_queue, queue_size);
printf("Lidar%02d[%s] imu queue size : %d %d\n", p_lidar->handle,\
p_lidar->info.broadcast_code, queue_size, p_queue->size);
}
if (!QueueIsFull(p_queue) && (cur_timestamp.stamp >= 0)) {
QueuePushAny(p_queue, (uint8_t *)eth_packet,\
GetEthPacketLen(eth_packet->data_type), packet_statistic->imu_timebase, \
GetPointsPerPacket(eth_packet->data_type));
}
}
}
void LdsHub::OnDeviceBroadcast(const BroadcastDeviceInfo *info) {
if (info == NULL) {
return;
}
if (info->dev_type != kDeviceTypeHub) {
printf("It's not a hub : %s\n", info->broadcast_code);
return;
}
if (g_lds_hub->IsAutoConnectMode()) {
printf("In automatic connection mode, will connect %s\n", info->broadcast_code);
} else {
if (!g_lds_hub->IsBroadcastCodeExistInWhitelist(info->broadcast_code)) {
printf("Not in the whitelist, please add %s to if want to connect!\n",\
info->broadcast_code);
return;
}
}
LidarDevice* p_hub = &g_lds_hub->hub_;
if (p_hub->connect_state == kConnectStateOff) {
bool result = false;
uint8_t handle = 0;
result = AddHubToConnect(info->broadcast_code, &handle);
if (result == kStatusSuccess && handle < kMaxLidarCount) {
SetDataCallback(handle, LdsHub::OnHubDataCb, (void *)g_lds_hub);
p_hub->handle = handle;
p_hub->connect_state = kConnectStateOff;
printf("add to connect\n");
UserRawConfig config;
if (strncmp(info->broadcast_code, g_lds_hub->hub_raw_config_.broadcast_code, \
sizeof(info->broadcast_code)) != 0) {
printf("Could not find hub raw config, set config to default!\n");
config.enable_fan = 1;
config.return_mode = kFirstReturn;
config.coordinate = kCoordinateCartesian;
config.imu_rate = kImuFreq200Hz;
} else {
config = g_lds_hub->hub_raw_config_;
}
p_hub->config.enable_fan = config.enable_fan;
p_hub->config.return_mode = config.return_mode;
p_hub->config.coordinate = config.coordinate;
p_hub->config.imu_rate = config.imu_rate;
} else {
printf("Add Hub to connect is failed : %d %d \n", result, handle);
}
}
}
/** Callback function of changing of device state. */
void LdsHub::OnDeviceChange(const DeviceInfo *info, DeviceEvent type) {
if (info == NULL) {
return;
}
if (info->handle >= kMaxLidarCount) {
return;
}
LidarDevice* p_hub = &g_lds_hub->hub_;
if (type == kEventHubConnectionChange) {
if (p_hub->connect_state == kConnectStateOff) {
p_hub->connect_state = kConnectStateOn;
p_hub->info = *info;
printf("Hub[%s] connect on\n", p_hub->info.broadcast_code);
}
} else if (type == kEventDisconnect) {
p_hub->connect_state = kConnectStateOff;
printf("Hub[%s] disconnect!\n", info->broadcast_code);
} else if (type == kEventStateChange) {
p_hub->info = *info;
printf("Hub[%s] StateChange\n", info->broadcast_code);
}
if (p_hub->connect_state == kConnectStateOn) {
printf("Hub[%s] status_code[%d] working state[%d] feature[%d]\n", \
p_hub->info.broadcast_code,\
p_hub->info.status.status_code.error_code,\
p_hub->info.state,\
p_hub->info.feature);
SetErrorMessageCallback(p_hub->handle, LdsHub::HubErrorStatusCb);
if (p_hub->info.state == kLidarStateNormal) {
HubQueryLidarInformation(HubQueryLidarInfoCb, g_lds_hub);
}
}
}
void LdsHub::HubQueryLidarInfoCb(livox_status status, uint8_t handle, \
HubQueryLidarInformationResponse *response,\
void *client_data) {
LdsHub* lds_hub = static_cast<LdsHub *>(client_data);
if ((handle >= kMaxLidarCount) || !response) {
return;
}
if ((status == kStatusSuccess) && !response->ret_code) {
if (response->count) {
printf("Hub have %d lidars:\n", response->count);
for (int i = 0; i < response->count; i++) {
uint32_t index = HubGetLidarHandle(response->device_info_list[i].slot,\
response->device_info_list[i].id);
if (index < kMaxLidarCount) {
LidarDevice* p_lidar = &lds_hub->lidars_[index];
p_lidar->handle = index;
p_lidar->info.handle = index;
p_lidar->info.slot = response->device_info_list[i].slot;
p_lidar->info.id = response->device_info_list[i].id;
p_lidar->info.type = response->device_info_list[i].dev_type;
p_lidar->connect_state = kConnectStateSampling;
strncpy(p_lidar->info.broadcast_code, \
response->device_info_list[i].broadcast_code, \
sizeof(p_lidar->info.broadcast_code));
printf("[%d]%s DeviceType[%d] Slot[%d] Ver[%d.%d.%d.%d]\n", index, \
p_lidar->info.broadcast_code,\
p_lidar->info.type, p_lidar->info.slot,\
response->device_info_list[i].version[0],\
response->device_info_list[i].version[1],\
response->device_info_list[i].version[2],\
response->device_info_list[i].version[3]);
}
}
ConfigLidarsOfHub(lds_hub);
} else {
printf("Hub have no lidar, will not start sample!\n");
HubQueryLidarInformation(HubQueryLidarInfoCb, lds_hub);
}
} else {
printf("Device Query Informations Failed %d\n", status);
HubQueryLidarInformation(HubQueryLidarInfoCb, lds_hub);
}
}
/** Callback function of hub error message. */
void LdsHub::HubErrorStatusCb(livox_status status, uint8_t handle, ErrorMessage *message) {
static uint32_t error_message_count = 0;
if (message != NULL) {
++error_message_count;
if (0 == (error_message_count % 100)) {
printf("handle: %u\n", handle);
printf("sync_status : %u\n", message->hub_error_code.sync_status);
printf("temp_status : %u\n", message->hub_error_code.temp_status);
printf("lidar_status :%u\n", message->hub_error_code.lidar_status);
printf("lidar_link_status : %u\n", message->hub_error_code.lidar_link_status);
printf("firmware_err : %u\n", message->hub_error_code.firmware_err);
printf("system_status : %u\n", message->hub_error_code.system_status);
}
}
}
void LdsHub::ControlFanCb(livox_status status, uint8_t handle, \
uint8_t response, void *clent_data) {
}
void LdsHub::HubSetPointCloudReturnModeCb(livox_status status, uint8_t handle, \
HubSetPointCloudReturnModeResponse* response,\
void *clent_data) {
LdsHub* lds_hub = static_cast<LdsHub *>(clent_data);
if ((handle >= kMaxLidarCount) || !response) {
return;
}
if ((status != kStatusSuccess) || (response->ret_code)) {
printf("Hub set return mode fail!\n");
ConfigPointCloudReturnMode(lds_hub);
} else {
printf("Hub set return mode success!\n");
lds_hub->hub_.config.set_bits &= ~((uint32_t)(kConfigReturnMode));
if (!lds_hub->hub_.config.set_bits) {
HubStartSampling(LdsHub::StartSampleCb, lds_hub);
lds_hub->hub_.connect_state = kConnectStateSampling;
}
}
}
void LdsHub::SetCoordinateCb(livox_status status, uint8_t handle, \
uint8_t response, void *clent_data) {
LdsHub* lds_hub = static_cast<LdsHub *>(clent_data);
if (handle >= kMaxLidarCount) {
return;
}
LidarDevice* p_hub = &(lds_hub->hub_);
if (status == kStatusSuccess) {
p_hub->config.set_bits &= ~((uint32_t)(kConfigCoordinate));
printf("Set coordinate success!\n");
if (!p_hub->config.set_bits) {
HubStartSampling(LdsHub::StartSampleCb, lds_hub);
p_hub->connect_state = kConnectStateSampling;
}
} else {
if (p_hub->config.coordinate != 0) {
SetSphericalCoordinate(handle, LdsHub::SetCoordinateCb, lds_hub);
} else {
SetCartesianCoordinate(handle, LdsHub::SetCoordinateCb, lds_hub);
}
printf("Set coordinate fail, try again!\n");
}
}
void LdsHub::HubSetImuRatePushFrequencyCb(livox_status status, uint8_t handle, \
HubSetImuPushFrequencyResponse* response,\
void *clent_data) {
LdsHub* lds_hub = static_cast<LdsHub *>(clent_data);
if ((handle >= kMaxLidarCount) || !response) {
return;
}
if ((status != kStatusSuccess) || (response->ret_code)) {
printf("Hub set imu freq fail [%d]!\n", response->ret_code);
ConfigImuPushFrequency(lds_hub);
} else {
printf("Hub set imu frequency success!\n");
lds_hub->hub_.config.set_bits &= ~((uint32_t)(kConfigImuRate));
if (!lds_hub->hub_.config.set_bits) {
HubStartSampling(LdsHub::StartSampleCb, lds_hub);
lds_hub->hub_.connect_state = kConnectStateSampling;
}
}
}
/** Callback function of starting sampling. */
void LdsHub::StartSampleCb(livox_status status, uint8_t handle, \
uint8_t response, void *clent_data) {
LdsHub* lds_hub = static_cast<LdsHub *>(clent_data);
if (handle >= kMaxLidarCount) {
return;
}
LidarDevice* p_hub = &lds_hub->hub_;
if ((status != kStatusSuccess) || (response != 0)) {
p_hub->connect_state = kConnectStateOn;
printf("Hub start sample fail : state[%d] handle[%d] res[%d]\n", \
status, handle, response);
for (int i = 0; i < kMaxLidarCount; i++) {
LidarDevice* p_lidar = &(lds_hub->lidars_[i]);
if (p_lidar->connect_state == kConnectStateSampling) {
p_lidar->connect_state = kConnectStateOn;
}
}
} else {
printf("Hub start sample success!\n");
}
}
/** Callback function of stopping sampling. */
void LdsHub::StopSampleCb(livox_status status, uint8_t handle, \
uint8_t response, void *clent_data) {
}
void LdsHub::ConfigPointCloudReturnMode(LdsHub* lds_hub) {
uint8_t req_buf[1024];
HubSetPointCloudReturnModeRequest* req = (HubSetPointCloudReturnModeRequest *)req_buf;
req->count = 0;
for (int i = 0; i < kMaxLidarCount; i++) {
LidarDevice* p_lidar = &(lds_hub->lidars_[i]);
if ((p_lidar->info.type != kDeviceTypeLidarMid40) && \
(p_lidar->connect_state == kConnectStateSampling)) {
UserRawConfig config;
if (lds_hub->GetRawConfig(p_lidar->info.broadcast_code, config)) {
printf("Could not find raw config, set config to default!\n");
config.enable_fan = 1;
config.return_mode = kFirstReturn;
config.coordinate = kCoordinateCartesian;
config.imu_rate = kImuFreq200Hz;
}
strncpy(req->lidar_cfg_list[req->count].broadcast_code, \
p_lidar->info.broadcast_code, \
sizeof(req->lidar_cfg_list[req->count].broadcast_code));
req->lidar_cfg_list[req->count].mode = config.return_mode;
req->count++;
}
}
if (req->count) {
uint32_t length = 1 + sizeof(SetPointCloudReturnModeRequestItem) * req->count;
HubSetPointCloudReturnMode(req, length,\
LdsHub::HubSetPointCloudReturnModeCb, lds_hub);
lds_hub->hub_.config.set_bits |= kConfigReturnMode;
}
}
void LdsHub::ConfigImuPushFrequency(LdsHub* lds_hub) {
uint8_t req_buf[1024];
HubSetImuPushFrequencyRequest* req = (HubSetImuPushFrequencyRequest *)req_buf;
req->count = 0;
for (int i = 0; i < kMaxLidarCount; i++) {
LidarDevice* p_lidar = &(lds_hub->lidars_[i]);
if ((p_lidar->info.type != kDeviceTypeLidarMid40) && \
(p_lidar->connect_state == kConnectStateSampling)) {
UserRawConfig config;
if (lds_hub->GetRawConfig(p_lidar->info.broadcast_code, config)) {
printf("Could not find raw config, set config to default!\n");
config.enable_fan = 1;
config.return_mode = kFirstReturn;
config.coordinate = kCoordinateCartesian;
config.imu_rate = kImuFreq200Hz;
}
strncpy(req->lidar_cfg_list[req->count].broadcast_code, \
p_lidar->info.broadcast_code, \
sizeof(req->lidar_cfg_list[req->count].broadcast_code));
req->lidar_cfg_list[req->count].freq = config.imu_rate;
req->count++;
}
}
if (req->count) {
uint32_t length = 1 + sizeof(SetImuPushFrequencyRequestItem) * req->count;
HubSetImuPushFrequency(req, length,\
LdsHub::HubSetImuRatePushFrequencyCb, lds_hub);
lds_hub->hub_.config.set_bits |= kConfigImuRate;
}
}
void LdsHub::ConfigLidarsOfHub(LdsHub* lds_hub) {
ConfigPointCloudReturnMode(lds_hub);
ConfigImuPushFrequency(lds_hub);
if (lds_hub->hub_.config.coordinate != 0) {
SetSphericalCoordinate(lds_hub->hub_.handle, LdsHub::SetCoordinateCb, lds_hub);
printf("Hub set coordinate spherical\n");
} else {
printf("Hub set coordinate cartesian\n");
SetCartesianCoordinate(lds_hub->hub_.handle, LdsHub::SetCoordinateCb, lds_hub);
}
lds_hub->hub_.config.set_bits |= kConfigCoordinate;
lds_hub->hub_.connect_state = kConnectStateConfig;
}
/** Add broadcast code to whitelist */
int LdsHub::AddBroadcastCodeToWhitelist(const char* broadcast_code) {
if (!broadcast_code || (strlen(broadcast_code) > kBroadcastCodeSize) || \
(whitelist_count_ >= kMaxLidarCount)) {
return -1;
}
if (IsBroadcastCodeExistInWhitelist(broadcast_code)) {
printf("%s is alrealy exist!\n", broadcast_code);
return -1;
}
strcpy(broadcast_code_whitelist_[whitelist_count_], broadcast_code);
++whitelist_count_;
return 0;
}
bool LdsHub::IsBroadcastCodeExistInWhitelist(const char* broadcast_code) {
if (!broadcast_code) {
return false;
}
for (uint32_t i=0; i<whitelist_count_; i++) {
if (strncmp(broadcast_code, broadcast_code_whitelist_[i], kBroadcastCodeSize) == 0) {
return true;
}
}
return false;
}
/** Get and update LiDAR info */
void LdsHub::UpdateHubLidarinfo(void) {
DeviceInfo *_lidars = (DeviceInfo *) malloc(sizeof(DeviceInfo) * kMaxLidarCount);
uint8_t count = kMaxLidarCount;
uint8_t status = GetConnectedDevices(_lidars, &count);
if (status == kStatusSuccess) {
printf("Hub have lidars : \n");
int i = 0;
for (i = 0; i < count; ++i) {
uint8_t handle = _lidars[i].handle;
if (handle < kMaxLidarCount) {
lidars_[handle].handle = handle;
lidars_[handle].info = _lidars[i];
lidars_[handle].connect_state = kConnectStateSampling;
printf("[%d] : %s\r\n", _lidars[i].handle, _lidars[i].broadcast_code);
}
}
}
if (_lidars) {
free(_lidars);
}
}
/** Config file process */
int LdsHub::ParseConfigFile(const char* pathname) {
FILE* raw_file = std::fopen(pathname, "rb");
if(!raw_file) {
printf("Open json config file fail!\n");
return -1;
}
char read_buffer[32768];
rapidjson::FileReadStream config_file(raw_file, read_buffer, sizeof(read_buffer));
rapidjson::Document doc;
if(!doc.ParseStream(config_file).HasParseError()) {
if(doc.HasMember("hub_config") && doc["hub_config"].IsObject()) {
const rapidjson::Value& object = doc["hub_config"];
UserRawConfig hub_config;
memset(&hub_config, 0, sizeof(hub_config));
if(object.HasMember("broadcast_code") && \
object["broadcast_code"].IsString()) {
std::string broadcast_code = object["broadcast_code"].GetString();
std::memcpy(hub_config.broadcast_code, broadcast_code.c_str(),\
sizeof(hub_config.broadcast_code));
if(object.HasMember("enable_connect") && \
object["enable_connect"].IsBool()) {
hub_config.enable_connect = object["enable_connect"].GetBool();
}
if(object.HasMember("coordinate") && object["coordinate"].IsInt()) {
hub_config.coordinate = object["coordinate"].GetInt();
}
printf("Hub[%s] : %d %d %d %d %d\n", hub_config.broadcast_code, \
hub_config.enable_connect, hub_config.enable_fan, hub_config.return_mode,\
hub_config.coordinate, hub_config.imu_rate);
if (hub_config.enable_connect) {
if (!AddBroadcastCodeToWhitelist(hub_config.broadcast_code)) {
hub_raw_config_ = hub_config;
printf("Add hub[%s] [%d] to whitelist\n", hub_raw_config_.broadcast_code, \
hub_raw_config_.coordinate);
} else {
memset(&hub_raw_config_, 0, sizeof(hub_raw_config_));
printf("Add hub[%s] to whitelist fail\n", hub_raw_config_.broadcast_code);
}
}
} else {
printf("User hub config file parse error\n");
}
}
if(doc.HasMember("lidar_config") && doc["lidar_config"].IsArray()) {
const rapidjson::Value& array = doc["lidar_config"];
size_t len = array.Size();
for(size_t i=0; i<len; i++) {
const rapidjson::Value& object = array[i];
if(object.IsObject()) {
UserRawConfig config;
memset(&config, 0, sizeof(config));
if(object.HasMember("broadcast_code") && \
object["broadcast_code"].IsString()) {
std::string broadcast_code = object["broadcast_code"].GetString();
std::memcpy(config.broadcast_code, broadcast_code.c_str(),\
sizeof(config.broadcast_code));
} else {
printf("User config file parse error\n");
continue;
}
if(object.HasMember("enable_fan") && \
object["enable_fan"].IsBool()) {
config.enable_fan = object["enable_fan"].GetBool();
}
if(object.HasMember("return_mode") && object["return_mode"].IsInt()) {
config.return_mode = object["return_mode"].GetInt();
}
if(object.HasMember("imu_rate") && object["imu_rate"].IsInt()) {
config.imu_rate = object["imu_rate"].GetInt();
}
if (hub_raw_config_.enable_connect) {
config.coordinate = hub_raw_config_.coordinate;
} else {
config.coordinate = 0;
}
printf("Lidar[%s] : %d %d %d %d %d\n", config.broadcast_code, \
config.enable_connect, config.enable_fan, config.return_mode,\
config.coordinate, config.imu_rate);
if (AddRawUserConfig(config)) {
printf("Lidar Raw config is already exist : %s \n", config.broadcast_code);
}
}
}
}
} else {
printf("User config file parse error[%d]\n", doc.ParseStream(config_file).HasParseError());
}
std::fclose(raw_file);
return 0;
}
int LdsHub::AddRawUserConfig(UserRawConfig& config) {
if (IsExistInRawConfig(config.broadcast_code)) {
return -1;
}
lidar_raw_config_.push_back(config);
printf("Add lidar raw user config : %s \n", config.broadcast_code);
return 0;
}
bool LdsHub::IsExistInRawConfig(const char* broadcast_code) {
if (broadcast_code == nullptr) {
return false;
}
for (auto ite_config : lidar_raw_config_) {
if (strncmp(ite_config.broadcast_code, broadcast_code, kBroadcastCodeSize) == 0) {
return true;
}
}
return false;
}
int LdsHub::GetRawConfig(const char* broadcast_code, UserRawConfig& config) {
if (broadcast_code == nullptr) {
return -1;
}
for (auto ite_config : lidar_raw_config_) {
if (strncmp(ite_config.broadcast_code, broadcast_code, kBroadcastCodeSize) == 0) {
config.enable_fan = ite_config.enable_fan;
config.return_mode = ite_config.return_mode;
config.coordinate = ite_config.coordinate;
config.imu_rate = ite_config.imu_rate;
return 0;
}
}
return -1;
}
}

115
livox_ros_driver/livox_ros_driver/lds_hub.h Normal file
View File

@@ -0,0 +1,115 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2019 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
/** Livox LiDAR data source, data from hub */
#ifndef LIVOX_ROS_DRIVER_LDS_HUB_H_
#define LIVOX_ROS_DRIVER_LDS_HUB_H_
#include <memory>
#include <vector>
#include "lds.h"
#include "livox_sdk.h"
namespace livox_ros {
/**
* LiDAR data source, data from hub.
*/
class LdsHub : public Lds {
public:
static LdsHub* GetInstance(uint32_t interval_ms) {
static LdsHub lds_hub(interval_ms);
return &lds_hub;
}
int InitLdsHub(std::vector<std::string>& broadcast_code_strs, const char *user_config_path);
int DeInitLdsHub(void);
private:
LdsHub(uint32_t interval_ms);
LdsHub(const LdsHub&) = delete;
~LdsHub();
LdsHub& operator=(const LdsHub&) = delete;
virtual void PrepareExit(void);
static void OnHubDataCb(uint8_t hub_handle, LivoxEthPacket *data,\
uint32_t data_num, void *client_data);
static void OnDeviceBroadcast(const BroadcastDeviceInfo *info);
static void OnDeviceChange(const DeviceInfo *info, DeviceEvent type);
static void StartSampleCb(livox_status status, uint8_t handle, uint8_t response, void *clent_data);
static void StopSampleCb(livox_status status, uint8_t handle, uint8_t response, void *clent_data);
static void HubQueryLidarInfoCb(livox_status status, uint8_t handle, \
HubQueryLidarInformationResponse *response, void *client_data);
static void ControlFanCb(livox_status status, uint8_t handle, \
uint8_t response, void *clent_data);
static void HubSetPointCloudReturnModeCb(livox_status status, uint8_t handle, \
HubSetPointCloudReturnModeResponse* response,\
void *clent_data);
static void SetCoordinateCb(livox_status status, uint8_t handle, \
uint8_t response, void *clent_data);
static void HubSetImuRatePushFrequencyCb(livox_status status, uint8_t handle, \
HubSetImuPushFrequencyResponse* response,\
void *clent_data);
static void HubErrorStatusCb(livox_status status, uint8_t handle, ErrorMessage *message);
static void ConfigPointCloudReturnMode(LdsHub* lds_hub);
static void ConfigImuPushFrequency(LdsHub* lds_hub);
static void ConfigLidarsOfHub(LdsHub* lds_hub);
void ResetLdsHub(void);
void StateReset(void);
int AddBroadcastCodeToWhitelist(const char* broadcast_code);
bool IsBroadcastCodeExistInWhitelist(const char* broadcast_code);
void UpdateHubLidarinfo(void);
void EnableAutoConnectMode(void) { auto_connect_mode_ = true; }
void DisableAutoConnectMode(void) { auto_connect_mode_ = false; }
bool IsAutoConnectMode(void) { return auto_connect_mode_; }
int ParseConfigFile(const char* pathname);
int AddRawUserConfig(UserRawConfig& config);
bool IsExistInRawConfig(const char* broadcast_code);
int GetRawConfig(const char* broadcast_code, UserRawConfig& config);
bool IsAllLidarSetBitsClear() {
for(int i=0; i<kMaxLidarCount; i++) {
if (lidars_[i].config.set_bits) {
return false;
}
}
return true;
}
bool auto_connect_mode_;
uint32_t whitelist_count_;
volatile bool is_initialized_;
char broadcast_code_whitelist_[kMaxLidarCount][kBroadcastCodeSize];
LidarDevice hub_;
std::vector<UserRawConfig> lidar_raw_config_;
UserRawConfig hub_raw_config_;
};
}
#endif

759
livox_ros_driver/livox_ros_driver/lds_lidar.cpp Normal file
View File

@@ -0,0 +1,759 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2019 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include "lds_lidar.h"
#include <stdio.h>
#include <string.h>
#include <thread>
#include <memory>
#include "rapidjson/document.h"
#include "rapidjson/stringbuffer.h"
#include "rapidjson/filereadstream.h"
namespace livox_ros {
/** Const varible ------------------------------------------------------------------------------- */
/** For callback use only */
LdsLidar* g_lds_ldiar = nullptr;
/** Global function for common use ---------------------------------------------------------------*/
/** Lds lidar function ---------------------------------------------------------------------------*/
LdsLidar::LdsLidar(uint32_t interval_ms) : Lds(interval_ms, kSourceRawLidar) {
auto_connect_mode_ = true;
is_initialized_ = false;
whitelist_count_ = 0;
memset(broadcast_code_whitelist_, 0, sizeof(broadcast_code_whitelist_));
ResetLdsLidar();
}
LdsLidar::~LdsLidar() {
}
void LdsLidar::ResetLdsLidar(void) {
ResetLds(kSourceRawLidar);
}
int LdsLidar::InitLdsLidar(std::vector<std::string>& broadcast_code_strs,\
const char *user_config_path) {
if (is_initialized_) {
printf("LiDAR data source is already inited!\n");
return -1;
}
if (!Init()) {
Uninit();
printf("Livox-SDK init fail!\n");
return -1;
}
LivoxSdkVersion _sdkversion;
GetLivoxSdkVersion(&_sdkversion);
printf("Livox SDK version %d.%d.%d\n", _sdkversion.major, _sdkversion.minor, _sdkversion.patch);
SetBroadcastCallback(OnDeviceBroadcast);
SetDeviceStateUpdateCallback(OnDeviceChange);
/** Add commandline input broadcast code */
for (auto input_str : broadcast_code_strs) {
AddBroadcastCodeToWhitelist(input_str.c_str());
}
ParseConfigFile(user_config_path);
if (whitelist_count_) {
DisableAutoConnectMode();
printf("Disable auto connect mode!\n");
printf("List all broadcast code in whiltelist:\n");
for (uint32_t i=0; i<whitelist_count_; i++) {
printf("%s\n", broadcast_code_whitelist_[i]);
}
} else {
EnableAutoConnectMode();
printf("No broadcast code was added to whitelist, swith to automatic connection mode!\n");
}
if (enable_timesync_) {
timesync_ = TimeSync::GetInstance();
if (timesync_->InitTimeSync(timesync_config_)) {
printf("Timesync init fail\n");
return -1;
}
if (timesync_->SetReceiveSyncTimeCb(ReceiveSyncTimeCallback, this)) {
printf("Set Timesync callback fail\n");
return -1;
}
timesync_->StartTimesync();
}
/** Start livox sdk to receive lidar data */
if (!Start()) {
Uninit();
printf("Livox-SDK init fail!\n");
return -1;
}
/** Add here, only for callback use */
if (g_lds_ldiar == nullptr) {
g_lds_ldiar = this;
}
is_initialized_= true;
printf("Livox-SDK init success!\n");
return 0;
}
int LdsLidar::DeInitLdsLidar(void) {
if (!is_initialized_) {
printf("LiDAR data source is not exit");
return -1;
}
Uninit();
printf("Livox SDK Deinit completely!\n");
if (timesync_) {
timesync_->DeInitTimeSync();
}
return 0;
}
void LdsLidar::PrepareExit(void) {
DeInitLdsLidar();
}
/** Static function in LdsLidar for callback or event process ------------------------------------*/
/** Receiving point cloud data from Livox LiDAR. */
void LdsLidar::OnLidarDataCb(uint8_t handle, LivoxEthPacket *data,
uint32_t data_num, void *client_data) {
using namespace std;
LdsLidar* lds_lidar = static_cast<LdsLidar *>(client_data);
LivoxEthPacket* eth_packet = data;
if (!data || !data_num || (handle >= kMaxLidarCount)) {
return;
}
LidarDevice* p_lidar = &lds_lidar->lidars_[handle];
LidarPacketStatistic *packet_statistic = &p_lidar->statistic_info;
LdsStamp cur_timestamp;
memcpy(cur_timestamp.stamp_bytes, eth_packet->timestamp, sizeof(cur_timestamp));
if (kImu != eth_packet->data_type) {
if (eth_packet->timestamp_type == kTimestampTypePps) {
if ((cur_timestamp.stamp< packet_statistic->last_timestamp) && \
(cur_timestamp.stamp < kPacketTimeGap)) { // whether a new sync frame
auto cur_time = std::chrono::high_resolution_clock::now();
int64_t sync_time = cur_time.time_since_epoch().count();
packet_statistic->timebase = sync_time; // used receive time as timebase
}
}
packet_statistic->last_timestamp = cur_timestamp.stamp;
LidarDataQueue *p_queue = &p_lidar->data;
if (nullptr == p_queue->storage_packet) {
uint32_t queue_size = CalculatePacketQueueSize(lds_lidar->buffer_time_ms_, \
eth_packet->data_type);
queue_size = queue_size * 16; /* 16 multiple the min size */
InitQueue(p_queue, queue_size);
printf("Lidar%02d[%s] queue size : %d %d\n", p_lidar->handle, p_lidar->info.broadcast_code, \
queue_size, p_queue->size);
}
if (!QueueIsFull(p_queue)) {
QueuePushAny(p_queue, (uint8_t *)eth_packet,\
GetEthPacketLen(eth_packet->data_type), packet_statistic->timebase, \
GetPointsPerPacket(eth_packet->data_type));
}
} else {
if (eth_packet->timestamp_type == kTimestampTypePps) {
if ((cur_timestamp.stamp< packet_statistic->last_imu_timestamp) && \
(cur_timestamp.stamp < kPacketTimeGap)) { // whether a new sync frame
auto cur_time = std::chrono::high_resolution_clock::now();
int64_t sync_time = cur_time.time_since_epoch().count();
packet_statistic->imu_timebase = sync_time; // used receive time as timebase
}
}
packet_statistic->last_imu_timestamp = cur_timestamp.stamp;
LidarDataQueue *p_queue = &p_lidar->imu_data;
if (nullptr == p_queue->storage_packet) {
uint32_t queue_size = 256;
InitQueue(p_queue, queue_size);
printf("Lidar%02d[%s] imu queue size : %d %d\n", p_lidar->handle, \
p_lidar->info.broadcast_code, queue_size, p_queue->size);
}
if (!QueueIsFull(p_queue) && (cur_timestamp.stamp >= 0)) {
QueuePushAny(p_queue, (uint8_t *)eth_packet,\
GetEthPacketLen(eth_packet->data_type), packet_statistic->timebase, \
GetPointsPerPacket(eth_packet->data_type));
}
}
}
void LdsLidar::OnDeviceBroadcast(const BroadcastDeviceInfo *info) {
if (info == nullptr) {
return;
}
if (info->dev_type == kDeviceTypeHub) {
printf("In lidar mode, couldn't connect a hub : %s\n", info->broadcast_code);
return;
}
if (g_lds_ldiar->IsAutoConnectMode()) {
printf("In automatic connection mode, will connect %s\n", info->broadcast_code);
} else {
if (!g_lds_ldiar->IsBroadcastCodeExistInWhitelist(info->broadcast_code)) {
printf("Not in the whitelist, please add %s to if want to connect!\n",\
info->broadcast_code);
return;
}
}
bool result = false;
uint8_t handle = 0;
result = AddLidarToConnect(info->broadcast_code, &handle);
if (result == kStatusSuccess && handle < kMaxLidarCount) {
SetDataCallback(handle, OnLidarDataCb, (void *)g_lds_ldiar);
LidarDevice* p_lidar = &(g_lds_ldiar->lidars_[handle]);
p_lidar->handle = handle;
p_lidar->connect_state = kConnectStateOff;
UserRawConfig config;
if (g_lds_ldiar->GetRawConfig(info->broadcast_code, config)) {
printf("Could not find raw config, set config to default!\n");
config.enable_fan = 1;
config.return_mode = kFirstReturn;
config.coordinate = kCoordinateCartesian;
config.imu_rate = kImuFreq200Hz;
config.extrinsic_parameter_source = kNoneExtrinsicParameter;
}
p_lidar->config.enable_fan = config.enable_fan;
p_lidar->config.return_mode = config.return_mode;
p_lidar->config.coordinate = config.coordinate;
p_lidar->config.imu_rate = config.imu_rate;
p_lidar->config.extrinsic_parameter_source = config.extrinsic_parameter_source;
} else {
printf("Add lidar to connect is failed : %d %d \n", result, handle);
}
}
/** Callback function of changing of device state. */
void LdsLidar::OnDeviceChange(const DeviceInfo *info, DeviceEvent type) {
if (info == nullptr) {
return;
}
uint8_t handle = info->handle;
if (handle >= kMaxLidarCount) {
return;
}
LidarDevice* p_lidar = &(g_lds_ldiar->lidars_[handle]);
if (type == kEventConnect) {
QueryDeviceInformation(handle, DeviceInformationCb, g_lds_ldiar);
if (p_lidar->connect_state == kConnectStateOff) {
p_lidar->connect_state = kConnectStateOn;
p_lidar->info = *info;
}
} else if (type == kEventDisconnect) {
printf("Lidar[%s] disconnect!\n", info->broadcast_code);
ResetLidar(p_lidar, kSourceRawLidar);
} else if (type == kEventStateChange) {
p_lidar->info = *info;
}
if (p_lidar->connect_state == kConnectStateOn) {
printf("Lidar[%s] status_code[%d] working state[%d] feature[%d]\n", \
p_lidar->info.broadcast_code,\
p_lidar->info.status.status_code.error_code,\
p_lidar->info.state,\
p_lidar->info.feature);
SetErrorMessageCallback(handle, LidarErrorStatusCb);
/** Config lidar parameter */
if (p_lidar->info.state == kLidarStateNormal) {
if (p_lidar->config.coordinate != 0) {
SetSphericalCoordinate(handle, SetCoordinateCb, g_lds_ldiar);
} else {
SetCartesianCoordinate(handle, SetCoordinateCb, g_lds_ldiar);
}
p_lidar->config.set_bits |= kConfigCoordinate;
if (kDeviceTypeLidarMid40 != info->type) {
LidarSetPointCloudReturnMode(handle, (PointCloudReturnMode)(p_lidar->config.return_mode),\
SetPointCloudReturnModeCb, g_lds_ldiar);
p_lidar->config.set_bits |= kConfigReturnMode;
}
if (kDeviceTypeLidarMid40 != info->type) {
LidarSetImuPushFrequency(handle, (ImuFreq)(p_lidar->config.imu_rate),\
SetImuRatePushFrequencyCb, g_lds_ldiar);
p_lidar->config.set_bits |= kConfigImuRate;
}
if (p_lidar->config.extrinsic_parameter_source == kExtrinsicParameterFromLidar) {
LidarGetExtrinsicParameter(handle, GetLidarExtrinsicParameterCb, g_lds_ldiar);
}
p_lidar->connect_state = kConnectStateConfig;
}
}
}
/** Query the firmware version of Livox LiDAR. */
void LdsLidar::DeviceInformationCb(livox_status status, uint8_t handle, \
DeviceInformationResponse *ack, void *clent_data) {
if (status != kStatusSuccess) {
printf("Device Query Informations Failed : %d\n", status);
}
if (ack) {
printf("firmware version: %d.%d.%d.%d\n",
ack->firmware_version[0],
ack->firmware_version[1],
ack->firmware_version[2],
ack->firmware_version[3]);
}
}
/** Callback function of Lidar error message. */
void LdsLidar::LidarErrorStatusCb(livox_status status, uint8_t handle, ErrorMessage *message) {
static uint32_t error_message_count = 0;
if (message != NULL) {
++error_message_count;
if (0 == (error_message_count % 100)) {
printf("handle: %u\n", handle);
printf("temp_status : %u\n", message->lidar_error_code.temp_status);
printf("volt_status : %u\n", message->lidar_error_code.volt_status);
printf("motor_status : %u\n", message->lidar_error_code.motor_status);
printf("dirty_warn : %u\n", message->lidar_error_code.dirty_warn);
printf("firmware_err : %u\n", message->lidar_error_code.firmware_err);
printf("pps_status : %u\n", message->lidar_error_code.device_status);
printf("fan_status : %u\n", message->lidar_error_code.fan_status);
printf("self_heating : %u\n", message->lidar_error_code.self_heating);
printf("ptp_status : %u\n", message->lidar_error_code.ptp_status);
printf("time_sync_status : %u\n", message->lidar_error_code.time_sync_status);
printf("system_status : %u\n", message->lidar_error_code.system_status);
}
}
}
void LdsLidar::ControlFanCb(livox_status status, uint8_t handle, \
uint8_t response, void *clent_data) {
}
void LdsLidar::SetPointCloudReturnModeCb(livox_status status, uint8_t handle, \
uint8_t response, void *clent_data) {
LdsLidar* lds_lidar = static_cast<LdsLidar *>(clent_data);
if (handle >= kMaxLidarCount) {
return;
}
LidarDevice* p_lidar = &(lds_lidar->lidars_[handle]);
if (status == kStatusSuccess) {
p_lidar->config.set_bits &= ~((uint32_t)(kConfigReturnMode));
printf("Set return mode success!\n");
if (!p_lidar->config.set_bits) {
LidarStartSampling(handle, StartSampleCb, lds_lidar);
p_lidar->connect_state = kConnectStateSampling;
}
} else {
LidarSetPointCloudReturnMode(handle, (PointCloudReturnMode)(p_lidar->config.return_mode),\
SetPointCloudReturnModeCb, lds_lidar);
printf("Set return mode fail, try again!\n");
}
}
void LdsLidar::SetCoordinateCb(livox_status status, uint8_t handle, \
uint8_t response, void *clent_data) {
LdsLidar* lds_lidar = static_cast<LdsLidar *>(clent_data);
if (handle >= kMaxLidarCount) {
return;
}
LidarDevice* p_lidar = &(lds_lidar->lidars_[handle]);
if (status == kStatusSuccess) {
p_lidar->config.set_bits &= ~((uint32_t)(kConfigCoordinate));
printf("Set coordinate success!\n");
if (!p_lidar->config.set_bits) {
LidarStartSampling(handle, StartSampleCb, lds_lidar);
p_lidar->connect_state = kConnectStateSampling;
}
} else {
if (p_lidar->config.coordinate != 0) {
SetSphericalCoordinate(handle, SetCoordinateCb, lds_lidar);
} else {
SetCartesianCoordinate(handle, SetCoordinateCb, lds_lidar);
}
printf("Set coordinate fail, try again!\n");
}
}
void LdsLidar::SetImuRatePushFrequencyCb(livox_status status, uint8_t handle, \
uint8_t response, void *clent_data) {
LdsLidar* lds_lidar = static_cast<LdsLidar *>(clent_data);
if (handle >= kMaxLidarCount) {
return;
}
LidarDevice* p_lidar = &(lds_lidar->lidars_[handle]);
if (status == kStatusSuccess) {
p_lidar->config.set_bits &= ~((uint32_t)(kConfigImuRate));
printf("Set imu rate success!\n");
if (!p_lidar->config.set_bits) {
LidarStartSampling(handle, StartSampleCb, lds_lidar);
p_lidar->connect_state = kConnectStateSampling;
}
} else {
LidarSetImuPushFrequency(handle, (ImuFreq)(p_lidar->config.imu_rate),\
SetImuRatePushFrequencyCb, g_lds_ldiar);
printf("Set imu rate fail, try again!\n");
}
}
/** Callback function of get LiDARs' extrinsic parameter. */
void LdsLidar::GetLidarExtrinsicParameterCb(livox_status status, uint8_t handle, \
LidarGetExtrinsicParameterResponse *response, void *clent_data) {
LdsLidar* lds_lidar = static_cast<LdsLidar *>(clent_data);
if (handle >= kMaxLidarCount) {
return;
}
if (status == kStatusSuccess) {
if (response != nullptr) {
printf("Lidar[%d] get ExtrinsicParameter status[%d] response[%d]\n", \
handle, status, response->ret_code);
LidarDevice* p_lidar = &(lds_lidar->lidars_[handle]);
ExtrinsicParameter* p_extrinsic = &p_lidar->extrinsic_parameter;
p_extrinsic->euler[0] = static_cast<float>(response->roll* PI / 180.0);
p_extrinsic->euler[1] = static_cast<float>(response->pitch * PI / 180.0);
p_extrinsic->euler[2] = static_cast<float>(response->yaw * PI / 180.0);
p_extrinsic->trans[0] = static_cast<float>(response->x / 1000.0);
p_extrinsic->trans[1] = static_cast<float>(response->y / 1000.0);
p_extrinsic->trans[2] = static_cast<float>(response->z / 1000.0);
EulerAnglesToRotationMatrix(p_extrinsic->euler, p_extrinsic->rotation);
if (p_lidar->config.extrinsic_parameter_source) {
p_extrinsic->enable = true;
}
p_lidar->config.set_bits &= ~((uint32_t)(kConfigGetExtrinsicParameter));
printf("Lidar[%d] get ExtrinsicParameter success!\n", handle);
if (!p_lidar->config.set_bits) {
LidarStartSampling(handle, StartSampleCb, lds_lidar);
p_lidar->connect_state = kConnectStateSampling;
}
} else {
printf("Lidar[%d] get ExtrinsicParameter fail!\n", handle);
}
} else if (status == kStatusTimeout) {
printf("Lidar[%d] get ExtrinsicParameter timeout!\n", handle);
}
}
/** Callback function of starting sampling. */
void LdsLidar::StartSampleCb(livox_status status, uint8_t handle, \
uint8_t response, void *clent_data) {
LdsLidar* lds_lidar = static_cast<LdsLidar *>(clent_data);
if (handle >= kMaxLidarCount) {
return;
}
LidarDevice* p_lidar = &(lds_lidar->lidars_[handle]);
if (status == kStatusSuccess) {
if (response != 0) {
p_lidar->connect_state = kConnectStateOn;
printf("Lidar start sample fail : state[%d] handle[%d] res[%d]\n", \
status, handle, response);
} else {
printf("Lidar start sample success\n");
}
} else if (status == kStatusTimeout) {
p_lidar->connect_state = kConnectStateOn;
printf("Lidar start sample timeout : state[%d] handle[%d] res[%d]\n", \
status, handle, response);
}
}
/** Callback function of stopping sampling. */
void LdsLidar::StopSampleCb(livox_status status, uint8_t handle, \
uint8_t response, void *clent_data) {
}
void LdsLidar::SetRmcSyncTimeCb(livox_status status, uint8_t handle, \
uint8_t response, void* client_data) {
if (handle >= kMaxLidarCount) {
return;
}
printf("Set lidar[%d] sync time status[%d] response[%d]\n", handle, status, response);
}
void LdsLidar::ReceiveSyncTimeCallback(const char* rmc, uint32_t rmc_length, void* client_data) {
LdsLidar* lds_lidar = static_cast<LdsLidar *>(client_data);
//std::unique_lock<std::mutex> lock(mtx);
LidarDevice* p_lidar = nullptr;
for (uint8_t handle = 0; handle < kMaxLidarCount; handle++) {
p_lidar = &(lds_lidar->lidars_[handle]);
if (p_lidar->connect_state == kConnectStateSampling && \
p_lidar->info.state == kLidarStateNormal) {
livox_status status = LidarSetRmcSyncTime(handle, rmc, rmc_length, \
SetRmcSyncTimeCb, lds_lidar);
if (status != kStatusSuccess) {
printf("Set GPRMC synchronization time error code: %d.\n",status);
}
}
}
}
/** Add broadcast code to whitelist */
int LdsLidar::AddBroadcastCodeToWhitelist(const char* broadcast_code) {
if (!broadcast_code || (strlen(broadcast_code) > kBroadcastCodeSize) || \
(whitelist_count_ >= kMaxLidarCount)) {
return -1;
}
if (LdsLidar::IsBroadcastCodeExistInWhitelist(broadcast_code)) {
printf("%s is alrealy exist!\n", broadcast_code);
return -1;
}
strcpy(broadcast_code_whitelist_[whitelist_count_], broadcast_code);
++whitelist_count_;
return 0;
}
bool LdsLidar::IsBroadcastCodeExistInWhitelist(const char* broadcast_code) {
if (!broadcast_code) {
return false;
}
for (uint32_t i=0; i<whitelist_count_; i++) {
if (strncmp(broadcast_code, broadcast_code_whitelist_[i], kBroadcastCodeSize) == 0) {
return true;
}
}
return false;
}
int LdsLidar::ParseTimesyncConfig(rapidjson::Document& doc) {
do {
if (!doc.HasMember("timesync_config") || !doc["timesync_config"].IsObject()) break;
const rapidjson::Value& object = doc["timesync_config"];
if (!object.IsObject()) break;
if(!object.HasMember("enable_timesync") || !object["enable_timesync"].IsBool()) break;
enable_timesync_ = object["enable_timesync"].GetBool();
if(!object.HasMember("device_name") || !object["device_name"].IsString()) break;
std::string device_name = object["device_name"].GetString();
std::strncpy(timesync_config_.dev_config.name, device_name.c_str(),\
sizeof(timesync_config_.dev_config.name));
if(!object.HasMember("comm_device_type") || !object["comm_device_type"].IsInt()) break;
timesync_config_.dev_config.type = object["comm_device_type"].GetInt();
if (timesync_config_.dev_config.type == kCommDevUart) {
if(!object.HasMember("baudrate_index") || !object["baudrate_index"].IsInt()) break;
timesync_config_.dev_config.config.uart.baudrate = object["baudrate_index"].GetInt();
if(!object.HasMember("parity_index") || !object["parity_index"].IsInt()) break;
timesync_config_.dev_config.config.uart.parity = object["parity_index"].GetInt();
}
if (enable_timesync_) {
printf("Enable timesync : \n");
if (timesync_config_.dev_config.type == kCommDevUart) {
printf("Uart[%s],baudrate index[%d],parity index[%d]\n", timesync_config_.dev_config.name,\
timesync_config_.dev_config.config.uart.baudrate,\
timesync_config_.dev_config.config.uart.parity);
}
} else {
printf("Disable timesync\n");
}
return 0;
}while(0);
return -1;
}
/** Config file process */
int LdsLidar::ParseConfigFile(const char* pathname) {
FILE* raw_file = std::fopen(pathname, "rb");
if(!raw_file) {
printf("Open json config file fail!\n");
return -1;
}
char read_buffer[32768];
rapidjson::FileReadStream config_file(raw_file, read_buffer, sizeof(read_buffer));
rapidjson::Document doc;
if(!doc.ParseStream(config_file).HasParseError()) {
if(doc.HasMember("lidar_config") && doc["lidar_config"].IsArray()) {
const rapidjson::Value& array = doc["lidar_config"];
size_t len = array.Size();
for(size_t i=0; i<len; i++) {
const rapidjson::Value& object = array[i];
if(object.IsObject()) {
UserRawConfig config;
memset(&config, 0, sizeof(config));
if(object.HasMember("broadcast_code") && object["broadcast_code"].IsString()) {
std::string broadcast_code = object["broadcast_code"].GetString();
std::strncpy(config.broadcast_code, broadcast_code.c_str(),\
sizeof(config.broadcast_code));
} else {
printf("User config file parse error\n");
continue;
}
if(object.HasMember("enable_connect") && object["enable_connect"].IsBool()) {
config.enable_connect = object["enable_connect"].GetBool();
}
if(object.HasMember("enable_fan") && object["enable_fan"].IsBool()) {
config.enable_fan = object["enable_fan"].GetBool();
}
if(object.HasMember("return_mode") && object["return_mode"].IsInt()) {
config.return_mode = object["return_mode"].GetInt();
}
if(object.HasMember("coordinate") && object["coordinate"].IsInt()) {
config.coordinate = object["coordinate"].GetInt();
}
if(object.HasMember("imu_rate") && object["imu_rate"].IsInt()) {
config.imu_rate = object["imu_rate"].GetInt();
}
if(object.HasMember("extrinsic_parameter_source") && \
object["extrinsic_parameter_source"].IsInt()) {
config.extrinsic_parameter_source = object["extrinsic_parameter_source"].GetInt();
}
printf("broadcast code[%s] : %d %d %d %d %d %d\n", config.broadcast_code, \
config.enable_connect, config.enable_fan, config.return_mode,\
config.coordinate, config.imu_rate, config.extrinsic_parameter_source);
if (config.enable_connect) {
if (!AddBroadcastCodeToWhitelist(config.broadcast_code)) {
if (AddRawUserConfig(config)) {
printf("Raw config is already exist : %s \n", config.broadcast_code);
}
}
}
}
}
}
if (ParseTimesyncConfig(doc)) {
printf("Parse timesync config fail\n");
enable_timesync_ = false;
}
} else {
printf("User config file parse error[%d]\n", doc.ParseStream(config_file).HasParseError());
}
std::fclose(raw_file);
return 0;
}
int LdsLidar::AddRawUserConfig(UserRawConfig& config) {
if (IsExistInRawConfig(config.broadcast_code)) {
return -1;
}
raw_config_.push_back(config);
printf("Add Raw user config : %s \n", config.broadcast_code);
return 0;
}
bool LdsLidar::IsExistInRawConfig(const char* broadcast_code) {
if (broadcast_code == nullptr) {
return false;
}
for (auto ite_config : raw_config_) {
if (strncmp(ite_config.broadcast_code, broadcast_code, kBroadcastCodeSize) == 0) {
return true;
}
}
return false;
}
int LdsLidar::GetRawConfig(const char* broadcast_code, UserRawConfig& config) {
if (broadcast_code == nullptr) {
return -1;
}
for (auto ite_config : raw_config_) {
if (strncmp(ite_config.broadcast_code, broadcast_code, kBroadcastCodeSize) == 0) {
config.enable_fan = ite_config.enable_fan;
config.return_mode = ite_config.return_mode;
config.coordinate = ite_config.coordinate;
config.imu_rate = ite_config.imu_rate;
config.extrinsic_parameter_source = ite_config.extrinsic_parameter_source;
return 0;
}
}
return -1;
}
}

109
livox_ros_driver/livox_ros_driver/lds_lidar.h Normal file
View File

@@ -0,0 +1,109 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2019 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
/** Livox LiDAR data source, data from dependent lidar */
#ifndef LIVOX_ROS_DRIVER_LDS_LIDAR_H_
#define LIVOX_ROS_DRIVER_LDS_LIDAR_H_
#include <memory>
#include <vector>
#include "lds.h"
#include "livox_sdk.h"
#include "timesync.h"
#include "rapidjson/document.h"
namespace livox_ros {
/**
* LiDAR data source, data from dependent lidar.
*/
class LdsLidar : public Lds {
public:
static LdsLidar* GetInstance(uint32_t interval_ms) {
static LdsLidar lds_lidar(interval_ms);
return &lds_lidar;
}
int InitLdsLidar(std::vector<std::string>& broadcast_code_strs, const char *user_config_path);
int DeInitLdsLidar(void);
private:
LdsLidar(uint32_t interval_ms);
LdsLidar(const LdsLidar&) = delete;
~LdsLidar();
LdsLidar& operator=(const LdsLidar&) = delete;
virtual void PrepareExit(void);
static void OnLidarDataCb(uint8_t handle, LivoxEthPacket *data,\
uint32_t data_num, void *client_data);
static void OnDeviceBroadcast(const BroadcastDeviceInfo *info);
static void OnDeviceChange(const DeviceInfo *info, DeviceEvent type);
static void StartSampleCb(livox_status status, uint8_t handle, uint8_t response, void *clent_data);
static void StopSampleCb(livox_status status, uint8_t handle, uint8_t response, void *clent_data);
static void DeviceInformationCb(livox_status status, uint8_t handle, \
DeviceInformationResponse *ack, void *clent_data);
static void LidarErrorStatusCb(livox_status status, uint8_t handle, ErrorMessage *message);
static void ControlFanCb(livox_status status, uint8_t handle, \
uint8_t response, void *clent_data);
static void SetPointCloudReturnModeCb(livox_status status, uint8_t handle, \
uint8_t response, void *clent_data);
static void SetCoordinateCb(livox_status status, uint8_t handle, \
uint8_t response, void *clent_data);
static void SetImuRatePushFrequencyCb(livox_status status, uint8_t handle, \
uint8_t response, void *clent_data);
static void SetRmcSyncTimeCb(livox_status status, uint8_t handle, \
uint8_t response, void* client_data);
static void ReceiveSyncTimeCallback(const char* rmc, uint32_t rmc_length, void* client_data);
static void GetLidarExtrinsicParameterCb(livox_status status, uint8_t handle, \
LidarGetExtrinsicParameterResponse *response, void *clent_data);
void ResetLdsLidar(void);
int AddBroadcastCodeToWhitelist(const char* broadcast_code);
bool IsBroadcastCodeExistInWhitelist(const char* broadcast_code);
void EnableAutoConnectMode(void) { auto_connect_mode_ = true; }
void DisableAutoConnectMode(void) { auto_connect_mode_ = false; }
bool IsAutoConnectMode(void) { return auto_connect_mode_; }
int ParseTimesyncConfig(rapidjson::Document& doc);
int ParseConfigFile(const char* pathname);
int AddRawUserConfig(UserRawConfig& config);
bool IsExistInRawConfig(const char* broadcast_code);
int GetRawConfig(const char* broadcast_code, UserRawConfig& config);
bool auto_connect_mode_;
uint32_t whitelist_count_;
volatile bool is_initialized_;
char broadcast_code_whitelist_[kMaxLidarCount][kBroadcastCodeSize];
std::vector<UserRawConfig> raw_config_;
bool enable_timesync_;
TimeSync *timesync_;
TimeSyncConfig timesync_config_;
};
}
#endif

207
livox_ros_driver/livox_ros_driver/lds_lvx.cpp Normal file
View File

@@ -0,0 +1,207 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2019 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include "lds_lvx.h"
#include <stdio.h>
#include <string.h>
#include <thread>
#include <memory>
#include <functional>
#include "lvx_file.h"
namespace livox_ros {
/** Const varible -------------------------------------------------------------------------------- */
const uint32_t kMaxPacketsNumOfFrame = 8192;
/** For device connect use ---------------------------------------------------------------------- */
LdsLvx::LdsLvx(uint32_t interval_ms) : Lds(interval_ms, kSourceLvxFile) {
start_read_lvx_ = false;
is_initialized_ = false;
lvx_file_ = std::make_shared<LvxFileHandle>();
packets_of_frame_.buffer_capacity = kMaxPacketsNumOfFrame * sizeof(LvxFilePacket);
packets_of_frame_.packet = new uint8_t[kMaxPacketsNumOfFrame * sizeof(LvxFilePacket)];
}
LdsLvx::~LdsLvx() {
if (packets_of_frame_.packet != nullptr) {
delete [] packets_of_frame_.packet;
}
}
void LdsLvx::PrepareExit(void) {
lvx_file_->CloseLvxFile();
printf("Lvx to rosbag convert complete and exit!\n");
}
int LdsLvx::InitLdsLvx(const char* lvx_path) {
if (is_initialized_) {
printf("Livox file data source is already inited!\n");
return -1;
}
int ret = lvx_file_->Open(lvx_path, std::ios::in);
if (ret) {
printf("Open %s file fail[%d]!\n", lvx_path, ret);
return ret;
}
if (lvx_file_->GetFileVersion() == kLvxFileV1) {
ResetLds(kSourceRawLidar);
} else {
ResetLds(kSourceLvxFile);
}
lidar_count_ = lvx_file_->GetDeviceCount();
if (!lidar_count_ || (lidar_count_ >= kMaxSourceLidar)) {
lvx_file_->CloseLvxFile();
printf("Lidar count error in %s : %d\n", lvx_path, lidar_count_);
return -1;
}
printf("LvxFile[%s] have %d lidars\n", lvx_path, lidar_count_);
for (int i=0; i<lidar_count_; i++) {
LvxFileDeviceInfo lvx_dev_info;
lvx_file_->GetDeviceInfo(i, &lvx_dev_info);
lidars_[i].handle = i;
lidars_[i].connect_state = kConnectStateSampling;
lidars_[i].info.handle = i;
lidars_[i].info.type = lvx_dev_info.device_type;
memcpy(lidars_[i].info.broadcast_code, lvx_dev_info.lidar_broadcast_code,\
sizeof(lidars_[i].info.broadcast_code));
if (lvx_file_->GetFileVersion() == kLvxFileV1) {
lidars_[i].data_src = kSourceRawLidar;
} else {
lidars_[i].data_src = kSourceLvxFile;
}
ExtrinsicParameter* p_extrinsic = &lidars_[i].extrinsic_parameter;
p_extrinsic->euler[0] = lvx_dev_info.roll* PI / 180.0;
p_extrinsic->euler[1] = lvx_dev_info.pitch * PI / 180.0;
p_extrinsic->euler[2] = lvx_dev_info.yaw * PI / 180.0;
p_extrinsic->trans[0] = lvx_dev_info.x;
p_extrinsic->trans[1] = lvx_dev_info.y;
p_extrinsic->trans[2] = lvx_dev_info.z;
EulerAnglesToRotationMatrix(p_extrinsic->euler, p_extrinsic->rotation);
p_extrinsic->enable = lvx_dev_info.extrinsic_enable;
uint32_t queue_size = kMaxEthPacketQueueSize * 16;
InitQueue(&lidars_[i].data, queue_size);
queue_size = kMaxEthPacketQueueSize;
InitQueue(&lidars_[i].imu_data, queue_size);
}
t_read_lvx_ = std::make_shared<std::thread>(std::bind(&LdsLvx::ReadLvxFile, this));
is_initialized_ = true;
StartRead();
return ret;
}
/** Global function in LdsLvx for callback */
void LdsLvx::ReadLvxFile() {
while (!start_read_lvx_);
printf("Start to read lvx file.\n");
int file_state = kLvxFileOk;
int progress = 0;
while (start_read_lvx_) {
file_state = lvx_file_->GetPacketsOfFrame(&packets_of_frame_);
if (!file_state) {
uint32_t data_size = packets_of_frame_.data_size;
uint8_t* packet_base = packets_of_frame_.packet;
uint32_t data_offset = 0;
while (data_offset < data_size) {
LivoxEthPacket* eth_packet;
int32_t handle;
uint8_t data_type;
if (lvx_file_->GetFileVersion()) {
LvxFilePacket* detail_packet = (LvxFilePacket*)&packet_base[data_offset];
eth_packet = (LivoxEthPacket*)(&detail_packet->version);
handle = detail_packet->device_index;
} else {
LvxFilePacketV0* detail_packet = (LvxFilePacketV0*)&packet_base[data_offset];
eth_packet = (LivoxEthPacket*)(&detail_packet->version);
handle = detail_packet->device_index;
}
data_type = eth_packet->data_type;
data_offset += (GetEthPacketLen(data_type) + 1); /* packet length + device index */
if (data_type != kImu) {
LidarDataQueue* p_queue = &lidars_[handle].data;
if ((p_queue != nullptr) && (handle < lidar_count_)) {
while(QueueIsFull(p_queue)) {
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
QueuePushAny(p_queue, (uint8_t *)eth_packet, GetEthPacketLen(data_type),
0, GetPointsPerPacket(data_type));
}
} else {
LidarDataQueue* p_queue = &lidars_[handle].imu_data;
if ((p_queue != nullptr) && (handle < lidar_count_)) {
while(QueueIsFull(p_queue)) {
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
QueuePushAny(p_queue, (uint8_t *)eth_packet, GetEthPacketLen(data_type),
0, GetPointsPerPacket(data_type));
}
}
}
} else {
if (file_state != kLvxFileAtEnd) {
printf("Exit read the lvx file, read file state[%d]!\n", file_state);
} else {
printf("Read the lvx file complete!\n");
}
break;
}
if (progress != lvx_file_->GetLvxFileReadProgress()) {
progress = lvx_file_->GetLvxFileReadProgress();
printf("Read progress : %d \n", progress);
}
}
while(IsAllQueueEmpty()) {
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
RequestExit();
}
bool LdsLvx::IsAllQueueEmpty() {
for (int i=0; i<lidar_count_; i++) {
LidarDevice* p_lidar = &lidars_[i];
if (!QueueIsEmpty(&p_lidar->data)) {
return false;
}
}
return true;
}
}

73
livox_ros_driver/livox_ros_driver/lds_lvx.h Normal file
View File

@@ -0,0 +1,73 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2019 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
// livox lidar lvx data source
#ifndef LIVOX_ROS_DRIVER_LDS_LVX_H_
#define LIVOX_ROS_DRIVER_LDS_LVX_H_
#include <memory>
#include <thread>
#include "lds.h"
#include "lvx_file.h"
namespace livox_ros {
/**
* Lidar data source abstract.
*/
class LdsLvx : public Lds {
public:
static LdsLvx* GetInstance(uint32_t interval_ms) {
static LdsLvx lds_lvx(interval_ms);
return &lds_lvx;
}
int InitLdsLvx(const char* lvx_path);
int DeInitLdsLvx(void);
void PrepareExit(void);
private:
LdsLvx(uint32_t interval_ms);
LdsLvx(const LdsLvx&) = delete;
~LdsLvx();
LdsLvx& operator=(const LdsLvx&) = delete;
void StartRead() { start_read_lvx_ = true; }
void StopRead() { start_read_lvx_ = false; }
bool IsStarted() { return start_read_lvx_; }
void ReadLvxFile();
bool IsAllQueueEmpty();
volatile bool is_initialized_;
OutPacketBuffer packets_of_frame_;
std::shared_ptr<LvxFileHandle> lvx_file_;
std::shared_ptr<std::thread> t_read_lvx_;
volatile bool start_read_lvx_;
};
}
#endif

160
livox_ros_driver/livox_ros_driver/livox_ros_driver.cpp Normal file
View File

@@ -0,0 +1,160 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2019 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include <vector>
#include <chrono>
#include "livox_sdk.h"
#include <ros/ros.h>
#include "lds_lvx.h"
#include "lds_lidar.h"
#include "lds_hub.h"
#include "lddc.h"
#include "include/livox_ros_driver.h"
using namespace livox_ros;
const int32_t kSdkVersionMajorLimit = 2;
int main(int argc, char **argv) {
ROS_INFO("Livox Ros Driver Version: %s", LIVOX_ROS_DRIVER_VERSION_STRING);
/** Ros related */
if(ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)) {
ros::console::notifyLoggerLevelsChanged();
}
ros::init(argc, argv, "livox_lidar_publisher");
ros::NodeHandle livox_node;
/** Check sdk version */
LivoxSdkVersion _sdkversion;
GetLivoxSdkVersion(&_sdkversion);
if (_sdkversion.major < kSdkVersionMajorLimit) {
ROS_INFO("The SDK version[%d.%d.%d] is too low", \
_sdkversion.major, _sdkversion.minor, _sdkversion.patch);
return 0;
}
/** Init defualt system parameter */
int xfer_format = kPointCloud2Msg;
int multi_topic = 0;
int data_src = kSourceRawLidar;
double publish_freq = 50.0; /* Hz */
int output_type = kOutputToRos;
livox_node.getParam("xfer_format", xfer_format);
livox_node.getParam("multi_topic", multi_topic);
livox_node.getParam("data_src", data_src);
livox_node.getParam("publish_freq", publish_freq);
livox_node.getParam("output_data_type", output_type);
/** Lidar data distribute control and lidar data source set */
Lddc* lddc = new Lddc(xfer_format, multi_topic, data_src, output_type, publish_freq);
lddc->SetRosNode(&livox_node);
int ret = 0;
if (data_src == kSourceRawLidar) {
ROS_INFO("Data Source is raw lidar.");
std::string user_config_path;
livox_node.getParam("user_config_path", user_config_path);
ROS_INFO("Config file : %s", user_config_path.c_str());
std::string cmdline_bd_code;
livox_node.getParam("cmdline_str", cmdline_bd_code);
std::vector<std::string> bd_code_list;
ParseCommandlineInputBdCode(cmdline_bd_code.c_str(), bd_code_list);
LdsLidar* read_lidar = LdsLidar::GetInstance(1000/publish_freq);
lddc->RegisterLds(static_cast<Lds *>(read_lidar));
ret = read_lidar->InitLdsLidar(bd_code_list, user_config_path.c_str());
if (!ret) {
ROS_INFO("Init lds lidar success!");
} else {
ROS_ERROR("Init lds lidar fail!");
}
} else if (data_src == kSourceRawHub) {
ROS_INFO("Data Source is hub.");
std::string user_config_path;
livox_node.getParam("user_config_path", user_config_path);
ROS_INFO("Config file : %s", user_config_path.c_str());
std::string cmdline_bd_code;
livox_node.getParam("cmdline_str", cmdline_bd_code);
std::vector<std::string> bd_code_list;
ParseCommandlineInputBdCode(cmdline_bd_code.c_str(), bd_code_list);
LdsHub* read_hub = LdsHub::GetInstance(1000/publish_freq);
lddc->RegisterLds(static_cast<Lds *>(read_hub));
ret = read_hub->InitLdsHub(bd_code_list, user_config_path.c_str());
if (!ret) {
ROS_INFO("Init lds hub success!");
} else {
ROS_ERROR("Init lds hub fail!");
}
} else {
ROS_INFO("Data Source is lvx file.");
std::string cmdline_file_path;
livox_node.getParam("cmdline_file_path", cmdline_file_path);
do {
if (!IsFilePathValid(cmdline_file_path.c_str())) {
ROS_ERROR("File path invalid : %s !", cmdline_file_path.c_str());
break;
}
std::string rosbag_file_path;
int path_end_pos = cmdline_file_path.find_last_of('.');
rosbag_file_path = cmdline_file_path.substr(0, path_end_pos);
rosbag_file_path += ".bag";
LdsLvx* read_lvx = LdsLvx::GetInstance(1000/publish_freq);
lddc->RegisterLds(static_cast<Lds *>(read_lvx));
lddc->CreateBagFile(rosbag_file_path);
int ret = read_lvx->InitLdsLvx(cmdline_file_path.c_str());
if (!ret) {
ROS_INFO("Init lds lvx file success!");
} else {
ROS_ERROR("Init lds lvx file fail!");
}
} while(0);
}
ros::Time::init();
ros::Rate r(publish_freq);
while (ros::ok()) {
lddc->DistributeLidarData();
r.sleep();
}
return 0;
}

399
livox_ros_driver/livox_ros_driver/lvx_file.cpp Normal file
View File

@@ -0,0 +1,399 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2019 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include "lvx_file.h"
#include <time.h>
#include <string.h>
#include <cmath>
#include "rapidxml/rapidxml.hpp"
#include "rapidxml/rapidxml_utils.hpp"
#include "lds.h"
namespace livox_ros {
#define M_PI 3.14159265358979323846
const uint32_t kMaxLvxFileHeaderLength = 16 * 1024;
const char* kLvxHeaderSigStr = "livox_tech";
const uint32_t kLvxHeaderMagicCode = 0xac0ea767;
LvxFileHandle::LvxFileHandle() : file_ver_(kLvxFileV1), device_count_(0), cur_frame_index_(0),
cur_offset_(0), data_start_offset_(0), size_(0), mode_(0), state_(0) {
memset((void *)&public_header_, 0, sizeof(public_header_));
memset((void *)&private_header_, 0, sizeof(private_header_));
memset((void *)&private_header_v0_, 0, sizeof(private_header_v0_));
}
bool LvxFileHandle::ReadAndCheckHeader() {
lvx_file_.seekg(0, std::ios::beg);
lvx_file_.read((char *)(&public_header_), sizeof(public_header_));
if (strcmp((const char *)public_header_.signature, kLvxHeaderSigStr)) {
return false;
}
/**
if (public_header_.magic_code != kLvxHeaderMagicCode) {
return false;
}
*/
if (public_header_.version[1] > kLvxFileV1) {
printf("Unkown lvx file version[%d.%d.%d.%d]\n", public_header_.version[0], \
public_header_.version[1], public_header_.version[2], public_header_.version[3]);
return false;
}
file_ver_ = public_header_.version[1];
printf("Livox file version[%d]\n", file_ver_);
return true;
}
uint64_t LvxFileHandle::MiniFileSize() {
if (file_ver_ == kLvxFileV1) {
return (sizeof(LvxFilePublicHeader) + sizeof(LvxFilePrivateHeader) + \
sizeof(LvxFileDeviceInfo) + sizeof(FrameHeader) + sizeof(LvxFilePacket));
} else {
return (sizeof(LvxFilePublicHeader) + sizeof(LvxFilePrivateHeaderV0) + \
sizeof(LvxFileDeviceInfoV0) + sizeof(FrameHeaderV0) + sizeof(LvxFilePacketV0));
}
}
uint64_t LvxFileHandle::PrivateHeaderOffset() {
return sizeof(LvxFilePublicHeader);
}
uint64_t LvxFileHandle::DataStartOffset() {
if (file_ver_ == kLvxFileV1) {
return (sizeof(LvxFilePublicHeader) + sizeof(LvxFilePrivateHeader) + \
sizeof(LvxFileDeviceInfo) * private_header_.device_count);
} else {
return (sizeof(LvxFilePublicHeader) + sizeof(LvxFilePrivateHeaderV0) + \
sizeof(LvxFileDeviceInfoV0) * private_header_v0_.device_count);
}
}
bool LvxFileHandle::AddAndCheckDeviceInfo() {
lvx_file_.seekg (PrivateHeaderOffset(), std::ios::beg);
if (file_ver_ == kLvxFileV1) {
lvx_file_.read((char *)(&private_header_), sizeof(private_header_));
device_count_ = private_header_.device_count;
} else {
lvx_file_.read((char *)(&private_header_v0_), sizeof(private_header_v0_));
device_count_ = private_header_v0_.device_count;
}
if (!device_count_) {
return false;
}
for (int i=0; i< device_count_; i++) {
LvxFileDeviceInfo device_info;
if (file_ver_ == kLvxFileV1) {
lvx_file_.read((char *)(&device_info), sizeof(LvxFileDeviceInfo));
} else { /* device info v0 to v1 */
LvxFileDeviceInfoV0 device_info_v0;
lvx_file_.read((char *)(&device_info_v0), sizeof(LvxFileDeviceInfoV0));
memcpy((void *)&device_info, (void *)&device_info_v0, \
&device_info.extrinsic_enable - device_info.lidar_broadcast_code);
memcpy((void *)&device_info.roll, (void *)&device_info_v0.roll, \
sizeof(float) * 6);
device_info.extrinsic_enable = 0;
}
AddDeviceInfo(device_info);
}
return true;
}
bool LvxFileHandle::PrepareDataRead() {
lvx_file_.seekg (DataStartOffset(), std::ios::beg);
FrameHeader frame_header; /* v0&v1 compatible */
lvx_file_.read((char *)(&frame_header), sizeof(frame_header));
if ((frame_header.current_offset != DataStartOffset()) ||\
(frame_header.frame_index != 0)) {
return false;
}
/** reset the read position to the start offset of data erea */
lvx_file_.seekg (DataStartOffset(), std::ios::beg);
return true;
}
int LvxFileHandle::Open(const char* filename,std::ios_base::openmode mode) {
if ((mode & std::ios::in) == std::ios::in) {
state_ = kLvxFileOk;
lvx_file_.open(filename, mode | std::ios_base::binary | std::ios_base::ate);
if (!lvx_file_.is_open()) {
state_ = kLvxFileNotExist;
return state_;
}
size_ = lvx_file_.tellg();
lvx_file_.seekg (0, std::ios::beg);
if (size_ < MiniFileSize()) {
state_ = kLvxFileSizeFault;
return state_;
}
if (!ReadAndCheckHeader()) {
state_ = kLvxFileHeaderFault;
return state_;
}
if (!AddAndCheckDeviceInfo()) {
state_ = kLvxFileDeviceInfoFault;
return state_;
}
if (!PrepareDataRead()) {
state_ = kLvxFileDataInfoFault;
return state_;
}
} else {
lvx_file_.open(filename, mode | std::ios_base::binary);
if (!lvx_file_.is_open()) {
state_ = kLvxFileNotExist;
return state_;
}
}
return state_;
}
bool LvxFileHandle::Eof() {
return lvx_file_.eof();
}
int LvxFileHandle::InitLvxFile() {
time_t curtime = time(nullptr);
char filename[30] = { 0 };
tm* local_time = localtime(&curtime);
strftime(filename, sizeof(filename), "%Y%m%d%H%M%S", local_time);
return Open(filename, std::ios::out | std::ios::binary);
}
void LvxFileHandle::InitLvxFileHeader() {
char write_buffer[kMaxLvxFileHeaderLength];
cur_offset_ = 0;
std::string signature = kLvxHeaderSigStr;
memcpy(public_header_.signature, signature.c_str(), signature.size());
public_header_.version[0] = 1;
public_header_.version[1] = file_ver_; /* default version 1 */
public_header_.version[2] = 0;
public_header_.version[3] = 0;
public_header_.magic_code = kLvxHeaderMagicCode;
memcpy(&write_buffer[cur_offset_], (void *)&public_header_, sizeof(public_header_));
cur_offset_ += sizeof(public_header_);
if (file_ver_ == kLvxFileV1) {
private_header_.device_count = static_cast<uint8_t>(device_info_list_.size());
private_header_.frame_duration = frame_duration_;
device_count_ = private_header_.device_count;
memcpy(&write_buffer[cur_offset_], (void *)&private_header_, sizeof(private_header_));
cur_offset_ += sizeof(private_header_);
} else {
private_header_v0_.device_count = static_cast<uint8_t>(device_info_list_.size());
device_count_ = private_header_v0_.device_count;
memcpy(&write_buffer[cur_offset_], (void *)&private_header_v0_, sizeof(private_header_v0_));
cur_offset_ += sizeof(private_header_v0_);
}
for (int i = 0; i < device_count_; i++) {
if (file_ver_ == kLvxFileV1) {
memcpy(&write_buffer[cur_offset_], (void *)&device_info_list_[i], sizeof(LvxFileDeviceInfo));
cur_offset_ += sizeof(LvxFileDeviceInfo);
} else {
LvxFileDeviceInfoV0 device_info_v0;
memcpy((void *)&device_info_v0, (void *)&device_info_list_[i], \
&device_info_list_[i].extrinsic_enable - device_info_list_[i].lidar_broadcast_code);
memcpy((void *)&device_info_v0.roll, (void *)&device_info_list_[i].roll, \
sizeof(float) * 6);
memcpy(&write_buffer[cur_offset_], (void *)&device_info_v0, sizeof(device_info_v0));
cur_offset_ += sizeof(device_info_v0);
}
}
lvx_file_.write(&write_buffer[cur_offset_], cur_offset_);
}
void LvxFileHandle::SaveFrameToLvxFile(std::list<LvxFilePacket> &point_packet_list_temp) {
uint64_t cur_pos = 0;
FrameHeader frame_header = { 0 };
std::unique_ptr<char[]> write_buffer(new char[kMaxFrameSize]);
frame_header.current_offset = cur_offset_;
frame_header.next_offset = cur_offset_ + sizeof(FrameHeader);
for (auto iter : point_packet_list_temp) {
frame_header.next_offset += iter.pack_size;
}
frame_header.frame_index = cur_frame_index_;
memcpy(write_buffer.get() + cur_pos, (void*)&frame_header, sizeof(FrameHeader));
cur_pos += sizeof(FrameHeader);
for (auto iter : point_packet_list_temp) {
if (cur_pos + iter.pack_size >= kMaxFrameSize) {
lvx_file_.write((char*)write_buffer.get(), cur_pos);
cur_pos = 0;
memcpy(write_buffer.get() + cur_pos, (void*)&(iter), iter.pack_size);
cur_pos += iter.pack_size;
} else {
memcpy(write_buffer.get() + cur_pos, (void*)&(iter), iter.pack_size);
cur_pos += iter.pack_size;
}
}
lvx_file_.write((char*)write_buffer.get(), cur_pos);
cur_offset_ = frame_header.next_offset;
cur_frame_index_++;
}
void LvxFileHandle::CloseLvxFile() {
if (lvx_file_ && lvx_file_.is_open())
lvx_file_.close();
}
void LvxFileHandle::BasePointsHandle(LivoxEthPacket *data, LvxFilePacket &packet) {
memcpy((void *)&packet, (void *)data, GetEthPacketLen(data->data_type));
}
int LvxFileHandle::GetDeviceInfo(uint8_t idx, LvxFileDeviceInfo* info) {
if (idx < device_info_list_.size()) {
*info = device_info_list_[idx];
return 0;
}
return -1;
}
int LvxFileHandle::GetPacketsOfFrame(OutPacketBuffer* packets_of_frame) {
if (!lvx_file_ || lvx_file_.eof()) {
state_ = kLvxFileAtEnd;
return kLvxFileAtEnd;
}
FrameHeader frame_header;
FrameHeaderV0 frame_header_v0;
uint64_t read_length;
if (file_ver_ == kLvxFileV1) {
lvx_file_.read((char *)&frame_header, sizeof(frame_header));
if (!lvx_file_) {
return kLvxFileReadFail;
}
if ((size_ < frame_header.current_offset) || \
(frame_header.next_offset < frame_header.current_offset)) {
return kLvxFileFrameHeaderError;
}
packets_of_frame->data_size = DataSizeOfFrame(frame_header);
read_length = packets_of_frame->data_size;
} else {
lvx_file_.read((char *)&frame_header_v0, sizeof(frame_header_v0));
if (!lvx_file_) {
return kLvxFileReadFail;
}
if ((size_ < frame_header_v0.current_offset) || \
(frame_header_v0.next_offset < frame_header_v0.current_offset)) {
return kLvxFileFrameHeaderError;
}
packets_of_frame->data_size = DataSizeOfFrame(frame_header_v0);
read_length = packets_of_frame->data_size;
}
lvx_file_.read((char *)(packets_of_frame->packet), read_length);
if (lvx_file_) {
return kLvxFileOk;
} else {
return kLvxFileReadFail;
}
}
int LvxFileHandle::GetLvxFileReadProgress() {
if (!size_) {
return 0;
}
if (!lvx_file_.eof()) {
return (lvx_file_.tellg() * 100ULL) / size_;
} else {
return 100;
}
}
void ParseExtrinsicXml(DeviceItem &item, LvxFileDeviceInfo &info) {
rapidxml::file<> extrinsic_param("extrinsic.xml");
rapidxml::xml_document<> doc;
doc.parse<0>(extrinsic_param.data());
rapidxml::xml_node<>* root = doc.first_node();
if ("Livox" == (std::string)root->name()) {
for (rapidxml::xml_node<>* device = root->first_node(); device; \
device = device->next_sibling()) {
if ("Device" == (std::string)device->name() && \
(strncmp(item.info.broadcast_code, device->value(), kBroadcastCodeSize) == 0)) {
memcpy(info.lidar_broadcast_code, device->value(), kBroadcastCodeSize);
memset(info.hub_broadcast_code, 0, kBroadcastCodeSize);
info.device_type = item.info.type;
info.device_index = item.handle;
for (rapidxml::xml_attribute<>* param = device->first_attribute(); param; \
param = param->next_attribute()) {
if ("roll" == (std::string)param->name()) {
info.roll = static_cast<float>(atof(param->value()));
}
if ("pitch" == (std::string)param->name()) {
info.pitch = static_cast<float>(atof(param->value()));
}
if ("yaw" == (std::string)param->name()) {
info.yaw = static_cast<float>(atof(param->value()));
}
if ("x" == (std::string)param->name()) {
info.x = static_cast<float>(atof(param->value()));
}
if ("y" == (std::string)param->name()) {
info.y = static_cast<float>(atof(param->value()));
}
if ("z" == (std::string)param->name()) {
info.z = static_cast<float>(atof(param->value()));
}
}
}
}
}
}
}

234
livox_ros_driver/livox_ros_driver/lvx_file.h Normal file
View File

@@ -0,0 +1,234 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2019 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef LIVOX_FILE_H_
#define LIVOX_FILE_H_
#include <memory>
#include <ios>
#include <fstream>
#include <list>
#include <vector>
#include <mutex>
#include "livox_sdk.h"
namespace livox_ros {
#define kMaxPointSize 1500
#define kDefaultFrameDurationTime 50
const uint32_t kMaxFrameSize = 2048*1024;
typedef enum {
kDeviceStateDisconnect = 0,
kDeviceStateConnect = 1,
kDeviceStateSampling = 2,
} DeviceState;
typedef enum {
kLvxFileOk = 0,
kLvxFileNotExist,
kLvxFileSizeFault,
kLvxFileHeaderFault,
kLvxFileDeviceInfoFault,
kLvxFileDataInfoFault,
kLvxFileAtEnd,
kLvxFileReadFail,
kLvxFileFrameHeaderError,
kLvxFileUndefFault
} LvxFileState;
typedef enum {
kLvxFileV0 = 0,
kLvxFileV1 = 1,
kLvxFileVersionUndef = 2,
} LvxFileVersion;
typedef struct {
uint8_t handle;
DeviceState device_state;
DeviceInfo info;
} DeviceItem;
#pragma pack(1)
typedef struct {
uint8_t signature[16];
uint8_t version[4];
uint32_t magic_code;
} LvxFilePublicHeader;
typedef struct {
uint32_t frame_duration;
uint8_t device_count;
} LvxFilePrivateHeader;
typedef struct {
uint8_t lidar_broadcast_code[16];
uint8_t hub_broadcast_code[16];
uint8_t device_index;
uint8_t device_type;
uint8_t extrinsic_enable;
float roll;
float pitch;
float yaw;
float x;
float y;
float z;
} LvxFileDeviceInfo;
typedef struct {
uint8_t device_index;
uint8_t version;
uint8_t port_id;
uint8_t lidar_index;
uint8_t rsvd;
uint32_t error_code;
uint8_t timestamp_type;
uint8_t data_type;
uint8_t timestamp[8];
uint8_t raw_point[kMaxPointSize];
uint32_t pack_size;
} LvxFilePacket;
typedef struct {
uint64_t current_offset;
uint64_t next_offset;
uint64_t frame_index;
} FrameHeader;
typedef struct {
FrameHeader header;
LvxFilePacket *packet;
} LvxFileFrame;
typedef struct {
uint8_t device_count;
} LvxFilePrivateHeaderV0;
typedef struct {
uint8_t lidar_broadcast_code[16];
uint8_t hub_broadcast_code[16];
uint8_t device_index;
uint8_t device_type;
float roll;
float pitch;
float yaw;
float x;
float y;
float z;
} LvxFileDeviceInfoV0;
typedef struct {
uint8_t device_index;
uint8_t version;
uint8_t port_id;
uint8_t lidar_index;
uint8_t rsvd;
uint32_t error_code;
uint8_t timestamp_type;
uint8_t data_type;
uint8_t timestamp[8];
LivoxPoint raw_point[100];
} LvxFilePacketV0;
typedef struct {
uint64_t current_offset;
uint64_t next_offset;
uint64_t frame_index;
uint64_t packet_count;
} FrameHeaderV0;
typedef struct {
FrameHeaderV0 header;
LvxFilePacketV0 *packet;
} LvxFileFrameV0;
typedef struct {
uint32_t buffer_capacity; /* max buffer size */
uint32_t data_size; /* frame data erea size */
uint8_t *packet; /* packet data erea */
} OutPacketBuffer;
#pragma pack()
class LvxFileHandle {
public:
LvxFileHandle();
~LvxFileHandle() = default;
int Open(const char* filename, std::ios_base::openmode mode);
bool Eof();
int InitLvxFile();
void InitLvxFileHeader();
void SaveFrameToLvxFile(std::list<LvxFilePacket>& point_packet_list_temp);
void BasePointsHandle(LivoxEthPacket* data, LvxFilePacket& packet);
void CloseLvxFile();
void AddDeviceInfo(LvxFileDeviceInfo& info) { device_info_list_.push_back(info); }
int GetDeviceInfoListSize() { return device_info_list_.size(); }
int GetDeviceCount() { return device_count_; }
int GetDeviceInfo(uint8_t idx, LvxFileDeviceInfo* info);
int GetFileState(void) { return state_; };
int GetPacketsOfFrame(OutPacketBuffer* PacketsOfFrame);
int GetLvxFileReadProgress();
int GetFileVersion() { return file_ver_; }
private:
std::fstream lvx_file_;
std::vector<LvxFileDeviceInfo> device_info_list_;
uint8_t file_ver_;
uint8_t device_count_;
LvxFilePublicHeader public_header_;
LvxFilePrivateHeader private_header_;
LvxFilePrivateHeaderV0 private_header_v0_;
uint32_t cur_frame_index_;
uint64_t cur_offset_;
uint32_t frame_duration_;
uint64_t data_start_offset_;
uint64_t size_;
int mode_;
int state_;
uint64_t MiniFileSize();
uint64_t PrivateHeaderOffset();
uint64_t DataStartOffset();
uint32_t PacketNumOfFrame();
bool ReadAndCheckHeader();
bool AddAndCheckDeviceInfo();
bool PrepareDataRead();
uint64_t DataSizeOfFrame(FrameHeader& frame_header) {
return (frame_header.next_offset - frame_header.current_offset - sizeof(frame_header));
}
uint64_t DataSizeOfFrame(FrameHeaderV0& frame_header_v0) {
return (frame_header_v0.next_offset - frame_header_v0.current_offset - \
sizeof(frame_header_v0));
}
};
}
#endif

1
livox_ros_driver/msg/CustomPoint.msg
View File

@@ -5,5 +5,6 @@ float32 x # X axis, unit:m
float32 y # Y axis, unit:m float32 y # Y axis, unit:m
float32 z # Z axis, unit:m float32 z # Z axis, unit:m
uint8 reflectivity # reflectivity, 0~255 uint8 reflectivity # reflectivity, 0~255
uint8 tag # livox tag
uint8 line # laser number in lidar uint8 line # laser number in lidar

7
livox_ros_driver/package.xml
View File

@@ -1,7 +1,7 @@
<?xml version="1.0"?> <?xml version="1.0"?>
<package format="2"> <package format="2">
<name>livox_ros_driver</name> <name>livox_ros_driver</name>
<version>1.1.0</version> <version>2.0.0</version>
<description>The ROS device driver for Livox 3D LiDARs and Livox Hub</description> <description>The ROS device driver for Livox 3D LiDARs and Livox Hub</description>
<!-- One maintainer tag required, multiple allowed, one person per tag --> <!-- One maintainer tag required, multiple allowed, one person per tag -->
@@ -54,20 +54,23 @@
<build_depend>rospy</build_depend> <build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend> <build_depend>std_msgs</build_depend>
<build_depend>message_generation</build_depend> <build_depend>message_generation</build_depend>
<build_depend>rosbag</build_depend>
<build_export_depend>roscpp</build_export_depend> <build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend> <build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend> <build_export_depend>std_msgs</build_export_depend>
<build_export_depend>rosbag</build_export_depend>
<exec_depend>roscpp</exec_depend> <exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend> <exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend> <exec_depend>std_msgs</exec_depend>
<exec_depend>message_runtime</exec_depend> <exec_depend>message_runtime</exec_depend>
<exec_depend>rosbag</exec_depend>
<depend>sensor_msgs</depend> <depend>sensor_msgs</depend>
<depend>git</depend> <depend>git</depend>
<depend>apr</depend> <depend>apr</depend>
<!-- The export tag contains other, unspecified, tags --> <!-- The export tag contains other, unspecified, tags -->
<export> <export>
<!-- Other tools can request additional information be placed here --> <!-- Other tools can request additional information be placed here -->

195
livox_ros_driver/timesync/timesync.cpp Normal file
View File

@@ -0,0 +1,195 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2019 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include "timesync.h"
#include <stdint.h>
#include <string.h>
#include <thread>
#include <chrono>
#include <functional>
namespace livox_ros {
using namespace std;
TimeSync::TimeSync() : exit_poll_state_(false), start_poll_state_(false),
exit_poll_data_(false), start_poll_data_(false) {
fsm_state_ = kOpenDev;
uart_ = nullptr;
comm_ = nullptr;
fn_cb_ = nullptr;
client_data_ = nullptr;
rx_bytes_ = 0;
}
TimeSync::~TimeSync() {
DeInitTimeSync();
}
int32_t TimeSync::InitTimeSync(const TimeSyncConfig& config) {
config_ = config;
if (config_.dev_config.type == kCommDevUart) {
uint8_t baudrate_index = config_.dev_config.config.uart.baudrate;
uint8_t parity_index = config_.dev_config.config.uart.parity;
if ((baudrate_index < BRUnkown) && (parity_index < ParityUnkown)) {
uart_ = new UserUart(baudrate_index, parity_index);
} else {
printf("Uart parameter error, please check the configuration file!\n");
return -1;
}
} else {
printf("Device type not supported, now only uart is supported!\n");
return -1;
}
config_.protocol_config.type = kGps;
comm_ = new CommProtocol(config_.protocol_config);
t_poll_state_ = std::make_shared<std::thread>(std::bind(&TimeSync::PollStateLoop, this));
t_poll_data_ = std::make_shared<std::thread>(std::bind(&TimeSync::PollDataLoop, this));
return 0;
}
int32_t TimeSync::DeInitTimeSync() {
StopTimesync();
if (uart_) delete uart_;
if (comm_) delete comm_;
fn_cb_ = nullptr;
client_data_ = nullptr;
return 0;
}
void TimeSync::StopTimesync() {
start_poll_state_ = false;
start_poll_data_ = false;
exit_poll_state_ = true;
exit_poll_data_ = true;
if (t_poll_state_) {
t_poll_state_->join();
t_poll_state_ = nullptr;
}
if (t_poll_state_) {
t_poll_data_->join();
t_poll_data_ = nullptr;
}
}
void TimeSync::PollStateLoop() {
while (!start_poll_state_) {
/* waiting to start */
}
while (!exit_poll_state_) {
if(fsm_state_ == kOpenDev) {
FsmOpenDev();
} else if (fsm_state_ == kPrepareDev) {
FsmPrepareDev();
} else if (fsm_state_ == kCheckDevState) {
FsmCheckDevState();
}
std::this_thread::sleep_for(std::chrono::milliseconds(50));
}
}
void TimeSync::PollDataLoop() {
while (!start_poll_data_) {
/* waiting to start */
}
while (!exit_poll_data_) {
if (uart_->IsOpen()) {
uint32_t get_buf_size;
uint8_t *cache_buf = comm_->FetchCacheFreeSpace(&get_buf_size);
if (get_buf_size) {
uint32_t read_data_size;
read_data_size = uart_->Read((char *)cache_buf, get_buf_size);
if (read_data_size) {
comm_->UpdateCacheWrIdx(read_data_size);
rx_bytes_ += read_data_size;
CommPacket packet;
memset(&packet, 0, sizeof(packet));
while ((kParseSuccess == comm_->ParseCommStream(&packet))) {
if ((fn_cb_ != nullptr) || (client_data_ != nullptr)) {
fn_cb_((const char*)packet.data, packet.data_len, client_data_);
}
}
}
}
} else {
std::this_thread::sleep_for(std::chrono::milliseconds(50));
}
}
}
void TimeSync::FsmTransferState(uint8_t new_state) {
if(new_state < kFsmDevUndef) {
fsm_state_ = new_state;
}
transfer_time_ = chrono::steady_clock::now();
}
void TimeSync::FsmOpenDev() {
if (!uart_->IsOpen()) {
if (!uart_->Open(config_.dev_config.name)) {
FsmTransferState(kPrepareDev);
}
} else {
FsmTransferState(kPrepareDev);
}
}
void TimeSync::FsmPrepareDev() {
chrono::steady_clock::time_point t = chrono::steady_clock::now();
chrono::milliseconds time_gap = chrono::duration_cast<chrono::milliseconds>(t-transfer_time_);
/** delay some time when device is opened, 4s */
if (time_gap.count() > 3000) {
FsmTransferState(kCheckDevState);
}
}
void TimeSync::FsmCheckDevState() {
static uint32_t last_rx_bytes = 0;
static chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::milliseconds time_gap = chrono::duration_cast<chrono::milliseconds>(t2 - t1);
if (time_gap.count() > 2000) { /* period : 2.5s */
if (last_rx_bytes == rx_bytes_) {
uart_->Close();
FsmTransferState(kOpenDev);
printf("Uart is disconnected, close it\n");
}
last_rx_bytes = rx_bytes_;
t1 = t2;
}
}
}

110
livox_ros_driver/timesync/timesync.h Normal file
View File

@@ -0,0 +1,110 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2019 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef TIMESYNC_TIMESYNC_H_
#define TIMESYNC_TIMESYNC_H_
#include <thread>
#include "comm_protocol.h"
#include "comm_device.h"
#include "user_uart.h"
namespace livox_ros {
typedef void (*FnReceiveSyncTimeCb)(const char* rmc, uint32_t rmc_length, void* client_data);
enum FsmPollState {
kOpenDev,
kPrepareDev,
kCheckDevState,
kFsmDevUndef
};
typedef struct {
CommDevConfig dev_config;
ProtocolConfig protocol_config;
} TimeSyncConfig;
class TimeSync {
public:
static TimeSync* GetInstance() {
static TimeSync time_sync;
return &time_sync;
}
int32_t InitTimeSync(const TimeSyncConfig& config);
int32_t DeInitTimeSync();
void StartTimesync() {
start_poll_state_ = true;
start_poll_data_ = true;
}
int32_t SetReceiveSyncTimeCb(FnReceiveSyncTimeCb cb, void* data) {
if ((cb != nullptr) || (data != nullptr)) {
fn_cb_ = cb;
client_data_ = data;
return 0;
} else {
return -1;
}
}
private:
TimeSync();
~TimeSync();
TimeSync(const TimeSync&) = delete;
TimeSync& operator=(const TimeSync&) = delete;
void PollStateLoop();
void PollDataLoop();
void StopTimesync();
std::shared_ptr<std::thread> t_poll_state_;
volatile bool exit_poll_state_;
volatile bool start_poll_state_;
std::shared_ptr<std::thread> t_poll_data_;
volatile bool exit_poll_data_;
volatile bool start_poll_data_;
TimeSyncConfig config_;
UserUart* uart_;
CommProtocol* comm_;
volatile uint32_t rx_bytes_;
FnReceiveSyncTimeCb fn_cb_;
void* client_data_;
volatile uint8_t fsm_state_;
std::chrono::steady_clock::time_point transfer_time_;
void FsmTransferState(uint8_t new_state);
void FsmOpenDev();
void FsmPrepareDev();
void FsmCheckDevState();
};
}
#endif

198
livox_ros_driver/timesync/user_uart/user_uart.cpp Normal file
View File

@@ -0,0 +1,198 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2019 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include "user_uart.h"
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <termios.h>
#include <sys/types.h>
#include <sys/stat.h>
namespace livox_ros {
UserUart::UserUart(uint8_t baudrate_index, uint8_t parity):
baudrate_(baudrate_index), parity_(parity) {
fd_ = 0;
is_open_ = false;
}
UserUart::~UserUart() {
is_open_ = false;
if (fd_ > 0) {
/** first we flush the port */
tcflush(fd_,TCOFLUSH);
tcflush(fd_,TCIFLUSH);
close(fd_);
}
}
int UserUart::Open(const char* filename) {
fd_ = open(filename, O_RDWR | O_NOCTTY); //| O_NDELAY
if (fd_ < 0) {
printf("Open %s fail!\n", filename);
return -1;
} else {
chmod(filename, S_IRWXU | S_IRWXG | S_IRWXO); /* need add here */
printf("Open %s success!\n", filename);
}
if (fd_ > 0) {
/** set baudrate and parity,etc. */
if (Setup(baudrate_, parity_)) {
return -1;
}
}
is_open_ = true;
return 0;
}
int UserUart::Close() {
is_open_ = false;
if (fd_ > 0) {
/** first we flush the port */
tcflush(fd_,TCOFLUSH);
tcflush(fd_,TCIFLUSH);
return close(fd_);
}
return -1;
}
/** sets up the port parameters */
int UserUart::Setup(uint8_t baudrate_index, uint8_t parity) {
static uint32_t baud_map[19] = {\
B2400, B4800, B9600, B19200, B38400, B57600,B115200, B230400,\
B460800, B500000, B576000,B921600,B1152000, B1500000, B2000000,\
B2500000, B3000000, B3500000, B4000000\
};
tcflag_t baudrate;
struct termios options;
if ((baudrate_index > BR4000000) || (parity > P_7S1)) {
return -1;
}
/** clear old setting completely,must add here for CDC serial */
tcgetattr(fd_, &options);
memset(&options, 0, sizeof(options));
tcflush(fd_, TCIOFLUSH);
tcsetattr(fd_, TCSANOW, &options);
usleep(10000);
/** Enable the receiver and set local mode... */
options.c_cflag |= (CLOCAL | CREAD);
/** Disable hardware flow */
//options.c_cflag &= ~CRTSCTS;
/** Disable software flow */
//options.c_iflag &= ~(IXON | IXOFF | IXANY);
//options.c_oflag &= ~OPOST;
/** set boadrate */
options.c_cflag &= ~CBAUD;
baudrate = baud_map[baudrate_index];
options.c_cflag |= baudrate;
switch (parity) {
case P_8N1:
/** No parity (8N1) */
options.c_cflag &= ~PARENB;
options.c_cflag &= ~CSTOPB;
options.c_cflag &= ~CSIZE;
options.c_cflag |= CS8;
break;
case P_7E1:
/** Even parity (7E1) */
options.c_cflag |= PARENB;
options.c_cflag &= ~PARODD;
options.c_cflag &= ~CSTOPB;
options.c_cflag &= ~CSIZE;
options.c_cflag |= CS7;
break;
case P_7O1:
/** Odd parity (7O1) */
options.c_cflag |= PARENB;
options.c_cflag |= PARODD;
options.c_cflag &= ~CSTOPB;
options.c_cflag &= ~CSIZE;
options.c_cflag |= CS7;
break;
case P_7S1:
/** Space parity is setup the same as no parity (7S1) */
options.c_cflag &= ~PARENB;
options.c_cflag &= ~CSTOPB;
options.c_cflag &= ~CSIZE;
options.c_cflag |= CS8;
break;
default:
return -1;
}
/** now we setup the values in port's termios */
options.c_iflag &= ~INPCK;
/** Enable non-canonical */
//options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
/** Time to wait for data */
options.c_cc[VTIME] = 1;
/** Minimum number of characters to read */
options.c_cc[VMIN] = 1;
/** flush the port */
tcflush(fd_, TCIOFLUSH);
/** send new config to the port */
tcsetattr(fd_, TCSANOW, &options);
return 0;
}
ssize_t UserUart::Write(const char *buffer, size_t size) {
if (fd_ > 0) {
return write(fd_, buffer, size);
} else {
return 0;
}
}
ssize_t UserUart::Read(char *buffer, size_t size) {
if (fd_ > 0) {
return read(fd_, buffer, size);
} else {
return 0;
}
}
}

92
livox_ros_driver/timesync/user_uart/user_uart.h Normal file
View File

@@ -0,0 +1,92 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2019 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef USER_UART_H_
#define USER_UART_H_
#include <stdint.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/fcntl.h>
#include <termios.h>
namespace livox_ros {
enum Parity {
P_8N1, /* No parity (8N1) */
P_7E1, /* Even parity (7E1)*/
P_7O1, /* Odd parity (7O1) */
P_7S1, /* Space parity is setup the same as no parity (7S1) */
ParityUnkown
};
enum BaudRate {
BR2400,
BR4800,
BR9600,
BR19200,
BR38400,
BR57600,
BR115200,
BR230400,
BR460800,
BR500000,
BR576000,
BR921600,
BR1152000,
BR1500000,
BR2000000,
BR2500000,
BR3000000,
BR3500000,
BR4000000,
BRUnkown,
};
class UserUart {
public:
UserUart(uint8_t baudrate_index, uint8_t parity);
~UserUart();
int Setup(uint8_t baudrate_index, uint8_t parity);
ssize_t Write(const char *buffer, size_t size);
ssize_t Read(char *buffer, size_t size);
int Close();
int Open(const char* filename);
bool IsOpen() { return is_open_; };
private:
int fd_;
volatile bool is_open_;
uint8_t baudrate_;
uint8_t parity_;
};
}
#endif