diff --git a/src/airy/rslidar_msg-master/CMakeLists.txt b/src/airy/rslidar_msg-master/CMakeLists.txt new file mode 100644 index 0000000..a825404 --- /dev/null +++ b/src/airy/rslidar_msg-master/CMakeLists.txt @@ -0,0 +1,41 @@ +cmake_minimum_required(VERSION 3.5) +project(rslidar_msg) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(std_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/RslidarPacket.msg" + DEPENDENCIES builtin_interfaces std_msgs + ) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/airy/rslidar_msg-master/LICENSE b/src/airy/rslidar_msg-master/LICENSE new file mode 100644 index 0000000..8b1fb13 --- /dev/null +++ b/src/airy/rslidar_msg-master/LICENSE @@ -0,0 +1,19 @@ +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. + diff --git a/src/airy/rslidar_msg-master/README.md b/src/airy/rslidar_msg-master/README.md new file mode 100644 index 0000000..2164d00 --- /dev/null +++ b/src/airy/rslidar_msg-master/README.md @@ -0,0 +1,5 @@ +## ROS2 LiDAR Packet Message + + + +**This project is the custom LiDAR packet message in ROS2 environment. User need to put this project and rslidar_sdk in same workspace to compile.** \ No newline at end of file diff --git a/src/airy/rslidar_msg-master/msg/RslidarPacket.msg b/src/airy/rslidar_msg-master/msg/RslidarPacket.msg new file mode 100644 index 0000000..b6a05f5 --- /dev/null +++ b/src/airy/rslidar_msg-master/msg/RslidarPacket.msg @@ -0,0 +1,4 @@ +std_msgs/Header header +uint8 is_difop +uint8 is_frame_begin +uint8[] data diff --git a/src/airy/rslidar_msg-master/package.xml b/src/airy/rslidar_msg-master/package.xml new file mode 100644 index 0000000..b8a8309 --- /dev/null +++ b/src/airy/rslidar_msg-master/package.xml @@ -0,0 +1,29 @@ + + + + rslidar_msg + 0.0.0 + ros msgs for the rslidar_sdk project + robosense + BSD + + ament_cmake + + std_msgs + rclcpp + + ament_lint_auto + ament_lint_common + + builtin_interfaces + + builtin_interfaces + rosidl_default_generators + + rosidl_default_runtime + +rosidl_interface_packages + + ament_cmake + + diff --git a/src/airy/rslidar_msg-master/ros1/CMakeLists.txt b/src/airy/rslidar_msg-master/ros1/CMakeLists.txt new file mode 100644 index 0000000..320fb43 --- /dev/null +++ b/src/airy/rslidar_msg-master/ros1/CMakeLists.txt @@ -0,0 +1,41 @@ +cmake_minimum_required(VERSION 3.5) +project(rslidar_msg) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(std_msgs REQUIRED) + +find_package(catkin REQUIRED COMPONENTS + roscpp + std_msgs + sensor_msgs + message_generation + ) + +add_message_files(FILES + RslidarPacket.msg + ) + +generate_messages(DEPENDENCIES + std_msgs + sensor_msgs + ) + +catkin_package(CATKIN_DEPENDS + std_msgs + sensor_msgs + message_runtime + ) diff --git a/src/airy/rslidar_msg-master/ros1/package.xml b/src/airy/rslidar_msg-master/ros1/package.xml new file mode 100644 index 0000000..831a653 --- /dev/null +++ b/src/airy/rslidar_msg-master/ros1/package.xml @@ -0,0 +1,22 @@ + + + + rslidar_msg + 0.0.0 + ros msgs for the rslidar_sdk project + robosense + BSD + + catkin + + roscpp + std_msgs + sensor_msgs + message_generation + + roscpp + std_msgs + sensor_msgs + message_runtime + + diff --git a/src/airy/rslidar_msg-master/ros2/CMakeLists.txt b/src/airy/rslidar_msg-master/ros2/CMakeLists.txt new file mode 100644 index 0000000..a825404 --- /dev/null +++ b/src/airy/rslidar_msg-master/ros2/CMakeLists.txt @@ -0,0 +1,41 @@ +cmake_minimum_required(VERSION 3.5) +project(rslidar_msg) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(std_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/RslidarPacket.msg" + DEPENDENCIES builtin_interfaces std_msgs + ) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/airy/rslidar_msg-master/ros2/package.xml b/src/airy/rslidar_msg-master/ros2/package.xml new file mode 100644 index 0000000..b8a8309 --- /dev/null +++ b/src/airy/rslidar_msg-master/ros2/package.xml @@ -0,0 +1,29 @@ + + + + rslidar_msg + 0.0.0 + ros msgs for the rslidar_sdk project + robosense + BSD + + ament_cmake + + std_msgs + rclcpp + + ament_lint_auto + ament_lint_common + + builtin_interfaces + + builtin_interfaces + rosidl_default_generators + + rosidl_default_runtime + +rosidl_interface_packages + + ament_cmake + + diff --git a/src/airy/rslidar_sdk-main/.clang-format b/src/airy/rslidar_sdk-main/.clang-format new file mode 100644 index 0000000..eca6051 --- /dev/null +++ b/src/airy/rslidar_sdk-main/.clang-format @@ -0,0 +1,66 @@ +--- +BasedOnStyle: Google +AccessModifierOffset: -2 +ConstructorInitializerIndentWidth: 2 +AlignEscapedNewlinesLeft: false +AlignTrailingComments: true +AllowAllParametersOfDeclarationOnNextLine: false +AllowShortIfStatementsOnASingleLine: false +AllowShortLoopsOnASingleLine: false +AllowShortFunctionsOnASingleLine: None +AllowShortLoopsOnASingleLine: false +AlwaysBreakTemplateDeclarations: true +AlwaysBreakBeforeMultilineStrings: false +BreakBeforeBinaryOperators: false +BreakBeforeTernaryOperators: false +BreakConstructorInitializersBeforeComma: true +BinPackParameters: true +ColumnLimit: 120 +ConstructorInitializerAllOnOneLineOrOnePerLine: true +DerivePointerBinding: false +PointerBindsToType: true +ExperimentalAutoDetectBinPacking: false +IndentCaseLabels: true +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCSpaceBeforeProtocolList: true +PenaltyBreakBeforeFirstCallParameter: 19 +PenaltyBreakComment: 60 +PenaltyBreakString: 1 +PenaltyBreakFirstLessLess: 1000 +PenaltyExcessCharacter: 1000 +PenaltyReturnTypeOnItsOwnLine: 90 +SpacesBeforeTrailingComments: 2 +Cpp11BracedListStyle: false +Standard: Auto +IndentWidth: 2 +TabWidth: 2 +UseTab: Never +IndentFunctionDeclarationAfterType: false +SpacesInParentheses: false +SpacesInAngles: false +SpaceInEmptyParentheses: false +SpacesInCStyleCastParentheses: false +SpaceAfterControlStatementKeyword: true +SpaceBeforeAssignmentOperators: true +ContinuationIndentWidth: 4 +SortIncludes: false +SpaceAfterCStyleCast: false + +# Configure each individual brace in BraceWrapping +BreakBeforeBraces: Custom + +# Control of individual brace wrapping cases +BraceWrapping: { + AfterClass: 'true' + AfterControlStatement: 'true' + AfterEnum : 'true' + AfterFunction : 'true' + AfterNamespace : 'true' + AfterStruct : 'true' + AfterUnion : 'true' + BeforeCatch : 'true' + BeforeElse : 'true' + IndentBraces : 'false' +} +... diff --git a/src/airy/rslidar_sdk-main/CHANGELOG.md b/src/airy/rslidar_sdk-main/CHANGELOG.md new file mode 100644 index 0000000..c4d6e45 --- /dev/null +++ b/src/airy/rslidar_sdk-main/CHANGELOG.md @@ -0,0 +1,181 @@ +# CHANGELOG + +## v1.5.17 2025-02-14 + +### Added +- Support RSAIRY. +- Support parsing IMU data for RSAIRY and RSE1. +- Support parsing IMU extrinsics parameters frome difop for RSAIRY. + +### Changed +- Add feature attribute to point type. +- Updated config file. +- Update help document. +- Update block_time_offset as us for RSE1 + +### Fixed +- Fix the issue of packets subscription failure under ros2. + + +## v1.5.16 2024-08-27 +### Added +- Load config path frome ros2 param. +### Changed +- Remove the original compilation method. +### Fixed +- Use single package.xml file for both ROS1 and ROS2 @Timple. +- Update msop protocol of RSMX. + +## v1.5.15 2024-08-07 +### Added +- Support RSM3. + +## v1.5.14 2024-07-15 +### Added +- Support multiple lidars with different multicast addresses and the same port. +### Fixed +- Fixed the bug that only one lidar was parsed correctly when multiple bp4.0 were used. +- Fix version number in the package.xml by @Timple. + +## v1.5.13 2024-05-10 +### Added +- Support RSMX. +### Fixed +- Update timestamp parsing unit and the number of packets per frame in decoder_RSE1. +- Update firing_tss of Helios/Helios16P/RubyPlus. +- Fix compilation bug of unit test. +- Remove duplicate text "/rslidar_packets" by @luhuadong. + +## v1.5.12 2023-12-28 +### Fixed +- Fix bug in getting device info and status. +- Fix bug in getting device temperature. + +## v1.5.11 2023-12-18 + +### Changed +- Enable modify socket buffer size. + +## v1.5.10 - 2023-02-17 + +### Changed +- Merge RSBPV4 into RSBP + + +## v1.5.9 - 2023-02-17 + +### Changed +- Increase sending DDS buffer queue to avoid packet loss + + +## v1.5.8 - 2022-12-09 + +### Added +- Support ROS2/Humble Hawksbill +- rename RSEOS as RSE1 + +### Fixed +- Fix wrong widthxheight while ros_send_by_rows=true + + +## v1.5.7 - 2022-10-09 + +### Added +- Seperate RSBPV4 from RSBP +- Support to receive MSOP/DIFOP packet from rosbag v1.3.x +- Support option ros_send_by_rows + +## v1.5.6 - 2022-09-01 + +### Added ++ Add a few build options according to rs_driver ++ Update help documents + +## v1.5.5 - 2022-08-01 + +### Changed +- Output intensity in point cloud as float32 + +### Fixed +- Fix compiling and runtime error on ROS2 Elequent +- Fix frame_id in help docs + +## v1.5.4 - 2022-07-01 + +### Added +- Support the option to stamp the point cloud with the first point + +### Changed +- Remove the dependency on the protobuf library + +## v1.5.3 - 2022-06-01 + +### Added +- Support Jumbo Mode + +### Fixed +- Fix compiling error when protobuf is unavailable + +## v1.5.0 + +### Changed +- refactory the project + +### Added +- support user_layer_bytes and tail_layer_bytes +- support M2 +- replace point with point cloud, as rs_driver's template parameter +- handle point cloud in rs_driver's thread + +## v1.3.0 - 2020-11-10 + +### Added + +- Add multi-cast support +- Add saved_by_rows argument +- Add different point types( XYZI & XYZIRT) + +### Changed +- Update driver core, please refer to CHANGELOG in rs_driver for details +- Update some documents +- Change angle_path argument to hiding parameter + +### Removed + +- Remove RSAUTO for lidar type +- Remove device_ip argument + +## v1.2.1 - 2020-09-04 + +### Fixed +- Fix bug in driver core, please refer to changelog in rs_driver for details. + +## v1.2.0 - 2020-09-01 + +### Added +- Add camera software trigger (base on target angle) + +### Changed +- Update driver core, please refer to changelog in rs_driver for details +- Update the compiler version from C++11 to C++14 + +## v1.1.0 - 2020-07-01 + +### Added +- Add ROS2 support + +### Changed +- Replace while loop with cv.wait +- Update the vector copy part +- Update the program structure + +### Removed +- Remove some unused variables in message struct + +## v1.0.0 - 2020-06-01 + +### Added +- New program structure +- Support ROS & Protobuf-UDP functions + + diff --git a/src/airy/rslidar_sdk-main/CMakeLists.txt b/src/airy/rslidar_sdk-main/CMakeLists.txt new file mode 100644 index 0000000..e0d545d --- /dev/null +++ b/src/airy/rslidar_sdk-main/CMakeLists.txt @@ -0,0 +1,210 @@ +cmake_minimum_required(VERSION 3.5) +cmake_policy(SET CMP0048 NEW) +project(rslidar_sdk) + +#======================================= +# Custom Point Type (XYZI,XYZIRT, XYZIF, XYZIRTF) +#======================================= +set(POINT_TYPE XYZI) + +option(ENABLE_TRANSFORM "Enable transform functions" OFF) +if(${ENABLE_TRANSFORM}) + add_definitions("-DENABLE_TRANSFORM") + find_package(Eigen3 REQUIRED) + include_directories(${EIGEN3_INCLUDE_DIR}) +endif(${ENABLE_TRANSFORM}) + +option(ENABLE_EPOLL_RECEIVE "Receive packets with epoll() instead of select()" OFF) +if(${ENABLE_EPOLL_RECEIVE}) + add_definitions("-DENABLE_EPOLL_RECEIVE") +endif(${ENABLE_EPOLL_RECEIVE}) + +option(ENABLE_MODIFY_RECVBUF "Enable modify size of RECVBUF" ON) +if(${ENABLE_MODIFY_RECVBUF}) + add_definitions("-DENABLE_MODIFY_RECVBUF") +endif(${ENABLE_MODIFY_RECVBUF}) + +option(ENABLE_WAIT_IF_QUEUE_EMPTY "Enable waiting for a while in handle thread if the queue is empty" OFF) +if(${ENABLE_WAIT_IF_QUEUE_EMPTY}) + add_definitions("-DENABLE_WAIT_IF_QUEUE_EMPTY") +endif(${ENABLE_WAIT_IF_QUEUE_EMPTY}) + +option(ENABLE_DIFOP_PARSE "Enable parsing DIFOP Packet" ON) +if(${ENABLE_DIFOP_PARSE}) + add_definitions("-DENABLE_DIFOP_PARSE") +endif(${ENABLE_DIFOP_PARSE}) + +option(ENABLE_STAMP_WITH_LOCAL "Enable stamp point cloud with local time" OFF) +if(${ENABLE_STAMP_WITH_LOCAL}) + add_definitions("-DENABLE_STAMP_WITH_LOCAL") +endif(${ENABLE_STAMP_WITH_LOCAL}) + + +option(ENABLE_SOURCE_PACKET_LEGACY "Enable ROS Source of MSOP/DIFOP Packet v1.3.x" OFF) +if(${ENABLE_SOURCE_PACKET_LEGACY}) + add_definitions("-DENABLE_SOURCE_PACKET_LEGACY") +endif(${ENABLE_SOURCE_PACKET_LEGACY}) + +option(ENABLE_IMU_DATA_PARSE "Enable imu data parse" OFF) +if(${ENABLE_IMU_DATA_PARSE}) + + message(=============================================================) + message("-- Enable imu data parse") + message(=============================================================) + + add_definitions("-DENABLE_IMU_DATA_PARSE") + +endif(${ENABLE_IMU_DATA_PARSE}) + +#======================== +# Project details / setup +#======================== +set(PROJECT_NAME rslidar_sdk) + +add_definitions(-DPROJECT_PATH="${PROJECT_SOURCE_DIR}") + +if (CMAKE_BUILD_TYPE STREQUAL "") + set(CMAKE_BUILD_TYPE Release) + add_definitions(-O3) +endif() + +add_definitions(-std=c++17) +add_compile_options(-Wall) + +#======================== +# Point Type Definition +#======================== +if(${POINT_TYPE} STREQUAL "XYZI") + add_definitions(-DPOINT_TYPE_XYZI) +elseif(${POINT_TYPE} STREQUAL "XYZIRT") + add_definitions(-DPOINT_TYPE_XYZIRT) +elseif(${POINT_TYPE} STREQUAL "XYZIF") + add_definitions(-DPOINT_TYPE_XYZIF) +elseif(${POINT_TYPE} STREQUAL "XYZIRTF") + add_definitions(-DPOINT_TYPE_XYZIRTF) +endif() + +message(=============================================================) +message("-- POINT_TYPE is ${POINT_TYPE}") +message(=============================================================) + +#======================== +# Dependencies Setup +#======================== + +#ROS# +find_package(roscpp 1.12 QUIET) + +if(roscpp_FOUND) + + message(=============================================================) + message("-- ROS Found. ROS Support is turned On.") + message(=============================================================) + + add_definitions(-DROS_FOUND) + + find_package(roslib QUIET) + include_directories(${roscpp_INCLUDE_DIRS} ${roslib_INCLUDE_DIRS}) + set(ROS_LIBS ${roscpp_LIBRARIES} ${roslib_LIBRARIES}) + + add_definitions(-DRUN_IN_ROS_WORKSPACE) + + find_package(catkin REQUIRED COMPONENTS + roscpp + sensor_msgs + roslib) + + catkin_package(CATKIN_DEPENDS + sensor_msgs + roslib) + +else(roscpp_FOUND) + + message(=============================================================) + message("-- ROS Not Found. ROS Support is turned Off.") + message(=============================================================) + +endif(roscpp_FOUND) + +#ROS2# +find_package(rclcpp QUIET) + +if(rclcpp_FOUND) + message(=============================================================) + message("-- ROS2 Found. ROS2 Support is turned On.") + message(=============================================================) + + add_definitions(-DROS2_FOUND) + include_directories(${rclcpp_INCLUDE_DIRS}) + set(CMAKE_CXX_STANDARD 14) + + find_package(ament_cmake REQUIRED) + find_package(sensor_msgs REQUIRED) + find_package(rslidar_msg REQUIRED) + find_package(std_msgs REQUIRED) + +else(rclcpp_FOUND) + message(=============================================================) + message("-- ROS2 Not Found. ROS2 Support is turned Off.") + message(=============================================================) +endif(rclcpp_FOUND) + +#Others# +find_package(yaml-cpp REQUIRED) + +#Include directory# +include_directories(${PROJECT_SOURCE_DIR}/src) + +#Driver core# +add_subdirectory(src/rs_driver) +find_package(rs_driver REQUIRED) +include_directories(${rs_driver_INCLUDE_DIRS}) + +#======================== +# Build Setup +#======================== + +add_executable(rslidar_sdk_node + node/rslidar_sdk_node.cpp + src/manager/node_manager.cpp) + +target_link_libraries(rslidar_sdk_node + ${YAML_CPP_LIBRARIES} + ${rs_driver_LIBRARIES}) + +#Ros# +if(roscpp_FOUND) + + target_link_libraries(rslidar_sdk_node + ${ROS_LIBS}) + + install(TARGETS rslidar_sdk_node + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + +endif(roscpp_FOUND) + +#Ros2# +if(rclcpp_FOUND) + + ament_target_dependencies(rslidar_sdk_node + rclcpp + std_msgs + sensor_msgs + rslidar_msg) + + install(TARGETS + rslidar_sdk_node + DESTINATION lib/${PROJECT_NAME}) + + install(DIRECTORY + launch + config + rviz + DESTINATION share/${PROJECT_NAME}) + + ament_package() + +endif(rclcpp_FOUND) + diff --git a/src/airy/rslidar_sdk-main/LICENSE b/src/airy/rslidar_sdk-main/LICENSE new file mode 100644 index 0000000..8b1fb13 --- /dev/null +++ b/src/airy/rslidar_sdk-main/LICENSE @@ -0,0 +1,19 @@ +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. + diff --git a/src/airy/rslidar_sdk-main/README.md b/src/airy/rslidar_sdk-main/README.md new file mode 100644 index 0000000..ed50e37 --- /dev/null +++ b/src/airy/rslidar_sdk-main/README.md @@ -0,0 +1,186 @@ +# 1 **rslidar_sdk** + + [中文介绍](README_CN.md) + + + +## 1 Introduction + +**rslidar_sdk** is the Software Development Kit of the RoboSense Lidar based on Ubuntu. It contains: + ++ The lidar driver core [rs_driver](https://github.com/RoboSense-LiDAR/rs_driver), ++ The ROS support, ++ The ROS2 support, + +To get point cloud through ROS/ROS2, please just use this SDK. + +To integrate the Lidar driver into your own projects, please use the rs_driver. + +### 1.1 LiDAR Supported + +- RS-LiDAR-16 +- RS-LiDAR-32 +- RS-Bpearl +- RS-Helios +- RS-Helios-16P +- RS-Ruby-128 +- RS-Ruby-80 +- RS-Ruby-48 +- RS-Ruby-Plus-128 +- RS-Ruby-Plus-80 +- RS-Ruby-Plus-48 +- RS-LiDAR-M1 +- RS-LiDAR-M2 +- RS-LiDAR-M3 +- RS-LiDAR-E1 +- RS-LiDAR-MX +- RS-LiDAR-AIRY + +### 1.2 Point Type Supported + +- XYZI - x, y, z, intensity +- XYZIRT - x, y, z, intensity, ring, timestamp + + + +## 2 Download + +### 2.1 Download via Git + +Download the rslidar_sdk as below. Since it contains the submodule rs_driver, please also use `git submodule` to download the submodule properly. + + +```sh +git clone https://github.com/RoboSense-LiDAR/rslidar_sdk.git +cd rslidar_sdk +git submodule init +git submodule update +``` + +### 2.2 Download directly + +Instead of using Git, user can also access [rslidar_sdk_release](https://github.com/RoboSense-LiDAR/rslidar_sdk/releases) to download the latest version of rslidar_sdk. + +Please download the **rslidar_sdk.tar.gz** archive instead of Source code. The Source code zip file does not contain the submodule rs_driver, so it has to be downloaded manaully. +![](./img/01_01_download_page.png) + + + +## 3 Dependencies + +### 3.1 ROS + +To run rslidar_sdk in the ROS environment, please install below libraries. ++ Ubuntu 16.04 - ROS Kinetic desktop ++ Ubuntu 18.04 - ROS Melodic desktop ++ Ubuntu 20.04 - ROS Noetic desktop + +For installation, please refer to http://wiki.ros.org. + +**It's highly recommanded to install ros-distro-desktop-full**. If you do so, the corresponding libraries, such as PCL, will be installed at the same time. + +This brings a lot of convenience, since you don't have to handle version conflict. + +### 3.2 ROS2 + +To use rslidar_sdk in the ROS2 environment, please install below libraries. ++ Ubuntu 16.04 - Not supported ++ Ubuntu 18.04 - ROS2 Eloquent desktop ++ Ubuntu 20.04 - ROS2 Galactic desktop ++ Ubuntu 22.04 - ROS2 Humble desktop + +For installation, please refer to https://index.ros.org/doc/ros2/Installation/Eloquent/Linux-Install-Debians/ + +**Please do not install ROS and ROS2 on the same computer, to avoid possible conflict and manually install some libraries, such as Yaml.** + +### 3.3 Yaml (Essential) + +version: >= v0.5.2 + +*If ros-distro-desktop-full is installed, this step can be skipped* + +Installation: + +```sh +sudo apt-get update +sudo apt-get install -y libyaml-cpp-dev +``` + +### 3.4 libpcap (Essential) + +version: >= v1.7.4 + +Installation: + +```sh +sudo apt-get install -y libpcap-dev +``` + + + +## 4 Compile & Run + + +### 4.1 Compile with ROS catkin tools + +(1) Create a new workspace folder, and create a *src* folder in it. Then put the rslidar_sdk project into the *src* folder. + +(2) Go back to the root of workspace, run the following commands to compile and run. (if using zsh, replace the 2nd command with *source devel/setup.zsh*). + +```sh +catkin_make +source devel/setup.bash +roslaunch rslidar_sdk start.launch +``` + +### 4.2 Compile with ROS2 colcon + +(1) Create a new workspace folder, and create a *src* folder in it. Then put the rslidar_sdk project in the *src* folder. + +(2) Download the packet definition project in ROS2 through [link](https://github.com/RoboSense-LiDAR/rslidar_msg), then put the project rslidar_msg in the *src* folder you just created. + +(3) Go back to the root of workspace, run the following commands to compile and run. (if using zsh, replace the 2nd command with *source install/setup.zsh*). +```sh +colcon build +source install/setup.bash +ros2 launch rslidar_sdk start.py +``` + +Another version of start.py may be used, since it is different on different versios of ROS2. For example, elequent_start.py is used instead for ROS2 elequent. + + + +## 5 Introduction to parameters + +To change behaviors of rslidar_sdk, change its parameters. please read the following links for detail information. + +[Intro to parameters](doc/intro/02_parameter_intro.md) + +[Intro to hidden parameters](doc/intro/03_hiding_parameters_intro.md) + + + +## 6 Quick start + +Below are some quick guides to use rslidar_sdk. + +[Connect to online LiDAR and send point cloud through ROS](doc/howto/06_how_to_decode_online_lidar.md) + +[Decode PCAP file and send point cloud through ROS](doc/howto/08_how_to_decode_pcap_file.md) + +[Change Point Type](doc/howto/05_how_to_change_point_type.md) + + + +## 7 Advanced Topics + +[Online Lidar - Advanced topics](doc/howto/07_online_lidar_advanced_topics.md) + +[PCAP file - Advanced topics](doc/howto/09_pcap_file_advanced_topics.md) + +[Coordinate Transformation](doc/howto/10_how_to_use_coordinate_transformation.md) + +[Record rosbag & Replay it](doc/howto/11_how_to_record_replay_packet_rosbag.md) + + + diff --git a/src/airy/rslidar_sdk-main/README_CN.md b/src/airy/rslidar_sdk-main/README_CN.md new file mode 100644 index 0000000..23b7e71 --- /dev/null +++ b/src/airy/rslidar_sdk-main/README_CN.md @@ -0,0 +1,180 @@ +# 1 **rslidar_sdk** + +[English Version](README.md) + + + +## 1.1 工程简介 + + **rslidar_sdk** 是速腾聚创在Ubuntu环境下的雷达驱动软件包。它包括： + + 雷达驱动内核[rs_driver](https://github.com/RoboSense-LiDAR/rs_driver)， + + ROS拓展功能， + + ROS2拓展功能 + +如果希望基于ROS/ROS2进行二次开发，可以使用本软件包。配合ROS/ROS2自带的可视化工具rviz，可以查看点云。 + +如果希望将雷达驱动集成到自己的工程，作更深一步的二次开发，请基于rs_driver进行开发。 + +### 1.1.1 支持的雷达型号 + +- RS-LiDAR-16 +- RS-LiDAR-32 +- RS-Bpearl +- RS-Helios +- RS-Helios-16P +- RS-Ruby-128 +- RS-Ruby-80 +- RS-Ruby-48 +- RS-Ruby-Plus-128 +- RS-Ruby-Plus-80 +- RS-Ruby-Plus-48 +- RS-LiDAR-M1 +- RS-LiDAR-M2 +- RS-LiDAR-M3 +- RS-LiDAR-E1 +- RS-LiDAR-MX +- RS-LiDAR-AIRY + +### 1.1.2 支持的点类型 + +- XYZI - x, y, z, intensity +- XYZIRT - x, y, z, intensity, ring, timestamp + + + +## 1.2 下载 + +### 1.2.1 使用 git clone + +rslidar_sdk项目包含子模块驱动内核rs_driver。在执行git clone后，还需要执行相关指令，初始化并更新子模块。 + + ```sh +git clone https://github.com/RoboSense-LiDAR/rslidar_sdk.git +cd rslidar_sdk +git submodule init +git submodule update + ``` + +### 1.2.2 直接下载 + +用户可以直接访问 [rslidar_sdk_release](https://github.com/RoboSense-LiDAR/rslidar_sdk/releases) 下载最新版本的rslidar_sdk. + +请下载 **rslidar_sdk.tar.gz** 压缩包，不要下载Source code。因为Source code压缩包内不包含子模块rs_driver的代码， 用户还需自行下载rs_driver的代码放入其中才行。 +![](./img/01_01_download_page.png) + + + +## 1.3 依赖介绍 + +### 1.3.1 ROS + +在ROS环境下使用雷达驱动，需要安装ROS相关依赖库。 ++ Ubuntu 16.04 - ROS Kinetic desktop ++ Ubuntu 18.04 - ROS Melodic desktop ++ Ubuntu 20.04 - ROS Noetic desktop + +安装方法请参考 http://wiki.ros.org。 + +**强烈建议安装ROS desktop-full版。这个过程会自动安装一些兼容版本的依赖库，如PCL库等。这样可以避免花大量时间，去逐个安装和配置它们**。 + +### 1.3.2 ROS2 + +在ROS2环境下使用雷达驱动，需要安装ROS2相关依赖库。 ++ Ubuntu 16.04 - 不支持 ++ Ubuntu 18.04 - ROS2 Eloquent desktop ++ Ubuntu 20.04 - ROS2 Galactic desktop ++ Ubuntu 22.04 - ROS2 Humble desktop + +安装方法请参考 https://index.ros.org/doc/ros2/Installation/Eloquent/Linux-Install-Debians/ + +**请不要在一台电脑上同时安装ROS和ROS2，以避免可能的版本冲突，和手工安装其他库（如Yaml）的麻烦。** + +### 1.3.3 Yaml (必需) + +版本号: >= v0.5.2 + +*若已安装ROS desktop-full, 可跳过* + +安装方法如下: + +```sh +sudo apt-get update +sudo apt-get install -y libyaml-cpp-dev +``` + +### 1.3.4 libpcap (必需) + +版本号： >= v1.7.4 + +安装方法如下： + +```sh +sudo apt-get install -y libpcap-dev +``` + + + +## 1.4 编译、运行 + +### 1.4.1 依赖于ROS-catkin编译 + +(1) 新建一个文件夹作为工作空间，然后再新建一个名为*src*的文件夹, 将rslidar_sdk工程放入*src*文件夹内。 + +(2) 返回工作空间目录，执行以下命令即可编译&运行(若使用.zsh,将第二句指令替换为 *source devel/setup.zsh*)。 + +```sh +catkin_make +source devel/setup.bash +roslaunch rslidar_sdk start.launch +``` + +### 1.4.2 依赖于ROS2-colcon编译 + +(1) 新建一个文件夹作为工作空间，然后再新建一个名为*src*的文件夹, 将rslidar_sdk工程放入*src*文件夹内。 + +(2) 通过[链接](https://github.com/RoboSense-LiDAR/rslidar_msg)，下载ROS2环境下的雷达packet消息定义，将rslidar_msg工程也放在刚刚新建的*src*文件夹内，与rslidar_sdk并列。 + +(3) 返回工作空间目录，执行以下命令即可编译、运行。如果使用.zsh，将第二行替换为*source install/setup.zsh*。 + +```sh +colcon build +source install/setup.bash +ros2 launch rslidar_sdk start.py +``` + +不同ROS2版本start.py的格式可能不同，请使用对应版本的start.py。如ROS2 Elequent，请使用elequent_start.py。 + + + +## 1.5 参数介绍 + +rslidar_sdk的功能通过配置参数文件来实现，请仔细阅读。 + +[参数介绍](doc/intro/02_parameter_intro_CN.md) + +[隐藏参数介绍](doc/intro/03_hiding_parameters_intro_CN.md) + + + +## 1.6 快速上手 + +以下是一些常用功能的使用指南。 + +[连接在线雷达数据并发送点云到ROS](doc/howto/06_how_to_decode_online_lidar_CN.md) + +[解析PCAP包并发送点云到ROS](doc/howto/08_how_to_decode_pcap_file_CN.md) + +[切换点类型](doc/howto/05_how_to_change_point_type_CN.md) + + + +## 1.7 使用进阶 + +[在线雷达-高级主题](doc/howto/07_online_lidar_advanced_topics_CN.md) + +[PCAP文件-高级主题](doc/howto/09_pcap_file_advanced_topics_CN.md) + +[点云坐标变换](doc/howto/10_how_to_use_coordinate_transformation_CN.md) + +[录制ROS数据包然后播放它](doc/howto/11_how_to_record_replay_packet_rosbag_CN.md) + diff --git a/src/airy/rslidar_sdk-main/config/config.yaml b/src/airy/rslidar_sdk-main/config/config.yaml new file mode 100644 index 0000000..a7057d6 --- /dev/null +++ b/src/airy/rslidar_sdk-main/config/config.yaml @@ -0,0 +1,50 @@ +common: + msg_source: + 1 # 0: not use Lidar + # 1: packet message comes from online Lidar + # 2: packet message comes from ROS or ROS2 + # 3: packet message comes from Pcap file + send_packet_ros: false # true: Send packets through ROS or ROS2(Used to record packet) + send_point_cloud_ros: true # true: Send point cloud through ROS or ROS2 +lidar: + - driver: + lidar_type: + RSAIRY # LiDAR type - RS16, RS32, RSBP, RSAIRY, RSHELIOS, RSHELIOS_16P, RS128, RS80, RS48, RSP128, RSP80, RSP48, + # RSM1, RSM1_JUMBO, RSM2, RSM3, RSE1, RSMX. + + msop_port: 6699 # Msop port of lidar + difop_port: 7788 # Difop port of lidar + imu_port: + 0 # IMU port of lidar(only for RSAIRY, RSE1), 0 means no imu. + # If you want to use IMU, please first set ENABLE_IMU_DATA_PARSE to ON in CMakeLists.txt + user_layer_bytes: 0 # Bytes of user layer. thers is no user layer if it is 0 + tail_layer_bytes: 0 # Bytes of tail layer. thers is no tail layer if it is 0 + + min_distance: 0.01 # Minimum distance of point cloud + max_distance: 200 # Maximum distance of point cloud + use_lidar_clock: + true # true--Use the lidar clock as the message timestamp + # false-- Use the system clock as the timestamp + dense_points: false # true: discard NAN points; false: reserve NAN points + + ts_first_point: + true # true: time-stamp point cloud with the first point; false: with the last point; + # these parameters are used from mechanical lidar + + start_angle: 0 # Start angle of point cloud + end_angle: + 360 # End angle of point cloud + + + # When msg_source is 3, the following parameters will be used + pcap_repeat: true # true: The pcap bag will repeat play + pcap_rate: 1.0 # Rate to read the pcap file + pcap_path: /home/robosense/lidar.pcap #The path of pcap file + + ros: + ros_frame_id: rslidar #Frame id of packet message and point cloud message + ros_recv_packet_topic: /rslidar_packets #Topic used to receive lidar packets from ROS + ros_send_packet_topic: /rslidar_packets #Topic used to send lidar packets through ROS + ros_send_imu_data_topic: /rslidar_imu_data #Topic used to send imu data through ROS + ros_send_point_cloud_topic: /rslidar_points #Topic used to send point cloud through ROS + ros_queue_length: 100 #Topic QoS history depth diff --git a/src/airy/rslidar_sdk-main/config/config_double.yaml b/src/airy/rslidar_sdk-main/config/config_double.yaml new file mode 100644 index 0000000..63ed49d --- /dev/null +++ b/src/airy/rslidar_sdk-main/config/config_double.yaml @@ -0,0 +1,92 @@ +common: + msg_source: + 1 # 0: not use Lidar + # 1: packet message comes from online Lidar + # 2: packet message comes from ROS or ROS2 + # 3: packet message comes from Pcap file + send_packet_ros: false # true: Send packets through ROS or ROS2(Used to record packet) + send_point_cloud_ros: true # true: Send point cloud through ROS or ROS2 +lidar: + - driver: # front_lidar + lidar_type: + RSAIRY # LiDAR type - RS16, RS32, RSBP, RSAIRY, RSHELIOS, RSHELIOS_16P, RS128, RS80, RS48, RSP128, RSP80, RSP48, + # RSM1, RSM1_JUMBO, RSM2, RSM3, RSE1, RSMX. + + msop_port: 6699 # Msop port of lidar + difop_port: 7788 # Difop port of lidar + imu_port: + 0 # IMU port of lidar(only for RSAIRY, RSE1), 0 means no imu. + # If you want to use IMU, please first set ENABLE_IMU_DATA_PARSE to ON in CMakeLists.txt + user_layer_bytes: 0 # Bytes of user layer. thers is no user layer if it is 0 + tail_layer_bytes: 0 # Bytes of tail layer. thers is no tail layer if it is 0 + + min_distance: 0.01 # Minimum distance of point cloud + max_distance: 200 # Maximum distance of point cloud + use_lidar_clock: + true # true--Use the lidar clock as the message timestamp + # false-- Use the system clock as the timestamp + dense_points: false # true: discard NAN points; false: reserve NAN points + + ts_first_point: + true # true: time-stamp point cloud with the first point; false: with the last point; + # these parameters are used from mechanical lidar + + start_angle: 0 # Start angle of point cloud + end_angle: + 360 # End angle of point cloud + + + # When msg_source is 3, the following parameters will be used + pcap_repeat: true # true: The pcap bag will repeat play + pcap_rate: 1.0 # Rate to read the pcap file + pcap_path: /home/robosense/lidar.pcap #The path of pcap file + + ros: + ros_frame_id: rslidar #Frame id of packet message and point cloud message + ros_recv_packet_topic: /rslidar_packets/front_lidar #Topic used to receive lidar packets from ROS + ros_send_packet_topic: /rslidar_packets/front_lidar #Topic used to send lidar packets through ROS + ros_send_imu_data_topic: /rslidar_imu_data/front_lidar #Topic used to send imu data through ROS + ros_send_point_cloud_topic: /rslidar_points #Topic used to send point cloud through ROS + ros_queue_length: 100 #Topic QoS history depth + + - driver: # rear_lidar + lidar_type: + RSAIRY # LiDAR type - RS16, RS32, RSBP, RSAIRY, RSHELIOS, RSHELIOS_16P, RS128, RS80, RS48, RSP128, RSP80, RSP48, + # RSM1, RSM1_JUMBO, RSM2, RSM3, RSE1, RSMX. + + msop_port: 9966 # Msop port of lidar + difop_port: 8877 # Difop port of lidar + imu_port: + 0 # IMU port of lidar(only for RSAIRY, RSE1), 0 means no imu. + # If you want to use IMU, please first set ENABLE_IMU_DATA_PARSE to ON in CMakeLists.txt + user_layer_bytes: 0 # Bytes of user layer. thers is no user layer if it is 0 + tail_layer_bytes: 0 # Bytes of tail layer. thers is no tail layer if it is 0 + + min_distance: 0.01 # Minimum distance of point cloud + max_distance: 200 # Maximum distance of point cloud + use_lidar_clock: + true # true--Use the lidar clock as the message timestamp + # false-- Use the system clock as the timestamp + dense_points: false # true: discard NAN points; false: reserve NAN points + + ts_first_point: + true # true: time-stamp point cloud with the first point; false: with the last point; + # these parameters are used from mechanical lidar + + start_angle: 0 # Start angle of point cloud + end_angle: + 360 # End angle of point cloud + + + # When msg_source is 3, the following parameters will be used + pcap_repeat: true # true: The pcap bag will repeat play + pcap_rate: 1.0 # Rate to read the pcap file + pcap_path: /home/robosense/lidar.pcap #The path of pcap file + + ros: + ros_frame_id: rslidar #Frame id of packet message and point cloud message + ros_recv_packet_topic: /rslidar_packets/rear_lidar #Topic used to receive lidar packets from ROS + ros_send_packet_topic: /rslidar_packets/rear_lidar #Topic used to send lidar packets through ROS + ros_send_imu_data_topic: /rslidar_imu_data/rear_lidar #Topic used to send imu data through ROS + ros_send_point_cloud_topic: /rslidar_points #Topic used to send point cloud through ROS + ros_queue_length: 100 #Topic QoS history depth \ No newline at end of file diff --git a/src/airy/rslidar_sdk-main/config/config_front.yaml b/src/airy/rslidar_sdk-main/config/config_front.yaml new file mode 100644 index 0000000..4dab7f4 --- /dev/null +++ b/src/airy/rslidar_sdk-main/config/config_front.yaml @@ -0,0 +1,50 @@ +common: + msg_source: + 1 # 0: not use Lidar + # 1: packet message comes from online Lidar + # 2: packet message comes from ROS or ROS2 + # 3: packet message comes from Pcap file + send_packet_ros: false # true: Send packets through ROS or ROS2(Used to record packet) + send_point_cloud_ros: true # true: Send point cloud through ROS or ROS2 +lidar: + - driver: + lidar_type: + RSAIRY # LiDAR type - RS16, RS32, RSBP, RSAIRY, RSHELIOS, RSHELIOS_16P, RS128, RS80, RS48, RSP128, RSP80, RSP48, + # RSM1, RSM1_JUMBO, RSM2, RSM3, RSE1, RSMX. + + msop_port: 6699 # Msop port of lidar + difop_port: 7788 # Difop port of lidar + imu_port: + 0 # IMU port of lidar(only for RSAIRY, RSE1), 0 means no imu. + # If you want to use IMU, please first set ENABLE_IMU_DATA_PARSE to ON in CMakeLists.txt + user_layer_bytes: 0 # Bytes of user layer. thers is no user layer if it is 0 + tail_layer_bytes: 0 # Bytes of tail layer. thers is no tail layer if it is 0 + + min_distance: 0.01 # Minimum distance of point cloud + max_distance: 200 # Maximum distance of point cloud + use_lidar_clock: + true # true--Use the lidar clock as the message timestamp + # false-- Use the system clock as the timestamp + dense_points: false # true: discard NAN points; false: reserve NAN points + + ts_first_point: + true # true: time-stamp point cloud with the first point; false: with the last point; + # these parameters are used from mechanical lidar + + start_angle: 0 # Start angle of point cloud + end_angle: + 360 # End angle of point cloud + + + # When msg_source is 3, the following parameters will be used + pcap_repeat: true # true: The pcap bag will repeat play + pcap_rate: 1.0 # Rate to read the pcap file + pcap_path: /home/robosense/lidar.pcap #The path of pcap file + + ros: + ros_frame_id: front_lidar #Frame id of packet message and point cloud message + ros_recv_packet_topic: /rslidar_packets #Topic used to receive lidar packets from ROS + ros_send_packet_topic: /rslidar_packets #Topic used to send lidar packets through ROS + ros_send_imu_data_topic: /rslidar_imu_data #Topic used to send imu data through ROS + ros_send_point_cloud_topic: /rslidar_points/front_lidar #Topic used to send point cloud through ROS + ros_queue_length: 100 #Topic QoS history depth diff --git a/src/airy/rslidar_sdk-main/config/config_rear.yaml b/src/airy/rslidar_sdk-main/config/config_rear.yaml new file mode 100644 index 0000000..2142f52 --- /dev/null +++ b/src/airy/rslidar_sdk-main/config/config_rear.yaml @@ -0,0 +1,50 @@ +common: + msg_source: + 1 # 0: not use Lidar + # 1: packet message comes from online Lidar + # 2: packet message comes from ROS or ROS2 + # 3: packet message comes from Pcap file + send_packet_ros: false # true: Send packets through ROS or ROS2(Used to record packet) + send_point_cloud_ros: true # true: Send point cloud through ROS or ROS2 +lidar: + - driver: + lidar_type: + RSAIRY # LiDAR type - RS16, RS32, RSBP, RSAIRY, RSHELIOS, RSHELIOS_16P, RS128, RS80, RS48, RSP128, RSP80, RSP48, + # RSM1, RSM1_JUMBO, RSM2, RSM3, RSE1, RSMX. + + msop_port: 9966 # Msop port of lidar + difop_port: 8877 # Difop port of lidar + imu_port: + 0 # IMU port of lidar(only for RSAIRY, RSE1), 0 means no imu. + # If you want to use IMU, please first set ENABLE_IMU_DATA_PARSE to ON in CMakeLists.txt + user_layer_bytes: 0 # Bytes of user layer. thers is no user layer if it is 0 + tail_layer_bytes: 0 # Bytes of tail layer. thers is no tail layer if it is 0 + + min_distance: 0.01 # Minimum distance of point cloud + max_distance: 200 # Maximum distance of point cloud + use_lidar_clock: + true # true--Use the lidar clock as the message timestamp + # false-- Use the system clock as the timestamp + dense_points: false # true: discard NAN points; false: reserve NAN points + + ts_first_point: + true # true: time-stamp point cloud with the first point; false: with the last point; + # these parameters are used from mechanical lidar + + start_angle: 0 # Start angle of point cloud + end_angle: + 360 # End angle of point cloud + + + # When msg_source is 3, the following parameters will be used + pcap_repeat: true # true: The pcap bag will repeat play + pcap_rate: 1.0 # Rate to read the pcap file + pcap_path: /home/robosense/lidar.pcap #The path of pcap file + + ros: + ros_frame_id: rear_lidar #Frame id of packet message and point cloud message + ros_recv_packet_topic: /rslidar_packets #Topic used to receive lidar packets from ROS + ros_send_packet_topic: /rslidar_packets #Topic used to send lidar packets through ROS + ros_send_imu_data_topic: /rslidar_imu_data #Topic used to send imu data through ROS + ros_send_point_cloud_topic: /rslidar_points/rear_lidar #Topic used to send point cloud through ROS + ros_queue_length: 100 #Topic QoS history depth diff --git a/src/airy/rslidar_sdk-main/create_debian.sh b/src/airy/rslidar_sdk-main/create_debian.sh new file mode 100755 index 0000000..d432b89 --- /dev/null +++ b/src/airy/rslidar_sdk-main/create_debian.sh @@ -0,0 +1,47 @@ +#! /usr/bin/env bash + +## install the python-bloom fakeroot + +function install_pkg() +{ + pkg_found=$(dpkg -l | grep ${1}) + if [[ $? -eq 0 ]] + then + echo "${1} already installed." + else + echo "Install ${1} ..." + sudo apt-get install ${1} -y + fi +} + +install_pkg python-bloom +install_pkg fakeroot + +echo -e "\n\033[1;32m ~~ (1). Delete old debian folders in the directory...\033[0m" +rm -rf debian/ obj-x86_64-linux-gnu/ + +echo -e "\n\033[1;32m ~~ (2). Delete any backup files...\033[0m" +find . -type f -name '*~' -delete + +echo -e "\n\033[1;32m ~~ (3). Create debian packages...\033[0m\n" +bloom-generate rosdebian --os-name ubuntu --os-version `echo $(lsb_release -sc)` --ros-distro `echo ${ROS_DISTRO}` + +# sed 's#dh_shlibdeps*#dh_shlibdeps --dpkg-shlibdeps-params=--ignore-missing-info#g' debian/rules +## when dpkg-shlibdeps: error: no dependency information found for +## adding bellow code to debian/rules +## +## override_dh_shlibdeps: +## dh_shlibdeps --dpkg-shlibdeps-params=--ignore-missing-info +## It is necessary to insert a TAB instead of spaces before "dh_shlibdeps ..." + +target_string=$(grep "dh_shlibdeps " debian/rules) +replace_string=" dh_shlibdeps --dpkg-shlibdeps-params=--ignore-missing-info" +sed -i "s#${target_string}#${replace_string}#g" debian/rules + +fakeroot debian/rules binary + +echo -e "\n\033[1;32m ~~ (4). Delete old debian folders in the directory...\033[0m" +rm -rf debian/ obj-x86_64-linux-gnu/ + +echo -e "\n\033[1;32m ~~ (5). Delete any backup files...\033[0m" +find . -type f -name '*~' -delete diff --git a/src/airy/rslidar_sdk-main/doc/howto/04_how_to_work_along_with_rslidar_sdk_node_v1.3.x_CN.md b/src/airy/rslidar_sdk-main/doc/howto/04_how_to_work_along_with_rslidar_sdk_node_v1.3.x_CN.md new file mode 100644 index 0000000..50bf6b6 --- /dev/null +++ b/src/airy/rslidar_sdk-main/doc/howto/04_how_to_work_along_with_rslidar_sdk_node_v1.3.x_CN.md @@ -0,0 +1,115 @@ +# 4 如何与rslidar_sdk_node v1.3.x共存？ + + + +## 4.1 问题描述 + +`rslidar_sdk_node` `v1.3.x`和`v1.5.x`的配置方式不同。除了如下两个可能有交互的场景外， 两者各自运行，没有关系。 + ++ `rslidar_sdk_node`在主题`/rslidar_points`下发布点云，rosbag订阅并录制到一个cloud rosbag文件。后面rosbag又会回放这个文件，发布到`/rslidar_points`，`rslidar_sdk_node`订阅并播放它。 + ++ `rslidar_sdk_node`在主题`/rslidar_packets`下发布原始的`MSOP/DIFOP Packet`，rosbag订阅并录制到一个packet rosbag文件。后面rosbag又会回放这个文件到`/rslidar_packets`，`rslidar_sdk_node`订阅并播放它。 + +第一种场景下，`v1.3.x`和`v1.5.x`发布的点云格式相同，所以`v1.3.x`录制的点云，在`v1.5.x`上播放是没有问题的。 + +第二种场景下，`v1.3.x`将MSOP/DIFOP Packet分别发布在两个主题`/rslidar_packets`和`/rslidar_packets_difop`下，而`v1.5.x`将MSOP/DIFOP Packet发布在单个主题`/rslidar_packets`下，而且`v1.3.x`和`v1.5.x`的消息定义也不同，所以`v1.3.x`录制的packet rosbag在`v1.5.x`上不能播放。ROS会检测出这两种格式的MD5 Checksum不匹配并报错。 + +本文说明如何配置`rslidar_sdk` `v1.5.x`，让它在第二种场景下可以同时播放`v1.3.x`和`v1.5.x`的packet rosbag。 + + + +## 4.2 场景说明 + +场景说明如下。 ++ 2个雷达，`Lidar1`是运行`v1.3.x`的雷达，`Lidar2`是运行`v1.5.x`的雷达。 ++ 1台主机，用于分析`Lidar1`和`Lidar2`的数据。 + +![](./img/04_01_packet_rosbag.png) + +## 4.3 步骤 + + +### 4.3.1 配置 v1.3.x 雷达 + +使用`v1.3.x` `rslidar_sdk_node`录制pacekt rosbag。 + +按照默认的`config.yaml`的配置，消息发布到主题`/rslidar_packets`和`/rslidar_packets_difop`下。 + +``` +common: + msg_source: 1 #0: not use Lidar + #1: packet message comes from online Lidar + #2: packet message comes from ROS or ROS2 + #3: packet message comes from Pcap file + #4: packet message comes from Protobuf-UDP + #5: point cloud comes from Protobuf-UDP + send_packet_ros: true #true: Send packets through ROS or ROS2(Used to record packet) + send_point_cloud_ros: true #true: Send point cloud through ROS or ROS2 +lidar: + - driver: + lidar_type: RSM1 #LiDAR type - RS16, RS32, RSBP, RSHELIOS, RS128, RS80, RSM1 + msop_port: 6699 #Msop port of lidar + difop_port: 7788 #Difop port of lidar + ros: + ros_send_packet_topic: /rslidar_packets #Topic used to send lidar packets through ROS + ros_send_point_cloud_topic: /rslidar_points #Topic used to send point cloud through ROS +``` + +### 4.3.2 配置 v1.5.x 雷达 + +使用`v1.5.6` `rslidar_sdk_node`录制packet rosbag。 + +为了与`v1.3.2`的消息区别，将消息输出到主题`/rslidar_packets_v2`下。 + +``` +common: + msg_source: 1 #0: not use Lidar + #1: packet message comes from online Lidar + #2: packet message comes from ROS or ROS2 + #3: packet message comes from Pcap file + send_packet_ros: true #true: Send packets through ROS or ROS2(Used to record packet) + send_point_cloud_ros: true #true: Send point cloud through ROS or ROS2 +lidar: + - driver: + lidar_type: RSM1 #LiDAR type - RS16, RS32, RSBP, RSHELIOS, RS128, RS80, RS48, RSM1 + msop_port: 6699 #Msop port of lidar + difop_port: 7788 #Difop port of lidar + ros: + ros_send_packet_topic: /rslidar_packets_v2 #Topic used to send lidar packets through ROS + ros_send_point_cloud_topic: /rslidar_points #Topic used to send point cloud through ROS +``` + + +### 4.3.3 配置 v1.5.x 主机 + ++ 打开CMake编译选项`ENABLE_SOURCE_PACKET_LEGACY=ON`，编译`rslidar_sdk`。 + +``` +# CMakeLists.txt + +option(ENABLE_SOURCE_PACKET_LEGACY "Enable ROS Source of MSOP/DIFOP Packet v1.3.x" ON) +``` + ++ 在`config.yaml`中，增加一个配置项`ros_recv_packet_legacy_topic`: `/rslidar_packets`。这样`rslidar_sdk_node`将同时订阅两个主题。 + + 订阅`/rslidar_packets`和`/rslidar_packets_difop`，读入`v1.3.x`的消息 + + 订阅`/rslidar_packets_v2`，读入`v1.5.x`的消息 + +``` +common: + msg_source: 1 #0: not use Lidar + #1: packet message comes from online Lidar + #2: packet message comes from ROS or ROS2 + #3: packet message comes from Pcap file + send_packet_ros: false #true: Send packets through ROS or ROS2(Used to record packet) + send_point_cloud_ros: true #true: Send point cloud through ROS or ROS2 +lidar: + - driver: + lidar_type: RSM1 #LiDAR type - RS16, RS32, RSBP, RSHELIOS, RS128, RS80, RS48, RSM1 + msop_port: 6699 #Msop port of lidar + difop_port: 7788 #Difop port of lidar + ros: + ros_recv_packet_legacy_topic: /rslidar_packets #Topic used to receive lidar packets from ROS + ros_recv_packet_topic: /rslidar_packets_v2 #Topic used to receive lidar packets from ROS + ros_send_point_cloud_topic: /rslidar_points #Topic used to send point cloud through ROS +``` + diff --git a/src/airy/rslidar_sdk-main/doc/howto/05_how_to_change_point_type.md b/src/airy/rslidar_sdk-main/doc/howto/05_how_to_change_point_type.md new file mode 100644 index 0000000..9de05b0 --- /dev/null +++ b/src/airy/rslidar_sdk-main/doc/howto/05_how_to_change_point_type.md @@ -0,0 +1,165 @@ +# 5 How to change point type + + + +## 5.1 Introduction + +This document illustrates how to change the point type. + +In ```CMakeLists.txt``` of the project, change the variable `POINT_TYPE`. Remember to **rebuild** the project after changing it. + +```cmake +#======================================= +# Custom Point Type (XYZI,XYZIRT, XYZIF, XYZIRTF) +#======================================= +set(POINT_TYPE XYZI) +``` + + + +## 5.2 XYZI + +If `POINT_TYPE` is `XYZI`, rslidar_sdk uses the RoboSense defined type as below. + +```c++ +struct PointXYZI +{ + float x; + float y; + float z; + uint8_t intensity; +}; +``` + +rslidar_sdk transforms point cloud of `PointXYZI` to ROS message of `PointCloud2`，and publish it. + +```c++ + sensor_msgs::PointCloud2 ros_msg; + int offset = 0; + offset = addPointField(ros_msg, "x", 1, sensor_msgs::PointField::FLOAT32, offset); + offset = addPointField(ros_msg, "y", 1, sensor_msgs::PointField::FLOAT32, offset); + offset = addPointField(ros_msg, "z", 1, sensor_msgs::PointField::FLOAT32, offset); + offset = addPointField(ros_msg, "intensity", 1, sensor_msgs::PointField::FLOAT32, offset); + + // + // copy points from point cloud of `PointXYZI` to `PointCloud2` + // + ... + +``` + +Here `intensity` of `PointCloud2` is `float` type, not `uint8_t`. This is because most ROS based applications require `intensity` of `float` type. + + + +## 5.3 XYZIRT + +If `POINT_TYPE` is `XYZIRT`, rslidar_sdk uses the RoboSense defined type as below. + +```c++ +struct PointXYZIRT +{ + float x; + float y; + float z; + uint8_t intensity; + uint16_t ring; + double timestamp; +}; +``` + +rslidar_sdk transforms point cloud of `PointXYZIRT` to ROS message of `PointCloud2`，and publish it. + +```c++ + sensor_msgs::PointCloud2 ros_msg; + int offset = 0; + offset = addPointField(ros_msg, "x", 1, sensor_msgs::PointField::FLOAT32, offset); + offset = addPointField(ros_msg, "y", 1, sensor_msgs::PointField::FLOAT32, offset); + offset = addPointField(ros_msg, "z", 1, sensor_msgs::PointField::FLOAT32, offset); + offset = addPointField(ros_msg, "intensity", 1, sensor_msgs::PointField::FLOAT32, offset); +#if defined(POINT_TYPE_XYZIRT) || defined(POINT_TYPE_XYZIRTF) + offset = addPointField(ros_msg, "ring", 1, sensor_msgs::PointField::UINT16, offset); + offset = addPointField(ros_msg, "timestamp", 1, sensor_msgs::PointField::FLOAT64, offset); +#endif + + // + // copy points from point cloud of `PointXYZIRT` to `PointCloud2` + // + ... + +``` +## 5.4 XYZIF + +If `POINT_TYPE` is `XYZIF`, rslidar_sdk uses the RoboSense defined type as below. + +```c++ +struct PointXYZIF +{ + float x; + float y; + float z; + uint8_t intensity; + uint16_t ring; + double timestamp; +}; +``` + +rslidar_sdk transforms point cloud of `PointXYZIF` to ROS message of `PointCloud2`，and publish it. + +```c++ + sensor_msgs::PointCloud2 ros_msg; + int offset = 0; + offset = addPointField(ros_msg, "x", 1, sensor_msgs::PointField::FLOAT32, offset); + offset = addPointField(ros_msg, "y", 1, sensor_msgs::PointField::FLOAT32, offset); + offset = addPointField(ros_msg, "z", 1, sensor_msgs::PointField::FLOAT32, offset); + offset = addPointField(ros_msg, "intensity", 1, sensor_msgs::PointField::FLOAT32, offset); +#if defined(POINT_TYPE_XYZIF) || defined(POINT_TYPE_XYZIRTF) + offset = addPointField(ros_msg, "feature", 1, sensor_msgs::PointField::UINT8, offset); +#endif + + // + // copy points from point cloud of `PointXYZIF` to `PointCloud2` + // + ... + +``` +## 5.5 XYZIRTF + +If `POINT_TYPE` is `XYZIRTF`, rslidar_sdk uses the RoboSense defined type as below. + +```c++ +struct PointXYZIRTF +{ + float x; + float y; + float z; + uint8_t intensity; + uint16_t ring; + double timestamp; +}; +``` + +rslidar_sdk transforms point cloud of `PointXYZIRTF` to ROS message of `PointCloud2`，and publish it. + +```c++ + sensor_msgs::PointCloud2 ros_msg; + int offset = 0; + offset = addPointField(ros_msg, "x", 1, sensor_msgs::PointField::FLOAT32, offset); + offset = addPointField(ros_msg, "y", 1, sensor_msgs::PointField::FLOAT32, offset); + offset = addPointField(ros_msg, "z", 1, sensor_msgs::PointField::FLOAT32, offset); + offset = addPointField(ros_msg, "intensity", 1, sensor_msgs::PointField::FLOAT32, offset); +#if defined(POINT_TYPE_XYZIRT) || defined(POINT_TYPE_XYZIRTF) + offset = addPointField(ros_msg, "ring", 1, sensor_msgs::PointField::UINT16, offset); + offset = addPointField(ros_msg, "timestamp", 1, sensor_msgs::PointField::FLOAT64, offset); +#endif +#if defined(POINT_TYPE_XYZIF) || defined(POINT_TYPE_XYZIRTF) + offset = addPointField(ros_msg, "feature", 1, sensor_msgs::PointField::UINT8, offset); +#endif + + // + // copy points from point cloud of `PointXYZIRTF` to `PointCloud2` + // + ... + +``` + diff --git a/src/airy/rslidar_sdk-main/doc/howto/05_how_to_change_point_type_CN.md b/src/airy/rslidar_sdk-main/doc/howto/05_how_to_change_point_type_CN.md new file mode 100644 index 0000000..f7c00d7 --- /dev/null +++ b/src/airy/rslidar_sdk-main/doc/howto/05_how_to_change_point_type_CN.md @@ -0,0 +1,162 @@ +# 5 如何改变点类型的定义 + + + +## 5.1 简介 + +本文档介绍如何改变点类型的定义。 + +在项目的```CMakeLists.txt```文件中设置`POINT_TYPE`变量。修改后，需要重新编译整个工程。 + +```cmake +#======================================= +# Custom Point Type (XYZI,XYZIRT, XYZIF, XYZIRTF) +#======================================= +set(POINT_TYPE XYZI) + +``` + + + +## 5.2 XYZI + +`POINT_TYPE`为`XYZI`时，rslidar_sdk使用RoboSense自定义的点类型```PointXYZI```. + +```c++ +struct PointXYZI +{ + float x; + float y; + float z; + uint8_t intensity; +}; +``` + +rslidar_sdk将基于`PointXYZI`的点云，转换为ROS的`PointCloud2`消息，再发布出去。 + +```c++ + sensor_msgs::PointCloud2 ros_msg; + int offset = 0; + offset = addPointField(ros_msg, "x", 1, sensor_msgs::PointField::FLOAT32, offset); + offset = addPointField(ros_msg, "y", 1, sensor_msgs::PointField::FLOAT32, offset); + offset = addPointField(ros_msg, "z", 1, sensor_msgs::PointField::FLOAT32, offset); + offset = addPointField(ros_msg, "intensity", 1, sensor_msgs::PointField::FLOAT32, offset); + + // + // copy points from point cloud of `PointXYZI` to `PointCloud2` + // + ... +``` + +这里`PointCloud2`中的`intensity`是`float`类型，而不是`uint8_t`类型。这是因为大多数基于ROS的程序都希望`float`类型的`intensity`。 + + + +## 5.3 XYZIRT + +`POINT_TYPE`为`XYZIRT`时，rslidar_sdk使用RoboSense自定义的点类型```PointXYZRT```。 + +```c++ +struct PointXYZIRT +{ + float x; + float y; + float z; + uint8_t intensity; + uint16_t ring; + double timestamp; +}; +``` + +rslidar_sdk将基于`PointXYZIRT`的点云，转换为ROS的PointCloud2消息，再发布出去。 + +```c++ + sensor_msgs::PointCloud2 ros_msg; + int offset = 0; + offset = addPointField(ros_msg, "x", 1, sensor_msgs::PointField::FLOAT32, offset); + offset = addPointField(ros_msg, "y", 1, sensor_msgs::PointField::FLOAT32, offset); + offset = addPointField(ros_msg, "z", 1, sensor_msgs::PointField::FLOAT32, offset); + offset = addPointField(ros_msg, "intensity", 1, sensor_msgs::PointField::FLOAT32, offset); +#if defined(POINT_TYPE_XYZIRT) || defined(POINT_TYPE_XYZIRTF) + offset = addPointField(ros_msg, "ring", 1, sensor_msgs::PointField::UINT16, offset); + offset = addPointField(ros_msg, "timestamp", 1, sensor_msgs::PointField::FLOAT64, offset); +#endif + + // + // copy points from point cloud of `PointXYZIRT` to `PointCloud2` + // + ... +``` +## 5.4 XYZIF + +`POINT_TYPE`为`XYZIF`时，rslidar_sdk使用RoboSense自定义的点类型```PointXYZIF```。 + +```c++ +struct PointXYZIF +{ + float x; + float y; + float z; + uint8_t intensity; + uint8_t feature; +}; +``` + +rslidar_sdk将基于`PointXYZIF`的点云，转换为ROS的PointCloud2消息，再发布出去。 + +```c++ + sensor_msgs::PointCloud2 ros_msg; + int offset = 0; + offset = addPointField(ros_msg, "x", 1, sensor_msgs::PointField::FLOAT32, offset); + offset = addPointField(ros_msg, "y", 1, sensor_msgs::PointField::FLOAT32, offset); + offset = addPointField(ros_msg, "z", 1, sensor_msgs::PointField::FLOAT32, offset); + offset = addPointField(ros_msg, "intensity", 1, sensor_msgs::PointField::FLOAT32, offset); +#if defined(POINT_TYPE_XYZIF) || defined(POINT_TYPE_XYZIRTF) + offset = addPointField(ros_msg, "feature", 1, sensor_msgs::PointField::UINT8, offset); +#endif + + // + // copy points from point cloud of `PointXYZIF` to `PointCloud2` + // + ... +``` +## 5.5 XYZIRTF + +`POINT_TYPE`为`XYZIRTF`时，rslidar_sdk使用RoboSense自定义的点类型```PointXYZIRTF```。 + +```c++ +struct PointXYZIRTF +{ + float x; + float y; + float z; + uint8_t intensity; + uint16_t ring; + double timestamp; + uint8_t feature; +}; +``` + +rslidar_sdk将基于`PointXYZIRTF`的点云，转换为ROS的PointCloud2消息，再发布出去。 + +```c++ + sensor_msgs::PointCloud2 ros_msg; + int offset = 0; + offset = addPointField(ros_msg, "x", 1, sensor_msgs::PointField::FLOAT32, offset); + offset = addPointField(ros_msg, "y", 1, sensor_msgs::PointField::FLOAT32, offset); + offset = addPointField(ros_msg, "z", 1, sensor_msgs::PointField::FLOAT32, offset); + offset = addPointField(ros_msg, "intensity", 1, sensor_msgs::PointField::FLOAT32, offset); +#if defined(POINT_TYPE_XYZIRT) || defined(POINT_TYPE_XYZIRTF) + offset = addPointField(ros_msg, "ring", 1, sensor_msgs::PointField::UINT16, offset); + offset = addPointField(ros_msg, "timestamp", 1, sensor_msgs::PointField::FLOAT64, offset); +#endif +#if defined(POINT_TYPE_XYZIF) || defined(POINT_TYPE_XYZIRTF) + offset = addPointField(ros_msg, "feature", 1, sensor_msgs::PointField::UINT8, offset); +#endif + + // + // copy points from point cloud of `PointXYZIRTF` to `PointCloud2` + // + ... +``` + diff --git a/src/airy/rslidar_sdk-main/doc/howto/06_how_to_decode_online_lidar.md b/src/airy/rslidar_sdk-main/doc/howto/06_how_to_decode_online_lidar.md new file mode 100644 index 0000000..6403345 --- /dev/null +++ b/src/airy/rslidar_sdk-main/doc/howto/06_how_to_decode_online_lidar.md @@ -0,0 +1,70 @@ +# 6 How to decode on-line LiDAR + +## 6.1 Introduction + +This document illustrates how to connect to an on-line LiDAR, and send point cloud to ROS. + +Please make sure you have read the LiDAR user-guide and [Intro to parameters](../intro/02_parameter_intro.md) before reading this document. + + + +## 6.2 Steps + +### 6.2.1 Get the LiDAR port number + +Please follow the instructions in LiDAR user-guide, to connect the LiDAR, and set up your computer's ip address. + +Please check the LiDAR user-guide, or use the 3rd-party tool(such as WireShark), to get your LiDAR's MSOP port number and DIFOP port number. The default values are ```msop-6699, difop-7788```. + +### 6.2.2 Set up the configuration file + +#### 6.2.2.1 common part + +```yaml +common: + msg_source: 1 + send_packet_ros: false + send_point_cloud_ros: true +``` + +The message come from the LiDAR, so set ```msg_source = 1```. + +Send point cloud to ROS, so set ```send_point_cloud_ros = true```. + +#### 6.2.2.2 lidar-driver part + +```yaml +lidar: + - driver: + lidar_type: RS128 + msop_port: 6699 + difop_port: 7788 + imu_port: 6688 + start_angle: 0 + end_angle: 360 + min_distance: 0.2 + max_distance: 200 + use_lidar_clock: true +``` + +Set the ```lidar_type``` to your LiDAR type. + +Set the ```msop_port```,```difop_port``` and ```difop_port``` to your LiDAR's port number. + +#### 6.2.2.3 lidar-ros part + +```yaml +ros: + ros_frame_id: rslidar + ros_recv_packet_topic: /rslidar_packets + ros_send_packet_topic: /rslidar_packets + ros_send_imu_data_topic: /rslidar_imu_data + ros_send_point_cloud_topic: /rslidar_points +``` + +Set the ```rslidar_imu_data``` and ```ros_send_point_cloud_topic``` to the topic you want to send to. + +### 6.2.3 Run + +Run the program. + diff --git a/src/airy/rslidar_sdk-main/doc/howto/06_how_to_decode_online_lidar_CN.md b/src/airy/rslidar_sdk-main/doc/howto/06_how_to_decode_online_lidar_CN.md new file mode 100644 index 0000000..7590739 --- /dev/null +++ b/src/airy/rslidar_sdk-main/doc/howto/06_how_to_decode_online_lidar_CN.md @@ -0,0 +1,74 @@ +# 6 如何连接在线雷达 + + + +## 6.1 简介 + +本文档描述如何连接在线雷达，并发送点云数据到ROS。 + +在阅读本文档之前， 请确保已经阅读过雷达用户手册和[参数简介](../intro/02_parameter_intro_CN.md) 。 + + + +## 6.2 步骤 + +### 6.2.1 获取数据端口号 + +根据雷达用户手册连接雷达, 并设置好您的电脑的IP地址。 + +请参考雷达用户手册，或使用第三方工具（如WireShark等）得到雷达的MSOP端口号和DIFOP端口号。端口的默认值分别为```6699```和```7788```。 + +### 6.2.2 设置参数文件 + +设置参数文件```config.yaml```。 + +#### 6.2.2.1 common部分 + +```yaml +common: + msg_source: 1 + send_packet_ros: false + send_point_cloud_ros: true +``` + +消息来源于在线雷达，因此请设置```msg_source=1```。 + +将点云发送到ROS以便查看，因此设置 ```send_point_cloud_ros = true``` 。 + +#### 6.2.2.2 lidar-driver部分 + +```yaml +lidar: + - driver: + lidar_type: RSAIRY + msop_port: 6699 + difop_port: 7788 + imu_port: 6688 + start_angle: 0 + end_angle: 360 + min_distance: 0.2 + max_distance: 200 + use_lidar_clock: true +``` + +将 ```lidar_type``` 设置为LiDAR类型 。 + +设置 ```msop_port``` 、 ```difop_port``` 和 ```imu_port``` 为雷达数据端口号。 + +#### 6.2.2.3 lidar-ros部分 + +```yaml +ros: + ros_frame_id: rslidar + ros_recv_packet_topic: /rslidar_packets + ros_send_packet_topic: /rslidar_packets + ros_send_imu_data_topic: /rslidar_imu_data + ros_send_point_cloud_topic: /rslidar_points +``` + +将 ```rslidar_imu_data``` 和```ros_send_point_cloud_topic``` 设置为发送Imu数据和发送点云的话题。 + +### 6.2.3 运行 + +运行程序。 + diff --git a/src/airy/rslidar_sdk-main/doc/howto/07_online_lidar_advanced_topics.md b/src/airy/rslidar_sdk-main/doc/howto/07_online_lidar_advanced_topics.md new file mode 100644 index 0000000..d5d529c --- /dev/null +++ b/src/airy/rslidar_sdk-main/doc/howto/07_online_lidar_advanced_topics.md @@ -0,0 +1,200 @@ +# 7 Online LiDAR - Advanced Topics + + + +## 7.1 Introduction + +The RoboSense LiDAR may work + ++ in unicast/multicast/broadcast mode, ++ with VLAN layer ++ with user layers. ++ Also rslidar_sdk supports multi-LiDARs. + +This document illustrates how to configure rslidar_sdk in each case. + +Before reading this document, please be sure that you have read: ++ LiDAR user-guide ++ [Intro to parameters](../intro/02_parameter_intro.md) ++ [Decode online LiDAR](./06_how_to_decode_online_lidar.md) + + + +## 7.2 Unicast, Multicast and Broadcast + +### 7.2.1 Broadcast mode + +The Lidar sends MSOP/DIFOP packets to the host machine (rslidar_sdk runs on it). For simplicity, the DIFOP port is ommited here. ++ The Lidar sends to `255.255.255.255` : `6699`, and the host binds to port `6699`. +![](./img/07_01_broadcast.png) + +Below is how to configure `config.yaml`. + +```yaml +common: + msg_source: 1 + send_point_cloud_ros: true + +lidar: + - driver: + lidar_type: RSAIRY + msop_port: 6699 + difop_port: 7788 + imu_port: 6688 + ros: + ros_frame_id: rslidar + ros_send_point_cloud_topic: /rslidar_points +``` + +The `common` part and the `lidar-ros` part is listed here. They will be ommited in the following examples, since they are not changed. + +### 7.2.2 Unicast mode + +To reduce the network load, the Lidar is suggested to work in unicast mode. ++ The Lidar sends to `192.168.1.102` : `6699`, and the host binds to port `6699`. +![](./img/07_02_unicast.png) + +Below is how to configure `config.yaml`. In fact, it same with the broadcast mode. + +```yaml +lidar: + - driver: + lidar_type: RSAIRY + msop_port: 6699 + difop_port: 7788 + imu_port: 6688 +``` + +### 7.2.3 Multicast mode + +The Lidar may also works in multicast mode. ++ The lidar sends to `224.1.1.1`:`6699` ++ The host binds to port `6699`. And it makes local NIC (Network Interface Card) join the multicast group `224.1.1.1`. The local NIC's IP is `192.168.1.102`. +![](./img/07_03_multicast.png) + +Below is how to configure `config.yaml`. + +```yaml +lidar: + - driver: + lidar_type: RSAIRY + msop_port: 6699 + difop_port: 7788 + imu_port: 6688 + group_address: 224.1.1.1 + host_address: 192.168.1.102 +``` + + + +## 7.3 Multiple LiDARs + +### 7.3.1 Different remote ports + +If you have two or more Lidars, it is suggested to set different remote ports. ++ First Lidar sends to `192.168.1.102`:`6699`, and the first driver instance binds to `6699`. ++ Second Lidar sends to `192.168.1.102`:`5599`, and the second driver instance binds to `5599`. +![](./img/07_04_multi_lidars_port.png) + +Below is how to configure `config.yaml`. + +```yaml +lidar: + - driver: + lidar_type: RSAIRY + msop_port: 6699 + difop_port: 7788 + imu_port: 6688 + - driver: + lidar_type: RSAIRY + msop_port: 6698 + difop_port: 7789 + imu_port: 6689 +``` + +### 7.3.2 Different remote IPs + +An alternate way is to set different remote IPs. ++ The host has two NICs: `192.168.1.102` and `192.168.1.103`. ++ First Lidar sends to `192.168.1.102`:`6699`, and the first driver instance binds to `192.168.1.102:6699`. ++ Second Lidar sends to `192.168.1.103`:`6699`, and the second driver instance binds to `192.168.1.103:6699`. +![](./img/07_05_multi_lidars_ip.png) + +Below is how to configure `config.yaml`. + +```yaml +lidar: + - driver: + lidar_type: RSAIRY + msop_port: 6699 + difop_port: 7788 + imu_port: 6688 + host_address: 192.168.1.102 + - driver: + lidar_type: RSAIRY + msop_port: 6699 + difop_port: 7788 + imu_port: 6688 + host_address: 192.168.1.103 +``` + + + +## 7.4 VLAN + +In some user cases, The Lidar may work on VLAN. Its packets have a VLAN layer. +![](./img/07_06_vlan_layer.png) + +The driver cannot parse this packet. Instead, it depends on a virtual NIC to strip the VLAN layer. + +Below is an example. ++ The Lidar works on VLAN `80`. It sends packets to `192.168.1.102` : `6699`. The packet has a VLAN layer. ++ Suppose there is a physical NIC `eno1` on the host. It receives packets with VLAN layer. +![](./img/07_07_vlan.png) + +To strip the VLAN layer, create a virtual NIC `eno1.80` on `eno1`, and assign IP `192.168.1.102` to it. + +``` +sudo apt-get install vlan -y +sudo modprobe 8021q + +sudo vconfig add eno1 80 +sudo ifconfig eno1.80 192.168.1.102 up +``` + +Now the driver may take `eno1.80` as a general NIC, and receives packets without VLAN layer. + +```yaml +lidar: + - driver: + lidar_type: RSAIRY + msop_port: 6699 + difop_port: 7788 + imu_port: 6688 +``` + + + +## 7.5 User Layer, Tail Layer + +In some user cases, User may add extra layers before or after the MSOP/DIFOP packet. ++ USER_LAYER is before the packet and TAIL_LAYER is after it. +![](./img/07_08_user_layer.png) + +These extra layers are parts of UDP data. The driver can strip them. + +To strip them, just give their lengths in bytes. + +In the following example, USER_LAYER is 8 bytes, and TAIL_LAYER is 4 bytes. + +```yaml +lidar: + - driver: + lidar_type: RSAIRY + msop_port: 6699 + difop_port: 7788 + imu_port: 6688 + user_layer_bytes: 8 + tail_layer_bytes: 4 +``` + diff --git a/src/airy/rslidar_sdk-main/doc/howto/07_online_lidar_advanced_topics_CN.md b/src/airy/rslidar_sdk-main/doc/howto/07_online_lidar_advanced_topics_CN.md new file mode 100644 index 0000000..ea24357 --- /dev/null +++ b/src/airy/rslidar_sdk-main/doc/howto/07_online_lidar_advanced_topics_CN.md @@ -0,0 +1,199 @@ +# 7 在线雷达 - 高级主题 + + + +## 7.1 简介 + +RoboSense雷达可以工作在如下的场景。 + ++ 单播/组播/广播模式。 ++ 运行在VLAN协议上。 ++ 向Packet加入用户自己的层。 ++ 接入多个雷达。 + +本文描述在这些场景下如何配置rslidar_sdk。 + +在阅读本文档之前， 请确保已经阅读过： ++ 雷达用户手册 ++ [参数介绍](../intro/02_parameter_intro_CN.md) ++ [连接在线雷达](./06_how_to_decode_online_lidar_CN.md) + + + +## 7.2 单播、组播、广播 + +### 7.2.1 广播 + +雷达发送 MSOP/DIFOP Packet到电脑主机。为简单起见，如下的图没有显示DIFOP端口。 ++ 雷达发送Packet到 `255.255.255.255` : `6699`, rslidar_sdk绑定到主机的端口 `6699`. +![](./img/07_01_broadcast.png) + +如下是配置`config.yaml`的方式。 + +```yaml +common: + msg_source: 1 + send_point_cloud_ros: true + +lidar: + - driver: + lidar_type: RSAIRY + msop_port: 6699 + difop_port: 7788 + imu_port: 6688 + ros: + ros_frame_id: rslidar + ros_send_point_cloud_topic: /rslidar_points +``` + +这里列出了`common`部分和`lidar-ros`部分的设置。这两部分设置将在本文中后面的例子沿用，不再列出。 + +### 7.2.2 单播 + +为了减少网络负载，建议雷达使用单播模式。 ++ 雷达发送Packet到 `192.168.1.102` : `6699`, rslidar_sdk绑定端口 `6699`。 +![](./img/07_02_unicast.png) + +如下是配置`config.yaml`的方式。这实际上与广播的方式一样。 + +```yaml +lidar: + - driver: + lidar_type: RSAIRY + msop_port: 6699 + difop_port: 7788 + imu_port: 6688 +``` + +### 7.2.3 组播 + +雷达也可以工作在组播模式。 ++ 雷达发送Packet到 `224.1.1.1`:`6699` ++ rslidar_sdk绑定到端口 `6699`。同时它将IP地址为`192.168.1.102`的本地网络接口加入组播组`224.1.1.1`。 +![](./img/07_03_multicast.png) + +如下是配置`config.yaml`的方式。 + +```yaml +lidar: + - driver: + lidar_type: RSAIRY + msop_port: 6699 + difop_port: 7788 + imu_port: 6688 + group_address: 224.1.1.1 + host_address: 192.168.1.102 +``` + + + +## 7.3 多个雷达的情况 + +### 7.3.1 不同的目标端口 + +如果有两个或多个雷达，首选的配置是让它们有不同的目标端口。 ++ 第一个雷达发送Packet到 `192.168.1.102`:`6699`, 给rslidar_sdk配置的第一个driver节点绑定到`6699`。 ++ 第二个雷达发送Packet到 `192.168.1.102`:`5599`, 给rslidar_sdk配置的第二个driver节点绑定到`5599`。 +![](./img/07_04_multi_lidars_port.png) + +如下是配置`config.yaml`的方式。 + +```yaml +lidar: + - driver: + lidar_type: RSAIRY + msop_port: 6699 + difop_port: 7788 + imu_port: 6688 + - driver: + lidar_type: RSAIRY + msop_port: 6698 + difop_port: 7789 + imu_port: 6689 +``` + +### 7.3.2 不同的目标IP + +也可以让多个雷达使用不同的目标IP。 ++ 主机有两个网卡， IP地址分别为`192.168.1.102` 和 `192.168.1.103`。 ++ 第一个雷达发送Packet到 `192.168.1.102`:`6699`, 给rslidar_sdk配置的第一个driver节点绑定到`192.168.1.102:6699`。 ++ 第二个雷达发送Packet到 `192.168.1.103`:`6699`, 给rslidar_sdk配置的第二个driver节点绑定到`192.168.1.103:6699`。 +![](./img/07_05_multi_lidars_ip.png) + +如下是配置`config.yaml`的方式。 + +```yaml +lidar: + - driver: + lidar_type: RSAIRY + msop_port: 6699 + difop_port: 7788 + imu_port: 6688 + host_address: 192.168.1.102 + - driver: + lidar_type: RSAIRY + msop_port: 6699 + difop_port: 7788 + imu_port: 6688 + host_address: 192.168.1.103 +``` + + + +## 7.4 VLAN + +在某些场景下，雷达工作在VLAN层之上。MSOP/DIFOP Packet有VLAN层，如下图。 +![](./img/07_06_vlan_layer.png) + +rslidar_sdk不能解析VLAN层。 + +需要一个虚拟网卡来剥除掉这一层。举例如下。 + ++ 雷达工作在VLAN id为`80`的VLAN层上。它发送Packet到`192.168.1.102` : `6699`，Packet有VLAN层。 ++ 假设主机上有一个支持VLAN的物理网卡`eno1`. 它接收带VLAN层的Packet。 +![](./img/07_07_vlan.png) + +要剥离VLAN层，需要基于`eno1`，创建一个虚拟网卡`eno1.80`, 并且将它的IP设置为`192.168.1.102`。 + +```shell +sudo apt-get install vlan -y +sudo modprobe 8021q + +sudo vconfig add eno1 80 +sudo ifconfig eno1.80 192.168.1.102 up +``` + +现在，rslidar_sdk可以将`eno1.80`当做一个一般的网卡来处理，从这里接收不带VLAN层的Packet. + +```yaml +lidar: + - driver: + lidar_type: RSAIRY + msop_port: 6699 + difop_port: 7788 + imu_port: 6688 +``` + + + +## 7.5 User Layer, Tail Layer + +在某些场景下，用户可能在MSOP/DIFOP数据前后加入自己的层。 ++ 在前面的是USER_LAYER，在后面的是TAIL_LAYER。 +![](./img/07_08_user_layer.png) + +这两个层是UDP数据的一部分，所以rslidar_sdk可以自己剥除它们。只需要指出这两个层的长度就可以了。 + +在下面的例子中，USER_LAYER是8字节，TAIL_LAYER是4字节。 + +```yaml +lidar: + - driver: + lidar_type: RSAIRY + msop_port: 6699 + difop_port: 7788 + imu_port: 6688 + user_layer_bytes: 8 + tail_layer_bytes: 4 +``` + diff --git a/src/airy/rslidar_sdk-main/doc/howto/08_how_to_decode_pcap_file.md b/src/airy/rslidar_sdk-main/doc/howto/08_how_to_decode_pcap_file.md new file mode 100644 index 0000000..8f3d3d3 --- /dev/null +++ b/src/airy/rslidar_sdk-main/doc/howto/08_how_to_decode_pcap_file.md @@ -0,0 +1,79 @@ +# 8 How to decode PCAP file + + + +## 8.1 Introduction + +This document illustrates how to decode PCAP file, and send point cloud to ROS. + +Please make sure you have read the LiDAR user-guide and [Intro to parameters](../intro/02_parameter_intro.md) before reading this document. + + + +## 8.2 Steps + +### 8.2.1 Get the LiDAR port number + +Please check the LiDAR user-guide, or use the 3rd-party tool(such as WireShark), to get your LiDAR's MSOP port number and DIFOP port number. The default values are ```msop-6699, difop-7788```. + +### 8.2.2 Set up the configuration file + +Set up the configuration file `config.yaml`. + +#### 8.2.2.1 common part + +```yaml +common: + msg_source: 3 + send_packet_ros: false + send_point_cloud_ros: true +``` + +The messages come from the PCAP bag, so set ```msg_source = 3```. + +Send point cloud to ROS, so set ```send_point_cloud_ros = true```. + +#### 8.2.2.2 lidar-driver part + +```yaml +lidar: + - driver: + lidar_type: RSAIRY + msop_port: 6699 + difop_port: 7788 + imu_port: 6688 + start_angle: 0 + end_angle: 360 + min_distance: 0.2 + max_distance: 200 + use_lidar_clock: true + pcap_path: /home/robosense/lidar.pcap +``` + +Set the ```pcap_path``` to the absolute path of the PCAP file. + +Set the ```lidar_type``` to your LiDAR type. + +Set the ```msop_port```,```difop_port``` and ```difop_port``` to your LiDAR's port number. + +#### 8.2.2.3 lidar-ros part + +```yaml +ros: + ros_frame_id: rslidar + ros_recv_packet_topic: /rslidar_packets + ros_send_packet_topic: /rslidar_packets + ros_send_imu_data_topic: /rslidar_imu_data + ros_send_point_cloud_topic: /rslidar_points +``` + +Set the ```rslidar_imu_data``` and ```ros_send_point_cloud_topic``` to the topic you want to send to. + +### 8.2.3 Run + +Run the program. + + + + + diff --git a/src/airy/rslidar_sdk-main/doc/howto/08_how_to_decode_pcap_file_CN.md b/src/airy/rslidar_sdk-main/doc/howto/08_how_to_decode_pcap_file_CN.md new file mode 100644 index 0000000..2704bf1 --- /dev/null +++ b/src/airy/rslidar_sdk-main/doc/howto/08_how_to_decode_pcap_file_CN.md @@ -0,0 +1,74 @@ +# 8 如何解码PCAP文件 + + + +## 8.1 简介 + +本文档展示如何解码PCAP文件, 并发送点云数据到ROS。 + +在阅读本文档之前，请确保已阅读雷达用户手册和 [参数简介](../intro/02_parameter_intro_CN.md) 。 + + + +## 8.2 步骤 + +### 8.2.1 获取数据的端口号 + +请参考雷达用户手册，或者使用第三方工具（WireShark等）抓包，得到雷达的目标MSOP端口和目标DIFOP端口。端口的默认值分别为`6699`和`7788`。 + +### 8.2.2 设置参数文件 + +设置参数文件```config.yaml```。 + +#### 8.2.2.1 common部分 + +```yaml +common: + msg_source: 3 + send_packet_ros: false + send_point_cloud_ros: true +``` + +消息来自PCAP包，所以设置 ```msg_source = 3``` 。 + +将点云发送到ROS以便查看，所以设置 ```send_point_cloud_ros = true``` 。 + +#### 8.2.2.2 lidar-driver部分 + +```yaml +lidar: + - driver: + lidar_type: RSAIRY + msop_port: 6699 + difop_port: 7788 + imu_port: 6688 + start_angle: 0 + end_angle: 360 + min_distance: 0.2 + max_distance: 200 + use_lidar_clock: true + pcap_path: /home/robosense/lidar.pcap +``` + +将```pcap_path``` 设置为PCAP文件的全路径。 + +将 ```lidar_type``` 设置为LiDAR类型。 + +设置 ```msop_port``` 、 ```difop_port``` 和 ```imu_port``` 为雷达数据端口号。 + +#### 8.2.2.3 lidar-ros部分 + +```yaml +ros: + ros_frame_id: rslidar + ros_recv_packet_topic: /rslidar_packets + ros_send_packet_topic: /rslidar_packets + ros_send_imu_data_topic: /rslidar_imu_data + ros_send_point_cloud_topic: /rslidar_points +``` + +将 ```rslidar_imu_data``` 和```ros_send_point_cloud_topic``` 设置为发送Imu数据和发送点云的话题。 + +### 8.2.3 运行 + +运行程序。 diff --git a/src/airy/rslidar_sdk-main/doc/howto/09_pcap_file_advanced_topics.md b/src/airy/rslidar_sdk-main/doc/howto/09_pcap_file_advanced_topics.md new file mode 100644 index 0000000..d17e39f --- /dev/null +++ b/src/airy/rslidar_sdk-main/doc/howto/09_pcap_file_advanced_topics.md @@ -0,0 +1,94 @@ +# 9 PCAP File - Advanced Topics + + + +## 9.1 Introduction + +The RoboSense LiDAR may work ++ in unicast/multicast/broadcast mode, ++ with VLAN layer ++ with user layers. ++ Also rslidar_sdk supports multi-LiDARs. + +This document illustrates how to configure rslidar_sdk in each case. + +Before reading this document, please be sure that you have read: ++ LiDAR user-guide ++ [Intro to parameters](../intro/02_parameter_intro.md) ++ [Online LiDAR - Advanced Topics](./07_online_lidar_advanced_topics.md) + + + +## 9.2 General Case + +Generally, below code is for decoding a PCAP file in these cases. ++ Broadcast/multicast/unicast mode ++ There are multiple LiDars in a file. + +```yaml +common: + msg_source: 3 + send_point_cloud_ros: true + +lidar: + - driver: + lidar_type: RSAIRY + pcap_path: /home/robosense/lidar.pcap + msop_port: 6699 + difop_port: 7788 + imu_port: 6688 + ros: + ros_frame_id: rslidar + ros_send_point_cloud_topic: /rslidar_points +``` + +The only exception is "Multiple Lidars with same ports but different IPs", which is not supported now. + + + +## 9.3 VLAN + +In some user cases, The LiDar may work on VLAN. Its packets have a VLAN layer. +![](./img/07_06_vlan_layer.png) + +rs_driver decodes PCAP file and gets all parts of MSOP packets, including the VLAN layer. + +To strip the VLAN layer, just set `use_vlan: true`. + +```yaml +lidar: + - driver: + lidar_type: RSAIRY + pcap_path: /home/robosense/lidar.pcap + msop_port: 6699 + difop_port: 7788 + imu_port: 6688 + use_vlan: true +``` + + + +## 9.4 User Layer, Tail Layer + +In some user cases, User may add extra layers before or/and after the MSOP/DIFOP packet. ++ USER_LAYER is before the packet and TAIL_LAYER is after it. +![](./img/07_08_user_layer.png) + +These extra layers are parts of UDP data. The driver can strip them. + +To strip them, just give their lengths in bytes. + +In the following example, USER_LAYER is 8 bytes, and TAIL_LAYER is 4 bytes. + +```yaml +lidar: + - driver: + lidar_type: RSAIRY + pcap_path: /home/robosense/lidar.pcap + msop_port: 6699 + difop_port: 7788 + imu_port: 6688 + user_layer_bytes: 8 + tail_layer_bytes: 4 +``` + diff --git a/src/airy/rslidar_sdk-main/doc/howto/09_pcap_file_advanced_topics_CN.md b/src/airy/rslidar_sdk-main/doc/howto/09_pcap_file_advanced_topics_CN.md new file mode 100644 index 0000000..b6d4bf2 --- /dev/null +++ b/src/airy/rslidar_sdk-main/doc/howto/09_pcap_file_advanced_topics_CN.md @@ -0,0 +1,92 @@ +# 9 PCAP文件 - 高级主题 + + + +## 9.1 简介 + +RoboSense雷达可以工作在如下场景。 ++ 单播/组播/广播模式 ++ 运行在VLAN协议上 ++ 向Packet中加入用户自己的层 ++ 接入多个雷达 + +本文说明在每种场景下，如何配置rslidar_sdk。 + +在阅读本文之前，请先阅读： ++ 雷达用户使用手册 ++ [参数介绍](../intro/02_parameter_intro_CN.md) ++ [在线雷达-高级主题](./07_online_lidar_advanced_topics_CN.md) + + + +## 9.2 一般场景 + +在下列场景下，使用如下配置解码PCAP文件。 ++ 广播/组播/单播模式 ++ PCAP文件中有多个雷达 + +```yaml +common: + msg_source: 3 + send_point_cloud_ros: true + +lidar: + - driver: + lidar_type: RSAIRY + pcap_path: /home/robosense/lidar.pcap + msop_port: 6699 + difop_port: 7788 + imu_port: 6688 + ros: + ros_frame_id: rslidar + ros_send_point_cloud_topic: /rslidar_points +``` + +一个例外是：PCAP文件中有多个雷达数据，但这些雷达目的端口相同，使用不同的目的IP地址来区分。这种情况不支持。 + + + +## 9.3 VLAN + +有些场景下，雷达工作在VLAN环境下。这时MSOP/DIFOP包带VLAN层，如下图。 +![](./img/07_06_vlan_layer.png) + +rs_driver使用libpcap库解析PCAP文件，可以得到完整的、包括VLAN层的MSOP/DIFOP包。 + +要剥除VLAN层，只需要设置`use_vlan: true`。 + +```yaml +lidar: + - driver: + lidar_type: RSAIRY + pcap_path: /home/robosense/lidar.pcap + msop_port: 6699 + difop_port: 7788 + imu_port: 6688 + use_vlan: true +``` + + + +## 9.4 User Layer, Tail Layer + +某些场景下，用户可能在MSOP/DIFOP数据前后加入自己的层。 ++ USER_LAYER 在MSOP/DIFOP数据之前，TAIL_LAYER在MSOP/DIFOP数据之后。 +![](./img/07_08_user_layer.png) + +这些层是UDP数据的一部分，所以rs_driver可以自己剥除他们。只需要告诉它每个层的字节数就可以。 + +如下的例子中，指定USER_LAYER为8字节，TAIL_LAYER为4字节。 + +```yaml +lidar: + - driver: + lidar_type: RSAIRY + pcap_path: /home/robosense/lidar.pcap + msop_port: 6699 + difop_port: 7788 + imu_port: 6688 + user_layer_bytes: 8 + tail_layer_bytes: 4 +``` + diff --git a/src/airy/rslidar_sdk-main/doc/howto/10_how_to_use_coordinate_transformation.md b/src/airy/rslidar_sdk-main/doc/howto/10_how_to_use_coordinate_transformation.md new file mode 100644 index 0000000..ce5c5ec --- /dev/null +++ b/src/airy/rslidar_sdk-main/doc/howto/10_how_to_use_coordinate_transformation.md @@ -0,0 +1,80 @@ +# 10 How to use coordinate transformation + + + +## 10.1 Introduction + +rslidar_sdk can transform the coordinate of point cloud. This document illustrate how to do so. + +Please check the [Intro to hiding parameters](../intro/03_hiding_parameters_intro.md) for more details. Here is an example of the config.yaml. + + + +## 10.2 Dependencies + +rslidar_sdk depends on the libeigen library to do coordinate transformation. Please install it first. + +```bash +sudo apt-get install libeigen3-dev +``` + + + +## 10.3 Compile + +To enable transformation, set the CMake option ```ENABLE_TRANSFORM```to be ```ON```. + +- Compile directly + + ```bash + cmake -DENABLE_TRANSFORM=ON .. + ``` + +- ROS + + ```bash + catkin_make -DENABLE_TRANSFORM=ON + ``` + +- ROS2 + + ```bash + colcon build --cmake-args '-DENABLE_TRANSFORM=ON' + ``` + + + +## 10.4 Set LiDAR parameters + +In the `lidar-driver` part of `config.yaml`, set the hiding parameter`x`, `y`, `z`, `roll`, `pitch` ,`yaw`. + +```yaml +common: + msg_source: 1 + send_packet_ros: false + send_point_cloud_ros: true +lidar: + - driver: + lidar_type: RSAIRY + msop_port: 6699 + difop_port: 7788 + imu_port: 6688 + start_angle: 0 + end_angle: 360 + min_distance: 0.2 + max_distance: 200 + use_lidar_clock: true + pcap_path: /home/robosense/lidar.pcap + x: 1 + y: 0 + z: 2.5 + roll: 0.1 + pitch: 0.2 + yaw: 1.57 +``` + + + +## 10.5 Run + +Run the program. diff --git a/src/airy/rslidar_sdk-main/doc/howto/10_how_to_use_coordinate_transformation_CN.md b/src/airy/rslidar_sdk-main/doc/howto/10_how_to_use_coordinate_transformation_CN.md new file mode 100644 index 0000000..6af570e --- /dev/null +++ b/src/airy/rslidar_sdk-main/doc/howto/10_how_to_use_coordinate_transformation_CN.md @@ -0,0 +1,80 @@ +# 10 如何使用坐标变换功能 + + + +## 10.1 简介 + +rslidar_sdk支持对点云进行坐标变换，本文档展示如何作这种变换。 + +在阅读本文档之前，请确保已阅读雷达用户手册和[隐藏参数介绍](../intro/03_hiding_parameters_intro_CN.md)。 + + + +## 10.2 依赖库 + +rslidar_sdk的坐标变换基于libeigen库，所以要先安装它。 + +```bash +sudo apt-get install libeigen3-dev +``` + + + +## 10.3 编译 + +要启用坐标变换，编译rslidar_sdk时，需要将```ENABLE_TRANSFORM```选项设置为```ON```. + +- 直接编译 + + ```bash + cmake -DENABLE_TRANSFORM=ON .. + ``` + +- ROS + + ```bash + catkin_make -DENABLE_TRANSFORM=ON + ``` + +- ROS2 + + ```bash + colcon build --cmake-args '-DENABLE_TRANSFORM=ON' + ``` + + + +## 10.4 设置雷达参数 + +在`config.yaml`中，设置`lidar-lidar`部分的参数`x`、, `y`、 `z`、 `roll`、 `pitch` 、`yaw`。 + +```yaml +common: + msg_source: 1 + send_packet_ros: false + send_point_cloud_ros: true +lidar: + - driver: + lidar_type: RSAIRY + msop_port: 6699 + difop_port: 7788 + imu_port: 6688 + start_angle: 0 + end_angle: 360 + min_distance: 0.2 + max_distance: 200 + use_lidar_clock: true + pcap_path: /home/robosense/lidar.pcap + x: 1 + y: 0 + z: 2.5 + roll: 0.1 + pitch: 0.2 + yaw: 1.57 +``` + + + +## 10.5 运行 + +运行程序。 diff --git a/src/airy/rslidar_sdk-main/doc/howto/11_how_to_record_replay_packet_rosbag.md b/src/airy/rslidar_sdk-main/doc/howto/11_how_to_record_replay_packet_rosbag.md new file mode 100644 index 0000000..4cd0942 --- /dev/null +++ b/src/airy/rslidar_sdk-main/doc/howto/11_how_to_record_replay_packet_rosbag.md @@ -0,0 +1,110 @@ +# 11 How to record and replay Packet rosbag + + + +## 11.1 Introduction + +This document illustrates how to record and replay MSOP/DIFOP Packet rosbag. + +It is possible to record the point cloud message into a rosbag and replay it, but the point cloud rosbag is very large. rslidar_sdk provides a better way - record packet rosbag and replay it. + +Please be sure you have read the LiDAR user-guide and [Connect to online LiDAR and send point cloud through ROS](./06_how_to_decode_online_lidar.md). + + + +## 11.2 Record + +### 11.2.1 Send packet to ROS + +Here suppose that you have connected to an on-line LiDAR, and have sent the point cloud to ROS. + + +```yaml +common: + msg_source: 1 + send_packet_ros: true + send_point_cloud_ros: true +``` + +To record packets, set ```send_packet_ros = true```. + +### 11.2.2 Record the topic of packet + +To change the topic of packet, change ```ros_send_packet_topic```. This topic sends out both MSOP and DIFOP packets. + +```yaml +ros: + ros_frame_id: rslidar + ros_recv_packet_topic: /rslidar_packets + ros_send_packet_topic: /rslidar_packets + ros_send_imu_data_topic: /rslidar_imu_data + ros_send_point_cloud_topic: /rslidar_points +``` + +Record rosbag as below. + +```sh +rosbag record /rslidar_packets -O bag +``` + + + +## 11.3 Replay + +Suppose you have recorded a rosbag, which contains MSOP/DIFOP packets with the topic ```/rslidar_packets```. + +### 11.3.1 Set Packet Source + +In `config.yaml`, set the `common` part. + +```yaml +common: + msg_source: 2 + send_packet_ros: false + send_point_cloud_ros: true +``` + +Packet is from the ROS, so set ```msg_source = 2```. + +To send point cloud to ROS, set ```send_point_cloud_ros = true```. + +### 11.3.2 Set parameters of Lidar + +In `config.yaml`, set the `lidar-driver` part. + +```yaml +lidar: + - driver: + lidar_type: RSAIRY + msop_port: 6699 + difop_port: 7788 + imu_port: 6688 + start_angle: 0 + end_angle: 360 + min_distance: 0.2 + max_distance: 200 + use_lidar_clock: true +``` + +Set the ```lidar_type``` to your LiDAR type. + +### 11.3.3 Set Topic of packet. + +In `config.yaml`, set the `lidar-ros` part. + +```yaml +ros: + ros_frame_id: rslidar + ros_recv_packet_topic: /rslidar_packets + ros_send_packet_topic: /rslidar_packets + ros_send_imu_data_topic: /rslidar_imu_data + ros_send_point_cloud_topic: /rslidar_points +``` + +To receive MSOP/DIFOP packest, set ```ros_recv_packet_topic``` to the topic in the rosbag. + +### 11.3.4 Run + +Run the demo, and replay rosbag. + + diff --git a/src/airy/rslidar_sdk-main/doc/howto/11_how_to_record_replay_packet_rosbag_CN.md b/src/airy/rslidar_sdk-main/doc/howto/11_how_to_record_replay_packet_rosbag_CN.md new file mode 100644 index 0000000..eb69586 --- /dev/null +++ b/src/airy/rslidar_sdk-main/doc/howto/11_how_to_record_replay_packet_rosbag_CN.md @@ -0,0 +1,113 @@ +# 11 如何录制与回放 Packet rosbag + + + +## 11.1 简介 + +本文档展示如何记录与回放MSOP/DIFOP rosbag。 + +使用ROS可以录制点云rosbag消息并回放，但点云包非常大，所以rslidar_sdk提供更好的选择，也就是录制Packet rosbag并回放。 + +在阅读本文档之前, 请先阅读雷达用户手册和 [连接在线雷达并发送点云到ROS](./06_how_to_decode_online_lidar_CN.md) 。 + + + +## 11.2 录制 + +### 11.2.1 将MSOP/DIFOP Packet发送至ROS + +这里假设已经连接在线雷达，并能发送点云到ROS。 + +```yaml +common: + msg_source: 1 + send_packet_ros: true + send_point_cloud_ros: true +``` + +要录制Packet, 设置 ```send_packet_ros = true```。 + +### 11.2.2 根据话题录制rosbag + +修改```ros_send_packet_topic```, 来改变发送的话题。这个话题包括MSOP Packet和DIFOP Packet。 + +```yaml +ros: + ros_frame_id: rslidar + ros_recv_packet_topic: /rslidar_packets + ros_send_packet_topic: /rslidar_packets + ros_send_imu_data_topic: /rslidar_imu_data + ros_send_point_cloud_topic: /rslidar_points +``` + +ROS录制rosbag的指令如下。 + +```bash +rosbag record /rslidar_packets -O bag +``` + + + +## 11.3 回放 + +假设录制了一个rosbag，其中包含话题为 `/rslidar_packets` 的MSOP/DIFOP Packet。 + +### 11.3.1 设置Packet源 + +配置`config.yaml`的`common`部分。 + +```yaml +common: + msg_source: 2 + send_packet_ros: false + send_point_cloud_ros: true +``` + +MSOP/DIFOP Packet来自ROS rosbag，因此设置 ```msg_source = 2``` 。 + +将点云发送到ROS，因此设置 ```send_point_cloud_ros = true```。 + +### 11.3.2 设置雷达参数 + +配置`config.yaml`的`lidar-driver`部分。 + +```yaml +lidar: + - driver: + lidar_type: RSAIRY + msop_port: 6699 + difop_port: 7788 + imu_port: 6688 + start_angle: 0 + end_angle: 360 + min_distance: 0.2 + max_distance: 200 + use_lidar_clock: true +``` + +将 ```lidar_type``` 设置为LiDAR类型 。 + +### 11.3.3 设置接收Packet的主题 + +设置`config.yaml`的`lidar-ros`部分。 + +```yaml +ros: + ros_frame_id: rslidar + ros_recv_packet_topic: /rslidar_packets + ros_send_packet_topic: /rslidar_packets + ros_send_imu_data_topic: /rslidar_imu_data + ros_send_point_cloud_topic: /rslidar_points +``` + +将 ```ros_recv_packet_topic``` 设置为rosbag中MSOP/DIFOP Packet的话题。 + + + +### 11.3.4 运行 + +运行程序，回放rosbag。 + + + + diff --git a/src/airy/rslidar_sdk-main/doc/howto/12_how_to_create_deb.md b/src/airy/rslidar_sdk-main/doc/howto/12_how_to_create_deb.md new file mode 100644 index 0000000..b183eaa --- /dev/null +++ b/src/airy/rslidar_sdk-main/doc/howto/12_how_to_create_deb.md @@ -0,0 +1,35 @@ +# 12 How to create deb + + + +## 12.1 Introduction + +Generating a ".deb" installable file is useful. + + + +## 12.2 Create deb + +Just run the shell script: +``` +./create_debian.sh +``` +The deb file will be generated in the parent directory of `rslidar_sdk`. + + +## 12.3 Use the deb + +Install the deb and set the right config_path. If leave the config_path empty, it will use the `config.yaml` in the ros package path. + +``` + + + + + + + +``` + + + diff --git a/src/airy/rslidar_sdk-main/doc/howto/img/04_01_packet_rosbag.png b/src/airy/rslidar_sdk-main/doc/howto/img/04_01_packet_rosbag.png new file mode 100644 index 0000000..90d823b Binary files /dev/null and b/src/airy/rslidar_sdk-main/doc/howto/img/04_01_packet_rosbag.png differ diff --git a/src/airy/rslidar_sdk-main/doc/howto/img/07_01_broadcast.png b/src/airy/rslidar_sdk-main/doc/howto/img/07_01_broadcast.png new file mode 100644 index 0000000..ebf2f4c Binary files /dev/null and b/src/airy/rslidar_sdk-main/doc/howto/img/07_01_broadcast.png differ diff --git a/src/airy/rslidar_sdk-main/doc/howto/img/07_02_unicast.png b/src/airy/rslidar_sdk-main/doc/howto/img/07_02_unicast.png new file mode 100644 index 0000000..65964a2 Binary files /dev/null and b/src/airy/rslidar_sdk-main/doc/howto/img/07_02_unicast.png differ diff --git a/src/airy/rslidar_sdk-main/doc/howto/img/07_03_multicast.png b/src/airy/rslidar_sdk-main/doc/howto/img/07_03_multicast.png new file mode 100644 index 0000000..fc39a8d Binary files /dev/null and b/src/airy/rslidar_sdk-main/doc/howto/img/07_03_multicast.png differ diff --git a/src/airy/rslidar_sdk-main/doc/howto/img/07_04_multi_lidars_port.png b/src/airy/rslidar_sdk-main/doc/howto/img/07_04_multi_lidars_port.png new file mode 100644 index 0000000..c426ca9 Binary files /dev/null and b/src/airy/rslidar_sdk-main/doc/howto/img/07_04_multi_lidars_port.png differ diff --git a/src/airy/rslidar_sdk-main/doc/howto/img/07_05_multi_lidars_ip.png b/src/airy/rslidar_sdk-main/doc/howto/img/07_05_multi_lidars_ip.png new file mode 100644 index 0000000..48e1e13 Binary files /dev/null and b/src/airy/rslidar_sdk-main/doc/howto/img/07_05_multi_lidars_ip.png differ diff --git a/src/airy/rslidar_sdk-main/doc/howto/img/07_06_vlan_layer.png b/src/airy/rslidar_sdk-main/doc/howto/img/07_06_vlan_layer.png new file mode 100644 index 0000000..8d678fc Binary files /dev/null and b/src/airy/rslidar_sdk-main/doc/howto/img/07_06_vlan_layer.png differ diff --git a/src/airy/rslidar_sdk-main/doc/howto/img/07_07_vlan.png b/src/airy/rslidar_sdk-main/doc/howto/img/07_07_vlan.png new file mode 100644 index 0000000..55f224b Binary files /dev/null and b/src/airy/rslidar_sdk-main/doc/howto/img/07_07_vlan.png differ diff --git a/src/airy/rslidar_sdk-main/doc/howto/img/07_08_user_layer.png b/src/airy/rslidar_sdk-main/doc/howto/img/07_08_user_layer.png new file mode 100644 index 0000000..d7b31a0 Binary files /dev/null and b/src/airy/rslidar_sdk-main/doc/howto/img/07_08_user_layer.png differ diff --git a/src/airy/rslidar_sdk-main/doc/intro/02_parameter_intro.md b/src/airy/rslidar_sdk-main/doc/intro/02_parameter_intro.md new file mode 100644 index 0000000..dec4886 --- /dev/null +++ b/src/airy/rslidar_sdk-main/doc/intro/02_parameter_intro.md @@ -0,0 +1,272 @@ +# 2 Introduction to Parameters + +rslidar_sdk reads parameters from the configuration file ```config.yaml```, which is stored in ```rslidar_sdk/config```. + +`config.yaml` contains two parts, the `common` part and the `lidar` part. + +rslidar_sdk supports multi-LiDARs case. The `common` part is shared by all LiDARs, while in the `lidar` part, each child node is for an individual Lidar. + +**config.yaml is indentation sensitive! Please make sure the indentation is not changed after adjusting the parameters!** + + + +## 2.1 Common + +The `common` part specifies the source of LiDAR packets, and where to publish point clouds and packets. + +```yaml +common: + msg_source: 1 + send_packet_ros: false + send_point_cloud_ros: false +``` + +- msg_source + + - 0 -- Unused. Never set this parameter to 0. + + - 1 -- LiDAR packets come from on-line LiDARs. For more details, please refer to [Connect to online LiDAR and send point cloud through ROS](../howto/06_how_to_decode_online_lidar.md) + + - 2 -- LiDAR packets come from ROS/ROS2. It is used to decode from an off-line rosbag. For more details, please refer to [Record rosbag & Replay it](../howto/11_how_to_record_replay_packet_rosbag.md) + + - 3 -- LiDAR packets come from a PCAP bag. For more details, please refer to [Decode PCAP file and send point cloud through ROS](../howto/08_how_to_decode_pcap_file.md) + +- send_packet_ros + + - true -- LiDAR packets will be sent to ROS/ROS2. + +*The ROS Packet message is of a customized message type, so you can't print its content via the ROS `echo` command. This option is used to record off-line Packet rosbags. For more details, please refer to the case of msg_source=2.* + +- send_point_cloud_ros + + - true -- The LiDAR point cloud will be sent to ROS/ROS2. + + *The ROS point cloud type is the ROS official defined type -- sensor_msgs/PointCloud2, so it can be visualized on the ROS `rviz` tool directly. It is not suggested to record the point cloud to rosbag, because its size may be very large. Please record Packets instead. Refer to the case of msg_source=2.* + + + +## 2.2 lidar + +The `lidar` part needs to be adjusted for every LiDAR seperately. + +```yaml +lidar: + - driver: + lidar_type: RSM1 # LiDAR type - RS16, RS32, RSBP, RSAIRY, RSHELIOS, RSHELIOS_16P, RS128, RS80, RS48, RSP128, RSP80, RSP48, + # RSM1, RSM1_JUMBO, RSM2, RSM3, RSE1, RSMX. + + msop_port: 6699 # Msop port of lidar + difop_port: 7788 # Difop port of lidar + imu_port: 0 # IMU port of lidar(only for RSAIRY, RSE1), 0 means no imu. + # If you want to use IMU, please first set ENABLE_IMU_DATA_PARSE to ON in CMakeLists.txt + user_layer_bytes: 0 # Bytes of user layer. thers is no user layer if it is 0 + tail_layer_bytes: 0 # Bytes of tail layer. thers is no tail layer if it is 0 + + + min_distance: 0.2 # Minimum distance of point cloud + max_distance: 200 # Maximum distance of point cloud + use_lidar_clock: true # true--Use the lidar clock as the message timestamp + # false-- Use the system clock as the timestamp + dense_points: false # true: discard NAN points; false: reserve NAN points + + ts_first_point: true # true: time-stamp point cloud with the first point; false: with the last point; + # these parameters are used from mechanical lidar + + start_angle: 0 # Start angle of point cloud + end_angle: 360 # End angle of point cloud + + # When msg_source is 3, the following parameters will be used + pcap_repeat: true # true: The pcap bag will repeat play + pcap_rate: 1.0 # Rate to read the pcap file + pcap_path: /home/robosense/lidar.pcap #The path of pcap file + + ros: + ros_frame_id: rslidar #Frame id of packet message and point cloud message + ros_recv_packet_topic: /rslidar_packets #Topic used to receive lidar packets from ROS + ros_send_packet_topic: /rslidar_packets #Topic used to send lidar packets through ROS + ros_send_imu_data_topic: /rslidar_imu_data #Topic used to send imu data through ROS + ros_send_point_cloud_topic: /rslidar_points #Topic used to send point cloud through ROS +``` + +- lidar_type + + Supported LiDAR types are listed in the README file. + +- msop_port, difop_port, imu_port + + The MSOP/DIFOP/IMU port to receive LiDAR packets. *If no data is received, please check these parameters first.* + +- user_layer_bytes, tail_layer_bytes + + The number of bytes of the user layer and tail layer. + +- min_distance, max_distance + + The minimum distance and maximum distance of the point cloud. + +- use_lidar_clock + + - true -- Use the Lidar clock as the message timestamp + - false -- Use the host machine clock as the message timestamp + +- dense_points + + Whether to discard NAN points. The default value is ```false```. + - Discard if ```true``` + - reserve if ```false```. + +- ts_first_point + + Stamp the point cloud with the first point or the last one. Stamp with the first point if ```true```, else stamp with the last point if ```false```. The default value is ```false```. + +- start_angle, end_angle + + The start angle and end angle of the point cloud, which should be in the range of 0~360°. *`start_angle` can be larger than `end_angle`*. + +- pcap_path + + The full path of the PCAP file. Valid if msg_source = 3. + +- ros_send_by_rows + + Meaningful only for Mechanical Lidars, and valid if dense_points = false。 + - true -- send point cloud row by row + - false -- send point cloud clolumn by column + + + +## 2.3 Examples + +### 2.3.1 Single Lidar Case + +Connect to 1 LiDAR of RSM1, and send point cloud to ROS. + +```yaml +common: + msg_source: 1 # 0: not use Lidar + # 1: packet message comes from online Lidar + # 2: packet message comes from ROS or ROS2 + # 3: packet message comes from Pcap file + send_packet_ros: false # true: Send packets through ROS or ROS2(Used to record packet) + send_point_cloud_ros: true # true: Send point cloud through ROS or ROS2 +lidar: + - driver: + lidar_type: RSM1 # LiDAR type - RS16, RS32, RSBP, RSAIRY, RSHELIOS, RSHELIOS_16P, RS128, RS80, RS48, RSP128, RSP80, RSP48, + # RSM1, RSM1_JUMBO, RSM2, RSM3, RSE1, RSMX. + + msop_port: 6699 # Msop port of lidar + difop_port: 7788 # Difop port of lidar + imu_port: 0 # IMU port of lidar(only for RSAIRY, RSE1), 0 means no imu. + # If you want to use IMU, please first set ENABLE_IMU_DATA_PARSE to ON in CMakeLists.txt + user_layer_bytes: 0 # Bytes of user layer. thers is no user layer if it is 0 + tail_layer_bytes: 0 # Bytes of tail layer. thers is no tail layer if it is 0 + + + min_distance: 0.2 # Minimum distance of point cloud + max_distance: 200 # Maximum distance of point cloud + use_lidar_clock: true # true--Use the lidar clock as the message timestamp + # false-- Use the system clock as the timestamp + dense_points: false # true: discard NAN points; false: reserve NAN points + + ts_first_point: true # true: time-stamp point cloud with the first point; false: with the last point; + # these parameters are used from mechanical lidar + + start_angle: 0 # Start angle of point cloud + end_angle: 360 # End angle of point cloud + + # When msg_source is 3, the following parameters will be used + pcap_repeat: true # true: The pcap bag will repeat play + pcap_rate: 1.0 # Rate to read the pcap file + pcap_path: /home/robosense/lidar.pcap #The path of pcap file + + ros: + ros_frame_id: rslidar #Frame id of packet message and point cloud message + ros_recv_packet_topic: /rslidar_packets #Topic used to receive lidar packets from ROS + ros_send_packet_topic: /rslidar_packets #Topic used to send lidar packets through ROS + ros_send_imu_data_topic: /rslidar_imu_data #Topic used to send imu data through ROS + ros_send_point_cloud_topic: /rslidar_points #Topic used to send point cloud through ROS +``` + +### 2.3.2 Multi Lidar Case + +Connect to 1 LiDAR of RSM1, and 1 LiDAR of RSE1, and send point cloud to ROS. + +*Pay attention to the indentation of the `lidar` part* + +```yaml +common: + msg_source: 1 + send_packet_ros: false + send_point_cloud_ros: true +lidar: + - driver: + lidar_type: RSM1 # LiDAR type - RS16, RS32, RSBP, RSAIRY, RSHELIOS, RSHELIOS_16P, RS128, RS80, RS48, RSP128, RSP80, RSP48, + # RSM1, RSM1_JUMBO, RSM2, RSM3, RSE1, RSMX. + + msop_port: 6699 # Msop port of lidar + difop_port: 7788 # Difop port of lidar + imu_port: 0 # IMU port of lidar(only for RSAIRY, RSE1), 0 means no imu. + # If you want to use IMU, please first set ENABLE_IMU_DATA_PARSE to ON in CMakeLists.txt + user_layer_bytes: 0 # Bytes of user layer. thers is no user layer if it is 0 + tail_layer_bytes: 0 # Bytes of tail layer. thers is no tail layer if it is 0 + + + min_distance: 0.2 # Minimum distance of point cloud + max_distance: 200 # Maximum distance of point cloud + use_lidar_clock: true # true--Use the lidar clock as the message timestamp + # false-- Use the system clock as the timestamp + dense_points: false # true: discard NAN points; false: reserve NAN points + + ts_first_point: true # true: time-stamp point cloud with the first point; false: with the last point; + # these parameters are used from mechanical lidar + + start_angle: 0 # Start angle of point cloud + end_angle: 360 # End angle of point cloud + + # When msg_source is 3, the following parameters will be used + pcap_repeat: true # true: The pcap bag will repeat play + pcap_rate: 1.0 # Rate to read the pcap file + pcap_path: /home/robosense/lidar.pcap #The path of pcap file + + ros: + ros_frame_id: rslidar #Frame id of packet message and point cloud message + ros_recv_packet_topic: /left/rslidar_packets #Topic used to receive lidar packets from ROS + ros_send_packet_topic: /left/rslidar_packets #Topic used to send lidar packets through ROS + ros_send_imu_data_topic: /left/rslidar_imu_data #Topic used to send imu data through ROS + ros_send_point_cloud_topic: /left/rslidar_points #Topic used to send point cloud through ROS + + - driver: + lidar_type: RSE1 # LiDAR type - RS16, RS32, RSBP, RSAIRY, RSHELIOS, RSHELIOS_16P, RS128, RS80, RS48, RSP128, RSP80, RSP48, + # RSM1, RSM1_JUMBO, RSM2, RSM3, RSE1, RSMX. + msop_port: 6699 # Msop port of lidar + difop_port: 7788 # Difop port of lidar + + user_layer_bytes: 0 # Bytes of user layer. thers is no user layer if it is 0 + tail_layer_bytes: 0 # Bytes of tail layer. thers is no tail layer if it is 0 + + + min_distance: 0.2 # Minimum distance of point cloud + max_distance: 200 # Maximum distance of point cloud + use_lidar_clock: true # true--Use the lidar clock as the message timestamp + # false-- Use the system clock as the timestamp + dense_points: false # true: discard NAN points; false: reserve NAN points + + ts_first_point: true # true: time-stamp point cloud with the first point; false: with the last point; + # these parameters are used from mechanical lidar + + start_angle: 0 # Start angle of point cloud + end_angle: 360 # End angle of point cloud + + # When msg_source is 3, the following parameters will be used + pcap_repeat: true # true: The pcap bag will repeat play + pcap_rate: 1.0 # Rate to read the pcap file + pcap_path: /home/robosense/lidar.pcap #The path of pcap file + + ros: + ros_frame_id: rslidar #Frame id of packet message and point cloud message + ros_recv_packet_topic: /right/rslidar_packets #Topic used to receive lidar packets from ROS + ros_send_packet_topic: /right/rslidar_packets #Topic used to send lidar packets through ROS + ros_send_imu_data_topic: /right/rslidar_imu_data #Topic used to send imu data through ROS + ros_send_point_cloud_topic: /right/rslidar_points #Topic used to send point cloud through ROS +``` + diff --git a/src/airy/rslidar_sdk-main/doc/intro/02_parameter_intro_CN.md b/src/airy/rslidar_sdk-main/doc/intro/02_parameter_intro_CN.md new file mode 100644 index 0000000..3242796 --- /dev/null +++ b/src/airy/rslidar_sdk-main/doc/intro/02_parameter_intro_CN.md @@ -0,0 +1,276 @@ +# 2 参数介绍 + +rslidar_sdk读取配置文件 ```config.yaml```，得到所有的参数。```config.yaml```在```rslidar_sdk/config```文件夹中。 + +**config.yaml遵循YAML格式。该格式对缩进有严格要求。修改config.yaml之后，请确保每行开头的缩进仍保持一致！** + +config.yaml包括两部分：common部分 和 lidar部分。 + +rslidar_sdk支持多个雷达。common部分为所有雷达共享。lidar部分，每一个子节点对应一个雷达，针对这个雷达的实际情况分别设置。 + + + +## 2.1 common部分 + +common部分设置雷达消息的源（Packet或点云从哪来）和目标（Packet或点云发布到哪去）。 + +```yaml +common: + msg_source: 1 + send_packet_ros: false + send_point_cloud_ros: false +``` + +- msg_source + + - 1 -- 连接在线雷达。更多使用细节，请参考[连接在线雷达并发送点云到ROS](../howto/06_how_to_decode_online_lidar_CN.md)。 + + - 2 -- 离线解析ROS/ROS2的Packet包。更多使用细节，请参考 [录制ROS数据包然后播放它](../howto/11_how_to_record_replay_packet_rosbag_CN.md)。 + + - 3 -- 离线解析PCAP包。更多使用细节，请参考[离线解析PCAP包并发送点云到ROS](../howto/08_how_to_decode_pcap_file_CN.md)。 + +- send_packet_ros + + - true -- 雷达Packet消息将通过ROS/ROS2发出 + + *雷达ROS packet消息为速腾聚创自定义ROS消息，用户使用ROS/ROS2 echo命令不能查看消息的具体内容。这个功能用于录制ROS/ROS2的Packet包，更多使用细节，请参考msg_source=2的情况。 + +- send_point_cloud_ros + + - true -- 雷达点云消息将通过ROS/ROS2发出 + + *点云消息的类型为ROS官方定义的点云类型sensor_msgs/PointCloud2, 用户可以使用Rviz直接查看点云。用户可以录制ROS/ROS2的点云包，但点云包的体积非常大，所以不建议这么做。更好的方式是录制Packet包，请参考send_packet_ros=true的情况。* + + + + + +## 2.2 lidar部分 + +lidar部分根据每个雷达的实际情况进行设置。 + +```yaml +lidar: + - driver: + lidar_type: RSM1 # LiDAR type - RS16, RS32, RSBP, RSAIRY, RSHELIOS, RSHELIOS_16P, RS128, RS80, RS48, RSP128, RSP80, RSP48, + # RSM1, RSM1_JUMBO, RSM2, RSM3, RSE1, RSMX. + + msop_port: 6699 # Msop port of lidar + difop_port: 7788 # Difop port of lidar + imu_port: 0 # IMU port of lidar(only for RSAIRY, RSE1), 0 means no imu. + # If you want to use IMU, please first set ENABLE_IMU_DATA_PARSE to ON in CMakeLists.txt + user_layer_bytes: 0 # Bytes of user layer. thers is no user layer if it is 0 + tail_layer_bytes: 0 # Bytes of tail layer. thers is no tail layer if it is 0 + + + min_distance: 0.2 # Minimum distance of point cloud + max_distance: 200 # Maximum distance of point cloud + use_lidar_clock: true # true--Use the lidar clock as the message timestamp + # false-- Use the system clock as the timestamp + dense_points: false # true: discard NAN points; false: reserve NAN points + + ts_first_point: true # true: time-stamp point cloud with the first point; false: with the last point; + # these parameters are used from mechanical lidar + + start_angle: 0 # Start angle of point cloud + end_angle: 360 # End angle of point cloud + + # When msg_source is 3, the following parameters will be used + pcap_repeat: true # true: The pcap bag will repeat play + pcap_rate: 1.0 # Rate to read the pcap file + pcap_path: /home/robosense/lidar.pcap #The path of pcap file + + ros: + ros_frame_id: rslidar #Frame id of packet message and point cloud message + ros_recv_packet_topic: /rslidar_packets #Topic used to receive lidar packets from ROS + ros_send_packet_topic: /rslidar_packets #Topic used to send lidar packets through ROS + ros_send_imu_data_topic: /rslidar_imu_data #Topic used to send imu data through ROS + ros_send_point_cloud_topic: /rslidar_points #Topic used to send point cloud through ROS +``` + +- lidar_type + + 支持的雷达型号在rslidar_sdk的README文件中列出。 + +- msop_port, difop_port, imu_port + + 接收MSOP/DIFOP/IMU Packet的msop端口号、difop端口号、 Imu端口号。 *若收不到消息，请优先确认这些参数是否配置正确。* + +- user_layer_bytes, tail_layer_bytes + + 用户自定义层和尾部层的字节数。默认为0。 + + +- min_distance, max_distance + + 点云的最小距离和最大距离。这个设置是软件屏蔽，会将区域外的点设置为NAN点，不会减小每帧点云的体积。 + +- use_lidar_clock + + - true -- 使用雷达时间作为消息时间戳。 + - false -- 使用电脑主机时间作为消息时间戳。 + +- dense_points + + 输出的点云中是否剔除NAN points。默认值为false。 + - true 为剔除， + - false为不剔除。 + +- ts_first_point + + 默认值为false。点云的时间戳是否第一个点的时间。true使用第一个点的时间，false使用第一个点的时间。 + +- start_angle, end_angle + + 点云消息的起始角度和结束角度。这个设置是软件屏蔽，将区域外的点设置为NAN点，不会减小每帧点云的体积。 start_angle和end_angle的范围是0~360°，**起始角可以大于结束角**. + +- pcap_path + + pcap包的路径。当 msg_source=3 时有效。 + +- pcap_rate + + pcap包播放的倍率。当 msg_source=3 时有效。 + +- pcap_repeat + + pcap包是否重复播放。当 msg_source=3 时有效。 + + + + +## 2.3 示例 + +### 2.3.1 单台雷达 + +在线连接1台RSM1雷达，并发送点云数据到ROS。 + +```yaml +common: + msg_source: 1 # 0: not use Lidar + # 1: packet message comes from online Lidar + # 2: packet message comes from ROS or ROS2 + # 3: packet message comes from Pcap file + send_packet_ros: false # true: Send packets through ROS or ROS2(Used to record packet) + send_point_cloud_ros: true # true: Send point cloud through ROS or ROS2 +lidar: + - driver: + lidar_type: RSM1 # LiDAR type - RS16, RS32, RSBP, RSAIRY, RSHELIOS, RSHELIOS_16P, RS128, RS80, RS48, RSP128, RSP80, RSP48, + # RSM1, RSM1_JUMBO, RSM2, RSM3, RSE1, RSMX. + + msop_port: 6699 # Msop port of lidar + difop_port: 7788 # Difop port of lidar + imu_port: 0 # IMU port of lidar(only for RSAIRY, RSE1), 0 means no imu. + # If you want to use IMU, please first set ENABLE_IMU_DATA_PARSE to ON in CMakeLists.txt + user_layer_bytes: 0 # Bytes of user layer. thers is no user layer if it is 0 + tail_layer_bytes: 0 # Bytes of tail layer. thers is no tail layer if it is 0 + + + min_distance: 0.2 # Minimum distance of point cloud + max_distance: 200 # Maximum distance of point cloud + use_lidar_clock: true # true--Use the lidar clock as the message timestamp + # false-- Use the system clock as the timestamp + dense_points: false # true: discard NAN points; false: reserve NAN points + + ts_first_point: true # true: time-stamp point cloud with the first point; false: with the last point; + # these parameters are used from mechanical lidar + + start_angle: 0 # Start angle of point cloud + end_angle: 360 # End angle of point cloud + + # When msg_source is 3, the following parameters will be used + pcap_repeat: true # true: The pcap bag will repeat play + pcap_rate: 1.0 # Rate to read the pcap file + pcap_path: /home/robosense/lidar.pcap #The path of pcap file + + ros: + ros_frame_id: rslidar #Frame id of packet message and point cloud message + ros_recv_packet_topic: /rslidar_packets #Topic used to receive lidar packets from ROS + ros_send_packet_topic: /rslidar_packets #Topic used to send lidar packets through ROS + ros_send_imu_data_topic: /rslidar_imu_data #Topic used to send imu data through ROS + ros_send_point_cloud_topic: /rslidar_points #Topic used to send point cloud through ROS +``` + +### 2.3.2 多台雷达 + +在线连接1台RSM1雷达和1台RSE1雷达，发送点云数据到ROS。 + +*注意lidar部分参数的缩进* + +```yaml +common: + msg_source: 1 + send_packet_ros: false + send_point_cloud_ros: true +lidar: + - driver: + lidar_type: RSM1 # LiDAR type - RS16, RS32, RSBP, RSAIRY, RSHELIOS, RSHELIOS_16P, RS128, RS80, RS48, RSP128, RSP80, RSP48, + # RSM1, RSM1_JUMBO, RSM2, RSM3, RSE1, RSMX. + + msop_port: 6699 # Msop port of lidar + difop_port: 7788 # Difop port of lidar + imu_port: 0 # IMU port of lidar(only for RSAIRY, RSE1), 0 means no imu. + # If you want to use IMU, please first set ENABLE_IMU_DATA_PARSE to ON in CMakeLists.txt + user_layer_bytes: 0 # Bytes of user layer. thers is no user layer if it is 0 + tail_layer_bytes: 0 # Bytes of tail layer. thers is no tail layer if it is 0 + + + min_distance: 0.2 # Minimum distance of point cloud + max_distance: 200 # Maximum distance of point cloud + use_lidar_clock: true # true--Use the lidar clock as the message timestamp + # false-- Use the system clock as the timestamp + dense_points: false # true: discard NAN points; false: reserve NAN points + + ts_first_point: true # true: time-stamp point cloud with the first point; false: with the last point; + # these parameters are used from mechanical lidar + + start_angle: 0 # Start angle of point cloud + end_angle: 360 # End angle of point cloud + + # When msg_source is 3, the following parameters will be used + pcap_repeat: true # true: The pcap bag will repeat play + pcap_rate: 1.0 # Rate to read the pcap file + pcap_path: /home/robosense/lidar.pcap #The path of pcap file + + ros: + ros_frame_id: rslidar #Frame id of packet message and point cloud message + ros_recv_packet_topic: /left/rslidar_packets #Topic used to receive lidar packets from ROS + ros_send_packet_topic: /left/rslidar_packets #Topic used to send lidar packets through ROS + ros_send_imu_data_topic: /left/rslidar_imu_data #Topic used to send imu data through ROS + ros_send_point_cloud_topic: /left/rslidar_points #Topic used to send point cloud through ROS + + - driver: + lidar_type: RSE1 # LiDAR type - RS16, RS32, RSBP, RSAIRY, RSHELIOS, RSHELIOS_16P, RS128, RS80, RS48, RSP128, RSP80, RSP48, + # RSM1, RSM1_JUMBO, RSM2, RSM3, RSE1, RSMX. + msop_port: 6699 # Msop port of lidar + difop_port: 7788 # Difop port of lidar + + user_layer_bytes: 0 # Bytes of user layer. thers is no user layer if it is 0 + tail_layer_bytes: 0 # Bytes of tail layer. thers is no tail layer if it is 0 + + + min_distance: 0.2 # Minimum distance of point cloud + max_distance: 200 # Maximum distance of point cloud + use_lidar_clock: true # true--Use the lidar clock as the message timestamp + # false-- Use the system clock as the timestamp + dense_points: false # true: discard NAN points; false: reserve NAN points + + ts_first_point: true # true: time-stamp point cloud with the first point; false: with the last point; + # these parameters are used from mechanical lidar + + start_angle: 0 # Start angle of point cloud + end_angle: 360 # End angle of point cloud + + # When msg_source is 3, the following parameters will be used + pcap_repeat: true # true: The pcap bag will repeat play + pcap_rate: 1.0 # Rate to read the pcap file + pcap_path: /home/robosense/lidar.pcap #The path of pcap file + + ros: + ros_frame_id: rslidar #Frame id of packet message and point cloud message + ros_recv_packet_topic: /right/rslidar_packets #Topic used to receive lidar packets from ROS + ros_send_packet_topic: /right/rslidar_packets #Topic used to send lidar packets through ROS + ros_send_imu_data_topic: /right/rslidar_imu_data #Topic used to send imu data through ROS + ros_send_point_cloud_topic: /right/rslidar_points #Topic used to send point cloud through ROS +``` + diff --git a/src/airy/rslidar_sdk-main/doc/intro/03_hiding_parameters_intro.md b/src/airy/rslidar_sdk-main/doc/intro/03_hiding_parameters_intro.md new file mode 100644 index 0000000..7675ccf --- /dev/null +++ b/src/airy/rslidar_sdk-main/doc/intro/03_hiding_parameters_intro.md @@ -0,0 +1,89 @@ +# 3 Introduction to hidden parameters + +In order to make the configuration file as simple as possible, we hide some parameters and use default values for them. + +This document explains the meanings of these hidden parameters. + + + +## 3.1 common + +```yaml +common: + msg_source: 1 + send_packet_ros: false + send_point_cloud_ros: false + send_point_cloud_proto: false +``` + + + +## 3.2 lidar + +```yaml +lidar: + - driver: + lidar_type: RSM1 # LiDAR type - RS16, RS32, RSBP, RSAIRY, RSHELIOS, RSHELIOS_16P, RS128, RS80, RS48, RSP128, RSP80, RSP48, + # RSM1, RSM1_JUMBO, RSM2, RSM3, RSE1, RSMX. + + msop_port: 6699 # Msop port of lidar + difop_port: 7788 # Difop port of lidar + imu_port: 0 # IMU port of lidar(only for RSAIRY, RSE1), 0 means no imu. + # If you want to use IMU, please first set ENABLE_IMU_DATA_PARSE to ON in CMakeLists.txt + group_address: 0.0.0.0 + host_address: 0.0.0.0 + user_layer_bytes: 0 # Bytes of user layer. thers is no user layer if it is 0 + tail_layer_bytes: 0 # Bytes of tail layer. thers is no tail layer if it is 0 + + + min_distance: 0.2 # Minimum distance of point cloud + max_distance: 200 # Maximum distance of point cloud + use_lidar_clock: true # true--Use the lidar clock as the message timestamp + # false-- Use the system clock as the timestamp + dense_points: false # true: discard NAN points; false: reserve NAN points + + ts_first_point: true # true: time-stamp point cloud with the first point; false: with the last point; + # these parameters are used from mechanical lidar + + start_angle: 0 # Start angle of point cloud + end_angle: 360 # End angle of point cloud + + # When msg_source is 3, the following parameters will be used + pcap_repeat: true # true: The pcap bag will repeat play + pcap_rate: 1.0 # Rate to read the pcap file + pcap_path: /home/robosense/lidar.pcap #The path of pcap file + use_vlan: false + + x: 0 + y: 0 + z: 0 + roll: 0 + pitch: 0 + yaw: 0 + ros: + ros_frame_id: rslidar #Frame id of packet message and point cloud message + ros_recv_packet_topic: /rslidar_packets #Topic used to receive lidar packets from ROS + ros_send_packet_topic: /rslidar_packets #Topic used to send lidar packets through ROS + ros_send_imu_data_topic: /rslidar_imu_data #Topic used to send imu data through ROS + ros_send_point_cloud_topic: /rslidar_points #Topic used to send point cloud through ROS + ros_send_by_rows: false +``` + +- ```config_from_file``` -- Whether to read Lidar configuration from file. Only used for debug purpose, and can be ignored. +- ```angle_path``` -- The path of the angle.csv. Only used for debug purpose and can be ignored. +- ```ts_first_point``` -- Stamp the point cloud with the first point or the last one. Stamp with the first point if ```true```, else stamp with the last point if ```false```. The default value is ```false```. +- ```split_frame_mode``` -- The way to split the LiDAR frames. Default value is ```1```. + - 1 -- Split frame depending on the split_angle + - 2 -- Split frame depending on a fixed number of blocks + - 3 -- Split frame depending on num_blks_split + +- ```split_angle``` -- The angle(in degree) to split frames. Only be used when ```split_frame_mode = 1```. The default value is ```0```. +- ```num_blks_split``` -- The number of blocks in one frame. Only be used when ```split_frame_mode = 3```. +- ```wait_for_difop``` -- If ```false```, the driver will not wait for difop packet(including lidar configuration data, especially angle data to calculate x, y, z), and send out the point cloud immediately. The default value is ```true```. +- ```group_address``` -- If use multi-cast function, this parameter needs to be set correctly. For more details, please refer to [Online LiDAR - Advanced Topics](../howto/07_online_lidar_advanced_topics.md) +- ```host_address``` -- Needed in two conditions. If the host receives packets from multiple Lidars via different IP addresses, use this parameter to specify destination IPs of the Lidars; If group_address is set, it should be set, so it will be joined into the multicast group. +- ```x, y, z, roll, pitch, yaw ``` -- The parameters to do coordinate transformation. If the coordinate transformation function is enabled in driver core, the output point cloud will be transformed based on these parameters. For more details, please refer to [Coordinate Transformation](../howto/10_how_to_use_coordinate_transformation.md) +- ```use_vlan``` -- Whether to use VLAN. The default value is ```false```. This parameter is only needed for pcap file. If it contains packets with VLAN layer, ```use_vlan``` should set to true. In the case of online Lidar, the VLAN layer is stripped by the protocol layer, so use_vlan can be ignored. +- ```ros_send_by_rows```This only applies to mechanical lidar and is only valid when dense_points=false. +-True - When sending a point cloud, arrange the points in a row by row order +-False - When sending a point cloud, arrange the points in a column by column order \ No newline at end of file diff --git a/src/airy/rslidar_sdk-main/doc/intro/03_hiding_parameters_intro_CN.md b/src/airy/rslidar_sdk-main/doc/intro/03_hiding_parameters_intro_CN.md new file mode 100644 index 0000000..d9dfa58 --- /dev/null +++ b/src/airy/rslidar_sdk-main/doc/intro/03_hiding_parameters_intro_CN.md @@ -0,0 +1,86 @@ +# 3 隐藏参数介绍 + +为了使配置文件config.yaml尽可能简洁，我们隐藏了部分不常用的参数，在代码中使用默认值。 + +本文档将详细介绍这些隐藏参数。用户可根据需要，将它们加入参数文件，重新设置。 + + + +## 3.1 common + +```yaml +common: + msg_source: 1 + send_packet_ros: false + send_point_cloud_ros: false +``` + + + +## 3.2 lidar + +```yaml +lidar: + - driver: + lidar_type: RSM1 # LiDAR type - RS16, RS32, RSBP, RSAIRY, RSHELIOS, RSHELIOS_16P, RS128, RS80, RS48, RSP128, RSP80, RSP48, + # RSM1, RSM1_JUMBO, RSM2, RSM3, RSE1, RSMX. + + msop_port: 6699 # Msop port of lidar + difop_port: 7788 # Difop port of lidar + imu_port: 0 # IMU port of lidar(only for RSAIRY, RSE1), 0 means no imu. + # If you want to use IMU, please first set ENABLE_IMU_DATA_PARSE to ON in CMakeLists.txt + group_address: 0.0.0.0 + host_address: 0.0.0.0 + user_layer_bytes: 0 # Bytes of user layer. thers is no user layer if it is 0 + tail_layer_bytes: 0 # Bytes of tail layer. thers is no tail layer if it is 0 + + + min_distance: 0.2 # Minimum distance of point cloud + max_distance: 200 # Maximum distance of point cloud + use_lidar_clock: true # true--Use the lidar clock as the message timestamp + # false-- Use the system clock as the timestamp + dense_points: false # true: discard NAN points; false: reserve NAN points + + ts_first_point: true # true: time-stamp point cloud with the first point; false: with the last point; + # these parameters are used from mechanical lidar + + start_angle: 0 # Start angle of point cloud + end_angle: 360 # End angle of point cloud + ros_send_by_rows: + # When msg_source is 3, the following parameters will be used + pcap_repeat: true # true: The pcap bag will repeat play + pcap_rate: 1.0 # Rate to read the pcap file + pcap_path: /home/robosense/lidar.pcap #The path of pcap file + use_vlan: false + + x: 0 + y: 0 + z: 0 + roll: 0 + pitch: 0 + yaw: 0 + ros: + ros_frame_id: rslidar #Frame id of packet message and point cloud message + ros_recv_packet_topic: /rslidar_packets #Topic used to receive lidar packets from ROS + ros_send_packet_topic: /rslidar_packets #Topic used to send lidar packets through ROS + ros_send_imu_data_topic: /rslidar_imu_data #Topic used to send imu data through ROS + ros_send_point_cloud_topic: /rslidar_points #Topic used to send point cloud through ROS + ros_send_by_rows: false +``` + +- ```config_from_file``` -- 默认值为false, 是否从外参文件读入雷达配置信息，仅用于调试，可忽略。 +- ```angle_path``` -- angle.csv外参文件的路径，仅用于调试，可忽略。 +- ```split_frame_mode``` -- 分帧模式设置，默认值为```1```。 + - 1 -- 角度分帧 + - 2 -- 固定block数分帧 + - 3 -- 自定义block数分帧 +- ```split_angle``` -- 用于分帧的角度(单位为度)， 在```split_frame_mode = 1``` 时才生效，默认值为```0```。 +- ```num_blks_split``` -- 用于分帧的包数，在 ```split_frame_mode = 3```时才生效，默认值为1。 +- ```wait_for_difop``` -- 若设置为false， 驱动将不会等待DIFOP包（包含配置数据，尤其是角度信息），而是立即解析MSOP包并发出点云。 默认值为```true```，也就是必须要有DIFOP包才会进行点云解析。 +- ```group_address``` -- 如果雷达为组播模式，此参数需要被设置为组播的地址。具体使用方式可以参考[在线雷达 - 高级主题](../howto/07_online_lidar_advanced_topics_CN.md) 。 +- ```host_address``` -- 有两种情况需要这个选项。如果主机上通过多个IP地址接收多个雷达的数据，则可以将此参数指定为雷达的目标IP；如果设置了group_address，那也需要设置host_address，以便将这个IP地址的网卡加入组播组。 +- ```x, y, z, roll, pitch, yaw ``` -- 坐标变换参数，若启用了内核的坐标变换功能，将会使用此参数输出经过变换后的点云。x, y, z, 单位为```米```, roll, pitch, yaw, 单位为```弧度```。具体使用方式可以参考 [坐标变换功能](../howto/10_how_to_use_coordinate_transformation_CN.md) 。 +- ```use_vlan``` -- 默认为false，指定是否使用vlan。如果pcap文件中的packet带vlan层，则需要设置这个选项为true。其他情况下不需要。在线雷达的情况下，协议层到达驱动时，已经剥离vlan层，所以不需要设置这个选项。 +- ```ros_send_by_rows```只对机械式雷达有意义，且只有当dense_points = false时才有效。 + - true -- 发送点云时，按照一行一行的顺序排列点 + - false -- 发送点云时，按照一列一列的顺序排列点 diff --git a/src/airy/rslidar_sdk-main/doc/src_intro/img/class_destination_packet.png b/src/airy/rslidar_sdk-main/doc/src_intro/img/class_destination_packet.png new file mode 100644 index 0000000..b38f89e Binary files /dev/null and b/src/airy/rslidar_sdk-main/doc/src_intro/img/class_destination_packet.png differ diff --git a/src/airy/rslidar_sdk-main/doc/src_intro/img/class_destination_pointcloud.png b/src/airy/rslidar_sdk-main/doc/src_intro/img/class_destination_pointcloud.png new file mode 100644 index 0000000..102bf56 Binary files /dev/null and b/src/airy/rslidar_sdk-main/doc/src_intro/img/class_destination_pointcloud.png differ diff --git a/src/airy/rslidar_sdk-main/doc/src_intro/img/class_node_manager.png b/src/airy/rslidar_sdk-main/doc/src_intro/img/class_node_manager.png new file mode 100644 index 0000000..2729d40 Binary files /dev/null and b/src/airy/rslidar_sdk-main/doc/src_intro/img/class_node_manager.png differ diff --git a/src/airy/rslidar_sdk-main/doc/src_intro/img/class_source_destination.png b/src/airy/rslidar_sdk-main/doc/src_intro/img/class_source_destination.png new file mode 100644 index 0000000..2dd63e5 Binary files /dev/null and b/src/airy/rslidar_sdk-main/doc/src_intro/img/class_source_destination.png differ diff --git a/src/airy/rslidar_sdk-main/doc/src_intro/img/class_source_driver.png b/src/airy/rslidar_sdk-main/doc/src_intro/img/class_source_driver.png new file mode 100644 index 0000000..d2a71ea Binary files /dev/null and b/src/airy/rslidar_sdk-main/doc/src_intro/img/class_source_driver.png differ diff --git a/src/airy/rslidar_sdk-main/doc/src_intro/img/class_source_packet_ros.png b/src/airy/rslidar_sdk-main/doc/src_intro/img/class_source_packet_ros.png new file mode 100644 index 0000000..49bd2a1 Binary files /dev/null and b/src/airy/rslidar_sdk-main/doc/src_intro/img/class_source_packet_ros.png differ diff --git a/src/airy/rslidar_sdk-main/doc/src_intro/rslidar_sdk_intro_CN.md b/src/airy/rslidar_sdk-main/doc/src_intro/rslidar_sdk_intro_CN.md new file mode 100644 index 0000000..d34a6b9 --- /dev/null +++ b/src/airy/rslidar_sdk-main/doc/src_intro/rslidar_sdk_intro_CN.md @@ -0,0 +1,250 @@ +# rslidar_sdk v1.5.17 源代码解析 + + + +## 1 简介 + +rslidar_sdk是基于ROS/ROS2的雷达驱动。rslidar_sdk依赖rs_driver接收和解析MSOP/DIFOP Packet。 + +rslidar_sdk的基本功能如下： ++ 从在线雷达或PCAP文件得到点云和Imu数据，通过ROS话题`/rslidar_points`和`/rslidar_imu_data`发布。使用者可以订阅这个两个话题，在rviz中看到点云和IMU数据。 ++ 从在线雷达得到原始的MSOP/DIFOP/IMU Packet，通过ROS话题`/rslidar_packets`发布。使用者可以订阅这个话题，将Packet记录到rosbag文件。 ++ 从ROS话题`/rslidar_packets`得到MSOP/DIFOP/IMU Packet，解析得到点云，再发布到话题`/rslidar_points`。 + + 这里的话题`/rslidar_packets`，由使用者通过回放Packet rosbag文件发布。 + + + +## 2 Source 与 Destination + +如前面所说，rslidar_sdk从在线雷达、PCAP文件、ROS话题这三种源得到MSOP/DIFOP/IMU Packet，将Packet发布到ROS话题`/rslidar_packets`，将点云发布到目标 - ROS话题`/rslidar_points`, 将Imu数据发布到目标 - ROS话题`/rslidar_imu_data`。 ++ Source定义源接口 ++ DestinationPointCloud定义发送点云和Imu数据的目标接口。 ++ DestinationPacket定义发送MSOP/DIFOP/IMU Packet的目标接口。 + +source + +### 2.1 DestinationPointCloud + +DestinationPointCloud定义发送点云的接口。 ++ 虚拟成员函数init()对DestinationPointCloud实例初始化 ++ 虚拟成员函数start()启动实例 ++ 虚拟成员函数sendPointCloud()发送PointCloud消息 ++ 虚拟成员函数sendImuData()发送Imu数据 + +### 2.2 DestinationPacket + +DestinationPacket定义发送MSOP/DIFOP Packet的接口。 ++ 虚拟成员函数init()对DestinationPacket实例初始化 ++ 虚拟成员函数start()启动实例 ++ 虚拟成员函数sendPacket()启动发送Packet消息 + +### 2.3 Source + +Source是定义源的接口。 + ++ 成员`src_type_`是源的类型 + + ``` + enum SourceType + { + MSG_FROM_LIDAR = 1, + MSG_FROM_ROS_PACKET = 2, + MSG_FROM_PCAP = 3, + }; + ``` + ++ 成员`pc_cb_vec_[]`中是一组DestinationPointCloud的实例。 + + 成员函数sendPointCloud()调用`point_cb_vec_[]`中的实例，发送点云消息。 ++ 成员`pkt_cb_vec_[]`中是一组DestinationPacket实例。 + + 成员函数sendPacket()将Packet消息发送到`pkt_cb_vec_[]`中的实例中。 + ++ 虚拟成员函数init()初始化Source实例 ++ 虚拟成员函数start()启动实例 ++ 虚拟成员函数regPointCloudCallback()将PointCloudDestination实例注册到`point_cb_vec_[]`。 ++ 虚拟成员函数regPacketCallback()将PacketDestination实例注册到`packet_cb_vec_[]`。 + +### 2.4 DestinationPointCloudRos + +DestinationPointCloudRos在ROS话题`/rslidar_points`发布点云。 ++ 成员`pkt_pub_`是点云数据的ROS话题发布器。 ++ 成员`imu_pub_`是IMU数据的ROS话题发布器。 ++ 成员`frame_id_`保存`frame_id`。`frame_id`是坐标系名字。 + +destination pointcloud ros + +#### 2.4.1 DestinationPointCloudRos::init() + +init()初始化DestinationPointCloudRos实例。 ++ 从YAML文件读入用户配置参数。 + + 读入`frame_id`，保存在成员`frame_id_`，默认值是`rslidar`。 + + 读入点云的ROS话题名称，保存在本地变量`ros_send_topic_`，默认值是`/rslidar_points`。 + + 读入IMU的ROS话题名称，保存在本地变量`ros_send_imu_data_topic`，默认值是`/rslidar_imu_data`。 + + 读入点云排列方式参数，保存在成员`send_by_rows_`，默认值是`false`。 ++ 创建ROS话题发布器，保存在成员`pkt_sub_`. + +#### 2.4.2 DestinationPointCloudRos::sendPointCloud() + +sendPointCloud()在ROS话题`/rslidar_points`发布点云。 ++ 调用Publisher::publish()发布ROS格式的点云消息。 + +#### 2.4.3 DestinationPointCloudRos::sendImuData() + +sendImuData()在ROS话题`/rslidar_imu_data`发布Imu数据。 ++ 调用Publisher::publish()发布ROS格式的Imu消息。 + +### 2.5 DestinationPacketRos + +DestinationPacketRos在ROS话题`/rslidar_packets`发布MSOP/DIFOP Packet。 ++ 成员`pkt_sub_`是ROS话题发布器。 ++ 成员`frame_id_`保存`frame_id`。`frame_id`是坐标系名字。 + +![destination packet ros](./img/class_destination_packet.png) + +#### 2.5.1 DestinationPacketRos::init() + +init()初始化DestinationPacketRos实例。 ++ 从YAML文件读入用户配置参数。 + + 读入`frame_id`，保存在成员`frame_id_`，默认值是`rslidar` + + 读入ROS话题，保存在本地变量`ros_send_topic_`，默认值是`/rslidar_packets`。 ++ 创建ROS话题发布器，保存在成员`pkt_sub_`. + +#### 2.5.2 DestinationPacketRos::sendPacket() + +sendPacket()在ROS话题`/rslidar_packets`发布MOSP/DIFOP packet。 ++ 调用Publisher::publish()发布ROS格式的Packet消息。 + +### 2.6 SourceDriver + +SourceDriver从在线雷达和PCAP文件得到MSOP/DIFOP/IMU Packet，并解析得到点云和Imu数据。 ++ 成员`driver_ptr_`是rs_driver驱动的实例，也就是LidarDriver。 ++ 成员`free_point_cloud_queue_`和`point_cloud_queue_`，分别是空闲点云的队列和待处理点云的队列。 ++ 成员`point_cloud_handle_thread_`是点云的处理线程。 ++ 成员`free_imu_data_queue_`和`imu_data_queue_`，分别是空闲Imu数据的队列和待处理Imu数据的队列。 ++ 成员`imu_data_process_thread_`是Imu数据的处理线程。 + +source driver + +#### 2.6.1 SourceDriver::init() + +init()初始化SourceDriver实例。 ++ 读取YAML配置文件，得到雷达的用户配置参数。 ++ 根据源类型，也就是成员`src_type_`，创建相应类型的LidarDriver实例，也就是成员`driver_ptr_`。 + + `src_type_`是在SourceDriver中的构造函数中指定的。 ++ 调用LidarDriver::regPointCloudCallback()，注册回调函数。这里是getPointCloud()和putPointCloud()。前者给`driver_ptr_`提供空闲点云，后者从`driver_ptr_`得到填充好的点云。 + + 注意，这里没有注册MSOP/DIFOP Packet的回调函数，因为Packet是按需获取的。这时为了避免不必要地消耗CPU资源。 ++ 调用LidarDriver::regImuDataCallback()，注册回调函数。这里是getImuData()和putImuData()。前者给`driver_ptr_`提供空闲Imu数据，后者从`driver_ptr_`得到填充好的Imu数据。 ++ 调用LidarDriver::init()，初始化`driver_ptr_`。 ++ 创建、启动点云处理线程`point_cloud_handle_thread_`， 线程函数是processPointCloud()。 ++ 创建、启动IMU处理线程`imu_data_process_thread_`， 线程函数是processImuData()。 + + +#### 2.6.2 SourceDriver::getPointCloud() + +getPointCloud()给成员`driver_ptr_`提供空闲的点云。 ++ 优先从成员`free_point_cloud_queue_`得到点云。 ++ 如果得不到，分配新的点云。 + +#### 2.6.3 SourceDriver::putPointCloud() + +putPointCloud()给从成员`driver_ptr_`得到填充好的点云。 ++ 将得到的点云推送到成员`point_cloud_queue_`，等待处理。 + +#### 2.6.4 SourceDriver::processPointCloud() + +processPointCloud()处理点云。在while循环中， ++ 从待处理点云的队列`point_cloud_queue_`，得到点云， ++ 调用sendPointCloud()，其中调用成员`pc_cb_vec_[]`中的DestinationPointCloud实例，发送点云。 ++ 回收点云，放入空闲点云的队列`free_cloud_queue_`，待下次使用。 + +#### 2.6.5 SourceDriver::getImuData() + +getImuData()给成员`driver_ptr_`提供空闲的Imu数据。 ++ 优先从成员`free_imu_data_queue_`得到Imu数据。 ++ 如果得不到，分配新的Imu数据。 + +#### 2.6.6 SourceDriver::putImuData() + +putImuData()给从成员`driver_ptr_`得到填充好的Imu数据。 ++ 将得到的Imu数据推送到成员`imu_data_queue_`，等待处理。 + +#### 2.6.7 SourceDriver::processImuData() + +processImuData()处理点云。在while循环中， ++ 从待处理点云的队列`imu_data_queue_`，得到点云， ++ 调用sendImuData()，其中调用成员`pc_cb_vec_[]`中的DestinationPointCloud实例，发送点云。 ++ 回收点云，放入空闲点云的队列`free_imu_data_queue_`，待下次使用。 + +#### 2.6.8 SourceDriver::regPacketCallback() + +regPacketCallback()用来注册DestinationPacket。 ++ 调用Source::regPacketCallback()，将DestinationPacket实例，加入成员`pkt_cb_vec_[]`。 ++ 如果这是首次要求Packet(`pkt_cb_vec_[]`的第1个实例)，调用LidarDriver::regPacketCallback()，向`driver_ptr_`注册Packet回调函数，开始接收Packet。回调函数是putPacket()。 + +#### 2.6.9 SourceDriver::putPacket() + +putPacket()调用sendPacket()，其中调用成员`pkt_cb_vec_[]`中的所有实例，发送MSOP/DIFOP Packet。 + +### 2.7 SourcePacketRos + +SourcePacketRos在ROS话题`/rslidar_packets`得到MSOP/DIFOP Packet，解析后得到点云。 ++ SourcePacketRos从SourceDriver派生，而不是直接从Source派生，是因为它用SourceDriver解析Packet得到点云。 ++ 成员`pkt_sub_`，是ROS话题`/rslidar_packets`的订阅器。 + +source packet ros + + +#### 2.7.1 SourcePacketRos::init() + +init()初始化SourcePacketRos实例。 ++ 调用SourceDriver::init()初始化成员`driver_ptr_`。 + + 在SourcePacketRos的构造函数中，SourceType设置为`SourceType::MSG_FROM_ROS_PACKET`。这样，在SourceDriver::init()中，`driver_ptr_`的`input_type`就是`InputType::RAW_PACKET`，它通过LidarDriver::feedPacket接收Packet作为源。 ++ 解析YAML文件得到雷达的用户配置参数 + + 得到接收Packet的话题，默认值为`/rslidar_packets`。 ++ 创建Packet话题的订阅器，也就是成员`pkt_sub_`，接收函数是putPacket()。 + +#### 2.7.2 SourcePacketRos::putPacket() + +putPacket()接收Packet，送到`driver_ptr_`解析。 ++ 调用LidarDriver::decodePacket()，将Packet喂给`driver_ptr_`。 ++ 点云的接收，使用SourceDriver的已有实现。 + + + +## 3 NodeManager + +NodeManager管理Source实例，包括创建、初始化、启动、停止Source。它支持多个源，但是这些源的类型必须相同。 ++ 成员`sources_[]`是一个Source实例的数组。 + +![node_manager](./img/class_node_manager.png) + +### 3.1 NodeManager::init() + +init()初始化NodeManger实例。 ++ 从config.yaml文件得到用户配置参数 + + 本地变量`msg_source`，数据源类型 + + 本地变量`send_point_cloud_ros`， 是否在ROS话题发送点云。 + + 本地变量`send_packet_ros`，是否在ROS话题发送MSOP/DIFOP packet， + ++ 在.yaml文件中遍历数据源。在循环中， + + 根据`msg_source`创建Source实例。 + + 如果是在线雷达（`SourceType::MSG_FROM_LIDAR`)，创建SourceDriver实例并初始化， 源类型为`MSG_FROM_LIDAR`。 + + 如果是PCAP文件(`SourceType::MSG_FROM_PCAP`)，创建SourceDriver实例并初始化，源类型为`MSG_FROM_PCAP`。 + + 如果是ROS话题(`SourceType::MSG_FROM_ROS_PACKET`), 创建SourcePacketRos并初始化。SourcePacketRos构造函数已将源类型设置为`MSG_FROM_ROS_PACKET` + + 如果在ROS话题发送点云(`send_point_cloud_ros` = `true`)，则创建DestinationPointCloudRos实例、初始化，调用Source::regPointCloudCallback(),将它加入Source的`pc_cb_vec_[]`。 + + 如果在ROS话题发送Packet(`send_packet_ros` = `true`)，则创建DestinationPacketRos实例、初始化，调用Source::regPacketCallback()将它加入Source的`pkt_cb_vec_[]`。 + + 将Source实例，加入成员`sources_[]`。 + +### 3.2 NodeManager::start() + +start()启动成员`sources_[]`中的所有实例。 + + + + + + + + + + + diff --git a/src/airy/rslidar_sdk-main/img/01_01_download_page.png b/src/airy/rslidar_sdk-main/img/01_01_download_page.png new file mode 100644 index 0000000..0ad5469 Binary files /dev/null and b/src/airy/rslidar_sdk-main/img/01_01_download_page.png differ diff --git a/src/airy/rslidar_sdk-main/launch/elequent_start.py b/src/airy/rslidar_sdk-main/launch/elequent_start.py new file mode 100755 index 0000000..98cc9e0 --- /dev/null +++ b/src/airy/rslidar_sdk-main/launch/elequent_start.py @@ -0,0 +1,21 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +def generate_launch_description(): + rviz_config=get_package_share_directory('rslidar_sdk')+'/rviz/rviz2.rviz' + return LaunchDescription([ + Node( + package='rslidar_sdk', + node_namespace='rslidar_sdk', + node_name='rslidar_sdk_node', + node_executable='rslidar_sdk_node', + output='screen' + ), + Node( + package='rviz2', + node_namespace='rviz2', + node_name='rviz2', + node_executable='rviz2', + arguments=['-d',rviz_config] + ) + ]) diff --git a/src/airy/rslidar_sdk-main/launch/start.launch b/src/airy/rslidar_sdk-main/launch/start.launch new file mode 100755 index 0000000..849e3b2 --- /dev/null +++ b/src/airy/rslidar_sdk-main/launch/start.launch @@ -0,0 +1,7 @@ + + + + + + + diff --git a/src/airy/rslidar_sdk-main/launch/start.py b/src/airy/rslidar_sdk-main/launch/start.py new file mode 100755 index 0000000..95f915f --- /dev/null +++ b/src/airy/rslidar_sdk-main/launch/start.py @@ -0,0 +1,28 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + + rviz_config = get_package_share_directory("rslidar_sdk") + "/rviz/rviz2.rviz" + + config_file = "" # your config file path + + return LaunchDescription( + [ + Node( + namespace="rslidar_sdk", + package="rslidar_sdk", + executable="rslidar_sdk_node", + output="screen", + parameters=[{"config_path": config_file}], + ), + # Node( + # namespace="rviz2", + # package="rviz2", + # executable="rviz2", + # arguments=["-d", rviz_config], + # ), + ] + ) diff --git a/src/airy/rslidar_sdk-main/launch/start_double.launch.py b/src/airy/rslidar_sdk-main/launch/start_double.launch.py new file mode 100755 index 0000000..4ba045b --- /dev/null +++ b/src/airy/rslidar_sdk-main/launch/start_double.launch.py @@ -0,0 +1,61 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from ament_index_python.packages import get_package_share_directory +from pathlib import Path +import os + + +def generate_launch_description(): + + pkg_share = get_package_share_directory("rslidar_sdk") + # RViz 配置文件路径 + rviz_config = os.path.join(pkg_share, "rviz", "rviz2.rviz") + # 雷达配置文件路径 + config_front = os.path.join(pkg_share, "config", "config_front.yaml") # 前雷达 + config_rear = os.path.join(pkg_share, "config", "config_rear.yaml") # 后雷达 + + # 点云合并 launch 文件路径 + pkg_share_merge = get_package_share_directory("rslidar_pointcloud_merger") + merge_launch = os.path.join(pkg_share_merge, "launch", "merge_two_lidars.launch.py") + + # 前雷达节点 + front_lidar_node = Node( + namespace="front_lidar", + package="rslidar_sdk", + executable="rslidar_sdk_node", + name="rslidar_front", + output="screen", + parameters=[{"config_path": config_front}], + ) + + # 后雷达节点 + rear_lidar_node = Node( + namespace="rear_lidar", + package="rslidar_sdk", + executable="rslidar_sdk_node", + name="rslidar_rear", + output="screen", + parameters=[{"config_path": config_rear}], + ) + + # RViz 可视化节点 + rviz_node = Node( + namespace="", # RViz 一般不加命名空间 + package="rviz2", + executable="rviz2", + output="log", + arguments=["-d", rviz_config], + ) + + # 点云合并 launch 文件 + merge_launch_action = IncludeLaunchDescription( + PythonLaunchDescriptionSource(merge_launch) + ) + + # 返回所有节点和 launch 文件 + return LaunchDescription( + # [front_lidar_node, rear_lidar_node, merge_launch_action, rviz_node] + [front_lidar_node, rear_lidar_node, merge_launch_action] + ) diff --git a/src/airy/rslidar_sdk-main/node/rslidar_sdk_node.cpp b/src/airy/rslidar_sdk-main/node/rslidar_sdk_node.cpp new file mode 100644 index 0000000..49d543b --- /dev/null +++ b/src/airy/rslidar_sdk-main/node/rslidar_sdk_node.cpp @@ -0,0 +1,136 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#include "manager/node_manager.hpp" + +#include +#include + +#ifdef ROS_FOUND +#include +#include +#elif ROS2_FOUND +#include +#endif + +using namespace robosense::lidar; + +#ifdef ROS2_FOUND +std::mutex g_mtx; +std::condition_variable g_cv; +#endif + +static void sigHandler(int sig) +{ + RS_MSG << "RoboSense-LiDAR-Driver is stopping....." << RS_REND; + +#ifdef ROS_FOUND + ros::shutdown(); +#elif ROS2_FOUND + g_cv.notify_all(); +#endif +} + +int main(int argc, char** argv) +{ + signal(SIGINT, sigHandler); ///< bind ctrl+c signal with the sigHandler function + + RS_TITLE << "********************************************************" << RS_REND; + RS_TITLE << "********** **********" << RS_REND; + RS_TITLE << "********** RSLidar_SDK Version: v" << RSLIDAR_VERSION_MAJOR + << "." << RSLIDAR_VERSION_MINOR + << "." << RSLIDAR_VERSION_PATCH << " **********" << RS_REND; + RS_TITLE << "********** **********" << RS_REND; + RS_TITLE << "********************************************************" << RS_REND; + +#ifdef ROS_FOUND + ros::init(argc, argv, "rslidar_sdk_node", ros::init_options::NoSigintHandler); +#elif ROS2_FOUND + rclcpp::init(argc, argv); +#endif + + std::string config_path; + +#ifdef RUN_IN_ROS_WORKSPACE + config_path = ros::package::getPath("rslidar_sdk"); +#else + config_path = (std::string)PROJECT_PATH; +#endif + + config_path += "/config/config.yaml"; + +#ifdef ROS_FOUND + ros::NodeHandle priv_hh("~"); + std::string path; + priv_hh.param("config_path", path, std::string("")); +#elif ROS2_FOUND + std::shared_ptr nd = rclcpp::Node::make_shared("param_handle"); + std::string path = nd->declare_parameter("config_path", ""); +#endif + +#if defined(ROS_FOUND) || defined(ROS2_FOUND) + if (!path.empty()) + { + config_path = path; + } +#endif + + YAML::Node config; + try + { + config = YAML::LoadFile(config_path); + RS_INFO << "--------------------------------------------------------" << RS_REND; + RS_INFO << "Config loaded from PATH:" << RS_REND; + RS_INFO << config_path << RS_REND; + RS_INFO << "--------------------------------------------------------" << RS_REND; + } + catch (...) + { + RS_ERROR << "The format of config file " << config_path << " is wrong. Please check (e.g. indentation)." << RS_REND; + return -1; + } + + std::shared_ptr demo_ptr = std::make_shared(); + demo_ptr->init(config); + demo_ptr->start(); + + RS_MSG << "RoboSense-LiDAR-Driver is running....." << RS_REND; + +#ifdef ROS_FOUND + ros::spin(); +#elif ROS2_FOUND + std::unique_lock lck(g_mtx); + g_cv.wait(lck); +#endif + + return 0; +} diff --git a/src/airy/rslidar_sdk-main/package.xml b/src/airy/rslidar_sdk-main/package.xml new file mode 100644 index 0000000..02ad159 --- /dev/null +++ b/src/airy/rslidar_sdk-main/package.xml @@ -0,0 +1,25 @@ + + + rslidar_sdk + 1.5.16 + The rslidar_sdk package + robosense + BSD + + catkin + ament_cmake + + libpcap + pcl_ros + rclcpp + roscpp + roslib + rslidar_msg + sensor_msgs + std_msgs + yaml-cpp + + + ament_cmake + + diff --git a/src/airy/rslidar_sdk-main/rviz/rviz.rviz b/src/airy/rslidar_sdk-main/rviz/rviz.rviz new file mode 100644 index 0000000..e3b70a8 --- /dev/null +++ b/src/airy/rslidar_sdk-main/rviz/rviz.rviz @@ -0,0 +1,164 @@ +Panels: + - Class: rviz/Displays + Help Height: 75 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Point Cloud1 + Splitter Ratio: 0.5 + Tree Height: 790 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: Point Cloud + - Class: rviz/Displays + Help Height: 75 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 366 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: Point Cloud + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 1.5 + Size (m): 0.10000000149011612 + Style: Squares + Topic: /rslidar_points + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 0.5 + Cell Size: 10 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 50 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz/Axes + Enabled: true + Length: 1 + Name: Axes + Radius: 0.10000000149011612 + Reference Frame: + Show Trail: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: /rslidar + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - 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 + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 107.46170806884766 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 1.4133121967315674 + Y: -0.1708243489265442 + Z: 1.6932628154754639 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.634796679019928 + Target Frame: + Yaw: 3.9649996757507324 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1016 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 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 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2504 + X: 56 + Y: 27 diff --git a/src/airy/rslidar_sdk-main/rviz/rviz2.rviz b/src/airy/rslidar_sdk-main/rviz/rviz2.rviz new file mode 100644 index 0000000..719b7a8 --- /dev/null +++ b/src/airy/rslidar_sdk-main/rviz/rviz2.rviz @@ -0,0 +1,184 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /PointCloud21 + - /Axes1 + - /Axes2 + - /Axes3 + Splitter Ratio: 0.5564202070236206 + Tree Height: 787 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 30 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 30.831819534301758 + Min Value: -6.263338565826416 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 255 + Min Color: 0; 0; 0 + Min Intensity: 1 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.05000000074505806 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /rslidar_points + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz_default_plugins/Axes + Enabled: true + Length: 1 + Name: Axes + Radius: 0.10000000149011612 + Reference Frame: front_lidar + Value: true + - Class: rviz_default_plugins/Axes + Enabled: true + Length: 1 + Name: Axes + Radius: 0.10000000149011612 + Reference Frame: rear_lidar + Value: true + - Class: rviz_default_plugins/Axes + Enabled: true + Length: 1 + Name: Axes + Radius: 0.10000000149011612 + Reference Frame: rslidar + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: rslidar + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 60.19597625732422 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 1.080678939819336 + Y: -0.35503947734832764 + Z: 0.8869286775588989 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: true + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.5697963237762451 + Target Frame: + Value: Orbit (rviz) + Yaw: 3.1403956413269043 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1016 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000002040000039efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000041b0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1850 + X: 70 + Y: 27 diff --git a/src/airy/rslidar_sdk-main/src/manager/node_manager.cpp b/src/airy/rslidar_sdk-main/src/manager/node_manager.cpp new file mode 100644 index 0000000..3646fb3 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/manager/node_manager.cpp @@ -0,0 +1,167 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#include "manager/node_manager.hpp" +#include "source/source_driver.hpp" +#include "source/source_pointcloud_ros.hpp" +#include "source/source_packet_ros.hpp" + +namespace robosense +{ +namespace lidar +{ + +void NodeManager::init(const YAML::Node& config) +{ + YAML::Node common_config = yamlSubNodeAbort(config, "common"); + + int msg_source = 0; + yamlRead(common_config, "msg_source", msg_source, 0); + + bool send_packet_ros; + yamlRead(common_config, "send_packet_ros", send_packet_ros, false); + + bool send_point_cloud_ros; + yamlRead(common_config, "send_point_cloud_ros", send_point_cloud_ros, false); + + bool send_point_cloud_proto; + yamlRead(common_config, "send_point_cloud_proto", send_point_cloud_proto, false); + + bool send_packet_proto; + yamlRead(common_config, "send_packet_proto", send_packet_proto, false); + + YAML::Node lidar_config = yamlSubNodeAbort(config, "lidar"); + + for (uint8_t i = 0; i < lidar_config.size(); ++i) + { + std::shared_ptr source; + + switch (msg_source) + { + case SourceType::MSG_FROM_LIDAR: // online lidar + + RS_INFO << "------------------------------------------------------" << RS_REND; + RS_INFO << "Receive Packets From : Online LiDAR" << RS_REND; + RS_INFO << "Msop Port: " << lidar_config[i]["driver"]["msop_port"].as() << RS_REND; + RS_INFO << "Difop Port: " << lidar_config[i]["driver"]["difop_port"].as() << RS_REND; + RS_INFO << "------------------------------------------------------" << RS_REND; + + source = std::make_shared(SourceType::MSG_FROM_LIDAR); + source->init(lidar_config[i]); + break; + + case SourceType::MSG_FROM_ROS_PACKET: // pkt from ros + + RS_INFO << "------------------------------------------------------" << RS_REND; + RS_INFO << "Receive Packets From : ROS" << RS_REND; + RS_INFO << "Msop Topic: " << lidar_config[i]["ros"]["ros_recv_packet_topic"].as() << RS_REND; + RS_INFO << "------------------------------------------------------" << RS_REND; + + source = std::make_shared(); + source->init(lidar_config[i]); + break; + + case SourceType::MSG_FROM_PCAP: // pcap + + RS_INFO << "------------------------------------------------------" << RS_REND; + RS_INFO << "Receive Packets From : Pcap" << RS_REND; + RS_INFO << "Msop Port: " << lidar_config[i]["driver"]["msop_port"].as() << RS_REND; + RS_INFO << "Difop Port: " << lidar_config[i]["driver"]["difop_port"].as() << RS_REND; + RS_INFO << "------------------------------------------------------" << RS_REND; + + source = std::make_shared(SourceType::MSG_FROM_PCAP); + source->init(lidar_config[i]); + break; + + default: + RS_ERROR << "Unsupported LiDAR message source:" << msg_source << "." << RS_REND; + exit(-1); + } + + if (send_packet_ros) + { + RS_DEBUG << "------------------------------------------------------" << RS_REND; + RS_DEBUG << "Send Packets To : ROS" << RS_REND; + RS_DEBUG << "Msop Topic: " << lidar_config[i]["ros"]["ros_send_packet_topic"].as() << RS_REND; + RS_DEBUG << "------------------------------------------------------" << RS_REND; + + std::shared_ptr dst = std::make_shared(); + dst->init(lidar_config[i]); + source->regPacketCallback(dst); + } + + if (send_point_cloud_ros) + { + RS_DEBUG << "------------------------------------------------------" << RS_REND; + RS_DEBUG << "Send PointCloud To : ROS" << RS_REND; + RS_DEBUG << "PointCloud Topic: " << lidar_config[i]["ros"]["ros_send_point_cloud_topic"].as() + << RS_REND; + RS_DEBUG << "------------------------------------------------------" << RS_REND; + + std::shared_ptr dst = std::make_shared(); + dst->init(lidar_config[i]); + source->regPointCloudCallback(dst); + } + + sources_.emplace_back(source); + } +} + +void NodeManager::start() +{ + for (auto& iter : sources_) + { + if (iter != nullptr) + { + iter->start(); + } + } +} + +void NodeManager::stop() +{ + for (auto& iter : sources_) + { + if (iter != nullptr) + { + iter->stop(); + } + } +} + +NodeManager::~NodeManager() +{ + stop(); +} + +} // namespace lidar +} // namespace robosense diff --git a/src/airy/rslidar_sdk-main/src/manager/node_manager.hpp b/src/airy/rslidar_sdk-main/src/manager/node_manager.hpp new file mode 100644 index 0000000..39e0bc8 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/manager/node_manager.hpp @@ -0,0 +1,61 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once + +#include "utility/yaml_reader.hpp" +#include "source/source.hpp" + +namespace robosense +{ +namespace lidar +{ + +class NodeManager +{ +public: + + void init(const YAML::Node& config); + void start(); + void stop(); + + ~NodeManager(); + NodeManager() = default; + +private: + + std::vector sources_; +}; + +} // namespace lidar +} // namespace robosense + diff --git a/src/airy/rslidar_sdk-main/src/msg/ros_msg/RsCompressedImage.msg b/src/airy/rslidar_sdk-main/src/msg/ros_msg/RsCompressedImage.msg new file mode 100644 index 0000000..a912af6 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/msg/ros_msg/RsCompressedImage.msg @@ -0,0 +1,3 @@ +std_msgs/Header header +uint8 type +uint8[] data diff --git a/src/airy/rslidar_sdk-main/src/msg/ros_msg/RslidarPacket.msg b/src/airy/rslidar_sdk-main/src/msg/ros_msg/RslidarPacket.msg new file mode 100644 index 0000000..b6a05f5 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/msg/ros_msg/RslidarPacket.msg @@ -0,0 +1,4 @@ +std_msgs/Header header +uint8 is_difop +uint8 is_frame_begin +uint8[] data diff --git a/src/airy/rslidar_sdk-main/src/msg/ros_msg/rs_compressed_image.hpp b/src/airy/rslidar_sdk-main/src/msg/ros_msg/rs_compressed_image.hpp new file mode 100644 index 0000000..f0a391d --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/msg/ros_msg/rs_compressed_image.hpp @@ -0,0 +1,237 @@ +// Generated by gencpp from file rscamera_msg/RsCompressedImage.msg +// DO NOT EDIT! + + +#ifndef RSCAMERA_MESSAGE_RSCOMPRESSEDIMAGE_H +#define RSCAMERA_MESSAGE_RSCOMPRESSEDIMAGE_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace rscamera_msg +{ +template +struct RsCompressedImage_ +{ + typedef RsCompressedImage_ Type; + + RsCompressedImage_() + : header() + , type(0) + , data() { + } + RsCompressedImage_(const ContainerAllocator& _alloc) + : header(_alloc) + , type(0) + , data(_alloc) { + (void)_alloc; + } + + + + typedef ::std_msgs::Header_ _header_type; + _header_type header; + + typedef uint8_t _type_type; + _type_type type; + + typedef std::vector::template rebind_alloc> _data_type; + _data_type data; + + + + + + typedef boost::shared_ptr< ::rscamera_msg::RsCompressedImage_ > Ptr; + typedef boost::shared_ptr< ::rscamera_msg::RsCompressedImage_ const> ConstPtr; + +}; // struct RsCompressedImage_ + +typedef ::rscamera_msg::RsCompressedImage_ > RsCompressedImage; + +typedef boost::shared_ptr< ::rscamera_msg::RsCompressedImage > RsCompressedImagePtr; +typedef boost::shared_ptr< ::rscamera_msg::RsCompressedImage const> RsCompressedImageConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::rscamera_msg::RsCompressedImage_ & v) +{ +ros::message_operations::Printer< ::rscamera_msg::RsCompressedImage_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::rscamera_msg::RsCompressedImage_ & lhs, const ::rscamera_msg::RsCompressedImage_ & rhs) +{ + return lhs.header == rhs.header && + lhs.type == rhs.type && + lhs.data == rhs.data; +} + +template +bool operator!=(const ::rscamera_msg::RsCompressedImage_ & lhs, const ::rscamera_msg::RsCompressedImage_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace rscamera_msg + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsMessage< ::rscamera_msg::RsCompressedImage_ > + : TrueType + { }; + +template +struct IsMessage< ::rscamera_msg::RsCompressedImage_ const> + : TrueType + { }; + +template +struct IsFixedSize< ::rscamera_msg::RsCompressedImage_ > + : FalseType + { }; + +template +struct IsFixedSize< ::rscamera_msg::RsCompressedImage_ const> + : FalseType + { }; + +template +struct HasHeader< ::rscamera_msg::RsCompressedImage_ > + : TrueType + { }; + +template +struct HasHeader< ::rscamera_msg::RsCompressedImage_ const> + : TrueType + { }; + + +template +struct MD5Sum< ::rscamera_msg::RsCompressedImage_ > +{ + static const char* value() + { + return "4044c7c7aa754eb9dc4031aaf0ca91a7"; + } + + static const char* value(const ::rscamera_msg::RsCompressedImage_&) { return value(); } + static const uint64_t static_value1 = 0x4044c7c7aa754eb9ULL; + static const uint64_t static_value2 = 0xdc4031aaf0ca91a7ULL; +}; + +template +struct DataType< ::rscamera_msg::RsCompressedImage_ > +{ + static const char* value() + { + return "rscamera_msg/RsCompressedImage"; + } + + static const char* value(const ::rscamera_msg::RsCompressedImage_&) { return value(); } +}; + +template +struct Definition< ::rscamera_msg::RsCompressedImage_ > +{ + static const char* value() + { + return "std_msgs/Header header\n" +"uint8 type\n" +"uint8[] data\n" +"\n" +"================================================================================\n" +"MSG: std_msgs/Header\n" +"# Standard metadata for higher-level stamped data types.\n" +"# This is generally used to communicate timestamped data \n" +"# in a particular coordinate frame.\n" +"# \n" +"# sequence ID: consecutively increasing ID \n" +"uint32 seq\n" +"#Two-integer timestamp that is expressed as:\n" +"# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n" +"# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n" +"# time-handling sugar is provided by the client library\n" +"time stamp\n" +"#Frame this data is associated with\n" +"string frame_id\n" +; + } + + static const char* value(const ::rscamera_msg::RsCompressedImage_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::rscamera_msg::RsCompressedImage_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.header); + stream.next(m.type); + stream.next(m.data); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct RsCompressedImage_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::rscamera_msg::RsCompressedImage_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::rscamera_msg::RsCompressedImage_& v) + { + s << indent << "header: "; + s << std::endl; + Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); + s << indent << "type: "; + Printer::stream(s, indent + " ", v.type); + s << indent << "data[]" << std::endl; + for (size_t i = 0; i < v.data.size(); ++i) + { + s << indent << " data[" << i << "]: "; + Printer::stream(s, indent + " ", v.data[i]); + } + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // RSCAMERA_MESSAGE_RSCOMPRESSEDIMAGE_H diff --git a/src/airy/rslidar_sdk-main/src/msg/ros_msg/rslidar_packet.hpp b/src/airy/rslidar_sdk-main/src/msg/ros_msg/rslidar_packet.hpp new file mode 100644 index 0000000..4a1a290 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/msg/ros_msg/rslidar_packet.hpp @@ -0,0 +1,247 @@ +// Generated by gencpp from file rslidar_msg/RslidarPacket.msg +// DO NOT EDIT! + + +#ifndef RSLIDAR_MSG_MESSAGE_RSLIDARPACKET_H +#define RSLIDAR_MSG_MESSAGE_RSLIDARPACKET_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace rslidar_msg +{ +template +struct RslidarPacket_ +{ + typedef RslidarPacket_ Type; + + RslidarPacket_() + : header() + , is_difop(0) + , is_frame_begin(0) + , data() { + } + RslidarPacket_(const ContainerAllocator& _alloc) + : header(_alloc) + , is_difop(0) + , is_frame_begin(0) + , data(_alloc) { + (void)_alloc; + } + + + + typedef ::std_msgs::Header_ _header_type; + _header_type header; + + typedef uint8_t _is_difop_type; + _is_difop_type is_difop; + + typedef uint8_t _is_frame_begin_type; + _is_frame_begin_type is_frame_begin; + + typedef std::vector::other > _data_type; + _data_type data; + + + + + + typedef boost::shared_ptr< ::rslidar_msg::RslidarPacket_ > Ptr; + typedef boost::shared_ptr< ::rslidar_msg::RslidarPacket_ const> ConstPtr; + +}; // struct RslidarPacket_ + +typedef ::rslidar_msg::RslidarPacket_ > RslidarPacket; + +typedef boost::shared_ptr< ::rslidar_msg::RslidarPacket > RslidarPacketPtr; +typedef boost::shared_ptr< ::rslidar_msg::RslidarPacket const> RslidarPacketConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::rslidar_msg::RslidarPacket_ & v) +{ +ros::message_operations::Printer< ::rslidar_msg::RslidarPacket_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::rslidar_msg::RslidarPacket_ & lhs, const ::rslidar_msg::RslidarPacket_ & rhs) +{ + return lhs.header == rhs.header && + lhs.is_difop == rhs.is_difop && + lhs.is_frame_begin == rhs.is_frame_begin && + lhs.data == rhs.data; +} + +template +bool operator!=(const ::rslidar_msg::RslidarPacket_ & lhs, const ::rslidar_msg::RslidarPacket_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace rslidar_msg + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsMessage< ::rslidar_msg::RslidarPacket_ > + : TrueType + { }; + +template +struct IsMessage< ::rslidar_msg::RslidarPacket_ const> + : TrueType + { }; + +template +struct IsFixedSize< ::rslidar_msg::RslidarPacket_ > + : FalseType + { }; + +template +struct IsFixedSize< ::rslidar_msg::RslidarPacket_ const> + : FalseType + { }; + +template +struct HasHeader< ::rslidar_msg::RslidarPacket_ > + : TrueType + { }; + +template +struct HasHeader< ::rslidar_msg::RslidarPacket_ const> + : TrueType + { }; + + +template +struct MD5Sum< ::rslidar_msg::RslidarPacket_ > +{ + static const char* value() + { + return "4b1cc155a9097c0cb935a7abf46d6eef"; + } + + static const char* value(const ::rslidar_msg::RslidarPacket_&) { return value(); } + static const uint64_t static_value1 = 0x4b1cc155a9097c0cULL; + static const uint64_t static_value2 = 0xb935a7abf46d6eefULL; +}; + +template +struct DataType< ::rslidar_msg::RslidarPacket_ > +{ + static const char* value() + { + return "rslidar_msg/RslidarPacket"; + } + + static const char* value(const ::rslidar_msg::RslidarPacket_&) { return value(); } +}; + +template +struct Definition< ::rslidar_msg::RslidarPacket_ > +{ + static const char* value() + { + return "std_msgs/Header header\n" +"uint8 is_difop\n" +"uint8 is_frame_begin\n" +"uint8[] data\n" +"\n" +"================================================================================\n" +"MSG: std_msgs/Header\n" +"# Standard metadata for higher-level stamped data types.\n" +"# This is generally used to communicate timestamped data \n" +"# in a particular coordinate frame.\n" +"# \n" +"# sequence ID: consecutively increasing ID \n" +"uint32 seq\n" +"#Two-integer timestamp that is expressed as:\n" +"# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n" +"# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n" +"# time-handling sugar is provided by the client library\n" +"time stamp\n" +"#Frame this data is associated with\n" +"string frame_id\n" +; + } + + static const char* value(const ::rslidar_msg::RslidarPacket_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::rslidar_msg::RslidarPacket_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.header); + stream.next(m.is_difop); + stream.next(m.is_frame_begin); + stream.next(m.data); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct RslidarPacket_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::rslidar_msg::RslidarPacket_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::rslidar_msg::RslidarPacket_& v) + { + s << indent << "header: "; + s << std::endl; + Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); + s << indent << "is_difop: "; + Printer::stream(s, indent + " ", v.is_difop); + s << indent << "is_frame_begin: "; + Printer::stream(s, indent + " ", v.is_frame_begin); + s << indent << "data[]" << std::endl; + for (size_t i = 0; i < v.data.size(); ++i) + { + s << indent << " data[" << i << "]: "; + Printer::stream(s, indent + " ", v.data[i]); + } + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // RSLIDAR_MSG_MESSAGE_RSLIDARPACKET_H diff --git a/src/airy/rslidar_sdk-main/src/msg/ros_msg/rslidar_packet_legacy.hpp b/src/airy/rslidar_sdk-main/src/msg/ros_msg/rslidar_packet_legacy.hpp new file mode 100644 index 0000000..fbd820b --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/msg/ros_msg/rslidar_packet_legacy.hpp @@ -0,0 +1,196 @@ +// Generated by gencpp from file rslidar_msgs/rslidarPacket.msg +// DO NOT EDIT! + +#pragma once + +#include +#include +#include + +#include +#include +#include +#include + +namespace rslidar_msgs +{ +template +struct rslidarPacket_ +{ + typedef rslidarPacket_ Type; + + rslidarPacket_() : stamp(), data() + { + data.assign(0); + } + rslidarPacket_(const ContainerAllocator& _alloc) : stamp(), data() + { + (void)_alloc; + data.assign(0); + } + + typedef ros::Time _stamp_type; + _stamp_type stamp; + + typedef boost::array _data_type; + _data_type data; + + typedef boost::shared_ptr< ::rslidar_msgs::rslidarPacket_ > Ptr; + typedef boost::shared_ptr< ::rslidar_msgs::rslidarPacket_ const> ConstPtr; + +}; // struct rslidarPacket_ + +typedef ::rslidar_msgs::rslidarPacket_ > rslidarPacket; + +typedef boost::shared_ptr< ::rslidar_msgs::rslidarPacket> rslidarPacketPtr; +typedef boost::shared_ptr< ::rslidar_msgs::rslidarPacket const> rslidarPacketConstPtr; + +// constants requiring out of line definition + +template +std::ostream& operator<<(std::ostream& s, const ::rslidar_msgs::rslidarPacket_& v) +{ + ros::message_operations::Printer< ::rslidar_msgs::rslidarPacket_ >::stream(s, "", v); + return s; +} + +} // namespace rslidar_msgs + +namespace ros +{ +namespace message_traits +{ +// BOOLTRAITS {'IsFixedSize': true, 'IsMessage': true, 'HasHeader': false} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'rslidar_msgs': +// ['/home/xzd/catkin_ws/src/rs_driver/rslidar_msgs/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', +// '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', +// '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', +// 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + +template +struct IsFixedSize< ::rslidar_msgs::rslidarPacket_ > : TrueType +{ +}; + +template +struct IsFixedSize< ::rslidar_msgs::rslidarPacket_ const> : TrueType +{ +}; + +template +struct IsMessage< ::rslidar_msgs::rslidarPacket_ > : TrueType +{ +}; + +template +struct IsMessage< ::rslidar_msgs::rslidarPacket_ const> : TrueType +{ +}; + +template +struct HasHeader< ::rslidar_msgs::rslidarPacket_ > : FalseType +{ +}; + +template +struct HasHeader< ::rslidar_msgs::rslidarPacket_ const> : FalseType +{ +}; + +template +struct MD5Sum< ::rslidar_msgs::rslidarPacket_ > +{ + static const char* value() + { + return "1e4288e00b9222ea477b73350bf24f51"; + } + + static const char* value(const ::rslidar_msgs::rslidarPacket_&) + { + return value(); + } + static const uint64_t static_value1 = 0x1e4288e00b9222eaULL; + static const uint64_t static_value2 = 0x477b73350bf24f51ULL; +}; + +template +struct DataType< ::rslidar_msgs::rslidarPacket_ > +{ + static const char* value() + { + return "rslidar_msgs/rslidarPacket"; + } + + static const char* value(const ::rslidar_msgs::rslidarPacket_&) + { + return value(); + } +}; + +template +struct Definition< ::rslidar_msgs::rslidarPacket_ > +{ + static const char* value() + { + return "# Raw LIDAR packet.\n\ +\n\ +timestamp # packet timestamp\n\ +uint8[1248] data # packet contents\n\ +\n\ +"; + } + + static const char* value(const ::rslidar_msgs::rslidarPacket_&) + { + return value(); + } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ +template +struct Serializer< ::rslidar_msgs::rslidarPacket_ > +{ + template + inline static void allInOne(Stream& stream, T m) + { + stream.next(m.stamp); + stream.next(m.data); + } + + ROS_DECLARE_ALLINONE_SERIALIZER +}; // struct rslidarPacket_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ +template +struct Printer< ::rslidar_msgs::rslidarPacket_ > +{ + template + static void stream(Stream& s, const std::string& indent, const ::rslidar_msgs::rslidarPacket_& v) + { + s << indent << "stamp: "; + Printer::stream(s, indent + " ", v.stamp); + s << indent << "data[]" << std::endl; + for (size_t i = 0; i < v.data.size(); ++i) + { + s << indent << " data[" << i << "]: "; + Printer::stream(s, indent + " ", v.data[i]); + } + } +}; + +} // namespace message_operations +} // namespace ros diff --git a/src/airy/rslidar_sdk-main/src/msg/ros_msg/rslidar_scan_legacy.hpp b/src/airy/rslidar_sdk-main/src/msg/ros_msg/rslidar_scan_legacy.hpp new file mode 100644 index 0000000..b9837e1 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/msg/ros_msg/rslidar_scan_legacy.hpp @@ -0,0 +1,228 @@ +// Generated by gencpp from file rslidar_msgs/rslidarScan.msg +// DO NOT EDIT! + +#pragma once + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include "msg/ros_msg/rslidar_packet_legacy.hpp" + +namespace rslidar_msgs +{ +template +struct rslidarScan_ +{ + typedef rslidarScan_ Type; + + rslidarScan_() : header(), packets() + { + } + rslidarScan_(const ContainerAllocator& _alloc) : header(_alloc), packets(_alloc) + { + (void)_alloc; + } + + typedef ::std_msgs::Header_ _header_type; + _header_type header; + + typedef std::vector< + ::rslidar_msgs::rslidarPacket_, + typename ContainerAllocator::template rebind<::rslidar_msgs::rslidarPacket_>::other> + _packets_type; + _packets_type packets; + + typedef boost::shared_ptr<::rslidar_msgs::rslidarScan_> Ptr; + typedef boost::shared_ptr<::rslidar_msgs::rslidarScan_ const> ConstPtr; + +}; // struct rslidarScan_ + +typedef ::rslidar_msgs::rslidarScan_> rslidarScan; + +typedef boost::shared_ptr<::rslidar_msgs::rslidarScan> rslidarScanPtr; +typedef boost::shared_ptr<::rslidar_msgs::rslidarScan const> rslidarScanConstPtr; + +// constants requiring out of line definition + +template +std::ostream& operator<<(std::ostream& s, const ::rslidar_msgs::rslidarScan_& v) +{ + ros::message_operations::Printer<::rslidar_msgs::rslidarScan_>::stream(s, "", v); + return s; +} + +} // namespace rslidar_msgs + +namespace ros +{ +namespace message_traits +{ +// BOOLTRAITS {'IsFixedSize': false, 'IsMessage': true, 'HasHeader': true} +// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'rslidar_msgs': +// ['/home/xzd/catkin_ws/src/rs_driver/rslidar_msgs/msg']} + +// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', +// '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', +// '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', +// 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] + +template +struct IsFixedSize<::rslidar_msgs::rslidarScan_> : FalseType +{ +}; + +template +struct IsFixedSize<::rslidar_msgs::rslidarScan_ const> : FalseType +{ +}; + +template +struct IsMessage<::rslidar_msgs::rslidarScan_> : TrueType +{ +}; + +template +struct IsMessage<::rslidar_msgs::rslidarScan_ const> : TrueType +{ +}; + +template +struct HasHeader<::rslidar_msgs::rslidarScan_> : TrueType +{ +}; + +template +struct HasHeader<::rslidar_msgs::rslidarScan_ const> : TrueType +{ +}; + +template +struct MD5Sum<::rslidar_msgs::rslidarScan_> +{ + static const char* value() + { + return "ff6baa58985b528481871cbaf1bb342f"; + } + + static const char* value(const ::rslidar_msgs::rslidarScan_&) + { + return value(); + } + static const uint64_t static_value1 = 0xff6baa58985b5284ULL; + static const uint64_t static_value2 = 0x81871cbaf1bb342fULL; +}; + +template +struct DataType<::rslidar_msgs::rslidarScan_> +{ + static const char* value() + { + return "rslidar_msgs/rslidarScan"; + } + + static const char* value(const ::rslidar_msgs::rslidarScan_&) + { + return value(); + } +}; + +template +struct Definition<::rslidar_msgs::rslidarScan_> +{ + static const char* value() + { + return "# LIDAR scan packets.\n\ +\n\ +Header header # standard ROS message header\n\ +rslidarPacket[] packets # vector of raw packets\n\ +\n\ +================================================================================\n\ +MSG: std_msgs/Header\n\ +# Standard metadata for higher-level stamped data types.\n\ +# This is generally used to communicate timestamped data \n\ +# in a particular coordinate frame.\n\ +# \n\ +# sequence ID: consecutively increasing ID \n\ +uint32 seq\n\ +#Two-integer timestamp that is expressed as:\n\ +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\ +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\ +# time-handling sugar is provided by the client library\n\ +timestamp\n\ +#Frame this data is associated with\n\ +# 0: no frame\n\ +# 1: global frame\n\ +string frame_id\n\ +\n\ +================================================================================\n\ +MSG: rslidar_msgs/rslidarPacket\n\ +# Raw LIDAR packet.\n\ +\n\ +timestamp # packet timestamp\n\ +uint8[1248] data # packet contents\n\ +\n\ +"; + } + + static const char* value(const ::rslidar_msgs::rslidarScan_&) + { + return value(); + } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ +template +struct Serializer<::rslidar_msgs::rslidarScan_> +{ + template + inline static void allInOne(Stream& stream, T m) + { + stream.next(m.header); + stream.next(m.packets); + } + + ROS_DECLARE_ALLINONE_SERIALIZER +}; // struct rslidarScan_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ +template +struct Printer<::rslidar_msgs::rslidarScan_> +{ + template + static void stream(Stream& s, const std::string& indent, const ::rslidar_msgs::rslidarScan_& v) + { + s << indent << "header: "; + s << std::endl; + Printer<::std_msgs::Header_>::stream(s, indent + " ", v.header); + s << indent << "packets[]" << std::endl; + for (size_t i = 0; i < v.packets.size(); ++i) + { + s << indent << " packets[" << i << "]: "; + s << std::endl; + s << indent; + Printer<::rslidar_msgs::rslidarPacket_>::stream(s, indent + " ", v.packets[i]); + } + } +}; + +} // namespace message_operations +} // namespace ros diff --git a/src/airy/rslidar_sdk-main/src/msg/rs_msg/lidar_point_cloud_msg.hpp b/src/airy/rslidar_sdk-main/src/msg/rs_msg/lidar_point_cloud_msg.hpp new file mode 100644 index 0000000..96b4c8b --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/msg/rs_msg/lidar_point_cloud_msg.hpp @@ -0,0 +1,50 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once + +#include "rs_driver/msg/point_cloud_msg.hpp" + +#ifdef POINT_TYPE_XYZI + typedef PointCloudT LidarPointCloudMsg; +#elif defined(POINT_TYPE_XYZIF) + typedef PointCloudT LidarPointCloudMsg; +#elif defined(POINT_TYPE_XYZIRT) + typedef PointCloudT LidarPointCloudMsg; +#elif defined(POINT_TYPE_XYZIRTF) + typedef PointCloudT LidarPointCloudMsg; +#endif + + +#ifdef ENABLE_IMU_DATA_PARSE +#include "rs_driver/msg/imu_data_msg.hpp" +#endif \ No newline at end of file diff --git a/src/airy/rslidar_sdk-main/src/rs_driver.zip b/src/airy/rslidar_sdk-main/src/rs_driver.zip new file mode 100644 index 0000000..2fc5d9e Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver.zip differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/.clang-format b/src/airy/rslidar_sdk-main/src/rs_driver/.clang-format new file mode 100644 index 0000000..a30530e --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/.clang-format @@ -0,0 +1,66 @@ +--- +BasedOnStyle: Google +AccessModifierOffset: -2 +ConstructorInitializerIndentWidth: 2 +AlignEscapedNewlinesLeft: false +AlignTrailingComments: true +AllowAllParametersOfDeclarationOnNextLine: false +AllowShortIfStatementsOnASingleLine: false +AllowShortLoopsOnASingleLine: false +AllowShortFunctionsOnASingleLine: None +AllowShortLoopsOnASingleLine: false +AlwaysBreakTemplateDeclarations: true +AlwaysBreakBeforeMultilineStrings: false +BreakBeforeBinaryOperators: false +BreakBeforeTernaryOperators: false +BreakConstructorInitializersBeforeComma: true +BinPackParameters: true +ColumnLimit: 125 +ConstructorInitializerAllOnOneLineOrOnePerLine: true +DerivePointerBinding: false +PointerBindsToType: true +ExperimentalAutoDetectBinPacking: false +IndentCaseLabels: true +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCSpaceBeforeProtocolList: true +PenaltyBreakBeforeFirstCallParameter: 19 +PenaltyBreakComment: 60 +PenaltyBreakString: 1 +PenaltyBreakFirstLessLess: 1000 +PenaltyExcessCharacter: 1000 +PenaltyReturnTypeOnItsOwnLine: 90 +SpacesBeforeTrailingComments: 2 +Cpp11BracedListStyle: false +Standard: Auto +IndentWidth: 2 +TabWidth: 2 +UseTab: Never +IndentFunctionDeclarationAfterType: false +SpacesInParentheses: false +SpacesInAngles: false +SpaceInEmptyParentheses: false +SpacesInCStyleCastParentheses: false +SpaceAfterControlStatementKeyword: true +SpaceBeforeAssignmentOperators: true +ContinuationIndentWidth: 4 +SortIncludes: false +SpaceAfterCStyleCast: false + +# Configure each individual brace in BraceWrapping +BreakBeforeBraces: Custom + +# Control of individual brace wrapping cases +BraceWrapping: { + AfterClass: 'true' + AfterControlStatement: 'true' + AfterEnum : 'true' + AfterFunction : 'true' + AfterNamespace : 'true' + AfterStruct : 'true' + AfterUnion : 'true' + BeforeCatch : 'true' + BeforeElse : 'true' + IndentBraces : 'false' +} +... diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/CHANGELOG.md b/src/airy/rslidar_sdk-main/src/rs_driver/CHANGELOG.md new file mode 100644 index 0000000..ac1c020 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/CHANGELOG.md @@ -0,0 +1,376 @@ +# CHANGLOG + +# UnReleased + +### Fix +- Fix PCAP playback speed issue. +- Fix the compilation issue of rs_driver_viewer on Ubuntu 22.04. + + +## v1.5.17 2025-02-14 + +### Added +- Support RSAIRY. +- Support parsing IMU data for RSAIRY and RSE1. +- Support parsing IMU extrinsics parameters frome difop for RSAIRY. + +### Changed +- Add feature attribute to point type. +- Update help document. +- Update block_time_offset as us for RSE1 + +### Fix +- Fix compilation bug in win/input_socket_select. + + +## v1.5.16 2024-08-27 + +### Added +- Support sn parsing of RSMX. +### Fixed +- Update msop protocol of RSMX. +- Fix compilation bug in epoll socket. +- Fix compilation warning for unit tests. + + +## v1.5.15 2024-08-07 + +### Added +- Support RSM3. + + +## v1.5.14 2024-07-15 + +### Added +- Support multiple lidars with different multicast addresses and the same port. +### Fixed +- Fix the bug that only one lidar was parsed correctly when multiple bp4.0 were used. + +## v1.5.13 2024-05-10 + +### Added +- Support RSMX. +### Fixed +- Update timestamp parsing unit and the number of packets per frame in decoder_RSE1. +- Update firing_tss of Helios/Helios16P/RubyPlus. +- Fix compilation bug of unit test. + + +## v1.5.12 2023-12-28 + +### Fixed +- Fix bug in getting device info and status. +- Fix bug in getting device temperature. + + +## v1.5.11 2023-12-18 + +### Added +- Split frame at the end of frame. + +### Changed +- Enable modify socket buffer size. + +### Fixed +- Fix the bug of rs_drvier_viewer.cpp abnormal crash. +- Fix fir_tss of RSBPV4. +- Fix rotation direction of RSBPV4 when reversal. + + +## v1.5.10 2023-04-11 + +### Added +- Merge RSBPV4 into RSBP + + +## v1.5.9 2023-02-17 + +### Added +- Support CRC32 check on MSOP/DIFOP packets. +- Support parsing DIFOP packets to get config/status data. + +### Changed +- Filter MOSP/DIFOP messages with two bytes instead of one byte. +- clear current cloud point when stop the instance of rs_driver. +- Recover frame_id field for C user. + +### Fixed +- Fix pcl point cloud message. Adapt it to the format of the output file of rslidar_sdk. +- Fix timestamp value of XYZIRT for RS128 + + + +## v1.5.8 2022-12-09 + +### Added +- Add User's guide document + +### Changed +- Rename RSEOS as RSE1 +- Let user's distance values cover LiDAR's + +### Fixed +- Revert "Report error ERRCODE_MSOP_TIMEOUT if only DIFOP packet is received", to avoid incorrect error report. +- Fix error distance of RSM2. Change it to 250m. + + + +## v1.5.7 2022-10-09 + +### Added +- Add tool to save as PCD file +- Seperate RSBPV4 from RSBP +- Add demo app demo_online_multi_lidars + +### Changed +- Disable error report in case of wrong block id for RS128/RS80 temporarily + +### Fixed +- Fix distance range of helios series. Also update distance ranges of other lidars +- Report error ERRCODE_MSOP_TIMEOUT if only DIFOP packet is received + + + +## v1.5.6 2022-09-01 + +### Added +- Add option ENABLE_DOUBLE_RCVBUF to solve the packet-loss problem +- Add option ENABLE_WAIT_IF_QUEUE_EMPTY to reduce CPU usage. +- Add option ENABLE_STAMP_WITH_LOCAL to convert point cloud's timestamp to local time + +### Changed +- Make ERRCODEs different for MSOP/DIFOP Packet +- Rename error code CLOUDOVERFLOW +- For RSM2, recover its coordinate to ROS-compatible +- For RSM2, adapt to increased MSOP packet len +- Update `demo_pcap` and `rs_driver_viewer` with cloud queue +- Accept angle.csv with vert angle only +- Update help documents + +### Fixed +- For Ruby and Ruby Plus, fix the problem of parsing point cloud' timestamp. +- Fix ERRCODE_MSOPTIMEOUT problem of input_sock_epoll + +### Removed +- Remove option ENABLE_RCVMMSG + + + +## v1.5.5 2022-08-01 + +### Added +- Compiled rs_driver_viewer on Windows, and add help doc +- Add option to double RECVBUF of UDP sockets + +### Changed +- Update demo_online to exit orderly. + +### Fixed +- Fix runtime error of Eigen in case of ENABLE_TRANSFORM +- Fix Compiling error on QNX +- Fix pcap_rate +- Fix the problem with repeated stop and start of driver + +### Removed +- Remove option of high priority thread + + + +## v1.5.4 2022-07-01 + +### Added +- Support Ruby_3.0_48 +- Add option to stamp point cloud with first point + +### Updated +- Distinguish 80/80v with lidar model +- Use ROS coordinate for EOS +- Enable PCAP file parsing in default mode +- Parse DIFOP packet in case of jumbo pcap file +- Update demo_online example to use ponit cloud queue +- Update help documents + +### Fixed +- Fix lidar temperature + + + +## v1.5.3 2022-06-01 + +### Added +- Add option to receive packet with epoll() +- Support Jumbo Mode + +### Fixed +- Check overflow of point cloud +- Fix compiling error of multiple definition + + + +## v1.5.2 2022-05-20 + +### Added +- Support RSP128/RSP80/RSP48 lidars +- Support EOS lidar +- Add option to usleep() when no packets to be handled + +### Changed +- Limit error information when error happens +- Use raw buffer for packet callback +- Split frame by seq 1 (for MEMS lidars) +- Remove difop handle thread + + + +## v1.5.1 - 2022-04-28 + +### Changed +- When replay MSOP/DIFOP file, use the timestamp when it is recording. +For Mechanical LiDARs, +- Split frame by block instead of by packet +- Let every point has its own timestamp, instead of using the block's one. +- + + + +## v1.5.0 - 2022-04-21 + +-- Refactory the coder part + + + +## v1.4.6 - 2022-04-21 + +### Added +- Check msop timeout +- Support M2 +- add cmake option ENABLE_RECVMMSG + +### Changed +- Optimize point cloud transform + + + +## v1.4.5 - 2022-03-09 + +### Added +- Support dense attribute +- Support to bind to a specifed ip +- Limit max size of packet queue +- Apply SO_REUSEADDR option to the receiving socket +- Support user layer and tail layer +- add macro option to disable the PCAP function. + +### Changed +- Join multicast group with code instead of shell script + +### Fixed +- Fix memory leaks problem +- Fix temperature calculation (for M1 only) + + + +## v1.4.0 - 2021-11-01 + +### Changed +Optimazation to decrease CPU uage, includes: +- Replace point with point cloud as template parameter +- Instead of alloc/free packet, use packet pool +- Instead of alloc/free point cloud, always keep point cloud memory +- By default, use conditional macro to disable scan_msg/camera_trigger related code + + + +## V1.3.1 + +### Added +- Add vlan support +- Add somip support +- Add split frame when pkt_cnt < last_pkt_cnt in mems +- Add temperature in mems +- Add ROCK support + +### Fixed +- Fix don't get time when PointType doesn't have timestamp member +- Fix ROCK light center compensation algorithm + +### Removed +- Remove redundance condition code in vec.emplace_back(std::move(point)) in mech lidars + + + +## v1.3.0 - 2020-11-10 + +### Added + +- Add RSHELIOS support +- Add RSM1 (B3) support +- Add Windows support +- Add rs_driver_viewer, a small tool to show point cloud +- Add save_by_rows argument +- Add multi-cast support +- Add points transformation function + +### Changed + +- Update some decoding part for LiDARs +- Change the definition of packet message +- Update documents + + + +## v1.2.1 - 2020-09-04 + +### Fixed + +- Fix the timestamp calculation for RS16,RS32 & RSBP. Now the output lidar timestamp will be UTC time and will not be affected by system time zone. + + + +## v1.2.0 - 2020-09-01 + +### Added + +- Add interface in driver core to get lidar temperature +- Add support for point type XYZIRT (R - ring id)(T - timestamp) +- Add RS80 support +- Add interface in driver core to get camera trigger info + +### Changed + +- Update the decoding part for ruby in echo-dual mode +- Update the compiler version from C++11 to C++14 + + + +## v1.1.0 - 2020-07-01 + +### Added + +- Add the limit of the length of the msop queue +- Add the exception capture when loading .csv file + +### Fixed +- Fix the bug in calculating the timestamp of 128 +- Fix the bug in calculating RPM + +### Changed +- Update some functions' names +- Update the program structure + +### Removed +- Remove unused variables in point cloud message + + + +## v1.0.0 - 2020-06-01 + +### Added + +- New program structure + +- Easy to do advanced development + +- Remove the redundant code in old driver. + + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/CMakeLists.txt b/src/airy/rslidar_sdk-main/src/rs_driver/CMakeLists.txt new file mode 100644 index 0000000..025fce0 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/CMakeLists.txt @@ -0,0 +1,246 @@ +cmake_minimum_required(VERSION 3.5) + +cmake_policy(SET CMP0048 NEW) # CMake 3.6 required + +if(WIN32) + cmake_policy(SET CMP0074 NEW) # CMake 3.12 required +endif(WIN32) + +project(rs_driver VERSION 1.5.17) + +#======================== +# Project setup +#======================== +if (CMAKE_BUILD_TYPE STREQUAL "") + set(CMAKE_BUILD_TYPE Release) +endif() + +#============================= +# Compile Features +#============================= +option(DISABLE_PCAP_PARSE "Disable PCAP file parse" OFF) +option(ENABLE_TRANSFORM "Enable transform functions" OFF) + +option(ENABLE_MODIFY_RECVBUF "Enable modify size of RCVBUF" OFF) +option(ENABLE_WAIT_IF_QUEUE_EMPTY "Enable waiting for a while in handle thread if the queue is empty" OFF) +option(ENABLE_EPOLL_RECEIVE "Receive packets with epoll() instead of select()" OFF) + +option(ENABLE_STAMP_WITH_LOCAL "Enable stamp point cloud with local time" OFF) +option(ENABLE_PCL_POINTCLOUD "Enable PCL Point Cloud" OFF) +option(ENABLE_CRC32_CHECK "Enable CRC32 Check on MSOP Packet" OFF) +option(ENABLE_DIFOP_PARSE "Enable parsing DIFOP Packet" OFF) + +#============================= +# Compile Demos, Tools, Tests +#============================= +option(COMPILE_DEMOS "Build rs_driver demos" OFF) +option(COMPILE_TOOLS "Build rs_driver tools" OFF) +option(COMPILE_TOOL_VIEWER "Build point cloud visualization tool" OFF) +option(COMPILE_TOOL_PCDSAVER "Build point cloud pcd saver tool" OFF) +option(COMPILE_TESTS "Build rs_driver unit tests" OFF) + +#======================== +# Platform cross setup +#======================== +if(MSVC) + + set(COMPILER "MSVC Compiler") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /Wall") + set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} /Od /Zi") + set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} /O2") + set(CompilerFlags + CMAKE_CXX_FLAGS CMAKE_C_FLAGS + CMAKE_CXX_FLAGS_DEBUG CMAKE_C_FLAGS_DEBUG + CMAKE_CXX_FLAGS_RELEASE CMAKE_C_FLAGS_RELEASE + CMAKE_CXX_FLAGS_MINSIZEREL CMAKE_C_FLAGS_MINSIZEREL + CMAKE_CXX_FLAGS_RELWITHDEBINFO CMAKE_C_FLAGS_RELWITHDEBINFO) + + foreach(CompilerFlag ${CompilerFlags}) + string(REPLACE "/MT" "/MD" ${CompilerFlag} "${${CompilerFlag}}") + endforeach() + + add_compile_definitions(_DISABLE_EXTENDED_ALIGNED_STORAGE) # to disable a msvc error + set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} /STACK:100000000") + +elseif(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") + + if(UNIX) + set(COMPILER "UNIX GNU Compiler") + elseif(MINGW) + set(COMPILER "MINGW Compiler") + else() + message(FATAL_ERROR "Unsupported compiler.") + endif() + + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -Wall -Wno-unused-parameter") + set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -g") + set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -O3") +endif() + +message(=============================================================) +message("-- CMake run for ${COMPILER}") +message(=============================================================) + +#======================== +# Path Setup +#======================== +set(CMAKE_INSTALL_PREFIX /usr/local) +set(INSTALL_DRIVER_DIR ${CMAKE_INSTALL_PREFIX}/${CMAKE_PROJECT_NAME}/include) +set(INSTALL_CMAKE_DIR ${CMAKE_INSTALL_PREFIX}/lib/cmake) +set(DRIVER_INCLUDE_DIRS ${CMAKE_CURRENT_LIST_DIR}/src) +set(DRIVER_CMAKE_ROOT ${CMAKE_CURRENT_LIST_DIR}/cmake) + +if(WIN32) + list(APPEND EXTERNAL_LIBS ws2_32) +else() + if (CMAKE_SYSTEM_NAME STREQUAL "QNX") + list(APPEND EXTERNAL_LIBS socket) + else() + list(APPEND EXTERNAL_LIBS pthread) + endif() +endif(WIN32) + +#======================== +# Build Features +#======================== + +if(${ENABLE_EPOLL_RECEIVE}) + + message(=============================================================) + message("-- Enable Epoll Receive") + message(=============================================================) + + add_definitions("-DENABLE_EPOLL_RECEIVE") +endif(${ENABLE_EPOLL_RECEIVE}) + +if(${DISABLE_PCAP_PARSE}) + + message(=============================================================) + message("-- Disable PCAP parse") + message(=============================================================) + + add_definitions("-DDISABLE_PCAP_PARSE") + +else() + + if(WIN32) + set(CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/cmake) + find_package(PCAP REQUIRED) + include_directories(${PCAP_INCLUDE_DIR}) + list(APPEND EXTERNAL_LIBS ${PCAP_LIBRARY}) + else() + list(APPEND EXTERNAL_LIBS pcap) + endif(WIN32) + +endif(${DISABLE_PCAP_PARSE}) + +if(${ENABLE_TRANSFORM}) + + message(=============================================================) + message("-- Enable Transform Fucntions") + message(=============================================================) + + add_definitions("-DENABLE_TRANSFORM") + + # Eigen + find_package(Eigen3 REQUIRED) + include_directories(${EIGEN3_INCLUDE_DIR}) + +endif(${ENABLE_TRANSFORM}) + + + +#============================ +# Build Demos, Tools, Tests +#============================ + +if(${ENABLE_MODIFY_RECVBUF}) + add_definitions("-DENABLE_MODIFY_RECVBUF") +endif(${ENABLE_MODIFY_RECVBUF}) + +if(${ENABLE_WAIT_IF_QUEUE_EMPTY}) + add_definitions("-DENABLE_WAIT_IF_QUEUE_EMPTY") +endif(${ENABLE_WAIT_IF_QUEUE_EMPTY}) + +if(${ENABLE_STAMP_WITH_LOCAL}) + add_definitions("-DENABLE_STAMP_WITH_LOCAL") +endif(${ENABLE_STAMP_WITH_LOCAL}) + +if(${ENABLE_PCL_POINTCLOUD}) + add_definitions("-DENABLE_PCL_POINTCLOUD") +endif(${ENABLE_PCL_POINTCLOUD}) + +if(${ENABLE_CRC32_CHECK}) + add_definitions("-DENABLE_CRC32_CHECK") +endif(${ENABLE_CRC32_CHECK}) + +if(${ENABLE_DIFOP_PARSE}) + add_definitions("-DENABLE_DIFOP_PARSE") +endif(${ENABLE_DIFOP_PARSE}) + +if(${COMPILE_DEMOS}) + add_subdirectory(${CMAKE_CURRENT_LIST_DIR}/demo) +endif(${COMPILE_DEMOS}) + +if (${COMPILE_TOOLS}) + set(COMPILE_TOOL_VIEWER ON) + set(COMPILE_TOOL_PCDSAVER ON) +endif (${COMPILE_TOOLS}) + +if(${COMPILE_TOOL_VIEWER} OR ${COMPILE_TOOL_PCDSAVER}) + add_subdirectory(${CMAKE_CURRENT_LIST_DIR}/tool) +endif(${COMPILE_TOOL_VIEWER} OR ${COMPILE_TOOL_PCDSAVER}) + +if(${COMPILE_TESTS}) + add_subdirectory(${CMAKE_CURRENT_LIST_DIR}/test) +endif(${COMPILE_TESTS}) + +#======================== +# Cmake +#======================== +configure_file( + ${DRIVER_CMAKE_ROOT}/rs_driverConfig.cmake.in + ${DRIVER_CMAKE_ROOT}/rs_driverConfig.cmake + @ONLY) + +configure_file( + ${DRIVER_CMAKE_ROOT}/rs_driverConfigVersion.cmake.in + ${DRIVER_CMAKE_ROOT}/rs_driverConfigVersion.cmake + @ONLY) + +configure_file ( + ${CMAKE_CURRENT_LIST_DIR}/src/rs_driver/macro/version.hpp.in + ${CMAKE_CURRENT_LIST_DIR}/src/rs_driver/macro/version.hpp + @ONLY) + +if(NOT ${CMAKE_SOURCE_DIR} STREQUAL ${CMAKE_CURRENT_SOURCE_DIR}) + set(rs_driver_DIR ${DRIVER_CMAKE_ROOT} PARENT_SCOPE) +endif() + +#======================== +# Install & Uninstall +#======================== +if(UNIX AND ${CMAKE_SOURCE_DIR} STREQUAL ${CMAKE_CURRENT_SOURCE_DIR}) + + install(FILES ${DRIVER_CMAKE_ROOT}/rs_driverConfig.cmake + ${DRIVER_CMAKE_ROOT}/rs_driverConfigVersion.cmake + DESTINATION ${INSTALL_CMAKE_DIR}/${CMAKE_PROJECT_NAME}) + + install(DIRECTORY src/ + DESTINATION ${INSTALL_DRIVER_DIR} + FILES_MATCHING PATTERN "*.h") + + install(DIRECTORY src/ + DESTINATION ${INSTALL_DRIVER_DIR} + FILES_MATCHING PATTERN "*.hpp") + + if(NOT TARGET uninstall) + configure_file( + ${CMAKE_CURRENT_LIST_DIR}/cmake/cmake_uninstall.cmake.in + ${PROJECT_BINARY_DIR}/cmake_uninstall.cmake @ONLY) + add_custom_target(uninstall + COMMAND ${CMAKE_COMMAND} -P ${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake) + endif(NOT TARGET uninstall) + +endif(UNIX AND ${CMAKE_SOURCE_DIR} STREQUAL ${CMAKE_CURRENT_SOURCE_DIR}) + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/LICENSE b/src/airy/rslidar_sdk-main/src/rs_driver/LICENSE new file mode 100644 index 0000000..8b1fb13 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/LICENSE @@ -0,0 +1,19 @@ +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/README.md b/src/airy/rslidar_sdk-main/src/rs_driver/README.md new file mode 100644 index 0000000..e60317b --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/README.md @@ -0,0 +1,260 @@ +# 1 **rs_driver** + +[中文介绍](README_CN.md) + + + +## 1.1 Introduction + +**rs_driver** is the driver kernel for the RoboSense LiDARs. + +Please download the official release from [github](https://github.com/RoboSense-LiDAR/rs_driver/releases), or get the latest version with the git client tool. + +````shell +git clone https://github.com/RoboSense-LiDAR/rs_driver.git +```` + + + +## 1.2 Supported LiDARs + +Below are the supported LiDARS. + +- RS-LiDAR-16 +- RS-LiDAR-32 +- RS-Bpearl +- RS-Helios +- RS-Helios-16P +- RS-Ruby-128 +- RS-Ruby-80 +- RS-Ruby-48 +- RS-Ruby-Plus-128 +- RS-Ruby-Plus-80 +- RS-Ruby-Plus-48 +- RS-LiDAR-M1 +- RS-LiDAR-M2 +- RS-LiDAR-M3 +- RS-LiDAR-E1 +- RS-LiDAR-MX +- RS-LiDAR-AIRY + + +## 1.3 Supported Platforms + +**rs_driver** is supported on the following platforms and compilers. Note the compiler should support `C++14`. + +- Ubuntu (16.04, 18.04) + - gcc (4.8+) + +- Windows + - MSVC ( tested with Win10 / VS2019) + + + +## 1.4 Dependency Libraries + +**rs_driver** depends on the following third-party libraries. + +- libpcap (optional, needed to parse PCAP file) +- Eigen3 (optional, needed to use the internal transformation function) +- PCL (optional, needed to build the visualization tool) +- Boost (optional, needed to build the visualization tool) + + + +## 1.5 Compile On Ubuntu + +### 1.5.1 Dependency Libraries + +```sh +sudo apt-get install libpcap-dev libeigen3-dev libboost-dev libpcl-dev +``` + +### 1.5.2 Compilation + +```bash +cd rs_driver +mkdir build && cd build +cmake .. && make -j4 +``` + +### 1.5.3 Installation + +```bash +sudo make install +``` + +### 1.5.4 Use `rs_driver` as a third party library + +In your ```CMakeLists.txt```, find the **rs_driver** package and link to it . + +```cmake +find_package(rs_driver REQUIRED) +include_directories(${rs_driver_INCLUDE_DIRS}) +target_link_libraries(your_project ${rs_driver_LIBRARIES}) +``` + +### 1.5.5 Use `rs_driver` as a submodule + +Add **rs_driver** into your project as a submodule. + +In your ```CMakeLists.txt```, find the **rs_driver** package and link to it . + +```cmake +add_subdirectory(${PROJECT_SOURCE_DIR}/rs_driver) +find_package(rs_driver REQUIRED) +include_directories(${rs_driver_INCLUDE_DIRS}) +target_link_libraries(your_project ${rs_driver_LIBRARIES}) +``` + + + +## 1.6 Compile On Windows + +### 1.6.1 Dependency Libraries + +#### 1.6.1.1 libpcap + +Install [libpcap runtime library](https://www.winpcap.org/install/bin/WinPcap_4_1_3.exe). + +Unzip [libpcap's developer's pack](https://www.winpcap.org/install/bin/WpdPack_4_1_2.zip) to your favorite location, and add the path to the folder ```WpdPack_4_1_2/WpdPack``` to the environment variable ```PATH``` . + +#### 1.6.1.2 PCL + +To compile with VS2019, please use the official installation package [PCL All-in-one installer](https://github.com/PointCloudLibrary/pcl/releases). + +Select the "Add PCL to the system PATH for xxx" option during the installation. +![](./img/01_01_install_pcl.png) + +### 1.6.2 Installation + +Installation is not supported on Windows. + + + +## 1.7 Quick Start + +**rs_driver** offers two demo programs in ```rs_driver/demo```. + +- demo_online.cpp + +​ `demo_online` connects to online lidar, and output point cloud. + +- demo_pcap.cpp + +​ `demo_pcap` parses pcap file, and output point cloud. It is based on libpcap. + + + +To build `demo_online` and `demo_pcap`, enable the CMake option `COMPILE_DEMOS`. + +```bash +cmake -DCOMPILE_DEMOS=ON .. +``` + +For more info about `demo_online`, Please refer to [Decode online LiDAR](doc/howto/07_how_to_decode_online_lidar.md) + +For more info about `demo_pcap`, Please refer to [Decode pcap file](doc/howto/09_how_to_decode_pcap_file.md) + + + +## 1.8 Visualization of Point Cloud + +**rs_driver** offers a visualization tool `rs_driver_viwer` in ```rs_driver/tool``` , which is based on PCL. + +To build it, enable the CMake option CMOPILE_TOOLS. + +```bash +cmake -DCOMPILE_TOOLS=ON .. +``` + +For more info about `rs_driver_viewer`, please refer to [Visualization tool guide](doc/howto/13_how_to_use_rs_driver_viewer.md) + + + +## 1.9 API files + +For more info about the `rs_driver` API, Please refer to: +- **Point Cloud message definition**: ```rs_driver/src/rs_driver/msg/point_cloud_msg.hpp```, ```rs_driver/src/rs_driver/msg/pcl_point_cloud_msg.hpp``` +- **API definition**: ```rs_driver/src/rs_driver/api/lidar_driver.hpp``` +- **Parameters definition**: ```rs_driver/src/rs_driver/driver/driver_param.hpp```, +- **Error code definition**: ```rs_driver/src/rs_driver/common/error_code.hpp``` + + + +## 1.10 More Topics + +For more topics, Please refer to: + ++ What is available in the **rs_driver** project? Include examples, tools, documents. + +​ Please see [Directories and Files](doc/intro/02_directories_and_files.md) + ++ There are two versions of **rs_driver**: `v1.3.x` and `v1.5.x`. Which is suggested? Is it suggested to upgrade to `v1.5.x` ? How to upgrade? + +​ Please see [How to port from v1.3.x to v1.5.x](doc/howto/07_how_to_port_your_app_from_rs_driver_1.3.x_to_1.5.x.md) + ++ How to compile **rs_driver** on Windows ? + +​ Please see [How to compile `rs_driver` on Windows](./doc/howto/16_how_to_compile_on_windows.md) + ++ What is the interface of **rs_driver** like ? How to integrate it into my system? + +​ Please see [Thread Model and Interface](./doc/intro/03_thread_model.md) + ++ Is there any examples about how to integrate **rs_driver** ? And if mulitple LiDARs? + +​ Please see [How to connect to online LiDAR](doc/howto/08_how_to_decode_online_lidar.md)，[How to decode PCAP file](doc/howto/10_how_to_decode_pcap_file.md)。 + ++ How to configure **rs_driver**'s network options in unicast/multicast/broadcast mode? And if mulitple Lidars? With VLAN layer? May I add extra layers before/after MSOP/DIFOP packet? All configurations are OK, and Wireshark/tcpdump can capture the packets, but **rs_driver** still fails to get them. Why ? + +​ Please see [Online LiDAR - Advanced Topics](doc/howto/09_online_lidar_advanced_topics.md) + ++ To decode a PCAP file with only one LiDAR data, how to configure `rs_driver`? And if multiple LiDARs? With VLAN layer? With extra layers before/after MSOP/DIFOP packet? + +​ Please see [PCAP File - Advanced Topics](doc/howto/11_pcap_file_advanced_topics.md) + ++ Can I determine how to configure `rs_driver` by a PCAP file? For both online LiDAR and PCAP file? + +​ Please see [How to configure rs_driver by PCAP file](doc/howto/12_how_to_configure_by_pcap_file.md) + ++ What format of PCAP file is supported by `rs_driver`? How to capture such a file? + +​ Please see [How to capture a PCAP file for rs_driver](doc/howto/13_how_to_capture_pcap_file.md) + ++ How to compile demo apps/tools? How to avoid packet loss? How much is CPU usage? Point cloud is UTC time. How to convert it to loca time? How to disable PCAP file decoding on embedded Linux? How to enable point transformation? + +​ Please see [rs_driver CMake Macros](./doc/intro/05_cmake_macros_intro.md) + ++ How to specify Lidar type? How to specify data source (online LiDAR, PCAP file, user's data) ? How to timestamp point cloud with LiDAR clock or Host clock? How to discard NAN points in point cloud? Where does point cloud get its timestamp? from its first point, or last point? How to configure point transform? How to change the splitting angle of mechanical LiDAR? + +​ Please see [rs_driver configuration parameters](./doc/intro/04_parameter_intro.md) + ++ **rs_driver** reports an error `ERRCODE_MSOPTIMEOUT`/`ERRCODE_WRONGMSOPID`/`ERRCODE_PKTBUFOVERFLOW` ...... What does it mean? + +​ Please see [rs_driver Error Code](./doc/intro/06_error_code_intro.md) + ++ How to transform point cloud to another position? + +​ Please see [How to transform point cloud](doc/howto/15_how_to_transform_pointcloud.md) + ++ How much is data flow of RoboSense LiDARs ? When may it loss packets? How to avoid this? + +​ Please see [How to avoid packet Loss](doc/howto/17_how_to_avoid_packet_loss.md) + ++ How much CPU and memory does **rs_driver** need？ + +​ Please see [CPU Usage and Memory Usage of `rs_driver`](doc/howto/20_about_usage_of_cpu_and_memory.md) + ++ What is point layout like? What's the coordinate system of **rs_driver**? What is `ring` of point? Where is point timestamp from? + +​ Please see [Point Layout in point cloud](doc/howto/18_about_point_layout.md) + ++ How does RoboSense split frames? It uses the UDP protocol. What if packet loss or out of order? + +​ Please see [Splitting frames](doc/howto/19_about_splitting_frame.md) + ++ Want to know more implementation details about `rs_driver`? + +​ Please see [Analysis of rs_driver's source code](doc/src_intro/rs_driver_intro_CN.md) + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/README_CN.md b/src/airy/rslidar_sdk-main/src/rs_driver/README_CN.md new file mode 100644 index 0000000..ef43c78 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/README_CN.md @@ -0,0 +1,268 @@ +# 1 **rs_driver预览** + +[English Version](README.md) + + + +## 1.1 概述 + +**rs_driver**是RoboSense雷达的驱动库。 + +可以从[github](https://github.com/RoboSense-LiDAR/rs_driver/releases)下载正式版本。也可以使用git工具得到最新版本。 + +``` +git clone https://github.com/RoboSense-LiDAR/rs_driver.git +``` + + + +## 1.2 支持的雷达型号 + +**rs_driver**支持的雷达型号如下。 + ++ RS-LiDAR-16 ++ RS-LiDAR-32 ++ RS-Bpearl ++ RS-Helios ++ RS-Helios-16P ++ RS-Ruby-128 ++ RS-Ruby-80 ++ RS-Ruby-48 ++ RS-Ruby-Plus-128 ++ RS-Ruby-Plus-80 ++ RS-Ruby-Plus-48 ++ RS-LiDAR-M1 ++ RS-LiDAR-M2 ++ RS-LiDAR-M3 ++ RS-LiDAR-E1 ++ RS-LiDAR-MX ++ RS-LiDAR-AIRY + + + +## 1.3 支持的操作系统 + +**rs_driver**支持的操作系统及编译器如下。注意编译器需支持`C++14`标准。 + +- Ubuntu (16.04, 18.04, 20.04) + - gcc (4.8+) + +- Windows + - MSVC (Win10 / VS2019 已测试) + + + +## 1.4 依赖的第三方库 + +**rs_driver**依赖的第三方库如下。 + +- `libpcap` (可选。如不需要解析PCAP文件，可忽略) +- `eigen3` (可选。如不需要内置坐标变换，可忽略) +- `PCL` (可选。如不需要可视化工具，可忽略) +- `Boost` (可选。如不需要可视化工具，可忽略) + + + +## 1.5 Ubuntu下的编译及安装 + +### 1.5.1 安装第三方库 + +```bash +sudo apt-get install libpcap-dev libeigen3-dev libboost-dev libpcl-dev +``` +### 1.5.2 编译 + +```bash +cd rs_driver +mkdir build && cd build +cmake .. && make -j4 +``` + +### 1.5.3 安装 + +```bash +sudo make install +``` + +### 1.5.4 作为第三方库使用 + +配置您的```CMakeLists.txt```文件，使用find_package()指令找到**rs_driver**库，并链接。 + +```cmake +find_package(rs_driver REQUIRED) +include_directories(${rs_driver_INCLUDE_DIRS}) +target_link_libraries(your_project ${rs_driver_LIBRARIES}) +``` + +### 1.5.5 作为子模块使用 + +将**rs_driver**作为子模块添加到您的工程，相应配置您的```CMakeLists.txt```文件。 + +使用find_package()指令找到**rs_driver**库，并链接。 + +```cmake +add_subdirectory(${PROJECT_SOURCE_DIR}/rs_driver) +find_package(rs_driver REQUIRED) +include_directories(${rs_driver_INCLUDE_DIRS}) +target_link_libraries(project ${rs_driver_LIBRARIES}) +``` + + + +## 1.6 Windows下的编译及安装 + +### 1.6.1 安装第三方库 + +#### 1.6.1.1 libpcap + +安装[libpcap运行库](https://www.winpcap.org/install/bin/WinPcap_4_1_3.exe)。 + +解压[libpcap开发者包](https://www.winpcap.org/install/bin/WpdPack_4_1_2.zip)到任意位置，并将```WpdPack_4_1_2/WpdPack``` 的路径添加到环境变量```PATH```。 + +#### 1.6.1.2 PCL + +如果使用MSVC编译器，可使用PCL官方提供的[PCL安装包](https://github.com/PointCloudLibrary/pcl/releases)安装。 + +安装过程中选择 `Add PCL to the system PATH for xxx`。 +![](./img/01_01_install_pcl.png) + +### 1.6.2 安装 + +Windows下，**rs_driver** 暂不支持安装。 + +### 1.6.3 VS工程文件 + +工程目录`rs_driver/win`下，有编译实例程序和工具的MSVS工程文件。 + +关于Windows下编译和使用**rs_driver**的更多说明，可以参考[如何在Windows下编译rs_driver](doc/howto/16_how_to_compile_on_windows_CN.md) + + + +## 1.7 示例程序 + +**rs_driver**在目录```rs_driver/demo``` 下，提供了两个示例程序。 + +- `demo_online.cpp` + +​ `demo_online`解析在线雷达的数据，输出点云。 + + +- `demo_pcap.cpp` + +​ `demo_pcap`解析PCAP文件，输出点云。编译`demo_pcap`需要安装`libpcap`库。 + + + +要编译这两个程序，需使能`COMPILE_DEMOS`选项。 + +```bash +cmake -DCOMPILE_DEMOS=ON .. +``` + +关于`demo_online`的更多说明，可以参考[如何连接在线雷达](doc/howto/08_how_to_decode_online_lidar_CN.md) + +关于`demo_pcap`的更多说明，可以参考[如何解码PCAP文件](doc/howto/10_how_to_decode_pcap_file_CN.md) + + + +## 1.8 点云可视化工具 + +**rs_driver**在目录```rs_driver/tool``` 下，提供了一个点云可视化工具`rs_driver_viewer`。 + +要编译这个工具，需使能`COMPILE_TOOS`选项。编译它需要安装PCL库和Boost库。 + +```bash +cmake -DCOMPILE_TOOLS=ON .. +``` + +关于`rs_driver_viewer`的使用方法，请参考[如何使用可视化工具rs_driver_viewer](doc/howto/14_how_to_use_rs_driver_viewer_CN.md) + + + +## 1.9 接口文件 + +**rs_driver**的主要接口文件如下。 + +- 点云消息定义: ```rs_driver/src/rs_driver/msg/point_cloud_msg.hpp```, ```rs_driver/src/rs_driver/msg/pcl_point_cloud_msg.hpp``` +- 接口定义: ```rs_driver/src/rs_driver/api/lidar_driver.hpp``` +- 参数定义: ```rs_driver/src/rs_driver/driver/driver_param.hpp``` +- 错误码定义: ```rs_driver/src/rs_driver/common/error_code.hpp``` + + + +## 1.10 更多主题 + +关于**rs_driver**的其他主题，请参考如下链接。 + ++ **rs_driver**工程中包括哪些例子、工具、和文档？ + +​ 请参考[rs_driver的目录结构](doc/intro/02_directories_and_files_CN.md) + ++ **rs_driver**有`v1.3.x`和`v1.5.x`两个版本？该选哪一个？现在正在使用`v1.3.x`，需要升级到`v1.5.x`吗？如何升级？ + +​ 请参考[如何从v1.3.x升级到v1.5.x](doc/howto/07_how_to_port_your_app_from_rs_driver_1.3.x_to_1.5.x_CN.md) + ++ Windows上如何编译**rs_driver**？ + +​ 请参考[如何在Windows上编译rs_driver](./doc/howto/16_how_to_compile_on_windows_CN.md) + ++ **rs_driver**的接口如何设计的？如何将**rs_driver**集成到我的系统中，有什么注意事项吗？ + +​ 请参考[rs_driver的线程模型与接口设计](./doc/intro/03_thread_model_CN.md) + ++ **rs_driver**如何集成到我自己的系统中？有参考的例子吗？有多雷达的例子吗？ + +​ 请参考[如何连接在线雷达](doc/howto/08_how_to_decode_online_lidar_CN.md)，[如何解析PCAP文件](doc/howto/10_how_to_decode_pcap_file_CN.md)。 + ++ 在单播、组播、广播的情况下，应该如何配置**rs_driver**的网络选项？在多雷达的情况下如何配置？在VLAN的情况下如何配置？可以在MSOP/DIFOP数据的前后加入自己的层吗？这些网络配置都确认无误，用抓包软件也能抓到MSOP/DIFOP包，为什么还是接收不到点云？ + +​ 请参考[在线雷达 - 高级主题](doc/howto/09_online_lidar_advanced_topics_CN.md) + ++ 一般情况下，PCAP文件中只有一个雷达的数据，如何配置网络选项？ 如果文件中有多个雷达的数据，如何配置？ 在VLAN的情况下如何配置？可以在MSOP/DIFOP数据的前后加入自己的层吗？ + +​ 请参考[PCAP文件 - 高级主题](doc/howto/11_pcap_file_advanced_topics_CN.md) + ++ 手上只有一个PCAP文件，可以根据它确定**rs_driver**的网络配置选项吗？包括连接在线雷达和解析PCAP文件。 + +​ 请参考[根据PCAP文件确定网络配置选项](doc/howto/12_how_to_configure_by_pcap_file_CN.md) + ++ **rs_driver**支持什么格式的PCAP文件？如何录制这样的文件？ + +​ 请参考[如何为rs_driver录制PCAP文件](doc/howto/13_how_to_capture_pcap_file_CN.md) + ++ 想编译示例程序/小工具，怎么指定编译选项？ **rs_driver**丢包了，如何解决？**rs_driver**的CPU占用率？点云的时间戳是UTC时间，能改成本地时间吗？嵌入式平台不需要解析PCAP文件，可以不编译这个特性吗？如何启用点云坐标转换功能？ + +​ 请参考[rs_driver CMake编译宏](./doc/intro/05_cmake_macros_intro_CN.md) + ++ **rs_driver**如何指定雷达类型？如何指定数据源为在线雷达、PCAP文件或用户数据？ 想先试用一下，但是对雷达作PTP时间同步很麻烦，可以先用电脑的系统时间给点云打时间戳吗？想节省一点内存空间，可以丢弃点云中的无效点吗？点云的时间戳来自它的第一个点，还是最后一个点？可以配置吗？点云坐标转换的位移、旋转参数如何设置？机械式雷达的分帧角度可以设置吗？ + +​ 请参考[rs_driver配置选项](./doc/intro/04_parameter_intro_CN.md) + ++ **rs_driver**报告了一个错误`ERRCODE_MSOPTIMEOUT`/`ERRCODE_WRONGMSOPID`/`ERRCODE_PKTBUFOVERFLOW` ...... 这个错误码是什么意思？ + +​ 请参考[rs_driver错误码](./doc/intro/06_error_code_intro_CN.md) + ++ 如何将点云变换到另一个位置和角度上去？ + +​ 请参考[点云坐标变换](doc/howto/15_how_to_transform_pointcloud_CN.md) + ++ RoboSense雷达的数据量有多大？在什么情况下可能丢包？怎么避免？ + +​ 请参考[丢包问题以及如何解决](doc/howto/17_how_to_avoid_packet_loss_CN.md) + ++ **rs_driver**要占用多少CPU和内存？ + +​ 请参考[rs_driver的CPU和内存占用](doc/howto/20_about_usage_of_cpu_and_memory_CN.md) + ++ 点在点云中是怎样布局的？点坐标的参考系是什么？点的通道 号指什么？点的时间戳来自哪里？ + +​ 请参考[点云中点的布局](doc/howto/18_about_point_layout_CN.md) + ++ RoboSense雷达怎么分帧？RoboSense使用UDP协议，万一丢包或乱序，会影响**rs_driver**的分帧处理吗？ + +​ 请参考[RoboSense雷达如何分帧](doc/howto/19_about_splitting_frame_CN.md) + ++ 希望进一步深入了解**rs_driver**的设计细节？ + +​ 请参考[rs_driver源代码解析](doc/src_intro/rs_driver_intro_CN.md) + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/cmake/FindPCAP.cmake b/src/airy/rslidar_sdk-main/src/rs_driver/cmake/FindPCAP.cmake new file mode 100644 index 0000000..3da93a3 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/cmake/FindPCAP.cmake @@ -0,0 +1,86 @@ +# - Try to find libpcap include dirs and libraries +# +# Usage of this module as follows: +# +# find_package(PCAP) +# +# Variables used by this module, they can change the default behaviour and need +# to be set before calling find_package: +# +# PCAP_ROOT_DIR Set this variable to the root installation of +# libpcap if the module has problems finding the +# proper installation path. +# +# Variables defined by this module: +# +# PCAP_FOUND System has libpcap, include and library dirs found +# PCAP_INCLUDE_DIR The libpcap include directories. +# PCAP_LIBRARY The libpcap library (possibly includes a thread +# library e.g. required by pf_ring's libpcap) +# HAVE_PF_RING If a found version of libpcap supports PF_RING +# HAVE_PCAP_IMMEDIATE_MODE If the version of libpcap found supports immediate mode + +find_path(PCAP_ROOT_DIR + NAMES include/pcap.h +) + +find_path(PCAP_INCLUDE_DIR + NAMES pcap.h + HINTS ${PCAP_ROOT_DIR}/include +) + +set (HINT_DIR ${PCAP_ROOT_DIR}/Lib) + +# On x64 windows, we should look also for the .lib at /lib/x64/ +# as this is the default path for the WinPcap developer's pack +if (${CMAKE_SIZEOF_VOID_P} EQUAL 8 AND WIN32) + set (HINT_DIR ${PCAP_ROOT_DIR}/Lib/x64 ${HINT_DIR}) +endif () + +find_library(PCAP_LIBRARY + NAMES pcap wpcap + HINTS ${HINT_DIR} + NO_DEFAULT_PATH +) + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(PCAP DEFAULT_MSG + PCAP_LIBRARY + PCAP_INCLUDE_DIR +) + +include(CheckCXXSourceCompiles) +set(CMAKE_REQUIRED_LIBRARIES ${PCAP_LIBRARY}) +check_cxx_source_compiles("int main() { return 0; }" PCAP_LINKS_SOLO) +set(CMAKE_REQUIRED_LIBRARIES) + +# check if linking against libpcap also needs to link against a thread library +if (NOT PCAP_LINKS_SOLO) + find_package(Threads) + if (THREADS_FOUND) + set(CMAKE_REQUIRED_LIBRARIES ${PCAP_LIBRARY} ${CMAKE_THREAD_LIBS_INIT}) + check_cxx_source_compiles("int main() { return 0; }" PCAP_NEEDS_THREADS) + set(CMAKE_REQUIRED_LIBRARIES) + endif (THREADS_FOUND) + if (THREADS_FOUND AND PCAP_NEEDS_THREADS) + set(_tmp ${PCAP_LIBRARY} ${CMAKE_THREAD_LIBS_INIT}) + list(REMOVE_DUPLICATES _tmp) + set(PCAP_LIBRARY ${_tmp} + CACHE STRING "Libraries needed to link against libpcap" FORCE) + else (THREADS_FOUND AND PCAP_NEEDS_THREADS) + message(FATAL_ERROR "Couldn't determine how to link against libpcap") + endif (THREADS_FOUND AND PCAP_NEEDS_THREADS) +endif (NOT PCAP_LINKS_SOLO) + +include(CheckFunctionExists) +set(CMAKE_REQUIRED_LIBRARIES ${PCAP_LIBRARY}) +check_function_exists(pcap_get_pfring_id HAVE_PF_RING) +check_function_exists(pcap_set_immediate_mode HAVE_PCAP_IMMEDIATE_MODE) +check_function_exists(pcap_set_tstamp_precision HAVE_PCAP_TIMESTAMP_PRECISION) +set(CMAKE_REQUIRED_LIBRARIES) + +mark_as_advanced( + PCAP_ROOT_DIR + PCAP_INCLUDE_DIR + PCAP_LIBRARY +) diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/cmake/cmake_uninstall.cmake.in b/src/airy/rslidar_sdk-main/src/rs_driver/cmake/cmake_uninstall.cmake.in new file mode 100644 index 0000000..ac47a0f --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/cmake/cmake_uninstall.cmake.in @@ -0,0 +1,23 @@ +if(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt") + message(FATAL_ERROR "Cannot find install manifest: @CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt") +endif(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt") + +file(READ "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt" files) +string(REGEX REPLACE "\n" ";" files "${files}") +foreach(file ${files}) + message(STATUS "Uninstalling $ENV{DESTDIR}${file}") + if(IS_SYMLINK "$ENV{DESTDIR}${file}" OR EXISTS "$ENV{DESTDIR}${file}") + exec_program( + "@CMAKE_COMMAND@" ARGS "-E remove \"$ENV{DESTDIR}${file}\"" + OUTPUT_VARIABLE rm_out + RETURN_VALUE rm_retval + ) + if(NOT "${rm_retval}" STREQUAL 0) + message(FATAL_ERROR "Problem when removing $ENV{DESTDIR}${file}") + endif(NOT "${rm_retval}" STREQUAL 0) + else(IS_SYMLINK "$ENV{DESTDIR}${file}" OR EXISTS "$ENV{DESTDIR}${file}") + message(STATUS "File $ENV{DESTDIR}${file} does not exist.") + endif(IS_SYMLINK "$ENV{DESTDIR}${file}" OR EXISTS "$ENV{DESTDIR}${file}") +endforeach(file) + + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/cmake/rs_driverConfig.cmake b/src/airy/rslidar_sdk-main/src/rs_driver/cmake/rs_driverConfig.cmake new file mode 100644 index 0000000..04a58e5 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/cmake/rs_driverConfig.cmake @@ -0,0 +1,29 @@ +# - Config file for the package +# It defines the following variables +# rs_driver_INCLUDE_DIRS - include directories for +# rs_driver_LIBRARIES - libraries to link against +# rs_driver_FOUND - found flag + +if(WIN32) + if(CMAKE_SIZEOF_VOID_P EQUAL 8) # 64-bit + set(Boost_ARCHITECTURE "-x64") + elseif(CMAKE_SIZEOF_VOID_P EQUAL 4) # 32-bit + set(Boost_ARCHITECTURE "-x32") + endif() + set(Boost_USE_STATIC_LIBS ON) + set(Boost_USE_MULTITHREADED ON) + set(Boost_USE_STATIC_RUNTIME OFF) +endif(WIN32) + +if(${ENABLE_TRANSFORM}) + add_definitions("-DENABLE_TRANSFORM") +endif(${ENABLE_TRANSFORM}) + +set(rs_driver_INCLUDE_DIRS "/home/alvin/project/zxwl/sweeper/200_whu/sweeper_whu/src/airy/rslidar_sdk-main/src/rs_driver/src;/usr/local/rslidar_sdk/include") +set(RS_DRIVER_INCLUDE_DIRS "/home/alvin/project/zxwl/sweeper/200_whu/sweeper_whu/src/airy/rslidar_sdk-main/src/rs_driver/src;/usr/local/rslidar_sdk/include") + +set(rs_driver_LIBRARIES "pthread;pcap") +set(RS_DRIVER_LIBRARIES "pthread;pcap") + +set(rs_driver_FOUND true) +set(RS_DRIVER_FOUND true) diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/cmake/rs_driverConfig.cmake.in b/src/airy/rslidar_sdk-main/src/rs_driver/cmake/rs_driverConfig.cmake.in new file mode 100644 index 0000000..078057c --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/cmake/rs_driverConfig.cmake.in @@ -0,0 +1,29 @@ +# - Config file for the @PROJECT_NAME_LOWER@ package +# It defines the following variables +# rs_driver_INCLUDE_DIRS - include directories for @PROJECT_NAME_LOWER@ +# rs_driver_LIBRARIES - libraries to link against +# rs_driver_FOUND - found flag + +if(WIN32) + if(CMAKE_SIZEOF_VOID_P EQUAL 8) # 64-bit + set(Boost_ARCHITECTURE "-x64") + elseif(CMAKE_SIZEOF_VOID_P EQUAL 4) # 32-bit + set(Boost_ARCHITECTURE "-x32") + endif() + set(Boost_USE_STATIC_LIBS ON) + set(Boost_USE_MULTITHREADED ON) + set(Boost_USE_STATIC_RUNTIME OFF) +endif(WIN32) + +if(${ENABLE_TRANSFORM}) + add_definitions("-DENABLE_TRANSFORM") +endif(${ENABLE_TRANSFORM}) + +set(rs_driver_INCLUDE_DIRS "@DRIVER_INCLUDE_DIRS@;@INSTALL_DRIVER_DIR@") +set(RS_DRIVER_INCLUDE_DIRS "@DRIVER_INCLUDE_DIRS@;@INSTALL_DRIVER_DIR@") + +set(rs_driver_LIBRARIES "@EXTERNAL_LIBS@") +set(RS_DRIVER_LIBRARIES "@EXTERNAL_LIBS@") + +set(rs_driver_FOUND true) +set(RS_DRIVER_FOUND true) diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/cmake/rs_driverConfigVersion.cmake b/src/airy/rslidar_sdk-main/src/rs_driver/cmake/rs_driverConfigVersion.cmake new file mode 100644 index 0000000..abaef6a --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/cmake/rs_driverConfigVersion.cmake @@ -0,0 +1,14 @@ +set (PACKAGE_VERSION "1.5.17") +message(=============================================================) +message("-- rs_driver Version : v${PACKAGE_VERSION}") +message(=============================================================) + +# Check whether the requested PACKAGE_FIND_VERSION is compatible +if ("${PACKAGE_VERSION}" VERSION_LESS "${PACKAGE_FIND_VERSION}") + set (PACKAGE_VERSION_COMPATIBLE FALSE) +else () + set (PACKAGE_VERSION_COMPATIBLE TRUE) + if ("${PACKAGE_VERSION}" VERSION_EQUAL "${PACKAGE_FIND_VERSION}") + set (PACKAGE_VERSION_EXACT TRUE) + endif () +endif () diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/cmake/rs_driverConfigVersion.cmake.in b/src/airy/rslidar_sdk-main/src/rs_driver/cmake/rs_driverConfigVersion.cmake.in new file mode 100644 index 0000000..4c2ff44 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/cmake/rs_driverConfigVersion.cmake.in @@ -0,0 +1,14 @@ +set (PACKAGE_VERSION "@rs_driver_VERSION@") +message(=============================================================) +message("-- rs_driver Version : v${PACKAGE_VERSION}") +message(=============================================================) + +# Check whether the requested PACKAGE_FIND_VERSION is compatible +if ("${PACKAGE_VERSION}" VERSION_LESS "${PACKAGE_FIND_VERSION}") + set (PACKAGE_VERSION_COMPATIBLE FALSE) +else () + set (PACKAGE_VERSION_COMPATIBLE TRUE) + if ("${PACKAGE_VERSION}" VERSION_EQUAL "${PACKAGE_FIND_VERSION}") + set (PACKAGE_VERSION_EXACT TRUE) + endif () +endif () \ No newline at end of file diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/demo/CMakeLists.txt b/src/airy/rslidar_sdk-main/src/rs_driver/demo/CMakeLists.txt new file mode 100644 index 0000000..6420c2c --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/demo/CMakeLists.txt @@ -0,0 +1,42 @@ +cmake_minimum_required(VERSION 3.5) + +project(rs_driver_demos) + +message(=============================================================) +message("-- Ready to compile demos") +message(=============================================================) + +if (${ENABLE_PCL_POINTCLOUD}) + +find_package(PCL REQUIRED) +include_directories(${PCL_INCLUDE_DIRS}) +link_directories(${PCL_LIBRARY_DIRS}) +add_definitions(${PCL_DEFINITIONS}) + +endif (${ENABLE_PCL_POINTCLOUD}) + +include_directories(${DRIVER_INCLUDE_DIRS}) + +add_executable(demo_online + demo_online.cpp) + +target_link_libraries(demo_online + ${EXTERNAL_LIBS}) + +add_executable(demo_online_multi_lidars + demo_online_multi_lidars.cpp) + +target_link_libraries(demo_online_multi_lidars + ${EXTERNAL_LIBS}) + +if(NOT ${DISABLE_PCAP_PARSE}) + +add_executable(demo_pcap + demo_pcap.cpp) + +target_link_libraries(demo_pcap + ${EXTERNAL_LIBS}) + +endif(NOT ${DISABLE_PCAP_PARSE}) + + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/demo/demo_online.cpp b/src/airy/rslidar_sdk-main/src/rs_driver/demo/demo_online.cpp new file mode 100644 index 0000000..98a5934 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/demo/demo_online.cpp @@ -0,0 +1,237 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#include + +#ifdef ENABLE_PCL_POINTCLOUD +#include +#else +#include +#endif + +//#define ORDERLY_EXIT + +// Define the macro: 1 to enable IMU parsing, 0 to disable IMU parsing +#define ENABLE_IMU_PARSE 1 + +typedef PointXYZI PointT; +typedef PointCloudT PointCloudMsg; + +using namespace robosense::lidar; + +SyncQueue> free_cloud_queue; +SyncQueue> stuffed_cloud_queue; + +SyncQueue> free_imu_data_queue; +SyncQueue> stuffed_imu_data_queue; + +// +// @brief point cloud callback function. The caller should register it to the lidar driver. +// Via this fucntion, the driver gets an free/unused point cloud message from the caller. +// @param msg The free/unused point cloud message. +// +std::shared_ptr driverGetPointCloudFromCallerCallback(void) +{ + // Note: This callback function runs in the packet-parsing/point-cloud-constructing thread of the driver, + // so please DO NOT do time-consuming task here. + std::shared_ptr msg = free_cloud_queue.pop(); + if (msg.get() != NULL) + { + return msg; + } + + return std::make_shared(); +} + +// +// @brief point cloud callback function. The caller should register it to the lidar driver. +// Via this function, the driver gets/returns a stuffed point cloud message to the caller. +// @param msg The stuffed point cloud message. +// +void driverReturnPointCloudToCallerCallback(std::shared_ptr msg) +{ + // Note: This callback function runs in the packet-parsing/point-cloud-constructing thread of the driver, + // so please DO NOT do time-consuming task here. Instead, process it in caller's own thread. (see processCloud() below) + stuffed_cloud_queue.push(msg); +} + +// +// @brief IMU data callback function. The caller should register it to the lidar driver. +// Via this function, the driver gets/returns a stuffed IMU data message to the caller. +// @param msg The stuffed IMU data message. +// +std::shared_ptr driverGetIMUDataFromCallerCallback(void) +{ + // Note: This callback function runs in the packet-parsing/imu-data-constructing thread of the driver, + // so please DO NOT do time-consuming task here. + std::shared_ptr msg = free_imu_data_queue.pop(); + if (msg.get() != NULL) + { + return msg; + } + + return std::make_shared(); +} + +// +// @brief IMU data callback function. The caller should register it to the lidar driver. +// Via this function, the driver gets/returns a stuffed IMU data message to the caller. +// @param msg The stuffed IMU data message. +// +void driverReturnImuDataToCallerCallback(const std::shared_ptr& msg) +{ + // Note: This callback function runs in the packet-parsing/imu-data-constructing thread of the driver, + // so please DO NOT do time-consuming task here. Instead, process it in caller's own thread. (see processImuData() below) + stuffed_imu_data_queue.push(msg); +} + + +bool to_exit_process = false; +void processImuData(void) +{ + uint32_t imu_cnt = 0; + while (!to_exit_process) + { + std::shared_ptr msg = stuffed_imu_data_queue.popWait(); + if (msg.get() == NULL) + { + continue; + } + + // Well, it is time to process the IMU data msg, even it is time-consuming. + + imu_cnt++; +#if 0 + RS_MSG << "msg: " << imu_cnt << " imu data ts: " <timestamp) << RS_REND; + RS_DEBUG <<"imu data: " << " , linear_a_x" << msg->linear_acceleration_x + << " , linear_a_y " << msg->linear_acceleration_y << " , linear_a_z" << msg->linear_acceleration_z + << " , angular_v_x " << msg->angular_velocity_x << " , angular_v_y" << msg->angular_velocity_y + << " , angular_v_z" <angular_velocity_z << RS_REND; +#endif + + free_imu_data_queue.push(msg); + } + +} +// +// @brief exception callback function. The caller should register it to the lidar driver. +// Via this function, the driver inform the caller that something happens. +// @param code The error code to represent the error/warning/information +// +void exceptionCallback(const Error& code) +{ + // Note: This callback function runs in the packet-receving and packet-parsing/point-cloud_constructing thread of the driver, + // so please DO NOT do time-consuming task here. + RS_WARNING << code.toString() << RS_REND; +} + + +void processCloud(void) +{ + while (!to_exit_process) + { + std::shared_ptr msg = stuffed_cloud_queue.popWait(); + if (msg.get() == NULL) + { + continue; + } + + // Well, it is time to process the point cloud msg, even it is time-consuming. + RS_MSG << "msg: " << msg->seq << " point cloud size: " << msg->points.size() << RS_REND; + +#if 0 + for (auto it = msg->points.begin(); it != msg->points.end(); it++) + { + std::cout << std::fixed << std::setprecision(3) + << "(" << it->x << ", " << it->y << ", " << it->z << ", " << (int)it->intensity << ")" + << std::endl; + } +#endif + + free_cloud_queue.push(msg); + + + } +} + +int main(int argc, char* argv[]) +{ + RS_TITLE << "------------------------------------------------------" << RS_REND; + RS_TITLE << " RS_Driver Core Version: v" << getDriverVersion() << RS_REND; + RS_TITLE << "------------------------------------------------------" << RS_REND; + + RSDriverParam param; ///< Create a parameter object + param.input_type = InputType::ONLINE_LIDAR; + param.input_param.msop_port = 6699; ///< Set the lidar msop port number, the default is 6699 + param.input_param.difop_port = 7788; ///< Set the lidar difop port number, the default is 7788 +#if ENABLE_IMU_PARSE + param.input_param.imu_port = 6688; ///< Set the lidar imu port number, the default is 0 +#endif + param.lidar_type = LidarType::RSAIRY; ///< Set the lidar type. Make sure this type is correct + param.print(); + + LidarDriver driver; ///< Declare the driver object + driver.regPointCloudCallback(driverGetPointCloudFromCallerCallback, driverReturnPointCloudToCallerCallback); ///< Register the point cloud callback functions + driver.regExceptionCallback(exceptionCallback); ///< Register the exception callback function +#if ENABLE_IMU_PARSE + driver.regImuDataCallback(driverGetIMUDataFromCallerCallback, driverReturnImuDataToCallerCallback); +#endif + + if (!driver.init(param)) ///< Call the init function + { + RS_ERROR << "Driver Initialize Error..." << RS_REND; + return -1; + } + + std::thread cloud_handle_thread = std::thread(processCloud); +#if ENABLE_IMU_PARSE + std::thread imuData_handle_thread = std::thread(processImuData); +#endif + driver.start(); ///< The driver thread will start + RS_DEBUG << "RoboSense Lidar-Driver Linux online demo start......" << RS_REND; + +#ifdef ORDERLY_EXIT + std::this_thread::sleep_for(std::chrono::seconds(10)); + + driver.stop(); + + to_exit_process = true; + cloud_handle_thread.join(); +#else + while (true) + { + std::this_thread::sleep_for(std::chrono::seconds(1)); + } +#endif + + return 0; +} diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/demo/demo_online_multi_lidars.cpp b/src/airy/rslidar_sdk-main/src/rs_driver/demo/demo_online_multi_lidars.cpp new file mode 100644 index 0000000..c8c07fd --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/demo/demo_online_multi_lidars.cpp @@ -0,0 +1,190 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#include + +#ifdef ENABLE_PCL_POINTCLOUD +#include +#else +#include +#endif + +//#define ORDERLY_EXIT + +typedef PointXYZI PointT; +typedef PointCloudT PointCloudMsg; + +using namespace robosense::lidar; + +class DriverClient +{ +public: + + DriverClient(const std::string name) + : name_(name) + { + } + + bool init(const RSDriverParam& param) + { + RS_INFO << "------------------------------------------------------" << RS_REND; + RS_INFO << " " << name_ << RS_REND; + RS_INFO << "------------------------------------------------------" << RS_REND; + param.print(); + + driver_.regPointCloudCallback (std::bind(&DriverClient::driverGetPointCloudFromCallerCallback, this), + std::bind(&DriverClient::driverReturnPointCloudToCallerCallback, this, std::placeholders::_1)); + driver_.regExceptionCallback (std::bind(&DriverClient::exceptionCallback, this, std::placeholders::_1)); + + if (!driver_.init(param)) + { + RS_ERROR << name_ << ": Failed to initialize driver." << RS_REND; + return false; + } + + return true; + } + + bool start() + { + to_exit_process_ = false; + cloud_handle_thread_ = std::thread(std::bind(&DriverClient::processCloud, this)); + + driver_.start(); + RS_DEBUG << name_ << ": Started driver." << RS_REND; + + return true; + } + + void stop() + { + driver_.stop(); + + to_exit_process_ = true; + cloud_handle_thread_.join(); + } + +protected: + + std::shared_ptr driverGetPointCloudFromCallerCallback(void) + { + std::shared_ptr msg = free_cloud_queue_.pop(); + if (msg.get() != NULL) + { + return msg; + } + + return std::make_shared(); + } + + void driverReturnPointCloudToCallerCallback(std::shared_ptr msg) + { + stuffed_cloud_queue_.push(msg); + } + + void processCloud(void) + { + while (!to_exit_process_) + { + std::shared_ptr msg = stuffed_cloud_queue_.popWait(); + if (msg.get() == NULL) + { + continue; + } + + RS_MSG << name_ << ": msg: " << msg->seq << " point cloud size: " << msg->points.size() << RS_REND; + + free_cloud_queue_.push(msg); + } + } + + void exceptionCallback(const Error& code) + { + RS_WARNING << name_ << ": " << code.toString() << RS_REND; + } + + std::string name_; + LidarDriver driver_; + bool to_exit_process_; + std::thread cloud_handle_thread_; + SyncQueue> free_cloud_queue_; + SyncQueue> stuffed_cloud_queue_; +}; + +int main(int argc, char* argv[]) +{ + RS_TITLE << "------------------------------------------------------" << RS_REND; + RS_TITLE << " RS_Driver Core Version: v" << getDriverVersion() << RS_REND; + RS_TITLE << "------------------------------------------------------" << RS_REND; + + RSDriverParam param_left; ///< Create a parameter object + param_left.input_type = InputType::ONLINE_LIDAR; + param_left.input_param.msop_port = 6004; ///< Set the lidar msop port number, the default is 6699 + param_left.input_param.difop_port = 7004; ///< Set the lidar difop port number, the default is 7788 + param_left.lidar_type = LidarType::RSM1; ///< Set the lidar type. Make sure this type is correct + + DriverClient client_left("LEFT "); + if (!client_left.init(param_left)) ///< Call the init function + { + return -1; + } + + RSDriverParam param_right; ///< Create a parameter object + param_right.input_type = InputType::ONLINE_LIDAR; + param_right.input_param.msop_port = 6005; ///< Set the lidar msop port number, the default is 6699 + param_right.input_param.difop_port = 7005; ///< Set the lidar difop port number, the default is 7788 + param_right.lidar_type = LidarType::RSM1; ///< Set the lidar type. Make sure this type is correct + + DriverClient client_right("RIGHT"); + if (!client_right.init(param_right)) ///< Call the init function + { + return -1; + } + + client_left.start(); + client_right.start(); + +#ifdef ORDERLY_EXIT + std::this_thread::sleep_for(std::chrono::seconds(10)); + + client_left.stop(); + client_right.stop(); +#else + while (true) + { + std::this_thread::sleep_for(std::chrono::seconds(1)); + } +#endif + + return 0; +} + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/demo/demo_pcap.cpp b/src/airy/rslidar_sdk-main/src/rs_driver/demo/demo_pcap.cpp new file mode 100644 index 0000000..e1fc56c --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/demo/demo_pcap.cpp @@ -0,0 +1,239 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#include + +#ifdef ENABLE_PCL_POINTCLOUD +#include +#else +#include +#endif + +//#define ORDERLY_EXIT + +// Define the macro: 1 to enable IMU parsing, 0 to disable IMU parsing +#define ENABLE_IMU_PARSE 1 +typedef PointXYZI PointT; +typedef PointCloudT PointCloudMsg; + +using namespace robosense::lidar; + +SyncQueue> free_cloud_queue; +SyncQueue> stuffed_cloud_queue; + + +SyncQueue> free_imu_data_queue; +SyncQueue> stuffed_imu_data_queue; +// +// @brief point cloud callback function. The caller should register it to the lidar driver. +// Via this fucntion, the driver gets an free/unused point cloud message from the caller. +// @param msg The free/unused point cloud message. +// +std::shared_ptr driverGetPointCloudFromCallerCallback(void) +{ + // Note: This callback function runs in the packet-parsing/point-cloud-constructing thread of the driver, + // so please DO NOT do time-consuming task here. + std::shared_ptr msg = free_cloud_queue.pop(); + if (msg.get() != NULL) + { + return msg; + } + + return std::make_shared(); +} + +// +// @brief point cloud callback function. The caller should register it to the lidar driver. +// Via this function, the driver gets/returns a stuffed point cloud message to the caller. +// @param msg The stuffed point cloud message. +// +void driverReturnPointCloudToCallerCallback(std::shared_ptr msg) +{ + // Note: This callback function runs in the packet-parsing/point-cloud-constructing thread of the driver, + // so please DO NOT do time-consuming task here. Instead, process it in caller's own thread. (see processCloud() below) + stuffed_cloud_queue.push(msg); +} + +// +// @brief IMU data callback function. The caller should register it to the lidar driver. +// Via this function, the driver gets/returns a stuffed IMU data message to the caller. +// @param msg The stuffed IMU data message. +// +std::shared_ptr driverGetIMUDataFromCallerCallback(void) +{ + // Note: This callback function runs in the packet-parsing/imu-data-constructing thread of the driver, + // so please DO NOT do time-consuming task here. + std::shared_ptr msg = free_imu_data_queue.pop(); + if (msg.get() != NULL) + { + return msg; + } + + return std::make_shared(); +} + +// +// @brief IMU data callback function. The caller should register it to the lidar driver. +// Via this function, the driver gets/returns a stuffed IMU data message to the caller. +// @param msg The stuffed IMU data message. +// +void driverReturnImuDataToCallerCallback(const std::shared_ptr& msg) +{ + // Note: This callback function runs in the packet-parsing/imu-data-constructing thread of the driver, + // so please DO NOT do time-consuming task here. Instead, process it in caller's own thread. (see processImuData() below) + stuffed_imu_data_queue.push(msg); +} + + +bool to_exit_process = false; +void processImuData(void) +{ + uint32_t imu_cnt = 0; + while (!to_exit_process) + { + std::shared_ptr msg = stuffed_imu_data_queue.popWait(); + if (msg.get() == NULL) + { + continue; + } + + // Well, it is time to process the IMU data msg, even it is time-consuming. + + imu_cnt++; +#if 0 + RS_MSG << "msg: " << imu_cnt << " imu data ts: " <timestamp) << RS_REND; + RS_DEBUG <<"imu data: " << " , linear_a_x" << msg->linear_acceleration_x + << " , linear_a_y " << msg->linear_acceleration_y << " , linear_a_z" << msg->linear_acceleration_z + << " , angular_v_x " << msg->angular_velocity_x << " , angular_v_y" << msg->angular_velocity_y + << " , angular_v_z" <angular_velocity_z << RS_REND; +#endif + + free_imu_data_queue.push(msg); + } + +} + +// +// @brief exception callback function. The caller should register it to the lidar driver. +// Via this function, the driver inform the caller that something happens. +// @param code The error code to represent the error/warning/information +// +void exceptionCallback(const Error& code) +{ + // Note: This callback function runs in the packet-receving and packet-parsing/point-cloud_constructing thread of the driver, + // so please DO NOT do time-consuming task here. + RS_WARNING << code.toString() << RS_REND; +} + +void processCloud(void) +{ + while (!to_exit_process) + { + std::shared_ptr msg = stuffed_cloud_queue.popWait(); + if (msg.get() == NULL) + { + continue; + } + + // Well, it is time to process the point cloud msg, even it is time-consuming. + RS_MSG << "msg: " << msg->seq << " point cloud size: " << msg->points.size() << RS_REND; + +#if 0 + for (auto it = msg->points.begin(); it != msg->points.end(); it++) + { + std::cout << std::fixed << std::setprecision(3) + << "(" << it->x << ", " << it->y << ", " << it->z << ", " << (int)it->intensity << ")" + << std::endl; + } +#endif + + free_cloud_queue.push(msg); + } +} + +int main(int argc, char* argv[]) +{ + RS_TITLE << "------------------------------------------------------" << RS_REND; + RS_TITLE << " RS_Driver Core Version: v" << getDriverVersion() << RS_REND; + RS_TITLE << "------------------------------------------------------" << RS_REND; + + RSDriverParam param; ///< Create a parameter object + param.input_type = InputType::PCAP_FILE; + param.input_param.pcap_path = "/home/robosense/lidar.pcap"; ///< Set the pcap file directory + param.input_param.msop_port = 6699; ///< Set the lidar msop port number, the default is 6699 + param.input_param.difop_port = 7788; ///< Set the lidar difop port number, the default is 7788 +#if ENABLE_IMU_PARSE + param.input_param.imu_port = 6688; ///< Set the lidar imu port number, the default is 0 +#endif + param.lidar_type = LidarType::RSAIRY; ///< Set the lidar type. Make sure this type is correct + param.input_param.pcap_rate = 1.0; + param.print(); + + LidarDriver driver; ///< Declare the driver object + driver.regPointCloudCallback(driverGetPointCloudFromCallerCallback, driverReturnPointCloudToCallerCallback); ///< Register the point cloud callback functions + driver.regExceptionCallback(exceptionCallback); ///< Register the exception callback function +#if ENABLE_IMU_PARSE + driver.regImuDataCallback(driverGetIMUDataFromCallerCallback, driverReturnImuDataToCallerCallback); +#endif + if (!driver.init(param)) ///< Call the init function + { + RS_ERROR << "Driver Initialize Error..." << RS_REND; + return -1; + } + + std::thread cloud_handle_thread = std::thread(processCloud); + +#if ENABLE_IMU_PARSE + std::thread imuData_handle_thread = std::thread(processImuData); +#endif + + driver.start(); ///< The driver thread will start + + RS_DEBUG << "RoboSense Lidar-Driver Linux pcap demo start......" << RS_REND; + + +#ifdef ORDERLY_EXIT + std::this_thread::sleep_for(std::chrono::seconds(10)); + + driver.stop(); + + to_exit_process = true; + cloud_handle_thread.join(); +#else + while (true) + { + std::this_thread::sleep_for(std::chrono::seconds(1)); + } +#endif + + return 0; +} diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/07_how_to_port_your_app_from_rs_driver_1.3.x_to_1.5.x.md b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/07_how_to_port_your_app_from_rs_driver_1.3.x_to_1.5.x.md new file mode 100644 index 0000000..0fda27e --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/07_how_to_port_your_app_from_rs_driver_1.3.x_to_1.5.x.md @@ -0,0 +1,236 @@ +# 7 How to port your app from rs_driver v1.3.x to v1.5.x + + + +## 7.1 Branches and Versions + +Now `rs_driver` have the following braches. ++ `dev`. The development branch of `v1.3.x`. The latest version is `v1.3.2`. It is no longer developed. ++ `dev_opt`. The development branch of `v1.5.x`. It is refactoried from `v1.3.x`. ++ `main`. stable branch. It tracked `dev` before, and tracked `dev_opt` now. + +Generally, the modifications in `dev_opt` will be merged into `main` monthly. At the same time, a new version is created. + +Occasionally, to solve a emegency problem, hotfix patches will be commited and merged into `dev_opt` and `main`. + + + +## 7.2 What enhancement ? + +The main advantage of `v1.5.x` is to reduce CPU usage obviously. + +On a platfrom, CPU usage of `dev` is as below. +![](./img/07_02_cpu_usage_dev.png) + +CPU usage of `dev_opt` is as below. +![](./img/07_03_cpu_usage_dev_opt.png) + + + +Other advantages are as below. + ++ Remove the dependency on the Boost library, help to port `rs_driver` to embedded Linux. ++ Add compile options to solve packet-loss problems on some platforms. ++ Refactory `Input` and `Decoder`, to support different use cases. ++ For mechanical LiDARs, use Block as splitting unit instead of Packet, to avoid flash of point cloud . ++ Improve the help documents, including [understand source code of rs_driver](../src_intro/rs_driver_intro_CN.md), to help user understand it. + + + +## 7.3 How to port ? + +The interface of rs_driver contains two parts: RSDriverParam and LidarDriver. + +### 7.3.1 RSDriverParam + +#### 7.3.1.1 RSDriverParam + +RSDriverParam contains the options to change the rs_driver's behavior. + +RSDriverParam is as below in rs_driver `1.3.x`. + +```c++ +typedef struct RSDriverParam +{ + RSInputParam input_param; ///< Input parameter + RSDecoderParam decoder_param; ///< Decoder parameter + std::string angle_path = "null"; ///< Path of angle calibration files(angle.csv).Only used for internal debugging. + std::string frame_id = "rslidar"; ///< The frame id of LiDAR message + LidarType lidar_type = LidarType::RS16; ///< Lidar type + bool wait_for_difop = true; ///< true: start sending point cloud until receive difop packet + bool saved_by_rows = false; ///< true: the output point cloud will be saved by rows (default is saved by columns) +}; +``` + +And it is as below in rs_driver `1.5.x`. + +```c++ +typedef struct RSDriverParam +{ + LidarType lidar_type = LidarType::RS16; ///< Lidar type + InputType input_type = InputType::ONLINE_LIDAR; ///< Input type + RSInputParam input_param; ///< Input parameter + RSDecoderParam decoder_param; ///< Decoder parameter +}; +``` + +Below are the changes. ++ A new member `input_type` is added. It means the type of data sources: + + `ONLINE_LIDAR` + + `PCAP_FILE` + + `RAW_PACKET`. User's data captured via `rs_driver`. In `v1.3.x`, a group of functions are for it, and in `v1.5.x`, it is uniformed to `input_type`. + ++ The member `wait_for_difop` specifies whether to handle MSOP packets before handling DIFOP packet. For mechenical LiDARs, the point cloud will be flat without the angle data in DIFOP packet。This option is about decoding, so it is shifted to RSDecoderParam. ++ The member `saved_by_rows` specifies to give point cloud row by row. There are two reasons to remove it: + + Even point cloud is given column by column, it is easier to access row by row (by skipping column). + + The option consumes two much CPU resources, so not suggested. + + As a exception on ROS, `rslidar_sdk` can give point cloud row by row with the option `ros_send_by_rows=true`. This is done while copying point cloud, so as not to cost more efforts. ++ The member `angle_path` specifies a file which contains the angle data. It is about data source, so it is shifted to RSInputParam. ++ The member `frame_id` is a concept from ROS. It is removed, and the driver `rslidar_sdk` (for ROS) will handle it. + +#### 7.3.1.2 RSInputParam + +RSInputParam is as below in rs_driver `1.3.x`. + +```c++ +typedef struct RSInputParam ///< The LiDAR input parameter +{ + std::string device_ip = "192.168.1.200"; ///< Ip of LiDAR + std::string multi_cast_address = "0.0.0.0"; ///< Address of multicast + std::string host_address = "0.0.0.0"; ///< Address of host + uint16_t msop_port = 6699; ///< Msop packet port number + uint16_t difop_port = 7788; ///< Difop packet port number + bool read_pcap = false; ///< true: The driver will process the pcap through pcap_path. false: The driver will + ///< Get data from online LiDAR + double pcap_rate = 1; ///< Rate to read the pcap file + bool pcap_repeat = true; ///< true: The pcap bag will repeat play + std::string pcap_path = "null"; ///< Absolute path of pcap file + bool use_vlan = false; ///< Vlan on-off + bool use_someip = false; ///< Someip on-off + bool use_custom_proto = false; ///< Customer Protocol on-off +}； +``` + +And it is as below in rs_driver `1.5.x`. + +```c++ +typedef struct RSInputParam ///< The LiDAR input parameter +{ + uint16_t msop_port = 6699; ///< Msop packet port number + uint16_t difop_port = 7788; ///< Difop packet port number + std::string host_address = "0.0.0.0"; ///< Address of host + std::string group_address = "0.0.0.0"; ///< Address of multicast group + std::string pcap_path = ""; ///< Absolute path of pcap file + bool pcap_repeat = true; ///< true: The pcap bag will repeat play + float pcap_rate = 1.0f; ///< Rate to read the pcap file + bool use_vlan = false; ///< Vlan on-off + uint16_t user_layer_bytes = 0; ///< Bytes of user layer. thers is no user layer if it is 0 + uint16_t tail_layer_bytes = 0; ///< Bytes of tail layer. thers is no tail layer if it is 0 + uint32_t socket_recv_buf = 106496; // +class LidarDriver +{ +public: + + inline void regRecvCallback(const std::function&)>& callback) + { + driver_ptr_->regRecvCallback(callback); + } + ... +}; +``` + +And it is as below in rs_driver 1.5.x. + +```c++ +template +class LidarDriver +{ +public: + + inline void regPointCloudCallback(const std::function(void)>& cb_get_cloud, + const std::function)>& cb_put_cloud) + { + driver_ptr_->regPointCloudCallback(cb_get_cloud, cb_put_cloud); + } + ... +}; +``` + +Below are the changes. ++ The template parameter is changed from typename `PointT` to typename `T_PointCloud`. ++ The function to get point cloud is now requested to have two callbacks instead of one. + + `cb_get_cloud` is to get free point cloud instance from caller. `cb_put_cloud` is to return stuffed point cloud to caller. + + +For details of these two changes, please refer to [Decode online LiDAR](./08_how_to_decode_online_lidar.md) and [Thread Mode and Interface](../intro/03_thread_model.md). + + + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/07_how_to_port_your_app_from_rs_driver_1.3.x_to_1.5.x_CN.md b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/07_how_to_port_your_app_from_rs_driver_1.3.x_to_1.5.x_CN.md new file mode 100644 index 0000000..50792b6 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/07_how_to_port_your_app_from_rs_driver_1.3.x_to_1.5.x_CN.md @@ -0,0 +1,256 @@ +# 7 **如何从rs_driver v1.3.x升级到v1.5.x** + + + +## 7.1 目前的分支与版本 + +目前github上的[rs_driver](https://github.com/RoboSense-LiDAR/rs_driver)工程主要有如下分支。 + ++ `dev`分支： `v1.3.x`的开发分支，最后版本是`v1.3.2`。这个分支已经停止开发。 ++ `dev_opt`分支： 在`v1.3.x`基础上重构的`v1.5.x`的开发分支。 ++ `main`分支： 稳定分支。以前跟踪`dev`分支，现在跟踪`dev_opt`分支。 +![](./img/07_01_branches.png) + +一般情况下，`dev_opt`分支中的修改，在基本稳定后，会合并到`main`分支中，并建立新的版本号。 + +特别地，对一些影响较大的问题，会推出紧急修复补丁，这个补丁会同时合并到`dev_opt`分支和`main`分支中。 + + + +## 7.2 v1.5.x相对于v1.3.x的改进 + +相对于`v1.3.x`，rs_driver `v1.5.x`的主要改进是明显减少CPU占用率。 + +如下是在某平台上针对`dev`分支和`dev_opt`分支进行对比测试的结果。 + +`dev`分支的cpu占用率如下图。 +![](./img/07_02_cpu_usage_dev.png) + +`dev_opt`分支的cpu占用率如下图。 +![](./img/07_03_cpu_usage_dev_opt.png) + +`dev_opt`分支的CPU占用率比`dev`分支降低了约`5%`，降低的比例为`16.7%`。 + + + +其他的改进还包括： + ++ 去除对Boost库的依赖，更容易移植到一些嵌入式Linux平台上 + ++ 增加可选的编译选项，解决在某些平台上丢包的问题 + ++ 重构网络部分和解码部分的代码，完整支持不同场景下的配置 + ++ 对于机械式雷达，将分帧的粒度从Packet细化为Block，避免分帧角度附近点云闪烁的问题 + ++ 完善帮助文档，尤其是提供了[rs_driver源代码解析](../src_intro/rs_driver_intro_CN.md)文档，帮助用户深入理解驱动的实现，以方便功能裁剪。 + +对于新客户，推荐使用`v1.5.x`。对于老客户，如果有以上问题的困扰，也推荐升级到`v1.5.x`。 + + + +## 7.3 如何升级？ + +相对于`v1.3.x`， `v1.5.x`调整了接口的定义，也调整了部分配置选项的名字和位置。对这些调整的说明如下。 + +`rs_driver`的接口包括了两个部分：`RSDriverParam`和`LidarDriver`。 + +### 7.3.1 RSDriverParam + +#### 7.3.1.1 RSDriverParam + +RSDriverParam包括一些配置选项，这些选项可以改变`rs_driver`的行为。 + +在`v1.3.x`中，RSDriverParam定义如下。 + +```c++ +typedef struct RSDriverParam +{ + RSInputParam input_param; ///< Input parameter + RSDecoderParam decoder_param; ///< Decoder parameter + std::string angle_path = "null"; ///< Path of angle calibration files(angle.csv).Only used for internal debugging. + std::string frame_id = "rslidar"; ///< The frame id of LiDAR message + LidarType lidar_type = LidarType::RS16; ///< Lidar type + bool wait_for_difop = true; ///< true: start sending point cloud until receive difop packet + bool saved_by_rows = false; ///< true: the output point cloud will be saved by rows (default is saved by columns) +}; +``` + +在`v1.5.x`中，RSDriverParam定义如下。 + +```c++ +typedef struct RSDriverParam +{ + LidarType lidar_type = LidarType::RS16; ///< Lidar type + InputType input_type = InputType::ONLINE_LIDAR; ///< Input type + RSInputParam input_param; ///< Input parameter + RSDecoderParam decoder_param; ///< Decoder parameter +}; +``` + +变动如下： ++ 加入新成员`input_type`。这个成员指定数据源的类型： + + `ONLINE_LIDAR` 在线雷达, + + `PCAP_FILE` PCAP文件 + + `RAW_PACKET` 用户自己保存的MSOP/DIFOP Packet数据 + +指定了哪种类型，就从哪类数据源获得数据。用户自己的数据在`v1.3.x`中有一组专门的函数处理，在`v1.5.x`中则统一到数据源类型`RAW_PACKET`。 + ++ 成员`wait_for_difop` 指定在处理MSOP包之前，是否先等待DIFOP包。对于机械式雷达，如果没有DIFOP包中的垂直角信息，点云就是扁平的。这个选项与解码相关，所以把它移到RSDecoderParam中。 + ++ 删除成员`saved_by_rows`。雷达每轮扫描得到一组点（每个通道一个点），依次保存在点云的vector成员中，这是`按列保存`。在`v1.3.x`中，成员`saved_by_rows` 指定`按行保存`，也就是每次保存一个通道上的所有点。删除这个成员有两个原因： + + 在`按列保存`的vector中，按照行的顺序检索点，也是容易的。跳过通道数就可以了。 + + `v1.3.x`实现`saved_by_rows`的方式是将点云重排一遍，这样复制点云的代码太大。 + + 如果您的代码依赖`按行保存`这个特性，建议按`跳过通道数`访问达到同样的效果。 + + 作为一个特例，如果您的代码基于ROS，那么适配ROS的驱动`rslidar_sdk`有个选项`ros_send_by_rows`，也可以做到`按行保存`。`rslidar_sdk`本来就需要将`rs_driver`点云转换到ROS点云，行列变换同时进行，不会带来额外的复制成本。 + ++ 成员`angle_path`指定`垂直角/水平角的修正值`从指定的文件读取，而不是解析DIFOP包得到。这个选项是关于数据源的，所以移到RSInputParam。 + ++ 成员`frame_id` 来自ROS的概念，`rs_driver`本身不需要它，所以删除。适配ROS的驱动 `rslidar_sdk`会创建和处理它。 + +#### 7.3.1.2 RSInputParam + +在`v1.3.x`中，RSInputParam定义如下。 + +```c++ +typedef struct RSInputParam ///< The LiDAR input parameter +{ + std::string device_ip = "192.168.1.200"; ///< Ip of LiDAR + std::string multi_cast_address = "0.0.0.0"; ///< Address of multicast + std::string host_address = "0.0.0.0"; ///< Address of host + uint16_t msop_port = 6699; ///< Msop packet port number + uint16_t difop_port = 7788; ///< Difop packet port number + bool read_pcap = false; ///< true: The driver will process the pcap through pcap_path. false: The driver will + ///< Get data from online LiDAR + double pcap_rate = 1; ///< Rate to read the pcap file + bool pcap_repeat = true; ///< true: The pcap bag will repeat play + std::string pcap_path = "null"; ///< Absolute path of pcap file + bool use_vlan = false; ///< Vlan on-off + bool use_someip = false; ///< Someip on-off + bool use_custom_proto = false; ///< Customer Protocol on-off +}； +``` + +在`v1.5.x`中，RSInputParam定义如下。 + +```c++ +typedef struct RSInputParam ///< The LiDAR input parameter +{ + uint16_t msop_port = 6699; ///< Msop packet port number + uint16_t difop_port = 7788; ///< Difop packet port number + std::string host_address = "0.0.0.0"; ///< Address of host + std::string group_address = "0.0.0.0"; ///< Address of multicast group + std::string pcap_path = ""; ///< Absolute path of pcap file + bool pcap_repeat = true; ///< true: The pcap bag will repeat play + float pcap_rate = 1.0f; ///< Rate to read the pcap file + bool use_vlan = false; ///< Vlan on-off + uint16_t user_layer_bytes = 0; ///< Bytes of user layer. thers is no user layer if it is 0 + uint16_t tail_layer_bytes = 0; ///< Bytes of tail layer. thers is no tail layer if it is 0 + uint32_t socket_recv_buf = 106496; // +class LidarDriver +{ +public: + + inline void regRecvCallback(const std::function&)>& callback) + { + driver_ptr_->regRecvCallback(callback); + } + ... +}; +``` + + +在`v1.5.x`中，LidarDriver定义如下。 + +```c++ +template +class LidarDriver +{ +public: + + inline void regPointCloudCallback(const std::function(void)>& cb_get_cloud, + const std::function)>& cb_put_cloud) + { + driver_ptr_->regPointCloudCallback(cb_get_cloud, cb_put_cloud); + } + ... +}; +``` + +变动如下： ++ LidarDriver类的模板参数从点 `PointT`改成了点云 `T_PointCloud`。 ++ LidarDriver的一个点云回调函数，变成了两个。 + + 在`v1.3.x`中，LidarDriver只需要一个点云回调函数`callback`。rs_driver通过这个函数将构建好的点云返回给调用者。 + + 在`v1.5.x`中，LidarDriver需要两个回调函数。一个函数`cb_put_cloud`与`v1.3.x`的类似，rs_driver将构建好的点云返回给调用者。另一个函数`cb_get_cloud`则是`rs_driver`需要通过这个函数，从调用者获取空闲的点云实例。关于这一点的更详细说明，请参考[连接在线雷达](./08_how_to_decode_online_lidar_CN.md) 和 [rs_driver的线程模型与接口设计](../intro/03_thread_model_CN.md)。 + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/08_how_to_decode_online_lidar.md b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/08_how_to_decode_online_lidar.md new file mode 100644 index 0000000..d7d8c07 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/08_how_to_decode_online_lidar.md @@ -0,0 +1,348 @@ +# 8 How to decode online LiDAR + + + +## 8.1 Introduction + +This document illustrates how to decode the MSOP/DIFOP packets from an online LiDAR with `rs_driver`'s API. + +For more info, please refer to the demo app `rs_driver/demo/demo_online.cpp`. + +Also refer to the demo app `rs_driver/demo/demo_online_multi_lidars.cpp`. It is an example of multiple LiDARs. + +Here the definitions of point and point cloud, is from the project file. + +`rs_driver/src/rs_driver/msg/point_cloud_msg.hpp`, `rs_driver/src/rs_driver/msg/pcl_point_cloud_msg.hpp` + + + +## 8.2 Steps + +### 8.2.1 Define Point + +Point is the base unit of Point Cloud. `rs_driver` supports these member variables. +- x -- The x coordinate of point. float type. +- y -- The y coordinate of point. float type. +- z -- The z coordinate of point. float type. +- intensity -- The intensity of point. uint8_t type. +- timestamp -- The timestamp of point. double type. It may be generated by the LiDAR or `rs_driver` on the host machine. +- ring -- The ring ID of the point, which represents the row/channel number. Take RS80 as an example, the range of ring ID is `0`~`79` (from bottom to top). + +Below are some examples: + +- The point type contains **x, y, z, intensity** + + ```c++ + struct PointXYZI + { + float x; + float y; + float z; + uint8_t intensity; + }; + ``` + +- If using PCL, a simple way is to use **pcl::PointXYZI**. + +- The point type contains **x, y, z, intensity, timestamp, ring** + + ```c++ + struct PointXYZIRT + { + float x; + float y; + float z; + uint8_t intensity; + double timestamp; + uint16_t ring; + }; + ``` + +Here user may add new member variables, remove member variables, or change the order of them, but should not change names or types of them. + +### 8.2.2 Define Point Cloud + + Below is the definition of point cloud. + + ```c++ + template + class PointCloudT + { + public: + typedef T_Point PointT; + typedef std::vector VectorT; + + uint32_t height = 0; ///< Height of point cloud + uint32_t width = 0; ///< Width of point cloud + bool is_dense = false; ///< If is_dense is true, the point cloud does not contain NAN points + double timestamp = 0.0; ///< Timestamp of point cloud + uint32_t seq = 0; ///< Sequence number of message + + VectorT points; + }; + ``` + + Here user may add new members, and change the order of these members, but should not change or remove them. + + This definition is a template class. It needs a Point type as template parameter. + + ``` + typedef PointXYZI PointT; + typedef PointCloudT PointCloudMsg; + ``` + + +### 8.2.3 Define the driver object + +Define a driver object. + +```c++ +int main() +{ + LidarDriver driver; ///< Declare the driver object + ... +} +``` + +### 8.2.4 Configure the driver parameter + +Define a RSDriverParam variable and configure it. ++ `InputType::ONLINE_LIDAR` means that the driver get packets from an online LiDAR. ++ `LidarType::RSAIRY` means a RSAIRY LiDAR. ++ Also set the local MSOP/DIFOP/IMU ports, if the lidar does not support IMU, change '#define ENABLE_IMU_PARSE 1' to '#define ENABLE_IMU_PARSE 0'. + +```c++ +int main() +{ + ... + RSDriverParam param; ///< Create a parameter object + param.input_type = InputType::ONLINE_LIDAR; /// get packet from online lidar + param.input_param.msop_port = 6699; ///< Set the lidar msop port number, the default is 6699 + param.input_param.difop_port = 7788; ///< Set the lidar difop port number, the default is 7788 +#if ENABLE_IMU_PARSE + param.input_param.imu_port = 6688; ///< Set the lidar imu port number, the default is 0 +#endif + param.lidar_type = LidarType::RSAIRY; ///< Set the lidar type. Make sure this type is correct + ... +} +``` + +### 8.2.5 Define and register Point Cloud callbacks + ++ User is supposed to provide free point cloud to `rs_driver`. Here is the first callback. + +```c++ +SyncQueue> free_cloud_queue; + +std::shared_ptr driverGetPointCloudFromCallerCallback(void) +{ + std::shared_ptr msg = free_cloud_queue.pop(); + if (msg.get() != NULL) + { + return msg; + } + + return std::make_shared(); +} +``` + ++ `rs_driver` returns stuffed point cloud to user. Here is the second callback. + +```c++ +SyncQueue> stuffed_cloud_queue; + +void driverReturnPointCloudToCallerCallback(std::shared_ptr msg) +{ + stuffed_cloud_queue.push(msg); + + RS_MSG << "msg: " << msg->seq << " point cloud size: " << msg->points.size() << RS_REND; +} +``` + +Note: The driver calls these two callback functions in the MSOP/DIFOP handling thread, so **don't do any time-consuming task** in them. + ++ User creates a new thread to processes the point cloud. + +```c++ +void processCloud(void) +{ + while (1) + { + std::shared_ptr msg = stuffed_cloud_queue.popWait(); + if (msg.get() == NULL) + { + continue; + } + + // Well, it is time to process the point cloud msg, even it is time-consuming. + RS_MSG << "msg: " << msg->seq << " point cloud size: " << msg->points.size() << RS_REND; + + free_cloud_queue.push(msg); + } +} + +``` + ++ Register them to `rs_driver`. + +```c++ +int main() +{ + ... + driver.regPointCloudCallback(driverReturnPointCloudToCallerCallback, + driverReturnPointCloudToCallerCallback); + ... +} +``` + +### 8.2.6 Define and register the IMU callback function (ignore if the lidar does not support IMU) + + ++ Similar to acquiring point clouds, the `rs_driver` requires the caller to provide an free IMU data instance through a callback function. Here, the first IMU callback function is defined. + +```c++ +SyncQueue> free_imu_data_queue; + +std::shared_ptr driverGetIMUDataFromCallerCallback(void) +{ + std::shared_ptr msg = free_imu_data_queue.pop(); + if (msg.get() != NULL) + { + return msg; + } + + return std::make_shared(); +} +``` + ++ `rs_driver` returns stuffed IMU data to user. Here is the second callback. + +```c++ +SyncQueue> stuffed_imu_data_queue; + +void driverReturnImuDataToCallerCallback(const std::shared_ptr& msg) +{ + stuffed_imu_data_queue.push(msg); +} +``` + +Note: The driver calls these two callback functions in the IMU handling thread, so **don't do any time-consuming task** in them. + ++ User creates a new thread to processes the IMU data. + +```c++ +void processImuData(void) +{ + uint32_t imu_cnt = 0; + while (!to_exit_process) + { + std::shared_ptr msg = stuffed_imu_data_queue.popWait(); + if (msg.get() == NULL) + { + continue; + } + + // Well, it is time to process the IMU data msg, even it is time-consuming. + RS_MSG << "msg: " << imu_cnt << " imu data ts: " <timestamp) << RS_REND; + + imu_cnt++; +#if 0 + RS_DEBUG <<"imu data: " << " , linear_a_x" << msg->linear_acceleration_x + << " , linear_a_y " << msg->linear_acceleration_y << " , linear_a_z" << msg->linear_acceleration_z + << " , angular_v_x " << msg->angular_velocity_x << " , angular_v_y" << msg->angular_velocity_y + << " , angular_v_z" <angular_velocity_z << RS_REND; +#endif + + free_imu_data_queue.push(msg); + } +} + +``` + ++ Register them to `rs_driver`. + +```c++ +int main() +{ + ... +#if ENABLE_IMU_PARSE + driver.regImuDataCallback(driverGetIMUDataFromCallerCallback, driverReturnImuDataToCallerCallback); +#endif + ... +} +``` + +### 8.2.7 Define and register exception callbacks + ++ When an error happens, `rs_driver` informs user. Here is the exception callback. + +```c++ +void exceptionCallback(const Error &code) +{ + RS_WARNING << "Error code : " << code.toString() << RS_REND; +} +``` + +Once again, **don't do any time-consuming task** in it. + ++ Register the callback. + +```c++ +int main() +{ + ... + driver.regExceptionCallback(exceptionCallback); /// + class PointCloudT + { + public: + typedef T_Point PointT; + typedef std::vector VectorT; + + uint32_t height = 0; ///< Height of point cloud + uint32_t width = 0; ///< Width of point cloud + bool is_dense = false; ///< If is_dense is true, the point cloud does not contain NAN points + double timestamp = 0.0; ///< Timestamp of point cloud + uint32_t seq = 0; ///< Sequence number of message + + VectorT points; + }; + + ``` +这个点云的定义，使用者可以加入新成员，改变成员顺序，但是不可以删除成员，或者改变成员的类型。 + +这个点云定义是一个模板类，还需要指定一个点类型作为模板参数。 + + ```c++ + typedef PointXYZI PointT; + typedef PointCloudT PointCloudMsg; + ``` + +### 8.2.3 定义LidarDriver对象 + +LidarDriver类是`rs_driver`的接口类。这里定义一个LidarDriver的实例。 + +```c++ +int main() +{ + LidarDriver driver; ///< Declare the driver object + ... +} +``` + +### 8.2.4 配置LidarDriver的参数 + +RSDriverParam定义LidarDriver的参数。这里定义RSDriverParam变量，并配置它。 + ++ `InputType::ONLINE_LIDAR`意味着从在线雷达得到MSOP/DIFOP包。 ++ `LidarType::RSAIRY`是雷达类型 ++ 分别设置接收MSOP/DIFOP/IMU Packet的端口号， 若雷达不支持IMU，则将`#define ENABLE_IMU_PARSE 1` 改为`#define ENABLE_IMU_PARSE 0`。 +```c++ +int main() +{ + ... + RSDriverParam param; ///< Create a parameter object + param.input_type = InputType::ONLINE_LIDAR; /// get packet from online lidar + param.input_param.msop_port = 6699; ///< Set the lidar msop port number, the default is 6699 + param.input_param.difop_port = 7788; ///< Set the lidar difop port number, the default is 7788 +#if ENABLE_IMU_PARSE + param.input_param.imu_port = 6688; ///< Set the lidar imu port number, the default is 0 +#endif + param.lidar_type = LidarType::RSAIRY; ///< Set the lidar type. Make sure this type is correct + ... +} +``` + +### 8.2.5 定义和注册点云回调函数 + ++ `rs_driver`需要调用者通过回调函数，提供空闲的点云实例。这里定义这第一个点云回调函数。 + +```c++ +SyncQueue> free_cloud_queue; + +std::shared_ptr driverGetPointCloudFromCallerCallback(void) +{ + std::shared_ptr msg = free_cloud_queue.pop(); + if (msg.get() != NULL) + { + return msg; + } + + return std::make_shared(); +} +``` + ++ `rs_driver`通过回调函数，将填充好的点云返回给调用者。这里定义这第二个点云回调函数。 + +```c++ +SyncQueue> stuffed_cloud_queue; + +void driverReturnPointCloudToCallerCallback(std::shared_ptr msg) +{ + stuffed_cloud_queue.push(msg); + + RS_MSG << "msg: " << msg->seq << " point cloud size: " << msg->points.size() << RS_REND; +} +``` + +注意这两个回调函数都运行在`rs_driver`的MSOP/DIFOP Packet的处理线程中，所以它们不可以做太耗时的任务，否则会导致MSOP/DIFOP Packet不能及时处理。 + ++ 使用者在自己的线程中，处理点云。 + +```c++ +void processCloud(void) +{ + while (1) + { + std::shared_ptr msg = stuffed_cloud_queue.popWait(); + if (msg.get() == NULL) + { + continue; + } + + // Well, it is time to process the point cloud msg, even it is time-consuming. + RS_MSG << "msg: " << msg->seq << " point cloud size: " << msg->points.size() << RS_REND; + + free_cloud_queue.push(msg); + } +} + +``` + ++ 注册两个点云回调函数。 + +```c++ +int main() +{ + ... + driver.regPointCloudCallback(driverReturnPointCloudToCallerCallback, + driverReturnPointCloudToCallerCallback); + ... +} +``` + +### 8.2.6 定义和注册IMU回调函数(若雷达不支持IMU则忽略) + ++ 和获取点云类似， `rs_driver`需要调用者通过回调函数，提供空闲的IMU实例。这里定义这第一个IMU回调函数。 + +```c++ +SyncQueue> free_imu_data_queue; + +std::shared_ptr driverGetIMUDataFromCallerCallback(void) +{ + std::shared_ptr msg = free_imu_data_queue.pop(); + if (msg.get() != NULL) + { + return msg; + } + + return std::make_shared(); +} + +``` + ++ `rs_driver`通过回调函数，将填充好的IMU数据返回给调用者。这里定义这第二个IMU回调函数。 + +```c++ +SyncQueue> stuffed_imu_data_queue; + +void driverReturnImuDataToCallerCallback(const std::shared_ptr& msg) +{ + stuffed_imu_data_queue.push(msg); +} + +``` + +注意这两个回调函数都运行在`rs_driver`的IMU Packet的处理线程中，所以它们不可以做太耗时的任务，否则会导致IMU Packet不能及时处理。 + ++ 使用者在自己的线程中，处理IMU数据。 + +```c++ +void processImuData(void) +{ + uint32_t imu_cnt = 0; + while (!to_exit_process) + { + std::shared_ptr msg = stuffed_imu_data_queue.popWait(); + if (msg.get() == NULL) + { + continue; + } + + // Well, it is time to process the IMU data msg, even it is time-consuming. + RS_MSG << "msg: " << imu_cnt << " imu data ts: " <timestamp) << RS_REND; + + imu_cnt++; +#if 0 + RS_DEBUG <<"imu data: " << " , linear_a_x" << msg->linear_acceleration_x + << " , linear_a_y " << msg->linear_acceleration_y << " , linear_a_z" << msg->linear_acceleration_z + << " , angular_v_x " << msg->angular_velocity_x << " , angular_v_y" << msg->angular_velocity_y + << " , angular_v_z" <angular_velocity_z << RS_REND; +#endif + + free_imu_data_queue.push(msg); + } +} + +``` + ++ 注册两个IMU回调函数。 + +```c++ +int main() +{ + ... +#if ENABLE_IMU_PARSE + driver.regImuDataCallback(driverGetIMUDataFromCallerCallback, driverReturnImuDataToCallerCallback); +#endif + ... +} +``` + + + +### 8.2.7 定义和注册异常回调函数 + ++ `rs_driver`检测到异常发生时，通过回调函数通知调用者。这里定义异常回调函数。 + +```c++ +void exceptionCallback(const Error &code) +{ + RS_WARNING << "Error code : " << code.toString() << RS_REND; +} +``` + +再一次提醒，这个回调函数运行在`rs_driver`的线程中，所以不可以做太耗时的任务，否则可能导致MSOP/DIFOP包不能及时接收和处理。 + ++ 在主函数中，注册异常回调函数。 + +```c++ +int main() +{ + ... + driver.regExceptionCallback(exceptionCallback); /// + class PointCloudT + { + public: + typedef T_Point PointT; + typedef std::vector VectorT; + + uint32_t height = 0; ///< Height of point cloud + uint32_t width = 0; ///< Width of point cloud + bool is_dense = false; ///< If is_dense is true, the point cloud does not contain NAN points + double timestamp = 0.0; ///< Timestamp of point cloud + uint32_t seq = 0; ///< Sequence number of message + + VectorT points; + }; + ``` + + Here user may add new members, and change the order of these members, but should not change or remove them. + + This definition is a template class. It needs a Point type as template parameter. + + ``` + typedef PointXYZI PointT; + typedef PointCloudT PointCloudMsg; + ``` + + + +### 10.2.3 Define the driver object + +Define a driver object. + +```c++ +int main() +{ + LidarDriver driver; ///< Declare the driver object + ... +} +``` + + + +### 10.2.4 Configure the driver parameter + +Define a RSDriverParam variable and configure it. ++ `InputType::PCAP_FILE` means that `rs_driver` get packets from a PCAP file, which is `/home/robosense/lidar.pcap`. ++ `LidarType::RSAIRY` means a RSAIRY LiDAR. ++ Also set the local MSOP/DIFOP/IMU ports, if the lidar does not support IMU, change '#define ENABLE_IMU_PARSE 1' to '#define ENABLE_IMU_PARSE 0'. + + +```c++ +int main() +{ + ... + RSDriverParam param; ///< Create a parameter object + param.input_type = InputType::ONLINE_LIDAR; /// get packet from online lidar + param.input_param.msop_port = 6699; ///< Set the lidar msop port number, the default is 6699 + param.input_param.difop_port = 7788; ///< Set the lidar difop port number, the default is 7788 +#if ENABLE_IMU_PARSE + param.input_param.imu_port = 6688; ///< Set the lidar imu port number, the default is 0 +#endif + param.lidar_type = LidarType::RSAIRY; ///< Set the lidar type. Make sure this type is correct + ... +} +``` + + + +### 10.2.5 Define and register Point Cloud callbacks + ++ User is supposed to provide free point cloud to `rs_driver`. Here is the first callback. + +```c++ +SyncQueue> free_cloud_queue; + +std::shared_ptr driverGetPointCloudFromCallerCallback(void) +{ + std::shared_ptr msg = free_cloud_queue.pop(); + if (msg.get() != NULL) + { + return msg; + } + + return std::make_shared(); +} +``` + ++ `rs_driver` returns stuffed point cloud to user. Here is the second callback. + +```c++ +SyncQueue> stuffed_cloud_queue; + +void driverReturnPointCloudToCallerCallback(std::shared_ptr msg) +{ + stuffed_cloud_queue.push(msg); + + RS_MSG << "msg: " << msg->seq << " point cloud size: " << msg->points.size() << RS_REND; +} +``` + +Note: The driver calls these two callback functions in the MSOP/DIFOP handling thread, so **don't do any time-consuming task** in them. + ++ User creates a new thread to processes the point cloud. + +```c++ +void processCloud(void) +{ + while (1) + { + std::shared_ptr msg = stuffed_cloud_queue.popWait(); + if (msg.get() == NULL) + { + continue; + } + + // Well, it is time to process the point cloud msg, even it is time-consuming. + RS_MSG << "msg: " << msg->seq << " point cloud size: " << msg->points.size() << RS_REND; + + free_cloud_queue.push(msg); + } +} + +``` + ++ Register them to `rs_driver`. + +```c++ +int main() +{ + ... + driver.regPointCloudCallback(driverReturnPointCloudToCallerCallback, + driverReturnPointCloudToCallerCallback); + ... +} +``` + + +### 10.2.6 Define and register the IMU callback function (ignore if the lidar does not support IMU) + + ++ Similar to acquiring point clouds, the `rs_driver` requires the caller to provide an idle IMU instance through a callback function. Here, the first IMU callback function is defined. + +```c++ +SyncQueue> free_imu_data_queue; + +std::shared_ptr driverGetIMUDataFromCallerCallback(void) +{ + std::shared_ptr msg = free_imu_data_queue.pop(); + if (msg.get() != NULL) + { + return msg; + } + + return std::make_shared(); +} +``` + ++ `rs_driver` returns stuffed IMU data to user. Here is the second callback. + +```c++ +SyncQueue> stuffed_imu_data_queue; + +void driverReturnImuDataToCallerCallback(const std::shared_ptr& msg) +{ + stuffed_imu_data_queue.push(msg); +} +``` + +Note: The driver calls these two callback functions in the IMU handling thread, so **don't do any time-consuming task** in them. + ++ User creates a new thread to processes the IMU data. + +```c++ +void processImuData(void) +{ + uint32_t imu_cnt = 0; + while (!to_exit_process) + { + std::shared_ptr msg = stuffed_imu_data_queue.popWait(); + if (msg.get() == NULL) + { + continue; + } + + // Well, it is time to process the IMU data msg, even it is time-consuming. + RS_MSG << "msg: " << imu_cnt << " imu data ts: " <timestamp) << RS_REND; + + imu_cnt++; +#if 0 + RS_DEBUG <<"imu data: " << " , linear_a_x" << msg->linear_acceleration_x + << " , linear_a_y " << msg->linear_acceleration_y << " , linear_a_z" << msg->linear_acceleration_z + << " , angular_v_x " << msg->angular_velocity_x << " , angular_v_y" << msg->angular_velocity_y + << " , angular_v_z" <angular_velocity_z << RS_REND; +#endif + + free_imu_data_queue.push(msg); + } +} + +``` + ++ Register them to `rs_driver`. + +```c++ +int main() +{ + ... +#if ENABLE_IMU_PARSE + driver.regImuDataCallback(driverGetIMUDataFromCallerCallback, driverReturnImuDataToCallerCallback); +#endif + ... +} +``` + +### 10.2.7 Define and register exception callbacks + ++ When an error happens, `rs_driver` informs user. Here is the exception callback. + +```c++ +void exceptionCallback(const Error &code) +{ + RS_WARNING << "Error code : " << code.toString() << RS_REND; +} +``` + +Once again, **don't do any time-consuming task** in it. + ++ Register the callback. + +```c++ +int main() +{ + ... + driver.regExceptionCallback(exceptionCallback); /// + class PointCloudT + { + public: + typedef T_Point PointT; + typedef std::vector VectorT; + + uint32_t height = 0; ///< Height of point cloud + uint32_t width = 0; ///< Width of point cloud + bool is_dense = false; ///< If is_dense is true, the point cloud does not contain NAN points + double timestamp = 0.0; ///< Timestamp of point cloud + uint32_t seq = 0; ///< Sequence number of message + + VectorT points; + }; + + ``` +这个点云的定义，使用者可以加入新成员，改变成员顺序，但是不可以删除成员，或者改变成员的类型。 + +这个点云定义是一个模板类，还需要指定一个点类型作为模板参数。 + + ```c++ + typedef PointXYZI PointT; + typedef PointCloudT PointCloudMsg; + ``` + +### 10.2.3 定义LidarDriver对象 + +LidarDriver类是`rs_driver`的接口类。这里定义一个LidarDriver的实例。 + +```c++ +int main() +{ + LidarDriver driver; ///< Declare the driver object + ... +} +``` + +### 10.2.4 配置LidarDriver的参数 + +RSDriverParam定义LidarDriver的参数。这里定义RSDriverParam变量，并配置它。 + ++ `InputType::PCAP_FILE` 意味着解析PCAP文件得到MSOP/DIFOP包。这里的PCAP文件是`/home/robosense/lidar.pcap`。 ++ `LidarType::RSAIRY`是雷达类型 ++ 分别设置接收MSOP/DIFOP/IMU Packet的端口号， 若雷达不支持IMU，则将`#define ENABLE_IMU_PARSE 1` 改为`#define ENABLE_IMU_PARSE 0`。 +```c++ +int main() +{ + ... + RSDriverParam param; ///< Create a parameter object + param.input_type = InputType::PCAP_FILE; /// get packet from online lidar + param.input_param.msop_port = 6699; ///< Set the lidar msop port number, the default is 6699 + param.input_param.difop_port = 7788; ///< Set the lidar difop port number, the default is 7788 +#if ENABLE_IMU_PARSE + param.input_param.imu_port = 6688; ///< Set the lidar imu port number, the default is 0 +#endif + param.lidar_type = LidarType::RSAIRY; ///< Set the lidar type. Make sure this type is correct + ... +} +``` + +### 10.2.5 定义和注册点云回调函数 + ++ `rs_driver`需要调用者通过回调函数，提供空闲的点云实例。这里定义这第一个点云回调函数。 + +```c++ +SyncQueue> free_cloud_queue; + +std::shared_ptr driverGetPointCloudFromCallerCallback(void) +{ + std::shared_ptr msg = free_cloud_queue.pop(); + if (msg.get() != NULL) + { + return msg; + } + + return std::make_shared(); +} +``` + ++ `rs_driver`通过回调函数，将填充好的点云返回给调用者。这里定义这第二个点云回调函数。 + +```c++ +SyncQueue> stuffed_cloud_queue; + +void driverReturnPointCloudToCallerCallback(std::shared_ptr msg) +{ + stuffed_cloud_queue.push(msg); + + RS_MSG << "msg: " << msg->seq << " point cloud size: " << msg->points.size() << RS_REND; +} +``` + +注意这两个回调函数都运行在`rs_driver`的MSOP/DIFOP Packet的处理线程中，所以它们不可以做太耗时的任务，否则会导致MSOP/DIFOP Packet不能及时处理。 + ++ 使用者在自己的线程中，处理点云。 + +```c++ +void processCloud(void) +{ + while (1) + { + std::shared_ptr msg = stuffed_cloud_queue.popWait(); + if (msg.get() == NULL) + { + continue; + } + + // Well, it is time to process the point cloud msg, even it is time-consuming. + RS_MSG << "msg: " << msg->seq << " point cloud size: " << msg->points.size() << RS_REND; + + free_cloud_queue.push(msg); + } +} + +``` + ++ 注册两个点云回调函数。 + +```c++ +int main() +{ + ... + driver.regPointCloudCallback(driverReturnPointCloudToCallerCallback, + driverReturnPointCloudToCallerCallback); + ... +} +``` + +### 10.2.6 定义和注册IMU回调函数(若雷达不支持IMU则忽略) + ++ 和获取点云类似， `rs_driver`需要调用者通过回调函数，提供空闲的IMU实例。这里定义这第一个IMU回调函数。 + +```c++ +SyncQueue> free_imu_data_queue; + +std::shared_ptr driverGetIMUDataFromCallerCallback(void) +{ + std::shared_ptr msg = free_imu_data_queue.pop(); + if (msg.get() != NULL) + { + return msg; + } + + return std::make_shared(); +} + +``` + ++ `rs_driver`通过回调函数，将填充好的IMU数据返回给调用者。这里定义这第二个IMU回调函数。 + +```c++ +SyncQueue> stuffed_imu_data_queue; + +void driverReturnImuDataToCallerCallback(const std::shared_ptr& msg) +{ + stuffed_imu_data_queue.push(msg); +} + +``` + +注意这两个回调函数都运行在`rs_driver`的IMU Packet的处理线程中，所以它们不可以做太耗时的任务，否则会导致IMU Packet不能及时处理。 + ++ 使用者在自己的线程中，处理IMU数据。 + +```c++ +void processImuData(void) +{ + uint32_t imu_cnt = 0; + while (!to_exit_process) + { + std::shared_ptr msg = stuffed_imu_data_queue.popWait(); + if (msg.get() == NULL) + { + continue; + } + + // Well, it is time to process the IMU data msg, even it is time-consuming. + RS_MSG << "msg: " << imu_cnt << " imu data ts: " <timestamp) << RS_REND; + + imu_cnt++; +#if 0 + RS_DEBUG <<"imu data: " << " , linear_a_x" << msg->linear_acceleration_x + << " , linear_a_y " << msg->linear_acceleration_y << " , linear_a_z" << msg->linear_acceleration_z + << " , angular_v_x " << msg->angular_velocity_x << " , angular_v_y" << msg->angular_velocity_y + << " , angular_v_z" <angular_velocity_z << RS_REND; +#endif + + free_imu_data_queue.push(msg); + } +} + +``` + ++ 注册两个IMU回调函数。 + +```c++ +int main() +{ + ... +#if ENABLE_IMU_PARSE + driver.regImuDataCallback(driverGetIMUDataFromCallerCallback, driverReturnImuDataToCallerCallback); +#endif + ... +} +``` + +### 10.2.7 定义和注册异常回调函数 + ++ `rs_driver`检测到异常发生时，通过回调函数通知调用者。这里定义异常回调函数。 + +```c++ +void exceptionCallback(const Error &code) +{ + RS_WARNING << "Error code : " << code.toString() << RS_REND; +} +``` + +再一次提醒，这个回调函数运行在`rs_driver`的线程中，所以不可以做太耗时的任务，否则可能导致MSOP/DIFOP包不能及时接收和处理。 + ++ 在主函数中，注册异常回调函数。 + +```c++ +int main() +{ + ... + driver.regExceptionCallback(exceptionCallback); /// `Export Specified Packets ...` to get this dialog. +![](./img/13_04_wireshark_export_all.png) + +Export all packets. In above picture, select `All Packets` under `Packet Range`, to export all packets. + ++ Select `pcap` format instead of `pcapng`. `rs_driver` doesn't support the latter. ++ Under `Packet Range`, there are two columns: `Captured` and `Displayed`. Keep `Displayed` here. + + `Captured` are all captured packets. `Displayed` are the filtered. For example, in the line of `All packets`, captures `24333`, and leaves `23879` after filtering. + + If `Captured` export packets from all captured packets, else export only the filtered. + +#### 13.1.3.3 Export a Range of packets + +Here export a range of packets. + ++ This example give a range `1`-`10000`. These two numbers are from all packets. Actually `9805` packets are exported. +![](./img/13_05_wireshark_export_range.png) + +#### 13.1.3.4 Mark and Export + +Here mark a few packets and export them. First mark the packets with Menu `Mark/Unmark Packet`. +![](./img/13_06_wireshark_mark.png) + +Select `Marked packets only`. +![](./img/13_07_wireshark_export_marked.png) + +#### 13.1.3.5 Mark and Export A Range + +Here mark two packets, and export all packet between them. Select `First to last marked`. +![](./img/13_08_wireshark_export_marked_range.png) + + + +## 13.2 Capture with tcpdump + +`tcpdump` is the a tool on Unix/Linux. + +As below, capture packets on NIC `eno1` and save them into `a.pcap`, with filter criteria `UDP` and destination port `6699` or `7788`. And capture `30000` packets. + +```shell +sudo tcpdump -i eno1 -w a.pcap -c 30000 'udp dst port 7788 or 6699' +``` + +Use `ifconfig` to find out the NIC name. + +Don't use the option`-i any`. It captures on NIC `any` , and the protocol is `linux cooked capture`. This is not the Ethernet protocol. rs_driver doesn't accept it. + +```shell +sudo tcpdump -i any +``` + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/13_how_to_capture_pcap_file_CN.md b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/13_how_to_capture_pcap_file_CN.md new file mode 100644 index 0000000..ae3abaa --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/13_how_to_capture_pcap_file_CN.md @@ -0,0 +1,90 @@ +# 13 **如何为rs_driver录制一个PCAP文件** + + + +## 13.1 使用WireShark抓包 + +### 13.1.1 选择网卡 +打开WireShark的第一步，是选择在哪个网卡抓包。这里选择的网卡是`enp0s3`。 ++ 不要选择`any`，这样录制的PCAP文件，协议为`linux cooked capture`。这不是`Ethernet`格式，`rs_driver`不能识别。 ++ 如果雷达支持`VLAN`，可以选择在物理网卡上还是在虚拟网卡上抓包。在前者抓包带`VLAN`层，在后者抓包则不带。 +![](./img/13_01_wireshark_select_nic.png) + + + +使用如下的网络工具，可以确定网卡的名字。 + ++ `ipconfig` - Windows平台 ++ `ifconfig` - Linux平台 + + + +### 13.1.2 抓包 + +指定了网卡，就可以抓包了。 +![](./img/13_02_wireshark_capture.png) + + + +### 13.1.3 导出PCAP文件 + +#### 13.1.3.1 过滤掉不要的包 + +导出PCAP文件时，你可能希望只保留MSOP/DIFOP Packet。指定过滤条件就可以了。 + ++ 这里指定了两个端口`6699`和`7788`进行过滤。 +![](./img/13_03_wireshark_filter_by_port.png) + + +#### 13.1.3.2 导出所有包 + +选择菜单`File` -> `Export Specified Packets ... `，得到如下的对话框。 +![](./img/13_04_wireshark_export_all.png) + +可以选择导出所有包。在上面的图中，`Packet Range`选项，选择`All Packets`，导出所有包。 + ++ 保存文件时，选择`pcap`格式，而不是`pcapng`格式，后者`rs_driver`还不支持。 ++ `Packet Range`下有两列数字：`Captured`和`Displayed`。这里保持`Displayed`就好了。 + + `Captured`是抓到的所有包，`Displayed`是根据前面的过滤条件过滤后的包。比如`All packets`这一栏，总共抓到`24333`个包，过滤后还有`23879`个包。 + + 如果选择`Captured`，则导出的PCAP文件是从所有包中提取；如果选择`Displayed`，则只从过滤后的包中提取。 + +#### 13.1.3.3 导出指定范围内的包 + +如果包数太多，只想导出部分包，则可以指定导出的范围。 + ++ 下面的例子指定的范围是`1`到`10000`，这两个序号是在抓到的所有包中的序号，而实际导出的包数是`9805`个。 +![](./img/13_05_wireshark_export_range.png) + +#### 13.1.3.4 标记并导出特定包 + +有时候需要精确指定导出哪几个包。这时可以先通过右键菜单 `Mark/Unmark Packet`标记这几个包。 +![](./img/13_06_wireshark_mark.png) + +导出时，选择`Marked packets only`。 +![](./img/13_07_wireshark_export_marked.png) + +#### 13.1.3.5 标记并导出特定范围内的包 + +也可以先标记两个包，再导出它们两个之间的所有包。在导出时，选择`First to last marked`就可以。 +![](./img/13_08_wireshark_export_marked_range.png) + + + +## 13.2 使用tcpdump抓包 + +`tcpdump`是Unix/Linux下的抓包工具。 + +如下的命令在网卡`eno1`上抓包，保存到文件`a.pcap`，过滤条件是`UDP`协议，且目标端口为`6699`或`7788`。这里抓包`30000`个，对于一般问题的分析，这个包数够用了。 + +```shell +sudo tcpdump -i eno1 -w a.pcap -c 30000 'udp dst port 7788 or 6699' +``` + +使用网络工具`ifconfig`，可以确定网卡的名字。 + +不要使用选项`-i any`抓包。这个选项针对任意网卡抓包，协议为`linux cooked capture`。这不是Ethernet格式，rs_driver不能识别。 + +```shell +sudo tcpdump -i any +``` + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/14_how_to_use_rs_driver_viewer.md b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/14_how_to_use_rs_driver_viewer.md new file mode 100644 index 0000000..4ead255 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/14_how_to_use_rs_driver_viewer.md @@ -0,0 +1,113 @@ +# 14 **How to visualize point cloud with `rs_driver_viewer`** + + + +## 14.1 Introduction + +The `rs_driver_viewer` is a visualization tool for the point cloud. This document illustrates how to use it. +![](./img/14_01_rs_driver_viewer_point_cloud.png) + + + +## 14.2 Compile and Run + +Compile the driver with the option COMPILE_TOOLS=ON. + +```bash +cmake -DCOMPILE_TOOS=ON .. +``` + +Run the tool. + +```bash +./tool/rs_driver_viewer +``` + +Note: On Ubuntu 22.04, using PCL version 1.12 with VTK 9.0 causes runtime failures. It has been verified that PCL version 1.14 with VTK 9.0 works correctly. You can specify the PCL version by modifying the tool/CMakeLists.txt file. + +```cmake +find_package(PCL 1.14 COMPONENTS common visualization io QUIET REQUIRED) +``` + +### 14.2.1 Help Menu + +- -h + + print the argument menu + +- -type + + LiDAR type, the default value is *RS16* + +- -pcap + + Full path of the PCAP file. If this argument is set, the driver read packets from the pcap file, else from online Lidar. + +- -msop + + MSOP port number of LiDAR, the default value is *6699* + +- -difop + + DIFOP port number of LiDAR, the default value is *7788* + +- -group + + the multicast group address + +- -host + + the host address + +- -x + + Transformation parameter, default is 0, unit: m + +- -y + + Transformation parameter, default is 0, unit: m + +- -z + + Transformation parameter, default is 0, unit: m + +- -roll + + Transformation parameter, default is 0, unit: radian + +- -pitch + + Transformation parameter, default is 0, unit: radian + +- -yaw + + Transformation parameter, default is 0, unit: radian + +Note: + +**The point cloud transformation function is available only if the CMake option ENABLE_TRANSFORM is ON.** + + + +## 14.3 Examples + +- Decode from an online RS128 LiDAR. Its MSOP port is ```9966```, and DIFOP port is ```8877``` + + ```bash + rs_driver_viewer -type RS128 -msop 9966 -difop 8877 + ``` + +- Decode from a PCAP file with RSHELIOS LiDAR data. + + ```bash + rs_driver_viewer -type RSHELIOS -pcap /home/robosense/helios.pcap + ``` + +- Decode with the coordinate transformation parameters: x=1.5, y=2, z=0, roll=1.57, pitch=0, yaw=0 + + ```bash + rs_driver_viewer -type RS16 -x 1.5 -y 2 -roll 1.57 + ``` + + + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/14_how_to_use_rs_driver_viewer_CN.md b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/14_how_to_use_rs_driver_viewer_CN.md new file mode 100644 index 0000000..192a8a3 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/14_how_to_use_rs_driver_viewer_CN.md @@ -0,0 +1,111 @@ +# 14 **如何可视化点云** + + + +## 14.1 概述 + +`rs_driver_viewer`是`rs_driver`自带的小工具，可以用于显示点云。 + +本文说明如何使用这个工具。 +![](./img/14_01_rs_driver_viewer_point_cloud.png) + + + +## 14.2 编译和运行 + +要编译`rs_driver_viewer`，需要使能编译选项COMPILE_TOOLS=ON。 + +```bash +cmake -DCOMPILE_TOOS=ON .. +``` + +运行`rs_driver_viewer`。 + +```bash +./tool/rs_driver_viewer +``` +注：在 Ubuntu 22.04 下，使用 PCL 1.12 版本与 VTK 9.0 组合时会导致运行失败。经过验证，PCL 1.14 版本与 VTK 9.0 组合能够正常运行。可以通过修改 tool/CMakeLists.txt 文件来指定使用 PCL 1.14 版本。 + +```cmake +find_package(PCL 1.14 COMPONENTS common visualization io QUIET REQUIRED) +``` + +### 14.2.1 帮助菜单 + +- -h + + 打印帮助菜单 + +- -type + + 雷达类型，默认值是*RS16* + +- -pcap + + PCAP文件的全路径。如果设置这个参数，则rs_driver_viewer从PCAP文件读取MSOP/DIFOP包，而不是从UDP socket。 + +- -msop + + 接收MSOP包的端口号。默认值是*6699* + +- -difop + + 接收DIFOP包的端口号，默认值是 *7788* + +- -group + + 主机要加入的组播组的IP地址 + +- -host + + 主机地址 + +- -x + + 坐标转换参数，默认值为0，单位`米` + +- -y + + 坐标转换参数，默认值为0，单位`米` + +- -z + + 坐标转换参数，默认值为0，单位`米` + +- -roll + + 坐标转换参数，默认值为0，单位`弧度` + +- -pitch + + 坐标转换参数，默认值为0，单位`弧度` + +- -yaw + + 坐标转换参数，默认值为0，单位`弧度` + +注意：**要使用坐标转换功能，需要使能编译选项ENABLE_TRANSFORM=ON.** + + + +## 14.3 使用示例 + +- 从在线雷达```RS128```接收MSOP/DIFOP包。MSOP端口是```9966```， DIFOP端口是```8877```。 + + ```bash + rs_driver_viewer -type RS128 -msop 9966 -difop 8877 + ``` + +- 从PCAP文件解码，雷达类型是```RSHELIOS```。 + + ```bash + rs_driver_viewer -type RSHELIOS -pcap /home/robosense/helios.pcap + ``` + +- 从在线雷达```RS16```接收MSOP/DIFOP包。坐标转换参数为x=1.5, y=2, z=0, roll=1.57, pitch=0, yaw=0。 + + ```bash + rs_driver_viewer -type RS16 -x 1.5 -y 2 -roll 1.57 + ``` + + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/15_how_to_transform_pointcloud.md b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/15_how_to_transform_pointcloud.md new file mode 100644 index 0000000..9aaed8b --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/15_how_to_transform_pointcloud.md @@ -0,0 +1,52 @@ +# 15 **How to transform point cloud** + + + +## 15.1 Introduction + +This document illustrate how to transform the point cloud to a different position with the built-in transform function. + +Warning: + ++ It costs much CPU resources. This function is only for test purpose. ++ **Never enable this function in the released products**. + + + +## 15.2 Steps + +### 15.2.1 Compile + +To enable the transformation function, compile the driver with the option ENABLE_TRANSFORM=ON. + +```bash +cmake -DENABLE_TRANSFORM=ON .. +``` + +### 15.2.2 Config parameters + +Configure the transformation parameters. These parameters' default value is ```0```. + ++ The unit of x, y, z, is ```m``` ++ the unit of roll, pitch, yaw, is ```radian``` + ++ The rotation order of the transformation is **yaw - pitch - row**. + +Below is an example with x=1, y=0, z=2.5, roll=0.1, pitch=0.2, yaw=1.57. + +```c++ +RSDriverParam param; ///< Create a parameter object +param.input_type = InputType::ONLINE_LIDAR; /// get packet from online lidar +param.input_param.msop_port = 6699; ///< Set the lidar msop port number, the default is 6699 +param.input_param.difop_port = 7788; ///< Set the lidar difop port number, the default is 7788 +param.lidar_type = LidarType::RS16; ///< Set the lidar type. Make sure this type is correct + +param.decoder_param.transform_param.x = 1; ///< unit: m +param.decoder_param.transform_param.y = 0; ///< unit: m +param.decoder_param.transform_param.z = 2.5; ///< unit: m +param.decoder_param.transform_param.roll = 0.1; ///< unit: radian +param.decoder_param.transform_param.pitch = 0.2;///< unit: radian +param.decoder_param.transform_param.yaw = 1.57; ///< unit: radian + +``` + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/15_how_to_transform_pointcloud_CN.md b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/15_how_to_transform_pointcloud_CN.md new file mode 100644 index 0000000..70b3e9a --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/15_how_to_transform_pointcloud_CN.md @@ -0,0 +1,47 @@ +# 15 **如何对点云作坐标转换** + +## 15.1 概述 + +本文说明如何使用坐标转换功能，将点云变换到另一个坐标系上去。 + +警告： ++ 坐标转换显著消耗CPU资源。提供这个功能，仅用于测试目的。 ++ **不要在发布的产品中使能坐标转换**，除非已经仔细评估，确定系统能容忍这些消耗。 + + + +## 15.2 步骤 + +### 15.2.1 CMake编译宏 + +要使用坐标转换功能，需要使能CMake编译选项```ENABLE_TRANSFORM=ON```。 + +```bash +cmake -DENABLE_TRANSFORM=ON .. +``` + +### 15.2.2 配置参数 + +配置坐标转换的参数，这些参数的默认值是`0`。 ++ x, y, z的单位是`米` ++ yaw, pitch, roll的单位是`弧度` ++ 旋转的顺序是 `yaw -> pitch -> roll` + +例子如下。 + +```c++ +RSDriverParam param; ///< Create a parameter object +param.input_type = InputType::ONLINE_LIDAR; /// get packet from online lidar +param.input_param.msop_port = 6699; ///< Set the lidar msop port number, the default is 6699 +param.input_param.difop_port = 7788; ///< Set the lidar difop port number, the default is 7788 +param.lidar_type = LidarType::RS16; ///< Set the lidar type. Make sure this type is correct + +param.decoder_param.transform_param.x = 1; ///< unit: m +param.decoder_param.transform_param.y = 0; ///< unit: m +param.decoder_param.transform_param.z = 2.5; ///< unit: m +param.decoder_param.transform_param.yaw = 1.57; ///< unit: radian +param.decoder_param.transform_param.pitch = 0.2; ///< unit: radian +param.decoder_param.transform_param.roll = 0.1; ///< unit: radian + +``` + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/16_how_to_compile_on_windows.md b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/16_how_to_compile_on_windows.md new file mode 100644 index 0000000..43a0db2 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/16_how_to_compile_on_windows.md @@ -0,0 +1,332 @@ +# 16 **How to compile rs_driver on Windows** + + + +## 16.1 Overview + +This document illustrates how to comile two parts of `rs_driver`. +- Demo apps, includes `demo_online` and`demo_pcap` +- Point point visualization tool `rs_driver_viewer` + +They depends on different 3rd-party libraries. + +Below steps are done with `VS2019` on `Windows 10`. + + + +## 16.2 Compile demo_online + +Here take `demo_online` as an example. `demo_pcap` is similar to `demo_online`. + +### 16.2.1 Setup 3rd Libraries + +- To parse PCAP files, first setup `libpcap`. + +``` +WpdPack_4_1_2.zip // header files and library files +WinPcap_4_1_3.exe // runtime libraries +``` + +- Unzip `WpdPack_4_1_2.zip` to the directory `C:/Program Files`. + +- Run `WinPcap_4_1_3.exe`, and setup into`C:/Program Files`. + +### 16.2.2 Setup Project demo_online + +- Setup the `demo_online` project, and add the source file`demo_online.cpp`. +![](./img/16_01_demo_project.png) + +### 16.2.3 Configure Project demo_online + +- Comply with `C++14`. +![](./img/16_02_demo_use_cpp14.png) + +- `demo_online` depends on `rs_driver`。set its header file path. +![](./img/16_03_demo_extra_include.png) + +- set the header file path of `libpcap`. +![](./img/16_04_demo_include_path.png) + +- set the library file path of `libpcap`. +![](./img/16_05_demo_lib_path.png) + +- Add the dependency library of `libpcap`. It is `wpcap.lib`。Also add`ws2_32.lib`. It is Windows socket library. `rs_driver` depends on it. +![](./img/16_06_demo_lib.png) + +- set the compile option `_CRT_SECURE_NO_WARNINGS` to avoid unnecessary compiling errors. +![](./img/16_07_demo_precompile_macro.png) + +### 16.2.4 Compile and Run + +- Compile the `demo_online` project, and run it. + + + + + +## 16.3 Compile rs_driver_viewer + +### 16.3.1 Setup 3rd Party Library + +- Setup the `libpcap` library. + +- `rs_driver_viewer` Also depends on `PCL`, and then `Boost`、`Eigen` etc. It is lucky that `PCL` offers a setup package which contains all these libraries. This package fits `VS2019`. + +``` +PCL-1.11.1-AllInOne-msvc2019-win64.exe +``` + + Setup it to the directory C:/Program Files`. +![](./img/16_08_viewer_install_pcl.png) + +it also setup its dependency libraries. +![](./img/16_09_viewer_install_pcl_dep.png) + +The components are in the these directories. +``` +C:\Program Files\PCL 1.11.1 # PCL libraries +C:\Program Files\OpenNI2 # OpenNI2 libraries depended by PCL +C:\Program Files\PCL 1.11.1\3rdParty # Other libraries depended by PCL +``` + +### 16.3.2 configure 3rd-party Library + +Add paths of runtime libraries as below. +![](./img/16_10_viewer_add_env_var.png) + +``` +C:\Program Files\PCL 1.11.1\bin +C:\Program Files\PCL 1.11.1\3rdParty\VTK\bin +C:\Program Files\PCL 1.11.1\3rdParty\OpenNI2\Redist +``` + +### 16.3.3 Setup the rs_driver_viewer Project + +- Setup the `rs_driver_viewer` project, and add the source file`rs_driver_viewer.cpp`. +![](./img/16_11_viewer_project.png) + +### 16.3.4 Configure Project rs_driver_viewer + +- Comply with `C++14` +![](./img/16_02_demo_use_cpp14.png) + +- Disable SDL check +![](./img/16_12_viewer_sdl_check.png) + +- Set the header file path of `rs_driver`. +![](./img/16_03_demo_extra_include.png) + +- Set the header file path of `PCL`. Set its dependency libraries also. +![](./img/16_13_viewer_include_path.png) + +``` +C:\Program Files\PCL 1.11.1\include\pcl-1.11 +C:\Program Files\PCL 1.11.1\3rdParty\Boost\include\boost-1_74 +C:\Program Files\PCL 1.11.1\3rdParty\Eigen\eigen3 +C:\Program Files\PCL 1.11.1\3rdParty\FLANN\include +C:\Program Files\PCL 1.11.1\3rdParty\Qhull\include +C:\Program Files\PCL 1.11.1\3rdParty\VTK\include\vtk-8.2 +C:\Program Files\OpenNI2\Include +``` + +- Set the library file path of `PCL`. Set its dependency libraries also. +![](./img/16_14_viewer_lib_path.png) + +``` +C:\Program Files\PCL 1.11.1\lib +C:\Program Files\PCL 1.11.1\3rdParty\Boost\lib +C:\Program Files\PCL 1.11.1\3rdParty\FLANN\lib +C:\Program Files\PCL 1.11.1\3rdParty\Qhull\lib +C:\Program Files\PCL 1.11.1\3rdParty\VTK\lib +C:\Program Files\OpenNI2\Lib +``` + +- Set `PCL` libraries, including `PCL` and `vtk`. Also set `wpcap.lib` and `ws2_32.lib`. + +`PCL` libraries are as below. + +``` +pcl_common.lib +pcl_commond.lib +pcl_features.lib +pcl_featuresd.lib +pcl_filters.lib +pcl_filtersd.lib +pcl_io.lib +pcl_iod.lib +pcl_io_ply.lib +pcl_io_plyd.lib +pcl_kdtree.lib +pcl_kdtreed.lib +pcl_keypoints.lib +pcl_keypointsd.lib +pcl_ml.lib +pcl_mld.lib +pcl_octree.lib +pcl_octreed.lib +pcl_outofcore.lib +pcl_outofcored.lib +pcl_people.lib +pcl_peopled.lib +pcl_recognition.lib +pcl_recognitiond.lib +pcl_registration.lib +pcl_registrationd.lib +pcl_sample_consensus.lib +pcl_sample_consensusd.lib +pcl_search.lib +pcl_searchd.lib +pcl_segmentation.lib +pcl_segmentationd.lib +pcl_stereo.lib +pcl_stereod.lib +pcl_surface.lib +pcl_surfaced.lib +pcl_tracking.lib +pcl_trackingd.lib +pcl_visualization.lib +pcl_visualizationd.lib +``` + +`vtk` has `debug` version and `release`version. Here take `release` as an example. +![](./img/16_15_viewer_lib.png) + +``` +vtkChartsCore-8.2.lib +vtkCommonColor-8.2.lib +vtkCommonComputationalGeometry-8.2.lib +vtkCommonCore-8.2.lib +vtkCommonDataModel-8.2.lib +vtkCommonExecutionModel-8.2.lib +vtkCommonMath-8.2.lib +vtkCommonMisc-8.2.lib +vtkCommonSystem-8.2.lib +vtkCommonTransforms-8.2.lib +vtkDICOMParser-8.2.lib +vtkDomainsChemistry-8.2.lib +vtkDomainsChemistryOpenGL2-8.2.lib +vtkdoubleconversion-8.2.lib +vtkexodusII-8.2.lib +vtkexpat-8.2.lib +vtkFiltersAMR-8.2.lib +vtkFiltersCore-8.2.lib +vtkFiltersExtraction-8.2.lib +vtkFiltersFlowPaths-8.2.lib +vtkFiltersGeneral-8.2.lib +vtkFiltersGeneric-8.2.lib +vtkFiltersGeometry-8.2.lib +vtkFiltersHybrid-8.2.lib +vtkFiltersHyperTree-8.2.lib +vtkFiltersImaging-8.2.lib +vtkFiltersModeling-8.2.lib +vtkFiltersParallel-8.2.lib +vtkFiltersParallelImaging-8.2.lib +vtkFiltersPoints-8.2.lib +vtkFiltersProgrammable-8.2.lib +vtkFiltersSelection-8.2.lib +vtkFiltersSMP-8.2.lib +vtkFiltersSources-8.2.lib +vtkFiltersStatistics-8.2.lib +vtkFiltersTexture-8.2.lib +vtkFiltersTopology-8.2.lib +vtkFiltersVerdict-8.2.lib +vtkfreetype-8.2.lib +vtkGeovisCore-8.2.lib +vtkgl2ps-8.2.lib +vtkglew-8.2.lib +vtkGUISupportMFC-8.2.lib +vtkhdf5-8.2.lib +vtkhdf5_hl-8.2.lib +vtkImagingColor-8.2.lib +vtkImagingCore-8.2.lib +vtkImagingFourier-8.2.lib +vtkImagingGeneral-8.2.lib +vtkImagingHybrid-8.2.lib +vtkImagingMath-8.2.lib +vtkImagingMorphological-8.2.lib +vtkImagingSources-8.2.lib +vtkImagingStatistics-8.2.lib +vtkImagingStencil-8.2.lib +vtkInfovisCore-8.2.lib +vtkInfovisLayout-8.2.lib +vtkInteractionImage-8.2.lib +vtkInteractionStyle-8.2.lib +vtkInteractionWidgets-8.2.lib +vtkIOAMR-8.2.lib +vtkIOAsynchronous-8.2.lib +vtkIOCityGML-8.2.lib +vtkIOCore-8.2.lib +vtkIOEnSight-8.2.lib +vtkIOExodus-8.2.lib +vtkIOExport-8.2.lib +vtkIOExportOpenGL2-8.2.lib +vtkIOExportPDF-8.2.lib +vtkIOGeometry-8.2.lib +vtkIOImage-8.2.lib +vtkIOImport-8.2.lib +vtkIOInfovis-8.2.lib +vtkIOLegacy-8.2.lib +vtkIOLSDyna-8.2.lib +vtkIOMINC-8.2.lib +vtkIOMovie-8.2.lib +vtkIONetCDF-8.2.lib +vtkIOParallel-8.2.lib +vtkIOParallelXML-8.2.lib +vtkIOPLY-8.2.lib +vtkIOSegY-8.2.lib +vtkIOSQL-8.2.lib +vtkIOTecplotTable-8.2.lib +vtkIOVeraOut-8.2.lib +vtkIOVideo-8.2.lib +vtkIOXML-8.2.lib +vtkIOXMLParser-8.2.lib +vtkjpeg-8.2.lib +vtkjsoncpp-8.2.lib +vtklibharu-8.2.lib +vtklibxml2-8.2.lib +vtklz4-8.2.lib +vtklzma-8.2.lib +vtkmetaio-8.2.lib +vtkNetCDF-8.2.lib +vtkogg-8.2.lib +vtkParallelCore-8.2.lib +vtkpng-8.2.lib +vtkproj-8.2.lib +vtkpugixml-8.2.lib +vtkRenderingAnnotation-8.2.lib +vtkRenderingContext2D-8.2.lib +vtkRenderingContextOpenGL2-8.2.lib +vtkRenderingCore-8.2.lib +vtkRenderingExternal-8.2.lib +vtkRenderingFreeType-8.2.lib +vtkRenderingGL2PSOpenGL2-8.2.lib +vtkRenderingImage-8.2.lib +vtkRenderingLabel-8.2.lib +vtkRenderingLOD-8.2.lib +vtkRenderingOpenGL2-8.2.lib +vtkRenderingVolume-8.2.lib +vtkRenderingVolumeOpenGL2-8.2.lib +vtksqlite-8.2.lib +vtksys-8.2.lib +vtktheora-8.2.lib +vtktiff-8.2.lib +vtkverdict-8.2.lib +vtkViewsContext2D-8.2.lib +vtkViewsCore-8.2.lib +vtkViewsInfovis-8.2.lib +vtkzlib-8.2.lib +``` + +- Set below compile options to avoid unnecessary compile errors. +![](./img/16_16_viewer_precompile_macro.png) + +``` +BOOST_USE_WINDOWS_H +NOMINMAX +_CRT_SECURE_NO_DEPRECATE +``` + +### 16.3.5 Compile and Run + +Compile `demo_online`, and run it. + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/16_how_to_compile_on_windows_CN.md b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/16_how_to_compile_on_windows_CN.md new file mode 100644 index 0000000..5bbf88f --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/16_how_to_compile_on_windows_CN.md @@ -0,0 +1,330 @@ +# 16 **如何在Windows上编译rs_driver** + +## 16.1 概述 + +这里的编译说明，针对`rs_driver`的两个部分。 +- 示例程序，包括`demo_online`和`demo_pcap` +- 点云显示工具 `rs_driver_viewer` + +两者对第三方库的依赖不同，这里将分开说明。 + +如下步骤在`Windows 10`系统下完成，使用的编译工具为`VS2019`。 + + + +## 16.2 编译demo_online + +演示程序的编译，以`demo_online`为例说明。`demo_pcap`的编译步骤与`demo_online`相同。 + +### 16.2.1 安装第三方库 + +- 如果需要解析PCAP文件，则需要安装`libpcap`库，包括： + +``` +WpdPack_4_1_2.zip, 编译时需要的头文件和库文件 +WinPcap_4_1_3.exe, 包括运行时库 +``` + +- 将`WpdPack_4_1_2.zip`解压到目录`C:/Program Files`下。 + +- 运行`WinPcap_4_1_3.exe`，安装到目录`C:/Program Files`下。 + +### 16.2.2 创建demo_online工程 + +- 创建`demo_online`工程，加入源文件`demo_online.cpp`。 +![](./img/16_01_demo_project.png) + +### 16.2.3 配置demo_online工程 + +- 遵循`C++14`标准 +![](./img/16_02_demo_use_cpp14.png) + +- `demo_online`当然依赖`rs_driver`库。设置`rs_driver`的头文件路径。 +![](./img/16_03_demo_extra_include.png) + +- 设置`libpcap`库的头文件路径。 +![](./img/16_04_demo_include_path.png) + +- 设置`libpcap`库的库文件路径。 +![](./img/16_05_demo_lib_path.png) + +- 设置依赖的`libpcap`库`wpcap.lib`。也同时设置`ws2_32.lib`，这是Windows的socket库，`rs_driver`依赖它。 +![](./img/16_06_demo_lib.png) + +- 设置编译选项 `_CRT_SECURE_NO_WARNINGS`，避免不必要的编译错误。 +![](./img/16_07_demo_precompile_macro.png) + +### 16.2.4 编译及运行 + +- 编译`demo_online`工程，并运行。 + + 这个步骤没有什么特别的。 + + + +## 16.3 编译rs_driver_viewer + +### 16.3.1 安装第三方库 + +- 与`demo_online`一样，安装`libpcap`库。 + +- `rs_driver_viewer`还依赖`PCL`库，后者又依赖`Boost`、`Eigen`等一系列库。幸运的是，`PCL`库提供了适配`MSVC2019`的安装包，而这个包中又自带了它所依赖的库。这里使用的安装包是： + +``` +PCL-1.11.1-AllInOne-msvc2019-win64.exe +``` + + 运行它，安装到目录`C:/Program Files`下。 +![](./img/16_08_viewer_install_pcl.png) + + 注意，安装`PCL`库的同时，也会安装它依赖的库。 +![](./img/16_09_viewer_install_pcl_dep.png) + +安装的组件位置如下： +``` +C:\Program Files\PCL 1.11.1 # PCL自身的库 +C:\Program Files\OpenNI2 # PCL依赖的OpenNI2库 +C:\Program Files\PCL 1.11.1\3rdParty # PCL依赖的其他库 +``` + +### 16.3.2 配置第三方库 + +将如下运行时库的路径加入`PATH`。 +![](./img/16_10_viewer_add_env_var.png) + +``` +C:\Program Files\PCL 1.11.1\bin +C:\Program Files\PCL 1.11.1\3rdParty\VTK\bin +C:\Program Files\PCL 1.11.1\3rdParty\OpenNI2\Redist +``` + +### 16.3.3 创建rs_driver_viewer工程 + +- 创建`rs_driver_viewer`工程，加入源文件`rs_driver_viewer.cpp`。 +![](./img/16_11_viewer_project.png) + +### 16.3.4 配置rs_driver_viewer工程 + +- 与`demo_online`一样，遵循`C++14`标准 +![](./img/16_02_demo_use_cpp14.png) + +- 禁止SDL检查 +![](./img/16_12_viewer_sdl_check.png) + +- 与`demo_online`一样，设置`rs_driver`的头文件路径。 +![](./img/16_03_demo_extra_include.png) + +- 设置`PCL`库的头文件路径如下。（与`demo_online`一样，同时设置`libpcap`库） +![](./img/16_13_viewer_include_path.png) + +``` +C:\Program Files\PCL 1.11.1\include\pcl-1.11 +C:\Program Files\PCL 1.11.1\3rdParty\Boost\include\boost-1_74 +C:\Program Files\PCL 1.11.1\3rdParty\Eigen\eigen3 +C:\Program Files\PCL 1.11.1\3rdParty\FLANN\include +C:\Program Files\PCL 1.11.1\3rdParty\Qhull\include +C:\Program Files\PCL 1.11.1\3rdParty\VTK\include\vtk-8.2 +C:\Program Files\OpenNI2\Include +``` + +- 设置`PCL`库的库文件路径。（与`demo_online`一样，同时设置`libpcap`） +![](./img/16_14_viewer_lib_path.png) + +``` +C:\Program Files\PCL 1.11.1\lib +C:\Program Files\PCL 1.11.1\3rdParty\Boost\lib +C:\Program Files\PCL 1.11.1\3rdParty\FLANN\lib +C:\Program Files\PCL 1.11.1\3rdParty\Qhull\lib +C:\Program Files\PCL 1.11.1\3rdParty\VTK\lib +C:\Program Files\OpenNI2\Lib +``` + +- 设置`PCL`及它依赖的库，包括`PCL`和`vtk`两部分。（与`demo_online`一样，同时设置`wpcap.lib`和`ws2_32.lib`） + +`PCL`的库文件如下： + +``` +pcl_common.lib +pcl_commond.lib +pcl_features.lib +pcl_featuresd.lib +pcl_filters.lib +pcl_filtersd.lib +pcl_io.lib +pcl_iod.lib +pcl_io_ply.lib +pcl_io_plyd.lib +pcl_kdtree.lib +pcl_kdtreed.lib +pcl_keypoints.lib +pcl_keypointsd.lib +pcl_ml.lib +pcl_mld.lib +pcl_octree.lib +pcl_octreed.lib +pcl_outofcore.lib +pcl_outofcored.lib +pcl_people.lib +pcl_peopled.lib +pcl_recognition.lib +pcl_recognitiond.lib +pcl_registration.lib +pcl_registrationd.lib +pcl_sample_consensus.lib +pcl_sample_consensusd.lib +pcl_search.lib +pcl_searchd.lib +pcl_segmentation.lib +pcl_segmentationd.lib +pcl_stereo.lib +pcl_stereod.lib +pcl_surface.lib +pcl_surfaced.lib +pcl_tracking.lib +pcl_trackingd.lib +pcl_visualization.lib +pcl_visualizationd.lib +``` + +`vtk`库分为`debug`/`release`版本。如下是`release`版本。这里的步骤以`release`版本举例。 +![](./img/16_15_viewer_lib.png) + +``` +vtkChartsCore-8.2.lib +vtkCommonColor-8.2.lib +vtkCommonComputationalGeometry-8.2.lib +vtkCommonCore-8.2.lib +vtkCommonDataModel-8.2.lib +vtkCommonExecutionModel-8.2.lib +vtkCommonMath-8.2.lib +vtkCommonMisc-8.2.lib +vtkCommonSystem-8.2.lib +vtkCommonTransforms-8.2.lib +vtkDICOMParser-8.2.lib +vtkDomainsChemistry-8.2.lib +vtkDomainsChemistryOpenGL2-8.2.lib +vtkdoubleconversion-8.2.lib +vtkexodusII-8.2.lib +vtkexpat-8.2.lib +vtkFiltersAMR-8.2.lib +vtkFiltersCore-8.2.lib +vtkFiltersExtraction-8.2.lib +vtkFiltersFlowPaths-8.2.lib +vtkFiltersGeneral-8.2.lib +vtkFiltersGeneric-8.2.lib +vtkFiltersGeometry-8.2.lib +vtkFiltersHybrid-8.2.lib +vtkFiltersHyperTree-8.2.lib +vtkFiltersImaging-8.2.lib +vtkFiltersModeling-8.2.lib +vtkFiltersParallel-8.2.lib +vtkFiltersParallelImaging-8.2.lib +vtkFiltersPoints-8.2.lib +vtkFiltersProgrammable-8.2.lib +vtkFiltersSelection-8.2.lib +vtkFiltersSMP-8.2.lib +vtkFiltersSources-8.2.lib +vtkFiltersStatistics-8.2.lib +vtkFiltersTexture-8.2.lib +vtkFiltersTopology-8.2.lib +vtkFiltersVerdict-8.2.lib +vtkfreetype-8.2.lib +vtkGeovisCore-8.2.lib +vtkgl2ps-8.2.lib +vtkglew-8.2.lib +vtkGUISupportMFC-8.2.lib +vtkhdf5-8.2.lib +vtkhdf5_hl-8.2.lib +vtkImagingColor-8.2.lib +vtkImagingCore-8.2.lib +vtkImagingFourier-8.2.lib +vtkImagingGeneral-8.2.lib +vtkImagingHybrid-8.2.lib +vtkImagingMath-8.2.lib +vtkImagingMorphological-8.2.lib +vtkImagingSources-8.2.lib +vtkImagingStatistics-8.2.lib +vtkImagingStencil-8.2.lib +vtkInfovisCore-8.2.lib +vtkInfovisLayout-8.2.lib +vtkInteractionImage-8.2.lib +vtkInteractionStyle-8.2.lib +vtkInteractionWidgets-8.2.lib +vtkIOAMR-8.2.lib +vtkIOAsynchronous-8.2.lib +vtkIOCityGML-8.2.lib +vtkIOCore-8.2.lib +vtkIOEnSight-8.2.lib +vtkIOExodus-8.2.lib +vtkIOExport-8.2.lib +vtkIOExportOpenGL2-8.2.lib +vtkIOExportPDF-8.2.lib +vtkIOGeometry-8.2.lib +vtkIOImage-8.2.lib +vtkIOImport-8.2.lib +vtkIOInfovis-8.2.lib +vtkIOLegacy-8.2.lib +vtkIOLSDyna-8.2.lib +vtkIOMINC-8.2.lib +vtkIOMovie-8.2.lib +vtkIONetCDF-8.2.lib +vtkIOParallel-8.2.lib +vtkIOParallelXML-8.2.lib +vtkIOPLY-8.2.lib +vtkIOSegY-8.2.lib +vtkIOSQL-8.2.lib +vtkIOTecplotTable-8.2.lib +vtkIOVeraOut-8.2.lib +vtkIOVideo-8.2.lib +vtkIOXML-8.2.lib +vtkIOXMLParser-8.2.lib +vtkjpeg-8.2.lib +vtkjsoncpp-8.2.lib +vtklibharu-8.2.lib +vtklibxml2-8.2.lib +vtklz4-8.2.lib +vtklzma-8.2.lib +vtkmetaio-8.2.lib +vtkNetCDF-8.2.lib +vtkogg-8.2.lib +vtkParallelCore-8.2.lib +vtkpng-8.2.lib +vtkproj-8.2.lib +vtkpugixml-8.2.lib +vtkRenderingAnnotation-8.2.lib +vtkRenderingContext2D-8.2.lib +vtkRenderingContextOpenGL2-8.2.lib +vtkRenderingCore-8.2.lib +vtkRenderingExternal-8.2.lib +vtkRenderingFreeType-8.2.lib +vtkRenderingGL2PSOpenGL2-8.2.lib +vtkRenderingImage-8.2.lib +vtkRenderingLabel-8.2.lib +vtkRenderingLOD-8.2.lib +vtkRenderingOpenGL2-8.2.lib +vtkRenderingVolume-8.2.lib +vtkRenderingVolumeOpenGL2-8.2.lib +vtksqlite-8.2.lib +vtksys-8.2.lib +vtktheora-8.2.lib +vtktiff-8.2.lib +vtkverdict-8.2.lib +vtkViewsContext2D-8.2.lib +vtkViewsCore-8.2.lib +vtkViewsInfovis-8.2.lib +vtkzlib-8.2.lib +``` + +- 设置如下编译选项，避免不必要的编译错误。（与`demo_online`一样，设置`_CRT_SECURE_NO_WARNINGS`选项） +![](./img/16_16_viewer_precompile_macro.png) + +``` +BOOST_USE_WINDOWS_H +NOMINMAX +_CRT_SECURE_NO_DEPRECATE +``` + +### 16.3.5 编译及运行 + +编译`demo_online`工程，并运行。 + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/17_how_to_avoid_packet_loss.md b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/17_how_to_avoid_packet_loss.md new file mode 100644 index 0000000..54bb352 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/17_how_to_avoid_packet_loss.md @@ -0,0 +1,68 @@ +# 17 **How to avoid Packet Loss** + + + +## 17.1 Overview + +This document illustrates when packet loss happens, and bring out a way to avoid that. + + + +## 17.2 Is there packet loss ? + +Run demo app`demo_online`, and check if it prints normal count of points. ++ Mechenical LiDARs should have a count close with its theoretical count. If it jump up and down, packet loss may happen. ++ M1 LiDAR should always have a count equal with its theoretical count, else packet loss happens. + +To observe multiple LiDARs case, open multiple terminals, and run multiple `demo_online`. Check if every LiDARs prints normally. + + + +## 17.3 In what cases Packet Loss happens + +Packet loss may happens in below cases. ++ on some platforms, such as Windows and embedded Linux ++ Multiple LiDARs ++ CPU resources is busy. + + + +## 17.4 Solution + +The solution is to increase the receiving buffer of MSOP Packet Socket. + +in `CMakeLists.txt`，CMake macro `ENABLE_MODIFY_RECVBUF` enable this feature. + +```cmake +option(ENABLE_MODIFY_RECVBUF "Enable modify size of RCVBUF" OFF) +``` + +The code is as below. Please test it in your cases, and change buffer size to a good value. + +```c++ +#ifdef ENABLE_MODIFY_RECVBUF + { + uint32_t opt_val = input_param_.socket_recv_buf, before_set_val,after_set_val = 0; + if(opt_val < 1024) + { + opt_val = 106496; + } + socklen_t opt_len = sizeof(uint32_t); + getsockopt(fd, SOL_SOCKET, SO_RCVBUF, (char*)&before_set_val, &opt_len); + RS_INFO << "before: recv buf opt_val:" < /proc/sys/net/core/rmem_max" +``` + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/17_how_to_avoid_packet_loss_CN.md b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/17_how_to_avoid_packet_loss_CN.md new file mode 100644 index 0000000..985c17e --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/17_how_to_avoid_packet_loss_CN.md @@ -0,0 +1,68 @@ +# **17 如何解决丢包问题** + + + +## 17.1 概述 + +本文分析丢包原因，给出解决丢包问题的方法。 + + + +## 17.2 如何确定是否丢包 + +运行示例程序`demo_online`，观察每帧的点数是否正常。 ++ 机械式雷达的每帧点数应该接近理论点数。如果有大幅波动，则可能是丢包了。 ++ M1雷达的每帧点数应该就是理论点数，是固定的。如果少了，则可能是丢包了。 + +要观察多雷达的情况，可以打开多个终端，分别运行多个`demo_online`的实例，看看每个雷达有没有丢包。 + + + +## 17.3 丢包原因 + +以下情况下可能丢包。 ++ 在某些平台上，如Windows和嵌入式Linux平台 ++ 多雷达的情况下 ++ 系统CPU资源紧张时 + + + +## 17.4 解决办法 + +解决丢包的办法是将接收MSOP Packet的Socket的接收缓存增大。 + +在`rs_driver`工程的`CMakeLists.txt`中，宏`ENABLE_MODIFY_RECVBUF`可以使能这个特性。 + +```cmake +option(ENABLE_MODIFY_RECVBUF "Enable modify size of RCVBUF" OFF) +``` + +代码如下。建议在实际场景下测试，再根据测试结果，将缓存大小调整为合适的值。 + +```c++ +#ifdef ENABLE_MODIFY_RECVBUF + { + uint32_t opt_val = input_param_.socket_recv_buf, before_set_val,after_set_val = 0; + if(opt_val < 1024) + { + opt_val = 106496; + } + socklen_t opt_len = sizeof(uint32_t); + getsockopt(fd, SOL_SOCKET, SO_RCVBUF, (char*)&before_set_val, &opt_len); + RS_INFO << "before: recv buf opt_val:" < /proc/sys/net/core/rmem_max" +``` + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/18_about_point_layout.md b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/18_about_point_layout.md new file mode 100644 index 0000000..d3eaa5e --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/18_about_point_layout.md @@ -0,0 +1,187 @@ + +# 18 **Point Type and Point Layout** + + + +## 18.1 Overview + +This document describes: + ++ the member variables of point and point cloud, and ++ what they really means + + + +## 18.2 Point And Point Cloud + +### 18.2.1 Point + +The member variables of point are as below. + +```c++ +struct PointXYZIRT +{ + float x; + float y; + float z; + uint8_t intensity; + uint16_t ring; + double timestamp; +}; +``` + +### 18.2.2 Point Cloud + +The member variables of point cloud are as below. Its member `points` is a `vector` of points. + +```c++ +template +class PointCloudT +{ +public: + typedef T_Point PointT; + typedef std::vector VectorT; + + uint32_t height = 0; ///< Height of point cloud + uint32_t width = 0; ///< Width of point cloud + bool is_dense = false; ///< If is_dense is true, the point cloud does not contain NAN points + double timestamp = 0.0; ///< Timestamp of point cloud + uint32_t seq = 0; ///< Sequence number of point cloud + + VectorT points; +}; +``` + + + +## 18.3 Member `ring` of Point + +### 18.3.1 Mechanical LiDAR + +In design of mechanical LiDAR, its channels are numbered by scan order. However, this order is not a ascending/descending order, so vertical angles of channels are not either. + +`rs_driver` sorts the channels by their vertical angels in ascending. The member `ring` is such a sorted number. +![](./img/18_01_mech_lasers.png) + + +### 18.3.2 MEMS LiDAR + +MEMS LiDAR scan 5 zone per scan, and get 5 points. Their timestamps are same. + +The member `ring` is the number of zone. + + +![](./img/18_02_mems_lasers.png) + + + +## 18.4 Point Layout + +### 18.4.1 Mechanical LiDAR + +Take RS32 as an example. + +It gets 32 points per scan, 1 point per channel. These 32 points is packed into a `Block`, and written into MSOP packet. + +After a round of scan, LiDAR get a point set on a loop surface. +![](./img/18_03_mech_lasers_and_points.png) + + + +MSOP packet consists of `Block`. `rs_driver` parses `Block`s one by one, and saves their points group by group. The points are in the member `points` of point cloud. LiDARs scans the channels, and `rs_driver` save their points in the same order. + +To traverse all points in a channel, just skip 32 points every time. +![](./img/18_04_mech_points.png) + +### 18.4.2 MEMS LiDAR + +MEMS LiDAR scans 5 zones per scan, and get 5 points, and packs them into a `Block`, write into MSOP packet. + +`rs_driver`parses `Block`s one by one, 5 points in a `Block`is saved as a group in the member `points` of point cloud. +![](./img/18_05_mems_lasers_and_points.png) + + + +Here is the point cloud in each zone. + +The zone is `126` lines x `125` columns. LiDAR scan from up to down in Z order. `rs_driver` same these points in the same order. +![](./img/18_06_mems_points.png) + + + +## 18.5 Coordinate of points + +`rs_driver`comply with the right-handed coordinate system. + +To change this, change mapping relationship of (x, y, z) as below. + +```c++ +//.../decoder/decoder_RSM1.hpp + + int pitch = ntohs(channel.pitch) - ANGLE_OFFSET; + int yaw = ntohs(channel.yaw) - ANGLE_OFFSET; + + float x = distance * COS (pitch) * COS (yaw); + float y = distance * COS (pitch) * SIN (yaw); + float z = distance * SIN (pitch); +``` + + + +## 18.6 Member `timestamp` of Point + +Mechanical LiDAR (600 rpm） and MEMS LiDAR finish a frame in 0.1 second. Its points scatter into this time range. + +To get higher resolution of timestamp than point cloud's, please use the point type of `XYZIRT`. `T` is the timestamp of point. + + + +## 18.7 member `timestamp` of Point Cloud + +`timestamp` of point cloud may be from from its first point or last point. + + + +### 18.7.1 From LiDAR, or From Host ? + +`rs_driver`may get `timestamp` of point cloud from LiDAR or from host. + ++ From MSOP packet. LiDAR set set the timestamp based on its own system clock. Generally, user synchronizes LiDAR's clock with the PTP protocol. ++ Invoke Operation System's API. With it, `rs_driver` gets the local system time. This is the default. + +Use the option `use_lidar_clock` to change this. + +```c++ +struct RSDecoderParam ///< LiDAR decoder parameter +{ + ... + bool use_lidar_clock = false; ///< true: use LiDAR clock as timestamp; false: use system clock as timestamp + ... +} +``` + +### 18.7.2 The first point, or The last point ? + +The default value is from the last point. + +Use the option `ts_first_point` to change this. + +```c++ +struct RSDecoderParam ///< LiDAR decoder parameter +{ + ... + bool ts_first_point = false; ///< true: time-stamp point cloud with the first point; false: with the last point; + ... +} +``` + +### 18.7.3 UTC time or Local time ? + +LiDAR writes UTC time into MSOP packets. + +In `CMakeLists.txt`, enable the macro `ENABLE_STAMP_WITH_LOCAL` to convert it to local time. + +```cmake +option(ENABLE_STAMP_WITH_LOCAL "Enable stamp point cloud with local time" OFF) +``` + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/18_about_point_layout_CN.md b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/18_about_point_layout_CN.md new file mode 100644 index 0000000..4133dcf --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/18_about_point_layout_CN.md @@ -0,0 +1,184 @@ + +# 18 **点云格式与点布局** + + + +## 18.1 概述 + +这里先回顾点及点云的定义，看看有哪些属性，再说明这些属性的确切含义。 + + + +## 18.2 点云格式 + +### 18.2.1 点 + +点的属性如下。这里列出的是`XYZIRT`类型，它包括了`XYZI`类型的所有属性。 + +```c++ +struct PointXYZIRT +{ + float x; + float y; + float z; + uint8_t intensity; + uint16_t ring; + double timestamp; +}; +``` + +### 18.2.2 定义点云类型 + +点云的属性如下。它的成员`points`是一个点的`vector`。 + +```c++ +template +class PointCloudT +{ +public: + typedef T_Point PointT; + typedef std::vector VectorT; + + uint32_t height = 0; ///< Height of point cloud + uint32_t width = 0; ///< Width of point cloud + bool is_dense = false; ///< If is_dense is true, the point cloud does not contain NAN points + double timestamp = 0.0; ///< Timestamp of point cloud + uint32_t seq = 0; ///< Sequence number of point cloud + + VectorT points; +}; +``` + + + +## 18.3 点的ring + +### 18.3.1 机械式雷达 + +机械式雷达的通道内部编号是按照激光扫描顺序排列的，但是这个扫描顺序并不是一直从下向上（或从上向下），所以按照这个编号的顺序，通道的垂直角不是递增（或递减）的。 + +`rs_driver`根据通道的垂直角，对它们从小到大（升序）重新编号。点的`ring`属性是这样重新排列后的编号。 +![](./img/18_01_mech_lasers.png) + + +### 18.3.2 MEMS雷达 + +MEMS雷达每轮扫描同时扫`5`个区域，得到`5`个点，这`5`个点有一样的时间戳。 + +点的`ring`是点所在区域的编号。 +![](./img/18_02_mems_lasers.png) + + + +## 18.4 点的布局 + +### 18.4.1 机械式雷达 + +这里以RS32雷达为例。 + +雷达每轮扫描得到`32`个点，每个通道一个点。这`32`个点打包到一个`Block`，写入MSOP Packet。 + +当雷达马达旋转一圈时，就得到环形面上一个点集合。 +![](./img/18_03_mech_lasers_and_points.png) + + + +`rs_driver`解析MSOP Packet时，按照`Block`的顺序解析。在点云的`points`数组中，每个`Block`的`32`个点连续保存为一组。 + +如果要顺序访问同一通道上的点，只需要每次跳过32个点就好了。 + +如前所述，在`Block`内，`32`个点的排列是按照扫描的顺序保存的，它们在`points`中排列也是这个顺序。 +![](./img/18_04_mech_points.png) + +### 18.4.2 MEMS雷达 + +MEMS雷达每轮扫描同时扫`5`个区域，得到`5`个点，打包到一个`Block`，写入MSOP Packet。 + +`rs_driver`解析MSOP Packet时，按照`Block`的顺序解析，所以在点云的`points`数组中，每个Block的`5`个点连续保存为一组。 +![](./img/18_05_mems_lasers_and_points.png) + + + +这里以M1为例，说明每个区域的点布局。 + +M1的每个区域`126`行 x `125`列，从上往下呈Z字型扫描。在点云的`points`数组中，点的排列也是这个顺序。 +![](./img/18_06_mems_points.png) + + + +## 18.5 点的坐标系 + +`rs_driver`输出的点遵循右手坐标系。 + +如果想改变这一点，可以改变雷达解码器`Decoder`的实现代码，改变坐标(x,y,z)的映射关系。如下是M1 Decoder的例子。 + +```c++ +//.../decoder/decoder_RSM1.hpp + + int pitch = ntohs(channel.pitch) - ANGLE_OFFSET; + int yaw = ntohs(channel.yaw) - ANGLE_OFFSET; + + float x = distance * COS (pitch) * COS (yaw); + float y = distance * COS (pitch) * SIN (yaw); + float z = distance * SIN (pitch); +``` + + + +## 18.6 点的timestamp + +机械式雷达（转速等于600圈/分钟时）和MEMS雷达扫描一帧的时间是100毫秒。点云中的点散布在这100毫秒的范围内。 + +如何想得到比点云时间戳更高精度的时间戳，请考虑使用`XYZIRT`点格式，它的`T`是点的时间戳。 + + + +## 18.7 点云的timestamp + +点云的时间戳可以来自第一个点，也可以来自最后一点。 + + + +### 18.7.1 雷达的时间，还是主机的时间？ + +`rs_driver`有两种方式得到点云的时间戳。 + ++ 从MSOP Packet解析。这个时间是雷达根据自身的时间设置。一般需要对雷达进行PTP时间同步，以保证雷达的时间与PTP Master保持一致。 ++ 调用操作系统的函数，得到`rs_driver`运行的这台主机的时间。这是`rs_driver`的默认配置。 + +`rs_driver`的选项`use_lidar_clock`可以改变这个配置。 + +```c++ +struct RSDecoderParam ///< LiDAR decoder parameter +{ + ... + bool use_lidar_clock = false; ///< true: use LiDAR clock as timestamp; false: use system clock as timestamp + ... +} +``` + +### 18.7.2 取第一个点的时间，还是最后一个点的？ + +默认情况下，取最后一个点的时间。 + +`rs_driver`的选项`ts_first_point`可以改变这个配置。 + +```c++ +struct RSDecoderParam ///< LiDAR decoder parameter +{ + ... + bool ts_first_point = false; ///< true: time-stamp point cloud with the first point; false: with the last point; + ... +} +``` + +### 18.7.3 UTC时间还是本地时间？ + +雷达写入MSOP Packet的时间是UTC时间。 + +`rs_driver`工程的`CMakeLists.txt`中的`ENABLE_STAMP_WITH_LOCAL`宏，可以根据本地的时区，将这个时间转换成本地时间。 + +```cmake +option(ENABLE_STAMP_WITH_LOCAL "Enable stamp point cloud with local time" OFF) +``` + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/19_about_splitting_frame.md b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/19_about_splitting_frame.md new file mode 100644 index 0000000..5fab6a2 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/19_about_splitting_frame.md @@ -0,0 +1,121 @@ +# 19 **Splitting Rule** + + + +## 19.1 Overview + +This document illustrates how RoboSense LiDAR splits frame. Mechanical LiDARs and MEMS LiDARs have different splitting rules. + +This document imports some concepts from below documents. + +[How_to avoid packet loss](./17_how_to_avoid_packet_loss.md) + +[About point layout](./18_about_point_layout.md) + + + +## 19.2 Mechanical LiDAR + +Mechanical LiDAR rotates and sends out points. `rs_driver` splits them by an angle. Everytime`rs_driver` gets a frame. This angle's default value is `0`°. User may change it with the option `RSDecoderParam.split_angle`. +![split angle](./img/19_01_split_angle.png) + +For per scan, LiDAR gets a group of points, one point per channel. `rs_driver` packs them into a `Block`, and writes it into MSOP packet. + +`Block` holds the angle. So called "splitting frame", is to check if the angle across the splitting angle. If so, this group belongs to the next frame, and the older groups belongs to the previous one. + +### 19.2.1 Deviation of Splitting by angle + +`Block`'s angle is from its first point, and for latter points, need to plus a offset. These points may across the splitting angle. Around this angle, points may lose or repeat. + +Let's estimate the angle span of `Block`. + +With `600` rpm, seconds per round is: + +```c++ +1 / (600 / 60) = 0.1 (second) +``` + +Generally, a scan takes 50~60 us. Take RS16 as an example, it takes `55.5` us. scans per round is: + +```c++ + 0.1 * 1000000 / 55.5 = 1801.8 +``` + +Then the angle span of `Block` is: + +```c++ +360 / 1801.8 = 0.1998 (degree) +``` + +Then the deviation by angle is `0.2` degree. + +### 19.2.2 Deviation of Splitting by time + +Seconds per scans is `50`~`60` us. This is the deviation by time. + +### 19.2.3 points per frame vary + +Take RS16 as an example. + +With `600` rpm, it takes `1801.8` scans per round. Round up to `1802` times. Then points per frame is: + +``` +16 * 1802 = 28,832 (points） +``` + +LiDAR doesn't rotate smoothly. takes more scans if faster, and takes less less if slower. + +The difference is the laser number of LiDAR. For RS16, it is `16`. + +### 19.2.4 Packet Loss and Out of Order + +Mechanical LiDAR sends MSOP packets smoothly. The time interval between two packets is enough, so packet loss and out of order is rare. + + + +## 19.3 MEMS LiDAR + +How MEMS LiDAR splits frame, is actually determined on the LiDAR end. + +It scans `5` zones, gets 5 points per scan, packs into a `Block`, and writes into MSOP packet. + +In each zone, scan in the Z order. And then get a group of MSOP packets. + +Number these packets from 1. Take M1 as an example, the numbers are `1`~`630`. + +What left for `rs_driver` to do, is to split packets by the numbers. + +### 19.3.1 Packet Loss and Out of Order + +MEMS LiDAR may send multiple MSOP packets at the same time, so packet loss and out of order may happen. + +Increase socket buffer is useful if packet loss happens on host, but make no help if it happens between LiDARs and host. + +`rs_driver` use `Safe Range` to handle this. + + + +First image how to handle this without `Safe Range`. + ++ No packet loss or out of order. Just check if the packet number is `1`. Split if so. ++ Packet loss, no out of order. For example, packet `1` is losed. Just add check if packet number is less than the previous. Split if so. ++ Out of order. For example, packet `301` reaches before packet `300`, then this frame is splitted into two. Dilemma. + + + +Consider `Safe Range` as below. + ++ Give a range `RANGE` around `prev_seq`, which is the previous packet number. + +```c++ +safe_seq_min = prev_seq_ - RANGE +safe_seq_max = prev_seq_ + RANGE +``` + ++ If MSOP packet number is in (`safe_seq_min_`, `safe_seq_max_`), accept it and don't split. +![safe range](./img/19_02_safe_range.png) + +Slight packet loss and out of order is tolerated. + + + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/19_about_splitting_frame_CN.md b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/19_about_splitting_frame_CN.md new file mode 100644 index 0000000..0554ad7 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/19_about_splitting_frame_CN.md @@ -0,0 +1,121 @@ +# 19 **分帧策略** + + + +## 19.1 概述 + +本文分别说明机械式雷达和MEMS的分帧策略。因为工作原理不同，所以它们的分帧策略也不同。 + +本文引用了如下一些文档的概念，请阅读。 + +[如何解决丢包问题](./17_how_to_avoid_packet_loss_CN.md) + +[点在点云中的布局](./18_about_point_layout_CN.md) + + + +## 19.2 机械式雷达 + +机械式雷达绕轴持续`360`°旋转扫描，并输出点，`rs_driver`按指定角度分割这些点，每一次分割得到一圈点云，也就是一帧。 + +这个分帧角度的默认值为`0`°。`rs_driver`的使用者，可以通过配置选项`RSDecoderParam.split_angle`指定这个角度。 +![split angle](./img/19_01_split_angle.png) + +每一轮扫描得到一组点，每个通道一个点，打包到一个`Block`，写入MSOP Packet中。`Block`保存这一组点的水平角。 + +所谓分帧，就是检查这个水平角是不是跨过了分帧角度，如果跨过了，这一组点就属于下一帧，而之前的点就可以打包到上一帧了。 + +### 19.2.1 分帧的水平角误差 + +`Block`的水平角是它第一个点的时间戳。由于雷达一直在转，后面点的水平角还要在这个基础上加上一个偏移值。这样它们就可能跨过分帧角度，进而导致点云在分帧角度附近重复点或遗漏点。 + +这里对`Block`跨过的水平角度估算一下，看看影响有多大。 + +如果转速为`600`圈/分钟，那么每圈的时间为： + +```c++ +1 / (600 / 60) = 0.1 (秒) +``` + +机械式雷达每轮扫描之间的时间差一般是`50`~`60`微秒。以RS16为例，它的扫描间隔为`55.5`微秒。那么一圈的扫描轮数是： + +```c++ + 0.1 * 1000000 / 55.5 = 1801.8 (次) +``` + +`Block`的水平角跨度为： + +```c++ +360 / 1801.8 = 0.1998 (度) +``` + +这就是说，分帧角度附近重复或遗漏点的最大水平角跨度大约是`0.2`度。 + +### 19.2.2 分帧的时间误差 + +机械式雷达每轮扫描之间的时间差一般是`50`~`60`微秒。这也就是分帧的时间误差。 + +### 19.2.3 每帧的点数是波动的 + +以RS16为例。在`600`圈/分钟的情况下，一圈扫描`1801.8`次，取整`1802`次，那就有 + +```c++ +16 * 1802 = 28832 (点） +``` + +但是雷达马达的旋转并不是这么匀速。转得快一点，每圈就少扫描几轮，慢一点，每圈就多扫描几轮。 + +因为每轮扫描的点数是雷达的线数，所以少/多的点数是雷达线数的整数倍。对于RS16，就是`16`的整数倍。 + +### 19.2.4 丢包与乱序处理 + +机械式雷达均匀发送MSOP Packet，每次发送一个Packet，两个Packet的时间间隔较大，所以暂时没有发现丢包与乱序需要处理。 + + + +## 19.3 MEMS雷达 + +MEMS雷达的分帧，本质上在雷达端就确定了。 + +雷达有`5`个扫描区域，每轮扫秒同时扫描这`5`个区域，得到`5`个点，打包到一个`Block`，写入MSOP Packet。 + +在每个区域，按照Z字型进行扫描。这样一直继续，直到完成全部区域，得到一组MSOP Packet。 + +这组Packet按扫描顺序从1开始编号。以M1雷达为例，编号为`1`~`630`。 + +最后`rs_driver`做的，是按照这些编号分割MSOP Packet。 + +### 19.3.1 丢包与乱序处理 + +MEMS雷达可能同时发送多个MSOP Packet，这样丢包和乱序的风险就变大了。 + +主机端的丢包，可以通过增大Socket接收缓存解决，但是从雷达到主机之间的丢包、乱序还是可能存在。 + +`rs_driver`引入`安全区间`的做法，来处理可能的丢包和乱序。 + + + +先讨论没有`安全区间`时，如何处理丢包、乱序。 + ++ 理想情况下，如果不丢包不乱序，MSOP Packet编号从`1`到`630`, 只需要检查Packet编号是不是`1`。如果是就分帧。 ++ 那假如只有丢包呢？举个例子，如果编号为`1`的Packet丢了，则可以加入检查条件，就是当前Packet编号小于前一个Packet的编号`prev_seq`，就分帧。 ++ 在乱序的情况下，这个检查条件会导致另一个困境。举个例子，如果编号为`300`和`301`的两个Packet乱序，那么这个位置分帧，会导致原本的一帧拆分成两帧。 + + + +`安全区间`的做法如下： + ++ 以`prev_seq`为参考点，划定一个范围值`RANGE`, + +```c++ +safe_seq_min = prev_seq_ - RANGE +safe_seq_max = prev_seq_ + RANGE +``` + ++ 如果MSOP Packet在范围(`safe_seq_min_`, `safe_seq_max_`)内，不算异常，不分帧。 +![safe range](./img/19_02_safe_range.png) + +这样轻微的乱序不会触发分帧，也就解决了前面说的困境。 + + + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/20_about_usage_of_cpu_and_memory.md b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/20_about_usage_of_cpu_and_memory.md new file mode 100644 index 0000000..b2975ca --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/20_about_usage_of_cpu_and_memory.md @@ -0,0 +1,131 @@ +# 20 **CPU Usage and Memory Usage** + + + +## 20.1 Overview + +This document illustrates CPU usage and memory usage of `rs_driver`. + +Please first read [thread model and interface](../intro/03_thread_model.md), since this document imports some concept from it. +![](./img/20_01_components_and_threads.png) + + + +## 20.2 CPU Usage + +### 20.2.1 What cost CPU resources ? + +CPU usage is mainly from two factors. + ++ The handing thread `process_thread` parses MSOP/DIFOP Packet, and creates point cloud. ++ While handling MSOP/DIFOP packets, `process_thread` loops in the routine of `waken` ->`sleep` ->`waken`. + + This routine cost much CPU resource, because MSOP packets are frequent but not continuous. `process_thread` switches too many times. + + + +### 20.2.2 Solution + +CMake macro `ENABLE_WAIT_IF_QUEUE_EMPTY` may lower CPU usage, but increase the latency of point cloud. + +Actually it make `process_thread` sleep a little longer, so the thread handle more packets every time, and switches less. + + +```cmake +option(ENABLE_WAIT_IF_QUEUE_EMPTY "Enable waiting for a while in handle thread if the queue is empty" OFF) +``` + +With this option enabled, `process_thread` calls usleep() instead of waiting for condition_variable. + +Based on a test on a specific platform, this macro lowers CPU usage from `25%` to `22%`. + +```c++ +#ifdef ENABLE_WAIT_IF_QUEUE_EMPTY + + ... + + std::this_thread::sleep_for(std::chrono::microseconds(1000)); + return value; +#else + + ... + + std::unique_lock ul(mtx_); + cv_.wait_for(ul, std::chrono::microseconds(usec), [this] { return (!queue_.empty()); }); + + ... + + return value; +#endif +``` + + + +Coins have two sides. The disadvantage is: + +usleep() is not definite. If sleep() acrosses the splitting point, such as + ++ Mechanical LiDAR, `Block`'s angle across the splitting angle ++ MEMS LiDAR, For M1, packet number across `630` + + +splitting is delayed, and then user "see" this frame later. + + + +## 20.3 Memory Usage + +### 20.3.1 MSOP/DIFOP Packet Queue + +MSOP/DIFOP Packet queue: `free_pkt_queue` and `stuffed_pkt_queue`. + + + Packets in these queues is allocated on demand. The count depends on LiDAR type and user case. + + `rs_driver` never releases these packets. + +### 20.3.2 Point Cloud Queue + +Point cloud is allocated by user. It is not discussed here. + +### 20.3.3 Calculate sin()/cos() by table + ++ Mechanical LiDAR calculates sin()/cos() by table. Class `Trigon` wrap this work. It is a member of class `Decoder`. + +```c++ +template +class Decoder +{ + ... + Trigon trigon_; + ... +}; +``` + +`Trigon`Calculates sin/cos values in the range (-90, 450), with unit `0.01`degree. The size of two arrays is: + +```c++ +(450 - (-90)) / 0.01 * sizeof(float) * 2 = 432,000 (bytes) +``` + ++ MEMS LiDAR + + M1 use tables and `Trigon` too. It takes same size of memory as mechanical LiDAR does. + + M2 doesn't need tables. No memory usage for this. + ++ To connect to multiple LiDARs, each `rs_driver` instance has its own `Trigon` instance. Each instance occupies `432,000` bytes. + +### 20.3.4 Socket Receiving Buffer + +A hidden memory usage is Socket Receiving Buffer。 ++ Each of MSOP/DIFOP Packets has their own sockets. ++ It may be increased in case of packet loss + + + + + + + + + + + + + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/20_about_usage_of_cpu_and_memory_CN.md b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/20_about_usage_of_cpu_and_memory_CN.md new file mode 100644 index 0000000..fe3eda1 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/20_about_usage_of_cpu_and_memory_CN.md @@ -0,0 +1,121 @@ +# 20 **CPU占用与内存占用** + +## 20.1 概述 + +本文说明`rs_driver`对CPU和内存的使用。 + +请先阅读[rs_driver的线程模型与接口设计](../intro/03_thread_model_CN.md)，因为本文引用了其中的概念。 +![](./img/20_01_components_and_threads.png) + + + +## 20.2 CPU占用 + +### 20.2.1 CPU占用的来源 + +`rs_driver`的CPU占用主要来自两个方面： + ++ MSOP/DIFOP Packet的处理线程`process_thread`解析MSOP/DIFOP Packet，构建点云 ++ `process_thread`等待MSOP/DIFOP Packet时，反复被`唤醒` ->`睡眠` ->`唤醒`。 + + 这个过程CPU占用率较高，原因是Packet是频率高但不连续的，`process_thread`切换状态的次数太多了。 + + + +### 20.2.2 降低CPU占用率的办法 + +CMake编译宏 ENABLE_WAIT_IF_QUEUE_EMPTY 可以让`rs_driver`的CPU占用率降低一点，但是弊端是点云延迟会加大。 + +它的原理是让`process_thread`每次睡眠的时间长一点，这样每次唤醒时处理的包数就多一些，反复`唤醒` ->`睡眠` ->`唤醒`的次数就少一些。 + + +```cmake +option(ENABLE_WAIT_IF_QUEUE_EMPTY "Enable waiting for a while in handle thread if the queue is empty" OFF) +``` + + + +打开这个宏后，Packet处理线程会调用usleep()，而不是等待条件变量通知。 + +在某平台的测试结果表明，这个宏可以将CPU占用率从`25%`降低到`22%`。 + +```c++ +#ifdef ENABLE_WAIT_IF_QUEUE_EMPTY + + ... + + std::this_thread::sleep_for(std::chrono::microseconds(1000)); + return value; +#else + + ... + + std::unique_lock ul(mtx_); + cv_.wait_for(ul, std::chrono::microseconds(usec), [this] { return (!queue_.empty()); }); + + ... + + return value; +#endif +``` + + + +这里也有必要说明这样做的弊端。 + +对于usleep()的等待时间是不确定的。如果等待跨过了点云的分帧点，比如 + ++ 机械式雷达，MSOP Packet的Block水平角跨过了分帧角度 ++ MEMS雷达，比如M1的MSOP Packet序列号跨过了`630` + + +这样分帧就会拖后，`rs_driver`的使用者“看”到这一帧的时间点就会推迟。 + +所以请您根据自己的场景，权衡CPU占用和点云延迟之间的利弊，再决定是否使能这个编译宏。 + + + +## 20.3 内存占用 + +### 20.3.1 MSOP/DIFOP Packet队列 + +`rs_driver`的内存占用主要来自MSOP/DIFOP Packet队列 `free_pkt_queue`和`stuffed_pkt_queue`。 + + + 这两个队列中的Packet实例是按需分配的，分配的数量与雷达类型、使用场景相关。 + + 在`rs_driver`的实现中，它不会释放这些实例。如果有偶然的意外发生，导致Packet实例的数量异常增多，那么后面它们所占用的内存也不会降低。 + +### 20.3.2 点云队列 + +`rs_driver`使用的点云实例是调用者分配、管理的，所以这里不讨论点云占用的内存。 + +### 20.3.3 查表方式计算三角函数值 + ++ 机械式雷达使用查表的方式计算三角函数值。`Trigon`类负责做这件事，它是解码器`Decoder`的成员。 + +```c++ +template +class Decoder +{ + ... + Trigon trigon_; + ... +}; +``` + +`Trigon`计算(-90, 450)角度范围内的`sin`和`cos`值，粒度是`0.01`度，所以保存它们的两个数组大小为： + +```c++ +(450 - (-90)) / 0.01 * sizeof(float) * 2 = 432,000 (字节) +``` + ++ 关于MEMS雷达， + + M1也需要使用查表方式计算三角函数值，它也使用`Trigon`类，占用内存大小与机械式雷达相同。 + + M2不需要计算三角函数值，也就不需要占用这部分内存。 + ++ 如果连接多个雷达，`rs_driver`的多个实例各有自己的`Trigon`实例。也就是每个实例都占用`432,000`字节。 + +### 20.3.4 Socket接收缓存 + +一个比较隐蔽的内存占用，是接收MSOP/DIFOP Packet的Socket的接收缓存。 ++ MSOP Packet和DIFOP Packet一般各有自己接收的Socket ++ 在丢包的情况下，可能需要增加Socket接收缓存的大小 + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/21_how_to_parse_difop.md b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/21_how_to_parse_difop.md new file mode 100644 index 0000000..73b0ebc --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/21_how_to_parse_difop.md @@ -0,0 +1,363 @@ +# 21 **How to Parse DIFOP packet** + + + +## 21.1 Overview + +`rs_driver` is supposed to parse MSOP/DIFOP packets and get point cloud. It gets the coordinate data from the MSOP packets, and gets only only some calibration parameters from the DIFOP packets. + ++ For mechanical LiDARs, DIFOP contains data about angles and wave mode. ++ For MEMS LiDARs, DIFOP is not needed, and MSOP is enough. + +Furthermore, the DIFOP packet also contains some configuration data and status data of the LiDAR device. `rs_dirver` seems to be a good candidate to parse them. + +Unfortunately, the format of DIFOP packet is different between different types of LiDARs and different projects. It is very hard for `rs_driver` to give a common and complete implementation. + +The compromise scheme is: `rs_driver` parses only some common fields from DIFOP, and users folllow these examples to add other user-specific fields. + + +This document illustrates how to change `rs_driver` to parse new fields. + + + +## 21.2 Extend the definition of DIFOP packet + +This definition is in the header file of the decoder. User should extend it, and even replace it with a new one. + +For RS128, The definition is in `decoder_RS128.hpp`. + +```c++ +// decoder_RS128.hpp + +typedef struct +{ + uint8_t id[8]; + uint16_t rpm; + RSEthNetV2 eth; + RSFOV fov; + uint8_t reserved1[2]; + uint16_t phase_lock_angle; + RSVersionV2 version; + uint8_t reserved2[229]; + RSSN sn; + ...... + +} RS128DifopPkt; +``` + +For RSM1, the definition is in `decoder_RSM1.hpp`. + +```c++ +// decoder_RSM1.hpp + +typedef struct +{ + uint8_t id[8]; + uint8_t reserved1[1]; + uint8_t frame_rate; + RSM1DifopEther eth; + RSM1DifopFov fov; + RSM1DifopVerInfo version; + RSSN sn; + ...... + +} RSM1DifopPkt; +``` + + + +## 21.3 Extend DeviceInfo and DeviceStatus + +`rs_driver` saves the parsing result in two structures: `DeviceInfo` and `DeviceStatus`. They are in `driver_param.hpp`. ++ `DeviceInfo` is supposed to save configuration data. It is stable in general. ++ `DeviceStatus` is supposed to save status data. It is variable. + +```c++ +// driver_param.hpp + +struct DeviceInfo +{ + DeviceInfo() + { + init(); + } + bool state; + uint8_t sn[6]; + uint8_t mac[6]; + uint8_t top_ver[5]; + uint8_t bottom_ver[5]; + + float qx{0.0f}; + float qy{0.0f}; + float qz{0.0f}; + float qw{1.0f}; + float x{0.0f}; + float y{0.0f}; + float z{0.0f}; + + void init() + { + memset(sn, 0, sizeof(sn)); + memset(mac, 0, sizeof(mac)); + memset(top_ver, 0, sizeof(top_ver)); + memset(bottom_ver, 0, sizeof(bottom_ver)); + qx = 0.0f; + qy = 0.0f; + qz = 0.0f; + qw = 1.0f; + x = 0.0f; + y = 0.0f; + z = 0.0f; + state = false; + } + + DeviceInfo& operator=(const DeviceInfo& other) + { + if (this != &other) + { + memcpy(sn, other.sn, sizeof(sn)); + memcpy(mac, other.mac, sizeof(mac)); + memcpy(top_ver, other.top_ver, sizeof(top_ver)); + memcpy(bottom_ver, other.bottom_ver, sizeof(bottom_ver)); + qx = other.qx; + qy = other.qy; + qz = other.qz; + qw = other.qw; + x = other.x; + y = other.y; + z = other.z; + state = other.state; + } + return *this; + } +}; + +struct DeviceStatus +{ + DeviceStatus() + { + init(); + } + float voltage = 0.0f; + bool state; + + void init() + { + voltage = 0.0f; + state = false; + } + + DeviceStatus& operator=(const DeviceStatus& other) + { + if (this != &other) + { + voltage = other.voltage; + state = other.state; + } + return *this; + } +}; +``` + +`Decoder` is the base class of all LiDAR decoders. Its members, `device_info_` and `device_status_`, saves configuration data and status data. + +```c++ +template +class Decoder +{ +public: + + ...... + + DeviceInfo device_info_; + DeviceStatus device_status_; + + ...... +}; +``` + + + +## 21.4 Parse DIFOP packet + +The Decoder parses DIFOP packets and save the result into the members of `Decoder`: `device_info_` and `device_status_`. + +For RSM1，the member of `DecoderRSM1::decodeDifopPkt()`, parses DIFOP packets. + +This function is a virtual function. It is defined in the base class `Decoder`. + +```c++ +//decoder_RSM1.hpp + +template +inline void DecoderRSM1::decodeDifopPkt(const uint8_t* packet, size_t size) +{ + const RSM1DifopPkt& pkt = *(RSM1DifopPkt*)packet; + this->echo_mode_ = this->getEchoMode(pkt.return_mode); + +#ifdef ENABLE_DIFOP_PARSE + // device info + memcpy (this->device_info_.sn, pkt.sn.num, 6), + memcpy (this->device_info_.mac, pkt.eth.mac_addr, 6), + memcpy (this->device_info_.top_ver, pkt.version.pl_ver, 5), + memcpy (this->device_info_.bottom_ver, pkt.version.ps_ver, 5), + + this->device_info_.state = true; + // device status + this->device_status_.voltage = ntohs(pkt.status.voltage_1); + this->device_status_.state = true; +#endif +} +``` + +For mechanical LiDARs, follow one of these two ways. + ++ One is to parse in the specific Decoder. For RS128, it is `DecoderRSM1::decodeDifopPkt()`. + +```c++ +template +inline void DecoderRS128::decodeDifopPkt(const uint8_t* packet, size_t size) +{ + const RS128DifopPkt& pkt = *(const RS128DifopPkt*)(packet); + this->template decodeDifopCommon(pkt); + + this->echo_mode_ = getEchoMode (pkt.return_mode); + this->split_blks_per_frame_ = (this->echo_mode_ == RSEchoMode::ECHO_DUAL) ? + (this->blks_per_frame_ << 1) : this->blks_per_frame_; + +#ifdef ENABLE_DIFOP_PARSE + // add code to parse RS128-specific fields +#endif +} +``` + ++ If the field is common for all mechanical LiDARS, better parse in `DecoderMech::decodeDifopCommon()`. The specific decoder will call it. + +Note this is a template function, and `T_Difop` is its template parameter. + +```c++ +template +template +inline void DecoderMech::decodeDifopCommon(const T_Difop& pkt) +{ + + ...... + + // load angles + if (!this->param_.config_from_file && !this->angles_ready_) + { + int ret = this->chan_angles_.loadFromDifop(pkt.vert_angle_cali, pkt.horiz_angle_cali); + this->angles_ready_ = (ret == 0); + } + +#ifdef ENABLE_DIFOP_PARSE + // add code to parse fields which are common to all mechanical LiDARs + + // device info + memcpy (this->device_info_.sn, pkt.sn.num, 6), + memcpy (this->device_info_.mac, pkt.eth.mac_addr, 6), + memcpy (this->device_info_.top_ver, pkt.version.top_ver, 5), + memcpy (this->device_info_.bottom_ver, pkt.version.bottom_ver, 5), + + this->device_info_.state = true; + // device status + this->device_status_.voltage = ntohs(pkt.status.vol_12v); + this->device_status_.state = true; +#endif +} +``` + + + +## 21.5 Read configuration data and status data + +User creates an instance of `LidarDriver`, and call its member funcitons, `getDeviceInfo()` and `getDeviceStatus()`, to get configuration status data. + +As describes above, the members of `Decoder`, `device_info_` and `device_status_` save them. + +Note user need to check the result value of these functions to confirm the validation of the data. This is to say, whether DIFOP is parsed successfully already or not. + +```c++ +template +class LidarDriver +{ +public: + + ...... + + inline bool getDeviceInfo(DeviceInfo& info) + { + return driver_ptr_->getDeviceInfo(info); + } + + inline bool getDeviceStatus(DeviceStatus& status) + { + return driver_ptr_->getDeviceStatus(status); + } + + ...... +}; +``` + + + +## 21.6 Enable ENABLE_DIFOP_PARSE + +In general, User needs only point cloud, and avoids parsing DIFOP packets. + +CMake compilation option `ENABLE_DIFOP_PARSE` specify whether to parse them. If so, enable it. + +This option is in `CMakeLists.txt`. + + +```cmake +CMakeLists.txt +option(ENABLE_DIFOP_PARSE "Enable DIFOP Packet Parse" OFF) +``` + + + +## 21.7 Special steps for RS16/RS32 + +RS16/RS32 are the earliest LiDARs. Their packet formats are different with other LiDARS. + +To adapt to `DecoderMech::decodeDifopCommon()`，`rs_driver` gives a common definition of DIFOP packet: `AdapterDifopPkt`. + +```c++ +typedef struct +{ + uint16_t rpm; + RSEthNetV1 eth; + RSFOV fov; + RSVersionV1 version; + RSSN sn; + uint8_t return_mode; + RSStatusV1 status; + RSCalibrationAngle vert_angle_cali[32]; + RSCalibrationAngle horiz_angle_cali[32]; +} AdapterDifopPkt; +``` + +Decoders of RS16/RS32 first copy the fields from their own packet to the instance of `AdapterDifopPkt`, and dispatch it to `DecoderMech::decodeDifopCommon()`. + +To add new fields for RS16/RS32. + ++ First extend the definition of `AdapterDifopPkt`. + ++ Copy these new fields to the instance of `AdapterDifopPkt`. For RS16, do this in `RS16DifopPkt2Adapter()`. + +```c++ +inline void RS16DifopPkt2Adapter (const RS16DifopPkt& src, AdapterDifopPkt& dst) +{ + dst.rpm = src.rpm; + dst.fov = src.fov; + dst.return_mode = src.return_mode; + dst.sn = src.sn; + dst.eth = src.eth; + dst.version = src.version; + dst.status = src.status; + + ...... +} +``` + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/21_how_to_parse_difop_CN.md b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/21_how_to_parse_difop_CN.md new file mode 100644 index 0000000..4125a14 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/21_how_to_parse_difop_CN.md @@ -0,0 +1,360 @@ +# 21 **如何解析DIFOP中的配置和状态数据** + + + +## 21.1 概述 + +`rs_driver`的首要目标是解析MSOP/DIFOP包，得到点云。其中MSOP包含点的坐标数据，DIFOP包只包含构造点云的一些参数。 + ++ 对于机械式雷达，DIFOP包含用于标定的角度数据，和回波模式。 ++ 对于MEMS雷达，MSOP包就够了，DIFOP包其实是不需要的。 + +除构造点云需要的数据之外，DIFOP还包含了一些雷达的配置和状态数据，`rs_driver`似乎是解析这些数据的合适的地方。 + +遗憾的是，不同类型的雷达，DIFOP格式一般不同，甚至同一类型的雷达，不同项目也可能定制自己的格式，这样`rs_driver`就很难给出一个通用而又完善的实现。所以`rs_driver`只能给出一个折中的方案：它针对DIFOP包中若干通用的字段进行解析。使用者可以根据这些范例来解析其他需要的字段。 + +本文描述如何一步步修改`rs_driver`，支持解析新的字段。 + + + +## 21.2 拓展DIFOP包的定义 + +DIFOP包的定义在雷达解码器的头文件中。 + +如果客户定制了自己的DIFOP包格式，则需要拓展原有的定义、甚至将这个定义替换成自己的。 + +以RS128雷达为例，这个定义在`decoder_RS128.hpp`中。 + +```c++ +// decoder_RS128.hpp + +typedef struct +{ + uint8_t id[8]; + uint16_t rpm; + RSEthNetV2 eth; + RSFOV fov; + uint8_t reserved1[2]; + uint16_t phase_lock_angle; + RSVersionV2 version; + uint8_t reserved2[229]; + RSSN sn; + ...... + +} RS128DifopPkt; +``` + +对于RSM1雷达，这个定义在`decoder_RSM1.hpp`中。 + +```c++ +// decoder_RSM1.hpp + +typedef struct +{ + uint8_t id[8]; + uint8_t reserved1[1]; + uint8_t frame_rate; + RSM1DifopEther eth; + RSM1DifopFov fov; + RSM1DifopVerInfo version; + RSSN sn; + ...... + +} RSM1DifopPkt; +``` + + + +## 21.3 扩充DeviceInfo和DeviceStatus + +`rs_driver`将解析后的结果保存在两个结构`DeviceInfo`和`DeviceStatus`中，它们定义在`driver_param.hpp`中。 + +DIFOP包中包含配置和状态数据。 + ++ `DeviceInfo`这个结构保存配置数据，这部分一般是不变的。 ++ `DeviceStatus`是状态数据，这部分是变化的。 + +```c++ +// driver_param.hpp + +struct DeviceInfo +{ + DeviceInfo() + { + init(); + } + bool state; + uint8_t sn[6]; + uint8_t mac[6]; + uint8_t top_ver[5]; + uint8_t bottom_ver[5]; + + float qx{0.0f}; + float qy{0.0f}; + float qz{0.0f}; + float qw{1.0f}; + float x{0.0f}; + float y{0.0f}; + float z{0.0f}; + + void init() + { + memset(sn, 0, sizeof(sn)); + memset(mac, 0, sizeof(mac)); + memset(top_ver, 0, sizeof(top_ver)); + memset(bottom_ver, 0, sizeof(bottom_ver)); + qx = 0.0f; + qy = 0.0f; + qz = 0.0f; + qw = 1.0f; + x = 0.0f; + y = 0.0f; + z = 0.0f; + state = false; + } + + DeviceInfo& operator=(const DeviceInfo& other) + { + if (this != &other) + { + memcpy(sn, other.sn, sizeof(sn)); + memcpy(mac, other.mac, sizeof(mac)); + memcpy(top_ver, other.top_ver, sizeof(top_ver)); + memcpy(bottom_ver, other.bottom_ver, sizeof(bottom_ver)); + qx = other.qx; + qy = other.qy; + qz = other.qz; + qw = other.qw; + x = other.x; + y = other.y; + z = other.z; + state = other.state; + } + return *this; + } +}; + +struct DeviceStatus +{ + DeviceStatus() + { + init(); + } + float voltage = 0.0f; + bool state; + + void init() + { + voltage = 0.0f; + state = false; + } + + DeviceStatus& operator=(const DeviceStatus& other) + { + if (this != &other) + { + voltage = other.voltage; + state = other.state; + } + return *this; + } +}; +``` + +`Decoder`是所有雷达解码器的基类。它的成员`device_info_`和`device_status_`分别保存了配置和状态数据。 + +```c++ +template +class Decoder +{ +public: + + ...... + + DeviceInfo device_info_; + DeviceStatus device_status_; + + ...... +}; +``` + + + +## 21.4 解析DIFOP包 + +雷达解码器负责解码DIFOP包，并将结果保存到`Decoder`的成员`device_info_`和`device_status_`中。 + +对于RSM1，`DecoderRSM1`的成员函数`decodeDifopPkt()`负责解析DIFOP包。 + +这个函数是一个虚拟函数，在基类`Decoder`中定义。 + +```c++ +//decoder_RSM1.hpp + +template +inline void DecoderRSM1::decodeDifopPkt(const uint8_t* packet, size_t size) +{ + const RSM1DifopPkt& pkt = *(RSM1DifopPkt*)packet; + this->echo_mode_ = this->getEchoMode(pkt.return_mode); + +#ifdef ENABLE_DIFOP_PARSE + // device info + memcpy (this->device_info_.sn, pkt.sn.num, 6), + memcpy (this->device_info_.mac, pkt.eth.mac_addr, 6), + memcpy (this->device_info_.top_ver, pkt.version.pl_ver, 5), + memcpy (this->device_info_.bottom_ver, pkt.version.ps_ver, 5), + + this->device_info_.state = true; + // device status + this->device_status_.voltage = ntohs(pkt.status.voltage_1); + this->device_status_.state = true; +#endif +} +``` + +对于机械式雷达，有两个选择。 + ++ 一个选择是在对应的解码器类的虚拟成员函数`decodeDifopPkt()`中解析。如RS128，就是`DecoderRSM1::decodeDifopPkt()`。 + +```c++ +template +inline void DecoderRS128::decodeDifopPkt(const uint8_t* packet, size_t size) +{ + const RS128DifopPkt& pkt = *(const RS128DifopPkt*)(packet); + this->template decodeDifopCommon(pkt); + + this->echo_mode_ = getEchoMode (pkt.return_mode); + this->split_blks_per_frame_ = (this->echo_mode_ == RSEchoMode::ECHO_DUAL) ? + (this->blks_per_frame_ << 1) : this->blks_per_frame_; + +#ifdef ENABLE_DIFOP_PARSE + // add code to parse RS128-specific fields +#endif +} +``` + ++ 如果解析的字段是所有机械式雷达都有的，那么另一个更好的选择是在DecoderMech::decodeDifopCommon()中解析，雷达解码器类的decodeDifopPkt()会调用这个函数。注意这个函数是以T_Difop为参数的模板函数。 + +```c++ +template +template +inline void DecoderMech::decodeDifopCommon(const T_Difop& pkt) +{ + + ...... + + // load angles + if (!this->param_.config_from_file && !this->angles_ready_) + { + int ret = this->chan_angles_.loadFromDifop(pkt.vert_angle_cali, pkt.horiz_angle_cali); + this->angles_ready_ = (ret == 0); + } + +#ifdef ENABLE_DIFOP_PARSE + // add code to parse fields which are common to all mechanical LiDARs + + // device info + memcpy (this->device_info_.sn, pkt.sn.num, 6), + memcpy (this->device_info_.mac, pkt.eth.mac_addr, 6), + memcpy (this->device_info_.top_ver, pkt.version.top_ver, 5), + memcpy (this->device_info_.bottom_ver, pkt.version.bottom_ver, 5), + + this->device_info_.state = true; + // device status + this->device_status_.voltage = ntohs(pkt.status.vol_12v); + this->device_status_.state = true; +#endif +} +``` + + + +## 21.5 读取配置和状态数据 + +使用者的代码创建`LidarDriver`的实例，所以可以调用它的成员函数`getDeviceInfo()`和`getDeviceStatus()`得到配置和状态数据。 + +如前所述，`Decoder`的成员`device_info_`和`device_status_`保存这两类数据。 + +这里需要注意的是，使用者的代码需要检查这两个成员的返回值，来确定数据是不是有效的。也就是DIFOP是不是解析好了。 + +```c++ +template +class LidarDriver +{ +public: + + ...... + + inline bool getDeviceInfo(DeviceInfo& info) + { + return driver_ptr_->getDeviceInfo(info); + } + + inline bool getDeviceStatus(DeviceStatus& status) + { + return driver_ptr_->getDeviceStatus(status); + } + + ...... +}; +``` + + + +## 21.6 使能ENABLE_DIFOP_PARSE + +一般的使用者只需要解析点云，所以默认情况下希望免去这些CPU资源的消耗。 +CMake编译选项`ENABLE_DIFOP_PARSE`用于指定是否解析DIFOP包。如果希望解析，则需要启用这个选项。这个选项定义在`CMakeLists.txt`下。 + + +```cmake +CMakeLists.txt +option(ENABLE_DIFOP_PARSE "Enable DIFOP Packet Parse" OFF) +``` + + + +## 21.7 对RS16/RS32雷达的特别处理 + +RS16/RS32是早期的雷达，它们的DIFOP包的定义与后来的雷达差别较大。 + +为了能适配到`DecoderMech::decodeDifopCommon()`，`rs_driver`给他们定义了一个通用的DIFOP包定义，也就是`AdapterDifopPkt`。 + +```c++ +typedef struct +{ + uint16_t rpm; + RSEthNetV1 eth; + RSFOV fov; + RSVersionV1 version; + RSSN sn; + uint8_t return_mode; + RSStatusV1 status; + RSCalibrationAngle vert_angle_cali[32]; + RSCalibrationAngle horiz_angle_cali[32]; +} AdapterDifopPkt; +``` + +RS16/RS32的解码器类会先将需要的字段，从自己的DIFOP包中复制到AdapterDifopPkt的实例，再将这个实例交给`DecoderMech::decodeDifopCommon()`处理。 + +要让RS16/RS32支持新的字段， + ++ 首先需要拓展AdapterDifopPkt的定义。 + ++ 然后需要将这些新的字段复制到AdapterDifopPkt的实例。以RS16为例，在函数RS16DifopPkt2Adapter()中处理。 + +```c++ +inline void RS16DifopPkt2Adapter (const RS16DifopPkt& src, AdapterDifopPkt& dst) +{ + dst.rpm = src.rpm; + dst.fov = src.fov; + dst.return_mode = src.return_mode; + dst.sn = src.sn; + dst.eth = src.eth; + dst.version = src.version; + dst.status = src.status; + + ...... +} +``` + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/07_01_branches.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/07_01_branches.png new file mode 100644 index 0000000..28ee798 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/07_01_branches.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/07_02_cpu_usage_dev.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/07_02_cpu_usage_dev.png new file mode 100644 index 0000000..1101a32 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/07_02_cpu_usage_dev.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/07_03_cpu_usage_dev_opt.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/07_03_cpu_usage_dev_opt.png new file mode 100644 index 0000000..6ba35b7 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/07_03_cpu_usage_dev_opt.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/09_01_broadcast.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/09_01_broadcast.png new file mode 100644 index 0000000..ebf2f4c Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/09_01_broadcast.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/09_02_unicast.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/09_02_unicast.png new file mode 100644 index 0000000..65964a2 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/09_02_unicast.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/09_03_multicast.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/09_03_multicast.png new file mode 100644 index 0000000..3134a59 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/09_03_multicast.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/09_04_multi_lidars_port.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/09_04_multi_lidars_port.png new file mode 100644 index 0000000..c426ca9 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/09_04_multi_lidars_port.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/09_05_multi_lidars_ip.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/09_05_multi_lidars_ip.png new file mode 100644 index 0000000..48e1e13 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/09_05_multi_lidars_ip.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/09_06_vlan_layer.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/09_06_vlan_layer.png new file mode 100644 index 0000000..8d678fc Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/09_06_vlan_layer.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/09_07_vlan.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/09_07_vlan.png new file mode 100644 index 0000000..899496d Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/09_07_vlan.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/09_08_user_layer.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/09_08_user_layer.png new file mode 100644 index 0000000..d7b31a0 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/09_08_user_layer.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/12_01_select_by_port.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/12_01_select_by_port.png new file mode 100644 index 0000000..c38a07a Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/12_01_select_by_port.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/12_02_select_by_non_port.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/12_02_select_by_non_port.png new file mode 100644 index 0000000..47df223 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/12_02_select_by_non_port.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/12_03_rs32_msop_packet.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/12_03_rs32_msop_packet.png new file mode 100644 index 0000000..bbebaaf Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/12_03_rs32_msop_packet.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/12_04_rs32_difop_packet.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/12_04_rs32_difop_packet.png new file mode 100644 index 0000000..db081a7 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/12_04_rs32_difop_packet.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/12_05_with_vlan.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/12_05_with_vlan.png new file mode 100644 index 0000000..75415c9 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/12_05_with_vlan.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/12_06_with_user_layer.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/12_06_with_user_layer.png new file mode 100644 index 0000000..dc4e96e Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/12_06_with_user_layer.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/12_07_with_tail_layer.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/12_07_with_tail_layer.png new file mode 100644 index 0000000..e927020 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/12_07_with_tail_layer.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/12_08_not_supported.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/12_08_not_supported.png new file mode 100644 index 0000000..dad05e8 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/12_08_not_supported.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/13_01_wireshark_select_nic.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/13_01_wireshark_select_nic.png new file mode 100644 index 0000000..67996d0 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/13_01_wireshark_select_nic.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/13_02_wireshark_capture.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/13_02_wireshark_capture.png new file mode 100644 index 0000000..cf02395 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/13_02_wireshark_capture.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/13_03_wireshark_filter_by_port.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/13_03_wireshark_filter_by_port.png new file mode 100644 index 0000000..c5e9e2c Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/13_03_wireshark_filter_by_port.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/13_04_wireshark_export_all.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/13_04_wireshark_export_all.png new file mode 100644 index 0000000..50f89c5 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/13_04_wireshark_export_all.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/13_05_wireshark_export_range.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/13_05_wireshark_export_range.png new file mode 100644 index 0000000..5aaa1f6 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/13_05_wireshark_export_range.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/13_06_wireshark_mark.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/13_06_wireshark_mark.png new file mode 100644 index 0000000..6e60dab Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/13_06_wireshark_mark.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/13_07_wireshark_export_marked.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/13_07_wireshark_export_marked.png new file mode 100644 index 0000000..42bfd28 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/13_07_wireshark_export_marked.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/13_08_wireshark_export_marked_range.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/13_08_wireshark_export_marked_range.png new file mode 100644 index 0000000..651a52d Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/13_08_wireshark_export_marked_range.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/14_01_rs_driver_viewer_point_cloud.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/14_01_rs_driver_viewer_point_cloud.png new file mode 100644 index 0000000..dff4b05 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/14_01_rs_driver_viewer_point_cloud.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_01_demo_project.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_01_demo_project.png new file mode 100644 index 0000000..37e3ce6 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_01_demo_project.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_02_demo_use_cpp14.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_02_demo_use_cpp14.png new file mode 100644 index 0000000..3db993e Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_02_demo_use_cpp14.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_03_demo_extra_include.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_03_demo_extra_include.png new file mode 100644 index 0000000..7d41659 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_03_demo_extra_include.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_04_demo_include_path.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_04_demo_include_path.png new file mode 100644 index 0000000..2195631 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_04_demo_include_path.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_05_demo_lib_path.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_05_demo_lib_path.png new file mode 100644 index 0000000..5e877c4 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_05_demo_lib_path.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_06_demo_lib.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_06_demo_lib.png new file mode 100644 index 0000000..d596373 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_06_demo_lib.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_07_demo_precompile_macro.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_07_demo_precompile_macro.png new file mode 100644 index 0000000..6a52cb0 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_07_demo_precompile_macro.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_08_viewer_install_pcl.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_08_viewer_install_pcl.png new file mode 100644 index 0000000..85882b3 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_08_viewer_install_pcl.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_09_viewer_install_pcl_dep.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_09_viewer_install_pcl_dep.png new file mode 100644 index 0000000..1f0ec96 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_09_viewer_install_pcl_dep.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_10_viewer_add_env_var.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_10_viewer_add_env_var.png new file mode 100644 index 0000000..826fa4e Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_10_viewer_add_env_var.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_11_viewer_project.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_11_viewer_project.png new file mode 100644 index 0000000..454b98b Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_11_viewer_project.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_12_viewer_sdl_check.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_12_viewer_sdl_check.png new file mode 100644 index 0000000..5f83fc8 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_12_viewer_sdl_check.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_13_viewer_include_path.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_13_viewer_include_path.png new file mode 100644 index 0000000..ced43ef Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_13_viewer_include_path.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_14_viewer_lib_path.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_14_viewer_lib_path.png new file mode 100644 index 0000000..ed0031b Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_14_viewer_lib_path.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_15_viewer_lib.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_15_viewer_lib.png new file mode 100644 index 0000000..316f8c2 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_15_viewer_lib.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_16_viewer_precompile_macro.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_16_viewer_precompile_macro.png new file mode 100644 index 0000000..1d78ece Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_16_viewer_precompile_macro.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/18_01_mech_lasers.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/18_01_mech_lasers.png new file mode 100644 index 0000000..e3e13d7 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/18_01_mech_lasers.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/18_02_mems_lasers.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/18_02_mems_lasers.png new file mode 100644 index 0000000..6602f74 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/18_02_mems_lasers.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/18_03_mech_lasers_and_points.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/18_03_mech_lasers_and_points.png new file mode 100644 index 0000000..bb21578 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/18_03_mech_lasers_and_points.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/18_04_mech_points.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/18_04_mech_points.png new file mode 100644 index 0000000..3660963 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/18_04_mech_points.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/18_05_mems_lasers_and_points.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/18_05_mems_lasers_and_points.png new file mode 100644 index 0000000..f9f4f00 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/18_05_mems_lasers_and_points.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/18_06_mems_points.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/18_06_mems_points.png new file mode 100644 index 0000000..0f9c368 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/18_06_mems_points.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/19_01_split_angle.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/19_01_split_angle.png new file mode 100644 index 0000000..ad69e3c Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/19_01_split_angle.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/19_02_safe_range.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/19_02_safe_range.png new file mode 100644 index 0000000..8950f21 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/19_02_safe_range.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/20_01_components_and_threads.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/20_01_components_and_threads.png new file mode 100644 index 0000000..1f088f6 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/20_01_components_and_threads.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/intro/02_directories_and_files.md b/src/airy/rslidar_sdk-main/src/rs_driver/doc/intro/02_directories_and_files.md new file mode 100644 index 0000000..b831935 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/doc/intro/02_directories_and_files.md @@ -0,0 +1,44 @@ +# 2 **Directory Structure** + +## 2.1 Directories + +Below are the directories of `rs_driver`. + ++ `src` Source code to build the `rs_driver` library. ++ `demo` Demo apps based on the `rs_driver` library. It includes: + + `demo_online.cpp` demo app to connect to an online LiDAR + + `demo_online_muti_lidars.cpp` demo app to connect to multiple liDARs + + `demo_pcap.cpp` demo app to parse PCAP file ++ `tool` Tool apps based on the `rs_driver` library. + + `rs_driver_viewer.cpp` the point cloud visualization tool based on the PCL library. + + `rs_driver_pcdsaver.cpp` A tool to save point cloud as PCD format. On embedded Linux platform, the PCL library is unavailable, so `rs_driver_viewer` is unavailable either. `rs_driver_pcdsaver` is used instead. ++ `test` Unit test app based on `Google Test`. ++ `doc` Help documents + + `howto` Answers some frequently asked questions about `rs_driver`. For example, illustrations to `demo_online`/`demo_pcap`, and `rs_driver_viewer`, network configuration options, how to transform point cloud, how to port from v1.3.x to v15.x, how to split frames, how to handle packet loss and out of order, how to stamp point cloud, layout of points in point cloud, etc. + + `intro` descriptions to `rs_driver`'s interface, such as parameters, error code, CMake macros. + + `src_intro` documents to analysis `rs_driver`'s source code, such as design consideration of `rs_driver`, and some important implement details. ++ `win` `MSVC` project files with compilation options ready to compile `demo_online`/`demo_pcap`/`rs_driver_viewer`。 + +``` +├── src +│   └── rs_driver +├── demo +│   ├── demo_online.cpp +│   ├── demo_online_multi_lidars.cpp +│   └── demo_pcap.cpp +├── tool +│   ├── rs_driver_pcdsaver.cpp +│   └── rs_driver_viewer.cpp +├── test +├── doc +│   ├── howto +│   ├── intro +│   └── src_intro +├── win +├── CMakeLists.txt +├── README_CN.md +├── README.md +├── CHANGELOG.md +├── LICENSE +``` + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/intro/02_directories_and_files_CN.md b/src/airy/rslidar_sdk-main/src/rs_driver/doc/intro/02_directories_and_files_CN.md new file mode 100644 index 0000000..4634772 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/doc/intro/02_directories_and_files_CN.md @@ -0,0 +1,44 @@ +# 2 **目录结构** + +## 2.1 目录列表 + +`rs_driver`工程的主要目录如下。 + ++ `src` 库的源代码 ++ `demo` 使用rs_driver库的示例程序，包括： + + `demo_online.cpp` 连接在线雷达的例子 + + `demo_online_muti_lidars.cpp` 连接多个在线雷达的例子 + + `demo_pcap.cpp` 解析PCAP文件的例子 ++ `tool` 实用工具程序 + + `rs_driver_viewer.cpp` 基于PCL库的点云可视化工具 + + `rs_driver_pcdsaver.cpp` 将点云保存为PCD格式的工具。在嵌入式环境下可能没有PCL库支持，所以`rs_driver_viewer`不可用。这时可以用`rs_driver_pcdsaver`导出点云。 ++ `test` 基于`Google Test`的单元测试 ++ `doc` 帮助文档 + + `howto` 回答了使用`rs_driver`的一些常见问题，如对`demo_online`/`demo_pcap`例子和`rs_driver_viewer`工具的讲解，如何配置网络选项，如何对点云作坐标转换，如何从`v1.3.x`移植到`v1.5.x`，如何分帧，如何处理丢包和乱序，如何给点云打时间戳，点在点云中如何布局等。 + + `intro` 对`rs_driver`的接口说明，包括参数选项、错误码、CMake编译宏等。 + + `src_intro` 解析rs_driver源代码的文档，说明`rs_driver`的设计思路和一些重要的实现细节。 ++ `win` Windows下`MSVC`的工程文件，已经设置好编译选项，可以成功编译`demo_online`/`demo_pcap`/`rs_driver_viewer`等。 + +``` +├── src +│   └── rs_driver +├── demo +│   ├── demo_online.cpp +│   ├── demo_online_multi_lidars.cpp +│   └── demo_pcap.cpp +├── tool +│   ├── rs_driver_pcdsaver.cpp +│   └── rs_driver_viewer.cpp +├── test +├── doc +│   ├── howto +│   ├── intro +│   └── src_intro +├── win +├── CMakeLists.txt +├── README_CN.md +├── README.md +├── CHANGELOG.md +├── LICENSE +``` + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/intro/03_thread_model.md b/src/airy/rslidar_sdk-main/src/rs_driver/doc/intro/03_thread_model.md new file mode 100644 index 0000000..415d50c --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/doc/intro/03_thread_model.md @@ -0,0 +1,64 @@ +# 3 Thread Model + + + +## 3.1 Overview + +This document is to describe the `rs_driver`'s API. + +However, it seems to be useful and necessary to describe its components first. Why? ++ the caller implemented callbacks are running in `rs_driver`'s threads. They may block the threads with bad design. ++ Caller is supposed to manage the point cloud queue, to avoid copy of point cloud, and repeat allocation and release. This is just how `rs_driver` manages the MSOP/DIFOP packet queue. + + + +## 3.2 Components and Threads + +`rs_driver` consists of three parts: `Input`, `Decoder`, and `LidarDriverImpl`. + ++ `Input` gets MSOP/DIFOP packets from network sockets, PCAP file, etc. Its implementation classes have their own threads `recv_thread`. ++ `Decoder` parses MSOP/DIFOP packets and constructs point cloud. `Decoder` has no threads. It runs in `LidarDriverImpl`'s MSOP/DIFOP packet handling threads `handle_thread`. ++ `LidarDriverImpl` combines `Input` and `Decoder`. It gets MSOP/DIFOP packets and dispatches them to `Decoder` by their type. After getting point cloud from `Decoder`, `LidarDriverImpl` returns it to caller via the callback function. +![](./img/03_01_components_and_threads.png) + + + +There are two details here. + ++ `LidarDriverImpl` manages a free MSOP/DIFOP packet queue `free_pkt_queue`, and a stuffed packet queue `pkt_queue`. Input first get free packet instance, stuffs it, and return it to `pkt_queue`. This is done with `LidarDriverImpl`'s callbacks. ++ `LidarDriverImpl`'s callbacks runs in Input's thread `recv_thread`. They fetch packet instance from queue, and return it to queue. This is simple work and will not block `recv_thread`. + + + +Apply this design to point cloud queue. + ++ Caller manage point cloud instances. `rs_driver` requests two callbacks. One is to get free point cloud, and the other is to return stuffed point cloud. Caller is encouraged to adopt the design of packet queue. ++ Caller's callbacks runs in `rs_driver`'s thread `handle_thread`. It parses MSOP/DIFOP packets. If it is slow, the packet queue will overflow. `rs_driver` then reports `ERRCODE_PKTBUFOVERFLOW` and discards packets. To avoid this, caller should NOT do any time-consuming task in the callbacks. + + + +## 3.3 Interface + +Now rs_driver's design objectives is clear. + ++ Avoid to copy point cloud, and avoid to malloc/free point cloud repeatedly. ++ Parallel the construction and the process of point cloud. + + + +Below is the supposed interaction between rs_driver and user's code. Most detail of `rs_driver` is omitted except the thread `handle_thread`. And it is renamed as `construct_thread` since we are focusing on point cloud. + +![](./img/03_02_interface_with_threads.png) + +`rs_driver` runs in its thread `construct_thread`. It + ++ Gets free point cloud from user. User fetches it from a free cloud queue `free_point_cloud_queue`. If the queue is empty, then create a new one. ++ Parses packets and constructs point cloud. ++ Returns stuffed point cloud to user. ++ User's code is supposed to shift it to the queue `stuffed_point_cloud_queue`. + +User's code runs in its thread `process_thread`. It ++ Fetches stuffed point cloud from the queue `stuffed_point_cloud_queue` ++ Process the point cloud ++ Return the point cloud back to the queue `free_point_cloud_queue`. rs_driver will use it again. + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/intro/03_thread_model_CN.md b/src/airy/rslidar_sdk-main/src/rs_driver/doc/intro/03_thread_model_CN.md new file mode 100644 index 0000000..f778c73 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/doc/intro/03_thread_model_CN.md @@ -0,0 +1,69 @@ +# 3 **线程模型与接口设计** + + + +## 3.1 概述 + +这个章节的本意是说明`rs_driver`的接口设计，而不是描述它的线程模型。 + +要求使用者了解`rs_driver`的内存运行机制，至少理论上是不应该的。但这里还是这么做了。 + +坚持这样做的原因是： + ++ 使用者实现的回调函数运行在`rs_driver`的线程中，设计不当可能导致`rs_driver`丢包 ++ `rs_driver`希望使用者自己管理点云实例，以避免点云的复制，反复分配和释放。这一点与它对MSOP/DIFOP Packet的处理类似。 + +所以先讲清楚线程模型，能帮助使用者更好地了解接口背后的设计考虑。 + + + +## 3.2 组件与线程 + +`rs_driver`主要由三部分组成： `Input`、`Decoder`、`LidarDriverImpl`。 + ++ `Input`部分负责从Socket、PCAP文件等数据源，获取MSOP/DIFOP Packet。`Input`的类一般有自己的接收线程`recv_thread`。 ++ `Decoder`部分负责解析MSOP/DIFOP Packet，得到点云。`Decoder`部分没有自己的线程，它运行在`LiarDriverImpl`的Packet处理线程`handle_thread`中。 ++ `LidarDrvierImpl`部分将`Input`和`Decoder`组合到一起。它从`Input`得到Packet，根据Packet的类型将它派发到`Decoder`。得到点云后，通过用户的回调函数传递给用户。 +![](./img/03_01_components_and_threads.png) + + + +这里有两点值得说明： + ++ `LidarDriverImpl`管理一个空闲MSOP/DIFOP Packet的队列`free_pkt_queue`，和一个填充了的Packet的队列`pkt_queue`。`Input`接收前从`free_pkt_queue`得到空闲的Packet实例，接收后将填充好的Packet保存到`pkt_queue`，这两者都通过`LidarDriverImpl`的两个回调函数完成。 ++ `LidarDriverImpl`的回调函数实际上运行在`Input`的线程`recv_thread`中，它们只是简单从队列中取出Packet实例，或将Packet实例放入队列。这是简单的工作，不会拖慢`recv_thread`的运行。 + + + +将Packet队列的设计想法平移到点云队列，就得到`rs_driver`的接口设计了。 + ++ 点云实例由调用者的代码自己管理，`rs_driver`只要求两个回调函数，一个得到空闲的点云实例，一个返回填充好的点云。调用者的代码可以借鉴Packet队列的做法，管理两个点云队列，但这点并不强制要求。 ++ 调用者实现的两个回调函数，运行在rs_driver的线程`handle_thread`中。这个线程负责解析MSOP/DIFOP Packet，如果不能及时处理它们，`pkt_queue`就会溢出，`rs_driver`除了报告`ERRCODE_PKTBUFOVERFLOW`之外，还会丢掉处理不了的Packet。所以使用者的回调函数不能做任何太耗时的事情。 + + + +## 3.3 接口设计 + +这样`rs_driver`接口设计的目标就清楚了。 ++ 避免点云的复制、避免点云的反复分配和释放 ++ 让点云的构建（填充）与点云的处理并行，两者不要互相干扰 + + + +如下图是设想的`rs_driver`与调用者之间交互的方式。 + +图中略去了`rs_driver`的大部分实现细节，只留下MSOP/DIFOP Packet的处理线程`handle_thread`。它同时也是点云的构建线程，这里以点云为说明对象，所以重命名为`construct_thread`。 +![](./img/03_02_interface_with_threads.png) + +`rs_driver`运行在线程`construct_thread`中。它 + ++ 从调用者得到空闲的点云实例。调用者从一个空闲的点云队列`free_point_cloud_queue`得到这个实例。如果队列为空，则创建一个新的。 ++ 解析MSOP/DIFOP Packet，构建（填充）点云实例 ++ 将填充好的点云返还给调用者 ++ 调用者将这个实例转移到另一个点云队列`stuffed_point_cloud_queue`，等待处理。 + +调用者的点云处理代码，运行在自己的线程`process_thread`中。它 ++ 从待处理点云队列`stuffed_point_cloud_queue`，取出一个点云实例。 ++ 处理这个点云实例 ++ 处理后，将它放回空闲队列`free_point_cloud_queue`，等待`rs_driver`再次使用。 + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/intro/04_parameter_intro.md b/src/airy/rslidar_sdk-main/src/rs_driver/doc/intro/04_parameter_intro.md new file mode 100644 index 0000000..fbca7f0 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/doc/intro/04_parameter_intro.md @@ -0,0 +1,242 @@ +# 4 Introduction to rs_driver's Parameters + + + +## 4.1 Parameter File + +The parameters are defined in the file `rs_driver/src/rs_driver/driver_param.h`. + +Basically, there are 3 structures: + ++ RSDriverParam ++ RSDecoderParam ++ RSInputParam + + + +## 4.2 RSDriverParam + +RSDriverParam contains RSDecoderParam and RSInputParam. + +```c++ +typedef struct RSDriverParam +{ + LidarType lidar_type = LidarType::RS16; ///< Lidar type + InputType input_type = InputType::ONLINE_LIDAR; ///< Input type + RSInputParam input_param; + RSDecoderParam decoder_param; +} RSDriverParam; +``` + ++ lidar_type - Lidar Type. Some LiDARs are mechanical, and some are MEMS. Some parameters of RSDecoderParam is only for mechanical LiDARs. + +```c++ +enum LidarType +{ + // mechanical + RS_MECH = 0x01, + RS16 = RS_MECH, + RS32, + RSBP, + RSAIRY, + RSHELIOS, + RSHELIOS_16P, + RS128, + RS80, + RS48, + RSP128, + RSP80, + RSP48, + + // mems + RS_MEMS = 0x20, + RSM1 = RS_MEMS, + RSM2, + RSM3, + RSE1, + RSMX, + + // jumbo + RS_JUMBO = 0x100, + RSM1_JUMBO = RS_JUMBO + RSM1, +}; +``` + ++ input_type - What source the Lidar packets is from. + + ONLINE_LIDAR means from online LiDAR; PCAP_FILE means from PCAP file, which is captured with 3rd party tool; RAW_PACKET is user's own data captured with the `rs_driver` API. + +```c++ +enum InputType +{ + ONLINE_LIDAR = 1, + PCAP_FILE, + RAW_PACKET +}; +``` + + + +## 4.3 RSInputParam + +RSInputParam specifies the detail paramters of packet source. + +The following parameters are for `ONLINE_LIDAR` and `PCAP_FILE`. ++ msop_port - The UDP port on the host, to receive MSOP packets. ++ difop_port - The UDP port on the host, to receive DIFOP Packets. ++ user_layer_bytes - Bytes of user layer. thers is no user layer if it is 0 ++ tail_layer_bytes - Bytes of tail layer. thers is no tail layer if it is 0 + +The following parameters are only for ONLINE_LIDAR. ++ host_address - The host's IP, to receive MSOP/DIFOP Packets ++ group_address - A multicast group to receive MSOP/DIFOP packts. `rs_driver` make `host_address` join it. ++ socket_recv_buf - Bytes of socket receive buffer. + +The following parameters are only for PCAP_FILE. ++ pcap_path - Full path of the PCAP file. ++ pcap_repeat - Whether to replay PCAP file repeatly ++ pcap_rate - `rs_driver` replay the PCAP file by the theological frame rate. `pcap_rate` gives a rate to it, so as to speed up or slow down. ++ use_vlan - If the PCAP file contains VLAN layer, use `use_vlan`=`true` to skip it. + +```c++ +typedef struct RSInputParam +{ + uint16_t msop_port = 6699; ///< Msop packet port number + uint16_t difop_port = 7788; ///< Difop packet port number + uint16_t imu_port = 0; ///< IMU packet port number, default disable + uint16_t user_layer_bytes = 0; ///< Bytes of user layer. thers is no user layer if it is 0 + uint16_t tail_layer_bytes = 0; ///< Bytes of tail layer. thers is no tail layer if it is 0 + + ///< These parameters are valid when the input type is online lidar + std::string host_address = "0.0.0.0"; ///< Address of host + std::string group_address = "0.0.0.0"; ///< Address of multicast group + uint32_t socket_recv_buf = 106496; // 0 + float max_distance = 0.0f; + bool use_lidar_clock = false; ///< true: use LiDAR clock as timestamp; false: use system clock as timestamp + bool dense_points = false; ///< true: discard NAN points; false: reserve NAN points + bool ts_first_point = false; ///< true: time-stamp point cloud with the first point; false: with the last point; + bool wait_for_difop = true; ///< true: start sending point cloud until receive difop packet + RSTransformParam transform_param; ///< Used to transform points + + ///< Theses parameters are only for mechanical Lidars. + bool config_from_file = false; ///< Internal use only for debugging + std::string angle_path = ""; ///< Internal use only for debugging + float start_angle = 0.0f; ///< Start angle of point cloud + float end_angle = 360.0f; ///< End angle of point cloud + SplitFrameMode split_frame_mode = SplitFrameMode::SPLIT_BY_ANGLE; + ///< 1: Split frames by split_angle; + ///< 2: Split frames by fixed number of blocks; + ///< 3: Split frames by custom number of blocks (num_blks_split) + float split_angle = 0.0f; ///< Split angle(degree) used to split frame, only be used when split_frame_mode=1 + uint16_t num_blks_split = 1; ///< Number of packets in one frame, only be used when split_frame_mode=3 + + void print() const + { + RS_INFO << "------------------------------------------------------" << RS_REND; + RS_INFO << " RoboSense Decoder Parameters " << RS_REND; + RS_INFOL << "min_distance: " << min_distance << RS_REND; + RS_INFOL << "max_distance: " << max_distance << RS_REND; + RS_INFOL << "use_lidar_clock: " << use_lidar_clock << RS_REND; + RS_INFOL << "dense_points: " << dense_points << RS_REND; + RS_INFOL << "ts_first_point: " << ts_first_point << RS_REND; + RS_INFOL << "wait_for_difop: " << wait_for_difop << RS_REND; + RS_INFOL << "config_from_file: " << config_from_file << RS_REND; + RS_INFOL << "angle_path: " << angle_path << RS_REND; + RS_INFOL << "start_angle: " << start_angle << RS_REND; + RS_INFOL << "end_angle: " << end_angle << RS_REND; + RS_INFOL << "split_frame_mode: " << split_frame_mode << RS_REND; + RS_INFOL << "split_angle: " << split_angle << RS_REND; + RS_INFOL << "num_blks_split: " << num_blks_split << RS_REND; + RS_INFO << "------------------------------------------------------" << RS_REND; + transform_param.print(); + } +} RSDecoderParam; +``` + +The following parameters are for all LiDARs。 + ++ min_distance、max_distance - Set measurement range. Internal use only. ++ use_lidar_clock - Where the point cloud's timestamp is from. From the LiDAR, or from `rs_driver` on the host? + + If `use_lidar_clock`=`true`，use the LiDAR timestamp, else use the host one. ++ dense_points - Whether the point cloud is dense. + + If `dense_points`=`false`, then point cloud contains NAN points, else discard them. ++ ts_first_point - Whether to stamp the point cloud with the first point, or the last point. + + If `ts_first_point`=`false`, then stamp it with the last point, else with the first point。 ++ wait_for_difop - Whether wait for DIFOP Packet before parse MSOP packets. + + DIFOP Packet contains angle calibration parameters. If it is unavailable, the point cloud is flat. + + If you get no point cloud, try `wait_for_difop`=`false`. It might help to locate the problem. ++ transform_param - paramters of coordinate transformation. It is only valid when the CMake option `ENABLE_TRANSFORM`=`ON`. + +```c++ +typedef struct RSTransformParam +{ + float x = 0.0f; ///< unit, m + float y = 0.0f; ///< unit, m + float z = 0.0f; ///< unit, m + float roll = 0.0f; ///< unit, radian + float pitch = 0.0f; ///< unit, radian + float yaw = 0.0f; ///< unit, radian +} RSTransformParam; +``` + +The following parameters are only for mechanical LiDARs. + ++ config_from_file - Where to get LiDAR configuration parameters, from extern files, or from DIFOP packet. Internal use only. ++ angle_path - File of angle calibration parameters. Internal use only. ++ start_angle、end_angle - Generally, mechanical LiDARs's point cloud's azimuths are in the range of [`0`, `360`]. Here you may assign a smaller range of [`start_angle`, `end_angle`). + ++ split_frame_mode - How to split frame. + + `SPLIT_BY_ANGLE` is by a user requested angle. User can specify it. This is default and suggested. + + `SPLIT_BY_FIXED_BLKS` is by blocks theologically; + + `SPLIT_BY_CUSTOM_BLKS` is by user requested blocks. + +```c++ +enum SplitFrameMode +{ + SPLIT_BY_ANGLE = 1, + SPLIT_BY_FIXED_BLKS, + SPLIT_BY_CUSTOM_BLKS +}; +``` ++ split_angle - If `split_frame_mode`=`SPLIT_BY_ANGLE`, then `split_angle` is the requested angle to split. ++ num_blks_split - If `split_frame_mode`=`SPLIT_BY_CUSTOM_BLKS`，then `num_blks_split` is blocks. + + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/intro/04_parameter_intro_CN.md b/src/airy/rslidar_sdk-main/src/rs_driver/doc/intro/04_parameter_intro_CN.md new file mode 100644 index 0000000..367b5c9 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/doc/intro/04_parameter_intro_CN.md @@ -0,0 +1,238 @@ +# 4 **配置参数介绍** + + + +## 4.1 概述 + +文件`rs_driver/src/rs_driver/driver_param.h`中， 定义了`rs_driver`的配置选项。 + +这些配置选项定义在如下结构中。 + ++ RSDriverParam ++ RSDecoderParam ++ RSInputParam + + + +## 4.2 RSDriverParam + +RSDriverParam包括RSDecoderParam、RSInputParam、和其他选项。 + +```c++ +typedef struct RSDriverParam +{ + LidarType lidar_type = LidarType::RS16; ///< Lidar type + InputType input_type = InputType::ONLINE_LIDAR; ///< Input type + RSInputParam input_param; + RSDecoderParam decoder_param; +} RSDriverParam; +``` + ++ 成员`lidar_type` - 指定雷达的类型。这些雷达部分是机械式雷达，部分是MEMS雷达。RSDecoderParam中的部分选项只针对机械式雷达。 + +```c++ +enum LidarType +{ + // mechanical + RS_MECH = 0x01, + RS16 = RS_MECH, + RS32, + RSBP, + RSAIRY, + RSHELIOS, + RSHELIOS_16P, + RS128, + RS80, + RS48, + RSP128, + RSP80, + RSP48, + + // mems + RS_MEMS = 0x20, + RSM1 = RS_MEMS, + RSM2, + RSM3, + RSE1, + RSMX, + + // jumbo + RS_JUMBO = 0x100, + RSM1_JUMBO = RS_JUMBO + RSM1, +}; +``` + ++ 成员`input_type` - 指定雷达的数据源类型 + + ONLINE_LIDAR是在线雷达；PCAP_FILE是包含MSOP/DIFOP Packet的PCAP文件；RAW_PACKET是使用者调用`rs_driver`的函数接口获得MSOP/DIFOP Packet，自己保存的数据。 + +```c++ +enum InputType +{ + ONLINE_LIDAR = 1, + PCAP_FILE, + RAW_PACKET +}; +``` + + + +## 4.3 RSInputParam + +RSInputParam指定`rs_driver`的网络配置选项。 + +如下参数针对`ONLINE_LIDAR`和`PCAP_FILE`。 ++ msop_port - 指定主机的本地端口，接收MSOP Packet ++ difop_port - 指定主机的本地端口，接收DIFOP Packet ++ imu_port - 指定主机的本地端口，接收IMU Packet ++ user_layer_bytes - 指定用户层数据的字节数。 用户层数据是MSOP Packet和DIFOP Packet中的一部分，这部分数据由用户自己定义。 ++ tail_layer_bytes - 指定尾部数据的字节数。尾部数据是MSOP Packet的最后一部分，这部分数据由用户自己定义。 + +如下参数仅针对 `ONLINE_LIDAR`。 + ++ host_address - 指定主机网卡的IP地址，接收MSOP/DIFOP Packet ++ group_address - 指定一个组播组的IP地址。`rs_driver`将 `host_address`指定的网卡加入这个组播组，以便接收MSOP/DIFOP Packet。 ++ socket_recv_buf - 指定socket的接收缓冲区大小。 `rs_driver`会接收MSOP Packet，因此需要足够大的缓冲区。 + +如下参数仅针对`PCAP_FILE`。 ++ pcap_path - PCAP文件的全路径 ++ pcap_repeat - 指定是否重复播放PCAP文件 ++ pcap_rate - `rs_driver`按理论上的MSOP Packet时间间隔，模拟播放PCAP文件。`pcap_rate`可以在这个速度上指定一个比例值，加快或放慢播放速度。 ++ use_vlan - 如果PCAP文件中的MSOP/DIFOP Packet包含VLAN层，可以指定`use_vlan`=`true`，跳过这一层。 + +```c++ +typedef struct RSInputParam +{ + uint16_t msop_port = 6699; ///< Msop packet port number + uint16_t difop_port = 7788; ///< Difop packet port number + uint16_t imu_port = 0; ///< IMU packet port number, default disable + uint16_t user_layer_bytes = 0; ///< Bytes of user layer. thers is no user layer if it is 0 + uint16_t tail_layer_bytes = 0; ///< Bytes of tail layer. thers is no tail layer if it is 0 + + ///< These parameters are valid when the input type is online lidar + std::string host_address = "0.0.0.0"; ///< Address of host + std::string group_address = "0.0.0.0"; ///< Address of multicast group + uint32_t socket_recv_buf = 106496; // 0 + float max_distance = 0.0f; + bool use_lidar_clock = false; ///< true: use LiDAR clock as timestamp; false: use system clock as timestamp + bool dense_points = false; ///< true: discard NAN points; false: reserve NAN points + bool ts_first_point = false; ///< true: time-stamp point cloud with the first point; false: with the last point; + bool wait_for_difop = true; ///< true: start sending point cloud until receive difop packet + RSTransformParam transform_param; ///< Used to transform points + + ///< Theses parameters are only for mechanical Lidars. + bool config_from_file = false; ///< Internal use only for debugging + std::string angle_path = ""; ///< Internal use only for debugging + float start_angle = 0.0f; ///< Start angle of point cloud + float end_angle = 360.0f; ///< End angle of point cloud + SplitFrameMode split_frame_mode = SplitFrameMode::SPLIT_BY_ANGLE; + ///< 1: Split frames by split_angle; + ///< 2: Split frames by fixed number of blocks; + ///< 3: Split frames by custom number of blocks (num_blks_split) + float split_angle = 0.0f; ///< Split angle(degree) used to split frame, only be used when split_frame_mode=1 + uint16_t num_blks_split = 1; ///< Number of packets in one frame, only be used when split_frame_mode=3 + + void print() const + { + RS_INFO << "------------------------------------------------------" << RS_REND; + RS_INFO << " RoboSense Decoder Parameters " << RS_REND; + RS_INFOL << "min_distance: " << min_distance << RS_REND; + RS_INFOL << "max_distance: " << max_distance << RS_REND; + RS_INFOL << "use_lidar_clock: " << use_lidar_clock << RS_REND; + RS_INFOL << "dense_points: " << dense_points << RS_REND; + RS_INFOL << "ts_first_point: " << ts_first_point << RS_REND; + RS_INFOL << "wait_for_difop: " << wait_for_difop << RS_REND; + RS_INFOL << "config_from_file: " << config_from_file << RS_REND; + RS_INFOL << "angle_path: " << angle_path << RS_REND; + RS_INFOL << "start_angle: " << start_angle << RS_REND; + RS_INFOL << "end_angle: " << end_angle << RS_REND; + RS_INFOL << "split_frame_mode: " << split_frame_mode << RS_REND; + RS_INFOL << "split_angle: " << split_angle << RS_REND; + RS_INFOL << "num_blks_split: " << num_blks_split << RS_REND; + RS_INFO << "------------------------------------------------------" << RS_REND; + transform_param.print(); + } +} RSDecoderParam; +``` + +如下参数针对所有雷达。 ++ min_distance、max_distance - 重置测距的最大、最小值。仅内部调试使用。 ++ use_lidar_clock - 指定点云的时间采用MSOP Packet中的时间（雷达根据自身时间设置），还是主机的系统时间。 + + 如果`use_lidar_clock`=`true`，则采用MSOP Packet的，否则采用主机的。 ++ dense_points - 指定点云是否是dense的。 + + 如果`dense_points`=`false`, 则点云中包含NAN点，否则去除点云中的NAN点。 ++ ts_first_point - 指定点云的时间戳来自它的第一个点，还是最后第一个点。 + + 如果`ts_first_point`=`true`, 则第一个点的时间作为点云的时间戳，否则最后一个点的时间作为点云的时间戳。 ++ wait_for_difop - 解析MSOP Packet之前，是否等待DIFOP Packet。 + + DIFOP Packet中包含垂直角等标定参数。如果没有这些参数，`rs_driver`输出的点云将是扁平的。 + + 在`rs_driver`不输出点云时，设置`wait_for_difop=false`，可以帮助定位问题。 ++ transform_param - 指定点的坐标转换参数。这个选项只有在CMake编译宏`ENABLE_TRANSFORM=ON`时才有效。 + +```c++ +typedef struct RSTransformParam +{ + float x = 0.0f; ///< unit, m + float y = 0.0f; ///< unit, m + float z = 0.0f; ///< unit, m + float roll = 0.0f; ///< unit, radian + float pitch = 0.0f; ///< unit, radian + float yaw = 0.0f; ///< unit, radian +} RSTransformParam; +``` + +如下参数仅针对机械式雷达。 + ++ config_from_file - 指定雷达本身的配置参数是从文件中读入，还是从DIFOP Packet中得到。这个选项仅内部调试使用。 ++ angle_path - 雷达的角度标定参数文件。仅内部调试使用。 ++ start_angle、end_angle - 机械式雷达一般输出的点云的水平角在[`0`, `360`]之间，这里可以指定一个更小的范围[`start_angle`, `end_angle`)。 ++ split_frame_mode - 指定分帧模式 + + `SPLIT_BY_ANGLE`是按 `用户指定的角度`分帧；`SPLIT_BY_FIXED_BLKS`是按 `理论上的每圈BLOCK数`分帧；`SPLIT_BY_CUSTOM_BLKS`按 `用户指定的BLOCK数`分帧。默认值是 `SPLIT_BY_ANGLE`。一般不建议使用其他两种模式。 + +```c++ +enum SplitFrameMode +{ + SPLIT_BY_ANGLE = 1, + SPLIT_BY_FIXED_BLKS, + SPLIT_BY_CUSTOM_BLKS +}; +``` ++ split_angle - 如果`split_frame_mode`=`SPLIT_BY_ANGLE`, 则`split_angle`指定分帧的角度 ++ num_blks_split - 如果`split_frame_mode`=`SPLIT_BY_CUSTOM_BLKS`，则`num_blks_split`指定每帧的BLOCK数。 + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/intro/05_cmake_macros_intro.md b/src/airy/rslidar_sdk-main/src/rs_driver/doc/intro/05_cmake_macros_intro.md new file mode 100644 index 0000000..38857c4 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/doc/intro/05_cmake_macros_intro.md @@ -0,0 +1,166 @@ +# 5 Introduction to rs_driver's CMake macros + + + +## 5.1 CMakeList.txt + +A group of macros can be used to determine what and how to compile `rs_driver`. Please find them in `CMakeLists.txt` of the project. + + + +## 5.2 What to compile + +The targets of compilation includes: ++ Demo apps, including `demo_online`, `demo_online_multi_lidars`, `demo_pcap`, etc. ++ Tools, including `rs_driver_viewer`, `rs_driver_pcdsaver`, etc. ++ Test cases + +### 5.2.1 COMPILE_DEMOS + +COMPILE_DEMOS determines whether to compile Demo apps. ++ COMPILE_DEMOS=OFF means No。This is the default. ++ COMPILE_DEMOS=ON means Yes。 + +``` +option(COMPILE_DEMOS "Build rs_driver demos" OFF) +``` + +### 5.2.2 COMPILE_TOOLS + +COMPILE_TOOLS determines whether to compile tools. ++ COMPILE_TOOLS=OFF. Whether to compile `rs_driver_viewer`/`rs_driver_pcdsaver`, is determined by COMPILE_TOOLS_VIEWER/COMPILE_TOOLS_PCDSAVER. This is the default. ++ COMPILE_TOOLS=ON. Compile both `rs_driver_viewer` and `rs_driver_pcdsaver`, no matter what COMPILE_TOOLS_VIEWER and COMPILE_TOOLS_PCDSAVER are. + +``` +option(COMPILE_TOOLS "Build rs_driver tools" OFF) +``` + +### 5.2.3 COMPILE_TOOLS_VIEWER + +COMPILE_TOOLS_VIEWER determines whether to compile `rs_driver_viewer`, in case of COMPILE_TOOLS=OFF. ++ COMPILE_TOOLS_VIEWER=OFF means No. This is the default. ++ COMPILE_TOOLS_VIEWER=ON means Yes. + +``` +option(COMPILE_TOOL_VIEWER "Build point cloud visualization tool" OFF) +``` + +### 5.2.4 COMPILE_TOOLS_PCDSAVER + +COMPILE_TOOL_PCDSAVER determines whether to compile `rs_driver_pcdsaver`, in case of COMPILE_TOOLS=OFF. ++ COMPILE_TOOLS_PCDSAVER=OFF means No. This is the default. ++ COMPILE_TOOLS_PCDSAVER=ON means Yes. + +``` +option(COMPILE_TOOL_PCDSAVER "Build point cloud pcd saver tool" OFF) +``` + +### 5.2.5 COMPILE_TESTS + +COMPILE_TESTS determines whether to compile test cases. ++ COMPILE_TESTS=OFF means No. This is the default. ++ COMPILE_TESTS=ON means Yes. + +``` +option(COMPILE_TESTS "Build rs_driver unit tests" OFF) +``` + + + +## 5.3 How to compile + +### 5.3.1 DISABLE_PCAP_PARSE + +DISALBE_PCAP_PARSE determines whether to support the PCAP source, that's to say, whether to parse MSOP/DIFOP packets from a PCAP file. ++ DISABLE_PCAP_PARSE=OFF means Yes, This is the default. ++ DISABLE_PCAP_PARSE=ON means No. On embedded Linux, PCAP source is not needed, so enable this macro to avoid porting the `libpcap` library. + +``` +option(DISABLE_PCAP_PARSE "Disable PCAP file parse" OFF) +``` + +### 5.3.2 ENABLE_TRANSFORM + +ENABLE_TRANSFORM determines whether to support coordinate transformation. ++ ENABLE_TRANSFORM=OFF means No. This is the default. ++ ENABLE_TRANSFORM=ON means Yes, **Enable this costs much CPU resource, so please don't do so in released products.** + +``` +option(ENABLE_TRANSFORM "Enable transform functions" OFF) +``` + +### 5.3.3 ENABLE_MODIFY_RECVBUF + +ENABLE_MODIFY_RECVBUF determines whether to change the receiving buffer of the MSOP/DIFOP sockets. + +On some platforms (such as embedded Linux, Windows, etc), the default size of the buffer is small. This may cause loss of MSOP/DIFOP packets, so enable this macro to enlarge the buffer. ++ ENABLE_MODIFY_RECVBUF=OFF means No. This is the default. ++ ENABLE_MODIFY_RECVBUF=ON means Yes. Modify the receive cache based on the size passed in by the user + +``` +option(ENABLE_MODIFY_RECVBUF "Enable modify size of RCVBUF" OFF) +``` + +### 5.3.4 ENABLE_WAIT_IF_QUEUE_EMPTY + +ENABLE_WAIT_IF_QUEUE_EMPTY determines what the handling thread do if the MSOP/DIFOP packet queue is empty. ++ ENABLE_WAIT_IF_QUEUE_EMPTY=OFF means to wait for condition variable. This is the default. ++ ENABLE_WAIT_IF_QUEUE_EMPTY=ON means to call usleep(). This can decrease CPU usage, however increase the delay of point cloud frames. + +You should weigh up the pros and cons based on your cases. + +``` +option(ENABLE_WAIT_IF_QUEUE_EMPTY "Enable waiting for a while in handle thread if the queue is empty" OFF) +``` + +### 5.3.5 ENABLE_EPOLL_RECEIVE + +ENABLE_EPOLL_RECEIVE determines how to recieve MSOP/DIFOP packets. ++ ENABLE_EPOLL_RECEIVE=OFF means to use select(). This is the default. ++ ENABLE_EPOLL_RECEIVE=ON means to use epoll(). + +``` +option(ENABLE_EPOLL_RECEIVE "Receive packets with epoll() instead of select()" OFF) +``` + +### 5.3.6 ENABLE_STAMP_WITH_LOCAL + +ENABLE_STAMP_WITH_LOCAL determines whether to convert the timestamp of point cloud to local time. ++ ENABLE_STAMP_WITH_LOCAL=OFF means No, and to use UTC time. This is the default. ++ ENABLE_STAMP_WITH_LOCAL=ON means Yes, and to convert the timestamp to local time。 + +``` +option(ENABLE_STAMP_WITH_LOCAL "Enable stamp point cloud with local time" OFF) +``` + +### 5.3.7 ENABLE_PCL_POINTCLOUD + +ENABLE_PCL_POINTCLOUD determines the format of point cloud in the Demo Apps. ++ ENABLE_PCL_POINTCLOUD=OFF means to use the RoboSense defined format. This is the default. ++ ENABLE_PCL_POINTCLOUD=ON means to use the PCL format. The PCL library is needed to enable this. + +``` +option(ENABLE_PCL_POINTCLOUD "Enable PCL Point Cloud" OFF) +``` + +### 5.3.8 ENABLE_CRC32_CHECK + +ENABLE_CRC32_CHECK determines whether to apply CRC32 check on MSOP/DIFOP Packet. ++ ENABLE_CRC32_CHECK=OFF means no CRC32 check. This is the default. ++ ENABLE_CRC32_CHECK=ON means CRC32 check. The LiDAR should support this feature to enable this. + +``` +option(ENABLE_CRC32_CHECK "Enable CRC32 Check on MSOP Packet" OFF) +``` + +### 5.3.9 ENABLE_DIFOP_PARSE + +ENABLE_DIFOP_PARSE determins whether to parse DIFOP Packet, to get the configuratioin data and status data. ++ ENABLE_DIFOP_PARSE=OFF means not to parse. This is the default. ++ ENABLE_DIFOP_PARSE=ON means to parse. Note `rs_driver` parses only a few fields as an example. Please refer to [how to parse difop packet](../howto/21_how_to_parse_difop.md) + +``` +option(ENABLE_DIFOP_PARSE "Enable Parsing DIFOP Packet" OFF) +``` + + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/intro/05_cmake_macros_intro_CN.md b/src/airy/rslidar_sdk-main/src/rs_driver/doc/intro/05_cmake_macros_intro_CN.md new file mode 100644 index 0000000..9cc87ba --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/doc/intro/05_cmake_macros_intro_CN.md @@ -0,0 +1,168 @@ +# 5 **CMake编译宏介绍** + + + +## 5.1 概述 + +`rs_driver`工程目录下的`CMakeLists.txt`中，定义了一组条件编译宏，可以改变编译`rs_driver`的目标和行为。 + + + +## 5.2 指定编译目标的宏 + +`rs_driver`的编译目标包括： + ++ 示例程序，包括`demo_online`、`demo_pcap`等。 ++ 小工具，包括点云可视化工具`rs_driver_viewer`、将点云保存为PCD文件的工具`rs_driver_pcdsaver`等。 ++ 基于Google Test的自动化测试用例。 + + + +### 5.2.1 COMPILE_DEMOS + +COMPILE_DEMOS 指定是否编译示例程序。 ++ COMPILE_DEMOS=OFF，不编译。这是默认值。 ++ COMPILE_DEMOS=ON，编译。 + +``` +option(COMPILE_DEMOS "Build rs_driver demos" OFF) +``` + +### 5.2.2 COMPILE_TOOLS + +COMPILE_TOOLS指定是否编译小工具。 ++ COMPILE_TOOLS=OFF。是否编译小工具，分别取决于COMPILE_TOOLS_VIEWER和COMPILE_TOOLS_PCDSAVER。这是默认值。 ++ COMPILE_TOOLS=ON。编译`rs_driver_viewer`和`rs_driver_pcdsaver`，不管COMPILE_TOOLS_VIEWER和COMPILE_TOOLS_PCDSAVER如何设置。 + +``` +option(COMPILE_TOOLS "Build rs_driver tools" OFF) +``` + +### 5.2.3 COMPILE_TOOLS_VIEWER + +COMPILE_TOOLS_VIEWER指定在COMPILE_TOOLS=OFF时，是否编译`rs_driver_viewer`。 ++ COMPILE_TOOLS_VIEWER=OFF，不编译。这是默认值。 ++ COMPILE_TOOLS_VIEWER=ON，编译。 + +``` +option(COMPILE_TOOL_VIEWER "Build point cloud visualization tool" OFF) +``` + +### 5.2.4 COMPILE_TOOLS_PCDSAVER + +COMPILE_TOOL_PCDSAVER指定在COMPILE_TOOLS=OFF时，是否编译`rs_driver_pcdsaver`。 ++ COMPILE_TOOLS_PCDSAVER=OFF，不编译。这是默认值。 ++ COMPILE_TOOLS_PCDSAVER=ON，编译。 + +``` +option(COMPILE_TOOL_PCDSAVER "Build point cloud pcd saver tool" OFF) +``` + +### 5.2.5 COMPILE_TESTS + +COMPILE_TESTS 指定是否编译测试用例。 ++ COMPILE_TESTS=OFF，不编译。这是默认值。 ++ COMPILE_TESTS=ON，编译。 + +``` +option(COMPILE_TESTS "Build rs_driver unit tests" OFF) +``` + + + +## 5.3 改变编译行为的宏 + +### 5.3.1 DISABLE_PCAP_PARSE + +DISALBE_PCAP_PARSE 指定是否支持PCAP数据源，也就是从PCAP文件解析MSOP/DIFOP Packet。 ++ DISABLE_PCAP_PARSE=OFF，支持PCAP数据源。这是默认值。 ++ DISABLE_PCAP_PARSE=ON，不支持。在嵌入式Linux上，一般不需要解析PCAP文件，这个宏可以避免移植第三方的`libpcap`库。 + +``` +option(DISABLE_PCAP_PARSE "Disable PCAP file parse" OFF) +``` + +### 5.3.2 ENABLE_TRANSFORM + +ENABLE_TRANSFORM 指定是否支持坐标转换功能。 ++ ENABLE_TRANSFORM=OFF，不支持。这是默认值。 ++ ENABLE_TRANSFORM=ON，支持。**坐标转换功能显著消耗CPU资源，请不要在正式发布的产品中使用它。** + +``` +option(ENABLE_TRANSFORM "Enable transform functions" OFF) +``` + +### 5.3.3 ENABLE_MODIFY_RECVBUF + +ENABLE_MODIFY_RECVBUF 指定是否修改接收MSOP/DIFOP的socket的接收缓存。 + +在某些平台上（如嵌入式Linux、Windows），默认的接收缓存比较小，会导致丢包。这时，使能这个宏可以避免丢包。 ++ ENABLE_MODIFY_RECVBUF=OFF，不修改，保持系统的默认设置。这是默认值。 ++ ENABLE_MODIFY_RECVBUF=ON，修改。根据用户传入的大小（input_param_.socket_recv_buf）来修改接收缓存。 + +``` +option(ENABLE_MODIFY_RECVBUF "Enable modify size of RCVBUF" OFF) +``` + +### 5.3.4 ENABLE_WAIT_IF_QUEUE_EMPTY + +ENABLE_WAIT_IF_QUEUE_EMPTY 指定在MSOP/DIFOP Packet队列为空时，`rs_driver`的处理线程等待的方式。 ++ ENABLE_WAIT_IF_QUEUE_EMPTY=OFF， 等待条件变量通知。这是默认值。 ++ ENABLE_WAIT_IF_QUEUE_EMPTY=ON，调用usleep()。这样处理可以减少CPU资源消耗，但是会增大点云帧的延迟。 + +是否使能这个宏，需要根据具体的应用场景权衡利弊，再作决定。 + +``` +option(ENABLE_WAIT_IF_QUEUE_EMPTY "Enable waiting for a while in handle thread if the queue is empty" OFF) +``` + +### 5.3.5 ENABLE_EPOLL_RECEIVE + +ENABLE_EPOLL_RECEIVE 指定接收MSOP/DIFOP Packet的实现方式。 ++ ENABLE_EPOLL_RECEIVE=OFF，使用select()。这是默认值。 ++ ENABLE_EPOLL_RECEIVE=ON，使用epoll()。 + +``` +option(ENABLE_EPOLL_RECEIVE "Receive packets with epoll() instead of select()" OFF) +``` + +### 5.3.6 ENABLE_STAMP_WITH_LOCAL + +ENABLE_STAMP_WITH_LOCAL 指定是否将点云的时间转换为本地时间。 ++ ENABLE_STAMP_WITH_LOCAL=OFF，保持UTC时间，不转换。这是默认值。 ++ ENABLE_STAMP_WITH_LOCAL=ON，根据时区设置，转换到本地时间。 + +``` +option(ENABLE_STAMP_WITH_LOCAL "Enable stamp point cloud with local time" OFF) +``` + +### 5.3.7 ENABLE_PCL_POINTCLOUD + +ENABLE_PCL_POINTCLOUD 指定示例程序中的点云格式。 ++ ENABLE_PCL_POINTCLOUD=OFF，RoboSense自定义格式。这是默认值。 ++ ENABLE_PCL_POINTCLOUD=ON，PCL格式。使能这个选项需要PCL库的支持。 + +``` +option(ENABLE_PCL_POINTCLOUD "Enable PCL Point Cloud" OFF) +``` + +### 5.3.8 ENABLE_CRC32_CHECK + +ENABLE_CRC32_CHECK 指定对MSOP/DIFOP Packet的数据作CRC32校验。 ++ ENABLE_CRC32_CHECK=OFF，不校验。这是默认值。 ++ ENABLE_CRC32_CHECK=ON，校验。使能这个选项，需要雷达本身支持这个特性。 + +``` +option(ENABLE_CRC32_CHECK "Enable CRC32 Check on MSOP Packet" OFF) +``` + +### 5.3.9 ENABLE_DIFOP_PARSE + +ENABLE_DIFOP_PARSE 指定是否解析DIFOP Packet，得到雷达的配置和状态数据。 ++ ENABLE_DIFOP_PARSE=OFF，不解析。这是默认值。 ++ ENABLE_DIFOP_PARSE=ON，解析。注意这个特性只是解析几个域作为例子。请参考文档 [how to parse difop packet](../howto/21_how_to_parse_difop_CN.md) + +``` +option(ENABLE_DIFOP_PARSE "Enable Parsing DIFOP Packet" OFF) +``` + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/intro/06_error_code_intro.md b/src/airy/rslidar_sdk-main/src/rs_driver/doc/intro/06_error_code_intro.md new file mode 100644 index 0000000..b5b445b --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/doc/intro/06_error_code_intro.md @@ -0,0 +1,115 @@ +# 6 Introduction to rs_driver's Error Code + + + +## 6.1 Summary + +When some events occur, rs_driver informs the caller via a callback function. + +Every event is defined as an error code. Please find their definitions in `common/error_code.hpp`. + + + +## 6.2 Error Code + ++ ERRCODE_SUCCESS + +​ Normal status. `rs_driver` doesn't report it. + ++ ERRCODE_PCAPEXIT + +​ Every time `rs_driver` reaches the end of the PCAP file, it reports the event ERRCODE_PCAPEXIT. + +​ The option `RSInputParam.pcap_repeat` determines whether to play the file repeatly. If yes, rs_driver reports the event ERRCODE_PCAPREPEAT, else reports ERRCODE_PCAPEXIT. + ++ ERRCODE_PCAPREPEAT + +​ Please see ERRCODE_PCAPEXIT. + ++ ERRCODE_MSOPTIMEOUT + +​ `rs_driver` waits for MSOP packets. If it loses the packets more than 1 second, it reports the event ERRCODE_MSOPTIMEOUT. + ++ ERRCODE_NODIFOPRECV + +​ For mechanical LiDARs, DIFOP packets contains vertical/horizontal angle data. Without the data, point cloud is flat and inaccurate. + +​ On receiving MSOP packets, `rs_driver` checks if the angle data is available. If it is unavailable in 1 second, it reports ERRCODE_NODIFOPRECV. + +​ For MEMS liDARs, the angle data is not needed, so `rs_driver` doesn't report this event. + ++ ERRCODE_WRONGMSOPLEN + +​ Every LiDAR has a fixed-length MSOP packet, which has a identy id at the beginning. + +​ On receiving MSOP packets, rs_driver checks the packet length. If it is wrong, rs_driver reports the event ERRCODE_WRONGMSOPLEN. Then rs_driver checks the identity id. If it is wrong, rs_driver reports ERRCODE_MSOPID. + ++ ERRCODE_WRONGMSOPID + +​ Please refer to the description of ERRCODE_WRONGMSOPLEN. + ++ ERRCODE_WRONGMSOPBLKID + +​ MSOP Packets contains blocks. For mechanical LiDARs, every block has its own identity id. rs_driver checks if this id is wrong. If so, rs_driver reports ERRCODE_WRONGMSOPBLKID. + ++ ERRCODE_WRONGDIFOPLEN + +​ Every LiDAR has a fixed-length DIFOP packet, which has a identy id at the beginning. + +​ On receiving DIFOP packets, rs_driver checks the packet length. If it is wrong, rs_driver reports the event ERRCODE_WRONGDIFOPLEN. Then rs_driver checks the identity id. If it is wrong, rs_driver reports ERRCODE_DIFOPID. + ++ ERRCODE_WRONGDIFOPID + +​ Please refer to the description of ERRCODE_WRONGDIFOPLEN. + ++ ERRCODE_ZEROPOINTS + +​ rs_driver gets free point cloud instance from the caller, stuffs it, and returns it to the caller. + +​ Before returning, rs_driver checks if the point cloud is empty (no points). If so, rs_driver reports ERRCODE_ZEROPOINTS. + ++ ERRCODE_PKTBUFOVERFLOW + + +​ To receive MSOP/DIFOP packets as soon as possible, there is a packet queue between the recieving thread and the handling thread. + +​ If the handling thread is too busy to take packets out from the queue, the queue will overflow. rs_driver will clear it and reports ERRCODE_PKTBUFOVERFLOW. + ++ ERRCODE_CLOUDOVERFLOW + +​ rs_driver parses MSOP packets to get points, and split them into point cloud frames. + +​ For mechanical LiDARs, split by block's azimuth(horizontal angle); For MEMS liDARs, split by Packet's sequence number. + +​ If something is wrong with the packet and splitting is not triggered, rs_driver will put more and more points into the point cloud. + +​ To avoid this, rs_driver checks the point cloud, and if it is too large, rs_driver clear it, and reports ERRCODE_CLOUDOVERFLOW. + ++ ERRCODE_WRONGCRC32 + When the ENABLE_CRC32_CHECK macro is enabled, the rs_driver will perform a CRC32 check on the MSOP/DIFOP Packet data, and if the verification fails, the rs_driver will report an error ERRCODE_WRONGCRC32. + ++ ERRCODE_WRONGIMULEN + After receiving the IMU packet, rs_river first checks whether the packet length matches. If it does not match, it reports an error ERRCODE_WRONGIMULEN, and then checks the flag byte. If it does not match, it reports an error ERRCODE_WRONGIMULE. + ++ ERRCODE_WRONGIMUID + Please refer to the description of ERRCODE_WRONGIMULEN. + ++ ERRCODE_WRONGPCAPPARSE + When there is a problem with parsing the data in the pcap file (message is too long or too short), rs_driver reports an error ERRCODE-WRONGPCAPRESE. + ++ ERRCODE_STARTBEFOREINIT + +​ To use rs_driver, follow these steps: create instance, Init() and Start(). + +​ Call Init() before Start(). If not, rs_driver reports ERRCODE_STARTBEFOREINIT. + ++ ERRCODE_PCAPWRONGPATH + +​ `rs_driver` reads MSOP/DIFOP Packet from the PCAP file `RSInputParam.pcap_path`. If fails, it reports ERRCODE_PCAPWRONGPATH. + ++ ERRCODE_POINTCLOUDNULL + +​ `rs_driver` does't allocate the point cloud instance. Instead, it gets the instance from the caller via a callback function. + +​ If the instance is null, rs_drive reports ERRCODE_POINTCLOUDNULL. + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/intro/06_error_code_intro_CN.md b/src/airy/rslidar_sdk-main/src/rs_driver/doc/intro/06_error_code_intro_CN.md new file mode 100644 index 0000000..313c5b8 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/doc/intro/06_error_code_intro_CN.md @@ -0,0 +1,110 @@ +# 6 **错误码介绍** + + + +## 6.1 概述 + +`rs_driver`在内部发生某些事件时，会通过回调函数通知调用者。 + +这些事件定义在`rs_driver`工程文件`common/error_code.hpp`中。 + +这些事件有三个级别：通知、警告和错误。每个事件有一个错误码，这里介绍错误码的含义。 + + + +## 6.2 错误码 + ++ ERRCODE_SUCCESS + +​ 正常状态。`rs_driver`不会通知这个事件。 + ++ ERRCODE_PCAPEXIT + +​ 播放PCAP文件时，`rs_driver`在每次解析到文件结束时，通知调用者。 + +​ 通过选项`RSInputParam.pcap_repeat`，可以指定是否循环播放文件。如果是循环播放，`rs_driver`报告事件ERRCODE_PCAPREPEAT；如果不是，则报告事件ERRCODE_PCAPEXIT。 + ++ ERRCODE_PCAPREPEAT + +​ 请参考ERRCODE_PCAPEXIT的说明。 + ++ ERRCODE_MSOPTIMEOUT + +​ `rs_driver`持续等待和接收MSOP Packet，如果在`1`秒内没有收到MSOP Packet，`rs_driver`报告事件ERRCODE_MSOPTIMEOUT。 + ++ ERRCODE_NODIFOPRECV + +​ 对于机械式雷达，DIFOP Packet中包含垂直角标定参数。如果缺少这些参数，则点云是扁平的。 + +​ 收到MSOP Packet时，会检查这些参数是否可用。如果在`1`秒内不可用，`rs_driver`报告事件ERRCODE_NODIFOPRECV。 + +​ 对于MEMS雷达，标定工作在雷达内部已经完成，DIFOP Packet不是必要的，所以`rs_driver`不会报告这个事件。 + ++ ERRCODE_WRONGMSOPLEN + +​ 每一种雷达的MSOP Packet的包长是确定的，且包头开始位置包括若干标志字节。 + +​ `rs_driver`接收到MSOP Packet后，先检查包长是否匹配，如果不匹配，则报告错误ERRCODE_WRONGMSOPLEN，然后再检查标志字节，如果不匹配，则报告错误ERRCODE_MSOPID。 + ++ ERRCODE_WRONGMSOPID + +​ 请参考ERRCODE_WRONGMSOPLEN的说明。 + ++ ERRCODE_WRONGMSOPBLKID + +​ 对于机械式雷达，MSOP Packet中，一组点组成一个`Block`，每个`Block`也有标志字节。`rs_driver`检查`Block`的标志字节是否匹配，如果不匹配，则报告错误ERRCODE_WRONGMSOPBLKID。 + ++ ERRCODE_WRONGDIFOPLEN + +​ 每一种雷达的DIFOP Packet的包长是确定的，且包头的开始位置包括若干标志字节。 + +​ `rs_driver`接收到DIFOP Packet后，会先检查包长是否匹配，如果不匹配，则报告错误ERRCODE_WRONGDIFOPLEN，然后再检查标志字节，如果不匹配，则报告错误ERRCODE_DIFOPID。 + ++ ERRCODE_WRONGDIFOPID + +​ 请参考ERRCODE_WRONGDIFOPLEN的说明。 + ++ ERRCODE_ZEROPOINTS + +​ `rs_driver`构建好点云后，通过回调函数返还给调用者。返还前，它检查点云是否为空（即一个点都没有），如果为空，则报告错误ERRCODE_ZEROPOINTS。 + ++ ERRCODE_PKTBUFOVERFLOW + +​ `rs_driver`有两个线程：接收线程和处理线程。为了让接收线程尽快接收，防止丢包，两个线程之间有一个MSOP/DIFOP Packet队列，接收线程将Packet放入队列，处理线程从队列中取出Packet。 + +​ 如果处理线程太忙，来不及读出Packet，则这个队列的长度会超过指定的阈值，这时`rs_driver`会清空队列，并报告错误ERRCODE_PKTBUFOVERFLOW。 + ++ ERRCODE_CLOUDOVERFLOW + +​ `rs_driver`从MSOP Packet解析点，并将它们分割成点云帧。 + +​ 对于机械式雷达，按照Block的水平角分帧；对于MEMS雷达，根据Packet的序列号分帧。 + +​ 如果Packet中的数据有问题，不能触发分帧，则`rs_driver`将在当前点云实例中持续累积点，并持续消耗内存。为了避免这个问题，`rs_driver`在收到解析MSOP Packet时，检查当前点云实例中点的数量，如果超过了指定的阈值，则报告错误ERRCODE_CLOUDOVERFLOW。 + ++ ERRCODE_WRONGCRC32 + 当开启ENABLE_CRC32_CHECK这个宏时，`rs_driver`会对MSOP/DIFOP Packet的数据作CRC32校验,如果校验失败，则`rs_driver`报告错误ERRCODE_WRONGCRC32。 + ++ ERRCODE_WRONGIMULEN + `rs_driver`接收到IMU Packet后，先检查包长是否匹配，如果不匹配，则报告错误ERRCODE_WRONGIMULEN，然后再检查标志字节，如果不匹配，则报告错误ERRCODE_WRONGIMUID。 + ++ ERRCODE_WRONGIMUID + 请参考ERRCODE_WRONGIMULEN的说明。 + ++ ERRCODE_WRONGPCAPPARSE + 当pcap文件里的数据解析出现问题（报文过长或过短），则`rs_driver`报告错误ERRCODE_WRONGPCAPPARSE。 + ++ ERRCODE_STARTBEFOREINIT + +​ 使用`rs_driver`包括三个步骤：创建实例、初始化Init()、和启动Start()。使用者调用Start()之前必须先调用Init()，如果没有遵循这个次序，则`rs_driver`报告错误ERRCODE_STARTBEFOREINIT。 + ++ ERRCODE_PCAPWRONGPATH + +​ 解析PCAP文件时，`rs_driver`从读取RSInputParam.pcap_path指定的文件路径读取PCAP数据。如果这个文件打开失败，则rs_driver报告错误ERRCODE_PCAPWRONGPATH。 + ++ ERRCODE_POINTCLOUDNULL + +​ `rs_driver`不负责分配点云实例，它通过回调函数从调用者获得空闲的点云实例，填充它，然后通过回调函数返还给调用者。 + +​ 如果从调用者获得的点云实例无效，则`rs_driver`报告错误ERRCODE_POINTCLOUDNULL。 + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/intro/img/03_01_components_and_threads.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/intro/img/03_01_components_and_threads.png new file mode 100644 index 0000000..1f088f6 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/intro/img/03_01_components_and_threads.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/intro/img/03_02_interface_with_threads.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/intro/img/03_02_interface_with_threads.png new file mode 100644 index 0000000..369e864 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/intro/img/03_02_interface_with_threads.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/.xdp_01_components.QEQTH1 b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/.xdp_01_components.QEQTH1 new file mode 100644 index 0000000..6d48737 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/.xdp_01_components.QEQTH1 differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/.xdp_components.8OXVH1 b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/.xdp_components.8OXVH1 new file mode 100644 index 0000000..c50b627 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/.xdp_components.8OXVH1 differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_azimuth_section.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_azimuth_section.png new file mode 100644 index 0000000..9334b3f Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_azimuth_section.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_chan_angles.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_chan_angles.png new file mode 100644 index 0000000..e965bd9 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_chan_angles.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_decoder.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_decoder.png new file mode 100644 index 0000000..a2c723e Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_decoder.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_decoder_factory.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_decoder_factory.png new file mode 100644 index 0000000..4c9d83b Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_decoder_factory.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_decoder_mech.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_decoder_mech.png new file mode 100644 index 0000000..fe543cd Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_decoder_mech.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_decoder_rsbp.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_decoder_rsbp.png new file mode 100644 index 0000000..257b3ec Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_decoder_rsbp.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_decoder_rsm1.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_decoder_rsm1.png new file mode 100644 index 0000000..c63a853 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_decoder_rsm1.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_distance_section.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_distance_section.png new file mode 100644 index 0000000..35f2624 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_distance_section.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_input.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_input.png new file mode 100644 index 0000000..42e3665 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_input.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_input_factory.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_input_factory.png new file mode 100644 index 0000000..53af18a Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_input_factory.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_input_pcap.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_input_pcap.png new file mode 100644 index 0000000..808935a Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_input_pcap.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_input_raw.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_input_raw.png new file mode 100644 index 0000000..2a2a2ae Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_input_raw.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_input_sock.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_input_sock.png new file mode 100644 index 0000000..2671c23 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_input_sock.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_lidar_driver_impl.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_lidar_driver_impl.png new file mode 100644 index 0000000..d6b79b7 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_lidar_driver_impl.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_trigon.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_trigon.png new file mode 100644 index 0000000..99b6972 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_trigon.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/classes_block_iterator.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/classes_block_iterator.png new file mode 100644 index 0000000..2dec796 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/classes_block_iterator.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/classes_decoder.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/classes_decoder.png new file mode 100644 index 0000000..228f9bf Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/classes_decoder.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/classes_input.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/classes_input.png new file mode 100644 index 0000000..89be452 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/classes_input.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/classes_rs16_block_iterator.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/classes_rs16_block_iterator.png new file mode 100644 index 0000000..61a4f18 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/classes_rs16_block_iterator.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/classes_split_strategy.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/classes_split_strategy.png new file mode 100644 index 0000000..d83939f Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/classes_split_strategy.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/classes_split_strategy_by_seq.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/classes_split_strategy_by_seq.png new file mode 100644 index 0000000..89c76dc Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/classes_split_strategy_by_seq.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/components.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/components.png new file mode 100644 index 0000000..f71cd3b Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/components.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/fov.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/fov.png new file mode 100644 index 0000000..693a49e Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/fov.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/msop_dual_return.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/msop_dual_return.png new file mode 100644 index 0000000..17688a5 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/msop_dual_return.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/msop_ruby128.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/msop_ruby128.png new file mode 100644 index 0000000..37aceae Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/msop_ruby128.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/msop_single_return.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/msop_single_return.png new file mode 100644 index 0000000..036fbb5 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/msop_single_return.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/packet_layers.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/packet_layers.png new file mode 100644 index 0000000..acc512f Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/packet_layers.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/packet_layers_full.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/packet_layers_full.png new file mode 100644 index 0000000..6ff501b Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/packet_layers_full.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/packet_record_replay.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/packet_record_replay.png new file mode 100644 index 0000000..726cd39 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/packet_record_replay.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/rs16_msop_dual_return.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/rs16_msop_dual_return.png new file mode 100644 index 0000000..36ef680 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/rs16_msop_dual_return.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/rs16_msop_single_return.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/rs16_msop_single_return.png new file mode 100644 index 0000000..152dae0 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/rs16_msop_single_return.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/safe_range.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/safe_range.png new file mode 100644 index 0000000..8950f21 Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/safe_range.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/split_angle.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/split_angle.png new file mode 100644 index 0000000..ad69e3c Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/split_angle.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/split_position.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/split_position.png new file mode 100644 index 0000000..85458cd Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/split_position.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/trigon_sinss.png b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/trigon_sinss.png new file mode 100644 index 0000000..382f3cc Binary files /dev/null and b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/trigon_sinss.png differ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/rs_driver_intro_CN.md b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/rs_driver_intro_CN.md new file mode 100644 index 0000000..fb69360 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/rs_driver_intro_CN.md @@ -0,0 +1,1557 @@ +# **rs_driver v1.5.17 源代码解析** + + + +## 1 基本概念 + +### 1.1 机械式雷达、MEMS雷达 + +rs_driver支持RoboSense的两种雷达： ++ 机械式雷达。如RS16/RS32/RSBP/RSHELIOS/RS80/RS128/RSAIRY。机械式雷达有控制激光发射角度的旋转部件，有360°扫描视场。 ++ MEMS雷达。如RSM1。MEMS雷达是单轴、谐振式的MEMS扫描镜，其水平扫描角度可达120°。 + +### 1.2 通道 Channel + +对于机械式雷达，通道指的是垂直方向上扫描的点数，每个通道上的点连成一条线。比如，RS16是16线雷达，也就是16个通道； RSBP是32线雷达，RS128是128线雷达。 + +MEMS雷达的通道与机械式雷达不同，它的每个通道可能对应一块区域，比如一个矩形区域。 + +### 1.3 MSOP/DIFOP + +RoboSense雷达与电脑主机的通信协议有三种。 ++ MSOP (Main data Stream Ouput Protocol)。 激光雷达将扫描出来的距离、角度、反射率等信息封装成MSOP Packet，输出给电脑主机。 ++ DIFOP (Device Information Output Protocol)。激光雷达将自身的配置信息，以及当前的状态封装成DIFOP Packet，输出给电脑主机。 ++ IMU Protocol。IMU数据由激光雷达直接输出, 有些雷达的imu信息放在difop里输出，例如E1,本文主要介绍的是IMU数据输出到单独端口的情况。 ++ UCWP (User Configuration Write Protocol)。用户可以修改激光雷达的某些配置参数。 + +rs_driver处理前两类协议的包，也就是MSOP Packet和DIFOP Packet。 + +一般来说，激光雷达与电脑主机通过以太网连接，使用UDP协议。MSOP/DIFOP/IMU的格式，不同的雷达可能有较大差异。 + +### 1.4 点云帧 + ++ 机械式雷达持续旋转，输出点。扫描一圈360°得到的所有点，构成一帧点云。 + + 使用者可以指定一个角度，rs_driver按照这个角度，分割MSOP Pacekt序列得到点云。 + ++ 对于MEMS雷达，点云在MSOP Packet序列中的开始和结束位置，由雷达自己确定。 + + 一帧点云包含固定数目（比如N）的MSOP Packet。雷达对MSOP Packet从 1 到 N 编号，并一直循环。 + + + +## 2 rs_driver的组件 + +rs_driver主要由三部分组成： Input、Decoder、LidarDriverImpl。 + +![components](./img/components.png) + ++ Input部分负责从Socket/PCAP文件等数据源，获取MSOP/DIFOP Packet。Input的类一般有自己的接收线程`recv_thread_`。 ++ Decoder部分负责解析MSOP/DIFOP Packet，得到点云。Decoder部分没有自己的线程，它运行在LiarDriverImpl的Packet处理线程`handle_thread_`中。 ++ LidarDrvierImpl部分将Input和Decoder组合到一起。它从Input得到Packet，根据Packet的类型将它派发到Decoder。得到点云后，通过用户的回调函数传递给用户。 + + LidarDriverImpl提供Packet队列。Input收到MSOP/DIFOP Packet后，调用LidarDriverImpl的回调函数。回调函数将它保存到Packet队列。 + + LidarDriverImpl提供Packet处理线程`handle_thread_`。在这个线程中，将MSOP Packet和DIFOP Packet分别派发给Decoder相应的处理函数。 + + Decoder解析完一帧点云时，通知LidarDriverImpl。后者再将点云传递给用户。 ++ IMU数据的传递与点云数据类似，就不再过多介绍。 + + + +## 3 Packet接收 + +Input部分负责接收MSOP/DIFOP Packet，包括： ++ Input， ++ Input的派生类，如InputSock、InputPcap、InputRaw ++ Input的工厂类 InputFactory + +![input classes](./img/classes_input.png) + +### 3.1 Input + +Input定义接收MSOP/DIFOP Packet的接口。 ++ 成员`input_param_`是用户配置参数RSInputParam，其中包括从哪个port接收Packet等信息。 ++ Input自己不分配接收Packet的缓存。 + + Input的使用者调用Input::regCallback()，提供两个回调函数cb_get_pkt和cb_put_pkt， 它们分别保存在成员变量`cb_get_pkt_`和`cb_put_pkt_`中。 + + Input的派生类调用`cb_get_pkt_`可以得到空闲的缓存；在缓存中填充好Packet后，可以调用`cb_put_pkt_`将它返回。 + ++ Input有自己的线程`recv_thread_`。 + + Input的派生类启动这个线程读取Packet。 + +![Input](./img/class_input.png) + +### 3.2 InputSock + +InputSock类从UDP Socket接收MSOP/DIFOP Packet。雷达将MSOP/DIFOP Packet发送到这个Socket。 + +![InputSock](./img/class_input_sock.png) + ++ 一般情况下，雷达将MSOP/DIFOP/IMU Packet发送到不同的目的Port，所以InputSock创建三个Socket来分别接收它们。 + + 成员变量`fds_[3]`保存这两个Socket的描述符。`fds_[0]`是MSOP socket, `fds_[1]`是DIFOP socket。但也可以配置雷达将MSOP/DIFOP Packet发到同一个Port，这时一个Socket就够了，`fds_[1]`就是为无效值`-1`。 + + MSOP/DIFOP对应的Port值可以在RSInputParam中设置，分别对应于`RSInputParam::msop_port`和`RSInputParam::difop_port`。 ++ 一般情况下，MSOP/DIFOP Packet直接构建在UDP协议上。但在某些客户的场景下(如车联网)，MSOP/DIFOP Packet可能构建在客户的协议上，客户协议再构建在UDP协议上。这时，InputSock派发MSOP/DIFOP Packet之前，会先丢弃`USER_LAYER`的部分。成员变量`sock_offset_`保存了`USER_LAYER`部分的字节数。 + + `USER_LAYER`部分的字节数可以在RSInputParam中设置，对应于`RSInputParam::user_layer_bytes`。 ++ 有的场景下，客户的协议会在MSOP/DIFOP Packet尾部附加额外的字节。这时，InputSock派发MSOP/DIFOP Packet之前，会先丢弃`TAIL_LAYER`的部分。成员变量`sock_tail_`保存了`TAIL_LAYER`部分的字节数。 + + `TAIL_LAYER`部分的字节数可以在RSInputParam中设置，对应于`RSInputParam::tail_layer_bytes`。 + +![layers of packet](./img/packet_layers.png) + +#### 3.2.1 InputSock::createSocket() + +createSocket()用于创建UDP Socket。 ++ 调用setsockopt(), 设置选项`SO_REUSEADDR` ++ 调用bind()将socket绑定到指定的(IP, PORT)组上 ++ 如果雷达是组播模式，则将指定IP加入该组播组。 ++ 调用fcntl()设置O_NONBLOCK选项，以异步模式接收MSOP/DIFOP Packet + +该Socket的配置参数可以在RSInputParam中设置。根据设置的不同，createSocket()支持如下几种模式。 + +| msop_port/difop_port/imu_port | host_address | group_address | | +|:-----------:|:----------------------:|:-----------------:|:-------------| +| 6699/7788/6688 | 0.0.0.0 | 0.0.0.0 | 雷达的目的地址可以为广播地址、或电脑主机地址 | +| 6699/7788/6688 | 192.168.1.201 | 0.0.0.0 | 雷达的目的地址可以为电脑主机地址 | +| 6699/7788/6688 | 192.168.1.201 | 239.255.0.1 | 雷达的目的地址可以为组播地址、或电脑主机地址 | + +#### 3.2.2 InputSock::init() + +init() 调用createSocket()，创建三个Socket,分别接收MSOP Packet、DIFOP Packet和IMU Packet。 + +#### 3.2.3 InputSock::start() + +start() 开始接收MSOP/DIFOP Packet。 ++ 启动接收线程，线程函数为InputSock::recvPacket() + +#### 3.2.4 InputSock::recvPacket() + +recvPacket() 接收MSOP/DIFOP Packet。 +在while()循环中， + ++ 调用FD_ZERO()初始化本地变量`rfds`，调用FD_SET()将`fds_[3]`中的两个fd加入`rfds`。当然，如果MSOP/DIFOP Packet共用一个socket， 无效的`fds_[1]`就不必加入了。 ++ 调用select()在`rfds`上等待Packet, 超时值设置为`1`秒。 +如果select()的返回值提示`rfds`上有信号，调用FD_ISSET()检查是`fds_[]`中的哪一个fd可读。对这个fd， ++ 调用回调函数`cb_get_pkt_`， 得到大小为`MAX_PKT_LEN`的缓存。`MAX_PKT_LEN` = `1500`，对当前RoboSense雷达来说，够大了。 ++ 调用recvfrom()接收Packet，保存到这个缓存中 ++ 调用回调函数`cb_put_pkt_`，将Packet派发给InputSock的使用者。 + + 注意在派发之前，调用Buffer::setData()设置了MSOP Packet在Buffer的中偏移量及长度，以便剥除`USER_LAYER`和`TAIL_LAYER`（如果有的话）。 + +### 3.3 InputPcap + +InputPcap解析PCAP文件得到MSOP/DIFOP Packet。使用第三方工具，如WireShark，可以将雷达数据保存到PCAP文件中。 + +![InputSock](./img/class_input_pcap.png) + ++ InputPcap基于第三方的libpcap库，使用它可以遍历PCAP文件，依次得到所有UDP Packet。 + + 成员变量`pcap_`变量保存Pcap文件指针，`pcap_t`定义来自libpcap库。 + ++ 与InputSock一样，在有的客户场景下，InputPcap也需要处理`USER_LAYER`和`TAIL_LAYER`的情况。InputPcap的成员`pcap_offset_`和`pcap_tail_`分别保存`USER_LAYER`和`TAIL_LAYER`的字节数。 ++ 但也有不同的地方。InputSock从Socket接收的Packet只有UDP数据部分，而InputPcap从PCAP文件得到的Packet不同，它包括所有Packet的所有层。`pcap_offset_`除了`USER_LAYER`的长度之外，还要加上其他所有层。 + + 对于一般的以太网包，`pcap_offset_`需要加上其他层的长度，也就是 `14`(ETHERNET) + `20`(IP) + `8`(UDP) = `42` 字节。 + + 如果还有VLAN层，`pcap_offset_`还需要加上 `4` 字节。 + +![layers of packet](./img/packet_layers_full.png) + ++ PCAP文件中可能不止包括MSOP/DIFOP Packet，所以需要使用libpcap库的过滤功能。libpcap过滤器`bpf_program`，由库函数pcap_compile()生成。成员`msop_filter_`和`difop_filter_`分别是MSOP Packet和DIFOP Packet的过滤器。 + + MSOP/DIFOP Packet都是UDP Packet，所以给pcap_compile()指定选项`udp`。 + + 如果是基于VLAN的，则需要指定选项`vlan` + + 如果在一个PCAP文件中包含多个雷达的Packet，则还需要指定选项 `udp dst port`，以便只提取其中一个雷达的Packet。 + + +用户配置参数RSInputParam中指定选项`udp dst port`。有如下几种情况。 + +| msop_port | difop_port | 说明 | +|:-----------:|:-------------:|:-------------| +| 6699 | 7788 | 如果PCAP文件中包含多个雷达的Packet，则可以只提取指定雷达的Packet（该雷达MSOP/DIFOP端口不同） | +| 6699 | 6699/0 | 如果PCAP文件中包含多个雷达的Packet，则可以只提取指定雷达的Packet（该雷达DIFOP/DIFOP端口相同） | + +#### 3.3.1 InputPcap::init() + +init()打开PCAP文件，构造PCAP过滤器。 ++ 调用pcap_open_offline()打开PCAP文件，保存在成员变量`pcap_`中。 ++ 调用pcap_compile()构造MSOP/DIFOP/IMU Packet的PCAP过滤器。 + + 如果它们使用不同端口，则需要三个过滤器，分别保存在`mosp_filter_`、`difop_filter_`和`imu_filter_`。 + + 如果使用同一端口，那么`difop_filter_`和`imu_filter_`就不需要了。 + +#### 3.3.2 InputPcap::start() + +start()开始解析PCAP文件。 ++ 调用std::thread()，创建并启动PCAP解析线程，线程的函数为recvPacket()。 + +#### 3.3.3 InputPcap::recvPacket() + +recvPacket()解析PCAP文件。 +在循环中， ++ 调用pcap_next_ex()得到文件中的下一个Packet。 + +如果pcap_next_ex()还能读出Packet， ++ 本地变量`header`指向Packet的头信息，变量`pkt_data`指向Packet的数据。 ++ 调用pcap_offline_filter()，使用PCAP过滤器校验Packet（检查端口、协议等是否匹配）。 + +如果是MSOP Packet, + + 调用`cb_get_pkt_`得到大小为`MAX_PKT_LEN`的缓存。`MAX_PKT_LEN` = `1500`，对当前的RoboSense雷达来说，够大了。 + + 调用memcpy()将Packet数据复制到缓存中，并调用Buffer::setData()设置Packet的长度。复制时剥除了不需要的层，包括`USER_LAYER`和`TAIL_LAYER`（如果有的话）。 + + 调用回调函数`cb_put_pkt_`，将Packet派发给InputSock的使用者。 + +如果是DIFOP/IMU Packet，处理与MSOP Packet一样。 + ++ 调用this_thread::sleep_for()让解析线程睡眠一小会。这是为了模拟雷达发送MSOP Packet的间隔。这个间隔时间来自每个雷达的`Decoder`类，每个雷达有自己的值。在Decoder部分，会说明如何计算这个值。 + +如果pcap_next_ex()不能读出Packet，一般意味着到了文件结尾，则： ++ 调用pcap_close()关闭pcap文件指针 `pcap_` 。 + +用户配置RSInputParam的设置决定是否重新进行下一轮的解析。这个选项是`RSInputParam::pcap_repeat`。 ++ 如果这个选项为真，调用pcap_open_offline()重新打开PCAP文件。这时成员变量`pcap_`回到文件的开始位置。下一次调用pcap_next_ex()，又可以重新得到PCAP文件的第一个Packet了。 + +### 3.4 InputRaw + +InputRaw是为了重播MSOP/DIFOP Packet而设计的Input类型。将在后面的Packet Record/Replay章节中说明。 + +### 3.5 InputFactory + +InputFactory是创建Input实例的工厂。 + +![InputFactory](./img/class_input_factory.png) + +Input类型如下。 + +``` +enum InputType +{ + ONLINE_LIDAR = 1, // InputSock + PCAP_FILE, // InputPcap + RAW_PACKET // InputRaw +}; +``` + +#### 3.5.1 InputFactory::creatInput() + +createInput() 根据指定的类型，创建Input实例。 ++ 创建InputPcap时，需指定`sec_to_delay`。这是InputPcap回放MSOP Packet的间隔。 ++ 创建InputRaw时，需指定`cb_feed_pkt`。这个将在后面的Packet Record/Replay章节中说明。 + + + +## 4 Packet解析 + +### 4.1 MSOP格式 + +这里说明MSOP格式中这些字段。 ++ 距离 `distance`、 ++ 角度 ++ 发射率 `intensity`、 ++ 通道 `ring`、 ++ 时间戳 `timestamp`、 ++ 温度 `temperature` ++ 回波模式 `echo_mode` + +其中前五个与点云直接相关。 + +MSOP格式中的点是极坐标系的坐标，包括极径和极角。距离就是这里的极径。从距离和角度可计算直角坐标系的坐标，也就是点云使用的坐标。 + +#### 4.1.1 Distance + +Distance用两个字节表示。它乘以一个解析度得到真实距离。 ++ 不同的雷达的解析度可能不同。 ++ 特定于雷达的配置参数`RSDecoderConstParam::DISTANCE_RES`保存这个解析度。 + +``` +uint16_t distance; +``` +#### 4.1.2 角度 + ++ 对于机械式雷达， MSOP格式的azimuth保存点的极角（水平角）。 + + ``` + uint16_t azimuth; + ``` + + 要计算点的直角坐标系坐标，除了`distance`和`azimuth`之外，还需要一个垂直角。 + + 垂直角从DIFOP Packet得到，一个机械式激光雷达有一组固定的垂直角，每个通道一个。后面的章节将说明垂直角。 + + + 水平角是MOSP Packet中点的`azimuth`。 + ++ 对于MEMS雷达， 角度是`pitch`和`yaw`。 + + ``` + uint16_t yaw; + uint16_t pitch; + ``` + 从`distance`、`pitch`、和`yaw`，可计算直角坐标系的坐标。 + ++ 雷达的角度分辨率是`0.01`度。这意味着一圈`360`度，可以划分成`36000`个单位。 + ++ MSOP格式中，角度以`0.01`度为单位，范围是(`0`, `36000`)，所以可以用`uint16_t`来表示。 + +#### 4.1.3 intensity + +`intensity`保存在1个字节中。 + +``` +uint8_t intensity; +``` + +#### 4.1.4 ring + +`ring`在后面的ChanAngles章节说明。 + +#### 4.1.5 timestamp + +RoboSense雷达使用了几种时间戳格式。 + +##### 4.1.5.1 YMD 格式 + +YMD格式定义如下，parseTimeYMD()负责解析这种时间戳格式。遵循这种格式的有RS16/RS32/RSBP等。 + +``` +typedef struct +{ + uint8_t year; + uint8_t month; + uint8_t day; + uint8_t hour; + uint8_t minute; + uint8_t second; + uint16_t ms; + uint16_t us; +} RSTimestampYMD; +``` + +##### 4.1.5.2 UTC 格式 + +UTC格式定义如下。 ++ 成员`sec[6]`保存的是秒数， ++ 成员`ss[4]`保存微秒值或纳秒值。 + +``` +typedef struct +{ + uint8_t sec[6]; + uint8_t ss[4]; +} RSTimestampUTC; +``` ++ 如果`ss[4]`保存微秒值，使用parseTimeUTCWithUs()解析。遵循这种格式的有RSHELIOS/RSM1。 ++ 如果`ss[4]`保存纳秒值，使用parseTimeUTCWithNs()解析。 ++ 目前出货的RS128/RS80都遵循微秒格式，只有早期出货的一些RS128/RS80是纳秒格式。当前版本的rs_driver只支持微秒格式的解析。 + +#### 4.1.6 temperature + +RoboSense雷达使用了几种温度格式。 + +##### 4.1.6.1 用解析度表示温度 + +一种是用2字节表示温度值，这个值乘以一个解析度得到真实温度。 ++ 特定于雷达的配置参数`RSDecoderConstParam::TEMPERATURE_RES`保存这个解析度。 + +``` +typedef struct +{ + uint8_t tt[2]; +} RSTemperature; +``` + ++ 如果这两个字节是`littlen endian`格式，使用parseTempInBe()解析。遵循这种格式的有RS16/RS32/RSBP/RSHELIOS。 ++ 如果这两个字节是`big endian`格式，使用parseTempInLe()解析。遵循这种格式的有RS80/RS128。 + +##### 4.1.6.2 相对温度 + +另一类用1个字节表示温度。这个值加上一个初始值得到真实温度。遵循这种格式的有RSM1。 ++ 特定于雷达的配置参数`RSDecoderConstParam::TEMPERATURE_RES`保存这个初始值。 + +``` +int8_t temperature; +``` + +#### 4.1.7 echo_mode + +雷达内部有多种回波模式。 ++ 最强回波， ++ 最后回波， ++ 双回波， + +MSOP格式中用一个字节表示： + +``` +int8_t return_mode; +``` + +但rs_driver并不在意是回波是“最强的”，还是“最后的”。因为影响MSOP解析的只是：有几个回波？ + +如下是才是rs_driver关心的回波模式。 + +``` +enum RSEchoMode +{ + ECHO_SINGLE = 0, + ECHO_DUAL +}; +``` + +不同雷达有不同的回波模式`return_mode`。每个Decoder实现自己的解析函数getEchoMode()，得到rs_driver的回波模式。 + +回波模式会影响MSOP Packet中数据的布局，还可能影响点云的分帧。 + +### 4.2 ChanAngles + +#### 4.2.1 垂直角/水平角的修正 + +如前面MSOP格式的章节所说，理论上，从distance、垂直角、水平角就可以计算点的直角坐标系的坐标。 + +但在生产实践中，装配雷达总是无可避免地有细微的误差，导致雷达的角度不精确，需要进行修正。雷达装配后的参数标定过程，会找出相关的修正值，写入雷达的寄存器。标定后，使用修正值调整点，就可以使其精度达到要求。 + +MEMS雷达的角度修正，在雷达内部完成，所以rs_driver不需要做什么；机械式雷达的角度修正，由rs_driver在电脑主机端完成。 + +这里说明机械式雷达的垂直角/水平角修正。 + +对于机械式雷达，每个通道的垂直角是固定的。以RSBP举例，它的理论垂直角如下。（这里有`32`个值，对应RSBP的`32`个通道） + +``` + 89.5, 81.0625, 78.25, 72.625, 67, 61.375, 55.75, 50.125, + 86.6875, 83.875, 75.4375, 69.8125, 64.1875, 58.5625, 52.9375, 47.3125, + 44.5, 38.875, 33.25, 27.625, 22, 16.375, 10.75, 5.125, + 41.6875, 36.0625, 30.4375, 24.8125, 19.1875, 13.5625, 7.9375, 2.3125 +``` + +装配过程中的误差，导致雷达的垂直角不是这里列出的理论值，水平角`azimuth`也不是从零开始。标定过程找出两组修正值，一组针对垂直角，一组针对水平角。 + +还是以RSBP为例。标定过程后，实际的垂直角可能是这样的。这里修正值已经累加了原来的垂直角。 + +``` + 89.4375, 81.0625, 78.25, 72.625, 67, 61.375, 55.75, 50.125, + 86.8125, 83.875, 75.4375, 69.8125, 64.1875, 58.5, 52.9375, 47.3125, + 44.5625, 38.875, 33.25, 27.625, 22, 16.375, 10.75, 5.125, + 41.6875, 36.1875, 30.4375, 24.8125, 19.0625, 13.5625, 7.9375, 2.3125 +``` + +类似的，水平角修正值的例子如下。（这里也有32个值，对应RSBP的32个通道） + +``` + 0.0625, 0.0625, -0.25, 0.625, 0, -0.375, 0.75, -0.125, + -0.3125, 0.875, -0.4375, 0.8125, 0.1875, 0.5, -0.9375, 0.3125, + 0.0, -0.875, 0.25, -0.625, 0, -0.375, 0.75, 0.125, + 0.125, 0.1875, 0.4375, 0.8125, 0.0625, 0.5625, 0.9375, 0.3125 +``` + +这两组修正值在参数标定过程中写入雷达寄存器，它们也包含在DIFOP Packet中。 + +#### 4.2.2 ChanAngles定义 + +ChanAngles从DIFOP Packet读入机械式雷达的垂直角/水平角的修正值。如果雷达修正值无效，也可以从外部文件读入。 + +如前面所说，只有机械式雷达需要ChanAngles。 + +![ChanAngles](./img/class_chan_angles.png) + ++ 成员变量chan_num_是雷达的通道数，用于决定修正值数组的大小。 ++ 成员变量vert_angles_是保存垂直角修正值的数组 ++ 成员变量horiz_angles_是保存水平角修正值的数组。 + +#### 4.2.3 ChanAngles::loadFromDifop() + +loadFromDifop()从DIFOP Packet读入角度修正值，写入成员`vert_angles_[]`和`horiz_angles_[]`。 ++ 它还调用genUserChan()， 设置用户通道编号数组。 + ++ 在DIFOP Packet中，修正值保存在RSClibrationAngle结构中。 + + 成员`value`是非负值， + + 成员`sign`则指定正负; `0`则修正值为正；除`0xFF`以外的非`0`值，则修正值为负；为`0xFF`时，修正值无效。 + +``` +typedef struct +{ + uint8_t sign; + uint16_t value; +} RSCalibrationAngle; +``` + +对于RSBP, MSOP Packet中的修正值保存在成员`vert_angle_cali[]`和`horiz_angle_cali[]`中。 + +``` +typedef struct +{ + ... + + RSCalibrationAngle vert_angle_cali[32]; + RSCalibrationAngle horiz_angle_cali[32]; + + ... +} RSBPDifopPkt; +``` + +#### 4.2.4 早期雷达适配ChanAngles + ++ 不是所有雷达的修正值都保存在RSCalibrationAngle中。比如早期的雷达RS16，它的修正值保存在成员`pitch_cali[]`中。 + + ``` + typedef struct + { + ... + uint8_t pitch_cali[48]; + ... + } RS16DifopPkt; + + ``` + + 为了像其他雷达一样处理RS16，将RS16DifopPkt适配到一个能兼容RSCalibrationAngle的结构 AdapterDifopPkt, + + ``` + typedef struct + { + uint16_t rpm; + RSFOV fov; + uint8_t return_mode; + RSCalibrationAngle vert_angle_cali[32]; + RSCalibrationAngle horiz_angle_cali[32]; + } AdapterDifopPkt; + ``` + + RS16使用了转换函数RS16DifopPkt2Adapter()，从RS16DifopPkt构造一个AdapterDifopPkt。 + + ``` + void RS16DifopPkt2Adapter (const RS16DifopPkt& src, AdapterDifopPkt& dst); + ``` + ++ RS32也有类似的情况。虽然它的修正值也保存在RSCalibrationAngle数组中，但角度值的含义不同。 + + ``` + typedef struct + { + ... + RSCalibrationAngle vert_angle_cali[32]; + RSCalibrationAngle horiz_angle_cali[32]; + ... + } RS32DifopPkt; + ``` + + 与RS16类似，也将RS32DifopPkt适配到AdapterDifopPkt。RS32使用的转换函数是RS32DifopPkt2Adapter()。 + +#### 4.2.5 ChanAngles::loadFromFile() + +有些场景下，雷达可能还没有写入有效的修正值，或者是因为还没有标定，或者是由于雷达故障。这时可以从外部文件读入角度修正值。 + +文件格式如下。 ++ 每行是对应一个通道的修正值。其中第`1`个值是垂直角，第`2`个值是水平角修正值。 ++ 每行对应一个通道。所以对于RSBP来说，应该有`32`行。这个例子略去了部分行。 + +``` +89.5,0.125 +81.0625,-0.025 +78.25,0 +72.625,0 +... +30.4375,0.625 +24.8125,0 +19.1875,-0.25 +13.5625,0 +7.9375,-0.1 +2.3125,0 +``` +loadFromFile() 解析这个文件得到修正值，写入成员`vert_angles_[]`和`horiz_angles_[]`。 ++ 它还调用genUserChan()， 设置用户通道编号数组。 + +#### 4.2.6 ChanAngles::horizAdjust() + +horizAdjust()对参数给出的水平角作修正 ++ 根据内部通道编号得到水平角修正值， ++ 水平角加入这个修正值，并返回 + +#### 4.2.7 ChanAngles::vertAdjust() + +vertAdjust()根据内部通道编号，得到修正后的垂直角。 + +#### 4.2.8 点云的ring值 + +点云中的`ring`值是从用户角度看的通道编号，它来自于ChanAngles的成员变量`user_chans_`。 + +回到RSBP的例子。如下是它的垂直角，它们按照雷达内部通道编号排列，而不是降序或升序排列。换句话说，雷达内部通道不是按照垂直角的升序/降序编号的。 + +``` + 89.5, 81.0625, 78.25, 72.625, 67, 61.375, 55.75, 50.125, + 86.6875, 83.875, 75.4375, 69.8125, 64.1875, 58.5625, 52.9375, 47.3125, + 44.5, 38.875, 33.25, 27.625, 22, 16.375, 10.75, 5.125, + 41.6875, 36.0625, 30.4375, 24.8125, 19.1875, 13.5625, 7.9375, 2.3125 +``` +但用户希望看到通道编号按照垂直角按升序/降序排列。 + +ChanAngles的成员变量`user_chans`保存的是按升序排列的编号，也就是角度小的通道在前。 + +#### 4.2.9 ChanAngles::genUserChan() + +genUserChan()根据成员变量`vert_angles_[]`中的角度值，计算升序排列的用户通道编号数组。 + +#### 4.2.10 ChanAngles::toUserChan() + +toUserChan()，从给出的雷达内部通道编号，得到用户通道编号。 + +### 4.3 Trigon + +#### 4.3.1 查表计算三角函数值 + +如前面所说，MSOP Packet中的点是极坐标系的坐标。rs_driver将点坐标，从极坐标系转换为用户使用的直角坐标系。这时需要计算角度的sin/cos值。 + +调用三角函数又耗时又耗CPU资源，优化的方式是查表。 ++ 首先确定表的范围。 + + 垂直角的范围在(`-90`,`90`)内。加上修正值，也还是在(`-90`, `90`)内。 + + 水平角的范围在(`0`, `360`)内。加上修正值，在(`-90`, `450`)内。 + ++ MSOP格式中，角度以`0.01`度为单位。rs_driver也是这样。对于(`-90`, `450`)的角度范围，需要对(`-9000`, `45000`)内的整数角度值，计算sin/cos值。 + +#### 4.3.2 Trigon定义 + +Trigon用于计算指定范围内的sin/cos值，并用于查询。 + +![Trigon](./img/class_trigon.png) + ++ 成员变量`ANGLE_MIN`和`ANGLE_MAX`保存角度范围。这里`ANGLE_MIN` = `-9000`, `ANGLE_MAX` = `45000`。 ++ 成员变量`o_sins_`保存所有角度的sin值，`o_coss_`保存所有角度的cos值。`o_sins_[]`和`o_coss_[]`是两个大小为 `AMGLE_MAX - ANGLE_MIN` 的数组。 ++ 引用`os_sins_[]`和`o_coss_[]`计算三角函数值时，需要减去一个偏移。为了免去这个麻烦，重新定义了两个指针`sins_`和`coss_`，让它们分别指向`os_sins_[9000]`和`os_cons_[9000]`。这样就可以用角度值直接引用`sins_`和`coss_`了。 + +![Trigon](./img/trigon_sinss.png) + +#### 4.3.3 Trigon::Trigon() + +Trigon的构造函数Trigon() 负责初始化`o_sins_[]`和`o_coss_[]`。 ++ 根据角度范围，给`o_sins[]`和`o_coss_[]`分配相应大小的空间， ++ 遍历范围内的角度值，调用std::sin()和std::cos()，将三角函数值分别保存到`o_sins_[]`和`o_coss_[]`中。 ++ 让`sins_`指向`sins_[]`中`0`度角的位置，这里是`sins_[9000]`。类似地设置`coss_`。 + +#### 4.3.4 Trigon::sin() + +sin()查表返回角度的sin值。 + +#### 4.3.5 Trigon::cos() + +cos()查表返回角度的cos值。 + +### 4.4 BlockIterator + +这一节"BlockIterator"，仅针对机械式雷达。 + +#### 4.4.1 Packet、Block、Channel + +在MSOP格式中，每个Packet中有`BLOCKS_PER_PKT`个Block，每个Block中有`CHANNELS_PER_BLOCK`个Channel。 ++ 这里的`BLOCKS_PER_PKT`和`CHANNEL_PER_BLOCK`分别在雷达配置参数`RSDecoderConstParam`中指定。 + +对于机械式雷达，雷达持续旋转，垂直方向上的每一轮激光发射，在MSOP Packet中对应一个Block。 +以RSBP雷达为例， ++ 一轮就是`32`次激光发射，对应`32`个channel。所以`RSDecoderConstParam::CHANNELS_PER_BLOCK` = `32`。 ++ MSOP的设计初衷，当然是向每个Packet尽可能多塞几个Block，这样就有`RSDecoderConstParam::BLOCKS_PER_PKT` = `12`。 + +![msop single return](./img/msop_single_return.png) + +雷达的每轮激光发射时序包括充能、发射等步骤。虽然每轮发射的持续时间（也是相邻两轮发射的时间差）相同，但在每轮发射内，每次发射的时间不是均匀分布。 + +以RSBP为例， + ++ 一轮发射的时长为`55.52`微秒，这是Block之间的时间差。 ++ 一轮发射内，`32`次发射的时间戳如下（相对于Block的相对时间，单位微秒）。这是每个Channel对所属Block的相对时间。 + +``` + 0.00, 2.56, 5.12, 7.68, 10.24, 12.80, 15.36, 17.92, + 25.68, 28.24, 30.80, 33.36, 35.92, 38.48, 41.04, 43.60, + 1.28, 3.84, 6.40, 8.96, 11.52, 14.08, 16.64, 19.20, + 26.96, 29.52, 32.08, 34.64, 37.20, 39.76, 42.32, 44.88 +``` + +#### 4.4.2 Channel的时间戳 + +MSOP格式中，Packet头部包含一个时间戳。 + +如RSBP雷达，Packet的时间戳如下。 + +``` +RSTimestampYMD timestamp; +``` + +通过如下方式可以计算Channel的时间戳。 + +``` +Block的时间戳 = Packet的时间戳 + Block的持续时间 * Block数 +Channel的时间戳 = 所在Block的时间戳 + Channel对Block的相对时间 +``` + +#### 4.4.3 Channel的角度 + +在MSOP格式中，Block的成员中包括水平角`azimuth`。 + +雷达的旋转当然不是匀速的，但放到一个Block这么短的时间内看，认为旋转是匀速的，还是合理的。 + +所以，通过Channel占Block的时间比例，可以估计Channel对Block的相对水平角。 + +``` +Channel的水平角 = Block的水平角 + 当前Block与下一个Block的水平角差 * Channel对Block的相对时间 / Block的持续时间 +``` + +#### 4.4.4 双回波模式的影响 + +双回波模式下，虽然一个Packet还是塞了同样数目的Block，但是第二个回波的Block，其水平角/时间戳与第一个回波相同。 + +如下是双回波模式下，RSBP的MSOP格式。 + +![msop dual return](./img/msop_dual_return.png) + +这样，遍历Block序列时，计算Block时间戳/角度差的方式就不一样了。 + +#### 4.4.5 BlockIterator定义 + +引入BlockIterator的目的，是定义一个接口来计算： ++ Block的时间戳。这个时间戳是相对于Packet的时间戳。 ++ Block与下一次Block的水平角差。也就是当前Block内雷达旋转的水平角。 + +BlockIterator的成员如下。 ++ 成员变量`pkt_`是Packet ++ 成员变量`BLOCKS_PER_PKT`是Packet中的Block数 ++ 成员`BLOCK_DURATION`是Block的持续时间 ++ 成员az_diffs[]保存所有Block的水平角差 ++ 成员tss[]保存所有Block的时间戳 + +![](./img/classes_block_iterator.png) + +##### 4.4.5.1 BlockIterator::get() + +成员函数get()从成员az_diffs[]和tss[]，得到Block的时间戳和水平角差。BlockIterator的派生类应该计算这两个数组中的值。 + +#### 4.4.6 SingleReturnBlockIterator + +SingleReturnBlockIterator实现单回波模式下的BlockIterator接口。 + +##### 4.4.6.1 SingleReturnBlockIterator() + +单回波模式下。 +在构造函数中，遍历Packet中的block，并计算az_diffs[]和tss[]。 + ++ Block之间的时间差是固定值，也就是`BLOCK_DURATION`。 ++ 1个Packet有`BLOCKS_PER_PKT`个Block。 + + + 对于前面的Block， + + ``` + Block水平角差 = 下一个Block的水平角 - 当前Block的水平角 + ``` + + 最后一个Block的水平角差，认为等于`BLOCK_AZ_DURATION`，这是雷达理论上每个Block的水平角差。 + ++ 相邻Block可能跨`角度0`，所以它们的水平角差可能小于`0`，这时需要将它修正到[`0`, `36000`)内。 + +#### 4.4.7 DualReturnBlockIterator + +DualReturnBlockIterator实现双回波模式下的BlockIterator接口。 + +##### 4.4.7.1 DualReturnBlockIterator() + +双回波模式下，Block两两成对。 +在构造函数中，遍历Packet中的Block，并计算az_diffs[]和tss[]。遍历时步进值为2。 + ++ 步进差为2的Block, 时间差为`BLOCK_DURATION`。奇数Block和前一个偶数Block的时间戳相同。 + + 对于前面的Block, + + ``` + Block水平角差 = 下下个Block的水平角 - 当前Block的水平角 + ``` + + + 最后两个Block的角度差，认为等于`BLOCK_AZ_DURATION`，这是雷达理论上每个Block的水平角差。 + ++ 由于相邻Block可能跨`角度0`，所以它们的水平角差可能小于`0`，这时需要将它修正到 [`0`, `36000`)内。 + +#### 4.4.8 ABReturnBlockIterator + +双回波模式下，Ruby128是个特殊情况。 + +Ruby128的每个Block有`128`个Channel，每个Block占的空间太大，以至于每个packet只能放下`3`个Block。这样一次扫描的两个Block可能在不同的Packet中。相邻的两个packet格式如下图。 + +![msop ruby128](./img/msop_ruby128.png) + +ABReturnBlockIterator类用于计算Ruby128的双回波模式的时间戳和角度。 + +##### 4.4.8.1 ABDualReturnBlockIterator() + +根据第0个Block和第1个Block的角度是否相同，判断这是一个`AAB` Packet还是`BAA` Packet。 ++ `A`与`B`之间的时间差为`BLOCK_DURATION`。 + ++ 不论是`AAB` Packet，还是`BAA` Packet, Block的角度差都是`A`与`B`之间的角度差。 + +``` +Block水平角差 = 第三个Block的水平角 - 第一个Block的水平角 +``` + +#### 4.4.9 RS16的Packet格式 + +为了充分利用MSOP Packet的空间，16线雷达(如RS16)的Packet格式与其他机械式雷达不同。 + +在单回波模式下，一组`16通道数据`包含一个回波，将相邻两组的回波数据放在同一Block中。如下图所示。 + +![](./img/rs16_msop_single_return.png) + +在双回波模式下，一组`16通道数据`就有两个回波，将两个回波的数据放在同一Block中。如下图所示。 + +![](./img/rs16_msop_dual_return.png) + +这样的结果是， ++ MSOP Packet中的相邻Block之间的角度差不再一定是`BLOCK_AZ_DURATION`，时间差也不再一定是`BLOCK_DURATION`。 ++ 对于RS16，RSDecoderConstParam.CHANNELS_PER_BLOCK = 32。遍历所有通道时，这个值需要做一次映射，才能得到实际的通道值。 + +``` +uint16_t laser = chan % 16; +``` + +RS16SingleReturnBlockIterator和RS16DualReturnBlockIterator，分别处理RS16单回波模式和双回波模式的情况。 ++ 新加入的成员函数calcChannel()，计算Block内每个Channel的角度占比，和时间戳偏移。 + +![](./img/classes_rs16_block_iterator.png) + +#### 4.4.9 RS16SingleReturnBlockIterator + +##### 4.4.9.1 Rs16SingleReturnBlockIterator() + +在构造函数中，遍历Packet中的Block，并计算az_diffs[]和tss[]。 + +与SingleReturnBlockIterator()中不同的地方是：单回波模式下，一个Block中包括相邻的两个通道数据。 ++ 缺省的角度差是`BLOCK_AZ_DURATION` * 2 ++ 缺省的时间差是`BLOCK_DURATION` * 2 + +##### 4.4.9.2 RS16SingleReturnBlockIterator::calcChannel() + +calcChannel()计算单回波模式下，32个Channel的角度占比，和时间戳偏移。 + +#### 4.4.10 RS16DualReturnBlockIterator + +##### 4.4.10.1 Rs16DualReturnBlockIterator() + +在构造函数中，遍历Packet中的Block，并计算az_diffs[]和tss[]。 + +与Rs16DualReturnBlockIterator不同的地方是：双回波模式下，一个Block中包括两个回波的数据。 + ++ 缺省的角度差是`BLOCK_AZ_DURATION` ++ 缺省的时间差是`BLOCK_DURATION` + +##### 4.4.10.2 RS16SingleReturnBlockIterator::calcChannel() + +calcChannel()计算双回波模式下，32个Channel的角度占比,和时间戳偏移。 + +### 4.5 FOV + +机械式雷达的扫描角度是[0,360)，这也是雷达输出点的角度范围。 + +也可以对雷达进行设置，限制它的输出角度，如下图。 ++ FOV的范围是[start_angle,end_angle)。 ++ 与FOV互补的角度范围FOV_blind是FOV的盲区，雷达不会输出这个范围的点。 + +![msop ruby128](./img/fov.png) + +#### 4.5.1 FOV设置 + +FOV可以从DIFOP Packet得到。 + +``` + RSFOV fov; +``` + +RSFOV的定义如下。 + +``` +typedef struct +{ + uint16_t start_angle; + uint16_t end_angle; +} RSFOV; +``` + +在DecoderMech::decodeDifopCommon()中解析DIFOP Packet得到FOV。 ++ 这里计算雷达扫描跨过盲区的时间差，也就是DecoderMech的成员`fov_blind_ts_diff_`. + +``` +void DecoderMech::decodeDifopCommon(const T_Difop& pkt); +``` + +#### 4.5.2 FOV对BlockIterator的影响 + +在BlockIterator的各种实现中，需要考虑Packet的相邻两个Block跨过FOV盲区的情况。 +如果跨过盲区，则： + ++ 第一个Block的水平角度差调整为`BLOCK_AZ_DURATION`。这时理论上每个Block的水平角差。 ++ 两个Block的时间差调整为`FOV_BLIND_DURATION`。这个值是盲区时间差，也就是前面说的`fov_blind_ts_diff_`。 + +### 4.6 分帧 + +机械式雷达和MEMS雷达的分帧策略不同。 + +#### 4.6.1 机械式雷达的分帧模式 + +机械式雷达持续旋转，输出点，驱动在某个分割位置分割前后帧。有三种分帧模式。 ++ 按Block的水平角分帧。这是默认的分帧模式。 + + 如果Block的水平角刚好跨过指定的水平角，则分帧。 + + 雷达的转动不是均匀的，所以每帧包含的Block数可能会有细微的变动，相应地，包含的点数也会变动。 + +![split angle](./img/split_angle.png) + ++ 按理论上每圈的Block数分帧。这样每帧包含的Block数和点数都是固定的。 + + Robosense雷达支持两种转速：`600`圈/分钟和`1200`圈/分钟。以`600`圈/分钟距离，相当于每圈`0.1`秒，而Block的持续时间是固定的，由此计算理论上每圈的Block数（实际上是假设雷达转速均匀） + +``` + 每圈Block数 = 每圈秒数 / Block持续时间 +``` + + 理论上每圈Block数，在不同回波模式下不同。上面的计算是针对单回波模式。如果是双回波模式，则每圈Block数要加倍。 + ++ 按照使用者指定的Block数分帧。当然这样每帧的Block数和点数也都是固定的。 + +#### 4.6.2 SplitStrategy + +SplitStrategy定义机械式雷达的分帧模式接口。 ++ 使用者遍历Packet中的Block，以Block的水平角为参数，调用SplitStrategy::newBlock()。应该分帧时，newBlock()返回`true`，否则返回`false`。 + +![split strategy](./img/classes_split_strategy.png) + +#### 4.6.3 SplitStrategyByAngle + +SplitStrategyByAngle按Block角度分帧。 + ++ 成员`split_angle_`保存分割帧的角度。 ++ 成员`prev_angle_`保存前一个Block的角度。 + +##### 4.6.3.1 SplitStrategyByAngle::newBlock() + +当前一个Block的角度`prev_angle_`在`split_angle_`之前，而当前Block的角度在`split_angles_`之后，则认为当前Block跨越了`split_angles_`，返回`true`。 ++ 这里考虑了Block的角度跨越`角度0`的情况。 + +#### 4.6.4 SplitStrategyByNum + +SplitStrategyByNum实现按Block数分帧。 ++ 成员`max_blks_`是每帧的Block数。 ++ 成员`blks_`是当前已累积的Block数。 + +##### 4.6.4.1 SplitStrategyByAngle::newBlock() + +newBlock()简单地累加Block数到成员`blks_`，当`blk_`达到`max_blks_`时，则返回`true`。 + +#### 4.6.5 MEMS雷达的分帧模式 + +MEMS雷达的分帧是在雷达内部确定的。 ++ 一帧的MSOP Packet数是固定的。假设这个数为`N`, 则雷达给Packet编号，从`1`开始，依次编号到`N`。 ++ 对于RSM1，单回波模式下，Packet数是`630`；在双回波模式下，输出的点数要翻倍，相应的，Packet数也要翻倍，Packet数是`1260`。 + +#### 4.6.6 SplitStrategyByPktSeq + +SplitStrategyBySeq按Packet编号分帧。 ++ 注意SplitStrategyBySeq的接口与SplitStrategy不同，不是后者的派生类。 ++ 成员变量`prev_seq_`是前一个Packet的编号。 ++ 成员变量`safe_seq_min_`和`safe_seq_max`，是基于`prev_seq_`的一个安全区间。 + +![split strategy by seq](./img/classes_split_strategy_by_seq.png) + +##### 4.6.6.1 SplitStrategyByPktSeq::newPacket() + +使用者用MSOP Packet的编号值，调用newPacket()。如果分帧，返回`true`。 + +MSOP使用UDP协议，理论上Packet可能丢包、乱序。 + +先讨论没有安全区间时，如何处理丢包、乱序。 ++ 理想情况下，如果不丢包不乱序，Packet编号从`1`到`630`, 只需要检查Packet编号是不是`1`。如果是就分帧。 ++ 那假如只有丢包呢？举个例子，如果编号为`1`的Packet丢了，则可以加入检查条件，就是当前Packet编号小于`prev_seq_`，就分帧。 ++ 在乱序的情况下，这个检查条件会导致另一个困境。举个例子，如果编号为`300`和`301`的两个Packet乱序，那么这个位置分帧，会导致原本的一帧拆分成两帧。 + +为了在一定程度上包容可能的Packet丢包、乱序情况，引入安全区间的概念。 ++ 以`prev_seq_`为参考点，划定一个范围值`RANGE`, + +``` +safe_seq_min_ = prev_seq_ - RANGE +safe_seq_max_ = prev_seq_ + RANGE +``` + +![safe range](./img/safe_range.png) + ++ 如果Packet在范围(`safe_seq_min_`, `safe_seq_max_`)内，都不算异常，丢包、乱序都不触发分帧。这样在大多数情况下，之前的问题可以避免。 + +### 4.7 点云的有效性校验 + +#### 4.7.1 AzimuthSection + +AzimuthSection检查水平角是否在有效范围内。 ++ 成员变量`start_`指定这个范围的起始角度，`end_`指定这个范围的结束角度。支持跨水平角`0`的情况，比如`start` = `35000`, `end` = `10`。 ++ 用户可以通过用户配置参数`RSDecoderParam::start_angle`，和`RSDecoderParam::start_angle`指定这个范围。 + +![azimuth section](./img/class_azimuth_section.png) + +##### 4.7.1.1 AzimuthSection::in() + +in()检查指定的角度`angle`是否在有效范围内。 + +#### 4.7.2 DistanceSection + +DistanceSection检查指定的`distance`是否在有效范围内。 ++ 成员`min_`和`max_`分别是这个范围的最小值和最大值。 ++ 不同雷达有不同的测距范围。雷达配置参数`RSDecoderConstParam::DISTANCE_MIN`，和`RSDecoderConstParam::DISTANCE_MAX`指定这个范围。 ++ 用户也可以通过用户配置参数`RSDecoderParam::min_distance`， 和`RSDecoderParam::max_distance`进一步限缩这个范围。 + +![distance section](./img/class_distance_section.png) + +##### 4.7.2.1 DistanceSection::in() + +in()检查指定的`distance`是否在有效范围内。 + +### 4.8 Decoder + +Decoder解析雷达MSOP/DIFOP Packet，得到点云。 ++ 它是针对所有雷达的接口类，包括机械式雷达和MEMS雷达。 + +DecoderMech派生于Decoder，给机械式雷达完成一些通用的功能，如解析DIFOP Packet。 + +每个机械式雷达分别派生自DecoderMech的类，如DecoderRS16、DecoderRS32、DecoderBP、DecoderRSHELIOS、DecoderRS80、DecoderRS128等。 + +MEMS雷达的类，如DecoderRSM1，直接派生自Decoder。 + +DecoderFactory根据指定的雷达类型，生成Decoder实例。 + +![decoder classes](./img/classes_decoder.png) + +#### 4.8.1 Decoder定义 + +如下图是Decoder的详细定义。 ++ 成员`const_param_`是雷达的参数配置。 ++ 成员`param_`是用户的参数配置。 ++ 成员`trigon_`是Trigon类的实例，提供快速的sin/cos计算。定义如下的宏，可以清晰、方便调用它。 + +``` +#define SIN(angle) this->trigon_.sin(angle) +#define COS(angle) this->trigon_.cos(angle) +``` ++ 成员`packet_duration_`是MSOP Packet理论上的持续时间，也就是相邻两个Packet之间的时间差。Decoder的派生类计算这个值。 + + InputPcap回放PCAP文件时，需要这个值在播放Packet之间设置延时。 ++ 成员`echo_mode_`是回波模式。Decoder的派生类解析DIFOP Packet时，得到这个值。 ++ 成员`temperature_`是雷达温度。Decoder的 派生类解析MSOP Packet时，应该保存这个值。 ++ 成员`angles_ready_`是当前的配置信息是否已经就绪。不管这些信息是来自于DIFOP Packet，还是来自外部文件。 ++ 成员`point_cloud_`是当前累积的点云。 ++ 成员`prev_pkt_ts_`是最后一个MSOP Packet的时间戳，成员`prev_point_ts_`则是最后一个点的时间戳。 ++ 成员`cb_split_frame_`是点云分帧时，要调用的回调函数。由使用者通过成员函数setSplitCallback()设置。 + +![decoder class](./img/class_decoder.png) + +##### 4.8.1.1 RSDecoderConstParam + +RSDecoderConstParam是雷达配置参数，这些参数都是特定于雷达的常量 ++ `MSOP_LEN`是MSOP Packet大小 ++ `DIFOP_LEN`是DIFOP Packet大小 ++ `MSOP_ID[]`是MSOP Packet的标志字节。各雷达的标志字节不同，用`MSOP_ID_LEN`指定其长度。 ++ `DIFOP_ID[]`是DIFOP Packet的标志字节。各雷达的标志字节不同，用`DIFOP_ID_LEN`指定其长度。 ++ `BLOCK_ID[]`是MSOP Packet中Block的标志字节。所有雷达都是两个字节。 ++ `LASER_NUM`是雷达的通道数。如RS16是16， RS32是32，RS128是128。 ++ `BLOCKS_PER_PKT`、`CHANNELS_PER_BLOCK`分别指定每个MSOP Packet中有几个Block，和每个Block中有几个Channel。 ++ `DISTANCE_MIN`、`DISTANCE_MAX`指定雷达测距范围 ++ `DISTANCE_RES`指定MSOP格式中`distance`的解析度。 ++ `TEMPERATURE_RES`指定MSOP格式中，雷达温度值的解析度。 ++ `IMU_LEN`IMU Packet大小，当雷达不支持单独发送IMU数据时，不用设置这个值。 ++ `IMU_ID_LEN`是IMU Packet大小，当雷达不支持单独发送IMU数据时，不用设置这个值。 ++ `IMU_ID[]`是IMUPacket的标志字节, 用`IMU_ID_LEN`指定其长度，当雷达不支持单独发送IMU数据时，不用设置这个值。 +``` +struct RSDecoderConstParam +{ + // packet len + uint16_t MSOP_LEN; + uint16_t DIFOP_LEN; + + // packet identity + uint8_t MSOP_ID_LEN; + uint8_t DIFOP_ID_LEN; + uint8_t MSOP_ID[8]; + uint8_t DIFOP_ID[8]; + uint8_t BLOCK_ID[2]; + + // packet structure + uint16_t LASER_NUM; + uint16_t BLOCKS_PER_PKT; + uint16_t CHANNELS_PER_BLOCK; + + // distance & temperature + float DISTANCE_MIN; + float DISTANCE_MAX; + float DISTANCE_RES; + float TEMPERATURE_RES; + + // IMU parameters (new fields with default values) + uint16_t IMU_LEN{0}; // Default to 0 for lidar without IMU + uint16_t IMU_ID_LEN{0}; // Default to 0 for lidar without IMU + uint8_t IMU_ID[4]{0}; // Default to {0, 0, 0, 0} for lidar without IMU +}; +``` + +##### 4.8.1.2 Decoder::processDifopPkt() + +processDifopPkt()处理DIFOP Packet。 ++ 校验Packet的长度是否匹配。 ++ 校验Packet的标志字节是否匹配。 ++ 如果校验无误，调用decodeDifopPkt()。这是一个纯虚拟函数，由各雷达的派生类提供自己的实现。 + +##### 4.8.1.3 Decoder::processMsopPkt() + +processMsopPkt()处理MSOP Packet。 ++ 检查当前配置信息是否已经就绪(`angles_ready_`)。 + + 对于机械式雷达，当`angles_readys` = `false`时，驱动还没有获得垂直角信息，这时得到的点云是扁平的。所以用户一般希望等待`angles_ready` = `true` 才输出点云。 + + 通过用户配置参数`RSDecoderParam::wait_for_difop`，可以设置是否等待配置信息就绪。 ++ 校验DIFOP Packet的长度是否匹配。 ++ 校验DIFOP Packet的标志字节是否匹配。 ++ 如果以上校验通过，调用decodeMsopPkt()。这是一个纯虚拟函数，由各雷达的派生类提供自己的实现。 + +##### 4.8.1.4 Decoder::processImuPkt() + +processImuPkt()处理IMU Packet。 ++ 校验Packet的长度是否匹配。 ++ 校验Packet的标志字节是否匹配。 ++ 如果校验无误，调用decodeImuPkt()。这是一个纯虚拟函数，由各雷达的派生类提供自己的实现。 + + +##### 4.8.1.5 Decoder::transformPoint() + +transformPoint() 对点做坐标变换。它基于第三方开源库Eigen。 + +默认情况下，transformPoint()功能不开启。要启用这个特性，编译时使用`-DENABLE_TRANSFORM`选项。 + +``` +cmake -DENABLE_TRANSFORM . +``` + +#### 4.8.2 DecoderMech + +DecoderMech处理机械式雷达的共同特性，如转速，分帧角度、光心补偿等。 ++ 成员`chan_angles_`是ChanAngles类实例，保存角度修正信息。 ++ 成员`scan_section_`是AzimuthSection类实例，保存角度校验信息。 ++ 成员`split_strategy_`是SplitStrategy类实例，保存分帧策略。 ++ 成员`rps_`是雷达转速，单位是转/秒(round/second)。 ++ 成员`blks_per_frame_`是单回波模式下，理论上的每帧Block数。 ++ 成员`split_blks_per_frame_`是按Block数分帧时，每帧的Block数。包括按理论上每圈Block数分帧，和按用户指定的Block数分帧。 ++ 成员`block_azi_diff_`是理论上相邻block之间的角度差。 ++ 成员`fov_blind_ts_diff_`是FOV盲区的时间差 + +![decoder mech](./img/class_decoder_mech.png) + +##### 4.8.2.1 RSDecoderMechConstParam + +RSDecoderMechConstParam基于RSDecoderConstParam，增加机械式雷达特有的参数。 ++ `RX`、`RY`、`RZ`是雷达光学中心相对于物理中心的坐标。 ++ `BLOCK_DURATION`是Block的持续时间。 ++ `CHAN_TSS[]`是Block中Channel对Block的相对时间。 ++ `CHAN_AZIS[]`是Block中Channel占Block的时间比例，也是水平角比例。 + +``` +struct RSDecoderMechConstParam +{ + RSDecoderConstParam base; + + // lens center + float RX; + float RY; + float RZ; + + // firing_ts/chan_ts + double BLOCK_DURATION; + double CHAN_TSS[128]; + float CHAN_AZIS[128]; +}; +``` ++ Decoder初始化时，从每轮激光发射中的每次发射时间表，计算`CHAN_TSS[]`、`CHAN_AZIS[]`。这可以简化每个Channel的时间戳和角度的计算。 +前面的"Channel的时间戳"章节，已经列出过RSBP的发射时间表，如下。 + +``` + 0.00, 2.56, 5.12, 7.68, 10.24, 12.80, 15.36, 17.92, + 25.68, 28.24, 30.80, 33.36, 35.92, 38.48, 41.04, 43.60, + 1.28, 3.84, 6.40, 8.96, 11.52, 14.08, 16.64, 19.20, + 26.96, 29.52, 32.08, 34.64, 37.20, 39.76, 42.32, 44.88 +``` + +##### 4.8.2.2 DecoderMech::decodeDifopCommon() + +decodeDifopCommon()解析DIFOP Packet。 ++ 解析Packet中的`rpm`，得到`rps_`. + +``` + uint16_t rpm; +``` + ++ 计算单回波模式下，每帧Block数，也就是`blks_per_frame_`。 + +``` +每帧Block数 = (1/rps) / Block的持续时间 +``` + ++ 计算相邻Block之间的角度差，也就是`block_azi_diff_`。 + +``` +Block间的角度差 = 360 / 每帧Block数 +``` + ++ 解析得到FOV的起始角度`fov_start_angle`和终止角度`fov_end_angle`，计算FOV的大小`fov_range`。 ++ 计算与FOV互补的盲区大小。按照盲区范围比例，计算盲区的时间戳差，也就是`fov_blind_ts_diff_`。 + ++ 如果用户设置从DIFOP Packet读入角度修正值(`RSDecoderParam.config_from_file` = `false`)，则调用ChanAngles::loadFromDifop()得到他们。 + + 一般角度修正值不改变，所以一旦解析成功（`angles_ready_ = true`），就没必要解析第二次。 + +#### 4.8.3 DecoderRSBP + +以RSBP举例说明机械式雷达的Decoder。 ++ RSBP的常量配置由成员函数getConstParam()生成。这个配置定义为静态本地变量。 + +![decoder rsbp](./img/class_decoder_rsbp.png) + +##### 4.8.3.1 RSDecoderConstParam设置 + +| 常量参数 | 值 | 说明 | +|:-------------|:---------|:-------------:| +| MSOP_LEN | 1248 | MSOP Packet字节数 | +| DIFOP_LEN | 1248 | DIFOP Packet字节数 | +| MSOP_ID[] | {0x55, 0xAA, 0x05, 0x0A, 0x5A, 0xA5, 0x50, 0xA0} | MSOP Packet标志字节，长度为8 | +| MSOP_ID_LEN | 8 | MSOP_LEN[]中标志字节的长度 | +| DIFOP_ID[] | {0xA5, 0xFF, 0x00, 0x5A, 0x11, 0x11, 0x55, 0x55} | DIFOP Packet标志字节，长度为8 | +| DIFOP_ID_LEN | 8 | DIFOP_LEN[]中的字节长度 | +| BLOCK_ID[] | {0xFF, 0xEE} | block标志字节，长度为2 | +| LASER_NUM | 32 | 32通道 | +| BLOCKS_PER_PKT | 12 | 每Packet中12个Block | +| CHANNEL_PER_BLOCK | 32 | RSBP为32线雷达 | +| DISTANCE_MIN | 0.1 | 测距最小值，单位米 | +| DISTANCE_MAX | 100.0 | 测距最大值，单位米 | +| DISTANCE_RES | 0.005 | Packet中distance的解析度，单位米 | +| TEMPERATURE_RES| 0.0625 | Packet中的温度的解析度 | + +##### 4.8.3.2 RSDecoderMechConstParam设置 + +| 常量参数 | 值 | 说明 | +|:---------------|:-----------|:-------------:| +| RX | 0.01473 | 光心相对于物理中心的X坐标 | +| RY | 0.0085 | 光心相对于物理中心的Y坐标 | +| RZ | 0.09427 | 光心相对于物理中心的Z坐标 | +| BLOCK_DURATION | 55.52 | Block的持续时间，单位微秒 | +| CHAN_TSS[] | - | 从发射时间列表得到 | +| CHAN_AZIS[] | - | 从发射时间列表得到 | + +##### 4.8.3.2 DecoderRSBP::getEchoMode() + +getEchoMode()解析回波模式。 + +##### 4.8.3.3 DecoderRSBP::decodeDifopPkt() + +decodeDifopPkt() 解析DIFOP Packet。 ++ 调用DecoderMech::decodeDifopCommon()解析DIFOP Packet，得到转速等信息。 ++ 调用getEchoMode()，解析`RSDifopPkt::return_mode`，得到回波模式 ++ 根据回波模式，设置成员成员`split_blks_per_frame_`。如前所说，如果当前以理论上的每圈Block数分帧，则需要使用这个成员。 + +##### 4.8.3.4 DecoderRSBP::decodeMsopPkt() + +decodeMsopPkt()使用不同的模板参数调用internDecodeMsopPkt()。 ++ 单回波模式下，模板参数是SingleReturnBlockDiff， ++ 双回波模式下，模板参数是DualReturnBlockDiff。 + +##### 4.8.3.5 DecoderRSBP::internDecodeMsopPkt() + ++ 调用parseTempInLe(), 得到雷达温度，保存到`temperature_`。 ++ 调用parseTimeYMD()得到Packet的时间戳，保存到本地变量`pkt_ts`。Block时间戳的初值设置为`pkt_ts`。 ++ 构造模板参数BlockDiff的实例。 ++ 遍历Packet内所有的Block。 + + 校验Block的标志字节 + + 得到Block的角度值。 + + 计算得到Block的时间戳，保存到本地变量`block_ts`。 + + 调用SplitStrategy::newBlock()，检查是否分帧。如果是，调用回调函数`cb_split_frame_`，通知使用者。 + `cb_split_frame_`应该转移点云`point_cloud_`，并重置它。 + ++ 遍历Block内所有的Channel。 + + 计算Channel的时间戳 + + 计算Channel的水平角 + + 调用ChanAngles::vertAdjust()，得到Channel的垂直角。 + + 调用ChanAngles::horizAdjust()，对Channel的水平角进行修正。 + + 解析Channel的`distance`。 + + 调用DistanceSection::in()校验distance，调用AzimuthSection::in()校验水平角。 + +如果合法， + + 计算点云坐标(`x`, `y`, `z`)。 + + 调用transfromPoint()做坐标转换。 + + 设置点云的`intensity`、`timestamp`，`ring`。 + + 将点保存到点云`point_cloud_`的尾部。 + +如果不合法， + + 将`NAN`点保存到点云`point_cloud_`尾部。 + ++ 当前点的时间戳保存到成员`prev_point_ts`。如果下一个Block分包，那么这个时间戳就是点云的时间戳。 ++ 将当前Packet的时间戳保存到成员`prev_pkt_ts`。这样，Decoder的使用者不需要重新解析Packet来得到它。 + +#### 4.8.4 DecoderRS16 + +RS16的处理与其他机械式雷达有差异，请先参考前面的“RS16的Packet格式”等章节。 + +##### 4.8.4.1 DecoderRS16::internDecodeMsopPkt() + +在internDecoderPkt()的处理中， ++ 因为Block的通道值在[0，31]，需要将它映射到实际的通道值。 + +#### 4.8.5 DecoderRSM1 + +RSM1是MEMS雷达。这里说明RSM1的Decoder。 ++ DecoderRSM1的常量配置由成员函数getConstParam()生成。这个配置定义为静态本地变量。 ++ 成员`split_strategy_`是SplitStrategyBy类的实例，保存分帧策略。 + +![decoder rsm1](./img/class_decoder_rsm1.png) + +##### 4.8.5.1 RSDecoderConstParam设置 + +| 常量参数 | 值 | 说明 | +|:-------------:|:---------:|:-------------| +| MSOP_LEN | 1210 | MSOP Packet字节数 | +| DIFOP_LEN | 256 | DIFOP Packet字节数 | +| MSOP_ID[] | {0x55, 0xAA, 0x5A, 0xA5} | MSOP Packet标志字节，长度为4 | +| MSOP_ID_LEN | 4 | MSOP_LEN[]中标志字节的长度 | +| DIFOP_ID[] | {0xA5, 0xFF, 0x00, 0x5A, 0x11, 0x11, 0x55, 0x55} | DIFOP Packet标志字节，长度为8 | +| DIFOP_ID_LEN | 8 | DIFOP_LEN[]中的字节长度 | +| BLOCK_ID[] | {0xFF, 0xEE} | block标志字节，长度为2 | +| LASER_NUM | 5 | 5通道 | +| BLOCKS_PER_PKT | 25 | 每Packet中25个Block | +| CHANNEL_PER_BLOCK | 5 | RSM1有5个通道 | +| DISTANCE_MIN | 0.2 | 测距最小值，单位米 | +| DISTANCE_MAX | 200.0 | 测距最大值，单位米 | +| DISTANCE_RES | 0.005 | Packet中distance的解析度，单位米 | +| TEMPERATURE_RES| 80 | 雷达温度的初始值 | + +##### 4.8.5.2 DecoderRSM1::decodeDifopPkt() + +decodeDifopPkt() 解析DIFOP Packet。 ++ 调用getEchoMode()，解析`RSDifopPkt::return_mode`，得到回波模式 ++ 根据回波模式，设置成员成员`max_seq_`。 + +##### 4.8.5.3 DecodeRSM1::decodeMsopPkt() + +decodeMsopPkt()解析MSOP Packet。 ++ 解析Packet中的`temperature`字段，得到雷达温度，保存到`temperature_`。 + ++ 调用parseTimeUTCWithUs()得到Packet的时间戳，保存到本地变量`pkt_ts`。 + ++ 调用SplitStrategyBySeq::newPacket()，检查是否分帧。如果是，调用回调函数`cb_split_frame_`，通知使用者。 + `cb_split_frame_`应该转移点云`pont_cloud_`，并重置它。 + ++ 遍历Packet内所有的Block。 + + + 从Block相对于Packet的偏移，得到Block的时间戳。对于RSM1, Block内的所有Channel的时间戳都是这个时间戳。 + ++ 遍历Block内所有的Channel。 + + 解析Channel的distance。 + + 调用DistanceSection::in()校验`distance`。 + + 如果distance合法， + + 计算点云坐标(`x`, `y`, `z`)。 + + 调用transfromPoint()做坐标转换。 + + 设置点云的`intensity`、`timestamp`，`ring`。 + + 将点保存到点云point_cloud_的尾部。 + + 如果`distance`不合法， + + 将`NAN`点保存到点云`point_cloud_`尾部。 + ++ 当前点的时间戳保存到成员`prev_point_ts_`。如果下一个Block分包，那么这个时间戳就是点云的时间戳。 + ++ 将当前Packet的时间戳保存到成员`prev_pkt_ts_`。这样，Decoder的使用者不需要重新解析Packet来得到它。 + +#### 4.8.6 DecoderFactory + +DecoderFactory是创建Decoder实例的工厂。 + +![decoder factory](./img/class_decoder_factory.png) + +Decoder/雷达的类型如下。 + +``` +enum LidarType +{ + // mechanical + RS_MECH = 0x01, + RS16 = RS_MECH, + RS32, + RSBP, + RSAIRY, + RSHELIOS, + RSHELIOS_16P, + RS128, + RS80, + RS48, + RSP128, + RSP80, + RSP48, + + // mems + RS_MEMS = 0x20, + RSM1 = RS_MEMS, + RSM2, + RSM3, + RSE1, + RSMX, + + // jumbo + RS_JUMBO = 0x100, + RSM1_JUMBO = RS_JUMBO + RSM1, +}; +``` + +##### 4.8.6.1 DecoderFactory::creatDecoder() + +createDecoder() 根据指定的雷达类型，创建Decdoer实例。 + +### 4.9 LidarDriverImpl - 组合Input与Decoder + +LidarDriverImpl组合Input部分和Decoder部分。 ++ 成员`input_ptr_`是Input实例。成员`decoder_ptr_`是Decoder实例。 + + LidarDriverImpl只有一个Input实例和一个Decoder实例，所以一个LidarDriverImpl实例只支持一个雷达。如果需要支持多个雷达，就需要分别创建多个LidarDriverImpl实例。 ++ 成员`handle_thread_`是MSOP/DIFOP/IMU Packet的处理线程。 ++ 成员`driver_param_`是RSDriverParam的实例。 + + RSDriverParam打包了RSInputParam和RSDecoderParam，它们分别是Input部分和Decoder部分的参数。 + +``` +typedef struct RSDriverParam +{ + LidarType lidar_type = LidarType::RS16; ///< Lidar type + InputType input_type = InputType::ONLINE_LIDAR; ///< Input type + RSInputParam input_param; + RSDecoderParam decoder_param; +} RSDriverParam; +``` + +![lidar driver impl](./img/class_lidar_driver_impl.png) + +组合Input, ++ 成员`free_pkt_queue_`、`pkt_queue_`分别保存空闲的Packet， 待处理的MSOP/DIFOP/IMU Packet。 + + 这2个队列是SyncQueue类的实例。SyncQueue提供多线程访问的互斥保护。 ++ 函数packetGet()和packetPut()用来向input_ptr_注册。`input_ptr_`调用前者得到空闲的Buffer，调用后者派发填充好Packet的Buffer。 + +组合Decoder, ++ 成员`cb_get_cloud_`和`cb_put_cloud_`是回调函数，由驱动的使用者提供。它们的作用类似于Input类的`cb_get_pkt_`和`cb_put_pkt_`。驱动调用`cb_get_cloud_`得到空闲的点云，调用`cb_put_cloud_`派发填充好的点云。 + + 驱动的使用者调用成员函数regPointCloudCallback()，设置`cb_get_cloud_`和`cb_put_cloud_`。 ++ 成员`cb_get_imu_data_` /`cb_put_imu_data_`作用和`cb_get_cloud_`/`cb_put_cloud_`类似, 用来获取和派发imu数据。 ++ 成员函数splitFrame()用来向`decoder_ptr_`注册。`decoder_ptr_`在需要分帧时，调用split_Frame()。这样LidarDriverImpl可以调用`cb_put_cloud_`将点云传给使用者，同时调用`cb_get_cloud_`得到空闲的点云，用于下一帧的累积。 + +#### 4.9.1 LidarDriverImpl::getPointCloud() + +LidarriverImpl的成员`cb_get_cloud_`是rs_driver的使用者提供的。getPointCloud（）对它加了一层包装，以便较验它是否合乎要求。 +在循环中， + ++ 调用`cb_get_cloud_`，得到点云， +如果点云有效， ++ 将点云大小设置为`0`。 +如果点云无效， ++ 调用runExceptionCallback()报告错误。 + +#### 4.9.2 LidarDriverImpl::init() + +init()初始化LidarDriverImpl实例。 + +初始化Decoder部分， ++ 调用DecoderFactory::createDecoder()，创建Decoder实例。 ++ 调用getPointCloud()得到空闲的点云，设置`decoder_ptr_`的成员`point_cloud_`。 ++ 调用Decoder::regCallback()， 传递成员函数splitFrame()作为参数。这样Decoder分帧时，会调用splitFrame()通知。 ++ 调用Decoder::getPacketDuration()得到Decoder的Packet持续时间。 + +初始化Input部分， ++ 调用InputFactory::createInput()，创建Input实例。 ++ 调用Input::regCallback()，传递成员函数packetGet()和packetPut()。这样Input可以得到Buffer， 和派发填充好Packet的Buffer。 ++ 调用Input::init()，初始化Input实例。 + +#### 4.9.3 LidarDriverImpl::start() + +start()开始处理MSOP/DIFOP Packet。 ++ 启动Packet处理线程`handle_thread_`， 线程函数为processPacket()。 ++ 调用Input::start()， 其中启动接收线程，接收MSOP/DIFOP Packet。 + +#### 4.9.4 LidarDriverImpl::packetGet() + +packetGet()分配空闲的Buffer。 ++ 优先从`free_pkt_queue_`队列得到可用的Buffer。 ++ 如果得不到，重新分配一个Buffer。 + +#### 4.9.5 LidarDriverImpl::packetPut() + +packetPut()将收到的Packet，放入队列`pkt_queue_`。 ++ 检查`msop_pkt_queue_`/`difop_pkt_queue`中的Packet数。如果处理线程太忙，不能及时处理， 则释放队列中所有Buffer。 + +#### 4.9.6 LidarDriverImpl::processPacket() + +processMsop()是MSOP Packet处理线程的函数。在循环中, ++ 调用SyncQueue::popWait()获得Packet， ++ 检查Packet的标志字节。 + + 如果是MSOP Packet，调用Decoder::processMsopPkt()，处理MSOP Packet。如果Packet触发了分帧，则Decoder会调用回调函数，也就是DriverImpl::splitFrame()。 + + 如果是DIFOP Packet， 调用Decoder::processDifopPkt()，处理Difop Packet。 ++ 将Packet的Buffer回收到`free_pkt_queue_`，等待下次使用。 + +#### 4.9.7 LidarDriverImpl::splitFrame() + +splitFrame()在Decoder通知分帧时，派发点云。 ++ 得到点云，也就是成员`decoder_ptr`的`point_cloud_`。 ++ 校验`point_cloud_`， +如果点云有效， ++ 调用setPointCloudHeader()设置点云的头部信息， ++ 调用`cb_put_pc_`，将点云传给驱动的使用者。 ++ 调用getPointCloud()得到空闲点云，重新设置成员`decoder_ptr`的`point_cloud_`。 + +#### 4.9.8 LidarDriverImpl::getTemperature() + +getTemperature()调用Decoder::getTemperature()， 得到雷达温度。 + + + +## 5 Packet的录制与回放 + +使用者调试自己的程序时，点云的录制与回放是有用的，只是点云占的空间比较大。MSOP/DIFOP Packet占的空间较小，所以Packet的录制与回放是更好的选择。 + +与MSOP/DIFP Packet的录制与回放相关的逻辑，散布在rs_driver的各个模块中，所以这里单独分一个章节说明。 + +![packet record and replay](./img/cla.png) + +### 5.1 录制 + +#### 5.1.1 LidarDriverImpl::regPacketCallback() + +通过regPacketCallback()，rs_driver的使用者注册一个回调函数，得到原始的MSOP/DIFOP Packet。 ++ 回调函数保存在成员`cb_put_pkt_`。 + +#### 5.1.2 LidarDriverImpl::processMsopPkt() + +在processMsopPkt()中， ++ 调用Decoder::processMsopPkt()， ++ 调用`cb_put_pkt_`，将MSOP Packet传给调用者。 + + 设置Packet的时间戳。这个时间戳调用Decoder::prevPktTs()得到。 + + 设置这个Packet是否触发分帧。这个标志是Decoder::processMsopPkt()的返回值。 + +#### 5.1.3 LidarDriverImpl::processDifopPkt() + +在processDifopPkt()中， ++ 调用Decoder::processDifopPkt()， ++ 调用`cb_put_pkt_`，将MSOP Packet传给调用者。DIFOP Packet的时间戳不重要。 + +### 5.2 回放 + +#### 5.2.1 InputRaw + +![InputRaw](./img/class_input_raw.png) + +InputRaw回放MOSP/DIFOP Packet。 ++ 使用者从某种数据源（比如rosbag文件）中解析MSOP/DIFOP Packet，调用InputRaw的成员函数feedPacket()，将Packet喂给它。 + + 在feedPacket()中，InputRaw简单地调用成员`cb_put_pkt_`，将Packet推送给调用者。这样，它的后端处理就与InputSock/InputPcap一样了。 + +#### 5.2.2 LidarDriverImpl + ++ InputRaw::feedBack()在InputFactory::createInput()中被打包，最后保存在LidarDriverImpl类的成员`cb_feed_pkt_`中。 ++ 使用者调用LidarDriverImpl的成员函数decodePacket()，可以将Packet喂给它。decodePacket()简单地调用成员`cb_feed_pkt_`。 + +### 5.3 时间戳处理 + +点云的时间戳来自于MSOP Packet的时间戳。MSOP Packet的时间戳可以有两种产生方式。 ++ 用户配置参数`RSDecoderParam::use_lidar_clock`决定使用哪种方式。 ++ `use_lidar_clock` = `true`时， 使用雷达产生的时间戳，这个时间戳在Packet中保存。这种情况下，一般已经使用时间同步协议对雷达做时间同步。 ++ `use_lidar_clock` = `false`时， 忽略Packet中的时间戳，在电脑主机侧由rs_driver重新产生一个。 + +#### 5.3.1 使用雷达时间戳 + +录制时，设置`use_lidar_clock` = `true` ++ 解析MSOP Packet的时间戳。这个时间戳是雷达产生的。 ++ 输出的点云使用这个时间戳 ++ 如果输出Packet，也是这个时间戳 + +回放时，设置`use_lidar_clock` = `true` ++ MSOP Packet内保存的仍然是雷达产生的时间戳。 ++ 输出点云仍然使用这个时间戳。 + +#### 5.3.2 使用主机时间戳 + +录制时，设置`use_lidar_clock` = `false` ++ rs_driver在电脑主机侧重新产生时间戳。这个时间戳覆盖Packet文件中原有的时间戳；如果这时有点云输出，使用rs_driver产生的时间戳 + + 在DecoderRSBP::internDecodeMsopPacket()中，rs_driver调用getTimeHost()产生时间戳，然后调用createTimeYMD()，用这个新时间戳替换Packet中原有的时间戳。 ++ 这时输出的Packet的时间戳是rs_driver产生的时间戳 + +回放时，设置`use_lidar_clock` = `true` ++ 解析MSOP Packet的时间戳。这个时间戳是录制时rs_driver在电脑主机侧产生的。 ++ 输出的点云使用这个时间戳。 + + + + + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/api/lidar_driver.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/api/lidar_driver.hpp new file mode 100644 index 0000000..47cfe5c --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/api/lidar_driver.hpp @@ -0,0 +1,173 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once + +#include +#include + +namespace robosense +{ +namespace lidar +{ + +std::string getDriverVersion(); + +/** + * @brief This is the RoboSense LiDAR driver interface class + */ +template +class LidarDriver +{ +public: + + /** + * @brief Constructor, instanciate the driver pointer + */ + LidarDriver() + : driver_ptr_(std::make_shared>()) + { + } + + /** + * @brief Register the lidar point cloud callback function to driver. When point cloud is ready, this function will be + * called + * @param callback The callback function + */ + inline void regPointCloudCallback(const std::function(void)>& cb_get_cloud, + const std::function)>& cb_put_cloud) + { + driver_ptr_->regPointCloudCallback(cb_get_cloud, cb_put_cloud); + } + /** + * @brief Register the imu data callback function to driver. When imu data is ready, this function will be + * called + * @param callback The callback function + */ + inline void regImuDataCallback(const std::function(void)>& cb_get_imu_data, const std::function &)>& cb_put_imu_data) + { + driver_ptr_->regImuDataCallback(cb_get_imu_data, cb_put_imu_data); + } + + + /** + * @brief Register the lidar difop packet message callback function to driver. When lidar difop packet message is + * ready, this function will be called + * @param callback The callback function + */ + inline void regPacketCallback(const std::function& cb_put_pkt) + { + driver_ptr_->regPacketCallback(cb_put_pkt); + } + + /** + * @brief Register the exception message callback function to driver. When error occurs, this function will be called + * @param callback The callback function + */ + inline void regExceptionCallback(const std::function& cb_excep) + { + driver_ptr_->regExceptionCallback(cb_excep); + } + + /** + * @brief The initialization function, used to set up parameters and instance objects, + * used when get packets from online lidar or pcap + * @param param The custom struct RSDriverParam + * @return If successful, return true; else return false + */ + inline bool init(const RSDriverParam& param) + { + return driver_ptr_->init(param); + } + + /** + * @brief Start the thread to receive and decode packets + * @return If successful, return true; else return false + */ + inline bool start() + { + return driver_ptr_->start(); + } + + /** + * @brief Decode lidar msop/difop messages + * @param pkt_msg The lidar msop/difop packet + */ + inline void decodePacket(const Packet& pkt) + { + driver_ptr_->decodePacket(pkt); + } + + /** + * @brief Get the current lidar temperature + * @param temp The variable to store lidar temperature + * @return if get temperature successfully, return true; else return false + */ + inline bool getTemperature(float& temp) + { + return driver_ptr_->getTemperature(temp); + } + + /** + * @brief Get device info + * @param info The variable to store device info + * @return if get device info successfully, return true; else return false + */ + inline bool getDeviceInfo(DeviceInfo& info) + { + return driver_ptr_->getDeviceInfo(info); + } + + /** + * @brief Get device status + * @param info The variable to store device status + * @return if get device info successfully, return true; else return false + */ + inline bool getDeviceStatus(DeviceStatus& status) + { + return driver_ptr_->getDeviceStatus(status); + } + + /** + * @brief Stop all threads + */ + inline void stop() + { + driver_ptr_->stop(); + } + +private: + std::shared_ptr> driver_ptr_; ///< The driver pointer +}; + +} // namespace lidar +} // namespace robosense diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/common/error_code.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/common/error_code.hpp new file mode 100644 index 0000000..c4c97ec --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/common/error_code.hpp @@ -0,0 +1,186 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once + +#include +#include + +namespace robosense +{ +namespace lidar +{ + +enum class ErrCodeType +{ + INFO_CODE, // 0x00 ~ 0x3F + WARNING_CODE, // 0x40 ~ 0x7F + ERROR_CODE // 0x80 ~ 0xBF +}; + +enum ErrCode +{ + // info + ERRCODE_SUCCESS = 0x00, ///< Normal Status + ERRCODE_PCAPREPEAT = 0x01, ///< Reach file end, and play PCAP file again. + ERRCODE_PCAPEXIT = 0x02, ///< Reach file end, and exit parsing PCAP file + + // warning + ERRCODE_MSOPTIMEOUT = 0x40, ///< Timeout (1s) of receiving MSOP Packets + ERRCODE_NODIFOPRECV = 0x41, ///< Calibration data (in DIFOP packet in general) is not ready while handling MOSP Packet + ERRCODE_WRONGMSOPLEN = 0x42, ///< MSOP Packet length is wrong + ERRCODE_WRONGMSOPID = 0x43, ///< MSOP Packet ID is wrong + ERRCODE_WRONGMSOPBLKID = 0x44, ///< Block ID in MSOP Packet is wrong + ERRCODE_WRONGDIFOPLEN = 0x45, ///< DIFOP Packet length is wrong + ERRCODE_WRONGDIFOPID = 0x46, ///< DIFOP Packet ID is wrong + ERRCODE_ZEROPOINTS = 0x47, ///< No points in PointCloud + ERRCODE_PKTBUFOVERFLOW = 0x48, ///< Packet queue is overflow + ERRCODE_CLOUDOVERFLOW = 0x49, ///< Point cloud buffer is overflow + ERRCODE_WRONGCRC32 = 0x4A, ///< Wrong CRC32 value of MSOP Packet + ERRCODE_WRONGIMULEN = 0x4B, ///< IMU Packet length is wrong + ERRCODE_WRONGIMUID = 0x4C, ///< IMU Packet ID is wrong + ERRCODE_WRONGPCAPPARSE = 0x4D, ///< Parse msop data frome pcap file failed + + // error + ERRCODE_STARTBEFOREINIT = 0x80, ///< User calls start() before init() + ERRCODE_PCAPWRONGPATH = 0x81, ///< Path of pcap file is wrong + ERRCODE_POINTCLOUDNULL = 0x82, ///< User provided PointCloud buffer is invalid + ERRCODE_IMUDATANULL = 0x83, ///< User provided ImuData buffer is invalid +}; + +struct Error +{ + ErrCode error_code; + ErrCodeType error_code_type; + + Error () + : error_code(ErrCode::ERRCODE_SUCCESS) + { + } + + explicit Error(const ErrCode& code) + : error_code(code), error_code_type(ErrCodeType::INFO_CODE) + { + if (error_code < 0x40) + { + error_code_type = ErrCodeType::INFO_CODE; + } + else if (error_code < 0x80) + { + error_code_type = ErrCodeType::WARNING_CODE; + } + else + { + error_code_type = ErrCodeType::ERROR_CODE; + } + } + std::string toString() const + { + switch (error_code) + { + // info + case ERRCODE_PCAPREPEAT: + return "Info_PcapRepeat"; + case ERRCODE_PCAPEXIT: + return "Info_PcapExit"; + + // warning + case ERRCODE_MSOPTIMEOUT: + return "ERRCODE_MSOPTIMEOUT"; + case ERRCODE_NODIFOPRECV: + return "ERRCODE_NODIFOPRECV"; + case ERRCODE_WRONGMSOPID: + return "ERRCODE_WRONGMSOPID"; + case ERRCODE_WRONGMSOPLEN: + return "ERRCODE_WRONGMSOPLEN"; + case ERRCODE_WRONGMSOPBLKID: + return "ERRCODE_WRONGMSOPBLKID"; + case ERRCODE_WRONGDIFOPID: + return "ERRCODE_WRONGDIFOPID"; + case ERRCODE_WRONGDIFOPLEN: + return "ERRCODE_WRONGDIFOPLEN"; + case ERRCODE_ZEROPOINTS: + return "ERRCODE_ZEROPOINTS"; + case ERRCODE_PKTBUFOVERFLOW: + return "ERRCODE_PKTBUFOVERFLOW"; + case ERRCODE_CLOUDOVERFLOW: + return "ERRCODE_CLOUDOVERFLOW"; + case ERRCODE_WRONGCRC32: + return "ERRCODE_WRONGCRC32"; + case ERRCODE_WRONGIMULEN: + return "ERRCODE_WRONGIMULEN"; + case ERRCODE_WRONGIMUID: + return "ERRCODE_WRONGIMUID"; + case ERRCODE_WRONGPCAPPARSE: + return "ERRCODE_WRONGPCAPPARSE"; + + // error + case ERRCODE_STARTBEFOREINIT: + return "ERRCODE_STARTBEFOREINIT"; + case ERRCODE_PCAPWRONGPATH: + return "ERRCODE_PCAPWRONGPATH"; + case ERRCODE_POINTCLOUDNULL: + return "ERRCODE_POINTCLOUDNULL"; + case ERRCODE_IMUDATANULL: + return "ERRCODE_IMUDATANULL"; + + //default + default: + return "ERRCODE_SUCCESS"; + } + } +}; + +#define LIMIT_CALL(func, sec) \ +{ \ + static time_t prev_tm = 0; \ + time_t cur_tm = time(NULL); \ + if ((cur_tm - prev_tm) > sec) \ + { \ + func; \ + prev_tm = cur_tm; \ + } \ +} + +#define DELAY_LIMIT_CALL(func, sec) \ +{ \ + static time_t prev_tm = time(NULL); \ + time_t cur_tm = time(NULL); \ + if ((cur_tm - prev_tm) > sec) \ + { \ + func; \ + prev_tm = cur_tm; \ + } \ +} + +} // namespace lidar +} // namespace robosense diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/common/rs_common.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/common/rs_common.hpp new file mode 100644 index 0000000..8fb2bef --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/common/rs_common.hpp @@ -0,0 +1,92 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once +#include +#include +// +// define ntohs() +// +#ifdef _WIN32 +#include +#else //__linux__ +#include +#endif + +// +// define M_PI +// +#ifndef _USE_MATH_DEFINES +#define _USE_MATH_DEFINES // for VC++, required to use const M_IP in +#endif + +#include + +#define DEGREE_TO_RADIAN(deg) ((deg) * M_PI / 180) +#define RADIAN_TO_DEGREE(deg) ((deg) * 180 / M_PI) + +namespace robosense +{ +namespace lidar +{ +inline int16_t RS_SWAP_INT16(int16_t value) +{ + uint8_t* v = (uint8_t*)&value; + + uint8_t temp; + temp = v[0]; + v[0] = v[1]; + v[1] = temp; + + return value; +} + +inline int32_t u8ArrayToInt32(const uint8_t* data, uint8_t len) { + if(len != 4) + { + printf("u8ArrayToInt32: len is not 4\n"); + return 0; + } + uint32_t uintValue = ntohl(*reinterpret_cast(data)); + return static_cast(uintValue); +} + +inline float convertUint32ToFloat(uint32_t byteArray) { + float floatValue; + std::memcpy(&floatValue, &byteArray, sizeof(float)); + return floatValue; +} + +} +} + + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/common/rs_log.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/common/rs_log.hpp new file mode 100644 index 0000000..94087e5 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/common/rs_log.hpp @@ -0,0 +1,46 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once + +#include + +#define RS_ERROR std::cout << "\033[1m\033[31m" // bold red +#define RS_WARNING std::cout << "\033[1m\033[33m" // bold yellow +#define RS_INFO std::cout << "\033[1m\033[32m" // bold green +#define RS_INFOL std::cout << "\033[32m" // green +#define RS_DEBUG std::cout << "\033[1m\033[36m" // bold cyan +#define RS_REND "\033[0m" << std::endl + +#define RS_TITLE std::cout << "\033[1m\033[35m" // bold magenta +#define RS_MSG std::cout << "\033[1m\033[37m" // bold white + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/basic_attr.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/basic_attr.hpp new file mode 100644 index 0000000..aa53be7 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/basic_attr.hpp @@ -0,0 +1,426 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include + +namespace robosense +{ +namespace lidar +{ + +#pragma pack(push, 1) + +typedef struct +{ + uint8_t year; + uint8_t month; + uint8_t day; + uint8_t hour; + uint8_t minute; + uint8_t second; + uint16_t ms; + uint16_t us; +} RSTimestampYMD; + +typedef struct +{ + uint8_t sec[6]; + uint8_t ss[4]; +} RSTimestampUTC; + +typedef struct +{ + uint8_t tt[2]; +} RSTemperature; + +#pragma pack(pop) + +#ifdef ENABLE_STAMP_WITH_LOCAL +inline long getTimezone(void) +{ + static long tzone = 0; + static bool tzoneReady = false; + + if (!tzoneReady) + { +#ifdef _MSC_VER + _get_timezone(&tzone); +#else + tzset(); + tzone = timezone; +#endif + + tzoneReady = true; + } + + return tzone; +} +#endif + +inline uint64_t parseTimeUTCWithUs(const RSTimestampUTC* tsUtc) +{ + // sec + uint64_t sec = 0; + for (int i = 0; i < 6; i++) + { + sec <<= 8; + sec += tsUtc->sec[i]; + } + + // us + uint64_t us = 0; + for (int i = 0; i < 4; i++) + { + us <<= 8; + us += tsUtc->ss[i]; + } + +#ifdef ENABLE_STAMP_WITH_LOCAL + sec -= getTimezone(); +#endif + + return (sec * 1000000 + us); +} +inline uint64_t parseTimeUTCWithNs(const RSTimestampUTC* tsUtc) +{ + // sec + uint64_t sec = 0; + for (int i = 0; i < 6; i++) + { + sec <<= 8; + sec += tsUtc->sec[i]; + } + + // ns + uint64_t ns = 0; + for (int i = 0; i < 4; i++) + { + ns <<= 8; + ns += tsUtc->ss[i]; + } + +#ifdef ENABLE_STAMP_WITH_LOCAL + sec -= getTimezone(); +#endif + + return (sec * 1000000000 + ns); +} +inline void createTimeUTCWithUs(uint64_t us, RSTimestampUTC* tsUtc) +{ + uint64_t sec = us / 1000000; + uint64_t usec = us % 1000000; + +#ifdef ENABLE_STAMP_WITH_LOCAL + sec += getTimezone(); +#endif + + for (int i = 5; i >= 0; i--) + { + tsUtc->sec[i] = sec & 0xFF; + sec >>= 8; + } + + for (int i = 3; i >= 0; i--) + { + tsUtc->ss[i] = usec & 0xFF; + usec >>= 8; + } +} +inline void createTimeUTCWithNs(uint64_t ns, RSTimestampUTC* tsUtc) +{ + uint64_t sec = ns / 1000000000; + uint64_t nanoSec = ns % 1000000000; + +#ifdef ENABLE_STAMP_WITH_LOCAL + sec += getTimezone(); +#endif + + for (int i = 5; i >= 0; i--) + { + tsUtc->sec[i] = sec & 0xFF; + sec >>= 8; + } + + for (int i = 3; i >= 0; i--) + { + tsUtc->ss[i] = nanoSec & 0xFF; + nanoSec >>= 8; + } +} + +inline uint64_t parseTimeYMD(const RSTimestampYMD* tsYmd) +{ + std::tm stm; + memset(&stm, 0, sizeof(stm)); + + // since 2000 in robosense YMD, and since 1900 in struct tm + stm.tm_year = tsYmd->year + (2000 - 1900); + // since 1 in robosense YMD, and since 0 in struct tm + stm.tm_mon = tsYmd->month - 1; + // since 1 in both robosense YMD and struct tm + stm.tm_mday = tsYmd->day; + stm.tm_hour = tsYmd->hour; + stm.tm_min = tsYmd->minute; + stm.tm_sec = tsYmd->second; + time_t sec = std::mktime(&stm); + + uint64_t ms = ntohs(tsYmd->ms); + uint64_t us = ntohs(tsYmd->us); + +#if 0 + std::cout << "tm_year:" << stm.tm_year + << ", tm_mon:" << stm.tm_mon + << ", tm_day:" << stm.tm_mday + << ", tm_hour:" << stm.tm_hour + << ", tm_min:" << stm.tm_min + << ", tm_sec:" << stm.tm_sec + << ", ms:" << ms + << ", us:" << us + << std::endl; +#endif + +#ifdef ENABLE_STAMP_WITH_LOCAL + sec -= getTimezone(); +#endif + + return (sec * 1000000 + ms * 1000 + us); +} + +inline void createTimeYMD(uint64_t usec, RSTimestampYMD* tsYmd) +{ + uint64_t us = usec % 1000; + uint64_t tot_ms = (usec - us) / 1000; + + uint64_t ms = tot_ms % 1000; + uint64_t sec = tot_ms / 1000; + +#ifdef ENABLE_STAMP_WITH_LOCAL + sec += getTimezone(); +#endif + + time_t t_sec = sec; + + std::tm* stm = localtime(&t_sec); + +#if 0 + std::cout << "+ tm_year:" << stm->tm_year + << ", tm_mon:" << stm->tm_mon + << ", tm_day:" << stm->tm_mday + << ", tm_hour:" << stm->tm_hour + << ", tm_min:" << stm->tm_min + << ", tm_sec:" << stm->tm_sec + << ", ms:" << ms + << ", us:" << us + << std::endl; +#endif + + // since 2000 in robosense YMD, and since 1900 in struct tm + tsYmd->year = stm->tm_year - (2000 - 1900); + // since 1 in robosense YMD, and since 0 in struct tm + tsYmd->month = stm->tm_mon + 1; + // since 1 in both robosense YMD and struct tm + tsYmd->day = stm->tm_mday; + tsYmd->hour = stm->tm_hour; + tsYmd->minute = stm->tm_min; + tsYmd->second = stm->tm_sec; + + tsYmd->ms = htons((uint16_t)ms); + tsYmd->us = htons((uint16_t)us); +} + +inline uint64_t getTimeHost(void) +{ + std::chrono::system_clock::time_point t = std::chrono::system_clock::now(); + std::chrono::system_clock::duration t_s = t.time_since_epoch(); + + std::chrono::duration> t_us = + std::chrono::duration_cast>>(t_s); + return t_us.count(); +} + +inline uint64_t getTimeHostWithNs(void) +{ + std::chrono::system_clock::time_point t = std::chrono::system_clock::now(); + std::chrono::system_clock::duration t_s = t.time_since_epoch(); + + std::chrono::duration t_ns = + std::chrono::duration_cast>(t_s); + return t_ns.count(); +} +inline int16_t parseTempInLe(const RSTemperature* tmp) // format of little endian +{ + // | lsb | padding | neg | msb | + // | 5 | 3 | 1 | 7 | (in bits) + uint8_t lsb = tmp->tt[0] >> 3; + uint8_t neg = tmp->tt[1] & 0x80; + uint8_t msb = tmp->tt[1] & 0x7F; + + int16_t t = ((uint16_t)msb << 5) + lsb; + if (neg) t = -t; + + return t; +} + +inline int16_t parseTempInBe(const RSTemperature* tmp) // format of big endian +{ + // | neg | msb | lsb | padding | + // | 1 | 7 | 4 | 4 | (in bits) + uint8_t neg = tmp->tt[0] & 0x80; + uint8_t msb = tmp->tt[0] & 0x7F; + uint8_t lsb = tmp->tt[1] >> 4; + + int16_t t = ((uint16_t)msb << 4) + lsb; + if (neg) t = -t; + + return t; +} + +inline uint32_t calcCrc32(const uint8_t *data, uint32_t len, + uint32_t startValue, bool isFirstCall) +{ + static const uint32_t crc32table[] = + { + 0x00000000U, 0x77073096U, 0xee0e612cU, 0x990951baU, 0x076dc419U, + 0x706af48fU, 0xe963a535U, 0x9e6495a3U, 0x0edb8832U, 0x79dcb8a4U, + 0xe0d5e91eU, 0x97d2d988U, 0x09b64c2bU, 0x7eb17cbdU, 0xe7b82d07U, + 0x90bf1d91U, 0x1db71064U, 0x6ab020f2U, 0xf3b97148U, 0x84be41deU, + 0x1adad47dU, 0x6ddde4ebU, 0xf4d4b551U, 0x83d385c7U, 0x136c9856U, + 0x646ba8c0U, 0xfd62f97aU, 0x8a65c9ecU, 0x14015c4fU, 0x63066cd9U, + 0xfa0f3d63U, 0x8d080df5U, 0x3b6e20c8U, 0x4c69105eU, 0xd56041e4U, + 0xa2677172U, 0x3c03e4d1U, 0x4b04d447U, 0xd20d85fdU, 0xa50ab56bU, + 0x35b5a8faU, 0x42b2986cU, 0xdbbbc9d6U, 0xacbcf940U, 0x32d86ce3U, + 0x45df5c75U, 0xdcd60dcfU, 0xabd13d59U, 0x26d930acU, 0x51de003aU, + 0xc8d75180U, 0xbfd06116U, 0x21b4f4b5U, 0x56b3c423U, 0xcfba9599U, + 0xb8bda50fU, 0x2802b89eU, 0x5f058808U, 0xc60cd9b2U, 0xb10be924U, + 0x2f6f7c87U, 0x58684c11U, 0xc1611dabU, 0xb6662d3dU, 0x76dc4190U, + 0x01db7106U, 0x98d220bcU, 0xefd5102aU, 0x71b18589U, 0x06b6b51fU, + 0x9fbfe4a5U, 0xe8b8d433U, 0x7807c9a2U, 0x0f00f934U, 0x9609a88eU, + 0xe10e9818U, 0x7f6a0dbbU, 0x086d3d2dU, 0x91646c97U, 0xe6635c01U, + 0x6b6b51f4U, 0x1c6c6162U, 0x856530d8U, 0xf262004eU, 0x6c0695edU, + 0x1b01a57bU, 0x8208f4c1U, 0xf50fc457U, 0x65b0d9c6U, 0x12b7e950U, + 0x8bbeb8eaU, 0xfcb9887cU, 0x62dd1ddfU, 0x15da2d49U, 0x8cd37cf3U, + 0xfbd44c65U, 0x4db26158U, 0x3ab551ceU, 0xa3bc0074U, 0xd4bb30e2U, + 0x4adfa541U, 0x3dd895d7U, 0xa4d1c46dU, 0xd3d6f4fbU, 0x4369e96aU, + 0x346ed9fcU, 0xad678846U, 0xda60b8d0U, 0x44042d73U, 0x33031de5U, + 0xaa0a4c5fU, 0xdd0d7cc9U, 0x5005713cU, 0x270241aaU, 0xbe0b1010U, + 0xc90c2086U, 0x5768b525U, 0x206f85b3U, 0xb966d409U, 0xce61e49fU, + 0x5edef90eU, 0x29d9c998U, 0xb0d09822U, 0xc7d7a8b4U, 0x59b33d17U, + 0x2eb40d81U, 0xb7bd5c3bU, 0xc0ba6cadU, 0xedb88320U, 0x9abfb3b6U, + 0x03b6e20cU, 0x74b1d29aU, 0xead54739U, 0x9dd277afU, 0x04db2615U, + 0x73dc1683U, 0xe3630b12U, 0x94643b84U, 0x0d6d6a3eU, 0x7a6a5aa8U, + 0xe40ecf0bU, 0x9309ff9dU, 0x0a00ae27U, 0x7d079eb1U, 0xf00f9344U, + 0x8708a3d2U, 0x1e01f268U, 0x6906c2feU, 0xf762575dU, 0x806567cbU, + 0x196c3671U, 0x6e6b06e7U, 0xfed41b76U, 0x89d32be0U, 0x10da7a5aU, + 0x67dd4accU, 0xf9b9df6fU, 0x8ebeeff9U, 0x17b7be43U, 0x60b08ed5U, + 0xd6d6a3e8U, 0xa1d1937eU, 0x38d8c2c4U, 0x4fdff252U, 0xd1bb67f1U, + 0xa6bc5767U, 0x3fb506ddU, 0x48b2364bU, 0xd80d2bdaU, 0xaf0a1b4cU, + 0x36034af6U, 0x41047a60U, 0xdf60efc3U, 0xa867df55U, 0x316e8eefU, + 0x4669be79U, 0xcb61b38cU, 0xbc66831aU, 0x256fd2a0U, 0x5268e236U, + 0xcc0c7795U, 0xbb0b4703U, 0x220216b9U, 0x5505262fU, 0xc5ba3bbeU, + 0xb2bd0b28U, 0x2bb45a92U, 0x5cb36a04U, 0xc2d7ffa7U, 0xb5d0cf31U, + 0x2cd99e8bU, 0x5bdeae1dU, 0x9b64c2b0U, 0xec63f226U, 0x756aa39cU, + 0x026d930aU, 0x9c0906a9U, 0xeb0e363fU, 0x72076785U, 0x05005713U, + 0x95bf4a82U, 0xe2b87a14U, 0x7bb12baeU, 0x0cb61b38U, 0x92d28e9bU, + 0xe5d5be0dU, 0x7cdcefb7U, 0x0bdbdf21U, 0x86d3d2d4U, 0xf1d4e242U, + 0x68ddb3f8U, 0x1fda836eU, 0x81be16cdU, 0xf6b9265bU, 0x6fb077e1U, + 0x18b74777U, 0x88085ae6U, 0xff0f6a70U, 0x66063bcaU, 0x11010b5cU, + 0x8f659effU, 0xf862ae69U, 0x616bffd3U, 0x166ccf45U, 0xa00ae278U, + 0xd70dd2eeU, 0x4e048354U, 0x3903b3c2U, 0xa7672661U, 0xd06016f7U, + 0x4969474dU, 0x3e6e77dbU, 0xaed16a4aU, 0xd9d65adcU, 0x40df0b66U, + 0x37d83bf0U, 0xa9bcae53U, 0xdebb9ec5U, 0x47b2cf7fU, 0x30b5ffe9U, + 0xbdbdf21cU, 0xcabac28aU, 0x53b39330U, 0x24b4a3a6U, 0xbad03605U, + 0xcdd70693U, 0x54de5729U, 0x23d967bfU, 0xb3667a2eU, 0xc4614ab8U, + 0x5d681b02U, 0x2a6f2b94U, 0xb40bbe37U, 0xc30c8ea1U, 0x5a05df1bU, + 0x2d02ef8dU + }; + + if (isFirstCall) + { + startValue = 0xFFFFFFFFU; + } + else + { + /* undo the XOR on the start value */ + startValue ^= 0xFFFFFFFFU; + + /* The reflection of the initial value is not necessary here as we used + * the "reflected" algorithm and reflected table values. */ + } + + /* Process all data byte-wise */ + while (len != 0U) + { + + /* Process one byte of data */ + startValue = crc32table[((uint8_t)startValue) ^ *data] ^ (startValue >> 8U); + /* Advance the pointer and decrease remaining bytes to calculate over + * until all bytes in the buffer have been used as input */ + ++data; + --len; + } /* while (u32Length != 0U) */ + + /* The reflection of the remainder is not necessary here as we used the + * "reflected" algorithm and reflected table values. */ + startValue ^= 0xFFFFFFFFU; /* XOR crc value */ + + return startValue; +} + +inline bool isCrc32Correct(const uint8_t* pkt, size_t size) +{ + // + // packet format + // + // | packet header + packet data | crc32 | rolling_counter | + // | n bytes | 4 bytes | 2 bytes | + // + uint32_t expected; + expected = calcCrc32 (pkt, size - 6, 0/* ignored */, true); + expected = calcCrc32 (pkt + size - 2, 2, expected, false); + + uint32_t actual = *(uint32_t*)(pkt + size - 6); + actual = htonl(actual); + + return (expected == actual); +} + +} // namespace lidar +} // namespace robosense diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/block_iterator.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/block_iterator.hpp new file mode 100644 index 0000000..6662517 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/block_iterator.hpp @@ -0,0 +1,346 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once + +namespace robosense +{ +namespace lidar +{ + +template +class BlockIterator +{ +public: + + static const int MAX_BLOCKS_PER_PKT = 12; + + void get(uint16_t blk, int32_t& az_diff, double& ts) + { + az_diff = az_diffs[blk]; + ts = tss[blk]; + } + + BlockIterator(const T_Packet& pkt, uint16_t blocks_per_pkt, double block_duration, + uint16_t block_az_duration, double fov_blind_duration) + : pkt_(pkt), BLOCKS_PER_PKT(blocks_per_pkt), BLOCK_DURATION(block_duration), + BLOCK_AZ_DURATION(block_az_duration), FOV_BLIND_DURATION(fov_blind_duration) + { + // assert(BLOCKS_PER_PKT <= MAX_BLOCKS_PER_PKT); + } + +protected: + + const T_Packet& pkt_; + const uint16_t BLOCKS_PER_PKT; + const double BLOCK_DURATION; + const uint16_t BLOCK_AZ_DURATION; + const double FOV_BLIND_DURATION; + int32_t az_diffs[MAX_BLOCKS_PER_PKT]; + double tss[MAX_BLOCKS_PER_PKT]; +}; + +template +class SingleReturnBlockIterator : public BlockIterator +{ +public: + + SingleReturnBlockIterator(const T_Packet& pkt, uint16_t blocks_per_pkt, double block_duration, + uint16_t block_az_duration, double fov_blind_duration) + : BlockIterator(pkt, blocks_per_pkt, block_duration, block_az_duration, fov_blind_duration) + { + double tss = 0; + uint16_t blk = 0; + for (; blk < (this->BLOCKS_PER_PKT - 1); blk++) + { + double ts_diff = this->BLOCK_DURATION; + int32_t az_diff = ntohs(this->pkt_.blocks[blk+1].azimuth) - ntohs(this->pkt_.blocks[blk].azimuth); + if (az_diff < 0) { az_diff += 36000; } + + // Skip FOV blind zone + if (az_diff > 100) + { + az_diff = this->BLOCK_AZ_DURATION; + ts_diff = this->FOV_BLIND_DURATION; + } + + this->az_diffs[blk] = az_diff; + this->tss[blk] = tss; + + tss += ts_diff; + } + + this->az_diffs[blk] = this->BLOCK_AZ_DURATION; + this->tss[blk] = tss; + } +}; + +template +class DualReturnBlockIterator : public BlockIterator +{ +public: + + DualReturnBlockIterator(const T_Packet& pkt, uint16_t blocks_per_pkt, double block_duration, + uint16_t block_az_duration, double fov_blind_duration) + : BlockIterator(pkt, blocks_per_pkt, block_duration, block_az_duration, fov_blind_duration) + { + double tss = 0; + uint16_t blk = 0; + for (; blk < (this->BLOCKS_PER_PKT - 2); blk = blk + 2) + { + double ts_diff = this->BLOCK_DURATION; + int32_t az_diff = ntohs(this->pkt_.blocks[blk+2].azimuth) - ntohs(this->pkt_.blocks[blk].azimuth); + if (az_diff < 0) { az_diff += 36000; } + + // Skip FOV blind zone + if (az_diff > 100) + { + az_diff = this->BLOCK_AZ_DURATION; + ts_diff = this->FOV_BLIND_DURATION; + } + + this->az_diffs[blk] = this->az_diffs[blk+1] = az_diff; + this->tss[blk] = this->tss[blk+1] = tss; + + tss += ts_diff; + } + + this->az_diffs[blk] = this->az_diffs[blk+1] = this->BLOCK_AZ_DURATION; + this->tss[blk] = this->tss[blk+1] = tss; + } +}; + +template +class TwoInOneBlockIterator : public BlockIterator +{ +public: + + TwoInOneBlockIterator(const T_Packet& pkt, uint16_t blocks_per_pkt, double block_duration, + uint16_t block_az_duration, double fov_blind_duration) + : BlockIterator(pkt, blocks_per_pkt, block_duration, block_az_duration, fov_blind_duration) + { + double tss = 0; + uint16_t blk = 0; + for (; blk < (this->BLOCKS_PER_PKT - 2); blk = blk + 2) + { + double ts_diff = this->BLOCK_DURATION; + int32_t az_diff = ntohs(this->pkt_.blocks[blk+2].azimuth) - ntohs(this->pkt_.blocks[blk].azimuth); + if (az_diff < 0) { az_diff += 36000; } + + // Skip FOV blind zone + if (az_diff > 100) + { + az_diff = this->BLOCK_AZ_DURATION; + ts_diff = this->FOV_BLIND_DURATION; + } + + this->az_diffs[blk] = this->az_diffs[blk+1] = az_diff; + this->tss[blk] = this->tss[blk+1] = tss; + + tss += ts_diff; + } + + this->az_diffs[blk] = this->az_diffs[blk+1] = this->BLOCK_AZ_DURATION; + this->tss[blk] = this->tss[blk+1] = tss; + } +}; + +template +class FourInOneBlockIterator : public BlockIterator +{ +public: + + FourInOneBlockIterator(const T_Packet& pkt, uint16_t blocks_per_pkt, double block_duration, + uint16_t block_az_duration, double fov_blind_duration) + : BlockIterator(pkt, blocks_per_pkt, block_duration, block_az_duration, fov_blind_duration) + { + double tss = 0; + uint16_t blk = 0; + for (; blk < (this->BLOCKS_PER_PKT - 4); blk = blk + 4) + { + double ts_diff = this->BLOCK_DURATION; + int32_t az_diff = ntohs(this->pkt_.blocks[blk+4].azimuth) - ntohs(this->pkt_.blocks[blk].azimuth); + if (az_diff < 0) { az_diff += 36000; } + + // Skip FOV blind zone + if (az_diff > 100) + { + az_diff = this->BLOCK_AZ_DURATION; + ts_diff = this->FOV_BLIND_DURATION; + } + + this->az_diffs[blk] = this->az_diffs[blk+1] = this->az_diffs[blk+2] = this->az_diffs[blk+3] =az_diff; + this->tss[blk] = this->tss[blk+1] = this->tss[blk+2] = this->tss[blk+3] = tss; + + tss += ts_diff; + } + + this->az_diffs[blk] = this->az_diffs[blk+1] = this->az_diffs[blk+2] = this->az_diffs[blk+3] = this->BLOCK_AZ_DURATION; + this->tss[blk] = this->tss[blk+1] = this->tss[blk+2] = this->tss[blk+3] = tss; + } +}; + +template +class ABDualReturnBlockIterator : public BlockIterator +{ +public: + + ABDualReturnBlockIterator(const T_Packet& pkt, uint16_t blocks_per_pkt, double block_duration, + uint16_t block_az_duration, double fov_blind_duration) + : BlockIterator(pkt, blocks_per_pkt, block_duration, block_az_duration, fov_blind_duration) + { + double ts_diff = this->BLOCK_DURATION; + int32_t az_diff = ntohs(this->pkt_.blocks[2].azimuth) - ntohs(this->pkt_.blocks[0].azimuth); + if (az_diff < 0) { az_diff += 36000; } + + // Skip FOV blind zone + if (az_diff > 100) + { + az_diff = this->BLOCK_AZ_DURATION; + ts_diff = this->FOV_BLIND_DURATION; + } + + if (ntohs(this->pkt_.blocks[0].azimuth) == ntohs(this->pkt_.blocks[1].azimuth)) // AAB + { + double tss = 0; + this->az_diffs[0] = this->az_diffs[1] = az_diff; + this->tss[0] = this->tss[1] = tss; + + tss += ts_diff; + this->az_diffs[2] = this->BLOCK_AZ_DURATION; + this->tss[2] = tss; + } + else // ABB + { + double tss = 0; + this->az_diffs[0] = az_diff; + this->tss[0] = tss; + + tss += ts_diff; + this->az_diffs[1] = this->az_diffs[2] = this->BLOCK_AZ_DURATION; + this->tss[1] = this->tss[2] = tss; + } + } +}; + +template +class Rs16SingleReturnBlockIterator : public BlockIterator +{ +public: + + static + void calcChannel(const float blk_ts, const float firing_tss[], float az_percents[], double ts_diffs[]) + { + for (uint16_t chan = 0; chan < 32; chan++) + { + az_percents[chan] = firing_tss[chan] / (blk_ts * 2); + ts_diffs[chan] = (double)firing_tss[chan] / 1000000; + } + } + + Rs16SingleReturnBlockIterator(const T_Packet& pkt, uint16_t blocks_per_pkt, double block_duration, + uint16_t block_az_duration, double fov_blind_duration) + : BlockIterator(pkt, blocks_per_pkt, block_duration, block_az_duration, fov_blind_duration) + { + double tss = 0; + uint16_t blk = 0; + for (; blk < (this->BLOCKS_PER_PKT - 1); blk++) + { + double ts_diff = this->BLOCK_DURATION * 2; + int32_t az_diff = ntohs(this->pkt_.blocks[blk+1].azimuth) - ntohs(this->pkt_.blocks[blk].azimuth); + if (az_diff < 0) { az_diff += 36000; } + + // Skip FOV blind zone + if (az_diff > 100) + { + az_diff = this->BLOCK_AZ_DURATION * 2; + ts_diff = this->FOV_BLIND_DURATION; + } + + this->az_diffs[blk] = az_diff; + this->tss[blk] = tss; + + tss += ts_diff; + } + + this->az_diffs[blk] = this->BLOCK_AZ_DURATION * 2; + this->tss[blk] = tss; + } +}; + +template +class Rs16DualReturnBlockIterator : public BlockIterator +{ +public: + + static + void calcChannel(const float blk_ts, const float firing_tss[], float az_percents[], double ts_diffs[]) + { + for (uint16_t chan = 0; chan < 32; chan++) + { + az_percents[chan] = firing_tss[chan%16] / blk_ts; + ts_diffs[chan] = (double)firing_tss[chan%16] / 1000000; + } + } + + Rs16DualReturnBlockIterator(const T_Packet& pkt, uint16_t blocks_per_pkt, double block_duration, + uint16_t block_az_duration, double fov_blind_duration) + : BlockIterator(pkt, blocks_per_pkt, block_duration, block_az_duration, fov_blind_duration) + { + double tss = 0; + uint16_t blk = 0; + for (; blk < (this->BLOCKS_PER_PKT - 1); blk++) + { + double ts_diff = this->BLOCK_DURATION; + int32_t az_diff = ntohs(this->pkt_.blocks[blk+1].azimuth) - ntohs(this->pkt_.blocks[blk].azimuth); + if (az_diff < 0) { az_diff += 36000; } + + // Skip FOV blind zone + if (az_diff > 100) + { + az_diff = this->BLOCK_AZ_DURATION; + ts_diff = this->FOV_BLIND_DURATION; + } + + this->az_diffs[blk] = az_diff; + this->tss[blk] = tss; + + tss += ts_diff; + } + + this->az_diffs[blk] = this->BLOCK_AZ_DURATION; + this->tss[blk] = tss; + } +}; + +} // namespace lidar +} // namespace robosense diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/chan_angles.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/chan_angles.hpp new file mode 100644 index 0000000..6f2e882 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/chan_angles.hpp @@ -0,0 +1,238 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once + +#include + +#include +#include + +namespace robosense +{ +namespace lidar +{ + +#pragma pack(push, 1) +typedef struct +{ + uint8_t sign; + uint16_t value; +} RSCalibrationAngle; +#pragma pack(pop) + +class ChanAngles +{ +public: + + ChanAngles(uint16_t chan_num) + : chan_num_(chan_num) + { + vert_angles_.resize(chan_num_); + horiz_angles_.resize(chan_num_); + user_chans_.resize(chan_num_); + } + + int loadFromFile(const std::string& angle_path) + { + std::vector vert_angles; + std::vector horiz_angles; + int ret = loadFromFile (angle_path, chan_num_, vert_angles, horiz_angles); + if (ret < 0) + return ret; + + if (vert_angles.size() != chan_num_) + { + return -1; + } + + vert_angles_.swap(vert_angles); + horiz_angles_.swap(horiz_angles); + genUserChan(vert_angles_, user_chans_); + return 0; + } + + int loadFromDifop(const RSCalibrationAngle vert_angle_arr[], + const RSCalibrationAngle horiz_angle_arr[]) + { + std::vector vert_angles; + std::vector horiz_angles; + int ret = + loadFromDifop (vert_angle_arr, horiz_angle_arr, chan_num_, vert_angles, horiz_angles); + if (ret < 0) + return ret; + + vert_angles_.swap(vert_angles); + horiz_angles_.swap(horiz_angles); + genUserChan(vert_angles_, user_chans_); + return 0; + } + + uint16_t toUserChan(uint16_t chan) + { + return user_chans_[chan]; + } + + int32_t horizAdjust(uint16_t chan, int32_t horiz) + { + return (horiz + round(horiz_angles_[chan])); + } + + int32_t vertAdjust(uint16_t chan) + { + return vert_angles_[chan]; + } + + void print() + { + std::cout << "---------------------" << std::endl + << "chan_num:" << chan_num_ << std::endl; + + std::cout << "vert_angle\thoriz_angle\tuser_chan" << std::endl; + + for (uint16_t chan = 0; chan < chan_num_; chan++) + { + std::cout << vert_angles_[chan] << "\t" + << horiz_angles_[chan] << "\t" + << user_chans_[chan] << std::endl; + } + } + +#ifndef UNIT_TEST +private: +#endif + + static + void genUserChan(const std::vector& vert_angles, std::vector& user_chans) + { + user_chans.resize(vert_angles.size()); + + for (size_t i = 0; i < vert_angles.size(); i++) + { + int32_t angle = vert_angles[i]; + uint16_t chan = 0; + + for (size_t j = 0; j < vert_angles.size(); j++) + { + if (vert_angles[j] < angle) + { + chan++; + } + } + + user_chans[i] = chan; + } + } + + static int loadFromFile(const std::string& angle_path, size_t size, + std::vector& vert_angles, std::vector& horiz_angles) + { + vert_angles.clear(); + horiz_angles.clear(); + + std::ifstream fd(angle_path.c_str(), std::ios::in); + if (!fd.is_open()) + { + std::cout << "fail to open angle file:" << angle_path << std::endl; + return -1; + } + + std::string line; + for (size_t i = 0; i < size; i++) + { + if (!std::getline(fd, line)) + return -1; + + float vert = std::stof(line); + + float horiz = 0; + size_t pos_comma = line.find_first_of(','); + if (pos_comma != std::string::npos) + { + horiz = std::stof(line.substr(pos_comma+1)); + } + + vert_angles.emplace_back(static_cast(vert * 100)); + horiz_angles.emplace_back(static_cast(horiz * 100)); + } + + fd.close(); + return 0; + } + + static int loadFromDifop(const RSCalibrationAngle* vert_angle_arr, + const RSCalibrationAngle* horiz_angle_arr, size_t size, + std::vector& vert_angles, std::vector& horiz_angles) + { + vert_angles.clear(); + horiz_angles.clear(); + + for (size_t i = 0; i < size; i++) + { + const RSCalibrationAngle& vert = vert_angle_arr[i]; + const RSCalibrationAngle& horiz = horiz_angle_arr[i]; + int32_t v; + if (vert.sign == 0xFF) + return -1; + + v = ntohs(vert.value); + if (vert.sign != 0) v = -v; + vert_angles.emplace_back(v); + + if (!angleCheck (v)) + return -1; + + v = ntohs(horiz.value); + if (horiz.sign != 0) v = -v; + horiz_angles.emplace_back(v); + + if (!angleCheck (v)) + return -1; + + } + + return ((vert_angles.size() > 0) ? 0 : -1); + } + + static bool angleCheck(int32_t v) + { + return ((-9000 <= v) && (v < 18000)); + } + + uint16_t chan_num_; + std::vector vert_angles_; + std::vector horiz_angles_; + std::vector user_chans_; +}; + +} // namespace lidar +} // namespace robosense diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder.hpp new file mode 100644 index 0000000..52676a5 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder.hpp @@ -0,0 +1,533 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include "rs_driver/msg/imu_data_msg.hpp" +#ifndef _USE_MATH_DEFINES +#define _USE_MATH_DEFINES // for VC++, required to use const M_IP in +#endif + +#ifdef ENABLE_TRANSFORM +// Eigen lib +#include +#endif + +#include +#include +#include +#include + +namespace robosense +{ +namespace lidar +{ +#pragma pack(push, 1) +typedef struct +{ + uint8_t id[8]; + uint8_t reserved_1[12]; + RSTimestampYMD timestamp; + uint8_t legacy_lidar_type; + uint8_t lidar_type; + uint8_t lidar_model; + uint8_t reserved_2[5]; + RSTemperature temp; + uint8_t reserved_3[2]; +} RSMsopHeaderV1; + +typedef struct +{ + uint8_t id[4]; + uint16_t protocol_version; + uint8_t reserved_1; + uint8_t wave_mode; + RSTemperature temp; + RSTimestampUTC timestamp; + uint8_t reserved_2[10]; + uint8_t lidar_type; + uint8_t lidar_model; + uint8_t reserved_3[48]; +} RSMsopHeaderV2; + +typedef struct +{ + uint16_t distance; + uint8_t intensity; +} RSChannel; + +typedef struct +{ + uint8_t lidar_ip[4]; + uint8_t host_ip[4]; + uint8_t mac_addr[6]; + uint16_t local_port; + uint16_t dest_port; + uint16_t port3; + uint16_t port4; +} RSEthNetV1; + +typedef struct +{ + uint8_t lidar_ip[4]; + uint8_t dest_ip[4]; + uint8_t mac_addr[6]; + uint16_t msop_port; + uint16_t reserve_1; + uint16_t difop_port; + uint16_t reserve_2; +} RSEthNetV2; + +typedef struct +{ + uint16_t start_angle; + uint16_t end_angle; +} RSFOV; + +typedef struct +{ + uint8_t top_ver[5]; + uint8_t bottom_ver[5]; +} RSVersionV1; + +typedef struct +{ + uint8_t top_ver[5]; // firmware + uint8_t bottom_ver[5]; // firmware + uint8_t bot_soft_ver[5]; + uint8_t motor_firmware_ver[5]; + uint8_t hw_ver[3]; +} RSVersionV2; + +typedef struct +{ + uint8_t num[6]; +} RSSN; + +typedef struct +{ + uint8_t device_current[3]; + uint8_t main_current[3]; + uint16_t vol_12v; + uint16_t vol_sim_1v8; + uint16_t vol_dig_3v3; + uint16_t vol_sim_3v3; + uint16_t vol_dig_5v4; + uint16_t vol_sim_5v; + uint16_t vol_ejc_5v; + uint16_t vol_recv_5v; + uint16_t vol_apd; +} RSStatusV1; + +typedef struct +{ + uint16_t device_current; + uint16_t vol_fpga; + uint16_t vol_12v; + uint16_t vol_dig_5v4; + uint16_t vol_sim_5v; + uint16_t vol_apd; + uint8_t reserved[12]; +} RSStatusV2; + +typedef struct +{ + uint8_t reserved_1[9]; + uint16_t checksum; + uint16_t manc_err1; + uint16_t manc_err2; + uint8_t gps_status; + uint16_t temperature1; + uint16_t temperature2; + uint16_t temperature3; + uint16_t temperature4; + uint16_t temperature5; + uint8_t reserved_2[5]; + uint16_t cur_rpm; + uint8_t reserved_3[7]; +} RSDiagnoV1; + +typedef struct +{ + uint16_t bot_fpga_temperature; + uint16_t recv_A_temperature; + uint16_t recv_B_temperature; + uint16_t main_fpga_temperature; + uint16_t main_fpga_core_temperature; + uint16_t real_rpm; + uint8_t lane_up; + uint16_t lane_up_cnt; + uint16_t main_status; + uint8_t gps_status; + uint8_t reserved[22]; +} RSDiagnoV2; + +typedef struct +{ + uint8_t sync_mode; + uint8_t sync_sts; + RSTimestampUTC timestamp; +} RSTimeInfo; + +#pragma pack(pop) + +// Echo mode +enum RSEchoMode +{ + ECHO_SINGLE = 0, + ECHO_DUAL +}; + +// decoder const param +struct RSDecoderConstParam +{ + // packet len + uint16_t MSOP_LEN; + uint16_t DIFOP_LEN; + + // packet identity + uint8_t MSOP_ID_LEN; + uint8_t DIFOP_ID_LEN; + uint8_t MSOP_ID[8]; + uint8_t DIFOP_ID[8]; + uint8_t BLOCK_ID[2]; + + // packet structure + uint16_t LASER_NUM; + uint16_t BLOCKS_PER_PKT; + uint16_t CHANNELS_PER_BLOCK; + + // distance & temperature + float DISTANCE_MIN; + float DISTANCE_MAX; + float DISTANCE_RES; + float TEMPERATURE_RES; + + // IMU parameters (new fields with default values) + uint16_t IMU_LEN{ 0 }; // Default to 0 for lidar without IMU + uint16_t IMU_ID_LEN{ 0 }; // Default to 0 for lidar without IMU + uint8_t IMU_ID[4]{ 0 }; // Default to {0, 0, 0, 0} for lidar without IMU +}; + +#define INIT_ONLY_ONCE() \ + static bool init_flag = false; \ + if (init_flag) \ + return param; \ + init_flag = true; + +template +class Decoder +{ +public: +#ifdef ENABLE_TRANSFORM + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +#endif + + virtual void decodeDifopPkt(const uint8_t* pkt, size_t size) = 0; + virtual bool decodeMsopPkt(const uint8_t* pkt, size_t size) = 0; + virtual void decodeImuPkt(const uint8_t* pkt, size_t size){}; + virtual ~Decoder() = default; + + void processDifopPkt(const uint8_t* pkt, size_t size); + bool processMsopPkt(const uint8_t* pkt, size_t size); + bool processImuPkt(const uint8_t* pkt, size_t size); + + explicit Decoder(const RSDecoderConstParam& const_param, const RSDecoderParam& param); + + bool getTemperature(float& temp); + bool getDeviceInfo(DeviceInfo& info); + bool getDeviceStatus(DeviceStatus& status); + double getPacketDuration(); + void enableWritePktTs(bool value); + double prevPktTs(); + void transformPoint(float& x, float& y, float& z); + + void regCallback(const std::function& cb_excep, + const std::function& cb_split_frame); + void regImuCallback(const std::function& cb_imu_data); + virtual bool isNewFrame(const uint8_t* packet); + bool isNewFrameCommon(const uint8_t* packet); + + std::shared_ptr point_cloud_; // accumulated point cloud currently + std::shared_ptr imuDataPtr_; + SplitStrategyBySeq pre_split_strategy_comm_; + bool is_mech_{ false }; + static constexpr uint8_t PKT_SEQ_HIGH_BYTE_OFFSET = 4; + static constexpr uint8_t PKT_SEQ_LOW_BYTE_OFFSET = 5; + +#ifndef UNIT_TEST +protected: +#endif + + double cloudTs(); + + RSDecoderConstParam const_param_; // const param + RSDecoderParam param_; // user param + std::function cb_split_frame_; + std::function cb_excep_; + std::function cb_imu_data_; + bool write_pkt_ts_; + +#ifdef ENABLE_TRANSFORM + Eigen::Matrix4d trans_; +#endif + + Trigon trigon_; +#define SIN(angle) this->trigon_.sin(angle) +#define COS(angle) this->trigon_.cos(angle) + + double packet_duration_; + DistanceSection distance_section_; // invalid section of distance + + RSEchoMode echo_mode_; // echo mode (defined by return mode) + float temperature_; // lidar temperature + DeviceInfo device_info_; + DeviceStatus device_status_; + + bool angles_ready_; // is vert_angles/horiz_angles ready from csv file/difop packet? + double prev_pkt_ts_; // timestamp of prevous packet + double prev_point_ts_; // timestamp of previous point + double first_point_ts_; // timestamp of first point + bool is_get_temperature_{ false }; +}; + +template +inline void Decoder::regCallback(const std::function& cb_excep, + const std::function& cb_split_frame) +{ + cb_excep_ = cb_excep; + cb_split_frame_ = cb_split_frame; +} +template +inline void Decoder::regImuCallback(const std::function& cb_imu_data) +{ + cb_imu_data_ = cb_imu_data; +} + +template +inline Decoder::Decoder(const RSDecoderConstParam& const_param, const RSDecoderParam& param) + : const_param_(const_param) + , param_(param) + , write_pkt_ts_(false) + , packet_duration_(0) + , distance_section_(const_param.DISTANCE_MIN, const_param.DISTANCE_MAX, param.min_distance, param.max_distance) + , echo_mode_(ECHO_SINGLE) + , temperature_(0.0) + , angles_ready_(false) + , prev_pkt_ts_(0.0) + , prev_point_ts_(0.0) + , first_point_ts_(0.0) + +{ +#ifdef ENABLE_TRANSFORM + Eigen::AngleAxisd current_rotation_x(param_.transform_param.roll, Eigen::Vector3d::UnitX()); + Eigen::AngleAxisd current_rotation_y(param_.transform_param.pitch, Eigen::Vector3d::UnitY()); + Eigen::AngleAxisd current_rotation_z(param_.transform_param.yaw, Eigen::Vector3d::UnitZ()); + Eigen::Translation3d current_translation(param_.transform_param.x, param_.transform_param.y, + param_.transform_param.z); + trans_ = (current_translation * current_rotation_z * current_rotation_y * current_rotation_x).matrix(); +#endif +} + +template +inline void Decoder::enableWritePktTs(bool value) +{ + write_pkt_ts_ = value; +} + +template +inline bool Decoder::getTemperature(float& temp) +{ + if (!is_get_temperature_) + { + return false; + } + + temp = temperature_; + return true; +} + +template +inline bool Decoder::getDeviceInfo(DeviceInfo& info) +{ + if (!device_info_.state) + { + return false; + } + info = device_info_; + return true; +} + +template +inline bool Decoder::getDeviceStatus(DeviceStatus& status) +{ + if (!device_status_.state) + { + return false; + } + status = device_status_; + device_status_.init(); + return true; +} + +template +inline double Decoder::getPacketDuration() +{ + return packet_duration_; +} + +template +inline double Decoder::prevPktTs() +{ + return prev_pkt_ts_; +} + +template +inline double Decoder::cloudTs() +{ + return (param_.ts_first_point ? first_point_ts_ : prev_point_ts_); +} + +template +inline void Decoder::transformPoint(float& x, float& y, float& z) +{ +#ifdef ENABLE_TRANSFORM + Eigen::Vector4d target_ori(x, y, z, 1); + Eigen::Vector4d target_rotate = trans_ * target_ori; + x = target_rotate(0); + y = target_rotate(1); + z = target_rotate(2); +#endif +} + +template +inline void Decoder::processDifopPkt(const uint8_t* pkt, size_t size) +{ + if (size != this->const_param_.DIFOP_LEN) + { + LIMIT_CALL(this->cb_excep_(Error(ERRCODE_WRONGDIFOPLEN)), 1); + return; + } + + if (memcmp(pkt, this->const_param_.DIFOP_ID, const_param_.DIFOP_ID_LEN) != 0) + { + LIMIT_CALL(this->cb_excep_(Error(ERRCODE_WRONGDIFOPID)), 1); + return; + } + + decodeDifopPkt(pkt, size); +} +template +inline bool Decoder::processImuPkt(const uint8_t* pkt, size_t size) +{ + if (size != this->const_param_.IMU_LEN) + { + LIMIT_CALL(this->cb_excep_(Error(ERRCODE_WRONGIMULEN)), 1); + return false; + } + + if (memcmp(pkt, this->const_param_.IMU_ID, this->const_param_.IMU_ID_LEN) != 0) + { + LIMIT_CALL(this->cb_excep_(Error(ERRCODE_WRONGIMUID)), 1); + return false; + } + decodeImuPkt(pkt, size); + return true; +} + +template +inline bool Decoder::processMsopPkt(const uint8_t* pkt, size_t size) +{ + constexpr static int CLOUD_POINT_MAX = 5000000; + + if (this->point_cloud_ && (this->point_cloud_->points.size() > CLOUD_POINT_MAX)) + { + LIMIT_CALL(this->cb_excep_(Error(ERRCODE_CLOUDOVERFLOW)), 1); + this->point_cloud_->points.clear(); + } + + if (param_.wait_for_difop && !angles_ready_) + { + DELAY_LIMIT_CALL(cb_excep_(Error(ERRCODE_NODIFOPRECV)), 1); + return false; + } + + if (size != this->const_param_.MSOP_LEN) + { + LIMIT_CALL(this->cb_excep_(Error(ERRCODE_WRONGMSOPLEN)), 1); + return false; + } + + if (memcmp(pkt, this->const_param_.MSOP_ID, this->const_param_.MSOP_ID_LEN) != 0) + { + LIMIT_CALL(this->cb_excep_(Error(ERRCODE_WRONGMSOPID)), 1); + return false; + } + +#ifdef ENABLE_CRC32_CHECK + if (!isCrc32Correct(pkt, size)) + { + LIMIT_CALL(this->cb_excep_(Error(ERRCODE_WRONGCRC32)), 1); + return false; + } +#endif + + return decodeMsopPkt(pkt, size); +} + +template +inline bool Decoder::isNewFrameCommon(const uint8_t* packet) +{ + if (packet == nullptr) + { + return false; + } + uint16_t pkt_seq = static_cast((packet[PKT_SEQ_HIGH_BYTE_OFFSET] << 8) | packet[PKT_SEQ_LOW_BYTE_OFFSET]); + return pre_split_strategy_comm_.newPacket(pkt_seq); +} + +template +inline bool Decoder::isNewFrame(const uint8_t* packet) +{ + if (!is_mech_) + return isNewFrameCommon(packet); + return false; +} +} // namespace lidar +} // namespace robosense diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RS128.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RS128.hpp new file mode 100644 index 0000000..bb8f4c1 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RS128.hpp @@ -0,0 +1,338 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once +#include + +namespace robosense +{ +namespace lidar +{ +#pragma pack(push, 1) +typedef struct +{ + uint8_t id[1]; + uint8_t ret_id; + uint16_t azimuth; + RSChannel channels[128]; +} RS128MsopBlock; + +typedef struct +{ + RSMsopHeaderV2 header; + RS128MsopBlock blocks[3]; + uint8_t reserved[4]; +} RS128MsopPkt; + +typedef struct +{ + uint8_t id[8]; + uint16_t rpm; + RSEthNetV2 eth; + RSFOV fov; + uint8_t reserved1[2]; + uint16_t phase_lock_angle; + RSVersionV2 version; + uint8_t reserved2[229]; + RSSN sn; + uint16_t zero_cali; + uint8_t return_mode; + RSTimeInfo time_info; + RSStatusV1 status; + uint8_t reserved3[5]; + RSDiagnoV1 diagno; + uint8_t gprmc[86]; + RSCalibrationAngle vert_angle_cali[128]; + RSCalibrationAngle horiz_angle_cali[128]; + uint8_t reserved4[10]; + uint16_t tail; +} RS128DifopPkt; + +#pragma pack(pop) + +template +class DecoderRS128 : public DecoderMech +{ +public: + virtual void decodeDifopPkt(const uint8_t* pkt, size_t size); + virtual bool decodeMsopPkt(const uint8_t* pkt, size_t size); + virtual ~DecoderRS128() = default; + + explicit DecoderRS128(const RSDecoderParam& param); + virtual bool isNewFrame(const uint8_t* packet) override; +#ifndef UNIT_TEST +protected: +#endif + + static RSDecoderMechConstParam& getConstParam(); + static RSEchoMode getEchoMode(uint8_t mode); + + template + bool internDecodeMsopPkt(const uint8_t* pkt, size_t size); +}; + +template +inline RSDecoderMechConstParam& DecoderRS128::getConstParam() +{ + static RSDecoderMechConstParam param = + { + { + 1248 // msop len + , 1248 // difop len + , 4 // msop id len + , 8 // difop id len + , {0x55, 0xAA, 0x05, 0x5A} // msop id + , {0xA5, 0xFF, 0x00, 0x5A, 0x11, 0x11, 0x55, 0x55} // difop id + , {0xFE} // block id + , 128 // laser number + , 3 // blocks per packet + , 128 // channels per block + , 0.4f // distance min + , 250.0f // distance max + , 0.005f // distance resolution + , 0.0625f // temperature resolution + } + // lens center + , 0.03615f // RX + , -0.017f // RY + , 0.0f // RZ + }; + + INIT_ONLY_ONCE(); + + float blk_ts = 55.55f; + float firing_tss[] = + { + 0.00f, 0.00f, 0.00f, 0.00f, 3.236f, 3.236f, 3.236f, 3.236f, + 6.472f, 6.472f, 6.472f, 6.472f, 9.708f, 9.708f, 9.708f, 9.708f, + 12.944f, 12.944f, 12.944f, 12.944f, 16.18f, 16.18f, 16.18f, 16.18f, + 19.416f, 19.416f, 19.416f, 19.416f, 22.652f, 22.652f, 22.652f, 22.652f, + 25.888f, 25.888f, 25.888f, 25.888f, 29.124f, 29.124f, 29.124f, 29.124f, + 32.36f, 32.36f, 32.36f, 32.36f, 35.596f, 35.596f, 35.596f, 35.596f, + 38.832f, 38.832f, 38.832f, 38.832f, 42.068f, 42.068f, 42.068f, 42.068f, + 45.304f, 45.304f, 45.304f, 45.304f, 48.54f, 48.54f, 48.54f, 48.54f, + + 00.00f, 0.00f, 0.00f, 0.00f, 3.236f, 3.236f, 3.236f, 3.236f, + 6.472f, 6.472f, 6.472f, 6.472f, 9.708f, 9.708f, 9.708f, 9.708f, + 12.944f, 12.944f, 12.944f, 12.944f, 16.18f, 16.18f, 16.18f, 16.18f, + 19.416f, 19.416f, 19.416f, 19.416f, 22.652f, 22.652f, 22.652f, 22.652f, + 25.888f, 25.888f, 25.888f, 25.888f, 29.124f, 29.124f, 29.124f, 29.124f, + 32.36f, 32.36f, 32.36f, 32.36f, 35.596f, 35.596f, 35.596f, 35.596f, + 38.832f, 38.832f, 38.832f, 38.832f, 42.068f, 42.068f, 42.068f, 42.068f, + 45.304f, 45.304f, 45.304f, 45.304f, 48.54f, 48.54f, 48.54f, 48.54f + }; + + param.BLOCK_DURATION = blk_ts / 1000000; + for (uint16_t i = 0; i < sizeof(firing_tss)/sizeof(firing_tss[0]); i++) + { + param.CHAN_TSS[i] = (double)firing_tss[i] / 1000000; + param.CHAN_AZIS[i] = firing_tss[i] / blk_ts; + } + + return param; +} + +template +inline RSEchoMode DecoderRS128::getEchoMode(uint8_t mode) +{ + switch (mode) + { + case 0x03: // dual return + return RSEchoMode::ECHO_DUAL; + case 0x01: // last return + case 0x02: // strongest return + default: + return RSEchoMode::ECHO_SINGLE; + } +} + +template +inline DecoderRS128::DecoderRS128(const RSDecoderParam& param) + : DecoderMech(getConstParam(), param) +{ +} + +template +inline void DecoderRS128::decodeDifopPkt(const uint8_t* packet, size_t size) +{ + const RS128DifopPkt& pkt = *(const RS128DifopPkt*)(packet); + this->template decodeDifopCommon(pkt); + + this->echo_mode_ = getEchoMode (pkt.return_mode); + this->split_blks_per_frame_ = (this->echo_mode_ == RSEchoMode::ECHO_DUAL) ? + (this->blks_per_frame_ << 1) : this->blks_per_frame_; +} + +template +inline bool DecoderRS128::decodeMsopPkt(const uint8_t* pkt, size_t size) +{ + if (this->echo_mode_ == RSEchoMode::ECHO_SINGLE) + { + return internDecodeMsopPkt>(pkt, size); + } + else + { + return internDecodeMsopPkt>(pkt, size); + } +} + +template +template +inline bool DecoderRS128::internDecodeMsopPkt(const uint8_t* packet, size_t size) +{ + const RS128MsopPkt& pkt = *(const RS128MsopPkt*)(packet); + bool ret = false; + + this->temperature_ = parseTempInBe(&(pkt.header.temp)) * this->const_param_.TEMPERATURE_RES; + this->is_get_temperature_ = true; + double pkt_ts = 0; + if (this->param_.use_lidar_clock) + { + pkt_ts = parseTimeUTCWithUs(&pkt.header.timestamp) * 1e-6; + } + else + { + uint64_t ts = getTimeHost(); + + // roll back to first block to approach lidar ts as near as possible. + pkt_ts = ts * 1e-6 - this->getPacketDuration(); + + if (this->write_pkt_ts_) + { + createTimeUTCWithUs (ts, (RSTimestampUTC*)&pkt.header.timestamp); + } + } + + T_BlockIterator iter(pkt, this->const_param_.BLOCKS_PER_PKT, this->mech_const_param_.BLOCK_DURATION, + this->block_az_diff_, this->fov_blind_ts_diff_); + + for (uint16_t blk = 0; blk < this->const_param_.BLOCKS_PER_PKT; blk++) + { + const RS128MsopBlock& block = pkt.blocks[blk]; + + if (memcmp(this->const_param_.BLOCK_ID, block.id, 1) != 0) + { + // + // Disable error report temporarily + // + //this->cb_excep_(Error(ERRCODE_WRONGMSOPBLKID)); + + break; + } + + int32_t block_az_diff; + double block_ts_off; + iter.get(blk, block_az_diff, block_ts_off); + + double block_ts = pkt_ts + block_ts_off; + int32_t block_az = ntohs(block.azimuth); + if (this->split_strategy_->newBlock(block_az)) + { + this->cb_split_frame_(this->const_param_.LASER_NUM, this->cloudTs()); + this->first_point_ts_ = block_ts; + ret = true; + } + + for (uint16_t chan = 0; chan < this->const_param_.CHANNELS_PER_BLOCK; chan++) + { + const RSChannel& channel = block.channels[chan]; + double chan_ts = block_ts + this->mech_const_param_.CHAN_TSS[chan]; + int32_t angle_horiz = block_az + + (int32_t)((float)block_az_diff * this->mech_const_param_.CHAN_AZIS[chan]); + + int32_t angle_vert = this->chan_angles_.vertAdjust(chan); + int32_t angle_horiz_final = this->chan_angles_.horizAdjust(chan, angle_horiz); + float distance = ntohs(channel.distance) * this->const_param_.DISTANCE_RES; + + if (this->distance_section_.in(distance) && this->scan_section_.in(angle_horiz_final)) + { + float x = distance * COS(angle_vert) * COS(angle_horiz_final) + this->mech_const_param_.RX * COS(angle_horiz); + float y = -distance * COS(angle_vert) * SIN(angle_horiz_final) - this->mech_const_param_.RX * SIN(angle_horiz); + float z = distance * SIN(angle_vert) + this->mech_const_param_.RZ; + this->transformPoint(x, y, z); + + typename T_PointCloud::PointT point; + setX(point, x); + setY(point, y); + setZ(point, z); + setIntensity(point, channel.intensity); + setTimestamp(point, chan_ts); + setRing(point, this->chan_angles_.toUserChan(chan)); + + this->point_cloud_->points.emplace_back(point); + } + else if (!this->param_.dense_points) + { + typename T_PointCloud::PointT point; + setX(point, NAN); + setY(point, NAN); + setZ(point, NAN); + setIntensity(point, 0); + setTimestamp(point, chan_ts); + setRing(point, this->chan_angles_.toUserChan(chan)); + + this->point_cloud_->points.emplace_back(point); + } + + this->prev_point_ts_ = chan_ts; + } + } + + this->prev_pkt_ts_ = pkt_ts; + return ret; +} + +template +inline bool DecoderRS128::isNewFrame(const uint8_t* packet) +{ + const RS128MsopPkt& pkt = *(const RS128MsopPkt*)(packet); + + for (uint16_t blk = 0; blk < this->const_param_.BLOCKS_PER_PKT; blk++) + { + const RS128MsopBlock& block = pkt.blocks[blk]; + + if (memcmp(this->const_param_.BLOCK_ID, block.id, 1) != 0) + { + break; + } + + int32_t block_az = ntohs(block.azimuth); + if (this->pre_split_strategy_->newBlock(block_az)) + { + return true; + } + } + return false; +} + +} // namespace lidar +} // namespace robosense diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RS16.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RS16.hpp new file mode 100644 index 0000000..3e81b42 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RS16.hpp @@ -0,0 +1,374 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once +#include + +namespace robosense +{ +namespace lidar +{ + +#pragma pack(push, 1) +typedef struct +{ + uint8_t id[2]; + uint16_t azimuth; + RSChannel channels[32]; +} RS16MsopBlock; + +typedef struct +{ + RSMsopHeaderV1 header; + RS16MsopBlock blocks[12]; + unsigned int index; + uint16_t tail; +} RS16MsopPkt; + +typedef struct +{ + uint8_t id[8]; + uint16_t rpm; + RSEthNetV1 eth; + RSFOV fov; + uint8_t reserved0[2]; + uint16_t phase_lock_angle; + RSVersionV1 version; + uint8_t reserved1[242]; + RSSN sn; + uint16_t zero_cali; + uint8_t return_mode; + uint16_t sw_ver; + RSTimestampYMD timestamp; + RSStatusV1 status; + uint8_t reserved2[5]; + RSDiagnoV1 diagno; + uint8_t gprmc[86]; + uint8_t reserved3[697]; + uint8_t pitch_cali[48]; + uint8_t reserved4[33]; + uint16_t tail; +} RS16DifopPkt; + +#pragma pack(pop) + +inline void RS16DifopPkt2Adapter (const RS16DifopPkt& src, AdapterDifopPkt& dst) +{ + dst.rpm = src.rpm; + dst.fov = src.fov; + dst.return_mode = src.return_mode; + dst.sn = src.sn; + dst.eth = src.eth; + dst.version = src.version; + dst.status = src.status; + + for (uint16_t i = 0; i < 16; i++) + { + uint32_t v = 0; + v += src.pitch_cali[i*3]; + v = v << 8; + v += src.pitch_cali[i*3 + 1]; + v = v << 8; + v += src.pitch_cali[i*3 + 2]; + + uint16_t v2 = (uint16_t)(v * 0.01); // higher resolution to lower one. + + dst.vert_angle_cali[i].sign = (i < 8) ? 1 : 0; + dst.vert_angle_cali[i].value = htons(v2); + dst.horiz_angle_cali[i].sign = 0; + dst.horiz_angle_cali[i].value = 0; + } +} + +template +class DecoderRS16 : public DecoderMech +{ +public: + virtual void decodeDifopPkt(const uint8_t* pkt, size_t size); + virtual bool decodeMsopPkt(const uint8_t* pkt, size_t size); + virtual ~DecoderRS16() = default; + + explicit DecoderRS16(const RSDecoderParam& param); + virtual bool isNewFrame(const uint8_t* packet) override; +#ifndef UNIT_TEST +protected: +#endif + + static RSDecoderMechConstParam& getConstParam(); + static RSEchoMode getEchoMode(uint8_t mode); + + void calcParam(); + template + bool internDecodeMsopPkt(const uint8_t* pkt, size_t size); +}; + +template +inline RSDecoderMechConstParam& DecoderRS16::getConstParam() +{ + static RSDecoderMechConstParam param = + { + { + 1248 // msop len + , 1248 // difop len + , 8 // msop id len + , 8 // difop id len + , {0x55, 0xAA, 0x05, 0x0A, 0x5A, 0xA5, 0x50, 0xA0} // msop id + , {0xA5, 0xFF, 0x00, 0x5A, 0x11, 0x11, 0x55, 0x55} // difop id + , {0xFF, 0xEE} // block id + , 16 // laser number + , 12 // blocks per packet + , 32 // channels per block. how many channels in the msop block. + , 0.4f // distance min + , 230.0f // distance max + , 0.005f // distance resolution + , 0.0625f // temperature resolution + } + // lens center + , 0.03825f // RX + , -0.01088f // RY + , 0.0f // RZ + }; + + float blk_ts = 55.50f; + param.BLOCK_DURATION = blk_ts / 1000000; + + return param; +} + +template +inline void DecoderRS16::calcParam() +{ + float blk_ts = 55.50f; + float firing_tss[] = + { + 0.00f, 2.80f, 5.60f, 8.40f, 11.20f, 14.00f, 16.80f, 19.60f, + 22.40f, 25.20f, 28.00f, 30.80f, 33.60f, 36.40f, 39.20f, 42.00f, + 55.50f, 58.30f, 61.10f, 63.90f, 66.70f, 69.50f, 72.30f, 75.10f, + 77.90f, 80.70f, 83.50f, 86.30f, 89.10f, 91.90f, 94.70f, 97.50f + }; + + if (this->echo_mode_ == RSEchoMode::ECHO_SINGLE) + { + Rs16SingleReturnBlockIterator::calcChannel(blk_ts, firing_tss, + this->mech_const_param_.CHAN_AZIS, this->mech_const_param_.CHAN_TSS); + } + else + { + Rs16DualReturnBlockIterator::calcChannel(blk_ts, firing_tss, + this->mech_const_param_.CHAN_AZIS, this->mech_const_param_.CHAN_TSS); + } +} + +template +inline RSEchoMode DecoderRS16::getEchoMode(uint8_t mode) +{ + switch (mode) + { + case 0x00: // dual return + return RSEchoMode::ECHO_DUAL; + case 0x01: // strongest return + case 0x02: // last return + default: + return RSEchoMode::ECHO_SINGLE; + } +} + +template +inline DecoderRS16::DecoderRS16(const RSDecoderParam& param) + : DecoderMech(getConstParam(), param) +{ + this->packet_duration_ = + this->mech_const_param_.BLOCK_DURATION * this->const_param_.BLOCKS_PER_PKT * 2; + + calcParam(); +} + +template +inline void DecoderRS16::decodeDifopPkt(const uint8_t* packet, size_t size) +{ + const RS16DifopPkt& orig = *(const RS16DifopPkt*)packet; + AdapterDifopPkt adapter; + RS16DifopPkt2Adapter (orig, adapter); + + this->template decodeDifopCommon(adapter); + + RSEchoMode echo_mode = getEchoMode (adapter.return_mode); + if (this->echo_mode_ != echo_mode) + { + this->echo_mode_ = echo_mode; + this->split_blks_per_frame_ = (this->echo_mode_ == RSEchoMode::ECHO_DUAL) ? + this->blks_per_frame_ : (this->blks_per_frame_ >> 1); + + calcParam(); + } +} + +template +inline bool DecoderRS16::decodeMsopPkt(const uint8_t* pkt, size_t size) +{ + if (this->echo_mode_ == RSEchoMode::ECHO_SINGLE) + { + return internDecodeMsopPkt>(pkt, size); + } + else + { + return internDecodeMsopPkt>(pkt, size); + } +} + +template +template +inline bool DecoderRS16::internDecodeMsopPkt(const uint8_t* packet, size_t size) +{ + const RS16MsopPkt& pkt = *(const RS16MsopPkt*)(packet); + bool ret = false; + + this->temperature_ = parseTempInLe(&(pkt.header.temp)) * this->const_param_.TEMPERATURE_RES; + this->is_get_temperature_ = true; + double pkt_ts = 0; + if (this->param_.use_lidar_clock) + { + pkt_ts = parseTimeYMD(&pkt.header.timestamp) * 1e-6; + } + else + { + uint64_t ts = getTimeHost(); + + // roll back to first block to approach lidar ts as near as possible. + pkt_ts = ts * 1e-6 - this->getPacketDuration(); + + if (this->write_pkt_ts_) + { + createTimeYMD (ts, (RSTimestampYMD*)&pkt.header.timestamp); + } + } + + T_BlockIterator iter(pkt, this->const_param_.BLOCKS_PER_PKT, this->mech_const_param_.BLOCK_DURATION, + this->block_az_diff_, this->fov_blind_ts_diff_); + + for (uint16_t blk = 0; blk < this->const_param_.BLOCKS_PER_PKT; blk++) + { + const RS16MsopBlock& block = pkt.blocks[blk]; + + if (memcmp(this->const_param_.BLOCK_ID, block.id, 2) != 0) + { + this->cb_excep_(Error(ERRCODE_WRONGMSOPBLKID)); + break; + } + + int32_t block_az_diff; + double block_ts_off; + iter.get(blk, block_az_diff, block_ts_off); + + double block_ts = pkt_ts + block_ts_off; + int32_t block_az = ntohs(block.azimuth); + if (this->split_strategy_->newBlock(block_az)) + { + this->cb_split_frame_(this->const_param_.LASER_NUM, this->cloudTs()); + this->first_point_ts_ = block_ts; + ret = true; + } + + for (uint16_t chan = 0; chan < this->const_param_.CHANNELS_PER_BLOCK; chan++) + { + const RSChannel& channel = block.channels[chan]; + + double chan_ts = block_ts + this->mech_const_param_.CHAN_TSS[chan]; + int32_t angle_horiz = block_az + + (int32_t)((float)block_az_diff * this->mech_const_param_.CHAN_AZIS[chan]); + + uint16_t laser = chan % 16; + int32_t angle_vert = this->chan_angles_.vertAdjust(laser); + int32_t angle_horiz_final = this->chan_angles_.horizAdjust(laser, angle_horiz); + float distance = ntohs(channel.distance) * this->const_param_.DISTANCE_RES; + + if (this->distance_section_.in(distance) && this->scan_section_.in(angle_horiz_final)) + { + float x = distance * COS(angle_vert) * COS(angle_horiz_final) + this->mech_const_param_.RX * COS(angle_horiz); + float y = -distance * COS(angle_vert) * SIN(angle_horiz_final) - this->mech_const_param_.RX * SIN(angle_horiz); + float z = distance * SIN(angle_vert) + this->mech_const_param_.RZ; + this->transformPoint(x, y, z); + + typename T_PointCloud::PointT point; + setX(point, x); + setY(point, y); + setZ(point, z); + setIntensity(point, channel.intensity); + setTimestamp(point, chan_ts); + setRing(point, (this->chan_angles_.toUserChan(laser))); + + this->point_cloud_->points.emplace_back(point); + } + else if (!this->param_.dense_points) + { + typename T_PointCloud::PointT point; + setX(point, NAN); + setY(point, NAN); + setZ(point, NAN); + setIntensity(point, 0); + setTimestamp(point, chan_ts); + setRing(point, (this->chan_angles_.toUserChan(laser))); + + this->point_cloud_->points.emplace_back(point); + } + + this->prev_point_ts_ = chan_ts; + } + } + + this->prev_pkt_ts_ = pkt_ts; + return ret; +} +template +inline bool DecoderRS16::isNewFrame(const uint8_t* packet) +{ + const RS16MsopPkt& pkt = *(const RS16MsopPkt*)(packet); + + for (uint16_t blk = 0; blk < this->const_param_.BLOCKS_PER_PKT; blk++) + { + const RS16MsopBlock& block = pkt.blocks[blk]; + + if (memcmp(this->const_param_.BLOCK_ID, block.id, 2) != 0) + { + break; + } + int32_t block_az = ntohs(block.azimuth); + if (this->pre_split_strategy_->newBlock(block_az)) + { + return true; + } + + } + return false; +} +} // namespace lidar +} // namespace robosense diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RS32.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RS32.hpp new file mode 100644 index 0000000..5c3a471 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RS32.hpp @@ -0,0 +1,355 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once +#include + +namespace robosense +{ +namespace lidar +{ + +#pragma pack(push, 1) +typedef struct +{ + uint8_t id[2]; + uint16_t azimuth; + RSChannel channels[32]; +} RS32MsopBlock; + +typedef struct +{ + RSMsopHeaderV1 header; + RS32MsopBlock blocks[12]; + unsigned int index; + uint16_t tail; +} RS32MsopPkt; + +typedef struct +{ + uint8_t id[8]; + uint16_t rpm; + RSEthNetV1 eth; + RSFOV fov; + uint8_t reserved0[2]; + uint16_t phase_lock_angle; + RSVersionV1 version; + uint8_t reserved1[242]; + RSSN sn; + uint16_t zero_cali; + uint8_t return_mode; + uint16_t sw_ver; + RSTimestampYMD timestamp; + RSStatusV1 status; + uint8_t reserved2[5]; + RSDiagnoV1 diagno; + uint8_t gprmc[86]; + RSCalibrationAngle vert_angle_cali[32]; + RSCalibrationAngle horiz_angle_cali[32]; + uint8_t reserved3[586]; + uint16_t tail; +} RS32DifopPkt; + +#pragma pack(pop) + +inline void RS32DifopPkt2Adapter (const RS32DifopPkt& src, AdapterDifopPkt& dst) +{ + dst.rpm = src.rpm; + dst.fov = src.fov; + dst.return_mode = src.return_mode; + dst.sn = src.sn; + dst.eth = src.eth; + dst.version = src.version; + dst.status = src.status; + + for (uint16_t i = 0; i < 32; i++) + { + uint16_t v; + + // vert angles + dst.vert_angle_cali[i].sign = src.vert_angle_cali[i].sign; + + v = ntohs(src.vert_angle_cali[i].value); + v = (uint16_t)std::round(v * 0.1f); + dst.vert_angle_cali[i].value = htons(v); + + // horiz_angles + dst.horiz_angle_cali[i].sign = src.horiz_angle_cali[i].sign; + + v = ntohs(src.horiz_angle_cali[i].value); + v = (uint16_t)std::round(v * 0.1f); + dst.horiz_angle_cali[i].value = htons(v); + } +} + +template +class DecoderRS32 : public DecoderMech +{ +public: + + virtual void decodeDifopPkt(const uint8_t* pkt, size_t size); + virtual bool decodeMsopPkt(const uint8_t* pkt, size_t size); + virtual ~DecoderRS32() = default; + + explicit DecoderRS32(const RSDecoderParam& param); + virtual bool isNewFrame(const uint8_t* packet) override; +#ifndef UNIT_TEST +protected: +#endif + + static RSDecoderMechConstParam& getConstParam(); + static RSEchoMode getEchoMode(uint8_t mode); + + template + bool internDecodeMsopPkt(const uint8_t* pkt, size_t size); +}; + +template +inline RSDecoderMechConstParam& DecoderRS32::getConstParam() +{ + static RSDecoderMechConstParam param = + { + { + 1248 // msop len + , 1248 // difop len + , 8 // msop id len + , 8 // difop id len + , {0x55, 0xAA, 0x05, 0x0A, 0x5A, 0xA5, 0x50, 0xA0} // msop id + , {0xA5, 0xFF, 0x00, 0x5A, 0x11, 0x11, 0x55, 0x55} // difop id + , {0xFF, 0xEE} // block id + , 32 // laser number + , 12 // blocks per packet + , 32 // channels per block + , 0.4f // distance min + , 230.0f // distance max + , 0.005f // distance resolution + , 0.0625f // temperature resolution + } + // lens center + , 0.03997f // RX + , -0.01087f // RY + , 0.0f // RZ + }; + + INIT_ONLY_ONCE(); + + float blk_ts = 55.52f; + float firing_tss[] = + { + 0.00f, 2.88f, 5.76f, 8.64f, 11.52f, 14.40f, 17.28f, 20.16f, + 23.04f, 25.92f, 28.80f, 31.68f, 34.56f, 37.44f, 40.32f, 44.64f, + 1.44f, 4.32f, 7.20f, 10.08f, 12.96f, 15.84f, 18.72f, 21.60f, + 24.48f, 27.36f, 30.24f, 33.12f, 36.00f, 38.88f, 41.76f, 46.08f + }; + + param.BLOCK_DURATION = blk_ts / 1000000; + for (uint16_t i = 0; i < sizeof(firing_tss)/sizeof(firing_tss[0]); i++) + { + param.CHAN_TSS[i] = (double)firing_tss[i] / 1000000; + param.CHAN_AZIS[i] = firing_tss[i] / blk_ts; + } + + return param; +} + +template +inline RSEchoMode DecoderRS32::getEchoMode(uint8_t mode) +{ + switch (mode) + { + case 0x00: // dual return + return RSEchoMode::ECHO_DUAL; + case 0x01: // strongest return + case 0x02: // last return + default: + return RSEchoMode::ECHO_SINGLE; + } +} + +template +inline DecoderRS32::DecoderRS32(const RSDecoderParam& param) + : DecoderMech(getConstParam(), param) +{ +} + +template +inline void DecoderRS32::decodeDifopPkt(const uint8_t* packet, size_t size) +{ + const RS32DifopPkt& orig = *(const RS32DifopPkt*)packet; + AdapterDifopPkt adapter; + RS32DifopPkt2Adapter (orig, adapter); + + this->template decodeDifopCommon(adapter); + + this->echo_mode_ = getEchoMode (adapter.return_mode); + this->split_blks_per_frame_ = (this->echo_mode_ == RSEchoMode::ECHO_DUAL) ? + (this->blks_per_frame_ << 1) : this->blks_per_frame_; +} + +template +inline bool DecoderRS32::decodeMsopPkt(const uint8_t* pkt, size_t size) +{ + if (this->echo_mode_ == RSEchoMode::ECHO_SINGLE) + { + return internDecodeMsopPkt>(pkt, size); + } + else + { + return internDecodeMsopPkt>(pkt, size); + } +} + +template +template +inline bool DecoderRS32::internDecodeMsopPkt(const uint8_t* packet, size_t size) +{ + const RS32MsopPkt& pkt = *(const RS32MsopPkt*)(packet); + bool ret = false; + + this->temperature_ = parseTempInLe(&(pkt.header.temp)) * this->const_param_.TEMPERATURE_RES; + this->is_get_temperature_ = true; + double pkt_ts = 0; + if (this->param_.use_lidar_clock) + { + pkt_ts = parseTimeYMD(&pkt.header.timestamp) * 1e-6; + } + else + { + uint64_t ts = getTimeHost(); + + // roll back to first block to approach lidar ts as near as possible. + pkt_ts = ts * 1e-6 - this->getPacketDuration(); + + if (this->write_pkt_ts_) + { + createTimeYMD (ts, (RSTimestampYMD*)&pkt.header.timestamp); + } + } + + T_BlockIterator iter(pkt, this->const_param_.BLOCKS_PER_PKT, this->mech_const_param_.BLOCK_DURATION, + this->block_az_diff_, this->fov_blind_ts_diff_); + + for (uint16_t blk = 0; blk < this->const_param_.BLOCKS_PER_PKT; blk++) + { + const RS32MsopBlock& block = pkt.blocks[blk]; + + if (memcmp(this->const_param_.BLOCK_ID, block.id, 2) != 0) + { + this->cb_excep_(Error(ERRCODE_WRONGMSOPID)); + break; + } + + int32_t block_az_diff; + double block_ts_off; + iter.get(blk, block_az_diff, block_ts_off); + + double block_ts = pkt_ts + block_ts_off; + int32_t block_az = ntohs(block.azimuth); + if (this->split_strategy_->newBlock(block_az)) + { + this->cb_split_frame_(this->const_param_.LASER_NUM, this->cloudTs()); + this->first_point_ts_ = block_ts; + ret = true; + } + + for (uint16_t chan = 0; chan < this->const_param_.CHANNELS_PER_BLOCK; chan++) + { + const RSChannel& channel = block.channels[chan]; + + double chan_ts = block_ts + this->mech_const_param_.CHAN_TSS[chan]; + int32_t angle_horiz = block_az + + (int32_t)((float)block_az_diff * this->mech_const_param_.CHAN_AZIS[chan]); + + int32_t angle_vert = this->chan_angles_.vertAdjust(chan); + int32_t angle_horiz_final = this->chan_angles_.horizAdjust(chan, angle_horiz); + float distance = ntohs(channel.distance) * this->const_param_.DISTANCE_RES; + + if (this->distance_section_.in(distance) && this->scan_section_.in(angle_horiz_final)) + { + float x = distance * COS(angle_vert) * COS(angle_horiz_final) + this->mech_const_param_.RX * COS(angle_horiz); + float y = -distance * COS(angle_vert) * SIN(angle_horiz_final) - this->mech_const_param_.RX * SIN(angle_horiz); + float z = distance * SIN(angle_vert) + this->mech_const_param_.RZ; + this->transformPoint(x, y, z); + + typename T_PointCloud::PointT point; + setX(point, x); + setY(point, y); + setZ(point, z); + setIntensity(point, channel.intensity); + setTimestamp(point, chan_ts); + setRing(point, this->chan_angles_.toUserChan(chan)); + + this->point_cloud_->points.emplace_back(point); + } + else if (!this->param_.dense_points) + { + typename T_PointCloud::PointT point; + setX(point, NAN); + setY(point, NAN); + setZ(point, NAN); + setIntensity(point, 0); + setTimestamp(point, chan_ts); + setRing(point, this->chan_angles_.toUserChan(chan)); + + this->point_cloud_->points.emplace_back(point); + } + + this->prev_point_ts_ = chan_ts; + } + } + + this->prev_pkt_ts_ = pkt_ts; + return ret; +} +template +inline bool DecoderRS32::isNewFrame(const uint8_t* packet) +{ + const RS32MsopPkt& pkt = *(const RS32MsopPkt*)(packet); + for (uint16_t blk = 0; blk < this->const_param_.BLOCKS_PER_PKT; blk++) + { + const RS32MsopBlock& block = pkt.blocks[blk]; + + if (memcmp(this->const_param_.BLOCK_ID, block.id, 2) != 0) + { + break; + } + + int32_t block_az = ntohs(block.azimuth); + if (this->pre_split_strategy_->newBlock(block_az)) + { + return true; + } + } + return false; +} +} // namespace lidar +} // namespace robosense diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RS48.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RS48.hpp new file mode 100644 index 0000000..c1afb9a --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RS48.hpp @@ -0,0 +1,295 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once + +#include + +namespace robosense +{ +namespace lidar +{ +template +class DecoderRS48 : public DecoderMech +{ +public: + virtual void decodeDifopPkt(const uint8_t* pkt, size_t size); + virtual bool decodeMsopPkt(const uint8_t* pkt, size_t size); + + explicit DecoderRS48(const RSDecoderParam& param); + virtual ~DecoderRS48() = default; + + virtual bool isNewFrame(const uint8_t* packet) override; + +#ifndef UNIT_TEST +protected: +#endif + + static RSDecoderMechConstParam& getConstParam(); + static RSEchoMode getEchoMode(uint8_t mode); + + template + bool internDecodeMsopPkt(const uint8_t* pkt, size_t size); +}; + +template +inline RSDecoderMechConstParam& DecoderRS48::getConstParam() +{ + static RSDecoderMechConstParam param = { + { + 1268 // msop len + , + 1248 // difop len + , + 4 // msop id len + , + 8 // difop id len + , + { 0x55, 0xAA, 0x05, 0x5A } // msop id + , + { 0xA5, 0xFF, 0x00, 0x5A, 0x11, 0x11, 0x55, 0x55 } // difop id + , + { 0xFE } // block id + , + 48 // laser number + , + 8 // blocks per packet + , + 48 // channels per block + , + 0.4f // distance min + , + 250.0f // distance max + , + 0.005f // distance resolution + , + 0.0625f // temperature resolution + } // lens center + , + 0.03615f // RX + , + -0.017f // RY + , + 0.0f // RZ + }; + + INIT_ONLY_ONCE(); + + float blk_ts = 55.552f; + float firing_tss[48] = { + 0.0f, 3.236f, 6.472f, 9.708f, 12.944f, 12.944f, 16.18f, 16.18f, 19.416f, 19.416f, 22.652f, 22.652f, + 25.888f, 29.124f, 32.36f, 35.596f, 38.832f, 38.832f, 42.068f, 42.068f, 45.304f, 45.304f, 48.54f, 48.54f, + + 0.0f, 3.236f, 6.472f, 9.708f, 12.944f, 12.944f, 16.18f, 16.18f, 19.416f, 19.416f, 22.652f, 22.652f, + 25.888f, 29.124f, 32.36f, 35.596f, 38.832f, 38.832f, 42.068f, 42.068f, 45.304f, 45.304f, 48.54f, 48.54f, + }; + + param.BLOCK_DURATION = blk_ts / 1000000; + for (uint16_t i = 0; i < sizeof(firing_tss) / sizeof(firing_tss[0]); i++) + { + param.CHAN_TSS[i] = (double)firing_tss[i] / 1000000; + param.CHAN_AZIS[i] = firing_tss[i] / blk_ts; + } + + return param; +} + +template +inline RSEchoMode DecoderRS48::getEchoMode(uint8_t mode) +{ + switch (mode) + { + case 0x03: // dual return + return RSEchoMode::ECHO_DUAL; + case 0x01: // strongest return + case 0x02: // last return + default: + return RSEchoMode::ECHO_SINGLE; + } +} + +template +inline void DecoderRS48::decodeDifopPkt(const uint8_t* packet, size_t size) +{ + const RSP48DifopPkt& pkt = *(const RSP48DifopPkt*)(packet); + this->template decodeDifopCommon(pkt); + + this->echo_mode_ = getEchoMode(pkt.return_mode); + this->split_blks_per_frame_ = + (this->echo_mode_ == RSEchoMode::ECHO_DUAL) ? (this->blks_per_frame_ << 1) : this->blks_per_frame_; +} + +template +inline DecoderRS48::DecoderRS48(const RSDecoderParam& param) + : DecoderMech(getConstParam(), param) +{ +} + +template +inline bool DecoderRS48::decodeMsopPkt(const uint8_t* pkt, size_t size) +{ + if (this->echo_mode_ == RSEchoMode::ECHO_SINGLE) + { + return internDecodeMsopPkt>(pkt, size); + } + else + { + return internDecodeMsopPkt>(pkt, size); + } +} + +template +template +inline bool DecoderRS48::internDecodeMsopPkt(const uint8_t* packet, size_t size) +{ + const RSP48MsopPkt& pkt = *(const RSP48MsopPkt*)(packet); + bool ret = false; + + this->temperature_ = parseTempInBe(&(pkt.header.temp)) * this->const_param_.TEMPERATURE_RES; + this->is_get_temperature_ = true; + double pkt_ts = 0; + if (this->param_.use_lidar_clock) + { + pkt_ts = parseTimeUTCWithUs(&pkt.header.timestamp) * 1e-6; + } + else + { + uint64_t ts = getTimeHost(); + + // roll back to first block to approach lidar ts as near as possible. + pkt_ts = ts * 1e-6 - this->getPacketDuration(); + + if (this->write_pkt_ts_) + { + createTimeUTCWithUs(ts, (RSTimestampUTC*)&pkt.header.timestamp); + } + } + + T_BlockIterator iter(pkt, this->const_param_.BLOCKS_PER_PKT, this->mech_const_param_.BLOCK_DURATION, + this->block_az_diff_, this->fov_blind_ts_diff_); + + for (uint16_t blk = 0; blk < this->const_param_.BLOCKS_PER_PKT; blk++) + { + const RSP48MsopBlock& block = pkt.blocks[blk]; + + if (memcmp(this->const_param_.BLOCK_ID, block.id, 1) != 0) + { + this->cb_excep_(Error(ERRCODE_WRONGMSOPBLKID)); + break; + } + + int32_t block_az_diff; + double block_ts_off; + iter.get(blk, block_az_diff, block_ts_off); + + double block_ts = pkt_ts + block_ts_off; + int32_t block_az = ntohs(block.azimuth); + if (this->split_strategy_->newBlock(block_az)) + { + this->cb_split_frame_(this->const_param_.LASER_NUM, this->cloudTs()); + this->first_point_ts_ = block_ts; + ret = true; + } + + for (uint16_t chan = 0; chan < this->const_param_.CHANNELS_PER_BLOCK; chan++) + { + const RSChannel& channel = block.channels[chan]; + + double chan_ts = block_ts + this->mech_const_param_.CHAN_TSS[chan]; + int32_t angle_horiz = block_az + (int32_t)((float)block_az_diff * this->mech_const_param_.CHAN_AZIS[chan]); + + int32_t angle_vert = this->chan_angles_.vertAdjust(chan); + int32_t angle_horiz_final = this->chan_angles_.horizAdjust(chan, angle_horiz); + float distance = ntohs(channel.distance) * this->const_param_.DISTANCE_RES; + + if (this->distance_section_.in(distance) && this->scan_section_.in(angle_horiz_final)) + { + float x = distance * COS(angle_vert) * COS(angle_horiz_final) + this->mech_const_param_.RX * COS(angle_horiz); + float y = -distance * COS(angle_vert) * SIN(angle_horiz_final) - this->mech_const_param_.RX * SIN(angle_horiz); + float z = distance * SIN(angle_vert) + this->mech_const_param_.RZ; + this->transformPoint(x, y, z); + + typename T_PointCloud::PointT point; + setX(point, x); + setY(point, y); + setZ(point, z); + setIntensity(point, channel.intensity); + setTimestamp(point, chan_ts); + setRing(point, this->chan_angles_.toUserChan(chan)); + + this->point_cloud_->points.emplace_back(point); + } + else if (!this->param_.dense_points) + { + typename T_PointCloud::PointT point; + setX(point, NAN); + setY(point, NAN); + setZ(point, NAN); + setIntensity(point, 0); + setTimestamp(point, chan_ts); + setRing(point, this->chan_angles_.toUserChan(chan)); + + this->point_cloud_->points.emplace_back(point); + } + + this->prev_point_ts_ = chan_ts; + } + } + + this->prev_pkt_ts_ = pkt_ts; + return ret; +} + +template +inline bool DecoderRS48::isNewFrame(const uint8_t* packet) +{ + const RSP48MsopPkt& pkt = *(const RSP48MsopPkt*)(packet); + + for (uint16_t blk = 0; blk < this->const_param_.BLOCKS_PER_PKT; blk++) + { + const RSP48MsopBlock& block = pkt.blocks[blk]; + if (memcmp(this->const_param_.BLOCK_ID, block.id, 1) != 0) + { + break; + } + + int32_t block_az = ntohs(block.azimuth); + if (this->pre_split_strategy_->newBlock(block_az)) + { + return true; + } + } + return false; +} + +} // namespace lidar +} // namespace robosense diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RS80.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RS80.hpp new file mode 100644 index 0000000..22b53a2 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RS80.hpp @@ -0,0 +1,331 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once +#include + +namespace robosense +{ +namespace lidar +{ + +#pragma pack(push, 1) +typedef struct +{ + uint8_t id[1]; + uint8_t ret_id; + uint16_t azimuth; + RSChannel channels[80]; +} RS80MsopBlock; + +typedef struct +{ + RSMsopHeaderV2 header; + RS80MsopBlock blocks[4]; + uint8_t reserved[192]; +} RS80MsopPkt; + +typedef struct +{ + uint8_t id[8]; + uint16_t rpm; + RSEthNetV2 eth; + RSFOV fov; + uint8_t reserved1[2]; + uint16_t phase_lock_angle; + RSVersionV2 version; + uint8_t reserved2[229]; + RSSN sn; + uint16_t zero_cali; + uint8_t return_mode; + RSTimeInfo time_info; + RSStatusV1 status; + uint8_t reserved3[5]; + RSDiagnoV1 diagno; + uint8_t gprmc[86]; + RSCalibrationAngle vert_angle_cali[128]; + RSCalibrationAngle horiz_angle_cali[128]; + uint8_t reserved4[10]; + uint16_t tail; +} RS80DifopPkt; + +#pragma pack(pop) + +template +class DecoderRS80 : public DecoderMech +{ +public: + virtual void decodeDifopPkt(const uint8_t* pkt, size_t size); + virtual bool decodeMsopPkt(const uint8_t* pkt, size_t size); + virtual ~DecoderRS80() = default; + + explicit DecoderRS80(const RSDecoderParam& param); + virtual bool isNewFrame(const uint8_t* packet) override; +#ifndef UNIT_TEST +protected: +#endif + + static RSDecoderMechConstParam& getConstParam(); + static RSEchoMode getEchoMode(uint8_t mode); + + template + bool internDecodeMsopPkt(const uint8_t* pkt, size_t size); +}; + +template +inline RSDecoderMechConstParam& DecoderRS80::getConstParam() +{ + static RSDecoderMechConstParam param = + { + { + 1248 // msop len + , 1248 // difop len + , 4 // msop id len + , 8 // difop id len + , {0x55, 0xAA, 0x05, 0x5A} // msop id + , {0xA5, 0xFF, 0x00, 0x5A, 0x11, 0x11, 0x55, 0x55} // difop id + , {0xFE} // block id + , 80 // laser number + , 4 // blocks per packet + , 80 // channels per block + , 0.4f // distance min + , 250.0f // distance max + , 0.005f // distance resolution + , 0.0625f // temperature resolution + } + // lens center + , 0.03615f // RX + , -0.017f // RY + , 0.0f // RZ + }; + + INIT_ONLY_ONCE(); + + float blk_ts = 55.552f; + float firing_tss[] = + { + 0.00f, 0.00f, 0.00f, 3.236f, 3.236f, 6.472f, 6.472f, 6.472f, + 6.472f, 9.708f, 9.708f, 9.708f, 12.944f, 12.944f, 12.944f, 16.18f, + 16.18f, 16.18f, 19.416f, 19.416f, 19.416f, 22.652f, 22.652f, 25.888f, + 25.888f, 29.124f, 29.124f, 32.36f, 32.36f, 35.596f, 35.596f, 38.832f, + 38.832f, 42.068f, 42.068f, 45.304f, 45.304f, 48.54f, 48.54f, 48.54f, + + 0.00f, 0.00f, 0.00f, 3.236f, 3.236f, 3.236f, 6.472f, 6.472f, + 6.472f, 9.708f, 9.708f, 12.944f, 12.944f, 12.944f, 12.944f, 16.18f, + 16.18f, 19.416f, 19.416f, 19.416f, 22.652f, 22.652f, 22.652f, 25.888f, + 25.888f, 29.124f, 29.124f, 32.36f, 32.36f, 35.596f, 35.596f, 35.596f, + 38.832f, 38.832f, 42.068f, 45.304f, 45.304f, 48.54f, 48.54f, 48.54f, + }; + + param.BLOCK_DURATION = blk_ts / 1000000; + for (uint16_t i = 0; i < sizeof(firing_tss)/sizeof(firing_tss[0]); i++) + { + param.CHAN_TSS[i] = (double)firing_tss[i] / 1000000; + param.CHAN_AZIS[i] = firing_tss[i] / blk_ts; + } + + return param; +} + +template +inline RSEchoMode DecoderRS80::getEchoMode(uint8_t mode) +{ + switch (mode) + { + case 0x03: // dual return + return RSEchoMode::ECHO_DUAL; + case 0x01: // strongest return + case 0x02: // last return + default: + return RSEchoMode::ECHO_SINGLE; + } +} + +template +inline DecoderRS80::DecoderRS80(const RSDecoderParam& param) + : DecoderMech(getConstParam(), param) +{ +} + +template +inline void DecoderRS80::decodeDifopPkt(const uint8_t* packet, size_t size) +{ + const RS80DifopPkt& pkt = *(const RS80DifopPkt*)(packet); + this->template decodeDifopCommon(pkt); + + this->echo_mode_ = getEchoMode (pkt.return_mode); + this->split_blks_per_frame_ = (this->echo_mode_ == RSEchoMode::ECHO_DUAL) ? + (this->blks_per_frame_ << 1) : this->blks_per_frame_; +} + +template +inline bool DecoderRS80::decodeMsopPkt(const uint8_t* pkt, size_t size) +{ + if (this->echo_mode_ == RSEchoMode::ECHO_SINGLE) + { + return internDecodeMsopPkt>(pkt, size); + } + else + { + return internDecodeMsopPkt>(pkt, size); + } +} + +template +template +inline bool DecoderRS80::internDecodeMsopPkt(const uint8_t* packet, size_t size) +{ + const RS80MsopPkt& pkt = *(const RS80MsopPkt*)(packet); + bool ret = false; + + this->temperature_ = parseTempInBe(&(pkt.header.temp)) * this->const_param_.TEMPERATURE_RES; + this->is_get_temperature_ = true; + double pkt_ts = 0; + if (this->param_.use_lidar_clock) + { + pkt_ts = parseTimeUTCWithUs(&pkt.header.timestamp) * 1e-6; + } + else + { + uint64_t ts = getTimeHost(); + + // roll back to first block to approach lidar ts as near as possible. + pkt_ts = ts * 1e-6 - this->getPacketDuration(); + + if (this->write_pkt_ts_) + { + createTimeUTCWithUs (ts, (RSTimestampUTC*)&pkt.header.timestamp); + } + } + + T_BlockIterator iter(pkt, this->const_param_.BLOCKS_PER_PKT, this->mech_const_param_.BLOCK_DURATION, + this->block_az_diff_, this->fov_blind_ts_diff_); + + for (uint16_t blk = 0; blk < this->const_param_.BLOCKS_PER_PKT; blk++) + { + const RS80MsopBlock& block = pkt.blocks[blk]; + + if (memcmp(this->const_param_.BLOCK_ID, block.id, 1) != 0) + { + // + // Disable error report temporarily + // + //this->cb_excep_(Error(ERRCODE_WRONGMSOPBLKID)); + + break; + } + + int32_t block_az_diff; + double block_ts_off; + iter.get(blk, block_az_diff, block_ts_off); + + double block_ts = pkt_ts + block_ts_off; + int32_t block_az = ntohs(block.azimuth); + if (this->split_strategy_->newBlock(block_az)) + { + this->cb_split_frame_(this->const_param_.LASER_NUM, this->cloudTs()); + this->first_point_ts_ = block_ts; + ret = true; + } + + for (uint16_t chan = 0; chan < this->const_param_.CHANNELS_PER_BLOCK; chan++) + { + const RSChannel& channel = block.channels[chan]; + + double chan_ts = block_ts + this->mech_const_param_.CHAN_TSS[chan]; + int32_t angle_horiz = block_az + + (int32_t)((float)block_az_diff * this->mech_const_param_.CHAN_AZIS[chan]); + + int32_t angle_vert = this->chan_angles_.vertAdjust(chan); + int32_t angle_horiz_final = this->chan_angles_.horizAdjust(chan, angle_horiz); + float distance = ntohs(channel.distance) * this->const_param_.DISTANCE_RES; + + if (this->distance_section_.in(distance) && this->scan_section_.in(angle_horiz_final)) + { + float x = distance * COS(angle_vert) * COS(angle_horiz_final) + this->mech_const_param_.RX * COS(angle_horiz); + float y = -distance * COS(angle_vert) * SIN(angle_horiz_final) - this->mech_const_param_.RX * SIN(angle_horiz); + float z = distance * SIN(angle_vert) + this->mech_const_param_.RZ; + this->transformPoint(x, y, z); + + typename T_PointCloud::PointT point; + setX(point, x); + setY(point, y); + setZ(point, z); + setIntensity(point, channel.intensity); + setTimestamp(point, chan_ts); + setRing(point, this->chan_angles_.toUserChan(chan)); + + this->point_cloud_->points.emplace_back(point); + } + else if (!this->param_.dense_points) + { + typename T_PointCloud::PointT point; + setX(point, NAN); + setY(point, NAN); + setZ(point, NAN); + setIntensity(point, 0); + setTimestamp(point, chan_ts); + setRing(point, this->chan_angles_.toUserChan(chan)); + + this->point_cloud_->points.emplace_back(point); + } + + this->prev_point_ts_ = chan_ts; + } + } + + this->prev_pkt_ts_ = pkt_ts; + return ret; +} +template +inline bool DecoderRS80::isNewFrame(const uint8_t* packet) +{ + const RS80MsopPkt& pkt = *(const RS80MsopPkt*)(packet); + for (uint16_t blk = 0; blk < this->const_param_.BLOCKS_PER_PKT; blk++) + { + const RS80MsopBlock& block = pkt.blocks[blk]; + if (memcmp(this->const_param_.BLOCK_ID, block.id, 1) != 0) + { + break; + } + + int32_t block_az = ntohs(block.azimuth); + if (this->pre_split_strategy_->newBlock(block_az)) + { + return true; + } + } + + return false; +} +} // namespace lidar +} // namespace robosense diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSAIRY.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSAIRY.hpp new file mode 100644 index 0000000..c7b28d9 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSAIRY.hpp @@ -0,0 +1,540 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once +#include +#include + +namespace robosense +{ +namespace lidar +{ +#pragma pack(push, 1) + +typedef struct +{ + uint8_t id[4]; + uint8_t reserved_0[4]; + uint8_t pkt_cnt_top2bot[4]; + uint8_t pkt_cnt_bot2top[4]; + uint8_t data_type[2]; + uint8_t reserved_1[2]; + RSTimestampUTC timestamp; + uint8_t reserved_2; + uint8_t lidar_type; + uint8_t lidar_mode; + uint8_t reserved_3[5]; + RSTemperature temp; + RSTemperature topboard_temp; +} RSAIRYHeader; + +typedef struct +{ + uint16_t distance; + uint8_t intensity; +} RSAIRYChannel; +typedef struct +{ + uint8_t id[2]; + uint16_t azimuth; + RSAIRYChannel channels[48]; +} RSAIRYMsopBlock; + +typedef struct +{ + RSAIRYHeader header; + RSAIRYMsopBlock blocks[8]; + uint8_t tail[6]; + uint8_t reserved_4[16]; +} RSAIRYMsopPkt; + +typedef struct +{ + uint16_t vol_main; + uint16_t vol_12v; + uint16_t vol_mcu; + uint16_t vol_1v; + uint16_t current_main; + uint8_t reserved[16]; +} RSAIRYStatus; + +typedef struct +{ + uint8_t id[8]; + uint16_t rpm; + RSEthNetV2 eth; + RSFOV fov; + uint8_t reserved_1[4]; + RSVersionV1 version; + uint8_t reserved_2[242]; + RSSN sn; + uint16_t zero_cal; + uint8_t return_mode; + uint8_t reserved_3[167]; + RSCalibrationAngle vert_angle_cali[96]; + RSCalibrationAngle horiz_angle_cali[96]; + uint8_t reserved_4[22]; + RSAIRYStatus status; + uint32_t qx; + uint32_t qy; + uint32_t qz; + uint32_t qw; + uint32_t x; + uint32_t y; + uint32_t z; + uint8_t reserved_5[126]; + uint16_t tail; +} RSAIRYDifopPkt; + +typedef struct +{ + uint8_t id[4]; + RSTimestampUTC timestamp; + uint8_t acceIx[4]; + uint8_t acceIy[4]; + uint8_t acceIz[4]; + uint8_t gyrox[4]; + uint8_t gyroy[4]; + uint8_t gyroz[4]; + int32_t temperature; + uint8_t odr; // output data rate + uint8_t acceIFsr; // accel full scale range + // 0: +/- 2g + // 1: +/- 4g + // 2: +/- 8g + // 3: +/- 16g + + uint8_t gyroIFsr; // gyro full scale range + // 0: +/- 250 dps + // 1: +/- 500 dps + // 2: +/- 1000 dps + // 3: +/- 2000 dps + uint32_t cnt; + uint16_t tail; +} RSAIRYImuPkt; +#pragma pack(pop) + +enum RSAIRYLidarModel +{ + RSAIRY_CHANNEL_48 = 0, + RSAIRY_CHANNEL_96 = 1, + RSAIRY_CHANNEL_192 = 2, +}; +template +class DecoderRSAIRY : public DecoderMech +{ +public: + virtual void decodeDifopPkt(const uint8_t* pkt, size_t size); + virtual bool decodeMsopPkt(const uint8_t* pkt, size_t size); + void decodeImuPkt(const uint8_t* pkt, size_t size) override; + virtual ~DecoderRSAIRY() = default; + + explicit DecoderRSAIRY(const RSDecoderParam& param); + virtual bool isNewFrame(const uint8_t* packet) override; +#ifndef UNIT_TEST +protected: +#endif + + static RSDecoderMechConstParam& getConstParam(); + static RSEchoMode getEchoMode(uint8_t mode); + RSAIRYLidarModel getLidarModel(uint8_t mode); + template + bool internDecodeMsopPkt(const uint8_t* pkt, size_t size); + + RSAIRYLidarModel lidarModel_{ RSAIRY_CHANNEL_96 }; + uint16_t u16ChannelNum_{ 96 }; + bool bInit_{ false }; +}; + +template +inline RSDecoderMechConstParam& DecoderRSAIRY::getConstParam() +{ + static RSDecoderMechConstParam param = { + { + 1248 // msop len + , + 1248 // difop len + , + 4 // msop id len + , + 8 // difop id len + , + { 0x55, 0xAA, 0x05, 0x5A } // msop id + , + { 0xA5, 0xFF, 0x00, 0x5A, 0x11, 0x11, 0x55, 0x55 } // difop id + , + { 0xFF, 0xEE } // block id + , + 96 // laser number + , + 8 // blocks per packet + , + 48 // channels per block + , + 0.1f // distance min + , + 60.0f // distance max + , + 0.005f // distance resolution + , + 0.0625f // temperature resolution + , + 51 // imu len + , + 4 // imu id len + , + { 0xAA, 0x55, 0x5A, 0x05 } // imu id + } // lens center + , + 0.0075f // RX + , + 0.00664f // RY + , + 0.04532f // RZ + }; + + INIT_ONLY_ONCE(); + + float blk_ts = 111.080f; + float firing_tss[] = { + 0.00f, 0.00f, 0.00f, 0.00f, 0.00f, 0.00f, 0.00f, 0.00f, 7.616f, 7.616f, 7.616f, 7.616f, + 7.616f, 7.616f, 7.616f, 7.616f, 16.184f, 16.184f, 16.184f, 16.184f, 16.184f, 16.184f, 16.184f, 16.184f, + 24.752f, 24.752f, 24.752f, 24.752f, 24.752f, 24.752f, 24.752f, 24.752f, 33.320f, 33.320f, 33.320f, 33.320f, + 33.320f, 33.320f, 33.320f, 33.320f, 42.840f, 42.840f, 42.840f, 42.840f, 42.840f, 42.840f, 42.840f, 42.840f, + 52.360f, 52.360f, 52.360f, 52.360f, 52.360f, 52.360f, 52.360f, 52.360f, 61.880f, 61.880f, 61.880f, 61.880f, + 61.880f, 61.880f, 61.880f, 61.880f, 71.400f, 71.400f, 71.400f, 71.400f, 71.400f, 71.400f, 71.400f, 71.400f, + 79.968f, 79.968f, 79.968f, 79.968f, 79.968f, 79.968f, 79.968f, 79.968f, 88.536f, 88.536f, 88.536f, 88.536f, + 88.536f, 88.536f, 88.536f, 88.536f, 97.104f, 97.104f, 97.104f, 97.104f, 97.104f, 97.104f, 97.104f, 97.104f, + }; + + param.BLOCK_DURATION = blk_ts / 1000000; + for (uint16_t i = 0; i < sizeof(firing_tss) / sizeof(firing_tss[0]); i++) + { + param.CHAN_TSS[i] = (double)firing_tss[i] / 1000000; + param.CHAN_AZIS[i] = firing_tss[i] / blk_ts; + } + + return param; +} + +template +inline RSEchoMode DecoderRSAIRY::getEchoMode(uint8_t mode) +{ + switch (mode) + { + case 0x03: // dual return + return RSEchoMode::ECHO_DUAL; + default: + return RSEchoMode::ECHO_SINGLE; + } +} + +template +inline RSAIRYLidarModel DecoderRSAIRY::getLidarModel(uint8_t mode) +{ + switch (mode) + { + case 0x01: + this->u16ChannelNum_ = 48; + return RSAIRYLidarModel::RSAIRY_CHANNEL_48; + case 0x02: + this->u16ChannelNum_ = 96; + return RSAIRYLidarModel::RSAIRY_CHANNEL_96; + case 0x03: + this->u16ChannelNum_ = 192; + return RSAIRYLidarModel::RSAIRY_CHANNEL_192; + default: + this->u16ChannelNum_ = 96; + return RSAIRYLidarModel::RSAIRY_CHANNEL_96; + } +} +template +inline DecoderRSAIRY::DecoderRSAIRY(const RSDecoderParam& param) + : DecoderMech(getConstParam(), param) +{ +} + +template +inline void DecoderRSAIRY::decodeImuPkt(const uint8_t* packet, size_t size) +{ + const RSAIRYImuPkt& pkt = *(const RSAIRYImuPkt*)(packet); + if (this->imuDataPtr_ && this->cb_imu_data_ && !this->imuDataPtr_->state) + { + if (this->param_.use_lidar_clock) + { + this->imuDataPtr_->timestamp = parseTimeUTCWithUs(&pkt.timestamp) * 1e-6; + } + else + { + this->imuDataPtr_->timestamp = getTimeHost() * 1e-6; + } + uint8_t acclFsr = 1 << (pkt.acceIFsr + 1); + float acceUnit = acclFsr / 32768.0; + + this->imuDataPtr_->linear_acceleration_x = u8ArrayToInt32(pkt.acceIx, 4) * acceUnit; + this->imuDataPtr_->linear_acceleration_y = u8ArrayToInt32(pkt.acceIy, 4) * acceUnit; + this->imuDataPtr_->linear_acceleration_z = u8ArrayToInt32(pkt.acceIz, 4) * acceUnit; + + uint16_t gyroFsr = 250 * (1 << pkt.gyroIFsr); + float gyroUnit = gyroFsr / 32768.0 * M_PI / 180; + this->imuDataPtr_->angular_velocity_x = u8ArrayToInt32(pkt.gyrox, 4) * gyroUnit; + this->imuDataPtr_->angular_velocity_y = u8ArrayToInt32(pkt.gyroy, 4) * gyroUnit; + this->imuDataPtr_->angular_velocity_z = u8ArrayToInt32(pkt.gyroz, 4) * gyroUnit; + + this->imuDataPtr_->state = true; + + this->cb_imu_data_(); + } +} +template +inline void DecoderRSAIRY::decodeDifopPkt(const uint8_t* packet, size_t size) +{ + const RSAIRYDifopPkt& pkt = *(const RSAIRYDifopPkt*)(packet); + this->template decodeDifopCommon(pkt); + + this->device_info_.qx = convertUint32ToFloat(ntohl(pkt.qx)); + this->device_info_.qy = convertUint32ToFloat(ntohl(pkt.qy)); + this->device_info_.qz = convertUint32ToFloat(ntohl(pkt.qz)); + this->device_info_.qw = convertUint32ToFloat(ntohl(pkt.qw)); + + this->device_info_.x = convertUint32ToFloat(ntohl(pkt.x)); + this->device_info_.y = convertUint32ToFloat(ntohl(pkt.y)); + this->device_info_.z = convertUint32ToFloat(ntohl(pkt.z)); + this->device_info_.state = true; +} + +template +inline bool DecoderRSAIRY::decodeMsopPkt(const uint8_t* pkt, size_t size) +{ + const RSAIRYMsopPkt& msopPkt = *(const RSAIRYMsopPkt*)(pkt); + if (msopPkt.header.data_type[0] != 0) + { + return false; + } + + if (!bInit_) + { + this->echo_mode_ = getEchoMode(msopPkt.header.data_type[1]); + this->split_blks_per_frame_ = + (this->echo_mode_ == RSEchoMode::ECHO_DUAL) ? (this->blks_per_frame_ << 1) : this->blks_per_frame_; + + lidarModel_ = getLidarModel(msopPkt.header.lidar_mode); + + if (lidarModel_ == RSAIRYLidarModel::RSAIRY_CHANNEL_48) + { + float blk_ts = 111.080f; + float firing_tss[] = { + 0.00f, 0.00f, 0.00f, 0.00f, 7.616f, 7.616f, 7.616f, 7.616f, 16.184f, 16.184f, 16.184f, 16.184f, + 24.752f, 24.752f, 24.752f, 24.752f, 33.320f, 33.320f, 33.320f, 33.320f, 42.840f, 42.840f, 42.840f, 42.840f, + 52.360f, 52.360f, 52.360f, 52.360f, 61.880f, 61.880f, 61.880f, 61.880f, 71.400f, 71.400f, 71.400f, 71.400f, + 79.968f, 79.968f, 79.968f, 79.968f, 88.536f, 88.536f, 88.536f, 88.536f, 97.104f, 97.104f, 97.104f, 97.104f, + }; + + this->mech_const_param_.BLOCK_DURATION = blk_ts / 1000000; + for (uint16_t i = 0; i < sizeof(firing_tss) / sizeof(firing_tss[0]); i++) + { + this->mech_const_param_.CHAN_TSS[i] = (double)firing_tss[i] / 1000000; + this->mech_const_param_.CHAN_AZIS[i] = firing_tss[i] / blk_ts; + } + } + + bInit_ = true; + } + + if (lidarModel_ == RSAIRYLidarModel::RSAIRY_CHANNEL_48) + { + if (this->echo_mode_ == RSEchoMode::ECHO_SINGLE) + { + return internDecodeMsopPkt>(pkt, size); + } + else + { + return internDecodeMsopPkt>(pkt, size); + } + } + else if (lidarModel_ == RSAIRYLidarModel::RSAIRY_CHANNEL_96) + { + if (this->echo_mode_ == RSEchoMode::ECHO_SINGLE) + { + return internDecodeMsopPkt>(pkt, size); + } + else + { + return internDecodeMsopPkt>(pkt, size); + } + } + else + { + RS_ERROR << "Unsupported lidar model:" << lidarModel_ << RS_REND; + return false; + } +} + +template +template +inline bool DecoderRSAIRY::internDecodeMsopPkt(const uint8_t* packet, size_t size) +{ + const RSAIRYMsopPkt& pkt = *(const RSAIRYMsopPkt*)(packet); + bool ret = false; + this->temperature_ = parseTempInLe(&(pkt.header.temp)) * this->const_param_.TEMPERATURE_RES; + this->is_get_temperature_ = true; + + double pkt_ts = 0; + if (this->param_.use_lidar_clock) + { + pkt_ts = parseTimeUTCWithUs((RSTimestampUTC*)&pkt.header.timestamp) * 1e-6; + } + else + { + uint64_t ts = getTimeHost(); + + // roll back to first block to approach lidar ts as near as possible. + pkt_ts = ts * 1e-6 - this->getPacketDuration(); + + if (this->write_pkt_ts_) + { + createTimeUTCWithUs(ts, (RSTimestampUTC*)&pkt.header.timestamp); + } + } + T_BlockIterator iter(pkt, this->const_param_.BLOCKS_PER_PKT, this->mech_const_param_.BLOCK_DURATION, this->block_az_diff_, + this->fov_blind_ts_diff_); + for (uint16_t blk = 0; blk < this->const_param_.BLOCKS_PER_PKT; blk++) + { + const RSAIRYMsopBlock& block = pkt.blocks[blk]; + + if (memcmp(this->const_param_.BLOCK_ID, block.id, 2) != 0) + { + this->cb_excep_(Error(ERRCODE_WRONGMSOPBLKID)); + break; + } + + int32_t block_az_diff; + double block_ts_off; + iter.get(blk, block_az_diff, block_ts_off); + + double block_ts = pkt_ts + block_ts_off; + int32_t block_az = ntohs(block.azimuth); + + if (this->split_strategy_->newBlock(block_az)) + { + this->cb_split_frame_(this->u16ChannelNum_, this->cloudTs()); + this->first_point_ts_ = block_ts; + ret = true; + } + + for (uint16_t chan = 0; chan < this->const_param_.CHANNELS_PER_BLOCK; chan++) + { + const RSAIRYChannel& channel = block.channels[chan]; + uint16_t chan_id = chan; + if (lidarModel_ == RSAIRYLidarModel::RSAIRY_CHANNEL_96 && (blk % 2) == 1) + { + chan_id = chan + 48; + } + double chan_ts = block_ts + this->mech_const_param_.CHAN_TSS[chan_id]; + int32_t angle_horiz = block_az + (int32_t)((float)block_az_diff * this->mech_const_param_.CHAN_AZIS[chan_id]); + + int32_t angle_vert = this->chan_angles_.vertAdjust(chan_id); + int32_t angle_horiz_final = this->chan_angles_.horizAdjust(chan_id, angle_horiz); + uint16_t u16RawDistance = ntohs(channel.distance); + uint16_t u16Distance = u16RawDistance & 0x3FFF; + uint8_t feature = (u16RawDistance >> 14) & 0x03; + float distance = u16Distance * this->const_param_.DISTANCE_RES; + + if (this->distance_section_.in(distance) && this->scan_section_.in(angle_horiz_final)) + { + float x = distance * COS(angle_vert) * COS(angle_horiz_final) + this->lidar_lens_center_Rxy_ * COS(angle_horiz); + float y = -distance * COS(angle_vert) * SIN(angle_horiz_final) - this->lidar_lens_center_Rxy_ * SIN(angle_horiz); + float z = distance * SIN(angle_vert) + this->mech_const_param_.RZ; + this->transformPoint(x, y, z); + typename T_PointCloud::PointT point; + setX(point, x); + setY(point, y); + setZ(point, z); + setIntensity(point, channel.intensity); + setRing(point, this->chan_angles_.toUserChan(chan_id)); + setTimestamp(point, chan_ts); + setFeature(point, feature); + this->point_cloud_->points.emplace_back(point); + } + else if (!this->param_.dense_points) + { + typename T_PointCloud::PointT point; + setX(point, NAN); + setY(point, NAN); + setZ(point, NAN); + setIntensity(point, 0); + setRing(point, this->chan_angles_.toUserChan(chan_id)); + setTimestamp(point, chan_ts); + setFeature(point, feature); + this->point_cloud_->points.emplace_back(point); + } + + this->prev_point_ts_ = chan_ts; + } + } + + this->prev_pkt_ts_ = pkt_ts; + return ret; +} + +template +inline bool DecoderRSAIRY::isNewFrame(const uint8_t* packet) +{ + const RSAIRYMsopPkt& pkt = *(const RSAIRYMsopPkt*)(packet); + + int data_type_ = (int)(pkt.header.data_type[0]); + if (data_type_ > 0) + { + return false; + } + + for (uint16_t blk = 0; blk < this->const_param_.BLOCKS_PER_PKT; blk++) + { + const RSAIRYMsopBlock& block = pkt.blocks[blk]; + + if (memcmp(this->const_param_.BLOCK_ID, block.id, 1) != 0) + { + break; + } + + int32_t block_az = ntohs(block.azimuth); + if (this->pre_split_strategy_->newBlock(block_az)) + { + return true; + } + } + + return false; +} + +} // namespace lidar +} // namespace robosense diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSBP.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSBP.hpp new file mode 100644 index 0000000..9b74b51 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSBP.hpp @@ -0,0 +1,374 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once +#include +#include + +namespace robosense +{ +namespace lidar +{ + +#pragma pack(push, 1) + +typedef struct +{ + uint8_t id[2]; + uint16_t azimuth; + RSChannel channels[32]; +} RSBPMsopBlock; + +typedef struct +{ + RSMsopHeaderV1 header; + RSBPMsopBlock blocks[12]; + unsigned int index; + uint16_t tail; +} RSBPMsopPkt; + +typedef struct +{ + uint8_t id[8]; + uint16_t rpm; + RSEthNetV1 eth; + RSFOV fov; + uint16_t reserved0; + uint16_t phase_lock_angle; + RSVersionV1 version; + uint8_t reserved_1[242]; + RSSN sn; + uint16_t zero_cali; + uint8_t return_mode; + uint16_t sw_ver; + RSTimestampYMD timestamp; + RSStatusV1 status; + uint8_t reserved_2[5]; + RSDiagnoV1 diagno; + uint8_t gprmc[86]; + RSCalibrationAngle vert_angle_cali[32]; + RSCalibrationAngle horiz_angle_cali[32]; + uint8_t reserved_3[586]; + uint16_t tail; +} RSBPDifopPkt; + +#pragma pack(pop) + +template +class DecoderRSBP : public DecoderMech +{ +public: + + virtual void decodeDifopPkt(const uint8_t* pkt, size_t size); + virtual bool decodeMsopPkt(const uint8_t* pkt, size_t size); + virtual ~DecoderRSBP() = default; + + explicit DecoderRSBP(const RSDecoderParam& param); + virtual bool isNewFrame(const uint8_t* packet) override; +#ifndef UNIT_TEST +protected: +#endif + + static RSDecoderMechConstParam& getConstParam(); + static RSEchoMode getEchoMode(uint8_t mode); + + template + bool internDecodeMsopPkt(const uint8_t* pkt, size_t size); + bool reversal_{false}; + bool isBpV4_{false}; + bool isFirstPkt_{true}; +}; + +template +inline RSDecoderMechConstParam& DecoderRSBP::getConstParam() +{ + static RSDecoderMechConstParam param = + { + { + 1248 // msop len + , 1248 // difop len + , 8 // msop id len + , 8 // difop id len + , {0x55, 0xAA, 0x05, 0x0A, 0x5A, 0xA5, 0x50, 0xA0} // msop id + , {0xA5, 0xFF, 0x00, 0x5A, 0x11, 0x11, 0x55, 0x55} // difop id + , {0xFF, 0xEE} // block id + , 32 // laser number + , 12 // blocks per packet + , 32 // channels per block + , 0.1f // distance min + , 150.0f // distance max + , 0.005f // distance resolution + , 0.0625f // temperature resolution + } + // lens center + , 0.01473f // RX + , 0.0085f // RY + , 0.09427f // RZ + }; + + INIT_ONLY_ONCE(); + + float blk_ts = 55.52f; + float firing_tss[] = + { + 0.00f, 2.56f, 5.12f, 7.68f, 10.24f, 12.80f, 15.36f, 17.92f, + 25.68f, 28.24f, 30.80f, 33.36f, 35.92f, 38.48f, 41.04f, 43.60f, + 1.28f, 3.84f, 6.40f, 8.96f, 11.52f, 14.08f, 16.64f, 19.20f, + 26.96f, 29.52f, 32.08f, 34.64f, 37.20f, 39.76f, 42.32f, 44.88f + }; + + param.BLOCK_DURATION = blk_ts / 1000000; + for (uint16_t i = 0; i < sizeof(firing_tss)/sizeof(firing_tss[0]); i++) + { + param.CHAN_TSS[i] = (double)firing_tss[i] / 1000000; + param.CHAN_AZIS[i] = firing_tss[i] / blk_ts; + } + + return param; +} + +template +inline RSEchoMode DecoderRSBP::getEchoMode(uint8_t mode) +{ + switch (mode) + { + case 0x00: // dual return + return RSEchoMode::ECHO_DUAL; + case 0x01: // strongest return + case 0x02: // last return + default: + return RSEchoMode::ECHO_SINGLE; + } +} + +template +inline DecoderRSBP::DecoderRSBP(const RSDecoderParam& param) + : DecoderMech(getConstParam(), param) +{ +} + +template +inline void DecoderRSBP::decodeDifopPkt(const uint8_t* packet, size_t size) +{ + const RSBPDifopPkt& pkt = *(const RSBPDifopPkt*)(packet); + this->template decodeDifopCommon(pkt); + if(pkt.reserved_2[0]) + { + reversal_ = true; + }else{ + reversal_ = false; + } + this->echo_mode_ = getEchoMode (pkt.return_mode); + this->split_blks_per_frame_ = (this->echo_mode_ == RSEchoMode::ECHO_DUAL) ? + (this->blks_per_frame_ << 1) : this->blks_per_frame_; +} + +template +inline bool DecoderRSBP::decodeMsopPkt(const uint8_t* pkt, size_t size) +{ + if (this->echo_mode_ == RSEchoMode::ECHO_SINGLE) + { + return internDecodeMsopPkt>(pkt, size); + } + else + { + return internDecodeMsopPkt>(pkt, size); + } +} + +template +template +inline bool DecoderRSBP::internDecodeMsopPkt(const uint8_t* packet, size_t size) +{ + const RSBPMsopPkt& pkt = *(const RSBPMsopPkt*)(packet); + bool ret = false; + + this->temperature_ = parseTempInLe(&(pkt.header.temp)) * this->const_param_.TEMPERATURE_RES; + this->is_get_temperature_ = true; + if(isFirstPkt_) + { + isFirstPkt_ = false; + if ((pkt.header.lidar_type == 0x03) && (pkt.header.lidar_model == 0x04)) + { + isBpV4_ = true; + this->const_param_.DISTANCE_RES = 0.0025f; + this->mech_const_param_.RX = 0.01619f; + this->mech_const_param_.RY = 0.0085f; + this->mech_const_param_.RZ = 0.09571f; + float blk_ts = 55.56f; + float firing_tss[] = + { + 0.00f, 1.67f, 3.34f, 5.00f, 6.67f, 8.34f, 10.01f, 11.68f, + 13.34f, 15.01f, 16.68f, 18.35f, 20.02f, 21.68f, 23.35f, 25.02f, + 26.69f, 28.36f, 30.02f, 31.69f, 33.36f, 35.03f, 36.70f, 38.36f, + 40.03f, 41.70f, 43.37f, 45.04f, 46.70f, 48.37f, 50.04f, 51.71f + }; + + this->mech_const_param_.BLOCK_DURATION = blk_ts / 1000000; + for (uint16_t i = 0; i < sizeof(firing_tss)/sizeof(firing_tss[0]); i++) + { + this->mech_const_param_.CHAN_TSS[i] = (double)firing_tss[i] / 1000000; + this->mech_const_param_.CHAN_AZIS[i] = firing_tss[i] / blk_ts; + } + } + + } + + + double pkt_ts = 0; + if (this->param_.use_lidar_clock) + { + if (isBpV4_) + pkt_ts = parseTimeUTCWithUs ((RSTimestampUTC*)&pkt.header.timestamp) * 1e-6; + else + pkt_ts = parseTimeYMD (&pkt.header.timestamp) * 1e-6; + } + else + { + uint64_t ts = getTimeHost(); + + // roll back to first block to approach lidar ts as near as possible. + pkt_ts = ts * 1e-6 - this->getPacketDuration(); + + if (this->write_pkt_ts_) + { + if (isBpV4_) + createTimeUTCWithUs (ts, (RSTimestampUTC*)&pkt.header.timestamp); + else + createTimeYMD (ts, (RSTimestampYMD*)&pkt.header.timestamp); + } + } + + T_BlockIterator iter(pkt, this->const_param_.BLOCKS_PER_PKT, this->mech_const_param_.BLOCK_DURATION, + this->block_az_diff_, this->fov_blind_ts_diff_); + + for (uint16_t blk = 0; blk < this->const_param_.BLOCKS_PER_PKT; blk++) + { + const RSBPMsopBlock& block = pkt.blocks[blk]; + + if (memcmp(this->const_param_.BLOCK_ID, block.id, 2) != 0) + { + this->cb_excep_(Error(ERRCODE_WRONGMSOPBLKID)); + break; + } + + int32_t block_az_diff; + double block_ts_off; + iter.get(blk, block_az_diff, block_ts_off); + + double block_ts = pkt_ts + block_ts_off; + int32_t block_az = ntohs(block.azimuth); + if (this->split_strategy_->newBlock(block_az)) + { + this->cb_split_frame_(this->const_param_.LASER_NUM, this->cloudTs()); + this->first_point_ts_ = block_ts; + ret = true; + } + + for (uint16_t chan = 0; chan < this->const_param_.CHANNELS_PER_BLOCK; chan++) + { + const RSChannel& channel = block.channels[chan]; + + double chan_ts = block_ts + this->mech_const_param_.CHAN_TSS[chan]; + int32_t angle_horiz = block_az + + (int32_t)((float)block_az_diff * this->mech_const_param_.CHAN_AZIS[chan]); + + int32_t angle_vert = this->chan_angles_.vertAdjust(chan); + int32_t angle_horiz_final = this->chan_angles_.horizAdjust(chan, angle_horiz); + float distance = ntohs(channel.distance) * this->const_param_.DISTANCE_RES; + if(reversal_) + { + angle_horiz_final = 36000 - angle_horiz_final; + angle_horiz = 36000 - angle_horiz; + } + if (this->distance_section_.in(distance) && this->scan_section_.in(angle_horiz_final)) + { + float x = distance * COS(angle_vert) * COS(angle_horiz_final) + this->mech_const_param_.RX * COS(angle_horiz); + float y = -distance * COS(angle_vert) * SIN(angle_horiz_final) - this->mech_const_param_.RX * SIN(angle_horiz); + float z = distance * SIN(angle_vert) + this->mech_const_param_.RZ; + this->transformPoint(x, y, z); + + typename T_PointCloud::PointT point; + setX(point, x); + setY(point, y); + setZ(point, z); + setIntensity(point, channel.intensity); + setTimestamp(point, chan_ts); + setRing(point, this->chan_angles_.toUserChan(chan)); + + this->point_cloud_->points.emplace_back(point); + } + else if (!this->param_.dense_points) + { + typename T_PointCloud::PointT point; + setX(point, NAN); + setY(point, NAN); + setZ(point, NAN); + setIntensity(point, 0); + setTimestamp(point, chan_ts); + setRing(point, this->chan_angles_.toUserChan(chan)); + + this->point_cloud_->points.emplace_back(point); + } + + this->prev_point_ts_ = chan_ts; + } + } + + this->prev_pkt_ts_ = pkt_ts; + return ret; +} + +template +inline bool DecoderRSBP::isNewFrame(const uint8_t* packet) +{ + const RSBPMsopPkt& pkt = *(const RSBPMsopPkt*)(packet); + + for (uint16_t blk = 0; blk < this->const_param_.BLOCKS_PER_PKT; blk++) + { + const RSBPMsopBlock& block = pkt.blocks[blk]; + + if (memcmp(this->const_param_.BLOCK_ID, block.id, 2) != 0) + { + break; + } + int32_t block_az = ntohs(block.azimuth); + if (this->pre_split_strategy_->newBlock(block_az)) + { + return true; + } + } + return false; +} + +} // namespace lidar +} // namespace robosense diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSE1.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSE1.hpp new file mode 100644 index 0000000..fae81bc --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSE1.hpp @@ -0,0 +1,294 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once + +#include + +namespace robosense +{ +namespace lidar +{ +#pragma pack(push, 1) + +typedef struct +{ + uint8_t id[4]; + uint16_t pkt_seq; + uint16_t protocol_version; + uint8_t return_mode; + uint8_t time_mode; + RSTimestampUTC timestamp; + uint8_t reserved[10]; + uint8_t lidar_type; + uint8_t temperature; +} RSEOSMsopHeader; + +typedef struct +{ + uint16_t distance; + int16_t x; + int16_t y; + int16_t z; + uint8_t intensity; + uint8_t point_attribute; +} RSEOSChannel; + +typedef struct +{ + uint16_t time_offset; + RSEOSChannel channel[1]; +} RSEOSBlock; + +typedef struct +{ + RSEOSMsopHeader header; + RSEOSBlock blocks[96]; + uint8_t reserved[16]; +} RSEOSMsopPkt; + + +typedef struct +{ + uint8_t id[8]; + uint8_t reserved1[93]; + uint8_t timeMode; + uint8_t timeSyncStatus; + RSTimestampUTC timestamp; + uint8_t reserved2[95]; + uint32_t acceIx; + uint32_t acceIy; + uint32_t acceIz; + uint32_t gyrox; + uint32_t gyroy; + uint32_t gyroz; + uint8_t reserved3[24]; +} RSE1DifopPkt; + +#pragma pack(pop) + +template +class DecoderRSE1 : public Decoder +{ +public: + + constexpr static double FRAME_DURATION = 0.1; + constexpr static uint32_t SINGLE_PKT_NUM = 288; + constexpr static int VECTOR_BASE = 32768; + + virtual void decodeDifopPkt(const uint8_t* pkt, size_t size); + virtual bool decodeMsopPkt(const uint8_t* pkt, size_t size); + virtual ~DecoderRSE1() = default; + + explicit DecoderRSE1(const RSDecoderParam& param); + +private: + + static RSDecoderConstParam& getConstParam(); + RSEchoMode getEchoMode(uint8_t mode); + + SplitStrategyBySeq split_strategy_; +}; + +template +inline RSDecoderConstParam& DecoderRSE1::getConstParam() +{ + static RSDecoderConstParam param = + { + 1200 // msop len + , 256 // difop len + , 4 // msop id len + , 8 // difop id len + , {0x55, 0xAA, 0x5A, 0xA5} // msop id + , {0xA5, 0xFF, 0x00, 0x5A, 0x11, 0x11, 0x55, 0x55} // difop id + , {0x00, 0x00} + , 1 // laser number + , 96 // blocks per packet + , 1 // channels per block + , 0.2f // distance min + , 200.0f // distance max + , 0.005f // distance resolution + , 80.0f // initial value of temperature + }; + + return param; +} + +template +inline DecoderRSE1::DecoderRSE1(const RSDecoderParam& param) + : Decoder(getConstParam(), param) +{ + this->packet_duration_ = FRAME_DURATION / SINGLE_PKT_NUM; + this->angles_ready_ = true; +} + +template +inline RSEchoMode DecoderRSE1::getEchoMode(uint8_t mode) +{ + switch (mode) + { + case 0x00: // dual return + return RSEchoMode::ECHO_DUAL; + case 0x04: // strongest return + case 0x05: // last return + case 0x06: // nearest return + default: + return RSEchoMode::ECHO_SINGLE; + } +} + +template +inline void DecoderRSE1::decodeDifopPkt(const uint8_t* packet, size_t size) +{ + #ifdef ENABLE_DIFOP_PARSE + const RSE1DifopPkt& pkt = *(RSE1DifopPkt*)packet; + + if(this->imuDataPtr_ && this->cb_imu_data_ && !this->imuDataPtr_->state) + { + if (this->param_.use_lidar_clock) + { + this->imuDataPtr_->timestamp = parseTimeUTCWithUs(&pkt.timestamp) * 1e-6; + } + else + { + this->imuDataPtr_->timestamp = getTimeHost() * 1e-6; + } + + this->imuDataPtr_->linear_acceleration_x = convertUint32ToFloat(ntohl(pkt.acceIx)) ; + this->imuDataPtr_->linear_acceleration_y = convertUint32ToFloat(ntohl(pkt.acceIy)) ; + this->imuDataPtr_->linear_acceleration_z = convertUint32ToFloat(ntohl(pkt.acceIz)) ; + + this->imuDataPtr_->angular_velocity_x = convertUint32ToFloat(ntohl(pkt.gyrox)) ; + this->imuDataPtr_->angular_velocity_y = convertUint32ToFloat(ntohl(pkt.gyroy)) ; + this->imuDataPtr_->angular_velocity_z = convertUint32ToFloat(ntohl(pkt.gyroz)) ; + this->imuDataPtr_->state = true; + this->cb_imu_data_(); + } + + + this->device_info_.state = true; + // device status + this->device_status_.state = true; + #endif +} + +template +inline bool DecoderRSE1::decodeMsopPkt(const uint8_t* packet, size_t size) +{ + const RSEOSMsopPkt& pkt = *(RSEOSMsopPkt*)packet; + bool ret = false; + + this->temperature_ = static_cast((int)pkt.header.temperature - this->const_param_.TEMPERATURE_RES); + this->is_get_temperature_ = true; + double pkt_ts = 0; + if (this->param_.use_lidar_clock) + { + pkt_ts = parseTimeUTCWithUs(&pkt.header.timestamp) * 1e-6; + } + else + { + uint64_t ts = getTimeHost(); + + // roll back to first block to approach lidar ts as near as possible. + pkt_ts = getTimeHost() * 1e-6 - this->getPacketDuration(); + + if (this->write_pkt_ts_) + { + createTimeUTCWithUs(ts, (RSTimestampUTC*)&pkt.header.timestamp); + } + } + + uint16_t pkt_seq = ntohs(pkt.header.pkt_seq); + if (split_strategy_.newPacket(pkt_seq)) + { + this->cb_split_frame_(this->const_param_.LASER_NUM, this->cloudTs()); + this->first_point_ts_ = pkt_ts; + ret = true; + } + + for (uint16_t blk = 0; blk < this->const_param_.BLOCKS_PER_PKT; blk++) + { + const RSEOSBlock& block = pkt.blocks[blk]; + + double point_time = pkt_ts + ntohs(block.time_offset) * 1e-6; + + for (uint16_t chan = 0; chan < this->const_param_.CHANNELS_PER_BLOCK; chan++) + { + const RSEOSChannel& channel = block.channel[chan]; + + float distance = ntohs(channel.distance) * this->const_param_.DISTANCE_RES; + + if (this->distance_section_.in(distance)) + { + int16_t vector_x = RS_SWAP_INT16(channel.x); + int16_t vector_y = RS_SWAP_INT16(channel.y); + int16_t vector_z = RS_SWAP_INT16(channel.z); + + float x = vector_x * distance / VECTOR_BASE; + float y = vector_y * distance / VECTOR_BASE; + float z = vector_z * distance / VECTOR_BASE; + + this->transformPoint(x, y, z); + + typename T_PointCloud::PointT point; + setX(point, x); + setY(point, y); + setZ(point, z); + setIntensity(point, channel.intensity); + setTimestamp(point, point_time); + setRing(point, chan); + + this->point_cloud_->points.emplace_back(point); + } + else if (!this->param_.dense_points) + { + typename T_PointCloud::PointT point; + setX(point, NAN); + setY(point, NAN); + setZ(point, NAN); + setIntensity(point, 0); + setTimestamp(point, point_time); + setRing(point, chan); + + this->point_cloud_->points.emplace_back(point); + } + } + + this->prev_point_ts_ = point_time; + } + + this->prev_pkt_ts_ = pkt_ts; + return ret; +} + +} // namespace lidar +} // namespace robosense diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSHELIOS.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSHELIOS.hpp new file mode 100644 index 0000000..3b17b31 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSHELIOS.hpp @@ -0,0 +1,338 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once +#include + +namespace robosense +{ +namespace lidar +{ + +#pragma pack(push, 1) + +typedef struct +{ + uint8_t id[2]; + uint16_t azimuth; + RSChannel channels[32]; +} RSHELIOSMsopBlock; + +typedef struct +{ + uint8_t id[4]; + uint16_t protocol_version; + uint8_t reserved1[14]; + RSTimestampUTC timestamp; + uint8_t lidar_type; + uint8_t reserved2[7]; + RSTemperature temp; + uint8_t reserved3[2]; +} RSHELIOSMsopHeader; + +typedef struct +{ + RSHELIOSMsopHeader header; + RSHELIOSMsopBlock blocks[12]; + unsigned int index; + uint16_t tail; +} RSHELIOSMsopPkt; + +typedef struct +{ + uint8_t id[8]; + uint16_t rpm; + RSEthNetV2 eth; + RSFOV fov; + uint8_t reserved1[2]; + uint16_t phase_lock_angle; + RSVersionV2 version; + uint8_t reserved2[229]; + RSSN sn; + uint16_t zero_cali; + uint8_t return_mode; + RSTimeInfo time_info; + RSStatusV2 status; + uint8_t reserved3[5]; + RSDiagnoV2 diagno; + uint8_t gprmc[86]; + RSCalibrationAngle vert_angle_cali[32]; + RSCalibrationAngle horiz_angle_cali[32]; + uint8_t reserved4[586]; + uint16_t tail; +} RSHELIOSDifopPkt; + +#pragma pack(pop) + +template +class DecoderRSHELIOS : public DecoderMech +{ +public: + virtual void decodeDifopPkt(const uint8_t* pkt, size_t size); + virtual bool decodeMsopPkt(const uint8_t* pkt, size_t size); + virtual ~DecoderRSHELIOS() = default; + + explicit DecoderRSHELIOS(const RSDecoderParam& param); + virtual bool isNewFrame(const uint8_t* packet) override; +#ifndef UNIT_TEST +protected: +#endif + + static RSDecoderMechConstParam& getConstParam(); + static RSEchoMode getEchoMode(uint8_t mode); + + template + bool internDecodeMsopPkt(const uint8_t* pkt, size_t size); +}; + +template +inline RSDecoderMechConstParam& DecoderRSHELIOS::getConstParam() +{ + static RSDecoderMechConstParam param = + { + { + 1248 // msop len + , 1248 // difop len + , 4 // msop id len + , 8 // difop id len + , {0x55, 0xAA, 0x05, 0x5A} // msop id + , {0xA5, 0xFF, 0x00, 0x5A, 0x11, 0x11, 0x55, 0x55} // difop id + , {0xFF, 0xEE} // block id + , 32 // laser number + , 12 // blocks per packet + , 32 // channels per block + , 0.1f // distance min + , 180.0f // distance max + , 0.0025f // distance resolution + , 0.0625f // temperature resolution + } + // lens center + , 0.03498f // RX + , -0.015f // RY + , 0.0f // RZ + }; + + INIT_ONLY_ONCE(); + + float blk_ts = 55.56f; + float firing_tss[] = + { + 0.00f, 1.73f, 3.46f, 5.19f, 6.92f, 8.65f, 10.38f, 12.11f, + 13.84f, 15.57f, 17.3f, 19.03f, 20.76f, 22.49f, 24.22f, 25.95f, + 27.68f, 29.41f, 31.14f, 32.87f, 34.6f, 36.33f, 38.06f, 39.79f, + 41.52f, 43.25f, 44.98f, 46.71f, 48.44f, 50.17f, 51.9f, 53.63f + }; + + param.BLOCK_DURATION = blk_ts / 1000000; + for (uint16_t i = 0; i < sizeof(firing_tss)/sizeof(firing_tss[0]); i++) + { + param.CHAN_TSS[i] = (double)firing_tss[i] / 1000000; + param.CHAN_AZIS[i] = firing_tss[i] / blk_ts; + } + + return param; +} + +template +inline RSEchoMode DecoderRSHELIOS::getEchoMode(uint8_t mode) +{ + switch (mode) + { + case 0x00: // dual return + return RSEchoMode::ECHO_DUAL; + case 0x04: // strongest return + case 0x05: // last return + case 0x06: // nearest return + default: + return RSEchoMode::ECHO_SINGLE; + } +} + +template +inline DecoderRSHELIOS::DecoderRSHELIOS(const RSDecoderParam& param) + : DecoderMech(getConstParam(), param) +{ +} + +template +inline void DecoderRSHELIOS::decodeDifopPkt(const uint8_t* packet, size_t size) +{ + const RSHELIOSDifopPkt& pkt = *(const RSHELIOSDifopPkt*)(packet); + this->template decodeDifopCommon(pkt); + + this->echo_mode_ = getEchoMode (pkt.return_mode); + this->split_blks_per_frame_ = (this->echo_mode_ == RSEchoMode::ECHO_DUAL) ? + (this->blks_per_frame_ << 1) : this->blks_per_frame_; +} + +template +inline bool DecoderRSHELIOS::decodeMsopPkt(const uint8_t* pkt, size_t size) +{ + if (this->echo_mode_ == RSEchoMode::ECHO_SINGLE) + { + return internDecodeMsopPkt>(pkt, size); + } + else + { + return internDecodeMsopPkt>(pkt, size); + } +} + +template +template +inline bool DecoderRSHELIOS::internDecodeMsopPkt(const uint8_t* packet, size_t size) +{ + const RSHELIOSMsopPkt& pkt = *(const RSHELIOSMsopPkt*)(packet); + bool ret = false; + + this->temperature_ = parseTempInLe(&(pkt.header.temp)) * this->const_param_.TEMPERATURE_RES; + this->is_get_temperature_ = true; + double pkt_ts = 0; + if (this->param_.use_lidar_clock) + { + pkt_ts = parseTimeUTCWithUs(&pkt.header.timestamp) * 1e-6; + } + else + { + uint64_t ts = getTimeHost(); + + // roll back to first block to approach lidar ts as near as possible. + pkt_ts = ts * 1e-6 - this->getPacketDuration(); + + if (this->write_pkt_ts_) + { + createTimeUTCWithUs (ts, (RSTimestampUTC*)&pkt.header.timestamp); + } + } + + T_BlockIterator iter(pkt, this->const_param_.BLOCKS_PER_PKT, this->mech_const_param_.BLOCK_DURATION, + this->block_az_diff_, this->fov_blind_ts_diff_); + + for (uint16_t blk = 0; blk < this->const_param_.BLOCKS_PER_PKT; blk++) + { + const RSHELIOSMsopBlock& block = pkt.blocks[blk]; + + if (memcmp(this->const_param_.BLOCK_ID, block.id, 2) != 0) + { + this->cb_excep_(Error(ERRCODE_WRONGMSOPBLKID)); + break; + } + + int32_t block_az_diff; + double block_ts_off; + iter.get(blk, block_az_diff, block_ts_off); + + double block_ts = pkt_ts + block_ts_off; + int32_t block_az = ntohs(block.azimuth); + if (this->split_strategy_->newBlock(block_az)) + { + this->cb_split_frame_(this->const_param_.LASER_NUM, this->cloudTs()); + this->first_point_ts_ = block_ts; + ret = true; + } + + for (uint16_t chan = 0; chan < this->const_param_.CHANNELS_PER_BLOCK; chan++) + { + const RSChannel& channel = block.channels[chan]; + + double chan_ts = block_ts + this->mech_const_param_.CHAN_TSS[chan]; + int32_t angle_horiz = block_az + + (int32_t)((float)block_az_diff * this->mech_const_param_.CHAN_AZIS[chan]); + + int32_t angle_vert = this->chan_angles_.vertAdjust(chan); + int32_t angle_horiz_final = this->chan_angles_.horizAdjust(chan, angle_horiz); + float distance = ntohs(channel.distance) * this->const_param_.DISTANCE_RES; + + if (this->distance_section_.in(distance) && this->scan_section_.in(angle_horiz_final)) + { + float x = distance * COS(angle_vert) * COS(angle_horiz_final) + this->mech_const_param_.RX * COS(angle_horiz); + float y = -distance * COS(angle_vert) * SIN(angle_horiz_final) - this->mech_const_param_.RX * SIN(angle_horiz); + float z = distance * SIN(angle_vert) + this->mech_const_param_.RZ; + this->transformPoint(x, y, z); + + typename T_PointCloud::PointT point; + setX(point, x); + setY(point, y); + setZ(point, z); + setIntensity(point, channel.intensity); + setTimestamp(point, chan_ts); + setRing(point, this->chan_angles_.toUserChan(chan)); + + this->point_cloud_->points.emplace_back(point); + } + else if (!this->param_.dense_points) + { + typename T_PointCloud::PointT point; + setX(point, NAN); + setY(point, NAN); + setZ(point, NAN); + setIntensity(point, 0); + setTimestamp(point, chan_ts); + setRing(point, this->chan_angles_.toUserChan(chan)); + + this->point_cloud_->points.emplace_back(point); + } + + this->prev_point_ts_ = chan_ts; + } + } + + this->prev_pkt_ts_ = pkt_ts; + return ret; +} + +template +inline bool DecoderRSHELIOS::isNewFrame(const uint8_t* packet) +{ + const RSHELIOSMsopPkt& pkt = *(const RSHELIOSMsopPkt*)(packet); + + for (uint16_t blk = 0; blk < this->const_param_.BLOCKS_PER_PKT; blk++) + { + const RSHELIOSMsopBlock& block = pkt.blocks[blk]; + + if (memcmp(this->const_param_.BLOCK_ID, block.id, 2) != 0) + { + break; + } + + int32_t block_az = ntohs(block.azimuth); + if (this->pre_split_strategy_->newBlock(block_az)) + { + return true; + } + } + + return false; +} + +} // namespace lidar +} // namespace robosense diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSHELIOS_16P.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSHELIOS_16P.hpp new file mode 100644 index 0000000..2cea047 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSHELIOS_16P.hpp @@ -0,0 +1,306 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once +#include +#include + +namespace robosense +{ +namespace lidar +{ + +template +class DecoderRSHELIOS_16P : public DecoderMech +{ +public: + virtual void decodeDifopPkt(const uint8_t* pkt, size_t size); + virtual bool decodeMsopPkt(const uint8_t* pkt, size_t size); + virtual ~DecoderRSHELIOS_16P() = default; + + explicit DecoderRSHELIOS_16P(const RSDecoderParam& param); + virtual bool isNewFrame(const uint8_t* packet) override; +#ifndef UNIT_TEST +protected: +#endif + + static RSDecoderMechConstParam& getConstParam(); + static RSEchoMode getEchoMode(uint8_t mode); + + void calcParam(); + + template + bool internDecodeMsopPkt(const uint8_t* pkt, size_t size); +}; + +template +inline RSDecoderMechConstParam& DecoderRSHELIOS_16P::getConstParam() +{ + static RSDecoderMechConstParam param = + { + { + 1248 // msop len + , 1248 // difop len + , 4 // msop id len + , 8 // difop id len + , {0x55, 0xAA, 0x05, 0x5A} // msop id + , {0xA5, 0xFF, 0x00, 0x5A, 0x11, 0x11, 0x55, 0x55} // difop id + , {0xFF, 0xEE} // block id + , 16 // laser number + , 12 // blocks per packet + , 32 // channels per block + , 0.1f // distance min + , 180.0f // distance max + , 0.0025f // distance resolution + , 0.0625f // temperature resolution + } + // lens center + , 0.03498f // RX + , -0.015f // RY + , 0.0f // RZ + }; + + float blk_ts = 55.56f; + param.BLOCK_DURATION = blk_ts / 1000000; + + return param; +} + +template +inline void DecoderRSHELIOS_16P::calcParam() +{ + float blk_ts = 55.56f; + float firing_tss[] = + { + 0.00f, 1.73f, 3.46f, 5.19f, 6.92f, 8.65f, 10.38f, 12.11f, + 13.84f, 15.57f, 17.3f, 19.03f, 20.76f, 22.49f, 24.22f, 25.95f, + 27.68f, 29.41f, 31.14f, 32.87f, 34.6f, 36.33f, 38.06f, 39.79f, + 41.52f, 43.25f, 44.98f, 46.71f, 48.44f, 50.17f, 51.9f, 53.63f + }; + + if (this->echo_mode_ == RSEchoMode::ECHO_SINGLE) + { + Rs16SingleReturnBlockIterator::calcChannel(blk_ts, firing_tss, + this->mech_const_param_.CHAN_AZIS, this->mech_const_param_.CHAN_TSS); + } + else + { + Rs16DualReturnBlockIterator::calcChannel(blk_ts, firing_tss, + this->mech_const_param_.CHAN_AZIS, this->mech_const_param_.CHAN_TSS); + } +} + +template +inline RSEchoMode DecoderRSHELIOS_16P::getEchoMode(uint8_t mode) +{ + switch (mode) + { + case 0x00: // dual return + return RSEchoMode::ECHO_DUAL; + case 0x04: // strongest return + case 0x05: // last return + case 0x06: // nearest return + default: + return RSEchoMode::ECHO_SINGLE; + } +} + +template +inline DecoderRSHELIOS_16P::DecoderRSHELIOS_16P(const RSDecoderParam& param) + : DecoderMech(getConstParam(), param) +{ + this->packet_duration_ = + this->mech_const_param_.BLOCK_DURATION * this->const_param_.BLOCKS_PER_PKT * 2; + + calcParam(); +} + +template +inline void DecoderRSHELIOS_16P::decodeDifopPkt(const uint8_t* packet, size_t size) +{ + const RSHELIOSDifopPkt& pkt = *(const RSHELIOSDifopPkt*)(packet); + this->template decodeDifopCommon(pkt); + + RSEchoMode echo_mode = getEchoMode (pkt.return_mode); + if (this->echo_mode_ != echo_mode) + { + this->echo_mode_ = echo_mode; + this->split_blks_per_frame_ = (this->echo_mode_ == RSEchoMode::ECHO_DUAL) ? + this->blks_per_frame_ : (this->blks_per_frame_ >> 1); + + calcParam(); + } +} + +template +inline bool DecoderRSHELIOS_16P::decodeMsopPkt(const uint8_t* pkt, size_t size) +{ + if (this->echo_mode_ == RSEchoMode::ECHO_SINGLE) + { + return internDecodeMsopPkt>(pkt, size); + } + else + { + return internDecodeMsopPkt>(pkt, size); + } +} + +template +template +inline bool DecoderRSHELIOS_16P::internDecodeMsopPkt(const uint8_t* packet, size_t size) +{ + const RSHELIOSMsopPkt& pkt = *(const RSHELIOSMsopPkt*)(packet); + bool ret = false; + + this->temperature_ = parseTempInLe(&(pkt.header.temp)) * this->const_param_.TEMPERATURE_RES; + this->is_get_temperature_ = true; + double pkt_ts = 0; + if (this->param_.use_lidar_clock) + { + pkt_ts = parseTimeUTCWithUs(&pkt.header.timestamp) * 1e-6; + } + else + { + uint64_t ts = getTimeHost(); + + // roll back to first block to approach lidar ts as near as possible. + pkt_ts = ts * 1e-6 - this->getPacketDuration(); + + if (this->write_pkt_ts_) + { + createTimeUTCWithUs (ts, (RSTimestampUTC*)&pkt.header.timestamp); + } + } + + T_BlockIterator iter(pkt, this->const_param_.BLOCKS_PER_PKT, this->mech_const_param_.BLOCK_DURATION, + this->block_az_diff_, this->fov_blind_ts_diff_); + + for (uint16_t blk = 0; blk < this->const_param_.BLOCKS_PER_PKT; blk++) + { + const RSHELIOSMsopBlock& block = pkt.blocks[blk]; + + if (memcmp(this->const_param_.BLOCK_ID, block.id, 2) != 0) + { + this->cb_excep_(Error(ERRCODE_WRONGMSOPBLKID)); + break; + } + + int32_t block_az_diff; + double block_ts_off; + iter.get(blk, block_az_diff, block_ts_off); + + double block_ts = pkt_ts + block_ts_off; + int32_t block_az = ntohs(block.azimuth); + if (this->split_strategy_->newBlock(block_az)) + { + this->cb_split_frame_(this->const_param_.LASER_NUM, this->cloudTs()); + this->first_point_ts_ = block_ts; + ret = true; + } + + for (uint16_t chan = 0; chan < this->const_param_.CHANNELS_PER_BLOCK; chan++) + { + const RSChannel& channel = block.channels[chan]; + + double chan_ts = block_ts + this->mech_const_param_.CHAN_TSS[chan]; + int32_t angle_horiz = block_az + + (int32_t)((float)block_az_diff * this->mech_const_param_.CHAN_AZIS[chan]); + + uint16_t laser = chan % 16; + int32_t angle_vert = this->chan_angles_.vertAdjust(laser); + int32_t angle_horiz_final = this->chan_angles_.horizAdjust(laser, angle_horiz); + float distance = ntohs(channel.distance) * this->const_param_.DISTANCE_RES; + + if (this->distance_section_.in(distance) && this->scan_section_.in(angle_horiz_final)) + { + float x = distance * COS(angle_vert) * COS(angle_horiz_final) + this->mech_const_param_.RX * COS(angle_horiz); + float y = -distance * COS(angle_vert) * SIN(angle_horiz_final) - this->mech_const_param_.RX * SIN(angle_horiz); + float z = distance * SIN(angle_vert) + this->mech_const_param_.RZ; + this->transformPoint(x, y, z); + + typename T_PointCloud::PointT point; + setX(point, x); + setY(point, y); + setZ(point, z); + setIntensity(point, channel.intensity); + setTimestamp(point, chan_ts); + setRing(point, (this->chan_angles_.toUserChan(laser))); + + this->point_cloud_->points.emplace_back(point); + } + else if (!this->param_.dense_points) + { + typename T_PointCloud::PointT point; + setX(point, NAN); + setY(point, NAN); + setZ(point, NAN); + setIntensity(point, 0); + setTimestamp(point, chan_ts); + setRing(point, (this->chan_angles_.toUserChan(laser))); + + this->point_cloud_->points.emplace_back(point); + } + + this->prev_point_ts_ = chan_ts; + } + } + + this->prev_pkt_ts_ = pkt_ts; + return ret; +} + +template +inline bool DecoderRSHELIOS_16P::isNewFrame(const uint8_t* packet) +{ + const RSHELIOSMsopPkt& pkt = *(const RSHELIOSMsopPkt*)(packet); + + for (uint16_t blk = 0; blk < this->const_param_.BLOCKS_PER_PKT; blk++) + { + const RSHELIOSMsopBlock& block = pkt.blocks[blk]; + + if (memcmp(this->const_param_.BLOCK_ID, block.id, 2) != 0) + { + break; + } + + int32_t block_az = ntohs(block.azimuth); + if (this->pre_split_strategy_->newBlock(block_az)) + { + return true; + } + } + + return false; +} + +} // namespace lidar +} // namespace robosense diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSM1.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSM1.hpp new file mode 100644 index 0000000..8190fd7 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSM1.hpp @@ -0,0 +1,317 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once +#include + +namespace robosense +{ +namespace lidar +{ +#pragma pack(push, 1) + +typedef struct +{ + uint8_t id[4]; + uint16_t pkt_seq; + uint16_t protocol_version; + uint8_t return_mode; + uint8_t time_mode; + RSTimestampUTC timestamp; + uint8_t reserved[10]; + uint8_t lidar_type; + uint8_t temperature; +} RSM1MsopHeader; + +typedef struct +{ + uint16_t distance; + uint16_t pitch; + uint16_t yaw; + uint8_t intensity; + uint8_t point_attribute; + uint8_t elongation; +} RSM1Channel; + +typedef struct +{ + uint8_t time_offset; + uint8_t return_seq; + RSM1Channel channel[5]; +} RSM1Block; + +typedef struct +{ + RSM1MsopHeader header; + RSM1Block blocks[25]; + uint8_t reserved[3]; +} RSM1MsopPkt; + +typedef struct +{ + uint8_t ip_local[4]; + uint8_t ip_remote[4]; + uint8_t mac_addr[6]; + uint8_t msop_port[2]; + uint8_t difop_port[2]; +} RSM1DifopEther; + +typedef struct +{ + uint8_t horizontal_fov_start[2]; + uint8_t horizontal_fov_end[2]; + uint8_t vertical_fov_start[2]; + uint8_t vertical_fov_end[2]; +} RSM1DifopFov; + +typedef struct +{ + uint8_t pl_ver[5]; + uint8_t ps_ver[5]; +} RSM1DifopVerInfo; + +typedef struct +{ + uint8_t current_1[3]; + uint8_t current_2[3]; + uint16_t voltage_1; + uint16_t voltage_2; + uint8_t reserved[10]; +} RSM1DifopRunSts; + +typedef struct +{ + uint8_t param_sign; + uint16_t data; +} RSM1DifopCalibration; + +typedef struct +{ + uint8_t id[8]; + uint8_t reserved1[1]; + uint8_t frame_rate; + RSM1DifopEther eth; + RSM1DifopVerInfo version; + RSSN sn; + uint8_t return_mode; + RSTimeInfo time_info; + RSM1DifopRunSts status; + uint8_t reserved2[40]; + RSM1DifopCalibration cali_param[20]; + uint8_t reserved3[79]; +} RSM1DifopPkt; + +#pragma pack(pop) + +template +class DecoderRSM1 : public Decoder +{ +public: + + constexpr static double FRAME_DURATION = 0.1; + constexpr static uint32_t SINGLE_PKT_NUM = 630; + constexpr static int ANGLE_OFFSET = 32768; + + virtual void decodeDifopPkt(const uint8_t* pkt, size_t size); + virtual bool decodeMsopPkt(const uint8_t* pkt, size_t size); + virtual ~DecoderRSM1() = default; + + explicit DecoderRSM1(const RSDecoderParam& param); + +private: + + static RSDecoderConstParam& getConstParam(); + RSEchoMode getEchoMode(uint8_t mode); + + SplitStrategyBySeq split_strategy_; +}; + +template +inline RSDecoderConstParam& DecoderRSM1::getConstParam() +{ + static RSDecoderConstParam param = + { + 1210 // msop len + , 256 // difop len + , 4 // msop id len + , 8 // difop id len + , {0x55, 0xAA, 0x5A, 0xA5} // msop id + , {0xA5, 0xFF, 0x00, 0x5A, 0x11, 0x11, 0x55, 0x55} // difop id + , {0x00, 0x00} + , 5 // laser number + , 25 // blocks per packet + , 5 // channels per block + , 0.2f // distance min + , 200.0f // distance max + , 0.005f // distance resolution + , 80.0f // initial value of temperature + }; + + return param; +} + +template +inline DecoderRSM1::DecoderRSM1(const RSDecoderParam& param) + : Decoder(getConstParam(), param) +{ + this->packet_duration_ = FRAME_DURATION / SINGLE_PKT_NUM; + this->angles_ready_ = true; +} + +template +inline RSEchoMode DecoderRSM1::getEchoMode(uint8_t mode) +{ + switch (mode) + { + case 0x00: // dual return + return RSEchoMode::ECHO_DUAL; + case 0x04: // strongest return + case 0x05: // last return + case 0x06: // first return + default: + return RSEchoMode::ECHO_SINGLE; + } +} + +template +inline void DecoderRSM1::decodeDifopPkt(const uint8_t* packet, size_t size) +{ + const RSM1DifopPkt& pkt = *(RSM1DifopPkt*)packet; + this->echo_mode_ = this->getEchoMode(pkt.return_mode); + +#ifdef ENABLE_DIFOP_PARSE + // device info + memcpy (this->device_info_.sn, pkt.sn.num, 6); + memcpy (this->device_info_.mac, pkt.eth.mac_addr, 6); + memcpy (this->device_info_.top_ver, pkt.version.pl_ver, 5); + memcpy (this->device_info_.bottom_ver, pkt.version.ps_ver, 5); + this->device_info_.state = true; + // device status + this->device_status_.voltage = ntohs(pkt.status.voltage_1); + this->device_status_.state = true; +#endif +} + +template +inline bool DecoderRSM1::decodeMsopPkt(const uint8_t* packet, size_t size) +{ + const RSM1MsopPkt& pkt = *(RSM1MsopPkt*)packet; + bool ret = false; + + this->temperature_ = static_cast((int)pkt.header.temperature - this->const_param_.TEMPERATURE_RES); + this->is_get_temperature_ = true; + double pkt_ts = 0; + if (this->param_.use_lidar_clock) + { + pkt_ts = parseTimeUTCWithUs(&pkt.header.timestamp) * 1e-6; + } + else + { + uint64_t ts = getTimeHost(); + + // roll back to first block to approach lidar ts as near as possible. + pkt_ts = getTimeHost() * 1e-6 - this->getPacketDuration(); + + if (this->write_pkt_ts_) + { + createTimeUTCWithUs (ts, (RSTimestampUTC*)&pkt.header.timestamp); + } + } + + uint16_t pkt_seq = ntohs(pkt.header.pkt_seq); + if (split_strategy_.newPacket(pkt_seq)) + { + this->cb_split_frame_(this->const_param_.LASER_NUM, this->cloudTs()); + this->first_point_ts_ = pkt_ts; + ret = true; + } + + for (uint16_t blk = 0; blk < this->const_param_.BLOCKS_PER_PKT; blk++) + { + const RSM1Block& block = pkt.blocks[blk]; + + double point_time = pkt_ts + block.time_offset * 1e-6; + + for (uint16_t chan = 0; chan < this->const_param_.CHANNELS_PER_BLOCK; chan++) + { + const RSM1Channel& channel = block.channel[chan]; + + float distance = ntohs(channel.distance) * this->const_param_.DISTANCE_RES; + + if (this->distance_section_.in(distance)) + { + int pitch = ntohs(channel.pitch) - ANGLE_OFFSET; + int yaw = ntohs(channel.yaw) - ANGLE_OFFSET; + + float x = distance * COS (pitch) * COS (yaw); + float y = distance * COS (pitch) * SIN (yaw); + float z = distance * SIN (pitch); + this->transformPoint(x, y, z); + + typename T_PointCloud::PointT point; + setX(point, x); + setY(point, y); + setZ(point, z); + setIntensity(point, channel.intensity); + setTimestamp(point, point_time); + setRing(point, chan); + + this->point_cloud_->points.emplace_back(point); + } + else if (!this->param_.dense_points) + { + typename T_PointCloud::PointT point; + setX(point, NAN); + setY(point, NAN); + setZ(point, NAN); + setIntensity(point, 0); + setTimestamp(point, point_time); + setRing(point, chan); + + this->point_cloud_->points.emplace_back(point); + } + } + + this->prev_point_ts_ = point_time; + } + + if (split_strategy_.maxSeq() == pkt_seq) + { + this->cb_split_frame_(this->const_param_.LASER_NUM, this->cloudTs()); + } + + this->prev_pkt_ts_ = pkt_ts; + return ret; +} + +} // namespace lidar +} // namespace robosense diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSM1_Jumbo.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSM1_Jumbo.hpp new file mode 100644 index 0000000..52cf6e2 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSM1_Jumbo.hpp @@ -0,0 +1,262 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once +#include + +namespace robosense +{ +namespace lidar +{ +#pragma pack(push, 1) + +typedef struct +{ + uint16_t distance; + uint16_t pitch; + uint16_t yaw; + uint8_t intensity; +} RSM1_Jumbo_Channel; + +typedef struct +{ + uint8_t time_offset; + uint8_t return_seq; + RSM1_Jumbo_Channel channel[5]; +} RSM1_Jumbo_Block; + +typedef struct +{ + RSM1MsopHeader header; + RSM1_Jumbo_Block blocks[25]; + uint8_t reserved1[11]; + uint8_t reserved2[16]; +} RSM1_Jumbo_MsopPkt; + +typedef struct +{ + RSM1_Jumbo_MsopPkt pkts[63]; + uint8_t reserved[160]; +} RSM1_Jumbo; + +#pragma pack(pop) + +template +class DecoderRSM1_Jumbo : public Decoder +{ +public: + + constexpr static double FRAME_DURATION = 0.1; + constexpr static uint32_t SINGLE_PKT_NUM = 630; + constexpr static int ANGLE_OFFSET = 32768; + + virtual void decodeDifopPkt(const uint8_t* pkt, size_t size); + virtual bool decodeMsopPkt(const uint8_t* pkt, size_t size); + virtual ~DecoderRSM1_Jumbo() = default; + + explicit DecoderRSM1_Jumbo(const RSDecoderParam& param); + +private: + + static RSDecoderConstParam& getConstParam(); + RSEchoMode getEchoMode(uint8_t mode); + + bool internDecodeMsopPkt(const uint8_t* pkt, size_t size); + SplitStrategyBySeq split_strategy_; +}; + +template +inline RSDecoderConstParam& DecoderRSM1_Jumbo::getConstParam() +{ + static RSDecoderConstParam param = + { + 62152 // msop len + , 256 // difop len + , 4 // msop id len + , 8 // difop id len + , {0x55, 0xAA, 0x5A, 0xA5} // msop id + , {0xA5, 0xFF, 0x00, 0x5A, 0x11, 0x11, 0x55, 0x55} // difop id + , {0x00, 0x00} + , 5 // laser number + , 25 // blocks per packet + , 5 // channels per block + , 0.2f // distance min + , 200.0f // distance max + , 0.005f // distance resolution + , 80.0f // initial value of temperature + }; + + return param; +} + +template +inline DecoderRSM1_Jumbo::DecoderRSM1_Jumbo(const RSDecoderParam& param) + : Decoder(getConstParam(), param) +{ + this->packet_duration_ = FRAME_DURATION / SINGLE_PKT_NUM; + this->angles_ready_ = true; +} + +template +inline RSEchoMode DecoderRSM1_Jumbo::getEchoMode(uint8_t mode) +{ + switch (mode) + { + case 0x00: // dual return + return RSEchoMode::ECHO_DUAL; + case 0x04: // strongest return + case 0x05: // last return + case 0x06: // first return + default: + return RSEchoMode::ECHO_SINGLE; + } +} + +template +inline void DecoderRSM1_Jumbo::decodeDifopPkt(const uint8_t* packet, size_t size) +{ + const RSM1DifopPkt& pkt = *(RSM1DifopPkt*)packet; + this->echo_mode_ = this->getEchoMode(pkt.return_mode); +} + +template +inline bool DecoderRSM1_Jumbo::decodeMsopPkt(const uint8_t* packet, size_t size) +{ + if (size != sizeof(RSM1_Jumbo)) + return false; + + constexpr static int PACKET_NUM = 63; + + const RSM1_Jumbo* jumbo = (const RSM1_Jumbo *)packet; + bool ret = false; + + for (size_t i = 0; i < PACKET_NUM; i++) + { + bool r = internDecodeMsopPkt((const uint8_t*)&(jumbo->pkts[i]), sizeof(RSM1_Jumbo_MsopPkt)); + ret = (ret || r); + } + + return ret; +} + +template +inline bool DecoderRSM1_Jumbo::internDecodeMsopPkt(const uint8_t* packet, size_t size) +{ + const RSM1_Jumbo_MsopPkt& pkt = *(RSM1_Jumbo_MsopPkt*)packet; + bool ret = false; + + if (memcmp(packet, this->const_param_.MSOP_ID, this->const_param_.MSOP_ID_LEN) != 0) + return false; + + this->temperature_ = static_cast((int)pkt.header.temperature - this->const_param_.TEMPERATURE_RES); + this->is_get_temperature_ = true; + double pkt_ts = 0; + if (this->param_.use_lidar_clock) + { + pkt_ts = parseTimeUTCWithUs(&pkt.header.timestamp) * 1e-6; + } + else + { + uint64_t ts = getTimeHost(); + + // roll back to first block to approach lidar ts as near as possible. + pkt_ts = getTimeHost() * 1e-6 - this->getPacketDuration(); + + if (this->write_pkt_ts_) + { + createTimeUTCWithUs (ts, (RSTimestampUTC*)&pkt.header.timestamp); + } + } + + uint16_t pkt_seq = ntohs(pkt.header.pkt_seq); + if (split_strategy_.newPacket(pkt_seq)) + { + this->cb_split_frame_(this->const_param_.LASER_NUM, this->cloudTs()); + this->first_point_ts_ = pkt_ts; + ret = true; + } + + for (uint16_t blk = 0; blk < this->const_param_.BLOCKS_PER_PKT; blk++) + { + const RSM1_Jumbo_Block& block = pkt.blocks[blk]; + + double point_time = pkt_ts + block.time_offset * 1e-6; + + for (uint16_t chan = 0; chan < this->const_param_.CHANNELS_PER_BLOCK; chan++) + { + const RSM1_Jumbo_Channel& channel = block.channel[chan]; + + float distance = ntohs(channel.distance) * this->const_param_.DISTANCE_RES; + + if (this->distance_section_.in(distance)) + { + int pitch = ntohs(channel.pitch) - ANGLE_OFFSET; + int yaw = ntohs(channel.yaw) - ANGLE_OFFSET; + + float x = distance * COS (pitch) * COS (yaw); + float y = distance * COS (pitch) * SIN (yaw); + float z = distance * SIN (pitch); + this->transformPoint(x, y, z); + + typename T_PointCloud::PointT point; + setX(point, x); + setY(point, y); + setZ(point, z); + setIntensity(point, channel.intensity); + setTimestamp(point, point_time); + setRing(point, chan); + + this->point_cloud_->points.emplace_back(point); + } + else if (!this->param_.dense_points) + { + typename T_PointCloud::PointT point; + setX(point, NAN); + setY(point, NAN); + setZ(point, NAN); + setIntensity(point, 0); + setTimestamp(point, point_time); + setRing(point, chan); + + this->point_cloud_->points.emplace_back(point); + } + } + + this->prev_point_ts_ = point_time; + } + + this->prev_pkt_ts_ = pkt_ts; + return ret; +} + +} // namespace lidar +} // namespace robosense diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSM2.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSM2.hpp new file mode 100644 index 0000000..17417a3 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSM2.hpp @@ -0,0 +1,264 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once + +#include + +namespace robosense +{ +namespace lidar +{ +#pragma pack(push, 1) + +typedef struct +{ + uint8_t id[4]; + uint16_t pkt_seq; + uint16_t protocol_version; + uint8_t return_mode; + uint8_t time_mode; + RSTimestampUTC timestamp; + uint8_t reserved[10]; + uint8_t lidar_type; + uint8_t temperature; +} RSM2MsopHeader; + +typedef struct +{ + uint16_t distance; + int16_t x; + int16_t y; + int16_t z; + uint8_t intensity; + uint8_t point_attribute; +} RSM2Channel; + +typedef struct +{ + uint8_t time_offset; + uint8_t return_seq; + RSM2Channel channel[5]; +} RSM2Block; + +typedef struct +{ + RSM2MsopHeader header; + RSM2Block blocks[25]; + uint8_t reserved[4]; + uint8_t crc32[4]; + uint8_t rolling_counter[2]; +} RSM2MsopPkt; + +#pragma pack(pop) + +template +class DecoderRSM2 : public Decoder +{ +public: + + constexpr static double FRAME_DURATION = 0.1; + constexpr static uint32_t SINGLE_PKT_NUM = 1260; + constexpr static int VECTOR_BASE = 32768; + + virtual void decodeDifopPkt(const uint8_t* pkt, size_t size); + virtual bool decodeMsopPkt(const uint8_t* pkt, size_t size); + virtual ~DecoderRSM2() = default; + + explicit DecoderRSM2(const RSDecoderParam& param); + +private: + + static RSDecoderConstParam& getConstParam(); + RSEchoMode getEchoMode(uint8_t mode); + + SplitStrategyBySeq split_strategy_; +}; + +template +inline RSDecoderConstParam& DecoderRSM2::getConstParam() +{ + static RSDecoderConstParam param = + { + 1342 // msop len + , 256 // difop len + , 4 // msop id len + , 8 // difop id len + , {0x55, 0xAA, 0x5A, 0xA5} // msop id + , {0xA5, 0xFF, 0x00, 0x5A, 0x11, 0x11, 0x55, 0x55} // difop id + , {0x00, 0x00} + , 5 // laser number + , 25 // blocks per packet + , 5 // channels per block + , 0.2f // distance min + , 250.0f // distance max + , 0.005f // distance resolution + , 80.0f // initial value of temperature + }; + + return param; +} + +template +inline DecoderRSM2::DecoderRSM2(const RSDecoderParam& param) + : Decoder(getConstParam(), param) +{ + this->packet_duration_ = FRAME_DURATION / SINGLE_PKT_NUM; + this->angles_ready_ = true; +} + +template +inline RSEchoMode DecoderRSM2::getEchoMode(uint8_t mode) +{ + switch (mode) + { + case 0x00: // dual return + return RSEchoMode::ECHO_DUAL; + case 0x04: // strongest return + case 0x05: // last return + case 0x06: // first return + default: + return RSEchoMode::ECHO_SINGLE; + } +} + +template +inline void DecoderRSM2::decodeDifopPkt(const uint8_t* packet, size_t size) +{ + const RSM1DifopPkt& pkt = *(RSM1DifopPkt*)packet; + this->echo_mode_ = this->getEchoMode(pkt.return_mode); + +#ifdef ENABLE_DIFOP_PARSE + // device info + memcpy (this->device_info_.sn, pkt.sn.num, 6); + memcpy (this->device_info_.mac, pkt.eth.mac_addr, 6); + memcpy (this->device_info_.top_ver, pkt.version.pl_ver, 5); + memcpy (this->device_info_.bottom_ver, pkt.version.ps_ver, 5); + this->device_info_.state = true; + // device status + this->device_status_.voltage = ntohs(pkt.status.voltage_1); + this->device_status_.state = true; + +#endif +} + +template +inline bool DecoderRSM2::decodeMsopPkt(const uint8_t* packet, size_t size) +{ + const RSM2MsopPkt& pkt = *(RSM2MsopPkt*)packet; + bool ret = false; + + this->temperature_ = static_cast((int)pkt.header.temperature - this->const_param_.TEMPERATURE_RES); + this->is_get_temperature_ = true; + double pkt_ts = 0; + if (this->param_.use_lidar_clock) + { + pkt_ts = parseTimeUTCWithUs(&pkt.header.timestamp) * 1e-6; + } + else + { + uint64_t ts = getTimeHost(); + + // roll back to first block to approach lidar ts as near as possible. + pkt_ts = getTimeHost() * 1e-6 - this->getPacketDuration(); + + if (this->write_pkt_ts_) + { + createTimeUTCWithUs (ts, (RSTimestampUTC*)&pkt.header.timestamp); + } + } + + uint16_t pkt_seq = ntohs(pkt.header.pkt_seq); + if (split_strategy_.newPacket(pkt_seq)) + { + this->cb_split_frame_(this->const_param_.LASER_NUM, this->cloudTs()); + this->first_point_ts_ = pkt_ts; + ret = true; + } + + for (uint16_t blk = 0; blk < this->const_param_.BLOCKS_PER_PKT; blk++) + { + const RSM2Block& block = pkt.blocks[blk]; + + double point_time = pkt_ts + block.time_offset * 1e-6; + + for (uint16_t chan = 0; chan < this->const_param_.CHANNELS_PER_BLOCK; chan++) + { + const RSM2Channel& channel = block.channel[chan]; + + float distance = ntohs(channel.distance) * this->const_param_.DISTANCE_RES; + + if (this->distance_section_.in(distance)) + { + int16_t vector_x = RS_SWAP_INT16(channel.x); + int16_t vector_y = RS_SWAP_INT16(channel.y); + int16_t vector_z = RS_SWAP_INT16(channel.z); + + float x = vector_x * distance / VECTOR_BASE; + float y = vector_y * distance / VECTOR_BASE; + float z = vector_z * distance / VECTOR_BASE; + + this->transformPoint(x, y, z); + + typename T_PointCloud::PointT point; + setX(point, x); + setY(point, y); + setZ(point, z); + setIntensity(point, channel.intensity); + setTimestamp(point, point_time); + setRing(point, chan); + + this->point_cloud_->points.emplace_back(point); + } + else if (!this->param_.dense_points) + { + typename T_PointCloud::PointT point; + setX(point, NAN); + setY(point, NAN); + setZ(point, NAN); + setIntensity(point, 0); + setTimestamp(point, point_time); + setRing(point, chan); + + this->point_cloud_->points.emplace_back(point); + } + } + + this->prev_point_ts_ = point_time; + } + + this->prev_pkt_ts_ = pkt_ts; + return ret; +} + +} // namespace lidar +} // namespace robosense diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSM3.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSM3.hpp new file mode 100644 index 0000000..1088321 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSM3.hpp @@ -0,0 +1,233 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once + +#include + +namespace robosense +{ +namespace lidar +{ +#pragma pack(push, 1) + +typedef struct +{ + uint16_t distance; + int16_t x; + int16_t y; + int16_t z; + uint8_t intensity; + uint8_t point_attribute; +} RSM3Channel; + +typedef struct +{ + uint16_t time_offset; + RSM3Channel channel[28]; +} RSM3Block; + +typedef struct +{ + RSM1MsopHeader header; + RSM3Block blocks[5]; + uint8_t tail[2]; + uint8_t crc32[4]; +} RSM3MsopPkt; + +typedef struct +{ + uint8_t id[8]; + RSSN sn; + uint8_t reserved1[96]; + RSTimeInfo time_info; + uint8_t reserved2[390]; +} RRSM3DifopPkt; +#pragma pack(pop) +template +class DecoderRSM3 :public Decoder +{ +public: + constexpr static double FRAME_DURATION = 0.1; + constexpr static uint32_t SINGLE_PKT_NUM = 2270; + constexpr static int VECTOR_BASE = 32768; // 2^15 + virtual bool decodeMsopPkt(const uint8_t* pkt, size_t size); + virtual void decodeDifopPkt(const uint8_t* pkt, size_t size); + virtual ~DecoderRSM3() = default; + + explicit DecoderRSM3(const RSDecoderParam& param); + +private: + + static RSDecoderConstParam& getConstParam(); + SplitStrategyBySeq split_strategy_; +}; +template +inline RSDecoderConstParam& DecoderRSM3::getConstParam() +{ + static RSDecoderConstParam param = + { + 1448 // msop len + , 512 // difop len + , 4 // msop id len + , 8 // difop id len + , {0x55, 0xAA, 0x5A, 0xA5} // msop id + , {0xA5, 0xFF, 0x00, 0x5A, 0x11, 0x11, 0x55, 0x55} // difop id + , {0x00, 0x00} + , 28 // laser number + , 5 // blocks per packet + , 28 // channels per block + , 0.1f // distance min + , 300.0f // distance max + , 0.005f // distance resolution + , 80.0f // initial value of temperature + }; + + return param; +} +template +inline DecoderRSM3::DecoderRSM3(const RSDecoderParam& param) + : Decoder(getConstParam(), param) +{ + this->packet_duration_ = FRAME_DURATION / SINGLE_PKT_NUM; + this->angles_ready_ = true; +} + +template +inline void DecoderRSM3::decodeDifopPkt(const uint8_t* packet, size_t size) +{ +#ifdef ENABLE_DIFOP_PARSE + const RRSM3DifopPkt& pkt = *(RRSM3DifopPkt*)packet; + double difop_pkt_ts = parseTimeUTCWithUs(&pkt.time_info.timestamp) * 1e-6; + if(0) + { + RS_DEBUG << "difop_pkt_ts:" << difop_pkt_ts << RS_REND; + } +#endif +} + + +template +inline bool DecoderRSM3::decodeMsopPkt(const uint8_t* packet, size_t size) +{ + + const RSM3MsopPkt& pkt = *(RSM3MsopPkt*)packet; + bool ret = false; + + this->temperature_ = static_cast((int)pkt.header.temperature - this->const_param_.TEMPERATURE_RES); + + double pkt_ts = 0; + if (this->param_.use_lidar_clock) + { + pkt_ts = parseTimeUTCWithUs(&pkt.header.timestamp) * 1e-6; + } + else + { + uint64_t ts = getTimeHost(); + + // roll back to first block to approach lidar ts as near as possible. + pkt_ts = getTimeHost() * 1e-6 - this->getPacketDuration(); + + if (this->write_pkt_ts_) + { + createTimeUTCWithUs(ts, (RSTimestampUTC*)&pkt.header.timestamp); + } + } + + uint16_t pkt_seq = ntohs(pkt.header.pkt_seq); + if (split_strategy_.newPacket(pkt_seq)) + { + + this->cb_split_frame_(this->const_param_.LASER_NUM, this->cloudTs()); + this->first_point_ts_ = pkt_ts; + ret = true; + } + + for (uint16_t blk = 0; blk < this->const_param_.BLOCKS_PER_PKT; blk++) + { + const RSM3Block& block = pkt.blocks[blk]; + + double point_time = pkt_ts + ntohs(block.time_offset) * 1e-6; + + for (uint16_t chan = 0; chan < this->const_param_.CHANNELS_PER_BLOCK; chan++) + { + const RSM3Channel& channel = block.channel[chan]; + + float distance = ntohs(channel.distance) * this->const_param_.DISTANCE_RES; + + if (this->distance_section_.in(distance)) + { + + int16_t vector_x = RS_SWAP_INT16(channel.x); + int16_t vector_y = RS_SWAP_INT16(channel.y); + int16_t vector_z = RS_SWAP_INT16(channel.z); + + float x = vector_x * distance / VECTOR_BASE; + float y = vector_y * distance / VECTOR_BASE; + float z = vector_z * distance / VECTOR_BASE; + + this->transformPoint(x, y, z); + + typename T_PointCloud::PointT point; + setX(point, x); + setY(point, y); + setZ(point, z); + setIntensity(point, channel.intensity); + setTimestamp(point, point_time); + setRing(point, chan); + + this->point_cloud_->points.emplace_back(point); + } + else if (!this->param_.dense_points) + { + typename T_PointCloud::PointT point; + setX(point, NAN); + setY(point, NAN); + setZ(point, NAN); + setIntensity(point, 0); + setTimestamp(point, point_time); + setRing(point, chan); + + this->point_cloud_->points.emplace_back(point); + } + } + + this->prev_point_ts_ = point_time; + + } + + this->prev_pkt_ts_ = pkt_ts; + return ret; +} + +} // namespace lidar +} // namespace robosense diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSMX.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSMX.hpp new file mode 100644 index 0000000..0b5a15b --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSMX.hpp @@ -0,0 +1,311 @@ +/********************************************************************************************************************* + +Copyright (c) 2020 RoboSense + +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this + +license, do not download, install, copy or use the software. + +License Agreement + +For RoboSense LiDAR SDK Library + +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used + +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. + +*********************************************************************************************************************/ + +#pragma once + +#include + +namespace robosense + +{ + +namespace lidar + +{ + +#pragma pack(push, 1) + +typedef struct + +{ + uint8_t id[4]; + uint16_t pkt_seq; + uint16_t protocol_version; + uint8_t return_mode; + uint8_t time_mode; + RSTimestampUTC timestamp; + uint8_t reserved[10]; + uint8_t lidar_type; + uint8_t temperature; +} RSMXMsopHeader; + +typedef struct + +{ + uint16_t radius_ft; + uint16_t radius_sd; + int16_t x; + int16_t y; + int16_t z; + uint8_t intensity_ft; + uint8_t intensity_sd; + uint8_t point_attribute; +} RSMXChannel; + +typedef struct + +{ + uint8_t time_offset; + RSMXChannel channel[2]; +} RSMXBlock; + +typedef struct +{ + RSMXMsopHeader header; + RSMXBlock blocks[50]; + uint8_t reserved[16]; + uint8_t crc32[4]; + uint8_t rolling_counter[2]; +} RSMXMsopPkt; + +typedef struct +{ + uint8_t id[8]; + uint8_t reserved1[30]; + RSSN sn; + uint8_t reserved2[212]; +} RSMXDifopPkt; + +#pragma pack(pop) + +template +class DecoderRSMX : public Decoder +{ +public: + + constexpr static double FRAME_DURATION = 0.1; + constexpr static uint32_t SINGLE_PKT_NUM = 630; + constexpr static int VECTOR_BASE = 32768; //2^15 + + virtual void decodeDifopPkt(const uint8_t* pkt, size_t size); + virtual bool decodeMsopPkt(const uint8_t* pkt, size_t size); + virtual ~DecoderRSMX() = default; + + explicit DecoderRSMX(const RSDecoderParam& param); + +private: + + static RSDecoderConstParam& getConstParam(); + RSEchoMode getEchoMode(uint8_t mode); + SplitStrategyBySeq split_strategy_; +}; + +template +inline RSDecoderConstParam& DecoderRSMX::getConstParam() +{ + static RSDecoderConstParam param = + { + 1404// msop len + , 256 // difop len + , 4 // msop id len + , 8 // difop id len + , {0x55, 0xAA, 0x5A, 0xA5} // msop id + , {0xA5, 0xFF, 0x00, 0x5A, 0x11, 0x11, 0x55, 0x55} // difop id + , {0x00, 0x00} //block id + , 2 // laser number + , 50 // blocks per packet + , 2 // channels per block + , 0.2f // distance min + , 200.0f // distance max + , 0.005f // distance resolution + , 80.0f // initial value of temperature + }; + + return param; +} + +template +inline DecoderRSMX::DecoderRSMX(const RSDecoderParam& param) + : Decoder(getConstParam(), param) +{ + this->packet_duration_ = FRAME_DURATION / SINGLE_PKT_NUM; + this->angles_ready_ = true; +} + +template +inline RSEchoMode DecoderRSMX::getEchoMode(const uint8_t mode) +{ + switch (mode) + { + case 0x00: // dual return + return RSEchoMode::ECHO_DUAL; + case 0x04: // strongest return + case 0x05: // last return + case 0x06: // first return + default: + return RSEchoMode::ECHO_SINGLE; + } +} + +template +inline void DecoderRSMX::decodeDifopPkt(const uint8_t* packet, size_t size) +{ + + #ifdef ENABLE_DIFOP_PARSE + const RSMXDifopPkt& pkt = *(RSMXDifopPkt*)packet; + // device info + memcpy (this->device_info_.sn, pkt.sn.num, 4); + this->device_info_.state = true; + // device status + this->device_status_.state = false; + #endif +} + +template +inline bool DecoderRSMX::decodeMsopPkt(const uint8_t* packet, size_t size) +{ + const RSMXMsopPkt& pkt = *(RSMXMsopPkt*)packet; + bool ret = false; + + this->temperature_ = static_cast((int)pkt.header.temperature - this->const_param_.TEMPERATURE_RES); + this->is_get_temperature_ = true; + double pkt_ts = 0; + if (this->param_.use_lidar_clock) + { + pkt_ts = parseTimeUTCWithUs(&pkt.header.timestamp) * 1e-6; + } + else + { + uint64_t ts = getTimeHost(); + + // roll back to first block to approach lidar ts as near as possible. + pkt_ts = getTimeHost() * 1e-6 - this->getPacketDuration(); + + if (this->write_pkt_ts_) + { + createTimeUTCWithUs (ts, (RSTimestampUTC*)&pkt.header.timestamp); + } + } + + + uint16_t pkt_seq = ntohs(pkt.header.pkt_seq); + if (split_strategy_.newPacket(pkt_seq)) + { + this->cb_split_frame_(this->const_param_.LASER_NUM, this->cloudTs()); + this->first_point_ts_ = pkt_ts; + ret = true; + } + + uint8_t loop_time; + if(pkt.header.return_mode == 0x0) + { + loop_time = 2; // dual return + }else + { + loop_time = 1; // single return + } + + for (uint16_t blk = 0; blk < this->const_param_.BLOCKS_PER_PKT; blk++) + { + const RSMXBlock& block = pkt.blocks[blk]; + + double point_time = pkt_ts + block.time_offset * 1e-6; + for (uint16_t chan = 0; chan < this->const_param_.CHANNELS_PER_BLOCK; chan++) + { + const RSMXChannel& channel = block.channel[chan]; + + float distance; + uint8_t intensity; + + for(int i = 0; i < loop_time; i++) + { + if(i == 0) // single return + { + distance = ntohs(channel.radius_ft) * this->const_param_.DISTANCE_RES; + intensity = channel.intensity_ft; + }else // dual return + { + distance = ntohs(channel.radius_sd) * this->const_param_.DISTANCE_RES; + intensity = channel.intensity_sd; + } + + if (this->distance_section_.in(distance)) + { + uint16_t vector_x = RS_SWAP_INT16(channel.x); + int16_t vector_y = RS_SWAP_INT16(channel.y); + int16_t vector_z = RS_SWAP_INT16(channel.z); + + float x = vector_x * distance / VECTOR_BASE; + float y = vector_y * distance / VECTOR_BASE; + float z = vector_z * distance / VECTOR_BASE; + + this->transformPoint(x, y, z); + + typename T_PointCloud::PointT point; + setX(point, x); + setY(point, y); + setZ(point, z); + setIntensity(point, intensity); + setTimestamp(point, point_time); + setRing(point, chan); + + this->point_cloud_->points.emplace_back(point); + } + else if (!this->param_.dense_points) + { + typename T_PointCloud::PointT point; + setX(point, NAN); + setY(point, NAN); + setZ(point, NAN); + setIntensity(point, 0); + setTimestamp(point, point_time); + setRing(point, chan); + + this->point_cloud_->points.emplace_back(point); + } + } + } + + this->prev_point_ts_ = point_time; + } + + this->prev_pkt_ts_ = pkt_ts; + return ret; +} +} // namespace lidar +} // namespace robosense diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSP128.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSP128.hpp new file mode 100644 index 0000000..bef3c54 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSP128.hpp @@ -0,0 +1,338 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once +#include + +namespace robosense +{ +namespace lidar +{ +#pragma pack(push, 1) +typedef struct +{ + uint8_t id[1]; + uint8_t ret_id; + uint16_t azimuth; + RSChannel channels[128]; +} RSP128MsopBlock; + +typedef struct +{ + RSMsopHeaderV2 header; + RSP128MsopBlock blocks[3]; + uint8_t reserved[4]; +} RSP128MsopPkt; + +typedef struct +{ + uint8_t id[8]; + uint16_t rpm; + RSEthNetV2 eth; + RSFOV fov; + uint8_t reserved1[2]; + uint16_t phase_lock_angle; + RSVersionV2 version; + uint8_t reserved2[229]; + RSSN sn; + uint16_t zero_cali; + uint8_t return_mode; + RSTimeInfo time_info; + RSStatusV1 status; + uint8_t reserved3[5]; + RSDiagnoV1 diagno; + uint8_t gprmc[86]; + RSCalibrationAngle vert_angle_cali[128]; + RSCalibrationAngle horiz_angle_cali[128]; + uint8_t reserved4[10]; + uint16_t tail; +} RSP128DifopPkt; + +#pragma pack(pop) + +template +class DecoderRSP128 : public DecoderMech +{ +public: + virtual void decodeDifopPkt(const uint8_t* pkt, size_t size); + virtual bool decodeMsopPkt(const uint8_t* pkt, size_t size); + virtual ~DecoderRSP128() = default; + + explicit DecoderRSP128(const RSDecoderParam& param); + virtual bool isNewFrame(const uint8_t* packet) override; +#ifndef UNIT_TEST +protected: +#endif + + static RSDecoderMechConstParam& getConstParam(); + static RSEchoMode getEchoMode(uint8_t mode); + + template + bool internDecodeMsopPkt(const uint8_t* pkt, size_t size); +}; + +template +inline RSDecoderMechConstParam& DecoderRSP128::getConstParam() +{ + static RSDecoderMechConstParam param = + { + { + 1248 // msop len + , 1248 // difop len + , 4 // msop id len + , 8 // difop id len + , {0x55, 0xAA, 0x05, 0x5A} // msop id + , {0xA5, 0xFF, 0x00, 0x5A, 0x11, 0x11, 0x55, 0x55} // difop id + , {0xFE} // block id + , 128 // laser number + , 3 // blocks per packet + , 128 // channels per block + , 0.4f // distance min + , 250.0f // distance max + , 0.005f // distance resolution + , 0.0625f // temperature resolution + } + // lens center + , 0.02892f // RX + , -0.013f // RY + , 0.0f // RZ + }; + + INIT_ONLY_ONCE(); + + float blk_ts = 55.56f; + float firing_tss[] = + { + 0.0f, 0.0f, 0.0f, 0.0f, 1.13f, 1.13f, 1.13f, 1.13f, + 2.13f, 2.13f, 2.13f, 2.13f, 3.26, 3.26f, 3.26f, 3.26f, + 4.26f, 4.26f, 4.26f, 4.26f, 5.38f, 5.38f, 5.38f, 5.38f, + 6.38f, 6.38f, 6.38f, 6.38f, 7.51f, 7.51f, 7.51f, 7.51f, + + 8.51f, 8.51f, 8.51f, 8.51f, 10.01f, 10.01f, 10.01f, 10.01f, + 11.38f, 11.38f, 11.38f, 11.38f, 13.31f, 13.31f, 13.31f, 13.31f, + 15.11f, 15.11f, 15.11f, 15.11f, 17.04f, 17.04f, 17.04f, 17.04f, + 18.85f, 18.85f, 18.85f, 18.85f, 21.14f, 21.14f, 21.14f, 21.14f, + + 21.14f, 23.31f, 23.31f, 23.31f, 23.31f, 25.61f, 25.61f, 25.61f, + 25.61f, 27.78f, 27.78f, 27.78f, 27.78f, 30.07f, 30.07f, 30.07f, + 30.07f, 32.24f, 32.24f, 32.24f, 32.24f, 34.54f, 34.54f, 34.54f, + 34.54f, 36.7f, 36.7f, 36.7f, 36.7f, 38.64f, 38.64f, 38.64f, + + 38.64f, 40.44f, 40.44f, 40.44f, 40.44f, 41.94f, 41.94f, 41.94f, + 41.94f, 43.3f, 43.3f, 43.3f, 43.3f, 44.8f, 44.8f, 44.8f, + 44.8f, 46.17f, 46.17f, 46.17f, 46.17f, 47.66f, 47.66f, 47.66f, + 47.66f, 49.03f, 49.03f, 49.03f, 49.03f, 50.53f, 50.53f, 53.771f + }; + + param.BLOCK_DURATION = blk_ts / 1000000; + for (uint16_t i = 0; i < sizeof(firing_tss)/sizeof(firing_tss[0]); i++) + { + param.CHAN_TSS[i] = (double)firing_tss[i] / 1000000; + param.CHAN_AZIS[i] = firing_tss[i] / blk_ts; + } + + return param; +} + +template +inline RSEchoMode DecoderRSP128::getEchoMode(uint8_t mode) +{ + switch (mode) + { + case 0x00: + case 0x01: + case 0x02: + return RSEchoMode::ECHO_SINGLE; + case 0x03: + case 0x04: + case 0x05: + default: + return RSEchoMode::ECHO_DUAL; + } +} + +template +inline void DecoderRSP128::decodeDifopPkt(const uint8_t* packet, size_t size) +{ + const RSP128DifopPkt& pkt = *(const RSP128DifopPkt*)(packet); + this->template decodeDifopCommon(pkt); + + this->echo_mode_ = getEchoMode (pkt.return_mode); + this->split_blks_per_frame_ = (this->echo_mode_ == RSEchoMode::ECHO_DUAL) ? + (this->blks_per_frame_ << 1) : this->blks_per_frame_; +} + +template +inline DecoderRSP128::DecoderRSP128(const RSDecoderParam& param) + : DecoderMech(getConstParam(), param) +{ +} + +template +inline bool DecoderRSP128::decodeMsopPkt(const uint8_t* pkt, size_t size) +{ + if (this->echo_mode_ == RSEchoMode::ECHO_SINGLE) + { + return internDecodeMsopPkt>(pkt, size); + } + else + { + return internDecodeMsopPkt>(pkt, size); + } +} + +template +template +inline bool DecoderRSP128::internDecodeMsopPkt(const uint8_t* packet, size_t size) +{ + const RSP128MsopPkt& pkt = *(const RSP128MsopPkt*)(packet); + bool ret = false; + + this->temperature_ = parseTempInBe(&(pkt.header.temp)) * this->const_param_.TEMPERATURE_RES; + this->is_get_temperature_ = true; + double pkt_ts = 0; + if (this->param_.use_lidar_clock) + { + pkt_ts = parseTimeUTCWithUs(&pkt.header.timestamp) * 1e-6; + } + else + { + uint64_t ts = getTimeHost(); + + // roll back to first block to approach lidar ts as near as possible. + pkt_ts = ts * 1e-6 - this->getPacketDuration(); + + if (this->write_pkt_ts_) + { + createTimeUTCWithUs (ts, (RSTimestampUTC*)&pkt.header.timestamp); + } + } + + T_BlockIterator iter(pkt, this->const_param_.BLOCKS_PER_PKT, this->mech_const_param_.BLOCK_DURATION, + this->block_az_diff_, this->fov_blind_ts_diff_); + + for (uint16_t blk = 0; blk < this->const_param_.BLOCKS_PER_PKT; blk++) + { + const RSP128MsopBlock& block = pkt.blocks[blk]; + + if (memcmp(this->const_param_.BLOCK_ID, block.id, 1) != 0) + { + this->cb_excep_(Error(ERRCODE_WRONGMSOPBLKID)); + break; + } + + int32_t block_az_diff; + double block_ts_off; + iter.get(blk, block_az_diff, block_ts_off); + + double block_ts = pkt_ts + block_ts_off; + int32_t block_az = ntohs(block.azimuth); + if (this->split_strategy_->newBlock(block_az)) + { + this->cb_split_frame_(this->const_param_.LASER_NUM, this->cloudTs()); + this->first_point_ts_ = block_ts; + ret = true; + } + + for (uint16_t chan = 0; chan < this->const_param_.CHANNELS_PER_BLOCK; chan++) + { + const RSChannel& channel = block.channels[chan]; + + double chan_ts = block_ts + this->mech_const_param_.CHAN_TSS[chan]; + int32_t angle_horiz = block_az + + (int32_t)((float)block_az_diff * this->mech_const_param_.CHAN_AZIS[chan]); + + int32_t angle_vert = this->chan_angles_.vertAdjust(chan); + int32_t angle_horiz_final = this->chan_angles_.horizAdjust(chan, angle_horiz); + float distance = ntohs(channel.distance) * this->const_param_.DISTANCE_RES; + + if (this->distance_section_.in(distance) && this->scan_section_.in(angle_horiz_final)) + { + float x = distance * COS(angle_vert) * COS(angle_horiz_final) + this->mech_const_param_.RX * COS(angle_horiz); + float y = -distance * COS(angle_vert) * SIN(angle_horiz_final) - this->mech_const_param_.RX * SIN(angle_horiz); + float z = distance * SIN(angle_vert) + this->mech_const_param_.RZ; + this->transformPoint(x, y, z); + + typename T_PointCloud::PointT point; + setX(point, x); + setY(point, y); + setZ(point, z); + setIntensity(point, channel.intensity); + setTimestamp(point, chan_ts); + setRing(point, this->chan_angles_.toUserChan(chan)); + + this->point_cloud_->points.emplace_back(point); + } + else if (!this->param_.dense_points) + { + typename T_PointCloud::PointT point; + setX(point, NAN); + setY(point, NAN); + setZ(point, NAN); + setIntensity(point, 0); + setTimestamp(point, chan_ts); + setRing(point, this->chan_angles_.toUserChan(chan)); + + this->point_cloud_->points.emplace_back(point); + } + + this->prev_point_ts_ = chan_ts; + } + } + + this->prev_pkt_ts_ = pkt_ts; + return ret; +} +template +inline bool DecoderRSP128::isNewFrame(const uint8_t* packet) +{ + const RSP128MsopPkt& pkt = *(const RSP128MsopPkt*)(packet); + for (uint16_t blk = 0; blk < this->const_param_.BLOCKS_PER_PKT; blk++) + { + const RSP128MsopBlock& block = pkt.blocks[blk]; + + if (memcmp(this->const_param_.BLOCK_ID, block.id, 1) != 0) + { + break; + } + + int32_t block_az = ntohs(block.azimuth); + if (this->pre_split_strategy_->newBlock(block_az)) + { + return true; + } + } + + return false; +} +} // namespace lidar +} // namespace robosense diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSP48.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSP48.hpp new file mode 100644 index 0000000..983c26d --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSP48.hpp @@ -0,0 +1,341 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once + +#include + +namespace robosense +{ +namespace lidar +{ +#pragma pack(push, 1) +typedef struct +{ + uint8_t id[1]; + uint8_t ret_id; + uint16_t azimuth; + RSChannel channels[48]; +} RSP48MsopBlock; + +typedef struct +{ + RSMsopHeaderV2 header; + RSP48MsopBlock blocks[8]; + uint8_t reserved[4]; +} RSP48MsopPkt; + +typedef struct +{ + uint8_t id[8]; + uint16_t rpm; + RSEthNetV2 eth; + RSFOV fov; + uint8_t reserved1[2]; + uint16_t phase_lock_angle; + RSVersionV2 version; + uint8_t reserved2[229]; + RSSN sn; + uint16_t zero_cali; + uint8_t return_mode; + RSTimeInfo time_info; + RSStatusV1 status; + uint8_t reserved3[5]; + RSDiagnoV1 diagno; + uint8_t gprmc[86]; + RSCalibrationAngle vert_angle_cali[128]; + RSCalibrationAngle horiz_angle_cali[128]; + uint8_t reserved4[10]; + uint16_t tail; +} RSP48DifopPkt; + +#pragma pack(pop) + +template +class DecoderRSP48 : public DecoderMech +{ +public: + virtual void decodeDifopPkt(const uint8_t* pkt, size_t size); + virtual bool decodeMsopPkt(const uint8_t* pkt, size_t size); + virtual ~DecoderRSP48() = default; + + explicit DecoderRSP48(const RSDecoderParam& param); + virtual bool isNewFrame(const uint8_t* packet) override; +#ifndef UNIT_TEST +protected: +#endif + + static RSDecoderMechConstParam& getConstParam(); + static RSEchoMode getEchoMode(uint8_t mode); + + template + bool internDecodeMsopPkt(const uint8_t* pkt, size_t size); +}; + +template +inline RSDecoderMechConstParam& DecoderRSP48::getConstParam() +{ + static RSDecoderMechConstParam param = { + { + 1268 // msop len + , + 1248 // difop len + , + 4 // msop id len + , + 8 // difop id len + , + { 0x55, 0xAA, 0x05, 0x5A } // msop id + , + { 0xA5, 0xFF, 0x00, 0x5A, 0x11, 0x11, 0x55, 0x55 } // difop id + , + { 0xFE } // block id + , + 48 // laser number + , + 8 // blocks per packet + , + 48 // channels per block + , + 0.4f // distance min + , + 250.0f // distance max + , + 0.005f // distance resolution + , + 0.0625f // temperature resolution + } // lens center + , + 0.02892f // RX + , + -0.013f // RY + , + 0.0f // RZ + }; + + INIT_ONLY_ONCE(); + + float blk_ts = 55.56f; + float firing_tss[48] = { + 0.0f, 0.0f, 1.217f, 1.217f, 2.434f, 2.434f, 3.652f, 3.652f, 4.869f, 4.869f, 6.086f, + 6.086f, 7.304f, 7.304f, 8.521f, 8.521f, 9.739f, 9.739f, 11.323f, 11.323f, 12.907f, 12.907f, + 14.924f, 14.924f, 16.941f, 16.941f, 18.959f, 18.959f, 20.976f, 20.976f, 23.127f, 23.127f, + + 25.278f, 25.278f, 27.428f, 27.428f, 29.579f, 29.579f, 31.963f, 31.963f, 34.347f, 34.347f, 36.498f, + 36.498f, 38.648f, 38.648f, 40.666f, 40.666f, + }; + + param.BLOCK_DURATION = blk_ts / 1000000; + for (uint16_t i = 0; i < sizeof(firing_tss) / sizeof(firing_tss[0]); i++) + { + param.CHAN_TSS[i] = (double)firing_tss[i] / 1000000; + param.CHAN_AZIS[i] = firing_tss[i] / blk_ts; + } + + return param; +} + +template +inline RSEchoMode DecoderRSP48::getEchoMode(uint8_t mode) +{ + switch (mode) + { + case 0x00: + case 0x01: + case 0x02: + return RSEchoMode::ECHO_SINGLE; + case 0x03: + case 0x04: + case 0x05: + default: + return RSEchoMode::ECHO_DUAL; + } +} + +template +inline void DecoderRSP48::decodeDifopPkt(const uint8_t* packet, size_t size) +{ + const RSP48DifopPkt& pkt = *(const RSP48DifopPkt*)(packet); + this->template decodeDifopCommon(pkt); + + this->echo_mode_ = getEchoMode(pkt.return_mode); + this->split_blks_per_frame_ = + (this->echo_mode_ == RSEchoMode::ECHO_DUAL) ? (this->blks_per_frame_ << 1) : this->blks_per_frame_; +} + +template +inline DecoderRSP48::DecoderRSP48(const RSDecoderParam& param) + : DecoderMech(getConstParam(), param) +{ +} + +template +inline bool DecoderRSP48::decodeMsopPkt(const uint8_t* pkt, size_t size) +{ + if (this->echo_mode_ == RSEchoMode::ECHO_SINGLE) + { + return internDecodeMsopPkt>(pkt, size); + } + else + { + return internDecodeMsopPkt>(pkt, size); + } +} + +template +template +inline bool DecoderRSP48::internDecodeMsopPkt(const uint8_t* packet, size_t size) +{ + const RSP48MsopPkt& pkt = *(const RSP48MsopPkt*)(packet); + bool ret = false; + + this->temperature_ = parseTempInBe(&(pkt.header.temp)) * this->const_param_.TEMPERATURE_RES; + this->is_get_temperature_ = true; + double pkt_ts = 0; + if (this->param_.use_lidar_clock) + { + pkt_ts = parseTimeUTCWithUs(&pkt.header.timestamp) * 1e-6; + } + else + { + uint64_t ts = getTimeHost(); + + // roll back to first block to approach lidar ts as near as possible. + pkt_ts = ts * 1e-6 - this->getPacketDuration(); + + if (this->write_pkt_ts_) + { + createTimeUTCWithUs(ts, (RSTimestampUTC*)&pkt.header.timestamp); + } + } + + T_BlockIterator iter(pkt, this->const_param_.BLOCKS_PER_PKT, this->mech_const_param_.BLOCK_DURATION, + this->block_az_diff_, this->fov_blind_ts_diff_); + + for (uint16_t blk = 0; blk < this->const_param_.BLOCKS_PER_PKT; blk++) + { + const RSP48MsopBlock& block = pkt.blocks[blk]; + + if (memcmp(this->const_param_.BLOCK_ID, block.id, 1) != 0) + { + this->cb_excep_(Error(ERRCODE_WRONGMSOPBLKID)); + break; + } + + int32_t block_az_diff; + double block_ts_off; + iter.get(blk, block_az_diff, block_ts_off); + + double block_ts = pkt_ts + block_ts_off; + int32_t block_az = ntohs(block.azimuth); + if (this->split_strategy_->newBlock(block_az)) + { + this->cb_split_frame_(this->const_param_.LASER_NUM, this->cloudTs()); + this->first_point_ts_ = block_ts; + ret = true; + } + + for (uint16_t chan = 0; chan < this->const_param_.CHANNELS_PER_BLOCK; chan++) + { + const RSChannel& channel = block.channels[chan]; + + double chan_ts = block_ts + this->mech_const_param_.CHAN_TSS[chan]; + int32_t angle_horiz = block_az + (int32_t)((float)block_az_diff * this->mech_const_param_.CHAN_AZIS[chan]); + + int32_t angle_vert = this->chan_angles_.vertAdjust(chan); + int32_t angle_horiz_final = this->chan_angles_.horizAdjust(chan, angle_horiz); + float distance = ntohs(channel.distance) * this->const_param_.DISTANCE_RES; + + if (this->distance_section_.in(distance) && this->scan_section_.in(angle_horiz_final)) + { + float x = distance * COS(angle_vert) * COS(angle_horiz_final) + this->mech_const_param_.RX * COS(angle_horiz); + float y = -distance * COS(angle_vert) * SIN(angle_horiz_final) - this->mech_const_param_.RX * SIN(angle_horiz); + float z = distance * SIN(angle_vert) + this->mech_const_param_.RZ; + this->transformPoint(x, y, z); + + typename T_PointCloud::PointT point; + setX(point, x); + setY(point, y); + setZ(point, z); + setIntensity(point, channel.intensity); + setTimestamp(point, chan_ts); + setRing(point, this->chan_angles_.toUserChan(chan)); + + this->point_cloud_->points.emplace_back(point); + } + else if (!this->param_.dense_points) + { + typename T_PointCloud::PointT point; + setX(point, NAN); + setY(point, NAN); + setZ(point, NAN); + setIntensity(point, 0); + setTimestamp(point, chan_ts); + setRing(point, this->chan_angles_.toUserChan(chan)); + + this->point_cloud_->points.emplace_back(point); + } + + this->prev_point_ts_ = chan_ts; + } + } + + this->prev_pkt_ts_ = pkt_ts; + return ret; +} + +template +inline bool DecoderRSP48::isNewFrame(const uint8_t* packet) +{ + const RSP48MsopPkt& pkt = *(const RSP48MsopPkt*)(packet); + + for (uint16_t blk = 0; blk < this->const_param_.BLOCKS_PER_PKT; blk++) + { + const RSP48MsopBlock& block = pkt.blocks[blk]; + + if (memcmp(this->const_param_.BLOCK_ID, block.id, 1) != 0) + { + break; + } + + int32_t block_az = ntohs(block.azimuth); + if (this->pre_split_strategy_->newBlock(block_az)) + { + return true; + } + } + + return false; +} + +} // namespace lidar +} // namespace robosense diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSP80.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSP80.hpp new file mode 100644 index 0000000..ac82169 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSP80.hpp @@ -0,0 +1,372 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once + +#include + +namespace robosense +{ +namespace lidar +{ +#pragma pack(push, 1) +typedef struct +{ + uint8_t id[1]; + uint8_t ret_id; + uint16_t azimuth; + RSChannel channels[80]; +} RSP80MsopBlock; + +typedef struct +{ + RSMsopHeaderV2 header; + RSP80MsopBlock blocks[4]; + uint8_t reserved[192]; +} RSP80MsopPkt; + +typedef struct +{ + uint8_t id[8]; + uint16_t rpm; + RSEthNetV2 eth; + RSFOV fov; + uint8_t reserved1[2]; + uint16_t phase_lock_angle; + RSVersionV2 version; + uint8_t reserved2[229]; + RSSN sn; + uint16_t zero_cali; + uint8_t return_mode; + RSTimeInfo time_info; + RSStatusV1 status; + uint8_t reserved3[5]; + RSDiagnoV1 diagno; + uint8_t gprmc[86]; + RSCalibrationAngle vert_angle_cali[128]; + RSCalibrationAngle horiz_angle_cali[128]; + uint8_t reserved4[10]; + uint16_t tail; +} RSP80DifopPkt; + +#pragma pack(pop) + +template +class DecoderRSP80 : public DecoderMech +{ +public: + virtual void decodeDifopPkt(const uint8_t* pkt, size_t size); + virtual bool decodeMsopPkt(const uint8_t* pkt, size_t size); + virtual ~DecoderRSP80() = default; + + explicit DecoderRSP80(const RSDecoderParam& param); + virtual bool isNewFrame(const uint8_t* packet) override; +#ifndef UNIT_TEST +protected: +#endif + + static RSDecoderMechConstParam& getConstParam(); + static RSEchoMode getEchoMode(uint8_t mode); + + void calcParam(); + template + bool internDecodeMsopPkt(const uint8_t* pkt, size_t size); + + uint8_t lidar_model_; +}; + +template +inline RSDecoderMechConstParam& DecoderRSP80::getConstParam() +{ + static RSDecoderMechConstParam param = + { + { + 1248 // msop len + , 1248 // difop len + , 4 // msop id len + , 8 // difop id len + , {0x55, 0xAA, 0x05, 0x5A} // msop id + , {0xA5, 0xFF, 0x00, 0x5A, 0x11, 0x11, 0x55, 0x55} // difop id + , {0xFE} // block id + , 80 // laser number + , 4 // blocks per packet + , 80 // channels per block + , 0.4f // distance min + , 250.0f // distance max + , 0.005f // distance resolution + , 0.0625f // temperature resolution + } + // lens center + , 0.02892f // RX + , -0.013f // RY + , 0.0f // RZ + }; + + float blk_ts = 55.56f; + param.BLOCK_DURATION = blk_ts / 1000000; + + return param; +} + +template +inline void DecoderRSP80::calcParam() +{ + float blk_ts = 55.56f; + + float firing_tss_80[80] = + { + 0.0f, 0.0f, 0.0f, 0.0f, 1.217f, 1.217f, 1.217f, 1.217f, + 2.434f, 2.434f, 3.652f, 3.652f, 3.652f, 4.869f, 4.869f, 6.086f, + 6.086f, 7.304f, 7.304f, 8.521f, 8.521f, 9.739f, 9.739f, 11.323f, + 11.323f, 12.907f, 12.907f, 14.924f, 14.924f, 16.941f, 16.941f, 16.941f, + + 16.941f, 18.959f, 18.959f, 18.959f, 18.959f, 20.976f, 20.976f, 20.976f, + 20.976f, 23.127f, 23.127f, 23.127f, 23.127f, 25.278f, 25.278f, 25.278f, + 25.278f, 27.428f, 27.428f, 27.428f, 27.428f, 29.579f, 29.579f, 29.579f, + 29.579f, 31.963f, 31.963f, 31.963f, 31.963f, 34.347f, 34.347f, 34.347f, + + 34.347f, 36.498f, 36.498f, 36.498f, 36.498f, 38.648f, 38.648f, 40.666f, + 40.666f, 42.683f, 50.603f, 52.187f, 52.187f, 52.187f, 53.771f, 53.771f, + }; + + float firing_tss_80v[80] = + { + 0.0f, 0.0f, 0.0f, 0.0f, 1.217f, 1.217f, 1.217f, 1.217f, + 2.434f, 2.434f, 2.434f, 2.434f, 3.652f, 3.652f, 3.652f, 3.652f, + 4.869f, 4.869f, 4.869f, 4.869f, 6.086f, 6.086f, 6.086f, 6.086f, + 7.304f, 7.304f, 7.304f, 7.304f, 8.521f, 8.521f, 8.521f, 8.521f, + + 9.739f, 9.739f, 9.739f, 9.739f, 11.323f, 11.323f, 11.323f, 11.323f, + 12.907f, 12.907f, 12.907f, 12.907f, 14.924f, 14.924f, 14.924f, 14.924f, + 16.941f, 16.941f, 16.941f, 16.941f, 18.959f, 18.959f, 18.959f, 18.959f, + 20.976f, 20.976f, 20.976f, 20.976f, 23.127f, 23.127f, 23.127f, 23.127f, + + 25.278f, 25.278f, 25.278f, 25.278f, 27.428f, 27.428f, 27.428f, 27.428f, + 29.579f, 29.579f, 29.579f, 29.579f, 31.963f, 31.963f, 31.963f, 31.963f, + }; + + float* firing_tss = firing_tss_80; // 80 - lidar_model = 0x02 + if (lidar_model_ == 0x03) // 80v - lidar_model = 0x03 + { + firing_tss = firing_tss_80v; + } + + RSDecoderMechConstParam& param = this->mech_const_param_; + for (uint16_t i = 0; i < 80; i++) + { + param.CHAN_TSS[i] = (double)firing_tss[i] / 1000000; + param.CHAN_AZIS[i] = firing_tss[i] / blk_ts; + } +} + +template +inline RSEchoMode DecoderRSP80::getEchoMode(uint8_t mode) +{ + switch (mode) + { + case 0x00: + case 0x01: + case 0x02: + return RSEchoMode::ECHO_SINGLE; + case 0x03: + case 0x04: + case 0x05: + default: + return RSEchoMode::ECHO_DUAL; + } +} + +template +inline void DecoderRSP80::decodeDifopPkt(const uint8_t* packet, size_t size) +{ + const RSP80DifopPkt& pkt = *(const RSP80DifopPkt*)(packet); + this->template decodeDifopCommon(pkt); + + this->echo_mode_ = getEchoMode (pkt.return_mode); + this->split_blks_per_frame_ = (this->echo_mode_ == RSEchoMode::ECHO_DUAL) ? + (this->blks_per_frame_ << 1) : this->blks_per_frame_; +} + +template +inline DecoderRSP80::DecoderRSP80(const RSDecoderParam& param) + : DecoderMech(getConstParam(), param), + lidar_model_(0) +{ +} + +template +inline bool DecoderRSP80::decodeMsopPkt(const uint8_t* pkt, size_t size) +{ + if (this->echo_mode_ == RSEchoMode::ECHO_SINGLE) + { + return internDecodeMsopPkt>(pkt, size); + } + else + { + return internDecodeMsopPkt>(pkt, size); + } +} + +template +template +inline bool DecoderRSP80::internDecodeMsopPkt(const uint8_t* packet, size_t size) +{ + const RSP80MsopPkt& pkt = *(const RSP80MsopPkt*)(packet); + bool ret = false; + + uint8_t lidar_model = pkt.header.lidar_model; + if (lidar_model_ != lidar_model) + { + lidar_model_ = lidar_model; + calcParam(); + } + + this->temperature_ = parseTempInBe(&(pkt.header.temp)) * this->const_param_.TEMPERATURE_RES; + this->is_get_temperature_ = true; + double pkt_ts = 0; + if (this->param_.use_lidar_clock) + { + pkt_ts = parseTimeUTCWithUs(&pkt.header.timestamp) * 1e-6; + } + else + { + uint64_t ts = getTimeHost(); + + // roll back to first block to approach lidar ts as near as possible. + pkt_ts = ts * 1e-6 - this->getPacketDuration(); + + if (this->write_pkt_ts_) + { + createTimeUTCWithUs (ts, (RSTimestampUTC*)&pkt.header.timestamp); + } + } + + T_BlockIterator iter(pkt, this->const_param_.BLOCKS_PER_PKT, this->mech_const_param_.BLOCK_DURATION, + this->block_az_diff_, this->fov_blind_ts_diff_); + + for (uint16_t blk = 0; blk < this->const_param_.BLOCKS_PER_PKT; blk++) + { + const RSP80MsopBlock& block = pkt.blocks[blk]; + + if (memcmp(this->const_param_.BLOCK_ID, block.id, 1) != 0) + { + this->cb_excep_(Error(ERRCODE_WRONGMSOPBLKID)); + break; + } + + int32_t block_az_diff; + double block_ts_off; + iter.get(blk, block_az_diff, block_ts_off); + + double block_ts = pkt_ts + block_ts_off; + int32_t block_az = ntohs(block.azimuth); + if (this->split_strategy_->newBlock(block_az)) + { + this->cb_split_frame_(this->const_param_.LASER_NUM, this->cloudTs()); + this->first_point_ts_ = block_ts; + ret = true; + } + + for (uint16_t chan = 0; chan < this->const_param_.CHANNELS_PER_BLOCK; chan++) + { + const RSChannel& channel = block.channels[chan]; + + double chan_ts = block_ts + this->mech_const_param_.CHAN_TSS[chan]; + int32_t angle_horiz = block_az + + (int32_t)((float)block_az_diff * this->mech_const_param_.CHAN_AZIS[chan]); + + int32_t angle_vert = this->chan_angles_.vertAdjust(chan); + int32_t angle_horiz_final = this->chan_angles_.horizAdjust(chan, angle_horiz); + float distance = ntohs(channel.distance) * this->const_param_.DISTANCE_RES; + + if (this->distance_section_.in(distance) && this->scan_section_.in(angle_horiz_final)) + { + float x = distance * COS(angle_vert) * COS(angle_horiz_final) + this->mech_const_param_.RX * COS(angle_horiz); + float y = -distance * COS(angle_vert) * SIN(angle_horiz_final) - this->mech_const_param_.RX * SIN(angle_horiz); + float z = distance * SIN(angle_vert) + this->mech_const_param_.RZ; + this->transformPoint(x, y, z); + + typename T_PointCloud::PointT point; + setX(point, x); + setY(point, y); + setZ(point, z); + setIntensity(point, channel.intensity); + setTimestamp(point, chan_ts); + setRing(point, this->chan_angles_.toUserChan(chan)); + + this->point_cloud_->points.emplace_back(point); + } + else if (!this->param_.dense_points) + { + typename T_PointCloud::PointT point; + setX(point, NAN); + setY(point, NAN); + setZ(point, NAN); + setIntensity(point, 0); + setTimestamp(point, chan_ts); + setRing(point, this->chan_angles_.toUserChan(chan)); + + this->point_cloud_->points.emplace_back(point); + } + + this->prev_point_ts_ = chan_ts; + } + } + + this->prev_pkt_ts_ = pkt_ts; + return ret; +} +template +inline bool DecoderRSP80::isNewFrame(const uint8_t* packet) +{ + const RSP80MsopPkt& pkt = *(const RSP80MsopPkt*)(packet); + + for (uint16_t blk = 0; blk < this->const_param_.BLOCKS_PER_PKT; blk++) + { + const RSP80MsopBlock& block = pkt.blocks[blk]; + + if (memcmp(this->const_param_.BLOCK_ID, block.id, 1) != 0) + { + break; + } + + int32_t block_az = ntohs(block.azimuth); + if (this->pre_split_strategy_->newBlock(block_az)) + { + return true; + } + } + + return false; +} +} // namespace lidar +} // namespace robosense diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_factory.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_factory.hpp new file mode 100644 index 0000000..a34fe03 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_factory.hpp @@ -0,0 +1,140 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace robosense +{ +namespace lidar +{ + +template +class DecoderFactory +{ +public: + + static std::shared_ptr> createDecoder( + LidarType type, const RSDecoderParam& param); +}; + +template +inline std::shared_ptr> DecoderFactory::createDecoder( + LidarType type, const RSDecoderParam& param) +{ + std::shared_ptr> ret_ptr; + + switch (type) + { + case LidarType::RS16: + ret_ptr = std::make_shared>(param); + break; + case LidarType::RS32: + ret_ptr = std::make_shared>(param); + break; + case LidarType::RSBP: + ret_ptr = std::make_shared>(param); + break; + case LidarType::RSAIRY: + ret_ptr = std::make_shared>(param); + break; + case LidarType::RSHELIOS: + ret_ptr = std::make_shared>(param); + break; + case LidarType::RSHELIOS_16P: + ret_ptr = std::make_shared>(param); + break; + case LidarType::RS128: + ret_ptr = std::make_shared>(param); + break; + case LidarType::RS80: + ret_ptr = std::make_shared>(param); + break; + case LidarType::RS48: + ret_ptr = std::make_shared>(param); + break; + case LidarType::RSP128: + ret_ptr = std::make_shared>(param); + break; + case LidarType::RSP80: + ret_ptr = std::make_shared>(param); + break; + case LidarType::RSP48: + ret_ptr = std::make_shared>(param); + break; + case LidarType::RSM1: + ret_ptr = std::make_shared>(param); + break; + case LidarType::RSM2: + ret_ptr = std::make_shared>(param); + break; + case LidarType::RSM3: + ret_ptr = std::make_shared>(param); + break; + case LidarType::RSE1: + ret_ptr = std::make_shared>(param); + break; + case LidarType::RSM1_JUMBO: + ret_ptr = std::make_shared>(param); + break; + case LidarType::RSMX: + ret_ptr = std::make_shared>(param); + break; + default: + RS_ERROR << "Wrong LiDAR Type. Please check your LiDAR Version! " << RS_REND; + exit(-1); + } + + return ret_ptr; +} + +} // namespace lidar +} // namespace robosense diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_mech.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_mech.hpp new file mode 100644 index 0000000..65f09cb --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_mech.hpp @@ -0,0 +1,219 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once +#include +#include +#include + +namespace robosense +{ +namespace lidar +{ +struct RSDecoderMechConstParam +{ + RSDecoderConstParam base; + + // lens center + float RX; + float RY; + float RZ; + + // firing_ts/chan_ts + double BLOCK_DURATION; + double CHAN_TSS[128]; + float CHAN_AZIS[128]; +}; + +typedef struct +{ + uint16_t rpm; + RSEthNetV1 eth; + RSFOV fov; + RSVersionV1 version; + RSSN sn; + uint8_t return_mode; + RSStatusV1 status; + RSCalibrationAngle vert_angle_cali[32]; + RSCalibrationAngle horiz_angle_cali[32]; +} AdapterDifopPkt; + +template +class DecoderMech : public Decoder +{ +public: + constexpr static int32_t RS_ONE_ROUND = 36000; + + virtual ~DecoderMech() = default; + + explicit DecoderMech(const RSDecoderMechConstParam& const_param, const RSDecoderParam& param); + + void print(); + +#ifndef UNIT_TEST +protected: +#endif + + template + void decodeDifopCommon(const T_Difop& pkt); + + RSDecoderMechConstParam mech_const_param_; // const param + ChanAngles chan_angles_; // vert_angles/horiz_angles adjustment + AzimuthSection scan_section_; // valid azimuth section + std::shared_ptr split_strategy_; // split strategy + std::shared_ptr pre_split_strategy_; + uint16_t rps_; // rounds per second + uint16_t blks_per_frame_; // blocks per frame/round + uint16_t split_blks_per_frame_; // blocks in msop pkt per frame/round. + uint16_t block_az_diff_; // azimuth difference between adjacent blocks. + double fov_blind_ts_diff_; // timestamp difference across blind section(defined by fov) + float lidar_lens_center_Rxy_; // sqrt(Rx*Rx + Ry*Ry) +}; + +template +inline DecoderMech::DecoderMech(const RSDecoderMechConstParam& const_param, const RSDecoderParam& param) + : Decoder(const_param.base, param) + , mech_const_param_(const_param) + , chan_angles_(this->const_param_.LASER_NUM) + , scan_section_((int32_t)(this->param_.start_angle * 100), (int32_t)(this->param_.end_angle * 100)) + , rps_(10) + , blks_per_frame_((uint16_t)(1 / (10 * this->mech_const_param_.BLOCK_DURATION))) + , split_blks_per_frame_(blks_per_frame_) + , block_az_diff_(20) + , fov_blind_ts_diff_(0.0) +{ + this->packet_duration_ = this->mech_const_param_.BLOCK_DURATION * this->const_param_.BLOCKS_PER_PKT; + + switch (this->param_.split_frame_mode) + { + case SplitFrameMode::SPLIT_BY_FIXED_BLKS: + split_strategy_ = std::make_shared(&this->split_blks_per_frame_); + pre_split_strategy_ = std::make_shared(&this->split_blks_per_frame_); + break; + + case SplitFrameMode::SPLIT_BY_CUSTOM_BLKS: + split_strategy_ = std::make_shared(&this->param_.num_blks_split); + pre_split_strategy_ = std::make_shared(&this->param_.num_blks_split); + break; + + case SplitFrameMode::SPLIT_BY_ANGLE: + default: + uint16_t angle = (uint16_t)(this->param_.split_angle * 100); + split_strategy_ = std::make_shared(angle); + pre_split_strategy_ = std::make_shared(angle); + break; + } + + if (this->param_.config_from_file) + { + int ret = chan_angles_.loadFromFile(this->param_.angle_path); + this->angles_ready_ = (ret == 0); + + if (this->param_.wait_for_difop) + { + this->param_.wait_for_difop = false; + + RS_WARNING << "wait_for_difop cannot be true when config_from_file is true." + << " reset it to be false." << RS_REND; + } + } + this->is_mech_ = true; + this->lidar_lens_center_Rxy_ = std::sqrt(this->mech_const_param_.RX * this->mech_const_param_.RX + + this->mech_const_param_.RY * this->mech_const_param_.RY); +} + +template +inline void DecoderMech::print() +{ + std::cout << "-----------------------------------------" << std::endl + << "rps:\t\t\t" << this->rps_ << std::endl + << "echo_mode:\t\t" << this->echo_mode_ << std::endl + << "blks_per_frame:\t\t" << this->blks_per_frame_ << std::endl + << "split_blks_per_frame:\t" << this->split_blks_per_frame_ << std::endl + << "block_az_diff:\t\t" << this->block_az_diff_ << std::endl + << "fov_blind_ts_diff:\t" << this->fov_blind_ts_diff_ << std::endl + << "angle_from_file:\t" << this->param_.config_from_file << std::endl + << "angles_ready:\t\t" << this->angles_ready_ << std::endl; + + this->chan_angles_.print(); +} + +template +template +inline void DecoderMech::decodeDifopCommon(const T_Difop& pkt) +{ + // rounds per second + this->rps_ = ntohs(pkt.rpm) / 60; + if (this->rps_ == 0) + { + RS_WARNING << "LiDAR RPM is 0. Use default value 600." << RS_REND; + this->rps_ = 10; + } + + // blocks per frame + this->blks_per_frame_ = (uint16_t)(1 / (this->rps_ * this->mech_const_param_.BLOCK_DURATION)); + + // block diff of azimuth + this->block_az_diff_ = (uint16_t)std::round(RS_ONE_ROUND * this->rps_ * this->mech_const_param_.BLOCK_DURATION); + + // fov related + uint16_t fov_start_angle = ntohs(pkt.fov.start_angle); + uint16_t fov_end_angle = ntohs(pkt.fov.end_angle); + uint16_t fov_range = (fov_start_angle < fov_end_angle) ? (fov_end_angle - fov_start_angle) : + (fov_end_angle + RS_ONE_ROUND - fov_start_angle); + uint16_t fov_blind_range = RS_ONE_ROUND - fov_range; + + // fov blind diff of timestamp + this->fov_blind_ts_diff_ = (double)fov_blind_range / ((double)RS_ONE_ROUND * (double)this->rps_); + + // load angles + if (!this->param_.config_from_file && !this->angles_ready_) + { + int ret = this->chan_angles_.loadFromDifop(pkt.vert_angle_cali, pkt.horiz_angle_cali); + this->angles_ready_ = (ret == 0); + } + +#ifdef ENABLE_DIFOP_PARSE + // device info + memcpy(this->device_info_.sn, pkt.sn.num, 6); + memcpy(this->device_info_.mac, pkt.eth.mac_addr, 6); + memcpy(this->device_info_.top_ver, pkt.version.top_ver, 5); + memcpy(this->device_info_.bottom_ver, pkt.version.bottom_ver, 5); + this->device_info_.state = true; + // device status + this->device_status_.voltage = ntohs(pkt.status.vol_12v); + this->device_status_.state = true; +#endif +} + +} // namespace lidar +} // namespace robosense diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/member_checker.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/member_checker.hpp new file mode 100644 index 0000000..4f9591c --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/member_checker.hpp @@ -0,0 +1,139 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once + +#define DEFINE_MEMBER_CHECKER(member) \ + template \ + struct has_##member : std::false_type \ + { \ + }; \ + template \ + struct has_##member< \ + T, typename std::enable_if().member), void>::value, bool>::type> \ + : std::true_type \ + { \ + }; + +DEFINE_MEMBER_CHECKER(x) +DEFINE_MEMBER_CHECKER(y) +DEFINE_MEMBER_CHECKER(z) +DEFINE_MEMBER_CHECKER(intensity) +DEFINE_MEMBER_CHECKER(ring) +DEFINE_MEMBER_CHECKER(timestamp) +DEFINE_MEMBER_CHECKER(feature) + +#define RS_HAS_MEMBER(C, member) has_##member::value + +template +inline typename std::enable_if::type setX(T_Point& point, const float& value) +{ +} + +template +inline typename std::enable_if::type setX(T_Point& point, const float& value) +{ + point.x = value; +} + +template +inline typename std::enable_if::type setY(T_Point& point, const float& value) +{ +} + +template +inline typename std::enable_if::type setY(T_Point& point, const float& value) +{ + point.y = value; +} + +template +inline typename std::enable_if::type setZ(T_Point& point, const float& value) +{ +} + +template +inline typename std::enable_if::type setZ(T_Point& point, const float& value) +{ + point.z = value; +} + +template +inline typename std::enable_if::type setIntensity(T_Point& point, + const uint8_t& value) +{ +} + +template +inline typename std::enable_if::type setIntensity(T_Point& point, + const uint8_t& value) +{ + point.intensity = value; +} + +template +inline typename std::enable_if::type setRing(T_Point& point, const uint16_t& value) +{ +} + +template +inline typename std::enable_if::type setRing(T_Point& point, const uint16_t& value) +{ + point.ring = value; +} + +template +inline typename std::enable_if::type setTimestamp(T_Point& point, + const double& value) +{ +} + +template +inline typename std::enable_if::type setTimestamp(T_Point& point, + const double& value) +{ + point.timestamp = value; +} + +template +inline typename std::enable_if::type setFeature(T_Point& point, + const uint8_t& value) +{ +} + +template +inline typename std::enable_if::type setFeature(T_Point& point, + const uint8_t& value) +{ + point.feature = value; +} + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/section.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/section.hpp new file mode 100644 index 0000000..969d481 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/section.hpp @@ -0,0 +1,113 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once + +namespace robosense +{ +namespace lidar +{ + +class AzimuthSection +{ +public: + AzimuthSection(int32_t start, int32_t end) + { + full_round_ = _round (end - start) == 0; + + start_ = _round (start); + end_ = _round (end); + cross_zero_ = (start_ > end_); + } + + bool in(int32_t angle) + { + if (full_round_) + return true; + + if (cross_zero_) + { + return (angle >= start_) || (angle < end_); + } + else + { + return (angle >= start_) && (angle < end_); + } + } + +#ifndef UNIT_TEST +private: +#endif + + static + int32_t _round(int32_t value) + { + return (value % 36000 + 36000) % 36000; + } + + bool full_round_; + int32_t start_; + int32_t end_; + bool cross_zero_; +}; + +class DistanceSection +{ +public: + DistanceSection (float min, float max, float usr_min, float usr_max) + : min_(min), max_(max) + { + if (usr_min < 0) usr_min = 0; + if (usr_max < 0) usr_max = 0; + + if ((usr_min != 0) || (usr_max != 0)) + { + min_ = usr_min; + max_ = usr_max; + } + } + + bool in(float distance) + { + return ((min_ <= distance) && (distance <= max_)); + } + +#ifndef UNIT_TEST +private: +#endif + + float min_; + float max_; +}; + +} // namespace lidar +} // namespace robosense diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/split_strategy.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/split_strategy.hpp new file mode 100644 index 0000000..43ff2de --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/split_strategy.hpp @@ -0,0 +1,184 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once + +namespace robosense +{ +namespace lidar +{ + +class SplitStrategy +{ +public: + virtual bool newBlock(int32_t angle) = 0; + virtual ~SplitStrategy() = default; +}; + +class SplitStrategyByAngle : public SplitStrategy +{ +public: + SplitStrategyByAngle (int32_t split_angle) + : split_angle_(split_angle), prev_angle_(split_angle) + { + } + + virtual ~SplitStrategyByAngle() = default; + + virtual bool newBlock(int32_t angle) + { + if (angle < prev_angle_) + { + prev_angle_ -= 36000; + } + + bool v = ((prev_angle_ < split_angle_) && (split_angle_ <= angle)); +#if 0 + if (v) + { + std::cout << prev_angle_ << "\t" << angle << std::endl; + } +#endif + prev_angle_ = angle; + return v; + } + + +#ifndef UNIT_TEST +private: +#endif + const int32_t split_angle_; + int32_t prev_angle_; +}; + +class SplitStrategyByNum : public SplitStrategy +{ +public: + SplitStrategyByNum (uint16_t* max_blks) + : max_blks_(max_blks), blks_(0) + { + } + + virtual ~SplitStrategyByNum() = default; + + virtual bool newBlock(int32_t) + { + blks_++; + if (blks_ >= *max_blks_) + { + blks_ = 0; + return true; + } + + return false; + } + +#ifndef UNIT_TEST +private: +#endif + uint16_t* max_blks_; + uint16_t blks_; +}; + +class SplitStrategyBySeq +{ +public: + + SplitStrategyBySeq() + : prev_seq_(0), max_seq_(0), looped_(false) + { + setSafeRange(); + } + + bool newPacket(uint16_t seq) + { + bool split = false; + + if (seq > max_seq_) + { + max_seq_ = seq; + } + + if (seq < safe_seq_min_) // rewind + { + prev_seq_ = seq; + split = true; + looped_ = true; + } + else if (seq < prev_seq_) + { + // do nothing. + } + else if (seq <= safe_seq_max_) + { + prev_seq_ = seq; + } + else + { + if (prev_seq_ == 0) + prev_seq_ = seq; + + //do nothing. + } + + setSafeRange(); + return split; + } + + uint16_t maxSeq() + { + return (looped_ ? max_seq_ : 0); + } + + +#ifndef UNIT_TEST +private: +#endif + + constexpr static uint16_t RANGE = 10; + + void setSafeRange() + { + safe_seq_min_ = (prev_seq_ > RANGE) ? (prev_seq_ - RANGE) : 0; + safe_seq_max_ = prev_seq_ + RANGE; + } + + uint16_t prev_seq_; + uint16_t safe_seq_min_; + uint16_t safe_seq_max_; + + uint16_t max_seq_; + bool looped_; +}; + +} // namespace lidar +} // namespace robosense diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/trigon.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/trigon.hpp new file mode 100644 index 0000000..b2d73a9 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/trigon.hpp @@ -0,0 +1,133 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once + +#include + +#include + +namespace robosense +{ +namespace lidar +{ + +//#define DBG + +class Trigon +{ +public: + + constexpr static int32_t ANGLE_MIN = -9000; + constexpr static int32_t ANGLE_MAX = 45000; + + Trigon() + { + int32_t range = ANGLE_MAX - ANGLE_MIN; +#ifdef DBG + o_angles_ = (int32_t*)malloc(range * sizeof(int32_t)); +#endif + o_sins_ = (float*)malloc(range * sizeof(float)); + o_coss_ = (float*)malloc(range * sizeof(float)); + + for (int32_t i = ANGLE_MIN, j = 0; i < ANGLE_MAX; i++, j++) + { + double rad = DEGREE_TO_RADIAN(static_cast(i) * 0.01); + +#ifdef DBG + o_angles_[j] = i; +#endif + o_sins_[j] = (float)std::sin(rad); + o_coss_[j] = (float)std::cos(rad); + } + +#ifdef DBG + angles_ = o_angles_ - ANGLE_MIN; +#endif + sins_ = o_sins_ - ANGLE_MIN; + coss_ = o_coss_ - ANGLE_MIN; + } + + ~Trigon() + { + free(o_coss_); + free(o_sins_); +#ifdef DBG + free(o_angles_); +#endif + } + + float sin(int32_t angle) + { + if (angle < ANGLE_MIN || angle >= ANGLE_MAX) + { + angle = 0; + } + + return sins_[angle]; + } + + float cos(int32_t angle) + { + if (angle < ANGLE_MIN || angle >= ANGLE_MAX) + { + angle = 0; + } + + return coss_[angle]; + } + + void print() + { + for (int32_t i = -10; i < 10; i++) + { + std::cout << +#ifdef DBG + angles_[i] << "\t" << +#endif + sins_[i] << "\t" << coss_[i] << std::endl; + } + } + +private: +#ifdef DBG + int32_t* o_angles_; + int32_t* angles_; +#endif + float* o_sins_; + float* o_coss_; + float* sins_; + float* coss_; +}; + +} // namespace lidar +} // namespace robosense diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/driver_param.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/driver_param.hpp new file mode 100644 index 0000000..48f01b6 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/driver_param.hpp @@ -0,0 +1,409 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once + +#include +#include "rs_driver/msg/imu_data_msg.hpp" +#include +#include +#include +#include +namespace robosense +{ +namespace lidar +{ +enum LidarType ///< LiDAR type +{ + // mechanical + RS_MECH = 0x01, + RS16 = RS_MECH, + RS32, + RSBP, + RSAIRY, + RSHELIOS, + RSHELIOS_16P, + RS128, + RS80, + RS48, + RSP128, + RSP80, + RSP48, + + // mems + RS_MEMS = 0x20, + RSM1 = RS_MEMS, + RSM2, + RSM3, + RSE1, + RSMX, + + // jumbo + RS_JUMBO = 0x100, + RSM1_JUMBO = RS_JUMBO + RSM1, +}; + +inline bool isMech(LidarType type) +{ + return ((LidarType::RS_MECH <= type) && (type < LidarType::RS_MEMS)); +} + +inline bool isMems (LidarType type) +{ + return ((LidarType::RS_MEMS <= type) && (type < LidarType::RS_JUMBO)); +} + +inline bool isJumbo (LidarType type) +{ + return (LidarType::RS_JUMBO <= type); +} + +inline std::string lidarTypeToStr(const LidarType& type) +{ + static const std::unordered_map lidarTypeMap = { + {LidarType::RS16, "RS16"}, + {LidarType::RS32, "RS32"}, + {LidarType::RSBP, "RSBP"}, + {LidarType::RSAIRY, "RSAIRY"}, + {LidarType::RSHELIOS, "RSHELIOS"}, + {LidarType::RSHELIOS_16P, "RSHELIOS_16P"}, + {LidarType::RS128, "RS128"}, + {LidarType::RS80, "RS80"}, + {LidarType::RS48, "RS48"}, + {LidarType::RSP128, "RSP128"}, + {LidarType::RSP80, "RSP80"}, + {LidarType::RSP48, "RSP48"}, + {LidarType::RSM1, "RSM1"}, + {LidarType::RSM2, "RSM2"}, + {LidarType::RSM3, "RSM3"}, + {LidarType::RSE1, "RSE1"}, + {LidarType::RSMX, "RSMX"}, + {LidarType::RSM1_JUMBO, "RSM1_JUMBO"}, + }; + + auto it = lidarTypeMap.find(type); + if (it != lidarTypeMap.end()) { + return it->second; + } else { + RS_ERROR << "RS_ERROR" << RS_REND; + std::string str = "ERROR"; + return str; + } +} + +inline LidarType strToLidarType(const std::string& type) +{ + static const std::unordered_map strLidarTypeMap = { + {"RS16", LidarType::RS16}, + {"RS32", LidarType::RS32}, + {"RSBP", LidarType::RSBP}, + {"RSHELIOS", LidarType::RSHELIOS}, + {"RSHELIOS_16P", LidarType::RSHELIOS_16P}, + {"RS128", LidarType::RS128}, + {"RS80", LidarType::RS80}, + {"RS48", LidarType::RS48}, + {"RSP128", LidarType::RSP128}, + {"RSP80", LidarType::RSP80}, + {"RSP48", LidarType::RSP48}, + {"RSM1", LidarType::RSM1}, + {"RSM2", LidarType::RSM2}, + {"RSM3", LidarType::RSM3}, + {"RSE1", LidarType::RSE1}, + {"RSMX", LidarType::RSMX}, + {"RSAIRY", LidarType::RSAIRY}, + {"RSM1_JUMBO", LidarType::RSM1_JUMBO}, + }; + + auto it = strLidarTypeMap.find(type); + if (it != strLidarTypeMap.end()) { + return it->second; + } else { + RS_ERROR << "Wrong lidar type: " << type << RS_REND; + RS_ERROR << "Please give correct type: RS16, RS32, RSBP, RSHELIOS, RSHELIOS_16P, RS48, RS80, RS128, RSP128, RSP80, RSP48, " + << "RSM1, RSM1_JUMBO, RSM2,RSM3, RSE1, RSMX, RSAIRY." + << RS_REND; + exit(-1); + } +} + +enum InputType +{ + ONLINE_LIDAR = 1, + PCAP_FILE, + RAW_PACKET +}; + +inline std::string inputTypeToStr(const InputType& type) +{ + std::string str = ""; + switch (type) + { + case InputType::ONLINE_LIDAR: + str = "ONLINE_LIDAR"; + break; + case InputType::PCAP_FILE: + str = "PCAP_FILE"; + break; + case InputType::RAW_PACKET: + str = "RAW_PACKET"; + break; + default: + str = "ERROR"; + RS_ERROR << "RS_ERROR" << RS_REND; + } + return str; +} + +enum SplitFrameMode +{ + SPLIT_BY_ANGLE = 1, + SPLIT_BY_FIXED_BLKS, + SPLIT_BY_CUSTOM_BLKS +}; + +struct RSTransformParam ///< The Point transform parameter +{ + float x = 0.0f; ///< unit, m + float y = 0.0f; ///< unit, m + float z = 0.0f; ///< unit, m + float roll = 0.0f; ///< unit, radian + float pitch = 0.0f; ///< unit, radian + float yaw = 0.0f; ///< unit, radian + + void print() const + { + RS_INFO << "------------------------------------------------------" << RS_REND; + RS_INFO << " RoboSense Transform Parameters " << RS_REND; + RS_INFOL << "x: " << x << RS_REND; + RS_INFOL << "y: " << y << RS_REND; + RS_INFOL << "z: " << z << RS_REND; + RS_INFOL << "roll: " << roll << RS_REND; + RS_INFOL << "pitch: " << pitch << RS_REND; + RS_INFOL << "yaw: " << yaw << RS_REND; + RS_INFO << "------------------------------------------------------" << RS_REND; + } +}; + +struct RSDecoderParam ///< LiDAR decoder parameter +{ + float min_distance = 0.0f; ///< min/max distances of point cloud range. valid if min distance or max distance > 0 + float max_distance = 0.0f; + bool use_lidar_clock = false; ///< true: use LiDAR clock as timestamp; false: use system clock as timestamp + bool dense_points = false; ///< true: discard NAN points; false: reserve NAN points + bool ts_first_point = false; ///< true: time-stamp point cloud with the first point; false: with the last point; + bool wait_for_difop = true; ///< true: start sending point cloud until receive difop packet + RSTransformParam transform_param; ///< Used to transform points + + ///< Theses parameters are only for mechanical Lidars. + bool config_from_file = false; ///< Internal use only for debugging + std::string angle_path = ""; ///< Internal use only for debugging + float start_angle = 0.0f; ///< Start angle of point cloud + float end_angle = 360.0f; ///< End angle of point cloud + SplitFrameMode split_frame_mode = SplitFrameMode::SPLIT_BY_ANGLE; + ///< 1: Split frames by split_angle; + ///< 2: Split frames by fixed number of blocks; + ///< 3: Split frames by custom number of blocks (num_blks_split) + float split_angle = 0.0f; ///< Split angle(degree) used to split frame, only be used when split_frame_mode=1 + uint16_t num_blks_split = 1; ///< Number of packets in one frame, only be used when split_frame_mode=3 + + void print() const + { + RS_INFO << "------------------------------------------------------" << RS_REND; + RS_INFO << " RoboSense Decoder Parameters " << RS_REND; + RS_INFOL << "min_distance: " << min_distance << RS_REND; + RS_INFOL << "max_distance: " << max_distance << RS_REND; + RS_INFOL << "use_lidar_clock: " << use_lidar_clock << RS_REND; + RS_INFOL << "dense_points: " << dense_points << RS_REND; + RS_INFOL << "ts_first_point: " << ts_first_point << RS_REND; + RS_INFOL << "wait_for_difop: " << wait_for_difop << RS_REND; + RS_INFOL << "config_from_file: " << config_from_file << RS_REND; + RS_INFOL << "angle_path: " << angle_path << RS_REND; + RS_INFOL << "start_angle: " << start_angle << RS_REND; + RS_INFOL << "end_angle: " << end_angle << RS_REND; + RS_INFOL << "split_frame_mode: " << split_frame_mode << RS_REND; + RS_INFOL << "split_angle: " << split_angle << RS_REND; + RS_INFOL << "num_blks_split: " << num_blks_split << RS_REND; + RS_INFO << "------------------------------------------------------" << RS_REND; + transform_param.print(); + } + +}; + +struct RSInputParam ///< The LiDAR input parameter +{ + uint16_t msop_port = 6699; ///< Msop packet port number + uint16_t difop_port = 7788; ///< Difop packet port number + uint16_t imu_port = 0; ///< IMU packet port number, default disable + uint16_t user_layer_bytes = 0; ///< Bytes of user layer. thers is no user layer if it is 0 + uint16_t tail_layer_bytes = 0; ///< Bytes of tail layer. thers is no tail layer if it is 0 + + ///< These parameters are valid when the input type is online lidar + std::string host_address = "0.0.0.0"; ///< Address of host + std::string group_address = "0.0.0.0"; ///< Address of multicast group + uint32_t socket_recv_buf = 106496; // +#include + +#include +#include +#include + +#define VLAN_HDR_LEN 4 +#define ETH_HDR_LEN 42 +#define ETH_LEN (ETH_HDR_LEN + VLAN_HDR_LEN + 1500) +#define IP_LEN 65536 +#define UDP_HDR_LEN 8 + +namespace robosense +{ +namespace lidar +{ +class Input +{ +public: + Input(const RSInputParam& input_param); + + inline void regCallback( + const std::function& cb_excep, + const std::function(size_t)>& cb_get_pkt, + const std::function, bool)>& cb_put_pkt); + inline void regPcapSplitFrameCallback(const std::function& cb_pcap_split_frame); + + virtual bool init() = 0; + virtual bool start() = 0; + virtual void stop(); + virtual ~Input() + { + } + +protected: + inline void pushPacket(std::shared_ptr pkt, bool stuffed = true); + + RSInputParam input_param_; + std::function(size_t size)> cb_get_pkt_; + std::function, bool)> cb_put_pkt_; + std::function cb_excep_; + std::function cb_pcap_split_frame_; // for split frame judegment + std::thread recv_thread_; + bool to_exit_recv_; + bool init_flag_; + bool start_flag_; +}; + +inline Input::Input(const RSInputParam& input_param) + : input_param_(input_param), to_exit_recv_(false), + init_flag_(false), start_flag_(false) +{ +} + +inline void Input::regCallback( + const std::function& cb_excep, + const std::function(size_t)>& cb_get_pkt, + const std::function, bool)>& cb_put_pkt) +{ + cb_excep_ = cb_excep; + cb_get_pkt_ = cb_get_pkt; + cb_put_pkt_ = cb_put_pkt; +} + +inline void Input::stop() +{ + if (start_flag_) + { + to_exit_recv_ = true; + recv_thread_.join(); + + start_flag_ = false; + } +} + +inline void Input::pushPacket(std::shared_ptr pkt, bool stuffed) +{ + cb_put_pkt_(pkt, stuffed); +} + +inline void Input::regPcapSplitFrameCallback(const std::function& cb_pcap_split_frame) +{ + cb_pcap_split_frame_ = cb_pcap_split_frame; + +} +} // namespace lidar +} // namespace robosense diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/input/input_factory.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/input/input_factory.hpp new file mode 100644 index 0000000..10b6e0e --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/input/input_factory.hpp @@ -0,0 +1,117 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include + +#ifndef DISABLE_PCAP_PARSE +#include +#include +#endif + +namespace robosense +{ +namespace lidar +{ + +class InputFactory +{ +public: + static std::shared_ptr createInput(InputType type, const RSInputParam& param, bool isJumbo, + double sec_to_delay, std::function& cb_feed_pkt); +}; + +inline std::shared_ptr InputFactory::createInput(InputType type, const RSInputParam& param, bool isJumbo, + double sec_to_delay, std::function& cb_feed_pkt) +{ + std::shared_ptr input; + + switch(type) + { + case InputType::ONLINE_LIDAR: + { + if (isJumbo) + input = std::make_shared(param); + else + input = std::make_shared(param); + } + break; + +#ifndef DISABLE_PCAP_PARSE + case InputType::PCAP_FILE: + { + if (isJumbo) + input = std::make_shared(param, sec_to_delay); + else + input = std::make_shared(param, sec_to_delay); + } + break; +#endif + + case InputType::RAW_PACKET: + { + std::shared_ptr inputRaw; + + if (isJumbo) + inputRaw = std::make_shared(param); + else + inputRaw = std::make_shared(param); + + cb_feed_pkt = std::bind(&InputRaw::feedPacket, inputRaw, + std::placeholders::_1, std::placeholders::_2); + + input = inputRaw; + } + break; + + default: + + RS_ERROR << "Wrong Input Type " << type << "." << RS_REND; + + if (type == InputType::PCAP_FILE) + { + RS_ERROR << "To use InputType::PCAP_FILE, please do not specify the make option DISABLE_PCAP_PARSE." << RS_REND; + } + + exit(-1); + } + + return input; +} + +} // namespace lidar +} // namespace robosense diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/input/input_pcap.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/input/input_pcap.hpp new file mode 100644 index 0000000..ba9ff7c --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/input/input_pcap.hpp @@ -0,0 +1,283 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once +#include + +#include + +#ifdef _WIN32 +#define WIN32 +#else //__linux__ +#endif + +#include + +namespace robosense +{ +namespace lidar +{ +class InputPcap : public Input +{ +public: + InputPcap(const RSInputParam& input_param, double sec_to_delay) + : Input(input_param) + , pcap_(NULL) + , pcap_offset_(ETH_HDR_LEN) + , pcap_tail_(0) + , difop_filter_valid_(false) + , imu_filter_valid_(false) + { + if (input_param.use_vlan) + { + pcap_offset_ += VLAN_HDR_LEN; + } + + pcap_offset_ += input_param.user_layer_bytes; + pcap_tail_ += input_param.tail_layer_bytes; + + std::stringstream msop_stream, difop_stream, imu_stream; + if (input_param_.use_vlan) + { + msop_stream << "vlan && "; + difop_stream << "vlan && "; + } + + msop_stream << "udp dst port " << input_param_.msop_port; + difop_stream << "udp dst port " << input_param_.difop_port; + imu_stream << "udp dst port " << input_param_.imu_port; + + msop_filter_str_ = msop_stream.str(); + difop_filter_str_ = difop_stream.str(); + imu_filter_str_ = imu_stream.str(); + + float play_rate_ = input_param.pcap_rate; + if (play_rate_ <= MIN_PLAY_RATE) + { + play_rate_ = 1.0f; + } + if (play_rate_ > MAX_PLAY_RATE) + { + play_rate_ = MAX_PLAY_RATE; + } + millisec_to_delay_ = (uint64_t)(DELAY_CALC_BASE / play_rate_); + } + + virtual bool init(); + virtual bool start(); + virtual ~InputPcap(); + +private: + void recvPacket(); + +private: + pcap_t* pcap_; + size_t pcap_offset_; + size_t pcap_tail_; + std::string msop_filter_str_; + std::string difop_filter_str_; + std::string imu_filter_str_; + bpf_program msop_filter_; + bpf_program difop_filter_; + bpf_program imu_filter_; + bool difop_filter_valid_; + bool imu_filter_valid_; + uint64_t millisec_to_delay_; + static constexpr float MIN_PLAY_RATE = 0.0f; + static constexpr float MAX_PLAY_RATE = 10.0f; + static constexpr float DELAY_CALC_BASE = 100.0f; +}; + +inline bool InputPcap::init() +{ + if (init_flag_) + return true; + + char errbuf[PCAP_ERRBUF_SIZE]; + pcap_ = pcap_open_offline(input_param_.pcap_path.c_str(), errbuf); + if (pcap_ == NULL) + { + cb_excep_(Error(ERRCODE_PCAPWRONGPATH)); + return false; + } + + pcap_compile(pcap_, &msop_filter_, msop_filter_str_.c_str(), 1, 0xFFFFFFFF); + + if ((input_param_.difop_port != 0) && (input_param_.difop_port != input_param_.msop_port)) + { + pcap_compile(pcap_, &difop_filter_, difop_filter_str_.c_str(), 1, 0xFFFFFFFF); + difop_filter_valid_ = true; + } + if ((input_param_.imu_port != 0) && (input_param_.imu_port != input_param_.msop_port) && + (input_param_.imu_port != input_param_.difop_port)) + { + pcap_compile(pcap_, &imu_filter_, imu_filter_str_.c_str(), 1, 0xFFFFFFFF); + imu_filter_valid_ = true; + } + + init_flag_ = true; + return true; +} + +inline bool InputPcap::start() +{ + if (start_flag_) + return true; + + if (!init_flag_) + { + cb_excep_(Error(ERRCODE_STARTBEFOREINIT)); + return false; + } + + to_exit_recv_ = false; + recv_thread_ = std::thread(std::bind(&InputPcap::recvPacket, this)); + + start_flag_ = true; + return true; +} + +inline InputPcap::~InputPcap() +{ + stop(); + + if (pcap_ != NULL) + { + pcap_close(pcap_); + pcap_ = NULL; + } +} +inline void InputPcap::recvPacket() +{ + auto start_time = std::chrono::steady_clock::now(); + while (!to_exit_recv_) + { + struct pcap_pkthdr* header; + const u_char* pkt_data; + int ret = pcap_next_ex(pcap_, &header, &pkt_data); + if (ret < 0) // reach file end. + { + pcap_close(pcap_); + pcap_ = NULL; + + if (input_param_.pcap_repeat) + { + cb_excep_(Error(ERRCODE_PCAPREPEAT)); + + char errbuf[PCAP_ERRBUF_SIZE]; + pcap_ = pcap_open_offline(input_param_.pcap_path.c_str(), errbuf); + continue; + } + else + { + cb_excep_(Error(ERRCODE_PCAPEXIT)); + break; + } + } + + if (pcap_offline_filter(&msop_filter_, header, pkt_data) != 0) + { + int dataLen = (int)(header->len - pcap_offset_ - pcap_tail_); + if (dataLen < 0) + { + cb_excep_(Error(ERRCODE_WRONGPCAPPARSE)); + continue; + } + std::shared_ptr pkt = cb_get_pkt_(ETH_LEN); + if (static_cast(dataLen) > pkt->bufSize()) + { + cb_excep_(Error(ERRCODE_WRONGPCAPPARSE)); + continue; + } + memcpy(pkt->data(), pkt_data + pcap_offset_, dataLen); + pkt->setData(0, dataLen); + + if (cb_pcap_split_frame_(pkt->data())) + { + auto end = std::chrono::steady_clock::now(); + auto duration = std::chrono::duration_cast(end - start_time); + auto sleepTime = std::chrono::milliseconds(millisec_to_delay_) - duration; + if (sleepTime > std::chrono::milliseconds(0)) + { + std::this_thread::sleep_for(sleepTime); + } + start_time = std::chrono::steady_clock::now(); + } + pushPacket(pkt); + } + else if (difop_filter_valid_ && (pcap_offline_filter(&difop_filter_, header, pkt_data) != 0)) + { + int dataLen = (int)(header->len - pcap_offset_ - pcap_tail_); + if (dataLen < 0) + { + cb_excep_(Error(ERRCODE_WRONGPCAPPARSE)); + continue; + } + + std::shared_ptr pkt = cb_get_pkt_(ETH_LEN); + if (static_cast(dataLen) > pkt->bufSize()) + { + cb_excep_(Error(ERRCODE_WRONGPCAPPARSE)); + continue; + } + + memcpy(pkt->data(), pkt_data + pcap_offset_, dataLen); + pkt->setData(0, dataLen); + pushPacket(pkt); + } + else if (imu_filter_valid_ && (pcap_offline_filter(&imu_filter_, header, pkt_data) != 0)) + { + int dataLen = (int)(header->len - pcap_offset_ - pcap_tail_); + if (dataLen < 0) + { + cb_excep_(Error(ERRCODE_WRONGPCAPPARSE)); + continue; + } + std::shared_ptr pkt = cb_get_pkt_(ETH_LEN); + if (static_cast(dataLen) > pkt->bufSize()) + { + cb_excep_(Error(ERRCODE_WRONGPCAPPARSE)); + continue; + } + memcpy(pkt->data(), pkt_data + pcap_offset_, dataLen); + pkt->setData(0, dataLen); + pushPacket(pkt); + } + else + { + continue; + } + } +} + +} // namespace lidar +} // namespace robosense diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/input/input_pcap_jumbo.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/input/input_pcap_jumbo.hpp new file mode 100644 index 0000000..b1d0b67 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/input/input_pcap_jumbo.hpp @@ -0,0 +1,198 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once +#include +#include + +#include + +#ifdef _WIN32 +#define WIN32 +#else //__linux__ +#endif + +#include + +namespace robosense +{ +namespace lidar +{ +class InputPcapJumbo : public Input +{ +public: + InputPcapJumbo(const RSInputParam& input_param, double sec_to_delay) + : Input(input_param), pcap_(NULL), pcap_offset_(ETH_HDR_LEN), pcap_tail_(0), difop_filter_valid_(false), + msec_to_delay_((uint64_t)(sec_to_delay / input_param.pcap_rate * 1000000)) + { + if (input_param.use_vlan) + { + pcap_offset_ += VLAN_HDR_LEN; + } + + pcap_offset_ += input_param.user_layer_bytes; + pcap_tail_ += input_param.tail_layer_bytes; + + std::stringstream msop_stream; + if (input_param_.use_vlan) + { + msop_stream << "vlan && "; + } + + msop_stream << "udp"; + + msop_filter_str_ = msop_stream.str(); + } + + virtual bool init(); + virtual bool start(); + virtual ~InputPcapJumbo(); + +private: + void recvPacket(); + +private: + pcap_t* pcap_; + size_t pcap_offset_; + size_t pcap_tail_; + std::string msop_filter_str_; + bpf_program msop_filter_; + bpf_program difop_filter_; + bool difop_filter_valid_; + uint64_t msec_to_delay_; + + Jumbo jumbo_; +}; + +inline bool InputPcapJumbo::init() +{ + if (init_flag_) + return true; + + char errbuf[PCAP_ERRBUF_SIZE]; + pcap_ = pcap_open_offline(input_param_.pcap_path.c_str(), errbuf); + if (pcap_ == NULL) + { + cb_excep_(Error(ERRCODE_PCAPWRONGPATH)); + return false; + } + + pcap_compile(pcap_, &msop_filter_, msop_filter_str_.c_str(), 1, 0xFFFFFFFF); + + init_flag_ = true; + return true; +} + +inline bool InputPcapJumbo::start() +{ + if (start_flag_) + return true; + + if (!init_flag_) + { + cb_excep_(Error(ERRCODE_STARTBEFOREINIT)); + return false; + } + + to_exit_recv_ = false; + recv_thread_ = std::thread(std::bind(&InputPcapJumbo::recvPacket, this)); + + start_flag_ = true; + return true; +} + +inline InputPcapJumbo::~InputPcapJumbo() +{ + stop(); + + if (pcap_ != NULL) + { + pcap_close(pcap_); + pcap_ = NULL; + } +} + +inline void InputPcapJumbo::recvPacket() +{ + while (!to_exit_recv_) + { + struct pcap_pkthdr* header; + const uint8_t* pkt_data; + int ret = pcap_next_ex(pcap_, &header, &pkt_data); + if (ret < 0) // reach file end. + { + pcap_close(pcap_); + pcap_ = NULL; + + if (input_param_.pcap_repeat) + { + cb_excep_(Error(ERRCODE_PCAPREPEAT)); + + char errbuf[PCAP_ERRBUF_SIZE]; + pcap_ = pcap_open_offline(input_param_.pcap_path.c_str(), errbuf); + continue; + } + else + { + cb_excep_(Error(ERRCODE_PCAPEXIT)); + break; + } + } + + if (pcap_offline_filter(&msop_filter_, header, pkt_data) != 0) + { + uint16_t udp_port = 0; + const uint8_t* udp_data = NULL; + size_t udp_data_len = 0; + bool new_pkt = jumbo_.new_fragment(pkt_data, header->len, &udp_port, &udp_data, &udp_data_len); + if (new_pkt) + { + if ((udp_port == input_param_.msop_port) || (udp_port == input_param_.difop_port)) + { + std::shared_ptr pkt = cb_get_pkt_(IP_LEN); + memcpy(pkt->data(), udp_data, udp_data_len); + pkt->setData(0, udp_data_len); + pushPacket(pkt); + } + } + } + else + { + continue; + } + + std::this_thread::sleep_for(std::chrono::microseconds(msec_to_delay_)); + } +} + +} // namespace lidar +} // namespace robosense diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/input/input_raw.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/input/input_raw.hpp new file mode 100644 index 0000000..201e02c --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/input/input_raw.hpp @@ -0,0 +1,77 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once + +#include + +namespace robosense +{ +namespace lidar +{ +class InputRaw : public Input +{ +public: + + virtual bool init(){return true;} + virtual bool start(){return true;}; + virtual void stop(){} + virtual ~InputRaw(){} + + void feedPacket(const uint8_t* data, size_t size); + + InputRaw(const RSInputParam& input_param); + +protected: + size_t pkt_buf_len_; + size_t raw_offset_; + size_t raw_tail_; +}; + +inline InputRaw::InputRaw(const RSInputParam& input_param) + : Input(input_param), pkt_buf_len_(ETH_LEN), + raw_offset_(0), raw_tail_(0) +{ + raw_offset_ += input_param.user_layer_bytes; + raw_tail_ += input_param.tail_layer_bytes; +} + +inline void InputRaw::feedPacket(const uint8_t* data, size_t size) +{ + std::shared_ptr pkt = cb_get_pkt_(pkt_buf_len_); + memcpy(pkt->data(), data + raw_offset_, size - raw_offset_ - raw_tail_); + pkt->setData(0, size - raw_offset_ - raw_tail_); + pushPacket(pkt); +} + +} // namespace lidar +} // namespace robosense diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/input/input_raw_jumbo.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/input/input_raw_jumbo.hpp new file mode 100644 index 0000000..1c9ca0f --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/input/input_raw_jumbo.hpp @@ -0,0 +1,53 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once + +#include + +namespace robosense +{ +namespace lidar +{ +class InputRawJumbo : public InputRaw +{ +public: + + InputRawJumbo(const RSInputParam& input_param) + : InputRaw(input_param) + { + pkt_buf_len_ = IP_LEN; + } +}; + +} // namespace lidar +} // namespace robosense diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/input/input_sock.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/input/input_sock.hpp new file mode 100644 index 0000000..a01dd60 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/input/input_sock.hpp @@ -0,0 +1,17 @@ + +#pragma once + +#ifdef _WIN32 + +#include + +#else //__linux__ + +#ifdef ENABLE_EPOLL_RECEIVE +#include +#else +#include +#endif + +#endif + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/input/input_sock_jumbo.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/input/input_sock_jumbo.hpp new file mode 100644 index 0000000..cc197fa --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/input/input_sock_jumbo.hpp @@ -0,0 +1,23 @@ + +#pragma once + +#include + +namespace robosense +{ +namespace lidar +{ + +class InputSockJumbo : public InputSock +{ +public: + + InputSockJumbo(const RSInputParam& input_param) + : InputSock(input_param) + { + pkt_buf_len_ = IP_LEN; + } +}; + +} // namespace lidar +} // namespace robosense diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/input/jumbo.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/input/jumbo.hpp new file mode 100644 index 0000000..a25f76a --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/input/jumbo.hpp @@ -0,0 +1,190 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once + +#include + +namespace robosense +{ +namespace lidar +{ + +#pragma pack(push, 1) + +struct iphdr +{ + uint8_t version; + uint8_t tos; + uint16_t tot_len; + + uint16_t id; + uint16_t frag_off; + + uint8_t ttl; + uint8_t protocol; + uint16_t check; + + uint32_t saddr; + uint32_t daddr; +}; + +struct udphdr +{ + uint16_t source; + uint16_t dest; + uint16_t len; + uint16_t check; +}; + +#pragma pack(pop) + +class Jumbo +{ +public: + + Jumbo() + : ip_id_(0), buf_off_(0) + { + } + + bool new_fragment(const uint8_t* pkt_data, size_t pkt_data_size, + uint16_t* udp_port, const uint8_t**ip_data, size_t* ip_data_len); + +private: + + uint16_t dst_port(const uint8_t* buf); + + uint16_t ip_id_; + uint8_t buf_[IP_LEN]; + uint16_t buf_off_; +}; + +inline bool Jumbo::new_fragment(const uint8_t* pkt_data, size_t pkt_data_size, + uint16_t* udp_port, const uint8_t** udp_data, size_t* udp_data_len) +{ + // Is it an ip packet ? + const uint16_t* eth_type = (const uint16_t*)(pkt_data + 12); + if (ntohs(*eth_type) != 0x0800) + return false; + + // is it a udp packet? + const struct iphdr* ip_hdr = (const struct iphdr*)(pkt_data + 14); + if (ip_hdr->protocol != 0x11) + return false; + + // ip data + uint16_t ip_hdr_size = (ip_hdr->version & 0xf) * 4; + uint16_t ip_len = ntohs(ip_hdr->tot_len); + + const uint8_t* ip_data = pkt_data + 14 + ip_hdr_size; + uint16_t ip_data_len = ip_len - ip_hdr_size; + + // ip fragment + uint16_t ip_id = ntohs (ip_hdr->id); + uint16_t f_off = ntohs(ip_hdr->frag_off); + uint16_t frag_flags = (f_off >> 13); + uint16_t frag_off = (f_off & 0x1fff) * 8; // 8 octet boudary + +#define MORE_FRAGS(flags) ((flags & 0x01) != 0) + + if (ip_id == ip_id_) + { + if (frag_off == buf_off_) + { + memcpy (buf_ + buf_off_, ip_data, ip_data_len); + buf_off_ += ip_data_len; + + if (!MORE_FRAGS(frag_flags)) + { +#if 0 + printf ("--- end new packet. ip_id:0x%x, len:%d\n", ip_id_, buf_off_); + + for (uint16_t off = UDP_HDR_LEN, i = 0; off < buf_len(); off += 984, i++) + { + char title[32]; + sprintf (title, "jumbo %d ", i); + + hexdump (buf() + off, 32, title); + } +#endif + + ip_id_ = 0; + + *udp_port = dst_port (buf_); + *udp_data = buf_ + UDP_HDR_LEN; + *udp_data_len = buf_off_ - UDP_HDR_LEN; + return true; + } + } + } + else + { + if (frag_off == 0) + { + if (MORE_FRAGS(frag_flags)) + { + ip_id_ = ip_id; + buf_off_ = 0; + + memcpy (buf_ + buf_off_, ip_data, ip_data_len); + buf_off_ += ip_data_len; + +#if 0 + printf ("+++ start packet 0x%x\n", ip_id_); +#endif + } + else + { +#if 0 + printf ("+++ non-fragment packet 0x%x\n", ip_id); +#endif + + *udp_port = dst_port (ip_data); + *udp_data = ip_data + UDP_HDR_LEN; + *udp_data_len = ip_data_len - UDP_HDR_LEN; + return true; + } + } + } + + return false; +} + +inline uint16_t Jumbo::dst_port(const uint8_t* buf) +{ + const struct udphdr* udp_hdr = (const struct udphdr*)buf; + return ntohs(udp_hdr->dest); +} + +} // namespace lidar +} // namespace robosense diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/input/unix/input_sock_epoll.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/input/unix/input_sock_epoll.hpp new file mode 100644 index 0000000..b0ac884 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/input/unix/input_sock_epoll.hpp @@ -0,0 +1,300 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include + +namespace robosense +{ +namespace lidar +{ +class InputSock : public Input +{ +public: + InputSock(const RSInputParam& input_param) : Input(input_param), pkt_buf_len_(ETH_LEN), sock_offset_(0), sock_tail_(0) + { + sock_offset_ += input_param.user_layer_bytes; + sock_tail_ += input_param.tail_layer_bytes; + } + + virtual bool init(); + virtual bool start(); + virtual ~InputSock(); + +private: + inline void recvPacket(); + inline int createSocket(uint16_t port, const std::string& hostIp, const std::string& grpIp); + +protected: + size_t pkt_buf_len_; + int epfd_; + int fds_[3]{ -1 }; + size_t sock_offset_; + size_t sock_tail_; +}; + +inline bool InputSock::init() +{ + if (init_flag_) + { + return true; + } + + int msop_fd = -1, difop_fd = -1, imu_fd = -1; + int epfd = epoll_create(1); + if (epfd < 0) + goto failEpfd; + + // + // msop + // + { + msop_fd = createSocket(input_param_.msop_port, input_param_.host_address, input_param_.group_address); + if (msop_fd < 0) + goto failMsop; + + struct epoll_event ev; + ev.data.fd = msop_fd; + ev.events = EPOLLIN; // level-triggered + epoll_ctl(epfd, EPOLL_CTL_ADD, msop_fd, &ev); + } + + // + // difop + // + if ((input_param_.difop_port != 0) && (input_param_.difop_port != input_param_.msop_port)) + { + difop_fd = createSocket(input_param_.difop_port, input_param_.host_address, input_param_.group_address); + if (difop_fd < 0) + goto failDifop; + + struct epoll_event ev; + ev.data.fd = difop_fd; + ev.events = EPOLLIN; // level-triggered + epoll_ctl(epfd, EPOLL_CTL_ADD, difop_fd, &ev); + } + + epfd_ = epfd; + fds_[0] = msop_fd; + fds_[1] = difop_fd; + + if ((input_param_.imu_port != 0) && (input_param_.imu_port != input_param_.msop_port) && + (input_param_.imu_port != input_param_.difop_port)) + { + imu_fd = createSocket(input_param_.imu_port, input_param_.host_address, input_param_.group_address); + if (imu_fd < 0) + goto failImu; + + struct epoll_event ev; + ev.data.fd = difop_fd; + ev.events = EPOLLIN; // level-triggered + epoll_ctl(epfd, EPOLL_CTL_ADD, imu_fd, &ev); + } + + init_flag_ = true; + return true; +failImu: + close(difop_fd); + +failDifop: + close(msop_fd); +failMsop: + close(epfd); +failEpfd: + return false; +} + +inline bool InputSock::start() +{ + if (start_flag_) + { + return true; + } + + if (!init_flag_) + { + cb_excep_(Error(ERRCODE_STARTBEFOREINIT)); + return false; + } + + to_exit_recv_ = false; + recv_thread_ = std::thread(std::bind(&InputSock::recvPacket, this)); + + start_flag_ = true; + return true; +} + +inline InputSock::~InputSock() +{ + stop(); + + close(fds_[0]); + if (fds_[1] >= 0) + close(fds_[1]); + if (fds_[2] >= 0) + close(fds_[2]); + + close(epfd_); +} + +inline int InputSock::createSocket(uint16_t port, const std::string& hostIp, const std::string& grpIp) +{ + int fd; + int ret; + int reuse = 1; + + fd = socket(PF_INET, SOCK_DGRAM, 0); + if (fd < 0) + { + perror("socket: "); + goto failSocket; + } + + ret = setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse)); + if (ret < 0) + { + perror("setsockopt(SO_REUSEADDR): "); + goto failOption; + } + + struct sockaddr_in host_addr; + memset(&host_addr, 0, sizeof(host_addr)); + host_addr.sin_family = AF_INET; + host_addr.sin_port = htons(port); + host_addr.sin_addr.s_addr = INADDR_ANY; + if (hostIp != "0.0.0.0" && grpIp == "0.0.0.0") + { + inet_pton(AF_INET, hostIp.c_str(), &(host_addr.sin_addr)); + } + + ret = bind(fd, (struct sockaddr*)&host_addr, sizeof(host_addr)); + if (ret < 0) + { + perror("bind: "); + goto failBind; + } + + if (grpIp != "0.0.0.0") + { +#if 0 + struct ip_mreqn ipm; + memset(&ipm, 0, sizeof(ipm)); + inet_pton(AF_INET, grpIp.c_str(), &(ipm.imr_multiaddr)); + inet_pton(AF_INET, hostIp.c_str(), &(ipm.imr_address)); +#else + struct ip_mreq ipm; + inet_pton(AF_INET, grpIp.c_str(), &(ipm.imr_multiaddr)); + inet_pton(AF_INET, hostIp.c_str(), &(ipm.imr_interface)); +#endif + ret = setsockopt(fd, IPPROTO_IP, IP_ADD_MEMBERSHIP, &ipm, sizeof(ipm)); + if (ret < 0) + { + perror("setsockopt(IP_ADD_MEMBERSHIP): "); + goto failGroup; + } + } + + { + int flags = fcntl(fd, F_GETFL, 0); + ret = fcntl(fd, F_SETFL, flags | O_NONBLOCK); + if (ret < 0) + { + perror("fcntl: "); + goto failNonBlock; + } + } + + return fd; + +failNonBlock: +failGroup: +failBind: +failOption: + close(fd); +failSocket: + return -1; +} + +inline void InputSock::recvPacket() +{ + while (!to_exit_recv_) + { + struct epoll_event events[8]; + int retval = epoll_wait(epfd_, events, 8, 1000); + if (retval == 0) + { + cb_excep_(Error(ERRCODE_MSOPTIMEOUT)); + continue; + } + else if (retval < 0) + { + if (errno == EINTR) + continue; + + perror("epoll_wait: "); + break; + } + + for (int i = 0; i < retval; i++) + { + if (events[i].events & EPOLLIN) + { + std::shared_ptr pkt = cb_get_pkt_(pkt_buf_len_); + ssize_t ret = recvfrom(events[i].data.fd, pkt->buf(), pkt->bufSize(), 0, NULL, NULL); + if (ret < 0) + { + perror("recvfrom: "); + goto failExit; + } + else if (ret > 0) + { + pkt->setData(sock_offset_, ret - sock_offset_ - sock_tail_); + pushPacket(pkt); + } + } + } + } + +failExit: + return; +} + +} // namespace lidar +} // namespace robosense diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/input/unix/input_sock_select.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/input/unix/input_sock_select.hpp new file mode 100644 index 0000000..3a73f8e --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/input/unix/input_sock_select.hpp @@ -0,0 +1,321 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include + +namespace robosense +{ +namespace lidar +{ +class InputSock : public Input +{ +public: + InputSock(const RSInputParam& input_param) : Input(input_param), pkt_buf_len_(ETH_LEN), sock_offset_(0), sock_tail_(0) + { + sock_offset_ += input_param.user_layer_bytes; + sock_tail_ += input_param.tail_layer_bytes; + } + + virtual bool init(); + virtual bool start(); + virtual ~InputSock(); + +private: + inline void recvPacket(); + inline int createSocket(uint16_t port, const std::string& hostIp, const std::string& grpIp); + +protected: + size_t pkt_buf_len_; + int fds_[3]{ -1 }; + size_t sock_offset_; + size_t sock_tail_; +}; + +inline bool InputSock::init() +{ + if (init_flag_) + { + return true; + } + + int msop_fd = -1, difop_fd = -1, imu_fd = -1; + + msop_fd = createSocket(input_param_.msop_port, input_param_.host_address, input_param_.group_address); + if (msop_fd < 0) + goto failMsop; + + if ((input_param_.difop_port != 0) && (input_param_.difop_port != input_param_.msop_port)) + { + difop_fd = createSocket(input_param_.difop_port, input_param_.host_address, input_param_.group_address); + if (difop_fd < 0) + goto failDifop; + } + fds_[0] = msop_fd; + fds_[1] = difop_fd; + + if ((input_param_.imu_port != 0) && (input_param_.imu_port != input_param_.msop_port) && + (input_param_.imu_port != input_param_.difop_port)) + { + imu_fd = createSocket(input_param_.imu_port, input_param_.host_address, input_param_.group_address); + if (imu_fd < 0) + goto failImu; + } + + fds_[2] = imu_fd; + + init_flag_ = true; + return true; + +failImu: + close(difop_fd); + +failDifop: + close(msop_fd); +failMsop: + return false; +} + +inline bool InputSock::start() +{ + if (start_flag_) + { + return true; + } + + if (!init_flag_) + { + cb_excep_(Error(ERRCODE_STARTBEFOREINIT)); + return false; + } + + to_exit_recv_ = false; + recv_thread_ = std::thread(std::bind(&InputSock::recvPacket, this)); + + start_flag_ = true; + return true; +} + +inline InputSock::~InputSock() +{ + stop(); + + close(fds_[0]); + if (fds_[1] >= 0) + close(fds_[1]); + + if (fds_[2] >= 0) + close(fds_[2]); +} + +inline int InputSock::createSocket(uint16_t port, const std::string& hostIp, const std::string& grpIp) +{ + int fd; + int ret; + int reuse = 1; + + fd = socket(PF_INET, SOCK_DGRAM, 0); + if (fd < 0) + { + perror("socket: "); + goto failSocket; + } + + ret = setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse)); + if (ret < 0) + { + perror("setsockopt(SO_REUSEADDR): "); + goto failOption; + } + + struct sockaddr_in host_addr; + memset(&host_addr, 0, sizeof(host_addr)); + host_addr.sin_family = AF_INET; + host_addr.sin_port = htons(port); + host_addr.sin_addr.s_addr = INADDR_ANY; + if (hostIp != "0.0.0.0" && grpIp == "0.0.0.0") + { + inet_pton(AF_INET, hostIp.c_str(), &(host_addr.sin_addr)); + } + if (hostIp != "0.0.0.0" && grpIp != "0.0.0.0") + { + inet_pton(AF_INET, grpIp.c_str(), &(host_addr.sin_addr)); + } + + ret = bind(fd, (struct sockaddr*)&host_addr, sizeof(host_addr)); + if (ret < 0) + { + perror("bind: "); + goto failBind; + } + + if (grpIp != "0.0.0.0") + { +#if 0 + struct ip_mreqn ipm; + memset(&ipm, 0, sizeof(ipm)); + inet_pton(AF_INET, grpIp.c_str(), &(ipm.imr_multiaddr)); + inet_pton(AF_INET, hostIp.c_str(), &(ipm.imr_address)); +#else + struct ip_mreq ipm; + inet_pton(AF_INET, grpIp.c_str(), &(ipm.imr_multiaddr)); + inet_pton(AF_INET, hostIp.c_str(), &(ipm.imr_interface)); +#endif + ret = setsockopt(fd, IPPROTO_IP, IP_ADD_MEMBERSHIP, &ipm, sizeof(ipm)); + if (ret < 0) + { + perror("setsockopt(IP_ADD_MEMBERSHIP): "); + goto failGroup; + } + } + +#ifdef ENABLE_MODIFY_RECVBUF + { + uint32_t opt_val = input_param_.socket_recv_buf; + uint32_t before_set_val = 0; + uint32_t after_set_val = 0; + if (opt_val < 1024) + { + opt_val = 106496; + } + socklen_t opt_len = sizeof(uint32_t); + // get original value + if (getsockopt(fd, SOL_SOCKET, SO_RCVBUF, &before_set_val, &opt_len) == -1) + { + perror("getsockopt before"); + return -1; + } + RS_INFO << "Original receive buffer size: " << before_set_val << " bytes" << RS_REND; + + // set new value + if (setsockopt(fd, SOL_SOCKET, SO_RCVBUF, &opt_val, opt_len) == -1) + { + perror("setsockopt"); + return -1; + } + + // get new value + if (getsockopt(fd, SOL_SOCKET, SO_RCVBUF, &after_set_val, &opt_len) == -1) + { + perror("getsockopt after"); + return -1; + } + RS_INFO << "After setting: receive buffer size: " << after_set_val << " bytes" << RS_REND; + } +#endif + + { + int flags = fcntl(fd, F_GETFL, 0); + ret = fcntl(fd, F_SETFL, flags | O_NONBLOCK); + if (ret < 0) + { + perror("fcntl: "); + goto failNonBlock; + } + } + + return fd; + +failNonBlock: +failGroup: +failBind: +failOption: + close(fd); +failSocket: + return -1; +} + +inline void InputSock::recvPacket() +{ + int max_fd = std::max(std::max(fds_[0], fds_[1]), fds_[2]); + + while (!to_exit_recv_) + { + fd_set rfds; + FD_ZERO(&rfds); + FD_SET(fds_[0], &rfds); + if (fds_[1] >= 0) + FD_SET(fds_[1], &rfds); + + if (fds_[2] >= 0) + { + FD_SET(fds_[2], &rfds); + } + struct timeval tv; + tv.tv_sec = 1; + tv.tv_usec = 0; + int retval = select(max_fd + 1, &rfds, NULL, NULL, &tv); + if (retval == 0) + { + cb_excep_(Error(ERRCODE_MSOPTIMEOUT)); + continue; + } + else if (retval < 0) + { + if (errno == EINTR) + continue; + + perror("select: "); + break; + } + + for (int i = 0; i < 3; i++) + { + if ((fds_[i] >= 0) && FD_ISSET(fds_[i], &rfds)) + { + std::shared_ptr pkt = cb_get_pkt_(pkt_buf_len_); + ssize_t ret = recvfrom(fds_[i], pkt->buf(), pkt->bufSize(), 0, NULL, NULL); + if (ret < 0) + { + perror("recvfrom: "); + break; + } + else if (ret > 0) + { + pkt->setData(sock_offset_, ret - sock_offset_ - sock_tail_); + pushPacket(pkt); + } + } + } + } +} + +} // namespace lidar +} // namespace robosense diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/input/win/input_sock_select.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/input/win/input_sock_select.hpp new file mode 100644 index 0000000..1d39d65 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/input/win/input_sock_select.hpp @@ -0,0 +1,317 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once + +#include + +#include +#include +#pragma warning(disable : 4244) + +namespace robosense +{ +namespace lidar +{ +class InputSock : public Input +{ +public: + InputSock(const RSInputParam& input_param) : Input(input_param), pkt_buf_len_(ETH_LEN), sock_offset_(0), sock_tail_(0) + { + sock_offset_ += input_param.user_layer_bytes; + sock_tail_ += input_param.tail_layer_bytes; + } + + virtual bool init(); + virtual bool start(); + virtual ~InputSock(); + +private: + inline void recvPacket(); + inline int createSocket(uint16_t port, const std::string& hostIp, const std::string& grpIp); + +protected: + size_t pkt_buf_len_; + int fds_[3]{ -1 }; + size_t sock_offset_; + size_t sock_tail_; +}; + +inline bool InputSock::init() +{ + if (init_flag_) + { + return true; + } + + int msop_fd = -1, difop_fd = -1, imu_fd = -1; + + WORD version = MAKEWORD(2, 2); + WSADATA wsaData; + int ret = WSAStartup(version, &wsaData); + if (ret < 0) + goto failWsa; + + msop_fd = createSocket(input_param_.msop_port, input_param_.host_address, input_param_.group_address); + if (msop_fd < 0) + goto failMsop; + + if ((input_param_.difop_port != 0) && (input_param_.difop_port != input_param_.msop_port)) + { + difop_fd = createSocket(input_param_.difop_port, input_param_.host_address, input_param_.group_address); + if (difop_fd < 0) + goto failDifop; + } + if ((input_param_.imu_port != 0) && (input_param_.imu_port != input_param_.msop_port) && + (input_param_.imu_port != input_param_.difop_port)) + { + imu_fd = createSocket(input_param_.imu_port, input_param_.host_address, input_param_.group_address); + if (imu_fd < 0) + goto failImu; + } + + fds_[0] = msop_fd; + fds_[1] = difop_fd; + fds_[2] = imu_fd; + + init_flag_ = true; + return true; + +failImu: + closesocket(difop_fd); +failDifop: + closesocket(msop_fd); +failMsop: +failWsa: + return false; +} + +inline bool InputSock::start() +{ + if (start_flag_) + { + return true; + } + + if (!init_flag_) + { + cb_excep_(Error(ERRCODE_STARTBEFOREINIT)); + return false; + } + + to_exit_recv_ = false; + recv_thread_ = std::thread(std::bind(&InputSock::recvPacket, this)); + + start_flag_ = true; + return true; +} + +inline InputSock::~InputSock() +{ + stop(); + + closesocket(fds_[0]); + if (fds_[1] >= 0) + closesocket(fds_[1]); + if (fds_[2] >= 0) + closesocket(fds_[2]); +} + +inline int InputSock::createSocket(uint16_t port, const std::string& hostIp, const std::string& grpIp) +{ + int fd; + int ret; + int reuse = 1; + + fd = socket(PF_INET, SOCK_DGRAM, 0); + if (fd < 0) + { + perror("socket: "); + goto failSocket; + } + + ret = setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, (const char*)&reuse, sizeof(reuse)); + if (ret < 0) + { + perror("setsockopt(SO_REUSEADDR): "); + goto failOption; + } + + struct sockaddr_in host_addr; + memset(&host_addr, 0, sizeof(host_addr)); + host_addr.sin_family = AF_INET; + host_addr.sin_port = htons(port); + host_addr.sin_addr.s_addr = INADDR_ANY; + if (hostIp != "0.0.0.0" && grpIp == "0.0.0.0") + { + inet_pton(AF_INET, hostIp.c_str(), &(host_addr.sin_addr)); + } + + ret = bind(fd, (struct sockaddr*)&host_addr, sizeof(host_addr)); + if (ret < 0) + { + perror("bind: "); + goto failBind; + } + + if (grpIp != "0.0.0.0") + { +#if 0 + struct ip_mreqn ipm; + memset(&ipm, 0, sizeof(ipm)); + inet_pton(AF_INET, grpIp.c_str(), &(ipm.imr_multiaddr)); + inet_pton(AF_INET, hostIp.c_str(), &(ipm.imr_address)); +#else + struct ip_mreq ipm; + inet_pton(AF_INET, grpIp.c_str(), &(ipm.imr_multiaddr)); + inet_pton(AF_INET, hostIp.c_str(), &(ipm.imr_interface)); +#endif + ret = setsockopt(fd, IPPROTO_IP, IP_ADD_MEMBERSHIP, (const char*)&ipm, sizeof(ipm)); + if (ret < 0) + { + perror("setsockopt(IP_ADD_MEMBERSHIP): "); + goto failGroup; + } + } + +#ifdef ENABLE_MODIFY_RECVBUF + { + uint32_t opt_val = input_param_.socket_recv_buf; + uint32_t before_set_val = 0; + uint32_t after_set_val = 0; + if (opt_val < 1024) + { + opt_val = 106496; + } + socklen_t opt_len = sizeof(uint32_t); + // get original value + if (getsockopt(fd, SOL_SOCKET, SO_RCVBUF, &before_set_val, &opt_len) == -1) + { + perror("getsockopt before"); + return -1; + } + RS_INFO << "Original receive buffer size: " << before_set_val << " bytes" << RS_REND; + + // set new value + if (setsockopt(fd, SOL_SOCKET, SO_RCVBUF, &opt_val, opt_len) == -1) + { + perror("setsockopt"); + return -1; + } + + // get new value + if (getsockopt(fd, SOL_SOCKET, SO_RCVBUF, &after_set_val, &opt_len) == -1) + { + perror("getsockopt after"); + return -1; + } + RS_INFO << "After setting: receive buffer size: " << after_set_val << " bytes" << RS_REND; + } +#endif + + { + u_long mode = 1; + ret = ioctlsocket(fd, FIONBIO, &mode); + if (ret < 0) + { + perror("ioctlsocket: "); + goto failNonBlock; + } + } + + return fd; + +failNonBlock: +failGroup: +failBind: +failOption: + closesocket(fd); +failSocket: + return -1; +} + +inline void InputSock::recvPacket() +{ + int max_fd = std::max(std::max(fds_[0], fds_[1]), fds_[2]); + + while (!to_exit_recv_) + { + fd_set rfds; + FD_ZERO(&rfds); + FD_SET(fds_[0], &rfds); + if (fds_[1] >= 0) + FD_SET(fds_[1], &rfds); + if (fds_[2] >= 0) + { + FD_SET(fds_[2], &rfds); + } + struct timeval tv; + tv.tv_sec = 1; + tv.tv_usec = 0; + int retval = select(max_fd + 1, &rfds, NULL, NULL, &tv); + if (retval == 0) + { + cb_excep_(Error(ERRCODE_MSOPTIMEOUT)); + continue; + } + else if (retval < 0) + { + if (errno == EINTR) + continue; + + perror("select: "); + break; + } + + for (int i = 0; i < 3; i++) + { + if ((fds_[i] >= 0) && FD_ISSET(fds_[i], &rfds)) + { + std::shared_ptr pkt = cb_get_pkt_(pkt_buf_len_); + int ret = recvfrom(fds_[i], (char*)pkt->buf(), (int)pkt->bufSize(), 0, NULL, NULL); + if (ret < 0) + { + perror("recvfrom: "); + break; + } + else if (ret > 0) + { + pkt->setData(sock_offset_, ret - sock_offset_ - sock_tail_); + pushPacket(pkt); + } + } + } + } +} + +} // namespace lidar +} // namespace robosense diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/lidar_driver_impl.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/lidar_driver_impl.hpp new file mode 100644 index 0000000..2fcb56c --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/lidar_driver_impl.hpp @@ -0,0 +1,498 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace robosense +{ +namespace lidar +{ +inline std::string getDriverVersion() +{ + std::stringstream stream; + stream << RSLIDAR_VERSION_MAJOR << "." << RSLIDAR_VERSION_MINOR << "." << RSLIDAR_VERSION_PATCH; + + return stream.str(); +} + +template +class LidarDriverImpl +{ +public: + LidarDriverImpl(); + ~LidarDriverImpl(); + + void regPointCloudCallback(const std::function(void)>& cb_get_cloud, + const std::function)>& cb_put_cloud); + void regPacketCallback(const std::function& cb_put_pkt); + void regImuDataCallback(const std::function(void)>& cb_get_imu_data, + const std::function& msg)>& cb_put_imu_data); + void regExceptionCallback(const std::function& cb_excep); + + bool init(const RSDriverParam& param); + bool start(); + void stop(); + + void decodePacket(const Packet& pkt); + bool getTemperature(float& temp); + bool getDeviceInfo(DeviceInfo& info); + bool getDeviceStatus(DeviceStatus& status); + +private: + void runPacketCallBack(uint8_t* data, size_t data_size, double timestamp, uint8_t is_difop, uint8_t is_frame_begin); + void runExceptionCallback(const Error& error); + + std::shared_ptr packetGet(size_t size); + void packetPut(std::shared_ptr pkt, bool stuffed); + + void processPacket(); + void internalProcessPacket(std::shared_ptr pkt); + + std::shared_ptr getImuData(); + void putImuData(); + + std::shared_ptr getPointCloud(); + void splitFrame(uint16_t height, double ts); + void setPointCloudHeader(std::shared_ptr msg, uint16_t height, double chan_ts); + + bool isNewFrame(const uint8_t* packet); + + RSDriverParam driver_param_; + std::function(void)> cb_get_cloud_; + std::function)> cb_put_cloud_; + std::function cb_put_pkt_; + std::function(void)> cb_get_imu_data_; + std::function& msg)> cb_put_imu_data_; + std::function cb_excep_; + std::function cb_feed_pkt_; + + std::shared_ptr input_ptr_; + std::shared_ptr> decoder_ptr_; + SyncQueue> free_pkt_queue_; + SyncQueue> pkt_queue_; + std::thread handle_thread_; + uint32_t pkt_seq_; + uint32_t point_cloud_seq_; + bool to_exit_handle_; + bool init_flag_; + bool start_flag_; + + static const uint8_t MSOP_HEADER_ID[2]; + static const uint8_t DIFOP_HEADER_ID[2]; + static const uint8_t IMU_HEADER_ID[2]; +}; + +template +const uint8_t LidarDriverImpl::MSOP_HEADER_ID[2] = { 0x55, 0xAA }; +template +const uint8_t LidarDriverImpl::DIFOP_HEADER_ID[2] = { 0xA5, 0xFF }; +template +const uint8_t LidarDriverImpl::IMU_HEADER_ID[2] = { 0xAA, 0x55 }; + +template +inline LidarDriverImpl::LidarDriverImpl() + : pkt_seq_(0), point_cloud_seq_(0), init_flag_(false), start_flag_(false) +{ +} + +template +inline LidarDriverImpl::~LidarDriverImpl() +{ + stop(); +} + +template +std::shared_ptr LidarDriverImpl::getImuData() +{ + while (1) + { + std::shared_ptr imuData = cb_get_imu_data_(); + if (imuData) + { + imuData->state = false; + return imuData; + } + LIMIT_CALL(runExceptionCallback(Error(ERRCODE_IMUDATANULL)), 1); + } +} + +template +std::shared_ptr LidarDriverImpl::getPointCloud() +{ + while (1) + { + std::shared_ptr cloud = cb_get_cloud_(); + if (cloud) + { + cloud->points.resize(0); + return cloud; + } + + LIMIT_CALL(runExceptionCallback(Error(ERRCODE_POINTCLOUDNULL)), 1); + } +} + +template +void LidarDriverImpl::regPointCloudCallback( + const std::function(void)>& cb_get_cloud, + const std::function)>& cb_put_cloud) +{ + cb_get_cloud_ = cb_get_cloud; + cb_put_cloud_ = cb_put_cloud; +} + +template +inline void LidarDriverImpl::regPacketCallback(const std::function& cb_put_pkt) +{ + cb_put_pkt_ = cb_put_pkt; +} +template +inline void LidarDriverImpl::regImuDataCallback( + const std::function(void)>& cb_get_imu_data, + const std::function& msg)>& cb_put_imu_data) +{ + cb_get_imu_data_ = cb_get_imu_data; + cb_put_imu_data_ = cb_put_imu_data; +} + +template +inline void LidarDriverImpl::regExceptionCallback(const std::function& cb_excep) +{ + cb_excep_ = cb_excep; +} + +template +inline bool LidarDriverImpl::init(const RSDriverParam& param) +{ + if (init_flag_) + { + return true; + } + + // + // decoder + // + decoder_ptr_ = DecoderFactory::createDecoder(param.lidar_type, param.decoder_param); + + // rewrite pkt timestamp or not ? + decoder_ptr_->enableWritePktTs((cb_put_pkt_ == nullptr) ? false : true); + + // point cloud related + decoder_ptr_->point_cloud_ = getPointCloud(); + decoder_ptr_->regCallback( + std::bind(&LidarDriverImpl::runExceptionCallback, this, std::placeholders::_1), + std::bind(&LidarDriverImpl::splitFrame, this, std::placeholders::_1, std::placeholders::_2)); + + if (cb_put_imu_data_) + { + decoder_ptr_->imuDataPtr_ = getImuData(); + decoder_ptr_->regImuCallback(std::bind(&LidarDriverImpl::putImuData, this)); + } + + double packet_duration = decoder_ptr_->getPacketDuration(); + bool is_jumbo = isJumbo(param.lidar_type); + + // + // input + // + input_ptr_ = InputFactory::createInput(param.input_type, param.input_param, is_jumbo, packet_duration, cb_feed_pkt_); + + input_ptr_->regCallback( + std::bind(&LidarDriverImpl::runExceptionCallback, this, std::placeholders::_1), + std::bind(&LidarDriverImpl::packetGet, this, std::placeholders::_1), + std::bind(&LidarDriverImpl::packetPut, this, std::placeholders::_1, std::placeholders::_2)); + if (param.input_type == InputType::PCAP_FILE) + { + input_ptr_->regPcapSplitFrameCallback( + std::bind(&LidarDriverImpl::isNewFrame, this, std::placeholders::_1)); + } + if (!input_ptr_->init()) + { + goto failInputInit; + } + + driver_param_ = param; + init_flag_ = true; + return true; + +failInputInit: + input_ptr_.reset(); + decoder_ptr_.reset(); + return false; +} + +template +inline bool LidarDriverImpl::start() +{ + if (start_flag_) + { + return true; + } + + if (!init_flag_) + { + return false; + } + + to_exit_handle_ = false; + handle_thread_ = std::thread(std::bind(&LidarDriverImpl::processPacket, this)); + + input_ptr_->start(); + + start_flag_ = true; + return true; +} + +template +inline void LidarDriverImpl::stop() +{ + if (!start_flag_) + { + return; + } + + input_ptr_->stop(); + + to_exit_handle_ = true; + handle_thread_.join(); + + // clear all points before next session + if (decoder_ptr_->point_cloud_) + { + decoder_ptr_->point_cloud_->points.clear(); + } + + start_flag_ = false; +} + +template +inline void LidarDriverImpl::decodePacket(const Packet& pkt) +{ + cb_feed_pkt_(pkt.buf_.data(), pkt.buf_.size()); +} + +template +inline bool LidarDriverImpl::getTemperature(float& temp) +{ + if (decoder_ptr_ == nullptr) + { + return false; + } + return decoder_ptr_->getTemperature(temp); +} + +template +inline bool LidarDriverImpl::getDeviceInfo(DeviceInfo& info) +{ + if (decoder_ptr_ == nullptr) + { + return false; + } + + return decoder_ptr_->getDeviceInfo(info); +} + +template +inline bool LidarDriverImpl::getDeviceStatus(DeviceStatus& status) +{ + if (decoder_ptr_ == nullptr) + { + return false; + } + + return decoder_ptr_->getDeviceStatus(status); +} + +template +inline void LidarDriverImpl::runPacketCallBack(uint8_t* data, size_t data_size, double timestamp, + uint8_t is_difop, uint8_t is_frame_begin) +{ + if (cb_put_pkt_) + { + Packet pkt; + pkt.timestamp = timestamp; + pkt.is_difop = is_difop; + pkt.is_frame_begin = is_frame_begin; + pkt.seq = pkt_seq_++; + pkt.frame_id = driver_param_.frame_id; + + pkt.buf_.resize(data_size); + memcpy(pkt.buf_.data(), data, data_size); + cb_put_pkt_(pkt); + } +} + +template +inline void LidarDriverImpl::runExceptionCallback(const Error& error) +{ + if (cb_excep_) + { + cb_excep_(error); + } +} + +template +inline std::shared_ptr LidarDriverImpl::packetGet(size_t size) +{ + std::shared_ptr pkt = free_pkt_queue_.pop(); + if (pkt.get() != NULL) + { + return pkt; + } + + return std::make_shared(size); +} + +template +inline void LidarDriverImpl::packetPut(std::shared_ptr pkt, bool stuffed) +{ + constexpr static int PACKET_POOL_MAX = 10240; + + if (!stuffed) + { + free_pkt_queue_.push(pkt); + return; + } + + size_t sz = pkt_queue_.push(pkt); + if (sz > PACKET_POOL_MAX) + { + LIMIT_CALL(runExceptionCallback(Error(ERRCODE_PKTBUFOVERFLOW)), 1); + pkt_queue_.clear(); + } +} + +template +inline void LidarDriverImpl::internalProcessPacket(std::shared_ptr pkt) +{ + uint8_t* id = pkt->data(); + if (memcmp(id, MSOP_HEADER_ID, sizeof(MSOP_HEADER_ID)) == 0) + { + bool pkt_to_split = decoder_ptr_->processMsopPkt(pkt->data(), pkt->dataSize()); + runPacketCallBack(pkt->data(), pkt->dataSize(), decoder_ptr_->prevPktTs(), false, pkt_to_split); // msop packet + } + else if (memcmp(id, DIFOP_HEADER_ID, sizeof(DIFOP_HEADER_ID)) == 0) + { + decoder_ptr_->processDifopPkt(pkt->data(), pkt->dataSize()); + runPacketCallBack(pkt->data(), pkt->dataSize(), 0, true, false); // difop packet + } + else if (memcmp(id, IMU_HEADER_ID, sizeof(IMU_HEADER_ID)) == 0) + { + decoder_ptr_->processImuPkt(pkt->data(), pkt->dataSize()); // imu packet + } + + free_pkt_queue_.push(pkt); +} + +template +inline void LidarDriverImpl::processPacket() +{ + while (!to_exit_handle_) + { + std::shared_ptr pkt = pkt_queue_.popWait(500000); + if (pkt.get() == NULL) + { + continue; + } + + internalProcessPacket(pkt); + } +} + +template +inline void LidarDriverImpl::putImuData() +{ + std::shared_ptr imuData = decoder_ptr_->imuDataPtr_; + if (imuData->state && this->cb_put_imu_data_) + { + this->cb_put_imu_data_(imuData); + decoder_ptr_->imuDataPtr_ = getImuData(); + } +} +template +void LidarDriverImpl::splitFrame(uint16_t height, double ts) +{ + std::shared_ptr cloud = decoder_ptr_->point_cloud_; + if (cloud->points.size() > 0) + { + setPointCloudHeader(cloud, height, ts); + cb_put_cloud_(cloud); + decoder_ptr_->point_cloud_ = getPointCloud(); + } +} + +template +void LidarDriverImpl::setPointCloudHeader(std::shared_ptr msg, uint16_t height, double ts) +{ + msg->seq = point_cloud_seq_++; + msg->timestamp = ts; + msg->is_dense = driver_param_.decoder_param.dense_points; + if (msg->is_dense) + { + msg->height = 1; + msg->width = (uint32_t)msg->points.size(); + } + else + { + msg->height = height; + msg->width = (uint32_t)msg->points.size() / msg->height; + } + + msg->frame_id = driver_param_.frame_id; +} + +template +inline bool LidarDriverImpl::isNewFrame(const uint8_t* packet) +{ + if (decoder_ptr_ != nullptr) + { + if (memcmp(packet, MSOP_HEADER_ID, sizeof(MSOP_HEADER_ID)) == 0) + { + return decoder_ptr_->isNewFrame(packet); + } + } + return false; +} + +} // namespace lidar +} // namespace robosense diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/macro/version.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/macro/version.hpp new file mode 100644 index 0000000..160a1b2 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/macro/version.hpp @@ -0,0 +1,3 @@ +#define RSLIDAR_VERSION_MAJOR 1 +#define RSLIDAR_VERSION_MINOR 5 +#define RSLIDAR_VERSION_PATCH 17 diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/macro/version.hpp.in b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/macro/version.hpp.in new file mode 100644 index 0000000..d54a6ba --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/macro/version.hpp.in @@ -0,0 +1,3 @@ +#define RSLIDAR_VERSION_MAJOR @PROJECT_VERSION_MAJOR@ +#define RSLIDAR_VERSION_MINOR @PROJECT_VERSION_MINOR@ +#define RSLIDAR_VERSION_PATCH @PROJECT_VERSION_PATCH@ diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/msg/imu_data_msg.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/msg/imu_data_msg.hpp new file mode 100644 index 0000000..9441b0f --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/msg/imu_data_msg.hpp @@ -0,0 +1,118 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once + +#include +#include +namespace robosense +{ +namespace lidar +{ +struct ImuData { + bool state; + double timestamp; // Time in nanoseconds + float orientation_x; + float orientation_y; + float orientation_z; + float orientation_w; + float angular_velocity_x; + float angular_velocity_y; + float angular_velocity_z; + float linear_acceleration_x; + float linear_acceleration_y; + float linear_acceleration_z; + ImuData() + : state{false}, + timestamp(0), + orientation_x(0.0), + orientation_y(0.0), + orientation_z(0.0), + orientation_w(1.0), + angular_velocity_x(0.0), + angular_velocity_y(0.0), + angular_velocity_z(0.0), + linear_acceleration_x(0.0), + linear_acceleration_y(0.0), + linear_acceleration_z(0.0) {} + + // Parameterized constructor to initialize all members + ImuData(bool valid, double ts, float ori_x, float ori_y, float ori_z, float ori_w, + float ang_vel_x, float ang_vel_y, float ang_vel_z, + float lin_acc_x, float lin_acc_y, float lin_acc_z) + : state{valid}, + timestamp(ts), + orientation_x(ori_x), + orientation_y(ori_y), + orientation_z(ori_z), + orientation_w(ori_w), + angular_velocity_x(ang_vel_x), + angular_velocity_y(ang_vel_y), + angular_velocity_z(ang_vel_z), + linear_acceleration_x(lin_acc_x), + linear_acceleration_y(lin_acc_y), + linear_acceleration_z(lin_acc_z) {} + ImuData& operator=(const ImuData& other) { + if (this != &other) { + state = other.state; + timestamp = other.timestamp; + orientation_x = other.orientation_x; + orientation_y = other.orientation_y; + orientation_z = other.orientation_z; + orientation_w = other.orientation_w; + angular_velocity_x = other.angular_velocity_x; + angular_velocity_y = other.angular_velocity_y; + angular_velocity_z = other.angular_velocity_z; + linear_acceleration_x = other.linear_acceleration_x; + linear_acceleration_y = other.linear_acceleration_y; + linear_acceleration_z = other.linear_acceleration_z; + } + return *this; + } + void init() + { + state = false; + timestamp = 0.0; + orientation_x = 0.0; + orientation_y = 0.0; + orientation_z = 0.0; + orientation_w = 1.0; + angular_velocity_x = 0.0; + angular_velocity_y = 0.0; + angular_velocity_z = 0.0; + linear_acceleration_x = 0.0; + linear_acceleration_y = 0.0; + linear_acceleration_z = 0.0; + } +}; +} // namespace lidar +} // namespace robosense \ No newline at end of file diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/msg/packet.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/msg/packet.hpp new file mode 100644 index 0000000..869237b --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/msg/packet.hpp @@ -0,0 +1,66 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once + +#include +#include +#include + +namespace robosense +{ +namespace lidar +{ + +struct Packet +{ + double timestamp = 0.0; + uint32_t seq = 0; + uint8_t is_difop = 0; + uint8_t is_frame_begin = 0; + std::string frame_id = ""; ///< Packet message frame id + + Packet(const Packet& msg) + { + buf_.assign(msg.buf_.begin(), msg.buf_.end()); + } + + Packet(size_t size = 0) + { + buf_.resize(size); + } + + std::vector buf_; +}; + +} // namespace lidar +} // namespace robosense diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/msg/pcl_point_cloud_msg.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/msg/pcl_point_cloud_msg.hpp new file mode 100644 index 0000000..86d76ac --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/msg/pcl_point_cloud_msg.hpp @@ -0,0 +1,68 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once + +#include +#include + +typedef pcl::PointXYZI PointXYZI; + +struct PointXYZIRT +{ + PCL_ADD_POINT4D; + float intensity; + std::uint16_t ring; + double timestamp; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +} EIGEN_ALIGN16; + +POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZIRT, + (float, x, x) + (float, y, y) + (float, z, z) + (float, intensity, intensity) + (std::uint16_t, ring, ring) + (double, timestamp, timestamp)) + +template +class PointCloudT : public pcl::PointCloud +{ +public: + typedef T_Point PointT; + typedef typename pcl::PointCloud::VectorType VectorT; + + double timestamp = 0.0; + uint32_t seq = 0; ///< Sequence number of message + std::string frame_id = ""; ///< Point cloud frame id +}; + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/msg/point_cloud_msg.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/msg/point_cloud_msg.hpp new file mode 100644 index 0000000..895b2ef --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/msg/point_cloud_msg.hpp @@ -0,0 +1,80 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once + +#include +#include + +#pragma pack(push, 1) +struct PointXYZI +{ + float x; + float y; + float z; + uint8_t intensity; +}; + +struct PointXYZIRT : public PointXYZI +{ + uint16_t ring; + double timestamp; +}; + +struct PointXYZIF : public PointXYZI +{ + uint8_t feature; +}; + +struct PointXYZIRTF : public PointXYZIRT +{ + uint8_t feature; +}; +#pragma pack(pop) + +template +class PointCloudT +{ +public: + typedef T_Point PointT; + typedef std::vector VectorT; + + uint32_t height = 0; ///< Height of point cloud + uint32_t width = 0; ///< Width of point cloud + bool is_dense = false; ///< If is_dense is true, the point cloud does not contain NAN points, + double timestamp = 0.0; + uint32_t seq = 0; ///< Sequence number of message + std::string frame_id = ""; ///< Point cloud frame id + + VectorT points; +}; + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/utility/buffer.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/utility/buffer.hpp new file mode 100644 index 0000000..32eb0a0 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/utility/buffer.hpp @@ -0,0 +1,89 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once + +#include +#include +#include + +namespace robosense +{ +namespace lidar +{ +class Buffer +{ +public: + + Buffer(size_t buf_size) + : data_off_(0), data_size_(0) + { + buf_.resize(buf_size); + buf_size_ = buf_size; + } + + ~Buffer() = default; + + uint8_t* buf() + { + return buf_.data(); + } + + size_t bufSize() const + { + return buf_size_; + } + + uint8_t* data() + { + return buf() + data_off_; + } + + size_t dataSize() const + { + return data_size_; + } + + void setData(size_t data_off, size_t data_size) + { + data_off_ = data_off; + data_size_ = data_size; + } + +private: + std::vector buf_; + size_t buf_size_; + size_t data_off_; + size_t data_size_; +}; +} // namespace lidar +} // namespace robosense diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/utility/dbg.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/utility/dbg.hpp new file mode 100644 index 0000000..4e6e3fb --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/utility/dbg.hpp @@ -0,0 +1,56 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once +#include + +namespace robosense +{ +namespace lidar +{ + +inline void hexdump(const uint8_t* data, size_t size, const char* desc = NULL) +{ + printf("\n---------------%s(size:%d)------------------", (desc ? desc : ""), (int)size); + + for (size_t i = 0; i < size; i++) + { + if (i % 16 == 0) + printf("\n"); + printf("%02x ", data[i]); + } + + printf("\n---------------------------------\n"); +} + +} // namespace lidar +} // namespace robosense diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/utility/sync_queue.hpp b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/utility/sync_queue.hpp new file mode 100644 index 0000000..378609b --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/utility/sync_queue.hpp @@ -0,0 +1,149 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once + +#include +#include +#include +#include + +namespace robosense +{ +namespace lidar +{ +template +class SyncQueue +{ +public: + inline size_t push(const T& value) + { +#ifndef ENABLE_WAIT_IF_QUEUE_EMPTY + bool empty = false; +#endif + size_t size = 0; + + { + std::lock_guard lg(mtx_); +#ifndef ENABLE_WAIT_IF_QUEUE_EMPTY + empty = queue_.empty(); +#endif + queue_.push(value); + size = queue_.size(); + } + +#ifndef ENABLE_WAIT_IF_QUEUE_EMPTY + if (empty) + cv_.notify_one(); +#endif + + return size; + } + + inline T pop() + { + T value; + + std::lock_guard lg(mtx_); + if (!queue_.empty()) + { + value = queue_.front(); + queue_.pop(); + } + + return value; + } + + inline T popWait(unsigned int usec = 1000000) + { + // + // Low latency, or low CPU usage, that is the question. + // - Hamlet + +#ifdef ENABLE_WAIT_IF_QUEUE_EMPTY + T value; + + { + std::lock_guard lg(mtx_); + if (!queue_.empty()) + { + value = queue_.front(); + queue_.pop(); + return value; + } + } + + std::this_thread::sleep_for(std::chrono::microseconds(1000)); + return value; +#else + + T value; + + std::unique_lock ul(mtx_); + cv_.wait_for(ul, std::chrono::microseconds(usec), [this] { return (!queue_.empty()); }); + + if (!queue_.empty()) + { + value = queue_.front(); + queue_.pop(); + } + + return value; +#endif + } + + inline void clear() + { + std::queue empty; + std::lock_guard lg(mtx_); + swap(empty, queue_); + } + + inline bool empty() { + std::lock_guard lg(mtx_); + return queue_.empty(); + } + + inline size_t size() { + std::lock_guard lg(mtx_); + return queue_.size(); + } + +private: + std::queue queue_; + std::mutex mtx_; +#ifndef ENABLE_WAIT_IF_QUEUE_EMPTY + std::condition_variable cv_; +#endif +}; +} // namespace lidar +} // namespace robosense diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/test/CMakeLists.txt b/src/airy/rslidar_sdk-main/src/rs_driver/test/CMakeLists.txt new file mode 100644 index 0000000..75e53e1 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/test/CMakeLists.txt @@ -0,0 +1,36 @@ + +cmake_minimum_required(VERSION 3.5) + +project(rs_driver_test) + +find_package(GTest REQUIRED) +include_directories(${GTEST_INCLUDE_DIRS}) +include_directories(${DRIVER_INCLUDE_DIRS}) + +add_definitions("-DUNIT_TEST") + +add_executable(rs_driver_test + rs_driver_test.cpp + buffer_test.cpp + sync_queue_test.cpp + trigon_test.cpp + basic_attr_test.cpp + section_test.cpp + chan_angles_test.cpp + split_strategy_test.cpp + single_return_block_iterator_test.cpp + dual_return_block_iterator_test.cpp + ab_dual_return_block_iterator_test.cpp + rs16_single_return_block_iterator_test.cpp + rs16_dual_return_block_iterator_test.cpp + decoder_test.cpp + decoder_rsbp_test.cpp + decoder_rs32_test.cpp + decoder_rs16_test.cpp + dbg_test.cpp + rs_common_test.cpp) + +target_link_libraries(rs_driver_test + ${GTEST_LIBRARIES} + ${EXTERNAL_LIBS}) + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/test/ab_dual_return_block_iterator_test.cpp b/src/airy/rslidar_sdk-main/src/rs_driver/test/ab_dual_return_block_iterator_test.cpp new file mode 100644 index 0000000..fdde7e0 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/test/ab_dual_return_block_iterator_test.cpp @@ -0,0 +1,136 @@ + +#include + +#include +#include + +using namespace robosense::lidar; + +typedef struct +{ + uint16_t distance; + uint8_t intensity; +} MyChannel; + +typedef struct +{ + uint16_t azimuth; + MyChannel channels[2]; +} MyBlock; + +typedef struct +{ + MyBlock blocks[3]; +} MyPacket; + +TEST(TestABDualPacketTraverser, ctor) +{ + { + // AAB + MyPacket pkt = + { + htons(1), 0x00, 0x00, 0x00, 0x00 + , htons(1), 0x00, 0x00, 0x00, 0x00 + , htons(21), 0x00, 0x00, 0x00, 0x00 + }; + + ABDualReturnBlockIterator iter(pkt, + 3, // blocks per packet + 0.5f, // block_duration + 25, // block_az_duraton + 2.0f); // fov_blind_duration + + int32_t az_diff; + double ts; + + // first block + iter.get (0, az_diff, ts); + ASSERT_EQ(az_diff, 20); + ASSERT_EQ(ts, 0.0f); + + // second block + iter.get (1, az_diff, ts); + ASSERT_EQ(az_diff, 20); + ASSERT_EQ(ts, 0.0f); + + // last block + iter.get (2, az_diff, ts); + ASSERT_EQ(az_diff, 25); + ASSERT_EQ(ts, 0.5f); + } + + { + // ABB + MyPacket pkt = + { + htons(1), 0x00, 0x00, 0x00, 0x00 + , htons(21), 0x00, 0x00, 0x00, 0x00 + , htons(21), 0x00, 0x00, 0x00, 0x00 + }; + + ABDualReturnBlockIterator iter(pkt, + 3, // blocks per packet + 0.5f, // block_duration + 25, // block_az_duraton + 2.0f); // fov_blind_duration + + int32_t az_diff; + double ts; + + // first block + iter.get (0, az_diff, ts); + ASSERT_EQ(az_diff, 20); + ASSERT_EQ(ts, 0.0f); + + // second block + iter.get (1, az_diff, ts); + ASSERT_EQ(az_diff, 25); + ASSERT_EQ(ts, 0.5f); + + // last block + iter.get (2, az_diff, ts); + ASSERT_EQ(az_diff, 25); + ASSERT_EQ(ts, 0.5f); + } + +} + +TEST(TestABDualPacketTraverser, ctor_fov) +{ + { + // AAB + MyPacket pkt = + { + htons(1), 0x00, 0x00, 0x00, 0x00 + , htons(1), 0x00, 0x00, 0x00, 0x00 + , htons(121), 0x00, 0x00, 0x00, 0x00 + }; + + ABDualReturnBlockIterator iter(pkt, + 3, // blocks per packet + 0.5f, // block_duration + 25, // block_az_duraton + 2.0f); // fov_blind_duration + + int32_t az_diff; + double ts; + + // first block + iter.get (0, az_diff, ts); + ASSERT_EQ(az_diff, 25); + ASSERT_EQ(ts, 0.0f); + + // second block + iter.get (1, az_diff, ts); + ASSERT_EQ(az_diff, 25); + ASSERT_EQ(ts, 0.0f); + + // last block + iter.get (2, az_diff, ts); + ASSERT_EQ(az_diff, 25); + ASSERT_EQ(ts, 2.0f); + } + +} + + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/test/basic_attr_test.cpp b/src/airy/rslidar_sdk-main/src/rs_driver/test/basic_attr_test.cpp new file mode 100644 index 0000000..7b157b2 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/test/basic_attr_test.cpp @@ -0,0 +1,76 @@ + +#include + +#include +#include + +using namespace robosense::lidar; + +TEST(TestParseTime, parseTimeYMD) +{ + uint8_t ts1[] = {0x15, 0x0a, 0x01, 0x01, 0x02, 0x03, 0x01, 0x11, 0x02, 0x22}; + uint8_t ts2[10]; + + ASSERT_EQ(parseTimeYMD((RSTimestampYMD*)ts1), 1633021323273546); + + createTimeYMD(1633021323273546, (RSTimestampYMD*)ts2); + ASSERT_EQ(memcmp(ts2, ts1, 10), 0); +} + +#if 0 +TEST(TestParseTime, parseTimeUTCWithNs) +{ + RSTimestampUTC ts1 = + {{0x01, 0x02, 0x03, 0x04, 0x05, 0x06}, {0x06, 0xA1, 0x1C, 0xF0}}; + RSTimestampUTC ts2; + + ASSERT_EQ(parseTimeUTCWithNs(&ts1), 0x010203040506 * 1000000 + 0x06A11CF0/1000); + + createTimeUTCWithNs(0x010203040506 * 1000000 + 0x06A11CF0/1000, &ts2); + ASSERT_EQ(memcmp(&ts2, &ts1, sizeof(ts1)), 0); +} +#endif + +TEST(TestParseTime, parseTimeUTCWithUs) +{ + RSTimestampUTC ts1 = + {{0x01, 0x02, 0x03, 0x04, 0x05, 0x06}, {0x00, 0x02, 0x33, 0x44}}; + RSTimestampUTC ts2; + + ASSERT_EQ(parseTimeUTCWithUs(&ts1), 0x010203040506 * 1000000 + 0x00023344); + + createTimeUTCWithUs(0x010203040506 * 1000000 + 0x00023344, &ts2); + ASSERT_EQ(memcmp(&ts2, &ts1, sizeof(ts1)), 0); +} + +TEST(TestParseTime, getTimeHost) +{ + getTimeHost(); +} + +TEST(TestParseTemp, parseTempInLe) +{ + { + uint8_t temp[] = {0x88, 0x11}; + ASSERT_EQ(parseTempInLe((RSTemperature*)&temp), 561); + } + + { + uint8_t temp[] = {0x88, 0x91}; + ASSERT_EQ(parseTempInLe((RSTemperature*)&temp), -561); + } +} + +TEST(TestParseTemp, parseTempInBe) +{ + { + uint8_t temp[] = {0x23, 0x10}; + ASSERT_EQ(parseTempInBe((RSTemperature*)&temp), 561); + } + + { + uint8_t temp[] = {0xA3, 0x10}; + ASSERT_EQ(parseTempInBe((RSTemperature*)&temp), -561); + } +} + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/test/buffer_test.cpp b/src/airy/rslidar_sdk-main/src/rs_driver/test/buffer_test.cpp new file mode 100644 index 0000000..e69bdd1 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/test/buffer_test.cpp @@ -0,0 +1,22 @@ + +#include + +#include + +using namespace robosense::lidar; + +TEST(TestBuffer, ctor) +{ + Buffer pkt(100); + + ASSERT_TRUE(pkt.buf() != NULL); + ASSERT_EQ(pkt.bufSize(), 100); + + ASSERT_EQ(pkt.data(), pkt.buf()); + ASSERT_EQ(pkt.dataSize(), 0); + + pkt.setData(5, 10); + ASSERT_EQ(pkt.data(), pkt.buf()+5); + ASSERT_EQ(pkt.dataSize(), 10); +} + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/test/chan_angles_test.cpp b/src/airy/rslidar_sdk-main/src/rs_driver/test/chan_angles_test.cpp new file mode 100644 index 0000000..19233db --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/test/chan_angles_test.cpp @@ -0,0 +1,223 @@ + +#include + +#include + +using namespace robosense::lidar; + +TEST(TestChanAngles, genUserChan) +{ + std::vector vert_angles; + std::vector user_chans; + + vert_angles.push_back(100); + vert_angles.push_back(0); + vert_angles.push_back(-100); + vert_angles.push_back(200); + + ChanAngles::genUserChan (vert_angles, user_chans); + ASSERT_EQ(user_chans.size(), 4); + ASSERT_EQ(user_chans[0], 2); + ASSERT_EQ(user_chans[1], 1); + ASSERT_EQ(user_chans[2], 0); + ASSERT_EQ(user_chans[3], 3); +} + +TEST(TestChanAngles, loadFromFile) +{ + std::vector vert_angles, horiz_angles; + + // load + ASSERT_EQ(ChanAngles::loadFromFile ("../test/res/angle.csv", 4, vert_angles, horiz_angles), 0); + ASSERT_EQ(vert_angles.size(), 4); + ASSERT_EQ(horiz_angles.size(), 4); + ASSERT_EQ(vert_angles[0], 500); + ASSERT_EQ(vert_angles[1], 250); + ASSERT_EQ(vert_angles[2], 0); + ASSERT_EQ(vert_angles[3], -250); + + ASSERT_EQ(horiz_angles[0], 10); + ASSERT_EQ(horiz_angles[1], -20); + ASSERT_EQ(horiz_angles[2], 0); + ASSERT_EQ(horiz_angles[3], -100); + + // load again + ASSERT_EQ(ChanAngles::loadFromFile ("../test/res/angle.csv", 4, vert_angles, horiz_angles), 0); + ASSERT_EQ(vert_angles.size(), 4); + ASSERT_EQ(horiz_angles.size(), 4); + + // load non-existing file + ASSERT_LT(ChanAngles::loadFromFile ("../test/res/non_exist.csv", 4, vert_angles, horiz_angles), 0); + ASSERT_EQ(vert_angles.size(), 0); + ASSERT_EQ(horiz_angles.size(), 0); +} + +TEST(TestChanAngles, loadFromDifop) +{ + uint8_t vert_angle_arr[] = {0x00, 0x01, 0x02, + 0x01, 0x03, 0x04, + 0x01, 0x05, 0x06, + 0x00, 0x07, 0x08}; + uint8_t horiz_angle_arr[] = {0x00, 0x01, 0x11, + 0x01, 0x02, 0x22, + 0x00, 0x03, 0x33, + 0x01, 0x04, 0x44}; + + std::vector vert_angles, horiz_angles; + + // load + ASSERT_EQ(ChanAngles::loadFromDifop( + (const RSCalibrationAngle*)vert_angle_arr, + (const RSCalibrationAngle*)horiz_angle_arr, + 4, + vert_angles, + horiz_angles), 0); + + ASSERT_EQ(vert_angles.size(), 4); + ASSERT_EQ(horiz_angles.size(), 4); + ASSERT_EQ(vert_angles[0], 258); + ASSERT_EQ(vert_angles[1], -772); + ASSERT_EQ(vert_angles[2], -1286); + ASSERT_EQ(vert_angles[3], 1800); + + ASSERT_EQ(horiz_angles[0], 273); + ASSERT_EQ(horiz_angles[1], -546); + ASSERT_EQ(horiz_angles[2], 819); + ASSERT_EQ(horiz_angles[3], -1092); + + // load again + ASSERT_EQ(ChanAngles::loadFromDifop( + (const RSCalibrationAngle*)vert_angle_arr, + (const RSCalibrationAngle*)horiz_angle_arr, + 4, + vert_angles, + horiz_angles), 0); + ASSERT_EQ(vert_angles.size(), 4); + ASSERT_EQ(horiz_angles.size(), 4); +} + +TEST(TestChanAngles, memberLoadFromFile) +{ + ChanAngles angles(4); + + // not loading yet + ASSERT_EQ(angles.chan_num_, 4); + ASSERT_EQ(angles.vert_angles_.size(), 4); + ASSERT_EQ(angles.horiz_angles_.size(), 4); + ASSERT_EQ(angles.user_chans_.size(), 4); + + // load + ASSERT_EQ(angles.loadFromFile ("../test/res/angle.csv"), 0); + ASSERT_EQ(angles.user_chans_.size(), 4); + ASSERT_EQ(angles.toUserChan(0), 3); + ASSERT_EQ(angles.toUserChan(1), 2); + ASSERT_EQ(angles.toUserChan(2), 1); + ASSERT_EQ(angles.toUserChan(3), 0); +} + +TEST(TestChanAngles, memberLoadFromFile_fail) +{ + ChanAngles angles(4); + ASSERT_EQ(angles.chan_num_, 4); + + // load non-existing file + ASSERT_LT(angles.loadFromFile ("../test/res/non_exist.csv"), 0); + ASSERT_EQ(angles.vert_angles_.size(), 4); + ASSERT_EQ(angles.vert_angles_[0], 0); +} + +TEST(TestChanAngles, memberLoadFromDifop) +{ + uint8_t vert_angle_arr[] = {0x00, 0x01, 0x02, + 0x01, 0x03, 0x04, + 0x01, 0x05, 0x06, + 0x00, 0x07, 0x08}; + uint8_t horiz_angle_arr[] = {0x00, 0x01, 0x11, + 0x01, 0x02, 0x22, + 0x00, 0x03, 0x33, + 0x01, 0x04, 0x44}; + + ChanAngles angles(4); + ASSERT_EQ(angles.chan_num_, 4); + + // load + ASSERT_EQ(angles.loadFromDifop((const RSCalibrationAngle*)vert_angle_arr, (const RSCalibrationAngle*)horiz_angle_arr), 0); + + ASSERT_EQ(angles.vert_angles_.size(), 4); + ASSERT_EQ(angles.vert_angles_[0], 258); + ASSERT_EQ(angles.vert_angles_[1], -772); + ASSERT_EQ(angles.vert_angles_[2], -1286); + ASSERT_EQ(angles.vert_angles_[3], 1800); + + ASSERT_EQ(angles.horiz_angles_[0], 273); + ASSERT_EQ(angles.horiz_angles_[1], -546); + ASSERT_EQ(angles.horiz_angles_[2], 819); + ASSERT_EQ(angles.horiz_angles_[3], -1092); + + ASSERT_EQ(angles.user_chans_.size(), 4); + ASSERT_EQ(angles.toUserChan(0), 2); + ASSERT_EQ(angles.toUserChan(1), 1); + ASSERT_EQ(angles.toUserChan(2), 0); + ASSERT_EQ(angles.toUserChan(3), 3); +} + +TEST(TestChanAngles, memberLoadFromDifop_fail) +{ + uint8_t vert_angle_arr[] = {0x00, 0x01, 0x02, + 0x01, 0x03, 0x04, + 0xFF, 0x05, 0x06, + 0xFF, 0x07, 0x08}; + uint8_t horiz_angle_arr[] = {0x00, 0x11, 0x22, + 0x01, 0x33, 0x44, + 0xFF, 0x55, 0x66, + 0xFF, 0x77, 0x88}; + + ChanAngles angles(4); + ASSERT_EQ(angles.chan_num_, 4); + + // load invalid difop + ASSERT_LT(angles.loadFromDifop((const RSCalibrationAngle*)vert_angle_arr, (const RSCalibrationAngle*)horiz_angle_arr), 0); + ASSERT_EQ(angles.vert_angles_.size(), 4); + ASSERT_EQ(angles.vert_angles_[0], 0); +} + +TEST(TestChanAngles, memberLoadFromDifop_fail_angle) +{ + uint8_t vert_angle_arr[] = {0x00, 0x01, 0x02, + 0x01, 0x03, 0x04, + 0x01, 0x05, 0x06, + 0x00, 0x07, 0x08}; + + // -9000 <= angle < 18000 + ChanAngles angles(4); + ASSERT_EQ(angles.chan_num_, 4); + + { + // 9000 + uint8_t horiz_angle_arr[] = + { + 0x00, 0x01, 0x11, + 0x01, 0x02, 0x22, + 0x00, 0x03, 0x33, + 0x00, 0x46, 0x52 //18000 + }; + + // load + ASSERT_LT(angles.loadFromDifop((const RSCalibrationAngle*)vert_angle_arr, (const RSCalibrationAngle*)horiz_angle_arr), 0); + } + + { + // -9001 + uint8_t horiz_angle_arr[] = + { + 0x00, 0x01, 0x11, + 0x01, 0x02, 0x22, + 0x00, 0x03, 0x33, + 0x01, 0x23, 0x29 + }; + + // load + ASSERT_LT(angles.loadFromDifop((const RSCalibrationAngle*)vert_angle_arr, (const RSCalibrationAngle*)horiz_angle_arr), 0); + } +} + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/test/dbg_test.cpp b/src/airy/rslidar_sdk-main/src/rs_driver/test/dbg_test.cpp new file mode 100644 index 0000000..d9e122d --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/test/dbg_test.cpp @@ -0,0 +1,13 @@ +#include + +#include + +using namespace robosense::lidar; + + +TEST(HexdumpTest, Basic) { + uint8_t data[] = {0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08}; + hexdump(data, sizeof(data), "Test Data"); + SUCCEED(); // if no exception thrown +} + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/test/decoder_rs16_test.cpp b/src/airy/rslidar_sdk-main/src/rs_driver/test/decoder_rs16_test.cpp new file mode 100644 index 0000000..b116032 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/test/decoder_rs16_test.cpp @@ -0,0 +1,56 @@ + +#include + +#include +#include +#include + +using namespace robosense::lidar; + +typedef PointXYZIRT PointT; +typedef PointCloudT PointCloud; + +TEST(TestDecoderRS16, getEchoMode) +{ + ASSERT_TRUE(DecoderRS16::getEchoMode(0) == RSEchoMode::ECHO_DUAL); + ASSERT_TRUE(DecoderRS16::getEchoMode(1) == RSEchoMode::ECHO_SINGLE); + ASSERT_TRUE(DecoderRS16::getEchoMode(2) == RSEchoMode::ECHO_SINGLE); +} + +TEST(TestDecoderRS16, RS16DifopPkt2Adapter) +{ + uint8_t pitch_cali[48] = + { + 0x00, 0x3a, 0x98, // 15.000 + 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, + 0x00, 0x3a, 0x98, // 15.000 + }; + + RS16DifopPkt src; + src.rpm = 0; + src.fov = {0}; + src.return_mode = 0; + src.sn = {0}; + src.eth = {0}; + src.version = {0}; + src.status = {0}; + memcpy (src.pitch_cali, pitch_cali, 48); + + AdapterDifopPkt dst; + RS16DifopPkt2Adapter(src, dst); + + ASSERT_EQ(dst.vert_angle_cali[0].sign, 1); + ASSERT_EQ(ntohs(dst.vert_angle_cali[0].value), 150); + ASSERT_EQ(dst.vert_angle_cali[8].sign, 0); + ASSERT_EQ(ntohs(dst.vert_angle_cali[8].value), 150); + + ASSERT_EQ(dst.horiz_angle_cali[0].sign, 0); + ASSERT_EQ(ntohs(dst.horiz_angle_cali[0].value), 0); +} + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/test/decoder_rs32_test.cpp b/src/airy/rslidar_sdk-main/src/rs_driver/test/decoder_rs32_test.cpp new file mode 100644 index 0000000..7948361 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/test/decoder_rs32_test.cpp @@ -0,0 +1,213 @@ + +#include + +#include +#include +#include + +using namespace robosense::lidar; + +typedef PointXYZIRT PointT; +typedef PointCloudT PointCloud; + +static ErrCode errCode = ERRCODE_SUCCESS; +static void errCallback(const Error& err) +{ + errCode = err.error_code; +} + +TEST(TestDecoderRS32, getEchoMode) +{ + ASSERT_TRUE(DecoderRS32::getEchoMode(0) == RSEchoMode::ECHO_DUAL); + ASSERT_TRUE(DecoderRS32::getEchoMode(1) == RSEchoMode::ECHO_SINGLE); + ASSERT_TRUE(DecoderRS32::getEchoMode(2) == RSEchoMode::ECHO_SINGLE); +} + +TEST(TestDecoderRS32, RS32DifopPkt2Adapter) +{ + RSCalibrationAngle v_angle_cali[32] = + { + 0x01, htons(0x2829), // -10.281 + 0x00, htons(0x091d) // 2.333 + }; + + RSCalibrationAngle h_angle_cali[32] = + { + 0x00, htons(0x01f4), // 0.5 + 0x01, htons(0x01c2) // -0.45 + }; + + RS32DifopPkt src; + src.rpm = 0; + src.fov = {0}; + src.return_mode = {0}; + src.sn = {0}; + src.eth = {0}; + src.version = {0}; + src.status = {0}; + + memcpy (src.vert_angle_cali, v_angle_cali, 32*sizeof(RSCalibrationAngle)); + memcpy (src.horiz_angle_cali, h_angle_cali, 32*sizeof(RSCalibrationAngle)); + + AdapterDifopPkt dst; + RS32DifopPkt2Adapter(src, dst); + + ASSERT_EQ(dst.vert_angle_cali[0].sign, 1); + ASSERT_EQ(ntohs(dst.vert_angle_cali[0].value), 1028); + ASSERT_EQ(dst.vert_angle_cali[1].sign, 0); + ASSERT_EQ(ntohs(dst.vert_angle_cali[1].value), 233); + + ASSERT_EQ(dst.horiz_angle_cali[0].sign, 0); + ASSERT_EQ(ntohs(dst.horiz_angle_cali[0].value), 50); + ASSERT_EQ(dst.horiz_angle_cali[1].sign, 1); + ASSERT_EQ(ntohs(dst.horiz_angle_cali[1].value), 45); +} + +TEST(TestDecoderRS32, decodeDifopPkt) +{ + // const_param + RSDecoderParam param; + DecoderRS32 decoder(param); + decoder.regCallback(errCallback, nullptr); + ASSERT_EQ(decoder.blks_per_frame_, 1801); + ASSERT_EQ(decoder.split_blks_per_frame_, 1801); + + // rpm = 600, dual return + RS32DifopPkt pkt; + pkt.rpm = htons(600); + pkt.return_mode = 0; + decoder.decodeDifopPkt((uint8_t*)&pkt, sizeof(pkt)); + ASSERT_EQ(decoder.rps_, 10); + ASSERT_EQ(decoder.echo_mode_, RSEchoMode::ECHO_DUAL); + ASSERT_EQ(decoder.blks_per_frame_, 1801); + ASSERT_EQ(decoder.split_blks_per_frame_, 3602); + + // rpm = 1200, single return + pkt.rpm = htons(1200); + pkt.return_mode = 1; + decoder.decodeDifopPkt((uint8_t*)&pkt, sizeof(pkt)); + ASSERT_EQ(decoder.rps_, 20); + ASSERT_EQ(decoder.echo_mode_, RSEchoMode::ECHO_SINGLE); + ASSERT_EQ(decoder.blks_per_frame_, 900); + ASSERT_EQ(decoder.split_blks_per_frame_, 900); +} + +static void splitFrame(uint16_t height, double ts) +{ +} + +TEST(TestDecoderRS32, decodeMsopPkt) +{ + uint8_t pkt[] = + { + // + // header + // + 0x55, 0xAA, 0x05, 0x0A, 0x5A, 0xA5, 0x50, 0xA0, // msop id + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // reserved_1 + 0x15, 0x0a, 0x01, 0x01, 0x02, 0x03, 0x11, 0x22, 0x33, 0x44, // ts_YMD + 0x00, // lidar type + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // reserved_2 + 0x18, 0x01, // temprature + 0x00, 0x00, // reserved_3 + + // + // block_01 + // + 0xFF, 0xEE, // block id + 0x00, 0x00, // azimuth + 0x03, 0xE8, // chan_00, distance + 0x01, // chan_00, intensity + 0x00, 0x00, // chan_01, distance + 0x00, // chan_01, intensity + 0x00, 0x00, // chan_02, distance + 0x00, // chan_02, intensity + 0x00, 0x00, // chan_03, distance + 0x00, // chan_03, intensity + 0x00, 0x00, // chan_04, distance + 0x00, // chan_04, intensity + 0x00, 0x00, // chan_05, distance + 0x00, // chan_05, intensity + 0x00, 0x00, // chan_06, distance + 0x00, // chan_06, intensity + 0x00, 0x00, // chan_07, distance + 0x00, // chan_07, intensity + 0x00, 0x00, // chan_08, distance + 0x00, // chan_08, intensity + 0x00, 0x00, // chan_09, distance + 0x00, // chan_09, intensity + 0x00, 0x00, // chan_10, distance + 0x00, // chan_10, intensity + 0x00, 0x00, // chan_11, distance + 0x00, // chan_11, intensity + 0x00, 0x00, // chan_12, distance + 0x00, // chan_12, intensity + 0x00, 0x00, // chan_13, distance + 0x00, // chan_13, intensity + 0x00, 0x00, // chan_14, distance + 0x00, // chan_14, intensity + 0x00, 0x00, // chan_15, distance + 0x00, // chan_15, intensity + 0x00, 0x00, // chan_16, distance + 0x00, // chan_16, intensity + 0x00, 0x00, // chan_17, distance + 0x00, // chan_17, intensity + 0x00, 0x00, // chan_18, distance + 0x00, // chan_18, intensity + 0x00, 0x00, // chan_19, distance + 0x00, // chan_19, intensity + 0x00, 0x00, // chan_20, distance + 0x00, // chan_20, intensity + 0x00, 0x00, // chan_21, distance + 0x00, // chan_21, intensity + 0x00, 0x00, // chan_22, distance + 0x00, // chan_22, intensity + 0x00, 0x00, // chan_23, distance + 0x00, // chan_23, intensity + 0x00, 0x00, // chan_24, distance + 0x00, // chan_24, intensity + 0x00, 0x00, // chan_25, distance + 0x00, // chan_25, intensity + 0x00, 0x00, // chan_26, distance + 0x00, // chan_26, intensity + 0x00, 0x00, // chan_27, distance + 0x00, // chan_27, intensity + 0x00, 0x00, // chan_28, distance + 0x00, // chan_28, intensity + 0x00, 0x00, // chan_29, distance + 0x00, // chan_29, intensity + 0x00, 0x00, // chan_30, distance + 0x00, // chan_30, intensity + 0x00, 0x00, // chan_31, distance + 0x00, // chan_31, intensity + + // + // block_02 + // + 0x00, 0x00, // block id + }; + + // dense_points = false, use_lidar_clock = true + RSDecoderParam param; + DecoderRS32 decoder(param); + decoder.regCallback(errCallback, splitFrame); + ASSERT_EQ(decoder.chan_angles_.user_chans_.size(), 32); + decoder.chan_angles_.user_chans_[0] = 2; + decoder.chan_angles_.user_chans_[1] = 1; + decoder.param_.dense_points = false; + decoder.param_.use_lidar_clock = true; + + decoder.point_cloud_ = std::make_shared(); + + decoder.decodeMsopPkt(pkt, sizeof(pkt)); + float temp = 0.0f; + ASSERT_EQ(decoder.getTemperature(temp), true); + ASSERT_EQ(temp, 2.1875); + ASSERT_EQ(decoder.point_cloud_->points.size(), 32); + + PointT& point = decoder.point_cloud_->points[0]; + ASSERT_EQ(point.intensity, 1); + ASSERT_NE(point.timestamp, 0); + ASSERT_EQ(point.ring, 2); +} + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/test/decoder_rsbp_test.cpp b/src/airy/rslidar_sdk-main/src/rs_driver/test/decoder_rsbp_test.cpp new file mode 100644 index 0000000..1086fb9 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/test/decoder_rsbp_test.cpp @@ -0,0 +1,175 @@ + +#include + +#include +#include +#include + +using namespace robosense::lidar; + +typedef PointXYZIRT PointT; +typedef PointCloudT PointCloud; + +static ErrCode errCode = ERRCODE_SUCCESS; +static void errCallback(const Error& err) +{ + errCode = err.error_code; +} + +TEST(TestDecoderRSBP, getEchoMode) +{ + ASSERT_TRUE(DecoderRSBP::getEchoMode(0) == RSEchoMode::ECHO_DUAL); + ASSERT_TRUE(DecoderRSBP::getEchoMode(1) == RSEchoMode::ECHO_SINGLE); + ASSERT_TRUE(DecoderRSBP::getEchoMode(2) == RSEchoMode::ECHO_SINGLE); +} + +TEST(TestDecoderRSBP, decodeDifopPkt) +{ + // const_param + RSDecoderParam param; + DecoderRSBP decoder(param); + decoder.regCallback(errCallback, nullptr); + + ASSERT_EQ(decoder.blks_per_frame_, 1801); + ASSERT_EQ(decoder.split_blks_per_frame_, 1801); + + // rpm = 600, dual return + RSBPDifopPkt pkt; + pkt.rpm = htons(600); + pkt.return_mode = 0; + decoder.decodeDifopPkt((uint8_t*)&pkt, sizeof(pkt)); + ASSERT_EQ(decoder.rps_, 10); + ASSERT_EQ(decoder.echo_mode_, RSEchoMode::ECHO_DUAL); + ASSERT_EQ(decoder.blks_per_frame_, 1801); + ASSERT_EQ(decoder.split_blks_per_frame_, 3602); + + // rpm = 1200, single return + pkt.rpm = htons(1200); + pkt.return_mode = 1; + decoder.decodeDifopPkt((uint8_t*)&pkt, sizeof(pkt)); + ASSERT_EQ(decoder.rps_, 20); + ASSERT_EQ(decoder.echo_mode_, RSEchoMode::ECHO_SINGLE); + ASSERT_EQ(decoder.blks_per_frame_, 900); + ASSERT_EQ(decoder.split_blks_per_frame_, 900); +} + +static void splitFrame(uint16_t height, double ts) +{ +} + +TEST(TestDecoderRSBP, decodeMsopPkt) +{ + uint8_t pkt[] = + { + // + // header + // + 0x55, 0xAA, 0x05, 0x0A, 0x5A, 0xA5, 0x50, 0xA0, // msop id + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // reserved_1 + 0x15, 0x0a, 0x01, 0x01, 0x02, 0x03, 0x11, 0x22, 0x33, 0x44, // ts_YMD + 0x00, // lidar type + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // reserved_2 + 0x18, 0x01, // temprature + 0x00, 0x00, // reserved_3 + + // + // block_01 + // + 0xFF, 0xEE, // block id + 0x00, 0x00, // azimuth + 0x03, 0xE8, // chan_00, distance + 0x01, // chan_00, intensity + 0x00, 0x00, // chan_01, distance + 0x00, // chan_01, intensity + 0x00, 0x00, // chan_02, distance + 0x00, // chan_02, intensity + 0x00, 0x00, // chan_03, distance + 0x00, // chan_03, intensity + 0x00, 0x00, // chan_04, distance + 0x00, // chan_04, intensity + 0x00, 0x00, // chan_05, distance + 0x00, // chan_05, intensity + 0x00, 0x00, // chan_06, distance + 0x00, // chan_06, intensity + 0x00, 0x00, // chan_07, distance + 0x00, // chan_07, intensity + 0x00, 0x00, // chan_08, distance + 0x00, // chan_08, intensity + 0x00, 0x00, // chan_09, distance + 0x00, // chan_09, intensity + 0x00, 0x00, // chan_10, distance + 0x00, // chan_10, intensity + 0x00, 0x00, // chan_11, distance + 0x00, // chan_11, intensity + 0x00, 0x00, // chan_12, distance + 0x00, // chan_12, intensity + 0x00, 0x00, // chan_13, distance + 0x00, // chan_13, intensity + 0x00, 0x00, // chan_14, distance + 0x00, // chan_14, intensity + 0x00, 0x00, // chan_15, distance + 0x00, // chan_15, intensity + 0x00, 0x00, // chan_16, distance + 0x00, // chan_16, intensity + 0x00, 0x00, // chan_17, distance + 0x00, // chan_17, intensity + 0x00, 0x00, // chan_18, distance + 0x00, // chan_18, intensity + 0x00, 0x00, // chan_19, distance + 0x00, // chan_19, intensity + 0x00, 0x00, // chan_20, distance + 0x00, // chan_20, intensity + 0x00, 0x00, // chan_21, distance + 0x00, // chan_21, intensity + 0x00, 0x00, // chan_22, distance + 0x00, // chan_22, intensity + 0x00, 0x00, // chan_23, distance + 0x00, // chan_23, intensity + 0x00, 0x00, // chan_24, distance + 0x00, // chan_24, intensity + 0x00, 0x00, // chan_25, distance + 0x00, // chan_25, intensity + 0x00, 0x00, // chan_26, distance + 0x00, // chan_26, intensity + 0x00, 0x00, // chan_27, distance + 0x00, // chan_27, intensity + 0x00, 0x00, // chan_28, distance + 0x00, // chan_28, intensity + 0x00, 0x00, // chan_29, distance + 0x00, // chan_29, intensity + 0x00, 0x00, // chan_30, distance + 0x00, // chan_30, intensity + 0x00, 0x00, // chan_31, distance + 0x00, // chan_31, intensity + + // + // block_02 + // + 0x00, 0x00, // block id + }; + + // dense_points = false, use_lidar_clock = true + RSDecoderParam param; + DecoderRSBP decoder(param); + decoder.regCallback(errCallback, splitFrame); + + ASSERT_EQ(decoder.chan_angles_.user_chans_.size(), 32); + decoder.chan_angles_.user_chans_[0] = 2; + decoder.chan_angles_.user_chans_[1] = 1; + decoder.param_.dense_points = false; + decoder.param_.use_lidar_clock = true; + + decoder.point_cloud_ = std::make_shared(); + + decoder.decodeMsopPkt(pkt, sizeof(pkt)); + float temp = 0.0f; + ASSERT_EQ(decoder.getTemperature(temp), true); + ASSERT_EQ(temp, 2.1875); + ASSERT_EQ(decoder.point_cloud_->points.size(), 32); + + PointT& point = decoder.point_cloud_->points[0]; + ASSERT_EQ(point.intensity, 1); + ASSERT_NE(point.timestamp, 0); + ASSERT_EQ(point.ring, 2); +} + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/test/decoder_test.cpp b/src/airy/rslidar_sdk-main/src/rs_driver/test/decoder_test.cpp new file mode 100644 index 0000000..e2ef49d --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/test/decoder_test.cpp @@ -0,0 +1,296 @@ + +#include + +#include +#include +#include + +using namespace robosense::lidar; + +typedef PointXYZI PointT; +typedef PointCloudT PointCloud; + +#pragma pack(push, 1) +struct MyMsopPkt +{ + uint8_t id[8]{0}; +}; + +struct MyDifopPkt +{ + uint8_t id[8]; + uint16_t rpm; + + RSFOV fov; + RSCalibrationAngle vert_angle_cali[2]; + RSCalibrationAngle horiz_angle_cali[2]; + RSSN sn; + RSEthNetV2 eth; + RSVersionV2 version; + RSStatusV1 status; +}; +#pragma pack(pop) + +class MyDecoder : public DecoderMech +{ +public: + MyDecoder(const RSDecoderMechConstParam& const_param, + const RSDecoderParam& param) + : DecoderMech(const_param, param) + { + } + + virtual void decodeDifopPkt(const uint8_t* packet, size_t size) + { + const MyDifopPkt& pkt = *(const MyDifopPkt*)(packet); + this->template decodeDifopCommon(pkt); + } + + virtual bool decodeMsopPkt(const uint8_t* pkt, size_t size) + { + return false; + } + +}; + +static ErrCode errCode = ERRCODE_SUCCESS; + +static void errCallback(const Error& err) +{ + errCode = err.error_code; +} + +TEST(TestDecoder, angles_from_file) +{ + RSDecoderMechConstParam const_param; + const_param.base.LASER_NUM = 4; + + RSDecoderParam param; + param.config_from_file = true; + param.angle_path = "../test/res/angle.csv"; + + errCode = ERRCODE_SUCCESS; + MyDecoder decoder(const_param, param); + decoder.regCallback(errCallback, nullptr); + ASSERT_EQ(errCode, ERRCODE_SUCCESS); + + ASSERT_TRUE(decoder.angles_ready_); +} + +TEST(TestDecoder, angles_from_file_fail) +{ + RSDecoderMechConstParam const_param; + const_param.base.LASER_NUM = 4; + + RSDecoderParam param; + param.config_from_file = true; + param.angle_path = "../test/res/non_exist.csv"; + + MyDecoder decoder(const_param, param); + decoder.regCallback(errCallback, nullptr); + ASSERT_FALSE(decoder.angles_ready_); +} + +TEST(TestDecoder, processDifopPkt_fail) +{ + RSDecoderMechConstParam const_param = + { + sizeof(MyMsopPkt) // msop len + , sizeof(MyDifopPkt) // difop len + , 8 // msop id len + , 8 // difop id len + , {0x55, 0xAA, 0x05, 0x0A, 0x5A, 0xA5, 0x50, 0xA0} // msop id + , {0xA5, 0xFF, 0x00, 0x5A, 0x11, 0x11, 0x55, 0x55} // difop id + }; + + RSDecoderParam param; + MyDecoder decoder(const_param, param); + decoder.regCallback(errCallback, nullptr); + + // wrong difop length + MyDifopPkt pkt = {0}; + errCode = ERRCODE_SUCCESS; + decoder.processDifopPkt((const uint8_t*)&pkt, 10); + ASSERT_EQ(errCode, ERRCODE_WRONGDIFOPLEN); + + // wrong difop id + errCode = ERRCODE_SUCCESS; + decoder.processDifopPkt((const uint8_t*)&pkt, sizeof(pkt)); + ASSERT_EQ(errCode, ERRCODE_WRONGDIFOPID); +} + +TEST(TestDecoder, processDifopPkt) +{ + RSDecoderMechConstParam const_param = + { + sizeof(MyMsopPkt) // msop len + , sizeof(MyDifopPkt) // difop len + , 8 // msop id len + , 8 // difop id len + , {0x55, 0xAA, 0x05, 0x0A, 0x5A, 0xA5, 0x50, 0xA0} // msop id + , {0xA5, 0xFF, 0x00, 0x5A, 0x11, 0x11, 0x55, 0x55} // difop id + , {0xFF, 0xEE} // block id + , 2 // laser number + , 1000 // blocks per packet + , 2 // channels per block + }; + + const_param.BLOCK_DURATION = 55.52f / 1000000; + + RSDecoderParam param; + param.config_from_file = false; + MyDecoder decoder(const_param, param); + decoder.regCallback(errCallback, nullptr); + ASSERT_FALSE(decoder.angles_ready_); + + // + // angles from difop. no angles in difop + // + + uint8_t pkt_no_angles[] = + { + 0xA5, 0xFF, 0x00, 0x5A, 0x11, 0x11, 0x55, 0x55 // difop id + , 0x02, 0x58 // rpm + , 0x23, 0x28 // start angle = 9000 + , 0x46, 0x50 // end angle = 18000 + , 0xFF, 0xFF, 0xFF // vert angles + , 0xFF, 0xFF, 0xFF + , 0xFF, 0xFF, 0xFF // horiz angles + , 0xFF, 0xFF, 0xFF + }; + + errCode = ERRCODE_SUCCESS; + decoder.processDifopPkt(pkt_no_angles, sizeof(MyDifopPkt)); + errCode = ERRCODE_SUCCESS; + + ASSERT_EQ(decoder.rps_, 10); + ASSERT_EQ(decoder.blks_per_frame_, 1801); + ASSERT_EQ(decoder.block_az_diff_, 20); + ASSERT_EQ(decoder.split_blks_per_frame_, 1801); + ASSERT_EQ(decoder.fov_blind_ts_diff_, 0.075); // 0.1 * 3/4 + ASSERT_FALSE(decoder.angles_ready_); + ASSERT_EQ(decoder.chan_angles_.vert_angles_.size(), 2); + ASSERT_EQ(decoder.chan_angles_.vert_angles_[0], 0); + ASSERT_EQ(decoder.chan_angles_.horiz_angles_.size(), 2); + ASSERT_EQ(decoder.chan_angles_.horiz_angles_[0], 0); + + // + // angles from difop. valid angels in difop. + // + uint8_t pkt[] = + { + 0xA5, 0xFF, 0x00, 0x5A, 0x11, 0x11, 0x55, 0x55 // difop id + , 0x02, 0x58 // rpm + , 0x00, 0x00 // start angle = 0 + , 0x8C, 0xA0 // end angle = 36000 + , 0x00, 0x00, 0x10 // vert angles + , 0x01, 0x00, 0x20 + , 0x00, 0x00, 0x01 // horiz angles + , 0x01, 0x00, 0x02 + }; + + ASSERT_LT(decoder.getPacketDuration() - 55.52/1000, 0.00001); + + errCode = ERRCODE_SUCCESS; + decoder.processDifopPkt(pkt, sizeof(MyDifopPkt)); + ASSERT_EQ(errCode, ERRCODE_SUCCESS); + + ASSERT_EQ(decoder.rps_, 10); + ASSERT_EQ(decoder.fov_blind_ts_diff_, 0.0f); // 0.1 * 3/4 + ASSERT_TRUE(decoder.angles_ready_); + ASSERT_EQ(decoder.chan_angles_.vert_angles_.size(), 2); + ASSERT_EQ(decoder.chan_angles_.vert_angles_[0], 16); + ASSERT_EQ(decoder.chan_angles_.horiz_angles_.size(), 2); + ASSERT_EQ(decoder.chan_angles_.horiz_angles_[0], 1); +} + +TEST(TestDecoder, processDifopPkt_invalid_rpm) +{ + RSDecoderMechConstParam const_param = + { + sizeof(MyMsopPkt) // msop len + , sizeof(MyDifopPkt) // difop len + , 8 // msop id len + , 8 // difop id len + , {0x55, 0xAA, 0x05, 0x0A, 0x5A, 0xA5, 0x50, 0xA0} // msop id + , {0xA5, 0xFF, 0x00, 0x5A, 0x11, 0x11, 0x55, 0x55} // difop id + , {0xFF, 0xEE} // block id + , 32 // laser number + , 12 // blocks per packet + , 32 // channels per block + }; + + RSDecoderParam param; + MyDecoder decoder(const_param, param); + decoder.regCallback(errCallback, nullptr); + + uint8_t pkt[] = + { + 0xA5, 0xFF, 0x00, 0x5A, 0x11, 0x11, 0x55, 0x55 // difop len + , 0x00, 0x00 // rpm = 0 + }; + + errCode = ERRCODE_SUCCESS; + decoder.processDifopPkt(pkt, sizeof(MyDifopPkt)); + ASSERT_EQ(errCode, ERRCODE_SUCCESS); + ASSERT_EQ(decoder.rps_, 10); +} + +TEST(TestDecoder, processMsopPkt) +{ + RSDecoderMechConstParam const_param = + { + sizeof(MyMsopPkt) // msop len + , sizeof(MyDifopPkt) // difop len + , 8 // msop id len + , 8 // difop id len + , {0x55, 0xAA, 0x05, 0x0A, 0x5A, 0xA5, 0x50, 0xA0} // msop id + , {0xA5, 0xFF, 0x00, 0x5A, 0x11, 0x11, 0x55, 0x55} // difop id + }; + + MyMsopPkt pkt; + RSDecoderParam param; + MyDecoder decoder(const_param, param); + decoder.regCallback(errCallback, nullptr); + + // wait_for_difop = true, angles not ready + decoder.param_.wait_for_difop = true; + decoder.angles_ready_ = false; + errCode = ERRCODE_SUCCESS; + decoder.processMsopPkt((const uint8_t*)&pkt, 2); + ASSERT_EQ(errCode, ERRCODE_SUCCESS); + +#if 0 + sleep(2); + errCode = ERRCODE_SUCCESS; + decoder.processMsopPkt((const uint8_t*)&pkt, 2); + ASSERT_EQ(errCode, ERRCODE_NODIFOPRECV); +#endif + + decoder.param_.wait_for_difop = true; + decoder.angles_ready_ = true; + + // wrong msop len + errCode = ERRCODE_SUCCESS; + decoder.processMsopPkt((const uint8_t*)&pkt, 2); + ASSERT_EQ(errCode, ERRCODE_WRONGMSOPLEN); + + decoder.param_.wait_for_difop = false; + + // wrong msop header + errCode = ERRCODE_SUCCESS; + decoder.processMsopPkt((const uint8_t*)&pkt, sizeof(pkt)); + ASSERT_EQ(errCode, ERRCODE_WRONGMSOPID); + + // valid msop + uint8_t id[] = {0x55, 0xAA, 0x05, 0x0A, 0x5A, 0xA5, 0x50, 0xA0}; + memcpy (pkt.id, id, 8); + errCode = ERRCODE_SUCCESS; + decoder.processMsopPkt((const uint8_t*)&pkt, sizeof(pkt)); +#ifdef ENABLE_CRC32_CHECK + ASSERT_EQ(errCode, ERRCODE_WRONGCRC32); +#else + ASSERT_EQ(errCode, ERRCODE_SUCCESS); +#endif +} + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/test/dual_return_block_iterator_test.cpp b/src/airy/rslidar_sdk-main/src/rs_driver/test/dual_return_block_iterator_test.cpp new file mode 100644 index 0000000..14223d8 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/test/dual_return_block_iterator_test.cpp @@ -0,0 +1,124 @@ + +#include + +#include +#include + +using namespace robosense::lidar; + +typedef struct +{ + uint16_t distance; + uint8_t intensity; +} MyChannel; + +typedef struct +{ + uint16_t azimuth; + MyChannel channels[2]; +} MyBlock; + +typedef struct +{ + MyBlock blocks[6]; +} MyPacket; + +TEST(TestDualPacketTraverser, ctor) +{ + MyPacket pkt = + { + htons(1), 0x00, 0x00, 0x00, 0x00 + , htons(1), 0x00, 0x00, 0x00, 0x00 + , htons(21), 0x00, 0x00, 0x00, 0x00 + , htons(21), 0x00, 0x00, 0x00, 0x00 + , htons(51), 0x00, 0x00, 0x00, 0x00 + , htons(51), 0x00, 0x00, 0x00, 0x00 + }; + + DualReturnBlockIterator iter(pkt, + 6, // blocks per packet + 0.5f, // block_duration + 25, // block_az_duraton + 2.0f); // fov_blind_duration + + int32_t az_diff; + double ts; + + // first block + iter.get (0, az_diff, ts); + ASSERT_EQ(az_diff, 20); + ASSERT_EQ(ts, 0.0f); + + // second block + iter.get (1, az_diff, ts); + ASSERT_EQ(az_diff, 20); + ASSERT_EQ(ts, 0.0f); + + // third block + iter.get (2, az_diff, ts); + ASSERT_EQ(az_diff, 30); + ASSERT_EQ(ts, 0.5f); + + // last block + iter.get (4, az_diff, ts); + ASSERT_EQ(az_diff, 25); + ASSERT_EQ(ts, 1.0f); + + // last block + iter.get (5, az_diff, ts); + ASSERT_EQ(az_diff, 25); + ASSERT_EQ(ts, 1.0f); +} + +TEST(TestDualPacketTraverser, ctor_fov) +{ + MyPacket pkt = + { + htons(1), 0x00, 0x00, 0x00, 0x00 + , htons(1), 0x00, 0x00, 0x00, 0x00 + , htons(21), 0x00, 0x00, 0x00, 0x00 + , htons(21), 0x00, 0x00, 0x00, 0x00 + , htons(141), 0x00, 0x00, 0x00, 0x00 + , htons(141), 0x00, 0x00, 0x00, 0x00 + }; + + DualReturnBlockIterator iter(pkt, + 6, // blocks per packet + 0.5f, // block_duration + 25, // block_az_duraton + 2.0f); // fov_blind_duration + + int32_t az_diff; + double ts; + + // first block + iter.get (0, az_diff, ts); + ASSERT_EQ(az_diff, 20); + ASSERT_EQ(ts, 0.0f); + + // second block + iter.get (1, az_diff, ts); + ASSERT_EQ(az_diff, 20); + ASSERT_EQ(ts, 0.0f); + + // third block + iter.get (2, az_diff, ts); + ASSERT_EQ(az_diff, 25); + ASSERT_EQ(ts, 0.5f); + + // fourth block + iter.get (2, az_diff, ts); + ASSERT_EQ(az_diff, 25); + ASSERT_EQ(ts, 0.5f); + + // last block + iter.get (4, az_diff, ts); + ASSERT_EQ(az_diff, 25); + ASSERT_EQ(ts, 2.5f); + + // last block + iter.get (5, az_diff, ts); + ASSERT_EQ(az_diff, 25); + ASSERT_EQ(ts, 2.5f); +} + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/test/res/angle.csv b/src/airy/rslidar_sdk-main/src/rs_driver/test/res/angle.csv new file mode 100644 index 0000000..0c02c83 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/test/res/angle.csv @@ -0,0 +1,4 @@ +5,0.1 +2.5,-0.2 +0,0 +-2.5,-1 diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/test/rs16_dual_return_block_iterator_test.cpp b/src/airy/rslidar_sdk-main/src/rs_driver/test/rs16_dual_return_block_iterator_test.cpp new file mode 100644 index 0000000..29cd933 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/test/rs16_dual_return_block_iterator_test.cpp @@ -0,0 +1,92 @@ + +#include + +#include +#include + +using namespace robosense::lidar; + +typedef struct +{ + uint16_t distance; + uint8_t intensity; +} MyChannel; + +typedef struct +{ + uint16_t azimuth; + MyChannel channels[2]; +} MyBlock; + +typedef struct +{ + MyBlock blocks[3]; +} MyPacket; + +TEST(TestRs16DualReturnBlockIterator, ctor) +{ + MyPacket pkt = + { + htons(1), 0x00, 0x00, 0x00, 0x00 + , htons(21), 0x00, 0x00, 0x00, 0x00 + , htons(51), 0x00, 0x00, 0x00, 0x00 + }; + + Rs16DualReturnBlockIterator iter(pkt, + 3, // blocks per packet + 0.5f, // block_duration + 25, // block_az_duraton + 2.0f); // fov_blind_duration + + int32_t az_diff; + double ts; + + // first block + iter.get (0, az_diff, ts); + ASSERT_EQ(az_diff, 20); + ASSERT_EQ(ts, 0.0f); + + // second block + iter.get (1, az_diff, ts); + ASSERT_EQ(az_diff, 30); + ASSERT_EQ(ts, 0.5f); + + // third block + iter.get (2, az_diff, ts); + ASSERT_EQ(az_diff, 25); + ASSERT_EQ(ts, 1.0f); +} + +TEST(TestRs16DualReturnBlockIterator, ctor_fov) +{ + MyPacket pkt = + { + htons(1), 0x00, 0x00, 0x00, 0x00 + , htons(21), 0x00, 0x00, 0x00, 0x00 + , htons(141), 0x00, 0x00, 0x00, 0x00 + }; + + Rs16DualReturnBlockIterator iter(pkt, + 3, // blocks per packet + 0.5f, // block_duration 25, // block_az_duraton + 25, // block_az_duraton + 2.0f); // fov_blind_duration + + int32_t az_diff; + double ts; + + // first block + iter.get (0, az_diff, ts); + ASSERT_EQ(az_diff, 20); + ASSERT_EQ(ts, 0.0f); + + // second block + iter.get (1, az_diff, ts); + ASSERT_EQ(az_diff, 25); + ASSERT_EQ(ts, 0.5f); + + // third block + iter.get (2, az_diff, ts); + ASSERT_EQ(az_diff, 25); + ASSERT_EQ(ts, 2.5f); +} diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/test/rs16_single_return_block_iterator_test.cpp b/src/airy/rslidar_sdk-main/src/rs_driver/test/rs16_single_return_block_iterator_test.cpp new file mode 100644 index 0000000..0174c79 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/test/rs16_single_return_block_iterator_test.cpp @@ -0,0 +1,93 @@ + +#include + +#include +#include + +using namespace robosense::lidar; + +typedef struct +{ + uint16_t distance; + uint8_t intensity; +} MyChannel; + +typedef struct +{ + uint16_t azimuth; + MyChannel channels[2]; +} MyBlock; + +typedef struct +{ + MyBlock blocks[3]; +} MyPacket; + +TEST(TestRs16SingleReturnBlockIterator, ctor) +{ + MyPacket pkt = + { + htons(1), 0x00, 0x00, 0x00, 0x00 + , htons(21), 0x00, 0x00, 0x00, 0x00 + , htons(51), 0x00, 0x00, 0x00, 0x00 + }; + + Rs16SingleReturnBlockIterator iter(pkt, + 3, // blocks per packet + 0.5f, // block_duration + 25, // block_az_duraton + 2.0f); // fov_blind_duration + + int32_t az_diff; + double ts; + + // first block + iter.get (0, az_diff, ts); + ASSERT_EQ(az_diff, 20); + ASSERT_EQ(ts, 0.0f); + + // second block + iter.get (1, az_diff, ts); + ASSERT_EQ(az_diff, 30); + ASSERT_EQ(ts, 1.0f); + + // third block + iter.get (2, az_diff, ts); + ASSERT_EQ(az_diff, 50); + ASSERT_EQ(ts, 2.0f); +} + +TEST(TestRs16SingleReturnBlockIterator, ctor_fov) +{ + MyPacket pkt = + { + htons(1), 0x00, 0x00, 0x00, 0x00 + , htons(21), 0x00, 0x00, 0x00, 0x00 + , htons(141), 0x00, 0x00, 0x00, 0x00 + }; + + Rs16SingleReturnBlockIterator iter(pkt, + 3, // blocks per packet + 0.5f, // block_duration 25, // block_az_duraton + 25, // block_az_duraton + 2.0f); // fov_blind_duration + + int32_t az_diff; + double ts; + + // first block + iter.get (0, az_diff, ts); + ASSERT_EQ(az_diff, 20); + ASSERT_EQ(ts, 0.0f); + + // second block + iter.get (1, az_diff, ts); + ASSERT_EQ(az_diff, 50); + ASSERT_EQ(ts, 1.0f); + + // third block + iter.get (2, az_diff, ts); + ASSERT_EQ(az_diff, 50); + ASSERT_EQ(ts, 3.0f); +} + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/test/rs_common_test.cpp b/src/airy/rslidar_sdk-main/src/rs_driver/test/rs_common_test.cpp new file mode 100644 index 0000000..a7b01be --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/test/rs_common_test.cpp @@ -0,0 +1,53 @@ +#include + +#include + +using namespace robosense::lidar; + +// Test RS_SWAP_INT16 +TEST(RS_SWAP_INT16Test, Basic) { + // Test little endian to big endian conversion + EXPECT_EQ(RS_SWAP_INT16(0x1234), 0x3412); + EXPECT_EQ(RS_SWAP_INT16(0x0001), 0x0100); + + // Test big endian to little endian conversion + EXPECT_EQ(RS_SWAP_INT16(0x3412), 0x1234); + EXPECT_EQ(RS_SWAP_INT16(0x0100), 0x0001); + + // Test zero and max values + EXPECT_EQ(RS_SWAP_INT16(0x0000), 0x0000); + EXPECT_EQ(RS_SWAP_INT16(0xFFFF), -1); +} + + +// Test u8ArrayToInt32 +TEST(U8ArrayToInt32Test, ValidInput) { + uint8_t data[] = {0x00, 0x00, 0x00, 0x7B}; // 123 in decimal + int32_t result = u8ArrayToInt32(data, sizeof(data)); + EXPECT_EQ(result, 123); +} + +TEST(U8ArrayToInt32Test, InvalidLength) { + uint8_t data[] = {0x00, 0x00, 0x00}; // Not enough bytes for a 32-bit integer + int32_t result = u8ArrayToInt32(data, sizeof(data)); + EXPECT_EQ(result, 0); // 0 is returned for invalid input +} + +// Test convertUint32ToFloat +TEST(ConvertUint32ToFloatTest, ValidInput) { + uint32_t byteArray = 0x4048F5C3; // 3.14 in decimal + float result = convertUint32ToFloat(byteArray); + EXPECT_FLOAT_EQ(result, 3.14f); // using EXPECT_FLOAT_EQ for float comparison +} + +TEST(ConvertUint32ToFloatTest, ZeroInput) { + uint32_t byteArray = 0x00000000; + float result = convertUint32ToFloat(byteArray); + EXPECT_FLOAT_EQ(result, 0.0f); +} + +TEST(ConvertUint32ToFloatTest, NegativeInput) { + uint32_t byteArray = 0xC0000000; // 对应浮点数 -2.0 + float result = convertUint32ToFloat(byteArray); + EXPECT_FLOAT_EQ(result, -2.0f); +} diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/test/rs_driver_test.cpp b/src/airy/rslidar_sdk-main/src/rs_driver/test/rs_driver_test.cpp new file mode 100644 index 0000000..9efd629 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/test/rs_driver_test.cpp @@ -0,0 +1,8 @@ + +#include + +int main(int argc, char** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/test/section_test.cpp b/src/airy/rslidar_sdk-main/src/rs_driver/test/section_test.cpp new file mode 100644 index 0000000..8c3c189 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/test/section_test.cpp @@ -0,0 +1,86 @@ + +#include + +#include + +using namespace robosense::lidar; + +TEST(TestAzimuthSection, round) +{ + ASSERT_EQ(AzimuthSection::_round(0), 0); + ASSERT_EQ(AzimuthSection::_round(36000), 0); + ASSERT_EQ(AzimuthSection::_round(-36000), 0); +} + +TEST(TestAzimuthSection, ctorFull) +{ + AzimuthSection sec(0, 36000); + ASSERT_TRUE(sec.full_round_); + + ASSERT_TRUE(sec.in(0)); + ASSERT_TRUE(sec.in(10)); + ASSERT_TRUE(sec.in(36000)); +} + +TEST(TestAzimuthSection, ctor) +{ + AzimuthSection sec(10, 20); + ASSERT_FALSE(sec.full_round_); + + ASSERT_EQ(sec.start_, 10); + ASSERT_EQ(sec.end_, 20); + + ASSERT_FALSE(sec.in(5)); + ASSERT_TRUE(sec.in(10)); + ASSERT_TRUE(sec.in(15)); + ASSERT_FALSE(sec.in(20)); + ASSERT_FALSE(sec.in(25)); +} + +TEST(TestAzimuthSection, ctorCrossZero) +{ + AzimuthSection sec(35000, 10); + ASSERT_EQ(sec.start_, 35000); + ASSERT_EQ(sec.end_, 10); + + ASSERT_FALSE(sec.in(34999)); + ASSERT_TRUE(sec.in(35000)); + ASSERT_TRUE(sec.in(0)); + ASSERT_FALSE(sec.in(10)); + ASSERT_FALSE(sec.in(15)); +} + +TEST(TestAzimuthSection, ctorBeyondRound) +{ + AzimuthSection sec(36100, 36200); + ASSERT_EQ(sec.start_, 100); + ASSERT_EQ(sec.end_, 200); +} + +TEST(TestDistanceSection, ctor) +{ + DistanceSection sec(0.5, 200, 0.75, 150); + ASSERT_EQ(sec.min_, 0.75); + ASSERT_EQ(sec.max_, 150); + + ASSERT_FALSE(sec.in(0.45)); + ASSERT_TRUE(sec.in(0.75)); + ASSERT_TRUE(sec.in(0.8)); + ASSERT_TRUE(sec.in(150)); + ASSERT_FALSE(sec.in(150.5)); +} + +TEST(TestDistanceSection, ctorZeroUserDistance) +{ + DistanceSection sec(0.5, 200, 0.0, 0.0); + ASSERT_EQ(sec.min_, 0.5); + ASSERT_EQ(sec.max_, 200); +} + +TEST(TestDistanceSection, ctorNegtiveUserDistance) +{ + DistanceSection sec(0.5, 200, -0.1, -0.2); + ASSERT_EQ(sec.min_, 0.5); + ASSERT_EQ(sec.max_, 200); +} + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/test/single_return_block_iterator_test.cpp b/src/airy/rslidar_sdk-main/src/rs_driver/test/single_return_block_iterator_test.cpp new file mode 100644 index 0000000..e389990 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/test/single_return_block_iterator_test.cpp @@ -0,0 +1,89 @@ + +#include + +#include +#include + +using namespace robosense::lidar; + +typedef struct +{ + uint16_t distance; + uint8_t intensity; +} MyChannel; + +typedef struct +{ + uint16_t azimuth; + MyChannel channels[2]; +} MyBlock; + +typedef struct +{ + MyBlock blocks[3]; +} MyPacket; + +TEST(TestSingleReturnBlockIterator, ctor) +{ + MyPacket pkt = + { + htons(1), 0x00, 0x00, 0x00, 0x00 + , htons(21), 0x00, 0x00, 0x00, 0x00 + , htons(51), 0x00, 0x00, 0x00, 0x00 + }; + + SingleReturnBlockIterator iter(pkt, + 3, // blocks per packet + 0.5f, // block_duration + 25, // block_az_duraton + 2.0f); // fov_blind_duration + + int32_t az_diff; + double ts; + + // first block + iter.get (0, az_diff, ts); + ASSERT_EQ(az_diff, 20); + ASSERT_EQ(ts, 0.0f); + + iter.get (1, az_diff, ts); + ASSERT_EQ(az_diff, 30); + ASSERT_EQ(ts, 0.5f); + + iter.get (2, az_diff, ts); + ASSERT_EQ(az_diff, 25); + ASSERT_EQ(ts, 1.0f); +} + +TEST(TestSingleReturnBlockIterator, ctor_fov) +{ + MyPacket pkt = + { + htons(1), 0x00, 0x00, 0x00, 0x00 + , htons(21), 0x00, 0x00, 0x00, 0x00 + , htons(141), 0x00, 0x00, 0x00, 0x00 + }; + + SingleReturnBlockIterator iter(pkt, + 3, // blocks per packet + 0.5f, // block_duration + 25, // block_az_duraton + 2.0f); // fov_blind_duration + + int32_t az_diff; + double ts; + + // first block + iter.get (0, az_diff, ts); + ASSERT_EQ(az_diff, 20); + ASSERT_EQ(ts, 0.0f); + + iter.get (1, az_diff, ts); + ASSERT_EQ(az_diff, 25); + ASSERT_EQ(ts, 0.5f); + + iter.get (2, az_diff, ts); + ASSERT_EQ(az_diff, 25); + ASSERT_EQ(ts, 2.5f); +} + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/test/split_strategy_test.cpp b/src/airy/rslidar_sdk-main/src/rs_driver/test/split_strategy_test.cpp new file mode 100644 index 0000000..cc29064 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/test/split_strategy_test.cpp @@ -0,0 +1,137 @@ + +#include + +#include + +using namespace robosense::lidar; + +TEST(TestSplitStrategyByAngle, newBlock) +{ + { + SplitStrategyByAngle sa(10); + ASSERT_FALSE(sa.newBlock(5)); + ASSERT_TRUE(sa.newBlock(15)); + } + + { + SplitStrategyByAngle sa(10); + ASSERT_FALSE(sa.newBlock(5)); + ASSERT_TRUE(sa.newBlock(10)); + ASSERT_FALSE(sa.newBlock(15)); + } + + { + SplitStrategyByAngle sa(10); + ASSERT_FALSE(sa.newBlock(10)); + ASSERT_FALSE(sa.newBlock(15)); + } +} + +TEST(TestSplitStrategyByAngle, newBlock_Zero) +{ + { + SplitStrategyByAngle sa(0); + ASSERT_FALSE(sa.newBlock(35999)); + ASSERT_TRUE(sa.newBlock(1)); + ASSERT_FALSE(sa.newBlock(2)); + } + + { + SplitStrategyByAngle sa(0); + ASSERT_FALSE(sa.newBlock(35999)); + ASSERT_TRUE(sa.newBlock(0)); + ASSERT_FALSE(sa.newBlock(2)); + } + + { + SplitStrategyByAngle sa(0); + ASSERT_FALSE(sa.newBlock(0)); + ASSERT_FALSE(sa.newBlock(2)); + } +} + +TEST(TestSplitStrategyByNum, newBlock) +{ + uint16_t max_blks = 2; + SplitStrategyByNum sn(&max_blks); + ASSERT_FALSE(sn.newBlock(0)); + ASSERT_TRUE(sn.newBlock(0)); + ASSERT_FALSE(sn.newBlock(0)); + ASSERT_TRUE(sn.newBlock(0)); + + max_blks = 3; + ASSERT_FALSE(sn.newBlock(0)); + ASSERT_FALSE(sn.newBlock(0)); + ASSERT_TRUE(sn.newBlock(0)); +} + +TEST(TestSplitStrategyBySeq, newPacket_by_seq) +{ + SplitStrategyBySeq sn; + ASSERT_EQ(sn.prev_seq_, 0); + ASSERT_EQ(sn.safe_seq_min_, 0); + ASSERT_EQ(sn.safe_seq_max_, 10); + + // init value + ASSERT_FALSE(sn.newPacket(1)); + ASSERT_EQ(sn.prev_seq_, 1); + ASSERT_EQ(sn.safe_seq_min_, 0); + ASSERT_EQ(sn.safe_seq_max_, 11); + + // too big value + ASSERT_FALSE(sn.newPacket(12)); + ASSERT_EQ(sn.prev_seq_, 1); + ASSERT_EQ(sn.safe_seq_min_, 0); + ASSERT_EQ(sn.safe_seq_max_, 11); + + ASSERT_FALSE(sn.newPacket(9)); + + ASSERT_FALSE(sn.newPacket(12)); + ASSERT_EQ(sn.prev_seq_, 12); + ASSERT_EQ(sn.safe_seq_min_, 2); + ASSERT_EQ(sn.safe_seq_max_, 22); + + ASSERT_FALSE(sn.newPacket(11)); + ASSERT_EQ(sn.prev_seq_, 12); + ASSERT_EQ(sn.safe_seq_min_, 2); + ASSERT_EQ(sn.safe_seq_max_, 22); +} + +TEST(TestSplitStrategyBySeq, newPacket_prev_seq) +{ + SplitStrategyBySeq sn; + ASSERT_EQ(sn.prev_seq_, 0); + ASSERT_EQ(sn.safe_seq_min_, 0); + ASSERT_EQ(sn.safe_seq_max_, 10); + + // init value + ASSERT_FALSE(sn.newPacket(15)); + ASSERT_EQ(sn.prev_seq_, 15); + ASSERT_EQ(sn.safe_seq_min_, 5); + ASSERT_EQ(sn.safe_seq_max_, 25); +} + +TEST(TestSplitStrategyBySeq, newPacket_rewind) +{ + SplitStrategyBySeq sn; + ASSERT_EQ(sn.prev_seq_, 0); + ASSERT_EQ(sn.safe_seq_min_, 0); + ASSERT_EQ(sn.safe_seq_max_, 10); + + // init value + ASSERT_FALSE(sn.newPacket(2)); + ASSERT_EQ(sn.prev_seq_, 2); + ASSERT_EQ(sn.safe_seq_min_, 0); + ASSERT_EQ(sn.safe_seq_max_, 12); + + ASSERT_FALSE(sn.newPacket(10)); + ASSERT_FALSE(sn.newPacket(14)); + ASSERT_EQ(sn.prev_seq_, 14); + ASSERT_EQ(sn.safe_seq_min_, 4); + ASSERT_EQ(sn.safe_seq_max_, 24); + + ASSERT_TRUE(sn.newPacket(1)); + ASSERT_EQ(sn.prev_seq_, 1); + ASSERT_EQ(sn.safe_seq_min_, 0); + ASSERT_EQ(sn.safe_seq_max_, 11); +} diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/test/sync_queue_test.cpp b/src/airy/rslidar_sdk-main/src/rs_driver/test/sync_queue_test.cpp new file mode 100644 index 0000000..f31b803 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/test/sync_queue_test.cpp @@ -0,0 +1,59 @@ + +#include + +#include + +using namespace robosense::lidar; + +TEST(TestSyncQueue, emptyPop) +{ + SyncQueue> queue; + + ASSERT_TRUE(queue.pop().get() == NULL); + ASSERT_TRUE(queue.popWait(1000).get() == NULL); +} + +TEST(TestSyncQueue, nulPtrPop) +{ + SyncQueue> queue; + + { + std::shared_ptr n_ptr; + ASSERT_EQ(queue.push(n_ptr), 1); + ASSERT_TRUE(queue.pop().get() == NULL); + } + + { + std::shared_ptr n_ptr; + ASSERT_EQ(queue.push(n_ptr), 1); + ASSERT_TRUE(queue.popWait(1000).get() == NULL); + } +} + +TEST(TestSyncQueue, valPtrPop) +{ + SyncQueue> queue; + + { + std::shared_ptr v_ptr = std::make_shared(100); + ASSERT_EQ(queue.push(v_ptr), 1); + ASSERT_TRUE(queue.pop().get() != NULL); + } + + { + std::shared_ptr v_ptr = std::make_shared(100); + ASSERT_EQ(queue.push(v_ptr), 1); + ASSERT_TRUE(queue.popWait(1000).get() != NULL); + } +} + +TEST(TestSyncQueue, clear) +{ + SyncQueue> queue; + + std::shared_ptr v_ptr = std::make_shared(100); + ASSERT_EQ(queue.push(v_ptr), 1); + ASSERT_EQ(queue.push(v_ptr), 2); + queue.clear(); + ASSERT_EQ(queue.push(v_ptr), 1); +} diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/test/trigon_test.cpp b/src/airy/rslidar_sdk-main/src/rs_driver/test/trigon_test.cpp new file mode 100644 index 0000000..63aa30f --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/test/trigon_test.cpp @@ -0,0 +1,32 @@ + +#include + +#include + +using namespace robosense::lidar; + +TEST(TestTrigon, ctor) +{ + Trigon trigon; +#if 0 + trigon.print(); +#endif + + ASSERT_EQ(trigon.sin(-9000), -1.0f); + ASSERT_LT(trigon.cos(-9000), 0.0001f); + + ASSERT_EQ(trigon.sin(0), 0.0f); + ASSERT_EQ(trigon.cos(0), 1.0f); + + ASSERT_EQ(trigon.sin(3000), 0.5f); + ASSERT_EQ(trigon.cos(6000), 0.5f); + + trigon.sin(44999); + trigon.cos(44999); + +#if 0 + trigon.sin(45000); + trigon.cos(45000); +#endif +} + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/tool/CMakeLists.txt b/src/airy/rslidar_sdk-main/src/rs_driver/tool/CMakeLists.txt new file mode 100644 index 0000000..8744eab --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/tool/CMakeLists.txt @@ -0,0 +1,64 @@ + +cmake_minimum_required(VERSION 3.5) + +project(rs_driver_viewer) + +message(=============================================================) +message("-- Ready to compile tools") +message(=============================================================) + +include_directories(${DRIVER_INCLUDE_DIRS}) + +if (CMAKE_BUILD_TYPE STREQUAL "") + set(CMAKE_BUILD_TYPE Release) +endif() + +if(${COMPILE_TOOL_VIEWER}) + +if(WIN32) + cmake_policy(SET CMP0074 NEW) + set(OPENNI_ROOT "C:\\Program Files\\OpenNI2") + set(OPENNI_LIBRARY "${OPENNI_ROOT}\\Lib\\OpenNI2.lib") + set(OPENNI_INCLUDE_DIRS "${OPENNI_ROOT}\\Include\\") + file(COPY ${OPENNI_ROOT}\\Redist\\OpenNI2.dll DESTINATION ${PROJECT_BINARY_DIR}\\Release) + file(COPY ${OPENNI_ROOT}\\Redist\\OpenNI2.dll DESTINATION ${PROJECT_BINARY_DIR}\\Debug) +endif(WIN32) + + +find_package(PCL COMPONENTS common visualization io QUIET REQUIRED) +# find_package(PCL 1.14 COMPONENTS common visualization io QUIET REQUIRED) +add_definitions(${PCL_DEFINITIONS}) +include_directories(${PCL_INCLUDE_DIRS}) +link_directories(${PCL_LIBRARY_DIRS}) + +if(PCL_FOUND) + +add_executable(rs_driver_viewer + rs_driver_viewer.cpp) + +target_link_libraries(rs_driver_viewer + ${EXTERNAL_LIBS} + ${PCL_LIBRARIES}) + +else() + +message("PCL Not found! Can not compile rs_driver_viewer!") + +endif() + +install(TARGETS rs_driver_viewer + RUNTIME DESTINATION /usr/bin) + +endif (${COMPILE_TOOL_VIEWER}) + + +if(${COMPILE_TOOL_PCDSAVER}) + +add_executable(rs_driver_pcdsaver + rs_driver_pcdsaver.cpp) + +target_link_libraries(rs_driver_pcdsaver + ${EXTERNAL_LIBS}) + +endif(${COMPILE_TOOL_PCDSAVER}) + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/tool/rs_driver_pcdsaver.cpp b/src/airy/rslidar_sdk-main/src/rs_driver/tool/rs_driver_pcdsaver.cpp new file mode 100644 index 0000000..50a68f9 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/tool/rs_driver_pcdsaver.cpp @@ -0,0 +1,284 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#include +#include + +using namespace robosense::lidar; + +typedef PointCloudT PointCloudMsg; + +SyncQueue> free_cloud_queue; +SyncQueue> stuffed_cloud_queue; + +bool checkKeywordExist(int argc, const char* const* argv, const char* str) +{ + for (int i = 1; i < argc; i++) + { + if (strcmp(argv[i], str) == 0) + { + return true; + } + } + return false; +} + +bool parseArgument(int argc, const char* const* argv, const char* str, std::string& val) +{ + int index = -1; + + for (int i = 1; i < argc; i++) + { + if (strcmp(argv[i], str) == 0) + { + index = i + 1; + } + } + + if (index > 0 && index < argc) + { + val = argv[index]; + return true; + } + + return false; +} + +void parseParam(int argc, char* argv[], RSDriverParam& param) +{ + std::string result_str; + + // + // input param + // + parseArgument(argc, argv, "-pcap", param.input_param.pcap_path); + if (param.input_param.pcap_path.empty()) + { + param.input_type = InputType::ONLINE_LIDAR; + } + else + { + param.input_type = InputType::PCAP_FILE; + } + + if (parseArgument(argc, argv, "-msop", result_str)) + { + param.input_param.msop_port = std::stoi(result_str); + } + + if (parseArgument(argc, argv, "-difop", result_str)) + { + param.input_param.difop_port = std::stoi(result_str); + } + + parseArgument(argc, argv, "-group", param.input_param.group_address); + parseArgument(argc, argv, "-host", param.input_param.host_address); + + // + // decoder param + // + if (parseArgument(argc, argv, "-type", result_str)) + { + param.lidar_type = strToLidarType(result_str); + } + + param.decoder_param.wait_for_difop = false; + + if (parseArgument(argc, argv, "-x", result_str)) + { + param.decoder_param.transform_param.x = std::stof(result_str); + } + + if (parseArgument(argc, argv, "-y", result_str)) + { + param.decoder_param.transform_param.y = std::stof(result_str); + } + + if (parseArgument(argc, argv, "-z", result_str)) + { + param.decoder_param.transform_param.z = std::stof(result_str); + } + + if (parseArgument(argc, argv, "-roll", result_str)) + { + param.decoder_param.transform_param.roll = std::stof(result_str); + } + + if (parseArgument(argc, argv, "-pitch", result_str)) + { + param.decoder_param.transform_param.pitch = std::stof(result_str); + } + + if (parseArgument(argc, argv, "-yaw", result_str)) + { + param.decoder_param.transform_param.yaw = std::stof(result_str); + } +} + +void printHelpMenu() +{ + RS_MSG << "Arguments: " << RS_REND; + RS_MSG << " -type = LiDAR type(RS16, RS32, RSBP, RSHELIOS, RSHELIOS_16P, RS48, RS80, RS128, RSP128, RSP80, RSP48, " + << "RSM1, RSM1_JUMBO, RSM2,RSM3, RSE1, RSMX, RSAIRY)" << RS_REND; + RS_MSG << " -pcap = The path of the pcap file, off-line mode if it is true, else online mode." << RS_REND; + RS_MSG << " -msop = LiDAR msop port number,the default value is 6699" << RS_REND; + RS_MSG << " -difop = LiDAR difop port number,the default value is 7788" << RS_REND; + RS_MSG << " -group = LiDAR destination group address if multi-cast mode." << RS_REND; + RS_MSG << " -host = Host address." << RS_REND; + RS_MSG << " -x = Transformation parameter, unit: m " << RS_REND; + RS_MSG << " -y = Transformation parameter, unit: m " << RS_REND; + RS_MSG << " -z = Transformation parameter, unit: m " << RS_REND; + RS_MSG << " -roll = Transformation parameter, unit: radian " << RS_REND; + RS_MSG << " -pitch = Transformation parameter, unit: radian " << RS_REND; + RS_MSG << " -yaw = Transformation parameter, unit: radian " << RS_REND; +} + +void exceptionCallback(const Error& code) +{ + RS_WARNING << code.toString() << RS_REND; +} + +std::shared_ptr pointCloudGetCallback(void) +{ + std::shared_ptr msg = free_cloud_queue.pop(); + if (msg.get() != NULL) + { + return msg; + } + + return std::make_shared(); +} + +void pointCloudPutCallback(std::shared_ptr msg) +{ + stuffed_cloud_queue.push(msg); +} + +void savePcd(const std::string &pcd_path, const PointCloudMsg &cloud) +{ + RS_MSG << "Save point cloud as " << pcd_path << RS_REND; + + std::ofstream os(pcd_path, std::ios::out | std::ios::trunc); + os << "# .PCD v0.7 - Point Cloud Data file format" << std::endl; + os << "VERSION 0.7" << std::endl; + os << "FIELDS x y z intensity" << std::endl; + os << "SIZE 4 4 4 4" << std::endl; + os << "TYPE F F F F" << std::endl; + os << "COUNT 1 1 1 1" << std::endl; + os << "WIDTH " << cloud.points.size() << std::endl; + os << "HEIGHT 1" << std::endl; + os << "VIEWPOINT 0 0 0 1 0 0 0" << std::endl; + os << "POINTS " << cloud.points.size() << std::endl; + os << "DATA ascii" << std::endl; + + for (size_t i = 0; i < cloud.points.size(); i++) + { + const PointXYZI& p = cloud.points[i]; + os << p.x << " "; + os << p.y << " "; + os << p.z << " "; + os << (float)p.intensity << std::endl; + } +} + +bool to_exit_process = false; +void processCloud(void) +{ + while (!to_exit_process) + { + std::shared_ptr msg = stuffed_cloud_queue.popWait(); + if (msg.get() == NULL) + { + continue; + } + + char pcd_path[128]; + sprintf (pcd_path, "%d.pcd", msg->seq); + savePcd(pcd_path, *msg); + + free_cloud_queue.push(msg); + } +} + +int main(int argc, char* argv[]) +{ + RS_TITLE << "------------------------------------------------------" << RS_REND; + RS_TITLE << " RS_Driver PCD Saver Version: v" << getDriverVersion() << RS_REND; + RS_TITLE << "------------------------------------------------------" << RS_REND; + + if (argc < 2) + { + printHelpMenu(); + return 0; + } + + if (checkKeywordExist(argc, argv, "-h") || checkKeywordExist(argc, argv, "--help")) + { + printHelpMenu(); + return 0; + } + + std::thread cloud_handle_thread = std::thread(processCloud); + + RSDriverParam param; + param.input_param.pcap_repeat = false; + param.decoder_param.dense_points = true; + + parseParam(argc, argv, param); + param.print(); + + LidarDriver driver; + driver.regExceptionCallback(exceptionCallback); + driver.regPointCloudCallback(pointCloudGetCallback, pointCloudPutCallback); + if (!driver.init(param)) + { + RS_ERROR << "Driver Initialize Error..." << RS_REND; + return -1; + } + + RS_INFO << "RoboSense Lidar-Driver PCD Saver start......" << RS_REND; + + driver.start(); + + while (1) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + driver.stop(); + + to_exit_process = true; + cloud_handle_thread.join(); + + return 0; +} + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/tool/rs_driver_viewer.cpp b/src/airy/rslidar_sdk-main/src/rs_driver/tool/rs_driver_viewer.cpp new file mode 100644 index 0000000..75bac92 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/tool/rs_driver_viewer.cpp @@ -0,0 +1,286 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#include +#include + +#include +#include + +using namespace robosense::lidar; +using namespace pcl::visualization; + +typedef PointCloudT PointCloudMsg; + +std::shared_ptr pcl_viewer; +std::mutex mtx_viewer; + +SyncQueue> free_cloud_queue; +SyncQueue> stuffed_cloud_queue; + +bool checkKeywordExist(int argc, const char* const* argv, const char* str) +{ + for (int i = 1; i < argc; i++) + { + if (strcmp(argv[i], str) == 0) + { + return true; + } + } + return false; +} + +bool parseArgument(int argc, const char* const* argv, const char* str, std::string& val) +{ + int index = -1; + + for (int i = 1; i < argc; i++) + { + if (strcmp(argv[i], str) == 0) + { + index = i + 1; + } + } + + if (index > 0 && index < argc) + { + val = argv[index]; + return true; + } + + return false; +} + +void parseParam(int argc, char* argv[], RSDriverParam& param) +{ + std::string result_str; + + // + // input param + // + parseArgument(argc, argv, "-pcap", param.input_param.pcap_path); + if (param.input_param.pcap_path.empty()) + { + param.input_type = InputType::ONLINE_LIDAR; + } + else + { + param.input_type = InputType::PCAP_FILE; + } + + if (parseArgument(argc, argv, "-msop", result_str)) + { + param.input_param.msop_port = std::stoi(result_str); + } + + if (parseArgument(argc, argv, "-difop", result_str)) + { + param.input_param.difop_port = std::stoi(result_str); + } + + parseArgument(argc, argv, "-group", param.input_param.group_address); + parseArgument(argc, argv, "-host", param.input_param.host_address); + + // + // decoder param + // + if (parseArgument(argc, argv, "-type", result_str)) + { + param.lidar_type = strToLidarType(result_str); + } + + param.decoder_param.wait_for_difop = false; + + if (parseArgument(argc, argv, "-x", result_str)) + { + param.decoder_param.transform_param.x = std::stof(result_str); + } + + if (parseArgument(argc, argv, "-y", result_str)) + { + param.decoder_param.transform_param.y = std::stof(result_str); + } + + if (parseArgument(argc, argv, "-z", result_str)) + { + param.decoder_param.transform_param.z = std::stof(result_str); + } + + if (parseArgument(argc, argv, "-roll", result_str)) + { + param.decoder_param.transform_param.roll = std::stof(result_str); + } + + if (parseArgument(argc, argv, "-pitch", result_str)) + { + param.decoder_param.transform_param.pitch = std::stof(result_str); + } + + if (parseArgument(argc, argv, "-yaw", result_str)) + { + param.decoder_param.transform_param.yaw = std::stof(result_str); + } +} + +void printHelpMenu() +{ + RS_MSG << "Arguments: " << RS_REND; + RS_MSG << " -type = LiDAR type(RS16, RS32, RSBP, RSHELIOS, RSHELIOS_16P, RS48, RS80, RS128, RSP128, RSP80, RSP48, " + << "RSM1, RSM1_JUMBO, RSM2,RSM3, RSE1, RSMX, RSAIRY)" << RS_REND; + RS_MSG << " -pcap = The path of the pcap file, off-line mode if it is true, else online mode." << RS_REND; + RS_MSG << " -msop = LiDAR msop port number,the default value is 6699" << RS_REND; + RS_MSG << " -difop = LiDAR difop port number,the default value is 7788" << RS_REND; + RS_MSG << " -group = LiDAR destination group address if multi-cast mode." << RS_REND; + RS_MSG << " -host = Host address." << RS_REND; + RS_MSG << " -x = Transformation parameter, unit: m " << RS_REND; + RS_MSG << " -y = Transformation parameter, unit: m " << RS_REND; + RS_MSG << " -z = Transformation parameter, unit: m " << RS_REND; + RS_MSG << " -roll = Transformation parameter, unit: radian " << RS_REND; + RS_MSG << " -pitch = Transformation parameter, unit: radian " << RS_REND; + RS_MSG << " -yaw = Transformation parameter, unit: radian " << RS_REND; +} + +void exceptionCallback(const Error& code) +{ + RS_WARNING << code.toString() << RS_REND; +} + +std::shared_ptr pointCloudGetCallback(void) +{ + std::shared_ptr msg = free_cloud_queue.pop(); + if (msg.get() != NULL) + { + return msg; + } + + return std::make_shared(); +} + +void pointCloudPutCallback(std::shared_ptr msg) +{ + stuffed_cloud_queue.push(msg); +} + +bool to_exit_process = false; +void processCloud(void) +{ + while (!to_exit_process) + { + std::shared_ptr msg = stuffed_cloud_queue.popWait(); + if (msg.get() == NULL) + { + continue; + } + + // + // show the point cloud + // + pcl::PointCloud::Ptr pcl_pointcloud(new pcl::PointCloud); + pcl_pointcloud->points.swap(msg->points); + pcl_pointcloud->height = msg->height; + pcl_pointcloud->width = msg->width; + pcl_pointcloud->is_dense = msg->is_dense; + + PointCloudColorHandlerGenericField point_color_handle(pcl_pointcloud, "intensity"); + + { + const std::lock_guard lock(mtx_viewer); + pcl_viewer->updatePointCloud(pcl_pointcloud, point_color_handle, "rslidar"); + } + + free_cloud_queue.push(msg); + } +} + +int main(int argc, char* argv[]) +{ + RS_TITLE << "------------------------------------------------------" << RS_REND; + RS_TITLE << " RS_Driver Viewer Version: v" << getDriverVersion() << RS_REND; + RS_TITLE << "------------------------------------------------------" << RS_REND; + + if (argc < 2) + { + printHelpMenu(); + return 0; + } + + if (checkKeywordExist(argc, argv, "-h") || checkKeywordExist(argc, argv, "--help")) + { + printHelpMenu(); + return 0; + } + + std::thread cloud_handle_thread = std::thread(processCloud); + + RSDriverParam param; + parseParam(argc, argv, param); + param.print(); + + pcl_viewer = std::make_shared("RSPointCloudViewer"); + pcl_viewer->setBackgroundColor(0.0, 0.0, 0.0); + pcl_viewer->addCoordinateSystem(1.0); + + pcl::PointCloud::Ptr pcl_pointcloud(new pcl::PointCloud); + pcl_viewer->addPointCloud(pcl_pointcloud, "rslidar"); + pcl_viewer->setPointCloudRenderingProperties(PCL_VISUALIZER_POINT_SIZE, 2, "rslidar"); + + LidarDriver driver; + driver.regExceptionCallback(exceptionCallback); + driver.regPointCloudCallback(pointCloudGetCallback, pointCloudPutCallback); + if (!driver.init(param)) + { + RS_ERROR << "Driver Initialize Error..." << RS_REND; + return -1; + } + + RS_INFO << "RoboSense Lidar-Driver Viewer start......" << RS_REND; + + driver.start(); + + while (!pcl_viewer->wasStopped()) + { + { + const std::lock_guard lock(mtx_viewer); + pcl_viewer->spinOnce(); + } + + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + driver.stop(); + + to_exit_process = true; + cloud_handle_thread.join(); + + return 0; +} + diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/win/demo_online/demo_online.vcxproj b/src/airy/rslidar_sdk-main/src/rs_driver/win/demo_online/demo_online.vcxproj new file mode 100644 index 0000000..1e17ae8 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/win/demo_online/demo_online.vcxproj @@ -0,0 +1,144 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + + + + 15.0 + {A30B3B76-84F3-4A17-B42B-6284FCC3138F} + demoonline + 10.0 + + + + Application + true + v142 + MultiByte + + + Application + false + v142 + true + MultiByte + + + Application + true + v142 + MultiByte + + + Application + false + v142 + true + MultiByte + + + + + + + + + + + + + + + + + + + + + C:\Program Files\WpdPack_4_1_2\WpdPack\Include;$(IncludePath) + C:\Program Files\WpdPack_4_1_2\WpdPack\Lib\x64;$(LibraryPath) + + + + Level3 + Disabled + true + true + ../../../src/ + _CRT_SECURE_NO_WARNINGS;%(PreprocessorDefinitions) + + + Console + kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies);Ws2_32.lib + + + + + Level3 + Disabled + true + true + ../../src;../../../3th-party/libpcap/Include + _MBCS;%(PreprocessorDefinitions);_CRT_SECURE_NO_WARNINGS + + + Console + kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies);Ws2_32.lib;wpcap.lib + ../../../3th-party/libpcap/Lib/x64 + + + + + Level3 + MaxSpeed + true + true + true + true + + + Console + true + true + + + + + Level3 + MaxSpeed + true + true + true + true + ../../src + _CRT_SECURE_NO_WARNINGS;%(PreprocessorDefinitions) + + + Console + true + true + wpcap.lib;ws2_32.lib;%(AdditionalDependencies) + + + + + + \ No newline at end of file diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/win/demo_pcap/demo_pcap.vcxproj b/src/airy/rslidar_sdk-main/src/rs_driver/win/demo_pcap/demo_pcap.vcxproj new file mode 100644 index 0000000..2a7052e --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/win/demo_pcap/demo_pcap.vcxproj @@ -0,0 +1,141 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + 15.0 + {E7971DE6-C89C-4572-A3A4-07BE68897D30} + demopcap + 10.0 + + + + Application + true + v142 + MultiByte + + + Application + false + v142 + true + MultiByte + + + Application + true + v142 + MultiByte + + + Application + false + v142 + true + MultiByte + + + + + + + + + + + + + + + + + + + + + C:\Program Files\WpdPack_4_1_2\WpdPack\Lib\x64;$(LibraryPath) + C:\Program Files\WpdPack_4_1_2\WpdPack\Include;$(IncludePath) + + + + Level3 + Disabled + true + true + ../../src;;../../../3th-party/libpcap/Include + _MBCS;%(PreprocessorDefinitions);_CRT_SECURE_NO_WARNINGS;ENABLE_PCAP_PARSE + + + Console + kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies);Ws2_32.lib;wpcap.lib + ../../../3th-party/libpcap/Lib/x64 + + + + + Level3 + Disabled + true + true + + + Console + + + + + Level3 + MaxSpeed + true + true + true + true + + + Console + true + true + + + + + Level3 + MaxSpeed + true + true + true + true + ../../src + _CRT_SECURE_NO_WARNINGS;%(PreprocessorDefinitions) + + + Console + true + true + wpcap.lib;ws2_32.lib;%(AdditionalDependencies) + + + + + + + + + \ No newline at end of file diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/win/rs_driver.sln b/src/airy/rslidar_sdk-main/src/rs_driver/win/rs_driver.sln new file mode 100644 index 0000000..c9d9963 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/win/rs_driver.sln @@ -0,0 +1,51 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Version 16 +VisualStudioVersion = 16.0.32602.291 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "demo_online", "demo_online\demo_online.vcxproj", "{A30B3B76-84F3-4A17-B42B-6284FCC3138F}" +EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "demo_pcap", "demo_pcap\demo_pcap.vcxproj", "{E7971DE6-C89C-4572-A3A4-07BE68897D30}" +EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "rs_driver_viewer", "rs_driver_viewer\rs_driver_viewer.vcxproj", "{F15B6ACB-D381-48D2-B6F1-E4817FADF216}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|x64 = Debug|x64 + Debug|x86 = Debug|x86 + Release|x64 = Release|x64 + Release|x86 = Release|x86 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {A30B3B76-84F3-4A17-B42B-6284FCC3138F}.Debug|x64.ActiveCfg = Debug|x64 + {A30B3B76-84F3-4A17-B42B-6284FCC3138F}.Debug|x64.Build.0 = Debug|x64 + {A30B3B76-84F3-4A17-B42B-6284FCC3138F}.Debug|x86.ActiveCfg = Debug|Win32 + {A30B3B76-84F3-4A17-B42B-6284FCC3138F}.Debug|x86.Build.0 = Debug|Win32 + {A30B3B76-84F3-4A17-B42B-6284FCC3138F}.Release|x64.ActiveCfg = Release|x64 + {A30B3B76-84F3-4A17-B42B-6284FCC3138F}.Release|x64.Build.0 = Release|x64 + {A30B3B76-84F3-4A17-B42B-6284FCC3138F}.Release|x86.ActiveCfg = Release|Win32 + {A30B3B76-84F3-4A17-B42B-6284FCC3138F}.Release|x86.Build.0 = Release|Win32 + {E7971DE6-C89C-4572-A3A4-07BE68897D30}.Debug|x64.ActiveCfg = Debug|x64 + {E7971DE6-C89C-4572-A3A4-07BE68897D30}.Debug|x64.Build.0 = Debug|x64 + {E7971DE6-C89C-4572-A3A4-07BE68897D30}.Debug|x86.ActiveCfg = Debug|Win32 + {E7971DE6-C89C-4572-A3A4-07BE68897D30}.Debug|x86.Build.0 = Debug|Win32 + {E7971DE6-C89C-4572-A3A4-07BE68897D30}.Release|x64.ActiveCfg = Release|x64 + {E7971DE6-C89C-4572-A3A4-07BE68897D30}.Release|x64.Build.0 = Release|x64 + {E7971DE6-C89C-4572-A3A4-07BE68897D30}.Release|x86.ActiveCfg = Release|Win32 + {E7971DE6-C89C-4572-A3A4-07BE68897D30}.Release|x86.Build.0 = Release|Win32 + {F15B6ACB-D381-48D2-B6F1-E4817FADF216}.Debug|x64.ActiveCfg = Debug|x64 + {F15B6ACB-D381-48D2-B6F1-E4817FADF216}.Debug|x64.Build.0 = Debug|x64 + {F15B6ACB-D381-48D2-B6F1-E4817FADF216}.Debug|x86.ActiveCfg = Debug|Win32 + {F15B6ACB-D381-48D2-B6F1-E4817FADF216}.Debug|x86.Build.0 = Debug|Win32 + {F15B6ACB-D381-48D2-B6F1-E4817FADF216}.Release|x64.ActiveCfg = Release|x64 + {F15B6ACB-D381-48D2-B6F1-E4817FADF216}.Release|x64.Build.0 = Release|x64 + {F15B6ACB-D381-48D2-B6F1-E4817FADF216}.Release|x86.ActiveCfg = Release|Win32 + {F15B6ACB-D381-48D2-B6F1-E4817FADF216}.Release|x86.Build.0 = Release|Win32 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection + GlobalSection(ExtensibilityGlobals) = postSolution + SolutionGuid = {56237513-8A68-4EC5-9B6B-BAB05BA945B4} + EndGlobalSection +EndGlobal diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/win/rs_driver_viewer/rs_driver_viewer.vcxproj b/src/airy/rslidar_sdk-main/src/rs_driver/win/rs_driver_viewer/rs_driver_viewer.vcxproj new file mode 100644 index 0000000..5de8a52 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/rs_driver/win/rs_driver_viewer/rs_driver_viewer.vcxproj @@ -0,0 +1,155 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + 16.0 + Win32Proj + {f15b6acb-d381-48d2-b6f1-e4817fadf216} + rsdriverviewer + 10.0 + + + + Application + true + v142 + Unicode + + + Application + false + v142 + true + Unicode + + + Application + true + v142 + Unicode + + + Application + false + v142 + true + Unicode + + + + + + + + + + + + + + + + + + + + + true + + + false + + + true + C:\Program Files\WpdPack_4_1_2\WpdPack\Include;C:\Program Files\PCL 1.11.1\include\pcl-1.11;C:\Program Files\PCL 1.11.1\3rdParty\Boost\include\boost-1_74;C:\Program Files\PCL 1.11.1\3rdParty\Eigen\eigen3;C:\Program Files\PCL 1.11.1\3rdParty\FLANN\include;C:\Program Files\PCL 1.11.1\3rdParty\Qhull\include;C:\Program Files\PCL 1.11.1\3rdParty\VTK\include\vtk-8.2;C:\Program Files\OpenNI2\Include;$(IncludePath) + C:\Program Files\WpdPack_4_1_2\WpdPack\Lib\x64;C:\Program Files\PCL 1.11.1\lib;C:\Program Files\PCL 1.11.1\3rdParty\Boost\lib;C:\Program Files\PCL 1.11.1\3rdParty\FLANN\lib;C:\Program Files\PCL 1.11.1\3rdParty\Qhull\lib;C:\Program Files\PCL 1.11.1\3rdParty\VTK\lib;C:\Program Files\OpenNI2\Lib;$(LibraryPath) + + + false + C:\Program Files\WpdPack_4_1_2\WpdPack\Include;C:\Program Files\PCL 1.11.1\include\pcl-1.11;C:\Program Files\PCL 1.11.1\3rdParty\Boost\include\boost-1_74;C:\Program Files\PCL 1.11.1\3rdParty\Eigen\eigen3;C:\Program Files\PCL 1.11.1\3rdParty\FLANN\include;C:\Program Files\PCL 1.11.1\3rdParty\Qhull\include;C:\Program Files\PCL 1.11.1\3rdParty\VTK\include\vtk-8.2;C:\Program Files\OpenNI2\Include;$(IncludePath) + C:\Program Files\WpdPack_4_1_2\WpdPack\Lib\x64;C:\Program Files\PCL 1.11.1\lib;C:\Program Files\PCL 1.11.1\3rdParty\Boost\lib;C:\Program Files\PCL 1.11.1\3rdParty\FLANN\lib;C:\Program Files\PCL 1.11.1\3rdParty\Qhull\lib;C:\Program Files\PCL 1.11.1\3rdParty\VTK\lib;C:\Program Files\OpenNI2\Lib;$(LibraryPath) + + + + Level3 + true + WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + + + Console + true + + + + + Level3 + true + true + true + WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + + + Console + true + true + true + + + + + Level3 + false + BOOST_USE_WINDOWS_H;NOMINMAX;_CRT_SECURE_NO_DEPRECATE;_CRT_SECURE_NO_WARNINGS;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + ../../src + + + Console + true + wpcap.lib;ws2_32.lib;vtkChartsCore-8.2.lib;vtkCommonColor-8.2.lib;vtkCommonComputationalGeometry-8.2.lib;vtkCommonCore-8.2.lib;vtkCommonDataModel-8.2.lib;vtkCommonExecutionModel-8.2.lib;vtkCommonMath-8.2.lib;vtkCommonMisc-8.2.lib;vtkCommonSystem-8.2.lib;vtkCommonTransforms-8.2.lib;vtkDICOMParser-8.2.lib;vtkDomainsChemistry-8.2.lib;vtkDomainsChemistryOpenGL2-8.2.lib;vtkdoubleconversion-8.2.lib;vtkexodusII-8.2.lib;vtkexpat-8.2.lib;vtkFiltersAMR-8.2.lib;vtkFiltersCore-8.2.lib;vtkFiltersExtraction-8.2.lib;vtkFiltersFlowPaths-8.2.lib;vtkFiltersGeneral-8.2.lib;vtkFiltersGeneric-8.2.lib;vtkFiltersGeometry-8.2.lib;vtkFiltersHybrid-8.2.lib;vtkFiltersHyperTree-8.2.lib;vtkFiltersImaging-8.2.lib;vtkFiltersModeling-8.2.lib;vtkFiltersParallel-8.2.lib;vtkFiltersParallelImaging-8.2.lib;vtkFiltersPoints-8.2.lib;vtkFiltersProgrammable-8.2.lib;vtkFiltersSelection-8.2.lib;vtkFiltersSMP-8.2.lib;vtkFiltersSources-8.2.lib;vtkFiltersStatistics-8.2.lib;vtkFiltersTexture-8.2.lib;vtkFiltersTopology-8.2.lib;vtkFiltersVerdict-8.2.lib;vtkfreetype-8.2.lib;vtkGeovisCore-8.2.lib;vtkgl2ps-8.2.lib;vtkglew-8.2.lib;vtkGUISupportMFC-8.2.lib;vtkhdf5-8.2.lib;vtkhdf5_hl-8.2.lib;vtkImagingColor-8.2.lib;vtkImagingCore-8.2.lib;vtkImagingFourier-8.2.lib;vtkImagingGeneral-8.2.lib;vtkImagingHybrid-8.2.lib;vtkImagingMath-8.2.lib;vtkImagingMorphological-8.2.lib;vtkImagingSources-8.2.lib;vtkImagingStatistics-8.2.lib;vtkImagingStencil-8.2.lib;vtkInfovisCore-8.2.lib;vtkInfovisLayout-8.2.lib;vtkInteractionImage-8.2.lib;vtkInteractionStyle-8.2.lib;vtkInteractionWidgets-8.2.lib;vtkIOAMR-8.2.lib;vtkIOAsynchronous-8.2.lib;vtkIOCityGML-8.2.lib;vtkIOCore-8.2.lib;vtkIOEnSight-8.2.lib;vtkIOExodus-8.2.lib;vtkIOExport-8.2.lib;vtkIOExportOpenGL2-8.2.lib;vtkIOExportPDF-8.2.lib;vtkIOGeometry-8.2.lib;vtkIOImage-8.2.lib;vtkIOImport-8.2.lib;vtkIOInfovis-8.2.lib;vtkIOLegacy-8.2.lib;vtkIOLSDyna-8.2.lib;vtkIOMINC-8.2.lib;vtkIOMovie-8.2.lib;vtkIONetCDF-8.2.lib;vtkIOParallel-8.2.lib;vtkIOParallelXML-8.2.lib;vtkIOPLY-8.2.lib;vtkIOSegY-8.2.lib;vtkIOSQL-8.2.lib;vtkIOTecplotTable-8.2.lib;vtkIOVeraOut-8.2.lib;vtkIOVideo-8.2.lib;vtkIOXML-8.2.lib;vtkIOXMLParser-8.2.lib;vtkjpeg-8.2.lib;vtkjsoncpp-8.2.lib;vtklibharu-8.2.lib;vtklibxml2-8.2.lib;vtklz4-8.2.lib;vtklzma-8.2.lib;vtkmetaio-8.2.lib;vtkNetCDF-8.2.lib;vtkogg-8.2.lib;vtkParallelCore-8.2.lib;vtkpng-8.2.lib;vtkproj-8.2.lib;vtkpugixml-8.2.lib;vtkRenderingAnnotation-8.2.lib;vtkRenderingContext2D-8.2.lib;vtkRenderingContextOpenGL2-8.2.lib;vtkRenderingCore-8.2.lib;vtkRenderingExternal-8.2.lib;vtkRenderingFreeType-8.2.lib;vtkRenderingGL2PSOpenGL2-8.2.lib;vtkRenderingImage-8.2.lib;vtkRenderingLabel-8.2.lib;vtkRenderingLOD-8.2.lib;vtkRenderingOpenGL2-8.2.lib;vtkRenderingVolume-8.2.lib;vtkRenderingVolumeOpenGL2-8.2.lib;vtksqlite-8.2.lib;vtksys-8.2.lib;vtktheora-8.2.lib;vtktiff-8.2.lib;vtkverdict-8.2.lib;vtkViewsContext2D-8.2.lib;vtkViewsCore-8.2.lib;vtkViewsInfovis-8.2.lib;vtkzlib-8.2.lib;pcl_common.lib;pcl_commond.lib;pcl_features.lib;pcl_featuresd.lib;pcl_filters.lib;pcl_filtersd.lib;pcl_io.lib;pcl_iod.lib;pcl_io_ply.lib;pcl_io_plyd.lib;pcl_kdtree.lib;pcl_kdtreed.lib;pcl_keypoints.lib;pcl_keypointsd.lib;pcl_ml.lib;pcl_mld.lib;pcl_octree.lib;pcl_octreed.lib;pcl_outofcore.lib;pcl_outofcored.lib;pcl_people.lib;pcl_peopled.lib;pcl_recognition.lib;pcl_recognitiond.lib;pcl_registration.lib;pcl_registrationd.lib;pcl_sample_consensus.lib;pcl_sample_consensusd.lib;pcl_search.lib;pcl_searchd.lib;pcl_segmentation.lib;pcl_segmentationd.lib;pcl_stereo.lib;pcl_stereod.lib;pcl_surface.lib;pcl_surfaced.lib;pcl_tracking.lib;pcl_trackingd.lib;pcl_visualization.lib;pcl_visualizationd.lib;%(AdditionalDependencies) + + + + + Level3 + true + true + false + BOOST_USE_WINDOWS_H;NOMINMAX;_CRT_SECURE_NO_DEPRECATE;_CRT_SECURE_NO_WARNINGS;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + ../../src + + + Console + true + true + true + wpcap.lib;ws2_32.lib;vtkChartsCore-8.2.lib;vtkCommonColor-8.2.lib;vtkCommonComputationalGeometry-8.2.lib;vtkCommonCore-8.2.lib;vtkCommonDataModel-8.2.lib;vtkCommonExecutionModel-8.2.lib;vtkCommonMath-8.2.lib;vtkCommonMisc-8.2.lib;vtkCommonSystem-8.2.lib;vtkCommonTransforms-8.2.lib;vtkDICOMParser-8.2.lib;vtkDomainsChemistry-8.2.lib;vtkDomainsChemistryOpenGL2-8.2.lib;vtkdoubleconversion-8.2.lib;vtkexodusII-8.2.lib;vtkexpat-8.2.lib;vtkFiltersAMR-8.2.lib;vtkFiltersCore-8.2.lib;vtkFiltersExtraction-8.2.lib;vtkFiltersFlowPaths-8.2.lib;vtkFiltersGeneral-8.2.lib;vtkFiltersGeneric-8.2.lib;vtkFiltersGeometry-8.2.lib;vtkFiltersHybrid-8.2.lib;vtkFiltersHyperTree-8.2.lib;vtkFiltersImaging-8.2.lib;vtkFiltersModeling-8.2.lib;vtkFiltersParallel-8.2.lib;vtkFiltersParallelImaging-8.2.lib;vtkFiltersPoints-8.2.lib;vtkFiltersProgrammable-8.2.lib;vtkFiltersSelection-8.2.lib;vtkFiltersSMP-8.2.lib;vtkFiltersSources-8.2.lib;vtkFiltersStatistics-8.2.lib;vtkFiltersTexture-8.2.lib;vtkFiltersTopology-8.2.lib;vtkFiltersVerdict-8.2.lib;vtkfreetype-8.2.lib;vtkGeovisCore-8.2.lib;vtkgl2ps-8.2.lib;vtkglew-8.2.lib;vtkGUISupportMFC-8.2.lib;vtkhdf5-8.2.lib;vtkhdf5_hl-8.2.lib;vtkImagingColor-8.2.lib;vtkImagingCore-8.2.lib;vtkImagingFourier-8.2.lib;vtkImagingGeneral-8.2.lib;vtkImagingHybrid-8.2.lib;vtkImagingMath-8.2.lib;vtkImagingMorphological-8.2.lib;vtkImagingSources-8.2.lib;vtkImagingStatistics-8.2.lib;vtkImagingStencil-8.2.lib;vtkInfovisCore-8.2.lib;vtkInfovisLayout-8.2.lib;vtkInteractionImage-8.2.lib;vtkInteractionStyle-8.2.lib;vtkInteractionWidgets-8.2.lib;vtkIOAMR-8.2.lib;vtkIOAsynchronous-8.2.lib;vtkIOCityGML-8.2.lib;vtkIOCore-8.2.lib;vtkIOEnSight-8.2.lib;vtkIOExodus-8.2.lib;vtkIOExport-8.2.lib;vtkIOExportOpenGL2-8.2.lib;vtkIOExportPDF-8.2.lib;vtkIOGeometry-8.2.lib;vtkIOImage-8.2.lib;vtkIOImport-8.2.lib;vtkIOInfovis-8.2.lib;vtkIOLegacy-8.2.lib;vtkIOLSDyna-8.2.lib;vtkIOMINC-8.2.lib;vtkIOMovie-8.2.lib;vtkIONetCDF-8.2.lib;vtkIOParallel-8.2.lib;vtkIOParallelXML-8.2.lib;vtkIOPLY-8.2.lib;vtkIOSegY-8.2.lib;vtkIOSQL-8.2.lib;vtkIOTecplotTable-8.2.lib;vtkIOVeraOut-8.2.lib;vtkIOVideo-8.2.lib;vtkIOXML-8.2.lib;vtkIOXMLParser-8.2.lib;vtkjpeg-8.2.lib;vtkjsoncpp-8.2.lib;vtklibharu-8.2.lib;vtklibxml2-8.2.lib;vtklz4-8.2.lib;vtklzma-8.2.lib;vtkmetaio-8.2.lib;vtkNetCDF-8.2.lib;vtkogg-8.2.lib;vtkParallelCore-8.2.lib;vtkpng-8.2.lib;vtkproj-8.2.lib;vtkpugixml-8.2.lib;vtkRenderingAnnotation-8.2.lib;vtkRenderingContext2D-8.2.lib;vtkRenderingContextOpenGL2-8.2.lib;vtkRenderingCore-8.2.lib;vtkRenderingExternal-8.2.lib;vtkRenderingFreeType-8.2.lib;vtkRenderingGL2PSOpenGL2-8.2.lib;vtkRenderingImage-8.2.lib;vtkRenderingLabel-8.2.lib;vtkRenderingLOD-8.2.lib;vtkRenderingOpenGL2-8.2.lib;vtkRenderingVolume-8.2.lib;vtkRenderingVolumeOpenGL2-8.2.lib;vtksqlite-8.2.lib;vtksys-8.2.lib;vtktheora-8.2.lib;vtktiff-8.2.lib;vtkverdict-8.2.lib;vtkViewsContext2D-8.2.lib;vtkViewsCore-8.2.lib;vtkViewsInfovis-8.2.lib;vtkzlib-8.2.lib;pcl_common.lib;pcl_commond.lib;pcl_features.lib;pcl_featuresd.lib;pcl_filters.lib;pcl_filtersd.lib;pcl_io.lib;pcl_iod.lib;pcl_io_ply.lib;pcl_io_plyd.lib;pcl_kdtree.lib;pcl_kdtreed.lib;pcl_keypoints.lib;pcl_keypointsd.lib;pcl_ml.lib;pcl_mld.lib;pcl_octree.lib;pcl_octreed.lib;pcl_outofcore.lib;pcl_outofcored.lib;pcl_people.lib;pcl_peopled.lib;pcl_recognition.lib;pcl_recognitiond.lib;pcl_registration.lib;pcl_registrationd.lib;pcl_sample_consensus.lib;pcl_sample_consensusd.lib;pcl_search.lib;pcl_searchd.lib;pcl_segmentation.lib;pcl_segmentationd.lib;pcl_stereo.lib;pcl_stereod.lib;pcl_surface.lib;pcl_surfaced.lib;pcl_tracking.lib;pcl_trackingd.lib;pcl_visualization.lib;pcl_visualizationd.lib;%(AdditionalDependencies) + + + + + + + + + \ No newline at end of file diff --git a/src/airy/rslidar_sdk-main/src/source/source.hpp b/src/airy/rslidar_sdk-main/src/source/source.hpp new file mode 100644 index 0000000..1a9eff4 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/source/source.hpp @@ -0,0 +1,146 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once + +#include "msg/rs_msg/lidar_point_cloud_msg.hpp" +#include "utility/yaml_reader.hpp" +#include + + +namespace robosense +{ +namespace lidar +{ + +class DestinationPointCloud +{ +public: + typedef std::shared_ptr Ptr; + + virtual void init(const YAML::Node& config){} + virtual void start() {} + virtual void stop() {} + virtual void sendPointCloud(const LidarPointCloudMsg& msg) = 0; +#ifdef ENABLE_IMU_DATA_PARSE + virtual void sendImuData(const std::shared_ptr& msg) = 0; +#endif + virtual ~DestinationPointCloud() = default; +}; + +class DestinationPacket +{ +public: + typedef std::shared_ptr Ptr; + + virtual void init(const YAML::Node& config){} + virtual void start() {} + virtual void stop() {} + virtual void sendPacket(const Packet& msg) = 0; + virtual ~DestinationPacket() = default; +}; + +enum SourceType +{ + MSG_FROM_LIDAR = 1, + MSG_FROM_ROS_PACKET = 2, + MSG_FROM_PCAP = 3, +}; + +class Source +{ +public: + typedef std::shared_ptr Ptr; + + virtual void init(const YAML::Node& config) {} + virtual void start() {} + virtual void stop() {} + virtual void regPointCloudCallback(DestinationPointCloud::Ptr dst); + virtual void regPacketCallback(DestinationPacket::Ptr dst); + virtual ~Source() = default; + Source(SourceType src_type); + +protected: + + void sendPacket(const Packet& msg); + void sendPointCloud(std::shared_ptr msg); +#ifdef ENABLE_IMU_DATA_PARSE + void sendImuData(const std::shared_ptr& msg); +#endif + SourceType src_type_; + std::vector pc_cb_vec_; + std::vector pkt_cb_vec_; +}; + +inline Source::Source(SourceType src_type) + : src_type_(src_type) +{ +} + +inline void Source::regPacketCallback(DestinationPacket::Ptr dst) +{ + pkt_cb_vec_.emplace_back(dst); +} + +inline void Source::regPointCloudCallback(DestinationPointCloud::Ptr dst) +{ + pc_cb_vec_.emplace_back(dst); +} + +inline void Source::sendPacket(const Packet& msg) +{ + for (auto iter : pkt_cb_vec_) + { + iter->sendPacket(msg); + } +} + +inline void Source::sendPointCloud(std::shared_ptr msg) +{ + for (auto iter : pc_cb_vec_) + { + iter->sendPointCloud(*msg); + } +} + +#ifdef ENABLE_IMU_DATA_PARSE +inline void Source::sendImuData(const std::shared_ptr& msg) +{ + for (auto iter : pc_cb_vec_) + { + iter->sendImuData(msg); + } +} +#endif + +} // namespace lidar +} // namespace robosense diff --git a/src/airy/rslidar_sdk-main/src/source/source_driver.hpp b/src/airy/rslidar_sdk-main/src/source/source_driver.hpp new file mode 100644 index 0000000..9b42c47 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/source/source_driver.hpp @@ -0,0 +1,283 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once + +#include "source/source.hpp" + +#include +#include + +namespace robosense +{ +namespace lidar +{ + +class SourceDriver : public Source +{ +public: + + virtual void init(const YAML::Node& config); + virtual void start(); + virtual void stop(); + virtual void regPacketCallback(DestinationPacket::Ptr dst); + virtual ~SourceDriver(); + + SourceDriver(SourceType src_type); + +protected: + + std::shared_ptr getPointCloud(void); + void putPointCloud(std::shared_ptr msg); + void putPacket(const Packet& msg); + void putException(const lidar::Error& msg); + void processPointCloud(); + + std::shared_ptr> driver_ptr_; + SyncQueue> free_point_cloud_queue_; + SyncQueue> point_cloud_queue_; +#ifdef ENABLE_IMU_DATA_PARSE + std::shared_ptr getImuData(void); + void putImuData(const std::shared_ptr& msg); + void processImuData(); + SyncQueue> free_imu_data_queue_; + SyncQueue> imu_data_queue_; + std::thread imu_data_process_thread_; +#endif + std::thread point_cloud_process_thread_; + bool to_exit_process_; +}; + +SourceDriver::SourceDriver(SourceType src_type) + : Source(src_type), to_exit_process_(false) +{ +} + +inline void SourceDriver::init(const YAML::Node& config) +{ + YAML::Node driver_config = yamlSubNodeAbort(config, "driver"); + lidar::RSDriverParam driver_param; + + // input related + yamlRead(driver_config, "msop_port", driver_param.input_param.msop_port, 6699); + yamlRead(driver_config, "difop_port", driver_param.input_param.difop_port, 7788); +#ifdef ENABLE_IMU_DATA_PARSE + yamlRead(driver_config, "imu_port", driver_param.input_param.imu_port, 6688); +#endif + yamlRead(driver_config, "host_address", driver_param.input_param.host_address, "0.0.0.0"); + yamlRead(driver_config, "group_address", driver_param.input_param.group_address, "0.0.0.0"); + yamlRead(driver_config, "use_vlan", driver_param.input_param.use_vlan, false); + yamlRead(driver_config, "pcap_path", driver_param.input_param.pcap_path, ""); + yamlRead(driver_config, "pcap_rate", driver_param.input_param.pcap_rate, 1); + yamlRead(driver_config, "pcap_repeat", driver_param.input_param.pcap_repeat, true); + yamlRead(driver_config, "user_layer_bytes", driver_param.input_param.user_layer_bytes, 0); + yamlRead(driver_config, "tail_layer_bytes", driver_param.input_param.tail_layer_bytes, 0); + yamlRead(driver_config, "socket_recv_buf", driver_param.input_param.socket_recv_buf, 106496); + // decoder related + std::string lidar_type; + yamlReadAbort(driver_config, "lidar_type", lidar_type); + driver_param.lidar_type = strToLidarType(lidar_type); + + // decoder + yamlRead(driver_config, "wait_for_difop", driver_param.decoder_param.wait_for_difop, true); + yamlRead(driver_config, "use_lidar_clock", driver_param.decoder_param.use_lidar_clock, false); + yamlRead(driver_config, "min_distance", driver_param.decoder_param.min_distance, 0.2); + yamlRead(driver_config, "max_distance", driver_param.decoder_param.max_distance, 200); + yamlRead(driver_config, "start_angle", driver_param.decoder_param.start_angle, 0); + yamlRead(driver_config, "end_angle", driver_param.decoder_param.end_angle, 360); + yamlRead(driver_config, "dense_points", driver_param.decoder_param.dense_points, false); + yamlRead(driver_config, "ts_first_point", driver_param.decoder_param.ts_first_point, false); + + // mechanical decoder + yamlRead(driver_config, "config_from_file", driver_param.decoder_param.config_from_file, false); + yamlRead(driver_config, "angle_path", driver_param.decoder_param.angle_path, ""); + + uint16_t split_frame_mode; + yamlRead(driver_config, "split_frame_mode", split_frame_mode, 1); + driver_param.decoder_param.split_frame_mode = SplitFrameMode(split_frame_mode); + + yamlRead(driver_config, "split_angle", driver_param.decoder_param.split_angle, 0); + yamlRead(driver_config, "num_blks_split", driver_param.decoder_param.num_blks_split, 0); + + // transform + yamlRead(driver_config, "x", driver_param.decoder_param.transform_param.x, 0); + yamlRead(driver_config, "y", driver_param.decoder_param.transform_param.y, 0); + yamlRead(driver_config, "z", driver_param.decoder_param.transform_param.z, 0); + yamlRead(driver_config, "roll", driver_param.decoder_param.transform_param.roll, 0); + yamlRead(driver_config, "pitch", driver_param.decoder_param.transform_param.pitch, 0); + yamlRead(driver_config, "yaw", driver_param.decoder_param.transform_param.yaw, 0); + + switch (src_type_) + { + case SourceType::MSG_FROM_LIDAR: + driver_param.input_type = InputType::ONLINE_LIDAR; + break; + case SourceType::MSG_FROM_PCAP: + driver_param.input_type = InputType::PCAP_FILE; + break; + default: + driver_param.input_type = InputType::RAW_PACKET; + break; + } + + driver_param.print(); + + driver_ptr_.reset(new lidar::LidarDriver()); + driver_ptr_->regPointCloudCallback(std::bind(&SourceDriver::getPointCloud, this), + std::bind(&SourceDriver::putPointCloud, this, std::placeholders::_1)); + driver_ptr_->regExceptionCallback( + std::bind(&SourceDriver::putException, this, std::placeholders::_1)); + point_cloud_process_thread_ = std::thread(std::bind(&SourceDriver::processPointCloud, this)); + +#ifdef ENABLE_IMU_DATA_PARSE + driver_ptr_->regImuDataCallback(std::bind(&SourceDriver::getImuData, this),std::bind(&SourceDriver::putImuData, this, std::placeholders::_1)); + imu_data_process_thread_ = std::thread(std::bind(&SourceDriver::processImuData, this)); +#endif + + if (!driver_ptr_->init(driver_param)) + { + RS_ERROR << "Driver Initialize Error...." << RS_REND; + exit(-1); + } +} + +inline void SourceDriver::start() +{ + driver_ptr_->start(); +} + +inline SourceDriver::~SourceDriver() +{ + stop(); +} + +inline void SourceDriver::stop() +{ + driver_ptr_->stop(); + + to_exit_process_ = true; + point_cloud_process_thread_.join(); +} + +inline std::shared_ptr SourceDriver::getPointCloud(void) +{ + std::shared_ptr point_cloud = free_point_cloud_queue_.pop(); + if (point_cloud.get() != NULL) + { + return point_cloud; + } + + return std::make_shared(); +} + +inline void SourceDriver::regPacketCallback(DestinationPacket::Ptr dst) +{ + Source::regPacketCallback(dst); + if (pkt_cb_vec_.size() == 1) + { + driver_ptr_->regPacketCallback( + std::bind(&SourceDriver::putPacket, this, std::placeholders::_1)); + } +} + +inline void SourceDriver::putPacket(const Packet& msg) +{ + sendPacket(msg); +} + +void SourceDriver::putPointCloud(std::shared_ptr msg) +{ + point_cloud_queue_.push(msg); +} +#ifdef ENABLE_IMU_DATA_PARSE +inline std::shared_ptr SourceDriver::getImuData(void) +{ + std::shared_ptr imuDataPtr = free_imu_data_queue_.pop(); + if (imuDataPtr.get() != NULL) + { + return imuDataPtr; + } + return std::make_shared(); +} +void SourceDriver::putImuData(const std::shared_ptr& msg) +{ + imu_data_queue_.push(msg); +} + +void SourceDriver::processImuData() +{ + while (!to_exit_process_) + { + std::shared_ptr msg = imu_data_queue_.popWait(100); + if (msg.get() == NULL) + { + continue; + } + sendImuData(msg); + + free_imu_data_queue_.push(msg); + } +} +#endif +void SourceDriver::processPointCloud() +{ + while (!to_exit_process_) + { + std::shared_ptr msg = point_cloud_queue_.popWait(1000); + if (msg.get() == NULL) + { + continue; + } + sendPointCloud(msg); + + free_point_cloud_queue_.push(msg); + } +} + +inline void SourceDriver::putException(const lidar::Error& msg) +{ + switch (msg.error_code_type) + { + case lidar::ErrCodeType::INFO_CODE: + RS_INFO << msg.toString() << RS_REND; + break; + case lidar::ErrCodeType::WARNING_CODE: + RS_WARNING << msg.toString() << RS_REND; + break; + case lidar::ErrCodeType::ERROR_CODE: + RS_ERROR << msg.toString() << RS_REND; + break; + } +} + +} // namespace lidar +} // namespace robosense diff --git a/src/airy/rslidar_sdk-main/src/source/source_packet_ros.hpp b/src/airy/rslidar_sdk-main/src/source/source_packet_ros.hpp new file mode 100644 index 0000000..6bebc9a --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/source/source_packet_ros.hpp @@ -0,0 +1,350 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once + +#include "source/source_driver.hpp" + +#ifdef ROS_FOUND + +#ifdef ENABLE_SOURCE_PACKET_LEGACY +#include "msg/ros_msg/rslidar_packet_legacy.hpp" +#include "msg/ros_msg/rslidar_scan_legacy.hpp" +#endif + +#include "msg/ros_msg/rslidar_packet.hpp" +#include + +namespace robosense +{ +namespace lidar +{ + +inline Packet toRsMsg(const rslidar_msg::RslidarPacket& ros_msg) +{ + Packet rs_msg; + rs_msg.timestamp = ros_msg.header.stamp.toSec(); + rs_msg.seq = ros_msg.header.seq; + rs_msg.is_difop = ros_msg.is_difop; + rs_msg.is_frame_begin = ros_msg.is_frame_begin; + + for (size_t i = 0; i < ros_msg.data.size(); i++) + { + rs_msg.buf_.emplace_back(ros_msg.data[i]); + } + + return rs_msg; +} + +class SourcePacketRos : public SourceDriver +{ +public: + + virtual void init(const YAML::Node& config); + + SourcePacketRos(); + +private: + +#ifdef ENABLE_SOURCE_PACKET_LEGACY + void putPacketLegacy(const rslidar_msgs::rslidarScan& msg); + void putPacketDifopLegacy(const rslidar_msgs::rslidarPacket& msg); + ros::Subscriber pkt_sub_legacy_; + ros::Subscriber pkt_sub_difop_legacy_; + LidarType lidar_type_; +#endif + + void putPacket(const rslidar_msg::RslidarPacket& msg); + ros::Subscriber pkt_sub_; + + std::unique_ptr nh_; +}; + +SourcePacketRos::SourcePacketRos() + : SourceDriver(SourceType::MSG_FROM_ROS_PACKET) +{ +} + +void SourcePacketRos::init(const YAML::Node& config) +{ + SourceDriver::init(config); + + nh_ = std::unique_ptr(new ros::NodeHandle()); + +#ifdef ENABLE_SOURCE_PACKET_LEGACY + std::string lidar_type; + yamlReadAbort(config["driver"], "lidar_type", lidar_type); + lidar_type_ = strToLidarType(lidar_type); + + std::string ros_recv_topic_legacy; + yamlRead(config["ros"], "ros_recv_packet_legacy_topic", + ros_recv_topic_legacy, "rslidar_packets"); + + pkt_sub_legacy_ = nh_->subscribe(ros_recv_topic_legacy, 1, &SourcePacketRos::putPacketLegacy, this); + pkt_sub_difop_legacy_ = nh_->subscribe(ros_recv_topic_legacy + "_difop", 1, &SourcePacketRos::putPacketDifopLegacy, this); +#endif + + std::string ros_recv_topic; + yamlRead(config["ros"], "ros_recv_packet_topic", + ros_recv_topic, "rslidar_packets"); + + pkt_sub_ = nh_->subscribe(ros_recv_topic, 100, &SourcePacketRos::putPacket, this); + +} + +#ifdef ENABLE_SOURCE_PACKET_LEGACY +inline Packet toRsMsg(const rslidar_msgs::rslidarPacket& ros_msg, LidarType lidar_type, bool isDifop) +{ + size_t pkt_len = 1248; + if (lidar_type == LidarType::RSM1) + { + pkt_len = isDifop ? 256 : 1210; + } + + Packet rs_msg; + for (size_t i = 0; i < pkt_len; i++) + { + rs_msg.buf_.emplace_back(ros_msg.data[i]); + } + + return rs_msg; +} + +void SourcePacketRos::putPacketLegacy(const rslidar_msgs::rslidarScan& msg) +{ + for (uint32_t i = 0; i < msg.packets.size(); i++) + { + const rslidar_msgs::rslidarPacket& packet = msg.packets[i]; + driver_ptr_->decodePacket(toRsMsg(packet, lidar_type_, false)); + } +} + +void SourcePacketRos::putPacketDifopLegacy(const rslidar_msgs::rslidarPacket& msg) +{ + driver_ptr_->decodePacket(toRsMsg(msg, lidar_type_, true)); +} +#endif + +void SourcePacketRos::putPacket(const rslidar_msg::RslidarPacket& msg) +{ + driver_ptr_->decodePacket(toRsMsg(msg)); +} + +inline rslidar_msg::RslidarPacket toRosMsg(const Packet& rs_msg, const std::string& frame_id) +{ + rslidar_msg::RslidarPacket ros_msg; + ros_msg.header.stamp = ros_msg.header.stamp.fromSec(rs_msg.timestamp); + ros_msg.header.seq = rs_msg.seq; + ros_msg.header.frame_id = frame_id; + ros_msg.is_difop = rs_msg.is_difop; + ros_msg.is_frame_begin = rs_msg.is_frame_begin; + + for (size_t i = 0; i < rs_msg.buf_.size(); i++) + { + ros_msg.data.emplace_back(rs_msg.buf_[i]); + } + + return ros_msg; +} + +class DestinationPacketRos : public DestinationPacket +{ +public: + + virtual void init(const YAML::Node& config); + virtual void sendPacket(const Packet& msg); + virtual ~DestinationPacketRos() = default; + +private: + + std::unique_ptr nh_; + ros::Publisher pkt_pub_; + std::string frame_id_; +}; + +inline void DestinationPacketRos::init(const YAML::Node& config) +{ + yamlRead(config["ros"], + "ros_frame_id", frame_id_, "rslidar"); + + std::string ros_send_topic; + yamlRead(config["ros"], "ros_send_packet_topic", + ros_send_topic, "rslidar_packets"); + + nh_ = std::unique_ptr(new ros::NodeHandle()); + pkt_pub_ = nh_->advertise(ros_send_topic, 100); +} + +inline void DestinationPacketRos::sendPacket(const Packet& msg) +{ + pkt_pub_.publish(toRosMsg(msg, frame_id_)); +} + +} // namespace lidar +} // namespace robosense + +#endif // ROS_FOUND + +#ifdef ROS2_FOUND +#include "rslidar_msg/msg/rslidar_packet.hpp" +#include +#include + +namespace robosense +{ +namespace lidar +{ + +inline Packet toRsMsg(const rslidar_msg::msg::RslidarPacket& ros_msg) +{ + Packet rs_msg; + rs_msg.timestamp = ros_msg.header.stamp.sec + double(ros_msg.header.stamp.nanosec) / 1e9; + //rs_msg.seq = ros_msg.header.seq; + rs_msg.is_difop = ros_msg.is_difop; + rs_msg.is_frame_begin = ros_msg.is_frame_begin; + + for (size_t i = 0; i < ros_msg.data.size(); i++) + { + rs_msg.buf_.emplace_back(ros_msg.data[i]); + } + + return rs_msg; +} + +class SourcePacketRos : public SourceDriver +{ +public: + + virtual void init(const YAML::Node& config); + + SourcePacketRos(); + +private: + void spin(); + void putPacket(const rslidar_msg::msg::RslidarPacket::SharedPtr msg) const; + std::thread subscription_spin_thread_; + std::shared_ptr node_ptr_; + rclcpp::Subscription::SharedPtr pkt_sub_; +}; + +SourcePacketRos::SourcePacketRos() + : SourceDriver(SourceType::MSG_FROM_ROS_PACKET) +{ +} +void SourcePacketRos::spin(){rclcpp::spin(node_ptr_);} +void SourcePacketRos::init(const YAML::Node& config) +{ + SourceDriver::init(config); + + std::string ros_recv_topic; + yamlRead(config["ros"], "ros_recv_packet_topic", + ros_recv_topic, "rslidar_packets"); + + static int node_index = 0; + std::stringstream node_name; + node_name << "rslidar_packets_source_" << node_index++; + + node_ptr_.reset(new rclcpp::Node(node_name.str())); + pkt_sub_ = node_ptr_->create_subscription(ros_recv_topic, 100, + std::bind(&SourcePacketRos::putPacket, this, std::placeholders::_1)); + subscription_spin_thread_ = std::thread(std::bind(&SourcePacketRos::spin,this)); +} + +void SourcePacketRos::putPacket(const rslidar_msg::msg::RslidarPacket::SharedPtr msg) const +{ + driver_ptr_->decodePacket(toRsMsg(*msg)); +} + +inline rslidar_msg::msg::RslidarPacket toRosMsg(const Packet& rs_msg, const std::string& frame_id) +{ + rslidar_msg::msg::RslidarPacket ros_msg; + ros_msg.header.stamp.sec = (uint32_t)floor(rs_msg.timestamp); + ros_msg.header.stamp.nanosec = (uint32_t)round((rs_msg.timestamp - ros_msg.header.stamp.sec) * 1e9); + //ros_msg.header.seq = rs_msg.seq; + ros_msg.header.frame_id = frame_id; + ros_msg.is_difop = rs_msg.is_difop; + ros_msg.is_frame_begin = rs_msg.is_frame_begin; + + for (size_t i = 0; i < rs_msg.buf_.size(); i++) + { + ros_msg.data.emplace_back(rs_msg.buf_[i]); + } + + return ros_msg; +} + +class DestinationPacketRos : public DestinationPacket +{ +public: + + virtual void init(const YAML::Node& config); + virtual void sendPacket(const Packet& msg); + virtual ~DestinationPacketRos() = default; + +private: + + std::shared_ptr node_ptr_; + rclcpp::Publisher::SharedPtr pkt_pub_; + std::string frame_id_; +}; + +inline void DestinationPacketRos::init(const YAML::Node& config) +{ + yamlRead(config["ros"], + "ros_frame_id", frame_id_, "rslidar"); + + std::string ros_send_topic; + yamlRead(config["ros"], "ros_send_packet_topic", + ros_send_topic, "rslidar_packets"); + + size_t ros_queue_length; + yamlRead(config["ros"], "ros_queue_length", ros_queue_length, 100); + + static int node_index = 0; + std::stringstream node_name; + node_name << "rslidar_packets_destination_" << node_index++; + + node_ptr_.reset(new rclcpp::Node(node_name.str())); + pkt_pub_ = node_ptr_->create_publisher(ros_send_topic, ros_queue_length); +} + +inline void DestinationPacketRos::sendPacket(const Packet& msg) +{ + pkt_pub_->publish(toRosMsg(msg, frame_id_)); +} + +} // namespace lidar +} // namespace robosense + +#endif // ROS2_FOUND + + diff --git a/src/airy/rslidar_sdk-main/src/source/source_pointcloud_ros.hpp b/src/airy/rslidar_sdk-main/src/source/source_pointcloud_ros.hpp new file mode 100644 index 0000000..06ccf3a --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/source/source_pointcloud_ros.hpp @@ -0,0 +1,501 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once + +#include "source/source.hpp" + +#ifdef ROS_FOUND +#include +#include +#ifdef ENABLE_IMU_DATA_PARSE + #include "sensor_msgs/Imu.h" +#endif +namespace robosense +{ +namespace lidar +{ + +inline sensor_msgs::PointCloud2 toRosMsg(const LidarPointCloudMsg& rs_msg, const std::string& frame_id, bool send_by_rows) +{ + sensor_msgs::PointCloud2 ros_msg; + + int fields = 4; +#ifdef POINT_TYPE_XYZIF + fields = 5; +#elif defined(POINT_TYPE_XYZIRT) + fields = 6; +#elif defined(POINT_TYPE_XYZIRTF) + fields = 7; +#endif + ros_msg.fields.clear(); + ros_msg.fields.reserve(fields); + + if (send_by_rows) + { + ros_msg.width = rs_msg.width; + ros_msg.height = rs_msg.height; + } + else + { + ros_msg.width = rs_msg.height; // exchange width and height to be compatible with pcl::PointCloud<> + ros_msg.height = rs_msg.width; + } + + int offset = 0; + offset = addPointField(ros_msg, "x", 1, sensor_msgs::PointField::FLOAT32, offset); + offset = addPointField(ros_msg, "y", 1, sensor_msgs::PointField::FLOAT32, offset); + offset = addPointField(ros_msg, "z", 1, sensor_msgs::PointField::FLOAT32, offset); + offset = addPointField(ros_msg, "intensity", 1, sensor_msgs::PointField::FLOAT32, offset); +#if defined(POINT_TYPE_XYZIRT) || defined(POINT_TYPE_XYZIRTF) + offset = addPointField(ros_msg, "ring", 1, sensor_msgs::PointField::UINT16, offset); + offset = addPointField(ros_msg, "timestamp", 1, sensor_msgs::PointField::FLOAT64, offset); +#endif + +#if defined(POINT_TYPE_XYZIF) || defined(POINT_TYPE_XYZIRTF) + offset = addPointField(ros_msg, "feature", 1, sensor_msgs::PointField::UINT8, offset); +#endif + +#if 0 + std::cout << "off:" << offset << std::endl; +#endif + + ros_msg.point_step = offset; + ros_msg.row_step = ros_msg.width * ros_msg.point_step; + ros_msg.is_dense = rs_msg.is_dense; + ros_msg.data.resize(ros_msg.point_step * ros_msg.width * ros_msg.height); + + sensor_msgs::PointCloud2Iterator iter_x_(ros_msg, "x"); + sensor_msgs::PointCloud2Iterator iter_y_(ros_msg, "y"); + sensor_msgs::PointCloud2Iterator iter_z_(ros_msg, "z"); + sensor_msgs::PointCloud2Iterator iter_intensity_(ros_msg, "intensity"); + +#if defined(POINT_TYPE_XYZIRT) || defined(POINT_TYPE_XYZIRTF) + sensor_msgs::PointCloud2Iterator iter_ring_(ros_msg, "ring"); + sensor_msgs::PointCloud2Iterator iter_timestamp_(ros_msg, "timestamp"); +#endif + +#if defined(POINT_TYPE_XYZIF) || defined(POINT_TYPE_XYZIRTF) + sensor_msgs::PointCloud2Iterator iter_feature_(ros_msg, "feature"); +#endif + + if (send_by_rows) + { + for (size_t i = 0; i < rs_msg.height; i++) + { + for (size_t j = 0; j < rs_msg.width; j++) + { + const LidarPointCloudMsg::PointT& point = rs_msg.points[i + j * rs_msg.height]; + + *iter_x_ = point.x; + *iter_y_ = point.y; + *iter_z_ = point.z; + *iter_intensity_ = point.intensity; + + ++iter_x_; + ++iter_y_; + ++iter_z_; + ++iter_intensity_; + +#if defined(POINT_TYPE_XYZIRT) || defined(POINT_TYPE_XYZIRTF) + *iter_ring_ = point.ring; + *iter_timestamp_ = point.timestamp; + + ++iter_ring_; + ++iter_timestamp_; +#endif + +#if defined(POINT_TYPE_XYZIF) || defined(POINT_TYPE_XYZIRTF) + *iter_feature_ = point.feature; + ++iter_feature_; +#endif + + } + } + } + else + { + for (size_t i = 0; i < rs_msg.points.size(); i++) + { + const LidarPointCloudMsg::PointT& point = rs_msg.points[i]; + + *iter_x_ = point.x; + *iter_y_ = point.y; + *iter_z_ = point.z; + *iter_intensity_ = point.intensity; + + ++iter_x_; + ++iter_y_; + ++iter_z_; + ++iter_intensity_; + +#if defined(POINT_TYPE_XYZIRT) || defined(POINT_TYPE_XYZIRTF) + *iter_ring_ = point.ring; + *iter_timestamp_ = point.timestamp; + + ++iter_ring_; + ++iter_timestamp_; +#endif + +#if defined(POINT_TYPE_XYZIF) || defined(POINT_TYPE_XYZIRTF) + *iter_feature_ = point.feature; + ++iter_feature_; +#endif + } + } + + ros_msg.header.seq = rs_msg.seq; + ros_msg.header.stamp = ros_msg.header.stamp.fromSec(rs_msg.timestamp); + ros_msg.header.frame_id = frame_id; + + return ros_msg; +} +#ifdef ENABLE_IMU_DATA_PARSE +sensor_msgs::Imu toRosMsg(const std::shared_ptr& data, const std::string& frame_id) +{ + sensor_msgs::Imu imu_msg; + + imu_msg.header.stamp = imu_msg.header.stamp.fromSec(data->timestamp); + imu_msg.header.frame_id = frame_id; + // Set IMU data + imu_msg.angular_velocity.x = data->angular_velocity_x; + imu_msg.angular_velocity.y = data->angular_velocity_y; + imu_msg.angular_velocity.z = data->angular_velocity_z; + + imu_msg.linear_acceleration.x = data->linear_acceleration_x; + imu_msg.linear_acceleration.y = data->linear_acceleration_y; + imu_msg.linear_acceleration.z = data->linear_acceleration_z; + return imu_msg; +} +#endif +class DestinationPointCloudRos : public DestinationPointCloud +{ +public: + + virtual void init(const YAML::Node& config); + virtual void sendPointCloud(const LidarPointCloudMsg& msg); + virtual ~DestinationPointCloudRos() = default; +#ifdef ENABLE_IMU_DATA_PARSE + virtual void sendImuData(const std::shared_ptr & data); +#endif +private: + std::shared_ptr nh_; + ros::Publisher pub_; +#ifdef ENABLE_IMU_DATA_PARSE + ros::Publisher imu_pub_; +#endif + std::string frame_id_; + bool send_by_rows_; +}; + +inline void DestinationPointCloudRos::init(const YAML::Node& config) +{ + yamlRead(config["ros"], + "ros_send_by_rows", send_by_rows_, false); + + bool dense_points; + yamlRead(config["driver"], "dense_points", dense_points, false); + if (dense_points) + send_by_rows_ = false; + + yamlRead(config["ros"], + "ros_frame_id", frame_id_, "rslidar"); + + std::string ros_send_topic; + yamlRead(config["ros"], + "ros_send_point_cloud_topic", ros_send_topic, "rslidar_points"); + + + + nh_ = std::unique_ptr(new ros::NodeHandle()); + pub_ = nh_->advertise(ros_send_topic, 10); +#ifdef ENABLE_IMU_DATA_PARSE + std::string ros_send_imu_data_topic; + yamlRead(config["ros"], + "ros_send_imu_data_topic", ros_send_imu_data_topic, "rslidar_imu_data"); + imu_pub_ = nh_->advertise(ros_send_imu_data_topic, 1000); +#endif +} + +inline void DestinationPointCloudRos::sendPointCloud(const LidarPointCloudMsg& msg) +{ + pub_.publish(toRosMsg(msg, frame_id_, send_by_rows_)); +} +#ifdef ENABLE_IMU_DATA_PARSE +inline void DestinationPointCloudRos::sendImuData(const std::shared_ptr & data) +{ + imu_pub_.publish(toRosMsg(data, frame_id_)); +} +#endif +} // namespace lidar +} // namespace robosense + +#endif // ROS_FOUND + +#ifdef ROS2_FOUND +#include +#include +#ifdef ENABLE_IMU_DATA_PARSE + #include +#endif +#include + +namespace robosense +{ +namespace lidar +{ + +inline sensor_msgs::msg::PointCloud2 toRosMsg(const LidarPointCloudMsg& rs_msg, const std::string& frame_id, bool send_by_rows) +{ + sensor_msgs::msg::PointCloud2 ros_msg; + + int fields = 4; +#ifdef POINT_TYPE_XYZIF + fields = 5; +#elif defined(POINT_TYPE_XYZIRT) + fields = 6; +#elif defined(POINT_TYPE_XYZIRTF) + fields = 7; +#endif + ros_msg.fields.clear(); + ros_msg.fields.reserve(fields); + + if (send_by_rows) + { + ros_msg.width = rs_msg.width; + ros_msg.height = rs_msg.height; + } + else + { + ros_msg.width = rs_msg.height; // exchange width and height to be compatible with pcl::PointCloud<> + ros_msg.height = rs_msg.width; + } + + int offset = 0; + offset = addPointField(ros_msg, "x", 1, sensor_msgs::msg::PointField::FLOAT32, offset); + offset = addPointField(ros_msg, "y", 1, sensor_msgs::msg::PointField::FLOAT32, offset); + offset = addPointField(ros_msg, "z", 1, sensor_msgs::msg::PointField::FLOAT32, offset); + offset = addPointField(ros_msg, "intensity", 1, sensor_msgs::msg::PointField::FLOAT32, offset); + +#if defined(POINT_TYPE_XYZIRT) || defined(POINT_TYPE_XYZIRTF) + offset = addPointField(ros_msg, "ring", 1, sensor_msgs::msg::PointField::UINT16, offset); + offset = addPointField(ros_msg, "timestamp", 1, sensor_msgs::msg::PointField::FLOAT64, offset); +#endif + +#if defined(POINT_TYPE_XYZIF) || defined(POINT_TYPE_XYZIRTF) + offset = addPointField(ros_msg, "feature", 1, sensor_msgs::msg::PointField::UINT8, offset); +#endif + +#if 0 + std::cout << "off:" << offset << std::endl; +#endif + + ros_msg.point_step = offset; + ros_msg.row_step = ros_msg.width * ros_msg.point_step; + ros_msg.is_dense = rs_msg.is_dense; + ros_msg.data.resize(ros_msg.point_step * ros_msg.width * ros_msg.height); + + sensor_msgs::PointCloud2Iterator iter_x_(ros_msg, "x"); + sensor_msgs::PointCloud2Iterator iter_y_(ros_msg, "y"); + sensor_msgs::PointCloud2Iterator iter_z_(ros_msg, "z"); + sensor_msgs::PointCloud2Iterator iter_intensity_(ros_msg, "intensity"); +#if defined(POINT_TYPE_XYZIRT) || defined(POINT_TYPE_XYZIRTF) + sensor_msgs::PointCloud2Iterator iter_ring_(ros_msg, "ring"); + sensor_msgs::PointCloud2Iterator iter_timestamp_(ros_msg, "timestamp"); +#endif + +#if defined(POINT_TYPE_XYZIF) || defined(POINT_TYPE_XYZIRTF) + sensor_msgs::PointCloud2Iterator iter_feature_(ros_msg, "feature"); +#endif + + if (send_by_rows) + { + for (size_t i = 0; i < rs_msg.height; i++) + { + for (size_t j = 0; j < rs_msg.width; j++) + { + const LidarPointCloudMsg::PointT& point = rs_msg.points[i + j * rs_msg.height]; + + *iter_x_ = point.x; + *iter_y_ = point.y; + *iter_z_ = point.z; + *iter_intensity_ = point.intensity; + + ++iter_x_; + ++iter_y_; + ++iter_z_; + ++iter_intensity_; + +#if defined(POINT_TYPE_XYZIRT) || defined(POINT_TYPE_XYZIRTF) + *iter_ring_ = point.ring; + *iter_timestamp_ = point.timestamp; + + ++iter_ring_; + ++iter_timestamp_; +#endif + +#if defined(POINT_TYPE_XYZIF) || defined(POINT_TYPE_XYZIRTF) + *iter_feature_ = point.feature; + ++iter_feature_; +#endif + + } + } + } + else + { + for (size_t i = 0; i < rs_msg.points.size(); i++) + { + const LidarPointCloudMsg::PointT& point = rs_msg.points[i]; + + *iter_x_ = point.x; + *iter_y_ = point.y; + *iter_z_ = point.z; + *iter_intensity_ = point.intensity; + + ++iter_x_; + ++iter_y_; + ++iter_z_; + ++iter_intensity_; + +#if defined(POINT_TYPE_XYZIRT) || defined(POINT_TYPE_XYZIRTF) + *iter_ring_ = point.ring; + *iter_timestamp_ = point.timestamp; + + ++iter_ring_; + ++iter_timestamp_; +#endif + +#if defined(POINT_TYPE_XYZIF) || defined(POINT_TYPE_XYZIRTF) + *iter_feature_ = point.feature; + ++iter_feature_; +#endif + } + } + + ros_msg.header.stamp.sec = (uint32_t)floor(rs_msg.timestamp); + ros_msg.header.stamp.nanosec = (uint32_t)round((rs_msg.timestamp - ros_msg.header.stamp.sec) * 1e9); + ros_msg.header.frame_id = frame_id; + + return ros_msg; +} +#ifdef ENABLE_IMU_DATA_PARSE +sensor_msgs::msg::Imu toRosMsg(const std::shared_ptr& data, const std::string& frame_id) +{ + sensor_msgs::msg::Imu imu_msg; + + imu_msg.header.stamp = rclcpp::Time(static_cast(data->timestamp * 1e9)); + imu_msg.header.frame_id = frame_id; + // Set IMU data + imu_msg.angular_velocity.x = data->angular_velocity_x; + imu_msg.angular_velocity.y = data->angular_velocity_y; + imu_msg.angular_velocity.z = data->angular_velocity_z; + + imu_msg.linear_acceleration.x = data->linear_acceleration_x; + imu_msg.linear_acceleration.y = data->linear_acceleration_y; + imu_msg.linear_acceleration.z = data->linear_acceleration_z; + return imu_msg; +} +#endif +class DestinationPointCloudRos : virtual public DestinationPointCloud +{ +public: + + virtual void init(const YAML::Node& config); + virtual void sendPointCloud(const LidarPointCloudMsg& msg); +#ifdef ENABLE_IMU_DATA_PARSE + virtual void sendImuData(const std::shared_ptr & data); +#endif + virtual ~DestinationPointCloudRos() = default; + +private: + std::shared_ptr node_ptr_; + rclcpp::Publisher::SharedPtr pub_; +#ifdef ENABLE_IMU_DATA_PARSE + rclcpp::Publisher::SharedPtr imu_pub_; +#endif + std::string frame_id_; + bool send_by_rows_; +}; + +inline void DestinationPointCloudRos::init(const YAML::Node& config) +{ + yamlRead(config["ros"], + "ros_send_by_rows", send_by_rows_, false); + + bool dense_points; + yamlRead(config["driver"], "dense_points", dense_points, false); + if (dense_points) + send_by_rows_ = false; + + yamlRead(config["ros"], + "ros_frame_id", frame_id_, "rslidar"); + + std::string ros_send_topic; + yamlRead(config["ros"], + "ros_send_point_cloud_topic", ros_send_topic, "rslidar_points"); + + size_t ros_queue_length; + yamlRead(config["ros"], "ros_queue_length", ros_queue_length, 100); + + static int node_index = 0; + std::stringstream node_name; + node_name << "rslidar_points_destination_" << node_index++; + + node_ptr_.reset(new rclcpp::Node(node_name.str())); + + pub_ = node_ptr_->create_publisher(ros_send_topic, ros_queue_length); + +#ifdef ENABLE_IMU_DATA_PARSE + std::string ros_send_imu_data_topic; + yamlRead(config["ros"], + "ros_send_imu_data_topic", ros_send_imu_data_topic, "rslidar_imu_data"); + imu_pub_ = node_ptr_->create_publisher(ros_send_imu_data_topic, 1000); +#endif + +} + +inline void DestinationPointCloudRos::sendPointCloud(const LidarPointCloudMsg& msg) +{ + pub_->publish(toRosMsg(msg, frame_id_, send_by_rows_)); +} +#ifdef ENABLE_IMU_DATA_PARSE +inline void DestinationPointCloudRos::sendImuData(const std::shared_ptr & data) +{ + imu_pub_->publish(toRosMsg(data, frame_id_)); +} +#endif +} // namespace lidar +} // namespace robosense + +#endif + diff --git a/src/airy/rslidar_sdk-main/src/utility/common.hpp b/src/airy/rslidar_sdk-main/src/utility/common.hpp new file mode 100644 index 0000000..fa96c83 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/utility/common.hpp @@ -0,0 +1,35 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once + +#include "rs_driver/common/rs_log.hpp" diff --git a/src/airy/rslidar_sdk-main/src/utility/yaml_reader.hpp b/src/airy/rslidar_sdk-main/src/utility/yaml_reader.hpp new file mode 100644 index 0000000..4c34609 --- /dev/null +++ b/src/airy/rslidar_sdk-main/src/utility/yaml_reader.hpp @@ -0,0 +1,86 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +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 names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT OWNER OR CONTRIBUTORS 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. +*********************************************************************************************************************/ + +#pragma once + +#include + +#include "utility/common.hpp" + +namespace robosense +{ +namespace lidar +{ + +template +inline void yamlReadAbort(const YAML::Node& yaml, const std::string& key, T& out_val) +{ + if (!yaml[key] || yaml[key].Type() == YAML::NodeType::Null) + { + RS_ERROR << " : Not set " << key; + RS_ERROR << " value, Aborting!!!" << RS_REND; + exit(-1); + } + else + { + out_val = yaml[key].as(); + } +} + +template +inline bool yamlRead(const YAML::Node& yaml, const std::string& key, T& out_val, const T& default_val) +{ + if (!yaml[key] || yaml[key].Type() == YAML::NodeType::Null) + { + out_val = default_val; + return false; + } + else + { + out_val = yaml[key].as(); + return true; + } +} + +inline YAML::Node yamlSubNodeAbort(const YAML::Node& yaml, const std::string& node) +{ + YAML::Node ret = yaml[node.c_str()]; + if (!ret) + { + RS_ERROR << " : Cannot find subnode " << node << ". Aborting!!!" << RS_REND; + exit(-1); + } + return ret; +} + +} // namespace lidar +} // namespace robosense diff --git a/src/fu/CMakeLists.txt b/src/fu/CMakeLists.txt new file mode 100644 index 0000000..8edeb60 --- /dev/null +++ b/src/fu/CMakeLists.txt @@ -0,0 +1,54 @@ +cmake_minimum_required(VERSION 3.5) +project(fu) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +include_directories( + include/fu + ${catkin_INCLUDE_DIRS} +) +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sweeper_interfaces REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(mc REQUIRED) + +include_directories(include) + +add_executable(fu_node src/fu_node.cpp src/jsoncpp.cpp) +ament_target_dependencies(fu_node rclcpp std_msgs sweeper_interfaces sensor_msgs mc) + +install(TARGETS + fu_node + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY launch config + DESTINATION share/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/fu/config/fu_params.yaml b/src/fu/config/fu_params.yaml new file mode 100644 index 0000000..758cb04 --- /dev/null +++ b/src/fu/config/fu_params.yaml @@ -0,0 +1,32 @@ +FU: + ros__parameters: + # 遇障停车功能 False or True + enable_obstacle_stop: True + + # 绕障功能 False or True + enable_obstacle_avoid: True + + # 可视化栅格 False or True + enable_visualize_grid: False + + # 车前必停区行数 int + FRONT_STOP_ZONE_ROWS: 3 + + # 车后必停区行数 int + REAR_STOP_ZONE_ROWS: 3 + + # 车左必停区列数 int + LEFT_STOP_ZONE_COLS: 2 + + # 车右必停区列数 int + RIGHT_STOP_ZONE_COLS: 2 + + # 前方扩展检测区域 int + front_area_extend: 12 + + # 左侧扩展检测区域 int + left_area_extend: 2 + + # 右侧扩展检测区域 int + right_area_extend: 2 + \ No newline at end of file diff --git a/src/fu/include/fu/json.h b/src/fu/include/fu/json.h new file mode 100644 index 0000000..d95fe6e --- /dev/null +++ b/src/fu/include/fu/json.h @@ -0,0 +1,2351 @@ +/// Json-cpp amalgamated header (http://jsoncpp.sourceforge.net/). +/// It is intended to be used with #include "json/json.h" + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: LICENSE +// ////////////////////////////////////////////////////////////////////// + +/* +The JsonCpp library's source code, including accompanying documentation, +tests and demonstration applications, are licensed under the following +conditions... + +Baptiste Lepilleur and The JsonCpp Authors explicitly disclaim copyright in all +jurisdictions which recognize such a disclaimer. In such jurisdictions, +this software is released into the Public Domain. + +In jurisdictions which do not recognize Public Domain property (e.g. Germany as of +2010), this software is Copyright (c) 2007-2010 by Baptiste Lepilleur and +The JsonCpp Authors, and is released under the terms of the MIT License (see below). + +In jurisdictions which recognize Public Domain property, the user of this +software may choose to accept it either as 1) Public Domain, 2) under the +conditions of the MIT License (see below), or 3) under the terms of dual +Public Domain/MIT License conditions described here, as they choose. + +The MIT License is about as close to Public Domain as a license can get, and is +described in clear, concise terms at: + + http://en.wikipedia.org/wiki/MIT_License + +The full text of the MIT License follows: + +======================================================================== +Copyright (c) 2007-2010 Baptiste Lepilleur and The JsonCpp Authors + +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. +======================================================================== +(END LICENSE TEXT) + +The MIT license is compatible with both the GPL and commercial +software, affording one all of the rights of Public Domain with the +minor nuisance of being required to keep the above copyright notice +and license text in the source code. Note also that by accepting the +Public Domain "license" you can re-license your copy using whatever +license you like. + +*/ + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: LICENSE +// ////////////////////////////////////////////////////////////////////// + + + + + +#ifndef JSON_AMALGAMATED_H_INCLUDED +# define JSON_AMALGAMATED_H_INCLUDED +/// If defined, indicates that the source file is amalgamated +/// to prevent private header inclusion. +#define JSON_IS_AMALGAMATION + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: include/json/version.h +// ////////////////////////////////////////////////////////////////////// + +#ifndef JSON_VERSION_H_INCLUDED +#define JSON_VERSION_H_INCLUDED + +// Note: version must be updated in three places when doing a release. This +// annoying process ensures that amalgamate, CMake, and meson all report the +// correct version. +// 1. /meson.build +// 2. /include/json/version.h +// 3. /CMakeLists.txt +// IMPORTANT: also update the SOVERSION!! + +#define JSONCPP_VERSION_STRING "1.9.5" +#define JSONCPP_VERSION_MAJOR 1 +#define JSONCPP_VERSION_MINOR 9 +#define JSONCPP_VERSION_PATCH 5 +#define JSONCPP_VERSION_QUALIFIER +#define JSONCPP_VERSION_HEXA \ + ((JSONCPP_VERSION_MAJOR << 24) | (JSONCPP_VERSION_MINOR << 16) | \ + (JSONCPP_VERSION_PATCH << 8)) + +#ifdef JSONCPP_USING_SECURE_MEMORY +#undef JSONCPP_USING_SECURE_MEMORY +#endif +#define JSONCPP_USING_SECURE_MEMORY 0 +// If non-zero, the library zeroes any memory that it has allocated before +// it frees its memory. + +#endif // JSON_VERSION_H_INCLUDED + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: include/json/version.h +// ////////////////////////////////////////////////////////////////////// + + + + + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: include/json/allocator.h +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2007-2010 Baptiste Lepilleur and The JsonCpp Authors +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +#ifndef JSON_ALLOCATOR_H_INCLUDED +#define JSON_ALLOCATOR_H_INCLUDED + +#include +#include + +#pragma pack(push) +#pragma pack() + +namespace Json { +template class SecureAllocator { +public: + // Type definitions + using value_type = T; + using pointer = T*; + using const_pointer = const T*; + using reference = T&; + using const_reference = const T&; + using size_type = std::size_t; + using difference_type = std::ptrdiff_t; + + /** + * Allocate memory for N items using the standard allocator. + */ + pointer allocate(size_type n) { + // allocate using "global operator new" + return static_cast(::operator new(n * sizeof(T))); + } + + /** + * Release memory which was allocated for N items at pointer P. + * + * The memory block is filled with zeroes before being released. + */ + void deallocate(pointer p, size_type n) { + // memset_s is used because memset may be optimized away by the compiler + memset_s(p, n * sizeof(T), 0, n * sizeof(T)); + // free using "global operator delete" + ::operator delete(p); + } + + /** + * Construct an item in-place at pointer P. + */ + template void construct(pointer p, Args&&... args) { + // construct using "placement new" and "perfect forwarding" + ::new (static_cast(p)) T(std::forward(args)...); + } + + size_type max_size() const { return size_t(-1) / sizeof(T); } + + pointer address(reference x) const { return std::addressof(x); } + + const_pointer address(const_reference x) const { return std::addressof(x); } + + /** + * Destroy an item in-place at pointer P. + */ + void destroy(pointer p) { + // destroy using "explicit destructor" + p->~T(); + } + + // Boilerplate + SecureAllocator() {} + template SecureAllocator(const SecureAllocator&) {} + template struct rebind { using other = SecureAllocator; }; +}; + +template +bool operator==(const SecureAllocator&, const SecureAllocator&) { + return true; +} + +template +bool operator!=(const SecureAllocator&, const SecureAllocator&) { + return false; +} + +} // namespace Json + +#pragma pack(pop) + +#endif // JSON_ALLOCATOR_H_INCLUDED + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: include/json/allocator.h +// ////////////////////////////////////////////////////////////////////// + + + + + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: include/json/config.h +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2007-2010 Baptiste Lepilleur and The JsonCpp Authors +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +#ifndef JSON_CONFIG_H_INCLUDED +#define JSON_CONFIG_H_INCLUDED +#include +#include +#include +#include +#include +#include +#include +#include + +// If non-zero, the library uses exceptions to report bad input instead of C +// assertion macros. The default is to use exceptions. +#ifndef JSON_USE_EXCEPTION +#define JSON_USE_EXCEPTION 1 +#endif + +// Temporary, tracked for removal with issue #982. +#ifndef JSON_USE_NULLREF +#define JSON_USE_NULLREF 1 +#endif + +/// If defined, indicates that the source file is amalgamated +/// to prevent private header inclusion. +/// Remarks: it is automatically defined in the generated amalgamated header. +// #define JSON_IS_AMALGAMATION + +// Export macros for DLL visibility +#if defined(JSON_DLL_BUILD) +#if defined(_MSC_VER) || defined(__MINGW32__) +#define JSON_API __declspec(dllexport) +#define JSONCPP_DISABLE_DLL_INTERFACE_WARNING +#elif defined(__GNUC__) || defined(__clang__) +#define JSON_API __attribute__((visibility("default"))) +#endif // if defined(_MSC_VER) + +#elif defined(JSON_DLL) +#if defined(_MSC_VER) || defined(__MINGW32__) +#define JSON_API __declspec(dllimport) +#define JSONCPP_DISABLE_DLL_INTERFACE_WARNING +#endif // if defined(_MSC_VER) +#endif // ifdef JSON_DLL_BUILD + +#if !defined(JSON_API) +#define JSON_API +#endif + +#if defined(_MSC_VER) && _MSC_VER < 1800 +#error \ + "ERROR: Visual Studio 12 (2013) with _MSC_VER=1800 is the oldest supported compiler with sufficient C++11 capabilities" +#endif + +#if defined(_MSC_VER) && _MSC_VER < 1900 +// As recommended at +// https://stackoverflow.com/questions/2915672/snprintf-and-visual-studio-2010 +extern JSON_API int msvc_pre1900_c99_snprintf(char* outBuf, size_t size, + const char* format, ...); +#define jsoncpp_snprintf msvc_pre1900_c99_snprintf +#else +#define jsoncpp_snprintf std::snprintf +#endif + +// If JSON_NO_INT64 is defined, then Json only support C++ "int" type for +// integer +// Storages, and 64 bits integer support is disabled. +// #define JSON_NO_INT64 1 + +// JSONCPP_OVERRIDE is maintained for backwards compatibility of external tools. +// C++11 should be used directly in JSONCPP. +#define JSONCPP_OVERRIDE override + +#ifdef __clang__ +#if __has_extension(attribute_deprecated_with_message) +#define JSONCPP_DEPRECATED(message) __attribute__((deprecated(message))) +#endif +#elif defined(__GNUC__) // not clang (gcc comes later since clang emulates gcc) +#if (__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 5)) +#define JSONCPP_DEPRECATED(message) __attribute__((deprecated(message))) +#elif (__GNUC__ > 3 || (__GNUC__ == 3 && __GNUC_MINOR__ >= 1)) +#define JSONCPP_DEPRECATED(message) __attribute__((__deprecated__)) +#endif // GNUC version +#elif defined(_MSC_VER) // MSVC (after clang because clang on Windows emulates + // MSVC) +#define JSONCPP_DEPRECATED(message) __declspec(deprecated(message)) +#endif // __clang__ || __GNUC__ || _MSC_VER + +#if !defined(JSONCPP_DEPRECATED) +#define JSONCPP_DEPRECATED(message) +#endif // if !defined(JSONCPP_DEPRECATED) + +#if defined(__clang__) || (defined(__GNUC__) && (__GNUC__ >= 6)) +#define JSON_USE_INT64_DOUBLE_CONVERSION 1 +#endif + +#if !defined(JSON_IS_AMALGAMATION) + +#include "allocator.h" +#include "version.h" + +#endif // if !defined(JSON_IS_AMALGAMATION) + +namespace Json { +using Int = int; +using UInt = unsigned int; +#if defined(JSON_NO_INT64) +using LargestInt = int; +using LargestUInt = unsigned int; +#undef JSON_HAS_INT64 +#else // if defined(JSON_NO_INT64) +// For Microsoft Visual use specific types as long long is not supported +#if defined(_MSC_VER) // Microsoft Visual Studio +using Int64 = __int64; +using UInt64 = unsigned __int64; +#else // if defined(_MSC_VER) // Other platforms, use long long +using Int64 = int64_t; +using UInt64 = uint64_t; +#endif // if defined(_MSC_VER) +using LargestInt = Int64; +using LargestUInt = UInt64; +#define JSON_HAS_INT64 +#endif // if defined(JSON_NO_INT64) + +template +using Allocator = + typename std::conditional, + std::allocator>::type; +using String = std::basic_string, Allocator>; +using IStringStream = + std::basic_istringstream; +using OStringStream = + std::basic_ostringstream; +using IStream = std::istream; +using OStream = std::ostream; +} // namespace Json + +// Legacy names (formerly macros). +using JSONCPP_STRING = Json::String; +using JSONCPP_ISTRINGSTREAM = Json::IStringStream; +using JSONCPP_OSTRINGSTREAM = Json::OStringStream; +using JSONCPP_ISTREAM = Json::IStream; +using JSONCPP_OSTREAM = Json::OStream; + +#endif // JSON_CONFIG_H_INCLUDED + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: include/json/config.h +// ////////////////////////////////////////////////////////////////////// + + + + + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: include/json/forwards.h +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2007-2010 Baptiste Lepilleur and The JsonCpp Authors +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +#ifndef JSON_FORWARDS_H_INCLUDED +#define JSON_FORWARDS_H_INCLUDED + +#if !defined(JSON_IS_AMALGAMATION) +#include "config.h" +#endif // if !defined(JSON_IS_AMALGAMATION) + +namespace Json { + +// writer.h +class StreamWriter; +class StreamWriterBuilder; +class Writer; +class FastWriter; +class StyledWriter; +class StyledStreamWriter; + +// reader.h +class Reader; +class CharReader; +class CharReaderBuilder; + +// json_features.h +class Features; + +// value.h +using ArrayIndex = unsigned int; +class StaticString; +class Path; +class PathArgument; +class Value; +class ValueIteratorBase; +class ValueIterator; +class ValueConstIterator; + +} // namespace Json + +#endif // JSON_FORWARDS_H_INCLUDED + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: include/json/forwards.h +// ////////////////////////////////////////////////////////////////////// + + + + + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: include/json/json_features.h +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2007-2010 Baptiste Lepilleur and The JsonCpp Authors +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +#ifndef JSON_FEATURES_H_INCLUDED +#define JSON_FEATURES_H_INCLUDED + +#if !defined(JSON_IS_AMALGAMATION) +#include "forwards.h" +#endif // if !defined(JSON_IS_AMALGAMATION) + +#pragma pack(push) +#pragma pack() + +namespace Json { + +/** \brief Configuration passed to reader and writer. + * This configuration object can be used to force the Reader or Writer + * to behave in a standard conforming way. + */ +class JSON_API Features { +public: + /** \brief A configuration that allows all features and assumes all strings + * are UTF-8. + * - C & C++ comments are allowed + * - Root object can be any JSON value + * - Assumes Value strings are encoded in UTF-8 + */ + static Features all(); + + /** \brief A configuration that is strictly compatible with the JSON + * specification. + * - Comments are forbidden. + * - Root object must be either an array or an object value. + * - Assumes Value strings are encoded in UTF-8 + */ + static Features strictMode(); + + /** \brief Initialize the configuration like JsonConfig::allFeatures; + */ + Features(); + + /// \c true if comments are allowed. Default: \c true. + bool allowComments_{true}; + + /// \c true if root must be either an array or an object value. Default: \c + /// false. + bool strictRoot_{false}; + + /// \c true if dropped null placeholders are allowed. Default: \c false. + bool allowDroppedNullPlaceholders_{false}; + + /// \c true if numeric object key are allowed. Default: \c false. + bool allowNumericKeys_{false}; +}; + +} // namespace Json + +#pragma pack(pop) + +#endif // JSON_FEATURES_H_INCLUDED + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: include/json/json_features.h +// ////////////////////////////////////////////////////////////////////// + + + + + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: include/json/value.h +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2007-2010 Baptiste Lepilleur and The JsonCpp Authors +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +#ifndef JSON_H_INCLUDED +#define JSON_H_INCLUDED + +#if !defined(JSON_IS_AMALGAMATION) +#include "forwards.h" +#endif // if !defined(JSON_IS_AMALGAMATION) + +// Conditional NORETURN attribute on the throw functions would: +// a) suppress false positives from static code analysis +// b) possibly improve optimization opportunities. +#if !defined(JSONCPP_NORETURN) +#if defined(_MSC_VER) && _MSC_VER == 1800 +#define JSONCPP_NORETURN __declspec(noreturn) +#else +#define JSONCPP_NORETURN [[noreturn]] +#endif +#endif + +// Support for '= delete' with template declarations was a late addition +// to the c++11 standard and is rejected by clang 3.8 and Apple clang 8.2 +// even though these declare themselves to be c++11 compilers. +#if !defined(JSONCPP_TEMPLATE_DELETE) +#if defined(__clang__) && defined(__apple_build_version__) +#if __apple_build_version__ <= 8000042 +#define JSONCPP_TEMPLATE_DELETE +#endif +#elif defined(__clang__) +#if __clang_major__ == 3 && __clang_minor__ <= 8 +#define JSONCPP_TEMPLATE_DELETE +#endif +#endif +#if !defined(JSONCPP_TEMPLATE_DELETE) +#define JSONCPP_TEMPLATE_DELETE = delete +#endif +#endif + +#include +#include +#include +#include +#include +#include + +// Disable warning C4251: : needs to have dll-interface to +// be used by... +#if defined(JSONCPP_DISABLE_DLL_INTERFACE_WARNING) +#pragma warning(push) +#pragma warning(disable : 4251 4275) +#endif // if defined(JSONCPP_DISABLE_DLL_INTERFACE_WARNING) + +#pragma pack(push) +#pragma pack() + +/** \brief JSON (JavaScript Object Notation). + */ +namespace Json { + +#if JSON_USE_EXCEPTION +/** Base class for all exceptions we throw. + * + * We use nothing but these internally. Of course, STL can throw others. + */ +class JSON_API Exception : public std::exception { +public: + Exception(String msg); + ~Exception() noexcept override; + char const* what() const noexcept override; + +protected: + String msg_; +}; + +/** Exceptions which the user cannot easily avoid. + * + * E.g. out-of-memory (when we use malloc), stack-overflow, malicious input + * + * \remark derived from Json::Exception + */ +class JSON_API RuntimeError : public Exception { +public: + RuntimeError(String const& msg); +}; + +/** Exceptions thrown by JSON_ASSERT/JSON_FAIL macros. + * + * These are precondition-violations (user bugs) and internal errors (our bugs). + * + * \remark derived from Json::Exception + */ +class JSON_API LogicError : public Exception { +public: + LogicError(String const& msg); +}; +#endif + +/// used internally +JSONCPP_NORETURN void throwRuntimeError(String const& msg); +/// used internally +JSONCPP_NORETURN void throwLogicError(String const& msg); + +/** \brief Type of the value held by a Value object. + */ +enum ValueType { + nullValue = 0, ///< 'null' value + intValue, ///< signed integer value + uintValue, ///< unsigned integer value + realValue, ///< double value + stringValue, ///< UTF-8 string value + booleanValue, ///< bool value + arrayValue, ///< array value (ordered list) + objectValue ///< object value (collection of name/value pairs). +}; + +enum CommentPlacement { + commentBefore = 0, ///< a comment placed on the line before a value + commentAfterOnSameLine, ///< a comment just after a value on the same line + commentAfter, ///< a comment on the line after a value (only make sense for + /// root value) + numberOfCommentPlacement +}; + +/** \brief Type of precision for formatting of real values. + */ +enum PrecisionType { + significantDigits = 0, ///< we set max number of significant digits in string + decimalPlaces ///< we set max number of digits after "." in string +}; + +/** \brief Lightweight wrapper to tag static string. + * + * Value constructor and objectValue member assignment takes advantage of the + * StaticString and avoid the cost of string duplication when storing the + * string or the member name. + * + * Example of usage: + * \code + * Json::Value aValue( StaticString("some text") ); + * Json::Value object; + * static const StaticString code("code"); + * object[code] = 1234; + * \endcode + */ +class JSON_API StaticString { +public: + explicit StaticString(const char* czstring) : c_str_(czstring) {} + + operator const char*() const { return c_str_; } + + const char* c_str() const { return c_str_; } + +private: + const char* c_str_; +}; + +/** \brief Represents a JSON value. + * + * This class is a discriminated union wrapper that can represents a: + * - signed integer [range: Value::minInt - Value::maxInt] + * - unsigned integer (range: 0 - Value::maxUInt) + * - double + * - UTF-8 string + * - boolean + * - 'null' + * - an ordered list of Value + * - collection of name/value pairs (javascript object) + * + * The type of the held value is represented by a #ValueType and + * can be obtained using type(). + * + * Values of an #objectValue or #arrayValue can be accessed using operator[]() + * methods. + * Non-const methods will automatically create the a #nullValue element + * if it does not exist. + * The sequence of an #arrayValue will be automatically resized and initialized + * with #nullValue. resize() can be used to enlarge or truncate an #arrayValue. + * + * The get() methods can be used to obtain default value in the case the + * required element does not exist. + * + * It is possible to iterate over the list of member keys of an object using + * the getMemberNames() method. + * + * \note #Value string-length fit in size_t, but keys must be < 2^30. + * (The reason is an implementation detail.) A #CharReader will raise an + * exception if a bound is exceeded to avoid security holes in your app, + * but the Value API does *not* check bounds. That is the responsibility + * of the caller. + */ +class JSON_API Value { + friend class ValueIteratorBase; + +public: + using Members = std::vector; + using iterator = ValueIterator; + using const_iterator = ValueConstIterator; + using UInt = Json::UInt; + using Int = Json::Int; +#if defined(JSON_HAS_INT64) + using UInt64 = Json::UInt64; + using Int64 = Json::Int64; +#endif // defined(JSON_HAS_INT64) + using LargestInt = Json::LargestInt; + using LargestUInt = Json::LargestUInt; + using ArrayIndex = Json::ArrayIndex; + + // Required for boost integration, e. g. BOOST_TEST + using value_type = std::string; + +#if JSON_USE_NULLREF + // Binary compatibility kludges, do not use. + static const Value& null; + static const Value& nullRef; +#endif + + // null and nullRef are deprecated, use this instead. + static Value const& nullSingleton(); + + /// Minimum signed integer value that can be stored in a Json::Value. + static constexpr LargestInt minLargestInt = + LargestInt(~(LargestUInt(-1) / 2)); + /// Maximum signed integer value that can be stored in a Json::Value. + static constexpr LargestInt maxLargestInt = LargestInt(LargestUInt(-1) / 2); + /// Maximum unsigned integer value that can be stored in a Json::Value. + static constexpr LargestUInt maxLargestUInt = LargestUInt(-1); + + /// Minimum signed int value that can be stored in a Json::Value. + static constexpr Int minInt = Int(~(UInt(-1) / 2)); + /// Maximum signed int value that can be stored in a Json::Value. + static constexpr Int maxInt = Int(UInt(-1) / 2); + /// Maximum unsigned int value that can be stored in a Json::Value. + static constexpr UInt maxUInt = UInt(-1); + +#if defined(JSON_HAS_INT64) + /// Minimum signed 64 bits int value that can be stored in a Json::Value. + static constexpr Int64 minInt64 = Int64(~(UInt64(-1) / 2)); + /// Maximum signed 64 bits int value that can be stored in a Json::Value. + static constexpr Int64 maxInt64 = Int64(UInt64(-1) / 2); + /// Maximum unsigned 64 bits int value that can be stored in a Json::Value. + static constexpr UInt64 maxUInt64 = UInt64(-1); +#endif // defined(JSON_HAS_INT64) + /// Default precision for real value for string representation. + static constexpr UInt defaultRealPrecision = 17; + // The constant is hard-coded because some compiler have trouble + // converting Value::maxUInt64 to a double correctly (AIX/xlC). + // Assumes that UInt64 is a 64 bits integer. + static constexpr double maxUInt64AsDouble = 18446744073709551615.0; +// Workaround for bug in the NVIDIAs CUDA 9.1 nvcc compiler +// when using gcc and clang backend compilers. CZString +// cannot be defined as private. See issue #486 +#ifdef __NVCC__ +public: +#else +private: +#endif +#ifndef JSONCPP_DOC_EXCLUDE_IMPLEMENTATION + class CZString { + public: + enum DuplicationPolicy { noDuplication = 0, duplicate, duplicateOnCopy }; + CZString(ArrayIndex index); + CZString(char const* str, unsigned length, DuplicationPolicy allocate); + CZString(CZString const& other); + CZString(CZString&& other) noexcept; + ~CZString(); + CZString& operator=(const CZString& other); + CZString& operator=(CZString&& other) noexcept; + + bool operator<(CZString const& other) const; + bool operator==(CZString const& other) const; + ArrayIndex index() const; + // const char* c_str() const; ///< \deprecated + char const* data() const; + unsigned length() const; + bool isStaticString() const; + + private: + void swap(CZString& other); + + struct StringStorage { + unsigned policy_ : 2; + unsigned length_ : 30; // 1GB max + }; + + char const* cstr_; // actually, a prefixed string, unless policy is noDup + union { + ArrayIndex index_; + StringStorage storage_; + }; + }; + +public: + typedef std::map ObjectValues; +#endif // ifndef JSONCPP_DOC_EXCLUDE_IMPLEMENTATION + +public: + /** + * \brief Create a default Value of the given type. + * + * This is a very useful constructor. + * To create an empty array, pass arrayValue. + * To create an empty object, pass objectValue. + * Another Value can then be set to this one by assignment. + * This is useful since clear() and resize() will not alter types. + * + * Examples: + * \code + * Json::Value null_value; // null + * Json::Value arr_value(Json::arrayValue); // [] + * Json::Value obj_value(Json::objectValue); // {} + * \endcode + */ + Value(ValueType type = nullValue); + Value(Int value); + Value(UInt value); +#if defined(JSON_HAS_INT64) + Value(Int64 value); + Value(UInt64 value); +#endif // if defined(JSON_HAS_INT64) + Value(double value); + Value(const char* value); ///< Copy til first 0. (NULL causes to seg-fault.) + Value(const char* begin, const char* end); ///< Copy all, incl zeroes. + /** + * \brief Constructs a value from a static string. + * + * Like other value string constructor but do not duplicate the string for + * internal storage. The given string must remain alive after the call to + * this constructor. + * + * \note This works only for null-terminated strings. (We cannot change the + * size of this class, so we have nowhere to store the length, which might be + * computed later for various operations.) + * + * Example of usage: + * \code + * static StaticString foo("some text"); + * Json::Value aValue(foo); + * \endcode + */ + Value(const StaticString& value); + Value(const String& value); + Value(bool value); + Value(std::nullptr_t ptr) = delete; + Value(const Value& other); + Value(Value&& other) noexcept; + ~Value(); + + /// \note Overwrite existing comments. To preserve comments, use + /// #swapPayload(). + Value& operator=(const Value& other); + Value& operator=(Value&& other) noexcept; + + /// Swap everything. + void swap(Value& other); + /// Swap values but leave comments and source offsets in place. + void swapPayload(Value& other); + + /// copy everything. + void copy(const Value& other); + /// copy values but leave comments and source offsets in place. + void copyPayload(const Value& other); + + ValueType type() const; + + /// Compare payload only, not comments etc. + bool operator<(const Value& other) const; + bool operator<=(const Value& other) const; + bool operator>=(const Value& other) const; + bool operator>(const Value& other) const; + bool operator==(const Value& other) const; + bool operator!=(const Value& other) const; + int compare(const Value& other) const; + + const char* asCString() const; ///< Embedded zeroes could cause you trouble! +#if JSONCPP_USING_SECURE_MEMORY + unsigned getCStringLength() const; // Allows you to understand the length of + // the CString +#endif + String asString() const; ///< Embedded zeroes are possible. + /** Get raw char* of string-value. + * \return false if !string. (Seg-fault if str or end are NULL.) + */ + bool getString(char const** begin, char const** end) const; + Int asInt() const; + UInt asUInt() const; +#if defined(JSON_HAS_INT64) + Int64 asInt64() const; + UInt64 asUInt64() const; +#endif // if defined(JSON_HAS_INT64) + LargestInt asLargestInt() const; + LargestUInt asLargestUInt() const; + float asFloat() const; + double asDouble() const; + bool asBool() const; + + bool isNull() const; + bool isBool() const; + bool isInt() const; + bool isInt64() const; + bool isUInt() const; + bool isUInt64() const; + bool isIntegral() const; + bool isDouble() const; + bool isNumeric() const; + bool isString() const; + bool isArray() const; + bool isObject() const; + + /// The `as` and `is` member function templates and specializations. + template T as() const JSONCPP_TEMPLATE_DELETE; + template bool is() const JSONCPP_TEMPLATE_DELETE; + + bool isConvertibleTo(ValueType other) const; + + /// Number of values in array or object + ArrayIndex size() const; + + /// \brief Return true if empty array, empty object, or null; + /// otherwise, false. + bool empty() const; + + /// Return !isNull() + explicit operator bool() const; + + /// Remove all object members and array elements. + /// \pre type() is arrayValue, objectValue, or nullValue + /// \post type() is unchanged + void clear(); + + /// Resize the array to newSize elements. + /// New elements are initialized to null. + /// May only be called on nullValue or arrayValue. + /// \pre type() is arrayValue or nullValue + /// \post type() is arrayValue + void resize(ArrayIndex newSize); + + ///@{ + /// Access an array element (zero based index). If the array contains less + /// than index element, then null value are inserted in the array so that + /// its size is index+1. + /// (You may need to say 'value[0u]' to get your compiler to distinguish + /// this from the operator[] which takes a string.) + Value& operator[](ArrayIndex index); + Value& operator[](int index); + ///@} + + ///@{ + /// Access an array element (zero based index). + /// (You may need to say 'value[0u]' to get your compiler to distinguish + /// this from the operator[] which takes a string.) + const Value& operator[](ArrayIndex index) const; + const Value& operator[](int index) const; + ///@} + + /// If the array contains at least index+1 elements, returns the element + /// value, otherwise returns defaultValue. + Value get(ArrayIndex index, const Value& defaultValue) const; + /// Return true if index < size(). + bool isValidIndex(ArrayIndex index) const; + /// \brief Append value to array at the end. + /// + /// Equivalent to jsonvalue[jsonvalue.size()] = value; + Value& append(const Value& value); + Value& append(Value&& value); + + /// \brief Insert value in array at specific index + bool insert(ArrayIndex index, const Value& newValue); + bool insert(ArrayIndex index, Value&& newValue); + + /// Access an object value by name, create a null member if it does not exist. + /// \note Because of our implementation, keys are limited to 2^30 -1 chars. + /// Exceeding that will cause an exception. + Value& operator[](const char* key); + /// Access an object value by name, returns null if there is no member with + /// that name. + const Value& operator[](const char* key) const; + /// Access an object value by name, create a null member if it does not exist. + /// \param key may contain embedded nulls. + Value& operator[](const String& key); + /// Access an object value by name, returns null if there is no member with + /// that name. + /// \param key may contain embedded nulls. + const Value& operator[](const String& key) const; + /** \brief Access an object value by name, create a null member if it does not + * exist. + * + * If the object has no entry for that name, then the member name used to + * store the new entry is not duplicated. + * Example of use: + * \code + * Json::Value object; + * static const StaticString code("code"); + * object[code] = 1234; + * \endcode + */ + Value& operator[](const StaticString& key); + /// Return the member named key if it exist, defaultValue otherwise. + /// \note deep copy + Value get(const char* key, const Value& defaultValue) const; + /// Return the member named key if it exist, defaultValue otherwise. + /// \note deep copy + /// \note key may contain embedded nulls. + Value get(const char* begin, const char* end, + const Value& defaultValue) const; + /// Return the member named key if it exist, defaultValue otherwise. + /// \note deep copy + /// \param key may contain embedded nulls. + Value get(const String& key, const Value& defaultValue) const; + /// Most general and efficient version of isMember()const, get()const, + /// and operator[]const + /// \note As stated elsewhere, behavior is undefined if (end-begin) >= 2^30 + Value const* find(char const* begin, char const* end) const; + /// Most general and efficient version of object-mutators. + /// \note As stated elsewhere, behavior is undefined if (end-begin) >= 2^30 + /// \return non-zero, but JSON_ASSERT if this is neither object nor nullValue. + Value* demand(char const* begin, char const* end); + /// \brief Remove and return the named member. + /// + /// Do nothing if it did not exist. + /// \pre type() is objectValue or nullValue + /// \post type() is unchanged + void removeMember(const char* key); + /// Same as removeMember(const char*) + /// \param key may contain embedded nulls. + void removeMember(const String& key); + /// Same as removeMember(const char* begin, const char* end, Value* removed), + /// but 'key' is null-terminated. + bool removeMember(const char* key, Value* removed); + /** \brief Remove the named map member. + * + * Update 'removed' iff removed. + * \param key may contain embedded nulls. + * \return true iff removed (no exceptions) + */ + bool removeMember(String const& key, Value* removed); + /// Same as removeMember(String const& key, Value* removed) + bool removeMember(const char* begin, const char* end, Value* removed); + /** \brief Remove the indexed array element. + * + * O(n) expensive operations. + * Update 'removed' iff removed. + * \return true if removed (no exceptions) + */ + bool removeIndex(ArrayIndex index, Value* removed); + + /// Return true if the object has a member named key. + /// \note 'key' must be null-terminated. + bool isMember(const char* key) const; + /// Return true if the object has a member named key. + /// \param key may contain embedded nulls. + bool isMember(const String& key) const; + /// Same as isMember(String const& key)const + bool isMember(const char* begin, const char* end) const; + + /// \brief Return a list of the member names. + /// + /// If null, return an empty list. + /// \pre type() is objectValue or nullValue + /// \post if type() was nullValue, it remains nullValue + Members getMemberNames() const; + + /// \deprecated Always pass len. + JSONCPP_DEPRECATED("Use setComment(String const&) instead.") + void setComment(const char* comment, CommentPlacement placement) { + setComment(String(comment, strlen(comment)), placement); + } + /// Comments must be //... or /* ... */ + void setComment(const char* comment, size_t len, CommentPlacement placement) { + setComment(String(comment, len), placement); + } + /// Comments must be //... or /* ... */ + void setComment(String comment, CommentPlacement placement); + bool hasComment(CommentPlacement placement) const; + /// Include delimiters and embedded newlines. + String getComment(CommentPlacement placement) const; + + String toStyledString() const; + + const_iterator begin() const; + const_iterator end() const; + + iterator begin(); + iterator end(); + + // Accessors for the [start, limit) range of bytes within the JSON text from + // which this value was parsed, if any. + void setOffsetStart(ptrdiff_t start); + void setOffsetLimit(ptrdiff_t limit); + ptrdiff_t getOffsetStart() const; + ptrdiff_t getOffsetLimit() const; + +private: + void setType(ValueType v) { + bits_.value_type_ = static_cast(v); + } + bool isAllocated() const { return bits_.allocated_; } + void setIsAllocated(bool v) { bits_.allocated_ = v; } + + void initBasic(ValueType type, bool allocated = false); + void dupPayload(const Value& other); + void releasePayload(); + void dupMeta(const Value& other); + + Value& resolveReference(const char* key); + Value& resolveReference(const char* key, const char* end); + + // struct MemberNamesTransform + //{ + // typedef const char *result_type; + // const char *operator()( const CZString &name ) const + // { + // return name.c_str(); + // } + //}; + + union ValueHolder { + LargestInt int_; + LargestUInt uint_; + double real_; + bool bool_; + char* string_; // if allocated_, ptr to { unsigned, char[] }. + ObjectValues* map_; + } value_; + + struct { + // Really a ValueType, but types should agree for bitfield packing. + unsigned int value_type_ : 8; + // Unless allocated_, string_ must be null-terminated. + unsigned int allocated_ : 1; + } bits_; + + class Comments { + public: + Comments() = default; + Comments(const Comments& that); + Comments(Comments&& that) noexcept; + Comments& operator=(const Comments& that); + Comments& operator=(Comments&& that) noexcept; + bool has(CommentPlacement slot) const; + String get(CommentPlacement slot) const; + void set(CommentPlacement slot, String comment); + + private: + using Array = std::array; + std::unique_ptr ptr_; + }; + Comments comments_; + + // [start, limit) byte offsets in the source JSON text from which this Value + // was extracted. + ptrdiff_t start_; + ptrdiff_t limit_; +}; + +template <> inline bool Value::as() const { return asBool(); } +template <> inline bool Value::is() const { return isBool(); } + +template <> inline Int Value::as() const { return asInt(); } +template <> inline bool Value::is() const { return isInt(); } + +template <> inline UInt Value::as() const { return asUInt(); } +template <> inline bool Value::is() const { return isUInt(); } + +#if defined(JSON_HAS_INT64) +template <> inline Int64 Value::as() const { return asInt64(); } +template <> inline bool Value::is() const { return isInt64(); } + +template <> inline UInt64 Value::as() const { return asUInt64(); } +template <> inline bool Value::is() const { return isUInt64(); } +#endif + +template <> inline double Value::as() const { return asDouble(); } +template <> inline bool Value::is() const { return isDouble(); } + +template <> inline String Value::as() const { return asString(); } +template <> inline bool Value::is() const { return isString(); } + +/// These `as` specializations are type conversions, and do not have a +/// corresponding `is`. +template <> inline float Value::as() const { return asFloat(); } +template <> inline const char* Value::as() const { + return asCString(); +} + +/** \brief Experimental and untested: represents an element of the "path" to + * access a node. + */ +class JSON_API PathArgument { +public: + friend class Path; + + PathArgument(); + PathArgument(ArrayIndex index); + PathArgument(const char* key); + PathArgument(String key); + +private: + enum Kind { kindNone = 0, kindIndex, kindKey }; + String key_; + ArrayIndex index_{}; + Kind kind_{kindNone}; +}; + +/** \brief Experimental and untested: represents a "path" to access a node. + * + * Syntax: + * - "." => root node + * - ".[n]" => elements at index 'n' of root node (an array value) + * - ".name" => member named 'name' of root node (an object value) + * - ".name1.name2.name3" + * - ".[0][1][2].name1[3]" + * - ".%" => member name is provided as parameter + * - ".[%]" => index is provided as parameter + */ +class JSON_API Path { +public: + Path(const String& path, const PathArgument& a1 = PathArgument(), + const PathArgument& a2 = PathArgument(), + const PathArgument& a3 = PathArgument(), + const PathArgument& a4 = PathArgument(), + const PathArgument& a5 = PathArgument()); + + const Value& resolve(const Value& root) const; + Value resolve(const Value& root, const Value& defaultValue) const; + /// Creates the "path" to access the specified node and returns a reference on + /// the node. + Value& make(Value& root) const; + +private: + using InArgs = std::vector; + using Args = std::vector; + + void makePath(const String& path, const InArgs& in); + void addPathInArg(const String& path, const InArgs& in, + InArgs::const_iterator& itInArg, PathArgument::Kind kind); + static void invalidPath(const String& path, int location); + + Args args_; +}; + +/** \brief base class for Value iterators. + * + */ +class JSON_API ValueIteratorBase { +public: + using iterator_category = std::bidirectional_iterator_tag; + using size_t = unsigned int; + using difference_type = int; + using SelfType = ValueIteratorBase; + + bool operator==(const SelfType& other) const { return isEqual(other); } + + bool operator!=(const SelfType& other) const { return !isEqual(other); } + + difference_type operator-(const SelfType& other) const { + return other.computeDistance(*this); + } + + /// Return either the index or the member name of the referenced value as a + /// Value. + Value key() const; + + /// Return the index of the referenced Value, or -1 if it is not an + /// arrayValue. + UInt index() const; + + /// Return the member name of the referenced Value, or "" if it is not an + /// objectValue. + /// \note Avoid `c_str()` on result, as embedded zeroes are possible. + String name() const; + + /// Return the member name of the referenced Value. "" if it is not an + /// objectValue. + /// \deprecated This cannot be used for UTF-8 strings, since there can be + /// embedded nulls. + JSONCPP_DEPRECATED("Use `key = name();` instead.") + char const* memberName() const; + /// Return the member name of the referenced Value, or NULL if it is not an + /// objectValue. + /// \note Better version than memberName(). Allows embedded nulls. + char const* memberName(char const** end) const; + +protected: + /*! Internal utility functions to assist with implementing + * other iterator functions. The const and non-const versions + * of the "deref" protected methods expose the protected + * current_ member variable in a way that can often be + * optimized away by the compiler. + */ + const Value& deref() const; + Value& deref(); + + void increment(); + + void decrement(); + + difference_type computeDistance(const SelfType& other) const; + + bool isEqual(const SelfType& other) const; + + void copy(const SelfType& other); + +private: + Value::ObjectValues::iterator current_; + // Indicates that iterator is for a null value. + bool isNull_{true}; + +public: + // For some reason, BORLAND needs these at the end, rather + // than earlier. No idea why. + ValueIteratorBase(); + explicit ValueIteratorBase(const Value::ObjectValues::iterator& current); +}; + +/** \brief const iterator for object and array value. + * + */ +class JSON_API ValueConstIterator : public ValueIteratorBase { + friend class Value; + +public: + using value_type = const Value; + // typedef unsigned int size_t; + // typedef int difference_type; + using reference = const Value&; + using pointer = const Value*; + using SelfType = ValueConstIterator; + + ValueConstIterator(); + ValueConstIterator(ValueIterator const& other); + +private: + /*! \internal Use by Value to create an iterator. + */ + explicit ValueConstIterator(const Value::ObjectValues::iterator& current); + +public: + SelfType& operator=(const ValueIteratorBase& other); + + SelfType operator++(int) { + SelfType temp(*this); + ++*this; + return temp; + } + + SelfType operator--(int) { + SelfType temp(*this); + --*this; + return temp; + } + + SelfType& operator--() { + decrement(); + return *this; + } + + SelfType& operator++() { + increment(); + return *this; + } + + reference operator*() const { return deref(); } + + pointer operator->() const { return &deref(); } +}; + +/** \brief Iterator for object and array value. + */ +class JSON_API ValueIterator : public ValueIteratorBase { + friend class Value; + +public: + using value_type = Value; + using size_t = unsigned int; + using difference_type = int; + using reference = Value&; + using pointer = Value*; + using SelfType = ValueIterator; + + ValueIterator(); + explicit ValueIterator(const ValueConstIterator& other); + ValueIterator(const ValueIterator& other); + +private: + /*! \internal Use by Value to create an iterator. + */ + explicit ValueIterator(const Value::ObjectValues::iterator& current); + +public: + SelfType& operator=(const SelfType& other); + + SelfType operator++(int) { + SelfType temp(*this); + ++*this; + return temp; + } + + SelfType operator--(int) { + SelfType temp(*this); + --*this; + return temp; + } + + SelfType& operator--() { + decrement(); + return *this; + } + + SelfType& operator++() { + increment(); + return *this; + } + + /*! The return value of non-const iterators can be + * changed, so the these functions are not const + * because the returned references/pointers can be used + * to change state of the base class. + */ + reference operator*() const { return const_cast(deref()); } + pointer operator->() const { return const_cast(&deref()); } +}; + +inline void swap(Value& a, Value& b) { a.swap(b); } + +} // namespace Json + +#pragma pack(pop) + +#if defined(JSONCPP_DISABLE_DLL_INTERFACE_WARNING) +#pragma warning(pop) +#endif // if defined(JSONCPP_DISABLE_DLL_INTERFACE_WARNING) + +#endif // JSON_H_INCLUDED + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: include/json/value.h +// ////////////////////////////////////////////////////////////////////// + + + + + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: include/json/reader.h +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2007-2010 Baptiste Lepilleur and The JsonCpp Authors +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +#ifndef JSON_READER_H_INCLUDED +#define JSON_READER_H_INCLUDED + +#if !defined(JSON_IS_AMALGAMATION) +#include "json_features.h" +#include "value.h" +#endif // if !defined(JSON_IS_AMALGAMATION) +#include +#include +#include +#include +#include + +// Disable warning C4251: : needs to have dll-interface to +// be used by... +#if defined(JSONCPP_DISABLE_DLL_INTERFACE_WARNING) +#pragma warning(push) +#pragma warning(disable : 4251) +#endif // if defined(JSONCPP_DISABLE_DLL_INTERFACE_WARNING) + +#pragma pack(push) +#pragma pack() + +namespace Json { + +/** \brief Unserialize a JSON document into a + * Value. + * + * \deprecated Use CharReader and CharReaderBuilder. + */ + +class JSON_API Reader { +public: + using Char = char; + using Location = const Char*; + + /** \brief An error tagged with where in the JSON text it was encountered. + * + * The offsets give the [start, limit) range of bytes within the text. Note + * that this is bytes, not codepoints. + */ + struct StructuredError { + ptrdiff_t offset_start; + ptrdiff_t offset_limit; + String message; + }; + + /** \brief Constructs a Reader allowing all features for parsing. + * \deprecated Use CharReader and CharReaderBuilder. + */ + Reader(); + + /** \brief Constructs a Reader allowing the specified feature set for parsing. + * \deprecated Use CharReader and CharReaderBuilder. + */ + Reader(const Features& features); + + /** \brief Read a Value from a JSON + * document. + * + * \param document UTF-8 encoded string containing the document + * to read. + * \param[out] root Contains the root value of the document if it + * was successfully parsed. + * \param collectComments \c true to collect comment and allow writing + * them back during serialization, \c false to + * discard comments. This parameter is ignored + * if Features::allowComments_ is \c false. + * \return \c true if the document was successfully parsed, \c false if an + * error occurred. + */ + bool parse(const std::string& document, Value& root, + bool collectComments = true); + + /** \brief Read a Value from a JSON + * document. + * + * \param beginDoc Pointer on the beginning of the UTF-8 encoded + * string of the document to read. + * \param endDoc Pointer on the end of the UTF-8 encoded string + * of the document to read. Must be >= beginDoc. + * \param[out] root Contains the root value of the document if it + * was successfully parsed. + * \param collectComments \c true to collect comment and allow writing + * them back during serialization, \c false to + * discard comments. This parameter is ignored + * if Features::allowComments_ is \c false. + * \return \c true if the document was successfully parsed, \c false if an + * error occurred. + */ + bool parse(const char* beginDoc, const char* endDoc, Value& root, + bool collectComments = true); + + /// \brief Parse from input stream. + /// \see Json::operator>>(std::istream&, Json::Value&). + bool parse(IStream& is, Value& root, bool collectComments = true); + + /** \brief Returns a user friendly string that list errors in the parsed + * document. + * + * \return Formatted error message with the list of errors with their + * location in the parsed document. An empty string is returned if no error + * occurred during parsing. + * \deprecated Use getFormattedErrorMessages() instead (typo fix). + */ + JSONCPP_DEPRECATED("Use getFormattedErrorMessages() instead.") + String getFormatedErrorMessages() const; + + /** \brief Returns a user friendly string that list errors in the parsed + * document. + * + * \return Formatted error message with the list of errors with their + * location in the parsed document. An empty string is returned if no error + * occurred during parsing. + */ + String getFormattedErrorMessages() const; + + /** \brief Returns a vector of structured errors encountered while parsing. + * + * \return A (possibly empty) vector of StructuredError objects. Currently + * only one error can be returned, but the caller should tolerate multiple + * errors. This can occur if the parser recovers from a non-fatal parse + * error and then encounters additional errors. + */ + std::vector getStructuredErrors() const; + + /** \brief Add a semantic error message. + * + * \param value JSON Value location associated with the error + * \param message The error message. + * \return \c true if the error was successfully added, \c false if the Value + * offset exceeds the document size. + */ + bool pushError(const Value& value, const String& message); + + /** \brief Add a semantic error message with extra context. + * + * \param value JSON Value location associated with the error + * \param message The error message. + * \param extra Additional JSON Value location to contextualize the error + * \return \c true if the error was successfully added, \c false if either + * Value offset exceeds the document size. + */ + bool pushError(const Value& value, const String& message, const Value& extra); + + /** \brief Return whether there are any errors. + * + * \return \c true if there are no errors to report \c false if errors have + * occurred. + */ + bool good() const; + +private: + enum TokenType { + tokenEndOfStream = 0, + tokenObjectBegin, + tokenObjectEnd, + tokenArrayBegin, + tokenArrayEnd, + tokenString, + tokenNumber, + tokenTrue, + tokenFalse, + tokenNull, + tokenArraySeparator, + tokenMemberSeparator, + tokenComment, + tokenError + }; + + class Token { + public: + TokenType type_; + Location start_; + Location end_; + }; + + class ErrorInfo { + public: + Token token_; + String message_; + Location extra_; + }; + + using Errors = std::deque; + + bool readToken(Token& token); + void skipSpaces(); + bool match(const Char* pattern, int patternLength); + bool readComment(); + bool readCStyleComment(); + bool readCppStyleComment(); + bool readString(); + void readNumber(); + bool readValue(); + bool readObject(Token& token); + bool readArray(Token& token); + bool decodeNumber(Token& token); + bool decodeNumber(Token& token, Value& decoded); + bool decodeString(Token& token); + bool decodeString(Token& token, String& decoded); + bool decodeDouble(Token& token); + bool decodeDouble(Token& token, Value& decoded); + bool decodeUnicodeCodePoint(Token& token, Location& current, Location end, + unsigned int& unicode); + bool decodeUnicodeEscapeSequence(Token& token, Location& current, + Location end, unsigned int& unicode); + bool addError(const String& message, Token& token, Location extra = nullptr); + bool recoverFromError(TokenType skipUntilToken); + bool addErrorAndRecover(const String& message, Token& token, + TokenType skipUntilToken); + void skipUntilSpace(); + Value& currentValue(); + Char getNextChar(); + void getLocationLineAndColumn(Location location, int& line, + int& column) const; + String getLocationLineAndColumn(Location location) const; + void addComment(Location begin, Location end, CommentPlacement placement); + void skipCommentTokens(Token& token); + + static bool containsNewLine(Location begin, Location end); + static String normalizeEOL(Location begin, Location end); + + using Nodes = std::stack; + Nodes nodes_; + Errors errors_; + String document_; + Location begin_{}; + Location end_{}; + Location current_{}; + Location lastValueEnd_{}; + Value* lastValue_{}; + String commentsBefore_; + Features features_; + bool collectComments_{}; +}; // Reader + +/** Interface for reading JSON from a char array. + */ +class JSON_API CharReader { +public: + virtual ~CharReader() = default; + /** \brief Read a Value from a JSON + * document. The document must be a UTF-8 encoded string containing the + * document to read. + * + * \param beginDoc Pointer on the beginning of the UTF-8 encoded string + * of the document to read. + * \param endDoc Pointer on the end of the UTF-8 encoded string of the + * document to read. Must be >= beginDoc. + * \param[out] root Contains the root value of the document if it was + * successfully parsed. + * \param[out] errs Formatted error messages (if not NULL) a user + * friendly string that lists errors in the parsed + * document. + * \return \c true if the document was successfully parsed, \c false if an + * error occurred. + */ + virtual bool parse(char const* beginDoc, char const* endDoc, Value* root, + String* errs) = 0; + + class JSON_API Factory { + public: + virtual ~Factory() = default; + /** \brief Allocate a CharReader via operator new(). + * \throw std::exception if something goes wrong (e.g. invalid settings) + */ + virtual CharReader* newCharReader() const = 0; + }; // Factory +}; // CharReader + +/** \brief Build a CharReader implementation. + * + * Usage: + * \code + * using namespace Json; + * CharReaderBuilder builder; + * builder["collectComments"] = false; + * Value value; + * String errs; + * bool ok = parseFromStream(builder, std::cin, &value, &errs); + * \endcode + */ +class JSON_API CharReaderBuilder : public CharReader::Factory { +public: + // Note: We use a Json::Value so that we can add data-members to this class + // without a major version bump. + /** Configuration of this builder. + * These are case-sensitive. + * Available settings (case-sensitive): + * - `"collectComments": false or true` + * - true to collect comment and allow writing them back during + * serialization, false to discard comments. This parameter is ignored + * if allowComments is false. + * - `"allowComments": false or true` + * - true if comments are allowed. + * - `"allowTrailingCommas": false or true` + * - true if trailing commas in objects and arrays are allowed. + * - `"strictRoot": false or true` + * - true if root must be either an array or an object value + * - `"allowDroppedNullPlaceholders": false or true` + * - true if dropped null placeholders are allowed. (See + * StreamWriterBuilder.) + * - `"allowNumericKeys": false or true` + * - true if numeric object keys are allowed. + * - `"allowSingleQuotes": false or true` + * - true if '' are allowed for strings (both keys and values) + * - `"stackLimit": integer` + * - Exceeding stackLimit (recursive depth of `readValue()`) will cause an + * exception. + * - This is a security issue (seg-faults caused by deeply nested JSON), so + * the default is low. + * - `"failIfExtra": false or true` + * - If true, `parse()` returns false when extra non-whitespace trails the + * JSON value in the input string. + * - `"rejectDupKeys": false or true` + * - If true, `parse()` returns false when a key is duplicated within an + * object. + * - `"allowSpecialFloats": false or true` + * - If true, special float values (NaNs and infinities) are allowed and + * their values are lossfree restorable. + * - `"skipBom": false or true` + * - If true, if the input starts with the Unicode byte order mark (BOM), + * it is skipped. + * + * You can examine 'settings_` yourself to see the defaults. You can also + * write and read them just like any JSON Value. + * \sa setDefaults() + */ + Json::Value settings_; + + CharReaderBuilder(); + ~CharReaderBuilder() override; + + CharReader* newCharReader() const override; + + /** \return true if 'settings' are legal and consistent; + * otherwise, indicate bad settings via 'invalid'. + */ + bool validate(Json::Value* invalid) const; + + /** A simple way to update a specific setting. + */ + Value& operator[](const String& key); + + /** Called by ctor, but you can use this to reset settings_. + * \pre 'settings' != NULL (but Json::null is fine) + * \remark Defaults: + * \snippet src/lib_json/json_reader.cpp CharReaderBuilderDefaults + */ + static void setDefaults(Json::Value* settings); + /** Same as old Features::strictMode(). + * \pre 'settings' != NULL (but Json::null is fine) + * \remark Defaults: + * \snippet src/lib_json/json_reader.cpp CharReaderBuilderStrictMode + */ + static void strictMode(Json::Value* settings); +}; + +/** Consume entire stream and use its begin/end. + * Someday we might have a real StreamReader, but for now this + * is convenient. + */ +bool JSON_API parseFromStream(CharReader::Factory const&, IStream&, Value* root, + String* errs); + +/** \brief Read from 'sin' into 'root'. + * + * Always keep comments from the input JSON. + * + * This can be used to read a file into a particular sub-object. + * For example: + * \code + * Json::Value root; + * cin >> root["dir"]["file"]; + * cout << root; + * \endcode + * Result: + * \verbatim + * { + * "dir": { + * "file": { + * // The input stream JSON would be nested here. + * } + * } + * } + * \endverbatim + * \throw std::exception on parse error. + * \see Json::operator<<() + */ +JSON_API IStream& operator>>(IStream&, Value&); + +} // namespace Json + +#pragma pack(pop) + +#if defined(JSONCPP_DISABLE_DLL_INTERFACE_WARNING) +#pragma warning(pop) +#endif // if defined(JSONCPP_DISABLE_DLL_INTERFACE_WARNING) + +#endif // JSON_READER_H_INCLUDED + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: include/json/reader.h +// ////////////////////////////////////////////////////////////////////// + + + + + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: include/json/writer.h +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2007-2010 Baptiste Lepilleur and The JsonCpp Authors +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +#ifndef JSON_WRITER_H_INCLUDED +#define JSON_WRITER_H_INCLUDED + +#if !defined(JSON_IS_AMALGAMATION) +#include "value.h" +#endif // if !defined(JSON_IS_AMALGAMATION) +#include +#include +#include + +// Disable warning C4251: : needs to have dll-interface to +// be used by... +#if defined(JSONCPP_DISABLE_DLL_INTERFACE_WARNING) && defined(_MSC_VER) +#pragma warning(push) +#pragma warning(disable : 4251) +#endif // if defined(JSONCPP_DISABLE_DLL_INTERFACE_WARNING) + +#pragma pack(push) +#pragma pack() + +namespace Json { + +class Value; + +/** + * + * Usage: + * \code + * using namespace Json; + * void writeToStdout(StreamWriter::Factory const& factory, Value const& value) + * { std::unique_ptr const writer( factory.newStreamWriter()); + * writer->write(value, &std::cout); + * std::cout << std::endl; // add lf and flush + * } + * \endcode + */ +class JSON_API StreamWriter { +protected: + OStream* sout_; // not owned; will not delete +public: + StreamWriter(); + virtual ~StreamWriter(); + /** Write Value into document as configured in sub-class. + * Do not take ownership of sout, but maintain a reference during function. + * \pre sout != NULL + * \return zero on success (For now, we always return zero, so check the + * stream instead.) \throw std::exception possibly, depending on + * configuration + */ + virtual int write(Value const& root, OStream* sout) = 0; + + /** \brief A simple abstract factory. + */ + class JSON_API Factory { + public: + virtual ~Factory(); + /** \brief Allocate a CharReader via operator new(). + * \throw std::exception if something goes wrong (e.g. invalid settings) + */ + virtual StreamWriter* newStreamWriter() const = 0; + }; // Factory +}; // StreamWriter + +/** \brief Write into stringstream, then return string, for convenience. + * A StreamWriter will be created from the factory, used, and then deleted. + */ +String JSON_API writeString(StreamWriter::Factory const& factory, + Value const& root); + +/** \brief Build a StreamWriter implementation. + +* Usage: +* \code +* using namespace Json; +* Value value = ...; +* StreamWriterBuilder builder; +* builder["commentStyle"] = "None"; +* builder["indentation"] = " "; // or whatever you like +* std::unique_ptr writer( +* builder.newStreamWriter()); +* writer->write(value, &std::cout); +* std::cout << std::endl; // add lf and flush +* \endcode +*/ +class JSON_API StreamWriterBuilder : public StreamWriter::Factory { +public: + // Note: We use a Json::Value so that we can add data-members to this class + // without a major version bump. + /** Configuration of this builder. + * Available settings (case-sensitive): + * - "commentStyle": "None" or "All" + * - "indentation": "". + * - Setting this to an empty string also omits newline characters. + * - "enableYAMLCompatibility": false or true + * - slightly change the whitespace around colons + * - "dropNullPlaceholders": false or true + * - Drop the "null" string from the writer's output for nullValues. + * Strictly speaking, this is not valid JSON. But when the output is being + * fed to a browser's JavaScript, it makes for smaller output and the + * browser can handle the output just fine. + * - "useSpecialFloats": false or true + * - If true, outputs non-finite floating point values in the following way: + * NaN values as "NaN", positive infinity as "Infinity", and negative + * infinity as "-Infinity". + * - "precision": int + * - Number of precision digits for formatting of real values. + * - "precisionType": "significant"(default) or "decimal" + * - Type of precision for formatting of real values. + * - "emitUTF8": false or true + * - If true, outputs raw UTF8 strings instead of escaping them. + + * You can examine 'settings_` yourself + * to see the defaults. You can also write and read them just like any + * JSON Value. + * \sa setDefaults() + */ + Json::Value settings_; + + StreamWriterBuilder(); + ~StreamWriterBuilder() override; + + /** + * \throw std::exception if something goes wrong (e.g. invalid settings) + */ + StreamWriter* newStreamWriter() const override; + + /** \return true if 'settings' are legal and consistent; + * otherwise, indicate bad settings via 'invalid'. + */ + bool validate(Json::Value* invalid) const; + /** A simple way to update a specific setting. + */ + Value& operator[](const String& key); + + /** Called by ctor, but you can use this to reset settings_. + * \pre 'settings' != NULL (but Json::null is fine) + * \remark Defaults: + * \snippet src/lib_json/json_writer.cpp StreamWriterBuilderDefaults + */ + static void setDefaults(Json::Value* settings); +}; + +/** \brief Abstract class for writers. + * \deprecated Use StreamWriter. (And really, this is an implementation detail.) + */ +class JSON_API Writer { +public: + virtual ~Writer(); + + virtual String write(const Value& root) = 0; +}; + +/** \brief Outputs a Value in JSON format + *without formatting (not human friendly). + * + * The JSON document is written in a single line. It is not intended for 'human' + *consumption, + * but may be useful to support feature such as RPC where bandwidth is limited. + * \sa Reader, Value + * \deprecated Use StreamWriterBuilder. + */ +#if defined(_MSC_VER) +#pragma warning(push) +#pragma warning(disable : 4996) // Deriving from deprecated class +#endif +class JSON_API FastWriter + : public Writer { +public: + FastWriter(); + ~FastWriter() override = default; + + void enableYAMLCompatibility(); + + /** \brief Drop the "null" string from the writer's output for nullValues. + * Strictly speaking, this is not valid JSON. But when the output is being + * fed to a browser's JavaScript, it makes for smaller output and the + * browser can handle the output just fine. + */ + void dropNullPlaceholders(); + + void omitEndingLineFeed(); + +public: // overridden from Writer + String write(const Value& root) override; + +private: + void writeValue(const Value& value); + + String document_; + bool yamlCompatibilityEnabled_{false}; + bool dropNullPlaceholders_{false}; + bool omitEndingLineFeed_{false}; +}; +#if defined(_MSC_VER) +#pragma warning(pop) +#endif + +/** \brief Writes a Value in JSON format in a + *human friendly way. + * + * The rules for line break and indent are as follow: + * - Object value: + * - if empty then print {} without indent and line break + * - if not empty the print '{', line break & indent, print one value per + *line + * and then unindent and line break and print '}'. + * - Array value: + * - if empty then print [] without indent and line break + * - if the array contains no object value, empty array or some other value + *types, + * and all the values fit on one lines, then print the array on a single + *line. + * - otherwise, it the values do not fit on one line, or the array contains + * object or non empty array, then print one value per line. + * + * If the Value have comments then they are outputted according to their + *#CommentPlacement. + * + * \sa Reader, Value, Value::setComment() + * \deprecated Use StreamWriterBuilder. + */ +#if defined(_MSC_VER) +#pragma warning(push) +#pragma warning(disable : 4996) // Deriving from deprecated class +#endif +class JSON_API + StyledWriter : public Writer { +public: + StyledWriter(); + ~StyledWriter() override = default; + +public: // overridden from Writer + /** \brief Serialize a Value in JSON format. + * \param root Value to serialize. + * \return String containing the JSON document that represents the root value. + */ + String write(const Value& root) override; + +private: + void writeValue(const Value& value); + void writeArrayValue(const Value& value); + bool isMultilineArray(const Value& value); + void pushValue(const String& value); + void writeIndent(); + void writeWithIndent(const String& value); + void indent(); + void unindent(); + void writeCommentBeforeValue(const Value& root); + void writeCommentAfterValueOnSameLine(const Value& root); + static bool hasCommentForValue(const Value& value); + static String normalizeEOL(const String& text); + + using ChildValues = std::vector; + + ChildValues childValues_; + String document_; + String indentString_; + unsigned int rightMargin_{74}; + unsigned int indentSize_{3}; + bool addChildValues_{false}; +}; +#if defined(_MSC_VER) +#pragma warning(pop) +#endif + +/** \brief Writes a Value in JSON format in a + human friendly way, + to a stream rather than to a string. + * + * The rules for line break and indent are as follow: + * - Object value: + * - if empty then print {} without indent and line break + * - if not empty the print '{', line break & indent, print one value per + line + * and then unindent and line break and print '}'. + * - Array value: + * - if empty then print [] without indent and line break + * - if the array contains no object value, empty array or some other value + types, + * and all the values fit on one lines, then print the array on a single + line. + * - otherwise, it the values do not fit on one line, or the array contains + * object or non empty array, then print one value per line. + * + * If the Value have comments then they are outputted according to their + #CommentPlacement. + * + * \sa Reader, Value, Value::setComment() + * \deprecated Use StreamWriterBuilder. + */ +#if defined(_MSC_VER) +#pragma warning(push) +#pragma warning(disable : 4996) // Deriving from deprecated class +#endif +class JSON_API + StyledStreamWriter { +public: + /** + * \param indentation Each level will be indented by this amount extra. + */ + StyledStreamWriter(String indentation = "\t"); + ~StyledStreamWriter() = default; + +public: + /** \brief Serialize a Value in JSON format. + * \param out Stream to write to. (Can be ostringstream, e.g.) + * \param root Value to serialize. + * \note There is no point in deriving from Writer, since write() should not + * return a value. + */ + void write(OStream& out, const Value& root); + +private: + void writeValue(const Value& value); + void writeArrayValue(const Value& value); + bool isMultilineArray(const Value& value); + void pushValue(const String& value); + void writeIndent(); + void writeWithIndent(const String& value); + void indent(); + void unindent(); + void writeCommentBeforeValue(const Value& root); + void writeCommentAfterValueOnSameLine(const Value& root); + static bool hasCommentForValue(const Value& value); + static String normalizeEOL(const String& text); + + using ChildValues = std::vector; + + ChildValues childValues_; + OStream* document_; + String indentString_; + unsigned int rightMargin_{74}; + String indentation_; + bool addChildValues_ : 1; + bool indented_ : 1; +}; +#if defined(_MSC_VER) +#pragma warning(pop) +#endif + +#if defined(JSON_HAS_INT64) +String JSON_API valueToString(Int value); +String JSON_API valueToString(UInt value); +#endif // if defined(JSON_HAS_INT64) +String JSON_API valueToString(LargestInt value); +String JSON_API valueToString(LargestUInt value); +String JSON_API valueToString( + double value, unsigned int precision = Value::defaultRealPrecision, + PrecisionType precisionType = PrecisionType::significantDigits); +String JSON_API valueToString(bool value); +String JSON_API valueToQuotedString(const char* value); + +/// \brief Output using the StyledStreamWriter. +/// \see Json::operator>>() +JSON_API OStream& operator<<(OStream&, const Value& root); + +} // namespace Json + +#pragma pack(pop) + +#if defined(JSONCPP_DISABLE_DLL_INTERFACE_WARNING) +#pragma warning(pop) +#endif // if defined(JSONCPP_DISABLE_DLL_INTERFACE_WARNING) + +#endif // JSON_WRITER_H_INCLUDED + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: include/json/writer.h +// ////////////////////////////////////////////////////////////////////// + + + + + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: include/json/assertions.h +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2007-2010 Baptiste Lepilleur and The JsonCpp Authors +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +#ifndef JSON_ASSERTIONS_H_INCLUDED +#define JSON_ASSERTIONS_H_INCLUDED + +#include +#include + +#if !defined(JSON_IS_AMALGAMATION) +#include "config.h" +#endif // if !defined(JSON_IS_AMALGAMATION) + +/** It should not be possible for a maliciously designed file to + * cause an abort() or seg-fault, so these macros are used only + * for pre-condition violations and internal logic errors. + */ +#if JSON_USE_EXCEPTION + +// @todo <= add detail about condition in exception +#define JSON_ASSERT(condition) \ + do { \ + if (!(condition)) { \ + Json::throwLogicError("assert json failed"); \ + } \ + } while (0) + +#define JSON_FAIL_MESSAGE(message) \ + do { \ + OStringStream oss; \ + oss << message; \ + Json::throwLogicError(oss.str()); \ + abort(); \ + } while (0) + +#else // JSON_USE_EXCEPTION + +#define JSON_ASSERT(condition) assert(condition) + +// The call to assert() will show the failure message in debug builds. In +// release builds we abort, for a core-dump or debugger. +#define JSON_FAIL_MESSAGE(message) \ + { \ + OStringStream oss; \ + oss << message; \ + assert(false && oss.str().c_str()); \ + abort(); \ + } + +#endif + +#define JSON_ASSERT_MESSAGE(condition, message) \ + do { \ + if (!(condition)) { \ + JSON_FAIL_MESSAGE(message); \ + } \ + } while (0) + +#endif // JSON_ASSERTIONS_H_INCLUDED + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: include/json/assertions.h +// ////////////////////////////////////////////////////////////////////// + + + + + +#endif //ifndef JSON_AMALGAMATED_H_INCLUDED diff --git a/src/fu/launch/fu.launch.py b/src/fu/launch/fu.launch.py new file mode 100644 index 0000000..595c142 --- /dev/null +++ b/src/fu/launch/fu.launch.py @@ -0,0 +1,23 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os + + +def generate_launch_description(): + # 获取配置文件路径 + config_dir = os.path.join( + get_package_share_directory("fu"), "config", "fu_params.yaml" + ) + + return LaunchDescription( + [ + Node( + package="fu", + executable="fu_node", + name="FU", + parameters=[config_dir], # 从YAML文件加载参数 + output="screen", + ), + ] + ) diff --git a/src/fu/package.xml b/src/fu/package.xml new file mode 100644 index 0000000..46cd03f --- /dev/null +++ b/src/fu/package.xml @@ -0,0 +1,23 @@ + + + + fu + 0.0.0 + TODO: Package description + zxwl + TODO: License declaration + + ament_cmake + + rclcpp + std_msgs + sweeper_interfaces + mc + + ament_lint_auto + ament_lint_common + + + ament_cmake + + \ No newline at end of file diff --git a/src/fu/src/fu_node.cpp b/src/fu/src/fu_node.cpp new file mode 100644 index 0000000..200fb19 --- /dev/null +++ b/src/fu/src/fu_node.cpp @@ -0,0 +1,1691 @@ +#include "rclcpp/rclcpp.hpp" +#include "sweeper_interfaces/msg/pl.hpp" +#include "sweeper_interfaces/msg/fu.hpp" +#include "sweeper_interfaces/msg/detect_line.hpp" +#include "sweeper_interfaces/msg/sub.hpp" +#include +#include +#include "sweeper_interfaces/msg/can_frame.hpp" +#include "sweeper_interfaces/msg/mc_ctrl.hpp" + +#include +#include +#include +#include + +#include +#include +#include +using namespace std; + +// 栅格值定义 +const int EMPTY = 0; // 空区域 +const int OBSTACLE = 1; // 障碍物 +const int VEHICLE = 2; // 车体 + +int located = 0; +bool detected_line = false; +float nl_adjust = 0; +float remain_adjust = -1; +int is_start = 0; + +class fu_node : public rclcpp::Node +{ +public: + fu_node(std::string name) : Node(name) + { + RCLCPP_INFO(this->get_logger(), "%s节点已经启动.", name.c_str()); + + // 声明并获取参数（默认开启遇障停车和绕障） + this->declare_parameter("enable_obstacle_stop", true); // 是否是否开启遇障停车 + this->declare_parameter("enable_obstacle_avoid", true); // 是否开启绕障 + this->declare_parameter("enable_visualize_grid", true); // 是否可视化栅格 + this->declare_parameter("FRONT_STOP_ZONE_ROWS", 3); // 车前必停区行数 + this->declare_parameter("REAR_STOP_ZONE_ROWS", 1); // 车后必停区行数 + this->declare_parameter("LEFT_STOP_ZONE_COLS", 3); // 车左必停区列数 + this->declare_parameter("RIGHT_STOP_ZONE_COLS", 3); // 车右必停区列数 + this->declare_parameter("front_area_extend", 7); // 前方扩展检测区域 + this->declare_parameter("left_area_extend", 2); // 左侧扩展检测区域 + this->declare_parameter("right_area_extend", 2); // 右侧扩展检测区域 + + this->get_parameter("enable_obstacle_stop", enable_obstacle_stop_); + this->get_parameter("enable_obstacle_avoid", enable_obstacle_avoid_); + this->get_parameter("enable_visualize_grid", enable_visualize_grid_); + this->get_parameter("FRONT_STOP_ZONE_ROWS", FRONT_STOP_ZONE_ROWS_); + this->get_parameter("REAR_STOP_ZONE_ROWS", REAR_STOP_ZONE_ROWS_); + this->get_parameter("LEFT_STOP_ZONE_COLS", LEFT_STOP_ZONE_COLS_); + this->get_parameter("RIGHT_STOP_ZONE_COLS", RIGHT_STOP_ZONE_COLS_); + this->get_parameter("front_area_extend", front_area_extend_); + this->get_parameter("left_area_extend", left_area_extend_); + this->get_parameter("right_area_extend", right_area_extend_); + + // 创建参数回调，支持动态修改 + parameter_callback_handle_ = this->add_on_set_parameters_callback( + std::bind(&fu_node::parameter_callback, this, std::placeholders::_1)); + + // 创建订阅器 + subscription_grid = this->create_subscription( + "grid_raw", 10, std::bind(&fu_node::gridCallback, this, std::placeholders::_1)); + + msg_subscribe_pl = this->create_subscription("pl_message", 10, + std::bind(&fu_node::msg_callback_pl, this, std::placeholders::_1)); + msg_subscribe_DetectLine = this->create_subscription("airy_message", 10, + std::bind(&fu_node::msg_callback_DetectLine, this, std::placeholders::_1)); + + // 发布控制指令 + pub_mc = this->create_publisher("auto_mc_ctrl", 10); + + // 创建定时器，100ms为周期 + timer_ = this->create_wall_timer(std::chrono::milliseconds(100), + std::bind(&fu_node::timer_callback, this)); + + // 避障相关参数初始化 + avoid_counter_ = 0; + avoid_state_ = AVOID_NONE; + avoid_direction_ = 0; + + // 初始化车体位置 + vehicle_min_x_ = -1; + vehicle_max_x_ = -1; + vehicle_min_y_ = -1; + vehicle_max_y_ = -1; + + // 打印所有参数值（添加到构造函数末尾） + RCLCPP_INFO_STREAM(this->get_logger(), + "\n\n---------- FU节点参数配置 ----------" + << "\n 遇障停车功能: " << (enable_obstacle_stop_ ? "开启" : "关闭") + << "\n 绕障功能: " << (enable_obstacle_avoid_ ? "开启" : "关闭") + << "\n 可视化栅格: " << (enable_visualize_grid_ ? "是" : "否") + << "\n 车前必停区行数: " << FRONT_STOP_ZONE_ROWS_ + << "\n 车后必停区行数: " << REAR_STOP_ZONE_ROWS_ + << "\n 车左必停区列数: " << LEFT_STOP_ZONE_COLS_ + << "\n 车右必停区列数: " << RIGHT_STOP_ZONE_COLS_ + << "\n 前方扩展检测区域: " << front_area_extend_ + << "\n 左侧扩展检测区域: " << left_area_extend_ + << "\n 右侧扩展检测区域: " << right_area_extend_ + << "\n--------------------------------------------\n"); + } + +private: + // 避障状态 + enum AvoidState + { + AVOID_NONE, // 无避障 + AVOID_WAITING, // 等待决策 + AVOID_LEFT, // 向左绕障 + AVOID_RIGHT, // 向右绕障 + AVOID_RETURNING // 返回原路径 + }; + + // 避障状态管理 + struct AvoidanceContext + { + AvoidState state = AVOID_NONE; + int direction = 0; // -1=左, 1=右 + int counter = 0; + rclcpp::Time start_time; + rclcpp::Time last_state_change_time; + + // 路径记录 + std::vector angle_history; // 记录最近的角度历史 + int original_angle = 0; // 开始绕障时的原始角度 + + // 动态参数 + int max_avoid_angle = 25; // 最大绕障角度 + double min_avoid_time = 1.5; // 最小绕障时间 + double max_avoid_time = 6.0; // 最大绕障时间 + + void reset() + { + state = AVOID_NONE; + direction = 0; + counter = 0; + angle_history.clear(); + original_angle = 0; + } + + void addAngleHistory(int angle) + { + angle_history.push_back(angle); + if (angle_history.size() > 20) + { // 只保留最近20个角度 + angle_history.erase(angle_history.begin()); + } + } + }; + + AvoidanceContext avoid_ctx_; + + // 障碍物分析结果结构体 + struct ObstacleAnalysis + { + bool has_front_critical = false; // 前方必停区 + bool has_front_area = false; // 前方扩展区域 + bool has_left_critical = false; // 左方必停区 + bool has_right_critical = false; // 右方必停区 + + int obstacle_distance = -1; // 最近障碍物距离 + int obstacle_width = 0; // 障碍物宽度 + int left_edge = -1, right_edge = -1; // 障碍物边缘 + int free_space_left = 0; // 左侧可用空间 + int free_space_right = 0; // 右侧可用空间 + + // 动态安全距离计算 + int calculateSafeDistance(int speed) const + { + // 基于速度计算安全距离：速度越快，需要的距离越远 + return std::max(3, speed / 10 + 2); // 最少3格，速度30时为5格 + } + + // 评估绕障可行性 + bool canAvoid() const + { + return obstacle_width > 0 && obstacle_width < 10 && + (free_space_left >= 3 || free_space_right >= 3); + } + + // 评估是否需要紧急制动 + bool needEmergencyBrake(int speed) const + { + if (!has_front_critical) + return false; + int safe_distance = calculateSafeDistance(speed); + return obstacle_distance <= safe_distance; + } + }; + + ObstacleAnalysis obstacle_analysis_; + + // 新增：控制参数 + bool enable_obstacle_stop_; // 遇障停车使能 + bool enable_obstacle_avoid_; // 绕障功能使能 + bool enable_visualize_grid_; // 可视化栅格使能 + + // 必停区范围参数 + int FRONT_STOP_ZONE_ROWS_; // 车前必停区行数 + int REAR_STOP_ZONE_ROWS_; // 车后必停区行数 + int LEFT_STOP_ZONE_COLS_; // 车左必停区列数 + int RIGHT_STOP_ZONE_COLS_; // 车右必停区列数 + + // 分析前方更宽区域的障碍物 + int front_area_extend_; // 前方扩展检测区域 + int left_area_extend_; // 左侧扩展检测区域 + int right_area_extend_; // 右侧扩展检测区域 + rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_; + + // 接收PL消息的参数 + double x = 0.0; + double y = 0.0; + int speed = 0; // 从PL消息获取的速度 + int reliability = 0; + + // 发布消息的参数 + int publish_speed = 0; // 转速占空比 0-100 + int publish_angle = 0; // [-66.0,66.0]，负数表示左转，正数表示右转 + + // 车体矩形边界（栅格坐标） + int vehicle_min_x_; // 车体最小行坐标（顶部） + int vehicle_max_x_; // 车体最大行坐标（底部） + int vehicle_min_y_; // 车体最小列坐标（左侧） + int vehicle_max_y_; // 车体最大列坐标（右侧） + + // 必停区定义（栅格坐标）- 动态计算 + struct SafetyZone + { + int min_x, max_x; // X轴范围（行） + int min_y, max_y; // Y轴范围（列） + }; + + SafetyZone front_stop_zone_; // 前方必停区 + SafetyZone rear_stop_zone_; // 后方必停区 + SafetyZone left_stop_zone_; // 左方必停区 + SafetyZone right_stop_zone_; // 右方必停区 + + AvoidState avoid_state_; // 当前避障状态 + int avoid_counter_; // 避障计数器 + int avoid_direction_; // 避障方向（-1=左，1=右） + int avoid_angle_; // 避障角度 + rclcpp::Time last_avoid_time_; // 上次避障时间 + + // 障碍物检测结果 + bool obstacle_in_front_ = false; // 前方必停区有障碍物 + bool obstacle_in_rear_ = false; // 后方必停区有障碍物 + bool obstacle_in_left_ = false; // 左方必停区有障碍物 + bool obstacle_in_right_ = false; // 右方必停区有障碍物 + bool obstacle_in_front_area_ = false; // 前方区域有障碍物 + int obstacle_left_edge_ = -1; // 障碍物左边缘 + int obstacle_right_edge_ = -1; // 障碍物右边缘 + int obstacle_max_x_ = 100; // 障碍物距离车体最近的X坐标 + int free_space_left_ = 0; // 左侧可用空间 + int free_space_right_ = 0; // 右侧可用空间 + + // 声明订阅者和发布者 + rclcpp::Subscription::SharedPtr subscription_grid; + rclcpp::Subscription::SharedPtr msg_subscribe_pl; + rclcpp::Subscription::SharedPtr msg_subscribe_DetectLine; + rclcpp::Subscription::SharedPtr subscription_; + + rclcpp::Publisher::SharedPtr pub_mc; + rclcpp::TimerBase::SharedPtr timer_; + + // 参数动态更新回调 + rcl_interfaces::msg::SetParametersResult parameter_callback(const std::vector ¶meters) + { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + + for (const auto ¶m : parameters) + { + if (param.get_name() == "enable_obstacle_stop") + { + enable_obstacle_stop_ = param.as_bool(); + RCLCPP_INFO(this->get_logger(), "遇障停车功能 %s", enable_obstacle_stop_ ? "开启" : "关闭"); + } + else if (param.get_name() == "enable_obstacle_avoid") + { + enable_obstacle_avoid_ = param.as_bool(); + RCLCPP_INFO(this->get_logger(), "绕障功能 %s", enable_obstacle_avoid_ ? "开启" : "关闭"); + // 关闭绕障时重置避障状态 + if (!enable_obstacle_avoid_) + { + avoid_ctx_.state = AVOID_NONE; + avoid_counter_ = 0; + } + } + } + return result; + } + + // 根据目标点x,y计算方向盘角度 + float calculate_the_angle(double x, double y) + { + // 处理x为0的情况 + if (x == 0) + { + return 90.0f; + } + + // 计算目标点的角度 + float angle_f = atan2(y, x) * 180 / M_PI; + + // 计算方向盘角度 + float steering_angle; + if (angle_f <= 90.0f) + { + steering_angle = 90.0f - angle_f; + } + else + { + steering_angle = -(angle_f - 90.0f); + } + + steering_angle = std::clamp(steering_angle, -50.0f, 50.0f); + return steering_angle; + } + + // 查找车体在栅格中的矩形边界 + void findVehicleRectangle(const std::vector> &grid) + { + // 重置车体边界 + vehicle_min_x_ = -1; + vehicle_max_x_ = -1; + vehicle_min_y_ = -1; + vehicle_max_y_ = -1; + + // 遍历栅格查找所有车体栅格 + for (size_t i = 0; i < grid.size(); ++i) + { + for (size_t j = 0; j < grid[i].size(); ++j) + { + if (grid[i][j] == VEHICLE) + { + // 更新最小和最大行坐标 + if (vehicle_min_x_ == -1 || static_cast(i) < vehicle_min_x_) + vehicle_min_x_ = i; + if (vehicle_max_x_ == -1 || static_cast(i) > vehicle_max_x_) + vehicle_max_x_ = i; + + // 更新最小和最大列坐标 + if (vehicle_min_y_ == -1 || static_cast(j) < vehicle_min_y_) + vehicle_min_y_ = j; + if (vehicle_max_y_ == -1 || static_cast(j) > vehicle_max_y_) + vehicle_max_y_ = j; + } + } + } + + // 如果找到车体，计算必停区域 + if (vehicle_min_x_ != -1 && vehicle_max_x_ != -1 && + vehicle_min_y_ != -1 && vehicle_max_y_ != -1) + { + calculateSafetyZones(grid.size(), grid[0].size()); + } + } + + // 基于矩形车体计算必停区域 + void calculateSafetyZones(int grid_height, int grid_width) + { + // 前方必停区：车体前方FRONT_STOP_ZONE_ROWS行，与车体同宽 + front_stop_zone_.min_x = max(0, vehicle_min_x_ - FRONT_STOP_ZONE_ROWS_); + front_stop_zone_.max_x = vehicle_min_x_ - 1; // 紧接车体前方 + front_stop_zone_.min_y = vehicle_min_y_; + front_stop_zone_.max_y = vehicle_max_y_; + + // 后方必停区：车体后方REAR_STOP_ZONE_ROWS行，与车体同宽 + rear_stop_zone_.min_x = vehicle_max_x_ + 1; // 紧接车体后方 + rear_stop_zone_.max_x = min(grid_height - 1, vehicle_max_x_ + REAR_STOP_ZONE_ROWS_); + rear_stop_zone_.min_y = vehicle_min_y_; + rear_stop_zone_.max_y = vehicle_max_y_; + + // 左方必停区：车体左侧LEFT_STOP_ZONE_COLS列，与车体同高 + left_stop_zone_.min_x = vehicle_min_x_; + left_stop_zone_.max_x = vehicle_max_x_; + left_stop_zone_.min_y = max(0, vehicle_min_y_ - LEFT_STOP_ZONE_COLS_); + left_stop_zone_.max_y = vehicle_min_y_ - 1; // 紧接车体左侧 + + // 右方必停区：车体右侧RIGHT_STOP_ZONE_COLS列，与车体同高 + right_stop_zone_.min_x = vehicle_min_x_; + right_stop_zone_.max_x = vehicle_max_x_; + right_stop_zone_.min_y = vehicle_max_y_ + 1; // 紧接车体右侧 + right_stop_zone_.max_y = min(grid_width - 1, vehicle_max_y_ + RIGHT_STOP_ZONE_COLS_); + } + + void gridCallback(const std_msgs::msg::Int32MultiArray::SharedPtr msg) + { + // 检查消息是否包含维度信息 + if (msg->layout.dim.size() != 2) + { + RCLCPP_ERROR(this->get_logger(), "Received grid message with invalid dimensions!"); + return; + } + + // 获取栅格尺寸 + size_t height = msg->layout.dim[0].size; + size_t width = msg->layout.dim[1].size; + + // 检查数据长度是否匹配 + if (msg->data.size() != height * width) + { + RCLCPP_ERROR(this->get_logger(), "Grid data size mismatch! Expected %zu, got %zu", + height * width, msg->data.size()); + return; + } + + // 重构二维数组 + std::vector> grid(height, std::vector(width)); + + // 填充数据 + for (size_t i = 0; i < height; ++i) + { + for (size_t j = 0; j < width; ++j) + { + grid[i][j] = msg->data[i * width + j]; + } + } + + // 查找车体矩形边界并计算必停区 + findVehicleRectangle(grid); + + // 障碍物检测与分析 + analyzeObstacles(grid); + + // 可视化栅格 + if (enable_visualize_grid_) + visualizeGridInTerminal(grid); + + cacheGridData(grid); // 缓存栅格数据 + } + + std::vector> cached_grid_; + bool grid_data_valid_ = false; + + void cacheGridData(const std::vector> &grid) + { + cached_grid_ = grid; + grid_data_valid_ = true; + } + + // 分析障碍物分布 + void analyzeObstacles(const std::vector> &grid) + { + // 重置检测结果 + obstacle_in_front_ = false; + obstacle_in_rear_ = false; + obstacle_in_left_ = false; + obstacle_in_right_ = false; + obstacle_in_front_area_ = false; + obstacle_left_edge_ = -1; + obstacle_right_edge_ = -1; + free_space_left_ = 0; + free_space_right_ = 0; + obstacle_max_x_ = 100; + + // 如果未找到车体位置，不进行检测 + if (vehicle_min_x_ == -1 || vehicle_max_x_ == -1 || + vehicle_min_y_ == -1 || vehicle_max_y_ == -1) + return; + + // 检查前方必停区（优化：按X从近到远检索，找到最近障碍物后立即退出） + bool found_front_obstacle = false; + for (int i = front_stop_zone_.max_x; i >= front_stop_zone_.min_x && !found_front_obstacle; --i) + { + for (int j = front_stop_zone_.min_y; j <= front_stop_zone_.max_y; ++j) + { + if (static_cast(i) < grid.size() && + static_cast(j) < grid[0].size() && + grid[i][j] == OBSTACLE) + { + obstacle_in_front_ = true; // 标记存在障碍物 + obstacle_max_x_ = vehicle_min_x_ - i; + found_front_obstacle = true; + break; // 退出内层循环 + } + } + } + + // 检查后方必停区 + for (int i = rear_stop_zone_.min_x; i <= rear_stop_zone_.max_x; ++i) + { + for (int j = rear_stop_zone_.min_y; j <= rear_stop_zone_.max_y; ++j) + { + if (static_cast(i) < grid.size() && static_cast(j) < grid[0].size() && + grid[i][j] == OBSTACLE) + { + obstacle_in_rear_ = true; + break; + } + } + if (obstacle_in_rear_) + break; + } + + // 检查左方必停区 + for (int i = left_stop_zone_.min_x; i <= left_stop_zone_.max_x; ++i) + { + for (int j = left_stop_zone_.min_y; j <= left_stop_zone_.max_y; ++j) + { + if (static_cast(i) < grid.size() && static_cast(j) < grid[0].size() && + grid[i][j] == OBSTACLE) + { + obstacle_in_left_ = true; + break; + } + } + if (obstacle_in_left_) + break; + } + + // 检查右方必停区 + for (int i = right_stop_zone_.min_x; i <= right_stop_zone_.max_x; ++i) + { + for (int j = right_stop_zone_.min_y; j <= right_stop_zone_.max_y; ++j) + { + if (static_cast(i) < grid.size() && static_cast(j) < grid[0].size() && + grid[i][j] == OBSTACLE) + { + obstacle_in_right_ = true; + break; + } + } + if (obstacle_in_right_) + break; + } + + size_t front_area_min_x = static_cast(max(0, vehicle_min_x_ - front_area_extend_)); + size_t front_area_max_x = static_cast(vehicle_min_x_ - 1); + size_t front_area_min_y = static_cast(max(0, vehicle_min_y_ - left_area_extend_)); + size_t front_area_max_y = static_cast(min(static_cast(grid[0].size() - 1), + vehicle_max_y_ + right_area_extend_)); + // RCLCPP_INFO_STREAM(this->get_logger(), + // "绕障检测区域： 前后=" << front_area_max_x - front_area_min_x + // << ", 左右=" << front_area_max_y - front_area_min_y); + + for (size_t i = front_area_min_x; i <= front_area_max_x; ++i) + { + for (size_t j = front_area_min_y; j <= front_area_max_y; ++j) + { + if (grid[i][j] == OBSTACLE) + { + obstacle_in_front_area_ = true; + + // 更新障碍物边缘 + if (obstacle_left_edge_ == -1 || static_cast(j) < obstacle_left_edge_) + obstacle_left_edge_ = j; + + if (obstacle_right_edge_ == -1 || static_cast(j) > obstacle_right_edge_) + obstacle_right_edge_ = j; + + // 更新最近障碍物X坐标 + int distance = vehicle_min_x_ - static_cast(i); + if (distance < obstacle_max_x_) + { + obstacle_max_x_ = distance; + } + } + } + } + + // 计算左右可用空间 + if (obstacle_in_front_area_) + { + free_space_left_ = obstacle_left_edge_ - front_area_min_y; + free_space_right_ = front_area_max_y - obstacle_right_edge_; + } + + // 如果没有检测到前方障碍物，设置为-1表示无前方障碍物 + if (obstacle_max_x_ == 100 && !obstacle_in_front_ && !obstacle_in_front_area_) + { + obstacle_max_x_ = -1; // -1表示无前方障碍物 + } + } + + void visualizeGridInTerminal(const std::vector> &grid) + { + std::system("clear"); + + std::cout << " --------------------障碍物矩阵-------------------- " << std::endl; + + // 打印顶部边框 + std::cout << " " << std::string(grid[0].size(), '-') << std::endl; + + // 打印列号（仅0-9） + std::cout << " "; + for (size_t i = 0; i < grid[0].size(); i++) + { + std::cout << (i % 10); + } + std::cout << std::endl; + + // 打印栅格内容（行号 + 栅格） + for (size_t i = 0; i < grid.size(); i++) + { + // 行号显示（两位数，右对齐） + std::cout << (i < 10 ? " " : "") << i << "|"; + + // 栅格内容 + for (size_t j = 0; j < grid[i].size(); j++) + { + // 检查当前位置是否在某个必停区内 + bool in_front_zone = (i >= static_cast(front_stop_zone_.min_x) && + i <= static_cast(front_stop_zone_.max_x) && + j >= static_cast(front_stop_zone_.min_y) && + j <= static_cast(front_stop_zone_.max_y)); + + bool in_left_zone = (i >= static_cast(left_stop_zone_.min_x) && + i <= static_cast(left_stop_zone_.max_x) && + j >= static_cast(left_stop_zone_.min_y) && + j <= static_cast(left_stop_zone_.max_y)); + + bool in_right_zone = (i >= static_cast(right_stop_zone_.min_x) && + i <= static_cast(right_stop_zone_.max_x) && + j >= static_cast(right_stop_zone_.min_y) && + j <= static_cast(right_stop_zone_.max_y)); + + // 根据栅格值和必停区状态选择显示符号 + if (grid[i][j] == OBSTACLE) + std::cout << "@"; + else if (grid[i][j] == VEHICLE) + std::cout << "V"; + else if (in_front_zone) + std::cout << "F"; // 前方必停区 + else if (in_left_zone) + std::cout << "L"; // 左方必停区 + else if (in_right_zone) + std::cout << "R"; // 右方必停区 + else + std::cout << "."; + } + + std::cout << "|" << std::endl; + } + + // 打印底部边框 + std::cout << " " << std::string(grid[0].size(), '-') << std::endl; + } + + void msg_callback_pl(const sweeper_interfaces::msg::Pl::SharedPtr msg) + { + x = msg->x; + y = msg->y; + speed = (int)msg->speed; // 获取PL消息中的速度 + reliability = msg->reliability; + located = msg->enter_range; + is_start = msg->is_start; + } + + void msg_callback_DetectLine(const sweeper_interfaces::msg::DetectLine::SharedPtr msg) + { + nl_adjust = 0 - msg->adjust + remain_adjust; + detected_line = msg->detected_line; + } + + void timer_callback() + { + if (speed == 0) + { + publish_speed = 0; + publish_angle = 0; + RCLCPP_INFO(this->get_logger(), "PL速度为0，已停车"); + } + else if (is_start) + { + // 障碍物检测与避障决策 + applyObstacleAvoidance(); + + // 发布控制指令 + sweeper_interfaces::msg::McCtrl message; + bool sweep = (avoid_ctx_.state == AVOID_NONE); // 避障状态为无避障 且 任务开始 才开启清扫 + message.edge_brush_lift = sweep; + message.sweep_ctrl = sweep; + message.spray = sweep; + message.mud_flap = sweep; + message.dust_shake = sweep; + message.gear = 0; + message.brake = (publish_speed == 0) ? 1 : 0; + message.rpm = publish_speed; + message.angle = publish_angle; // 负数表示左转，正数表示右转 + message.angle_speed = 800; + + pub_mc->publish(message); + + RCLCPP_INFO_STREAM(this->get_logger(), "控制: 速度=" << publish_speed << ", 角度=" << publish_angle << ", 状态=" << [&]() + { + switch(avoid_state_){ + case AVOID_NONE: return "无避障"; + case AVOID_WAITING: return "等待决策"; + case AVOID_LEFT: return "向左绕障"; + case AVOID_RIGHT: return "向右绕障"; + case AVOID_RETURNING: return "返回原路径"; + default: return "未知状态"; + } }() << ", 前方" << (obstacle_in_front_ ? "有" : "无") << "障碍物"); + } + else + { + // 未启动状态，停车 + publish_speed = 0; + publish_angle = 0; + } + } + + // 避障决策与控制 + void applyObstacleAvoidance() + { + // 计算基础目标角度 + float target_angle = calculate_the_angle(x, y); + + // 初始化控制参数 + publish_speed = std::max(15, std::min(speed, 30)); // 限制速度范围 + publish_angle = static_cast(target_angle); + + // 安全检查：未检测到车体位置 + if (vehicle_min_x_ == -1) + { + publish_speed = 0; + publish_angle = 0; + avoid_ctx_.reset(); + RCLCPP_WARN(this->get_logger(), "未检测到车体位置，安全停车"); + return; + } + + // 更新障碍物分析 + updateObstacleAnalysis(); + + // 执行分层避障策略 + if (!executeEmergencyStop()) + { + if (!executeTurnProtection()) + { + executeSmartAvoidance(); + } + } + + // 记录角度历史 + avoid_ctx_.addAngleHistory(publish_angle); + } + + void updateObstacleAnalysis() + { + obstacle_analysis_ = ObstacleAnalysis{}; // 重置 + + obstacle_analysis_.has_front_critical = obstacle_in_front_; + obstacle_analysis_.has_front_area = obstacle_in_front_area_; + obstacle_analysis_.has_left_critical = obstacle_in_left_; + obstacle_analysis_.has_right_critical = obstacle_in_right_; + + obstacle_analysis_.obstacle_distance = (obstacle_max_x_ > 0) ? obstacle_max_x_ : -1; + obstacle_analysis_.left_edge = obstacle_left_edge_; + obstacle_analysis_.right_edge = obstacle_right_edge_; + obstacle_analysis_.free_space_left = free_space_left_; + obstacle_analysis_.free_space_right = free_space_right_; + + if (obstacle_left_edge_ != -1 && obstacle_right_edge_ != -1) + { + obstacle_analysis_.obstacle_width = obstacle_right_edge_ - obstacle_left_edge_ + 1; + } + } + + // 第一优先级：紧急停车策略 + bool executeEmergencyStop() + { + if (!enable_obstacle_stop_) + return false; + + // 前方必停区有障碍物，立即停车 + if (obstacle_analysis_.has_front_critical) + { + publish_speed = 0; + publish_angle = 0; + avoid_ctx_.state = AVOID_WAITING; + avoid_ctx_.counter++; + + // 每2秒打印一次状态 + if (avoid_ctx_.counter % 20 == 0) + { + RCLCPP_WARN(this->get_logger(), "紧急停车：前方必停区有障碍物，距离=%d格", + obstacle_analysis_.obstacle_distance); + } + return true; + } + + // 动态紧急制动：基于速度和距离 + if (obstacle_analysis_.needEmergencyBrake(publish_speed)) + { + publish_speed = 0; + publish_angle = 0; + avoid_ctx_.state = AVOID_WAITING; + RCLCPP_WARN(this->get_logger(), "动态紧急制动：障碍物距离%d格，速度%d", + obstacle_analysis_.obstacle_distance, publish_speed); + return true; + } + + return false; + } + + // 第二优先级：转向保护策略 + bool executeTurnProtection() + { + if (!enable_obstacle_stop_) + return false; + + // 检查转向方向的安全性 + if (publish_angle < -20 && obstacle_analysis_.has_left_critical) + { + publish_speed = 0; + publish_angle = 0; + RCLCPP_WARN(this->get_logger(), "转向保护：左转时左侧有障碍物"); + return true; + } + + if (publish_angle > 20 && obstacle_analysis_.has_right_critical) + { + publish_speed = 0; + publish_angle = 0; + RCLCPP_WARN(this->get_logger(), "转向保护：右转时右侧有障碍物"); + return true; + } + + return false; + } + + // 第三优先级：智能绕障策略 + void executeSmartAvoidance() + { + if (!enable_obstacle_avoid_ || !obstacle_analysis_.has_front_area) + { + // 无障碍物或绕障功能关闭，执行路径回正 + if (avoid_ctx_.state != AVOID_NONE) + { + executePathReturn(); + } + return; + } + + // 评估是否需要绕障 + if (!obstacle_analysis_.canAvoid()) + { + // 无法绕障，等待或减速 + handleUnpassableObstacle(); + return; + } + + // 状态机处理 + switch (avoid_ctx_.state) + { + case AVOID_NONE: + case AVOID_WAITING: + initiateAvoidance(); + break; + + case AVOID_LEFT: + case AVOID_RIGHT: + executeActiveAvoidance(); + break; + + case AVOID_RETURNING: + executePathReturn(); + break; + } + } + + // 处理无法绕行的障碍物 + void handleUnpassableObstacle() + { + int safe_distance = obstacle_analysis_.calculateSafeDistance(publish_speed); + + if (obstacle_analysis_.obstacle_distance <= safe_distance) + { + // 太近，必须停车 + publish_speed = 0; + avoid_ctx_.state = AVOID_WAITING; + avoid_ctx_.counter++; + + if (avoid_ctx_.counter % 30 == 0) + { // 每3秒打印一次 + RCLCPP_INFO(this->get_logger(), "障碍物无法绕行(宽度=%d格)，等待通过...", + obstacle_analysis_.obstacle_width); + } + } + else + { + // 距离足够，可以减速接近 + publish_speed = std::max(10, publish_speed / 2); + RCLCPP_INFO(this->get_logger(), "障碍物较宽，减速接近中，距离=%d格", + obstacle_analysis_.obstacle_distance); + } + } + + // 启动绕障 + void initiateAvoidance() + { + auto current_time = this->get_clock()->now(); + + // 选择最优绕障方向 + int direction = selectOptimalDirection(); + + avoid_ctx_.state = (direction == -1) ? AVOID_LEFT : AVOID_RIGHT; + avoid_ctx_.direction = direction; + avoid_ctx_.counter = 0; + avoid_ctx_.start_time = current_time; + avoid_ctx_.last_state_change_time = current_time; + avoid_ctx_.original_angle = static_cast(calculate_the_angle(x, y)); + + RCLCPP_INFO(this->get_logger(), "启动绕障：方向=%s，障碍物宽度=%d，左侧空间=%d，右侧空间=%d", + (direction == -1) ? "左" : "右", + obstacle_analysis_.obstacle_width, + obstacle_analysis_.free_space_left, + obstacle_analysis_.free_space_right); + } + + // 智能方向选择算法 + int selectOptimalDirection() + { + int left_score = 0; + int right_score = 0; + + // 1. 可用空间评分 (权重最高) + left_score += obstacle_analysis_.free_space_left * 10; + right_score += obstacle_analysis_.free_space_right * 10; + + // 2. 必停区安全性评分 + if (obstacle_analysis_.has_left_critical) + left_score -= 100; + if (obstacle_analysis_.has_right_critical) + right_score -= 100; + + // 3. 目标方向偏好 + float target_angle = calculate_the_angle(x, y); + if (target_angle < -5) + left_score += 15; // 目标偏左 + if (target_angle > 5) + right_score += 15; // 目标偏右 + + // 4. 历史方向稳定性 - 避免频繁切换 + if (!avoid_ctx_.angle_history.empty()) + { + int recent_trend = 0; + int history_size = std::min(5, static_cast(avoid_ctx_.angle_history.size())); + + for (int i = avoid_ctx_.angle_history.size() - history_size; + i < static_cast(avoid_ctx_.angle_history.size()); i++) + { + recent_trend += avoid_ctx_.angle_history[i]; + } + recent_trend /= history_size; + + if (recent_trend < -10) + left_score += 8; // 最近偏左 + if (recent_trend > 10) + right_score += 8; // 最近偏右 + } + + // 5. 障碍物位置偏向性 + if (obstacle_analysis_.left_edge > -1 && obstacle_analysis_.right_edge > -1) + { + int vehicle_center = (vehicle_min_y_ + vehicle_max_y_) / 2; + int obstacle_center = (obstacle_analysis_.left_edge + obstacle_analysis_.right_edge) / 2; + + if (obstacle_center < vehicle_center) + { + right_score += 5; // 障碍物偏左，倾向于右绕 + } + else + { + left_score += 5; // 障碍物偏右，倾向于左绕 + } + } + + int selected_direction = (left_score > right_score) ? -1 : 1; + + RCLCPP_INFO(this->get_logger(), "方向选择评分：左=%d 右=%d，选择%s", + left_score, right_score, (selected_direction == -1) ? "左绕" : "右绕"); + + return selected_direction; + } + + // 执行主动绕障 + void executeActiveAvoidance() + { + auto current_time = this->get_clock()->now(); + double avoid_time = (current_time - avoid_ctx_.start_time).seconds(); + + // 动态速度调整 + publish_speed = calculateAvoidanceSpeed(avoid_time); + + // 自适应角度控制 + publish_angle = calculateAvoidanceAngle(avoid_time); + + // 绕障过程中的安全检查 + if (checkAvoidanceSafety()) + { + return; // 检查到危险，已经处理 + } + + // 判断是否应该结束绕障 + if (shouldEndAvoidance(avoid_time)) + { + avoid_ctx_.state = AVOID_RETURNING; + avoid_ctx_.last_state_change_time = current_time; + RCLCPP_INFO(this->get_logger(), "绕障完成，开始回正路径"); + } + } + + // 计算绕障速度 + int calculateAvoidanceSpeed(double avoid_time) + { + // 绕障初期降低速度 + if (avoid_time < 1.0) + { + return 12; // 起始阶段低速 + } + else if (avoid_time < 3.0) + { + return 18; // 中期稍微提速 + } + else + { + return 15; // 后期保持稳定 + } + } + + // 计算绕障角度 - 更平滑的角度变化 + int calculateAvoidanceAngle(double avoid_time) + { + // 使用平滑的角度增长曲线 + double progress = std::min(1.0, avoid_time / 2.0); // 2秒内达到最大角度 + double smooth_progress = 0.5 * (1 - cos(M_PI * progress)); // 正弦曲线，更平滑 + + int target_angle = static_cast(avoid_ctx_.max_avoid_angle * smooth_progress); + return avoid_ctx_.direction * target_angle; + } + + // 绕障安全检查 + bool checkAvoidanceSafety() + { + // 检查绕障方向是否出现新障碍物 + if (avoid_ctx_.direction == -1 && obstacle_analysis_.has_left_critical) + { + publish_speed = 0; + avoid_ctx_.state = AVOID_WAITING; + RCLCPP_ERROR(this->get_logger(), "左绕过程中左侧出现障碍物！"); + return true; + } + + if (avoid_ctx_.direction == 1 && obstacle_analysis_.has_right_critical) + { + publish_speed = 0; + avoid_ctx_.state = AVOID_WAITING; + RCLCPP_ERROR(this->get_logger(), "右绕过程中右侧出现障碍物！"); + return true; + } + + return false; + } + + // 判断是否应该结束绕障 + bool shouldEndAvoidance(double avoid_time) + { + // 基础时间检查 + if (avoid_time < avoid_ctx_.min_avoid_time) + { + return false; // 未达到最小绕障时间 + } + + // 强制超时保护 + if (avoid_time >= avoid_ctx_.max_avoid_time) + { + RCLCPP_WARN(this->get_logger(), "绕障时间过长(%.1fs)，强制结束", avoid_time); + return true; + } + + // 综合评估绕障完成条件 + AvoidanceCompletionAnalysis completion = analyzeAvoidanceCompletion(avoid_time); + + // 记录评估结果用于调试 + if (avoid_ctx_.counter % 10 == 0) + { // 每1秒打印一次 + RCLCPP_INFO(this->get_logger(), + "绕障评估 - 前方清空:%s, 侧向安全:%s, 角度合理:%s, 位置偏移:%s, 综合评分:%.1f", + completion.front_clear ? "✓" : "✗", + completion.lateral_safe ? "✓" : "✗", + completion.angle_reasonable ? "✓" : "✗", + completion.position_good ? "✓" : "✗", + completion.overall_score); + } + + // 决策逻辑：综合评分达到阈值或满足关键条件 + if (completion.overall_score >= 0.75) + { + RCLCPP_INFO(this->get_logger(), "绕障完成：综合评分%.2f达标", completion.overall_score); + return true; + } + + // 紧急情况：即使评分不够也要结束绕障 + if (completion.emergency_end) + { + RCLCPP_WARN(this->get_logger(), "紧急结束绕障：%s", completion.emergency_reason.c_str()); + return true; + } + + return false; + } + + // 绕障完成度分析结构 + struct AvoidanceCompletionAnalysis + { + bool front_clear = false; // 前方通道清晰 + bool lateral_safe = false; // 侧向安全 + bool angle_reasonable = false; // 当前角度合理 + bool position_good = false; // 位置偏移适中 + bool path_aligned = false; // 路径对齐良好 + bool stability_good = false; // 行驶稳定 + + bool emergency_end = false; // 紧急结束标志 + std::string emergency_reason; // 紧急结束原因 + + double overall_score = 0.0; // 综合评分 [0,1] + + // 各项权重 + static constexpr double WEIGHT_FRONT = 0.3; // 前方清空权重 + static constexpr double WEIGHT_LATERAL = 0.25; // 侧向安全权重 + static constexpr double WEIGHT_ANGLE = 0.2; // 角度合理权重 + static constexpr double WEIGHT_POSITION = 0.15; // 位置权重 + static constexpr double WEIGHT_PATH = 0.1; // 路径对齐权重 + }; + + // 绕障完成度综合分析 + AvoidanceCompletionAnalysis analyzeAvoidanceCompletion(double avoid_time) + { + AvoidanceCompletionAnalysis analysis; + + // 1. 前方通道清晰度分析 + analysis.front_clear = analyzeFrontClearance(); + + // 2. 侧向安全性分析 + analysis.lateral_safe = analyzeLateralSafety(); + + // 3. 当前角度合理性分析 + analysis.angle_reasonable = analyzeAngleReasonability(); + + // 4. 车辆位置偏移分析 + analysis.position_good = analyzePositionOffset(); + + // 5. 路径对齐度分析 + analysis.path_aligned = analyzePathAlignment(); + + // 6. 行驶稳定性分析 + analysis.stability_good = analyzeStability(); + + // 7. 紧急情况检查 + checkEmergencyConditions(analysis, avoid_time); + + // 8. 计算综合评分 + analysis.overall_score = calculateOverallScore(analysis); + + return analysis; + } + + // 方向性通道清晰度分析结构 + struct DirectionalClearance + { + bool can_return_safely = false; // 是否可以安全回正 + int clear_width = 0; // 清晰通道宽度 + int clear_depth = 0; // 清晰通道深度 + std::string analysis_detail; // 分析详情 + }; + + // 前方通道清晰度分析 + bool analyzeFrontClearance() + { + // 方案1：全局前方区域完全清空（最理想情况） + if (!obstacle_analysis_.has_front_area && obstacle_analysis_.obstacle_distance > 5) + { + return true; + } + + // 方案2：智能方向性清晰度检测 + // 如果全局前方有障碍物，但回正方向的通道清晰，也应该允许结束绕障 + DirectionalClearance clearance = analyzeDirectionalClearance(); + + if (clearance.can_return_safely) + { + RCLCPP_INFO(this->get_logger(), + "方向性通道分析：回正方向清晰，允许结束绕障 (清晰宽度:%d格)", + clearance.clear_width); + return true; + } + + return false; + } + + // 分析方向性通道清晰度 + DirectionalClearance analyzeDirectionalClearance() + { + DirectionalClearance result; + + if (vehicle_min_y_ == -1 || vehicle_max_y_ == -1) + { + result.analysis_detail = "无法确定车辆位置"; + return result; + } + + // 计算目标回正角度 + float target_angle = calculate_the_angle(x, y); + int target_angle_int = static_cast(target_angle); + + // 根据目标角度确定主要行驶方向 + // 目标角度：负数=左转，正数=右转，接近0=直行 + if (std::abs(target_angle_int) <= 10) + { + // 直行为主，检查车辆前方中央区域 + result = analyzeForwardClearance(); + } + else if (target_angle_int < -10) + { + // 需要左转，重点检查左前方区域 + result = analyzeLeftwardClearance(); + } + else + { + // 需要右转，重点检查右前方区域 + result = analyzeRightwardClearance(); + } + + // 补充安全检查：即使方向性通道清晰，也要确保有最小安全距离 + if (result.can_return_safely && result.clear_depth < 3) + { + result.can_return_safely = false; + result.analysis_detail += " (但安全距离不足)"; + } + + return result; + } + + // 分析正前方通道（直行场景） + DirectionalClearance analyzeForwardClearance() + { + DirectionalClearance result; + result.analysis_detail = "直行通道分析"; + + // 检查车辆正前方的通道 + int vehicle_center_y = (vehicle_min_y_ + vehicle_max_y_) / 2; + int vehicle_width = vehicle_max_y_ - vehicle_min_y_ + 1; + + // 定义正前方检测区域：车辆宽度 + 2格安全余量 + int check_left = vehicle_center_y - vehicle_width / 2 - 1; + int check_right = vehicle_center_y + vehicle_width / 2 + 1; + int check_depth = std::min(8, front_area_extend_ + 2); // 检查深度 + + result.clear_width = check_right - check_left + 1; + result.clear_depth = checkClearDepthInRangeAccurate(check_left, check_right, check_depth); + + // 如果正前方通道深度足够，认为可以安全直行 + result.can_return_safely = (result.clear_depth >= 4); + + return result; + } + + // 分析左前方通道（左转场景） + DirectionalClearance analyzeLeftwardClearance() + { + DirectionalClearance result; + result.analysis_detail = "左转通道分析"; + + // 重点检查左前方区域是否清晰 + int vehicle_left = vehicle_min_y_; + int check_left = std::max(0, vehicle_left - 2); // 车辆左侧4格范围 + int check_right = vehicle_left + 2; // 延伸到车辆中部 + int check_depth = std::min(6, front_area_extend_); + + result.clear_width = check_right - check_left + 1; + result.clear_depth = checkClearDepthInRangeAccurate(check_left, check_right, check_depth); + + // 左转场景：左前方清晰深度要求可以稍低一些 + result.can_return_safely = (result.clear_depth >= 3) && + !obstacle_analysis_.has_left_critical; + + return result; + } + + // 分析右前方通道（右转场景） + DirectionalClearance analyzeRightwardClearance() + { + DirectionalClearance result; + result.analysis_detail = "右转通道分析"; + + // 重点检查右前方区域是否清晰 + int vehicle_right = vehicle_max_y_; + int check_left = vehicle_right - 2; // 从车辆中部开始 + int check_right = vehicle_right + 2; // 车辆右侧4格范围 + int check_depth = std::min(6, front_area_extend_); + + result.clear_width = check_right - check_left + 1; + result.clear_depth = checkClearDepthInRangeAccurate(check_left, check_right, check_depth); + + // 右转场景：右前方清晰深度要求可以稍低一些 + result.can_return_safely = (result.clear_depth >= 3) && + !obstacle_analysis_.has_right_critical; + + return result; + } + + // 检查指定范围内的清晰深度 + int checkClearDepthInRange(int left_bound, int right_bound, int max_depth) + { + // 简化实现：基于现有的障碍物分析数据进行推断 + if (obstacle_analysis_.obstacle_distance <= 0) + { + return max_depth; // 无障碍物，认为完全清晰 + } + + // 如果有障碍物，检查障碍物是否在我们关心的范围内 + bool obstacle_in_range = false; + if (obstacle_analysis_.left_edge != -1 && obstacle_analysis_.right_edge != -1) + { + // 检查障碍物边缘是否与检测范围重叠 + obstacle_in_range = !(obstacle_analysis_.right_edge < left_bound || + obstacle_analysis_.left_edge > right_bound); + } + + if (!obstacle_in_range) + { + return max_depth; // 障碍物不在检测范围内，认为该方向清晰 + } + + // 障碍物在范围内，返回到障碍物的距离 + return std::max(0, obstacle_analysis_.obstacle_distance - 1); + } + + // 精确的范围清晰度检查（使用缓存的栅格数据） + int checkClearDepthInRangeAccurate(int left_bound, int right_bound, int max_depth) + { + if (!grid_data_valid_ || cached_grid_.empty()) + { + return checkClearDepthInRange(left_bound, right_bound, max_depth); // 回退到简化版本 + } + + // 从车辆前方开始向前检查 + for (int depth = 1; depth <= max_depth; depth++) + { + int check_row = vehicle_min_x_ - depth; + if (check_row < 0) + break; + + // 检查该行的指定列范围 + for (int col = left_bound; col <= right_bound; col++) + { + if (col < 0 || col >= static_cast(cached_grid_[0].size())) + continue; + + if (cached_grid_[check_row][col] == OBSTACLE) + { + return depth - 1; // 遇到障碍物，返回清晰深度 + } + } + } + + return max_depth; // 整个范围都清晰 + } + + // 侧向安全性分析 + bool analyzeLateralSafety() + { + // 基础安全：绕障方向无障碍物 + bool basic_safe = true; + if (avoid_ctx_.direction == -1 && obstacle_analysis_.has_left_critical) + { + basic_safe = false; + } + if (avoid_ctx_.direction == 1 && obstacle_analysis_.has_right_critical) + { + basic_safe = false; + } + + if (!basic_safe) + return false; + + // 进阶安全：有足够的侧向安全余量 + int required_clearance = 2; // 需要至少2格的安全间隙 - Claude ： 4格 + bool lateral_clearance_ok = false; + + if (avoid_ctx_.direction == -1) + { + lateral_clearance_ok = obstacle_analysis_.free_space_left >= required_clearance; + } + else + { + lateral_clearance_ok = obstacle_analysis_.free_space_right >= required_clearance; + } + + return lateral_clearance_ok; + } + + // 角度合理性分析 + bool analyzeAngleReasonability() + { + // 当前角度不应该过大 + int current_angle = publish_angle; + if (std::abs(current_angle) > avoid_ctx_.max_avoid_angle + 5) + { + return false; // 角度过大 + } + + // 计算理想回正角度 + float target_angle = calculate_the_angle(x, y); + int angle_diff = std::abs(current_angle - static_cast(target_angle)); + + // 角度差应该在合理范围内 + bool angle_diff_ok = angle_diff <= 25; // 允许25度的偏差 + + // 角度变化应该稳定（不剧烈摆动） + bool angle_stable = true; + if (avoid_ctx_.angle_history.size() >= 3) + { + int recent_changes = 0; + for (size_t i = avoid_ctx_.angle_history.size() - 3; + i < avoid_ctx_.angle_history.size() - 1; i++) + { + int change = std::abs(avoid_ctx_.angle_history[i + 1] - avoid_ctx_.angle_history[i]); + if (change > 10) + recent_changes++; + } + angle_stable = (recent_changes <= 1); // 最多允许1次大幅变化 + } + + return angle_diff_ok && angle_stable; + } + + // 位置偏移分析 + bool analyzePositionOffset() + { + // 分析车辆是否已经偏移到足够安全的位置 + // 这需要基于车辆在栅格中的位置来判断 + + if (vehicle_min_y_ == -1 || vehicle_max_y_ == -1) + { + return false; // 无法确定车辆位置 + } + + // 计算车辆中心位置 + int vehicle_center_y = (vehicle_min_y_ + vehicle_max_y_) / 2; + + // 如果有障碍物，检查车辆是否已经偏移到安全位置 + if (obstacle_analysis_.left_edge != -1 && obstacle_analysis_.right_edge != -1) + { + int obstacle_center = (obstacle_analysis_.left_edge + obstacle_analysis_.right_edge) / 2; + int lateral_offset = std::abs(vehicle_center_y - obstacle_center); + + // 侧向偏移应该足够大 + bool offset_sufficient = lateral_offset >= 5; // 至少5格的偏移 + + // 车辆应该在障碍物的安全一侧 + bool on_safe_side = false; + if (avoid_ctx_.direction == -1) + { + on_safe_side = vehicle_center_y < obstacle_analysis_.left_edge - 2; // 在障碍物左侧 + } + else + { + on_safe_side = vehicle_center_y > obstacle_analysis_.right_edge + 2; // 在障碍物右侧 + } + + return offset_sufficient && on_safe_side; + } + + return true; // 无障碍物时认为位置良好 + } + + // 路径对齐度分析 + bool analyzePathAlignment() + { + float target_angle = calculate_the_angle(x, y); + int current_angle = publish_angle; + + // 检查当前角度是否朝向目标方向 + bool heading_towards_target = std::abs(current_angle - target_angle) <= 30; + + // 检查是否过度绕行（偏离目标太远） + bool not_over_avoiding = std::abs(current_angle) <= 35; + + return heading_towards_target && not_over_avoiding; + } + + // 行驶稳定性分析 + bool analyzeStability() + { + // 检查最近的角度变化是否稳定 + if (avoid_ctx_.angle_history.size() < 5) + { + return true; // 历史数据不足，假设稳定 + } + + // 计算最近5次角度的标准差 + double sum = 0; + int count = std::min(5, static_cast(avoid_ctx_.angle_history.size())); + for (int i = avoid_ctx_.angle_history.size() - count; + i < static_cast(avoid_ctx_.angle_history.size()); i++) + { + sum += avoid_ctx_.angle_history[i]; + } + double mean = sum / count; + + double variance = 0; + for (int i = avoid_ctx_.angle_history.size() - count; + i < static_cast(avoid_ctx_.angle_history.size()); i++) + { + variance += (avoid_ctx_.angle_history[i] - mean) * (avoid_ctx_.angle_history[i] - mean); + } + variance /= count; + double std_dev = sqrt(variance); + + // 标准差小于10度认为稳定 + return std_dev <= 10.0; + } + + // 紧急情况检查 + void checkEmergencyConditions(AvoidanceCompletionAnalysis &analysis, double avoid_time) + { + // 紧急情况1：绕障方向出现新障碍物 + if ((avoid_ctx_.direction == -1 && obstacle_analysis_.has_left_critical) || + (avoid_ctx_.direction == 1 && obstacle_analysis_.has_right_critical)) + { + analysis.emergency_end = true; + analysis.emergency_reason = "绕障方向出现障碍物"; + return; + } + + // 紧急情况2：前方突然出现更危险的障碍物 + if (obstacle_analysis_.has_front_critical && + obstacle_analysis_.obstacle_distance <= 2) + { + analysis.emergency_end = true; + analysis.emergency_reason = "前方出现危险障碍物"; + return; + } + + // 紧急情况3：车辆位置异常（可能传感器故障） + if (vehicle_min_x_ == -1 || vehicle_max_x_ == -1) + { + analysis.emergency_end = true; + analysis.emergency_reason = "车辆位置检测异常"; + return; + } + + // 紧急情况4：角度异常（可能失控） + if (std::abs(publish_angle) > 50) + { + analysis.emergency_end = true; + analysis.emergency_reason = "转向角度异常过大"; + return; + } + + // 紧急情况5：绕障过程中路径目标丢失 + if (x == 0 && y == 0 && avoid_time > 2.0) + { + analysis.emergency_end = true; + analysis.emergency_reason = "路径目标信号丢失"; + return; + } + } + + // 计算综合评分 + double calculateOverallScore(const AvoidanceCompletionAnalysis &analysis) + { + double score = 0.0; + + if (analysis.front_clear) + score += AvoidanceCompletionAnalysis::WEIGHT_FRONT; + if (analysis.lateral_safe) + score += AvoidanceCompletionAnalysis::WEIGHT_LATERAL; + if (analysis.angle_reasonable) + score += AvoidanceCompletionAnalysis::WEIGHT_ANGLE; + if (analysis.position_good) + score += AvoidanceCompletionAnalysis::WEIGHT_POSITION; + if (analysis.path_aligned) + score += AvoidanceCompletionAnalysis::WEIGHT_PATH; + + // 稳定性作为加分项 + if (analysis.stability_good) + score += 0.05; + + return std::min(1.0, score); // 确保不超过1.0 + } + + // 改进的路径回正策略 + void executePathReturn() + { + auto current_time = this->get_clock()->now(); + double return_time = (current_time - avoid_ctx_.last_state_change_time).seconds(); + + // 恢复正常速度 + publish_speed = std::min(25, speed); + + // 计算目标角度 + float target_angle = calculate_the_angle(x, y); + int target_angle_int = static_cast(target_angle); + + // 转向过程中的安全检查 + if (checkTurnSafety(target_angle_int)) + { + return; // 检查到危险，已处理 + } + + // 平滑回正到目标角度 + int angle_diff = target_angle_int - publish_angle; + int return_rate = calculateReturnRate(return_time, angle_diff); + + if (std::abs(angle_diff) <= return_rate) + { + // 已接近目标角度，完成回正 + publish_angle = target_angle_int; + avoid_ctx_.reset(); + publish_speed = speed; // 恢复原始速度 + RCLCPP_INFO(this->get_logger(), "路径回正完成，恢复正常行驶"); + } + else + { + // 逐步调整到目标角度 + if (angle_diff > 0) + { + publish_angle += return_rate; + } + else + { + publish_angle -= return_rate; + } + } + } + + // 计算回正速率 + int calculateReturnRate(double return_time, int angle_diff) + { + // 基础回正速率 + int base_rate = 3; + + // 角度差越大，回正越快 + if (std::abs(angle_diff) > 30) + { + base_rate = 6; + } + else if (std::abs(angle_diff) > 15) + { + base_rate = 4; + } + + // 时间越长，回正越快 + if (return_time > 3.0) + { + base_rate += 2; + } + + return base_rate; + } + + // 转向安全检查 + bool checkTurnSafety(int target_angle) + { + if (target_angle < -20 && obstacle_analysis_.has_left_critical) + { + publish_speed = 0; + RCLCPP_WARN(this->get_logger(), "回正过程中检测到左侧障碍物"); + return true; + } + + if (target_angle > 20 && obstacle_analysis_.has_right_critical) + { + publish_speed = 0; + RCLCPP_WARN(this->get_logger(), "回正过程中检测到右侧障碍物"); + return true; + } + + return false; + } +}; + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared("fu_node"); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/src/fu/src/jsoncpp.cpp b/src/fu/src/jsoncpp.cpp new file mode 100644 index 0000000..89b7bc0 --- /dev/null +++ b/src/fu/src/jsoncpp.cpp @@ -0,0 +1,5342 @@ +/// Json-cpp amalgamated source (http://jsoncpp.sourceforge.net/). +/// It is intended to be used with #include "json/json.h" + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: LICENSE +// ////////////////////////////////////////////////////////////////////// + +/* +The JsonCpp library's source code, including accompanying documentation, +tests and demonstration applications, are licensed under the following +conditions... + +Baptiste Lepilleur and The JsonCpp Authors explicitly disclaim copyright in all +jurisdictions which recognize such a disclaimer. In such jurisdictions, +this software is released into the Public Domain. + +In jurisdictions which do not recognize Public Domain property (e.g. Germany as of +2010), this software is Copyright (c) 2007-2010 by Baptiste Lepilleur and +The JsonCpp Authors, and is released under the terms of the MIT License (see below). + +In jurisdictions which recognize Public Domain property, the user of this +software may choose to accept it either as 1) Public Domain, 2) under the +conditions of the MIT License (see below), or 3) under the terms of dual +Public Domain/MIT License conditions described here, as they choose. + +The MIT License is about as close to Public Domain as a license can get, and is +described in clear, concise terms at: + + http://en.wikipedia.org/wiki/MIT_License + +The full text of the MIT License follows: + +======================================================================== +Copyright (c) 2007-2010 Baptiste Lepilleur and The JsonCpp Authors + +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. +======================================================================== +(END LICENSE TEXT) + +The MIT license is compatible with both the GPL and commercial +software, affording one all of the rights of Public Domain with the +minor nuisance of being required to keep the above copyright notice +and license text in the source code. Note also that by accepting the +Public Domain "license" you can re-license your copy using whatever +license you like. + +*/ + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: LICENSE +// ////////////////////////////////////////////////////////////////////// + + + + + + +#include "json.h" + +#ifndef JSON_IS_AMALGAMATION +#error "Compile with -I PATH_TO_JSON_DIRECTORY" +#endif + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: src/lib_json/json_tool.h +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2007-2010 Baptiste Lepilleur and The JsonCpp Authors +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +#ifndef LIB_JSONCPP_JSON_TOOL_H_INCLUDED +#define LIB_JSONCPP_JSON_TOOL_H_INCLUDED + +#if !defined(JSON_IS_AMALGAMATION) +#include +#endif + +// Also support old flag NO_LOCALE_SUPPORT +#ifdef NO_LOCALE_SUPPORT +#define JSONCPP_NO_LOCALE_SUPPORT +#endif + +#ifndef JSONCPP_NO_LOCALE_SUPPORT +#include +#endif + +/* This header provides common string manipulation support, such as UTF-8, + * portable conversion from/to string... + * + * It is an internal header that must not be exposed. + */ + +namespace Json { +static inline char getDecimalPoint() { +#ifdef JSONCPP_NO_LOCALE_SUPPORT + return '\0'; +#else + struct lconv* lc = localeconv(); + return lc ? *(lc->decimal_point) : '\0'; +#endif +} + +/// Converts a unicode code-point to UTF-8. +static inline String codePointToUTF8(unsigned int cp) { + String result; + + // based on description from http://en.wikipedia.org/wiki/UTF-8 + + if (cp <= 0x7f) { + result.resize(1); + result[0] = static_cast(cp); + } else if (cp <= 0x7FF) { + result.resize(2); + result[1] = static_cast(0x80 | (0x3f & cp)); + result[0] = static_cast(0xC0 | (0x1f & (cp >> 6))); + } else if (cp <= 0xFFFF) { + result.resize(3); + result[2] = static_cast(0x80 | (0x3f & cp)); + result[1] = static_cast(0x80 | (0x3f & (cp >> 6))); + result[0] = static_cast(0xE0 | (0xf & (cp >> 12))); + } else if (cp <= 0x10FFFF) { + result.resize(4); + result[3] = static_cast(0x80 | (0x3f & cp)); + result[2] = static_cast(0x80 | (0x3f & (cp >> 6))); + result[1] = static_cast(0x80 | (0x3f & (cp >> 12))); + result[0] = static_cast(0xF0 | (0x7 & (cp >> 18))); + } + + return result; +} + +enum { + /// Constant that specify the size of the buffer that must be passed to + /// uintToString. + uintToStringBufferSize = 3 * sizeof(LargestUInt) + 1 +}; + +// Defines a char buffer for use with uintToString(). +using UIntToStringBuffer = char[uintToStringBufferSize]; + +/** Converts an unsigned integer to string. + * @param value Unsigned integer to convert to string + * @param current Input/Output string buffer. + * Must have at least uintToStringBufferSize chars free. + */ +static inline void uintToString(LargestUInt value, char*& current) { + *--current = 0; + do { + *--current = static_cast(value % 10U + static_cast('0')); + value /= 10; + } while (value != 0); +} + +/** Change ',' to '.' everywhere in buffer. + * + * We had a sophisticated way, but it did not work in WinCE. + * @see https://github.com/open-source-parsers/jsoncpp/pull/9 + */ +template Iter fixNumericLocale(Iter begin, Iter end) { + for (; begin != end; ++begin) { + if (*begin == ',') { + *begin = '.'; + } + } + return begin; +} + +template void fixNumericLocaleInput(Iter begin, Iter end) { + char decimalPoint = getDecimalPoint(); + if (decimalPoint == '\0' || decimalPoint == '.') { + return; + } + for (; begin != end; ++begin) { + if (*begin == '.') { + *begin = decimalPoint; + } + } +} + +/** + * Return iterator that would be the new end of the range [begin,end), if we + * were to delete zeros in the end of string, but not the last zero before '.'. + */ +template +Iter fixZerosInTheEnd(Iter begin, Iter end, unsigned int precision) { + for (; begin != end; --end) { + if (*(end - 1) != '0') { + return end; + } + // Don't delete the last zero before the decimal point. + if (begin != (end - 1) && begin != (end - 2) && *(end - 2) == '.') { + if (precision) { + return end; + } + return end - 2; + } + } + return end; +} + +} // namespace Json + +#endif // LIB_JSONCPP_JSON_TOOL_H_INCLUDED + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: src/lib_json/json_tool.h +// ////////////////////////////////////////////////////////////////////// + + + + + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: src/lib_json/json_reader.cpp +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2007-2011 Baptiste Lepilleur and The JsonCpp Authors +// Copyright (C) 2016 InfoTeCS JSC. All rights reserved. +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +#if !defined(JSON_IS_AMALGAMATION) +#include "json_tool.h" +#include +#include +#include +#endif // if !defined(JSON_IS_AMALGAMATION) +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#if __cplusplus >= 201103L + +#if !defined(sscanf) +#define sscanf std::sscanf +#endif + +#endif //__cplusplus + +#if defined(_MSC_VER) +#if !defined(_CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES) +#define _CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES 1 +#endif //_CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES +#endif //_MSC_VER + +#if defined(_MSC_VER) +// Disable warning about strdup being deprecated. +#pragma warning(disable : 4996) +#endif + +// Define JSONCPP_DEPRECATED_STACK_LIMIT as an appropriate integer at compile +// time to change the stack limit +#if !defined(JSONCPP_DEPRECATED_STACK_LIMIT) +#define JSONCPP_DEPRECATED_STACK_LIMIT 1000 +#endif + +static size_t const stackLimit_g = + JSONCPP_DEPRECATED_STACK_LIMIT; // see readValue() + +namespace Json { + +#if __cplusplus >= 201103L || (defined(_CPPLIB_VER) && _CPPLIB_VER >= 520) +using CharReaderPtr = std::unique_ptr; +#else +using CharReaderPtr = std::auto_ptr; +#endif + +// Implementation of class Features +// //////////////////////////////// + +Features::Features() = default; + +Features Features::all() { return {}; } + +Features Features::strictMode() { + Features features; + features.allowComments_ = false; + features.strictRoot_ = true; + features.allowDroppedNullPlaceholders_ = false; + features.allowNumericKeys_ = false; + return features; +} + +// Implementation of class Reader +// //////////////////////////////// + +bool Reader::containsNewLine(Reader::Location begin, Reader::Location end) { + return std::any_of(begin, end, [](char b) { return b == '\n' || b == '\r'; }); +} + +// Class Reader +// ////////////////////////////////////////////////////////////////// + +Reader::Reader() : features_(Features::all()) {} + +Reader::Reader(const Features& features) : features_(features) {} + +bool Reader::parse(const std::string& document, Value& root, + bool collectComments) { + document_.assign(document.begin(), document.end()); + const char* begin = document_.c_str(); + const char* end = begin + document_.length(); + return parse(begin, end, root, collectComments); +} + +bool Reader::parse(std::istream& is, Value& root, bool collectComments) { + // std::istream_iterator begin(is); + // std::istream_iterator end; + // Those would allow streamed input from a file, if parse() were a + // template function. + + // Since String is reference-counted, this at least does not + // create an extra copy. + String doc(std::istreambuf_iterator(is), {}); + return parse(doc.data(), doc.data() + doc.size(), root, collectComments); +} + +bool Reader::parse(const char* beginDoc, const char* endDoc, Value& root, + bool collectComments) { + if (!features_.allowComments_) { + collectComments = false; + } + + begin_ = beginDoc; + end_ = endDoc; + collectComments_ = collectComments; + current_ = begin_; + lastValueEnd_ = nullptr; + lastValue_ = nullptr; + commentsBefore_.clear(); + errors_.clear(); + while (!nodes_.empty()) + nodes_.pop(); + nodes_.push(&root); + + bool successful = readValue(); + Token token; + skipCommentTokens(token); + if (collectComments_ && !commentsBefore_.empty()) + root.setComment(commentsBefore_, commentAfter); + if (features_.strictRoot_) { + if (!root.isArray() && !root.isObject()) { + // Set error location to start of doc, ideally should be first token found + // in doc + token.type_ = tokenError; + token.start_ = beginDoc; + token.end_ = endDoc; + addError( + "A valid JSON document must be either an array or an object value.", + token); + return false; + } + } + return successful; +} + +bool Reader::readValue() { + // readValue() may call itself only if it calls readObject() or ReadArray(). + // These methods execute nodes_.push() just before and nodes_.pop)() just + // after calling readValue(). parse() executes one nodes_.push(), so > instead + // of >=. + if (nodes_.size() > stackLimit_g) + throwRuntimeError("Exceeded stackLimit in readValue()."); + + Token token; + skipCommentTokens(token); + bool successful = true; + + if (collectComments_ && !commentsBefore_.empty()) { + currentValue().setComment(commentsBefore_, commentBefore); + commentsBefore_.clear(); + } + + switch (token.type_) { + case tokenObjectBegin: + successful = readObject(token); + currentValue().setOffsetLimit(current_ - begin_); + break; + case tokenArrayBegin: + successful = readArray(token); + currentValue().setOffsetLimit(current_ - begin_); + break; + case tokenNumber: + successful = decodeNumber(token); + break; + case tokenString: + successful = decodeString(token); + break; + case tokenTrue: { + Value v(true); + currentValue().swapPayload(v); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + } break; + case tokenFalse: { + Value v(false); + currentValue().swapPayload(v); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + } break; + case tokenNull: { + Value v; + currentValue().swapPayload(v); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + } break; + case tokenArraySeparator: + case tokenObjectEnd: + case tokenArrayEnd: + if (features_.allowDroppedNullPlaceholders_) { + // "Un-read" the current token and mark the current value as a null + // token. + current_--; + Value v; + currentValue().swapPayload(v); + currentValue().setOffsetStart(current_ - begin_ - 1); + currentValue().setOffsetLimit(current_ - begin_); + break; + } // Else, fall through... + default: + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + return addError("Syntax error: value, object or array expected.", token); + } + + if (collectComments_) { + lastValueEnd_ = current_; + lastValue_ = ¤tValue(); + } + + return successful; +} + +void Reader::skipCommentTokens(Token& token) { + if (features_.allowComments_) { + do { + readToken(token); + } while (token.type_ == tokenComment); + } else { + readToken(token); + } +} + +bool Reader::readToken(Token& token) { + skipSpaces(); + token.start_ = current_; + Char c = getNextChar(); + bool ok = true; + switch (c) { + case '{': + token.type_ = tokenObjectBegin; + break; + case '}': + token.type_ = tokenObjectEnd; + break; + case '[': + token.type_ = tokenArrayBegin; + break; + case ']': + token.type_ = tokenArrayEnd; + break; + case '"': + token.type_ = tokenString; + ok = readString(); + break; + case '/': + token.type_ = tokenComment; + ok = readComment(); + break; + case '0': + case '1': + case '2': + case '3': + case '4': + case '5': + case '6': + case '7': + case '8': + case '9': + case '-': + token.type_ = tokenNumber; + readNumber(); + break; + case 't': + token.type_ = tokenTrue; + ok = match("rue", 3); + break; + case 'f': + token.type_ = tokenFalse; + ok = match("alse", 4); + break; + case 'n': + token.type_ = tokenNull; + ok = match("ull", 3); + break; + case ',': + token.type_ = tokenArraySeparator; + break; + case ':': + token.type_ = tokenMemberSeparator; + break; + case 0: + token.type_ = tokenEndOfStream; + break; + default: + ok = false; + break; + } + if (!ok) + token.type_ = tokenError; + token.end_ = current_; + return ok; +} + +void Reader::skipSpaces() { + while (current_ != end_) { + Char c = *current_; + if (c == ' ' || c == '\t' || c == '\r' || c == '\n') + ++current_; + else + break; + } +} + +bool Reader::match(const Char* pattern, int patternLength) { + if (end_ - current_ < patternLength) + return false; + int index = patternLength; + while (index--) + if (current_[index] != pattern[index]) + return false; + current_ += patternLength; + return true; +} + +bool Reader::readComment() { + Location commentBegin = current_ - 1; + Char c = getNextChar(); + bool successful = false; + if (c == '*') + successful = readCStyleComment(); + else if (c == '/') + successful = readCppStyleComment(); + if (!successful) + return false; + + if (collectComments_) { + CommentPlacement placement = commentBefore; + if (lastValueEnd_ && !containsNewLine(lastValueEnd_, commentBegin)) { + if (c != '*' || !containsNewLine(commentBegin, current_)) + placement = commentAfterOnSameLine; + } + + addComment(commentBegin, current_, placement); + } + return true; +} + +String Reader::normalizeEOL(Reader::Location begin, Reader::Location end) { + String normalized; + normalized.reserve(static_cast(end - begin)); + Reader::Location current = begin; + while (current != end) { + char c = *current++; + if (c == '\r') { + if (current != end && *current == '\n') + // convert dos EOL + ++current; + // convert Mac EOL + normalized += '\n'; + } else { + normalized += c; + } + } + return normalized; +} + +void Reader::addComment(Location begin, Location end, + CommentPlacement placement) { + assert(collectComments_); + const String& normalized = normalizeEOL(begin, end); + if (placement == commentAfterOnSameLine) { + assert(lastValue_ != nullptr); + lastValue_->setComment(normalized, placement); + } else { + commentsBefore_ += normalized; + } +} + +bool Reader::readCStyleComment() { + while ((current_ + 1) < end_) { + Char c = getNextChar(); + if (c == '*' && *current_ == '/') + break; + } + return getNextChar() == '/'; +} + +bool Reader::readCppStyleComment() { + while (current_ != end_) { + Char c = getNextChar(); + if (c == '\n') + break; + if (c == '\r') { + // Consume DOS EOL. It will be normalized in addComment. + if (current_ != end_ && *current_ == '\n') + getNextChar(); + // Break on Moc OS 9 EOL. + break; + } + } + return true; +} + +void Reader::readNumber() { + Location p = current_; + char c = '0'; // stopgap for already consumed character + // integral part + while (c >= '0' && c <= '9') + c = (current_ = p) < end_ ? *p++ : '\0'; + // fractional part + if (c == '.') { + c = (current_ = p) < end_ ? *p++ : '\0'; + while (c >= '0' && c <= '9') + c = (current_ = p) < end_ ? *p++ : '\0'; + } + // exponential part + if (c == 'e' || c == 'E') { + c = (current_ = p) < end_ ? *p++ : '\0'; + if (c == '+' || c == '-') + c = (current_ = p) < end_ ? *p++ : '\0'; + while (c >= '0' && c <= '9') + c = (current_ = p) < end_ ? *p++ : '\0'; + } +} + +bool Reader::readString() { + Char c = '\0'; + while (current_ != end_) { + c = getNextChar(); + if (c == '\\') + getNextChar(); + else if (c == '"') + break; + } + return c == '"'; +} + +bool Reader::readObject(Token& token) { + Token tokenName; + String name; + Value init(objectValue); + currentValue().swapPayload(init); + currentValue().setOffsetStart(token.start_ - begin_); + while (readToken(tokenName)) { + bool initialTokenOk = true; + while (tokenName.type_ == tokenComment && initialTokenOk) + initialTokenOk = readToken(tokenName); + if (!initialTokenOk) + break; + if (tokenName.type_ == tokenObjectEnd && name.empty()) // empty object + return true; + name.clear(); + if (tokenName.type_ == tokenString) { + if (!decodeString(tokenName, name)) + return recoverFromError(tokenObjectEnd); + } else if (tokenName.type_ == tokenNumber && features_.allowNumericKeys_) { + Value numberName; + if (!decodeNumber(tokenName, numberName)) + return recoverFromError(tokenObjectEnd); + name = numberName.asString(); + } else { + break; + } + + Token colon; + if (!readToken(colon) || colon.type_ != tokenMemberSeparator) { + return addErrorAndRecover("Missing ':' after object member name", colon, + tokenObjectEnd); + } + Value& value = currentValue()[name]; + nodes_.push(&value); + bool ok = readValue(); + nodes_.pop(); + if (!ok) // error already set + return recoverFromError(tokenObjectEnd); + + Token comma; + if (!readToken(comma) || + (comma.type_ != tokenObjectEnd && comma.type_ != tokenArraySeparator && + comma.type_ != tokenComment)) { + return addErrorAndRecover("Missing ',' or '}' in object declaration", + comma, tokenObjectEnd); + } + bool finalizeTokenOk = true; + while (comma.type_ == tokenComment && finalizeTokenOk) + finalizeTokenOk = readToken(comma); + if (comma.type_ == tokenObjectEnd) + return true; + } + return addErrorAndRecover("Missing '}' or object member name", tokenName, + tokenObjectEnd); +} + +bool Reader::readArray(Token& token) { + Value init(arrayValue); + currentValue().swapPayload(init); + currentValue().setOffsetStart(token.start_ - begin_); + skipSpaces(); + if (current_ != end_ && *current_ == ']') // empty array + { + Token endArray; + readToken(endArray); + return true; + } + int index = 0; + for (;;) { + Value& value = currentValue()[index++]; + nodes_.push(&value); + bool ok = readValue(); + nodes_.pop(); + if (!ok) // error already set + return recoverFromError(tokenArrayEnd); + + Token currentToken; + // Accept Comment after last item in the array. + ok = readToken(currentToken); + while (currentToken.type_ == tokenComment && ok) { + ok = readToken(currentToken); + } + bool badTokenType = (currentToken.type_ != tokenArraySeparator && + currentToken.type_ != tokenArrayEnd); + if (!ok || badTokenType) { + return addErrorAndRecover("Missing ',' or ']' in array declaration", + currentToken, tokenArrayEnd); + } + if (currentToken.type_ == tokenArrayEnd) + break; + } + return true; +} + +bool Reader::decodeNumber(Token& token) { + Value decoded; + if (!decodeNumber(token, decoded)) + return false; + currentValue().swapPayload(decoded); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + return true; +} + +bool Reader::decodeNumber(Token& token, Value& decoded) { + // Attempts to parse the number as an integer. If the number is + // larger than the maximum supported value of an integer then + // we decode the number as a double. + Location current = token.start_; + bool isNegative = *current == '-'; + if (isNegative) + ++current; + // TODO: Help the compiler do the div and mod at compile time or get rid of + // them. + Value::LargestUInt maxIntegerValue = + isNegative ? Value::LargestUInt(Value::maxLargestInt) + 1 + : Value::maxLargestUInt; + Value::LargestUInt threshold = maxIntegerValue / 10; + Value::LargestUInt value = 0; + while (current < token.end_) { + Char c = *current++; + if (c < '0' || c > '9') + return decodeDouble(token, decoded); + auto digit(static_cast(c - '0')); + if (value >= threshold) { + // We've hit or exceeded the max value divided by 10 (rounded down). If + // a) we've only just touched the limit, b) this is the last digit, and + // c) it's small enough to fit in that rounding delta, we're okay. + // Otherwise treat this number as a double to avoid overflow. + if (value > threshold || current != token.end_ || + digit > maxIntegerValue % 10) { + return decodeDouble(token, decoded); + } + } + value = value * 10 + digit; + } + if (isNegative && value == maxIntegerValue) + decoded = Value::minLargestInt; + else if (isNegative) + decoded = -Value::LargestInt(value); + else if (value <= Value::LargestUInt(Value::maxInt)) + decoded = Value::LargestInt(value); + else + decoded = value; + return true; +} + +bool Reader::decodeDouble(Token& token) { + Value decoded; + if (!decodeDouble(token, decoded)) + return false; + currentValue().swapPayload(decoded); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + return true; +} + +bool Reader::decodeDouble(Token& token, Value& decoded) { + double value = 0; + String buffer(token.start_, token.end_); + IStringStream is(buffer); + if (!(is >> value)) { + if (value == std::numeric_limits::max()) + value = std::numeric_limits::infinity(); + else if (value == std::numeric_limits::lowest()) + value = -std::numeric_limits::infinity(); + else if (!std::isinf(value)) + return addError( + "'" + String(token.start_, token.end_) + "' is not a number.", token); + } + decoded = value; + return true; +} + +bool Reader::decodeString(Token& token) { + String decoded_string; + if (!decodeString(token, decoded_string)) + return false; + Value decoded(decoded_string); + currentValue().swapPayload(decoded); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + return true; +} + +bool Reader::decodeString(Token& token, String& decoded) { + decoded.reserve(static_cast(token.end_ - token.start_ - 2)); + Location current = token.start_ + 1; // skip '"' + Location end = token.end_ - 1; // do not include '"' + while (current != end) { + Char c = *current++; + if (c == '"') + break; + if (c == '\\') { + if (current == end) + return addError("Empty escape sequence in string", token, current); + Char escape = *current++; + switch (escape) { + case '"': + decoded += '"'; + break; + case '/': + decoded += '/'; + break; + case '\\': + decoded += '\\'; + break; + case 'b': + decoded += '\b'; + break; + case 'f': + decoded += '\f'; + break; + case 'n': + decoded += '\n'; + break; + case 'r': + decoded += '\r'; + break; + case 't': + decoded += '\t'; + break; + case 'u': { + unsigned int unicode; + if (!decodeUnicodeCodePoint(token, current, end, unicode)) + return false; + decoded += codePointToUTF8(unicode); + } break; + default: + return addError("Bad escape sequence in string", token, current); + } + } else { + decoded += c; + } + } + return true; +} + +bool Reader::decodeUnicodeCodePoint(Token& token, Location& current, + Location end, unsigned int& unicode) { + + if (!decodeUnicodeEscapeSequence(token, current, end, unicode)) + return false; + if (unicode >= 0xD800 && unicode <= 0xDBFF) { + // surrogate pairs + if (end - current < 6) + return addError( + "additional six characters expected to parse unicode surrogate pair.", + token, current); + if (*(current++) == '\\' && *(current++) == 'u') { + unsigned int surrogatePair; + if (decodeUnicodeEscapeSequence(token, current, end, surrogatePair)) { + unicode = 0x10000 + ((unicode & 0x3FF) << 10) + (surrogatePair & 0x3FF); + } else + return false; + } else + return addError("expecting another \\u token to begin the second half of " + "a unicode surrogate pair", + token, current); + } + return true; +} + +bool Reader::decodeUnicodeEscapeSequence(Token& token, Location& current, + Location end, + unsigned int& ret_unicode) { + if (end - current < 4) + return addError( + "Bad unicode escape sequence in string: four digits expected.", token, + current); + int unicode = 0; + for (int index = 0; index < 4; ++index) { + Char c = *current++; + unicode *= 16; + if (c >= '0' && c <= '9') + unicode += c - '0'; + else if (c >= 'a' && c <= 'f') + unicode += c - 'a' + 10; + else if (c >= 'A' && c <= 'F') + unicode += c - 'A' + 10; + else + return addError( + "Bad unicode escape sequence in string: hexadecimal digit expected.", + token, current); + } + ret_unicode = static_cast(unicode); + return true; +} + +bool Reader::addError(const String& message, Token& token, Location extra) { + ErrorInfo info; + info.token_ = token; + info.message_ = message; + info.extra_ = extra; + errors_.push_back(info); + return false; +} + +bool Reader::recoverFromError(TokenType skipUntilToken) { + size_t const errorCount = errors_.size(); + Token skip; + for (;;) { + if (!readToken(skip)) + errors_.resize(errorCount); // discard errors caused by recovery + if (skip.type_ == skipUntilToken || skip.type_ == tokenEndOfStream) + break; + } + errors_.resize(errorCount); + return false; +} + +bool Reader::addErrorAndRecover(const String& message, Token& token, + TokenType skipUntilToken) { + addError(message, token); + return recoverFromError(skipUntilToken); +} + +Value& Reader::currentValue() { return *(nodes_.top()); } + +Reader::Char Reader::getNextChar() { + if (current_ == end_) + return 0; + return *current_++; +} + +void Reader::getLocationLineAndColumn(Location location, int& line, + int& column) const { + Location current = begin_; + Location lastLineStart = current; + line = 0; + while (current < location && current != end_) { + Char c = *current++; + if (c == '\r') { + if (*current == '\n') + ++current; + lastLineStart = current; + ++line; + } else if (c == '\n') { + lastLineStart = current; + ++line; + } + } + // column & line start at 1 + column = int(location - lastLineStart) + 1; + ++line; +} + +String Reader::getLocationLineAndColumn(Location location) const { + int line, column; + getLocationLineAndColumn(location, line, column); + char buffer[18 + 16 + 16 + 1]; + jsoncpp_snprintf(buffer, sizeof(buffer), "Line %d, Column %d", line, column); + return buffer; +} + +// Deprecated. Preserved for backward compatibility +String Reader::getFormatedErrorMessages() const { + return getFormattedErrorMessages(); +} + +String Reader::getFormattedErrorMessages() const { + String formattedMessage; + for (const auto& error : errors_) { + formattedMessage += + "* " + getLocationLineAndColumn(error.token_.start_) + "\n"; + formattedMessage += " " + error.message_ + "\n"; + if (error.extra_) + formattedMessage += + "See " + getLocationLineAndColumn(error.extra_) + " for detail.\n"; + } + return formattedMessage; +} + +std::vector Reader::getStructuredErrors() const { + std::vector allErrors; + for (const auto& error : errors_) { + Reader::StructuredError structured; + structured.offset_start = error.token_.start_ - begin_; + structured.offset_limit = error.token_.end_ - begin_; + structured.message = error.message_; + allErrors.push_back(structured); + } + return allErrors; +} + +bool Reader::pushError(const Value& value, const String& message) { + ptrdiff_t const length = end_ - begin_; + if (value.getOffsetStart() > length || value.getOffsetLimit() > length) + return false; + Token token; + token.type_ = tokenError; + token.start_ = begin_ + value.getOffsetStart(); + token.end_ = begin_ + value.getOffsetLimit(); + ErrorInfo info; + info.token_ = token; + info.message_ = message; + info.extra_ = nullptr; + errors_.push_back(info); + return true; +} + +bool Reader::pushError(const Value& value, const String& message, + const Value& extra) { + ptrdiff_t const length = end_ - begin_; + if (value.getOffsetStart() > length || value.getOffsetLimit() > length || + extra.getOffsetLimit() > length) + return false; + Token token; + token.type_ = tokenError; + token.start_ = begin_ + value.getOffsetStart(); + token.end_ = begin_ + value.getOffsetLimit(); + ErrorInfo info; + info.token_ = token; + info.message_ = message; + info.extra_ = begin_ + extra.getOffsetStart(); + errors_.push_back(info); + return true; +} + +bool Reader::good() const { return errors_.empty(); } + +// Originally copied from the Features class (now deprecated), used internally +// for features implementation. +class OurFeatures { +public: + static OurFeatures all(); + bool allowComments_; + bool allowTrailingCommas_; + bool strictRoot_; + bool allowDroppedNullPlaceholders_; + bool allowNumericKeys_; + bool allowSingleQuotes_; + bool failIfExtra_; + bool rejectDupKeys_; + bool allowSpecialFloats_; + bool skipBom_; + size_t stackLimit_; +}; // OurFeatures + +OurFeatures OurFeatures::all() { return {}; } + +// Implementation of class Reader +// //////////////////////////////// + +// Originally copied from the Reader class (now deprecated), used internally +// for implementing JSON reading. +class OurReader { +public: + using Char = char; + using Location = const Char*; + struct StructuredError { + ptrdiff_t offset_start; + ptrdiff_t offset_limit; + String message; + }; + + explicit OurReader(OurFeatures const& features); + bool parse(const char* beginDoc, const char* endDoc, Value& root, + bool collectComments = true); + String getFormattedErrorMessages() const; + std::vector getStructuredErrors() const; + +private: + OurReader(OurReader const&); // no impl + void operator=(OurReader const&); // no impl + + enum TokenType { + tokenEndOfStream = 0, + tokenObjectBegin, + tokenObjectEnd, + tokenArrayBegin, + tokenArrayEnd, + tokenString, + tokenNumber, + tokenTrue, + tokenFalse, + tokenNull, + tokenNaN, + tokenPosInf, + tokenNegInf, + tokenArraySeparator, + tokenMemberSeparator, + tokenComment, + tokenError + }; + + class Token { + public: + TokenType type_; + Location start_; + Location end_; + }; + + class ErrorInfo { + public: + Token token_; + String message_; + Location extra_; + }; + + using Errors = std::deque; + + bool readToken(Token& token); + void skipSpaces(); + void skipBom(bool skipBom); + bool match(const Char* pattern, int patternLength); + bool readComment(); + bool readCStyleComment(bool* containsNewLineResult); + bool readCppStyleComment(); + bool readString(); + bool readStringSingleQuote(); + bool readNumber(bool checkInf); + bool readValue(); + bool readObject(Token& token); + bool readArray(Token& token); + bool decodeNumber(Token& token); + bool decodeNumber(Token& token, Value& decoded); + bool decodeString(Token& token); + bool decodeString(Token& token, String& decoded); + bool decodeDouble(Token& token); + bool decodeDouble(Token& token, Value& decoded); + bool decodeUnicodeCodePoint(Token& token, Location& current, Location end, + unsigned int& unicode); + bool decodeUnicodeEscapeSequence(Token& token, Location& current, + Location end, unsigned int& unicode); + bool addError(const String& message, Token& token, Location extra = nullptr); + bool recoverFromError(TokenType skipUntilToken); + bool addErrorAndRecover(const String& message, Token& token, + TokenType skipUntilToken); + void skipUntilSpace(); + Value& currentValue(); + Char getNextChar(); + void getLocationLineAndColumn(Location location, int& line, + int& column) const; + String getLocationLineAndColumn(Location location) const; + void addComment(Location begin, Location end, CommentPlacement placement); + void skipCommentTokens(Token& token); + + static String normalizeEOL(Location begin, Location end); + static bool containsNewLine(Location begin, Location end); + + using Nodes = std::stack; + + Nodes nodes_{}; + Errors errors_{}; + String document_{}; + Location begin_ = nullptr; + Location end_ = nullptr; + Location current_ = nullptr; + Location lastValueEnd_ = nullptr; + Value* lastValue_ = nullptr; + bool lastValueHasAComment_ = false; + String commentsBefore_{}; + + OurFeatures const features_; + bool collectComments_ = false; +}; // OurReader + +// complete copy of Read impl, for OurReader + +bool OurReader::containsNewLine(OurReader::Location begin, + OurReader::Location end) { + return std::any_of(begin, end, [](char b) { return b == '\n' || b == '\r'; }); +} + +OurReader::OurReader(OurFeatures const& features) : features_(features) {} + +bool OurReader::parse(const char* beginDoc, const char* endDoc, Value& root, + bool collectComments) { + if (!features_.allowComments_) { + collectComments = false; + } + + begin_ = beginDoc; + end_ = endDoc; + collectComments_ = collectComments; + current_ = begin_; + lastValueEnd_ = nullptr; + lastValue_ = nullptr; + commentsBefore_.clear(); + errors_.clear(); + while (!nodes_.empty()) + nodes_.pop(); + nodes_.push(&root); + + // skip byte order mark if it exists at the beginning of the UTF-8 text. + skipBom(features_.skipBom_); + bool successful = readValue(); + nodes_.pop(); + Token token; + skipCommentTokens(token); + if (features_.failIfExtra_ && (token.type_ != tokenEndOfStream)) { + addError("Extra non-whitespace after JSON value.", token); + return false; + } + if (collectComments_ && !commentsBefore_.empty()) + root.setComment(commentsBefore_, commentAfter); + if (features_.strictRoot_) { + if (!root.isArray() && !root.isObject()) { + // Set error location to start of doc, ideally should be first token found + // in doc + token.type_ = tokenError; + token.start_ = beginDoc; + token.end_ = endDoc; + addError( + "A valid JSON document must be either an array or an object value.", + token); + return false; + } + } + return successful; +} + +bool OurReader::readValue() { + // To preserve the old behaviour we cast size_t to int. + if (nodes_.size() > features_.stackLimit_) + throwRuntimeError("Exceeded stackLimit in readValue()."); + Token token; + skipCommentTokens(token); + bool successful = true; + + if (collectComments_ && !commentsBefore_.empty()) { + currentValue().setComment(commentsBefore_, commentBefore); + commentsBefore_.clear(); + } + + switch (token.type_) { + case tokenObjectBegin: + successful = readObject(token); + currentValue().setOffsetLimit(current_ - begin_); + break; + case tokenArrayBegin: + successful = readArray(token); + currentValue().setOffsetLimit(current_ - begin_); + break; + case tokenNumber: + successful = decodeNumber(token); + break; + case tokenString: + successful = decodeString(token); + break; + case tokenTrue: { + Value v(true); + currentValue().swapPayload(v); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + } break; + case tokenFalse: { + Value v(false); + currentValue().swapPayload(v); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + } break; + case tokenNull: { + Value v; + currentValue().swapPayload(v); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + } break; + case tokenNaN: { + Value v(std::numeric_limits::quiet_NaN()); + currentValue().swapPayload(v); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + } break; + case tokenPosInf: { + Value v(std::numeric_limits::infinity()); + currentValue().swapPayload(v); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + } break; + case tokenNegInf: { + Value v(-std::numeric_limits::infinity()); + currentValue().swapPayload(v); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + } break; + case tokenArraySeparator: + case tokenObjectEnd: + case tokenArrayEnd: + if (features_.allowDroppedNullPlaceholders_) { + // "Un-read" the current token and mark the current value as a null + // token. + current_--; + Value v; + currentValue().swapPayload(v); + currentValue().setOffsetStart(current_ - begin_ - 1); + currentValue().setOffsetLimit(current_ - begin_); + break; + } // else, fall through ... + default: + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + return addError("Syntax error: value, object or array expected.", token); + } + + if (collectComments_) { + lastValueEnd_ = current_; + lastValueHasAComment_ = false; + lastValue_ = ¤tValue(); + } + + return successful; +} + +void OurReader::skipCommentTokens(Token& token) { + if (features_.allowComments_) { + do { + readToken(token); + } while (token.type_ == tokenComment); + } else { + readToken(token); + } +} + +bool OurReader::readToken(Token& token) { + skipSpaces(); + token.start_ = current_; + Char c = getNextChar(); + bool ok = true; + switch (c) { + case '{': + token.type_ = tokenObjectBegin; + break; + case '}': + token.type_ = tokenObjectEnd; + break; + case '[': + token.type_ = tokenArrayBegin; + break; + case ']': + token.type_ = tokenArrayEnd; + break; + case '"': + token.type_ = tokenString; + ok = readString(); + break; + case '\'': + if (features_.allowSingleQuotes_) { + token.type_ = tokenString; + ok = readStringSingleQuote(); + } else { + // If we don't allow single quotes, this is a failure case. + ok = false; + } + break; + case '/': + token.type_ = tokenComment; + ok = readComment(); + break; + case '0': + case '1': + case '2': + case '3': + case '4': + case '5': + case '6': + case '7': + case '8': + case '9': + token.type_ = tokenNumber; + readNumber(false); + break; + case '-': + if (readNumber(true)) { + token.type_ = tokenNumber; + } else { + token.type_ = tokenNegInf; + ok = features_.allowSpecialFloats_ && match("nfinity", 7); + } + break; + case '+': + if (readNumber(true)) { + token.type_ = tokenNumber; + } else { + token.type_ = tokenPosInf; + ok = features_.allowSpecialFloats_ && match("nfinity", 7); + } + break; + case 't': + token.type_ = tokenTrue; + ok = match("rue", 3); + break; + case 'f': + token.type_ = tokenFalse; + ok = match("alse", 4); + break; + case 'n': + token.type_ = tokenNull; + ok = match("ull", 3); + break; + case 'N': + if (features_.allowSpecialFloats_) { + token.type_ = tokenNaN; + ok = match("aN", 2); + } else { + ok = false; + } + break; + case 'I': + if (features_.allowSpecialFloats_) { + token.type_ = tokenPosInf; + ok = match("nfinity", 7); + } else { + ok = false; + } + break; + case ',': + token.type_ = tokenArraySeparator; + break; + case ':': + token.type_ = tokenMemberSeparator; + break; + case 0: + token.type_ = tokenEndOfStream; + break; + default: + ok = false; + break; + } + if (!ok) + token.type_ = tokenError; + token.end_ = current_; + return ok; +} + +void OurReader::skipSpaces() { + while (current_ != end_) { + Char c = *current_; + if (c == ' ' || c == '\t' || c == '\r' || c == '\n') + ++current_; + else + break; + } +} + +void OurReader::skipBom(bool skipBom) { + // The default behavior is to skip BOM. + if (skipBom) { + if ((end_ - begin_) >= 3 && strncmp(begin_, "\xEF\xBB\xBF", 3) == 0) { + begin_ += 3; + current_ = begin_; + } + } +} + +bool OurReader::match(const Char* pattern, int patternLength) { + if (end_ - current_ < patternLength) + return false; + int index = patternLength; + while (index--) + if (current_[index] != pattern[index]) + return false; + current_ += patternLength; + return true; +} + +bool OurReader::readComment() { + const Location commentBegin = current_ - 1; + const Char c = getNextChar(); + bool successful = false; + bool cStyleWithEmbeddedNewline = false; + + const bool isCStyleComment = (c == '*'); + const bool isCppStyleComment = (c == '/'); + if (isCStyleComment) { + successful = readCStyleComment(&cStyleWithEmbeddedNewline); + } else if (isCppStyleComment) { + successful = readCppStyleComment(); + } + + if (!successful) + return false; + + if (collectComments_) { + CommentPlacement placement = commentBefore; + + if (!lastValueHasAComment_) { + if (lastValueEnd_ && !containsNewLine(lastValueEnd_, commentBegin)) { + if (isCppStyleComment || !cStyleWithEmbeddedNewline) { + placement = commentAfterOnSameLine; + lastValueHasAComment_ = true; + } + } + } + + addComment(commentBegin, current_, placement); + } + return true; +} + +String OurReader::normalizeEOL(OurReader::Location begin, + OurReader::Location end) { + String normalized; + normalized.reserve(static_cast(end - begin)); + OurReader::Location current = begin; + while (current != end) { + char c = *current++; + if (c == '\r') { + if (current != end && *current == '\n') + // convert dos EOL + ++current; + // convert Mac EOL + normalized += '\n'; + } else { + normalized += c; + } + } + return normalized; +} + +void OurReader::addComment(Location begin, Location end, + CommentPlacement placement) { + assert(collectComments_); + const String& normalized = normalizeEOL(begin, end); + if (placement == commentAfterOnSameLine) { + assert(lastValue_ != nullptr); + lastValue_->setComment(normalized, placement); + } else { + commentsBefore_ += normalized; + } +} + +bool OurReader::readCStyleComment(bool* containsNewLineResult) { + *containsNewLineResult = false; + + while ((current_ + 1) < end_) { + Char c = getNextChar(); + if (c == '*' && *current_ == '/') + break; + if (c == '\n') + *containsNewLineResult = true; + } + + return getNextChar() == '/'; +} + +bool OurReader::readCppStyleComment() { + while (current_ != end_) { + Char c = getNextChar(); + if (c == '\n') + break; + if (c == '\r') { + // Consume DOS EOL. It will be normalized in addComment. + if (current_ != end_ && *current_ == '\n') + getNextChar(); + // Break on Moc OS 9 EOL. + break; + } + } + return true; +} + +bool OurReader::readNumber(bool checkInf) { + Location p = current_; + if (checkInf && p != end_ && *p == 'I') { + current_ = ++p; + return false; + } + char c = '0'; // stopgap for already consumed character + // integral part + while (c >= '0' && c <= '9') + c = (current_ = p) < end_ ? *p++ : '\0'; + // fractional part + if (c == '.') { + c = (current_ = p) < end_ ? *p++ : '\0'; + while (c >= '0' && c <= '9') + c = (current_ = p) < end_ ? *p++ : '\0'; + } + // exponential part + if (c == 'e' || c == 'E') { + c = (current_ = p) < end_ ? *p++ : '\0'; + if (c == '+' || c == '-') + c = (current_ = p) < end_ ? *p++ : '\0'; + while (c >= '0' && c <= '9') + c = (current_ = p) < end_ ? *p++ : '\0'; + } + return true; +} +bool OurReader::readString() { + Char c = 0; + while (current_ != end_) { + c = getNextChar(); + if (c == '\\') + getNextChar(); + else if (c == '"') + break; + } + return c == '"'; +} + +bool OurReader::readStringSingleQuote() { + Char c = 0; + while (current_ != end_) { + c = getNextChar(); + if (c == '\\') + getNextChar(); + else if (c == '\'') + break; + } + return c == '\''; +} + +bool OurReader::readObject(Token& token) { + Token tokenName; + String name; + Value init(objectValue); + currentValue().swapPayload(init); + currentValue().setOffsetStart(token.start_ - begin_); + while (readToken(tokenName)) { + bool initialTokenOk = true; + while (tokenName.type_ == tokenComment && initialTokenOk) + initialTokenOk = readToken(tokenName); + if (!initialTokenOk) + break; + if (tokenName.type_ == tokenObjectEnd && + (name.empty() || + features_.allowTrailingCommas_)) // empty object or trailing comma + return true; + name.clear(); + if (tokenName.type_ == tokenString) { + if (!decodeString(tokenName, name)) + return recoverFromError(tokenObjectEnd); + } else if (tokenName.type_ == tokenNumber && features_.allowNumericKeys_) { + Value numberName; + if (!decodeNumber(tokenName, numberName)) + return recoverFromError(tokenObjectEnd); + name = numberName.asString(); + } else { + break; + } + if (name.length() >= (1U << 30)) + throwRuntimeError("keylength >= 2^30"); + if (features_.rejectDupKeys_ && currentValue().isMember(name)) { + String msg = "Duplicate key: '" + name + "'"; + return addErrorAndRecover(msg, tokenName, tokenObjectEnd); + } + + Token colon; + if (!readToken(colon) || colon.type_ != tokenMemberSeparator) { + return addErrorAndRecover("Missing ':' after object member name", colon, + tokenObjectEnd); + } + Value& value = currentValue()[name]; + nodes_.push(&value); + bool ok = readValue(); + nodes_.pop(); + if (!ok) // error already set + return recoverFromError(tokenObjectEnd); + + Token comma; + if (!readToken(comma) || + (comma.type_ != tokenObjectEnd && comma.type_ != tokenArraySeparator && + comma.type_ != tokenComment)) { + return addErrorAndRecover("Missing ',' or '}' in object declaration", + comma, tokenObjectEnd); + } + bool finalizeTokenOk = true; + while (comma.type_ == tokenComment && finalizeTokenOk) + finalizeTokenOk = readToken(comma); + if (comma.type_ == tokenObjectEnd) + return true; + } + return addErrorAndRecover("Missing '}' or object member name", tokenName, + tokenObjectEnd); +} + +bool OurReader::readArray(Token& token) { + Value init(arrayValue); + currentValue().swapPayload(init); + currentValue().setOffsetStart(token.start_ - begin_); + int index = 0; + for (;;) { + skipSpaces(); + if (current_ != end_ && *current_ == ']' && + (index == 0 || + (features_.allowTrailingCommas_ && + !features_.allowDroppedNullPlaceholders_))) // empty array or trailing + // comma + { + Token endArray; + readToken(endArray); + return true; + } + Value& value = currentValue()[index++]; + nodes_.push(&value); + bool ok = readValue(); + nodes_.pop(); + if (!ok) // error already set + return recoverFromError(tokenArrayEnd); + + Token currentToken; + // Accept Comment after last item in the array. + ok = readToken(currentToken); + while (currentToken.type_ == tokenComment && ok) { + ok = readToken(currentToken); + } + bool badTokenType = (currentToken.type_ != tokenArraySeparator && + currentToken.type_ != tokenArrayEnd); + if (!ok || badTokenType) { + return addErrorAndRecover("Missing ',' or ']' in array declaration", + currentToken, tokenArrayEnd); + } + if (currentToken.type_ == tokenArrayEnd) + break; + } + return true; +} + +bool OurReader::decodeNumber(Token& token) { + Value decoded; + if (!decodeNumber(token, decoded)) + return false; + currentValue().swapPayload(decoded); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + return true; +} + +bool OurReader::decodeNumber(Token& token, Value& decoded) { + // Attempts to parse the number as an integer. If the number is + // larger than the maximum supported value of an integer then + // we decode the number as a double. + Location current = token.start_; + const bool isNegative = *current == '-'; + if (isNegative) { + ++current; + } + + // We assume we can represent the largest and smallest integer types as + // unsigned integers with separate sign. This is only true if they can fit + // into an unsigned integer. + static_assert(Value::maxLargestInt <= Value::maxLargestUInt, + "Int must be smaller than UInt"); + + // We need to convert minLargestInt into a positive number. The easiest way + // to do this conversion is to assume our "threshold" value of minLargestInt + // divided by 10 can fit in maxLargestInt when absolute valued. This should + // be a safe assumption. + static_assert(Value::minLargestInt <= -Value::maxLargestInt, + "The absolute value of minLargestInt must be greater than or " + "equal to maxLargestInt"); + static_assert(Value::minLargestInt / 10 >= -Value::maxLargestInt, + "The absolute value of minLargestInt must be only 1 magnitude " + "larger than maxLargest Int"); + + static constexpr Value::LargestUInt positive_threshold = + Value::maxLargestUInt / 10; + static constexpr Value::UInt positive_last_digit = Value::maxLargestUInt % 10; + + // For the negative values, we have to be more careful. Since typically + // -Value::minLargestInt will cause an overflow, we first divide by 10 and + // then take the inverse. This assumes that minLargestInt is only a single + // power of 10 different in magnitude, which we check above. For the last + // digit, we take the modulus before negating for the same reason. + static constexpr auto negative_threshold = + Value::LargestUInt(-(Value::minLargestInt / 10)); + static constexpr auto negative_last_digit = + Value::UInt(-(Value::minLargestInt % 10)); + + const Value::LargestUInt threshold = + isNegative ? negative_threshold : positive_threshold; + const Value::UInt max_last_digit = + isNegative ? negative_last_digit : positive_last_digit; + + Value::LargestUInt value = 0; + while (current < token.end_) { + Char c = *current++; + if (c < '0' || c > '9') + return decodeDouble(token, decoded); + + const auto digit(static_cast(c - '0')); + if (value >= threshold) { + // We've hit or exceeded the max value divided by 10 (rounded down). If + // a) we've only just touched the limit, meaning value == threshold, + // b) this is the last digit, or + // c) it's small enough to fit in that rounding delta, we're okay. + // Otherwise treat this number as a double to avoid overflow. + if (value > threshold || current != token.end_ || + digit > max_last_digit) { + return decodeDouble(token, decoded); + } + } + value = value * 10 + digit; + } + + if (isNegative) { + // We use the same magnitude assumption here, just in case. + const auto last_digit = static_cast(value % 10); + decoded = -Value::LargestInt(value / 10) * 10 - last_digit; + } else if (value <= Value::LargestUInt(Value::maxLargestInt)) { + decoded = Value::LargestInt(value); + } else { + decoded = value; + } + + return true; +} + +bool OurReader::decodeDouble(Token& token) { + Value decoded; + if (!decodeDouble(token, decoded)) + return false; + currentValue().swapPayload(decoded); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + return true; +} + +bool OurReader::decodeDouble(Token& token, Value& decoded) { + double value = 0; + const String buffer(token.start_, token.end_); + IStringStream is(buffer); + if (!(is >> value)) { + if (value == std::numeric_limits::max()) + value = std::numeric_limits::infinity(); + else if (value == std::numeric_limits::lowest()) + value = -std::numeric_limits::infinity(); + else if (!std::isinf(value)) + return addError( + "'" + String(token.start_, token.end_) + "' is not a number.", token); + } + decoded = value; + return true; +} + +bool OurReader::decodeString(Token& token) { + String decoded_string; + if (!decodeString(token, decoded_string)) + return false; + Value decoded(decoded_string); + currentValue().swapPayload(decoded); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + return true; +} + +bool OurReader::decodeString(Token& token, String& decoded) { + decoded.reserve(static_cast(token.end_ - token.start_ - 2)); + Location current = token.start_ + 1; // skip '"' + Location end = token.end_ - 1; // do not include '"' + while (current != end) { + Char c = *current++; + if (c == '"') + break; + if (c == '\\') { + if (current == end) + return addError("Empty escape sequence in string", token, current); + Char escape = *current++; + switch (escape) { + case '"': + decoded += '"'; + break; + case '/': + decoded += '/'; + break; + case '\\': + decoded += '\\'; + break; + case 'b': + decoded += '\b'; + break; + case 'f': + decoded += '\f'; + break; + case 'n': + decoded += '\n'; + break; + case 'r': + decoded += '\r'; + break; + case 't': + decoded += '\t'; + break; + case 'u': { + unsigned int unicode; + if (!decodeUnicodeCodePoint(token, current, end, unicode)) + return false; + decoded += codePointToUTF8(unicode); + } break; + default: + return addError("Bad escape sequence in string", token, current); + } + } else { + decoded += c; + } + } + return true; +} + +bool OurReader::decodeUnicodeCodePoint(Token& token, Location& current, + Location end, unsigned int& unicode) { + + if (!decodeUnicodeEscapeSequence(token, current, end, unicode)) + return false; + if (unicode >= 0xD800 && unicode <= 0xDBFF) { + // surrogate pairs + if (end - current < 6) + return addError( + "additional six characters expected to parse unicode surrogate pair.", + token, current); + if (*(current++) == '\\' && *(current++) == 'u') { + unsigned int surrogatePair; + if (decodeUnicodeEscapeSequence(token, current, end, surrogatePair)) { + unicode = 0x10000 + ((unicode & 0x3FF) << 10) + (surrogatePair & 0x3FF); + } else + return false; + } else + return addError("expecting another \\u token to begin the second half of " + "a unicode surrogate pair", + token, current); + } + return true; +} + +bool OurReader::decodeUnicodeEscapeSequence(Token& token, Location& current, + Location end, + unsigned int& ret_unicode) { + if (end - current < 4) + return addError( + "Bad unicode escape sequence in string: four digits expected.", token, + current); + int unicode = 0; + for (int index = 0; index < 4; ++index) { + Char c = *current++; + unicode *= 16; + if (c >= '0' && c <= '9') + unicode += c - '0'; + else if (c >= 'a' && c <= 'f') + unicode += c - 'a' + 10; + else if (c >= 'A' && c <= 'F') + unicode += c - 'A' + 10; + else + return addError( + "Bad unicode escape sequence in string: hexadecimal digit expected.", + token, current); + } + ret_unicode = static_cast(unicode); + return true; +} + +bool OurReader::addError(const String& message, Token& token, Location extra) { + ErrorInfo info; + info.token_ = token; + info.message_ = message; + info.extra_ = extra; + errors_.push_back(info); + return false; +} + +bool OurReader::recoverFromError(TokenType skipUntilToken) { + size_t errorCount = errors_.size(); + Token skip; + for (;;) { + if (!readToken(skip)) + errors_.resize(errorCount); // discard errors caused by recovery + if (skip.type_ == skipUntilToken || skip.type_ == tokenEndOfStream) + break; + } + errors_.resize(errorCount); + return false; +} + +bool OurReader::addErrorAndRecover(const String& message, Token& token, + TokenType skipUntilToken) { + addError(message, token); + return recoverFromError(skipUntilToken); +} + +Value& OurReader::currentValue() { return *(nodes_.top()); } + +OurReader::Char OurReader::getNextChar() { + if (current_ == end_) + return 0; + return *current_++; +} + +void OurReader::getLocationLineAndColumn(Location location, int& line, + int& column) const { + Location current = begin_; + Location lastLineStart = current; + line = 0; + while (current < location && current != end_) { + Char c = *current++; + if (c == '\r') { + if (*current == '\n') + ++current; + lastLineStart = current; + ++line; + } else if (c == '\n') { + lastLineStart = current; + ++line; + } + } + // column & line start at 1 + column = int(location - lastLineStart) + 1; + ++line; +} + +String OurReader::getLocationLineAndColumn(Location location) const { + int line, column; + getLocationLineAndColumn(location, line, column); + char buffer[18 + 16 + 16 + 1]; + jsoncpp_snprintf(buffer, sizeof(buffer), "Line %d, Column %d", line, column); + return buffer; +} + +String OurReader::getFormattedErrorMessages() const { + String formattedMessage; + for (const auto& error : errors_) { + formattedMessage += + "* " + getLocationLineAndColumn(error.token_.start_) + "\n"; + formattedMessage += " " + error.message_ + "\n"; + if (error.extra_) + formattedMessage += + "See " + getLocationLineAndColumn(error.extra_) + " for detail.\n"; + } + return formattedMessage; +} + +std::vector OurReader::getStructuredErrors() const { + std::vector allErrors; + for (const auto& error : errors_) { + OurReader::StructuredError structured; + structured.offset_start = error.token_.start_ - begin_; + structured.offset_limit = error.token_.end_ - begin_; + structured.message = error.message_; + allErrors.push_back(structured); + } + return allErrors; +} + +class OurCharReader : public CharReader { + bool const collectComments_; + OurReader reader_; + +public: + OurCharReader(bool collectComments, OurFeatures const& features) + : collectComments_(collectComments), reader_(features) {} + bool parse(char const* beginDoc, char const* endDoc, Value* root, + String* errs) override { + bool ok = reader_.parse(beginDoc, endDoc, *root, collectComments_); + if (errs) { + *errs = reader_.getFormattedErrorMessages(); + } + return ok; + } +}; + +CharReaderBuilder::CharReaderBuilder() { setDefaults(&settings_); } +CharReaderBuilder::~CharReaderBuilder() = default; +CharReader* CharReaderBuilder::newCharReader() const { + bool collectComments = settings_["collectComments"].asBool(); + OurFeatures features = OurFeatures::all(); + features.allowComments_ = settings_["allowComments"].asBool(); + features.allowTrailingCommas_ = settings_["allowTrailingCommas"].asBool(); + features.strictRoot_ = settings_["strictRoot"].asBool(); + features.allowDroppedNullPlaceholders_ = + settings_["allowDroppedNullPlaceholders"].asBool(); + features.allowNumericKeys_ = settings_["allowNumericKeys"].asBool(); + features.allowSingleQuotes_ = settings_["allowSingleQuotes"].asBool(); + + // Stack limit is always a size_t, so we get this as an unsigned int + // regardless of it we have 64-bit integer support enabled. + features.stackLimit_ = static_cast(settings_["stackLimit"].asUInt()); + features.failIfExtra_ = settings_["failIfExtra"].asBool(); + features.rejectDupKeys_ = settings_["rejectDupKeys"].asBool(); + features.allowSpecialFloats_ = settings_["allowSpecialFloats"].asBool(); + features.skipBom_ = settings_["skipBom"].asBool(); + return new OurCharReader(collectComments, features); +} + +bool CharReaderBuilder::validate(Json::Value* invalid) const { + static const auto& valid_keys = *new std::set{ + "collectComments", + "allowComments", + "allowTrailingCommas", + "strictRoot", + "allowDroppedNullPlaceholders", + "allowNumericKeys", + "allowSingleQuotes", + "stackLimit", + "failIfExtra", + "rejectDupKeys", + "allowSpecialFloats", + "skipBom", + }; + for (auto si = settings_.begin(); si != settings_.end(); ++si) { + auto key = si.name(); + if (valid_keys.count(key)) + continue; + if (invalid) + (*invalid)[key] = *si; + else + return false; + } + return invalid ? invalid->empty() : true; +} + +Value& CharReaderBuilder::operator[](const String& key) { + return settings_[key]; +} +// static +void CharReaderBuilder::strictMode(Json::Value* settings) { + //! [CharReaderBuilderStrictMode] + (*settings)["allowComments"] = false; + (*settings)["allowTrailingCommas"] = false; + (*settings)["strictRoot"] = true; + (*settings)["allowDroppedNullPlaceholders"] = false; + (*settings)["allowNumericKeys"] = false; + (*settings)["allowSingleQuotes"] = false; + (*settings)["stackLimit"] = 1000; + (*settings)["failIfExtra"] = true; + (*settings)["rejectDupKeys"] = true; + (*settings)["allowSpecialFloats"] = false; + (*settings)["skipBom"] = true; + //! [CharReaderBuilderStrictMode] +} +// static +void CharReaderBuilder::setDefaults(Json::Value* settings) { + //! [CharReaderBuilderDefaults] + (*settings)["collectComments"] = true; + (*settings)["allowComments"] = true; + (*settings)["allowTrailingCommas"] = true; + (*settings)["strictRoot"] = false; + (*settings)["allowDroppedNullPlaceholders"] = false; + (*settings)["allowNumericKeys"] = false; + (*settings)["allowSingleQuotes"] = false; + (*settings)["stackLimit"] = 1000; + (*settings)["failIfExtra"] = false; + (*settings)["rejectDupKeys"] = false; + (*settings)["allowSpecialFloats"] = false; + (*settings)["skipBom"] = true; + //! [CharReaderBuilderDefaults] +} + +////////////////////////////////// +// global functions + +bool parseFromStream(CharReader::Factory const& fact, IStream& sin, Value* root, + String* errs) { + OStringStream ssin; + ssin << sin.rdbuf(); + String doc = ssin.str(); + char const* begin = doc.data(); + char const* end = begin + doc.size(); + // Note that we do not actually need a null-terminator. + CharReaderPtr const reader(fact.newCharReader()); + return reader->parse(begin, end, root, errs); +} + +IStream& operator>>(IStream& sin, Value& root) { + CharReaderBuilder b; + String errs; + bool ok = parseFromStream(b, sin, &root, &errs); + if (!ok) { + throwRuntimeError(errs); + } + return sin; +} + +} // namespace Json + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: src/lib_json/json_reader.cpp +// ////////////////////////////////////////////////////////////////////// + + + + + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: src/lib_json/json_valueiterator.inl +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2007-2010 Baptiste Lepilleur and The JsonCpp Authors +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +// included by json_value.cpp + +namespace Json { + +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// class ValueIteratorBase +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// + +ValueIteratorBase::ValueIteratorBase() : current_() {} + +ValueIteratorBase::ValueIteratorBase( + const Value::ObjectValues::iterator& current) + : current_(current), isNull_(false) {} + +Value& ValueIteratorBase::deref() { return current_->second; } +const Value& ValueIteratorBase::deref() const { return current_->second; } + +void ValueIteratorBase::increment() { ++current_; } + +void ValueIteratorBase::decrement() { --current_; } + +ValueIteratorBase::difference_type +ValueIteratorBase::computeDistance(const SelfType& other) const { + // Iterator for null value are initialized using the default + // constructor, which initialize current_ to the default + // std::map::iterator. As begin() and end() are two instance + // of the default std::map::iterator, they can not be compared. + // To allow this, we handle this comparison specifically. + if (isNull_ && other.isNull_) { + return 0; + } + + // Usage of std::distance is not portable (does not compile with Sun Studio 12 + // RogueWave STL, + // which is the one used by default). + // Using a portable hand-made version for non random iterator instead: + // return difference_type( std::distance( current_, other.current_ ) ); + difference_type myDistance = 0; + for (Value::ObjectValues::iterator it = current_; it != other.current_; + ++it) { + ++myDistance; + } + return myDistance; +} + +bool ValueIteratorBase::isEqual(const SelfType& other) const { + if (isNull_) { + return other.isNull_; + } + return current_ == other.current_; +} + +void ValueIteratorBase::copy(const SelfType& other) { + current_ = other.current_; + isNull_ = other.isNull_; +} + +Value ValueIteratorBase::key() const { + const Value::CZString czstring = (*current_).first; + if (czstring.data()) { + if (czstring.isStaticString()) + return Value(StaticString(czstring.data())); + return Value(czstring.data(), czstring.data() + czstring.length()); + } + return Value(czstring.index()); +} + +UInt ValueIteratorBase::index() const { + const Value::CZString czstring = (*current_).first; + if (!czstring.data()) + return czstring.index(); + return Value::UInt(-1); +} + +String ValueIteratorBase::name() const { + char const* keey; + char const* end; + keey = memberName(&end); + if (!keey) + return String(); + return String(keey, end); +} + +char const* ValueIteratorBase::memberName() const { + const char* cname = (*current_).first.data(); + return cname ? cname : ""; +} + +char const* ValueIteratorBase::memberName(char const** end) const { + const char* cname = (*current_).first.data(); + if (!cname) { + *end = nullptr; + return nullptr; + } + *end = cname + (*current_).first.length(); + return cname; +} + +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// class ValueConstIterator +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// + +ValueConstIterator::ValueConstIterator() = default; + +ValueConstIterator::ValueConstIterator( + const Value::ObjectValues::iterator& current) + : ValueIteratorBase(current) {} + +ValueConstIterator::ValueConstIterator(ValueIterator const& other) + : ValueIteratorBase(other) {} + +ValueConstIterator& ValueConstIterator:: +operator=(const ValueIteratorBase& other) { + copy(other); + return *this; +} + +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// class ValueIterator +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// + +ValueIterator::ValueIterator() = default; + +ValueIterator::ValueIterator(const Value::ObjectValues::iterator& current) + : ValueIteratorBase(current) {} + +ValueIterator::ValueIterator(const ValueConstIterator& other) + : ValueIteratorBase(other) { + throwRuntimeError("ConstIterator to Iterator should never be allowed."); +} + +ValueIterator::ValueIterator(const ValueIterator& other) = default; + +ValueIterator& ValueIterator::operator=(const SelfType& other) { + copy(other); + return *this; +} + +} // namespace Json + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: src/lib_json/json_valueiterator.inl +// ////////////////////////////////////////////////////////////////////// + + + + + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: src/lib_json/json_value.cpp +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2011 Baptiste Lepilleur and The JsonCpp Authors +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +#if !defined(JSON_IS_AMALGAMATION) +#include +#include +#include +#endif // if !defined(JSON_IS_AMALGAMATION) +#include +#include +#include +#include +#include +#include +#include +#include + +// Provide implementation equivalent of std::snprintf for older _MSC compilers +#if defined(_MSC_VER) && _MSC_VER < 1900 +#include +static int msvc_pre1900_c99_vsnprintf(char* outBuf, size_t size, + const char* format, va_list ap) { + int count = -1; + if (size != 0) + count = _vsnprintf_s(outBuf, size, _TRUNCATE, format, ap); + if (count == -1) + count = _vscprintf(format, ap); + return count; +} + +int JSON_API msvc_pre1900_c99_snprintf(char* outBuf, size_t size, + const char* format, ...) { + va_list ap; + va_start(ap, format); + const int count = msvc_pre1900_c99_vsnprintf(outBuf, size, format, ap); + va_end(ap); + return count; +} +#endif + +// Disable warning C4702 : unreachable code +#if defined(_MSC_VER) +#pragma warning(disable : 4702) +#endif + +#define JSON_ASSERT_UNREACHABLE assert(false) + +namespace Json { +template +static std::unique_ptr cloneUnique(const std::unique_ptr& p) { + std::unique_ptr r; + if (p) { + r = std::unique_ptr(new T(*p)); + } + return r; +} + +// This is a walkaround to avoid the static initialization of Value::null. +// kNull must be word-aligned to avoid crashing on ARM. We use an alignment of +// 8 (instead of 4) as a bit of future-proofing. +#if defined(__ARMEL__) +#define ALIGNAS(byte_alignment) __attribute__((aligned(byte_alignment))) +#else +#define ALIGNAS(byte_alignment) +#endif + +// static +Value const& Value::nullSingleton() { + static Value const nullStatic; + return nullStatic; +} + +#if JSON_USE_NULLREF +// for backwards compatibility, we'll leave these global references around, but +// DO NOT use them in JSONCPP library code any more! +// static +Value const& Value::null = Value::nullSingleton(); + +// static +Value const& Value::nullRef = Value::nullSingleton(); +#endif + +#if !defined(JSON_USE_INT64_DOUBLE_CONVERSION) +template +static inline bool InRange(double d, T min, U max) { + // The casts can lose precision, but we are looking only for + // an approximate range. Might fail on edge cases though. ~cdunn + return d >= static_cast(min) && d <= static_cast(max); +} +#else // if !defined(JSON_USE_INT64_DOUBLE_CONVERSION) +static inline double integerToDouble(Json::UInt64 value) { + return static_cast(Int64(value / 2)) * 2.0 + + static_cast(Int64(value & 1)); +} + +template static inline double integerToDouble(T value) { + return static_cast(value); +} + +template +static inline bool InRange(double d, T min, U max) { + return d >= integerToDouble(min) && d <= integerToDouble(max); +} +#endif // if !defined(JSON_USE_INT64_DOUBLE_CONVERSION) + +/** Duplicates the specified string value. + * @param value Pointer to the string to duplicate. Must be zero-terminated if + * length is "unknown". + * @param length Length of the value. if equals to unknown, then it will be + * computed using strlen(value). + * @return Pointer on the duplicate instance of string. + */ +static inline char* duplicateStringValue(const char* value, size_t length) { + // Avoid an integer overflow in the call to malloc below by limiting length + // to a sane value. + if (length >= static_cast(Value::maxInt)) + length = Value::maxInt - 1; + + auto newString = static_cast(malloc(length + 1)); + if (newString == nullptr) { + throwRuntimeError("in Json::Value::duplicateStringValue(): " + "Failed to allocate string value buffer"); + } + memcpy(newString, value, length); + newString[length] = 0; + return newString; +} + +/* Record the length as a prefix. + */ +static inline char* duplicateAndPrefixStringValue(const char* value, + unsigned int length) { + // Avoid an integer overflow in the call to malloc below by limiting length + // to a sane value. + JSON_ASSERT_MESSAGE(length <= static_cast(Value::maxInt) - + sizeof(unsigned) - 1U, + "in Json::Value::duplicateAndPrefixStringValue(): " + "length too big for prefixing"); + size_t actualLength = sizeof(length) + length + 1; + auto newString = static_cast(malloc(actualLength)); + if (newString == nullptr) { + throwRuntimeError("in Json::Value::duplicateAndPrefixStringValue(): " + "Failed to allocate string value buffer"); + } + *reinterpret_cast(newString) = length; + memcpy(newString + sizeof(unsigned), value, length); + newString[actualLength - 1U] = + 0; // to avoid buffer over-run accidents by users later + return newString; +} +inline static void decodePrefixedString(bool isPrefixed, char const* prefixed, + unsigned* length, char const** value) { + if (!isPrefixed) { + *length = static_cast(strlen(prefixed)); + *value = prefixed; + } else { + *length = *reinterpret_cast(prefixed); + *value = prefixed + sizeof(unsigned); + } +} +/** Free the string duplicated by + * duplicateStringValue()/duplicateAndPrefixStringValue(). + */ +#if JSONCPP_USING_SECURE_MEMORY +static inline void releasePrefixedStringValue(char* value) { + unsigned length = 0; + char const* valueDecoded; + decodePrefixedString(true, value, &length, &valueDecoded); + size_t const size = sizeof(unsigned) + length + 1U; + memset(value, 0, size); + free(value); +} +static inline void releaseStringValue(char* value, unsigned length) { + // length==0 => we allocated the strings memory + size_t size = (length == 0) ? strlen(value) : length; + memset(value, 0, size); + free(value); +} +#else // !JSONCPP_USING_SECURE_MEMORY +static inline void releasePrefixedStringValue(char* value) { free(value); } +static inline void releaseStringValue(char* value, unsigned) { free(value); } +#endif // JSONCPP_USING_SECURE_MEMORY + +} // namespace Json + +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ValueInternals... +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +#if !defined(JSON_IS_AMALGAMATION) + +#include "json_valueiterator.inl" +#endif // if !defined(JSON_IS_AMALGAMATION) + +namespace Json { + +#if JSON_USE_EXCEPTION +Exception::Exception(String msg) : msg_(std::move(msg)) {} +Exception::~Exception() noexcept = default; +char const* Exception::what() const noexcept { return msg_.c_str(); } +RuntimeError::RuntimeError(String const& msg) : Exception(msg) {} +LogicError::LogicError(String const& msg) : Exception(msg) {} +JSONCPP_NORETURN void throwRuntimeError(String const& msg) { + throw RuntimeError(msg); +} +JSONCPP_NORETURN void throwLogicError(String const& msg) { + throw LogicError(msg); +} +#else // !JSON_USE_EXCEPTION +JSONCPP_NORETURN void throwRuntimeError(String const& msg) { + std::cerr << msg << std::endl; + abort(); +} +JSONCPP_NORETURN void throwLogicError(String const& msg) { + std::cerr << msg << std::endl; + abort(); +} +#endif + +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// class Value::CZString +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// + +// Notes: policy_ indicates if the string was allocated when +// a string is stored. + +Value::CZString::CZString(ArrayIndex index) : cstr_(nullptr), index_(index) {} + +Value::CZString::CZString(char const* str, unsigned length, + DuplicationPolicy allocate) + : cstr_(str) { + // allocate != duplicate + storage_.policy_ = allocate & 0x3; + storage_.length_ = length & 0x3FFFFFFF; +} + +Value::CZString::CZString(const CZString& other) { + cstr_ = (other.storage_.policy_ != noDuplication && other.cstr_ != nullptr + ? duplicateStringValue(other.cstr_, other.storage_.length_) + : other.cstr_); + storage_.policy_ = + static_cast( + other.cstr_ + ? (static_cast(other.storage_.policy_) == + noDuplication + ? noDuplication + : duplicate) + : static_cast(other.storage_.policy_)) & + 3U; + storage_.length_ = other.storage_.length_; +} + +Value::CZString::CZString(CZString&& other) noexcept + : cstr_(other.cstr_), index_(other.index_) { + other.cstr_ = nullptr; +} + +Value::CZString::~CZString() { + if (cstr_ && storage_.policy_ == duplicate) { + releaseStringValue(const_cast(cstr_), + storage_.length_ + 1U); // +1 for null terminating + // character for sake of + // completeness but not actually + // necessary + } +} + +void Value::CZString::swap(CZString& other) { + std::swap(cstr_, other.cstr_); + std::swap(index_, other.index_); +} + +Value::CZString& Value::CZString::operator=(const CZString& other) { + cstr_ = other.cstr_; + index_ = other.index_; + return *this; +} + +Value::CZString& Value::CZString::operator=(CZString&& other) noexcept { + cstr_ = other.cstr_; + index_ = other.index_; + other.cstr_ = nullptr; + return *this; +} + +bool Value::CZString::operator<(const CZString& other) const { + if (!cstr_) + return index_ < other.index_; + // return strcmp(cstr_, other.cstr_) < 0; + // Assume both are strings. + unsigned this_len = this->storage_.length_; + unsigned other_len = other.storage_.length_; + unsigned min_len = std::min(this_len, other_len); + JSON_ASSERT(this->cstr_ && other.cstr_); + int comp = memcmp(this->cstr_, other.cstr_, min_len); + if (comp < 0) + return true; + if (comp > 0) + return false; + return (this_len < other_len); +} + +bool Value::CZString::operator==(const CZString& other) const { + if (!cstr_) + return index_ == other.index_; + // return strcmp(cstr_, other.cstr_) == 0; + // Assume both are strings. + unsigned this_len = this->storage_.length_; + unsigned other_len = other.storage_.length_; + if (this_len != other_len) + return false; + JSON_ASSERT(this->cstr_ && other.cstr_); + int comp = memcmp(this->cstr_, other.cstr_, this_len); + return comp == 0; +} + +ArrayIndex Value::CZString::index() const { return index_; } + +// const char* Value::CZString::c_str() const { return cstr_; } +const char* Value::CZString::data() const { return cstr_; } +unsigned Value::CZString::length() const { return storage_.length_; } +bool Value::CZString::isStaticString() const { + return storage_.policy_ == noDuplication; +} + +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// class Value::Value +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// + +/*! \internal Default constructor initialization must be equivalent to: + * memset( this, 0, sizeof(Value) ) + * This optimization is used in ValueInternalMap fast allocator. + */ +Value::Value(ValueType type) { + static char const emptyString[] = ""; + initBasic(type); + switch (type) { + case nullValue: + break; + case intValue: + case uintValue: + value_.int_ = 0; + break; + case realValue: + value_.real_ = 0.0; + break; + case stringValue: + // allocated_ == false, so this is safe. + value_.string_ = const_cast(static_cast(emptyString)); + break; + case arrayValue: + case objectValue: + value_.map_ = new ObjectValues(); + break; + case booleanValue: + value_.bool_ = false; + break; + default: + JSON_ASSERT_UNREACHABLE; + } +} + +Value::Value(Int value) { + initBasic(intValue); + value_.int_ = value; +} + +Value::Value(UInt value) { + initBasic(uintValue); + value_.uint_ = value; +} +#if defined(JSON_HAS_INT64) +Value::Value(Int64 value) { + initBasic(intValue); + value_.int_ = value; +} +Value::Value(UInt64 value) { + initBasic(uintValue); + value_.uint_ = value; +} +#endif // defined(JSON_HAS_INT64) + +Value::Value(double value) { + initBasic(realValue); + value_.real_ = value; +} + +Value::Value(const char* value) { + initBasic(stringValue, true); + JSON_ASSERT_MESSAGE(value != nullptr, + "Null Value Passed to Value Constructor"); + value_.string_ = duplicateAndPrefixStringValue( + value, static_cast(strlen(value))); +} + +Value::Value(const char* begin, const char* end) { + initBasic(stringValue, true); + value_.string_ = + duplicateAndPrefixStringValue(begin, static_cast(end - begin)); +} + +Value::Value(const String& value) { + initBasic(stringValue, true); + value_.string_ = duplicateAndPrefixStringValue( + value.data(), static_cast(value.length())); +} + +Value::Value(const StaticString& value) { + initBasic(stringValue); + value_.string_ = const_cast(value.c_str()); +} + +Value::Value(bool value) { + initBasic(booleanValue); + value_.bool_ = value; +} + +Value::Value(const Value& other) { + dupPayload(other); + dupMeta(other); +} + +Value::Value(Value&& other) noexcept { + initBasic(nullValue); + swap(other); +} + +Value::~Value() { + releasePayload(); + value_.uint_ = 0; +} + +Value& Value::operator=(const Value& other) { + Value(other).swap(*this); + return *this; +} + +Value& Value::operator=(Value&& other) noexcept { + other.swap(*this); + return *this; +} + +void Value::swapPayload(Value& other) { + std::swap(bits_, other.bits_); + std::swap(value_, other.value_); +} + +void Value::copyPayload(const Value& other) { + releasePayload(); + dupPayload(other); +} + +void Value::swap(Value& other) { + swapPayload(other); + std::swap(comments_, other.comments_); + std::swap(start_, other.start_); + std::swap(limit_, other.limit_); +} + +void Value::copy(const Value& other) { + copyPayload(other); + dupMeta(other); +} + +ValueType Value::type() const { + return static_cast(bits_.value_type_); +} + +int Value::compare(const Value& other) const { + if (*this < other) + return -1; + if (*this > other) + return 1; + return 0; +} + +bool Value::operator<(const Value& other) const { + int typeDelta = type() - other.type(); + if (typeDelta) + return typeDelta < 0; + switch (type()) { + case nullValue: + return false; + case intValue: + return value_.int_ < other.value_.int_; + case uintValue: + return value_.uint_ < other.value_.uint_; + case realValue: + return value_.real_ < other.value_.real_; + case booleanValue: + return value_.bool_ < other.value_.bool_; + case stringValue: { + if ((value_.string_ == nullptr) || (other.value_.string_ == nullptr)) { + return other.value_.string_ != nullptr; + } + unsigned this_len; + unsigned other_len; + char const* this_str; + char const* other_str; + decodePrefixedString(this->isAllocated(), this->value_.string_, &this_len, + &this_str); + decodePrefixedString(other.isAllocated(), other.value_.string_, &other_len, + &other_str); + unsigned min_len = std::min(this_len, other_len); + JSON_ASSERT(this_str && other_str); + int comp = memcmp(this_str, other_str, min_len); + if (comp < 0) + return true; + if (comp > 0) + return false; + return (this_len < other_len); + } + case arrayValue: + case objectValue: { + auto thisSize = value_.map_->size(); + auto otherSize = other.value_.map_->size(); + if (thisSize != otherSize) + return thisSize < otherSize; + return (*value_.map_) < (*other.value_.map_); + } + default: + JSON_ASSERT_UNREACHABLE; + } + return false; // unreachable +} + +bool Value::operator<=(const Value& other) const { return !(other < *this); } + +bool Value::operator>=(const Value& other) const { return !(*this < other); } + +bool Value::operator>(const Value& other) const { return other < *this; } + +bool Value::operator==(const Value& other) const { + if (type() != other.type()) + return false; + switch (type()) { + case nullValue: + return true; + case intValue: + return value_.int_ == other.value_.int_; + case uintValue: + return value_.uint_ == other.value_.uint_; + case realValue: + return value_.real_ == other.value_.real_; + case booleanValue: + return value_.bool_ == other.value_.bool_; + case stringValue: { + if ((value_.string_ == nullptr) || (other.value_.string_ == nullptr)) { + return (value_.string_ == other.value_.string_); + } + unsigned this_len; + unsigned other_len; + char const* this_str; + char const* other_str; + decodePrefixedString(this->isAllocated(), this->value_.string_, &this_len, + &this_str); + decodePrefixedString(other.isAllocated(), other.value_.string_, &other_len, + &other_str); + if (this_len != other_len) + return false; + JSON_ASSERT(this_str && other_str); + int comp = memcmp(this_str, other_str, this_len); + return comp == 0; + } + case arrayValue: + case objectValue: + return value_.map_->size() == other.value_.map_->size() && + (*value_.map_) == (*other.value_.map_); + default: + JSON_ASSERT_UNREACHABLE; + } + return false; // unreachable +} + +bool Value::operator!=(const Value& other) const { return !(*this == other); } + +const char* Value::asCString() const { + JSON_ASSERT_MESSAGE(type() == stringValue, + "in Json::Value::asCString(): requires stringValue"); + if (value_.string_ == nullptr) + return nullptr; + unsigned this_len; + char const* this_str; + decodePrefixedString(this->isAllocated(), this->value_.string_, &this_len, + &this_str); + return this_str; +} + +#if JSONCPP_USING_SECURE_MEMORY +unsigned Value::getCStringLength() const { + JSON_ASSERT_MESSAGE(type() == stringValue, + "in Json::Value::asCString(): requires stringValue"); + if (value_.string_ == 0) + return 0; + unsigned this_len; + char const* this_str; + decodePrefixedString(this->isAllocated(), this->value_.string_, &this_len, + &this_str); + return this_len; +} +#endif + +bool Value::getString(char const** begin, char const** end) const { + if (type() != stringValue) + return false; + if (value_.string_ == nullptr) + return false; + unsigned length; + decodePrefixedString(this->isAllocated(), this->value_.string_, &length, + begin); + *end = *begin + length; + return true; +} + +String Value::asString() const { + switch (type()) { + case nullValue: + return ""; + case stringValue: { + if (value_.string_ == nullptr) + return ""; + unsigned this_len; + char const* this_str; + decodePrefixedString(this->isAllocated(), this->value_.string_, &this_len, + &this_str); + return String(this_str, this_len); + } + case booleanValue: + return value_.bool_ ? "true" : "false"; + case intValue: + return valueToString(value_.int_); + case uintValue: + return valueToString(value_.uint_); + case realValue: + return valueToString(value_.real_); + default: + JSON_FAIL_MESSAGE("Type is not convertible to string"); + } +} + +Value::Int Value::asInt() const { + switch (type()) { + case intValue: + JSON_ASSERT_MESSAGE(isInt(), "LargestInt out of Int range"); + return Int(value_.int_); + case uintValue: + JSON_ASSERT_MESSAGE(isInt(), "LargestUInt out of Int range"); + return Int(value_.uint_); + case realValue: + JSON_ASSERT_MESSAGE(InRange(value_.real_, minInt, maxInt), + "double out of Int range"); + return Int(value_.real_); + case nullValue: + return 0; + case booleanValue: + return value_.bool_ ? 1 : 0; + default: + break; + } + JSON_FAIL_MESSAGE("Value is not convertible to Int."); +} + +Value::UInt Value::asUInt() const { + switch (type()) { + case intValue: + JSON_ASSERT_MESSAGE(isUInt(), "LargestInt out of UInt range"); + return UInt(value_.int_); + case uintValue: + JSON_ASSERT_MESSAGE(isUInt(), "LargestUInt out of UInt range"); + return UInt(value_.uint_); + case realValue: + JSON_ASSERT_MESSAGE(InRange(value_.real_, 0, maxUInt), + "double out of UInt range"); + return UInt(value_.real_); + case nullValue: + return 0; + case booleanValue: + return value_.bool_ ? 1 : 0; + default: + break; + } + JSON_FAIL_MESSAGE("Value is not convertible to UInt."); +} + +#if defined(JSON_HAS_INT64) + +Value::Int64 Value::asInt64() const { + switch (type()) { + case intValue: + return Int64(value_.int_); + case uintValue: + JSON_ASSERT_MESSAGE(isInt64(), "LargestUInt out of Int64 range"); + return Int64(value_.uint_); + case realValue: + JSON_ASSERT_MESSAGE(InRange(value_.real_, minInt64, maxInt64), + "double out of Int64 range"); + return Int64(value_.real_); + case nullValue: + return 0; + case booleanValue: + return value_.bool_ ? 1 : 0; + default: + break; + } + JSON_FAIL_MESSAGE("Value is not convertible to Int64."); +} + +Value::UInt64 Value::asUInt64() const { + switch (type()) { + case intValue: + JSON_ASSERT_MESSAGE(isUInt64(), "LargestInt out of UInt64 range"); + return UInt64(value_.int_); + case uintValue: + return UInt64(value_.uint_); + case realValue: + JSON_ASSERT_MESSAGE(InRange(value_.real_, 0, maxUInt64), + "double out of UInt64 range"); + return UInt64(value_.real_); + case nullValue: + return 0; + case booleanValue: + return value_.bool_ ? 1 : 0; + default: + break; + } + JSON_FAIL_MESSAGE("Value is not convertible to UInt64."); +} +#endif // if defined(JSON_HAS_INT64) + +LargestInt Value::asLargestInt() const { +#if defined(JSON_NO_INT64) + return asInt(); +#else + return asInt64(); +#endif +} + +LargestUInt Value::asLargestUInt() const { +#if defined(JSON_NO_INT64) + return asUInt(); +#else + return asUInt64(); +#endif +} + +double Value::asDouble() const { + switch (type()) { + case intValue: + return static_cast(value_.int_); + case uintValue: +#if !defined(JSON_USE_INT64_DOUBLE_CONVERSION) + return static_cast(value_.uint_); +#else // if !defined(JSON_USE_INT64_DOUBLE_CONVERSION) + return integerToDouble(value_.uint_); +#endif // if !defined(JSON_USE_INT64_DOUBLE_CONVERSION) + case realValue: + return value_.real_; + case nullValue: + return 0.0; + case booleanValue: + return value_.bool_ ? 1.0 : 0.0; + default: + break; + } + JSON_FAIL_MESSAGE("Value is not convertible to double."); +} + +float Value::asFloat() const { + switch (type()) { + case intValue: + return static_cast(value_.int_); + case uintValue: +#if !defined(JSON_USE_INT64_DOUBLE_CONVERSION) + return static_cast(value_.uint_); +#else // if !defined(JSON_USE_INT64_DOUBLE_CONVERSION) + // This can fail (silently?) if the value is bigger than MAX_FLOAT. + return static_cast(integerToDouble(value_.uint_)); +#endif // if !defined(JSON_USE_INT64_DOUBLE_CONVERSION) + case realValue: + return static_cast(value_.real_); + case nullValue: + return 0.0; + case booleanValue: + return value_.bool_ ? 1.0F : 0.0F; + default: + break; + } + JSON_FAIL_MESSAGE("Value is not convertible to float."); +} + +bool Value::asBool() const { + switch (type()) { + case booleanValue: + return value_.bool_; + case nullValue: + return false; + case intValue: + return value_.int_ != 0; + case uintValue: + return value_.uint_ != 0; + case realValue: { + // According to JavaScript language zero or NaN is regarded as false + const auto value_classification = std::fpclassify(value_.real_); + return value_classification != FP_ZERO && value_classification != FP_NAN; + } + default: + break; + } + JSON_FAIL_MESSAGE("Value is not convertible to bool."); +} + +bool Value::isConvertibleTo(ValueType other) const { + switch (other) { + case nullValue: + return (isNumeric() && asDouble() == 0.0) || + (type() == booleanValue && !value_.bool_) || + (type() == stringValue && asString().empty()) || + (type() == arrayValue && value_.map_->empty()) || + (type() == objectValue && value_.map_->empty()) || + type() == nullValue; + case intValue: + return isInt() || + (type() == realValue && InRange(value_.real_, minInt, maxInt)) || + type() == booleanValue || type() == nullValue; + case uintValue: + return isUInt() || + (type() == realValue && InRange(value_.real_, 0, maxUInt)) || + type() == booleanValue || type() == nullValue; + case realValue: + return isNumeric() || type() == booleanValue || type() == nullValue; + case booleanValue: + return isNumeric() || type() == booleanValue || type() == nullValue; + case stringValue: + return isNumeric() || type() == booleanValue || type() == stringValue || + type() == nullValue; + case arrayValue: + return type() == arrayValue || type() == nullValue; + case objectValue: + return type() == objectValue || type() == nullValue; + } + JSON_ASSERT_UNREACHABLE; + return false; +} + +/// Number of values in array or object +ArrayIndex Value::size() const { + switch (type()) { + case nullValue: + case intValue: + case uintValue: + case realValue: + case booleanValue: + case stringValue: + return 0; + case arrayValue: // size of the array is highest index + 1 + if (!value_.map_->empty()) { + ObjectValues::const_iterator itLast = value_.map_->end(); + --itLast; + return (*itLast).first.index() + 1; + } + return 0; + case objectValue: + return ArrayIndex(value_.map_->size()); + } + JSON_ASSERT_UNREACHABLE; + return 0; // unreachable; +} + +bool Value::empty() const { + if (isNull() || isArray() || isObject()) + return size() == 0U; + return false; +} + +Value::operator bool() const { return !isNull(); } + +void Value::clear() { + JSON_ASSERT_MESSAGE(type() == nullValue || type() == arrayValue || + type() == objectValue, + "in Json::Value::clear(): requires complex value"); + start_ = 0; + limit_ = 0; + switch (type()) { + case arrayValue: + case objectValue: + value_.map_->clear(); + break; + default: + break; + } +} + +void Value::resize(ArrayIndex newSize) { + JSON_ASSERT_MESSAGE(type() == nullValue || type() == arrayValue, + "in Json::Value::resize(): requires arrayValue"); + if (type() == nullValue) + *this = Value(arrayValue); + ArrayIndex oldSize = size(); + if (newSize == 0) + clear(); + else if (newSize > oldSize) + for (ArrayIndex i = oldSize; i < newSize; ++i) + (*this)[i]; + else { + for (ArrayIndex index = newSize; index < oldSize; ++index) { + value_.map_->erase(index); + } + JSON_ASSERT(size() == newSize); + } +} + +Value& Value::operator[](ArrayIndex index) { + JSON_ASSERT_MESSAGE( + type() == nullValue || type() == arrayValue, + "in Json::Value::operator[](ArrayIndex): requires arrayValue"); + if (type() == nullValue) + *this = Value(arrayValue); + CZString key(index); + auto it = value_.map_->lower_bound(key); + if (it != value_.map_->end() && (*it).first == key) + return (*it).second; + + ObjectValues::value_type defaultValue(key, nullSingleton()); + it = value_.map_->insert(it, defaultValue); + return (*it).second; +} + +Value& Value::operator[](int index) { + JSON_ASSERT_MESSAGE( + index >= 0, + "in Json::Value::operator[](int index): index cannot be negative"); + return (*this)[ArrayIndex(index)]; +} + +const Value& Value::operator[](ArrayIndex index) const { + JSON_ASSERT_MESSAGE( + type() == nullValue || type() == arrayValue, + "in Json::Value::operator[](ArrayIndex)const: requires arrayValue"); + if (type() == nullValue) + return nullSingleton(); + CZString key(index); + ObjectValues::const_iterator it = value_.map_->find(key); + if (it == value_.map_->end()) + return nullSingleton(); + return (*it).second; +} + +const Value& Value::operator[](int index) const { + JSON_ASSERT_MESSAGE( + index >= 0, + "in Json::Value::operator[](int index) const: index cannot be negative"); + return (*this)[ArrayIndex(index)]; +} + +void Value::initBasic(ValueType type, bool allocated) { + setType(type); + setIsAllocated(allocated); + comments_ = Comments{}; + start_ = 0; + limit_ = 0; +} + +void Value::dupPayload(const Value& other) { + setType(other.type()); + setIsAllocated(false); + switch (type()) { + case nullValue: + case intValue: + case uintValue: + case realValue: + case booleanValue: + value_ = other.value_; + break; + case stringValue: + if (other.value_.string_ && other.isAllocated()) { + unsigned len; + char const* str; + decodePrefixedString(other.isAllocated(), other.value_.string_, &len, + &str); + value_.string_ = duplicateAndPrefixStringValue(str, len); + setIsAllocated(true); + } else { + value_.string_ = other.value_.string_; + } + break; + case arrayValue: + case objectValue: + value_.map_ = new ObjectValues(*other.value_.map_); + break; + default: + JSON_ASSERT_UNREACHABLE; + } +} + +void Value::releasePayload() { + switch (type()) { + case nullValue: + case intValue: + case uintValue: + case realValue: + case booleanValue: + break; + case stringValue: + if (isAllocated()) + releasePrefixedStringValue(value_.string_); + break; + case arrayValue: + case objectValue: + delete value_.map_; + break; + default: + JSON_ASSERT_UNREACHABLE; + } +} + +void Value::dupMeta(const Value& other) { + comments_ = other.comments_; + start_ = other.start_; + limit_ = other.limit_; +} + +// Access an object value by name, create a null member if it does not exist. +// @pre Type of '*this' is object or null. +// @param key is null-terminated. +Value& Value::resolveReference(const char* key) { + JSON_ASSERT_MESSAGE( + type() == nullValue || type() == objectValue, + "in Json::Value::resolveReference(): requires objectValue"); + if (type() == nullValue) + *this = Value(objectValue); + CZString actualKey(key, static_cast(strlen(key)), + CZString::noDuplication); // NOTE! + auto it = value_.map_->lower_bound(actualKey); + if (it != value_.map_->end() && (*it).first == actualKey) + return (*it).second; + + ObjectValues::value_type defaultValue(actualKey, nullSingleton()); + it = value_.map_->insert(it, defaultValue); + Value& value = (*it).second; + return value; +} + +// @param key is not null-terminated. +Value& Value::resolveReference(char const* key, char const* end) { + JSON_ASSERT_MESSAGE( + type() == nullValue || type() == objectValue, + "in Json::Value::resolveReference(key, end): requires objectValue"); + if (type() == nullValue) + *this = Value(objectValue); + CZString actualKey(key, static_cast(end - key), + CZString::duplicateOnCopy); + auto it = value_.map_->lower_bound(actualKey); + if (it != value_.map_->end() && (*it).first == actualKey) + return (*it).second; + + ObjectValues::value_type defaultValue(actualKey, nullSingleton()); + it = value_.map_->insert(it, defaultValue); + Value& value = (*it).second; + return value; +} + +Value Value::get(ArrayIndex index, const Value& defaultValue) const { + const Value* value = &((*this)[index]); + return value == &nullSingleton() ? defaultValue : *value; +} + +bool Value::isValidIndex(ArrayIndex index) const { return index < size(); } + +Value const* Value::find(char const* begin, char const* end) const { + JSON_ASSERT_MESSAGE(type() == nullValue || type() == objectValue, + "in Json::Value::find(begin, end): requires " + "objectValue or nullValue"); + if (type() == nullValue) + return nullptr; + CZString actualKey(begin, static_cast(end - begin), + CZString::noDuplication); + ObjectValues::const_iterator it = value_.map_->find(actualKey); + if (it == value_.map_->end()) + return nullptr; + return &(*it).second; +} +Value* Value::demand(char const* begin, char const* end) { + JSON_ASSERT_MESSAGE(type() == nullValue || type() == objectValue, + "in Json::Value::demand(begin, end): requires " + "objectValue or nullValue"); + return &resolveReference(begin, end); +} +const Value& Value::operator[](const char* key) const { + Value const* found = find(key, key + strlen(key)); + if (!found) + return nullSingleton(); + return *found; +} +Value const& Value::operator[](const String& key) const { + Value const* found = find(key.data(), key.data() + key.length()); + if (!found) + return nullSingleton(); + return *found; +} + +Value& Value::operator[](const char* key) { + return resolveReference(key, key + strlen(key)); +} + +Value& Value::operator[](const String& key) { + return resolveReference(key.data(), key.data() + key.length()); +} + +Value& Value::operator[](const StaticString& key) { + return resolveReference(key.c_str()); +} + +Value& Value::append(const Value& value) { return append(Value(value)); } + +Value& Value::append(Value&& value) { + JSON_ASSERT_MESSAGE(type() == nullValue || type() == arrayValue, + "in Json::Value::append: requires arrayValue"); + if (type() == nullValue) { + *this = Value(arrayValue); + } + return this->value_.map_->emplace(size(), std::move(value)).first->second; +} + +bool Value::insert(ArrayIndex index, const Value& newValue) { + return insert(index, Value(newValue)); +} + +bool Value::insert(ArrayIndex index, Value&& newValue) { + JSON_ASSERT_MESSAGE(type() == nullValue || type() == arrayValue, + "in Json::Value::insert: requires arrayValue"); + ArrayIndex length = size(); + if (index > length) { + return false; + } + for (ArrayIndex i = length; i > index; i--) { + (*this)[i] = std::move((*this)[i - 1]); + } + (*this)[index] = std::move(newValue); + return true; +} + +Value Value::get(char const* begin, char const* end, + Value const& defaultValue) const { + Value const* found = find(begin, end); + return !found ? defaultValue : *found; +} +Value Value::get(char const* key, Value const& defaultValue) const { + return get(key, key + strlen(key), defaultValue); +} +Value Value::get(String const& key, Value const& defaultValue) const { + return get(key.data(), key.data() + key.length(), defaultValue); +} + +bool Value::removeMember(const char* begin, const char* end, Value* removed) { + if (type() != objectValue) { + return false; + } + CZString actualKey(begin, static_cast(end - begin), + CZString::noDuplication); + auto it = value_.map_->find(actualKey); + if (it == value_.map_->end()) + return false; + if (removed) + *removed = std::move(it->second); + value_.map_->erase(it); + return true; +} +bool Value::removeMember(const char* key, Value* removed) { + return removeMember(key, key + strlen(key), removed); +} +bool Value::removeMember(String const& key, Value* removed) { + return removeMember(key.data(), key.data() + key.length(), removed); +} +void Value::removeMember(const char* key) { + JSON_ASSERT_MESSAGE(type() == nullValue || type() == objectValue, + "in Json::Value::removeMember(): requires objectValue"); + if (type() == nullValue) + return; + + CZString actualKey(key, unsigned(strlen(key)), CZString::noDuplication); + value_.map_->erase(actualKey); +} +void Value::removeMember(const String& key) { removeMember(key.c_str()); } + +bool Value::removeIndex(ArrayIndex index, Value* removed) { + if (type() != arrayValue) { + return false; + } + CZString key(index); + auto it = value_.map_->find(key); + if (it == value_.map_->end()) { + return false; + } + if (removed) + *removed = it->second; + ArrayIndex oldSize = size(); + // shift left all items left, into the place of the "removed" + for (ArrayIndex i = index; i < (oldSize - 1); ++i) { + CZString keey(i); + (*value_.map_)[keey] = (*this)[i + 1]; + } + // erase the last one ("leftover") + CZString keyLast(oldSize - 1); + auto itLast = value_.map_->find(keyLast); + value_.map_->erase(itLast); + return true; +} + +bool Value::isMember(char const* begin, char const* end) const { + Value const* value = find(begin, end); + return nullptr != value; +} +bool Value::isMember(char const* key) const { + return isMember(key, key + strlen(key)); +} +bool Value::isMember(String const& key) const { + return isMember(key.data(), key.data() + key.length()); +} + +Value::Members Value::getMemberNames() const { + JSON_ASSERT_MESSAGE( + type() == nullValue || type() == objectValue, + "in Json::Value::getMemberNames(), value must be objectValue"); + if (type() == nullValue) + return Value::Members(); + Members members; + members.reserve(value_.map_->size()); + ObjectValues::const_iterator it = value_.map_->begin(); + ObjectValues::const_iterator itEnd = value_.map_->end(); + for (; it != itEnd; ++it) { + members.push_back(String((*it).first.data(), (*it).first.length())); + } + return members; +} + +static bool IsIntegral(double d) { + double integral_part; + return modf(d, &integral_part) == 0.0; +} + +bool Value::isNull() const { return type() == nullValue; } + +bool Value::isBool() const { return type() == booleanValue; } + +bool Value::isInt() const { + switch (type()) { + case intValue: +#if defined(JSON_HAS_INT64) + return value_.int_ >= minInt && value_.int_ <= maxInt; +#else + return true; +#endif + case uintValue: + return value_.uint_ <= UInt(maxInt); + case realValue: + return value_.real_ >= minInt && value_.real_ <= maxInt && + IsIntegral(value_.real_); + default: + break; + } + return false; +} + +bool Value::isUInt() const { + switch (type()) { + case intValue: +#if defined(JSON_HAS_INT64) + return value_.int_ >= 0 && LargestUInt(value_.int_) <= LargestUInt(maxUInt); +#else + return value_.int_ >= 0; +#endif + case uintValue: +#if defined(JSON_HAS_INT64) + return value_.uint_ <= maxUInt; +#else + return true; +#endif + case realValue: + return value_.real_ >= 0 && value_.real_ <= maxUInt && + IsIntegral(value_.real_); + default: + break; + } + return false; +} + +bool Value::isInt64() const { +#if defined(JSON_HAS_INT64) + switch (type()) { + case intValue: + return true; + case uintValue: + return value_.uint_ <= UInt64(maxInt64); + case realValue: + // Note that maxInt64 (= 2^63 - 1) is not exactly representable as a + // double, so double(maxInt64) will be rounded up to 2^63. Therefore we + // require the value to be strictly less than the limit. + return value_.real_ >= double(minInt64) && + value_.real_ < double(maxInt64) && IsIntegral(value_.real_); + default: + break; + } +#endif // JSON_HAS_INT64 + return false; +} + +bool Value::isUInt64() const { +#if defined(JSON_HAS_INT64) + switch (type()) { + case intValue: + return value_.int_ >= 0; + case uintValue: + return true; + case realValue: + // Note that maxUInt64 (= 2^64 - 1) is not exactly representable as a + // double, so double(maxUInt64) will be rounded up to 2^64. Therefore we + // require the value to be strictly less than the limit. + return value_.real_ >= 0 && value_.real_ < maxUInt64AsDouble && + IsIntegral(value_.real_); + default: + break; + } +#endif // JSON_HAS_INT64 + return false; +} + +bool Value::isIntegral() const { + switch (type()) { + case intValue: + case uintValue: + return true; + case realValue: +#if defined(JSON_HAS_INT64) + // Note that maxUInt64 (= 2^64 - 1) is not exactly representable as a + // double, so double(maxUInt64) will be rounded up to 2^64. Therefore we + // require the value to be strictly less than the limit. + return value_.real_ >= double(minInt64) && + value_.real_ < maxUInt64AsDouble && IsIntegral(value_.real_); +#else + return value_.real_ >= minInt && value_.real_ <= maxUInt && + IsIntegral(value_.real_); +#endif // JSON_HAS_INT64 + default: + break; + } + return false; +} + +bool Value::isDouble() const { + return type() == intValue || type() == uintValue || type() == realValue; +} + +bool Value::isNumeric() const { return isDouble(); } + +bool Value::isString() const { return type() == stringValue; } + +bool Value::isArray() const { return type() == arrayValue; } + +bool Value::isObject() const { return type() == objectValue; } + +Value::Comments::Comments(const Comments& that) + : ptr_{cloneUnique(that.ptr_)} {} + +Value::Comments::Comments(Comments&& that) noexcept + : ptr_{std::move(that.ptr_)} {} + +Value::Comments& Value::Comments::operator=(const Comments& that) { + ptr_ = cloneUnique(that.ptr_); + return *this; +} + +Value::Comments& Value::Comments::operator=(Comments&& that) noexcept { + ptr_ = std::move(that.ptr_); + return *this; +} + +bool Value::Comments::has(CommentPlacement slot) const { + return ptr_ && !(*ptr_)[slot].empty(); +} + +String Value::Comments::get(CommentPlacement slot) const { + if (!ptr_) + return {}; + return (*ptr_)[slot]; +} + +void Value::Comments::set(CommentPlacement slot, String comment) { + if (slot >= CommentPlacement::numberOfCommentPlacement) + return; + if (!ptr_) + ptr_ = std::unique_ptr(new Array()); + (*ptr_)[slot] = std::move(comment); +} + +void Value::setComment(String comment, CommentPlacement placement) { + if (!comment.empty() && (comment.back() == '\n')) { + // Always discard trailing newline, to aid indentation. + comment.pop_back(); + } + JSON_ASSERT(!comment.empty()); + JSON_ASSERT_MESSAGE( + comment[0] == '\0' || comment[0] == '/', + "in Json::Value::setComment(): Comments must start with /"); + comments_.set(placement, std::move(comment)); +} + +bool Value::hasComment(CommentPlacement placement) const { + return comments_.has(placement); +} + +String Value::getComment(CommentPlacement placement) const { + return comments_.get(placement); +} + +void Value::setOffsetStart(ptrdiff_t start) { start_ = start; } + +void Value::setOffsetLimit(ptrdiff_t limit) { limit_ = limit; } + +ptrdiff_t Value::getOffsetStart() const { return start_; } + +ptrdiff_t Value::getOffsetLimit() const { return limit_; } + +String Value::toStyledString() const { + StreamWriterBuilder builder; + + String out = this->hasComment(commentBefore) ? "\n" : ""; + out += Json::writeString(builder, *this); + out += '\n'; + + return out; +} + +Value::const_iterator Value::begin() const { + switch (type()) { + case arrayValue: + case objectValue: + if (value_.map_) + return const_iterator(value_.map_->begin()); + break; + default: + break; + } + return {}; +} + +Value::const_iterator Value::end() const { + switch (type()) { + case arrayValue: + case objectValue: + if (value_.map_) + return const_iterator(value_.map_->end()); + break; + default: + break; + } + return {}; +} + +Value::iterator Value::begin() { + switch (type()) { + case arrayValue: + case objectValue: + if (value_.map_) + return iterator(value_.map_->begin()); + break; + default: + break; + } + return iterator(); +} + +Value::iterator Value::end() { + switch (type()) { + case arrayValue: + case objectValue: + if (value_.map_) + return iterator(value_.map_->end()); + break; + default: + break; + } + return iterator(); +} + +// class PathArgument +// ////////////////////////////////////////////////////////////////// + +PathArgument::PathArgument() = default; + +PathArgument::PathArgument(ArrayIndex index) + : index_(index), kind_(kindIndex) {} + +PathArgument::PathArgument(const char* key) : key_(key), kind_(kindKey) {} + +PathArgument::PathArgument(String key) : key_(std::move(key)), kind_(kindKey) {} + +// class Path +// ////////////////////////////////////////////////////////////////// + +Path::Path(const String& path, const PathArgument& a1, const PathArgument& a2, + const PathArgument& a3, const PathArgument& a4, + const PathArgument& a5) { + InArgs in; + in.reserve(5); + in.push_back(&a1); + in.push_back(&a2); + in.push_back(&a3); + in.push_back(&a4); + in.push_back(&a5); + makePath(path, in); +} + +void Path::makePath(const String& path, const InArgs& in) { + const char* current = path.c_str(); + const char* end = current + path.length(); + auto itInArg = in.begin(); + while (current != end) { + if (*current == '[') { + ++current; + if (*current == '%') + addPathInArg(path, in, itInArg, PathArgument::kindIndex); + else { + ArrayIndex index = 0; + for (; current != end && *current >= '0' && *current <= '9'; ++current) + index = index * 10 + ArrayIndex(*current - '0'); + args_.push_back(index); + } + if (current == end || *++current != ']') + invalidPath(path, int(current - path.c_str())); + } else if (*current == '%') { + addPathInArg(path, in, itInArg, PathArgument::kindKey); + ++current; + } else if (*current == '.' || *current == ']') { + ++current; + } else { + const char* beginName = current; + while (current != end && !strchr("[.", *current)) + ++current; + args_.push_back(String(beginName, current)); + } + } +} + +void Path::addPathInArg(const String& /*path*/, const InArgs& in, + InArgs::const_iterator& itInArg, + PathArgument::Kind kind) { + if (itInArg == in.end()) { + // Error: missing argument %d + } else if ((*itInArg)->kind_ != kind) { + // Error: bad argument type + } else { + args_.push_back(**itInArg++); + } +} + +void Path::invalidPath(const String& /*path*/, int /*location*/) { + // Error: invalid path. +} + +const Value& Path::resolve(const Value& root) const { + const Value* node = &root; + for (const auto& arg : args_) { + if (arg.kind_ == PathArgument::kindIndex) { + if (!node->isArray() || !node->isValidIndex(arg.index_)) { + // Error: unable to resolve path (array value expected at position... ) + return Value::nullSingleton(); + } + node = &((*node)[arg.index_]); + } else if (arg.kind_ == PathArgument::kindKey) { + if (!node->isObject()) { + // Error: unable to resolve path (object value expected at position...) + return Value::nullSingleton(); + } + node = &((*node)[arg.key_]); + if (node == &Value::nullSingleton()) { + // Error: unable to resolve path (object has no member named '' at + // position...) + return Value::nullSingleton(); + } + } + } + return *node; +} + +Value Path::resolve(const Value& root, const Value& defaultValue) const { + const Value* node = &root; + for (const auto& arg : args_) { + if (arg.kind_ == PathArgument::kindIndex) { + if (!node->isArray() || !node->isValidIndex(arg.index_)) + return defaultValue; + node = &((*node)[arg.index_]); + } else if (arg.kind_ == PathArgument::kindKey) { + if (!node->isObject()) + return defaultValue; + node = &((*node)[arg.key_]); + if (node == &Value::nullSingleton()) + return defaultValue; + } + } + return *node; +} + +Value& Path::make(Value& root) const { + Value* node = &root; + for (const auto& arg : args_) { + if (arg.kind_ == PathArgument::kindIndex) { + if (!node->isArray()) { + // Error: node is not an array at position ... + } + node = &((*node)[arg.index_]); + } else if (arg.kind_ == PathArgument::kindKey) { + if (!node->isObject()) { + // Error: node is not an object at position... + } + node = &((*node)[arg.key_]); + } + } + return *node; +} + +} // namespace Json + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: src/lib_json/json_value.cpp +// ////////////////////////////////////////////////////////////////////// + + + + + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: src/lib_json/json_writer.cpp +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2011 Baptiste Lepilleur and The JsonCpp Authors +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +#if !defined(JSON_IS_AMALGAMATION) +#include "json_tool.h" +#include +#endif // if !defined(JSON_IS_AMALGAMATION) +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#if __cplusplus >= 201103L +#include +#include + +#if !defined(isnan) +#define isnan std::isnan +#endif + +#if !defined(isfinite) +#define isfinite std::isfinite +#endif + +#else +#include +#include + +#if defined(_MSC_VER) +#if !defined(isnan) +#include +#define isnan _isnan +#endif + +#if !defined(isfinite) +#include +#define isfinite _finite +#endif + +#if !defined(_CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES) +#define _CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES 1 +#endif //_CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES + +#endif //_MSC_VER + +#if defined(__sun) && defined(__SVR4) // Solaris +#if !defined(isfinite) +#include +#define isfinite finite +#endif +#endif + +#if defined(__hpux) +#if !defined(isfinite) +#if defined(__ia64) && !defined(finite) +#define isfinite(x) \ + ((sizeof(x) == sizeof(float) ? _Isfinitef(x) : _IsFinite(x))) +#endif +#endif +#endif + +#if !defined(isnan) +// IEEE standard states that NaN values will not compare to themselves +#define isnan(x) ((x) != (x)) +#endif + +#if !defined(__APPLE__) +#if !defined(isfinite) +#define isfinite finite +#endif +#endif +#endif + +#if defined(_MSC_VER) +// Disable warning about strdup being deprecated. +#pragma warning(disable : 4996) +#endif + +namespace Json { + +#if __cplusplus >= 201103L || (defined(_CPPLIB_VER) && _CPPLIB_VER >= 520) +using StreamWriterPtr = std::unique_ptr; +#else +using StreamWriterPtr = std::auto_ptr; +#endif + +String valueToString(LargestInt value) { + UIntToStringBuffer buffer; + char* current = buffer + sizeof(buffer); + if (value == Value::minLargestInt) { + uintToString(LargestUInt(Value::maxLargestInt) + 1, current); + *--current = '-'; + } else if (value < 0) { + uintToString(LargestUInt(-value), current); + *--current = '-'; + } else { + uintToString(LargestUInt(value), current); + } + assert(current >= buffer); + return current; +} + +String valueToString(LargestUInt value) { + UIntToStringBuffer buffer; + char* current = buffer + sizeof(buffer); + uintToString(value, current); + assert(current >= buffer); + return current; +} + +#if defined(JSON_HAS_INT64) + +String valueToString(Int value) { return valueToString(LargestInt(value)); } + +String valueToString(UInt value) { return valueToString(LargestUInt(value)); } + +#endif // # if defined(JSON_HAS_INT64) + +namespace { +String valueToString(double value, bool useSpecialFloats, + unsigned int precision, PrecisionType precisionType) { + // Print into the buffer. We need not request the alternative representation + // that always has a decimal point because JSON doesn't distinguish the + // concepts of reals and integers. + if (!isfinite(value)) { + static const char* const reps[2][3] = {{"NaN", "-Infinity", "Infinity"}, + {"null", "-1e+9999", "1e+9999"}}; + return reps[useSpecialFloats ? 0 : 1] + [isnan(value) ? 0 : (value < 0) ? 1 : 2]; + } + + String buffer(size_t(36), '\0'); + while (true) { + int len = jsoncpp_snprintf( + &*buffer.begin(), buffer.size(), + (precisionType == PrecisionType::significantDigits) ? "%.*g" : "%.*f", + precision, value); + assert(len >= 0); + auto wouldPrint = static_cast(len); + if (wouldPrint >= buffer.size()) { + buffer.resize(wouldPrint + 1); + continue; + } + buffer.resize(wouldPrint); + break; + } + + buffer.erase(fixNumericLocale(buffer.begin(), buffer.end()), buffer.end()); + + // try to ensure we preserve the fact that this was given to us as a double on + // input + if (buffer.find('.') == buffer.npos && buffer.find('e') == buffer.npos) { + buffer += ".0"; + } + + // strip the zero padding from the right + if (precisionType == PrecisionType::decimalPlaces) { + buffer.erase(fixZerosInTheEnd(buffer.begin(), buffer.end(), precision), + buffer.end()); + } + + return buffer; +} +} // namespace + +String valueToString(double value, unsigned int precision, + PrecisionType precisionType) { + return valueToString(value, false, precision, precisionType); +} + +String valueToString(bool value) { return value ? "true" : "false"; } + +static bool doesAnyCharRequireEscaping(char const* s, size_t n) { + assert(s || !n); + + return std::any_of(s, s + n, [](unsigned char c) { + return c == '\\' || c == '"' || c < 0x20 || c > 0x7F; + }); +} + +static unsigned int utf8ToCodepoint(const char*& s, const char* e) { + const unsigned int REPLACEMENT_CHARACTER = 0xFFFD; + + unsigned int firstByte = static_cast(*s); + + if (firstByte < 0x80) + return firstByte; + + if (firstByte < 0xE0) { + if (e - s < 2) + return REPLACEMENT_CHARACTER; + + unsigned int calculated = + ((firstByte & 0x1F) << 6) | (static_cast(s[1]) & 0x3F); + s += 1; + // oversized encoded characters are invalid + return calculated < 0x80 ? REPLACEMENT_CHARACTER : calculated; + } + + if (firstByte < 0xF0) { + if (e - s < 3) + return REPLACEMENT_CHARACTER; + + unsigned int calculated = ((firstByte & 0x0F) << 12) | + ((static_cast(s[1]) & 0x3F) << 6) | + (static_cast(s[2]) & 0x3F); + s += 2; + // surrogates aren't valid codepoints itself + // shouldn't be UTF-8 encoded + if (calculated >= 0xD800 && calculated <= 0xDFFF) + return REPLACEMENT_CHARACTER; + // oversized encoded characters are invalid + return calculated < 0x800 ? REPLACEMENT_CHARACTER : calculated; + } + + if (firstByte < 0xF8) { + if (e - s < 4) + return REPLACEMENT_CHARACTER; + + unsigned int calculated = ((firstByte & 0x07) << 18) | + ((static_cast(s[1]) & 0x3F) << 12) | + ((static_cast(s[2]) & 0x3F) << 6) | + (static_cast(s[3]) & 0x3F); + s += 3; + // oversized encoded characters are invalid + return calculated < 0x10000 ? REPLACEMENT_CHARACTER : calculated; + } + + return REPLACEMENT_CHARACTER; +} + +static const char hex2[] = "000102030405060708090a0b0c0d0e0f" + "101112131415161718191a1b1c1d1e1f" + "202122232425262728292a2b2c2d2e2f" + "303132333435363738393a3b3c3d3e3f" + "404142434445464748494a4b4c4d4e4f" + "505152535455565758595a5b5c5d5e5f" + "606162636465666768696a6b6c6d6e6f" + "707172737475767778797a7b7c7d7e7f" + "808182838485868788898a8b8c8d8e8f" + "909192939495969798999a9b9c9d9e9f" + "a0a1a2a3a4a5a6a7a8a9aaabacadaeaf" + "b0b1b2b3b4b5b6b7b8b9babbbcbdbebf" + "c0c1c2c3c4c5c6c7c8c9cacbcccdcecf" + "d0d1d2d3d4d5d6d7d8d9dadbdcdddedf" + "e0e1e2e3e4e5e6e7e8e9eaebecedeeef" + "f0f1f2f3f4f5f6f7f8f9fafbfcfdfeff"; + +static String toHex16Bit(unsigned int x) { + const unsigned int hi = (x >> 8) & 0xff; + const unsigned int lo = x & 0xff; + String result(4, ' '); + result[0] = hex2[2 * hi]; + result[1] = hex2[2 * hi + 1]; + result[2] = hex2[2 * lo]; + result[3] = hex2[2 * lo + 1]; + return result; +} + +static void appendRaw(String& result, unsigned ch) { + result += static_cast(ch); +} + +static void appendHex(String& result, unsigned ch) { + result.append("\\u").append(toHex16Bit(ch)); +} + +static String valueToQuotedStringN(const char* value, size_t length, + bool emitUTF8 = false) { + if (value == nullptr) + return ""; + + if (!doesAnyCharRequireEscaping(value, length)) + return String("\"") + value + "\""; + // We have to walk value and escape any special characters. + // Appending to String is not efficient, but this should be rare. + // (Note: forward slashes are *not* rare, but I am not escaping them.) + String::size_type maxsize = length * 2 + 3; // allescaped+quotes+NULL + String result; + result.reserve(maxsize); // to avoid lots of mallocs + result += "\""; + char const* end = value + length; + for (const char* c = value; c != end; ++c) { + switch (*c) { + case '\"': + result += "\\\""; + break; + case '\\': + result += "\\\\"; + break; + case '\b': + result += "\\b"; + break; + case '\f': + result += "\\f"; + break; + case '\n': + result += "\\n"; + break; + case '\r': + result += "\\r"; + break; + case '\t': + result += "\\t"; + break; + // case '/': + // Even though \/ is considered a legal escape in JSON, a bare + // slash is also legal, so I see no reason to escape it. + // (I hope I am not misunderstanding something.) + // blep notes: actually escaping \/ may be useful in javascript to avoid (*c); + if (codepoint < 0x20) { + appendHex(result, codepoint); + } else { + appendRaw(result, codepoint); + } + } else { + unsigned codepoint = utf8ToCodepoint(c, end); // modifies `c` + if (codepoint < 0x20) { + appendHex(result, codepoint); + } else if (codepoint < 0x80) { + appendRaw(result, codepoint); + } else if (codepoint < 0x10000) { + // Basic Multilingual Plane + appendHex(result, codepoint); + } else { + // Extended Unicode. Encode 20 bits as a surrogate pair. + codepoint -= 0x10000; + appendHex(result, 0xd800 + ((codepoint >> 10) & 0x3ff)); + appendHex(result, 0xdc00 + (codepoint & 0x3ff)); + } + } + } break; + } + } + result += "\""; + return result; +} + +String valueToQuotedString(const char* value) { + return valueToQuotedStringN(value, strlen(value)); +} + +// Class Writer +// ////////////////////////////////////////////////////////////////// +Writer::~Writer() = default; + +// Class FastWriter +// ////////////////////////////////////////////////////////////////// + +FastWriter::FastWriter() + + = default; + +void FastWriter::enableYAMLCompatibility() { yamlCompatibilityEnabled_ = true; } + +void FastWriter::dropNullPlaceholders() { dropNullPlaceholders_ = true; } + +void FastWriter::omitEndingLineFeed() { omitEndingLineFeed_ = true; } + +String FastWriter::write(const Value& root) { + document_.clear(); + writeValue(root); + if (!omitEndingLineFeed_) + document_ += '\n'; + return document_; +} + +void FastWriter::writeValue(const Value& value) { + switch (value.type()) { + case nullValue: + if (!dropNullPlaceholders_) + document_ += "null"; + break; + case intValue: + document_ += valueToString(value.asLargestInt()); + break; + case uintValue: + document_ += valueToString(value.asLargestUInt()); + break; + case realValue: + document_ += valueToString(value.asDouble()); + break; + case stringValue: { + // Is NULL possible for value.string_? No. + char const* str; + char const* end; + bool ok = value.getString(&str, &end); + if (ok) + document_ += valueToQuotedStringN(str, static_cast(end - str)); + break; + } + case booleanValue: + document_ += valueToString(value.asBool()); + break; + case arrayValue: { + document_ += '['; + ArrayIndex size = value.size(); + for (ArrayIndex index = 0; index < size; ++index) { + if (index > 0) + document_ += ','; + writeValue(value[index]); + } + document_ += ']'; + } break; + case objectValue: { + Value::Members members(value.getMemberNames()); + document_ += '{'; + for (auto it = members.begin(); it != members.end(); ++it) { + const String& name = *it; + if (it != members.begin()) + document_ += ','; + document_ += valueToQuotedStringN(name.data(), name.length()); + document_ += yamlCompatibilityEnabled_ ? ": " : ":"; + writeValue(value[name]); + } + document_ += '}'; + } break; + } +} + +// Class StyledWriter +// ////////////////////////////////////////////////////////////////// + +StyledWriter::StyledWriter() = default; + +String StyledWriter::write(const Value& root) { + document_.clear(); + addChildValues_ = false; + indentString_.clear(); + writeCommentBeforeValue(root); + writeValue(root); + writeCommentAfterValueOnSameLine(root); + document_ += '\n'; + return document_; +} + +void StyledWriter::writeValue(const Value& value) { + switch (value.type()) { + case nullValue: + pushValue("null"); + break; + case intValue: + pushValue(valueToString(value.asLargestInt())); + break; + case uintValue: + pushValue(valueToString(value.asLargestUInt())); + break; + case realValue: + pushValue(valueToString(value.asDouble())); + break; + case stringValue: { + // Is NULL possible for value.string_? No. + char const* str; + char const* end; + bool ok = value.getString(&str, &end); + if (ok) + pushValue(valueToQuotedStringN(str, static_cast(end - str))); + else + pushValue(""); + break; + } + case booleanValue: + pushValue(valueToString(value.asBool())); + break; + case arrayValue: + writeArrayValue(value); + break; + case objectValue: { + Value::Members members(value.getMemberNames()); + if (members.empty()) + pushValue("{}"); + else { + writeWithIndent("{"); + indent(); + auto it = members.begin(); + for (;;) { + const String& name = *it; + const Value& childValue = value[name]; + writeCommentBeforeValue(childValue); + writeWithIndent(valueToQuotedString(name.c_str())); + document_ += " : "; + writeValue(childValue); + if (++it == members.end()) { + writeCommentAfterValueOnSameLine(childValue); + break; + } + document_ += ','; + writeCommentAfterValueOnSameLine(childValue); + } + unindent(); + writeWithIndent("}"); + } + } break; + } +} + +void StyledWriter::writeArrayValue(const Value& value) { + size_t size = value.size(); + if (size == 0) + pushValue("[]"); + else { + bool isArrayMultiLine = isMultilineArray(value); + if (isArrayMultiLine) { + writeWithIndent("["); + indent(); + bool hasChildValue = !childValues_.empty(); + ArrayIndex index = 0; + for (;;) { + const Value& childValue = value[index]; + writeCommentBeforeValue(childValue); + if (hasChildValue) + writeWithIndent(childValues_[index]); + else { + writeIndent(); + writeValue(childValue); + } + if (++index == size) { + writeCommentAfterValueOnSameLine(childValue); + break; + } + document_ += ','; + writeCommentAfterValueOnSameLine(childValue); + } + unindent(); + writeWithIndent("]"); + } else // output on a single line + { + assert(childValues_.size() == size); + document_ += "[ "; + for (size_t index = 0; index < size; ++index) { + if (index > 0) + document_ += ", "; + document_ += childValues_[index]; + } + document_ += " ]"; + } + } +} + +bool StyledWriter::isMultilineArray(const Value& value) { + ArrayIndex const size = value.size(); + bool isMultiLine = size * 3 >= rightMargin_; + childValues_.clear(); + for (ArrayIndex index = 0; index < size && !isMultiLine; ++index) { + const Value& childValue = value[index]; + isMultiLine = ((childValue.isArray() || childValue.isObject()) && + !childValue.empty()); + } + if (!isMultiLine) // check if line length > max line length + { + childValues_.reserve(size); + addChildValues_ = true; + ArrayIndex lineLength = 4 + (size - 1) * 2; // '[ ' + ', '*n + ' ]' + for (ArrayIndex index = 0; index < size; ++index) { + if (hasCommentForValue(value[index])) { + isMultiLine = true; + } + writeValue(value[index]); + lineLength += static_cast(childValues_[index].length()); + } + addChildValues_ = false; + isMultiLine = isMultiLine || lineLength >= rightMargin_; + } + return isMultiLine; +} + +void StyledWriter::pushValue(const String& value) { + if (addChildValues_) + childValues_.push_back(value); + else + document_ += value; +} + +void StyledWriter::writeIndent() { + if (!document_.empty()) { + char last = document_[document_.length() - 1]; + if (last == ' ') // already indented + return; + if (last != '\n') // Comments may add new-line + document_ += '\n'; + } + document_ += indentString_; +} + +void StyledWriter::writeWithIndent(const String& value) { + writeIndent(); + document_ += value; +} + +void StyledWriter::indent() { indentString_ += String(indentSize_, ' '); } + +void StyledWriter::unindent() { + assert(indentString_.size() >= indentSize_); + indentString_.resize(indentString_.size() - indentSize_); +} + +void StyledWriter::writeCommentBeforeValue(const Value& root) { + if (!root.hasComment(commentBefore)) + return; + + document_ += '\n'; + writeIndent(); + const String& comment = root.getComment(commentBefore); + String::const_iterator iter = comment.begin(); + while (iter != comment.end()) { + document_ += *iter; + if (*iter == '\n' && ((iter + 1) != comment.end() && *(iter + 1) == '/')) + writeIndent(); + ++iter; + } + + // Comments are stripped of trailing newlines, so add one here + document_ += '\n'; +} + +void StyledWriter::writeCommentAfterValueOnSameLine(const Value& root) { + if (root.hasComment(commentAfterOnSameLine)) + document_ += " " + root.getComment(commentAfterOnSameLine); + + if (root.hasComment(commentAfter)) { + document_ += '\n'; + document_ += root.getComment(commentAfter); + document_ += '\n'; + } +} + +bool StyledWriter::hasCommentForValue(const Value& value) { + return value.hasComment(commentBefore) || + value.hasComment(commentAfterOnSameLine) || + value.hasComment(commentAfter); +} + +// Class StyledStreamWriter +// ////////////////////////////////////////////////////////////////// + +StyledStreamWriter::StyledStreamWriter(String indentation) + : document_(nullptr), indentation_(std::move(indentation)), + addChildValues_(), indented_(false) {} + +void StyledStreamWriter::write(OStream& out, const Value& root) { + document_ = &out; + addChildValues_ = false; + indentString_.clear(); + indented_ = true; + writeCommentBeforeValue(root); + if (!indented_) + writeIndent(); + indented_ = true; + writeValue(root); + writeCommentAfterValueOnSameLine(root); + *document_ << "\n"; + document_ = nullptr; // Forget the stream, for safety. +} + +void StyledStreamWriter::writeValue(const Value& value) { + switch (value.type()) { + case nullValue: + pushValue("null"); + break; + case intValue: + pushValue(valueToString(value.asLargestInt())); + break; + case uintValue: + pushValue(valueToString(value.asLargestUInt())); + break; + case realValue: + pushValue(valueToString(value.asDouble())); + break; + case stringValue: { + // Is NULL possible for value.string_? No. + char const* str; + char const* end; + bool ok = value.getString(&str, &end); + if (ok) + pushValue(valueToQuotedStringN(str, static_cast(end - str))); + else + pushValue(""); + break; + } + case booleanValue: + pushValue(valueToString(value.asBool())); + break; + case arrayValue: + writeArrayValue(value); + break; + case objectValue: { + Value::Members members(value.getMemberNames()); + if (members.empty()) + pushValue("{}"); + else { + writeWithIndent("{"); + indent(); + auto it = members.begin(); + for (;;) { + const String& name = *it; + const Value& childValue = value[name]; + writeCommentBeforeValue(childValue); + writeWithIndent(valueToQuotedString(name.c_str())); + *document_ << " : "; + writeValue(childValue); + if (++it == members.end()) { + writeCommentAfterValueOnSameLine(childValue); + break; + } + *document_ << ","; + writeCommentAfterValueOnSameLine(childValue); + } + unindent(); + writeWithIndent("}"); + } + } break; + } +} + +void StyledStreamWriter::writeArrayValue(const Value& value) { + unsigned size = value.size(); + if (size == 0) + pushValue("[]"); + else { + bool isArrayMultiLine = isMultilineArray(value); + if (isArrayMultiLine) { + writeWithIndent("["); + indent(); + bool hasChildValue = !childValues_.empty(); + unsigned index = 0; + for (;;) { + const Value& childValue = value[index]; + writeCommentBeforeValue(childValue); + if (hasChildValue) + writeWithIndent(childValues_[index]); + else { + if (!indented_) + writeIndent(); + indented_ = true; + writeValue(childValue); + indented_ = false; + } + if (++index == size) { + writeCommentAfterValueOnSameLine(childValue); + break; + } + *document_ << ","; + writeCommentAfterValueOnSameLine(childValue); + } + unindent(); + writeWithIndent("]"); + } else // output on a single line + { + assert(childValues_.size() == size); + *document_ << "[ "; + for (unsigned index = 0; index < size; ++index) { + if (index > 0) + *document_ << ", "; + *document_ << childValues_[index]; + } + *document_ << " ]"; + } + } +} + +bool StyledStreamWriter::isMultilineArray(const Value& value) { + ArrayIndex const size = value.size(); + bool isMultiLine = size * 3 >= rightMargin_; + childValues_.clear(); + for (ArrayIndex index = 0; index < size && !isMultiLine; ++index) { + const Value& childValue = value[index]; + isMultiLine = ((childValue.isArray() || childValue.isObject()) && + !childValue.empty()); + } + if (!isMultiLine) // check if line length > max line length + { + childValues_.reserve(size); + addChildValues_ = true; + ArrayIndex lineLength = 4 + (size - 1) * 2; // '[ ' + ', '*n + ' ]' + for (ArrayIndex index = 0; index < size; ++index) { + if (hasCommentForValue(value[index])) { + isMultiLine = true; + } + writeValue(value[index]); + lineLength += static_cast(childValues_[index].length()); + } + addChildValues_ = false; + isMultiLine = isMultiLine || lineLength >= rightMargin_; + } + return isMultiLine; +} + +void StyledStreamWriter::pushValue(const String& value) { + if (addChildValues_) + childValues_.push_back(value); + else + *document_ << value; +} + +void StyledStreamWriter::writeIndent() { + // blep intended this to look at the so-far-written string + // to determine whether we are already indented, but + // with a stream we cannot do that. So we rely on some saved state. + // The caller checks indented_. + *document_ << '\n' << indentString_; +} + +void StyledStreamWriter::writeWithIndent(const String& value) { + if (!indented_) + writeIndent(); + *document_ << value; + indented_ = false; +} + +void StyledStreamWriter::indent() { indentString_ += indentation_; } + +void StyledStreamWriter::unindent() { + assert(indentString_.size() >= indentation_.size()); + indentString_.resize(indentString_.size() - indentation_.size()); +} + +void StyledStreamWriter::writeCommentBeforeValue(const Value& root) { + if (!root.hasComment(commentBefore)) + return; + + if (!indented_) + writeIndent(); + const String& comment = root.getComment(commentBefore); + String::const_iterator iter = comment.begin(); + while (iter != comment.end()) { + *document_ << *iter; + if (*iter == '\n' && ((iter + 1) != comment.end() && *(iter + 1) == '/')) + // writeIndent(); // would include newline + *document_ << indentString_; + ++iter; + } + indented_ = false; +} + +void StyledStreamWriter::writeCommentAfterValueOnSameLine(const Value& root) { + if (root.hasComment(commentAfterOnSameLine)) + *document_ << ' ' << root.getComment(commentAfterOnSameLine); + + if (root.hasComment(commentAfter)) { + writeIndent(); + *document_ << root.getComment(commentAfter); + } + indented_ = false; +} + +bool StyledStreamWriter::hasCommentForValue(const Value& value) { + return value.hasComment(commentBefore) || + value.hasComment(commentAfterOnSameLine) || + value.hasComment(commentAfter); +} + +////////////////////////// +// BuiltStyledStreamWriter + +/// Scoped enums are not available until C++11. +struct CommentStyle { + /// Decide whether to write comments. + enum Enum { + None, ///< Drop all comments. + Most, ///< Recover odd behavior of previous versions (not implemented yet). + All ///< Keep all comments. + }; +}; + +struct BuiltStyledStreamWriter : public StreamWriter { + BuiltStyledStreamWriter(String indentation, CommentStyle::Enum cs, + String colonSymbol, String nullSymbol, + String endingLineFeedSymbol, bool useSpecialFloats, + bool emitUTF8, unsigned int precision, + PrecisionType precisionType); + int write(Value const& root, OStream* sout) override; + +private: + void writeValue(Value const& value); + void writeArrayValue(Value const& value); + bool isMultilineArray(Value const& value); + void pushValue(String const& value); + void writeIndent(); + void writeWithIndent(String const& value); + void indent(); + void unindent(); + void writeCommentBeforeValue(Value const& root); + void writeCommentAfterValueOnSameLine(Value const& root); + static bool hasCommentForValue(const Value& value); + + using ChildValues = std::vector; + + ChildValues childValues_; + String indentString_; + unsigned int rightMargin_; + String indentation_; + CommentStyle::Enum cs_; + String colonSymbol_; + String nullSymbol_; + String endingLineFeedSymbol_; + bool addChildValues_ : 1; + bool indented_ : 1; + bool useSpecialFloats_ : 1; + bool emitUTF8_ : 1; + unsigned int precision_; + PrecisionType precisionType_; +}; +BuiltStyledStreamWriter::BuiltStyledStreamWriter( + String indentation, CommentStyle::Enum cs, String colonSymbol, + String nullSymbol, String endingLineFeedSymbol, bool useSpecialFloats, + bool emitUTF8, unsigned int precision, PrecisionType precisionType) + : rightMargin_(74), indentation_(std::move(indentation)), cs_(cs), + colonSymbol_(std::move(colonSymbol)), nullSymbol_(std::move(nullSymbol)), + endingLineFeedSymbol_(std::move(endingLineFeedSymbol)), + addChildValues_(false), indented_(false), + useSpecialFloats_(useSpecialFloats), emitUTF8_(emitUTF8), + precision_(precision), precisionType_(precisionType) {} +int BuiltStyledStreamWriter::write(Value const& root, OStream* sout) { + sout_ = sout; + addChildValues_ = false; + indented_ = true; + indentString_.clear(); + writeCommentBeforeValue(root); + if (!indented_) + writeIndent(); + indented_ = true; + writeValue(root); + writeCommentAfterValueOnSameLine(root); + *sout_ << endingLineFeedSymbol_; + sout_ = nullptr; + return 0; +} +void BuiltStyledStreamWriter::writeValue(Value const& value) { + switch (value.type()) { + case nullValue: + pushValue(nullSymbol_); + break; + case intValue: + pushValue(valueToString(value.asLargestInt())); + break; + case uintValue: + pushValue(valueToString(value.asLargestUInt())); + break; + case realValue: + pushValue(valueToString(value.asDouble(), useSpecialFloats_, precision_, + precisionType_)); + break; + case stringValue: { + // Is NULL is possible for value.string_? No. + char const* str; + char const* end; + bool ok = value.getString(&str, &end); + if (ok) + pushValue( + valueToQuotedStringN(str, static_cast(end - str), emitUTF8_)); + else + pushValue(""); + break; + } + case booleanValue: + pushValue(valueToString(value.asBool())); + break; + case arrayValue: + writeArrayValue(value); + break; + case objectValue: { + Value::Members members(value.getMemberNames()); + if (members.empty()) + pushValue("{}"); + else { + writeWithIndent("{"); + indent(); + auto it = members.begin(); + for (;;) { + String const& name = *it; + Value const& childValue = value[name]; + writeCommentBeforeValue(childValue); + writeWithIndent( + valueToQuotedStringN(name.data(), name.length(), emitUTF8_)); + *sout_ << colonSymbol_; + writeValue(childValue); + if (++it == members.end()) { + writeCommentAfterValueOnSameLine(childValue); + break; + } + *sout_ << ","; + writeCommentAfterValueOnSameLine(childValue); + } + unindent(); + writeWithIndent("}"); + } + } break; + } +} + +void BuiltStyledStreamWriter::writeArrayValue(Value const& value) { + unsigned size = value.size(); + if (size == 0) + pushValue("[]"); + else { + bool isMultiLine = (cs_ == CommentStyle::All) || isMultilineArray(value); + if (isMultiLine) { + writeWithIndent("["); + indent(); + bool hasChildValue = !childValues_.empty(); + unsigned index = 0; + for (;;) { + Value const& childValue = value[index]; + writeCommentBeforeValue(childValue); + if (hasChildValue) + writeWithIndent(childValues_[index]); + else { + if (!indented_) + writeIndent(); + indented_ = true; + writeValue(childValue); + indented_ = false; + } + if (++index == size) { + writeCommentAfterValueOnSameLine(childValue); + break; + } + *sout_ << ","; + writeCommentAfterValueOnSameLine(childValue); + } + unindent(); + writeWithIndent("]"); + } else // output on a single line + { + assert(childValues_.size() == size); + *sout_ << "["; + if (!indentation_.empty()) + *sout_ << " "; + for (unsigned index = 0; index < size; ++index) { + if (index > 0) + *sout_ << ((!indentation_.empty()) ? ", " : ","); + *sout_ << childValues_[index]; + } + if (!indentation_.empty()) + *sout_ << " "; + *sout_ << "]"; + } + } +} + +bool BuiltStyledStreamWriter::isMultilineArray(Value const& value) { + ArrayIndex const size = value.size(); + bool isMultiLine = size * 3 >= rightMargin_; + childValues_.clear(); + for (ArrayIndex index = 0; index < size && !isMultiLine; ++index) { + Value const& childValue = value[index]; + isMultiLine = ((childValue.isArray() || childValue.isObject()) && + !childValue.empty()); + } + if (!isMultiLine) // check if line length > max line length + { + childValues_.reserve(size); + addChildValues_ = true; + ArrayIndex lineLength = 4 + (size - 1) * 2; // '[ ' + ', '*n + ' ]' + for (ArrayIndex index = 0; index < size; ++index) { + if (hasCommentForValue(value[index])) { + isMultiLine = true; + } + writeValue(value[index]); + lineLength += static_cast(childValues_[index].length()); + } + addChildValues_ = false; + isMultiLine = isMultiLine || lineLength >= rightMargin_; + } + return isMultiLine; +} + +void BuiltStyledStreamWriter::pushValue(String const& value) { + if (addChildValues_) + childValues_.push_back(value); + else + *sout_ << value; +} + +void BuiltStyledStreamWriter::writeIndent() { + // blep intended this to look at the so-far-written string + // to determine whether we are already indented, but + // with a stream we cannot do that. So we rely on some saved state. + // The caller checks indented_. + + if (!indentation_.empty()) { + // In this case, drop newlines too. + *sout_ << '\n' << indentString_; + } +} + +void BuiltStyledStreamWriter::writeWithIndent(String const& value) { + if (!indented_) + writeIndent(); + *sout_ << value; + indented_ = false; +} + +void BuiltStyledStreamWriter::indent() { indentString_ += indentation_; } + +void BuiltStyledStreamWriter::unindent() { + assert(indentString_.size() >= indentation_.size()); + indentString_.resize(indentString_.size() - indentation_.size()); +} + +void BuiltStyledStreamWriter::writeCommentBeforeValue(Value const& root) { + if (cs_ == CommentStyle::None) + return; + if (!root.hasComment(commentBefore)) + return; + + if (!indented_) + writeIndent(); + const String& comment = root.getComment(commentBefore); + String::const_iterator iter = comment.begin(); + while (iter != comment.end()) { + *sout_ << *iter; + if (*iter == '\n' && ((iter + 1) != comment.end() && *(iter + 1) == '/')) + // writeIndent(); // would write extra newline + *sout_ << indentString_; + ++iter; + } + indented_ = false; +} + +void BuiltStyledStreamWriter::writeCommentAfterValueOnSameLine( + Value const& root) { + if (cs_ == CommentStyle::None) + return; + if (root.hasComment(commentAfterOnSameLine)) + *sout_ << " " + root.getComment(commentAfterOnSameLine); + + if (root.hasComment(commentAfter)) { + writeIndent(); + *sout_ << root.getComment(commentAfter); + } +} + +// static +bool BuiltStyledStreamWriter::hasCommentForValue(const Value& value) { + return value.hasComment(commentBefore) || + value.hasComment(commentAfterOnSameLine) || + value.hasComment(commentAfter); +} + +/////////////// +// StreamWriter + +StreamWriter::StreamWriter() : sout_(nullptr) {} +StreamWriter::~StreamWriter() = default; +StreamWriter::Factory::~Factory() = default; +StreamWriterBuilder::StreamWriterBuilder() { setDefaults(&settings_); } +StreamWriterBuilder::~StreamWriterBuilder() = default; +StreamWriter* StreamWriterBuilder::newStreamWriter() const { + const String indentation = settings_["indentation"].asString(); + const String cs_str = settings_["commentStyle"].asString(); + const String pt_str = settings_["precisionType"].asString(); + const bool eyc = settings_["enableYAMLCompatibility"].asBool(); + const bool dnp = settings_["dropNullPlaceholders"].asBool(); + const bool usf = settings_["useSpecialFloats"].asBool(); + const bool emitUTF8 = settings_["emitUTF8"].asBool(); + unsigned int pre = settings_["precision"].asUInt(); + CommentStyle::Enum cs = CommentStyle::All; + if (cs_str == "All") { + cs = CommentStyle::All; + } else if (cs_str == "None") { + cs = CommentStyle::None; + } else { + throwRuntimeError("commentStyle must be 'All' or 'None'"); + } + PrecisionType precisionType(significantDigits); + if (pt_str == "significant") { + precisionType = PrecisionType::significantDigits; + } else if (pt_str == "decimal") { + precisionType = PrecisionType::decimalPlaces; + } else { + throwRuntimeError("precisionType must be 'significant' or 'decimal'"); + } + String colonSymbol = " : "; + if (eyc) { + colonSymbol = ": "; + } else if (indentation.empty()) { + colonSymbol = ":"; + } + String nullSymbol = "null"; + if (dnp) { + nullSymbol.clear(); + } + if (pre > 17) + pre = 17; + String endingLineFeedSymbol; + return new BuiltStyledStreamWriter(indentation, cs, colonSymbol, nullSymbol, + endingLineFeedSymbol, usf, emitUTF8, pre, + precisionType); +} + +bool StreamWriterBuilder::validate(Json::Value* invalid) const { + static const auto& valid_keys = *new std::set{ + "indentation", + "commentStyle", + "enableYAMLCompatibility", + "dropNullPlaceholders", + "useSpecialFloats", + "emitUTF8", + "precision", + "precisionType", + }; + for (auto si = settings_.begin(); si != settings_.end(); ++si) { + auto key = si.name(); + if (valid_keys.count(key)) + continue; + if (invalid) + (*invalid)[key] = *si; + else + return false; + } + return invalid ? invalid->empty() : true; +} + +Value& StreamWriterBuilder::operator[](const String& key) { + return settings_[key]; +} +// static +void StreamWriterBuilder::setDefaults(Json::Value* settings) { + //! [StreamWriterBuilderDefaults] + (*settings)["commentStyle"] = "All"; + (*settings)["indentation"] = "\t"; + (*settings)["enableYAMLCompatibility"] = false; + (*settings)["dropNullPlaceholders"] = false; + (*settings)["useSpecialFloats"] = false; + (*settings)["emitUTF8"] = false; + (*settings)["precision"] = 17; + (*settings)["precisionType"] = "significant"; + //! [StreamWriterBuilderDefaults] +} + +String writeString(StreamWriter::Factory const& factory, Value const& root) { + OStringStream sout; + StreamWriterPtr const writer(factory.newStreamWriter()); + writer->write(root, &sout); + return sout.str(); +} + +OStream& operator<<(OStream& sout, Value const& root) { + StreamWriterBuilder builder; + StreamWriterPtr const writer(builder.newStreamWriter()); + writer->write(root, &sout); + return sout; +} + +} // namespace Json + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: src/lib_json/json_writer.cpp +// ////////////////////////////////////////////////////////////////////// + + + + + diff --git a/src/pl/CMakeLists.txt b/src/pl/CMakeLists.txt new file mode 100644 index 0000000..f4e3038 --- /dev/null +++ b/src/pl/CMakeLists.txt @@ -0,0 +1,49 @@ +cmake_minimum_required(VERSION 3.5) +project(pl) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sweeper_interfaces REQUIRED) +# find_package(mc REQUIRED) + +include_directories( + include/pl + ${catkin_INCLUDE_DIRS} +) + +add_executable(pl_node src/pl_node.cpp src/pl.cpp src/jsoncpp.cpp) +ament_target_dependencies(pl_node rclcpp std_msgs sweeper_interfaces) + +install(TARGETS + pl_node + DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/pl/include/pl/json.h b/src/pl/include/pl/json.h new file mode 100644 index 0000000..d95fe6e --- /dev/null +++ b/src/pl/include/pl/json.h @@ -0,0 +1,2351 @@ +/// Json-cpp amalgamated header (http://jsoncpp.sourceforge.net/). +/// It is intended to be used with #include "json/json.h" + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: LICENSE +// ////////////////////////////////////////////////////////////////////// + +/* +The JsonCpp library's source code, including accompanying documentation, +tests and demonstration applications, are licensed under the following +conditions... + +Baptiste Lepilleur and The JsonCpp Authors explicitly disclaim copyright in all +jurisdictions which recognize such a disclaimer. In such jurisdictions, +this software is released into the Public Domain. + +In jurisdictions which do not recognize Public Domain property (e.g. Germany as of +2010), this software is Copyright (c) 2007-2010 by Baptiste Lepilleur and +The JsonCpp Authors, and is released under the terms of the MIT License (see below). + +In jurisdictions which recognize Public Domain property, the user of this +software may choose to accept it either as 1) Public Domain, 2) under the +conditions of the MIT License (see below), or 3) under the terms of dual +Public Domain/MIT License conditions described here, as they choose. + +The MIT License is about as close to Public Domain as a license can get, and is +described in clear, concise terms at: + + http://en.wikipedia.org/wiki/MIT_License + +The full text of the MIT License follows: + +======================================================================== +Copyright (c) 2007-2010 Baptiste Lepilleur and The JsonCpp Authors + +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. +======================================================================== +(END LICENSE TEXT) + +The MIT license is compatible with both the GPL and commercial +software, affording one all of the rights of Public Domain with the +minor nuisance of being required to keep the above copyright notice +and license text in the source code. Note also that by accepting the +Public Domain "license" you can re-license your copy using whatever +license you like. + +*/ + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: LICENSE +// ////////////////////////////////////////////////////////////////////// + + + + + +#ifndef JSON_AMALGAMATED_H_INCLUDED +# define JSON_AMALGAMATED_H_INCLUDED +/// If defined, indicates that the source file is amalgamated +/// to prevent private header inclusion. +#define JSON_IS_AMALGAMATION + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: include/json/version.h +// ////////////////////////////////////////////////////////////////////// + +#ifndef JSON_VERSION_H_INCLUDED +#define JSON_VERSION_H_INCLUDED + +// Note: version must be updated in three places when doing a release. This +// annoying process ensures that amalgamate, CMake, and meson all report the +// correct version. +// 1. /meson.build +// 2. /include/json/version.h +// 3. /CMakeLists.txt +// IMPORTANT: also update the SOVERSION!! + +#define JSONCPP_VERSION_STRING "1.9.5" +#define JSONCPP_VERSION_MAJOR 1 +#define JSONCPP_VERSION_MINOR 9 +#define JSONCPP_VERSION_PATCH 5 +#define JSONCPP_VERSION_QUALIFIER +#define JSONCPP_VERSION_HEXA \ + ((JSONCPP_VERSION_MAJOR << 24) | (JSONCPP_VERSION_MINOR << 16) | \ + (JSONCPP_VERSION_PATCH << 8)) + +#ifdef JSONCPP_USING_SECURE_MEMORY +#undef JSONCPP_USING_SECURE_MEMORY +#endif +#define JSONCPP_USING_SECURE_MEMORY 0 +// If non-zero, the library zeroes any memory that it has allocated before +// it frees its memory. + +#endif // JSON_VERSION_H_INCLUDED + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: include/json/version.h +// ////////////////////////////////////////////////////////////////////// + + + + + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: include/json/allocator.h +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2007-2010 Baptiste Lepilleur and The JsonCpp Authors +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +#ifndef JSON_ALLOCATOR_H_INCLUDED +#define JSON_ALLOCATOR_H_INCLUDED + +#include +#include + +#pragma pack(push) +#pragma pack() + +namespace Json { +template class SecureAllocator { +public: + // Type definitions + using value_type = T; + using pointer = T*; + using const_pointer = const T*; + using reference = T&; + using const_reference = const T&; + using size_type = std::size_t; + using difference_type = std::ptrdiff_t; + + /** + * Allocate memory for N items using the standard allocator. + */ + pointer allocate(size_type n) { + // allocate using "global operator new" + return static_cast(::operator new(n * sizeof(T))); + } + + /** + * Release memory which was allocated for N items at pointer P. + * + * The memory block is filled with zeroes before being released. + */ + void deallocate(pointer p, size_type n) { + // memset_s is used because memset may be optimized away by the compiler + memset_s(p, n * sizeof(T), 0, n * sizeof(T)); + // free using "global operator delete" + ::operator delete(p); + } + + /** + * Construct an item in-place at pointer P. + */ + template void construct(pointer p, Args&&... args) { + // construct using "placement new" and "perfect forwarding" + ::new (static_cast(p)) T(std::forward(args)...); + } + + size_type max_size() const { return size_t(-1) / sizeof(T); } + + pointer address(reference x) const { return std::addressof(x); } + + const_pointer address(const_reference x) const { return std::addressof(x); } + + /** + * Destroy an item in-place at pointer P. + */ + void destroy(pointer p) { + // destroy using "explicit destructor" + p->~T(); + } + + // Boilerplate + SecureAllocator() {} + template SecureAllocator(const SecureAllocator&) {} + template struct rebind { using other = SecureAllocator; }; +}; + +template +bool operator==(const SecureAllocator&, const SecureAllocator&) { + return true; +} + +template +bool operator!=(const SecureAllocator&, const SecureAllocator&) { + return false; +} + +} // namespace Json + +#pragma pack(pop) + +#endif // JSON_ALLOCATOR_H_INCLUDED + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: include/json/allocator.h +// ////////////////////////////////////////////////////////////////////// + + + + + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: include/json/config.h +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2007-2010 Baptiste Lepilleur and The JsonCpp Authors +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +#ifndef JSON_CONFIG_H_INCLUDED +#define JSON_CONFIG_H_INCLUDED +#include +#include +#include +#include +#include +#include +#include +#include + +// If non-zero, the library uses exceptions to report bad input instead of C +// assertion macros. The default is to use exceptions. +#ifndef JSON_USE_EXCEPTION +#define JSON_USE_EXCEPTION 1 +#endif + +// Temporary, tracked for removal with issue #982. +#ifndef JSON_USE_NULLREF +#define JSON_USE_NULLREF 1 +#endif + +/// If defined, indicates that the source file is amalgamated +/// to prevent private header inclusion. +/// Remarks: it is automatically defined in the generated amalgamated header. +// #define JSON_IS_AMALGAMATION + +// Export macros for DLL visibility +#if defined(JSON_DLL_BUILD) +#if defined(_MSC_VER) || defined(__MINGW32__) +#define JSON_API __declspec(dllexport) +#define JSONCPP_DISABLE_DLL_INTERFACE_WARNING +#elif defined(__GNUC__) || defined(__clang__) +#define JSON_API __attribute__((visibility("default"))) +#endif // if defined(_MSC_VER) + +#elif defined(JSON_DLL) +#if defined(_MSC_VER) || defined(__MINGW32__) +#define JSON_API __declspec(dllimport) +#define JSONCPP_DISABLE_DLL_INTERFACE_WARNING +#endif // if defined(_MSC_VER) +#endif // ifdef JSON_DLL_BUILD + +#if !defined(JSON_API) +#define JSON_API +#endif + +#if defined(_MSC_VER) && _MSC_VER < 1800 +#error \ + "ERROR: Visual Studio 12 (2013) with _MSC_VER=1800 is the oldest supported compiler with sufficient C++11 capabilities" +#endif + +#if defined(_MSC_VER) && _MSC_VER < 1900 +// As recommended at +// https://stackoverflow.com/questions/2915672/snprintf-and-visual-studio-2010 +extern JSON_API int msvc_pre1900_c99_snprintf(char* outBuf, size_t size, + const char* format, ...); +#define jsoncpp_snprintf msvc_pre1900_c99_snprintf +#else +#define jsoncpp_snprintf std::snprintf +#endif + +// If JSON_NO_INT64 is defined, then Json only support C++ "int" type for +// integer +// Storages, and 64 bits integer support is disabled. +// #define JSON_NO_INT64 1 + +// JSONCPP_OVERRIDE is maintained for backwards compatibility of external tools. +// C++11 should be used directly in JSONCPP. +#define JSONCPP_OVERRIDE override + +#ifdef __clang__ +#if __has_extension(attribute_deprecated_with_message) +#define JSONCPP_DEPRECATED(message) __attribute__((deprecated(message))) +#endif +#elif defined(__GNUC__) // not clang (gcc comes later since clang emulates gcc) +#if (__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 5)) +#define JSONCPP_DEPRECATED(message) __attribute__((deprecated(message))) +#elif (__GNUC__ > 3 || (__GNUC__ == 3 && __GNUC_MINOR__ >= 1)) +#define JSONCPP_DEPRECATED(message) __attribute__((__deprecated__)) +#endif // GNUC version +#elif defined(_MSC_VER) // MSVC (after clang because clang on Windows emulates + // MSVC) +#define JSONCPP_DEPRECATED(message) __declspec(deprecated(message)) +#endif // __clang__ || __GNUC__ || _MSC_VER + +#if !defined(JSONCPP_DEPRECATED) +#define JSONCPP_DEPRECATED(message) +#endif // if !defined(JSONCPP_DEPRECATED) + +#if defined(__clang__) || (defined(__GNUC__) && (__GNUC__ >= 6)) +#define JSON_USE_INT64_DOUBLE_CONVERSION 1 +#endif + +#if !defined(JSON_IS_AMALGAMATION) + +#include "allocator.h" +#include "version.h" + +#endif // if !defined(JSON_IS_AMALGAMATION) + +namespace Json { +using Int = int; +using UInt = unsigned int; +#if defined(JSON_NO_INT64) +using LargestInt = int; +using LargestUInt = unsigned int; +#undef JSON_HAS_INT64 +#else // if defined(JSON_NO_INT64) +// For Microsoft Visual use specific types as long long is not supported +#if defined(_MSC_VER) // Microsoft Visual Studio +using Int64 = __int64; +using UInt64 = unsigned __int64; +#else // if defined(_MSC_VER) // Other platforms, use long long +using Int64 = int64_t; +using UInt64 = uint64_t; +#endif // if defined(_MSC_VER) +using LargestInt = Int64; +using LargestUInt = UInt64; +#define JSON_HAS_INT64 +#endif // if defined(JSON_NO_INT64) + +template +using Allocator = + typename std::conditional, + std::allocator>::type; +using String = std::basic_string, Allocator>; +using IStringStream = + std::basic_istringstream; +using OStringStream = + std::basic_ostringstream; +using IStream = std::istream; +using OStream = std::ostream; +} // namespace Json + +// Legacy names (formerly macros). +using JSONCPP_STRING = Json::String; +using JSONCPP_ISTRINGSTREAM = Json::IStringStream; +using JSONCPP_OSTRINGSTREAM = Json::OStringStream; +using JSONCPP_ISTREAM = Json::IStream; +using JSONCPP_OSTREAM = Json::OStream; + +#endif // JSON_CONFIG_H_INCLUDED + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: include/json/config.h +// ////////////////////////////////////////////////////////////////////// + + + + + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: include/json/forwards.h +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2007-2010 Baptiste Lepilleur and The JsonCpp Authors +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +#ifndef JSON_FORWARDS_H_INCLUDED +#define JSON_FORWARDS_H_INCLUDED + +#if !defined(JSON_IS_AMALGAMATION) +#include "config.h" +#endif // if !defined(JSON_IS_AMALGAMATION) + +namespace Json { + +// writer.h +class StreamWriter; +class StreamWriterBuilder; +class Writer; +class FastWriter; +class StyledWriter; +class StyledStreamWriter; + +// reader.h +class Reader; +class CharReader; +class CharReaderBuilder; + +// json_features.h +class Features; + +// value.h +using ArrayIndex = unsigned int; +class StaticString; +class Path; +class PathArgument; +class Value; +class ValueIteratorBase; +class ValueIterator; +class ValueConstIterator; + +} // namespace Json + +#endif // JSON_FORWARDS_H_INCLUDED + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: include/json/forwards.h +// ////////////////////////////////////////////////////////////////////// + + + + + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: include/json/json_features.h +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2007-2010 Baptiste Lepilleur and The JsonCpp Authors +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +#ifndef JSON_FEATURES_H_INCLUDED +#define JSON_FEATURES_H_INCLUDED + +#if !defined(JSON_IS_AMALGAMATION) +#include "forwards.h" +#endif // if !defined(JSON_IS_AMALGAMATION) + +#pragma pack(push) +#pragma pack() + +namespace Json { + +/** \brief Configuration passed to reader and writer. + * This configuration object can be used to force the Reader or Writer + * to behave in a standard conforming way. + */ +class JSON_API Features { +public: + /** \brief A configuration that allows all features and assumes all strings + * are UTF-8. + * - C & C++ comments are allowed + * - Root object can be any JSON value + * - Assumes Value strings are encoded in UTF-8 + */ + static Features all(); + + /** \brief A configuration that is strictly compatible with the JSON + * specification. + * - Comments are forbidden. + * - Root object must be either an array or an object value. + * - Assumes Value strings are encoded in UTF-8 + */ + static Features strictMode(); + + /** \brief Initialize the configuration like JsonConfig::allFeatures; + */ + Features(); + + /// \c true if comments are allowed. Default: \c true. + bool allowComments_{true}; + + /// \c true if root must be either an array or an object value. Default: \c + /// false. + bool strictRoot_{false}; + + /// \c true if dropped null placeholders are allowed. Default: \c false. + bool allowDroppedNullPlaceholders_{false}; + + /// \c true if numeric object key are allowed. Default: \c false. + bool allowNumericKeys_{false}; +}; + +} // namespace Json + +#pragma pack(pop) + +#endif // JSON_FEATURES_H_INCLUDED + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: include/json/json_features.h +// ////////////////////////////////////////////////////////////////////// + + + + + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: include/json/value.h +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2007-2010 Baptiste Lepilleur and The JsonCpp Authors +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +#ifndef JSON_H_INCLUDED +#define JSON_H_INCLUDED + +#if !defined(JSON_IS_AMALGAMATION) +#include "forwards.h" +#endif // if !defined(JSON_IS_AMALGAMATION) + +// Conditional NORETURN attribute on the throw functions would: +// a) suppress false positives from static code analysis +// b) possibly improve optimization opportunities. +#if !defined(JSONCPP_NORETURN) +#if defined(_MSC_VER) && _MSC_VER == 1800 +#define JSONCPP_NORETURN __declspec(noreturn) +#else +#define JSONCPP_NORETURN [[noreturn]] +#endif +#endif + +// Support for '= delete' with template declarations was a late addition +// to the c++11 standard and is rejected by clang 3.8 and Apple clang 8.2 +// even though these declare themselves to be c++11 compilers. +#if !defined(JSONCPP_TEMPLATE_DELETE) +#if defined(__clang__) && defined(__apple_build_version__) +#if __apple_build_version__ <= 8000042 +#define JSONCPP_TEMPLATE_DELETE +#endif +#elif defined(__clang__) +#if __clang_major__ == 3 && __clang_minor__ <= 8 +#define JSONCPP_TEMPLATE_DELETE +#endif +#endif +#if !defined(JSONCPP_TEMPLATE_DELETE) +#define JSONCPP_TEMPLATE_DELETE = delete +#endif +#endif + +#include +#include +#include +#include +#include +#include + +// Disable warning C4251: : needs to have dll-interface to +// be used by... +#if defined(JSONCPP_DISABLE_DLL_INTERFACE_WARNING) +#pragma warning(push) +#pragma warning(disable : 4251 4275) +#endif // if defined(JSONCPP_DISABLE_DLL_INTERFACE_WARNING) + +#pragma pack(push) +#pragma pack() + +/** \brief JSON (JavaScript Object Notation). + */ +namespace Json { + +#if JSON_USE_EXCEPTION +/** Base class for all exceptions we throw. + * + * We use nothing but these internally. Of course, STL can throw others. + */ +class JSON_API Exception : public std::exception { +public: + Exception(String msg); + ~Exception() noexcept override; + char const* what() const noexcept override; + +protected: + String msg_; +}; + +/** Exceptions which the user cannot easily avoid. + * + * E.g. out-of-memory (when we use malloc), stack-overflow, malicious input + * + * \remark derived from Json::Exception + */ +class JSON_API RuntimeError : public Exception { +public: + RuntimeError(String const& msg); +}; + +/** Exceptions thrown by JSON_ASSERT/JSON_FAIL macros. + * + * These are precondition-violations (user bugs) and internal errors (our bugs). + * + * \remark derived from Json::Exception + */ +class JSON_API LogicError : public Exception { +public: + LogicError(String const& msg); +}; +#endif + +/// used internally +JSONCPP_NORETURN void throwRuntimeError(String const& msg); +/// used internally +JSONCPP_NORETURN void throwLogicError(String const& msg); + +/** \brief Type of the value held by a Value object. + */ +enum ValueType { + nullValue = 0, ///< 'null' value + intValue, ///< signed integer value + uintValue, ///< unsigned integer value + realValue, ///< double value + stringValue, ///< UTF-8 string value + booleanValue, ///< bool value + arrayValue, ///< array value (ordered list) + objectValue ///< object value (collection of name/value pairs). +}; + +enum CommentPlacement { + commentBefore = 0, ///< a comment placed on the line before a value + commentAfterOnSameLine, ///< a comment just after a value on the same line + commentAfter, ///< a comment on the line after a value (only make sense for + /// root value) + numberOfCommentPlacement +}; + +/** \brief Type of precision for formatting of real values. + */ +enum PrecisionType { + significantDigits = 0, ///< we set max number of significant digits in string + decimalPlaces ///< we set max number of digits after "." in string +}; + +/** \brief Lightweight wrapper to tag static string. + * + * Value constructor and objectValue member assignment takes advantage of the + * StaticString and avoid the cost of string duplication when storing the + * string or the member name. + * + * Example of usage: + * \code + * Json::Value aValue( StaticString("some text") ); + * Json::Value object; + * static const StaticString code("code"); + * object[code] = 1234; + * \endcode + */ +class JSON_API StaticString { +public: + explicit StaticString(const char* czstring) : c_str_(czstring) {} + + operator const char*() const { return c_str_; } + + const char* c_str() const { return c_str_; } + +private: + const char* c_str_; +}; + +/** \brief Represents a JSON value. + * + * This class is a discriminated union wrapper that can represents a: + * - signed integer [range: Value::minInt - Value::maxInt] + * - unsigned integer (range: 0 - Value::maxUInt) + * - double + * - UTF-8 string + * - boolean + * - 'null' + * - an ordered list of Value + * - collection of name/value pairs (javascript object) + * + * The type of the held value is represented by a #ValueType and + * can be obtained using type(). + * + * Values of an #objectValue or #arrayValue can be accessed using operator[]() + * methods. + * Non-const methods will automatically create the a #nullValue element + * if it does not exist. + * The sequence of an #arrayValue will be automatically resized and initialized + * with #nullValue. resize() can be used to enlarge or truncate an #arrayValue. + * + * The get() methods can be used to obtain default value in the case the + * required element does not exist. + * + * It is possible to iterate over the list of member keys of an object using + * the getMemberNames() method. + * + * \note #Value string-length fit in size_t, but keys must be < 2^30. + * (The reason is an implementation detail.) A #CharReader will raise an + * exception if a bound is exceeded to avoid security holes in your app, + * but the Value API does *not* check bounds. That is the responsibility + * of the caller. + */ +class JSON_API Value { + friend class ValueIteratorBase; + +public: + using Members = std::vector; + using iterator = ValueIterator; + using const_iterator = ValueConstIterator; + using UInt = Json::UInt; + using Int = Json::Int; +#if defined(JSON_HAS_INT64) + using UInt64 = Json::UInt64; + using Int64 = Json::Int64; +#endif // defined(JSON_HAS_INT64) + using LargestInt = Json::LargestInt; + using LargestUInt = Json::LargestUInt; + using ArrayIndex = Json::ArrayIndex; + + // Required for boost integration, e. g. BOOST_TEST + using value_type = std::string; + +#if JSON_USE_NULLREF + // Binary compatibility kludges, do not use. + static const Value& null; + static const Value& nullRef; +#endif + + // null and nullRef are deprecated, use this instead. + static Value const& nullSingleton(); + + /// Minimum signed integer value that can be stored in a Json::Value. + static constexpr LargestInt minLargestInt = + LargestInt(~(LargestUInt(-1) / 2)); + /// Maximum signed integer value that can be stored in a Json::Value. + static constexpr LargestInt maxLargestInt = LargestInt(LargestUInt(-1) / 2); + /// Maximum unsigned integer value that can be stored in a Json::Value. + static constexpr LargestUInt maxLargestUInt = LargestUInt(-1); + + /// Minimum signed int value that can be stored in a Json::Value. + static constexpr Int minInt = Int(~(UInt(-1) / 2)); + /// Maximum signed int value that can be stored in a Json::Value. + static constexpr Int maxInt = Int(UInt(-1) / 2); + /// Maximum unsigned int value that can be stored in a Json::Value. + static constexpr UInt maxUInt = UInt(-1); + +#if defined(JSON_HAS_INT64) + /// Minimum signed 64 bits int value that can be stored in a Json::Value. + static constexpr Int64 minInt64 = Int64(~(UInt64(-1) / 2)); + /// Maximum signed 64 bits int value that can be stored in a Json::Value. + static constexpr Int64 maxInt64 = Int64(UInt64(-1) / 2); + /// Maximum unsigned 64 bits int value that can be stored in a Json::Value. + static constexpr UInt64 maxUInt64 = UInt64(-1); +#endif // defined(JSON_HAS_INT64) + /// Default precision for real value for string representation. + static constexpr UInt defaultRealPrecision = 17; + // The constant is hard-coded because some compiler have trouble + // converting Value::maxUInt64 to a double correctly (AIX/xlC). + // Assumes that UInt64 is a 64 bits integer. + static constexpr double maxUInt64AsDouble = 18446744073709551615.0; +// Workaround for bug in the NVIDIAs CUDA 9.1 nvcc compiler +// when using gcc and clang backend compilers. CZString +// cannot be defined as private. See issue #486 +#ifdef __NVCC__ +public: +#else +private: +#endif +#ifndef JSONCPP_DOC_EXCLUDE_IMPLEMENTATION + class CZString { + public: + enum DuplicationPolicy { noDuplication = 0, duplicate, duplicateOnCopy }; + CZString(ArrayIndex index); + CZString(char const* str, unsigned length, DuplicationPolicy allocate); + CZString(CZString const& other); + CZString(CZString&& other) noexcept; + ~CZString(); + CZString& operator=(const CZString& other); + CZString& operator=(CZString&& other) noexcept; + + bool operator<(CZString const& other) const; + bool operator==(CZString const& other) const; + ArrayIndex index() const; + // const char* c_str() const; ///< \deprecated + char const* data() const; + unsigned length() const; + bool isStaticString() const; + + private: + void swap(CZString& other); + + struct StringStorage { + unsigned policy_ : 2; + unsigned length_ : 30; // 1GB max + }; + + char const* cstr_; // actually, a prefixed string, unless policy is noDup + union { + ArrayIndex index_; + StringStorage storage_; + }; + }; + +public: + typedef std::map ObjectValues; +#endif // ifndef JSONCPP_DOC_EXCLUDE_IMPLEMENTATION + +public: + /** + * \brief Create a default Value of the given type. + * + * This is a very useful constructor. + * To create an empty array, pass arrayValue. + * To create an empty object, pass objectValue. + * Another Value can then be set to this one by assignment. + * This is useful since clear() and resize() will not alter types. + * + * Examples: + * \code + * Json::Value null_value; // null + * Json::Value arr_value(Json::arrayValue); // [] + * Json::Value obj_value(Json::objectValue); // {} + * \endcode + */ + Value(ValueType type = nullValue); + Value(Int value); + Value(UInt value); +#if defined(JSON_HAS_INT64) + Value(Int64 value); + Value(UInt64 value); +#endif // if defined(JSON_HAS_INT64) + Value(double value); + Value(const char* value); ///< Copy til first 0. (NULL causes to seg-fault.) + Value(const char* begin, const char* end); ///< Copy all, incl zeroes. + /** + * \brief Constructs a value from a static string. + * + * Like other value string constructor but do not duplicate the string for + * internal storage. The given string must remain alive after the call to + * this constructor. + * + * \note This works only for null-terminated strings. (We cannot change the + * size of this class, so we have nowhere to store the length, which might be + * computed later for various operations.) + * + * Example of usage: + * \code + * static StaticString foo("some text"); + * Json::Value aValue(foo); + * \endcode + */ + Value(const StaticString& value); + Value(const String& value); + Value(bool value); + Value(std::nullptr_t ptr) = delete; + Value(const Value& other); + Value(Value&& other) noexcept; + ~Value(); + + /// \note Overwrite existing comments. To preserve comments, use + /// #swapPayload(). + Value& operator=(const Value& other); + Value& operator=(Value&& other) noexcept; + + /// Swap everything. + void swap(Value& other); + /// Swap values but leave comments and source offsets in place. + void swapPayload(Value& other); + + /// copy everything. + void copy(const Value& other); + /// copy values but leave comments and source offsets in place. + void copyPayload(const Value& other); + + ValueType type() const; + + /// Compare payload only, not comments etc. + bool operator<(const Value& other) const; + bool operator<=(const Value& other) const; + bool operator>=(const Value& other) const; + bool operator>(const Value& other) const; + bool operator==(const Value& other) const; + bool operator!=(const Value& other) const; + int compare(const Value& other) const; + + const char* asCString() const; ///< Embedded zeroes could cause you trouble! +#if JSONCPP_USING_SECURE_MEMORY + unsigned getCStringLength() const; // Allows you to understand the length of + // the CString +#endif + String asString() const; ///< Embedded zeroes are possible. + /** Get raw char* of string-value. + * \return false if !string. (Seg-fault if str or end are NULL.) + */ + bool getString(char const** begin, char const** end) const; + Int asInt() const; + UInt asUInt() const; +#if defined(JSON_HAS_INT64) + Int64 asInt64() const; + UInt64 asUInt64() const; +#endif // if defined(JSON_HAS_INT64) + LargestInt asLargestInt() const; + LargestUInt asLargestUInt() const; + float asFloat() const; + double asDouble() const; + bool asBool() const; + + bool isNull() const; + bool isBool() const; + bool isInt() const; + bool isInt64() const; + bool isUInt() const; + bool isUInt64() const; + bool isIntegral() const; + bool isDouble() const; + bool isNumeric() const; + bool isString() const; + bool isArray() const; + bool isObject() const; + + /// The `as` and `is` member function templates and specializations. + template T as() const JSONCPP_TEMPLATE_DELETE; + template bool is() const JSONCPP_TEMPLATE_DELETE; + + bool isConvertibleTo(ValueType other) const; + + /// Number of values in array or object + ArrayIndex size() const; + + /// \brief Return true if empty array, empty object, or null; + /// otherwise, false. + bool empty() const; + + /// Return !isNull() + explicit operator bool() const; + + /// Remove all object members and array elements. + /// \pre type() is arrayValue, objectValue, or nullValue + /// \post type() is unchanged + void clear(); + + /// Resize the array to newSize elements. + /// New elements are initialized to null. + /// May only be called on nullValue or arrayValue. + /// \pre type() is arrayValue or nullValue + /// \post type() is arrayValue + void resize(ArrayIndex newSize); + + ///@{ + /// Access an array element (zero based index). If the array contains less + /// than index element, then null value are inserted in the array so that + /// its size is index+1. + /// (You may need to say 'value[0u]' to get your compiler to distinguish + /// this from the operator[] which takes a string.) + Value& operator[](ArrayIndex index); + Value& operator[](int index); + ///@} + + ///@{ + /// Access an array element (zero based index). + /// (You may need to say 'value[0u]' to get your compiler to distinguish + /// this from the operator[] which takes a string.) + const Value& operator[](ArrayIndex index) const; + const Value& operator[](int index) const; + ///@} + + /// If the array contains at least index+1 elements, returns the element + /// value, otherwise returns defaultValue. + Value get(ArrayIndex index, const Value& defaultValue) const; + /// Return true if index < size(). + bool isValidIndex(ArrayIndex index) const; + /// \brief Append value to array at the end. + /// + /// Equivalent to jsonvalue[jsonvalue.size()] = value; + Value& append(const Value& value); + Value& append(Value&& value); + + /// \brief Insert value in array at specific index + bool insert(ArrayIndex index, const Value& newValue); + bool insert(ArrayIndex index, Value&& newValue); + + /// Access an object value by name, create a null member if it does not exist. + /// \note Because of our implementation, keys are limited to 2^30 -1 chars. + /// Exceeding that will cause an exception. + Value& operator[](const char* key); + /// Access an object value by name, returns null if there is no member with + /// that name. + const Value& operator[](const char* key) const; + /// Access an object value by name, create a null member if it does not exist. + /// \param key may contain embedded nulls. + Value& operator[](const String& key); + /// Access an object value by name, returns null if there is no member with + /// that name. + /// \param key may contain embedded nulls. + const Value& operator[](const String& key) const; + /** \brief Access an object value by name, create a null member if it does not + * exist. + * + * If the object has no entry for that name, then the member name used to + * store the new entry is not duplicated. + * Example of use: + * \code + * Json::Value object; + * static const StaticString code("code"); + * object[code] = 1234; + * \endcode + */ + Value& operator[](const StaticString& key); + /// Return the member named key if it exist, defaultValue otherwise. + /// \note deep copy + Value get(const char* key, const Value& defaultValue) const; + /// Return the member named key if it exist, defaultValue otherwise. + /// \note deep copy + /// \note key may contain embedded nulls. + Value get(const char* begin, const char* end, + const Value& defaultValue) const; + /// Return the member named key if it exist, defaultValue otherwise. + /// \note deep copy + /// \param key may contain embedded nulls. + Value get(const String& key, const Value& defaultValue) const; + /// Most general and efficient version of isMember()const, get()const, + /// and operator[]const + /// \note As stated elsewhere, behavior is undefined if (end-begin) >= 2^30 + Value const* find(char const* begin, char const* end) const; + /// Most general and efficient version of object-mutators. + /// \note As stated elsewhere, behavior is undefined if (end-begin) >= 2^30 + /// \return non-zero, but JSON_ASSERT if this is neither object nor nullValue. + Value* demand(char const* begin, char const* end); + /// \brief Remove and return the named member. + /// + /// Do nothing if it did not exist. + /// \pre type() is objectValue or nullValue + /// \post type() is unchanged + void removeMember(const char* key); + /// Same as removeMember(const char*) + /// \param key may contain embedded nulls. + void removeMember(const String& key); + /// Same as removeMember(const char* begin, const char* end, Value* removed), + /// but 'key' is null-terminated. + bool removeMember(const char* key, Value* removed); + /** \brief Remove the named map member. + * + * Update 'removed' iff removed. + * \param key may contain embedded nulls. + * \return true iff removed (no exceptions) + */ + bool removeMember(String const& key, Value* removed); + /// Same as removeMember(String const& key, Value* removed) + bool removeMember(const char* begin, const char* end, Value* removed); + /** \brief Remove the indexed array element. + * + * O(n) expensive operations. + * Update 'removed' iff removed. + * \return true if removed (no exceptions) + */ + bool removeIndex(ArrayIndex index, Value* removed); + + /// Return true if the object has a member named key. + /// \note 'key' must be null-terminated. + bool isMember(const char* key) const; + /// Return true if the object has a member named key. + /// \param key may contain embedded nulls. + bool isMember(const String& key) const; + /// Same as isMember(String const& key)const + bool isMember(const char* begin, const char* end) const; + + /// \brief Return a list of the member names. + /// + /// If null, return an empty list. + /// \pre type() is objectValue or nullValue + /// \post if type() was nullValue, it remains nullValue + Members getMemberNames() const; + + /// \deprecated Always pass len. + JSONCPP_DEPRECATED("Use setComment(String const&) instead.") + void setComment(const char* comment, CommentPlacement placement) { + setComment(String(comment, strlen(comment)), placement); + } + /// Comments must be //... or /* ... */ + void setComment(const char* comment, size_t len, CommentPlacement placement) { + setComment(String(comment, len), placement); + } + /// Comments must be //... or /* ... */ + void setComment(String comment, CommentPlacement placement); + bool hasComment(CommentPlacement placement) const; + /// Include delimiters and embedded newlines. + String getComment(CommentPlacement placement) const; + + String toStyledString() const; + + const_iterator begin() const; + const_iterator end() const; + + iterator begin(); + iterator end(); + + // Accessors for the [start, limit) range of bytes within the JSON text from + // which this value was parsed, if any. + void setOffsetStart(ptrdiff_t start); + void setOffsetLimit(ptrdiff_t limit); + ptrdiff_t getOffsetStart() const; + ptrdiff_t getOffsetLimit() const; + +private: + void setType(ValueType v) { + bits_.value_type_ = static_cast(v); + } + bool isAllocated() const { return bits_.allocated_; } + void setIsAllocated(bool v) { bits_.allocated_ = v; } + + void initBasic(ValueType type, bool allocated = false); + void dupPayload(const Value& other); + void releasePayload(); + void dupMeta(const Value& other); + + Value& resolveReference(const char* key); + Value& resolveReference(const char* key, const char* end); + + // struct MemberNamesTransform + //{ + // typedef const char *result_type; + // const char *operator()( const CZString &name ) const + // { + // return name.c_str(); + // } + //}; + + union ValueHolder { + LargestInt int_; + LargestUInt uint_; + double real_; + bool bool_; + char* string_; // if allocated_, ptr to { unsigned, char[] }. + ObjectValues* map_; + } value_; + + struct { + // Really a ValueType, but types should agree for bitfield packing. + unsigned int value_type_ : 8; + // Unless allocated_, string_ must be null-terminated. + unsigned int allocated_ : 1; + } bits_; + + class Comments { + public: + Comments() = default; + Comments(const Comments& that); + Comments(Comments&& that) noexcept; + Comments& operator=(const Comments& that); + Comments& operator=(Comments&& that) noexcept; + bool has(CommentPlacement slot) const; + String get(CommentPlacement slot) const; + void set(CommentPlacement slot, String comment); + + private: + using Array = std::array; + std::unique_ptr ptr_; + }; + Comments comments_; + + // [start, limit) byte offsets in the source JSON text from which this Value + // was extracted. + ptrdiff_t start_; + ptrdiff_t limit_; +}; + +template <> inline bool Value::as() const { return asBool(); } +template <> inline bool Value::is() const { return isBool(); } + +template <> inline Int Value::as() const { return asInt(); } +template <> inline bool Value::is() const { return isInt(); } + +template <> inline UInt Value::as() const { return asUInt(); } +template <> inline bool Value::is() const { return isUInt(); } + +#if defined(JSON_HAS_INT64) +template <> inline Int64 Value::as() const { return asInt64(); } +template <> inline bool Value::is() const { return isInt64(); } + +template <> inline UInt64 Value::as() const { return asUInt64(); } +template <> inline bool Value::is() const { return isUInt64(); } +#endif + +template <> inline double Value::as() const { return asDouble(); } +template <> inline bool Value::is() const { return isDouble(); } + +template <> inline String Value::as() const { return asString(); } +template <> inline bool Value::is() const { return isString(); } + +/// These `as` specializations are type conversions, and do not have a +/// corresponding `is`. +template <> inline float Value::as() const { return asFloat(); } +template <> inline const char* Value::as() const { + return asCString(); +} + +/** \brief Experimental and untested: represents an element of the "path" to + * access a node. + */ +class JSON_API PathArgument { +public: + friend class Path; + + PathArgument(); + PathArgument(ArrayIndex index); + PathArgument(const char* key); + PathArgument(String key); + +private: + enum Kind { kindNone = 0, kindIndex, kindKey }; + String key_; + ArrayIndex index_{}; + Kind kind_{kindNone}; +}; + +/** \brief Experimental and untested: represents a "path" to access a node. + * + * Syntax: + * - "." => root node + * - ".[n]" => elements at index 'n' of root node (an array value) + * - ".name" => member named 'name' of root node (an object value) + * - ".name1.name2.name3" + * - ".[0][1][2].name1[3]" + * - ".%" => member name is provided as parameter + * - ".[%]" => index is provided as parameter + */ +class JSON_API Path { +public: + Path(const String& path, const PathArgument& a1 = PathArgument(), + const PathArgument& a2 = PathArgument(), + const PathArgument& a3 = PathArgument(), + const PathArgument& a4 = PathArgument(), + const PathArgument& a5 = PathArgument()); + + const Value& resolve(const Value& root) const; + Value resolve(const Value& root, const Value& defaultValue) const; + /// Creates the "path" to access the specified node and returns a reference on + /// the node. + Value& make(Value& root) const; + +private: + using InArgs = std::vector; + using Args = std::vector; + + void makePath(const String& path, const InArgs& in); + void addPathInArg(const String& path, const InArgs& in, + InArgs::const_iterator& itInArg, PathArgument::Kind kind); + static void invalidPath(const String& path, int location); + + Args args_; +}; + +/** \brief base class for Value iterators. + * + */ +class JSON_API ValueIteratorBase { +public: + using iterator_category = std::bidirectional_iterator_tag; + using size_t = unsigned int; + using difference_type = int; + using SelfType = ValueIteratorBase; + + bool operator==(const SelfType& other) const { return isEqual(other); } + + bool operator!=(const SelfType& other) const { return !isEqual(other); } + + difference_type operator-(const SelfType& other) const { + return other.computeDistance(*this); + } + + /// Return either the index or the member name of the referenced value as a + /// Value. + Value key() const; + + /// Return the index of the referenced Value, or -1 if it is not an + /// arrayValue. + UInt index() const; + + /// Return the member name of the referenced Value, or "" if it is not an + /// objectValue. + /// \note Avoid `c_str()` on result, as embedded zeroes are possible. + String name() const; + + /// Return the member name of the referenced Value. "" if it is not an + /// objectValue. + /// \deprecated This cannot be used for UTF-8 strings, since there can be + /// embedded nulls. + JSONCPP_DEPRECATED("Use `key = name();` instead.") + char const* memberName() const; + /// Return the member name of the referenced Value, or NULL if it is not an + /// objectValue. + /// \note Better version than memberName(). Allows embedded nulls. + char const* memberName(char const** end) const; + +protected: + /*! Internal utility functions to assist with implementing + * other iterator functions. The const and non-const versions + * of the "deref" protected methods expose the protected + * current_ member variable in a way that can often be + * optimized away by the compiler. + */ + const Value& deref() const; + Value& deref(); + + void increment(); + + void decrement(); + + difference_type computeDistance(const SelfType& other) const; + + bool isEqual(const SelfType& other) const; + + void copy(const SelfType& other); + +private: + Value::ObjectValues::iterator current_; + // Indicates that iterator is for a null value. + bool isNull_{true}; + +public: + // For some reason, BORLAND needs these at the end, rather + // than earlier. No idea why. + ValueIteratorBase(); + explicit ValueIteratorBase(const Value::ObjectValues::iterator& current); +}; + +/** \brief const iterator for object and array value. + * + */ +class JSON_API ValueConstIterator : public ValueIteratorBase { + friend class Value; + +public: + using value_type = const Value; + // typedef unsigned int size_t; + // typedef int difference_type; + using reference = const Value&; + using pointer = const Value*; + using SelfType = ValueConstIterator; + + ValueConstIterator(); + ValueConstIterator(ValueIterator const& other); + +private: + /*! \internal Use by Value to create an iterator. + */ + explicit ValueConstIterator(const Value::ObjectValues::iterator& current); + +public: + SelfType& operator=(const ValueIteratorBase& other); + + SelfType operator++(int) { + SelfType temp(*this); + ++*this; + return temp; + } + + SelfType operator--(int) { + SelfType temp(*this); + --*this; + return temp; + } + + SelfType& operator--() { + decrement(); + return *this; + } + + SelfType& operator++() { + increment(); + return *this; + } + + reference operator*() const { return deref(); } + + pointer operator->() const { return &deref(); } +}; + +/** \brief Iterator for object and array value. + */ +class JSON_API ValueIterator : public ValueIteratorBase { + friend class Value; + +public: + using value_type = Value; + using size_t = unsigned int; + using difference_type = int; + using reference = Value&; + using pointer = Value*; + using SelfType = ValueIterator; + + ValueIterator(); + explicit ValueIterator(const ValueConstIterator& other); + ValueIterator(const ValueIterator& other); + +private: + /*! \internal Use by Value to create an iterator. + */ + explicit ValueIterator(const Value::ObjectValues::iterator& current); + +public: + SelfType& operator=(const SelfType& other); + + SelfType operator++(int) { + SelfType temp(*this); + ++*this; + return temp; + } + + SelfType operator--(int) { + SelfType temp(*this); + --*this; + return temp; + } + + SelfType& operator--() { + decrement(); + return *this; + } + + SelfType& operator++() { + increment(); + return *this; + } + + /*! The return value of non-const iterators can be + * changed, so the these functions are not const + * because the returned references/pointers can be used + * to change state of the base class. + */ + reference operator*() const { return const_cast(deref()); } + pointer operator->() const { return const_cast(&deref()); } +}; + +inline void swap(Value& a, Value& b) { a.swap(b); } + +} // namespace Json + +#pragma pack(pop) + +#if defined(JSONCPP_DISABLE_DLL_INTERFACE_WARNING) +#pragma warning(pop) +#endif // if defined(JSONCPP_DISABLE_DLL_INTERFACE_WARNING) + +#endif // JSON_H_INCLUDED + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: include/json/value.h +// ////////////////////////////////////////////////////////////////////// + + + + + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: include/json/reader.h +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2007-2010 Baptiste Lepilleur and The JsonCpp Authors +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +#ifndef JSON_READER_H_INCLUDED +#define JSON_READER_H_INCLUDED + +#if !defined(JSON_IS_AMALGAMATION) +#include "json_features.h" +#include "value.h" +#endif // if !defined(JSON_IS_AMALGAMATION) +#include +#include +#include +#include +#include + +// Disable warning C4251: : needs to have dll-interface to +// be used by... +#if defined(JSONCPP_DISABLE_DLL_INTERFACE_WARNING) +#pragma warning(push) +#pragma warning(disable : 4251) +#endif // if defined(JSONCPP_DISABLE_DLL_INTERFACE_WARNING) + +#pragma pack(push) +#pragma pack() + +namespace Json { + +/** \brief Unserialize a JSON document into a + * Value. + * + * \deprecated Use CharReader and CharReaderBuilder. + */ + +class JSON_API Reader { +public: + using Char = char; + using Location = const Char*; + + /** \brief An error tagged with where in the JSON text it was encountered. + * + * The offsets give the [start, limit) range of bytes within the text. Note + * that this is bytes, not codepoints. + */ + struct StructuredError { + ptrdiff_t offset_start; + ptrdiff_t offset_limit; + String message; + }; + + /** \brief Constructs a Reader allowing all features for parsing. + * \deprecated Use CharReader and CharReaderBuilder. + */ + Reader(); + + /** \brief Constructs a Reader allowing the specified feature set for parsing. + * \deprecated Use CharReader and CharReaderBuilder. + */ + Reader(const Features& features); + + /** \brief Read a Value from a JSON + * document. + * + * \param document UTF-8 encoded string containing the document + * to read. + * \param[out] root Contains the root value of the document if it + * was successfully parsed. + * \param collectComments \c true to collect comment and allow writing + * them back during serialization, \c false to + * discard comments. This parameter is ignored + * if Features::allowComments_ is \c false. + * \return \c true if the document was successfully parsed, \c false if an + * error occurred. + */ + bool parse(const std::string& document, Value& root, + bool collectComments = true); + + /** \brief Read a Value from a JSON + * document. + * + * \param beginDoc Pointer on the beginning of the UTF-8 encoded + * string of the document to read. + * \param endDoc Pointer on the end of the UTF-8 encoded string + * of the document to read. Must be >= beginDoc. + * \param[out] root Contains the root value of the document if it + * was successfully parsed. + * \param collectComments \c true to collect comment and allow writing + * them back during serialization, \c false to + * discard comments. This parameter is ignored + * if Features::allowComments_ is \c false. + * \return \c true if the document was successfully parsed, \c false if an + * error occurred. + */ + bool parse(const char* beginDoc, const char* endDoc, Value& root, + bool collectComments = true); + + /// \brief Parse from input stream. + /// \see Json::operator>>(std::istream&, Json::Value&). + bool parse(IStream& is, Value& root, bool collectComments = true); + + /** \brief Returns a user friendly string that list errors in the parsed + * document. + * + * \return Formatted error message with the list of errors with their + * location in the parsed document. An empty string is returned if no error + * occurred during parsing. + * \deprecated Use getFormattedErrorMessages() instead (typo fix). + */ + JSONCPP_DEPRECATED("Use getFormattedErrorMessages() instead.") + String getFormatedErrorMessages() const; + + /** \brief Returns a user friendly string that list errors in the parsed + * document. + * + * \return Formatted error message with the list of errors with their + * location in the parsed document. An empty string is returned if no error + * occurred during parsing. + */ + String getFormattedErrorMessages() const; + + /** \brief Returns a vector of structured errors encountered while parsing. + * + * \return A (possibly empty) vector of StructuredError objects. Currently + * only one error can be returned, but the caller should tolerate multiple + * errors. This can occur if the parser recovers from a non-fatal parse + * error and then encounters additional errors. + */ + std::vector getStructuredErrors() const; + + /** \brief Add a semantic error message. + * + * \param value JSON Value location associated with the error + * \param message The error message. + * \return \c true if the error was successfully added, \c false if the Value + * offset exceeds the document size. + */ + bool pushError(const Value& value, const String& message); + + /** \brief Add a semantic error message with extra context. + * + * \param value JSON Value location associated with the error + * \param message The error message. + * \param extra Additional JSON Value location to contextualize the error + * \return \c true if the error was successfully added, \c false if either + * Value offset exceeds the document size. + */ + bool pushError(const Value& value, const String& message, const Value& extra); + + /** \brief Return whether there are any errors. + * + * \return \c true if there are no errors to report \c false if errors have + * occurred. + */ + bool good() const; + +private: + enum TokenType { + tokenEndOfStream = 0, + tokenObjectBegin, + tokenObjectEnd, + tokenArrayBegin, + tokenArrayEnd, + tokenString, + tokenNumber, + tokenTrue, + tokenFalse, + tokenNull, + tokenArraySeparator, + tokenMemberSeparator, + tokenComment, + tokenError + }; + + class Token { + public: + TokenType type_; + Location start_; + Location end_; + }; + + class ErrorInfo { + public: + Token token_; + String message_; + Location extra_; + }; + + using Errors = std::deque; + + bool readToken(Token& token); + void skipSpaces(); + bool match(const Char* pattern, int patternLength); + bool readComment(); + bool readCStyleComment(); + bool readCppStyleComment(); + bool readString(); + void readNumber(); + bool readValue(); + bool readObject(Token& token); + bool readArray(Token& token); + bool decodeNumber(Token& token); + bool decodeNumber(Token& token, Value& decoded); + bool decodeString(Token& token); + bool decodeString(Token& token, String& decoded); + bool decodeDouble(Token& token); + bool decodeDouble(Token& token, Value& decoded); + bool decodeUnicodeCodePoint(Token& token, Location& current, Location end, + unsigned int& unicode); + bool decodeUnicodeEscapeSequence(Token& token, Location& current, + Location end, unsigned int& unicode); + bool addError(const String& message, Token& token, Location extra = nullptr); + bool recoverFromError(TokenType skipUntilToken); + bool addErrorAndRecover(const String& message, Token& token, + TokenType skipUntilToken); + void skipUntilSpace(); + Value& currentValue(); + Char getNextChar(); + void getLocationLineAndColumn(Location location, int& line, + int& column) const; + String getLocationLineAndColumn(Location location) const; + void addComment(Location begin, Location end, CommentPlacement placement); + void skipCommentTokens(Token& token); + + static bool containsNewLine(Location begin, Location end); + static String normalizeEOL(Location begin, Location end); + + using Nodes = std::stack; + Nodes nodes_; + Errors errors_; + String document_; + Location begin_{}; + Location end_{}; + Location current_{}; + Location lastValueEnd_{}; + Value* lastValue_{}; + String commentsBefore_; + Features features_; + bool collectComments_{}; +}; // Reader + +/** Interface for reading JSON from a char array. + */ +class JSON_API CharReader { +public: + virtual ~CharReader() = default; + /** \brief Read a Value from a JSON + * document. The document must be a UTF-8 encoded string containing the + * document to read. + * + * \param beginDoc Pointer on the beginning of the UTF-8 encoded string + * of the document to read. + * \param endDoc Pointer on the end of the UTF-8 encoded string of the + * document to read. Must be >= beginDoc. + * \param[out] root Contains the root value of the document if it was + * successfully parsed. + * \param[out] errs Formatted error messages (if not NULL) a user + * friendly string that lists errors in the parsed + * document. + * \return \c true if the document was successfully parsed, \c false if an + * error occurred. + */ + virtual bool parse(char const* beginDoc, char const* endDoc, Value* root, + String* errs) = 0; + + class JSON_API Factory { + public: + virtual ~Factory() = default; + /** \brief Allocate a CharReader via operator new(). + * \throw std::exception if something goes wrong (e.g. invalid settings) + */ + virtual CharReader* newCharReader() const = 0; + }; // Factory +}; // CharReader + +/** \brief Build a CharReader implementation. + * + * Usage: + * \code + * using namespace Json; + * CharReaderBuilder builder; + * builder["collectComments"] = false; + * Value value; + * String errs; + * bool ok = parseFromStream(builder, std::cin, &value, &errs); + * \endcode + */ +class JSON_API CharReaderBuilder : public CharReader::Factory { +public: + // Note: We use a Json::Value so that we can add data-members to this class + // without a major version bump. + /** Configuration of this builder. + * These are case-sensitive. + * Available settings (case-sensitive): + * - `"collectComments": false or true` + * - true to collect comment and allow writing them back during + * serialization, false to discard comments. This parameter is ignored + * if allowComments is false. + * - `"allowComments": false or true` + * - true if comments are allowed. + * - `"allowTrailingCommas": false or true` + * - true if trailing commas in objects and arrays are allowed. + * - `"strictRoot": false or true` + * - true if root must be either an array or an object value + * - `"allowDroppedNullPlaceholders": false or true` + * - true if dropped null placeholders are allowed. (See + * StreamWriterBuilder.) + * - `"allowNumericKeys": false or true` + * - true if numeric object keys are allowed. + * - `"allowSingleQuotes": false or true` + * - true if '' are allowed for strings (both keys and values) + * - `"stackLimit": integer` + * - Exceeding stackLimit (recursive depth of `readValue()`) will cause an + * exception. + * - This is a security issue (seg-faults caused by deeply nested JSON), so + * the default is low. + * - `"failIfExtra": false or true` + * - If true, `parse()` returns false when extra non-whitespace trails the + * JSON value in the input string. + * - `"rejectDupKeys": false or true` + * - If true, `parse()` returns false when a key is duplicated within an + * object. + * - `"allowSpecialFloats": false or true` + * - If true, special float values (NaNs and infinities) are allowed and + * their values are lossfree restorable. + * - `"skipBom": false or true` + * - If true, if the input starts with the Unicode byte order mark (BOM), + * it is skipped. + * + * You can examine 'settings_` yourself to see the defaults. You can also + * write and read them just like any JSON Value. + * \sa setDefaults() + */ + Json::Value settings_; + + CharReaderBuilder(); + ~CharReaderBuilder() override; + + CharReader* newCharReader() const override; + + /** \return true if 'settings' are legal and consistent; + * otherwise, indicate bad settings via 'invalid'. + */ + bool validate(Json::Value* invalid) const; + + /** A simple way to update a specific setting. + */ + Value& operator[](const String& key); + + /** Called by ctor, but you can use this to reset settings_. + * \pre 'settings' != NULL (but Json::null is fine) + * \remark Defaults: + * \snippet src/lib_json/json_reader.cpp CharReaderBuilderDefaults + */ + static void setDefaults(Json::Value* settings); + /** Same as old Features::strictMode(). + * \pre 'settings' != NULL (but Json::null is fine) + * \remark Defaults: + * \snippet src/lib_json/json_reader.cpp CharReaderBuilderStrictMode + */ + static void strictMode(Json::Value* settings); +}; + +/** Consume entire stream and use its begin/end. + * Someday we might have a real StreamReader, but for now this + * is convenient. + */ +bool JSON_API parseFromStream(CharReader::Factory const&, IStream&, Value* root, + String* errs); + +/** \brief Read from 'sin' into 'root'. + * + * Always keep comments from the input JSON. + * + * This can be used to read a file into a particular sub-object. + * For example: + * \code + * Json::Value root; + * cin >> root["dir"]["file"]; + * cout << root; + * \endcode + * Result: + * \verbatim + * { + * "dir": { + * "file": { + * // The input stream JSON would be nested here. + * } + * } + * } + * \endverbatim + * \throw std::exception on parse error. + * \see Json::operator<<() + */ +JSON_API IStream& operator>>(IStream&, Value&); + +} // namespace Json + +#pragma pack(pop) + +#if defined(JSONCPP_DISABLE_DLL_INTERFACE_WARNING) +#pragma warning(pop) +#endif // if defined(JSONCPP_DISABLE_DLL_INTERFACE_WARNING) + +#endif // JSON_READER_H_INCLUDED + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: include/json/reader.h +// ////////////////////////////////////////////////////////////////////// + + + + + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: include/json/writer.h +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2007-2010 Baptiste Lepilleur and The JsonCpp Authors +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +#ifndef JSON_WRITER_H_INCLUDED +#define JSON_WRITER_H_INCLUDED + +#if !defined(JSON_IS_AMALGAMATION) +#include "value.h" +#endif // if !defined(JSON_IS_AMALGAMATION) +#include +#include +#include + +// Disable warning C4251: : needs to have dll-interface to +// be used by... +#if defined(JSONCPP_DISABLE_DLL_INTERFACE_WARNING) && defined(_MSC_VER) +#pragma warning(push) +#pragma warning(disable : 4251) +#endif // if defined(JSONCPP_DISABLE_DLL_INTERFACE_WARNING) + +#pragma pack(push) +#pragma pack() + +namespace Json { + +class Value; + +/** + * + * Usage: + * \code + * using namespace Json; + * void writeToStdout(StreamWriter::Factory const& factory, Value const& value) + * { std::unique_ptr const writer( factory.newStreamWriter()); + * writer->write(value, &std::cout); + * std::cout << std::endl; // add lf and flush + * } + * \endcode + */ +class JSON_API StreamWriter { +protected: + OStream* sout_; // not owned; will not delete +public: + StreamWriter(); + virtual ~StreamWriter(); + /** Write Value into document as configured in sub-class. + * Do not take ownership of sout, but maintain a reference during function. + * \pre sout != NULL + * \return zero on success (For now, we always return zero, so check the + * stream instead.) \throw std::exception possibly, depending on + * configuration + */ + virtual int write(Value const& root, OStream* sout) = 0; + + /** \brief A simple abstract factory. + */ + class JSON_API Factory { + public: + virtual ~Factory(); + /** \brief Allocate a CharReader via operator new(). + * \throw std::exception if something goes wrong (e.g. invalid settings) + */ + virtual StreamWriter* newStreamWriter() const = 0; + }; // Factory +}; // StreamWriter + +/** \brief Write into stringstream, then return string, for convenience. + * A StreamWriter will be created from the factory, used, and then deleted. + */ +String JSON_API writeString(StreamWriter::Factory const& factory, + Value const& root); + +/** \brief Build a StreamWriter implementation. + +* Usage: +* \code +* using namespace Json; +* Value value = ...; +* StreamWriterBuilder builder; +* builder["commentStyle"] = "None"; +* builder["indentation"] = " "; // or whatever you like +* std::unique_ptr writer( +* builder.newStreamWriter()); +* writer->write(value, &std::cout); +* std::cout << std::endl; // add lf and flush +* \endcode +*/ +class JSON_API StreamWriterBuilder : public StreamWriter::Factory { +public: + // Note: We use a Json::Value so that we can add data-members to this class + // without a major version bump. + /** Configuration of this builder. + * Available settings (case-sensitive): + * - "commentStyle": "None" or "All" + * - "indentation": "". + * - Setting this to an empty string also omits newline characters. + * - "enableYAMLCompatibility": false or true + * - slightly change the whitespace around colons + * - "dropNullPlaceholders": false or true + * - Drop the "null" string from the writer's output for nullValues. + * Strictly speaking, this is not valid JSON. But when the output is being + * fed to a browser's JavaScript, it makes for smaller output and the + * browser can handle the output just fine. + * - "useSpecialFloats": false or true + * - If true, outputs non-finite floating point values in the following way: + * NaN values as "NaN", positive infinity as "Infinity", and negative + * infinity as "-Infinity". + * - "precision": int + * - Number of precision digits for formatting of real values. + * - "precisionType": "significant"(default) or "decimal" + * - Type of precision for formatting of real values. + * - "emitUTF8": false or true + * - If true, outputs raw UTF8 strings instead of escaping them. + + * You can examine 'settings_` yourself + * to see the defaults. You can also write and read them just like any + * JSON Value. + * \sa setDefaults() + */ + Json::Value settings_; + + StreamWriterBuilder(); + ~StreamWriterBuilder() override; + + /** + * \throw std::exception if something goes wrong (e.g. invalid settings) + */ + StreamWriter* newStreamWriter() const override; + + /** \return true if 'settings' are legal and consistent; + * otherwise, indicate bad settings via 'invalid'. + */ + bool validate(Json::Value* invalid) const; + /** A simple way to update a specific setting. + */ + Value& operator[](const String& key); + + /** Called by ctor, but you can use this to reset settings_. + * \pre 'settings' != NULL (but Json::null is fine) + * \remark Defaults: + * \snippet src/lib_json/json_writer.cpp StreamWriterBuilderDefaults + */ + static void setDefaults(Json::Value* settings); +}; + +/** \brief Abstract class for writers. + * \deprecated Use StreamWriter. (And really, this is an implementation detail.) + */ +class JSON_API Writer { +public: + virtual ~Writer(); + + virtual String write(const Value& root) = 0; +}; + +/** \brief Outputs a Value in JSON format + *without formatting (not human friendly). + * + * The JSON document is written in a single line. It is not intended for 'human' + *consumption, + * but may be useful to support feature such as RPC where bandwidth is limited. + * \sa Reader, Value + * \deprecated Use StreamWriterBuilder. + */ +#if defined(_MSC_VER) +#pragma warning(push) +#pragma warning(disable : 4996) // Deriving from deprecated class +#endif +class JSON_API FastWriter + : public Writer { +public: + FastWriter(); + ~FastWriter() override = default; + + void enableYAMLCompatibility(); + + /** \brief Drop the "null" string from the writer's output for nullValues. + * Strictly speaking, this is not valid JSON. But when the output is being + * fed to a browser's JavaScript, it makes for smaller output and the + * browser can handle the output just fine. + */ + void dropNullPlaceholders(); + + void omitEndingLineFeed(); + +public: // overridden from Writer + String write(const Value& root) override; + +private: + void writeValue(const Value& value); + + String document_; + bool yamlCompatibilityEnabled_{false}; + bool dropNullPlaceholders_{false}; + bool omitEndingLineFeed_{false}; +}; +#if defined(_MSC_VER) +#pragma warning(pop) +#endif + +/** \brief Writes a Value in JSON format in a + *human friendly way. + * + * The rules for line break and indent are as follow: + * - Object value: + * - if empty then print {} without indent and line break + * - if not empty the print '{', line break & indent, print one value per + *line + * and then unindent and line break and print '}'. + * - Array value: + * - if empty then print [] without indent and line break + * - if the array contains no object value, empty array or some other value + *types, + * and all the values fit on one lines, then print the array on a single + *line. + * - otherwise, it the values do not fit on one line, or the array contains + * object or non empty array, then print one value per line. + * + * If the Value have comments then they are outputted according to their + *#CommentPlacement. + * + * \sa Reader, Value, Value::setComment() + * \deprecated Use StreamWriterBuilder. + */ +#if defined(_MSC_VER) +#pragma warning(push) +#pragma warning(disable : 4996) // Deriving from deprecated class +#endif +class JSON_API + StyledWriter : public Writer { +public: + StyledWriter(); + ~StyledWriter() override = default; + +public: // overridden from Writer + /** \brief Serialize a Value in JSON format. + * \param root Value to serialize. + * \return String containing the JSON document that represents the root value. + */ + String write(const Value& root) override; + +private: + void writeValue(const Value& value); + void writeArrayValue(const Value& value); + bool isMultilineArray(const Value& value); + void pushValue(const String& value); + void writeIndent(); + void writeWithIndent(const String& value); + void indent(); + void unindent(); + void writeCommentBeforeValue(const Value& root); + void writeCommentAfterValueOnSameLine(const Value& root); + static bool hasCommentForValue(const Value& value); + static String normalizeEOL(const String& text); + + using ChildValues = std::vector; + + ChildValues childValues_; + String document_; + String indentString_; + unsigned int rightMargin_{74}; + unsigned int indentSize_{3}; + bool addChildValues_{false}; +}; +#if defined(_MSC_VER) +#pragma warning(pop) +#endif + +/** \brief Writes a Value in JSON format in a + human friendly way, + to a stream rather than to a string. + * + * The rules for line break and indent are as follow: + * - Object value: + * - if empty then print {} without indent and line break + * - if not empty the print '{', line break & indent, print one value per + line + * and then unindent and line break and print '}'. + * - Array value: + * - if empty then print [] without indent and line break + * - if the array contains no object value, empty array or some other value + types, + * and all the values fit on one lines, then print the array on a single + line. + * - otherwise, it the values do not fit on one line, or the array contains + * object or non empty array, then print one value per line. + * + * If the Value have comments then they are outputted according to their + #CommentPlacement. + * + * \sa Reader, Value, Value::setComment() + * \deprecated Use StreamWriterBuilder. + */ +#if defined(_MSC_VER) +#pragma warning(push) +#pragma warning(disable : 4996) // Deriving from deprecated class +#endif +class JSON_API + StyledStreamWriter { +public: + /** + * \param indentation Each level will be indented by this amount extra. + */ + StyledStreamWriter(String indentation = "\t"); + ~StyledStreamWriter() = default; + +public: + /** \brief Serialize a Value in JSON format. + * \param out Stream to write to. (Can be ostringstream, e.g.) + * \param root Value to serialize. + * \note There is no point in deriving from Writer, since write() should not + * return a value. + */ + void write(OStream& out, const Value& root); + +private: + void writeValue(const Value& value); + void writeArrayValue(const Value& value); + bool isMultilineArray(const Value& value); + void pushValue(const String& value); + void writeIndent(); + void writeWithIndent(const String& value); + void indent(); + void unindent(); + void writeCommentBeforeValue(const Value& root); + void writeCommentAfterValueOnSameLine(const Value& root); + static bool hasCommentForValue(const Value& value); + static String normalizeEOL(const String& text); + + using ChildValues = std::vector; + + ChildValues childValues_; + OStream* document_; + String indentString_; + unsigned int rightMargin_{74}; + String indentation_; + bool addChildValues_ : 1; + bool indented_ : 1; +}; +#if defined(_MSC_VER) +#pragma warning(pop) +#endif + +#if defined(JSON_HAS_INT64) +String JSON_API valueToString(Int value); +String JSON_API valueToString(UInt value); +#endif // if defined(JSON_HAS_INT64) +String JSON_API valueToString(LargestInt value); +String JSON_API valueToString(LargestUInt value); +String JSON_API valueToString( + double value, unsigned int precision = Value::defaultRealPrecision, + PrecisionType precisionType = PrecisionType::significantDigits); +String JSON_API valueToString(bool value); +String JSON_API valueToQuotedString(const char* value); + +/// \brief Output using the StyledStreamWriter. +/// \see Json::operator>>() +JSON_API OStream& operator<<(OStream&, const Value& root); + +} // namespace Json + +#pragma pack(pop) + +#if defined(JSONCPP_DISABLE_DLL_INTERFACE_WARNING) +#pragma warning(pop) +#endif // if defined(JSONCPP_DISABLE_DLL_INTERFACE_WARNING) + +#endif // JSON_WRITER_H_INCLUDED + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: include/json/writer.h +// ////////////////////////////////////////////////////////////////////// + + + + + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: include/json/assertions.h +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2007-2010 Baptiste Lepilleur and The JsonCpp Authors +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +#ifndef JSON_ASSERTIONS_H_INCLUDED +#define JSON_ASSERTIONS_H_INCLUDED + +#include +#include + +#if !defined(JSON_IS_AMALGAMATION) +#include "config.h" +#endif // if !defined(JSON_IS_AMALGAMATION) + +/** It should not be possible for a maliciously designed file to + * cause an abort() or seg-fault, so these macros are used only + * for pre-condition violations and internal logic errors. + */ +#if JSON_USE_EXCEPTION + +// @todo <= add detail about condition in exception +#define JSON_ASSERT(condition) \ + do { \ + if (!(condition)) { \ + Json::throwLogicError("assert json failed"); \ + } \ + } while (0) + +#define JSON_FAIL_MESSAGE(message) \ + do { \ + OStringStream oss; \ + oss << message; \ + Json::throwLogicError(oss.str()); \ + abort(); \ + } while (0) + +#else // JSON_USE_EXCEPTION + +#define JSON_ASSERT(condition) assert(condition) + +// The call to assert() will show the failure message in debug builds. In +// release builds we abort, for a core-dump or debugger. +#define JSON_FAIL_MESSAGE(message) \ + { \ + OStringStream oss; \ + oss << message; \ + assert(false && oss.str().c_str()); \ + abort(); \ + } + +#endif + +#define JSON_ASSERT_MESSAGE(condition, message) \ + do { \ + if (!(condition)) { \ + JSON_FAIL_MESSAGE(message); \ + } \ + } while (0) + +#endif // JSON_ASSERTIONS_H_INCLUDED + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: include/json/assertions.h +// ////////////////////////////////////////////////////////////////////// + + + + + +#endif //ifndef JSON_AMALGAMATED_H_INCLUDED diff --git a/src/pl/include/pl/pl.hpp b/src/pl/include/pl/pl.hpp new file mode 100644 index 0000000..8e588f2 --- /dev/null +++ b/src/pl/include/pl/pl.hpp @@ -0,0 +1,73 @@ +#ifndef __PL_H__ +#define __PL_H__ + +#include + +enum Direction +{ + STRAIGHT_D, + LEFT_TURN, + RIGHT_TURN +}; + +// 任务状态定义 +enum TaskStatus +{ + PENDING = 0, // 待执行 + RUNNING = 1, // 执行中 + COMPLETED = 2, // 执行完成 + FAILED = 3, // 执行异常 +}; + +#pragma pack(push) +#pragma pack(1) + +/* 存储寻迹路线的GPS点信息 */ +typedef struct tagWRC_GPS_Point +{ + double LatitudeInfo; // 纬度信息 + double LongitudeInfo; // 经度信息 + double Heading; // 航向角 + int idealSpeed; // 建议车速 +} WRC_GPS_Point; + +/* 存储目标位置GPS点信息 */ +typedef struct tagWRC_MC_NAV_INFO +{ + double Latitude_degree; // 单位:度 + double Longitude_degree; // 单位:度 + + float Yaw_rad; // 单位:弧度 +} WRC_MC_NAV_INFO; + +/* 存储接收RTK的GPS点信息 */ +struct struct_rtk +{ + double lon; + double lat; + double direction; + double speed; + int reliability; +}; + +#pragma pack(pop) + +extern WRC_MC_NAV_INFO g_NavInfo; +extern struct_rtk g_rtk; + +void *pl_thread(void *args); + +extern double x; +extern double y; +extern int pl_speed; +// 在头文件中添加外部声明 +extern float nl_radius; +extern int nl_within_radius; + +extern int mode; +extern int last_mode; +extern std::string filename; +extern int d_file; +extern Direction drive_mode; +extern TaskStatus task_status; +#endif \ No newline at end of file diff --git a/src/pl/package.xml b/src/pl/package.xml new file mode 100644 index 0000000..4c8f22b --- /dev/null +++ b/src/pl/package.xml @@ -0,0 +1,23 @@ + + + + pl + 0.0.0 + TODO: Package description + zxwl + TODO: License declaration + + ament_cmake + + rclcpp + std_msgs + sweeper_interfaces + mc + + ament_lint_auto + ament_lint_common + + + ament_cmake + + \ No newline at end of file diff --git a/src/pl/src/jsoncpp.cpp b/src/pl/src/jsoncpp.cpp new file mode 100644 index 0000000..89b7bc0 --- /dev/null +++ b/src/pl/src/jsoncpp.cpp @@ -0,0 +1,5342 @@ +/// Json-cpp amalgamated source (http://jsoncpp.sourceforge.net/). +/// It is intended to be used with #include "json/json.h" + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: LICENSE +// ////////////////////////////////////////////////////////////////////// + +/* +The JsonCpp library's source code, including accompanying documentation, +tests and demonstration applications, are licensed under the following +conditions... + +Baptiste Lepilleur and The JsonCpp Authors explicitly disclaim copyright in all +jurisdictions which recognize such a disclaimer. In such jurisdictions, +this software is released into the Public Domain. + +In jurisdictions which do not recognize Public Domain property (e.g. Germany as of +2010), this software is Copyright (c) 2007-2010 by Baptiste Lepilleur and +The JsonCpp Authors, and is released under the terms of the MIT License (see below). + +In jurisdictions which recognize Public Domain property, the user of this +software may choose to accept it either as 1) Public Domain, 2) under the +conditions of the MIT License (see below), or 3) under the terms of dual +Public Domain/MIT License conditions described here, as they choose. + +The MIT License is about as close to Public Domain as a license can get, and is +described in clear, concise terms at: + + http://en.wikipedia.org/wiki/MIT_License + +The full text of the MIT License follows: + +======================================================================== +Copyright (c) 2007-2010 Baptiste Lepilleur and The JsonCpp Authors + +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. +======================================================================== +(END LICENSE TEXT) + +The MIT license is compatible with both the GPL and commercial +software, affording one all of the rights of Public Domain with the +minor nuisance of being required to keep the above copyright notice +and license text in the source code. Note also that by accepting the +Public Domain "license" you can re-license your copy using whatever +license you like. + +*/ + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: LICENSE +// ////////////////////////////////////////////////////////////////////// + + + + + + +#include "json.h" + +#ifndef JSON_IS_AMALGAMATION +#error "Compile with -I PATH_TO_JSON_DIRECTORY" +#endif + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: src/lib_json/json_tool.h +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2007-2010 Baptiste Lepilleur and The JsonCpp Authors +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +#ifndef LIB_JSONCPP_JSON_TOOL_H_INCLUDED +#define LIB_JSONCPP_JSON_TOOL_H_INCLUDED + +#if !defined(JSON_IS_AMALGAMATION) +#include +#endif + +// Also support old flag NO_LOCALE_SUPPORT +#ifdef NO_LOCALE_SUPPORT +#define JSONCPP_NO_LOCALE_SUPPORT +#endif + +#ifndef JSONCPP_NO_LOCALE_SUPPORT +#include +#endif + +/* This header provides common string manipulation support, such as UTF-8, + * portable conversion from/to string... + * + * It is an internal header that must not be exposed. + */ + +namespace Json { +static inline char getDecimalPoint() { +#ifdef JSONCPP_NO_LOCALE_SUPPORT + return '\0'; +#else + struct lconv* lc = localeconv(); + return lc ? *(lc->decimal_point) : '\0'; +#endif +} + +/// Converts a unicode code-point to UTF-8. +static inline String codePointToUTF8(unsigned int cp) { + String result; + + // based on description from http://en.wikipedia.org/wiki/UTF-8 + + if (cp <= 0x7f) { + result.resize(1); + result[0] = static_cast(cp); + } else if (cp <= 0x7FF) { + result.resize(2); + result[1] = static_cast(0x80 | (0x3f & cp)); + result[0] = static_cast(0xC0 | (0x1f & (cp >> 6))); + } else if (cp <= 0xFFFF) { + result.resize(3); + result[2] = static_cast(0x80 | (0x3f & cp)); + result[1] = static_cast(0x80 | (0x3f & (cp >> 6))); + result[0] = static_cast(0xE0 | (0xf & (cp >> 12))); + } else if (cp <= 0x10FFFF) { + result.resize(4); + result[3] = static_cast(0x80 | (0x3f & cp)); + result[2] = static_cast(0x80 | (0x3f & (cp >> 6))); + result[1] = static_cast(0x80 | (0x3f & (cp >> 12))); + result[0] = static_cast(0xF0 | (0x7 & (cp >> 18))); + } + + return result; +} + +enum { + /// Constant that specify the size of the buffer that must be passed to + /// uintToString. + uintToStringBufferSize = 3 * sizeof(LargestUInt) + 1 +}; + +// Defines a char buffer for use with uintToString(). +using UIntToStringBuffer = char[uintToStringBufferSize]; + +/** Converts an unsigned integer to string. + * @param value Unsigned integer to convert to string + * @param current Input/Output string buffer. + * Must have at least uintToStringBufferSize chars free. + */ +static inline void uintToString(LargestUInt value, char*& current) { + *--current = 0; + do { + *--current = static_cast(value % 10U + static_cast('0')); + value /= 10; + } while (value != 0); +} + +/** Change ',' to '.' everywhere in buffer. + * + * We had a sophisticated way, but it did not work in WinCE. + * @see https://github.com/open-source-parsers/jsoncpp/pull/9 + */ +template Iter fixNumericLocale(Iter begin, Iter end) { + for (; begin != end; ++begin) { + if (*begin == ',') { + *begin = '.'; + } + } + return begin; +} + +template void fixNumericLocaleInput(Iter begin, Iter end) { + char decimalPoint = getDecimalPoint(); + if (decimalPoint == '\0' || decimalPoint == '.') { + return; + } + for (; begin != end; ++begin) { + if (*begin == '.') { + *begin = decimalPoint; + } + } +} + +/** + * Return iterator that would be the new end of the range [begin,end), if we + * were to delete zeros in the end of string, but not the last zero before '.'. + */ +template +Iter fixZerosInTheEnd(Iter begin, Iter end, unsigned int precision) { + for (; begin != end; --end) { + if (*(end - 1) != '0') { + return end; + } + // Don't delete the last zero before the decimal point. + if (begin != (end - 1) && begin != (end - 2) && *(end - 2) == '.') { + if (precision) { + return end; + } + return end - 2; + } + } + return end; +} + +} // namespace Json + +#endif // LIB_JSONCPP_JSON_TOOL_H_INCLUDED + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: src/lib_json/json_tool.h +// ////////////////////////////////////////////////////////////////////// + + + + + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: src/lib_json/json_reader.cpp +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2007-2011 Baptiste Lepilleur and The JsonCpp Authors +// Copyright (C) 2016 InfoTeCS JSC. All rights reserved. +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +#if !defined(JSON_IS_AMALGAMATION) +#include "json_tool.h" +#include +#include +#include +#endif // if !defined(JSON_IS_AMALGAMATION) +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#if __cplusplus >= 201103L + +#if !defined(sscanf) +#define sscanf std::sscanf +#endif + +#endif //__cplusplus + +#if defined(_MSC_VER) +#if !defined(_CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES) +#define _CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES 1 +#endif //_CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES +#endif //_MSC_VER + +#if defined(_MSC_VER) +// Disable warning about strdup being deprecated. +#pragma warning(disable : 4996) +#endif + +// Define JSONCPP_DEPRECATED_STACK_LIMIT as an appropriate integer at compile +// time to change the stack limit +#if !defined(JSONCPP_DEPRECATED_STACK_LIMIT) +#define JSONCPP_DEPRECATED_STACK_LIMIT 1000 +#endif + +static size_t const stackLimit_g = + JSONCPP_DEPRECATED_STACK_LIMIT; // see readValue() + +namespace Json { + +#if __cplusplus >= 201103L || (defined(_CPPLIB_VER) && _CPPLIB_VER >= 520) +using CharReaderPtr = std::unique_ptr; +#else +using CharReaderPtr = std::auto_ptr; +#endif + +// Implementation of class Features +// //////////////////////////////// + +Features::Features() = default; + +Features Features::all() { return {}; } + +Features Features::strictMode() { + Features features; + features.allowComments_ = false; + features.strictRoot_ = true; + features.allowDroppedNullPlaceholders_ = false; + features.allowNumericKeys_ = false; + return features; +} + +// Implementation of class Reader +// //////////////////////////////// + +bool Reader::containsNewLine(Reader::Location begin, Reader::Location end) { + return std::any_of(begin, end, [](char b) { return b == '\n' || b == '\r'; }); +} + +// Class Reader +// ////////////////////////////////////////////////////////////////// + +Reader::Reader() : features_(Features::all()) {} + +Reader::Reader(const Features& features) : features_(features) {} + +bool Reader::parse(const std::string& document, Value& root, + bool collectComments) { + document_.assign(document.begin(), document.end()); + const char* begin = document_.c_str(); + const char* end = begin + document_.length(); + return parse(begin, end, root, collectComments); +} + +bool Reader::parse(std::istream& is, Value& root, bool collectComments) { + // std::istream_iterator begin(is); + // std::istream_iterator end; + // Those would allow streamed input from a file, if parse() were a + // template function. + + // Since String is reference-counted, this at least does not + // create an extra copy. + String doc(std::istreambuf_iterator(is), {}); + return parse(doc.data(), doc.data() + doc.size(), root, collectComments); +} + +bool Reader::parse(const char* beginDoc, const char* endDoc, Value& root, + bool collectComments) { + if (!features_.allowComments_) { + collectComments = false; + } + + begin_ = beginDoc; + end_ = endDoc; + collectComments_ = collectComments; + current_ = begin_; + lastValueEnd_ = nullptr; + lastValue_ = nullptr; + commentsBefore_.clear(); + errors_.clear(); + while (!nodes_.empty()) + nodes_.pop(); + nodes_.push(&root); + + bool successful = readValue(); + Token token; + skipCommentTokens(token); + if (collectComments_ && !commentsBefore_.empty()) + root.setComment(commentsBefore_, commentAfter); + if (features_.strictRoot_) { + if (!root.isArray() && !root.isObject()) { + // Set error location to start of doc, ideally should be first token found + // in doc + token.type_ = tokenError; + token.start_ = beginDoc; + token.end_ = endDoc; + addError( + "A valid JSON document must be either an array or an object value.", + token); + return false; + } + } + return successful; +} + +bool Reader::readValue() { + // readValue() may call itself only if it calls readObject() or ReadArray(). + // These methods execute nodes_.push() just before and nodes_.pop)() just + // after calling readValue(). parse() executes one nodes_.push(), so > instead + // of >=. + if (nodes_.size() > stackLimit_g) + throwRuntimeError("Exceeded stackLimit in readValue()."); + + Token token; + skipCommentTokens(token); + bool successful = true; + + if (collectComments_ && !commentsBefore_.empty()) { + currentValue().setComment(commentsBefore_, commentBefore); + commentsBefore_.clear(); + } + + switch (token.type_) { + case tokenObjectBegin: + successful = readObject(token); + currentValue().setOffsetLimit(current_ - begin_); + break; + case tokenArrayBegin: + successful = readArray(token); + currentValue().setOffsetLimit(current_ - begin_); + break; + case tokenNumber: + successful = decodeNumber(token); + break; + case tokenString: + successful = decodeString(token); + break; + case tokenTrue: { + Value v(true); + currentValue().swapPayload(v); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + } break; + case tokenFalse: { + Value v(false); + currentValue().swapPayload(v); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + } break; + case tokenNull: { + Value v; + currentValue().swapPayload(v); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + } break; + case tokenArraySeparator: + case tokenObjectEnd: + case tokenArrayEnd: + if (features_.allowDroppedNullPlaceholders_) { + // "Un-read" the current token and mark the current value as a null + // token. + current_--; + Value v; + currentValue().swapPayload(v); + currentValue().setOffsetStart(current_ - begin_ - 1); + currentValue().setOffsetLimit(current_ - begin_); + break; + } // Else, fall through... + default: + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + return addError("Syntax error: value, object or array expected.", token); + } + + if (collectComments_) { + lastValueEnd_ = current_; + lastValue_ = ¤tValue(); + } + + return successful; +} + +void Reader::skipCommentTokens(Token& token) { + if (features_.allowComments_) { + do { + readToken(token); + } while (token.type_ == tokenComment); + } else { + readToken(token); + } +} + +bool Reader::readToken(Token& token) { + skipSpaces(); + token.start_ = current_; + Char c = getNextChar(); + bool ok = true; + switch (c) { + case '{': + token.type_ = tokenObjectBegin; + break; + case '}': + token.type_ = tokenObjectEnd; + break; + case '[': + token.type_ = tokenArrayBegin; + break; + case ']': + token.type_ = tokenArrayEnd; + break; + case '"': + token.type_ = tokenString; + ok = readString(); + break; + case '/': + token.type_ = tokenComment; + ok = readComment(); + break; + case '0': + case '1': + case '2': + case '3': + case '4': + case '5': + case '6': + case '7': + case '8': + case '9': + case '-': + token.type_ = tokenNumber; + readNumber(); + break; + case 't': + token.type_ = tokenTrue; + ok = match("rue", 3); + break; + case 'f': + token.type_ = tokenFalse; + ok = match("alse", 4); + break; + case 'n': + token.type_ = tokenNull; + ok = match("ull", 3); + break; + case ',': + token.type_ = tokenArraySeparator; + break; + case ':': + token.type_ = tokenMemberSeparator; + break; + case 0: + token.type_ = tokenEndOfStream; + break; + default: + ok = false; + break; + } + if (!ok) + token.type_ = tokenError; + token.end_ = current_; + return ok; +} + +void Reader::skipSpaces() { + while (current_ != end_) { + Char c = *current_; + if (c == ' ' || c == '\t' || c == '\r' || c == '\n') + ++current_; + else + break; + } +} + +bool Reader::match(const Char* pattern, int patternLength) { + if (end_ - current_ < patternLength) + return false; + int index = patternLength; + while (index--) + if (current_[index] != pattern[index]) + return false; + current_ += patternLength; + return true; +} + +bool Reader::readComment() { + Location commentBegin = current_ - 1; + Char c = getNextChar(); + bool successful = false; + if (c == '*') + successful = readCStyleComment(); + else if (c == '/') + successful = readCppStyleComment(); + if (!successful) + return false; + + if (collectComments_) { + CommentPlacement placement = commentBefore; + if (lastValueEnd_ && !containsNewLine(lastValueEnd_, commentBegin)) { + if (c != '*' || !containsNewLine(commentBegin, current_)) + placement = commentAfterOnSameLine; + } + + addComment(commentBegin, current_, placement); + } + return true; +} + +String Reader::normalizeEOL(Reader::Location begin, Reader::Location end) { + String normalized; + normalized.reserve(static_cast(end - begin)); + Reader::Location current = begin; + while (current != end) { + char c = *current++; + if (c == '\r') { + if (current != end && *current == '\n') + // convert dos EOL + ++current; + // convert Mac EOL + normalized += '\n'; + } else { + normalized += c; + } + } + return normalized; +} + +void Reader::addComment(Location begin, Location end, + CommentPlacement placement) { + assert(collectComments_); + const String& normalized = normalizeEOL(begin, end); + if (placement == commentAfterOnSameLine) { + assert(lastValue_ != nullptr); + lastValue_->setComment(normalized, placement); + } else { + commentsBefore_ += normalized; + } +} + +bool Reader::readCStyleComment() { + while ((current_ + 1) < end_) { + Char c = getNextChar(); + if (c == '*' && *current_ == '/') + break; + } + return getNextChar() == '/'; +} + +bool Reader::readCppStyleComment() { + while (current_ != end_) { + Char c = getNextChar(); + if (c == '\n') + break; + if (c == '\r') { + // Consume DOS EOL. It will be normalized in addComment. + if (current_ != end_ && *current_ == '\n') + getNextChar(); + // Break on Moc OS 9 EOL. + break; + } + } + return true; +} + +void Reader::readNumber() { + Location p = current_; + char c = '0'; // stopgap for already consumed character + // integral part + while (c >= '0' && c <= '9') + c = (current_ = p) < end_ ? *p++ : '\0'; + // fractional part + if (c == '.') { + c = (current_ = p) < end_ ? *p++ : '\0'; + while (c >= '0' && c <= '9') + c = (current_ = p) < end_ ? *p++ : '\0'; + } + // exponential part + if (c == 'e' || c == 'E') { + c = (current_ = p) < end_ ? *p++ : '\0'; + if (c == '+' || c == '-') + c = (current_ = p) < end_ ? *p++ : '\0'; + while (c >= '0' && c <= '9') + c = (current_ = p) < end_ ? *p++ : '\0'; + } +} + +bool Reader::readString() { + Char c = '\0'; + while (current_ != end_) { + c = getNextChar(); + if (c == '\\') + getNextChar(); + else if (c == '"') + break; + } + return c == '"'; +} + +bool Reader::readObject(Token& token) { + Token tokenName; + String name; + Value init(objectValue); + currentValue().swapPayload(init); + currentValue().setOffsetStart(token.start_ - begin_); + while (readToken(tokenName)) { + bool initialTokenOk = true; + while (tokenName.type_ == tokenComment && initialTokenOk) + initialTokenOk = readToken(tokenName); + if (!initialTokenOk) + break; + if (tokenName.type_ == tokenObjectEnd && name.empty()) // empty object + return true; + name.clear(); + if (tokenName.type_ == tokenString) { + if (!decodeString(tokenName, name)) + return recoverFromError(tokenObjectEnd); + } else if (tokenName.type_ == tokenNumber && features_.allowNumericKeys_) { + Value numberName; + if (!decodeNumber(tokenName, numberName)) + return recoverFromError(tokenObjectEnd); + name = numberName.asString(); + } else { + break; + } + + Token colon; + if (!readToken(colon) || colon.type_ != tokenMemberSeparator) { + return addErrorAndRecover("Missing ':' after object member name", colon, + tokenObjectEnd); + } + Value& value = currentValue()[name]; + nodes_.push(&value); + bool ok = readValue(); + nodes_.pop(); + if (!ok) // error already set + return recoverFromError(tokenObjectEnd); + + Token comma; + if (!readToken(comma) || + (comma.type_ != tokenObjectEnd && comma.type_ != tokenArraySeparator && + comma.type_ != tokenComment)) { + return addErrorAndRecover("Missing ',' or '}' in object declaration", + comma, tokenObjectEnd); + } + bool finalizeTokenOk = true; + while (comma.type_ == tokenComment && finalizeTokenOk) + finalizeTokenOk = readToken(comma); + if (comma.type_ == tokenObjectEnd) + return true; + } + return addErrorAndRecover("Missing '}' or object member name", tokenName, + tokenObjectEnd); +} + +bool Reader::readArray(Token& token) { + Value init(arrayValue); + currentValue().swapPayload(init); + currentValue().setOffsetStart(token.start_ - begin_); + skipSpaces(); + if (current_ != end_ && *current_ == ']') // empty array + { + Token endArray; + readToken(endArray); + return true; + } + int index = 0; + for (;;) { + Value& value = currentValue()[index++]; + nodes_.push(&value); + bool ok = readValue(); + nodes_.pop(); + if (!ok) // error already set + return recoverFromError(tokenArrayEnd); + + Token currentToken; + // Accept Comment after last item in the array. + ok = readToken(currentToken); + while (currentToken.type_ == tokenComment && ok) { + ok = readToken(currentToken); + } + bool badTokenType = (currentToken.type_ != tokenArraySeparator && + currentToken.type_ != tokenArrayEnd); + if (!ok || badTokenType) { + return addErrorAndRecover("Missing ',' or ']' in array declaration", + currentToken, tokenArrayEnd); + } + if (currentToken.type_ == tokenArrayEnd) + break; + } + return true; +} + +bool Reader::decodeNumber(Token& token) { + Value decoded; + if (!decodeNumber(token, decoded)) + return false; + currentValue().swapPayload(decoded); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + return true; +} + +bool Reader::decodeNumber(Token& token, Value& decoded) { + // Attempts to parse the number as an integer. If the number is + // larger than the maximum supported value of an integer then + // we decode the number as a double. + Location current = token.start_; + bool isNegative = *current == '-'; + if (isNegative) + ++current; + // TODO: Help the compiler do the div and mod at compile time or get rid of + // them. + Value::LargestUInt maxIntegerValue = + isNegative ? Value::LargestUInt(Value::maxLargestInt) + 1 + : Value::maxLargestUInt; + Value::LargestUInt threshold = maxIntegerValue / 10; + Value::LargestUInt value = 0; + while (current < token.end_) { + Char c = *current++; + if (c < '0' || c > '9') + return decodeDouble(token, decoded); + auto digit(static_cast(c - '0')); + if (value >= threshold) { + // We've hit or exceeded the max value divided by 10 (rounded down). If + // a) we've only just touched the limit, b) this is the last digit, and + // c) it's small enough to fit in that rounding delta, we're okay. + // Otherwise treat this number as a double to avoid overflow. + if (value > threshold || current != token.end_ || + digit > maxIntegerValue % 10) { + return decodeDouble(token, decoded); + } + } + value = value * 10 + digit; + } + if (isNegative && value == maxIntegerValue) + decoded = Value::minLargestInt; + else if (isNegative) + decoded = -Value::LargestInt(value); + else if (value <= Value::LargestUInt(Value::maxInt)) + decoded = Value::LargestInt(value); + else + decoded = value; + return true; +} + +bool Reader::decodeDouble(Token& token) { + Value decoded; + if (!decodeDouble(token, decoded)) + return false; + currentValue().swapPayload(decoded); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + return true; +} + +bool Reader::decodeDouble(Token& token, Value& decoded) { + double value = 0; + String buffer(token.start_, token.end_); + IStringStream is(buffer); + if (!(is >> value)) { + if (value == std::numeric_limits::max()) + value = std::numeric_limits::infinity(); + else if (value == std::numeric_limits::lowest()) + value = -std::numeric_limits::infinity(); + else if (!std::isinf(value)) + return addError( + "'" + String(token.start_, token.end_) + "' is not a number.", token); + } + decoded = value; + return true; +} + +bool Reader::decodeString(Token& token) { + String decoded_string; + if (!decodeString(token, decoded_string)) + return false; + Value decoded(decoded_string); + currentValue().swapPayload(decoded); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + return true; +} + +bool Reader::decodeString(Token& token, String& decoded) { + decoded.reserve(static_cast(token.end_ - token.start_ - 2)); + Location current = token.start_ + 1; // skip '"' + Location end = token.end_ - 1; // do not include '"' + while (current != end) { + Char c = *current++; + if (c == '"') + break; + if (c == '\\') { + if (current == end) + return addError("Empty escape sequence in string", token, current); + Char escape = *current++; + switch (escape) { + case '"': + decoded += '"'; + break; + case '/': + decoded += '/'; + break; + case '\\': + decoded += '\\'; + break; + case 'b': + decoded += '\b'; + break; + case 'f': + decoded += '\f'; + break; + case 'n': + decoded += '\n'; + break; + case 'r': + decoded += '\r'; + break; + case 't': + decoded += '\t'; + break; + case 'u': { + unsigned int unicode; + if (!decodeUnicodeCodePoint(token, current, end, unicode)) + return false; + decoded += codePointToUTF8(unicode); + } break; + default: + return addError("Bad escape sequence in string", token, current); + } + } else { + decoded += c; + } + } + return true; +} + +bool Reader::decodeUnicodeCodePoint(Token& token, Location& current, + Location end, unsigned int& unicode) { + + if (!decodeUnicodeEscapeSequence(token, current, end, unicode)) + return false; + if (unicode >= 0xD800 && unicode <= 0xDBFF) { + // surrogate pairs + if (end - current < 6) + return addError( + "additional six characters expected to parse unicode surrogate pair.", + token, current); + if (*(current++) == '\\' && *(current++) == 'u') { + unsigned int surrogatePair; + if (decodeUnicodeEscapeSequence(token, current, end, surrogatePair)) { + unicode = 0x10000 + ((unicode & 0x3FF) << 10) + (surrogatePair & 0x3FF); + } else + return false; + } else + return addError("expecting another \\u token to begin the second half of " + "a unicode surrogate pair", + token, current); + } + return true; +} + +bool Reader::decodeUnicodeEscapeSequence(Token& token, Location& current, + Location end, + unsigned int& ret_unicode) { + if (end - current < 4) + return addError( + "Bad unicode escape sequence in string: four digits expected.", token, + current); + int unicode = 0; + for (int index = 0; index < 4; ++index) { + Char c = *current++; + unicode *= 16; + if (c >= '0' && c <= '9') + unicode += c - '0'; + else if (c >= 'a' && c <= 'f') + unicode += c - 'a' + 10; + else if (c >= 'A' && c <= 'F') + unicode += c - 'A' + 10; + else + return addError( + "Bad unicode escape sequence in string: hexadecimal digit expected.", + token, current); + } + ret_unicode = static_cast(unicode); + return true; +} + +bool Reader::addError(const String& message, Token& token, Location extra) { + ErrorInfo info; + info.token_ = token; + info.message_ = message; + info.extra_ = extra; + errors_.push_back(info); + return false; +} + +bool Reader::recoverFromError(TokenType skipUntilToken) { + size_t const errorCount = errors_.size(); + Token skip; + for (;;) { + if (!readToken(skip)) + errors_.resize(errorCount); // discard errors caused by recovery + if (skip.type_ == skipUntilToken || skip.type_ == tokenEndOfStream) + break; + } + errors_.resize(errorCount); + return false; +} + +bool Reader::addErrorAndRecover(const String& message, Token& token, + TokenType skipUntilToken) { + addError(message, token); + return recoverFromError(skipUntilToken); +} + +Value& Reader::currentValue() { return *(nodes_.top()); } + +Reader::Char Reader::getNextChar() { + if (current_ == end_) + return 0; + return *current_++; +} + +void Reader::getLocationLineAndColumn(Location location, int& line, + int& column) const { + Location current = begin_; + Location lastLineStart = current; + line = 0; + while (current < location && current != end_) { + Char c = *current++; + if (c == '\r') { + if (*current == '\n') + ++current; + lastLineStart = current; + ++line; + } else if (c == '\n') { + lastLineStart = current; + ++line; + } + } + // column & line start at 1 + column = int(location - lastLineStart) + 1; + ++line; +} + +String Reader::getLocationLineAndColumn(Location location) const { + int line, column; + getLocationLineAndColumn(location, line, column); + char buffer[18 + 16 + 16 + 1]; + jsoncpp_snprintf(buffer, sizeof(buffer), "Line %d, Column %d", line, column); + return buffer; +} + +// Deprecated. Preserved for backward compatibility +String Reader::getFormatedErrorMessages() const { + return getFormattedErrorMessages(); +} + +String Reader::getFormattedErrorMessages() const { + String formattedMessage; + for (const auto& error : errors_) { + formattedMessage += + "* " + getLocationLineAndColumn(error.token_.start_) + "\n"; + formattedMessage += " " + error.message_ + "\n"; + if (error.extra_) + formattedMessage += + "See " + getLocationLineAndColumn(error.extra_) + " for detail.\n"; + } + return formattedMessage; +} + +std::vector Reader::getStructuredErrors() const { + std::vector allErrors; + for (const auto& error : errors_) { + Reader::StructuredError structured; + structured.offset_start = error.token_.start_ - begin_; + structured.offset_limit = error.token_.end_ - begin_; + structured.message = error.message_; + allErrors.push_back(structured); + } + return allErrors; +} + +bool Reader::pushError(const Value& value, const String& message) { + ptrdiff_t const length = end_ - begin_; + if (value.getOffsetStart() > length || value.getOffsetLimit() > length) + return false; + Token token; + token.type_ = tokenError; + token.start_ = begin_ + value.getOffsetStart(); + token.end_ = begin_ + value.getOffsetLimit(); + ErrorInfo info; + info.token_ = token; + info.message_ = message; + info.extra_ = nullptr; + errors_.push_back(info); + return true; +} + +bool Reader::pushError(const Value& value, const String& message, + const Value& extra) { + ptrdiff_t const length = end_ - begin_; + if (value.getOffsetStart() > length || value.getOffsetLimit() > length || + extra.getOffsetLimit() > length) + return false; + Token token; + token.type_ = tokenError; + token.start_ = begin_ + value.getOffsetStart(); + token.end_ = begin_ + value.getOffsetLimit(); + ErrorInfo info; + info.token_ = token; + info.message_ = message; + info.extra_ = begin_ + extra.getOffsetStart(); + errors_.push_back(info); + return true; +} + +bool Reader::good() const { return errors_.empty(); } + +// Originally copied from the Features class (now deprecated), used internally +// for features implementation. +class OurFeatures { +public: + static OurFeatures all(); + bool allowComments_; + bool allowTrailingCommas_; + bool strictRoot_; + bool allowDroppedNullPlaceholders_; + bool allowNumericKeys_; + bool allowSingleQuotes_; + bool failIfExtra_; + bool rejectDupKeys_; + bool allowSpecialFloats_; + bool skipBom_; + size_t stackLimit_; +}; // OurFeatures + +OurFeatures OurFeatures::all() { return {}; } + +// Implementation of class Reader +// //////////////////////////////// + +// Originally copied from the Reader class (now deprecated), used internally +// for implementing JSON reading. +class OurReader { +public: + using Char = char; + using Location = const Char*; + struct StructuredError { + ptrdiff_t offset_start; + ptrdiff_t offset_limit; + String message; + }; + + explicit OurReader(OurFeatures const& features); + bool parse(const char* beginDoc, const char* endDoc, Value& root, + bool collectComments = true); + String getFormattedErrorMessages() const; + std::vector getStructuredErrors() const; + +private: + OurReader(OurReader const&); // no impl + void operator=(OurReader const&); // no impl + + enum TokenType { + tokenEndOfStream = 0, + tokenObjectBegin, + tokenObjectEnd, + tokenArrayBegin, + tokenArrayEnd, + tokenString, + tokenNumber, + tokenTrue, + tokenFalse, + tokenNull, + tokenNaN, + tokenPosInf, + tokenNegInf, + tokenArraySeparator, + tokenMemberSeparator, + tokenComment, + tokenError + }; + + class Token { + public: + TokenType type_; + Location start_; + Location end_; + }; + + class ErrorInfo { + public: + Token token_; + String message_; + Location extra_; + }; + + using Errors = std::deque; + + bool readToken(Token& token); + void skipSpaces(); + void skipBom(bool skipBom); + bool match(const Char* pattern, int patternLength); + bool readComment(); + bool readCStyleComment(bool* containsNewLineResult); + bool readCppStyleComment(); + bool readString(); + bool readStringSingleQuote(); + bool readNumber(bool checkInf); + bool readValue(); + bool readObject(Token& token); + bool readArray(Token& token); + bool decodeNumber(Token& token); + bool decodeNumber(Token& token, Value& decoded); + bool decodeString(Token& token); + bool decodeString(Token& token, String& decoded); + bool decodeDouble(Token& token); + bool decodeDouble(Token& token, Value& decoded); + bool decodeUnicodeCodePoint(Token& token, Location& current, Location end, + unsigned int& unicode); + bool decodeUnicodeEscapeSequence(Token& token, Location& current, + Location end, unsigned int& unicode); + bool addError(const String& message, Token& token, Location extra = nullptr); + bool recoverFromError(TokenType skipUntilToken); + bool addErrorAndRecover(const String& message, Token& token, + TokenType skipUntilToken); + void skipUntilSpace(); + Value& currentValue(); + Char getNextChar(); + void getLocationLineAndColumn(Location location, int& line, + int& column) const; + String getLocationLineAndColumn(Location location) const; + void addComment(Location begin, Location end, CommentPlacement placement); + void skipCommentTokens(Token& token); + + static String normalizeEOL(Location begin, Location end); + static bool containsNewLine(Location begin, Location end); + + using Nodes = std::stack; + + Nodes nodes_{}; + Errors errors_{}; + String document_{}; + Location begin_ = nullptr; + Location end_ = nullptr; + Location current_ = nullptr; + Location lastValueEnd_ = nullptr; + Value* lastValue_ = nullptr; + bool lastValueHasAComment_ = false; + String commentsBefore_{}; + + OurFeatures const features_; + bool collectComments_ = false; +}; // OurReader + +// complete copy of Read impl, for OurReader + +bool OurReader::containsNewLine(OurReader::Location begin, + OurReader::Location end) { + return std::any_of(begin, end, [](char b) { return b == '\n' || b == '\r'; }); +} + +OurReader::OurReader(OurFeatures const& features) : features_(features) {} + +bool OurReader::parse(const char* beginDoc, const char* endDoc, Value& root, + bool collectComments) { + if (!features_.allowComments_) { + collectComments = false; + } + + begin_ = beginDoc; + end_ = endDoc; + collectComments_ = collectComments; + current_ = begin_; + lastValueEnd_ = nullptr; + lastValue_ = nullptr; + commentsBefore_.clear(); + errors_.clear(); + while (!nodes_.empty()) + nodes_.pop(); + nodes_.push(&root); + + // skip byte order mark if it exists at the beginning of the UTF-8 text. + skipBom(features_.skipBom_); + bool successful = readValue(); + nodes_.pop(); + Token token; + skipCommentTokens(token); + if (features_.failIfExtra_ && (token.type_ != tokenEndOfStream)) { + addError("Extra non-whitespace after JSON value.", token); + return false; + } + if (collectComments_ && !commentsBefore_.empty()) + root.setComment(commentsBefore_, commentAfter); + if (features_.strictRoot_) { + if (!root.isArray() && !root.isObject()) { + // Set error location to start of doc, ideally should be first token found + // in doc + token.type_ = tokenError; + token.start_ = beginDoc; + token.end_ = endDoc; + addError( + "A valid JSON document must be either an array or an object value.", + token); + return false; + } + } + return successful; +} + +bool OurReader::readValue() { + // To preserve the old behaviour we cast size_t to int. + if (nodes_.size() > features_.stackLimit_) + throwRuntimeError("Exceeded stackLimit in readValue()."); + Token token; + skipCommentTokens(token); + bool successful = true; + + if (collectComments_ && !commentsBefore_.empty()) { + currentValue().setComment(commentsBefore_, commentBefore); + commentsBefore_.clear(); + } + + switch (token.type_) { + case tokenObjectBegin: + successful = readObject(token); + currentValue().setOffsetLimit(current_ - begin_); + break; + case tokenArrayBegin: + successful = readArray(token); + currentValue().setOffsetLimit(current_ - begin_); + break; + case tokenNumber: + successful = decodeNumber(token); + break; + case tokenString: + successful = decodeString(token); + break; + case tokenTrue: { + Value v(true); + currentValue().swapPayload(v); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + } break; + case tokenFalse: { + Value v(false); + currentValue().swapPayload(v); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + } break; + case tokenNull: { + Value v; + currentValue().swapPayload(v); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + } break; + case tokenNaN: { + Value v(std::numeric_limits::quiet_NaN()); + currentValue().swapPayload(v); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + } break; + case tokenPosInf: { + Value v(std::numeric_limits::infinity()); + currentValue().swapPayload(v); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + } break; + case tokenNegInf: { + Value v(-std::numeric_limits::infinity()); + currentValue().swapPayload(v); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + } break; + case tokenArraySeparator: + case tokenObjectEnd: + case tokenArrayEnd: + if (features_.allowDroppedNullPlaceholders_) { + // "Un-read" the current token and mark the current value as a null + // token. + current_--; + Value v; + currentValue().swapPayload(v); + currentValue().setOffsetStart(current_ - begin_ - 1); + currentValue().setOffsetLimit(current_ - begin_); + break; + } // else, fall through ... + default: + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + return addError("Syntax error: value, object or array expected.", token); + } + + if (collectComments_) { + lastValueEnd_ = current_; + lastValueHasAComment_ = false; + lastValue_ = ¤tValue(); + } + + return successful; +} + +void OurReader::skipCommentTokens(Token& token) { + if (features_.allowComments_) { + do { + readToken(token); + } while (token.type_ == tokenComment); + } else { + readToken(token); + } +} + +bool OurReader::readToken(Token& token) { + skipSpaces(); + token.start_ = current_; + Char c = getNextChar(); + bool ok = true; + switch (c) { + case '{': + token.type_ = tokenObjectBegin; + break; + case '}': + token.type_ = tokenObjectEnd; + break; + case '[': + token.type_ = tokenArrayBegin; + break; + case ']': + token.type_ = tokenArrayEnd; + break; + case '"': + token.type_ = tokenString; + ok = readString(); + break; + case '\'': + if (features_.allowSingleQuotes_) { + token.type_ = tokenString; + ok = readStringSingleQuote(); + } else { + // If we don't allow single quotes, this is a failure case. + ok = false; + } + break; + case '/': + token.type_ = tokenComment; + ok = readComment(); + break; + case '0': + case '1': + case '2': + case '3': + case '4': + case '5': + case '6': + case '7': + case '8': + case '9': + token.type_ = tokenNumber; + readNumber(false); + break; + case '-': + if (readNumber(true)) { + token.type_ = tokenNumber; + } else { + token.type_ = tokenNegInf; + ok = features_.allowSpecialFloats_ && match("nfinity", 7); + } + break; + case '+': + if (readNumber(true)) { + token.type_ = tokenNumber; + } else { + token.type_ = tokenPosInf; + ok = features_.allowSpecialFloats_ && match("nfinity", 7); + } + break; + case 't': + token.type_ = tokenTrue; + ok = match("rue", 3); + break; + case 'f': + token.type_ = tokenFalse; + ok = match("alse", 4); + break; + case 'n': + token.type_ = tokenNull; + ok = match("ull", 3); + break; + case 'N': + if (features_.allowSpecialFloats_) { + token.type_ = tokenNaN; + ok = match("aN", 2); + } else { + ok = false; + } + break; + case 'I': + if (features_.allowSpecialFloats_) { + token.type_ = tokenPosInf; + ok = match("nfinity", 7); + } else { + ok = false; + } + break; + case ',': + token.type_ = tokenArraySeparator; + break; + case ':': + token.type_ = tokenMemberSeparator; + break; + case 0: + token.type_ = tokenEndOfStream; + break; + default: + ok = false; + break; + } + if (!ok) + token.type_ = tokenError; + token.end_ = current_; + return ok; +} + +void OurReader::skipSpaces() { + while (current_ != end_) { + Char c = *current_; + if (c == ' ' || c == '\t' || c == '\r' || c == '\n') + ++current_; + else + break; + } +} + +void OurReader::skipBom(bool skipBom) { + // The default behavior is to skip BOM. + if (skipBom) { + if ((end_ - begin_) >= 3 && strncmp(begin_, "\xEF\xBB\xBF", 3) == 0) { + begin_ += 3; + current_ = begin_; + } + } +} + +bool OurReader::match(const Char* pattern, int patternLength) { + if (end_ - current_ < patternLength) + return false; + int index = patternLength; + while (index--) + if (current_[index] != pattern[index]) + return false; + current_ += patternLength; + return true; +} + +bool OurReader::readComment() { + const Location commentBegin = current_ - 1; + const Char c = getNextChar(); + bool successful = false; + bool cStyleWithEmbeddedNewline = false; + + const bool isCStyleComment = (c == '*'); + const bool isCppStyleComment = (c == '/'); + if (isCStyleComment) { + successful = readCStyleComment(&cStyleWithEmbeddedNewline); + } else if (isCppStyleComment) { + successful = readCppStyleComment(); + } + + if (!successful) + return false; + + if (collectComments_) { + CommentPlacement placement = commentBefore; + + if (!lastValueHasAComment_) { + if (lastValueEnd_ && !containsNewLine(lastValueEnd_, commentBegin)) { + if (isCppStyleComment || !cStyleWithEmbeddedNewline) { + placement = commentAfterOnSameLine; + lastValueHasAComment_ = true; + } + } + } + + addComment(commentBegin, current_, placement); + } + return true; +} + +String OurReader::normalizeEOL(OurReader::Location begin, + OurReader::Location end) { + String normalized; + normalized.reserve(static_cast(end - begin)); + OurReader::Location current = begin; + while (current != end) { + char c = *current++; + if (c == '\r') { + if (current != end && *current == '\n') + // convert dos EOL + ++current; + // convert Mac EOL + normalized += '\n'; + } else { + normalized += c; + } + } + return normalized; +} + +void OurReader::addComment(Location begin, Location end, + CommentPlacement placement) { + assert(collectComments_); + const String& normalized = normalizeEOL(begin, end); + if (placement == commentAfterOnSameLine) { + assert(lastValue_ != nullptr); + lastValue_->setComment(normalized, placement); + } else { + commentsBefore_ += normalized; + } +} + +bool OurReader::readCStyleComment(bool* containsNewLineResult) { + *containsNewLineResult = false; + + while ((current_ + 1) < end_) { + Char c = getNextChar(); + if (c == '*' && *current_ == '/') + break; + if (c == '\n') + *containsNewLineResult = true; + } + + return getNextChar() == '/'; +} + +bool OurReader::readCppStyleComment() { + while (current_ != end_) { + Char c = getNextChar(); + if (c == '\n') + break; + if (c == '\r') { + // Consume DOS EOL. It will be normalized in addComment. + if (current_ != end_ && *current_ == '\n') + getNextChar(); + // Break on Moc OS 9 EOL. + break; + } + } + return true; +} + +bool OurReader::readNumber(bool checkInf) { + Location p = current_; + if (checkInf && p != end_ && *p == 'I') { + current_ = ++p; + return false; + } + char c = '0'; // stopgap for already consumed character + // integral part + while (c >= '0' && c <= '9') + c = (current_ = p) < end_ ? *p++ : '\0'; + // fractional part + if (c == '.') { + c = (current_ = p) < end_ ? *p++ : '\0'; + while (c >= '0' && c <= '9') + c = (current_ = p) < end_ ? *p++ : '\0'; + } + // exponential part + if (c == 'e' || c == 'E') { + c = (current_ = p) < end_ ? *p++ : '\0'; + if (c == '+' || c == '-') + c = (current_ = p) < end_ ? *p++ : '\0'; + while (c >= '0' && c <= '9') + c = (current_ = p) < end_ ? *p++ : '\0'; + } + return true; +} +bool OurReader::readString() { + Char c = 0; + while (current_ != end_) { + c = getNextChar(); + if (c == '\\') + getNextChar(); + else if (c == '"') + break; + } + return c == '"'; +} + +bool OurReader::readStringSingleQuote() { + Char c = 0; + while (current_ != end_) { + c = getNextChar(); + if (c == '\\') + getNextChar(); + else if (c == '\'') + break; + } + return c == '\''; +} + +bool OurReader::readObject(Token& token) { + Token tokenName; + String name; + Value init(objectValue); + currentValue().swapPayload(init); + currentValue().setOffsetStart(token.start_ - begin_); + while (readToken(tokenName)) { + bool initialTokenOk = true; + while (tokenName.type_ == tokenComment && initialTokenOk) + initialTokenOk = readToken(tokenName); + if (!initialTokenOk) + break; + if (tokenName.type_ == tokenObjectEnd && + (name.empty() || + features_.allowTrailingCommas_)) // empty object or trailing comma + return true; + name.clear(); + if (tokenName.type_ == tokenString) { + if (!decodeString(tokenName, name)) + return recoverFromError(tokenObjectEnd); + } else if (tokenName.type_ == tokenNumber && features_.allowNumericKeys_) { + Value numberName; + if (!decodeNumber(tokenName, numberName)) + return recoverFromError(tokenObjectEnd); + name = numberName.asString(); + } else { + break; + } + if (name.length() >= (1U << 30)) + throwRuntimeError("keylength >= 2^30"); + if (features_.rejectDupKeys_ && currentValue().isMember(name)) { + String msg = "Duplicate key: '" + name + "'"; + return addErrorAndRecover(msg, tokenName, tokenObjectEnd); + } + + Token colon; + if (!readToken(colon) || colon.type_ != tokenMemberSeparator) { + return addErrorAndRecover("Missing ':' after object member name", colon, + tokenObjectEnd); + } + Value& value = currentValue()[name]; + nodes_.push(&value); + bool ok = readValue(); + nodes_.pop(); + if (!ok) // error already set + return recoverFromError(tokenObjectEnd); + + Token comma; + if (!readToken(comma) || + (comma.type_ != tokenObjectEnd && comma.type_ != tokenArraySeparator && + comma.type_ != tokenComment)) { + return addErrorAndRecover("Missing ',' or '}' in object declaration", + comma, tokenObjectEnd); + } + bool finalizeTokenOk = true; + while (comma.type_ == tokenComment && finalizeTokenOk) + finalizeTokenOk = readToken(comma); + if (comma.type_ == tokenObjectEnd) + return true; + } + return addErrorAndRecover("Missing '}' or object member name", tokenName, + tokenObjectEnd); +} + +bool OurReader::readArray(Token& token) { + Value init(arrayValue); + currentValue().swapPayload(init); + currentValue().setOffsetStart(token.start_ - begin_); + int index = 0; + for (;;) { + skipSpaces(); + if (current_ != end_ && *current_ == ']' && + (index == 0 || + (features_.allowTrailingCommas_ && + !features_.allowDroppedNullPlaceholders_))) // empty array or trailing + // comma + { + Token endArray; + readToken(endArray); + return true; + } + Value& value = currentValue()[index++]; + nodes_.push(&value); + bool ok = readValue(); + nodes_.pop(); + if (!ok) // error already set + return recoverFromError(tokenArrayEnd); + + Token currentToken; + // Accept Comment after last item in the array. + ok = readToken(currentToken); + while (currentToken.type_ == tokenComment && ok) { + ok = readToken(currentToken); + } + bool badTokenType = (currentToken.type_ != tokenArraySeparator && + currentToken.type_ != tokenArrayEnd); + if (!ok || badTokenType) { + return addErrorAndRecover("Missing ',' or ']' in array declaration", + currentToken, tokenArrayEnd); + } + if (currentToken.type_ == tokenArrayEnd) + break; + } + return true; +} + +bool OurReader::decodeNumber(Token& token) { + Value decoded; + if (!decodeNumber(token, decoded)) + return false; + currentValue().swapPayload(decoded); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + return true; +} + +bool OurReader::decodeNumber(Token& token, Value& decoded) { + // Attempts to parse the number as an integer. If the number is + // larger than the maximum supported value of an integer then + // we decode the number as a double. + Location current = token.start_; + const bool isNegative = *current == '-'; + if (isNegative) { + ++current; + } + + // We assume we can represent the largest and smallest integer types as + // unsigned integers with separate sign. This is only true if they can fit + // into an unsigned integer. + static_assert(Value::maxLargestInt <= Value::maxLargestUInt, + "Int must be smaller than UInt"); + + // We need to convert minLargestInt into a positive number. The easiest way + // to do this conversion is to assume our "threshold" value of minLargestInt + // divided by 10 can fit in maxLargestInt when absolute valued. This should + // be a safe assumption. + static_assert(Value::minLargestInt <= -Value::maxLargestInt, + "The absolute value of minLargestInt must be greater than or " + "equal to maxLargestInt"); + static_assert(Value::minLargestInt / 10 >= -Value::maxLargestInt, + "The absolute value of minLargestInt must be only 1 magnitude " + "larger than maxLargest Int"); + + static constexpr Value::LargestUInt positive_threshold = + Value::maxLargestUInt / 10; + static constexpr Value::UInt positive_last_digit = Value::maxLargestUInt % 10; + + // For the negative values, we have to be more careful. Since typically + // -Value::minLargestInt will cause an overflow, we first divide by 10 and + // then take the inverse. This assumes that minLargestInt is only a single + // power of 10 different in magnitude, which we check above. For the last + // digit, we take the modulus before negating for the same reason. + static constexpr auto negative_threshold = + Value::LargestUInt(-(Value::minLargestInt / 10)); + static constexpr auto negative_last_digit = + Value::UInt(-(Value::minLargestInt % 10)); + + const Value::LargestUInt threshold = + isNegative ? negative_threshold : positive_threshold; + const Value::UInt max_last_digit = + isNegative ? negative_last_digit : positive_last_digit; + + Value::LargestUInt value = 0; + while (current < token.end_) { + Char c = *current++; + if (c < '0' || c > '9') + return decodeDouble(token, decoded); + + const auto digit(static_cast(c - '0')); + if (value >= threshold) { + // We've hit or exceeded the max value divided by 10 (rounded down). If + // a) we've only just touched the limit, meaning value == threshold, + // b) this is the last digit, or + // c) it's small enough to fit in that rounding delta, we're okay. + // Otherwise treat this number as a double to avoid overflow. + if (value > threshold || current != token.end_ || + digit > max_last_digit) { + return decodeDouble(token, decoded); + } + } + value = value * 10 + digit; + } + + if (isNegative) { + // We use the same magnitude assumption here, just in case. + const auto last_digit = static_cast(value % 10); + decoded = -Value::LargestInt(value / 10) * 10 - last_digit; + } else if (value <= Value::LargestUInt(Value::maxLargestInt)) { + decoded = Value::LargestInt(value); + } else { + decoded = value; + } + + return true; +} + +bool OurReader::decodeDouble(Token& token) { + Value decoded; + if (!decodeDouble(token, decoded)) + return false; + currentValue().swapPayload(decoded); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + return true; +} + +bool OurReader::decodeDouble(Token& token, Value& decoded) { + double value = 0; + const String buffer(token.start_, token.end_); + IStringStream is(buffer); + if (!(is >> value)) { + if (value == std::numeric_limits::max()) + value = std::numeric_limits::infinity(); + else if (value == std::numeric_limits::lowest()) + value = -std::numeric_limits::infinity(); + else if (!std::isinf(value)) + return addError( + "'" + String(token.start_, token.end_) + "' is not a number.", token); + } + decoded = value; + return true; +} + +bool OurReader::decodeString(Token& token) { + String decoded_string; + if (!decodeString(token, decoded_string)) + return false; + Value decoded(decoded_string); + currentValue().swapPayload(decoded); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + return true; +} + +bool OurReader::decodeString(Token& token, String& decoded) { + decoded.reserve(static_cast(token.end_ - token.start_ - 2)); + Location current = token.start_ + 1; // skip '"' + Location end = token.end_ - 1; // do not include '"' + while (current != end) { + Char c = *current++; + if (c == '"') + break; + if (c == '\\') { + if (current == end) + return addError("Empty escape sequence in string", token, current); + Char escape = *current++; + switch (escape) { + case '"': + decoded += '"'; + break; + case '/': + decoded += '/'; + break; + case '\\': + decoded += '\\'; + break; + case 'b': + decoded += '\b'; + break; + case 'f': + decoded += '\f'; + break; + case 'n': + decoded += '\n'; + break; + case 'r': + decoded += '\r'; + break; + case 't': + decoded += '\t'; + break; + case 'u': { + unsigned int unicode; + if (!decodeUnicodeCodePoint(token, current, end, unicode)) + return false; + decoded += codePointToUTF8(unicode); + } break; + default: + return addError("Bad escape sequence in string", token, current); + } + } else { + decoded += c; + } + } + return true; +} + +bool OurReader::decodeUnicodeCodePoint(Token& token, Location& current, + Location end, unsigned int& unicode) { + + if (!decodeUnicodeEscapeSequence(token, current, end, unicode)) + return false; + if (unicode >= 0xD800 && unicode <= 0xDBFF) { + // surrogate pairs + if (end - current < 6) + return addError( + "additional six characters expected to parse unicode surrogate pair.", + token, current); + if (*(current++) == '\\' && *(current++) == 'u') { + unsigned int surrogatePair; + if (decodeUnicodeEscapeSequence(token, current, end, surrogatePair)) { + unicode = 0x10000 + ((unicode & 0x3FF) << 10) + (surrogatePair & 0x3FF); + } else + return false; + } else + return addError("expecting another \\u token to begin the second half of " + "a unicode surrogate pair", + token, current); + } + return true; +} + +bool OurReader::decodeUnicodeEscapeSequence(Token& token, Location& current, + Location end, + unsigned int& ret_unicode) { + if (end - current < 4) + return addError( + "Bad unicode escape sequence in string: four digits expected.", token, + current); + int unicode = 0; + for (int index = 0; index < 4; ++index) { + Char c = *current++; + unicode *= 16; + if (c >= '0' && c <= '9') + unicode += c - '0'; + else if (c >= 'a' && c <= 'f') + unicode += c - 'a' + 10; + else if (c >= 'A' && c <= 'F') + unicode += c - 'A' + 10; + else + return addError( + "Bad unicode escape sequence in string: hexadecimal digit expected.", + token, current); + } + ret_unicode = static_cast(unicode); + return true; +} + +bool OurReader::addError(const String& message, Token& token, Location extra) { + ErrorInfo info; + info.token_ = token; + info.message_ = message; + info.extra_ = extra; + errors_.push_back(info); + return false; +} + +bool OurReader::recoverFromError(TokenType skipUntilToken) { + size_t errorCount = errors_.size(); + Token skip; + for (;;) { + if (!readToken(skip)) + errors_.resize(errorCount); // discard errors caused by recovery + if (skip.type_ == skipUntilToken || skip.type_ == tokenEndOfStream) + break; + } + errors_.resize(errorCount); + return false; +} + +bool OurReader::addErrorAndRecover(const String& message, Token& token, + TokenType skipUntilToken) { + addError(message, token); + return recoverFromError(skipUntilToken); +} + +Value& OurReader::currentValue() { return *(nodes_.top()); } + +OurReader::Char OurReader::getNextChar() { + if (current_ == end_) + return 0; + return *current_++; +} + +void OurReader::getLocationLineAndColumn(Location location, int& line, + int& column) const { + Location current = begin_; + Location lastLineStart = current; + line = 0; + while (current < location && current != end_) { + Char c = *current++; + if (c == '\r') { + if (*current == '\n') + ++current; + lastLineStart = current; + ++line; + } else if (c == '\n') { + lastLineStart = current; + ++line; + } + } + // column & line start at 1 + column = int(location - lastLineStart) + 1; + ++line; +} + +String OurReader::getLocationLineAndColumn(Location location) const { + int line, column; + getLocationLineAndColumn(location, line, column); + char buffer[18 + 16 + 16 + 1]; + jsoncpp_snprintf(buffer, sizeof(buffer), "Line %d, Column %d", line, column); + return buffer; +} + +String OurReader::getFormattedErrorMessages() const { + String formattedMessage; + for (const auto& error : errors_) { + formattedMessage += + "* " + getLocationLineAndColumn(error.token_.start_) + "\n"; + formattedMessage += " " + error.message_ + "\n"; + if (error.extra_) + formattedMessage += + "See " + getLocationLineAndColumn(error.extra_) + " for detail.\n"; + } + return formattedMessage; +} + +std::vector OurReader::getStructuredErrors() const { + std::vector allErrors; + for (const auto& error : errors_) { + OurReader::StructuredError structured; + structured.offset_start = error.token_.start_ - begin_; + structured.offset_limit = error.token_.end_ - begin_; + structured.message = error.message_; + allErrors.push_back(structured); + } + return allErrors; +} + +class OurCharReader : public CharReader { + bool const collectComments_; + OurReader reader_; + +public: + OurCharReader(bool collectComments, OurFeatures const& features) + : collectComments_(collectComments), reader_(features) {} + bool parse(char const* beginDoc, char const* endDoc, Value* root, + String* errs) override { + bool ok = reader_.parse(beginDoc, endDoc, *root, collectComments_); + if (errs) { + *errs = reader_.getFormattedErrorMessages(); + } + return ok; + } +}; + +CharReaderBuilder::CharReaderBuilder() { setDefaults(&settings_); } +CharReaderBuilder::~CharReaderBuilder() = default; +CharReader* CharReaderBuilder::newCharReader() const { + bool collectComments = settings_["collectComments"].asBool(); + OurFeatures features = OurFeatures::all(); + features.allowComments_ = settings_["allowComments"].asBool(); + features.allowTrailingCommas_ = settings_["allowTrailingCommas"].asBool(); + features.strictRoot_ = settings_["strictRoot"].asBool(); + features.allowDroppedNullPlaceholders_ = + settings_["allowDroppedNullPlaceholders"].asBool(); + features.allowNumericKeys_ = settings_["allowNumericKeys"].asBool(); + features.allowSingleQuotes_ = settings_["allowSingleQuotes"].asBool(); + + // Stack limit is always a size_t, so we get this as an unsigned int + // regardless of it we have 64-bit integer support enabled. + features.stackLimit_ = static_cast(settings_["stackLimit"].asUInt()); + features.failIfExtra_ = settings_["failIfExtra"].asBool(); + features.rejectDupKeys_ = settings_["rejectDupKeys"].asBool(); + features.allowSpecialFloats_ = settings_["allowSpecialFloats"].asBool(); + features.skipBom_ = settings_["skipBom"].asBool(); + return new OurCharReader(collectComments, features); +} + +bool CharReaderBuilder::validate(Json::Value* invalid) const { + static const auto& valid_keys = *new std::set{ + "collectComments", + "allowComments", + "allowTrailingCommas", + "strictRoot", + "allowDroppedNullPlaceholders", + "allowNumericKeys", + "allowSingleQuotes", + "stackLimit", + "failIfExtra", + "rejectDupKeys", + "allowSpecialFloats", + "skipBom", + }; + for (auto si = settings_.begin(); si != settings_.end(); ++si) { + auto key = si.name(); + if (valid_keys.count(key)) + continue; + if (invalid) + (*invalid)[key] = *si; + else + return false; + } + return invalid ? invalid->empty() : true; +} + +Value& CharReaderBuilder::operator[](const String& key) { + return settings_[key]; +} +// static +void CharReaderBuilder::strictMode(Json::Value* settings) { + //! [CharReaderBuilderStrictMode] + (*settings)["allowComments"] = false; + (*settings)["allowTrailingCommas"] = false; + (*settings)["strictRoot"] = true; + (*settings)["allowDroppedNullPlaceholders"] = false; + (*settings)["allowNumericKeys"] = false; + (*settings)["allowSingleQuotes"] = false; + (*settings)["stackLimit"] = 1000; + (*settings)["failIfExtra"] = true; + (*settings)["rejectDupKeys"] = true; + (*settings)["allowSpecialFloats"] = false; + (*settings)["skipBom"] = true; + //! [CharReaderBuilderStrictMode] +} +// static +void CharReaderBuilder::setDefaults(Json::Value* settings) { + //! [CharReaderBuilderDefaults] + (*settings)["collectComments"] = true; + (*settings)["allowComments"] = true; + (*settings)["allowTrailingCommas"] = true; + (*settings)["strictRoot"] = false; + (*settings)["allowDroppedNullPlaceholders"] = false; + (*settings)["allowNumericKeys"] = false; + (*settings)["allowSingleQuotes"] = false; + (*settings)["stackLimit"] = 1000; + (*settings)["failIfExtra"] = false; + (*settings)["rejectDupKeys"] = false; + (*settings)["allowSpecialFloats"] = false; + (*settings)["skipBom"] = true; + //! [CharReaderBuilderDefaults] +} + +////////////////////////////////// +// global functions + +bool parseFromStream(CharReader::Factory const& fact, IStream& sin, Value* root, + String* errs) { + OStringStream ssin; + ssin << sin.rdbuf(); + String doc = ssin.str(); + char const* begin = doc.data(); + char const* end = begin + doc.size(); + // Note that we do not actually need a null-terminator. + CharReaderPtr const reader(fact.newCharReader()); + return reader->parse(begin, end, root, errs); +} + +IStream& operator>>(IStream& sin, Value& root) { + CharReaderBuilder b; + String errs; + bool ok = parseFromStream(b, sin, &root, &errs); + if (!ok) { + throwRuntimeError(errs); + } + return sin; +} + +} // namespace Json + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: src/lib_json/json_reader.cpp +// ////////////////////////////////////////////////////////////////////// + + + + + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: src/lib_json/json_valueiterator.inl +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2007-2010 Baptiste Lepilleur and The JsonCpp Authors +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +// included by json_value.cpp + +namespace Json { + +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// class ValueIteratorBase +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// + +ValueIteratorBase::ValueIteratorBase() : current_() {} + +ValueIteratorBase::ValueIteratorBase( + const Value::ObjectValues::iterator& current) + : current_(current), isNull_(false) {} + +Value& ValueIteratorBase::deref() { return current_->second; } +const Value& ValueIteratorBase::deref() const { return current_->second; } + +void ValueIteratorBase::increment() { ++current_; } + +void ValueIteratorBase::decrement() { --current_; } + +ValueIteratorBase::difference_type +ValueIteratorBase::computeDistance(const SelfType& other) const { + // Iterator for null value are initialized using the default + // constructor, which initialize current_ to the default + // std::map::iterator. As begin() and end() are two instance + // of the default std::map::iterator, they can not be compared. + // To allow this, we handle this comparison specifically. + if (isNull_ && other.isNull_) { + return 0; + } + + // Usage of std::distance is not portable (does not compile with Sun Studio 12 + // RogueWave STL, + // which is the one used by default). + // Using a portable hand-made version for non random iterator instead: + // return difference_type( std::distance( current_, other.current_ ) ); + difference_type myDistance = 0; + for (Value::ObjectValues::iterator it = current_; it != other.current_; + ++it) { + ++myDistance; + } + return myDistance; +} + +bool ValueIteratorBase::isEqual(const SelfType& other) const { + if (isNull_) { + return other.isNull_; + } + return current_ == other.current_; +} + +void ValueIteratorBase::copy(const SelfType& other) { + current_ = other.current_; + isNull_ = other.isNull_; +} + +Value ValueIteratorBase::key() const { + const Value::CZString czstring = (*current_).first; + if (czstring.data()) { + if (czstring.isStaticString()) + return Value(StaticString(czstring.data())); + return Value(czstring.data(), czstring.data() + czstring.length()); + } + return Value(czstring.index()); +} + +UInt ValueIteratorBase::index() const { + const Value::CZString czstring = (*current_).first; + if (!czstring.data()) + return czstring.index(); + return Value::UInt(-1); +} + +String ValueIteratorBase::name() const { + char const* keey; + char const* end; + keey = memberName(&end); + if (!keey) + return String(); + return String(keey, end); +} + +char const* ValueIteratorBase::memberName() const { + const char* cname = (*current_).first.data(); + return cname ? cname : ""; +} + +char const* ValueIteratorBase::memberName(char const** end) const { + const char* cname = (*current_).first.data(); + if (!cname) { + *end = nullptr; + return nullptr; + } + *end = cname + (*current_).first.length(); + return cname; +} + +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// class ValueConstIterator +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// + +ValueConstIterator::ValueConstIterator() = default; + +ValueConstIterator::ValueConstIterator( + const Value::ObjectValues::iterator& current) + : ValueIteratorBase(current) {} + +ValueConstIterator::ValueConstIterator(ValueIterator const& other) + : ValueIteratorBase(other) {} + +ValueConstIterator& ValueConstIterator:: +operator=(const ValueIteratorBase& other) { + copy(other); + return *this; +} + +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// class ValueIterator +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// + +ValueIterator::ValueIterator() = default; + +ValueIterator::ValueIterator(const Value::ObjectValues::iterator& current) + : ValueIteratorBase(current) {} + +ValueIterator::ValueIterator(const ValueConstIterator& other) + : ValueIteratorBase(other) { + throwRuntimeError("ConstIterator to Iterator should never be allowed."); +} + +ValueIterator::ValueIterator(const ValueIterator& other) = default; + +ValueIterator& ValueIterator::operator=(const SelfType& other) { + copy(other); + return *this; +} + +} // namespace Json + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: src/lib_json/json_valueiterator.inl +// ////////////////////////////////////////////////////////////////////// + + + + + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: src/lib_json/json_value.cpp +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2011 Baptiste Lepilleur and The JsonCpp Authors +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +#if !defined(JSON_IS_AMALGAMATION) +#include +#include +#include +#endif // if !defined(JSON_IS_AMALGAMATION) +#include +#include +#include +#include +#include +#include +#include +#include + +// Provide implementation equivalent of std::snprintf for older _MSC compilers +#if defined(_MSC_VER) && _MSC_VER < 1900 +#include +static int msvc_pre1900_c99_vsnprintf(char* outBuf, size_t size, + const char* format, va_list ap) { + int count = -1; + if (size != 0) + count = _vsnprintf_s(outBuf, size, _TRUNCATE, format, ap); + if (count == -1) + count = _vscprintf(format, ap); + return count; +} + +int JSON_API msvc_pre1900_c99_snprintf(char* outBuf, size_t size, + const char* format, ...) { + va_list ap; + va_start(ap, format); + const int count = msvc_pre1900_c99_vsnprintf(outBuf, size, format, ap); + va_end(ap); + return count; +} +#endif + +// Disable warning C4702 : unreachable code +#if defined(_MSC_VER) +#pragma warning(disable : 4702) +#endif + +#define JSON_ASSERT_UNREACHABLE assert(false) + +namespace Json { +template +static std::unique_ptr cloneUnique(const std::unique_ptr& p) { + std::unique_ptr r; + if (p) { + r = std::unique_ptr(new T(*p)); + } + return r; +} + +// This is a walkaround to avoid the static initialization of Value::null. +// kNull must be word-aligned to avoid crashing on ARM. We use an alignment of +// 8 (instead of 4) as a bit of future-proofing. +#if defined(__ARMEL__) +#define ALIGNAS(byte_alignment) __attribute__((aligned(byte_alignment))) +#else +#define ALIGNAS(byte_alignment) +#endif + +// static +Value const& Value::nullSingleton() { + static Value const nullStatic; + return nullStatic; +} + +#if JSON_USE_NULLREF +// for backwards compatibility, we'll leave these global references around, but +// DO NOT use them in JSONCPP library code any more! +// static +Value const& Value::null = Value::nullSingleton(); + +// static +Value const& Value::nullRef = Value::nullSingleton(); +#endif + +#if !defined(JSON_USE_INT64_DOUBLE_CONVERSION) +template +static inline bool InRange(double d, T min, U max) { + // The casts can lose precision, but we are looking only for + // an approximate range. Might fail on edge cases though. ~cdunn + return d >= static_cast(min) && d <= static_cast(max); +} +#else // if !defined(JSON_USE_INT64_DOUBLE_CONVERSION) +static inline double integerToDouble(Json::UInt64 value) { + return static_cast(Int64(value / 2)) * 2.0 + + static_cast(Int64(value & 1)); +} + +template static inline double integerToDouble(T value) { + return static_cast(value); +} + +template +static inline bool InRange(double d, T min, U max) { + return d >= integerToDouble(min) && d <= integerToDouble(max); +} +#endif // if !defined(JSON_USE_INT64_DOUBLE_CONVERSION) + +/** Duplicates the specified string value. + * @param value Pointer to the string to duplicate. Must be zero-terminated if + * length is "unknown". + * @param length Length of the value. if equals to unknown, then it will be + * computed using strlen(value). + * @return Pointer on the duplicate instance of string. + */ +static inline char* duplicateStringValue(const char* value, size_t length) { + // Avoid an integer overflow in the call to malloc below by limiting length + // to a sane value. + if (length >= static_cast(Value::maxInt)) + length = Value::maxInt - 1; + + auto newString = static_cast(malloc(length + 1)); + if (newString == nullptr) { + throwRuntimeError("in Json::Value::duplicateStringValue(): " + "Failed to allocate string value buffer"); + } + memcpy(newString, value, length); + newString[length] = 0; + return newString; +} + +/* Record the length as a prefix. + */ +static inline char* duplicateAndPrefixStringValue(const char* value, + unsigned int length) { + // Avoid an integer overflow in the call to malloc below by limiting length + // to a sane value. + JSON_ASSERT_MESSAGE(length <= static_cast(Value::maxInt) - + sizeof(unsigned) - 1U, + "in Json::Value::duplicateAndPrefixStringValue(): " + "length too big for prefixing"); + size_t actualLength = sizeof(length) + length + 1; + auto newString = static_cast(malloc(actualLength)); + if (newString == nullptr) { + throwRuntimeError("in Json::Value::duplicateAndPrefixStringValue(): " + "Failed to allocate string value buffer"); + } + *reinterpret_cast(newString) = length; + memcpy(newString + sizeof(unsigned), value, length); + newString[actualLength - 1U] = + 0; // to avoid buffer over-run accidents by users later + return newString; +} +inline static void decodePrefixedString(bool isPrefixed, char const* prefixed, + unsigned* length, char const** value) { + if (!isPrefixed) { + *length = static_cast(strlen(prefixed)); + *value = prefixed; + } else { + *length = *reinterpret_cast(prefixed); + *value = prefixed + sizeof(unsigned); + } +} +/** Free the string duplicated by + * duplicateStringValue()/duplicateAndPrefixStringValue(). + */ +#if JSONCPP_USING_SECURE_MEMORY +static inline void releasePrefixedStringValue(char* value) { + unsigned length = 0; + char const* valueDecoded; + decodePrefixedString(true, value, &length, &valueDecoded); + size_t const size = sizeof(unsigned) + length + 1U; + memset(value, 0, size); + free(value); +} +static inline void releaseStringValue(char* value, unsigned length) { + // length==0 => we allocated the strings memory + size_t size = (length == 0) ? strlen(value) : length; + memset(value, 0, size); + free(value); +} +#else // !JSONCPP_USING_SECURE_MEMORY +static inline void releasePrefixedStringValue(char* value) { free(value); } +static inline void releaseStringValue(char* value, unsigned) { free(value); } +#endif // JSONCPP_USING_SECURE_MEMORY + +} // namespace Json + +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ValueInternals... +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +#if !defined(JSON_IS_AMALGAMATION) + +#include "json_valueiterator.inl" +#endif // if !defined(JSON_IS_AMALGAMATION) + +namespace Json { + +#if JSON_USE_EXCEPTION +Exception::Exception(String msg) : msg_(std::move(msg)) {} +Exception::~Exception() noexcept = default; +char const* Exception::what() const noexcept { return msg_.c_str(); } +RuntimeError::RuntimeError(String const& msg) : Exception(msg) {} +LogicError::LogicError(String const& msg) : Exception(msg) {} +JSONCPP_NORETURN void throwRuntimeError(String const& msg) { + throw RuntimeError(msg); +} +JSONCPP_NORETURN void throwLogicError(String const& msg) { + throw LogicError(msg); +} +#else // !JSON_USE_EXCEPTION +JSONCPP_NORETURN void throwRuntimeError(String const& msg) { + std::cerr << msg << std::endl; + abort(); +} +JSONCPP_NORETURN void throwLogicError(String const& msg) { + std::cerr << msg << std::endl; + abort(); +} +#endif + +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// class Value::CZString +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// + +// Notes: policy_ indicates if the string was allocated when +// a string is stored. + +Value::CZString::CZString(ArrayIndex index) : cstr_(nullptr), index_(index) {} + +Value::CZString::CZString(char const* str, unsigned length, + DuplicationPolicy allocate) + : cstr_(str) { + // allocate != duplicate + storage_.policy_ = allocate & 0x3; + storage_.length_ = length & 0x3FFFFFFF; +} + +Value::CZString::CZString(const CZString& other) { + cstr_ = (other.storage_.policy_ != noDuplication && other.cstr_ != nullptr + ? duplicateStringValue(other.cstr_, other.storage_.length_) + : other.cstr_); + storage_.policy_ = + static_cast( + other.cstr_ + ? (static_cast(other.storage_.policy_) == + noDuplication + ? noDuplication + : duplicate) + : static_cast(other.storage_.policy_)) & + 3U; + storage_.length_ = other.storage_.length_; +} + +Value::CZString::CZString(CZString&& other) noexcept + : cstr_(other.cstr_), index_(other.index_) { + other.cstr_ = nullptr; +} + +Value::CZString::~CZString() { + if (cstr_ && storage_.policy_ == duplicate) { + releaseStringValue(const_cast(cstr_), + storage_.length_ + 1U); // +1 for null terminating + // character for sake of + // completeness but not actually + // necessary + } +} + +void Value::CZString::swap(CZString& other) { + std::swap(cstr_, other.cstr_); + std::swap(index_, other.index_); +} + +Value::CZString& Value::CZString::operator=(const CZString& other) { + cstr_ = other.cstr_; + index_ = other.index_; + return *this; +} + +Value::CZString& Value::CZString::operator=(CZString&& other) noexcept { + cstr_ = other.cstr_; + index_ = other.index_; + other.cstr_ = nullptr; + return *this; +} + +bool Value::CZString::operator<(const CZString& other) const { + if (!cstr_) + return index_ < other.index_; + // return strcmp(cstr_, other.cstr_) < 0; + // Assume both are strings. + unsigned this_len = this->storage_.length_; + unsigned other_len = other.storage_.length_; + unsigned min_len = std::min(this_len, other_len); + JSON_ASSERT(this->cstr_ && other.cstr_); + int comp = memcmp(this->cstr_, other.cstr_, min_len); + if (comp < 0) + return true; + if (comp > 0) + return false; + return (this_len < other_len); +} + +bool Value::CZString::operator==(const CZString& other) const { + if (!cstr_) + return index_ == other.index_; + // return strcmp(cstr_, other.cstr_) == 0; + // Assume both are strings. + unsigned this_len = this->storage_.length_; + unsigned other_len = other.storage_.length_; + if (this_len != other_len) + return false; + JSON_ASSERT(this->cstr_ && other.cstr_); + int comp = memcmp(this->cstr_, other.cstr_, this_len); + return comp == 0; +} + +ArrayIndex Value::CZString::index() const { return index_; } + +// const char* Value::CZString::c_str() const { return cstr_; } +const char* Value::CZString::data() const { return cstr_; } +unsigned Value::CZString::length() const { return storage_.length_; } +bool Value::CZString::isStaticString() const { + return storage_.policy_ == noDuplication; +} + +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// class Value::Value +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// + +/*! \internal Default constructor initialization must be equivalent to: + * memset( this, 0, sizeof(Value) ) + * This optimization is used in ValueInternalMap fast allocator. + */ +Value::Value(ValueType type) { + static char const emptyString[] = ""; + initBasic(type); + switch (type) { + case nullValue: + break; + case intValue: + case uintValue: + value_.int_ = 0; + break; + case realValue: + value_.real_ = 0.0; + break; + case stringValue: + // allocated_ == false, so this is safe. + value_.string_ = const_cast(static_cast(emptyString)); + break; + case arrayValue: + case objectValue: + value_.map_ = new ObjectValues(); + break; + case booleanValue: + value_.bool_ = false; + break; + default: + JSON_ASSERT_UNREACHABLE; + } +} + +Value::Value(Int value) { + initBasic(intValue); + value_.int_ = value; +} + +Value::Value(UInt value) { + initBasic(uintValue); + value_.uint_ = value; +} +#if defined(JSON_HAS_INT64) +Value::Value(Int64 value) { + initBasic(intValue); + value_.int_ = value; +} +Value::Value(UInt64 value) { + initBasic(uintValue); + value_.uint_ = value; +} +#endif // defined(JSON_HAS_INT64) + +Value::Value(double value) { + initBasic(realValue); + value_.real_ = value; +} + +Value::Value(const char* value) { + initBasic(stringValue, true); + JSON_ASSERT_MESSAGE(value != nullptr, + "Null Value Passed to Value Constructor"); + value_.string_ = duplicateAndPrefixStringValue( + value, static_cast(strlen(value))); +} + +Value::Value(const char* begin, const char* end) { + initBasic(stringValue, true); + value_.string_ = + duplicateAndPrefixStringValue(begin, static_cast(end - begin)); +} + +Value::Value(const String& value) { + initBasic(stringValue, true); + value_.string_ = duplicateAndPrefixStringValue( + value.data(), static_cast(value.length())); +} + +Value::Value(const StaticString& value) { + initBasic(stringValue); + value_.string_ = const_cast(value.c_str()); +} + +Value::Value(bool value) { + initBasic(booleanValue); + value_.bool_ = value; +} + +Value::Value(const Value& other) { + dupPayload(other); + dupMeta(other); +} + +Value::Value(Value&& other) noexcept { + initBasic(nullValue); + swap(other); +} + +Value::~Value() { + releasePayload(); + value_.uint_ = 0; +} + +Value& Value::operator=(const Value& other) { + Value(other).swap(*this); + return *this; +} + +Value& Value::operator=(Value&& other) noexcept { + other.swap(*this); + return *this; +} + +void Value::swapPayload(Value& other) { + std::swap(bits_, other.bits_); + std::swap(value_, other.value_); +} + +void Value::copyPayload(const Value& other) { + releasePayload(); + dupPayload(other); +} + +void Value::swap(Value& other) { + swapPayload(other); + std::swap(comments_, other.comments_); + std::swap(start_, other.start_); + std::swap(limit_, other.limit_); +} + +void Value::copy(const Value& other) { + copyPayload(other); + dupMeta(other); +} + +ValueType Value::type() const { + return static_cast(bits_.value_type_); +} + +int Value::compare(const Value& other) const { + if (*this < other) + return -1; + if (*this > other) + return 1; + return 0; +} + +bool Value::operator<(const Value& other) const { + int typeDelta = type() - other.type(); + if (typeDelta) + return typeDelta < 0; + switch (type()) { + case nullValue: + return false; + case intValue: + return value_.int_ < other.value_.int_; + case uintValue: + return value_.uint_ < other.value_.uint_; + case realValue: + return value_.real_ < other.value_.real_; + case booleanValue: + return value_.bool_ < other.value_.bool_; + case stringValue: { + if ((value_.string_ == nullptr) || (other.value_.string_ == nullptr)) { + return other.value_.string_ != nullptr; + } + unsigned this_len; + unsigned other_len; + char const* this_str; + char const* other_str; + decodePrefixedString(this->isAllocated(), this->value_.string_, &this_len, + &this_str); + decodePrefixedString(other.isAllocated(), other.value_.string_, &other_len, + &other_str); + unsigned min_len = std::min(this_len, other_len); + JSON_ASSERT(this_str && other_str); + int comp = memcmp(this_str, other_str, min_len); + if (comp < 0) + return true; + if (comp > 0) + return false; + return (this_len < other_len); + } + case arrayValue: + case objectValue: { + auto thisSize = value_.map_->size(); + auto otherSize = other.value_.map_->size(); + if (thisSize != otherSize) + return thisSize < otherSize; + return (*value_.map_) < (*other.value_.map_); + } + default: + JSON_ASSERT_UNREACHABLE; + } + return false; // unreachable +} + +bool Value::operator<=(const Value& other) const { return !(other < *this); } + +bool Value::operator>=(const Value& other) const { return !(*this < other); } + +bool Value::operator>(const Value& other) const { return other < *this; } + +bool Value::operator==(const Value& other) const { + if (type() != other.type()) + return false; + switch (type()) { + case nullValue: + return true; + case intValue: + return value_.int_ == other.value_.int_; + case uintValue: + return value_.uint_ == other.value_.uint_; + case realValue: + return value_.real_ == other.value_.real_; + case booleanValue: + return value_.bool_ == other.value_.bool_; + case stringValue: { + if ((value_.string_ == nullptr) || (other.value_.string_ == nullptr)) { + return (value_.string_ == other.value_.string_); + } + unsigned this_len; + unsigned other_len; + char const* this_str; + char const* other_str; + decodePrefixedString(this->isAllocated(), this->value_.string_, &this_len, + &this_str); + decodePrefixedString(other.isAllocated(), other.value_.string_, &other_len, + &other_str); + if (this_len != other_len) + return false; + JSON_ASSERT(this_str && other_str); + int comp = memcmp(this_str, other_str, this_len); + return comp == 0; + } + case arrayValue: + case objectValue: + return value_.map_->size() == other.value_.map_->size() && + (*value_.map_) == (*other.value_.map_); + default: + JSON_ASSERT_UNREACHABLE; + } + return false; // unreachable +} + +bool Value::operator!=(const Value& other) const { return !(*this == other); } + +const char* Value::asCString() const { + JSON_ASSERT_MESSAGE(type() == stringValue, + "in Json::Value::asCString(): requires stringValue"); + if (value_.string_ == nullptr) + return nullptr; + unsigned this_len; + char const* this_str; + decodePrefixedString(this->isAllocated(), this->value_.string_, &this_len, + &this_str); + return this_str; +} + +#if JSONCPP_USING_SECURE_MEMORY +unsigned Value::getCStringLength() const { + JSON_ASSERT_MESSAGE(type() == stringValue, + "in Json::Value::asCString(): requires stringValue"); + if (value_.string_ == 0) + return 0; + unsigned this_len; + char const* this_str; + decodePrefixedString(this->isAllocated(), this->value_.string_, &this_len, + &this_str); + return this_len; +} +#endif + +bool Value::getString(char const** begin, char const** end) const { + if (type() != stringValue) + return false; + if (value_.string_ == nullptr) + return false; + unsigned length; + decodePrefixedString(this->isAllocated(), this->value_.string_, &length, + begin); + *end = *begin + length; + return true; +} + +String Value::asString() const { + switch (type()) { + case nullValue: + return ""; + case stringValue: { + if (value_.string_ == nullptr) + return ""; + unsigned this_len; + char const* this_str; + decodePrefixedString(this->isAllocated(), this->value_.string_, &this_len, + &this_str); + return String(this_str, this_len); + } + case booleanValue: + return value_.bool_ ? "true" : "false"; + case intValue: + return valueToString(value_.int_); + case uintValue: + return valueToString(value_.uint_); + case realValue: + return valueToString(value_.real_); + default: + JSON_FAIL_MESSAGE("Type is not convertible to string"); + } +} + +Value::Int Value::asInt() const { + switch (type()) { + case intValue: + JSON_ASSERT_MESSAGE(isInt(), "LargestInt out of Int range"); + return Int(value_.int_); + case uintValue: + JSON_ASSERT_MESSAGE(isInt(), "LargestUInt out of Int range"); + return Int(value_.uint_); + case realValue: + JSON_ASSERT_MESSAGE(InRange(value_.real_, minInt, maxInt), + "double out of Int range"); + return Int(value_.real_); + case nullValue: + return 0; + case booleanValue: + return value_.bool_ ? 1 : 0; + default: + break; + } + JSON_FAIL_MESSAGE("Value is not convertible to Int."); +} + +Value::UInt Value::asUInt() const { + switch (type()) { + case intValue: + JSON_ASSERT_MESSAGE(isUInt(), "LargestInt out of UInt range"); + return UInt(value_.int_); + case uintValue: + JSON_ASSERT_MESSAGE(isUInt(), "LargestUInt out of UInt range"); + return UInt(value_.uint_); + case realValue: + JSON_ASSERT_MESSAGE(InRange(value_.real_, 0, maxUInt), + "double out of UInt range"); + return UInt(value_.real_); + case nullValue: + return 0; + case booleanValue: + return value_.bool_ ? 1 : 0; + default: + break; + } + JSON_FAIL_MESSAGE("Value is not convertible to UInt."); +} + +#if defined(JSON_HAS_INT64) + +Value::Int64 Value::asInt64() const { + switch (type()) { + case intValue: + return Int64(value_.int_); + case uintValue: + JSON_ASSERT_MESSAGE(isInt64(), "LargestUInt out of Int64 range"); + return Int64(value_.uint_); + case realValue: + JSON_ASSERT_MESSAGE(InRange(value_.real_, minInt64, maxInt64), + "double out of Int64 range"); + return Int64(value_.real_); + case nullValue: + return 0; + case booleanValue: + return value_.bool_ ? 1 : 0; + default: + break; + } + JSON_FAIL_MESSAGE("Value is not convertible to Int64."); +} + +Value::UInt64 Value::asUInt64() const { + switch (type()) { + case intValue: + JSON_ASSERT_MESSAGE(isUInt64(), "LargestInt out of UInt64 range"); + return UInt64(value_.int_); + case uintValue: + return UInt64(value_.uint_); + case realValue: + JSON_ASSERT_MESSAGE(InRange(value_.real_, 0, maxUInt64), + "double out of UInt64 range"); + return UInt64(value_.real_); + case nullValue: + return 0; + case booleanValue: + return value_.bool_ ? 1 : 0; + default: + break; + } + JSON_FAIL_MESSAGE("Value is not convertible to UInt64."); +} +#endif // if defined(JSON_HAS_INT64) + +LargestInt Value::asLargestInt() const { +#if defined(JSON_NO_INT64) + return asInt(); +#else + return asInt64(); +#endif +} + +LargestUInt Value::asLargestUInt() const { +#if defined(JSON_NO_INT64) + return asUInt(); +#else + return asUInt64(); +#endif +} + +double Value::asDouble() const { + switch (type()) { + case intValue: + return static_cast(value_.int_); + case uintValue: +#if !defined(JSON_USE_INT64_DOUBLE_CONVERSION) + return static_cast(value_.uint_); +#else // if !defined(JSON_USE_INT64_DOUBLE_CONVERSION) + return integerToDouble(value_.uint_); +#endif // if !defined(JSON_USE_INT64_DOUBLE_CONVERSION) + case realValue: + return value_.real_; + case nullValue: + return 0.0; + case booleanValue: + return value_.bool_ ? 1.0 : 0.0; + default: + break; + } + JSON_FAIL_MESSAGE("Value is not convertible to double."); +} + +float Value::asFloat() const { + switch (type()) { + case intValue: + return static_cast(value_.int_); + case uintValue: +#if !defined(JSON_USE_INT64_DOUBLE_CONVERSION) + return static_cast(value_.uint_); +#else // if !defined(JSON_USE_INT64_DOUBLE_CONVERSION) + // This can fail (silently?) if the value is bigger than MAX_FLOAT. + return static_cast(integerToDouble(value_.uint_)); +#endif // if !defined(JSON_USE_INT64_DOUBLE_CONVERSION) + case realValue: + return static_cast(value_.real_); + case nullValue: + return 0.0; + case booleanValue: + return value_.bool_ ? 1.0F : 0.0F; + default: + break; + } + JSON_FAIL_MESSAGE("Value is not convertible to float."); +} + +bool Value::asBool() const { + switch (type()) { + case booleanValue: + return value_.bool_; + case nullValue: + return false; + case intValue: + return value_.int_ != 0; + case uintValue: + return value_.uint_ != 0; + case realValue: { + // According to JavaScript language zero or NaN is regarded as false + const auto value_classification = std::fpclassify(value_.real_); + return value_classification != FP_ZERO && value_classification != FP_NAN; + } + default: + break; + } + JSON_FAIL_MESSAGE("Value is not convertible to bool."); +} + +bool Value::isConvertibleTo(ValueType other) const { + switch (other) { + case nullValue: + return (isNumeric() && asDouble() == 0.0) || + (type() == booleanValue && !value_.bool_) || + (type() == stringValue && asString().empty()) || + (type() == arrayValue && value_.map_->empty()) || + (type() == objectValue && value_.map_->empty()) || + type() == nullValue; + case intValue: + return isInt() || + (type() == realValue && InRange(value_.real_, minInt, maxInt)) || + type() == booleanValue || type() == nullValue; + case uintValue: + return isUInt() || + (type() == realValue && InRange(value_.real_, 0, maxUInt)) || + type() == booleanValue || type() == nullValue; + case realValue: + return isNumeric() || type() == booleanValue || type() == nullValue; + case booleanValue: + return isNumeric() || type() == booleanValue || type() == nullValue; + case stringValue: + return isNumeric() || type() == booleanValue || type() == stringValue || + type() == nullValue; + case arrayValue: + return type() == arrayValue || type() == nullValue; + case objectValue: + return type() == objectValue || type() == nullValue; + } + JSON_ASSERT_UNREACHABLE; + return false; +} + +/// Number of values in array or object +ArrayIndex Value::size() const { + switch (type()) { + case nullValue: + case intValue: + case uintValue: + case realValue: + case booleanValue: + case stringValue: + return 0; + case arrayValue: // size of the array is highest index + 1 + if (!value_.map_->empty()) { + ObjectValues::const_iterator itLast = value_.map_->end(); + --itLast; + return (*itLast).first.index() + 1; + } + return 0; + case objectValue: + return ArrayIndex(value_.map_->size()); + } + JSON_ASSERT_UNREACHABLE; + return 0; // unreachable; +} + +bool Value::empty() const { + if (isNull() || isArray() || isObject()) + return size() == 0U; + return false; +} + +Value::operator bool() const { return !isNull(); } + +void Value::clear() { + JSON_ASSERT_MESSAGE(type() == nullValue || type() == arrayValue || + type() == objectValue, + "in Json::Value::clear(): requires complex value"); + start_ = 0; + limit_ = 0; + switch (type()) { + case arrayValue: + case objectValue: + value_.map_->clear(); + break; + default: + break; + } +} + +void Value::resize(ArrayIndex newSize) { + JSON_ASSERT_MESSAGE(type() == nullValue || type() == arrayValue, + "in Json::Value::resize(): requires arrayValue"); + if (type() == nullValue) + *this = Value(arrayValue); + ArrayIndex oldSize = size(); + if (newSize == 0) + clear(); + else if (newSize > oldSize) + for (ArrayIndex i = oldSize; i < newSize; ++i) + (*this)[i]; + else { + for (ArrayIndex index = newSize; index < oldSize; ++index) { + value_.map_->erase(index); + } + JSON_ASSERT(size() == newSize); + } +} + +Value& Value::operator[](ArrayIndex index) { + JSON_ASSERT_MESSAGE( + type() == nullValue || type() == arrayValue, + "in Json::Value::operator[](ArrayIndex): requires arrayValue"); + if (type() == nullValue) + *this = Value(arrayValue); + CZString key(index); + auto it = value_.map_->lower_bound(key); + if (it != value_.map_->end() && (*it).first == key) + return (*it).second; + + ObjectValues::value_type defaultValue(key, nullSingleton()); + it = value_.map_->insert(it, defaultValue); + return (*it).second; +} + +Value& Value::operator[](int index) { + JSON_ASSERT_MESSAGE( + index >= 0, + "in Json::Value::operator[](int index): index cannot be negative"); + return (*this)[ArrayIndex(index)]; +} + +const Value& Value::operator[](ArrayIndex index) const { + JSON_ASSERT_MESSAGE( + type() == nullValue || type() == arrayValue, + "in Json::Value::operator[](ArrayIndex)const: requires arrayValue"); + if (type() == nullValue) + return nullSingleton(); + CZString key(index); + ObjectValues::const_iterator it = value_.map_->find(key); + if (it == value_.map_->end()) + return nullSingleton(); + return (*it).second; +} + +const Value& Value::operator[](int index) const { + JSON_ASSERT_MESSAGE( + index >= 0, + "in Json::Value::operator[](int index) const: index cannot be negative"); + return (*this)[ArrayIndex(index)]; +} + +void Value::initBasic(ValueType type, bool allocated) { + setType(type); + setIsAllocated(allocated); + comments_ = Comments{}; + start_ = 0; + limit_ = 0; +} + +void Value::dupPayload(const Value& other) { + setType(other.type()); + setIsAllocated(false); + switch (type()) { + case nullValue: + case intValue: + case uintValue: + case realValue: + case booleanValue: + value_ = other.value_; + break; + case stringValue: + if (other.value_.string_ && other.isAllocated()) { + unsigned len; + char const* str; + decodePrefixedString(other.isAllocated(), other.value_.string_, &len, + &str); + value_.string_ = duplicateAndPrefixStringValue(str, len); + setIsAllocated(true); + } else { + value_.string_ = other.value_.string_; + } + break; + case arrayValue: + case objectValue: + value_.map_ = new ObjectValues(*other.value_.map_); + break; + default: + JSON_ASSERT_UNREACHABLE; + } +} + +void Value::releasePayload() { + switch (type()) { + case nullValue: + case intValue: + case uintValue: + case realValue: + case booleanValue: + break; + case stringValue: + if (isAllocated()) + releasePrefixedStringValue(value_.string_); + break; + case arrayValue: + case objectValue: + delete value_.map_; + break; + default: + JSON_ASSERT_UNREACHABLE; + } +} + +void Value::dupMeta(const Value& other) { + comments_ = other.comments_; + start_ = other.start_; + limit_ = other.limit_; +} + +// Access an object value by name, create a null member if it does not exist. +// @pre Type of '*this' is object or null. +// @param key is null-terminated. +Value& Value::resolveReference(const char* key) { + JSON_ASSERT_MESSAGE( + type() == nullValue || type() == objectValue, + "in Json::Value::resolveReference(): requires objectValue"); + if (type() == nullValue) + *this = Value(objectValue); + CZString actualKey(key, static_cast(strlen(key)), + CZString::noDuplication); // NOTE! + auto it = value_.map_->lower_bound(actualKey); + if (it != value_.map_->end() && (*it).first == actualKey) + return (*it).second; + + ObjectValues::value_type defaultValue(actualKey, nullSingleton()); + it = value_.map_->insert(it, defaultValue); + Value& value = (*it).second; + return value; +} + +// @param key is not null-terminated. +Value& Value::resolveReference(char const* key, char const* end) { + JSON_ASSERT_MESSAGE( + type() == nullValue || type() == objectValue, + "in Json::Value::resolveReference(key, end): requires objectValue"); + if (type() == nullValue) + *this = Value(objectValue); + CZString actualKey(key, static_cast(end - key), + CZString::duplicateOnCopy); + auto it = value_.map_->lower_bound(actualKey); + if (it != value_.map_->end() && (*it).first == actualKey) + return (*it).second; + + ObjectValues::value_type defaultValue(actualKey, nullSingleton()); + it = value_.map_->insert(it, defaultValue); + Value& value = (*it).second; + return value; +} + +Value Value::get(ArrayIndex index, const Value& defaultValue) const { + const Value* value = &((*this)[index]); + return value == &nullSingleton() ? defaultValue : *value; +} + +bool Value::isValidIndex(ArrayIndex index) const { return index < size(); } + +Value const* Value::find(char const* begin, char const* end) const { + JSON_ASSERT_MESSAGE(type() == nullValue || type() == objectValue, + "in Json::Value::find(begin, end): requires " + "objectValue or nullValue"); + if (type() == nullValue) + return nullptr; + CZString actualKey(begin, static_cast(end - begin), + CZString::noDuplication); + ObjectValues::const_iterator it = value_.map_->find(actualKey); + if (it == value_.map_->end()) + return nullptr; + return &(*it).second; +} +Value* Value::demand(char const* begin, char const* end) { + JSON_ASSERT_MESSAGE(type() == nullValue || type() == objectValue, + "in Json::Value::demand(begin, end): requires " + "objectValue or nullValue"); + return &resolveReference(begin, end); +} +const Value& Value::operator[](const char* key) const { + Value const* found = find(key, key + strlen(key)); + if (!found) + return nullSingleton(); + return *found; +} +Value const& Value::operator[](const String& key) const { + Value const* found = find(key.data(), key.data() + key.length()); + if (!found) + return nullSingleton(); + return *found; +} + +Value& Value::operator[](const char* key) { + return resolveReference(key, key + strlen(key)); +} + +Value& Value::operator[](const String& key) { + return resolveReference(key.data(), key.data() + key.length()); +} + +Value& Value::operator[](const StaticString& key) { + return resolveReference(key.c_str()); +} + +Value& Value::append(const Value& value) { return append(Value(value)); } + +Value& Value::append(Value&& value) { + JSON_ASSERT_MESSAGE(type() == nullValue || type() == arrayValue, + "in Json::Value::append: requires arrayValue"); + if (type() == nullValue) { + *this = Value(arrayValue); + } + return this->value_.map_->emplace(size(), std::move(value)).first->second; +} + +bool Value::insert(ArrayIndex index, const Value& newValue) { + return insert(index, Value(newValue)); +} + +bool Value::insert(ArrayIndex index, Value&& newValue) { + JSON_ASSERT_MESSAGE(type() == nullValue || type() == arrayValue, + "in Json::Value::insert: requires arrayValue"); + ArrayIndex length = size(); + if (index > length) { + return false; + } + for (ArrayIndex i = length; i > index; i--) { + (*this)[i] = std::move((*this)[i - 1]); + } + (*this)[index] = std::move(newValue); + return true; +} + +Value Value::get(char const* begin, char const* end, + Value const& defaultValue) const { + Value const* found = find(begin, end); + return !found ? defaultValue : *found; +} +Value Value::get(char const* key, Value const& defaultValue) const { + return get(key, key + strlen(key), defaultValue); +} +Value Value::get(String const& key, Value const& defaultValue) const { + return get(key.data(), key.data() + key.length(), defaultValue); +} + +bool Value::removeMember(const char* begin, const char* end, Value* removed) { + if (type() != objectValue) { + return false; + } + CZString actualKey(begin, static_cast(end - begin), + CZString::noDuplication); + auto it = value_.map_->find(actualKey); + if (it == value_.map_->end()) + return false; + if (removed) + *removed = std::move(it->second); + value_.map_->erase(it); + return true; +} +bool Value::removeMember(const char* key, Value* removed) { + return removeMember(key, key + strlen(key), removed); +} +bool Value::removeMember(String const& key, Value* removed) { + return removeMember(key.data(), key.data() + key.length(), removed); +} +void Value::removeMember(const char* key) { + JSON_ASSERT_MESSAGE(type() == nullValue || type() == objectValue, + "in Json::Value::removeMember(): requires objectValue"); + if (type() == nullValue) + return; + + CZString actualKey(key, unsigned(strlen(key)), CZString::noDuplication); + value_.map_->erase(actualKey); +} +void Value::removeMember(const String& key) { removeMember(key.c_str()); } + +bool Value::removeIndex(ArrayIndex index, Value* removed) { + if (type() != arrayValue) { + return false; + } + CZString key(index); + auto it = value_.map_->find(key); + if (it == value_.map_->end()) { + return false; + } + if (removed) + *removed = it->second; + ArrayIndex oldSize = size(); + // shift left all items left, into the place of the "removed" + for (ArrayIndex i = index; i < (oldSize - 1); ++i) { + CZString keey(i); + (*value_.map_)[keey] = (*this)[i + 1]; + } + // erase the last one ("leftover") + CZString keyLast(oldSize - 1); + auto itLast = value_.map_->find(keyLast); + value_.map_->erase(itLast); + return true; +} + +bool Value::isMember(char const* begin, char const* end) const { + Value const* value = find(begin, end); + return nullptr != value; +} +bool Value::isMember(char const* key) const { + return isMember(key, key + strlen(key)); +} +bool Value::isMember(String const& key) const { + return isMember(key.data(), key.data() + key.length()); +} + +Value::Members Value::getMemberNames() const { + JSON_ASSERT_MESSAGE( + type() == nullValue || type() == objectValue, + "in Json::Value::getMemberNames(), value must be objectValue"); + if (type() == nullValue) + return Value::Members(); + Members members; + members.reserve(value_.map_->size()); + ObjectValues::const_iterator it = value_.map_->begin(); + ObjectValues::const_iterator itEnd = value_.map_->end(); + for (; it != itEnd; ++it) { + members.push_back(String((*it).first.data(), (*it).first.length())); + } + return members; +} + +static bool IsIntegral(double d) { + double integral_part; + return modf(d, &integral_part) == 0.0; +} + +bool Value::isNull() const { return type() == nullValue; } + +bool Value::isBool() const { return type() == booleanValue; } + +bool Value::isInt() const { + switch (type()) { + case intValue: +#if defined(JSON_HAS_INT64) + return value_.int_ >= minInt && value_.int_ <= maxInt; +#else + return true; +#endif + case uintValue: + return value_.uint_ <= UInt(maxInt); + case realValue: + return value_.real_ >= minInt && value_.real_ <= maxInt && + IsIntegral(value_.real_); + default: + break; + } + return false; +} + +bool Value::isUInt() const { + switch (type()) { + case intValue: +#if defined(JSON_HAS_INT64) + return value_.int_ >= 0 && LargestUInt(value_.int_) <= LargestUInt(maxUInt); +#else + return value_.int_ >= 0; +#endif + case uintValue: +#if defined(JSON_HAS_INT64) + return value_.uint_ <= maxUInt; +#else + return true; +#endif + case realValue: + return value_.real_ >= 0 && value_.real_ <= maxUInt && + IsIntegral(value_.real_); + default: + break; + } + return false; +} + +bool Value::isInt64() const { +#if defined(JSON_HAS_INT64) + switch (type()) { + case intValue: + return true; + case uintValue: + return value_.uint_ <= UInt64(maxInt64); + case realValue: + // Note that maxInt64 (= 2^63 - 1) is not exactly representable as a + // double, so double(maxInt64) will be rounded up to 2^63. Therefore we + // require the value to be strictly less than the limit. + return value_.real_ >= double(minInt64) && + value_.real_ < double(maxInt64) && IsIntegral(value_.real_); + default: + break; + } +#endif // JSON_HAS_INT64 + return false; +} + +bool Value::isUInt64() const { +#if defined(JSON_HAS_INT64) + switch (type()) { + case intValue: + return value_.int_ >= 0; + case uintValue: + return true; + case realValue: + // Note that maxUInt64 (= 2^64 - 1) is not exactly representable as a + // double, so double(maxUInt64) will be rounded up to 2^64. Therefore we + // require the value to be strictly less than the limit. + return value_.real_ >= 0 && value_.real_ < maxUInt64AsDouble && + IsIntegral(value_.real_); + default: + break; + } +#endif // JSON_HAS_INT64 + return false; +} + +bool Value::isIntegral() const { + switch (type()) { + case intValue: + case uintValue: + return true; + case realValue: +#if defined(JSON_HAS_INT64) + // Note that maxUInt64 (= 2^64 - 1) is not exactly representable as a + // double, so double(maxUInt64) will be rounded up to 2^64. Therefore we + // require the value to be strictly less than the limit. + return value_.real_ >= double(minInt64) && + value_.real_ < maxUInt64AsDouble && IsIntegral(value_.real_); +#else + return value_.real_ >= minInt && value_.real_ <= maxUInt && + IsIntegral(value_.real_); +#endif // JSON_HAS_INT64 + default: + break; + } + return false; +} + +bool Value::isDouble() const { + return type() == intValue || type() == uintValue || type() == realValue; +} + +bool Value::isNumeric() const { return isDouble(); } + +bool Value::isString() const { return type() == stringValue; } + +bool Value::isArray() const { return type() == arrayValue; } + +bool Value::isObject() const { return type() == objectValue; } + +Value::Comments::Comments(const Comments& that) + : ptr_{cloneUnique(that.ptr_)} {} + +Value::Comments::Comments(Comments&& that) noexcept + : ptr_{std::move(that.ptr_)} {} + +Value::Comments& Value::Comments::operator=(const Comments& that) { + ptr_ = cloneUnique(that.ptr_); + return *this; +} + +Value::Comments& Value::Comments::operator=(Comments&& that) noexcept { + ptr_ = std::move(that.ptr_); + return *this; +} + +bool Value::Comments::has(CommentPlacement slot) const { + return ptr_ && !(*ptr_)[slot].empty(); +} + +String Value::Comments::get(CommentPlacement slot) const { + if (!ptr_) + return {}; + return (*ptr_)[slot]; +} + +void Value::Comments::set(CommentPlacement slot, String comment) { + if (slot >= CommentPlacement::numberOfCommentPlacement) + return; + if (!ptr_) + ptr_ = std::unique_ptr(new Array()); + (*ptr_)[slot] = std::move(comment); +} + +void Value::setComment(String comment, CommentPlacement placement) { + if (!comment.empty() && (comment.back() == '\n')) { + // Always discard trailing newline, to aid indentation. + comment.pop_back(); + } + JSON_ASSERT(!comment.empty()); + JSON_ASSERT_MESSAGE( + comment[0] == '\0' || comment[0] == '/', + "in Json::Value::setComment(): Comments must start with /"); + comments_.set(placement, std::move(comment)); +} + +bool Value::hasComment(CommentPlacement placement) const { + return comments_.has(placement); +} + +String Value::getComment(CommentPlacement placement) const { + return comments_.get(placement); +} + +void Value::setOffsetStart(ptrdiff_t start) { start_ = start; } + +void Value::setOffsetLimit(ptrdiff_t limit) { limit_ = limit; } + +ptrdiff_t Value::getOffsetStart() const { return start_; } + +ptrdiff_t Value::getOffsetLimit() const { return limit_; } + +String Value::toStyledString() const { + StreamWriterBuilder builder; + + String out = this->hasComment(commentBefore) ? "\n" : ""; + out += Json::writeString(builder, *this); + out += '\n'; + + return out; +} + +Value::const_iterator Value::begin() const { + switch (type()) { + case arrayValue: + case objectValue: + if (value_.map_) + return const_iterator(value_.map_->begin()); + break; + default: + break; + } + return {}; +} + +Value::const_iterator Value::end() const { + switch (type()) { + case arrayValue: + case objectValue: + if (value_.map_) + return const_iterator(value_.map_->end()); + break; + default: + break; + } + return {}; +} + +Value::iterator Value::begin() { + switch (type()) { + case arrayValue: + case objectValue: + if (value_.map_) + return iterator(value_.map_->begin()); + break; + default: + break; + } + return iterator(); +} + +Value::iterator Value::end() { + switch (type()) { + case arrayValue: + case objectValue: + if (value_.map_) + return iterator(value_.map_->end()); + break; + default: + break; + } + return iterator(); +} + +// class PathArgument +// ////////////////////////////////////////////////////////////////// + +PathArgument::PathArgument() = default; + +PathArgument::PathArgument(ArrayIndex index) + : index_(index), kind_(kindIndex) {} + +PathArgument::PathArgument(const char* key) : key_(key), kind_(kindKey) {} + +PathArgument::PathArgument(String key) : key_(std::move(key)), kind_(kindKey) {} + +// class Path +// ////////////////////////////////////////////////////////////////// + +Path::Path(const String& path, const PathArgument& a1, const PathArgument& a2, + const PathArgument& a3, const PathArgument& a4, + const PathArgument& a5) { + InArgs in; + in.reserve(5); + in.push_back(&a1); + in.push_back(&a2); + in.push_back(&a3); + in.push_back(&a4); + in.push_back(&a5); + makePath(path, in); +} + +void Path::makePath(const String& path, const InArgs& in) { + const char* current = path.c_str(); + const char* end = current + path.length(); + auto itInArg = in.begin(); + while (current != end) { + if (*current == '[') { + ++current; + if (*current == '%') + addPathInArg(path, in, itInArg, PathArgument::kindIndex); + else { + ArrayIndex index = 0; + for (; current != end && *current >= '0' && *current <= '9'; ++current) + index = index * 10 + ArrayIndex(*current - '0'); + args_.push_back(index); + } + if (current == end || *++current != ']') + invalidPath(path, int(current - path.c_str())); + } else if (*current == '%') { + addPathInArg(path, in, itInArg, PathArgument::kindKey); + ++current; + } else if (*current == '.' || *current == ']') { + ++current; + } else { + const char* beginName = current; + while (current != end && !strchr("[.", *current)) + ++current; + args_.push_back(String(beginName, current)); + } + } +} + +void Path::addPathInArg(const String& /*path*/, const InArgs& in, + InArgs::const_iterator& itInArg, + PathArgument::Kind kind) { + if (itInArg == in.end()) { + // Error: missing argument %d + } else if ((*itInArg)->kind_ != kind) { + // Error: bad argument type + } else { + args_.push_back(**itInArg++); + } +} + +void Path::invalidPath(const String& /*path*/, int /*location*/) { + // Error: invalid path. +} + +const Value& Path::resolve(const Value& root) const { + const Value* node = &root; + for (const auto& arg : args_) { + if (arg.kind_ == PathArgument::kindIndex) { + if (!node->isArray() || !node->isValidIndex(arg.index_)) { + // Error: unable to resolve path (array value expected at position... ) + return Value::nullSingleton(); + } + node = &((*node)[arg.index_]); + } else if (arg.kind_ == PathArgument::kindKey) { + if (!node->isObject()) { + // Error: unable to resolve path (object value expected at position...) + return Value::nullSingleton(); + } + node = &((*node)[arg.key_]); + if (node == &Value::nullSingleton()) { + // Error: unable to resolve path (object has no member named '' at + // position...) + return Value::nullSingleton(); + } + } + } + return *node; +} + +Value Path::resolve(const Value& root, const Value& defaultValue) const { + const Value* node = &root; + for (const auto& arg : args_) { + if (arg.kind_ == PathArgument::kindIndex) { + if (!node->isArray() || !node->isValidIndex(arg.index_)) + return defaultValue; + node = &((*node)[arg.index_]); + } else if (arg.kind_ == PathArgument::kindKey) { + if (!node->isObject()) + return defaultValue; + node = &((*node)[arg.key_]); + if (node == &Value::nullSingleton()) + return defaultValue; + } + } + return *node; +} + +Value& Path::make(Value& root) const { + Value* node = &root; + for (const auto& arg : args_) { + if (arg.kind_ == PathArgument::kindIndex) { + if (!node->isArray()) { + // Error: node is not an array at position ... + } + node = &((*node)[arg.index_]); + } else if (arg.kind_ == PathArgument::kindKey) { + if (!node->isObject()) { + // Error: node is not an object at position... + } + node = &((*node)[arg.key_]); + } + } + return *node; +} + +} // namespace Json + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: src/lib_json/json_value.cpp +// ////////////////////////////////////////////////////////////////////// + + + + + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: src/lib_json/json_writer.cpp +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2011 Baptiste Lepilleur and The JsonCpp Authors +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +#if !defined(JSON_IS_AMALGAMATION) +#include "json_tool.h" +#include +#endif // if !defined(JSON_IS_AMALGAMATION) +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#if __cplusplus >= 201103L +#include +#include + +#if !defined(isnan) +#define isnan std::isnan +#endif + +#if !defined(isfinite) +#define isfinite std::isfinite +#endif + +#else +#include +#include + +#if defined(_MSC_VER) +#if !defined(isnan) +#include +#define isnan _isnan +#endif + +#if !defined(isfinite) +#include +#define isfinite _finite +#endif + +#if !defined(_CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES) +#define _CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES 1 +#endif //_CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES + +#endif //_MSC_VER + +#if defined(__sun) && defined(__SVR4) // Solaris +#if !defined(isfinite) +#include +#define isfinite finite +#endif +#endif + +#if defined(__hpux) +#if !defined(isfinite) +#if defined(__ia64) && !defined(finite) +#define isfinite(x) \ + ((sizeof(x) == sizeof(float) ? _Isfinitef(x) : _IsFinite(x))) +#endif +#endif +#endif + +#if !defined(isnan) +// IEEE standard states that NaN values will not compare to themselves +#define isnan(x) ((x) != (x)) +#endif + +#if !defined(__APPLE__) +#if !defined(isfinite) +#define isfinite finite +#endif +#endif +#endif + +#if defined(_MSC_VER) +// Disable warning about strdup being deprecated. +#pragma warning(disable : 4996) +#endif + +namespace Json { + +#if __cplusplus >= 201103L || (defined(_CPPLIB_VER) && _CPPLIB_VER >= 520) +using StreamWriterPtr = std::unique_ptr; +#else +using StreamWriterPtr = std::auto_ptr; +#endif + +String valueToString(LargestInt value) { + UIntToStringBuffer buffer; + char* current = buffer + sizeof(buffer); + if (value == Value::minLargestInt) { + uintToString(LargestUInt(Value::maxLargestInt) + 1, current); + *--current = '-'; + } else if (value < 0) { + uintToString(LargestUInt(-value), current); + *--current = '-'; + } else { + uintToString(LargestUInt(value), current); + } + assert(current >= buffer); + return current; +} + +String valueToString(LargestUInt value) { + UIntToStringBuffer buffer; + char* current = buffer + sizeof(buffer); + uintToString(value, current); + assert(current >= buffer); + return current; +} + +#if defined(JSON_HAS_INT64) + +String valueToString(Int value) { return valueToString(LargestInt(value)); } + +String valueToString(UInt value) { return valueToString(LargestUInt(value)); } + +#endif // # if defined(JSON_HAS_INT64) + +namespace { +String valueToString(double value, bool useSpecialFloats, + unsigned int precision, PrecisionType precisionType) { + // Print into the buffer. We need not request the alternative representation + // that always has a decimal point because JSON doesn't distinguish the + // concepts of reals and integers. + if (!isfinite(value)) { + static const char* const reps[2][3] = {{"NaN", "-Infinity", "Infinity"}, + {"null", "-1e+9999", "1e+9999"}}; + return reps[useSpecialFloats ? 0 : 1] + [isnan(value) ? 0 : (value < 0) ? 1 : 2]; + } + + String buffer(size_t(36), '\0'); + while (true) { + int len = jsoncpp_snprintf( + &*buffer.begin(), buffer.size(), + (precisionType == PrecisionType::significantDigits) ? "%.*g" : "%.*f", + precision, value); + assert(len >= 0); + auto wouldPrint = static_cast(len); + if (wouldPrint >= buffer.size()) { + buffer.resize(wouldPrint + 1); + continue; + } + buffer.resize(wouldPrint); + break; + } + + buffer.erase(fixNumericLocale(buffer.begin(), buffer.end()), buffer.end()); + + // try to ensure we preserve the fact that this was given to us as a double on + // input + if (buffer.find('.') == buffer.npos && buffer.find('e') == buffer.npos) { + buffer += ".0"; + } + + // strip the zero padding from the right + if (precisionType == PrecisionType::decimalPlaces) { + buffer.erase(fixZerosInTheEnd(buffer.begin(), buffer.end(), precision), + buffer.end()); + } + + return buffer; +} +} // namespace + +String valueToString(double value, unsigned int precision, + PrecisionType precisionType) { + return valueToString(value, false, precision, precisionType); +} + +String valueToString(bool value) { return value ? "true" : "false"; } + +static bool doesAnyCharRequireEscaping(char const* s, size_t n) { + assert(s || !n); + + return std::any_of(s, s + n, [](unsigned char c) { + return c == '\\' || c == '"' || c < 0x20 || c > 0x7F; + }); +} + +static unsigned int utf8ToCodepoint(const char*& s, const char* e) { + const unsigned int REPLACEMENT_CHARACTER = 0xFFFD; + + unsigned int firstByte = static_cast(*s); + + if (firstByte < 0x80) + return firstByte; + + if (firstByte < 0xE0) { + if (e - s < 2) + return REPLACEMENT_CHARACTER; + + unsigned int calculated = + ((firstByte & 0x1F) << 6) | (static_cast(s[1]) & 0x3F); + s += 1; + // oversized encoded characters are invalid + return calculated < 0x80 ? REPLACEMENT_CHARACTER : calculated; + } + + if (firstByte < 0xF0) { + if (e - s < 3) + return REPLACEMENT_CHARACTER; + + unsigned int calculated = ((firstByte & 0x0F) << 12) | + ((static_cast(s[1]) & 0x3F) << 6) | + (static_cast(s[2]) & 0x3F); + s += 2; + // surrogates aren't valid codepoints itself + // shouldn't be UTF-8 encoded + if (calculated >= 0xD800 && calculated <= 0xDFFF) + return REPLACEMENT_CHARACTER; + // oversized encoded characters are invalid + return calculated < 0x800 ? REPLACEMENT_CHARACTER : calculated; + } + + if (firstByte < 0xF8) { + if (e - s < 4) + return REPLACEMENT_CHARACTER; + + unsigned int calculated = ((firstByte & 0x07) << 18) | + ((static_cast(s[1]) & 0x3F) << 12) | + ((static_cast(s[2]) & 0x3F) << 6) | + (static_cast(s[3]) & 0x3F); + s += 3; + // oversized encoded characters are invalid + return calculated < 0x10000 ? REPLACEMENT_CHARACTER : calculated; + } + + return REPLACEMENT_CHARACTER; +} + +static const char hex2[] = "000102030405060708090a0b0c0d0e0f" + "101112131415161718191a1b1c1d1e1f" + "202122232425262728292a2b2c2d2e2f" + "303132333435363738393a3b3c3d3e3f" + "404142434445464748494a4b4c4d4e4f" + "505152535455565758595a5b5c5d5e5f" + "606162636465666768696a6b6c6d6e6f" + "707172737475767778797a7b7c7d7e7f" + "808182838485868788898a8b8c8d8e8f" + "909192939495969798999a9b9c9d9e9f" + "a0a1a2a3a4a5a6a7a8a9aaabacadaeaf" + "b0b1b2b3b4b5b6b7b8b9babbbcbdbebf" + "c0c1c2c3c4c5c6c7c8c9cacbcccdcecf" + "d0d1d2d3d4d5d6d7d8d9dadbdcdddedf" + "e0e1e2e3e4e5e6e7e8e9eaebecedeeef" + "f0f1f2f3f4f5f6f7f8f9fafbfcfdfeff"; + +static String toHex16Bit(unsigned int x) { + const unsigned int hi = (x >> 8) & 0xff; + const unsigned int lo = x & 0xff; + String result(4, ' '); + result[0] = hex2[2 * hi]; + result[1] = hex2[2 * hi + 1]; + result[2] = hex2[2 * lo]; + result[3] = hex2[2 * lo + 1]; + return result; +} + +static void appendRaw(String& result, unsigned ch) { + result += static_cast(ch); +} + +static void appendHex(String& result, unsigned ch) { + result.append("\\u").append(toHex16Bit(ch)); +} + +static String valueToQuotedStringN(const char* value, size_t length, + bool emitUTF8 = false) { + if (value == nullptr) + return ""; + + if (!doesAnyCharRequireEscaping(value, length)) + return String("\"") + value + "\""; + // We have to walk value and escape any special characters. + // Appending to String is not efficient, but this should be rare. + // (Note: forward slashes are *not* rare, but I am not escaping them.) + String::size_type maxsize = length * 2 + 3; // allescaped+quotes+NULL + String result; + result.reserve(maxsize); // to avoid lots of mallocs + result += "\""; + char const* end = value + length; + for (const char* c = value; c != end; ++c) { + switch (*c) { + case '\"': + result += "\\\""; + break; + case '\\': + result += "\\\\"; + break; + case '\b': + result += "\\b"; + break; + case '\f': + result += "\\f"; + break; + case '\n': + result += "\\n"; + break; + case '\r': + result += "\\r"; + break; + case '\t': + result += "\\t"; + break; + // case '/': + // Even though \/ is considered a legal escape in JSON, a bare + // slash is also legal, so I see no reason to escape it. + // (I hope I am not misunderstanding something.) + // blep notes: actually escaping \/ may be useful in javascript to avoid (*c); + if (codepoint < 0x20) { + appendHex(result, codepoint); + } else { + appendRaw(result, codepoint); + } + } else { + unsigned codepoint = utf8ToCodepoint(c, end); // modifies `c` + if (codepoint < 0x20) { + appendHex(result, codepoint); + } else if (codepoint < 0x80) { + appendRaw(result, codepoint); + } else if (codepoint < 0x10000) { + // Basic Multilingual Plane + appendHex(result, codepoint); + } else { + // Extended Unicode. Encode 20 bits as a surrogate pair. + codepoint -= 0x10000; + appendHex(result, 0xd800 + ((codepoint >> 10) & 0x3ff)); + appendHex(result, 0xdc00 + (codepoint & 0x3ff)); + } + } + } break; + } + } + result += "\""; + return result; +} + +String valueToQuotedString(const char* value) { + return valueToQuotedStringN(value, strlen(value)); +} + +// Class Writer +// ////////////////////////////////////////////////////////////////// +Writer::~Writer() = default; + +// Class FastWriter +// ////////////////////////////////////////////////////////////////// + +FastWriter::FastWriter() + + = default; + +void FastWriter::enableYAMLCompatibility() { yamlCompatibilityEnabled_ = true; } + +void FastWriter::dropNullPlaceholders() { dropNullPlaceholders_ = true; } + +void FastWriter::omitEndingLineFeed() { omitEndingLineFeed_ = true; } + +String FastWriter::write(const Value& root) { + document_.clear(); + writeValue(root); + if (!omitEndingLineFeed_) + document_ += '\n'; + return document_; +} + +void FastWriter::writeValue(const Value& value) { + switch (value.type()) { + case nullValue: + if (!dropNullPlaceholders_) + document_ += "null"; + break; + case intValue: + document_ += valueToString(value.asLargestInt()); + break; + case uintValue: + document_ += valueToString(value.asLargestUInt()); + break; + case realValue: + document_ += valueToString(value.asDouble()); + break; + case stringValue: { + // Is NULL possible for value.string_? No. + char const* str; + char const* end; + bool ok = value.getString(&str, &end); + if (ok) + document_ += valueToQuotedStringN(str, static_cast(end - str)); + break; + } + case booleanValue: + document_ += valueToString(value.asBool()); + break; + case arrayValue: { + document_ += '['; + ArrayIndex size = value.size(); + for (ArrayIndex index = 0; index < size; ++index) { + if (index > 0) + document_ += ','; + writeValue(value[index]); + } + document_ += ']'; + } break; + case objectValue: { + Value::Members members(value.getMemberNames()); + document_ += '{'; + for (auto it = members.begin(); it != members.end(); ++it) { + const String& name = *it; + if (it != members.begin()) + document_ += ','; + document_ += valueToQuotedStringN(name.data(), name.length()); + document_ += yamlCompatibilityEnabled_ ? ": " : ":"; + writeValue(value[name]); + } + document_ += '}'; + } break; + } +} + +// Class StyledWriter +// ////////////////////////////////////////////////////////////////// + +StyledWriter::StyledWriter() = default; + +String StyledWriter::write(const Value& root) { + document_.clear(); + addChildValues_ = false; + indentString_.clear(); + writeCommentBeforeValue(root); + writeValue(root); + writeCommentAfterValueOnSameLine(root); + document_ += '\n'; + return document_; +} + +void StyledWriter::writeValue(const Value& value) { + switch (value.type()) { + case nullValue: + pushValue("null"); + break; + case intValue: + pushValue(valueToString(value.asLargestInt())); + break; + case uintValue: + pushValue(valueToString(value.asLargestUInt())); + break; + case realValue: + pushValue(valueToString(value.asDouble())); + break; + case stringValue: { + // Is NULL possible for value.string_? No. + char const* str; + char const* end; + bool ok = value.getString(&str, &end); + if (ok) + pushValue(valueToQuotedStringN(str, static_cast(end - str))); + else + pushValue(""); + break; + } + case booleanValue: + pushValue(valueToString(value.asBool())); + break; + case arrayValue: + writeArrayValue(value); + break; + case objectValue: { + Value::Members members(value.getMemberNames()); + if (members.empty()) + pushValue("{}"); + else { + writeWithIndent("{"); + indent(); + auto it = members.begin(); + for (;;) { + const String& name = *it; + const Value& childValue = value[name]; + writeCommentBeforeValue(childValue); + writeWithIndent(valueToQuotedString(name.c_str())); + document_ += " : "; + writeValue(childValue); + if (++it == members.end()) { + writeCommentAfterValueOnSameLine(childValue); + break; + } + document_ += ','; + writeCommentAfterValueOnSameLine(childValue); + } + unindent(); + writeWithIndent("}"); + } + } break; + } +} + +void StyledWriter::writeArrayValue(const Value& value) { + size_t size = value.size(); + if (size == 0) + pushValue("[]"); + else { + bool isArrayMultiLine = isMultilineArray(value); + if (isArrayMultiLine) { + writeWithIndent("["); + indent(); + bool hasChildValue = !childValues_.empty(); + ArrayIndex index = 0; + for (;;) { + const Value& childValue = value[index]; + writeCommentBeforeValue(childValue); + if (hasChildValue) + writeWithIndent(childValues_[index]); + else { + writeIndent(); + writeValue(childValue); + } + if (++index == size) { + writeCommentAfterValueOnSameLine(childValue); + break; + } + document_ += ','; + writeCommentAfterValueOnSameLine(childValue); + } + unindent(); + writeWithIndent("]"); + } else // output on a single line + { + assert(childValues_.size() == size); + document_ += "[ "; + for (size_t index = 0; index < size; ++index) { + if (index > 0) + document_ += ", "; + document_ += childValues_[index]; + } + document_ += " ]"; + } + } +} + +bool StyledWriter::isMultilineArray(const Value& value) { + ArrayIndex const size = value.size(); + bool isMultiLine = size * 3 >= rightMargin_; + childValues_.clear(); + for (ArrayIndex index = 0; index < size && !isMultiLine; ++index) { + const Value& childValue = value[index]; + isMultiLine = ((childValue.isArray() || childValue.isObject()) && + !childValue.empty()); + } + if (!isMultiLine) // check if line length > max line length + { + childValues_.reserve(size); + addChildValues_ = true; + ArrayIndex lineLength = 4 + (size - 1) * 2; // '[ ' + ', '*n + ' ]' + for (ArrayIndex index = 0; index < size; ++index) { + if (hasCommentForValue(value[index])) { + isMultiLine = true; + } + writeValue(value[index]); + lineLength += static_cast(childValues_[index].length()); + } + addChildValues_ = false; + isMultiLine = isMultiLine || lineLength >= rightMargin_; + } + return isMultiLine; +} + +void StyledWriter::pushValue(const String& value) { + if (addChildValues_) + childValues_.push_back(value); + else + document_ += value; +} + +void StyledWriter::writeIndent() { + if (!document_.empty()) { + char last = document_[document_.length() - 1]; + if (last == ' ') // already indented + return; + if (last != '\n') // Comments may add new-line + document_ += '\n'; + } + document_ += indentString_; +} + +void StyledWriter::writeWithIndent(const String& value) { + writeIndent(); + document_ += value; +} + +void StyledWriter::indent() { indentString_ += String(indentSize_, ' '); } + +void StyledWriter::unindent() { + assert(indentString_.size() >= indentSize_); + indentString_.resize(indentString_.size() - indentSize_); +} + +void StyledWriter::writeCommentBeforeValue(const Value& root) { + if (!root.hasComment(commentBefore)) + return; + + document_ += '\n'; + writeIndent(); + const String& comment = root.getComment(commentBefore); + String::const_iterator iter = comment.begin(); + while (iter != comment.end()) { + document_ += *iter; + if (*iter == '\n' && ((iter + 1) != comment.end() && *(iter + 1) == '/')) + writeIndent(); + ++iter; + } + + // Comments are stripped of trailing newlines, so add one here + document_ += '\n'; +} + +void StyledWriter::writeCommentAfterValueOnSameLine(const Value& root) { + if (root.hasComment(commentAfterOnSameLine)) + document_ += " " + root.getComment(commentAfterOnSameLine); + + if (root.hasComment(commentAfter)) { + document_ += '\n'; + document_ += root.getComment(commentAfter); + document_ += '\n'; + } +} + +bool StyledWriter::hasCommentForValue(const Value& value) { + return value.hasComment(commentBefore) || + value.hasComment(commentAfterOnSameLine) || + value.hasComment(commentAfter); +} + +// Class StyledStreamWriter +// ////////////////////////////////////////////////////////////////// + +StyledStreamWriter::StyledStreamWriter(String indentation) + : document_(nullptr), indentation_(std::move(indentation)), + addChildValues_(), indented_(false) {} + +void StyledStreamWriter::write(OStream& out, const Value& root) { + document_ = &out; + addChildValues_ = false; + indentString_.clear(); + indented_ = true; + writeCommentBeforeValue(root); + if (!indented_) + writeIndent(); + indented_ = true; + writeValue(root); + writeCommentAfterValueOnSameLine(root); + *document_ << "\n"; + document_ = nullptr; // Forget the stream, for safety. +} + +void StyledStreamWriter::writeValue(const Value& value) { + switch (value.type()) { + case nullValue: + pushValue("null"); + break; + case intValue: + pushValue(valueToString(value.asLargestInt())); + break; + case uintValue: + pushValue(valueToString(value.asLargestUInt())); + break; + case realValue: + pushValue(valueToString(value.asDouble())); + break; + case stringValue: { + // Is NULL possible for value.string_? No. + char const* str; + char const* end; + bool ok = value.getString(&str, &end); + if (ok) + pushValue(valueToQuotedStringN(str, static_cast(end - str))); + else + pushValue(""); + break; + } + case booleanValue: + pushValue(valueToString(value.asBool())); + break; + case arrayValue: + writeArrayValue(value); + break; + case objectValue: { + Value::Members members(value.getMemberNames()); + if (members.empty()) + pushValue("{}"); + else { + writeWithIndent("{"); + indent(); + auto it = members.begin(); + for (;;) { + const String& name = *it; + const Value& childValue = value[name]; + writeCommentBeforeValue(childValue); + writeWithIndent(valueToQuotedString(name.c_str())); + *document_ << " : "; + writeValue(childValue); + if (++it == members.end()) { + writeCommentAfterValueOnSameLine(childValue); + break; + } + *document_ << ","; + writeCommentAfterValueOnSameLine(childValue); + } + unindent(); + writeWithIndent("}"); + } + } break; + } +} + +void StyledStreamWriter::writeArrayValue(const Value& value) { + unsigned size = value.size(); + if (size == 0) + pushValue("[]"); + else { + bool isArrayMultiLine = isMultilineArray(value); + if (isArrayMultiLine) { + writeWithIndent("["); + indent(); + bool hasChildValue = !childValues_.empty(); + unsigned index = 0; + for (;;) { + const Value& childValue = value[index]; + writeCommentBeforeValue(childValue); + if (hasChildValue) + writeWithIndent(childValues_[index]); + else { + if (!indented_) + writeIndent(); + indented_ = true; + writeValue(childValue); + indented_ = false; + } + if (++index == size) { + writeCommentAfterValueOnSameLine(childValue); + break; + } + *document_ << ","; + writeCommentAfterValueOnSameLine(childValue); + } + unindent(); + writeWithIndent("]"); + } else // output on a single line + { + assert(childValues_.size() == size); + *document_ << "[ "; + for (unsigned index = 0; index < size; ++index) { + if (index > 0) + *document_ << ", "; + *document_ << childValues_[index]; + } + *document_ << " ]"; + } + } +} + +bool StyledStreamWriter::isMultilineArray(const Value& value) { + ArrayIndex const size = value.size(); + bool isMultiLine = size * 3 >= rightMargin_; + childValues_.clear(); + for (ArrayIndex index = 0; index < size && !isMultiLine; ++index) { + const Value& childValue = value[index]; + isMultiLine = ((childValue.isArray() || childValue.isObject()) && + !childValue.empty()); + } + if (!isMultiLine) // check if line length > max line length + { + childValues_.reserve(size); + addChildValues_ = true; + ArrayIndex lineLength = 4 + (size - 1) * 2; // '[ ' + ', '*n + ' ]' + for (ArrayIndex index = 0; index < size; ++index) { + if (hasCommentForValue(value[index])) { + isMultiLine = true; + } + writeValue(value[index]); + lineLength += static_cast(childValues_[index].length()); + } + addChildValues_ = false; + isMultiLine = isMultiLine || lineLength >= rightMargin_; + } + return isMultiLine; +} + +void StyledStreamWriter::pushValue(const String& value) { + if (addChildValues_) + childValues_.push_back(value); + else + *document_ << value; +} + +void StyledStreamWriter::writeIndent() { + // blep intended this to look at the so-far-written string + // to determine whether we are already indented, but + // with a stream we cannot do that. So we rely on some saved state. + // The caller checks indented_. + *document_ << '\n' << indentString_; +} + +void StyledStreamWriter::writeWithIndent(const String& value) { + if (!indented_) + writeIndent(); + *document_ << value; + indented_ = false; +} + +void StyledStreamWriter::indent() { indentString_ += indentation_; } + +void StyledStreamWriter::unindent() { + assert(indentString_.size() >= indentation_.size()); + indentString_.resize(indentString_.size() - indentation_.size()); +} + +void StyledStreamWriter::writeCommentBeforeValue(const Value& root) { + if (!root.hasComment(commentBefore)) + return; + + if (!indented_) + writeIndent(); + const String& comment = root.getComment(commentBefore); + String::const_iterator iter = comment.begin(); + while (iter != comment.end()) { + *document_ << *iter; + if (*iter == '\n' && ((iter + 1) != comment.end() && *(iter + 1) == '/')) + // writeIndent(); // would include newline + *document_ << indentString_; + ++iter; + } + indented_ = false; +} + +void StyledStreamWriter::writeCommentAfterValueOnSameLine(const Value& root) { + if (root.hasComment(commentAfterOnSameLine)) + *document_ << ' ' << root.getComment(commentAfterOnSameLine); + + if (root.hasComment(commentAfter)) { + writeIndent(); + *document_ << root.getComment(commentAfter); + } + indented_ = false; +} + +bool StyledStreamWriter::hasCommentForValue(const Value& value) { + return value.hasComment(commentBefore) || + value.hasComment(commentAfterOnSameLine) || + value.hasComment(commentAfter); +} + +////////////////////////// +// BuiltStyledStreamWriter + +/// Scoped enums are not available until C++11. +struct CommentStyle { + /// Decide whether to write comments. + enum Enum { + None, ///< Drop all comments. + Most, ///< Recover odd behavior of previous versions (not implemented yet). + All ///< Keep all comments. + }; +}; + +struct BuiltStyledStreamWriter : public StreamWriter { + BuiltStyledStreamWriter(String indentation, CommentStyle::Enum cs, + String colonSymbol, String nullSymbol, + String endingLineFeedSymbol, bool useSpecialFloats, + bool emitUTF8, unsigned int precision, + PrecisionType precisionType); + int write(Value const& root, OStream* sout) override; + +private: + void writeValue(Value const& value); + void writeArrayValue(Value const& value); + bool isMultilineArray(Value const& value); + void pushValue(String const& value); + void writeIndent(); + void writeWithIndent(String const& value); + void indent(); + void unindent(); + void writeCommentBeforeValue(Value const& root); + void writeCommentAfterValueOnSameLine(Value const& root); + static bool hasCommentForValue(const Value& value); + + using ChildValues = std::vector; + + ChildValues childValues_; + String indentString_; + unsigned int rightMargin_; + String indentation_; + CommentStyle::Enum cs_; + String colonSymbol_; + String nullSymbol_; + String endingLineFeedSymbol_; + bool addChildValues_ : 1; + bool indented_ : 1; + bool useSpecialFloats_ : 1; + bool emitUTF8_ : 1; + unsigned int precision_; + PrecisionType precisionType_; +}; +BuiltStyledStreamWriter::BuiltStyledStreamWriter( + String indentation, CommentStyle::Enum cs, String colonSymbol, + String nullSymbol, String endingLineFeedSymbol, bool useSpecialFloats, + bool emitUTF8, unsigned int precision, PrecisionType precisionType) + : rightMargin_(74), indentation_(std::move(indentation)), cs_(cs), + colonSymbol_(std::move(colonSymbol)), nullSymbol_(std::move(nullSymbol)), + endingLineFeedSymbol_(std::move(endingLineFeedSymbol)), + addChildValues_(false), indented_(false), + useSpecialFloats_(useSpecialFloats), emitUTF8_(emitUTF8), + precision_(precision), precisionType_(precisionType) {} +int BuiltStyledStreamWriter::write(Value const& root, OStream* sout) { + sout_ = sout; + addChildValues_ = false; + indented_ = true; + indentString_.clear(); + writeCommentBeforeValue(root); + if (!indented_) + writeIndent(); + indented_ = true; + writeValue(root); + writeCommentAfterValueOnSameLine(root); + *sout_ << endingLineFeedSymbol_; + sout_ = nullptr; + return 0; +} +void BuiltStyledStreamWriter::writeValue(Value const& value) { + switch (value.type()) { + case nullValue: + pushValue(nullSymbol_); + break; + case intValue: + pushValue(valueToString(value.asLargestInt())); + break; + case uintValue: + pushValue(valueToString(value.asLargestUInt())); + break; + case realValue: + pushValue(valueToString(value.asDouble(), useSpecialFloats_, precision_, + precisionType_)); + break; + case stringValue: { + // Is NULL is possible for value.string_? No. + char const* str; + char const* end; + bool ok = value.getString(&str, &end); + if (ok) + pushValue( + valueToQuotedStringN(str, static_cast(end - str), emitUTF8_)); + else + pushValue(""); + break; + } + case booleanValue: + pushValue(valueToString(value.asBool())); + break; + case arrayValue: + writeArrayValue(value); + break; + case objectValue: { + Value::Members members(value.getMemberNames()); + if (members.empty()) + pushValue("{}"); + else { + writeWithIndent("{"); + indent(); + auto it = members.begin(); + for (;;) { + String const& name = *it; + Value const& childValue = value[name]; + writeCommentBeforeValue(childValue); + writeWithIndent( + valueToQuotedStringN(name.data(), name.length(), emitUTF8_)); + *sout_ << colonSymbol_; + writeValue(childValue); + if (++it == members.end()) { + writeCommentAfterValueOnSameLine(childValue); + break; + } + *sout_ << ","; + writeCommentAfterValueOnSameLine(childValue); + } + unindent(); + writeWithIndent("}"); + } + } break; + } +} + +void BuiltStyledStreamWriter::writeArrayValue(Value const& value) { + unsigned size = value.size(); + if (size == 0) + pushValue("[]"); + else { + bool isMultiLine = (cs_ == CommentStyle::All) || isMultilineArray(value); + if (isMultiLine) { + writeWithIndent("["); + indent(); + bool hasChildValue = !childValues_.empty(); + unsigned index = 0; + for (;;) { + Value const& childValue = value[index]; + writeCommentBeforeValue(childValue); + if (hasChildValue) + writeWithIndent(childValues_[index]); + else { + if (!indented_) + writeIndent(); + indented_ = true; + writeValue(childValue); + indented_ = false; + } + if (++index == size) { + writeCommentAfterValueOnSameLine(childValue); + break; + } + *sout_ << ","; + writeCommentAfterValueOnSameLine(childValue); + } + unindent(); + writeWithIndent("]"); + } else // output on a single line + { + assert(childValues_.size() == size); + *sout_ << "["; + if (!indentation_.empty()) + *sout_ << " "; + for (unsigned index = 0; index < size; ++index) { + if (index > 0) + *sout_ << ((!indentation_.empty()) ? ", " : ","); + *sout_ << childValues_[index]; + } + if (!indentation_.empty()) + *sout_ << " "; + *sout_ << "]"; + } + } +} + +bool BuiltStyledStreamWriter::isMultilineArray(Value const& value) { + ArrayIndex const size = value.size(); + bool isMultiLine = size * 3 >= rightMargin_; + childValues_.clear(); + for (ArrayIndex index = 0; index < size && !isMultiLine; ++index) { + Value const& childValue = value[index]; + isMultiLine = ((childValue.isArray() || childValue.isObject()) && + !childValue.empty()); + } + if (!isMultiLine) // check if line length > max line length + { + childValues_.reserve(size); + addChildValues_ = true; + ArrayIndex lineLength = 4 + (size - 1) * 2; // '[ ' + ', '*n + ' ]' + for (ArrayIndex index = 0; index < size; ++index) { + if (hasCommentForValue(value[index])) { + isMultiLine = true; + } + writeValue(value[index]); + lineLength += static_cast(childValues_[index].length()); + } + addChildValues_ = false; + isMultiLine = isMultiLine || lineLength >= rightMargin_; + } + return isMultiLine; +} + +void BuiltStyledStreamWriter::pushValue(String const& value) { + if (addChildValues_) + childValues_.push_back(value); + else + *sout_ << value; +} + +void BuiltStyledStreamWriter::writeIndent() { + // blep intended this to look at the so-far-written string + // to determine whether we are already indented, but + // with a stream we cannot do that. So we rely on some saved state. + // The caller checks indented_. + + if (!indentation_.empty()) { + // In this case, drop newlines too. + *sout_ << '\n' << indentString_; + } +} + +void BuiltStyledStreamWriter::writeWithIndent(String const& value) { + if (!indented_) + writeIndent(); + *sout_ << value; + indented_ = false; +} + +void BuiltStyledStreamWriter::indent() { indentString_ += indentation_; } + +void BuiltStyledStreamWriter::unindent() { + assert(indentString_.size() >= indentation_.size()); + indentString_.resize(indentString_.size() - indentation_.size()); +} + +void BuiltStyledStreamWriter::writeCommentBeforeValue(Value const& root) { + if (cs_ == CommentStyle::None) + return; + if (!root.hasComment(commentBefore)) + return; + + if (!indented_) + writeIndent(); + const String& comment = root.getComment(commentBefore); + String::const_iterator iter = comment.begin(); + while (iter != comment.end()) { + *sout_ << *iter; + if (*iter == '\n' && ((iter + 1) != comment.end() && *(iter + 1) == '/')) + // writeIndent(); // would write extra newline + *sout_ << indentString_; + ++iter; + } + indented_ = false; +} + +void BuiltStyledStreamWriter::writeCommentAfterValueOnSameLine( + Value const& root) { + if (cs_ == CommentStyle::None) + return; + if (root.hasComment(commentAfterOnSameLine)) + *sout_ << " " + root.getComment(commentAfterOnSameLine); + + if (root.hasComment(commentAfter)) { + writeIndent(); + *sout_ << root.getComment(commentAfter); + } +} + +// static +bool BuiltStyledStreamWriter::hasCommentForValue(const Value& value) { + return value.hasComment(commentBefore) || + value.hasComment(commentAfterOnSameLine) || + value.hasComment(commentAfter); +} + +/////////////// +// StreamWriter + +StreamWriter::StreamWriter() : sout_(nullptr) {} +StreamWriter::~StreamWriter() = default; +StreamWriter::Factory::~Factory() = default; +StreamWriterBuilder::StreamWriterBuilder() { setDefaults(&settings_); } +StreamWriterBuilder::~StreamWriterBuilder() = default; +StreamWriter* StreamWriterBuilder::newStreamWriter() const { + const String indentation = settings_["indentation"].asString(); + const String cs_str = settings_["commentStyle"].asString(); + const String pt_str = settings_["precisionType"].asString(); + const bool eyc = settings_["enableYAMLCompatibility"].asBool(); + const bool dnp = settings_["dropNullPlaceholders"].asBool(); + const bool usf = settings_["useSpecialFloats"].asBool(); + const bool emitUTF8 = settings_["emitUTF8"].asBool(); + unsigned int pre = settings_["precision"].asUInt(); + CommentStyle::Enum cs = CommentStyle::All; + if (cs_str == "All") { + cs = CommentStyle::All; + } else if (cs_str == "None") { + cs = CommentStyle::None; + } else { + throwRuntimeError("commentStyle must be 'All' or 'None'"); + } + PrecisionType precisionType(significantDigits); + if (pt_str == "significant") { + precisionType = PrecisionType::significantDigits; + } else if (pt_str == "decimal") { + precisionType = PrecisionType::decimalPlaces; + } else { + throwRuntimeError("precisionType must be 'significant' or 'decimal'"); + } + String colonSymbol = " : "; + if (eyc) { + colonSymbol = ": "; + } else if (indentation.empty()) { + colonSymbol = ":"; + } + String nullSymbol = "null"; + if (dnp) { + nullSymbol.clear(); + } + if (pre > 17) + pre = 17; + String endingLineFeedSymbol; + return new BuiltStyledStreamWriter(indentation, cs, colonSymbol, nullSymbol, + endingLineFeedSymbol, usf, emitUTF8, pre, + precisionType); +} + +bool StreamWriterBuilder::validate(Json::Value* invalid) const { + static const auto& valid_keys = *new std::set{ + "indentation", + "commentStyle", + "enableYAMLCompatibility", + "dropNullPlaceholders", + "useSpecialFloats", + "emitUTF8", + "precision", + "precisionType", + }; + for (auto si = settings_.begin(); si != settings_.end(); ++si) { + auto key = si.name(); + if (valid_keys.count(key)) + continue; + if (invalid) + (*invalid)[key] = *si; + else + return false; + } + return invalid ? invalid->empty() : true; +} + +Value& StreamWriterBuilder::operator[](const String& key) { + return settings_[key]; +} +// static +void StreamWriterBuilder::setDefaults(Json::Value* settings) { + //! [StreamWriterBuilderDefaults] + (*settings)["commentStyle"] = "All"; + (*settings)["indentation"] = "\t"; + (*settings)["enableYAMLCompatibility"] = false; + (*settings)["dropNullPlaceholders"] = false; + (*settings)["useSpecialFloats"] = false; + (*settings)["emitUTF8"] = false; + (*settings)["precision"] = 17; + (*settings)["precisionType"] = "significant"; + //! [StreamWriterBuilderDefaults] +} + +String writeString(StreamWriter::Factory const& factory, Value const& root) { + OStringStream sout; + StreamWriterPtr const writer(factory.newStreamWriter()); + writer->write(root, &sout); + return sout.str(); +} + +OStream& operator<<(OStream& sout, Value const& root) { + StreamWriterBuilder builder; + StreamWriterPtr const writer(builder.newStreamWriter()); + writer->write(root, &sout); + return sout; +} + +} // namespace Json + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: src/lib_json/json_writer.cpp +// ////////////////////////////////////////////////////////////////////// + + + + + diff --git a/src/pl/src/pl.cpp b/src/pl/src/pl.cpp new file mode 100644 index 0000000..c66f369 --- /dev/null +++ b/src/pl/src/pl.cpp @@ -0,0 +1,329 @@ +#include "pl.hpp" + +#include +#include +#include +#include + +using namespace std; + +#define WRC_MAX_POINTS_IN_PATH 2000 + +#define Lat_Origin 32.045062 // 地面平面坐标系原点的纬度 +#define Lon_Origin 118.845200366 // 地面平面坐标系原点的经度 +#define Re 6378137 +#define Rn 6356755 +#define deg_rad (0.01745329252) // Transfer from angle degree to rad +#define R_LATI (6378137) +#define R_LONT (5407872) // 这个需要根据所在区域的纬度进行换算：R_LONT = R_LATI*cos(所在地的纬度转化为弧度) + +float nl_radius; +int nl_within_radius; +double x = 0.0; +double y = 1.0; +int pl_speed = 0; + +int GpsPointNum = 0; +WRC_GPS_Point GPSPointQueue[WRC_MAX_POINTS_IN_PATH]; + +WRC_MC_NAV_INFO g_NavInfo; +struct_rtk g_rtk; + +int mode = 0; +int last_mode = 0; + +Direction drive_mode = Direction::STRAIGHT_D; +TaskStatus task_status = TaskStatus::PENDING; + +// 返回单位：m +double ntzx_GPS_length(double lonti1, double lati1, double lonti2, double lati2) +{ + double x, y, length; + x = (R_LONT) * (lonti2 - lonti1) * deg_rad; // 弧长公式 + y = (R_LATI) * (lati2 - lati1) * deg_rad; + length = sqrt(x * x + y * y); + return length; +} + +// Cur_navAngle指定y轴正方向.Cur_lonti，Cur_lati为原点；x_diff,y_diff是Dest_lonti,Dest_lati在该坐标系的直角坐标:m +int ntzx_GPS_posit(double Cur_navAngle, double Cur_lonti, double Cur_lati, double Dest_lonti, double Dest_lati, double *x_diff, double *y_diff) +{ + double Navi_rad, x, y; + float k1, k2, k3, k4; // 旋转矩阵的四个参数，对应cos(theta),-sin(theta),sin(theta),cos(theta); + Navi_rad = Cur_navAngle * deg_rad; // 角度转化为弧度 + // 以当前位置为原点，以正北为y正轴，以正东为x正轴； + x = (Dest_lonti - Cur_lonti) * deg_rad * R_LONT; + y = (Dest_lati - Cur_lati) * deg_rad * R_LATI; + k1 = cos(Navi_rad); + k2 = (-1) * sin(Navi_rad); + k3 = (-1) * k2; + k4 = k1; + // 以当前航向角作为旋转角 + *x_diff = x * k1 + y * k2; + *y_diff = x * k3 + y * k4; + + return 0; +} + +// Find_Nearest_Point +int Road_Planning_Find_Nearest_Point(int start_index, WRC_MC_NAV_INFO Cur_GPS_info, WRC_GPS_Point *Road_info) +{ + if (start_index < 0) + { + return -1; + } + + int Nearest_point_index = start_index; // 初始化为起始索引 + double min_distance = ntzx_GPS_length(Cur_GPS_info.Longitude_degree, Cur_GPS_info.Latitude_degree, + Road_info[start_index].LongitudeInfo, + Road_info[start_index].LatitudeInfo); + + // 从 start_index + 1 开始搜索 + for (int i = 1; i < GpsPointNum; i++) + { + int current_index = (start_index + i) % GpsPointNum; // 环绕访问 + + double current_distance = ntzx_GPS_length(Cur_GPS_info.Longitude_degree, Cur_GPS_info.Latitude_degree, + Road_info[current_index].LongitudeInfo, + Road_info[current_index].LatitudeInfo); + + if (current_distance < min_distance) + { + min_distance = current_distance; + Nearest_point_index = current_index; // 更新最近点的索引 + + // 如果距离小于 0.8 m，提前退出 + if (min_distance < 0.8) + { + return Nearest_point_index; + } + } + } + + return Nearest_point_index; // 返回最近点的索引 +} +/* +int Road_Planning_Find_Nearest_Point(int start_index, WRC_MC_NAV_INFO Cur_GPS_info, WRC_GPS_Point *Road_info) +{ + + if (start_index < 0) + { + return -1; + } + int Nearest_point_index = 0; + double aft_distance = 0, pre_distance = 0; + pre_distance = ntzx_GPS_length(Cur_GPS_info.Longitude_degree, Cur_GPS_info.Latitude_degree, Road_info[start_index].LongitudeInfo, Road_info[start_index].LatitudeInfo); + + int start = start_index + 1; + for (int i = start; i < GpsPointNum; i++) + { + + aft_distance = ntzx_GPS_length(Cur_GPS_info.Longitude_degree, Cur_GPS_info.Latitude_degree, Road_info[i].LongitudeInfo, Road_info[i].LatitudeInfo); + // printf("i: %d , aft_distance: %lf \t",i,aft_distance); + if (aft_distance < pre_distance) + { + pre_distance = aft_distance; + continue; + } + else + { + Nearest_point_index = i - 1; + + return Nearest_point_index; + } + } + return GpsPointNum - 1; +} +*/ + +// Find_Aim_Point +int Road_Planning_Find_Aim_Point(int start_index, int dest_num) +{ + int dest_index = start_index; + if (start_index + dest_num <= (GpsPointNum - 1)) + { + dest_index = start_index + dest_num; + } + else + { + dest_index = (GpsPointNum - 1); + } + + return (dest_index); +} + +int star_find_nearest_point(double lon, double lat, WRC_GPS_Point *Road_info) +{ + double dis = 10000; + int point = 0; + for (int i = 0; i < GpsPointNum; i++) + { + double dis_temp = ntzx_GPS_length(lon, lat, Road_info[i].LongitudeInfo, Road_info[i].LatitudeInfo); + if (dis_temp < dis) + { + dis = dis_temp; + point = i; + } + printf("i: %d , dis_temp: %lf", i, dis_temp); + } + return point; +} + +// 判断车辆是否在左转、右转还是直行 +Direction straight_or_turn(double cur_direction, double des_direction, int threshold) +{ + double diff = std::fmod(std::fabs(cur_direction - des_direction), 360.0); + + if (diff > 180) + diff = 360 - diff; + + // std::cout << "diff : " << diff << std::endl; + + if (diff < threshold) + { + return Direction::STRAIGHT_D; + } + else + { + // 计算角度差的符号来确定转向方向 + double angle_diff = std::fmod(des_direction - cur_direction + 360.0, 360.0); + + if (angle_diff > 180) + return Direction::LEFT_TURN; + else + return Direction::RIGHT_TURN; + } +} + +void PL_ProcThread() +{ + usleep(70000); + + int road_pos = 0; + int des_pos = 3; + int direction_pos = 20; + + while (1) + { + g_NavInfo.Latitude_degree = g_rtk.lat; + g_NavInfo.Longitude_degree = g_rtk.lon; + g_NavInfo.Yaw_rad = g_rtk.direction; + // double head_raw = g_NavInfo.Yaw_rad; + + road_pos = Road_Planning_Find_Nearest_Point(des_pos - 3, g_NavInfo, GPSPointQueue); + des_pos = Road_Planning_Find_Aim_Point(road_pos, 3); // 找到当前点后第 3 个点，作为目标点 + direction_pos = Road_Planning_Find_Aim_Point(road_pos, 6); // 找到当前点后第 6 个点，判断直行/转弯 + + printf("当前位置: %d\n", road_pos); + printf("目标位置: %d\n", des_pos); + // printf("direction_pos: %d\n", direction_pos); + printf("\n"); + + drive_mode = straight_or_turn(g_rtk.direction, GPSPointQueue[direction_pos].Heading, 12); + + // 新增距离判断逻辑 + double current_lon = g_rtk.lon; + double current_lat = g_rtk.lat; + double current_head = g_rtk.direction; + double target_lon = GPSPointQueue[des_pos].LongitudeInfo; + double target_lat = GPSPointQueue[des_pos].LatitudeInfo; + double target_head = GPSPointQueue[des_pos].Heading; + + // 计算当前定位点与目标点距离 + double distance = ntzx_GPS_length(current_lon, current_lat, target_lon, target_lat); + int nl_within_head = (abs(target_head - current_head) < 30) ? 1 : 0; + + nl_within_radius = ((distance <= nl_radius) && nl_within_head) ? 1 : 0; + // + // cout << " * * * * * * * * * * * * * * * * * * * * *" << endl; + // cout << endl; + + // cout << "--- -target_head " << target_head << "--- current_head " << current_head << " " << abs(target_head - current_head) << endl; + // cout << endl; + // cout << "--- -distance " << distance << "- -nl_radius " << nl_radius << endl; + + // cout << endl; + // cout << " * * * * * * * * * * * * * * * * * * * * *" << endl; + + if (road_pos >= (GpsPointNum - 1)) + { + + x = 0.0; + y = 1.0; + pl_speed = 0; + task_status = TaskStatus::COMPLETED; + sleep(3); + continue; + } + else if (des_pos != -1) + { + double x_cm_tmp = 0; + double y_cm_tmp = 0; + + ntzx_GPS_posit(g_rtk.direction, g_rtk.lon, g_rtk.lat, + GPSPointQueue[des_pos].LongitudeInfo, + GPSPointQueue[des_pos].LatitudeInfo, + &x_cm_tmp, &y_cm_tmp); + + x = x_cm_tmp; + y = y_cm_tmp; + pl_speed = 80; + task_status = TaskStatus::RUNNING; + } + + // printf("x_cm:%lf y_cm:%lf speed:%d\n", x, y, pl_speed); + } + + return; +} + +void *pl_thread(void *args) +{ + (void)args; + sleep(3); + + memset(GPSPointQueue, 0, sizeof(GPSPointQueue)); + + FILE *fp_gps = fopen("gps_load_now.txt", "r"); + if (NULL == fp_gps) + { + printf("open route file error\n"); + return NULL; + } + std::cout << "have opened " << "gps_load_now.txt" << std::endl; + + char mystring[200]; + memset(mystring, '\0', 200); + int point_num = 0; + int i = 0; + while (fgets(mystring, 200, fp_gps)) + { + i++; + + string line_string(mystring); + point_num = i / 4; + + if (i % 4 == 1) + { + GPSPointQueue[point_num].LatitudeInfo = atof(line_string.c_str()); + } + else if (i % 4 == 2) + { + GPSPointQueue[point_num].LongitudeInfo = atof(line_string.c_str()); + } + else if (i % 4 == 3) + { + GPSPointQueue[point_num].Heading = atof(line_string.c_str()); + } + } + + GpsPointNum = i / 4; + printf("point_num:%d\n", GpsPointNum); + fclose(fp_gps); + std::cout << std::endl; + + printf("#################### auto drive on! ####################\n"); + + PL_ProcThread(); + return NULL; +} diff --git a/src/pl/src/pl_node.cpp b/src/pl/src/pl_node.cpp new file mode 100644 index 0000000..1ea27af --- /dev/null +++ b/src/pl/src/pl_node.cpp @@ -0,0 +1,199 @@ +#include "rclcpp/rclcpp.hpp" +#include "sweeper_interfaces/msg/rtk.hpp" +#include "sweeper_interfaces/msg/pl.hpp" +#include "sweeper_interfaces/msg/route.hpp" +#include "sweeper_interfaces/msg/task.hpp" +#include "json.h" +#include +#include +#include +#include +#include "pl.hpp" +#include +#include +#include +#include + +using namespace std; +pthread_t pl_thread_t; +int is_start = 0; + +static int sock = -1; + +class pl_node : public rclcpp::Node +{ +public: + pl_node(std::string name) : Node(name) + { + RCLCPP_INFO(this->get_logger(), "%s节点已经启动.", name.c_str()); + // 创建订阅者订阅话题 + msg_subscribe_ = this->create_subscription("rtk_message", 10, std::bind(&pl_node::msg_callback, this, std::placeholders::_1)); + task_subscribe_ = this->create_subscription("task_message/download", 10, std::bind(&pl_node::task_callback, this, std::placeholders::_1)); + + // 创建发布者 + msg_publisher_ = this->create_publisher("pl_message", 10); + task_publisher_ = this->create_publisher("task_message/upload", 10); + + // 创建定时器，定时发布 + timer_pl = this->create_wall_timer(std::chrono::milliseconds(500), std::bind(&pl_node::timer_callback_pl, this)); + timer_task = this->create_wall_timer(std::chrono::milliseconds(200), std::bind(&pl_node::timer_callback_task, this)); + } + +private: + // 收到话题数据的回调函数 + void msg_callback(const sweeper_interfaces::msg::Rtk::SharedPtr msg) + { + // RCLCPP_INFO(this->get_logger(), "lat:'%.9lf',lon:'%.9lf',head:'%lf',speed:'%lf',p_quality:'%d',h_quality:'%d'", + // msg->lat, msg->lon, msg->head, msg->speed, msg->p_quality, msg->h_quality); + + g_rtk.reliability = 1; + if (msg->p_quality != 4 || msg->h_quality != 4) + { + g_rtk.reliability = 0; + RCLCPP_INFO(this->get_logger(), "rtk信号差!"); + } + + g_rtk.lat = msg->lat; + g_rtk.lon = msg->lon; + g_rtk.direction = msg->head; + } + + void task_callback(const sweeper_interfaces::msg::Task::SharedPtr msg) + { + if (is_start == 0 && msg->task_status == 1) + { + pthread_create(&pl_thread_t, NULL, pl_thread, NULL); + cout << " started" << endl; + is_start = 1; + task_status = TaskStatus::PENDING; + } + else if (is_start == 1 && msg->task_status == 0) + { + pthread_cancel(pl_thread_t); + cout << "pl_thread_t is canceled " << endl; + is_start = 0; + task_status = TaskStatus::COMPLETED; + } + // RCLCPP_INFO(this->get_logger(), " ==================================== is_start : %d", is_start); + // RCLCPP_INFO(this->get_logger(), " ==================================== task_status : %d", msg->task_status); + } + + void timer_callback_pl() + { + if (is_start == 1) + { + if (sock < 0) // 首次连接 + { + struct sockaddr_in serv_addr; + if ((sock = socket(AF_INET, SOCK_STREAM, 0)) < 0) + { + RCLCPP_ERROR(this->get_logger(), "Socket creation error"); + return; + } + + serv_addr.sin_family = AF_INET; + serv_addr.sin_port = htons(9999); + + if (inet_pton(AF_INET, "192.168.3.17", &serv_addr.sin_addr) <= 0) + { + RCLCPP_ERROR(this->get_logger(), "Invalid address/ Address not supported"); + close(sock); + return; + } + + if (connect(sock, (struct sockaddr *)&serv_addr, sizeof(serv_addr)) < 0) + { + RCLCPP_ERROR(this->get_logger(), "Connection Failed"); + close(sock); + sock = -1; + return; + } + } + + const char *message = "Status: RUNNING"; + if (send(sock, message, strlen(message), 0) < 0) // 发送失败则重建连接 + { + close(sock); + sock = -1; + return timer_callback_pl(); // 重试 + } + } + else if (sock >= 0) + { + close(sock); // 停止时关闭连接 + sock = -1; + } + + // 创建消息 + sweeper_interfaces::msg::Pl message; + message.x = x; + message.y = y; + message.speed = (float)pl_speed; + message.reliability = g_rtk.reliability; + // message.drive_mode = drive_mode; + if (drive_mode == Direction::STRAIGHT_D) // 直行 + message.drive_mode = 0; + else if (drive_mode == Direction::LEFT_TURN || drive_mode == Direction::RIGHT_TURN) // 转弯 + message.drive_mode = 1; + + message.enter_range = nl_within_radius; + message.is_start = is_start; + // RCLCPP_INFO(this->get_logger(), " ==================================== message.is_start : %d", message.is_start); + + // 日志打印 + // RCLCPP_INFO(this->get_logger(), "x:'%lf',y:'%lf',speed:'%lf',drive_mode:'%d'", + // message.x, message.y, message.speed, message.drive_mode); + // 发布消息 + msg_publisher_->publish(message); + } + + void timer_callback_task() + { + // 创建消息 + sweeper_interfaces::msg::Task message; + message.task_status = task_status; + task_publisher_->publish(message); + } + + // 声名定时器指针 + rclcpp::TimerBase::SharedPtr timer_pl; + rclcpp::TimerBase::SharedPtr timer_task; + + // 声明话题发布者指针 + rclcpp::Publisher::SharedPtr msg_publisher_; + rclcpp::Publisher::SharedPtr task_publisher_; + + // 声明订阅者 + rclcpp::Subscription::SharedPtr task_subscribe_; + rclcpp::Subscription::SharedPtr msg_subscribe_; +}; + +void initConfig() +{ + Json::Reader reader; + Json::Value root; + std::ifstream in("config.json", std::ios::binary); + if (!in.is_open()) + { + std::cout << "read config file error" << std::endl; + return; + } + if (reader.parse(in, root)) + { + nl_radius = root["detect_line_tolerance"].as(); + } + in.close(); // 关闭文件流 +} +int main(int argc, char **argv) +{ + initConfig(); + rclcpp::init(argc, argv); + /*创建对应节点的共享指针对象*/ + auto node = std::make_shared("pl_node"); + /* 运行节点，并检测退出信号*/ + rclcpp::spin(node); + rclcpp::shutdown(); + + // pthread_join(pl_thread_t, NULL); + return 0; +} \ No newline at end of file diff --git a/src/route/CMakeLists.txt b/src/route/CMakeLists.txt new file mode 100644 index 0000000..c253540 --- /dev/null +++ b/src/route/CMakeLists.txt @@ -0,0 +1,52 @@ +cmake_minimum_required(VERSION 3.5) +project(route) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sweeper_interfaces REQUIRED) +find_package(CURL REQUIRED) + + +include_directories( + include/route + ${catkin_INCLUDE_DIRS} +) + +add_executable(route_node + src/route_node.cpp + src/md5.cpp + src/jsoncpp.cpp) +ament_target_dependencies(route_node rclcpp std_msgs sweeper_interfaces CURL) + +install(TARGETS + route_node + DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() +ament_package() diff --git a/src/route/include/route/json.h b/src/route/include/route/json.h new file mode 100644 index 0000000..d95fe6e --- /dev/null +++ b/src/route/include/route/json.h @@ -0,0 +1,2351 @@ +/// Json-cpp amalgamated header (http://jsoncpp.sourceforge.net/). +/// It is intended to be used with #include "json/json.h" + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: LICENSE +// ////////////////////////////////////////////////////////////////////// + +/* +The JsonCpp library's source code, including accompanying documentation, +tests and demonstration applications, are licensed under the following +conditions... + +Baptiste Lepilleur and The JsonCpp Authors explicitly disclaim copyright in all +jurisdictions which recognize such a disclaimer. In such jurisdictions, +this software is released into the Public Domain. + +In jurisdictions which do not recognize Public Domain property (e.g. Germany as of +2010), this software is Copyright (c) 2007-2010 by Baptiste Lepilleur and +The JsonCpp Authors, and is released under the terms of the MIT License (see below). + +In jurisdictions which recognize Public Domain property, the user of this +software may choose to accept it either as 1) Public Domain, 2) under the +conditions of the MIT License (see below), or 3) under the terms of dual +Public Domain/MIT License conditions described here, as they choose. + +The MIT License is about as close to Public Domain as a license can get, and is +described in clear, concise terms at: + + http://en.wikipedia.org/wiki/MIT_License + +The full text of the MIT License follows: + +======================================================================== +Copyright (c) 2007-2010 Baptiste Lepilleur and The JsonCpp Authors + +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. +======================================================================== +(END LICENSE TEXT) + +The MIT license is compatible with both the GPL and commercial +software, affording one all of the rights of Public Domain with the +minor nuisance of being required to keep the above copyright notice +and license text in the source code. Note also that by accepting the +Public Domain "license" you can re-license your copy using whatever +license you like. + +*/ + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: LICENSE +// ////////////////////////////////////////////////////////////////////// + + + + + +#ifndef JSON_AMALGAMATED_H_INCLUDED +# define JSON_AMALGAMATED_H_INCLUDED +/// If defined, indicates that the source file is amalgamated +/// to prevent private header inclusion. +#define JSON_IS_AMALGAMATION + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: include/json/version.h +// ////////////////////////////////////////////////////////////////////// + +#ifndef JSON_VERSION_H_INCLUDED +#define JSON_VERSION_H_INCLUDED + +// Note: version must be updated in three places when doing a release. This +// annoying process ensures that amalgamate, CMake, and meson all report the +// correct version. +// 1. /meson.build +// 2. /include/json/version.h +// 3. /CMakeLists.txt +// IMPORTANT: also update the SOVERSION!! + +#define JSONCPP_VERSION_STRING "1.9.5" +#define JSONCPP_VERSION_MAJOR 1 +#define JSONCPP_VERSION_MINOR 9 +#define JSONCPP_VERSION_PATCH 5 +#define JSONCPP_VERSION_QUALIFIER +#define JSONCPP_VERSION_HEXA \ + ((JSONCPP_VERSION_MAJOR << 24) | (JSONCPP_VERSION_MINOR << 16) | \ + (JSONCPP_VERSION_PATCH << 8)) + +#ifdef JSONCPP_USING_SECURE_MEMORY +#undef JSONCPP_USING_SECURE_MEMORY +#endif +#define JSONCPP_USING_SECURE_MEMORY 0 +// If non-zero, the library zeroes any memory that it has allocated before +// it frees its memory. + +#endif // JSON_VERSION_H_INCLUDED + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: include/json/version.h +// ////////////////////////////////////////////////////////////////////// + + + + + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: include/json/allocator.h +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2007-2010 Baptiste Lepilleur and The JsonCpp Authors +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +#ifndef JSON_ALLOCATOR_H_INCLUDED +#define JSON_ALLOCATOR_H_INCLUDED + +#include +#include + +#pragma pack(push) +#pragma pack() + +namespace Json { +template class SecureAllocator { +public: + // Type definitions + using value_type = T; + using pointer = T*; + using const_pointer = const T*; + using reference = T&; + using const_reference = const T&; + using size_type = std::size_t; + using difference_type = std::ptrdiff_t; + + /** + * Allocate memory for N items using the standard allocator. + */ + pointer allocate(size_type n) { + // allocate using "global operator new" + return static_cast(::operator new(n * sizeof(T))); + } + + /** + * Release memory which was allocated for N items at pointer P. + * + * The memory block is filled with zeroes before being released. + */ + void deallocate(pointer p, size_type n) { + // memset_s is used because memset may be optimized away by the compiler + memset_s(p, n * sizeof(T), 0, n * sizeof(T)); + // free using "global operator delete" + ::operator delete(p); + } + + /** + * Construct an item in-place at pointer P. + */ + template void construct(pointer p, Args&&... args) { + // construct using "placement new" and "perfect forwarding" + ::new (static_cast(p)) T(std::forward(args)...); + } + + size_type max_size() const { return size_t(-1) / sizeof(T); } + + pointer address(reference x) const { return std::addressof(x); } + + const_pointer address(const_reference x) const { return std::addressof(x); } + + /** + * Destroy an item in-place at pointer P. + */ + void destroy(pointer p) { + // destroy using "explicit destructor" + p->~T(); + } + + // Boilerplate + SecureAllocator() {} + template SecureAllocator(const SecureAllocator&) {} + template struct rebind { using other = SecureAllocator; }; +}; + +template +bool operator==(const SecureAllocator&, const SecureAllocator&) { + return true; +} + +template +bool operator!=(const SecureAllocator&, const SecureAllocator&) { + return false; +} + +} // namespace Json + +#pragma pack(pop) + +#endif // JSON_ALLOCATOR_H_INCLUDED + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: include/json/allocator.h +// ////////////////////////////////////////////////////////////////////// + + + + + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: include/json/config.h +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2007-2010 Baptiste Lepilleur and The JsonCpp Authors +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +#ifndef JSON_CONFIG_H_INCLUDED +#define JSON_CONFIG_H_INCLUDED +#include +#include +#include +#include +#include +#include +#include +#include + +// If non-zero, the library uses exceptions to report bad input instead of C +// assertion macros. The default is to use exceptions. +#ifndef JSON_USE_EXCEPTION +#define JSON_USE_EXCEPTION 1 +#endif + +// Temporary, tracked for removal with issue #982. +#ifndef JSON_USE_NULLREF +#define JSON_USE_NULLREF 1 +#endif + +/// If defined, indicates that the source file is amalgamated +/// to prevent private header inclusion. +/// Remarks: it is automatically defined in the generated amalgamated header. +// #define JSON_IS_AMALGAMATION + +// Export macros for DLL visibility +#if defined(JSON_DLL_BUILD) +#if defined(_MSC_VER) || defined(__MINGW32__) +#define JSON_API __declspec(dllexport) +#define JSONCPP_DISABLE_DLL_INTERFACE_WARNING +#elif defined(__GNUC__) || defined(__clang__) +#define JSON_API __attribute__((visibility("default"))) +#endif // if defined(_MSC_VER) + +#elif defined(JSON_DLL) +#if defined(_MSC_VER) || defined(__MINGW32__) +#define JSON_API __declspec(dllimport) +#define JSONCPP_DISABLE_DLL_INTERFACE_WARNING +#endif // if defined(_MSC_VER) +#endif // ifdef JSON_DLL_BUILD + +#if !defined(JSON_API) +#define JSON_API +#endif + +#if defined(_MSC_VER) && _MSC_VER < 1800 +#error \ + "ERROR: Visual Studio 12 (2013) with _MSC_VER=1800 is the oldest supported compiler with sufficient C++11 capabilities" +#endif + +#if defined(_MSC_VER) && _MSC_VER < 1900 +// As recommended at +// https://stackoverflow.com/questions/2915672/snprintf-and-visual-studio-2010 +extern JSON_API int msvc_pre1900_c99_snprintf(char* outBuf, size_t size, + const char* format, ...); +#define jsoncpp_snprintf msvc_pre1900_c99_snprintf +#else +#define jsoncpp_snprintf std::snprintf +#endif + +// If JSON_NO_INT64 is defined, then Json only support C++ "int" type for +// integer +// Storages, and 64 bits integer support is disabled. +// #define JSON_NO_INT64 1 + +// JSONCPP_OVERRIDE is maintained for backwards compatibility of external tools. +// C++11 should be used directly in JSONCPP. +#define JSONCPP_OVERRIDE override + +#ifdef __clang__ +#if __has_extension(attribute_deprecated_with_message) +#define JSONCPP_DEPRECATED(message) __attribute__((deprecated(message))) +#endif +#elif defined(__GNUC__) // not clang (gcc comes later since clang emulates gcc) +#if (__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 5)) +#define JSONCPP_DEPRECATED(message) __attribute__((deprecated(message))) +#elif (__GNUC__ > 3 || (__GNUC__ == 3 && __GNUC_MINOR__ >= 1)) +#define JSONCPP_DEPRECATED(message) __attribute__((__deprecated__)) +#endif // GNUC version +#elif defined(_MSC_VER) // MSVC (after clang because clang on Windows emulates + // MSVC) +#define JSONCPP_DEPRECATED(message) __declspec(deprecated(message)) +#endif // __clang__ || __GNUC__ || _MSC_VER + +#if !defined(JSONCPP_DEPRECATED) +#define JSONCPP_DEPRECATED(message) +#endif // if !defined(JSONCPP_DEPRECATED) + +#if defined(__clang__) || (defined(__GNUC__) && (__GNUC__ >= 6)) +#define JSON_USE_INT64_DOUBLE_CONVERSION 1 +#endif + +#if !defined(JSON_IS_AMALGAMATION) + +#include "allocator.h" +#include "version.h" + +#endif // if !defined(JSON_IS_AMALGAMATION) + +namespace Json { +using Int = int; +using UInt = unsigned int; +#if defined(JSON_NO_INT64) +using LargestInt = int; +using LargestUInt = unsigned int; +#undef JSON_HAS_INT64 +#else // if defined(JSON_NO_INT64) +// For Microsoft Visual use specific types as long long is not supported +#if defined(_MSC_VER) // Microsoft Visual Studio +using Int64 = __int64; +using UInt64 = unsigned __int64; +#else // if defined(_MSC_VER) // Other platforms, use long long +using Int64 = int64_t; +using UInt64 = uint64_t; +#endif // if defined(_MSC_VER) +using LargestInt = Int64; +using LargestUInt = UInt64; +#define JSON_HAS_INT64 +#endif // if defined(JSON_NO_INT64) + +template +using Allocator = + typename std::conditional, + std::allocator>::type; +using String = std::basic_string, Allocator>; +using IStringStream = + std::basic_istringstream; +using OStringStream = + std::basic_ostringstream; +using IStream = std::istream; +using OStream = std::ostream; +} // namespace Json + +// Legacy names (formerly macros). +using JSONCPP_STRING = Json::String; +using JSONCPP_ISTRINGSTREAM = Json::IStringStream; +using JSONCPP_OSTRINGSTREAM = Json::OStringStream; +using JSONCPP_ISTREAM = Json::IStream; +using JSONCPP_OSTREAM = Json::OStream; + +#endif // JSON_CONFIG_H_INCLUDED + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: include/json/config.h +// ////////////////////////////////////////////////////////////////////// + + + + + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: include/json/forwards.h +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2007-2010 Baptiste Lepilleur and The JsonCpp Authors +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +#ifndef JSON_FORWARDS_H_INCLUDED +#define JSON_FORWARDS_H_INCLUDED + +#if !defined(JSON_IS_AMALGAMATION) +#include "config.h" +#endif // if !defined(JSON_IS_AMALGAMATION) + +namespace Json { + +// writer.h +class StreamWriter; +class StreamWriterBuilder; +class Writer; +class FastWriter; +class StyledWriter; +class StyledStreamWriter; + +// reader.h +class Reader; +class CharReader; +class CharReaderBuilder; + +// json_features.h +class Features; + +// value.h +using ArrayIndex = unsigned int; +class StaticString; +class Path; +class PathArgument; +class Value; +class ValueIteratorBase; +class ValueIterator; +class ValueConstIterator; + +} // namespace Json + +#endif // JSON_FORWARDS_H_INCLUDED + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: include/json/forwards.h +// ////////////////////////////////////////////////////////////////////// + + + + + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: include/json/json_features.h +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2007-2010 Baptiste Lepilleur and The JsonCpp Authors +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +#ifndef JSON_FEATURES_H_INCLUDED +#define JSON_FEATURES_H_INCLUDED + +#if !defined(JSON_IS_AMALGAMATION) +#include "forwards.h" +#endif // if !defined(JSON_IS_AMALGAMATION) + +#pragma pack(push) +#pragma pack() + +namespace Json { + +/** \brief Configuration passed to reader and writer. + * This configuration object can be used to force the Reader or Writer + * to behave in a standard conforming way. + */ +class JSON_API Features { +public: + /** \brief A configuration that allows all features and assumes all strings + * are UTF-8. + * - C & C++ comments are allowed + * - Root object can be any JSON value + * - Assumes Value strings are encoded in UTF-8 + */ + static Features all(); + + /** \brief A configuration that is strictly compatible with the JSON + * specification. + * - Comments are forbidden. + * - Root object must be either an array or an object value. + * - Assumes Value strings are encoded in UTF-8 + */ + static Features strictMode(); + + /** \brief Initialize the configuration like JsonConfig::allFeatures; + */ + Features(); + + /// \c true if comments are allowed. Default: \c true. + bool allowComments_{true}; + + /// \c true if root must be either an array or an object value. Default: \c + /// false. + bool strictRoot_{false}; + + /// \c true if dropped null placeholders are allowed. Default: \c false. + bool allowDroppedNullPlaceholders_{false}; + + /// \c true if numeric object key are allowed. Default: \c false. + bool allowNumericKeys_{false}; +}; + +} // namespace Json + +#pragma pack(pop) + +#endif // JSON_FEATURES_H_INCLUDED + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: include/json/json_features.h +// ////////////////////////////////////////////////////////////////////// + + + + + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: include/json/value.h +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2007-2010 Baptiste Lepilleur and The JsonCpp Authors +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +#ifndef JSON_H_INCLUDED +#define JSON_H_INCLUDED + +#if !defined(JSON_IS_AMALGAMATION) +#include "forwards.h" +#endif // if !defined(JSON_IS_AMALGAMATION) + +// Conditional NORETURN attribute on the throw functions would: +// a) suppress false positives from static code analysis +// b) possibly improve optimization opportunities. +#if !defined(JSONCPP_NORETURN) +#if defined(_MSC_VER) && _MSC_VER == 1800 +#define JSONCPP_NORETURN __declspec(noreturn) +#else +#define JSONCPP_NORETURN [[noreturn]] +#endif +#endif + +// Support for '= delete' with template declarations was a late addition +// to the c++11 standard and is rejected by clang 3.8 and Apple clang 8.2 +// even though these declare themselves to be c++11 compilers. +#if !defined(JSONCPP_TEMPLATE_DELETE) +#if defined(__clang__) && defined(__apple_build_version__) +#if __apple_build_version__ <= 8000042 +#define JSONCPP_TEMPLATE_DELETE +#endif +#elif defined(__clang__) +#if __clang_major__ == 3 && __clang_minor__ <= 8 +#define JSONCPP_TEMPLATE_DELETE +#endif +#endif +#if !defined(JSONCPP_TEMPLATE_DELETE) +#define JSONCPP_TEMPLATE_DELETE = delete +#endif +#endif + +#include +#include +#include +#include +#include +#include + +// Disable warning C4251: : needs to have dll-interface to +// be used by... +#if defined(JSONCPP_DISABLE_DLL_INTERFACE_WARNING) +#pragma warning(push) +#pragma warning(disable : 4251 4275) +#endif // if defined(JSONCPP_DISABLE_DLL_INTERFACE_WARNING) + +#pragma pack(push) +#pragma pack() + +/** \brief JSON (JavaScript Object Notation). + */ +namespace Json { + +#if JSON_USE_EXCEPTION +/** Base class for all exceptions we throw. + * + * We use nothing but these internally. Of course, STL can throw others. + */ +class JSON_API Exception : public std::exception { +public: + Exception(String msg); + ~Exception() noexcept override; + char const* what() const noexcept override; + +protected: + String msg_; +}; + +/** Exceptions which the user cannot easily avoid. + * + * E.g. out-of-memory (when we use malloc), stack-overflow, malicious input + * + * \remark derived from Json::Exception + */ +class JSON_API RuntimeError : public Exception { +public: + RuntimeError(String const& msg); +}; + +/** Exceptions thrown by JSON_ASSERT/JSON_FAIL macros. + * + * These are precondition-violations (user bugs) and internal errors (our bugs). + * + * \remark derived from Json::Exception + */ +class JSON_API LogicError : public Exception { +public: + LogicError(String const& msg); +}; +#endif + +/// used internally +JSONCPP_NORETURN void throwRuntimeError(String const& msg); +/// used internally +JSONCPP_NORETURN void throwLogicError(String const& msg); + +/** \brief Type of the value held by a Value object. + */ +enum ValueType { + nullValue = 0, ///< 'null' value + intValue, ///< signed integer value + uintValue, ///< unsigned integer value + realValue, ///< double value + stringValue, ///< UTF-8 string value + booleanValue, ///< bool value + arrayValue, ///< array value (ordered list) + objectValue ///< object value (collection of name/value pairs). +}; + +enum CommentPlacement { + commentBefore = 0, ///< a comment placed on the line before a value + commentAfterOnSameLine, ///< a comment just after a value on the same line + commentAfter, ///< a comment on the line after a value (only make sense for + /// root value) + numberOfCommentPlacement +}; + +/** \brief Type of precision for formatting of real values. + */ +enum PrecisionType { + significantDigits = 0, ///< we set max number of significant digits in string + decimalPlaces ///< we set max number of digits after "." in string +}; + +/** \brief Lightweight wrapper to tag static string. + * + * Value constructor and objectValue member assignment takes advantage of the + * StaticString and avoid the cost of string duplication when storing the + * string or the member name. + * + * Example of usage: + * \code + * Json::Value aValue( StaticString("some text") ); + * Json::Value object; + * static const StaticString code("code"); + * object[code] = 1234; + * \endcode + */ +class JSON_API StaticString { +public: + explicit StaticString(const char* czstring) : c_str_(czstring) {} + + operator const char*() const { return c_str_; } + + const char* c_str() const { return c_str_; } + +private: + const char* c_str_; +}; + +/** \brief Represents a JSON value. + * + * This class is a discriminated union wrapper that can represents a: + * - signed integer [range: Value::minInt - Value::maxInt] + * - unsigned integer (range: 0 - Value::maxUInt) + * - double + * - UTF-8 string + * - boolean + * - 'null' + * - an ordered list of Value + * - collection of name/value pairs (javascript object) + * + * The type of the held value is represented by a #ValueType and + * can be obtained using type(). + * + * Values of an #objectValue or #arrayValue can be accessed using operator[]() + * methods. + * Non-const methods will automatically create the a #nullValue element + * if it does not exist. + * The sequence of an #arrayValue will be automatically resized and initialized + * with #nullValue. resize() can be used to enlarge or truncate an #arrayValue. + * + * The get() methods can be used to obtain default value in the case the + * required element does not exist. + * + * It is possible to iterate over the list of member keys of an object using + * the getMemberNames() method. + * + * \note #Value string-length fit in size_t, but keys must be < 2^30. + * (The reason is an implementation detail.) A #CharReader will raise an + * exception if a bound is exceeded to avoid security holes in your app, + * but the Value API does *not* check bounds. That is the responsibility + * of the caller. + */ +class JSON_API Value { + friend class ValueIteratorBase; + +public: + using Members = std::vector; + using iterator = ValueIterator; + using const_iterator = ValueConstIterator; + using UInt = Json::UInt; + using Int = Json::Int; +#if defined(JSON_HAS_INT64) + using UInt64 = Json::UInt64; + using Int64 = Json::Int64; +#endif // defined(JSON_HAS_INT64) + using LargestInt = Json::LargestInt; + using LargestUInt = Json::LargestUInt; + using ArrayIndex = Json::ArrayIndex; + + // Required for boost integration, e. g. BOOST_TEST + using value_type = std::string; + +#if JSON_USE_NULLREF + // Binary compatibility kludges, do not use. + static const Value& null; + static const Value& nullRef; +#endif + + // null and nullRef are deprecated, use this instead. + static Value const& nullSingleton(); + + /// Minimum signed integer value that can be stored in a Json::Value. + static constexpr LargestInt minLargestInt = + LargestInt(~(LargestUInt(-1) / 2)); + /// Maximum signed integer value that can be stored in a Json::Value. + static constexpr LargestInt maxLargestInt = LargestInt(LargestUInt(-1) / 2); + /// Maximum unsigned integer value that can be stored in a Json::Value. + static constexpr LargestUInt maxLargestUInt = LargestUInt(-1); + + /// Minimum signed int value that can be stored in a Json::Value. + static constexpr Int minInt = Int(~(UInt(-1) / 2)); + /// Maximum signed int value that can be stored in a Json::Value. + static constexpr Int maxInt = Int(UInt(-1) / 2); + /// Maximum unsigned int value that can be stored in a Json::Value. + static constexpr UInt maxUInt = UInt(-1); + +#if defined(JSON_HAS_INT64) + /// Minimum signed 64 bits int value that can be stored in a Json::Value. + static constexpr Int64 minInt64 = Int64(~(UInt64(-1) / 2)); + /// Maximum signed 64 bits int value that can be stored in a Json::Value. + static constexpr Int64 maxInt64 = Int64(UInt64(-1) / 2); + /// Maximum unsigned 64 bits int value that can be stored in a Json::Value. + static constexpr UInt64 maxUInt64 = UInt64(-1); +#endif // defined(JSON_HAS_INT64) + /// Default precision for real value for string representation. + static constexpr UInt defaultRealPrecision = 17; + // The constant is hard-coded because some compiler have trouble + // converting Value::maxUInt64 to a double correctly (AIX/xlC). + // Assumes that UInt64 is a 64 bits integer. + static constexpr double maxUInt64AsDouble = 18446744073709551615.0; +// Workaround for bug in the NVIDIAs CUDA 9.1 nvcc compiler +// when using gcc and clang backend compilers. CZString +// cannot be defined as private. See issue #486 +#ifdef __NVCC__ +public: +#else +private: +#endif +#ifndef JSONCPP_DOC_EXCLUDE_IMPLEMENTATION + class CZString { + public: + enum DuplicationPolicy { noDuplication = 0, duplicate, duplicateOnCopy }; + CZString(ArrayIndex index); + CZString(char const* str, unsigned length, DuplicationPolicy allocate); + CZString(CZString const& other); + CZString(CZString&& other) noexcept; + ~CZString(); + CZString& operator=(const CZString& other); + CZString& operator=(CZString&& other) noexcept; + + bool operator<(CZString const& other) const; + bool operator==(CZString const& other) const; + ArrayIndex index() const; + // const char* c_str() const; ///< \deprecated + char const* data() const; + unsigned length() const; + bool isStaticString() const; + + private: + void swap(CZString& other); + + struct StringStorage { + unsigned policy_ : 2; + unsigned length_ : 30; // 1GB max + }; + + char const* cstr_; // actually, a prefixed string, unless policy is noDup + union { + ArrayIndex index_; + StringStorage storage_; + }; + }; + +public: + typedef std::map ObjectValues; +#endif // ifndef JSONCPP_DOC_EXCLUDE_IMPLEMENTATION + +public: + /** + * \brief Create a default Value of the given type. + * + * This is a very useful constructor. + * To create an empty array, pass arrayValue. + * To create an empty object, pass objectValue. + * Another Value can then be set to this one by assignment. + * This is useful since clear() and resize() will not alter types. + * + * Examples: + * \code + * Json::Value null_value; // null + * Json::Value arr_value(Json::arrayValue); // [] + * Json::Value obj_value(Json::objectValue); // {} + * \endcode + */ + Value(ValueType type = nullValue); + Value(Int value); + Value(UInt value); +#if defined(JSON_HAS_INT64) + Value(Int64 value); + Value(UInt64 value); +#endif // if defined(JSON_HAS_INT64) + Value(double value); + Value(const char* value); ///< Copy til first 0. (NULL causes to seg-fault.) + Value(const char* begin, const char* end); ///< Copy all, incl zeroes. + /** + * \brief Constructs a value from a static string. + * + * Like other value string constructor but do not duplicate the string for + * internal storage. The given string must remain alive after the call to + * this constructor. + * + * \note This works only for null-terminated strings. (We cannot change the + * size of this class, so we have nowhere to store the length, which might be + * computed later for various operations.) + * + * Example of usage: + * \code + * static StaticString foo("some text"); + * Json::Value aValue(foo); + * \endcode + */ + Value(const StaticString& value); + Value(const String& value); + Value(bool value); + Value(std::nullptr_t ptr) = delete; + Value(const Value& other); + Value(Value&& other) noexcept; + ~Value(); + + /// \note Overwrite existing comments. To preserve comments, use + /// #swapPayload(). + Value& operator=(const Value& other); + Value& operator=(Value&& other) noexcept; + + /// Swap everything. + void swap(Value& other); + /// Swap values but leave comments and source offsets in place. + void swapPayload(Value& other); + + /// copy everything. + void copy(const Value& other); + /// copy values but leave comments and source offsets in place. + void copyPayload(const Value& other); + + ValueType type() const; + + /// Compare payload only, not comments etc. + bool operator<(const Value& other) const; + bool operator<=(const Value& other) const; + bool operator>=(const Value& other) const; + bool operator>(const Value& other) const; + bool operator==(const Value& other) const; + bool operator!=(const Value& other) const; + int compare(const Value& other) const; + + const char* asCString() const; ///< Embedded zeroes could cause you trouble! +#if JSONCPP_USING_SECURE_MEMORY + unsigned getCStringLength() const; // Allows you to understand the length of + // the CString +#endif + String asString() const; ///< Embedded zeroes are possible. + /** Get raw char* of string-value. + * \return false if !string. (Seg-fault if str or end are NULL.) + */ + bool getString(char const** begin, char const** end) const; + Int asInt() const; + UInt asUInt() const; +#if defined(JSON_HAS_INT64) + Int64 asInt64() const; + UInt64 asUInt64() const; +#endif // if defined(JSON_HAS_INT64) + LargestInt asLargestInt() const; + LargestUInt asLargestUInt() const; + float asFloat() const; + double asDouble() const; + bool asBool() const; + + bool isNull() const; + bool isBool() const; + bool isInt() const; + bool isInt64() const; + bool isUInt() const; + bool isUInt64() const; + bool isIntegral() const; + bool isDouble() const; + bool isNumeric() const; + bool isString() const; + bool isArray() const; + bool isObject() const; + + /// The `as` and `is` member function templates and specializations. + template T as() const JSONCPP_TEMPLATE_DELETE; + template bool is() const JSONCPP_TEMPLATE_DELETE; + + bool isConvertibleTo(ValueType other) const; + + /// Number of values in array or object + ArrayIndex size() const; + + /// \brief Return true if empty array, empty object, or null; + /// otherwise, false. + bool empty() const; + + /// Return !isNull() + explicit operator bool() const; + + /// Remove all object members and array elements. + /// \pre type() is arrayValue, objectValue, or nullValue + /// \post type() is unchanged + void clear(); + + /// Resize the array to newSize elements. + /// New elements are initialized to null. + /// May only be called on nullValue or arrayValue. + /// \pre type() is arrayValue or nullValue + /// \post type() is arrayValue + void resize(ArrayIndex newSize); + + ///@{ + /// Access an array element (zero based index). If the array contains less + /// than index element, then null value are inserted in the array so that + /// its size is index+1. + /// (You may need to say 'value[0u]' to get your compiler to distinguish + /// this from the operator[] which takes a string.) + Value& operator[](ArrayIndex index); + Value& operator[](int index); + ///@} + + ///@{ + /// Access an array element (zero based index). + /// (You may need to say 'value[0u]' to get your compiler to distinguish + /// this from the operator[] which takes a string.) + const Value& operator[](ArrayIndex index) const; + const Value& operator[](int index) const; + ///@} + + /// If the array contains at least index+1 elements, returns the element + /// value, otherwise returns defaultValue. + Value get(ArrayIndex index, const Value& defaultValue) const; + /// Return true if index < size(). + bool isValidIndex(ArrayIndex index) const; + /// \brief Append value to array at the end. + /// + /// Equivalent to jsonvalue[jsonvalue.size()] = value; + Value& append(const Value& value); + Value& append(Value&& value); + + /// \brief Insert value in array at specific index + bool insert(ArrayIndex index, const Value& newValue); + bool insert(ArrayIndex index, Value&& newValue); + + /// Access an object value by name, create a null member if it does not exist. + /// \note Because of our implementation, keys are limited to 2^30 -1 chars. + /// Exceeding that will cause an exception. + Value& operator[](const char* key); + /// Access an object value by name, returns null if there is no member with + /// that name. + const Value& operator[](const char* key) const; + /// Access an object value by name, create a null member if it does not exist. + /// \param key may contain embedded nulls. + Value& operator[](const String& key); + /// Access an object value by name, returns null if there is no member with + /// that name. + /// \param key may contain embedded nulls. + const Value& operator[](const String& key) const; + /** \brief Access an object value by name, create a null member if it does not + * exist. + * + * If the object has no entry for that name, then the member name used to + * store the new entry is not duplicated. + * Example of use: + * \code + * Json::Value object; + * static const StaticString code("code"); + * object[code] = 1234; + * \endcode + */ + Value& operator[](const StaticString& key); + /// Return the member named key if it exist, defaultValue otherwise. + /// \note deep copy + Value get(const char* key, const Value& defaultValue) const; + /// Return the member named key if it exist, defaultValue otherwise. + /// \note deep copy + /// \note key may contain embedded nulls. + Value get(const char* begin, const char* end, + const Value& defaultValue) const; + /// Return the member named key if it exist, defaultValue otherwise. + /// \note deep copy + /// \param key may contain embedded nulls. + Value get(const String& key, const Value& defaultValue) const; + /// Most general and efficient version of isMember()const, get()const, + /// and operator[]const + /// \note As stated elsewhere, behavior is undefined if (end-begin) >= 2^30 + Value const* find(char const* begin, char const* end) const; + /// Most general and efficient version of object-mutators. + /// \note As stated elsewhere, behavior is undefined if (end-begin) >= 2^30 + /// \return non-zero, but JSON_ASSERT if this is neither object nor nullValue. + Value* demand(char const* begin, char const* end); + /// \brief Remove and return the named member. + /// + /// Do nothing if it did not exist. + /// \pre type() is objectValue or nullValue + /// \post type() is unchanged + void removeMember(const char* key); + /// Same as removeMember(const char*) + /// \param key may contain embedded nulls. + void removeMember(const String& key); + /// Same as removeMember(const char* begin, const char* end, Value* removed), + /// but 'key' is null-terminated. + bool removeMember(const char* key, Value* removed); + /** \brief Remove the named map member. + * + * Update 'removed' iff removed. + * \param key may contain embedded nulls. + * \return true iff removed (no exceptions) + */ + bool removeMember(String const& key, Value* removed); + /// Same as removeMember(String const& key, Value* removed) + bool removeMember(const char* begin, const char* end, Value* removed); + /** \brief Remove the indexed array element. + * + * O(n) expensive operations. + * Update 'removed' iff removed. + * \return true if removed (no exceptions) + */ + bool removeIndex(ArrayIndex index, Value* removed); + + /// Return true if the object has a member named key. + /// \note 'key' must be null-terminated. + bool isMember(const char* key) const; + /// Return true if the object has a member named key. + /// \param key may contain embedded nulls. + bool isMember(const String& key) const; + /// Same as isMember(String const& key)const + bool isMember(const char* begin, const char* end) const; + + /// \brief Return a list of the member names. + /// + /// If null, return an empty list. + /// \pre type() is objectValue or nullValue + /// \post if type() was nullValue, it remains nullValue + Members getMemberNames() const; + + /// \deprecated Always pass len. + JSONCPP_DEPRECATED("Use setComment(String const&) instead.") + void setComment(const char* comment, CommentPlacement placement) { + setComment(String(comment, strlen(comment)), placement); + } + /// Comments must be //... or /* ... */ + void setComment(const char* comment, size_t len, CommentPlacement placement) { + setComment(String(comment, len), placement); + } + /// Comments must be //... or /* ... */ + void setComment(String comment, CommentPlacement placement); + bool hasComment(CommentPlacement placement) const; + /// Include delimiters and embedded newlines. + String getComment(CommentPlacement placement) const; + + String toStyledString() const; + + const_iterator begin() const; + const_iterator end() const; + + iterator begin(); + iterator end(); + + // Accessors for the [start, limit) range of bytes within the JSON text from + // which this value was parsed, if any. + void setOffsetStart(ptrdiff_t start); + void setOffsetLimit(ptrdiff_t limit); + ptrdiff_t getOffsetStart() const; + ptrdiff_t getOffsetLimit() const; + +private: + void setType(ValueType v) { + bits_.value_type_ = static_cast(v); + } + bool isAllocated() const { return bits_.allocated_; } + void setIsAllocated(bool v) { bits_.allocated_ = v; } + + void initBasic(ValueType type, bool allocated = false); + void dupPayload(const Value& other); + void releasePayload(); + void dupMeta(const Value& other); + + Value& resolveReference(const char* key); + Value& resolveReference(const char* key, const char* end); + + // struct MemberNamesTransform + //{ + // typedef const char *result_type; + // const char *operator()( const CZString &name ) const + // { + // return name.c_str(); + // } + //}; + + union ValueHolder { + LargestInt int_; + LargestUInt uint_; + double real_; + bool bool_; + char* string_; // if allocated_, ptr to { unsigned, char[] }. + ObjectValues* map_; + } value_; + + struct { + // Really a ValueType, but types should agree for bitfield packing. + unsigned int value_type_ : 8; + // Unless allocated_, string_ must be null-terminated. + unsigned int allocated_ : 1; + } bits_; + + class Comments { + public: + Comments() = default; + Comments(const Comments& that); + Comments(Comments&& that) noexcept; + Comments& operator=(const Comments& that); + Comments& operator=(Comments&& that) noexcept; + bool has(CommentPlacement slot) const; + String get(CommentPlacement slot) const; + void set(CommentPlacement slot, String comment); + + private: + using Array = std::array; + std::unique_ptr ptr_; + }; + Comments comments_; + + // [start, limit) byte offsets in the source JSON text from which this Value + // was extracted. + ptrdiff_t start_; + ptrdiff_t limit_; +}; + +template <> inline bool Value::as() const { return asBool(); } +template <> inline bool Value::is() const { return isBool(); } + +template <> inline Int Value::as() const { return asInt(); } +template <> inline bool Value::is() const { return isInt(); } + +template <> inline UInt Value::as() const { return asUInt(); } +template <> inline bool Value::is() const { return isUInt(); } + +#if defined(JSON_HAS_INT64) +template <> inline Int64 Value::as() const { return asInt64(); } +template <> inline bool Value::is() const { return isInt64(); } + +template <> inline UInt64 Value::as() const { return asUInt64(); } +template <> inline bool Value::is() const { return isUInt64(); } +#endif + +template <> inline double Value::as() const { return asDouble(); } +template <> inline bool Value::is() const { return isDouble(); } + +template <> inline String Value::as() const { return asString(); } +template <> inline bool Value::is() const { return isString(); } + +/// These `as` specializations are type conversions, and do not have a +/// corresponding `is`. +template <> inline float Value::as() const { return asFloat(); } +template <> inline const char* Value::as() const { + return asCString(); +} + +/** \brief Experimental and untested: represents an element of the "path" to + * access a node. + */ +class JSON_API PathArgument { +public: + friend class Path; + + PathArgument(); + PathArgument(ArrayIndex index); + PathArgument(const char* key); + PathArgument(String key); + +private: + enum Kind { kindNone = 0, kindIndex, kindKey }; + String key_; + ArrayIndex index_{}; + Kind kind_{kindNone}; +}; + +/** \brief Experimental and untested: represents a "path" to access a node. + * + * Syntax: + * - "." => root node + * - ".[n]" => elements at index 'n' of root node (an array value) + * - ".name" => member named 'name' of root node (an object value) + * - ".name1.name2.name3" + * - ".[0][1][2].name1[3]" + * - ".%" => member name is provided as parameter + * - ".[%]" => index is provided as parameter + */ +class JSON_API Path { +public: + Path(const String& path, const PathArgument& a1 = PathArgument(), + const PathArgument& a2 = PathArgument(), + const PathArgument& a3 = PathArgument(), + const PathArgument& a4 = PathArgument(), + const PathArgument& a5 = PathArgument()); + + const Value& resolve(const Value& root) const; + Value resolve(const Value& root, const Value& defaultValue) const; + /// Creates the "path" to access the specified node and returns a reference on + /// the node. + Value& make(Value& root) const; + +private: + using InArgs = std::vector; + using Args = std::vector; + + void makePath(const String& path, const InArgs& in); + void addPathInArg(const String& path, const InArgs& in, + InArgs::const_iterator& itInArg, PathArgument::Kind kind); + static void invalidPath(const String& path, int location); + + Args args_; +}; + +/** \brief base class for Value iterators. + * + */ +class JSON_API ValueIteratorBase { +public: + using iterator_category = std::bidirectional_iterator_tag; + using size_t = unsigned int; + using difference_type = int; + using SelfType = ValueIteratorBase; + + bool operator==(const SelfType& other) const { return isEqual(other); } + + bool operator!=(const SelfType& other) const { return !isEqual(other); } + + difference_type operator-(const SelfType& other) const { + return other.computeDistance(*this); + } + + /// Return either the index or the member name of the referenced value as a + /// Value. + Value key() const; + + /// Return the index of the referenced Value, or -1 if it is not an + /// arrayValue. + UInt index() const; + + /// Return the member name of the referenced Value, or "" if it is not an + /// objectValue. + /// \note Avoid `c_str()` on result, as embedded zeroes are possible. + String name() const; + + /// Return the member name of the referenced Value. "" if it is not an + /// objectValue. + /// \deprecated This cannot be used for UTF-8 strings, since there can be + /// embedded nulls. + JSONCPP_DEPRECATED("Use `key = name();` instead.") + char const* memberName() const; + /// Return the member name of the referenced Value, or NULL if it is not an + /// objectValue. + /// \note Better version than memberName(). Allows embedded nulls. + char const* memberName(char const** end) const; + +protected: + /*! Internal utility functions to assist with implementing + * other iterator functions. The const and non-const versions + * of the "deref" protected methods expose the protected + * current_ member variable in a way that can often be + * optimized away by the compiler. + */ + const Value& deref() const; + Value& deref(); + + void increment(); + + void decrement(); + + difference_type computeDistance(const SelfType& other) const; + + bool isEqual(const SelfType& other) const; + + void copy(const SelfType& other); + +private: + Value::ObjectValues::iterator current_; + // Indicates that iterator is for a null value. + bool isNull_{true}; + +public: + // For some reason, BORLAND needs these at the end, rather + // than earlier. No idea why. + ValueIteratorBase(); + explicit ValueIteratorBase(const Value::ObjectValues::iterator& current); +}; + +/** \brief const iterator for object and array value. + * + */ +class JSON_API ValueConstIterator : public ValueIteratorBase { + friend class Value; + +public: + using value_type = const Value; + // typedef unsigned int size_t; + // typedef int difference_type; + using reference = const Value&; + using pointer = const Value*; + using SelfType = ValueConstIterator; + + ValueConstIterator(); + ValueConstIterator(ValueIterator const& other); + +private: + /*! \internal Use by Value to create an iterator. + */ + explicit ValueConstIterator(const Value::ObjectValues::iterator& current); + +public: + SelfType& operator=(const ValueIteratorBase& other); + + SelfType operator++(int) { + SelfType temp(*this); + ++*this; + return temp; + } + + SelfType operator--(int) { + SelfType temp(*this); + --*this; + return temp; + } + + SelfType& operator--() { + decrement(); + return *this; + } + + SelfType& operator++() { + increment(); + return *this; + } + + reference operator*() const { return deref(); } + + pointer operator->() const { return &deref(); } +}; + +/** \brief Iterator for object and array value. + */ +class JSON_API ValueIterator : public ValueIteratorBase { + friend class Value; + +public: + using value_type = Value; + using size_t = unsigned int; + using difference_type = int; + using reference = Value&; + using pointer = Value*; + using SelfType = ValueIterator; + + ValueIterator(); + explicit ValueIterator(const ValueConstIterator& other); + ValueIterator(const ValueIterator& other); + +private: + /*! \internal Use by Value to create an iterator. + */ + explicit ValueIterator(const Value::ObjectValues::iterator& current); + +public: + SelfType& operator=(const SelfType& other); + + SelfType operator++(int) { + SelfType temp(*this); + ++*this; + return temp; + } + + SelfType operator--(int) { + SelfType temp(*this); + --*this; + return temp; + } + + SelfType& operator--() { + decrement(); + return *this; + } + + SelfType& operator++() { + increment(); + return *this; + } + + /*! The return value of non-const iterators can be + * changed, so the these functions are not const + * because the returned references/pointers can be used + * to change state of the base class. + */ + reference operator*() const { return const_cast(deref()); } + pointer operator->() const { return const_cast(&deref()); } +}; + +inline void swap(Value& a, Value& b) { a.swap(b); } + +} // namespace Json + +#pragma pack(pop) + +#if defined(JSONCPP_DISABLE_DLL_INTERFACE_WARNING) +#pragma warning(pop) +#endif // if defined(JSONCPP_DISABLE_DLL_INTERFACE_WARNING) + +#endif // JSON_H_INCLUDED + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: include/json/value.h +// ////////////////////////////////////////////////////////////////////// + + + + + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: include/json/reader.h +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2007-2010 Baptiste Lepilleur and The JsonCpp Authors +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +#ifndef JSON_READER_H_INCLUDED +#define JSON_READER_H_INCLUDED + +#if !defined(JSON_IS_AMALGAMATION) +#include "json_features.h" +#include "value.h" +#endif // if !defined(JSON_IS_AMALGAMATION) +#include +#include +#include +#include +#include + +// Disable warning C4251: : needs to have dll-interface to +// be used by... +#if defined(JSONCPP_DISABLE_DLL_INTERFACE_WARNING) +#pragma warning(push) +#pragma warning(disable : 4251) +#endif // if defined(JSONCPP_DISABLE_DLL_INTERFACE_WARNING) + +#pragma pack(push) +#pragma pack() + +namespace Json { + +/** \brief Unserialize a JSON document into a + * Value. + * + * \deprecated Use CharReader and CharReaderBuilder. + */ + +class JSON_API Reader { +public: + using Char = char; + using Location = const Char*; + + /** \brief An error tagged with where in the JSON text it was encountered. + * + * The offsets give the [start, limit) range of bytes within the text. Note + * that this is bytes, not codepoints. + */ + struct StructuredError { + ptrdiff_t offset_start; + ptrdiff_t offset_limit; + String message; + }; + + /** \brief Constructs a Reader allowing all features for parsing. + * \deprecated Use CharReader and CharReaderBuilder. + */ + Reader(); + + /** \brief Constructs a Reader allowing the specified feature set for parsing. + * \deprecated Use CharReader and CharReaderBuilder. + */ + Reader(const Features& features); + + /** \brief Read a Value from a JSON + * document. + * + * \param document UTF-8 encoded string containing the document + * to read. + * \param[out] root Contains the root value of the document if it + * was successfully parsed. + * \param collectComments \c true to collect comment and allow writing + * them back during serialization, \c false to + * discard comments. This parameter is ignored + * if Features::allowComments_ is \c false. + * \return \c true if the document was successfully parsed, \c false if an + * error occurred. + */ + bool parse(const std::string& document, Value& root, + bool collectComments = true); + + /** \brief Read a Value from a JSON + * document. + * + * \param beginDoc Pointer on the beginning of the UTF-8 encoded + * string of the document to read. + * \param endDoc Pointer on the end of the UTF-8 encoded string + * of the document to read. Must be >= beginDoc. + * \param[out] root Contains the root value of the document if it + * was successfully parsed. + * \param collectComments \c true to collect comment and allow writing + * them back during serialization, \c false to + * discard comments. This parameter is ignored + * if Features::allowComments_ is \c false. + * \return \c true if the document was successfully parsed, \c false if an + * error occurred. + */ + bool parse(const char* beginDoc, const char* endDoc, Value& root, + bool collectComments = true); + + /// \brief Parse from input stream. + /// \see Json::operator>>(std::istream&, Json::Value&). + bool parse(IStream& is, Value& root, bool collectComments = true); + + /** \brief Returns a user friendly string that list errors in the parsed + * document. + * + * \return Formatted error message with the list of errors with their + * location in the parsed document. An empty string is returned if no error + * occurred during parsing. + * \deprecated Use getFormattedErrorMessages() instead (typo fix). + */ + JSONCPP_DEPRECATED("Use getFormattedErrorMessages() instead.") + String getFormatedErrorMessages() const; + + /** \brief Returns a user friendly string that list errors in the parsed + * document. + * + * \return Formatted error message with the list of errors with their + * location in the parsed document. An empty string is returned if no error + * occurred during parsing. + */ + String getFormattedErrorMessages() const; + + /** \brief Returns a vector of structured errors encountered while parsing. + * + * \return A (possibly empty) vector of StructuredError objects. Currently + * only one error can be returned, but the caller should tolerate multiple + * errors. This can occur if the parser recovers from a non-fatal parse + * error and then encounters additional errors. + */ + std::vector getStructuredErrors() const; + + /** \brief Add a semantic error message. + * + * \param value JSON Value location associated with the error + * \param message The error message. + * \return \c true if the error was successfully added, \c false if the Value + * offset exceeds the document size. + */ + bool pushError(const Value& value, const String& message); + + /** \brief Add a semantic error message with extra context. + * + * \param value JSON Value location associated with the error + * \param message The error message. + * \param extra Additional JSON Value location to contextualize the error + * \return \c true if the error was successfully added, \c false if either + * Value offset exceeds the document size. + */ + bool pushError(const Value& value, const String& message, const Value& extra); + + /** \brief Return whether there are any errors. + * + * \return \c true if there are no errors to report \c false if errors have + * occurred. + */ + bool good() const; + +private: + enum TokenType { + tokenEndOfStream = 0, + tokenObjectBegin, + tokenObjectEnd, + tokenArrayBegin, + tokenArrayEnd, + tokenString, + tokenNumber, + tokenTrue, + tokenFalse, + tokenNull, + tokenArraySeparator, + tokenMemberSeparator, + tokenComment, + tokenError + }; + + class Token { + public: + TokenType type_; + Location start_; + Location end_; + }; + + class ErrorInfo { + public: + Token token_; + String message_; + Location extra_; + }; + + using Errors = std::deque; + + bool readToken(Token& token); + void skipSpaces(); + bool match(const Char* pattern, int patternLength); + bool readComment(); + bool readCStyleComment(); + bool readCppStyleComment(); + bool readString(); + void readNumber(); + bool readValue(); + bool readObject(Token& token); + bool readArray(Token& token); + bool decodeNumber(Token& token); + bool decodeNumber(Token& token, Value& decoded); + bool decodeString(Token& token); + bool decodeString(Token& token, String& decoded); + bool decodeDouble(Token& token); + bool decodeDouble(Token& token, Value& decoded); + bool decodeUnicodeCodePoint(Token& token, Location& current, Location end, + unsigned int& unicode); + bool decodeUnicodeEscapeSequence(Token& token, Location& current, + Location end, unsigned int& unicode); + bool addError(const String& message, Token& token, Location extra = nullptr); + bool recoverFromError(TokenType skipUntilToken); + bool addErrorAndRecover(const String& message, Token& token, + TokenType skipUntilToken); + void skipUntilSpace(); + Value& currentValue(); + Char getNextChar(); + void getLocationLineAndColumn(Location location, int& line, + int& column) const; + String getLocationLineAndColumn(Location location) const; + void addComment(Location begin, Location end, CommentPlacement placement); + void skipCommentTokens(Token& token); + + static bool containsNewLine(Location begin, Location end); + static String normalizeEOL(Location begin, Location end); + + using Nodes = std::stack; + Nodes nodes_; + Errors errors_; + String document_; + Location begin_{}; + Location end_{}; + Location current_{}; + Location lastValueEnd_{}; + Value* lastValue_{}; + String commentsBefore_; + Features features_; + bool collectComments_{}; +}; // Reader + +/** Interface for reading JSON from a char array. + */ +class JSON_API CharReader { +public: + virtual ~CharReader() = default; + /** \brief Read a Value from a JSON + * document. The document must be a UTF-8 encoded string containing the + * document to read. + * + * \param beginDoc Pointer on the beginning of the UTF-8 encoded string + * of the document to read. + * \param endDoc Pointer on the end of the UTF-8 encoded string of the + * document to read. Must be >= beginDoc. + * \param[out] root Contains the root value of the document if it was + * successfully parsed. + * \param[out] errs Formatted error messages (if not NULL) a user + * friendly string that lists errors in the parsed + * document. + * \return \c true if the document was successfully parsed, \c false if an + * error occurred. + */ + virtual bool parse(char const* beginDoc, char const* endDoc, Value* root, + String* errs) = 0; + + class JSON_API Factory { + public: + virtual ~Factory() = default; + /** \brief Allocate a CharReader via operator new(). + * \throw std::exception if something goes wrong (e.g. invalid settings) + */ + virtual CharReader* newCharReader() const = 0; + }; // Factory +}; // CharReader + +/** \brief Build a CharReader implementation. + * + * Usage: + * \code + * using namespace Json; + * CharReaderBuilder builder; + * builder["collectComments"] = false; + * Value value; + * String errs; + * bool ok = parseFromStream(builder, std::cin, &value, &errs); + * \endcode + */ +class JSON_API CharReaderBuilder : public CharReader::Factory { +public: + // Note: We use a Json::Value so that we can add data-members to this class + // without a major version bump. + /** Configuration of this builder. + * These are case-sensitive. + * Available settings (case-sensitive): + * - `"collectComments": false or true` + * - true to collect comment and allow writing them back during + * serialization, false to discard comments. This parameter is ignored + * if allowComments is false. + * - `"allowComments": false or true` + * - true if comments are allowed. + * - `"allowTrailingCommas": false or true` + * - true if trailing commas in objects and arrays are allowed. + * - `"strictRoot": false or true` + * - true if root must be either an array or an object value + * - `"allowDroppedNullPlaceholders": false or true` + * - true if dropped null placeholders are allowed. (See + * StreamWriterBuilder.) + * - `"allowNumericKeys": false or true` + * - true if numeric object keys are allowed. + * - `"allowSingleQuotes": false or true` + * - true if '' are allowed for strings (both keys and values) + * - `"stackLimit": integer` + * - Exceeding stackLimit (recursive depth of `readValue()`) will cause an + * exception. + * - This is a security issue (seg-faults caused by deeply nested JSON), so + * the default is low. + * - `"failIfExtra": false or true` + * - If true, `parse()` returns false when extra non-whitespace trails the + * JSON value in the input string. + * - `"rejectDupKeys": false or true` + * - If true, `parse()` returns false when a key is duplicated within an + * object. + * - `"allowSpecialFloats": false or true` + * - If true, special float values (NaNs and infinities) are allowed and + * their values are lossfree restorable. + * - `"skipBom": false or true` + * - If true, if the input starts with the Unicode byte order mark (BOM), + * it is skipped. + * + * You can examine 'settings_` yourself to see the defaults. You can also + * write and read them just like any JSON Value. + * \sa setDefaults() + */ + Json::Value settings_; + + CharReaderBuilder(); + ~CharReaderBuilder() override; + + CharReader* newCharReader() const override; + + /** \return true if 'settings' are legal and consistent; + * otherwise, indicate bad settings via 'invalid'. + */ + bool validate(Json::Value* invalid) const; + + /** A simple way to update a specific setting. + */ + Value& operator[](const String& key); + + /** Called by ctor, but you can use this to reset settings_. + * \pre 'settings' != NULL (but Json::null is fine) + * \remark Defaults: + * \snippet src/lib_json/json_reader.cpp CharReaderBuilderDefaults + */ + static void setDefaults(Json::Value* settings); + /** Same as old Features::strictMode(). + * \pre 'settings' != NULL (but Json::null is fine) + * \remark Defaults: + * \snippet src/lib_json/json_reader.cpp CharReaderBuilderStrictMode + */ + static void strictMode(Json::Value* settings); +}; + +/** Consume entire stream and use its begin/end. + * Someday we might have a real StreamReader, but for now this + * is convenient. + */ +bool JSON_API parseFromStream(CharReader::Factory const&, IStream&, Value* root, + String* errs); + +/** \brief Read from 'sin' into 'root'. + * + * Always keep comments from the input JSON. + * + * This can be used to read a file into a particular sub-object. + * For example: + * \code + * Json::Value root; + * cin >> root["dir"]["file"]; + * cout << root; + * \endcode + * Result: + * \verbatim + * { + * "dir": { + * "file": { + * // The input stream JSON would be nested here. + * } + * } + * } + * \endverbatim + * \throw std::exception on parse error. + * \see Json::operator<<() + */ +JSON_API IStream& operator>>(IStream&, Value&); + +} // namespace Json + +#pragma pack(pop) + +#if defined(JSONCPP_DISABLE_DLL_INTERFACE_WARNING) +#pragma warning(pop) +#endif // if defined(JSONCPP_DISABLE_DLL_INTERFACE_WARNING) + +#endif // JSON_READER_H_INCLUDED + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: include/json/reader.h +// ////////////////////////////////////////////////////////////////////// + + + + + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: include/json/writer.h +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2007-2010 Baptiste Lepilleur and The JsonCpp Authors +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +#ifndef JSON_WRITER_H_INCLUDED +#define JSON_WRITER_H_INCLUDED + +#if !defined(JSON_IS_AMALGAMATION) +#include "value.h" +#endif // if !defined(JSON_IS_AMALGAMATION) +#include +#include +#include + +// Disable warning C4251: : needs to have dll-interface to +// be used by... +#if defined(JSONCPP_DISABLE_DLL_INTERFACE_WARNING) && defined(_MSC_VER) +#pragma warning(push) +#pragma warning(disable : 4251) +#endif // if defined(JSONCPP_DISABLE_DLL_INTERFACE_WARNING) + +#pragma pack(push) +#pragma pack() + +namespace Json { + +class Value; + +/** + * + * Usage: + * \code + * using namespace Json; + * void writeToStdout(StreamWriter::Factory const& factory, Value const& value) + * { std::unique_ptr const writer( factory.newStreamWriter()); + * writer->write(value, &std::cout); + * std::cout << std::endl; // add lf and flush + * } + * \endcode + */ +class JSON_API StreamWriter { +protected: + OStream* sout_; // not owned; will not delete +public: + StreamWriter(); + virtual ~StreamWriter(); + /** Write Value into document as configured in sub-class. + * Do not take ownership of sout, but maintain a reference during function. + * \pre sout != NULL + * \return zero on success (For now, we always return zero, so check the + * stream instead.) \throw std::exception possibly, depending on + * configuration + */ + virtual int write(Value const& root, OStream* sout) = 0; + + /** \brief A simple abstract factory. + */ + class JSON_API Factory { + public: + virtual ~Factory(); + /** \brief Allocate a CharReader via operator new(). + * \throw std::exception if something goes wrong (e.g. invalid settings) + */ + virtual StreamWriter* newStreamWriter() const = 0; + }; // Factory +}; // StreamWriter + +/** \brief Write into stringstream, then return string, for convenience. + * A StreamWriter will be created from the factory, used, and then deleted. + */ +String JSON_API writeString(StreamWriter::Factory const& factory, + Value const& root); + +/** \brief Build a StreamWriter implementation. + +* Usage: +* \code +* using namespace Json; +* Value value = ...; +* StreamWriterBuilder builder; +* builder["commentStyle"] = "None"; +* builder["indentation"] = " "; // or whatever you like +* std::unique_ptr writer( +* builder.newStreamWriter()); +* writer->write(value, &std::cout); +* std::cout << std::endl; // add lf and flush +* \endcode +*/ +class JSON_API StreamWriterBuilder : public StreamWriter::Factory { +public: + // Note: We use a Json::Value so that we can add data-members to this class + // without a major version bump. + /** Configuration of this builder. + * Available settings (case-sensitive): + * - "commentStyle": "None" or "All" + * - "indentation": "". + * - Setting this to an empty string also omits newline characters. + * - "enableYAMLCompatibility": false or true + * - slightly change the whitespace around colons + * - "dropNullPlaceholders": false or true + * - Drop the "null" string from the writer's output for nullValues. + * Strictly speaking, this is not valid JSON. But when the output is being + * fed to a browser's JavaScript, it makes for smaller output and the + * browser can handle the output just fine. + * - "useSpecialFloats": false or true + * - If true, outputs non-finite floating point values in the following way: + * NaN values as "NaN", positive infinity as "Infinity", and negative + * infinity as "-Infinity". + * - "precision": int + * - Number of precision digits for formatting of real values. + * - "precisionType": "significant"(default) or "decimal" + * - Type of precision for formatting of real values. + * - "emitUTF8": false or true + * - If true, outputs raw UTF8 strings instead of escaping them. + + * You can examine 'settings_` yourself + * to see the defaults. You can also write and read them just like any + * JSON Value. + * \sa setDefaults() + */ + Json::Value settings_; + + StreamWriterBuilder(); + ~StreamWriterBuilder() override; + + /** + * \throw std::exception if something goes wrong (e.g. invalid settings) + */ + StreamWriter* newStreamWriter() const override; + + /** \return true if 'settings' are legal and consistent; + * otherwise, indicate bad settings via 'invalid'. + */ + bool validate(Json::Value* invalid) const; + /** A simple way to update a specific setting. + */ + Value& operator[](const String& key); + + /** Called by ctor, but you can use this to reset settings_. + * \pre 'settings' != NULL (but Json::null is fine) + * \remark Defaults: + * \snippet src/lib_json/json_writer.cpp StreamWriterBuilderDefaults + */ + static void setDefaults(Json::Value* settings); +}; + +/** \brief Abstract class for writers. + * \deprecated Use StreamWriter. (And really, this is an implementation detail.) + */ +class JSON_API Writer { +public: + virtual ~Writer(); + + virtual String write(const Value& root) = 0; +}; + +/** \brief Outputs a Value in JSON format + *without formatting (not human friendly). + * + * The JSON document is written in a single line. It is not intended for 'human' + *consumption, + * but may be useful to support feature such as RPC where bandwidth is limited. + * \sa Reader, Value + * \deprecated Use StreamWriterBuilder. + */ +#if defined(_MSC_VER) +#pragma warning(push) +#pragma warning(disable : 4996) // Deriving from deprecated class +#endif +class JSON_API FastWriter + : public Writer { +public: + FastWriter(); + ~FastWriter() override = default; + + void enableYAMLCompatibility(); + + /** \brief Drop the "null" string from the writer's output for nullValues. + * Strictly speaking, this is not valid JSON. But when the output is being + * fed to a browser's JavaScript, it makes for smaller output and the + * browser can handle the output just fine. + */ + void dropNullPlaceholders(); + + void omitEndingLineFeed(); + +public: // overridden from Writer + String write(const Value& root) override; + +private: + void writeValue(const Value& value); + + String document_; + bool yamlCompatibilityEnabled_{false}; + bool dropNullPlaceholders_{false}; + bool omitEndingLineFeed_{false}; +}; +#if defined(_MSC_VER) +#pragma warning(pop) +#endif + +/** \brief Writes a Value in JSON format in a + *human friendly way. + * + * The rules for line break and indent are as follow: + * - Object value: + * - if empty then print {} without indent and line break + * - if not empty the print '{', line break & indent, print one value per + *line + * and then unindent and line break and print '}'. + * - Array value: + * - if empty then print [] without indent and line break + * - if the array contains no object value, empty array or some other value + *types, + * and all the values fit on one lines, then print the array on a single + *line. + * - otherwise, it the values do not fit on one line, or the array contains + * object or non empty array, then print one value per line. + * + * If the Value have comments then they are outputted according to their + *#CommentPlacement. + * + * \sa Reader, Value, Value::setComment() + * \deprecated Use StreamWriterBuilder. + */ +#if defined(_MSC_VER) +#pragma warning(push) +#pragma warning(disable : 4996) // Deriving from deprecated class +#endif +class JSON_API + StyledWriter : public Writer { +public: + StyledWriter(); + ~StyledWriter() override = default; + +public: // overridden from Writer + /** \brief Serialize a Value in JSON format. + * \param root Value to serialize. + * \return String containing the JSON document that represents the root value. + */ + String write(const Value& root) override; + +private: + void writeValue(const Value& value); + void writeArrayValue(const Value& value); + bool isMultilineArray(const Value& value); + void pushValue(const String& value); + void writeIndent(); + void writeWithIndent(const String& value); + void indent(); + void unindent(); + void writeCommentBeforeValue(const Value& root); + void writeCommentAfterValueOnSameLine(const Value& root); + static bool hasCommentForValue(const Value& value); + static String normalizeEOL(const String& text); + + using ChildValues = std::vector; + + ChildValues childValues_; + String document_; + String indentString_; + unsigned int rightMargin_{74}; + unsigned int indentSize_{3}; + bool addChildValues_{false}; +}; +#if defined(_MSC_VER) +#pragma warning(pop) +#endif + +/** \brief Writes a Value in JSON format in a + human friendly way, + to a stream rather than to a string. + * + * The rules for line break and indent are as follow: + * - Object value: + * - if empty then print {} without indent and line break + * - if not empty the print '{', line break & indent, print one value per + line + * and then unindent and line break and print '}'. + * - Array value: + * - if empty then print [] without indent and line break + * - if the array contains no object value, empty array or some other value + types, + * and all the values fit on one lines, then print the array on a single + line. + * - otherwise, it the values do not fit on one line, or the array contains + * object or non empty array, then print one value per line. + * + * If the Value have comments then they are outputted according to their + #CommentPlacement. + * + * \sa Reader, Value, Value::setComment() + * \deprecated Use StreamWriterBuilder. + */ +#if defined(_MSC_VER) +#pragma warning(push) +#pragma warning(disable : 4996) // Deriving from deprecated class +#endif +class JSON_API + StyledStreamWriter { +public: + /** + * \param indentation Each level will be indented by this amount extra. + */ + StyledStreamWriter(String indentation = "\t"); + ~StyledStreamWriter() = default; + +public: + /** \brief Serialize a Value in JSON format. + * \param out Stream to write to. (Can be ostringstream, e.g.) + * \param root Value to serialize. + * \note There is no point in deriving from Writer, since write() should not + * return a value. + */ + void write(OStream& out, const Value& root); + +private: + void writeValue(const Value& value); + void writeArrayValue(const Value& value); + bool isMultilineArray(const Value& value); + void pushValue(const String& value); + void writeIndent(); + void writeWithIndent(const String& value); + void indent(); + void unindent(); + void writeCommentBeforeValue(const Value& root); + void writeCommentAfterValueOnSameLine(const Value& root); + static bool hasCommentForValue(const Value& value); + static String normalizeEOL(const String& text); + + using ChildValues = std::vector; + + ChildValues childValues_; + OStream* document_; + String indentString_; + unsigned int rightMargin_{74}; + String indentation_; + bool addChildValues_ : 1; + bool indented_ : 1; +}; +#if defined(_MSC_VER) +#pragma warning(pop) +#endif + +#if defined(JSON_HAS_INT64) +String JSON_API valueToString(Int value); +String JSON_API valueToString(UInt value); +#endif // if defined(JSON_HAS_INT64) +String JSON_API valueToString(LargestInt value); +String JSON_API valueToString(LargestUInt value); +String JSON_API valueToString( + double value, unsigned int precision = Value::defaultRealPrecision, + PrecisionType precisionType = PrecisionType::significantDigits); +String JSON_API valueToString(bool value); +String JSON_API valueToQuotedString(const char* value); + +/// \brief Output using the StyledStreamWriter. +/// \see Json::operator>>() +JSON_API OStream& operator<<(OStream&, const Value& root); + +} // namespace Json + +#pragma pack(pop) + +#if defined(JSONCPP_DISABLE_DLL_INTERFACE_WARNING) +#pragma warning(pop) +#endif // if defined(JSONCPP_DISABLE_DLL_INTERFACE_WARNING) + +#endif // JSON_WRITER_H_INCLUDED + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: include/json/writer.h +// ////////////////////////////////////////////////////////////////////// + + + + + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: include/json/assertions.h +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2007-2010 Baptiste Lepilleur and The JsonCpp Authors +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +#ifndef JSON_ASSERTIONS_H_INCLUDED +#define JSON_ASSERTIONS_H_INCLUDED + +#include +#include + +#if !defined(JSON_IS_AMALGAMATION) +#include "config.h" +#endif // if !defined(JSON_IS_AMALGAMATION) + +/** It should not be possible for a maliciously designed file to + * cause an abort() or seg-fault, so these macros are used only + * for pre-condition violations and internal logic errors. + */ +#if JSON_USE_EXCEPTION + +// @todo <= add detail about condition in exception +#define JSON_ASSERT(condition) \ + do { \ + if (!(condition)) { \ + Json::throwLogicError("assert json failed"); \ + } \ + } while (0) + +#define JSON_FAIL_MESSAGE(message) \ + do { \ + OStringStream oss; \ + oss << message; \ + Json::throwLogicError(oss.str()); \ + abort(); \ + } while (0) + +#else // JSON_USE_EXCEPTION + +#define JSON_ASSERT(condition) assert(condition) + +// The call to assert() will show the failure message in debug builds. In +// release builds we abort, for a core-dump or debugger. +#define JSON_FAIL_MESSAGE(message) \ + { \ + OStringStream oss; \ + oss << message; \ + assert(false && oss.str().c_str()); \ + abort(); \ + } + +#endif + +#define JSON_ASSERT_MESSAGE(condition, message) \ + do { \ + if (!(condition)) { \ + JSON_FAIL_MESSAGE(message); \ + } \ + } while (0) + +#endif // JSON_ASSERTIONS_H_INCLUDED + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: include/json/assertions.h +// ////////////////////////////////////////////////////////////////////// + + + + + +#endif //ifndef JSON_AMALGAMATED_H_INCLUDED diff --git a/src/route/include/route/md5.h b/src/route/include/route/md5.h new file mode 100644 index 0000000..df10a8c --- /dev/null +++ b/src/route/include/route/md5.h @@ -0,0 +1,56 @@ +#ifndef MD5_H +#define MD5_H + +#include +#include + +/* Type define */ +typedef unsigned char byte; +typedef unsigned int uint32; + +using std::ifstream; +using std::string; + +/* MD5 declaration. */ +class MD5 +{ +public: + MD5(); + MD5(const void *input, size_t length); + MD5(const string &str); + MD5(ifstream &in); + void update(const void *input, size_t length); + void update(const string &str); + void update(ifstream &in); + const byte *digest(); + string toString(); + void reset(); + +private: + void update(const byte *input, size_t length); + void final(); + void transform(const byte block[64]); + void encode(const uint32 *input, byte *output, size_t length); + void decode(const byte *input, uint32 *output, size_t length); + string bytesToHexString(const byte *input, size_t length); + + /* class uncopyable */ + MD5(const MD5 &); + MD5 &operator=(const MD5 &); + +private: + uint32 _state[4]; /* state (ABCD) */ + uint32 _count[2]; /* number of bits, modulo 2^64 (low-order word first) */ + byte _buffer[64]; /* input buffer */ + byte _digest[16]; /* message digest */ + bool _finished; /* calculate finished ? */ + + static const byte PADDING[64]; /* padding for calculate */ + static const char HEX[16]; + enum + { + BUFFER_SIZE = 1024 + }; +}; + +#endif /*MD5_H*/ \ No newline at end of file diff --git a/src/route/package.xml b/src/route/package.xml new file mode 100644 index 0000000..430bac2 --- /dev/null +++ b/src/route/package.xml @@ -0,0 +1,22 @@ + + + + route + 0.0.0 + TODO: Package description + zxwl + TODO: License declaration + + ament_cmake + + rclcpp + std_msgs + sweeper_interfaces + + ament_lint_auto + ament_lint_common + + + ament_cmake + + \ No newline at end of file diff --git a/src/route/src/jsoncpp.cpp b/src/route/src/jsoncpp.cpp new file mode 100644 index 0000000..89b7bc0 --- /dev/null +++ b/src/route/src/jsoncpp.cpp @@ -0,0 +1,5342 @@ +/// Json-cpp amalgamated source (http://jsoncpp.sourceforge.net/). +/// It is intended to be used with #include "json/json.h" + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: LICENSE +// ////////////////////////////////////////////////////////////////////// + +/* +The JsonCpp library's source code, including accompanying documentation, +tests and demonstration applications, are licensed under the following +conditions... + +Baptiste Lepilleur and The JsonCpp Authors explicitly disclaim copyright in all +jurisdictions which recognize such a disclaimer. In such jurisdictions, +this software is released into the Public Domain. + +In jurisdictions which do not recognize Public Domain property (e.g. Germany as of +2010), this software is Copyright (c) 2007-2010 by Baptiste Lepilleur and +The JsonCpp Authors, and is released under the terms of the MIT License (see below). + +In jurisdictions which recognize Public Domain property, the user of this +software may choose to accept it either as 1) Public Domain, 2) under the +conditions of the MIT License (see below), or 3) under the terms of dual +Public Domain/MIT License conditions described here, as they choose. + +The MIT License is about as close to Public Domain as a license can get, and is +described in clear, concise terms at: + + http://en.wikipedia.org/wiki/MIT_License + +The full text of the MIT License follows: + +======================================================================== +Copyright (c) 2007-2010 Baptiste Lepilleur and The JsonCpp Authors + +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. +======================================================================== +(END LICENSE TEXT) + +The MIT license is compatible with both the GPL and commercial +software, affording one all of the rights of Public Domain with the +minor nuisance of being required to keep the above copyright notice +and license text in the source code. Note also that by accepting the +Public Domain "license" you can re-license your copy using whatever +license you like. + +*/ + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: LICENSE +// ////////////////////////////////////////////////////////////////////// + + + + + + +#include "json.h" + +#ifndef JSON_IS_AMALGAMATION +#error "Compile with -I PATH_TO_JSON_DIRECTORY" +#endif + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: src/lib_json/json_tool.h +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2007-2010 Baptiste Lepilleur and The JsonCpp Authors +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +#ifndef LIB_JSONCPP_JSON_TOOL_H_INCLUDED +#define LIB_JSONCPP_JSON_TOOL_H_INCLUDED + +#if !defined(JSON_IS_AMALGAMATION) +#include +#endif + +// Also support old flag NO_LOCALE_SUPPORT +#ifdef NO_LOCALE_SUPPORT +#define JSONCPP_NO_LOCALE_SUPPORT +#endif + +#ifndef JSONCPP_NO_LOCALE_SUPPORT +#include +#endif + +/* This header provides common string manipulation support, such as UTF-8, + * portable conversion from/to string... + * + * It is an internal header that must not be exposed. + */ + +namespace Json { +static inline char getDecimalPoint() { +#ifdef JSONCPP_NO_LOCALE_SUPPORT + return '\0'; +#else + struct lconv* lc = localeconv(); + return lc ? *(lc->decimal_point) : '\0'; +#endif +} + +/// Converts a unicode code-point to UTF-8. +static inline String codePointToUTF8(unsigned int cp) { + String result; + + // based on description from http://en.wikipedia.org/wiki/UTF-8 + + if (cp <= 0x7f) { + result.resize(1); + result[0] = static_cast(cp); + } else if (cp <= 0x7FF) { + result.resize(2); + result[1] = static_cast(0x80 | (0x3f & cp)); + result[0] = static_cast(0xC0 | (0x1f & (cp >> 6))); + } else if (cp <= 0xFFFF) { + result.resize(3); + result[2] = static_cast(0x80 | (0x3f & cp)); + result[1] = static_cast(0x80 | (0x3f & (cp >> 6))); + result[0] = static_cast(0xE0 | (0xf & (cp >> 12))); + } else if (cp <= 0x10FFFF) { + result.resize(4); + result[3] = static_cast(0x80 | (0x3f & cp)); + result[2] = static_cast(0x80 | (0x3f & (cp >> 6))); + result[1] = static_cast(0x80 | (0x3f & (cp >> 12))); + result[0] = static_cast(0xF0 | (0x7 & (cp >> 18))); + } + + return result; +} + +enum { + /// Constant that specify the size of the buffer that must be passed to + /// uintToString. + uintToStringBufferSize = 3 * sizeof(LargestUInt) + 1 +}; + +// Defines a char buffer for use with uintToString(). +using UIntToStringBuffer = char[uintToStringBufferSize]; + +/** Converts an unsigned integer to string. + * @param value Unsigned integer to convert to string + * @param current Input/Output string buffer. + * Must have at least uintToStringBufferSize chars free. + */ +static inline void uintToString(LargestUInt value, char*& current) { + *--current = 0; + do { + *--current = static_cast(value % 10U + static_cast('0')); + value /= 10; + } while (value != 0); +} + +/** Change ',' to '.' everywhere in buffer. + * + * We had a sophisticated way, but it did not work in WinCE. + * @see https://github.com/open-source-parsers/jsoncpp/pull/9 + */ +template Iter fixNumericLocale(Iter begin, Iter end) { + for (; begin != end; ++begin) { + if (*begin == ',') { + *begin = '.'; + } + } + return begin; +} + +template void fixNumericLocaleInput(Iter begin, Iter end) { + char decimalPoint = getDecimalPoint(); + if (decimalPoint == '\0' || decimalPoint == '.') { + return; + } + for (; begin != end; ++begin) { + if (*begin == '.') { + *begin = decimalPoint; + } + } +} + +/** + * Return iterator that would be the new end of the range [begin,end), if we + * were to delete zeros in the end of string, but not the last zero before '.'. + */ +template +Iter fixZerosInTheEnd(Iter begin, Iter end, unsigned int precision) { + for (; begin != end; --end) { + if (*(end - 1) != '0') { + return end; + } + // Don't delete the last zero before the decimal point. + if (begin != (end - 1) && begin != (end - 2) && *(end - 2) == '.') { + if (precision) { + return end; + } + return end - 2; + } + } + return end; +} + +} // namespace Json + +#endif // LIB_JSONCPP_JSON_TOOL_H_INCLUDED + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: src/lib_json/json_tool.h +// ////////////////////////////////////////////////////////////////////// + + + + + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: src/lib_json/json_reader.cpp +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2007-2011 Baptiste Lepilleur and The JsonCpp Authors +// Copyright (C) 2016 InfoTeCS JSC. All rights reserved. +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +#if !defined(JSON_IS_AMALGAMATION) +#include "json_tool.h" +#include +#include +#include +#endif // if !defined(JSON_IS_AMALGAMATION) +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#if __cplusplus >= 201103L + +#if !defined(sscanf) +#define sscanf std::sscanf +#endif + +#endif //__cplusplus + +#if defined(_MSC_VER) +#if !defined(_CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES) +#define _CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES 1 +#endif //_CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES +#endif //_MSC_VER + +#if defined(_MSC_VER) +// Disable warning about strdup being deprecated. +#pragma warning(disable : 4996) +#endif + +// Define JSONCPP_DEPRECATED_STACK_LIMIT as an appropriate integer at compile +// time to change the stack limit +#if !defined(JSONCPP_DEPRECATED_STACK_LIMIT) +#define JSONCPP_DEPRECATED_STACK_LIMIT 1000 +#endif + +static size_t const stackLimit_g = + JSONCPP_DEPRECATED_STACK_LIMIT; // see readValue() + +namespace Json { + +#if __cplusplus >= 201103L || (defined(_CPPLIB_VER) && _CPPLIB_VER >= 520) +using CharReaderPtr = std::unique_ptr; +#else +using CharReaderPtr = std::auto_ptr; +#endif + +// Implementation of class Features +// //////////////////////////////// + +Features::Features() = default; + +Features Features::all() { return {}; } + +Features Features::strictMode() { + Features features; + features.allowComments_ = false; + features.strictRoot_ = true; + features.allowDroppedNullPlaceholders_ = false; + features.allowNumericKeys_ = false; + return features; +} + +// Implementation of class Reader +// //////////////////////////////// + +bool Reader::containsNewLine(Reader::Location begin, Reader::Location end) { + return std::any_of(begin, end, [](char b) { return b == '\n' || b == '\r'; }); +} + +// Class Reader +// ////////////////////////////////////////////////////////////////// + +Reader::Reader() : features_(Features::all()) {} + +Reader::Reader(const Features& features) : features_(features) {} + +bool Reader::parse(const std::string& document, Value& root, + bool collectComments) { + document_.assign(document.begin(), document.end()); + const char* begin = document_.c_str(); + const char* end = begin + document_.length(); + return parse(begin, end, root, collectComments); +} + +bool Reader::parse(std::istream& is, Value& root, bool collectComments) { + // std::istream_iterator begin(is); + // std::istream_iterator end; + // Those would allow streamed input from a file, if parse() were a + // template function. + + // Since String is reference-counted, this at least does not + // create an extra copy. + String doc(std::istreambuf_iterator(is), {}); + return parse(doc.data(), doc.data() + doc.size(), root, collectComments); +} + +bool Reader::parse(const char* beginDoc, const char* endDoc, Value& root, + bool collectComments) { + if (!features_.allowComments_) { + collectComments = false; + } + + begin_ = beginDoc; + end_ = endDoc; + collectComments_ = collectComments; + current_ = begin_; + lastValueEnd_ = nullptr; + lastValue_ = nullptr; + commentsBefore_.clear(); + errors_.clear(); + while (!nodes_.empty()) + nodes_.pop(); + nodes_.push(&root); + + bool successful = readValue(); + Token token; + skipCommentTokens(token); + if (collectComments_ && !commentsBefore_.empty()) + root.setComment(commentsBefore_, commentAfter); + if (features_.strictRoot_) { + if (!root.isArray() && !root.isObject()) { + // Set error location to start of doc, ideally should be first token found + // in doc + token.type_ = tokenError; + token.start_ = beginDoc; + token.end_ = endDoc; + addError( + "A valid JSON document must be either an array or an object value.", + token); + return false; + } + } + return successful; +} + +bool Reader::readValue() { + // readValue() may call itself only if it calls readObject() or ReadArray(). + // These methods execute nodes_.push() just before and nodes_.pop)() just + // after calling readValue(). parse() executes one nodes_.push(), so > instead + // of >=. + if (nodes_.size() > stackLimit_g) + throwRuntimeError("Exceeded stackLimit in readValue()."); + + Token token; + skipCommentTokens(token); + bool successful = true; + + if (collectComments_ && !commentsBefore_.empty()) { + currentValue().setComment(commentsBefore_, commentBefore); + commentsBefore_.clear(); + } + + switch (token.type_) { + case tokenObjectBegin: + successful = readObject(token); + currentValue().setOffsetLimit(current_ - begin_); + break; + case tokenArrayBegin: + successful = readArray(token); + currentValue().setOffsetLimit(current_ - begin_); + break; + case tokenNumber: + successful = decodeNumber(token); + break; + case tokenString: + successful = decodeString(token); + break; + case tokenTrue: { + Value v(true); + currentValue().swapPayload(v); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + } break; + case tokenFalse: { + Value v(false); + currentValue().swapPayload(v); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + } break; + case tokenNull: { + Value v; + currentValue().swapPayload(v); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + } break; + case tokenArraySeparator: + case tokenObjectEnd: + case tokenArrayEnd: + if (features_.allowDroppedNullPlaceholders_) { + // "Un-read" the current token and mark the current value as a null + // token. + current_--; + Value v; + currentValue().swapPayload(v); + currentValue().setOffsetStart(current_ - begin_ - 1); + currentValue().setOffsetLimit(current_ - begin_); + break; + } // Else, fall through... + default: + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + return addError("Syntax error: value, object or array expected.", token); + } + + if (collectComments_) { + lastValueEnd_ = current_; + lastValue_ = ¤tValue(); + } + + return successful; +} + +void Reader::skipCommentTokens(Token& token) { + if (features_.allowComments_) { + do { + readToken(token); + } while (token.type_ == tokenComment); + } else { + readToken(token); + } +} + +bool Reader::readToken(Token& token) { + skipSpaces(); + token.start_ = current_; + Char c = getNextChar(); + bool ok = true; + switch (c) { + case '{': + token.type_ = tokenObjectBegin; + break; + case '}': + token.type_ = tokenObjectEnd; + break; + case '[': + token.type_ = tokenArrayBegin; + break; + case ']': + token.type_ = tokenArrayEnd; + break; + case '"': + token.type_ = tokenString; + ok = readString(); + break; + case '/': + token.type_ = tokenComment; + ok = readComment(); + break; + case '0': + case '1': + case '2': + case '3': + case '4': + case '5': + case '6': + case '7': + case '8': + case '9': + case '-': + token.type_ = tokenNumber; + readNumber(); + break; + case 't': + token.type_ = tokenTrue; + ok = match("rue", 3); + break; + case 'f': + token.type_ = tokenFalse; + ok = match("alse", 4); + break; + case 'n': + token.type_ = tokenNull; + ok = match("ull", 3); + break; + case ',': + token.type_ = tokenArraySeparator; + break; + case ':': + token.type_ = tokenMemberSeparator; + break; + case 0: + token.type_ = tokenEndOfStream; + break; + default: + ok = false; + break; + } + if (!ok) + token.type_ = tokenError; + token.end_ = current_; + return ok; +} + +void Reader::skipSpaces() { + while (current_ != end_) { + Char c = *current_; + if (c == ' ' || c == '\t' || c == '\r' || c == '\n') + ++current_; + else + break; + } +} + +bool Reader::match(const Char* pattern, int patternLength) { + if (end_ - current_ < patternLength) + return false; + int index = patternLength; + while (index--) + if (current_[index] != pattern[index]) + return false; + current_ += patternLength; + return true; +} + +bool Reader::readComment() { + Location commentBegin = current_ - 1; + Char c = getNextChar(); + bool successful = false; + if (c == '*') + successful = readCStyleComment(); + else if (c == '/') + successful = readCppStyleComment(); + if (!successful) + return false; + + if (collectComments_) { + CommentPlacement placement = commentBefore; + if (lastValueEnd_ && !containsNewLine(lastValueEnd_, commentBegin)) { + if (c != '*' || !containsNewLine(commentBegin, current_)) + placement = commentAfterOnSameLine; + } + + addComment(commentBegin, current_, placement); + } + return true; +} + +String Reader::normalizeEOL(Reader::Location begin, Reader::Location end) { + String normalized; + normalized.reserve(static_cast(end - begin)); + Reader::Location current = begin; + while (current != end) { + char c = *current++; + if (c == '\r') { + if (current != end && *current == '\n') + // convert dos EOL + ++current; + // convert Mac EOL + normalized += '\n'; + } else { + normalized += c; + } + } + return normalized; +} + +void Reader::addComment(Location begin, Location end, + CommentPlacement placement) { + assert(collectComments_); + const String& normalized = normalizeEOL(begin, end); + if (placement == commentAfterOnSameLine) { + assert(lastValue_ != nullptr); + lastValue_->setComment(normalized, placement); + } else { + commentsBefore_ += normalized; + } +} + +bool Reader::readCStyleComment() { + while ((current_ + 1) < end_) { + Char c = getNextChar(); + if (c == '*' && *current_ == '/') + break; + } + return getNextChar() == '/'; +} + +bool Reader::readCppStyleComment() { + while (current_ != end_) { + Char c = getNextChar(); + if (c == '\n') + break; + if (c == '\r') { + // Consume DOS EOL. It will be normalized in addComment. + if (current_ != end_ && *current_ == '\n') + getNextChar(); + // Break on Moc OS 9 EOL. + break; + } + } + return true; +} + +void Reader::readNumber() { + Location p = current_; + char c = '0'; // stopgap for already consumed character + // integral part + while (c >= '0' && c <= '9') + c = (current_ = p) < end_ ? *p++ : '\0'; + // fractional part + if (c == '.') { + c = (current_ = p) < end_ ? *p++ : '\0'; + while (c >= '0' && c <= '9') + c = (current_ = p) < end_ ? *p++ : '\0'; + } + // exponential part + if (c == 'e' || c == 'E') { + c = (current_ = p) < end_ ? *p++ : '\0'; + if (c == '+' || c == '-') + c = (current_ = p) < end_ ? *p++ : '\0'; + while (c >= '0' && c <= '9') + c = (current_ = p) < end_ ? *p++ : '\0'; + } +} + +bool Reader::readString() { + Char c = '\0'; + while (current_ != end_) { + c = getNextChar(); + if (c == '\\') + getNextChar(); + else if (c == '"') + break; + } + return c == '"'; +} + +bool Reader::readObject(Token& token) { + Token tokenName; + String name; + Value init(objectValue); + currentValue().swapPayload(init); + currentValue().setOffsetStart(token.start_ - begin_); + while (readToken(tokenName)) { + bool initialTokenOk = true; + while (tokenName.type_ == tokenComment && initialTokenOk) + initialTokenOk = readToken(tokenName); + if (!initialTokenOk) + break; + if (tokenName.type_ == tokenObjectEnd && name.empty()) // empty object + return true; + name.clear(); + if (tokenName.type_ == tokenString) { + if (!decodeString(tokenName, name)) + return recoverFromError(tokenObjectEnd); + } else if (tokenName.type_ == tokenNumber && features_.allowNumericKeys_) { + Value numberName; + if (!decodeNumber(tokenName, numberName)) + return recoverFromError(tokenObjectEnd); + name = numberName.asString(); + } else { + break; + } + + Token colon; + if (!readToken(colon) || colon.type_ != tokenMemberSeparator) { + return addErrorAndRecover("Missing ':' after object member name", colon, + tokenObjectEnd); + } + Value& value = currentValue()[name]; + nodes_.push(&value); + bool ok = readValue(); + nodes_.pop(); + if (!ok) // error already set + return recoverFromError(tokenObjectEnd); + + Token comma; + if (!readToken(comma) || + (comma.type_ != tokenObjectEnd && comma.type_ != tokenArraySeparator && + comma.type_ != tokenComment)) { + return addErrorAndRecover("Missing ',' or '}' in object declaration", + comma, tokenObjectEnd); + } + bool finalizeTokenOk = true; + while (comma.type_ == tokenComment && finalizeTokenOk) + finalizeTokenOk = readToken(comma); + if (comma.type_ == tokenObjectEnd) + return true; + } + return addErrorAndRecover("Missing '}' or object member name", tokenName, + tokenObjectEnd); +} + +bool Reader::readArray(Token& token) { + Value init(arrayValue); + currentValue().swapPayload(init); + currentValue().setOffsetStart(token.start_ - begin_); + skipSpaces(); + if (current_ != end_ && *current_ == ']') // empty array + { + Token endArray; + readToken(endArray); + return true; + } + int index = 0; + for (;;) { + Value& value = currentValue()[index++]; + nodes_.push(&value); + bool ok = readValue(); + nodes_.pop(); + if (!ok) // error already set + return recoverFromError(tokenArrayEnd); + + Token currentToken; + // Accept Comment after last item in the array. + ok = readToken(currentToken); + while (currentToken.type_ == tokenComment && ok) { + ok = readToken(currentToken); + } + bool badTokenType = (currentToken.type_ != tokenArraySeparator && + currentToken.type_ != tokenArrayEnd); + if (!ok || badTokenType) { + return addErrorAndRecover("Missing ',' or ']' in array declaration", + currentToken, tokenArrayEnd); + } + if (currentToken.type_ == tokenArrayEnd) + break; + } + return true; +} + +bool Reader::decodeNumber(Token& token) { + Value decoded; + if (!decodeNumber(token, decoded)) + return false; + currentValue().swapPayload(decoded); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + return true; +} + +bool Reader::decodeNumber(Token& token, Value& decoded) { + // Attempts to parse the number as an integer. If the number is + // larger than the maximum supported value of an integer then + // we decode the number as a double. + Location current = token.start_; + bool isNegative = *current == '-'; + if (isNegative) + ++current; + // TODO: Help the compiler do the div and mod at compile time or get rid of + // them. + Value::LargestUInt maxIntegerValue = + isNegative ? Value::LargestUInt(Value::maxLargestInt) + 1 + : Value::maxLargestUInt; + Value::LargestUInt threshold = maxIntegerValue / 10; + Value::LargestUInt value = 0; + while (current < token.end_) { + Char c = *current++; + if (c < '0' || c > '9') + return decodeDouble(token, decoded); + auto digit(static_cast(c - '0')); + if (value >= threshold) { + // We've hit or exceeded the max value divided by 10 (rounded down). If + // a) we've only just touched the limit, b) this is the last digit, and + // c) it's small enough to fit in that rounding delta, we're okay. + // Otherwise treat this number as a double to avoid overflow. + if (value > threshold || current != token.end_ || + digit > maxIntegerValue % 10) { + return decodeDouble(token, decoded); + } + } + value = value * 10 + digit; + } + if (isNegative && value == maxIntegerValue) + decoded = Value::minLargestInt; + else if (isNegative) + decoded = -Value::LargestInt(value); + else if (value <= Value::LargestUInt(Value::maxInt)) + decoded = Value::LargestInt(value); + else + decoded = value; + return true; +} + +bool Reader::decodeDouble(Token& token) { + Value decoded; + if (!decodeDouble(token, decoded)) + return false; + currentValue().swapPayload(decoded); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + return true; +} + +bool Reader::decodeDouble(Token& token, Value& decoded) { + double value = 0; + String buffer(token.start_, token.end_); + IStringStream is(buffer); + if (!(is >> value)) { + if (value == std::numeric_limits::max()) + value = std::numeric_limits::infinity(); + else if (value == std::numeric_limits::lowest()) + value = -std::numeric_limits::infinity(); + else if (!std::isinf(value)) + return addError( + "'" + String(token.start_, token.end_) + "' is not a number.", token); + } + decoded = value; + return true; +} + +bool Reader::decodeString(Token& token) { + String decoded_string; + if (!decodeString(token, decoded_string)) + return false; + Value decoded(decoded_string); + currentValue().swapPayload(decoded); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + return true; +} + +bool Reader::decodeString(Token& token, String& decoded) { + decoded.reserve(static_cast(token.end_ - token.start_ - 2)); + Location current = token.start_ + 1; // skip '"' + Location end = token.end_ - 1; // do not include '"' + while (current != end) { + Char c = *current++; + if (c == '"') + break; + if (c == '\\') { + if (current == end) + return addError("Empty escape sequence in string", token, current); + Char escape = *current++; + switch (escape) { + case '"': + decoded += '"'; + break; + case '/': + decoded += '/'; + break; + case '\\': + decoded += '\\'; + break; + case 'b': + decoded += '\b'; + break; + case 'f': + decoded += '\f'; + break; + case 'n': + decoded += '\n'; + break; + case 'r': + decoded += '\r'; + break; + case 't': + decoded += '\t'; + break; + case 'u': { + unsigned int unicode; + if (!decodeUnicodeCodePoint(token, current, end, unicode)) + return false; + decoded += codePointToUTF8(unicode); + } break; + default: + return addError("Bad escape sequence in string", token, current); + } + } else { + decoded += c; + } + } + return true; +} + +bool Reader::decodeUnicodeCodePoint(Token& token, Location& current, + Location end, unsigned int& unicode) { + + if (!decodeUnicodeEscapeSequence(token, current, end, unicode)) + return false; + if (unicode >= 0xD800 && unicode <= 0xDBFF) { + // surrogate pairs + if (end - current < 6) + return addError( + "additional six characters expected to parse unicode surrogate pair.", + token, current); + if (*(current++) == '\\' && *(current++) == 'u') { + unsigned int surrogatePair; + if (decodeUnicodeEscapeSequence(token, current, end, surrogatePair)) { + unicode = 0x10000 + ((unicode & 0x3FF) << 10) + (surrogatePair & 0x3FF); + } else + return false; + } else + return addError("expecting another \\u token to begin the second half of " + "a unicode surrogate pair", + token, current); + } + return true; +} + +bool Reader::decodeUnicodeEscapeSequence(Token& token, Location& current, + Location end, + unsigned int& ret_unicode) { + if (end - current < 4) + return addError( + "Bad unicode escape sequence in string: four digits expected.", token, + current); + int unicode = 0; + for (int index = 0; index < 4; ++index) { + Char c = *current++; + unicode *= 16; + if (c >= '0' && c <= '9') + unicode += c - '0'; + else if (c >= 'a' && c <= 'f') + unicode += c - 'a' + 10; + else if (c >= 'A' && c <= 'F') + unicode += c - 'A' + 10; + else + return addError( + "Bad unicode escape sequence in string: hexadecimal digit expected.", + token, current); + } + ret_unicode = static_cast(unicode); + return true; +} + +bool Reader::addError(const String& message, Token& token, Location extra) { + ErrorInfo info; + info.token_ = token; + info.message_ = message; + info.extra_ = extra; + errors_.push_back(info); + return false; +} + +bool Reader::recoverFromError(TokenType skipUntilToken) { + size_t const errorCount = errors_.size(); + Token skip; + for (;;) { + if (!readToken(skip)) + errors_.resize(errorCount); // discard errors caused by recovery + if (skip.type_ == skipUntilToken || skip.type_ == tokenEndOfStream) + break; + } + errors_.resize(errorCount); + return false; +} + +bool Reader::addErrorAndRecover(const String& message, Token& token, + TokenType skipUntilToken) { + addError(message, token); + return recoverFromError(skipUntilToken); +} + +Value& Reader::currentValue() { return *(nodes_.top()); } + +Reader::Char Reader::getNextChar() { + if (current_ == end_) + return 0; + return *current_++; +} + +void Reader::getLocationLineAndColumn(Location location, int& line, + int& column) const { + Location current = begin_; + Location lastLineStart = current; + line = 0; + while (current < location && current != end_) { + Char c = *current++; + if (c == '\r') { + if (*current == '\n') + ++current; + lastLineStart = current; + ++line; + } else if (c == '\n') { + lastLineStart = current; + ++line; + } + } + // column & line start at 1 + column = int(location - lastLineStart) + 1; + ++line; +} + +String Reader::getLocationLineAndColumn(Location location) const { + int line, column; + getLocationLineAndColumn(location, line, column); + char buffer[18 + 16 + 16 + 1]; + jsoncpp_snprintf(buffer, sizeof(buffer), "Line %d, Column %d", line, column); + return buffer; +} + +// Deprecated. Preserved for backward compatibility +String Reader::getFormatedErrorMessages() const { + return getFormattedErrorMessages(); +} + +String Reader::getFormattedErrorMessages() const { + String formattedMessage; + for (const auto& error : errors_) { + formattedMessage += + "* " + getLocationLineAndColumn(error.token_.start_) + "\n"; + formattedMessage += " " + error.message_ + "\n"; + if (error.extra_) + formattedMessage += + "See " + getLocationLineAndColumn(error.extra_) + " for detail.\n"; + } + return formattedMessage; +} + +std::vector Reader::getStructuredErrors() const { + std::vector allErrors; + for (const auto& error : errors_) { + Reader::StructuredError structured; + structured.offset_start = error.token_.start_ - begin_; + structured.offset_limit = error.token_.end_ - begin_; + structured.message = error.message_; + allErrors.push_back(structured); + } + return allErrors; +} + +bool Reader::pushError(const Value& value, const String& message) { + ptrdiff_t const length = end_ - begin_; + if (value.getOffsetStart() > length || value.getOffsetLimit() > length) + return false; + Token token; + token.type_ = tokenError; + token.start_ = begin_ + value.getOffsetStart(); + token.end_ = begin_ + value.getOffsetLimit(); + ErrorInfo info; + info.token_ = token; + info.message_ = message; + info.extra_ = nullptr; + errors_.push_back(info); + return true; +} + +bool Reader::pushError(const Value& value, const String& message, + const Value& extra) { + ptrdiff_t const length = end_ - begin_; + if (value.getOffsetStart() > length || value.getOffsetLimit() > length || + extra.getOffsetLimit() > length) + return false; + Token token; + token.type_ = tokenError; + token.start_ = begin_ + value.getOffsetStart(); + token.end_ = begin_ + value.getOffsetLimit(); + ErrorInfo info; + info.token_ = token; + info.message_ = message; + info.extra_ = begin_ + extra.getOffsetStart(); + errors_.push_back(info); + return true; +} + +bool Reader::good() const { return errors_.empty(); } + +// Originally copied from the Features class (now deprecated), used internally +// for features implementation. +class OurFeatures { +public: + static OurFeatures all(); + bool allowComments_; + bool allowTrailingCommas_; + bool strictRoot_; + bool allowDroppedNullPlaceholders_; + bool allowNumericKeys_; + bool allowSingleQuotes_; + bool failIfExtra_; + bool rejectDupKeys_; + bool allowSpecialFloats_; + bool skipBom_; + size_t stackLimit_; +}; // OurFeatures + +OurFeatures OurFeatures::all() { return {}; } + +// Implementation of class Reader +// //////////////////////////////// + +// Originally copied from the Reader class (now deprecated), used internally +// for implementing JSON reading. +class OurReader { +public: + using Char = char; + using Location = const Char*; + struct StructuredError { + ptrdiff_t offset_start; + ptrdiff_t offset_limit; + String message; + }; + + explicit OurReader(OurFeatures const& features); + bool parse(const char* beginDoc, const char* endDoc, Value& root, + bool collectComments = true); + String getFormattedErrorMessages() const; + std::vector getStructuredErrors() const; + +private: + OurReader(OurReader const&); // no impl + void operator=(OurReader const&); // no impl + + enum TokenType { + tokenEndOfStream = 0, + tokenObjectBegin, + tokenObjectEnd, + tokenArrayBegin, + tokenArrayEnd, + tokenString, + tokenNumber, + tokenTrue, + tokenFalse, + tokenNull, + tokenNaN, + tokenPosInf, + tokenNegInf, + tokenArraySeparator, + tokenMemberSeparator, + tokenComment, + tokenError + }; + + class Token { + public: + TokenType type_; + Location start_; + Location end_; + }; + + class ErrorInfo { + public: + Token token_; + String message_; + Location extra_; + }; + + using Errors = std::deque; + + bool readToken(Token& token); + void skipSpaces(); + void skipBom(bool skipBom); + bool match(const Char* pattern, int patternLength); + bool readComment(); + bool readCStyleComment(bool* containsNewLineResult); + bool readCppStyleComment(); + bool readString(); + bool readStringSingleQuote(); + bool readNumber(bool checkInf); + bool readValue(); + bool readObject(Token& token); + bool readArray(Token& token); + bool decodeNumber(Token& token); + bool decodeNumber(Token& token, Value& decoded); + bool decodeString(Token& token); + bool decodeString(Token& token, String& decoded); + bool decodeDouble(Token& token); + bool decodeDouble(Token& token, Value& decoded); + bool decodeUnicodeCodePoint(Token& token, Location& current, Location end, + unsigned int& unicode); + bool decodeUnicodeEscapeSequence(Token& token, Location& current, + Location end, unsigned int& unicode); + bool addError(const String& message, Token& token, Location extra = nullptr); + bool recoverFromError(TokenType skipUntilToken); + bool addErrorAndRecover(const String& message, Token& token, + TokenType skipUntilToken); + void skipUntilSpace(); + Value& currentValue(); + Char getNextChar(); + void getLocationLineAndColumn(Location location, int& line, + int& column) const; + String getLocationLineAndColumn(Location location) const; + void addComment(Location begin, Location end, CommentPlacement placement); + void skipCommentTokens(Token& token); + + static String normalizeEOL(Location begin, Location end); + static bool containsNewLine(Location begin, Location end); + + using Nodes = std::stack; + + Nodes nodes_{}; + Errors errors_{}; + String document_{}; + Location begin_ = nullptr; + Location end_ = nullptr; + Location current_ = nullptr; + Location lastValueEnd_ = nullptr; + Value* lastValue_ = nullptr; + bool lastValueHasAComment_ = false; + String commentsBefore_{}; + + OurFeatures const features_; + bool collectComments_ = false; +}; // OurReader + +// complete copy of Read impl, for OurReader + +bool OurReader::containsNewLine(OurReader::Location begin, + OurReader::Location end) { + return std::any_of(begin, end, [](char b) { return b == '\n' || b == '\r'; }); +} + +OurReader::OurReader(OurFeatures const& features) : features_(features) {} + +bool OurReader::parse(const char* beginDoc, const char* endDoc, Value& root, + bool collectComments) { + if (!features_.allowComments_) { + collectComments = false; + } + + begin_ = beginDoc; + end_ = endDoc; + collectComments_ = collectComments; + current_ = begin_; + lastValueEnd_ = nullptr; + lastValue_ = nullptr; + commentsBefore_.clear(); + errors_.clear(); + while (!nodes_.empty()) + nodes_.pop(); + nodes_.push(&root); + + // skip byte order mark if it exists at the beginning of the UTF-8 text. + skipBom(features_.skipBom_); + bool successful = readValue(); + nodes_.pop(); + Token token; + skipCommentTokens(token); + if (features_.failIfExtra_ && (token.type_ != tokenEndOfStream)) { + addError("Extra non-whitespace after JSON value.", token); + return false; + } + if (collectComments_ && !commentsBefore_.empty()) + root.setComment(commentsBefore_, commentAfter); + if (features_.strictRoot_) { + if (!root.isArray() && !root.isObject()) { + // Set error location to start of doc, ideally should be first token found + // in doc + token.type_ = tokenError; + token.start_ = beginDoc; + token.end_ = endDoc; + addError( + "A valid JSON document must be either an array or an object value.", + token); + return false; + } + } + return successful; +} + +bool OurReader::readValue() { + // To preserve the old behaviour we cast size_t to int. + if (nodes_.size() > features_.stackLimit_) + throwRuntimeError("Exceeded stackLimit in readValue()."); + Token token; + skipCommentTokens(token); + bool successful = true; + + if (collectComments_ && !commentsBefore_.empty()) { + currentValue().setComment(commentsBefore_, commentBefore); + commentsBefore_.clear(); + } + + switch (token.type_) { + case tokenObjectBegin: + successful = readObject(token); + currentValue().setOffsetLimit(current_ - begin_); + break; + case tokenArrayBegin: + successful = readArray(token); + currentValue().setOffsetLimit(current_ - begin_); + break; + case tokenNumber: + successful = decodeNumber(token); + break; + case tokenString: + successful = decodeString(token); + break; + case tokenTrue: { + Value v(true); + currentValue().swapPayload(v); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + } break; + case tokenFalse: { + Value v(false); + currentValue().swapPayload(v); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + } break; + case tokenNull: { + Value v; + currentValue().swapPayload(v); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + } break; + case tokenNaN: { + Value v(std::numeric_limits::quiet_NaN()); + currentValue().swapPayload(v); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + } break; + case tokenPosInf: { + Value v(std::numeric_limits::infinity()); + currentValue().swapPayload(v); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + } break; + case tokenNegInf: { + Value v(-std::numeric_limits::infinity()); + currentValue().swapPayload(v); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + } break; + case tokenArraySeparator: + case tokenObjectEnd: + case tokenArrayEnd: + if (features_.allowDroppedNullPlaceholders_) { + // "Un-read" the current token and mark the current value as a null + // token. + current_--; + Value v; + currentValue().swapPayload(v); + currentValue().setOffsetStart(current_ - begin_ - 1); + currentValue().setOffsetLimit(current_ - begin_); + break; + } // else, fall through ... + default: + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + return addError("Syntax error: value, object or array expected.", token); + } + + if (collectComments_) { + lastValueEnd_ = current_; + lastValueHasAComment_ = false; + lastValue_ = ¤tValue(); + } + + return successful; +} + +void OurReader::skipCommentTokens(Token& token) { + if (features_.allowComments_) { + do { + readToken(token); + } while (token.type_ == tokenComment); + } else { + readToken(token); + } +} + +bool OurReader::readToken(Token& token) { + skipSpaces(); + token.start_ = current_; + Char c = getNextChar(); + bool ok = true; + switch (c) { + case '{': + token.type_ = tokenObjectBegin; + break; + case '}': + token.type_ = tokenObjectEnd; + break; + case '[': + token.type_ = tokenArrayBegin; + break; + case ']': + token.type_ = tokenArrayEnd; + break; + case '"': + token.type_ = tokenString; + ok = readString(); + break; + case '\'': + if (features_.allowSingleQuotes_) { + token.type_ = tokenString; + ok = readStringSingleQuote(); + } else { + // If we don't allow single quotes, this is a failure case. + ok = false; + } + break; + case '/': + token.type_ = tokenComment; + ok = readComment(); + break; + case '0': + case '1': + case '2': + case '3': + case '4': + case '5': + case '6': + case '7': + case '8': + case '9': + token.type_ = tokenNumber; + readNumber(false); + break; + case '-': + if (readNumber(true)) { + token.type_ = tokenNumber; + } else { + token.type_ = tokenNegInf; + ok = features_.allowSpecialFloats_ && match("nfinity", 7); + } + break; + case '+': + if (readNumber(true)) { + token.type_ = tokenNumber; + } else { + token.type_ = tokenPosInf; + ok = features_.allowSpecialFloats_ && match("nfinity", 7); + } + break; + case 't': + token.type_ = tokenTrue; + ok = match("rue", 3); + break; + case 'f': + token.type_ = tokenFalse; + ok = match("alse", 4); + break; + case 'n': + token.type_ = tokenNull; + ok = match("ull", 3); + break; + case 'N': + if (features_.allowSpecialFloats_) { + token.type_ = tokenNaN; + ok = match("aN", 2); + } else { + ok = false; + } + break; + case 'I': + if (features_.allowSpecialFloats_) { + token.type_ = tokenPosInf; + ok = match("nfinity", 7); + } else { + ok = false; + } + break; + case ',': + token.type_ = tokenArraySeparator; + break; + case ':': + token.type_ = tokenMemberSeparator; + break; + case 0: + token.type_ = tokenEndOfStream; + break; + default: + ok = false; + break; + } + if (!ok) + token.type_ = tokenError; + token.end_ = current_; + return ok; +} + +void OurReader::skipSpaces() { + while (current_ != end_) { + Char c = *current_; + if (c == ' ' || c == '\t' || c == '\r' || c == '\n') + ++current_; + else + break; + } +} + +void OurReader::skipBom(bool skipBom) { + // The default behavior is to skip BOM. + if (skipBom) { + if ((end_ - begin_) >= 3 && strncmp(begin_, "\xEF\xBB\xBF", 3) == 0) { + begin_ += 3; + current_ = begin_; + } + } +} + +bool OurReader::match(const Char* pattern, int patternLength) { + if (end_ - current_ < patternLength) + return false; + int index = patternLength; + while (index--) + if (current_[index] != pattern[index]) + return false; + current_ += patternLength; + return true; +} + +bool OurReader::readComment() { + const Location commentBegin = current_ - 1; + const Char c = getNextChar(); + bool successful = false; + bool cStyleWithEmbeddedNewline = false; + + const bool isCStyleComment = (c == '*'); + const bool isCppStyleComment = (c == '/'); + if (isCStyleComment) { + successful = readCStyleComment(&cStyleWithEmbeddedNewline); + } else if (isCppStyleComment) { + successful = readCppStyleComment(); + } + + if (!successful) + return false; + + if (collectComments_) { + CommentPlacement placement = commentBefore; + + if (!lastValueHasAComment_) { + if (lastValueEnd_ && !containsNewLine(lastValueEnd_, commentBegin)) { + if (isCppStyleComment || !cStyleWithEmbeddedNewline) { + placement = commentAfterOnSameLine; + lastValueHasAComment_ = true; + } + } + } + + addComment(commentBegin, current_, placement); + } + return true; +} + +String OurReader::normalizeEOL(OurReader::Location begin, + OurReader::Location end) { + String normalized; + normalized.reserve(static_cast(end - begin)); + OurReader::Location current = begin; + while (current != end) { + char c = *current++; + if (c == '\r') { + if (current != end && *current == '\n') + // convert dos EOL + ++current; + // convert Mac EOL + normalized += '\n'; + } else { + normalized += c; + } + } + return normalized; +} + +void OurReader::addComment(Location begin, Location end, + CommentPlacement placement) { + assert(collectComments_); + const String& normalized = normalizeEOL(begin, end); + if (placement == commentAfterOnSameLine) { + assert(lastValue_ != nullptr); + lastValue_->setComment(normalized, placement); + } else { + commentsBefore_ += normalized; + } +} + +bool OurReader::readCStyleComment(bool* containsNewLineResult) { + *containsNewLineResult = false; + + while ((current_ + 1) < end_) { + Char c = getNextChar(); + if (c == '*' && *current_ == '/') + break; + if (c == '\n') + *containsNewLineResult = true; + } + + return getNextChar() == '/'; +} + +bool OurReader::readCppStyleComment() { + while (current_ != end_) { + Char c = getNextChar(); + if (c == '\n') + break; + if (c == '\r') { + // Consume DOS EOL. It will be normalized in addComment. + if (current_ != end_ && *current_ == '\n') + getNextChar(); + // Break on Moc OS 9 EOL. + break; + } + } + return true; +} + +bool OurReader::readNumber(bool checkInf) { + Location p = current_; + if (checkInf && p != end_ && *p == 'I') { + current_ = ++p; + return false; + } + char c = '0'; // stopgap for already consumed character + // integral part + while (c >= '0' && c <= '9') + c = (current_ = p) < end_ ? *p++ : '\0'; + // fractional part + if (c == '.') { + c = (current_ = p) < end_ ? *p++ : '\0'; + while (c >= '0' && c <= '9') + c = (current_ = p) < end_ ? *p++ : '\0'; + } + // exponential part + if (c == 'e' || c == 'E') { + c = (current_ = p) < end_ ? *p++ : '\0'; + if (c == '+' || c == '-') + c = (current_ = p) < end_ ? *p++ : '\0'; + while (c >= '0' && c <= '9') + c = (current_ = p) < end_ ? *p++ : '\0'; + } + return true; +} +bool OurReader::readString() { + Char c = 0; + while (current_ != end_) { + c = getNextChar(); + if (c == '\\') + getNextChar(); + else if (c == '"') + break; + } + return c == '"'; +} + +bool OurReader::readStringSingleQuote() { + Char c = 0; + while (current_ != end_) { + c = getNextChar(); + if (c == '\\') + getNextChar(); + else if (c == '\'') + break; + } + return c == '\''; +} + +bool OurReader::readObject(Token& token) { + Token tokenName; + String name; + Value init(objectValue); + currentValue().swapPayload(init); + currentValue().setOffsetStart(token.start_ - begin_); + while (readToken(tokenName)) { + bool initialTokenOk = true; + while (tokenName.type_ == tokenComment && initialTokenOk) + initialTokenOk = readToken(tokenName); + if (!initialTokenOk) + break; + if (tokenName.type_ == tokenObjectEnd && + (name.empty() || + features_.allowTrailingCommas_)) // empty object or trailing comma + return true; + name.clear(); + if (tokenName.type_ == tokenString) { + if (!decodeString(tokenName, name)) + return recoverFromError(tokenObjectEnd); + } else if (tokenName.type_ == tokenNumber && features_.allowNumericKeys_) { + Value numberName; + if (!decodeNumber(tokenName, numberName)) + return recoverFromError(tokenObjectEnd); + name = numberName.asString(); + } else { + break; + } + if (name.length() >= (1U << 30)) + throwRuntimeError("keylength >= 2^30"); + if (features_.rejectDupKeys_ && currentValue().isMember(name)) { + String msg = "Duplicate key: '" + name + "'"; + return addErrorAndRecover(msg, tokenName, tokenObjectEnd); + } + + Token colon; + if (!readToken(colon) || colon.type_ != tokenMemberSeparator) { + return addErrorAndRecover("Missing ':' after object member name", colon, + tokenObjectEnd); + } + Value& value = currentValue()[name]; + nodes_.push(&value); + bool ok = readValue(); + nodes_.pop(); + if (!ok) // error already set + return recoverFromError(tokenObjectEnd); + + Token comma; + if (!readToken(comma) || + (comma.type_ != tokenObjectEnd && comma.type_ != tokenArraySeparator && + comma.type_ != tokenComment)) { + return addErrorAndRecover("Missing ',' or '}' in object declaration", + comma, tokenObjectEnd); + } + bool finalizeTokenOk = true; + while (comma.type_ == tokenComment && finalizeTokenOk) + finalizeTokenOk = readToken(comma); + if (comma.type_ == tokenObjectEnd) + return true; + } + return addErrorAndRecover("Missing '}' or object member name", tokenName, + tokenObjectEnd); +} + +bool OurReader::readArray(Token& token) { + Value init(arrayValue); + currentValue().swapPayload(init); + currentValue().setOffsetStart(token.start_ - begin_); + int index = 0; + for (;;) { + skipSpaces(); + if (current_ != end_ && *current_ == ']' && + (index == 0 || + (features_.allowTrailingCommas_ && + !features_.allowDroppedNullPlaceholders_))) // empty array or trailing + // comma + { + Token endArray; + readToken(endArray); + return true; + } + Value& value = currentValue()[index++]; + nodes_.push(&value); + bool ok = readValue(); + nodes_.pop(); + if (!ok) // error already set + return recoverFromError(tokenArrayEnd); + + Token currentToken; + // Accept Comment after last item in the array. + ok = readToken(currentToken); + while (currentToken.type_ == tokenComment && ok) { + ok = readToken(currentToken); + } + bool badTokenType = (currentToken.type_ != tokenArraySeparator && + currentToken.type_ != tokenArrayEnd); + if (!ok || badTokenType) { + return addErrorAndRecover("Missing ',' or ']' in array declaration", + currentToken, tokenArrayEnd); + } + if (currentToken.type_ == tokenArrayEnd) + break; + } + return true; +} + +bool OurReader::decodeNumber(Token& token) { + Value decoded; + if (!decodeNumber(token, decoded)) + return false; + currentValue().swapPayload(decoded); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + return true; +} + +bool OurReader::decodeNumber(Token& token, Value& decoded) { + // Attempts to parse the number as an integer. If the number is + // larger than the maximum supported value of an integer then + // we decode the number as a double. + Location current = token.start_; + const bool isNegative = *current == '-'; + if (isNegative) { + ++current; + } + + // We assume we can represent the largest and smallest integer types as + // unsigned integers with separate sign. This is only true if they can fit + // into an unsigned integer. + static_assert(Value::maxLargestInt <= Value::maxLargestUInt, + "Int must be smaller than UInt"); + + // We need to convert minLargestInt into a positive number. The easiest way + // to do this conversion is to assume our "threshold" value of minLargestInt + // divided by 10 can fit in maxLargestInt when absolute valued. This should + // be a safe assumption. + static_assert(Value::minLargestInt <= -Value::maxLargestInt, + "The absolute value of minLargestInt must be greater than or " + "equal to maxLargestInt"); + static_assert(Value::minLargestInt / 10 >= -Value::maxLargestInt, + "The absolute value of minLargestInt must be only 1 magnitude " + "larger than maxLargest Int"); + + static constexpr Value::LargestUInt positive_threshold = + Value::maxLargestUInt / 10; + static constexpr Value::UInt positive_last_digit = Value::maxLargestUInt % 10; + + // For the negative values, we have to be more careful. Since typically + // -Value::minLargestInt will cause an overflow, we first divide by 10 and + // then take the inverse. This assumes that minLargestInt is only a single + // power of 10 different in magnitude, which we check above. For the last + // digit, we take the modulus before negating for the same reason. + static constexpr auto negative_threshold = + Value::LargestUInt(-(Value::minLargestInt / 10)); + static constexpr auto negative_last_digit = + Value::UInt(-(Value::minLargestInt % 10)); + + const Value::LargestUInt threshold = + isNegative ? negative_threshold : positive_threshold; + const Value::UInt max_last_digit = + isNegative ? negative_last_digit : positive_last_digit; + + Value::LargestUInt value = 0; + while (current < token.end_) { + Char c = *current++; + if (c < '0' || c > '9') + return decodeDouble(token, decoded); + + const auto digit(static_cast(c - '0')); + if (value >= threshold) { + // We've hit or exceeded the max value divided by 10 (rounded down). If + // a) we've only just touched the limit, meaning value == threshold, + // b) this is the last digit, or + // c) it's small enough to fit in that rounding delta, we're okay. + // Otherwise treat this number as a double to avoid overflow. + if (value > threshold || current != token.end_ || + digit > max_last_digit) { + return decodeDouble(token, decoded); + } + } + value = value * 10 + digit; + } + + if (isNegative) { + // We use the same magnitude assumption here, just in case. + const auto last_digit = static_cast(value % 10); + decoded = -Value::LargestInt(value / 10) * 10 - last_digit; + } else if (value <= Value::LargestUInt(Value::maxLargestInt)) { + decoded = Value::LargestInt(value); + } else { + decoded = value; + } + + return true; +} + +bool OurReader::decodeDouble(Token& token) { + Value decoded; + if (!decodeDouble(token, decoded)) + return false; + currentValue().swapPayload(decoded); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + return true; +} + +bool OurReader::decodeDouble(Token& token, Value& decoded) { + double value = 0; + const String buffer(token.start_, token.end_); + IStringStream is(buffer); + if (!(is >> value)) { + if (value == std::numeric_limits::max()) + value = std::numeric_limits::infinity(); + else if (value == std::numeric_limits::lowest()) + value = -std::numeric_limits::infinity(); + else if (!std::isinf(value)) + return addError( + "'" + String(token.start_, token.end_) + "' is not a number.", token); + } + decoded = value; + return true; +} + +bool OurReader::decodeString(Token& token) { + String decoded_string; + if (!decodeString(token, decoded_string)) + return false; + Value decoded(decoded_string); + currentValue().swapPayload(decoded); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + return true; +} + +bool OurReader::decodeString(Token& token, String& decoded) { + decoded.reserve(static_cast(token.end_ - token.start_ - 2)); + Location current = token.start_ + 1; // skip '"' + Location end = token.end_ - 1; // do not include '"' + while (current != end) { + Char c = *current++; + if (c == '"') + break; + if (c == '\\') { + if (current == end) + return addError("Empty escape sequence in string", token, current); + Char escape = *current++; + switch (escape) { + case '"': + decoded += '"'; + break; + case '/': + decoded += '/'; + break; + case '\\': + decoded += '\\'; + break; + case 'b': + decoded += '\b'; + break; + case 'f': + decoded += '\f'; + break; + case 'n': + decoded += '\n'; + break; + case 'r': + decoded += '\r'; + break; + case 't': + decoded += '\t'; + break; + case 'u': { + unsigned int unicode; + if (!decodeUnicodeCodePoint(token, current, end, unicode)) + return false; + decoded += codePointToUTF8(unicode); + } break; + default: + return addError("Bad escape sequence in string", token, current); + } + } else { + decoded += c; + } + } + return true; +} + +bool OurReader::decodeUnicodeCodePoint(Token& token, Location& current, + Location end, unsigned int& unicode) { + + if (!decodeUnicodeEscapeSequence(token, current, end, unicode)) + return false; + if (unicode >= 0xD800 && unicode <= 0xDBFF) { + // surrogate pairs + if (end - current < 6) + return addError( + "additional six characters expected to parse unicode surrogate pair.", + token, current); + if (*(current++) == '\\' && *(current++) == 'u') { + unsigned int surrogatePair; + if (decodeUnicodeEscapeSequence(token, current, end, surrogatePair)) { + unicode = 0x10000 + ((unicode & 0x3FF) << 10) + (surrogatePair & 0x3FF); + } else + return false; + } else + return addError("expecting another \\u token to begin the second half of " + "a unicode surrogate pair", + token, current); + } + return true; +} + +bool OurReader::decodeUnicodeEscapeSequence(Token& token, Location& current, + Location end, + unsigned int& ret_unicode) { + if (end - current < 4) + return addError( + "Bad unicode escape sequence in string: four digits expected.", token, + current); + int unicode = 0; + for (int index = 0; index < 4; ++index) { + Char c = *current++; + unicode *= 16; + if (c >= '0' && c <= '9') + unicode += c - '0'; + else if (c >= 'a' && c <= 'f') + unicode += c - 'a' + 10; + else if (c >= 'A' && c <= 'F') + unicode += c - 'A' + 10; + else + return addError( + "Bad unicode escape sequence in string: hexadecimal digit expected.", + token, current); + } + ret_unicode = static_cast(unicode); + return true; +} + +bool OurReader::addError(const String& message, Token& token, Location extra) { + ErrorInfo info; + info.token_ = token; + info.message_ = message; + info.extra_ = extra; + errors_.push_back(info); + return false; +} + +bool OurReader::recoverFromError(TokenType skipUntilToken) { + size_t errorCount = errors_.size(); + Token skip; + for (;;) { + if (!readToken(skip)) + errors_.resize(errorCount); // discard errors caused by recovery + if (skip.type_ == skipUntilToken || skip.type_ == tokenEndOfStream) + break; + } + errors_.resize(errorCount); + return false; +} + +bool OurReader::addErrorAndRecover(const String& message, Token& token, + TokenType skipUntilToken) { + addError(message, token); + return recoverFromError(skipUntilToken); +} + +Value& OurReader::currentValue() { return *(nodes_.top()); } + +OurReader::Char OurReader::getNextChar() { + if (current_ == end_) + return 0; + return *current_++; +} + +void OurReader::getLocationLineAndColumn(Location location, int& line, + int& column) const { + Location current = begin_; + Location lastLineStart = current; + line = 0; + while (current < location && current != end_) { + Char c = *current++; + if (c == '\r') { + if (*current == '\n') + ++current; + lastLineStart = current; + ++line; + } else if (c == '\n') { + lastLineStart = current; + ++line; + } + } + // column & line start at 1 + column = int(location - lastLineStart) + 1; + ++line; +} + +String OurReader::getLocationLineAndColumn(Location location) const { + int line, column; + getLocationLineAndColumn(location, line, column); + char buffer[18 + 16 + 16 + 1]; + jsoncpp_snprintf(buffer, sizeof(buffer), "Line %d, Column %d", line, column); + return buffer; +} + +String OurReader::getFormattedErrorMessages() const { + String formattedMessage; + for (const auto& error : errors_) { + formattedMessage += + "* " + getLocationLineAndColumn(error.token_.start_) + "\n"; + formattedMessage += " " + error.message_ + "\n"; + if (error.extra_) + formattedMessage += + "See " + getLocationLineAndColumn(error.extra_) + " for detail.\n"; + } + return formattedMessage; +} + +std::vector OurReader::getStructuredErrors() const { + std::vector allErrors; + for (const auto& error : errors_) { + OurReader::StructuredError structured; + structured.offset_start = error.token_.start_ - begin_; + structured.offset_limit = error.token_.end_ - begin_; + structured.message = error.message_; + allErrors.push_back(structured); + } + return allErrors; +} + +class OurCharReader : public CharReader { + bool const collectComments_; + OurReader reader_; + +public: + OurCharReader(bool collectComments, OurFeatures const& features) + : collectComments_(collectComments), reader_(features) {} + bool parse(char const* beginDoc, char const* endDoc, Value* root, + String* errs) override { + bool ok = reader_.parse(beginDoc, endDoc, *root, collectComments_); + if (errs) { + *errs = reader_.getFormattedErrorMessages(); + } + return ok; + } +}; + +CharReaderBuilder::CharReaderBuilder() { setDefaults(&settings_); } +CharReaderBuilder::~CharReaderBuilder() = default; +CharReader* CharReaderBuilder::newCharReader() const { + bool collectComments = settings_["collectComments"].asBool(); + OurFeatures features = OurFeatures::all(); + features.allowComments_ = settings_["allowComments"].asBool(); + features.allowTrailingCommas_ = settings_["allowTrailingCommas"].asBool(); + features.strictRoot_ = settings_["strictRoot"].asBool(); + features.allowDroppedNullPlaceholders_ = + settings_["allowDroppedNullPlaceholders"].asBool(); + features.allowNumericKeys_ = settings_["allowNumericKeys"].asBool(); + features.allowSingleQuotes_ = settings_["allowSingleQuotes"].asBool(); + + // Stack limit is always a size_t, so we get this as an unsigned int + // regardless of it we have 64-bit integer support enabled. + features.stackLimit_ = static_cast(settings_["stackLimit"].asUInt()); + features.failIfExtra_ = settings_["failIfExtra"].asBool(); + features.rejectDupKeys_ = settings_["rejectDupKeys"].asBool(); + features.allowSpecialFloats_ = settings_["allowSpecialFloats"].asBool(); + features.skipBom_ = settings_["skipBom"].asBool(); + return new OurCharReader(collectComments, features); +} + +bool CharReaderBuilder::validate(Json::Value* invalid) const { + static const auto& valid_keys = *new std::set{ + "collectComments", + "allowComments", + "allowTrailingCommas", + "strictRoot", + "allowDroppedNullPlaceholders", + "allowNumericKeys", + "allowSingleQuotes", + "stackLimit", + "failIfExtra", + "rejectDupKeys", + "allowSpecialFloats", + "skipBom", + }; + for (auto si = settings_.begin(); si != settings_.end(); ++si) { + auto key = si.name(); + if (valid_keys.count(key)) + continue; + if (invalid) + (*invalid)[key] = *si; + else + return false; + } + return invalid ? invalid->empty() : true; +} + +Value& CharReaderBuilder::operator[](const String& key) { + return settings_[key]; +} +// static +void CharReaderBuilder::strictMode(Json::Value* settings) { + //! [CharReaderBuilderStrictMode] + (*settings)["allowComments"] = false; + (*settings)["allowTrailingCommas"] = false; + (*settings)["strictRoot"] = true; + (*settings)["allowDroppedNullPlaceholders"] = false; + (*settings)["allowNumericKeys"] = false; + (*settings)["allowSingleQuotes"] = false; + (*settings)["stackLimit"] = 1000; + (*settings)["failIfExtra"] = true; + (*settings)["rejectDupKeys"] = true; + (*settings)["allowSpecialFloats"] = false; + (*settings)["skipBom"] = true; + //! [CharReaderBuilderStrictMode] +} +// static +void CharReaderBuilder::setDefaults(Json::Value* settings) { + //! [CharReaderBuilderDefaults] + (*settings)["collectComments"] = true; + (*settings)["allowComments"] = true; + (*settings)["allowTrailingCommas"] = true; + (*settings)["strictRoot"] = false; + (*settings)["allowDroppedNullPlaceholders"] = false; + (*settings)["allowNumericKeys"] = false; + (*settings)["allowSingleQuotes"] = false; + (*settings)["stackLimit"] = 1000; + (*settings)["failIfExtra"] = false; + (*settings)["rejectDupKeys"] = false; + (*settings)["allowSpecialFloats"] = false; + (*settings)["skipBom"] = true; + //! [CharReaderBuilderDefaults] +} + +////////////////////////////////// +// global functions + +bool parseFromStream(CharReader::Factory const& fact, IStream& sin, Value* root, + String* errs) { + OStringStream ssin; + ssin << sin.rdbuf(); + String doc = ssin.str(); + char const* begin = doc.data(); + char const* end = begin + doc.size(); + // Note that we do not actually need a null-terminator. + CharReaderPtr const reader(fact.newCharReader()); + return reader->parse(begin, end, root, errs); +} + +IStream& operator>>(IStream& sin, Value& root) { + CharReaderBuilder b; + String errs; + bool ok = parseFromStream(b, sin, &root, &errs); + if (!ok) { + throwRuntimeError(errs); + } + return sin; +} + +} // namespace Json + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: src/lib_json/json_reader.cpp +// ////////////////////////////////////////////////////////////////////// + + + + + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: src/lib_json/json_valueiterator.inl +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2007-2010 Baptiste Lepilleur and The JsonCpp Authors +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +// included by json_value.cpp + +namespace Json { + +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// class ValueIteratorBase +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// + +ValueIteratorBase::ValueIteratorBase() : current_() {} + +ValueIteratorBase::ValueIteratorBase( + const Value::ObjectValues::iterator& current) + : current_(current), isNull_(false) {} + +Value& ValueIteratorBase::deref() { return current_->second; } +const Value& ValueIteratorBase::deref() const { return current_->second; } + +void ValueIteratorBase::increment() { ++current_; } + +void ValueIteratorBase::decrement() { --current_; } + +ValueIteratorBase::difference_type +ValueIteratorBase::computeDistance(const SelfType& other) const { + // Iterator for null value are initialized using the default + // constructor, which initialize current_ to the default + // std::map::iterator. As begin() and end() are two instance + // of the default std::map::iterator, they can not be compared. + // To allow this, we handle this comparison specifically. + if (isNull_ && other.isNull_) { + return 0; + } + + // Usage of std::distance is not portable (does not compile with Sun Studio 12 + // RogueWave STL, + // which is the one used by default). + // Using a portable hand-made version for non random iterator instead: + // return difference_type( std::distance( current_, other.current_ ) ); + difference_type myDistance = 0; + for (Value::ObjectValues::iterator it = current_; it != other.current_; + ++it) { + ++myDistance; + } + return myDistance; +} + +bool ValueIteratorBase::isEqual(const SelfType& other) const { + if (isNull_) { + return other.isNull_; + } + return current_ == other.current_; +} + +void ValueIteratorBase::copy(const SelfType& other) { + current_ = other.current_; + isNull_ = other.isNull_; +} + +Value ValueIteratorBase::key() const { + const Value::CZString czstring = (*current_).first; + if (czstring.data()) { + if (czstring.isStaticString()) + return Value(StaticString(czstring.data())); + return Value(czstring.data(), czstring.data() + czstring.length()); + } + return Value(czstring.index()); +} + +UInt ValueIteratorBase::index() const { + const Value::CZString czstring = (*current_).first; + if (!czstring.data()) + return czstring.index(); + return Value::UInt(-1); +} + +String ValueIteratorBase::name() const { + char const* keey; + char const* end; + keey = memberName(&end); + if (!keey) + return String(); + return String(keey, end); +} + +char const* ValueIteratorBase::memberName() const { + const char* cname = (*current_).first.data(); + return cname ? cname : ""; +} + +char const* ValueIteratorBase::memberName(char const** end) const { + const char* cname = (*current_).first.data(); + if (!cname) { + *end = nullptr; + return nullptr; + } + *end = cname + (*current_).first.length(); + return cname; +} + +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// class ValueConstIterator +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// + +ValueConstIterator::ValueConstIterator() = default; + +ValueConstIterator::ValueConstIterator( + const Value::ObjectValues::iterator& current) + : ValueIteratorBase(current) {} + +ValueConstIterator::ValueConstIterator(ValueIterator const& other) + : ValueIteratorBase(other) {} + +ValueConstIterator& ValueConstIterator:: +operator=(const ValueIteratorBase& other) { + copy(other); + return *this; +} + +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// class ValueIterator +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// + +ValueIterator::ValueIterator() = default; + +ValueIterator::ValueIterator(const Value::ObjectValues::iterator& current) + : ValueIteratorBase(current) {} + +ValueIterator::ValueIterator(const ValueConstIterator& other) + : ValueIteratorBase(other) { + throwRuntimeError("ConstIterator to Iterator should never be allowed."); +} + +ValueIterator::ValueIterator(const ValueIterator& other) = default; + +ValueIterator& ValueIterator::operator=(const SelfType& other) { + copy(other); + return *this; +} + +} // namespace Json + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: src/lib_json/json_valueiterator.inl +// ////////////////////////////////////////////////////////////////////// + + + + + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: src/lib_json/json_value.cpp +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2011 Baptiste Lepilleur and The JsonCpp Authors +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +#if !defined(JSON_IS_AMALGAMATION) +#include +#include +#include +#endif // if !defined(JSON_IS_AMALGAMATION) +#include +#include +#include +#include +#include +#include +#include +#include + +// Provide implementation equivalent of std::snprintf for older _MSC compilers +#if defined(_MSC_VER) && _MSC_VER < 1900 +#include +static int msvc_pre1900_c99_vsnprintf(char* outBuf, size_t size, + const char* format, va_list ap) { + int count = -1; + if (size != 0) + count = _vsnprintf_s(outBuf, size, _TRUNCATE, format, ap); + if (count == -1) + count = _vscprintf(format, ap); + return count; +} + +int JSON_API msvc_pre1900_c99_snprintf(char* outBuf, size_t size, + const char* format, ...) { + va_list ap; + va_start(ap, format); + const int count = msvc_pre1900_c99_vsnprintf(outBuf, size, format, ap); + va_end(ap); + return count; +} +#endif + +// Disable warning C4702 : unreachable code +#if defined(_MSC_VER) +#pragma warning(disable : 4702) +#endif + +#define JSON_ASSERT_UNREACHABLE assert(false) + +namespace Json { +template +static std::unique_ptr cloneUnique(const std::unique_ptr& p) { + std::unique_ptr r; + if (p) { + r = std::unique_ptr(new T(*p)); + } + return r; +} + +// This is a walkaround to avoid the static initialization of Value::null. +// kNull must be word-aligned to avoid crashing on ARM. We use an alignment of +// 8 (instead of 4) as a bit of future-proofing. +#if defined(__ARMEL__) +#define ALIGNAS(byte_alignment) __attribute__((aligned(byte_alignment))) +#else +#define ALIGNAS(byte_alignment) +#endif + +// static +Value const& Value::nullSingleton() { + static Value const nullStatic; + return nullStatic; +} + +#if JSON_USE_NULLREF +// for backwards compatibility, we'll leave these global references around, but +// DO NOT use them in JSONCPP library code any more! +// static +Value const& Value::null = Value::nullSingleton(); + +// static +Value const& Value::nullRef = Value::nullSingleton(); +#endif + +#if !defined(JSON_USE_INT64_DOUBLE_CONVERSION) +template +static inline bool InRange(double d, T min, U max) { + // The casts can lose precision, but we are looking only for + // an approximate range. Might fail on edge cases though. ~cdunn + return d >= static_cast(min) && d <= static_cast(max); +} +#else // if !defined(JSON_USE_INT64_DOUBLE_CONVERSION) +static inline double integerToDouble(Json::UInt64 value) { + return static_cast(Int64(value / 2)) * 2.0 + + static_cast(Int64(value & 1)); +} + +template static inline double integerToDouble(T value) { + return static_cast(value); +} + +template +static inline bool InRange(double d, T min, U max) { + return d >= integerToDouble(min) && d <= integerToDouble(max); +} +#endif // if !defined(JSON_USE_INT64_DOUBLE_CONVERSION) + +/** Duplicates the specified string value. + * @param value Pointer to the string to duplicate. Must be zero-terminated if + * length is "unknown". + * @param length Length of the value. if equals to unknown, then it will be + * computed using strlen(value). + * @return Pointer on the duplicate instance of string. + */ +static inline char* duplicateStringValue(const char* value, size_t length) { + // Avoid an integer overflow in the call to malloc below by limiting length + // to a sane value. + if (length >= static_cast(Value::maxInt)) + length = Value::maxInt - 1; + + auto newString = static_cast(malloc(length + 1)); + if (newString == nullptr) { + throwRuntimeError("in Json::Value::duplicateStringValue(): " + "Failed to allocate string value buffer"); + } + memcpy(newString, value, length); + newString[length] = 0; + return newString; +} + +/* Record the length as a prefix. + */ +static inline char* duplicateAndPrefixStringValue(const char* value, + unsigned int length) { + // Avoid an integer overflow in the call to malloc below by limiting length + // to a sane value. + JSON_ASSERT_MESSAGE(length <= static_cast(Value::maxInt) - + sizeof(unsigned) - 1U, + "in Json::Value::duplicateAndPrefixStringValue(): " + "length too big for prefixing"); + size_t actualLength = sizeof(length) + length + 1; + auto newString = static_cast(malloc(actualLength)); + if (newString == nullptr) { + throwRuntimeError("in Json::Value::duplicateAndPrefixStringValue(): " + "Failed to allocate string value buffer"); + } + *reinterpret_cast(newString) = length; + memcpy(newString + sizeof(unsigned), value, length); + newString[actualLength - 1U] = + 0; // to avoid buffer over-run accidents by users later + return newString; +} +inline static void decodePrefixedString(bool isPrefixed, char const* prefixed, + unsigned* length, char const** value) { + if (!isPrefixed) { + *length = static_cast(strlen(prefixed)); + *value = prefixed; + } else { + *length = *reinterpret_cast(prefixed); + *value = prefixed + sizeof(unsigned); + } +} +/** Free the string duplicated by + * duplicateStringValue()/duplicateAndPrefixStringValue(). + */ +#if JSONCPP_USING_SECURE_MEMORY +static inline void releasePrefixedStringValue(char* value) { + unsigned length = 0; + char const* valueDecoded; + decodePrefixedString(true, value, &length, &valueDecoded); + size_t const size = sizeof(unsigned) + length + 1U; + memset(value, 0, size); + free(value); +} +static inline void releaseStringValue(char* value, unsigned length) { + // length==0 => we allocated the strings memory + size_t size = (length == 0) ? strlen(value) : length; + memset(value, 0, size); + free(value); +} +#else // !JSONCPP_USING_SECURE_MEMORY +static inline void releasePrefixedStringValue(char* value) { free(value); } +static inline void releaseStringValue(char* value, unsigned) { free(value); } +#endif // JSONCPP_USING_SECURE_MEMORY + +} // namespace Json + +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ValueInternals... +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +#if !defined(JSON_IS_AMALGAMATION) + +#include "json_valueiterator.inl" +#endif // if !defined(JSON_IS_AMALGAMATION) + +namespace Json { + +#if JSON_USE_EXCEPTION +Exception::Exception(String msg) : msg_(std::move(msg)) {} +Exception::~Exception() noexcept = default; +char const* Exception::what() const noexcept { return msg_.c_str(); } +RuntimeError::RuntimeError(String const& msg) : Exception(msg) {} +LogicError::LogicError(String const& msg) : Exception(msg) {} +JSONCPP_NORETURN void throwRuntimeError(String const& msg) { + throw RuntimeError(msg); +} +JSONCPP_NORETURN void throwLogicError(String const& msg) { + throw LogicError(msg); +} +#else // !JSON_USE_EXCEPTION +JSONCPP_NORETURN void throwRuntimeError(String const& msg) { + std::cerr << msg << std::endl; + abort(); +} +JSONCPP_NORETURN void throwLogicError(String const& msg) { + std::cerr << msg << std::endl; + abort(); +} +#endif + +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// class Value::CZString +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// + +// Notes: policy_ indicates if the string was allocated when +// a string is stored. + +Value::CZString::CZString(ArrayIndex index) : cstr_(nullptr), index_(index) {} + +Value::CZString::CZString(char const* str, unsigned length, + DuplicationPolicy allocate) + : cstr_(str) { + // allocate != duplicate + storage_.policy_ = allocate & 0x3; + storage_.length_ = length & 0x3FFFFFFF; +} + +Value::CZString::CZString(const CZString& other) { + cstr_ = (other.storage_.policy_ != noDuplication && other.cstr_ != nullptr + ? duplicateStringValue(other.cstr_, other.storage_.length_) + : other.cstr_); + storage_.policy_ = + static_cast( + other.cstr_ + ? (static_cast(other.storage_.policy_) == + noDuplication + ? noDuplication + : duplicate) + : static_cast(other.storage_.policy_)) & + 3U; + storage_.length_ = other.storage_.length_; +} + +Value::CZString::CZString(CZString&& other) noexcept + : cstr_(other.cstr_), index_(other.index_) { + other.cstr_ = nullptr; +} + +Value::CZString::~CZString() { + if (cstr_ && storage_.policy_ == duplicate) { + releaseStringValue(const_cast(cstr_), + storage_.length_ + 1U); // +1 for null terminating + // character for sake of + // completeness but not actually + // necessary + } +} + +void Value::CZString::swap(CZString& other) { + std::swap(cstr_, other.cstr_); + std::swap(index_, other.index_); +} + +Value::CZString& Value::CZString::operator=(const CZString& other) { + cstr_ = other.cstr_; + index_ = other.index_; + return *this; +} + +Value::CZString& Value::CZString::operator=(CZString&& other) noexcept { + cstr_ = other.cstr_; + index_ = other.index_; + other.cstr_ = nullptr; + return *this; +} + +bool Value::CZString::operator<(const CZString& other) const { + if (!cstr_) + return index_ < other.index_; + // return strcmp(cstr_, other.cstr_) < 0; + // Assume both are strings. + unsigned this_len = this->storage_.length_; + unsigned other_len = other.storage_.length_; + unsigned min_len = std::min(this_len, other_len); + JSON_ASSERT(this->cstr_ && other.cstr_); + int comp = memcmp(this->cstr_, other.cstr_, min_len); + if (comp < 0) + return true; + if (comp > 0) + return false; + return (this_len < other_len); +} + +bool Value::CZString::operator==(const CZString& other) const { + if (!cstr_) + return index_ == other.index_; + // return strcmp(cstr_, other.cstr_) == 0; + // Assume both are strings. + unsigned this_len = this->storage_.length_; + unsigned other_len = other.storage_.length_; + if (this_len != other_len) + return false; + JSON_ASSERT(this->cstr_ && other.cstr_); + int comp = memcmp(this->cstr_, other.cstr_, this_len); + return comp == 0; +} + +ArrayIndex Value::CZString::index() const { return index_; } + +// const char* Value::CZString::c_str() const { return cstr_; } +const char* Value::CZString::data() const { return cstr_; } +unsigned Value::CZString::length() const { return storage_.length_; } +bool Value::CZString::isStaticString() const { + return storage_.policy_ == noDuplication; +} + +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// class Value::Value +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// + +/*! \internal Default constructor initialization must be equivalent to: + * memset( this, 0, sizeof(Value) ) + * This optimization is used in ValueInternalMap fast allocator. + */ +Value::Value(ValueType type) { + static char const emptyString[] = ""; + initBasic(type); + switch (type) { + case nullValue: + break; + case intValue: + case uintValue: + value_.int_ = 0; + break; + case realValue: + value_.real_ = 0.0; + break; + case stringValue: + // allocated_ == false, so this is safe. + value_.string_ = const_cast(static_cast(emptyString)); + break; + case arrayValue: + case objectValue: + value_.map_ = new ObjectValues(); + break; + case booleanValue: + value_.bool_ = false; + break; + default: + JSON_ASSERT_UNREACHABLE; + } +} + +Value::Value(Int value) { + initBasic(intValue); + value_.int_ = value; +} + +Value::Value(UInt value) { + initBasic(uintValue); + value_.uint_ = value; +} +#if defined(JSON_HAS_INT64) +Value::Value(Int64 value) { + initBasic(intValue); + value_.int_ = value; +} +Value::Value(UInt64 value) { + initBasic(uintValue); + value_.uint_ = value; +} +#endif // defined(JSON_HAS_INT64) + +Value::Value(double value) { + initBasic(realValue); + value_.real_ = value; +} + +Value::Value(const char* value) { + initBasic(stringValue, true); + JSON_ASSERT_MESSAGE(value != nullptr, + "Null Value Passed to Value Constructor"); + value_.string_ = duplicateAndPrefixStringValue( + value, static_cast(strlen(value))); +} + +Value::Value(const char* begin, const char* end) { + initBasic(stringValue, true); + value_.string_ = + duplicateAndPrefixStringValue(begin, static_cast(end - begin)); +} + +Value::Value(const String& value) { + initBasic(stringValue, true); + value_.string_ = duplicateAndPrefixStringValue( + value.data(), static_cast(value.length())); +} + +Value::Value(const StaticString& value) { + initBasic(stringValue); + value_.string_ = const_cast(value.c_str()); +} + +Value::Value(bool value) { + initBasic(booleanValue); + value_.bool_ = value; +} + +Value::Value(const Value& other) { + dupPayload(other); + dupMeta(other); +} + +Value::Value(Value&& other) noexcept { + initBasic(nullValue); + swap(other); +} + +Value::~Value() { + releasePayload(); + value_.uint_ = 0; +} + +Value& Value::operator=(const Value& other) { + Value(other).swap(*this); + return *this; +} + +Value& Value::operator=(Value&& other) noexcept { + other.swap(*this); + return *this; +} + +void Value::swapPayload(Value& other) { + std::swap(bits_, other.bits_); + std::swap(value_, other.value_); +} + +void Value::copyPayload(const Value& other) { + releasePayload(); + dupPayload(other); +} + +void Value::swap(Value& other) { + swapPayload(other); + std::swap(comments_, other.comments_); + std::swap(start_, other.start_); + std::swap(limit_, other.limit_); +} + +void Value::copy(const Value& other) { + copyPayload(other); + dupMeta(other); +} + +ValueType Value::type() const { + return static_cast(bits_.value_type_); +} + +int Value::compare(const Value& other) const { + if (*this < other) + return -1; + if (*this > other) + return 1; + return 0; +} + +bool Value::operator<(const Value& other) const { + int typeDelta = type() - other.type(); + if (typeDelta) + return typeDelta < 0; + switch (type()) { + case nullValue: + return false; + case intValue: + return value_.int_ < other.value_.int_; + case uintValue: + return value_.uint_ < other.value_.uint_; + case realValue: + return value_.real_ < other.value_.real_; + case booleanValue: + return value_.bool_ < other.value_.bool_; + case stringValue: { + if ((value_.string_ == nullptr) || (other.value_.string_ == nullptr)) { + return other.value_.string_ != nullptr; + } + unsigned this_len; + unsigned other_len; + char const* this_str; + char const* other_str; + decodePrefixedString(this->isAllocated(), this->value_.string_, &this_len, + &this_str); + decodePrefixedString(other.isAllocated(), other.value_.string_, &other_len, + &other_str); + unsigned min_len = std::min(this_len, other_len); + JSON_ASSERT(this_str && other_str); + int comp = memcmp(this_str, other_str, min_len); + if (comp < 0) + return true; + if (comp > 0) + return false; + return (this_len < other_len); + } + case arrayValue: + case objectValue: { + auto thisSize = value_.map_->size(); + auto otherSize = other.value_.map_->size(); + if (thisSize != otherSize) + return thisSize < otherSize; + return (*value_.map_) < (*other.value_.map_); + } + default: + JSON_ASSERT_UNREACHABLE; + } + return false; // unreachable +} + +bool Value::operator<=(const Value& other) const { return !(other < *this); } + +bool Value::operator>=(const Value& other) const { return !(*this < other); } + +bool Value::operator>(const Value& other) const { return other < *this; } + +bool Value::operator==(const Value& other) const { + if (type() != other.type()) + return false; + switch (type()) { + case nullValue: + return true; + case intValue: + return value_.int_ == other.value_.int_; + case uintValue: + return value_.uint_ == other.value_.uint_; + case realValue: + return value_.real_ == other.value_.real_; + case booleanValue: + return value_.bool_ == other.value_.bool_; + case stringValue: { + if ((value_.string_ == nullptr) || (other.value_.string_ == nullptr)) { + return (value_.string_ == other.value_.string_); + } + unsigned this_len; + unsigned other_len; + char const* this_str; + char const* other_str; + decodePrefixedString(this->isAllocated(), this->value_.string_, &this_len, + &this_str); + decodePrefixedString(other.isAllocated(), other.value_.string_, &other_len, + &other_str); + if (this_len != other_len) + return false; + JSON_ASSERT(this_str && other_str); + int comp = memcmp(this_str, other_str, this_len); + return comp == 0; + } + case arrayValue: + case objectValue: + return value_.map_->size() == other.value_.map_->size() && + (*value_.map_) == (*other.value_.map_); + default: + JSON_ASSERT_UNREACHABLE; + } + return false; // unreachable +} + +bool Value::operator!=(const Value& other) const { return !(*this == other); } + +const char* Value::asCString() const { + JSON_ASSERT_MESSAGE(type() == stringValue, + "in Json::Value::asCString(): requires stringValue"); + if (value_.string_ == nullptr) + return nullptr; + unsigned this_len; + char const* this_str; + decodePrefixedString(this->isAllocated(), this->value_.string_, &this_len, + &this_str); + return this_str; +} + +#if JSONCPP_USING_SECURE_MEMORY +unsigned Value::getCStringLength() const { + JSON_ASSERT_MESSAGE(type() == stringValue, + "in Json::Value::asCString(): requires stringValue"); + if (value_.string_ == 0) + return 0; + unsigned this_len; + char const* this_str; + decodePrefixedString(this->isAllocated(), this->value_.string_, &this_len, + &this_str); + return this_len; +} +#endif + +bool Value::getString(char const** begin, char const** end) const { + if (type() != stringValue) + return false; + if (value_.string_ == nullptr) + return false; + unsigned length; + decodePrefixedString(this->isAllocated(), this->value_.string_, &length, + begin); + *end = *begin + length; + return true; +} + +String Value::asString() const { + switch (type()) { + case nullValue: + return ""; + case stringValue: { + if (value_.string_ == nullptr) + return ""; + unsigned this_len; + char const* this_str; + decodePrefixedString(this->isAllocated(), this->value_.string_, &this_len, + &this_str); + return String(this_str, this_len); + } + case booleanValue: + return value_.bool_ ? "true" : "false"; + case intValue: + return valueToString(value_.int_); + case uintValue: + return valueToString(value_.uint_); + case realValue: + return valueToString(value_.real_); + default: + JSON_FAIL_MESSAGE("Type is not convertible to string"); + } +} + +Value::Int Value::asInt() const { + switch (type()) { + case intValue: + JSON_ASSERT_MESSAGE(isInt(), "LargestInt out of Int range"); + return Int(value_.int_); + case uintValue: + JSON_ASSERT_MESSAGE(isInt(), "LargestUInt out of Int range"); + return Int(value_.uint_); + case realValue: + JSON_ASSERT_MESSAGE(InRange(value_.real_, minInt, maxInt), + "double out of Int range"); + return Int(value_.real_); + case nullValue: + return 0; + case booleanValue: + return value_.bool_ ? 1 : 0; + default: + break; + } + JSON_FAIL_MESSAGE("Value is not convertible to Int."); +} + +Value::UInt Value::asUInt() const { + switch (type()) { + case intValue: + JSON_ASSERT_MESSAGE(isUInt(), "LargestInt out of UInt range"); + return UInt(value_.int_); + case uintValue: + JSON_ASSERT_MESSAGE(isUInt(), "LargestUInt out of UInt range"); + return UInt(value_.uint_); + case realValue: + JSON_ASSERT_MESSAGE(InRange(value_.real_, 0, maxUInt), + "double out of UInt range"); + return UInt(value_.real_); + case nullValue: + return 0; + case booleanValue: + return value_.bool_ ? 1 : 0; + default: + break; + } + JSON_FAIL_MESSAGE("Value is not convertible to UInt."); +} + +#if defined(JSON_HAS_INT64) + +Value::Int64 Value::asInt64() const { + switch (type()) { + case intValue: + return Int64(value_.int_); + case uintValue: + JSON_ASSERT_MESSAGE(isInt64(), "LargestUInt out of Int64 range"); + return Int64(value_.uint_); + case realValue: + JSON_ASSERT_MESSAGE(InRange(value_.real_, minInt64, maxInt64), + "double out of Int64 range"); + return Int64(value_.real_); + case nullValue: + return 0; + case booleanValue: + return value_.bool_ ? 1 : 0; + default: + break; + } + JSON_FAIL_MESSAGE("Value is not convertible to Int64."); +} + +Value::UInt64 Value::asUInt64() const { + switch (type()) { + case intValue: + JSON_ASSERT_MESSAGE(isUInt64(), "LargestInt out of UInt64 range"); + return UInt64(value_.int_); + case uintValue: + return UInt64(value_.uint_); + case realValue: + JSON_ASSERT_MESSAGE(InRange(value_.real_, 0, maxUInt64), + "double out of UInt64 range"); + return UInt64(value_.real_); + case nullValue: + return 0; + case booleanValue: + return value_.bool_ ? 1 : 0; + default: + break; + } + JSON_FAIL_MESSAGE("Value is not convertible to UInt64."); +} +#endif // if defined(JSON_HAS_INT64) + +LargestInt Value::asLargestInt() const { +#if defined(JSON_NO_INT64) + return asInt(); +#else + return asInt64(); +#endif +} + +LargestUInt Value::asLargestUInt() const { +#if defined(JSON_NO_INT64) + return asUInt(); +#else + return asUInt64(); +#endif +} + +double Value::asDouble() const { + switch (type()) { + case intValue: + return static_cast(value_.int_); + case uintValue: +#if !defined(JSON_USE_INT64_DOUBLE_CONVERSION) + return static_cast(value_.uint_); +#else // if !defined(JSON_USE_INT64_DOUBLE_CONVERSION) + return integerToDouble(value_.uint_); +#endif // if !defined(JSON_USE_INT64_DOUBLE_CONVERSION) + case realValue: + return value_.real_; + case nullValue: + return 0.0; + case booleanValue: + return value_.bool_ ? 1.0 : 0.0; + default: + break; + } + JSON_FAIL_MESSAGE("Value is not convertible to double."); +} + +float Value::asFloat() const { + switch (type()) { + case intValue: + return static_cast(value_.int_); + case uintValue: +#if !defined(JSON_USE_INT64_DOUBLE_CONVERSION) + return static_cast(value_.uint_); +#else // if !defined(JSON_USE_INT64_DOUBLE_CONVERSION) + // This can fail (silently?) if the value is bigger than MAX_FLOAT. + return static_cast(integerToDouble(value_.uint_)); +#endif // if !defined(JSON_USE_INT64_DOUBLE_CONVERSION) + case realValue: + return static_cast(value_.real_); + case nullValue: + return 0.0; + case booleanValue: + return value_.bool_ ? 1.0F : 0.0F; + default: + break; + } + JSON_FAIL_MESSAGE("Value is not convertible to float."); +} + +bool Value::asBool() const { + switch (type()) { + case booleanValue: + return value_.bool_; + case nullValue: + return false; + case intValue: + return value_.int_ != 0; + case uintValue: + return value_.uint_ != 0; + case realValue: { + // According to JavaScript language zero or NaN is regarded as false + const auto value_classification = std::fpclassify(value_.real_); + return value_classification != FP_ZERO && value_classification != FP_NAN; + } + default: + break; + } + JSON_FAIL_MESSAGE("Value is not convertible to bool."); +} + +bool Value::isConvertibleTo(ValueType other) const { + switch (other) { + case nullValue: + return (isNumeric() && asDouble() == 0.0) || + (type() == booleanValue && !value_.bool_) || + (type() == stringValue && asString().empty()) || + (type() == arrayValue && value_.map_->empty()) || + (type() == objectValue && value_.map_->empty()) || + type() == nullValue; + case intValue: + return isInt() || + (type() == realValue && InRange(value_.real_, minInt, maxInt)) || + type() == booleanValue || type() == nullValue; + case uintValue: + return isUInt() || + (type() == realValue && InRange(value_.real_, 0, maxUInt)) || + type() == booleanValue || type() == nullValue; + case realValue: + return isNumeric() || type() == booleanValue || type() == nullValue; + case booleanValue: + return isNumeric() || type() == booleanValue || type() == nullValue; + case stringValue: + return isNumeric() || type() == booleanValue || type() == stringValue || + type() == nullValue; + case arrayValue: + return type() == arrayValue || type() == nullValue; + case objectValue: + return type() == objectValue || type() == nullValue; + } + JSON_ASSERT_UNREACHABLE; + return false; +} + +/// Number of values in array or object +ArrayIndex Value::size() const { + switch (type()) { + case nullValue: + case intValue: + case uintValue: + case realValue: + case booleanValue: + case stringValue: + return 0; + case arrayValue: // size of the array is highest index + 1 + if (!value_.map_->empty()) { + ObjectValues::const_iterator itLast = value_.map_->end(); + --itLast; + return (*itLast).first.index() + 1; + } + return 0; + case objectValue: + return ArrayIndex(value_.map_->size()); + } + JSON_ASSERT_UNREACHABLE; + return 0; // unreachable; +} + +bool Value::empty() const { + if (isNull() || isArray() || isObject()) + return size() == 0U; + return false; +} + +Value::operator bool() const { return !isNull(); } + +void Value::clear() { + JSON_ASSERT_MESSAGE(type() == nullValue || type() == arrayValue || + type() == objectValue, + "in Json::Value::clear(): requires complex value"); + start_ = 0; + limit_ = 0; + switch (type()) { + case arrayValue: + case objectValue: + value_.map_->clear(); + break; + default: + break; + } +} + +void Value::resize(ArrayIndex newSize) { + JSON_ASSERT_MESSAGE(type() == nullValue || type() == arrayValue, + "in Json::Value::resize(): requires arrayValue"); + if (type() == nullValue) + *this = Value(arrayValue); + ArrayIndex oldSize = size(); + if (newSize == 0) + clear(); + else if (newSize > oldSize) + for (ArrayIndex i = oldSize; i < newSize; ++i) + (*this)[i]; + else { + for (ArrayIndex index = newSize; index < oldSize; ++index) { + value_.map_->erase(index); + } + JSON_ASSERT(size() == newSize); + } +} + +Value& Value::operator[](ArrayIndex index) { + JSON_ASSERT_MESSAGE( + type() == nullValue || type() == arrayValue, + "in Json::Value::operator[](ArrayIndex): requires arrayValue"); + if (type() == nullValue) + *this = Value(arrayValue); + CZString key(index); + auto it = value_.map_->lower_bound(key); + if (it != value_.map_->end() && (*it).first == key) + return (*it).second; + + ObjectValues::value_type defaultValue(key, nullSingleton()); + it = value_.map_->insert(it, defaultValue); + return (*it).second; +} + +Value& Value::operator[](int index) { + JSON_ASSERT_MESSAGE( + index >= 0, + "in Json::Value::operator[](int index): index cannot be negative"); + return (*this)[ArrayIndex(index)]; +} + +const Value& Value::operator[](ArrayIndex index) const { + JSON_ASSERT_MESSAGE( + type() == nullValue || type() == arrayValue, + "in Json::Value::operator[](ArrayIndex)const: requires arrayValue"); + if (type() == nullValue) + return nullSingleton(); + CZString key(index); + ObjectValues::const_iterator it = value_.map_->find(key); + if (it == value_.map_->end()) + return nullSingleton(); + return (*it).second; +} + +const Value& Value::operator[](int index) const { + JSON_ASSERT_MESSAGE( + index >= 0, + "in Json::Value::operator[](int index) const: index cannot be negative"); + return (*this)[ArrayIndex(index)]; +} + +void Value::initBasic(ValueType type, bool allocated) { + setType(type); + setIsAllocated(allocated); + comments_ = Comments{}; + start_ = 0; + limit_ = 0; +} + +void Value::dupPayload(const Value& other) { + setType(other.type()); + setIsAllocated(false); + switch (type()) { + case nullValue: + case intValue: + case uintValue: + case realValue: + case booleanValue: + value_ = other.value_; + break; + case stringValue: + if (other.value_.string_ && other.isAllocated()) { + unsigned len; + char const* str; + decodePrefixedString(other.isAllocated(), other.value_.string_, &len, + &str); + value_.string_ = duplicateAndPrefixStringValue(str, len); + setIsAllocated(true); + } else { + value_.string_ = other.value_.string_; + } + break; + case arrayValue: + case objectValue: + value_.map_ = new ObjectValues(*other.value_.map_); + break; + default: + JSON_ASSERT_UNREACHABLE; + } +} + +void Value::releasePayload() { + switch (type()) { + case nullValue: + case intValue: + case uintValue: + case realValue: + case booleanValue: + break; + case stringValue: + if (isAllocated()) + releasePrefixedStringValue(value_.string_); + break; + case arrayValue: + case objectValue: + delete value_.map_; + break; + default: + JSON_ASSERT_UNREACHABLE; + } +} + +void Value::dupMeta(const Value& other) { + comments_ = other.comments_; + start_ = other.start_; + limit_ = other.limit_; +} + +// Access an object value by name, create a null member if it does not exist. +// @pre Type of '*this' is object or null. +// @param key is null-terminated. +Value& Value::resolveReference(const char* key) { + JSON_ASSERT_MESSAGE( + type() == nullValue || type() == objectValue, + "in Json::Value::resolveReference(): requires objectValue"); + if (type() == nullValue) + *this = Value(objectValue); + CZString actualKey(key, static_cast(strlen(key)), + CZString::noDuplication); // NOTE! + auto it = value_.map_->lower_bound(actualKey); + if (it != value_.map_->end() && (*it).first == actualKey) + return (*it).second; + + ObjectValues::value_type defaultValue(actualKey, nullSingleton()); + it = value_.map_->insert(it, defaultValue); + Value& value = (*it).second; + return value; +} + +// @param key is not null-terminated. +Value& Value::resolveReference(char const* key, char const* end) { + JSON_ASSERT_MESSAGE( + type() == nullValue || type() == objectValue, + "in Json::Value::resolveReference(key, end): requires objectValue"); + if (type() == nullValue) + *this = Value(objectValue); + CZString actualKey(key, static_cast(end - key), + CZString::duplicateOnCopy); + auto it = value_.map_->lower_bound(actualKey); + if (it != value_.map_->end() && (*it).first == actualKey) + return (*it).second; + + ObjectValues::value_type defaultValue(actualKey, nullSingleton()); + it = value_.map_->insert(it, defaultValue); + Value& value = (*it).second; + return value; +} + +Value Value::get(ArrayIndex index, const Value& defaultValue) const { + const Value* value = &((*this)[index]); + return value == &nullSingleton() ? defaultValue : *value; +} + +bool Value::isValidIndex(ArrayIndex index) const { return index < size(); } + +Value const* Value::find(char const* begin, char const* end) const { + JSON_ASSERT_MESSAGE(type() == nullValue || type() == objectValue, + "in Json::Value::find(begin, end): requires " + "objectValue or nullValue"); + if (type() == nullValue) + return nullptr; + CZString actualKey(begin, static_cast(end - begin), + CZString::noDuplication); + ObjectValues::const_iterator it = value_.map_->find(actualKey); + if (it == value_.map_->end()) + return nullptr; + return &(*it).second; +} +Value* Value::demand(char const* begin, char const* end) { + JSON_ASSERT_MESSAGE(type() == nullValue || type() == objectValue, + "in Json::Value::demand(begin, end): requires " + "objectValue or nullValue"); + return &resolveReference(begin, end); +} +const Value& Value::operator[](const char* key) const { + Value const* found = find(key, key + strlen(key)); + if (!found) + return nullSingleton(); + return *found; +} +Value const& Value::operator[](const String& key) const { + Value const* found = find(key.data(), key.data() + key.length()); + if (!found) + return nullSingleton(); + return *found; +} + +Value& Value::operator[](const char* key) { + return resolveReference(key, key + strlen(key)); +} + +Value& Value::operator[](const String& key) { + return resolveReference(key.data(), key.data() + key.length()); +} + +Value& Value::operator[](const StaticString& key) { + return resolveReference(key.c_str()); +} + +Value& Value::append(const Value& value) { return append(Value(value)); } + +Value& Value::append(Value&& value) { + JSON_ASSERT_MESSAGE(type() == nullValue || type() == arrayValue, + "in Json::Value::append: requires arrayValue"); + if (type() == nullValue) { + *this = Value(arrayValue); + } + return this->value_.map_->emplace(size(), std::move(value)).first->second; +} + +bool Value::insert(ArrayIndex index, const Value& newValue) { + return insert(index, Value(newValue)); +} + +bool Value::insert(ArrayIndex index, Value&& newValue) { + JSON_ASSERT_MESSAGE(type() == nullValue || type() == arrayValue, + "in Json::Value::insert: requires arrayValue"); + ArrayIndex length = size(); + if (index > length) { + return false; + } + for (ArrayIndex i = length; i > index; i--) { + (*this)[i] = std::move((*this)[i - 1]); + } + (*this)[index] = std::move(newValue); + return true; +} + +Value Value::get(char const* begin, char const* end, + Value const& defaultValue) const { + Value const* found = find(begin, end); + return !found ? defaultValue : *found; +} +Value Value::get(char const* key, Value const& defaultValue) const { + return get(key, key + strlen(key), defaultValue); +} +Value Value::get(String const& key, Value const& defaultValue) const { + return get(key.data(), key.data() + key.length(), defaultValue); +} + +bool Value::removeMember(const char* begin, const char* end, Value* removed) { + if (type() != objectValue) { + return false; + } + CZString actualKey(begin, static_cast(end - begin), + CZString::noDuplication); + auto it = value_.map_->find(actualKey); + if (it == value_.map_->end()) + return false; + if (removed) + *removed = std::move(it->second); + value_.map_->erase(it); + return true; +} +bool Value::removeMember(const char* key, Value* removed) { + return removeMember(key, key + strlen(key), removed); +} +bool Value::removeMember(String const& key, Value* removed) { + return removeMember(key.data(), key.data() + key.length(), removed); +} +void Value::removeMember(const char* key) { + JSON_ASSERT_MESSAGE(type() == nullValue || type() == objectValue, + "in Json::Value::removeMember(): requires objectValue"); + if (type() == nullValue) + return; + + CZString actualKey(key, unsigned(strlen(key)), CZString::noDuplication); + value_.map_->erase(actualKey); +} +void Value::removeMember(const String& key) { removeMember(key.c_str()); } + +bool Value::removeIndex(ArrayIndex index, Value* removed) { + if (type() != arrayValue) { + return false; + } + CZString key(index); + auto it = value_.map_->find(key); + if (it == value_.map_->end()) { + return false; + } + if (removed) + *removed = it->second; + ArrayIndex oldSize = size(); + // shift left all items left, into the place of the "removed" + for (ArrayIndex i = index; i < (oldSize - 1); ++i) { + CZString keey(i); + (*value_.map_)[keey] = (*this)[i + 1]; + } + // erase the last one ("leftover") + CZString keyLast(oldSize - 1); + auto itLast = value_.map_->find(keyLast); + value_.map_->erase(itLast); + return true; +} + +bool Value::isMember(char const* begin, char const* end) const { + Value const* value = find(begin, end); + return nullptr != value; +} +bool Value::isMember(char const* key) const { + return isMember(key, key + strlen(key)); +} +bool Value::isMember(String const& key) const { + return isMember(key.data(), key.data() + key.length()); +} + +Value::Members Value::getMemberNames() const { + JSON_ASSERT_MESSAGE( + type() == nullValue || type() == objectValue, + "in Json::Value::getMemberNames(), value must be objectValue"); + if (type() == nullValue) + return Value::Members(); + Members members; + members.reserve(value_.map_->size()); + ObjectValues::const_iterator it = value_.map_->begin(); + ObjectValues::const_iterator itEnd = value_.map_->end(); + for (; it != itEnd; ++it) { + members.push_back(String((*it).first.data(), (*it).first.length())); + } + return members; +} + +static bool IsIntegral(double d) { + double integral_part; + return modf(d, &integral_part) == 0.0; +} + +bool Value::isNull() const { return type() == nullValue; } + +bool Value::isBool() const { return type() == booleanValue; } + +bool Value::isInt() const { + switch (type()) { + case intValue: +#if defined(JSON_HAS_INT64) + return value_.int_ >= minInt && value_.int_ <= maxInt; +#else + return true; +#endif + case uintValue: + return value_.uint_ <= UInt(maxInt); + case realValue: + return value_.real_ >= minInt && value_.real_ <= maxInt && + IsIntegral(value_.real_); + default: + break; + } + return false; +} + +bool Value::isUInt() const { + switch (type()) { + case intValue: +#if defined(JSON_HAS_INT64) + return value_.int_ >= 0 && LargestUInt(value_.int_) <= LargestUInt(maxUInt); +#else + return value_.int_ >= 0; +#endif + case uintValue: +#if defined(JSON_HAS_INT64) + return value_.uint_ <= maxUInt; +#else + return true; +#endif + case realValue: + return value_.real_ >= 0 && value_.real_ <= maxUInt && + IsIntegral(value_.real_); + default: + break; + } + return false; +} + +bool Value::isInt64() const { +#if defined(JSON_HAS_INT64) + switch (type()) { + case intValue: + return true; + case uintValue: + return value_.uint_ <= UInt64(maxInt64); + case realValue: + // Note that maxInt64 (= 2^63 - 1) is not exactly representable as a + // double, so double(maxInt64) will be rounded up to 2^63. Therefore we + // require the value to be strictly less than the limit. + return value_.real_ >= double(minInt64) && + value_.real_ < double(maxInt64) && IsIntegral(value_.real_); + default: + break; + } +#endif // JSON_HAS_INT64 + return false; +} + +bool Value::isUInt64() const { +#if defined(JSON_HAS_INT64) + switch (type()) { + case intValue: + return value_.int_ >= 0; + case uintValue: + return true; + case realValue: + // Note that maxUInt64 (= 2^64 - 1) is not exactly representable as a + // double, so double(maxUInt64) will be rounded up to 2^64. Therefore we + // require the value to be strictly less than the limit. + return value_.real_ >= 0 && value_.real_ < maxUInt64AsDouble && + IsIntegral(value_.real_); + default: + break; + } +#endif // JSON_HAS_INT64 + return false; +} + +bool Value::isIntegral() const { + switch (type()) { + case intValue: + case uintValue: + return true; + case realValue: +#if defined(JSON_HAS_INT64) + // Note that maxUInt64 (= 2^64 - 1) is not exactly representable as a + // double, so double(maxUInt64) will be rounded up to 2^64. Therefore we + // require the value to be strictly less than the limit. + return value_.real_ >= double(minInt64) && + value_.real_ < maxUInt64AsDouble && IsIntegral(value_.real_); +#else + return value_.real_ >= minInt && value_.real_ <= maxUInt && + IsIntegral(value_.real_); +#endif // JSON_HAS_INT64 + default: + break; + } + return false; +} + +bool Value::isDouble() const { + return type() == intValue || type() == uintValue || type() == realValue; +} + +bool Value::isNumeric() const { return isDouble(); } + +bool Value::isString() const { return type() == stringValue; } + +bool Value::isArray() const { return type() == arrayValue; } + +bool Value::isObject() const { return type() == objectValue; } + +Value::Comments::Comments(const Comments& that) + : ptr_{cloneUnique(that.ptr_)} {} + +Value::Comments::Comments(Comments&& that) noexcept + : ptr_{std::move(that.ptr_)} {} + +Value::Comments& Value::Comments::operator=(const Comments& that) { + ptr_ = cloneUnique(that.ptr_); + return *this; +} + +Value::Comments& Value::Comments::operator=(Comments&& that) noexcept { + ptr_ = std::move(that.ptr_); + return *this; +} + +bool Value::Comments::has(CommentPlacement slot) const { + return ptr_ && !(*ptr_)[slot].empty(); +} + +String Value::Comments::get(CommentPlacement slot) const { + if (!ptr_) + return {}; + return (*ptr_)[slot]; +} + +void Value::Comments::set(CommentPlacement slot, String comment) { + if (slot >= CommentPlacement::numberOfCommentPlacement) + return; + if (!ptr_) + ptr_ = std::unique_ptr(new Array()); + (*ptr_)[slot] = std::move(comment); +} + +void Value::setComment(String comment, CommentPlacement placement) { + if (!comment.empty() && (comment.back() == '\n')) { + // Always discard trailing newline, to aid indentation. + comment.pop_back(); + } + JSON_ASSERT(!comment.empty()); + JSON_ASSERT_MESSAGE( + comment[0] == '\0' || comment[0] == '/', + "in Json::Value::setComment(): Comments must start with /"); + comments_.set(placement, std::move(comment)); +} + +bool Value::hasComment(CommentPlacement placement) const { + return comments_.has(placement); +} + +String Value::getComment(CommentPlacement placement) const { + return comments_.get(placement); +} + +void Value::setOffsetStart(ptrdiff_t start) { start_ = start; } + +void Value::setOffsetLimit(ptrdiff_t limit) { limit_ = limit; } + +ptrdiff_t Value::getOffsetStart() const { return start_; } + +ptrdiff_t Value::getOffsetLimit() const { return limit_; } + +String Value::toStyledString() const { + StreamWriterBuilder builder; + + String out = this->hasComment(commentBefore) ? "\n" : ""; + out += Json::writeString(builder, *this); + out += '\n'; + + return out; +} + +Value::const_iterator Value::begin() const { + switch (type()) { + case arrayValue: + case objectValue: + if (value_.map_) + return const_iterator(value_.map_->begin()); + break; + default: + break; + } + return {}; +} + +Value::const_iterator Value::end() const { + switch (type()) { + case arrayValue: + case objectValue: + if (value_.map_) + return const_iterator(value_.map_->end()); + break; + default: + break; + } + return {}; +} + +Value::iterator Value::begin() { + switch (type()) { + case arrayValue: + case objectValue: + if (value_.map_) + return iterator(value_.map_->begin()); + break; + default: + break; + } + return iterator(); +} + +Value::iterator Value::end() { + switch (type()) { + case arrayValue: + case objectValue: + if (value_.map_) + return iterator(value_.map_->end()); + break; + default: + break; + } + return iterator(); +} + +// class PathArgument +// ////////////////////////////////////////////////////////////////// + +PathArgument::PathArgument() = default; + +PathArgument::PathArgument(ArrayIndex index) + : index_(index), kind_(kindIndex) {} + +PathArgument::PathArgument(const char* key) : key_(key), kind_(kindKey) {} + +PathArgument::PathArgument(String key) : key_(std::move(key)), kind_(kindKey) {} + +// class Path +// ////////////////////////////////////////////////////////////////// + +Path::Path(const String& path, const PathArgument& a1, const PathArgument& a2, + const PathArgument& a3, const PathArgument& a4, + const PathArgument& a5) { + InArgs in; + in.reserve(5); + in.push_back(&a1); + in.push_back(&a2); + in.push_back(&a3); + in.push_back(&a4); + in.push_back(&a5); + makePath(path, in); +} + +void Path::makePath(const String& path, const InArgs& in) { + const char* current = path.c_str(); + const char* end = current + path.length(); + auto itInArg = in.begin(); + while (current != end) { + if (*current == '[') { + ++current; + if (*current == '%') + addPathInArg(path, in, itInArg, PathArgument::kindIndex); + else { + ArrayIndex index = 0; + for (; current != end && *current >= '0' && *current <= '9'; ++current) + index = index * 10 + ArrayIndex(*current - '0'); + args_.push_back(index); + } + if (current == end || *++current != ']') + invalidPath(path, int(current - path.c_str())); + } else if (*current == '%') { + addPathInArg(path, in, itInArg, PathArgument::kindKey); + ++current; + } else if (*current == '.' || *current == ']') { + ++current; + } else { + const char* beginName = current; + while (current != end && !strchr("[.", *current)) + ++current; + args_.push_back(String(beginName, current)); + } + } +} + +void Path::addPathInArg(const String& /*path*/, const InArgs& in, + InArgs::const_iterator& itInArg, + PathArgument::Kind kind) { + if (itInArg == in.end()) { + // Error: missing argument %d + } else if ((*itInArg)->kind_ != kind) { + // Error: bad argument type + } else { + args_.push_back(**itInArg++); + } +} + +void Path::invalidPath(const String& /*path*/, int /*location*/) { + // Error: invalid path. +} + +const Value& Path::resolve(const Value& root) const { + const Value* node = &root; + for (const auto& arg : args_) { + if (arg.kind_ == PathArgument::kindIndex) { + if (!node->isArray() || !node->isValidIndex(arg.index_)) { + // Error: unable to resolve path (array value expected at position... ) + return Value::nullSingleton(); + } + node = &((*node)[arg.index_]); + } else if (arg.kind_ == PathArgument::kindKey) { + if (!node->isObject()) { + // Error: unable to resolve path (object value expected at position...) + return Value::nullSingleton(); + } + node = &((*node)[arg.key_]); + if (node == &Value::nullSingleton()) { + // Error: unable to resolve path (object has no member named '' at + // position...) + return Value::nullSingleton(); + } + } + } + return *node; +} + +Value Path::resolve(const Value& root, const Value& defaultValue) const { + const Value* node = &root; + for (const auto& arg : args_) { + if (arg.kind_ == PathArgument::kindIndex) { + if (!node->isArray() || !node->isValidIndex(arg.index_)) + return defaultValue; + node = &((*node)[arg.index_]); + } else if (arg.kind_ == PathArgument::kindKey) { + if (!node->isObject()) + return defaultValue; + node = &((*node)[arg.key_]); + if (node == &Value::nullSingleton()) + return defaultValue; + } + } + return *node; +} + +Value& Path::make(Value& root) const { + Value* node = &root; + for (const auto& arg : args_) { + if (arg.kind_ == PathArgument::kindIndex) { + if (!node->isArray()) { + // Error: node is not an array at position ... + } + node = &((*node)[arg.index_]); + } else if (arg.kind_ == PathArgument::kindKey) { + if (!node->isObject()) { + // Error: node is not an object at position... + } + node = &((*node)[arg.key_]); + } + } + return *node; +} + +} // namespace Json + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: src/lib_json/json_value.cpp +// ////////////////////////////////////////////////////////////////////// + + + + + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: src/lib_json/json_writer.cpp +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2011 Baptiste Lepilleur and The JsonCpp Authors +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +#if !defined(JSON_IS_AMALGAMATION) +#include "json_tool.h" +#include +#endif // if !defined(JSON_IS_AMALGAMATION) +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#if __cplusplus >= 201103L +#include +#include + +#if !defined(isnan) +#define isnan std::isnan +#endif + +#if !defined(isfinite) +#define isfinite std::isfinite +#endif + +#else +#include +#include + +#if defined(_MSC_VER) +#if !defined(isnan) +#include +#define isnan _isnan +#endif + +#if !defined(isfinite) +#include +#define isfinite _finite +#endif + +#if !defined(_CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES) +#define _CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES 1 +#endif //_CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES + +#endif //_MSC_VER + +#if defined(__sun) && defined(__SVR4) // Solaris +#if !defined(isfinite) +#include +#define isfinite finite +#endif +#endif + +#if defined(__hpux) +#if !defined(isfinite) +#if defined(__ia64) && !defined(finite) +#define isfinite(x) \ + ((sizeof(x) == sizeof(float) ? _Isfinitef(x) : _IsFinite(x))) +#endif +#endif +#endif + +#if !defined(isnan) +// IEEE standard states that NaN values will not compare to themselves +#define isnan(x) ((x) != (x)) +#endif + +#if !defined(__APPLE__) +#if !defined(isfinite) +#define isfinite finite +#endif +#endif +#endif + +#if defined(_MSC_VER) +// Disable warning about strdup being deprecated. +#pragma warning(disable : 4996) +#endif + +namespace Json { + +#if __cplusplus >= 201103L || (defined(_CPPLIB_VER) && _CPPLIB_VER >= 520) +using StreamWriterPtr = std::unique_ptr; +#else +using StreamWriterPtr = std::auto_ptr; +#endif + +String valueToString(LargestInt value) { + UIntToStringBuffer buffer; + char* current = buffer + sizeof(buffer); + if (value == Value::minLargestInt) { + uintToString(LargestUInt(Value::maxLargestInt) + 1, current); + *--current = '-'; + } else if (value < 0) { + uintToString(LargestUInt(-value), current); + *--current = '-'; + } else { + uintToString(LargestUInt(value), current); + } + assert(current >= buffer); + return current; +} + +String valueToString(LargestUInt value) { + UIntToStringBuffer buffer; + char* current = buffer + sizeof(buffer); + uintToString(value, current); + assert(current >= buffer); + return current; +} + +#if defined(JSON_HAS_INT64) + +String valueToString(Int value) { return valueToString(LargestInt(value)); } + +String valueToString(UInt value) { return valueToString(LargestUInt(value)); } + +#endif // # if defined(JSON_HAS_INT64) + +namespace { +String valueToString(double value, bool useSpecialFloats, + unsigned int precision, PrecisionType precisionType) { + // Print into the buffer. We need not request the alternative representation + // that always has a decimal point because JSON doesn't distinguish the + // concepts of reals and integers. + if (!isfinite(value)) { + static const char* const reps[2][3] = {{"NaN", "-Infinity", "Infinity"}, + {"null", "-1e+9999", "1e+9999"}}; + return reps[useSpecialFloats ? 0 : 1] + [isnan(value) ? 0 : (value < 0) ? 1 : 2]; + } + + String buffer(size_t(36), '\0'); + while (true) { + int len = jsoncpp_snprintf( + &*buffer.begin(), buffer.size(), + (precisionType == PrecisionType::significantDigits) ? "%.*g" : "%.*f", + precision, value); + assert(len >= 0); + auto wouldPrint = static_cast(len); + if (wouldPrint >= buffer.size()) { + buffer.resize(wouldPrint + 1); + continue; + } + buffer.resize(wouldPrint); + break; + } + + buffer.erase(fixNumericLocale(buffer.begin(), buffer.end()), buffer.end()); + + // try to ensure we preserve the fact that this was given to us as a double on + // input + if (buffer.find('.') == buffer.npos && buffer.find('e') == buffer.npos) { + buffer += ".0"; + } + + // strip the zero padding from the right + if (precisionType == PrecisionType::decimalPlaces) { + buffer.erase(fixZerosInTheEnd(buffer.begin(), buffer.end(), precision), + buffer.end()); + } + + return buffer; +} +} // namespace + +String valueToString(double value, unsigned int precision, + PrecisionType precisionType) { + return valueToString(value, false, precision, precisionType); +} + +String valueToString(bool value) { return value ? "true" : "false"; } + +static bool doesAnyCharRequireEscaping(char const* s, size_t n) { + assert(s || !n); + + return std::any_of(s, s + n, [](unsigned char c) { + return c == '\\' || c == '"' || c < 0x20 || c > 0x7F; + }); +} + +static unsigned int utf8ToCodepoint(const char*& s, const char* e) { + const unsigned int REPLACEMENT_CHARACTER = 0xFFFD; + + unsigned int firstByte = static_cast(*s); + + if (firstByte < 0x80) + return firstByte; + + if (firstByte < 0xE0) { + if (e - s < 2) + return REPLACEMENT_CHARACTER; + + unsigned int calculated = + ((firstByte & 0x1F) << 6) | (static_cast(s[1]) & 0x3F); + s += 1; + // oversized encoded characters are invalid + return calculated < 0x80 ? REPLACEMENT_CHARACTER : calculated; + } + + if (firstByte < 0xF0) { + if (e - s < 3) + return REPLACEMENT_CHARACTER; + + unsigned int calculated = ((firstByte & 0x0F) << 12) | + ((static_cast(s[1]) & 0x3F) << 6) | + (static_cast(s[2]) & 0x3F); + s += 2; + // surrogates aren't valid codepoints itself + // shouldn't be UTF-8 encoded + if (calculated >= 0xD800 && calculated <= 0xDFFF) + return REPLACEMENT_CHARACTER; + // oversized encoded characters are invalid + return calculated < 0x800 ? REPLACEMENT_CHARACTER : calculated; + } + + if (firstByte < 0xF8) { + if (e - s < 4) + return REPLACEMENT_CHARACTER; + + unsigned int calculated = ((firstByte & 0x07) << 18) | + ((static_cast(s[1]) & 0x3F) << 12) | + ((static_cast(s[2]) & 0x3F) << 6) | + (static_cast(s[3]) & 0x3F); + s += 3; + // oversized encoded characters are invalid + return calculated < 0x10000 ? REPLACEMENT_CHARACTER : calculated; + } + + return REPLACEMENT_CHARACTER; +} + +static const char hex2[] = "000102030405060708090a0b0c0d0e0f" + "101112131415161718191a1b1c1d1e1f" + "202122232425262728292a2b2c2d2e2f" + "303132333435363738393a3b3c3d3e3f" + "404142434445464748494a4b4c4d4e4f" + "505152535455565758595a5b5c5d5e5f" + "606162636465666768696a6b6c6d6e6f" + "707172737475767778797a7b7c7d7e7f" + "808182838485868788898a8b8c8d8e8f" + "909192939495969798999a9b9c9d9e9f" + "a0a1a2a3a4a5a6a7a8a9aaabacadaeaf" + "b0b1b2b3b4b5b6b7b8b9babbbcbdbebf" + "c0c1c2c3c4c5c6c7c8c9cacbcccdcecf" + "d0d1d2d3d4d5d6d7d8d9dadbdcdddedf" + "e0e1e2e3e4e5e6e7e8e9eaebecedeeef" + "f0f1f2f3f4f5f6f7f8f9fafbfcfdfeff"; + +static String toHex16Bit(unsigned int x) { + const unsigned int hi = (x >> 8) & 0xff; + const unsigned int lo = x & 0xff; + String result(4, ' '); + result[0] = hex2[2 * hi]; + result[1] = hex2[2 * hi + 1]; + result[2] = hex2[2 * lo]; + result[3] = hex2[2 * lo + 1]; + return result; +} + +static void appendRaw(String& result, unsigned ch) { + result += static_cast(ch); +} + +static void appendHex(String& result, unsigned ch) { + result.append("\\u").append(toHex16Bit(ch)); +} + +static String valueToQuotedStringN(const char* value, size_t length, + bool emitUTF8 = false) { + if (value == nullptr) + return ""; + + if (!doesAnyCharRequireEscaping(value, length)) + return String("\"") + value + "\""; + // We have to walk value and escape any special characters. + // Appending to String is not efficient, but this should be rare. + // (Note: forward slashes are *not* rare, but I am not escaping them.) + String::size_type maxsize = length * 2 + 3; // allescaped+quotes+NULL + String result; + result.reserve(maxsize); // to avoid lots of mallocs + result += "\""; + char const* end = value + length; + for (const char* c = value; c != end; ++c) { + switch (*c) { + case '\"': + result += "\\\""; + break; + case '\\': + result += "\\\\"; + break; + case '\b': + result += "\\b"; + break; + case '\f': + result += "\\f"; + break; + case '\n': + result += "\\n"; + break; + case '\r': + result += "\\r"; + break; + case '\t': + result += "\\t"; + break; + // case '/': + // Even though \/ is considered a legal escape in JSON, a bare + // slash is also legal, so I see no reason to escape it. + // (I hope I am not misunderstanding something.) + // blep notes: actually escaping \/ may be useful in javascript to avoid (*c); + if (codepoint < 0x20) { + appendHex(result, codepoint); + } else { + appendRaw(result, codepoint); + } + } else { + unsigned codepoint = utf8ToCodepoint(c, end); // modifies `c` + if (codepoint < 0x20) { + appendHex(result, codepoint); + } else if (codepoint < 0x80) { + appendRaw(result, codepoint); + } else if (codepoint < 0x10000) { + // Basic Multilingual Plane + appendHex(result, codepoint); + } else { + // Extended Unicode. Encode 20 bits as a surrogate pair. + codepoint -= 0x10000; + appendHex(result, 0xd800 + ((codepoint >> 10) & 0x3ff)); + appendHex(result, 0xdc00 + (codepoint & 0x3ff)); + } + } + } break; + } + } + result += "\""; + return result; +} + +String valueToQuotedString(const char* value) { + return valueToQuotedStringN(value, strlen(value)); +} + +// Class Writer +// ////////////////////////////////////////////////////////////////// +Writer::~Writer() = default; + +// Class FastWriter +// ////////////////////////////////////////////////////////////////// + +FastWriter::FastWriter() + + = default; + +void FastWriter::enableYAMLCompatibility() { yamlCompatibilityEnabled_ = true; } + +void FastWriter::dropNullPlaceholders() { dropNullPlaceholders_ = true; } + +void FastWriter::omitEndingLineFeed() { omitEndingLineFeed_ = true; } + +String FastWriter::write(const Value& root) { + document_.clear(); + writeValue(root); + if (!omitEndingLineFeed_) + document_ += '\n'; + return document_; +} + +void FastWriter::writeValue(const Value& value) { + switch (value.type()) { + case nullValue: + if (!dropNullPlaceholders_) + document_ += "null"; + break; + case intValue: + document_ += valueToString(value.asLargestInt()); + break; + case uintValue: + document_ += valueToString(value.asLargestUInt()); + break; + case realValue: + document_ += valueToString(value.asDouble()); + break; + case stringValue: { + // Is NULL possible for value.string_? No. + char const* str; + char const* end; + bool ok = value.getString(&str, &end); + if (ok) + document_ += valueToQuotedStringN(str, static_cast(end - str)); + break; + } + case booleanValue: + document_ += valueToString(value.asBool()); + break; + case arrayValue: { + document_ += '['; + ArrayIndex size = value.size(); + for (ArrayIndex index = 0; index < size; ++index) { + if (index > 0) + document_ += ','; + writeValue(value[index]); + } + document_ += ']'; + } break; + case objectValue: { + Value::Members members(value.getMemberNames()); + document_ += '{'; + for (auto it = members.begin(); it != members.end(); ++it) { + const String& name = *it; + if (it != members.begin()) + document_ += ','; + document_ += valueToQuotedStringN(name.data(), name.length()); + document_ += yamlCompatibilityEnabled_ ? ": " : ":"; + writeValue(value[name]); + } + document_ += '}'; + } break; + } +} + +// Class StyledWriter +// ////////////////////////////////////////////////////////////////// + +StyledWriter::StyledWriter() = default; + +String StyledWriter::write(const Value& root) { + document_.clear(); + addChildValues_ = false; + indentString_.clear(); + writeCommentBeforeValue(root); + writeValue(root); + writeCommentAfterValueOnSameLine(root); + document_ += '\n'; + return document_; +} + +void StyledWriter::writeValue(const Value& value) { + switch (value.type()) { + case nullValue: + pushValue("null"); + break; + case intValue: + pushValue(valueToString(value.asLargestInt())); + break; + case uintValue: + pushValue(valueToString(value.asLargestUInt())); + break; + case realValue: + pushValue(valueToString(value.asDouble())); + break; + case stringValue: { + // Is NULL possible for value.string_? No. + char const* str; + char const* end; + bool ok = value.getString(&str, &end); + if (ok) + pushValue(valueToQuotedStringN(str, static_cast(end - str))); + else + pushValue(""); + break; + } + case booleanValue: + pushValue(valueToString(value.asBool())); + break; + case arrayValue: + writeArrayValue(value); + break; + case objectValue: { + Value::Members members(value.getMemberNames()); + if (members.empty()) + pushValue("{}"); + else { + writeWithIndent("{"); + indent(); + auto it = members.begin(); + for (;;) { + const String& name = *it; + const Value& childValue = value[name]; + writeCommentBeforeValue(childValue); + writeWithIndent(valueToQuotedString(name.c_str())); + document_ += " : "; + writeValue(childValue); + if (++it == members.end()) { + writeCommentAfterValueOnSameLine(childValue); + break; + } + document_ += ','; + writeCommentAfterValueOnSameLine(childValue); + } + unindent(); + writeWithIndent("}"); + } + } break; + } +} + +void StyledWriter::writeArrayValue(const Value& value) { + size_t size = value.size(); + if (size == 0) + pushValue("[]"); + else { + bool isArrayMultiLine = isMultilineArray(value); + if (isArrayMultiLine) { + writeWithIndent("["); + indent(); + bool hasChildValue = !childValues_.empty(); + ArrayIndex index = 0; + for (;;) { + const Value& childValue = value[index]; + writeCommentBeforeValue(childValue); + if (hasChildValue) + writeWithIndent(childValues_[index]); + else { + writeIndent(); + writeValue(childValue); + } + if (++index == size) { + writeCommentAfterValueOnSameLine(childValue); + break; + } + document_ += ','; + writeCommentAfterValueOnSameLine(childValue); + } + unindent(); + writeWithIndent("]"); + } else // output on a single line + { + assert(childValues_.size() == size); + document_ += "[ "; + for (size_t index = 0; index < size; ++index) { + if (index > 0) + document_ += ", "; + document_ += childValues_[index]; + } + document_ += " ]"; + } + } +} + +bool StyledWriter::isMultilineArray(const Value& value) { + ArrayIndex const size = value.size(); + bool isMultiLine = size * 3 >= rightMargin_; + childValues_.clear(); + for (ArrayIndex index = 0; index < size && !isMultiLine; ++index) { + const Value& childValue = value[index]; + isMultiLine = ((childValue.isArray() || childValue.isObject()) && + !childValue.empty()); + } + if (!isMultiLine) // check if line length > max line length + { + childValues_.reserve(size); + addChildValues_ = true; + ArrayIndex lineLength = 4 + (size - 1) * 2; // '[ ' + ', '*n + ' ]' + for (ArrayIndex index = 0; index < size; ++index) { + if (hasCommentForValue(value[index])) { + isMultiLine = true; + } + writeValue(value[index]); + lineLength += static_cast(childValues_[index].length()); + } + addChildValues_ = false; + isMultiLine = isMultiLine || lineLength >= rightMargin_; + } + return isMultiLine; +} + +void StyledWriter::pushValue(const String& value) { + if (addChildValues_) + childValues_.push_back(value); + else + document_ += value; +} + +void StyledWriter::writeIndent() { + if (!document_.empty()) { + char last = document_[document_.length() - 1]; + if (last == ' ') // already indented + return; + if (last != '\n') // Comments may add new-line + document_ += '\n'; + } + document_ += indentString_; +} + +void StyledWriter::writeWithIndent(const String& value) { + writeIndent(); + document_ += value; +} + +void StyledWriter::indent() { indentString_ += String(indentSize_, ' '); } + +void StyledWriter::unindent() { + assert(indentString_.size() >= indentSize_); + indentString_.resize(indentString_.size() - indentSize_); +} + +void StyledWriter::writeCommentBeforeValue(const Value& root) { + if (!root.hasComment(commentBefore)) + return; + + document_ += '\n'; + writeIndent(); + const String& comment = root.getComment(commentBefore); + String::const_iterator iter = comment.begin(); + while (iter != comment.end()) { + document_ += *iter; + if (*iter == '\n' && ((iter + 1) != comment.end() && *(iter + 1) == '/')) + writeIndent(); + ++iter; + } + + // Comments are stripped of trailing newlines, so add one here + document_ += '\n'; +} + +void StyledWriter::writeCommentAfterValueOnSameLine(const Value& root) { + if (root.hasComment(commentAfterOnSameLine)) + document_ += " " + root.getComment(commentAfterOnSameLine); + + if (root.hasComment(commentAfter)) { + document_ += '\n'; + document_ += root.getComment(commentAfter); + document_ += '\n'; + } +} + +bool StyledWriter::hasCommentForValue(const Value& value) { + return value.hasComment(commentBefore) || + value.hasComment(commentAfterOnSameLine) || + value.hasComment(commentAfter); +} + +// Class StyledStreamWriter +// ////////////////////////////////////////////////////////////////// + +StyledStreamWriter::StyledStreamWriter(String indentation) + : document_(nullptr), indentation_(std::move(indentation)), + addChildValues_(), indented_(false) {} + +void StyledStreamWriter::write(OStream& out, const Value& root) { + document_ = &out; + addChildValues_ = false; + indentString_.clear(); + indented_ = true; + writeCommentBeforeValue(root); + if (!indented_) + writeIndent(); + indented_ = true; + writeValue(root); + writeCommentAfterValueOnSameLine(root); + *document_ << "\n"; + document_ = nullptr; // Forget the stream, for safety. +} + +void StyledStreamWriter::writeValue(const Value& value) { + switch (value.type()) { + case nullValue: + pushValue("null"); + break; + case intValue: + pushValue(valueToString(value.asLargestInt())); + break; + case uintValue: + pushValue(valueToString(value.asLargestUInt())); + break; + case realValue: + pushValue(valueToString(value.asDouble())); + break; + case stringValue: { + // Is NULL possible for value.string_? No. + char const* str; + char const* end; + bool ok = value.getString(&str, &end); + if (ok) + pushValue(valueToQuotedStringN(str, static_cast(end - str))); + else + pushValue(""); + break; + } + case booleanValue: + pushValue(valueToString(value.asBool())); + break; + case arrayValue: + writeArrayValue(value); + break; + case objectValue: { + Value::Members members(value.getMemberNames()); + if (members.empty()) + pushValue("{}"); + else { + writeWithIndent("{"); + indent(); + auto it = members.begin(); + for (;;) { + const String& name = *it; + const Value& childValue = value[name]; + writeCommentBeforeValue(childValue); + writeWithIndent(valueToQuotedString(name.c_str())); + *document_ << " : "; + writeValue(childValue); + if (++it == members.end()) { + writeCommentAfterValueOnSameLine(childValue); + break; + } + *document_ << ","; + writeCommentAfterValueOnSameLine(childValue); + } + unindent(); + writeWithIndent("}"); + } + } break; + } +} + +void StyledStreamWriter::writeArrayValue(const Value& value) { + unsigned size = value.size(); + if (size == 0) + pushValue("[]"); + else { + bool isArrayMultiLine = isMultilineArray(value); + if (isArrayMultiLine) { + writeWithIndent("["); + indent(); + bool hasChildValue = !childValues_.empty(); + unsigned index = 0; + for (;;) { + const Value& childValue = value[index]; + writeCommentBeforeValue(childValue); + if (hasChildValue) + writeWithIndent(childValues_[index]); + else { + if (!indented_) + writeIndent(); + indented_ = true; + writeValue(childValue); + indented_ = false; + } + if (++index == size) { + writeCommentAfterValueOnSameLine(childValue); + break; + } + *document_ << ","; + writeCommentAfterValueOnSameLine(childValue); + } + unindent(); + writeWithIndent("]"); + } else // output on a single line + { + assert(childValues_.size() == size); + *document_ << "[ "; + for (unsigned index = 0; index < size; ++index) { + if (index > 0) + *document_ << ", "; + *document_ << childValues_[index]; + } + *document_ << " ]"; + } + } +} + +bool StyledStreamWriter::isMultilineArray(const Value& value) { + ArrayIndex const size = value.size(); + bool isMultiLine = size * 3 >= rightMargin_; + childValues_.clear(); + for (ArrayIndex index = 0; index < size && !isMultiLine; ++index) { + const Value& childValue = value[index]; + isMultiLine = ((childValue.isArray() || childValue.isObject()) && + !childValue.empty()); + } + if (!isMultiLine) // check if line length > max line length + { + childValues_.reserve(size); + addChildValues_ = true; + ArrayIndex lineLength = 4 + (size - 1) * 2; // '[ ' + ', '*n + ' ]' + for (ArrayIndex index = 0; index < size; ++index) { + if (hasCommentForValue(value[index])) { + isMultiLine = true; + } + writeValue(value[index]); + lineLength += static_cast(childValues_[index].length()); + } + addChildValues_ = false; + isMultiLine = isMultiLine || lineLength >= rightMargin_; + } + return isMultiLine; +} + +void StyledStreamWriter::pushValue(const String& value) { + if (addChildValues_) + childValues_.push_back(value); + else + *document_ << value; +} + +void StyledStreamWriter::writeIndent() { + // blep intended this to look at the so-far-written string + // to determine whether we are already indented, but + // with a stream we cannot do that. So we rely on some saved state. + // The caller checks indented_. + *document_ << '\n' << indentString_; +} + +void StyledStreamWriter::writeWithIndent(const String& value) { + if (!indented_) + writeIndent(); + *document_ << value; + indented_ = false; +} + +void StyledStreamWriter::indent() { indentString_ += indentation_; } + +void StyledStreamWriter::unindent() { + assert(indentString_.size() >= indentation_.size()); + indentString_.resize(indentString_.size() - indentation_.size()); +} + +void StyledStreamWriter::writeCommentBeforeValue(const Value& root) { + if (!root.hasComment(commentBefore)) + return; + + if (!indented_) + writeIndent(); + const String& comment = root.getComment(commentBefore); + String::const_iterator iter = comment.begin(); + while (iter != comment.end()) { + *document_ << *iter; + if (*iter == '\n' && ((iter + 1) != comment.end() && *(iter + 1) == '/')) + // writeIndent(); // would include newline + *document_ << indentString_; + ++iter; + } + indented_ = false; +} + +void StyledStreamWriter::writeCommentAfterValueOnSameLine(const Value& root) { + if (root.hasComment(commentAfterOnSameLine)) + *document_ << ' ' << root.getComment(commentAfterOnSameLine); + + if (root.hasComment(commentAfter)) { + writeIndent(); + *document_ << root.getComment(commentAfter); + } + indented_ = false; +} + +bool StyledStreamWriter::hasCommentForValue(const Value& value) { + return value.hasComment(commentBefore) || + value.hasComment(commentAfterOnSameLine) || + value.hasComment(commentAfter); +} + +////////////////////////// +// BuiltStyledStreamWriter + +/// Scoped enums are not available until C++11. +struct CommentStyle { + /// Decide whether to write comments. + enum Enum { + None, ///< Drop all comments. + Most, ///< Recover odd behavior of previous versions (not implemented yet). + All ///< Keep all comments. + }; +}; + +struct BuiltStyledStreamWriter : public StreamWriter { + BuiltStyledStreamWriter(String indentation, CommentStyle::Enum cs, + String colonSymbol, String nullSymbol, + String endingLineFeedSymbol, bool useSpecialFloats, + bool emitUTF8, unsigned int precision, + PrecisionType precisionType); + int write(Value const& root, OStream* sout) override; + +private: + void writeValue(Value const& value); + void writeArrayValue(Value const& value); + bool isMultilineArray(Value const& value); + void pushValue(String const& value); + void writeIndent(); + void writeWithIndent(String const& value); + void indent(); + void unindent(); + void writeCommentBeforeValue(Value const& root); + void writeCommentAfterValueOnSameLine(Value const& root); + static bool hasCommentForValue(const Value& value); + + using ChildValues = std::vector; + + ChildValues childValues_; + String indentString_; + unsigned int rightMargin_; + String indentation_; + CommentStyle::Enum cs_; + String colonSymbol_; + String nullSymbol_; + String endingLineFeedSymbol_; + bool addChildValues_ : 1; + bool indented_ : 1; + bool useSpecialFloats_ : 1; + bool emitUTF8_ : 1; + unsigned int precision_; + PrecisionType precisionType_; +}; +BuiltStyledStreamWriter::BuiltStyledStreamWriter( + String indentation, CommentStyle::Enum cs, String colonSymbol, + String nullSymbol, String endingLineFeedSymbol, bool useSpecialFloats, + bool emitUTF8, unsigned int precision, PrecisionType precisionType) + : rightMargin_(74), indentation_(std::move(indentation)), cs_(cs), + colonSymbol_(std::move(colonSymbol)), nullSymbol_(std::move(nullSymbol)), + endingLineFeedSymbol_(std::move(endingLineFeedSymbol)), + addChildValues_(false), indented_(false), + useSpecialFloats_(useSpecialFloats), emitUTF8_(emitUTF8), + precision_(precision), precisionType_(precisionType) {} +int BuiltStyledStreamWriter::write(Value const& root, OStream* sout) { + sout_ = sout; + addChildValues_ = false; + indented_ = true; + indentString_.clear(); + writeCommentBeforeValue(root); + if (!indented_) + writeIndent(); + indented_ = true; + writeValue(root); + writeCommentAfterValueOnSameLine(root); + *sout_ << endingLineFeedSymbol_; + sout_ = nullptr; + return 0; +} +void BuiltStyledStreamWriter::writeValue(Value const& value) { + switch (value.type()) { + case nullValue: + pushValue(nullSymbol_); + break; + case intValue: + pushValue(valueToString(value.asLargestInt())); + break; + case uintValue: + pushValue(valueToString(value.asLargestUInt())); + break; + case realValue: + pushValue(valueToString(value.asDouble(), useSpecialFloats_, precision_, + precisionType_)); + break; + case stringValue: { + // Is NULL is possible for value.string_? No. + char const* str; + char const* end; + bool ok = value.getString(&str, &end); + if (ok) + pushValue( + valueToQuotedStringN(str, static_cast(end - str), emitUTF8_)); + else + pushValue(""); + break; + } + case booleanValue: + pushValue(valueToString(value.asBool())); + break; + case arrayValue: + writeArrayValue(value); + break; + case objectValue: { + Value::Members members(value.getMemberNames()); + if (members.empty()) + pushValue("{}"); + else { + writeWithIndent("{"); + indent(); + auto it = members.begin(); + for (;;) { + String const& name = *it; + Value const& childValue = value[name]; + writeCommentBeforeValue(childValue); + writeWithIndent( + valueToQuotedStringN(name.data(), name.length(), emitUTF8_)); + *sout_ << colonSymbol_; + writeValue(childValue); + if (++it == members.end()) { + writeCommentAfterValueOnSameLine(childValue); + break; + } + *sout_ << ","; + writeCommentAfterValueOnSameLine(childValue); + } + unindent(); + writeWithIndent("}"); + } + } break; + } +} + +void BuiltStyledStreamWriter::writeArrayValue(Value const& value) { + unsigned size = value.size(); + if (size == 0) + pushValue("[]"); + else { + bool isMultiLine = (cs_ == CommentStyle::All) || isMultilineArray(value); + if (isMultiLine) { + writeWithIndent("["); + indent(); + bool hasChildValue = !childValues_.empty(); + unsigned index = 0; + for (;;) { + Value const& childValue = value[index]; + writeCommentBeforeValue(childValue); + if (hasChildValue) + writeWithIndent(childValues_[index]); + else { + if (!indented_) + writeIndent(); + indented_ = true; + writeValue(childValue); + indented_ = false; + } + if (++index == size) { + writeCommentAfterValueOnSameLine(childValue); + break; + } + *sout_ << ","; + writeCommentAfterValueOnSameLine(childValue); + } + unindent(); + writeWithIndent("]"); + } else // output on a single line + { + assert(childValues_.size() == size); + *sout_ << "["; + if (!indentation_.empty()) + *sout_ << " "; + for (unsigned index = 0; index < size; ++index) { + if (index > 0) + *sout_ << ((!indentation_.empty()) ? ", " : ","); + *sout_ << childValues_[index]; + } + if (!indentation_.empty()) + *sout_ << " "; + *sout_ << "]"; + } + } +} + +bool BuiltStyledStreamWriter::isMultilineArray(Value const& value) { + ArrayIndex const size = value.size(); + bool isMultiLine = size * 3 >= rightMargin_; + childValues_.clear(); + for (ArrayIndex index = 0; index < size && !isMultiLine; ++index) { + Value const& childValue = value[index]; + isMultiLine = ((childValue.isArray() || childValue.isObject()) && + !childValue.empty()); + } + if (!isMultiLine) // check if line length > max line length + { + childValues_.reserve(size); + addChildValues_ = true; + ArrayIndex lineLength = 4 + (size - 1) * 2; // '[ ' + ', '*n + ' ]' + for (ArrayIndex index = 0; index < size; ++index) { + if (hasCommentForValue(value[index])) { + isMultiLine = true; + } + writeValue(value[index]); + lineLength += static_cast(childValues_[index].length()); + } + addChildValues_ = false; + isMultiLine = isMultiLine || lineLength >= rightMargin_; + } + return isMultiLine; +} + +void BuiltStyledStreamWriter::pushValue(String const& value) { + if (addChildValues_) + childValues_.push_back(value); + else + *sout_ << value; +} + +void BuiltStyledStreamWriter::writeIndent() { + // blep intended this to look at the so-far-written string + // to determine whether we are already indented, but + // with a stream we cannot do that. So we rely on some saved state. + // The caller checks indented_. + + if (!indentation_.empty()) { + // In this case, drop newlines too. + *sout_ << '\n' << indentString_; + } +} + +void BuiltStyledStreamWriter::writeWithIndent(String const& value) { + if (!indented_) + writeIndent(); + *sout_ << value; + indented_ = false; +} + +void BuiltStyledStreamWriter::indent() { indentString_ += indentation_; } + +void BuiltStyledStreamWriter::unindent() { + assert(indentString_.size() >= indentation_.size()); + indentString_.resize(indentString_.size() - indentation_.size()); +} + +void BuiltStyledStreamWriter::writeCommentBeforeValue(Value const& root) { + if (cs_ == CommentStyle::None) + return; + if (!root.hasComment(commentBefore)) + return; + + if (!indented_) + writeIndent(); + const String& comment = root.getComment(commentBefore); + String::const_iterator iter = comment.begin(); + while (iter != comment.end()) { + *sout_ << *iter; + if (*iter == '\n' && ((iter + 1) != comment.end() && *(iter + 1) == '/')) + // writeIndent(); // would write extra newline + *sout_ << indentString_; + ++iter; + } + indented_ = false; +} + +void BuiltStyledStreamWriter::writeCommentAfterValueOnSameLine( + Value const& root) { + if (cs_ == CommentStyle::None) + return; + if (root.hasComment(commentAfterOnSameLine)) + *sout_ << " " + root.getComment(commentAfterOnSameLine); + + if (root.hasComment(commentAfter)) { + writeIndent(); + *sout_ << root.getComment(commentAfter); + } +} + +// static +bool BuiltStyledStreamWriter::hasCommentForValue(const Value& value) { + return value.hasComment(commentBefore) || + value.hasComment(commentAfterOnSameLine) || + value.hasComment(commentAfter); +} + +/////////////// +// StreamWriter + +StreamWriter::StreamWriter() : sout_(nullptr) {} +StreamWriter::~StreamWriter() = default; +StreamWriter::Factory::~Factory() = default; +StreamWriterBuilder::StreamWriterBuilder() { setDefaults(&settings_); } +StreamWriterBuilder::~StreamWriterBuilder() = default; +StreamWriter* StreamWriterBuilder::newStreamWriter() const { + const String indentation = settings_["indentation"].asString(); + const String cs_str = settings_["commentStyle"].asString(); + const String pt_str = settings_["precisionType"].asString(); + const bool eyc = settings_["enableYAMLCompatibility"].asBool(); + const bool dnp = settings_["dropNullPlaceholders"].asBool(); + const bool usf = settings_["useSpecialFloats"].asBool(); + const bool emitUTF8 = settings_["emitUTF8"].asBool(); + unsigned int pre = settings_["precision"].asUInt(); + CommentStyle::Enum cs = CommentStyle::All; + if (cs_str == "All") { + cs = CommentStyle::All; + } else if (cs_str == "None") { + cs = CommentStyle::None; + } else { + throwRuntimeError("commentStyle must be 'All' or 'None'"); + } + PrecisionType precisionType(significantDigits); + if (pt_str == "significant") { + precisionType = PrecisionType::significantDigits; + } else if (pt_str == "decimal") { + precisionType = PrecisionType::decimalPlaces; + } else { + throwRuntimeError("precisionType must be 'significant' or 'decimal'"); + } + String colonSymbol = " : "; + if (eyc) { + colonSymbol = ": "; + } else if (indentation.empty()) { + colonSymbol = ":"; + } + String nullSymbol = "null"; + if (dnp) { + nullSymbol.clear(); + } + if (pre > 17) + pre = 17; + String endingLineFeedSymbol; + return new BuiltStyledStreamWriter(indentation, cs, colonSymbol, nullSymbol, + endingLineFeedSymbol, usf, emitUTF8, pre, + precisionType); +} + +bool StreamWriterBuilder::validate(Json::Value* invalid) const { + static const auto& valid_keys = *new std::set{ + "indentation", + "commentStyle", + "enableYAMLCompatibility", + "dropNullPlaceholders", + "useSpecialFloats", + "emitUTF8", + "precision", + "precisionType", + }; + for (auto si = settings_.begin(); si != settings_.end(); ++si) { + auto key = si.name(); + if (valid_keys.count(key)) + continue; + if (invalid) + (*invalid)[key] = *si; + else + return false; + } + return invalid ? invalid->empty() : true; +} + +Value& StreamWriterBuilder::operator[](const String& key) { + return settings_[key]; +} +// static +void StreamWriterBuilder::setDefaults(Json::Value* settings) { + //! [StreamWriterBuilderDefaults] + (*settings)["commentStyle"] = "All"; + (*settings)["indentation"] = "\t"; + (*settings)["enableYAMLCompatibility"] = false; + (*settings)["dropNullPlaceholders"] = false; + (*settings)["useSpecialFloats"] = false; + (*settings)["emitUTF8"] = false; + (*settings)["precision"] = 17; + (*settings)["precisionType"] = "significant"; + //! [StreamWriterBuilderDefaults] +} + +String writeString(StreamWriter::Factory const& factory, Value const& root) { + OStringStream sout; + StreamWriterPtr const writer(factory.newStreamWriter()); + writer->write(root, &sout); + return sout.str(); +} + +OStream& operator<<(OStream& sout, Value const& root) { + StreamWriterBuilder builder; + StreamWriterPtr const writer(builder.newStreamWriter()); + writer->write(root, &sout); + return sout; +} + +} // namespace Json + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: src/lib_json/json_writer.cpp +// ////////////////////////////////////////////////////////////////////// + + + + + diff --git a/src/route/src/md5.cpp b/src/route/src/md5.cpp new file mode 100644 index 0000000..eb7f064 --- /dev/null +++ b/src/route/src/md5.cpp @@ -0,0 +1,371 @@ +#include "md5.h" +#include +#include +#include + +// 重命名自定义的 byte 类型 +typedef unsigned char uchar; + +using namespace std; + +/* Constants for MD5Transform routine. */ +#define S11 7 +#define S12 12 +#define S13 17 +#define S14 22 +#define S21 5 +#define S22 9 +#define S23 14 +#define S24 20 +#define S31 4 +#define S32 11 +#define S33 16 +#define S34 23 +#define S41 6 +#define S42 10 +#define S43 15 +#define S44 21 + +/* F, G, H and I are basic MD5 functions. + */ +#define F(x, y, z) (((x) & (y)) | ((~x) & (z))) +#define G(x, y, z) (((x) & (z)) | ((y) & (~z))) +#define H(x, y, z) ((x) ^ (y) ^ (z)) +#define I(x, y, z) ((y) ^ ((x) | (~z))) + +/* ROTATE_LEFT rotates x left n bits. + */ +#define ROTATE_LEFT(x, n) (((x) << (n)) | ((x) >> (32 - (n)))) + +/* FF, GG, HH, and II transformations for rounds 1, 2, 3, and 4. +Rotation is separate from addition to prevent recomputation. +*/ +#define FF(a, b, c, d, x, s, ac) \ + { \ + (a) += F((b), (c), (d)) + (x) + ac; \ + (a) = ROTATE_LEFT((a), (s)); \ + (a) += (b); \ + } +#define GG(a, b, c, d, x, s, ac) \ + { \ + (a) += G((b), (c), (d)) + (x) + ac; \ + (a) = ROTATE_LEFT((a), (s)); \ + (a) += (b); \ + } +#define HH(a, b, c, d, x, s, ac) \ + { \ + (a) += H((b), (c), (d)) + (x) + ac; \ + (a) = ROTATE_LEFT((a), (s)); \ + (a) += (b); \ + } +#define II(a, b, c, d, x, s, ac) \ + { \ + (a) += I((b), (c), (d)) + (x) + ac; \ + (a) = ROTATE_LEFT((a), (s)); \ + (a) += (b); \ + } + +const uchar MD5::PADDING[64] = {0x80}; +const char MD5::HEX[16] = { + '0', '1', '2', '3', + '4', '5', '6', '7', + '8', '9', 'a', 'b', + 'c', 'd', 'e', 'f'}; + +/* Default construct. */ +MD5::MD5() +{ + reset(); +} + +/* Construct a MD5 object with a input buffer. */ +MD5::MD5(const void *input, size_t length) +{ + reset(); + update(input, length); +} + +/* Construct a MD5 object with a string. */ +MD5::MD5(const string &str) +{ + reset(); + update(str); +} + +/* Construct a MD5 object with a file. */ +MD5::MD5(ifstream &in) +{ + reset(); + update(in); +} + +/* Return the message-digest */ +const uchar *MD5::digest() +{ + if (!_finished) + { + _finished = true; + final(); + } + return _digest; +} + +/* Reset the calculate state */ +void MD5::reset() +{ + _finished = false; + /* reset number of bits. */ + _count[0] = _count[1] = 0; + /* Load magic initialization constants. */ + _state[0] = 0x67452301; + _state[1] = 0xefcdab89; + _state[2] = 0x98badcfe; + _state[3] = 0x10325476; +} + +/* Updating the context with a input buffer. */ +void MD5::update(const void *input, size_t length) +{ + const uchar *uinput = static_cast(input); + update(uinput, length); +} + +/* Updating the context with a string. */ +void MD5::update(const string &str) +{ + const uchar *uinput = reinterpret_cast(str.c_str()); + update(uinput, str.length()); +} + +/* Updating the context with a file. */ +void MD5::update(ifstream &in) +{ + if (!in) + { + return; + } + + std::streamsize length; + char buffer[1024]; + while (!in.eof()) + { + in.read(buffer, 1024); + length = in.gcount(); + if (length > 0) + { + const uchar *uinput = reinterpret_cast(buffer); + update(uinput, length); + } + } + in.close(); +} + +/* MD5 block update operation. Continues an MD5 message-digest +operation, processing another message block, and updating the +context. +*/ +void MD5::update(const uchar *input, size_t length) +{ + uint32 i, index, partLen; + + _finished = false; + + /* Compute number of bytes mod 64 */ + index = (uint32)((_count[0] >> 3) & 0x3f); + + /* update number of bits */ + if ((_count[0] += ((uint32)length << 3)) < ((uint32)length << 3)) + { + ++_count[1]; + } + _count[1] += ((uint32)length >> 29); + + partLen = 64 - index; + + /* transform as many times as possible. */ + if (length >= partLen) + { + memcpy(&_buffer[index], input, partLen); + transform(_buffer); + + for (i = partLen; i + 63 < length; i += 64) + { + transform(&input[i]); + } + index = 0; + } + else + { + i = 0; + } + + /* Buffer remaining input */ + memcpy(&_buffer[index], &input[i], length - i); +} + +/* MD5 finalization. Ends an MD5 message-_digest operation, writing the +the message _digest and zeroizing the context. +*/ +void MD5::final() +{ + uchar bits[8]; + uint32 oldState[4]; + uint32 oldCount[2]; + uint32 index, padLen; + + /* Save current state and count. */ + memcpy(oldState, _state, 16); + memcpy(oldCount, _count, 8); + + /* Save number of bits */ + encode(_count, bits, 8); + + /* Pad out to 56 mod 64. */ + index = (uint32)((_count[0] >> 3) & 0x3f); + padLen = (index < 56) ? (56 - index) : (120 - index); + update(PADDING, padLen); + + /* Append length (before padding) */ + update(bits, 8); + + /* Store state in digest */ + encode(_state, _digest, 16); + + /* Restore current state and count. */ + memcpy(_state, oldState, 16); + memcpy(_count, oldCount, 8); +} + +/* MD5 basic transformation. Transforms _state based on block. */ +/* MD5 basic transformation. Transforms _state based on block. */ +void MD5::transform(const uchar block[64]) +{ + uint32 a = _state[0], b = _state[1], c = _state[2], d = _state[3], x[16]; + + decode(block, x, 64); + + /* Round 1 */ + FF(a, b, c, d, x[0], S11, 0xd76aa478); /* 1 */ + FF(d, a, b, c, x[1], S12, 0xe8c7b756); /* 2 */ + FF(c, d, a, b, x[2], S13, 0x242070db); /* 3 */ + FF(b, c, d, a, x[3], S14, 0xc1bdceee); /* 4 */ + FF(a, b, c, d, x[4], S11, 0xf57c0faf); /* 5 */ + FF(d, a, b, c, x[5], S12, 0x4787c62a); /* 6 */ + FF(c, d, a, b, x[6], S13, 0xa8304613); /* 7 */ + FF(b, c, d, a, x[7], S14, 0xfd469501); /* 8 */ + FF(a, b, c, d, x[8], S11, 0x698098d8); /* 9 */ + FF(d, a, b, c, x[9], S12, 0x8b44f7af); /* 10 */ + FF(c, d, a, b, x[10], S13, 0xffff5bb1); /* 11 */ + FF(b, c, d, a, x[11], S14, 0x895cd7be); /* 12 */ + FF(a, b, c, d, x[12], S11, 0x6b901122); /* 13 */ + FF(d, a, b, c, x[13], S12, 0xfd987193); /* 14 */ + FF(c, d, a, b, x[14], S13, 0xa679438e); /* 15 */ + FF(b, c, d, a, x[15], S14, 0x49b40821); /* 16 */ + + /* Round 2 */ + GG(a, b, c, d, x[1], S21, 0xf61e2562); /* 17 */ + GG(d, a, b, c, x[6], S22, 0xc040b340); /* 18 */ + GG(c, d, a, b, x[11], S23, 0x265e5a51); /* 19 */ + GG(b, c, d, a, x[0], S24, 0xe9b6c7aa); /* 20 */ + GG(a, b, c, d, x[5], S21, 0xd62f105d); /* 21 */ + GG(d, a, b, c, x[10], S22, 0x2441453); /* 22 */ + GG(c, d, a, b, x[15], S23, 0xd8a1e681); /* 23 */ + GG(b, c, d, a, x[4], S24, 0xe7d3fbc8); /* 24 */ + GG(a, b, c, d, x[9], S21, 0x21e1cde6); /* 25 */ + GG(d, a, b, c, x[14], S22, 0xc33707d6); /* 26 */ + GG(c, d, a, b, x[3], S23, 0xf4d50d87); /* 27 */ + GG(b, c, d, a, x[8], S24, 0x455a14ed); /* 28 */ + GG(a, b, c, d, x[13], S21, 0xa9e3e905); /* 29 */ + GG(d, a, b, c, x[2], S22, 0xfcefa3f8); /* 30 */ + GG(c, d, a, b, x[7], S23, 0x676f02d9); /* 31 */ + GG(b, c, d, a, x[12], S24, 0x8d2a4c8a); /* 32 */ + + /* Round 3 */ + HH(a, b, c, d, x[5], S31, 0xfffa3942); /* 33 */ + HH(d, a, b, c, x[8], S32, 0x8771f681); /* 34 */ + HH(c, d, a, b, x[11], S33, 0x6d9d6122); /* 35 */ + HH(b, c, d, a, x[14], S34, 0xfde5380c); /* 36 */ + HH(a, b, c, d, x[1], S31, 0xa4beea44); /* 37 */ + HH(d, a, b, c, x[4], S32, 0x4bdecfa9); /* 38 */ + HH(c, d, a, b, x[7], S33, 0xf6bb4b60); /* 39 */ + HH(b, c, d, a, x[10], S34, 0xbebfbc70); /* 40 */ + HH(a, b, c, d, x[13], S31, 0x289b7ec6); /* 41 */ + HH(d, a, b, c, x[0], S32, 0xeaa127fa); /* 42 */ + HH(c, d, a, b, x[3], S33, 0xd4ef3085); /* 43 */ + HH(b, c, d, a, x[6], S34, 0x4881d05); /* 44 */ + HH(a, b, c, d, x[9], S31, 0xd9d4d039); /* 45 */ + HH(d, a, b, c, x[12], S32, 0xe6db99e5); /* 46 */ + HH(c, d, a, b, x[15], S33, 0x1fa27cf8); /* 47 */ + HH(b, c, d, a, x[2], S34, 0xc4ac5665); /* 48 */ + + /* Round 4 */ + II(a, b, c, d, x[0], S41, 0xf4292244); /* 49 */ + II(d, a, b, c, x[7], S42, 0x432aff97); /* 50 */ + II(c, d, a, b, x[14], S43, 0xab9423a7); /* 51 */ + II(b, c, d, a, x[5], S44, 0xfc93a039); /* 52 */ + II(a, b, c, d, x[12], S41, 0x655b59c3); /* 53 */ + II(d, a, b, c, x[3], S42, 0x8f0ccc92); /* 54 */ + II(c, d, a, b, x[10], S43, 0xffeff47d); /* 55 */ + II(b, c, d, a, x[1], S44, 0x85845dd1); /* 56 */ + II(a, b, c, d, x[8], S41, 0x6fa87e4f); /* 57 */ + II(d, a, b, c, x[15], S42, 0xfe2ce6e0); /* 58 */ + II(c, d, a, b, x[6], S43, 0xa3014314); /* 59 */ + II(b, c, d, a, x[13], S44, 0x4e0811a1); /* 60 */ + II(a, b, c, d, x[4], S41, 0xf7537e82); /* 61 */ + II(d, a, b, c, x[11], S42, 0xbd3af235); /* 62 */ + II(c, d, a, b, x[2], S43, 0x2ad7d2bb); /* 63 */ + II(b, c, d, a, x[9], S44, 0xeb86d391); /* 64 */ + + _state[0] += a; + _state[1] += b; + _state[2] += c; + _state[3] += d; +} + +/* Encodes input (ulong) into output (byte). Assumes length is +a multiple of 4. +*/ +void MD5::encode(const uint32 *input, uchar *output, size_t length) +{ + for (size_t i = 0, j = 0; j < length; ++i, j += 4) + { + output[j] = (uchar)(input[i] & 0xff); + output[j + 1] = (uchar)((input[i] >> 8) & 0xff); + output[j + 2] = (uchar)((input[i] >> 16) & 0xff); + output[j + 3] = (uchar)((input[i] >> 24) & 0xff); + } +} + +/* Decodes input (byte) into output (ulong). Assumes length is +a multiple of 4. +*/ +void MD5::decode(const uchar *input, uint32 *output, size_t length) +{ + for (size_t i = 0, j = 0; j < length; ++i, j += 4) + { + output[i] = ((uint32)input[j]) | (((uint32)input[j + 1]) << 8) | + (((uint32)input[j + 2]) << 16) | (((uint32)input[j + 3]) << 24); + } +} + +/* Convert byte array to hex string. */ +string MD5::bytesToHexString(const uchar *input, size_t length) +{ + string str; + str.reserve(length << 1); + for (size_t i = 0; i < length; ++i) + { + int t = input[i]; + int a = t / 16; + int b = t % 16; + str.append(1, HEX[a]); + str.append(1, HEX[b]); + } + return str; +} + +/* Convert digest to string value */ +string MD5::toString() +{ + return bytesToHexString(digest(), 16); +} \ No newline at end of file diff --git a/src/route/src/route_node.cpp b/src/route/src/route_node.cpp new file mode 100644 index 0000000..fea33f7 --- /dev/null +++ b/src/route/src/route_node.cpp @@ -0,0 +1,244 @@ +#include "rclcpp/rclcpp.hpp" +#include "sweeper_interfaces/msg/rtk.hpp" +#include "sweeper_interfaces/msg/sub.hpp" +#include +#include "md5.h" +#include "json.h" +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +std::mutex collect_mutex; // 互斥锁，用于保护 isCollecting 变量 + +#define deg_rad (0.01745329252) // Transfer from angle degree to rad +#define R_LATI (6378137) +#define R_LONT (5407872) // 这个需要根据所在区域的纬度进行换算：R_LONT = R_LATI*cos(所在地的纬度转化为弧度) +using namespace std; +/* 存储接收RTK的GPS点信息 */ +struct struct_rtk +{ + double lon; + double lat; + double head; + double speed; + int reliability; +}; +std::string filename; +pthread_t route_thread_t; +// 添加一个新的标志变量来跟踪采集状态 +bool isCollecting = false; +int first_flag = true; +struct_rtk g_rtk = {}; +struct_rtk last_g_rtk = {}; +string vid; +string upload_URL; +std::string destinationFilePath1 = "./gps_load_now.txt"; +void *route_thread(void *args); +string calculate_md5(string filename) +{ + MD5 md5; + ifstream file; + file.open(filename, ios::binary); + md5.update(file); + cout << md5.toString() << endl; + return md5.toString(); +} +bool upload_file(string filename) +{ + CURL *curl; + CURLcode ret; + curl = curl_easy_init(); + struct curl_httppost *post = NULL; + struct curl_httppost *last = NULL; + curl_formadd(&post, &last, CURLFORM_PTRNAME, "vid", CURLFORM_PTRCONTENTS, vid.c_str(), CURLFORM_END); // form-data key(path) 和 value(device_cover) + curl_formadd(&post, &last, CURLFORM_COPYNAME, "file", CURLFORM_FILE, filename.c_str(), CURLFORM_END); + curl_formadd(&post, &last, CURLFORM_COPYNAME, "md5", CURLFORM_COPYCONTENTS, calculate_md5(filename).c_str(), CURLFORM_END); + curl_easy_setopt(curl, CURLOPT_URL, upload_URL.c_str()); + curl_easy_setopt(curl, CURLOPT_HTTPPOST, post); + ret = curl_easy_perform(curl); + if (ret != CURLE_OK) + { + fprintf(stderr, "curl_easy_perform() failed: %s\n", curl_easy_strerror(ret)); + return false; + } + curl_formfree(post); + curl_easy_cleanup(curl); + cout << "上传成功" << endl; + return true; +} +void copyFileAndOverwrite(const std::string &sourceFilePath, const std::string &destinationFilePath) +{ + std::ifstream src(sourceFilePath, std::ios::binary); + std::ofstream dst(destinationFilePath, std::ios::binary); + + if (!src) + { + std::cerr << "无法打开源文件: " << sourceFilePath << std::endl; + return; + } + + if (!dst) + { + std::cerr << "无法打开目标文件: " << destinationFilePath << std::endl; + return; + } + + dst << src.rdbuf(); // 将源文件内容复制到目标文件 + + if (!dst) + { + std::cerr << "复制文件时出错" << std::endl; + } +} +class route_node : public rclcpp::Node +{ +public: + route_node(std::string name) : Node(name) + { + RCLCPP_INFO(this->get_logger(), "%s节点已经启动.", name.c_str()); + // 创建一个订阅者订阅话题 + sub_subscribe_ = this->create_subscription("gather", 10, std::bind(&route_node::sub_callback, this, std::placeholders::_1)); + + msg_subscribe_ = this->create_subscription("rtk_message", 10, std::bind(&route_node::msg_callback, this, std::placeholders::_1)); + } + +private: + // 收到话题数据的回调函数 + void msg_callback(const sweeper_interfaces::msg::Rtk::SharedPtr msg) + { + if (msg->p_quality < 4 || msg->h_quality < 4) + { + g_rtk.reliability = 0; + RCLCPP_INFO(this->get_logger(), "rtk信号差!"); + } + if (first_flag) + { + last_g_rtk.reliability = 1; + last_g_rtk.lat = msg->lat; + last_g_rtk.lon = msg->lon; + last_g_rtk.head = msg->head; + first_flag = false; + } + g_rtk.reliability = 1; + g_rtk.lat = msg->lat; + g_rtk.lon = msg->lon; + g_rtk.head = msg->head; + + if (!isCollecting) + { + cout << "等待采集...." << endl; + } + else + { + cout << "平台采集中" << endl; + } + } + void sub_callback(const sweeper_interfaces::msg::Sub::SharedPtr msg) + { + if (msg->get_route && !isCollecting) + { + std::lock_guard lock(collect_mutex); // 加锁 + isCollecting = true; + cout << "平台开始采集" << endl; + pthread_create(&route_thread_t, NULL, route_thread, NULL); + } + else if (!msg->get_route && isCollecting) + { + isCollecting = false; + cout << "平台结束采集" << endl; + pthread_cancel(route_thread_t); + if (upload_file(filename)) + { + cout << "上传成功" << endl; + } + else + { + cout << "上传失败" << endl; + } + } + } + // 声明订阅者 + rclcpp::Subscription::SharedPtr sub_subscribe_; + rclcpp::Subscription::SharedPtr msg_subscribe_; +}; + +double ntzx_GPS_length(double lonti1, double lati1, double lonti2, double lati2) +{ + double x, y, length; + x = (R_LONT) * (lonti2 - lonti1) * deg_rad; // 弧长公式 + y = (R_LATI) * (lati2 - lati1) * deg_rad; + length = sqrt(x * x + y * y); + return length; +} + +void *route_thread(void *args) +{ + (void)args; + filename = ""; + + // 获取当前时间点 + auto now = std::chrono::system_clock::now(); + // 转换为时间戳（毫秒级） + auto timestamp = std::chrono::duration_cast(now.time_since_epoch()).count(); + + // 将时间戳转换为字符串 + std::ostringstream oss; + oss << "routes/gps_load_" << timestamp << ".txt"; + filename = oss.str(); + + FILE *fp_routes = fopen(filename.c_str(), "w"); + usleep(5000); + while (1) + { + double distance = ntzx_GPS_length(last_g_rtk.lon, last_g_rtk.lat, g_rtk.lon, g_rtk.lat); + if (distance >= 1.0) // 每隔1米采样一次 + { + fprintf(fp_routes, "%.10lf\n%.11lf\n%lf\n%lf\n", g_rtk.lat, g_rtk.lon, g_rtk.head, 0.0); + fflush(fp_routes); + last_g_rtk.reliability = g_rtk.reliability; + last_g_rtk.lat = g_rtk.lat; + last_g_rtk.lon = g_rtk.lon; + last_g_rtk.head = g_rtk.head; + printf("hit!\n"); + } + } +} + +void init_main() +{ + Json::Reader reader; + Json::Value root; + std::ifstream in("config.json", std::ios::binary); + if (!in.is_open()) + { + std::cout << "read config file error" << std::endl; + return; + } + if (reader.parse(in, root)) + { + vid = root["mqtt"]["vid"].asString(); + upload_URL = root["mqtt"]["upload_url"].asString(); + cout << "vid:" << vid << endl; + } + in.close(); // 关闭文件流 +} + +int main(int argc, char **argv) +{ + init_main(); + rclcpp::init(argc, argv); + /*创建对应节点的共享指针对象*/ + auto node = std::make_shared("route_node"); + /* 运行节点，并检测退出信号*/ + rclcpp::spin(node); + rclcpp::shutdown(); + // pthread_join(route_thread_t, NULL); + return 0; +} diff --git a/src/rslidar_pointcloud_merger/CMakeLists.txt b/src/rslidar_pointcloud_merger/CMakeLists.txt new file mode 100644 index 0000000..811ca18 --- /dev/null +++ b/src/rslidar_pointcloud_merger/CMakeLists.txt @@ -0,0 +1,39 @@ +cmake_minimum_required(VERSION 3.8) +cmake_policy(SET CMP0074 NEW) +project(rslidar_pointcloud_merger) + +# 1. 默认开启 C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(pcl_conversions REQUIRED) +find_package(pcl_ros REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_sensor_msgs REQUIRED) +find_package(PCL REQUIRED COMPONENTS common io) + +include_directories( + include + ${PCL_INCLUDE_DIRS} +) + +add_executable(merge_two_lidars src/merge_two_lidars.cpp) +ament_target_dependencies(merge_two_lidars + rclcpp sensor_msgs pcl_conversions pcl_ros + tf2 tf2_ros tf2_sensor_msgs +) + +target_link_libraries(merge_two_lidars ${PCL_LIBRARIES}) + +install(TARGETS merge_two_lidars + DESTINATION lib/${PROJECT_NAME}) + +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME}) + +ament_package() diff --git a/src/rslidar_pointcloud_merger/launch/merge_two_lidars.launch.py b/src/rslidar_pointcloud_merger/launch/merge_two_lidars.launch.py new file mode 100644 index 0000000..88fd1c7 --- /dev/null +++ b/src/rslidar_pointcloud_merger/launch/merge_two_lidars.launch.py @@ -0,0 +1,93 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +from pathlib import Path + + +def generate_launch_description(): + pkg_share = get_package_share_directory("rslidar_pointcloud_merger") + + # front_lidar（右前角） -> rslidar（车辆中心） + # [x, y, z, yaw, pitch, roll] + tf_front = [ + 1.15, # x 前方 1.15 m + 0.66, # y 右侧 0.66 m + 0.0, # z 高度 1.7 m + 0.7854, # yaw π/4 ≈ 0.785 rad (水平右前 45°) + 0.0, # pitch + 0.0, # roll + ] + + # rear_lidar（左后角） -> rslidar（车辆中心） + # [x, y, z, yaw, pitch, roll] + tf_rear = [ + -1.15, # x 后方 1.15 m + -0.66, # y 左侧 0.66 m + 0.0, # z 高度 1.7 m + 3.9270, # yaw +5π/4 ≈ +3.927rad (水平后左 135°) + 0.0, # pitch + 0.0, # roll + ] + + return LaunchDescription( + [ + # ---------- 静态 TF (front) ---------- + Node( + package="tf2_ros", + executable="static_transform_publisher", + name="static_tf_front", + arguments=[*map(str, tf_front), "rslidar", "front_lidar"], + # arguments=[*map(str, tf_front), "rslidar", "front_lidar", "100"], + output="log", + ), + # ---------- 静态 TF (rear) ---------- + Node( + package="tf2_ros", + executable="static_transform_publisher", + name="static_tf_rear", + arguments=[*map(str, tf_rear), "rslidar", "rear_lidar"], + # arguments=[*map(str, tf_rear), "rslidar", "rear_lidar", "100"], + output="log", + ), + # ---------- 点云合并节点 ---------- + Node( + package="rslidar_pointcloud_merger", + executable="merge_two_lidars", + name="lidar_merger", + parameters=[ + { + "front_topic": "/rslidar_points/front_lidar", + "rear_topic": "/rslidar_points/rear_lidar", + "output_topic": "/rslidar_points", + "frame_id": "rslidar", + "queue_size": 3, + "cache_size": 10, + "time_tolerance": 0.1, + "max_wait_time": 1.0, + "enable_debug": False, + # 点云处理参数 + "filter_min_x": -5.0, # X轴最小坐标（米） + "filter_max_x": 10.0, # X轴最大坐标（米） + "filter_min_y": -5.0, # Y轴最小坐标（米） + "filter_max_y": 5.0, # Y轴最大坐标（米） + "filter_min_z": 0.0, # Z轴最小坐标（米） + "filter_max_z": 1.65, # Z轴最大坐标（米） + "voxel_size": 0.1, # 体素网格大小（米） 0.1~0.3m + "stat_mean_k": 30, # 计算点的平均距离时考虑的邻居数 + "stat_std_thresh": 1.5, # 标准差倍数阈值 噪声较多 ：0.5~1.0。 噪声较少 ：1.0~2.0。 + "grid_size": 50, # 栅格矩阵的边长（单元格数） + "grid_range": 15.0, # 栅格覆盖的实际空间范围（米） + # 单元格尺寸：由 grid_range / grid_size 决定 + "enable_print": False, # 是否打印栅格 + # 车身过滤参数 + "filter_car": True, # 是否启用车身过滤 + "car_length": 2.4, # 车长（米） + "car_width": 1.4, # 车宽（米） + "car_lidar_offset_x": 0.0, # LiDAR在x轴的安装偏移 + "car_lidar_offset_y": 0.0, # LiDAR在y轴的安装偏移 + } + ], + output="screen", + ), + ] + ) diff --git a/src/rslidar_pointcloud_merger/package.xml b/src/rslidar_pointcloud_merger/package.xml new file mode 100644 index 0000000..2f90f3a --- /dev/null +++ b/src/rslidar_pointcloud_merger/package.xml @@ -0,0 +1,26 @@ + + + rslidar_pointcloud_merger + 0.1.0 + Merge two RSLiDAR point clouds into one topic. + + Your Name + Apache-2.0 + + ament_cmake + + + rclcpp + sensor_msgs + pcl_conversions + pcl_ros + tf2 + tf2_ros + tf2_sensor_msgs + + rviz2 + + + ament_cmake + + \ No newline at end of file diff --git a/src/rslidar_pointcloud_merger/src/merge_two_lidars.cpp b/src/rslidar_pointcloud_merger/src/merge_two_lidars.cpp new file mode 100644 index 0000000..d4b80d8 --- /dev/null +++ b/src/rslidar_pointcloud_merger/src/merge_two_lidars.cpp @@ -0,0 +1,914 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +using sensor_msgs::msg::PointCloud2; +using std::placeholders::_1; + +// 点云帧结构体 +struct CloudFrame +{ + std::shared_ptr cloud; + rclcpp::Time stamp; + rclcpp::Time received_time; // 添加接收时间用于延时分析 + std::string source; +}; + +// 内存池实现 +class PointCloud2Pool +{ +public: + std::shared_ptr acquire() + { + std::lock_guard lock(mutex_); + if (pool_.empty()) + { + return std::make_shared(); + } + else + { + auto cloud = std::move(pool_.back()); + pool_.pop_back(); + return cloud; + } + } + + void release(std::shared_ptr cloud) + { + if (!cloud) + return; + + cloud->data.clear(); + cloud->width = 0; + cloud->height = 0; + cloud->row_step = 0; + + std::lock_guard lock(mutex_); + if (pool_.size() < 10) // 限制池大小 + { + pool_.push_back(cloud); + } + } + +private: + std::vector> pool_; + std::mutex mutex_; +}; + +class LidarMerger : public rclcpp::Node +{ +public: + LidarMerger() : Node("lidar_merger") + { + /* ---------- 参数 ---------- */ + front_topic_ = declare_parameter("front_topic", "/rslidar_points/front_lidar"); + rear_topic_ = declare_parameter("rear_topic", "/rslidar_points/rear_lidar"); + output_topic_ = declare_parameter("output_topic", "/rslidar_points"); + target_frame_ = declare_parameter("frame_id", "rslidar"); + queue_size_ = declare_parameter("queue_size", 3); // 减小队列大小 + cache_size_ = declare_parameter("cache_size", 10); // 增加缓存以应对延时 + time_tolerance_ = declare_parameter("time_tolerance", 0.1); // 放宽时间容差 + max_wait_time_ = declare_parameter("max_wait_time", 1.0); // 增加到1.0s以适应实际延时 + enable_debug_ = declare_parameter("enable_debug", false); // 默认关闭调试减少日志开销 + + // 点云处理参数 + filter_min_x_ = declare_parameter("filter_min_x", -10.0f); + filter_max_x_ = declare_parameter("filter_max_x", 10.0f); + filter_min_y_ = declare_parameter("filter_min_y", -10.0f); + filter_max_y_ = declare_parameter("filter_max_y", 10.0f); + filter_min_z_ = declare_parameter("filter_min_z", -2.0f); + filter_max_z_ = declare_parameter("filter_max_z", 2.0f); + voxel_size_ = declare_parameter("voxel_size", 0.1f); // 降采样体素大小 + stat_mean_k_ = declare_parameter("stat_mean_k", 30); // 统计离群点去除的k值 + stat_std_thresh_ = declare_parameter("stat_std_thresh", 1.0f); // 统计离群点去除的标准差阈值 + grid_size_ = declare_parameter("grid_size", 100); // 栅格大小 + grid_range_ = declare_parameter("grid_range", 20.0f); // 栅格范围 + enable_print_ = declare_parameter("enable_print", true); + + // 车身过滤参数 + filter_car_ = declare_parameter("filter_car", true); // 是否启用车身过滤 + car_length_ = declare_parameter("car_length", 2.3f); // 车长（米） + car_width_ = declare_parameter("car_width", 1.32f); // 车宽（米） + car_lidar_offset_x_ = declare_parameter("car_lidar_offset_x", 0.0f); // LiDAR在x轴的安装偏移 + car_lidar_offset_y_ = declare_parameter("car_lidar_offset_y", 0.0f); // LiDAR在y轴的安装偏移 + + // 打印所有参数值（添加到构造函数末尾） + RCLCPP_INFO_STREAM(this->get_logger(), + "\n\n---------- 点云融合节点参数配置 ----------" + << "\n [输入输出]" + << "\n 前雷达话题: " << front_topic_ + << "\n 后雷达话题: " << rear_topic_ + << "\n 输出话题: " << output_topic_ + << "\n 目标坐标系: " << target_frame_ + << "\n [同步参数]" + << "\n 队列大小: " << queue_size_ + << "\n 缓存大小: " << cache_size_ + << "\n 时间容差(s): " << time_tolerance_ + << "\n 最大等待时间(s): " << max_wait_time_ + << "\n [调试选项]" + << "\n 调试模式: " << (enable_debug_ ? "开启" : "关闭") + << "\n [点云处理]" + << "\n 空间过滤范围(m): [" << filter_min_x_ << ", " << filter_max_x_ + << "] x [" << filter_min_y_ << ", " << filter_max_y_ + << "] x [" << filter_min_z_ << ", " << filter_max_z_ << "]" + << "\n 体素大小(m): " << voxel_size_ + << "\n 离群点k值: " << stat_mean_k_ + << "\n 离群点标准差阈值: " << stat_std_thresh_ + << "\n 栅格大小: " << grid_size_ << "x" << grid_size_ + << "\n 栅格范围(m): " << grid_range_ + << "\n 打印栅格: " << (enable_print_ ? "开启" : "关闭") + << "\n [车身过滤]" + << "\n 启用车身过滤: " << (filter_car_ ? "是" : "否") + << "\n 车身尺寸(m): " << car_length_ << " x " << car_width_ + << "\n LiDAR偏移(m): (" << car_lidar_offset_x_ << ", " << car_lidar_offset_y_ << ")" + << "\n--------------------------------------------\n"); + + /* ---------- TF ---------- */ + tf_buffer_ = std::make_shared(get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + + /* ---------- QoS - 优化为低延时 ---------- */ + auto qos = rclcpp::QoS(rclcpp::KeepLast(queue_size_)) + .best_effort() + .durability_volatile(); + // 移除deadline约束，避免因延时导致的消息丢弃 + + /* ---------- 订阅 ---------- */ + sub_front_ = create_subscription( + front_topic_, qos, std::bind(&LidarMerger::frontCB, this, std::placeholders::_1)); + sub_rear_ = create_subscription( + rear_topic_, qos, std::bind(&LidarMerger::rearCB, this, std::placeholders::_1)); + + /* ---------- 发布 ---------- */ + auto pub_qos = rclcpp::QoS(rclcpp::KeepLast(queue_size_)) + .reliable() + .durability_volatile(); + pub_ = create_publisher(output_topic_, pub_qos); + + // 创建栅格发布者 + grid_pub_ = this->create_publisher("grid_raw", 10); + + // 统计信息 + last_stats_time_ = now(); + front_count_ = 0; + rear_count_ = 0; + merged_count_ = 0; + + RCLCPP_INFO(get_logger(), "LidarMerger started with cache_size=%d, time_tolerance=%.3fs, max_wait=%.3fs", + cache_size_, time_tolerance_, max_wait_time_); + } + +private: + /* ---------- 回调函数 - 同步处理 ---------- */ + void frontCB(const PointCloud2::SharedPtr msg) + { + auto receive_time = now(); + front_count_++; + + if (enable_debug_) + { + auto data_delay = (receive_time - rclcpp::Time(msg->header.stamp)).seconds(); + RCLCPP_DEBUG(get_logger(), "Front cloud received, data_delay=%.3fs", data_delay); + } + + processCloudSync(msg, "front", receive_time); + } + + void rearCB(const PointCloud2::SharedPtr msg) + { + auto receive_time = now(); + rear_count_++; + + if (enable_debug_) + { + auto data_delay = (receive_time - rclcpp::Time(msg->header.stamp)).seconds(); + RCLCPP_DEBUG(get_logger(), "Rear cloud received, data_delay=%.3fs", data_delay); + } + + processCloudSync(msg, "rear", receive_time); + } + + /* ---------- 同步处理点云 ---------- */ + void processCloudSync(const PointCloud2::SharedPtr msg, const std::string &source, + const rclcpp::Time &receive_time) + { + // 立即进行坐标变换 + auto transformed_cloud = transformCloud(*msg); + if (!transformed_cloud) + { + RCLCPP_WARN(get_logger(), "Failed to transform %s cloud", source.c_str()); + return; + } + + // 创建CloudFrame + auto cloud_frame = std::make_shared(CloudFrame{ + transformed_cloud, + rclcpp::Time(msg->header.stamp), + receive_time, + source}); + + // 添加到缓存并立即尝试合并 + { + std::lock_guard lock(cache_mutex_); + addToCache(cloud_frame); + + // 立即尝试合并，无需等待定时器 + tryMerge(); + } + + // 定期输出统计信息 + printStats(); + } + + /* ---------- 添加到缓存 ---------- */ + void addToCache(std::shared_ptr frame) + { + auto &cache = (frame->source == "front") ? front_clouds_ : rear_clouds_; + + // 保持时间排序（最新的在前） + auto it = cache.begin(); + while (it != cache.end() && (*it)->stamp > frame->stamp) + { + ++it; + } + cache.insert(it, frame); + + // 限制缓存大小 + while (cache.size() > cache_size_) + { + cloud_pool_.release(cache.back()->cloud); + cache.pop_back(); + } + } + + /* ---------- 尝试合并 - 触发式 ---------- */ + void tryMerge() + { + if (front_clouds_.empty() || rear_clouds_.empty()) + return; + + // 查找最佳匹配 + auto [front_frame, rear_frame] = findBestMatchOptimized(); + + if (!front_frame || !rear_frame) + return; + + // 自适应延时检查 - 基于实际网络条件调整 + auto now_time = now(); + auto front_age = (now_time - front_frame->received_time).seconds(); + auto rear_age = (now_time - rear_frame->received_time).seconds(); + + // 动态调整max_wait_time基于观察到的延时 + static double observed_max_delay = 0.0; + double current_max_delay = std::max(front_age, rear_age); + observed_max_delay = std::max(observed_max_delay * 0.95, current_max_delay); // 指数衰减 + + // 使用观察到的延时来决定是否处理，而不是固定阈值 + double adaptive_threshold = std::min(max_wait_time_, observed_max_delay + 0.1); + + if (front_age > adaptive_threshold || rear_age > adaptive_threshold) + { + if (enable_debug_) + { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, + "Data age check: front_age=%.3fs, rear_age=%.3fs, threshold=%.3fs", + front_age, rear_age, adaptive_threshold); + } + + // 不直接返回，而是清理最旧的数据后重试 + cleanupExpiredClouds(); + if (!front_clouds_.empty() && !rear_clouds_.empty()) + { + // 递归重试一次 + static int retry_count = 0; + if (retry_count < 2) + { + retry_count++; + tryMerge(); + retry_count--; + } + } + return; + } + + // 合并点云 + auto merge_start = std::chrono::high_resolution_clock::now(); + auto merged = mergeClouds(*front_frame, *rear_frame); + auto merge_end = std::chrono::high_resolution_clock::now(); + + if (!merged) + return; + + // 设置时间戳和frame_id + auto front_time = front_frame->stamp.nanoseconds(); + auto rear_time = rear_frame->stamp.nanoseconds(); + merged->header.stamp = rclcpp::Time((front_time + rear_time) / 2, RCL_ROS_TIME); + merged->header.frame_id = target_frame_; + + // 发布合并后的点云 + // pub_->publish(*merged); + merged_count_++; + + // 将ROS点云消息转换为PCL点云 + pcl::PointCloud::Ptr merged_pcl(new pcl::PointCloud); + pcl::fromROSMsg(*merged, *merged_pcl); + + // ========================= 点云 ========================= + auto processed_pcl = processPointCloud(merged_pcl); + if (!processed_pcl) + return; + + // 将处理后的PCL点云转回ROS消息 + auto processed = std::make_shared(); + pcl::toROSMsg(*processed_pcl, *processed); + processed->header = merged->header; + + // 发布处理后的点云 + pub_->publish(*processed); + // ========================= 点云 ========================= + // ========================= 栅格 ========================= + auto grid = processPointCloud_grid(merged_pcl); + if (grid.empty()) + return; + + // 可视化栅格 + if (enable_print_) + visualizeGridInTerminal(grid); + + // 发布栅格到ROS话题 + publishGrid(grid); + // ========================= 栅格 ========================= + + // 延时分析（降低频率） + if (enable_debug_ && merged_count_ % 10 == 0) // 每10次输出一次 + { + auto total_delay = (now_time - processed->header.stamp).seconds(); + auto process_time = std::chrono::duration(std::chrono::high_resolution_clock::now() - merge_start).count(); + + RCLCPP_INFO(get_logger(), + "Merged #%d: points=%zu → %zu, total_delay=%.3fs, process_time=%.3fms, adaptive_threshold=%.3fs", + merged_count_, merged->data.size() / merged->point_step, + processed->data.size() / processed->point_step, + total_delay, process_time * 1000, adaptive_threshold); + } + + // 清理已使用的点云 + removeUsedClouds(front_frame, rear_frame); + } + + /* ---------- 点云处理流程 ---------- */ + pcl::PointCloud::Ptr processPointCloud(const pcl::PointCloud::Ptr &cloud) + { + if (!cloud || cloud->empty()) + return nullptr; + + // 1. 条件过滤 + auto filtered = applyConditionalFiltering(cloud); + if (filtered->empty()) + { + RCLCPP_WARN(get_logger(), "[processPointCloud] Filtered cloud is empty!"); + return cloud; // 返回原始云而不是nullptr + } + + // 2. 降采样 + auto downsampled = applyVoxelGridFiltering(filtered); + if (downsampled->empty()) + { + RCLCPP_WARN(get_logger(), "[processPointCloud] Downsampled cloud is empty!"); + return filtered; + } + + // 3. 离群点去除 + auto outlier_removed = applyStatisticalOutlierRemoval(downsampled); + if (outlier_removed->empty()) + { + RCLCPP_WARN(get_logger(), "[processPointCloud] Outlier-removed cloud is empty!"); + return downsampled; + } + + return outlier_removed; + } + std::vector> processPointCloud_grid(const pcl::PointCloud::Ptr &cloud) + { + if (!cloud || cloud->empty()) + return {}; // 返回空栅格 + + // 1. 条件过滤 + auto filtered = applyConditionalFiltering(cloud); + if (filtered->empty()) + { + RCLCPP_WARN(get_logger(), "[processPointCloud_grid] Filtered cloud is empty!"); + return {}; + } + + // 2. 降采样 + auto downsampled = applyVoxelGridFiltering(filtered); + if (downsampled->empty()) + { + RCLCPP_WARN(get_logger(), "[processPointCloud_grid] Downsampled cloud is empty!"); + return {}; + } + + // 3. 离群点去除 + auto outlier_removed = applyStatisticalOutlierRemoval(downsampled); + if (outlier_removed->empty()) + { + RCLCPP_WARN(get_logger(), "[processPointCloud_grid] Outlier-removed cloud is empty!"); + return {}; + } + + // 4. 栅格化处理并返回栅格结果 + return generateOccupancyGrid(outlier_removed); + } + + /* ---------- 条件过滤 ---------- */ + pcl::PointCloud::Ptr applyConditionalFiltering(const pcl::PointCloud::Ptr &cloud) + { + pcl::PointCloud::Ptr filtered(new pcl::PointCloud); + + for (const auto &point : cloud->points) + { + // 自定义过滤条件：保留特定区域内的点 + if (point.x > filter_min_x_ && point.x < filter_max_x_ && + point.y > filter_min_y_ && point.y < filter_max_y_ && + point.z > filter_min_z_ && point.z < filter_max_z_) + { + filtered->points.push_back(point); + } + } + + filtered->width = filtered->points.size(); + filtered->height = 1; + filtered->is_dense = true; + + // 不启用车身过滤，直接返回 + if (!filter_car_) + return filtered; + // RCLCPP_INFO(get_logger(), "启用车身过滤"); + + // 使用CropBox移除车身区域 + pcl::CropBox crop; + crop.setInputCloud(filtered); + + // 设置车身区域（最小点和最大点） + Eigen::Vector4f min_point(car_lidar_offset_x_ - car_length_ / 2.0f, + car_lidar_offset_y_ - car_width_ / 2.0f, + filter_min_z_, 1.0); + + Eigen::Vector4f max_point(car_lidar_offset_x_ + car_length_ / 2.0f, + car_lidar_offset_y_ + car_width_ / 2.0f, + filter_max_z_, 1.0); + + crop.setMin(min_point); + crop.setMax(max_point); + crop.setNegative(true); // true表示保留外部点，false表示保留内部点 + + pcl::PointCloud::Ptr output(new pcl::PointCloud); + crop.filter(*output); + + // return output; + + // 右侧主刷 (x:0.85~1.25, y:0.6~0.96, z:1.2~1.65) + pcl::CropBox crop1; + crop1.setInputCloud(output); + Eigen::Vector4f min1(0.85f, 0.6f, 1.2f, 1.0); // 右侧主刷最小点 + Eigen::Vector4f max1(1.25f, 0.96f, 1.65f, 1.0); // 右侧主刷最大点 + crop1.setMin(min1); + crop1.setMax(max1); + crop1.setNegative(true); // 剔除右侧主刷内的点 + + pcl::PointCloud::Ptr output1(new pcl::PointCloud); + crop1.filter(*output1); + + // 左侧主刷 - 右侧主刷的X轴对称（y坐标取相反数） + pcl::CropBox crop2; + crop2.setInputCloud(output1); + // X轴对称：将y坐标取相反数，保持x和z坐标不变 + Eigen::Vector4f min2(0.85f, -0.96f, 1.2f, 1.0); // 左侧主刷最小点（y取反后注意交换大小） + Eigen::Vector4f max2(1.25f, -0.6f, 1.65f, 1.0); // 左侧主刷最大点（y取反后注意交换大小） + crop2.setMin(min2); + crop2.setMax(max2); + crop2.setNegative(true); // 剔除左侧主刷内的点 + + pcl::PointCloud::Ptr final_output(new pcl::PointCloud); + crop2.filter(*final_output); + + return final_output; // 剔除两侧主刷 + } + + /* ---------- 降采样（体素网格过滤） ---------- */ + pcl::PointCloud::Ptr applyVoxelGridFiltering(const pcl::PointCloud::Ptr &cloud) + { + pcl::PointCloud::Ptr downsampled(new pcl::PointCloud); + pcl::VoxelGrid vg; + + vg.setInputCloud(cloud); + vg.setLeafSize(voxel_size_, voxel_size_, voxel_size_); + vg.filter(*downsampled); + + return downsampled; + } + + /* ---------- 离群点去除 ---------- */ + pcl::PointCloud::Ptr applyStatisticalOutlierRemoval(const pcl::PointCloud::Ptr &cloud) + { + pcl::PointCloud::Ptr filtered(new pcl::PointCloud); + pcl::StatisticalOutlierRemoval sor; + + sor.setInputCloud(cloud); + sor.setMeanK(stat_mean_k_); // 考虑每个点的k个最近邻 + sor.setStddevMulThresh(stat_std_thresh_); // 标准差倍数阈值 + sor.filter(*filtered); + + return filtered; + } + + /* ---------- 栅格化处理 ---------- */ + std::vector> generateOccupancyGrid(const pcl::PointCloud::Ptr &cloud) + { + // 0:空 1:障碍物 2:车体 + std::vector> grid(grid_size_, std::vector(grid_size_, 0)); + float resolution = grid_range_ / static_cast(grid_size_); + + // 标记车体区域 + if (filter_car_) + { + // 计算车体在栅格中的位置 + int car_min_x = static_cast((grid_range_ / 2 - (car_lidar_offset_x_ + car_length_ / 2.0f)) / resolution); + int car_max_x = static_cast((grid_range_ / 2 - (car_lidar_offset_x_ - car_length_ / 2.0f)) / resolution); + int car_min_y = static_cast((-car_lidar_offset_y_ - car_width_ / 2.0f + grid_range_ / 2) / resolution); + int car_max_y = static_cast((-car_lidar_offset_y_ + car_width_ / 2.0f + grid_range_ / 2) / resolution); + + // 确保索引在有效范围内 + car_min_x = std::max(0, std::min(car_min_x, grid_size_ - 1)); + car_max_x = std::max(0, std::min(car_max_x, grid_size_ - 1)); + car_min_y = std::max(0, std::min(car_min_y, grid_size_ - 1)); + car_max_y = std::max(0, std::min(car_max_y, grid_size_ - 1)); + + // 在栅格中标记车体区域 + for (int x = car_min_x; x <= car_max_x; ++x) + { + for (int y = car_min_y; y <= car_max_y; ++y) + { + grid[x][y] = 2; + } + } + } + + // 标记点云障碍物 + for (const auto &point : cloud->points) + { + // X轴（前向）映射到行索引（从上到下为正X到负X） + int grid_x = static_cast((grid_range_ / 2 - point.x) / resolution); + + // Y轴（右向）映射到列索引（从左到右为负Y到正Y） + int grid_y = static_cast((point.y + grid_range_ / 2) / resolution); + + if (grid_x >= 0 && grid_x < grid_size_ && grid_y >= 0 && grid_y < grid_size_) + { + // 只有当该位置不是车体时才标记为障碍物 + if (grid[grid_x][grid_y] != 2) + { + grid[grid_x][grid_y] = 1; + } + } + } + + return grid; + } + + /* ---------- 可视化栅格 - 终端打印 ---------- */ + void visualizeGridInTerminal(const std::vector> &grid) + { + std::system("clear"); + + std::cout << " -----------------------------障碍物矩阵------------------------------- " << std::endl; + + // 打印顶部边框 + std::cout << " " << std::string(grid[0].size(), '-') << std::endl; + + // 打印列号（仅0-9） + std::cout << " "; + for (size_t i = 0; i < grid[0].size(); i++) + { + std::cout << (i % 10); + } + std::cout << std::endl; + + // 打印栅格内容（行号 + 栅格） + for (size_t i = 0; i < grid.size(); i++) + { + // 行号显示（两位数，右对齐） + std::cout << (i < 10 ? " " : "") << i << "|"; + + // 栅格内容 + for (int cell : grid[i]) + { + // 根据栅格值选择显示符号 + std::cout << (cell == 1 ? "@" : (cell == 2 ? " " : ".")); + } + + std::cout << "|" << std::endl; + } + + // 打印底部边框 + std::cout << " " << std::string(grid[0].size(), '-') << std::endl; + } + + /* ---------- 发布栅格 ---------- */ + void publishGrid(const std::vector> &grid) + { + // 创建Int32MultiArray消息 + auto msg = std_msgs::msg::Int32MultiArray(); + + // 设置维度信息 + msg.layout.dim.resize(2); + msg.layout.dim[0].label = "height"; // 行（对应X轴） + msg.layout.dim[0].size = grid.size(); + msg.layout.dim[0].stride = grid.size() * grid[0].size(); + + msg.layout.dim[1].label = "width"; // 列（对应Y轴） + msg.layout.dim[1].size = grid[0].size(); + msg.layout.dim[1].stride = grid[0].size(); + + // 设置数据（按行优先展开） + msg.data.clear(); + for (const auto &row : grid) + { + msg.data.insert(msg.data.end(), row.begin(), row.end()); + } + + // 发布消息 + grid_pub_->publish(msg); + } + /* ---------- 优化的匹配算法 ---------- */ + std::pair, std::shared_ptr> findBestMatchOptimized() + { + if (front_clouds_.empty() || rear_clouds_.empty()) + return {nullptr, nullptr}; + + // 快速策略：优先使用最新的点云进行匹配 + auto front_candidate = front_clouds_.front(); + auto rear_candidate = rear_clouds_.front(); + + // 检查时间差 + auto time_diff = std::abs((front_candidate->stamp - rear_candidate->stamp).seconds()); + + if (time_diff <= time_tolerance_) + { + return {front_candidate, rear_candidate}; + } + + // 如果最新的不匹配，寻找更好的组合（但限制搜索范围） + std::shared_ptr best_front = nullptr; + std::shared_ptr best_rear = nullptr; + double min_diff = std::numeric_limits::max(); + + // 限制搜索范围到前3个元素 + size_t search_limit = std::min(3, std::min(front_clouds_.size(), rear_clouds_.size())); + + for (size_t i = 0; i < search_limit && i < front_clouds_.size(); ++i) + { + for (size_t j = 0; j < search_limit && j < rear_clouds_.size(); ++j) + { + auto diff = std::abs((front_clouds_[i]->stamp - rear_clouds_[j]->stamp).seconds()); + if (diff < min_diff) + { + min_diff = diff; + best_front = front_clouds_[i]; + best_rear = rear_clouds_[j]; + } + } + } + + if (min_diff <= time_tolerance_) + { + return {best_front, best_rear}; + } + + return {nullptr, nullptr}; + } + + /* ---------- 移除已使用的点云 ---------- */ + void removeUsedClouds(std::shared_ptr used_front, + std::shared_ptr used_rear) + { + // 从front_clouds_中移除 + auto front_it = std::find(front_clouds_.begin(), front_clouds_.end(), used_front); + if (front_it != front_clouds_.end()) + { + cloud_pool_.release((*front_it)->cloud); + front_clouds_.erase(front_it); + } + + // 从rear_clouds_中移除 + auto rear_it = std::find(rear_clouds_.begin(), rear_clouds_.end(), used_rear); + if (rear_it != rear_clouds_.end()) + { + cloud_pool_.release((*rear_it)->cloud); + rear_clouds_.erase(rear_it); + } + + // 清理过期的点云 + cleanupExpiredClouds(); + } + + /* ---------- 清理过期点云 ---------- */ + void cleanupExpiredClouds() + { + auto now_time = now(); + auto cleanup_threshold = max_wait_time_; // 使用配置的等待时间 + + // 清理front_clouds_中过期的数据 + while (!front_clouds_.empty()) + { + auto age = (now_time - front_clouds_.back()->received_time).seconds(); + if (age > cleanup_threshold) + { + if (enable_debug_) + { + RCLCPP_DEBUG(get_logger(), "Cleaning up old front cloud, age=%.3fs", age); + } + cloud_pool_.release(front_clouds_.back()->cloud); + front_clouds_.pop_back(); + } + else + { + break; + } + } + + // 清理rear_clouds_中过期的数据 + while (!rear_clouds_.empty()) + { + auto age = (now_time - rear_clouds_.back()->received_time).seconds(); + if (age > cleanup_threshold) + { + if (enable_debug_) + { + RCLCPP_DEBUG(get_logger(), "Cleaning up old rear cloud, age=%.3fs", age); + } + cloud_pool_.release(rear_clouds_.back()->cloud); + rear_clouds_.pop_back(); + } + else + { + break; + } + } + } + + /* ---------- 合并点云 ---------- */ + std::shared_ptr mergeClouds(const CloudFrame &front, const CloudFrame &rear) + { + size_t front_points = front.cloud->data.size() / front.cloud->point_step; + size_t rear_points = rear.cloud->data.size() / rear.cloud->point_step; + + if (front.cloud->fields != rear.cloud->fields || + front.cloud->point_step != rear.cloud->point_step || + front.cloud->is_bigendian != rear.cloud->is_bigendian) + { + RCLCPP_ERROR(get_logger(), "PointCloud2 formats do not match!"); + return nullptr; + } + + auto merged = cloud_pool_.acquire(); + + // 复制元数据 + merged->header = front.cloud->header; + merged->fields = front.cloud->fields; + merged->is_bigendian = front.cloud->is_bigendian; + merged->point_step = front.cloud->point_step; + merged->height = 1; + merged->width = front_points + rear_points; + + // 高效合并数据 + merged->data.clear(); + merged->data.reserve(front.cloud->data.size() + rear.cloud->data.size()); + merged->data.insert(merged->data.end(), front.cloud->data.begin(), front.cloud->data.end()); + merged->data.insert(merged->data.end(), rear.cloud->data.begin(), rear.cloud->data.end()); + merged->row_step = merged->data.size(); + + return merged; + } + + /* ---------- 坐标变换 - 简化版 ---------- */ + std::shared_ptr transformCloud(const PointCloud2 &in) + { + if (in.header.frame_id == target_frame_) + { + auto out = cloud_pool_.acquire(); + *out = in; + return out; + } + + try + { + // 使用最新可用的变换，不等待 + auto tf = tf_buffer_->lookupTransform( + target_frame_, in.header.frame_id, + tf2::TimePointZero); + + auto out = cloud_pool_.acquire(); + tf2::doTransform(in, *out, tf); + return out; + } + catch (const tf2::TransformException &e) + { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, + "TF failed (%s -> %s): %s", + in.header.frame_id.c_str(), + target_frame_.c_str(), e.what()); + return nullptr; + } + } + + /* ---------- 统计信息 ---------- */ + void printStats() + { + auto current_time = now(); + if ((current_time - last_stats_time_).seconds() >= 5.0) // 每5秒输出一次 + { + RCLCPP_INFO(get_logger(), + "Stats: Front=%d, Rear=%d, Merged=%d, Cache(F:%zu,R:%zu)", + front_count_, rear_count_, merged_count_, + front_clouds_.size(), rear_clouds_.size()); + + last_stats_time_ = current_time; + } + } + + /* ---------- 成员变量 ---------- */ + std::string front_topic_, rear_topic_, output_topic_, target_frame_; + int queue_size_, cache_size_; + double time_tolerance_, max_wait_time_; + bool enable_debug_; + + // 点云处理参数 + float filter_min_x_, filter_max_x_; + float filter_min_y_, filter_max_y_; + float filter_min_z_, filter_max_z_; + float voxel_size_; // 降采样体素大小 + int stat_mean_k_; // 统计离群点去除的k值 + float stat_std_thresh_; // 统计离群点去除的标准差阈值 + int grid_size_; // 栅格大小 + float grid_range_; // 栅格范围 + bool enable_print_; + + // 车身过滤参数 + bool filter_car_{true}; + float car_length_{2.3f}; + float car_width_{1.32f}; + float car_lidar_offset_x_{0.0f}; + float car_lidar_offset_y_{0.0f}; + + rclcpp::Subscription::SharedPtr sub_front_, sub_rear_; + rclcpp::Publisher::SharedPtr pub_; + + rclcpp::Publisher::SharedPtr grid_pub_; + + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + // 点云缓存 + std::deque> front_clouds_; + std::deque> rear_clouds_; + std::mutex cache_mutex_; + + // 内存池 + PointCloud2Pool cloud_pool_; + + // 统计信息 + rclcpp::Time last_stats_time_; + int front_count_, rear_count_, merged_count_; +}; + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + + // 设置线程优先级 + auto node = std::make_shared(); + + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/src/task_manager/CMakeLists.txt b/src/task_manager/CMakeLists.txt new file mode 100644 index 0000000..077c2da --- /dev/null +++ b/src/task_manager/CMakeLists.txt @@ -0,0 +1,67 @@ +cmake_minimum_required(VERSION 3.5) +project(task_manager) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sweeper_interfaces REQUIRED) +find_package(CURL REQUIRED) + +include_directories( + include/task_manager + include/paho_mqtt_3c + ${catkin_INCLUDE_DIRS} +) + +add_executable(task_manager_node + src/task_manager_node.cpp + src/jsoncpp.cpp + src/md5.cpp +) + +ament_target_dependencies(task_manager_node rclcpp std_msgs sweeper_interfaces CURL) + + +if(CMAKE_SYSTEM_PROCESSOR MATCHES aarch64) + target_link_libraries( + task_manager_node + ${PROJECT_SOURCE_DIR}/lib/libpaho-mqtt3c-static.a + ) +else() + target_link_libraries( + task_manager_node + ${PROJECT_SOURCE_DIR}/lib/libpaho-mqtt3c.a + ) +endif() + +install(TARGETS + task_manager_node + DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() +ament_package() \ No newline at end of file diff --git a/src/task_manager/include/paho_mqtt_3c/Base64.h b/src/task_manager/include/paho_mqtt_3c/Base64.h new file mode 100644 index 0000000..df9dd75 --- /dev/null +++ b/src/task_manager/include/paho_mqtt_3c/Base64.h @@ -0,0 +1,83 @@ +/******************************************************************************* + * Copyright (c) 2018 Wind River Systems, Inc. All Rights Reserved. + * + * All rights reserved. This program and the accompanying materials + * are made available under the terms of the Eclipse Public License v1.0 + * and Eclipse Distribution License v1.0 which accompany this distribution. + * + * The Eclipse Public License is available at + * http://www.eclipse.org/legal/epl-v10.html + * and the Eclipse Distribution License is available at + * http://www.eclipse.org/org/documents/edl-v10.php. + * + * Contributors: + * Keith Holman - initial implementation and documentation + *******************************************************************************/ + +#if !defined(BASE64_H) +#define BASE64_H + +/** type for size of a buffer, it saves passing around @p size_t (unsigned long long or unsigned long int) */ +typedef unsigned int b64_size_t; +/** type for raw base64 data */ +typedef unsigned char b64_data_t; + +/** + * Decodes base64 data + * + * @param[out] out decoded data + * @param[in] out_len length of output buffer + * @param[in] in base64 string to decode + * @param[in] in_len length of input buffer + * + * @return the amount of data decoded + * + * @see Base64_decodeLength + * @see Base64_encode + */ +b64_size_t Base64_decode( b64_data_t *out, b64_size_t out_len, + const char *in, b64_size_t in_len ); + +/** + * Size of buffer required to decode base64 data + * + * @param[in] in base64 string to decode + * @param[in] in_len length of input buffer + * + * @return the size of buffer the decoded string would require + * + * @see Base64_decode + * @see Base64_encodeLength + */ +b64_size_t Base64_decodeLength( const char *in, b64_size_t in_len ); + +/** + * Encodes base64 data + * + * @param[out] out encode base64 string + * @param[in] out_len length of output buffer + * @param[in] in raw data to encode + * @param[in] in_len length of input buffer + * + * @return the amount of data encoded + * + * @see Base64_decode + * @see Base64_encodeLength + */ +b64_size_t Base64_encode( char *out, b64_size_t out_len, + const b64_data_t *in, b64_size_t in_len ); + +/** + * Size of buffer required to encode base64 data + * + * @param[in] in raw data to encode + * @param[in] in_len length of input buffer + * + * @return the size of buffer the encoded string would require + * + * @see Base64_decodeLength + * @see Base64_encode + */ +b64_size_t Base64_encodeLength( const b64_data_t *in, b64_size_t in_len ); + +#endif /* BASE64_H */ diff --git a/src/task_manager/include/paho_mqtt_3c/Clients.h b/src/task_manager/include/paho_mqtt_3c/Clients.h new file mode 100644 index 0000000..fd32e3b --- /dev/null +++ b/src/task_manager/include/paho_mqtt_3c/Clients.h @@ -0,0 +1,153 @@ +/******************************************************************************* + * Copyright (c) 2009, 2018 IBM Corp. + * + * All rights reserved. This program and the accompanying materials + * are made available under the terms of the Eclipse Public License v1.0 + * and Eclipse Distribution License v1.0 which accompany this distribution. + * + * The Eclipse Public License is available at + * http://www.eclipse.org/legal/epl-v10.html + * and the Eclipse Distribution License is available at + * http://www.eclipse.org/org/documents/edl-v10.php. + * + * Contributors: + * Ian Craggs - initial API and implementation and/or initial documentation + * Ian Craggs - add SSL support + * Ian Craggs - fix for bug 413429 - connectionLost not called + * Ian Craggs - change will payload to binary + * Ian Craggs - password to binary + * Ian Craggs - MQTT 5 support + *******************************************************************************/ + +#if !defined(CLIENTS_H) +#define CLIENTS_H + +#include +#if defined(OPENSSL) +#if defined(WIN32) || defined(WIN64) +#include +#endif +#include +#endif +#include "MQTTClient.h" +#include "LinkedList.h" +#include "MQTTClientPersistence.h" + + +/** + * Stored publication data to minimize copying + */ +typedef struct +{ + char *topic; + int topiclen; + char* payload; + int payloadlen; + int refcount; +} Publications; + +/** + * Client publication message data + */ +typedef struct +{ + int qos; + int retain; + int msgid; + int MQTTVersion; + MQTTProperties properties; + Publications *publish; + time_t lastTouch; /**> used for retry and expiry */ + char nextMessageType; /**> PUBREC, PUBREL, PUBCOMP */ + int len; /**> length of the whole structure+data */ +} Messages; + +/** + * Client will message data + */ +typedef struct +{ + char *topic; + int payloadlen; + void *payload; + int retained; + int qos; +} willMessages; + +typedef struct +{ + int socket; + time_t lastSent; + time_t lastReceived; +#if defined(OPENSSL) + SSL* ssl; + SSL_CTX* ctx; +#endif + int websocket; /**< socket has been upgraded to use web sockets */ + char *websocket_key; +} networkHandles; + + +/* connection states */ +/** no connection in progress, see connected value */ +#define NOT_IN_PROGRESS 0x0 +/** TCP connection in progress */ +#define TCP_IN_PROGRESS 0x1 +/** SSL connection in progress */ +#define SSL_IN_PROGRESS 0x2 +/** Websocket connection in progress */ +#define WEBSOCKET_IN_PROGRESS 0x3 +/** TCP completed, waiting for MQTT ACK */ +#define WAIT_FOR_CONNACK 0x4 +/** Disconnecting */ +#define DISCONNECTING -2 + +/** + * Data related to one client + */ +typedef struct +{ + char* clientID; /**< the string id of the client */ + const char* username; /**< MQTT v3.1 user name */ + int passwordlen; /**< MQTT password length */ + const void* password; /**< MQTT v3.1 binary password */ + unsigned int cleansession : 1; /**< MQTT V3 clean session flag */ + unsigned int cleanstart : 1; /**< MQTT V5 clean start flag */ + unsigned int connected : 1; /**< whether it is currently connected */ + unsigned int good : 1; /**< if we have an error on the socket we turn this off */ + unsigned int ping_outstanding : 1; + signed int connect_state : 4; + networkHandles net; + int msgID; + int keepAliveInterval; + int retryInterval; + int maxInflightMessages; + willMessages* will; + List* inboundMsgs; + List* outboundMsgs; /**< in flight */ + List* messageQueue; + unsigned int qentry_seqno; + void* phandle; /* the persistence handle */ + MQTTClient_persistence* persistence; /* a persistence implementation */ + void* context; /* calling context - used when calling disconnect_internal */ + int MQTTVersion; + int sessionExpiry; /**< MQTT 5 session expiry */ +#if defined(OPENSSL) + MQTTClient_SSLOptions *sslopts; + SSL_SESSION* session; /***< SSL session pointer for fast handhake */ +#endif +} Clients; + +int clientIDCompare(void* a, void* b); +int clientSocketCompare(void* a, void* b); + +/** + * Configuration data related to all clients + */ +typedef struct +{ + const char* version; + List* clients; +} ClientStates; + +#endif diff --git a/src/task_manager/include/paho_mqtt_3c/Heap.h b/src/task_manager/include/paho_mqtt_3c/Heap.h new file mode 100644 index 0000000..6d24c04 --- /dev/null +++ b/src/task_manager/include/paho_mqtt_3c/Heap.h @@ -0,0 +1,82 @@ +/******************************************************************************* + * Copyright (c) 2009, 2013 IBM Corp. + * + * All rights reserved. This program and the accompanying materials + * are made available under the terms of the Eclipse Public License v1.0 + * and Eclipse Distribution License v1.0 which accompany this distribution. + * + * The Eclipse Public License is available at + * http://www.eclipse.org/legal/epl-v10.html + * and the Eclipse Distribution License is available at + * http://www.eclipse.org/org/documents/edl-v10.php. + * + * Contributors: + * Ian Craggs - initial API and implementation and/or initial documentation + * Ian Craggs - use tree data structure instead of list + *******************************************************************************/ + + +#if !defined(HEAP_H) +#define HEAP_H + +#if defined(HIGH_PERFORMANCE) +#define NO_HEAP_TRACKING 1 +#endif + +#include +#include + +#if !defined(NO_HEAP_TRACKING) +/** + * redefines malloc to use "mymalloc" so that heap allocation can be tracked + * @param x the size of the item to be allocated + * @return the pointer to the item allocated, or NULL + */ +#define malloc(x) mymalloc(__FILE__, __LINE__, x) + +/** + * redefines realloc to use "myrealloc" so that heap allocation can be tracked + * @param a the heap item to be reallocated + * @param b the new size of the item + * @return the new pointer to the heap item + */ +#define realloc(a, b) myrealloc(__FILE__, __LINE__, a, b) + +/** + * redefines free to use "myfree" so that heap allocation can be tracked + * @param x the size of the item to be freed + */ +#define free(x) myfree(__FILE__, __LINE__, x) + +#endif + +/** + * Information about the state of the heap. + */ +typedef struct +{ + size_t current_size; /**< current size of the heap in bytes */ + size_t max_size; /**< max size the heap has reached in bytes */ +} heap_info; + +#if defined(__cplusplus) + extern "C" { +#endif + +void* mymalloc(char*, int, size_t size); +void* myrealloc(char*, int, void* p, size_t size); +void myfree(char*, int, void* p); + +void Heap_scan(FILE* file); +int Heap_initialize(void); +void Heap_terminate(void); +heap_info* Heap_get_info(void); +int HeapDump(FILE* file); +int HeapDumpString(FILE* file, char* str); +void* Heap_findItem(void* p); +void Heap_unlink(char* file, int line, void* p); +#ifdef __cplusplus + } +#endif + +#endif diff --git a/src/task_manager/include/paho_mqtt_3c/LinkedList.h b/src/task_manager/include/paho_mqtt_3c/LinkedList.h new file mode 100644 index 0000000..102a4fd --- /dev/null +++ b/src/task_manager/include/paho_mqtt_3c/LinkedList.h @@ -0,0 +1,105 @@ +/******************************************************************************* + * Copyright (c) 2009, 2013 IBM Corp. + * + * All rights reserved. This program and the accompanying materials + * are made available under the terms of the Eclipse Public License v1.0 + * and Eclipse Distribution License v1.0 which accompany this distribution. + * + * The Eclipse Public License is available at + * http://www.eclipse.org/legal/epl-v10.html + * and the Eclipse Distribution License is available at + * http://www.eclipse.org/org/documents/edl-v10.php. + * + * Contributors: + * Ian Craggs - initial API and implementation and/or initial documentation + * Ian Craggs - updates for the async client + * Ian Craggs - change size types from int to size_t + *******************************************************************************/ + +#if !defined(LINKEDLIST_H) +#define LINKEDLIST_H + +#include /* for size_t definition */ + +/*BE +defm defList(T) + +def T concat Item +{ + at 4 + n32 ptr T concat Item suppress "next" + at 0 + n32 ptr T concat Item suppress "prev" + at 8 + n32 ptr T id2str(T) +} + +def T concat List +{ + n32 ptr T concat Item suppress "first" + n32 ptr T concat Item suppress "last" + n32 ptr T concat Item suppress "current" + n32 dec "count" + n32 suppress "size" +} +endm + +defList(INT) +defList(STRING) +defList(TMP) + +BE*/ + +/** + * Structure to hold all data for one list element + */ +typedef struct ListElementStruct +{ + struct ListElementStruct *prev, /**< pointer to previous list element */ + *next; /**< pointer to next list element */ + void* content; /**< pointer to element content */ +} ListElement; + + +/** + * Structure to hold all data for one list + */ +typedef struct +{ + ListElement *first, /**< first element in the list */ + *last, /**< last element in the list */ + *current; /**< current element in the list, for iteration */ + int count; /**< no of items */ + size_t size; /**< heap storage used */ +} List; + +void ListZero(List*); +List* ListInitialize(void); + +void ListAppend(List* aList, void* content, size_t size); +void ListAppendNoMalloc(List* aList, void* content, ListElement* newel, size_t size); +void ListInsert(List* aList, void* content, size_t size, ListElement* index); + +int ListRemove(List* aList, void* content); +int ListRemoveItem(List* aList, void* content, int(*callback)(void*, void*)); +void* ListDetachHead(List* aList); +int ListRemoveHead(List* aList); +void* ListPopTail(List* aList); + +int ListDetach(List* aList, void* content); +int ListDetachItem(List* aList, void* content, int(*callback)(void*, void*)); + +void ListFree(List* aList); +void ListEmpty(List* aList); +void ListFreeNoContent(List* aList); + +ListElement* ListNextElement(List* aList, ListElement** pos); +ListElement* ListPrevElement(List* aList, ListElement** pos); + +ListElement* ListFind(List* aList, void* content); +ListElement* ListFindItem(List* aList, void* content, int(*callback)(void*, void*)); + +int intcompare(void* a, void* b); +int stringcompare(void* a, void* b); + +#endif diff --git a/src/task_manager/include/paho_mqtt_3c/Log.h b/src/task_manager/include/paho_mqtt_3c/Log.h new file mode 100644 index 0000000..455beb6 --- /dev/null +++ b/src/task_manager/include/paho_mqtt_3c/Log.h @@ -0,0 +1,85 @@ +/******************************************************************************* + * Copyright (c) 2009, 2013 IBM Corp. + * + * All rights reserved. This program and the accompanying materials + * are made available under the terms of the Eclipse Public License v1.0 + * and Eclipse Distribution License v1.0 which accompany this distribution. + * + * The Eclipse Public License is available at + * http://www.eclipse.org/legal/epl-v10.html + * and the Eclipse Distribution License is available at + * http://www.eclipse.org/org/documents/edl-v10.php. + * + * Contributors: + * Ian Craggs - initial API and implementation and/or initial documentation + * Ian Craggs - updates for the async client + *******************************************************************************/ + +#if !defined(LOG_H) +#define LOG_H + +/*BE +map LOG_LEVELS +{ + "TRACE_MAXIMUM" 1 + "TRACE_MEDIUM" 2 + "TRACE_MINIMUM" 3 + "TRACE_PROTOCOL" 4 + + "ERROR" 5 + "SEVERE" 6 + "FATAL" 7 +} +BE*/ + +enum LOG_LEVELS { + INVALID_LEVEL = -1, + TRACE_MAXIMUM = 1, + TRACE_MEDIUM, + TRACE_MINIMUM, + TRACE_PROTOCOL, + LOG_ERROR, + LOG_SEVERE, + LOG_FATAL, +}; + + +/*BE +def trace_settings_type +{ + n32 map LOG_LEVELS "trace_level" + n32 dec "max_trace_entries" + n32 dec "trace_output_level" +} +BE*/ +typedef struct +{ + enum LOG_LEVELS trace_level; /**< trace level */ + int max_trace_entries; /**< max no of entries in the trace buffer */ + enum LOG_LEVELS trace_output_level; /**< trace level to output to destination */ +} trace_settings_type; + +extern trace_settings_type trace_settings; + +#define LOG_PROTOCOL TRACE_PROTOCOL +#define TRACE_MAX TRACE_MAXIMUM +#define TRACE_MIN TRACE_MINIMUM +#define TRACE_MED TRACE_MEDIUM + +typedef struct +{ + const char* name; + const char* value; +} Log_nameValue; + +int Log_initialize(Log_nameValue*); +void Log_terminate(void); + +void Log(enum LOG_LEVELS, int, const char *, ...); +void Log_stackTrace(enum LOG_LEVELS, int, int, int, const char*, int, int*); + +typedef void Log_traceCallback(enum LOG_LEVELS level, const char *message); +void Log_setTraceCallback(Log_traceCallback* callback); +void Log_setTraceLevel(enum LOG_LEVELS level); + +#endif diff --git a/src/task_manager/include/paho_mqtt_3c/MQTTAsync.h b/src/task_manager/include/paho_mqtt_3c/MQTTAsync.h new file mode 100644 index 0000000..7b2067a --- /dev/null +++ b/src/task_manager/include/paho_mqtt_3c/MQTTAsync.h @@ -0,0 +1,2068 @@ +/******************************************************************************* + * Copyright (c) 2009, 2018 IBM Corp. + * + * All rights reserved. This program and the accompanying materials + * are made available under the terms of the Eclipse Public License v1.0 + * and Eclipse Distribution License v1.0 which accompany this distribution. + * + * The Eclipse Public License is available at + * http://www.eclipse.org/legal/epl-v10.html + * and the Eclipse Distribution License is available at + * http://www.eclipse.org/org/documents/edl-v10.php. + * + * Contributors: + * Ian Craggs - initial API and implementation + * Ian Craggs, Allan Stockdill-Mander - SSL connections + * Ian Craggs - multiple server connection support + * Ian Craggs - MQTT 3.1.1 support + * Ian Craggs - fix for bug 444103 - success/failure callbacks not invoked + * Ian Craggs - automatic reconnect and offline buffering (send while disconnected) + * Ian Craggs - binary will message + * Ian Craggs - binary password + * Ian Craggs - remove const on eyecatchers #168 + * Ian Craggs - MQTT 5.0 + *******************************************************************************/ + +/********************************************************************/ + +/** + * @cond MQTTAsync_main + * @mainpage Asynchronous MQTT client library for C + * + * © Copyright IBM Corp. 2009, 2018 + * + * @brief An Asynchronous MQTT client library for C. + * + * An MQTT client application connects to MQTT-capable servers. + * A typical client is responsible for collecting information from a telemetry + * device and publishing the information to the server. It can also subscribe + * to topics, receive messages, and use this information to control the + * telemetry device. + * + * MQTT clients implement the published MQTT v3 protocol. You can write your own + * API to the MQTT protocol using the programming language and platform of your + * choice. This can be time-consuming and error-prone. + * + * To simplify writing MQTT client applications, this library encapsulates + * the MQTT v3 protocol for you. Using this library enables a fully functional + * MQTT client application to be written in a few lines of code. + * The information presented here documents the API provided + * by the Asynchronous MQTT Client library for C. + * + * Using the client
+ * Applications that use the client library typically use a similar structure: + *
    + *
  • Create a client object
    • + *
  • Set the options to connect to an MQTT server
    • + *
  • Set up callback functions
    • + *
  • Connect the client to an MQTT server
    • + *
  • Subscribe to any topics the client needs to receive
    • + *
  • Repeat until finished:
    • + *
      + *
    • Publish any messages the client needs to
      • + *
    • Handle any incoming messages
      • + *
    + *
  • Disconnect the client
    • + *
  • Free any memory being used by the client
    • + *
+ * Some simple examples are shown here: + *
    + *
  • @ref publish
    • + *
  • @ref subscribe
    • + *
+ * Additional information about important concepts is provided here: + *
    + *
  • @ref async
    • + *
  • @ref wildcard
    • + *
  • @ref qos
    • + *
  • @ref tracing
    • + *
  • @ref auto_reconnect
    • + *
  • @ref offline_publish
    • + *
+ * @endcond + */ + +/* +/// @cond EXCLUDE +*/ +#if !defined(MQTTASYNC_H) +#define MQTTASYNC_H + +#if defined(__cplusplus) + extern "C" { +#endif + +#if defined(WIN32) || defined(WIN64) + #define DLLImport __declspec(dllimport) + #define DLLExport __declspec(dllexport) +#else + #define DLLImport extern + #define DLLExport __attribute__ ((visibility ("default"))) +#endif + +#include +/* +/// @endcond +*/ + +#include "MQTTProperties.h" +#include "MQTTReasonCodes.h" +#include "MQTTSubscribeOpts.h" +#if !defined(NO_PERSISTENCE) +#include "MQTTClientPersistence.h" +#endif + +/** + * Return code: No error. Indicates successful completion of an MQTT client + * operation. + */ +#define MQTTASYNC_SUCCESS 0 +/** + * Return code: A generic error code indicating the failure of an MQTT client + * operation. + */ +#define MQTTASYNC_FAILURE -1 + +/* error code -2 is MQTTAsync_PERSISTENCE_ERROR */ + +#define MQTTASYNC_PERSISTENCE_ERROR -2 + +/** + * Return code: The client is disconnected. + */ +#define MQTTASYNC_DISCONNECTED -3 +/** + * Return code: The maximum number of messages allowed to be simultaneously + * in-flight has been reached. + */ +#define MQTTASYNC_MAX_MESSAGES_INFLIGHT -4 +/** + * Return code: An invalid UTF-8 string has been detected. + */ +#define MQTTASYNC_BAD_UTF8_STRING -5 +/** + * Return code: A NULL parameter has been supplied when this is invalid. + */ +#define MQTTASYNC_NULL_PARAMETER -6 +/** + * Return code: The topic has been truncated (the topic string includes + * embedded NULL characters). String functions will not access the full topic. + * Use the topic length value to access the full topic. + */ +#define MQTTASYNC_TOPICNAME_TRUNCATED -7 +/** + * Return code: A structure parameter does not have the correct eyecatcher + * and version number. + */ +#define MQTTASYNC_BAD_STRUCTURE -8 +/** + * Return code: A qos parameter is not 0, 1 or 2 + */ +#define MQTTASYNC_BAD_QOS -9 +/** + * Return code: All 65535 MQTT msgids are being used + */ +#define MQTTASYNC_NO_MORE_MSGIDS -10 +/** + * Return code: the request is being discarded when not complete + */ +#define MQTTASYNC_OPERATION_INCOMPLETE -11 +/** + * Return code: no more messages can be buffered + */ +#define MQTTASYNC_MAX_BUFFERED_MESSAGES -12 +/** + * Return code: Attempting SSL connection using non-SSL version of library + */ +#define MQTTASYNC_SSL_NOT_SUPPORTED -13 +/** + * Return code: protocol prefix in serverURI should be tcp:// or ssl:// + */ +#define MQTTASYNC_BAD_PROTOCOL -14 + /** + * Return code: don't use options for another version of MQTT + */ + #define MQTTASYNC_BAD_MQTT_OPTION -15 + /** + * Return code: call not applicable to the client's version of MQTT + */ + #define MQTTASYNC_WRONG_MQTT_VERSION -16 + + +/** + * Default MQTT version to connect with. Use 3.1.1 then fall back to 3.1 + */ +#define MQTTVERSION_DEFAULT 0 +/** + * MQTT version to connect with: 3.1 + */ +#define MQTTVERSION_3_1 3 +/** + * MQTT version to connect with: 3.1.1 + */ +#define MQTTVERSION_3_1_1 4 +/** + * MQTT version to connect with: 5 + */ +#define MQTTVERSION_5 5 +/** + * Bad return code from subscribe, as defined in the 3.1.1 specification + */ +#define MQTT_BAD_SUBSCRIBE 0x80 + + +/** + * Initialization options + */ +typedef struct +{ + /** The eyecatcher for this structure. Must be MQTG. */ + char struct_id[4]; + /** The version number of this structure. Must be 0 */ + int struct_version; + /** 1 = we do openssl init, 0 = leave it to the application */ + int do_openssl_init; +} MQTTAsync_init_options; + +#define MQTTAsync_init_options_initializer { {'M', 'Q', 'T', 'G'}, 0, 0 } + +/** + * Global init of mqtt library. Call once on program start to set global behaviour. + * handle_openssl_init - if mqtt library should handle openssl init (1) or rely on the caller to init it before using mqtt (0) + */ +DLLExport void MQTTAsync_global_init(MQTTAsync_init_options* inits); + +/** + * A handle representing an MQTT client. A valid client handle is available + * following a successful call to MQTTAsync_create(). + */ +typedef void* MQTTAsync; +/** + * A value representing an MQTT message. A token is returned to the + * client application when a message is published. The token can then be used to + * check that the message was successfully delivered to its destination (see + * MQTTAsync_publish(), + * MQTTAsync_publishMessage(), + * MQTTAsync_deliveryComplete(), and + * MQTTAsync_getPendingTokens()). + */ +typedef int MQTTAsync_token; + +/** + * A structure representing the payload and attributes of an MQTT message. The + * message topic is not part of this structure (see MQTTAsync_publishMessage(), + * MQTTAsync_publish(), MQTTAsync_receive(), MQTTAsync_freeMessage() + * and MQTTAsync_messageArrived()). + */ +typedef struct +{ + /** The eyecatcher for this structure. must be MQTM. */ + char struct_id[4]; + /** The version number of this structure. Must be 0 or 1. + * 0 indicates no message properties */ + int struct_version; + /** The length of the MQTT message payload in bytes. */ + int payloadlen; + /** A pointer to the payload of the MQTT message. */ + void* payload; + /** + * The quality of service (QoS) assigned to the message. + * There are three levels of QoS: + *
+ *
QoS0
+ *
Fire and forget - the message may not be delivered
+ *
QoS1
+ *
At least once - the message will be delivered, but may be + * delivered more than once in some circumstances.
+ *
QoS2
+ *
Once and one only - the message will be delivered exactly once.
+ *
+ */ + int qos; + /** + * The retained flag serves two purposes depending on whether the message + * it is associated with is being published or received. + * + * retained = true
+ * For messages being published, a true setting indicates that the MQTT + * server should retain a copy of the message. The message will then be + * transmitted to new subscribers to a topic that matches the message topic. + * For subscribers registering a new subscription, the flag being true + * indicates that the received message is not a new one, but one that has + * been retained by the MQTT server. + * + * retained = false
+ * For publishers, this indicates that this message should not be retained + * by the MQTT server. For subscribers, a false setting indicates this is + * a normal message, received as a result of it being published to the + * server. + */ + int retained; + /** + * The dup flag indicates whether or not this message is a duplicate. + * It is only meaningful when receiving QoS1 messages. When true, the + * client application should take appropriate action to deal with the + * duplicate message. + */ + int dup; + /** The message identifier is normally reserved for internal use by the + * MQTT client and server. + */ + int msgid; + /** + * The MQTT V5 properties associated with the message. + */ + MQTTProperties properties; +} MQTTAsync_message; + +#define MQTTAsync_message_initializer { {'M', 'Q', 'T', 'M'}, 1, 0, NULL, 0, 0, 0, 0, MQTTProperties_initializer } + +/** + * This is a callback function. The client application + * must provide an implementation of this function to enable asynchronous + * receipt of messages. The function is registered with the client library by + * passing it as an argument to MQTTAsync_setCallbacks(). It is + * called by the client library when a new message that matches a client + * subscription has been received from the server. This function is executed on + * a separate thread to the one on which the client application is running. + * @param context A pointer to the context value originally passed to + * MQTTAsync_setCallbacks(), which contains any application-specific context. + * @param topicName The topic associated with the received message. + * @param topicLen The length of the topic if there are one + * more NULL characters embedded in topicName, otherwise topicLen + * is 0. If topicLen is 0, the value returned by strlen(topicName) + * can be trusted. If topicLen is greater than 0, the full topic name + * can be retrieved by accessing topicName as a byte array of length + * topicLen. + * @param message The MQTTAsync_message structure for the received message. + * This structure contains the message payload and attributes. + * @return This function must return a boolean value indicating whether or not + * the message has been safely received by the client application. Returning + * true indicates that the message has been successfully handled. + * Returning false indicates that there was a problem. In this + * case, the client library will reinvoke MQTTAsync_messageArrived() to + * attempt to deliver the message to the application again. + */ +typedef int MQTTAsync_messageArrived(void* context, char* topicName, int topicLen, MQTTAsync_message* message); + +/** + * This is a callback function. The client application + * must provide an implementation of this function to enable asynchronous + * notification of delivery of messages to the server. The function is + * registered with the client library by passing it as an argument to MQTTAsync_setCallbacks(). + * It is called by the client library after the client application has + * published a message to the server. It indicates that the necessary + * handshaking and acknowledgements for the requested quality of service (see + * MQTTAsync_message.qos) have been completed. This function is executed on a + * separate thread to the one on which the client application is running. + * @param context A pointer to the context value originally passed to + * MQTTAsync_setCallbacks(), which contains any application-specific context. + * @param token The ::MQTTAsync_token associated with + * the published message. Applications can check that all messages have been + * correctly published by matching the tokens returned from calls to + * MQTTAsync_send() and MQTTAsync_sendMessage() with the tokens passed + * to this callback. + */ +typedef void MQTTAsync_deliveryComplete(void* context, MQTTAsync_token token); + +/** + * This is a callback function. The client application + * must provide an implementation of this function to enable asynchronous + * notification of the loss of connection to the server. The function is + * registered with the client library by passing it as an argument to + * MQTTAsync_setCallbacks(). It is called by the client library if the client + * loses its connection to the server. The client application must take + * appropriate action, such as trying to reconnect or reporting the problem. + * This function is executed on a separate thread to the one on which the + * client application is running. + * @param context A pointer to the context value originally passed to + * MQTTAsync_setCallbacks(), which contains any application-specific context. + * @param cause The reason for the disconnection. + * Currently, cause is always set to NULL. + */ +typedef void MQTTAsync_connectionLost(void* context, char* cause); + + +/** + * This is a callback function, which will be called when the client + * library successfully connects. This is superfluous when the connection + * is made in response to a MQTTAsync_connect call, because the onSuccess + * callback can be used. It is intended for use when automatic reconnect + * is enabled, so that when a reconnection attempt succeeds in the background, + * the application is notified and can take any required actions. + * @param context A pointer to the context value originally passed to + * MQTTAsync_setCallbacks(), which contains any application-specific context. + * @param cause The reason for the disconnection. + * Currently, cause is always set to NULL. + */ +typedef void MQTTAsync_connected(void* context, char* cause); + +/** + * This is a callback function, which will be called when the client + * library receives a disconnect packet. + * @param context A pointer to the context value originally passed to + * MQTTAsync_setCallbacks(), which contains any application-specific context. + * @param properties the properties in the disconnect packet. + * @param properties the reason code from the disconnect packet + * Currently, cause is always set to NULL. + */ +typedef void MQTTAsync_disconnected(void* context, MQTTProperties* properties, + enum MQTTReasonCodes reasonCode); + +/** + * Sets the MQTTAsync_disconnected() callback function for a client. + * @param handle A valid client handle from a successful call to + * MQTTAsync_create(). + * @param context A pointer to any application-specific context. The + * the context pointer is passed to each of the callback functions to + * provide access to the context information in the callback. + * @param co A pointer to an MQTTAsync_connected() callback + * function. NULL removes the callback setting. + * @return ::MQTTASYNC_SUCCESS if the callbacks were correctly set, + * ::MQTTASYNC_FAILURE if an error occurred. + */ +DLLExport int MQTTAsync_setDisconnected(MQTTAsync handle, void* context, MQTTAsync_disconnected* co); + + +/** The data returned on completion of an unsuccessful API call in the response callback onFailure. */ +typedef struct +{ + /** A token identifying the failed request. */ + MQTTAsync_token token; + /** A numeric code identifying the error. */ + int code; + /** Optional text explaining the error. Can be NULL. */ + const char *message; +} MQTTAsync_failureData; + + +/** The data returned on completion of an unsuccessful API call in the response callback onFailure. */ +typedef struct +{ + /** The eyecatcher for this structure. Will be MQFD. */ + char struct_id[4]; + /** The version number of this structure. Will be 0 */ + int struct_version; + /** A token identifying the failed request. */ + MQTTAsync_token token; + /** The MQTT reason code returned. */ + enum MQTTReasonCodes reasonCode; + /** The MQTT properties on the ack, if any. */ + MQTTProperties properties; + /** A numeric code identifying the MQTT client library error. */ + int code; + /** Optional further text explaining the error. Can be NULL. */ + const char *message; + /** Packet type on which the failure occurred - used for publish QoS 1/2 exchanges*/ + int packet_type; +} MQTTAsync_failureData5; + +#define MQTTAsync_failureData5_initializer {{'M', 'Q', 'F', 'D'}, 0, 0, MQTTREASONCODE_SUCCESS, MQTTProperties_initializer, 0, NULL} + +/** The data returned on completion of a successful API call in the response callback onSuccess. */ +typedef struct +{ + /** A token identifying the successful request. Can be used to refer to the request later. */ + MQTTAsync_token token; + /** A union of the different values that can be returned for subscribe, unsubscribe and publish. */ + union + { + /** For subscribe, the granted QoS of the subscription returned by the server. */ + int qos; + /** For subscribeMany, the list of granted QoSs of the subscriptions returned by the server. */ + int* qosList; + /** For publish, the message being sent to the server. */ + struct + { + MQTTAsync_message message; + char* destinationName; + } pub; + /* For connect, the server connected to, MQTT version used, and sessionPresent flag */ + struct + { + char* serverURI; + int MQTTVersion; + int sessionPresent; + } connect; + } alt; +} MQTTAsync_successData; + + +/** The data returned on completion of a successful API call in the response callback onSuccess. */ +typedef struct +{ + char struct_id[4]; /**< The eyecatcher for this structure. Will be MQSD. */ + int struct_version; /**< The version number of this structure. Will be 0 */ + /** A token identifying the successful request. Can be used to refer to the request later. */ + MQTTAsync_token token; + enum MQTTReasonCodes reasonCode; /**< MQTT V5 reason code returned */ + MQTTProperties properties; /**< MQTT V5 properties returned, if any */ + /** A union of the different values that can be returned for subscribe, unsubscribe and publish. */ + union + { + /** For subscribeMany, the list of reasonCodes returned by the server. */ + struct + { + int reasonCodeCount; /**< the number of reason codes in the reasonCodes array */ + enum MQTTReasonCodes* reasonCodes; /**< an array of reasonCodes */ + } sub; + /** For publish, the message being sent to the server. */ + struct + { + MQTTAsync_message message; /**< the message being sent to the server */ + char* destinationName; /**< the topic destination for the message */ + } pub; + /* For connect, the server connected to, MQTT version used, and sessionPresent flag */ + struct + { + char* serverURI; /**< the connection string of the server */ + int MQTTVersion; /**< the version of MQTT being used */ + int sessionPresent; /**< the session present flag returned from the server */ + } connect; + /** For unsubscribeMany, the list of reasonCodes returned by the server. */ + struct + { + int reasonCodeCount; /**< the number of reason codes in the reasonCodes array */ + enum MQTTReasonCodes* reasonCodes; /**< an array of reasonCodes */ + } unsub; + } alt; +} MQTTAsync_successData5; + +#define MQTTAsync_successData5_initializer {{'M', 'Q', 'S', 'D'}, 0, 0, MQTTREASONCODE_SUCCESS, MQTTProperties_initializer} + +/** + * This is a callback function. The client application + * must provide an implementation of this function to enable asynchronous + * notification of the successful completion of an API call. The function is + * registered with the client library by passing it as an argument in + * ::MQTTAsync_responseOptions. + * @param context A pointer to the context value originally passed to + * ::MQTTAsync_responseOptions, which contains any application-specific context. + * @param response Any success data associated with the API completion. + */ +typedef void MQTTAsync_onSuccess(void* context, MQTTAsync_successData* response); + +/** + * This is a callback function, the MQTT V5 version of ::MQTTAsync_onSuccess. + * The client application + * must provide an implementation of this function to enable asynchronous + * notification of the successful completion of an API call. The function is + * registered with the client library by passing it as an argument in + * ::MQTTAsync_responseOptions. + * @param context A pointer to the context value originally passed to + * ::MQTTAsync_responseOptions, which contains any application-specific context. + * @param response Any success data associated with the API completion. + */ +typedef void MQTTAsync_onSuccess5(void* context, MQTTAsync_successData5* response); + +/** + * This is a callback function. The client application + * must provide an implementation of this function to enable asynchronous + * notification of the unsuccessful completion of an API call. The function is + * registered with the client library by passing it as an argument in + * ::MQTTAsync_responseOptions. + * @param context A pointer to the context value originally passed to + * ::MQTTAsync_responseOptions, which contains any application-specific context. + * @param response Failure data associated with the API completion. + */ +typedef void MQTTAsync_onFailure(void* context, MQTTAsync_failureData* response); + +/** + * This is a callback function, the MQTT V5 version of ::MQTTAsync_onFailure. + * The application must provide an implementation of this function to enable asynchronous + * notification of the unsuccessful completion of an API call. The function is + * registered with the client library by passing it as an argument in + * ::MQTTAsync_responseOptions. + * @param context A pointer to the context value originally passed to + * ::MQTTAsync_responseOptions, which contains any application-specific context. + * @param response Failure data associated with the API completion. + */ +typedef void MQTTAsync_onFailure5(void* context, MQTTAsync_failureData5* response); + +typedef struct MQTTAsync_responseOptions +{ + /** The eyecatcher for this structure. Must be MQTR */ + char struct_id[4]; + /** The version number of this structure. Must be 0 or 1 + * if 0, no MQTTV5 options */ + int struct_version; + /** + * A pointer to a callback function to be called if the API call successfully + * completes. Can be set to NULL, in which case no indication of successful + * completion will be received. + */ + MQTTAsync_onSuccess* onSuccess; + /** + * A pointer to a callback function to be called if the API call fails. + * Can be set to NULL, in which case no indication of unsuccessful + * completion will be received. + */ + MQTTAsync_onFailure* onFailure; + /** + * A pointer to any application-specific context. The + * the context pointer is passed to success or failure callback functions to + * provide access to the context information in the callback. + */ + void* context; + /** + * A token is returned from the call. It can be used to track + * the state of this request, both in the callbacks and in future calls + * such as ::MQTTAsync_waitForCompletion. + */ + MQTTAsync_token token; + /** + * A pointer to a callback function to be called if the API call successfully + * completes. Can be set to NULL, in which case no indication of successful + * completion will be received. + */ + MQTTAsync_onSuccess5* onSuccess5; + /** + * A pointer to a callback function to be called if the API call successfully + * completes. Can be set to NULL, in which case no indication of successful + * completion will be received. + */ + MQTTAsync_onFailure5* onFailure5; + /** + * MQTT V5 input properties + */ + MQTTProperties properties; + /* + * MQTT V5 subscribe options, when used with subscribe only. + */ + MQTTSubscribe_options subscribeOptions; + /* + * MQTT V5 subscribe option count, when used with subscribeMany only. + * The number of entries in the subscribe_options_list array. + */ + int subscribeOptionsCount; + /* + * MQTT V5 subscribe option array, when used with subscribeMany only. + */ + MQTTSubscribe_options* subscribeOptionsList; +} MQTTAsync_responseOptions; + +#define MQTTAsync_responseOptions_initializer { {'M', 'Q', 'T', 'R'}, 1, NULL, NULL, 0, 0, NULL, NULL, MQTTProperties_initializer, MQTTSubscribe_options_initializer, 0, NULL} + +typedef struct MQTTAsync_responseOptions MQTTAsync_callOptions; +#define MQTTAsync_callOptions_initializer MQTTAsync_responseOptions_initializer + +/** + * This function sets the global callback functions for a specific client. + * If your client application doesn't use a particular callback, set the + * relevant parameter to NULL. Any necessary message acknowledgements and + * status communications are handled in the background without any intervention + * from the client application. If you do not set a messageArrived callback + * function, you will not be notified of the receipt of any messages as a + * result of a subscription. + * + * Note: The MQTT client must be disconnected when this function is + * called. + * @param handle A valid client handle from a successful call to + * MQTTAsync_create(). + * @param context A pointer to any application-specific context. The + * the context pointer is passed to each of the callback functions to + * provide access to the context information in the callback. + * @param cl A pointer to an MQTTAsync_connectionLost() callback + * function. You can set this to NULL if your application doesn't handle + * disconnections. + * @param ma A pointer to an MQTTAsync_messageArrived() callback + * function. You can set this to NULL if your application doesn't handle + * receipt of messages. + * @param dc A pointer to an MQTTAsync_deliveryComplete() callback + * function. You can set this to NULL if you do not want to check + * for successful delivery. + * @return ::MQTTASYNC_SUCCESS if the callbacks were correctly set, + * ::MQTTASYNC_FAILURE if an error occurred. + */ +DLLExport int MQTTAsync_setCallbacks(MQTTAsync handle, void* context, MQTTAsync_connectionLost* cl, + MQTTAsync_messageArrived* ma, MQTTAsync_deliveryComplete* dc); + +/** + * This function sets the callback function for a connection lost event for + * a specific client. Any necessary message acknowledgements and status + * communications are handled in the background without any intervention + * from the client application. + * + * Note: The MQTT client must be disconnected when this function is + * called. + * @param handle A valid client handle from a successful call to + * MQTTAsync_create(). + * @param context A pointer to any application-specific context. The + * the context pointer is passed the callback functions to provide + * access to the context information in the callback. + * @param cl A pointer to an MQTTAsync_connectionLost() callback + * function. You can set this to NULL if your application doesn't handle + * disconnections. + * @return ::MQTTASYNC_SUCCESS if the callbacks were correctly set, + * ::MQTTASYNC_FAILURE if an error occurred. + */ + +DLLExport int MQTTAsync_setConnectionLostCallback(MQTTAsync handle, void* context, + MQTTAsync_connectionLost* cl); + +/** + * This function sets the callback function for a message arrived event for + * a specific client. Any necessary message acknowledgements and status + * communications are handled in the background without any intervention + * from the client application. If you do not set a messageArrived callback + * function, you will not be notified of the receipt of any messages as a + * result of a subscription. + * + * Note: The MQTT client must be disconnected when this function is + * called. + * @param handle A valid client handle from a successful call to + * MQTTAsync_create(). + * @param context A pointer to any application-specific context. The + * the context pointer is passed to the callback functions to provide + * access to the context information in the callback. + * @param ma A pointer to an MQTTAsync_messageArrived() callback + * function. You can set this to NULL if your application doesn't handle + * receipt of messages. + * @return ::MQTTASYNC_SUCCESS if the callbacks were correctly set, + * ::MQTTASYNC_FAILURE if an error occurred. + */ +DLLExport int MQTTAsync_setMessageArrivedCallback(MQTTAsync handle, void* context, + MQTTAsync_messageArrived* ma); + +/** + * This function sets the callback function for a delivery complete event + * for a specific client. Any necessary message acknowledgements and status + * communications are handled in the background without any intervention + * from the client application. + * + * Note: The MQTT client must be disconnected when this function is + * called. + * @param handle A valid client handle from a successful call to + * MQTTAsync_create(). + * @param context A pointer to any application-specific context. The + * the context pointer is passed to the callback functions to provide + * access to the context information in the callback. + * @param dc A pointer to an MQTTAsync_deliveryComplete() callback + * function. You can set this to NULL if you do not want to check + * for successful delivery. + * @return ::MQTTASYNC_SUCCESS if the callbacks were correctly set, + * ::MQTTASYNC_FAILURE if an error occurred. + */ +DLLExport int MQTTAsync_setDeliveryCompleteCallback(MQTTAsync handle, void* context, + MQTTAsync_deliveryComplete* dc); + +/** + * Sets the MQTTAsync_connected() callback function for a client. + * @param handle A valid client handle from a successful call to + * MQTTAsync_create(). + * @param context A pointer to any application-specific context. The + * the context pointer is passed to each of the callback functions to + * provide access to the context information in the callback. + * @param co A pointer to an MQTTAsync_connected() callback + * function. NULL removes the callback setting. + * @return ::MQTTASYNC_SUCCESS if the callbacks were correctly set, + * ::MQTTASYNC_FAILURE if an error occurred. + */ +DLLExport int MQTTAsync_setConnected(MQTTAsync handle, void* context, MQTTAsync_connected* co); + + +/** + * Reconnects a client with the previously used connect options. Connect + * must have previously been called for this to work. + * @param handle A valid client handle from a successful call to + * MQTTAsync_create(). + * @return ::MQTTASYNC_SUCCESS if the callbacks were correctly set, + * ::MQTTASYNC_FAILURE if an error occurred. + */ +DLLExport int MQTTAsync_reconnect(MQTTAsync handle); + + +/** + * This function creates an MQTT client ready for connection to the + * specified server and using the specified persistent storage (see + * MQTTAsync_persistence). See also MQTTAsync_destroy(). + * @param handle A pointer to an ::MQTTAsync handle. The handle is + * populated with a valid client reference following a successful return from + * this function. + * @param serverURI A null-terminated string specifying the server to + * which the client will connect. It takes the form protocol://host:port. + * protocol must be tcp or ssl. For host, you can + * specify either an IP address or a host name. For instance, to connect to + * a server running on the local machines with the default MQTT port, specify + * tcp://localhost:1883. + * @param clientId The client identifier passed to the server when the + * client connects to it. It is a null-terminated UTF-8 encoded string. + * @param persistence_type The type of persistence to be used by the client: + *
+ * ::MQTTCLIENT_PERSISTENCE_NONE: Use in-memory persistence. If the device or + * system on which the client is running fails or is switched off, the current + * state of any in-flight messages is lost and some messages may not be + * delivered even at QoS1 and QoS2. + *
+ * ::MQTTCLIENT_PERSISTENCE_DEFAULT: Use the default (file system-based) + * persistence mechanism. Status about in-flight messages is held in persistent + * storage and provides some protection against message loss in the case of + * unexpected failure. + *
+ * ::MQTTCLIENT_PERSISTENCE_USER: Use an application-specific persistence + * implementation. Using this type of persistence gives control of the + * persistence mechanism to the application. The application has to implement + * the MQTTClient_persistence interface. + * @param persistence_context If the application uses + * ::MQTTCLIENT_PERSISTENCE_NONE persistence, this argument is unused and should + * be set to NULL. For ::MQTTCLIENT_PERSISTENCE_DEFAULT persistence, it + * should be set to the location of the persistence directory (if set + * to NULL, the persistence directory used is the working directory). + * Applications that use ::MQTTCLIENT_PERSISTENCE_USER persistence set this + * argument to point to a valid MQTTClient_persistence structure. + * @return ::MQTTASYNC_SUCCESS if the client is successfully created, otherwise + * an error code is returned. + */ +DLLExport int MQTTAsync_create(MQTTAsync* handle, const char* serverURI, const char* clientId, + int persistence_type, void* persistence_context); + +typedef struct +{ + /** The eyecatcher for this structure. must be MQCO. */ + char struct_id[4]; + /** The version number of this structure. Must be 0 or 1 + * 0 means no MQTTVersion + */ + int struct_version; + /** Whether to allow messages to be sent when the client library is not connected. */ + int sendWhileDisconnected; + /** the maximum number of messages allowed to be buffered while not connected. */ + int maxBufferedMessages; + /** Whether the MQTT version is 3.1, 3.1.1, or 5. To use V5, this must be set. + * MQTT V5 has to be chosen here, because during the create call the message persistence + * is initialized, and we want to know whether the format of any persisted messages + * is appropriate for the MQTT version we are going to connect with. Selecting 3.1 or + * 3.1.1 and attempting to read 5.0 persisted messages will result in an error on create. */ + int MQTTVersion; +} MQTTAsync_createOptions; + +#define MQTTAsync_createOptions_initializer { {'M', 'Q', 'C', 'O'}, 0, 0, 100, MQTTVERSION_DEFAULT } + + +DLLExport int MQTTAsync_createWithOptions(MQTTAsync* handle, const char* serverURI, const char* clientId, + int persistence_type, void* persistence_context, MQTTAsync_createOptions* options); + +/** + * MQTTAsync_willOptions defines the MQTT "Last Will and Testament" (LWT) settings for + * the client. In the event that a client unexpectedly loses its connection to + * the server, the server publishes the LWT message to the LWT topic on + * behalf of the client. This allows other clients (subscribed to the LWT topic) + * to be made aware that the client has disconnected. To enable the LWT + * function for a specific client, a valid pointer to an MQTTAsync_willOptions + * structure is passed in the MQTTAsync_connectOptions structure used in the + * MQTTAsync_connect() call that connects the client to the server. The pointer + * to MQTTAsync_willOptions can be set to NULL if the LWT function is not + * required. + */ +typedef struct +{ + /** The eyecatcher for this structure. must be MQTW. */ + char struct_id[4]; + /** The version number of this structure. Must be 0 or 1 + 0 indicates no binary will message support + */ + int struct_version; + /** The LWT topic to which the LWT message will be published. */ + const char* topicName; + /** The LWT payload. */ + const char* message; + /** + * The retained flag for the LWT message (see MQTTAsync_message.retained). + */ + int retained; + /** + * The quality of service setting for the LWT message (see + * MQTTAsync_message.qos and @ref qos). + */ + int qos; + /** The LWT payload in binary form. This is only checked and used if the message option is NULL */ + struct + { + int len; /**< binary payload length */ + const void* data; /**< binary payload data */ + } payload; +} MQTTAsync_willOptions; + +#define MQTTAsync_willOptions_initializer { {'M', 'Q', 'T', 'W'}, 1, NULL, NULL, 0, 0, { 0, NULL } } + +#define MQTT_SSL_VERSION_DEFAULT 0 +#define MQTT_SSL_VERSION_TLS_1_0 1 +#define MQTT_SSL_VERSION_TLS_1_1 2 +#define MQTT_SSL_VERSION_TLS_1_2 3 + +/** +* MQTTAsync_sslProperties defines the settings to establish an SSL/TLS connection using the +* OpenSSL library. It covers the following scenarios: +* - Server authentication: The client needs the digital certificate of the server. It is included +* in a store containting trusted material (also known as "trust store"). +* - Mutual authentication: Both client and server are authenticated during the SSL handshake. In +* addition to the digital certificate of the server in a trust store, the client will need its own +* digital certificate and the private key used to sign its digital certificate stored in a "key store". +* - Anonymous connection: Both client and server do not get authenticated and no credentials are needed +* to establish an SSL connection. Note that this scenario is not fully secure since it is subject to +* man-in-the-middle attacks. +*/ +typedef struct +{ + /** The eyecatcher for this structure. Must be MQTS */ + char struct_id[4]; + /** The version number of this structure. Must be 0, or 1 to enable TLS version selection. */ + int struct_version; + + /** The file in PEM format containing the public digital certificates trusted by the client. */ + const char* trustStore; + + /** The file in PEM format containing the public certificate chain of the client. It may also include + * the client's private key. + */ + const char* keyStore; + + /** If not included in the sslKeyStore, this setting points to the file in PEM format containing + * the client's private key. + */ + const char* privateKey; + /** The password to load the client's privateKey if encrypted. */ + const char* privateKeyPassword; + + /** + * The list of cipher suites that the client will present to the server during the SSL handshake. For a + * full explanation of the cipher list format, please see the OpenSSL on-line documentation: + * http://www.openssl.org/docs/apps/ciphers.html#CIPHER_LIST_FORMAT + * If this setting is ommitted, its default value will be "ALL", that is, all the cipher suites -excluding + * those offering no encryption- will be considered. + * This setting can be used to set an SSL anonymous connection ("aNULL" string value, for instance). + */ + const char* enabledCipherSuites; + + /** True/False option to enable verification of the server certificate **/ + int enableServerCertAuth; + + /** The SSL/TLS version to use. Specify one of MQTT_SSL_VERSION_DEFAULT (0), + * MQTT_SSL_VERSION_TLS_1_0 (1), MQTT_SSL_VERSION_TLS_1_1 (2) or MQTT_SSL_VERSION_TLS_1_2 (3). + * Only used if struct_version is >= 1. + */ + int sslVersion; + + /** + * Whether to carry out post-connect checks, including that a certificate + * matches the given host name. + * Exists only if struct_version >= 2 + */ + int verify; + + /** + * From the OpenSSL documentation: + * If CApath is not NULL, it points to a directory containing CA certificates in PEM format. + * Exists only if struct_version >= 2 + */ + const char* CApath; + + /** + * Callback function for OpenSSL error handler ERR_print_errors_cb + * Exists only if struct_version >= 3 + */ + int (*ssl_error_cb) (const char *str, size_t len, void *u); + + /** + * Application-specific contex for OpenSSL error handler ERR_print_errors_cb + * Exists only if struct_version >= 3 + */ + void* ssl_error_context; +} MQTTAsync_SSLOptions; + +#define MQTTAsync_SSLOptions_initializer { {'M', 'Q', 'T', 'S'}, 3, NULL, NULL, NULL, NULL, NULL, 1, MQTT_SSL_VERSION_DEFAULT, 0, NULL, NULL, NULL } + +/** + * MQTTAsync_connectOptions defines several settings that control the way the + * client connects to an MQTT server. Default values are set in + * MQTTAsync_connectOptions_initializer. + */ +typedef struct +{ + /** The eyecatcher for this structure. must be MQTC. */ + char struct_id[4]; + /** The version number of this structure. Must be 0, 1, 2, 3 4 5 or 6. + * 0 signifies no SSL options and no serverURIs + * 1 signifies no serverURIs + * 2 signifies no MQTTVersion + * 3 signifies no automatic reconnect options + * 4 signifies no binary password option (just string) + * 5 signifies no MQTTV5 properties + */ + int struct_version; + /** The "keep alive" interval, measured in seconds, defines the maximum time + * that should pass without communication between the client and the server + * The client will ensure that at least one message travels across the + * network within each keep alive period. In the absence of a data-related + * message during the time period, the client sends a very small MQTT + * "ping" message, which the server will acknowledge. The keep alive + * interval enables the client to detect when the server is no longer + * available without having to wait for the long TCP/IP timeout. + * Set to 0 if you do not want any keep alive processing. + */ + int keepAliveInterval; + /** + * This is a boolean value. The cleansession setting controls the behaviour + * of both the client and the server at connection and disconnection time. + * The client and server both maintain session state information. This + * information is used to ensure "at least once" and "exactly once" + * delivery, and "exactly once" receipt of messages. Session state also + * includes subscriptions created by an MQTT client. You can choose to + * maintain or discard state information between sessions. + * + * When cleansession is true, the state information is discarded at + * connect and disconnect. Setting cleansession to false keeps the state + * information. When you connect an MQTT client application with + * MQTTAsync_connect(), the client identifies the connection using the + * client identifier and the address of the server. The server checks + * whether session information for this client + * has been saved from a previous connection to the server. If a previous + * session still exists, and cleansession=true, then the previous session + * information at the client and server is cleared. If cleansession=false, + * the previous session is resumed. If no previous session exists, a new + * session is started. + */ + int cleansession; + /** + * This controls how many messages can be in-flight simultaneously. + */ + int maxInflight; + /** + * This is a pointer to an MQTTAsync_willOptions structure. If your + * application does not make use of the Last Will and Testament feature, + * set this pointer to NULL. + */ + MQTTAsync_willOptions* will; + /** + * MQTT servers that support the MQTT v3.1 protocol provide authentication + * and authorisation by user name and password. This is the user name + * parameter. + */ + const char* username; + /** + * MQTT servers that support the MQTT v3.1 protocol provide authentication + * and authorisation by user name and password. This is the password + * parameter. + */ + const char* password; + /** + * The time interval in seconds to allow a connect to complete. + */ + int connectTimeout; + /** + * The time interval in seconds after which unacknowledged publish requests are + * retried during a TCP session. With MQTT 3.1.1 and later, retries are + * not required except on reconnect. 0 turns off in-session retries, and is the + * recommended setting. Adding retries to an already overloaded network only + * exacerbates the problem. + */ + int retryInterval; + /** + * This is a pointer to an MQTTAsync_SSLOptions structure. If your + * application does not make use of SSL, set this pointer to NULL. + */ + MQTTAsync_SSLOptions* ssl; + /** + * A pointer to a callback function to be called if the connect successfully + * completes. Can be set to NULL, in which case no indication of successful + * completion will be received. + */ + MQTTAsync_onSuccess* onSuccess; + /** + * A pointer to a callback function to be called if the connect fails. + * Can be set to NULL, in which case no indication of unsuccessful + * completion will be received. + */ + MQTTAsync_onFailure* onFailure; + /** + * A pointer to any application-specific context. The + * the context pointer is passed to success or failure callback functions to + * provide access to the context information in the callback. + */ + void* context; + /** + * The number of entries in the serverURIs array. + */ + int serverURIcount; + /** + * An array of null-terminated strings specifying the servers to + * which the client will connect. Each string takes the form protocol://host:port. + * protocol must be tcp or ssl. For host, you can + * specify either an IP address or a domain name. For instance, to connect to + * a server running on the local machines with the default MQTT port, specify + * tcp://localhost:1883. + */ + char* const* serverURIs; + /** + * Sets the version of MQTT to be used on the connect. + * MQTTVERSION_DEFAULT (0) = default: start with 3.1.1, and if that fails, fall back to 3.1 + * MQTTVERSION_3_1 (3) = only try version 3.1 + * MQTTVERSION_3_1_1 (4) = only try version 3.1.1 + */ + int MQTTVersion; + /** + * Reconnect automatically in the case of a connection being lost? + */ + int automaticReconnect; + /** + * Minimum retry interval in seconds. Doubled on each failed retry. + */ + int minRetryInterval; + /** + * Maximum retry interval in seconds. The doubling stops here on failed retries. + */ + int maxRetryInterval; + /** + * Optional binary password. Only checked and used if the password option is NULL + */ + struct { + int len; /**< binary password length */ + const void* data; /**< binary password data */ + } binarypwd; + /* + * MQTT V5 clean start flag. Only clears state at the beginning of the session. + */ + int cleanstart; + /** + * MQTT V5 properties for connect + */ + MQTTProperties *connectProperties; + /** + * MQTT V5 properties for the will message in the connect + */ + MQTTProperties *willProperties; + /** + * A pointer to a callback function to be called if the connect successfully + * completes. Can be set to NULL, in which case no indication of successful + * completion will be received. + */ + MQTTAsync_onSuccess5* onSuccess5; + /** + * A pointer to a callback function to be called if the connect fails. + * Can be set to NULL, in which case no indication of unsuccessful + * completion will be received. + */ + MQTTAsync_onFailure5* onFailure5; +} MQTTAsync_connectOptions; + + +#define MQTTAsync_connectOptions_initializer { {'M', 'Q', 'T', 'C'}, 6, 60, 1, 65535, NULL, NULL, NULL, 30, 0,\ +NULL, NULL, NULL, NULL, 0, NULL, MQTTVERSION_DEFAULT, 0, 1, 60, {0, NULL}, 0, NULL, NULL, NULL, NULL} + +#define MQTTAsync_connectOptions_initializer5 { {'M', 'Q', 'T', 'C'}, 6, 60, 0, 65535, NULL, NULL, NULL, 30, 0,\ +NULL, NULL, NULL, NULL, 0, NULL, MQTTVERSION_5, 0, 1, 60, {0, NULL}, 1, NULL, NULL, NULL, NULL} + + +/** + * This function attempts to connect a previously-created client (see + * MQTTAsync_create()) to an MQTT server using the specified options. If you + * want to enable asynchronous message and status notifications, you must call + * MQTTAsync_setCallbacks() prior to MQTTAsync_connect(). + * @param handle A valid client handle from a successful call to + * MQTTAsync_create(). + * @param options A pointer to a valid MQTTAsync_connectOptions + * structure. + * @return ::MQTTASYNC_SUCCESS if the client connect request was accepted. + * If the client was unable to connect to the server, an error code is + * returned via the onFailure callback, if set. + * Error codes greater than 0 are returned by the MQTT protocol:

+ * 1: Connection refused: Unacceptable protocol version
+ * 2: Connection refused: Identifier rejected
+ * 3: Connection refused: Server unavailable
+ * 4: Connection refused: Bad user name or password
+ * 5: Connection refused: Not authorized
+ * 6-255: Reserved for future use
+ */ +DLLExport int MQTTAsync_connect(MQTTAsync handle, const MQTTAsync_connectOptions* options); + + +typedef struct +{ + /** The eyecatcher for this structure. Must be MQTD. */ + char struct_id[4]; + /** The version number of this structure. Must be 0 or 1. 0 signifies no V5 properties */ + int struct_version; + /** + * The client delays disconnection for up to this time (in + * milliseconds) in order to allow in-flight message transfers to complete. + */ + int timeout; + /** + * A pointer to a callback function to be called if the disconnect successfully + * completes. Can be set to NULL, in which case no indication of successful + * completion will be received. + */ + MQTTAsync_onSuccess* onSuccess; + /** + * A pointer to a callback function to be called if the disconnect fails. + * Can be set to NULL, in which case no indication of unsuccessful + * completion will be received. + */ + MQTTAsync_onFailure* onFailure; + /** + * A pointer to any application-specific context. The + * the context pointer is passed to success or failure callback functions to + * provide access to the context information in the callback. + */ + void* context; + /** + * MQTT V5 input properties + */ + MQTTProperties properties; + /** + * Reason code for MQTTV5 disconnect + */ + enum MQTTReasonCodes reasonCode; + /** + * A pointer to a callback function to be called if the disconnect successfully + * completes. Can be set to NULL, in which case no indication of successful + * completion will be received. + */ + MQTTAsync_onSuccess5* onSuccess5; + /** + * A pointer to a callback function to be called if the disconnect fails. + * Can be set to NULL, in which case no indication of unsuccessful + * completion will be received. + */ + MQTTAsync_onFailure5* onFailure5; +} MQTTAsync_disconnectOptions; + +#define MQTTAsync_disconnectOptions_initializer { {'M', 'Q', 'T', 'D'}, 1, 0, NULL, NULL, NULL, MQTTProperties_initializer, MQTTREASONCODE_SUCCESS } + + +/** + * This function attempts to disconnect the client from the MQTT + * server. In order to allow the client time to complete handling of messages + * that are in-flight when this function is called, a timeout period is + * specified. When the timeout period has expired, the client disconnects even + * if there are still outstanding message acknowledgements. + * The next time the client connects to the same server, any QoS 1 or 2 + * messages which have not completed will be retried depending on the + * cleansession settings for both the previous and the new connection (see + * MQTTAsync_connectOptions.cleansession and MQTTAsync_connect()). + * @param handle A valid client handle from a successful call to + * MQTTAsync_create(). + * @param options The client delays disconnection for up to this time (in + * milliseconds) in order to allow in-flight message transfers to complete. + * @return ::MQTTASYNC_SUCCESS if the client successfully disconnects from + * the server. An error code is returned if the client was unable to disconnect + * from the server + */ +DLLExport int MQTTAsync_disconnect(MQTTAsync handle, const MQTTAsync_disconnectOptions* options); + + +/** + * This function allows the client application to test whether or not a + * client is currently connected to the MQTT server. + * @param handle A valid client handle from a successful call to + * MQTTAsync_create(). + * @return Boolean true if the client is connected, otherwise false. + */ +DLLExport int MQTTAsync_isConnected(MQTTAsync handle); + + +/** + * This function attempts to subscribe a client to a single topic, which may + * contain wildcards (see @ref wildcard). This call also specifies the + * @ref qos requested for the subscription + * (see also MQTTAsync_subscribeMany()). + * @param handle A valid client handle from a successful call to + * MQTTAsync_create(). + * @param topic The subscription topic, which may include wildcards. + * @param qos The requested quality of service for the subscription. + * @param response A pointer to a response options structure. Used to set callback functions. + * @return ::MQTTASYNC_SUCCESS if the subscription request is successful. + * An error code is returned if there was a problem registering the + * subscription. + */ +DLLExport int MQTTAsync_subscribe(MQTTAsync handle, const char* topic, int qos, MQTTAsync_responseOptions* response); + + +/** + * This function attempts to subscribe a client to a list of topics, which may + * contain wildcards (see @ref wildcard). This call also specifies the + * @ref qos requested for each topic (see also MQTTAsync_subscribe()). + * @param handle A valid client handle from a successful call to + * MQTTAsync_create(). + * @param count The number of topics for which the client is requesting + * subscriptions. + * @param topic An array (of length count) of pointers to + * topics, each of which may include wildcards. + * @param qos An array (of length count) of @ref qos + * values. qos[n] is the requested QoS for topic[n]. + * @param response A pointer to a response options structure. Used to set callback functions. + * @return ::MQTTASYNC_SUCCESS if the subscription request is successful. + * An error code is returned if there was a problem registering the + * subscriptions. + */ +DLLExport int MQTTAsync_subscribeMany(MQTTAsync handle, int count, char* const* topic, int* qos, MQTTAsync_responseOptions* response); + +/** + * This function attempts to remove an existing subscription made by the + * specified client. + * @param handle A valid client handle from a successful call to + * MQTTAsync_create(). + * @param topic The topic for the subscription to be removed, which may + * include wildcards (see @ref wildcard). + * @param response A pointer to a response options structure. Used to set callback functions. + * @return ::MQTTASYNC_SUCCESS if the subscription is removed. + * An error code is returned if there was a problem removing the + * subscription. + */ +DLLExport int MQTTAsync_unsubscribe(MQTTAsync handle, const char* topic, MQTTAsync_responseOptions* response); + +/** + * This function attempts to remove existing subscriptions to a list of topics + * made by the specified client. + * @param handle A valid client handle from a successful call to + * MQTTAsync_create(). + * @param count The number subscriptions to be removed. + * @param topic An array (of length count) of pointers to the topics of + * the subscriptions to be removed, each of which may include wildcards. + * @param response A pointer to a response options structure. Used to set callback functions. + * @return ::MQTTASYNC_SUCCESS if the subscriptions are removed. + * An error code is returned if there was a problem removing the subscriptions. + */ +DLLExport int MQTTAsync_unsubscribeMany(MQTTAsync handle, int count, char* const* topic, MQTTAsync_responseOptions* response); + + +/** + * This function attempts to publish a message to a given topic (see also + * ::MQTTAsync_sendMessage()). An ::MQTTAsync_token is issued when + * this function returns successfully. If the client application needs to + * test for successful delivery of messages, a callback should be set + * (see ::MQTTAsync_onSuccess() and ::MQTTAsync_deliveryComplete()). + * @param handle A valid client handle from a successful call to + * MQTTAsync_create(). + * @param destinationName The topic associated with this message. + * @param payloadlen The length of the payload in bytes. + * @param payload A pointer to the byte array payload of the message. + * @param qos The @ref qos of the message. + * @param retained The retained flag for the message. + * @param response A pointer to an ::MQTTAsync_responseOptions structure. Used to set callback functions. + * This is optional and can be set to NULL. + * @return ::MQTTASYNC_SUCCESS if the message is accepted for publication. + * An error code is returned if there was a problem accepting the message. + */ +DLLExport int MQTTAsync_send(MQTTAsync handle, const char* destinationName, int payloadlen, const void* payload, int qos, + int retained, MQTTAsync_responseOptions* response); + + +/** + * This function attempts to publish a message to a given topic (see also + * MQTTAsync_publish()). An ::MQTTAsync_token is issued when + * this function returns successfully. If the client application needs to + * test for successful delivery of messages, a callback should be set + * (see ::MQTTAsync_onSuccess() and ::MQTTAsync_deliveryComplete()). + * @param handle A valid client handle from a successful call to + * MQTTAsync_create(). + * @param destinationName The topic associated with this message. + * @param msg A pointer to a valid MQTTAsync_message structure containing + * the payload and attributes of the message to be published. + * @param response A pointer to an ::MQTTAsync_responseOptions structure. Used to set callback functions. + * @return ::MQTTASYNC_SUCCESS if the message is accepted for publication. + * An error code is returned if there was a problem accepting the message. + */ +DLLExport int MQTTAsync_sendMessage(MQTTAsync handle, const char* destinationName, const MQTTAsync_message* msg, MQTTAsync_responseOptions* response); + + +/** + * This function sets a pointer to an array of tokens for + * messages that are currently in-flight (pending completion). + * + * Important note: The memory used to hold the array of tokens is + * malloc()'d in this function. The client application is responsible for + * freeing this memory when it is no longer required. + * @param handle A valid client handle from a successful call to + * MQTTAsync_create(). + * @param tokens The address of a pointer to an ::MQTTAsync_token. + * When the function returns successfully, the pointer is set to point to an + * array of tokens representing messages pending completion. The last member of + * the array is set to -1 to indicate there are no more tokens. If no tokens + * are pending, the pointer is set to NULL. + * @return ::MQTTASYNC_SUCCESS if the function returns successfully. + * An error code is returned if there was a problem obtaining the list of + * pending tokens. + */ +DLLExport int MQTTAsync_getPendingTokens(MQTTAsync handle, MQTTAsync_token **tokens); + +/** + * Tests whether a request corresponding to a token is complete. + * + * @param handle A valid client handle from a successful call to + * MQTTAsync_create(). + * @param token An ::MQTTAsync_token associated with a request. + * @return 1 if the request has been completed, 0 if not. + */ +#define MQTTASYNC_TRUE 1 +DLLExport int MQTTAsync_isComplete(MQTTAsync handle, MQTTAsync_token token); + + +/** + * Waits for a request corresponding to a token to complete. + * + * @param handle A valid client handle from a successful call to + * MQTTAsync_create(). + * @param token An ::MQTTAsync_token associated with a request. + * @param timeout the maximum time to wait for completion, in milliseconds + * @return ::MQTTASYNC_SUCCESS if the request has been completed in the time allocated, + * ::MQTTASYNC_FAILURE if not. + */ +DLLExport int MQTTAsync_waitForCompletion(MQTTAsync handle, MQTTAsync_token token, unsigned long timeout); + + +/** + * This function frees memory allocated to an MQTT message, including the + * additional memory allocated to the message payload. The client application + * calls this function when the message has been fully processed. Important + * note: This function does not free the memory allocated to a message + * topic string. It is the responsibility of the client application to free + * this memory using the MQTTAsync_free() library function. + * @param msg The address of a pointer to the ::MQTTAsync_message structure + * to be freed. + */ +DLLExport void MQTTAsync_freeMessage(MQTTAsync_message** msg); + +/** + * This function frees memory allocated by the MQTT C client library, especially the + * topic name. This is needed on Windows when the client libary and application + * program have been compiled with different versions of the C compiler. It is + * thus good policy to always use this function when freeing any MQTT C client- + * allocated memory. + * @param ptr The pointer to the client library storage to be freed. + */ +DLLExport void MQTTAsync_free(void* ptr); + +/** + * This function frees the memory allocated to an MQTT client (see + * MQTTAsync_create()). It should be called when the client is no longer + * required. + * @param handle A pointer to the handle referring to the ::MQTTAsync + * structure to be freed. + */ +DLLExport void MQTTAsync_destroy(MQTTAsync* handle); + + + +enum MQTTASYNC_TRACE_LEVELS +{ + MQTTASYNC_TRACE_MAXIMUM = 1, + MQTTASYNC_TRACE_MEDIUM, + MQTTASYNC_TRACE_MINIMUM, + MQTTASYNC_TRACE_PROTOCOL, + MQTTASYNC_TRACE_ERROR, + MQTTASYNC_TRACE_SEVERE, + MQTTASYNC_TRACE_FATAL, +}; + + +/** + * This function sets the level of trace information which will be + * returned in the trace callback. + * @param level the trace level required + */ +DLLExport void MQTTAsync_setTraceLevel(enum MQTTASYNC_TRACE_LEVELS level); + + +/** + * This is a callback function prototype which must be implemented if you want + * to receive trace information. + * @param level the trace level of the message returned + * @param message the trace message. This is a pointer to a static buffer which + * will be overwritten on each call. You must copy the data if you want to keep + * it for later. + */ +typedef void MQTTAsync_traceCallback(enum MQTTASYNC_TRACE_LEVELS level, char* message); + +/** + * This function sets the trace callback if needed. If set to NULL, + * no trace information will be returned. The default trace level is + * MQTTASYNC_TRACE_MINIMUM. + * @param callback a pointer to the function which will handle the trace information + */ +DLLExport void MQTTAsync_setTraceCallback(MQTTAsync_traceCallback* callback); + + +typedef struct +{ + const char* name; + const char* value; +} MQTTAsync_nameValue; + +/** + * This function returns version information about the library. + * no trace information will be returned. The default trace level is + * MQTTASYNC_TRACE_MINIMUM + * @return an array of strings describing the library. The last entry is a NULL pointer. + */ +DLLExport MQTTAsync_nameValue* MQTTAsync_getVersionInfo(void); + +/** + * Returns a pointer to a string representation of the error code, or NULL. + * Do not free after use. Returns NULL if the error code is unknown. + * @param code the MQTTASYNC_ return code. + * @return a static string representation of the error code. + */ +DLLExport const char* MQTTAsync_strerror(int code); + + +/** + * @cond MQTTAsync_main + * @page async Threading + * The client application runs on several threads. + * Processing of handshaking and maintaining + * the network connection is performed in the background. + * This API is thread safe: functions may be called by multiple application + * threads. + * Notifications of status and message reception are provided to the client + * application using callbacks registered with the library by the call to + * MQTTAsync_setCallbacks() (see MQTTAsync_messageArrived(), + * MQTTAsync_connectionLost() and MQTTAsync_deliveryComplete()). + * In addition, some functions allow success and failure callbacks to be set + * for individual requests, in the ::MQTTAsync_responseOptions structure. Applications + * can be written as a chain of callback functions. Note that it is a theoretically + * possible but unlikely event, that a success or failure callback could be called + * before function requesting the callback has returned. In this case the token + * delivered in the callback would not yet be known to the application program (see + * Race condition for MQTTAsync_token in MQTTAsync.c + * https://bugs.eclipse.org/bugs/show_bug.cgi?id=444093) + * + * @page auto_reconnect Automatic Reconnect + * The ability for the client library to reconnect automatically in the event + * of a connection failure was added in 1.1. The connection lost callback + * allows a flexible response to the loss of a connection, so almost any + * behaviour can be implemented in that way. Automatic reconnect does have the + * advantage of being a little simpler to use. + * + * To switch on automatic reconnect, the connect options field + * automaticReconnect should be set to non-zero. The minimum and maximum times + * before the next connection attempt can also be set, the defaults being 1 and + * 60 seconds. At each failure to reconnect, the retry interval is doubled until + * the maximum value is reached, and there it stays until the connection is + * successfully re-established whereupon it is reset. + * + * When a reconnection attempt is successful, the ::MQTTAsync_connected callback + * function is invoked, if set by calling ::MQTTAsync_setConnected. This allows + * the application to take any actions needed, such as amending subscriptions. + * + * @page offline_publish Publish While Disconnected + * This feature was not originally available because with persistence enabled, + * messages could be stored locally without ever knowing if they could be sent. + * The client application could have created the client with an erroneous broker + * address or port for instance. + * + * To enable messages to be published when the application is disconnected + * ::MQTTAsync_createWithOptions must be used instead of ::MQTTAsync_create to + * create the client object. The ::createOptions field sendWhileDisconnected + * must be set to non-zero, and the maxBufferedMessages field set as required - + * the default being 100. + * + * ::MQTTAsync_getPendingTokens can be called to return the ids of the messages + * waiting to be sent, or for which the sending process has not completed. + * + * @page wildcard Subscription wildcards + * Every MQTT message includes a topic that classifies it. MQTT servers use + * topics to determine which subscribers should receive messages published to + * the server. + * + * Consider the server receiving messages from several environmental sensors. + * Each sensor publishes its measurement data as a message with an associated + * topic. Subscribing applications need to know which sensor originally + * published each received message. A unique topic is thus used to identify + * each sensor and measurement type. Topics such as SENSOR1TEMP, + * SENSOR1HUMIDITY, SENSOR2TEMP and so on achieve this but are not very + * flexible. If additional sensors are added to the system at a later date, + * subscribing applications must be modified to receive them. + * + * To provide more flexibility, MQTT supports a hierarchical topic namespace. + * This allows application designers to organize topics to simplify their + * management. Levels in the hierarchy are delimited by the '/' character, + * such as SENSOR/1/HUMIDITY. Publishers and subscribers use these + * hierarchical topics as already described. + * + * For subscriptions, two wildcard characters are supported: + *
    + *
  • A '#' character represents a complete sub-tree of the hierarchy and + * thus must be the last character in a subscription topic string, such as + * SENSOR/#. This will match any topic starting with SENSOR/, such as + * SENSOR/1/TEMP and SENSOR/2/HUMIDITY.
    • + *
  • A '+' character represents a single level of the hierarchy and is + * used between delimiters. For example, SENSOR/+/TEMP will match + * SENSOR/1/TEMP and SENSOR/2/TEMP.
    • + *
+ * Publishers are not allowed to use the wildcard characters in their topic + * names. + * + * Deciding on your topic hierarchy is an important step in your system design. + * + * @page qos Quality of service + * The MQTT protocol provides three qualities of service for delivering + * messages between clients and servers: "at most once", "at least once" and + * "exactly once". + * + * Quality of service (QoS) is an attribute of an individual message being + * published. An application sets the QoS for a specific message by setting the + * MQTTAsync_message.qos field to the required value. + * + * A subscribing client can set the maximum quality of service a server uses + * to send messages that match the client subscriptions. The + * MQTTAsync_subscribe() and MQTTAsync_subscribeMany() functions set this + * maximum. The QoS of a message forwarded to a subscriber thus might be + * different to the QoS given to the message by the original publisher. + * The lower of the two values is used to forward a message. + * + * The three levels are: + * + * QoS0, At most once: The message is delivered at most once, or it + * may not be delivered at all. Its delivery across the network is not + * acknowledged. The message is not stored. The message could be lost if the + * client is disconnected, or if the server fails. QoS0 is the fastest mode of + * transfer. It is sometimes called "fire and forget". + * + * The MQTT protocol does not require servers to forward publications at QoS0 + * to a client. If the client is disconnected at the time the server receives + * the publication, the publication might be discarded, depending on the + * server implementation. + * + * QoS1, At least once: The message is always delivered at least once. + * It might be delivered multiple times if there is a failure before an + * acknowledgment is received by the sender. The message must be stored + * locally at the sender, until the sender receives confirmation that the + * message has been published by the receiver. The message is stored in case + * the message must be sent again. + * + * QoS2, Exactly once: The message is always delivered exactly once. + * The message must be stored locally at the sender, until the sender receives + * confirmation that the message has been published by the receiver. The + * message is stored in case the message must be sent again. QoS2 is the + * safest, but slowest mode of transfer. A more sophisticated handshaking + * and acknowledgement sequence is used than for QoS1 to ensure no duplication + * of messages occurs. + + + * @page publish Publication example +@code +#include +#include +#include +#include "MQTTAsync.h" + +#define ADDRESS "tcp://localhost:1883" +#define CLIENTID "ExampleClientPub" +#define TOPIC "MQTT Examples" +#define PAYLOAD "Hello World!" +#define QOS 1 +#define TIMEOUT 10000L + +volatile MQTTAsync_token deliveredtoken; + +int finished = 0; + +void connlost(void *context, char *cause) +{ + MQTTAsync client = (MQTTAsync)context; + MQTTAsync_connectOptions conn_opts = MQTTAsync_connectOptions_initializer; + int rc; + + printf("\nConnection lost\n"); + printf(" cause: %s\n", cause); + + printf("Reconnecting\n"); + conn_opts.keepAliveInterval = 20; + conn_opts.cleansession = 1; + if ((rc = MQTTAsync_connect(client, &conn_opts)) != MQTTASYNC_SUCCESS) + { + printf("Failed to start connect, return code %d\n", rc); + finished = 1; + } +} + + +void onDisconnect(void* context, MQTTAsync_successData* response) +{ + printf("Successful disconnection\n"); + finished = 1; +} + + +void onSend(void* context, MQTTAsync_successData* response) +{ + MQTTAsync client = (MQTTAsync)context; + MQTTAsync_disconnectOptions opts = MQTTAsync_disconnectOptions_initializer; + int rc; + + printf("Message with token value %d delivery confirmed\n", response->token); + + opts.onSuccess = onDisconnect; + opts.context = client; + + if ((rc = MQTTAsync_disconnect(client, &opts)) != MQTTASYNC_SUCCESS) + { + printf("Failed to start sendMessage, return code %d\n", rc); + exit(EXIT_FAILURE); + } +} + + +void onConnectFailure(void* context, MQTTAsync_failureData* response) +{ + printf("Connect failed, rc %d\n", response ? response->code : 0); + finished = 1; +} + + +void onConnect(void* context, MQTTAsync_successData* response) +{ + MQTTAsync client = (MQTTAsync)context; + MQTTAsync_responseOptions opts = MQTTAsync_responseOptions_initializer; + MQTTAsync_message pubmsg = MQTTAsync_message_initializer; + int rc; + + printf("Successful connection\n"); + + opts.onSuccess = onSend; + opts.context = client; + + pubmsg.payload = PAYLOAD; + pubmsg.payloadlen = strlen(PAYLOAD); + pubmsg.qos = QOS; + pubmsg.retained = 0; + deliveredtoken = 0; + + if ((rc = MQTTAsync_sendMessage(client, TOPIC, &pubmsg, &opts)) != MQTTASYNC_SUCCESS) + { + printf("Failed to start sendMessage, return code %d\n", rc); + exit(EXIT_FAILURE); + } +} + + +int main(int argc, char* argv[]) +{ + MQTTAsync client; + MQTTAsync_connectOptions conn_opts = MQTTAsync_connectOptions_initializer; + MQTTAsync_message pubmsg = MQTTAsync_message_initializer; + MQTTAsync_token token; + int rc; + + MQTTAsync_create(&client, ADDRESS, CLIENTID, MQTTCLIENT_PERSISTENCE_NONE, NULL); + + MQTTAsync_setCallbacks(client, NULL, connlost, NULL, NULL); + + conn_opts.keepAliveInterval = 20; + conn_opts.cleansession = 1; + conn_opts.onSuccess = onConnect; + conn_opts.onFailure = onConnectFailure; + conn_opts.context = client; + if ((rc = MQTTAsync_connect(client, &conn_opts)) != MQTTASYNC_SUCCESS) + { + printf("Failed to start connect, return code %d\n", rc); + exit(EXIT_FAILURE); + } + + printf("Waiting for publication of %s\n" + "on topic %s for client with ClientID: %s\n", + PAYLOAD, TOPIC, CLIENTID); + while (!finished) + #if defined(WIN32) || defined(WIN64) + Sleep(100); + #else + usleep(10000L); + #endif + + MQTTAsync_destroy(&client); + return rc; +} + + * @endcode + * @page subscribe Subscription example +@code +#include +#include +#include +#include "MQTTAsync.h" + +#define ADDRESS "tcp://localhost:1883" +#define CLIENTID "ExampleClientSub" +#define TOPIC "MQTT Examples" +#define PAYLOAD "Hello World!" +#define QOS 1 +#define TIMEOUT 10000L + +volatile MQTTAsync_token deliveredtoken; + +int disc_finished = 0; +int subscribed = 0; +int finished = 0; + +void connlost(void *context, char *cause) +{ + MQTTAsync client = (MQTTAsync)context; + MQTTAsync_connectOptions conn_opts = MQTTAsync_connectOptions_initializer; + int rc; + + printf("\nConnection lost\n"); + printf(" cause: %s\n", cause); + + printf("Reconnecting\n"); + conn_opts.keepAliveInterval = 20; + conn_opts.cleansession = 1; + if ((rc = MQTTAsync_connect(client, &conn_opts)) != MQTTASYNC_SUCCESS) + { + printf("Failed to start connect, return code %d\n", rc); + finished = 1; + } +} + + +int msgarrvd(void *context, char *topicName, int topicLen, MQTTAsync_message *message) +{ + int i; + char* payloadptr; + + printf("Message arrived\n"); + printf(" topic: %s\n", topicName); + printf(" message: "); + + payloadptr = message->payload; + for(i=0; ipayloadlen; i++) + { + putchar(*payloadptr++); + } + putchar('\n'); + MQTTAsync_freeMessage(&message); + MQTTAsync_free(topicName); + return 1; +} + + +void onDisconnect(void* context, MQTTAsync_successData* response) +{ + printf("Successful disconnection\n"); + disc_finished = 1; +} + + +void onSubscribe(void* context, MQTTAsync_successData* response) +{ + printf("Subscribe succeeded\n"); + subscribed = 1; +} + +void onSubscribeFailure(void* context, MQTTAsync_failureData* response) +{ + printf("Subscribe failed, rc %d\n", response ? response->code : 0); + finished = 1; +} + + +void onConnectFailure(void* context, MQTTAsync_failureData* response) +{ + printf("Connect failed, rc %d\n", response ? response->code : 0); + finished = 1; +} + + +void onConnect(void* context, MQTTAsync_successData* response) +{ + MQTTAsync client = (MQTTAsync)context; + MQTTAsync_responseOptions opts = MQTTAsync_responseOptions_initializer; + MQTTAsync_message pubmsg = MQTTAsync_message_initializer; + int rc; + + printf("Successful connection\n"); + + printf("Subscribing to topic %s\nfor client %s using QoS%d\n\n" + "Press Q to quit\n\n", TOPIC, CLIENTID, QOS); + opts.onSuccess = onSubscribe; + opts.onFailure = onSubscribeFailure; + opts.context = client; + + deliveredtoken = 0; + + if ((rc = MQTTAsync_subscribe(client, TOPIC, QOS, &opts)) != MQTTASYNC_SUCCESS) + { + printf("Failed to start subscribe, return code %d\n", rc); + exit(EXIT_FAILURE); + } +} + + +int main(int argc, char* argv[]) +{ + MQTTAsync client; + MQTTAsync_connectOptions conn_opts = MQTTAsync_connectOptions_initializer; + MQTTAsync_disconnectOptions disc_opts = MQTTAsync_disconnectOptions_initializer; + MQTTAsync_message pubmsg = MQTTAsync_message_initializer; + MQTTAsync_token token; + int rc; + int ch; + + MQTTAsync_create(&client, ADDRESS, CLIENTID, MQTTCLIENT_PERSISTENCE_NONE, NULL); + + MQTTAsync_setCallbacks(client, NULL, connlost, msgarrvd, NULL); + + conn_opts.keepAliveInterval = 20; + conn_opts.cleansession = 1; + conn_opts.onSuccess = onConnect; + conn_opts.onFailure = onConnectFailure; + conn_opts.context = client; + if ((rc = MQTTAsync_connect(client, &conn_opts)) != MQTTASYNC_SUCCESS) + { + printf("Failed to start connect, return code %d\n", rc); + exit(EXIT_FAILURE); + } + + while (!subscribed) + #if defined(WIN32) || defined(WIN64) + Sleep(100); + #else + usleep(10000L); + #endif + + if (finished) + goto exit; + + do + { + ch = getchar(); + } while (ch!='Q' && ch != 'q'); + + disc_opts.onSuccess = onDisconnect; + if ((rc = MQTTAsync_disconnect(client, &disc_opts)) != MQTTASYNC_SUCCESS) + { + printf("Failed to start disconnect, return code %d\n", rc); + exit(EXIT_FAILURE); + } + while (!disc_finished) + #if defined(WIN32) || defined(WIN64) + Sleep(100); + #else + usleep(10000L); + #endif + +exit: + MQTTAsync_destroy(&client); + return rc; +} + + * @endcode +* @page tracing Tracing + * + * Runtime tracing can be controlled by environment variables or API calls. + * + * #### Environment variables + * + * Tracing is switched on by setting the MQTT_C_CLIENT_TRACE environment variable. + * A value of ON, or stdout, prints to stdout, any other value is interpreted as a file name to use. + * + * The amount of trace detail is controlled with the MQTT_C_CLIENT_TRACE_LEVEL environment + * variable - valid values are ERROR, PROTOCOL, MINIMUM, MEDIUM and MAXIMUM + * (from least to most verbose). + * + * The variable MQTT_C_CLIENT_TRACE_MAX_LINES limits the number of lines of trace that are output + * to a file. Two files are used at most, when they are full, the last one is overwritten with the + * new trace entries. The default size is 1000 lines. + * + * #### Trace API calls + * + * MQTTAsync_traceCallback() is used to set a callback function which is called whenever trace + * information is available. This will be the same information as that printed if the + * environment variables were used to control the trace. + * + * The MQTTAsync_setTraceLevel() calls is used to set the maximum level of trace entries that will be + * passed to the callback function. The levels are: + * 1. ::MQTTASYNC_TRACE_MAXIMUM + * 2. ::MQTTASYNC_TRACE_MEDIUM + * 3. ::MQTTASYNC_TRACE_MINIMUM + * 4. ::MQTTASYNC_TRACE_PROTOCOL + * 5. ::MQTTASYNC_TRACE_ERROR + * 6. ::MQTTASYNC_TRACE_SEVERE + * 7. ::MQTTASYNC_TRACE_FATAL + * + * Selecting ::MQTTASYNC_TRACE_MAXIMUM will cause all trace entries at all levels to be returned. + * Choosing ::MQTTASYNC_TRACE_ERROR will cause ERROR, SEVERE and FATAL trace entries to be returned + * to the callback function. + * + * ### MQTT Packet Tracing + * + * A feature that can be very useful is printing the MQTT packets that are sent and received. To + * achieve this, use the following environment variable settings: + * @code + MQTT_C_CLIENT_TRACE=ON + MQTT_C_CLIENT_TRACE_LEVEL=PROTOCOL + * @endcode + * The output you should see looks like this: + * @code + 20130528 155936.813 3 stdout-subscriber -> CONNECT cleansession: 1 (0) + 20130528 155936.813 3 stdout-subscriber <- CONNACK rc: 0 + 20130528 155936.813 3 stdout-subscriber -> SUBSCRIBE msgid: 1 (0) + 20130528 155936.813 3 stdout-subscriber <- SUBACK msgid: 1 + 20130528 155941.818 3 stdout-subscriber -> DISCONNECT (0) + * @endcode + * where the fields are: + * 1. date + * 2. time + * 3. socket number + * 4. client id + * 5. direction (-> from client to server, <- from server to client) + * 6. packet details + * + * ### Default Level Tracing + * + * This is an extract of a default level trace of a call to connect: + * @code + 19700101 010000.000 (1152206656) (0)> MQTTClient_connect:893 + 19700101 010000.000 (1152206656) (1)> MQTTClient_connectURI:716 + 20130528 160447.479 Connecting to serverURI localhost:1883 + 20130528 160447.479 (1152206656) (2)> MQTTProtocol_connect:98 + 20130528 160447.479 (1152206656) (3)> MQTTProtocol_addressPort:48 + 20130528 160447.479 (1152206656) (3)< MQTTProtocol_addressPort:73 + 20130528 160447.479 (1152206656) (3)> Socket_new:599 + 20130528 160447.479 New socket 4 for localhost, port 1883 + 20130528 160447.479 (1152206656) (4)> Socket_addSocket:163 + 20130528 160447.479 (1152206656) (5)> Socket_setnonblocking:73 + 20130528 160447.479 (1152206656) (5)< Socket_setnonblocking:78 (0) + 20130528 160447.479 (1152206656) (4)< Socket_addSocket:176 (0) + 20130528 160447.479 (1152206656) (4)> Socket_error:95 + 20130528 160447.479 (1152206656) (4)< Socket_error:104 (115) + 20130528 160447.479 Connect pending + 20130528 160447.479 (1152206656) (3)< Socket_new:683 (115) + 20130528 160447.479 (1152206656) (2)< MQTTProtocol_connect:131 (115) + * @endcode + * where the fields are: + * 1. date + * 2. time + * 3. thread id + * 4. function nesting level + * 5. function entry (>) or exit (<) + * 6. function name : line of source code file + * 7. return value (if there is one) + * + * ### Memory Allocation Tracing + * + * Setting the trace level to maximum causes memory allocations and frees to be traced along with + * the default trace entries, with messages like the following: + * @code + 20130528 161819.657 Allocating 16 bytes in heap at file /home/icraggs/workspaces/mqrtc/mqttv3c/src/MQTTPacket.c line 177 ptr 0x179f930 + + 20130528 161819.657 Freeing 16 bytes in heap at file /home/icraggs/workspaces/mqrtc/mqttv3c/src/MQTTPacket.c line 201, heap use now 896 bytes + * @endcode + * When the last MQTT client object is destroyed, if the trace is being recorded + * and all memory allocated by the client library has not been freed, an error message will be + * written to the trace. This can help with fixing memory leaks. The message will look like this: + * @code + 20130528 163909.208 Some memory not freed at shutdown, possible memory leak + 20130528 163909.208 Heap scan start, total 880 bytes + 20130528 163909.208 Heap element size 32, line 354, file /home/icraggs/workspaces/mqrtc/mqttv3c/src/MQTTPacket.c, ptr 0x260cb00 + 20130528 163909.208 Content + 20130528 163909.209 Heap scan end + * @endcode + * @endcond + */ + +#ifdef __cplusplus + } +#endif + +#endif diff --git a/src/task_manager/include/paho_mqtt_3c/MQTTClient.h b/src/task_manager/include/paho_mqtt_3c/MQTTClient.h new file mode 100644 index 0000000..518dbbd --- /dev/null +++ b/src/task_manager/include/paho_mqtt_3c/MQTTClient.h @@ -0,0 +1,1670 @@ +/******************************************************************************* + * Copyright (c) 2009, 2018 IBM Corp. + * + * All rights reserved. This program and the accompanying materials + * are made available under the terms of the Eclipse Public License v1.0 + * and Eclipse Distribution License v1.0 which accompany this distribution. + * + * The Eclipse Public License is available at + * http://www.eclipse.org/legal/epl-v10.html + * and the Eclipse Distribution License is available at + * http://www.eclipse.org/org/documents/edl-v10.php. + * + * Contributors: + * Ian Craggs - initial API and implementation and/or initial documentation + * Ian Craggs, Allan Stockdill-Mander - SSL updates + * Ian Craggs - multiple server connection support + * Ian Craggs - MQTT 3.1.1 support + * Ian Craggs - remove const from eyecatchers #168 + *******************************************************************************/ + +/** + * @cond MQTTClient_internal + * @mainpage MQTT Client Library Internals + * In the beginning there was one MQTT C client library, MQTTClient, as implemented in MQTTClient.c + * This library was designed to be easy to use for applications which didn't mind if some of the calls + * blocked for a while. For instance, the MQTTClient_connect call will block until a successful + * connection has completed, or a connection has failed, which could be as long as the "connection + * timeout" interval, whose default is 30 seconds. + * + * However in mobile devices and other windowing environments, blocking on the GUI thread is a bad + * thing as it causes the user interface to freeze. Hence a new API, MQTTAsync, implemented + * in MQTTAsync.c, was devised. There are no blocking calls in this library, so it is well suited + * to GUI and mobile environments, at the expense of some extra complexity. + * + * Both libraries are designed to be sparing in the use of threads. So multiple client objects are + * handled by one or two threads, with a select call in Socket_getReadySocket(), used to determine + * when a socket has incoming data. This API is thread safe: functions may be called by multiple application + * threads, with the exception of ::MQTTClient_yield and ::MQTTClient_receive, which are intended + * for single threaded environments only. + * + * @endcond + * @cond MQTTClient_main + * @mainpage MQTT Client library for C + * © Copyright IBM Corp. 2009, 2018 + * + * @brief An MQTT client library in C. + * + * These pages describe the original more synchronous API which might be + * considered easier to use. Some of the calls will block. For the new + * totally asynchronous API where no calls block, which is especially suitable + * for use in windowed environments, see the + * MQTT C Client Asynchronous API Documentation. + * The MQTTClient API is not thread safe, whereas the MQTTAsync API is. + * + * An MQTT client application connects to MQTT-capable servers. + * A typical client is responsible for collecting information from a telemetry + * device and publishing the information to the server. It can also subscribe + * to topics, receive messages, and use this information to control the + * telemetry device. + * + * MQTT clients implement the published MQTT v3 protocol. You can write your own + * API to the MQTT protocol using the programming language and platform of your + * choice. This can be time-consuming and error-prone. + * + * To simplify writing MQTT client applications, this library encapsulates + * the MQTT v3 protocol for you. Using this library enables a fully functional + * MQTT client application to be written in a few lines of code. + * The information presented here documents the API provided + * by the MQTT Client library for C. + * + * Using the client
+ * Applications that use the client library typically use a similar structure: + *
    + *
  • Create a client object
    • + *
  • Set the options to connect to an MQTT server
    • + *
  • Set up callback functions if multi-threaded (asynchronous mode) + * operation is being used (see @ref async).
    • + *
  • Subscribe to any topics the client needs to receive
    • + *
  • Repeat until finished:
    • + *
      + *
    • Publish any messages the client needs to
      • + *
    • Handle any incoming messages
      • + *
    + *
  • Disconnect the client
    • + *
  • Free any memory being used by the client
    • + *
+ * Some simple examples are shown here: + *
    + *
  • @ref pubsync
    • + *
  • @ref pubasync
    • + *
  • @ref subasync
    • + *
+ * Additional information about important concepts is provided here: + *
    + *
  • @ref async
    • + *
  • @ref wildcard
    • + *
  • @ref qos
    • + *
  • @ref tracing
    • + *
+ * @endcond + */ + +/* +/// @cond EXCLUDE +*/ +#if !defined(MQTTCLIENT_H) +#define MQTTCLIENT_H + +#if defined(__cplusplus) + extern "C" { +#endif + +#if defined(WIN32) || defined(WIN64) + #define DLLImport __declspec(dllimport) + #define DLLExport __declspec(dllexport) +#else + #define DLLImport extern + #define DLLExport __attribute__ ((visibility ("default"))) +#endif + +#include +/* +/// @endcond +*/ + +#include "MQTTProperties.h" +#include "MQTTReasonCodes.h" +#include "MQTTSubscribeOpts.h" +#if !defined(NO_PERSISTENCE) +#include "MQTTClientPersistence.h" +#endif + +/** + * Return code: No error. Indicates successful completion of an MQTT client + * operation. + */ +#define MQTTCLIENT_SUCCESS 0 +/** + * Return code: A generic error code indicating the failure of an MQTT client + * operation. + */ +#define MQTTCLIENT_FAILURE -1 + +/* error code -2 is MQTTCLIENT_PERSISTENCE_ERROR */ + +/** + * Return code: The client is disconnected. + */ +#define MQTTCLIENT_DISCONNECTED -3 +/** + * Return code: The maximum number of messages allowed to be simultaneously + * in-flight has been reached. + */ +#define MQTTCLIENT_MAX_MESSAGES_INFLIGHT -4 +/** + * Return code: An invalid UTF-8 string has been detected. + */ +#define MQTTCLIENT_BAD_UTF8_STRING -5 +/** + * Return code: A NULL parameter has been supplied when this is invalid. + */ +#define MQTTCLIENT_NULL_PARAMETER -6 +/** + * Return code: The topic has been truncated (the topic string includes + * embedded NULL characters). String functions will not access the full topic. + * Use the topic length value to access the full topic. + */ +#define MQTTCLIENT_TOPICNAME_TRUNCATED -7 +/** + * Return code: A structure parameter does not have the correct eyecatcher + * and version number. + */ +#define MQTTCLIENT_BAD_STRUCTURE -8 +/** + * Return code: A QoS value that falls outside of the acceptable range (0,1,2) + */ +#define MQTTCLIENT_BAD_QOS -9 +/** + * Return code: Attempting SSL connection using non-SSL version of library + */ +#define MQTTCLIENT_SSL_NOT_SUPPORTED -10 + /** + * Return code: unrecognized MQTT version + */ + #define MQTTCLIENT_BAD_MQTT_VERSION -11 +/** + * Return code: protocol prefix in serverURI should be tcp:// or ssl:// + */ +#define MQTTCLIENT_BAD_PROTOCOL -14 + /** + * Return code: option not applicable to the requested version of MQTT + */ + #define MQTTCLIENT_BAD_MQTT_OPTION -15 + /** + * Return code: call not applicable to the requested version of MQTT + */ + #define MQTTCLIENT_WRONG_MQTT_VERSION -16 + + +/** + * Default MQTT version to connect with. Use 3.1.1 then fall back to 3.1 + */ +#define MQTTVERSION_DEFAULT 0 +/** + * MQTT version to connect with: 3.1 + */ +#define MQTTVERSION_3_1 3 +/** + * MQTT version to connect with: 3.1.1 + */ +#define MQTTVERSION_3_1_1 4 + /** + * MQTT version to connect with: 5 + */ + #define MQTTVERSION_5 5 +/** + * Bad return code from subscribe, as defined in the 3.1.1 specification + */ +#define MQTT_BAD_SUBSCRIBE 0x80 + +/** + * Initialization options + */ +typedef struct +{ + /** The eyecatcher for this structure. Must be MQTG. */ + char struct_id[4]; + /** The version number of this structure. Must be 0 */ + int struct_version; + /** 1 = we do openssl init, 0 = leave it to the application */ + int do_openssl_init; +} MQTTClient_init_options; + +#define MQTTClient_init_options_initializer { {'M', 'Q', 'T', 'G'}, 0, 0 } + +/** + * Global init of mqtt library. Call once on program start to set global behaviour. + * do_openssl_init - if mqtt library should initialize OpenSSL (1) or rely on the caller to do it before using the library (0) + */ +DLLExport void MQTTClient_global_init(MQTTClient_init_options* inits); + +/** + * A handle representing an MQTT client. A valid client handle is available + * following a successful call to MQTTClient_create(). + */ +typedef void* MQTTClient; +/** + * A value representing an MQTT message. A delivery token is returned to the + * client application when a message is published. The token can then be used to + * check that the message was successfully delivered to its destination (see + * MQTTClient_publish(), + * MQTTClient_publishMessage(), + * MQTTClient_deliveryComplete(), + * MQTTClient_waitForCompletion() and + * MQTTClient_getPendingDeliveryTokens()). + */ +typedef int MQTTClient_deliveryToken; +typedef int MQTTClient_token; + +/** + * A structure representing the payload and attributes of an MQTT message. The + * message topic is not part of this structure (see MQTTClient_publishMessage(), + * MQTTClient_publish(), MQTTClient_receive(), MQTTClient_freeMessage() + * and MQTTClient_messageArrived()). + */ +typedef struct +{ + /** The eyecatcher for this structure. must be MQTM. */ + char struct_id[4]; + /** The version number of this structure. Must be 0 or 1 + * 0 indicates no message properties */ + int struct_version; + /** The length of the MQTT message payload in bytes. */ + int payloadlen; + /** A pointer to the payload of the MQTT message. */ + void* payload; + /** + * The quality of service (QoS) assigned to the message. + * There are three levels of QoS: + *
+ *
QoS0
+ *
Fire and forget - the message may not be delivered
+ *
QoS1
+ *
At least once - the message will be delivered, but may be + * delivered more than once in some circumstances.
+ *
QoS2
+ *
Once and one only - the message will be delivered exactly once.
+ *
+ */ + int qos; + /** + * The retained flag serves two purposes depending on whether the message + * it is associated with is being published or received. + * + * retained = true
+ * For messages being published, a true setting indicates that the MQTT + * server should retain a copy of the message. The message will then be + * transmitted to new subscribers to a topic that matches the message topic. + * For subscribers registering a new subscription, the flag being true + * indicates that the received message is not a new one, but one that has + * been retained by the MQTT server. + * + * retained = false
+ * For publishers, this indicates that this message should not be retained + * by the MQTT server. For subscribers, a false setting indicates this is + * a normal message, received as a result of it being published to the + * server. + */ + int retained; + /** + * The dup flag indicates whether or not this message is a duplicate. + * It is only meaningful when receiving QoS1 messages. When true, the + * client application should take appropriate action to deal with the + * duplicate message. + */ + int dup; + /** The message identifier is normally reserved for internal use by the + * MQTT client and server. + */ + int msgid; + /** + * The MQTT V5 properties associated with the message. + */ + MQTTProperties properties; +} MQTTClient_message; + +#define MQTTClient_message_initializer { {'M', 'Q', 'T', 'M'}, 1, 0, NULL, 0, 0, 0, 0, MQTTProperties_initializer } + +/** + * This is a callback function. The client application + * must provide an implementation of this function to enable asynchronous + * receipt of messages. The function is registered with the client library by + * passing it as an argument to MQTTClient_setCallbacks(). It is + * called by the client library when a new message that matches a client + * subscription has been received from the server. This function is executed on + * a separate thread to the one on which the client application is running. + * @param context A pointer to the context value originally passed to + * MQTTClient_setCallbacks(), which contains any application-specific context. + * @param topicName The topic associated with the received message. + * @param topicLen The length of the topic if there are one + * more NULL characters embedded in topicName, otherwise topicLen + * is 0. If topicLen is 0, the value returned by strlen(topicName) + * can be trusted. If topicLen is greater than 0, the full topic name + * can be retrieved by accessing topicName as a byte array of length + * topicLen. + * @param message The MQTTClient_message structure for the received message. + * This structure contains the message payload and attributes. + * @return This function must return a boolean value indicating whether or not + * the message has been safely received by the client application. Returning + * true indicates that the message has been successfully handled. + * Returning false indicates that there was a problem. In this + * case, the client library will reinvoke MQTTClient_messageArrived() to + * attempt to deliver the message to the application again. + */ +typedef int MQTTClient_messageArrived(void* context, char* topicName, int topicLen, MQTTClient_message* message); + +/** + * This is a callback function. The client application + * must provide an implementation of this function to enable asynchronous + * notification of delivery of messages. The function is registered with the + * client library by passing it as an argument to MQTTClient_setCallbacks(). + * It is called by the client library after the client application has + * published a message to the server. It indicates that the necessary + * handshaking and acknowledgements for the requested quality of service (see + * MQTTClient_message.qos) have been completed. This function is executed on a + * separate thread to the one on which the client application is running. + * Note:MQTTClient_deliveryComplete() is not called when messages are + * published at QoS0. + * @param context A pointer to the context value originally passed to + * MQTTClient_setCallbacks(), which contains any application-specific context. + * @param dt The ::MQTTClient_deliveryToken associated with + * the published message. Applications can check that all messages have been + * correctly published by matching the delivery tokens returned from calls to + * MQTTClient_publish() and MQTTClient_publishMessage() with the tokens passed + * to this callback. + */ +typedef void MQTTClient_deliveryComplete(void* context, MQTTClient_deliveryToken dt); + +/** + * This is a callback function. The client application + * must provide an implementation of this function to enable asynchronous + * notification of the loss of connection to the server. The function is + * registered with the client library by passing it as an argument to + * MQTTClient_setCallbacks(). It is called by the client library if the client + * loses its connection to the server. The client application must take + * appropriate action, such as trying to reconnect or reporting the problem. + * This function is executed on a separate thread to the one on which the + * client application is running. + * @param context A pointer to the context value originally passed to + * MQTTClient_setCallbacks(), which contains any application-specific context. + * @param cause The reason for the disconnection. + * Currently, cause is always set to NULL. + */ +typedef void MQTTClient_connectionLost(void* context, char* cause); + +/** + * This function sets the callback functions for a specific client. + * If your client application doesn't use a particular callback, set the + * relevant parameter to NULL. Calling MQTTClient_setCallbacks() puts the + * client into multi-threaded mode. Any necessary message acknowledgements and + * status communications are handled in the background without any intervention + * from the client application. See @ref async for more information. + * + * Note: The MQTT client must be disconnected when this function is + * called. + * @param handle A valid client handle from a successful call to + * MQTTClient_create(). + * @param context A pointer to any application-specific context. The + * the context pointer is passed to each of the callback functions to + * provide access to the context information in the callback. + * @param cl A pointer to an MQTTClient_connectionLost() callback + * function. You can set this to NULL if your application doesn't handle + * disconnections. + * @param ma A pointer to an MQTTClient_messageArrived() callback + * function. This callback function must be specified when you call + * MQTTClient_setCallbacks(). + * @param dc A pointer to an MQTTClient_deliveryComplete() callback + * function. You can set this to NULL if your application publishes + * synchronously or if you do not want to check for successful delivery. + * @return ::MQTTCLIENT_SUCCESS if the callbacks were correctly set, + * ::MQTTCLIENT_FAILURE if an error occurred. + */ +DLLExport int MQTTClient_setCallbacks(MQTTClient handle, void* context, MQTTClient_connectionLost* cl, + MQTTClient_messageArrived* ma, MQTTClient_deliveryComplete* dc); + + +/** + * This is a callback function, which will be called when the a disconnect + * packet is received from the server. This applies to MQTT V5 and above only. + * @param context A pointer to the context value originally passed to + * ::MQTTAsync_setDisconnected(), which contains any application-specific context. + * @param properties The MQTT V5 properties received with the disconnect, if any. + * @param reasonCode The MQTT V5 reason code received with the disconnect. + * Currently, cause is always set to NULL. + */ +typedef void MQTTClient_disconnected(void* context, MQTTProperties* properties, + enum MQTTReasonCodes reasonCode); + +/** + * Sets the MQTTClient_disconnected() callback function for a client. This will be called + * if a disconnect packet is received from the server. Only valid for MQTT V5 and above. + * @param handle A valid client handle from a successful call to + * MQTTClient_create(). + * @param context A pointer to any application-specific context. The + * the context pointer is passed to each of the callback functions to + * provide access to the context information in the callback. + * @param co A pointer to an MQTTClient_disconnected() callback + * function. NULL removes the callback setting. + * @return ::MQTTCLIENT_SUCCESS if the callbacks were correctly set, + * ::MQTTCLIENT_FAILURE if an error occurred. + */ +DLLExport int MQTTClient_setDisconnected(MQTTClient handle, void* context, MQTTClient_disconnected* co); + +/** + * This is a callback function, the MQTT V5 version of MQTTClient_deliveryComplete(). + * The client application + * must provide an implementation of this function to enable asynchronous + * notification of the completed delivery of messages. + * It is called by the client library after the client application has + * published a message to the server. It indicates that the necessary + * handshaking and acknowledgements for the requested quality of service (see + * MQTTClient_message.qos) have been completed. This function is executed on a + * separate thread to the one on which the client application is running. + * Note: It is not called when messages are published at QoS0. + * @param context A pointer to the context value originally passed to + * MQTTClient_setCallbacks(), which contains any application-specific context. + * @param dt The ::MQTTClient_deliveryToken associated with + * the published message. Applications can check that all messages have been + * correctly published by matching the delivery tokens returned from calls to + * MQTTClient_publish() and MQTTClient_publishMessage() with the tokens passed + * to this callback. + * @param packet_type the last received packet type for this completion. For QoS 1 + * always PUBACK. For QoS 2 could be PUBREC or PUBCOMP. + * @param properties the MQTT V5 properties returned with the last packet from the server + * @param reasonCode the reason code returned from the server + */ +typedef void MQTTClient_published(void* context, int dt, int packet_type, MQTTProperties* properties, + enum MQTTReasonCodes reasonCode); + +DLLExport int MQTTClient_setPublished(MQTTClient handle, void* context, MQTTClient_published* co); + +/** + * This function creates an MQTT client ready for connection to the + * specified server and using the specified persistent storage (see + * MQTTClient_persistence). See also MQTTClient_destroy(). + * @param handle A pointer to an ::MQTTClient handle. The handle is + * populated with a valid client reference following a successful return from + * this function. + * @param serverURI A null-terminated string specifying the server to + * which the client will connect. It takes the form protocol://host:port. + * Currently, protocol must be tcp or ssl. + * For host, you can + * specify either an IP address or a host name. For instance, to connect to + * a server running on the local machines with the default MQTT port, specify + * tcp://localhost:1883. + * @param clientId The client identifier passed to the server when the + * client connects to it. It is a null-terminated UTF-8 encoded string. + * @param persistence_type The type of persistence to be used by the client: + *
+ * ::MQTTCLIENT_PERSISTENCE_NONE: Use in-memory persistence. If the device or + * system on which the client is running fails or is switched off, the current + * state of any in-flight messages is lost and some messages may not be + * delivered even at QoS1 and QoS2. + *
+ * ::MQTTCLIENT_PERSISTENCE_DEFAULT: Use the default (file system-based) + * persistence mechanism. Status about in-flight messages is held in persistent + * storage and provides some protection against message loss in the case of + * unexpected failure. + *
+ * ::MQTTCLIENT_PERSISTENCE_USER: Use an application-specific persistence + * implementation. Using this type of persistence gives control of the + * persistence mechanism to the application. The application has to implement + * the MQTTClient_persistence interface. + * @param persistence_context If the application uses + * ::MQTTCLIENT_PERSISTENCE_NONE persistence, this argument is unused and should + * be set to NULL. For ::MQTTCLIENT_PERSISTENCE_DEFAULT persistence, it + * should be set to the location of the persistence directory (if set + * to NULL, the persistence directory used is the working directory). + * Applications that use ::MQTTCLIENT_PERSISTENCE_USER persistence set this + * argument to point to a valid MQTTClient_persistence structure. + * @return ::MQTTCLIENT_SUCCESS if the client is successfully created, otherwise + * an error code is returned. + */ +DLLExport int MQTTClient_create(MQTTClient* handle, const char* serverURI, const char* clientId, + int persistence_type, void* persistence_context); + +typedef struct +{ + /** The eyecatcher for this structure. must be MQCO. */ + char struct_id[4]; + /** The version number of this structure. Must be 0 */ + int struct_version; + /** Whether the MQTT version is 3.1, 3.1.1, or 5. To use V5, this must be set. + * MQTT V5 has to be chosen here, because during the create call the message persistence + * is initialized, and we want to know whether the format of any persisted messages + * is appropriate for the MQTT version we are going to connect with. Selecting 3.1 or + * 3.1.1 and attempting to read 5.0 persisted messages will result in an error on create. */ + int MQTTVersion; +} MQTTClient_createOptions; + +#define MQTTClient_createOptions_initializer { {'M', 'Q', 'C', 'O'}, MQTTVERSION_DEFAULT } + +/** + * A version of :MQTTClient_create() with additional options. + * This function creates an MQTT client ready for connection to the + * specified server and using the specified persistent storage (see + * MQTTClient_persistence). See also MQTTClient_destroy(). + * @param handle A pointer to an ::MQTTClient handle. The handle is + * populated with a valid client reference following a successful return from + * this function. + * @param serverURI A null-terminated string specifying the server to + * which the client will connect. It takes the form protocol://host:port. + * Currently, protocol must be tcp or ssl. + * For host, you can + * specify either an IP address or a host name. For instance, to connect to + * a server running on the local machines with the default MQTT port, specify + * tcp://localhost:1883. + * @param clientId The client identifier passed to the server when the + * client connects to it. It is a null-terminated UTF-8 encoded string. + * @param persistence_type The type of persistence to be used by the client: + *
+ * ::MQTTCLIENT_PERSISTENCE_NONE: Use in-memory persistence. If the device or + * system on which the client is running fails or is switched off, the current + * state of any in-flight messages is lost and some messages may not be + * delivered even at QoS1 and QoS2. + *
+ * ::MQTTCLIENT_PERSISTENCE_DEFAULT: Use the default (file system-based) + * persistence mechanism. Status about in-flight messages is held in persistent + * storage and provides some protection against message loss in the case of + * unexpected failure. + *
+ * ::MQTTCLIENT_PERSISTENCE_USER: Use an application-specific persistence + * implementation. Using this type of persistence gives control of the + * persistence mechanism to the application. The application has to implement + * the MQTTClient_persistence interface. + * @param persistence_context If the application uses + * ::MQTTCLIENT_PERSISTENCE_NONE persistence, this argument is unused and should + * be set to NULL. For ::MQTTCLIENT_PERSISTENCE_DEFAULT persistence, it + * should be set to the location of the persistence directory (if set + * to NULL, the persistence directory used is the working directory). + * Applications that use ::MQTTCLIENT_PERSISTENCE_USER persistence set this + * argument to point to a valid MQTTClient_persistence structure. + * @param options additional options for the create. + * @return ::MQTTCLIENT_SUCCESS if the client is successfully created, otherwise + * an error code is returned. + */ +DLLExport int MQTTClient_createWithOptions(MQTTClient* handle, const char* serverURI, const char* clientId, + int persistence_type, void* persistence_context, MQTTClient_createOptions* options); + +/** + * MQTTClient_willOptions defines the MQTT "Last Will and Testament" (LWT) settings for + * the client. In the event that a client unexpectedly loses its connection to + * the server, the server publishes the LWT message to the LWT topic on + * behalf of the client. This allows other clients (subscribed to the LWT topic) + * to be made aware that the client has disconnected. To enable the LWT + * function for a specific client, a valid pointer to an MQTTClient_willOptions + * structure is passed in the MQTTClient_connectOptions structure used in the + * MQTTClient_connect() call that connects the client to the server. The pointer + * to MQTTClient_willOptions can be set to NULL if the LWT function is not + * required. + */ +typedef struct +{ + /** The eyecatcher for this structure. must be MQTW. */ + char struct_id[4]; + /** The version number of this structure. Must be 0 or 1 + 0 means there is no binary payload option + */ + int struct_version; + /** The LWT topic to which the LWT message will be published. */ + const char* topicName; + /** The LWT payload in string form. */ + const char* message; + /** + * The retained flag for the LWT message (see MQTTClient_message.retained). + */ + int retained; + /** + * The quality of service setting for the LWT message (see + * MQTTClient_message.qos and @ref qos). + */ + int qos; + /** The LWT payload in binary form. This is only checked and used if the message option is NULL */ + struct + { + int len; /**< binary payload length */ + const void* data; /**< binary payload data */ + } payload; +} MQTTClient_willOptions; + +#define MQTTClient_willOptions_initializer { {'M', 'Q', 'T', 'W'}, 1, NULL, NULL, 0, 0, {0, NULL} } + +#define MQTT_SSL_VERSION_DEFAULT 0 +#define MQTT_SSL_VERSION_TLS_1_0 1 +#define MQTT_SSL_VERSION_TLS_1_1 2 +#define MQTT_SSL_VERSION_TLS_1_2 3 + +/** +* MQTTClient_sslProperties defines the settings to establish an SSL/TLS connection using the +* OpenSSL library. It covers the following scenarios: +* - Server authentication: The client needs the digital certificate of the server. It is included +* in a store containting trusted material (also known as "trust store"). +* - Mutual authentication: Both client and server are authenticated during the SSL handshake. In +* addition to the digital certificate of the server in a trust store, the client will need its own +* digital certificate and the private key used to sign its digital certificate stored in a "key store". +* - Anonymous connection: Both client and server do not get authenticated and no credentials are needed +* to establish an SSL connection. Note that this scenario is not fully secure since it is subject to +* man-in-the-middle attacks. +*/ +typedef struct +{ + /** The eyecatcher for this structure. Must be MQTS */ + char struct_id[4]; + /** The version number of this structure. Must be 0, or 1 to enable TLS version selection. */ + int struct_version; + + /** The file in PEM format containing the public digital certificates trusted by the client. */ + const char* trustStore; + + /** The file in PEM format containing the public certificate chain of the client. It may also include + * the client's private key. + */ + const char* keyStore; + + /** If not included in the sslKeyStore, this setting points to the file in PEM format containing + * the client's private key. + */ + const char* privateKey; + /** The password to load the client's privateKey if encrypted. */ + const char* privateKeyPassword; + + /** + * The list of cipher suites that the client will present to the server during the SSL handshake. For a + * full explanation of the cipher list format, please see the OpenSSL on-line documentation: + * http://www.openssl.org/docs/apps/ciphers.html#CIPHER_LIST_FORMAT + * If this setting is ommitted, its default value will be "ALL", that is, all the cipher suites -excluding + * those offering no encryption- will be considered. + * This setting can be used to set an SSL anonymous connection ("aNULL" string value, for instance). + */ + const char* enabledCipherSuites; + + /** True/False option to enable verification of the server certificate **/ + int enableServerCertAuth; + + /** The SSL/TLS version to use. Specify one of MQTT_SSL_VERSION_DEFAULT (0), + * MQTT_SSL_VERSION_TLS_1_0 (1), MQTT_SSL_VERSION_TLS_1_1 (2) or MQTT_SSL_VERSION_TLS_1_2 (3). + * Only used if struct_version is >= 1. + */ + int sslVersion; + + /** + * Whether to carry out post-connect checks, including that a certificate + * matches the given host name. + * Exists only if struct_version >= 2 + */ + int verify; + + /** + * From the OpenSSL documentation: + * If CApath is not NULL, it points to a directory containing CA certificates in PEM format. + * Exists only if struct_version >= 2 + */ + const char* CApath; + + /** + * Callback function for OpenSSL error handler ERR_print_errors_cb + * Exists only if struct_version >= 3 + */ + int (*ssl_error_cb) (const char *str, size_t len, void *u); + + /** + * Application-specific contex for OpenSSL error handler ERR_print_errors_cb + * Exists only if struct_version >= 3 + */ + void* ssl_error_context; + +} MQTTClient_SSLOptions; + +#define MQTTClient_SSLOptions_initializer { {'M', 'Q', 'T', 'S'}, 3, NULL, NULL, NULL, NULL, NULL, 1, MQTT_SSL_VERSION_DEFAULT, 0, NULL, NULL, NULL } + +/** + * MQTTClient_connectOptions defines several settings that control the way the + * client connects to an MQTT server. + * + * Note: Default values are not defined for members of + * MQTTClient_connectOptions so it is good practice to specify all settings. + * If the MQTTClient_connectOptions structure is defined as an automatic + * variable, all members are set to random values and thus must be set by the + * client application. If the MQTTClient_connectOptions structure is defined + * as a static variable, initialization (in compliant compilers) sets all + * values to 0 (NULL for pointers). A #keepAliveInterval setting of 0 prevents + * correct operation of the client and so you must at least set a value + * for #keepAliveInterval. + */ +typedef struct +{ + /** The eyecatcher for this structure. must be MQTC. */ + char struct_id[4]; + /** The version number of this structure. Must be 0, 1, 2, 3, 4, 5 or 6. + * 0 signifies no SSL options and no serverURIs + * 1 signifies no serverURIs + * 2 signifies no MQTTVersion + * 3 signifies no returned values + * 4 signifies no binary password option + * 5 signifies no maxInflightMessages and cleanstart + */ + int struct_version; + /** The "keep alive" interval, measured in seconds, defines the maximum time + * that should pass without communication between the client and the server + * The client will ensure that at least one message travels across the + * network within each keep alive period. In the absence of a data-related + * message during the time period, the client sends a very small MQTT + * "ping" message, which the server will acknowledge. The keep alive + * interval enables the client to detect when the server is no longer + * available without having to wait for the long TCP/IP timeout. + */ + int keepAliveInterval; + /** + * This is a boolean value. The cleansession setting controls the behaviour + * of both the client and the server at connection and disconnection time. + * The client and server both maintain session state information. This + * information is used to ensure "at least once" and "exactly once" + * delivery, and "exactly once" receipt of messages. Session state also + * includes subscriptions created by an MQTT client. You can choose to + * maintain or discard state information between sessions. + * + * When cleansession is true, the state information is discarded at + * connect and disconnect. Setting cleansession to false keeps the state + * information. When you connect an MQTT client application with + * MQTTClient_connect(), the client identifies the connection using the + * client identifier and the address of the server. The server checks + * whether session information for this client + * has been saved from a previous connection to the server. If a previous + * session still exists, and cleansession=true, then the previous session + * information at the client and server is cleared. If cleansession=false, + * the previous session is resumed. If no previous session exists, a new + * session is started. + */ + int cleansession; + /** + * This is a boolean value that controls how many messages can be in-flight + * simultaneously. Setting reliable to true means that a published + * message must be completed (acknowledgements received) before another + * can be sent. Attempts to publish additional messages receive an + * ::MQTTCLIENT_MAX_MESSAGES_INFLIGHT return code. Setting this flag to + * false allows up to 10 messages to be in-flight. This can increase + * overall throughput in some circumstances. + */ + int reliable; + /** + * This is a pointer to an MQTTClient_willOptions structure. If your + * application does not make use of the Last Will and Testament feature, + * set this pointer to NULL. + */ + MQTTClient_willOptions* will; + /** + * MQTT servers that support the MQTT v3.1.1 protocol provide authentication + * and authorisation by user name and password. This is the user name + * parameter. + */ + const char* username; + /** + * MQTT servers that support the MQTT v3.1.1 protocol provide authentication + * and authorisation by user name and password. This is the password + * parameter. + */ + const char* password; + /** + * The time interval in seconds to allow a connect to complete. + */ + int connectTimeout; + /** + * The time interval in seconds after which unacknowledged publish requests are + * retried during a TCP session. With MQTT 3.1.1 and later, retries are + * not required except on reconnect. 0 turns off in-session retries, and is the + * recommended setting. Adding retries to an already overloaded network only + * exacerbates the problem. + */ + int retryInterval; + /** + * This is a pointer to an MQTTClient_SSLOptions structure. If your + * application does not make use of SSL, set this pointer to NULL. + */ + MQTTClient_SSLOptions* ssl; + /** + * The number of entries in the optional serverURIs array. Defaults to 0. + */ + int serverURIcount; + /** + * An optional array of null-terminated strings specifying the servers to + * which the client will connect. Each string takes the form protocol://host:port. + * protocol must be tcp or ssl. For host, you can + * specify either an IP address or a host name. For instance, to connect to + * a server running on the local machines with the default MQTT port, specify + * tcp://localhost:1883. + * If this list is empty (the default), the server URI specified on MQTTClient_create() + * is used. + */ + char* const* serverURIs; + /** + * Sets the version of MQTT to be used on the connect. + * MQTTVERSION_DEFAULT (0) = default: start with 3.1.1, and if that fails, fall back to 3.1 + * MQTTVERSION_3_1 (3) = only try version 3.1 + * MQTTVERSION_3_1_1 (4) = only try version 3.1.1 + * MQTTVERSION_5 (5) = only try version 5.0 + */ + int MQTTVersion; + /** + * Returned from the connect when the MQTT version used to connect is 3.1.1 + */ + struct + { + const char* serverURI; /**< the serverURI connected to */ + int MQTTVersion; /**< the MQTT version used to connect with */ + int sessionPresent; /**< if the MQTT version is 3.1.1, the value of sessionPresent returned in the connack */ + } returned; + /** + * Optional binary password. Only checked and used if the password option is NULL + */ + struct + { + int len; /**< binary password length */ + const void* data; /**< binary password data */ + } binarypwd; + /** + * The maximum number of messages in flight + */ + int maxInflightMessages; + /* + * MQTT V5 clean start flag. Only clears state at the beginning of the session. + */ + int cleanstart; +} MQTTClient_connectOptions; + +#define MQTTClient_connectOptions_initializer { {'M', 'Q', 'T', 'C'}, 6, 60, 1, 1, NULL, NULL, NULL, 30, 0, NULL, 0, NULL, MQTTVERSION_DEFAULT, {NULL, 0, 0}, {0, NULL}, -1, 0} + +#define MQTTClient_connectOptions_initializer5 { {'M', 'Q', 'T', 'C'}, 6, 60, 0, 1, NULL, NULL, NULL, 30, 0, NULL, 0, NULL, MQTTVERSION_5, {NULL, 0, 0}, {0, NULL}, -1, 1} + +/** + * MQTTClient_libraryInfo is used to store details relating to the currently used + * library such as the version in use, the time it was built and relevant openSSL + * options. + * There is one static instance of this struct in MQTTClient.c + */ + +typedef struct +{ + const char* name; + const char* value; +} MQTTClient_nameValue; + +/** + * This function returns version information about the library. + * no trace information will be returned. + * @return an array of strings describing the library. The last entry is a NULL pointer. + */ +DLLExport MQTTClient_nameValue* MQTTClient_getVersionInfo(void); + +/** + * This function attempts to connect a previously-created client (see + * MQTTClient_create()) to an MQTT server using the specified options. If you + * want to enable asynchronous message and status notifications, you must call + * MQTTClient_setCallbacks() prior to MQTTClient_connect(). + * @param handle A valid client handle from a successful call to + * MQTTClient_create(). + * @param options A pointer to a valid MQTTClient_connectOptions + * structure. + * @return ::MQTTCLIENT_SUCCESS if the client successfully connects to the + * server. An error code is returned if the client was unable to connect to + * the server. + * Error codes greater than 0 are returned by the MQTT protocol:

+ * 1: Connection refused: Unacceptable protocol version
+ * 2: Connection refused: Identifier rejected
+ * 3: Connection refused: Server unavailable
+ * 4: Connection refused: Bad user name or password
+ * 5: Connection refused: Not authorized
+ * 6-255: Reserved for future use
+ */ +DLLExport int MQTTClient_connect(MQTTClient handle, MQTTClient_connectOptions* options); + + +typedef struct MQTTResponse +{ + int version; + enum MQTTReasonCodes reasonCode; + int reasonCodeCount; /* used for subscribeMany5 and unsubscribeMany5 */ + enum MQTTReasonCodes* reasonCodes; /* used for subscribeMany5 and unsubscribeMany5 */ + MQTTProperties* properties; /* optional */ +} MQTTResponse; + +#define MQTTResponse_initializer {1, MQTTREASONCODE_SUCCESS, 0, NULL, NULL} + +DLLExport void MQTTResponse_free(MQTTResponse response); + +DLLExport MQTTResponse MQTTClient_connect5(MQTTClient handle, MQTTClient_connectOptions* options, + MQTTProperties* connectProperties, MQTTProperties* willProperties); + +/** + * This function attempts to disconnect the client from the MQTT + * server. In order to allow the client time to complete handling of messages + * that are in-flight when this function is called, a timeout period is + * specified. When the timeout period has expired, the client disconnects even + * if there are still outstanding message acknowledgements. + * The next time the client connects to the same server, any QoS 1 or 2 + * messages which have not completed will be retried depending on the + * cleansession settings for both the previous and the new connection (see + * MQTTClient_connectOptions.cleansession and MQTTClient_connect()). + * @param handle A valid client handle from a successful call to + * MQTTClient_create(). + * @param timeout The client delays disconnection for up to this time (in + * milliseconds) in order to allow in-flight message transfers to complete. + * @return ::MQTTCLIENT_SUCCESS if the client successfully disconnects from + * the server. An error code is returned if the client was unable to disconnect + * from the server + */ +DLLExport int MQTTClient_disconnect(MQTTClient handle, int timeout); + +DLLExport int MQTTClient_disconnect5(MQTTClient handle, int timeout, enum MQTTReasonCodes reason, MQTTProperties* props); + +/** + * This function allows the client application to test whether or not a + * client is currently connected to the MQTT server. + * @param handle A valid client handle from a successful call to + * MQTTClient_create(). + * @return Boolean true if the client is connected, otherwise false. + */ +DLLExport int MQTTClient_isConnected(MQTTClient handle); + + +/* Subscribe is synchronous. QoS list parameter is changed on return to granted QoSs. + Returns return code, MQTTCLIENT_SUCCESS == success, non-zero some sort of error (TBD) */ + +/** + * This function attempts to subscribe a client to a single topic, which may + * contain wildcards (see @ref wildcard). This call also specifies the + * @ref qos requested for the subscription + * (see also MQTTClient_subscribeMany()). + * @param handle A valid client handle from a successful call to + * MQTTClient_create(). + * @param topic The subscription topic, which may include wildcards. + * @param qos The requested quality of service for the subscription. + * @return ::MQTTCLIENT_SUCCESS if the subscription request is successful. + * An error code is returned if there was a problem registering the + * subscription. + */ +DLLExport int MQTTClient_subscribe(MQTTClient handle, const char* topic, int qos); + + +DLLExport MQTTResponse MQTTClient_subscribe5(MQTTClient handle, const char* topic, int qos, + MQTTSubscribe_options* opts, MQTTProperties* props); + +/** + * This function attempts to subscribe a client to a list of topics, which may + * contain wildcards (see @ref wildcard). This call also specifies the + * @ref qos requested for each topic (see also MQTTClient_subscribe()). + * @param handle A valid client handle from a successful call to + * MQTTClient_create(). + * @param count The number of topics for which the client is requesting + * subscriptions. + * @param topic An array (of length count) of pointers to + * topics, each of which may include wildcards. + * @param qos An array (of length count) of @ref qos + * values. qos[n] is the requested QoS for topic[n]. + * @return ::MQTTCLIENT_SUCCESS if the subscription request is successful. + * An error code is returned if there was a problem registering the + * subscriptions. + */ +DLLExport int MQTTClient_subscribeMany(MQTTClient handle, int count, char* const* topic, int* qos); + +DLLExport MQTTResponse MQTTClient_subscribeMany5(MQTTClient handle, int count, char* const* topic, + int* qos, MQTTSubscribe_options* opts, MQTTProperties* props); + +/** + * This function attempts to remove an existing subscription made by the + * specified client. + * @param handle A valid client handle from a successful call to + * MQTTClient_create(). + * @param topic The topic for the subscription to be removed, which may + * include wildcards (see @ref wildcard). + * @return ::MQTTCLIENT_SUCCESS if the subscription is removed. + * An error code is returned if there was a problem removing the + * subscription. + */ +DLLExport int MQTTClient_unsubscribe(MQTTClient handle, const char* topic); + +DLLExport MQTTResponse MQTTClient_unsubscribe5(MQTTClient handle, const char* topic, MQTTProperties* props); + +/** + * This function attempts to remove existing subscriptions to a list of topics + * made by the specified client. + * @param handle A valid client handle from a successful call to + * MQTTClient_create(). + * @param count The number subscriptions to be removed. + * @param topic An array (of length count) of pointers to the topics of + * the subscriptions to be removed, each of which may include wildcards. + * @return ::MQTTCLIENT_SUCCESS if the subscriptions are removed. + * An error code is returned if there was a problem removing the subscriptions. + */ +DLLExport int MQTTClient_unsubscribeMany(MQTTClient handle, int count, char* const* topic); + +DLLExport MQTTResponse MQTTClient_unsubscribeMany5(MQTTClient handle, int count, char* const* topic, MQTTProperties* props); + +/** + * This function attempts to publish a message to a given topic (see also + * MQTTClient_publishMessage()). An ::MQTTClient_deliveryToken is issued when + * this function returns successfully. If the client application needs to + * test for succesful delivery of QoS1 and QoS2 messages, this can be done + * either asynchronously or synchronously (see @ref async, + * ::MQTTClient_waitForCompletion and MQTTClient_deliveryComplete()). + * @param handle A valid client handle from a successful call to + * MQTTClient_create(). + * @param topicName The topic associated with this message. + * @param payloadlen The length of the payload in bytes. + * @param payload A pointer to the byte array payload of the message. + * @param qos The @ref qos of the message. + * @param retained The retained flag for the message. + * @param dt A pointer to an ::MQTTClient_deliveryToken. This is populated + * with a token representing the message when the function returns + * successfully. If your application does not use delivery tokens, set this + * argument to NULL. + * @return ::MQTTCLIENT_SUCCESS if the message is accepted for publication. + * An error code is returned if there was a problem accepting the message. + */ +DLLExport int MQTTClient_publish(MQTTClient handle, const char* topicName, int payloadlen, const void* payload, int qos, int retained, + MQTTClient_deliveryToken* dt); + +DLLExport MQTTResponse MQTTClient_publish5(MQTTClient handle, const char* topicName, int payloadlen, const void* payload, + int qos, int retained, MQTTProperties* properties, MQTTClient_deliveryToken* dt); +/** + * This function attempts to publish a message to a given topic (see also + * MQTTClient_publish()). An ::MQTTClient_deliveryToken is issued when + * this function returns successfully. If the client application needs to + * test for succesful delivery of QoS1 and QoS2 messages, this can be done + * either asynchronously or synchronously (see @ref async, + * ::MQTTClient_waitForCompletion and MQTTClient_deliveryComplete()). + * @param handle A valid client handle from a successful call to + * MQTTClient_create(). + * @param topicName The topic associated with this message. + * @param msg A pointer to a valid MQTTClient_message structure containing + * the payload and attributes of the message to be published. + * @param dt A pointer to an ::MQTTClient_deliveryToken. This is populated + * with a token representing the message when the function returns + * successfully. If your application does not use delivery tokens, set this + * argument to NULL. + * @return ::MQTTCLIENT_SUCCESS if the message is accepted for publication. + * An error code is returned if there was a problem accepting the message. + */ +DLLExport int MQTTClient_publishMessage(MQTTClient handle, const char* topicName, MQTTClient_message* msg, MQTTClient_deliveryToken* dt); + + +DLLExport MQTTResponse MQTTClient_publishMessage5(MQTTClient handle, const char* topicName, MQTTClient_message* msg, + MQTTClient_deliveryToken* dt); + +/** + * This function is called by the client application to synchronize execution + * of the main thread with completed publication of a message. When called, + * MQTTClient_waitForCompletion() blocks execution until the message has been + * successful delivered or the specified timeout has expired. See @ref async. + * @param handle A valid client handle from a successful call to + * MQTTClient_create(). + * @param dt The ::MQTTClient_deliveryToken that represents the message being + * tested for successful delivery. Delivery tokens are issued by the + * publishing functions MQTTClient_publish() and MQTTClient_publishMessage(). + * @param timeout The maximum time to wait in milliseconds. + * @return ::MQTTCLIENT_SUCCESS if the message was successfully delivered. + * An error code is returned if the timeout expires or there was a problem + * checking the token. + */ +DLLExport int MQTTClient_waitForCompletion(MQTTClient handle, MQTTClient_deliveryToken dt, unsigned long timeout); + + +/** + * This function sets a pointer to an array of delivery tokens for + * messages that are currently in-flight (pending completion). + * + * Important note: The memory used to hold the array of tokens is + * malloc()'d in this function. The client application is responsible for + * freeing this memory when it is no longer required. + * @param handle A valid client handle from a successful call to + * MQTTClient_create(). + * @param tokens The address of a pointer to an ::MQTTClient_deliveryToken. + * When the function returns successfully, the pointer is set to point to an + * array of tokens representing messages pending completion. The last member of + * the array is set to -1 to indicate there are no more tokens. If no tokens + * are pending, the pointer is set to NULL. + * @return ::MQTTCLIENT_SUCCESS if the function returns successfully. + * An error code is returned if there was a problem obtaining the list of + * pending tokens. + */ +DLLExport int MQTTClient_getPendingDeliveryTokens(MQTTClient handle, MQTTClient_deliveryToken **tokens); + +/** + * When implementing a single-threaded client, call this function periodically + * to allow processing of message retries and to send MQTT keepalive pings. + * If the application is calling MQTTClient_receive() regularly, then it is + * not necessary to call this function. + */ +DLLExport void MQTTClient_yield(void); + +/** + * This function performs a synchronous receive of incoming messages. It should + * be used only when the client application has not set callback methods to + * support asynchronous receipt of messages (see @ref async and + * MQTTClient_setCallbacks()). Using this function allows a single-threaded + * client subscriber application to be written. When called, this function + * blocks until the next message arrives or the specified timeout expires + *(see also MQTTClient_yield()). + * + * Important note: The application must free() the memory allocated + * to the topic and the message when processing is complete (see + * MQTTClient_freeMessage()). + * @param handle A valid client handle from a successful call to + * MQTTClient_create(). + * @param topicName The address of a pointer to a topic. This function + * allocates the memory for the topic and returns it to the application + * by setting topicName to point to the topic. + * @param topicLen The length of the topic. If the return code from this + * function is ::MQTTCLIENT_TOPICNAME_TRUNCATED, the topic contains embedded + * NULL characters and the full topic should be retrieved by using + * topicLen. + * @param message The address of a pointer to the received message. This + * function allocates the memory for the message and returns it to the + * application by setting message to point to the received message. + * The pointer is set to NULL if the timeout expires. + * @param timeout The length of time to wait for a message in milliseconds. + * @return ::MQTTCLIENT_SUCCESS or ::MQTTCLIENT_TOPICNAME_TRUNCATED if a + * message is received. ::MQTTCLIENT_SUCCESS can also indicate that the + * timeout expired, in which case message is NULL. An error code is + * returned if there was a problem trying to receive a message. + */ +DLLExport int MQTTClient_receive(MQTTClient handle, char** topicName, int* topicLen, MQTTClient_message** message, + unsigned long timeout); + +/** + * This function frees memory allocated to an MQTT message, including the + * additional memory allocated to the message payload. The client application + * calls this function when the message has been fully processed. Important + * note: This function does not free the memory allocated to a message + * topic string. It is the responsibility of the client application to free + * this memory using the MQTTClient_free() library function. + * @param msg The address of a pointer to the ::MQTTClient_message structure + * to be freed. + */ +DLLExport void MQTTClient_freeMessage(MQTTClient_message** msg); + +/** + * This function frees memory allocated by the MQTT C client library, especially the + * topic name. This is needed on Windows when the client libary and application + * program have been compiled with different versions of the C compiler. It is + * thus good policy to always use this function when freeing any MQTT C client- + * allocated memory. + * @param ptr The pointer to the client library storage to be freed. + */ +DLLExport void MQTTClient_free(void* ptr); + +/** + * This function frees the memory allocated to an MQTT client (see + * MQTTClient_create()). It should be called when the client is no longer + * required. + * @param handle A pointer to the handle referring to the ::MQTTClient + * structure to be freed. + */ +DLLExport void MQTTClient_destroy(MQTTClient* handle); + + +enum MQTTCLIENT_TRACE_LEVELS +{ + MQTTCLIENT_TRACE_MAXIMUM = 1, + MQTTCLIENT_TRACE_MEDIUM, + MQTTCLIENT_TRACE_MINIMUM, + MQTTCLIENT_TRACE_PROTOCOL, + MQTTCLIENT_TRACE_ERROR, + MQTTCLIENT_TRACE_SEVERE, + MQTTCLIENT_TRACE_FATAL, +}; + + +/** + * This function sets the level of trace information which will be + * returned in the trace callback. + * @param level the trace level required + */ +DLLExport void MQTTClient_setTraceLevel(enum MQTTCLIENT_TRACE_LEVELS level); + + +/** + * This is a callback function prototype which must be implemented if you want + * to receive trace information. + * @param level the trace level of the message returned + * @param message the trace message. This is a pointer to a static buffer which + * will be overwritten on each call. You must copy the data if you want to keep + * it for later. + */ +typedef void MQTTClient_traceCallback(enum MQTTCLIENT_TRACE_LEVELS level, char* message); + +/** + * This function sets the trace callback if needed. If set to NULL, + * no trace information will be returned. The default trace level is + * MQTTASYNC_TRACE_MINIMUM. + * @param callback a pointer to the function which will handle the trace information + */ +DLLExport void MQTTClient_setTraceCallback(MQTTClient_traceCallback* callback); + +/** + * Returns a pointer to the string representation of the error or NULL. + * + * Do not free after use. Returns NULL if the error code is unknown. + */ +DLLExport const char* MQTTClient_strerror(int code); + +#ifdef __cplusplus + } +#endif + +#endif + +/** + * @cond MQTTClient_main + * @page async Asynchronous vs synchronous client applications + * The client library supports two modes of operation. These are referred to + * as synchronous and asynchronous modes. If your application + * calls MQTTClient_setCallbacks(), this puts the client into asynchronous + * mode, otherwise it operates in synchronous mode. + * + * In synchronous mode, the client application runs on a single thread. + * Messages are published using the MQTTClient_publish() and + * MQTTClient_publishMessage() functions. To determine that a QoS1 or QoS2 + * (see @ref qos) message has been successfully delivered, the application + * must call the MQTTClient_waitForCompletion() function. An example showing + * synchronous publication is shown in @ref pubsync. Receiving messages in + * synchronous mode uses the MQTTClient_receive() function. Client applications + * must call either MQTTClient_receive() or MQTTClient_yield() relatively + * frequently in order to allow processing of acknowledgements and the MQTT + * "pings" that keep the network connection to the server alive. + * + * In asynchronous mode, the client application runs on several threads. The + * main program calls functions in the client library to publish and subscribe, + * just as for the synchronous mode. Processing of handshaking and maintaining + * the network connection is performed in the background, however. + * Notifications of status and message reception are provided to the client + * application using callbacks registered with the library by the call to + * MQTTClient_setCallbacks() (see MQTTClient_messageArrived(), + * MQTTClient_connectionLost() and MQTTClient_deliveryComplete()). + * This API is not thread safe however - it is not possible to call it from multiple + * threads without synchronization. You can use the MQTTAsync API for that. + * + * @page wildcard Subscription wildcards + * Every MQTT message includes a topic that classifies it. MQTT servers use + * topics to determine which subscribers should receive messages published to + * the server. + * + * Consider the server receiving messages from several environmental sensors. + * Each sensor publishes its measurement data as a message with an associated + * topic. Subscribing applications need to know which sensor originally + * published each received message. A unique topic is thus used to identify + * each sensor and measurement type. Topics such as SENSOR1TEMP, + * SENSOR1HUMIDITY, SENSOR2TEMP and so on achieve this but are not very + * flexible. If additional sensors are added to the system at a later date, + * subscribing applications must be modified to receive them. + * + * To provide more flexibility, MQTT supports a hierarchical topic namespace. + * This allows application designers to organize topics to simplify their + * management. Levels in the hierarchy are delimited by the '/' character, + * such as SENSOR/1/HUMIDITY. Publishers and subscribers use these + * hierarchical topics as already described. + * + * For subscriptions, two wildcard characters are supported: + *
    + *
  • A '#' character represents a complete sub-tree of the hierarchy and + * thus must be the last character in a subscription topic string, such as + * SENSOR/#. This will match any topic starting with SENSOR/, such as + * SENSOR/1/TEMP and SENSOR/2/HUMIDITY.
    • + *
  • A '+' character represents a single level of the hierarchy and is + * used between delimiters. For example, SENSOR/+/TEMP will match + * SENSOR/1/TEMP and SENSOR/2/TEMP.
    • + *
+ * Publishers are not allowed to use the wildcard characters in their topic + * names. + * + * Deciding on your topic hierarchy is an important step in your system design. + * + * @page qos Quality of service + * The MQTT protocol provides three qualities of service for delivering + * messages between clients and servers: "at most once", "at least once" and + * "exactly once". + * + * Quality of service (QoS) is an attribute of an individual message being + * published. An application sets the QoS for a specific message by setting the + * MQTTClient_message.qos field to the required value. + * + * A subscribing client can set the maximum quality of service a server uses + * to send messages that match the client subscriptions. The + * MQTTClient_subscribe() and MQTTClient_subscribeMany() functions set this + * maximum. The QoS of a message forwarded to a subscriber thus might be + * different to the QoS given to the message by the original publisher. + * The lower of the two values is used to forward a message. + * + * The three levels are: + * + * QoS0, At most once: The message is delivered at most once, or it + * may not be delivered at all. Its delivery across the network is not + * acknowledged. The message is not stored. The message could be lost if the + * client is disconnected, or if the server fails. QoS0 is the fastest mode of + * transfer. It is sometimes called "fire and forget". + * + * The MQTT protocol does not require servers to forward publications at QoS0 + * to a client. If the client is disconnected at the time the server receives + * the publication, the publication might be discarded, depending on the + * server implementation. + * + * QoS1, At least once: The message is always delivered at least once. + * It might be delivered multiple times if there is a failure before an + * acknowledgment is received by the sender. The message must be stored + * locally at the sender, until the sender receives confirmation that the + * message has been published by the receiver. The message is stored in case + * the message must be sent again. + * + * QoS2, Exactly once: The message is always delivered exactly once. + * The message must be stored locally at the sender, until the sender receives + * confirmation that the message has been published by the receiver. The + * message is stored in case the message must be sent again. QoS2 is the + * safest, but slowest mode of transfer. A more sophisticated handshaking + * and acknowledgement sequence is used than for QoS1 to ensure no duplication + * of messages occurs. + * @page pubsync Synchronous publication example +@code +#include +#include +#include +#include "MQTTClient.h" + +#define ADDRESS "tcp://localhost:1883" +#define CLIENTID "ExampleClientPub" +#define TOPIC "MQTT Examples" +#define PAYLOAD "Hello World!" +#define QOS 1 +#define TIMEOUT 10000L + +int main(int argc, char* argv[]) +{ + MQTTClient client; + MQTTClient_connectOptions conn_opts = MQTTClient_connectOptions_initializer; + MQTTClient_message pubmsg = MQTTClient_message_initializer; + MQTTClient_deliveryToken token; + int rc; + + MQTTClient_create(&client, ADDRESS, CLIENTID, + MQTTCLIENT_PERSISTENCE_NONE, NULL); + conn_opts.keepAliveInterval = 20; + conn_opts.cleansession = 1; + + if ((rc = MQTTClient_connect(client, &conn_opts)) != MQTTCLIENT_SUCCESS) + { + printf("Failed to connect, return code %d\n", rc); + exit(EXIT_FAILURE); + } + pubmsg.payload = PAYLOAD; + pubmsg.payloadlen = strlen(PAYLOAD); + pubmsg.qos = QOS; + pubmsg.retained = 0; + MQTTClient_publishMessage(client, TOPIC, &pubmsg, &token); + printf("Waiting for up to %d seconds for publication of %s\n" + "on topic %s for client with ClientID: %s\n", + (int)(TIMEOUT/1000), PAYLOAD, TOPIC, CLIENTID); + rc = MQTTClient_waitForCompletion(client, token, TIMEOUT); + printf("Message with delivery token %d delivered\n", token); + MQTTClient_disconnect(client, 10000); + MQTTClient_destroy(&client); + return rc; +} + + * @endcode + * + * @page pubasync Asynchronous publication example +@code{.c} +#include +#include +#include +#include "MQTTClient.h" + +#define ADDRESS "tcp://localhost:1883" +#define CLIENTID "ExampleClientPub" +#define TOPIC "MQTT Examples" +#define PAYLOAD "Hello World!" +#define QOS 1 +#define TIMEOUT 10000L + +volatile MQTTClient_deliveryToken deliveredtoken; + +void delivered(void *context, MQTTClient_deliveryToken dt) +{ + printf("Message with token value %d delivery confirmed\n", dt); + deliveredtoken = dt; +} + +int msgarrvd(void *context, char *topicName, int topicLen, MQTTClient_message *message) +{ + int i; + char* payloadptr; + + printf("Message arrived\n"); + printf(" topic: %s\n", topicName); + printf(" message: "); + + payloadptr = message->payload; + for(i=0; ipayloadlen; i++) + { + putchar(*payloadptr++); + } + putchar('\n'); + MQTTClient_freeMessage(&message); + MQTTClient_free(topicName); + return 1; +} + +void connlost(void *context, char *cause) +{ + printf("\nConnection lost\n"); + printf(" cause: %s\n", cause); +} + +int main(int argc, char* argv[]) +{ + MQTTClient client; + MQTTClient_connectOptions conn_opts = MQTTClient_connectOptions_initializer; + MQTTClient_message pubmsg = MQTTClient_message_initializer; + MQTTClient_deliveryToken token; + int rc; + + MQTTClient_create(&client, ADDRESS, CLIENTID, + MQTTCLIENT_PERSISTENCE_NONE, NULL); + conn_opts.keepAliveInterval = 20; + conn_opts.cleansession = 1; + + MQTTClient_setCallbacks(client, NULL, connlost, msgarrvd, delivered); + + if ((rc = MQTTClient_connect(client, &conn_opts)) != MQTTCLIENT_SUCCESS) + { + printf("Failed to connect, return code %d\n", rc); + exit(EXIT_FAILURE); + } + pubmsg.payload = PAYLOAD; + pubmsg.payloadlen = strlen(PAYLOAD); + pubmsg.qos = QOS; + pubmsg.retained = 0; + deliveredtoken = 0; + MQTTClient_publishMessage(client, TOPIC, &pubmsg, &token); + printf("Waiting for publication of %s\n" + "on topic %s for client with ClientID: %s\n", + PAYLOAD, TOPIC, CLIENTID); + while(deliveredtoken != token); + MQTTClient_disconnect(client, 10000); + MQTTClient_destroy(&client); + return rc; +} + + * @endcode + * @page subasync Asynchronous subscription example +@code +#include +#include +#include +#include "MQTTClient.h" + +#define ADDRESS "tcp://localhost:1883" +#define CLIENTID "ExampleClientSub" +#define TOPIC "MQTT Examples" +#define PAYLOAD "Hello World!" +#define QOS 1 +#define TIMEOUT 10000L + +volatile MQTTClient_deliveryToken deliveredtoken; + +void delivered(void *context, MQTTClient_deliveryToken dt) +{ + printf("Message with token value %d delivery confirmed\n", dt); + deliveredtoken = dt; +} + +int msgarrvd(void *context, char *topicName, int topicLen, MQTTClient_message *message) +{ + int i; + char* payloadptr; + + printf("Message arrived\n"); + printf(" topic: %s\n", topicName); + printf(" message: "); + + payloadptr = message->payload; + for(i=0; ipayloadlen; i++) + { + putchar(*payloadptr++); + } + putchar('\n'); + MQTTClient_freeMessage(&message); + MQTTClient_free(topicName); + return 1; +} + +void connlost(void *context, char *cause) +{ + printf("\nConnection lost\n"); + printf(" cause: %s\n", cause); +} + +int main(int argc, char* argv[]) +{ + MQTTClient client; + MQTTClient_connectOptions conn_opts = MQTTClient_connectOptions_initializer; + int rc; + int ch; + + MQTTClient_create(&client, ADDRESS, CLIENTID, + MQTTCLIENT_PERSISTENCE_NONE, NULL); + conn_opts.keepAliveInterval = 20; + conn_opts.cleansession = 1; + + MQTTClient_setCallbacks(client, NULL, connlost, msgarrvd, delivered); + + if ((rc = MQTTClient_connect(client, &conn_opts)) != MQTTCLIENT_SUCCESS) + { + printf("Failed to connect, return code %d\n", rc); + exit(EXIT_FAILURE); + } + printf("Subscribing to topic %s\nfor client %s using QoS%d\n\n" + "Press Q to quit\n\n", TOPIC, CLIENTID, QOS); + MQTTClient_subscribe(client, TOPIC, QOS); + + do + { + ch = getchar(); + } while(ch!='Q' && ch != 'q'); + + MQTTClient_disconnect(client, 10000); + MQTTClient_destroy(&client); + return rc; +} + + * @endcode + * @page tracing Tracing + * + * Runtime tracing is controlled by environment variables. + * + * Tracing is switched on by setting MQTT_C_CLIENT_TRACE. A value of ON, or stdout, prints to + * stdout, any other value is interpreted as a file name to use. + * + * The amount of trace detail is controlled with the MQTT_C_CLIENT_TRACE_LEVEL environment + * variable - valid values are ERROR, PROTOCOL, MINIMUM, MEDIUM and MAXIMUM + * (from least to most verbose). + * + * The variable MQTT_C_CLIENT_TRACE_MAX_LINES limits the number of lines of trace that are output + * to a file. Two files are used at most, when they are full, the last one is overwritten with the + * new trace entries. The default size is 1000 lines. + * + * ### MQTT Packet Tracing + * + * A feature that can be very useful is printing the MQTT packets that are sent and received. To + * achieve this, use the following environment variable settings: + * @code + MQTT_C_CLIENT_TRACE=ON + MQTT_C_CLIENT_TRACE_LEVEL=PROTOCOL + * @endcode + * The output you should see looks like this: + * @code + 20130528 155936.813 3 stdout-subscriber -> CONNECT cleansession: 1 (0) + 20130528 155936.813 3 stdout-subscriber <- CONNACK rc: 0 + 20130528 155936.813 3 stdout-subscriber -> SUBSCRIBE msgid: 1 (0) + 20130528 155936.813 3 stdout-subscriber <- SUBACK msgid: 1 + 20130528 155941.818 3 stdout-subscriber -> DISCONNECT (0) + * @endcode + * where the fields are: + * 1. date + * 2. time + * 3. socket number + * 4. client id + * 5. direction (-> from client to server, <- from server to client) + * 6. packet details + * + * ### Default Level Tracing + * + * This is an extract of a default level trace of a call to connect: + * @code + 19700101 010000.000 (1152206656) (0)> MQTTClient_connect:893 + 19700101 010000.000 (1152206656) (1)> MQTTClient_connectURI:716 + 20130528 160447.479 Connecting to serverURI localhost:1883 + 20130528 160447.479 (1152206656) (2)> MQTTProtocol_connect:98 + 20130528 160447.479 (1152206656) (3)> MQTTProtocol_addressPort:48 + 20130528 160447.479 (1152206656) (3)< MQTTProtocol_addressPort:73 + 20130528 160447.479 (1152206656) (3)> Socket_new:599 + 20130528 160447.479 New socket 4 for localhost, port 1883 + 20130528 160447.479 (1152206656) (4)> Socket_addSocket:163 + 20130528 160447.479 (1152206656) (5)> Socket_setnonblocking:73 + 20130528 160447.479 (1152206656) (5)< Socket_setnonblocking:78 (0) + 20130528 160447.479 (1152206656) (4)< Socket_addSocket:176 (0) + 20130528 160447.479 (1152206656) (4)> Socket_error:95 + 20130528 160447.479 (1152206656) (4)< Socket_error:104 (115) + 20130528 160447.479 Connect pending + 20130528 160447.479 (1152206656) (3)< Socket_new:683 (115) + 20130528 160447.479 (1152206656) (2)< MQTTProtocol_connect:131 (115) + * @endcode + * where the fields are: + * 1. date + * 2. time + * 3. thread id + * 4. function nesting level + * 5. function entry (>) or exit (<) + * 6. function name : line of source code file + * 7. return value (if there is one) + * + * ### Memory Allocation Tracing + * + * Setting the trace level to maximum causes memory allocations and frees to be traced along with + * the default trace entries, with messages like the following: + * @code + 20130528 161819.657 Allocating 16 bytes in heap at file /home/icraggs/workspaces/mqrtc/mqttv3c/src/MQTTPacket.c line 177 ptr 0x179f930 + + 20130528 161819.657 Freeing 16 bytes in heap at file /home/icraggs/workspaces/mqrtc/mqttv3c/src/MQTTPacket.c line 201, heap use now 896 bytes + * @endcode + * When the last MQTT client object is destroyed, if the trace is being recorded + * and all memory allocated by the client library has not been freed, an error message will be + * written to the trace. This can help with fixing memory leaks. The message will look like this: + * @code + 20130528 163909.208 Some memory not freed at shutdown, possible memory leak + 20130528 163909.208 Heap scan start, total 880 bytes + 20130528 163909.208 Heap element size 32, line 354, file /home/icraggs/workspaces/mqrtc/mqttv3c/src/MQTTPacket.c, ptr 0x260cb00 + 20130528 163909.208 Content + 20130528 163909.209 Heap scan end + * @endcode + * @endcond + */ diff --git a/src/task_manager/include/paho_mqtt_3c/MQTTClientPersistence.h b/src/task_manager/include/paho_mqtt_3c/MQTTClientPersistence.h new file mode 100644 index 0000000..4c9014d --- /dev/null +++ b/src/task_manager/include/paho_mqtt_3c/MQTTClientPersistence.h @@ -0,0 +1,254 @@ +/******************************************************************************* + * Copyright (c) 2009, 2012 IBM Corp. + * + * All rights reserved. This program and the accompanying materials + * are made available under the terms of the Eclipse Public License v1.0 + * and Eclipse Distribution License v1.0 which accompany this distribution. + * + * The Eclipse Public License is available at + * http://www.eclipse.org/legal/epl-v10.html + * and the Eclipse Distribution License is available at + * http://www.eclipse.org/org/documents/edl-v10.php. + * + * Contributors: + * Ian Craggs - initial API and implementation and/or initial documentation + *******************************************************************************/ + +/** + * @file + * \brief This structure represents a persistent data store, used to store + * outbound and inbound messages, in order to achieve reliable messaging. + * + * The MQTT Client persists QoS1 and QoS2 messages in order to meet the + * assurances of delivery associated with these @ref qos levels. The messages + * are saved in persistent storage + * The type and context of the persistence implementation are specified when + * the MQTT client is created (see MQTTClient_create()). The default + * persistence type (::MQTTCLIENT_PERSISTENCE_DEFAULT) uses a file system-based + * persistence mechanism. The persistence_context argument passed to + * MQTTClient_create() when using the default peristence is a string + * representing the location of the persistence directory. If the context + * argument is NULL, the working directory will be used. + * + * To use memory-based persistence, an application passes + * ::MQTTCLIENT_PERSISTENCE_NONE as the persistence_type to + * MQTTClient_create(). This can lead to message loss in certain situations, + * but can be appropriate in some cases (see @ref qos). + * + * Client applications can provide their own persistence mechanism by passing + * ::MQTTCLIENT_PERSISTENCE_USER as the persistence_type. To implement a + * custom persistence mechanism, the application must pass an initialized + * ::MQTTClient_persistence structure as the persistence_context + * argument to MQTTClient_create(). + * + * If the functions defined return an ::MQTTCLIENT_PERSISTENCE_ERROR then the + * state of the persisted data should remain as it was prior to the function + * being called. For example, if Persistence_put() returns + * ::MQTTCLIENT_PERSISTENCE_ERROR, then it is assumed tha tthe persistent store + * does not contain the data that was passed to the function. Similarly, if + * Persistence_remove() returns ::MQTTCLIENT_PERSISTENCE_ERROR then it is + * assumed that the data to be removed is still held in the persistent store. + * + * It is up to the persistence implementation to log any error information that + * may be required to diagnose a persistence mechanism failure. + */ + +/* +/// @cond EXCLUDE +*/ +#if !defined(MQTTCLIENTPERSISTENCE_H) +#define MQTTCLIENTPERSISTENCE_H +/* +/// @endcond +*/ + +/** + * This persistence_type value specifies the default file system-based + * persistence mechanism (see MQTTClient_create()). + */ +#define MQTTCLIENT_PERSISTENCE_DEFAULT 0 +/** + * This persistence_type value specifies a memory-based + * persistence mechanism (see MQTTClient_create()). + */ +#define MQTTCLIENT_PERSISTENCE_NONE 1 +/** + * This persistence_type value specifies an application-specific + * persistence mechanism (see MQTTClient_create()). + */ +#define MQTTCLIENT_PERSISTENCE_USER 2 + +/** + * Application-specific persistence functions must return this error code if + * there is a problem executing the function. + */ +#define MQTTCLIENT_PERSISTENCE_ERROR -2 + +/** + * @brief Initialize the persistent store. + * + * Either open the existing persistent store for this client ID or create a new + * one if one doesn't exist. If the persistent store is already open, return + * without taking any action. + * + * An application can use the same client identifier to connect to many + * different servers. The clientid in conjunction with the + * serverURI uniquely identifies the persistence store required. + * + * @param handle The address of a pointer to a handle for this persistence + * implementation. This function must set handle to a valid reference to the + * persistence following a successful return. + * The handle pointer is passed as an argument to all the other + * persistence functions. It may include the context parameter and/or any other + * data for use by the persistence functions. + * @param clientID The client identifier for which the persistent store should + * be opened. + * @param serverURI The connection string specified when the MQTT client was + * created (see MQTTClient_create()). + * @param context A pointer to any data required to initialize the persistent + * store (see ::MQTTClient_persistence). + * @return Return 0 if the function completes successfully, otherwise return + * ::MQTTCLIENT_PERSISTENCE_ERROR. + */ +typedef int (*Persistence_open)(void** handle, const char* clientID, const char* serverURI, void* context); + +/** + * @brief Close the persistent store referred to by the handle. + * + * @param handle The handle pointer from a successful call to + * Persistence_open(). + * @return Return 0 if the function completes successfully, otherwise return + * ::MQTTCLIENT_PERSISTENCE_ERROR. + */ +typedef int (*Persistence_close)(void* handle); + +/** + * @brief Put the specified data into the persistent store. + * + * @param handle The handle pointer from a successful call to + * Persistence_open(). + * @param key A string used as the key for the data to be put in the store. The + * key is later used to retrieve data from the store with Persistence_get(). + * @param bufcount The number of buffers to write to the persistence store. + * @param buffers An array of pointers to the data buffers associated with + * this key. + * @param buflens An array of lengths of the data buffers. buflen[n] + * gives the length of buffer[n]. + * @return Return 0 if the function completes successfully, otherwise return + * ::MQTTCLIENT_PERSISTENCE_ERROR. + */ +typedef int (*Persistence_put)(void* handle, char* key, int bufcount, char* buffers[], int buflens[]); + +/** + * @brief Retrieve the specified data from the persistent store. + * + * @param handle The handle pointer from a successful call to + * Persistence_open(). + * @param key A string that is the key for the data to be retrieved. This is + * the same key used to save the data to the store with Persistence_put(). + * @param buffer The address of a pointer to a buffer. This function sets the + * pointer to point at the retrieved data, if successful. + * @param buflen The address of an int that is set to the length of + * buffer by this function if successful. + * @return Return 0 if the function completes successfully, otherwise return + * ::MQTTCLIENT_PERSISTENCE_ERROR. + */ +typedef int (*Persistence_get)(void* handle, char* key, char** buffer, int* buflen); + +/** + * @brief Remove the data for the specified key from the store. + * + * @param handle The handle pointer from a successful call to + * Persistence_open(). + * @param key A string that is the key for the data to be removed from the + * store. This is the same key used to save the data to the store with + * Persistence_put(). + * @return Return 0 if the function completes successfully, otherwise return + * ::MQTTCLIENT_PERSISTENCE_ERROR. + */ +typedef int (*Persistence_remove)(void* handle, char* key); + +/** + * @brief Returns the keys in this persistent data store. + * + * @param handle The handle pointer from a successful call to + * Persistence_open(). + * @param keys The address of a pointer to pointers to strings. Assuming + * successful execution, this function allocates memory to hold the returned + * keys (strings used to store the data with Persistence_put()). It also + * allocates memory to hold an array of pointers to these strings. keys + * is set to point to the array of pointers to strings. + * @param nkeys A pointer to the number of keys in this persistent data store. + * This function sets the number of keys, if successful. + * @return Return 0 if the function completes successfully, otherwise return + * ::MQTTCLIENT_PERSISTENCE_ERROR. + */ +typedef int (*Persistence_keys)(void* handle, char*** keys, int* nkeys); + +/** + * @brief Clears the persistence store, so that it no longer contains any + * persisted data. + * + * @param handle The handle pointer from a successful call to + * Persistence_open(). + * @return Return 0 if the function completes successfully, otherwise return + * ::MQTTCLIENT_PERSISTENCE_ERROR. + */ +typedef int (*Persistence_clear)(void* handle); + +/** + * @brief Returns whether any data has been persisted using the specified key. + * + * @param handle The handle pointer from a successful call to + * Persistence_open(). + * @param key The string to be tested for existence in the store. + * @return Return 0 if the key was found in the store, otherwise return + * ::MQTTCLIENT_PERSISTENCE_ERROR. + */ +typedef int (*Persistence_containskey)(void* handle, char* key); + +/** + * @brief A structure containing the function pointers to a persistence + * implementation and the context or state that will be shared across all + * the persistence functions. + */ +typedef struct { + /** + * A pointer to any data required to initialize the persistent store. + */ + void* context; + /** + * A function pointer to an implementation of Persistence_open(). + */ + Persistence_open popen; + /** + * A function pointer to an implementation of Persistence_close(). + */ + Persistence_close pclose; + /** + * A function pointer to an implementation of Persistence_put(). + */ + Persistence_put pput; + /** + * A function pointer to an implementation of Persistence_get(). + */ + Persistence_get pget; + /** + * A function pointer to an implementation of Persistence_remove(). + */ + Persistence_remove premove; + /** + * A function pointer to an implementation of Persistence_keys(). + */ + Persistence_keys pkeys; + /** + * A function pointer to an implementation of Persistence_clear(). + */ + Persistence_clear pclear; + /** + * A function pointer to an implementation of Persistence_containskey(). + */ + Persistence_containskey pcontainskey; +} MQTTClient_persistence; + +#endif diff --git a/src/task_manager/include/paho_mqtt_3c/MQTTConnect.h b/src/task_manager/include/paho_mqtt_3c/MQTTConnect.h new file mode 100644 index 0000000..98c2c16 --- /dev/null +++ b/src/task_manager/include/paho_mqtt_3c/MQTTConnect.h @@ -0,0 +1,148 @@ +/******************************************************************************* + * Copyright (c) 2014, 2017 IBM Corp. + * + * All rights reserved. This program and the accompanying materials + * are made available under the terms of the Eclipse Public License v1.0 + * and Eclipse Distribution License v1.0 which accompany this distribution. + * + * The Eclipse Public License is available at + * http://www.eclipse.org/legal/epl-v10.html + * and the Eclipse Distribution License is available at + * http://www.eclipse.org/org/documents/edl-v10.php. + * + * Contributors: + * Ian Craggs - initial API and implementation and/or initial documentation + * Ian Craggs - add connack return code definitions + * Xiang Rong - 442039 Add makefile to Embedded C client + * Ian Craggs - fix for issue #64, bit order in connack response + *******************************************************************************/ + +#ifndef MQTTCONNECT_H_ +#define MQTTCONNECT_H_ + +enum connack_return_codes +{ + MQTT_CONNECTION_ACCEPTED = 0, + MQTT_UNNACCEPTABLE_PROTOCOL = 1, + MQTT_CLIENTID_REJECTED = 2, + MQTT_SERVER_UNAVAILABLE = 3, + MQTT_BAD_USERNAME_OR_PASSWORD = 4, + MQTT_NOT_AUTHORIZED = 5, +}; + +#if !defined(DLLImport) + #define DLLImport +#endif +#if !defined(DLLExport) + #define DLLExport +#endif + + +typedef union +{ + unsigned char all; /**< all connect flags */ +#if defined(REVERSED) + struct + { + unsigned int username : 1; /**< 3.1 user name */ + unsigned int password : 1; /**< 3.1 password */ + unsigned int willRetain : 1; /**< will retain setting */ + unsigned int willQoS : 2; /**< will QoS value */ + unsigned int will : 1; /**< will flag */ + unsigned int cleansession : 1; /**< clean session flag */ + unsigned int : 1; /**< unused */ + } bits; +#else + struct + { + unsigned int : 1; /**< unused */ + unsigned int cleansession : 1; /**< cleansession flag */ + unsigned int will : 1; /**< will flag */ + unsigned int willQoS : 2; /**< will QoS value */ + unsigned int willRetain : 1; /**< will retain setting */ + unsigned int password : 1; /**< 3.1 password */ + unsigned int username : 1; /**< 3.1 user name */ + } bits; +#endif +} MQTTConnectFlags; /**< connect flags byte */ + + + +/** + * Defines the MQTT "Last Will and Testament" (LWT) settings for + * the connect packet. + */ +typedef struct +{ + /** The eyecatcher for this structure. must be MQTW. */ + char struct_id[4]; + /** The version number of this structure. Must be 0 */ + int struct_version; + /** The LWT topic to which the LWT message will be published. */ + MQTTString topicName; + /** The LWT payload. */ + MQTTString message; + /** + * The retained flag for the LWT message (see MQTTAsync_message.retained). + */ + unsigned char retained; + /** + * The quality of service setting for the LWT message (see + * MQTTAsync_message.qos and @ref qos). + */ + char qos; +} MQTTPacket_willOptions; + + +#define MQTTPacket_willOptions_initializer { {'M', 'Q', 'T', 'W'}, 0, {NULL, {0, NULL}}, {NULL, {0, NULL}}, 0, 0 } + + +typedef struct +{ + /** The eyecatcher for this structure. must be MQTC. */ + char struct_id[4]; + /** The version number of this structure. Must be 0 */ + int struct_version; + /** Version of MQTT to be used. 3 = 3.1 4 = 3.1.1 + */ + unsigned char MQTTVersion; + MQTTString clientID; + unsigned short keepAliveInterval; + unsigned char cleansession; + unsigned char willFlag; + MQTTPacket_willOptions will; + MQTTString username; + MQTTString password; +} MQTTPacket_connectData; + +typedef union +{ + unsigned char all; /**< all connack flags */ +#if defined(REVERSED) + struct + { + unsigned int reserved : 7; /**< unused */ + unsigned int sessionpresent : 1; /**< session present flag */ + } bits; +#else + struct + { + unsigned int sessionpresent : 1; /**< session present flag */ + unsigned int reserved: 7; /**< unused */ + } bits; +#endif +} MQTTConnackFlags; /**< connack flags byte */ + +#define MQTTPacket_connectData_initializer { {'M', 'Q', 'T', 'C'}, 0, 4, {NULL, {0, NULL}}, 60, 1, 0, \ + MQTTPacket_willOptions_initializer, {NULL, {0, NULL}}, {NULL, {0, NULL}} } + +DLLExport int MQTTSerialize_connect(unsigned char* buf, int buflen, MQTTPacket_connectData* options); +DLLExport int MQTTDeserialize_connect(MQTTPacket_connectData* data, unsigned char* buf, int len); + +DLLExport int MQTTSerialize_connack(unsigned char* buf, int buflen, unsigned char connack_rc, unsigned char sessionPresent); +DLLExport int MQTTDeserialize_connack(unsigned char* sessionPresent, unsigned char* connack_rc, unsigned char* buf, int buflen); + +DLLExport int MQTTSerialize_disconnect(unsigned char* buf, int buflen); +DLLExport int MQTTSerialize_pingreq(unsigned char* buf, int buflen); + +#endif /* MQTTCONNECT_H_ */ diff --git a/src/task_manager/include/paho_mqtt_3c/MQTTFormat.h b/src/task_manager/include/paho_mqtt_3c/MQTTFormat.h new file mode 100644 index 0000000..47b0c41 --- /dev/null +++ b/src/task_manager/include/paho_mqtt_3c/MQTTFormat.h @@ -0,0 +1,37 @@ +/******************************************************************************* + * Copyright (c) 2014 IBM Corp. + * + * All rights reserved. This program and the accompanying materials + * are made available under the terms of the Eclipse Public License v1.0 + * and Eclipse Distribution License v1.0 which accompany this distribution. + * + * The Eclipse Public License is available at + * http://www.eclipse.org/legal/epl-v10.html + * and the Eclipse Distribution License is available at + * http://www.eclipse.org/org/documents/edl-v10.php. + * + * Contributors: + * Ian Craggs - initial API and implementation and/or initial documentation + *******************************************************************************/ + +#if !defined(MQTTFORMAT_H) +#define MQTTFORMAT_H + +#include "StackTrace.h" +#include "MQTTPacket.h" + +const char* MQTTPacket_getName(unsigned short packetid); +int MQTTStringFormat_connect(char* strbuf, int strbuflen, MQTTPacket_connectData* data); +int MQTTStringFormat_connack(char* strbuf, int strbuflen, unsigned char connack_rc, unsigned char sessionPresent); +int MQTTStringFormat_publish(char* strbuf, int strbuflen, unsigned char dup, int qos, unsigned char retained, + unsigned short packetid, MQTTString topicName, unsigned char* payload, int payloadlen); +int MQTTStringFormat_ack(char* strbuf, int strbuflen, unsigned char packettype, unsigned char dup, unsigned short packetid); +int MQTTStringFormat_subscribe(char* strbuf, int strbuflen, unsigned char dup, unsigned short packetid, int count, + MQTTString topicFilters[], int requestedQoSs[]); +int MQTTStringFormat_suback(char* strbuf, int strbuflen, unsigned short packetid, int count, int* grantedQoSs); +int MQTTStringFormat_unsubscribe(char* strbuf, int strbuflen, unsigned char dup, unsigned short packetid, + int count, MQTTString topicFilters[]); +char* MQTTFormat_toClientString(char* strbuf, int strbuflen, unsigned char* buf, int buflen); +char* MQTTFormat_toServerString(char* strbuf, int strbuflen, unsigned char* buf, int buflen); + +#endif diff --git a/src/task_manager/include/paho_mqtt_3c/MQTTPacket.h b/src/task_manager/include/paho_mqtt_3c/MQTTPacket.h new file mode 100644 index 0000000..350886e --- /dev/null +++ b/src/task_manager/include/paho_mqtt_3c/MQTTPacket.h @@ -0,0 +1,270 @@ +/******************************************************************************* + * Copyright (c) 2009, 2018 IBM Corp. + * + * All rights reserved. This program and the accompanying materials + * are made available under the terms of the Eclipse Public License v1.0 + * and Eclipse Distribution License v1.0 which accompany this distribution. + * + * The Eclipse Public License is available at + * http://www.eclipse.org/legal/epl-v10.html + * and the Eclipse Distribution License is available at + * http://www.eclipse.org/org/documents/edl-v10.php. + * + * Contributors: + * Ian Craggs - initial API and implementation and/or initial documentation + * Ian Craggs, Allan Stockdill-Mander - SSL updates + * Ian Craggs - MQTT 3.1.1 support + * Ian Craggs - big endian Linux reversed definition + * Ian Craggs - MQTT 5.0 support + *******************************************************************************/ + +#if !defined(MQTTPACKET_H) +#define MQTTPACKET_H + +#include "Socket.h" +#if defined(OPENSSL) +#include "SSLSocket.h" +#endif +#include "LinkedList.h" +#include "Clients.h" + +typedef unsigned int bool; +typedef void* (*pf)(int, unsigned char, char*, size_t); + +#include "MQTTProperties.h" +#include "MQTTReasonCodes.h" + +enum errors +{ + MQTTPACKET_BAD = -4, + MQTTPACKET_BUFFER_TOO_SHORT = -2, + MQTTPACKET_READ_ERROR = -1, + MQTTPACKET_READ_COMPLETE +}; + + +enum msgTypes +{ + CONNECT = 1, CONNACK, PUBLISH, PUBACK, PUBREC, PUBREL, + PUBCOMP, SUBSCRIBE, SUBACK, UNSUBSCRIBE, UNSUBACK, + PINGREQ, PINGRESP, DISCONNECT, AUTH +}; + +#if defined(__linux__) +#include +#if __BYTE_ORDER == __BIG_ENDIAN + #define REVERSED 1 +#endif +#endif + +/** + * Bitfields for the MQTT header byte. + */ +typedef union +{ + /*unsigned*/ char byte; /**< the whole byte */ +#if defined(REVERSED) + struct + { + unsigned int type : 4; /**< message type nibble */ + bool dup : 1; /**< DUP flag bit */ + unsigned int qos : 2; /**< QoS value, 0, 1 or 2 */ + bool retain : 1; /**< retained flag bit */ + } bits; +#else + struct + { + bool retain : 1; /**< retained flag bit */ + unsigned int qos : 2; /**< QoS value, 0, 1 or 2 */ + bool dup : 1; /**< DUP flag bit */ + unsigned int type : 4; /**< message type nibble */ + } bits; +#endif +} Header; + + +/** + * Data for a connect packet. + */ +typedef struct +{ + Header header; /**< MQTT header byte */ + union + { + unsigned char all; /**< all connect flags */ +#if defined(REVERSED) + struct + { + bool username : 1; /**< 3.1 user name */ + bool password : 1; /**< 3.1 password */ + bool willRetain : 1; /**< will retain setting */ + unsigned int willQoS : 2; /**< will QoS value */ + bool will : 1; /**< will flag */ + bool cleanstart : 1; /**< cleansession flag */ + int : 1; /**< unused */ + } bits; +#else + struct + { + int : 1; /**< unused */ + bool cleanstart : 1; /**< cleansession flag */ + bool will : 1; /**< will flag */ + unsigned int willQoS : 2; /**< will QoS value */ + bool willRetain : 1; /**< will retain setting */ + bool password : 1; /**< 3.1 password */ + bool username : 1; /**< 3.1 user name */ + } bits; +#endif + } flags; /**< connect flags byte */ + + char *Protocol, /**< MQTT protocol name */ + *clientID, /**< string client id */ + *willTopic, /**< will topic */ + *willMsg; /**< will payload */ + + int keepAliveTimer; /**< keepalive timeout value in seconds */ + unsigned char version; /**< MQTT version number */ +} Connect; + + +/** + * Data for a connack packet. + */ +typedef struct +{ + Header header; /**< MQTT header byte */ + union + { + unsigned char all; /**< all connack flags */ +#if defined(REVERSED) + struct + { + unsigned int reserved : 7; /**< message type nibble */ + bool sessionPresent : 1; /**< was a session found on the server? */ + } bits; +#else + struct + { + bool sessionPresent : 1; /**< was a session found on the server? */ + unsigned int reserved : 7; /**< message type nibble */ + } bits; +#endif + } flags; /**< connack flags byte */ + unsigned char rc; /**< connack reason code */ + unsigned int MQTTVersion; /**< the version of MQTT */ + MQTTProperties properties; /**< MQTT 5.0 properties. Not used for MQTT < 5.0 */ +} Connack; + + +/** + * Data for a packet with header only. + */ +typedef struct +{ + Header header; /**< MQTT header byte */ +} MQTTPacket; + + +/** + * Data for a suback packet. + */ +typedef struct +{ + Header header; /**< MQTT header byte */ + int msgId; /**< MQTT message id */ + int MQTTVersion; /**< the version of MQTT */ + MQTTProperties properties; /**< MQTT 5.0 properties. Not used for MQTT < 5.0 */ + List* qoss; /**< list of granted QoSs (MQTT 3/4) / reason codes (MQTT 5) */ +} Suback; + + +/** + * Data for an MQTT V5 unsuback packet. + */ +typedef struct +{ + Header header; /**< MQTT header byte */ + int msgId; /**< MQTT message id */ + int MQTTVersion; /**< the version of MQTT */ + MQTTProperties properties; /**< MQTT 5.0 properties. Not used for MQTT < 5.0 */ + List* reasonCodes; /**< list of reason codes */ +} Unsuback; + + +/** + * Data for a publish packet. + */ +typedef struct +{ + Header header; /**< MQTT header byte */ + char* topic; /**< topic string */ + int topiclen; + int msgId; /**< MQTT message id */ + char* payload; /**< binary payload, length delimited */ + int payloadlen; /**< payload length */ + int MQTTVersion; /**< the version of MQTT */ + MQTTProperties properties; /**< MQTT 5.0 properties. Not used for MQTT < 5.0 */ +} Publish; + + +/** + * Data for one of the ack packets. + */ +typedef struct +{ + Header header; /**< MQTT header byte */ + int msgId; /**< MQTT message id */ + unsigned char rc; /**< MQTT 5 reason code */ + int MQTTVersion; /**< the version of MQTT */ + MQTTProperties properties; /**< MQTT 5.0 properties. Not used for MQTT < 5.0 */ +} Ack; + +typedef Ack Puback; +typedef Ack Pubrec; +typedef Ack Pubrel; +typedef Ack Pubcomp; + +int MQTTPacket_encode(char* buf, size_t length); +int MQTTPacket_decode(networkHandles* net, size_t* value); +int readInt(char** pptr); +char* readUTF(char** pptr, char* enddata); +unsigned char readChar(char** pptr); +void writeChar(char** pptr, char c); +void writeInt(char** pptr, int anInt); +void writeUTF(char** pptr, const char* string); +void writeData(char** pptr, const void* data, int datalen); + +const char* MQTTPacket_name(int ptype); + +void* MQTTPacket_Factory(int MQTTVersion, networkHandles* net, int* error); +int MQTTPacket_send(networkHandles* net, Header header, char* buffer, size_t buflen, int free, int MQTTVersion); +int MQTTPacket_sends(networkHandles* net, Header header, int count, char** buffers, size_t* buflens, int* frees, int MQTTVersion); + +void* MQTTPacket_header_only(int MQTTVersion, unsigned char aHeader, char* data, size_t datalen); +int MQTTPacket_send_disconnect(Clients* client, enum MQTTReasonCodes reason, MQTTProperties* props); + +void* MQTTPacket_publish(int MQTTVersion, unsigned char aHeader, char* data, size_t datalen); +void MQTTPacket_freePublish(Publish* pack); +int MQTTPacket_send_publish(Publish* pack, int dup, int qos, int retained, networkHandles* net, const char* clientID); +int MQTTPacket_send_puback(int msgid, networkHandles* net, const char* clientID); +void* MQTTPacket_ack(int MQTTVersion, unsigned char aHeader, char* data, size_t datalen); + +void MQTTPacket_freeAck(Ack* pack); +void MQTTPacket_freeSuback(Suback* pack); +void MQTTPacket_freeUnsuback(Unsuback* pack); +int MQTTPacket_send_pubrec(int msgid, networkHandles* net, const char* clientID); +int MQTTPacket_send_pubrel(int msgid, int dup, networkHandles* net, const char* clientID); +int MQTTPacket_send_pubcomp(int msgid, networkHandles* net, const char* clientID); + +void MQTTPacket_free_packet(MQTTPacket* pack); + +void writeInt4(char** pptr, int anInt); +int readInt4(char** pptr); +void writeMQTTLenString(char** pptr, MQTTLenString lenstring); +int MQTTLenStringRead(MQTTLenString* lenstring, char** pptr, char* enddata); +int MQTTPacket_VBIlen(int rem_len); +int MQTTPacket_decodeBuf(char* buf, int* value); + +#include "MQTTPacketOut.h" + +#endif /* MQTTPACKET_H */ diff --git a/src/task_manager/include/paho_mqtt_3c/MQTTPacketOut.h b/src/task_manager/include/paho_mqtt_3c/MQTTPacketOut.h new file mode 100644 index 0000000..4e0ae10 --- /dev/null +++ b/src/task_manager/include/paho_mqtt_3c/MQTTPacketOut.h @@ -0,0 +1,39 @@ +/******************************************************************************* + * Copyright (c) 2009, 2018 IBM Corp. + * + * All rights reserved. This program and the accompanying materials + * are made available under the terms of the Eclipse Public License v1.0 + * and Eclipse Distribution License v1.0 which accompany this distribution. + * + * The Eclipse Public License is available at + * http://www.eclipse.org/legal/epl-v10.html + * and the Eclipse Distribution License is available at + * http://www.eclipse.org/org/documents/edl-v10.php. + * + * Contributors: + * Ian Craggs - initial API and implementation and/or initial documentation + * Ian Craggs, Allan Stockdill-Mander - SSL updates + * Ian Craggs - MQTT 3.1.1 support + * Ian Craggs - MQTT 5.0 support + *******************************************************************************/ + +#if !defined(MQTTPACKETOUT_H) +#define MQTTPACKETOUT_H + +#include "MQTTPacket.h" + +int MQTTPacket_send_connect(Clients* client, int MQTTVersion, + MQTTProperties* connectProperties, MQTTProperties* willProperties); +void* MQTTPacket_connack(int MQTTVersion, unsigned char aHeader, char* data, size_t datalen); +void MQTTPacket_freeConnack(Connack* pack); + +int MQTTPacket_send_pingreq(networkHandles* net, const char* clientID); + +int MQTTPacket_send_subscribe(List* topics, List* qoss, MQTTSubscribe_options* opts, MQTTProperties* props, + int msgid, int dup, Clients* client); +void* MQTTPacket_suback(int MQTTVersion, unsigned char aHeader, char* data, size_t datalen); + +int MQTTPacket_send_unsubscribe(List* topics, MQTTProperties* props, int msgid, int dup, Clients* client); +void* MQTTPacket_unsuback(int MQTTVersion, unsigned char aHeader, char* data, size_t datalen); + +#endif diff --git a/src/task_manager/include/paho_mqtt_3c/MQTTPersistence.h b/src/task_manager/include/paho_mqtt_3c/MQTTPersistence.h new file mode 100644 index 0000000..93edb2b --- /dev/null +++ b/src/task_manager/include/paho_mqtt_3c/MQTTPersistence.h @@ -0,0 +1,94 @@ +/******************************************************************************* + * Copyright (c) 2009, 2018 IBM Corp. + * + * All rights reserved. This program and the accompanying materials + * are made available under the terms of the Eclipse Public License v1.0 + * and Eclipse Distribution License v1.0 which accompany this distribution. + * + * The Eclipse Public License is available at + * http://www.eclipse.org/legal/epl-v10.html + * and the Eclipse Distribution License is available at + * http://www.eclipse.org/org/documents/edl-v10.php. + * + * Contributors: + * Ian Craggs - initial API and implementation and/or initial documentation + * Ian Craggs - async client updates + * Ian Craggs - fix for bug 432903 - queue persistence + * Ian Craggs - MQTT V5 updates + *******************************************************************************/ + +#if !defined(MQTTPERSISTENCE_H) +#define MQTTPERSISTENCE_H + +#if defined(__cplusplus) + extern "C" { +#endif + +#include "Clients.h" +#include "MQTTProperties.h" + +/** Stem of the key for a sent PUBLISH QoS1 or QoS2 */ +#define PERSISTENCE_PUBLISH_SENT "s-" +/** Stem of the key for a sent PUBREL */ +#define PERSISTENCE_PUBREL "sc-" +/** Stem of the key for a received PUBLISH QoS2 */ +#define PERSISTENCE_PUBLISH_RECEIVED "r-" + +/** Stem of the key for a sent MQTT V5 PUBLISH QoS1 or QoS2 */ +#define PERSISTENCE_V5_PUBLISH_SENT "s5-" +/** Stem of the key for a sent MQTT V5 PUBREL */ +#define PERSISTENCE_V5_PUBREL "sc5-" +/** Stem of the key for a received MQTT V5 PUBLISH QoS2 */ +#define PERSISTENCE_V5_PUBLISH_RECEIVED "r5-" + +/** Stem of the key for an async client command */ +#define PERSISTENCE_COMMAND_KEY "c-" +/** Stem of the key for an MQTT V5 async client command */ +#define PERSISTENCE_V5_COMMAND_KEY "c-" +/** Stem of the key for an async client message queue */ +#define PERSISTENCE_QUEUE_KEY "q-" +/** Stem of the key for an MQTT V5 message queue */ +#define PERSISTENCE_V5_QUEUE_KEY "q5-" +#define PERSISTENCE_MAX_KEY_LENGTH 8 + +int MQTTPersistence_create(MQTTClient_persistence** per, int type, void* pcontext); +int MQTTPersistence_initialize(Clients* c, const char* serverURI); +int MQTTPersistence_close(Clients* c); +int MQTTPersistence_clear(Clients* c); +int MQTTPersistence_restore(Clients* c); +void* MQTTPersistence_restorePacket(int MQTTVersion, char* buffer, size_t buflen); +void MQTTPersistence_insertInOrder(List* list, void* content, size_t size); +int MQTTPersistence_put(int socket, char* buf0, size_t buf0len, int count, + char** buffers, size_t* buflens, int htype, int msgId, int scr, int MQTTVersion); +int MQTTPersistence_remove(Clients* c, char* type, int qos, int msgId); +void MQTTPersistence_wrapMsgID(Clients *c); + +typedef struct +{ + char struct_id[4]; + int struct_version; + int payloadlen; + void* payload; + int qos; + int retained; + int dup; + int msgid; + MQTTProperties properties; +} MQTTPersistence_message; + +typedef struct +{ + MQTTPersistence_message* msg; + char* topicName; + int topicLen; + unsigned int seqno; /* only used on restore */ +} MQTTPersistence_qEntry; + +int MQTTPersistence_unpersistQueueEntry(Clients* client, MQTTPersistence_qEntry* qe); +int MQTTPersistence_persistQueueEntry(Clients* aclient, MQTTPersistence_qEntry* qe); +int MQTTPersistence_restoreMessageQueue(Clients* c); +#ifdef __cplusplus + } +#endif + +#endif diff --git a/src/task_manager/include/paho_mqtt_3c/MQTTPersistenceDefault.h b/src/task_manager/include/paho_mqtt_3c/MQTTPersistenceDefault.h new file mode 100644 index 0000000..9c1034c --- /dev/null +++ b/src/task_manager/include/paho_mqtt_3c/MQTTPersistenceDefault.h @@ -0,0 +1,38 @@ +/******************************************************************************* + * Copyright (c) 2009, 2018 IBM Corp. + * + * All rights reserved. This program and the accompanying materials + * are made available under the terms of the Eclipse Public License v1.0 + * and Eclipse Distribution License v1.0 which accompany this distribution. + * + * The Eclipse Public License is available at + * http://www.eclipse.org/legal/epl-v10.html + * and the Eclipse Distribution License is available at + * http://www.eclipse.org/org/documents/edl-v10.php. + * + * Contributors: + * Ian Craggs - initial API and implementation and/or initial documentation + *******************************************************************************/ + +#if !defined(MQTTPERSISTENCEDEFAULT_H) +#define MQTTPERSISTENCEDEFAULT_H + +/** 8.3 filesystem */ +#define MESSAGE_FILENAME_LENGTH 8 +/** Extension of the filename */ +#define MESSAGE_FILENAME_EXTENSION ".msg" + +/* prototypes of the functions for the default file system persistence */ +int pstopen(void** handle, const char* clientID, const char* serverURI, void* context); +int pstclose(void* handle); +int pstput(void* handle, char* key, int bufcount, char* buffers[], int buflens[]); +int pstget(void* handle, char* key, char** buffer, int* buflen); +int pstremove(void* handle, char* key); +int pstkeys(void* handle, char*** keys, int* nkeys); +int pstclear(void* handle); +int pstcontainskey(void* handle, char* key); + +int pstmkdir(char *pPathname); + +#endif + diff --git a/src/task_manager/include/paho_mqtt_3c/MQTTProperties.h b/src/task_manager/include/paho_mqtt_3c/MQTTProperties.h new file mode 100644 index 0000000..deec124 --- /dev/null +++ b/src/task_manager/include/paho_mqtt_3c/MQTTProperties.h @@ -0,0 +1,225 @@ +/******************************************************************************* + * Copyright (c) 2017, 2018 IBM Corp. + * + * All rights reserved. This program and the accompanying materials + * are made available under the terms of the Eclipse Public License v1.0 + * and Eclipse Distribution License v1.0 which accompany this distribution. + * + * The Eclipse Public License is available at + * http://www.eclipse.org/legal/epl-v10.html + * and the Eclipse Distribution License is available at + * http://www.eclipse.org/org/documents/edl-v10.php. + * + * Contributors: + * Ian Craggs - initial API and implementation and/or initial documentation + *******************************************************************************/ + +#if !defined(MQTTPROPERTIES_H) +#define MQTTPROPERTIES_H + +#define MQTT_INVALID_PROPERTY_ID -2 + +/** The one byte MQTT V5 property indicator */ +enum MQTTPropertyCodes { + MQTTPROPERTY_CODE_PAYLOAD_FORMAT_INDICATOR = 1, /**< The value is 1 */ + MQTTPROPERTY_CODE_MESSAGE_EXPIRY_INTERVAL = 2, /**< The value is 2 */ + MQTTPROPERTY_CODE_CONTENT_TYPE = 3, /**< The value is 3 */ + MQTTPROPERTY_CODE_RESPONSE_TOPIC = 8, /**< The value is 8 */ + MQTTPROPERTY_CODE_CORRELATION_DATA = 9, /**< The value is 9 */ + MQTTPROPERTY_CODE_SUBSCRIPTION_IDENTIFIER = 11, /**< The value is 11 */ + MQTTPROPERTY_CODE_SESSION_EXPIRY_INTERVAL = 17, /**< The value is 17 */ + MQTTPROPERTY_CODE_ASSIGNED_CLIENT_IDENTIFER = 18,/**< The value is 18 */ + MQTTPROPERTY_CODE_SERVER_KEEP_ALIVE = 19, /**< The value is 19 */ + MQTTPROPERTY_CODE_AUTHENTICATION_METHOD = 21, /**< The value is 21 */ + MQTTPROPERTY_CODE_AUTHENTICATION_DATA = 22, /**< The value is 22 */ + MQTTPROPERTY_CODE_REQUEST_PROBLEM_INFORMATION = 23,/**< The value is 23 */ + MQTTPROPERTY_CODE_WILL_DELAY_INTERVAL = 24, /**< The value is 24 */ + MQTTPROPERTY_CODE_REQUEST_RESPONSE_INFORMATION = 25,/**< The value is 25 */ + MQTTPROPERTY_CODE_RESPONSE_INFORMATION = 26, /**< The value is 26 */ + MQTTPROPERTY_CODE_SERVER_REFERENCE = 28, /**< The value is 28 */ + MQTTPROPERTY_CODE_REASON_STRING = 31, /**< The value is 31 */ + MQTTPROPERTY_CODE_RECEIVE_MAXIMUM = 33, /**< The value is 33*/ + MQTTPROPERTY_CODE_TOPIC_ALIAS_MAXIMUM = 34, /**< The value is 34 */ + MQTTPROPERTY_CODE_TOPIC_ALIAS = 35, /**< The value is 35 */ + MQTTPROPERTY_CODE_MAXIMUM_QOS = 36, /**< The value is 36 */ + MQTTPROPERTY_CODE_RETAIN_AVAILABLE = 37, /**< The value is 37 */ + MQTTPROPERTY_CODE_USER_PROPERTY = 38, /**< The value is 38 */ + MQTTPROPERTY_CODE_MAXIMUM_PACKET_SIZE = 39, /**< The value is 39 */ + MQTTPROPERTY_CODE_WILDCARD_SUBSCRIPTION_AVAILABLE = 40,/**< The value is 40 */ + MQTTPROPERTY_CODE_SUBSCRIPTION_IDENTIFIERS_AVAILABLE = 41,/**< The value is 41 */ + MQTTPROPERTY_CODE_SHARED_SUBSCRIPTION_AVAILABLE = 42/**< The value is 241 */ +}; + +#if defined(WIN32) || defined(WIN64) + #define DLLImport __declspec(dllimport) + #define DLLExport __declspec(dllexport) +#else + #define DLLImport extern + #define DLLExport __attribute__ ((visibility ("default"))) +#endif + +/** + * Returns a printable string description of an MQTT V5 property code. + * @param value an MQTT V5 property code. + * @return the printable string description of the input property code. + * NULL if the code was not found. + */ +DLLExport const char* MQTTPropertyName(enum MQTTPropertyCodes value); + +/** The one byte MQTT V5 property type */ +enum MQTTPropertyTypes { + MQTTPROPERTY_TYPE_BYTE, + MQTTPROPERTY_TYPE_TWO_BYTE_INTEGER, + MQTTPROPERTY_TYPE_FOUR_BYTE_INTEGER, + MQTTPROPERTY_TYPE_VARIABLE_BYTE_INTEGER, + MQTTPROPERTY_TYPE_BINARY_DATA, + MQTTPROPERTY_TYPE_UTF_8_ENCODED_STRING, + MQTTPROPERTY_TYPE_UTF_8_STRING_PAIR +}; + +/** + * Returns the MQTT V5 type code of an MQTT V5 property. + * @param value an MQTT V5 property code. + * @return the MQTT V5 type code of the input property. -1 if the code was not found. + */ +DLLExport int MQTTProperty_getType(enum MQTTPropertyCodes value); + +/** + * The data for a length delimited string + */ +typedef struct +{ + int len; /**< the length of the string */ + char* data; /**< pointer to the string data */ +} MQTTLenString; + + +/** + * Structure to hold an MQTT version 5 property of any type + */ +typedef struct +{ + enum MQTTPropertyCodes identifier; /**< The MQTT V5 property id. A multi-byte integer. */ + /** The value of the property, as a union of the different possible types. */ + union { + char byte; /**< holds the value of a byte property type */ + short integer2; /**< holds the value of a 2 byte integer property type */ + int integer4; /**< holds the value of a 4 byte integer property type */ + struct { + MQTTLenString data; /**< The value of a string property, or the name of a user property. */ + MQTTLenString value; /**< The value of a user property. */ + }; + } value; +} MQTTProperty; + +/** + * MQTT version 5 property list + */ +typedef struct MQTTProperties +{ + int count; /**< number of property entries in the array */ + int max_count; /**< max number of properties that the currently allocated array can store */ + int length; /**< mbi: byte length of all properties */ + MQTTProperty *array; /**< array of properties */ +} MQTTProperties; + +#define MQTTProperties_initializer {0, 0, 0, NULL} + +/** + * Returns the length of the properties structure when serialized ready for network transmission. + * @param props an MQTT V5 property structure. + * @return the length in bytes of the properties when serialized. + */ +int MQTTProperties_len(MQTTProperties* props); + +/** + * Add a property pointer to the property array. There is no memory allocation. + * @param props The property list to add the property to. + * @param prop The property to add to the list. + * @return 0 on success, -1 on failure. + */ +DLLExport int MQTTProperties_add(MQTTProperties* props, const MQTTProperty* prop); + +/** + * Serialize the given property list to a character buffer, e.g. for writing to the network. + * @param pptr pointer to the buffer - move the pointer as we add data + * @param properties pointer to the property list, can be NULL + * @return whether the write succeeded or not: number of bytes written, or < 0 on failure. + */ +int MQTTProperties_write(char** pptr, const MQTTProperties* properties); + +/** + * Reads a property list from a character buffer into an array. + * @param properties pointer to the property list to be filled. Should be initalized but empty. + * @param pptr pointer to the character buffer. + * @param enddata pointer to the end of the character buffer so we don't read beyond. + * @return 1 if the properties were read successfully. + */ +int MQTTProperties_read(MQTTProperties* properties, char** pptr, char* enddata); + +/** + * Free all memory allocated to the property list, including any to individual properties. + * @param properties pointer to the property list. + */ +DLLExport void MQTTProperties_free(MQTTProperties* properties); + +/** + * Copy the contents of a property list, allocating additional memory if needed. + * @param props pointer to the property list. + * @return the duplicated property list. + */ +DLLExport MQTTProperties MQTTProperties_copy(const MQTTProperties* props); + +/** + * Checks if property list contains a specific property. + * @param props pointer to the property list. + * @param propid the property id to check for. + * @return 1 if found, 0 if not. + */ +DLLExport int MQTTProperties_hasProperty(MQTTProperties *props, enum MQTTPropertyCodes propid); + +/** + * Returns the number of instances of a property id. Most properties can exist only once. + * User properties and subscription ids can exist more than once. + * @param props pointer to the property list. + * @param propid the property id to check for. + * @return the number of times found. Can be 0. + */ +DLLExport int MQTTProperties_propertyCount(MQTTProperties *props, enum MQTTPropertyCodes propid); + +/** + * Returns the integer value of a specific property. The property given must be a numeric type. + * @param props pointer to the property list. + * @param propid the property id to check for. + * @return the integer value of the property. -9999999 on failure. + */ +DLLExport int MQTTProperties_getNumericValue(MQTTProperties *props, enum MQTTPropertyCodes propid); + +/** + * Returns the integer value of a specific property when it's not the only instance. + * The property given must be a numeric type. + * @param props pointer to the property list. + * @param propid the property id to check for. + * @param index the instance number, starting at 0. + * @return the integer value of the property. -9999999 on failure. + */ +DLLExport int MQTTProperties_getNumericValueAt(MQTTProperties *props, enum MQTTPropertyCodes propid, int index); + +/** + * Returns a pointer to the property structure for a specific property. + * @param props pointer to the property list. + * @param propid the property id to check for. + * @return the pointer to the property structure if found. NULL if not found. + */ +DLLExport MQTTProperty* MQTTProperties_getProperty(MQTTProperties *props, enum MQTTPropertyCodes propid); + +/** + * Returns a pointer to the property structure for a specific property when it's not the only instance. + * @param props pointer to the property list. + * @param propid the property id to check for. + * @param index the instance number, starting at 0. + * @return the pointer to the property structure if found. NULL if not found. + */ +DLLExport MQTTProperty* MQTTProperties_getPropertyAt(MQTTProperties *props, enum MQTTPropertyCodes propid, int index); + +#endif /* MQTTPROPERTIES_H */ diff --git a/src/task_manager/include/paho_mqtt_3c/MQTTProtocol.h b/src/task_manager/include/paho_mqtt_3c/MQTTProtocol.h new file mode 100644 index 0000000..7478103 --- /dev/null +++ b/src/task_manager/include/paho_mqtt_3c/MQTTProtocol.h @@ -0,0 +1,46 @@ +/******************************************************************************* + * Copyright (c) 2009, 2014 IBM Corp. + * + * All rights reserved. This program and the accompanying materials + * are made available under the terms of the Eclipse Public License v1.0 + * and Eclipse Distribution License v1.0 which accompany this distribution. + * + * The Eclipse Public License is available at + * http://www.eclipse.org/legal/epl-v10.html + * and the Eclipse Distribution License is available at + * http://www.eclipse.org/org/documents/edl-v10.php. + * + * Contributors: + * Ian Craggs - initial API and implementation and/or initial documentation + * Ian Craggs - MQTT 3.1.1 updates + *******************************************************************************/ + +#if !defined(MQTTPROTOCOL_H) +#define MQTTPROTOCOL_H + +#include "LinkedList.h" +#include "MQTTPacket.h" +#include "Clients.h" + +#define MAX_MSG_ID 65535 +#define MAX_CLIENTID_LEN 65535 + +typedef struct +{ + int socket; + Publications* p; +} pending_write; + + +typedef struct +{ + List publications; + unsigned int msgs_received; + unsigned int msgs_sent; + List pending_writes; /* for qos 0 writes not complete */ +} MQTTProtocol; + + +#include "MQTTProtocolOut.h" + +#endif diff --git a/src/task_manager/include/paho_mqtt_3c/MQTTProtocolClient.h b/src/task_manager/include/paho_mqtt_3c/MQTTProtocolClient.h new file mode 100644 index 0000000..92b43a8 --- /dev/null +++ b/src/task_manager/include/paho_mqtt_3c/MQTTProtocolClient.h @@ -0,0 +1,60 @@ +/******************************************************************************* + * Copyright (c) 2009, 2017 IBM Corp. + * + * All rights reserved. This program and the accompanying materials + * are made available under the terms of the Eclipse Public License v1.0 + * and Eclipse Distribution License v1.0 which accompany this distribution. + * + * The Eclipse Public License is available at + * http://www.eclipse.org/legal/epl-v10.html + * and the Eclipse Distribution License is available at + * http://www.eclipse.org/org/documents/edl-v10.php. + * + * Contributors: + * Ian Craggs - initial API and implementation and/or initial documentation + * Ian Craggs, Allan Stockdill-Mander - SSL updates + * Ian Craggs - MQTT 3.1.1 updates + * Rong Xiang, Ian Craggs - C++ compatibility + * Ian Craggs - add debug definition of MQTTStrdup for when needed + *******************************************************************************/ + +#if !defined(MQTTPROTOCOLCLIENT_H) +#define MQTTPROTOCOLCLIENT_H + +#include "LinkedList.h" +#include "MQTTPacket.h" +#include "Log.h" +#include "MQTTProtocol.h" +#include "Messages.h" +#include "MQTTProperties.h" + +#define MAX_MSG_ID 65535 +#define MAX_CLIENTID_LEN 65535 + +int MQTTProtocol_startPublish(Clients* pubclient, Publish* publish, int qos, int retained, Messages** m); +Messages* MQTTProtocol_createMessage(Publish* publish, Messages** mm, int qos, int retained); +Publications* MQTTProtocol_storePublication(Publish* publish, int* len); +int messageIDCompare(void* a, void* b); +int MQTTProtocol_assignMsgId(Clients* client); +void MQTTProtocol_removePublication(Publications* p); +void Protocol_processPublication(Publish* publish, Clients* client); + +int MQTTProtocol_handlePublishes(void* pack, int sock); +int MQTTProtocol_handlePubacks(void* pack, int sock); +int MQTTProtocol_handlePubrecs(void* pack, int sock); +int MQTTProtocol_handlePubrels(void* pack, int sock); +int MQTTProtocol_handlePubcomps(void* pack, int sock); + +void MQTTProtocol_closeSession(Clients* c, int sendwill); +void MQTTProtocol_keepalive(time_t); +void MQTTProtocol_retry(time_t, int, int); +void MQTTProtocol_freeClient(Clients* client); +void MQTTProtocol_emptyMessageList(List* msgList); +void MQTTProtocol_freeMessageList(List* msgList); + +char* MQTTStrncpy(char *dest, const char* src, size_t num); +char* MQTTStrdup(const char* src); + +//#define MQTTStrdup(src) MQTTStrncpy(malloc(strlen(src)+1), src, strlen(src)+1) + +#endif diff --git a/src/task_manager/include/paho_mqtt_3c/MQTTProtocolOut.h b/src/task_manager/include/paho_mqtt_3c/MQTTProtocolOut.h new file mode 100644 index 0000000..e2c2645 --- /dev/null +++ b/src/task_manager/include/paho_mqtt_3c/MQTTProtocolOut.h @@ -0,0 +1,50 @@ +/******************************************************************************* + * Copyright (c) 2009, 2018 IBM Corp. + * + * All rights reserved. This program and the accompanying materials + * are made available under the terms of the Eclipse Public License v1.0 + * and Eclipse Distribution License v1.0 which accompany this distribution. + * + * The Eclipse Public License is available at + * http://www.eclipse.org/legal/epl-v10.html + * and the Eclipse Distribution License is available at + * http://www.eclipse.org/org/documents/edl-v10.php. + * + * Contributors: + * Ian Craggs - initial API and implementation and/or initial documentation + * Ian Craggs, Allan Stockdill-Mander - SSL updates + * Ian Craggs - MQTT 3.1.1 support + * Ian Craggs - SNI support + * Ian Craggs - MQTT 5.0 support + *******************************************************************************/ + +#if !defined(MQTTPROTOCOLOUT_H) +#define MQTTPROTOCOLOUT_H + +#include "LinkedList.h" +#include "MQTTPacket.h" +#include "Clients.h" +#include "Log.h" +#include "Messages.h" +#include "MQTTProtocol.h" +#include "MQTTProtocolClient.h" + +#define DEFAULT_PORT 1883 + +size_t MQTTProtocol_addressPort(const char* uri, int* port, const char **topic); +void MQTTProtocol_reconnect(const char* ip_address, Clients* client); +#if defined(OPENSSL) +int MQTTProtocol_connect(const char* ip_address, Clients* acClients, int ssl, int websocket, int MQTTVersion, + MQTTProperties* connectProperties, MQTTProperties* willProperties); +#else +int MQTTProtocol_connect(const char* ip_address, Clients* acClients, int websocket, int MQTTVersion, + MQTTProperties* connectProperties, MQTTProperties* willProperties); +#endif +int MQTTProtocol_handlePingresps(void* pack, int sock); +int MQTTProtocol_subscribe(Clients* client, List* topics, List* qoss, int msgID, + MQTTSubscribe_options* opts, MQTTProperties* props); +int MQTTProtocol_handleSubacks(void* pack, int sock); +int MQTTProtocol_unsubscribe(Clients* client, List* topics, int msgID, MQTTProperties* props); +int MQTTProtocol_handleUnsubacks(void* pack, int sock); + +#endif diff --git a/src/task_manager/include/paho_mqtt_3c/MQTTPublish.h b/src/task_manager/include/paho_mqtt_3c/MQTTPublish.h new file mode 100644 index 0000000..ebe479d --- /dev/null +++ b/src/task_manager/include/paho_mqtt_3c/MQTTPublish.h @@ -0,0 +1,38 @@ +/******************************************************************************* + * Copyright (c) 2014 IBM Corp. + * + * All rights reserved. This program and the accompanying materials + * are made available under the terms of the Eclipse Public License v1.0 + * and Eclipse Distribution License v1.0 which accompany this distribution. + * + * The Eclipse Public License is available at + * http://www.eclipse.org/legal/epl-v10.html + * and the Eclipse Distribution License is available at + * http://www.eclipse.org/org/documents/edl-v10.php. + * + * Contributors: + * Ian Craggs - initial API and implementation and/or initial documentation + * Xiang Rong - 442039 Add makefile to Embedded C client + *******************************************************************************/ + +#ifndef MQTTPUBLISH_H_ +#define MQTTPUBLISH_H_ + +#if !defined(DLLImport) + #define DLLImport +#endif +#if !defined(DLLExport) + #define DLLExport +#endif + +DLLExport int MQTTSerialize_publish(unsigned char* buf, int buflen, unsigned char dup, int qos, unsigned char retained, unsigned short packetid, + MQTTString topicName, unsigned char* payload, int payloadlen); + +DLLExport int MQTTDeserialize_publish(unsigned char* dup, int* qos, unsigned char* retained, unsigned short* packetid, MQTTString* topicName, + unsigned char** payload, int* payloadlen, unsigned char* buf, int len); + +DLLExport int MQTTSerialize_puback(unsigned char* buf, int buflen, unsigned short packetid); +DLLExport int MQTTSerialize_pubrel(unsigned char* buf, int buflen, unsigned char dup, unsigned short packetid); +DLLExport int MQTTSerialize_pubcomp(unsigned char* buf, int buflen, unsigned short packetid); + +#endif /* MQTTPUBLISH_H_ */ diff --git a/src/task_manager/include/paho_mqtt_3c/MQTTReasonCodes.h b/src/task_manager/include/paho_mqtt_3c/MQTTReasonCodes.h new file mode 100644 index 0000000..369543b --- /dev/null +++ b/src/task_manager/include/paho_mqtt_3c/MQTTReasonCodes.h @@ -0,0 +1,85 @@ +/******************************************************************************* + * Copyright (c) 2017, 2018 IBM Corp. + * + * All rights reserved. This program and the accompanying materials + * are made available under the terms of the Eclipse Public License v1.0 + * and Eclipse Distribution License v1.0 which accompany this distribution. + * + * The Eclipse Public License is available at + * http://www.eclipse.org/legal/epl-v10.html + * and the Eclipse Distribution License is available at + * http://www.eclipse.org/org/documents/edl-v10.php. + * + * Contributors: + * Ian Craggs - initial API and implementation and/or initial documentation + *******************************************************************************/ + +#if !defined(MQTTREASONCODES_H) +#define MQTTREASONCODES_H + +/** The MQTT V5 one byte reason code */ +enum MQTTReasonCodes { + MQTTREASONCODE_SUCCESS = 0, + MQTTREASONCODE_NORMAL_DISCONNECTION = 0, + MQTTREASONCODE_GRANTED_QOS_0 = 0, + MQTTREASONCODE_GRANTED_QOS_1 = 1, + MQTTREASONCODE_GRANTED_QOS_2 = 2, + MQTTREASONCODE_DISCONNECT_WITH_WILL_MESSAGE = 4, + MQTTREASONCODE_NO_MATCHING_SUBSCRIBERS = 16, + MQTTREASONCODE_NO_SUBSCRIPTION_FOUND = 17, + MQTTREASONCODE_CONTINUE_AUTHENTICATION = 24, + MQTTREASONCODE_RE_AUTHENTICATE = 25, + MQTTREASONCODE_UNSPECIFIED_ERROR = 128, + MQTTREASONCODE_MALFORMED_PACKET = 129, + MQTTREASONCODE_PROTOCOL_ERROR = 130, + MQTTREASONCODE_IMPLEMENTATION_SPECIFIC_ERROR = 131, + MQTTREASONCODE_UNSUPPORTED_PROTOCOL_VERSION = 132, + MQTTREASONCODE_CLIENT_IDENTIFIER_NOT_VALID = 133, + MQTTREASONCODE_BAD_USER_NAME_OR_PASSWORD = 134, + MQTTREASONCODE_NOT_AUTHORIZED = 135, + MQTTREASONCODE_SERVER_UNAVAILABLE = 136, + MQTTREASONCODE_SERVER_BUSY = 137, + MQTTREASONCODE_BANNED = 138, + MQTTREASONCODE_SERVER_SHUTTING_DOWN = 139, + MQTTREASONCODE_BAD_AUTHENTICATION_METHOD = 140, + MQTTREASONCODE_KEEP_ALIVE_TIMEOUT = 141, + MQTTREASONCODE_SESSION_TAKEN_OVER = 142, + MQTTREASONCODE_TOPIC_FILTER_INVALID = 143, + MQTTREASONCODE_TOPIC_NAME_INVALID = 144, + MQTTREASONCODE_PACKET_IDENTIFIER_IN_USE = 145, + MQTTREASONCODE_PACKET_IDENTIFIER_NOT_FOUND = 146, + MQTTREASONCODE_RECEIVE_MAXIMUM_EXCEEDED = 147, + MQTTREASONCODE_TOPIC_ALIAS_INVALID = 148, + MQTTREASONCODE_PACKET_TOO_LARGE = 149, + MQTTREASONCODE_MESSAGE_RATE_TOO_HIGH = 150, + MQTTREASONCODE_QUOTA_EXCEEDED = 151, + MQTTREASONCODE_ADMINISTRATIVE_ACTION = 152, + MQTTREASONCODE_PAYLOAD_FORMAT_INVALID = 153, + MQTTREASONCODE_RETAIN_NOT_SUPPORTED = 154, + MQTTREASONCODE_QOS_NOT_SUPPORTED = 155, + MQTTREASONCODE_USE_ANOTHER_SERVER = 156, + MQTTREASONCODE_SERVER_MOVED = 157, + MQTTREASONCODE_SHARED_SUBSCRIPTIONS_NOT_SUPPORTED = 158, + MQTTREASONCODE_CONNECTION_RATE_EXCEEDED = 159, + MQTTREASONCODE_MAXIMUM_CONNECT_TIME = 160, + MQTTREASONCODE_SUBSCRIPTION_IDENTIFIERS_NOT_SUPPORTED = 161, + MQTTREASONCODE_WILDCARD_SUBSCRIPTIONS_NOT_SUPPORTED = 162 +}; + +#if defined(WIN32) || defined(WIN64) + #define DLLImport __declspec(dllimport) + #define DLLExport __declspec(dllexport) +#else + #define DLLImport extern + #define DLLExport __attribute__ ((visibility ("default"))) +#endif + +/** + * Returns a printable string description of an MQTT V5 reason code. + * @param value an MQTT V5 reason code. + * @return the printable string description of the input reason code. + * NULL if the code was not found. + */ +DLLExport const char* MQTTReasonCode_toString(enum MQTTReasonCodes value); + +#endif diff --git a/src/task_manager/include/paho_mqtt_3c/MQTTSubscribe.h b/src/task_manager/include/paho_mqtt_3c/MQTTSubscribe.h new file mode 100644 index 0000000..aa91826 --- /dev/null +++ b/src/task_manager/include/paho_mqtt_3c/MQTTSubscribe.h @@ -0,0 +1,39 @@ +/******************************************************************************* + * Copyright (c) 2014 IBM Corp. + * + * All rights reserved. This program and the accompanying materials + * are made available under the terms of the Eclipse Public License v1.0 + * and Eclipse Distribution License v1.0 which accompany this distribution. + * + * The Eclipse Public License is available at + * http://www.eclipse.org/legal/epl-v10.html + * and the Eclipse Distribution License is available at + * http://www.eclipse.org/org/documents/edl-v10.php. + * + * Contributors: + * Ian Craggs - initial API and implementation and/or initial documentation + * Xiang Rong - 442039 Add makefile to Embedded C client + *******************************************************************************/ + +#ifndef MQTTSUBSCRIBE_H_ +#define MQTTSUBSCRIBE_H_ + +#if !defined(DLLImport) + #define DLLImport +#endif +#if !defined(DLLExport) + #define DLLExport +#endif + +DLLExport int MQTTSerialize_subscribe(unsigned char* buf, int buflen, unsigned char dup, unsigned short packetid, + int count, MQTTString topicFilters[], int requestedQoSs[]); + +DLLExport int MQTTDeserialize_subscribe(unsigned char* dup, unsigned short* packetid, + int maxcount, int* count, MQTTString topicFilters[], int requestedQoSs[], unsigned char* buf, int len); + +DLLExport int MQTTSerialize_suback(unsigned char* buf, int buflen, unsigned short packetid, int count, int* grantedQoSs); + +DLLExport int MQTTDeserialize_suback(unsigned short* packetid, int maxcount, int* count, int grantedQoSs[], unsigned char* buf, int len); + + +#endif /* MQTTSUBSCRIBE_H_ */ diff --git a/src/task_manager/include/paho_mqtt_3c/MQTTSubscribeOpts.h b/src/task_manager/include/paho_mqtt_3c/MQTTSubscribeOpts.h new file mode 100644 index 0000000..1ae4678 --- /dev/null +++ b/src/task_manager/include/paho_mqtt_3c/MQTTSubscribeOpts.h @@ -0,0 +1,46 @@ +/******************************************************************************* + * Copyright (c) 2018 IBM Corp. + * + * All rights reserved. This program and the accompanying materials + * are made available under the terms of the Eclipse Public License v1.0 + * and Eclipse Distribution License v1.0 which accompany this distribution. + * + * The Eclipse Public License is available at + * http://www.eclipse.org/legal/epl-v10.html + * and the Eclipse Distribution License is available at + * http://www.eclipse.org/org/documents/edl-v10.php. + * + * Contributors: + * Ian Craggs - initial API and implementation and/or initial documentation + *******************************************************************************/ + +#if !defined(SUBOPTS_H) +#define SUBOPTS_H + +/** The MQTT V5 subscribe options, apart from QoS which existed before V5. */ +typedef struct MQTTSubscribe_options +{ + /** The eyecatcher for this structure. Must be MQSO. */ + char struct_id[4]; + /** The version number of this structure. Must be 0. + */ + int struct_version; + /** To not receive our own publications, set to 1. + * 0 is the original MQTT behaviour - all messages matching the subscription are received. + */ + unsigned char noLocal; + /** To keep the retain flag as on the original publish message, set to 1. + * If 0, defaults to the original MQTT behaviour where the retain flag is only set on + * publications sent by a broker if in response to a subscribe request. + */ + unsigned char retainAsPublished; + /** 0 - send retained messages at the time of the subscribe (original MQTT behaviour) + * 1 - send retained messages on subscribe only if the subscription is new + * 2 - do not send retained messages at all + */ + unsigned char retainHandling; +} MQTTSubscribe_options; + +#define MQTTSubscribe_options_initializer { {'M', 'Q', 'S', 'O'}, 0, 0, 0, 0 } + +#endif diff --git a/src/task_manager/include/paho_mqtt_3c/MQTTUnsubscribe.h b/src/task_manager/include/paho_mqtt_3c/MQTTUnsubscribe.h new file mode 100644 index 0000000..355ca9a --- /dev/null +++ b/src/task_manager/include/paho_mqtt_3c/MQTTUnsubscribe.h @@ -0,0 +1,38 @@ +/******************************************************************************* + * Copyright (c) 2014 IBM Corp. + * + * All rights reserved. This program and the accompanying materials + * are made available under the terms of the Eclipse Public License v1.0 + * and Eclipse Distribution License v1.0 which accompany this distribution. + * + * The Eclipse Public License is available at + * http://www.eclipse.org/legal/epl-v10.html + * and the Eclipse Distribution License is available at + * http://www.eclipse.org/org/documents/edl-v10.php. + * + * Contributors: + * Ian Craggs - initial API and implementation and/or initial documentation + * Xiang Rong - 442039 Add makefile to Embedded C client + *******************************************************************************/ + +#ifndef MQTTUNSUBSCRIBE_H_ +#define MQTTUNSUBSCRIBE_H_ + +#if !defined(DLLImport) + #define DLLImport +#endif +#if !defined(DLLExport) + #define DLLExport +#endif + +DLLExport int MQTTSerialize_unsubscribe(unsigned char* buf, int buflen, unsigned char dup, unsigned short packetid, + int count, MQTTString topicFilters[]); + +DLLExport int MQTTDeserialize_unsubscribe(unsigned char* dup, unsigned short* packetid, int max_count, int* count, MQTTString topicFilters[], + unsigned char* buf, int len); + +DLLExport int MQTTSerialize_unsuback(unsigned char* buf, int buflen, unsigned short packetid); + +DLLExport int MQTTDeserialize_unsuback(unsigned short* packetid, unsigned char* buf, int len); + +#endif /* MQTTUNSUBSCRIBE_H_ */ diff --git a/src/task_manager/include/paho_mqtt_3c/Messages.h b/src/task_manager/include/paho_mqtt_3c/Messages.h new file mode 100644 index 0000000..08f292f --- /dev/null +++ b/src/task_manager/include/paho_mqtt_3c/Messages.h @@ -0,0 +1,24 @@ +/******************************************************************************* + * Copyright (c) 2009, 2013 IBM Corp. + * + * All rights reserved. This program and the accompanying materials + * are made available under the terms of the Eclipse Public License v1.0 + * and Eclipse Distribution License v1.0 which accompany this distribution. + * + * The Eclipse Public License is available at + * http://www.eclipse.org/legal/epl-v10.html + * and the Eclipse Distribution License is available at + * http://www.eclipse.org/org/documents/edl-v10.php. + * + * Contributors: + * Ian Craggs - initial API and implementation and/or initial documentation + *******************************************************************************/ + +#if !defined(MESSAGES_H) +#define MESSAGES_H + +#include "Log.h" + +const char* Messages_get(int, enum LOG_LEVELS); + +#endif diff --git a/src/task_manager/include/paho_mqtt_3c/OsWrapper.h b/src/task_manager/include/paho_mqtt_3c/OsWrapper.h new file mode 100644 index 0000000..f657ab1 --- /dev/null +++ b/src/task_manager/include/paho_mqtt_3c/OsWrapper.h @@ -0,0 +1,42 @@ +/******************************************************************************* + * Copyright (c) 2016, 2017 logi.cals GmbH + * + * All rights reserved. This program and the accompanying materials + * are made available under the terms of the Eclipse Public License v1.0 + * and Eclipse Distribution License v1.0 which accompany this distribution. + * + * The Eclipse Public License is available at + * http://www.eclipse.org/legal/epl-v10.html + * and the Eclipse Distribution License is available at + * http://www.eclipse.org/org/documents/edl-v10.php. + * + * Contributors: + * Gunter Raidl - timer support for VxWorks + * Rainer Poisel - reusability + *******************************************************************************/ + +#if !defined(OSWRAPPER_H) +#define OSWRAPPER_H + +#if defined(_WRS_KERNEL) +#include + +#define lstat stat + +typedef unsigned long useconds_t; +void usleep(useconds_t useconds); + +#define timersub(a, b, result) \ + do \ + { \ + (result)->tv_sec = (a)->tv_sec - (b)->tv_sec; \ + (result)->tv_usec = (a)->tv_usec - (b)->tv_usec; \ + if ((result)->tv_usec < 0) \ + { \ + --(result)->tv_sec; \ + (result)->tv_usec += 1000000L; \ + } \ + } while (0) +#endif /* defined(_WRS_KERNEL) */ + +#endif /* OSWRAPPER_H */ diff --git a/src/task_manager/include/paho_mqtt_3c/SHA1.h b/src/task_manager/include/paho_mqtt_3c/SHA1.h new file mode 100644 index 0000000..f8e8abd --- /dev/null +++ b/src/task_manager/include/paho_mqtt_3c/SHA1.h @@ -0,0 +1,91 @@ +/******************************************************************************* + * Copyright (c) 2018 Wind River Systems, Inc. All Rights Reserved. + * + * All rights reserved. This program and the accompanying materials + * are made available under the terms of the Eclipse Public License v1.0 + * and Eclipse Distribution License v1.0 which accompany this distribution. + * + * The Eclipse Public License is available at + * http://www.eclipse.org/legal/epl-v10.html + * and the Eclipse Distribution License is available at + * http://www.eclipse.org/org/documents/edl-v10.php. + * + * Contributors: + * Keith Holman - initial implementation and documentation + *******************************************************************************/ + +#if !defined(SHA1_H) +#define SHA1_H + +#if defined(OPENSSL) +#include + +/** SHA-1 Digest Length */ +#define SHA1_DIGEST_LENGTH SHA_DIGEST_LENGTH + +#else /* if defined(OPENSSL) */ + +#if defined(WIN32) || defined(WIN64) +#include +#include +typedef struct SHA_CTX_S +{ + HCRYPTPROV hProv; + HCRYPTHASH hHash; +} SHA_CTX; +#else /* if defined(WIN32) || defined(WIN64) */ + +#include +typedef struct SHA_CTX_S { + uint32_t h[5]; + union { + uint32_t w[16]; + uint8_t buffer[64]; + }; + unsigned int size; + unsigned int total; +} SHA_CTX; +#endif /* else if defined(WIN32) || defined(WIN64) */ + +#include + +/** SHA-1 Digest Length (number of bytes in SHA1) */ +#define SHA1_DIGEST_LENGTH (160/8) + +/** + * Initializes the SHA1 hashing algorithm + * + * @param[in,out] ctx hashing context structure + * + * @see SHA1_Update + * @see SHA1_Final + */ +int SHA1_Init(SHA_CTX *ctx); + +/** + * Updates a block to the SHA1 hash + * + * @param[in,out] ctx hashing context structure + * @param[in] data block of data to hash + * @param[in] len length of block to hash + * + * @see SHA1_Init + * @see SHA1_Final + */ +int SHA1_Update(SHA_CTX *ctx, const void *data, size_t len); + +/** + * Produce final SHA1 hash + * + * @param[out] md SHA1 hash produced (must be atleast + * @p SHA1_DIGEST_LENGTH in length) + * @param[in,out] ctx hashing context structure + * + * @see SHA1_Init + * @see SHA1_Final + */ +int SHA1_Final(unsigned char *md, SHA_CTX *ctx); + +#endif /* if defined(OPENSSL) */ +#endif /* SHA1_H */ + diff --git a/src/task_manager/include/paho_mqtt_3c/SSLSocket.h b/src/task_manager/include/paho_mqtt_3c/SSLSocket.h new file mode 100644 index 0000000..09547fd --- /dev/null +++ b/src/task_manager/include/paho_mqtt_3c/SSLSocket.h @@ -0,0 +1,52 @@ +/******************************************************************************* + * Copyright (c) 2009, 2018 IBM Corp. + * + * All rights reserved. This program and the accompanying materials + * are made available under the terms of the Eclipse Public License v1.0 + * and Eclipse Distribution License v1.0 which accompany this distribution. + * + * The Eclipse Public License is available at + * http://www.eclipse.org/legal/epl-v10.html + * and the Eclipse Distribution License is available at + * http://www.eclipse.org/org/documents/edl-v10.php. + * + * Contributors: + * Ian Craggs, Allan Stockdill-Mander - initial implementation + * Ian Craggs - SNI support + * Ian Craggs - post connect checks and CApath + *******************************************************************************/ +#if !defined(SSLSOCKET_H) +#define SSLSOCKET_H + +#if defined(WIN32) || defined(WIN64) + #define ssl_mutex_type HANDLE +#else + #include + #include + #define ssl_mutex_type pthread_mutex_t +#endif + +#include +#include "SocketBuffer.h" +#include "Clients.h" + +#define URI_SSL "ssl://" + +/** if we should handle openssl initialization (bool_value == 1) or depend on it to be initalized externally (bool_value == 0) */ +void SSLSocket_handleOpensslInit(int bool_value); + +int SSLSocket_initialize(void); +void SSLSocket_terminate(void); +int SSLSocket_setSocketForSSL(networkHandles* net, MQTTClient_SSLOptions* opts, const char* hostname, size_t hostname_len); + +int SSLSocket_getch(SSL* ssl, int socket, char* c); +char *SSLSocket_getdata(SSL* ssl, int socket, size_t bytes, size_t* actual_len); + +int SSLSocket_close(networkHandles* net); +int SSLSocket_putdatas(SSL* ssl, int socket, char* buf0, size_t buf0len, int count, char** buffers, size_t* buflens, int* frees); +int SSLSocket_connect(SSL* ssl, int sock, const char* hostname, int verify, int (*cb)(const char *str, size_t len, void *u), void* u); + +int SSLSocket_getPendingRead(void); +int SSLSocket_continueWrite(pending_writes* pw); + +#endif diff --git a/src/task_manager/include/paho_mqtt_3c/Socket.h b/src/task_manager/include/paho_mqtt_3c/Socket.h new file mode 100644 index 0000000..e042069 --- /dev/null +++ b/src/task_manager/include/paho_mqtt_3c/Socket.h @@ -0,0 +1,145 @@ +/******************************************************************************* + * Copyright (c) 2009, 2014 IBM Corp. + * + * All rights reserved. This program and the accompanying materials + * are made available under the terms of the Eclipse Public License v1.0 + * and Eclipse Distribution License v1.0 which accompany this distribution. + * + * The Eclipse Public License is available at + * http://www.eclipse.org/legal/epl-v10.html + * and the Eclipse Distribution License is available at + * http://www.eclipse.org/org/documents/edl-v10.php. + * + * Contributors: + * Ian Craggs - initial implementation and documentation + * Ian Craggs - async client updates + *******************************************************************************/ + +#if !defined(SOCKET_H) +#define SOCKET_H + +#include + +#if defined(WIN32) || defined(WIN64) +#include +#include +#define MAXHOSTNAMELEN 256 +#if !defined(SSLSOCKET_H) +#undef EAGAIN +#define EAGAIN WSAEWOULDBLOCK +#undef EINTR +#define EINTR WSAEINTR +#undef EINPROGRESS +#define EINPROGRESS WSAEINPROGRESS +#undef EWOULDBLOCK +#define EWOULDBLOCK WSAEWOULDBLOCK +#undef ENOTCONN +#define ENOTCONN WSAENOTCONN +#undef ECONNRESET +#define ECONNRESET WSAECONNRESET +#undef ETIMEDOUT +#define ETIMEDOUT WAIT_TIMEOUT +#endif +#define ioctl ioctlsocket +#define socklen_t int +#else +#define INVALID_SOCKET SOCKET_ERROR +#include +#if !defined(_WRS_KERNEL) +#include +#include +#include +#include +#else +#include +#endif +#include +#include +#include +#include +#include +#include +#include +#include +#include +#define ULONG size_t +#endif + +#include "mutex_type.h" /* Needed for mutex_type */ + +/** socket operation completed successfully */ +#define TCPSOCKET_COMPLETE 0 +#if !defined(SOCKET_ERROR) + /** error in socket operation */ + #define SOCKET_ERROR -1 +#endif +/** must be the same as SOCKETBUFFER_INTERRUPTED */ +#define TCPSOCKET_INTERRUPTED -22 +#define SSL_FATAL -3 + +#if !defined(INET6_ADDRSTRLEN) +#define INET6_ADDRSTRLEN 46 /** only needed for gcc/cygwin on windows */ +#endif + + +#if !defined(max) +#define max(A,B) ( (A) > (B) ? (A):(B)) +#endif + +#include "LinkedList.h" + +/*BE +def FD_SET +{ + 128 n8 "data" +} + +def SOCKETS +{ + FD_SET "rset" + FD_SET "rset_saved" + n32 dec "maxfdp1" + n32 ptr INTList "clientsds" + n32 ptr INTItem "cur_clientsds" + n32 ptr INTList "connect_pending" + n32 ptr INTList "write_pending" + FD_SET "pending_wset" +} +BE*/ + + +/** + * Structure to hold all socket data for the module + */ +typedef struct +{ + fd_set rset, /**< socket read set (see select doc) */ + rset_saved; /**< saved socket read set */ + int maxfdp1; /**< max descriptor used +1 (again see select doc) */ + List* clientsds; /**< list of client socket descriptors */ + ListElement* cur_clientsds; /**< current client socket descriptor (iterator) */ + List* connect_pending; /**< list of sockets for which a connect is pending */ + List* write_pending; /**< list of sockets for which a write is pending */ + fd_set pending_wset; /**< socket pending write set for select */ +} Sockets; + + +void Socket_outInitialize(void); +void Socket_outTerminate(void); +int Socket_getReadySocket(int more_work, struct timeval *tp, mutex_type mutex); +int Socket_getch(int socket, char* c); +char *Socket_getdata(int socket, size_t bytes, size_t* actual_len); +int Socket_putdatas(int socket, char* buf0, size_t buf0len, int count, char** buffers, size_t* buflens, int* frees); +void Socket_close(int socket); +int Socket_new(const char* addr, size_t addr_len, int port, int* socket); + +int Socket_noPendingWrites(int socket); +char* Socket_getpeer(int sock); + +void Socket_addPendingWrite(int socket); +void Socket_clearPendingWrite(int socket); + +typedef void Socket_writeComplete(int socket, int rc); +void Socket_setWriteCompleteCallback(Socket_writeComplete*); + +#endif /* SOCKET_H */ diff --git a/src/task_manager/include/paho_mqtt_3c/SocketBuffer.h b/src/task_manager/include/paho_mqtt_3c/SocketBuffer.h new file mode 100644 index 0000000..f7702dc --- /dev/null +++ b/src/task_manager/include/paho_mqtt_3c/SocketBuffer.h @@ -0,0 +1,84 @@ +/******************************************************************************* + * Copyright (c) 2009, 2014 IBM Corp. + * + * All rights reserved. This program and the accompanying materials + * are made available under the terms of the Eclipse Public License v1.0 + * and Eclipse Distribution License v1.0 which accompany this distribution. + * + * The Eclipse Public License is available at + * http://www.eclipse.org/legal/epl-v10.html + * and the Eclipse Distribution License is available at + * http://www.eclipse.org/org/documents/edl-v10.php. + * + * Contributors: + * Ian Craggs - initial API and implementation and/or initial documentation + * Ian Craggs, Allan Stockdill-Mander - SSL updates + *******************************************************************************/ + +#if !defined(SOCKETBUFFER_H) +#define SOCKETBUFFER_H + +#if defined(WIN32) || defined(WIN64) +#include +#else +#include +#endif + +#if defined(OPENSSL) +#include +#endif + +#if defined(WIN32) || defined(WIN64) + typedef WSABUF iobuf; +#else + typedef struct iovec iobuf; +#endif + +typedef struct +{ + int socket; + unsigned int index; + size_t headerlen; + char fixed_header[5]; /**< header plus up to 4 length bytes */ + size_t buflen, /**< total length of the buffer */ + datalen; /**< current length of data in buf */ + char* buf; +} socket_queue; + +typedef struct +{ + int socket, count; + size_t total; +#if defined(OPENSSL) + SSL* ssl; +#endif + size_t bytes; + iobuf iovecs[5]; + int frees[5]; +} pending_writes; + +#define SOCKETBUFFER_COMPLETE 0 +#if !defined(SOCKET_ERROR) + #define SOCKET_ERROR -1 +#endif +#define SOCKETBUFFER_INTERRUPTED -22 /* must be the same value as TCPSOCKET_INTERRUPTED */ + +void SocketBuffer_initialize(void); +void SocketBuffer_terminate(void); +void SocketBuffer_cleanup(int socket); +char* SocketBuffer_getQueuedData(int socket, size_t bytes, size_t* actual_len); +int SocketBuffer_getQueuedChar(int socket, char* c); +void SocketBuffer_interrupted(int socket, size_t actual_len); +char* SocketBuffer_complete(int socket); +void SocketBuffer_queueChar(int socket, char c); + +#if defined(OPENSSL) +void SocketBuffer_pendingWrite(int socket, SSL* ssl, int count, iobuf* iovecs, int* frees, size_t total, size_t bytes); +#else +void SocketBuffer_pendingWrite(int socket, int count, iobuf* iovecs, int* frees, size_t total, size_t bytes); +#endif +pending_writes* SocketBuffer_getWrite(int socket); +int SocketBuffer_writeComplete(int socket); +pending_writes* SocketBuffer_updateWrite(int socket, char* topic, char* payload); + +#endif diff --git a/src/task_manager/include/paho_mqtt_3c/StackTrace.h b/src/task_manager/include/paho_mqtt_3c/StackTrace.h new file mode 100644 index 0000000..21a0a64 --- /dev/null +++ b/src/task_manager/include/paho_mqtt_3c/StackTrace.h @@ -0,0 +1,71 @@ +/******************************************************************************* + * Copyright (c) 2009, 2017 IBM Corp. + * + * All rights reserved. This program and the accompanying materials + * are made available under the terms of the Eclipse Public License v1.0 + * and Eclipse Distribution License v1.0 which accompany this distribution. + * + * The Eclipse Public License is available at + * http://www.eclipse.org/legal/epl-v10.html + * and the Eclipse Distribution License is available at + * http://www.eclipse.org/org/documents/edl-v10.php. + * + * Contributors: + * Ian Craggs - initial API and implementation and/or initial documentation + *******************************************************************************/ + +#ifndef STACKTRACE_H_ +#define STACKTRACE_H_ + +#include +#include "Log.h" +#include "Thread.h" + +#if defined(NOSTACKTRACE) +#define FUNC_ENTRY +#define FUNC_ENTRY_NOLOG +#define FUNC_ENTRY_MED +#define FUNC_ENTRY_MAX +#define FUNC_EXIT +#define FUNC_EXIT_NOLOG +#define FUNC_EXIT_MED +#define FUNC_EXIT_MAX +#define FUNC_EXIT_RC(x) +#define FUNC_EXIT_MED_RC(x) +#define FUNC_EXIT_MAX_RC(x) +#else +#if defined(WIN32) || defined(WIN64) +#define inline __inline +#define FUNC_ENTRY StackTrace_entry(__FUNCTION__, __LINE__, TRACE_MINIMUM) +#define FUNC_ENTRY_NOLOG StackTrace_entry(__FUNCTION__, __LINE__, -1) +#define FUNC_ENTRY_MED StackTrace_entry(__FUNCTION__, __LINE__, TRACE_MEDIUM) +#define FUNC_ENTRY_MAX StackTrace_entry(__FUNCTION__, __LINE__, TRACE_MAXIMUM) +#define FUNC_EXIT StackTrace_exit(__FUNCTION__, __LINE__, NULL, TRACE_MINIMUM) +#define FUNC_EXIT_NOLOG StackTrace_exit(__FUNCTION__, __LINE__, -1) +#define FUNC_EXIT_MED StackTrace_exit(__FUNCTION__, __LINE__, NULL, TRACE_MEDIUM) +#define FUNC_EXIT_MAX StackTrace_exit(__FUNCTION__, __LINE__, NULL, TRACE_MAXIMUM) +#define FUNC_EXIT_RC(x) StackTrace_exit(__FUNCTION__, __LINE__, &x, TRACE_MINIMUM) +#define FUNC_EXIT_MED_RC(x) StackTrace_exit(__FUNCTION__, __LINE__, &x, TRACE_MEDIUM) +#define FUNC_EXIT_MAX_RC(x) StackTrace_exit(__FUNCTION__, __LINE__, &x, TRACE_MAXIMUM) +#else +#define FUNC_ENTRY StackTrace_entry(__func__, __LINE__, TRACE_MINIMUM) +#define FUNC_ENTRY_NOLOG StackTrace_entry(__func__, __LINE__, -1) +#define FUNC_ENTRY_MED StackTrace_entry(__func__, __LINE__, TRACE_MEDIUM) +#define FUNC_ENTRY_MAX StackTrace_entry(__func__, __LINE__, TRACE_MAXIMUM) +#define FUNC_EXIT StackTrace_exit(__func__, __LINE__, NULL, TRACE_MINIMUM) +#define FUNC_EXIT_NOLOG StackTrace_exit(__func__, __LINE__, NULL, -1) +#define FUNC_EXIT_MED StackTrace_exit(__func__, __LINE__, NULL, TRACE_MEDIUM) +#define FUNC_EXIT_MAX StackTrace_exit(__func__, __LINE__, NULL, TRACE_MAXIMUM) +#define FUNC_EXIT_RC(x) StackTrace_exit(__func__, __LINE__, &x, TRACE_MINIMUM) +#define FUNC_EXIT_MED_RC(x) StackTrace_exit(__func__, __LINE__, &x, TRACE_MEDIUM) +#define FUNC_EXIT_MAX_RC(x) StackTrace_exit(__func__, __LINE__, &x, TRACE_MAXIMUM) +#endif +#endif + +void StackTrace_entry(const char* name, int line, enum LOG_LEVELS trace); +void StackTrace_exit(const char* name, int line, void* return_value, enum LOG_LEVELS trace); + +void StackTrace_printStack(FILE* dest); +char* StackTrace_get(thread_id_type, char* buf, int bufsize); + +#endif /* STACKTRACE_H_ */ diff --git a/src/task_manager/include/paho_mqtt_3c/Thread.h b/src/task_manager/include/paho_mqtt_3c/Thread.h new file mode 100644 index 0000000..88fd008 --- /dev/null +++ b/src/task_manager/include/paho_mqtt_3c/Thread.h @@ -0,0 +1,75 @@ +/******************************************************************************* + * Copyright (c) 2009, 2018 IBM Corp. + * + * All rights reserved. This program and the accompanying materials + * are made available under the terms of the Eclipse Public License v1.0 + * and Eclipse Distribution License v1.0 which accompany this distribution. + * + * The Eclipse Public License is available at + * http://www.eclipse.org/legal/epl-v10.html + * and the Eclipse Distribution License is available at + * http://www.eclipse.org/org/documents/edl-v10.php. + * + * Contributors: + * Ian Craggs - initial implementation + * Ian Craggs, Allan Stockdill-Mander - async client updates + * Ian Craggs - fix for bug #420851 + * Ian Craggs - change MacOS semaphore implementation + *******************************************************************************/ +#include "MQTTClient.h" + +#if !defined(THREAD_H) +#define THREAD_H + +#include "mutex_type.h" /* Needed for mutex_type */ + +#if defined(WIN32) || defined(WIN64) + #include + #define thread_type HANDLE + #define thread_id_type DWORD + #define thread_return_type DWORD + #define thread_fn LPTHREAD_START_ROUTINE + #define cond_type HANDLE + #define sem_type HANDLE + #undef ETIMEDOUT + #define ETIMEDOUT WSAETIMEDOUT +#else + #include + + #define thread_type pthread_t + #define thread_id_type pthread_t + #define thread_return_type void* + typedef thread_return_type (*thread_fn)(void*); + typedef struct { pthread_cond_t cond; pthread_mutex_t mutex; } cond_type_struct; + typedef cond_type_struct *cond_type; + #if defined(OSX) + #include + typedef dispatch_semaphore_t sem_type; + #else + #include + typedef sem_t *sem_type; + #endif + + cond_type Thread_create_cond(void); + int Thread_signal_cond(cond_type); + int Thread_wait_cond(cond_type condvar, int timeout); + int Thread_destroy_cond(cond_type); +#endif + +DLLExport thread_type Thread_start(thread_fn, void*); + +DLLExport mutex_type Thread_create_mutex(); +DLLExport int Thread_lock_mutex(mutex_type); +DLLExport int Thread_unlock_mutex(mutex_type); +void Thread_destroy_mutex(mutex_type); + +DLLExport thread_id_type Thread_getid(); + +sem_type Thread_create_sem(void); +int Thread_wait_sem(sem_type sem, int timeout); +int Thread_check_sem(sem_type sem); +int Thread_post_sem(sem_type sem); +int Thread_destroy_sem(sem_type sem); + + +#endif diff --git a/src/task_manager/include/paho_mqtt_3c/Tree.h b/src/task_manager/include/paho_mqtt_3c/Tree.h new file mode 100644 index 0000000..bbbd014 --- /dev/null +++ b/src/task_manager/include/paho_mqtt_3c/Tree.h @@ -0,0 +1,115 @@ +/******************************************************************************* + * Copyright (c) 2009, 2013 IBM Corp. + * + * All rights reserved. This program and the accompanying materials + * are made available under the terms of the Eclipse Public License v1.0 + * and Eclipse Distribution License v1.0 which accompany this distribution. + * + * The Eclipse Public License is available at + * http://www.eclipse.org/legal/epl-v10.html + * and the Eclipse Distribution License is available at + * http://www.eclipse.org/org/documents/edl-v10.php. + * + * Contributors: + * Ian Craggs - initial implementation and documentation + *******************************************************************************/ + + +#if !defined(TREE_H) +#define TREE_H + +#include /* for size_t definition */ + +/*BE +defm defTree(T) // macro to define a tree + +def T concat Node +{ + n32 ptr T concat Node "parent" + n32 ptr T concat Node "left" + n32 ptr T concat Node "right" + n32 ptr T id2str(T) + n32 suppress "size" +} + + +def T concat Tree +{ + struct + { + n32 ptr T concat Node suppress "root" + n32 ptr DATA suppress "compare" + } + struct + { + n32 ptr T concat Node suppress "root" + n32 ptr DATA suppress "compare" + } + n32 dec "count" + n32 dec suppress "size" +} + +endm + +defTree(INT) +defTree(STRING) +defTree(TMP) + +BE*/ + +/** + * Structure to hold all data for one list element + */ +typedef struct NodeStruct +{ + struct NodeStruct *parent, /**< pointer to parent tree node, in case we need it */ + *child[2]; /**< pointers to child tree nodes 0 = left, 1 = right */ + void* content; /**< pointer to element content */ + size_t size; /**< size of content */ + unsigned int red : 1; +} Node; + + +/** + * Structure to hold all data for one tree + */ +typedef struct +{ + struct + { + Node *root; /**< root node pointer */ + int (*compare)(void*, void*, int); /**< comparison function */ + } index[2]; + int indexes, /**< no of indexes into tree */ + count; /**< no of items */ + size_t size; /**< heap storage used */ + unsigned int heap_tracking : 1; /**< switch on heap tracking for this tree? */ + unsigned int allow_duplicates : 1; /**< switch to allow duplicate entries */ +} Tree; + + +Tree* TreeInitialize(int(*compare)(void*, void*, int)); +void TreeInitializeNoMalloc(Tree* aTree, int(*compare)(void*, void*, int)); +void TreeAddIndex(Tree* aTree, int(*compare)(void*, void*, int)); + +void* TreeAdd(Tree* aTree, void* content, size_t size); + +void* TreeRemove(Tree* aTree, void* content); + +void* TreeRemoveKey(Tree* aTree, void* key); +void* TreeRemoveKeyIndex(Tree* aTree, void* key, int index); + +void* TreeRemoveNodeIndex(Tree* aTree, Node* aNode, int index); + +void TreeFree(Tree* aTree); + +Node* TreeFind(Tree* aTree, void* key); +Node* TreeFindIndex(Tree* aTree, void* key, int index); + +Node* TreeNextElement(Tree* aTree, Node* curnode); + +int TreeIntCompare(void* a, void* b, int); +int TreePtrCompare(void* a, void* b, int); +int TreeStringCompare(void* a, void* b, int); + +#endif diff --git a/src/task_manager/include/paho_mqtt_3c/VersionInfo.h.in b/src/task_manager/include/paho_mqtt_3c/VersionInfo.h.in new file mode 100644 index 0000000..5b91bf3 --- /dev/null +++ b/src/task_manager/include/paho_mqtt_3c/VersionInfo.h.in @@ -0,0 +1,7 @@ +#ifndef VERSIONINFO_H +#define VERSIONINFO_H + +#define BUILD_TIMESTAMP "@BUILD_TIMESTAMP@" +#define CLIENT_VERSION "@CLIENT_VERSION@" + +#endif /* VERSIONINFO_H */ diff --git a/src/task_manager/include/paho_mqtt_3c/WebSocket.h b/src/task_manager/include/paho_mqtt_3c/WebSocket.h new file mode 100644 index 0000000..33cc844 --- /dev/null +++ b/src/task_manager/include/paho_mqtt_3c/WebSocket.h @@ -0,0 +1,77 @@ +/******************************************************************************* + * Copyright (c) 2018 Wind River Systems, Inc. All Rights Reserved. + * + * All rights reserved. This program and the accompanying materials + * are made available under the terms of the Eclipse Public License v1.0 + * and Eclipse Distribution License v1.0 which accompany this distribution. + * + * The Eclipse Public License is available at + * http://www.eclipse.org/legal/epl-v10.html + * and the Eclipse Distribution License is available at + * http://www.eclipse.org/org/documents/edl-v10.php. + * + * Contributors: + * Keith Holman - initial implementation and documentation + *******************************************************************************/ + +#if !defined(WEBSOCKET_H) +#define WEBSOCKET_H + +#include "Clients.h" + +/** + * WebSocket op codes + * @{ + */ +#define WebSocket_OP_CONTINUE 0x0 /* 0000 - continue frame */ +#define WebSocket_OP_TEXT 0x1 /* 0001 - text frame */ +#define WebSocket_OP_BINARY 0x2 /* 0010 - binary frame */ +#define WebSocket_OP_CLOSE 0x8 /* 1000 - close frame */ +#define WebSocket_OP_PING 0x9 /* 1001 - ping frame */ +#define WebSocket_OP_PONG 0xA /* 1010 - pong frame */ +/** @} */ + +/** + * Various close status codes + * @{ + */ +#define WebSocket_CLOSE_NORMAL 1000 +#define WebSocket_CLOSE_GOING_AWAY 1001 +#define WebSocket_CLOSE_PROTOCOL_ERROR 1002 +#define WebSocket_CLOSE_UNKNOWN_DATA 1003 +#define WebSocket_CLOSE_RESERVED 1004 +#define WebSocket_CLOSE_NO_STATUS_CODE 1005 /* reserved: not to be used */ +#define WebSocket_CLOSE_ABNORMAL 1006 /* reserved: not to be used */ +#define WebSocket_CLOSE_BAD_DATA 1007 +#define WebSocket_CLOSE_POLICY 1008 +#define WebSocket_CLOSE_MSG_TOO_BIG 1009 +#define WebSocket_CLOSE_NO_EXTENSION 1010 +#define WebScoket_CLOSE_UNEXPECTED 1011 +#define WebSocket_CLOSE_TLS_FAIL 1015 /* reserved: not be used */ +/** @} */ + +/* closes a websocket connection */ +void WebSocket_close(networkHandles *net, int status_code, const char *reason); + +/* sends upgrade request */ +int WebSocket_connect(networkHandles *net, const char *uri); + +/* calculates the extra data required in a packet to hold a WebSocket frame header */ +size_t WebSocket_calculateFrameHeaderSize(networkHandles *net, int mask_data, + size_t data_len); + +/* obtain data from network socket */ +int WebSocket_getch(networkHandles *net, char* c); +char *WebSocket_getdata(networkHandles *net, size_t bytes, size_t* actual_len); + +/* send data out, in websocket format only if required */ +int WebSocket_putdatas(networkHandles* net, char* buf0, size_t buf0len, + int count, char** buffers, size_t* buflens, int* freeData); + +/* releases any resources used by the websocket system */ +void WebSocket_terminate(void); + +/* handles websocket upgrade request */ +int WebSocket_upgrade(networkHandles *net); + +#endif /* WEBSOCKET_H */ diff --git a/src/task_manager/include/paho_mqtt_3c/mutex_type.h b/src/task_manager/include/paho_mqtt_3c/mutex_type.h new file mode 100644 index 0000000..5760b37 --- /dev/null +++ b/src/task_manager/include/paho_mqtt_3c/mutex_type.h @@ -0,0 +1,25 @@ +/******************************************************************************* + * Copyright (c) 2009, 2018 IBM Corp. + * + * All rights reserved. This program and the accompanying materials + * are made available under the terms of the Eclipse Public License v1.0 + * and Eclipse Distribution License v1.0 which accompany this distribution. + * + * The Eclipse Public License is available at + * http://www.eclipse.org/legal/epl-v10.html + * and the Eclipse Distribution License is available at + * http://www.eclipse.org/org/documents/edl-v10.php. + * + *******************************************************************************/ +#if !defined(_MUTEX_TYPE_H_) +#define _MUTEX_TYPE_H_ + +#if defined(WIN32) || defined(WIN64) + #include + #define mutex_type HANDLE +#else + #include + #define mutex_type pthread_mutex_t* +#endif + +#endif /* _MUTEX_TYPE_H_ */ diff --git a/src/task_manager/include/paho_mqtt_3c/pubsub_opts.h b/src/task_manager/include/paho_mqtt_3c/pubsub_opts.h new file mode 100644 index 0000000..a506a68 --- /dev/null +++ b/src/task_manager/include/paho_mqtt_3c/pubsub_opts.h @@ -0,0 +1,86 @@ +/******************************************************************************* + * Copyright (c) 2012, 2018 IBM Corp. + * + * All rights reserved. This program and the accompanying materials + * are made available under the terms of the Eclipse Public License v1.0 + * and Eclipse Distribution License v1.0 which accompany this distribution. + * + * The Eclipse Public License is available at + * http://www.eclipse.org/legal/epl-v10.html + * and the Eclipse Distribution License is available at + * http://www.eclipse.org/org/documents/edl-v10.php. + * + * Contributors: + * Ian Craggs - initial contribution + * Guilherme Maciel Ferreira - add keep alive option + *******************************************************************************/ + +#if !defined(PUBSUB_OPTS_H) +#define PUBSUB_OPTS_H + +#include "MQTTAsync.h" +#include "MQTTClientPersistence.h" + +struct pubsub_opts +{ + /* debug app options */ + int publisher; /* publisher app? */ + int quiet; + int verbose; + int tracelevel; + char* delimiter; + int maxdatalen; + /* message options */ + char* message; + char* filename; + int stdin_lines; + int stdlin_complete; + int null_message; + /* MQTT options */ + int MQTTVersion; + char* topic; + char* clientid; + int qos; + int retained; + char* username; + char* password; + char* host; + char* port; + char* connection; + int keepalive; + /* will options */ + char* will_topic; + char* will_payload; + int will_qos; + int will_retain; + /* TLS options */ + int insecure; + char* capath; + char* cert; + char* cafile; + char* key; + char* keypass; + char* ciphers; + /* MQTT V5 options */ + int message_expiry; + struct { + char *name; + char *value; + } user_property; +}; + +typedef struct +{ + const char* name; + const char* value; +} pubsub_opts_nameValue; + +//void usage(struct pubsub_opts* opts, const char* version, const char* program_name); +void usage(struct pubsub_opts* opts, pubsub_opts_nameValue* name_values, const char* program_name); +int getopts(int argc, char** argv, struct pubsub_opts* opts); +char* readfile(int* data_len, struct pubsub_opts* opts); +void logProperties(MQTTProperties *props); + +#endif + + diff --git a/src/task_manager/include/paho_mqtt_3c/utf-8.h b/src/task_manager/include/paho_mqtt_3c/utf-8.h new file mode 100644 index 0000000..8bce4b3 --- /dev/null +++ b/src/task_manager/include/paho_mqtt_3c/utf-8.h @@ -0,0 +1,23 @@ +/******************************************************************************* + * Copyright (c) 2009, 2013 IBM Corp. + * + * All rights reserved. This program and the accompanying materials + * are made available under the terms of the Eclipse Public License v1.0 + * and Eclipse Distribution License v1.0 which accompany this distribution. + * + * The Eclipse Public License is available at + * http://www.eclipse.org/legal/epl-v10.html + * and the Eclipse Distribution License is available at + * http://www.eclipse.org/org/documents/edl-v10.php. + * + * Contributors: + * Ian Craggs - initial API and implementation and/or initial documentation + *******************************************************************************/ + +#if !defined(UTF8_H) +#define UTF8_H + +int UTF8_validate(int len, const char *data); +int UTF8_validateString(const char* string); + +#endif diff --git a/src/task_manager/include/task_manager/json.h b/src/task_manager/include/task_manager/json.h new file mode 100644 index 0000000..d95fe6e --- /dev/null +++ b/src/task_manager/include/task_manager/json.h @@ -0,0 +1,2351 @@ +/// Json-cpp amalgamated header (http://jsoncpp.sourceforge.net/). +/// It is intended to be used with #include "json/json.h" + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: LICENSE +// ////////////////////////////////////////////////////////////////////// + +/* +The JsonCpp library's source code, including accompanying documentation, +tests and demonstration applications, are licensed under the following +conditions... + +Baptiste Lepilleur and The JsonCpp Authors explicitly disclaim copyright in all +jurisdictions which recognize such a disclaimer. In such jurisdictions, +this software is released into the Public Domain. + +In jurisdictions which do not recognize Public Domain property (e.g. Germany as of +2010), this software is Copyright (c) 2007-2010 by Baptiste Lepilleur and +The JsonCpp Authors, and is released under the terms of the MIT License (see below). + +In jurisdictions which recognize Public Domain property, the user of this +software may choose to accept it either as 1) Public Domain, 2) under the +conditions of the MIT License (see below), or 3) under the terms of dual +Public Domain/MIT License conditions described here, as they choose. + +The MIT License is about as close to Public Domain as a license can get, and is +described in clear, concise terms at: + + http://en.wikipedia.org/wiki/MIT_License + +The full text of the MIT License follows: + +======================================================================== +Copyright (c) 2007-2010 Baptiste Lepilleur and The JsonCpp Authors + +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. +======================================================================== +(END LICENSE TEXT) + +The MIT license is compatible with both the GPL and commercial +software, affording one all of the rights of Public Domain with the +minor nuisance of being required to keep the above copyright notice +and license text in the source code. Note also that by accepting the +Public Domain "license" you can re-license your copy using whatever +license you like. + +*/ + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: LICENSE +// ////////////////////////////////////////////////////////////////////// + + + + + +#ifndef JSON_AMALGAMATED_H_INCLUDED +# define JSON_AMALGAMATED_H_INCLUDED +/// If defined, indicates that the source file is amalgamated +/// to prevent private header inclusion. +#define JSON_IS_AMALGAMATION + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: include/json/version.h +// ////////////////////////////////////////////////////////////////////// + +#ifndef JSON_VERSION_H_INCLUDED +#define JSON_VERSION_H_INCLUDED + +// Note: version must be updated in three places when doing a release. This +// annoying process ensures that amalgamate, CMake, and meson all report the +// correct version. +// 1. /meson.build +// 2. /include/json/version.h +// 3. /CMakeLists.txt +// IMPORTANT: also update the SOVERSION!! + +#define JSONCPP_VERSION_STRING "1.9.5" +#define JSONCPP_VERSION_MAJOR 1 +#define JSONCPP_VERSION_MINOR 9 +#define JSONCPP_VERSION_PATCH 5 +#define JSONCPP_VERSION_QUALIFIER +#define JSONCPP_VERSION_HEXA \ + ((JSONCPP_VERSION_MAJOR << 24) | (JSONCPP_VERSION_MINOR << 16) | \ + (JSONCPP_VERSION_PATCH << 8)) + +#ifdef JSONCPP_USING_SECURE_MEMORY +#undef JSONCPP_USING_SECURE_MEMORY +#endif +#define JSONCPP_USING_SECURE_MEMORY 0 +// If non-zero, the library zeroes any memory that it has allocated before +// it frees its memory. + +#endif // JSON_VERSION_H_INCLUDED + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: include/json/version.h +// ////////////////////////////////////////////////////////////////////// + + + + + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: include/json/allocator.h +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2007-2010 Baptiste Lepilleur and The JsonCpp Authors +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +#ifndef JSON_ALLOCATOR_H_INCLUDED +#define JSON_ALLOCATOR_H_INCLUDED + +#include +#include + +#pragma pack(push) +#pragma pack() + +namespace Json { +template class SecureAllocator { +public: + // Type definitions + using value_type = T; + using pointer = T*; + using const_pointer = const T*; + using reference = T&; + using const_reference = const T&; + using size_type = std::size_t; + using difference_type = std::ptrdiff_t; + + /** + * Allocate memory for N items using the standard allocator. + */ + pointer allocate(size_type n) { + // allocate using "global operator new" + return static_cast(::operator new(n * sizeof(T))); + } + + /** + * Release memory which was allocated for N items at pointer P. + * + * The memory block is filled with zeroes before being released. + */ + void deallocate(pointer p, size_type n) { + // memset_s is used because memset may be optimized away by the compiler + memset_s(p, n * sizeof(T), 0, n * sizeof(T)); + // free using "global operator delete" + ::operator delete(p); + } + + /** + * Construct an item in-place at pointer P. + */ + template void construct(pointer p, Args&&... args) { + // construct using "placement new" and "perfect forwarding" + ::new (static_cast(p)) T(std::forward(args)...); + } + + size_type max_size() const { return size_t(-1) / sizeof(T); } + + pointer address(reference x) const { return std::addressof(x); } + + const_pointer address(const_reference x) const { return std::addressof(x); } + + /** + * Destroy an item in-place at pointer P. + */ + void destroy(pointer p) { + // destroy using "explicit destructor" + p->~T(); + } + + // Boilerplate + SecureAllocator() {} + template SecureAllocator(const SecureAllocator&) {} + template struct rebind { using other = SecureAllocator; }; +}; + +template +bool operator==(const SecureAllocator&, const SecureAllocator&) { + return true; +} + +template +bool operator!=(const SecureAllocator&, const SecureAllocator&) { + return false; +} + +} // namespace Json + +#pragma pack(pop) + +#endif // JSON_ALLOCATOR_H_INCLUDED + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: include/json/allocator.h +// ////////////////////////////////////////////////////////////////////// + + + + + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: include/json/config.h +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2007-2010 Baptiste Lepilleur and The JsonCpp Authors +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +#ifndef JSON_CONFIG_H_INCLUDED +#define JSON_CONFIG_H_INCLUDED +#include +#include +#include +#include +#include +#include +#include +#include + +// If non-zero, the library uses exceptions to report bad input instead of C +// assertion macros. The default is to use exceptions. +#ifndef JSON_USE_EXCEPTION +#define JSON_USE_EXCEPTION 1 +#endif + +// Temporary, tracked for removal with issue #982. +#ifndef JSON_USE_NULLREF +#define JSON_USE_NULLREF 1 +#endif + +/// If defined, indicates that the source file is amalgamated +/// to prevent private header inclusion. +/// Remarks: it is automatically defined in the generated amalgamated header. +// #define JSON_IS_AMALGAMATION + +// Export macros for DLL visibility +#if defined(JSON_DLL_BUILD) +#if defined(_MSC_VER) || defined(__MINGW32__) +#define JSON_API __declspec(dllexport) +#define JSONCPP_DISABLE_DLL_INTERFACE_WARNING +#elif defined(__GNUC__) || defined(__clang__) +#define JSON_API __attribute__((visibility("default"))) +#endif // if defined(_MSC_VER) + +#elif defined(JSON_DLL) +#if defined(_MSC_VER) || defined(__MINGW32__) +#define JSON_API __declspec(dllimport) +#define JSONCPP_DISABLE_DLL_INTERFACE_WARNING +#endif // if defined(_MSC_VER) +#endif // ifdef JSON_DLL_BUILD + +#if !defined(JSON_API) +#define JSON_API +#endif + +#if defined(_MSC_VER) && _MSC_VER < 1800 +#error \ + "ERROR: Visual Studio 12 (2013) with _MSC_VER=1800 is the oldest supported compiler with sufficient C++11 capabilities" +#endif + +#if defined(_MSC_VER) && _MSC_VER < 1900 +// As recommended at +// https://stackoverflow.com/questions/2915672/snprintf-and-visual-studio-2010 +extern JSON_API int msvc_pre1900_c99_snprintf(char* outBuf, size_t size, + const char* format, ...); +#define jsoncpp_snprintf msvc_pre1900_c99_snprintf +#else +#define jsoncpp_snprintf std::snprintf +#endif + +// If JSON_NO_INT64 is defined, then Json only support C++ "int" type for +// integer +// Storages, and 64 bits integer support is disabled. +// #define JSON_NO_INT64 1 + +// JSONCPP_OVERRIDE is maintained for backwards compatibility of external tools. +// C++11 should be used directly in JSONCPP. +#define JSONCPP_OVERRIDE override + +#ifdef __clang__ +#if __has_extension(attribute_deprecated_with_message) +#define JSONCPP_DEPRECATED(message) __attribute__((deprecated(message))) +#endif +#elif defined(__GNUC__) // not clang (gcc comes later since clang emulates gcc) +#if (__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 5)) +#define JSONCPP_DEPRECATED(message) __attribute__((deprecated(message))) +#elif (__GNUC__ > 3 || (__GNUC__ == 3 && __GNUC_MINOR__ >= 1)) +#define JSONCPP_DEPRECATED(message) __attribute__((__deprecated__)) +#endif // GNUC version +#elif defined(_MSC_VER) // MSVC (after clang because clang on Windows emulates + // MSVC) +#define JSONCPP_DEPRECATED(message) __declspec(deprecated(message)) +#endif // __clang__ || __GNUC__ || _MSC_VER + +#if !defined(JSONCPP_DEPRECATED) +#define JSONCPP_DEPRECATED(message) +#endif // if !defined(JSONCPP_DEPRECATED) + +#if defined(__clang__) || (defined(__GNUC__) && (__GNUC__ >= 6)) +#define JSON_USE_INT64_DOUBLE_CONVERSION 1 +#endif + +#if !defined(JSON_IS_AMALGAMATION) + +#include "allocator.h" +#include "version.h" + +#endif // if !defined(JSON_IS_AMALGAMATION) + +namespace Json { +using Int = int; +using UInt = unsigned int; +#if defined(JSON_NO_INT64) +using LargestInt = int; +using LargestUInt = unsigned int; +#undef JSON_HAS_INT64 +#else // if defined(JSON_NO_INT64) +// For Microsoft Visual use specific types as long long is not supported +#if defined(_MSC_VER) // Microsoft Visual Studio +using Int64 = __int64; +using UInt64 = unsigned __int64; +#else // if defined(_MSC_VER) // Other platforms, use long long +using Int64 = int64_t; +using UInt64 = uint64_t; +#endif // if defined(_MSC_VER) +using LargestInt = Int64; +using LargestUInt = UInt64; +#define JSON_HAS_INT64 +#endif // if defined(JSON_NO_INT64) + +template +using Allocator = + typename std::conditional, + std::allocator>::type; +using String = std::basic_string, Allocator>; +using IStringStream = + std::basic_istringstream; +using OStringStream = + std::basic_ostringstream; +using IStream = std::istream; +using OStream = std::ostream; +} // namespace Json + +// Legacy names (formerly macros). +using JSONCPP_STRING = Json::String; +using JSONCPP_ISTRINGSTREAM = Json::IStringStream; +using JSONCPP_OSTRINGSTREAM = Json::OStringStream; +using JSONCPP_ISTREAM = Json::IStream; +using JSONCPP_OSTREAM = Json::OStream; + +#endif // JSON_CONFIG_H_INCLUDED + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: include/json/config.h +// ////////////////////////////////////////////////////////////////////// + + + + + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: include/json/forwards.h +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2007-2010 Baptiste Lepilleur and The JsonCpp Authors +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +#ifndef JSON_FORWARDS_H_INCLUDED +#define JSON_FORWARDS_H_INCLUDED + +#if !defined(JSON_IS_AMALGAMATION) +#include "config.h" +#endif // if !defined(JSON_IS_AMALGAMATION) + +namespace Json { + +// writer.h +class StreamWriter; +class StreamWriterBuilder; +class Writer; +class FastWriter; +class StyledWriter; +class StyledStreamWriter; + +// reader.h +class Reader; +class CharReader; +class CharReaderBuilder; + +// json_features.h +class Features; + +// value.h +using ArrayIndex = unsigned int; +class StaticString; +class Path; +class PathArgument; +class Value; +class ValueIteratorBase; +class ValueIterator; +class ValueConstIterator; + +} // namespace Json + +#endif // JSON_FORWARDS_H_INCLUDED + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: include/json/forwards.h +// ////////////////////////////////////////////////////////////////////// + + + + + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: include/json/json_features.h +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2007-2010 Baptiste Lepilleur and The JsonCpp Authors +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +#ifndef JSON_FEATURES_H_INCLUDED +#define JSON_FEATURES_H_INCLUDED + +#if !defined(JSON_IS_AMALGAMATION) +#include "forwards.h" +#endif // if !defined(JSON_IS_AMALGAMATION) + +#pragma pack(push) +#pragma pack() + +namespace Json { + +/** \brief Configuration passed to reader and writer. + * This configuration object can be used to force the Reader or Writer + * to behave in a standard conforming way. + */ +class JSON_API Features { +public: + /** \brief A configuration that allows all features and assumes all strings + * are UTF-8. + * - C & C++ comments are allowed + * - Root object can be any JSON value + * - Assumes Value strings are encoded in UTF-8 + */ + static Features all(); + + /** \brief A configuration that is strictly compatible with the JSON + * specification. + * - Comments are forbidden. + * - Root object must be either an array or an object value. + * - Assumes Value strings are encoded in UTF-8 + */ + static Features strictMode(); + + /** \brief Initialize the configuration like JsonConfig::allFeatures; + */ + Features(); + + /// \c true if comments are allowed. Default: \c true. + bool allowComments_{true}; + + /// \c true if root must be either an array or an object value. Default: \c + /// false. + bool strictRoot_{false}; + + /// \c true if dropped null placeholders are allowed. Default: \c false. + bool allowDroppedNullPlaceholders_{false}; + + /// \c true if numeric object key are allowed. Default: \c false. + bool allowNumericKeys_{false}; +}; + +} // namespace Json + +#pragma pack(pop) + +#endif // JSON_FEATURES_H_INCLUDED + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: include/json/json_features.h +// ////////////////////////////////////////////////////////////////////// + + + + + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: include/json/value.h +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2007-2010 Baptiste Lepilleur and The JsonCpp Authors +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +#ifndef JSON_H_INCLUDED +#define JSON_H_INCLUDED + +#if !defined(JSON_IS_AMALGAMATION) +#include "forwards.h" +#endif // if !defined(JSON_IS_AMALGAMATION) + +// Conditional NORETURN attribute on the throw functions would: +// a) suppress false positives from static code analysis +// b) possibly improve optimization opportunities. +#if !defined(JSONCPP_NORETURN) +#if defined(_MSC_VER) && _MSC_VER == 1800 +#define JSONCPP_NORETURN __declspec(noreturn) +#else +#define JSONCPP_NORETURN [[noreturn]] +#endif +#endif + +// Support for '= delete' with template declarations was a late addition +// to the c++11 standard and is rejected by clang 3.8 and Apple clang 8.2 +// even though these declare themselves to be c++11 compilers. +#if !defined(JSONCPP_TEMPLATE_DELETE) +#if defined(__clang__) && defined(__apple_build_version__) +#if __apple_build_version__ <= 8000042 +#define JSONCPP_TEMPLATE_DELETE +#endif +#elif defined(__clang__) +#if __clang_major__ == 3 && __clang_minor__ <= 8 +#define JSONCPP_TEMPLATE_DELETE +#endif +#endif +#if !defined(JSONCPP_TEMPLATE_DELETE) +#define JSONCPP_TEMPLATE_DELETE = delete +#endif +#endif + +#include +#include +#include +#include +#include +#include + +// Disable warning C4251: : needs to have dll-interface to +// be used by... +#if defined(JSONCPP_DISABLE_DLL_INTERFACE_WARNING) +#pragma warning(push) +#pragma warning(disable : 4251 4275) +#endif // if defined(JSONCPP_DISABLE_DLL_INTERFACE_WARNING) + +#pragma pack(push) +#pragma pack() + +/** \brief JSON (JavaScript Object Notation). + */ +namespace Json { + +#if JSON_USE_EXCEPTION +/** Base class for all exceptions we throw. + * + * We use nothing but these internally. Of course, STL can throw others. + */ +class JSON_API Exception : public std::exception { +public: + Exception(String msg); + ~Exception() noexcept override; + char const* what() const noexcept override; + +protected: + String msg_; +}; + +/** Exceptions which the user cannot easily avoid. + * + * E.g. out-of-memory (when we use malloc), stack-overflow, malicious input + * + * \remark derived from Json::Exception + */ +class JSON_API RuntimeError : public Exception { +public: + RuntimeError(String const& msg); +}; + +/** Exceptions thrown by JSON_ASSERT/JSON_FAIL macros. + * + * These are precondition-violations (user bugs) and internal errors (our bugs). + * + * \remark derived from Json::Exception + */ +class JSON_API LogicError : public Exception { +public: + LogicError(String const& msg); +}; +#endif + +/// used internally +JSONCPP_NORETURN void throwRuntimeError(String const& msg); +/// used internally +JSONCPP_NORETURN void throwLogicError(String const& msg); + +/** \brief Type of the value held by a Value object. + */ +enum ValueType { + nullValue = 0, ///< 'null' value + intValue, ///< signed integer value + uintValue, ///< unsigned integer value + realValue, ///< double value + stringValue, ///< UTF-8 string value + booleanValue, ///< bool value + arrayValue, ///< array value (ordered list) + objectValue ///< object value (collection of name/value pairs). +}; + +enum CommentPlacement { + commentBefore = 0, ///< a comment placed on the line before a value + commentAfterOnSameLine, ///< a comment just after a value on the same line + commentAfter, ///< a comment on the line after a value (only make sense for + /// root value) + numberOfCommentPlacement +}; + +/** \brief Type of precision for formatting of real values. + */ +enum PrecisionType { + significantDigits = 0, ///< we set max number of significant digits in string + decimalPlaces ///< we set max number of digits after "." in string +}; + +/** \brief Lightweight wrapper to tag static string. + * + * Value constructor and objectValue member assignment takes advantage of the + * StaticString and avoid the cost of string duplication when storing the + * string or the member name. + * + * Example of usage: + * \code + * Json::Value aValue( StaticString("some text") ); + * Json::Value object; + * static const StaticString code("code"); + * object[code] = 1234; + * \endcode + */ +class JSON_API StaticString { +public: + explicit StaticString(const char* czstring) : c_str_(czstring) {} + + operator const char*() const { return c_str_; } + + const char* c_str() const { return c_str_; } + +private: + const char* c_str_; +}; + +/** \brief Represents a JSON value. + * + * This class is a discriminated union wrapper that can represents a: + * - signed integer [range: Value::minInt - Value::maxInt] + * - unsigned integer (range: 0 - Value::maxUInt) + * - double + * - UTF-8 string + * - boolean + * - 'null' + * - an ordered list of Value + * - collection of name/value pairs (javascript object) + * + * The type of the held value is represented by a #ValueType and + * can be obtained using type(). + * + * Values of an #objectValue or #arrayValue can be accessed using operator[]() + * methods. + * Non-const methods will automatically create the a #nullValue element + * if it does not exist. + * The sequence of an #arrayValue will be automatically resized and initialized + * with #nullValue. resize() can be used to enlarge or truncate an #arrayValue. + * + * The get() methods can be used to obtain default value in the case the + * required element does not exist. + * + * It is possible to iterate over the list of member keys of an object using + * the getMemberNames() method. + * + * \note #Value string-length fit in size_t, but keys must be < 2^30. + * (The reason is an implementation detail.) A #CharReader will raise an + * exception if a bound is exceeded to avoid security holes in your app, + * but the Value API does *not* check bounds. That is the responsibility + * of the caller. + */ +class JSON_API Value { + friend class ValueIteratorBase; + +public: + using Members = std::vector; + using iterator = ValueIterator; + using const_iterator = ValueConstIterator; + using UInt = Json::UInt; + using Int = Json::Int; +#if defined(JSON_HAS_INT64) + using UInt64 = Json::UInt64; + using Int64 = Json::Int64; +#endif // defined(JSON_HAS_INT64) + using LargestInt = Json::LargestInt; + using LargestUInt = Json::LargestUInt; + using ArrayIndex = Json::ArrayIndex; + + // Required for boost integration, e. g. BOOST_TEST + using value_type = std::string; + +#if JSON_USE_NULLREF + // Binary compatibility kludges, do not use. + static const Value& null; + static const Value& nullRef; +#endif + + // null and nullRef are deprecated, use this instead. + static Value const& nullSingleton(); + + /// Minimum signed integer value that can be stored in a Json::Value. + static constexpr LargestInt minLargestInt = + LargestInt(~(LargestUInt(-1) / 2)); + /// Maximum signed integer value that can be stored in a Json::Value. + static constexpr LargestInt maxLargestInt = LargestInt(LargestUInt(-1) / 2); + /// Maximum unsigned integer value that can be stored in a Json::Value. + static constexpr LargestUInt maxLargestUInt = LargestUInt(-1); + + /// Minimum signed int value that can be stored in a Json::Value. + static constexpr Int minInt = Int(~(UInt(-1) / 2)); + /// Maximum signed int value that can be stored in a Json::Value. + static constexpr Int maxInt = Int(UInt(-1) / 2); + /// Maximum unsigned int value that can be stored in a Json::Value. + static constexpr UInt maxUInt = UInt(-1); + +#if defined(JSON_HAS_INT64) + /// Minimum signed 64 bits int value that can be stored in a Json::Value. + static constexpr Int64 minInt64 = Int64(~(UInt64(-1) / 2)); + /// Maximum signed 64 bits int value that can be stored in a Json::Value. + static constexpr Int64 maxInt64 = Int64(UInt64(-1) / 2); + /// Maximum unsigned 64 bits int value that can be stored in a Json::Value. + static constexpr UInt64 maxUInt64 = UInt64(-1); +#endif // defined(JSON_HAS_INT64) + /// Default precision for real value for string representation. + static constexpr UInt defaultRealPrecision = 17; + // The constant is hard-coded because some compiler have trouble + // converting Value::maxUInt64 to a double correctly (AIX/xlC). + // Assumes that UInt64 is a 64 bits integer. + static constexpr double maxUInt64AsDouble = 18446744073709551615.0; +// Workaround for bug in the NVIDIAs CUDA 9.1 nvcc compiler +// when using gcc and clang backend compilers. CZString +// cannot be defined as private. See issue #486 +#ifdef __NVCC__ +public: +#else +private: +#endif +#ifndef JSONCPP_DOC_EXCLUDE_IMPLEMENTATION + class CZString { + public: + enum DuplicationPolicy { noDuplication = 0, duplicate, duplicateOnCopy }; + CZString(ArrayIndex index); + CZString(char const* str, unsigned length, DuplicationPolicy allocate); + CZString(CZString const& other); + CZString(CZString&& other) noexcept; + ~CZString(); + CZString& operator=(const CZString& other); + CZString& operator=(CZString&& other) noexcept; + + bool operator<(CZString const& other) const; + bool operator==(CZString const& other) const; + ArrayIndex index() const; + // const char* c_str() const; ///< \deprecated + char const* data() const; + unsigned length() const; + bool isStaticString() const; + + private: + void swap(CZString& other); + + struct StringStorage { + unsigned policy_ : 2; + unsigned length_ : 30; // 1GB max + }; + + char const* cstr_; // actually, a prefixed string, unless policy is noDup + union { + ArrayIndex index_; + StringStorage storage_; + }; + }; + +public: + typedef std::map ObjectValues; +#endif // ifndef JSONCPP_DOC_EXCLUDE_IMPLEMENTATION + +public: + /** + * \brief Create a default Value of the given type. + * + * This is a very useful constructor. + * To create an empty array, pass arrayValue. + * To create an empty object, pass objectValue. + * Another Value can then be set to this one by assignment. + * This is useful since clear() and resize() will not alter types. + * + * Examples: + * \code + * Json::Value null_value; // null + * Json::Value arr_value(Json::arrayValue); // [] + * Json::Value obj_value(Json::objectValue); // {} + * \endcode + */ + Value(ValueType type = nullValue); + Value(Int value); + Value(UInt value); +#if defined(JSON_HAS_INT64) + Value(Int64 value); + Value(UInt64 value); +#endif // if defined(JSON_HAS_INT64) + Value(double value); + Value(const char* value); ///< Copy til first 0. (NULL causes to seg-fault.) + Value(const char* begin, const char* end); ///< Copy all, incl zeroes. + /** + * \brief Constructs a value from a static string. + * + * Like other value string constructor but do not duplicate the string for + * internal storage. The given string must remain alive after the call to + * this constructor. + * + * \note This works only for null-terminated strings. (We cannot change the + * size of this class, so we have nowhere to store the length, which might be + * computed later for various operations.) + * + * Example of usage: + * \code + * static StaticString foo("some text"); + * Json::Value aValue(foo); + * \endcode + */ + Value(const StaticString& value); + Value(const String& value); + Value(bool value); + Value(std::nullptr_t ptr) = delete; + Value(const Value& other); + Value(Value&& other) noexcept; + ~Value(); + + /// \note Overwrite existing comments. To preserve comments, use + /// #swapPayload(). + Value& operator=(const Value& other); + Value& operator=(Value&& other) noexcept; + + /// Swap everything. + void swap(Value& other); + /// Swap values but leave comments and source offsets in place. + void swapPayload(Value& other); + + /// copy everything. + void copy(const Value& other); + /// copy values but leave comments and source offsets in place. + void copyPayload(const Value& other); + + ValueType type() const; + + /// Compare payload only, not comments etc. + bool operator<(const Value& other) const; + bool operator<=(const Value& other) const; + bool operator>=(const Value& other) const; + bool operator>(const Value& other) const; + bool operator==(const Value& other) const; + bool operator!=(const Value& other) const; + int compare(const Value& other) const; + + const char* asCString() const; ///< Embedded zeroes could cause you trouble! +#if JSONCPP_USING_SECURE_MEMORY + unsigned getCStringLength() const; // Allows you to understand the length of + // the CString +#endif + String asString() const; ///< Embedded zeroes are possible. + /** Get raw char* of string-value. + * \return false if !string. (Seg-fault if str or end are NULL.) + */ + bool getString(char const** begin, char const** end) const; + Int asInt() const; + UInt asUInt() const; +#if defined(JSON_HAS_INT64) + Int64 asInt64() const; + UInt64 asUInt64() const; +#endif // if defined(JSON_HAS_INT64) + LargestInt asLargestInt() const; + LargestUInt asLargestUInt() const; + float asFloat() const; + double asDouble() const; + bool asBool() const; + + bool isNull() const; + bool isBool() const; + bool isInt() const; + bool isInt64() const; + bool isUInt() const; + bool isUInt64() const; + bool isIntegral() const; + bool isDouble() const; + bool isNumeric() const; + bool isString() const; + bool isArray() const; + bool isObject() const; + + /// The `as` and `is` member function templates and specializations. + template T as() const JSONCPP_TEMPLATE_DELETE; + template bool is() const JSONCPP_TEMPLATE_DELETE; + + bool isConvertibleTo(ValueType other) const; + + /// Number of values in array or object + ArrayIndex size() const; + + /// \brief Return true if empty array, empty object, or null; + /// otherwise, false. + bool empty() const; + + /// Return !isNull() + explicit operator bool() const; + + /// Remove all object members and array elements. + /// \pre type() is arrayValue, objectValue, or nullValue + /// \post type() is unchanged + void clear(); + + /// Resize the array to newSize elements. + /// New elements are initialized to null. + /// May only be called on nullValue or arrayValue. + /// \pre type() is arrayValue or nullValue + /// \post type() is arrayValue + void resize(ArrayIndex newSize); + + ///@{ + /// Access an array element (zero based index). If the array contains less + /// than index element, then null value are inserted in the array so that + /// its size is index+1. + /// (You may need to say 'value[0u]' to get your compiler to distinguish + /// this from the operator[] which takes a string.) + Value& operator[](ArrayIndex index); + Value& operator[](int index); + ///@} + + ///@{ + /// Access an array element (zero based index). + /// (You may need to say 'value[0u]' to get your compiler to distinguish + /// this from the operator[] which takes a string.) + const Value& operator[](ArrayIndex index) const; + const Value& operator[](int index) const; + ///@} + + /// If the array contains at least index+1 elements, returns the element + /// value, otherwise returns defaultValue. + Value get(ArrayIndex index, const Value& defaultValue) const; + /// Return true if index < size(). + bool isValidIndex(ArrayIndex index) const; + /// \brief Append value to array at the end. + /// + /// Equivalent to jsonvalue[jsonvalue.size()] = value; + Value& append(const Value& value); + Value& append(Value&& value); + + /// \brief Insert value in array at specific index + bool insert(ArrayIndex index, const Value& newValue); + bool insert(ArrayIndex index, Value&& newValue); + + /// Access an object value by name, create a null member if it does not exist. + /// \note Because of our implementation, keys are limited to 2^30 -1 chars. + /// Exceeding that will cause an exception. + Value& operator[](const char* key); + /// Access an object value by name, returns null if there is no member with + /// that name. + const Value& operator[](const char* key) const; + /// Access an object value by name, create a null member if it does not exist. + /// \param key may contain embedded nulls. + Value& operator[](const String& key); + /// Access an object value by name, returns null if there is no member with + /// that name. + /// \param key may contain embedded nulls. + const Value& operator[](const String& key) const; + /** \brief Access an object value by name, create a null member if it does not + * exist. + * + * If the object has no entry for that name, then the member name used to + * store the new entry is not duplicated. + * Example of use: + * \code + * Json::Value object; + * static const StaticString code("code"); + * object[code] = 1234; + * \endcode + */ + Value& operator[](const StaticString& key); + /// Return the member named key if it exist, defaultValue otherwise. + /// \note deep copy + Value get(const char* key, const Value& defaultValue) const; + /// Return the member named key if it exist, defaultValue otherwise. + /// \note deep copy + /// \note key may contain embedded nulls. + Value get(const char* begin, const char* end, + const Value& defaultValue) const; + /// Return the member named key if it exist, defaultValue otherwise. + /// \note deep copy + /// \param key may contain embedded nulls. + Value get(const String& key, const Value& defaultValue) const; + /// Most general and efficient version of isMember()const, get()const, + /// and operator[]const + /// \note As stated elsewhere, behavior is undefined if (end-begin) >= 2^30 + Value const* find(char const* begin, char const* end) const; + /// Most general and efficient version of object-mutators. + /// \note As stated elsewhere, behavior is undefined if (end-begin) >= 2^30 + /// \return non-zero, but JSON_ASSERT if this is neither object nor nullValue. + Value* demand(char const* begin, char const* end); + /// \brief Remove and return the named member. + /// + /// Do nothing if it did not exist. + /// \pre type() is objectValue or nullValue + /// \post type() is unchanged + void removeMember(const char* key); + /// Same as removeMember(const char*) + /// \param key may contain embedded nulls. + void removeMember(const String& key); + /// Same as removeMember(const char* begin, const char* end, Value* removed), + /// but 'key' is null-terminated. + bool removeMember(const char* key, Value* removed); + /** \brief Remove the named map member. + * + * Update 'removed' iff removed. + * \param key may contain embedded nulls. + * \return true iff removed (no exceptions) + */ + bool removeMember(String const& key, Value* removed); + /// Same as removeMember(String const& key, Value* removed) + bool removeMember(const char* begin, const char* end, Value* removed); + /** \brief Remove the indexed array element. + * + * O(n) expensive operations. + * Update 'removed' iff removed. + * \return true if removed (no exceptions) + */ + bool removeIndex(ArrayIndex index, Value* removed); + + /// Return true if the object has a member named key. + /// \note 'key' must be null-terminated. + bool isMember(const char* key) const; + /// Return true if the object has a member named key. + /// \param key may contain embedded nulls. + bool isMember(const String& key) const; + /// Same as isMember(String const& key)const + bool isMember(const char* begin, const char* end) const; + + /// \brief Return a list of the member names. + /// + /// If null, return an empty list. + /// \pre type() is objectValue or nullValue + /// \post if type() was nullValue, it remains nullValue + Members getMemberNames() const; + + /// \deprecated Always pass len. + JSONCPP_DEPRECATED("Use setComment(String const&) instead.") + void setComment(const char* comment, CommentPlacement placement) { + setComment(String(comment, strlen(comment)), placement); + } + /// Comments must be //... or /* ... */ + void setComment(const char* comment, size_t len, CommentPlacement placement) { + setComment(String(comment, len), placement); + } + /// Comments must be //... or /* ... */ + void setComment(String comment, CommentPlacement placement); + bool hasComment(CommentPlacement placement) const; + /// Include delimiters and embedded newlines. + String getComment(CommentPlacement placement) const; + + String toStyledString() const; + + const_iterator begin() const; + const_iterator end() const; + + iterator begin(); + iterator end(); + + // Accessors for the [start, limit) range of bytes within the JSON text from + // which this value was parsed, if any. + void setOffsetStart(ptrdiff_t start); + void setOffsetLimit(ptrdiff_t limit); + ptrdiff_t getOffsetStart() const; + ptrdiff_t getOffsetLimit() const; + +private: + void setType(ValueType v) { + bits_.value_type_ = static_cast(v); + } + bool isAllocated() const { return bits_.allocated_; } + void setIsAllocated(bool v) { bits_.allocated_ = v; } + + void initBasic(ValueType type, bool allocated = false); + void dupPayload(const Value& other); + void releasePayload(); + void dupMeta(const Value& other); + + Value& resolveReference(const char* key); + Value& resolveReference(const char* key, const char* end); + + // struct MemberNamesTransform + //{ + // typedef const char *result_type; + // const char *operator()( const CZString &name ) const + // { + // return name.c_str(); + // } + //}; + + union ValueHolder { + LargestInt int_; + LargestUInt uint_; + double real_; + bool bool_; + char* string_; // if allocated_, ptr to { unsigned, char[] }. + ObjectValues* map_; + } value_; + + struct { + // Really a ValueType, but types should agree for bitfield packing. + unsigned int value_type_ : 8; + // Unless allocated_, string_ must be null-terminated. + unsigned int allocated_ : 1; + } bits_; + + class Comments { + public: + Comments() = default; + Comments(const Comments& that); + Comments(Comments&& that) noexcept; + Comments& operator=(const Comments& that); + Comments& operator=(Comments&& that) noexcept; + bool has(CommentPlacement slot) const; + String get(CommentPlacement slot) const; + void set(CommentPlacement slot, String comment); + + private: + using Array = std::array; + std::unique_ptr ptr_; + }; + Comments comments_; + + // [start, limit) byte offsets in the source JSON text from which this Value + // was extracted. + ptrdiff_t start_; + ptrdiff_t limit_; +}; + +template <> inline bool Value::as() const { return asBool(); } +template <> inline bool Value::is() const { return isBool(); } + +template <> inline Int Value::as() const { return asInt(); } +template <> inline bool Value::is() const { return isInt(); } + +template <> inline UInt Value::as() const { return asUInt(); } +template <> inline bool Value::is() const { return isUInt(); } + +#if defined(JSON_HAS_INT64) +template <> inline Int64 Value::as() const { return asInt64(); } +template <> inline bool Value::is() const { return isInt64(); } + +template <> inline UInt64 Value::as() const { return asUInt64(); } +template <> inline bool Value::is() const { return isUInt64(); } +#endif + +template <> inline double Value::as() const { return asDouble(); } +template <> inline bool Value::is() const { return isDouble(); } + +template <> inline String Value::as() const { return asString(); } +template <> inline bool Value::is() const { return isString(); } + +/// These `as` specializations are type conversions, and do not have a +/// corresponding `is`. +template <> inline float Value::as() const { return asFloat(); } +template <> inline const char* Value::as() const { + return asCString(); +} + +/** \brief Experimental and untested: represents an element of the "path" to + * access a node. + */ +class JSON_API PathArgument { +public: + friend class Path; + + PathArgument(); + PathArgument(ArrayIndex index); + PathArgument(const char* key); + PathArgument(String key); + +private: + enum Kind { kindNone = 0, kindIndex, kindKey }; + String key_; + ArrayIndex index_{}; + Kind kind_{kindNone}; +}; + +/** \brief Experimental and untested: represents a "path" to access a node. + * + * Syntax: + * - "." => root node + * - ".[n]" => elements at index 'n' of root node (an array value) + * - ".name" => member named 'name' of root node (an object value) + * - ".name1.name2.name3" + * - ".[0][1][2].name1[3]" + * - ".%" => member name is provided as parameter + * - ".[%]" => index is provided as parameter + */ +class JSON_API Path { +public: + Path(const String& path, const PathArgument& a1 = PathArgument(), + const PathArgument& a2 = PathArgument(), + const PathArgument& a3 = PathArgument(), + const PathArgument& a4 = PathArgument(), + const PathArgument& a5 = PathArgument()); + + const Value& resolve(const Value& root) const; + Value resolve(const Value& root, const Value& defaultValue) const; + /// Creates the "path" to access the specified node and returns a reference on + /// the node. + Value& make(Value& root) const; + +private: + using InArgs = std::vector; + using Args = std::vector; + + void makePath(const String& path, const InArgs& in); + void addPathInArg(const String& path, const InArgs& in, + InArgs::const_iterator& itInArg, PathArgument::Kind kind); + static void invalidPath(const String& path, int location); + + Args args_; +}; + +/** \brief base class for Value iterators. + * + */ +class JSON_API ValueIteratorBase { +public: + using iterator_category = std::bidirectional_iterator_tag; + using size_t = unsigned int; + using difference_type = int; + using SelfType = ValueIteratorBase; + + bool operator==(const SelfType& other) const { return isEqual(other); } + + bool operator!=(const SelfType& other) const { return !isEqual(other); } + + difference_type operator-(const SelfType& other) const { + return other.computeDistance(*this); + } + + /// Return either the index or the member name of the referenced value as a + /// Value. + Value key() const; + + /// Return the index of the referenced Value, or -1 if it is not an + /// arrayValue. + UInt index() const; + + /// Return the member name of the referenced Value, or "" if it is not an + /// objectValue. + /// \note Avoid `c_str()` on result, as embedded zeroes are possible. + String name() const; + + /// Return the member name of the referenced Value. "" if it is not an + /// objectValue. + /// \deprecated This cannot be used for UTF-8 strings, since there can be + /// embedded nulls. + JSONCPP_DEPRECATED("Use `key = name();` instead.") + char const* memberName() const; + /// Return the member name of the referenced Value, or NULL if it is not an + /// objectValue. + /// \note Better version than memberName(). Allows embedded nulls. + char const* memberName(char const** end) const; + +protected: + /*! Internal utility functions to assist with implementing + * other iterator functions. The const and non-const versions + * of the "deref" protected methods expose the protected + * current_ member variable in a way that can often be + * optimized away by the compiler. + */ + const Value& deref() const; + Value& deref(); + + void increment(); + + void decrement(); + + difference_type computeDistance(const SelfType& other) const; + + bool isEqual(const SelfType& other) const; + + void copy(const SelfType& other); + +private: + Value::ObjectValues::iterator current_; + // Indicates that iterator is for a null value. + bool isNull_{true}; + +public: + // For some reason, BORLAND needs these at the end, rather + // than earlier. No idea why. + ValueIteratorBase(); + explicit ValueIteratorBase(const Value::ObjectValues::iterator& current); +}; + +/** \brief const iterator for object and array value. + * + */ +class JSON_API ValueConstIterator : public ValueIteratorBase { + friend class Value; + +public: + using value_type = const Value; + // typedef unsigned int size_t; + // typedef int difference_type; + using reference = const Value&; + using pointer = const Value*; + using SelfType = ValueConstIterator; + + ValueConstIterator(); + ValueConstIterator(ValueIterator const& other); + +private: + /*! \internal Use by Value to create an iterator. + */ + explicit ValueConstIterator(const Value::ObjectValues::iterator& current); + +public: + SelfType& operator=(const ValueIteratorBase& other); + + SelfType operator++(int) { + SelfType temp(*this); + ++*this; + return temp; + } + + SelfType operator--(int) { + SelfType temp(*this); + --*this; + return temp; + } + + SelfType& operator--() { + decrement(); + return *this; + } + + SelfType& operator++() { + increment(); + return *this; + } + + reference operator*() const { return deref(); } + + pointer operator->() const { return &deref(); } +}; + +/** \brief Iterator for object and array value. + */ +class JSON_API ValueIterator : public ValueIteratorBase { + friend class Value; + +public: + using value_type = Value; + using size_t = unsigned int; + using difference_type = int; + using reference = Value&; + using pointer = Value*; + using SelfType = ValueIterator; + + ValueIterator(); + explicit ValueIterator(const ValueConstIterator& other); + ValueIterator(const ValueIterator& other); + +private: + /*! \internal Use by Value to create an iterator. + */ + explicit ValueIterator(const Value::ObjectValues::iterator& current); + +public: + SelfType& operator=(const SelfType& other); + + SelfType operator++(int) { + SelfType temp(*this); + ++*this; + return temp; + } + + SelfType operator--(int) { + SelfType temp(*this); + --*this; + return temp; + } + + SelfType& operator--() { + decrement(); + return *this; + } + + SelfType& operator++() { + increment(); + return *this; + } + + /*! The return value of non-const iterators can be + * changed, so the these functions are not const + * because the returned references/pointers can be used + * to change state of the base class. + */ + reference operator*() const { return const_cast(deref()); } + pointer operator->() const { return const_cast(&deref()); } +}; + +inline void swap(Value& a, Value& b) { a.swap(b); } + +} // namespace Json + +#pragma pack(pop) + +#if defined(JSONCPP_DISABLE_DLL_INTERFACE_WARNING) +#pragma warning(pop) +#endif // if defined(JSONCPP_DISABLE_DLL_INTERFACE_WARNING) + +#endif // JSON_H_INCLUDED + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: include/json/value.h +// ////////////////////////////////////////////////////////////////////// + + + + + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: include/json/reader.h +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2007-2010 Baptiste Lepilleur and The JsonCpp Authors +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +#ifndef JSON_READER_H_INCLUDED +#define JSON_READER_H_INCLUDED + +#if !defined(JSON_IS_AMALGAMATION) +#include "json_features.h" +#include "value.h" +#endif // if !defined(JSON_IS_AMALGAMATION) +#include +#include +#include +#include +#include + +// Disable warning C4251: : needs to have dll-interface to +// be used by... +#if defined(JSONCPP_DISABLE_DLL_INTERFACE_WARNING) +#pragma warning(push) +#pragma warning(disable : 4251) +#endif // if defined(JSONCPP_DISABLE_DLL_INTERFACE_WARNING) + +#pragma pack(push) +#pragma pack() + +namespace Json { + +/** \brief Unserialize a JSON document into a + * Value. + * + * \deprecated Use CharReader and CharReaderBuilder. + */ + +class JSON_API Reader { +public: + using Char = char; + using Location = const Char*; + + /** \brief An error tagged with where in the JSON text it was encountered. + * + * The offsets give the [start, limit) range of bytes within the text. Note + * that this is bytes, not codepoints. + */ + struct StructuredError { + ptrdiff_t offset_start; + ptrdiff_t offset_limit; + String message; + }; + + /** \brief Constructs a Reader allowing all features for parsing. + * \deprecated Use CharReader and CharReaderBuilder. + */ + Reader(); + + /** \brief Constructs a Reader allowing the specified feature set for parsing. + * \deprecated Use CharReader and CharReaderBuilder. + */ + Reader(const Features& features); + + /** \brief Read a Value from a JSON + * document. + * + * \param document UTF-8 encoded string containing the document + * to read. + * \param[out] root Contains the root value of the document if it + * was successfully parsed. + * \param collectComments \c true to collect comment and allow writing + * them back during serialization, \c false to + * discard comments. This parameter is ignored + * if Features::allowComments_ is \c false. + * \return \c true if the document was successfully parsed, \c false if an + * error occurred. + */ + bool parse(const std::string& document, Value& root, + bool collectComments = true); + + /** \brief Read a Value from a JSON + * document. + * + * \param beginDoc Pointer on the beginning of the UTF-8 encoded + * string of the document to read. + * \param endDoc Pointer on the end of the UTF-8 encoded string + * of the document to read. Must be >= beginDoc. + * \param[out] root Contains the root value of the document if it + * was successfully parsed. + * \param collectComments \c true to collect comment and allow writing + * them back during serialization, \c false to + * discard comments. This parameter is ignored + * if Features::allowComments_ is \c false. + * \return \c true if the document was successfully parsed, \c false if an + * error occurred. + */ + bool parse(const char* beginDoc, const char* endDoc, Value& root, + bool collectComments = true); + + /// \brief Parse from input stream. + /// \see Json::operator>>(std::istream&, Json::Value&). + bool parse(IStream& is, Value& root, bool collectComments = true); + + /** \brief Returns a user friendly string that list errors in the parsed + * document. + * + * \return Formatted error message with the list of errors with their + * location in the parsed document. An empty string is returned if no error + * occurred during parsing. + * \deprecated Use getFormattedErrorMessages() instead (typo fix). + */ + JSONCPP_DEPRECATED("Use getFormattedErrorMessages() instead.") + String getFormatedErrorMessages() const; + + /** \brief Returns a user friendly string that list errors in the parsed + * document. + * + * \return Formatted error message with the list of errors with their + * location in the parsed document. An empty string is returned if no error + * occurred during parsing. + */ + String getFormattedErrorMessages() const; + + /** \brief Returns a vector of structured errors encountered while parsing. + * + * \return A (possibly empty) vector of StructuredError objects. Currently + * only one error can be returned, but the caller should tolerate multiple + * errors. This can occur if the parser recovers from a non-fatal parse + * error and then encounters additional errors. + */ + std::vector getStructuredErrors() const; + + /** \brief Add a semantic error message. + * + * \param value JSON Value location associated with the error + * \param message The error message. + * \return \c true if the error was successfully added, \c false if the Value + * offset exceeds the document size. + */ + bool pushError(const Value& value, const String& message); + + /** \brief Add a semantic error message with extra context. + * + * \param value JSON Value location associated with the error + * \param message The error message. + * \param extra Additional JSON Value location to contextualize the error + * \return \c true if the error was successfully added, \c false if either + * Value offset exceeds the document size. + */ + bool pushError(const Value& value, const String& message, const Value& extra); + + /** \brief Return whether there are any errors. + * + * \return \c true if there are no errors to report \c false if errors have + * occurred. + */ + bool good() const; + +private: + enum TokenType { + tokenEndOfStream = 0, + tokenObjectBegin, + tokenObjectEnd, + tokenArrayBegin, + tokenArrayEnd, + tokenString, + tokenNumber, + tokenTrue, + tokenFalse, + tokenNull, + tokenArraySeparator, + tokenMemberSeparator, + tokenComment, + tokenError + }; + + class Token { + public: + TokenType type_; + Location start_; + Location end_; + }; + + class ErrorInfo { + public: + Token token_; + String message_; + Location extra_; + }; + + using Errors = std::deque; + + bool readToken(Token& token); + void skipSpaces(); + bool match(const Char* pattern, int patternLength); + bool readComment(); + bool readCStyleComment(); + bool readCppStyleComment(); + bool readString(); + void readNumber(); + bool readValue(); + bool readObject(Token& token); + bool readArray(Token& token); + bool decodeNumber(Token& token); + bool decodeNumber(Token& token, Value& decoded); + bool decodeString(Token& token); + bool decodeString(Token& token, String& decoded); + bool decodeDouble(Token& token); + bool decodeDouble(Token& token, Value& decoded); + bool decodeUnicodeCodePoint(Token& token, Location& current, Location end, + unsigned int& unicode); + bool decodeUnicodeEscapeSequence(Token& token, Location& current, + Location end, unsigned int& unicode); + bool addError(const String& message, Token& token, Location extra = nullptr); + bool recoverFromError(TokenType skipUntilToken); + bool addErrorAndRecover(const String& message, Token& token, + TokenType skipUntilToken); + void skipUntilSpace(); + Value& currentValue(); + Char getNextChar(); + void getLocationLineAndColumn(Location location, int& line, + int& column) const; + String getLocationLineAndColumn(Location location) const; + void addComment(Location begin, Location end, CommentPlacement placement); + void skipCommentTokens(Token& token); + + static bool containsNewLine(Location begin, Location end); + static String normalizeEOL(Location begin, Location end); + + using Nodes = std::stack; + Nodes nodes_; + Errors errors_; + String document_; + Location begin_{}; + Location end_{}; + Location current_{}; + Location lastValueEnd_{}; + Value* lastValue_{}; + String commentsBefore_; + Features features_; + bool collectComments_{}; +}; // Reader + +/** Interface for reading JSON from a char array. + */ +class JSON_API CharReader { +public: + virtual ~CharReader() = default; + /** \brief Read a Value from a JSON + * document. The document must be a UTF-8 encoded string containing the + * document to read. + * + * \param beginDoc Pointer on the beginning of the UTF-8 encoded string + * of the document to read. + * \param endDoc Pointer on the end of the UTF-8 encoded string of the + * document to read. Must be >= beginDoc. + * \param[out] root Contains the root value of the document if it was + * successfully parsed. + * \param[out] errs Formatted error messages (if not NULL) a user + * friendly string that lists errors in the parsed + * document. + * \return \c true if the document was successfully parsed, \c false if an + * error occurred. + */ + virtual bool parse(char const* beginDoc, char const* endDoc, Value* root, + String* errs) = 0; + + class JSON_API Factory { + public: + virtual ~Factory() = default; + /** \brief Allocate a CharReader via operator new(). + * \throw std::exception if something goes wrong (e.g. invalid settings) + */ + virtual CharReader* newCharReader() const = 0; + }; // Factory +}; // CharReader + +/** \brief Build a CharReader implementation. + * + * Usage: + * \code + * using namespace Json; + * CharReaderBuilder builder; + * builder["collectComments"] = false; + * Value value; + * String errs; + * bool ok = parseFromStream(builder, std::cin, &value, &errs); + * \endcode + */ +class JSON_API CharReaderBuilder : public CharReader::Factory { +public: + // Note: We use a Json::Value so that we can add data-members to this class + // without a major version bump. + /** Configuration of this builder. + * These are case-sensitive. + * Available settings (case-sensitive): + * - `"collectComments": false or true` + * - true to collect comment and allow writing them back during + * serialization, false to discard comments. This parameter is ignored + * if allowComments is false. + * - `"allowComments": false or true` + * - true if comments are allowed. + * - `"allowTrailingCommas": false or true` + * - true if trailing commas in objects and arrays are allowed. + * - `"strictRoot": false or true` + * - true if root must be either an array or an object value + * - `"allowDroppedNullPlaceholders": false or true` + * - true if dropped null placeholders are allowed. (See + * StreamWriterBuilder.) + * - `"allowNumericKeys": false or true` + * - true if numeric object keys are allowed. + * - `"allowSingleQuotes": false or true` + * - true if '' are allowed for strings (both keys and values) + * - `"stackLimit": integer` + * - Exceeding stackLimit (recursive depth of `readValue()`) will cause an + * exception. + * - This is a security issue (seg-faults caused by deeply nested JSON), so + * the default is low. + * - `"failIfExtra": false or true` + * - If true, `parse()` returns false when extra non-whitespace trails the + * JSON value in the input string. + * - `"rejectDupKeys": false or true` + * - If true, `parse()` returns false when a key is duplicated within an + * object. + * - `"allowSpecialFloats": false or true` + * - If true, special float values (NaNs and infinities) are allowed and + * their values are lossfree restorable. + * - `"skipBom": false or true` + * - If true, if the input starts with the Unicode byte order mark (BOM), + * it is skipped. + * + * You can examine 'settings_` yourself to see the defaults. You can also + * write and read them just like any JSON Value. + * \sa setDefaults() + */ + Json::Value settings_; + + CharReaderBuilder(); + ~CharReaderBuilder() override; + + CharReader* newCharReader() const override; + + /** \return true if 'settings' are legal and consistent; + * otherwise, indicate bad settings via 'invalid'. + */ + bool validate(Json::Value* invalid) const; + + /** A simple way to update a specific setting. + */ + Value& operator[](const String& key); + + /** Called by ctor, but you can use this to reset settings_. + * \pre 'settings' != NULL (but Json::null is fine) + * \remark Defaults: + * \snippet src/lib_json/json_reader.cpp CharReaderBuilderDefaults + */ + static void setDefaults(Json::Value* settings); + /** Same as old Features::strictMode(). + * \pre 'settings' != NULL (but Json::null is fine) + * \remark Defaults: + * \snippet src/lib_json/json_reader.cpp CharReaderBuilderStrictMode + */ + static void strictMode(Json::Value* settings); +}; + +/** Consume entire stream and use its begin/end. + * Someday we might have a real StreamReader, but for now this + * is convenient. + */ +bool JSON_API parseFromStream(CharReader::Factory const&, IStream&, Value* root, + String* errs); + +/** \brief Read from 'sin' into 'root'. + * + * Always keep comments from the input JSON. + * + * This can be used to read a file into a particular sub-object. + * For example: + * \code + * Json::Value root; + * cin >> root["dir"]["file"]; + * cout << root; + * \endcode + * Result: + * \verbatim + * { + * "dir": { + * "file": { + * // The input stream JSON would be nested here. + * } + * } + * } + * \endverbatim + * \throw std::exception on parse error. + * \see Json::operator<<() + */ +JSON_API IStream& operator>>(IStream&, Value&); + +} // namespace Json + +#pragma pack(pop) + +#if defined(JSONCPP_DISABLE_DLL_INTERFACE_WARNING) +#pragma warning(pop) +#endif // if defined(JSONCPP_DISABLE_DLL_INTERFACE_WARNING) + +#endif // JSON_READER_H_INCLUDED + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: include/json/reader.h +// ////////////////////////////////////////////////////////////////////// + + + + + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: include/json/writer.h +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2007-2010 Baptiste Lepilleur and The JsonCpp Authors +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +#ifndef JSON_WRITER_H_INCLUDED +#define JSON_WRITER_H_INCLUDED + +#if !defined(JSON_IS_AMALGAMATION) +#include "value.h" +#endif // if !defined(JSON_IS_AMALGAMATION) +#include +#include +#include + +// Disable warning C4251: : needs to have dll-interface to +// be used by... +#if defined(JSONCPP_DISABLE_DLL_INTERFACE_WARNING) && defined(_MSC_VER) +#pragma warning(push) +#pragma warning(disable : 4251) +#endif // if defined(JSONCPP_DISABLE_DLL_INTERFACE_WARNING) + +#pragma pack(push) +#pragma pack() + +namespace Json { + +class Value; + +/** + * + * Usage: + * \code + * using namespace Json; + * void writeToStdout(StreamWriter::Factory const& factory, Value const& value) + * { std::unique_ptr const writer( factory.newStreamWriter()); + * writer->write(value, &std::cout); + * std::cout << std::endl; // add lf and flush + * } + * \endcode + */ +class JSON_API StreamWriter { +protected: + OStream* sout_; // not owned; will not delete +public: + StreamWriter(); + virtual ~StreamWriter(); + /** Write Value into document as configured in sub-class. + * Do not take ownership of sout, but maintain a reference during function. + * \pre sout != NULL + * \return zero on success (For now, we always return zero, so check the + * stream instead.) \throw std::exception possibly, depending on + * configuration + */ + virtual int write(Value const& root, OStream* sout) = 0; + + /** \brief A simple abstract factory. + */ + class JSON_API Factory { + public: + virtual ~Factory(); + /** \brief Allocate a CharReader via operator new(). + * \throw std::exception if something goes wrong (e.g. invalid settings) + */ + virtual StreamWriter* newStreamWriter() const = 0; + }; // Factory +}; // StreamWriter + +/** \brief Write into stringstream, then return string, for convenience. + * A StreamWriter will be created from the factory, used, and then deleted. + */ +String JSON_API writeString(StreamWriter::Factory const& factory, + Value const& root); + +/** \brief Build a StreamWriter implementation. + +* Usage: +* \code +* using namespace Json; +* Value value = ...; +* StreamWriterBuilder builder; +* builder["commentStyle"] = "None"; +* builder["indentation"] = " "; // or whatever you like +* std::unique_ptr writer( +* builder.newStreamWriter()); +* writer->write(value, &std::cout); +* std::cout << std::endl; // add lf and flush +* \endcode +*/ +class JSON_API StreamWriterBuilder : public StreamWriter::Factory { +public: + // Note: We use a Json::Value so that we can add data-members to this class + // without a major version bump. + /** Configuration of this builder. + * Available settings (case-sensitive): + * - "commentStyle": "None" or "All" + * - "indentation": "". + * - Setting this to an empty string also omits newline characters. + * - "enableYAMLCompatibility": false or true + * - slightly change the whitespace around colons + * - "dropNullPlaceholders": false or true + * - Drop the "null" string from the writer's output for nullValues. + * Strictly speaking, this is not valid JSON. But when the output is being + * fed to a browser's JavaScript, it makes for smaller output and the + * browser can handle the output just fine. + * - "useSpecialFloats": false or true + * - If true, outputs non-finite floating point values in the following way: + * NaN values as "NaN", positive infinity as "Infinity", and negative + * infinity as "-Infinity". + * - "precision": int + * - Number of precision digits for formatting of real values. + * - "precisionType": "significant"(default) or "decimal" + * - Type of precision for formatting of real values. + * - "emitUTF8": false or true + * - If true, outputs raw UTF8 strings instead of escaping them. + + * You can examine 'settings_` yourself + * to see the defaults. You can also write and read them just like any + * JSON Value. + * \sa setDefaults() + */ + Json::Value settings_; + + StreamWriterBuilder(); + ~StreamWriterBuilder() override; + + /** + * \throw std::exception if something goes wrong (e.g. invalid settings) + */ + StreamWriter* newStreamWriter() const override; + + /** \return true if 'settings' are legal and consistent; + * otherwise, indicate bad settings via 'invalid'. + */ + bool validate(Json::Value* invalid) const; + /** A simple way to update a specific setting. + */ + Value& operator[](const String& key); + + /** Called by ctor, but you can use this to reset settings_. + * \pre 'settings' != NULL (but Json::null is fine) + * \remark Defaults: + * \snippet src/lib_json/json_writer.cpp StreamWriterBuilderDefaults + */ + static void setDefaults(Json::Value* settings); +}; + +/** \brief Abstract class for writers. + * \deprecated Use StreamWriter. (And really, this is an implementation detail.) + */ +class JSON_API Writer { +public: + virtual ~Writer(); + + virtual String write(const Value& root) = 0; +}; + +/** \brief Outputs a Value in JSON format + *without formatting (not human friendly). + * + * The JSON document is written in a single line. It is not intended for 'human' + *consumption, + * but may be useful to support feature such as RPC where bandwidth is limited. + * \sa Reader, Value + * \deprecated Use StreamWriterBuilder. + */ +#if defined(_MSC_VER) +#pragma warning(push) +#pragma warning(disable : 4996) // Deriving from deprecated class +#endif +class JSON_API FastWriter + : public Writer { +public: + FastWriter(); + ~FastWriter() override = default; + + void enableYAMLCompatibility(); + + /** \brief Drop the "null" string from the writer's output for nullValues. + * Strictly speaking, this is not valid JSON. But when the output is being + * fed to a browser's JavaScript, it makes for smaller output and the + * browser can handle the output just fine. + */ + void dropNullPlaceholders(); + + void omitEndingLineFeed(); + +public: // overridden from Writer + String write(const Value& root) override; + +private: + void writeValue(const Value& value); + + String document_; + bool yamlCompatibilityEnabled_{false}; + bool dropNullPlaceholders_{false}; + bool omitEndingLineFeed_{false}; +}; +#if defined(_MSC_VER) +#pragma warning(pop) +#endif + +/** \brief Writes a Value in JSON format in a + *human friendly way. + * + * The rules for line break and indent are as follow: + * - Object value: + * - if empty then print {} without indent and line break + * - if not empty the print '{', line break & indent, print one value per + *line + * and then unindent and line break and print '}'. + * - Array value: + * - if empty then print [] without indent and line break + * - if the array contains no object value, empty array or some other value + *types, + * and all the values fit on one lines, then print the array on a single + *line. + * - otherwise, it the values do not fit on one line, or the array contains + * object or non empty array, then print one value per line. + * + * If the Value have comments then they are outputted according to their + *#CommentPlacement. + * + * \sa Reader, Value, Value::setComment() + * \deprecated Use StreamWriterBuilder. + */ +#if defined(_MSC_VER) +#pragma warning(push) +#pragma warning(disable : 4996) // Deriving from deprecated class +#endif +class JSON_API + StyledWriter : public Writer { +public: + StyledWriter(); + ~StyledWriter() override = default; + +public: // overridden from Writer + /** \brief Serialize a Value in JSON format. + * \param root Value to serialize. + * \return String containing the JSON document that represents the root value. + */ + String write(const Value& root) override; + +private: + void writeValue(const Value& value); + void writeArrayValue(const Value& value); + bool isMultilineArray(const Value& value); + void pushValue(const String& value); + void writeIndent(); + void writeWithIndent(const String& value); + void indent(); + void unindent(); + void writeCommentBeforeValue(const Value& root); + void writeCommentAfterValueOnSameLine(const Value& root); + static bool hasCommentForValue(const Value& value); + static String normalizeEOL(const String& text); + + using ChildValues = std::vector; + + ChildValues childValues_; + String document_; + String indentString_; + unsigned int rightMargin_{74}; + unsigned int indentSize_{3}; + bool addChildValues_{false}; +}; +#if defined(_MSC_VER) +#pragma warning(pop) +#endif + +/** \brief Writes a Value in JSON format in a + human friendly way, + to a stream rather than to a string. + * + * The rules for line break and indent are as follow: + * - Object value: + * - if empty then print {} without indent and line break + * - if not empty the print '{', line break & indent, print one value per + line + * and then unindent and line break and print '}'. + * - Array value: + * - if empty then print [] without indent and line break + * - if the array contains no object value, empty array or some other value + types, + * and all the values fit on one lines, then print the array on a single + line. + * - otherwise, it the values do not fit on one line, or the array contains + * object or non empty array, then print one value per line. + * + * If the Value have comments then they are outputted according to their + #CommentPlacement. + * + * \sa Reader, Value, Value::setComment() + * \deprecated Use StreamWriterBuilder. + */ +#if defined(_MSC_VER) +#pragma warning(push) +#pragma warning(disable : 4996) // Deriving from deprecated class +#endif +class JSON_API + StyledStreamWriter { +public: + /** + * \param indentation Each level will be indented by this amount extra. + */ + StyledStreamWriter(String indentation = "\t"); + ~StyledStreamWriter() = default; + +public: + /** \brief Serialize a Value in JSON format. + * \param out Stream to write to. (Can be ostringstream, e.g.) + * \param root Value to serialize. + * \note There is no point in deriving from Writer, since write() should not + * return a value. + */ + void write(OStream& out, const Value& root); + +private: + void writeValue(const Value& value); + void writeArrayValue(const Value& value); + bool isMultilineArray(const Value& value); + void pushValue(const String& value); + void writeIndent(); + void writeWithIndent(const String& value); + void indent(); + void unindent(); + void writeCommentBeforeValue(const Value& root); + void writeCommentAfterValueOnSameLine(const Value& root); + static bool hasCommentForValue(const Value& value); + static String normalizeEOL(const String& text); + + using ChildValues = std::vector; + + ChildValues childValues_; + OStream* document_; + String indentString_; + unsigned int rightMargin_{74}; + String indentation_; + bool addChildValues_ : 1; + bool indented_ : 1; +}; +#if defined(_MSC_VER) +#pragma warning(pop) +#endif + +#if defined(JSON_HAS_INT64) +String JSON_API valueToString(Int value); +String JSON_API valueToString(UInt value); +#endif // if defined(JSON_HAS_INT64) +String JSON_API valueToString(LargestInt value); +String JSON_API valueToString(LargestUInt value); +String JSON_API valueToString( + double value, unsigned int precision = Value::defaultRealPrecision, + PrecisionType precisionType = PrecisionType::significantDigits); +String JSON_API valueToString(bool value); +String JSON_API valueToQuotedString(const char* value); + +/// \brief Output using the StyledStreamWriter. +/// \see Json::operator>>() +JSON_API OStream& operator<<(OStream&, const Value& root); + +} // namespace Json + +#pragma pack(pop) + +#if defined(JSONCPP_DISABLE_DLL_INTERFACE_WARNING) +#pragma warning(pop) +#endif // if defined(JSONCPP_DISABLE_DLL_INTERFACE_WARNING) + +#endif // JSON_WRITER_H_INCLUDED + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: include/json/writer.h +// ////////////////////////////////////////////////////////////////////// + + + + + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: include/json/assertions.h +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2007-2010 Baptiste Lepilleur and The JsonCpp Authors +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +#ifndef JSON_ASSERTIONS_H_INCLUDED +#define JSON_ASSERTIONS_H_INCLUDED + +#include +#include + +#if !defined(JSON_IS_AMALGAMATION) +#include "config.h" +#endif // if !defined(JSON_IS_AMALGAMATION) + +/** It should not be possible for a maliciously designed file to + * cause an abort() or seg-fault, so these macros are used only + * for pre-condition violations and internal logic errors. + */ +#if JSON_USE_EXCEPTION + +// @todo <= add detail about condition in exception +#define JSON_ASSERT(condition) \ + do { \ + if (!(condition)) { \ + Json::throwLogicError("assert json failed"); \ + } \ + } while (0) + +#define JSON_FAIL_MESSAGE(message) \ + do { \ + OStringStream oss; \ + oss << message; \ + Json::throwLogicError(oss.str()); \ + abort(); \ + } while (0) + +#else // JSON_USE_EXCEPTION + +#define JSON_ASSERT(condition) assert(condition) + +// The call to assert() will show the failure message in debug builds. In +// release builds we abort, for a core-dump or debugger. +#define JSON_FAIL_MESSAGE(message) \ + { \ + OStringStream oss; \ + oss << message; \ + assert(false && oss.str().c_str()); \ + abort(); \ + } + +#endif + +#define JSON_ASSERT_MESSAGE(condition, message) \ + do { \ + if (!(condition)) { \ + JSON_FAIL_MESSAGE(message); \ + } \ + } while (0) + +#endif // JSON_ASSERTIONS_H_INCLUDED + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: include/json/assertions.h +// ////////////////////////////////////////////////////////////////////// + + + + + +#endif //ifndef JSON_AMALGAMATED_H_INCLUDED diff --git a/src/task_manager/include/task_manager/md5.h b/src/task_manager/include/task_manager/md5.h new file mode 100644 index 0000000..df10a8c --- /dev/null +++ b/src/task_manager/include/task_manager/md5.h @@ -0,0 +1,56 @@ +#ifndef MD5_H +#define MD5_H + +#include +#include + +/* Type define */ +typedef unsigned char byte; +typedef unsigned int uint32; + +using std::ifstream; +using std::string; + +/* MD5 declaration. */ +class MD5 +{ +public: + MD5(); + MD5(const void *input, size_t length); + MD5(const string &str); + MD5(ifstream &in); + void update(const void *input, size_t length); + void update(const string &str); + void update(ifstream &in); + const byte *digest(); + string toString(); + void reset(); + +private: + void update(const byte *input, size_t length); + void final(); + void transform(const byte block[64]); + void encode(const uint32 *input, byte *output, size_t length); + void decode(const byte *input, uint32 *output, size_t length); + string bytesToHexString(const byte *input, size_t length); + + /* class uncopyable */ + MD5(const MD5 &); + MD5 &operator=(const MD5 &); + +private: + uint32 _state[4]; /* state (ABCD) */ + uint32 _count[2]; /* number of bits, modulo 2^64 (low-order word first) */ + byte _buffer[64]; /* input buffer */ + byte _digest[16]; /* message digest */ + bool _finished; /* calculate finished ? */ + + static const byte PADDING[64]; /* padding for calculate */ + static const char HEX[16]; + enum + { + BUFFER_SIZE = 1024 + }; +}; + +#endif /*MD5_H*/ \ No newline at end of file diff --git a/src/task_manager/include/task_manager/task_manager_node.hpp b/src/task_manager/include/task_manager/task_manager_node.hpp new file mode 100644 index 0000000..a7bbe2a --- /dev/null +++ b/src/task_manager/include/task_manager/task_manager_node.hpp @@ -0,0 +1,98 @@ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifndef __SUB_NODE_H__ +#define __SUB_NODE_H__ +using namespace std; + +/*示例： +{ + "type": "request", //消息类型 + "seqNo": 1, //流水号 + "data": { + "command": "start", + "value": { + "id": 1, + "name": "任务11", //任务名称 + "mode": 0, //清扫模式 + "count": 1, //清扫次数 + "routeInfo": { //路径信息 + "routeName": "测试路径1", //路径名称 + "fileName": "gps_load_1736931111771.txt", //文件名称 + "url": "http://192.168.4.117:9600/file/gps_load_1736931111771.txt", //路径地址 + "md5": "93c635ea6251f650dfcf0f9a8a72351c" //md5值 + } + } + } +}*/ + +struct ROUTE_INFO +{ + string routeName; + string fileName; + string url; + string md5; +}; + +struct TASK_VALUE +{ + long id; + string name; + int mode; + int count; + ROUTE_INFO routeInfo; +}; +struct JSON_DATA +{ + string command; + TASK_VALUE value; +}; + +struct MQTT_JSON +{ + string type; + long seqNo; + boost::any data; +}; + +// 任务状态定义 +enum TaskStatus +{ + PENDING = 0, // 待执行 + RUNNING = 1, // 执行中 + COMPLETED = 2, // 执行完成 + FAILED = 3, // 执行异常 +}; + +// 当前任务信息 +struct CurrentTask +{ + int id; + TaskStatus status; + std::chrono::time_point lastUpdateTime; + + CurrentTask() : id(0), status(PENDING) {} +}; + +// extern TASK_VALUE task_value; +// extern ROUTE_INFO routeInfo; + +// extern MQTT_JSON mqtt_json; +// extern ROUTE_INFO routeInfo; + +#endif \ No newline at end of file diff --git a/src/task_manager/lib/libpaho-mqtt3c-static.a b/src/task_manager/lib/libpaho-mqtt3c-static.a new file mode 100644 index 0000000..cbfe668 Binary files /dev/null and b/src/task_manager/lib/libpaho-mqtt3c-static.a differ diff --git a/src/task_manager/lib/libpaho-mqtt3c.a b/src/task_manager/lib/libpaho-mqtt3c.a new file mode 100644 index 0000000..9f7e1d5 Binary files /dev/null and b/src/task_manager/lib/libpaho-mqtt3c.a differ diff --git a/src/task_manager/package.xml b/src/task_manager/package.xml new file mode 100644 index 0000000..a7e2c48 --- /dev/null +++ b/src/task_manager/package.xml @@ -0,0 +1,22 @@ + + + + task_manager + 0.0.0 + TODO: Package description + nvidia + TODO: License declaration + + ament_cmake + + rclcpp + std_msgs + sweeper_interfaces + + ament_lint_auto + ament_lint_common + + + ament_cmake + + \ No newline at end of file diff --git a/src/task_manager/src/jsoncpp.cpp b/src/task_manager/src/jsoncpp.cpp new file mode 100644 index 0000000..89b7bc0 --- /dev/null +++ b/src/task_manager/src/jsoncpp.cpp @@ -0,0 +1,5342 @@ +/// Json-cpp amalgamated source (http://jsoncpp.sourceforge.net/). +/// It is intended to be used with #include "json/json.h" + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: LICENSE +// ////////////////////////////////////////////////////////////////////// + +/* +The JsonCpp library's source code, including accompanying documentation, +tests and demonstration applications, are licensed under the following +conditions... + +Baptiste Lepilleur and The JsonCpp Authors explicitly disclaim copyright in all +jurisdictions which recognize such a disclaimer. In such jurisdictions, +this software is released into the Public Domain. + +In jurisdictions which do not recognize Public Domain property (e.g. Germany as of +2010), this software is Copyright (c) 2007-2010 by Baptiste Lepilleur and +The JsonCpp Authors, and is released under the terms of the MIT License (see below). + +In jurisdictions which recognize Public Domain property, the user of this +software may choose to accept it either as 1) Public Domain, 2) under the +conditions of the MIT License (see below), or 3) under the terms of dual +Public Domain/MIT License conditions described here, as they choose. + +The MIT License is about as close to Public Domain as a license can get, and is +described in clear, concise terms at: + + http://en.wikipedia.org/wiki/MIT_License + +The full text of the MIT License follows: + +======================================================================== +Copyright (c) 2007-2010 Baptiste Lepilleur and The JsonCpp Authors + +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. +======================================================================== +(END LICENSE TEXT) + +The MIT license is compatible with both the GPL and commercial +software, affording one all of the rights of Public Domain with the +minor nuisance of being required to keep the above copyright notice +and license text in the source code. Note also that by accepting the +Public Domain "license" you can re-license your copy using whatever +license you like. + +*/ + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: LICENSE +// ////////////////////////////////////////////////////////////////////// + + + + + + +#include "json.h" + +#ifndef JSON_IS_AMALGAMATION +#error "Compile with -I PATH_TO_JSON_DIRECTORY" +#endif + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: src/lib_json/json_tool.h +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2007-2010 Baptiste Lepilleur and The JsonCpp Authors +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +#ifndef LIB_JSONCPP_JSON_TOOL_H_INCLUDED +#define LIB_JSONCPP_JSON_TOOL_H_INCLUDED + +#if !defined(JSON_IS_AMALGAMATION) +#include +#endif + +// Also support old flag NO_LOCALE_SUPPORT +#ifdef NO_LOCALE_SUPPORT +#define JSONCPP_NO_LOCALE_SUPPORT +#endif + +#ifndef JSONCPP_NO_LOCALE_SUPPORT +#include +#endif + +/* This header provides common string manipulation support, such as UTF-8, + * portable conversion from/to string... + * + * It is an internal header that must not be exposed. + */ + +namespace Json { +static inline char getDecimalPoint() { +#ifdef JSONCPP_NO_LOCALE_SUPPORT + return '\0'; +#else + struct lconv* lc = localeconv(); + return lc ? *(lc->decimal_point) : '\0'; +#endif +} + +/// Converts a unicode code-point to UTF-8. +static inline String codePointToUTF8(unsigned int cp) { + String result; + + // based on description from http://en.wikipedia.org/wiki/UTF-8 + + if (cp <= 0x7f) { + result.resize(1); + result[0] = static_cast(cp); + } else if (cp <= 0x7FF) { + result.resize(2); + result[1] = static_cast(0x80 | (0x3f & cp)); + result[0] = static_cast(0xC0 | (0x1f & (cp >> 6))); + } else if (cp <= 0xFFFF) { + result.resize(3); + result[2] = static_cast(0x80 | (0x3f & cp)); + result[1] = static_cast(0x80 | (0x3f & (cp >> 6))); + result[0] = static_cast(0xE0 | (0xf & (cp >> 12))); + } else if (cp <= 0x10FFFF) { + result.resize(4); + result[3] = static_cast(0x80 | (0x3f & cp)); + result[2] = static_cast(0x80 | (0x3f & (cp >> 6))); + result[1] = static_cast(0x80 | (0x3f & (cp >> 12))); + result[0] = static_cast(0xF0 | (0x7 & (cp >> 18))); + } + + return result; +} + +enum { + /// Constant that specify the size of the buffer that must be passed to + /// uintToString. + uintToStringBufferSize = 3 * sizeof(LargestUInt) + 1 +}; + +// Defines a char buffer for use with uintToString(). +using UIntToStringBuffer = char[uintToStringBufferSize]; + +/** Converts an unsigned integer to string. + * @param value Unsigned integer to convert to string + * @param current Input/Output string buffer. + * Must have at least uintToStringBufferSize chars free. + */ +static inline void uintToString(LargestUInt value, char*& current) { + *--current = 0; + do { + *--current = static_cast(value % 10U + static_cast('0')); + value /= 10; + } while (value != 0); +} + +/** Change ',' to '.' everywhere in buffer. + * + * We had a sophisticated way, but it did not work in WinCE. + * @see https://github.com/open-source-parsers/jsoncpp/pull/9 + */ +template Iter fixNumericLocale(Iter begin, Iter end) { + for (; begin != end; ++begin) { + if (*begin == ',') { + *begin = '.'; + } + } + return begin; +} + +template void fixNumericLocaleInput(Iter begin, Iter end) { + char decimalPoint = getDecimalPoint(); + if (decimalPoint == '\0' || decimalPoint == '.') { + return; + } + for (; begin != end; ++begin) { + if (*begin == '.') { + *begin = decimalPoint; + } + } +} + +/** + * Return iterator that would be the new end of the range [begin,end), if we + * were to delete zeros in the end of string, but not the last zero before '.'. + */ +template +Iter fixZerosInTheEnd(Iter begin, Iter end, unsigned int precision) { + for (; begin != end; --end) { + if (*(end - 1) != '0') { + return end; + } + // Don't delete the last zero before the decimal point. + if (begin != (end - 1) && begin != (end - 2) && *(end - 2) == '.') { + if (precision) { + return end; + } + return end - 2; + } + } + return end; +} + +} // namespace Json + +#endif // LIB_JSONCPP_JSON_TOOL_H_INCLUDED + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: src/lib_json/json_tool.h +// ////////////////////////////////////////////////////////////////////// + + + + + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: src/lib_json/json_reader.cpp +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2007-2011 Baptiste Lepilleur and The JsonCpp Authors +// Copyright (C) 2016 InfoTeCS JSC. All rights reserved. +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +#if !defined(JSON_IS_AMALGAMATION) +#include "json_tool.h" +#include +#include +#include +#endif // if !defined(JSON_IS_AMALGAMATION) +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#if __cplusplus >= 201103L + +#if !defined(sscanf) +#define sscanf std::sscanf +#endif + +#endif //__cplusplus + +#if defined(_MSC_VER) +#if !defined(_CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES) +#define _CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES 1 +#endif //_CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES +#endif //_MSC_VER + +#if defined(_MSC_VER) +// Disable warning about strdup being deprecated. +#pragma warning(disable : 4996) +#endif + +// Define JSONCPP_DEPRECATED_STACK_LIMIT as an appropriate integer at compile +// time to change the stack limit +#if !defined(JSONCPP_DEPRECATED_STACK_LIMIT) +#define JSONCPP_DEPRECATED_STACK_LIMIT 1000 +#endif + +static size_t const stackLimit_g = + JSONCPP_DEPRECATED_STACK_LIMIT; // see readValue() + +namespace Json { + +#if __cplusplus >= 201103L || (defined(_CPPLIB_VER) && _CPPLIB_VER >= 520) +using CharReaderPtr = std::unique_ptr; +#else +using CharReaderPtr = std::auto_ptr; +#endif + +// Implementation of class Features +// //////////////////////////////// + +Features::Features() = default; + +Features Features::all() { return {}; } + +Features Features::strictMode() { + Features features; + features.allowComments_ = false; + features.strictRoot_ = true; + features.allowDroppedNullPlaceholders_ = false; + features.allowNumericKeys_ = false; + return features; +} + +// Implementation of class Reader +// //////////////////////////////// + +bool Reader::containsNewLine(Reader::Location begin, Reader::Location end) { + return std::any_of(begin, end, [](char b) { return b == '\n' || b == '\r'; }); +} + +// Class Reader +// ////////////////////////////////////////////////////////////////// + +Reader::Reader() : features_(Features::all()) {} + +Reader::Reader(const Features& features) : features_(features) {} + +bool Reader::parse(const std::string& document, Value& root, + bool collectComments) { + document_.assign(document.begin(), document.end()); + const char* begin = document_.c_str(); + const char* end = begin + document_.length(); + return parse(begin, end, root, collectComments); +} + +bool Reader::parse(std::istream& is, Value& root, bool collectComments) { + // std::istream_iterator begin(is); + // std::istream_iterator end; + // Those would allow streamed input from a file, if parse() were a + // template function. + + // Since String is reference-counted, this at least does not + // create an extra copy. + String doc(std::istreambuf_iterator(is), {}); + return parse(doc.data(), doc.data() + doc.size(), root, collectComments); +} + +bool Reader::parse(const char* beginDoc, const char* endDoc, Value& root, + bool collectComments) { + if (!features_.allowComments_) { + collectComments = false; + } + + begin_ = beginDoc; + end_ = endDoc; + collectComments_ = collectComments; + current_ = begin_; + lastValueEnd_ = nullptr; + lastValue_ = nullptr; + commentsBefore_.clear(); + errors_.clear(); + while (!nodes_.empty()) + nodes_.pop(); + nodes_.push(&root); + + bool successful = readValue(); + Token token; + skipCommentTokens(token); + if (collectComments_ && !commentsBefore_.empty()) + root.setComment(commentsBefore_, commentAfter); + if (features_.strictRoot_) { + if (!root.isArray() && !root.isObject()) { + // Set error location to start of doc, ideally should be first token found + // in doc + token.type_ = tokenError; + token.start_ = beginDoc; + token.end_ = endDoc; + addError( + "A valid JSON document must be either an array or an object value.", + token); + return false; + } + } + return successful; +} + +bool Reader::readValue() { + // readValue() may call itself only if it calls readObject() or ReadArray(). + // These methods execute nodes_.push() just before and nodes_.pop)() just + // after calling readValue(). parse() executes one nodes_.push(), so > instead + // of >=. + if (nodes_.size() > stackLimit_g) + throwRuntimeError("Exceeded stackLimit in readValue()."); + + Token token; + skipCommentTokens(token); + bool successful = true; + + if (collectComments_ && !commentsBefore_.empty()) { + currentValue().setComment(commentsBefore_, commentBefore); + commentsBefore_.clear(); + } + + switch (token.type_) { + case tokenObjectBegin: + successful = readObject(token); + currentValue().setOffsetLimit(current_ - begin_); + break; + case tokenArrayBegin: + successful = readArray(token); + currentValue().setOffsetLimit(current_ - begin_); + break; + case tokenNumber: + successful = decodeNumber(token); + break; + case tokenString: + successful = decodeString(token); + break; + case tokenTrue: { + Value v(true); + currentValue().swapPayload(v); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + } break; + case tokenFalse: { + Value v(false); + currentValue().swapPayload(v); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + } break; + case tokenNull: { + Value v; + currentValue().swapPayload(v); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + } break; + case tokenArraySeparator: + case tokenObjectEnd: + case tokenArrayEnd: + if (features_.allowDroppedNullPlaceholders_) { + // "Un-read" the current token and mark the current value as a null + // token. + current_--; + Value v; + currentValue().swapPayload(v); + currentValue().setOffsetStart(current_ - begin_ - 1); + currentValue().setOffsetLimit(current_ - begin_); + break; + } // Else, fall through... + default: + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + return addError("Syntax error: value, object or array expected.", token); + } + + if (collectComments_) { + lastValueEnd_ = current_; + lastValue_ = ¤tValue(); + } + + return successful; +} + +void Reader::skipCommentTokens(Token& token) { + if (features_.allowComments_) { + do { + readToken(token); + } while (token.type_ == tokenComment); + } else { + readToken(token); + } +} + +bool Reader::readToken(Token& token) { + skipSpaces(); + token.start_ = current_; + Char c = getNextChar(); + bool ok = true; + switch (c) { + case '{': + token.type_ = tokenObjectBegin; + break; + case '}': + token.type_ = tokenObjectEnd; + break; + case '[': + token.type_ = tokenArrayBegin; + break; + case ']': + token.type_ = tokenArrayEnd; + break; + case '"': + token.type_ = tokenString; + ok = readString(); + break; + case '/': + token.type_ = tokenComment; + ok = readComment(); + break; + case '0': + case '1': + case '2': + case '3': + case '4': + case '5': + case '6': + case '7': + case '8': + case '9': + case '-': + token.type_ = tokenNumber; + readNumber(); + break; + case 't': + token.type_ = tokenTrue; + ok = match("rue", 3); + break; + case 'f': + token.type_ = tokenFalse; + ok = match("alse", 4); + break; + case 'n': + token.type_ = tokenNull; + ok = match("ull", 3); + break; + case ',': + token.type_ = tokenArraySeparator; + break; + case ':': + token.type_ = tokenMemberSeparator; + break; + case 0: + token.type_ = tokenEndOfStream; + break; + default: + ok = false; + break; + } + if (!ok) + token.type_ = tokenError; + token.end_ = current_; + return ok; +} + +void Reader::skipSpaces() { + while (current_ != end_) { + Char c = *current_; + if (c == ' ' || c == '\t' || c == '\r' || c == '\n') + ++current_; + else + break; + } +} + +bool Reader::match(const Char* pattern, int patternLength) { + if (end_ - current_ < patternLength) + return false; + int index = patternLength; + while (index--) + if (current_[index] != pattern[index]) + return false; + current_ += patternLength; + return true; +} + +bool Reader::readComment() { + Location commentBegin = current_ - 1; + Char c = getNextChar(); + bool successful = false; + if (c == '*') + successful = readCStyleComment(); + else if (c == '/') + successful = readCppStyleComment(); + if (!successful) + return false; + + if (collectComments_) { + CommentPlacement placement = commentBefore; + if (lastValueEnd_ && !containsNewLine(lastValueEnd_, commentBegin)) { + if (c != '*' || !containsNewLine(commentBegin, current_)) + placement = commentAfterOnSameLine; + } + + addComment(commentBegin, current_, placement); + } + return true; +} + +String Reader::normalizeEOL(Reader::Location begin, Reader::Location end) { + String normalized; + normalized.reserve(static_cast(end - begin)); + Reader::Location current = begin; + while (current != end) { + char c = *current++; + if (c == '\r') { + if (current != end && *current == '\n') + // convert dos EOL + ++current; + // convert Mac EOL + normalized += '\n'; + } else { + normalized += c; + } + } + return normalized; +} + +void Reader::addComment(Location begin, Location end, + CommentPlacement placement) { + assert(collectComments_); + const String& normalized = normalizeEOL(begin, end); + if (placement == commentAfterOnSameLine) { + assert(lastValue_ != nullptr); + lastValue_->setComment(normalized, placement); + } else { + commentsBefore_ += normalized; + } +} + +bool Reader::readCStyleComment() { + while ((current_ + 1) < end_) { + Char c = getNextChar(); + if (c == '*' && *current_ == '/') + break; + } + return getNextChar() == '/'; +} + +bool Reader::readCppStyleComment() { + while (current_ != end_) { + Char c = getNextChar(); + if (c == '\n') + break; + if (c == '\r') { + // Consume DOS EOL. It will be normalized in addComment. + if (current_ != end_ && *current_ == '\n') + getNextChar(); + // Break on Moc OS 9 EOL. + break; + } + } + return true; +} + +void Reader::readNumber() { + Location p = current_; + char c = '0'; // stopgap for already consumed character + // integral part + while (c >= '0' && c <= '9') + c = (current_ = p) < end_ ? *p++ : '\0'; + // fractional part + if (c == '.') { + c = (current_ = p) < end_ ? *p++ : '\0'; + while (c >= '0' && c <= '9') + c = (current_ = p) < end_ ? *p++ : '\0'; + } + // exponential part + if (c == 'e' || c == 'E') { + c = (current_ = p) < end_ ? *p++ : '\0'; + if (c == '+' || c == '-') + c = (current_ = p) < end_ ? *p++ : '\0'; + while (c >= '0' && c <= '9') + c = (current_ = p) < end_ ? *p++ : '\0'; + } +} + +bool Reader::readString() { + Char c = '\0'; + while (current_ != end_) { + c = getNextChar(); + if (c == '\\') + getNextChar(); + else if (c == '"') + break; + } + return c == '"'; +} + +bool Reader::readObject(Token& token) { + Token tokenName; + String name; + Value init(objectValue); + currentValue().swapPayload(init); + currentValue().setOffsetStart(token.start_ - begin_); + while (readToken(tokenName)) { + bool initialTokenOk = true; + while (tokenName.type_ == tokenComment && initialTokenOk) + initialTokenOk = readToken(tokenName); + if (!initialTokenOk) + break; + if (tokenName.type_ == tokenObjectEnd && name.empty()) // empty object + return true; + name.clear(); + if (tokenName.type_ == tokenString) { + if (!decodeString(tokenName, name)) + return recoverFromError(tokenObjectEnd); + } else if (tokenName.type_ == tokenNumber && features_.allowNumericKeys_) { + Value numberName; + if (!decodeNumber(tokenName, numberName)) + return recoverFromError(tokenObjectEnd); + name = numberName.asString(); + } else { + break; + } + + Token colon; + if (!readToken(colon) || colon.type_ != tokenMemberSeparator) { + return addErrorAndRecover("Missing ':' after object member name", colon, + tokenObjectEnd); + } + Value& value = currentValue()[name]; + nodes_.push(&value); + bool ok = readValue(); + nodes_.pop(); + if (!ok) // error already set + return recoverFromError(tokenObjectEnd); + + Token comma; + if (!readToken(comma) || + (comma.type_ != tokenObjectEnd && comma.type_ != tokenArraySeparator && + comma.type_ != tokenComment)) { + return addErrorAndRecover("Missing ',' or '}' in object declaration", + comma, tokenObjectEnd); + } + bool finalizeTokenOk = true; + while (comma.type_ == tokenComment && finalizeTokenOk) + finalizeTokenOk = readToken(comma); + if (comma.type_ == tokenObjectEnd) + return true; + } + return addErrorAndRecover("Missing '}' or object member name", tokenName, + tokenObjectEnd); +} + +bool Reader::readArray(Token& token) { + Value init(arrayValue); + currentValue().swapPayload(init); + currentValue().setOffsetStart(token.start_ - begin_); + skipSpaces(); + if (current_ != end_ && *current_ == ']') // empty array + { + Token endArray; + readToken(endArray); + return true; + } + int index = 0; + for (;;) { + Value& value = currentValue()[index++]; + nodes_.push(&value); + bool ok = readValue(); + nodes_.pop(); + if (!ok) // error already set + return recoverFromError(tokenArrayEnd); + + Token currentToken; + // Accept Comment after last item in the array. + ok = readToken(currentToken); + while (currentToken.type_ == tokenComment && ok) { + ok = readToken(currentToken); + } + bool badTokenType = (currentToken.type_ != tokenArraySeparator && + currentToken.type_ != tokenArrayEnd); + if (!ok || badTokenType) { + return addErrorAndRecover("Missing ',' or ']' in array declaration", + currentToken, tokenArrayEnd); + } + if (currentToken.type_ == tokenArrayEnd) + break; + } + return true; +} + +bool Reader::decodeNumber(Token& token) { + Value decoded; + if (!decodeNumber(token, decoded)) + return false; + currentValue().swapPayload(decoded); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + return true; +} + +bool Reader::decodeNumber(Token& token, Value& decoded) { + // Attempts to parse the number as an integer. If the number is + // larger than the maximum supported value of an integer then + // we decode the number as a double. + Location current = token.start_; + bool isNegative = *current == '-'; + if (isNegative) + ++current; + // TODO: Help the compiler do the div and mod at compile time or get rid of + // them. + Value::LargestUInt maxIntegerValue = + isNegative ? Value::LargestUInt(Value::maxLargestInt) + 1 + : Value::maxLargestUInt; + Value::LargestUInt threshold = maxIntegerValue / 10; + Value::LargestUInt value = 0; + while (current < token.end_) { + Char c = *current++; + if (c < '0' || c > '9') + return decodeDouble(token, decoded); + auto digit(static_cast(c - '0')); + if (value >= threshold) { + // We've hit or exceeded the max value divided by 10 (rounded down). If + // a) we've only just touched the limit, b) this is the last digit, and + // c) it's small enough to fit in that rounding delta, we're okay. + // Otherwise treat this number as a double to avoid overflow. + if (value > threshold || current != token.end_ || + digit > maxIntegerValue % 10) { + return decodeDouble(token, decoded); + } + } + value = value * 10 + digit; + } + if (isNegative && value == maxIntegerValue) + decoded = Value::minLargestInt; + else if (isNegative) + decoded = -Value::LargestInt(value); + else if (value <= Value::LargestUInt(Value::maxInt)) + decoded = Value::LargestInt(value); + else + decoded = value; + return true; +} + +bool Reader::decodeDouble(Token& token) { + Value decoded; + if (!decodeDouble(token, decoded)) + return false; + currentValue().swapPayload(decoded); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + return true; +} + +bool Reader::decodeDouble(Token& token, Value& decoded) { + double value = 0; + String buffer(token.start_, token.end_); + IStringStream is(buffer); + if (!(is >> value)) { + if (value == std::numeric_limits::max()) + value = std::numeric_limits::infinity(); + else if (value == std::numeric_limits::lowest()) + value = -std::numeric_limits::infinity(); + else if (!std::isinf(value)) + return addError( + "'" + String(token.start_, token.end_) + "' is not a number.", token); + } + decoded = value; + return true; +} + +bool Reader::decodeString(Token& token) { + String decoded_string; + if (!decodeString(token, decoded_string)) + return false; + Value decoded(decoded_string); + currentValue().swapPayload(decoded); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + return true; +} + +bool Reader::decodeString(Token& token, String& decoded) { + decoded.reserve(static_cast(token.end_ - token.start_ - 2)); + Location current = token.start_ + 1; // skip '"' + Location end = token.end_ - 1; // do not include '"' + while (current != end) { + Char c = *current++; + if (c == '"') + break; + if (c == '\\') { + if (current == end) + return addError("Empty escape sequence in string", token, current); + Char escape = *current++; + switch (escape) { + case '"': + decoded += '"'; + break; + case '/': + decoded += '/'; + break; + case '\\': + decoded += '\\'; + break; + case 'b': + decoded += '\b'; + break; + case 'f': + decoded += '\f'; + break; + case 'n': + decoded += '\n'; + break; + case 'r': + decoded += '\r'; + break; + case 't': + decoded += '\t'; + break; + case 'u': { + unsigned int unicode; + if (!decodeUnicodeCodePoint(token, current, end, unicode)) + return false; + decoded += codePointToUTF8(unicode); + } break; + default: + return addError("Bad escape sequence in string", token, current); + } + } else { + decoded += c; + } + } + return true; +} + +bool Reader::decodeUnicodeCodePoint(Token& token, Location& current, + Location end, unsigned int& unicode) { + + if (!decodeUnicodeEscapeSequence(token, current, end, unicode)) + return false; + if (unicode >= 0xD800 && unicode <= 0xDBFF) { + // surrogate pairs + if (end - current < 6) + return addError( + "additional six characters expected to parse unicode surrogate pair.", + token, current); + if (*(current++) == '\\' && *(current++) == 'u') { + unsigned int surrogatePair; + if (decodeUnicodeEscapeSequence(token, current, end, surrogatePair)) { + unicode = 0x10000 + ((unicode & 0x3FF) << 10) + (surrogatePair & 0x3FF); + } else + return false; + } else + return addError("expecting another \\u token to begin the second half of " + "a unicode surrogate pair", + token, current); + } + return true; +} + +bool Reader::decodeUnicodeEscapeSequence(Token& token, Location& current, + Location end, + unsigned int& ret_unicode) { + if (end - current < 4) + return addError( + "Bad unicode escape sequence in string: four digits expected.", token, + current); + int unicode = 0; + for (int index = 0; index < 4; ++index) { + Char c = *current++; + unicode *= 16; + if (c >= '0' && c <= '9') + unicode += c - '0'; + else if (c >= 'a' && c <= 'f') + unicode += c - 'a' + 10; + else if (c >= 'A' && c <= 'F') + unicode += c - 'A' + 10; + else + return addError( + "Bad unicode escape sequence in string: hexadecimal digit expected.", + token, current); + } + ret_unicode = static_cast(unicode); + return true; +} + +bool Reader::addError(const String& message, Token& token, Location extra) { + ErrorInfo info; + info.token_ = token; + info.message_ = message; + info.extra_ = extra; + errors_.push_back(info); + return false; +} + +bool Reader::recoverFromError(TokenType skipUntilToken) { + size_t const errorCount = errors_.size(); + Token skip; + for (;;) { + if (!readToken(skip)) + errors_.resize(errorCount); // discard errors caused by recovery + if (skip.type_ == skipUntilToken || skip.type_ == tokenEndOfStream) + break; + } + errors_.resize(errorCount); + return false; +} + +bool Reader::addErrorAndRecover(const String& message, Token& token, + TokenType skipUntilToken) { + addError(message, token); + return recoverFromError(skipUntilToken); +} + +Value& Reader::currentValue() { return *(nodes_.top()); } + +Reader::Char Reader::getNextChar() { + if (current_ == end_) + return 0; + return *current_++; +} + +void Reader::getLocationLineAndColumn(Location location, int& line, + int& column) const { + Location current = begin_; + Location lastLineStart = current; + line = 0; + while (current < location && current != end_) { + Char c = *current++; + if (c == '\r') { + if (*current == '\n') + ++current; + lastLineStart = current; + ++line; + } else if (c == '\n') { + lastLineStart = current; + ++line; + } + } + // column & line start at 1 + column = int(location - lastLineStart) + 1; + ++line; +} + +String Reader::getLocationLineAndColumn(Location location) const { + int line, column; + getLocationLineAndColumn(location, line, column); + char buffer[18 + 16 + 16 + 1]; + jsoncpp_snprintf(buffer, sizeof(buffer), "Line %d, Column %d", line, column); + return buffer; +} + +// Deprecated. Preserved for backward compatibility +String Reader::getFormatedErrorMessages() const { + return getFormattedErrorMessages(); +} + +String Reader::getFormattedErrorMessages() const { + String formattedMessage; + for (const auto& error : errors_) { + formattedMessage += + "* " + getLocationLineAndColumn(error.token_.start_) + "\n"; + formattedMessage += " " + error.message_ + "\n"; + if (error.extra_) + formattedMessage += + "See " + getLocationLineAndColumn(error.extra_) + " for detail.\n"; + } + return formattedMessage; +} + +std::vector Reader::getStructuredErrors() const { + std::vector allErrors; + for (const auto& error : errors_) { + Reader::StructuredError structured; + structured.offset_start = error.token_.start_ - begin_; + structured.offset_limit = error.token_.end_ - begin_; + structured.message = error.message_; + allErrors.push_back(structured); + } + return allErrors; +} + +bool Reader::pushError(const Value& value, const String& message) { + ptrdiff_t const length = end_ - begin_; + if (value.getOffsetStart() > length || value.getOffsetLimit() > length) + return false; + Token token; + token.type_ = tokenError; + token.start_ = begin_ + value.getOffsetStart(); + token.end_ = begin_ + value.getOffsetLimit(); + ErrorInfo info; + info.token_ = token; + info.message_ = message; + info.extra_ = nullptr; + errors_.push_back(info); + return true; +} + +bool Reader::pushError(const Value& value, const String& message, + const Value& extra) { + ptrdiff_t const length = end_ - begin_; + if (value.getOffsetStart() > length || value.getOffsetLimit() > length || + extra.getOffsetLimit() > length) + return false; + Token token; + token.type_ = tokenError; + token.start_ = begin_ + value.getOffsetStart(); + token.end_ = begin_ + value.getOffsetLimit(); + ErrorInfo info; + info.token_ = token; + info.message_ = message; + info.extra_ = begin_ + extra.getOffsetStart(); + errors_.push_back(info); + return true; +} + +bool Reader::good() const { return errors_.empty(); } + +// Originally copied from the Features class (now deprecated), used internally +// for features implementation. +class OurFeatures { +public: + static OurFeatures all(); + bool allowComments_; + bool allowTrailingCommas_; + bool strictRoot_; + bool allowDroppedNullPlaceholders_; + bool allowNumericKeys_; + bool allowSingleQuotes_; + bool failIfExtra_; + bool rejectDupKeys_; + bool allowSpecialFloats_; + bool skipBom_; + size_t stackLimit_; +}; // OurFeatures + +OurFeatures OurFeatures::all() { return {}; } + +// Implementation of class Reader +// //////////////////////////////// + +// Originally copied from the Reader class (now deprecated), used internally +// for implementing JSON reading. +class OurReader { +public: + using Char = char; + using Location = const Char*; + struct StructuredError { + ptrdiff_t offset_start; + ptrdiff_t offset_limit; + String message; + }; + + explicit OurReader(OurFeatures const& features); + bool parse(const char* beginDoc, const char* endDoc, Value& root, + bool collectComments = true); + String getFormattedErrorMessages() const; + std::vector getStructuredErrors() const; + +private: + OurReader(OurReader const&); // no impl + void operator=(OurReader const&); // no impl + + enum TokenType { + tokenEndOfStream = 0, + tokenObjectBegin, + tokenObjectEnd, + tokenArrayBegin, + tokenArrayEnd, + tokenString, + tokenNumber, + tokenTrue, + tokenFalse, + tokenNull, + tokenNaN, + tokenPosInf, + tokenNegInf, + tokenArraySeparator, + tokenMemberSeparator, + tokenComment, + tokenError + }; + + class Token { + public: + TokenType type_; + Location start_; + Location end_; + }; + + class ErrorInfo { + public: + Token token_; + String message_; + Location extra_; + }; + + using Errors = std::deque; + + bool readToken(Token& token); + void skipSpaces(); + void skipBom(bool skipBom); + bool match(const Char* pattern, int patternLength); + bool readComment(); + bool readCStyleComment(bool* containsNewLineResult); + bool readCppStyleComment(); + bool readString(); + bool readStringSingleQuote(); + bool readNumber(bool checkInf); + bool readValue(); + bool readObject(Token& token); + bool readArray(Token& token); + bool decodeNumber(Token& token); + bool decodeNumber(Token& token, Value& decoded); + bool decodeString(Token& token); + bool decodeString(Token& token, String& decoded); + bool decodeDouble(Token& token); + bool decodeDouble(Token& token, Value& decoded); + bool decodeUnicodeCodePoint(Token& token, Location& current, Location end, + unsigned int& unicode); + bool decodeUnicodeEscapeSequence(Token& token, Location& current, + Location end, unsigned int& unicode); + bool addError(const String& message, Token& token, Location extra = nullptr); + bool recoverFromError(TokenType skipUntilToken); + bool addErrorAndRecover(const String& message, Token& token, + TokenType skipUntilToken); + void skipUntilSpace(); + Value& currentValue(); + Char getNextChar(); + void getLocationLineAndColumn(Location location, int& line, + int& column) const; + String getLocationLineAndColumn(Location location) const; + void addComment(Location begin, Location end, CommentPlacement placement); + void skipCommentTokens(Token& token); + + static String normalizeEOL(Location begin, Location end); + static bool containsNewLine(Location begin, Location end); + + using Nodes = std::stack; + + Nodes nodes_{}; + Errors errors_{}; + String document_{}; + Location begin_ = nullptr; + Location end_ = nullptr; + Location current_ = nullptr; + Location lastValueEnd_ = nullptr; + Value* lastValue_ = nullptr; + bool lastValueHasAComment_ = false; + String commentsBefore_{}; + + OurFeatures const features_; + bool collectComments_ = false; +}; // OurReader + +// complete copy of Read impl, for OurReader + +bool OurReader::containsNewLine(OurReader::Location begin, + OurReader::Location end) { + return std::any_of(begin, end, [](char b) { return b == '\n' || b == '\r'; }); +} + +OurReader::OurReader(OurFeatures const& features) : features_(features) {} + +bool OurReader::parse(const char* beginDoc, const char* endDoc, Value& root, + bool collectComments) { + if (!features_.allowComments_) { + collectComments = false; + } + + begin_ = beginDoc; + end_ = endDoc; + collectComments_ = collectComments; + current_ = begin_; + lastValueEnd_ = nullptr; + lastValue_ = nullptr; + commentsBefore_.clear(); + errors_.clear(); + while (!nodes_.empty()) + nodes_.pop(); + nodes_.push(&root); + + // skip byte order mark if it exists at the beginning of the UTF-8 text. + skipBom(features_.skipBom_); + bool successful = readValue(); + nodes_.pop(); + Token token; + skipCommentTokens(token); + if (features_.failIfExtra_ && (token.type_ != tokenEndOfStream)) { + addError("Extra non-whitespace after JSON value.", token); + return false; + } + if (collectComments_ && !commentsBefore_.empty()) + root.setComment(commentsBefore_, commentAfter); + if (features_.strictRoot_) { + if (!root.isArray() && !root.isObject()) { + // Set error location to start of doc, ideally should be first token found + // in doc + token.type_ = tokenError; + token.start_ = beginDoc; + token.end_ = endDoc; + addError( + "A valid JSON document must be either an array or an object value.", + token); + return false; + } + } + return successful; +} + +bool OurReader::readValue() { + // To preserve the old behaviour we cast size_t to int. + if (nodes_.size() > features_.stackLimit_) + throwRuntimeError("Exceeded stackLimit in readValue()."); + Token token; + skipCommentTokens(token); + bool successful = true; + + if (collectComments_ && !commentsBefore_.empty()) { + currentValue().setComment(commentsBefore_, commentBefore); + commentsBefore_.clear(); + } + + switch (token.type_) { + case tokenObjectBegin: + successful = readObject(token); + currentValue().setOffsetLimit(current_ - begin_); + break; + case tokenArrayBegin: + successful = readArray(token); + currentValue().setOffsetLimit(current_ - begin_); + break; + case tokenNumber: + successful = decodeNumber(token); + break; + case tokenString: + successful = decodeString(token); + break; + case tokenTrue: { + Value v(true); + currentValue().swapPayload(v); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + } break; + case tokenFalse: { + Value v(false); + currentValue().swapPayload(v); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + } break; + case tokenNull: { + Value v; + currentValue().swapPayload(v); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + } break; + case tokenNaN: { + Value v(std::numeric_limits::quiet_NaN()); + currentValue().swapPayload(v); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + } break; + case tokenPosInf: { + Value v(std::numeric_limits::infinity()); + currentValue().swapPayload(v); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + } break; + case tokenNegInf: { + Value v(-std::numeric_limits::infinity()); + currentValue().swapPayload(v); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + } break; + case tokenArraySeparator: + case tokenObjectEnd: + case tokenArrayEnd: + if (features_.allowDroppedNullPlaceholders_) { + // "Un-read" the current token and mark the current value as a null + // token. + current_--; + Value v; + currentValue().swapPayload(v); + currentValue().setOffsetStart(current_ - begin_ - 1); + currentValue().setOffsetLimit(current_ - begin_); + break; + } // else, fall through ... + default: + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + return addError("Syntax error: value, object or array expected.", token); + } + + if (collectComments_) { + lastValueEnd_ = current_; + lastValueHasAComment_ = false; + lastValue_ = ¤tValue(); + } + + return successful; +} + +void OurReader::skipCommentTokens(Token& token) { + if (features_.allowComments_) { + do { + readToken(token); + } while (token.type_ == tokenComment); + } else { + readToken(token); + } +} + +bool OurReader::readToken(Token& token) { + skipSpaces(); + token.start_ = current_; + Char c = getNextChar(); + bool ok = true; + switch (c) { + case '{': + token.type_ = tokenObjectBegin; + break; + case '}': + token.type_ = tokenObjectEnd; + break; + case '[': + token.type_ = tokenArrayBegin; + break; + case ']': + token.type_ = tokenArrayEnd; + break; + case '"': + token.type_ = tokenString; + ok = readString(); + break; + case '\'': + if (features_.allowSingleQuotes_) { + token.type_ = tokenString; + ok = readStringSingleQuote(); + } else { + // If we don't allow single quotes, this is a failure case. + ok = false; + } + break; + case '/': + token.type_ = tokenComment; + ok = readComment(); + break; + case '0': + case '1': + case '2': + case '3': + case '4': + case '5': + case '6': + case '7': + case '8': + case '9': + token.type_ = tokenNumber; + readNumber(false); + break; + case '-': + if (readNumber(true)) { + token.type_ = tokenNumber; + } else { + token.type_ = tokenNegInf; + ok = features_.allowSpecialFloats_ && match("nfinity", 7); + } + break; + case '+': + if (readNumber(true)) { + token.type_ = tokenNumber; + } else { + token.type_ = tokenPosInf; + ok = features_.allowSpecialFloats_ && match("nfinity", 7); + } + break; + case 't': + token.type_ = tokenTrue; + ok = match("rue", 3); + break; + case 'f': + token.type_ = tokenFalse; + ok = match("alse", 4); + break; + case 'n': + token.type_ = tokenNull; + ok = match("ull", 3); + break; + case 'N': + if (features_.allowSpecialFloats_) { + token.type_ = tokenNaN; + ok = match("aN", 2); + } else { + ok = false; + } + break; + case 'I': + if (features_.allowSpecialFloats_) { + token.type_ = tokenPosInf; + ok = match("nfinity", 7); + } else { + ok = false; + } + break; + case ',': + token.type_ = tokenArraySeparator; + break; + case ':': + token.type_ = tokenMemberSeparator; + break; + case 0: + token.type_ = tokenEndOfStream; + break; + default: + ok = false; + break; + } + if (!ok) + token.type_ = tokenError; + token.end_ = current_; + return ok; +} + +void OurReader::skipSpaces() { + while (current_ != end_) { + Char c = *current_; + if (c == ' ' || c == '\t' || c == '\r' || c == '\n') + ++current_; + else + break; + } +} + +void OurReader::skipBom(bool skipBom) { + // The default behavior is to skip BOM. + if (skipBom) { + if ((end_ - begin_) >= 3 && strncmp(begin_, "\xEF\xBB\xBF", 3) == 0) { + begin_ += 3; + current_ = begin_; + } + } +} + +bool OurReader::match(const Char* pattern, int patternLength) { + if (end_ - current_ < patternLength) + return false; + int index = patternLength; + while (index--) + if (current_[index] != pattern[index]) + return false; + current_ += patternLength; + return true; +} + +bool OurReader::readComment() { + const Location commentBegin = current_ - 1; + const Char c = getNextChar(); + bool successful = false; + bool cStyleWithEmbeddedNewline = false; + + const bool isCStyleComment = (c == '*'); + const bool isCppStyleComment = (c == '/'); + if (isCStyleComment) { + successful = readCStyleComment(&cStyleWithEmbeddedNewline); + } else if (isCppStyleComment) { + successful = readCppStyleComment(); + } + + if (!successful) + return false; + + if (collectComments_) { + CommentPlacement placement = commentBefore; + + if (!lastValueHasAComment_) { + if (lastValueEnd_ && !containsNewLine(lastValueEnd_, commentBegin)) { + if (isCppStyleComment || !cStyleWithEmbeddedNewline) { + placement = commentAfterOnSameLine; + lastValueHasAComment_ = true; + } + } + } + + addComment(commentBegin, current_, placement); + } + return true; +} + +String OurReader::normalizeEOL(OurReader::Location begin, + OurReader::Location end) { + String normalized; + normalized.reserve(static_cast(end - begin)); + OurReader::Location current = begin; + while (current != end) { + char c = *current++; + if (c == '\r') { + if (current != end && *current == '\n') + // convert dos EOL + ++current; + // convert Mac EOL + normalized += '\n'; + } else { + normalized += c; + } + } + return normalized; +} + +void OurReader::addComment(Location begin, Location end, + CommentPlacement placement) { + assert(collectComments_); + const String& normalized = normalizeEOL(begin, end); + if (placement == commentAfterOnSameLine) { + assert(lastValue_ != nullptr); + lastValue_->setComment(normalized, placement); + } else { + commentsBefore_ += normalized; + } +} + +bool OurReader::readCStyleComment(bool* containsNewLineResult) { + *containsNewLineResult = false; + + while ((current_ + 1) < end_) { + Char c = getNextChar(); + if (c == '*' && *current_ == '/') + break; + if (c == '\n') + *containsNewLineResult = true; + } + + return getNextChar() == '/'; +} + +bool OurReader::readCppStyleComment() { + while (current_ != end_) { + Char c = getNextChar(); + if (c == '\n') + break; + if (c == '\r') { + // Consume DOS EOL. It will be normalized in addComment. + if (current_ != end_ && *current_ == '\n') + getNextChar(); + // Break on Moc OS 9 EOL. + break; + } + } + return true; +} + +bool OurReader::readNumber(bool checkInf) { + Location p = current_; + if (checkInf && p != end_ && *p == 'I') { + current_ = ++p; + return false; + } + char c = '0'; // stopgap for already consumed character + // integral part + while (c >= '0' && c <= '9') + c = (current_ = p) < end_ ? *p++ : '\0'; + // fractional part + if (c == '.') { + c = (current_ = p) < end_ ? *p++ : '\0'; + while (c >= '0' && c <= '9') + c = (current_ = p) < end_ ? *p++ : '\0'; + } + // exponential part + if (c == 'e' || c == 'E') { + c = (current_ = p) < end_ ? *p++ : '\0'; + if (c == '+' || c == '-') + c = (current_ = p) < end_ ? *p++ : '\0'; + while (c >= '0' && c <= '9') + c = (current_ = p) < end_ ? *p++ : '\0'; + } + return true; +} +bool OurReader::readString() { + Char c = 0; + while (current_ != end_) { + c = getNextChar(); + if (c == '\\') + getNextChar(); + else if (c == '"') + break; + } + return c == '"'; +} + +bool OurReader::readStringSingleQuote() { + Char c = 0; + while (current_ != end_) { + c = getNextChar(); + if (c == '\\') + getNextChar(); + else if (c == '\'') + break; + } + return c == '\''; +} + +bool OurReader::readObject(Token& token) { + Token tokenName; + String name; + Value init(objectValue); + currentValue().swapPayload(init); + currentValue().setOffsetStart(token.start_ - begin_); + while (readToken(tokenName)) { + bool initialTokenOk = true; + while (tokenName.type_ == tokenComment && initialTokenOk) + initialTokenOk = readToken(tokenName); + if (!initialTokenOk) + break; + if (tokenName.type_ == tokenObjectEnd && + (name.empty() || + features_.allowTrailingCommas_)) // empty object or trailing comma + return true; + name.clear(); + if (tokenName.type_ == tokenString) { + if (!decodeString(tokenName, name)) + return recoverFromError(tokenObjectEnd); + } else if (tokenName.type_ == tokenNumber && features_.allowNumericKeys_) { + Value numberName; + if (!decodeNumber(tokenName, numberName)) + return recoverFromError(tokenObjectEnd); + name = numberName.asString(); + } else { + break; + } + if (name.length() >= (1U << 30)) + throwRuntimeError("keylength >= 2^30"); + if (features_.rejectDupKeys_ && currentValue().isMember(name)) { + String msg = "Duplicate key: '" + name + "'"; + return addErrorAndRecover(msg, tokenName, tokenObjectEnd); + } + + Token colon; + if (!readToken(colon) || colon.type_ != tokenMemberSeparator) { + return addErrorAndRecover("Missing ':' after object member name", colon, + tokenObjectEnd); + } + Value& value = currentValue()[name]; + nodes_.push(&value); + bool ok = readValue(); + nodes_.pop(); + if (!ok) // error already set + return recoverFromError(tokenObjectEnd); + + Token comma; + if (!readToken(comma) || + (comma.type_ != tokenObjectEnd && comma.type_ != tokenArraySeparator && + comma.type_ != tokenComment)) { + return addErrorAndRecover("Missing ',' or '}' in object declaration", + comma, tokenObjectEnd); + } + bool finalizeTokenOk = true; + while (comma.type_ == tokenComment && finalizeTokenOk) + finalizeTokenOk = readToken(comma); + if (comma.type_ == tokenObjectEnd) + return true; + } + return addErrorAndRecover("Missing '}' or object member name", tokenName, + tokenObjectEnd); +} + +bool OurReader::readArray(Token& token) { + Value init(arrayValue); + currentValue().swapPayload(init); + currentValue().setOffsetStart(token.start_ - begin_); + int index = 0; + for (;;) { + skipSpaces(); + if (current_ != end_ && *current_ == ']' && + (index == 0 || + (features_.allowTrailingCommas_ && + !features_.allowDroppedNullPlaceholders_))) // empty array or trailing + // comma + { + Token endArray; + readToken(endArray); + return true; + } + Value& value = currentValue()[index++]; + nodes_.push(&value); + bool ok = readValue(); + nodes_.pop(); + if (!ok) // error already set + return recoverFromError(tokenArrayEnd); + + Token currentToken; + // Accept Comment after last item in the array. + ok = readToken(currentToken); + while (currentToken.type_ == tokenComment && ok) { + ok = readToken(currentToken); + } + bool badTokenType = (currentToken.type_ != tokenArraySeparator && + currentToken.type_ != tokenArrayEnd); + if (!ok || badTokenType) { + return addErrorAndRecover("Missing ',' or ']' in array declaration", + currentToken, tokenArrayEnd); + } + if (currentToken.type_ == tokenArrayEnd) + break; + } + return true; +} + +bool OurReader::decodeNumber(Token& token) { + Value decoded; + if (!decodeNumber(token, decoded)) + return false; + currentValue().swapPayload(decoded); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + return true; +} + +bool OurReader::decodeNumber(Token& token, Value& decoded) { + // Attempts to parse the number as an integer. If the number is + // larger than the maximum supported value of an integer then + // we decode the number as a double. + Location current = token.start_; + const bool isNegative = *current == '-'; + if (isNegative) { + ++current; + } + + // We assume we can represent the largest and smallest integer types as + // unsigned integers with separate sign. This is only true if they can fit + // into an unsigned integer. + static_assert(Value::maxLargestInt <= Value::maxLargestUInt, + "Int must be smaller than UInt"); + + // We need to convert minLargestInt into a positive number. The easiest way + // to do this conversion is to assume our "threshold" value of minLargestInt + // divided by 10 can fit in maxLargestInt when absolute valued. This should + // be a safe assumption. + static_assert(Value::minLargestInt <= -Value::maxLargestInt, + "The absolute value of minLargestInt must be greater than or " + "equal to maxLargestInt"); + static_assert(Value::minLargestInt / 10 >= -Value::maxLargestInt, + "The absolute value of minLargestInt must be only 1 magnitude " + "larger than maxLargest Int"); + + static constexpr Value::LargestUInt positive_threshold = + Value::maxLargestUInt / 10; + static constexpr Value::UInt positive_last_digit = Value::maxLargestUInt % 10; + + // For the negative values, we have to be more careful. Since typically + // -Value::minLargestInt will cause an overflow, we first divide by 10 and + // then take the inverse. This assumes that minLargestInt is only a single + // power of 10 different in magnitude, which we check above. For the last + // digit, we take the modulus before negating for the same reason. + static constexpr auto negative_threshold = + Value::LargestUInt(-(Value::minLargestInt / 10)); + static constexpr auto negative_last_digit = + Value::UInt(-(Value::minLargestInt % 10)); + + const Value::LargestUInt threshold = + isNegative ? negative_threshold : positive_threshold; + const Value::UInt max_last_digit = + isNegative ? negative_last_digit : positive_last_digit; + + Value::LargestUInt value = 0; + while (current < token.end_) { + Char c = *current++; + if (c < '0' || c > '9') + return decodeDouble(token, decoded); + + const auto digit(static_cast(c - '0')); + if (value >= threshold) { + // We've hit or exceeded the max value divided by 10 (rounded down). If + // a) we've only just touched the limit, meaning value == threshold, + // b) this is the last digit, or + // c) it's small enough to fit in that rounding delta, we're okay. + // Otherwise treat this number as a double to avoid overflow. + if (value > threshold || current != token.end_ || + digit > max_last_digit) { + return decodeDouble(token, decoded); + } + } + value = value * 10 + digit; + } + + if (isNegative) { + // We use the same magnitude assumption here, just in case. + const auto last_digit = static_cast(value % 10); + decoded = -Value::LargestInt(value / 10) * 10 - last_digit; + } else if (value <= Value::LargestUInt(Value::maxLargestInt)) { + decoded = Value::LargestInt(value); + } else { + decoded = value; + } + + return true; +} + +bool OurReader::decodeDouble(Token& token) { + Value decoded; + if (!decodeDouble(token, decoded)) + return false; + currentValue().swapPayload(decoded); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + return true; +} + +bool OurReader::decodeDouble(Token& token, Value& decoded) { + double value = 0; + const String buffer(token.start_, token.end_); + IStringStream is(buffer); + if (!(is >> value)) { + if (value == std::numeric_limits::max()) + value = std::numeric_limits::infinity(); + else if (value == std::numeric_limits::lowest()) + value = -std::numeric_limits::infinity(); + else if (!std::isinf(value)) + return addError( + "'" + String(token.start_, token.end_) + "' is not a number.", token); + } + decoded = value; + return true; +} + +bool OurReader::decodeString(Token& token) { + String decoded_string; + if (!decodeString(token, decoded_string)) + return false; + Value decoded(decoded_string); + currentValue().swapPayload(decoded); + currentValue().setOffsetStart(token.start_ - begin_); + currentValue().setOffsetLimit(token.end_ - begin_); + return true; +} + +bool OurReader::decodeString(Token& token, String& decoded) { + decoded.reserve(static_cast(token.end_ - token.start_ - 2)); + Location current = token.start_ + 1; // skip '"' + Location end = token.end_ - 1; // do not include '"' + while (current != end) { + Char c = *current++; + if (c == '"') + break; + if (c == '\\') { + if (current == end) + return addError("Empty escape sequence in string", token, current); + Char escape = *current++; + switch (escape) { + case '"': + decoded += '"'; + break; + case '/': + decoded += '/'; + break; + case '\\': + decoded += '\\'; + break; + case 'b': + decoded += '\b'; + break; + case 'f': + decoded += '\f'; + break; + case 'n': + decoded += '\n'; + break; + case 'r': + decoded += '\r'; + break; + case 't': + decoded += '\t'; + break; + case 'u': { + unsigned int unicode; + if (!decodeUnicodeCodePoint(token, current, end, unicode)) + return false; + decoded += codePointToUTF8(unicode); + } break; + default: + return addError("Bad escape sequence in string", token, current); + } + } else { + decoded += c; + } + } + return true; +} + +bool OurReader::decodeUnicodeCodePoint(Token& token, Location& current, + Location end, unsigned int& unicode) { + + if (!decodeUnicodeEscapeSequence(token, current, end, unicode)) + return false; + if (unicode >= 0xD800 && unicode <= 0xDBFF) { + // surrogate pairs + if (end - current < 6) + return addError( + "additional six characters expected to parse unicode surrogate pair.", + token, current); + if (*(current++) == '\\' && *(current++) == 'u') { + unsigned int surrogatePair; + if (decodeUnicodeEscapeSequence(token, current, end, surrogatePair)) { + unicode = 0x10000 + ((unicode & 0x3FF) << 10) + (surrogatePair & 0x3FF); + } else + return false; + } else + return addError("expecting another \\u token to begin the second half of " + "a unicode surrogate pair", + token, current); + } + return true; +} + +bool OurReader::decodeUnicodeEscapeSequence(Token& token, Location& current, + Location end, + unsigned int& ret_unicode) { + if (end - current < 4) + return addError( + "Bad unicode escape sequence in string: four digits expected.", token, + current); + int unicode = 0; + for (int index = 0; index < 4; ++index) { + Char c = *current++; + unicode *= 16; + if (c >= '0' && c <= '9') + unicode += c - '0'; + else if (c >= 'a' && c <= 'f') + unicode += c - 'a' + 10; + else if (c >= 'A' && c <= 'F') + unicode += c - 'A' + 10; + else + return addError( + "Bad unicode escape sequence in string: hexadecimal digit expected.", + token, current); + } + ret_unicode = static_cast(unicode); + return true; +} + +bool OurReader::addError(const String& message, Token& token, Location extra) { + ErrorInfo info; + info.token_ = token; + info.message_ = message; + info.extra_ = extra; + errors_.push_back(info); + return false; +} + +bool OurReader::recoverFromError(TokenType skipUntilToken) { + size_t errorCount = errors_.size(); + Token skip; + for (;;) { + if (!readToken(skip)) + errors_.resize(errorCount); // discard errors caused by recovery + if (skip.type_ == skipUntilToken || skip.type_ == tokenEndOfStream) + break; + } + errors_.resize(errorCount); + return false; +} + +bool OurReader::addErrorAndRecover(const String& message, Token& token, + TokenType skipUntilToken) { + addError(message, token); + return recoverFromError(skipUntilToken); +} + +Value& OurReader::currentValue() { return *(nodes_.top()); } + +OurReader::Char OurReader::getNextChar() { + if (current_ == end_) + return 0; + return *current_++; +} + +void OurReader::getLocationLineAndColumn(Location location, int& line, + int& column) const { + Location current = begin_; + Location lastLineStart = current; + line = 0; + while (current < location && current != end_) { + Char c = *current++; + if (c == '\r') { + if (*current == '\n') + ++current; + lastLineStart = current; + ++line; + } else if (c == '\n') { + lastLineStart = current; + ++line; + } + } + // column & line start at 1 + column = int(location - lastLineStart) + 1; + ++line; +} + +String OurReader::getLocationLineAndColumn(Location location) const { + int line, column; + getLocationLineAndColumn(location, line, column); + char buffer[18 + 16 + 16 + 1]; + jsoncpp_snprintf(buffer, sizeof(buffer), "Line %d, Column %d", line, column); + return buffer; +} + +String OurReader::getFormattedErrorMessages() const { + String formattedMessage; + for (const auto& error : errors_) { + formattedMessage += + "* " + getLocationLineAndColumn(error.token_.start_) + "\n"; + formattedMessage += " " + error.message_ + "\n"; + if (error.extra_) + formattedMessage += + "See " + getLocationLineAndColumn(error.extra_) + " for detail.\n"; + } + return formattedMessage; +} + +std::vector OurReader::getStructuredErrors() const { + std::vector allErrors; + for (const auto& error : errors_) { + OurReader::StructuredError structured; + structured.offset_start = error.token_.start_ - begin_; + structured.offset_limit = error.token_.end_ - begin_; + structured.message = error.message_; + allErrors.push_back(structured); + } + return allErrors; +} + +class OurCharReader : public CharReader { + bool const collectComments_; + OurReader reader_; + +public: + OurCharReader(bool collectComments, OurFeatures const& features) + : collectComments_(collectComments), reader_(features) {} + bool parse(char const* beginDoc, char const* endDoc, Value* root, + String* errs) override { + bool ok = reader_.parse(beginDoc, endDoc, *root, collectComments_); + if (errs) { + *errs = reader_.getFormattedErrorMessages(); + } + return ok; + } +}; + +CharReaderBuilder::CharReaderBuilder() { setDefaults(&settings_); } +CharReaderBuilder::~CharReaderBuilder() = default; +CharReader* CharReaderBuilder::newCharReader() const { + bool collectComments = settings_["collectComments"].asBool(); + OurFeatures features = OurFeatures::all(); + features.allowComments_ = settings_["allowComments"].asBool(); + features.allowTrailingCommas_ = settings_["allowTrailingCommas"].asBool(); + features.strictRoot_ = settings_["strictRoot"].asBool(); + features.allowDroppedNullPlaceholders_ = + settings_["allowDroppedNullPlaceholders"].asBool(); + features.allowNumericKeys_ = settings_["allowNumericKeys"].asBool(); + features.allowSingleQuotes_ = settings_["allowSingleQuotes"].asBool(); + + // Stack limit is always a size_t, so we get this as an unsigned int + // regardless of it we have 64-bit integer support enabled. + features.stackLimit_ = static_cast(settings_["stackLimit"].asUInt()); + features.failIfExtra_ = settings_["failIfExtra"].asBool(); + features.rejectDupKeys_ = settings_["rejectDupKeys"].asBool(); + features.allowSpecialFloats_ = settings_["allowSpecialFloats"].asBool(); + features.skipBom_ = settings_["skipBom"].asBool(); + return new OurCharReader(collectComments, features); +} + +bool CharReaderBuilder::validate(Json::Value* invalid) const { + static const auto& valid_keys = *new std::set{ + "collectComments", + "allowComments", + "allowTrailingCommas", + "strictRoot", + "allowDroppedNullPlaceholders", + "allowNumericKeys", + "allowSingleQuotes", + "stackLimit", + "failIfExtra", + "rejectDupKeys", + "allowSpecialFloats", + "skipBom", + }; + for (auto si = settings_.begin(); si != settings_.end(); ++si) { + auto key = si.name(); + if (valid_keys.count(key)) + continue; + if (invalid) + (*invalid)[key] = *si; + else + return false; + } + return invalid ? invalid->empty() : true; +} + +Value& CharReaderBuilder::operator[](const String& key) { + return settings_[key]; +} +// static +void CharReaderBuilder::strictMode(Json::Value* settings) { + //! [CharReaderBuilderStrictMode] + (*settings)["allowComments"] = false; + (*settings)["allowTrailingCommas"] = false; + (*settings)["strictRoot"] = true; + (*settings)["allowDroppedNullPlaceholders"] = false; + (*settings)["allowNumericKeys"] = false; + (*settings)["allowSingleQuotes"] = false; + (*settings)["stackLimit"] = 1000; + (*settings)["failIfExtra"] = true; + (*settings)["rejectDupKeys"] = true; + (*settings)["allowSpecialFloats"] = false; + (*settings)["skipBom"] = true; + //! [CharReaderBuilderStrictMode] +} +// static +void CharReaderBuilder::setDefaults(Json::Value* settings) { + //! [CharReaderBuilderDefaults] + (*settings)["collectComments"] = true; + (*settings)["allowComments"] = true; + (*settings)["allowTrailingCommas"] = true; + (*settings)["strictRoot"] = false; + (*settings)["allowDroppedNullPlaceholders"] = false; + (*settings)["allowNumericKeys"] = false; + (*settings)["allowSingleQuotes"] = false; + (*settings)["stackLimit"] = 1000; + (*settings)["failIfExtra"] = false; + (*settings)["rejectDupKeys"] = false; + (*settings)["allowSpecialFloats"] = false; + (*settings)["skipBom"] = true; + //! [CharReaderBuilderDefaults] +} + +////////////////////////////////// +// global functions + +bool parseFromStream(CharReader::Factory const& fact, IStream& sin, Value* root, + String* errs) { + OStringStream ssin; + ssin << sin.rdbuf(); + String doc = ssin.str(); + char const* begin = doc.data(); + char const* end = begin + doc.size(); + // Note that we do not actually need a null-terminator. + CharReaderPtr const reader(fact.newCharReader()); + return reader->parse(begin, end, root, errs); +} + +IStream& operator>>(IStream& sin, Value& root) { + CharReaderBuilder b; + String errs; + bool ok = parseFromStream(b, sin, &root, &errs); + if (!ok) { + throwRuntimeError(errs); + } + return sin; +} + +} // namespace Json + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: src/lib_json/json_reader.cpp +// ////////////////////////////////////////////////////////////////////// + + + + + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: src/lib_json/json_valueiterator.inl +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2007-2010 Baptiste Lepilleur and The JsonCpp Authors +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +// included by json_value.cpp + +namespace Json { + +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// class ValueIteratorBase +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// + +ValueIteratorBase::ValueIteratorBase() : current_() {} + +ValueIteratorBase::ValueIteratorBase( + const Value::ObjectValues::iterator& current) + : current_(current), isNull_(false) {} + +Value& ValueIteratorBase::deref() { return current_->second; } +const Value& ValueIteratorBase::deref() const { return current_->second; } + +void ValueIteratorBase::increment() { ++current_; } + +void ValueIteratorBase::decrement() { --current_; } + +ValueIteratorBase::difference_type +ValueIteratorBase::computeDistance(const SelfType& other) const { + // Iterator for null value are initialized using the default + // constructor, which initialize current_ to the default + // std::map::iterator. As begin() and end() are two instance + // of the default std::map::iterator, they can not be compared. + // To allow this, we handle this comparison specifically. + if (isNull_ && other.isNull_) { + return 0; + } + + // Usage of std::distance is not portable (does not compile with Sun Studio 12 + // RogueWave STL, + // which is the one used by default). + // Using a portable hand-made version for non random iterator instead: + // return difference_type( std::distance( current_, other.current_ ) ); + difference_type myDistance = 0; + for (Value::ObjectValues::iterator it = current_; it != other.current_; + ++it) { + ++myDistance; + } + return myDistance; +} + +bool ValueIteratorBase::isEqual(const SelfType& other) const { + if (isNull_) { + return other.isNull_; + } + return current_ == other.current_; +} + +void ValueIteratorBase::copy(const SelfType& other) { + current_ = other.current_; + isNull_ = other.isNull_; +} + +Value ValueIteratorBase::key() const { + const Value::CZString czstring = (*current_).first; + if (czstring.data()) { + if (czstring.isStaticString()) + return Value(StaticString(czstring.data())); + return Value(czstring.data(), czstring.data() + czstring.length()); + } + return Value(czstring.index()); +} + +UInt ValueIteratorBase::index() const { + const Value::CZString czstring = (*current_).first; + if (!czstring.data()) + return czstring.index(); + return Value::UInt(-1); +} + +String ValueIteratorBase::name() const { + char const* keey; + char const* end; + keey = memberName(&end); + if (!keey) + return String(); + return String(keey, end); +} + +char const* ValueIteratorBase::memberName() const { + const char* cname = (*current_).first.data(); + return cname ? cname : ""; +} + +char const* ValueIteratorBase::memberName(char const** end) const { + const char* cname = (*current_).first.data(); + if (!cname) { + *end = nullptr; + return nullptr; + } + *end = cname + (*current_).first.length(); + return cname; +} + +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// class ValueConstIterator +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// + +ValueConstIterator::ValueConstIterator() = default; + +ValueConstIterator::ValueConstIterator( + const Value::ObjectValues::iterator& current) + : ValueIteratorBase(current) {} + +ValueConstIterator::ValueConstIterator(ValueIterator const& other) + : ValueIteratorBase(other) {} + +ValueConstIterator& ValueConstIterator:: +operator=(const ValueIteratorBase& other) { + copy(other); + return *this; +} + +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// class ValueIterator +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// + +ValueIterator::ValueIterator() = default; + +ValueIterator::ValueIterator(const Value::ObjectValues::iterator& current) + : ValueIteratorBase(current) {} + +ValueIterator::ValueIterator(const ValueConstIterator& other) + : ValueIteratorBase(other) { + throwRuntimeError("ConstIterator to Iterator should never be allowed."); +} + +ValueIterator::ValueIterator(const ValueIterator& other) = default; + +ValueIterator& ValueIterator::operator=(const SelfType& other) { + copy(other); + return *this; +} + +} // namespace Json + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: src/lib_json/json_valueiterator.inl +// ////////////////////////////////////////////////////////////////////// + + + + + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: src/lib_json/json_value.cpp +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2011 Baptiste Lepilleur and The JsonCpp Authors +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +#if !defined(JSON_IS_AMALGAMATION) +#include +#include +#include +#endif // if !defined(JSON_IS_AMALGAMATION) +#include +#include +#include +#include +#include +#include +#include +#include + +// Provide implementation equivalent of std::snprintf for older _MSC compilers +#if defined(_MSC_VER) && _MSC_VER < 1900 +#include +static int msvc_pre1900_c99_vsnprintf(char* outBuf, size_t size, + const char* format, va_list ap) { + int count = -1; + if (size != 0) + count = _vsnprintf_s(outBuf, size, _TRUNCATE, format, ap); + if (count == -1) + count = _vscprintf(format, ap); + return count; +} + +int JSON_API msvc_pre1900_c99_snprintf(char* outBuf, size_t size, + const char* format, ...) { + va_list ap; + va_start(ap, format); + const int count = msvc_pre1900_c99_vsnprintf(outBuf, size, format, ap); + va_end(ap); + return count; +} +#endif + +// Disable warning C4702 : unreachable code +#if defined(_MSC_VER) +#pragma warning(disable : 4702) +#endif + +#define JSON_ASSERT_UNREACHABLE assert(false) + +namespace Json { +template +static std::unique_ptr cloneUnique(const std::unique_ptr& p) { + std::unique_ptr r; + if (p) { + r = std::unique_ptr(new T(*p)); + } + return r; +} + +// This is a walkaround to avoid the static initialization of Value::null. +// kNull must be word-aligned to avoid crashing on ARM. We use an alignment of +// 8 (instead of 4) as a bit of future-proofing. +#if defined(__ARMEL__) +#define ALIGNAS(byte_alignment) __attribute__((aligned(byte_alignment))) +#else +#define ALIGNAS(byte_alignment) +#endif + +// static +Value const& Value::nullSingleton() { + static Value const nullStatic; + return nullStatic; +} + +#if JSON_USE_NULLREF +// for backwards compatibility, we'll leave these global references around, but +// DO NOT use them in JSONCPP library code any more! +// static +Value const& Value::null = Value::nullSingleton(); + +// static +Value const& Value::nullRef = Value::nullSingleton(); +#endif + +#if !defined(JSON_USE_INT64_DOUBLE_CONVERSION) +template +static inline bool InRange(double d, T min, U max) { + // The casts can lose precision, but we are looking only for + // an approximate range. Might fail on edge cases though. ~cdunn + return d >= static_cast(min) && d <= static_cast(max); +} +#else // if !defined(JSON_USE_INT64_DOUBLE_CONVERSION) +static inline double integerToDouble(Json::UInt64 value) { + return static_cast(Int64(value / 2)) * 2.0 + + static_cast(Int64(value & 1)); +} + +template static inline double integerToDouble(T value) { + return static_cast(value); +} + +template +static inline bool InRange(double d, T min, U max) { + return d >= integerToDouble(min) && d <= integerToDouble(max); +} +#endif // if !defined(JSON_USE_INT64_DOUBLE_CONVERSION) + +/** Duplicates the specified string value. + * @param value Pointer to the string to duplicate. Must be zero-terminated if + * length is "unknown". + * @param length Length of the value. if equals to unknown, then it will be + * computed using strlen(value). + * @return Pointer on the duplicate instance of string. + */ +static inline char* duplicateStringValue(const char* value, size_t length) { + // Avoid an integer overflow in the call to malloc below by limiting length + // to a sane value. + if (length >= static_cast(Value::maxInt)) + length = Value::maxInt - 1; + + auto newString = static_cast(malloc(length + 1)); + if (newString == nullptr) { + throwRuntimeError("in Json::Value::duplicateStringValue(): " + "Failed to allocate string value buffer"); + } + memcpy(newString, value, length); + newString[length] = 0; + return newString; +} + +/* Record the length as a prefix. + */ +static inline char* duplicateAndPrefixStringValue(const char* value, + unsigned int length) { + // Avoid an integer overflow in the call to malloc below by limiting length + // to a sane value. + JSON_ASSERT_MESSAGE(length <= static_cast(Value::maxInt) - + sizeof(unsigned) - 1U, + "in Json::Value::duplicateAndPrefixStringValue(): " + "length too big for prefixing"); + size_t actualLength = sizeof(length) + length + 1; + auto newString = static_cast(malloc(actualLength)); + if (newString == nullptr) { + throwRuntimeError("in Json::Value::duplicateAndPrefixStringValue(): " + "Failed to allocate string value buffer"); + } + *reinterpret_cast(newString) = length; + memcpy(newString + sizeof(unsigned), value, length); + newString[actualLength - 1U] = + 0; // to avoid buffer over-run accidents by users later + return newString; +} +inline static void decodePrefixedString(bool isPrefixed, char const* prefixed, + unsigned* length, char const** value) { + if (!isPrefixed) { + *length = static_cast(strlen(prefixed)); + *value = prefixed; + } else { + *length = *reinterpret_cast(prefixed); + *value = prefixed + sizeof(unsigned); + } +} +/** Free the string duplicated by + * duplicateStringValue()/duplicateAndPrefixStringValue(). + */ +#if JSONCPP_USING_SECURE_MEMORY +static inline void releasePrefixedStringValue(char* value) { + unsigned length = 0; + char const* valueDecoded; + decodePrefixedString(true, value, &length, &valueDecoded); + size_t const size = sizeof(unsigned) + length + 1U; + memset(value, 0, size); + free(value); +} +static inline void releaseStringValue(char* value, unsigned length) { + // length==0 => we allocated the strings memory + size_t size = (length == 0) ? strlen(value) : length; + memset(value, 0, size); + free(value); +} +#else // !JSONCPP_USING_SECURE_MEMORY +static inline void releasePrefixedStringValue(char* value) { free(value); } +static inline void releaseStringValue(char* value, unsigned) { free(value); } +#endif // JSONCPP_USING_SECURE_MEMORY + +} // namespace Json + +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ValueInternals... +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +#if !defined(JSON_IS_AMALGAMATION) + +#include "json_valueiterator.inl" +#endif // if !defined(JSON_IS_AMALGAMATION) + +namespace Json { + +#if JSON_USE_EXCEPTION +Exception::Exception(String msg) : msg_(std::move(msg)) {} +Exception::~Exception() noexcept = default; +char const* Exception::what() const noexcept { return msg_.c_str(); } +RuntimeError::RuntimeError(String const& msg) : Exception(msg) {} +LogicError::LogicError(String const& msg) : Exception(msg) {} +JSONCPP_NORETURN void throwRuntimeError(String const& msg) { + throw RuntimeError(msg); +} +JSONCPP_NORETURN void throwLogicError(String const& msg) { + throw LogicError(msg); +} +#else // !JSON_USE_EXCEPTION +JSONCPP_NORETURN void throwRuntimeError(String const& msg) { + std::cerr << msg << std::endl; + abort(); +} +JSONCPP_NORETURN void throwLogicError(String const& msg) { + std::cerr << msg << std::endl; + abort(); +} +#endif + +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// class Value::CZString +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// + +// Notes: policy_ indicates if the string was allocated when +// a string is stored. + +Value::CZString::CZString(ArrayIndex index) : cstr_(nullptr), index_(index) {} + +Value::CZString::CZString(char const* str, unsigned length, + DuplicationPolicy allocate) + : cstr_(str) { + // allocate != duplicate + storage_.policy_ = allocate & 0x3; + storage_.length_ = length & 0x3FFFFFFF; +} + +Value::CZString::CZString(const CZString& other) { + cstr_ = (other.storage_.policy_ != noDuplication && other.cstr_ != nullptr + ? duplicateStringValue(other.cstr_, other.storage_.length_) + : other.cstr_); + storage_.policy_ = + static_cast( + other.cstr_ + ? (static_cast(other.storage_.policy_) == + noDuplication + ? noDuplication + : duplicate) + : static_cast(other.storage_.policy_)) & + 3U; + storage_.length_ = other.storage_.length_; +} + +Value::CZString::CZString(CZString&& other) noexcept + : cstr_(other.cstr_), index_(other.index_) { + other.cstr_ = nullptr; +} + +Value::CZString::~CZString() { + if (cstr_ && storage_.policy_ == duplicate) { + releaseStringValue(const_cast(cstr_), + storage_.length_ + 1U); // +1 for null terminating + // character for sake of + // completeness but not actually + // necessary + } +} + +void Value::CZString::swap(CZString& other) { + std::swap(cstr_, other.cstr_); + std::swap(index_, other.index_); +} + +Value::CZString& Value::CZString::operator=(const CZString& other) { + cstr_ = other.cstr_; + index_ = other.index_; + return *this; +} + +Value::CZString& Value::CZString::operator=(CZString&& other) noexcept { + cstr_ = other.cstr_; + index_ = other.index_; + other.cstr_ = nullptr; + return *this; +} + +bool Value::CZString::operator<(const CZString& other) const { + if (!cstr_) + return index_ < other.index_; + // return strcmp(cstr_, other.cstr_) < 0; + // Assume both are strings. + unsigned this_len = this->storage_.length_; + unsigned other_len = other.storage_.length_; + unsigned min_len = std::min(this_len, other_len); + JSON_ASSERT(this->cstr_ && other.cstr_); + int comp = memcmp(this->cstr_, other.cstr_, min_len); + if (comp < 0) + return true; + if (comp > 0) + return false; + return (this_len < other_len); +} + +bool Value::CZString::operator==(const CZString& other) const { + if (!cstr_) + return index_ == other.index_; + // return strcmp(cstr_, other.cstr_) == 0; + // Assume both are strings. + unsigned this_len = this->storage_.length_; + unsigned other_len = other.storage_.length_; + if (this_len != other_len) + return false; + JSON_ASSERT(this->cstr_ && other.cstr_); + int comp = memcmp(this->cstr_, other.cstr_, this_len); + return comp == 0; +} + +ArrayIndex Value::CZString::index() const { return index_; } + +// const char* Value::CZString::c_str() const { return cstr_; } +const char* Value::CZString::data() const { return cstr_; } +unsigned Value::CZString::length() const { return storage_.length_; } +bool Value::CZString::isStaticString() const { + return storage_.policy_ == noDuplication; +} + +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// class Value::Value +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// +// ////////////////////////////////////////////////////////////////// + +/*! \internal Default constructor initialization must be equivalent to: + * memset( this, 0, sizeof(Value) ) + * This optimization is used in ValueInternalMap fast allocator. + */ +Value::Value(ValueType type) { + static char const emptyString[] = ""; + initBasic(type); + switch (type) { + case nullValue: + break; + case intValue: + case uintValue: + value_.int_ = 0; + break; + case realValue: + value_.real_ = 0.0; + break; + case stringValue: + // allocated_ == false, so this is safe. + value_.string_ = const_cast(static_cast(emptyString)); + break; + case arrayValue: + case objectValue: + value_.map_ = new ObjectValues(); + break; + case booleanValue: + value_.bool_ = false; + break; + default: + JSON_ASSERT_UNREACHABLE; + } +} + +Value::Value(Int value) { + initBasic(intValue); + value_.int_ = value; +} + +Value::Value(UInt value) { + initBasic(uintValue); + value_.uint_ = value; +} +#if defined(JSON_HAS_INT64) +Value::Value(Int64 value) { + initBasic(intValue); + value_.int_ = value; +} +Value::Value(UInt64 value) { + initBasic(uintValue); + value_.uint_ = value; +} +#endif // defined(JSON_HAS_INT64) + +Value::Value(double value) { + initBasic(realValue); + value_.real_ = value; +} + +Value::Value(const char* value) { + initBasic(stringValue, true); + JSON_ASSERT_MESSAGE(value != nullptr, + "Null Value Passed to Value Constructor"); + value_.string_ = duplicateAndPrefixStringValue( + value, static_cast(strlen(value))); +} + +Value::Value(const char* begin, const char* end) { + initBasic(stringValue, true); + value_.string_ = + duplicateAndPrefixStringValue(begin, static_cast(end - begin)); +} + +Value::Value(const String& value) { + initBasic(stringValue, true); + value_.string_ = duplicateAndPrefixStringValue( + value.data(), static_cast(value.length())); +} + +Value::Value(const StaticString& value) { + initBasic(stringValue); + value_.string_ = const_cast(value.c_str()); +} + +Value::Value(bool value) { + initBasic(booleanValue); + value_.bool_ = value; +} + +Value::Value(const Value& other) { + dupPayload(other); + dupMeta(other); +} + +Value::Value(Value&& other) noexcept { + initBasic(nullValue); + swap(other); +} + +Value::~Value() { + releasePayload(); + value_.uint_ = 0; +} + +Value& Value::operator=(const Value& other) { + Value(other).swap(*this); + return *this; +} + +Value& Value::operator=(Value&& other) noexcept { + other.swap(*this); + return *this; +} + +void Value::swapPayload(Value& other) { + std::swap(bits_, other.bits_); + std::swap(value_, other.value_); +} + +void Value::copyPayload(const Value& other) { + releasePayload(); + dupPayload(other); +} + +void Value::swap(Value& other) { + swapPayload(other); + std::swap(comments_, other.comments_); + std::swap(start_, other.start_); + std::swap(limit_, other.limit_); +} + +void Value::copy(const Value& other) { + copyPayload(other); + dupMeta(other); +} + +ValueType Value::type() const { + return static_cast(bits_.value_type_); +} + +int Value::compare(const Value& other) const { + if (*this < other) + return -1; + if (*this > other) + return 1; + return 0; +} + +bool Value::operator<(const Value& other) const { + int typeDelta = type() - other.type(); + if (typeDelta) + return typeDelta < 0; + switch (type()) { + case nullValue: + return false; + case intValue: + return value_.int_ < other.value_.int_; + case uintValue: + return value_.uint_ < other.value_.uint_; + case realValue: + return value_.real_ < other.value_.real_; + case booleanValue: + return value_.bool_ < other.value_.bool_; + case stringValue: { + if ((value_.string_ == nullptr) || (other.value_.string_ == nullptr)) { + return other.value_.string_ != nullptr; + } + unsigned this_len; + unsigned other_len; + char const* this_str; + char const* other_str; + decodePrefixedString(this->isAllocated(), this->value_.string_, &this_len, + &this_str); + decodePrefixedString(other.isAllocated(), other.value_.string_, &other_len, + &other_str); + unsigned min_len = std::min(this_len, other_len); + JSON_ASSERT(this_str && other_str); + int comp = memcmp(this_str, other_str, min_len); + if (comp < 0) + return true; + if (comp > 0) + return false; + return (this_len < other_len); + } + case arrayValue: + case objectValue: { + auto thisSize = value_.map_->size(); + auto otherSize = other.value_.map_->size(); + if (thisSize != otherSize) + return thisSize < otherSize; + return (*value_.map_) < (*other.value_.map_); + } + default: + JSON_ASSERT_UNREACHABLE; + } + return false; // unreachable +} + +bool Value::operator<=(const Value& other) const { return !(other < *this); } + +bool Value::operator>=(const Value& other) const { return !(*this < other); } + +bool Value::operator>(const Value& other) const { return other < *this; } + +bool Value::operator==(const Value& other) const { + if (type() != other.type()) + return false; + switch (type()) { + case nullValue: + return true; + case intValue: + return value_.int_ == other.value_.int_; + case uintValue: + return value_.uint_ == other.value_.uint_; + case realValue: + return value_.real_ == other.value_.real_; + case booleanValue: + return value_.bool_ == other.value_.bool_; + case stringValue: { + if ((value_.string_ == nullptr) || (other.value_.string_ == nullptr)) { + return (value_.string_ == other.value_.string_); + } + unsigned this_len; + unsigned other_len; + char const* this_str; + char const* other_str; + decodePrefixedString(this->isAllocated(), this->value_.string_, &this_len, + &this_str); + decodePrefixedString(other.isAllocated(), other.value_.string_, &other_len, + &other_str); + if (this_len != other_len) + return false; + JSON_ASSERT(this_str && other_str); + int comp = memcmp(this_str, other_str, this_len); + return comp == 0; + } + case arrayValue: + case objectValue: + return value_.map_->size() == other.value_.map_->size() && + (*value_.map_) == (*other.value_.map_); + default: + JSON_ASSERT_UNREACHABLE; + } + return false; // unreachable +} + +bool Value::operator!=(const Value& other) const { return !(*this == other); } + +const char* Value::asCString() const { + JSON_ASSERT_MESSAGE(type() == stringValue, + "in Json::Value::asCString(): requires stringValue"); + if (value_.string_ == nullptr) + return nullptr; + unsigned this_len; + char const* this_str; + decodePrefixedString(this->isAllocated(), this->value_.string_, &this_len, + &this_str); + return this_str; +} + +#if JSONCPP_USING_SECURE_MEMORY +unsigned Value::getCStringLength() const { + JSON_ASSERT_MESSAGE(type() == stringValue, + "in Json::Value::asCString(): requires stringValue"); + if (value_.string_ == 0) + return 0; + unsigned this_len; + char const* this_str; + decodePrefixedString(this->isAllocated(), this->value_.string_, &this_len, + &this_str); + return this_len; +} +#endif + +bool Value::getString(char const** begin, char const** end) const { + if (type() != stringValue) + return false; + if (value_.string_ == nullptr) + return false; + unsigned length; + decodePrefixedString(this->isAllocated(), this->value_.string_, &length, + begin); + *end = *begin + length; + return true; +} + +String Value::asString() const { + switch (type()) { + case nullValue: + return ""; + case stringValue: { + if (value_.string_ == nullptr) + return ""; + unsigned this_len; + char const* this_str; + decodePrefixedString(this->isAllocated(), this->value_.string_, &this_len, + &this_str); + return String(this_str, this_len); + } + case booleanValue: + return value_.bool_ ? "true" : "false"; + case intValue: + return valueToString(value_.int_); + case uintValue: + return valueToString(value_.uint_); + case realValue: + return valueToString(value_.real_); + default: + JSON_FAIL_MESSAGE("Type is not convertible to string"); + } +} + +Value::Int Value::asInt() const { + switch (type()) { + case intValue: + JSON_ASSERT_MESSAGE(isInt(), "LargestInt out of Int range"); + return Int(value_.int_); + case uintValue: + JSON_ASSERT_MESSAGE(isInt(), "LargestUInt out of Int range"); + return Int(value_.uint_); + case realValue: + JSON_ASSERT_MESSAGE(InRange(value_.real_, minInt, maxInt), + "double out of Int range"); + return Int(value_.real_); + case nullValue: + return 0; + case booleanValue: + return value_.bool_ ? 1 : 0; + default: + break; + } + JSON_FAIL_MESSAGE("Value is not convertible to Int."); +} + +Value::UInt Value::asUInt() const { + switch (type()) { + case intValue: + JSON_ASSERT_MESSAGE(isUInt(), "LargestInt out of UInt range"); + return UInt(value_.int_); + case uintValue: + JSON_ASSERT_MESSAGE(isUInt(), "LargestUInt out of UInt range"); + return UInt(value_.uint_); + case realValue: + JSON_ASSERT_MESSAGE(InRange(value_.real_, 0, maxUInt), + "double out of UInt range"); + return UInt(value_.real_); + case nullValue: + return 0; + case booleanValue: + return value_.bool_ ? 1 : 0; + default: + break; + } + JSON_FAIL_MESSAGE("Value is not convertible to UInt."); +} + +#if defined(JSON_HAS_INT64) + +Value::Int64 Value::asInt64() const { + switch (type()) { + case intValue: + return Int64(value_.int_); + case uintValue: + JSON_ASSERT_MESSAGE(isInt64(), "LargestUInt out of Int64 range"); + return Int64(value_.uint_); + case realValue: + JSON_ASSERT_MESSAGE(InRange(value_.real_, minInt64, maxInt64), + "double out of Int64 range"); + return Int64(value_.real_); + case nullValue: + return 0; + case booleanValue: + return value_.bool_ ? 1 : 0; + default: + break; + } + JSON_FAIL_MESSAGE("Value is not convertible to Int64."); +} + +Value::UInt64 Value::asUInt64() const { + switch (type()) { + case intValue: + JSON_ASSERT_MESSAGE(isUInt64(), "LargestInt out of UInt64 range"); + return UInt64(value_.int_); + case uintValue: + return UInt64(value_.uint_); + case realValue: + JSON_ASSERT_MESSAGE(InRange(value_.real_, 0, maxUInt64), + "double out of UInt64 range"); + return UInt64(value_.real_); + case nullValue: + return 0; + case booleanValue: + return value_.bool_ ? 1 : 0; + default: + break; + } + JSON_FAIL_MESSAGE("Value is not convertible to UInt64."); +} +#endif // if defined(JSON_HAS_INT64) + +LargestInt Value::asLargestInt() const { +#if defined(JSON_NO_INT64) + return asInt(); +#else + return asInt64(); +#endif +} + +LargestUInt Value::asLargestUInt() const { +#if defined(JSON_NO_INT64) + return asUInt(); +#else + return asUInt64(); +#endif +} + +double Value::asDouble() const { + switch (type()) { + case intValue: + return static_cast(value_.int_); + case uintValue: +#if !defined(JSON_USE_INT64_DOUBLE_CONVERSION) + return static_cast(value_.uint_); +#else // if !defined(JSON_USE_INT64_DOUBLE_CONVERSION) + return integerToDouble(value_.uint_); +#endif // if !defined(JSON_USE_INT64_DOUBLE_CONVERSION) + case realValue: + return value_.real_; + case nullValue: + return 0.0; + case booleanValue: + return value_.bool_ ? 1.0 : 0.0; + default: + break; + } + JSON_FAIL_MESSAGE("Value is not convertible to double."); +} + +float Value::asFloat() const { + switch (type()) { + case intValue: + return static_cast(value_.int_); + case uintValue: +#if !defined(JSON_USE_INT64_DOUBLE_CONVERSION) + return static_cast(value_.uint_); +#else // if !defined(JSON_USE_INT64_DOUBLE_CONVERSION) + // This can fail (silently?) if the value is bigger than MAX_FLOAT. + return static_cast(integerToDouble(value_.uint_)); +#endif // if !defined(JSON_USE_INT64_DOUBLE_CONVERSION) + case realValue: + return static_cast(value_.real_); + case nullValue: + return 0.0; + case booleanValue: + return value_.bool_ ? 1.0F : 0.0F; + default: + break; + } + JSON_FAIL_MESSAGE("Value is not convertible to float."); +} + +bool Value::asBool() const { + switch (type()) { + case booleanValue: + return value_.bool_; + case nullValue: + return false; + case intValue: + return value_.int_ != 0; + case uintValue: + return value_.uint_ != 0; + case realValue: { + // According to JavaScript language zero or NaN is regarded as false + const auto value_classification = std::fpclassify(value_.real_); + return value_classification != FP_ZERO && value_classification != FP_NAN; + } + default: + break; + } + JSON_FAIL_MESSAGE("Value is not convertible to bool."); +} + +bool Value::isConvertibleTo(ValueType other) const { + switch (other) { + case nullValue: + return (isNumeric() && asDouble() == 0.0) || + (type() == booleanValue && !value_.bool_) || + (type() == stringValue && asString().empty()) || + (type() == arrayValue && value_.map_->empty()) || + (type() == objectValue && value_.map_->empty()) || + type() == nullValue; + case intValue: + return isInt() || + (type() == realValue && InRange(value_.real_, minInt, maxInt)) || + type() == booleanValue || type() == nullValue; + case uintValue: + return isUInt() || + (type() == realValue && InRange(value_.real_, 0, maxUInt)) || + type() == booleanValue || type() == nullValue; + case realValue: + return isNumeric() || type() == booleanValue || type() == nullValue; + case booleanValue: + return isNumeric() || type() == booleanValue || type() == nullValue; + case stringValue: + return isNumeric() || type() == booleanValue || type() == stringValue || + type() == nullValue; + case arrayValue: + return type() == arrayValue || type() == nullValue; + case objectValue: + return type() == objectValue || type() == nullValue; + } + JSON_ASSERT_UNREACHABLE; + return false; +} + +/// Number of values in array or object +ArrayIndex Value::size() const { + switch (type()) { + case nullValue: + case intValue: + case uintValue: + case realValue: + case booleanValue: + case stringValue: + return 0; + case arrayValue: // size of the array is highest index + 1 + if (!value_.map_->empty()) { + ObjectValues::const_iterator itLast = value_.map_->end(); + --itLast; + return (*itLast).first.index() + 1; + } + return 0; + case objectValue: + return ArrayIndex(value_.map_->size()); + } + JSON_ASSERT_UNREACHABLE; + return 0; // unreachable; +} + +bool Value::empty() const { + if (isNull() || isArray() || isObject()) + return size() == 0U; + return false; +} + +Value::operator bool() const { return !isNull(); } + +void Value::clear() { + JSON_ASSERT_MESSAGE(type() == nullValue || type() == arrayValue || + type() == objectValue, + "in Json::Value::clear(): requires complex value"); + start_ = 0; + limit_ = 0; + switch (type()) { + case arrayValue: + case objectValue: + value_.map_->clear(); + break; + default: + break; + } +} + +void Value::resize(ArrayIndex newSize) { + JSON_ASSERT_MESSAGE(type() == nullValue || type() == arrayValue, + "in Json::Value::resize(): requires arrayValue"); + if (type() == nullValue) + *this = Value(arrayValue); + ArrayIndex oldSize = size(); + if (newSize == 0) + clear(); + else if (newSize > oldSize) + for (ArrayIndex i = oldSize; i < newSize; ++i) + (*this)[i]; + else { + for (ArrayIndex index = newSize; index < oldSize; ++index) { + value_.map_->erase(index); + } + JSON_ASSERT(size() == newSize); + } +} + +Value& Value::operator[](ArrayIndex index) { + JSON_ASSERT_MESSAGE( + type() == nullValue || type() == arrayValue, + "in Json::Value::operator[](ArrayIndex): requires arrayValue"); + if (type() == nullValue) + *this = Value(arrayValue); + CZString key(index); + auto it = value_.map_->lower_bound(key); + if (it != value_.map_->end() && (*it).first == key) + return (*it).second; + + ObjectValues::value_type defaultValue(key, nullSingleton()); + it = value_.map_->insert(it, defaultValue); + return (*it).second; +} + +Value& Value::operator[](int index) { + JSON_ASSERT_MESSAGE( + index >= 0, + "in Json::Value::operator[](int index): index cannot be negative"); + return (*this)[ArrayIndex(index)]; +} + +const Value& Value::operator[](ArrayIndex index) const { + JSON_ASSERT_MESSAGE( + type() == nullValue || type() == arrayValue, + "in Json::Value::operator[](ArrayIndex)const: requires arrayValue"); + if (type() == nullValue) + return nullSingleton(); + CZString key(index); + ObjectValues::const_iterator it = value_.map_->find(key); + if (it == value_.map_->end()) + return nullSingleton(); + return (*it).second; +} + +const Value& Value::operator[](int index) const { + JSON_ASSERT_MESSAGE( + index >= 0, + "in Json::Value::operator[](int index) const: index cannot be negative"); + return (*this)[ArrayIndex(index)]; +} + +void Value::initBasic(ValueType type, bool allocated) { + setType(type); + setIsAllocated(allocated); + comments_ = Comments{}; + start_ = 0; + limit_ = 0; +} + +void Value::dupPayload(const Value& other) { + setType(other.type()); + setIsAllocated(false); + switch (type()) { + case nullValue: + case intValue: + case uintValue: + case realValue: + case booleanValue: + value_ = other.value_; + break; + case stringValue: + if (other.value_.string_ && other.isAllocated()) { + unsigned len; + char const* str; + decodePrefixedString(other.isAllocated(), other.value_.string_, &len, + &str); + value_.string_ = duplicateAndPrefixStringValue(str, len); + setIsAllocated(true); + } else { + value_.string_ = other.value_.string_; + } + break; + case arrayValue: + case objectValue: + value_.map_ = new ObjectValues(*other.value_.map_); + break; + default: + JSON_ASSERT_UNREACHABLE; + } +} + +void Value::releasePayload() { + switch (type()) { + case nullValue: + case intValue: + case uintValue: + case realValue: + case booleanValue: + break; + case stringValue: + if (isAllocated()) + releasePrefixedStringValue(value_.string_); + break; + case arrayValue: + case objectValue: + delete value_.map_; + break; + default: + JSON_ASSERT_UNREACHABLE; + } +} + +void Value::dupMeta(const Value& other) { + comments_ = other.comments_; + start_ = other.start_; + limit_ = other.limit_; +} + +// Access an object value by name, create a null member if it does not exist. +// @pre Type of '*this' is object or null. +// @param key is null-terminated. +Value& Value::resolveReference(const char* key) { + JSON_ASSERT_MESSAGE( + type() == nullValue || type() == objectValue, + "in Json::Value::resolveReference(): requires objectValue"); + if (type() == nullValue) + *this = Value(objectValue); + CZString actualKey(key, static_cast(strlen(key)), + CZString::noDuplication); // NOTE! + auto it = value_.map_->lower_bound(actualKey); + if (it != value_.map_->end() && (*it).first == actualKey) + return (*it).second; + + ObjectValues::value_type defaultValue(actualKey, nullSingleton()); + it = value_.map_->insert(it, defaultValue); + Value& value = (*it).second; + return value; +} + +// @param key is not null-terminated. +Value& Value::resolveReference(char const* key, char const* end) { + JSON_ASSERT_MESSAGE( + type() == nullValue || type() == objectValue, + "in Json::Value::resolveReference(key, end): requires objectValue"); + if (type() == nullValue) + *this = Value(objectValue); + CZString actualKey(key, static_cast(end - key), + CZString::duplicateOnCopy); + auto it = value_.map_->lower_bound(actualKey); + if (it != value_.map_->end() && (*it).first == actualKey) + return (*it).second; + + ObjectValues::value_type defaultValue(actualKey, nullSingleton()); + it = value_.map_->insert(it, defaultValue); + Value& value = (*it).second; + return value; +} + +Value Value::get(ArrayIndex index, const Value& defaultValue) const { + const Value* value = &((*this)[index]); + return value == &nullSingleton() ? defaultValue : *value; +} + +bool Value::isValidIndex(ArrayIndex index) const { return index < size(); } + +Value const* Value::find(char const* begin, char const* end) const { + JSON_ASSERT_MESSAGE(type() == nullValue || type() == objectValue, + "in Json::Value::find(begin, end): requires " + "objectValue or nullValue"); + if (type() == nullValue) + return nullptr; + CZString actualKey(begin, static_cast(end - begin), + CZString::noDuplication); + ObjectValues::const_iterator it = value_.map_->find(actualKey); + if (it == value_.map_->end()) + return nullptr; + return &(*it).second; +} +Value* Value::demand(char const* begin, char const* end) { + JSON_ASSERT_MESSAGE(type() == nullValue || type() == objectValue, + "in Json::Value::demand(begin, end): requires " + "objectValue or nullValue"); + return &resolveReference(begin, end); +} +const Value& Value::operator[](const char* key) const { + Value const* found = find(key, key + strlen(key)); + if (!found) + return nullSingleton(); + return *found; +} +Value const& Value::operator[](const String& key) const { + Value const* found = find(key.data(), key.data() + key.length()); + if (!found) + return nullSingleton(); + return *found; +} + +Value& Value::operator[](const char* key) { + return resolveReference(key, key + strlen(key)); +} + +Value& Value::operator[](const String& key) { + return resolveReference(key.data(), key.data() + key.length()); +} + +Value& Value::operator[](const StaticString& key) { + return resolveReference(key.c_str()); +} + +Value& Value::append(const Value& value) { return append(Value(value)); } + +Value& Value::append(Value&& value) { + JSON_ASSERT_MESSAGE(type() == nullValue || type() == arrayValue, + "in Json::Value::append: requires arrayValue"); + if (type() == nullValue) { + *this = Value(arrayValue); + } + return this->value_.map_->emplace(size(), std::move(value)).first->second; +} + +bool Value::insert(ArrayIndex index, const Value& newValue) { + return insert(index, Value(newValue)); +} + +bool Value::insert(ArrayIndex index, Value&& newValue) { + JSON_ASSERT_MESSAGE(type() == nullValue || type() == arrayValue, + "in Json::Value::insert: requires arrayValue"); + ArrayIndex length = size(); + if (index > length) { + return false; + } + for (ArrayIndex i = length; i > index; i--) { + (*this)[i] = std::move((*this)[i - 1]); + } + (*this)[index] = std::move(newValue); + return true; +} + +Value Value::get(char const* begin, char const* end, + Value const& defaultValue) const { + Value const* found = find(begin, end); + return !found ? defaultValue : *found; +} +Value Value::get(char const* key, Value const& defaultValue) const { + return get(key, key + strlen(key), defaultValue); +} +Value Value::get(String const& key, Value const& defaultValue) const { + return get(key.data(), key.data() + key.length(), defaultValue); +} + +bool Value::removeMember(const char* begin, const char* end, Value* removed) { + if (type() != objectValue) { + return false; + } + CZString actualKey(begin, static_cast(end - begin), + CZString::noDuplication); + auto it = value_.map_->find(actualKey); + if (it == value_.map_->end()) + return false; + if (removed) + *removed = std::move(it->second); + value_.map_->erase(it); + return true; +} +bool Value::removeMember(const char* key, Value* removed) { + return removeMember(key, key + strlen(key), removed); +} +bool Value::removeMember(String const& key, Value* removed) { + return removeMember(key.data(), key.data() + key.length(), removed); +} +void Value::removeMember(const char* key) { + JSON_ASSERT_MESSAGE(type() == nullValue || type() == objectValue, + "in Json::Value::removeMember(): requires objectValue"); + if (type() == nullValue) + return; + + CZString actualKey(key, unsigned(strlen(key)), CZString::noDuplication); + value_.map_->erase(actualKey); +} +void Value::removeMember(const String& key) { removeMember(key.c_str()); } + +bool Value::removeIndex(ArrayIndex index, Value* removed) { + if (type() != arrayValue) { + return false; + } + CZString key(index); + auto it = value_.map_->find(key); + if (it == value_.map_->end()) { + return false; + } + if (removed) + *removed = it->second; + ArrayIndex oldSize = size(); + // shift left all items left, into the place of the "removed" + for (ArrayIndex i = index; i < (oldSize - 1); ++i) { + CZString keey(i); + (*value_.map_)[keey] = (*this)[i + 1]; + } + // erase the last one ("leftover") + CZString keyLast(oldSize - 1); + auto itLast = value_.map_->find(keyLast); + value_.map_->erase(itLast); + return true; +} + +bool Value::isMember(char const* begin, char const* end) const { + Value const* value = find(begin, end); + return nullptr != value; +} +bool Value::isMember(char const* key) const { + return isMember(key, key + strlen(key)); +} +bool Value::isMember(String const& key) const { + return isMember(key.data(), key.data() + key.length()); +} + +Value::Members Value::getMemberNames() const { + JSON_ASSERT_MESSAGE( + type() == nullValue || type() == objectValue, + "in Json::Value::getMemberNames(), value must be objectValue"); + if (type() == nullValue) + return Value::Members(); + Members members; + members.reserve(value_.map_->size()); + ObjectValues::const_iterator it = value_.map_->begin(); + ObjectValues::const_iterator itEnd = value_.map_->end(); + for (; it != itEnd; ++it) { + members.push_back(String((*it).first.data(), (*it).first.length())); + } + return members; +} + +static bool IsIntegral(double d) { + double integral_part; + return modf(d, &integral_part) == 0.0; +} + +bool Value::isNull() const { return type() == nullValue; } + +bool Value::isBool() const { return type() == booleanValue; } + +bool Value::isInt() const { + switch (type()) { + case intValue: +#if defined(JSON_HAS_INT64) + return value_.int_ >= minInt && value_.int_ <= maxInt; +#else + return true; +#endif + case uintValue: + return value_.uint_ <= UInt(maxInt); + case realValue: + return value_.real_ >= minInt && value_.real_ <= maxInt && + IsIntegral(value_.real_); + default: + break; + } + return false; +} + +bool Value::isUInt() const { + switch (type()) { + case intValue: +#if defined(JSON_HAS_INT64) + return value_.int_ >= 0 && LargestUInt(value_.int_) <= LargestUInt(maxUInt); +#else + return value_.int_ >= 0; +#endif + case uintValue: +#if defined(JSON_HAS_INT64) + return value_.uint_ <= maxUInt; +#else + return true; +#endif + case realValue: + return value_.real_ >= 0 && value_.real_ <= maxUInt && + IsIntegral(value_.real_); + default: + break; + } + return false; +} + +bool Value::isInt64() const { +#if defined(JSON_HAS_INT64) + switch (type()) { + case intValue: + return true; + case uintValue: + return value_.uint_ <= UInt64(maxInt64); + case realValue: + // Note that maxInt64 (= 2^63 - 1) is not exactly representable as a + // double, so double(maxInt64) will be rounded up to 2^63. Therefore we + // require the value to be strictly less than the limit. + return value_.real_ >= double(minInt64) && + value_.real_ < double(maxInt64) && IsIntegral(value_.real_); + default: + break; + } +#endif // JSON_HAS_INT64 + return false; +} + +bool Value::isUInt64() const { +#if defined(JSON_HAS_INT64) + switch (type()) { + case intValue: + return value_.int_ >= 0; + case uintValue: + return true; + case realValue: + // Note that maxUInt64 (= 2^64 - 1) is not exactly representable as a + // double, so double(maxUInt64) will be rounded up to 2^64. Therefore we + // require the value to be strictly less than the limit. + return value_.real_ >= 0 && value_.real_ < maxUInt64AsDouble && + IsIntegral(value_.real_); + default: + break; + } +#endif // JSON_HAS_INT64 + return false; +} + +bool Value::isIntegral() const { + switch (type()) { + case intValue: + case uintValue: + return true; + case realValue: +#if defined(JSON_HAS_INT64) + // Note that maxUInt64 (= 2^64 - 1) is not exactly representable as a + // double, so double(maxUInt64) will be rounded up to 2^64. Therefore we + // require the value to be strictly less than the limit. + return value_.real_ >= double(minInt64) && + value_.real_ < maxUInt64AsDouble && IsIntegral(value_.real_); +#else + return value_.real_ >= minInt && value_.real_ <= maxUInt && + IsIntegral(value_.real_); +#endif // JSON_HAS_INT64 + default: + break; + } + return false; +} + +bool Value::isDouble() const { + return type() == intValue || type() == uintValue || type() == realValue; +} + +bool Value::isNumeric() const { return isDouble(); } + +bool Value::isString() const { return type() == stringValue; } + +bool Value::isArray() const { return type() == arrayValue; } + +bool Value::isObject() const { return type() == objectValue; } + +Value::Comments::Comments(const Comments& that) + : ptr_{cloneUnique(that.ptr_)} {} + +Value::Comments::Comments(Comments&& that) noexcept + : ptr_{std::move(that.ptr_)} {} + +Value::Comments& Value::Comments::operator=(const Comments& that) { + ptr_ = cloneUnique(that.ptr_); + return *this; +} + +Value::Comments& Value::Comments::operator=(Comments&& that) noexcept { + ptr_ = std::move(that.ptr_); + return *this; +} + +bool Value::Comments::has(CommentPlacement slot) const { + return ptr_ && !(*ptr_)[slot].empty(); +} + +String Value::Comments::get(CommentPlacement slot) const { + if (!ptr_) + return {}; + return (*ptr_)[slot]; +} + +void Value::Comments::set(CommentPlacement slot, String comment) { + if (slot >= CommentPlacement::numberOfCommentPlacement) + return; + if (!ptr_) + ptr_ = std::unique_ptr(new Array()); + (*ptr_)[slot] = std::move(comment); +} + +void Value::setComment(String comment, CommentPlacement placement) { + if (!comment.empty() && (comment.back() == '\n')) { + // Always discard trailing newline, to aid indentation. + comment.pop_back(); + } + JSON_ASSERT(!comment.empty()); + JSON_ASSERT_MESSAGE( + comment[0] == '\0' || comment[0] == '/', + "in Json::Value::setComment(): Comments must start with /"); + comments_.set(placement, std::move(comment)); +} + +bool Value::hasComment(CommentPlacement placement) const { + return comments_.has(placement); +} + +String Value::getComment(CommentPlacement placement) const { + return comments_.get(placement); +} + +void Value::setOffsetStart(ptrdiff_t start) { start_ = start; } + +void Value::setOffsetLimit(ptrdiff_t limit) { limit_ = limit; } + +ptrdiff_t Value::getOffsetStart() const { return start_; } + +ptrdiff_t Value::getOffsetLimit() const { return limit_; } + +String Value::toStyledString() const { + StreamWriterBuilder builder; + + String out = this->hasComment(commentBefore) ? "\n" : ""; + out += Json::writeString(builder, *this); + out += '\n'; + + return out; +} + +Value::const_iterator Value::begin() const { + switch (type()) { + case arrayValue: + case objectValue: + if (value_.map_) + return const_iterator(value_.map_->begin()); + break; + default: + break; + } + return {}; +} + +Value::const_iterator Value::end() const { + switch (type()) { + case arrayValue: + case objectValue: + if (value_.map_) + return const_iterator(value_.map_->end()); + break; + default: + break; + } + return {}; +} + +Value::iterator Value::begin() { + switch (type()) { + case arrayValue: + case objectValue: + if (value_.map_) + return iterator(value_.map_->begin()); + break; + default: + break; + } + return iterator(); +} + +Value::iterator Value::end() { + switch (type()) { + case arrayValue: + case objectValue: + if (value_.map_) + return iterator(value_.map_->end()); + break; + default: + break; + } + return iterator(); +} + +// class PathArgument +// ////////////////////////////////////////////////////////////////// + +PathArgument::PathArgument() = default; + +PathArgument::PathArgument(ArrayIndex index) + : index_(index), kind_(kindIndex) {} + +PathArgument::PathArgument(const char* key) : key_(key), kind_(kindKey) {} + +PathArgument::PathArgument(String key) : key_(std::move(key)), kind_(kindKey) {} + +// class Path +// ////////////////////////////////////////////////////////////////// + +Path::Path(const String& path, const PathArgument& a1, const PathArgument& a2, + const PathArgument& a3, const PathArgument& a4, + const PathArgument& a5) { + InArgs in; + in.reserve(5); + in.push_back(&a1); + in.push_back(&a2); + in.push_back(&a3); + in.push_back(&a4); + in.push_back(&a5); + makePath(path, in); +} + +void Path::makePath(const String& path, const InArgs& in) { + const char* current = path.c_str(); + const char* end = current + path.length(); + auto itInArg = in.begin(); + while (current != end) { + if (*current == '[') { + ++current; + if (*current == '%') + addPathInArg(path, in, itInArg, PathArgument::kindIndex); + else { + ArrayIndex index = 0; + for (; current != end && *current >= '0' && *current <= '9'; ++current) + index = index * 10 + ArrayIndex(*current - '0'); + args_.push_back(index); + } + if (current == end || *++current != ']') + invalidPath(path, int(current - path.c_str())); + } else if (*current == '%') { + addPathInArg(path, in, itInArg, PathArgument::kindKey); + ++current; + } else if (*current == '.' || *current == ']') { + ++current; + } else { + const char* beginName = current; + while (current != end && !strchr("[.", *current)) + ++current; + args_.push_back(String(beginName, current)); + } + } +} + +void Path::addPathInArg(const String& /*path*/, const InArgs& in, + InArgs::const_iterator& itInArg, + PathArgument::Kind kind) { + if (itInArg == in.end()) { + // Error: missing argument %d + } else if ((*itInArg)->kind_ != kind) { + // Error: bad argument type + } else { + args_.push_back(**itInArg++); + } +} + +void Path::invalidPath(const String& /*path*/, int /*location*/) { + // Error: invalid path. +} + +const Value& Path::resolve(const Value& root) const { + const Value* node = &root; + for (const auto& arg : args_) { + if (arg.kind_ == PathArgument::kindIndex) { + if (!node->isArray() || !node->isValidIndex(arg.index_)) { + // Error: unable to resolve path (array value expected at position... ) + return Value::nullSingleton(); + } + node = &((*node)[arg.index_]); + } else if (arg.kind_ == PathArgument::kindKey) { + if (!node->isObject()) { + // Error: unable to resolve path (object value expected at position...) + return Value::nullSingleton(); + } + node = &((*node)[arg.key_]); + if (node == &Value::nullSingleton()) { + // Error: unable to resolve path (object has no member named '' at + // position...) + return Value::nullSingleton(); + } + } + } + return *node; +} + +Value Path::resolve(const Value& root, const Value& defaultValue) const { + const Value* node = &root; + for (const auto& arg : args_) { + if (arg.kind_ == PathArgument::kindIndex) { + if (!node->isArray() || !node->isValidIndex(arg.index_)) + return defaultValue; + node = &((*node)[arg.index_]); + } else if (arg.kind_ == PathArgument::kindKey) { + if (!node->isObject()) + return defaultValue; + node = &((*node)[arg.key_]); + if (node == &Value::nullSingleton()) + return defaultValue; + } + } + return *node; +} + +Value& Path::make(Value& root) const { + Value* node = &root; + for (const auto& arg : args_) { + if (arg.kind_ == PathArgument::kindIndex) { + if (!node->isArray()) { + // Error: node is not an array at position ... + } + node = &((*node)[arg.index_]); + } else if (arg.kind_ == PathArgument::kindKey) { + if (!node->isObject()) { + // Error: node is not an object at position... + } + node = &((*node)[arg.key_]); + } + } + return *node; +} + +} // namespace Json + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: src/lib_json/json_value.cpp +// ////////////////////////////////////////////////////////////////////// + + + + + + +// ////////////////////////////////////////////////////////////////////// +// Beginning of content of file: src/lib_json/json_writer.cpp +// ////////////////////////////////////////////////////////////////////// + +// Copyright 2011 Baptiste Lepilleur and The JsonCpp Authors +// Distributed under MIT license, or public domain if desired and +// recognized in your jurisdiction. +// See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE + +#if !defined(JSON_IS_AMALGAMATION) +#include "json_tool.h" +#include +#endif // if !defined(JSON_IS_AMALGAMATION) +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#if __cplusplus >= 201103L +#include +#include + +#if !defined(isnan) +#define isnan std::isnan +#endif + +#if !defined(isfinite) +#define isfinite std::isfinite +#endif + +#else +#include +#include + +#if defined(_MSC_VER) +#if !defined(isnan) +#include +#define isnan _isnan +#endif + +#if !defined(isfinite) +#include +#define isfinite _finite +#endif + +#if !defined(_CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES) +#define _CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES 1 +#endif //_CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES + +#endif //_MSC_VER + +#if defined(__sun) && defined(__SVR4) // Solaris +#if !defined(isfinite) +#include +#define isfinite finite +#endif +#endif + +#if defined(__hpux) +#if !defined(isfinite) +#if defined(__ia64) && !defined(finite) +#define isfinite(x) \ + ((sizeof(x) == sizeof(float) ? _Isfinitef(x) : _IsFinite(x))) +#endif +#endif +#endif + +#if !defined(isnan) +// IEEE standard states that NaN values will not compare to themselves +#define isnan(x) ((x) != (x)) +#endif + +#if !defined(__APPLE__) +#if !defined(isfinite) +#define isfinite finite +#endif +#endif +#endif + +#if defined(_MSC_VER) +// Disable warning about strdup being deprecated. +#pragma warning(disable : 4996) +#endif + +namespace Json { + +#if __cplusplus >= 201103L || (defined(_CPPLIB_VER) && _CPPLIB_VER >= 520) +using StreamWriterPtr = std::unique_ptr; +#else +using StreamWriterPtr = std::auto_ptr; +#endif + +String valueToString(LargestInt value) { + UIntToStringBuffer buffer; + char* current = buffer + sizeof(buffer); + if (value == Value::minLargestInt) { + uintToString(LargestUInt(Value::maxLargestInt) + 1, current); + *--current = '-'; + } else if (value < 0) { + uintToString(LargestUInt(-value), current); + *--current = '-'; + } else { + uintToString(LargestUInt(value), current); + } + assert(current >= buffer); + return current; +} + +String valueToString(LargestUInt value) { + UIntToStringBuffer buffer; + char* current = buffer + sizeof(buffer); + uintToString(value, current); + assert(current >= buffer); + return current; +} + +#if defined(JSON_HAS_INT64) + +String valueToString(Int value) { return valueToString(LargestInt(value)); } + +String valueToString(UInt value) { return valueToString(LargestUInt(value)); } + +#endif // # if defined(JSON_HAS_INT64) + +namespace { +String valueToString(double value, bool useSpecialFloats, + unsigned int precision, PrecisionType precisionType) { + // Print into the buffer. We need not request the alternative representation + // that always has a decimal point because JSON doesn't distinguish the + // concepts of reals and integers. + if (!isfinite(value)) { + static const char* const reps[2][3] = {{"NaN", "-Infinity", "Infinity"}, + {"null", "-1e+9999", "1e+9999"}}; + return reps[useSpecialFloats ? 0 : 1] + [isnan(value) ? 0 : (value < 0) ? 1 : 2]; + } + + String buffer(size_t(36), '\0'); + while (true) { + int len = jsoncpp_snprintf( + &*buffer.begin(), buffer.size(), + (precisionType == PrecisionType::significantDigits) ? "%.*g" : "%.*f", + precision, value); + assert(len >= 0); + auto wouldPrint = static_cast(len); + if (wouldPrint >= buffer.size()) { + buffer.resize(wouldPrint + 1); + continue; + } + buffer.resize(wouldPrint); + break; + } + + buffer.erase(fixNumericLocale(buffer.begin(), buffer.end()), buffer.end()); + + // try to ensure we preserve the fact that this was given to us as a double on + // input + if (buffer.find('.') == buffer.npos && buffer.find('e') == buffer.npos) { + buffer += ".0"; + } + + // strip the zero padding from the right + if (precisionType == PrecisionType::decimalPlaces) { + buffer.erase(fixZerosInTheEnd(buffer.begin(), buffer.end(), precision), + buffer.end()); + } + + return buffer; +} +} // namespace + +String valueToString(double value, unsigned int precision, + PrecisionType precisionType) { + return valueToString(value, false, precision, precisionType); +} + +String valueToString(bool value) { return value ? "true" : "false"; } + +static bool doesAnyCharRequireEscaping(char const* s, size_t n) { + assert(s || !n); + + return std::any_of(s, s + n, [](unsigned char c) { + return c == '\\' || c == '"' || c < 0x20 || c > 0x7F; + }); +} + +static unsigned int utf8ToCodepoint(const char*& s, const char* e) { + const unsigned int REPLACEMENT_CHARACTER = 0xFFFD; + + unsigned int firstByte = static_cast(*s); + + if (firstByte < 0x80) + return firstByte; + + if (firstByte < 0xE0) { + if (e - s < 2) + return REPLACEMENT_CHARACTER; + + unsigned int calculated = + ((firstByte & 0x1F) << 6) | (static_cast(s[1]) & 0x3F); + s += 1; + // oversized encoded characters are invalid + return calculated < 0x80 ? REPLACEMENT_CHARACTER : calculated; + } + + if (firstByte < 0xF0) { + if (e - s < 3) + return REPLACEMENT_CHARACTER; + + unsigned int calculated = ((firstByte & 0x0F) << 12) | + ((static_cast(s[1]) & 0x3F) << 6) | + (static_cast(s[2]) & 0x3F); + s += 2; + // surrogates aren't valid codepoints itself + // shouldn't be UTF-8 encoded + if (calculated >= 0xD800 && calculated <= 0xDFFF) + return REPLACEMENT_CHARACTER; + // oversized encoded characters are invalid + return calculated < 0x800 ? REPLACEMENT_CHARACTER : calculated; + } + + if (firstByte < 0xF8) { + if (e - s < 4) + return REPLACEMENT_CHARACTER; + + unsigned int calculated = ((firstByte & 0x07) << 18) | + ((static_cast(s[1]) & 0x3F) << 12) | + ((static_cast(s[2]) & 0x3F) << 6) | + (static_cast(s[3]) & 0x3F); + s += 3; + // oversized encoded characters are invalid + return calculated < 0x10000 ? REPLACEMENT_CHARACTER : calculated; + } + + return REPLACEMENT_CHARACTER; +} + +static const char hex2[] = "000102030405060708090a0b0c0d0e0f" + "101112131415161718191a1b1c1d1e1f" + "202122232425262728292a2b2c2d2e2f" + "303132333435363738393a3b3c3d3e3f" + "404142434445464748494a4b4c4d4e4f" + "505152535455565758595a5b5c5d5e5f" + "606162636465666768696a6b6c6d6e6f" + "707172737475767778797a7b7c7d7e7f" + "808182838485868788898a8b8c8d8e8f" + "909192939495969798999a9b9c9d9e9f" + "a0a1a2a3a4a5a6a7a8a9aaabacadaeaf" + "b0b1b2b3b4b5b6b7b8b9babbbcbdbebf" + "c0c1c2c3c4c5c6c7c8c9cacbcccdcecf" + "d0d1d2d3d4d5d6d7d8d9dadbdcdddedf" + "e0e1e2e3e4e5e6e7e8e9eaebecedeeef" + "f0f1f2f3f4f5f6f7f8f9fafbfcfdfeff"; + +static String toHex16Bit(unsigned int x) { + const unsigned int hi = (x >> 8) & 0xff; + const unsigned int lo = x & 0xff; + String result(4, ' '); + result[0] = hex2[2 * hi]; + result[1] = hex2[2 * hi + 1]; + result[2] = hex2[2 * lo]; + result[3] = hex2[2 * lo + 1]; + return result; +} + +static void appendRaw(String& result, unsigned ch) { + result += static_cast(ch); +} + +static void appendHex(String& result, unsigned ch) { + result.append("\\u").append(toHex16Bit(ch)); +} + +static String valueToQuotedStringN(const char* value, size_t length, + bool emitUTF8 = false) { + if (value == nullptr) + return ""; + + if (!doesAnyCharRequireEscaping(value, length)) + return String("\"") + value + "\""; + // We have to walk value and escape any special characters. + // Appending to String is not efficient, but this should be rare. + // (Note: forward slashes are *not* rare, but I am not escaping them.) + String::size_type maxsize = length * 2 + 3; // allescaped+quotes+NULL + String result; + result.reserve(maxsize); // to avoid lots of mallocs + result += "\""; + char const* end = value + length; + for (const char* c = value; c != end; ++c) { + switch (*c) { + case '\"': + result += "\\\""; + break; + case '\\': + result += "\\\\"; + break; + case '\b': + result += "\\b"; + break; + case '\f': + result += "\\f"; + break; + case '\n': + result += "\\n"; + break; + case '\r': + result += "\\r"; + break; + case '\t': + result += "\\t"; + break; + // case '/': + // Even though \/ is considered a legal escape in JSON, a bare + // slash is also legal, so I see no reason to escape it. + // (I hope I am not misunderstanding something.) + // blep notes: actually escaping \/ may be useful in javascript to avoid (*c); + if (codepoint < 0x20) { + appendHex(result, codepoint); + } else { + appendRaw(result, codepoint); + } + } else { + unsigned codepoint = utf8ToCodepoint(c, end); // modifies `c` + if (codepoint < 0x20) { + appendHex(result, codepoint); + } else if (codepoint < 0x80) { + appendRaw(result, codepoint); + } else if (codepoint < 0x10000) { + // Basic Multilingual Plane + appendHex(result, codepoint); + } else { + // Extended Unicode. Encode 20 bits as a surrogate pair. + codepoint -= 0x10000; + appendHex(result, 0xd800 + ((codepoint >> 10) & 0x3ff)); + appendHex(result, 0xdc00 + (codepoint & 0x3ff)); + } + } + } break; + } + } + result += "\""; + return result; +} + +String valueToQuotedString(const char* value) { + return valueToQuotedStringN(value, strlen(value)); +} + +// Class Writer +// ////////////////////////////////////////////////////////////////// +Writer::~Writer() = default; + +// Class FastWriter +// ////////////////////////////////////////////////////////////////// + +FastWriter::FastWriter() + + = default; + +void FastWriter::enableYAMLCompatibility() { yamlCompatibilityEnabled_ = true; } + +void FastWriter::dropNullPlaceholders() { dropNullPlaceholders_ = true; } + +void FastWriter::omitEndingLineFeed() { omitEndingLineFeed_ = true; } + +String FastWriter::write(const Value& root) { + document_.clear(); + writeValue(root); + if (!omitEndingLineFeed_) + document_ += '\n'; + return document_; +} + +void FastWriter::writeValue(const Value& value) { + switch (value.type()) { + case nullValue: + if (!dropNullPlaceholders_) + document_ += "null"; + break; + case intValue: + document_ += valueToString(value.asLargestInt()); + break; + case uintValue: + document_ += valueToString(value.asLargestUInt()); + break; + case realValue: + document_ += valueToString(value.asDouble()); + break; + case stringValue: { + // Is NULL possible for value.string_? No. + char const* str; + char const* end; + bool ok = value.getString(&str, &end); + if (ok) + document_ += valueToQuotedStringN(str, static_cast(end - str)); + break; + } + case booleanValue: + document_ += valueToString(value.asBool()); + break; + case arrayValue: { + document_ += '['; + ArrayIndex size = value.size(); + for (ArrayIndex index = 0; index < size; ++index) { + if (index > 0) + document_ += ','; + writeValue(value[index]); + } + document_ += ']'; + } break; + case objectValue: { + Value::Members members(value.getMemberNames()); + document_ += '{'; + for (auto it = members.begin(); it != members.end(); ++it) { + const String& name = *it; + if (it != members.begin()) + document_ += ','; + document_ += valueToQuotedStringN(name.data(), name.length()); + document_ += yamlCompatibilityEnabled_ ? ": " : ":"; + writeValue(value[name]); + } + document_ += '}'; + } break; + } +} + +// Class StyledWriter +// ////////////////////////////////////////////////////////////////// + +StyledWriter::StyledWriter() = default; + +String StyledWriter::write(const Value& root) { + document_.clear(); + addChildValues_ = false; + indentString_.clear(); + writeCommentBeforeValue(root); + writeValue(root); + writeCommentAfterValueOnSameLine(root); + document_ += '\n'; + return document_; +} + +void StyledWriter::writeValue(const Value& value) { + switch (value.type()) { + case nullValue: + pushValue("null"); + break; + case intValue: + pushValue(valueToString(value.asLargestInt())); + break; + case uintValue: + pushValue(valueToString(value.asLargestUInt())); + break; + case realValue: + pushValue(valueToString(value.asDouble())); + break; + case stringValue: { + // Is NULL possible for value.string_? No. + char const* str; + char const* end; + bool ok = value.getString(&str, &end); + if (ok) + pushValue(valueToQuotedStringN(str, static_cast(end - str))); + else + pushValue(""); + break; + } + case booleanValue: + pushValue(valueToString(value.asBool())); + break; + case arrayValue: + writeArrayValue(value); + break; + case objectValue: { + Value::Members members(value.getMemberNames()); + if (members.empty()) + pushValue("{}"); + else { + writeWithIndent("{"); + indent(); + auto it = members.begin(); + for (;;) { + const String& name = *it; + const Value& childValue = value[name]; + writeCommentBeforeValue(childValue); + writeWithIndent(valueToQuotedString(name.c_str())); + document_ += " : "; + writeValue(childValue); + if (++it == members.end()) { + writeCommentAfterValueOnSameLine(childValue); + break; + } + document_ += ','; + writeCommentAfterValueOnSameLine(childValue); + } + unindent(); + writeWithIndent("}"); + } + } break; + } +} + +void StyledWriter::writeArrayValue(const Value& value) { + size_t size = value.size(); + if (size == 0) + pushValue("[]"); + else { + bool isArrayMultiLine = isMultilineArray(value); + if (isArrayMultiLine) { + writeWithIndent("["); + indent(); + bool hasChildValue = !childValues_.empty(); + ArrayIndex index = 0; + for (;;) { + const Value& childValue = value[index]; + writeCommentBeforeValue(childValue); + if (hasChildValue) + writeWithIndent(childValues_[index]); + else { + writeIndent(); + writeValue(childValue); + } + if (++index == size) { + writeCommentAfterValueOnSameLine(childValue); + break; + } + document_ += ','; + writeCommentAfterValueOnSameLine(childValue); + } + unindent(); + writeWithIndent("]"); + } else // output on a single line + { + assert(childValues_.size() == size); + document_ += "[ "; + for (size_t index = 0; index < size; ++index) { + if (index > 0) + document_ += ", "; + document_ += childValues_[index]; + } + document_ += " ]"; + } + } +} + +bool StyledWriter::isMultilineArray(const Value& value) { + ArrayIndex const size = value.size(); + bool isMultiLine = size * 3 >= rightMargin_; + childValues_.clear(); + for (ArrayIndex index = 0; index < size && !isMultiLine; ++index) { + const Value& childValue = value[index]; + isMultiLine = ((childValue.isArray() || childValue.isObject()) && + !childValue.empty()); + } + if (!isMultiLine) // check if line length > max line length + { + childValues_.reserve(size); + addChildValues_ = true; + ArrayIndex lineLength = 4 + (size - 1) * 2; // '[ ' + ', '*n + ' ]' + for (ArrayIndex index = 0; index < size; ++index) { + if (hasCommentForValue(value[index])) { + isMultiLine = true; + } + writeValue(value[index]); + lineLength += static_cast(childValues_[index].length()); + } + addChildValues_ = false; + isMultiLine = isMultiLine || lineLength >= rightMargin_; + } + return isMultiLine; +} + +void StyledWriter::pushValue(const String& value) { + if (addChildValues_) + childValues_.push_back(value); + else + document_ += value; +} + +void StyledWriter::writeIndent() { + if (!document_.empty()) { + char last = document_[document_.length() - 1]; + if (last == ' ') // already indented + return; + if (last != '\n') // Comments may add new-line + document_ += '\n'; + } + document_ += indentString_; +} + +void StyledWriter::writeWithIndent(const String& value) { + writeIndent(); + document_ += value; +} + +void StyledWriter::indent() { indentString_ += String(indentSize_, ' '); } + +void StyledWriter::unindent() { + assert(indentString_.size() >= indentSize_); + indentString_.resize(indentString_.size() - indentSize_); +} + +void StyledWriter::writeCommentBeforeValue(const Value& root) { + if (!root.hasComment(commentBefore)) + return; + + document_ += '\n'; + writeIndent(); + const String& comment = root.getComment(commentBefore); + String::const_iterator iter = comment.begin(); + while (iter != comment.end()) { + document_ += *iter; + if (*iter == '\n' && ((iter + 1) != comment.end() && *(iter + 1) == '/')) + writeIndent(); + ++iter; + } + + // Comments are stripped of trailing newlines, so add one here + document_ += '\n'; +} + +void StyledWriter::writeCommentAfterValueOnSameLine(const Value& root) { + if (root.hasComment(commentAfterOnSameLine)) + document_ += " " + root.getComment(commentAfterOnSameLine); + + if (root.hasComment(commentAfter)) { + document_ += '\n'; + document_ += root.getComment(commentAfter); + document_ += '\n'; + } +} + +bool StyledWriter::hasCommentForValue(const Value& value) { + return value.hasComment(commentBefore) || + value.hasComment(commentAfterOnSameLine) || + value.hasComment(commentAfter); +} + +// Class StyledStreamWriter +// ////////////////////////////////////////////////////////////////// + +StyledStreamWriter::StyledStreamWriter(String indentation) + : document_(nullptr), indentation_(std::move(indentation)), + addChildValues_(), indented_(false) {} + +void StyledStreamWriter::write(OStream& out, const Value& root) { + document_ = &out; + addChildValues_ = false; + indentString_.clear(); + indented_ = true; + writeCommentBeforeValue(root); + if (!indented_) + writeIndent(); + indented_ = true; + writeValue(root); + writeCommentAfterValueOnSameLine(root); + *document_ << "\n"; + document_ = nullptr; // Forget the stream, for safety. +} + +void StyledStreamWriter::writeValue(const Value& value) { + switch (value.type()) { + case nullValue: + pushValue("null"); + break; + case intValue: + pushValue(valueToString(value.asLargestInt())); + break; + case uintValue: + pushValue(valueToString(value.asLargestUInt())); + break; + case realValue: + pushValue(valueToString(value.asDouble())); + break; + case stringValue: { + // Is NULL possible for value.string_? No. + char const* str; + char const* end; + bool ok = value.getString(&str, &end); + if (ok) + pushValue(valueToQuotedStringN(str, static_cast(end - str))); + else + pushValue(""); + break; + } + case booleanValue: + pushValue(valueToString(value.asBool())); + break; + case arrayValue: + writeArrayValue(value); + break; + case objectValue: { + Value::Members members(value.getMemberNames()); + if (members.empty()) + pushValue("{}"); + else { + writeWithIndent("{"); + indent(); + auto it = members.begin(); + for (;;) { + const String& name = *it; + const Value& childValue = value[name]; + writeCommentBeforeValue(childValue); + writeWithIndent(valueToQuotedString(name.c_str())); + *document_ << " : "; + writeValue(childValue); + if (++it == members.end()) { + writeCommentAfterValueOnSameLine(childValue); + break; + } + *document_ << ","; + writeCommentAfterValueOnSameLine(childValue); + } + unindent(); + writeWithIndent("}"); + } + } break; + } +} + +void StyledStreamWriter::writeArrayValue(const Value& value) { + unsigned size = value.size(); + if (size == 0) + pushValue("[]"); + else { + bool isArrayMultiLine = isMultilineArray(value); + if (isArrayMultiLine) { + writeWithIndent("["); + indent(); + bool hasChildValue = !childValues_.empty(); + unsigned index = 0; + for (;;) { + const Value& childValue = value[index]; + writeCommentBeforeValue(childValue); + if (hasChildValue) + writeWithIndent(childValues_[index]); + else { + if (!indented_) + writeIndent(); + indented_ = true; + writeValue(childValue); + indented_ = false; + } + if (++index == size) { + writeCommentAfterValueOnSameLine(childValue); + break; + } + *document_ << ","; + writeCommentAfterValueOnSameLine(childValue); + } + unindent(); + writeWithIndent("]"); + } else // output on a single line + { + assert(childValues_.size() == size); + *document_ << "[ "; + for (unsigned index = 0; index < size; ++index) { + if (index > 0) + *document_ << ", "; + *document_ << childValues_[index]; + } + *document_ << " ]"; + } + } +} + +bool StyledStreamWriter::isMultilineArray(const Value& value) { + ArrayIndex const size = value.size(); + bool isMultiLine = size * 3 >= rightMargin_; + childValues_.clear(); + for (ArrayIndex index = 0; index < size && !isMultiLine; ++index) { + const Value& childValue = value[index]; + isMultiLine = ((childValue.isArray() || childValue.isObject()) && + !childValue.empty()); + } + if (!isMultiLine) // check if line length > max line length + { + childValues_.reserve(size); + addChildValues_ = true; + ArrayIndex lineLength = 4 + (size - 1) * 2; // '[ ' + ', '*n + ' ]' + for (ArrayIndex index = 0; index < size; ++index) { + if (hasCommentForValue(value[index])) { + isMultiLine = true; + } + writeValue(value[index]); + lineLength += static_cast(childValues_[index].length()); + } + addChildValues_ = false; + isMultiLine = isMultiLine || lineLength >= rightMargin_; + } + return isMultiLine; +} + +void StyledStreamWriter::pushValue(const String& value) { + if (addChildValues_) + childValues_.push_back(value); + else + *document_ << value; +} + +void StyledStreamWriter::writeIndent() { + // blep intended this to look at the so-far-written string + // to determine whether we are already indented, but + // with a stream we cannot do that. So we rely on some saved state. + // The caller checks indented_. + *document_ << '\n' << indentString_; +} + +void StyledStreamWriter::writeWithIndent(const String& value) { + if (!indented_) + writeIndent(); + *document_ << value; + indented_ = false; +} + +void StyledStreamWriter::indent() { indentString_ += indentation_; } + +void StyledStreamWriter::unindent() { + assert(indentString_.size() >= indentation_.size()); + indentString_.resize(indentString_.size() - indentation_.size()); +} + +void StyledStreamWriter::writeCommentBeforeValue(const Value& root) { + if (!root.hasComment(commentBefore)) + return; + + if (!indented_) + writeIndent(); + const String& comment = root.getComment(commentBefore); + String::const_iterator iter = comment.begin(); + while (iter != comment.end()) { + *document_ << *iter; + if (*iter == '\n' && ((iter + 1) != comment.end() && *(iter + 1) == '/')) + // writeIndent(); // would include newline + *document_ << indentString_; + ++iter; + } + indented_ = false; +} + +void StyledStreamWriter::writeCommentAfterValueOnSameLine(const Value& root) { + if (root.hasComment(commentAfterOnSameLine)) + *document_ << ' ' << root.getComment(commentAfterOnSameLine); + + if (root.hasComment(commentAfter)) { + writeIndent(); + *document_ << root.getComment(commentAfter); + } + indented_ = false; +} + +bool StyledStreamWriter::hasCommentForValue(const Value& value) { + return value.hasComment(commentBefore) || + value.hasComment(commentAfterOnSameLine) || + value.hasComment(commentAfter); +} + +////////////////////////// +// BuiltStyledStreamWriter + +/// Scoped enums are not available until C++11. +struct CommentStyle { + /// Decide whether to write comments. + enum Enum { + None, ///< Drop all comments. + Most, ///< Recover odd behavior of previous versions (not implemented yet). + All ///< Keep all comments. + }; +}; + +struct BuiltStyledStreamWriter : public StreamWriter { + BuiltStyledStreamWriter(String indentation, CommentStyle::Enum cs, + String colonSymbol, String nullSymbol, + String endingLineFeedSymbol, bool useSpecialFloats, + bool emitUTF8, unsigned int precision, + PrecisionType precisionType); + int write(Value const& root, OStream* sout) override; + +private: + void writeValue(Value const& value); + void writeArrayValue(Value const& value); + bool isMultilineArray(Value const& value); + void pushValue(String const& value); + void writeIndent(); + void writeWithIndent(String const& value); + void indent(); + void unindent(); + void writeCommentBeforeValue(Value const& root); + void writeCommentAfterValueOnSameLine(Value const& root); + static bool hasCommentForValue(const Value& value); + + using ChildValues = std::vector; + + ChildValues childValues_; + String indentString_; + unsigned int rightMargin_; + String indentation_; + CommentStyle::Enum cs_; + String colonSymbol_; + String nullSymbol_; + String endingLineFeedSymbol_; + bool addChildValues_ : 1; + bool indented_ : 1; + bool useSpecialFloats_ : 1; + bool emitUTF8_ : 1; + unsigned int precision_; + PrecisionType precisionType_; +}; +BuiltStyledStreamWriter::BuiltStyledStreamWriter( + String indentation, CommentStyle::Enum cs, String colonSymbol, + String nullSymbol, String endingLineFeedSymbol, bool useSpecialFloats, + bool emitUTF8, unsigned int precision, PrecisionType precisionType) + : rightMargin_(74), indentation_(std::move(indentation)), cs_(cs), + colonSymbol_(std::move(colonSymbol)), nullSymbol_(std::move(nullSymbol)), + endingLineFeedSymbol_(std::move(endingLineFeedSymbol)), + addChildValues_(false), indented_(false), + useSpecialFloats_(useSpecialFloats), emitUTF8_(emitUTF8), + precision_(precision), precisionType_(precisionType) {} +int BuiltStyledStreamWriter::write(Value const& root, OStream* sout) { + sout_ = sout; + addChildValues_ = false; + indented_ = true; + indentString_.clear(); + writeCommentBeforeValue(root); + if (!indented_) + writeIndent(); + indented_ = true; + writeValue(root); + writeCommentAfterValueOnSameLine(root); + *sout_ << endingLineFeedSymbol_; + sout_ = nullptr; + return 0; +} +void BuiltStyledStreamWriter::writeValue(Value const& value) { + switch (value.type()) { + case nullValue: + pushValue(nullSymbol_); + break; + case intValue: + pushValue(valueToString(value.asLargestInt())); + break; + case uintValue: + pushValue(valueToString(value.asLargestUInt())); + break; + case realValue: + pushValue(valueToString(value.asDouble(), useSpecialFloats_, precision_, + precisionType_)); + break; + case stringValue: { + // Is NULL is possible for value.string_? No. + char const* str; + char const* end; + bool ok = value.getString(&str, &end); + if (ok) + pushValue( + valueToQuotedStringN(str, static_cast(end - str), emitUTF8_)); + else + pushValue(""); + break; + } + case booleanValue: + pushValue(valueToString(value.asBool())); + break; + case arrayValue: + writeArrayValue(value); + break; + case objectValue: { + Value::Members members(value.getMemberNames()); + if (members.empty()) + pushValue("{}"); + else { + writeWithIndent("{"); + indent(); + auto it = members.begin(); + for (;;) { + String const& name = *it; + Value const& childValue = value[name]; + writeCommentBeforeValue(childValue); + writeWithIndent( + valueToQuotedStringN(name.data(), name.length(), emitUTF8_)); + *sout_ << colonSymbol_; + writeValue(childValue); + if (++it == members.end()) { + writeCommentAfterValueOnSameLine(childValue); + break; + } + *sout_ << ","; + writeCommentAfterValueOnSameLine(childValue); + } + unindent(); + writeWithIndent("}"); + } + } break; + } +} + +void BuiltStyledStreamWriter::writeArrayValue(Value const& value) { + unsigned size = value.size(); + if (size == 0) + pushValue("[]"); + else { + bool isMultiLine = (cs_ == CommentStyle::All) || isMultilineArray(value); + if (isMultiLine) { + writeWithIndent("["); + indent(); + bool hasChildValue = !childValues_.empty(); + unsigned index = 0; + for (;;) { + Value const& childValue = value[index]; + writeCommentBeforeValue(childValue); + if (hasChildValue) + writeWithIndent(childValues_[index]); + else { + if (!indented_) + writeIndent(); + indented_ = true; + writeValue(childValue); + indented_ = false; + } + if (++index == size) { + writeCommentAfterValueOnSameLine(childValue); + break; + } + *sout_ << ","; + writeCommentAfterValueOnSameLine(childValue); + } + unindent(); + writeWithIndent("]"); + } else // output on a single line + { + assert(childValues_.size() == size); + *sout_ << "["; + if (!indentation_.empty()) + *sout_ << " "; + for (unsigned index = 0; index < size; ++index) { + if (index > 0) + *sout_ << ((!indentation_.empty()) ? ", " : ","); + *sout_ << childValues_[index]; + } + if (!indentation_.empty()) + *sout_ << " "; + *sout_ << "]"; + } + } +} + +bool BuiltStyledStreamWriter::isMultilineArray(Value const& value) { + ArrayIndex const size = value.size(); + bool isMultiLine = size * 3 >= rightMargin_; + childValues_.clear(); + for (ArrayIndex index = 0; index < size && !isMultiLine; ++index) { + Value const& childValue = value[index]; + isMultiLine = ((childValue.isArray() || childValue.isObject()) && + !childValue.empty()); + } + if (!isMultiLine) // check if line length > max line length + { + childValues_.reserve(size); + addChildValues_ = true; + ArrayIndex lineLength = 4 + (size - 1) * 2; // '[ ' + ', '*n + ' ]' + for (ArrayIndex index = 0; index < size; ++index) { + if (hasCommentForValue(value[index])) { + isMultiLine = true; + } + writeValue(value[index]); + lineLength += static_cast(childValues_[index].length()); + } + addChildValues_ = false; + isMultiLine = isMultiLine || lineLength >= rightMargin_; + } + return isMultiLine; +} + +void BuiltStyledStreamWriter::pushValue(String const& value) { + if (addChildValues_) + childValues_.push_back(value); + else + *sout_ << value; +} + +void BuiltStyledStreamWriter::writeIndent() { + // blep intended this to look at the so-far-written string + // to determine whether we are already indented, but + // with a stream we cannot do that. So we rely on some saved state. + // The caller checks indented_. + + if (!indentation_.empty()) { + // In this case, drop newlines too. + *sout_ << '\n' << indentString_; + } +} + +void BuiltStyledStreamWriter::writeWithIndent(String const& value) { + if (!indented_) + writeIndent(); + *sout_ << value; + indented_ = false; +} + +void BuiltStyledStreamWriter::indent() { indentString_ += indentation_; } + +void BuiltStyledStreamWriter::unindent() { + assert(indentString_.size() >= indentation_.size()); + indentString_.resize(indentString_.size() - indentation_.size()); +} + +void BuiltStyledStreamWriter::writeCommentBeforeValue(Value const& root) { + if (cs_ == CommentStyle::None) + return; + if (!root.hasComment(commentBefore)) + return; + + if (!indented_) + writeIndent(); + const String& comment = root.getComment(commentBefore); + String::const_iterator iter = comment.begin(); + while (iter != comment.end()) { + *sout_ << *iter; + if (*iter == '\n' && ((iter + 1) != comment.end() && *(iter + 1) == '/')) + // writeIndent(); // would write extra newline + *sout_ << indentString_; + ++iter; + } + indented_ = false; +} + +void BuiltStyledStreamWriter::writeCommentAfterValueOnSameLine( + Value const& root) { + if (cs_ == CommentStyle::None) + return; + if (root.hasComment(commentAfterOnSameLine)) + *sout_ << " " + root.getComment(commentAfterOnSameLine); + + if (root.hasComment(commentAfter)) { + writeIndent(); + *sout_ << root.getComment(commentAfter); + } +} + +// static +bool BuiltStyledStreamWriter::hasCommentForValue(const Value& value) { + return value.hasComment(commentBefore) || + value.hasComment(commentAfterOnSameLine) || + value.hasComment(commentAfter); +} + +/////////////// +// StreamWriter + +StreamWriter::StreamWriter() : sout_(nullptr) {} +StreamWriter::~StreamWriter() = default; +StreamWriter::Factory::~Factory() = default; +StreamWriterBuilder::StreamWriterBuilder() { setDefaults(&settings_); } +StreamWriterBuilder::~StreamWriterBuilder() = default; +StreamWriter* StreamWriterBuilder::newStreamWriter() const { + const String indentation = settings_["indentation"].asString(); + const String cs_str = settings_["commentStyle"].asString(); + const String pt_str = settings_["precisionType"].asString(); + const bool eyc = settings_["enableYAMLCompatibility"].asBool(); + const bool dnp = settings_["dropNullPlaceholders"].asBool(); + const bool usf = settings_["useSpecialFloats"].asBool(); + const bool emitUTF8 = settings_["emitUTF8"].asBool(); + unsigned int pre = settings_["precision"].asUInt(); + CommentStyle::Enum cs = CommentStyle::All; + if (cs_str == "All") { + cs = CommentStyle::All; + } else if (cs_str == "None") { + cs = CommentStyle::None; + } else { + throwRuntimeError("commentStyle must be 'All' or 'None'"); + } + PrecisionType precisionType(significantDigits); + if (pt_str == "significant") { + precisionType = PrecisionType::significantDigits; + } else if (pt_str == "decimal") { + precisionType = PrecisionType::decimalPlaces; + } else { + throwRuntimeError("precisionType must be 'significant' or 'decimal'"); + } + String colonSymbol = " : "; + if (eyc) { + colonSymbol = ": "; + } else if (indentation.empty()) { + colonSymbol = ":"; + } + String nullSymbol = "null"; + if (dnp) { + nullSymbol.clear(); + } + if (pre > 17) + pre = 17; + String endingLineFeedSymbol; + return new BuiltStyledStreamWriter(indentation, cs, colonSymbol, nullSymbol, + endingLineFeedSymbol, usf, emitUTF8, pre, + precisionType); +} + +bool StreamWriterBuilder::validate(Json::Value* invalid) const { + static const auto& valid_keys = *new std::set{ + "indentation", + "commentStyle", + "enableYAMLCompatibility", + "dropNullPlaceholders", + "useSpecialFloats", + "emitUTF8", + "precision", + "precisionType", + }; + for (auto si = settings_.begin(); si != settings_.end(); ++si) { + auto key = si.name(); + if (valid_keys.count(key)) + continue; + if (invalid) + (*invalid)[key] = *si; + else + return false; + } + return invalid ? invalid->empty() : true; +} + +Value& StreamWriterBuilder::operator[](const String& key) { + return settings_[key]; +} +// static +void StreamWriterBuilder::setDefaults(Json::Value* settings) { + //! [StreamWriterBuilderDefaults] + (*settings)["commentStyle"] = "All"; + (*settings)["indentation"] = "\t"; + (*settings)["enableYAMLCompatibility"] = false; + (*settings)["dropNullPlaceholders"] = false; + (*settings)["useSpecialFloats"] = false; + (*settings)["emitUTF8"] = false; + (*settings)["precision"] = 17; + (*settings)["precisionType"] = "significant"; + //! [StreamWriterBuilderDefaults] +} + +String writeString(StreamWriter::Factory const& factory, Value const& root) { + OStringStream sout; + StreamWriterPtr const writer(factory.newStreamWriter()); + writer->write(root, &sout); + return sout.str(); +} + +OStream& operator<<(OStream& sout, Value const& root) { + StreamWriterBuilder builder; + StreamWriterPtr const writer(builder.newStreamWriter()); + writer->write(root, &sout); + return sout; +} + +} // namespace Json + +// ////////////////////////////////////////////////////////////////////// +// End of content of file: src/lib_json/json_writer.cpp +// ////////////////////////////////////////////////////////////////////// + + + + + diff --git a/src/task_manager/src/md5.cpp b/src/task_manager/src/md5.cpp new file mode 100644 index 0000000..f6f1245 --- /dev/null +++ b/src/task_manager/src/md5.cpp @@ -0,0 +1,372 @@ +#include "md5.h" +#include +#include +#include + +// 重命名自定义的 byte 类型 +typedef unsigned char uchar; + +using namespace std; + +/* Constants for MD5Transform routine. */ +#define S11 7 +#define S12 12 +#define S13 17 +#define S14 22 +#define S21 5 +#define S22 9 +#define S23 14 +#define S24 20 +#define S31 4 +#define S32 11 +#define S33 16 +#define S34 23 +#define S41 6 +#define S42 10 +#define S43 15 +#define S44 21 + + +/* F, G, H and I are basic MD5 functions. + */ +#define F(x, y, z) (((x) & (y)) | ((~x) & (z))) +#define G(x, y, z) (((x) & (z)) | ((y) & (~z))) +#define H(x, y, z) ((x) ^ (y) ^ (z)) +#define I(x, y, z) ((y) ^ ((x) | (~z))) + +/* ROTATE_LEFT rotates x left n bits. + */ +#define ROTATE_LEFT(x, n) (((x) << (n)) | ((x) >> (32 - (n)))) + +/* FF, GG, HH, and II transformations for rounds 1, 2, 3, and 4. +Rotation is separate from addition to prevent recomputation. +*/ +#define FF(a, b, c, d, x, s, ac) \ + { \ + (a) += F((b), (c), (d)) + (x) + ac; \ + (a) = ROTATE_LEFT((a), (s)); \ + (a) += (b); \ + } +#define GG(a, b, c, d, x, s, ac) \ + { \ + (a) += G((b), (c), (d)) + (x) + ac; \ + (a) = ROTATE_LEFT((a), (s)); \ + (a) += (b); \ + } +#define HH(a, b, c, d, x, s, ac) \ + { \ + (a) += H((b), (c), (d)) + (x) + ac; \ + (a) = ROTATE_LEFT((a), (s)); \ + (a) += (b); \ + } +#define II(a, b, c, d, x, s, ac) \ + { \ + (a) += I((b), (c), (d)) + (x) + ac; \ + (a) = ROTATE_LEFT((a), (s)); \ + (a) += (b); \ + } + +const uchar MD5::PADDING[64] = {0x80}; +const char MD5::HEX[16] = { + '0', '1', '2', '3', + '4', '5', '6', '7', + '8', '9', 'a', 'b', + 'c', 'd', 'e', 'f'}; + +/* Default construct. */ +MD5::MD5() +{ + reset(); +} + +/* Construct a MD5 object with a input buffer. */ +MD5::MD5(const void *input, size_t length) +{ + reset(); + update(input, length); +} + +/* Construct a MD5 object with a string. */ +MD5::MD5(const string &str) +{ + reset(); + update(str); +} + +/* Construct a MD5 object with a file. */ +MD5::MD5(ifstream &in) +{ + reset(); + update(in); +} + +/* Return the message-digest */ +const uchar *MD5::digest() +{ + if (!_finished) + { + _finished = true; + final(); + } + return _digest; +} + +/* Reset the calculate state */ +void MD5::reset() +{ + _finished = false; + /* reset number of bits. */ + _count[0] = _count[1] = 0; + /* Load magic initialization constants. */ + _state[0] = 0x67452301; + _state[1] = 0xefcdab89; + _state[2] = 0x98badcfe; + _state[3] = 0x10325476; +} + +/* Updating the context with a input buffer. */ +void MD5::update(const void *input, size_t length) +{ + const uchar* uinput = static_cast(input); + update(uinput, length); +} + +/* Updating the context with a string. */ +void MD5::update(const string &str) +{ + const uchar* uinput = reinterpret_cast(str.c_str()); + update(uinput, str.length()); +} + +/* Updating the context with a file. */ +void MD5::update(ifstream &in) +{ + if (!in) + { + return; + } + + std::streamsize length; + char buffer[1024]; + while (!in.eof()) + { + in.read(buffer, 1024); + length = in.gcount(); + if (length > 0) + { + const uchar* uinput = reinterpret_cast(buffer); + update(uinput, length); + } + } + in.close(); +} + +/* MD5 block update operation. Continues an MD5 message-digest +operation, processing another message block, and updating the +context. +*/ +void MD5::update(const uchar *input, size_t length) +{ + uint32 i, index, partLen; + + _finished = false; + + /* Compute number of bytes mod 64 */ + index = (uint32)((_count[0] >> 3) & 0x3f); + + /* update number of bits */ + if ((_count[0] += ((uint32)length << 3)) < ((uint32)length << 3)) + { + ++_count[1]; + } + _count[1] += ((uint32)length >> 29); + + partLen = 64 - index; + + /* transform as many times as possible. */ + if (length >= partLen) + { + memcpy(&_buffer[index], input, partLen); + transform(_buffer); + + for (i = partLen; i + 63 < length; i += 64) + { + transform(&input[i]); + } + index = 0; + } + else + { + i = 0; + } + + /* Buffer remaining input */ + memcpy(&_buffer[index], &input[i], length - i); +} + +/* MD5 finalization. Ends an MD5 message-_digest operation, writing the +the message _digest and zeroizing the context. +*/ +void MD5::final() +{ + uchar bits[8]; + uint32 oldState[4]; + uint32 oldCount[2]; + uint32 index, padLen; + + /* Save current state and count. */ + memcpy(oldState, _state, 16); + memcpy(oldCount, _count, 8); + + /* Save number of bits */ + encode(_count, bits, 8); + + /* Pad out to 56 mod 64. */ + index = (uint32)((_count[0] >> 3) & 0x3f); + padLen = (index < 56) ? (56 - index) : (120 - index); + update(PADDING, padLen); + + /* Append length (before padding) */ + update(bits, 8); + + /* Store state in digest */ + encode(_state, _digest, 16); + + /* Restore current state and count. */ + memcpy(_state, oldState, 16); + memcpy(_count, oldCount, 8); +} + +/* MD5 basic transformation. Transforms _state based on block. */ +/* MD5 basic transformation. Transforms _state based on block. */ +void MD5::transform(const uchar block[64]) +{ + uint32 a = _state[0], b = _state[1], c = _state[2], d = _state[3], x[16]; + + decode(block, x, 64); + + /* Round 1 */ + FF(a, b, c, d, x[0], S11, 0xd76aa478); /* 1 */ + FF(d, a, b, c, x[1], S12, 0xe8c7b756); /* 2 */ + FF(c, d, a, b, x[2], S13, 0x242070db); /* 3 */ + FF(b, c, d, a, x[3], S14, 0xc1bdceee); /* 4 */ + FF(a, b, c, d, x[4], S11, 0xf57c0faf); /* 5 */ + FF(d, a, b, c, x[5], S12, 0x4787c62a); /* 6 */ + FF(c, d, a, b, x[6], S13, 0xa8304613); /* 7 */ + FF(b, c, d, a, x[7], S14, 0xfd469501); /* 8 */ + FF(a, b, c, d, x[8], S11, 0x698098d8); /* 9 */ + FF(d, a, b, c, x[9], S12, 0x8b44f7af); /* 10 */ + FF(c, d, a, b, x[10], S13, 0xffff5bb1); /* 11 */ + FF(b, c, d, a, x[11], S14, 0x895cd7be); /* 12 */ + FF(a, b, c, d, x[12], S11, 0x6b901122); /* 13 */ + FF(d, a, b, c, x[13], S12, 0xfd987193); /* 14 */ + FF(c, d, a, b, x[14], S13, 0xa679438e); /* 15 */ + FF(b, c, d, a, x[15], S14, 0x49b40821); /* 16 */ + + /* Round 2 */ + GG(a, b, c, d, x[1], S21, 0xf61e2562); /* 17 */ + GG(d, a, b, c, x[6], S22, 0xc040b340); /* 18 */ + GG(c, d, a, b, x[11], S23, 0x265e5a51); /* 19 */ + GG(b, c, d, a, x[0], S24, 0xe9b6c7aa); /* 20 */ + GG(a, b, c, d, x[5], S21, 0xd62f105d); /* 21 */ + GG(d, a, b, c, x[10], S22, 0x2441453); /* 22 */ + GG(c, d, a, b, x[15], S23, 0xd8a1e681); /* 23 */ + GG(b, c, d, a, x[4], S24, 0xe7d3fbc8); /* 24 */ + GG(a, b, c, d, x[9], S21, 0x21e1cde6); /* 25 */ + GG(d, a, b, c, x[14], S22, 0xc33707d6); /* 26 */ + GG(c, d, a, b, x[3], S23, 0xf4d50d87); /* 27 */ + GG(b, c, d, a, x[8], S24, 0x455a14ed); /* 28 */ + GG(a, b, c, d, x[13], S21, 0xa9e3e905); /* 29 */ + GG(d, a, b, c, x[2], S22, 0xfcefa3f8); /* 30 */ + GG(c, d, a, b, x[7], S23, 0x676f02d9); /* 31 */ + GG(b, c, d, a, x[12], S24, 0x8d2a4c8a); /* 32 */ + + /* Round 3 */ + HH(a, b, c, d, x[5], S31, 0xfffa3942); /* 33 */ + HH(d, a, b, c, x[8], S32, 0x8771f681); /* 34 */ + HH(c, d, a, b, x[11], S33, 0x6d9d6122); /* 35 */ + HH(b, c, d, a, x[14], S34, 0xfde5380c); /* 36 */ + HH(a, b, c, d, x[1], S31, 0xa4beea44); /* 37 */ + HH(d, a, b, c, x[4], S32, 0x4bdecfa9); /* 38 */ + HH(c, d, a, b, x[7], S33, 0xf6bb4b60); /* 39 */ + HH(b, c, d, a, x[10], S34, 0xbebfbc70); /* 40 */ + HH(a, b, c, d, x[13], S31, 0x289b7ec6); /* 41 */ + HH(d, a, b, c, x[0], S32, 0xeaa127fa); /* 42 */ + HH(c, d, a, b, x[3], S33, 0xd4ef3085); /* 43 */ + HH(b, c, d, a, x[6], S34, 0x4881d05); /* 44 */ + HH(a, b, c, d, x[9], S31, 0xd9d4d039); /* 45 */ + HH(d, a, b, c, x[12], S32, 0xe6db99e5); /* 46 */ + HH(c, d, a, b, x[15], S33, 0x1fa27cf8); /* 47 */ + HH(b, c, d, a, x[2], S34, 0xc4ac5665); /* 48 */ + + /* Round 4 */ + II(a, b, c, d, x[0], S41, 0xf4292244); /* 49 */ + II(d, a, b, c, x[7], S42, 0x432aff97); /* 50 */ + II(c, d, a, b, x[14], S43, 0xab9423a7); /* 51 */ + II(b, c, d, a, x[5], S44, 0xfc93a039); /* 52 */ + II(a, b, c, d, x[12], S41, 0x655b59c3); /* 53 */ + II(d, a, b, c, x[3], S42, 0x8f0ccc92); /* 54 */ + II(c, d, a, b, x[10], S43, 0xffeff47d); /* 55 */ + II(b, c, d, a, x[1], S44, 0x85845dd1); /* 56 */ + II(a, b, c, d, x[8], S41, 0x6fa87e4f); /* 57 */ + II(d, a, b, c, x[15], S42, 0xfe2ce6e0); /* 58 */ + II(c, d, a, b, x[6], S43, 0xa3014314); /* 59 */ + II(b, c, d, a, x[13], S44, 0x4e0811a1); /* 60 */ + II(a, b, c, d, x[4], S41, 0xf7537e82); /* 61 */ + II(d, a, b, c, x[11], S42, 0xbd3af235); /* 62 */ + II(c, d, a, b, x[2], S43, 0x2ad7d2bb); /* 63 */ + II(b, c, d, a, x[9], S44, 0xeb86d391); /* 64 */ + + _state[0] += a; + _state[1] += b; + _state[2] += c; + _state[3] += d; +} + +/* Encodes input (ulong) into output (byte). Assumes length is +a multiple of 4. +*/ +void MD5::encode(const uint32 *input, uchar *output, size_t length) +{ + for (size_t i = 0, j = 0; j < length; ++i, j += 4) + { + output[j] = (uchar)(input[i] & 0xff); + output[j + 1] = (uchar)((input[i] >> 8) & 0xff); + output[j + 2] = (uchar)((input[i] >> 16) & 0xff); + output[j + 3] = (uchar)((input[i] >> 24) & 0xff); + } +} + +/* Decodes input (byte) into output (ulong). Assumes length is +a multiple of 4. +*/ +void MD5::decode(const uchar *input, uint32 *output, size_t length) +{ + for (size_t i = 0, j = 0; j < length; ++i, j += 4) + { + output[i] = ((uint32)input[j]) | (((uint32)input[j + 1]) << 8) | + (((uint32)input[j + 2]) << 16) | (((uint32)input[j + 3]) << 24); + } +} + +/* Convert byte array to hex string. */ +string MD5::bytesToHexString(const uchar *input, size_t length) +{ + string str; + str.reserve(length << 1); + for (size_t i = 0; i < length; ++i) + { + int t = input[i]; + int a = t / 16; + int b = t % 16; + str.append(1, HEX[a]); + str.append(1, HEX[b]); + } + return str; +} + +/* Convert digest to string value */ +string MD5::toString() +{ + return bytesToHexString(digest(), 16); +} \ No newline at end of file diff --git a/src/task_manager/src/task_manager_node.cpp b/src/task_manager/src/task_manager_node.cpp new file mode 100644 index 0000000..5365e12 --- /dev/null +++ b/src/task_manager/src/task_manager_node.cpp @@ -0,0 +1,787 @@ +#include "MQTTClient.h" +#include "rclcpp/rclcpp.hpp" +#include "md5.h" +#include "sweeper_interfaces/msg/task.hpp" +#include "json.h" +#include "task_manager_node.hpp" +#include "sweeper_interfaces/msg/sub.hpp" +#include // 添加信号处理头文件 + +using namespace std; + +// 信号标志 +volatile std::sig_atomic_t signal_received = 0; +// 任务队列和相关同步原语 +std::queue task_queue; +std::mutex queue_mutex; +std::condition_variable queue_cv; +bool thread_running = true; + +MQTTClient client; +MQTTClient_deliveryToken token_d_m; +MQTTClient_connectOptions conn_opts = MQTTClient_connectOptions_initializer; +MQTTClient_message pubmsg_d_m = MQTTClient_message_initializer; +// mqtt相关 +std::string mqtt_vid; +std::string mqtt_inter_net_address; +int mqtt_inter_net_port; +std::string mqtt_external_net_address; +int mqtt_external_net_port; +std::string mqtt_user; +std::string mqtt_password; +std::string mqtt_topic_sub_task; +std::string mqtt_topic_push_status; +std::string sub_topic; + +// 修复变量声明和初始化 +std::string ADDRESS; +std::string CLIENTID_SUB = "sweeper_CLIENT_800"; // 客户端ID +std::string destinationFilePath1 = "./gps_load_now.txt"; +std::chrono::steady_clock::time_point last_message_time = std::chrono::steady_clock::now(); +constexpr auto QOS = 0; +constexpr auto TIMEOUT = 10000L; +volatile MQTTClient_deliveryToken deliveredtoken; +bool isDisconn = false; +int task_status = 0; +int last_status = 0; +TaskStatus status_up = TaskStatus::PENDING; + +CurrentTask currentTask; +std::mutex taskMutex; + +// 信号处理函数 +void signalHandler(int signum) +{ + std::cout << "Interrupt signal (" << signum << ") received." << std::endl; + signal_received = signum; + + // 第一次收到信号时，触发ROS2关闭 + if (signum == SIGINT && rclcpp::ok()) + { + std::cout << "Shutting down ROS2 node..." << std::endl; + rclcpp::shutdown(); + } +} + +void copyFileAndOverwrite(const std::string &sourceFilePath, const std::string &destinationFilePath) +{ + std::ifstream src(sourceFilePath, std::ios::binary); + std::ofstream dst(destinationFilePath, std::ios::binary); + + if (!src) + { + std::cerr << "无法打开源文件: " << sourceFilePath << std::endl; + return; + } + + if (!dst) + { + std::cerr << "无法打开目标文件: " << destinationFilePath << std::endl; + return; + } + + dst << src.rdbuf(); // 将源文件内容复制到目标文件 + + if (!dst) + { + std::cerr << "复制文件时出错" << std::endl; + } +} + +string calculate_md5(string filename) +{ + MD5 md5; + ifstream file; + file.open(filename, ios::binary); + md5.update(file); + cout << md5.toString() << endl; + return md5.toString(); +} + +bool downloadFile(const std::string &url, const std::string &expected_md5) +{ + // 确保URL不为空 + if (url.empty()) + { + std::cerr << "下载URL为空" << std::endl; + return false; + } + // 使用系统调用下载文件 + std::string filename = url.substr(url.find_last_of('/') + 1); + std::string command = "wget -O routes/" + filename + " \"" + url + "\""; + cout << command << endl; + int result = system(command.c_str()); + if (result != 0) + { + std::cerr << "下载失败: " << url << std::endl; + return false; + } + + // 计算下载文件的MD5并与预期的MD5比较 + std::string filepath = "routes/" + filename; + std::string actual_md5 = calculate_md5(filepath); + if (actual_md5 != expected_md5) + { + cout << actual_md5 << " " << expected_md5 << endl; + std::cerr << "MD5校验失败: " << filepath << std::endl; + return false; + } + + std::cout << filepath << " 下载并校验成功" << std::endl; + return true; +} + +ROUTE_INFO get_route_info(const Json::Value &root) +{ + ROUTE_INFO route_info; + if (root.isObject()) + { + if (root.isMember("routeName") && root["routeName"].isString()) + route_info.routeName = root["routeName"].asString(); + else + std::cerr << "Missing or invalid 'routeName' field" << std::endl; + + if (root.isMember("fileName") && root["fileName"].isString()) + route_info.fileName = root["fileName"].asString(); + else + std::cerr << "Missing or invalid 'fileName' field" << std::endl; + + if (root.isMember("url") && root["url"].isString()) + route_info.url = root["url"].asString(); + else + std::cerr << "Missing or invalid 'url' field" << std::endl; + + if (root.isMember("md5") && root["md5"].isString()) + route_info.md5 = root["md5"].asString(); + else + std::cerr << "Missing or invalid 'md5' field" << std::endl; + } + else + { + std::cerr << "routeInfo is not a valid JSON object" << std::endl; + } + + return route_info; +} + +TASK_VALUE get_task_value(const Json::Value &root) +{ + TASK_VALUE task_value; + + if (root.isObject()) + { + if (root.isMember("id") && root["id"].isInt()) + task_value.id = root["id"].asInt(); + else + std::cerr << "Missing or invalid 'id' field" << std::endl; + + if (root.isMember("name") && root["name"].isString()) + task_value.name = root["name"].asString(); + else + std::cerr << "Missing or invalid 'name' field" << std::endl; + + if (root.isMember("mode") && root["mode"].isInt()) + task_value.mode = root["mode"].asInt(); + else + std::cerr << "Missing or invalid 'mode' field" << std::endl; + + if (root.isMember("count") && root["count"].isInt()) + task_value.count = root["count"].asInt(); + else + std::cerr << "Missing or invalid 'count' field" << std::endl; + + if (root.isMember("routeInfo") && root["routeInfo"].isObject()) + task_value.routeInfo = get_route_info(root["routeInfo"]); + else + std::cerr << "Missing or invalid 'routeInfo' field" << std::endl; + } + else + { + std::cerr << "task value is not a valid JSON object" << std::endl; + } + + return task_value; +} + +void sendResponse(MQTTClient client, const std::string &topic, int seqNo, int code, std::string msg) +{ + // 使用JSON库构建响应数据 + Json::Value responseData; + responseData["type"] = "response"; + responseData["seqNo"] = seqNo; + + Json::Value data; + data["code"] = code; + data["msg"] = msg; + + responseData["data"] = data; + + // 使用JSON writer生成字符串 + Json::FastWriter writer; + std::string responseJson = writer.write(responseData); + + // 发布MQTT消息 + pubmsg_d_m.payload = (void *)responseJson.c_str(); + pubmsg_d_m.payloadlen = responseJson.length(); + pubmsg_d_m.qos = QOS; + pubmsg_d_m.retained = 0; + + MQTTClient_publishMessage(client, topic.c_str(), &pubmsg_d_m, &token_d_m); + MQTTClient_waitForCompletion(client, token_d_m, TIMEOUT); + printf("Message with delivery token %d delivered\n", token_d_m); +} + +// 更新当前任务信息 +void updateCurrentTask(int taskId, TaskStatus status) +{ + std::lock_guard lock(taskMutex); + currentTask.id = taskId; + currentTask.status = status; + currentTask.lastUpdateTime = std::chrono::steady_clock::now(); + std::cout << "Task updated: ID=" << taskId << ", Status=" << status << std::endl; +} + +// 从MQTT消息中提取任务ID并更新任务状态 +void extractTaskIdFromMessage(const Json::Value &root) +{ + if (root.isMember("data") && root["data"].isObject()) + { + Json::Value data = root["data"]; + + if (data.isMember("value") && data["value"].isObject()) + { + Json::Value value = data["value"]; + + if (value.isMember("id") && value["id"].isInt()) + { + int taskId = value["id"].asInt(); + + // 根据命令类型设置初始状态 + if (data.isMember("command") && data["command"].isString()) + { + std::string command = data["command"].asString(); + + if (command == "start") + { + updateCurrentTask(taskId, RUNNING); + } + else if (command == "stop") + { + updateCurrentTask(taskId, PENDING); + } + } + } + } + } +} + +void updateTaskStatus(int taskId, int status) +{ + // 构建JSON消息体 + Json::Value message; + message["id"] = taskId; + message["status"] = status; + + // 转换为字符串 + Json::FastWriter writer; + std::string jsonStr = writer.write(message); + + // 发布MQTT消息 + MQTTClient_message pubmsg = MQTTClient_message_initializer; + pubmsg.payload = (void *)jsonStr.c_str(); + pubmsg.payloadlen = jsonStr.length(); + pubmsg.qos = QOS; + pubmsg.retained = 0; + + MQTTClient_deliveryToken token; + int rc = MQTTClient_publishMessage(client, mqtt_topic_push_status.c_str(), &pubmsg, &token); + if (rc != MQTTCLIENT_SUCCESS) + { + std::cerr << "Failed to publish task status, return code " << rc << std::endl; + } + else + { + MQTTClient_waitForCompletion(client, token, TIMEOUT); + std::cout << "Task status updated: ID=" << taskId << ", Status=" << status << std::endl; + } +} + +// 周期性上报任务状态 +void *reportTaskStatus(void *arg) +{ + (void)arg; + + while (thread_running) + { + // 200ms周期 + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + + // 获取当前任务信息 + int taskId; + TaskStatus status; + + { + std::lock_guard lock(taskMutex); + taskId = currentTask.id; + status = status_up; + + // 仅在任务ID有效时上报 + if (taskId > 0) + { + currentTask.status = status; + } + } + + // 上报任务状态 + if (taskId > 0) + { + updateTaskStatus(taskId, static_cast(status)); + } + } + + return NULL; +} + +// 任务处理线程函数 +void *process_tasks(void *arg) +{ + (void)arg; + + while (thread_running) + { + std::unique_lock lock(queue_mutex); + + // 等待队列中有任务或线程终止信号 + queue_cv.wait(lock, [] + { return !task_queue.empty() || !thread_running; }); + + // 检查是否需要终止线程 + if (!thread_running && task_queue.empty()) + { + break; + } + + // 处理队列中的任务 + if (!task_queue.empty()) + { + MQTT_JSON mqtt_json = task_queue.front(); + task_queue.pop(); + lock.unlock(); // 释放锁，允许其他线程操作队列 + + // 处理任务 + if (mqtt_json.type == "request") + { + try + { + // 从boost::any中提取数据 + JSON_DATA json_data = boost::any_cast(mqtt_json.data); + if (json_data.command == "start") + { + int flag_ok = 0; + // sleep(10); + cout << "即将开始任务" << json_data.value.id << endl; + cout << "url: " << json_data.value.routeInfo.url << endl; + cout << "md5: " << json_data.value.routeInfo.md5 << endl; + string downFileName = json_data.value.routeInfo.fileName; + if (access(("routes/" + downFileName).c_str(), F_OK) == -1) + { + std::cout << "文件不存在，正在下载" << downFileName << std::endl; + bool isdown = downloadFile(json_data.value.routeInfo.url, json_data.value.routeInfo.md5); + if (!isdown) + { + std::cout << "下载失败，跳过" << std::endl; + remove(("routes/" + downFileName).c_str()); + } + else + { + std::cout << downFileName << "下载完成" << std::endl; + std::string sourceFilePath1 = ("routes/" + downFileName).c_str(); + copyFileAndOverwrite(sourceFilePath1, destinationFilePath1); + flag_ok = 1; + } + } + else + { + // 文件已存在，跳过下载 + std::cout << "文件已存在，跳过下载: routes/" << downFileName << std::endl; + std::string sourceFilePath1 = ("routes/" + downFileName).c_str(); + copyFileAndOverwrite(sourceFilePath1, destinationFilePath1); + flag_ok = 1; + } + if (flag_ok == 1) + { + // 启动任务 + cout << "开始执行任务" << json_data.value.id << endl; + task_status = 1; + } + // 回复 + sendResponse(client, mqtt_topic_sub_task, mqtt_json.seqNo, 200, "Task started successfully"); + } + else if (json_data.command == "stop") + { + // task_status = 0; + // 回复 + // sendResponse(client, mqtt_topic_sub_task, mqtt_json.seqNo, 200, "Task stopped successfully"); + } + } + catch (const boost::bad_any_cast &e) + { + std::cerr << "Bad any_cast: " << e.what() << std::endl; + } + } + } + } + + return NULL; +} + +void delivered(void *context, MQTTClient_deliveryToken dt) +{ + (void)context; + + printf("Message with token value %d delivery confirmed\n", dt); + deliveredtoken = dt; +} + +int msgarrvd(void *context, char *topicName, int topicLen, MQTTClient_message *message) +{ + (void)context; + (void)topicLen; + + printf("Message arrived\n"); + last_message_time = std::chrono::steady_clock::now(); // 更新时间戳 + printf("topic: %s\n", topicName); + printf("message: %.*s\n", message->payloadlen, (char *)message->payload); + + // 增加缓冲区大小 + const int BUFFER_SIZE = 4096; // 增大缓冲区 + char *buffer = new char[BUFFER_SIZE]; + memset(buffer, 0, BUFFER_SIZE); + + // 确保不溢出 + size_t copy_size = std::min(message->payloadlen, BUFFER_SIZE - 1); + memcpy(buffer, message->payload, copy_size); + + Json::Reader reader; + Json::Value root; + + if (!reader.parse(buffer, root)) + { + printf("recv json fail\n"); + return 1; + } + else + { + isDisconn = false; + // 创建MQTT_JSON对象 + if (root.isMember("type") && root["type"].isString() && root["type"].asString() == "request") + { + // 提取任务ID + extractTaskIdFromMessage(root); + + MQTT_JSON mqtt_json; + mqtt_json.type = root["type"].asString(); + mqtt_json.seqNo = root["seqNo"].asInt(); + Json::Value data = root["data"]; + // 创建JSON_DATA对象 + JSON_DATA json_data; + if (data.isMember("command") && data["command"].isString()) + { + json_data.command = data["command"].asString(); + if (json_data.command.compare("start") == 0) + { + try + { + json_data.value = get_task_value(data["value"]); + // 将JSON_DATA对象存入boost::any + mqtt_json.data = json_data; + cout << "即将开始任务" << json_data.value.id << endl; + // 将任务添加到队列 + std::lock_guard lock(queue_mutex); + task_queue.push(mqtt_json); + queue_cv.notify_one(); // 通知工作线程有新任务 + } + catch (const std::exception &e) + { + std::cerr << e.what() << '\n'; + } + } + else if (json_data.command.compare("stop") == 0) + { + try + { + long stop_id = data["value"].asInt(); + mqtt_json.data = stop_id; + cout << "stop_id:" << stop_id << "\t 即将停止任务" << endl; + + task_status = 0; + // 回复 + sendResponse(client, mqtt_topic_sub_task, mqtt_json.seqNo, 200, "Task stopped successfully"); + + // 将stop任务也加入队列处理 + std::lock_guard lock(queue_mutex); + task_queue.push(mqtt_json); + queue_cv.notify_one(); + } + catch (const std::exception &e) + { + std::cerr << e.what() << '\n'; + } + } + } + else + std::cerr << "Missing or invalid 'command' field" << std::endl; + } + else if (root["type"].asString() == "response") + { + } + else + { + std::cerr << "Missing or invalid 'type' field" << std::endl; + } + } + + MQTTClient_freeMessage(&message); + MQTTClient_free(topicName); + return 1; +} + +void connlost(void *context, char *cause) +{ + (void)context; + isDisconn = 1; + printf("\nConnection lost\n"); + printf("cause: %s\n", cause); +} + +void *mqtt_sub(void *arg) +{ + (void)arg; + + const char *SUB_TOPIC = sub_topic.c_str(); + + int rc; + // 修正：使用const char*而非char* + const char *username = mqtt_user.c_str(); + const char *password = mqtt_password.c_str(); + + if ((rc = MQTTClient_create(&client, ADDRESS.c_str(), CLIENTID_SUB.c_str(), MQTTCLIENT_PERSISTENCE_NONE, NULL)) != MQTTCLIENT_SUCCESS) + { + printf("Failed to create client, return code %d\n", rc); + MQTTClient_destroy(&client); + return NULL; + } + + if ((rc = MQTTClient_setCallbacks(client, NULL, connlost, msgarrvd, delivered)) != MQTTCLIENT_SUCCESS) + { + printf("Failed to set callbacks, return code %d\n", rc); + MQTTClient_destroy(&client); + return NULL; + } + + conn_opts.keepAliveInterval = 20; + conn_opts.cleansession = 1; + conn_opts.username = username; + conn_opts.password = password; + + if ((rc = MQTTClient_connect(client, &conn_opts)) != MQTTCLIENT_SUCCESS) + { + printf("Failed to connect, return code %d\n", rc); + MQTTClient_destroy(&client); + return NULL; + } + + printf("Subscribing to topic %s\nfor client %s using QoS%d\n\n", SUB_TOPIC, CLIENTID_SUB.c_str(), QOS); + if ((rc = MQTTClient_subscribe(client, SUB_TOPIC, QOS)) != MQTTCLIENT_SUCCESS) + { + printf("Failed to subscribe, return code %d\n", rc); + MQTTClient_destroy(&client); + return NULL; + } + + while (1) + { + usleep(200000); + // 检查连接状态，如果连接丢失，则尝试重新连接 + if (MQTTClient_isConnected(client) == 0) + { + printf("MQTT connection lost, trying to reconnect...\n"); + + if ((rc = MQTTClient_connect(client, &conn_opts)) != MQTTCLIENT_SUCCESS) + { + isDisconn = true; + printf("Failed to reconnect, return code %d\n", rc); + // 处理连接失败的情况，例如等待一段时间后再次尝试 + } + else + { + isDisconn = false; + printf("Reconnected to MQTT server.\n"); + // 重新订阅主题 + if ((rc = MQTTClient_subscribe(client, SUB_TOPIC, QOS)) != MQTTCLIENT_SUCCESS) + { + printf("Failed to resubscribe, return code %d\n", rc); + } + } + } + else + { + isDisconn = false; + auto current_time = std::chrono::steady_clock::now(); + if (std::chrono::duration_cast(current_time - last_message_time).count() > 500) + { + isDisconn = true; + // printf("Heartbeat timeout: No message received in 500ms.\n"); + // 执行心跳超时的处理逻辑 + } + } + } + + if ((rc = MQTTClient_unsubscribe(client, SUB_TOPIC)) != MQTTCLIENT_SUCCESS) + { + printf("Failed to unsubscribe, return code %d\n", rc); + MQTTClient_destroy(&client); + return NULL; + } + + if ((rc = MQTTClient_disconnect(client, 10000)) != MQTTCLIENT_SUCCESS) + { + printf("Failed to disconnect, return code %d\n", rc); + MQTTClient_destroy(&client); + return NULL; + } + + MQTTClient_destroy(&client); + return NULL; +} + +class task_manager_node : public rclcpp::Node +{ +public: + // 构造函数,有一个参数为节点名称 + task_manager_node(std::string name) : Node(name) + { + RCLCPP_INFO(this->get_logger(), "%s节点已经启动.", name.c_str()); + msg_publisher_ = this->create_publisher("task_message/download", 10); + task_subscribe_ = this->create_subscription("task_message/upload", 10, std::bind(&task_manager_node::task_callback, this, std::placeholders::_1)); + + timer_ = this->create_wall_timer(std::chrono::milliseconds(500), std::bind(&task_manager_node::timer_callback, this)); + } + +private: + void timer_callback() + { + // 发布任务消息 + sweeper_interfaces::msg::Task msg; + msg.task_status = task_status; + + if (task_status != last_status) + { + msg_publisher_->publish(msg); + RCLCPP_INFO(this->get_logger(), "发布任务消息"); + } + + last_status = task_status; + + // RCLCPP_INFO(this->get_logger(), "msg.task_status : %d", msg.task_status); + // RCLCPP_INFO(this->get_logger(), "task_status : %d", task_status); + // RCLCPP_INFO(this->get_logger(), "last_status : %d", last_status); + // cout << endl; + } + + void task_callback(const sweeper_interfaces::msg::Task::SharedPtr msg) + { + // 安全转换：检查枚举值是否有效 + if (msg->task_status >= TaskStatus::PENDING && msg->task_status <= TaskStatus::FAILED) + { + status_up = static_cast(msg->task_status); + } + else + { + // 处理无效状态（例如设为默认值或记录错误） + RCLCPP_WARN(this->get_logger(), "Invalid task status received: %d", msg->task_status); + status_up = TaskStatus::FAILED; + } + } + + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr msg_publisher_; + rclcpp::Subscription::SharedPtr task_subscribe_; +}; + +void init_main() +{ + Json::Reader reader; + Json::Value root; + std::ifstream in("config.json", std::ios::binary); + if (!in.is_open()) + { + std::cout << "read config file error" << std::endl; + return; + } + if (reader.parse(in, root)) + { + mqtt_vid = root["mqtt"]["vid"].asString(); + mqtt_inter_net_address = root["mqtt"]["inter_net_address"].asString(); + mqtt_inter_net_port = root["mqtt"]["inter_net_port"].asInt(); + mqtt_external_net_address = root["mqtt"]["external_net_address"].asString(); + mqtt_external_net_port = root["mqtt"]["external_net_port"].asInt(); + mqtt_user = root["mqtt"]["mqtt_user"].asString(); + mqtt_password = root["mqtt"]["mqtt_password"].asString(); + mqtt_topic_sub_task = root["mqtt"]["mqtt_topic_sub_task"].asString(); + mqtt_topic_push_status = root["mqtt"]["mqtt_topic_push_status"].asString(); + // 添加对sub_topic的初始化 + sub_topic = mqtt_topic_sub_task; + CLIENTID_SUB = CLIENTID_SUB + "_" + mqtt_vid; + // ADDRESS = mqtt_inter_net_address + ":" + std::to_string(mqtt_inter_net_port); + ADDRESS = mqtt_external_net_address + ":" + std::to_string(mqtt_external_net_port); + cout << "ADDRESS: " << ADDRESS << endl; + cout << "CLIENTID_SUB: " << CLIENTID_SUB << endl; + cout << "mqtt_vid: " << mqtt_vid << endl; + } + in.close(); // 关闭文件流 +} + +int main(int argc, char **argv) +{ + // 注册信号处理函数 + signal(SIGINT, signalHandler); + + init_main(); + pthread_t mqtt_sub_thread_t; + pthread_create(&mqtt_sub_thread_t, NULL, mqtt_sub, NULL); + + // 创建任务处理线程 + pthread_t task_thread; + pthread_create(&task_thread, NULL, process_tasks, NULL); + + pthread_t reportThread; + pthread_create(&reportThread, NULL, reportTaskStatus, NULL); + + rclcpp::init(argc, argv); + /*创建对应节点的共享指针对象*/ + auto node = std::make_shared("task_manager_node"); + + // 使用非阻塞的spin + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + + // 循环执行spin_some，允许检查信号 + while (rclcpp::ok() && !signal_received) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + // 优雅地停止线程 + { + std::lock_guard lock(queue_mutex); + thread_running = false; + } + queue_cv.notify_one(); + pthread_join(task_thread, NULL); + pthread_join(mqtt_sub_thread_t, NULL); + pthread_join(reportThread, NULL); + + return 0; +}