diff --git a/README.md b/README.md index c54b985..4231cc9 100644 --- a/README.md +++ b/README.md @@ -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. -### 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. 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. -### 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` -2. Build livox_ros_driver package: +2. Build livox_ros_driver package : ``` cd ws_livox 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 livox_lidar_rviz.launch -``` + `roslaunch livox_ros_driver [launch file] [param]` -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` -``` -roslaunch livox_ros_driver livox_lidar_rviz.launch bd_list:="broadcast_code1&broadcast_code2&broadcast_code3" -``` +2. Connect to the specific LiDAR uint(s), suppose there are LiDAR(0TFDG3B006H2Z11) and LiDAR(1HDDG8M00100191) . -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 | | ------------------------- | ------------------------------------------------------------ | -| *livox_lidar_rviz.launch* | Connect to Livox LiDAR units
Publish pointcloud2 format point cloud
Automatically load rviz | +| livox_lidar_rviz.launch | Connect to Livox LiDAR units
Publish pointcloud2 format point cloud
Automatically load rviz | | livox_hub_rviz.launch | Connect to Livox Hub units
Publish pointcloud2 format point cloud
Automatically load rviz | | livox_lidar.launch | Connect to Livox LiDAR units
Publish pointcloud2 format point cloud | | livox_hub.launch | Connect to Livox Hub units
Publish pointcloud2 format point cloud | | livox_lidar_msg.launch | Connect to Livox LiDAR units
Publish livox custom format point cloud | | livox_hub_msg.launch | Connect to Livox Hub units
Publish livox custom format point cloud | +| lvx_to_rosbag.launch | Covert lvx file to rosbag file
Covert lvx file directly to rosbag file | +| lvx_to_rosbag_rviz.launch | Covert lvx file to rosbag file
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
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
0 -- all lidars share the same topic
1 -- each lidar uses a topic independently | +| xfer_format | Set publish data format
0 -- livox pointcloud2(PointXYZRTL)
1 -- livox custom msg format
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 @@ -65,12 +106,139 @@ uint8 lidar_id # Lidar device id number uint8[3] rsvd # Reserved use CustomPoint[] points # Pointcloud data ``` -pointcloud format: + +The Custom Point format in custom message : + ``` uint32 offset_time # offset time relative to the base time float32 x # X axis, unit:m float32 y # Y axis, unit:m float32 z # Z axis, unit:m uint8 reflectivity # reflectivity, 0~255 +uint8 tag # livox tag 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. + + + + + diff --git a/images/broadcast_code.png b/images/broadcast_code.png new file mode 100644 index 0000000..78deb5a Binary files /dev/null and b/images/broadcast_code.png differ diff --git a/livox_ros_driver/CMakeLists.txt b/livox_ros_driver/CMakeLists.txt old mode 100755 new mode 100644 index 61e7d2c..0b95e1b --- a/livox_ros_driver/CMakeLists.txt +++ b/livox_ros_driver/CMakeLists.txt @@ -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) -add_definitions(-std=c++11) +#--------------------------------------------------------------------------------------- +# Start livox_ros_driver project +#--------------------------------------------------------------------------------------- +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 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS - roscpp - rospy - std_msgs - message_generation -) + roscpp + rospy + sensor_msgs + std_msgs + message_generation + rosbag + ) -find_package(Boost REQUIRED) - -find_package(Boost REQUIRED COMPONENTS thread) - -## get pointcloud package +## Find pcl lib 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 add_message_files(FILES CustomPoint.msg CustomMsg.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_messages(DEPENDENCIES - 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() + std_msgs + ) find_package(PkgConfig) pkg_check_modules(APR apr-1) @@ -165,14 +53,32 @@ if (APR_FOUND) message(${APR_LIBRARIES}) endif (APR_FOUND) -include_directories( - ./ - ${APR_INCLUDE_DIRS} -) +################################### +## 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 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}) -add_definitions(${PCL_DEFINITIONS}) +#--------------------------------------------------------------------------------------- +# Set default build to release +#--------------------------------------------------------------------------------------- +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 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 ) - ## 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() 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() -############# -## Install ## -############# +## PCL library +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 -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) +#--------------------------------------------------------------------------------------- +# precompile macro and compile option +#--------------------------------------------------------------------------------------- +target_compile_options(${PROJECT_NAME}_node + PRIVATE $<$:-Wall -Werror> + ) -## Mark executables and/or libraries for installation -# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) +#--------------------------------------------------------------------------------------- +# add projects that depend on +#--------------------------------------------------------------------------------------- +add_dependencies(${PROJECT_NAME}_node ${PROJECT_NAME}_generate_messages_cpp) -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) +#--------------------------------------------------------------------------------------- +# source file +#--------------------------------------------------------------------------------------- +target_sources(${PROJECT_NAME}_node + 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 -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) +#--------------------------------------------------------------------------------------- +# include file +#--------------------------------------------------------------------------------------- +target_include_directories(${PROJECT_NAME}_node + 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() diff --git a/livox_ros_driver/cmake/version.cmake b/livox_ros_driver/cmake/version.cmake new file mode 100644 index 0000000..74bb86b --- /dev/null +++ b/livox_ros_driver/cmake/version.cmake @@ -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}") diff --git a/livox_ros_driver/common/FastCRC/FastCRC.h b/livox_ros_driver/common/FastCRC/FastCRC.h new file mode 100644 index 0000000..d796171 --- /dev/null +++ b/livox_ros_driver/common/FastCRC/FastCRC.h @@ -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 + + +// ================= 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_ + diff --git a/livox_ros_driver/common/FastCRC/FastCRC_tables.hpp b/livox_ros_driver/common/FastCRC/FastCRC_tables.hpp new file mode 100644 index 0000000..4726e26 --- /dev/null +++ b/livox_ros_driver/common/FastCRC/FastCRC_tables.hpp @@ -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 + +// 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 + diff --git a/livox_ros_driver/common/FastCRC/FastCRCsw.cpp b/livox_ros_driver/common/FastCRC/FastCRCsw.cpp new file mode 100644 index 0000000..e3b659d --- /dev/null +++ b/livox_ros_driver/common/FastCRC/FastCRCsw.cpp @@ -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; +} + + diff --git a/livox_ros_driver/common/FastCRC/LICENSE.md b/livox_ros_driver/common/FastCRC/LICENSE.md new file mode 100644 index 0000000..b7c7a98 --- /dev/null +++ b/livox_ros_driver/common/FastCRC/LICENSE.md @@ -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. diff --git a/livox_ros_driver/common/FastCRC/README.md b/livox_ros_driver/common/FastCRC/README.md new file mode 100644 index 0000000..97ae86c --- /dev/null +++ b/livox_ros_driver/common/FastCRC/README.md @@ -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) diff --git a/livox_ros_driver/common/comm/comm_device.h b/livox_ros_driver/common/comm/comm_device.h new file mode 100644 index 0000000..f64508d --- /dev/null +++ b/livox_ros_driver/common/comm/comm_device.h @@ -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 + +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_ diff --git a/livox_ros_driver/common/comm/comm_protocol.cpp b/livox_ros_driver/common/comm/comm_protocol.cpp new file mode 100644 index 0000000..6a82387 --- /dev/null +++ b/livox_ros_driver/common/comm/comm_protocol.cpp @@ -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 +#include +#include + +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 diff --git a/livox_ros_driver/common/comm/comm_protocol.h b/livox_ros_driver/common/comm/comm_protocol.h new file mode 100644 index 0000000..c4396c7 --- /dev/null +++ b/livox_ros_driver/common/comm/comm_protocol.h @@ -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 +#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_ diff --git a/livox_ros_driver/common/comm/gps_protocol.cpp b/livox_ros_driver/common/comm/gps_protocol.cpp new file mode 100644 index 0000000..fa57c66 --- /dev/null +++ b/livox_ros_driver/common/comm/gps_protocol.cpp @@ -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 +#include +#include + +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 (; (isof == 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 diff --git a/livox_ros_driver/common/comm/gps_protocol.h b/livox_ros_driver/common/comm/gps_protocol.h new file mode 100644 index 0000000..0506d79 --- /dev/null +++ b/livox_ros_driver/common/comm/gps_protocol.h @@ -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 +#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_ diff --git a/livox_ros_driver/common/comm/protocol.h b/livox_ros_driver/common/comm/protocol.h new file mode 100644 index 0000000..0c914b5 --- /dev/null +++ b/livox_ros_driver/common/comm/protocol.h @@ -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 + +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_ diff --git a/livox_ros_driver/common/comm/sdk_protocol.cpp b/livox_ros_driver/common/comm/sdk_protocol.cpp new file mode 100644 index 0000000..ec8242a --- /dev/null +++ b/livox_ros_driver/common/comm/sdk_protocol.cpp @@ -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 +#include + +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 diff --git a/livox_ros_driver/common/comm/sdk_protocol.h b/livox_ros_driver/common/comm/sdk_protocol.h new file mode 100644 index 0000000..e6f5b06 --- /dev/null +++ b/livox_ros_driver/common/comm/sdk_protocol.h @@ -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 +#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_ diff --git a/livox_ros_driver/common/rapidjson/allocators.h b/livox_ros_driver/common/rapidjson/allocators.h new file mode 100644 index 0000000..cc67c89 --- /dev/null +++ b/livox_ros_driver/common/rapidjson/allocators.h @@ -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 +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(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(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(chunkHead_) + RAPIDJSON_ALIGN(sizeof(ChunkHeader)) + chunkHead_->size - originalSize) { + size_t increment = static_cast(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(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_ diff --git a/livox_ros_driver/common/rapidjson/cursorstreamwrapper.h b/livox_ros_driver/common/rapidjson/cursorstreamwrapper.h new file mode 100644 index 0000000..52c11a7 --- /dev/null +++ b/livox_ros_driver/common/rapidjson/cursorstreamwrapper.h @@ -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 > +class CursorStreamWrapper : public GenericStreamWrapper { +public: + typedef typename Encoding::Ch Ch; + + CursorStreamWrapper(InputStream& is): + GenericStreamWrapper(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_ diff --git a/livox_ros_driver/common/rapidjson/document.h b/livox_ros_driver/common/rapidjson/document.h new file mode 100644 index 0000000..f070234 --- /dev/null +++ b/livox_ros_driver/common/rapidjson/document.h @@ -0,0 +1,2682 @@ +// 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_DOCUMENT_H_ +#define RAPIDJSON_DOCUMENT_H_ + +/*! \file document.h */ + +#include "reader.h" +#include "internal/meta.h" +#include "internal/strfunc.h" +#include "memorystream.h" +#include "encodedstream.h" +#include // placement new +#include + +RAPIDJSON_DIAG_PUSH +#ifdef __clang__ +RAPIDJSON_DIAG_OFF(padded) +RAPIDJSON_DIAG_OFF(switch-enum) +RAPIDJSON_DIAG_OFF(c++98-compat) +#elif defined(_MSC_VER) +RAPIDJSON_DIAG_OFF(4127) // conditional expression is constant +RAPIDJSON_DIAG_OFF(4244) // conversion from kXxxFlags to 'uint16_t', possible loss of data +#endif + +#ifdef __GNUC__ +RAPIDJSON_DIAG_OFF(effc++) +#endif // __GNUC__ + +#ifndef RAPIDJSON_NOMEMBERITERATORCLASS +#include // std::random_access_iterator_tag +#endif + +#if RAPIDJSON_HAS_CXX11_RVALUE_REFS +#include // std::move +#endif + +RAPIDJSON_NAMESPACE_BEGIN + +// Forward declaration. +template +class GenericValue; + +template +class GenericDocument; + +//! Name-value pair in a JSON object value. +/*! + This class was internal to GenericValue. It used to be a inner struct. + But a compiler (IBM XL C/C++ for AIX) have reported to have problem with that so it moved as a namespace scope struct. + https://code.google.com/p/rapidjson/issues/detail?id=64 +*/ +template +class GenericMember { +public: + GenericValue name; //!< name of member (must be a string) + GenericValue value; //!< value of member. + +#if RAPIDJSON_HAS_CXX11_RVALUE_REFS + //! Move constructor in C++11 + GenericMember(GenericMember&& rhs) RAPIDJSON_NOEXCEPT + : name(std::move(rhs.name)), + value(std::move(rhs.value)) + { + } + + //! Move assignment in C++11 + GenericMember& operator=(GenericMember&& rhs) RAPIDJSON_NOEXCEPT { + return *this = static_cast(rhs); + } +#endif + + //! Assignment with move semantics. + /*! \param rhs Source of the assignment. Its name and value will become a null value after assignment. + */ + GenericMember& operator=(GenericMember& rhs) RAPIDJSON_NOEXCEPT { + if (RAPIDJSON_LIKELY(this != &rhs)) { + name = rhs.name; + value = rhs.value; + } + return *this; + } + + // swap() for std::sort() and other potential use in STL. + friend inline void swap(GenericMember& a, GenericMember& b) RAPIDJSON_NOEXCEPT { + a.name.Swap(b.name); + a.value.Swap(b.value); + } + +private: + //! Copy constructor is not permitted. + GenericMember(const GenericMember& rhs); +}; + +/////////////////////////////////////////////////////////////////////////////// +// GenericMemberIterator + +#ifndef RAPIDJSON_NOMEMBERITERATORCLASS + +//! (Constant) member iterator for a JSON object value +/*! + \tparam Const Is this a constant iterator? + \tparam Encoding Encoding of the value. (Even non-string values need to have the same encoding in a document) + \tparam Allocator Allocator type for allocating memory of object, array and string. + + This class implements a Random Access Iterator for GenericMember elements + of a GenericValue, see ISO/IEC 14882:2003(E) C++ standard, 24.1 [lib.iterator.requirements]. + + \note This iterator implementation is mainly intended to avoid implicit + conversions from iterator values to \c NULL, + e.g. from GenericValue::FindMember. + + \note Define \c RAPIDJSON_NOMEMBERITERATORCLASS to fall back to a + pointer-based implementation, if your platform doesn't provide + the C++ header. + + \see GenericMember, GenericValue::MemberIterator, GenericValue::ConstMemberIterator + */ +template +class GenericMemberIterator { + + friend class GenericValue; + template friend class GenericMemberIterator; + + typedef GenericMember PlainType; + typedef typename internal::MaybeAddConst::Type ValueType; + +public: + //! Iterator type itself + typedef GenericMemberIterator Iterator; + //! Constant iterator type + typedef GenericMemberIterator ConstIterator; + //! Non-constant iterator type + typedef GenericMemberIterator NonConstIterator; + + /** \name std::iterator_traits support */ + //@{ + typedef ValueType value_type; + typedef ValueType * pointer; + typedef ValueType & reference; + typedef std::ptrdiff_t difference_type; + typedef std::random_access_iterator_tag iterator_category; + //@} + + //! Pointer to (const) GenericMember + typedef pointer Pointer; + //! Reference to (const) GenericMember + typedef reference Reference; + //! Signed integer type (e.g. \c ptrdiff_t) + typedef difference_type DifferenceType; + + //! Default constructor (singular value) + /*! Creates an iterator pointing to no element. + \note All operations, except for comparisons, are undefined on such values. + */ + GenericMemberIterator() : ptr_() {} + + //! Iterator conversions to more const + /*! + \param it (Non-const) iterator to copy from + + Allows the creation of an iterator from another GenericMemberIterator + that is "less const". Especially, creating a non-constant iterator + from a constant iterator are disabled: + \li const -> non-const (not ok) + \li const -> const (ok) + \li non-const -> const (ok) + \li non-const -> non-const (ok) + + \note If the \c Const template parameter is already \c false, this + constructor effectively defines a regular copy-constructor. + Otherwise, the copy constructor is implicitly defined. + */ + GenericMemberIterator(const NonConstIterator & it) : ptr_(it.ptr_) {} + Iterator& operator=(const NonConstIterator & it) { ptr_ = it.ptr_; return *this; } + + //! @name stepping + //@{ + Iterator& operator++(){ ++ptr_; return *this; } + Iterator& operator--(){ --ptr_; return *this; } + Iterator operator++(int){ Iterator old(*this); ++ptr_; return old; } + Iterator operator--(int){ Iterator old(*this); --ptr_; return old; } + //@} + + //! @name increment/decrement + //@{ + Iterator operator+(DifferenceType n) const { return Iterator(ptr_+n); } + Iterator operator-(DifferenceType n) const { return Iterator(ptr_-n); } + + Iterator& operator+=(DifferenceType n) { ptr_+=n; return *this; } + Iterator& operator-=(DifferenceType n) { ptr_-=n; return *this; } + //@} + + //! @name relations + //@{ + bool operator==(ConstIterator that) const { return ptr_ == that.ptr_; } + bool operator!=(ConstIterator that) const { return ptr_ != that.ptr_; } + bool operator<=(ConstIterator that) const { return ptr_ <= that.ptr_; } + bool operator>=(ConstIterator that) const { return ptr_ >= that.ptr_; } + bool operator< (ConstIterator that) const { return ptr_ < that.ptr_; } + bool operator> (ConstIterator that) const { return ptr_ > that.ptr_; } + //@} + + //! @name dereference + //@{ + Reference operator*() const { return *ptr_; } + Pointer operator->() const { return ptr_; } + Reference operator[](DifferenceType n) const { return ptr_[n]; } + //@} + + //! Distance + DifferenceType operator-(ConstIterator that) const { return ptr_-that.ptr_; } + +private: + //! Internal constructor from plain pointer + explicit GenericMemberIterator(Pointer p) : ptr_(p) {} + + Pointer ptr_; //!< raw pointer +}; + +#else // RAPIDJSON_NOMEMBERITERATORCLASS + +// class-based member iterator implementation disabled, use plain pointers + +template +class GenericMemberIterator; + +//! non-const GenericMemberIterator +template +class GenericMemberIterator { + //! use plain pointer as iterator type + typedef GenericMember* Iterator; +}; +//! const GenericMemberIterator +template +class GenericMemberIterator { + //! use plain const pointer as iterator type + typedef const GenericMember* Iterator; +}; + +#endif // RAPIDJSON_NOMEMBERITERATORCLASS + +/////////////////////////////////////////////////////////////////////////////// +// GenericStringRef + +//! Reference to a constant string (not taking a copy) +/*! + \tparam CharType character type of the string + + This helper class is used to automatically infer constant string + references for string literals, especially from \c const \b (!) + character arrays. + + The main use is for creating JSON string values without copying the + source string via an \ref Allocator. This requires that the referenced + string pointers have a sufficient lifetime, which exceeds the lifetime + of the associated GenericValue. + + \b Example + \code + Value v("foo"); // ok, no need to copy & calculate length + const char foo[] = "foo"; + v.SetString(foo); // ok + + const char* bar = foo; + // Value x(bar); // not ok, can't rely on bar's lifetime + Value x(StringRef(bar)); // lifetime explicitly guaranteed by user + Value y(StringRef(bar, 3)); // ok, explicitly pass length + \endcode + + \see StringRef, GenericValue::SetString +*/ +template +struct GenericStringRef { + typedef CharType Ch; //!< character type of the string + + //! Create string reference from \c const character array +#ifndef __clang__ // -Wdocumentation + /*! + This constructor implicitly creates a constant string reference from + a \c const character array. It has better performance than + \ref StringRef(const CharType*) by inferring the string \ref length + from the array length, and also supports strings containing null + characters. + + \tparam N length of the string, automatically inferred + + \param str Constant character array, lifetime assumed to be longer + than the use of the string in e.g. a GenericValue + + \post \ref s == str + + \note Constant complexity. + \note There is a hidden, private overload to disallow references to + non-const character arrays to be created via this constructor. + By this, e.g. function-scope arrays used to be filled via + \c snprintf are excluded from consideration. + In such cases, the referenced string should be \b copied to the + GenericValue instead. + */ +#endif + template + GenericStringRef(const CharType (&str)[N]) RAPIDJSON_NOEXCEPT + : s(str), length(N-1) {} + + //! Explicitly create string reference from \c const character pointer +#ifndef __clang__ // -Wdocumentation + /*! + This constructor can be used to \b explicitly create a reference to + a constant string pointer. + + \see StringRef(const CharType*) + + \param str Constant character pointer, lifetime assumed to be longer + than the use of the string in e.g. a GenericValue + + \post \ref s == str + + \note There is a hidden, private overload to disallow references to + non-const character arrays to be created via this constructor. + By this, e.g. function-scope arrays used to be filled via + \c snprintf are excluded from consideration. + In such cases, the referenced string should be \b copied to the + GenericValue instead. + */ +#endif + explicit GenericStringRef(const CharType* str) + : s(str), length(NotNullStrLen(str)) {} + + //! Create constant string reference from pointer and length +#ifndef __clang__ // -Wdocumentation + /*! \param str constant string, lifetime assumed to be longer than the use of the string in e.g. a GenericValue + \param len length of the string, excluding the trailing NULL terminator + + \post \ref s == str && \ref length == len + \note Constant complexity. + */ +#endif + GenericStringRef(const CharType* str, SizeType len) + : s(RAPIDJSON_LIKELY(str) ? str : emptyString), length(len) { RAPIDJSON_ASSERT(str != 0 || len == 0u); } + + GenericStringRef(const GenericStringRef& rhs) : s(rhs.s), length(rhs.length) {} + + //! implicit conversion to plain CharType pointer + operator const Ch *() const { return s; } + + const Ch* const s; //!< plain CharType pointer + const SizeType length; //!< length of the string (excluding the trailing NULL terminator) + +private: + SizeType NotNullStrLen(const CharType* str) { + RAPIDJSON_ASSERT(str != 0); + return internal::StrLen(str); + } + + /// Empty string - used when passing in a NULL pointer + static const Ch emptyString[]; + + //! Disallow construction from non-const array + template + GenericStringRef(CharType (&str)[N]) /* = delete */; + //! Copy assignment operator not permitted - immutable type + GenericStringRef& operator=(const GenericStringRef& rhs) /* = delete */; +}; + +template +const CharType GenericStringRef::emptyString[] = { CharType() }; + +//! Mark a character pointer as constant string +/*! Mark a plain character pointer as a "string literal". This function + can be used to avoid copying a character string to be referenced as a + value in a JSON GenericValue object, if the string's lifetime is known + to be valid long enough. + \tparam CharType Character type of the string + \param str Constant string, lifetime assumed to be longer than the use of the string in e.g. a GenericValue + \return GenericStringRef string reference object + \relatesalso GenericStringRef + + \see GenericValue::GenericValue(StringRefType), GenericValue::operator=(StringRefType), GenericValue::SetString(StringRefType), GenericValue::PushBack(StringRefType, Allocator&), GenericValue::AddMember +*/ +template +inline GenericStringRef StringRef(const CharType* str) { + return GenericStringRef(str); +} + +//! Mark a character pointer as constant string +/*! Mark a plain character pointer as a "string literal". This function + can be used to avoid copying a character string to be referenced as a + value in a JSON GenericValue object, if the string's lifetime is known + to be valid long enough. + + This version has better performance with supplied length, and also + supports string containing null characters. + + \tparam CharType character type of the string + \param str Constant string, lifetime assumed to be longer than the use of the string in e.g. a GenericValue + \param length The length of source string. + \return GenericStringRef string reference object + \relatesalso GenericStringRef +*/ +template +inline GenericStringRef StringRef(const CharType* str, size_t length) { + return GenericStringRef(str, SizeType(length)); +} + +#if RAPIDJSON_HAS_STDSTRING +//! Mark a string object as constant string +/*! Mark a string object (e.g. \c std::string) as a "string literal". + This function can be used to avoid copying a string to be referenced as a + value in a JSON GenericValue object, if the string's lifetime is known + to be valid long enough. + + \tparam CharType character type of the string + \param str Constant string, lifetime assumed to be longer than the use of the string in e.g. a GenericValue + \return GenericStringRef string reference object + \relatesalso GenericStringRef + \note Requires the definition of the preprocessor symbol \ref RAPIDJSON_HAS_STDSTRING. +*/ +template +inline GenericStringRef StringRef(const std::basic_string& str) { + return GenericStringRef(str.data(), SizeType(str.size())); +} +#endif + +/////////////////////////////////////////////////////////////////////////////// +// GenericValue type traits +namespace internal { + +template +struct IsGenericValueImpl : FalseType {}; + +// select candidates according to nested encoding and allocator types +template struct IsGenericValueImpl::Type, typename Void::Type> + : IsBaseOf, T>::Type {}; + +// helper to match arbitrary GenericValue instantiations, including derived classes +template struct IsGenericValue : IsGenericValueImpl::Type {}; + +} // namespace internal + +/////////////////////////////////////////////////////////////////////////////// +// TypeHelper + +namespace internal { + +template +struct TypeHelper {}; + +template +struct TypeHelper { + static bool Is(const ValueType& v) { return v.IsBool(); } + static bool Get(const ValueType& v) { return v.GetBool(); } + static ValueType& Set(ValueType& v, bool data) { return v.SetBool(data); } + static ValueType& Set(ValueType& v, bool data, typename ValueType::AllocatorType&) { return v.SetBool(data); } +}; + +template +struct TypeHelper { + static bool Is(const ValueType& v) { return v.IsInt(); } + static int Get(const ValueType& v) { return v.GetInt(); } + static ValueType& Set(ValueType& v, int data) { return v.SetInt(data); } + static ValueType& Set(ValueType& v, int data, typename ValueType::AllocatorType&) { return v.SetInt(data); } +}; + +template +struct TypeHelper { + static bool Is(const ValueType& v) { return v.IsUint(); } + static unsigned Get(const ValueType& v) { return v.GetUint(); } + static ValueType& Set(ValueType& v, unsigned data) { return v.SetUint(data); } + static ValueType& Set(ValueType& v, unsigned data, typename ValueType::AllocatorType&) { return v.SetUint(data); } +}; + +#ifdef _MSC_VER +RAPIDJSON_STATIC_ASSERT(sizeof(long) == sizeof(int)); +template +struct TypeHelper { + static bool Is(const ValueType& v) { return v.IsInt(); } + static long Get(const ValueType& v) { return v.GetInt(); } + static ValueType& Set(ValueType& v, long data) { return v.SetInt(data); } + static ValueType& Set(ValueType& v, long data, typename ValueType::AllocatorType&) { return v.SetInt(data); } +}; + +RAPIDJSON_STATIC_ASSERT(sizeof(unsigned long) == sizeof(unsigned)); +template +struct TypeHelper { + static bool Is(const ValueType& v) { return v.IsUint(); } + static unsigned long Get(const ValueType& v) { return v.GetUint(); } + static ValueType& Set(ValueType& v, unsigned long data) { return v.SetUint(data); } + static ValueType& Set(ValueType& v, unsigned long data, typename ValueType::AllocatorType&) { return v.SetUint(data); } +}; +#endif + +template +struct TypeHelper { + static bool Is(const ValueType& v) { return v.IsInt64(); } + static int64_t Get(const ValueType& v) { return v.GetInt64(); } + static ValueType& Set(ValueType& v, int64_t data) { return v.SetInt64(data); } + static ValueType& Set(ValueType& v, int64_t data, typename ValueType::AllocatorType&) { return v.SetInt64(data); } +}; + +template +struct TypeHelper { + static bool Is(const ValueType& v) { return v.IsUint64(); } + static uint64_t Get(const ValueType& v) { return v.GetUint64(); } + static ValueType& Set(ValueType& v, uint64_t data) { return v.SetUint64(data); } + static ValueType& Set(ValueType& v, uint64_t data, typename ValueType::AllocatorType&) { return v.SetUint64(data); } +}; + +template +struct TypeHelper { + static bool Is(const ValueType& v) { return v.IsDouble(); } + static double Get(const ValueType& v) { return v.GetDouble(); } + static ValueType& Set(ValueType& v, double data) { return v.SetDouble(data); } + static ValueType& Set(ValueType& v, double data, typename ValueType::AllocatorType&) { return v.SetDouble(data); } +}; + +template +struct TypeHelper { + static bool Is(const ValueType& v) { return v.IsFloat(); } + static float Get(const ValueType& v) { return v.GetFloat(); } + static ValueType& Set(ValueType& v, float data) { return v.SetFloat(data); } + static ValueType& Set(ValueType& v, float data, typename ValueType::AllocatorType&) { return v.SetFloat(data); } +}; + +template +struct TypeHelper { + typedef const typename ValueType::Ch* StringType; + static bool Is(const ValueType& v) { return v.IsString(); } + static StringType Get(const ValueType& v) { return v.GetString(); } + static ValueType& Set(ValueType& v, const StringType data) { return v.SetString(typename ValueType::StringRefType(data)); } + static ValueType& Set(ValueType& v, const StringType data, typename ValueType::AllocatorType& a) { return v.SetString(data, a); } +}; + +#if RAPIDJSON_HAS_STDSTRING +template +struct TypeHelper > { + typedef std::basic_string StringType; + static bool Is(const ValueType& v) { return v.IsString(); } + static StringType Get(const ValueType& v) { return StringType(v.GetString(), v.GetStringLength()); } + static ValueType& Set(ValueType& v, const StringType& data, typename ValueType::AllocatorType& a) { return v.SetString(data, a); } +}; +#endif + +template +struct TypeHelper { + typedef typename ValueType::Array ArrayType; + static bool Is(const ValueType& v) { return v.IsArray(); } + static ArrayType Get(ValueType& v) { return v.GetArray(); } + static ValueType& Set(ValueType& v, ArrayType data) { return v = data; } + static ValueType& Set(ValueType& v, ArrayType data, typename ValueType::AllocatorType&) { return v = data; } +}; + +template +struct TypeHelper { + typedef typename ValueType::ConstArray ArrayType; + static bool Is(const ValueType& v) { return v.IsArray(); } + static ArrayType Get(const ValueType& v) { return v.GetArray(); } +}; + +template +struct TypeHelper { + typedef typename ValueType::Object ObjectType; + static bool Is(const ValueType& v) { return v.IsObject(); } + static ObjectType Get(ValueType& v) { return v.GetObject(); } + static ValueType& Set(ValueType& v, ObjectType data) { return v = data; } + static ValueType& Set(ValueType& v, ObjectType data, typename ValueType::AllocatorType&) { return v = data; } +}; + +template +struct TypeHelper { + typedef typename ValueType::ConstObject ObjectType; + static bool Is(const ValueType& v) { return v.IsObject(); } + static ObjectType Get(const ValueType& v) { return v.GetObject(); } +}; + +} // namespace internal + +// Forward declarations +template class GenericArray; +template class GenericObject; + +/////////////////////////////////////////////////////////////////////////////// +// GenericValue + +//! Represents a JSON value. Use Value for UTF8 encoding and default allocator. +/*! + A JSON value can be one of 7 types. This class is a variant type supporting + these types. + + Use the Value if UTF8 and default allocator + + \tparam Encoding Encoding of the value. (Even non-string values need to have the same encoding in a document) + \tparam Allocator Allocator type for allocating memory of object, array and string. +*/ +template > +class GenericValue { +public: + //! Name-value pair in an object. + typedef GenericMember Member; + typedef Encoding EncodingType; //!< Encoding type from template parameter. + typedef Allocator AllocatorType; //!< Allocator type from template parameter. + typedef typename Encoding::Ch Ch; //!< Character type derived from Encoding. + typedef GenericStringRef StringRefType; //!< Reference to a constant string + typedef typename GenericMemberIterator::Iterator MemberIterator; //!< Member iterator for iterating in object. + typedef typename GenericMemberIterator::Iterator ConstMemberIterator; //!< Constant member iterator for iterating in object. + typedef GenericValue* ValueIterator; //!< Value iterator for iterating in array. + typedef const GenericValue* ConstValueIterator; //!< Constant value iterator for iterating in array. + typedef GenericValue ValueType; //!< Value type of itself. + typedef GenericArray Array; + typedef GenericArray ConstArray; + typedef GenericObject Object; + typedef GenericObject ConstObject; + + //!@name Constructors and destructor. + //@{ + + //! Default constructor creates a null value. + GenericValue() RAPIDJSON_NOEXCEPT : data_() { data_.f.flags = kNullFlag; } + +#if RAPIDJSON_HAS_CXX11_RVALUE_REFS + //! Move constructor in C++11 + GenericValue(GenericValue&& rhs) RAPIDJSON_NOEXCEPT : data_(rhs.data_) { + rhs.data_.f.flags = kNullFlag; // give up contents + } +#endif + +private: + //! Copy constructor is not permitted. + GenericValue(const GenericValue& rhs); + +#if RAPIDJSON_HAS_CXX11_RVALUE_REFS + //! Moving from a GenericDocument is not permitted. + template + GenericValue(GenericDocument&& rhs); + + //! Move assignment from a GenericDocument is not permitted. + template + GenericValue& operator=(GenericDocument&& rhs); +#endif + +public: + + //! Constructor with JSON value type. + /*! This creates a Value of specified type with default content. + \param type Type of the value. + \note Default content for number is zero. + */ + explicit GenericValue(Type type) RAPIDJSON_NOEXCEPT : data_() { + static const uint16_t defaultFlags[] = { + kNullFlag, kFalseFlag, kTrueFlag, kObjectFlag, kArrayFlag, kShortStringFlag, + kNumberAnyFlag + }; + RAPIDJSON_NOEXCEPT_ASSERT(type >= kNullType && type <= kNumberType); + data_.f.flags = defaultFlags[type]; + + // Use ShortString to store empty string. + if (type == kStringType) + data_.ss.SetLength(0); + } + + //! Explicit copy constructor (with allocator) + /*! Creates a copy of a Value by using the given Allocator + \tparam SourceAllocator allocator of \c rhs + \param rhs Value to copy from (read-only) + \param allocator Allocator for allocating copied elements and buffers. Commonly use GenericDocument::GetAllocator(). + \param copyConstStrings Force copying of constant strings (e.g. referencing an in-situ buffer) + \see CopyFrom() + */ + template + GenericValue(const GenericValue& rhs, Allocator& allocator, bool copyConstStrings = false) { + switch (rhs.GetType()) { + case kObjectType: { + SizeType count = rhs.data_.o.size; + Member* lm = reinterpret_cast(allocator.Malloc(count * sizeof(Member))); + const typename GenericValue::Member* rm = rhs.GetMembersPointer(); + for (SizeType i = 0; i < count; i++) { + new (&lm[i].name) GenericValue(rm[i].name, allocator, copyConstStrings); + new (&lm[i].value) GenericValue(rm[i].value, allocator, copyConstStrings); + } + data_.f.flags = kObjectFlag; + data_.o.size = data_.o.capacity = count; + SetMembersPointer(lm); + } + break; + case kArrayType: { + SizeType count = rhs.data_.a.size; + GenericValue* le = reinterpret_cast(allocator.Malloc(count * sizeof(GenericValue))); + const GenericValue* re = rhs.GetElementsPointer(); + for (SizeType i = 0; i < count; i++) + new (&le[i]) GenericValue(re[i], allocator, copyConstStrings); + data_.f.flags = kArrayFlag; + data_.a.size = data_.a.capacity = count; + SetElementsPointer(le); + } + break; + case kStringType: + if (rhs.data_.f.flags == kConstStringFlag && !copyConstStrings) { + data_.f.flags = rhs.data_.f.flags; + data_ = *reinterpret_cast(&rhs.data_); + } + else + SetStringRaw(StringRef(rhs.GetString(), rhs.GetStringLength()), allocator); + break; + default: + data_.f.flags = rhs.data_.f.flags; + data_ = *reinterpret_cast(&rhs.data_); + break; + } + } + + //! Constructor for boolean value. + /*! \param b Boolean value + \note This constructor is limited to \em real boolean values and rejects + implicitly converted types like arbitrary pointers. Use an explicit cast + to \c bool, if you want to construct a boolean JSON value in such cases. + */ +#ifndef RAPIDJSON_DOXYGEN_RUNNING // hide SFINAE from Doxygen + template + explicit GenericValue(T b, RAPIDJSON_ENABLEIF((internal::IsSame))) RAPIDJSON_NOEXCEPT // See #472 +#else + explicit GenericValue(bool b) RAPIDJSON_NOEXCEPT +#endif + : data_() { + // safe-guard against failing SFINAE + RAPIDJSON_STATIC_ASSERT((internal::IsSame::Value)); + data_.f.flags = b ? kTrueFlag : kFalseFlag; + } + + //! Constructor for int value. + explicit GenericValue(int i) RAPIDJSON_NOEXCEPT : data_() { + data_.n.i64 = i; + data_.f.flags = (i >= 0) ? (kNumberIntFlag | kUintFlag | kUint64Flag) : kNumberIntFlag; + } + + //! Constructor for unsigned value. + explicit GenericValue(unsigned u) RAPIDJSON_NOEXCEPT : data_() { + data_.n.u64 = u; + data_.f.flags = (u & 0x80000000) ? kNumberUintFlag : (kNumberUintFlag | kIntFlag | kInt64Flag); + } + + //! Constructor for int64_t value. + explicit GenericValue(int64_t i64) RAPIDJSON_NOEXCEPT : data_() { + data_.n.i64 = i64; + data_.f.flags = kNumberInt64Flag; + if (i64 >= 0) { + data_.f.flags |= kNumberUint64Flag; + if (!(static_cast(i64) & RAPIDJSON_UINT64_C2(0xFFFFFFFF, 0x00000000))) + data_.f.flags |= kUintFlag; + if (!(static_cast(i64) & RAPIDJSON_UINT64_C2(0xFFFFFFFF, 0x80000000))) + data_.f.flags |= kIntFlag; + } + else if (i64 >= static_cast(RAPIDJSON_UINT64_C2(0xFFFFFFFF, 0x80000000))) + data_.f.flags |= kIntFlag; + } + + //! Constructor for uint64_t value. + explicit GenericValue(uint64_t u64) RAPIDJSON_NOEXCEPT : data_() { + data_.n.u64 = u64; + data_.f.flags = kNumberUint64Flag; + if (!(u64 & RAPIDJSON_UINT64_C2(0x80000000, 0x00000000))) + data_.f.flags |= kInt64Flag; + if (!(u64 & RAPIDJSON_UINT64_C2(0xFFFFFFFF, 0x00000000))) + data_.f.flags |= kUintFlag; + if (!(u64 & RAPIDJSON_UINT64_C2(0xFFFFFFFF, 0x80000000))) + data_.f.flags |= kIntFlag; + } + + //! Constructor for double value. + explicit GenericValue(double d) RAPIDJSON_NOEXCEPT : data_() { data_.n.d = d; data_.f.flags = kNumberDoubleFlag; } + + //! Constructor for float value. + explicit GenericValue(float f) RAPIDJSON_NOEXCEPT : data_() { data_.n.d = static_cast(f); data_.f.flags = kNumberDoubleFlag; } + + //! Constructor for constant string (i.e. do not make a copy of string) + GenericValue(const Ch* s, SizeType length) RAPIDJSON_NOEXCEPT : data_() { SetStringRaw(StringRef(s, length)); } + + //! Constructor for constant string (i.e. do not make a copy of string) + explicit GenericValue(StringRefType s) RAPIDJSON_NOEXCEPT : data_() { SetStringRaw(s); } + + //! Constructor for copy-string (i.e. do make a copy of string) + GenericValue(const Ch* s, SizeType length, Allocator& allocator) : data_() { SetStringRaw(StringRef(s, length), allocator); } + + //! Constructor for copy-string (i.e. do make a copy of string) + GenericValue(const Ch*s, Allocator& allocator) : data_() { SetStringRaw(StringRef(s), allocator); } + +#if RAPIDJSON_HAS_STDSTRING + //! Constructor for copy-string from a string object (i.e. do make a copy of string) + /*! \note Requires the definition of the preprocessor symbol \ref RAPIDJSON_HAS_STDSTRING. + */ + GenericValue(const std::basic_string& s, Allocator& allocator) : data_() { SetStringRaw(StringRef(s), allocator); } +#endif + + //! Constructor for Array. + /*! + \param a An array obtained by \c GetArray(). + \note \c Array is always pass-by-value. + \note the source array is moved into this value and the sourec array becomes empty. + */ + GenericValue(Array a) RAPIDJSON_NOEXCEPT : data_(a.value_.data_) { + a.value_.data_ = Data(); + a.value_.data_.f.flags = kArrayFlag; + } + + //! Constructor for Object. + /*! + \param o An object obtained by \c GetObject(). + \note \c Object is always pass-by-value. + \note the source object is moved into this value and the sourec object becomes empty. + */ + GenericValue(Object o) RAPIDJSON_NOEXCEPT : data_(o.value_.data_) { + o.value_.data_ = Data(); + o.value_.data_.f.flags = kObjectFlag; + } + + //! Destructor. + /*! Need to destruct elements of array, members of object, or copy-string. + */ + ~GenericValue() { + if (Allocator::kNeedFree) { // Shortcut by Allocator's trait + switch(data_.f.flags) { + case kArrayFlag: + { + GenericValue* e = GetElementsPointer(); + for (GenericValue* v = e; v != e + data_.a.size; ++v) + v->~GenericValue(); + Allocator::Free(e); + } + break; + + case kObjectFlag: + for (MemberIterator m = MemberBegin(); m != MemberEnd(); ++m) + m->~Member(); + Allocator::Free(GetMembersPointer()); + break; + + case kCopyStringFlag: + Allocator::Free(const_cast(GetStringPointer())); + break; + + default: + break; // Do nothing for other types. + } + } + } + + //@} + + //!@name Assignment operators + //@{ + + //! Assignment with move semantics. + /*! \param rhs Source of the assignment. It will become a null value after assignment. + */ + GenericValue& operator=(GenericValue& rhs) RAPIDJSON_NOEXCEPT { + if (RAPIDJSON_LIKELY(this != &rhs)) { + this->~GenericValue(); + RawAssign(rhs); + } + return *this; + } + +#if RAPIDJSON_HAS_CXX11_RVALUE_REFS + //! Move assignment in C++11 + GenericValue& operator=(GenericValue&& rhs) RAPIDJSON_NOEXCEPT { + return *this = rhs.Move(); + } +#endif + + //! Assignment of constant string reference (no copy) + /*! \param str Constant string reference to be assigned + \note This overload is needed to avoid clashes with the generic primitive type assignment overload below. + \see GenericStringRef, operator=(T) + */ + GenericValue& operator=(StringRefType str) RAPIDJSON_NOEXCEPT { + GenericValue s(str); + return *this = s; + } + + //! Assignment with primitive types. + /*! \tparam T Either \ref Type, \c int, \c unsigned, \c int64_t, \c uint64_t + \param value The value to be assigned. + + \note The source type \c T explicitly disallows all pointer types, + especially (\c const) \ref Ch*. This helps avoiding implicitly + referencing character strings with insufficient lifetime, use + \ref SetString(const Ch*, Allocator&) (for copying) or + \ref StringRef() (to explicitly mark the pointer as constant) instead. + All other pointer types would implicitly convert to \c bool, + use \ref SetBool() instead. + */ + template + RAPIDJSON_DISABLEIF_RETURN((internal::IsPointer), (GenericValue&)) + operator=(T value) { + GenericValue v(value); + return *this = v; + } + + //! Deep-copy assignment from Value + /*! Assigns a \b copy of the Value to the current Value object + \tparam SourceAllocator Allocator type of \c rhs + \param rhs Value to copy from (read-only) + \param allocator Allocator to use for copying + \param copyConstStrings Force copying of constant strings (e.g. referencing an in-situ buffer) + */ + template + GenericValue& CopyFrom(const GenericValue& rhs, Allocator& allocator, bool copyConstStrings = false) { + RAPIDJSON_ASSERT(static_cast(this) != static_cast(&rhs)); + this->~GenericValue(); + new (this) GenericValue(rhs, allocator, copyConstStrings); + return *this; + } + + //! Exchange the contents of this value with those of other. + /*! + \param other Another value. + \note Constant complexity. + */ + GenericValue& Swap(GenericValue& other) RAPIDJSON_NOEXCEPT { + GenericValue temp; + temp.RawAssign(*this); + RawAssign(other); + other.RawAssign(temp); + return *this; + } + + //! free-standing swap function helper + /*! + Helper function to enable support for common swap implementation pattern based on \c std::swap: + \code + void swap(MyClass& a, MyClass& b) { + using std::swap; + swap(a.value, b.value); + // ... + } + \endcode + \see Swap() + */ + friend inline void swap(GenericValue& a, GenericValue& b) RAPIDJSON_NOEXCEPT { a.Swap(b); } + + //! Prepare Value for move semantics + /*! \return *this */ + GenericValue& Move() RAPIDJSON_NOEXCEPT { return *this; } + //@} + + //!@name Equal-to and not-equal-to operators + //@{ + //! Equal-to operator + /*! + \note If an object contains duplicated named member, comparing equality with any object is always \c false. + \note Complexity is quadratic in Object's member number and linear for the rest (number of all values in the subtree and total lengths of all strings). + */ + template + bool operator==(const GenericValue& rhs) const { + typedef GenericValue RhsType; + if (GetType() != rhs.GetType()) + return false; + + switch (GetType()) { + case kObjectType: // Warning: O(n^2) inner-loop + if (data_.o.size != rhs.data_.o.size) + return false; + for (ConstMemberIterator lhsMemberItr = MemberBegin(); lhsMemberItr != MemberEnd(); ++lhsMemberItr) { + typename RhsType::ConstMemberIterator rhsMemberItr = rhs.FindMember(lhsMemberItr->name); + if (rhsMemberItr == rhs.MemberEnd() || lhsMemberItr->value != rhsMemberItr->value) + return false; + } + return true; + + case kArrayType: + if (data_.a.size != rhs.data_.a.size) + return false; + for (SizeType i = 0; i < data_.a.size; i++) + if ((*this)[i] != rhs[i]) + return false; + return true; + + case kStringType: + return StringEqual(rhs); + + case kNumberType: + if (IsDouble() || rhs.IsDouble()) { + double a = GetDouble(); // May convert from integer to double. + double b = rhs.GetDouble(); // Ditto + return a >= b && a <= b; // Prevent -Wfloat-equal + } + else + return data_.n.u64 == rhs.data_.n.u64; + + default: + return true; + } + } + + //! Equal-to operator with const C-string pointer + bool operator==(const Ch* rhs) const { return *this == GenericValue(StringRef(rhs)); } + +#if RAPIDJSON_HAS_STDSTRING + //! Equal-to operator with string object + /*! \note Requires the definition of the preprocessor symbol \ref RAPIDJSON_HAS_STDSTRING. + */ + bool operator==(const std::basic_string& rhs) const { return *this == GenericValue(StringRef(rhs)); } +#endif + + //! Equal-to operator with primitive types + /*! \tparam T Either \ref Type, \c int, \c unsigned, \c int64_t, \c uint64_t, \c double, \c true, \c false + */ + template RAPIDJSON_DISABLEIF_RETURN((internal::OrExpr,internal::IsGenericValue >), (bool)) operator==(const T& rhs) const { return *this == GenericValue(rhs); } + + //! Not-equal-to operator + /*! \return !(*this == rhs) + */ + template + bool operator!=(const GenericValue& rhs) const { return !(*this == rhs); } + + //! Not-equal-to operator with const C-string pointer + bool operator!=(const Ch* rhs) const { return !(*this == rhs); } + + //! Not-equal-to operator with arbitrary types + /*! \return !(*this == rhs) + */ + template RAPIDJSON_DISABLEIF_RETURN((internal::IsGenericValue), (bool)) operator!=(const T& rhs) const { return !(*this == rhs); } + + //! Equal-to operator with arbitrary types (symmetric version) + /*! \return (rhs == lhs) + */ + template friend RAPIDJSON_DISABLEIF_RETURN((internal::IsGenericValue), (bool)) operator==(const T& lhs, const GenericValue& rhs) { return rhs == lhs; } + + //! Not-Equal-to operator with arbitrary types (symmetric version) + /*! \return !(rhs == lhs) + */ + template friend RAPIDJSON_DISABLEIF_RETURN((internal::IsGenericValue), (bool)) operator!=(const T& lhs, const GenericValue& rhs) { return !(rhs == lhs); } + //@} + + //!@name Type + //@{ + + Type GetType() const { return static_cast(data_.f.flags & kTypeMask); } + bool IsNull() const { return data_.f.flags == kNullFlag; } + bool IsFalse() const { return data_.f.flags == kFalseFlag; } + bool IsTrue() const { return data_.f.flags == kTrueFlag; } + bool IsBool() const { return (data_.f.flags & kBoolFlag) != 0; } + bool IsObject() const { return data_.f.flags == kObjectFlag; } + bool IsArray() const { return data_.f.flags == kArrayFlag; } + bool IsNumber() const { return (data_.f.flags & kNumberFlag) != 0; } + bool IsInt() const { return (data_.f.flags & kIntFlag) != 0; } + bool IsUint() const { return (data_.f.flags & kUintFlag) != 0; } + bool IsInt64() const { return (data_.f.flags & kInt64Flag) != 0; } + bool IsUint64() const { return (data_.f.flags & kUint64Flag) != 0; } + bool IsDouble() const { return (data_.f.flags & kDoubleFlag) != 0; } + bool IsString() const { return (data_.f.flags & kStringFlag) != 0; } + + // Checks whether a number can be losslessly converted to a double. + bool IsLosslessDouble() const { + if (!IsNumber()) return false; + if (IsUint64()) { + uint64_t u = GetUint64(); + volatile double d = static_cast(u); + return (d >= 0.0) + && (d < static_cast((std::numeric_limits::max)())) + && (u == static_cast(d)); + } + if (IsInt64()) { + int64_t i = GetInt64(); + volatile double d = static_cast(i); + return (d >= static_cast((std::numeric_limits::min)())) + && (d < static_cast((std::numeric_limits::max)())) + && (i == static_cast(d)); + } + return true; // double, int, uint are always lossless + } + + // Checks whether a number is a float (possible lossy). + bool IsFloat() const { + if ((data_.f.flags & kDoubleFlag) == 0) + return false; + double d = GetDouble(); + return d >= -3.4028234e38 && d <= 3.4028234e38; + } + // Checks whether a number can be losslessly converted to a float. + bool IsLosslessFloat() const { + if (!IsNumber()) return false; + double a = GetDouble(); + if (a < static_cast(-(std::numeric_limits::max)()) + || a > static_cast((std::numeric_limits::max)())) + return false; + double b = static_cast(static_cast(a)); + return a >= b && a <= b; // Prevent -Wfloat-equal + } + + //@} + + //!@name Null + //@{ + + GenericValue& SetNull() { this->~GenericValue(); new (this) GenericValue(); return *this; } + + //@} + + //!@name Bool + //@{ + + bool GetBool() const { RAPIDJSON_ASSERT(IsBool()); return data_.f.flags == kTrueFlag; } + //!< Set boolean value + /*! \post IsBool() == true */ + GenericValue& SetBool(bool b) { this->~GenericValue(); new (this) GenericValue(b); return *this; } + + //@} + + //!@name Object + //@{ + + //! Set this value as an empty object. + /*! \post IsObject() == true */ + GenericValue& SetObject() { this->~GenericValue(); new (this) GenericValue(kObjectType); return *this; } + + //! Get the number of members in the object. + SizeType MemberCount() const { RAPIDJSON_ASSERT(IsObject()); return data_.o.size; } + + //! Get the capacity of object. + SizeType MemberCapacity() const { RAPIDJSON_ASSERT(IsObject()); return data_.o.capacity; } + + //! Check whether the object is empty. + bool ObjectEmpty() const { RAPIDJSON_ASSERT(IsObject()); return data_.o.size == 0; } + + //! Get a value from an object associated with the name. + /*! \pre IsObject() == true + \tparam T Either \c Ch or \c const \c Ch (template used for disambiguation with \ref operator[](SizeType)) + \note In version 0.1x, if the member is not found, this function returns a null value. This makes issue 7. + Since 0.2, if the name is not correct, it will assert. + If user is unsure whether a member exists, user should use HasMember() first. + A better approach is to use FindMember(). + \note Linear time complexity. + */ + template + RAPIDJSON_DISABLEIF_RETURN((internal::NotExpr::Type, Ch> >),(GenericValue&)) operator[](T* name) { + GenericValue n(StringRef(name)); + return (*this)[n]; + } + template + RAPIDJSON_DISABLEIF_RETURN((internal::NotExpr::Type, Ch> >),(const GenericValue&)) operator[](T* name) const { return const_cast(*this)[name]; } + + //! Get a value from an object associated with the name. + /*! \pre IsObject() == true + \tparam SourceAllocator Allocator of the \c name value + + \note Compared to \ref operator[](T*), this version is faster because it does not need a StrLen(). + And it can also handle strings with embedded null characters. + + \note Linear time complexity. + */ + template + GenericValue& operator[](const GenericValue& name) { + MemberIterator member = FindMember(name); + if (member != MemberEnd()) + return member->value; + else { + RAPIDJSON_ASSERT(false); // see above note + + // This will generate -Wexit-time-destructors in clang + // static GenericValue NullValue; + // return NullValue; + + // Use static buffer and placement-new to prevent destruction + static char buffer[sizeof(GenericValue)]; + return *new (buffer) GenericValue(); + } + } + template + const GenericValue& operator[](const GenericValue& name) const { return const_cast(*this)[name]; } + +#if RAPIDJSON_HAS_STDSTRING + //! Get a value from an object associated with name (string object). + GenericValue& operator[](const std::basic_string& name) { return (*this)[GenericValue(StringRef(name))]; } + const GenericValue& operator[](const std::basic_string& name) const { return (*this)[GenericValue(StringRef(name))]; } +#endif + + //! Const member iterator + /*! \pre IsObject() == true */ + ConstMemberIterator MemberBegin() const { RAPIDJSON_ASSERT(IsObject()); return ConstMemberIterator(GetMembersPointer()); } + //! Const \em past-the-end member iterator + /*! \pre IsObject() == true */ + ConstMemberIterator MemberEnd() const { RAPIDJSON_ASSERT(IsObject()); return ConstMemberIterator(GetMembersPointer() + data_.o.size); } + //! Member iterator + /*! \pre IsObject() == true */ + MemberIterator MemberBegin() { RAPIDJSON_ASSERT(IsObject()); return MemberIterator(GetMembersPointer()); } + //! \em Past-the-end member iterator + /*! \pre IsObject() == true */ + MemberIterator MemberEnd() { RAPIDJSON_ASSERT(IsObject()); return MemberIterator(GetMembersPointer() + data_.o.size); } + + //! Request the object to have enough capacity to store members. + /*! \param newCapacity The capacity that the object at least need to have. + \param allocator Allocator for reallocating memory. It must be the same one as used before. Commonly use GenericDocument::GetAllocator(). + \return The value itself for fluent API. + \note Linear time complexity. + */ + GenericValue& MemberReserve(SizeType newCapacity, Allocator &allocator) { + RAPIDJSON_ASSERT(IsObject()); + if (newCapacity > data_.o.capacity) { + SetMembersPointer(reinterpret_cast(allocator.Realloc(GetMembersPointer(), data_.o.capacity * sizeof(Member), newCapacity * sizeof(Member)))); + data_.o.capacity = newCapacity; + } + return *this; + } + + //! Check whether a member exists in the object. + /*! + \param name Member name to be searched. + \pre IsObject() == true + \return Whether a member with that name exists. + \note It is better to use FindMember() directly if you need the obtain the value as well. + \note Linear time complexity. + */ + bool HasMember(const Ch* name) const { return FindMember(name) != MemberEnd(); } + +#if RAPIDJSON_HAS_STDSTRING + //! Check whether a member exists in the object with string object. + /*! + \param name Member name to be searched. + \pre IsObject() == true + \return Whether a member with that name exists. + \note It is better to use FindMember() directly if you need the obtain the value as well. + \note Linear time complexity. + */ + bool HasMember(const std::basic_string& name) const { return FindMember(name) != MemberEnd(); } +#endif + + //! Check whether a member exists in the object with GenericValue name. + /*! + This version is faster because it does not need a StrLen(). It can also handle string with null character. + \param name Member name to be searched. + \pre IsObject() == true + \return Whether a member with that name exists. + \note It is better to use FindMember() directly if you need the obtain the value as well. + \note Linear time complexity. + */ + template + bool HasMember(const GenericValue& name) const { return FindMember(name) != MemberEnd(); } + + //! Find member by name. + /*! + \param name Member name to be searched. + \pre IsObject() == true + \return Iterator to member, if it exists. + Otherwise returns \ref MemberEnd(). + + \note Earlier versions of Rapidjson returned a \c NULL pointer, in case + the requested member doesn't exist. For consistency with e.g. + \c std::map, this has been changed to MemberEnd() now. + \note Linear time complexity. + */ + MemberIterator FindMember(const Ch* name) { + GenericValue n(StringRef(name)); + return FindMember(n); + } + + ConstMemberIterator FindMember(const Ch* name) const { return const_cast(*this).FindMember(name); } + + //! Find member by name. + /*! + This version is faster because it does not need a StrLen(). It can also handle string with null character. + \param name Member name to be searched. + \pre IsObject() == true + \return Iterator to member, if it exists. + Otherwise returns \ref MemberEnd(). + + \note Earlier versions of Rapidjson returned a \c NULL pointer, in case + the requested member doesn't exist. For consistency with e.g. + \c std::map, this has been changed to MemberEnd() now. + \note Linear time complexity. + */ + template + MemberIterator FindMember(const GenericValue& name) { + RAPIDJSON_ASSERT(IsObject()); + RAPIDJSON_ASSERT(name.IsString()); + MemberIterator member = MemberBegin(); + for ( ; member != MemberEnd(); ++member) + if (name.StringEqual(member->name)) + break; + return member; + } + template ConstMemberIterator FindMember(const GenericValue& name) const { return const_cast(*this).FindMember(name); } + +#if RAPIDJSON_HAS_STDSTRING + //! Find member by string object name. + /*! + \param name Member name to be searched. + \pre IsObject() == true + \return Iterator to member, if it exists. + Otherwise returns \ref MemberEnd(). + */ + MemberIterator FindMember(const std::basic_string& name) { return FindMember(GenericValue(StringRef(name))); } + ConstMemberIterator FindMember(const std::basic_string& name) const { return FindMember(GenericValue(StringRef(name))); } +#endif + + //! Add a member (name-value pair) to the object. + /*! \param name A string value as name of member. + \param value Value of any type. + \param allocator Allocator for reallocating memory. It must be the same one as used before. Commonly use GenericDocument::GetAllocator(). + \return The value itself for fluent API. + \note The ownership of \c name and \c value will be transferred to this object on success. + \pre IsObject() && name.IsString() + \post name.IsNull() && value.IsNull() + \note Amortized Constant time complexity. + */ + GenericValue& AddMember(GenericValue& name, GenericValue& value, Allocator& allocator) { + RAPIDJSON_ASSERT(IsObject()); + RAPIDJSON_ASSERT(name.IsString()); + + ObjectData& o = data_.o; + if (o.size >= o.capacity) + MemberReserve(o.capacity == 0 ? kDefaultObjectCapacity : (o.capacity + (o.capacity + 1) / 2), allocator); + Member* members = GetMembersPointer(); + members[o.size].name.RawAssign(name); + members[o.size].value.RawAssign(value); + o.size++; + return *this; + } + + //! Add a constant string value as member (name-value pair) to the object. + /*! \param name A string value as name of member. + \param value constant string reference as value of member. + \param allocator Allocator for reallocating memory. It must be the same one as used before. Commonly use GenericDocument::GetAllocator(). + \return The value itself for fluent API. + \pre IsObject() + \note This overload is needed to avoid clashes with the generic primitive type AddMember(GenericValue&,T,Allocator&) overload below. + \note Amortized Constant time complexity. + */ + GenericValue& AddMember(GenericValue& name, StringRefType value, Allocator& allocator) { + GenericValue v(value); + return AddMember(name, v, allocator); + } + +#if RAPIDJSON_HAS_STDSTRING + //! Add a string object as member (name-value pair) to the object. + /*! \param name A string value as name of member. + \param value constant string reference as value of member. + \param allocator Allocator for reallocating memory. It must be the same one as used before. Commonly use GenericDocument::GetAllocator(). + \return The value itself for fluent API. + \pre IsObject() + \note This overload is needed to avoid clashes with the generic primitive type AddMember(GenericValue&,T,Allocator&) overload below. + \note Amortized Constant time complexity. + */ + GenericValue& AddMember(GenericValue& name, std::basic_string& value, Allocator& allocator) { + GenericValue v(value, allocator); + return AddMember(name, v, allocator); + } +#endif + + //! Add any primitive value as member (name-value pair) to the object. + /*! \tparam T Either \ref Type, \c int, \c unsigned, \c int64_t, \c uint64_t + \param name A string value as name of member. + \param value Value of primitive type \c T as value of member + \param allocator Allocator for reallocating memory. Commonly use GenericDocument::GetAllocator(). + \return The value itself for fluent API. + \pre IsObject() + + \note The source type \c T explicitly disallows all pointer types, + especially (\c const) \ref Ch*. This helps avoiding implicitly + referencing character strings with insufficient lifetime, use + \ref AddMember(StringRefType, GenericValue&, Allocator&) or \ref + AddMember(StringRefType, StringRefType, Allocator&). + All other pointer types would implicitly convert to \c bool, + use an explicit cast instead, if needed. + \note Amortized Constant time complexity. + */ + template + RAPIDJSON_DISABLEIF_RETURN((internal::OrExpr, internal::IsGenericValue >), (GenericValue&)) + AddMember(GenericValue& name, T value, Allocator& allocator) { + GenericValue v(value); + return AddMember(name, v, allocator); + } + +#if RAPIDJSON_HAS_CXX11_RVALUE_REFS + GenericValue& AddMember(GenericValue&& name, GenericValue&& value, Allocator& allocator) { + return AddMember(name, value, allocator); + } + GenericValue& AddMember(GenericValue&& name, GenericValue& value, Allocator& allocator) { + return AddMember(name, value, allocator); + } + GenericValue& AddMember(GenericValue& name, GenericValue&& value, Allocator& allocator) { + return AddMember(name, value, allocator); + } + GenericValue& AddMember(StringRefType name, GenericValue&& value, Allocator& allocator) { + GenericValue n(name); + return AddMember(n, value, allocator); + } +#endif // RAPIDJSON_HAS_CXX11_RVALUE_REFS + + + //! Add a member (name-value pair) to the object. + /*! \param name A constant string reference as name of member. + \param value Value of any type. + \param allocator Allocator for reallocating memory. It must be the same one as used before. Commonly use GenericDocument::GetAllocator(). + \return The value itself for fluent API. + \note The ownership of \c value will be transferred to this object on success. + \pre IsObject() + \post value.IsNull() + \note Amortized Constant time complexity. + */ + GenericValue& AddMember(StringRefType name, GenericValue& value, Allocator& allocator) { + GenericValue n(name); + return AddMember(n, value, allocator); + } + + //! Add a constant string value as member (name-value pair) to the object. + /*! \param name A constant string reference as name of member. + \param value constant string reference as value of member. + \param allocator Allocator for reallocating memory. It must be the same one as used before. Commonly use GenericDocument::GetAllocator(). + \return The value itself for fluent API. + \pre IsObject() + \note This overload is needed to avoid clashes with the generic primitive type AddMember(StringRefType,T,Allocator&) overload below. + \note Amortized Constant time complexity. + */ + GenericValue& AddMember(StringRefType name, StringRefType value, Allocator& allocator) { + GenericValue v(value); + return AddMember(name, v, allocator); + } + + //! Add any primitive value as member (name-value pair) to the object. + /*! \tparam T Either \ref Type, \c int, \c unsigned, \c int64_t, \c uint64_t + \param name A constant string reference as name of member. + \param value Value of primitive type \c T as value of member + \param allocator Allocator for reallocating memory. Commonly use GenericDocument::GetAllocator(). + \return The value itself for fluent API. + \pre IsObject() + + \note The source type \c T explicitly disallows all pointer types, + especially (\c const) \ref Ch*. This helps avoiding implicitly + referencing character strings with insufficient lifetime, use + \ref AddMember(StringRefType, GenericValue&, Allocator&) or \ref + AddMember(StringRefType, StringRefType, Allocator&). + All other pointer types would implicitly convert to \c bool, + use an explicit cast instead, if needed. + \note Amortized Constant time complexity. + */ + template + RAPIDJSON_DISABLEIF_RETURN((internal::OrExpr, internal::IsGenericValue >), (GenericValue&)) + AddMember(StringRefType name, T value, Allocator& allocator) { + GenericValue n(name); + return AddMember(n, value, allocator); + } + + //! Remove all members in the object. + /*! This function do not deallocate memory in the object, i.e. the capacity is unchanged. + \note Linear time complexity. + */ + void RemoveAllMembers() { + RAPIDJSON_ASSERT(IsObject()); + for (MemberIterator m = MemberBegin(); m != MemberEnd(); ++m) + m->~Member(); + data_.o.size = 0; + } + + //! Remove a member in object by its name. + /*! \param name Name of member to be removed. + \return Whether the member existed. + \note This function may reorder the object members. Use \ref + EraseMember(ConstMemberIterator) if you need to preserve the + relative order of the remaining members. + \note Linear time complexity. + */ + bool RemoveMember(const Ch* name) { + GenericValue n(StringRef(name)); + return RemoveMember(n); + } + +#if RAPIDJSON_HAS_STDSTRING + bool RemoveMember(const std::basic_string& name) { return RemoveMember(GenericValue(StringRef(name))); } +#endif + + template + bool RemoveMember(const GenericValue& name) { + MemberIterator m = FindMember(name); + if (m != MemberEnd()) { + RemoveMember(m); + return true; + } + else + return false; + } + + //! Remove a member in object by iterator. + /*! \param m member iterator (obtained by FindMember() or MemberBegin()). + \return the new iterator after removal. + \note This function may reorder the object members. Use \ref + EraseMember(ConstMemberIterator) if you need to preserve the + relative order of the remaining members. + \note Constant time complexity. + */ + MemberIterator RemoveMember(MemberIterator m) { + RAPIDJSON_ASSERT(IsObject()); + RAPIDJSON_ASSERT(data_.o.size > 0); + RAPIDJSON_ASSERT(GetMembersPointer() != 0); + RAPIDJSON_ASSERT(m >= MemberBegin() && m < MemberEnd()); + + MemberIterator last(GetMembersPointer() + (data_.o.size - 1)); + if (data_.o.size > 1 && m != last) + *m = *last; // Move the last one to this place + else + m->~Member(); // Only one left, just destroy + --data_.o.size; + return m; + } + + //! Remove a member from an object by iterator. + /*! \param pos iterator to the member to remove + \pre IsObject() == true && \ref MemberBegin() <= \c pos < \ref MemberEnd() + \return Iterator following the removed element. + If the iterator \c pos refers to the last element, the \ref MemberEnd() iterator is returned. + \note This function preserves the relative order of the remaining object + members. If you do not need this, use the more efficient \ref RemoveMember(MemberIterator). + \note Linear time complexity. + */ + MemberIterator EraseMember(ConstMemberIterator pos) { + return EraseMember(pos, pos +1); + } + + //! Remove members in the range [first, last) from an object. + /*! \param first iterator to the first member to remove + \param last iterator following the last member to remove + \pre IsObject() == true && \ref MemberBegin() <= \c first <= \c last <= \ref MemberEnd() + \return Iterator following the last removed element. + \note This function preserves the relative order of the remaining object + members. + \note Linear time complexity. + */ + MemberIterator EraseMember(ConstMemberIterator first, ConstMemberIterator last) { + RAPIDJSON_ASSERT(IsObject()); + RAPIDJSON_ASSERT(data_.o.size > 0); + RAPIDJSON_ASSERT(GetMembersPointer() != 0); + RAPIDJSON_ASSERT(first >= MemberBegin()); + RAPIDJSON_ASSERT(first <= last); + RAPIDJSON_ASSERT(last <= MemberEnd()); + + MemberIterator pos = MemberBegin() + (first - MemberBegin()); + for (MemberIterator itr = pos; itr != last; ++itr) + itr->~Member(); + std::memmove(static_cast(&*pos), &*last, static_cast(MemberEnd() - last) * sizeof(Member)); + data_.o.size -= static_cast(last - first); + return pos; + } + + //! Erase a member in object by its name. + /*! \param name Name of member to be removed. + \return Whether the member existed. + \note Linear time complexity. + */ + bool EraseMember(const Ch* name) { + GenericValue n(StringRef(name)); + return EraseMember(n); + } + +#if RAPIDJSON_HAS_STDSTRING + bool EraseMember(const std::basic_string& name) { return EraseMember(GenericValue(StringRef(name))); } +#endif + + template + bool EraseMember(const GenericValue& name) { + MemberIterator m = FindMember(name); + if (m != MemberEnd()) { + EraseMember(m); + return true; + } + else + return false; + } + + Object GetObject() { RAPIDJSON_ASSERT(IsObject()); return Object(*this); } + ConstObject GetObject() const { RAPIDJSON_ASSERT(IsObject()); return ConstObject(*this); } + + //@} + + //!@name Array + //@{ + + //! Set this value as an empty array. + /*! \post IsArray == true */ + GenericValue& SetArray() { this->~GenericValue(); new (this) GenericValue(kArrayType); return *this; } + + //! Get the number of elements in array. + SizeType Size() const { RAPIDJSON_ASSERT(IsArray()); return data_.a.size; } + + //! Get the capacity of array. + SizeType Capacity() const { RAPIDJSON_ASSERT(IsArray()); return data_.a.capacity; } + + //! Check whether the array is empty. + bool Empty() const { RAPIDJSON_ASSERT(IsArray()); return data_.a.size == 0; } + + //! Remove all elements in the array. + /*! This function do not deallocate memory in the array, i.e. the capacity is unchanged. + \note Linear time complexity. + */ + void Clear() { + RAPIDJSON_ASSERT(IsArray()); + GenericValue* e = GetElementsPointer(); + for (GenericValue* v = e; v != e + data_.a.size; ++v) + v->~GenericValue(); + data_.a.size = 0; + } + + //! Get an element from array by index. + /*! \pre IsArray() == true + \param index Zero-based index of element. + \see operator[](T*) + */ + GenericValue& operator[](SizeType index) { + RAPIDJSON_ASSERT(IsArray()); + RAPIDJSON_ASSERT(index < data_.a.size); + return GetElementsPointer()[index]; + } + const GenericValue& operator[](SizeType index) const { return const_cast(*this)[index]; } + + //! Element iterator + /*! \pre IsArray() == true */ + ValueIterator Begin() { RAPIDJSON_ASSERT(IsArray()); return GetElementsPointer(); } + //! \em Past-the-end element iterator + /*! \pre IsArray() == true */ + ValueIterator End() { RAPIDJSON_ASSERT(IsArray()); return GetElementsPointer() + data_.a.size; } + //! Constant element iterator + /*! \pre IsArray() == true */ + ConstValueIterator Begin() const { return const_cast(*this).Begin(); } + //! Constant \em past-the-end element iterator + /*! \pre IsArray() == true */ + ConstValueIterator End() const { return const_cast(*this).End(); } + + //! Request the array to have enough capacity to store elements. + /*! \param newCapacity The capacity that the array at least need to have. + \param allocator Allocator for reallocating memory. It must be the same one as used before. Commonly use GenericDocument::GetAllocator(). + \return The value itself for fluent API. + \note Linear time complexity. + */ + GenericValue& Reserve(SizeType newCapacity, Allocator &allocator) { + RAPIDJSON_ASSERT(IsArray()); + if (newCapacity > data_.a.capacity) { + SetElementsPointer(reinterpret_cast(allocator.Realloc(GetElementsPointer(), data_.a.capacity * sizeof(GenericValue), newCapacity * sizeof(GenericValue)))); + data_.a.capacity = newCapacity; + } + return *this; + } + + //! Append a GenericValue at the end of the array. + /*! \param value Value to be appended. + \param allocator Allocator for reallocating memory. It must be the same one as used before. Commonly use GenericDocument::GetAllocator(). + \pre IsArray() == true + \post value.IsNull() == true + \return The value itself for fluent API. + \note The ownership of \c value will be transferred to this array on success. + \note If the number of elements to be appended is known, calls Reserve() once first may be more efficient. + \note Amortized constant time complexity. + */ + GenericValue& PushBack(GenericValue& value, Allocator& allocator) { + RAPIDJSON_ASSERT(IsArray()); + if (data_.a.size >= data_.a.capacity) + Reserve(data_.a.capacity == 0 ? kDefaultArrayCapacity : (data_.a.capacity + (data_.a.capacity + 1) / 2), allocator); + GetElementsPointer()[data_.a.size++].RawAssign(value); + return *this; + } + +#if RAPIDJSON_HAS_CXX11_RVALUE_REFS + GenericValue& PushBack(GenericValue&& value, Allocator& allocator) { + return PushBack(value, allocator); + } +#endif // RAPIDJSON_HAS_CXX11_RVALUE_REFS + + //! Append a constant string reference at the end of the array. + /*! \param value Constant string reference to be appended. + \param allocator Allocator for reallocating memory. It must be the same one used previously. Commonly use GenericDocument::GetAllocator(). + \pre IsArray() == true + \return The value itself for fluent API. + \note If the number of elements to be appended is known, calls Reserve() once first may be more efficient. + \note Amortized constant time complexity. + \see GenericStringRef + */ + GenericValue& PushBack(StringRefType value, Allocator& allocator) { + return (*this).template PushBack(value, allocator); + } + + //! Append a primitive value at the end of the array. + /*! \tparam T Either \ref Type, \c int, \c unsigned, \c int64_t, \c uint64_t + \param value Value of primitive type T to be appended. + \param allocator Allocator for reallocating memory. It must be the same one as used before. Commonly use GenericDocument::GetAllocator(). + \pre IsArray() == true + \return The value itself for fluent API. + \note If the number of elements to be appended is known, calls Reserve() once first may be more efficient. + + \note The source type \c T explicitly disallows all pointer types, + especially (\c const) \ref Ch*. This helps avoiding implicitly + referencing character strings with insufficient lifetime, use + \ref PushBack(GenericValue&, Allocator&) or \ref + PushBack(StringRefType, Allocator&). + All other pointer types would implicitly convert to \c bool, + use an explicit cast instead, if needed. + \note Amortized constant time complexity. + */ + template + RAPIDJSON_DISABLEIF_RETURN((internal::OrExpr, internal::IsGenericValue >), (GenericValue&)) + PushBack(T value, Allocator& allocator) { + GenericValue v(value); + return PushBack(v, allocator); + } + + //! Remove the last element in the array. + /*! + \note Constant time complexity. + */ + GenericValue& PopBack() { + RAPIDJSON_ASSERT(IsArray()); + RAPIDJSON_ASSERT(!Empty()); + GetElementsPointer()[--data_.a.size].~GenericValue(); + return *this; + } + + //! Remove an element of array by iterator. + /*! + \param pos iterator to the element to remove + \pre IsArray() == true && \ref Begin() <= \c pos < \ref End() + \return Iterator following the removed element. If the iterator pos refers to the last element, the End() iterator is returned. + \note Linear time complexity. + */ + ValueIterator Erase(ConstValueIterator pos) { + return Erase(pos, pos + 1); + } + + //! Remove elements in the range [first, last) of the array. + /*! + \param first iterator to the first element to remove + \param last iterator following the last element to remove + \pre IsArray() == true && \ref Begin() <= \c first <= \c last <= \ref End() + \return Iterator following the last removed element. + \note Linear time complexity. + */ + ValueIterator Erase(ConstValueIterator first, ConstValueIterator last) { + RAPIDJSON_ASSERT(IsArray()); + RAPIDJSON_ASSERT(data_.a.size > 0); + RAPIDJSON_ASSERT(GetElementsPointer() != 0); + RAPIDJSON_ASSERT(first >= Begin()); + RAPIDJSON_ASSERT(first <= last); + RAPIDJSON_ASSERT(last <= End()); + ValueIterator pos = Begin() + (first - Begin()); + for (ValueIterator itr = pos; itr != last; ++itr) + itr->~GenericValue(); + std::memmove(static_cast(pos), last, static_cast(End() - last) * sizeof(GenericValue)); + data_.a.size -= static_cast(last - first); + return pos; + } + + Array GetArray() { RAPIDJSON_ASSERT(IsArray()); return Array(*this); } + ConstArray GetArray() const { RAPIDJSON_ASSERT(IsArray()); return ConstArray(*this); } + + //@} + + //!@name Number + //@{ + + int GetInt() const { RAPIDJSON_ASSERT(data_.f.flags & kIntFlag); return data_.n.i.i; } + unsigned GetUint() const { RAPIDJSON_ASSERT(data_.f.flags & kUintFlag); return data_.n.u.u; } + int64_t GetInt64() const { RAPIDJSON_ASSERT(data_.f.flags & kInt64Flag); return data_.n.i64; } + uint64_t GetUint64() const { RAPIDJSON_ASSERT(data_.f.flags & kUint64Flag); return data_.n.u64; } + + //! Get the value as double type. + /*! \note If the value is 64-bit integer type, it may lose precision. Use \c IsLosslessDouble() to check whether the converison is lossless. + */ + double GetDouble() const { + RAPIDJSON_ASSERT(IsNumber()); + if ((data_.f.flags & kDoubleFlag) != 0) return data_.n.d; // exact type, no conversion. + if ((data_.f.flags & kIntFlag) != 0) return data_.n.i.i; // int -> double + if ((data_.f.flags & kUintFlag) != 0) return data_.n.u.u; // unsigned -> double + if ((data_.f.flags & kInt64Flag) != 0) return static_cast(data_.n.i64); // int64_t -> double (may lose precision) + RAPIDJSON_ASSERT((data_.f.flags & kUint64Flag) != 0); return static_cast(data_.n.u64); // uint64_t -> double (may lose precision) + } + + //! Get the value as float type. + /*! \note If the value is 64-bit integer type, it may lose precision. Use \c IsLosslessFloat() to check whether the converison is lossless. + */ + float GetFloat() const { + return static_cast(GetDouble()); + } + + GenericValue& SetInt(int i) { this->~GenericValue(); new (this) GenericValue(i); return *this; } + GenericValue& SetUint(unsigned u) { this->~GenericValue(); new (this) GenericValue(u); return *this; } + GenericValue& SetInt64(int64_t i64) { this->~GenericValue(); new (this) GenericValue(i64); return *this; } + GenericValue& SetUint64(uint64_t u64) { this->~GenericValue(); new (this) GenericValue(u64); return *this; } + GenericValue& SetDouble(double d) { this->~GenericValue(); new (this) GenericValue(d); return *this; } + GenericValue& SetFloat(float f) { this->~GenericValue(); new (this) GenericValue(static_cast(f)); return *this; } + + //@} + + //!@name String + //@{ + + const Ch* GetString() const { RAPIDJSON_ASSERT(IsString()); return (data_.f.flags & kInlineStrFlag) ? data_.ss.str : GetStringPointer(); } + + //! Get the length of string. + /*! Since rapidjson permits "\\u0000" in the json string, strlen(v.GetString()) may not equal to v.GetStringLength(). + */ + SizeType GetStringLength() const { RAPIDJSON_ASSERT(IsString()); return ((data_.f.flags & kInlineStrFlag) ? (data_.ss.GetLength()) : data_.s.length); } + + //! Set this value as a string without copying source string. + /*! This version has better performance with supplied length, and also support string containing null character. + \param s source string pointer. + \param length The length of source string, excluding the trailing null terminator. + \return The value itself for fluent API. + \post IsString() == true && GetString() == s && GetStringLength() == length + \see SetString(StringRefType) + */ + GenericValue& SetString(const Ch* s, SizeType length) { return SetString(StringRef(s, length)); } + + //! Set this value as a string without copying source string. + /*! \param s source string reference + \return The value itself for fluent API. + \post IsString() == true && GetString() == s && GetStringLength() == s.length + */ + GenericValue& SetString(StringRefType s) { this->~GenericValue(); SetStringRaw(s); return *this; } + + //! Set this value as a string by copying from source string. + /*! This version has better performance with supplied length, and also support string containing null character. + \param s source string. + \param length The length of source string, excluding the trailing null terminator. + \param allocator Allocator for allocating copied buffer. Commonly use GenericDocument::GetAllocator(). + \return The value itself for fluent API. + \post IsString() == true && GetString() != s && strcmp(GetString(),s) == 0 && GetStringLength() == length + */ + GenericValue& SetString(const Ch* s, SizeType length, Allocator& allocator) { return SetString(StringRef(s, length), allocator); } + + //! Set this value as a string by copying from source string. + /*! \param s source string. + \param allocator Allocator for allocating copied buffer. Commonly use GenericDocument::GetAllocator(). + \return The value itself for fluent API. + \post IsString() == true && GetString() != s && strcmp(GetString(),s) == 0 && GetStringLength() == length + */ + GenericValue& SetString(const Ch* s, Allocator& allocator) { return SetString(StringRef(s), allocator); } + + //! Set this value as a string by copying from source string. + /*! \param s source string reference + \param allocator Allocator for allocating copied buffer. Commonly use GenericDocument::GetAllocator(). + \return The value itself for fluent API. + \post IsString() == true && GetString() != s.s && strcmp(GetString(),s) == 0 && GetStringLength() == length + */ + GenericValue& SetString(StringRefType s, Allocator& allocator) { this->~GenericValue(); SetStringRaw(s, allocator); return *this; } + +#if RAPIDJSON_HAS_STDSTRING + //! Set this value as a string by copying from source string. + /*! \param s source string. + \param allocator Allocator for allocating copied buffer. Commonly use GenericDocument::GetAllocator(). + \return The value itself for fluent API. + \post IsString() == true && GetString() != s.data() && strcmp(GetString(),s.data() == 0 && GetStringLength() == s.size() + \note Requires the definition of the preprocessor symbol \ref RAPIDJSON_HAS_STDSTRING. + */ + GenericValue& SetString(const std::basic_string& s, Allocator& allocator) { return SetString(StringRef(s), allocator); } +#endif + + //@} + + //!@name Array + //@{ + + //! Templated version for checking whether this value is type T. + /*! + \tparam T Either \c bool, \c int, \c unsigned, \c int64_t, \c uint64_t, \c double, \c float, \c const \c char*, \c std::basic_string + */ + template + bool Is() const { return internal::TypeHelper::Is(*this); } + + template + T Get() const { return internal::TypeHelper::Get(*this); } + + template + T Get() { return internal::TypeHelper::Get(*this); } + + template + ValueType& Set(const T& data) { return internal::TypeHelper::Set(*this, data); } + + template + ValueType& Set(const T& data, AllocatorType& allocator) { return internal::TypeHelper::Set(*this, data, allocator); } + + //@} + + //! Generate events of this value to a Handler. + /*! This function adopts the GoF visitor pattern. + Typical usage is to output this JSON value as JSON text via Writer, which is a Handler. + It can also be used to deep clone this value via GenericDocument, which is also a Handler. + \tparam Handler type of handler. + \param handler An object implementing concept Handler. + */ + template + bool Accept(Handler& handler) const { + switch(GetType()) { + case kNullType: return handler.Null(); + case kFalseType: return handler.Bool(false); + case kTrueType: return handler.Bool(true); + + case kObjectType: + if (RAPIDJSON_UNLIKELY(!handler.StartObject())) + return false; + for (ConstMemberIterator m = MemberBegin(); m != MemberEnd(); ++m) { + RAPIDJSON_ASSERT(m->name.IsString()); // User may change the type of name by MemberIterator. + if (RAPIDJSON_UNLIKELY(!handler.Key(m->name.GetString(), m->name.GetStringLength(), (m->name.data_.f.flags & kCopyFlag) != 0))) + return false; + if (RAPIDJSON_UNLIKELY(!m->value.Accept(handler))) + return false; + } + return handler.EndObject(data_.o.size); + + case kArrayType: + if (RAPIDJSON_UNLIKELY(!handler.StartArray())) + return false; + for (const GenericValue* v = Begin(); v != End(); ++v) + if (RAPIDJSON_UNLIKELY(!v->Accept(handler))) + return false; + return handler.EndArray(data_.a.size); + + case kStringType: + return handler.String(GetString(), GetStringLength(), (data_.f.flags & kCopyFlag) != 0); + + default: + RAPIDJSON_ASSERT(GetType() == kNumberType); + if (IsDouble()) return handler.Double(data_.n.d); + else if (IsInt()) return handler.Int(data_.n.i.i); + else if (IsUint()) return handler.Uint(data_.n.u.u); + else if (IsInt64()) return handler.Int64(data_.n.i64); + else return handler.Uint64(data_.n.u64); + } + } + +private: + template friend class GenericValue; + template friend class GenericDocument; + + enum { + kBoolFlag = 0x0008, + kNumberFlag = 0x0010, + kIntFlag = 0x0020, + kUintFlag = 0x0040, + kInt64Flag = 0x0080, + kUint64Flag = 0x0100, + kDoubleFlag = 0x0200, + kStringFlag = 0x0400, + kCopyFlag = 0x0800, + kInlineStrFlag = 0x1000, + + // Initial flags of different types. + kNullFlag = kNullType, + kTrueFlag = kTrueType | kBoolFlag, + kFalseFlag = kFalseType | kBoolFlag, + kNumberIntFlag = kNumberType | kNumberFlag | kIntFlag | kInt64Flag, + kNumberUintFlag = kNumberType | kNumberFlag | kUintFlag | kUint64Flag | kInt64Flag, + kNumberInt64Flag = kNumberType | kNumberFlag | kInt64Flag, + kNumberUint64Flag = kNumberType | kNumberFlag | kUint64Flag, + kNumberDoubleFlag = kNumberType | kNumberFlag | kDoubleFlag, + kNumberAnyFlag = kNumberType | kNumberFlag | kIntFlag | kInt64Flag | kUintFlag | kUint64Flag | kDoubleFlag, + kConstStringFlag = kStringType | kStringFlag, + kCopyStringFlag = kStringType | kStringFlag | kCopyFlag, + kShortStringFlag = kStringType | kStringFlag | kCopyFlag | kInlineStrFlag, + kObjectFlag = kObjectType, + kArrayFlag = kArrayType, + + kTypeMask = 0x07 + }; + + static const SizeType kDefaultArrayCapacity = 16; + static const SizeType kDefaultObjectCapacity = 16; + + struct Flag { +#if RAPIDJSON_48BITPOINTER_OPTIMIZATION + char payload[sizeof(SizeType) * 2 + 6]; // 2 x SizeType + lower 48-bit pointer +#elif RAPIDJSON_64BIT + char payload[sizeof(SizeType) * 2 + sizeof(void*) + 6]; // 6 padding bytes +#else + char payload[sizeof(SizeType) * 2 + sizeof(void*) + 2]; // 2 padding bytes +#endif + uint16_t flags; + }; + + struct String { + SizeType length; + SizeType hashcode; //!< reserved + const Ch* str; + }; // 12 bytes in 32-bit mode, 16 bytes in 64-bit mode + + // implementation detail: ShortString can represent zero-terminated strings up to MaxSize chars + // (excluding the terminating zero) and store a value to determine the length of the contained + // string in the last character str[LenPos] by storing "MaxSize - length" there. If the string + // to store has the maximal length of MaxSize then str[LenPos] will be 0 and therefore act as + // the string terminator as well. For getting the string length back from that value just use + // "MaxSize - str[LenPos]". + // This allows to store 13-chars strings in 32-bit mode, 21-chars strings in 64-bit mode, + // 13-chars strings for RAPIDJSON_48BITPOINTER_OPTIMIZATION=1 inline (for `UTF8`-encoded strings). + struct ShortString { + enum { MaxChars = sizeof(static_cast(0)->payload) / sizeof(Ch), MaxSize = MaxChars - 1, LenPos = MaxSize }; + Ch str[MaxChars]; + + inline static bool Usable(SizeType len) { return (MaxSize >= len); } + inline void SetLength(SizeType len) { str[LenPos] = static_cast(MaxSize - len); } + inline SizeType GetLength() const { return static_cast(MaxSize - str[LenPos]); } + }; // at most as many bytes as "String" above => 12 bytes in 32-bit mode, 16 bytes in 64-bit mode + + // By using proper binary layout, retrieval of different integer types do not need conversions. + union Number { +#if RAPIDJSON_ENDIAN == RAPIDJSON_LITTLEENDIAN + struct I { + int i; + char padding[4]; + }i; + struct U { + unsigned u; + char padding2[4]; + }u; +#else + struct I { + char padding[4]; + int i; + }i; + struct U { + char padding2[4]; + unsigned u; + }u; +#endif + int64_t i64; + uint64_t u64; + double d; + }; // 8 bytes + + struct ObjectData { + SizeType size; + SizeType capacity; + Member* members; + }; // 12 bytes in 32-bit mode, 16 bytes in 64-bit mode + + struct ArrayData { + SizeType size; + SizeType capacity; + GenericValue* elements; + }; // 12 bytes in 32-bit mode, 16 bytes in 64-bit mode + + union Data { + String s; + ShortString ss; + Number n; + ObjectData o; + ArrayData a; + Flag f; + }; // 16 bytes in 32-bit mode, 24 bytes in 64-bit mode, 16 bytes in 64-bit with RAPIDJSON_48BITPOINTER_OPTIMIZATION + + RAPIDJSON_FORCEINLINE const Ch* GetStringPointer() const { return RAPIDJSON_GETPOINTER(Ch, data_.s.str); } + RAPIDJSON_FORCEINLINE const Ch* SetStringPointer(const Ch* str) { return RAPIDJSON_SETPOINTER(Ch, data_.s.str, str); } + RAPIDJSON_FORCEINLINE GenericValue* GetElementsPointer() const { return RAPIDJSON_GETPOINTER(GenericValue, data_.a.elements); } + RAPIDJSON_FORCEINLINE GenericValue* SetElementsPointer(GenericValue* elements) { return RAPIDJSON_SETPOINTER(GenericValue, data_.a.elements, elements); } + RAPIDJSON_FORCEINLINE Member* GetMembersPointer() const { return RAPIDJSON_GETPOINTER(Member, data_.o.members); } + RAPIDJSON_FORCEINLINE Member* SetMembersPointer(Member* members) { return RAPIDJSON_SETPOINTER(Member, data_.o.members, members); } + + // Initialize this value as array with initial data, without calling destructor. + void SetArrayRaw(GenericValue* values, SizeType count, Allocator& allocator) { + data_.f.flags = kArrayFlag; + if (count) { + GenericValue* e = static_cast(allocator.Malloc(count * sizeof(GenericValue))); + SetElementsPointer(e); + std::memcpy(static_cast(e), values, count * sizeof(GenericValue)); + } + else + SetElementsPointer(0); + data_.a.size = data_.a.capacity = count; + } + + //! Initialize this value as object with initial data, without calling destructor. + void SetObjectRaw(Member* members, SizeType count, Allocator& allocator) { + data_.f.flags = kObjectFlag; + if (count) { + Member* m = static_cast(allocator.Malloc(count * sizeof(Member))); + SetMembersPointer(m); + std::memcpy(static_cast(m), members, count * sizeof(Member)); + } + else + SetMembersPointer(0); + data_.o.size = data_.o.capacity = count; + } + + //! Initialize this value as constant string, without calling destructor. + void SetStringRaw(StringRefType s) RAPIDJSON_NOEXCEPT { + data_.f.flags = kConstStringFlag; + SetStringPointer(s); + data_.s.length = s.length; + } + + //! Initialize this value as copy string with initial data, without calling destructor. + void SetStringRaw(StringRefType s, Allocator& allocator) { + Ch* str = 0; + if (ShortString::Usable(s.length)) { + data_.f.flags = kShortStringFlag; + data_.ss.SetLength(s.length); + str = data_.ss.str; + } else { + data_.f.flags = kCopyStringFlag; + data_.s.length = s.length; + str = static_cast(allocator.Malloc((s.length + 1) * sizeof(Ch))); + SetStringPointer(str); + } + std::memcpy(str, s, s.length * sizeof(Ch)); + str[s.length] = '\0'; + } + + //! Assignment without calling destructor + void RawAssign(GenericValue& rhs) RAPIDJSON_NOEXCEPT { + data_ = rhs.data_; + // data_.f.flags = rhs.data_.f.flags; + rhs.data_.f.flags = kNullFlag; + } + + template + bool StringEqual(const GenericValue& rhs) const { + RAPIDJSON_ASSERT(IsString()); + RAPIDJSON_ASSERT(rhs.IsString()); + + const SizeType len1 = GetStringLength(); + const SizeType len2 = rhs.GetStringLength(); + if(len1 != len2) { return false; } + + const Ch* const str1 = GetString(); + const Ch* const str2 = rhs.GetString(); + if(str1 == str2) { return true; } // fast path for constant string + + return (std::memcmp(str1, str2, sizeof(Ch) * len1) == 0); + } + + Data data_; +}; + +//! GenericValue with UTF8 encoding +typedef GenericValue > Value; + +/////////////////////////////////////////////////////////////////////////////// +// GenericDocument + +//! A document for parsing JSON text as DOM. +/*! + \note implements Handler concept + \tparam Encoding Encoding for both parsing and string storage. + \tparam Allocator Allocator for allocating memory for the DOM + \tparam StackAllocator Allocator for allocating memory for stack during parsing. + \warning Although GenericDocument inherits from GenericValue, the API does \b not provide any virtual functions, especially no virtual destructor. To avoid memory leaks, do not \c delete a GenericDocument object via a pointer to a GenericValue. +*/ +template , typename StackAllocator = CrtAllocator> +class GenericDocument : public GenericValue { +public: + typedef typename Encoding::Ch Ch; //!< Character type derived from Encoding. + typedef GenericValue ValueType; //!< Value type of the document. + typedef Allocator AllocatorType; //!< Allocator type from template parameter. + + //! Constructor + /*! Creates an empty document of specified type. + \param type Mandatory type of object to create. + \param allocator Optional allocator for allocating memory. + \param stackCapacity Optional initial capacity of stack in bytes. + \param stackAllocator Optional allocator for allocating memory for stack. + */ + explicit GenericDocument(Type type, Allocator* allocator = 0, size_t stackCapacity = kDefaultStackCapacity, StackAllocator* stackAllocator = 0) : + GenericValue(type), allocator_(allocator), ownAllocator_(0), stack_(stackAllocator, stackCapacity), parseResult_() + { + if (!allocator_) + ownAllocator_ = allocator_ = RAPIDJSON_NEW(Allocator)(); + } + + //! Constructor + /*! Creates an empty document which type is Null. + \param allocator Optional allocator for allocating memory. + \param stackCapacity Optional initial capacity of stack in bytes. + \param stackAllocator Optional allocator for allocating memory for stack. + */ + GenericDocument(Allocator* allocator = 0, size_t stackCapacity = kDefaultStackCapacity, StackAllocator* stackAllocator = 0) : + allocator_(allocator), ownAllocator_(0), stack_(stackAllocator, stackCapacity), parseResult_() + { + if (!allocator_) + ownAllocator_ = allocator_ = RAPIDJSON_NEW(Allocator)(); + } + +#if RAPIDJSON_HAS_CXX11_RVALUE_REFS + //! Move constructor in C++11 + GenericDocument(GenericDocument&& rhs) RAPIDJSON_NOEXCEPT + : ValueType(std::forward(rhs)), // explicit cast to avoid prohibited move from Document + allocator_(rhs.allocator_), + ownAllocator_(rhs.ownAllocator_), + stack_(std::move(rhs.stack_)), + parseResult_(rhs.parseResult_) + { + rhs.allocator_ = 0; + rhs.ownAllocator_ = 0; + rhs.parseResult_ = ParseResult(); + } +#endif + + ~GenericDocument() { + Destroy(); + } + +#if RAPIDJSON_HAS_CXX11_RVALUE_REFS + //! Move assignment in C++11 + GenericDocument& operator=(GenericDocument&& rhs) RAPIDJSON_NOEXCEPT + { + // The cast to ValueType is necessary here, because otherwise it would + // attempt to call GenericValue's templated assignment operator. + ValueType::operator=(std::forward(rhs)); + + // Calling the destructor here would prematurely call stack_'s destructor + Destroy(); + + allocator_ = rhs.allocator_; + ownAllocator_ = rhs.ownAllocator_; + stack_ = std::move(rhs.stack_); + parseResult_ = rhs.parseResult_; + + rhs.allocator_ = 0; + rhs.ownAllocator_ = 0; + rhs.parseResult_ = ParseResult(); + + return *this; + } +#endif + + //! Exchange the contents of this document with those of another. + /*! + \param rhs Another document. + \note Constant complexity. + \see GenericValue::Swap + */ + GenericDocument& Swap(GenericDocument& rhs) RAPIDJSON_NOEXCEPT { + ValueType::Swap(rhs); + stack_.Swap(rhs.stack_); + internal::Swap(allocator_, rhs.allocator_); + internal::Swap(ownAllocator_, rhs.ownAllocator_); + internal::Swap(parseResult_, rhs.parseResult_); + return *this; + } + + // Allow Swap with ValueType. + // Refer to Effective C++ 3rd Edition/Item 33: Avoid hiding inherited names. + using ValueType::Swap; + + //! free-standing swap function helper + /*! + Helper function to enable support for common swap implementation pattern based on \c std::swap: + \code + void swap(MyClass& a, MyClass& b) { + using std::swap; + swap(a.doc, b.doc); + // ... + } + \endcode + \see Swap() + */ + friend inline void swap(GenericDocument& a, GenericDocument& b) RAPIDJSON_NOEXCEPT { a.Swap(b); } + + //! Populate this document by a generator which produces SAX events. + /*! \tparam Generator A functor with bool f(Handler) prototype. + \param g Generator functor which sends SAX events to the parameter. + \return The document itself for fluent API. + */ + template + GenericDocument& Populate(Generator& g) { + ClearStackOnExit scope(*this); + if (g(*this)) { + RAPIDJSON_ASSERT(stack_.GetSize() == sizeof(ValueType)); // Got one and only one root object + ValueType::operator=(*stack_.template Pop(1));// Move value from stack to document + } + return *this; + } + + //!@name Parse from stream + //!@{ + + //! Parse JSON text from an input stream (with Encoding conversion) + /*! \tparam parseFlags Combination of \ref ParseFlag. + \tparam SourceEncoding Encoding of input stream + \tparam InputStream Type of input stream, implementing Stream concept + \param is Input stream to be parsed. + \return The document itself for fluent API. + */ + template + GenericDocument& ParseStream(InputStream& is) { + GenericReader reader( + stack_.HasAllocator() ? &stack_.GetAllocator() : 0); + ClearStackOnExit scope(*this); + parseResult_ = reader.template Parse(is, *this); + if (parseResult_) { + RAPIDJSON_ASSERT(stack_.GetSize() == sizeof(ValueType)); // Got one and only one root object + ValueType::operator=(*stack_.template Pop(1));// Move value from stack to document + } + return *this; + } + + //! Parse JSON text from an input stream + /*! \tparam parseFlags Combination of \ref ParseFlag. + \tparam InputStream Type of input stream, implementing Stream concept + \param is Input stream to be parsed. + \return The document itself for fluent API. + */ + template + GenericDocument& ParseStream(InputStream& is) { + return ParseStream(is); + } + + //! Parse JSON text from an input stream (with \ref kParseDefaultFlags) + /*! \tparam InputStream Type of input stream, implementing Stream concept + \param is Input stream to be parsed. + \return The document itself for fluent API. + */ + template + GenericDocument& ParseStream(InputStream& is) { + return ParseStream(is); + } + //!@} + + //!@name Parse in-place from mutable string + //!@{ + + //! Parse JSON text from a mutable string + /*! \tparam parseFlags Combination of \ref ParseFlag. + \param str Mutable zero-terminated string to be parsed. + \return The document itself for fluent API. + */ + template + GenericDocument& ParseInsitu(Ch* str) { + GenericInsituStringStream s(str); + return ParseStream(s); + } + + //! Parse JSON text from a mutable string (with \ref kParseDefaultFlags) + /*! \param str Mutable zero-terminated string to be parsed. + \return The document itself for fluent API. + */ + GenericDocument& ParseInsitu(Ch* str) { + return ParseInsitu(str); + } + //!@} + + //!@name Parse from read-only string + //!@{ + + //! Parse JSON text from a read-only string (with Encoding conversion) + /*! \tparam parseFlags Combination of \ref ParseFlag (must not contain \ref kParseInsituFlag). + \tparam SourceEncoding Transcoding from input Encoding + \param str Read-only zero-terminated string to be parsed. + */ + template + GenericDocument& Parse(const typename SourceEncoding::Ch* str) { + RAPIDJSON_ASSERT(!(parseFlags & kParseInsituFlag)); + GenericStringStream s(str); + return ParseStream(s); + } + + //! Parse JSON text from a read-only string + /*! \tparam parseFlags Combination of \ref ParseFlag (must not contain \ref kParseInsituFlag). + \param str Read-only zero-terminated string to be parsed. + */ + template + GenericDocument& Parse(const Ch* str) { + return Parse(str); + } + + //! Parse JSON text from a read-only string (with \ref kParseDefaultFlags) + /*! \param str Read-only zero-terminated string to be parsed. + */ + GenericDocument& Parse(const Ch* str) { + return Parse(str); + } + + template + GenericDocument& Parse(const typename SourceEncoding::Ch* str, size_t length) { + RAPIDJSON_ASSERT(!(parseFlags & kParseInsituFlag)); + MemoryStream ms(reinterpret_cast(str), length * sizeof(typename SourceEncoding::Ch)); + EncodedInputStream is(ms); + ParseStream(is); + return *this; + } + + template + GenericDocument& Parse(const Ch* str, size_t length) { + return Parse(str, length); + } + + GenericDocument& Parse(const Ch* str, size_t length) { + return Parse(str, length); + } + +#if RAPIDJSON_HAS_STDSTRING + template + GenericDocument& Parse(const std::basic_string& str) { + // c_str() is constant complexity according to standard. Should be faster than Parse(const char*, size_t) + return Parse(str.c_str()); + } + + template + GenericDocument& Parse(const std::basic_string& str) { + return Parse(str.c_str()); + } + + GenericDocument& Parse(const std::basic_string& str) { + return Parse(str); + } +#endif // RAPIDJSON_HAS_STDSTRING + + //!@} + + //!@name Handling parse errors + //!@{ + + //! Whether a parse error has occurred in the last parsing. + bool HasParseError() const { return parseResult_.IsError(); } + + //! Get the \ref ParseErrorCode of last parsing. + ParseErrorCode GetParseError() const { return parseResult_.Code(); } + + //! Get the position of last parsing error in input, 0 otherwise. + size_t GetErrorOffset() const { return parseResult_.Offset(); } + + //! Implicit conversion to get the last parse result +#ifndef __clang // -Wdocumentation + /*! \return \ref ParseResult of the last parse operation + + \code + Document doc; + ParseResult ok = doc.Parse(json); + if (!ok) + printf( "JSON parse error: %s (%u)\n", GetParseError_En(ok.Code()), ok.Offset()); + \endcode + */ +#endif + operator ParseResult() const { return parseResult_; } + //!@} + + //! Get the allocator of this document. + Allocator& GetAllocator() { + RAPIDJSON_ASSERT(allocator_); + return *allocator_; + } + + //! Get the capacity of stack in bytes. + size_t GetStackCapacity() const { return stack_.GetCapacity(); } + +private: + // clear stack on any exit from ParseStream, e.g. due to exception + struct ClearStackOnExit { + explicit ClearStackOnExit(GenericDocument& d) : d_(d) {} + ~ClearStackOnExit() { d_.ClearStack(); } + private: + ClearStackOnExit(const ClearStackOnExit&); + ClearStackOnExit& operator=(const ClearStackOnExit&); + GenericDocument& d_; + }; + + // callers of the following private Handler functions + // template friend class GenericReader; // for parsing + template friend class GenericValue; // for deep copying + +public: + // Implementation of Handler + bool Null() { new (stack_.template Push()) ValueType(); return true; } + bool Bool(bool b) { new (stack_.template Push()) ValueType(b); return true; } + bool Int(int i) { new (stack_.template Push()) ValueType(i); return true; } + bool Uint(unsigned i) { new (stack_.template Push()) ValueType(i); return true; } + bool Int64(int64_t i) { new (stack_.template Push()) ValueType(i); return true; } + bool Uint64(uint64_t i) { new (stack_.template Push()) ValueType(i); return true; } + bool Double(double d) { new (stack_.template Push()) ValueType(d); return true; } + + bool RawNumber(const Ch* str, SizeType length, bool copy) { + if (copy) + new (stack_.template Push()) ValueType(str, length, GetAllocator()); + else + new (stack_.template Push()) ValueType(str, length); + return true; + } + + bool String(const Ch* str, SizeType length, bool copy) { + if (copy) + new (stack_.template Push()) ValueType(str, length, GetAllocator()); + else + new (stack_.template Push()) ValueType(str, length); + return true; + } + + bool StartObject() { new (stack_.template Push()) ValueType(kObjectType); return true; } + + bool Key(const Ch* str, SizeType length, bool copy) { return String(str, length, copy); } + + bool EndObject(SizeType memberCount) { + typename ValueType::Member* members = stack_.template Pop(memberCount); + stack_.template Top()->SetObjectRaw(members, memberCount, GetAllocator()); + return true; + } + + bool StartArray() { new (stack_.template Push()) ValueType(kArrayType); return true; } + + bool EndArray(SizeType elementCount) { + ValueType* elements = stack_.template Pop(elementCount); + stack_.template Top()->SetArrayRaw(elements, elementCount, GetAllocator()); + return true; + } + +private: + //! Prohibit copying + GenericDocument(const GenericDocument&); + //! Prohibit assignment + GenericDocument& operator=(const GenericDocument&); + + void ClearStack() { + if (Allocator::kNeedFree) + while (stack_.GetSize() > 0) // Here assumes all elements in stack array are GenericValue (Member is actually 2 GenericValue objects) + (stack_.template Pop(1))->~ValueType(); + else + stack_.Clear(); + stack_.ShrinkToFit(); + } + + void Destroy() { + RAPIDJSON_DELETE(ownAllocator_); + } + + static const size_t kDefaultStackCapacity = 1024; + Allocator* allocator_; + Allocator* ownAllocator_; + internal::Stack stack_; + ParseResult parseResult_; +}; + +//! GenericDocument with UTF8 encoding +typedef GenericDocument > Document; + +//! Helper class for accessing Value of array type. +/*! + Instance of this helper class is obtained by \c GenericValue::GetArray(). + In addition to all APIs for array type, it provides range-based for loop if \c RAPIDJSON_HAS_CXX11_RANGE_FOR=1. +*/ +template +class GenericArray { +public: + typedef GenericArray ConstArray; + typedef GenericArray Array; + typedef ValueT PlainType; + typedef typename internal::MaybeAddConst::Type ValueType; + typedef ValueType* ValueIterator; // This may be const or non-const iterator + typedef const ValueT* ConstValueIterator; + typedef typename ValueType::AllocatorType AllocatorType; + typedef typename ValueType::StringRefType StringRefType; + + template + friend class GenericValue; + + GenericArray(const GenericArray& rhs) : value_(rhs.value_) {} + GenericArray& operator=(const GenericArray& rhs) { value_ = rhs.value_; return *this; } + ~GenericArray() {} + + SizeType Size() const { return value_.Size(); } + SizeType Capacity() const { return value_.Capacity(); } + bool Empty() const { return value_.Empty(); } + void Clear() const { value_.Clear(); } + ValueType& operator[](SizeType index) const { return value_[index]; } + ValueIterator Begin() const { return value_.Begin(); } + ValueIterator End() const { return value_.End(); } + GenericArray Reserve(SizeType newCapacity, AllocatorType &allocator) const { value_.Reserve(newCapacity, allocator); return *this; } + GenericArray PushBack(ValueType& value, AllocatorType& allocator) const { value_.PushBack(value, allocator); return *this; } +#if RAPIDJSON_HAS_CXX11_RVALUE_REFS + GenericArray PushBack(ValueType&& value, AllocatorType& allocator) const { value_.PushBack(value, allocator); return *this; } +#endif // RAPIDJSON_HAS_CXX11_RVALUE_REFS + GenericArray PushBack(StringRefType value, AllocatorType& allocator) const { value_.PushBack(value, allocator); return *this; } + template RAPIDJSON_DISABLEIF_RETURN((internal::OrExpr, internal::IsGenericValue >), (const GenericArray&)) PushBack(T value, AllocatorType& allocator) const { value_.PushBack(value, allocator); return *this; } + GenericArray PopBack() const { value_.PopBack(); return *this; } + ValueIterator Erase(ConstValueIterator pos) const { return value_.Erase(pos); } + ValueIterator Erase(ConstValueIterator first, ConstValueIterator last) const { return value_.Erase(first, last); } + +#if RAPIDJSON_HAS_CXX11_RANGE_FOR + ValueIterator begin() const { return value_.Begin(); } + ValueIterator end() const { return value_.End(); } +#endif + +private: + GenericArray(); + GenericArray(ValueType& value) : value_(value) {} + ValueType& value_; +}; + +//! Helper class for accessing Value of object type. +/*! + Instance of this helper class is obtained by \c GenericValue::GetObject(). + In addition to all APIs for array type, it provides range-based for loop if \c RAPIDJSON_HAS_CXX11_RANGE_FOR=1. +*/ +template +class GenericObject { +public: + typedef GenericObject ConstObject; + typedef GenericObject Object; + typedef ValueT PlainType; + typedef typename internal::MaybeAddConst::Type ValueType; + typedef GenericMemberIterator MemberIterator; // This may be const or non-const iterator + typedef GenericMemberIterator ConstMemberIterator; + typedef typename ValueType::AllocatorType AllocatorType; + typedef typename ValueType::StringRefType StringRefType; + typedef typename ValueType::EncodingType EncodingType; + typedef typename ValueType::Ch Ch; + + template + friend class GenericValue; + + GenericObject(const GenericObject& rhs) : value_(rhs.value_) {} + GenericObject& operator=(const GenericObject& rhs) { value_ = rhs.value_; return *this; } + ~GenericObject() {} + + SizeType MemberCount() const { return value_.MemberCount(); } + SizeType MemberCapacity() const { return value_.MemberCapacity(); } + bool ObjectEmpty() const { return value_.ObjectEmpty(); } + template ValueType& operator[](T* name) const { return value_[name]; } + template ValueType& operator[](const GenericValue& name) const { return value_[name]; } +#if RAPIDJSON_HAS_STDSTRING + ValueType& operator[](const std::basic_string& name) const { return value_[name]; } +#endif + MemberIterator MemberBegin() const { return value_.MemberBegin(); } + MemberIterator MemberEnd() const { return value_.MemberEnd(); } + GenericObject MemberReserve(SizeType newCapacity, AllocatorType &allocator) const { value_.MemberReserve(newCapacity, allocator); return *this; } + bool HasMember(const Ch* name) const { return value_.HasMember(name); } +#if RAPIDJSON_HAS_STDSTRING + bool HasMember(const std::basic_string& name) const { return value_.HasMember(name); } +#endif + template bool HasMember(const GenericValue& name) const { return value_.HasMember(name); } + MemberIterator FindMember(const Ch* name) const { return value_.FindMember(name); } + template MemberIterator FindMember(const GenericValue& name) const { return value_.FindMember(name); } +#if RAPIDJSON_HAS_STDSTRING + MemberIterator FindMember(const std::basic_string& name) const { return value_.FindMember(name); } +#endif + GenericObject AddMember(ValueType& name, ValueType& value, AllocatorType& allocator) const { value_.AddMember(name, value, allocator); return *this; } + GenericObject AddMember(ValueType& name, StringRefType value, AllocatorType& allocator) const { value_.AddMember(name, value, allocator); return *this; } +#if RAPIDJSON_HAS_STDSTRING + GenericObject AddMember(ValueType& name, std::basic_string& value, AllocatorType& allocator) const { value_.AddMember(name, value, allocator); return *this; } +#endif + template RAPIDJSON_DISABLEIF_RETURN((internal::OrExpr, internal::IsGenericValue >), (ValueType&)) AddMember(ValueType& name, T value, AllocatorType& allocator) const { value_.AddMember(name, value, allocator); return *this; } +#if RAPIDJSON_HAS_CXX11_RVALUE_REFS + GenericObject AddMember(ValueType&& name, ValueType&& value, AllocatorType& allocator) const { value_.AddMember(name, value, allocator); return *this; } + GenericObject AddMember(ValueType&& name, ValueType& value, AllocatorType& allocator) const { value_.AddMember(name, value, allocator); return *this; } + GenericObject AddMember(ValueType& name, ValueType&& value, AllocatorType& allocator) const { value_.AddMember(name, value, allocator); return *this; } + GenericObject AddMember(StringRefType name, ValueType&& value, AllocatorType& allocator) const { value_.AddMember(name, value, allocator); return *this; } +#endif // RAPIDJSON_HAS_CXX11_RVALUE_REFS + GenericObject AddMember(StringRefType name, ValueType& value, AllocatorType& allocator) const { value_.AddMember(name, value, allocator); return *this; } + GenericObject AddMember(StringRefType name, StringRefType value, AllocatorType& allocator) const { value_.AddMember(name, value, allocator); return *this; } + template RAPIDJSON_DISABLEIF_RETURN((internal::OrExpr, internal::IsGenericValue >), (GenericObject)) AddMember(StringRefType name, T value, AllocatorType& allocator) const { value_.AddMember(name, value, allocator); return *this; } + void RemoveAllMembers() { value_.RemoveAllMembers(); } + bool RemoveMember(const Ch* name) const { return value_.RemoveMember(name); } +#if RAPIDJSON_HAS_STDSTRING + bool RemoveMember(const std::basic_string& name) const { return value_.RemoveMember(name); } +#endif + template bool RemoveMember(const GenericValue& name) const { return value_.RemoveMember(name); } + MemberIterator RemoveMember(MemberIterator m) const { return value_.RemoveMember(m); } + MemberIterator EraseMember(ConstMemberIterator pos) const { return value_.EraseMember(pos); } + MemberIterator EraseMember(ConstMemberIterator first, ConstMemberIterator last) const { return value_.EraseMember(first, last); } + bool EraseMember(const Ch* name) const { return value_.EraseMember(name); } +#if RAPIDJSON_HAS_STDSTRING + bool EraseMember(const std::basic_string& name) const { return EraseMember(ValueType(StringRef(name))); } +#endif + template bool EraseMember(const GenericValue& name) const { return value_.EraseMember(name); } + +#if RAPIDJSON_HAS_CXX11_RANGE_FOR + MemberIterator begin() const { return value_.MemberBegin(); } + MemberIterator end() const { return value_.MemberEnd(); } +#endif + +private: + GenericObject(); + GenericObject(ValueType& value) : value_(value) {} + ValueType& value_; +}; + +RAPIDJSON_NAMESPACE_END +RAPIDJSON_DIAG_POP + +#endif // RAPIDJSON_DOCUMENT_H_ diff --git a/livox_ros_driver/common/rapidjson/encodedstream.h b/livox_ros_driver/common/rapidjson/encodedstream.h new file mode 100644 index 0000000..223601c --- /dev/null +++ b/livox_ros_driver/common/rapidjson/encodedstream.h @@ -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 +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, MemoryStream> { +public: + typedef UTF8<>::Ch Ch; + + EncodedInputStream(MemoryStream& is) : is_(is) { + if (static_cast(is_.Peek()) == 0xEFu) is_.Take(); + if (static_cast(is_.Peek()) == 0xBBu) is_.Take(); + if (static_cast(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 +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::x, UTF16LE::x, UTF16BE::x, UTF32LE::x, UTF32BE::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 +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(is_->Peek4()); + if (!c) + return; + + unsigned bom = static_cast(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 +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_ diff --git a/livox_ros_driver/common/rapidjson/encodings.h b/livox_ros_driver/common/rapidjson/encodings.h new file mode 100644 index 0000000..0b24467 --- /dev/null +++ b/livox_ros_driver/common/rapidjson/encodings.h @@ -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 + 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 + 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 + 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 + static CharType TakeBOM(InputByteStream& is); + + //! Take a character from input byte stream. + template + static Ch Take(InputByteStream& is); + + //! Put BOM to output byte stream. + template + static void PutBOM(OutputByteStream& os); + + //! Put a character to output byte stream. + template + 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 +struct UTF8 { + typedef CharType Ch; + + enum { supportUnicode = 1 }; + + template + static void Encode(OutputStream& os, unsigned codepoint) { + if (codepoint <= 0x7F) + os.Put(static_cast(codepoint & 0xFF)); + else if (codepoint <= 0x7FF) { + os.Put(static_cast(0xC0 | ((codepoint >> 6) & 0xFF))); + os.Put(static_cast(0x80 | ((codepoint & 0x3F)))); + } + else if (codepoint <= 0xFFFF) { + os.Put(static_cast(0xE0 | ((codepoint >> 12) & 0xFF))); + os.Put(static_cast(0x80 | ((codepoint >> 6) & 0x3F))); + os.Put(static_cast(0x80 | (codepoint & 0x3F))); + } + else { + RAPIDJSON_ASSERT(codepoint <= 0x10FFFF); + os.Put(static_cast(0xF0 | ((codepoint >> 18) & 0xFF))); + os.Put(static_cast(0x80 | ((codepoint >> 12) & 0x3F))); + os.Put(static_cast(0x80 | ((codepoint >> 6) & 0x3F))); + os.Put(static_cast(0x80 | (codepoint & 0x3F))); + } + } + + template + static void EncodeUnsafe(OutputStream& os, unsigned codepoint) { + if (codepoint <= 0x7F) + PutUnsafe(os, static_cast(codepoint & 0xFF)); + else if (codepoint <= 0x7FF) { + PutUnsafe(os, static_cast(0xC0 | ((codepoint >> 6) & 0xFF))); + PutUnsafe(os, static_cast(0x80 | ((codepoint & 0x3F)))); + } + else if (codepoint <= 0xFFFF) { + PutUnsafe(os, static_cast(0xE0 | ((codepoint >> 12) & 0xFF))); + PutUnsafe(os, static_cast(0x80 | ((codepoint >> 6) & 0x3F))); + PutUnsafe(os, static_cast(0x80 | (codepoint & 0x3F))); + } + else { + RAPIDJSON_ASSERT(codepoint <= 0x10FFFF); + PutUnsafe(os, static_cast(0xF0 | ((codepoint >> 18) & 0xFF))); + PutUnsafe(os, static_cast(0x80 | ((codepoint >> 12) & 0x3F))); + PutUnsafe(os, static_cast(0x80 | ((codepoint >> 6) & 0x3F))); + PutUnsafe(os, static_cast(0x80 | (codepoint & 0x3F))); + } + } + + template + static bool Decode(InputStream& is, unsigned* codepoint) { +#define RAPIDJSON_COPY() c = is.Take(); *codepoint = (*codepoint << 6) | (static_cast(c) & 0x3Fu) +#define RAPIDJSON_TRANS(mask) result &= ((GetRange(static_cast(c)) & mask) != 0) +#define RAPIDJSON_TAIL() RAPIDJSON_COPY(); RAPIDJSON_TRANS(0x70) + typename InputStream::Ch c = is.Take(); + if (!(c & 0x80)) { + *codepoint = static_cast(c); + return true; + } + + unsigned char type = GetRange(static_cast(c)); + if (type >= 32) { + *codepoint = 0; + } else { + *codepoint = (0xFFu >> type) & static_cast(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 + static bool Validate(InputStream& is, OutputStream& os) { +#define RAPIDJSON_COPY() os.Put(c = is.Take()) +#define RAPIDJSON_TRANS(mask) result &= ((GetRange(static_cast(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(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 + static CharType TakeBOM(InputByteStream& is) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1); + typename InputByteStream::Ch c = Take(is); + if (static_cast(c) != 0xEFu) return c; + c = is.Take(); + if (static_cast(c) != 0xBBu) return c; + c = is.Take(); + if (static_cast(c) != 0xBFu) return c; + c = is.Take(); + return c; + } + + template + static Ch Take(InputByteStream& is) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1); + return static_cast(is.Take()); + } + + template + static void PutBOM(OutputByteStream& os) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1); + os.Put(static_cast(0xEFu)); + os.Put(static_cast(0xBBu)); + os.Put(static_cast(0xBFu)); + } + + template + static void Put(OutputByteStream& os, Ch c) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1); + os.Put(static_cast(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 +struct UTF16 { + typedef CharType Ch; + RAPIDJSON_STATIC_ASSERT(sizeof(Ch) >= 2); + + enum { supportUnicode = 1 }; + + template + 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(codepoint)); + } + else { + RAPIDJSON_ASSERT(codepoint <= 0x10FFFF); + unsigned v = codepoint - 0x10000; + os.Put(static_cast((v >> 10) | 0xD800)); + os.Put(static_cast((v & 0x3FF) | 0xDC00)); + } + } + + + template + 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(codepoint)); + } + else { + RAPIDJSON_ASSERT(codepoint <= 0x10FFFF); + unsigned v = codepoint - 0x10000; + PutUnsafe(os, static_cast((v >> 10) | 0xD800)); + PutUnsafe(os, static_cast((v & 0x3FF) | 0xDC00)); + } + } + + template + 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(c); + return true; + } + else if (c <= 0xDBFF) { + *codepoint = (static_cast(c) & 0x3FF) << 10; + c = is.Take(); + *codepoint |= (static_cast(c) & 0x3FF); + *codepoint += 0x10000; + return c >= 0xDC00 && c <= 0xDFFF; + } + return false; + } + + template + 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(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 +struct UTF16LE : UTF16 { + template + static CharType TakeBOM(InputByteStream& is) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1); + CharType c = Take(is); + return static_cast(c) == 0xFEFFu ? Take(is) : c; + } + + template + static CharType Take(InputByteStream& is) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1); + unsigned c = static_cast(is.Take()); + c |= static_cast(static_cast(is.Take())) << 8; + return static_cast(c); + } + + template + static void PutBOM(OutputByteStream& os) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1); + os.Put(static_cast(0xFFu)); + os.Put(static_cast(0xFEu)); + } + + template + static void Put(OutputByteStream& os, CharType c) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1); + os.Put(static_cast(static_cast(c) & 0xFFu)); + os.Put(static_cast((static_cast(c) >> 8) & 0xFFu)); + } +}; + +//! UTF-16 big endian encoding. +template +struct UTF16BE : UTF16 { + template + static CharType TakeBOM(InputByteStream& is) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1); + CharType c = Take(is); + return static_cast(c) == 0xFEFFu ? Take(is) : c; + } + + template + static CharType Take(InputByteStream& is) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1); + unsigned c = static_cast(static_cast(is.Take())) << 8; + c |= static_cast(static_cast(is.Take())); + return static_cast(c); + } + + template + static void PutBOM(OutputByteStream& os) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1); + os.Put(static_cast(0xFEu)); + os.Put(static_cast(0xFFu)); + } + + template + static void Put(OutputByteStream& os, CharType c) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1); + os.Put(static_cast((static_cast(c) >> 8) & 0xFFu)); + os.Put(static_cast(static_cast(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 +struct UTF32 { + typedef CharType Ch; + RAPIDJSON_STATIC_ASSERT(sizeof(Ch) >= 4); + + enum { supportUnicode = 1 }; + + template + static void Encode(OutputStream& os, unsigned codepoint) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputStream::Ch) >= 4); + RAPIDJSON_ASSERT(codepoint <= 0x10FFFF); + os.Put(codepoint); + } + + template + static void EncodeUnsafe(OutputStream& os, unsigned codepoint) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputStream::Ch) >= 4); + RAPIDJSON_ASSERT(codepoint <= 0x10FFFF); + PutUnsafe(os, codepoint); + } + + template + 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 + 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 +struct UTF32LE : UTF32 { + template + static CharType TakeBOM(InputByteStream& is) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1); + CharType c = Take(is); + return static_cast(c) == 0x0000FEFFu ? Take(is) : c; + } + + template + static CharType Take(InputByteStream& is) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1); + unsigned c = static_cast(is.Take()); + c |= static_cast(static_cast(is.Take())) << 8; + c |= static_cast(static_cast(is.Take())) << 16; + c |= static_cast(static_cast(is.Take())) << 24; + return static_cast(c); + } + + template + static void PutBOM(OutputByteStream& os) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1); + os.Put(static_cast(0xFFu)); + os.Put(static_cast(0xFEu)); + os.Put(static_cast(0x00u)); + os.Put(static_cast(0x00u)); + } + + template + static void Put(OutputByteStream& os, CharType c) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1); + os.Put(static_cast(c & 0xFFu)); + os.Put(static_cast((c >> 8) & 0xFFu)); + os.Put(static_cast((c >> 16) & 0xFFu)); + os.Put(static_cast((c >> 24) & 0xFFu)); + } +}; + +//! UTF-32 big endian encoding. +template +struct UTF32BE : UTF32 { + template + static CharType TakeBOM(InputByteStream& is) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1); + CharType c = Take(is); + return static_cast(c) == 0x0000FEFFu ? Take(is) : c; + } + + template + static CharType Take(InputByteStream& is) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1); + unsigned c = static_cast(static_cast(is.Take())) << 24; + c |= static_cast(static_cast(is.Take())) << 16; + c |= static_cast(static_cast(is.Take())) << 8; + c |= static_cast(static_cast(is.Take())); + return static_cast(c); + } + + template + static void PutBOM(OutputByteStream& os) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1); + os.Put(static_cast(0x00u)); + os.Put(static_cast(0x00u)); + os.Put(static_cast(0xFEu)); + os.Put(static_cast(0xFFu)); + } + + template + static void Put(OutputByteStream& os, CharType c) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1); + os.Put(static_cast((c >> 24) & 0xFFu)); + os.Put(static_cast((c >> 16) & 0xFFu)); + os.Put(static_cast((c >> 8) & 0xFFu)); + os.Put(static_cast(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 +struct ASCII { + typedef CharType Ch; + + enum { supportUnicode = 0 }; + + template + static void Encode(OutputStream& os, unsigned codepoint) { + RAPIDJSON_ASSERT(codepoint <= 0x7F); + os.Put(static_cast(codepoint & 0xFF)); + } + + template + static void EncodeUnsafe(OutputStream& os, unsigned codepoint) { + RAPIDJSON_ASSERT(codepoint <= 0x7F); + PutUnsafe(os, static_cast(codepoint & 0xFF)); + } + + template + static bool Decode(InputStream& is, unsigned* codepoint) { + uint8_t c = static_cast(is.Take()); + *codepoint = c; + return c <= 0X7F; + } + + template + static bool Validate(InputStream& is, OutputStream& os) { + uint8_t c = static_cast(is.Take()); + os.Put(static_cast(c)); + return c <= 0x7F; + } + + template + static CharType TakeBOM(InputByteStream& is) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1); + uint8_t c = static_cast(Take(is)); + return static_cast(c); + } + + template + static Ch Take(InputByteStream& is) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1); + return static_cast(is.Take()); + } + + template + static void PutBOM(OutputByteStream& os) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1); + (void)os; + } + + template + static void Put(OutputByteStream& os, Ch c) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1); + os.Put(static_cast(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 +struct AutoUTF { + typedef CharType Ch; + + enum { supportUnicode = 1 }; + +#define RAPIDJSON_ENCODINGS_FUNC(x) UTF8::x, UTF16LE::x, UTF16BE::x, UTF32LE::x, UTF32BE::x + + template + 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 + 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 + 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 + 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 +struct Transcoder { + //! Take one Unicode codepoint from source encoding, convert it to target encoding and put it to the output stream. + template + 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 + 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 + static RAPIDJSON_FORCEINLINE bool Validate(InputStream& is, OutputStream& os) { + return Transcode(is, os); // Since source/target encoding is different, must transcode. + } +}; + +// Forward declaration. +template +inline void PutUnsafe(Stream& stream, typename Stream::Ch c); + +//! Specialization of Transcoder with same source and target encoding. +template +struct Transcoder { + template + 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 + 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 + 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_ diff --git a/livox_ros_driver/common/rapidjson/error/en.h b/livox_ros_driver/common/rapidjson/error/en.h new file mode 100644 index 0000000..2db838b --- /dev/null +++ b/livox_ros_driver/common/rapidjson/error/en.h @@ -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_ diff --git a/livox_ros_driver/common/rapidjson/error/error.h b/livox_ros_driver/common/rapidjson/error/error.h new file mode 100644 index 0000000..9311d2f --- /dev/null +++ b/livox_ros_driver/common/rapidjson/error/error.h @@ -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_ diff --git a/livox_ros_driver/common/rapidjson/filereadstream.h b/livox_ros_driver/common/rapidjson/filereadstream.h new file mode 100644 index 0000000..6b34370 --- /dev/null +++ b/livox_ros_driver/common/rapidjson/filereadstream.h @@ -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 + +#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(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_ diff --git a/livox_ros_driver/common/rapidjson/filewritestream.h b/livox_ros_driver/common/rapidjson/filewritestream.h new file mode 100644 index 0000000..8b48fee --- /dev/null +++ b/livox_ros_driver/common/rapidjson/filewritestream.h @@ -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 + +#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(bufferEnd_ - current_); + while (n > avail) { + std::memset(current_, c, avail); + current_ += avail; + Flush(); + n -= avail; + avail = static_cast(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(current_ - buffer_), fp_); + if (result < static_cast(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_ diff --git a/livox_ros_driver/common/rapidjson/fwd.h b/livox_ros_driver/common/rapidjson/fwd.h new file mode 100644 index 0000000..b74a2b8 --- /dev/null +++ b/livox_ros_driver/common/rapidjson/fwd.h @@ -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 struct UTF8; +template struct UTF16; +template struct UTF16BE; +template struct UTF16LE; +template struct UTF32; +template struct UTF32BE; +template struct UTF32LE; +template struct ASCII; +template struct AutoUTF; + +template +struct Transcoder; + +// allocators.h + +class CrtAllocator; + +template +class MemoryPoolAllocator; + +// stream.h + +template +struct GenericStringStream; + +typedef GenericStringStream > StringStream; + +template +struct GenericInsituStringStream; + +typedef GenericInsituStringStream > InsituStringStream; + +// stringbuffer.h + +template +class GenericStringBuffer; + +typedef GenericStringBuffer, CrtAllocator> StringBuffer; + +// filereadstream.h + +class FileReadStream; + +// filewritestream.h + +class FileWriteStream; + +// memorybuffer.h + +template +struct GenericMemoryBuffer; + +typedef GenericMemoryBuffer MemoryBuffer; + +// memorystream.h + +struct MemoryStream; + +// reader.h + +template +struct BaseReaderHandler; + +template +class GenericReader; + +typedef GenericReader, UTF8, CrtAllocator> Reader; + +// writer.h + +template +class Writer; + +// prettywriter.h + +template +class PrettyWriter; + +// document.h + +template +class GenericMember; + +template +class GenericMemberIterator; + +template +struct GenericStringRef; + +template +class GenericValue; + +typedef GenericValue, MemoryPoolAllocator > Value; + +template +class GenericDocument; + +typedef GenericDocument, MemoryPoolAllocator, CrtAllocator> Document; + +// pointer.h + +template +class GenericPointer; + +typedef GenericPointer Pointer; + +// schema.h + +template +class IGenericRemoteSchemaDocumentProvider; + +template +class GenericSchemaDocument; + +typedef GenericSchemaDocument SchemaDocument; +typedef IGenericRemoteSchemaDocumentProvider IRemoteSchemaDocumentProvider; + +template < + typename SchemaDocumentType, + typename OutputHandler, + typename StateAllocator> +class GenericSchemaValidator; + +typedef GenericSchemaValidator, void>, CrtAllocator> SchemaValidator; + +RAPIDJSON_NAMESPACE_END + +#endif // RAPIDJSON_RAPIDJSONFWD_H_ diff --git a/livox_ros_driver/common/rapidjson/internal/biginteger.h b/livox_ros_driver/common/rapidjson/internal/biginteger.h new file mode 100644 index 0000000..a31c8a8 --- /dev/null +++ b/livox_ros_driver/common/rapidjson/internal/biginteger.h @@ -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 // 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(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(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(*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(a) * static_cast(b); + p += k; + *outHigh = static_cast(p >> 64); + return static_cast(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(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_ diff --git a/livox_ros_driver/common/rapidjson/internal/clzll.h b/livox_ros_driver/common/rapidjson/internal/clzll.h new file mode 100644 index 0000000..abe44df --- /dev/null +++ b/livox_ros_driver/common/rapidjson/internal/clzll.h @@ -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 +#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(x >> 32))) + return 63 - (r + 32); + + // Scan the low 32 bits. + _BitScanReverse(&r, static_cast(x & 0xFFFFFFFF)); +#endif // _WIN64 + + return 63 - r; +#else + uint32_t r; + while (!(x & (static_cast(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_ \ No newline at end of file diff --git a/livox_ros_driver/common/rapidjson/internal/diyfp.h b/livox_ros_driver/common/rapidjson/internal/diyfp.h new file mode 100644 index 0000000..f46059a --- /dev/null +++ b/livox_ros_driver/common/rapidjson/internal/diyfp.h @@ -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 + +#if defined(_MSC_VER) && defined(_M_AMD64) && !defined(__INTEL_COMPILER) +#include +#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((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(f) * static_cast(rhs.f); + uint64_t h = static_cast(p >> 64); + uint64_t l = static_cast(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(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::infinity(); + } + const uint64_t be = (e == kDpDenormalExponent && (f & kDpHiddenBit) == 0) ? 0 : + static_cast(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(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(dk); + if (dk - k > 0.0) + k++; + + unsigned index = static_cast((k >> 3) + 1); + *K = -(-348 + static_cast(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(exp + 348) / 8u; + *outExp = -348 + static_cast(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_ diff --git a/livox_ros_driver/common/rapidjson/internal/dtoa.h b/livox_ros_driver/common/rapidjson/internal/dtoa.h new file mode 100644 index 0000000..bf2e9b2 --- /dev/null +++ b/livox_ros_driver/common/rapidjson/internal/dtoa.h @@ -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(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('0' + static_cast(d)); + kappa--; + uint64_t tmp = (static_cast(p1) << -one.e) + p2; + if (tmp <= delta) { + *K += kappa; + GrisuRound(buffer, *len, delta, tmp, static_cast(kPow10[kappa]) << -one.e, wp_w.f); + return; + } + } + + // kappa = 0 + for (;;) { + p2 *= 10; + delta *= 10; + char d = static_cast(p2 >> -one.e); + if (d || *len) + buffer[(*len)++] = static_cast('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('0' + static_cast(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('0' + static_cast(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(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(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(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_ diff --git a/livox_ros_driver/common/rapidjson/internal/ieee754.h b/livox_ros_driver/common/rapidjson/internal/ieee754.h new file mode 100644 index 0000000..c2684ba --- /dev/null +++ b/livox_ros_driver/common/rapidjson/internal/ieee754.h @@ -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(((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_ diff --git a/livox_ros_driver/common/rapidjson/internal/itoa.h b/livox_ros_driver/common/rapidjson/internal/itoa.h new file mode 100644 index 0000000..9b1c45c --- /dev/null +++ b/livox_ros_driver/common/rapidjson/internal/itoa.h @@ -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('0' + static_cast(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(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(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(value / kTen8); + const uint32_t v1 = static_cast(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(value / kTen16); // 1 to 1844 + value %= kTen16; + + if (a < 10) + *buffer++ = static_cast('0' + static_cast(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('0' + static_cast(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(value / kTen8); + const uint32_t v1 = static_cast(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(value); + if (value < 0) { + *buffer++ = '-'; + u = ~u + 1; + } + + return u64toa(u, buffer); +} + +} // namespace internal +RAPIDJSON_NAMESPACE_END + +#endif // RAPIDJSON_ITOA_ diff --git a/livox_ros_driver/common/rapidjson/internal/meta.h b/livox_ros_driver/common/rapidjson/internal/meta.h new file mode 100644 index 0000000..d401edf --- /dev/null +++ b/livox_ros_driver/common/rapidjson/internal/meta.h @@ -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 +#endif + +//@cond RAPIDJSON_INTERNAL +RAPIDJSON_NAMESPACE_BEGIN +namespace internal { + +// Helper to wrap/convert arbitrary types to void, useful for arbitrary type matching +template struct Void { typedef void Type; }; + +/////////////////////////////////////////////////////////////////////////////// +// BoolType, TrueType, FalseType +// +template struct BoolType { + static const bool Value = Cond; + typedef BoolType Type; +}; +typedef BoolType TrueType; +typedef BoolType FalseType; + + +/////////////////////////////////////////////////////////////////////////////// +// SelectIf, BoolExpr, NotExpr, AndExpr, OrExpr +// + +template struct SelectIfImpl { template struct Apply { typedef T1 Type; }; }; +template <> struct SelectIfImpl { template struct Apply { typedef T2 Type; }; }; +template struct SelectIfCond : SelectIfImpl::template Apply {}; +template struct SelectIf : SelectIfCond {}; + +template struct AndExprCond : FalseType {}; +template <> struct AndExprCond : TrueType {}; +template struct OrExprCond : TrueType {}; +template <> struct OrExprCond : FalseType {}; + +template struct BoolExpr : SelectIf::Type {}; +template struct NotExpr : SelectIf::Type {}; +template struct AndExpr : AndExprCond::Type {}; +template struct OrExpr : OrExprCond::Type {}; + + +/////////////////////////////////////////////////////////////////////////////// +// AddConst, MaybeAddConst, RemoveConst +template struct AddConst { typedef const T Type; }; +template struct MaybeAddConst : SelectIfCond {}; +template struct RemoveConst { typedef T Type; }; +template struct RemoveConst { typedef T Type; }; + + +/////////////////////////////////////////////////////////////////////////////// +// IsSame, IsConst, IsMoreConst, IsPointer +// +template struct IsSame : FalseType {}; +template struct IsSame : TrueType {}; + +template struct IsConst : FalseType {}; +template struct IsConst : TrueType {}; + +template +struct IsMoreConst + : AndExpr::Type, typename RemoveConst::Type>, + BoolType::Value >= IsConst::Value> >::Type {}; + +template struct IsPointer : FalseType {}; +template struct IsPointer : TrueType {}; + +/////////////////////////////////////////////////////////////////////////////// +// IsBaseOf +// +#if RAPIDJSON_HAS_CXX11_TYPETRAITS + +template struct IsBaseOf + : BoolType< ::std::is_base_of::value> {}; + +#else // simplified version adopted from Boost + +template struct IsBaseOfImpl { + RAPIDJSON_STATIC_ASSERT(sizeof(B) != 0); + RAPIDJSON_STATIC_ASSERT(sizeof(D) != 0); + + typedef char (&Yes)[1]; + typedef char (&No) [2]; + + template + 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 struct IsBaseOf + : OrExpr, BoolExpr > >::Type {}; + +#endif // RAPIDJSON_HAS_CXX11_TYPETRAITS + + +////////////////////////////////////////////////////////////////////////// +// EnableIf / DisableIf +// +template struct EnableIfCond { typedef T Type; }; +template struct EnableIfCond { /* empty */ }; + +template struct DisableIfCond { typedef T Type; }; +template struct DisableIfCond { /* empty */ }; + +template +struct EnableIf : EnableIfCond {}; + +template +struct DisableIf : DisableIfCond {}; + +// SFINAE helpers +struct SfinaeTag {}; +template struct RemoveSfinaeTag; +template struct RemoveSfinaeTag { 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 \ + ::Type * = NULL + +#define RAPIDJSON_DISABLEIF(cond) \ + typename ::RAPIDJSON_NAMESPACE::internal::DisableIf \ + ::Type * = NULL + +#define RAPIDJSON_ENABLEIF_RETURN(cond,returntype) \ + typename ::RAPIDJSON_NAMESPACE::internal::EnableIf \ + ::Type + +#define RAPIDJSON_DISABLEIF_RETURN(cond,returntype) \ + typename ::RAPIDJSON_NAMESPACE::internal::DisableIf \ + ::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_ diff --git a/livox_ros_driver/common/rapidjson/internal/pow10.h b/livox_ros_driver/common/rapidjson/internal/pow10.h new file mode 100644 index 0000000..02f475d --- /dev/null +++ b/livox_ros_driver/common/rapidjson/internal/pow10.h @@ -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_ diff --git a/livox_ros_driver/common/rapidjson/internal/regex.h b/livox_ros_driver/common/rapidjson/internal/regex.h new file mode 100644 index 0000000..af7e06d --- /dev/null +++ b/livox_ros_driver/common/rapidjson/internal/regex.h @@ -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 +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 +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 +class GenericRegex { +public: + typedef Encoding EncodingType; + typedef typename Encoding::Ch Ch; + template 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 ss(source); + DecodedStream, 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()[index]; + } + + const State& GetState(SizeType index) const { + RAPIDJSON_ASSERT(index < stateCount_); + return states_.template Bottom()[index]; + } + + Range& GetRange(SizeType index) { + RAPIDJSON_ASSERT(index < rangeCount_); + return ranges_.template Bottom()[index]; + } + + const Range& GetRange(SizeType index) const { + RAPIDJSON_ASSERT(index < rangeCount_); + return ranges_.template Bottom()[index]; + } + + template + void Parse(DecodedStream& ds) { + Stack operandStack(allocator_, 256); // Frag + Stack operatorStack(allocator_, 256); // Operator + Stack atomCountStack(allocator_, 256); // unsigned (Atom per parenthesis) + + *atomCountStack.template Push() = 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() < kAlternation) + if (!Eval(operandStack, *operatorStack.template Pop(1))) + return; + *operatorStack.template Push() = kAlternation; + *atomCountStack.template Top() = 0; + break; + + case '(': + *operatorStack.template Push() = kLeftParenthesis; + *atomCountStack.template Push() = 0; + break; + + case ')': + while (!operatorStack.Empty() && *operatorStack.template Top() != kLeftParenthesis) + if (!Eval(operandStack, *operatorStack.template Pop(1))) + return; + if (operatorStack.Empty()) + return; + operatorStack.template Pop(1); + atomCountStack.template Pop(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(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(1))) + return; + + // Link the operand to matching state. + if (operandStack.GetSize() == sizeof(Frag)) { + Frag* e = operandStack.template Pop(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(); + s->out = out; + s->out1 = out1; + s->codepoint = codepoint; + s->rangeStart = kRegexInvalidRange; + return stateCount_++; + } + + void PushOperand(Stack& operandStack, unsigned codepoint) { + SizeType s = NewState(kRegexInvalidState, kRegexInvalidState, codepoint); + *operandStack.template Push() = Frag(s, s, s); + } + + void ImplicitConcatenation(Stack& atomCountStack, Stack& operatorStack) { + if (*atomCountStack.template Top()) + *operatorStack.template Push() = kConcatenation; + (*atomCountStack.template Top())++; + } + + 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& operandStack, Operator op) { + switch (op) { + case kConcatenation: + RAPIDJSON_ASSERT(operandStack.GetSize() >= sizeof(Frag) * 2); + { + Frag e2 = *operandStack.template Pop(1); + Frag e1 = *operandStack.template Pop(1); + Patch(e1.out, e2.start); + *operandStack.template Push() = 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(1); + Frag e1 = *operandStack.template Pop(1); + SizeType s = NewState(e1.start, e2.start, 0); + *operandStack.template Push() = 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(1); + SizeType s = NewState(kRegexInvalidState, e.start, 0); + *operandStack.template Push() = Frag(s, Append(e.out, s), e.minIndex); + return true; + } + return false; + + case kZeroOrMore: + if (operandStack.GetSize() >= sizeof(Frag)) { + Frag e = *operandStack.template Pop(1); + SizeType s = NewState(kRegexInvalidState, e.start, 0); + Patch(e.out, s); + *operandStack.template Push() = Frag(s, s, e.minIndex); + return true; + } + return false; + + case kOneOrMore: + if (operandStack.GetSize() >= sizeof(Frag)) { + Frag e = *operandStack.template Pop(1); + SizeType s = NewState(kRegexInvalidState, e.start, 0); + Patch(e.out, s); + *operandStack.template Push() = Frag(e.start, s, e.minIndex); + return true; + } + return false; + + default: + // syntax error (e.g. unclosed kLeftParenthesis) + return false; + } + } + + bool EvalQuantifier(Stack& 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& operandStack) { + const Frag src = *operandStack.template Top(); // Copy constructor to prevent invalidation + SizeType count = stateCount_ - src.minIndex; // Assumes top operand contains states in [src->minIndex, stateCount_) + State* s = states_.template Push(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(src.start + count, src.out + count, src.minIndex + count); + stateCount_ += count; + } + + template + bool ParseUnsigned(DecodedStream& 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 + bool ParseRange(DecodedStream& 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(); + r->start = r->end = codepoint; + r->next = kRegexInvalidRange; + return rangeCount_++; + } + + template + bool CharacterEscape(DecodedStream& 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 states_; + Stack ranges_; + SizeType root_; + SizeType stateCount_; + SizeType rangeCount_; + + static const unsigned kInfinityQuantifier = ~0u; + + // For SearchWithAnchoring() + bool anchorBegin_; + bool anchorEnd_; +}; + +template +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(allocator_->Malloc(GetStateSetSize())); + state0_.template Reserve(regex_.stateCount_); + state1_.template Reserve(regex_.stateCount_); + } + + ~GenericRegexSearch() { + Allocator::Free(stateSet_); + RAPIDJSON_DELETE(ownAllocator_); + } + + template + bool Match(InputStream& is) { + return SearchWithAnchoring(is, true, true); + } + + bool Match(const Ch* s) { + GenericStringStream is(s); + return Match(is); + } + + template + bool Search(InputStream& is) { + return SearchWithAnchoring(is, regex_.anchorBegin_, regex_.anchorEnd_); + } + + bool Search(const Ch* s) { + GenericStringStream is(s); + return Search(is); + } + +private: + typedef typename RegexType::State State; + typedef typename RegexType::Range Range; + + template + bool SearchWithAnchoring(InputStream& is, bool anchorBegin, bool anchorEnd) { + DecodedStream ds(is); + + state0_.Clear(); + Stack *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(); s != current->template End(); ++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& 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() = 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 state0_; + Stack state1_; + uint32_t* stateSet_; +}; + +typedef GenericRegex > Regex; +typedef GenericRegexSearch 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_ diff --git a/livox_ros_driver/common/rapidjson/internal/stack.h b/livox_ros_driver/common/rapidjson/internal/stack.h new file mode 100644 index 0000000..45dca6a --- /dev/null +++ b/livox_ros_driver/common/rapidjson/internal/stack.h @@ -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 + +#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 +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 + RAPIDJSON_FORCEINLINE void Reserve(size_t count = 1) { + // Expand the stack if needed + if (RAPIDJSON_UNLIKELY(static_cast(sizeof(T) * count) > (stackEnd_ - stackTop_))) + Expand(count); + } + + template + RAPIDJSON_FORCEINLINE T* Push(size_t count = 1) { + Reserve(count); + return PushUnsafe(count); + } + + template + RAPIDJSON_FORCEINLINE T* PushUnsafe(size_t count = 1) { + RAPIDJSON_ASSERT(stackTop_); + RAPIDJSON_ASSERT(static_cast(sizeof(T) * count) <= (stackEnd_ - stackTop_)); + T* ret = reinterpret_cast(stackTop_); + stackTop_ += sizeof(T) * count; + return ret; + } + + template + T* Pop(size_t count) { + RAPIDJSON_ASSERT(GetSize() >= count * sizeof(T)); + stackTop_ -= count * sizeof(T); + return reinterpret_cast(stackTop_); + } + + template + T* Top() { + RAPIDJSON_ASSERT(GetSize() >= sizeof(T)); + return reinterpret_cast(stackTop_ - sizeof(T)); + } + + template + const T* Top() const { + RAPIDJSON_ASSERT(GetSize() >= sizeof(T)); + return reinterpret_cast(stackTop_ - sizeof(T)); + } + + template + T* End() { return reinterpret_cast(stackTop_); } + + template + const T* End() const { return reinterpret_cast(stackTop_); } + + template + T* Bottom() { return reinterpret_cast(stack_); } + + template + const T* Bottom() const { return reinterpret_cast(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(stackTop_ - stack_); } + size_t GetCapacity() const { return static_cast(stackEnd_ - stack_); } + +private: + template + 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(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_ diff --git a/livox_ros_driver/common/rapidjson/internal/strfunc.h b/livox_ros_driver/common/rapidjson/internal/strfunc.h new file mode 100644 index 0000000..226439a --- /dev/null +++ b/livox_ros_driver/common/rapidjson/internal/strfunc.h @@ -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 + +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 +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 +bool CountStringCodePoint(const typename Encoding::Ch* s, SizeType length, SizeType* outCount) { + RAPIDJSON_ASSERT(s != 0); + RAPIDJSON_ASSERT(outCount != 0); + GenericStringStream 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_ diff --git a/livox_ros_driver/common/rapidjson/internal/strtod.h b/livox_ros_driver/common/rapidjson/internal/strtod.h new file mode 100644 index 0000000..dfca22b --- /dev/null +++ b/livox_ros_driver/common/rapidjson/internal/strtod.h @@ -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 +#include + +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 +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(dS_Exp5)) <<= static_cast(dS_Exp2); + + BigInteger bS(bInt); + bS.MultiplyPow5(static_cast(bS_Exp5)) <<= static_cast(bS_Exp2); + + BigInteger hS(1); + hS.MultiplyPow5(static_cast(hS_Exp5)) <<= static_cast(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(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(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(error) >= precisionBits || precisionBits >= halfWay + static_cast(error); +} + +inline double StrtodBigInteger(double approx, const char* decimals, int dLen, int dExp) { + RAPIDJSON_ASSERT(dLen >= 0); + const BigInteger dInt(decimals, static_cast(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(length); + + RAPIDJSON_ASSERT(length >= decimalPosition); + RAPIDJSON_ASSERT(length - decimalPosition <= INT_MAX); + int dExpAdjust = static_cast(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::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_ diff --git a/livox_ros_driver/common/rapidjson/internal/swap.h b/livox_ros_driver/common/rapidjson/internal/swap.h new file mode 100644 index 0000000..666e49f --- /dev/null +++ b/livox_ros_driver/common/rapidjson/internal/swap.h @@ -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++ 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 +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_ diff --git a/livox_ros_driver/common/rapidjson/istreamwrapper.h b/livox_ros_driver/common/rapidjson/istreamwrapper.h new file mode 100644 index 0000000..c4950b9 --- /dev/null +++ b/livox_ros_driver/common/rapidjson/istreamwrapper.h @@ -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 +#include + +#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 +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(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(bufferSize_))) { + readCount_ = static_cast(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 IStreamWrapper; +typedef BasicIStreamWrapper WIStreamWrapper; + +#if defined(__clang__) || defined(_MSC_VER) +RAPIDJSON_DIAG_POP +#endif + +RAPIDJSON_NAMESPACE_END + +#endif // RAPIDJSON_ISTREAMWRAPPER_H_ diff --git a/livox_ros_driver/common/rapidjson/memorybuffer.h b/livox_ros_driver/common/rapidjson/memorybuffer.h new file mode 100644 index 0000000..39bee1d --- /dev/null +++ b/livox_ros_driver/common/rapidjson/memorybuffer.h @@ -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 +struct GenericMemoryBuffer { + typedef char Ch; // byte + + GenericMemoryBuffer(Allocator* allocator = 0, size_t capacity = kDefaultCapacity) : stack_(allocator, capacity) {} + + void Put(Ch c) { *stack_.template Push() = c; } + void Flush() {} + + void Clear() { stack_.Clear(); } + void ShrinkToFit() { stack_.ShrinkToFit(); } + Ch* Push(size_t count) { return stack_.template Push(count); } + void Pop(size_t count) { stack_.template Pop(count); } + + const Ch* GetBuffer() const { + return stack_.template Bottom(); + } + + size_t GetSize() const { return stack_.GetSize(); } + + static const size_t kDefaultCapacity = 256; + mutable internal::Stack 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(n), c, n * sizeof(c)); +} + +RAPIDJSON_NAMESPACE_END + +#endif // RAPIDJSON_MEMORYBUFFER_H_ diff --git a/livox_ros_driver/common/rapidjson/memorystream.h b/livox_ros_driver/common/rapidjson/memorystream.h new file mode 100644 index 0000000..1d71d8a --- /dev/null +++ b/livox_ros_driver/common/rapidjson/memorystream.h @@ -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(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_ diff --git a/livox_ros_driver/common/rapidjson/msinttypes/inttypes.h b/livox_ros_driver/common/rapidjson/msinttypes/inttypes.h new file mode 100644 index 0000000..1811128 --- /dev/null +++ b/livox_ros_driver/common/rapidjson/msinttypes/inttypes.h @@ -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 +#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_ ] diff --git a/livox_ros_driver/common/rapidjson/msinttypes/stdint.h b/livox_ros_driver/common/rapidjson/msinttypes/stdint.h new file mode 100644 index 0000000..3d4477b --- /dev/null +++ b/livox_ros_driver/common/rapidjson/msinttypes/stdint.h @@ -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 + +#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 . +// 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 + +// For Visual Studio 6 in C++ mode and for many Visual Studio versions when +// compiling for ARM we have to wrap 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 +#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 +#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 . +// 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_ ] diff --git a/livox_ros_driver/common/rapidjson/ostreamwrapper.h b/livox_ros_driver/common/rapidjson/ostreamwrapper.h new file mode 100644 index 0000000..6f4667c --- /dev/null +++ b/livox_ros_driver/common/rapidjson/ostreamwrapper.h @@ -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 + +#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 +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 OStreamWrapper; +typedef BasicOStreamWrapper WOStreamWrapper; + +#ifdef __clang__ +RAPIDJSON_DIAG_POP +#endif + +RAPIDJSON_NAMESPACE_END + +#endif // RAPIDJSON_OSTREAMWRAPPER_H_ diff --git a/livox_ros_driver/common/rapidjson/pointer.h b/livox_ros_driver/common/rapidjson/pointer.h new file mode 100644 index 0000000..b8143b6 --- /dev/null +++ b/livox_ros_driver/common/rapidjson/pointer.h @@ -0,0 +1,1415 @@ +// 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_POINTER_H_ +#define RAPIDJSON_POINTER_H_ + +#include "document.h" +#include "internal/itoa.h" + +#ifdef __clang__ +RAPIDJSON_DIAG_PUSH +RAPIDJSON_DIAG_OFF(switch-enum) +#elif defined(_MSC_VER) +RAPIDJSON_DIAG_PUSH +RAPIDJSON_DIAG_OFF(4512) // assignment operator could not be generated +#endif + +RAPIDJSON_NAMESPACE_BEGIN + +static const SizeType kPointerInvalidIndex = ~SizeType(0); //!< Represents an invalid index in GenericPointer::Token + +//! Error code of parsing. +/*! \ingroup RAPIDJSON_ERRORS + \see GenericPointer::GenericPointer, GenericPointer::GetParseErrorCode +*/ +enum PointerParseErrorCode { + kPointerParseErrorNone = 0, //!< The parse is successful + + kPointerParseErrorTokenMustBeginWithSolidus, //!< A token must begin with a '/' + kPointerParseErrorInvalidEscape, //!< Invalid escape + kPointerParseErrorInvalidPercentEncoding, //!< Invalid percent encoding in URI fragment + kPointerParseErrorCharacterMustPercentEncode //!< A character must percent encoded in URI fragment +}; + +/////////////////////////////////////////////////////////////////////////////// +// GenericPointer + +//! Represents a JSON Pointer. Use Pointer for UTF8 encoding and default allocator. +/*! + This class implements RFC 6901 "JavaScript Object Notation (JSON) Pointer" + (https://tools.ietf.org/html/rfc6901). + + A JSON pointer is for identifying a specific value in a JSON document + (GenericDocument). It can simplify coding of DOM tree manipulation, because it + can access multiple-level depth of DOM tree with single API call. + + After it parses a string representation (e.g. "/foo/0" or URI fragment + representation (e.g. "#/foo/0") into its internal representation (tokens), + it can be used to resolve a specific value in multiple documents, or sub-tree + of documents. + + Contrary to GenericValue, Pointer can be copy constructed and copy assigned. + Apart from assignment, a Pointer cannot be modified after construction. + + Although Pointer is very convenient, please aware that constructing Pointer + involves parsing and dynamic memory allocation. A special constructor with user- + supplied tokens eliminates these. + + GenericPointer depends on GenericDocument and GenericValue. + + \tparam ValueType The value type of the DOM tree. E.g. GenericValue > + \tparam Allocator The allocator type for allocating memory for internal representation. + + \note GenericPointer uses same encoding of ValueType. + However, Allocator of GenericPointer is independent of Allocator of Value. +*/ +template +class GenericPointer { +public: + typedef typename ValueType::EncodingType EncodingType; //!< Encoding type from Value + typedef typename ValueType::Ch Ch; //!< Character type from Value + + //! A token is the basic units of internal representation. + /*! + A JSON pointer string representation "/foo/123" is parsed to two tokens: + "foo" and 123. 123 will be represented in both numeric form and string form. + They are resolved according to the actual value type (object or array). + + For token that are not numbers, or the numeric value is out of bound + (greater than limits of SizeType), they are only treated as string form + (i.e. the token's index will be equal to kPointerInvalidIndex). + + This struct is public so that user can create a Pointer without parsing and + allocation, using a special constructor. + */ + struct Token { + const Ch* name; //!< Name of the token. It has null character at the end but it can contain null character. + SizeType length; //!< Length of the name. + SizeType index; //!< A valid array index, if it is not equal to kPointerInvalidIndex. + }; + + //!@name Constructors and destructor. + //@{ + + //! Default constructor. + GenericPointer(Allocator* allocator = 0) : allocator_(allocator), ownAllocator_(), nameBuffer_(), tokens_(), tokenCount_(), parseErrorOffset_(), parseErrorCode_(kPointerParseErrorNone) {} + + //! Constructor that parses a string or URI fragment representation. + /*! + \param source A null-terminated, string or URI fragment representation of JSON pointer. + \param allocator User supplied allocator for this pointer. If no allocator is provided, it creates a self-owned one. + */ + explicit GenericPointer(const Ch* source, Allocator* allocator = 0) : allocator_(allocator), ownAllocator_(), nameBuffer_(), tokens_(), tokenCount_(), parseErrorOffset_(), parseErrorCode_(kPointerParseErrorNone) { + Parse(source, internal::StrLen(source)); + } + +#if RAPIDJSON_HAS_STDSTRING + //! Constructor that parses a string or URI fragment representation. + /*! + \param source A string or URI fragment representation of JSON pointer. + \param allocator User supplied allocator for this pointer. If no allocator is provided, it creates a self-owned one. + \note Requires the definition of the preprocessor symbol \ref RAPIDJSON_HAS_STDSTRING. + */ + explicit GenericPointer(const std::basic_string& source, Allocator* allocator = 0) : allocator_(allocator), ownAllocator_(), nameBuffer_(), tokens_(), tokenCount_(), parseErrorOffset_(), parseErrorCode_(kPointerParseErrorNone) { + Parse(source.c_str(), source.size()); + } +#endif + + //! Constructor that parses a string or URI fragment representation, with length of the source string. + /*! + \param source A string or URI fragment representation of JSON pointer. + \param length Length of source. + \param allocator User supplied allocator for this pointer. If no allocator is provided, it creates a self-owned one. + \note Slightly faster than the overload without length. + */ + GenericPointer(const Ch* source, size_t length, Allocator* allocator = 0) : allocator_(allocator), ownAllocator_(), nameBuffer_(), tokens_(), tokenCount_(), parseErrorOffset_(), parseErrorCode_(kPointerParseErrorNone) { + Parse(source, length); + } + + //! Constructor with user-supplied tokens. + /*! + This constructor let user supplies const array of tokens. + This prevents the parsing process and eliminates allocation. + This is preferred for memory constrained environments. + + \param tokens An constant array of tokens representing the JSON pointer. + \param tokenCount Number of tokens. + + \b Example + \code + #define NAME(s) { s, sizeof(s) / sizeof(s[0]) - 1, kPointerInvalidIndex } + #define INDEX(i) { #i, sizeof(#i) - 1, i } + + static const Pointer::Token kTokens[] = { NAME("foo"), INDEX(123) }; + static const Pointer p(kTokens, sizeof(kTokens) / sizeof(kTokens[0])); + // Equivalent to static const Pointer p("/foo/123"); + + #undef NAME + #undef INDEX + \endcode + */ + GenericPointer(const Token* tokens, size_t tokenCount) : allocator_(), ownAllocator_(), nameBuffer_(), tokens_(const_cast(tokens)), tokenCount_(tokenCount), parseErrorOffset_(), parseErrorCode_(kPointerParseErrorNone) {} + + //! Copy constructor. + GenericPointer(const GenericPointer& rhs) : allocator_(rhs.allocator_), ownAllocator_(), nameBuffer_(), tokens_(), tokenCount_(), parseErrorOffset_(), parseErrorCode_(kPointerParseErrorNone) { + *this = rhs; + } + + //! Copy constructor. + GenericPointer(const GenericPointer& rhs, Allocator* allocator) : allocator_(allocator), ownAllocator_(), nameBuffer_(), tokens_(), tokenCount_(), parseErrorOffset_(), parseErrorCode_(kPointerParseErrorNone) { + *this = rhs; + } + + //! Destructor. + ~GenericPointer() { + if (nameBuffer_) // If user-supplied tokens constructor is used, nameBuffer_ is nullptr and tokens_ are not deallocated. + Allocator::Free(tokens_); + RAPIDJSON_DELETE(ownAllocator_); + } + + //! Assignment operator. + GenericPointer& operator=(const GenericPointer& rhs) { + if (this != &rhs) { + // Do not delete ownAllcator + if (nameBuffer_) + Allocator::Free(tokens_); + + tokenCount_ = rhs.tokenCount_; + parseErrorOffset_ = rhs.parseErrorOffset_; + parseErrorCode_ = rhs.parseErrorCode_; + + if (rhs.nameBuffer_) + CopyFromRaw(rhs); // Normally parsed tokens. + else { + tokens_ = rhs.tokens_; // User supplied const tokens. + nameBuffer_ = 0; + } + } + return *this; + } + + //! Swap the content of this pointer with an other. + /*! + \param other The pointer to swap with. + \note Constant complexity. + */ + GenericPointer& Swap(GenericPointer& other) RAPIDJSON_NOEXCEPT { + internal::Swap(allocator_, other.allocator_); + internal::Swap(ownAllocator_, other.ownAllocator_); + internal::Swap(nameBuffer_, other.nameBuffer_); + internal::Swap(tokens_, other.tokens_); + internal::Swap(tokenCount_, other.tokenCount_); + internal::Swap(parseErrorOffset_, other.parseErrorOffset_); + internal::Swap(parseErrorCode_, other.parseErrorCode_); + return *this; + } + + //! free-standing swap function helper + /*! + Helper function to enable support for common swap implementation pattern based on \c std::swap: + \code + void swap(MyClass& a, MyClass& b) { + using std::swap; + swap(a.pointer, b.pointer); + // ... + } + \endcode + \see Swap() + */ + friend inline void swap(GenericPointer& a, GenericPointer& b) RAPIDJSON_NOEXCEPT { a.Swap(b); } + + //@} + + //!@name Append token + //@{ + + //! Append a token and return a new Pointer + /*! + \param token Token to be appended. + \param allocator Allocator for the newly return Pointer. + \return A new Pointer with appended token. + */ + GenericPointer Append(const Token& token, Allocator* allocator = 0) const { + GenericPointer r; + r.allocator_ = allocator; + Ch *p = r.CopyFromRaw(*this, 1, token.length + 1); + std::memcpy(p, token.name, (token.length + 1) * sizeof(Ch)); + r.tokens_[tokenCount_].name = p; + r.tokens_[tokenCount_].length = token.length; + r.tokens_[tokenCount_].index = token.index; + return r; + } + + //! Append a name token with length, and return a new Pointer + /*! + \param name Name to be appended. + \param length Length of name. + \param allocator Allocator for the newly return Pointer. + \return A new Pointer with appended token. + */ + GenericPointer Append(const Ch* name, SizeType length, Allocator* allocator = 0) const { + Token token = { name, length, kPointerInvalidIndex }; + return Append(token, allocator); + } + + //! Append a name token without length, and return a new Pointer + /*! + \param name Name (const Ch*) to be appended. + \param allocator Allocator for the newly return Pointer. + \return A new Pointer with appended token. + */ + template + RAPIDJSON_DISABLEIF_RETURN((internal::NotExpr::Type, Ch> >), (GenericPointer)) + Append(T* name, Allocator* allocator = 0) const { + return Append(name, internal::StrLen(name), allocator); + } + +#if RAPIDJSON_HAS_STDSTRING + //! Append a name token, and return a new Pointer + /*! + \param name Name to be appended. + \param allocator Allocator for the newly return Pointer. + \return A new Pointer with appended token. + */ + GenericPointer Append(const std::basic_string& name, Allocator* allocator = 0) const { + return Append(name.c_str(), static_cast(name.size()), allocator); + } +#endif + + //! Append a index token, and return a new Pointer + /*! + \param index Index to be appended. + \param allocator Allocator for the newly return Pointer. + \return A new Pointer with appended token. + */ + GenericPointer Append(SizeType index, Allocator* allocator = 0) const { + char buffer[21]; + char* end = sizeof(SizeType) == 4 ? internal::u32toa(index, buffer) : internal::u64toa(index, buffer); + SizeType length = static_cast(end - buffer); + buffer[length] = '\0'; + + if (sizeof(Ch) == 1) { + Token token = { reinterpret_cast(buffer), length, index }; + return Append(token, allocator); + } + else { + Ch name[21]; + for (size_t i = 0; i <= length; i++) + name[i] = static_cast(buffer[i]); + Token token = { name, length, index }; + return Append(token, allocator); + } + } + + //! Append a token by value, and return a new Pointer + /*! + \param token token to be appended. + \param allocator Allocator for the newly return Pointer. + \return A new Pointer with appended token. + */ + GenericPointer Append(const ValueType& token, Allocator* allocator = 0) const { + if (token.IsString()) + return Append(token.GetString(), token.GetStringLength(), allocator); + else { + RAPIDJSON_ASSERT(token.IsUint64()); + RAPIDJSON_ASSERT(token.GetUint64() <= SizeType(~0)); + return Append(static_cast(token.GetUint64()), allocator); + } + } + + //!@name Handling Parse Error + //@{ + + //! Check whether this is a valid pointer. + bool IsValid() const { return parseErrorCode_ == kPointerParseErrorNone; } + + //! Get the parsing error offset in code unit. + size_t GetParseErrorOffset() const { return parseErrorOffset_; } + + //! Get the parsing error code. + PointerParseErrorCode GetParseErrorCode() const { return parseErrorCode_; } + + //@} + + //! Get the allocator of this pointer. + Allocator& GetAllocator() { return *allocator_; } + + //!@name Tokens + //@{ + + //! Get the token array (const version only). + const Token* GetTokens() const { return tokens_; } + + //! Get the number of tokens. + size_t GetTokenCount() const { return tokenCount_; } + + //@} + + //!@name Equality/inequality operators + //@{ + + //! Equality operator. + /*! + \note When any pointers are invalid, always returns false. + */ + bool operator==(const GenericPointer& rhs) const { + if (!IsValid() || !rhs.IsValid() || tokenCount_ != rhs.tokenCount_) + return false; + + for (size_t i = 0; i < tokenCount_; i++) { + if (tokens_[i].index != rhs.tokens_[i].index || + tokens_[i].length != rhs.tokens_[i].length || + (tokens_[i].length != 0 && std::memcmp(tokens_[i].name, rhs.tokens_[i].name, sizeof(Ch)* tokens_[i].length) != 0)) + { + return false; + } + } + + return true; + } + + //! Inequality operator. + /*! + \note When any pointers are invalid, always returns true. + */ + bool operator!=(const GenericPointer& rhs) const { return !(*this == rhs); } + + //! Less than operator. + /*! + \note Invalid pointers are always greater than valid ones. + */ + bool operator<(const GenericPointer& rhs) const { + if (!IsValid()) + return false; + if (!rhs.IsValid()) + return true; + + if (tokenCount_ != rhs.tokenCount_) + return tokenCount_ < rhs.tokenCount_; + + for (size_t i = 0; i < tokenCount_; i++) { + if (tokens_[i].index != rhs.tokens_[i].index) + return tokens_[i].index < rhs.tokens_[i].index; + + if (tokens_[i].length != rhs.tokens_[i].length) + return tokens_[i].length < rhs.tokens_[i].length; + + if (int cmp = std::memcmp(tokens_[i].name, rhs.tokens_[i].name, sizeof(Ch) * tokens_[i].length)) + return cmp < 0; + } + + return false; + } + + //@} + + //!@name Stringify + //@{ + + //! Stringify the pointer into string representation. + /*! + \tparam OutputStream Type of output stream. + \param os The output stream. + */ + template + bool Stringify(OutputStream& os) const { + return Stringify(os); + } + + //! Stringify the pointer into URI fragment representation. + /*! + \tparam OutputStream Type of output stream. + \param os The output stream. + */ + template + bool StringifyUriFragment(OutputStream& os) const { + return Stringify(os); + } + + //@} + + //!@name Create value + //@{ + + //! Create a value in a subtree. + /*! + If the value is not exist, it creates all parent values and a JSON Null value. + So it always succeed and return the newly created or existing value. + + Remind that it may change types of parents according to tokens, so it + potentially removes previously stored values. For example, if a document + was an array, and "/foo" is used to create a value, then the document + will be changed to an object, and all existing array elements are lost. + + \param root Root value of a DOM subtree to be resolved. It can be any value other than document root. + \param allocator Allocator for creating the values if the specified value or its parents are not exist. + \param alreadyExist If non-null, it stores whether the resolved value is already exist. + \return The resolved newly created (a JSON Null value), or already exists value. + */ + ValueType& Create(ValueType& root, typename ValueType::AllocatorType& allocator, bool* alreadyExist = 0) const { + RAPIDJSON_ASSERT(IsValid()); + ValueType* v = &root; + bool exist = true; + for (const Token *t = tokens_; t != tokens_ + tokenCount_; ++t) { + if (v->IsArray() && t->name[0] == '-' && t->length == 1) { + v->PushBack(ValueType().Move(), allocator); + v = &((*v)[v->Size() - 1]); + exist = false; + } + else { + if (t->index == kPointerInvalidIndex) { // must be object name + if (!v->IsObject()) + v->SetObject(); // Change to Object + } + else { // object name or array index + if (!v->IsArray() && !v->IsObject()) + v->SetArray(); // Change to Array + } + + if (v->IsArray()) { + if (t->index >= v->Size()) { + v->Reserve(t->index + 1, allocator); + while (t->index >= v->Size()) + v->PushBack(ValueType().Move(), allocator); + exist = false; + } + v = &((*v)[t->index]); + } + else { + typename ValueType::MemberIterator m = v->FindMember(GenericValue(GenericStringRef(t->name, t->length))); + if (m == v->MemberEnd()) { + v->AddMember(ValueType(t->name, t->length, allocator).Move(), ValueType().Move(), allocator); + m = v->MemberEnd(); + v = &(--m)->value; // Assumes AddMember() appends at the end + exist = false; + } + else + v = &m->value; + } + } + } + + if (alreadyExist) + *alreadyExist = exist; + + return *v; + } + + //! Creates a value in a document. + /*! + \param document A document to be resolved. + \param alreadyExist If non-null, it stores whether the resolved value is already exist. + \return The resolved newly created, or already exists value. + */ + template + ValueType& Create(GenericDocument& document, bool* alreadyExist = 0) const { + return Create(document, document.GetAllocator(), alreadyExist); + } + + //@} + + //!@name Query value + //@{ + + //! Query a value in a subtree. + /*! + \param root Root value of a DOM sub-tree to be resolved. It can be any value other than document root. + \param unresolvedTokenIndex If the pointer cannot resolve a token in the pointer, this parameter can obtain the index of unresolved token. + \return Pointer to the value if it can be resolved. Otherwise null. + + \note + There are only 3 situations when a value cannot be resolved: + 1. A value in the path is not an array nor object. + 2. An object value does not contain the token. + 3. A token is out of range of an array value. + + Use unresolvedTokenIndex to retrieve the token index. + */ + ValueType* Get(ValueType& root, size_t* unresolvedTokenIndex = 0) const { + RAPIDJSON_ASSERT(IsValid()); + ValueType* v = &root; + for (const Token *t = tokens_; t != tokens_ + tokenCount_; ++t) { + switch (v->GetType()) { + case kObjectType: + { + typename ValueType::MemberIterator m = v->FindMember(GenericValue(GenericStringRef(t->name, t->length))); + if (m == v->MemberEnd()) + break; + v = &m->value; + } + continue; + case kArrayType: + if (t->index == kPointerInvalidIndex || t->index >= v->Size()) + break; + v = &((*v)[t->index]); + continue; + default: + break; + } + + // Error: unresolved token + if (unresolvedTokenIndex) + *unresolvedTokenIndex = static_cast(t - tokens_); + return 0; + } + return v; + } + + //! Query a const value in a const subtree. + /*! + \param root Root value of a DOM sub-tree to be resolved. It can be any value other than document root. + \return Pointer to the value if it can be resolved. Otherwise null. + */ + const ValueType* Get(const ValueType& root, size_t* unresolvedTokenIndex = 0) const { + return Get(const_cast(root), unresolvedTokenIndex); + } + + //@} + + //!@name Query a value with default + //@{ + + //! Query a value in a subtree with default value. + /*! + Similar to Get(), but if the specified value do not exists, it creates all parents and clone the default value. + So that this function always succeed. + + \param root Root value of a DOM sub-tree to be resolved. It can be any value other than document root. + \param defaultValue Default value to be cloned if the value was not exists. + \param allocator Allocator for creating the values if the specified value or its parents are not exist. + \see Create() + */ + ValueType& GetWithDefault(ValueType& root, const ValueType& defaultValue, typename ValueType::AllocatorType& allocator) const { + bool alreadyExist; + ValueType& v = Create(root, allocator, &alreadyExist); + return alreadyExist ? v : v.CopyFrom(defaultValue, allocator); + } + + //! Query a value in a subtree with default null-terminated string. + ValueType& GetWithDefault(ValueType& root, const Ch* defaultValue, typename ValueType::AllocatorType& allocator) const { + bool alreadyExist; + ValueType& v = Create(root, allocator, &alreadyExist); + return alreadyExist ? v : v.SetString(defaultValue, allocator); + } + +#if RAPIDJSON_HAS_STDSTRING + //! Query a value in a subtree with default std::basic_string. + ValueType& GetWithDefault(ValueType& root, const std::basic_string& defaultValue, typename ValueType::AllocatorType& allocator) const { + bool alreadyExist; + ValueType& v = Create(root, allocator, &alreadyExist); + return alreadyExist ? v : v.SetString(defaultValue, allocator); + } +#endif + + //! Query a value in a subtree with default primitive value. + /*! + \tparam T Either \ref Type, \c int, \c unsigned, \c int64_t, \c uint64_t, \c bool + */ + template + RAPIDJSON_DISABLEIF_RETURN((internal::OrExpr, internal::IsGenericValue >), (ValueType&)) + GetWithDefault(ValueType& root, T defaultValue, typename ValueType::AllocatorType& allocator) const { + return GetWithDefault(root, ValueType(defaultValue).Move(), allocator); + } + + //! Query a value in a document with default value. + template + ValueType& GetWithDefault(GenericDocument& document, const ValueType& defaultValue) const { + return GetWithDefault(document, defaultValue, document.GetAllocator()); + } + + //! Query a value in a document with default null-terminated string. + template + ValueType& GetWithDefault(GenericDocument& document, const Ch* defaultValue) const { + return GetWithDefault(document, defaultValue, document.GetAllocator()); + } + +#if RAPIDJSON_HAS_STDSTRING + //! Query a value in a document with default std::basic_string. + template + ValueType& GetWithDefault(GenericDocument& document, const std::basic_string& defaultValue) const { + return GetWithDefault(document, defaultValue, document.GetAllocator()); + } +#endif + + //! Query a value in a document with default primitive value. + /*! + \tparam T Either \ref Type, \c int, \c unsigned, \c int64_t, \c uint64_t, \c bool + */ + template + RAPIDJSON_DISABLEIF_RETURN((internal::OrExpr, internal::IsGenericValue >), (ValueType&)) + GetWithDefault(GenericDocument& document, T defaultValue) const { + return GetWithDefault(document, defaultValue, document.GetAllocator()); + } + + //@} + + //!@name Set a value + //@{ + + //! Set a value in a subtree, with move semantics. + /*! + It creates all parents if they are not exist or types are different to the tokens. + So this function always succeeds but potentially remove existing values. + + \param root Root value of a DOM sub-tree to be resolved. It can be any value other than document root. + \param value Value to be set. + \param allocator Allocator for creating the values if the specified value or its parents are not exist. + \see Create() + */ + ValueType& Set(ValueType& root, ValueType& value, typename ValueType::AllocatorType& allocator) const { + return Create(root, allocator) = value; + } + + //! Set a value in a subtree, with copy semantics. + ValueType& Set(ValueType& root, const ValueType& value, typename ValueType::AllocatorType& allocator) const { + return Create(root, allocator).CopyFrom(value, allocator); + } + + //! Set a null-terminated string in a subtree. + ValueType& Set(ValueType& root, const Ch* value, typename ValueType::AllocatorType& allocator) const { + return Create(root, allocator) = ValueType(value, allocator).Move(); + } + +#if RAPIDJSON_HAS_STDSTRING + //! Set a std::basic_string in a subtree. + ValueType& Set(ValueType& root, const std::basic_string& value, typename ValueType::AllocatorType& allocator) const { + return Create(root, allocator) = ValueType(value, allocator).Move(); + } +#endif + + //! Set a primitive value in a subtree. + /*! + \tparam T Either \ref Type, \c int, \c unsigned, \c int64_t, \c uint64_t, \c bool + */ + template + RAPIDJSON_DISABLEIF_RETURN((internal::OrExpr, internal::IsGenericValue >), (ValueType&)) + Set(ValueType& root, T value, typename ValueType::AllocatorType& allocator) const { + return Create(root, allocator) = ValueType(value).Move(); + } + + //! Set a value in a document, with move semantics. + template + ValueType& Set(GenericDocument& document, ValueType& value) const { + return Create(document) = value; + } + + //! Set a value in a document, with copy semantics. + template + ValueType& Set(GenericDocument& document, const ValueType& value) const { + return Create(document).CopyFrom(value, document.GetAllocator()); + } + + //! Set a null-terminated string in a document. + template + ValueType& Set(GenericDocument& document, const Ch* value) const { + return Create(document) = ValueType(value, document.GetAllocator()).Move(); + } + +#if RAPIDJSON_HAS_STDSTRING + //! Sets a std::basic_string in a document. + template + ValueType& Set(GenericDocument& document, const std::basic_string& value) const { + return Create(document) = ValueType(value, document.GetAllocator()).Move(); + } +#endif + + //! Set a primitive value in a document. + /*! + \tparam T Either \ref Type, \c int, \c unsigned, \c int64_t, \c uint64_t, \c bool + */ + template + RAPIDJSON_DISABLEIF_RETURN((internal::OrExpr, internal::IsGenericValue >), (ValueType&)) + Set(GenericDocument& document, T value) const { + return Create(document) = value; + } + + //@} + + //!@name Swap a value + //@{ + + //! Swap a value with a value in a subtree. + /*! + It creates all parents if they are not exist or types are different to the tokens. + So this function always succeeds but potentially remove existing values. + + \param root Root value of a DOM sub-tree to be resolved. It can be any value other than document root. + \param value Value to be swapped. + \param allocator Allocator for creating the values if the specified value or its parents are not exist. + \see Create() + */ + ValueType& Swap(ValueType& root, ValueType& value, typename ValueType::AllocatorType& allocator) const { + return Create(root, allocator).Swap(value); + } + + //! Swap a value with a value in a document. + template + ValueType& Swap(GenericDocument& document, ValueType& value) const { + return Create(document).Swap(value); + } + + //@} + + //! Erase a value in a subtree. + /*! + \param root Root value of a DOM sub-tree to be resolved. It can be any value other than document root. + \return Whether the resolved value is found and erased. + + \note Erasing with an empty pointer \c Pointer(""), i.e. the root, always fail and return false. + */ + bool Erase(ValueType& root) const { + RAPIDJSON_ASSERT(IsValid()); + if (tokenCount_ == 0) // Cannot erase the root + return false; + + ValueType* v = &root; + const Token* last = tokens_ + (tokenCount_ - 1); + for (const Token *t = tokens_; t != last; ++t) { + switch (v->GetType()) { + case kObjectType: + { + typename ValueType::MemberIterator m = v->FindMember(GenericValue(GenericStringRef(t->name, t->length))); + if (m == v->MemberEnd()) + return false; + v = &m->value; + } + break; + case kArrayType: + if (t->index == kPointerInvalidIndex || t->index >= v->Size()) + return false; + v = &((*v)[t->index]); + break; + default: + return false; + } + } + + switch (v->GetType()) { + case kObjectType: + return v->EraseMember(GenericStringRef(last->name, last->length)); + case kArrayType: + if (last->index == kPointerInvalidIndex || last->index >= v->Size()) + return false; + v->Erase(v->Begin() + last->index); + return true; + default: + return false; + } + } + +private: + //! Clone the content from rhs to this. + /*! + \param rhs Source pointer. + \param extraToken Extra tokens to be allocated. + \param extraNameBufferSize Extra name buffer size (in number of Ch) to be allocated. + \return Start of non-occupied name buffer, for storing extra names. + */ + Ch* CopyFromRaw(const GenericPointer& rhs, size_t extraToken = 0, size_t extraNameBufferSize = 0) { + if (!allocator_) // allocator is independently owned. + ownAllocator_ = allocator_ = RAPIDJSON_NEW(Allocator)(); + + size_t nameBufferSize = rhs.tokenCount_; // null terminators for tokens + for (Token *t = rhs.tokens_; t != rhs.tokens_ + rhs.tokenCount_; ++t) + nameBufferSize += t->length; + + tokenCount_ = rhs.tokenCount_ + extraToken; + tokens_ = static_cast(allocator_->Malloc(tokenCount_ * sizeof(Token) + (nameBufferSize + extraNameBufferSize) * sizeof(Ch))); + nameBuffer_ = reinterpret_cast(tokens_ + tokenCount_); + if (rhs.tokenCount_ > 0) { + std::memcpy(tokens_, rhs.tokens_, rhs.tokenCount_ * sizeof(Token)); + } + if (nameBufferSize > 0) { + std::memcpy(nameBuffer_, rhs.nameBuffer_, nameBufferSize * sizeof(Ch)); + } + + // Adjust pointers to name buffer + std::ptrdiff_t diff = nameBuffer_ - rhs.nameBuffer_; + for (Token *t = tokens_; t != tokens_ + rhs.tokenCount_; ++t) + t->name += diff; + + return nameBuffer_ + nameBufferSize; + } + + //! Check whether a character should be percent-encoded. + /*! + According to RFC 3986 2.3 Unreserved Characters. + \param c The character (code unit) to be tested. + */ + bool NeedPercentEncode(Ch c) const { + return !((c >= '0' && c <= '9') || (c >= 'A' && c <='Z') || (c >= 'a' && c <= 'z') || c == '-' || c == '.' || c == '_' || c =='~'); + } + + //! Parse a JSON String or its URI fragment representation into tokens. +#ifndef __clang__ // -Wdocumentation + /*! + \param source Either a JSON Pointer string, or its URI fragment representation. Not need to be null terminated. + \param length Length of the source string. + \note Source cannot be JSON String Representation of JSON Pointer, e.g. In "/\u0000", \u0000 will not be unescaped. + */ +#endif + void Parse(const Ch* source, size_t length) { + RAPIDJSON_ASSERT(source != NULL); + RAPIDJSON_ASSERT(nameBuffer_ == 0); + RAPIDJSON_ASSERT(tokens_ == 0); + + // Create own allocator if user did not supply. + if (!allocator_) + ownAllocator_ = allocator_ = RAPIDJSON_NEW(Allocator)(); + + // Count number of '/' as tokenCount + tokenCount_ = 0; + for (const Ch* s = source; s != source + length; s++) + if (*s == '/') + tokenCount_++; + + Token* token = tokens_ = static_cast(allocator_->Malloc(tokenCount_ * sizeof(Token) + length * sizeof(Ch))); + Ch* name = nameBuffer_ = reinterpret_cast(tokens_ + tokenCount_); + size_t i = 0; + + // Detect if it is a URI fragment + bool uriFragment = false; + if (source[i] == '#') { + uriFragment = true; + i++; + } + + if (i != length && source[i] != '/') { + parseErrorCode_ = kPointerParseErrorTokenMustBeginWithSolidus; + goto error; + } + + while (i < length) { + RAPIDJSON_ASSERT(source[i] == '/'); + i++; // consumes '/' + + token->name = name; + bool isNumber = true; + + while (i < length && source[i] != '/') { + Ch c = source[i]; + if (uriFragment) { + // Decoding percent-encoding for URI fragment + if (c == '%') { + PercentDecodeStream is(&source[i], source + length); + GenericInsituStringStream os(name); + Ch* begin = os.PutBegin(); + if (!Transcoder, EncodingType>().Validate(is, os) || !is.IsValid()) { + parseErrorCode_ = kPointerParseErrorInvalidPercentEncoding; + goto error; + } + size_t len = os.PutEnd(begin); + i += is.Tell() - 1; + if (len == 1) + c = *name; + else { + name += len; + isNumber = false; + i++; + continue; + } + } + else if (NeedPercentEncode(c)) { + parseErrorCode_ = kPointerParseErrorCharacterMustPercentEncode; + goto error; + } + } + + i++; + + // Escaping "~0" -> '~', "~1" -> '/' + if (c == '~') { + if (i < length) { + c = source[i]; + if (c == '0') c = '~'; + else if (c == '1') c = '/'; + else { + parseErrorCode_ = kPointerParseErrorInvalidEscape; + goto error; + } + i++; + } + else { + parseErrorCode_ = kPointerParseErrorInvalidEscape; + goto error; + } + } + + // First check for index: all of characters are digit + if (c < '0' || c > '9') + isNumber = false; + + *name++ = c; + } + token->length = static_cast(name - token->name); + if (token->length == 0) + isNumber = false; + *name++ = '\0'; // Null terminator + + // Second check for index: more than one digit cannot have leading zero + if (isNumber && token->length > 1 && token->name[0] == '0') + isNumber = false; + + // String to SizeType conversion + SizeType n = 0; + if (isNumber) { + for (size_t j = 0; j < token->length; j++) { + SizeType m = n * 10 + static_cast(token->name[j] - '0'); + if (m < n) { // overflow detection + isNumber = false; + break; + } + n = m; + } + } + + token->index = isNumber ? n : kPointerInvalidIndex; + token++; + } + + RAPIDJSON_ASSERT(name <= nameBuffer_ + length); // Should not overflow buffer + parseErrorCode_ = kPointerParseErrorNone; + return; + + error: + Allocator::Free(tokens_); + nameBuffer_ = 0; + tokens_ = 0; + tokenCount_ = 0; + parseErrorOffset_ = i; + return; + } + + //! Stringify to string or URI fragment representation. + /*! + \tparam uriFragment True for stringifying to URI fragment representation. False for string representation. + \tparam OutputStream type of output stream. + \param os The output stream. + */ + template + bool Stringify(OutputStream& os) const { + RAPIDJSON_ASSERT(IsValid()); + + if (uriFragment) + os.Put('#'); + + for (Token *t = tokens_; t != tokens_ + tokenCount_; ++t) { + os.Put('/'); + for (size_t j = 0; j < t->length; j++) { + Ch c = t->name[j]; + if (c == '~') { + os.Put('~'); + os.Put('0'); + } + else if (c == '/') { + os.Put('~'); + os.Put('1'); + } + else if (uriFragment && NeedPercentEncode(c)) { + // Transcode to UTF8 sequence + GenericStringStream source(&t->name[j]); + PercentEncodeStream target(os); + if (!Transcoder >().Validate(source, target)) + return false; + j += source.Tell() - 1; + } + else + os.Put(c); + } + } + return true; + } + + //! A helper stream for decoding a percent-encoded sequence into code unit. + /*! + This stream decodes %XY triplet into code unit (0-255). + If it encounters invalid characters, it sets output code unit as 0 and + mark invalid, and to be checked by IsValid(). + */ + class PercentDecodeStream { + public: + typedef typename ValueType::Ch Ch; + + //! Constructor + /*! + \param source Start of the stream + \param end Past-the-end of the stream. + */ + PercentDecodeStream(const Ch* source, const Ch* end) : src_(source), head_(source), end_(end), valid_(true) {} + + Ch Take() { + if (*src_ != '%' || src_ + 3 > end_) { // %XY triplet + valid_ = false; + return 0; + } + src_++; + Ch c = 0; + for (int j = 0; j < 2; j++) { + c = static_cast(c << 4); + Ch h = *src_; + if (h >= '0' && h <= '9') c = static_cast(c + h - '0'); + else if (h >= 'A' && h <= 'F') c = static_cast(c + h - 'A' + 10); + else if (h >= 'a' && h <= 'f') c = static_cast(c + h - 'a' + 10); + else { + valid_ = false; + return 0; + } + src_++; + } + return c; + } + + size_t Tell() const { return static_cast(src_ - head_); } + bool IsValid() const { return valid_; } + + private: + const Ch* src_; //!< Current read position. + const Ch* head_; //!< Original head of the string. + const Ch* end_; //!< Past-the-end position. + bool valid_; //!< Whether the parsing is valid. + }; + + //! A helper stream to encode character (UTF-8 code unit) into percent-encoded sequence. + template + class PercentEncodeStream { + public: + PercentEncodeStream(OutputStream& os) : os_(os) {} + void Put(char c) { // UTF-8 must be byte + unsigned char u = static_cast(c); + static const char hexDigits[16] = { '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F' }; + os_.Put('%'); + os_.Put(static_cast(hexDigits[u >> 4])); + os_.Put(static_cast(hexDigits[u & 15])); + } + private: + OutputStream& os_; + }; + + Allocator* allocator_; //!< The current allocator. It is either user-supplied or equal to ownAllocator_. + Allocator* ownAllocator_; //!< Allocator owned by this Pointer. + Ch* nameBuffer_; //!< A buffer containing all names in tokens. + Token* tokens_; //!< A list of tokens. + size_t tokenCount_; //!< Number of tokens in tokens_. + size_t parseErrorOffset_; //!< Offset in code unit when parsing fail. + PointerParseErrorCode parseErrorCode_; //!< Parsing error code. +}; + +//! GenericPointer for Value (UTF-8, default allocator). +typedef GenericPointer Pointer; + +//!@name Helper functions for GenericPointer +//@{ + +////////////////////////////////////////////////////////////////////////////// + +template +typename T::ValueType& CreateValueByPointer(T& root, const GenericPointer& pointer, typename T::AllocatorType& a) { + return pointer.Create(root, a); +} + +template +typename T::ValueType& CreateValueByPointer(T& root, const CharType(&source)[N], typename T::AllocatorType& a) { + return GenericPointer(source, N - 1).Create(root, a); +} + +// No allocator parameter + +template +typename DocumentType::ValueType& CreateValueByPointer(DocumentType& document, const GenericPointer& pointer) { + return pointer.Create(document); +} + +template +typename DocumentType::ValueType& CreateValueByPointer(DocumentType& document, const CharType(&source)[N]) { + return GenericPointer(source, N - 1).Create(document); +} + +////////////////////////////////////////////////////////////////////////////// + +template +typename T::ValueType* GetValueByPointer(T& root, const GenericPointer& pointer, size_t* unresolvedTokenIndex = 0) { + return pointer.Get(root, unresolvedTokenIndex); +} + +template +const typename T::ValueType* GetValueByPointer(const T& root, const GenericPointer& pointer, size_t* unresolvedTokenIndex = 0) { + return pointer.Get(root, unresolvedTokenIndex); +} + +template +typename T::ValueType* GetValueByPointer(T& root, const CharType (&source)[N], size_t* unresolvedTokenIndex = 0) { + return GenericPointer(source, N - 1).Get(root, unresolvedTokenIndex); +} + +template +const typename T::ValueType* GetValueByPointer(const T& root, const CharType(&source)[N], size_t* unresolvedTokenIndex = 0) { + return GenericPointer(source, N - 1).Get(root, unresolvedTokenIndex); +} + +////////////////////////////////////////////////////////////////////////////// + +template +typename T::ValueType& GetValueByPointerWithDefault(T& root, const GenericPointer& pointer, const typename T::ValueType& defaultValue, typename T::AllocatorType& a) { + return pointer.GetWithDefault(root, defaultValue, a); +} + +template +typename T::ValueType& GetValueByPointerWithDefault(T& root, const GenericPointer& pointer, const typename T::Ch* defaultValue, typename T::AllocatorType& a) { + return pointer.GetWithDefault(root, defaultValue, a); +} + +#if RAPIDJSON_HAS_STDSTRING +template +typename T::ValueType& GetValueByPointerWithDefault(T& root, const GenericPointer& pointer, const std::basic_string& defaultValue, typename T::AllocatorType& a) { + return pointer.GetWithDefault(root, defaultValue, a); +} +#endif + +template +RAPIDJSON_DISABLEIF_RETURN((internal::OrExpr, internal::IsGenericValue >), (typename T::ValueType&)) +GetValueByPointerWithDefault(T& root, const GenericPointer& pointer, T2 defaultValue, typename T::AllocatorType& a) { + return pointer.GetWithDefault(root, defaultValue, a); +} + +template +typename T::ValueType& GetValueByPointerWithDefault(T& root, const CharType(&source)[N], const typename T::ValueType& defaultValue, typename T::AllocatorType& a) { + return GenericPointer(source, N - 1).GetWithDefault(root, defaultValue, a); +} + +template +typename T::ValueType& GetValueByPointerWithDefault(T& root, const CharType(&source)[N], const typename T::Ch* defaultValue, typename T::AllocatorType& a) { + return GenericPointer(source, N - 1).GetWithDefault(root, defaultValue, a); +} + +#if RAPIDJSON_HAS_STDSTRING +template +typename T::ValueType& GetValueByPointerWithDefault(T& root, const CharType(&source)[N], const std::basic_string& defaultValue, typename T::AllocatorType& a) { + return GenericPointer(source, N - 1).GetWithDefault(root, defaultValue, a); +} +#endif + +template +RAPIDJSON_DISABLEIF_RETURN((internal::OrExpr, internal::IsGenericValue >), (typename T::ValueType&)) +GetValueByPointerWithDefault(T& root, const CharType(&source)[N], T2 defaultValue, typename T::AllocatorType& a) { + return GenericPointer(source, N - 1).GetWithDefault(root, defaultValue, a); +} + +// No allocator parameter + +template +typename DocumentType::ValueType& GetValueByPointerWithDefault(DocumentType& document, const GenericPointer& pointer, const typename DocumentType::ValueType& defaultValue) { + return pointer.GetWithDefault(document, defaultValue); +} + +template +typename DocumentType::ValueType& GetValueByPointerWithDefault(DocumentType& document, const GenericPointer& pointer, const typename DocumentType::Ch* defaultValue) { + return pointer.GetWithDefault(document, defaultValue); +} + +#if RAPIDJSON_HAS_STDSTRING +template +typename DocumentType::ValueType& GetValueByPointerWithDefault(DocumentType& document, const GenericPointer& pointer, const std::basic_string& defaultValue) { + return pointer.GetWithDefault(document, defaultValue); +} +#endif + +template +RAPIDJSON_DISABLEIF_RETURN((internal::OrExpr, internal::IsGenericValue >), (typename DocumentType::ValueType&)) +GetValueByPointerWithDefault(DocumentType& document, const GenericPointer& pointer, T2 defaultValue) { + return pointer.GetWithDefault(document, defaultValue); +} + +template +typename DocumentType::ValueType& GetValueByPointerWithDefault(DocumentType& document, const CharType(&source)[N], const typename DocumentType::ValueType& defaultValue) { + return GenericPointer(source, N - 1).GetWithDefault(document, defaultValue); +} + +template +typename DocumentType::ValueType& GetValueByPointerWithDefault(DocumentType& document, const CharType(&source)[N], const typename DocumentType::Ch* defaultValue) { + return GenericPointer(source, N - 1).GetWithDefault(document, defaultValue); +} + +#if RAPIDJSON_HAS_STDSTRING +template +typename DocumentType::ValueType& GetValueByPointerWithDefault(DocumentType& document, const CharType(&source)[N], const std::basic_string& defaultValue) { + return GenericPointer(source, N - 1).GetWithDefault(document, defaultValue); +} +#endif + +template +RAPIDJSON_DISABLEIF_RETURN((internal::OrExpr, internal::IsGenericValue >), (typename DocumentType::ValueType&)) +GetValueByPointerWithDefault(DocumentType& document, const CharType(&source)[N], T2 defaultValue) { + return GenericPointer(source, N - 1).GetWithDefault(document, defaultValue); +} + +////////////////////////////////////////////////////////////////////////////// + +template +typename T::ValueType& SetValueByPointer(T& root, const GenericPointer& pointer, typename T::ValueType& value, typename T::AllocatorType& a) { + return pointer.Set(root, value, a); +} + +template +typename T::ValueType& SetValueByPointer(T& root, const GenericPointer& pointer, const typename T::ValueType& value, typename T::AllocatorType& a) { + return pointer.Set(root, value, a); +} + +template +typename T::ValueType& SetValueByPointer(T& root, const GenericPointer& pointer, const typename T::Ch* value, typename T::AllocatorType& a) { + return pointer.Set(root, value, a); +} + +#if RAPIDJSON_HAS_STDSTRING +template +typename T::ValueType& SetValueByPointer(T& root, const GenericPointer& pointer, const std::basic_string& value, typename T::AllocatorType& a) { + return pointer.Set(root, value, a); +} +#endif + +template +RAPIDJSON_DISABLEIF_RETURN((internal::OrExpr, internal::IsGenericValue >), (typename T::ValueType&)) +SetValueByPointer(T& root, const GenericPointer& pointer, T2 value, typename T::AllocatorType& a) { + return pointer.Set(root, value, a); +} + +template +typename T::ValueType& SetValueByPointer(T& root, const CharType(&source)[N], typename T::ValueType& value, typename T::AllocatorType& a) { + return GenericPointer(source, N - 1).Set(root, value, a); +} + +template +typename T::ValueType& SetValueByPointer(T& root, const CharType(&source)[N], const typename T::ValueType& value, typename T::AllocatorType& a) { + return GenericPointer(source, N - 1).Set(root, value, a); +} + +template +typename T::ValueType& SetValueByPointer(T& root, const CharType(&source)[N], const typename T::Ch* value, typename T::AllocatorType& a) { + return GenericPointer(source, N - 1).Set(root, value, a); +} + +#if RAPIDJSON_HAS_STDSTRING +template +typename T::ValueType& SetValueByPointer(T& root, const CharType(&source)[N], const std::basic_string& value, typename T::AllocatorType& a) { + return GenericPointer(source, N - 1).Set(root, value, a); +} +#endif + +template +RAPIDJSON_DISABLEIF_RETURN((internal::OrExpr, internal::IsGenericValue >), (typename T::ValueType&)) +SetValueByPointer(T& root, const CharType(&source)[N], T2 value, typename T::AllocatorType& a) { + return GenericPointer(source, N - 1).Set(root, value, a); +} + +// No allocator parameter + +template +typename DocumentType::ValueType& SetValueByPointer(DocumentType& document, const GenericPointer& pointer, typename DocumentType::ValueType& value) { + return pointer.Set(document, value); +} + +template +typename DocumentType::ValueType& SetValueByPointer(DocumentType& document, const GenericPointer& pointer, const typename DocumentType::ValueType& value) { + return pointer.Set(document, value); +} + +template +typename DocumentType::ValueType& SetValueByPointer(DocumentType& document, const GenericPointer& pointer, const typename DocumentType::Ch* value) { + return pointer.Set(document, value); +} + +#if RAPIDJSON_HAS_STDSTRING +template +typename DocumentType::ValueType& SetValueByPointer(DocumentType& document, const GenericPointer& pointer, const std::basic_string& value) { + return pointer.Set(document, value); +} +#endif + +template +RAPIDJSON_DISABLEIF_RETURN((internal::OrExpr, internal::IsGenericValue >), (typename DocumentType::ValueType&)) +SetValueByPointer(DocumentType& document, const GenericPointer& pointer, T2 value) { + return pointer.Set(document, value); +} + +template +typename DocumentType::ValueType& SetValueByPointer(DocumentType& document, const CharType(&source)[N], typename DocumentType::ValueType& value) { + return GenericPointer(source, N - 1).Set(document, value); +} + +template +typename DocumentType::ValueType& SetValueByPointer(DocumentType& document, const CharType(&source)[N], const typename DocumentType::ValueType& value) { + return GenericPointer(source, N - 1).Set(document, value); +} + +template +typename DocumentType::ValueType& SetValueByPointer(DocumentType& document, const CharType(&source)[N], const typename DocumentType::Ch* value) { + return GenericPointer(source, N - 1).Set(document, value); +} + +#if RAPIDJSON_HAS_STDSTRING +template +typename DocumentType::ValueType& SetValueByPointer(DocumentType& document, const CharType(&source)[N], const std::basic_string& value) { + return GenericPointer(source, N - 1).Set(document, value); +} +#endif + +template +RAPIDJSON_DISABLEIF_RETURN((internal::OrExpr, internal::IsGenericValue >), (typename DocumentType::ValueType&)) +SetValueByPointer(DocumentType& document, const CharType(&source)[N], T2 value) { + return GenericPointer(source, N - 1).Set(document, value); +} + +////////////////////////////////////////////////////////////////////////////// + +template +typename T::ValueType& SwapValueByPointer(T& root, const GenericPointer& pointer, typename T::ValueType& value, typename T::AllocatorType& a) { + return pointer.Swap(root, value, a); +} + +template +typename T::ValueType& SwapValueByPointer(T& root, const CharType(&source)[N], typename T::ValueType& value, typename T::AllocatorType& a) { + return GenericPointer(source, N - 1).Swap(root, value, a); +} + +template +typename DocumentType::ValueType& SwapValueByPointer(DocumentType& document, const GenericPointer& pointer, typename DocumentType::ValueType& value) { + return pointer.Swap(document, value); +} + +template +typename DocumentType::ValueType& SwapValueByPointer(DocumentType& document, const CharType(&source)[N], typename DocumentType::ValueType& value) { + return GenericPointer(source, N - 1).Swap(document, value); +} + +////////////////////////////////////////////////////////////////////////////// + +template +bool EraseValueByPointer(T& root, const GenericPointer& pointer) { + return pointer.Erase(root); +} + +template +bool EraseValueByPointer(T& root, const CharType(&source)[N]) { + return GenericPointer(source, N - 1).Erase(root); +} + +//@} + +RAPIDJSON_NAMESPACE_END + +#if defined(__clang__) || defined(_MSC_VER) +RAPIDJSON_DIAG_POP +#endif + +#endif // RAPIDJSON_POINTER_H_ diff --git a/livox_ros_driver/common/rapidjson/prettywriter.h b/livox_ros_driver/common/rapidjson/prettywriter.h new file mode 100644 index 0000000..45afb69 --- /dev/null +++ b/livox_ros_driver/common/rapidjson/prettywriter.h @@ -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 TargetEncoding = UTF8<>, typename StackAllocator = CrtAllocator, unsigned writeFlags = kWriteDefaultFlags> +class PrettyWriter : public Writer { +public: + typedef Writer 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(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& str) { + return String(str.data(), SizeType(str.size())); + } +#endif + + bool StartObject() { + PrettyPrefix(kObjectType); + new (Base::level_stack_.template Push()) 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& 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()->inArray); // currently inside an Array, not Object + RAPIDJSON_ASSERT(0 == Base::level_stack_.template Top()->valueCount % 2); // Object has a Key without a Value + + bool empty = Base::level_stack_.template Pop(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(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()->inArray); + bool empty = Base::level_stack_.template Pop(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(); + + 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(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_ diff --git a/livox_ros_driver/common/rapidjson/rapidjson.h b/livox_ros_driver/common/rapidjson/rapidjson.h new file mode 100644 index 0000000..b71ea79 --- /dev/null +++ b/livox_ros_driver/common/rapidjson/rapidjson.h @@ -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 // malloc(), realloc(), free(), size_t +#include // 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 ".." 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 +#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 +#include +#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 +# 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(7u)) & ~static_cast(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(high32) << 32) | static_cast(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((reinterpret_cast(p) & static_cast(RAPIDJSON_UINT64_C2(0xFFFF0000, 0x00000000))) | reinterpret_cast(reinterpret_cast(x)))) +#define RAPIDJSON_GETPOINTER(type, p) (reinterpret_cast(reinterpret_cast(p) & static_cast(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 +#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 struct STATIC_ASSERTION_FAILURE; +template <> struct STATIC_ASSERTION_FAILURE { enum { value = 1 }; }; +template 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)> \ + 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_ diff --git a/livox_ros_driver/common/rapidjson/reader.h b/livox_ros_driver/common/rapidjson/reader.h new file mode 100644 index 0000000..13d27c2 --- /dev/null +++ b/livox_ros_driver/common/rapidjson/reader.h @@ -0,0 +1,2231 @@ +// 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_READER_H_ +#define RAPIDJSON_READER_H_ + +/*! \file reader.h */ + +#include "allocators.h" +#include "stream.h" +#include "encodedstream.h" +#include "internal/clzll.h" +#include "internal/meta.h" +#include "internal/stack.h" +#include "internal/strtod.h" +#include + +#if defined(RAPIDJSON_SIMD) && defined(_MSC_VER) +#include +#pragma intrinsic(_BitScanForward) +#endif +#ifdef RAPIDJSON_SSE42 +#include +#elif defined(RAPIDJSON_SSE2) +#include +#elif defined(RAPIDJSON_NEON) +#include +#endif + +#ifdef __clang__ +RAPIDJSON_DIAG_PUSH +RAPIDJSON_DIAG_OFF(old-style-cast) +RAPIDJSON_DIAG_OFF(padded) +RAPIDJSON_DIAG_OFF(switch-enum) +#elif defined(_MSC_VER) +RAPIDJSON_DIAG_PUSH +RAPIDJSON_DIAG_OFF(4127) // conditional expression is constant +RAPIDJSON_DIAG_OFF(4702) // unreachable code +#endif + +#ifdef __GNUC__ +RAPIDJSON_DIAG_PUSH +RAPIDJSON_DIAG_OFF(effc++) +#endif + +//!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN +#define RAPIDJSON_NOTHING /* deliberately empty */ +#ifndef RAPIDJSON_PARSE_ERROR_EARLY_RETURN +#define RAPIDJSON_PARSE_ERROR_EARLY_RETURN(value) \ + RAPIDJSON_MULTILINEMACRO_BEGIN \ + if (RAPIDJSON_UNLIKELY(HasParseError())) { return value; } \ + RAPIDJSON_MULTILINEMACRO_END +#endif +#define RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID \ + RAPIDJSON_PARSE_ERROR_EARLY_RETURN(RAPIDJSON_NOTHING) +//!@endcond + +/*! \def RAPIDJSON_PARSE_ERROR_NORETURN + \ingroup RAPIDJSON_ERRORS + \brief Macro to indicate a parse error. + \param parseErrorCode \ref rapidjson::ParseErrorCode of the error + \param offset position of the error in JSON input (\c size_t) + + This macros can be used as a customization point for the internal + error handling mechanism of RapidJSON. + + A common usage model is to throw an exception instead of requiring the + caller to explicitly check the \ref rapidjson::GenericReader::Parse's + return value: + + \code + #define RAPIDJSON_PARSE_ERROR_NORETURN(parseErrorCode,offset) \ + throw ParseException(parseErrorCode, #parseErrorCode, offset) + + #include // std::runtime_error + #include "rapidjson/error/error.h" // rapidjson::ParseResult + + struct ParseException : std::runtime_error, rapidjson::ParseResult { + ParseException(rapidjson::ParseErrorCode code, const char* msg, size_t offset) + : std::runtime_error(msg), ParseResult(code, offset) {} + }; + + #include "rapidjson/reader.h" + \endcode + + \see RAPIDJSON_PARSE_ERROR, rapidjson::GenericReader::Parse + */ +#ifndef RAPIDJSON_PARSE_ERROR_NORETURN +#define RAPIDJSON_PARSE_ERROR_NORETURN(parseErrorCode, offset) \ + RAPIDJSON_MULTILINEMACRO_BEGIN \ + RAPIDJSON_ASSERT(!HasParseError()); /* Error can only be assigned once */ \ + SetParseError(parseErrorCode, offset); \ + RAPIDJSON_MULTILINEMACRO_END +#endif + +/*! \def RAPIDJSON_PARSE_ERROR + \ingroup RAPIDJSON_ERRORS + \brief (Internal) macro to indicate and handle a parse error. + \param parseErrorCode \ref rapidjson::ParseErrorCode of the error + \param offset position of the error in JSON input (\c size_t) + + Invokes RAPIDJSON_PARSE_ERROR_NORETURN and stops the parsing. + + \see RAPIDJSON_PARSE_ERROR_NORETURN + \hideinitializer + */ +#ifndef RAPIDJSON_PARSE_ERROR +#define RAPIDJSON_PARSE_ERROR(parseErrorCode, offset) \ + RAPIDJSON_MULTILINEMACRO_BEGIN \ + RAPIDJSON_PARSE_ERROR_NORETURN(parseErrorCode, offset); \ + RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID; \ + RAPIDJSON_MULTILINEMACRO_END +#endif + +#include "error/error.h" // ParseErrorCode, ParseResult + +RAPIDJSON_NAMESPACE_BEGIN + +/////////////////////////////////////////////////////////////////////////////// +// ParseFlag + +/*! \def RAPIDJSON_PARSE_DEFAULT_FLAGS + \ingroup RAPIDJSON_CONFIG + \brief User-defined kParseDefaultFlags definition. + + User can define this as any \c ParseFlag combinations. +*/ +#ifndef RAPIDJSON_PARSE_DEFAULT_FLAGS +#define RAPIDJSON_PARSE_DEFAULT_FLAGS kParseNoFlags +#endif + +//! Combination of parseFlags +/*! \see Reader::Parse, Document::Parse, Document::ParseInsitu, Document::ParseStream + */ +enum ParseFlag { + kParseNoFlags = 0, //!< No flags are set. + kParseInsituFlag = 1, //!< In-situ(destructive) parsing. + kParseValidateEncodingFlag = 2, //!< Validate encoding of JSON strings. + kParseIterativeFlag = 4, //!< Iterative(constant complexity in terms of function call stack size) parsing. + kParseStopWhenDoneFlag = 8, //!< After parsing a complete JSON root from stream, stop further processing the rest of stream. When this flag is used, parser will not generate kParseErrorDocumentRootNotSingular error. + kParseFullPrecisionFlag = 16, //!< Parse number in full precision (but slower). + kParseCommentsFlag = 32, //!< Allow one-line (//) and multi-line (/**/) comments. + kParseNumbersAsStringsFlag = 64, //!< Parse all numbers (ints/doubles) as strings. + kParseTrailingCommasFlag = 128, //!< Allow trailing commas at the end of objects and arrays. + kParseNanAndInfFlag = 256, //!< Allow parsing NaN, Inf, Infinity, -Inf and -Infinity as doubles. + kParseDefaultFlags = RAPIDJSON_PARSE_DEFAULT_FLAGS //!< Default parse flags. Can be customized by defining RAPIDJSON_PARSE_DEFAULT_FLAGS +}; + +/////////////////////////////////////////////////////////////////////////////// +// Handler + +/*! \class rapidjson::Handler + \brief Concept for receiving events from GenericReader upon parsing. + The functions return true if no error occurs. If they return false, + the event publisher should terminate the process. +\code +concept Handler { + typename Ch; + + bool Null(); + bool Bool(bool b); + bool Int(int i); + bool Uint(unsigned i); + bool Int64(int64_t i); + bool Uint64(uint64_t i); + bool Double(double d); + /// enabled via kParseNumbersAsStringsFlag, string is not null-terminated (use length) + bool RawNumber(const Ch* str, SizeType length, bool copy); + bool String(const Ch* str, SizeType length, bool copy); + bool StartObject(); + bool Key(const Ch* str, SizeType length, bool copy); + bool EndObject(SizeType memberCount); + bool StartArray(); + bool EndArray(SizeType elementCount); +}; +\endcode +*/ +/////////////////////////////////////////////////////////////////////////////// +// BaseReaderHandler + +//! Default implementation of Handler. +/*! This can be used as base class of any reader handler. + \note implements Handler concept +*/ +template, typename Derived = void> +struct BaseReaderHandler { + typedef typename Encoding::Ch Ch; + + typedef typename internal::SelectIf, BaseReaderHandler, Derived>::Type Override; + + bool Default() { return true; } + bool Null() { return static_cast(*this).Default(); } + bool Bool(bool) { return static_cast(*this).Default(); } + bool Int(int) { return static_cast(*this).Default(); } + bool Uint(unsigned) { return static_cast(*this).Default(); } + bool Int64(int64_t) { return static_cast(*this).Default(); } + bool Uint64(uint64_t) { return static_cast(*this).Default(); } + bool Double(double) { return static_cast(*this).Default(); } + /// enabled via kParseNumbersAsStringsFlag, string is not null-terminated (use length) + bool RawNumber(const Ch* str, SizeType len, bool copy) { return static_cast(*this).String(str, len, copy); } + bool String(const Ch*, SizeType, bool) { return static_cast(*this).Default(); } + bool StartObject() { return static_cast(*this).Default(); } + bool Key(const Ch* str, SizeType len, bool copy) { return static_cast(*this).String(str, len, copy); } + bool EndObject(SizeType) { return static_cast(*this).Default(); } + bool StartArray() { return static_cast(*this).Default(); } + bool EndArray(SizeType) { return static_cast(*this).Default(); } +}; + +/////////////////////////////////////////////////////////////////////////////// +// StreamLocalCopy + +namespace internal { + +template::copyOptimization> +class StreamLocalCopy; + +//! Do copy optimization. +template +class StreamLocalCopy { +public: + StreamLocalCopy(Stream& original) : s(original), original_(original) {} + ~StreamLocalCopy() { original_ = s; } + + Stream s; + +private: + StreamLocalCopy& operator=(const StreamLocalCopy&) /* = delete */; + + Stream& original_; +}; + +//! Keep reference. +template +class StreamLocalCopy { +public: + StreamLocalCopy(Stream& original) : s(original) {} + + Stream& s; + +private: + StreamLocalCopy& operator=(const StreamLocalCopy&) /* = delete */; +}; + +} // namespace internal + +/////////////////////////////////////////////////////////////////////////////// +// SkipWhitespace + +//! Skip the JSON white spaces in a stream. +/*! \param is A input stream for skipping white spaces. + \note This function has SSE2/SSE4.2 specialization. +*/ +template +void SkipWhitespace(InputStream& is) { + internal::StreamLocalCopy copy(is); + InputStream& s(copy.s); + + typename InputStream::Ch c; + while ((c = s.Peek()) == ' ' || c == '\n' || c == '\r' || c == '\t') + s.Take(); +} + +inline const char* SkipWhitespace(const char* p, const char* end) { + while (p != end && (*p == ' ' || *p == '\n' || *p == '\r' || *p == '\t')) + ++p; + return p; +} + +#ifdef RAPIDJSON_SSE42 +//! Skip whitespace with SSE 4.2 pcmpistrm instruction, testing 16 8-byte characters at once. +inline const char *SkipWhitespace_SIMD(const char* p) { + // Fast return for single non-whitespace + if (*p == ' ' || *p == '\n' || *p == '\r' || *p == '\t') + ++p; + else + return p; + + // 16-byte align to the next boundary + const char* nextAligned = reinterpret_cast((reinterpret_cast(p) + 15) & static_cast(~15)); + while (p != nextAligned) + if (*p == ' ' || *p == '\n' || *p == '\r' || *p == '\t') + ++p; + else + return p; + + // The rest of string using SIMD + static const char whitespace[16] = " \n\r\t"; + const __m128i w = _mm_loadu_si128(reinterpret_cast(&whitespace[0])); + + for (;; p += 16) { + const __m128i s = _mm_load_si128(reinterpret_cast(p)); + const int r = _mm_cmpistri(w, s, _SIDD_UBYTE_OPS | _SIDD_CMP_EQUAL_ANY | _SIDD_LEAST_SIGNIFICANT | _SIDD_NEGATIVE_POLARITY); + if (r != 16) // some of characters is non-whitespace + return p + r; + } +} + +inline const char *SkipWhitespace_SIMD(const char* p, const char* end) { + // Fast return for single non-whitespace + if (p != end && (*p == ' ' || *p == '\n' || *p == '\r' || *p == '\t')) + ++p; + else + return p; + + // The middle of string using SIMD + static const char whitespace[16] = " \n\r\t"; + const __m128i w = _mm_loadu_si128(reinterpret_cast(&whitespace[0])); + + for (; p <= end - 16; p += 16) { + const __m128i s = _mm_loadu_si128(reinterpret_cast(p)); + const int r = _mm_cmpistri(w, s, _SIDD_UBYTE_OPS | _SIDD_CMP_EQUAL_ANY | _SIDD_LEAST_SIGNIFICANT | _SIDD_NEGATIVE_POLARITY); + if (r != 16) // some of characters is non-whitespace + return p + r; + } + + return SkipWhitespace(p, end); +} + +#elif defined(RAPIDJSON_SSE2) + +//! Skip whitespace with SSE2 instructions, testing 16 8-byte characters at once. +inline const char *SkipWhitespace_SIMD(const char* p) { + // Fast return for single non-whitespace + if (*p == ' ' || *p == '\n' || *p == '\r' || *p == '\t') + ++p; + else + return p; + + // 16-byte align to the next boundary + const char* nextAligned = reinterpret_cast((reinterpret_cast(p) + 15) & static_cast(~15)); + while (p != nextAligned) + if (*p == ' ' || *p == '\n' || *p == '\r' || *p == '\t') + ++p; + else + return p; + + // The rest of string + #define C16(c) { c, c, c, c, c, c, c, c, c, c, c, c, c, c, c, c } + static const char whitespaces[4][16] = { C16(' '), C16('\n'), C16('\r'), C16('\t') }; + #undef C16 + + const __m128i w0 = _mm_loadu_si128(reinterpret_cast(&whitespaces[0][0])); + const __m128i w1 = _mm_loadu_si128(reinterpret_cast(&whitespaces[1][0])); + const __m128i w2 = _mm_loadu_si128(reinterpret_cast(&whitespaces[2][0])); + const __m128i w3 = _mm_loadu_si128(reinterpret_cast(&whitespaces[3][0])); + + for (;; p += 16) { + const __m128i s = _mm_load_si128(reinterpret_cast(p)); + __m128i x = _mm_cmpeq_epi8(s, w0); + x = _mm_or_si128(x, _mm_cmpeq_epi8(s, w1)); + x = _mm_or_si128(x, _mm_cmpeq_epi8(s, w2)); + x = _mm_or_si128(x, _mm_cmpeq_epi8(s, w3)); + unsigned short r = static_cast(~_mm_movemask_epi8(x)); + if (r != 0) { // some of characters may be non-whitespace +#ifdef _MSC_VER // Find the index of first non-whitespace + unsigned long offset; + _BitScanForward(&offset, r); + return p + offset; +#else + return p + __builtin_ffs(r) - 1; +#endif + } + } +} + +inline const char *SkipWhitespace_SIMD(const char* p, const char* end) { + // Fast return for single non-whitespace + if (p != end && (*p == ' ' || *p == '\n' || *p == '\r' || *p == '\t')) + ++p; + else + return p; + + // The rest of string + #define C16(c) { c, c, c, c, c, c, c, c, c, c, c, c, c, c, c, c } + static const char whitespaces[4][16] = { C16(' '), C16('\n'), C16('\r'), C16('\t') }; + #undef C16 + + const __m128i w0 = _mm_loadu_si128(reinterpret_cast(&whitespaces[0][0])); + const __m128i w1 = _mm_loadu_si128(reinterpret_cast(&whitespaces[1][0])); + const __m128i w2 = _mm_loadu_si128(reinterpret_cast(&whitespaces[2][0])); + const __m128i w3 = _mm_loadu_si128(reinterpret_cast(&whitespaces[3][0])); + + for (; p <= end - 16; p += 16) { + const __m128i s = _mm_loadu_si128(reinterpret_cast(p)); + __m128i x = _mm_cmpeq_epi8(s, w0); + x = _mm_or_si128(x, _mm_cmpeq_epi8(s, w1)); + x = _mm_or_si128(x, _mm_cmpeq_epi8(s, w2)); + x = _mm_or_si128(x, _mm_cmpeq_epi8(s, w3)); + unsigned short r = static_cast(~_mm_movemask_epi8(x)); + if (r != 0) { // some of characters may be non-whitespace +#ifdef _MSC_VER // Find the index of first non-whitespace + unsigned long offset; + _BitScanForward(&offset, r); + return p + offset; +#else + return p + __builtin_ffs(r) - 1; +#endif + } + } + + return SkipWhitespace(p, end); +} + +#elif defined(RAPIDJSON_NEON) + +//! Skip whitespace with ARM Neon instructions, testing 16 8-byte characters at once. +inline const char *SkipWhitespace_SIMD(const char* p) { + // Fast return for single non-whitespace + if (*p == ' ' || *p == '\n' || *p == '\r' || *p == '\t') + ++p; + else + return p; + + // 16-byte align to the next boundary + const char* nextAligned = reinterpret_cast((reinterpret_cast(p) + 15) & static_cast(~15)); + while (p != nextAligned) + if (*p == ' ' || *p == '\n' || *p == '\r' || *p == '\t') + ++p; + else + return p; + + const uint8x16_t w0 = vmovq_n_u8(' '); + const uint8x16_t w1 = vmovq_n_u8('\n'); + const uint8x16_t w2 = vmovq_n_u8('\r'); + const uint8x16_t w3 = vmovq_n_u8('\t'); + + for (;; p += 16) { + const uint8x16_t s = vld1q_u8(reinterpret_cast(p)); + uint8x16_t x = vceqq_u8(s, w0); + x = vorrq_u8(x, vceqq_u8(s, w1)); + x = vorrq_u8(x, vceqq_u8(s, w2)); + x = vorrq_u8(x, vceqq_u8(s, w3)); + + x = vmvnq_u8(x); // Negate + 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 + + if (low == 0) { + if (high != 0) { + uint32_t lz = RAPIDJSON_CLZLL(high); + return p + 8 + (lz >> 3); + } + } else { + uint32_t lz = RAPIDJSON_CLZLL(low); + return p + (lz >> 3); + } + } +} + +inline const char *SkipWhitespace_SIMD(const char* p, const char* end) { + // Fast return for single non-whitespace + if (p != end && (*p == ' ' || *p == '\n' || *p == '\r' || *p == '\t')) + ++p; + else + return p; + + const uint8x16_t w0 = vmovq_n_u8(' '); + const uint8x16_t w1 = vmovq_n_u8('\n'); + const uint8x16_t w2 = vmovq_n_u8('\r'); + const uint8x16_t w3 = vmovq_n_u8('\t'); + + for (; p <= end - 16; p += 16) { + const uint8x16_t s = vld1q_u8(reinterpret_cast(p)); + uint8x16_t x = vceqq_u8(s, w0); + x = vorrq_u8(x, vceqq_u8(s, w1)); + x = vorrq_u8(x, vceqq_u8(s, w2)); + x = vorrq_u8(x, vceqq_u8(s, w3)); + + x = vmvnq_u8(x); // Negate + 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 + + if (low == 0) { + if (high != 0) { + uint32_t lz = RAPIDJSON_CLZLL(high); + return p + 8 + (lz >> 3); + } + } else { + uint32_t lz = RAPIDJSON_CLZLL(low); + return p + (lz >> 3); + } + } + + return SkipWhitespace(p, end); +} + +#endif // RAPIDJSON_NEON + +#ifdef RAPIDJSON_SIMD +//! Template function specialization for InsituStringStream +template<> inline void SkipWhitespace(InsituStringStream& is) { + is.src_ = const_cast(SkipWhitespace_SIMD(is.src_)); +} + +//! Template function specialization for StringStream +template<> inline void SkipWhitespace(StringStream& is) { + is.src_ = SkipWhitespace_SIMD(is.src_); +} + +template<> inline void SkipWhitespace(EncodedInputStream, MemoryStream>& is) { + is.is_.src_ = SkipWhitespace_SIMD(is.is_.src_, is.is_.end_); +} +#endif // RAPIDJSON_SIMD + +/////////////////////////////////////////////////////////////////////////////// +// GenericReader + +//! SAX-style JSON parser. Use \ref Reader for UTF8 encoding and default allocator. +/*! GenericReader parses JSON text from a stream, and send events synchronously to an + object implementing Handler concept. + + It needs to allocate a stack for storing a single decoded string during + non-destructive parsing. + + For in-situ parsing, the decoded string is directly written to the source + text string, no temporary buffer is required. + + A GenericReader object can be reused for parsing multiple JSON text. + + \tparam SourceEncoding Encoding of the input stream. + \tparam TargetEncoding Encoding of the parse output. + \tparam StackAllocator Allocator type for stack. +*/ +template +class GenericReader { +public: + typedef typename SourceEncoding::Ch Ch; //!< SourceEncoding character type + + //! Constructor. + /*! \param stackAllocator Optional allocator for allocating stack memory. (Only use for non-destructive parsing) + \param stackCapacity stack capacity in bytes for storing a single decoded string. (Only use for non-destructive parsing) + */ + GenericReader(StackAllocator* stackAllocator = 0, size_t stackCapacity = kDefaultStackCapacity) : + stack_(stackAllocator, stackCapacity), parseResult_(), state_(IterativeParsingStartState) {} + + //! Parse JSON text. + /*! \tparam parseFlags Combination of \ref ParseFlag. + \tparam InputStream Type of input stream, implementing Stream concept. + \tparam Handler Type of handler, implementing Handler concept. + \param is Input stream to be parsed. + \param handler The handler to receive events. + \return Whether the parsing is successful. + */ + template + ParseResult Parse(InputStream& is, Handler& handler) { + if (parseFlags & kParseIterativeFlag) + return IterativeParse(is, handler); + + parseResult_.Clear(); + + ClearStackOnExit scope(*this); + + SkipWhitespaceAndComments(is); + RAPIDJSON_PARSE_ERROR_EARLY_RETURN(parseResult_); + + if (RAPIDJSON_UNLIKELY(is.Peek() == '\0')) { + RAPIDJSON_PARSE_ERROR_NORETURN(kParseErrorDocumentEmpty, is.Tell()); + RAPIDJSON_PARSE_ERROR_EARLY_RETURN(parseResult_); + } + else { + ParseValue(is, handler); + RAPIDJSON_PARSE_ERROR_EARLY_RETURN(parseResult_); + + if (!(parseFlags & kParseStopWhenDoneFlag)) { + SkipWhitespaceAndComments(is); + RAPIDJSON_PARSE_ERROR_EARLY_RETURN(parseResult_); + + if (RAPIDJSON_UNLIKELY(is.Peek() != '\0')) { + RAPIDJSON_PARSE_ERROR_NORETURN(kParseErrorDocumentRootNotSingular, is.Tell()); + RAPIDJSON_PARSE_ERROR_EARLY_RETURN(parseResult_); + } + } + } + + return parseResult_; + } + + //! Parse JSON text (with \ref kParseDefaultFlags) + /*! \tparam InputStream Type of input stream, implementing Stream concept + \tparam Handler Type of handler, implementing Handler concept. + \param is Input stream to be parsed. + \param handler The handler to receive events. + \return Whether the parsing is successful. + */ + template + ParseResult Parse(InputStream& is, Handler& handler) { + return Parse(is, handler); + } + + //! Initialize JSON text token-by-token parsing + /*! + */ + void IterativeParseInit() { + parseResult_.Clear(); + state_ = IterativeParsingStartState; + } + + //! Parse one token from JSON text + /*! \tparam InputStream Type of input stream, implementing Stream concept + \tparam Handler Type of handler, implementing Handler concept. + \param is Input stream to be parsed. + \param handler The handler to receive events. + \return Whether the parsing is successful. + */ + template + bool IterativeParseNext(InputStream& is, Handler& handler) { + while (RAPIDJSON_LIKELY(is.Peek() != '\0')) { + SkipWhitespaceAndComments(is); + + Token t = Tokenize(is.Peek()); + IterativeParsingState n = Predict(state_, t); + IterativeParsingState d = Transit(state_, t, n, is, handler); + + // If we've finished or hit an error... + if (RAPIDJSON_UNLIKELY(IsIterativeParsingCompleteState(d))) { + // Report errors. + if (d == IterativeParsingErrorState) { + HandleError(state_, is); + return false; + } + + // Transition to the finish state. + RAPIDJSON_ASSERT(d == IterativeParsingFinishState); + state_ = d; + + // If StopWhenDone is not set... + if (!(parseFlags & kParseStopWhenDoneFlag)) { + // ... and extra non-whitespace data is found... + SkipWhitespaceAndComments(is); + if (is.Peek() != '\0') { + // ... this is considered an error. + HandleError(state_, is); + return false; + } + } + + // Success! We are done! + return true; + } + + // Transition to the new state. + state_ = d; + + // If we parsed anything other than a delimiter, we invoked the handler, so we can return true now. + if (!IsIterativeParsingDelimiterState(n)) + return true; + } + + // We reached the end of file. + stack_.Clear(); + + if (state_ != IterativeParsingFinishState) { + HandleError(state_, is); + return false; + } + + return true; + } + + //! Check if token-by-token parsing JSON text is complete + /*! \return Whether the JSON has been fully decoded. + */ + RAPIDJSON_FORCEINLINE bool IterativeParseComplete() const { + return IsIterativeParsingCompleteState(state_); + } + + //! Whether a parse error has occurred in the last parsing. + bool HasParseError() const { return parseResult_.IsError(); } + + //! Get the \ref ParseErrorCode of last parsing. + ParseErrorCode GetParseErrorCode() const { return parseResult_.Code(); } + + //! Get the position of last parsing error in input, 0 otherwise. + size_t GetErrorOffset() const { return parseResult_.Offset(); } + +protected: + void SetParseError(ParseErrorCode code, size_t offset) { parseResult_.Set(code, offset); } + +private: + // Prohibit copy constructor & assignment operator. + GenericReader(const GenericReader&); + GenericReader& operator=(const GenericReader&); + + void ClearStack() { stack_.Clear(); } + + // clear stack on any exit from ParseStream, e.g. due to exception + struct ClearStackOnExit { + explicit ClearStackOnExit(GenericReader& r) : r_(r) {} + ~ClearStackOnExit() { r_.ClearStack(); } + private: + GenericReader& r_; + ClearStackOnExit(const ClearStackOnExit&); + ClearStackOnExit& operator=(const ClearStackOnExit&); + }; + + template + void SkipWhitespaceAndComments(InputStream& is) { + SkipWhitespace(is); + + if (parseFlags & kParseCommentsFlag) { + while (RAPIDJSON_UNLIKELY(Consume(is, '/'))) { + if (Consume(is, '*')) { + while (true) { + if (RAPIDJSON_UNLIKELY(is.Peek() == '\0')) + RAPIDJSON_PARSE_ERROR(kParseErrorUnspecificSyntaxError, is.Tell()); + else if (Consume(is, '*')) { + if (Consume(is, '/')) + break; + } + else + is.Take(); + } + } + else if (RAPIDJSON_LIKELY(Consume(is, '/'))) + while (is.Peek() != '\0' && is.Take() != '\n') {} + else + RAPIDJSON_PARSE_ERROR(kParseErrorUnspecificSyntaxError, is.Tell()); + + SkipWhitespace(is); + } + } + } + + // Parse object: { string : value, ... } + template + void ParseObject(InputStream& is, Handler& handler) { + RAPIDJSON_ASSERT(is.Peek() == '{'); + is.Take(); // Skip '{' + + if (RAPIDJSON_UNLIKELY(!handler.StartObject())) + RAPIDJSON_PARSE_ERROR(kParseErrorTermination, is.Tell()); + + SkipWhitespaceAndComments(is); + RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID; + + if (Consume(is, '}')) { + if (RAPIDJSON_UNLIKELY(!handler.EndObject(0))) // empty object + RAPIDJSON_PARSE_ERROR(kParseErrorTermination, is.Tell()); + return; + } + + for (SizeType memberCount = 0;;) { + if (RAPIDJSON_UNLIKELY(is.Peek() != '"')) + RAPIDJSON_PARSE_ERROR(kParseErrorObjectMissName, is.Tell()); + + ParseString(is, handler, true); + RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID; + + SkipWhitespaceAndComments(is); + RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID; + + if (RAPIDJSON_UNLIKELY(!Consume(is, ':'))) + RAPIDJSON_PARSE_ERROR(kParseErrorObjectMissColon, is.Tell()); + + SkipWhitespaceAndComments(is); + RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID; + + ParseValue(is, handler); + RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID; + + SkipWhitespaceAndComments(is); + RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID; + + ++memberCount; + + switch (is.Peek()) { + case ',': + is.Take(); + SkipWhitespaceAndComments(is); + RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID; + break; + case '}': + is.Take(); + if (RAPIDJSON_UNLIKELY(!handler.EndObject(memberCount))) + RAPIDJSON_PARSE_ERROR(kParseErrorTermination, is.Tell()); + return; + default: + RAPIDJSON_PARSE_ERROR(kParseErrorObjectMissCommaOrCurlyBracket, is.Tell()); break; // This useless break is only for making warning and coverage happy + } + + if (parseFlags & kParseTrailingCommasFlag) { + if (is.Peek() == '}') { + if (RAPIDJSON_UNLIKELY(!handler.EndObject(memberCount))) + RAPIDJSON_PARSE_ERROR(kParseErrorTermination, is.Tell()); + is.Take(); + return; + } + } + } + } + + // Parse array: [ value, ... ] + template + void ParseArray(InputStream& is, Handler& handler) { + RAPIDJSON_ASSERT(is.Peek() == '['); + is.Take(); // Skip '[' + + if (RAPIDJSON_UNLIKELY(!handler.StartArray())) + RAPIDJSON_PARSE_ERROR(kParseErrorTermination, is.Tell()); + + SkipWhitespaceAndComments(is); + RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID; + + if (Consume(is, ']')) { + if (RAPIDJSON_UNLIKELY(!handler.EndArray(0))) // empty array + RAPIDJSON_PARSE_ERROR(kParseErrorTermination, is.Tell()); + return; + } + + for (SizeType elementCount = 0;;) { + ParseValue(is, handler); + RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID; + + ++elementCount; + SkipWhitespaceAndComments(is); + RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID; + + if (Consume(is, ',')) { + SkipWhitespaceAndComments(is); + RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID; + } + else if (Consume(is, ']')) { + if (RAPIDJSON_UNLIKELY(!handler.EndArray(elementCount))) + RAPIDJSON_PARSE_ERROR(kParseErrorTermination, is.Tell()); + return; + } + else + RAPIDJSON_PARSE_ERROR(kParseErrorArrayMissCommaOrSquareBracket, is.Tell()); + + if (parseFlags & kParseTrailingCommasFlag) { + if (is.Peek() == ']') { + if (RAPIDJSON_UNLIKELY(!handler.EndArray(elementCount))) + RAPIDJSON_PARSE_ERROR(kParseErrorTermination, is.Tell()); + is.Take(); + return; + } + } + } + } + + template + void ParseNull(InputStream& is, Handler& handler) { + RAPIDJSON_ASSERT(is.Peek() == 'n'); + is.Take(); + + if (RAPIDJSON_LIKELY(Consume(is, 'u') && Consume(is, 'l') && Consume(is, 'l'))) { + if (RAPIDJSON_UNLIKELY(!handler.Null())) + RAPIDJSON_PARSE_ERROR(kParseErrorTermination, is.Tell()); + } + else + RAPIDJSON_PARSE_ERROR(kParseErrorValueInvalid, is.Tell()); + } + + template + void ParseTrue(InputStream& is, Handler& handler) { + RAPIDJSON_ASSERT(is.Peek() == 't'); + is.Take(); + + if (RAPIDJSON_LIKELY(Consume(is, 'r') && Consume(is, 'u') && Consume(is, 'e'))) { + if (RAPIDJSON_UNLIKELY(!handler.Bool(true))) + RAPIDJSON_PARSE_ERROR(kParseErrorTermination, is.Tell()); + } + else + RAPIDJSON_PARSE_ERROR(kParseErrorValueInvalid, is.Tell()); + } + + template + void ParseFalse(InputStream& is, Handler& handler) { + RAPIDJSON_ASSERT(is.Peek() == 'f'); + is.Take(); + + if (RAPIDJSON_LIKELY(Consume(is, 'a') && Consume(is, 'l') && Consume(is, 's') && Consume(is, 'e'))) { + if (RAPIDJSON_UNLIKELY(!handler.Bool(false))) + RAPIDJSON_PARSE_ERROR(kParseErrorTermination, is.Tell()); + } + else + RAPIDJSON_PARSE_ERROR(kParseErrorValueInvalid, is.Tell()); + } + + template + RAPIDJSON_FORCEINLINE static bool Consume(InputStream& is, typename InputStream::Ch expect) { + if (RAPIDJSON_LIKELY(is.Peek() == expect)) { + is.Take(); + return true; + } + else + return false; + } + + // Helper function to parse four hexadecimal digits in \uXXXX in ParseString(). + template + unsigned ParseHex4(InputStream& is, size_t escapeOffset) { + unsigned codepoint = 0; + for (int i = 0; i < 4; i++) { + Ch c = is.Peek(); + codepoint <<= 4; + codepoint += static_cast(c); + if (c >= '0' && c <= '9') + codepoint -= '0'; + else if (c >= 'A' && c <= 'F') + codepoint -= 'A' - 10; + else if (c >= 'a' && c <= 'f') + codepoint -= 'a' - 10; + else { + RAPIDJSON_PARSE_ERROR_NORETURN(kParseErrorStringUnicodeEscapeInvalidHex, escapeOffset); + RAPIDJSON_PARSE_ERROR_EARLY_RETURN(0); + } + is.Take(); + } + return codepoint; + } + + template + class StackStream { + public: + typedef CharType Ch; + + StackStream(internal::Stack& stack) : stack_(stack), length_(0) {} + RAPIDJSON_FORCEINLINE void Put(Ch c) { + *stack_.template Push() = c; + ++length_; + } + + RAPIDJSON_FORCEINLINE void* Push(SizeType count) { + length_ += count; + return stack_.template Push(count); + } + + size_t Length() const { return length_; } + + Ch* Pop() { + return stack_.template Pop(length_); + } + + private: + StackStream(const StackStream&); + StackStream& operator=(const StackStream&); + + internal::Stack& stack_; + SizeType length_; + }; + + // Parse string and generate String event. Different code paths for kParseInsituFlag. + template + void ParseString(InputStream& is, Handler& handler, bool isKey = false) { + internal::StreamLocalCopy copy(is); + InputStream& s(copy.s); + + RAPIDJSON_ASSERT(s.Peek() == '\"'); + s.Take(); // Skip '\"' + + bool success = false; + if (parseFlags & kParseInsituFlag) { + typename InputStream::Ch *head = s.PutBegin(); + ParseStringToStream(s, s); + RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID; + size_t length = s.PutEnd(head) - 1; + RAPIDJSON_ASSERT(length <= 0xFFFFFFFF); + const typename TargetEncoding::Ch* const str = reinterpret_cast(head); + success = (isKey ? handler.Key(str, SizeType(length), false) : handler.String(str, SizeType(length), false)); + } + else { + StackStream stackStream(stack_); + ParseStringToStream(s, stackStream); + RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID; + SizeType length = static_cast(stackStream.Length()) - 1; + const typename TargetEncoding::Ch* const str = stackStream.Pop(); + success = (isKey ? handler.Key(str, length, true) : handler.String(str, length, true)); + } + if (RAPIDJSON_UNLIKELY(!success)) + RAPIDJSON_PARSE_ERROR(kParseErrorTermination, s.Tell()); + } + + // Parse string to an output is + // This function handles the prefix/suffix double quotes, escaping, and optional encoding validation. + template + RAPIDJSON_FORCEINLINE void ParseStringToStream(InputStream& is, OutputStream& os) { +//!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN +#define Z16 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 + static const char escape[256] = { + Z16, Z16, 0, 0,'\"', 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,'/', + Z16, Z16, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,'\\', 0, 0, 0, + 0, 0,'\b', 0, 0, 0,'\f', 0, 0, 0, 0, 0, 0, 0,'\n', 0, + 0, 0,'\r', 0,'\t', 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + Z16, Z16, Z16, Z16, Z16, Z16, Z16, Z16 + }; +#undef Z16 +//!@endcond + + for (;;) { + // Scan and copy string before "\\\"" or < 0x20. This is an optional optimzation. + if (!(parseFlags & kParseValidateEncodingFlag)) + ScanCopyUnescapedString(is, os); + + Ch c = is.Peek(); + if (RAPIDJSON_UNLIKELY(c == '\\')) { // Escape + size_t escapeOffset = is.Tell(); // For invalid escaping, report the initial '\\' as error offset + is.Take(); + Ch e = is.Peek(); + if ((sizeof(Ch) == 1 || unsigned(e) < 256) && RAPIDJSON_LIKELY(escape[static_cast(e)])) { + is.Take(); + os.Put(static_cast(escape[static_cast(e)])); + } + else if (RAPIDJSON_LIKELY(e == 'u')) { // Unicode + is.Take(); + unsigned codepoint = ParseHex4(is, escapeOffset); + RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID; + if (RAPIDJSON_UNLIKELY(codepoint >= 0xD800 && codepoint <= 0xDBFF)) { + // Handle UTF-16 surrogate pair + if (RAPIDJSON_UNLIKELY(!Consume(is, '\\') || !Consume(is, 'u'))) + RAPIDJSON_PARSE_ERROR(kParseErrorStringUnicodeSurrogateInvalid, escapeOffset); + unsigned codepoint2 = ParseHex4(is, escapeOffset); + RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID; + if (RAPIDJSON_UNLIKELY(codepoint2 < 0xDC00 || codepoint2 > 0xDFFF)) + RAPIDJSON_PARSE_ERROR(kParseErrorStringUnicodeSurrogateInvalid, escapeOffset); + codepoint = (((codepoint - 0xD800) << 10) | (codepoint2 - 0xDC00)) + 0x10000; + } + TEncoding::Encode(os, codepoint); + } + else + RAPIDJSON_PARSE_ERROR(kParseErrorStringEscapeInvalid, escapeOffset); + } + else if (RAPIDJSON_UNLIKELY(c == '"')) { // Closing double quote + is.Take(); + os.Put('\0'); // null-terminate the string + return; + } + else if (RAPIDJSON_UNLIKELY(static_cast(c) < 0x20)) { // RFC 4627: unescaped = %x20-21 / %x23-5B / %x5D-10FFFF + if (c == '\0') + RAPIDJSON_PARSE_ERROR(kParseErrorStringMissQuotationMark, is.Tell()); + else + RAPIDJSON_PARSE_ERROR(kParseErrorStringInvalidEncoding, is.Tell()); + } + else { + size_t offset = is.Tell(); + if (RAPIDJSON_UNLIKELY((parseFlags & kParseValidateEncodingFlag ? + !Transcoder::Validate(is, os) : + !Transcoder::Transcode(is, os)))) + RAPIDJSON_PARSE_ERROR(kParseErrorStringInvalidEncoding, offset); + } + } + } + + template + static RAPIDJSON_FORCEINLINE void ScanCopyUnescapedString(InputStream&, OutputStream&) { + // Do nothing for generic version + } + +#if defined(RAPIDJSON_SSE2) || defined(RAPIDJSON_SSE42) + // StringStream -> StackStream + static RAPIDJSON_FORCEINLINE void ScanCopyUnescapedString(StringStream& is, StackStream& os) { + const char* p = is.src_; + + // Scan one by one until alignment (unaligned load may cross page boundary and cause crash) + const char* nextAligned = reinterpret_cast((reinterpret_cast(p) + 15) & static_cast(~15)); + while (p != nextAligned) + if (RAPIDJSON_UNLIKELY(*p == '\"') || RAPIDJSON_UNLIKELY(*p == '\\') || RAPIDJSON_UNLIKELY(static_cast(*p) < 0x20)) { + is.src_ = p; + return; + } + else + os.Put(*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(&dquote[0])); + const __m128i bs = _mm_loadu_si128(reinterpret_cast(&bslash[0])); + const __m128i sp = _mm_loadu_si128(reinterpret_cast(&space[0])); + + for (;; p += 16) { + const __m128i s = _mm_load_si128(reinterpret_cast(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(_mm_movemask_epi8(x)); + if (RAPIDJSON_UNLIKELY(r != 0)) { // some of characters is escaped + SizeType length; + #ifdef _MSC_VER // Find the index of first escaped + unsigned long offset; + _BitScanForward(&offset, r); + length = offset; + #else + length = static_cast(__builtin_ffs(r) - 1); + #endif + if (length != 0) { + char* q = reinterpret_cast(os.Push(length)); + for (size_t i = 0; i < length; i++) + q[i] = p[i]; + + p += length; + } + break; + } + _mm_storeu_si128(reinterpret_cast<__m128i *>(os.Push(16)), s); + } + + is.src_ = p; + } + + // InsituStringStream -> InsituStringStream + static RAPIDJSON_FORCEINLINE void ScanCopyUnescapedString(InsituStringStream& is, InsituStringStream& os) { + RAPIDJSON_ASSERT(&is == &os); + (void)os; + + if (is.src_ == is.dst_) { + SkipUnescapedString(is); + return; + } + + char* p = is.src_; + char *q = is.dst_; + + // Scan one by one until alignment (unaligned load may cross page boundary and cause crash) + const char* nextAligned = reinterpret_cast((reinterpret_cast(p) + 15) & static_cast(~15)); + while (p != nextAligned) + if (RAPIDJSON_UNLIKELY(*p == '\"') || RAPIDJSON_UNLIKELY(*p == '\\') || RAPIDJSON_UNLIKELY(static_cast(*p) < 0x20)) { + is.src_ = p; + is.dst_ = q; + return; + } + else + *q++ = *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(&dquote[0])); + const __m128i bs = _mm_loadu_si128(reinterpret_cast(&bslash[0])); + const __m128i sp = _mm_loadu_si128(reinterpret_cast(&space[0])); + + for (;; p += 16, q += 16) { + const __m128i s = _mm_load_si128(reinterpret_cast(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(_mm_movemask_epi8(x)); + if (RAPIDJSON_UNLIKELY(r != 0)) { // some of characters is escaped + size_t length; +#ifdef _MSC_VER // Find the index of first escaped + unsigned long offset; + _BitScanForward(&offset, r); + length = offset; +#else + length = static_cast(__builtin_ffs(r) - 1); +#endif + for (const char* pend = p + length; p != pend; ) + *q++ = *p++; + break; + } + _mm_storeu_si128(reinterpret_cast<__m128i *>(q), s); + } + + is.src_ = p; + is.dst_ = q; + } + + // When read/write pointers are the same for insitu stream, just skip unescaped characters + static RAPIDJSON_FORCEINLINE void SkipUnescapedString(InsituStringStream& is) { + RAPIDJSON_ASSERT(is.src_ == is.dst_); + char* p = is.src_; + + // Scan one by one until alignment (unaligned load may cross page boundary and cause crash) + const char* nextAligned = reinterpret_cast((reinterpret_cast(p) + 15) & static_cast(~15)); + for (; p != nextAligned; p++) + if (RAPIDJSON_UNLIKELY(*p == '\"') || RAPIDJSON_UNLIKELY(*p == '\\') || RAPIDJSON_UNLIKELY(static_cast(*p) < 0x20)) { + is.src_ = is.dst_ = p; + return; + } + + // 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(&dquote[0])); + const __m128i bs = _mm_loadu_si128(reinterpret_cast(&bslash[0])); + const __m128i sp = _mm_loadu_si128(reinterpret_cast(&space[0])); + + for (;; p += 16) { + const __m128i s = _mm_load_si128(reinterpret_cast(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(_mm_movemask_epi8(x)); + if (RAPIDJSON_UNLIKELY(r != 0)) { // some of characters is escaped + size_t length; +#ifdef _MSC_VER // Find the index of first escaped + unsigned long offset; + _BitScanForward(&offset, r); + length = offset; +#else + length = static_cast(__builtin_ffs(r) - 1); +#endif + p += length; + break; + } + } + + is.src_ = is.dst_ = p; + } +#elif defined(RAPIDJSON_NEON) + // StringStream -> StackStream + static RAPIDJSON_FORCEINLINE void ScanCopyUnescapedString(StringStream& is, StackStream& os) { + const char* p = is.src_; + + // Scan one by one until alignment (unaligned load may cross page boundary and cause crash) + const char* nextAligned = reinterpret_cast((reinterpret_cast(p) + 15) & static_cast(~15)); + while (p != nextAligned) + if (RAPIDJSON_UNLIKELY(*p == '\"') || RAPIDJSON_UNLIKELY(*p == '\\') || RAPIDJSON_UNLIKELY(static_cast(*p) < 0x20)) { + is.src_ = p; + return; + } + else + os.Put(*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 += 16) { + const uint8x16_t s = vld1q_u8(reinterpret_cast(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 length = 0; + bool escaped = false; + if (low == 0) { + if (high != 0) { + uint32_t lz = RAPIDJSON_CLZLL(high); + length = 8 + (lz >> 3); + escaped = true; + } + } else { + uint32_t lz = RAPIDJSON_CLZLL(low); + length = lz >> 3; + escaped = true; + } + if (RAPIDJSON_UNLIKELY(escaped)) { // some of characters is escaped + if (length != 0) { + char* q = reinterpret_cast(os.Push(length)); + for (size_t i = 0; i < length; i++) + q[i] = p[i]; + + p += length; + } + break; + } + vst1q_u8(reinterpret_cast(os.Push(16)), s); + } + + is.src_ = p; + } + + // InsituStringStream -> InsituStringStream + static RAPIDJSON_FORCEINLINE void ScanCopyUnescapedString(InsituStringStream& is, InsituStringStream& os) { + RAPIDJSON_ASSERT(&is == &os); + (void)os; + + if (is.src_ == is.dst_) { + SkipUnescapedString(is); + return; + } + + char* p = is.src_; + char *q = is.dst_; + + // Scan one by one until alignment (unaligned load may cross page boundary and cause crash) + const char* nextAligned = reinterpret_cast((reinterpret_cast(p) + 15) & static_cast(~15)); + while (p != nextAligned) + if (RAPIDJSON_UNLIKELY(*p == '\"') || RAPIDJSON_UNLIKELY(*p == '\\') || RAPIDJSON_UNLIKELY(static_cast(*p) < 0x20)) { + is.src_ = p; + is.dst_ = q; + return; + } + else + *q++ = *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 += 16, q += 16) { + const uint8x16_t s = vld1q_u8(reinterpret_cast(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 length = 0; + bool escaped = false; + if (low == 0) { + if (high != 0) { + uint32_t lz = RAPIDJSON_CLZLL(high); + length = 8 + (lz >> 3); + escaped = true; + } + } else { + uint32_t lz = RAPIDJSON_CLZLL(low); + length = lz >> 3; + escaped = true; + } + if (RAPIDJSON_UNLIKELY(escaped)) { // some of characters is escaped + for (const char* pend = p + length; p != pend; ) { + *q++ = *p++; + } + break; + } + vst1q_u8(reinterpret_cast(q), s); + } + + is.src_ = p; + is.dst_ = q; + } + + // When read/write pointers are the same for insitu stream, just skip unescaped characters + static RAPIDJSON_FORCEINLINE void SkipUnescapedString(InsituStringStream& is) { + RAPIDJSON_ASSERT(is.src_ == is.dst_); + char* p = is.src_; + + // Scan one by one until alignment (unaligned load may cross page boundary and cause crash) + const char* nextAligned = reinterpret_cast((reinterpret_cast(p) + 15) & static_cast(~15)); + for (; p != nextAligned; p++) + if (RAPIDJSON_UNLIKELY(*p == '\"') || RAPIDJSON_UNLIKELY(*p == '\\') || RAPIDJSON_UNLIKELY(static_cast(*p) < 0x20)) { + is.src_ = is.dst_ = p; + return; + } + + // 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 += 16) { + const uint8x16_t s = vld1q_u8(reinterpret_cast(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 + + if (low == 0) { + if (high != 0) { + uint32_t lz = RAPIDJSON_CLZLL(high); + p += 8 + (lz >> 3); + break; + } + } else { + uint32_t lz = RAPIDJSON_CLZLL(low); + p += lz >> 3; + break; + } + } + + is.src_ = is.dst_ = p; + } +#endif // RAPIDJSON_NEON + + template + class NumberStream; + + template + class NumberStream { + public: + typedef typename InputStream::Ch Ch; + + NumberStream(GenericReader& reader, InputStream& s) : is(s) { (void)reader; } + + RAPIDJSON_FORCEINLINE Ch Peek() const { return is.Peek(); } + RAPIDJSON_FORCEINLINE Ch TakePush() { return is.Take(); } + RAPIDJSON_FORCEINLINE Ch Take() { return is.Take(); } + RAPIDJSON_FORCEINLINE void Push(char) {} + + size_t Tell() { return is.Tell(); } + size_t Length() { return 0; } + const char* Pop() { return 0; } + + protected: + NumberStream& operator=(const NumberStream&); + + InputStream& is; + }; + + template + class NumberStream : public NumberStream { + typedef NumberStream Base; + public: + NumberStream(GenericReader& reader, InputStream& is) : Base(reader, is), stackStream(reader.stack_) {} + + RAPIDJSON_FORCEINLINE Ch TakePush() { + stackStream.Put(static_cast(Base::is.Peek())); + return Base::is.Take(); + } + + RAPIDJSON_FORCEINLINE void Push(char c) { + stackStream.Put(c); + } + + size_t Length() { return stackStream.Length(); } + + const char* Pop() { + stackStream.Put('\0'); + return stackStream.Pop(); + } + + private: + StackStream stackStream; + }; + + template + class NumberStream : public NumberStream { + typedef NumberStream Base; + public: + NumberStream(GenericReader& reader, InputStream& is) : Base(reader, is) {} + + RAPIDJSON_FORCEINLINE Ch Take() { return Base::TakePush(); } + }; + + template + void ParseNumber(InputStream& is, Handler& handler) { + internal::StreamLocalCopy copy(is); + NumberStream s(*this, copy.s); + + size_t startOffset = s.Tell(); + double d = 0.0; + bool useNanOrInf = false; + + // Parse minus + bool minus = Consume(s, '-'); + + // Parse int: zero / ( digit1-9 *DIGIT ) + unsigned i = 0; + uint64_t i64 = 0; + bool use64bit = false; + int significandDigit = 0; + if (RAPIDJSON_UNLIKELY(s.Peek() == '0')) { + i = 0; + s.TakePush(); + } + else if (RAPIDJSON_LIKELY(s.Peek() >= '1' && s.Peek() <= '9')) { + i = static_cast(s.TakePush() - '0'); + + if (minus) + while (RAPIDJSON_LIKELY(s.Peek() >= '0' && s.Peek() <= '9')) { + if (RAPIDJSON_UNLIKELY(i >= 214748364)) { // 2^31 = 2147483648 + if (RAPIDJSON_LIKELY(i != 214748364 || s.Peek() > '8')) { + i64 = i; + use64bit = true; + break; + } + } + i = i * 10 + static_cast(s.TakePush() - '0'); + significandDigit++; + } + else + while (RAPIDJSON_LIKELY(s.Peek() >= '0' && s.Peek() <= '9')) { + if (RAPIDJSON_UNLIKELY(i >= 429496729)) { // 2^32 - 1 = 4294967295 + if (RAPIDJSON_LIKELY(i != 429496729 || s.Peek() > '5')) { + i64 = i; + use64bit = true; + break; + } + } + i = i * 10 + static_cast(s.TakePush() - '0'); + significandDigit++; + } + } + // Parse NaN or Infinity here + else if ((parseFlags & kParseNanAndInfFlag) && RAPIDJSON_LIKELY((s.Peek() == 'I' || s.Peek() == 'N'))) { + if (Consume(s, 'N')) { + if (Consume(s, 'a') && Consume(s, 'N')) { + d = std::numeric_limits::quiet_NaN(); + useNanOrInf = true; + } + } + else if (RAPIDJSON_LIKELY(Consume(s, 'I'))) { + if (Consume(s, 'n') && Consume(s, 'f')) { + d = (minus ? -std::numeric_limits::infinity() : std::numeric_limits::infinity()); + useNanOrInf = true; + + if (RAPIDJSON_UNLIKELY(s.Peek() == 'i' && !(Consume(s, 'i') && Consume(s, 'n') + && Consume(s, 'i') && Consume(s, 't') && Consume(s, 'y')))) { + RAPIDJSON_PARSE_ERROR(kParseErrorValueInvalid, s.Tell()); + } + } + } + + if (RAPIDJSON_UNLIKELY(!useNanOrInf)) { + RAPIDJSON_PARSE_ERROR(kParseErrorValueInvalid, s.Tell()); + } + } + else + RAPIDJSON_PARSE_ERROR(kParseErrorValueInvalid, s.Tell()); + + // Parse 64bit int + bool useDouble = false; + if (use64bit) { + if (minus) + while (RAPIDJSON_LIKELY(s.Peek() >= '0' && s.Peek() <= '9')) { + if (RAPIDJSON_UNLIKELY(i64 >= RAPIDJSON_UINT64_C2(0x0CCCCCCC, 0xCCCCCCCC))) // 2^63 = 9223372036854775808 + if (RAPIDJSON_LIKELY(i64 != RAPIDJSON_UINT64_C2(0x0CCCCCCC, 0xCCCCCCCC) || s.Peek() > '8')) { + d = static_cast(i64); + useDouble = true; + break; + } + i64 = i64 * 10 + static_cast(s.TakePush() - '0'); + significandDigit++; + } + else + while (RAPIDJSON_LIKELY(s.Peek() >= '0' && s.Peek() <= '9')) { + if (RAPIDJSON_UNLIKELY(i64 >= RAPIDJSON_UINT64_C2(0x19999999, 0x99999999))) // 2^64 - 1 = 18446744073709551615 + if (RAPIDJSON_LIKELY(i64 != RAPIDJSON_UINT64_C2(0x19999999, 0x99999999) || s.Peek() > '5')) { + d = static_cast(i64); + useDouble = true; + break; + } + i64 = i64 * 10 + static_cast(s.TakePush() - '0'); + significandDigit++; + } + } + + // Force double for big integer + if (useDouble) { + while (RAPIDJSON_LIKELY(s.Peek() >= '0' && s.Peek() <= '9')) { + d = d * 10 + (s.TakePush() - '0'); + } + } + + // Parse frac = decimal-point 1*DIGIT + int expFrac = 0; + size_t decimalPosition; + if (Consume(s, '.')) { + decimalPosition = s.Length(); + + if (RAPIDJSON_UNLIKELY(!(s.Peek() >= '0' && s.Peek() <= '9'))) + RAPIDJSON_PARSE_ERROR(kParseErrorNumberMissFraction, s.Tell()); + + if (!useDouble) { +#if RAPIDJSON_64BIT + // Use i64 to store significand in 64-bit architecture + if (!use64bit) + i64 = i; + + while (RAPIDJSON_LIKELY(s.Peek() >= '0' && s.Peek() <= '9')) { + if (i64 > RAPIDJSON_UINT64_C2(0x1FFFFF, 0xFFFFFFFF)) // 2^53 - 1 for fast path + break; + else { + i64 = i64 * 10 + static_cast(s.TakePush() - '0'); + --expFrac; + if (i64 != 0) + significandDigit++; + } + } + + d = static_cast(i64); +#else + // Use double to store significand in 32-bit architecture + d = static_cast(use64bit ? i64 : i); +#endif + useDouble = true; + } + + while (RAPIDJSON_LIKELY(s.Peek() >= '0' && s.Peek() <= '9')) { + if (significandDigit < 17) { + d = d * 10.0 + (s.TakePush() - '0'); + --expFrac; + if (RAPIDJSON_LIKELY(d > 0.0)) + significandDigit++; + } + else + s.TakePush(); + } + } + else + decimalPosition = s.Length(); // decimal position at the end of integer. + + // Parse exp = e [ minus / plus ] 1*DIGIT + int exp = 0; + if (Consume(s, 'e') || Consume(s, 'E')) { + if (!useDouble) { + d = static_cast(use64bit ? i64 : i); + useDouble = true; + } + + bool expMinus = false; + if (Consume(s, '+')) + ; + else if (Consume(s, '-')) + expMinus = true; + + if (RAPIDJSON_LIKELY(s.Peek() >= '0' && s.Peek() <= '9')) { + exp = static_cast(s.Take() - '0'); + if (expMinus) { + // (exp + expFrac) must not underflow int => we're detecting when -exp gets + // dangerously close to INT_MIN (a pessimistic next digit 9 would push it into + // underflow territory): + // + // -(exp * 10 + 9) + expFrac >= INT_MIN + // <=> exp <= (expFrac - INT_MIN - 9) / 10 + RAPIDJSON_ASSERT(expFrac <= 0); + int maxExp = (expFrac + 2147483639) / 10; + + while (RAPIDJSON_LIKELY(s.Peek() >= '0' && s.Peek() <= '9')) { + exp = exp * 10 + static_cast(s.Take() - '0'); + if (RAPIDJSON_UNLIKELY(exp > maxExp)) { + while (RAPIDJSON_UNLIKELY(s.Peek() >= '0' && s.Peek() <= '9')) // Consume the rest of exponent + s.Take(); + } + } + } + else { // positive exp + int maxExp = 308 - expFrac; + while (RAPIDJSON_LIKELY(s.Peek() >= '0' && s.Peek() <= '9')) { + exp = exp * 10 + static_cast(s.Take() - '0'); + if (RAPIDJSON_UNLIKELY(exp > maxExp)) + RAPIDJSON_PARSE_ERROR(kParseErrorNumberTooBig, startOffset); + } + } + } + else + RAPIDJSON_PARSE_ERROR(kParseErrorNumberMissExponent, s.Tell()); + + if (expMinus) + exp = -exp; + } + + // Finish parsing, call event according to the type of number. + bool cont = true; + + if (parseFlags & kParseNumbersAsStringsFlag) { + if (parseFlags & kParseInsituFlag) { + s.Pop(); // Pop stack no matter if it will be used or not. + typename InputStream::Ch* head = is.PutBegin(); + const size_t length = s.Tell() - startOffset; + RAPIDJSON_ASSERT(length <= 0xFFFFFFFF); + // unable to insert the \0 character here, it will erase the comma after this number + const typename TargetEncoding::Ch* const str = reinterpret_cast(head); + cont = handler.RawNumber(str, SizeType(length), false); + } + else { + SizeType numCharsToCopy = static_cast(s.Length()); + StringStream srcStream(s.Pop()); + StackStream dstStream(stack_); + while (numCharsToCopy--) { + Transcoder, TargetEncoding>::Transcode(srcStream, dstStream); + } + dstStream.Put('\0'); + const typename TargetEncoding::Ch* str = dstStream.Pop(); + const SizeType length = static_cast(dstStream.Length()) - 1; + cont = handler.RawNumber(str, SizeType(length), true); + } + } + else { + size_t length = s.Length(); + const char* decimal = s.Pop(); // Pop stack no matter if it will be used or not. + + if (useDouble) { + int p = exp + expFrac; + if (parseFlags & kParseFullPrecisionFlag) + d = internal::StrtodFullPrecision(d, p, decimal, length, decimalPosition, exp); + else + d = internal::StrtodNormalPrecision(d, p); + + // Use > max, instead of == inf, to fix bogus warning -Wfloat-equal + if (d > (std::numeric_limits::max)()) { + // Overflow + // TODO: internal::StrtodX should report overflow (or underflow) + RAPIDJSON_PARSE_ERROR(kParseErrorNumberTooBig, startOffset); + } + + cont = handler.Double(minus ? -d : d); + } + else if (useNanOrInf) { + cont = handler.Double(d); + } + else { + if (use64bit) { + if (minus) + cont = handler.Int64(static_cast(~i64 + 1)); + else + cont = handler.Uint64(i64); + } + else { + if (minus) + cont = handler.Int(static_cast(~i + 1)); + else + cont = handler.Uint(i); + } + } + } + if (RAPIDJSON_UNLIKELY(!cont)) + RAPIDJSON_PARSE_ERROR(kParseErrorTermination, startOffset); + } + + // Parse any JSON value + template + void ParseValue(InputStream& is, Handler& handler) { + switch (is.Peek()) { + case 'n': ParseNull (is, handler); break; + case 't': ParseTrue (is, handler); break; + case 'f': ParseFalse (is, handler); break; + case '"': ParseString(is, handler); break; + case '{': ParseObject(is, handler); break; + case '[': ParseArray (is, handler); break; + default : + ParseNumber(is, handler); + break; + + } + } + + // Iterative Parsing + + // States + enum IterativeParsingState { + IterativeParsingFinishState = 0, // sink states at top + IterativeParsingErrorState, // sink states at top + IterativeParsingStartState, + + // Object states + IterativeParsingObjectInitialState, + IterativeParsingMemberKeyState, + IterativeParsingMemberValueState, + IterativeParsingObjectFinishState, + + // Array states + IterativeParsingArrayInitialState, + IterativeParsingElementState, + IterativeParsingArrayFinishState, + + // Single value state + IterativeParsingValueState, + + // Delimiter states (at bottom) + IterativeParsingElementDelimiterState, + IterativeParsingMemberDelimiterState, + IterativeParsingKeyValueDelimiterState, + + cIterativeParsingStateCount + }; + + // Tokens + enum Token { + LeftBracketToken = 0, + RightBracketToken, + + LeftCurlyBracketToken, + RightCurlyBracketToken, + + CommaToken, + ColonToken, + + StringToken, + FalseToken, + TrueToken, + NullToken, + NumberToken, + + kTokenCount + }; + + RAPIDJSON_FORCEINLINE Token Tokenize(Ch c) const { + +//!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN +#define N NumberToken +#define N16 N,N,N,N,N,N,N,N,N,N,N,N,N,N,N,N + // Maps from ASCII to Token + static const unsigned char tokenMap[256] = { + N16, // 00~0F + N16, // 10~1F + N, N, StringToken, N, N, N, N, N, N, N, N, N, CommaToken, N, N, N, // 20~2F + N, N, N, N, N, N, N, N, N, N, ColonToken, N, N, N, N, N, // 30~3F + N16, // 40~4F + N, N, N, N, N, N, N, N, N, N, N, LeftBracketToken, N, RightBracketToken, N, N, // 50~5F + N, N, N, N, N, N, FalseToken, N, N, N, N, N, N, N, NullToken, N, // 60~6F + N, N, N, N, TrueToken, N, N, N, N, N, N, LeftCurlyBracketToken, N, RightCurlyBracketToken, N, N, // 70~7F + N16, N16, N16, N16, N16, N16, N16, N16 // 80~FF + }; +#undef N +#undef N16 +//!@endcond + + if (sizeof(Ch) == 1 || static_cast(c) < 256) + return static_cast(tokenMap[static_cast(c)]); + else + return NumberToken; + } + + RAPIDJSON_FORCEINLINE IterativeParsingState Predict(IterativeParsingState state, Token token) const { + // current state x one lookahead token -> new state + static const char G[cIterativeParsingStateCount][kTokenCount] = { + // Finish(sink state) + { + IterativeParsingErrorState, IterativeParsingErrorState, IterativeParsingErrorState, IterativeParsingErrorState, IterativeParsingErrorState, + IterativeParsingErrorState, IterativeParsingErrorState, IterativeParsingErrorState, IterativeParsingErrorState, IterativeParsingErrorState, + IterativeParsingErrorState + }, + // Error(sink state) + { + IterativeParsingErrorState, IterativeParsingErrorState, IterativeParsingErrorState, IterativeParsingErrorState, IterativeParsingErrorState, + IterativeParsingErrorState, IterativeParsingErrorState, IterativeParsingErrorState, IterativeParsingErrorState, IterativeParsingErrorState, + IterativeParsingErrorState + }, + // Start + { + IterativeParsingArrayInitialState, // Left bracket + IterativeParsingErrorState, // Right bracket + IterativeParsingObjectInitialState, // Left curly bracket + IterativeParsingErrorState, // Right curly bracket + IterativeParsingErrorState, // Comma + IterativeParsingErrorState, // Colon + IterativeParsingValueState, // String + IterativeParsingValueState, // False + IterativeParsingValueState, // True + IterativeParsingValueState, // Null + IterativeParsingValueState // Number + }, + // ObjectInitial + { + IterativeParsingErrorState, // Left bracket + IterativeParsingErrorState, // Right bracket + IterativeParsingErrorState, // Left curly bracket + IterativeParsingObjectFinishState, // Right curly bracket + IterativeParsingErrorState, // Comma + IterativeParsingErrorState, // Colon + IterativeParsingMemberKeyState, // String + IterativeParsingErrorState, // False + IterativeParsingErrorState, // True + IterativeParsingErrorState, // Null + IterativeParsingErrorState // Number + }, + // MemberKey + { + IterativeParsingErrorState, // Left bracket + IterativeParsingErrorState, // Right bracket + IterativeParsingErrorState, // Left curly bracket + IterativeParsingErrorState, // Right curly bracket + IterativeParsingErrorState, // Comma + IterativeParsingKeyValueDelimiterState, // Colon + IterativeParsingErrorState, // String + IterativeParsingErrorState, // False + IterativeParsingErrorState, // True + IterativeParsingErrorState, // Null + IterativeParsingErrorState // Number + }, + // MemberValue + { + IterativeParsingErrorState, // Left bracket + IterativeParsingErrorState, // Right bracket + IterativeParsingErrorState, // Left curly bracket + IterativeParsingObjectFinishState, // Right curly bracket + IterativeParsingMemberDelimiterState, // Comma + IterativeParsingErrorState, // Colon + IterativeParsingErrorState, // String + IterativeParsingErrorState, // False + IterativeParsingErrorState, // True + IterativeParsingErrorState, // Null + IterativeParsingErrorState // Number + }, + // ObjectFinish(sink state) + { + IterativeParsingErrorState, IterativeParsingErrorState, IterativeParsingErrorState, IterativeParsingErrorState, IterativeParsingErrorState, + IterativeParsingErrorState, IterativeParsingErrorState, IterativeParsingErrorState, IterativeParsingErrorState, IterativeParsingErrorState, + IterativeParsingErrorState + }, + // ArrayInitial + { + IterativeParsingArrayInitialState, // Left bracket(push Element state) + IterativeParsingArrayFinishState, // Right bracket + IterativeParsingObjectInitialState, // Left curly bracket(push Element state) + IterativeParsingErrorState, // Right curly bracket + IterativeParsingErrorState, // Comma + IterativeParsingErrorState, // Colon + IterativeParsingElementState, // String + IterativeParsingElementState, // False + IterativeParsingElementState, // True + IterativeParsingElementState, // Null + IterativeParsingElementState // Number + }, + // Element + { + IterativeParsingErrorState, // Left bracket + IterativeParsingArrayFinishState, // Right bracket + IterativeParsingErrorState, // Left curly bracket + IterativeParsingErrorState, // Right curly bracket + IterativeParsingElementDelimiterState, // Comma + IterativeParsingErrorState, // Colon + IterativeParsingErrorState, // String + IterativeParsingErrorState, // False + IterativeParsingErrorState, // True + IterativeParsingErrorState, // Null + IterativeParsingErrorState // Number + }, + // ArrayFinish(sink state) + { + IterativeParsingErrorState, IterativeParsingErrorState, IterativeParsingErrorState, IterativeParsingErrorState, IterativeParsingErrorState, + IterativeParsingErrorState, IterativeParsingErrorState, IterativeParsingErrorState, IterativeParsingErrorState, IterativeParsingErrorState, + IterativeParsingErrorState + }, + // Single Value (sink state) + { + IterativeParsingErrorState, IterativeParsingErrorState, IterativeParsingErrorState, IterativeParsingErrorState, IterativeParsingErrorState, + IterativeParsingErrorState, IterativeParsingErrorState, IterativeParsingErrorState, IterativeParsingErrorState, IterativeParsingErrorState, + IterativeParsingErrorState + }, + // ElementDelimiter + { + IterativeParsingArrayInitialState, // Left bracket(push Element state) + IterativeParsingArrayFinishState, // Right bracket + IterativeParsingObjectInitialState, // Left curly bracket(push Element state) + IterativeParsingErrorState, // Right curly bracket + IterativeParsingErrorState, // Comma + IterativeParsingErrorState, // Colon + IterativeParsingElementState, // String + IterativeParsingElementState, // False + IterativeParsingElementState, // True + IterativeParsingElementState, // Null + IterativeParsingElementState // Number + }, + // MemberDelimiter + { + IterativeParsingErrorState, // Left bracket + IterativeParsingErrorState, // Right bracket + IterativeParsingErrorState, // Left curly bracket + IterativeParsingObjectFinishState, // Right curly bracket + IterativeParsingErrorState, // Comma + IterativeParsingErrorState, // Colon + IterativeParsingMemberKeyState, // String + IterativeParsingErrorState, // False + IterativeParsingErrorState, // True + IterativeParsingErrorState, // Null + IterativeParsingErrorState // Number + }, + // KeyValueDelimiter + { + IterativeParsingArrayInitialState, // Left bracket(push MemberValue state) + IterativeParsingErrorState, // Right bracket + IterativeParsingObjectInitialState, // Left curly bracket(push MemberValue state) + IterativeParsingErrorState, // Right curly bracket + IterativeParsingErrorState, // Comma + IterativeParsingErrorState, // Colon + IterativeParsingMemberValueState, // String + IterativeParsingMemberValueState, // False + IterativeParsingMemberValueState, // True + IterativeParsingMemberValueState, // Null + IterativeParsingMemberValueState // Number + }, + }; // End of G + + return static_cast(G[state][token]); + } + + // Make an advance in the token stream and state based on the candidate destination state which was returned by Transit(). + // May return a new state on state pop. + template + RAPIDJSON_FORCEINLINE IterativeParsingState Transit(IterativeParsingState src, Token token, IterativeParsingState dst, InputStream& is, Handler& handler) { + (void)token; + + switch (dst) { + case IterativeParsingErrorState: + return dst; + + case IterativeParsingObjectInitialState: + case IterativeParsingArrayInitialState: + { + // Push the state(Element or MemeberValue) if we are nested in another array or value of member. + // In this way we can get the correct state on ObjectFinish or ArrayFinish by frame pop. + IterativeParsingState n = src; + if (src == IterativeParsingArrayInitialState || src == IterativeParsingElementDelimiterState) + n = IterativeParsingElementState; + else if (src == IterativeParsingKeyValueDelimiterState) + n = IterativeParsingMemberValueState; + // Push current state. + *stack_.template Push(1) = n; + // Initialize and push the member/element count. + *stack_.template Push(1) = 0; + // Call handler + bool hr = (dst == IterativeParsingObjectInitialState) ? handler.StartObject() : handler.StartArray(); + // On handler short circuits the parsing. + if (!hr) { + RAPIDJSON_PARSE_ERROR_NORETURN(kParseErrorTermination, is.Tell()); + return IterativeParsingErrorState; + } + else { + is.Take(); + return dst; + } + } + + case IterativeParsingMemberKeyState: + ParseString(is, handler, true); + if (HasParseError()) + return IterativeParsingErrorState; + else + return dst; + + case IterativeParsingKeyValueDelimiterState: + RAPIDJSON_ASSERT(token == ColonToken); + is.Take(); + return dst; + + case IterativeParsingMemberValueState: + // Must be non-compound value. Or it would be ObjectInitial or ArrayInitial state. + ParseValue(is, handler); + if (HasParseError()) { + return IterativeParsingErrorState; + } + return dst; + + case IterativeParsingElementState: + // Must be non-compound value. Or it would be ObjectInitial or ArrayInitial state. + ParseValue(is, handler); + if (HasParseError()) { + return IterativeParsingErrorState; + } + return dst; + + case IterativeParsingMemberDelimiterState: + case IterativeParsingElementDelimiterState: + is.Take(); + // Update member/element count. + *stack_.template Top() = *stack_.template Top() + 1; + return dst; + + case IterativeParsingObjectFinishState: + { + // Transit from delimiter is only allowed when trailing commas are enabled + if (!(parseFlags & kParseTrailingCommasFlag) && src == IterativeParsingMemberDelimiterState) { + RAPIDJSON_PARSE_ERROR_NORETURN(kParseErrorObjectMissName, is.Tell()); + return IterativeParsingErrorState; + } + // Get member count. + SizeType c = *stack_.template Pop(1); + // If the object is not empty, count the last member. + if (src == IterativeParsingMemberValueState) + ++c; + // Restore the state. + IterativeParsingState n = static_cast(*stack_.template Pop(1)); + // Transit to Finish state if this is the topmost scope. + if (n == IterativeParsingStartState) + n = IterativeParsingFinishState; + // Call handler + bool hr = handler.EndObject(c); + // On handler short circuits the parsing. + if (!hr) { + RAPIDJSON_PARSE_ERROR_NORETURN(kParseErrorTermination, is.Tell()); + return IterativeParsingErrorState; + } + else { + is.Take(); + return n; + } + } + + case IterativeParsingArrayFinishState: + { + // Transit from delimiter is only allowed when trailing commas are enabled + if (!(parseFlags & kParseTrailingCommasFlag) && src == IterativeParsingElementDelimiterState) { + RAPIDJSON_PARSE_ERROR_NORETURN(kParseErrorValueInvalid, is.Tell()); + return IterativeParsingErrorState; + } + // Get element count. + SizeType c = *stack_.template Pop(1); + // If the array is not empty, count the last element. + if (src == IterativeParsingElementState) + ++c; + // Restore the state. + IterativeParsingState n = static_cast(*stack_.template Pop(1)); + // Transit to Finish state if this is the topmost scope. + if (n == IterativeParsingStartState) + n = IterativeParsingFinishState; + // Call handler + bool hr = handler.EndArray(c); + // On handler short circuits the parsing. + if (!hr) { + RAPIDJSON_PARSE_ERROR_NORETURN(kParseErrorTermination, is.Tell()); + return IterativeParsingErrorState; + } + else { + is.Take(); + return n; + } + } + + default: + // This branch is for IterativeParsingValueState actually. + // Use `default:` rather than + // `case IterativeParsingValueState:` is for code coverage. + + // The IterativeParsingStartState is not enumerated in this switch-case. + // It is impossible for that case. And it can be caught by following assertion. + + // The IterativeParsingFinishState is not enumerated in this switch-case either. + // It is a "derivative" state which cannot triggered from Predict() directly. + // Therefore it cannot happen here. And it can be caught by following assertion. + RAPIDJSON_ASSERT(dst == IterativeParsingValueState); + + // Must be non-compound value. Or it would be ObjectInitial or ArrayInitial state. + ParseValue(is, handler); + if (HasParseError()) { + return IterativeParsingErrorState; + } + return IterativeParsingFinishState; + } + } + + template + void HandleError(IterativeParsingState src, InputStream& is) { + if (HasParseError()) { + // Error flag has been set. + return; + } + + switch (src) { + case IterativeParsingStartState: RAPIDJSON_PARSE_ERROR(kParseErrorDocumentEmpty, is.Tell()); return; + case IterativeParsingFinishState: RAPIDJSON_PARSE_ERROR(kParseErrorDocumentRootNotSingular, is.Tell()); return; + case IterativeParsingObjectInitialState: + case IterativeParsingMemberDelimiterState: RAPIDJSON_PARSE_ERROR(kParseErrorObjectMissName, is.Tell()); return; + case IterativeParsingMemberKeyState: RAPIDJSON_PARSE_ERROR(kParseErrorObjectMissColon, is.Tell()); return; + case IterativeParsingMemberValueState: RAPIDJSON_PARSE_ERROR(kParseErrorObjectMissCommaOrCurlyBracket, is.Tell()); return; + case IterativeParsingKeyValueDelimiterState: + case IterativeParsingArrayInitialState: + case IterativeParsingElementDelimiterState: RAPIDJSON_PARSE_ERROR(kParseErrorValueInvalid, is.Tell()); return; + default: RAPIDJSON_ASSERT(src == IterativeParsingElementState); RAPIDJSON_PARSE_ERROR(kParseErrorArrayMissCommaOrSquareBracket, is.Tell()); return; + } + } + + RAPIDJSON_FORCEINLINE bool IsIterativeParsingDelimiterState(IterativeParsingState s) const { + return s >= IterativeParsingElementDelimiterState; + } + + RAPIDJSON_FORCEINLINE bool IsIterativeParsingCompleteState(IterativeParsingState s) const { + return s <= IterativeParsingErrorState; + } + + template + ParseResult IterativeParse(InputStream& is, Handler& handler) { + parseResult_.Clear(); + ClearStackOnExit scope(*this); + IterativeParsingState state = IterativeParsingStartState; + + SkipWhitespaceAndComments(is); + RAPIDJSON_PARSE_ERROR_EARLY_RETURN(parseResult_); + while (is.Peek() != '\0') { + Token t = Tokenize(is.Peek()); + IterativeParsingState n = Predict(state, t); + IterativeParsingState d = Transit(state, t, n, is, handler); + + if (d == IterativeParsingErrorState) { + HandleError(state, is); + break; + } + + state = d; + + // Do not further consume streams if a root JSON has been parsed. + if ((parseFlags & kParseStopWhenDoneFlag) && state == IterativeParsingFinishState) + break; + + SkipWhitespaceAndComments(is); + RAPIDJSON_PARSE_ERROR_EARLY_RETURN(parseResult_); + } + + // Handle the end of file. + if (state != IterativeParsingFinishState) + HandleError(state, is); + + return parseResult_; + } + + static const size_t kDefaultStackCapacity = 256; //!< Default stack capacity in bytes for storing a single decoded string. + internal::Stack stack_; //!< A stack for storing decoded string temporarily during non-destructive parsing. + ParseResult parseResult_; + IterativeParsingState state_; +}; // class GenericReader + +//! Reader with UTF8 encoding and default allocator. +typedef GenericReader, UTF8<> > Reader; + +RAPIDJSON_NAMESPACE_END + +#if defined(__clang__) || defined(_MSC_VER) +RAPIDJSON_DIAG_POP +#endif + + +#ifdef __GNUC__ +RAPIDJSON_DIAG_POP +#endif + +#endif // RAPIDJSON_READER_H_ diff --git a/livox_ros_driver/common/rapidjson/schema.h b/livox_ros_driver/common/rapidjson/schema.h new file mode 100644 index 0000000..1ff8883 --- /dev/null +++ b/livox_ros_driver/common/rapidjson/schema.h @@ -0,0 +1,2497 @@ +// 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_SCHEMA_H_ +#define RAPIDJSON_SCHEMA_H_ + +#include "document.h" +#include "pointer.h" +#include "stringbuffer.h" +#include // abs, floor + +#if !defined(RAPIDJSON_SCHEMA_USE_INTERNALREGEX) +#define RAPIDJSON_SCHEMA_USE_INTERNALREGEX 1 +#else +#define RAPIDJSON_SCHEMA_USE_INTERNALREGEX 0 +#endif + +#if !RAPIDJSON_SCHEMA_USE_INTERNALREGEX && defined(RAPIDJSON_SCHEMA_USE_STDREGEX) && (__cplusplus >=201103L || (defined(_MSC_VER) && _MSC_VER >= 1800)) +#define RAPIDJSON_SCHEMA_USE_STDREGEX 1 +#else +#define RAPIDJSON_SCHEMA_USE_STDREGEX 0 +#endif + +#if RAPIDJSON_SCHEMA_USE_INTERNALREGEX +#include "internal/regex.h" +#elif RAPIDJSON_SCHEMA_USE_STDREGEX +#include +#endif + +#if RAPIDJSON_SCHEMA_USE_INTERNALREGEX || RAPIDJSON_SCHEMA_USE_STDREGEX +#define RAPIDJSON_SCHEMA_HAS_REGEX 1 +#else +#define RAPIDJSON_SCHEMA_HAS_REGEX 0 +#endif + +#ifndef RAPIDJSON_SCHEMA_VERBOSE +#define RAPIDJSON_SCHEMA_VERBOSE 0 +#endif + +#if RAPIDJSON_SCHEMA_VERBOSE +#include "stringbuffer.h" +#endif + +RAPIDJSON_DIAG_PUSH + +#if defined(__GNUC__) +RAPIDJSON_DIAG_OFF(effc++) +#endif + +#ifdef __clang__ +RAPIDJSON_DIAG_OFF(weak-vtables) +RAPIDJSON_DIAG_OFF(exit-time-destructors) +RAPIDJSON_DIAG_OFF(c++98-compat-pedantic) +RAPIDJSON_DIAG_OFF(variadic-macros) +#elif defined(_MSC_VER) +RAPIDJSON_DIAG_OFF(4512) // assignment operator could not be generated +#endif + +RAPIDJSON_NAMESPACE_BEGIN + +/////////////////////////////////////////////////////////////////////////////// +// Verbose Utilities + +#if RAPIDJSON_SCHEMA_VERBOSE + +namespace internal { + +inline void PrintInvalidKeyword(const char* keyword) { + printf("Fail keyword: %s\n", keyword); +} + +inline void PrintInvalidKeyword(const wchar_t* keyword) { + wprintf(L"Fail keyword: %ls\n", keyword); +} + +inline void PrintInvalidDocument(const char* document) { + printf("Fail document: %s\n\n", document); +} + +inline void PrintInvalidDocument(const wchar_t* document) { + wprintf(L"Fail document: %ls\n\n", document); +} + +inline void PrintValidatorPointers(unsigned depth, const char* s, const char* d) { + printf("S: %*s%s\nD: %*s%s\n\n", depth * 4, " ", s, depth * 4, " ", d); +} + +inline void PrintValidatorPointers(unsigned depth, const wchar_t* s, const wchar_t* d) { + wprintf(L"S: %*ls%ls\nD: %*ls%ls\n\n", depth * 4, L" ", s, depth * 4, L" ", d); +} + +} // namespace internal + +#endif // RAPIDJSON_SCHEMA_VERBOSE + +/////////////////////////////////////////////////////////////////////////////// +// RAPIDJSON_INVALID_KEYWORD_RETURN + +#if RAPIDJSON_SCHEMA_VERBOSE +#define RAPIDJSON_INVALID_KEYWORD_VERBOSE(keyword) internal::PrintInvalidKeyword(keyword) +#else +#define RAPIDJSON_INVALID_KEYWORD_VERBOSE(keyword) +#endif + +#define RAPIDJSON_INVALID_KEYWORD_RETURN(keyword)\ +RAPIDJSON_MULTILINEMACRO_BEGIN\ + context.invalidKeyword = keyword.GetString();\ + RAPIDJSON_INVALID_KEYWORD_VERBOSE(keyword.GetString());\ + return false;\ +RAPIDJSON_MULTILINEMACRO_END + +/////////////////////////////////////////////////////////////////////////////// +// Forward declarations + +template +class GenericSchemaDocument; + +namespace internal { + +template +class Schema; + +/////////////////////////////////////////////////////////////////////////////// +// ISchemaValidator + +class ISchemaValidator { +public: + virtual ~ISchemaValidator() {} + virtual bool IsValid() const = 0; +}; + +/////////////////////////////////////////////////////////////////////////////// +// ISchemaStateFactory + +template +class ISchemaStateFactory { +public: + virtual ~ISchemaStateFactory() {} + virtual ISchemaValidator* CreateSchemaValidator(const SchemaType&) = 0; + virtual void DestroySchemaValidator(ISchemaValidator* validator) = 0; + virtual void* CreateHasher() = 0; + virtual uint64_t GetHashCode(void* hasher) = 0; + virtual void DestroryHasher(void* hasher) = 0; + virtual void* MallocState(size_t size) = 0; + virtual void FreeState(void* p) = 0; +}; + +/////////////////////////////////////////////////////////////////////////////// +// IValidationErrorHandler + +template +class IValidationErrorHandler { +public: + typedef typename SchemaType::Ch Ch; + typedef typename SchemaType::SValue SValue; + + virtual ~IValidationErrorHandler() {} + + virtual void NotMultipleOf(int64_t actual, const SValue& expected) = 0; + virtual void NotMultipleOf(uint64_t actual, const SValue& expected) = 0; + virtual void NotMultipleOf(double actual, const SValue& expected) = 0; + virtual void AboveMaximum(int64_t actual, const SValue& expected, bool exclusive) = 0; + virtual void AboveMaximum(uint64_t actual, const SValue& expected, bool exclusive) = 0; + virtual void AboveMaximum(double actual, const SValue& expected, bool exclusive) = 0; + virtual void BelowMinimum(int64_t actual, const SValue& expected, bool exclusive) = 0; + virtual void BelowMinimum(uint64_t actual, const SValue& expected, bool exclusive) = 0; + virtual void BelowMinimum(double actual, const SValue& expected, bool exclusive) = 0; + + virtual void TooLong(const Ch* str, SizeType length, SizeType expected) = 0; + virtual void TooShort(const Ch* str, SizeType length, SizeType expected) = 0; + virtual void DoesNotMatch(const Ch* str, SizeType length) = 0; + + virtual void DisallowedItem(SizeType index) = 0; + virtual void TooFewItems(SizeType actualCount, SizeType expectedCount) = 0; + virtual void TooManyItems(SizeType actualCount, SizeType expectedCount) = 0; + virtual void DuplicateItems(SizeType index1, SizeType index2) = 0; + + virtual void TooManyProperties(SizeType actualCount, SizeType expectedCount) = 0; + virtual void TooFewProperties(SizeType actualCount, SizeType expectedCount) = 0; + virtual void StartMissingProperties() = 0; + virtual void AddMissingProperty(const SValue& name) = 0; + virtual bool EndMissingProperties() = 0; + virtual void PropertyViolations(ISchemaValidator** subvalidators, SizeType count) = 0; + virtual void DisallowedProperty(const Ch* name, SizeType length) = 0; + + virtual void StartDependencyErrors() = 0; + virtual void StartMissingDependentProperties() = 0; + virtual void AddMissingDependentProperty(const SValue& targetName) = 0; + virtual void EndMissingDependentProperties(const SValue& sourceName) = 0; + virtual void AddDependencySchemaError(const SValue& souceName, ISchemaValidator* subvalidator) = 0; + virtual bool EndDependencyErrors() = 0; + + virtual void DisallowedValue() = 0; + virtual void StartDisallowedType() = 0; + virtual void AddExpectedType(const typename SchemaType::ValueType& expectedType) = 0; + virtual void EndDisallowedType(const typename SchemaType::ValueType& actualType) = 0; + virtual void NotAllOf(ISchemaValidator** subvalidators, SizeType count) = 0; + virtual void NoneOf(ISchemaValidator** subvalidators, SizeType count) = 0; + virtual void NotOneOf(ISchemaValidator** subvalidators, SizeType count) = 0; + virtual void Disallowed() = 0; +}; + + +/////////////////////////////////////////////////////////////////////////////// +// Hasher + +// For comparison of compound value +template +class Hasher { +public: + typedef typename Encoding::Ch Ch; + + Hasher(Allocator* allocator = 0, size_t stackCapacity = kDefaultSize) : stack_(allocator, stackCapacity) {} + + bool Null() { return WriteType(kNullType); } + bool Bool(bool b) { return WriteType(b ? kTrueType : kFalseType); } + bool Int(int i) { Number n; n.u.i = i; n.d = static_cast(i); return WriteNumber(n); } + bool Uint(unsigned u) { Number n; n.u.u = u; n.d = static_cast(u); return WriteNumber(n); } + bool Int64(int64_t i) { Number n; n.u.i = i; n.d = static_cast(i); return WriteNumber(n); } + bool Uint64(uint64_t u) { Number n; n.u.u = u; n.d = static_cast(u); return WriteNumber(n); } + bool Double(double d) { + Number n; + if (d < 0) n.u.i = static_cast(d); + else n.u.u = static_cast(d); + n.d = d; + return WriteNumber(n); + } + + bool RawNumber(const Ch* str, SizeType len, bool) { + WriteBuffer(kNumberType, str, len * sizeof(Ch)); + return true; + } + + bool String(const Ch* str, SizeType len, bool) { + WriteBuffer(kStringType, str, len * sizeof(Ch)); + return true; + } + + bool StartObject() { return true; } + bool Key(const Ch* str, SizeType len, bool copy) { return String(str, len, copy); } + bool EndObject(SizeType memberCount) { + uint64_t h = Hash(0, kObjectType); + uint64_t* kv = stack_.template Pop(memberCount * 2); + for (SizeType i = 0; i < memberCount; i++) + h ^= Hash(kv[i * 2], kv[i * 2 + 1]); // Use xor to achieve member order insensitive + *stack_.template Push() = h; + return true; + } + + bool StartArray() { return true; } + bool EndArray(SizeType elementCount) { + uint64_t h = Hash(0, kArrayType); + uint64_t* e = stack_.template Pop(elementCount); + for (SizeType i = 0; i < elementCount; i++) + h = Hash(h, e[i]); // Use hash to achieve element order sensitive + *stack_.template Push() = h; + return true; + } + + bool IsValid() const { return stack_.GetSize() == sizeof(uint64_t); } + + uint64_t GetHashCode() const { + RAPIDJSON_ASSERT(IsValid()); + return *stack_.template Top(); + } + +private: + static const size_t kDefaultSize = 256; + struct Number { + union U { + uint64_t u; + int64_t i; + }u; + double d; + }; + + bool WriteType(Type type) { return WriteBuffer(type, 0, 0); } + + bool WriteNumber(const Number& n) { return WriteBuffer(kNumberType, &n, sizeof(n)); } + + bool WriteBuffer(Type type, const void* data, size_t len) { + // FNV-1a from http://isthe.com/chongo/tech/comp/fnv/ + uint64_t h = Hash(RAPIDJSON_UINT64_C2(0x84222325, 0xcbf29ce4), type); + const unsigned char* d = static_cast(data); + for (size_t i = 0; i < len; i++) + h = Hash(h, d[i]); + *stack_.template Push() = h; + return true; + } + + static uint64_t Hash(uint64_t h, uint64_t d) { + static const uint64_t kPrime = RAPIDJSON_UINT64_C2(0x00000100, 0x000001b3); + h ^= d; + h *= kPrime; + return h; + } + + Stack stack_; +}; + +/////////////////////////////////////////////////////////////////////////////// +// SchemaValidationContext + +template +struct SchemaValidationContext { + typedef Schema SchemaType; + typedef ISchemaStateFactory SchemaValidatorFactoryType; + typedef IValidationErrorHandler ErrorHandlerType; + typedef typename SchemaType::ValueType ValueType; + typedef typename ValueType::Ch Ch; + + enum PatternValidatorType { + kPatternValidatorOnly, + kPatternValidatorWithProperty, + kPatternValidatorWithAdditionalProperty + }; + + SchemaValidationContext(SchemaValidatorFactoryType& f, ErrorHandlerType& eh, const SchemaType* s) : + factory(f), + error_handler(eh), + schema(s), + valueSchema(), + invalidKeyword(), + hasher(), + arrayElementHashCodes(), + validators(), + validatorCount(), + patternPropertiesValidators(), + patternPropertiesValidatorCount(), + patternPropertiesSchemas(), + patternPropertiesSchemaCount(), + valuePatternValidatorType(kPatternValidatorOnly), + propertyExist(), + inArray(false), + valueUniqueness(false), + arrayUniqueness(false) + { + } + + ~SchemaValidationContext() { + if (hasher) + factory.DestroryHasher(hasher); + if (validators) { + for (SizeType i = 0; i < validatorCount; i++) + factory.DestroySchemaValidator(validators[i]); + factory.FreeState(validators); + } + if (patternPropertiesValidators) { + for (SizeType i = 0; i < patternPropertiesValidatorCount; i++) + factory.DestroySchemaValidator(patternPropertiesValidators[i]); + factory.FreeState(patternPropertiesValidators); + } + if (patternPropertiesSchemas) + factory.FreeState(patternPropertiesSchemas); + if (propertyExist) + factory.FreeState(propertyExist); + } + + SchemaValidatorFactoryType& factory; + ErrorHandlerType& error_handler; + const SchemaType* schema; + const SchemaType* valueSchema; + const Ch* invalidKeyword; + void* hasher; // Only validator access + void* arrayElementHashCodes; // Only validator access this + ISchemaValidator** validators; + SizeType validatorCount; + ISchemaValidator** patternPropertiesValidators; + SizeType patternPropertiesValidatorCount; + const SchemaType** patternPropertiesSchemas; + SizeType patternPropertiesSchemaCount; + PatternValidatorType valuePatternValidatorType; + PatternValidatorType objectPatternValidatorType; + SizeType arrayElementIndex; + bool* propertyExist; + bool inArray; + bool valueUniqueness; + bool arrayUniqueness; +}; + +/////////////////////////////////////////////////////////////////////////////// +// Schema + +template +class Schema { +public: + typedef typename SchemaDocumentType::ValueType ValueType; + typedef typename SchemaDocumentType::AllocatorType AllocatorType; + typedef typename SchemaDocumentType::PointerType PointerType; + typedef typename ValueType::EncodingType EncodingType; + typedef typename EncodingType::Ch Ch; + typedef SchemaValidationContext Context; + typedef Schema SchemaType; + typedef GenericValue SValue; + typedef IValidationErrorHandler ErrorHandler; + friend class GenericSchemaDocument; + + Schema(SchemaDocumentType* schemaDocument, const PointerType& p, const ValueType& value, const ValueType& document, AllocatorType* allocator) : + allocator_(allocator), + uri_(schemaDocument->GetURI(), *allocator), + pointer_(p, allocator), + typeless_(schemaDocument->GetTypeless()), + enum_(), + enumCount_(), + not_(), + type_((1 << kTotalSchemaType) - 1), // typeless + validatorCount_(), + notValidatorIndex_(), + properties_(), + additionalPropertiesSchema_(), + patternProperties_(), + patternPropertyCount_(), + propertyCount_(), + minProperties_(), + maxProperties_(SizeType(~0)), + additionalProperties_(true), + hasDependencies_(), + hasRequired_(), + hasSchemaDependencies_(), + additionalItemsSchema_(), + itemsList_(), + itemsTuple_(), + itemsTupleCount_(), + minItems_(), + maxItems_(SizeType(~0)), + additionalItems_(true), + uniqueItems_(false), + pattern_(), + minLength_(0), + maxLength_(~SizeType(0)), + exclusiveMinimum_(false), + exclusiveMaximum_(false), + defaultValueLength_(0) + { + typedef typename SchemaDocumentType::ValueType ValueType; + typedef typename ValueType::ConstValueIterator ConstValueIterator; + typedef typename ValueType::ConstMemberIterator ConstMemberIterator; + + if (!value.IsObject()) + return; + + if (const ValueType* v = GetMember(value, GetTypeString())) { + type_ = 0; + if (v->IsString()) + AddType(*v); + else if (v->IsArray()) + for (ConstValueIterator itr = v->Begin(); itr != v->End(); ++itr) + AddType(*itr); + } + + if (const ValueType* v = GetMember(value, GetEnumString())) + if (v->IsArray() && v->Size() > 0) { + enum_ = static_cast(allocator_->Malloc(sizeof(uint64_t) * v->Size())); + for (ConstValueIterator itr = v->Begin(); itr != v->End(); ++itr) { + typedef Hasher > EnumHasherType; + char buffer[256u + 24]; + MemoryPoolAllocator<> hasherAllocator(buffer, sizeof(buffer)); + EnumHasherType h(&hasherAllocator, 256); + itr->Accept(h); + enum_[enumCount_++] = h.GetHashCode(); + } + } + + if (schemaDocument) { + AssignIfExist(allOf_, *schemaDocument, p, value, GetAllOfString(), document); + AssignIfExist(anyOf_, *schemaDocument, p, value, GetAnyOfString(), document); + AssignIfExist(oneOf_, *schemaDocument, p, value, GetOneOfString(), document); + } + + if (const ValueType* v = GetMember(value, GetNotString())) { + schemaDocument->CreateSchema(¬_, p.Append(GetNotString(), allocator_), *v, document); + notValidatorIndex_ = validatorCount_; + validatorCount_++; + } + + // Object + + const ValueType* properties = GetMember(value, GetPropertiesString()); + const ValueType* required = GetMember(value, GetRequiredString()); + const ValueType* dependencies = GetMember(value, GetDependenciesString()); + { + // Gather properties from properties/required/dependencies + SValue allProperties(kArrayType); + + if (properties && properties->IsObject()) + for (ConstMemberIterator itr = properties->MemberBegin(); itr != properties->MemberEnd(); ++itr) + AddUniqueElement(allProperties, itr->name); + + if (required && required->IsArray()) + for (ConstValueIterator itr = required->Begin(); itr != required->End(); ++itr) + if (itr->IsString()) + AddUniqueElement(allProperties, *itr); + + if (dependencies && dependencies->IsObject()) + for (ConstMemberIterator itr = dependencies->MemberBegin(); itr != dependencies->MemberEnd(); ++itr) { + AddUniqueElement(allProperties, itr->name); + if (itr->value.IsArray()) + for (ConstValueIterator i = itr->value.Begin(); i != itr->value.End(); ++i) + if (i->IsString()) + AddUniqueElement(allProperties, *i); + } + + if (allProperties.Size() > 0) { + propertyCount_ = allProperties.Size(); + properties_ = static_cast(allocator_->Malloc(sizeof(Property) * propertyCount_)); + for (SizeType i = 0; i < propertyCount_; i++) { + new (&properties_[i]) Property(); + properties_[i].name = allProperties[i]; + properties_[i].schema = typeless_; + } + } + } + + if (properties && properties->IsObject()) { + PointerType q = p.Append(GetPropertiesString(), allocator_); + for (ConstMemberIterator itr = properties->MemberBegin(); itr != properties->MemberEnd(); ++itr) { + SizeType index; + if (FindPropertyIndex(itr->name, &index)) + schemaDocument->CreateSchema(&properties_[index].schema, q.Append(itr->name, allocator_), itr->value, document); + } + } + + if (const ValueType* v = GetMember(value, GetPatternPropertiesString())) { + PointerType q = p.Append(GetPatternPropertiesString(), allocator_); + patternProperties_ = static_cast(allocator_->Malloc(sizeof(PatternProperty) * v->MemberCount())); + patternPropertyCount_ = 0; + + for (ConstMemberIterator itr = v->MemberBegin(); itr != v->MemberEnd(); ++itr) { + new (&patternProperties_[patternPropertyCount_]) PatternProperty(); + patternProperties_[patternPropertyCount_].pattern = CreatePattern(itr->name); + schemaDocument->CreateSchema(&patternProperties_[patternPropertyCount_].schema, q.Append(itr->name, allocator_), itr->value, document); + patternPropertyCount_++; + } + } + + if (required && required->IsArray()) + for (ConstValueIterator itr = required->Begin(); itr != required->End(); ++itr) + if (itr->IsString()) { + SizeType index; + if (FindPropertyIndex(*itr, &index)) { + properties_[index].required = true; + hasRequired_ = true; + } + } + + if (dependencies && dependencies->IsObject()) { + PointerType q = p.Append(GetDependenciesString(), allocator_); + hasDependencies_ = true; + for (ConstMemberIterator itr = dependencies->MemberBegin(); itr != dependencies->MemberEnd(); ++itr) { + SizeType sourceIndex; + if (FindPropertyIndex(itr->name, &sourceIndex)) { + if (itr->value.IsArray()) { + properties_[sourceIndex].dependencies = static_cast(allocator_->Malloc(sizeof(bool) * propertyCount_)); + std::memset(properties_[sourceIndex].dependencies, 0, sizeof(bool)* propertyCount_); + for (ConstValueIterator targetItr = itr->value.Begin(); targetItr != itr->value.End(); ++targetItr) { + SizeType targetIndex; + if (FindPropertyIndex(*targetItr, &targetIndex)) + properties_[sourceIndex].dependencies[targetIndex] = true; + } + } + else if (itr->value.IsObject()) { + hasSchemaDependencies_ = true; + schemaDocument->CreateSchema(&properties_[sourceIndex].dependenciesSchema, q.Append(itr->name, allocator_), itr->value, document); + properties_[sourceIndex].dependenciesValidatorIndex = validatorCount_; + validatorCount_++; + } + } + } + } + + if (const ValueType* v = GetMember(value, GetAdditionalPropertiesString())) { + if (v->IsBool()) + additionalProperties_ = v->GetBool(); + else if (v->IsObject()) + schemaDocument->CreateSchema(&additionalPropertiesSchema_, p.Append(GetAdditionalPropertiesString(), allocator_), *v, document); + } + + AssignIfExist(minProperties_, value, GetMinPropertiesString()); + AssignIfExist(maxProperties_, value, GetMaxPropertiesString()); + + // Array + if (const ValueType* v = GetMember(value, GetItemsString())) { + PointerType q = p.Append(GetItemsString(), allocator_); + if (v->IsObject()) // List validation + schemaDocument->CreateSchema(&itemsList_, q, *v, document); + else if (v->IsArray()) { // Tuple validation + itemsTuple_ = static_cast(allocator_->Malloc(sizeof(const Schema*) * v->Size())); + SizeType index = 0; + for (ConstValueIterator itr = v->Begin(); itr != v->End(); ++itr, index++) + schemaDocument->CreateSchema(&itemsTuple_[itemsTupleCount_++], q.Append(index, allocator_), *itr, document); + } + } + + AssignIfExist(minItems_, value, GetMinItemsString()); + AssignIfExist(maxItems_, value, GetMaxItemsString()); + + if (const ValueType* v = GetMember(value, GetAdditionalItemsString())) { + if (v->IsBool()) + additionalItems_ = v->GetBool(); + else if (v->IsObject()) + schemaDocument->CreateSchema(&additionalItemsSchema_, p.Append(GetAdditionalItemsString(), allocator_), *v, document); + } + + AssignIfExist(uniqueItems_, value, GetUniqueItemsString()); + + // String + AssignIfExist(minLength_, value, GetMinLengthString()); + AssignIfExist(maxLength_, value, GetMaxLengthString()); + + if (const ValueType* v = GetMember(value, GetPatternString())) + pattern_ = CreatePattern(*v); + + // Number + if (const ValueType* v = GetMember(value, GetMinimumString())) + if (v->IsNumber()) + minimum_.CopyFrom(*v, *allocator_); + + if (const ValueType* v = GetMember(value, GetMaximumString())) + if (v->IsNumber()) + maximum_.CopyFrom(*v, *allocator_); + + AssignIfExist(exclusiveMinimum_, value, GetExclusiveMinimumString()); + AssignIfExist(exclusiveMaximum_, value, GetExclusiveMaximumString()); + + if (const ValueType* v = GetMember(value, GetMultipleOfString())) + if (v->IsNumber() && v->GetDouble() > 0.0) + multipleOf_.CopyFrom(*v, *allocator_); + + // Default + if (const ValueType* v = GetMember(value, GetDefaultValueString())) + if (v->IsString()) + defaultValueLength_ = v->GetStringLength(); + + } + + ~Schema() { + AllocatorType::Free(enum_); + if (properties_) { + for (SizeType i = 0; i < propertyCount_; i++) + properties_[i].~Property(); + AllocatorType::Free(properties_); + } + if (patternProperties_) { + for (SizeType i = 0; i < patternPropertyCount_; i++) + patternProperties_[i].~PatternProperty(); + AllocatorType::Free(patternProperties_); + } + AllocatorType::Free(itemsTuple_); +#if RAPIDJSON_SCHEMA_HAS_REGEX + if (pattern_) { + pattern_->~RegexType(); + AllocatorType::Free(pattern_); + } +#endif + } + + const SValue& GetURI() const { + return uri_; + } + + const PointerType& GetPointer() const { + return pointer_; + } + + bool BeginValue(Context& context) const { + if (context.inArray) { + if (uniqueItems_) + context.valueUniqueness = true; + + if (itemsList_) + context.valueSchema = itemsList_; + else if (itemsTuple_) { + if (context.arrayElementIndex < itemsTupleCount_) + context.valueSchema = itemsTuple_[context.arrayElementIndex]; + else if (additionalItemsSchema_) + context.valueSchema = additionalItemsSchema_; + else if (additionalItems_) + context.valueSchema = typeless_; + else { + context.error_handler.DisallowedItem(context.arrayElementIndex); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetItemsString()); + } + } + else + context.valueSchema = typeless_; + + context.arrayElementIndex++; + } + return true; + } + + RAPIDJSON_FORCEINLINE bool EndValue(Context& context) const { + if (context.patternPropertiesValidatorCount > 0) { + bool otherValid = false; + SizeType count = context.patternPropertiesValidatorCount; + if (context.objectPatternValidatorType != Context::kPatternValidatorOnly) + otherValid = context.patternPropertiesValidators[--count]->IsValid(); + + bool patternValid = true; + for (SizeType i = 0; i < count; i++) + if (!context.patternPropertiesValidators[i]->IsValid()) { + patternValid = false; + break; + } + + if (context.objectPatternValidatorType == Context::kPatternValidatorOnly) { + if (!patternValid) { + context.error_handler.PropertyViolations(context.patternPropertiesValidators, count); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetPatternPropertiesString()); + } + } + else if (context.objectPatternValidatorType == Context::kPatternValidatorWithProperty) { + if (!patternValid || !otherValid) { + context.error_handler.PropertyViolations(context.patternPropertiesValidators, count + 1); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetPatternPropertiesString()); + } + } + else if (!patternValid && !otherValid) { // kPatternValidatorWithAdditionalProperty) + context.error_handler.PropertyViolations(context.patternPropertiesValidators, count + 1); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetPatternPropertiesString()); + } + } + + if (enum_) { + const uint64_t h = context.factory.GetHashCode(context.hasher); + for (SizeType i = 0; i < enumCount_; i++) + if (enum_[i] == h) + goto foundEnum; + context.error_handler.DisallowedValue(); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetEnumString()); + foundEnum:; + } + + if (allOf_.schemas) + for (SizeType i = allOf_.begin; i < allOf_.begin + allOf_.count; i++) + if (!context.validators[i]->IsValid()) { + context.error_handler.NotAllOf(&context.validators[allOf_.begin], allOf_.count); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetAllOfString()); + } + + if (anyOf_.schemas) { + for (SizeType i = anyOf_.begin; i < anyOf_.begin + anyOf_.count; i++) + if (context.validators[i]->IsValid()) + goto foundAny; + context.error_handler.NoneOf(&context.validators[anyOf_.begin], anyOf_.count); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetAnyOfString()); + foundAny:; + } + + if (oneOf_.schemas) { + bool oneValid = false; + for (SizeType i = oneOf_.begin; i < oneOf_.begin + oneOf_.count; i++) + if (context.validators[i]->IsValid()) { + if (oneValid) { + context.error_handler.NotOneOf(&context.validators[oneOf_.begin], oneOf_.count); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetOneOfString()); + } else + oneValid = true; + } + if (!oneValid) { + context.error_handler.NotOneOf(&context.validators[oneOf_.begin], oneOf_.count); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetOneOfString()); + } + } + + if (not_ && context.validators[notValidatorIndex_]->IsValid()) { + context.error_handler.Disallowed(); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetNotString()); + } + + return true; + } + + bool Null(Context& context) const { + if (!(type_ & (1 << kNullSchemaType))) { + DisallowedType(context, GetNullString()); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetTypeString()); + } + return CreateParallelValidator(context); + } + + bool Bool(Context& context, bool) const { + if (!(type_ & (1 << kBooleanSchemaType))) { + DisallowedType(context, GetBooleanString()); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetTypeString()); + } + return CreateParallelValidator(context); + } + + bool Int(Context& context, int i) const { + if (!CheckInt(context, i)) + return false; + return CreateParallelValidator(context); + } + + bool Uint(Context& context, unsigned u) const { + if (!CheckUint(context, u)) + return false; + return CreateParallelValidator(context); + } + + bool Int64(Context& context, int64_t i) const { + if (!CheckInt(context, i)) + return false; + return CreateParallelValidator(context); + } + + bool Uint64(Context& context, uint64_t u) const { + if (!CheckUint(context, u)) + return false; + return CreateParallelValidator(context); + } + + bool Double(Context& context, double d) const { + if (!(type_ & (1 << kNumberSchemaType))) { + DisallowedType(context, GetNumberString()); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetTypeString()); + } + + if (!minimum_.IsNull() && !CheckDoubleMinimum(context, d)) + return false; + + if (!maximum_.IsNull() && !CheckDoubleMaximum(context, d)) + return false; + + if (!multipleOf_.IsNull() && !CheckDoubleMultipleOf(context, d)) + return false; + + return CreateParallelValidator(context); + } + + bool String(Context& context, const Ch* str, SizeType length, bool) const { + if (!(type_ & (1 << kStringSchemaType))) { + DisallowedType(context, GetStringString()); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetTypeString()); + } + + if (minLength_ != 0 || maxLength_ != SizeType(~0)) { + SizeType count; + if (internal::CountStringCodePoint(str, length, &count)) { + if (count < minLength_) { + context.error_handler.TooShort(str, length, minLength_); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetMinLengthString()); + } + if (count > maxLength_) { + context.error_handler.TooLong(str, length, maxLength_); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetMaxLengthString()); + } + } + } + + if (pattern_ && !IsPatternMatch(pattern_, str, length)) { + context.error_handler.DoesNotMatch(str, length); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetPatternString()); + } + + return CreateParallelValidator(context); + } + + bool StartObject(Context& context) const { + if (!(type_ & (1 << kObjectSchemaType))) { + DisallowedType(context, GetObjectString()); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetTypeString()); + } + + if (hasDependencies_ || hasRequired_) { + context.propertyExist = static_cast(context.factory.MallocState(sizeof(bool) * propertyCount_)); + std::memset(context.propertyExist, 0, sizeof(bool) * propertyCount_); + } + + if (patternProperties_) { // pre-allocate schema array + SizeType count = patternPropertyCount_ + 1; // extra for valuePatternValidatorType + context.patternPropertiesSchemas = static_cast(context.factory.MallocState(sizeof(const SchemaType*) * count)); + context.patternPropertiesSchemaCount = 0; + std::memset(context.patternPropertiesSchemas, 0, sizeof(SchemaType*) * count); + } + + return CreateParallelValidator(context); + } + + bool Key(Context& context, const Ch* str, SizeType len, bool) const { + if (patternProperties_) { + context.patternPropertiesSchemaCount = 0; + for (SizeType i = 0; i < patternPropertyCount_; i++) + if (patternProperties_[i].pattern && IsPatternMatch(patternProperties_[i].pattern, str, len)) { + context.patternPropertiesSchemas[context.patternPropertiesSchemaCount++] = patternProperties_[i].schema; + context.valueSchema = typeless_; + } + } + + SizeType index = 0; + if (FindPropertyIndex(ValueType(str, len).Move(), &index)) { + if (context.patternPropertiesSchemaCount > 0) { + context.patternPropertiesSchemas[context.patternPropertiesSchemaCount++] = properties_[index].schema; + context.valueSchema = typeless_; + context.valuePatternValidatorType = Context::kPatternValidatorWithProperty; + } + else + context.valueSchema = properties_[index].schema; + + if (context.propertyExist) + context.propertyExist[index] = true; + + return true; + } + + if (additionalPropertiesSchema_) { + if (additionalPropertiesSchema_ && context.patternPropertiesSchemaCount > 0) { + context.patternPropertiesSchemas[context.patternPropertiesSchemaCount++] = additionalPropertiesSchema_; + context.valueSchema = typeless_; + context.valuePatternValidatorType = Context::kPatternValidatorWithAdditionalProperty; + } + else + context.valueSchema = additionalPropertiesSchema_; + return true; + } + else if (additionalProperties_) { + context.valueSchema = typeless_; + return true; + } + + if (context.patternPropertiesSchemaCount == 0) { // patternProperties are not additional properties + context.error_handler.DisallowedProperty(str, len); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetAdditionalPropertiesString()); + } + + return true; + } + + bool EndObject(Context& context, SizeType memberCount) const { + if (hasRequired_) { + context.error_handler.StartMissingProperties(); + for (SizeType index = 0; index < propertyCount_; index++) + if (properties_[index].required && !context.propertyExist[index]) + if (properties_[index].schema->defaultValueLength_ == 0 ) + context.error_handler.AddMissingProperty(properties_[index].name); + if (context.error_handler.EndMissingProperties()) + RAPIDJSON_INVALID_KEYWORD_RETURN(GetRequiredString()); + } + + if (memberCount < minProperties_) { + context.error_handler.TooFewProperties(memberCount, minProperties_); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetMinPropertiesString()); + } + + if (memberCount > maxProperties_) { + context.error_handler.TooManyProperties(memberCount, maxProperties_); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetMaxPropertiesString()); + } + + if (hasDependencies_) { + context.error_handler.StartDependencyErrors(); + for (SizeType sourceIndex = 0; sourceIndex < propertyCount_; sourceIndex++) { + const Property& source = properties_[sourceIndex]; + if (context.propertyExist[sourceIndex]) { + if (source.dependencies) { + context.error_handler.StartMissingDependentProperties(); + for (SizeType targetIndex = 0; targetIndex < propertyCount_; targetIndex++) + if (source.dependencies[targetIndex] && !context.propertyExist[targetIndex]) + context.error_handler.AddMissingDependentProperty(properties_[targetIndex].name); + context.error_handler.EndMissingDependentProperties(source.name); + } + else if (source.dependenciesSchema) { + ISchemaValidator* dependenciesValidator = context.validators[source.dependenciesValidatorIndex]; + if (!dependenciesValidator->IsValid()) + context.error_handler.AddDependencySchemaError(source.name, dependenciesValidator); + } + } + } + if (context.error_handler.EndDependencyErrors()) + RAPIDJSON_INVALID_KEYWORD_RETURN(GetDependenciesString()); + } + + return true; + } + + bool StartArray(Context& context) const { + if (!(type_ & (1 << kArraySchemaType))) { + DisallowedType(context, GetArrayString()); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetTypeString()); + } + + context.arrayElementIndex = 0; + context.inArray = true; + + return CreateParallelValidator(context); + } + + bool EndArray(Context& context, SizeType elementCount) const { + context.inArray = false; + + if (elementCount < minItems_) { + context.error_handler.TooFewItems(elementCount, minItems_); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetMinItemsString()); + } + + if (elementCount > maxItems_) { + context.error_handler.TooManyItems(elementCount, maxItems_); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetMaxItemsString()); + } + + return true; + } + + // Generate functions for string literal according to Ch +#define RAPIDJSON_STRING_(name, ...) \ + static const ValueType& Get##name##String() {\ + static const Ch s[] = { __VA_ARGS__, '\0' };\ + static const ValueType v(s, static_cast(sizeof(s) / sizeof(Ch) - 1));\ + return v;\ + } + + RAPIDJSON_STRING_(Null, 'n', 'u', 'l', 'l') + RAPIDJSON_STRING_(Boolean, 'b', 'o', 'o', 'l', 'e', 'a', 'n') + RAPIDJSON_STRING_(Object, 'o', 'b', 'j', 'e', 'c', 't') + RAPIDJSON_STRING_(Array, 'a', 'r', 'r', 'a', 'y') + RAPIDJSON_STRING_(String, 's', 't', 'r', 'i', 'n', 'g') + RAPIDJSON_STRING_(Number, 'n', 'u', 'm', 'b', 'e', 'r') + RAPIDJSON_STRING_(Integer, 'i', 'n', 't', 'e', 'g', 'e', 'r') + RAPIDJSON_STRING_(Type, 't', 'y', 'p', 'e') + RAPIDJSON_STRING_(Enum, 'e', 'n', 'u', 'm') + RAPIDJSON_STRING_(AllOf, 'a', 'l', 'l', 'O', 'f') + RAPIDJSON_STRING_(AnyOf, 'a', 'n', 'y', 'O', 'f') + RAPIDJSON_STRING_(OneOf, 'o', 'n', 'e', 'O', 'f') + RAPIDJSON_STRING_(Not, 'n', 'o', 't') + RAPIDJSON_STRING_(Properties, 'p', 'r', 'o', 'p', 'e', 'r', 't', 'i', 'e', 's') + RAPIDJSON_STRING_(Required, 'r', 'e', 'q', 'u', 'i', 'r', 'e', 'd') + RAPIDJSON_STRING_(Dependencies, 'd', 'e', 'p', 'e', 'n', 'd', 'e', 'n', 'c', 'i', 'e', 's') + RAPIDJSON_STRING_(PatternProperties, 'p', 'a', 't', 't', 'e', 'r', 'n', 'P', 'r', 'o', 'p', 'e', 'r', 't', 'i', 'e', 's') + RAPIDJSON_STRING_(AdditionalProperties, 'a', 'd', 'd', 'i', 't', 'i', 'o', 'n', 'a', 'l', 'P', 'r', 'o', 'p', 'e', 'r', 't', 'i', 'e', 's') + RAPIDJSON_STRING_(MinProperties, 'm', 'i', 'n', 'P', 'r', 'o', 'p', 'e', 'r', 't', 'i', 'e', 's') + RAPIDJSON_STRING_(MaxProperties, 'm', 'a', 'x', 'P', 'r', 'o', 'p', 'e', 'r', 't', 'i', 'e', 's') + RAPIDJSON_STRING_(Items, 'i', 't', 'e', 'm', 's') + RAPIDJSON_STRING_(MinItems, 'm', 'i', 'n', 'I', 't', 'e', 'm', 's') + RAPIDJSON_STRING_(MaxItems, 'm', 'a', 'x', 'I', 't', 'e', 'm', 's') + RAPIDJSON_STRING_(AdditionalItems, 'a', 'd', 'd', 'i', 't', 'i', 'o', 'n', 'a', 'l', 'I', 't', 'e', 'm', 's') + RAPIDJSON_STRING_(UniqueItems, 'u', 'n', 'i', 'q', 'u', 'e', 'I', 't', 'e', 'm', 's') + RAPIDJSON_STRING_(MinLength, 'm', 'i', 'n', 'L', 'e', 'n', 'g', 't', 'h') + RAPIDJSON_STRING_(MaxLength, 'm', 'a', 'x', 'L', 'e', 'n', 'g', 't', 'h') + RAPIDJSON_STRING_(Pattern, 'p', 'a', 't', 't', 'e', 'r', 'n') + RAPIDJSON_STRING_(Minimum, 'm', 'i', 'n', 'i', 'm', 'u', 'm') + RAPIDJSON_STRING_(Maximum, 'm', 'a', 'x', 'i', 'm', 'u', 'm') + RAPIDJSON_STRING_(ExclusiveMinimum, 'e', 'x', 'c', 'l', 'u', 's', 'i', 'v', 'e', 'M', 'i', 'n', 'i', 'm', 'u', 'm') + RAPIDJSON_STRING_(ExclusiveMaximum, 'e', 'x', 'c', 'l', 'u', 's', 'i', 'v', 'e', 'M', 'a', 'x', 'i', 'm', 'u', 'm') + RAPIDJSON_STRING_(MultipleOf, 'm', 'u', 'l', 't', 'i', 'p', 'l', 'e', 'O', 'f') + RAPIDJSON_STRING_(DefaultValue, 'd', 'e', 'f', 'a', 'u', 'l', 't') + +#undef RAPIDJSON_STRING_ + +private: + enum SchemaValueType { + kNullSchemaType, + kBooleanSchemaType, + kObjectSchemaType, + kArraySchemaType, + kStringSchemaType, + kNumberSchemaType, + kIntegerSchemaType, + kTotalSchemaType + }; + +#if RAPIDJSON_SCHEMA_USE_INTERNALREGEX + typedef internal::GenericRegex RegexType; +#elif RAPIDJSON_SCHEMA_USE_STDREGEX + typedef std::basic_regex RegexType; +#else + typedef char RegexType; +#endif + + struct SchemaArray { + SchemaArray() : schemas(), count() {} + ~SchemaArray() { AllocatorType::Free(schemas); } + const SchemaType** schemas; + SizeType begin; // begin index of context.validators + SizeType count; + }; + + template + void AddUniqueElement(V1& a, const V2& v) { + for (typename V1::ConstValueIterator itr = a.Begin(); itr != a.End(); ++itr) + if (*itr == v) + return; + V1 c(v, *allocator_); + a.PushBack(c, *allocator_); + } + + static const ValueType* GetMember(const ValueType& value, const ValueType& name) { + typename ValueType::ConstMemberIterator itr = value.FindMember(name); + return itr != value.MemberEnd() ? &(itr->value) : 0; + } + + static void AssignIfExist(bool& out, const ValueType& value, const ValueType& name) { + if (const ValueType* v = GetMember(value, name)) + if (v->IsBool()) + out = v->GetBool(); + } + + static void AssignIfExist(SizeType& out, const ValueType& value, const ValueType& name) { + if (const ValueType* v = GetMember(value, name)) + if (v->IsUint64() && v->GetUint64() <= SizeType(~0)) + out = static_cast(v->GetUint64()); + } + + void AssignIfExist(SchemaArray& out, SchemaDocumentType& schemaDocument, const PointerType& p, const ValueType& value, const ValueType& name, const ValueType& document) { + if (const ValueType* v = GetMember(value, name)) { + if (v->IsArray() && v->Size() > 0) { + PointerType q = p.Append(name, allocator_); + out.count = v->Size(); + out.schemas = static_cast(allocator_->Malloc(out.count * sizeof(const Schema*))); + memset(out.schemas, 0, sizeof(Schema*)* out.count); + for (SizeType i = 0; i < out.count; i++) + schemaDocument.CreateSchema(&out.schemas[i], q.Append(i, allocator_), (*v)[i], document); + out.begin = validatorCount_; + validatorCount_ += out.count; + } + } + } + +#if RAPIDJSON_SCHEMA_USE_INTERNALREGEX + template + RegexType* CreatePattern(const ValueType& value) { + if (value.IsString()) { + RegexType* r = new (allocator_->Malloc(sizeof(RegexType))) RegexType(value.GetString(), allocator_); + if (!r->IsValid()) { + r->~RegexType(); + AllocatorType::Free(r); + r = 0; + } + return r; + } + return 0; + } + + static bool IsPatternMatch(const RegexType* pattern, const Ch *str, SizeType) { + GenericRegexSearch rs(*pattern); + return rs.Search(str); + } +#elif RAPIDJSON_SCHEMA_USE_STDREGEX + template + RegexType* CreatePattern(const ValueType& value) { + if (value.IsString()) { + RegexType *r = static_cast(allocator_->Malloc(sizeof(RegexType))); + try { + return new (r) RegexType(value.GetString(), std::size_t(value.GetStringLength()), std::regex_constants::ECMAScript); + } + catch (const std::regex_error&) { + AllocatorType::Free(r); + } + } + return 0; + } + + static bool IsPatternMatch(const RegexType* pattern, const Ch *str, SizeType length) { + std::match_results r; + return std::regex_search(str, str + length, r, *pattern); + } +#else + template + RegexType* CreatePattern(const ValueType&) { return 0; } + + static bool IsPatternMatch(const RegexType*, const Ch *, SizeType) { return true; } +#endif // RAPIDJSON_SCHEMA_USE_STDREGEX + + void AddType(const ValueType& type) { + if (type == GetNullString() ) type_ |= 1 << kNullSchemaType; + else if (type == GetBooleanString()) type_ |= 1 << kBooleanSchemaType; + else if (type == GetObjectString() ) type_ |= 1 << kObjectSchemaType; + else if (type == GetArrayString() ) type_ |= 1 << kArraySchemaType; + else if (type == GetStringString() ) type_ |= 1 << kStringSchemaType; + else if (type == GetIntegerString()) type_ |= 1 << kIntegerSchemaType; + else if (type == GetNumberString() ) type_ |= (1 << kNumberSchemaType) | (1 << kIntegerSchemaType); + } + + bool CreateParallelValidator(Context& context) const { + if (enum_ || context.arrayUniqueness) + context.hasher = context.factory.CreateHasher(); + + if (validatorCount_) { + RAPIDJSON_ASSERT(context.validators == 0); + context.validators = static_cast(context.factory.MallocState(sizeof(ISchemaValidator*) * validatorCount_)); + context.validatorCount = validatorCount_; + + if (allOf_.schemas) + CreateSchemaValidators(context, allOf_); + + if (anyOf_.schemas) + CreateSchemaValidators(context, anyOf_); + + if (oneOf_.schemas) + CreateSchemaValidators(context, oneOf_); + + if (not_) + context.validators[notValidatorIndex_] = context.factory.CreateSchemaValidator(*not_); + + if (hasSchemaDependencies_) { + for (SizeType i = 0; i < propertyCount_; i++) + if (properties_[i].dependenciesSchema) + context.validators[properties_[i].dependenciesValidatorIndex] = context.factory.CreateSchemaValidator(*properties_[i].dependenciesSchema); + } + } + + return true; + } + + void CreateSchemaValidators(Context& context, const SchemaArray& schemas) const { + for (SizeType i = 0; i < schemas.count; i++) + context.validators[schemas.begin + i] = context.factory.CreateSchemaValidator(*schemas.schemas[i]); + } + + // O(n) + bool FindPropertyIndex(const ValueType& name, SizeType* outIndex) const { + SizeType len = name.GetStringLength(); + const Ch* str = name.GetString(); + for (SizeType index = 0; index < propertyCount_; index++) + if (properties_[index].name.GetStringLength() == len && + (std::memcmp(properties_[index].name.GetString(), str, sizeof(Ch) * len) == 0)) + { + *outIndex = index; + return true; + } + return false; + } + + bool CheckInt(Context& context, int64_t i) const { + if (!(type_ & ((1 << kIntegerSchemaType) | (1 << kNumberSchemaType)))) { + DisallowedType(context, GetIntegerString()); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetTypeString()); + } + + if (!minimum_.IsNull()) { + if (minimum_.IsInt64()) { + if (exclusiveMinimum_ ? i <= minimum_.GetInt64() : i < minimum_.GetInt64()) { + context.error_handler.BelowMinimum(i, minimum_, exclusiveMinimum_); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetMinimumString()); + } + } + else if (minimum_.IsUint64()) { + context.error_handler.BelowMinimum(i, minimum_, exclusiveMinimum_); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetMinimumString()); // i <= max(int64_t) < minimum.GetUint64() + } + else if (!CheckDoubleMinimum(context, static_cast(i))) + return false; + } + + if (!maximum_.IsNull()) { + if (maximum_.IsInt64()) { + if (exclusiveMaximum_ ? i >= maximum_.GetInt64() : i > maximum_.GetInt64()) { + context.error_handler.AboveMaximum(i, maximum_, exclusiveMaximum_); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetMaximumString()); + } + } + else if (maximum_.IsUint64()) { } + /* do nothing */ // i <= max(int64_t) < maximum_.GetUint64() + else if (!CheckDoubleMaximum(context, static_cast(i))) + return false; + } + + if (!multipleOf_.IsNull()) { + if (multipleOf_.IsUint64()) { + if (static_cast(i >= 0 ? i : -i) % multipleOf_.GetUint64() != 0) { + context.error_handler.NotMultipleOf(i, multipleOf_); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetMultipleOfString()); + } + } + else if (!CheckDoubleMultipleOf(context, static_cast(i))) + return false; + } + + return true; + } + + bool CheckUint(Context& context, uint64_t i) const { + if (!(type_ & ((1 << kIntegerSchemaType) | (1 << kNumberSchemaType)))) { + DisallowedType(context, GetIntegerString()); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetTypeString()); + } + + if (!minimum_.IsNull()) { + if (minimum_.IsUint64()) { + if (exclusiveMinimum_ ? i <= minimum_.GetUint64() : i < minimum_.GetUint64()) { + context.error_handler.BelowMinimum(i, minimum_, exclusiveMinimum_); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetMinimumString()); + } + } + else if (minimum_.IsInt64()) + /* do nothing */; // i >= 0 > minimum.Getint64() + else if (!CheckDoubleMinimum(context, static_cast(i))) + return false; + } + + if (!maximum_.IsNull()) { + if (maximum_.IsUint64()) { + if (exclusiveMaximum_ ? i >= maximum_.GetUint64() : i > maximum_.GetUint64()) { + context.error_handler.AboveMaximum(i, maximum_, exclusiveMaximum_); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetMaximumString()); + } + } + else if (maximum_.IsInt64()) { + context.error_handler.AboveMaximum(i, maximum_, exclusiveMaximum_); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetMaximumString()); // i >= 0 > maximum_ + } + else if (!CheckDoubleMaximum(context, static_cast(i))) + return false; + } + + if (!multipleOf_.IsNull()) { + if (multipleOf_.IsUint64()) { + if (i % multipleOf_.GetUint64() != 0) { + context.error_handler.NotMultipleOf(i, multipleOf_); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetMultipleOfString()); + } + } + else if (!CheckDoubleMultipleOf(context, static_cast(i))) + return false; + } + + return true; + } + + bool CheckDoubleMinimum(Context& context, double d) const { + if (exclusiveMinimum_ ? d <= minimum_.GetDouble() : d < minimum_.GetDouble()) { + context.error_handler.BelowMinimum(d, minimum_, exclusiveMinimum_); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetMinimumString()); + } + return true; + } + + bool CheckDoubleMaximum(Context& context, double d) const { + if (exclusiveMaximum_ ? d >= maximum_.GetDouble() : d > maximum_.GetDouble()) { + context.error_handler.AboveMaximum(d, maximum_, exclusiveMaximum_); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetMaximumString()); + } + return true; + } + + bool CheckDoubleMultipleOf(Context& context, double d) const { + double a = std::abs(d), b = std::abs(multipleOf_.GetDouble()); + double q = std::floor(a / b); + double r = a - q * b; + if (r > 0.0) { + context.error_handler.NotMultipleOf(d, multipleOf_); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetMultipleOfString()); + } + return true; + } + + void DisallowedType(Context& context, const ValueType& actualType) const { + ErrorHandler& eh = context.error_handler; + eh.StartDisallowedType(); + + if (type_ & (1 << kNullSchemaType)) eh.AddExpectedType(GetNullString()); + if (type_ & (1 << kBooleanSchemaType)) eh.AddExpectedType(GetBooleanString()); + if (type_ & (1 << kObjectSchemaType)) eh.AddExpectedType(GetObjectString()); + if (type_ & (1 << kArraySchemaType)) eh.AddExpectedType(GetArrayString()); + if (type_ & (1 << kStringSchemaType)) eh.AddExpectedType(GetStringString()); + + if (type_ & (1 << kNumberSchemaType)) eh.AddExpectedType(GetNumberString()); + else if (type_ & (1 << kIntegerSchemaType)) eh.AddExpectedType(GetIntegerString()); + + eh.EndDisallowedType(actualType); + } + + struct Property { + Property() : schema(), dependenciesSchema(), dependenciesValidatorIndex(), dependencies(), required(false) {} + ~Property() { AllocatorType::Free(dependencies); } + SValue name; + const SchemaType* schema; + const SchemaType* dependenciesSchema; + SizeType dependenciesValidatorIndex; + bool* dependencies; + bool required; + }; + + struct PatternProperty { + PatternProperty() : schema(), pattern() {} + ~PatternProperty() { + if (pattern) { + pattern->~RegexType(); + AllocatorType::Free(pattern); + } + } + const SchemaType* schema; + RegexType* pattern; + }; + + AllocatorType* allocator_; + SValue uri_; + PointerType pointer_; + const SchemaType* typeless_; + uint64_t* enum_; + SizeType enumCount_; + SchemaArray allOf_; + SchemaArray anyOf_; + SchemaArray oneOf_; + const SchemaType* not_; + unsigned type_; // bitmask of kSchemaType + SizeType validatorCount_; + SizeType notValidatorIndex_; + + Property* properties_; + const SchemaType* additionalPropertiesSchema_; + PatternProperty* patternProperties_; + SizeType patternPropertyCount_; + SizeType propertyCount_; + SizeType minProperties_; + SizeType maxProperties_; + bool additionalProperties_; + bool hasDependencies_; + bool hasRequired_; + bool hasSchemaDependencies_; + + const SchemaType* additionalItemsSchema_; + const SchemaType* itemsList_; + const SchemaType** itemsTuple_; + SizeType itemsTupleCount_; + SizeType minItems_; + SizeType maxItems_; + bool additionalItems_; + bool uniqueItems_; + + RegexType* pattern_; + SizeType minLength_; + SizeType maxLength_; + + SValue minimum_; + SValue maximum_; + SValue multipleOf_; + bool exclusiveMinimum_; + bool exclusiveMaximum_; + + SizeType defaultValueLength_; +}; + +template +struct TokenHelper { + RAPIDJSON_FORCEINLINE static void AppendIndexToken(Stack& documentStack, SizeType index) { + *documentStack.template Push() = '/'; + char buffer[21]; + size_t length = static_cast((sizeof(SizeType) == 4 ? u32toa(index, buffer) : u64toa(index, buffer)) - buffer); + for (size_t i = 0; i < length; i++) + *documentStack.template Push() = static_cast(buffer[i]); + } +}; + +// Partial specialized version for char to prevent buffer copying. +template +struct TokenHelper { + RAPIDJSON_FORCEINLINE static void AppendIndexToken(Stack& documentStack, SizeType index) { + if (sizeof(SizeType) == 4) { + char *buffer = documentStack.template Push(1 + 10); // '/' + uint + *buffer++ = '/'; + const char* end = internal::u32toa(index, buffer); + documentStack.template Pop(static_cast(10 - (end - buffer))); + } + else { + char *buffer = documentStack.template Push(1 + 20); // '/' + uint64 + *buffer++ = '/'; + const char* end = internal::u64toa(index, buffer); + documentStack.template Pop(static_cast(20 - (end - buffer))); + } + } +}; + +} // namespace internal + +/////////////////////////////////////////////////////////////////////////////// +// IGenericRemoteSchemaDocumentProvider + +template +class IGenericRemoteSchemaDocumentProvider { +public: + typedef typename SchemaDocumentType::Ch Ch; + + virtual ~IGenericRemoteSchemaDocumentProvider() {} + virtual const SchemaDocumentType* GetRemoteDocument(const Ch* uri, SizeType length) = 0; +}; + +/////////////////////////////////////////////////////////////////////////////// +// GenericSchemaDocument + +//! JSON schema document. +/*! + A JSON schema document is a compiled version of a JSON schema. + It is basically a tree of internal::Schema. + + \note This is an immutable class (i.e. its instance cannot be modified after construction). + \tparam ValueT Type of JSON value (e.g. \c Value ), which also determine the encoding. + \tparam Allocator Allocator type for allocating memory of this document. +*/ +template +class GenericSchemaDocument { +public: + typedef ValueT ValueType; + typedef IGenericRemoteSchemaDocumentProvider IRemoteSchemaDocumentProviderType; + typedef Allocator AllocatorType; + typedef typename ValueType::EncodingType EncodingType; + typedef typename EncodingType::Ch Ch; + typedef internal::Schema SchemaType; + typedef GenericPointer PointerType; + typedef GenericValue URIType; + friend class internal::Schema; + template + friend class GenericSchemaValidator; + + //! Constructor. + /*! + Compile a JSON document into schema document. + + \param document A JSON document as source. + \param uri The base URI of this schema document for purposes of violation reporting. + \param uriLength Length of \c name, in code points. + \param remoteProvider An optional remote schema document provider for resolving remote reference. Can be null. + \param allocator An optional allocator instance for allocating memory. Can be null. + */ + explicit GenericSchemaDocument(const ValueType& document, const Ch* uri = 0, SizeType uriLength = 0, + IRemoteSchemaDocumentProviderType* remoteProvider = 0, Allocator* allocator = 0) : + remoteProvider_(remoteProvider), + allocator_(allocator), + ownAllocator_(), + root_(), + typeless_(), + schemaMap_(allocator, kInitialSchemaMapSize), + schemaRef_(allocator, kInitialSchemaRefSize) + { + if (!allocator_) + ownAllocator_ = allocator_ = RAPIDJSON_NEW(Allocator)(); + + Ch noUri[1] = {0}; + uri_.SetString(uri ? uri : noUri, uriLength, *allocator_); + + typeless_ = static_cast(allocator_->Malloc(sizeof(SchemaType))); + new (typeless_) SchemaType(this, PointerType(), ValueType(kObjectType).Move(), ValueType(kObjectType).Move(), allocator_); + + // Generate root schema, it will call CreateSchema() to create sub-schemas, + // And call AddRefSchema() if there are $ref. + CreateSchemaRecursive(&root_, PointerType(), document, document); + + // Resolve $ref + while (!schemaRef_.Empty()) { + SchemaRefEntry* refEntry = schemaRef_.template Pop(1); + if (const SchemaType* s = GetSchema(refEntry->target)) { + if (refEntry->schema) + *refEntry->schema = s; + + // Create entry in map if not exist + if (!GetSchema(refEntry->source)) { + new (schemaMap_.template Push()) SchemaEntry(refEntry->source, const_cast(s), false, allocator_); + } + } + else if (refEntry->schema) + *refEntry->schema = typeless_; + + refEntry->~SchemaRefEntry(); + } + + RAPIDJSON_ASSERT(root_ != 0); + + schemaRef_.ShrinkToFit(); // Deallocate all memory for ref + } + +#if RAPIDJSON_HAS_CXX11_RVALUE_REFS + //! Move constructor in C++11 + GenericSchemaDocument(GenericSchemaDocument&& rhs) RAPIDJSON_NOEXCEPT : + remoteProvider_(rhs.remoteProvider_), + allocator_(rhs.allocator_), + ownAllocator_(rhs.ownAllocator_), + root_(rhs.root_), + typeless_(rhs.typeless_), + schemaMap_(std::move(rhs.schemaMap_)), + schemaRef_(std::move(rhs.schemaRef_)), + uri_(std::move(rhs.uri_)) + { + rhs.remoteProvider_ = 0; + rhs.allocator_ = 0; + rhs.ownAllocator_ = 0; + rhs.typeless_ = 0; + } +#endif + + //! Destructor + ~GenericSchemaDocument() { + while (!schemaMap_.Empty()) + schemaMap_.template Pop(1)->~SchemaEntry(); + + if (typeless_) { + typeless_->~SchemaType(); + Allocator::Free(typeless_); + } + + RAPIDJSON_DELETE(ownAllocator_); + } + + const URIType& GetURI() const { return uri_; } + + //! Get the root schema. + const SchemaType& GetRoot() const { return *root_; } + +private: + //! Prohibit copying + GenericSchemaDocument(const GenericSchemaDocument&); + //! Prohibit assignment + GenericSchemaDocument& operator=(const GenericSchemaDocument&); + + struct SchemaRefEntry { + SchemaRefEntry(const PointerType& s, const PointerType& t, const SchemaType** outSchema, Allocator *allocator) : source(s, allocator), target(t, allocator), schema(outSchema) {} + PointerType source; + PointerType target; + const SchemaType** schema; + }; + + struct SchemaEntry { + SchemaEntry(const PointerType& p, SchemaType* s, bool o, Allocator* allocator) : pointer(p, allocator), schema(s), owned(o) {} + ~SchemaEntry() { + if (owned) { + schema->~SchemaType(); + Allocator::Free(schema); + } + } + PointerType pointer; + SchemaType* schema; + bool owned; + }; + + void CreateSchemaRecursive(const SchemaType** schema, const PointerType& pointer, const ValueType& v, const ValueType& document) { + if (schema) + *schema = typeless_; + + if (v.GetType() == kObjectType) { + const SchemaType* s = GetSchema(pointer); + if (!s) + CreateSchema(schema, pointer, v, document); + + for (typename ValueType::ConstMemberIterator itr = v.MemberBegin(); itr != v.MemberEnd(); ++itr) + CreateSchemaRecursive(0, pointer.Append(itr->name, allocator_), itr->value, document); + } + else if (v.GetType() == kArrayType) + for (SizeType i = 0; i < v.Size(); i++) + CreateSchemaRecursive(0, pointer.Append(i, allocator_), v[i], document); + } + + void CreateSchema(const SchemaType** schema, const PointerType& pointer, const ValueType& v, const ValueType& document) { + RAPIDJSON_ASSERT(pointer.IsValid()); + if (v.IsObject()) { + if (!HandleRefSchema(pointer, schema, v, document)) { + SchemaType* s = new (allocator_->Malloc(sizeof(SchemaType))) SchemaType(this, pointer, v, document, allocator_); + new (schemaMap_.template Push()) SchemaEntry(pointer, s, true, allocator_); + if (schema) + *schema = s; + } + } + } + + bool HandleRefSchema(const PointerType& source, const SchemaType** schema, const ValueType& v, const ValueType& document) { + static const Ch kRefString[] = { '$', 'r', 'e', 'f', '\0' }; + static const ValueType kRefValue(kRefString, 4); + + typename ValueType::ConstMemberIterator itr = v.FindMember(kRefValue); + if (itr == v.MemberEnd()) + return false; + + if (itr->value.IsString()) { + SizeType len = itr->value.GetStringLength(); + if (len > 0) { + const Ch* s = itr->value.GetString(); + SizeType i = 0; + while (i < len && s[i] != '#') // Find the first # + i++; + + if (i > 0) { // Remote reference, resolve immediately + if (remoteProvider_) { + if (const GenericSchemaDocument* remoteDocument = remoteProvider_->GetRemoteDocument(s, i)) { + PointerType pointer(&s[i], len - i, allocator_); + if (pointer.IsValid()) { + if (const SchemaType* sc = remoteDocument->GetSchema(pointer)) { + if (schema) + *schema = sc; + new (schemaMap_.template Push()) SchemaEntry(source, const_cast(sc), false, allocator_); + return true; + } + } + } + } + } + else if (s[i] == '#') { // Local reference, defer resolution + PointerType pointer(&s[i], len - i, allocator_); + if (pointer.IsValid()) { + if (const ValueType* nv = pointer.Get(document)) + if (HandleRefSchema(source, schema, *nv, document)) + return true; + + new (schemaRef_.template Push()) SchemaRefEntry(source, pointer, schema, allocator_); + return true; + } + } + } + } + return false; + } + + const SchemaType* GetSchema(const PointerType& pointer) const { + for (const SchemaEntry* target = schemaMap_.template Bottom(); target != schemaMap_.template End(); ++target) + if (pointer == target->pointer) + return target->schema; + return 0; + } + + PointerType GetPointer(const SchemaType* schema) const { + for (const SchemaEntry* target = schemaMap_.template Bottom(); target != schemaMap_.template End(); ++target) + if (schema == target->schema) + return target->pointer; + return PointerType(); + } + + const SchemaType* GetTypeless() const { return typeless_; } + + static const size_t kInitialSchemaMapSize = 64; + static const size_t kInitialSchemaRefSize = 64; + + IRemoteSchemaDocumentProviderType* remoteProvider_; + Allocator *allocator_; + Allocator *ownAllocator_; + const SchemaType* root_; //!< Root schema. + SchemaType* typeless_; + internal::Stack schemaMap_; // Stores created Pointer -> Schemas + internal::Stack schemaRef_; // Stores Pointer from $ref and schema which holds the $ref + URIType uri_; +}; + +//! GenericSchemaDocument using Value type. +typedef GenericSchemaDocument SchemaDocument; +//! IGenericRemoteSchemaDocumentProvider using SchemaDocument. +typedef IGenericRemoteSchemaDocumentProvider IRemoteSchemaDocumentProvider; + +/////////////////////////////////////////////////////////////////////////////// +// GenericSchemaValidator + +//! JSON Schema Validator. +/*! + A SAX style JSON schema validator. + It uses a \c GenericSchemaDocument to validate SAX events. + It delegates the incoming SAX events to an output handler. + The default output handler does nothing. + It can be reused multiple times by calling \c Reset(). + + \tparam SchemaDocumentType Type of schema document. + \tparam OutputHandler Type of output handler. Default handler does nothing. + \tparam StateAllocator Allocator for storing the internal validation states. +*/ +template < + typename SchemaDocumentType, + typename OutputHandler = BaseReaderHandler, + typename StateAllocator = CrtAllocator> +class GenericSchemaValidator : + public internal::ISchemaStateFactory, + public internal::ISchemaValidator, + public internal::IValidationErrorHandler +{ +public: + typedef typename SchemaDocumentType::SchemaType SchemaType; + typedef typename SchemaDocumentType::PointerType PointerType; + typedef typename SchemaType::EncodingType EncodingType; + typedef typename SchemaType::SValue SValue; + typedef typename EncodingType::Ch Ch; + typedef GenericStringRef StringRefType; + typedef GenericValue ValueType; + + //! Constructor without output handler. + /*! + \param schemaDocument The schema document to conform to. + \param allocator Optional allocator for storing internal validation states. + \param schemaStackCapacity Optional initial capacity of schema path stack. + \param documentStackCapacity Optional initial capacity of document path stack. + */ + GenericSchemaValidator( + const SchemaDocumentType& schemaDocument, + StateAllocator* allocator = 0, + size_t schemaStackCapacity = kDefaultSchemaStackCapacity, + size_t documentStackCapacity = kDefaultDocumentStackCapacity) + : + schemaDocument_(&schemaDocument), + root_(schemaDocument.GetRoot()), + stateAllocator_(allocator), + ownStateAllocator_(0), + schemaStack_(allocator, schemaStackCapacity), + documentStack_(allocator, documentStackCapacity), + outputHandler_(0), + error_(kObjectType), + currentError_(), + missingDependents_(), + valid_(true) +#if RAPIDJSON_SCHEMA_VERBOSE + , depth_(0) +#endif + { + } + + //! Constructor with output handler. + /*! + \param schemaDocument The schema document to conform to. + \param allocator Optional allocator for storing internal validation states. + \param schemaStackCapacity Optional initial capacity of schema path stack. + \param documentStackCapacity Optional initial capacity of document path stack. + */ + GenericSchemaValidator( + const SchemaDocumentType& schemaDocument, + OutputHandler& outputHandler, + StateAllocator* allocator = 0, + size_t schemaStackCapacity = kDefaultSchemaStackCapacity, + size_t documentStackCapacity = kDefaultDocumentStackCapacity) + : + schemaDocument_(&schemaDocument), + root_(schemaDocument.GetRoot()), + stateAllocator_(allocator), + ownStateAllocator_(0), + schemaStack_(allocator, schemaStackCapacity), + documentStack_(allocator, documentStackCapacity), + outputHandler_(&outputHandler), + error_(kObjectType), + currentError_(), + missingDependents_(), + valid_(true) +#if RAPIDJSON_SCHEMA_VERBOSE + , depth_(0) +#endif + { + } + + //! Destructor. + ~GenericSchemaValidator() { + Reset(); + RAPIDJSON_DELETE(ownStateAllocator_); + } + + //! Reset the internal states. + void Reset() { + while (!schemaStack_.Empty()) + PopSchema(); + documentStack_.Clear(); + error_.SetObject(); + currentError_.SetNull(); + missingDependents_.SetNull(); + valid_ = true; + } + + //! Checks whether the current state is valid. + // Implementation of ISchemaValidator + virtual bool IsValid() const { return valid_; } + + //! Gets the error object. + ValueType& GetError() { return error_; } + const ValueType& GetError() const { return error_; } + + //! Gets the JSON pointer pointed to the invalid schema. + PointerType GetInvalidSchemaPointer() const { + return schemaStack_.Empty() ? PointerType() : CurrentSchema().GetPointer(); + } + + //! Gets the keyword of invalid schema. + const Ch* GetInvalidSchemaKeyword() const { + return schemaStack_.Empty() ? 0 : CurrentContext().invalidKeyword; + } + + //! Gets the JSON pointer pointed to the invalid value. + PointerType GetInvalidDocumentPointer() const { + if (documentStack_.Empty()) { + return PointerType(); + } + else { + return PointerType(documentStack_.template Bottom(), documentStack_.GetSize() / sizeof(Ch)); + } + } + + void NotMultipleOf(int64_t actual, const SValue& expected) { + AddNumberError(SchemaType::GetMultipleOfString(), ValueType(actual).Move(), expected); + } + void NotMultipleOf(uint64_t actual, const SValue& expected) { + AddNumberError(SchemaType::GetMultipleOfString(), ValueType(actual).Move(), expected); + } + void NotMultipleOf(double actual, const SValue& expected) { + AddNumberError(SchemaType::GetMultipleOfString(), ValueType(actual).Move(), expected); + } + void AboveMaximum(int64_t actual, const SValue& expected, bool exclusive) { + AddNumberError(SchemaType::GetMaximumString(), ValueType(actual).Move(), expected, + exclusive ? &SchemaType::GetExclusiveMaximumString : 0); + } + void AboveMaximum(uint64_t actual, const SValue& expected, bool exclusive) { + AddNumberError(SchemaType::GetMaximumString(), ValueType(actual).Move(), expected, + exclusive ? &SchemaType::GetExclusiveMaximumString : 0); + } + void AboveMaximum(double actual, const SValue& expected, bool exclusive) { + AddNumberError(SchemaType::GetMaximumString(), ValueType(actual).Move(), expected, + exclusive ? &SchemaType::GetExclusiveMaximumString : 0); + } + void BelowMinimum(int64_t actual, const SValue& expected, bool exclusive) { + AddNumberError(SchemaType::GetMinimumString(), ValueType(actual).Move(), expected, + exclusive ? &SchemaType::GetExclusiveMinimumString : 0); + } + void BelowMinimum(uint64_t actual, const SValue& expected, bool exclusive) { + AddNumberError(SchemaType::GetMinimumString(), ValueType(actual).Move(), expected, + exclusive ? &SchemaType::GetExclusiveMinimumString : 0); + } + void BelowMinimum(double actual, const SValue& expected, bool exclusive) { + AddNumberError(SchemaType::GetMinimumString(), ValueType(actual).Move(), expected, + exclusive ? &SchemaType::GetExclusiveMinimumString : 0); + } + + void TooLong(const Ch* str, SizeType length, SizeType expected) { + AddNumberError(SchemaType::GetMaxLengthString(), + ValueType(str, length, GetStateAllocator()).Move(), SValue(expected).Move()); + } + void TooShort(const Ch* str, SizeType length, SizeType expected) { + AddNumberError(SchemaType::GetMinLengthString(), + ValueType(str, length, GetStateAllocator()).Move(), SValue(expected).Move()); + } + void DoesNotMatch(const Ch* str, SizeType length) { + currentError_.SetObject(); + currentError_.AddMember(GetActualString(), ValueType(str, length, GetStateAllocator()).Move(), GetStateAllocator()); + AddCurrentError(SchemaType::GetPatternString()); + } + + void DisallowedItem(SizeType index) { + currentError_.SetObject(); + currentError_.AddMember(GetDisallowedString(), ValueType(index).Move(), GetStateAllocator()); + AddCurrentError(SchemaType::GetAdditionalItemsString(), true); + } + void TooFewItems(SizeType actualCount, SizeType expectedCount) { + AddNumberError(SchemaType::GetMinItemsString(), + ValueType(actualCount).Move(), SValue(expectedCount).Move()); + } + void TooManyItems(SizeType actualCount, SizeType expectedCount) { + AddNumberError(SchemaType::GetMaxItemsString(), + ValueType(actualCount).Move(), SValue(expectedCount).Move()); + } + void DuplicateItems(SizeType index1, SizeType index2) { + ValueType duplicates(kArrayType); + duplicates.PushBack(index1, GetStateAllocator()); + duplicates.PushBack(index2, GetStateAllocator()); + currentError_.SetObject(); + currentError_.AddMember(GetDuplicatesString(), duplicates, GetStateAllocator()); + AddCurrentError(SchemaType::GetUniqueItemsString(), true); + } + + void TooManyProperties(SizeType actualCount, SizeType expectedCount) { + AddNumberError(SchemaType::GetMaxPropertiesString(), + ValueType(actualCount).Move(), SValue(expectedCount).Move()); + } + void TooFewProperties(SizeType actualCount, SizeType expectedCount) { + AddNumberError(SchemaType::GetMinPropertiesString(), + ValueType(actualCount).Move(), SValue(expectedCount).Move()); + } + void StartMissingProperties() { + currentError_.SetArray(); + } + void AddMissingProperty(const SValue& name) { + currentError_.PushBack(ValueType(name, GetStateAllocator()).Move(), GetStateAllocator()); + } + bool EndMissingProperties() { + if (currentError_.Empty()) + return false; + ValueType error(kObjectType); + error.AddMember(GetMissingString(), currentError_, GetStateAllocator()); + currentError_ = error; + AddCurrentError(SchemaType::GetRequiredString()); + return true; + } + void PropertyViolations(ISchemaValidator** subvalidators, SizeType count) { + for (SizeType i = 0; i < count; ++i) + MergeError(static_cast(subvalidators[i])->GetError()); + } + void DisallowedProperty(const Ch* name, SizeType length) { + currentError_.SetObject(); + currentError_.AddMember(GetDisallowedString(), ValueType(name, length, GetStateAllocator()).Move(), GetStateAllocator()); + AddCurrentError(SchemaType::GetAdditionalPropertiesString(), true); + } + + void StartDependencyErrors() { + currentError_.SetObject(); + } + void StartMissingDependentProperties() { + missingDependents_.SetArray(); + } + void AddMissingDependentProperty(const SValue& targetName) { + missingDependents_.PushBack(ValueType(targetName, GetStateAllocator()).Move(), GetStateAllocator()); + } + void EndMissingDependentProperties(const SValue& sourceName) { + if (!missingDependents_.Empty()) + currentError_.AddMember(ValueType(sourceName, GetStateAllocator()).Move(), + missingDependents_, GetStateAllocator()); + } + void AddDependencySchemaError(const SValue& sourceName, ISchemaValidator* subvalidator) { + currentError_.AddMember(ValueType(sourceName, GetStateAllocator()).Move(), + static_cast(subvalidator)->GetError(), GetStateAllocator()); + } + bool EndDependencyErrors() { + if (currentError_.ObjectEmpty()) + return false; + ValueType error(kObjectType); + error.AddMember(GetErrorsString(), currentError_, GetStateAllocator()); + currentError_ = error; + AddCurrentError(SchemaType::GetDependenciesString()); + return true; + } + + void DisallowedValue() { + currentError_.SetObject(); + AddCurrentError(SchemaType::GetEnumString()); + } + void StartDisallowedType() { + currentError_.SetArray(); + } + void AddExpectedType(const typename SchemaType::ValueType& expectedType) { + currentError_.PushBack(ValueType(expectedType, GetStateAllocator()).Move(), GetStateAllocator()); + } + void EndDisallowedType(const typename SchemaType::ValueType& actualType) { + ValueType error(kObjectType); + error.AddMember(GetExpectedString(), currentError_, GetStateAllocator()); + error.AddMember(GetActualString(), ValueType(actualType, GetStateAllocator()).Move(), GetStateAllocator()); + currentError_ = error; + AddCurrentError(SchemaType::GetTypeString()); + } + void NotAllOf(ISchemaValidator** subvalidators, SizeType count) { + for (SizeType i = 0; i < count; ++i) { + MergeError(static_cast(subvalidators[i])->GetError()); + } + } + void NoneOf(ISchemaValidator** subvalidators, SizeType count) { + AddErrorArray(SchemaType::GetAnyOfString(), subvalidators, count); + } + void NotOneOf(ISchemaValidator** subvalidators, SizeType count) { + AddErrorArray(SchemaType::GetOneOfString(), subvalidators, count); + } + void Disallowed() { + currentError_.SetObject(); + AddCurrentError(SchemaType::GetNotString()); + } + +#define RAPIDJSON_STRING_(name, ...) \ + static const StringRefType& Get##name##String() {\ + static const Ch s[] = { __VA_ARGS__, '\0' };\ + static const StringRefType v(s, static_cast(sizeof(s) / sizeof(Ch) - 1)); \ + return v;\ + } + + RAPIDJSON_STRING_(InstanceRef, 'i', 'n', 's', 't', 'a', 'n', 'c', 'e', 'R', 'e', 'f') + RAPIDJSON_STRING_(SchemaRef, 's', 'c', 'h', 'e', 'm', 'a', 'R', 'e', 'f') + RAPIDJSON_STRING_(Expected, 'e', 'x', 'p', 'e', 'c', 't', 'e', 'd') + RAPIDJSON_STRING_(Actual, 'a', 'c', 't', 'u', 'a', 'l') + RAPIDJSON_STRING_(Disallowed, 'd', 'i', 's', 'a', 'l', 'l', 'o', 'w', 'e', 'd') + RAPIDJSON_STRING_(Missing, 'm', 'i', 's', 's', 'i', 'n', 'g') + RAPIDJSON_STRING_(Errors, 'e', 'r', 'r', 'o', 'r', 's') + RAPIDJSON_STRING_(Duplicates, 'd', 'u', 'p', 'l', 'i', 'c', 'a', 't', 'e', 's') + +#undef RAPIDJSON_STRING_ + +#if RAPIDJSON_SCHEMA_VERBOSE +#define RAPIDJSON_SCHEMA_HANDLE_BEGIN_VERBOSE_() \ +RAPIDJSON_MULTILINEMACRO_BEGIN\ + *documentStack_.template Push() = '\0';\ + documentStack_.template Pop(1);\ + internal::PrintInvalidDocument(documentStack_.template Bottom());\ +RAPIDJSON_MULTILINEMACRO_END +#else +#define RAPIDJSON_SCHEMA_HANDLE_BEGIN_VERBOSE_() +#endif + +#define RAPIDJSON_SCHEMA_HANDLE_BEGIN_(method, arg1)\ + if (!valid_) return false; \ + if (!BeginValue() || !CurrentSchema().method arg1) {\ + RAPIDJSON_SCHEMA_HANDLE_BEGIN_VERBOSE_();\ + return valid_ = false;\ + } + +#define RAPIDJSON_SCHEMA_HANDLE_PARALLEL_(method, arg2)\ + for (Context* context = schemaStack_.template Bottom(); context != schemaStack_.template End(); context++) {\ + if (context->hasher)\ + static_cast(context->hasher)->method arg2;\ + if (context->validators)\ + for (SizeType i_ = 0; i_ < context->validatorCount; i_++)\ + static_cast(context->validators[i_])->method arg2;\ + if (context->patternPropertiesValidators)\ + for (SizeType i_ = 0; i_ < context->patternPropertiesValidatorCount; i_++)\ + static_cast(context->patternPropertiesValidators[i_])->method arg2;\ + } + +#define RAPIDJSON_SCHEMA_HANDLE_END_(method, arg2)\ + return valid_ = EndValue() && (!outputHandler_ || outputHandler_->method arg2) + +#define RAPIDJSON_SCHEMA_HANDLE_VALUE_(method, arg1, arg2) \ + RAPIDJSON_SCHEMA_HANDLE_BEGIN_ (method, arg1);\ + RAPIDJSON_SCHEMA_HANDLE_PARALLEL_(method, arg2);\ + RAPIDJSON_SCHEMA_HANDLE_END_ (method, arg2) + + bool Null() { RAPIDJSON_SCHEMA_HANDLE_VALUE_(Null, (CurrentContext()), ( )); } + bool Bool(bool b) { RAPIDJSON_SCHEMA_HANDLE_VALUE_(Bool, (CurrentContext(), b), (b)); } + bool Int(int i) { RAPIDJSON_SCHEMA_HANDLE_VALUE_(Int, (CurrentContext(), i), (i)); } + bool Uint(unsigned u) { RAPIDJSON_SCHEMA_HANDLE_VALUE_(Uint, (CurrentContext(), u), (u)); } + bool Int64(int64_t i) { RAPIDJSON_SCHEMA_HANDLE_VALUE_(Int64, (CurrentContext(), i), (i)); } + bool Uint64(uint64_t u) { RAPIDJSON_SCHEMA_HANDLE_VALUE_(Uint64, (CurrentContext(), u), (u)); } + bool Double(double d) { RAPIDJSON_SCHEMA_HANDLE_VALUE_(Double, (CurrentContext(), d), (d)); } + bool RawNumber(const Ch* str, SizeType length, bool copy) + { RAPIDJSON_SCHEMA_HANDLE_VALUE_(String, (CurrentContext(), str, length, copy), (str, length, copy)); } + bool String(const Ch* str, SizeType length, bool copy) + { RAPIDJSON_SCHEMA_HANDLE_VALUE_(String, (CurrentContext(), str, length, copy), (str, length, copy)); } + + bool StartObject() { + RAPIDJSON_SCHEMA_HANDLE_BEGIN_(StartObject, (CurrentContext())); + RAPIDJSON_SCHEMA_HANDLE_PARALLEL_(StartObject, ()); + return valid_ = !outputHandler_ || outputHandler_->StartObject(); + } + + bool Key(const Ch* str, SizeType len, bool copy) { + if (!valid_) return false; + AppendToken(str, len); + if (!CurrentSchema().Key(CurrentContext(), str, len, copy)) return valid_ = false; + RAPIDJSON_SCHEMA_HANDLE_PARALLEL_(Key, (str, len, copy)); + return valid_ = !outputHandler_ || outputHandler_->Key(str, len, copy); + } + + bool EndObject(SizeType memberCount) { + if (!valid_) return false; + RAPIDJSON_SCHEMA_HANDLE_PARALLEL_(EndObject, (memberCount)); + if (!CurrentSchema().EndObject(CurrentContext(), memberCount)) return valid_ = false; + RAPIDJSON_SCHEMA_HANDLE_END_(EndObject, (memberCount)); + } + + bool StartArray() { + RAPIDJSON_SCHEMA_HANDLE_BEGIN_(StartArray, (CurrentContext())); + RAPIDJSON_SCHEMA_HANDLE_PARALLEL_(StartArray, ()); + return valid_ = !outputHandler_ || outputHandler_->StartArray(); + } + + bool EndArray(SizeType elementCount) { + if (!valid_) return false; + RAPIDJSON_SCHEMA_HANDLE_PARALLEL_(EndArray, (elementCount)); + if (!CurrentSchema().EndArray(CurrentContext(), elementCount)) return valid_ = false; + RAPIDJSON_SCHEMA_HANDLE_END_(EndArray, (elementCount)); + } + +#undef RAPIDJSON_SCHEMA_HANDLE_BEGIN_VERBOSE_ +#undef RAPIDJSON_SCHEMA_HANDLE_BEGIN_ +#undef RAPIDJSON_SCHEMA_HANDLE_PARALLEL_ +#undef RAPIDJSON_SCHEMA_HANDLE_VALUE_ + + // Implementation of ISchemaStateFactory + virtual ISchemaValidator* CreateSchemaValidator(const SchemaType& root) { + return new (GetStateAllocator().Malloc(sizeof(GenericSchemaValidator))) GenericSchemaValidator(*schemaDocument_, root, documentStack_.template Bottom(), documentStack_.GetSize(), +#if RAPIDJSON_SCHEMA_VERBOSE + depth_ + 1, +#endif + &GetStateAllocator()); + } + + virtual void DestroySchemaValidator(ISchemaValidator* validator) { + GenericSchemaValidator* v = static_cast(validator); + v->~GenericSchemaValidator(); + StateAllocator::Free(v); + } + + virtual void* CreateHasher() { + return new (GetStateAllocator().Malloc(sizeof(HasherType))) HasherType(&GetStateAllocator()); + } + + virtual uint64_t GetHashCode(void* hasher) { + return static_cast(hasher)->GetHashCode(); + } + + virtual void DestroryHasher(void* hasher) { + HasherType* h = static_cast(hasher); + h->~HasherType(); + StateAllocator::Free(h); + } + + virtual void* MallocState(size_t size) { + return GetStateAllocator().Malloc(size); + } + + virtual void FreeState(void* p) { + StateAllocator::Free(p); + } + +private: + typedef typename SchemaType::Context Context; + typedef GenericValue, StateAllocator> HashCodeArray; + typedef internal::Hasher HasherType; + + GenericSchemaValidator( + const SchemaDocumentType& schemaDocument, + const SchemaType& root, + const char* basePath, size_t basePathSize, +#if RAPIDJSON_SCHEMA_VERBOSE + unsigned depth, +#endif + StateAllocator* allocator = 0, + size_t schemaStackCapacity = kDefaultSchemaStackCapacity, + size_t documentStackCapacity = kDefaultDocumentStackCapacity) + : + schemaDocument_(&schemaDocument), + root_(root), + stateAllocator_(allocator), + ownStateAllocator_(0), + schemaStack_(allocator, schemaStackCapacity), + documentStack_(allocator, documentStackCapacity), + outputHandler_(0), + error_(kObjectType), + currentError_(), + missingDependents_(), + valid_(true) +#if RAPIDJSON_SCHEMA_VERBOSE + , depth_(depth) +#endif + { + if (basePath && basePathSize) + memcpy(documentStack_.template Push(basePathSize), basePath, basePathSize); + } + + StateAllocator& GetStateAllocator() { + if (!stateAllocator_) + stateAllocator_ = ownStateAllocator_ = RAPIDJSON_NEW(StateAllocator)(); + return *stateAllocator_; + } + + bool BeginValue() { + if (schemaStack_.Empty()) + PushSchema(root_); + else { + if (CurrentContext().inArray) + internal::TokenHelper, Ch>::AppendIndexToken(documentStack_, CurrentContext().arrayElementIndex); + + if (!CurrentSchema().BeginValue(CurrentContext())) + return false; + + SizeType count = CurrentContext().patternPropertiesSchemaCount; + const SchemaType** sa = CurrentContext().patternPropertiesSchemas; + typename Context::PatternValidatorType patternValidatorType = CurrentContext().valuePatternValidatorType; + bool valueUniqueness = CurrentContext().valueUniqueness; + RAPIDJSON_ASSERT(CurrentContext().valueSchema); + PushSchema(*CurrentContext().valueSchema); + + if (count > 0) { + CurrentContext().objectPatternValidatorType = patternValidatorType; + ISchemaValidator**& va = CurrentContext().patternPropertiesValidators; + SizeType& validatorCount = CurrentContext().patternPropertiesValidatorCount; + va = static_cast(MallocState(sizeof(ISchemaValidator*) * count)); + for (SizeType i = 0; i < count; i++) + va[validatorCount++] = CreateSchemaValidator(*sa[i]); + } + + CurrentContext().arrayUniqueness = valueUniqueness; + } + return true; + } + + bool EndValue() { + if (!CurrentSchema().EndValue(CurrentContext())) + return false; + +#if RAPIDJSON_SCHEMA_VERBOSE + GenericStringBuffer sb; + schemaDocument_->GetPointer(&CurrentSchema()).Stringify(sb); + + *documentStack_.template Push() = '\0'; + documentStack_.template Pop(1); + internal::PrintValidatorPointers(depth_, sb.GetString(), documentStack_.template Bottom()); +#endif + + uint64_t h = CurrentContext().arrayUniqueness ? static_cast(CurrentContext().hasher)->GetHashCode() : 0; + + PopSchema(); + + if (!schemaStack_.Empty()) { + Context& context = CurrentContext(); + if (context.valueUniqueness) { + HashCodeArray* a = static_cast(context.arrayElementHashCodes); + if (!a) + CurrentContext().arrayElementHashCodes = a = new (GetStateAllocator().Malloc(sizeof(HashCodeArray))) HashCodeArray(kArrayType); + for (typename HashCodeArray::ConstValueIterator itr = a->Begin(); itr != a->End(); ++itr) + if (itr->GetUint64() == h) { + DuplicateItems(static_cast(itr - a->Begin()), a->Size()); + RAPIDJSON_INVALID_KEYWORD_RETURN(SchemaType::GetUniqueItemsString()); + } + a->PushBack(h, GetStateAllocator()); + } + } + + // Remove the last token of document pointer + while (!documentStack_.Empty() && *documentStack_.template Pop(1) != '/') + ; + + return true; + } + + void AppendToken(const Ch* str, SizeType len) { + documentStack_.template Reserve(1 + len * 2); // worst case all characters are escaped as two characters + *documentStack_.template PushUnsafe() = '/'; + for (SizeType i = 0; i < len; i++) { + if (str[i] == '~') { + *documentStack_.template PushUnsafe() = '~'; + *documentStack_.template PushUnsafe() = '0'; + } + else if (str[i] == '/') { + *documentStack_.template PushUnsafe() = '~'; + *documentStack_.template PushUnsafe() = '1'; + } + else + *documentStack_.template PushUnsafe() = str[i]; + } + } + + RAPIDJSON_FORCEINLINE void PushSchema(const SchemaType& schema) { new (schemaStack_.template Push()) Context(*this, *this, &schema); } + + RAPIDJSON_FORCEINLINE void PopSchema() { + Context* c = schemaStack_.template Pop(1); + if (HashCodeArray* a = static_cast(c->arrayElementHashCodes)) { + a->~HashCodeArray(); + StateAllocator::Free(a); + } + c->~Context(); + } + + void AddErrorLocation(ValueType& result, bool parent) { + GenericStringBuffer sb; + PointerType instancePointer = GetInvalidDocumentPointer(); + ((parent && instancePointer.GetTokenCount() > 0) + ? PointerType(instancePointer.GetTokens(), instancePointer.GetTokenCount() - 1) + : instancePointer).StringifyUriFragment(sb); + ValueType instanceRef(sb.GetString(), static_cast(sb.GetSize() / sizeof(Ch)), + GetStateAllocator()); + result.AddMember(GetInstanceRefString(), instanceRef, GetStateAllocator()); + sb.Clear(); + memcpy(sb.Push(CurrentSchema().GetURI().GetStringLength()), + CurrentSchema().GetURI().GetString(), + CurrentSchema().GetURI().GetStringLength() * sizeof(Ch)); + GetInvalidSchemaPointer().StringifyUriFragment(sb); + ValueType schemaRef(sb.GetString(), static_cast(sb.GetSize() / sizeof(Ch)), + GetStateAllocator()); + result.AddMember(GetSchemaRefString(), schemaRef, GetStateAllocator()); + } + + void AddError(ValueType& keyword, ValueType& error) { + typename ValueType::MemberIterator member = error_.FindMember(keyword); + if (member == error_.MemberEnd()) + error_.AddMember(keyword, error, GetStateAllocator()); + else { + if (member->value.IsObject()) { + ValueType errors(kArrayType); + errors.PushBack(member->value, GetStateAllocator()); + member->value = errors; + } + member->value.PushBack(error, GetStateAllocator()); + } + } + + void AddCurrentError(const typename SchemaType::ValueType& keyword, bool parent = false) { + AddErrorLocation(currentError_, parent); + AddError(ValueType(keyword, GetStateAllocator(), false).Move(), currentError_); + } + + void MergeError(ValueType& other) { + for (typename ValueType::MemberIterator it = other.MemberBegin(), end = other.MemberEnd(); it != end; ++it) { + AddError(it->name, it->value); + } + } + + void AddNumberError(const typename SchemaType::ValueType& keyword, ValueType& actual, const SValue& expected, + const typename SchemaType::ValueType& (*exclusive)() = 0) { + currentError_.SetObject(); + currentError_.AddMember(GetActualString(), actual, GetStateAllocator()); + currentError_.AddMember(GetExpectedString(), ValueType(expected, GetStateAllocator()).Move(), GetStateAllocator()); + if (exclusive) + currentError_.AddMember(ValueType(exclusive(), GetStateAllocator()).Move(), true, GetStateAllocator()); + AddCurrentError(keyword); + } + + void AddErrorArray(const typename SchemaType::ValueType& keyword, + ISchemaValidator** subvalidators, SizeType count) { + ValueType errors(kArrayType); + for (SizeType i = 0; i < count; ++i) + errors.PushBack(static_cast(subvalidators[i])->GetError(), GetStateAllocator()); + currentError_.SetObject(); + currentError_.AddMember(GetErrorsString(), errors, GetStateAllocator()); + AddCurrentError(keyword); + } + + const SchemaType& CurrentSchema() const { return *schemaStack_.template Top()->schema; } + Context& CurrentContext() { return *schemaStack_.template Top(); } + const Context& CurrentContext() const { return *schemaStack_.template Top(); } + + static const size_t kDefaultSchemaStackCapacity = 1024; + static const size_t kDefaultDocumentStackCapacity = 256; + const SchemaDocumentType* schemaDocument_; + const SchemaType& root_; + StateAllocator* stateAllocator_; + StateAllocator* ownStateAllocator_; + internal::Stack schemaStack_; //!< stack to store the current path of schema (BaseSchemaType *) + internal::Stack documentStack_; //!< stack to store the current path of validating document (Ch) + OutputHandler* outputHandler_; + ValueType error_; + ValueType currentError_; + ValueType missingDependents_; + bool valid_; +#if RAPIDJSON_SCHEMA_VERBOSE + unsigned depth_; +#endif +}; + +typedef GenericSchemaValidator SchemaValidator; + +/////////////////////////////////////////////////////////////////////////////// +// SchemaValidatingReader + +//! A helper class for parsing with validation. +/*! + This helper class is a functor, designed as a parameter of \ref GenericDocument::Populate(). + + \tparam parseFlags Combination of \ref ParseFlag. + \tparam InputStream Type of input stream, implementing Stream concept. + \tparam SourceEncoding Encoding of the input stream. + \tparam SchemaDocumentType Type of schema document. + \tparam StackAllocator Allocator type for stack. +*/ +template < + unsigned parseFlags, + typename InputStream, + typename SourceEncoding, + typename SchemaDocumentType = SchemaDocument, + typename StackAllocator = CrtAllocator> +class SchemaValidatingReader { +public: + typedef typename SchemaDocumentType::PointerType PointerType; + typedef typename InputStream::Ch Ch; + typedef GenericValue ValueType; + + //! Constructor + /*! + \param is Input stream. + \param sd Schema document. + */ + SchemaValidatingReader(InputStream& is, const SchemaDocumentType& sd) : is_(is), sd_(sd), invalidSchemaKeyword_(), error_(kObjectType), isValid_(true) {} + + template + bool operator()(Handler& handler) { + GenericReader reader; + GenericSchemaValidator validator(sd_, handler); + parseResult_ = reader.template Parse(is_, validator); + + isValid_ = validator.IsValid(); + if (isValid_) { + invalidSchemaPointer_ = PointerType(); + invalidSchemaKeyword_ = 0; + invalidDocumentPointer_ = PointerType(); + error_.SetObject(); + } + else { + invalidSchemaPointer_ = validator.GetInvalidSchemaPointer(); + invalidSchemaKeyword_ = validator.GetInvalidSchemaKeyword(); + invalidDocumentPointer_ = validator.GetInvalidDocumentPointer(); + error_.CopyFrom(validator.GetError(), allocator_); + } + + return parseResult_; + } + + const ParseResult& GetParseResult() const { return parseResult_; } + bool IsValid() const { return isValid_; } + const PointerType& GetInvalidSchemaPointer() const { return invalidSchemaPointer_; } + const Ch* GetInvalidSchemaKeyword() const { return invalidSchemaKeyword_; } + const PointerType& GetInvalidDocumentPointer() const { return invalidDocumentPointer_; } + const ValueType& GetError() const { return error_; } + +private: + InputStream& is_; + const SchemaDocumentType& sd_; + + ParseResult parseResult_; + PointerType invalidSchemaPointer_; + const Ch* invalidSchemaKeyword_; + PointerType invalidDocumentPointer_; + StackAllocator allocator_; + ValueType error_; + bool isValid_; +}; + +RAPIDJSON_NAMESPACE_END +RAPIDJSON_DIAG_POP + +#endif // RAPIDJSON_SCHEMA_H_ diff --git a/livox_ros_driver/common/rapidjson/stream.h b/livox_ros_driver/common/rapidjson/stream.h new file mode 100644 index 0000000..7f2643e --- /dev/null +++ b/livox_ros_driver/common/rapidjson/stream.h @@ -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 +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. + */ + enum { copyOptimization = 0 }; +}; + +//! Reserve n characters for writing to a stream. +template +inline void PutReserve(Stream& stream, size_t count) { + (void)stream; + (void)count; +} + +//! Write character to a stream, presuming buffer is reserved. +template +inline void PutUnsafe(Stream& stream, typename Stream::Ch c) { + stream.Put(c); +} + +//! Put N copies of a character to a stream. +template +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 > +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 +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(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 +struct StreamTraits > { + enum { copyOptimization = 1 }; +}; + +//! String stream with UTF8 encoding. +typedef GenericStringStream > StringStream; + +/////////////////////////////////////////////////////////////////////////////// +// InsituStringStream + +//! A read-write string stream. +/*! This string stream is particularly designed for in-situ parsing. + \note implements Stream concept +*/ +template +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(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(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 +struct StreamTraits > { + enum { copyOptimization = 1 }; +}; + +//! Insitu string stream with UTF8 encoding. +typedef GenericInsituStringStream > InsituStringStream; + +RAPIDJSON_NAMESPACE_END + +#endif // RAPIDJSON_STREAM_H_ diff --git a/livox_ros_driver/common/rapidjson/stringbuffer.h b/livox_ros_driver/common/rapidjson/stringbuffer.h new file mode 100644 index 0000000..4e38b82 --- /dev/null +++ b/livox_ros_driver/common/rapidjson/stringbuffer.h @@ -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 // 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 +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() = c; } + void PutUnsafe(Ch c) { *stack_.template PushUnsafe() = c; } + void Flush() {} + + void Clear() { stack_.Clear(); } + void ShrinkToFit() { + // Push and pop a null terminator. This is safe. + *stack_.template Push() = '\0'; + stack_.ShrinkToFit(); + stack_.template Pop(1); + } + + void Reserve(size_t count) { stack_.template Reserve(count); } + Ch* Push(size_t count) { return stack_.template Push(count); } + Ch* PushUnsafe(size_t count) { return stack_.template PushUnsafe(count); } + void Pop(size_t count) { stack_.template Pop(count); } + + const Ch* GetString() const { + // Push and pop a null terminator. This is safe. + *stack_.template Push() = '\0'; + stack_.template Pop(1); + + return stack_.template Bottom(); + } + + //! 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 stack_; + +private: + // Prohibit copy constructor & assignment operator. + GenericStringBuffer(const GenericStringBuffer&); + GenericStringBuffer& operator=(const GenericStringBuffer&); +}; + +//! String buffer with UTF8 encoding +typedef GenericStringBuffer > StringBuffer; + +template +inline void PutReserve(GenericStringBuffer& stream, size_t count) { + stream.Reserve(count); +} + +template +inline void PutUnsafe(GenericStringBuffer& stream, typename Encoding::Ch c) { + stream.PutUnsafe(c); +} + +//! Implement specialized version of PutN() with memset() for better performance. +template<> +inline void PutN(GenericStringBuffer >& stream, char c, size_t n) { + std::memset(stream.stack_.Push(n), c, n * sizeof(c)); +} + +RAPIDJSON_NAMESPACE_END + +#if defined(__clang__) +RAPIDJSON_DIAG_POP +#endif + +#endif // RAPIDJSON_STRINGBUFFER_H_ diff --git a/livox_ros_driver/common/rapidjson/writer.h b/livox_ros_driver/common/rapidjson/writer.h new file mode 100644 index 0000000..ce39e76 --- /dev/null +++ b/livox_ros_driver/common/rapidjson/writer.h @@ -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 // placement new + +#if defined(RAPIDJSON_SIMD) && defined(_MSC_VER) +#include +#pragma intrinsic(_BitScanForward) +#endif +#ifdef RAPIDJSON_SSE42 +#include +#elif defined(RAPIDJSON_SSE2) +#include +#elif defined(RAPIDJSON_NEON) +#include +#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 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 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& str) { + return String(str.data(), SizeType(str.size())); + } +#endif + + bool StartObject() { + Prefix(kObjectType); + new (level_stack_.template Push()) 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& 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()->inArray); // currently inside an Array, not Object + RAPIDJSON_ASSERT(0 == level_stack_.template Top()->valueCount % 2); // Object has a Key without a Value + level_stack_.template Pop(1); + return EndValue(WriteEndObject()); + } + + bool StartArray() { + Prefix(kArrayType); + new (level_stack_.template Push()) Level(true); + return WriteStartArray(); + } + + bool EndArray(SizeType elementCount = 0) { + (void)elementCount; + RAPIDJSON_ASSERT(level_stack_.GetSize() >= sizeof(Level)); + RAPIDJSON_ASSERT(level_stack_.template Top()->inArray); + level_stack_.template Pop(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(end - buffer)); + for (const char* p = buffer; p != end; ++p) + PutUnsafe(*os_, static_cast(*p)); + return true; + } + + bool WriteUint(unsigned u) { + char buffer[10]; + const char* end = internal::u32toa(u, buffer); + PutReserve(*os_, static_cast(end - buffer)); + for (const char* p = buffer; p != end; ++p) + PutUnsafe(*os_, static_cast(*p)); + return true; + } + + bool WriteInt64(int64_t i64) { + char buffer[21]; + const char* end = internal::i64toa(i64, buffer); + PutReserve(*os_, static_cast(end - buffer)); + for (const char* p = buffer; p != end; ++p) + PutUnsafe(*os_, static_cast(*p)); + return true; + } + + bool WriteUint64(uint64_t u64) { + char buffer[20]; + char* end = internal::u64toa(u64, buffer); + PutReserve(*os_, static_cast(end - buffer)); + for (char* p = buffer; p != end; ++p) + PutUnsafe(*os_, static_cast(*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(end - buffer)); + for (char* p = buffer; p != end; ++p) + PutUnsafe(*os_, static_cast(*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 is(str); + while (ScanWriteUnescapedString(is, length)) { + const Ch c = is.Peek(); + if (!TargetEncoding::supportUnicode && static_cast(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(c) < 256) && RAPIDJSON_UNLIKELY(escape[static_cast(c)])) { + is.Take(); + PutUnsafe(*os_, '\\'); + PutUnsafe(*os_, static_cast(escape[static_cast(c)])); + if (escape[static_cast(c)] == 'u') { + PutUnsafe(*os_, '0'); + PutUnsafe(*os_, '0'); + PutUnsafe(*os_, hexDigits[static_cast(c) >> 4]); + PutUnsafe(*os_, hexDigits[static_cast(c) & 0xF]); + } + } + else if (RAPIDJSON_UNLIKELY(!(writeFlags & kWriteValidateEncodingFlag ? + Transcoder::Validate(is, *os_) : + Transcoder::TranscodeUnsafe(is, *os_)))) + return false; + } + PutUnsafe(*os_, '\"'); + return true; + } + + bool ScanWriteUnescapedString(GenericStringStream& 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 is(json); + while (RAPIDJSON_LIKELY(is.Tell() < length)) { + RAPIDJSON_ASSERT(is.Peek() != '\0'); + if (RAPIDJSON_UNLIKELY(!(writeFlags & kWriteValidateEncodingFlag ? + Transcoder::Validate(is, *os_) : + Transcoder::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(); + 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 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::WriteInt(int i) { + char *buffer = os_->Push(11); + const char* end = internal::i32toa(i, buffer); + os_->Pop(static_cast(11 - (end - buffer))); + return true; +} + +template<> +inline bool Writer::WriteUint(unsigned u) { + char *buffer = os_->Push(10); + const char* end = internal::u32toa(u, buffer); + os_->Pop(static_cast(10 - (end - buffer))); + return true; +} + +template<> +inline bool Writer::WriteInt64(int64_t i64) { + char *buffer = os_->Push(21); + const char* end = internal::i64toa(i64, buffer); + os_->Pop(static_cast(21 - (end - buffer))); + return true; +} + +template<> +inline bool Writer::WriteUint64(uint64_t u) { + char *buffer = os_->Push(20); + const char* end = internal::u64toa(u, buffer); + os_->Pop(static_cast(20 - (end - buffer))); + return true; +} + +template<> +inline bool Writer::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(25 - (end - buffer))); + return true; +} + +#if defined(RAPIDJSON_SSE2) || defined(RAPIDJSON_SSE42) +template<> +inline bool Writer::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((reinterpret_cast(p) + 15) & static_cast(~15)); + const char* endAligned = reinterpret_cast(reinterpret_cast(end) & static_cast(~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(&dquote[0])); + const __m128i bs = _mm_loadu_si128(reinterpret_cast(&bslash[0])); + const __m128i sp = _mm_loadu_si128(reinterpret_cast(&space[0])); + + for (; p != endAligned; p += 16) { + const __m128i s = _mm_load_si128(reinterpret_cast(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(_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(__builtin_ffs(r) - 1); +#endif + char* q = reinterpret_cast(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::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((reinterpret_cast(p) + 15) & static_cast(~15)); + const char* endAligned = reinterpret_cast(reinterpret_cast(end) & static_cast(~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(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(os_->PushUnsafe(len)); + for (size_t i = 0; i < len; i++) + q[i] = p[i]; + + p += len; + break; + } + vst1q_u8(reinterpret_cast(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_ diff --git a/livox_ros_driver/common/rapidxml/rapidxml.hpp b/livox_ros_driver/common/rapidxml/rapidxml.hpp new file mode 100644 index 0000000..ae91e08 --- /dev/null +++ b/livox_ros_driver/common/rapidxml/rapidxml.hpp @@ -0,0 +1,2596 @@ +#ifndef RAPIDXML_HPP_INCLUDED +#define RAPIDXML_HPP_INCLUDED + +// Copyright (C) 2006, 2009 Marcin Kalicinski +// Version 1.13 +// Revision $DateTime: 2009/05/13 01:46:17 $ +//! \file rapidxml.hpp This file contains rapidxml parser and DOM implementation + +// If standard library is disabled, user must provide implementations of required functions and typedefs +#if !defined(RAPIDXML_NO_STDLIB) + #include // For std::size_t + #include // For assert + #include // For placement new +#endif + +// On MSVC, disable "conditional expression is constant" warning (level 4). +// This warning is almost impossible to avoid with certain types of templated code +#ifdef _MSC_VER + #pragma warning(push) + #pragma warning(disable:4127) // Conditional expression is constant +#endif + +/////////////////////////////////////////////////////////////////////////// +// RAPIDXML_PARSE_ERROR + +#if defined(RAPIDXML_NO_EXCEPTIONS) + +#define RAPIDXML_PARSE_ERROR(what, where) { parse_error_handler(what, where); assert(0); } + +namespace rapidxml +{ + //! When exceptions are disabled by defining RAPIDXML_NO_EXCEPTIONS, + //! this function is called to notify user about the error. + //! It must be defined by the user. + //!

+ //! This function cannot return. If it does, the results are undefined. + //!

+ //! A very simple definition might look like that: + //! 
+    //! void %rapidxml::%parse_error_handler(const char *what, void *where)
+    //! {
+    //!     std::cout << "Parse error: " << what << "\n";
+    //!     std::abort();
+    //! }
+    //!
+ //! \param what Human readable description of the error. + //! \param where Pointer to character data where error was detected. + void parse_error_handler(const char *what, void *where); +} + +#else + +#include // For std::exception + +#define RAPIDXML_PARSE_ERROR(what, where) throw parse_error(what, where) + +namespace rapidxml +{ + + //! Parse error exception. + //! This exception is thrown by the parser when an error occurs. + //! Use what() function to get human-readable error message. + //! Use where() function to get a pointer to position within source text where error was detected. + //!

+ //! If throwing exceptions by the parser is undesirable, + //! it can be disabled by defining RAPIDXML_NO_EXCEPTIONS macro before rapidxml.hpp is included. + //! This will cause the parser to call rapidxml::parse_error_handler() function instead of throwing an exception. + //! This function must be defined by the user. + //!

+ //! This class derives from std::exception class. + class parse_error: public std::exception + { + + public: + + //! Constructs parse error + parse_error(const char *what, void *where) + : m_what(what) + , m_where(where) + { + } + + //! Gets human readable description of error. + //! \return Pointer to null terminated description of the error. + virtual const char *what() const throw() + { + return m_what; + } + + //! Gets pointer to character data where error happened. + //! Ch should be the same as char type of xml_document that produced the error. + //! \return Pointer to location within the parsed string where error occured. + template + Ch *where() const + { + return reinterpret_cast(m_where); + } + + private: + + const char *m_what; + void *m_where; + + }; +} + +#endif + +/////////////////////////////////////////////////////////////////////////// +// Pool sizes + +#ifndef RAPIDXML_STATIC_POOL_SIZE + // Size of static memory block of memory_pool. + // Define RAPIDXML_STATIC_POOL_SIZE before including rapidxml.hpp if you want to override the default value. + // No dynamic memory allocations are performed by memory_pool until static memory is exhausted. + #define RAPIDXML_STATIC_POOL_SIZE (64 * 1024) +#endif + +#ifndef RAPIDXML_DYNAMIC_POOL_SIZE + // Size of dynamic memory block of memory_pool. + // Define RAPIDXML_DYNAMIC_POOL_SIZE before including rapidxml.hpp if you want to override the default value. + // After the static block is exhausted, dynamic blocks with approximately this size are allocated by memory_pool. + #define RAPIDXML_DYNAMIC_POOL_SIZE (64 * 1024) +#endif + +#ifndef RAPIDXML_ALIGNMENT + // Memory allocation alignment. + // Define RAPIDXML_ALIGNMENT before including rapidxml.hpp if you want to override the default value, which is the size of pointer. + // All memory allocations for nodes, attributes and strings will be aligned to this value. + // This must be a power of 2 and at least 1, otherwise memory_pool will not work. + #define RAPIDXML_ALIGNMENT sizeof(void *) +#endif + +namespace rapidxml +{ + // Forward declarations + template class xml_node; + template class xml_attribute; + template class xml_document; + + //! Enumeration listing all node types produced by the parser. + //! Use xml_node::type() function to query node type. + enum node_type + { + node_document, //!< A document node. Name and value are empty. + node_element, //!< An element node. Name contains element name. Value contains text of first data node. + node_data, //!< A data node. Name is empty. Value contains data text. + node_cdata, //!< A CDATA node. Name is empty. Value contains data text. + node_comment, //!< A comment node. Name is empty. Value contains comment text. + node_declaration, //!< A declaration node. Name and value are empty. Declaration parameters (version, encoding and standalone) are in node attributes. + node_doctype, //!< A DOCTYPE node. Name is empty. Value contains DOCTYPE text. + node_pi //!< A PI node. Name contains target. Value contains instructions. + }; + + /////////////////////////////////////////////////////////////////////// + // Parsing flags + + //! Parse flag instructing the parser to not create data nodes. + //! Text of first data node will still be placed in value of parent element, unless rapidxml::parse_no_element_values flag is also specified. + //! Can be combined with other flags by use of | operator. + //!

+ //! See xml_document::parse() function. + const int parse_no_data_nodes = 0x1; + + //! Parse flag instructing the parser to not use text of first data node as a value of parent element. + //! Can be combined with other flags by use of | operator. + //! Note that child data nodes of element node take precendence over its value when printing. + //! That is, if element has one or more child data nodes and a value, the value will be ignored. + //! Use rapidxml::parse_no_data_nodes flag to prevent creation of data nodes if you want to manipulate data using values of elements. + //!

+ //! See xml_document::parse() function. + const int parse_no_element_values = 0x2; + + //! Parse flag instructing the parser to not place zero terminators after strings in the source text. + //! By default zero terminators are placed, modifying source text. + //! Can be combined with other flags by use of | operator. + //!

+ //! See xml_document::parse() function. + const int parse_no_string_terminators = 0x4; + + //! Parse flag instructing the parser to not translate entities in the source text. + //! By default entities are translated, modifying source text. + //! Can be combined with other flags by use of | operator. + //!

+ //! See xml_document::parse() function. + const int parse_no_entity_translation = 0x8; + + //! Parse flag instructing the parser to disable UTF-8 handling and assume plain 8 bit characters. + //! By default, UTF-8 handling is enabled. + //! Can be combined with other flags by use of | operator. + //!

+ //! See xml_document::parse() function. + const int parse_no_utf8 = 0x10; + + //! Parse flag instructing the parser to create XML declaration node. + //! By default, declaration node is not created. + //! Can be combined with other flags by use of | operator. + //!

+ //! See xml_document::parse() function. + const int parse_declaration_node = 0x20; + + //! Parse flag instructing the parser to create comments nodes. + //! By default, comment nodes are not created. + //! Can be combined with other flags by use of | operator. + //!

+ //! See xml_document::parse() function. + const int parse_comment_nodes = 0x40; + + //! Parse flag instructing the parser to create DOCTYPE node. + //! By default, doctype node is not created. + //! Although W3C specification allows at most one DOCTYPE node, RapidXml will silently accept documents with more than one. + //! Can be combined with other flags by use of | operator. + //!

+ //! See xml_document::parse() function. + const int parse_doctype_node = 0x80; + + //! Parse flag instructing the parser to create PI nodes. + //! By default, PI nodes are not created. + //! Can be combined with other flags by use of | operator. + //!

+ //! See xml_document::parse() function. + const int parse_pi_nodes = 0x100; + + //! Parse flag instructing the parser to validate closing tag names. + //! If not set, name inside closing tag is irrelevant to the parser. + //! By default, closing tags are not validated. + //! Can be combined with other flags by use of | operator. + //!

+ //! See xml_document::parse() function. + const int parse_validate_closing_tags = 0x200; + + //! Parse flag instructing the parser to trim all leading and trailing whitespace of data nodes. + //! By default, whitespace is not trimmed. + //! This flag does not cause the parser to modify source text. + //! Can be combined with other flags by use of | operator. + //!

+ //! See xml_document::parse() function. + const int parse_trim_whitespace = 0x400; + + //! Parse flag instructing the parser to condense all whitespace runs of data nodes to a single space character. + //! Trimming of leading and trailing whitespace of data is controlled by rapidxml::parse_trim_whitespace flag. + //! By default, whitespace is not normalized. + //! If this flag is specified, source text will be modified. + //! Can be combined with other flags by use of | operator. + //!

+ //! See xml_document::parse() function. + const int parse_normalize_whitespace = 0x800; + + // Compound flags + + //! Parse flags which represent default behaviour of the parser. + //! This is always equal to 0, so that all other flags can be simply ored together. + //! Normally there is no need to inconveniently disable flags by anding with their negated (~) values. + //! This also means that meaning of each flag is a negation of the default setting. + //! For example, if flag name is rapidxml::parse_no_utf8, it means that utf-8 is enabled by default, + //! and using the flag will disable it. + //!

+ //! See xml_document::parse() function. + const int parse_default = 0; + + //! A combination of parse flags that forbids any modifications of the source text. + //! This also results in faster parsing. However, note that the following will occur: + //!
    + //!
  • names and values of nodes will not be zero terminated, you have to use xml_base::name_size() and xml_base::value_size() functions to determine where name and value ends
    • + //!
  • entities will not be translated
    • + //!
  • whitespace will not be normalized
    • + //!
+ //! See xml_document::parse() function. + const int parse_non_destructive = parse_no_string_terminators | parse_no_entity_translation; + + //! A combination of parse flags resulting in fastest possible parsing, without sacrificing important data. + //!

+ //! See xml_document::parse() function. + const int parse_fastest = parse_non_destructive | parse_no_data_nodes; + + //! A combination of parse flags resulting in largest amount of data being extracted. + //! This usually results in slowest parsing. + //!

+ //! See xml_document::parse() function. + const int parse_full = parse_declaration_node | parse_comment_nodes | parse_doctype_node | parse_pi_nodes | parse_validate_closing_tags; + + /////////////////////////////////////////////////////////////////////// + // Internals + + //! \cond internal + namespace internal + { + + // Struct that contains lookup tables for the parser + // It must be a template to allow correct linking (because it has static data members, which are defined in a header file). + template + struct lookup_tables + { + static const unsigned char lookup_whitespace[256]; // Whitespace table + static const unsigned char lookup_node_name[256]; // Node name table + static const unsigned char lookup_text[256]; // Text table + static const unsigned char lookup_text_pure_no_ws[256]; // Text table + static const unsigned char lookup_text_pure_with_ws[256]; // Text table + static const unsigned char lookup_attribute_name[256]; // Attribute name table + static const unsigned char lookup_attribute_data_1[256]; // Attribute data table with single quote + static const unsigned char lookup_attribute_data_1_pure[256]; // Attribute data table with single quote + static const unsigned char lookup_attribute_data_2[256]; // Attribute data table with double quotes + static const unsigned char lookup_attribute_data_2_pure[256]; // Attribute data table with double quotes + static const unsigned char lookup_digits[256]; // Digits + static const unsigned char lookup_upcase[256]; // To uppercase conversion table for ASCII characters + }; + + // Find length of the string + template + inline std::size_t measure(const Ch *p) + { + const Ch *tmp = p; + while (*tmp) + ++tmp; + return tmp - p; + } + + // Compare strings for equality + template + inline bool compare(const Ch *p1, std::size_t size1, const Ch *p2, std::size_t size2, bool case_sensitive) + { + if (size1 != size2) + return false; + if (case_sensitive) + { + for (const Ch *end = p1 + size1; p1 < end; ++p1, ++p2) + if (*p1 != *p2) + return false; + } + else + { + for (const Ch *end = p1 + size1; p1 < end; ++p1, ++p2) + if (lookup_tables<0>::lookup_upcase[static_cast(*p1)] != lookup_tables<0>::lookup_upcase[static_cast(*p2)]) + return false; + } + return true; + } + } + //! \endcond + + /////////////////////////////////////////////////////////////////////// + // Memory pool + + //! This class is used by the parser to create new nodes and attributes, without overheads of dynamic memory allocation. + //! In most cases, you will not need to use this class directly. + //! However, if you need to create nodes manually or modify names/values of nodes, + //! you are encouraged to use memory_pool of relevant xml_document to allocate the memory. + //! Not only is this faster than allocating them by using new operator, + //! but also their lifetime will be tied to the lifetime of document, + //! possibly simplyfing memory management. + //!

+ //! Call allocate_node() or allocate_attribute() functions to obtain new nodes or attributes from the pool. + //! You can also call allocate_string() function to allocate strings. + //! Such strings can then be used as names or values of nodes without worrying about their lifetime. + //! Note that there is no free() function -- all allocations are freed at once when clear() function is called, + //! or when the pool is destroyed. + //!

+ //! It is also possible to create a standalone memory_pool, and use it + //! to allocate nodes, whose lifetime will not be tied to any document. + //!

+ //! Pool maintains RAPIDXML_STATIC_POOL_SIZE bytes of statically allocated memory. + //! Until static memory is exhausted, no dynamic memory allocations are done. + //! When static memory is exhausted, pool allocates additional blocks of memory of size RAPIDXML_DYNAMIC_POOL_SIZE each, + //! by using global new[] and delete[] operators. + //! This behaviour can be changed by setting custom allocation routines. + //! Use set_allocator() function to set them. + //!

+ //! Allocations for nodes, attributes and strings are aligned at RAPIDXML_ALIGNMENT bytes. + //! This value defaults to the size of pointer on target architecture. + //!

+ //! To obtain absolutely top performance from the parser, + //! it is important that all nodes are allocated from a single, contiguous block of memory. + //! Otherwise, cache misses when jumping between two (or more) disjoint blocks of memory can slow down parsing quite considerably. + //! If required, you can tweak RAPIDXML_STATIC_POOL_SIZE, RAPIDXML_DYNAMIC_POOL_SIZE and RAPIDXML_ALIGNMENT + //! to obtain best wasted memory to performance compromise. + //! To do it, define their values before rapidxml.hpp file is included. + //! \param Ch Character type of created nodes. + template + class memory_pool + { + + public: + + //! \cond internal + typedef void *(alloc_func)(std::size_t); // Type of user-defined function used to allocate memory + typedef void (free_func)(void *); // Type of user-defined function used to free memory + //! \endcond + + //! Constructs empty pool with default allocator functions. + memory_pool() + : m_alloc_func(0) + , m_free_func(0) + { + init(); + } + + //! Destroys pool and frees all the memory. + //! This causes memory occupied by nodes allocated by the pool to be freed. + //! Nodes allocated from the pool are no longer valid. + ~memory_pool() + { + clear(); + } + + //! Allocates a new node from the pool, and optionally assigns name and value to it. + //! If the allocation request cannot be accomodated, this function will throw std::bad_alloc. + //! If exceptions are disabled by defining RAPIDXML_NO_EXCEPTIONS, this function + //! will call rapidxml::parse_error_handler() function. + //! \param type Type of node to create. + //! \param name Name to assign to the node, or 0 to assign no name. + //! \param value Value to assign to the node, or 0 to assign no value. + //! \param name_size Size of name to assign, or 0 to automatically calculate size from name string. + //! \param value_size Size of value to assign, or 0 to automatically calculate size from value string. + //! \return Pointer to allocated node. This pointer will never be NULL. + xml_node *allocate_node(node_type type, + const Ch *name = 0, const Ch *value = 0, + std::size_t name_size = 0, std::size_t value_size = 0) + { + void *memory = allocate_aligned(sizeof(xml_node)); + xml_node *node = new(memory) xml_node(type); + if (name) + { + if (name_size > 0) + node->name(name, name_size); + else + node->name(name); + } + if (value) + { + if (value_size > 0) + node->value(value, value_size); + else + node->value(value); + } + return node; + } + + //! Allocates a new attribute from the pool, and optionally assigns name and value to it. + //! If the allocation request cannot be accomodated, this function will throw std::bad_alloc. + //! If exceptions are disabled by defining RAPIDXML_NO_EXCEPTIONS, this function + //! will call rapidxml::parse_error_handler() function. + //! \param name Name to assign to the attribute, or 0 to assign no name. + //! \param value Value to assign to the attribute, or 0 to assign no value. + //! \param name_size Size of name to assign, or 0 to automatically calculate size from name string. + //! \param value_size Size of value to assign, or 0 to automatically calculate size from value string. + //! \return Pointer to allocated attribute. This pointer will never be NULL. + xml_attribute *allocate_attribute(const Ch *name = 0, const Ch *value = 0, + std::size_t name_size = 0, std::size_t value_size = 0) + { + void *memory = allocate_aligned(sizeof(xml_attribute)); + xml_attribute *attribute = new(memory) xml_attribute; + if (name) + { + if (name_size > 0) + attribute->name(name, name_size); + else + attribute->name(name); + } + if (value) + { + if (value_size > 0) + attribute->value(value, value_size); + else + attribute->value(value); + } + return attribute; + } + + //! Allocates a char array of given size from the pool, and optionally copies a given string to it. + //! If the allocation request cannot be accomodated, this function will throw std::bad_alloc. + //! If exceptions are disabled by defining RAPIDXML_NO_EXCEPTIONS, this function + //! will call rapidxml::parse_error_handler() function. + //! \param source String to initialize the allocated memory with, or 0 to not initialize it. + //! \param size Number of characters to allocate, or zero to calculate it automatically from source string length; if size is 0, source string must be specified and null terminated. + //! \return Pointer to allocated char array. This pointer will never be NULL. + Ch *allocate_string(const Ch *source = 0, std::size_t size = 0) + { + assert(source || size); // Either source or size (or both) must be specified + if (size == 0) + size = internal::measure(source) + 1; + Ch *result = static_cast(allocate_aligned(size * sizeof(Ch))); + if (source) + for (std::size_t i = 0; i < size; ++i) + result[i] = source[i]; + return result; + } + + //! Clones an xml_node and its hierarchy of child nodes and attributes. + //! Nodes and attributes are allocated from this memory pool. + //! Names and values are not cloned, they are shared between the clone and the source. + //! Result node can be optionally specified as a second parameter, + //! in which case its contents will be replaced with cloned source node. + //! This is useful when you want to clone entire document. + //! \param source Node to clone. + //! \param result Node to put results in, or 0 to automatically allocate result node + //! \return Pointer to cloned node. This pointer will never be NULL. + xml_node *clone_node(const xml_node *source, xml_node *result = 0) + { + // Prepare result node + if (result) + { + result->remove_all_attributes(); + result->remove_all_nodes(); + result->type(source->type()); + } + else + result = allocate_node(source->type()); + + // Clone name and value + result->name(source->name(), source->name_size()); + result->value(source->value(), source->value_size()); + + // Clone child nodes and attributes + for (xml_node *child = source->first_node(); child; child = child->next_sibling()) + result->append_node(clone_node(child)); + for (xml_attribute *attr = source->first_attribute(); attr; attr = attr->next_attribute()) + result->append_attribute(allocate_attribute(attr->name(), attr->value(), attr->name_size(), attr->value_size())); + + return result; + } + + //! Clears the pool. + //! This causes memory occupied by nodes allocated by the pool to be freed. + //! Any nodes or strings allocated from the pool will no longer be valid. + void clear() + { + while (m_begin != m_static_memory) + { + char *previous_begin = reinterpret_cast
(align(m_begin))->previous_begin; + if (m_free_func) + m_free_func(m_begin); + else + delete[] m_begin; + m_begin = previous_begin; + } + init(); + } + + //! Sets or resets the user-defined memory allocation functions for the pool. + //! This can only be called when no memory is allocated from the pool yet, otherwise results are undefined. + //! Allocation function must not return invalid pointer on failure. It should either throw, + //! stop the program, or use longjmp() function to pass control to other place of program. + //! If it returns invalid pointer, results are undefined. + //!

+ //! User defined allocation functions must have the following forms: + //!
+ //!
void *allocate(std::size_t size); + //!
void free(void *pointer); + //!
+ //! \param af Allocation function, or 0 to restore default function + //! \param ff Free function, or 0 to restore default function + void set_allocator(alloc_func *af, free_func *ff) + { + assert(m_begin == m_static_memory && m_ptr == align(m_begin)); // Verify that no memory is allocated yet + m_alloc_func = af; + m_free_func = ff; + } + + private: + + struct header + { + char *previous_begin; + }; + + void init() + { + m_begin = m_static_memory; + m_ptr = align(m_begin); + m_end = m_static_memory + sizeof(m_static_memory); + } + + char *align(char *ptr) + { + std::size_t alignment = ((RAPIDXML_ALIGNMENT - (std::size_t(ptr) & (RAPIDXML_ALIGNMENT - 1))) & (RAPIDXML_ALIGNMENT - 1)); + return ptr + alignment; + } + + char *allocate_raw(std::size_t size) + { + // Allocate + void *memory; + if (m_alloc_func) // Allocate memory using either user-specified allocation function or global operator new[] + { + memory = m_alloc_func(size); + assert(memory); // Allocator is not allowed to return 0, on failure it must either throw, stop the program or use longjmp + } + else + { + memory = new char[size]; +#ifdef RAPIDXML_NO_EXCEPTIONS + if (!memory) // If exceptions are disabled, verify memory allocation, because new will not be able to throw bad_alloc + RAPIDXML_PARSE_ERROR("out of memory", 0); +#endif + } + return static_cast(memory); + } + + void *allocate_aligned(std::size_t size) + { + // Calculate aligned pointer + char *result = align(m_ptr); + + // If not enough memory left in current pool, allocate a new pool + if (result + size > m_end) + { + // Calculate required pool size (may be bigger than RAPIDXML_DYNAMIC_POOL_SIZE) + std::size_t pool_size = RAPIDXML_DYNAMIC_POOL_SIZE; + if (pool_size < size) + pool_size = size; + + // Allocate + std::size_t alloc_size = sizeof(header) + (2 * RAPIDXML_ALIGNMENT - 2) + pool_size; // 2 alignments required in worst case: one for header, one for actual allocation + char *raw_memory = allocate_raw(alloc_size); + + // Setup new pool in allocated memory + char *pool = align(raw_memory); + header *new_header = reinterpret_cast
(pool); + new_header->previous_begin = m_begin; + m_begin = raw_memory; + m_ptr = pool + sizeof(header); + m_end = raw_memory + alloc_size; + + // Calculate aligned pointer again using new pool + result = align(m_ptr); + } + + // Update pool and return aligned pointer + m_ptr = result + size; + return result; + } + + char *m_begin; // Start of raw memory making up current pool + char *m_ptr; // First free byte in current pool + char *m_end; // One past last available byte in current pool + char m_static_memory[RAPIDXML_STATIC_POOL_SIZE]; // Static raw memory + alloc_func *m_alloc_func; // Allocator function, or 0 if default is to be used + free_func *m_free_func; // Free function, or 0 if default is to be used + }; + + /////////////////////////////////////////////////////////////////////////// + // XML base + + //! Base class for xml_node and xml_attribute implementing common functions: + //! name(), name_size(), value(), value_size() and parent(). + //! \param Ch Character type to use + template + class xml_base + { + + public: + + /////////////////////////////////////////////////////////////////////////// + // Construction & destruction + + // Construct a base with empty name, value and parent + xml_base() + : m_name(0) + , m_value(0) + , m_parent(0) + { + } + + /////////////////////////////////////////////////////////////////////////// + // Node data access + + //! Gets name of the node. + //! Interpretation of name depends on type of node. + //! Note that name will not be zero-terminated if rapidxml::parse_no_string_terminators option was selected during parse. + //!

+ //! Use name_size() function to determine length of the name. + //! \return Name of node, or empty string if node has no name. + Ch *name() const + { + return m_name ? m_name : nullstr(); + } + + //! Gets size of node name, not including terminator character. + //! This function works correctly irrespective of whether name is or is not zero terminated. + //! \return Size of node name, in characters. + std::size_t name_size() const + { + return m_name ? m_name_size : 0; + } + + //! Gets value of node. + //! Interpretation of value depends on type of node. + //! Note that value will not be zero-terminated if rapidxml::parse_no_string_terminators option was selected during parse. + //!

+ //! Use value_size() function to determine length of the value. + //! \return Value of node, or empty string if node has no value. + Ch *value() const + { + return m_value ? m_value : nullstr(); + } + + //! Gets size of node value, not including terminator character. + //! This function works correctly irrespective of whether value is or is not zero terminated. + //! \return Size of node value, in characters. + std::size_t value_size() const + { + return m_value ? m_value_size : 0; + } + + /////////////////////////////////////////////////////////////////////////// + // Node modification + + //! Sets name of node to a non zero-terminated string. + //! See \ref ownership_of_strings. + //!

+ //! Note that node does not own its name or value, it only stores a pointer to it. + //! It will not delete or otherwise free the pointer on destruction. + //! It is reponsibility of the user to properly manage lifetime of the string. + //! The easiest way to achieve it is to use memory_pool of the document to allocate the string - + //! on destruction of the document the string will be automatically freed. + //!

+ //! Size of name must be specified separately, because name does not have to be zero terminated. + //! Use name(const Ch *) function to have the length automatically calculated (string must be zero terminated). + //! \param name Name of node to set. Does not have to be zero terminated. + //! \param size Size of name, in characters. This does not include zero terminator, if one is present. + void name(const Ch *name, std::size_t size) + { + m_name = const_cast(name); + m_name_size = size; + } + + //! Sets name of node to a zero-terminated string. + //! See also \ref ownership_of_strings and xml_node::name(const Ch *, std::size_t). + //! \param name Name of node to set. Must be zero terminated. + void name(const Ch *name) + { + this->name(name, internal::measure(name)); + } + + //! Sets value of node to a non zero-terminated string. + //! See \ref ownership_of_strings. + //!

+ //! Note that node does not own its name or value, it only stores a pointer to it. + //! It will not delete or otherwise free the pointer on destruction. + //! It is reponsibility of the user to properly manage lifetime of the string. + //! The easiest way to achieve it is to use memory_pool of the document to allocate the string - + //! on destruction of the document the string will be automatically freed. + //!

+ //! Size of value must be specified separately, because it does not have to be zero terminated. + //! Use value(const Ch *) function to have the length automatically calculated (string must be zero terminated). + //!

+ //! If an element has a child node of type node_data, it will take precedence over element value when printing. + //! If you want to manipulate data of elements using values, use parser flag rapidxml::parse_no_data_nodes to prevent creation of data nodes by the parser. + //! \param value value of node to set. Does not have to be zero terminated. + //! \param size Size of value, in characters. This does not include zero terminator, if one is present. + void value(const Ch *value, std::size_t size) + { + m_value = const_cast(value); + m_value_size = size; + } + + //! Sets value of node to a zero-terminated string. + //! See also \ref ownership_of_strings and xml_node::value(const Ch *, std::size_t). + //! \param value Vame of node to set. Must be zero terminated. + void value(const Ch *value) + { + this->value(value, internal::measure(value)); + } + + /////////////////////////////////////////////////////////////////////////// + // Related nodes access + + //! Gets node parent. + //! \return Pointer to parent node, or 0 if there is no parent. + xml_node *parent() const + { + return m_parent; + } + + protected: + + // Return empty string + static Ch *nullstr() + { + static Ch zero = Ch('\0'); + return &zero; + } + + Ch *m_name; // Name of node, or 0 if no name + Ch *m_value; // Value of node, or 0 if no value + std::size_t m_name_size; // Length of node name, or undefined of no name + std::size_t m_value_size; // Length of node value, or undefined if no value + xml_node *m_parent; // Pointer to parent node, or 0 if none + + }; + + //! Class representing attribute node of XML document. + //! Each attribute has name and value strings, which are available through name() and value() functions (inherited from xml_base). + //! Note that after parse, both name and value of attribute will point to interior of source text used for parsing. + //! Thus, this text must persist in memory for the lifetime of attribute. + //! \param Ch Character type to use. + template + class xml_attribute: public xml_base + { + + friend class xml_node; + + public: + + /////////////////////////////////////////////////////////////////////////// + // Construction & destruction + + //! Constructs an empty attribute with the specified type. + //! Consider using memory_pool of appropriate xml_document if allocating attributes manually. + xml_attribute() + { + } + + /////////////////////////////////////////////////////////////////////////// + // Related nodes access + + //! Gets document of which attribute is a child. + //! \return Pointer to document that contains this attribute, or 0 if there is no parent document. + xml_document *document() const + { + if (xml_node *node = this->parent()) + { + while (node->parent()) + node = node->parent(); + return node->type() == node_document ? static_cast *>(node) : 0; + } + else + return 0; + } + + //! Gets previous attribute, optionally matching attribute name. + //! \param name Name of attribute to find, or 0 to return previous attribute regardless of its name; this string doesn't have to be zero-terminated if name_size is non-zero + //! \param name_size Size of name, in characters, or 0 to have size calculated automatically from string + //! \param case_sensitive Should name comparison be case-sensitive; non case-sensitive comparison works properly only for ASCII characters + //! \return Pointer to found attribute, or 0 if not found. + xml_attribute *previous_attribute(const Ch *name = 0, std::size_t name_size = 0, bool case_sensitive = true) const + { + if (name) + { + if (name_size == 0) + name_size = internal::measure(name); + for (xml_attribute *attribute = m_prev_attribute; attribute; attribute = attribute->m_prev_attribute) + if (internal::compare(attribute->name(), attribute->name_size(), name, name_size, case_sensitive)) + return attribute; + return 0; + } + else + return this->m_parent ? m_prev_attribute : 0; + } + + //! Gets next attribute, optionally matching attribute name. + //! \param name Name of attribute to find, or 0 to return next attribute regardless of its name; this string doesn't have to be zero-terminated if name_size is non-zero + //! \param name_size Size of name, in characters, or 0 to have size calculated automatically from string + //! \param case_sensitive Should name comparison be case-sensitive; non case-sensitive comparison works properly only for ASCII characters + //! \return Pointer to found attribute, or 0 if not found. + xml_attribute *next_attribute(const Ch *name = 0, std::size_t name_size = 0, bool case_sensitive = true) const + { + if (name) + { + if (name_size == 0) + name_size = internal::measure(name); + for (xml_attribute *attribute = m_next_attribute; attribute; attribute = attribute->m_next_attribute) + if (internal::compare(attribute->name(), attribute->name_size(), name, name_size, case_sensitive)) + return attribute; + return 0; + } + else + return this->m_parent ? m_next_attribute : 0; + } + + private: + + xml_attribute *m_prev_attribute; // Pointer to previous sibling of attribute, or 0 if none; only valid if parent is non-zero + xml_attribute *m_next_attribute; // Pointer to next sibling of attribute, or 0 if none; only valid if parent is non-zero + + }; + + /////////////////////////////////////////////////////////////////////////// + // XML node + + //! Class representing a node of XML document. + //! Each node may have associated name and value strings, which are available through name() and value() functions. + //! Interpretation of name and value depends on type of the node. + //! Type of node can be determined by using type() function. + //!

+ //! Note that after parse, both name and value of node, if any, will point interior of source text used for parsing. + //! Thus, this text must persist in the memory for the lifetime of node. + //! \param Ch Character type to use. + template + class xml_node: public xml_base + { + + public: + + /////////////////////////////////////////////////////////////////////////// + // Construction & destruction + + //! Constructs an empty node with the specified type. + //! Consider using memory_pool of appropriate document to allocate nodes manually. + //! \param type Type of node to construct. + xml_node(node_type type) + : m_type(type) + , m_first_node(0) + , m_first_attribute(0) + { + } + + /////////////////////////////////////////////////////////////////////////// + // Node data access + + //! Gets type of node. + //! \return Type of node. + node_type type() const + { + return m_type; + } + + /////////////////////////////////////////////////////////////////////////// + // Related nodes access + + //! Gets document of which node is a child. + //! \return Pointer to document that contains this node, or 0 if there is no parent document. + xml_document *document() const + { + xml_node *node = const_cast *>(this); + while (node->parent()) + node = node->parent(); + return node->type() == node_document ? static_cast *>(node) : 0; + } + + //! Gets first child node, optionally matching node name. + //! \param name Name of child to find, or 0 to return first child regardless of its name; this string doesn't have to be zero-terminated if name_size is non-zero + //! \param name_size Size of name, in characters, or 0 to have size calculated automatically from string + //! \param case_sensitive Should name comparison be case-sensitive; non case-sensitive comparison works properly only for ASCII characters + //! \return Pointer to found child, or 0 if not found. + xml_node *first_node(const Ch *name = 0, std::size_t name_size = 0, bool case_sensitive = true) const + { + if (name) + { + if (name_size == 0) + name_size = internal::measure(name); + for (xml_node *child = m_first_node; child; child = child->next_sibling()) + if (internal::compare(child->name(), child->name_size(), name, name_size, case_sensitive)) + return child; + return 0; + } + else + return m_first_node; + } + + //! Gets last child node, optionally matching node name. + //! Behaviour is undefined if node has no children. + //! Use first_node() to test if node has children. + //! \param name Name of child to find, or 0 to return last child regardless of its name; this string doesn't have to be zero-terminated if name_size is non-zero + //! \param name_size Size of name, in characters, or 0 to have size calculated automatically from string + //! \param case_sensitive Should name comparison be case-sensitive; non case-sensitive comparison works properly only for ASCII characters + //! \return Pointer to found child, or 0 if not found. + xml_node *last_node(const Ch *name = 0, std::size_t name_size = 0, bool case_sensitive = true) const + { + assert(m_first_node); // Cannot query for last child if node has no children + if (name) + { + if (name_size == 0) + name_size = internal::measure(name); + for (xml_node *child = m_last_node; child; child = child->previous_sibling()) + if (internal::compare(child->name(), child->name_size(), name, name_size, case_sensitive)) + return child; + return 0; + } + else + return m_last_node; + } + + //! Gets previous sibling node, optionally matching node name. + //! Behaviour is undefined if node has no parent. + //! Use parent() to test if node has a parent. + //! \param name Name of sibling to find, or 0 to return previous sibling regardless of its name; this string doesn't have to be zero-terminated if name_size is non-zero + //! \param name_size Size of name, in characters, or 0 to have size calculated automatically from string + //! \param case_sensitive Should name comparison be case-sensitive; non case-sensitive comparison works properly only for ASCII characters + //! \return Pointer to found sibling, or 0 if not found. + xml_node *previous_sibling(const Ch *name = 0, std::size_t name_size = 0, bool case_sensitive = true) const + { + assert(this->m_parent); // Cannot query for siblings if node has no parent + if (name) + { + if (name_size == 0) + name_size = internal::measure(name); + for (xml_node *sibling = m_prev_sibling; sibling; sibling = sibling->m_prev_sibling) + if (internal::compare(sibling->name(), sibling->name_size(), name, name_size, case_sensitive)) + return sibling; + return 0; + } + else + return m_prev_sibling; + } + + //! Gets next sibling node, optionally matching node name. + //! Behaviour is undefined if node has no parent. + //! Use parent() to test if node has a parent. + //! \param name Name of sibling to find, or 0 to return next sibling regardless of its name; this string doesn't have to be zero-terminated if name_size is non-zero + //! \param name_size Size of name, in characters, or 0 to have size calculated automatically from string + //! \param case_sensitive Should name comparison be case-sensitive; non case-sensitive comparison works properly only for ASCII characters + //! \return Pointer to found sibling, or 0 if not found. + xml_node *next_sibling(const Ch *name = 0, std::size_t name_size = 0, bool case_sensitive = true) const + { + assert(this->m_parent); // Cannot query for siblings if node has no parent + if (name) + { + if (name_size == 0) + name_size = internal::measure(name); + for (xml_node *sibling = m_next_sibling; sibling; sibling = sibling->m_next_sibling) + if (internal::compare(sibling->name(), sibling->name_size(), name, name_size, case_sensitive)) + return sibling; + return 0; + } + else + return m_next_sibling; + } + + //! Gets first attribute of node, optionally matching attribute name. + //! \param name Name of attribute to find, or 0 to return first attribute regardless of its name; this string doesn't have to be zero-terminated if name_size is non-zero + //! \param name_size Size of name, in characters, or 0 to have size calculated automatically from string + //! \param case_sensitive Should name comparison be case-sensitive; non case-sensitive comparison works properly only for ASCII characters + //! \return Pointer to found attribute, or 0 if not found. + xml_attribute *first_attribute(const Ch *name = 0, std::size_t name_size = 0, bool case_sensitive = true) const + { + if (name) + { + if (name_size == 0) + name_size = internal::measure(name); + for (xml_attribute *attribute = m_first_attribute; attribute; attribute = attribute->m_next_attribute) + if (internal::compare(attribute->name(), attribute->name_size(), name, name_size, case_sensitive)) + return attribute; + return 0; + } + else + return m_first_attribute; + } + + //! Gets last attribute of node, optionally matching attribute name. + //! \param name Name of attribute to find, or 0 to return last attribute regardless of its name; this string doesn't have to be zero-terminated if name_size is non-zero + //! \param name_size Size of name, in characters, or 0 to have size calculated automatically from string + //! \param case_sensitive Should name comparison be case-sensitive; non case-sensitive comparison works properly only for ASCII characters + //! \return Pointer to found attribute, or 0 if not found. + xml_attribute *last_attribute(const Ch *name = 0, std::size_t name_size = 0, bool case_sensitive = true) const + { + if (name) + { + if (name_size == 0) + name_size = internal::measure(name); + for (xml_attribute *attribute = m_last_attribute; attribute; attribute = attribute->m_prev_attribute) + if (internal::compare(attribute->name(), attribute->name_size(), name, name_size, case_sensitive)) + return attribute; + return 0; + } + else + return m_first_attribute ? m_last_attribute : 0; + } + + /////////////////////////////////////////////////////////////////////////// + // Node modification + + //! Sets type of node. + //! \param type Type of node to set. + void type(node_type type) + { + m_type = type; + } + + /////////////////////////////////////////////////////////////////////////// + // Node manipulation + + //! Prepends a new child node. + //! The prepended child becomes the first child, and all existing children are moved one position back. + //! \param child Node to prepend. + void prepend_node(xml_node *child) + { + assert(child && !child->parent() && child->type() != node_document); + if (first_node()) + { + child->m_next_sibling = m_first_node; + m_first_node->m_prev_sibling = child; + } + else + { + child->m_next_sibling = 0; + m_last_node = child; + } + m_first_node = child; + child->m_parent = this; + child->m_prev_sibling = 0; + } + + //! Appends a new child node. + //! The appended child becomes the last child. + //! \param child Node to append. + void append_node(xml_node *child) + { + assert(child && !child->parent() && child->type() != node_document); + if (first_node()) + { + child->m_prev_sibling = m_last_node; + m_last_node->m_next_sibling = child; + } + else + { + child->m_prev_sibling = 0; + m_first_node = child; + } + m_last_node = child; + child->m_parent = this; + child->m_next_sibling = 0; + } + + //! Inserts a new child node at specified place inside the node. + //! All children after and including the specified node are moved one position back. + //! \param where Place where to insert the child, or 0 to insert at the back. + //! \param child Node to insert. + void insert_node(xml_node *where, xml_node *child) + { + assert(!where || where->parent() == this); + assert(child && !child->parent() && child->type() != node_document); + if (where == m_first_node) + prepend_node(child); + else if (where == 0) + append_node(child); + else + { + child->m_prev_sibling = where->m_prev_sibling; + child->m_next_sibling = where; + where->m_prev_sibling->m_next_sibling = child; + where->m_prev_sibling = child; + child->m_parent = this; + } + } + + //! Removes first child node. + //! If node has no children, behaviour is undefined. + //! Use first_node() to test if node has children. + void remove_first_node() + { + assert(first_node()); + xml_node *child = m_first_node; + m_first_node = child->m_next_sibling; + if (child->m_next_sibling) + child->m_next_sibling->m_prev_sibling = 0; + else + m_last_node = 0; + child->m_parent = 0; + } + + //! Removes last child of the node. + //! If node has no children, behaviour is undefined. + //! Use first_node() to test if node has children. + void remove_last_node() + { + assert(first_node()); + xml_node *child = m_last_node; + if (child->m_prev_sibling) + { + m_last_node = child->m_prev_sibling; + child->m_prev_sibling->m_next_sibling = 0; + } + else + m_first_node = 0; + child->m_parent = 0; + } + + //! Removes specified child from the node + // \param where Pointer to child to be removed. + void remove_node(xml_node *where) + { + assert(where && where->parent() == this); + assert(first_node()); + if (where == m_first_node) + remove_first_node(); + else if (where == m_last_node) + remove_last_node(); + else + { + where->m_prev_sibling->m_next_sibling = where->m_next_sibling; + where->m_next_sibling->m_prev_sibling = where->m_prev_sibling; + where->m_parent = 0; + } + } + + //! Removes all child nodes (but not attributes). + void remove_all_nodes() + { + for (xml_node *node = first_node(); node; node = node->m_next_sibling) + node->m_parent = 0; + m_first_node = 0; + } + + //! Prepends a new attribute to the node. + //! \param attribute Attribute to prepend. + void prepend_attribute(xml_attribute *attribute) + { + assert(attribute && !attribute->parent()); + if (first_attribute()) + { + attribute->m_next_attribute = m_first_attribute; + m_first_attribute->m_prev_attribute = attribute; + } + else + { + attribute->m_next_attribute = 0; + m_last_attribute = attribute; + } + m_first_attribute = attribute; + attribute->m_parent = this; + attribute->m_prev_attribute = 0; + } + + //! Appends a new attribute to the node. + //! \param attribute Attribute to append. + void append_attribute(xml_attribute *attribute) + { + assert(attribute && !attribute->parent()); + if (first_attribute()) + { + attribute->m_prev_attribute = m_last_attribute; + m_last_attribute->m_next_attribute = attribute; + } + else + { + attribute->m_prev_attribute = 0; + m_first_attribute = attribute; + } + m_last_attribute = attribute; + attribute->m_parent = this; + attribute->m_next_attribute = 0; + } + + //! Inserts a new attribute at specified place inside the node. + //! All attributes after and including the specified attribute are moved one position back. + //! \param where Place where to insert the attribute, or 0 to insert at the back. + //! \param attribute Attribute to insert. + void insert_attribute(xml_attribute *where, xml_attribute *attribute) + { + assert(!where || where->parent() == this); + assert(attribute && !attribute->parent()); + if (where == m_first_attribute) + prepend_attribute(attribute); + else if (where == 0) + append_attribute(attribute); + else + { + attribute->m_prev_attribute = where->m_prev_attribute; + attribute->m_next_attribute = where; + where->m_prev_attribute->m_next_attribute = attribute; + where->m_prev_attribute = attribute; + attribute->m_parent = this; + } + } + + //! Removes first attribute of the node. + //! If node has no attributes, behaviour is undefined. + //! Use first_attribute() to test if node has attributes. + void remove_first_attribute() + { + assert(first_attribute()); + xml_attribute *attribute = m_first_attribute; + if (attribute->m_next_attribute) + { + attribute->m_next_attribute->m_prev_attribute = 0; + } + else + m_last_attribute = 0; + attribute->m_parent = 0; + m_first_attribute = attribute->m_next_attribute; + } + + //! Removes last attribute of the node. + //! If node has no attributes, behaviour is undefined. + //! Use first_attribute() to test if node has attributes. + void remove_last_attribute() + { + assert(first_attribute()); + xml_attribute *attribute = m_last_attribute; + if (attribute->m_prev_attribute) + { + attribute->m_prev_attribute->m_next_attribute = 0; + m_last_attribute = attribute->m_prev_attribute; + } + else + m_first_attribute = 0; + attribute->m_parent = 0; + } + + //! Removes specified attribute from node. + //! \param where Pointer to attribute to be removed. + void remove_attribute(xml_attribute *where) + { + assert(first_attribute() && where->parent() == this); + if (where == m_first_attribute) + remove_first_attribute(); + else if (where == m_last_attribute) + remove_last_attribute(); + else + { + where->m_prev_attribute->m_next_attribute = where->m_next_attribute; + where->m_next_attribute->m_prev_attribute = where->m_prev_attribute; + where->m_parent = 0; + } + } + + //! Removes all attributes of node. + void remove_all_attributes() + { + for (xml_attribute *attribute = first_attribute(); attribute; attribute = attribute->m_next_attribute) + attribute->m_parent = 0; + m_first_attribute = 0; + } + + private: + + /////////////////////////////////////////////////////////////////////////// + // Restrictions + + // No copying + xml_node(const xml_node &); + void operator =(const xml_node &); + + /////////////////////////////////////////////////////////////////////////// + // Data members + + // Note that some of the pointers below have UNDEFINED values if certain other pointers are 0. + // This is required for maximum performance, as it allows the parser to omit initialization of + // unneded/redundant values. + // + // The rules are as follows: + // 1. first_node and first_attribute contain valid pointers, or 0 if node has no children/attributes respectively + // 2. last_node and last_attribute are valid only if node has at least one child/attribute respectively, otherwise they contain garbage + // 3. prev_sibling and next_sibling are valid only if node has a parent, otherwise they contain garbage + + node_type m_type; // Type of node; always valid + xml_node *m_first_node; // Pointer to first child node, or 0 if none; always valid + xml_node *m_last_node; // Pointer to last child node, or 0 if none; this value is only valid if m_first_node is non-zero + xml_attribute *m_first_attribute; // Pointer to first attribute of node, or 0 if none; always valid + xml_attribute *m_last_attribute; // Pointer to last attribute of node, or 0 if none; this value is only valid if m_first_attribute is non-zero + xml_node *m_prev_sibling; // Pointer to previous sibling of node, or 0 if none; this value is only valid if m_parent is non-zero + xml_node *m_next_sibling; // Pointer to next sibling of node, or 0 if none; this value is only valid if m_parent is non-zero + + }; + + /////////////////////////////////////////////////////////////////////////// + // XML document + + //! This class represents root of the DOM hierarchy. + //! It is also an xml_node and a memory_pool through public inheritance. + //! Use parse() function to build a DOM tree from a zero-terminated XML text string. + //! parse() function allocates memory for nodes and attributes by using functions of xml_document, + //! which are inherited from memory_pool. + //! To access root node of the document, use the document itself, as if it was an xml_node. + //! \param Ch Character type to use. + template + class xml_document: public xml_node, public memory_pool + { + + public: + + //! Constructs empty XML document + xml_document() + : xml_node(node_document) + { + } + + //! Parses zero-terminated XML string according to given flags. + //! Passed string will be modified by the parser, unless rapidxml::parse_non_destructive flag is used. + //! The string must persist for the lifetime of the document. + //! In case of error, rapidxml::parse_error exception will be thrown. + //!

+ //! If you want to parse contents of a file, you must first load the file into the memory, and pass pointer to its beginning. + //! Make sure that data is zero-terminated. + //!

+ //! Document can be parsed into multiple times. + //! Each new call to parse removes previous nodes and attributes (if any), but does not clear memory pool. + //! \param text XML data to parse; pointer is non-const to denote fact that this data may be modified by the parser. + template + void parse(Ch *text) + { + assert(text); + + // Remove current contents + this->remove_all_nodes(); + this->remove_all_attributes(); + + // Parse BOM, if any + parse_bom(text); + + // Parse children + while (1) + { + // Skip whitespace before node + skip(text); + if (*text == 0) + break; + + // Parse and append new child + if (*text == Ch('<')) + { + ++text; // Skip '<' + if (xml_node *node = parse_node(text)) + this->append_node(node); + } + else + RAPIDXML_PARSE_ERROR("expected <", text); + } + + } + + //! Clears the document by deleting all nodes and clearing the memory pool. + //! All nodes owned by document pool are destroyed. + void clear() + { + this->remove_all_nodes(); + this->remove_all_attributes(); + memory_pool::clear(); + } + + private: + + /////////////////////////////////////////////////////////////////////// + // Internal character utility functions + + // Detect whitespace character + struct whitespace_pred + { + static unsigned char test(Ch ch) + { + return internal::lookup_tables<0>::lookup_whitespace[static_cast(ch)]; + } + }; + + // Detect node name character + struct node_name_pred + { + static unsigned char test(Ch ch) + { + return internal::lookup_tables<0>::lookup_node_name[static_cast(ch)]; + } + }; + + // Detect attribute name character + struct attribute_name_pred + { + static unsigned char test(Ch ch) + { + return internal::lookup_tables<0>::lookup_attribute_name[static_cast(ch)]; + } + }; + + // Detect text character (PCDATA) + struct text_pred + { + static unsigned char test(Ch ch) + { + return internal::lookup_tables<0>::lookup_text[static_cast(ch)]; + } + }; + + // Detect text character (PCDATA) that does not require processing + struct text_pure_no_ws_pred + { + static unsigned char test(Ch ch) + { + return internal::lookup_tables<0>::lookup_text_pure_no_ws[static_cast(ch)]; + } + }; + + // Detect text character (PCDATA) that does not require processing + struct text_pure_with_ws_pred + { + static unsigned char test(Ch ch) + { + return internal::lookup_tables<0>::lookup_text_pure_with_ws[static_cast(ch)]; + } + }; + + // Detect attribute value character + template + struct attribute_value_pred + { + static unsigned char test(Ch ch) + { + if (Quote == Ch('\'')) + return internal::lookup_tables<0>::lookup_attribute_data_1[static_cast(ch)]; + if (Quote == Ch('\"')) + return internal::lookup_tables<0>::lookup_attribute_data_2[static_cast(ch)]; + return 0; // Should never be executed, to avoid warnings on Comeau + } + }; + + // Detect attribute value character + template + struct attribute_value_pure_pred + { + static unsigned char test(Ch ch) + { + if (Quote == Ch('\'')) + return internal::lookup_tables<0>::lookup_attribute_data_1_pure[static_cast(ch)]; + if (Quote == Ch('\"')) + return internal::lookup_tables<0>::lookup_attribute_data_2_pure[static_cast(ch)]; + return 0; // Should never be executed, to avoid warnings on Comeau + } + }; + + // Insert coded character, using UTF8 or 8-bit ASCII + template + static void insert_coded_character(Ch *&text, unsigned long code) + { + if (Flags & parse_no_utf8) + { + // Insert 8-bit ASCII character + // Todo: possibly verify that code is less than 256 and use replacement char otherwise? + text[0] = static_cast(code); + text += 1; + } + else + { + // Insert UTF8 sequence + if (code < 0x80) // 1 byte sequence + { + text[0] = static_cast(code); + text += 1; + } + else if (code < 0x800) // 2 byte sequence + { + text[1] = static_cast((code | 0x80) & 0xBF); code >>= 6; + text[0] = static_cast(code | 0xC0); + text += 2; + } + else if (code < 0x10000) // 3 byte sequence + { + text[2] = static_cast((code | 0x80) & 0xBF); code >>= 6; + text[1] = static_cast((code | 0x80) & 0xBF); code >>= 6; + text[0] = static_cast(code | 0xE0); + text += 3; + } + else if (code < 0x110000) // 4 byte sequence + { + text[3] = static_cast((code | 0x80) & 0xBF); code >>= 6; + text[2] = static_cast((code | 0x80) & 0xBF); code >>= 6; + text[1] = static_cast((code | 0x80) & 0xBF); code >>= 6; + text[0] = static_cast(code | 0xF0); + text += 4; + } + else // Invalid, only codes up to 0x10FFFF are allowed in Unicode + { + RAPIDXML_PARSE_ERROR("invalid numeric character entity", text); + } + } + } + + // Skip characters until predicate evaluates to true + template + static void skip(Ch *&text) + { + Ch *tmp = text; + while (StopPred::test(*tmp)) + ++tmp; + text = tmp; + } + + // Skip characters until predicate evaluates to true while doing the following: + // - replacing XML character entity references with proper characters (' & " < > &#...;) + // - condensing whitespace sequences to single space character + template + static Ch *skip_and_expand_character_refs(Ch *&text) + { + // If entity translation, whitespace condense and whitespace trimming is disabled, use plain skip + if (Flags & parse_no_entity_translation && + !(Flags & parse_normalize_whitespace) && + !(Flags & parse_trim_whitespace)) + { + skip(text); + return text; + } + + // Use simple skip until first modification is detected + skip(text); + + // Use translation skip + Ch *src = text; + Ch *dest = src; + while (StopPred::test(*src)) + { + // If entity translation is enabled + if (!(Flags & parse_no_entity_translation)) + { + // Test if replacement is needed + if (src[0] == Ch('&')) + { + switch (src[1]) + { + + // & ' + case Ch('a'): + if (src[2] == Ch('m') && src[3] == Ch('p') && src[4] == Ch(';')) + { + *dest = Ch('&'); + ++dest; + src += 5; + continue; + } + if (src[2] == Ch('p') && src[3] == Ch('o') && src[4] == Ch('s') && src[5] == Ch(';')) + { + *dest = Ch('\''); + ++dest; + src += 6; + continue; + } + break; + + // " + case Ch('q'): + if (src[2] == Ch('u') && src[3] == Ch('o') && src[4] == Ch('t') && src[5] == Ch(';')) + { + *dest = Ch('"'); + ++dest; + src += 6; + continue; + } + break; + + // > + case Ch('g'): + if (src[2] == Ch('t') && src[3] == Ch(';')) + { + *dest = Ch('>'); + ++dest; + src += 4; + continue; + } + break; + + // < + case Ch('l'): + if (src[2] == Ch('t') && src[3] == Ch(';')) + { + *dest = Ch('<'); + ++dest; + src += 4; + continue; + } + break; + + // &#...; - assumes ASCII + case Ch('#'): + if (src[2] == Ch('x')) + { + unsigned long code = 0; + src += 3; // Skip &#x + while (1) + { + unsigned char digit = internal::lookup_tables<0>::lookup_digits[static_cast(*src)]; + if (digit == 0xFF) + break; + code = code * 16 + digit; + ++src; + } + insert_coded_character(dest, code); // Put character in output + } + else + { + unsigned long code = 0; + src += 2; // Skip &# + while (1) + { + unsigned char digit = internal::lookup_tables<0>::lookup_digits[static_cast(*src)]; + if (digit == 0xFF) + break; + code = code * 10 + digit; + ++src; + } + insert_coded_character(dest, code); // Put character in output + } + if (*src == Ch(';')) + ++src; + else + RAPIDXML_PARSE_ERROR("expected ;", src); + continue; + + // Something else + default: + // Ignore, just copy '&' verbatim + break; + + } + } + } + + // If whitespace condensing is enabled + if (Flags & parse_normalize_whitespace) + { + // Test if condensing is needed + if (whitespace_pred::test(*src)) + { + *dest = Ch(' '); ++dest; // Put single space in dest + ++src; // Skip first whitespace char + // Skip remaining whitespace chars + while (whitespace_pred::test(*src)) + ++src; + continue; + } + } + + // No replacement, only copy character + *dest++ = *src++; + + } + + // Return new end + text = src; + return dest; + + } + + /////////////////////////////////////////////////////////////////////// + // Internal parsing functions + + // Parse BOM, if any + template + void parse_bom(Ch *&text) + { + // UTF-8? + if (static_cast(text[0]) == 0xEF && + static_cast(text[1]) == 0xBB && + static_cast(text[2]) == 0xBF) + { + text += 3; // Skup utf-8 bom + } + } + + // Parse XML declaration ( + xml_node *parse_xml_declaration(Ch *&text) + { + // If parsing of declaration is disabled + if (!(Flags & parse_declaration_node)) + { + // Skip until end of declaration + while (text[0] != Ch('?') || text[1] != Ch('>')) + { + if (!text[0]) + RAPIDXML_PARSE_ERROR("unexpected end of data", text); + ++text; + } + text += 2; // Skip '?>' + return 0; + } + + // Create declaration + xml_node *declaration = this->allocate_node(node_declaration); + + // Skip whitespace before attributes or ?> + skip(text); + + // Parse declaration attributes + parse_node_attributes(text, declaration); + + // Skip ?> + if (text[0] != Ch('?') || text[1] != Ch('>')) + RAPIDXML_PARSE_ERROR("expected ?>", text); + text += 2; + + return declaration; + } + + // Parse XML comment (' + return 0; // Do not produce comment node + } + + // Remember value start + Ch *value = text; + + // Skip until end of comment + while (text[0] != Ch('-') || text[1] != Ch('-') || text[2] != Ch('>')) + { + if (!text[0]) + RAPIDXML_PARSE_ERROR("unexpected end of data", text); + ++text; + } + + // Create comment node + xml_node *comment = this->allocate_node(node_comment); + comment->value(value, text - value); + + // Place zero terminator after comment value + if (!(Flags & parse_no_string_terminators)) + *text = Ch('\0'); + + text += 3; // Skip '-->' + return comment; + } + + // Parse DOCTYPE + template + xml_node *parse_doctype(Ch *&text) + { + // Remember value start + Ch *value = text; + + // Skip to > + while (*text != Ch('>')) + { + // Determine character type + switch (*text) + { + + // If '[' encountered, scan for matching ending ']' using naive algorithm with depth + // This works for all W3C test files except for 2 most wicked + case Ch('['): + { + ++text; // Skip '[' + int depth = 1; + while (depth > 0) + { + switch (*text) + { + case Ch('['): ++depth; break; + case Ch(']'): --depth; break; + case 0: RAPIDXML_PARSE_ERROR("unexpected end of data", text); + } + ++text; + } + break; + } + + // Error on end of text + case Ch('\0'): + RAPIDXML_PARSE_ERROR("unexpected end of data", text); + + // Other character, skip it + default: + ++text; + + } + } + + // If DOCTYPE nodes enabled + if (Flags & parse_doctype_node) + { + // Create a new doctype node + xml_node *doctype = this->allocate_node(node_doctype); + doctype->value(value, text - value); + + // Place zero terminator after value + if (!(Flags & parse_no_string_terminators)) + *text = Ch('\0'); + + text += 1; // skip '>' + return doctype; + } + else + { + text += 1; // skip '>' + return 0; + } + + } + + // Parse PI + template + xml_node *parse_pi(Ch *&text) + { + // If creation of PI nodes is enabled + if (Flags & parse_pi_nodes) + { + // Create pi node + xml_node *pi = this->allocate_node(node_pi); + + // Extract PI target name + Ch *name = text; + skip(text); + if (text == name) + RAPIDXML_PARSE_ERROR("expected PI target", text); + pi->name(name, text - name); + + // Skip whitespace between pi target and pi + skip(text); + + // Remember start of pi + Ch *value = text; + + // Skip to '?>' + while (text[0] != Ch('?') || text[1] != Ch('>')) + { + if (*text == Ch('\0')) + RAPIDXML_PARSE_ERROR("unexpected end of data", text); + ++text; + } + + // Set pi value (verbatim, no entity expansion or whitespace normalization) + pi->value(value, text - value); + + // Place zero terminator after name and value + if (!(Flags & parse_no_string_terminators)) + { + pi->name()[pi->name_size()] = Ch('\0'); + pi->value()[pi->value_size()] = Ch('\0'); + } + + text += 2; // Skip '?>' + return pi; + } + else + { + // Skip to '?>' + while (text[0] != Ch('?') || text[1] != Ch('>')) + { + if (*text == Ch('\0')) + RAPIDXML_PARSE_ERROR("unexpected end of data", text); + ++text; + } + text += 2; // Skip '?>' + return 0; + } + } + + // Parse and append data + // Return character that ends data. + // This is necessary because this character might have been overwritten by a terminating 0 + template + Ch parse_and_append_data(xml_node *node, Ch *&text, Ch *contents_start) + { + // Backup to contents start if whitespace trimming is disabled + if (!(Flags & parse_trim_whitespace)) + text = contents_start; + + // Skip until end of data + Ch *value = text, *end; + if (Flags & parse_normalize_whitespace) + end = skip_and_expand_character_refs(text); + else + end = skip_and_expand_character_refs(text); + + // Trim trailing whitespace if flag is set; leading was already trimmed by whitespace skip after > + if (Flags & parse_trim_whitespace) + { + if (Flags & parse_normalize_whitespace) + { + // Whitespace is already condensed to single space characters by skipping function, so just trim 1 char off the end + if (*(end - 1) == Ch(' ')) + --end; + } + else + { + // Backup until non-whitespace character is found + while (whitespace_pred::test(*(end - 1))) + --end; + } + } + + // If characters are still left between end and value (this test is only necessary if normalization is enabled) + // Create new data node + if (!(Flags & parse_no_data_nodes)) + { + xml_node *data = this->allocate_node(node_data); + data->value(value, end - value); + node->append_node(data); + } + + // Add data to parent node if no data exists yet + if (!(Flags & parse_no_element_values)) + if (*node->value() == Ch('\0')) + node->value(value, end - value); + + // Place zero terminator after value + if (!(Flags & parse_no_string_terminators)) + { + Ch ch = *text; + *end = Ch('\0'); + return ch; // Return character that ends data; this is required because zero terminator overwritten it + } + + // Return character that ends data + return *text; + } + + // Parse CDATA + template + xml_node *parse_cdata(Ch *&text) + { + // If CDATA is disabled + if (Flags & parse_no_data_nodes) + { + // Skip until end of cdata + while (text[0] != Ch(']') || text[1] != Ch(']') || text[2] != Ch('>')) + { + if (!text[0]) + RAPIDXML_PARSE_ERROR("unexpected end of data", text); + ++text; + } + text += 3; // Skip ]]> + return 0; // Do not produce CDATA node + } + + // Skip until end of cdata + Ch *value = text; + while (text[0] != Ch(']') || text[1] != Ch(']') || text[2] != Ch('>')) + { + if (!text[0]) + RAPIDXML_PARSE_ERROR("unexpected end of data", text); + ++text; + } + + // Create new cdata node + xml_node *cdata = this->allocate_node(node_cdata); + cdata->value(value, text - value); + + // Place zero terminator after value + if (!(Flags & parse_no_string_terminators)) + *text = Ch('\0'); + + text += 3; // Skip ]]> + return cdata; + } + + // Parse element node + template + xml_node *parse_element(Ch *&text) + { + // Create element node + xml_node *element = this->allocate_node(node_element); + + // Extract element name + Ch *name = text; + skip(text); + if (text == name) + RAPIDXML_PARSE_ERROR("expected element name", text); + element->name(name, text - name); + + // Skip whitespace between element name and attributes or > + skip(text); + + // Parse attributes, if any + parse_node_attributes(text, element); + + // Determine ending type + if (*text == Ch('>')) + { + ++text; + parse_node_contents(text, element); + } + else if (*text == Ch('/')) + { + ++text; + if (*text != Ch('>')) + RAPIDXML_PARSE_ERROR("expected >", text); + ++text; + } + else + RAPIDXML_PARSE_ERROR("expected >", text); + + // Place zero terminator after name + if (!(Flags & parse_no_string_terminators)) + element->name()[element->name_size()] = Ch('\0'); + + // Return parsed element + return element; + } + + // Determine node type, and parse it + template + xml_node *parse_node(Ch *&text) + { + // Parse proper node type + switch (text[0]) + { + + // <... + default: + // Parse and append element node + return parse_element(text); + + // (text); + } + else + { + // Parse PI + return parse_pi(text); + } + + // (text); + } + break; + + // (text); + } + break; + + // (text); + } + + } // switch + + // Attempt to skip other, unrecognized node types starting with ')) + { + if (*text == 0) + RAPIDXML_PARSE_ERROR("unexpected end of data", text); + ++text; + } + ++text; // Skip '>' + return 0; // No node recognized + + } + } + + // Parse contents of the node - children, data etc. + template + void parse_node_contents(Ch *&text, xml_node *node) + { + // For all children and text + while (1) + { + // Skip whitespace between > and node contents + Ch *contents_start = text; // Store start of node contents before whitespace is skipped + skip(text); + Ch next_char = *text; + + // After data nodes, instead of continuing the loop, control jumps here. + // This is because zero termination inside parse_and_append_data() function + // would wreak havoc with the above code. + // Also, skipping whitespace after data nodes is unnecessary. + after_data_node: + + // Determine what comes next: node closing, child node, data node, or 0? + switch (next_char) + { + + // Node closing or child node + case Ch('<'): + if (text[1] == Ch('/')) + { + // Node closing + text += 2; // Skip '(text); + if (!internal::compare(node->name(), node->name_size(), closing_name, text - closing_name, true)) + RAPIDXML_PARSE_ERROR("invalid closing tag name", text); + } + else + { + // No validation, just skip name + skip(text); + } + // Skip remaining whitespace after node name + skip(text); + if (*text != Ch('>')) + RAPIDXML_PARSE_ERROR("expected >", text); + ++text; // Skip '>' + return; // Node closed, finished parsing contents + } + else + { + // Child node + ++text; // Skip '<' + if (xml_node *child = parse_node(text)) + node->append_node(child); + } + break; + + // End of data - error + case Ch('\0'): + RAPIDXML_PARSE_ERROR("unexpected end of data", text); + + // Data node + default: + next_char = parse_and_append_data(node, text, contents_start); + goto after_data_node; // Bypass regular processing after data nodes + + } + } + } + + // Parse XML attributes of the node + template + void parse_node_attributes(Ch *&text, xml_node *node) + { + // For all attributes + while (attribute_name_pred::test(*text)) + { + // Extract attribute name + Ch *name = text; + ++text; // Skip first character of attribute name + skip(text); + if (text == name) + RAPIDXML_PARSE_ERROR("expected attribute name", name); + + // Create new attribute + xml_attribute *attribute = this->allocate_attribute(); + attribute->name(name, text - name); + node->append_attribute(attribute); + + // Skip whitespace after attribute name + skip(text); + + // Skip = + if (*text != Ch('=')) + RAPIDXML_PARSE_ERROR("expected =", text); + ++text; + + // Add terminating zero after name + if (!(Flags & parse_no_string_terminators)) + attribute->name()[attribute->name_size()] = 0; + + // Skip whitespace after = + skip(text); + + // Skip quote and remember if it was ' or " + Ch quote = *text; + if (quote != Ch('\'') && quote != Ch('"')) + RAPIDXML_PARSE_ERROR("expected ' or \"", text); + ++text; + + // Extract attribute value and expand char refs in it + Ch *value = text, *end; + const int AttFlags = Flags & ~parse_normalize_whitespace; // No whitespace normalization in attributes + if (quote == Ch('\'')) + end = skip_and_expand_character_refs, attribute_value_pure_pred, AttFlags>(text); + else + end = skip_and_expand_character_refs, attribute_value_pure_pred, AttFlags>(text); + + // Set attribute value + attribute->value(value, end - value); + + // Make sure that end quote is present + if (*text != quote) + RAPIDXML_PARSE_ERROR("expected ' or \"", text); + ++text; // Skip quote + + // Add terminating zero after value + if (!(Flags & parse_no_string_terminators)) + attribute->value()[attribute->value_size()] = 0; + + // Skip whitespace after attribute value + skip(text); + } + } + + }; + + //! \cond internal + namespace internal + { + + // Whitespace (space \n \r \t) + template + const unsigned char lookup_tables::lookup_whitespace[256] = + { + // 0 1 2 3 4 5 6 7 8 9 A B C D E F + 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 1, 0, 0, // 0 + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // 1 + 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // 2 + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // 3 + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // 4 + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // 5 + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // 6 + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // 7 + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // 8 + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // 9 + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // A + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // B + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // C + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // D + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // E + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 // F + }; + + // Node name (anything but space \n \r \t / > ? \0) + template + const unsigned char lookup_tables::lookup_node_name[256] = + { + // 0 1 2 3 4 5 6 7 8 9 A B C D E F + 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 1, 1, 0, 1, 1, // 0 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 1 + 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, // 2 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, // 3 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 4 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 5 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 6 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 7 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 8 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 9 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // A + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // B + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // C + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // D + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // E + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 // F + }; + + // Text (i.e. PCDATA) (anything but < \0) + template + const unsigned char lookup_tables::lookup_text[256] = + { + // 0 1 2 3 4 5 6 7 8 9 A B C D E F + 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 1 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 2 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, // 3 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 4 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 5 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 6 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 7 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 8 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 9 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // A + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // B + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // C + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // D + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // E + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 // F + }; + + // Text (i.e. PCDATA) that does not require processing when ws normalization is disabled + // (anything but < \0 &) + template + const unsigned char lookup_tables::lookup_text_pure_no_ws[256] = + { + // 0 1 2 3 4 5 6 7 8 9 A B C D E F + 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 1 + 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 2 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, // 3 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 4 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 5 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 6 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 7 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 8 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 9 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // A + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // B + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // C + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // D + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // E + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 // F + }; + + // Text (i.e. PCDATA) that does not require processing when ws normalizationis is enabled + // (anything but < \0 & space \n \r \t) + template + const unsigned char lookup_tables::lookup_text_pure_with_ws[256] = + { + // 0 1 2 3 4 5 6 7 8 9 A B C D E F + 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 1, 1, 0, 1, 1, // 0 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 1 + 0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 2 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, // 3 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 4 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 5 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 6 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 7 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 8 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 9 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // A + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // B + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // C + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // D + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // E + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 // F + }; + + // Attribute name (anything but space \n \r \t / < > = ? ! \0) + template + const unsigned char lookup_tables::lookup_attribute_name[256] = + { + // 0 1 2 3 4 5 6 7 8 9 A B C D E F + 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 1, 1, 0, 1, 1, // 0 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 1 + 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, // 2 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, // 3 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 4 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 5 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 6 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 7 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 8 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 9 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // A + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // B + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // C + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // D + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // E + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 // F + }; + + // Attribute data with single quote (anything but ' \0) + template + const unsigned char lookup_tables::lookup_attribute_data_1[256] = + { + // 0 1 2 3 4 5 6 7 8 9 A B C D E F + 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 1 + 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, // 2 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 3 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 4 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 5 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 6 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 7 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 8 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 9 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // A + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // B + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // C + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // D + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // E + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 // F + }; + + // Attribute data with single quote that does not require processing (anything but ' \0 &) + template + const unsigned char lookup_tables::lookup_attribute_data_1_pure[256] = + { + // 0 1 2 3 4 5 6 7 8 9 A B C D E F + 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 1 + 1, 1, 1, 1, 1, 1, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, // 2 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 3 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 4 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 5 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 6 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 7 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 8 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 9 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // A + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // B + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // C + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // D + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // E + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 // F + }; + + // Attribute data with double quote (anything but " \0) + template + const unsigned char lookup_tables::lookup_attribute_data_2[256] = + { + // 0 1 2 3 4 5 6 7 8 9 A B C D E F + 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 1 + 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 2 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 3 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 4 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 5 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 6 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 7 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 8 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 9 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // A + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // B + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // C + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // D + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // E + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 // F + }; + + // Attribute data with double quote that does not require processing (anything but " \0 &) + template + const unsigned char lookup_tables::lookup_attribute_data_2_pure[256] = + { + // 0 1 2 3 4 5 6 7 8 9 A B C D E F + 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 1 + 1, 1, 0, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 2 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 3 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 4 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 5 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 6 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 7 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 8 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 9 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // A + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // B + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // C + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // D + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // E + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 // F + }; + + // Digits (dec and hex, 255 denotes end of numeric character reference) + template + const unsigned char lookup_tables::lookup_digits[256] = + { + // 0 1 2 3 4 5 6 7 8 9 A B C D E F + 255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, // 0 + 255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, // 1 + 255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, // 2 + 0, 1, 2, 3, 4, 5, 6, 7, 8, 9,255,255,255,255,255,255, // 3 + 255, 10, 11, 12, 13, 14, 15,255,255,255,255,255,255,255,255,255, // 4 + 255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, // 5 + 255, 10, 11, 12, 13, 14, 15,255,255,255,255,255,255,255,255,255, // 6 + 255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, // 7 + 255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, // 8 + 255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, // 9 + 255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, // A + 255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, // B + 255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, // C + 255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, // D + 255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, // E + 255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255 // F + }; + + // Upper case conversion + template + const unsigned char lookup_tables::lookup_upcase[256] = + { + // 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, A B C D E F + 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, // 0 + 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, // 1 + 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, // 2 + 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, // 3 + 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, // 4 + 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94, 95, // 5 + 96, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, // 6 + 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 123,124,125,126,127, // 7 + 128,129,130,131,132,133,134,135,136,137,138,139,140,141,142,143, // 8 + 144,145,146,147,148,149,150,151,152,153,154,155,156,157,158,159, // 9 + 160,161,162,163,164,165,166,167,168,169,170,171,172,173,174,175, // A + 176,177,178,179,180,181,182,183,184,185,186,187,188,189,190,191, // B + 192,193,194,195,196,197,198,199,200,201,202,203,204,205,206,207, // C + 208,209,210,211,212,213,214,215,216,217,218,219,220,221,222,223, // D + 224,225,226,227,228,229,230,231,232,233,234,235,236,237,238,239, // E + 240,241,242,243,244,245,246,247,248,249,250,251,252,253,254,255 // F + }; + } + //! \endcond + +} + +// Undefine internal macros +#undef RAPIDXML_PARSE_ERROR + +// On MSVC, restore warnings state +#ifdef _MSC_VER + #pragma warning(pop) +#endif + +#endif diff --git a/livox_ros_driver/common/rapidxml/rapidxml_iterators.hpp b/livox_ros_driver/common/rapidxml/rapidxml_iterators.hpp new file mode 100644 index 0000000..52ebc29 --- /dev/null +++ b/livox_ros_driver/common/rapidxml/rapidxml_iterators.hpp @@ -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 node_iterator + { + + public: + + typedef typename xml_node value_type; + typedef typename xml_node &reference; + typedef typename xml_node *pointer; + typedef std::ptrdiff_t difference_type; + typedef std::bidirectional_iterator_tag iterator_category; + + node_iterator() + : m_node(0) + { + } + + node_iterator(xml_node *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 &rhs) + { + return m_node == rhs.m_node; + } + + bool operator !=(const node_iterator &rhs) + { + return m_node != rhs.m_node; + } + + private: + + xml_node *m_node; + + }; + + //! Iterator of child attributes of xml_node + template + class attribute_iterator + { + + public: + + typedef typename xml_attribute value_type; + typedef typename xml_attribute &reference; + typedef typename xml_attribute *pointer; + typedef std::ptrdiff_t difference_type; + typedef std::bidirectional_iterator_tag iterator_category; + + attribute_iterator() + : m_attribute(0) + { + } + + attribute_iterator(xml_node *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 &rhs) + { + return m_attribute == rhs.m_attribute; + } + + bool operator !=(const attribute_iterator &rhs) + { + return m_attribute != rhs.m_attribute; + } + + private: + + xml_attribute *m_attribute; + + }; + +} + +#endif diff --git a/livox_ros_driver/common/rapidxml/rapidxml_print.hpp b/livox_ros_driver/common/rapidxml/rapidxml_print.hpp new file mode 100644 index 0000000..0ae2b14 --- /dev/null +++ b/livox_ros_driver/common/rapidxml/rapidxml_print.hpp @@ -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 + #include +#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 + 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 (< > ' " &) + template + 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 + inline OutIt fill_chars(OutIt out, int n, Ch ch) + { + for (int i = 0; i < n; ++i) + *out++ = ch; + return out; + } + + // Find character + template + 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 + inline OutIt print_node(OutIt out, const xml_node *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 + inline OutIt print_children(OutIt out, const xml_node *node, int flags, int indent) + { + for (xml_node *child = node->first_node(); child; child = child->next_sibling()) + out = print_node(out, child, flags, indent); + return out; + } + + // Print attributes of the node + template + inline OutIt print_attributes(OutIt out, const xml_node *node, int flags) + { + for (xml_attribute *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(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 + inline OutIt print_data_node(OutIt out, const xml_node *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 + inline OutIt print_cdata_node(OutIt out, const xml_node *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 + inline OutIt print_element_node(OutIt out, const xml_node *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 *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 + inline OutIt print_declaration_node(OutIt out, const xml_node *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 + inline OutIt print_comment_node(OutIt out, const xml_node *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 + inline OutIt print_doctype_node(OutIt out, const xml_node *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 + inline OutIt print_pi_node(OutIt out, const xml_node *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 + inline OutIt print(OutIt out, const xml_node &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 + inline std::basic_ostream &print(std::basic_ostream &out, const xml_node &node, int flags = 0) + { + print(std::ostream_iterator(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 + inline std::basic_ostream &operator <<(std::basic_ostream &out, const xml_node &node) + { + return print(out, node); + } + +#endif + +} + +#endif diff --git a/livox_ros_driver/common/rapidxml/rapidxml_utils.hpp b/livox_ros_driver/common/rapidxml/rapidxml_utils.hpp new file mode 100644 index 0000000..37c2953 --- /dev/null +++ b/livox_ros_driver/common/rapidxml/rapidxml_utils.hpp @@ -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 +#include +#include +#include + +namespace rapidxml +{ + + //! Represents data loaded from a file + template + 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 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(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 &stream) + { + using namespace std; + + // Load data and add terminating 0 + stream.unsetf(ios::skipws); + m_data.assign(istreambuf_iterator(stream), istreambuf_iterator()); + 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 m_data; // File data + + }; + + //! Counts children of node. Time complexity is O(n). + //! \return Number of children of node + template + inline std::size_t count_children(xml_node *node) + { + xml_node *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 + inline std::size_t count_attributes(xml_node *node) + { + xml_attribute *attr = node->first_attribute(); + std::size_t count = 0; + while (attr) + { + ++count; + attr = attr->next_attribute(); + } + return count; + } + +} + +#endif diff --git a/livox_ros_driver/config/display_hub_points.rviz b/livox_ros_driver/config/display_hub_points.rviz index aa01297..32cc93d 100644 --- a/livox_ros_driver/config/display_hub_points.rviz +++ b/livox_ros_driver/config/display_hub_points.rviz @@ -9,8 +9,8 @@ Panels: - /Grid1 - /PointCloud1/Autocompute Value Bounds1 - /PointCloud21 - Splitter Ratio: 0.500695 - Tree Height: 747 + Splitter Ratio: 0.500694990158081 + Tree Height: 728 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -19,7 +19,7 @@ Panels: - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties - Splitter Ratio: 0.588679 + Splitter Ratio: 0.5886790156364441 - Class: rviz/Views Expanded: - /Current View1 @@ -30,6 +30,10 @@ Panels: Name: Time SyncMode: 0 SyncSource: PointCloud2 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 Visualization Manager: Class: "" Displays: @@ -39,7 +43,7 @@ Visualization Manager: Color: 160; 160; 164 Enabled: true Line Style: - Line Width: 0.03 + Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 @@ -54,8 +58,8 @@ Visualization Manager: - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 0.856 - Min Value: -0.735 + Max Value: 0.8560000061988831 + Min Value: -0.7350000143051147 Value: true Axis: Z Channel Name: x @@ -66,15 +70,15 @@ Visualization Manager: Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: -0.088 + Max Intensity: -0.08799999952316284 Min Color: 0; 0; 0 - Min Intensity: -1.951 + Min Intensity: -1.9509999752044678 Name: PointCloud Position Transformer: XYZ Queue Size: 1000 Selectable: true Size (Pixels): 2 - Size (m): 0.005 + Size (m): 0.004999999888241291 Style: Flat Squares Topic: /livox/lidar Unreliable: false @@ -84,8 +88,8 @@ Visualization Manager: - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 0.816 - Min Value: -0.674 + Max Value: 0.8159999847412109 + Min Value: -0.6740000247955322 Value: true Axis: Z Channel Name: intensity @@ -96,17 +100,17 @@ Visualization Manager: Enabled: true Invert Rainbow: true Max Color: 255; 255; 255 - Max Intensity: 151 + Max Intensity: 152 Min Color: 0; 0; 0 Min Intensity: 0 Name: PointCloud2 Position Transformer: XYZ - Queue Size: 1000 + Queue Size: 10 Selectable: true - Size (Pixels): 3 - Size (m): 0.005 - Style: Flat Squares - Topic: /livox/hub + Size (Pixels): 2 + Size (m): 0.004999999888241291 + Style: Points + Topic: /livox/lidar Unreliable: false Use Fixed Frame: true Use rainbow: true @@ -114,8 +118,9 @@ Visualization Manager: Enabled: true Global Options: Background Color: 48; 48; 48 + Default Light: true Fixed Frame: livox_frame - Frame Rate: 20 + Frame Rate: 50 Name: root Tools: - Class: rviz/Interact @@ -125,7 +130,10 @@ Visualization Manager: - Class: rviz/FocusCamera - Class: rviz/Measure - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 - Class: rviz/SetGoal Topic: /move_base_simple/goal - Class: rviz/PublishPoint @@ -135,27 +143,30 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 1.61081 + Distance: 29.202434539794922 Enable Stereo Rendering: - Stereo Eye Separation: 0.06 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0.267255 - Y: 0.0618536 - Z: 0.150874 + X: 0.2672550082206726 + Y: 0.061853598803281784 + Z: 0.15087400376796722 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false Name: Current View - Near Clip Distance: 0.01 - Pitch: 0.3048 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.679796040058136 Target Frame: Value: Orbit (rviz) - Yaw: 3.01742 + Yaw: 3.0174102783203125 Saved: - Class: rviz/Orbit Distance: 10 Enable Stereo Rendering: - Stereo Eye Separation: 0.06 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false @@ -163,19 +174,22 @@ Visualization Manager: X: 0 Y: 0 Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false Name: Orbit - Near Clip Distance: 0.01 - Pitch: 1.1104 + Near Clip Distance: 0.009999999776482582 + Pitch: 1.1103999614715576 Target Frame: Value: Orbit (rviz) - Yaw: 0.570397 + Yaw: 0.5703970193862915 Window Geometry: Displays: collapsed: false - Height: 1028 + Height: 1025 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001830000037afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000037a000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005110000003efc0100000002fb0000000800540069006d0065010000000000000511000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000003880000037a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001a900000363fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000363000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000058e0000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -184,6 +198,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1297 - X: 209 - Y: 14 + Width: 1853 + X: 67 + Y: 27 diff --git a/livox_ros_driver/config/display_lidar_points.rviz b/livox_ros_driver/config/display_lidar_points.rviz index 6ed297d..6239170 100644 --- a/livox_ros_driver/config/display_lidar_points.rviz +++ b/livox_ros_driver/config/display_lidar_points.rviz @@ -7,10 +7,9 @@ Panels: - /Global Options1 - /Status1 - /Grid1 - - /PointCloud1/Autocompute Value Bounds1 - /PointCloud21 - Splitter Ratio: 0.500695 - Tree Height: 747 + Splitter Ratio: 0.500694990158081 + Tree Height: 728 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -19,7 +18,7 @@ Panels: - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties - Splitter Ratio: 0.588679 + Splitter Ratio: 0.5886790156364441 - Class: rviz/Views Expanded: - /Current View1 @@ -30,6 +29,10 @@ Panels: Name: Time SyncMode: 0 SyncSource: PointCloud2 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 Visualization Manager: Class: "" Displays: @@ -39,7 +42,7 @@ Visualization Manager: Color: 160; 160; 164 Enabled: true Line Style: - Line Width: 0.03 + Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 @@ -54,58 +57,28 @@ Visualization Manager: - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 0.856 - Min Value: -0.735 - 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 + Max Value: 0.4569999873638153 + Min Value: -0.367000013589859 Value: true Axis: Z Channel Name: intensity Class: rviz/PointCloud2 Color: 255; 255; 255 - Color Transformer: AxisColor + Color Transformer: Intensity Decay Time: 1 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 151 + Max Intensity: 255 Min Color: 0; 0; 0 Min Intensity: 0 Name: PointCloud2 Position Transformer: XYZ - Queue Size: 100 + Queue Size: 10 Selectable: true - Size (Pixels): 3 - Size (m): 0.005 - Style: Flat Squares + Size (Pixels): 2 + Size (m): 0.004999999888241291 + Style: Points Topic: /livox/lidar Unreliable: false Use Fixed Frame: true @@ -114,8 +87,9 @@ Visualization Manager: Enabled: true Global Options: Background Color: 48; 48; 48 + Default Light: true Fixed Frame: livox_frame - Frame Rate: 20 + Frame Rate: 40 Name: root Tools: - Class: rviz/Interact @@ -125,7 +99,10 @@ Visualization Manager: - Class: rviz/FocusCamera - Class: rviz/Measure - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 - Class: rviz/SetGoal Topic: /move_base_simple/goal - Class: rviz/PublishPoint @@ -135,27 +112,30 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 0.591995 + Distance: 25.80008888244629 Enable Stereo Rendering: - Stereo Eye Separation: 0.06 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0.267255 - Y: 0.0618536 - Z: 0.150874 + X: 0.2672550082206726 + Y: 0.061853598803281784 + Z: 0.15087400376796722 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false Name: Current View - Near Clip Distance: 0.01 - Pitch: 0.229799 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5597995519638062 Target Frame: Value: Orbit (rviz) - Yaw: 2.99561 + Yaw: 3.065610408782959 Saved: - Class: rviz/Orbit Distance: 10 Enable Stereo Rendering: - Stereo Eye Separation: 0.06 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false @@ -163,19 +143,22 @@ Visualization Manager: X: 0 Y: 0 Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false Name: Orbit - Near Clip Distance: 0.01 - Pitch: 1.1104 + Near Clip Distance: 0.009999999776482582 + Pitch: 1.1103999614715576 Target Frame: Value: Orbit (rviz) - Yaw: 0.570397 + Yaw: 0.5703970193862915 Window Geometry: Displays: collapsed: false - Height: 1028 + Height: 1025 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001830000037afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000037a000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005110000003efc0100000002fb0000000800540069006d0065010000000000000511000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000003880000037a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001c400000363fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000363000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005730000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -184,6 +167,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1297 - X: 407 - Y: 14 + Width: 1853 + X: 67 + Y: 27 diff --git a/livox_ros_driver/config/livox_hub_config.json b/livox_ros_driver/config/livox_hub_config.json new file mode 100644 index 0000000..bc92e95 --- /dev/null +++ b/livox_ros_driver/config/livox_hub_config.json @@ -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 + } + ] +} diff --git a/livox_ros_driver/config/livox_lidar_config.json b/livox_ros_driver/config/livox_lidar_config.json new file mode 100644 index 0000000..5063524 --- /dev/null +++ b/livox_ros_driver/config/livox_lidar_config.json @@ -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 + } +} diff --git a/livox_ros_driver/launch/livox_hub.launch b/livox_ros_driver/launch/livox_hub.launch index 669142c..b3f670b 100644 --- a/livox_ros_driver/launch/livox_hub.launch +++ b/livox_ros_driver/launch/livox_hub.launch @@ -1,14 +1,37 @@ - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/livox_ros_driver/launch/livox_hub_msg.launch b/livox_ros_driver/launch/livox_hub_msg.launch index 669142c..34d2302 100644 --- a/livox_ros_driver/launch/livox_hub_msg.launch +++ b/livox_ros_driver/launch/livox_hub_msg.launch @@ -1,14 +1,37 @@ - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/livox_ros_driver/launch/livox_hub_rviz.launch b/livox_ros_driver/launch/livox_hub_rviz.launch index faa9af0..bb3c774 100644 --- a/livox_ros_driver/launch/livox_hub_rviz.launch +++ b/livox_ros_driver/launch/livox_hub_rviz.launch @@ -1,12 +1,37 @@ - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - diff --git a/livox_ros_driver/launch/livox_lidar.launch b/livox_ros_driver/launch/livox_lidar.launch index a78e3ef..4b9bd91 100644 --- a/livox_ros_driver/launch/livox_lidar.launch +++ b/livox_ros_driver/launch/livox_lidar.launch @@ -1,13 +1,37 @@ - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/livox_ros_driver/launch/livox_lidar_msg.launch b/livox_ros_driver/launch/livox_lidar_msg.launch index 3858614..e89a8cd 100644 --- a/livox_ros_driver/launch/livox_lidar_msg.launch +++ b/livox_ros_driver/launch/livox_lidar_msg.launch @@ -1,13 +1,37 @@ - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/livox_ros_driver/launch/livox_lidar_rviz.launch b/livox_ros_driver/launch/livox_lidar_rviz.launch index ce5cb98..8b51ef5 100644 --- a/livox_ros_driver/launch/livox_lidar_rviz.launch +++ b/livox_ros_driver/launch/livox_lidar_rviz.launch @@ -1,12 +1,37 @@ - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - diff --git a/livox_ros_driver/launch/livox_template.launch b/livox_ros_driver/launch/livox_template.launch new file mode 100644 index 0000000..d0ebdf4 --- /dev/null +++ b/livox_ros_driver/launch/livox_template.launch @@ -0,0 +1,37 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/livox_ros_driver/launch/lvx_to_rosbag.launch b/livox_ros_driver/launch/lvx_to_rosbag.launch new file mode 100644 index 0000000..34c0ae6 --- /dev/null +++ b/livox_ros_driver/launch/lvx_to_rosbag.launch @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/livox_ros_driver/launch/lvx_to_rosbag_rviz.launch b/livox_ros_driver/launch/lvx_to_rosbag_rviz.launch new file mode 100644 index 0000000..31a5ae4 --- /dev/null +++ b/livox_ros_driver/launch/lvx_to_rosbag_rviz.launch @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/livox_ros_driver/livox_hub.cpp b/livox_ros_driver/livox_hub.cpp deleted file mode 100755 index 13e5c97..0000000 --- a/livox_ros_driver/livox_hub.cpp +++ /dev/null @@ -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 -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include "livox_sdk.h" -#include -#include -#include -#include -#include - - -/* 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 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 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(eth_packet), \ - GetEthPacketLen(eth_packet, point_num)); - - queue->wr_idx++; - - return 1; -} - -static uint64_t GetStoragePacketTimestamp(StoragePacket* packet) { - - LivoxEthPacket* raw_packet = reinterpret_cast(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(storage_packet.raw_data); - LivoxRawPoint* raw_points = reinterpret_cast(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(storage_packet.raw_data); - LivoxRawPoint* raw_points = reinterpret_cast(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(storage_packet.raw_data); - LivoxRawPoint* raw_points = reinterpret_cast(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("livox/hub", kMaxEthPacketQueueSize); - ROS_INFO("Publish PointCloud2"); - } else { - cloud_pub = livox_node.advertise("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(); -} - - diff --git a/livox_ros_driver/livox_lidar.cpp b/livox_ros_driver/livox_lidar.cpp deleted file mode 100755 index d915c69..0000000 --- a/livox_ros_driver/livox_lidar.cpp +++ /dev/null @@ -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 -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include "livox_sdk.h" -#include -#include -#include -#include -#include - - -/* 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 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 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(eth_packet), \ - GetEthPacketLen(eth_packet, point_num)); - - queue->wr_idx++; - - return 1; -} - -static uint64_t GetStoragePacketTimestamp(StoragePacket* packet) { - - LivoxEthPacket* raw_packet = reinterpret_cast(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(storage_packet.raw_data); - LivoxRawPoint* raw_points = reinterpret_cast(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(storage_packet.raw_data); - LivoxRawPoint* raw_points = reinterpret_cast(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(storage_packet.raw_data); - LivoxRawPoint* raw_points = reinterpret_cast(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("livox/lidar", \ - kMaxEthPacketQueueSize); - ROS_INFO("Publish PointCloud2"); - } else { - cloud_pub = livox_node.advertise("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(); -} - - diff --git a/livox_ros_driver/livox_ros_driver/include/livox_ros_driver.h b/livox_ros_driver/livox_ros_driver/include/livox_ros_driver.h new file mode 100644 index 0000000..f80ae1a --- /dev/null +++ b/livox_ros_driver/livox_ros_driver/include/livox_ros_driver.h @@ -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 + diff --git a/livox_ros_driver/livox_ros_driver/lddc.cpp b/livox_ros_driver/livox_ros_driver/lddc.cpp new file mode 100644 index 0000000..f7b1e35 --- /dev/null +++ b/livox_ros_driver/livox_ros_driver/lddc.cpp @@ -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 +#include +#include + +#include "lds_lvx.h" +#include "lds_lidar.h" +#include +#include +#include +#include +#include +#include +#include + +namespace livox_ros { + +typedef pcl::PointCloud 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; ilidars_[handle].data_src; + StoragePacket storage_packet; + while (published_packet < packet_num) { + QueueProPop(queue, &storage_packet); + LivoxEthPacket* raw_packet = reinterpret_cast(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(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(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(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(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(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(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(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; + } +} + +} diff --git a/livox_ros_driver/livox_ros_driver/lddc.h b/livox_ros_driver/livox_ros_driver/lddc.h new file mode 100644 index 0000000..f0851ec --- /dev/null +++ b/livox_ros_driver/livox_ros_driver/lddc.h @@ -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 +#include + +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 diff --git a/livox_ros_driver/livox_ros_driver/ldq.cpp b/livox_ros_driver/livox_ros_driver/ldq.cpp new file mode 100644 index 0000000..5451bdd --- /dev/null +++ b/livox_ros_driver/livox_ros_driver/ldq.cpp @@ -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 +#include + +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; +} + +} diff --git a/livox_ros_driver/livox_ros_driver/ldq.h b/livox_ros_driver/livox_ros_driver/ldq.h new file mode 100644 index 0000000..21490d8 --- /dev/null +++ b/livox_ros_driver/livox_ros_driver/ldq.h @@ -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 + +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 diff --git a/livox_ros_driver/livox_ros_driver/lds.cpp b/livox_ros_driver/livox_ros_driver/lds.cpp new file mode 100644 index 0000000..eb1e619 --- /dev/null +++ b/livox_ros_driver/livox_ros_driver/lds.cpp @@ -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 +#include +#include +#include +#include + +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(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& 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(euler[0])); + double cos_pitch = cos(static_cast(euler[1])); + double cos_yaw = cos(static_cast(euler[2])); + double sin_roll = sin(static_cast(euler[0])); + double sin_pitch = sin(static_cast(euler[1])); + double sin_yaw = sin(static_cast(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(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(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(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(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(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(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(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 +#include +#include +#include +#include +#include + +#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& 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 diff --git a/livox_ros_driver/livox_ros_driver/lds_hub.cpp b/livox_ros_driver/livox_ros_driver/lds_hub.cpp new file mode 100644 index 0000000..612419f --- /dev/null +++ b/livox_ros_driver/livox_ros_driver/lds_hub.cpp @@ -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 +#include +#include +#include + +#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& 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(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(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(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(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(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(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 +#include + +#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& 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 lidar_raw_config_; + UserRawConfig hub_raw_config_; +}; + +} +#endif diff --git a/livox_ros_driver/livox_ros_driver/lds_lidar.cpp b/livox_ros_driver/livox_ros_driver/lds_lidar.cpp new file mode 100644 index 0000000..f67d436 --- /dev/null +++ b/livox_ros_driver/livox_ros_driver/lds_lidar.cpp @@ -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 +#include +#include +#include + +#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& 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; iInitTimeSync(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(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(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(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(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(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(response->roll* PI / 180.0); + p_extrinsic->euler[1] = static_cast(response->pitch * PI / 180.0); + p_extrinsic->euler[2] = static_cast(response->yaw * PI / 180.0); + p_extrinsic->trans[0] = static_cast(response->x / 1000.0); + p_extrinsic->trans[1] = static_cast(response->y / 1000.0); + p_extrinsic->trans[2] = static_cast(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(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(client_data); + //std::unique_lock 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 +#include + +#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& 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 raw_config_; + + bool enable_timesync_; + TimeSync *timesync_; + TimeSyncConfig timesync_config_; +}; + +} +#endif diff --git a/livox_ros_driver/livox_ros_driver/lds_lvx.cpp b/livox_ros_driver/livox_ros_driver/lds_lvx.cpp new file mode 100644 index 0000000..3fdb7d8 --- /dev/null +++ b/livox_ros_driver/livox_ros_driver/lds_lvx.cpp @@ -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 +#include +#include +#include +#include + +#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(); + 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; iGetDeviceInfo(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::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; idata)) { + return false; + } + } + + return true; +} + +} diff --git a/livox_ros_driver/livox_ros_driver/lds_lvx.h b/livox_ros_driver/livox_ros_driver/lds_lvx.h new file mode 100644 index 0000000..e130eb1 --- /dev/null +++ b/livox_ros_driver/livox_ros_driver/lds_lvx.h @@ -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 +#include + +#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 lvx_file_; + std::shared_ptr t_read_lvx_; + volatile bool start_read_lvx_; +}; + +} +#endif diff --git a/livox_ros_driver/livox_ros_driver/livox_ros_driver.cpp b/livox_ros_driver/livox_ros_driver/livox_ros_driver.cpp new file mode 100644 index 0000000..549444a --- /dev/null +++ b/livox_ros_driver/livox_ros_driver/livox_ros_driver.cpp @@ -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 +#include + +#include "livox_sdk.h" +#include + +#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 bd_code_list; + ParseCommandlineInputBdCode(cmdline_bd_code.c_str(), bd_code_list); + + LdsLidar* read_lidar = LdsLidar::GetInstance(1000/publish_freq); + lddc->RegisterLds(static_cast(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 bd_code_list; + ParseCommandlineInputBdCode(cmdline_bd_code.c_str(), bd_code_list); + + LdsHub* read_hub = LdsHub::GetInstance(1000/publish_freq); + lddc->RegisterLds(static_cast(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(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; +} + + diff --git a/livox_ros_driver/livox_ros_driver/lvx_file.cpp b/livox_ros_driver/livox_ros_driver/lvx_file.cpp new file mode 100644 index 0000000..069b76b --- /dev/null +++ b/livox_ros_driver/livox_ros_driver/lvx_file.cpp @@ -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 +#include +#include + +#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(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(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 &point_packet_list_temp) { + uint64_t cur_pos = 0; + FrameHeader frame_header = { 0 }; + std::unique_ptr 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(atof(param->value())); + } + if ("pitch" == (std::string)param->name()) { + info.pitch = static_cast(atof(param->value())); + } + if ("yaw" == (std::string)param->name()) { + info.yaw = static_cast(atof(param->value())); + } + if ("x" == (std::string)param->name()) { + info.x = static_cast(atof(param->value())); + } + if ("y" == (std::string)param->name()) { + info.y = static_cast(atof(param->value())); + } + if ("z" == (std::string)param->name()) { + info.z = static_cast(atof(param->value())); + } + } + } + } + } +} + +} + diff --git a/livox_ros_driver/livox_ros_driver/lvx_file.h b/livox_ros_driver/livox_ros_driver/lvx_file.h new file mode 100644 index 0000000..1050b83 --- /dev/null +++ b/livox_ros_driver/livox_ros_driver/lvx_file.h @@ -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 +#include +#include +#include +#include +#include +#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& 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 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 diff --git a/livox_ros_driver/msg/CustomPoint.msg b/livox_ros_driver/msg/CustomPoint.msg index 644736d..34719b2 100644 --- a/livox_ros_driver/msg/CustomPoint.msg +++ b/livox_ros_driver/msg/CustomPoint.msg @@ -5,5 +5,6 @@ float32 x # X axis, unit:m float32 y # Y axis, unit:m float32 z # Z axis, unit:m uint8 reflectivity # reflectivity, 0~255 +uint8 tag # livox tag uint8 line # laser number in lidar diff --git a/livox_ros_driver/package.xml b/livox_ros_driver/package.xml index e23a995..4e7161e 100644 --- a/livox_ros_driver/package.xml +++ b/livox_ros_driver/package.xml @@ -1,7 +1,7 @@ livox_ros_driver - 1.1.0 + 2.0.0 The ROS device driver for Livox 3D LiDARs and Livox Hub @@ -54,20 +54,23 @@ rospy std_msgs message_generation + rosbag roscpp rospy std_msgs + rosbag roscpp rospy std_msgs message_runtime + rosbag sensor_msgs git apr - + diff --git a/livox_ros_driver/timesync/timesync.cpp b/livox_ros_driver/timesync/timesync.cpp new file mode 100644 index 0000000..a07f4c3 --- /dev/null +++ b/livox_ros_driver/timesync/timesync.cpp @@ -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 +#include +#include +#include +#include + +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::bind(&TimeSync::PollStateLoop, this)); + t_poll_data_ = std::make_shared(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(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(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; + } +} + +} diff --git a/livox_ros_driver/timesync/timesync.h b/livox_ros_driver/timesync/timesync.h new file mode 100644 index 0000000..636e647 --- /dev/null +++ b/livox_ros_driver/timesync/timesync.h @@ -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 +#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 t_poll_state_; + volatile bool exit_poll_state_; + volatile bool start_poll_state_; + + std::shared_ptr 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 diff --git a/livox_ros_driver/timesync/user_uart/user_uart.cpp b/livox_ros_driver/timesync/user_uart/user_uart.cpp new file mode 100644 index 0000000..5ab0bad --- /dev/null +++ b/livox_ros_driver/timesync/user_uart/user_uart.cpp @@ -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 +#include +#include +#include +#include +#include + +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; + } +} + +} + diff --git a/livox_ros_driver/timesync/user_uart/user_uart.h b/livox_ros_driver/timesync/user_uart/user_uart.h new file mode 100644 index 0000000..2905a46 --- /dev/null +++ b/livox_ros_driver/timesync/user_uart/user_uart.h @@ -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 +#include +#include +#include + +#include + +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 +