commit 1c212079656b3e940977f53f618bfd38e93cd48d Author: Alvin-lyq <2601685812@qq.com> Date: Fri Aug 29 16:37:57 2025 +0800 first commit diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..31d0318 --- /dev/null +++ b/.gitignore @@ -0,0 +1,4 @@ +build +install +log +.vscode diff --git a/can.sh b/can.sh new file mode 100755 index 0000000..2ecdd8a --- /dev/null +++ b/can.sh @@ -0,0 +1,33 @@ +#!/bin/bash +# Script: can.sh +# Description: Configures CAN interface can0 on startup + +# Check if running as root +if [[ $EUID -ne 0 ]]; then + echo "This script must be run as root" + exit 1 +fi + +# Disable CAN interface can0 if it's up +if ip link show can0 &> /dev/null; then + ip link set can0 down +fi + +# Load necessary kernel modules +modprobe can +modprobe can_raw +modprobe mttcan + +# Configure CAN interface can0 with bitrate 250000 +ip link set can0 type can bitrate 500000 +ip link set up can0 + +# Check the return status of the last command +if [[ $? -ne 0 ]]; then + echo "Error configuring CAN interface can0" + exit 1 +fi + +echo "CAN interface can0 configured successfully" +exit 0 + diff --git a/config.json b/config.json new file mode 100644 index 0000000..43449fd --- /dev/null +++ b/config.json @@ -0,0 +1,12 @@ +{ + "mqtt": { + "vid": "V060002", + "inter_net_address": "tcp://192.168.4.196", + "inter_net_port": 11883, + "external_net_address": "tcp://36.153.162.171", + "external_net_port": 19683, + "mqtt_user": "zxwl", + "mqtt_password": "zxwl1234@", + "pub_gps_topic": "/zxwl/vehicle/V060002/gps" + } +} \ No newline at end of file diff --git a/prepare.sh b/prepare.sh new file mode 100755 index 0000000..50fa184 --- /dev/null +++ b/prepare.sh @@ -0,0 +1,6 @@ +#!/bin/bash + +sudo ./can.sh + +sudo chmod 777 /dev/ttyUSB0 +sudo chmod 777 /dev/ttyTHS1 diff --git a/ros2_nodes.sh b/ros2_nodes.sh new file mode 100755 index 0000000..663a705 --- /dev/null +++ b/ros2_nodes.sh @@ -0,0 +1,200 @@ +#!/bin/bash +############################################################################### +# ros2_nodes.sh +# 一键启动 / 停止 / 重启 / 状态查询 13 个 ROS 2 节点（或仅核心 4 节点） +# 用法： +# ./ros2_nodes.sh start # 默认启动全部 13 节点 +# ./ros2_nodes.sh start all # 同上 +# ./ros2_nodes.sh start core # 仅启动 4 个核心节点 +# ./ros2_nodes.sh stop # 停止 +# ./ros2_nodes.sh restart [core] # 重启（可选 core / all） +# ./ros2_nodes.sh status # 查看状态 +# ./ros2_nodes.sh rotate # 手动轮换日志 +############################################################################### + +##== 基本路径与环境 =========================================================== +WORKSPACE_PATH="/home/nvidia/sweeper_e800" +source "$WORKSPACE_PATH/install/setup.bash" + +##== 日志目录与轮换配置 ======================================================= +LOG_DIR="/var/log/ros2" +MAX_LOG_SIZE="10M" # 单个日志大小 +MAX_LOG_FILES=5 # 保留文件数 + +##== 节点命令列表 ============================================================== + +# 1) 全量 13 节点（键名 = 节点标识 , 值 = 启动命令） +declare -A NODES_ALL=( + # ---- 核心 4 节点 ---- + # ["mc"]="ros2 run mc mc_node" + ["radio_ctrl"]="ros2 run radio_ctrl radio_ctrl_node" + ["ctrl_arbiter"]="ros2 run ctrl_arbiter ctrl_arbiter_node" + ["mqtt_report"]="ros2 run mqtt_report mqtt_report_node" + + # ---- 新增 9 节点 ---- + ["rslidar_sdk"]="ros2 launch rslidar_sdk start.py" + ["rtk"]="ros2 run rtk rtk_node" + ["route"]="ros2 run route route_node" + ["sub"]="ros2 run sub sub_node" + ["task_manager"]="ros2 run task_manager task_manager_node" + ["detect_line"]="ros2 run detect_line detect_line_node" + ["pl"]="ros2 run pl pl_node" + ["fu"]="ros2 run fu fu_node" + ["pub_gps"]="ros2 run pub_gps pub_gps_node" +) + +# 2) 核心节点名称数组（便于 core 模式筛选） +CORE_NAMES=(mc radio_ctrl ctrl_arbiter mqtt_report) + +##== 函数 ===================================================================== + +# 预启动环境准备：CAN、USB 权限、日志目录与 logrotate +setup_environment() { + echo "[INFO] 设置 CAN0 波特率为 500K..." + sudo ip link set can0 down 2>/dev/null + sudo ip link set can0 type can bitrate 500000 + sudo ip link set can0 up + echo "[INFO] CAN0 配置完成。" + + echo "[INFO] 设置 /dev/ttyUSB0 权限为 777..." + sudo chmod 777 /dev/ttyUSB0 + echo "[INFO] ttyUSB0 权限设置完成。" + + echo "[INFO] 设置 /dev/ttyTHS1 权限为 777..." + sudo chmod 777 /dev/ttyTHS1 + echo "[INFO] ttyTHS1 权限设置完成。" + + # 日志目录 + echo "[INFO] 创建日志目录: $LOG_DIR" + sudo mkdir -p "$LOG_DIR" + sudo chown -R nvidia:nvidia "$LOG_DIR" + + # logrotate 配置 + local LOGROTATE_CONF="/etc/logrotate.d/ros2_nodes" + echo "[INFO] 配置 logrotate: $LOGROTATE_CONF" + sudo bash -c "cat > $LOGROTATE_CONF" <> "$log_file" 2>&1 & + # gnome-terminal --title="$name" \ + # -- bash -c "source $WORKSPACE_PATH/install/setup.bash && ${NODES_ALL[$name]}; exec bash" & + #===================================================================== + + local pid=$! + echo $pid > "/tmp/$name.pid" + echo "[INFO] $name PID: $pid | 日志: $log_file" + + sleep 0.1 # 每个节点之间暂停 0.1 秒 + done + + echo "[INFO] 所有节点已启动 (${mode})." +} + +# 停止全部正在运行的节点 +stop_nodes() { + echo "[INFO] 停止 ROS 2 节点..." + for pidfile in /tmp/*.pid; do + [ -e "$pidfile" ] || continue + local name=$(basename "$pidfile" .pid) + local pid=$(cat "$pidfile") + if ps -p "$pid" > /dev/null 2>&1; then + kill -SIGINT "$pid" && echo "[STOP] $name (PID $pid) - 发送 SIGINT" + local timeout=5 + while kill -0 "$pid" 2>/dev/null && [ $timeout -gt 0 ]; do + sleep 0.5; ((timeout--)) + done + if kill -0 "$pid" 2>/dev/null; then + kill -9 "$pid" && echo "[STOP] $name (PID $pid) - 强制终止" + fi + fi + rm -f "$pidfile" + done + echo "[INFO] 所有节点已停止。" +} + +# 手动触发日志轮换 +rotate_logs() { + echo "[INFO] 手动轮换日志..." + sudo logrotate -f /etc/logrotate.d/ros2_nodes + echo "[INFO] 日志轮换完成。" +} + +# 查看节点运行状态 +show_status() { + echo "[INFO] 节点状态:" + for name in "${!NODES_ALL[@]}"; do + if [ -f "/tmp/$name.pid" ]; then + local pid=$(cat "/tmp/$name.pid") + if ps -p "$pid" > /dev/null 2>&1; then + echo "[RUNNING] $name (PID $pid)" + else + echo "[DEAD] $name (PID $pid)" + fi + else + echo "[STOPPED] $name" + fi + done +} + +##== 主入口 =================================================================== + +case "$1" in + start) + # 用法: ./ros2_nodes.sh start [core|all] + start_nodes "${2:-all}" + ;; + stop) + stop_nodes + ;; + restart) + stop_nodes + sleep 1 + start_nodes "${2:-all}" + ;; + rotate) + rotate_logs + ;; + status) + show_status + ;; + *) + echo "Usage: $0 {start|stop|restart|rotate|status} [core|all]" + echo " core : 仅启动 4 个核心节点 (mc radio_ctrl ctrl_arbiter mqtt_report)" + echo " all : 启动全部 13 个节点 (默认)" + ;; +esac diff --git a/run.sh b/run.sh new file mode 100755 index 0000000..78a89cc --- /dev/null +++ b/run.sh @@ -0,0 +1,37 @@ +#!/bin/bash + +source install/setup.bash # ros workspace +{ + gnome-terminal --title="radio" --tab "XXD_ros" -- bash -c "ros2 run radio_ctrl radio_ctrl_node" +}& +sleep 0.2s + +{ + gnome-terminal --title="mc" --tab "XXD_ros" -- bash -c "ros2 run mc mc_node" +} & +sleep 0.2s + +{ + gnome-terminal --title="ctrl_arbiter" --tab "XXD_ros" -- bash -c "ros2 run ctrl_arbiter ctrl_arbiter_node" +} & +sleep 0.2s + +{ + gnome-terminal --title="report" --tab "XXD_ros" -- bash -c "ros2 run mqtt_report mqtt_report_node" +} +sleep 0.2s + +{ + # gnome-terminal --title="rslidar_sdk" --tab "XXD_ros" -- bash -c "ros2 launch rslidar_sdk start.py" + gnome-terminal --title="lidar" --tab "XXD_ros" -- bash -c "ros2 launch rslidar_sdk start_double.launch.py" +}& +sleep 0.2s + +{ + gnome-terminal --title="rtk" --tab "XXD_ros" -- bash -c "ros2 run rtk rtk_node" +}& +sleep 0.2s + +{ + gnome-terminal --title="pub_gps" --tab "XXD_ros" -- bash -c "ros2 run pub_gps pub_gps_node" +} 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..9d38c5f --- /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/ubuntu/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/ubuntu/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/ctrl_arbiter/CMakeLists.txt b/src/ctrl_arbiter/CMakeLists.txt new file mode 100644 index 0000000..64faf2e --- /dev/null +++ b/src/ctrl_arbiter/CMakeLists.txt @@ -0,0 +1,41 @@ +cmake_minimum_required(VERSION 3.8) +project(ctrl_arbiter) + +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(sweeper_interfaces REQUIRED) +find_package(std_msgs REQUIRED) + +file(GLOB SRC_FILES "src/*.cpp") + +add_executable(ctrl_arbiter_node ${SRC_FILES}) + +ament_target_dependencies(ctrl_arbiter_node + rclcpp + sweeper_interfaces + std_msgs +) + +install(TARGETS + ctrl_arbiter_node + DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/ctrl_arbiter/package.xml b/src/ctrl_arbiter/package.xml new file mode 100644 index 0000000..725c7a8 --- /dev/null +++ b/src/ctrl_arbiter/package.xml @@ -0,0 +1,21 @@ + + + + ctrl_arbiter + 0.0.0 + TODO: Package description + root + MIT + + ament_cmake + + sweeper_interfaces + rclcpp + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/ctrl_arbiter/src/ctrl_arbiter.cpp b/src/ctrl_arbiter/src/ctrl_arbiter.cpp new file mode 100644 index 0000000..8d20122 --- /dev/null +++ b/src/ctrl_arbiter/src/ctrl_arbiter.cpp @@ -0,0 +1,120 @@ +#include "rclcpp/rclcpp.hpp" +#include "sweeper_interfaces/msg/mc_ctrl.hpp" +#include +#include + +using namespace std::chrono_literals; +namespace sweeperMsg = sweeper_interfaces::msg; + +class ArbitrationNode : public rclcpp::Node +{ +public: + ArbitrationNode() + : Node("control_arbitrator") + { + timeout_ms_ = 200; // 200毫秒超时阈值，可调整 + + publisher_ = this->create_publisher("/mc_ctrl", 10); + + sub_radio_ = this->create_subscription( + "/radio_mc_ctrl", 10, + [this](const sweeperMsg::McCtrl::SharedPtr msg) + { + std::lock_guard lock(mutex_); + radio_msg_ = *msg; + radio_last_time_ = this->now(); + radio_valid_ = true; + }); + + sub_remote_ = this->create_subscription( + "/remote_mc_ctrl", 10, + [this](const sweeperMsg::McCtrl::SharedPtr msg) + { + std::lock_guard lock(mutex_); + remote_msg_ = *msg; + remote_last_time_ = this->now(); + remote_valid_ = true; + }); + + sub_auto_ = this->create_subscription( + "/auto_mc_ctrl", 10, + [this](const sweeperMsg::McCtrl::SharedPtr msg) + { + std::lock_guard lock(mutex_); + auto_msg_ = *msg; + auto_last_time_ = this->now(); + auto_valid_ = true; + }); + + timer_ = this->create_wall_timer(20ms, [this]() + { this->arbitrateAndPublish(); }); + RCLCPP_INFO(this->get_logger(), "ArbitrationNode started, waiting for control sources..."); + } + +private: + void arbitrateAndPublish() + { + std::lock_guard lock(mutex_); + rclcpp::Time now = this->now(); + + // 检查每个控制源是否在线 + if (radio_valid_ && (now - radio_last_time_).nanoseconds() < timeout_ms_ * 1000000) + { + publisher_->publish(radio_msg_); + RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 2000, "[ARBITER] Using RADIO control"); + return; + } + if (auto_valid_ && (now - auto_last_time_).nanoseconds() < timeout_ms_ * 1000000) + { + publisher_->publish(auto_msg_); + RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 2000, "[ARBITER] Using AUTO control"); + return; + } + if (remote_valid_ && (now - remote_last_time_).nanoseconds() < timeout_ms_ * 1000000) + { + publisher_->publish(remote_msg_); + RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 2000, "[ARBITER] Using REMOTE control"); + return; + } + + // 所有控制源均失联，发布安全默认指令 + sweeperMsg::McCtrl safe_msg; + safe_msg.brake = 1; + safe_msg.gear = 0; + safe_msg.rpm = 0; + safe_msg.angle = 0; + safe_msg.angle_speed = 120; + safe_msg.edge_brush_lift = false; + safe_msg.sweep_ctrl = false; + safe_msg.spray = false; + safe_msg.mud_flap = false; + safe_msg.dust_shake = false; + + publisher_->publish(safe_msg); + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, + "[ARBITER] All sources timeout, publishing FAILSAFE control"); + } + + rclcpp::Publisher::SharedPtr publisher_; + + rclcpp::Subscription::SharedPtr sub_radio_, sub_remote_, sub_auto_; + rclcpp::TimerBase::SharedPtr timer_; + + sweeperMsg::McCtrl radio_msg_, remote_msg_, auto_msg_; + rclcpp::Time radio_last_time_, remote_last_time_, auto_last_time_; + bool radio_valid_ = false; + bool remote_valid_ = false; + bool auto_valid_ = false; + + std::mutex mutex_; + int timeout_ms_; +}; + +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/mc/CMakeLists.txt b/src/mc/CMakeLists.txt new file mode 100644 index 0000000..ed4d78f --- /dev/null +++ b/src/mc/CMakeLists.txt @@ -0,0 +1,42 @@ +cmake_minimum_required(VERSION 3.8) +project(mc) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# Find dependencies +find_package(sweeper_interfaces REQUIRED) +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) + +# 搜索 src 目录下所有 .cpp 文件 +file(GLOB SRC_FILES src/*.cpp) + +# 创建可执行文件 +add_executable(mc_node ${SRC_FILES}) + +ament_target_dependencies(mc_node rclcpp std_msgs sweeper_interfaces) + +# Set include directories for the target +target_include_directories( + mc_node + PUBLIC $ + $) + +target_compile_features(mc_node PUBLIC c_std_99 cxx_std_17) + +# Install target +install(TARGETS mc_node DESTINATION lib/${PROJECT_NAME}) +install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) + +# Handle testing if needed +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/mc/config/config.json b/src/mc/config/config.json new file mode 100644 index 0000000..0f5a1b7 --- /dev/null +++ b/src/mc/config/config.json @@ -0,0 +1,3 @@ +{ + "can_dev": "can0" +} diff --git a/src/mc/include/mc/can_driver.h b/src/mc/include/mc/can_driver.h new file mode 100644 index 0000000..db7af0e --- /dev/null +++ b/src/mc/include/mc/can_driver.h @@ -0,0 +1,66 @@ +#ifndef CAN_DRIVER_H +#define CAN_DRIVER_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +struct CANFrame +{ + uint32_t id; + uint8_t data[8]; + uint8_t dlc; + bool ext; // 扩展帧 + bool rtr; // 远程帧 +}; + +class CANDriver +{ +public: + using ReceiveCallback = std::function; + + CANDriver(); + ~CANDriver(); + + // 打开CAN接口 + bool open(const std::string &interface); + + // 关闭CAN接口 + void close(); + + // 发送CAN帧 + bool sendFrame(const CANFrame &frame); + + // 设置接收回调 + void setReceiveCallback(ReceiveCallback callback, void *userData = nullptr); + + // 设置硬件过滤规则 + bool setFilter(const std::vector &filters); + + // 追加一个过滤器 + bool addFilter(const can_filter &filter); + + // 追加一组过滤器 + bool addFilters(const std::vector &filters); + +private: + void receiveThreadFunc(); + bool applyFilters(); // 应用当前filters_ + + int sockfd = -1; + std::atomic running{false}; + std::thread receiveThread; + ReceiveCallback callback; + void *userData = nullptr; + std::vector filters_; // 当前所有过滤器 +}; + +#endif // CAN_DRIVER_H diff --git a/src/mc/include/mc/can_struct.h b/src/mc/include/mc/can_struct.h new file mode 100644 index 0000000..3677f9d --- /dev/null +++ b/src/mc/include/mc/can_struct.h @@ -0,0 +1,390 @@ +#ifndef CAN_STRUCT_H +#define CAN_STRUCT_H + +#include +#include +#include +#include "can_driver.h" + +#pragma pack(push, 1) // 禁止结构体补齐，确保8字节紧密排列 + +union can_MCU_cmd_union +{ + struct + { + // Byte 0-1 + uint8_t reserve1; + uint8_t reserve2; + + // Byte 2 + uint8_t rpm; + + // Byte 3 + uint8_t gear; + + // Byte 4 + uint8_t work_enabled; + + // Byte 5 + uint8_t brake; + + // Byte 6 + uint8_t reserve3; + + // Byte 7 + uint8_t ctrl_enabled; + } fields; + + uint8_t bytes[8]; +}; + +union can_EPS_cmd_union +{ + struct + { + uint8_t control_mode; // 控制方式 + uint8_t reserve1; // 保留 + uint8_t reserve2; // 保留 + uint8_t angle_h; // 角度高位 + uint8_t angle_l; // 角度低位 + uint8_t center_cmd; // 角度对中指令 + uint8_t angular_speed; // 角速度控制 + uint8_t checksum; // 异或校验 + } fields; + + uint8_t bytes[8]; +}; + +#pragma pack(pop) + +union can_DBS_cmd_union +{ + struct __attribute__((packed)) + { + uint8_t valid; // Byte 0: VCU_DBS_Valid + uint8_t work_mode; // Byte 1: VCU_DBS_Work_Mode + uint8_t pressure; // Byte 2: VCU_DBS_HP_Pressure（0.1MPa） + uint8_t abs_active; // Byte 3: ABS_Active + uint8_t reserved[4]; // Byte 4~7: 保留 + } fields; + uint8_t bytes[8]; // 原始字节流 +}; + +struct can_MCU_cmd +{ + can_MCU_cmd_union data; + + static constexpr uint32_t CMD_ID = 0x0CFF17EF; + static constexpr bool EXT_FLAG = true; + static constexpr bool RTR_FLAG = false; + + // 构造函数自动初始化常量 + can_MCU_cmd() + { + memset(&data, 0, sizeof(data)); + data.fields.ctrl_enabled = 1; + data.fields.reserve1 = 0; + data.fields.reserve2 = 0; + data.fields.reserve3 = 0; + } + + void setEnabled(bool en) + { + data.fields.work_enabled = en ? 1 : 0; + } + + void setGear(uint8_t gear) + { + data.fields.gear = gear & 0x01; + } + + void setBrake(uint8_t brake) + { + data.fields.brake = brake & 0x01; + } + + void setRPM(uint8_t rpm) + { + data.fields.rpm = std::clamp(rpm, static_cast(0), static_cast(100)); + } + + CANFrame toFrame() const + { + CANFrame frame; + frame.id = CMD_ID; + memcpy(frame.data, data.bytes, 8); + frame.dlc = 8; + frame.ext = EXT_FLAG; + frame.rtr = RTR_FLAG; + return frame; + } +}; + +struct can_EPS_cmd +{ + can_EPS_cmd_union data; + + static constexpr uint32_t CMD_ID = 0x469; + static constexpr bool EXT_FLAG = false; + static constexpr bool RTR_FLAG = false; + + can_EPS_cmd() + { + memset(&data, 0, sizeof(data)); + data.fields.control_mode = 0x20; + } + + void setAngle(float angle) + { + // angle公式：receive[3]*256+receive[4]-1024 + int16_t raw = static_cast(angle * 7.0f + 1024.0f); + data.fields.angle_h = (raw >> 8) & 0xFF; + data.fields.angle_l = raw & 0xFF; + } + + void setCenterCmd(uint8_t cmd) + { + data.fields.center_cmd = cmd; + } + + void setAngularSpeed(uint16_t angle_speed) + { + uint8_t value = angle_speed / 6; + if (value < 20) + value = 20; + else if (value > 250) + value = 250; + data.fields.angular_speed = static_cast(value); + } + + void pack() + { + // 计算校验：Byte0 ~ Byte6的异或 + uint8_t xor_sum = 0; + for (int i = 0; i < 7; ++i) + { + xor_sum ^= data.bytes[i]; + } + data.fields.checksum = xor_sum; + } + + CANFrame toFrame() const + { + CANFrame frame; + frame.id = CMD_ID; + memcpy(frame.data, data.bytes, 8); + frame.dlc = 8; + frame.ext = EXT_FLAG; + frame.rtr = RTR_FLAG; + return frame; + } +}; + +struct can_VCU_out1_cmd +{ + static constexpr uint32_t CMD_ID = 0x18f21117; + static constexpr bool EXT_FLAG = true; + static constexpr bool RTR_FLAG = false; + + union Byte0 + { + struct + { + uint8_t reserved1 : 3; // 保留位 + uint8_t dust_shake : 1; // 振尘 + uint8_t reserved2 : 4; // 保留位 + } bits; + uint8_t raw = 0; + }; + + union Byte3 + { + struct + { + uint8_t headlight : 4; // 大灯 + uint8_t reserved : 3; // 保留位 + uint8_t leftLight : 1; // 左转灯 + } bits; + uint8_t raw = 0; + }; + + uint8_t data[8]{}; + + Byte0 &byte0() { return *reinterpret_cast(&data[0]); } + Byte3 &byte3() { return *reinterpret_cast(&data[3]); } + + void setDustShake(bool on) + { + byte0().bits.dust_shake = on ? 1 : 0; + } + + void setLeftLight(bool on) + { + byte3().bits.leftLight = on ? 1 : 0; + } + + void setHeadLight(bool on) + { + byte3().bits.headlight = on ? 0x64 : 0x00; + } + + CANFrame toFrame() const + { + CANFrame frame; + frame.id = CMD_ID; + std::copy(std::begin(data), std::end(data), frame.data); + frame.dlc = 8; + frame.ext = EXT_FLAG; + frame.rtr = RTR_FLAG; + return frame; + } +}; + +struct can_VCU_out2_cmd +{ + static constexpr uint32_t CMD_ID = 0x18fa1117; + static constexpr bool EXT_FLAG = true; + static constexpr bool RTR_FLAG = false; + + union Byte0 + { + struct + { + uint8_t right_light : 1; // 右转灯 + uint8_t brake_light : 1; // 刹车灯 + uint8_t reserved0 : 6; // 保留位 + } bits; + uint8_t raw = 0; + }; + + union Byte1 + { + struct + { + uint8_t reserved1 : 5; // 保留位 + uint8_t sweep_ctrl : 1; // 扫地控制 + uint8_t reserved2 : 1; // 保留位 + uint8_t headlight : 1; // 大灯 + } bits; + uint8_t raw = 0; + }; + + union Byte3 + { + struct + { + uint8_t edge_brush_lift : 1; // 边刷推杆 + uint8_t reserved3 : 1; // 保留位 + uint8_t spray : 1; // 喷水 + uint8_t reserved4 : 3; // 保留位 + uint8_t mud_flap : 1; // 挡皮 + uint8_t reserved5 : 1; // 保留位 + } bits; + uint8_t raw = 0; + }; + + uint8_t data[8]{}; + + Byte0 &byte0() { return *reinterpret_cast(&data[0]); } + Byte1 &byte1() { return *reinterpret_cast(&data[1]); } + Byte3 &byte3() { return *reinterpret_cast(&data[3]); } + + // === Byte0 控制 === + void setRightLight(bool on) + { + byte0().bits.right_light = on ? 1 : 0; + } + + void setBrakeLight(bool on) + { + byte0().bits.brake_light = on ? 1 : 0; + } + + // === Byte1 控制 === + void setSweeepCtrl(bool on) + { + byte1().bits.sweep_ctrl = on ? 1 : 0; + } + + void setHeadlight(bool on) + { + byte1().bits.headlight = on ? 1 : 0; + } + + // === Byte3 控制 === + void setEdgeBrushLift(bool on) + { + byte3().bits.edge_brush_lift = on ? 1 : 0; + } + + void setSpray(bool on) + { + byte3().bits.spray = on ? 1 : 0; + } + + void setMudFlap(bool on) + { + byte3().bits.mud_flap = on ? 1 : 0; + } + + CANFrame toFrame() const + { + CANFrame frame; + frame.id = CMD_ID; + std::copy(std::begin(data), std::end(data), frame.data); + frame.dlc = 8; + frame.ext = EXT_FLAG; + frame.rtr = RTR_FLAG; + return frame; + } +}; + +struct can_DBS_cmd +{ + can_DBS_cmd_union data; + + static constexpr uint32_t CMD_ID = 0x154; + static constexpr bool EXT_FLAG = false; + static constexpr bool RTR_FLAG = false; + + // 构造函数初始化固定字段 + can_DBS_cmd() + { + memset(&data, 0, sizeof(data)); + data.fields.work_mode = 0; + data.fields.abs_active = 1; + } + + void setEnabled(bool en) + { + data.fields.work_mode = en ? 1 : 0; + } + + // 设置压力（单位 MPa，0~8.0，有效范围 0~80） + can_DBS_cmd &setPressure(float mpa) + { + uint8_t val = static_cast(std::clamp(mpa * 10.0f, 0.0f, 80.0f)); + data.fields.pressure = val; + return *this; + } + + // 转换为 CAN 帧 + CANFrame toFrame() const + { + CANFrame frame; + frame.id = CMD_ID; + memcpy(frame.data, data.bytes, 8); + frame.dlc = 8; + frame.ext = EXT_FLAG; + frame.rtr = RTR_FLAG; + return frame; + } +}; + +extern can_MCU_cmd mcu_cmd; +extern can_EPS_cmd eps_cmd; +extern can_VCU_out1_cmd vcu1_cmd; +extern can_VCU_out2_cmd vcu2_cmd; +extern can_DBS_cmd dbs_cmd; + +#endif diff --git a/src/mc/include/mc/can_utils.hpp b/src/mc/include/mc/can_utils.hpp new file mode 100644 index 0000000..4d5f106 --- /dev/null +++ b/src/mc/include/mc/can_utils.hpp @@ -0,0 +1,14 @@ +#pragma once +#include "rclcpp/rclcpp.hpp" +#include "sweeper_interfaces/msg/can_frame.hpp" +#include "mc/can_struct.h" + +extern bool g_can_print_enable; + +struct CanHandlerContext +{ + rclcpp::Node::SharedPtr node; + std::shared_ptr> publisher; +}; + +void receiveHandler(const CANFrame &frame, void *userData); diff --git a/src/mc/include/mc/control_cache.hpp b/src/mc/include/mc/control_cache.hpp new file mode 100644 index 0000000..436c39c --- /dev/null +++ b/src/mc/include/mc/control_cache.hpp @@ -0,0 +1,20 @@ +#pragma once +#include "sweeper_interfaces/msg/mc_ctrl.hpp" +#include +#include + +class ControlCache +{ +public: + void update(const sweeper_interfaces::msg::McCtrl &msg); + bool get(sweeper_interfaces::msg::McCtrl &msg); + +private: + std::mutex mutex_; + sweeper_interfaces::msg::McCtrl latest_msg_; + std::chrono::steady_clock::time_point last_update_time_; + bool has_data_ = false; +}; + +extern ControlCache control_cache; +sweeper_interfaces::msg::McCtrl get_safe_control(); // 获取带超时判断的控制指令 diff --git a/src/mc/include/mc/dumper.h b/src/mc/include/mc/dumper.h new file mode 100644 index 0000000..6ec3022 --- /dev/null +++ b/src/mc/include/mc/dumper.h @@ -0,0 +1,63 @@ +// 专门用于垃圾倾倒的头文件 +#ifndef DUMPER_H +#define DUMPER_H + +#include "can_driver.h" +#include +#include +#include + +enum class DumperState +{ + IDLE, + RAISING, // 斗升中 + DUMPING, // 放斗中 + RETRACTING, // 收斗中 + LOWERING // 斗降中 +}; + +enum class DumperAction +{ + RAISE, + DUMP, + RETRACT, + LOWER +}; + +class DumperController +{ +public: + DumperController(CANDriver &driver); + + void onStartup(); // 上电复位（只在内部用） + void tryStartup(); // 唤醒后自动执行复位（防重） + void updateDial(int new_dial, int current_state); // 拨钮变化检测与处理 + void resetAwake(); // 清除唤醒状态（VCU 掉线） + void setSweeping(bool sweeping) { sweeping_ = sweeping; } // 外部设置扫地状态， + int getSimpleState() const; + +private: + void handleDialChange(int from, int to); + void runActionSequence(const std::vector &actions); + void doAction(DumperAction act); + void sendControlOff(); + void logState() const; + + bool raised_ = true; + bool dumped_ = true; + bool in_startup_reset_ = true; + + bool awakened_ = false; + int last_dial_target_ = -1; + + std::mutex queue_mutex_; + std::queue action_queue_; + std::atomic busy_; + std::atomic has_pending_task_; + std::atomic sweeping_ = false; // 扫地任务标志 + DumperState current_state_; + CANDriver &can_; + rclcpp::Logger logger_; +}; + +#endif \ No newline at end of file diff --git a/src/mc/include/mc/get_config.h b/src/mc/include/mc/get_config.h new file mode 100644 index 0000000..7cb3eec --- /dev/null +++ b/src/mc/include/mc/get_config.h @@ -0,0 +1,21 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include "ament_index_cpp/get_package_share_directory.hpp" +#include "nlohmann/json.hpp" + +using json = nlohmann::json; +using ordered_json = nlohmann::ordered_json; + +struct Config +{ + std::string can_dev; +}; + +bool load_config(Config &config); \ No newline at end of file diff --git a/src/mc/include/mc/timer_tasks.hpp b/src/mc/include/mc/timer_tasks.hpp new file mode 100644 index 0000000..c37327f --- /dev/null +++ b/src/mc/include/mc/timer_tasks.hpp @@ -0,0 +1,7 @@ +#pragma once +#include "rclcpp/rclcpp.hpp" +#include "mc/can_driver.h" +#include "mc/dumper.h" + +// 注册所有定时器任务 +void setupTimers(rclcpp::Node::SharedPtr node, DumperController &dumper, CANDriver &canctl); diff --git a/src/mc/include/mc/vcu_wakeup.hpp b/src/mc/include/mc/vcu_wakeup.hpp new file mode 100644 index 0000000..d5a5f12 --- /dev/null +++ b/src/mc/include/mc/vcu_wakeup.hpp @@ -0,0 +1,4 @@ +#pragma once +#include "mc/can_struct.h" + +void VCUWakeUp(CANDriver &driver); // 发送唤醒帧 diff --git a/src/mc/include/nlohmann/adl_serializer.hpp b/src/mc/include/nlohmann/adl_serializer.hpp new file mode 100755 index 0000000..5df1af3 --- /dev/null +++ b/src/mc/include/nlohmann/adl_serializer.hpp @@ -0,0 +1,55 @@ +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.12.0 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2013 - 2025 Niels Lohmann +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +#include +#include +#include +#include + +NLOHMANN_JSON_NAMESPACE_BEGIN + +/// @sa https://json.nlohmann.me/api/adl_serializer/ +template +struct adl_serializer +{ + /// @brief convert a JSON value to any value type + /// @sa https://json.nlohmann.me/api/adl_serializer/from_json/ + template + static auto from_json(BasicJsonType && j, TargetType& val) noexcept( + noexcept(::nlohmann::from_json(std::forward(j), val))) + -> decltype(::nlohmann::from_json(std::forward(j), val), void()) + { + ::nlohmann::from_json(std::forward(j), val); + } + + /// @brief convert a JSON value to any value type + /// @sa https://json.nlohmann.me/api/adl_serializer/from_json/ + template + static auto from_json(BasicJsonType && j) noexcept( + noexcept(::nlohmann::from_json(std::forward(j), detail::identity_tag {}))) + -> decltype(::nlohmann::from_json(std::forward(j), detail::identity_tag {})) + { + return ::nlohmann::from_json(std::forward(j), detail::identity_tag {}); + } + + /// @brief convert any value type to a JSON value + /// @sa https://json.nlohmann.me/api/adl_serializer/to_json/ + template + static auto to_json(BasicJsonType& j, TargetType && val) noexcept( + noexcept(::nlohmann::to_json(j, std::forward(val)))) + -> decltype(::nlohmann::to_json(j, std::forward(val)), void()) + { + ::nlohmann::to_json(j, std::forward(val)); + } +}; + +NLOHMANN_JSON_NAMESPACE_END diff --git a/src/mc/include/nlohmann/byte_container_with_subtype.hpp b/src/mc/include/nlohmann/byte_container_with_subtype.hpp new file mode 100755 index 0000000..d6398e6 --- /dev/null +++ b/src/mc/include/nlohmann/byte_container_with_subtype.hpp @@ -0,0 +1,103 @@ +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.12.0 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2013 - 2025 Niels Lohmann +// SPDX-License-Identifier: MIT + +#pragma once + +#include // uint8_t, uint64_t +#include // tie +#include // move + +#include + +NLOHMANN_JSON_NAMESPACE_BEGIN + +/// @brief an internal type for a backed binary type +/// @sa https://json.nlohmann.me/api/byte_container_with_subtype/ +template +class byte_container_with_subtype : public BinaryType +{ + public: + using container_type = BinaryType; + using subtype_type = std::uint64_t; + + /// @sa https://json.nlohmann.me/api/byte_container_with_subtype/byte_container_with_subtype/ + byte_container_with_subtype() noexcept(noexcept(container_type())) + : container_type() + {} + + /// @sa https://json.nlohmann.me/api/byte_container_with_subtype/byte_container_with_subtype/ + byte_container_with_subtype(const container_type& b) noexcept(noexcept(container_type(b))) + : container_type(b) + {} + + /// @sa https://json.nlohmann.me/api/byte_container_with_subtype/byte_container_with_subtype/ + byte_container_with_subtype(container_type&& b) noexcept(noexcept(container_type(std::move(b)))) + : container_type(std::move(b)) + {} + + /// @sa https://json.nlohmann.me/api/byte_container_with_subtype/byte_container_with_subtype/ + byte_container_with_subtype(const container_type& b, subtype_type subtype_) noexcept(noexcept(container_type(b))) + : container_type(b) + , m_subtype(subtype_) + , m_has_subtype(true) + {} + + /// @sa https://json.nlohmann.me/api/byte_container_with_subtype/byte_container_with_subtype/ + byte_container_with_subtype(container_type&& b, subtype_type subtype_) noexcept(noexcept(container_type(std::move(b)))) + : container_type(std::move(b)) + , m_subtype(subtype_) + , m_has_subtype(true) + {} + + bool operator==(const byte_container_with_subtype& rhs) const + { + return std::tie(static_cast(*this), m_subtype, m_has_subtype) == + std::tie(static_cast(rhs), rhs.m_subtype, rhs.m_has_subtype); + } + + bool operator!=(const byte_container_with_subtype& rhs) const + { + return !(rhs == *this); + } + + /// @brief sets the binary subtype + /// @sa https://json.nlohmann.me/api/byte_container_with_subtype/set_subtype/ + void set_subtype(subtype_type subtype_) noexcept + { + m_subtype = subtype_; + m_has_subtype = true; + } + + /// @brief return the binary subtype + /// @sa https://json.nlohmann.me/api/byte_container_with_subtype/subtype/ + constexpr subtype_type subtype() const noexcept + { + return m_has_subtype ? m_subtype : static_cast(-1); + } + + /// @brief return whether the value has a subtype + /// @sa https://json.nlohmann.me/api/byte_container_with_subtype/has_subtype/ + constexpr bool has_subtype() const noexcept + { + return m_has_subtype; + } + + /// @brief clears the binary subtype + /// @sa https://json.nlohmann.me/api/byte_container_with_subtype/clear_subtype/ + void clear_subtype() noexcept + { + m_subtype = 0; + m_has_subtype = false; + } + + private: + subtype_type m_subtype = 0; + bool m_has_subtype = false; +}; + +NLOHMANN_JSON_NAMESPACE_END diff --git a/src/mc/include/nlohmann/detail/abi_macros.hpp b/src/mc/include/nlohmann/detail/abi_macros.hpp new file mode 100755 index 0000000..76cf336 --- /dev/null +++ b/src/mc/include/nlohmann/detail/abi_macros.hpp @@ -0,0 +1,111 @@ +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.12.0 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2013 - 2025 Niels Lohmann +// SPDX-License-Identifier: MIT + +#pragma once + +// This file contains all macro definitions affecting or depending on the ABI + +#ifndef JSON_SKIP_LIBRARY_VERSION_CHECK + #if defined(NLOHMANN_JSON_VERSION_MAJOR) && defined(NLOHMANN_JSON_VERSION_MINOR) && defined(NLOHMANN_JSON_VERSION_PATCH) + #if NLOHMANN_JSON_VERSION_MAJOR != 3 || NLOHMANN_JSON_VERSION_MINOR != 12 || NLOHMANN_JSON_VERSION_PATCH != 0 + #warning "Already included a different version of the library!" + #endif + #endif +#endif + +#define NLOHMANN_JSON_VERSION_MAJOR 3 // NOLINT(modernize-macro-to-enum) +#define NLOHMANN_JSON_VERSION_MINOR 12 // NOLINT(modernize-macro-to-enum) +#define NLOHMANN_JSON_VERSION_PATCH 0 // NOLINT(modernize-macro-to-enum) + +#ifndef JSON_DIAGNOSTICS + #define JSON_DIAGNOSTICS 0 +#endif + +#ifndef JSON_DIAGNOSTIC_POSITIONS + #define JSON_DIAGNOSTIC_POSITIONS 0 +#endif + +#ifndef JSON_USE_LEGACY_DISCARDED_VALUE_COMPARISON + #define JSON_USE_LEGACY_DISCARDED_VALUE_COMPARISON 0 +#endif + +#if JSON_DIAGNOSTICS + #define NLOHMANN_JSON_ABI_TAG_DIAGNOSTICS _diag +#else + #define NLOHMANN_JSON_ABI_TAG_DIAGNOSTICS +#endif + +#if JSON_DIAGNOSTIC_POSITIONS + #define NLOHMANN_JSON_ABI_TAG_DIAGNOSTIC_POSITIONS _dp +#else + #define NLOHMANN_JSON_ABI_TAG_DIAGNOSTIC_POSITIONS +#endif + +#if JSON_USE_LEGACY_DISCARDED_VALUE_COMPARISON + #define NLOHMANN_JSON_ABI_TAG_LEGACY_DISCARDED_VALUE_COMPARISON _ldvcmp +#else + #define NLOHMANN_JSON_ABI_TAG_LEGACY_DISCARDED_VALUE_COMPARISON +#endif + +#ifndef NLOHMANN_JSON_NAMESPACE_NO_VERSION + #define NLOHMANN_JSON_NAMESPACE_NO_VERSION 0 +#endif + +// Construct the namespace ABI tags component +#define NLOHMANN_JSON_ABI_TAGS_CONCAT_EX(a, b, c) json_abi ## a ## b ## c +#define NLOHMANN_JSON_ABI_TAGS_CONCAT(a, b, c) \ + NLOHMANN_JSON_ABI_TAGS_CONCAT_EX(a, b, c) + +#define NLOHMANN_JSON_ABI_TAGS \ + NLOHMANN_JSON_ABI_TAGS_CONCAT( \ + NLOHMANN_JSON_ABI_TAG_DIAGNOSTICS, \ + NLOHMANN_JSON_ABI_TAG_LEGACY_DISCARDED_VALUE_COMPARISON, \ + NLOHMANN_JSON_ABI_TAG_DIAGNOSTIC_POSITIONS) + +// Construct the namespace version component +#define NLOHMANN_JSON_NAMESPACE_VERSION_CONCAT_EX(major, minor, patch) \ + _v ## major ## _ ## minor ## _ ## patch +#define NLOHMANN_JSON_NAMESPACE_VERSION_CONCAT(major, minor, patch) \ + NLOHMANN_JSON_NAMESPACE_VERSION_CONCAT_EX(major, minor, patch) + +#if NLOHMANN_JSON_NAMESPACE_NO_VERSION +#define NLOHMANN_JSON_NAMESPACE_VERSION +#else +#define NLOHMANN_JSON_NAMESPACE_VERSION \ + NLOHMANN_JSON_NAMESPACE_VERSION_CONCAT(NLOHMANN_JSON_VERSION_MAJOR, \ + NLOHMANN_JSON_VERSION_MINOR, \ + NLOHMANN_JSON_VERSION_PATCH) +#endif + +// Combine namespace components +#define NLOHMANN_JSON_NAMESPACE_CONCAT_EX(a, b) a ## b +#define NLOHMANN_JSON_NAMESPACE_CONCAT(a, b) \ + NLOHMANN_JSON_NAMESPACE_CONCAT_EX(a, b) + +#ifndef NLOHMANN_JSON_NAMESPACE +#define NLOHMANN_JSON_NAMESPACE \ + nlohmann::NLOHMANN_JSON_NAMESPACE_CONCAT( \ + NLOHMANN_JSON_ABI_TAGS, \ + NLOHMANN_JSON_NAMESPACE_VERSION) +#endif + +#ifndef NLOHMANN_JSON_NAMESPACE_BEGIN +#define NLOHMANN_JSON_NAMESPACE_BEGIN \ + namespace nlohmann \ + { \ + inline namespace NLOHMANN_JSON_NAMESPACE_CONCAT( \ + NLOHMANN_JSON_ABI_TAGS, \ + NLOHMANN_JSON_NAMESPACE_VERSION) \ + { +#endif + +#ifndef NLOHMANN_JSON_NAMESPACE_END +#define NLOHMANN_JSON_NAMESPACE_END \ + } /* namespace (inline namespace) NOLINT(readability/namespace) */ \ + } // namespace nlohmann +#endif diff --git a/src/mc/include/nlohmann/detail/conversions/from_json.hpp b/src/mc/include/nlohmann/detail/conversions/from_json.hpp new file mode 100755 index 0000000..ebd4d39 --- /dev/null +++ b/src/mc/include/nlohmann/detail/conversions/from_json.hpp @@ -0,0 +1,583 @@ +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.12.0 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2013 - 2025 Niels Lohmann +// SPDX-License-Identifier: MIT + +#pragma once + +#include // transform +#include // array +#include // forward_list +#include // inserter, front_inserter, end +#include // map +#include // string +#include // tuple, make_tuple +#include // is_arithmetic, is_same, is_enum, underlying_type, is_convertible +#include // unordered_map +#include // pair, declval +#include // valarray + +#include +#include +#include +#include +#include +#include +#include +#include + +// include after macro_scope.hpp +#ifdef JSON_HAS_CPP_17 + #include // optional +#endif + +#if JSON_HAS_FILESYSTEM || JSON_HAS_EXPERIMENTAL_FILESYSTEM + #include // u8string_view +#endif + +NLOHMANN_JSON_NAMESPACE_BEGIN +namespace detail +{ + +template +inline void from_json(const BasicJsonType& j, typename std::nullptr_t& n) +{ + if (JSON_HEDLEY_UNLIKELY(!j.is_null())) + { + JSON_THROW(type_error::create(302, concat("type must be null, but is ", j.type_name()), &j)); + } + n = nullptr; +} + +#ifdef JSON_HAS_CPP_17 +template +void from_json(const BasicJsonType& j, std::optional& opt) +{ + if (j.is_null()) + { + opt = std::nullopt; + } + else + { + opt.emplace(j.template get()); + } +} +#endif // JSON_HAS_CPP_17 + +// overloads for basic_json template parameters +template < typename BasicJsonType, typename ArithmeticType, + enable_if_t < std::is_arithmetic::value&& + !std::is_same::value, + int > = 0 > +void get_arithmetic_value(const BasicJsonType& j, ArithmeticType& val) +{ + switch (static_cast(j)) + { + case value_t::number_unsigned: + { + val = static_cast(*j.template get_ptr()); + break; + } + case value_t::number_integer: + { + val = static_cast(*j.template get_ptr()); + break; + } + case value_t::number_float: + { + val = static_cast(*j.template get_ptr()); + break; + } + + case value_t::null: + case value_t::object: + case value_t::array: + case value_t::string: + case value_t::boolean: + case value_t::binary: + case value_t::discarded: + default: + JSON_THROW(type_error::create(302, concat("type must be number, but is ", j.type_name()), &j)); + } +} + +template +inline void from_json(const BasicJsonType& j, typename BasicJsonType::boolean_t& b) +{ + if (JSON_HEDLEY_UNLIKELY(!j.is_boolean())) + { + JSON_THROW(type_error::create(302, concat("type must be boolean, but is ", j.type_name()), &j)); + } + b = *j.template get_ptr(); +} + +template +inline void from_json(const BasicJsonType& j, typename BasicJsonType::string_t& s) +{ + if (JSON_HEDLEY_UNLIKELY(!j.is_string())) + { + JSON_THROW(type_error::create(302, concat("type must be string, but is ", j.type_name()), &j)); + } + s = *j.template get_ptr(); +} + +template < + typename BasicJsonType, typename StringType, + enable_if_t < + std::is_assignable::value + && is_detected_exact::value + && !std::is_same::value + && !is_json_ref::value, int > = 0 > +inline void from_json(const BasicJsonType& j, StringType& s) +{ + if (JSON_HEDLEY_UNLIKELY(!j.is_string())) + { + JSON_THROW(type_error::create(302, concat("type must be string, but is ", j.type_name()), &j)); + } + + s = *j.template get_ptr(); +} + +template +inline void from_json(const BasicJsonType& j, typename BasicJsonType::number_float_t& val) +{ + get_arithmetic_value(j, val); +} + +template +inline void from_json(const BasicJsonType& j, typename BasicJsonType::number_unsigned_t& val) +{ + get_arithmetic_value(j, val); +} + +template +inline void from_json(const BasicJsonType& j, typename BasicJsonType::number_integer_t& val) +{ + get_arithmetic_value(j, val); +} + +#if !JSON_DISABLE_ENUM_SERIALIZATION +template::value, int> = 0> +inline void from_json(const BasicJsonType& j, EnumType& e) +{ + typename std::underlying_type::type val; + get_arithmetic_value(j, val); + e = static_cast(val); +} +#endif // JSON_DISABLE_ENUM_SERIALIZATION + +// forward_list doesn't have an insert method +template::value, int> = 0> +inline void from_json(const BasicJsonType& j, std::forward_list& l) +{ + if (JSON_HEDLEY_UNLIKELY(!j.is_array())) + { + JSON_THROW(type_error::create(302, concat("type must be array, but is ", j.type_name()), &j)); + } + l.clear(); + std::transform(j.rbegin(), j.rend(), + std::front_inserter(l), [](const BasicJsonType & i) + { + return i.template get(); + }); +} + +// valarray doesn't have an insert method +template::value, int> = 0> +inline void from_json(const BasicJsonType& j, std::valarray& l) +{ + if (JSON_HEDLEY_UNLIKELY(!j.is_array())) + { + JSON_THROW(type_error::create(302, concat("type must be array, but is ", j.type_name()), &j)); + } + l.resize(j.size()); + std::transform(j.begin(), j.end(), std::begin(l), + [](const BasicJsonType & elem) + { + return elem.template get(); + }); +} + +template +auto from_json(const BasicJsonType& j, T (&arr)[N]) // NOLINT(cppcoreguidelines-avoid-c-arrays,hicpp-avoid-c-arrays,modernize-avoid-c-arrays) +-> decltype(j.template get(), void()) +{ + for (std::size_t i = 0; i < N; ++i) + { + arr[i] = j.at(i).template get(); + } +} + +template +auto from_json(const BasicJsonType& j, T (&arr)[N1][N2]) // NOLINT(cppcoreguidelines-avoid-c-arrays,hicpp-avoid-c-arrays,modernize-avoid-c-arrays) +-> decltype(j.template get(), void()) +{ + for (std::size_t i1 = 0; i1 < N1; ++i1) + { + for (std::size_t i2 = 0; i2 < N2; ++i2) + { + arr[i1][i2] = j.at(i1).at(i2).template get(); + } + } +} + +template +auto from_json(const BasicJsonType& j, T (&arr)[N1][N2][N3]) // NOLINT(cppcoreguidelines-avoid-c-arrays,hicpp-avoid-c-arrays,modernize-avoid-c-arrays) +-> decltype(j.template get(), void()) +{ + for (std::size_t i1 = 0; i1 < N1; ++i1) + { + for (std::size_t i2 = 0; i2 < N2; ++i2) + { + for (std::size_t i3 = 0; i3 < N3; ++i3) + { + arr[i1][i2][i3] = j.at(i1).at(i2).at(i3).template get(); + } + } + } +} + +template +auto from_json(const BasicJsonType& j, T (&arr)[N1][N2][N3][N4]) // NOLINT(cppcoreguidelines-avoid-c-arrays,hicpp-avoid-c-arrays,modernize-avoid-c-arrays) +-> decltype(j.template get(), void()) +{ + for (std::size_t i1 = 0; i1 < N1; ++i1) + { + for (std::size_t i2 = 0; i2 < N2; ++i2) + { + for (std::size_t i3 = 0; i3 < N3; ++i3) + { + for (std::size_t i4 = 0; i4 < N4; ++i4) + { + arr[i1][i2][i3][i4] = j.at(i1).at(i2).at(i3).at(i4).template get(); + } + } + } + } +} + +template +inline void from_json_array_impl(const BasicJsonType& j, typename BasicJsonType::array_t& arr, priority_tag<3> /*unused*/) +{ + arr = *j.template get_ptr(); +} + +template +auto from_json_array_impl(const BasicJsonType& j, std::array& arr, + priority_tag<2> /*unused*/) +-> decltype(j.template get(), void()) +{ + for (std::size_t i = 0; i < N; ++i) + { + arr[i] = j.at(i).template get(); + } +} + +template::value, + int> = 0> +auto from_json_array_impl(const BasicJsonType& j, ConstructibleArrayType& arr, priority_tag<1> /*unused*/) +-> decltype( + arr.reserve(std::declval()), + j.template get(), + void()) +{ + using std::end; + + ConstructibleArrayType ret; + ret.reserve(j.size()); + std::transform(j.begin(), j.end(), + std::inserter(ret, end(ret)), [](const BasicJsonType & i) + { + // get() returns *this, this won't call a from_json + // method when value_type is BasicJsonType + return i.template get(); + }); + arr = std::move(ret); +} + +template::value, + int> = 0> +inline void from_json_array_impl(const BasicJsonType& j, ConstructibleArrayType& arr, + priority_tag<0> /*unused*/) +{ + using std::end; + + ConstructibleArrayType ret; + std::transform( + j.begin(), j.end(), std::inserter(ret, end(ret)), + [](const BasicJsonType & i) + { + // get() returns *this, this won't call a from_json + // method when value_type is BasicJsonType + return i.template get(); + }); + arr = std::move(ret); +} + +template < typename BasicJsonType, typename ConstructibleArrayType, + enable_if_t < + is_constructible_array_type::value&& + !is_constructible_object_type::value&& + !is_constructible_string_type::value&& + !std::is_same::value&& + !is_basic_json::value, + int > = 0 > +auto from_json(const BasicJsonType& j, ConstructibleArrayType& arr) +-> decltype(from_json_array_impl(j, arr, priority_tag<3> {}), +j.template get(), +void()) +{ + if (JSON_HEDLEY_UNLIKELY(!j.is_array())) + { + JSON_THROW(type_error::create(302, concat("type must be array, but is ", j.type_name()), &j)); + } + + from_json_array_impl(j, arr, priority_tag<3> {}); +} + +template < typename BasicJsonType, typename T, std::size_t... Idx > +std::array from_json_inplace_array_impl(BasicJsonType&& j, + identity_tag> /*unused*/, index_sequence /*unused*/) +{ + return { { std::forward(j).at(Idx).template get()... } }; +} + +template < typename BasicJsonType, typename T, std::size_t N > +auto from_json(BasicJsonType&& j, identity_tag> tag) +-> decltype(from_json_inplace_array_impl(std::forward(j), tag, make_index_sequence {})) +{ + if (JSON_HEDLEY_UNLIKELY(!j.is_array())) + { + JSON_THROW(type_error::create(302, concat("type must be array, but is ", j.type_name()), &j)); + } + + return from_json_inplace_array_impl(std::forward(j), tag, make_index_sequence {}); +} + +template +inline void from_json(const BasicJsonType& j, typename BasicJsonType::binary_t& bin) +{ + if (JSON_HEDLEY_UNLIKELY(!j.is_binary())) + { + JSON_THROW(type_error::create(302, concat("type must be binary, but is ", j.type_name()), &j)); + } + + bin = *j.template get_ptr(); +} + +template::value, int> = 0> +inline void from_json(const BasicJsonType& j, ConstructibleObjectType& obj) +{ + if (JSON_HEDLEY_UNLIKELY(!j.is_object())) + { + JSON_THROW(type_error::create(302, concat("type must be object, but is ", j.type_name()), &j)); + } + + ConstructibleObjectType ret; + const auto* inner_object = j.template get_ptr(); + using value_type = typename ConstructibleObjectType::value_type; + std::transform( + inner_object->begin(), inner_object->end(), + std::inserter(ret, ret.begin()), + [](typename BasicJsonType::object_t::value_type const & p) + { + return value_type(p.first, p.second.template get()); + }); + obj = std::move(ret); +} + +// overload for arithmetic types, not chosen for basic_json template arguments +// (BooleanType, etc.); note: Is it really necessary to provide explicit +// overloads for boolean_t etc. in case of a custom BooleanType which is not +// an arithmetic type? +template < typename BasicJsonType, typename ArithmeticType, + enable_if_t < + std::is_arithmetic::value&& + !std::is_same::value&& + !std::is_same::value&& + !std::is_same::value&& + !std::is_same::value, + int > = 0 > +inline void from_json(const BasicJsonType& j, ArithmeticType& val) +{ + switch (static_cast(j)) + { + case value_t::number_unsigned: + { + val = static_cast(*j.template get_ptr()); + break; + } + case value_t::number_integer: + { + val = static_cast(*j.template get_ptr()); + break; + } + case value_t::number_float: + { + val = static_cast(*j.template get_ptr()); + break; + } + case value_t::boolean: + { + val = static_cast(*j.template get_ptr()); + break; + } + + case value_t::null: + case value_t::object: + case value_t::array: + case value_t::string: + case value_t::binary: + case value_t::discarded: + default: + JSON_THROW(type_error::create(302, concat("type must be number, but is ", j.type_name()), &j)); + } +} + +template +std::tuple from_json_tuple_impl_base(BasicJsonType&& j, index_sequence /*unused*/) +{ + return std::make_tuple(std::forward(j).at(Idx).template get()...); +} + +template +std::tuple<> from_json_tuple_impl_base(BasicJsonType& /*unused*/, index_sequence<> /*unused*/) +{ + return {}; +} + +template < typename BasicJsonType, class A1, class A2 > +std::pair from_json_tuple_impl(BasicJsonType&& j, identity_tag> /*unused*/, priority_tag<0> /*unused*/) +{ + return {std::forward(j).at(0).template get(), + std::forward(j).at(1).template get()}; +} + +template +inline void from_json_tuple_impl(BasicJsonType&& j, std::pair& p, priority_tag<1> /*unused*/) +{ + p = from_json_tuple_impl(std::forward(j), identity_tag> {}, priority_tag<0> {}); +} + +template +std::tuple from_json_tuple_impl(BasicJsonType&& j, identity_tag> /*unused*/, priority_tag<2> /*unused*/) +{ + return from_json_tuple_impl_base(std::forward(j), index_sequence_for {}); +} + +template +inline void from_json_tuple_impl(BasicJsonType&& j, std::tuple& t, priority_tag<3> /*unused*/) +{ + t = from_json_tuple_impl_base(std::forward(j), index_sequence_for {}); +} + +template +auto from_json(BasicJsonType&& j, TupleRelated&& t) +-> decltype(from_json_tuple_impl(std::forward(j), std::forward(t), priority_tag<3> {})) +{ + if (JSON_HEDLEY_UNLIKELY(!j.is_array())) + { + JSON_THROW(type_error::create(302, concat("type must be array, but is ", j.type_name()), &j)); + } + + return from_json_tuple_impl(std::forward(j), std::forward(t), priority_tag<3> {}); +} + +template < typename BasicJsonType, typename Key, typename Value, typename Compare, typename Allocator, + typename = enable_if_t < !std::is_constructible < + typename BasicJsonType::string_t, Key >::value >> +inline void from_json(const BasicJsonType& j, std::map& m) +{ + if (JSON_HEDLEY_UNLIKELY(!j.is_array())) + { + JSON_THROW(type_error::create(302, concat("type must be array, but is ", j.type_name()), &j)); + } + m.clear(); + for (const auto& p : j) + { + if (JSON_HEDLEY_UNLIKELY(!p.is_array())) + { + JSON_THROW(type_error::create(302, concat("type must be array, but is ", p.type_name()), &j)); + } + m.emplace(p.at(0).template get(), p.at(1).template get()); + } +} + +template < typename BasicJsonType, typename Key, typename Value, typename Hash, typename KeyEqual, typename Allocator, + typename = enable_if_t < !std::is_constructible < + typename BasicJsonType::string_t, Key >::value >> +inline void from_json(const BasicJsonType& j, std::unordered_map& m) +{ + if (JSON_HEDLEY_UNLIKELY(!j.is_array())) + { + JSON_THROW(type_error::create(302, concat("type must be array, but is ", j.type_name()), &j)); + } + m.clear(); + for (const auto& p : j) + { + if (JSON_HEDLEY_UNLIKELY(!p.is_array())) + { + JSON_THROW(type_error::create(302, concat("type must be array, but is ", p.type_name()), &j)); + } + m.emplace(p.at(0).template get(), p.at(1).template get()); + } +} + +#if JSON_HAS_FILESYSTEM || JSON_HAS_EXPERIMENTAL_FILESYSTEM +template +inline void from_json(const BasicJsonType& j, std_fs::path& p) +{ + if (JSON_HEDLEY_UNLIKELY(!j.is_string())) + { + JSON_THROW(type_error::create(302, concat("type must be string, but is ", j.type_name()), &j)); + } + const auto& s = *j.template get_ptr(); + // Checking for C++20 standard or later can be insufficient in case the + // library support for char8_t is either incomplete or was disabled + // altogether. Use the __cpp_lib_char8_t feature test instead. +#if defined(__cpp_lib_char8_t) && (__cpp_lib_char8_t >= 201907L) + p = std_fs::path(std::u8string_view(reinterpret_cast(s.data()), s.size())); +#else + p = std_fs::u8path(s); // accepts UTF-8 encoded std::string in C++17, deprecated in C++20 +#endif +} +#endif + +struct from_json_fn +{ + template + auto operator()(const BasicJsonType& j, T&& val) const + noexcept(noexcept(from_json(j, std::forward(val)))) + -> decltype(from_json(j, std::forward(val))) + { + return from_json(j, std::forward(val)); + } +}; + +} // namespace detail + +#ifndef JSON_HAS_CPP_17 +/// namespace to hold default `from_json` function +/// to see why this is required: +/// http://www.open-std.org/jtc1/sc22/wg21/docs/papers/2015/n4381.html +namespace // NOLINT(cert-dcl59-cpp,fuchsia-header-anon-namespaces,google-build-namespaces) +{ +#endif +JSON_INLINE_VARIABLE constexpr const auto& from_json = // NOLINT(misc-definitions-in-headers) + detail::static_const::value; +#ifndef JSON_HAS_CPP_17 +} // namespace +#endif + +NLOHMANN_JSON_NAMESPACE_END diff --git a/src/mc/include/nlohmann/detail/conversions/to_chars.hpp b/src/mc/include/nlohmann/detail/conversions/to_chars.hpp new file mode 100755 index 0000000..3c663cf --- /dev/null +++ b/src/mc/include/nlohmann/detail/conversions/to_chars.hpp @@ -0,0 +1,1118 @@ +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.12.0 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2009 Florian Loitsch +// SPDX-FileCopyrightText: 2013 - 2025 Niels Lohmann +// SPDX-License-Identifier: MIT + +#pragma once + +#include // array +#include // signbit, isfinite +#include // intN_t, uintN_t +#include // memcpy, memmove +#include // numeric_limits +#include // conditional + +#include + +NLOHMANN_JSON_NAMESPACE_BEGIN +namespace detail +{ + +/*! +@brief implements the Grisu2 algorithm for binary to decimal floating-point +conversion. + +This implementation is a slightly modified version of the reference +implementation which may be obtained from +http://florian.loitsch.com/publications (bench.tar.gz). + +The code is distributed under the MIT license, Copyright (c) 2009 Florian Loitsch. + +For a detailed description of the algorithm see: + +[1] Loitsch, "Printing Floating-Point Numbers Quickly and Accurately with + Integers", Proceedings of the ACM SIGPLAN 2010 Conference on Programming + Language Design and Implementation, PLDI 2010 +[2] Burger, Dybvig, "Printing Floating-Point Numbers Quickly and Accurately", + Proceedings of the ACM SIGPLAN 1996 Conference on Programming Language + Design and Implementation, PLDI 1996 +*/ +namespace dtoa_impl +{ + +template +Target reinterpret_bits(const Source source) +{ + static_assert(sizeof(Target) == sizeof(Source), "size mismatch"); + + Target target; + std::memcpy(&target, &source, sizeof(Source)); + return target; +} + +struct diyfp // f * 2^e +{ + static constexpr int kPrecision = 64; // = q + + std::uint64_t f = 0; + int e = 0; + + constexpr diyfp(std::uint64_t f_, int e_) noexcept : f(f_), e(e_) {} + + /*! + @brief returns x - y + @pre x.e == y.e and x.f >= y.f + */ + static diyfp sub(const diyfp& x, const diyfp& y) noexcept + { + JSON_ASSERT(x.e == y.e); + JSON_ASSERT(x.f >= y.f); + + return {x.f - y.f, x.e}; + } + + /*! + @brief returns x * y + @note The result is rounded. (Only the upper q bits are returned.) + */ + static diyfp mul(const diyfp& x, const diyfp& y) noexcept + { + static_assert(kPrecision == 64, "internal error"); + + // Computes: + // f = round((x.f * y.f) / 2^q) + // e = x.e + y.e + q + + // Emulate the 64-bit * 64-bit multiplication: + // + // p = u * v + // = (u_lo + 2^32 u_hi) (v_lo + 2^32 v_hi) + // = (u_lo v_lo ) + 2^32 ((u_lo v_hi ) + (u_hi v_lo )) + 2^64 (u_hi v_hi ) + // = (p0 ) + 2^32 ((p1 ) + (p2 )) + 2^64 (p3 ) + // = (p0_lo + 2^32 p0_hi) + 2^32 ((p1_lo + 2^32 p1_hi) + (p2_lo + 2^32 p2_hi)) + 2^64 (p3 ) + // = (p0_lo ) + 2^32 (p0_hi + p1_lo + p2_lo ) + 2^64 (p1_hi + p2_hi + p3) + // = (p0_lo ) + 2^32 (Q ) + 2^64 (H ) + // = (p0_lo ) + 2^32 (Q_lo + 2^32 Q_hi ) + 2^64 (H ) + // + // (Since Q might be larger than 2^32 - 1) + // + // = (p0_lo + 2^32 Q_lo) + 2^64 (Q_hi + H) + // + // (Q_hi + H does not overflow a 64-bit int) + // + // = p_lo + 2^64 p_hi + + const std::uint64_t u_lo = x.f & 0xFFFFFFFFu; + const std::uint64_t u_hi = x.f >> 32u; + const std::uint64_t v_lo = y.f & 0xFFFFFFFFu; + const std::uint64_t v_hi = y.f >> 32u; + + const std::uint64_t p0 = u_lo * v_lo; + const std::uint64_t p1 = u_lo * v_hi; + const std::uint64_t p2 = u_hi * v_lo; + const std::uint64_t p3 = u_hi * v_hi; + + const std::uint64_t p0_hi = p0 >> 32u; + const std::uint64_t p1_lo = p1 & 0xFFFFFFFFu; + const std::uint64_t p1_hi = p1 >> 32u; + const std::uint64_t p2_lo = p2 & 0xFFFFFFFFu; + const std::uint64_t p2_hi = p2 >> 32u; + + std::uint64_t Q = p0_hi + p1_lo + p2_lo; + + // The full product might now be computed as + // + // p_hi = p3 + p2_hi + p1_hi + (Q >> 32) + // p_lo = p0_lo + (Q << 32) + // + // But in this particular case here, the full p_lo is not required. + // Effectively, we only need to add the highest bit in p_lo to p_hi (and + // Q_hi + 1 does not overflow). + + Q += std::uint64_t{1} << (64u - 32u - 1u); // round, ties up + + const std::uint64_t h = p3 + p2_hi + p1_hi + (Q >> 32u); + + return {h, x.e + y.e + 64}; + } + + /*! + @brief normalize x such that the significand is >= 2^(q-1) + @pre x.f != 0 + */ + static diyfp normalize(diyfp x) noexcept + { + JSON_ASSERT(x.f != 0); + + while ((x.f >> 63u) == 0) + { + x.f <<= 1u; + x.e--; + } + + return x; + } + + /*! + @brief normalize x such that the result has the exponent E + @pre e >= x.e and the upper e - x.e bits of x.f must be zero. + */ + static diyfp normalize_to(const diyfp& x, const int target_exponent) noexcept + { + const int delta = x.e - target_exponent; + + JSON_ASSERT(delta >= 0); + JSON_ASSERT(((x.f << delta) >> delta) == x.f); + + return {x.f << delta, target_exponent}; + } +}; + +struct boundaries +{ + diyfp w; + diyfp minus; + diyfp plus; +}; + +/*! +Compute the (normalized) diyfp representing the input number 'value' and its +boundaries. + +@pre value must be finite and positive +*/ +template +boundaries compute_boundaries(FloatType value) +{ + JSON_ASSERT(std::isfinite(value)); + JSON_ASSERT(value > 0); + + // Convert the IEEE representation into a diyfp. + // + // If v is denormal: + // value = 0.F * 2^(1 - bias) = ( F) * 2^(1 - bias - (p-1)) + // If v is normalized: + // value = 1.F * 2^(E - bias) = (2^(p-1) + F) * 2^(E - bias - (p-1)) + + static_assert(std::numeric_limits::is_iec559, + "internal error: dtoa_short requires an IEEE-754 floating-point implementation"); + + constexpr int kPrecision = std::numeric_limits::digits; // = p (includes the hidden bit) + constexpr int kBias = std::numeric_limits::max_exponent - 1 + (kPrecision - 1); + constexpr int kMinExp = 1 - kBias; + constexpr std::uint64_t kHiddenBit = std::uint64_t{1} << (kPrecision - 1); // = 2^(p-1) + + using bits_type = typename std::conditional::type; + + const auto bits = static_cast(reinterpret_bits(value)); + const std::uint64_t E = bits >> (kPrecision - 1); + const std::uint64_t F = bits & (kHiddenBit - 1); + + const bool is_denormal = E == 0; + const diyfp v = is_denormal + ? diyfp(F, kMinExp) + : diyfp(F + kHiddenBit, static_cast(E) - kBias); + + // Compute the boundaries m- and m+ of the floating-point value + // v = f * 2^e. + // + // Determine v- and v+, the floating-point predecessor and successor of v, + // respectively. + // + // v- = v - 2^e if f != 2^(p-1) or e == e_min (A) + // = v - 2^(e-1) if f == 2^(p-1) and e > e_min (B) + // + // v+ = v + 2^e + // + // Let m- = (v- + v) / 2 and m+ = (v + v+) / 2. All real numbers _strictly_ + // between m- and m+ round to v, regardless of how the input rounding + // algorithm breaks ties. + // + // ---+-------------+-------------+-------------+-------------+--- (A) + // v- m- v m+ v+ + // + // -----------------+------+------+-------------+-------------+--- (B) + // v- m- v m+ v+ + + const bool lower_boundary_is_closer = F == 0 && E > 1; + const diyfp m_plus = diyfp((2 * v.f) + 1, v.e - 1); + const diyfp m_minus = lower_boundary_is_closer + ? diyfp((4 * v.f) - 1, v.e - 2) // (B) + : diyfp((2 * v.f) - 1, v.e - 1); // (A) + + // Determine the normalized w+ = m+. + const diyfp w_plus = diyfp::normalize(m_plus); + + // Determine w- = m- such that e_(w-) = e_(w+). + const diyfp w_minus = diyfp::normalize_to(m_minus, w_plus.e); + + return {diyfp::normalize(v), w_minus, w_plus}; +} + +// Given normalized diyfp w, Grisu needs to find a (normalized) cached +// power-of-ten c, such that the exponent of the product c * w = f * 2^e lies +// within a certain range [alpha, gamma] (Definition 3.2 from [1]) +// +// alpha <= e = e_c + e_w + q <= gamma +// +// or +// +// f_c * f_w * 2^alpha <= f_c 2^(e_c) * f_w 2^(e_w) * 2^q +// <= f_c * f_w * 2^gamma +// +// Since c and w are normalized, i.e. 2^(q-1) <= f < 2^q, this implies +// +// 2^(q-1) * 2^(q-1) * 2^alpha <= c * w * 2^q < 2^q * 2^q * 2^gamma +// +// or +// +// 2^(q - 2 + alpha) <= c * w < 2^(q + gamma) +// +// The choice of (alpha,gamma) determines the size of the table and the form of +// the digit generation procedure. Using (alpha,gamma)=(-60,-32) works out well +// in practice: +// +// The idea is to cut the number c * w = f * 2^e into two parts, which can be +// processed independently: An integral part p1, and a fractional part p2: +// +// f * 2^e = ( (f div 2^-e) * 2^-e + (f mod 2^-e) ) * 2^e +// = (f div 2^-e) + (f mod 2^-e) * 2^e +// = p1 + p2 * 2^e +// +// The conversion of p1 into decimal form requires a series of divisions and +// modulos by (a power of) 10. These operations are faster for 32-bit than for +// 64-bit integers, so p1 should ideally fit into a 32-bit integer. This can be +// achieved by choosing +// +// -e >= 32 or e <= -32 := gamma +// +// In order to convert the fractional part +// +// p2 * 2^e = p2 / 2^-e = d[-1] / 10^1 + d[-2] / 10^2 + ... +// +// into decimal form, the fraction is repeatedly multiplied by 10 and the digits +// d[-i] are extracted in order: +// +// (10 * p2) div 2^-e = d[-1] +// (10 * p2) mod 2^-e = d[-2] / 10^1 + ... +// +// The multiplication by 10 must not overflow. It is sufficient to choose +// +// 10 * p2 < 16 * p2 = 2^4 * p2 <= 2^64. +// +// Since p2 = f mod 2^-e < 2^-e, +// +// -e <= 60 or e >= -60 := alpha + +constexpr int kAlpha = -60; +constexpr int kGamma = -32; + +struct cached_power // c = f * 2^e ~= 10^k +{ + std::uint64_t f; + int e; + int k; +}; + +/*! +For a normalized diyfp w = f * 2^e, this function returns a (normalized) cached +power-of-ten c = f_c * 2^e_c, such that the exponent of the product w * c +satisfies (Definition 3.2 from [1]) + + alpha <= e_c + e + q <= gamma. +*/ +inline cached_power get_cached_power_for_binary_exponent(int e) +{ + // Now + // + // alpha <= e_c + e + q <= gamma (1) + // ==> f_c * 2^alpha <= c * 2^e * 2^q + // + // and since the c's are normalized, 2^(q-1) <= f_c, + // + // ==> 2^(q - 1 + alpha) <= c * 2^(e + q) + // ==> 2^(alpha - e - 1) <= c + // + // If c were an exact power of ten, i.e. c = 10^k, one may determine k as + // + // k = ceil( log_10( 2^(alpha - e - 1) ) ) + // = ceil( (alpha - e - 1) * log_10(2) ) + // + // From the paper: + // "In theory the result of the procedure could be wrong since c is rounded, + // and the computation itself is approximated [...]. In practice, however, + // this simple function is sufficient." + // + // For IEEE double precision floating-point numbers converted into + // normalized diyfp's w = f * 2^e, with q = 64, + // + // e >= -1022 (min IEEE exponent) + // -52 (p - 1) + // -52 (p - 1, possibly normalize denormal IEEE numbers) + // -11 (normalize the diyfp) + // = -1137 + // + // and + // + // e <= +1023 (max IEEE exponent) + // -52 (p - 1) + // -11 (normalize the diyfp) + // = 960 + // + // This binary exponent range [-1137,960] results in a decimal exponent + // range [-307,324]. One does not need to store a cached power for each + // k in this range. For each such k it suffices to find a cached power + // such that the exponent of the product lies in [alpha,gamma]. + // This implies that the difference of the decimal exponents of adjacent + // table entries must be less than or equal to + // + // floor( (gamma - alpha) * log_10(2) ) = 8. + // + // (A smaller distance gamma-alpha would require a larger table.) + + // NB: + // Actually, this function returns c, such that -60 <= e_c + e + 64 <= -34. + + constexpr int kCachedPowersMinDecExp = -300; + constexpr int kCachedPowersDecStep = 8; + + static constexpr std::array kCachedPowers = + { + { + { 0xAB70FE17C79AC6CA, -1060, -300 }, + { 0xFF77B1FCBEBCDC4F, -1034, -292 }, + { 0xBE5691EF416BD60C, -1007, -284 }, + { 0x8DD01FAD907FFC3C, -980, -276 }, + { 0xD3515C2831559A83, -954, -268 }, + { 0x9D71AC8FADA6C9B5, -927, -260 }, + { 0xEA9C227723EE8BCB, -901, -252 }, + { 0xAECC49914078536D, -874, -244 }, + { 0x823C12795DB6CE57, -847, -236 }, + { 0xC21094364DFB5637, -821, -228 }, + { 0x9096EA6F3848984F, -794, -220 }, + { 0xD77485CB25823AC7, -768, -212 }, + { 0xA086CFCD97BF97F4, -741, -204 }, + { 0xEF340A98172AACE5, -715, -196 }, + { 0xB23867FB2A35B28E, -688, -188 }, + { 0x84C8D4DFD2C63F3B, -661, -180 }, + { 0xC5DD44271AD3CDBA, -635, -172 }, + { 0x936B9FCEBB25C996, -608, -164 }, + { 0xDBAC6C247D62A584, -582, -156 }, + { 0xA3AB66580D5FDAF6, -555, -148 }, + { 0xF3E2F893DEC3F126, -529, -140 }, + { 0xB5B5ADA8AAFF80B8, -502, -132 }, + { 0x87625F056C7C4A8B, -475, -124 }, + { 0xC9BCFF6034C13053, -449, -116 }, + { 0x964E858C91BA2655, -422, -108 }, + { 0xDFF9772470297EBD, -396, -100 }, + { 0xA6DFBD9FB8E5B88F, -369, -92 }, + { 0xF8A95FCF88747D94, -343, -84 }, + { 0xB94470938FA89BCF, -316, -76 }, + { 0x8A08F0F8BF0F156B, -289, -68 }, + { 0xCDB02555653131B6, -263, -60 }, + { 0x993FE2C6D07B7FAC, -236, -52 }, + { 0xE45C10C42A2B3B06, -210, -44 }, + { 0xAA242499697392D3, -183, -36 }, + { 0xFD87B5F28300CA0E, -157, -28 }, + { 0xBCE5086492111AEB, -130, -20 }, + { 0x8CBCCC096F5088CC, -103, -12 }, + { 0xD1B71758E219652C, -77, -4 }, + { 0x9C40000000000000, -50, 4 }, + { 0xE8D4A51000000000, -24, 12 }, + { 0xAD78EBC5AC620000, 3, 20 }, + { 0x813F3978F8940984, 30, 28 }, + { 0xC097CE7BC90715B3, 56, 36 }, + { 0x8F7E32CE7BEA5C70, 83, 44 }, + { 0xD5D238A4ABE98068, 109, 52 }, + { 0x9F4F2726179A2245, 136, 60 }, + { 0xED63A231D4C4FB27, 162, 68 }, + { 0xB0DE65388CC8ADA8, 189, 76 }, + { 0x83C7088E1AAB65DB, 216, 84 }, + { 0xC45D1DF942711D9A, 242, 92 }, + { 0x924D692CA61BE758, 269, 100 }, + { 0xDA01EE641A708DEA, 295, 108 }, + { 0xA26DA3999AEF774A, 322, 116 }, + { 0xF209787BB47D6B85, 348, 124 }, + { 0xB454E4A179DD1877, 375, 132 }, + { 0x865B86925B9BC5C2, 402, 140 }, + { 0xC83553C5C8965D3D, 428, 148 }, + { 0x952AB45CFA97A0B3, 455, 156 }, + { 0xDE469FBD99A05FE3, 481, 164 }, + { 0xA59BC234DB398C25, 508, 172 }, + { 0xF6C69A72A3989F5C, 534, 180 }, + { 0xB7DCBF5354E9BECE, 561, 188 }, + { 0x88FCF317F22241E2, 588, 196 }, + { 0xCC20CE9BD35C78A5, 614, 204 }, + { 0x98165AF37B2153DF, 641, 212 }, + { 0xE2A0B5DC971F303A, 667, 220 }, + { 0xA8D9D1535CE3B396, 694, 228 }, + { 0xFB9B7CD9A4A7443C, 720, 236 }, + { 0xBB764C4CA7A44410, 747, 244 }, + { 0x8BAB8EEFB6409C1A, 774, 252 }, + { 0xD01FEF10A657842C, 800, 260 }, + { 0x9B10A4E5E9913129, 827, 268 }, + { 0xE7109BFBA19C0C9D, 853, 276 }, + { 0xAC2820D9623BF429, 880, 284 }, + { 0x80444B5E7AA7CF85, 907, 292 }, + { 0xBF21E44003ACDD2D, 933, 300 }, + { 0x8E679C2F5E44FF8F, 960, 308 }, + { 0xD433179D9C8CB841, 986, 316 }, + { 0x9E19DB92B4E31BA9, 1013, 324 }, + } + }; + + // This computation gives exactly the same results for k as + // k = ceil((kAlpha - e - 1) * 0.30102999566398114) + // for |e| <= 1500, but doesn't require floating-point operations. + // NB: log_10(2) ~= 78913 / 2^18 + JSON_ASSERT(e >= -1500); + JSON_ASSERT(e <= 1500); + const int f = kAlpha - e - 1; + const int k = ((f * 78913) / (1 << 18)) + static_cast(f > 0); + + const int index = (-kCachedPowersMinDecExp + k + (kCachedPowersDecStep - 1)) / kCachedPowersDecStep; + JSON_ASSERT(index >= 0); + JSON_ASSERT(static_cast(index) < kCachedPowers.size()); + + const cached_power cached = kCachedPowers[static_cast(index)]; + JSON_ASSERT(kAlpha <= cached.e + e + 64); + JSON_ASSERT(kGamma >= cached.e + e + 64); + + return cached; +} + +/*! +For n != 0, returns k, such that pow10 := 10^(k-1) <= n < 10^k. +For n == 0, returns 1 and sets pow10 := 1. +*/ +inline int find_largest_pow10(const std::uint32_t n, std::uint32_t& pow10) +{ + // LCOV_EXCL_START + if (n >= 1000000000) + { + pow10 = 1000000000; + return 10; + } + // LCOV_EXCL_STOP + if (n >= 100000000) + { + pow10 = 100000000; + return 9; + } + if (n >= 10000000) + { + pow10 = 10000000; + return 8; + } + if (n >= 1000000) + { + pow10 = 1000000; + return 7; + } + if (n >= 100000) + { + pow10 = 100000; + return 6; + } + if (n >= 10000) + { + pow10 = 10000; + return 5; + } + if (n >= 1000) + { + pow10 = 1000; + return 4; + } + if (n >= 100) + { + pow10 = 100; + return 3; + } + if (n >= 10) + { + pow10 = 10; + return 2; + } + + pow10 = 1; + return 1; +} + +inline void grisu2_round(char* buf, int len, std::uint64_t dist, std::uint64_t delta, + std::uint64_t rest, std::uint64_t ten_k) +{ + JSON_ASSERT(len >= 1); + JSON_ASSERT(dist <= delta); + JSON_ASSERT(rest <= delta); + JSON_ASSERT(ten_k > 0); + + // <--------------------------- delta ----> + // <---- dist ---------> + // --------------[------------------+-------------------]-------------- + // M- w M+ + // + // ten_k + // <------> + // <---- rest ----> + // --------------[------------------+----+--------------]-------------- + // w V + // = buf * 10^k + // + // ten_k represents a unit-in-the-last-place in the decimal representation + // stored in buf. + // Decrement buf by ten_k while this takes buf closer to w. + + // The tests are written in this order to avoid overflow in unsigned + // integer arithmetic. + + while (rest < dist + && delta - rest >= ten_k + && (rest + ten_k < dist || dist - rest > rest + ten_k - dist)) + { + JSON_ASSERT(buf[len - 1] != '0'); + buf[len - 1]--; + rest += ten_k; + } +} + +/*! +Generates V = buffer * 10^decimal_exponent, such that M- <= V <= M+. +M- and M+ must be normalized and share the same exponent -60 <= e <= -32. +*/ +inline void grisu2_digit_gen(char* buffer, int& length, int& decimal_exponent, + diyfp M_minus, diyfp w, diyfp M_plus) +{ + static_assert(kAlpha >= -60, "internal error"); + static_assert(kGamma <= -32, "internal error"); + + // Generates the digits (and the exponent) of a decimal floating-point + // number V = buffer * 10^decimal_exponent in the range [M-, M+]. The diyfp's + // w, M- and M+ share the same exponent e, which satisfies alpha <= e <= gamma. + // + // <--------------------------- delta ----> + // <---- dist ---------> + // --------------[------------------+-------------------]-------------- + // M- w M+ + // + // Grisu2 generates the digits of M+ from left to right and stops as soon as + // V is in [M-,M+]. + + JSON_ASSERT(M_plus.e >= kAlpha); + JSON_ASSERT(M_plus.e <= kGamma); + + std::uint64_t delta = diyfp::sub(M_plus, M_minus).f; // (significand of (M+ - M-), implicit exponent is e) + std::uint64_t dist = diyfp::sub(M_plus, w ).f; // (significand of (M+ - w ), implicit exponent is e) + + // Split M+ = f * 2^e into two parts p1 and p2 (note: e < 0): + // + // M+ = f * 2^e + // = ((f div 2^-e) * 2^-e + (f mod 2^-e)) * 2^e + // = ((p1 ) * 2^-e + (p2 )) * 2^e + // = p1 + p2 * 2^e + + const diyfp one(std::uint64_t{1} << -M_plus.e, M_plus.e); + + auto p1 = static_cast(M_plus.f >> -one.e); // p1 = f div 2^-e (Since -e >= 32, p1 fits into a 32-bit int.) + std::uint64_t p2 = M_plus.f & (one.f - 1); // p2 = f mod 2^-e + + // 1) + // + // Generate the digits of the integral part p1 = d[n-1]...d[1]d[0] + + JSON_ASSERT(p1 > 0); + + std::uint32_t pow10{}; + const int k = find_largest_pow10(p1, pow10); + + // 10^(k-1) <= p1 < 10^k, pow10 = 10^(k-1) + // + // p1 = (p1 div 10^(k-1)) * 10^(k-1) + (p1 mod 10^(k-1)) + // = (d[k-1] ) * 10^(k-1) + (p1 mod 10^(k-1)) + // + // M+ = p1 + p2 * 2^e + // = d[k-1] * 10^(k-1) + (p1 mod 10^(k-1)) + p2 * 2^e + // = d[k-1] * 10^(k-1) + ((p1 mod 10^(k-1)) * 2^-e + p2) * 2^e + // = d[k-1] * 10^(k-1) + ( rest) * 2^e + // + // Now generate the digits d[n] of p1 from left to right (n = k-1,...,0) + // + // p1 = d[k-1]...d[n] * 10^n + d[n-1]...d[0] + // + // but stop as soon as + // + // rest * 2^e = (d[n-1]...d[0] * 2^-e + p2) * 2^e <= delta * 2^e + + int n = k; + while (n > 0) + { + // Invariants: + // M+ = buffer * 10^n + (p1 + p2 * 2^e) (buffer = 0 for n = k) + // pow10 = 10^(n-1) <= p1 < 10^n + // + const std::uint32_t d = p1 / pow10; // d = p1 div 10^(n-1) + const std::uint32_t r = p1 % pow10; // r = p1 mod 10^(n-1) + // + // M+ = buffer * 10^n + (d * 10^(n-1) + r) + p2 * 2^e + // = (buffer * 10 + d) * 10^(n-1) + (r + p2 * 2^e) + // + JSON_ASSERT(d <= 9); + buffer[length++] = static_cast('0' + d); // buffer := buffer * 10 + d + // + // M+ = buffer * 10^(n-1) + (r + p2 * 2^e) + // + p1 = r; + n--; + // + // M+ = buffer * 10^n + (p1 + p2 * 2^e) + // pow10 = 10^n + // + + // Now check if enough digits have been generated. + // Compute + // + // p1 + p2 * 2^e = (p1 * 2^-e + p2) * 2^e = rest * 2^e + // + // Note: + // Since rest and delta share the same exponent e, it suffices to + // compare the significands. + const std::uint64_t rest = (std::uint64_t{p1} << -one.e) + p2; + if (rest <= delta) + { + // V = buffer * 10^n, with M- <= V <= M+. + + decimal_exponent += n; + + // We may now just stop. But instead, it looks as if the buffer + // could be decremented to bring V closer to w. + // + // pow10 = 10^n is now 1 ulp in the decimal representation V. + // The rounding procedure works with diyfp's with an implicit + // exponent of e. + // + // 10^n = (10^n * 2^-e) * 2^e = ulp * 2^e + // + const std::uint64_t ten_n = std::uint64_t{pow10} << -one.e; + grisu2_round(buffer, length, dist, delta, rest, ten_n); + + return; + } + + pow10 /= 10; + // + // pow10 = 10^(n-1) <= p1 < 10^n + // Invariants restored. + } + + // 2) + // + // The digits of the integral part have been generated: + // + // M+ = d[k-1]...d[1]d[0] + p2 * 2^e + // = buffer + p2 * 2^e + // + // Now generate the digits of the fractional part p2 * 2^e. + // + // Note: + // No decimal point is generated: the exponent is adjusted instead. + // + // p2 actually represents the fraction + // + // p2 * 2^e + // = p2 / 2^-e + // = d[-1] / 10^1 + d[-2] / 10^2 + ... + // + // Now generate the digits d[-m] of p1 from left to right (m = 1,2,...) + // + // p2 * 2^e = d[-1]d[-2]...d[-m] * 10^-m + // + 10^-m * (d[-m-1] / 10^1 + d[-m-2] / 10^2 + ...) + // + // using + // + // 10^m * p2 = ((10^m * p2) div 2^-e) * 2^-e + ((10^m * p2) mod 2^-e) + // = ( d) * 2^-e + ( r) + // + // or + // 10^m * p2 * 2^e = d + r * 2^e + // + // i.e. + // + // M+ = buffer + p2 * 2^e + // = buffer + 10^-m * (d + r * 2^e) + // = (buffer * 10^m + d) * 10^-m + 10^-m * r * 2^e + // + // and stop as soon as 10^-m * r * 2^e <= delta * 2^e + + JSON_ASSERT(p2 > delta); + + int m = 0; + for (;;) + { + // Invariant: + // M+ = buffer * 10^-m + 10^-m * (d[-m-1] / 10 + d[-m-2] / 10^2 + ...) * 2^e + // = buffer * 10^-m + 10^-m * (p2 ) * 2^e + // = buffer * 10^-m + 10^-m * (1/10 * (10 * p2) ) * 2^e + // = buffer * 10^-m + 10^-m * (1/10 * ((10*p2 div 2^-e) * 2^-e + (10*p2 mod 2^-e)) * 2^e + // + JSON_ASSERT(p2 <= (std::numeric_limits::max)() / 10); + p2 *= 10; + const std::uint64_t d = p2 >> -one.e; // d = (10 * p2) div 2^-e + const std::uint64_t r = p2 & (one.f - 1); // r = (10 * p2) mod 2^-e + // + // M+ = buffer * 10^-m + 10^-m * (1/10 * (d * 2^-e + r) * 2^e + // = buffer * 10^-m + 10^-m * (1/10 * (d + r * 2^e)) + // = (buffer * 10 + d) * 10^(-m-1) + 10^(-m-1) * r * 2^e + // + JSON_ASSERT(d <= 9); + buffer[length++] = static_cast('0' + d); // buffer := buffer * 10 + d + // + // M+ = buffer * 10^(-m-1) + 10^(-m-1) * r * 2^e + // + p2 = r; + m++; + // + // M+ = buffer * 10^-m + 10^-m * p2 * 2^e + // Invariant restored. + + // Check if enough digits have been generated. + // + // 10^-m * p2 * 2^e <= delta * 2^e + // p2 * 2^e <= 10^m * delta * 2^e + // p2 <= 10^m * delta + delta *= 10; + dist *= 10; + if (p2 <= delta) + { + break; + } + } + + // V = buffer * 10^-m, with M- <= V <= M+. + + decimal_exponent -= m; + + // 1 ulp in the decimal representation is now 10^-m. + // Since delta and dist are now scaled by 10^m, we need to do the + // same with ulp in order to keep the units in sync. + // + // 10^m * 10^-m = 1 = 2^-e * 2^e = ten_m * 2^e + // + const std::uint64_t ten_m = one.f; + grisu2_round(buffer, length, dist, delta, p2, ten_m); + + // By construction this algorithm generates the shortest possible decimal + // number (Loitsch, Theorem 6.2) which rounds back to w. + // For an input number of precision p, at least + // + // N = 1 + ceil(p * log_10(2)) + // + // decimal digits are sufficient to identify all binary floating-point + // numbers (Matula, "In-and-Out conversions"). + // This implies that the algorithm does not produce more than N decimal + // digits. + // + // N = 17 for p = 53 (IEEE double precision) + // N = 9 for p = 24 (IEEE single precision) +} + +/*! +v = buf * 10^decimal_exponent +len is the length of the buffer (number of decimal digits) +The buffer must be large enough, i.e. >= max_digits10. +*/ +JSON_HEDLEY_NON_NULL(1) +inline void grisu2(char* buf, int& len, int& decimal_exponent, + diyfp m_minus, diyfp v, diyfp m_plus) +{ + JSON_ASSERT(m_plus.e == m_minus.e); + JSON_ASSERT(m_plus.e == v.e); + + // --------(-----------------------+-----------------------)-------- (A) + // m- v m+ + // + // --------------------(-----------+-----------------------)-------- (B) + // m- v m+ + // + // First scale v (and m- and m+) such that the exponent is in the range + // [alpha, gamma]. + + const cached_power cached = get_cached_power_for_binary_exponent(m_plus.e); + + const diyfp c_minus_k(cached.f, cached.e); // = c ~= 10^-k + + // The exponent of the products is = v.e + c_minus_k.e + q and is in the range [alpha,gamma] + const diyfp w = diyfp::mul(v, c_minus_k); + const diyfp w_minus = diyfp::mul(m_minus, c_minus_k); + const diyfp w_plus = diyfp::mul(m_plus, c_minus_k); + + // ----(---+---)---------------(---+---)---------------(---+---)---- + // w- w w+ + // = c*m- = c*v = c*m+ + // + // diyfp::mul rounds its result and c_minus_k is approximated too. w, w- and + // w+ are now off by a small amount. + // In fact: + // + // w - v * 10^k < 1 ulp + // + // To account for this inaccuracy, add resp. subtract 1 ulp. + // + // --------+---[---------------(---+---)---------------]---+-------- + // w- M- w M+ w+ + // + // Now any number in [M-, M+] (bounds included) will round to w when input, + // regardless of how the input rounding algorithm breaks ties. + // + // And digit_gen generates the shortest possible such number in [M-, M+]. + // Note that this does not mean that Grisu2 always generates the shortest + // possible number in the interval (m-, m+). + const diyfp M_minus(w_minus.f + 1, w_minus.e); + const diyfp M_plus (w_plus.f - 1, w_plus.e ); + + decimal_exponent = -cached.k; // = -(-k) = k + + grisu2_digit_gen(buf, len, decimal_exponent, M_minus, w, M_plus); +} + +/*! +v = buf * 10^decimal_exponent +len is the length of the buffer (number of decimal digits) +The buffer must be large enough, i.e. >= max_digits10. +*/ +template +JSON_HEDLEY_NON_NULL(1) +void grisu2(char* buf, int& len, int& decimal_exponent, FloatType value) +{ + static_assert(diyfp::kPrecision >= std::numeric_limits::digits + 3, + "internal error: not enough precision"); + + JSON_ASSERT(std::isfinite(value)); + JSON_ASSERT(value > 0); + + // If the neighbors (and boundaries) of 'value' are always computed for double-precision + // numbers, all float's can be recovered using strtod (and strtof). However, the resulting + // decimal representations are not exactly "short". + // + // The documentation for 'std::to_chars' (https://en.cppreference.com/w/cpp/utility/to_chars) + // says "value is converted to a string as if by std::sprintf in the default ("C") locale" + // and since sprintf promotes floats to doubles, I think this is exactly what 'std::to_chars' + // does. + // On the other hand, the documentation for 'std::to_chars' requires that "parsing the + // representation using the corresponding std::from_chars function recovers value exactly". That + // indicates that single precision floating-point numbers should be recovered using + // 'std::strtof'. + // + // NB: If the neighbors are computed for single-precision numbers, there is a single float + // (7.0385307e-26f) which can't be recovered using strtod. The resulting double precision + // value is off by 1 ulp. +#if 0 // NOLINT(readability-avoid-unconditional-preprocessor-if) + const boundaries w = compute_boundaries(static_cast(value)); +#else + const boundaries w = compute_boundaries(value); +#endif + + grisu2(buf, len, decimal_exponent, w.minus, w.w, w.plus); +} + +/*! +@brief appends a decimal representation of e to buf +@return a pointer to the element following the exponent. +@pre -1000 < e < 1000 +*/ +JSON_HEDLEY_NON_NULL(1) +JSON_HEDLEY_RETURNS_NON_NULL +inline char* append_exponent(char* buf, int e) +{ + JSON_ASSERT(e > -1000); + JSON_ASSERT(e < 1000); + + if (e < 0) + { + e = -e; + *buf++ = '-'; + } + else + { + *buf++ = '+'; + } + + auto k = static_cast(e); + if (k < 10) + { + // Always print at least two digits in the exponent. + // This is for compatibility with printf("%g"). + *buf++ = '0'; + *buf++ = static_cast('0' + k); + } + else if (k < 100) + { + *buf++ = static_cast('0' + (k / 10)); + k %= 10; + *buf++ = static_cast('0' + k); + } + else + { + *buf++ = static_cast('0' + (k / 100)); + k %= 100; + *buf++ = static_cast('0' + (k / 10)); + k %= 10; + *buf++ = static_cast('0' + k); + } + + return buf; +} + +/*! +@brief prettify v = buf * 10^decimal_exponent + +If v is in the range [10^min_exp, 10^max_exp) it will be printed in fixed-point +notation. Otherwise it will be printed in exponential notation. + +@pre min_exp < 0 +@pre max_exp > 0 +*/ +JSON_HEDLEY_NON_NULL(1) +JSON_HEDLEY_RETURNS_NON_NULL +inline char* format_buffer(char* buf, int len, int decimal_exponent, + int min_exp, int max_exp) +{ + JSON_ASSERT(min_exp < 0); + JSON_ASSERT(max_exp > 0); + + const int k = len; + const int n = len + decimal_exponent; + + // v = buf * 10^(n-k) + // k is the length of the buffer (number of decimal digits) + // n is the position of the decimal point relative to the start of the buffer. + + if (k <= n && n <= max_exp) + { + // digits[000] + // len <= max_exp + 2 + + std::memset(buf + k, '0', static_cast(n) - static_cast(k)); + // Make it look like a floating-point number (#362, #378) + buf[n + 0] = '.'; + buf[n + 1] = '0'; + return buf + (static_cast(n) + 2); + } + + if (0 < n && n <= max_exp) + { + // dig.its + // len <= max_digits10 + 1 + + JSON_ASSERT(k > n); + + std::memmove(buf + (static_cast(n) + 1), buf + n, static_cast(k) - static_cast(n)); + buf[n] = '.'; + return buf + (static_cast(k) + 1U); + } + + if (min_exp < n && n <= 0) + { + // 0.[000]digits + // len <= 2 + (-min_exp - 1) + max_digits10 + + std::memmove(buf + (2 + static_cast(-n)), buf, static_cast(k)); + buf[0] = '0'; + buf[1] = '.'; + std::memset(buf + 2, '0', static_cast(-n)); + return buf + (2U + static_cast(-n) + static_cast(k)); + } + + if (k == 1) + { + // dE+123 + // len <= 1 + 5 + + buf += 1; + } + else + { + // d.igitsE+123 + // len <= max_digits10 + 1 + 5 + + std::memmove(buf + 2, buf + 1, static_cast(k) - 1); + buf[1] = '.'; + buf += 1 + static_cast(k); + } + + *buf++ = 'e'; + return append_exponent(buf, n - 1); +} + +} // namespace dtoa_impl + +/*! +@brief generates a decimal representation of the floating-point number value in [first, last). + +The format of the resulting decimal representation is similar to printf's %g +format. Returns an iterator pointing past-the-end of the decimal representation. + +@note The input number must be finite, i.e. NaN's and Inf's are not supported. +@note The buffer must be large enough. +@note The result is NOT null-terminated. +*/ +template +JSON_HEDLEY_NON_NULL(1, 2) +JSON_HEDLEY_RETURNS_NON_NULL +char* to_chars(char* first, const char* last, FloatType value) +{ + static_cast(last); // maybe unused - fix warning + JSON_ASSERT(std::isfinite(value)); + + // Use signbit(value) instead of (value < 0) since signbit works for -0. + if (std::signbit(value)) + { + value = -value; + *first++ = '-'; + } + +#ifdef __GNUC__ +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wfloat-equal" +#endif + if (value == 0) // +-0 + { + *first++ = '0'; + // Make it look like a floating-point number (#362, #378) + *first++ = '.'; + *first++ = '0'; + return first; + } +#ifdef __GNUC__ +#pragma GCC diagnostic pop +#endif + + JSON_ASSERT(last - first >= std::numeric_limits::max_digits10); + + // Compute v = buffer * 10^decimal_exponent. + // The decimal digits are stored in the buffer, which needs to be interpreted + // as an unsigned decimal integer. + // len is the length of the buffer, i.e., the number of decimal digits. + int len = 0; + int decimal_exponent = 0; + dtoa_impl::grisu2(first, len, decimal_exponent, value); + + JSON_ASSERT(len <= std::numeric_limits::max_digits10); + + // Format the buffer like printf("%.*g", prec, value) + constexpr int kMinExp = -4; + // Use digits10 here to increase compatibility with version 2. + constexpr int kMaxExp = std::numeric_limits::digits10; + + JSON_ASSERT(last - first >= kMaxExp + 2); + JSON_ASSERT(last - first >= 2 + (-kMinExp - 1) + std::numeric_limits::max_digits10); + JSON_ASSERT(last - first >= std::numeric_limits::max_digits10 + 6); + + return dtoa_impl::format_buffer(first, len, decimal_exponent, kMinExp, kMaxExp); +} + +} // namespace detail +NLOHMANN_JSON_NAMESPACE_END diff --git a/src/mc/include/nlohmann/detail/conversions/to_json.hpp b/src/mc/include/nlohmann/detail/conversions/to_json.hpp new file mode 100755 index 0000000..8b910dd --- /dev/null +++ b/src/mc/include/nlohmann/detail/conversions/to_json.hpp @@ -0,0 +1,486 @@ +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.12.0 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2013 - 2025 Niels Lohmann +// SPDX-License-Identifier: MIT + +#pragma once + +#include // JSON_HAS_CPP_17 +#ifdef JSON_HAS_CPP_17 + #include // optional +#endif + +#include // copy +#include // begin, end +#include // allocator_traits +#include // basic_string, char_traits +#include // tuple, get +#include // is_same, is_constructible, is_floating_point, is_enum, underlying_type +#include // move, forward, declval, pair +#include // valarray +#include // vector + +#include +#include +#include +#include +#include + +NLOHMANN_JSON_NAMESPACE_BEGIN +namespace detail +{ + +////////////////// +// constructors // +////////////////// + +/* + * Note all external_constructor<>::construct functions need to call + * j.m_data.m_value.destroy(j.m_data.m_type) to avoid a memory leak in case j contains an + * allocated value (e.g., a string). See bug issue + * https://github.com/nlohmann/json/issues/2865 for more information. + */ + +template struct external_constructor; + +template<> +struct external_constructor +{ + template + static void construct(BasicJsonType& j, typename BasicJsonType::boolean_t b) noexcept + { + j.m_data.m_value.destroy(j.m_data.m_type); + j.m_data.m_type = value_t::boolean; + j.m_data.m_value = b; + j.assert_invariant(); + } +}; + +template<> +struct external_constructor +{ + template + static void construct(BasicJsonType& j, const typename BasicJsonType::string_t& s) + { + j.m_data.m_value.destroy(j.m_data.m_type); + j.m_data.m_type = value_t::string; + j.m_data.m_value = s; + j.assert_invariant(); + } + + template + static void construct(BasicJsonType& j, typename BasicJsonType::string_t&& s) + { + j.m_data.m_value.destroy(j.m_data.m_type); + j.m_data.m_type = value_t::string; + j.m_data.m_value = std::move(s); + j.assert_invariant(); + } + + template < typename BasicJsonType, typename CompatibleStringType, + enable_if_t < !std::is_same::value, + int > = 0 > + static void construct(BasicJsonType& j, const CompatibleStringType& str) + { + j.m_data.m_value.destroy(j.m_data.m_type); + j.m_data.m_type = value_t::string; + j.m_data.m_value.string = j.template create(str); + j.assert_invariant(); + } +}; + +template<> +struct external_constructor +{ + template + static void construct(BasicJsonType& j, const typename BasicJsonType::binary_t& b) + { + j.m_data.m_value.destroy(j.m_data.m_type); + j.m_data.m_type = value_t::binary; + j.m_data.m_value = typename BasicJsonType::binary_t(b); + j.assert_invariant(); + } + + template + static void construct(BasicJsonType& j, typename BasicJsonType::binary_t&& b) + { + j.m_data.m_value.destroy(j.m_data.m_type); + j.m_data.m_type = value_t::binary; + j.m_data.m_value = typename BasicJsonType::binary_t(std::move(b)); + j.assert_invariant(); + } +}; + +template<> +struct external_constructor +{ + template + static void construct(BasicJsonType& j, typename BasicJsonType::number_float_t val) noexcept + { + j.m_data.m_value.destroy(j.m_data.m_type); + j.m_data.m_type = value_t::number_float; + j.m_data.m_value = val; + j.assert_invariant(); + } +}; + +template<> +struct external_constructor +{ + template + static void construct(BasicJsonType& j, typename BasicJsonType::number_unsigned_t val) noexcept + { + j.m_data.m_value.destroy(j.m_data.m_type); + j.m_data.m_type = value_t::number_unsigned; + j.m_data.m_value = val; + j.assert_invariant(); + } +}; + +template<> +struct external_constructor +{ + template + static void construct(BasicJsonType& j, typename BasicJsonType::number_integer_t val) noexcept + { + j.m_data.m_value.destroy(j.m_data.m_type); + j.m_data.m_type = value_t::number_integer; + j.m_data.m_value = val; + j.assert_invariant(); + } +}; + +template<> +struct external_constructor +{ + template + static void construct(BasicJsonType& j, const typename BasicJsonType::array_t& arr) + { + j.m_data.m_value.destroy(j.m_data.m_type); + j.m_data.m_type = value_t::array; + j.m_data.m_value = arr; + j.set_parents(); + j.assert_invariant(); + } + + template + static void construct(BasicJsonType& j, typename BasicJsonType::array_t&& arr) + { + j.m_data.m_value.destroy(j.m_data.m_type); + j.m_data.m_type = value_t::array; + j.m_data.m_value = std::move(arr); + j.set_parents(); + j.assert_invariant(); + } + + template < typename BasicJsonType, typename CompatibleArrayType, + enable_if_t < !std::is_same::value, + int > = 0 > + static void construct(BasicJsonType& j, const CompatibleArrayType& arr) + { + using std::begin; + using std::end; + + j.m_data.m_value.destroy(j.m_data.m_type); + j.m_data.m_type = value_t::array; + j.m_data.m_value.array = j.template create(begin(arr), end(arr)); + j.set_parents(); + j.assert_invariant(); + } + + template + static void construct(BasicJsonType& j, const std::vector& arr) + { + j.m_data.m_value.destroy(j.m_data.m_type); + j.m_data.m_type = value_t::array; + j.m_data.m_value = value_t::array; + j.m_data.m_value.array->reserve(arr.size()); + for (const bool x : arr) + { + j.m_data.m_value.array->push_back(x); + j.set_parent(j.m_data.m_value.array->back()); + } + j.assert_invariant(); + } + + template::value, int> = 0> + static void construct(BasicJsonType& j, const std::valarray& arr) + { + j.m_data.m_value.destroy(j.m_data.m_type); + j.m_data.m_type = value_t::array; + j.m_data.m_value = value_t::array; + j.m_data.m_value.array->resize(arr.size()); + if (arr.size() > 0) + { + std::copy(std::begin(arr), std::end(arr), j.m_data.m_value.array->begin()); + } + j.set_parents(); + j.assert_invariant(); + } +}; + +template<> +struct external_constructor +{ + template + static void construct(BasicJsonType& j, const typename BasicJsonType::object_t& obj) + { + j.m_data.m_value.destroy(j.m_data.m_type); + j.m_data.m_type = value_t::object; + j.m_data.m_value = obj; + j.set_parents(); + j.assert_invariant(); + } + + template + static void construct(BasicJsonType& j, typename BasicJsonType::object_t&& obj) + { + j.m_data.m_value.destroy(j.m_data.m_type); + j.m_data.m_type = value_t::object; + j.m_data.m_value = std::move(obj); + j.set_parents(); + j.assert_invariant(); + } + + template < typename BasicJsonType, typename CompatibleObjectType, + enable_if_t < !std::is_same::value, int > = 0 > + static void construct(BasicJsonType& j, const CompatibleObjectType& obj) + { + using std::begin; + using std::end; + + j.m_data.m_value.destroy(j.m_data.m_type); + j.m_data.m_type = value_t::object; + j.m_data.m_value.object = j.template create(begin(obj), end(obj)); + j.set_parents(); + j.assert_invariant(); + } +}; + +///////////// +// to_json // +///////////// + +#ifdef JSON_HAS_CPP_17 +template::value, int> = 0> +void to_json(BasicJsonType& j, const std::optional& opt) noexcept +{ + if (opt.has_value()) + { + j = *opt; + } + else + { + j = nullptr; + } +} +#endif + +template::value, int> = 0> +inline void to_json(BasicJsonType& j, T b) noexcept +{ + external_constructor::construct(j, b); +} + +template < typename BasicJsonType, typename BoolRef, + enable_if_t < + ((std::is_same::reference, BoolRef>::value + && !std::is_same ::reference, typename BasicJsonType::boolean_t&>::value) + || (std::is_same::const_reference, BoolRef>::value + && !std::is_same ::const_reference>, + typename BasicJsonType::boolean_t >::value)) + && std::is_convertible::value, int > = 0 > +inline void to_json(BasicJsonType& j, const BoolRef& b) noexcept +{ + external_constructor::construct(j, static_cast(b)); +} + +template::value, int> = 0> +inline void to_json(BasicJsonType& j, const CompatibleString& s) +{ + external_constructor::construct(j, s); +} + +template +inline void to_json(BasicJsonType& j, typename BasicJsonType::string_t&& s) +{ + external_constructor::construct(j, std::move(s)); +} + +template::value, int> = 0> +inline void to_json(BasicJsonType& j, FloatType val) noexcept +{ + external_constructor::construct(j, static_cast(val)); +} + +template::value, int> = 0> +inline void to_json(BasicJsonType& j, CompatibleNumberUnsignedType val) noexcept +{ + external_constructor::construct(j, static_cast(val)); +} + +template::value, int> = 0> +inline void to_json(BasicJsonType& j, CompatibleNumberIntegerType val) noexcept +{ + external_constructor::construct(j, static_cast(val)); +} + +#if !JSON_DISABLE_ENUM_SERIALIZATION +template::value, int> = 0> +inline void to_json(BasicJsonType& j, EnumType e) noexcept +{ + using underlying_type = typename std::underlying_type::type; + static constexpr value_t integral_value_t = std::is_unsigned::value ? value_t::number_unsigned : value_t::number_integer; + external_constructor::construct(j, static_cast(e)); +} +#endif // JSON_DISABLE_ENUM_SERIALIZATION + +template +inline void to_json(BasicJsonType& j, const std::vector& e) +{ + external_constructor::construct(j, e); +} + +template < typename BasicJsonType, typename CompatibleArrayType, + enable_if_t < is_compatible_array_type::value&& + !is_compatible_object_type::value&& + !is_compatible_string_type::value&& + !std::is_same::value&& + !is_basic_json::value, + int > = 0 > +inline void to_json(BasicJsonType& j, const CompatibleArrayType& arr) +{ + external_constructor::construct(j, arr); +} + +template +inline void to_json(BasicJsonType& j, const typename BasicJsonType::binary_t& bin) +{ + external_constructor::construct(j, bin); +} + +template::value, int> = 0> +inline void to_json(BasicJsonType& j, const std::valarray& arr) +{ + external_constructor::construct(j, std::move(arr)); +} + +template +inline void to_json(BasicJsonType& j, typename BasicJsonType::array_t&& arr) +{ + external_constructor::construct(j, std::move(arr)); +} + +template < typename BasicJsonType, typename CompatibleObjectType, + enable_if_t < is_compatible_object_type::value&& !is_basic_json::value, int > = 0 > +inline void to_json(BasicJsonType& j, const CompatibleObjectType& obj) +{ + external_constructor::construct(j, obj); +} + +template +inline void to_json(BasicJsonType& j, typename BasicJsonType::object_t&& obj) +{ + external_constructor::construct(j, std::move(obj)); +} + +template < + typename BasicJsonType, typename T, std::size_t N, + enable_if_t < !std::is_constructible::value, // NOLINT(cppcoreguidelines-avoid-c-arrays,hicpp-avoid-c-arrays,modernize-avoid-c-arrays) + int > = 0 > +inline void to_json(BasicJsonType& j, const T(&arr)[N]) // NOLINT(cppcoreguidelines-avoid-c-arrays,hicpp-avoid-c-arrays,modernize-avoid-c-arrays) +{ + external_constructor::construct(j, arr); +} + +template < typename BasicJsonType, typename T1, typename T2, enable_if_t < std::is_constructible::value&& std::is_constructible::value, int > = 0 > +inline void to_json(BasicJsonType& j, const std::pair& p) +{ + j = { p.first, p.second }; +} + +// for https://github.com/nlohmann/json/pull/1134 +template>::value, int> = 0> +inline void to_json(BasicJsonType& j, const T& b) +{ + j = { {b.key(), b.value()} }; +} + +template +inline void to_json_tuple_impl(BasicJsonType& j, const Tuple& t, index_sequence /*unused*/) +{ + j = { std::get(t)... }; +} + +template +inline void to_json_tuple_impl(BasicJsonType& j, const Tuple& /*unused*/, index_sequence<> /*unused*/) +{ + using array_t = typename BasicJsonType::array_t; + j = array_t(); +} + +template::value, int > = 0> +inline void to_json(BasicJsonType& j, const T& t) +{ + to_json_tuple_impl(j, t, make_index_sequence::value> {}); +} + +#if JSON_HAS_FILESYSTEM || JSON_HAS_EXPERIMENTAL_FILESYSTEM +#if defined(__cpp_lib_char8_t) +template +inline void to_json(BasicJsonType& j, const std::basic_string& s) +{ + using OtherAllocator = typename std::allocator_traits::template rebind_alloc; + j = std::basic_string, OtherAllocator>(s.begin(), s.end(), s.get_allocator()); +} +#endif + +template +inline void to_json(BasicJsonType& j, const std_fs::path& p) +{ + // Returns either a std::string or a std::u8string depending whether library + // support for char8_t is enabled. + j = p.u8string(); +} +#endif + +struct to_json_fn +{ + template + auto operator()(BasicJsonType& j, T&& val) const noexcept(noexcept(to_json(j, std::forward(val)))) + -> decltype(to_json(j, std::forward(val)), void()) + { + return to_json(j, std::forward(val)); + } +}; +} // namespace detail + +#ifndef JSON_HAS_CPP_17 +/// namespace to hold default `to_json` function +/// to see why this is required: +/// http://www.open-std.org/jtc1/sc22/wg21/docs/papers/2015/n4381.html +namespace // NOLINT(cert-dcl59-cpp,fuchsia-header-anon-namespaces,google-build-namespaces) +{ +#endif +JSON_INLINE_VARIABLE constexpr const auto& to_json = // NOLINT(misc-definitions-in-headers) + detail::static_const::value; +#ifndef JSON_HAS_CPP_17 +} // namespace +#endif + +NLOHMANN_JSON_NAMESPACE_END diff --git a/src/mc/include/nlohmann/detail/exceptions.hpp b/src/mc/include/nlohmann/detail/exceptions.hpp new file mode 100755 index 0000000..2740e4c --- /dev/null +++ b/src/mc/include/nlohmann/detail/exceptions.hpp @@ -0,0 +1,291 @@ +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.12.0 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2013 - 2025 Niels Lohmann +// SPDX-License-Identifier: MIT + +#pragma once + +#include // nullptr_t +#include // exception +#if JSON_DIAGNOSTICS + #include // accumulate +#endif +#include // runtime_error +#include // to_string +#include // vector + +#include +#include +#include +#include +#include +#include +#include + +// With -Wweak-vtables, Clang will complain about the exception classes as they +// have no out-of-line virtual method definitions and their vtable will be +// emitted in every translation unit. This issue cannot be fixed with a +// header-only library as there is no implementation file to move these +// functions to. As a result, we suppress this warning here to avoid client +// code stumbling over this. See https://github.com/nlohmann/json/issues/4087 +// for a discussion. +#if defined(__clang__) + #pragma clang diagnostic push + #pragma clang diagnostic ignored "-Wweak-vtables" +#endif + +NLOHMANN_JSON_NAMESPACE_BEGIN +namespace detail +{ + +//////////////// +// exceptions // +//////////////// + +/// @brief general exception of the @ref basic_json class +/// @sa https://json.nlohmann.me/api/basic_json/exception/ +class exception : public std::exception +{ + public: + /// returns the explanatory string + const char* what() const noexcept override + { + return m.what(); + } + + /// the id of the exception + const int id; // NOLINT(cppcoreguidelines-non-private-member-variables-in-classes) + + protected: + JSON_HEDLEY_NON_NULL(3) + exception(int id_, const char* what_arg) : id(id_), m(what_arg) {} // NOLINT(bugprone-throw-keyword-missing) + + static std::string name(const std::string& ename, int id_) + { + return concat("[json.exception.", ename, '.', std::to_string(id_), "] "); + } + + static std::string diagnostics(std::nullptr_t /*leaf_element*/) + { + return ""; + } + + template + static std::string diagnostics(const BasicJsonType* leaf_element) + { +#if JSON_DIAGNOSTICS + std::vector tokens; + for (const auto* current = leaf_element; current != nullptr && current->m_parent != nullptr; current = current->m_parent) + { + switch (current->m_parent->type()) + { + case value_t::array: + { + for (std::size_t i = 0; i < current->m_parent->m_data.m_value.array->size(); ++i) + { + if (¤t->m_parent->m_data.m_value.array->operator[](i) == current) + { + tokens.emplace_back(std::to_string(i)); + break; + } + } + break; + } + + case value_t::object: + { + for (const auto& element : *current->m_parent->m_data.m_value.object) + { + if (&element.second == current) + { + tokens.emplace_back(element.first.c_str()); + break; + } + } + break; + } + + case value_t::null: // LCOV_EXCL_LINE + case value_t::string: // LCOV_EXCL_LINE + case value_t::boolean: // LCOV_EXCL_LINE + case value_t::number_integer: // LCOV_EXCL_LINE + case value_t::number_unsigned: // LCOV_EXCL_LINE + case value_t::number_float: // LCOV_EXCL_LINE + case value_t::binary: // LCOV_EXCL_LINE + case value_t::discarded: // LCOV_EXCL_LINE + default: // LCOV_EXCL_LINE + break; // LCOV_EXCL_LINE + } + } + + if (tokens.empty()) + { + return ""; + } + + auto str = std::accumulate(tokens.rbegin(), tokens.rend(), std::string{}, + [](const std::string & a, const std::string & b) + { + return concat(a, '/', detail::escape(b)); + }); + + return concat('(', str, ") ", get_byte_positions(leaf_element)); +#else + return get_byte_positions(leaf_element); +#endif + } + + private: + /// an exception object as storage for error messages + std::runtime_error m; +#if JSON_DIAGNOSTIC_POSITIONS + template + static std::string get_byte_positions(const BasicJsonType* leaf_element) + { + if ((leaf_element->start_pos() != std::string::npos) && (leaf_element->end_pos() != std::string::npos)) + { + return concat("(bytes ", std::to_string(leaf_element->start_pos()), "-", std::to_string(leaf_element->end_pos()), ") "); + } + return ""; + } +#else + template + static std::string get_byte_positions(const BasicJsonType* leaf_element) + { + static_cast(leaf_element); + return ""; + } +#endif +}; + +/// @brief exception indicating a parse error +/// @sa https://json.nlohmann.me/api/basic_json/parse_error/ +class parse_error : public exception +{ + public: + /*! + @brief create a parse error exception + @param[in] id_ the id of the exception + @param[in] pos the position where the error occurred (or with + chars_read_total=0 if the position cannot be + determined) + @param[in] what_arg the explanatory string + @return parse_error object + */ + template::value, int> = 0> + static parse_error create(int id_, const position_t& pos, const std::string& what_arg, BasicJsonContext context) + { + const std::string w = concat(exception::name("parse_error", id_), "parse error", + position_string(pos), ": ", exception::diagnostics(context), what_arg); + return {id_, pos.chars_read_total, w.c_str()}; + } + + template::value, int> = 0> + static parse_error create(int id_, std::size_t byte_, const std::string& what_arg, BasicJsonContext context) + { + const std::string w = concat(exception::name("parse_error", id_), "parse error", + (byte_ != 0 ? (concat(" at byte ", std::to_string(byte_))) : ""), + ": ", exception::diagnostics(context), what_arg); + return {id_, byte_, w.c_str()}; + } + + /*! + @brief byte index of the parse error + + The byte index of the last read character in the input file. + + @note For an input with n bytes, 1 is the index of the first character and + n+1 is the index of the terminating null byte or the end of file. + This also holds true when reading a byte vector (CBOR or MessagePack). + */ + const std::size_t byte; + + private: + parse_error(int id_, std::size_t byte_, const char* what_arg) + : exception(id_, what_arg), byte(byte_) {} + + static std::string position_string(const position_t& pos) + { + return concat(" at line ", std::to_string(pos.lines_read + 1), + ", column ", std::to_string(pos.chars_read_current_line)); + } +}; + +/// @brief exception indicating errors with iterators +/// @sa https://json.nlohmann.me/api/basic_json/invalid_iterator/ +class invalid_iterator : public exception +{ + public: + template::value, int> = 0> + static invalid_iterator create(int id_, const std::string& what_arg, BasicJsonContext context) + { + const std::string w = concat(exception::name("invalid_iterator", id_), exception::diagnostics(context), what_arg); + return {id_, w.c_str()}; + } + + private: + JSON_HEDLEY_NON_NULL(3) + invalid_iterator(int id_, const char* what_arg) + : exception(id_, what_arg) {} +}; + +/// @brief exception indicating executing a member function with a wrong type +/// @sa https://json.nlohmann.me/api/basic_json/type_error/ +class type_error : public exception +{ + public: + template::value, int> = 0> + static type_error create(int id_, const std::string& what_arg, BasicJsonContext context) + { + const std::string w = concat(exception::name("type_error", id_), exception::diagnostics(context), what_arg); + return {id_, w.c_str()}; + } + + private: + JSON_HEDLEY_NON_NULL(3) + type_error(int id_, const char* what_arg) : exception(id_, what_arg) {} +}; + +/// @brief exception indicating access out of the defined range +/// @sa https://json.nlohmann.me/api/basic_json/out_of_range/ +class out_of_range : public exception +{ + public: + template::value, int> = 0> + static out_of_range create(int id_, const std::string& what_arg, BasicJsonContext context) + { + const std::string w = concat(exception::name("out_of_range", id_), exception::diagnostics(context), what_arg); + return {id_, w.c_str()}; + } + + private: + JSON_HEDLEY_NON_NULL(3) + out_of_range(int id_, const char* what_arg) : exception(id_, what_arg) {} +}; + +/// @brief exception indicating other library errors +/// @sa https://json.nlohmann.me/api/basic_json/other_error/ +class other_error : public exception +{ + public: + template::value, int> = 0> + static other_error create(int id_, const std::string& what_arg, BasicJsonContext context) + { + const std::string w = concat(exception::name("other_error", id_), exception::diagnostics(context), what_arg); + return {id_, w.c_str()}; + } + + private: + JSON_HEDLEY_NON_NULL(3) + other_error(int id_, const char* what_arg) : exception(id_, what_arg) {} +}; + +} // namespace detail +NLOHMANN_JSON_NAMESPACE_END + +#if defined(__clang__) + #pragma clang diagnostic pop +#endif diff --git a/src/mc/include/nlohmann/detail/hash.hpp b/src/mc/include/nlohmann/detail/hash.hpp new file mode 100755 index 0000000..973943e --- /dev/null +++ b/src/mc/include/nlohmann/detail/hash.hpp @@ -0,0 +1,129 @@ +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.12.0 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2013 - 2025 Niels Lohmann +// SPDX-License-Identifier: MIT + +#pragma once + +#include // uint8_t +#include // size_t +#include // hash + +#include +#include + +NLOHMANN_JSON_NAMESPACE_BEGIN +namespace detail +{ + +// boost::hash_combine +inline std::size_t combine(std::size_t seed, std::size_t h) noexcept +{ + seed ^= h + 0x9e3779b9 + (seed << 6U) + (seed >> 2U); + return seed; +} + +/*! +@brief hash a JSON value + +The hash function tries to rely on std::hash where possible. Furthermore, the +type of the JSON value is taken into account to have different hash values for +null, 0, 0U, and false, etc. + +@tparam BasicJsonType basic_json specialization +@param j JSON value to hash +@return hash value of j +*/ +template +std::size_t hash(const BasicJsonType& j) +{ + using string_t = typename BasicJsonType::string_t; + using number_integer_t = typename BasicJsonType::number_integer_t; + using number_unsigned_t = typename BasicJsonType::number_unsigned_t; + using number_float_t = typename BasicJsonType::number_float_t; + + const auto type = static_cast(j.type()); + switch (j.type()) + { + case BasicJsonType::value_t::null: + case BasicJsonType::value_t::discarded: + { + return combine(type, 0); + } + + case BasicJsonType::value_t::object: + { + auto seed = combine(type, j.size()); + for (const auto& element : j.items()) + { + const auto h = std::hash {}(element.key()); + seed = combine(seed, h); + seed = combine(seed, hash(element.value())); + } + return seed; + } + + case BasicJsonType::value_t::array: + { + auto seed = combine(type, j.size()); + for (const auto& element : j) + { + seed = combine(seed, hash(element)); + } + return seed; + } + + case BasicJsonType::value_t::string: + { + const auto h = std::hash {}(j.template get_ref()); + return combine(type, h); + } + + case BasicJsonType::value_t::boolean: + { + const auto h = std::hash {}(j.template get()); + return combine(type, h); + } + + case BasicJsonType::value_t::number_integer: + { + const auto h = std::hash {}(j.template get()); + return combine(type, h); + } + + case BasicJsonType::value_t::number_unsigned: + { + const auto h = std::hash {}(j.template get()); + return combine(type, h); + } + + case BasicJsonType::value_t::number_float: + { + const auto h = std::hash {}(j.template get()); + return combine(type, h); + } + + case BasicJsonType::value_t::binary: + { + auto seed = combine(type, j.get_binary().size()); + const auto h = std::hash {}(j.get_binary().has_subtype()); + seed = combine(seed, h); + seed = combine(seed, static_cast(j.get_binary().subtype())); + for (const auto byte : j.get_binary()) + { + seed = combine(seed, std::hash {}(byte)); + } + return seed; + } + + default: // LCOV_EXCL_LINE + JSON_ASSERT(false); // NOLINT(cert-dcl03-c,hicpp-static-assert,misc-static-assert) LCOV_EXCL_LINE + return 0; // LCOV_EXCL_LINE + } +} + +} // namespace detail +NLOHMANN_JSON_NAMESPACE_END diff --git a/src/mc/include/nlohmann/detail/input/binary_reader.hpp b/src/mc/include/nlohmann/detail/input/binary_reader.hpp new file mode 100755 index 0000000..3d62ff2 --- /dev/null +++ b/src/mc/include/nlohmann/detail/input/binary_reader.hpp @@ -0,0 +1,3081 @@ +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.12.0 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2013 - 2025 Niels Lohmann +// SPDX-License-Identifier: MIT + +#pragma once + +#include // generate_n +#include // array +#include // ldexp +#include // size_t +#include // uint8_t, uint16_t, uint32_t, uint64_t +#include // snprintf +#include // memcpy +#include // back_inserter +#include // numeric_limits +#include // char_traits, string +#include // make_pair, move +#include // vector +#ifdef __cpp_lib_byteswap + #include //byteswap +#endif + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +NLOHMANN_JSON_NAMESPACE_BEGIN +namespace detail +{ + +/// how to treat CBOR tags +enum class cbor_tag_handler_t +{ + error, ///< throw a parse_error exception in case of a tag + ignore, ///< ignore tags + store ///< store tags as binary type +}; + +/*! +@brief determine system byte order + +@return true if and only if system's byte order is little endian + +@note from https://stackoverflow.com/a/1001328/266378 +*/ +inline bool little_endianness(int num = 1) noexcept +{ + return *reinterpret_cast(&num) == 1; +} + +/////////////////// +// binary reader // +/////////////////// + +/*! +@brief deserialization of CBOR, MessagePack, and UBJSON values +*/ +template> +class binary_reader +{ + using number_integer_t = typename BasicJsonType::number_integer_t; + using number_unsigned_t = typename BasicJsonType::number_unsigned_t; + using number_float_t = typename BasicJsonType::number_float_t; + using string_t = typename BasicJsonType::string_t; + using binary_t = typename BasicJsonType::binary_t; + using json_sax_t = SAX; + using char_type = typename InputAdapterType::char_type; + using char_int_type = typename char_traits::int_type; + + public: + /*! + @brief create a binary reader + + @param[in] adapter input adapter to read from + */ + explicit binary_reader(InputAdapterType&& adapter, const input_format_t format = input_format_t::json) noexcept : ia(std::move(adapter)), input_format(format) + { + (void)detail::is_sax_static_asserts {}; + } + + // make class move-only + binary_reader(const binary_reader&) = delete; + binary_reader(binary_reader&&) = default; // NOLINT(hicpp-noexcept-move,performance-noexcept-move-constructor) + binary_reader& operator=(const binary_reader&) = delete; + binary_reader& operator=(binary_reader&&) = default; // NOLINT(hicpp-noexcept-move,performance-noexcept-move-constructor) + ~binary_reader() = default; + + /*! + @param[in] format the binary format to parse + @param[in] sax_ a SAX event processor + @param[in] strict whether to expect the input to be consumed completed + @param[in] tag_handler how to treat CBOR tags + + @return whether parsing was successful + */ + JSON_HEDLEY_NON_NULL(3) + bool sax_parse(const input_format_t format, + json_sax_t* sax_, + const bool strict = true, + const cbor_tag_handler_t tag_handler = cbor_tag_handler_t::error) + { + sax = sax_; + bool result = false; + + switch (format) + { + case input_format_t::bson: + result = parse_bson_internal(); + break; + + case input_format_t::cbor: + result = parse_cbor_internal(true, tag_handler); + break; + + case input_format_t::msgpack: + result = parse_msgpack_internal(); + break; + + case input_format_t::ubjson: + case input_format_t::bjdata: + result = parse_ubjson_internal(); + break; + + case input_format_t::json: // LCOV_EXCL_LINE + default: // LCOV_EXCL_LINE + JSON_ASSERT(false); // NOLINT(cert-dcl03-c,hicpp-static-assert,misc-static-assert) LCOV_EXCL_LINE + } + + // strict mode: next byte must be EOF + if (result && strict) + { + if (input_format == input_format_t::ubjson || input_format == input_format_t::bjdata) + { + get_ignore_noop(); + } + else + { + get(); + } + + if (JSON_HEDLEY_UNLIKELY(current != char_traits::eof())) + { + return sax->parse_error(chars_read, get_token_string(), parse_error::create(110, chars_read, + exception_message(input_format, concat("expected end of input; last byte: 0x", get_token_string()), "value"), nullptr)); + } + } + + return result; + } + + private: + ////////// + // BSON // + ////////// + + /*! + @brief Reads in a BSON-object and passes it to the SAX-parser. + @return whether a valid BSON-value was passed to the SAX parser + */ + bool parse_bson_internal() + { + std::int32_t document_size{}; + get_number(input_format_t::bson, document_size); + + if (JSON_HEDLEY_UNLIKELY(!sax->start_object(detail::unknown_size()))) + { + return false; + } + + if (JSON_HEDLEY_UNLIKELY(!parse_bson_element_list(/*is_array*/false))) + { + return false; + } + + return sax->end_object(); + } + + /*! + @brief Parses a C-style string from the BSON input. + @param[in,out] result A reference to the string variable where the read + string is to be stored. + @return `true` if the \x00-byte indicating the end of the string was + encountered before the EOF; false` indicates an unexpected EOF. + */ + bool get_bson_cstr(string_t& result) + { + auto out = std::back_inserter(result); + while (true) + { + get(); + if (JSON_HEDLEY_UNLIKELY(!unexpect_eof(input_format_t::bson, "cstring"))) + { + return false; + } + if (current == 0x00) + { + return true; + } + *out++ = static_cast(current); + } + } + + /*! + @brief Parses a zero-terminated string of length @a len from the BSON + input. + @param[in] len The length (including the zero-byte at the end) of the + string to be read. + @param[in,out] result A reference to the string variable where the read + string is to be stored. + @tparam NumberType The type of the length @a len + @pre len >= 1 + @return `true` if the string was successfully parsed + */ + template + bool get_bson_string(const NumberType len, string_t& result) + { + if (JSON_HEDLEY_UNLIKELY(len < 1)) + { + auto last_token = get_token_string(); + return sax->parse_error(chars_read, last_token, parse_error::create(112, chars_read, + exception_message(input_format_t::bson, concat("string length must be at least 1, is ", std::to_string(len)), "string"), nullptr)); + } + + return get_string(input_format_t::bson, len - static_cast(1), result) && get() != char_traits::eof(); + } + + /*! + @brief Parses a byte array input of length @a len from the BSON input. + @param[in] len The length of the byte array to be read. + @param[in,out] result A reference to the binary variable where the read + array is to be stored. + @tparam NumberType The type of the length @a len + @pre len >= 0 + @return `true` if the byte array was successfully parsed + */ + template + bool get_bson_binary(const NumberType len, binary_t& result) + { + if (JSON_HEDLEY_UNLIKELY(len < 0)) + { + auto last_token = get_token_string(); + return sax->parse_error(chars_read, last_token, parse_error::create(112, chars_read, + exception_message(input_format_t::bson, concat("byte array length cannot be negative, is ", std::to_string(len)), "binary"), nullptr)); + } + + // All BSON binary values have a subtype + std::uint8_t subtype{}; + get_number(input_format_t::bson, subtype); + result.set_subtype(subtype); + + return get_binary(input_format_t::bson, len, result); + } + + /*! + @brief Read a BSON document element of the given @a element_type. + @param[in] element_type The BSON element type, c.f. http://bsonspec.org/spec.html + @param[in] element_type_parse_position The position in the input stream, + where the `element_type` was read. + @warning Not all BSON element types are supported yet. An unsupported + @a element_type will give rise to a parse_error.114: + Unsupported BSON record type 0x... + @return whether a valid BSON-object/array was passed to the SAX parser + */ + bool parse_bson_element_internal(const char_int_type element_type, + const std::size_t element_type_parse_position) + { + switch (element_type) + { + case 0x01: // double + { + double number{}; + return get_number(input_format_t::bson, number) && sax->number_float(static_cast(number), ""); + } + + case 0x02: // string + { + std::int32_t len{}; + string_t value; + return get_number(input_format_t::bson, len) && get_bson_string(len, value) && sax->string(value); + } + + case 0x03: // object + { + return parse_bson_internal(); + } + + case 0x04: // array + { + return parse_bson_array(); + } + + case 0x05: // binary + { + std::int32_t len{}; + binary_t value; + return get_number(input_format_t::bson, len) && get_bson_binary(len, value) && sax->binary(value); + } + + case 0x08: // boolean + { + return sax->boolean(get() != 0); + } + + case 0x0A: // null + { + return sax->null(); + } + + case 0x10: // int32 + { + std::int32_t value{}; + return get_number(input_format_t::bson, value) && sax->number_integer(value); + } + + case 0x12: // int64 + { + std::int64_t value{}; + return get_number(input_format_t::bson, value) && sax->number_integer(value); + } + + case 0x11: // uint64 + { + std::uint64_t value{}; + return get_number(input_format_t::bson, value) && sax->number_unsigned(value); + } + + default: // anything else is not supported (yet) + { + std::array cr{{}}; + static_cast((std::snprintf)(cr.data(), cr.size(), "%.2hhX", static_cast(element_type))); // NOLINT(cppcoreguidelines-pro-type-vararg,hicpp-vararg) + const std::string cr_str{cr.data()}; + return sax->parse_error(element_type_parse_position, cr_str, + parse_error::create(114, element_type_parse_position, concat("Unsupported BSON record type 0x", cr_str), nullptr)); + } + } + } + + /*! + @brief Read a BSON element list (as specified in the BSON-spec) + + The same binary layout is used for objects and arrays, hence it must be + indicated with the argument @a is_array which one is expected + (true --> array, false --> object). + + @param[in] is_array Determines if the element list being read is to be + treated as an object (@a is_array == false), or as an + array (@a is_array == true). + @return whether a valid BSON-object/array was passed to the SAX parser + */ + bool parse_bson_element_list(const bool is_array) + { + string_t key; + + while (auto element_type = get()) + { + if (JSON_HEDLEY_UNLIKELY(!unexpect_eof(input_format_t::bson, "element list"))) + { + return false; + } + + const std::size_t element_type_parse_position = chars_read; + if (JSON_HEDLEY_UNLIKELY(!get_bson_cstr(key))) + { + return false; + } + + if (!is_array && !sax->key(key)) + { + return false; + } + + if (JSON_HEDLEY_UNLIKELY(!parse_bson_element_internal(element_type, element_type_parse_position))) + { + return false; + } + + // get_bson_cstr only appends + key.clear(); + } + + return true; + } + + /*! + @brief Reads an array from the BSON input and passes it to the SAX-parser. + @return whether a valid BSON-array was passed to the SAX parser + */ + bool parse_bson_array() + { + std::int32_t document_size{}; + get_number(input_format_t::bson, document_size); + + if (JSON_HEDLEY_UNLIKELY(!sax->start_array(detail::unknown_size()))) + { + return false; + } + + if (JSON_HEDLEY_UNLIKELY(!parse_bson_element_list(/*is_array*/true))) + { + return false; + } + + return sax->end_array(); + } + + ////////// + // CBOR // + ////////// + + /*! + @param[in] get_char whether a new character should be retrieved from the + input (true) or whether the last read character should + be considered instead (false) + @param[in] tag_handler how CBOR tags should be treated + + @return whether a valid CBOR value was passed to the SAX parser + */ + bool parse_cbor_internal(const bool get_char, + const cbor_tag_handler_t tag_handler) + { + switch (get_char ? get() : current) + { + // EOF + case char_traits::eof(): + return unexpect_eof(input_format_t::cbor, "value"); + + // Integer 0x00..0x17 (0..23) + case 0x00: + case 0x01: + case 0x02: + case 0x03: + case 0x04: + case 0x05: + case 0x06: + case 0x07: + case 0x08: + case 0x09: + case 0x0A: + case 0x0B: + case 0x0C: + case 0x0D: + case 0x0E: + case 0x0F: + case 0x10: + case 0x11: + case 0x12: + case 0x13: + case 0x14: + case 0x15: + case 0x16: + case 0x17: + return sax->number_unsigned(static_cast(current)); + + case 0x18: // Unsigned integer (one-byte uint8_t follows) + { + std::uint8_t number{}; + return get_number(input_format_t::cbor, number) && sax->number_unsigned(number); + } + + case 0x19: // Unsigned integer (two-byte uint16_t follows) + { + std::uint16_t number{}; + return get_number(input_format_t::cbor, number) && sax->number_unsigned(number); + } + + case 0x1A: // Unsigned integer (four-byte uint32_t follows) + { + std::uint32_t number{}; + return get_number(input_format_t::cbor, number) && sax->number_unsigned(number); + } + + case 0x1B: // Unsigned integer (eight-byte uint64_t follows) + { + std::uint64_t number{}; + return get_number(input_format_t::cbor, number) && sax->number_unsigned(number); + } + + // Negative integer -1-0x00..-1-0x17 (-1..-24) + case 0x20: + case 0x21: + case 0x22: + case 0x23: + case 0x24: + case 0x25: + case 0x26: + case 0x27: + case 0x28: + case 0x29: + case 0x2A: + case 0x2B: + case 0x2C: + case 0x2D: + case 0x2E: + case 0x2F: + case 0x30: + case 0x31: + case 0x32: + case 0x33: + case 0x34: + case 0x35: + case 0x36: + case 0x37: + return sax->number_integer(static_cast(0x20 - 1 - current)); + + case 0x38: // Negative integer (one-byte uint8_t follows) + { + std::uint8_t number{}; + return get_number(input_format_t::cbor, number) && sax->number_integer(static_cast(-1) - number); + } + + case 0x39: // Negative integer -1-n (two-byte uint16_t follows) + { + std::uint16_t number{}; + return get_number(input_format_t::cbor, number) && sax->number_integer(static_cast(-1) - number); + } + + case 0x3A: // Negative integer -1-n (four-byte uint32_t follows) + { + std::uint32_t number{}; + return get_number(input_format_t::cbor, number) && sax->number_integer(static_cast(-1) - number); + } + + case 0x3B: // Negative integer -1-n (eight-byte uint64_t follows) + { + std::uint64_t number{}; + return get_number(input_format_t::cbor, number) && sax->number_integer(static_cast(-1) + - static_cast(number)); + } + + // Binary data (0x00..0x17 bytes follow) + case 0x40: + case 0x41: + case 0x42: + case 0x43: + case 0x44: + case 0x45: + case 0x46: + case 0x47: + case 0x48: + case 0x49: + case 0x4A: + case 0x4B: + case 0x4C: + case 0x4D: + case 0x4E: + case 0x4F: + case 0x50: + case 0x51: + case 0x52: + case 0x53: + case 0x54: + case 0x55: + case 0x56: + case 0x57: + case 0x58: // Binary data (one-byte uint8_t for n follows) + case 0x59: // Binary data (two-byte uint16_t for n follow) + case 0x5A: // Binary data (four-byte uint32_t for n follow) + case 0x5B: // Binary data (eight-byte uint64_t for n follow) + case 0x5F: // Binary data (indefinite length) + { + binary_t b; + return get_cbor_binary(b) && sax->binary(b); + } + + // UTF-8 string (0x00..0x17 bytes follow) + case 0x60: + case 0x61: + case 0x62: + case 0x63: + case 0x64: + case 0x65: + case 0x66: + case 0x67: + case 0x68: + case 0x69: + case 0x6A: + case 0x6B: + case 0x6C: + case 0x6D: + case 0x6E: + case 0x6F: + case 0x70: + case 0x71: + case 0x72: + case 0x73: + case 0x74: + case 0x75: + case 0x76: + case 0x77: + case 0x78: // UTF-8 string (one-byte uint8_t for n follows) + case 0x79: // UTF-8 string (two-byte uint16_t for n follow) + case 0x7A: // UTF-8 string (four-byte uint32_t for n follow) + case 0x7B: // UTF-8 string (eight-byte uint64_t for n follow) + case 0x7F: // UTF-8 string (indefinite length) + { + string_t s; + return get_cbor_string(s) && sax->string(s); + } + + // array (0x00..0x17 data items follow) + case 0x80: + case 0x81: + case 0x82: + case 0x83: + case 0x84: + case 0x85: + case 0x86: + case 0x87: + case 0x88: + case 0x89: + case 0x8A: + case 0x8B: + case 0x8C: + case 0x8D: + case 0x8E: + case 0x8F: + case 0x90: + case 0x91: + case 0x92: + case 0x93: + case 0x94: + case 0x95: + case 0x96: + case 0x97: + return get_cbor_array( + conditional_static_cast(static_cast(current) & 0x1Fu), tag_handler); + + case 0x98: // array (one-byte uint8_t for n follows) + { + std::uint8_t len{}; + return get_number(input_format_t::cbor, len) && get_cbor_array(static_cast(len), tag_handler); + } + + case 0x99: // array (two-byte uint16_t for n follow) + { + std::uint16_t len{}; + return get_number(input_format_t::cbor, len) && get_cbor_array(static_cast(len), tag_handler); + } + + case 0x9A: // array (four-byte uint32_t for n follow) + { + std::uint32_t len{}; + return get_number(input_format_t::cbor, len) && get_cbor_array(conditional_static_cast(len), tag_handler); + } + + case 0x9B: // array (eight-byte uint64_t for n follow) + { + std::uint64_t len{}; + return get_number(input_format_t::cbor, len) && get_cbor_array(conditional_static_cast(len), tag_handler); + } + + case 0x9F: // array (indefinite length) + return get_cbor_array(detail::unknown_size(), tag_handler); + + // map (0x00..0x17 pairs of data items follow) + case 0xA0: + case 0xA1: + case 0xA2: + case 0xA3: + case 0xA4: + case 0xA5: + case 0xA6: + case 0xA7: + case 0xA8: + case 0xA9: + case 0xAA: + case 0xAB: + case 0xAC: + case 0xAD: + case 0xAE: + case 0xAF: + case 0xB0: + case 0xB1: + case 0xB2: + case 0xB3: + case 0xB4: + case 0xB5: + case 0xB6: + case 0xB7: + return get_cbor_object(conditional_static_cast(static_cast(current) & 0x1Fu), tag_handler); + + case 0xB8: // map (one-byte uint8_t for n follows) + { + std::uint8_t len{}; + return get_number(input_format_t::cbor, len) && get_cbor_object(static_cast(len), tag_handler); + } + + case 0xB9: // map (two-byte uint16_t for n follow) + { + std::uint16_t len{}; + return get_number(input_format_t::cbor, len) && get_cbor_object(static_cast(len), tag_handler); + } + + case 0xBA: // map (four-byte uint32_t for n follow) + { + std::uint32_t len{}; + return get_number(input_format_t::cbor, len) && get_cbor_object(conditional_static_cast(len), tag_handler); + } + + case 0xBB: // map (eight-byte uint64_t for n follow) + { + std::uint64_t len{}; + return get_number(input_format_t::cbor, len) && get_cbor_object(conditional_static_cast(len), tag_handler); + } + + case 0xBF: // map (indefinite length) + return get_cbor_object(detail::unknown_size(), tag_handler); + + case 0xC6: // tagged item + case 0xC7: + case 0xC8: + case 0xC9: + case 0xCA: + case 0xCB: + case 0xCC: + case 0xCD: + case 0xCE: + case 0xCF: + case 0xD0: + case 0xD1: + case 0xD2: + case 0xD3: + case 0xD4: + case 0xD8: // tagged item (1 byte follows) + case 0xD9: // tagged item (2 bytes follow) + case 0xDA: // tagged item (4 bytes follow) + case 0xDB: // tagged item (8 bytes follow) + { + switch (tag_handler) + { + case cbor_tag_handler_t::error: + { + auto last_token = get_token_string(); + return sax->parse_error(chars_read, last_token, parse_error::create(112, chars_read, + exception_message(input_format_t::cbor, concat("invalid byte: 0x", last_token), "value"), nullptr)); + } + + case cbor_tag_handler_t::ignore: + { + // ignore binary subtype + switch (current) + { + case 0xD8: + { + std::uint8_t subtype_to_ignore{}; + get_number(input_format_t::cbor, subtype_to_ignore); + break; + } + case 0xD9: + { + std::uint16_t subtype_to_ignore{}; + get_number(input_format_t::cbor, subtype_to_ignore); + break; + } + case 0xDA: + { + std::uint32_t subtype_to_ignore{}; + get_number(input_format_t::cbor, subtype_to_ignore); + break; + } + case 0xDB: + { + std::uint64_t subtype_to_ignore{}; + get_number(input_format_t::cbor, subtype_to_ignore); + break; + } + default: + break; + } + return parse_cbor_internal(true, tag_handler); + } + + case cbor_tag_handler_t::store: + { + binary_t b; + // use binary subtype and store in a binary container + switch (current) + { + case 0xD8: + { + std::uint8_t subtype{}; + get_number(input_format_t::cbor, subtype); + b.set_subtype(detail::conditional_static_cast(subtype)); + break; + } + case 0xD9: + { + std::uint16_t subtype{}; + get_number(input_format_t::cbor, subtype); + b.set_subtype(detail::conditional_static_cast(subtype)); + break; + } + case 0xDA: + { + std::uint32_t subtype{}; + get_number(input_format_t::cbor, subtype); + b.set_subtype(detail::conditional_static_cast(subtype)); + break; + } + case 0xDB: + { + std::uint64_t subtype{}; + get_number(input_format_t::cbor, subtype); + b.set_subtype(detail::conditional_static_cast(subtype)); + break; + } + default: + return parse_cbor_internal(true, tag_handler); + } + get(); + return get_cbor_binary(b) && sax->binary(b); + } + + default: // LCOV_EXCL_LINE + JSON_ASSERT(false); // NOLINT(cert-dcl03-c,hicpp-static-assert,misc-static-assert) LCOV_EXCL_LINE + return false; // LCOV_EXCL_LINE + } + } + + case 0xF4: // false + return sax->boolean(false); + + case 0xF5: // true + return sax->boolean(true); + + case 0xF6: // null + return sax->null(); + + case 0xF9: // Half-Precision Float (two-byte IEEE 754) + { + const auto byte1_raw = get(); + if (JSON_HEDLEY_UNLIKELY(!unexpect_eof(input_format_t::cbor, "number"))) + { + return false; + } + const auto byte2_raw = get(); + if (JSON_HEDLEY_UNLIKELY(!unexpect_eof(input_format_t::cbor, "number"))) + { + return false; + } + + const auto byte1 = static_cast(byte1_raw); + const auto byte2 = static_cast(byte2_raw); + + // Code from RFC 7049, Appendix D, Figure 3: + // As half-precision floating-point numbers were only added + // to IEEE 754 in 2008, today's programming platforms often + // still only have limited support for them. It is very + // easy to include at least decoding support for them even + // without such support. An example of a small decoder for + // half-precision floating-point numbers in the C language + // is shown in Fig. 3. + const auto half = static_cast((byte1 << 8u) + byte2); + const double val = [&half] + { + const int exp = (half >> 10u) & 0x1Fu; + const unsigned int mant = half & 0x3FFu; + JSON_ASSERT(0 <= exp&& exp <= 32); + JSON_ASSERT(mant <= 1024); + switch (exp) + { + case 0: + return std::ldexp(mant, -24); + case 31: + return (mant == 0) + ? std::numeric_limits::infinity() + : std::numeric_limits::quiet_NaN(); + default: + return std::ldexp(mant + 1024, exp - 25); + } + }(); + return sax->number_float((half & 0x8000u) != 0 + ? static_cast(-val) + : static_cast(val), ""); + } + + case 0xFA: // Single-Precision Float (four-byte IEEE 754) + { + float number{}; + return get_number(input_format_t::cbor, number) && sax->number_float(static_cast(number), ""); + } + + case 0xFB: // Double-Precision Float (eight-byte IEEE 754) + { + double number{}; + return get_number(input_format_t::cbor, number) && sax->number_float(static_cast(number), ""); + } + + default: // anything else (0xFF is handled inside the other types) + { + auto last_token = get_token_string(); + return sax->parse_error(chars_read, last_token, parse_error::create(112, chars_read, + exception_message(input_format_t::cbor, concat("invalid byte: 0x", last_token), "value"), nullptr)); + } + } + } + + /*! + @brief reads a CBOR string + + This function first reads starting bytes to determine the expected + string length and then copies this number of bytes into a string. + Additionally, CBOR's strings with indefinite lengths are supported. + + @param[out] result created string + + @return whether string creation completed + */ + bool get_cbor_string(string_t& result) + { + if (JSON_HEDLEY_UNLIKELY(!unexpect_eof(input_format_t::cbor, "string"))) + { + return false; + } + + switch (current) + { + // UTF-8 string (0x00..0x17 bytes follow) + case 0x60: + case 0x61: + case 0x62: + case 0x63: + case 0x64: + case 0x65: + case 0x66: + case 0x67: + case 0x68: + case 0x69: + case 0x6A: + case 0x6B: + case 0x6C: + case 0x6D: + case 0x6E: + case 0x6F: + case 0x70: + case 0x71: + case 0x72: + case 0x73: + case 0x74: + case 0x75: + case 0x76: + case 0x77: + { + return get_string(input_format_t::cbor, static_cast(current) & 0x1Fu, result); + } + + case 0x78: // UTF-8 string (one-byte uint8_t for n follows) + { + std::uint8_t len{}; + return get_number(input_format_t::cbor, len) && get_string(input_format_t::cbor, len, result); + } + + case 0x79: // UTF-8 string (two-byte uint16_t for n follow) + { + std::uint16_t len{}; + return get_number(input_format_t::cbor, len) && get_string(input_format_t::cbor, len, result); + } + + case 0x7A: // UTF-8 string (four-byte uint32_t for n follow) + { + std::uint32_t len{}; + return get_number(input_format_t::cbor, len) && get_string(input_format_t::cbor, len, result); + } + + case 0x7B: // UTF-8 string (eight-byte uint64_t for n follow) + { + std::uint64_t len{}; + return get_number(input_format_t::cbor, len) && get_string(input_format_t::cbor, len, result); + } + + case 0x7F: // UTF-8 string (indefinite length) + { + while (get() != 0xFF) + { + string_t chunk; + if (!get_cbor_string(chunk)) + { + return false; + } + result.append(chunk); + } + return true; + } + + default: + { + auto last_token = get_token_string(); + return sax->parse_error(chars_read, last_token, parse_error::create(113, chars_read, + exception_message(input_format_t::cbor, concat("expected length specification (0x60-0x7B) or indefinite string type (0x7F); last byte: 0x", last_token), "string"), nullptr)); + } + } + } + + /*! + @brief reads a CBOR byte array + + This function first reads starting bytes to determine the expected + byte array length and then copies this number of bytes into the byte array. + Additionally, CBOR's byte arrays with indefinite lengths are supported. + + @param[out] result created byte array + + @return whether byte array creation completed + */ + bool get_cbor_binary(binary_t& result) + { + if (JSON_HEDLEY_UNLIKELY(!unexpect_eof(input_format_t::cbor, "binary"))) + { + return false; + } + + switch (current) + { + // Binary data (0x00..0x17 bytes follow) + case 0x40: + case 0x41: + case 0x42: + case 0x43: + case 0x44: + case 0x45: + case 0x46: + case 0x47: + case 0x48: + case 0x49: + case 0x4A: + case 0x4B: + case 0x4C: + case 0x4D: + case 0x4E: + case 0x4F: + case 0x50: + case 0x51: + case 0x52: + case 0x53: + case 0x54: + case 0x55: + case 0x56: + case 0x57: + { + return get_binary(input_format_t::cbor, static_cast(current) & 0x1Fu, result); + } + + case 0x58: // Binary data (one-byte uint8_t for n follows) + { + std::uint8_t len{}; + return get_number(input_format_t::cbor, len) && + get_binary(input_format_t::cbor, len, result); + } + + case 0x59: // Binary data (two-byte uint16_t for n follow) + { + std::uint16_t len{}; + return get_number(input_format_t::cbor, len) && + get_binary(input_format_t::cbor, len, result); + } + + case 0x5A: // Binary data (four-byte uint32_t for n follow) + { + std::uint32_t len{}; + return get_number(input_format_t::cbor, len) && + get_binary(input_format_t::cbor, len, result); + } + + case 0x5B: // Binary data (eight-byte uint64_t for n follow) + { + std::uint64_t len{}; + return get_number(input_format_t::cbor, len) && + get_binary(input_format_t::cbor, len, result); + } + + case 0x5F: // Binary data (indefinite length) + { + while (get() != 0xFF) + { + binary_t chunk; + if (!get_cbor_binary(chunk)) + { + return false; + } + result.insert(result.end(), chunk.begin(), chunk.end()); + } + return true; + } + + default: + { + auto last_token = get_token_string(); + return sax->parse_error(chars_read, last_token, parse_error::create(113, chars_read, + exception_message(input_format_t::cbor, concat("expected length specification (0x40-0x5B) or indefinite binary array type (0x5F); last byte: 0x", last_token), "binary"), nullptr)); + } + } + } + + /*! + @param[in] len the length of the array or detail::unknown_size() for an + array of indefinite size + @param[in] tag_handler how CBOR tags should be treated + @return whether array creation completed + */ + bool get_cbor_array(const std::size_t len, + const cbor_tag_handler_t tag_handler) + { + if (JSON_HEDLEY_UNLIKELY(!sax->start_array(len))) + { + return false; + } + + if (len != detail::unknown_size()) + { + for (std::size_t i = 0; i < len; ++i) + { + if (JSON_HEDLEY_UNLIKELY(!parse_cbor_internal(true, tag_handler))) + { + return false; + } + } + } + else + { + while (get() != 0xFF) + { + if (JSON_HEDLEY_UNLIKELY(!parse_cbor_internal(false, tag_handler))) + { + return false; + } + } + } + + return sax->end_array(); + } + + /*! + @param[in] len the length of the object or detail::unknown_size() for an + object of indefinite size + @param[in] tag_handler how CBOR tags should be treated + @return whether object creation completed + */ + bool get_cbor_object(const std::size_t len, + const cbor_tag_handler_t tag_handler) + { + if (JSON_HEDLEY_UNLIKELY(!sax->start_object(len))) + { + return false; + } + + if (len != 0) + { + string_t key; + if (len != detail::unknown_size()) + { + for (std::size_t i = 0; i < len; ++i) + { + get(); + if (JSON_HEDLEY_UNLIKELY(!get_cbor_string(key) || !sax->key(key))) + { + return false; + } + + if (JSON_HEDLEY_UNLIKELY(!parse_cbor_internal(true, tag_handler))) + { + return false; + } + key.clear(); + } + } + else + { + while (get() != 0xFF) + { + if (JSON_HEDLEY_UNLIKELY(!get_cbor_string(key) || !sax->key(key))) + { + return false; + } + + if (JSON_HEDLEY_UNLIKELY(!parse_cbor_internal(true, tag_handler))) + { + return false; + } + key.clear(); + } + } + } + + return sax->end_object(); + } + + ///////////// + // MsgPack // + ///////////// + + /*! + @return whether a valid MessagePack value was passed to the SAX parser + */ + bool parse_msgpack_internal() + { + switch (get()) + { + // EOF + case char_traits::eof(): + return unexpect_eof(input_format_t::msgpack, "value"); + + // positive fixint + case 0x00: + case 0x01: + case 0x02: + case 0x03: + case 0x04: + case 0x05: + case 0x06: + case 0x07: + case 0x08: + case 0x09: + case 0x0A: + case 0x0B: + case 0x0C: + case 0x0D: + case 0x0E: + case 0x0F: + case 0x10: + case 0x11: + case 0x12: + case 0x13: + case 0x14: + case 0x15: + case 0x16: + case 0x17: + case 0x18: + case 0x19: + case 0x1A: + case 0x1B: + case 0x1C: + case 0x1D: + case 0x1E: + case 0x1F: + case 0x20: + case 0x21: + case 0x22: + case 0x23: + case 0x24: + case 0x25: + case 0x26: + case 0x27: + case 0x28: + case 0x29: + case 0x2A: + case 0x2B: + case 0x2C: + case 0x2D: + case 0x2E: + case 0x2F: + case 0x30: + case 0x31: + case 0x32: + case 0x33: + case 0x34: + case 0x35: + case 0x36: + case 0x37: + case 0x38: + case 0x39: + case 0x3A: + case 0x3B: + case 0x3C: + case 0x3D: + case 0x3E: + case 0x3F: + case 0x40: + case 0x41: + case 0x42: + case 0x43: + case 0x44: + case 0x45: + case 0x46: + case 0x47: + case 0x48: + case 0x49: + case 0x4A: + case 0x4B: + case 0x4C: + case 0x4D: + case 0x4E: + case 0x4F: + case 0x50: + case 0x51: + case 0x52: + case 0x53: + case 0x54: + case 0x55: + case 0x56: + case 0x57: + case 0x58: + case 0x59: + case 0x5A: + case 0x5B: + case 0x5C: + case 0x5D: + case 0x5E: + case 0x5F: + case 0x60: + case 0x61: + case 0x62: + case 0x63: + case 0x64: + case 0x65: + case 0x66: + case 0x67: + case 0x68: + case 0x69: + case 0x6A: + case 0x6B: + case 0x6C: + case 0x6D: + case 0x6E: + case 0x6F: + case 0x70: + case 0x71: + case 0x72: + case 0x73: + case 0x74: + case 0x75: + case 0x76: + case 0x77: + case 0x78: + case 0x79: + case 0x7A: + case 0x7B: + case 0x7C: + case 0x7D: + case 0x7E: + case 0x7F: + return sax->number_unsigned(static_cast(current)); + + // fixmap + case 0x80: + case 0x81: + case 0x82: + case 0x83: + case 0x84: + case 0x85: + case 0x86: + case 0x87: + case 0x88: + case 0x89: + case 0x8A: + case 0x8B: + case 0x8C: + case 0x8D: + case 0x8E: + case 0x8F: + return get_msgpack_object(conditional_static_cast(static_cast(current) & 0x0Fu)); + + // fixarray + case 0x90: + case 0x91: + case 0x92: + case 0x93: + case 0x94: + case 0x95: + case 0x96: + case 0x97: + case 0x98: + case 0x99: + case 0x9A: + case 0x9B: + case 0x9C: + case 0x9D: + case 0x9E: + case 0x9F: + return get_msgpack_array(conditional_static_cast(static_cast(current) & 0x0Fu)); + + // fixstr + case 0xA0: + case 0xA1: + case 0xA2: + case 0xA3: + case 0xA4: + case 0xA5: + case 0xA6: + case 0xA7: + case 0xA8: + case 0xA9: + case 0xAA: + case 0xAB: + case 0xAC: + case 0xAD: + case 0xAE: + case 0xAF: + case 0xB0: + case 0xB1: + case 0xB2: + case 0xB3: + case 0xB4: + case 0xB5: + case 0xB6: + case 0xB7: + case 0xB8: + case 0xB9: + case 0xBA: + case 0xBB: + case 0xBC: + case 0xBD: + case 0xBE: + case 0xBF: + case 0xD9: // str 8 + case 0xDA: // str 16 + case 0xDB: // str 32 + { + string_t s; + return get_msgpack_string(s) && sax->string(s); + } + + case 0xC0: // nil + return sax->null(); + + case 0xC2: // false + return sax->boolean(false); + + case 0xC3: // true + return sax->boolean(true); + + case 0xC4: // bin 8 + case 0xC5: // bin 16 + case 0xC6: // bin 32 + case 0xC7: // ext 8 + case 0xC8: // ext 16 + case 0xC9: // ext 32 + case 0xD4: // fixext 1 + case 0xD5: // fixext 2 + case 0xD6: // fixext 4 + case 0xD7: // fixext 8 + case 0xD8: // fixext 16 + { + binary_t b; + return get_msgpack_binary(b) && sax->binary(b); + } + + case 0xCA: // float 32 + { + float number{}; + return get_number(input_format_t::msgpack, number) && sax->number_float(static_cast(number), ""); + } + + case 0xCB: // float 64 + { + double number{}; + return get_number(input_format_t::msgpack, number) && sax->number_float(static_cast(number), ""); + } + + case 0xCC: // uint 8 + { + std::uint8_t number{}; + return get_number(input_format_t::msgpack, number) && sax->number_unsigned(number); + } + + case 0xCD: // uint 16 + { + std::uint16_t number{}; + return get_number(input_format_t::msgpack, number) && sax->number_unsigned(number); + } + + case 0xCE: // uint 32 + { + std::uint32_t number{}; + return get_number(input_format_t::msgpack, number) && sax->number_unsigned(number); + } + + case 0xCF: // uint 64 + { + std::uint64_t number{}; + return get_number(input_format_t::msgpack, number) && sax->number_unsigned(number); + } + + case 0xD0: // int 8 + { + std::int8_t number{}; + return get_number(input_format_t::msgpack, number) && sax->number_integer(number); + } + + case 0xD1: // int 16 + { + std::int16_t number{}; + return get_number(input_format_t::msgpack, number) && sax->number_integer(number); + } + + case 0xD2: // int 32 + { + std::int32_t number{}; + return get_number(input_format_t::msgpack, number) && sax->number_integer(number); + } + + case 0xD3: // int 64 + { + std::int64_t number{}; + return get_number(input_format_t::msgpack, number) && sax->number_integer(number); + } + + case 0xDC: // array 16 + { + std::uint16_t len{}; + return get_number(input_format_t::msgpack, len) && get_msgpack_array(static_cast(len)); + } + + case 0xDD: // array 32 + { + std::uint32_t len{}; + return get_number(input_format_t::msgpack, len) && get_msgpack_array(conditional_static_cast(len)); + } + + case 0xDE: // map 16 + { + std::uint16_t len{}; + return get_number(input_format_t::msgpack, len) && get_msgpack_object(static_cast(len)); + } + + case 0xDF: // map 32 + { + std::uint32_t len{}; + return get_number(input_format_t::msgpack, len) && get_msgpack_object(conditional_static_cast(len)); + } + + // negative fixint + case 0xE0: + case 0xE1: + case 0xE2: + case 0xE3: + case 0xE4: + case 0xE5: + case 0xE6: + case 0xE7: + case 0xE8: + case 0xE9: + case 0xEA: + case 0xEB: + case 0xEC: + case 0xED: + case 0xEE: + case 0xEF: + case 0xF0: + case 0xF1: + case 0xF2: + case 0xF3: + case 0xF4: + case 0xF5: + case 0xF6: + case 0xF7: + case 0xF8: + case 0xF9: + case 0xFA: + case 0xFB: + case 0xFC: + case 0xFD: + case 0xFE: + case 0xFF: + return sax->number_integer(static_cast(current)); + + default: // anything else + { + auto last_token = get_token_string(); + return sax->parse_error(chars_read, last_token, parse_error::create(112, chars_read, + exception_message(input_format_t::msgpack, concat("invalid byte: 0x", last_token), "value"), nullptr)); + } + } + } + + /*! + @brief reads a MessagePack string + + This function first reads starting bytes to determine the expected + string length and then copies this number of bytes into a string. + + @param[out] result created string + + @return whether string creation completed + */ + bool get_msgpack_string(string_t& result) + { + if (JSON_HEDLEY_UNLIKELY(!unexpect_eof(input_format_t::msgpack, "string"))) + { + return false; + } + + switch (current) + { + // fixstr + case 0xA0: + case 0xA1: + case 0xA2: + case 0xA3: + case 0xA4: + case 0xA5: + case 0xA6: + case 0xA7: + case 0xA8: + case 0xA9: + case 0xAA: + case 0xAB: + case 0xAC: + case 0xAD: + case 0xAE: + case 0xAF: + case 0xB0: + case 0xB1: + case 0xB2: + case 0xB3: + case 0xB4: + case 0xB5: + case 0xB6: + case 0xB7: + case 0xB8: + case 0xB9: + case 0xBA: + case 0xBB: + case 0xBC: + case 0xBD: + case 0xBE: + case 0xBF: + { + return get_string(input_format_t::msgpack, static_cast(current) & 0x1Fu, result); + } + + case 0xD9: // str 8 + { + std::uint8_t len{}; + return get_number(input_format_t::msgpack, len) && get_string(input_format_t::msgpack, len, result); + } + + case 0xDA: // str 16 + { + std::uint16_t len{}; + return get_number(input_format_t::msgpack, len) && get_string(input_format_t::msgpack, len, result); + } + + case 0xDB: // str 32 + { + std::uint32_t len{}; + return get_number(input_format_t::msgpack, len) && get_string(input_format_t::msgpack, len, result); + } + + default: + { + auto last_token = get_token_string(); + return sax->parse_error(chars_read, last_token, parse_error::create(113, chars_read, + exception_message(input_format_t::msgpack, concat("expected length specification (0xA0-0xBF, 0xD9-0xDB); last byte: 0x", last_token), "string"), nullptr)); + } + } + } + + /*! + @brief reads a MessagePack byte array + + This function first reads starting bytes to determine the expected + byte array length and then copies this number of bytes into a byte array. + + @param[out] result created byte array + + @return whether byte array creation completed + */ + bool get_msgpack_binary(binary_t& result) + { + // helper function to set the subtype + auto assign_and_return_true = [&result](std::int8_t subtype) + { + result.set_subtype(static_cast(subtype)); + return true; + }; + + switch (current) + { + case 0xC4: // bin 8 + { + std::uint8_t len{}; + return get_number(input_format_t::msgpack, len) && + get_binary(input_format_t::msgpack, len, result); + } + + case 0xC5: // bin 16 + { + std::uint16_t len{}; + return get_number(input_format_t::msgpack, len) && + get_binary(input_format_t::msgpack, len, result); + } + + case 0xC6: // bin 32 + { + std::uint32_t len{}; + return get_number(input_format_t::msgpack, len) && + get_binary(input_format_t::msgpack, len, result); + } + + case 0xC7: // ext 8 + { + std::uint8_t len{}; + std::int8_t subtype{}; + return get_number(input_format_t::msgpack, len) && + get_number(input_format_t::msgpack, subtype) && + get_binary(input_format_t::msgpack, len, result) && + assign_and_return_true(subtype); + } + + case 0xC8: // ext 16 + { + std::uint16_t len{}; + std::int8_t subtype{}; + return get_number(input_format_t::msgpack, len) && + get_number(input_format_t::msgpack, subtype) && + get_binary(input_format_t::msgpack, len, result) && + assign_and_return_true(subtype); + } + + case 0xC9: // ext 32 + { + std::uint32_t len{}; + std::int8_t subtype{}; + return get_number(input_format_t::msgpack, len) && + get_number(input_format_t::msgpack, subtype) && + get_binary(input_format_t::msgpack, len, result) && + assign_and_return_true(subtype); + } + + case 0xD4: // fixext 1 + { + std::int8_t subtype{}; + return get_number(input_format_t::msgpack, subtype) && + get_binary(input_format_t::msgpack, 1, result) && + assign_and_return_true(subtype); + } + + case 0xD5: // fixext 2 + { + std::int8_t subtype{}; + return get_number(input_format_t::msgpack, subtype) && + get_binary(input_format_t::msgpack, 2, result) && + assign_and_return_true(subtype); + } + + case 0xD6: // fixext 4 + { + std::int8_t subtype{}; + return get_number(input_format_t::msgpack, subtype) && + get_binary(input_format_t::msgpack, 4, result) && + assign_and_return_true(subtype); + } + + case 0xD7: // fixext 8 + { + std::int8_t subtype{}; + return get_number(input_format_t::msgpack, subtype) && + get_binary(input_format_t::msgpack, 8, result) && + assign_and_return_true(subtype); + } + + case 0xD8: // fixext 16 + { + std::int8_t subtype{}; + return get_number(input_format_t::msgpack, subtype) && + get_binary(input_format_t::msgpack, 16, result) && + assign_and_return_true(subtype); + } + + default: // LCOV_EXCL_LINE + return false; // LCOV_EXCL_LINE + } + } + + /*! + @param[in] len the length of the array + @return whether array creation completed + */ + bool get_msgpack_array(const std::size_t len) + { + if (JSON_HEDLEY_UNLIKELY(!sax->start_array(len))) + { + return false; + } + + for (std::size_t i = 0; i < len; ++i) + { + if (JSON_HEDLEY_UNLIKELY(!parse_msgpack_internal())) + { + return false; + } + } + + return sax->end_array(); + } + + /*! + @param[in] len the length of the object + @return whether object creation completed + */ + bool get_msgpack_object(const std::size_t len) + { + if (JSON_HEDLEY_UNLIKELY(!sax->start_object(len))) + { + return false; + } + + string_t key; + for (std::size_t i = 0; i < len; ++i) + { + get(); + if (JSON_HEDLEY_UNLIKELY(!get_msgpack_string(key) || !sax->key(key))) + { + return false; + } + + if (JSON_HEDLEY_UNLIKELY(!parse_msgpack_internal())) + { + return false; + } + key.clear(); + } + + return sax->end_object(); + } + + //////////// + // UBJSON // + //////////// + + /*! + @param[in] get_char whether a new character should be retrieved from the + input (true, default) or whether the last read + character should be considered instead + + @return whether a valid UBJSON value was passed to the SAX parser + */ + bool parse_ubjson_internal(const bool get_char = true) + { + return get_ubjson_value(get_char ? get_ignore_noop() : current); + } + + /*! + @brief reads a UBJSON string + + This function is either called after reading the 'S' byte explicitly + indicating a string, or in case of an object key where the 'S' byte can be + left out. + + @param[out] result created string + @param[in] get_char whether a new character should be retrieved from the + input (true, default) or whether the last read + character should be considered instead + + @return whether string creation completed + */ + bool get_ubjson_string(string_t& result, const bool get_char = true) + { + if (get_char) + { + get(); // TODO(niels): may we ignore N here? + } + + if (JSON_HEDLEY_UNLIKELY(!unexpect_eof(input_format, "value"))) + { + return false; + } + + switch (current) + { + case 'U': + { + std::uint8_t len{}; + return get_number(input_format, len) && get_string(input_format, len, result); + } + + case 'i': + { + std::int8_t len{}; + return get_number(input_format, len) && get_string(input_format, len, result); + } + + case 'I': + { + std::int16_t len{}; + return get_number(input_format, len) && get_string(input_format, len, result); + } + + case 'l': + { + std::int32_t len{}; + return get_number(input_format, len) && get_string(input_format, len, result); + } + + case 'L': + { + std::int64_t len{}; + return get_number(input_format, len) && get_string(input_format, len, result); + } + + case 'u': + { + if (input_format != input_format_t::bjdata) + { + break; + } + std::uint16_t len{}; + return get_number(input_format, len) && get_string(input_format, len, result); + } + + case 'm': + { + if (input_format != input_format_t::bjdata) + { + break; + } + std::uint32_t len{}; + return get_number(input_format, len) && get_string(input_format, len, result); + } + + case 'M': + { + if (input_format != input_format_t::bjdata) + { + break; + } + std::uint64_t len{}; + return get_number(input_format, len) && get_string(input_format, len, result); + } + + default: + break; + } + auto last_token = get_token_string(); + std::string message; + + if (input_format != input_format_t::bjdata) + { + message = "expected length type specification (U, i, I, l, L); last byte: 0x" + last_token; + } + else + { + message = "expected length type specification (U, i, u, I, m, l, M, L); last byte: 0x" + last_token; + } + return sax->parse_error(chars_read, last_token, parse_error::create(113, chars_read, exception_message(input_format, message, "string"), nullptr)); + } + + /*! + @param[out] dim an integer vector storing the ND array dimensions + @return whether reading ND array size vector is successful + */ + bool get_ubjson_ndarray_size(std::vector& dim) + { + std::pair size_and_type; + size_t dimlen = 0; + bool no_ndarray = true; + + if (JSON_HEDLEY_UNLIKELY(!get_ubjson_size_type(size_and_type, no_ndarray))) + { + return false; + } + + if (size_and_type.first != npos) + { + if (size_and_type.second != 0) + { + if (size_and_type.second != 'N') + { + for (std::size_t i = 0; i < size_and_type.first; ++i) + { + if (JSON_HEDLEY_UNLIKELY(!get_ubjson_size_value(dimlen, no_ndarray, size_and_type.second))) + { + return false; + } + dim.push_back(dimlen); + } + } + } + else + { + for (std::size_t i = 0; i < size_and_type.first; ++i) + { + if (JSON_HEDLEY_UNLIKELY(!get_ubjson_size_value(dimlen, no_ndarray))) + { + return false; + } + dim.push_back(dimlen); + } + } + } + else + { + while (current != ']') + { + if (JSON_HEDLEY_UNLIKELY(!get_ubjson_size_value(dimlen, no_ndarray, current))) + { + return false; + } + dim.push_back(dimlen); + get_ignore_noop(); + } + } + return true; + } + + /*! + @param[out] result determined size + @param[in,out] is_ndarray for input, `true` means already inside an ndarray vector + or ndarray dimension is not allowed; `false` means ndarray + is allowed; for output, `true` means an ndarray is found; + is_ndarray can only return `true` when its initial value + is `false` + @param[in] prefix type marker if already read, otherwise set to 0 + + @return whether size determination completed + */ + bool get_ubjson_size_value(std::size_t& result, bool& is_ndarray, char_int_type prefix = 0) + { + if (prefix == 0) + { + prefix = get_ignore_noop(); + } + + switch (prefix) + { + case 'U': + { + std::uint8_t number{}; + if (JSON_HEDLEY_UNLIKELY(!get_number(input_format, number))) + { + return false; + } + result = static_cast(number); + return true; + } + + case 'i': + { + std::int8_t number{}; + if (JSON_HEDLEY_UNLIKELY(!get_number(input_format, number))) + { + return false; + } + if (number < 0) + { + return sax->parse_error(chars_read, get_token_string(), parse_error::create(113, chars_read, + exception_message(input_format, "count in an optimized container must be positive", "size"), nullptr)); + } + result = static_cast(number); // NOLINT(bugprone-signed-char-misuse,cert-str34-c): number is not a char + return true; + } + + case 'I': + { + std::int16_t number{}; + if (JSON_HEDLEY_UNLIKELY(!get_number(input_format, number))) + { + return false; + } + if (number < 0) + { + return sax->parse_error(chars_read, get_token_string(), parse_error::create(113, chars_read, + exception_message(input_format, "count in an optimized container must be positive", "size"), nullptr)); + } + result = static_cast(number); + return true; + } + + case 'l': + { + std::int32_t number{}; + if (JSON_HEDLEY_UNLIKELY(!get_number(input_format, number))) + { + return false; + } + if (number < 0) + { + return sax->parse_error(chars_read, get_token_string(), parse_error::create(113, chars_read, + exception_message(input_format, "count in an optimized container must be positive", "size"), nullptr)); + } + result = static_cast(number); + return true; + } + + case 'L': + { + std::int64_t number{}; + if (JSON_HEDLEY_UNLIKELY(!get_number(input_format, number))) + { + return false; + } + if (number < 0) + { + return sax->parse_error(chars_read, get_token_string(), parse_error::create(113, chars_read, + exception_message(input_format, "count in an optimized container must be positive", "size"), nullptr)); + } + if (!value_in_range_of(number)) + { + return sax->parse_error(chars_read, get_token_string(), out_of_range::create(408, + exception_message(input_format, "integer value overflow", "size"), nullptr)); + } + result = static_cast(number); + return true; + } + + case 'u': + { + if (input_format != input_format_t::bjdata) + { + break; + } + std::uint16_t number{}; + if (JSON_HEDLEY_UNLIKELY(!get_number(input_format, number))) + { + return false; + } + result = static_cast(number); + return true; + } + + case 'm': + { + if (input_format != input_format_t::bjdata) + { + break; + } + std::uint32_t number{}; + if (JSON_HEDLEY_UNLIKELY(!get_number(input_format, number))) + { + return false; + } + result = conditional_static_cast(number); + return true; + } + + case 'M': + { + if (input_format != input_format_t::bjdata) + { + break; + } + std::uint64_t number{}; + if (JSON_HEDLEY_UNLIKELY(!get_number(input_format, number))) + { + return false; + } + if (!value_in_range_of(number)) + { + return sax->parse_error(chars_read, get_token_string(), out_of_range::create(408, + exception_message(input_format, "integer value overflow", "size"), nullptr)); + } + result = detail::conditional_static_cast(number); + return true; + } + + case '[': + { + if (input_format != input_format_t::bjdata) + { + break; + } + if (is_ndarray) // ndarray dimensional vector can only contain integers and cannot embed another array + { + return sax->parse_error(chars_read, get_token_string(), parse_error::create(113, chars_read, exception_message(input_format, "ndarray dimensional vector is not allowed", "size"), nullptr)); + } + std::vector dim; + if (JSON_HEDLEY_UNLIKELY(!get_ubjson_ndarray_size(dim))) + { + return false; + } + if (dim.size() == 1 || (dim.size() == 2 && dim.at(0) == 1)) // return normal array size if 1D row vector + { + result = dim.at(dim.size() - 1); + return true; + } + if (!dim.empty()) // if ndarray, convert to an object in JData annotated array format + { + for (auto i : dim) // test if any dimension in an ndarray is 0, if so, return a 1D empty container + { + if ( i == 0 ) + { + result = 0; + return true; + } + } + + string_t key = "_ArraySize_"; + if (JSON_HEDLEY_UNLIKELY(!sax->start_object(3) || !sax->key(key) || !sax->start_array(dim.size()))) + { + return false; + } + result = 1; + for (auto i : dim) + { + // Pre-multiplication overflow check: if i > 0 and result > SIZE_MAX/i, then result*i would overflow. + // This check must happen before multiplication since overflow detection after the fact is unreliable + // as modular arithmetic can produce any value, not just 0 or SIZE_MAX. + if (JSON_HEDLEY_UNLIKELY(i > 0 && result > (std::numeric_limits::max)() / i)) + { + return sax->parse_error(chars_read, get_token_string(), out_of_range::create(408, exception_message(input_format, "excessive ndarray size caused overflow", "size"), nullptr)); + } + result *= i; + // Additional post-multiplication check to catch any edge cases the pre-check might miss + if (result == 0 || result == npos) + { + return sax->parse_error(chars_read, get_token_string(), out_of_range::create(408, exception_message(input_format, "excessive ndarray size caused overflow", "size"), nullptr)); + } + if (JSON_HEDLEY_UNLIKELY(!sax->number_unsigned(static_cast(i)))) + { + return false; + } + } + is_ndarray = true; + return sax->end_array(); + } + result = 0; + return true; + } + + default: + break; + } + auto last_token = get_token_string(); + std::string message; + + if (input_format != input_format_t::bjdata) + { + message = "expected length type specification (U, i, I, l, L) after '#'; last byte: 0x" + last_token; + } + else + { + message = "expected length type specification (U, i, u, I, m, l, M, L) after '#'; last byte: 0x" + last_token; + } + return sax->parse_error(chars_read, last_token, parse_error::create(113, chars_read, exception_message(input_format, message, "size"), nullptr)); + } + + /*! + @brief determine the type and size for a container + + In the optimized UBJSON format, a type and a size can be provided to allow + for a more compact representation. + + @param[out] result pair of the size and the type + @param[in] inside_ndarray whether the parser is parsing an ND array dimensional vector + + @return whether pair creation completed + */ + bool get_ubjson_size_type(std::pair& result, bool inside_ndarray = false) + { + result.first = npos; // size + result.second = 0; // type + bool is_ndarray = false; + + get_ignore_noop(); + + if (current == '$') + { + result.second = get(); // must not ignore 'N', because 'N' maybe the type + if (input_format == input_format_t::bjdata + && JSON_HEDLEY_UNLIKELY(std::binary_search(bjd_optimized_type_markers.begin(), bjd_optimized_type_markers.end(), result.second))) + { + auto last_token = get_token_string(); + return sax->parse_error(chars_read, last_token, parse_error::create(112, chars_read, + exception_message(input_format, concat("marker 0x", last_token, " is not a permitted optimized array type"), "type"), nullptr)); + } + + if (JSON_HEDLEY_UNLIKELY(!unexpect_eof(input_format, "type"))) + { + return false; + } + + get_ignore_noop(); + if (JSON_HEDLEY_UNLIKELY(current != '#')) + { + if (JSON_HEDLEY_UNLIKELY(!unexpect_eof(input_format, "value"))) + { + return false; + } + auto last_token = get_token_string(); + return sax->parse_error(chars_read, last_token, parse_error::create(112, chars_read, + exception_message(input_format, concat("expected '#' after type information; last byte: 0x", last_token), "size"), nullptr)); + } + + const bool is_error = get_ubjson_size_value(result.first, is_ndarray); + if (input_format == input_format_t::bjdata && is_ndarray) + { + if (inside_ndarray) + { + return sax->parse_error(chars_read, get_token_string(), parse_error::create(112, chars_read, + exception_message(input_format, "ndarray can not be recursive", "size"), nullptr)); + } + result.second |= (1 << 8); // use bit 8 to indicate ndarray, all UBJSON and BJData markers should be ASCII letters + } + return is_error; + } + + if (current == '#') + { + const bool is_error = get_ubjson_size_value(result.first, is_ndarray); + if (input_format == input_format_t::bjdata && is_ndarray) + { + return sax->parse_error(chars_read, get_token_string(), parse_error::create(112, chars_read, + exception_message(input_format, "ndarray requires both type and size", "size"), nullptr)); + } + return is_error; + } + + return true; + } + + /*! + @param prefix the previously read or set type prefix + @return whether value creation completed + */ + bool get_ubjson_value(const char_int_type prefix) + { + switch (prefix) + { + case char_traits::eof(): // EOF + return unexpect_eof(input_format, "value"); + + case 'T': // true + return sax->boolean(true); + case 'F': // false + return sax->boolean(false); + + case 'Z': // null + return sax->null(); + + case 'B': // byte + { + if (input_format != input_format_t::bjdata) + { + break; + } + std::uint8_t number{}; + return get_number(input_format, number) && sax->number_unsigned(number); + } + + case 'U': + { + std::uint8_t number{}; + return get_number(input_format, number) && sax->number_unsigned(number); + } + + case 'i': + { + std::int8_t number{}; + return get_number(input_format, number) && sax->number_integer(number); + } + + case 'I': + { + std::int16_t number{}; + return get_number(input_format, number) && sax->number_integer(number); + } + + case 'l': + { + std::int32_t number{}; + return get_number(input_format, number) && sax->number_integer(number); + } + + case 'L': + { + std::int64_t number{}; + return get_number(input_format, number) && sax->number_integer(number); + } + + case 'u': + { + if (input_format != input_format_t::bjdata) + { + break; + } + std::uint16_t number{}; + return get_number(input_format, number) && sax->number_unsigned(number); + } + + case 'm': + { + if (input_format != input_format_t::bjdata) + { + break; + } + std::uint32_t number{}; + return get_number(input_format, number) && sax->number_unsigned(number); + } + + case 'M': + { + if (input_format != input_format_t::bjdata) + { + break; + } + std::uint64_t number{}; + return get_number(input_format, number) && sax->number_unsigned(number); + } + + case 'h': + { + if (input_format != input_format_t::bjdata) + { + break; + } + const auto byte1_raw = get(); + if (JSON_HEDLEY_UNLIKELY(!unexpect_eof(input_format, "number"))) + { + return false; + } + const auto byte2_raw = get(); + if (JSON_HEDLEY_UNLIKELY(!unexpect_eof(input_format, "number"))) + { + return false; + } + + const auto byte1 = static_cast(byte1_raw); + const auto byte2 = static_cast(byte2_raw); + + // Code from RFC 7049, Appendix D, Figure 3: + // As half-precision floating-point numbers were only added + // to IEEE 754 in 2008, today's programming platforms often + // still only have limited support for them. It is very + // easy to include at least decoding support for them even + // without such support. An example of a small decoder for + // half-precision floating-point numbers in the C language + // is shown in Fig. 3. + const auto half = static_cast((byte2 << 8u) + byte1); + const double val = [&half] + { + const int exp = (half >> 10u) & 0x1Fu; + const unsigned int mant = half & 0x3FFu; + JSON_ASSERT(0 <= exp&& exp <= 32); + JSON_ASSERT(mant <= 1024); + switch (exp) + { + case 0: + return std::ldexp(mant, -24); + case 31: + return (mant == 0) + ? std::numeric_limits::infinity() + : std::numeric_limits::quiet_NaN(); + default: + return std::ldexp(mant + 1024, exp - 25); + } + }(); + return sax->number_float((half & 0x8000u) != 0 + ? static_cast(-val) + : static_cast(val), ""); + } + + case 'd': + { + float number{}; + return get_number(input_format, number) && sax->number_float(static_cast(number), ""); + } + + case 'D': + { + double number{}; + return get_number(input_format, number) && sax->number_float(static_cast(number), ""); + } + + case 'H': + { + return get_ubjson_high_precision_number(); + } + + case 'C': // char + { + get(); + if (JSON_HEDLEY_UNLIKELY(!unexpect_eof(input_format, "char"))) + { + return false; + } + if (JSON_HEDLEY_UNLIKELY(current > 127)) + { + auto last_token = get_token_string(); + return sax->parse_error(chars_read, last_token, parse_error::create(113, chars_read, + exception_message(input_format, concat("byte after 'C' must be in range 0x00..0x7F; last byte: 0x", last_token), "char"), nullptr)); + } + string_t s(1, static_cast(current)); + return sax->string(s); + } + + case 'S': // string + { + string_t s; + return get_ubjson_string(s) && sax->string(s); + } + + case '[': // array + return get_ubjson_array(); + + case '{': // object + return get_ubjson_object(); + + default: // anything else + break; + } + auto last_token = get_token_string(); + return sax->parse_error(chars_read, last_token, parse_error::create(112, chars_read, exception_message(input_format, "invalid byte: 0x" + last_token, "value"), nullptr)); + } + + /*! + @return whether array creation completed + */ + bool get_ubjson_array() + { + std::pair size_and_type; + if (JSON_HEDLEY_UNLIKELY(!get_ubjson_size_type(size_and_type))) + { + return false; + } + + // if bit-8 of size_and_type.second is set to 1, encode bjdata ndarray as an object in JData annotated array format (https://github.com/NeuroJSON/jdata): + // {"_ArrayType_" : "typeid", "_ArraySize_" : [n1, n2, ...], "_ArrayData_" : [v1, v2, ...]} + + if (input_format == input_format_t::bjdata && size_and_type.first != npos && (size_and_type.second & (1 << 8)) != 0) + { + size_and_type.second &= ~(static_cast(1) << 8); // use bit 8 to indicate ndarray, here we remove the bit to restore the type marker + auto it = std::lower_bound(bjd_types_map.begin(), bjd_types_map.end(), size_and_type.second, [](const bjd_type & p, char_int_type t) + { + return p.first < t; + }); + string_t key = "_ArrayType_"; + if (JSON_HEDLEY_UNLIKELY(it == bjd_types_map.end() || it->first != size_and_type.second)) + { + auto last_token = get_token_string(); + return sax->parse_error(chars_read, last_token, parse_error::create(112, chars_read, + exception_message(input_format, "invalid byte: 0x" + last_token, "type"), nullptr)); + } + + string_t type = it->second; // sax->string() takes a reference + if (JSON_HEDLEY_UNLIKELY(!sax->key(key) || !sax->string(type))) + { + return false; + } + + if (size_and_type.second == 'C' || size_and_type.second == 'B') + { + size_and_type.second = 'U'; + } + + key = "_ArrayData_"; + if (JSON_HEDLEY_UNLIKELY(!sax->key(key) || !sax->start_array(size_and_type.first) )) + { + return false; + } + + for (std::size_t i = 0; i < size_and_type.first; ++i) + { + if (JSON_HEDLEY_UNLIKELY(!get_ubjson_value(size_and_type.second))) + { + return false; + } + } + + return (sax->end_array() && sax->end_object()); + } + + // If BJData type marker is 'B' decode as binary + if (input_format == input_format_t::bjdata && size_and_type.first != npos && size_and_type.second == 'B') + { + binary_t result; + return get_binary(input_format, size_and_type.first, result) && sax->binary(result); + } + + if (size_and_type.first != npos) + { + if (JSON_HEDLEY_UNLIKELY(!sax->start_array(size_and_type.first))) + { + return false; + } + + if (size_and_type.second != 0) + { + if (size_and_type.second != 'N') + { + for (std::size_t i = 0; i < size_and_type.first; ++i) + { + if (JSON_HEDLEY_UNLIKELY(!get_ubjson_value(size_and_type.second))) + { + return false; + } + } + } + } + else + { + for (std::size_t i = 0; i < size_and_type.first; ++i) + { + if (JSON_HEDLEY_UNLIKELY(!parse_ubjson_internal())) + { + return false; + } + } + } + } + else + { + if (JSON_HEDLEY_UNLIKELY(!sax->start_array(detail::unknown_size()))) + { + return false; + } + + while (current != ']') + { + if (JSON_HEDLEY_UNLIKELY(!parse_ubjson_internal(false))) + { + return false; + } + get_ignore_noop(); + } + } + + return sax->end_array(); + } + + /*! + @return whether object creation completed + */ + bool get_ubjson_object() + { + std::pair size_and_type; + if (JSON_HEDLEY_UNLIKELY(!get_ubjson_size_type(size_and_type))) + { + return false; + } + + // do not accept ND-array size in objects in BJData + if (input_format == input_format_t::bjdata && size_and_type.first != npos && (size_and_type.second & (1 << 8)) != 0) + { + auto last_token = get_token_string(); + return sax->parse_error(chars_read, last_token, parse_error::create(112, chars_read, + exception_message(input_format, "BJData object does not support ND-array size in optimized format", "object"), nullptr)); + } + + string_t key; + if (size_and_type.first != npos) + { + if (JSON_HEDLEY_UNLIKELY(!sax->start_object(size_and_type.first))) + { + return false; + } + + if (size_and_type.second != 0) + { + for (std::size_t i = 0; i < size_and_type.first; ++i) + { + if (JSON_HEDLEY_UNLIKELY(!get_ubjson_string(key) || !sax->key(key))) + { + return false; + } + if (JSON_HEDLEY_UNLIKELY(!get_ubjson_value(size_and_type.second))) + { + return false; + } + key.clear(); + } + } + else + { + for (std::size_t i = 0; i < size_and_type.first; ++i) + { + if (JSON_HEDLEY_UNLIKELY(!get_ubjson_string(key) || !sax->key(key))) + { + return false; + } + if (JSON_HEDLEY_UNLIKELY(!parse_ubjson_internal())) + { + return false; + } + key.clear(); + } + } + } + else + { + if (JSON_HEDLEY_UNLIKELY(!sax->start_object(detail::unknown_size()))) + { + return false; + } + + while (current != '}') + { + if (JSON_HEDLEY_UNLIKELY(!get_ubjson_string(key, false) || !sax->key(key))) + { + return false; + } + if (JSON_HEDLEY_UNLIKELY(!parse_ubjson_internal())) + { + return false; + } + get_ignore_noop(); + key.clear(); + } + } + + return sax->end_object(); + } + + // Note, no reader for UBJSON binary types is implemented because they do + // not exist + + bool get_ubjson_high_precision_number() + { + // get the size of the following number string + std::size_t size{}; + bool no_ndarray = true; + auto res = get_ubjson_size_value(size, no_ndarray); + if (JSON_HEDLEY_UNLIKELY(!res)) + { + return res; + } + + // get number string + std::vector number_vector; + for (std::size_t i = 0; i < size; ++i) + { + get(); + if (JSON_HEDLEY_UNLIKELY(!unexpect_eof(input_format, "number"))) + { + return false; + } + number_vector.push_back(static_cast(current)); + } + + // parse number string + using ia_type = decltype(detail::input_adapter(number_vector)); + auto number_lexer = detail::lexer(detail::input_adapter(number_vector), false); + const auto result_number = number_lexer.scan(); + const auto number_string = number_lexer.get_token_string(); + const auto result_remainder = number_lexer.scan(); + + using token_type = typename detail::lexer_base::token_type; + + if (JSON_HEDLEY_UNLIKELY(result_remainder != token_type::end_of_input)) + { + return sax->parse_error(chars_read, number_string, parse_error::create(115, chars_read, + exception_message(input_format, concat("invalid number text: ", number_lexer.get_token_string()), "high-precision number"), nullptr)); + } + + switch (result_number) + { + case token_type::value_integer: + return sax->number_integer(number_lexer.get_number_integer()); + case token_type::value_unsigned: + return sax->number_unsigned(number_lexer.get_number_unsigned()); + case token_type::value_float: + return sax->number_float(number_lexer.get_number_float(), std::move(number_string)); + case token_type::uninitialized: + case token_type::literal_true: + case token_type::literal_false: + case token_type::literal_null: + case token_type::value_string: + case token_type::begin_array: + case token_type::begin_object: + case token_type::end_array: + case token_type::end_object: + case token_type::name_separator: + case token_type::value_separator: + case token_type::parse_error: + case token_type::end_of_input: + case token_type::literal_or_value: + default: + return sax->parse_error(chars_read, number_string, parse_error::create(115, chars_read, + exception_message(input_format, concat("invalid number text: ", number_lexer.get_token_string()), "high-precision number"), nullptr)); + } + } + + /////////////////////// + // Utility functions // + /////////////////////// + + /*! + @brief get next character from the input + + This function provides the interface to the used input adapter. It does + not throw in case the input reached EOF, but returns a -'ve valued + `char_traits::eof()` in that case. + + @return character read from the input + */ + char_int_type get() + { + ++chars_read; + return current = ia.get_character(); + } + + /*! + @brief get_to read into a primitive type + + This function provides the interface to the used input adapter. It does + not throw in case the input reached EOF, but returns false instead + + @return bool, whether the read was successful + */ + template + bool get_to(T& dest, const input_format_t format, const char* context) + { + auto new_chars_read = ia.get_elements(&dest); + chars_read += new_chars_read; + if (JSON_HEDLEY_UNLIKELY(new_chars_read < sizeof(T))) + { + // in case of failure, advance position by 1 to report the failing location + ++chars_read; + sax->parse_error(chars_read, "", parse_error::create(110, chars_read, exception_message(format, "unexpected end of input", context), nullptr)); + return false; + } + return true; + } + + /*! + @return character read from the input after ignoring all 'N' entries + */ + char_int_type get_ignore_noop() + { + do + { + get(); + } + while (current == 'N'); + + return current; + } + + template + static void byte_swap(NumberType& number) + { + constexpr std::size_t sz = sizeof(number); +#ifdef __cpp_lib_byteswap + if constexpr (sz == 1) + { + return; + } + else if constexpr(std::is_integral_v) + { + number = std::byteswap(number); + return; + } + else + { +#endif + auto* ptr = reinterpret_cast(&number); + for (std::size_t i = 0; i < sz / 2; ++i) + { + std::swap(ptr[i], ptr[sz - i - 1]); + } +#ifdef __cpp_lib_byteswap + } +#endif + } + + /* + @brief read a number from the input + + @tparam NumberType the type of the number + @param[in] format the current format (for diagnostics) + @param[out] result number of type @a NumberType + + @return whether conversion completed + + @note This function needs to respect the system's endianness, because + bytes in CBOR, MessagePack, and UBJSON are stored in network order + (big endian) and therefore need reordering on little endian systems. + On the other hand, BSON and BJData use little endian and should reorder + on big endian systems. + */ + template + bool get_number(const input_format_t format, NumberType& result) + { + // read in the original format + + if (JSON_HEDLEY_UNLIKELY(!get_to(result, format, "number"))) + { + return false; + } + if (is_little_endian != (InputIsLittleEndian || format == input_format_t::bjdata)) + { + byte_swap(result); + } + return true; + } + + /*! + @brief create a string by reading characters from the input + + @tparam NumberType the type of the number + @param[in] format the current format (for diagnostics) + @param[in] len number of characters to read + @param[out] result string created by reading @a len bytes + + @return whether string creation completed + + @note We can not reserve @a len bytes for the result, because @a len + may be too large. Usually, @ref unexpect_eof() detects the end of + the input before we run out of string memory. + */ + template + bool get_string(const input_format_t format, + const NumberType len, + string_t& result) + { + bool success = true; + for (NumberType i = 0; i < len; i++) + { + get(); + if (JSON_HEDLEY_UNLIKELY(!unexpect_eof(format, "string"))) + { + success = false; + break; + } + result.push_back(static_cast(current)); + } + return success; + } + + /*! + @brief create a byte array by reading bytes from the input + + @tparam NumberType the type of the number + @param[in] format the current format (for diagnostics) + @param[in] len number of bytes to read + @param[out] result byte array created by reading @a len bytes + + @return whether byte array creation completed + + @note We can not reserve @a len bytes for the result, because @a len + may be too large. Usually, @ref unexpect_eof() detects the end of + the input before we run out of memory. + */ + template + bool get_binary(const input_format_t format, + const NumberType len, + binary_t& result) + { + bool success = true; + for (NumberType i = 0; i < len; i++) + { + get(); + if (JSON_HEDLEY_UNLIKELY(!unexpect_eof(format, "binary"))) + { + success = false; + break; + } + result.push_back(static_cast(current)); + } + return success; + } + + /*! + @param[in] format the current format (for diagnostics) + @param[in] context further context information (for diagnostics) + @return whether the last read character is not EOF + */ + JSON_HEDLEY_NON_NULL(3) + bool unexpect_eof(const input_format_t format, const char* context) const + { + if (JSON_HEDLEY_UNLIKELY(current == char_traits::eof())) + { + return sax->parse_error(chars_read, "", + parse_error::create(110, chars_read, exception_message(format, "unexpected end of input", context), nullptr)); + } + return true; + } + + /*! + @return a string representation of the last read byte + */ + std::string get_token_string() const + { + std::array cr{{}}; + static_cast((std::snprintf)(cr.data(), cr.size(), "%.2hhX", static_cast(current))); // NOLINT(cppcoreguidelines-pro-type-vararg,hicpp-vararg) + return std::string{cr.data()}; + } + + /*! + @param[in] format the current format + @param[in] detail a detailed error message + @param[in] context further context information + @return a message string to use in the parse_error exceptions + */ + std::string exception_message(const input_format_t format, + const std::string& detail, + const std::string& context) const + { + std::string error_msg = "syntax error while parsing "; + + switch (format) + { + case input_format_t::cbor: + error_msg += "CBOR"; + break; + + case input_format_t::msgpack: + error_msg += "MessagePack"; + break; + + case input_format_t::ubjson: + error_msg += "UBJSON"; + break; + + case input_format_t::bson: + error_msg += "BSON"; + break; + + case input_format_t::bjdata: + error_msg += "BJData"; + break; + + case input_format_t::json: // LCOV_EXCL_LINE + default: // LCOV_EXCL_LINE + JSON_ASSERT(false); // NOLINT(cert-dcl03-c,hicpp-static-assert,misc-static-assert) LCOV_EXCL_LINE + } + + return concat(error_msg, ' ', context, ": ", detail); + } + + private: + static JSON_INLINE_VARIABLE constexpr std::size_t npos = detail::unknown_size(); + + /// input adapter + InputAdapterType ia; + + /// the current character + char_int_type current = char_traits::eof(); + + /// the number of characters read + std::size_t chars_read = 0; + + /// whether we can assume little endianness + const bool is_little_endian = little_endianness(); + + /// input format + const input_format_t input_format = input_format_t::json; + + /// the SAX parser + json_sax_t* sax = nullptr; + + // excluded markers in bjdata optimized type +#define JSON_BINARY_READER_MAKE_BJD_OPTIMIZED_TYPE_MARKERS_ \ + make_array('F', 'H', 'N', 'S', 'T', 'Z', '[', '{') + +#define JSON_BINARY_READER_MAKE_BJD_TYPES_MAP_ \ + make_array( \ + bjd_type{'B', "byte"}, \ + bjd_type{'C', "char"}, \ + bjd_type{'D', "double"}, \ + bjd_type{'I', "int16"}, \ + bjd_type{'L', "int64"}, \ + bjd_type{'M', "uint64"}, \ + bjd_type{'U', "uint8"}, \ + bjd_type{'d', "single"}, \ + bjd_type{'i', "int8"}, \ + bjd_type{'l', "int32"}, \ + bjd_type{'m', "uint32"}, \ + bjd_type{'u', "uint16"}) + + JSON_PRIVATE_UNLESS_TESTED: + // lookup tables + // NOLINTNEXTLINE(cppcoreguidelines-non-private-member-variables-in-classes) + const decltype(JSON_BINARY_READER_MAKE_BJD_OPTIMIZED_TYPE_MARKERS_) bjd_optimized_type_markers = + JSON_BINARY_READER_MAKE_BJD_OPTIMIZED_TYPE_MARKERS_; + + using bjd_type = std::pair; + // NOLINTNEXTLINE(cppcoreguidelines-non-private-member-variables-in-classes) + const decltype(JSON_BINARY_READER_MAKE_BJD_TYPES_MAP_) bjd_types_map = + JSON_BINARY_READER_MAKE_BJD_TYPES_MAP_; + +#undef JSON_BINARY_READER_MAKE_BJD_OPTIMIZED_TYPE_MARKERS_ +#undef JSON_BINARY_READER_MAKE_BJD_TYPES_MAP_ +}; + +#ifndef JSON_HAS_CPP_17 + template + constexpr std::size_t binary_reader::npos; +#endif + +} // namespace detail +NLOHMANN_JSON_NAMESPACE_END diff --git a/src/mc/include/nlohmann/detail/input/input_adapters.hpp b/src/mc/include/nlohmann/detail/input/input_adapters.hpp new file mode 100755 index 0000000..2c23a8e --- /dev/null +++ b/src/mc/include/nlohmann/detail/input/input_adapters.hpp @@ -0,0 +1,549 @@ +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.12.0 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2013 - 2025 Niels Lohmann +// SPDX-License-Identifier: MIT + +#pragma once + +#include // array +#include // size_t +#include // strlen +#include // begin, end, iterator_traits, random_access_iterator_tag, distance, next +#include // shared_ptr, make_shared, addressof +#include // accumulate +#include // string, char_traits +#include // enable_if, is_base_of, is_pointer, is_integral, remove_pointer +#include // pair, declval + +#ifndef JSON_NO_IO + #include // FILE * + #include // istream +#endif // JSON_NO_IO + +#include +#include +#include +#include + +NLOHMANN_JSON_NAMESPACE_BEGIN +namespace detail +{ + +/// the supported input formats +enum class input_format_t { json, cbor, msgpack, ubjson, bson, bjdata }; + +//////////////////// +// input adapters // +//////////////////// + +#ifndef JSON_NO_IO +/*! +Input adapter for stdio file access. This adapter read only 1 byte and do not use any + buffer. This adapter is a very low level adapter. +*/ +class file_input_adapter +{ + public: + using char_type = char; + + JSON_HEDLEY_NON_NULL(2) + explicit file_input_adapter(std::FILE* f) noexcept + : m_file(f) + { + JSON_ASSERT(m_file != nullptr); + } + + // make class move-only + file_input_adapter(const file_input_adapter&) = delete; + file_input_adapter(file_input_adapter&&) noexcept = default; + file_input_adapter& operator=(const file_input_adapter&) = delete; + file_input_adapter& operator=(file_input_adapter&&) = delete; + ~file_input_adapter() = default; + + std::char_traits::int_type get_character() noexcept + { + return std::fgetc(m_file); + } + + // returns the number of characters successfully read + template + std::size_t get_elements(T* dest, std::size_t count = 1) + { + return fread(dest, 1, sizeof(T) * count, m_file); + } + + private: + /// the file pointer to read from + std::FILE* m_file; +}; + +/*! +Input adapter for a (caching) istream. Ignores a UFT Byte Order Mark at +beginning of input. Does not support changing the underlying std::streambuf +in mid-input. Maintains underlying std::istream and std::streambuf to support +subsequent use of standard std::istream operations to process any input +characters following those used in parsing the JSON input. Clears the +std::istream flags; any input errors (e.g., EOF) will be detected by the first +subsequent call for input from the std::istream. +*/ +class input_stream_adapter +{ + public: + using char_type = char; + + ~input_stream_adapter() + { + // clear stream flags; we use underlying streambuf I/O, do not + // maintain ifstream flags, except eof + if (is != nullptr) + { + is->clear(is->rdstate() & std::ios::eofbit); + } + } + + explicit input_stream_adapter(std::istream& i) + : is(&i), sb(i.rdbuf()) + {} + + // deleted because of pointer members + input_stream_adapter(const input_stream_adapter&) = delete; + input_stream_adapter& operator=(input_stream_adapter&) = delete; + input_stream_adapter& operator=(input_stream_adapter&&) = delete; + + input_stream_adapter(input_stream_adapter&& rhs) noexcept + : is(rhs.is), sb(rhs.sb) + { + rhs.is = nullptr; + rhs.sb = nullptr; + } + + // std::istream/std::streambuf use std::char_traits::to_int_type, to + // ensure that std::char_traits::eof() and the character 0xFF do not + // end up as the same value, e.g., 0xFFFFFFFF. + std::char_traits::int_type get_character() + { + auto res = sb->sbumpc(); + // set eof manually, as we don't use the istream interface. + if (JSON_HEDLEY_UNLIKELY(res == std::char_traits::eof())) + { + is->clear(is->rdstate() | std::ios::eofbit); + } + return res; + } + + template + std::size_t get_elements(T* dest, std::size_t count = 1) + { + auto res = static_cast(sb->sgetn(reinterpret_cast(dest), static_cast(count * sizeof(T)))); + if (JSON_HEDLEY_UNLIKELY(res < count * sizeof(T))) + { + is->clear(is->rdstate() | std::ios::eofbit); + } + return res; + } + + private: + /// the associated input stream + std::istream* is = nullptr; + std::streambuf* sb = nullptr; +}; +#endif // JSON_NO_IO + +// General-purpose iterator-based adapter. It might not be as fast as +// theoretically possible for some containers, but it is extremely versatile. +template +class iterator_input_adapter +{ + public: + using char_type = typename std::iterator_traits::value_type; + + iterator_input_adapter(IteratorType first, IteratorType last) + : current(std::move(first)), end(std::move(last)) + {} + + typename char_traits::int_type get_character() + { + if (JSON_HEDLEY_LIKELY(current != end)) + { + auto result = char_traits::to_int_type(*current); + std::advance(current, 1); + return result; + } + + return char_traits::eof(); + } + + // for general iterators, we cannot really do something better than falling back to processing the range one-by-one + template + std::size_t get_elements(T* dest, std::size_t count = 1) + { + auto* ptr = reinterpret_cast(dest); + for (std::size_t read_index = 0; read_index < count * sizeof(T); ++read_index) + { + if (JSON_HEDLEY_LIKELY(current != end)) + { + ptr[read_index] = static_cast(*current); + std::advance(current, 1); + } + else + { + return read_index; + } + } + return count * sizeof(T); + } + + private: + IteratorType current; + IteratorType end; + + template + friend struct wide_string_input_helper; + + bool empty() const + { + return current == end; + } +}; + +template +struct wide_string_input_helper; + +template +struct wide_string_input_helper +{ + // UTF-32 + static void fill_buffer(BaseInputAdapter& input, + std::array::int_type, 4>& utf8_bytes, + size_t& utf8_bytes_index, + size_t& utf8_bytes_filled) + { + utf8_bytes_index = 0; + + if (JSON_HEDLEY_UNLIKELY(input.empty())) + { + utf8_bytes[0] = std::char_traits::eof(); + utf8_bytes_filled = 1; + } + else + { + // get the current character + const auto wc = input.get_character(); + + // UTF-32 to UTF-8 encoding + if (wc < 0x80) + { + utf8_bytes[0] = static_cast::int_type>(wc); + utf8_bytes_filled = 1; + } + else if (wc <= 0x7FF) + { + utf8_bytes[0] = static_cast::int_type>(0xC0u | ((static_cast(wc) >> 6u) & 0x1Fu)); + utf8_bytes[1] = static_cast::int_type>(0x80u | (static_cast(wc) & 0x3Fu)); + utf8_bytes_filled = 2; + } + else if (wc <= 0xFFFF) + { + utf8_bytes[0] = static_cast::int_type>(0xE0u | ((static_cast(wc) >> 12u) & 0x0Fu)); + utf8_bytes[1] = static_cast::int_type>(0x80u | ((static_cast(wc) >> 6u) & 0x3Fu)); + utf8_bytes[2] = static_cast::int_type>(0x80u | (static_cast(wc) & 0x3Fu)); + utf8_bytes_filled = 3; + } + else if (wc <= 0x10FFFF) + { + utf8_bytes[0] = static_cast::int_type>(0xF0u | ((static_cast(wc) >> 18u) & 0x07u)); + utf8_bytes[1] = static_cast::int_type>(0x80u | ((static_cast(wc) >> 12u) & 0x3Fu)); + utf8_bytes[2] = static_cast::int_type>(0x80u | ((static_cast(wc) >> 6u) & 0x3Fu)); + utf8_bytes[3] = static_cast::int_type>(0x80u | (static_cast(wc) & 0x3Fu)); + utf8_bytes_filled = 4; + } + else + { + // unknown character + utf8_bytes[0] = static_cast::int_type>(wc); + utf8_bytes_filled = 1; + } + } + } +}; + +template +struct wide_string_input_helper +{ + // UTF-16 + static void fill_buffer(BaseInputAdapter& input, + std::array::int_type, 4>& utf8_bytes, + size_t& utf8_bytes_index, + size_t& utf8_bytes_filled) + { + utf8_bytes_index = 0; + + if (JSON_HEDLEY_UNLIKELY(input.empty())) + { + utf8_bytes[0] = std::char_traits::eof(); + utf8_bytes_filled = 1; + } + else + { + // get the current character + const auto wc = input.get_character(); + + // UTF-16 to UTF-8 encoding + if (wc < 0x80) + { + utf8_bytes[0] = static_cast::int_type>(wc); + utf8_bytes_filled = 1; + } + else if (wc <= 0x7FF) + { + utf8_bytes[0] = static_cast::int_type>(0xC0u | ((static_cast(wc) >> 6u))); + utf8_bytes[1] = static_cast::int_type>(0x80u | (static_cast(wc) & 0x3Fu)); + utf8_bytes_filled = 2; + } + else if (0xD800 > wc || wc >= 0xE000) + { + utf8_bytes[0] = static_cast::int_type>(0xE0u | ((static_cast(wc) >> 12u))); + utf8_bytes[1] = static_cast::int_type>(0x80u | ((static_cast(wc) >> 6u) & 0x3Fu)); + utf8_bytes[2] = static_cast::int_type>(0x80u | (static_cast(wc) & 0x3Fu)); + utf8_bytes_filled = 3; + } + else + { + if (JSON_HEDLEY_UNLIKELY(!input.empty())) + { + const auto wc2 = static_cast(input.get_character()); + const auto charcode = 0x10000u + (((static_cast(wc) & 0x3FFu) << 10u) | (wc2 & 0x3FFu)); + utf8_bytes[0] = static_cast::int_type>(0xF0u | (charcode >> 18u)); + utf8_bytes[1] = static_cast::int_type>(0x80u | ((charcode >> 12u) & 0x3Fu)); + utf8_bytes[2] = static_cast::int_type>(0x80u | ((charcode >> 6u) & 0x3Fu)); + utf8_bytes[3] = static_cast::int_type>(0x80u | (charcode & 0x3Fu)); + utf8_bytes_filled = 4; + } + else + { + utf8_bytes[0] = static_cast::int_type>(wc); + utf8_bytes_filled = 1; + } + } + } + } +}; + +// Wraps another input adapter to convert wide character types into individual bytes. +template +class wide_string_input_adapter +{ + public: + using char_type = char; + + wide_string_input_adapter(BaseInputAdapter base) + : base_adapter(base) {} + + typename std::char_traits::int_type get_character() noexcept + { + // check if the buffer needs to be filled + if (utf8_bytes_index == utf8_bytes_filled) + { + fill_buffer(); + + JSON_ASSERT(utf8_bytes_filled > 0); + JSON_ASSERT(utf8_bytes_index == 0); + } + + // use buffer + JSON_ASSERT(utf8_bytes_filled > 0); + JSON_ASSERT(utf8_bytes_index < utf8_bytes_filled); + return utf8_bytes[utf8_bytes_index++]; + } + + // parsing binary with wchar doesn't make sense, but since the parsing mode can be runtime, we need something here + template + std::size_t get_elements(T* /*dest*/, std::size_t /*count*/ = 1) + { + JSON_THROW(parse_error::create(112, 1, "wide string type cannot be interpreted as binary data", nullptr)); + } + + private: + BaseInputAdapter base_adapter; + + template + void fill_buffer() + { + wide_string_input_helper::fill_buffer(base_adapter, utf8_bytes, utf8_bytes_index, utf8_bytes_filled); + } + + /// a buffer for UTF-8 bytes + std::array::int_type, 4> utf8_bytes = {{0, 0, 0, 0}}; + + /// index to the utf8_codes array for the next valid byte + std::size_t utf8_bytes_index = 0; + /// number of valid bytes in the utf8_codes array + std::size_t utf8_bytes_filled = 0; +}; + +template +struct iterator_input_adapter_factory +{ + using iterator_type = IteratorType; + using char_type = typename std::iterator_traits::value_type; + using adapter_type = iterator_input_adapter; + + static adapter_type create(IteratorType first, IteratorType last) + { + return adapter_type(std::move(first), std::move(last)); + } +}; + +template +struct is_iterator_of_multibyte +{ + using value_type = typename std::iterator_traits::value_type; + enum + { + value = sizeof(value_type) > 1 + }; +}; + +template +struct iterator_input_adapter_factory::value>> +{ + using iterator_type = IteratorType; + using char_type = typename std::iterator_traits::value_type; + using base_adapter_type = iterator_input_adapter; + using adapter_type = wide_string_input_adapter; + + static adapter_type create(IteratorType first, IteratorType last) + { + return adapter_type(base_adapter_type(std::move(first), std::move(last))); + } +}; + +// General purpose iterator-based input +template +typename iterator_input_adapter_factory::adapter_type input_adapter(IteratorType first, IteratorType last) +{ + using factory_type = iterator_input_adapter_factory; + return factory_type::create(first, last); +} + +// Convenience shorthand from container to iterator +// Enables ADL on begin(container) and end(container) +// Encloses the using declarations in namespace for not to leak them to outside scope + +namespace container_input_adapter_factory_impl +{ + +using std::begin; +using std::end; + +template +struct container_input_adapter_factory {}; + +template +struct container_input_adapter_factory< ContainerType, + void_t()), end(std::declval()))>> + { + using adapter_type = decltype(input_adapter(begin(std::declval()), end(std::declval()))); + + static adapter_type create(const ContainerType& container) +{ + return input_adapter(begin(container), end(container)); +} + }; + +} // namespace container_input_adapter_factory_impl + +template +typename container_input_adapter_factory_impl::container_input_adapter_factory::adapter_type input_adapter(const ContainerType& container) +{ + return container_input_adapter_factory_impl::container_input_adapter_factory::create(container); +} + +// specialization for std::string +using string_input_adapter_type = decltype(input_adapter(std::declval())); + +#ifndef JSON_NO_IO +// Special cases with fast paths +inline file_input_adapter input_adapter(std::FILE* file) +{ + if (file == nullptr) + { + JSON_THROW(parse_error::create(101, 0, "attempting to parse an empty input; check that your input string or stream contains the expected JSON", nullptr)); + } + return file_input_adapter(file); +} + +inline input_stream_adapter input_adapter(std::istream& stream) +{ + return input_stream_adapter(stream); +} + +inline input_stream_adapter input_adapter(std::istream&& stream) +{ + return input_stream_adapter(stream); +} +#endif // JSON_NO_IO + +using contiguous_bytes_input_adapter = decltype(input_adapter(std::declval(), std::declval())); + +// Null-delimited strings, and the like. +template < typename CharT, + typename std::enable_if < + std::is_pointer::value&& + !std::is_array::value&& + std::is_integral::type>::value&& + sizeof(typename std::remove_pointer::type) == 1, + int >::type = 0 > +contiguous_bytes_input_adapter input_adapter(CharT b) +{ + if (b == nullptr) + { + JSON_THROW(parse_error::create(101, 0, "attempting to parse an empty input; check that your input string or stream contains the expected JSON", nullptr)); + } + auto length = std::strlen(reinterpret_cast(b)); + const auto* ptr = reinterpret_cast(b); + return input_adapter(ptr, ptr + length); // cppcheck-suppress[nullPointerArithmeticRedundantCheck] +} + +template +auto input_adapter(T (&array)[N]) -> decltype(input_adapter(array, array + N)) // NOLINT(cppcoreguidelines-avoid-c-arrays,hicpp-avoid-c-arrays,modernize-avoid-c-arrays) +{ + return input_adapter(array, array + N); +} + +// This class only handles inputs of input_buffer_adapter type. +// It's required so that expressions like {ptr, len} can be implicitly cast +// to the correct adapter. +class span_input_adapter +{ + public: + template < typename CharT, + typename std::enable_if < + std::is_pointer::value&& + std::is_integral::type>::value&& + sizeof(typename std::remove_pointer::type) == 1, + int >::type = 0 > + span_input_adapter(CharT b, std::size_t l) + : ia(reinterpret_cast(b), reinterpret_cast(b) + l) {} + + template::iterator_category, std::random_access_iterator_tag>::value, + int>::type = 0> + span_input_adapter(IteratorType first, IteratorType last) + : ia(input_adapter(first, last)) {} + + contiguous_bytes_input_adapter&& get() + { + return std::move(ia); // NOLINT(hicpp-move-const-arg,performance-move-const-arg) + } + + private: + contiguous_bytes_input_adapter ia; +}; + +} // namespace detail +NLOHMANN_JSON_NAMESPACE_END diff --git a/src/mc/include/nlohmann/detail/input/json_sax.hpp b/src/mc/include/nlohmann/detail/input/json_sax.hpp new file mode 100755 index 0000000..0b06ad5 --- /dev/null +++ b/src/mc/include/nlohmann/detail/input/json_sax.hpp @@ -0,0 +1,986 @@ +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.12.0 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2013 - 2025 Niels Lohmann +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include // string +#include // enable_if_t +#include // move +#include // vector + +#include +#include +#include +#include +NLOHMANN_JSON_NAMESPACE_BEGIN + +/*! +@brief SAX interface + +This class describes the SAX interface used by @ref nlohmann::json::sax_parse. +Each function is called in different situations while the input is parsed. The +boolean return value informs the parser whether to continue processing the +input. +*/ +template +struct json_sax +{ + using number_integer_t = typename BasicJsonType::number_integer_t; + using number_unsigned_t = typename BasicJsonType::number_unsigned_t; + using number_float_t = typename BasicJsonType::number_float_t; + using string_t = typename BasicJsonType::string_t; + using binary_t = typename BasicJsonType::binary_t; + + /*! + @brief a null value was read + @return whether parsing should proceed + */ + virtual bool null() = 0; + + /*! + @brief a boolean value was read + @param[in] val boolean value + @return whether parsing should proceed + */ + virtual bool boolean(bool val) = 0; + + /*! + @brief an integer number was read + @param[in] val integer value + @return whether parsing should proceed + */ + virtual bool number_integer(number_integer_t val) = 0; + + /*! + @brief an unsigned integer number was read + @param[in] val unsigned integer value + @return whether parsing should proceed + */ + virtual bool number_unsigned(number_unsigned_t val) = 0; + + /*! + @brief a floating-point number was read + @param[in] val floating-point value + @param[in] s raw token value + @return whether parsing should proceed + */ + virtual bool number_float(number_float_t val, const string_t& s) = 0; + + /*! + @brief a string value was read + @param[in] val string value + @return whether parsing should proceed + @note It is safe to move the passed string value. + */ + virtual bool string(string_t& val) = 0; + + /*! + @brief a binary value was read + @param[in] val binary value + @return whether parsing should proceed + @note It is safe to move the passed binary value. + */ + virtual bool binary(binary_t& val) = 0; + + /*! + @brief the beginning of an object was read + @param[in] elements number of object elements or -1 if unknown + @return whether parsing should proceed + @note binary formats may report the number of elements + */ + virtual bool start_object(std::size_t elements) = 0; + + /*! + @brief an object key was read + @param[in] val object key + @return whether parsing should proceed + @note It is safe to move the passed string. + */ + virtual bool key(string_t& val) = 0; + + /*! + @brief the end of an object was read + @return whether parsing should proceed + */ + virtual bool end_object() = 0; + + /*! + @brief the beginning of an array was read + @param[in] elements number of array elements or -1 if unknown + @return whether parsing should proceed + @note binary formats may report the number of elements + */ + virtual bool start_array(std::size_t elements) = 0; + + /*! + @brief the end of an array was read + @return whether parsing should proceed + */ + virtual bool end_array() = 0; + + /*! + @brief a parse error occurred + @param[in] position the position in the input where the error occurs + @param[in] last_token the last read token + @param[in] ex an exception object describing the error + @return whether parsing should proceed (must return false) + */ + virtual bool parse_error(std::size_t position, + const std::string& last_token, + const detail::exception& ex) = 0; + + json_sax() = default; + json_sax(const json_sax&) = default; + json_sax(json_sax&&) noexcept = default; + json_sax& operator=(const json_sax&) = default; + json_sax& operator=(json_sax&&) noexcept = default; + virtual ~json_sax() = default; +}; + +namespace detail +{ +constexpr std::size_t unknown_size() +{ + return (std::numeric_limits::max)(); +} + +/*! +@brief SAX implementation to create a JSON value from SAX events + +This class implements the @ref json_sax interface and processes the SAX events +to create a JSON value which makes it basically a DOM parser. The structure or +hierarchy of the JSON value is managed by the stack `ref_stack` which contains +a pointer to the respective array or object for each recursion depth. + +After successful parsing, the value that is passed by reference to the +constructor contains the parsed value. + +@tparam BasicJsonType the JSON type +*/ +template +class json_sax_dom_parser +{ + public: + using number_integer_t = typename BasicJsonType::number_integer_t; + using number_unsigned_t = typename BasicJsonType::number_unsigned_t; + using number_float_t = typename BasicJsonType::number_float_t; + using string_t = typename BasicJsonType::string_t; + using binary_t = typename BasicJsonType::binary_t; + using lexer_t = lexer; + + /*! + @param[in,out] r reference to a JSON value that is manipulated while + parsing + @param[in] allow_exceptions_ whether parse errors yield exceptions + */ + explicit json_sax_dom_parser(BasicJsonType& r, const bool allow_exceptions_ = true, lexer_t* lexer_ = nullptr) + : root(r), allow_exceptions(allow_exceptions_), m_lexer_ref(lexer_) + {} + + // make class move-only + json_sax_dom_parser(const json_sax_dom_parser&) = delete; + json_sax_dom_parser(json_sax_dom_parser&&) = default; // NOLINT(hicpp-noexcept-move,performance-noexcept-move-constructor) + json_sax_dom_parser& operator=(const json_sax_dom_parser&) = delete; + json_sax_dom_parser& operator=(json_sax_dom_parser&&) = default; // NOLINT(hicpp-noexcept-move,performance-noexcept-move-constructor) + ~json_sax_dom_parser() = default; + + bool null() + { + handle_value(nullptr); + return true; + } + + bool boolean(bool val) + { + handle_value(val); + return true; + } + + bool number_integer(number_integer_t val) + { + handle_value(val); + return true; + } + + bool number_unsigned(number_unsigned_t val) + { + handle_value(val); + return true; + } + + bool number_float(number_float_t val, const string_t& /*unused*/) + { + handle_value(val); + return true; + } + + bool string(string_t& val) + { + handle_value(val); + return true; + } + + bool binary(binary_t& val) + { + handle_value(std::move(val)); + return true; + } + + bool start_object(std::size_t len) + { + ref_stack.push_back(handle_value(BasicJsonType::value_t::object)); + +#if JSON_DIAGNOSTIC_POSITIONS + // Manually set the start position of the object here. + // Ensure this is after the call to handle_value to ensure correct start position. + if (m_lexer_ref) + { + // Lexer has read the first character of the object, so + // subtract 1 from the position to get the correct start position. + ref_stack.back()->start_position = m_lexer_ref->get_position() - 1; + } +#endif + + if (JSON_HEDLEY_UNLIKELY(len != detail::unknown_size() && len > ref_stack.back()->max_size())) + { + JSON_THROW(out_of_range::create(408, concat("excessive object size: ", std::to_string(len)), ref_stack.back())); + } + + return true; + } + + bool key(string_t& val) + { + JSON_ASSERT(!ref_stack.empty()); + JSON_ASSERT(ref_stack.back()->is_object()); + + // add null at the given key and store the reference for later + object_element = &(ref_stack.back()->m_data.m_value.object->operator[](val)); + return true; + } + + bool end_object() + { + JSON_ASSERT(!ref_stack.empty()); + JSON_ASSERT(ref_stack.back()->is_object()); + +#if JSON_DIAGNOSTIC_POSITIONS + if (m_lexer_ref) + { + // Lexer's position is past the closing brace, so set that as the end position. + ref_stack.back()->end_position = m_lexer_ref->get_position(); + } +#endif + + ref_stack.back()->set_parents(); + ref_stack.pop_back(); + return true; + } + + bool start_array(std::size_t len) + { + ref_stack.push_back(handle_value(BasicJsonType::value_t::array)); + +#if JSON_DIAGNOSTIC_POSITIONS + // Manually set the start position of the array here. + // Ensure this is after the call to handle_value to ensure correct start position. + if (m_lexer_ref) + { + ref_stack.back()->start_position = m_lexer_ref->get_position() - 1; + } +#endif + + if (JSON_HEDLEY_UNLIKELY(len != detail::unknown_size() && len > ref_stack.back()->max_size())) + { + JSON_THROW(out_of_range::create(408, concat("excessive array size: ", std::to_string(len)), ref_stack.back())); + } + + return true; + } + + bool end_array() + { + JSON_ASSERT(!ref_stack.empty()); + JSON_ASSERT(ref_stack.back()->is_array()); + +#if JSON_DIAGNOSTIC_POSITIONS + if (m_lexer_ref) + { + // Lexer's position is past the closing bracket, so set that as the end position. + ref_stack.back()->end_position = m_lexer_ref->get_position(); + } +#endif + + ref_stack.back()->set_parents(); + ref_stack.pop_back(); + return true; + } + + template + bool parse_error(std::size_t /*unused*/, const std::string& /*unused*/, + const Exception& ex) + { + errored = true; + static_cast(ex); + if (allow_exceptions) + { + JSON_THROW(ex); + } + return false; + } + + constexpr bool is_errored() const + { + return errored; + } + + private: + +#if JSON_DIAGNOSTIC_POSITIONS + void handle_diagnostic_positions_for_json_value(BasicJsonType& v) + { + if (m_lexer_ref) + { + // Lexer has read past the current field value, so set the end position to the current position. + // The start position will be set below based on the length of the string representation + // of the value. + v.end_position = m_lexer_ref->get_position(); + + switch (v.type()) + { + case value_t::boolean: + { + // 4 and 5 are the string length of "true" and "false" + v.start_position = v.end_position - (v.m_data.m_value.boolean ? 4 : 5); + break; + } + + case value_t::null: + { + // 4 is the string length of "null" + v.start_position = v.end_position - 4; + break; + } + + case value_t::string: + { + // include the length of the quotes, which is 2 + v.start_position = v.end_position - v.m_data.m_value.string->size() - 2; + break; + } + + // As we handle the start and end positions for values created during parsing, + // we do not expect the following value type to be called. Regardless, set the positions + // in case this is created manually or through a different constructor. Exclude from lcov + // since the exact condition of this switch is esoteric. + // LCOV_EXCL_START + case value_t::discarded: + { + v.end_position = std::string::npos; + v.start_position = v.end_position; + break; + } + // LCOV_EXCL_STOP + case value_t::binary: + case value_t::number_integer: + case value_t::number_unsigned: + case value_t::number_float: + { + v.start_position = v.end_position - m_lexer_ref->get_string().size(); + break; + } + case value_t::object: + case value_t::array: + { + // object and array are handled in start_object() and start_array() handlers + // skip setting the values here. + break; + } + default: // LCOV_EXCL_LINE + // Handle all possible types discretely, default handler should never be reached. + JSON_ASSERT(false); // NOLINT(cert-dcl03-c,hicpp-static-assert,misc-static-assert,-warnings-as-errors) LCOV_EXCL_LINE + } + } + } +#endif + + /*! + @invariant If the ref stack is empty, then the passed value will be the new + root. + @invariant If the ref stack contains a value, then it is an array or an + object to which we can add elements + */ + template + JSON_HEDLEY_RETURNS_NON_NULL + BasicJsonType* handle_value(Value&& v) + { + if (ref_stack.empty()) + { + root = BasicJsonType(std::forward(v)); + +#if JSON_DIAGNOSTIC_POSITIONS + handle_diagnostic_positions_for_json_value(root); +#endif + + return &root; + } + + JSON_ASSERT(ref_stack.back()->is_array() || ref_stack.back()->is_object()); + + if (ref_stack.back()->is_array()) + { + ref_stack.back()->m_data.m_value.array->emplace_back(std::forward(v)); + +#if JSON_DIAGNOSTIC_POSITIONS + handle_diagnostic_positions_for_json_value(ref_stack.back()->m_data.m_value.array->back()); +#endif + + return &(ref_stack.back()->m_data.m_value.array->back()); + } + + JSON_ASSERT(ref_stack.back()->is_object()); + JSON_ASSERT(object_element); + *object_element = BasicJsonType(std::forward(v)); + +#if JSON_DIAGNOSTIC_POSITIONS + handle_diagnostic_positions_for_json_value(*object_element); +#endif + + return object_element; + } + + /// the parsed JSON value + BasicJsonType& root; + /// stack to model hierarchy of values + std::vector ref_stack {}; + /// helper to hold the reference for the next object element + BasicJsonType* object_element = nullptr; + /// whether a syntax error occurred + bool errored = false; + /// whether to throw exceptions in case of errors + const bool allow_exceptions = true; + /// the lexer reference to obtain the current position + lexer_t* m_lexer_ref = nullptr; +}; + +template +class json_sax_dom_callback_parser +{ + public: + using number_integer_t = typename BasicJsonType::number_integer_t; + using number_unsigned_t = typename BasicJsonType::number_unsigned_t; + using number_float_t = typename BasicJsonType::number_float_t; + using string_t = typename BasicJsonType::string_t; + using binary_t = typename BasicJsonType::binary_t; + using parser_callback_t = typename BasicJsonType::parser_callback_t; + using parse_event_t = typename BasicJsonType::parse_event_t; + using lexer_t = lexer; + + json_sax_dom_callback_parser(BasicJsonType& r, + parser_callback_t cb, + const bool allow_exceptions_ = true, + lexer_t* lexer_ = nullptr) + : root(r), callback(std::move(cb)), allow_exceptions(allow_exceptions_), m_lexer_ref(lexer_) + { + keep_stack.push_back(true); + } + + // make class move-only + json_sax_dom_callback_parser(const json_sax_dom_callback_parser&) = delete; + json_sax_dom_callback_parser(json_sax_dom_callback_parser&&) = default; // NOLINT(hicpp-noexcept-move,performance-noexcept-move-constructor) + json_sax_dom_callback_parser& operator=(const json_sax_dom_callback_parser&) = delete; + json_sax_dom_callback_parser& operator=(json_sax_dom_callback_parser&&) = default; // NOLINT(hicpp-noexcept-move,performance-noexcept-move-constructor) + ~json_sax_dom_callback_parser() = default; + + bool null() + { + handle_value(nullptr); + return true; + } + + bool boolean(bool val) + { + handle_value(val); + return true; + } + + bool number_integer(number_integer_t val) + { + handle_value(val); + return true; + } + + bool number_unsigned(number_unsigned_t val) + { + handle_value(val); + return true; + } + + bool number_float(number_float_t val, const string_t& /*unused*/) + { + handle_value(val); + return true; + } + + bool string(string_t& val) + { + handle_value(val); + return true; + } + + bool binary(binary_t& val) + { + handle_value(std::move(val)); + return true; + } + + bool start_object(std::size_t len) + { + // check callback for object start + const bool keep = callback(static_cast(ref_stack.size()), parse_event_t::object_start, discarded); + keep_stack.push_back(keep); + + auto val = handle_value(BasicJsonType::value_t::object, true); + ref_stack.push_back(val.second); + + if (ref_stack.back()) + { + +#if JSON_DIAGNOSTIC_POSITIONS + // Manually set the start position of the object here. + // Ensure this is after the call to handle_value to ensure correct start position. + if (m_lexer_ref) + { + // Lexer has read the first character of the object, so + // subtract 1 from the position to get the correct start position. + ref_stack.back()->start_position = m_lexer_ref->get_position() - 1; + } +#endif + + // check object limit + if (JSON_HEDLEY_UNLIKELY(len != detail::unknown_size() && len > ref_stack.back()->max_size())) + { + JSON_THROW(out_of_range::create(408, concat("excessive object size: ", std::to_string(len)), ref_stack.back())); + } + } + return true; + } + + bool key(string_t& val) + { + BasicJsonType k = BasicJsonType(val); + + // check callback for the key + const bool keep = callback(static_cast(ref_stack.size()), parse_event_t::key, k); + key_keep_stack.push_back(keep); + + // add discarded value at the given key and store the reference for later + if (keep && ref_stack.back()) + { + object_element = &(ref_stack.back()->m_data.m_value.object->operator[](val) = discarded); + } + + return true; + } + + bool end_object() + { + if (ref_stack.back()) + { + if (!callback(static_cast(ref_stack.size()) - 1, parse_event_t::object_end, *ref_stack.back())) + { + // discard object + *ref_stack.back() = discarded; + +#if JSON_DIAGNOSTIC_POSITIONS + // Set start/end positions for discarded object. + handle_diagnostic_positions_for_json_value(*ref_stack.back()); +#endif + } + else + { + +#if JSON_DIAGNOSTIC_POSITIONS + if (m_lexer_ref) + { + // Lexer's position is past the closing brace, so set that as the end position. + ref_stack.back()->end_position = m_lexer_ref->get_position(); + } +#endif + + ref_stack.back()->set_parents(); + } + } + + JSON_ASSERT(!ref_stack.empty()); + JSON_ASSERT(!keep_stack.empty()); + ref_stack.pop_back(); + keep_stack.pop_back(); + + if (!ref_stack.empty() && ref_stack.back() && ref_stack.back()->is_structured()) + { + // remove discarded value + for (auto it = ref_stack.back()->begin(); it != ref_stack.back()->end(); ++it) + { + if (it->is_discarded()) + { + ref_stack.back()->erase(it); + break; + } + } + } + + return true; + } + + bool start_array(std::size_t len) + { + const bool keep = callback(static_cast(ref_stack.size()), parse_event_t::array_start, discarded); + keep_stack.push_back(keep); + + auto val = handle_value(BasicJsonType::value_t::array, true); + ref_stack.push_back(val.second); + + if (ref_stack.back()) + { + +#if JSON_DIAGNOSTIC_POSITIONS + // Manually set the start position of the array here. + // Ensure this is after the call to handle_value to ensure correct start position. + if (m_lexer_ref) + { + // Lexer has read the first character of the array, so + // subtract 1 from the position to get the correct start position. + ref_stack.back()->start_position = m_lexer_ref->get_position() - 1; + } +#endif + + // check array limit + if (JSON_HEDLEY_UNLIKELY(len != detail::unknown_size() && len > ref_stack.back()->max_size())) + { + JSON_THROW(out_of_range::create(408, concat("excessive array size: ", std::to_string(len)), ref_stack.back())); + } + } + + return true; + } + + bool end_array() + { + bool keep = true; + + if (ref_stack.back()) + { + keep = callback(static_cast(ref_stack.size()) - 1, parse_event_t::array_end, *ref_stack.back()); + if (keep) + { + +#if JSON_DIAGNOSTIC_POSITIONS + if (m_lexer_ref) + { + // Lexer's position is past the closing bracket, so set that as the end position. + ref_stack.back()->end_position = m_lexer_ref->get_position(); + } +#endif + + ref_stack.back()->set_parents(); + } + else + { + // discard array + *ref_stack.back() = discarded; + +#if JSON_DIAGNOSTIC_POSITIONS + // Set start/end positions for discarded array. + handle_diagnostic_positions_for_json_value(*ref_stack.back()); +#endif + } + } + + JSON_ASSERT(!ref_stack.empty()); + JSON_ASSERT(!keep_stack.empty()); + ref_stack.pop_back(); + keep_stack.pop_back(); + + // remove discarded value + if (!keep && !ref_stack.empty() && ref_stack.back()->is_array()) + { + ref_stack.back()->m_data.m_value.array->pop_back(); + } + + return true; + } + + template + bool parse_error(std::size_t /*unused*/, const std::string& /*unused*/, + const Exception& ex) + { + errored = true; + static_cast(ex); + if (allow_exceptions) + { + JSON_THROW(ex); + } + return false; + } + + constexpr bool is_errored() const + { + return errored; + } + + private: + +#if JSON_DIAGNOSTIC_POSITIONS + void handle_diagnostic_positions_for_json_value(BasicJsonType& v) + { + if (m_lexer_ref) + { + // Lexer has read past the current field value, so set the end position to the current position. + // The start position will be set below based on the length of the string representation + // of the value. + v.end_position = m_lexer_ref->get_position(); + + switch (v.type()) + { + case value_t::boolean: + { + // 4 and 5 are the string length of "true" and "false" + v.start_position = v.end_position - (v.m_data.m_value.boolean ? 4 : 5); + break; + } + + case value_t::null: + { + // 4 is the string length of "null" + v.start_position = v.end_position - 4; + break; + } + + case value_t::string: + { + // include the length of the quotes, which is 2 + v.start_position = v.end_position - v.m_data.m_value.string->size() - 2; + break; + } + + case value_t::discarded: + { + v.end_position = std::string::npos; + v.start_position = v.end_position; + break; + } + + case value_t::binary: + case value_t::number_integer: + case value_t::number_unsigned: + case value_t::number_float: + { + v.start_position = v.end_position - m_lexer_ref->get_string().size(); + break; + } + + case value_t::object: + case value_t::array: + { + // object and array are handled in start_object() and start_array() handlers + // skip setting the values here. + break; + } + default: // LCOV_EXCL_LINE + // Handle all possible types discretely, default handler should never be reached. + JSON_ASSERT(false); // NOLINT(cert-dcl03-c,hicpp-static-assert,misc-static-assert,-warnings-as-errors) LCOV_EXCL_LINE + } + } + } +#endif + + /*! + @param[in] v value to add to the JSON value we build during parsing + @param[in] skip_callback whether we should skip calling the callback + function; this is required after start_array() and + start_object() SAX events, because otherwise we would call the + callback function with an empty array or object, respectively. + + @invariant If the ref stack is empty, then the passed value will be the new + root. + @invariant If the ref stack contains a value, then it is an array or an + object to which we can add elements + + @return pair of boolean (whether value should be kept) and pointer (to the + passed value in the ref_stack hierarchy; nullptr if not kept) + */ + template + std::pair handle_value(Value&& v, const bool skip_callback = false) + { + JSON_ASSERT(!keep_stack.empty()); + + // do not handle this value if we know it would be added to a discarded + // container + if (!keep_stack.back()) + { + return {false, nullptr}; + } + + // create value + auto value = BasicJsonType(std::forward(v)); + +#if JSON_DIAGNOSTIC_POSITIONS + handle_diagnostic_positions_for_json_value(value); +#endif + + // check callback + const bool keep = skip_callback || callback(static_cast(ref_stack.size()), parse_event_t::value, value); + + // do not handle this value if we just learnt it shall be discarded + if (!keep) + { + return {false, nullptr}; + } + + if (ref_stack.empty()) + { + root = std::move(value); + return {true, & root}; + } + + // skip this value if we already decided to skip the parent + // (https://github.com/nlohmann/json/issues/971#issuecomment-413678360) + if (!ref_stack.back()) + { + return {false, nullptr}; + } + + // we now only expect arrays and objects + JSON_ASSERT(ref_stack.back()->is_array() || ref_stack.back()->is_object()); + + // array + if (ref_stack.back()->is_array()) + { + ref_stack.back()->m_data.m_value.array->emplace_back(std::move(value)); + return {true, & (ref_stack.back()->m_data.m_value.array->back())}; + } + + // object + JSON_ASSERT(ref_stack.back()->is_object()); + // check if we should store an element for the current key + JSON_ASSERT(!key_keep_stack.empty()); + const bool store_element = key_keep_stack.back(); + key_keep_stack.pop_back(); + + if (!store_element) + { + return {false, nullptr}; + } + + JSON_ASSERT(object_element); + *object_element = std::move(value); + return {true, object_element}; + } + + /// the parsed JSON value + BasicJsonType& root; + /// stack to model hierarchy of values + std::vector ref_stack {}; + /// stack to manage which values to keep + std::vector keep_stack {}; // NOLINT(readability-redundant-member-init) + /// stack to manage which object keys to keep + std::vector key_keep_stack {}; // NOLINT(readability-redundant-member-init) + /// helper to hold the reference for the next object element + BasicJsonType* object_element = nullptr; + /// whether a syntax error occurred + bool errored = false; + /// callback function + const parser_callback_t callback = nullptr; + /// whether to throw exceptions in case of errors + const bool allow_exceptions = true; + /// a discarded value for the callback + BasicJsonType discarded = BasicJsonType::value_t::discarded; + /// the lexer reference to obtain the current position + lexer_t* m_lexer_ref = nullptr; +}; + +template +class json_sax_acceptor +{ + public: + using number_integer_t = typename BasicJsonType::number_integer_t; + using number_unsigned_t = typename BasicJsonType::number_unsigned_t; + using number_float_t = typename BasicJsonType::number_float_t; + using string_t = typename BasicJsonType::string_t; + using binary_t = typename BasicJsonType::binary_t; + + bool null() + { + return true; + } + + bool boolean(bool /*unused*/) + { + return true; + } + + bool number_integer(number_integer_t /*unused*/) + { + return true; + } + + bool number_unsigned(number_unsigned_t /*unused*/) + { + return true; + } + + bool number_float(number_float_t /*unused*/, const string_t& /*unused*/) + { + return true; + } + + bool string(string_t& /*unused*/) + { + return true; + } + + bool binary(binary_t& /*unused*/) + { + return true; + } + + bool start_object(std::size_t /*unused*/ = detail::unknown_size()) + { + return true; + } + + bool key(string_t& /*unused*/) + { + return true; + } + + bool end_object() + { + return true; + } + + bool start_array(std::size_t /*unused*/ = detail::unknown_size()) + { + return true; + } + + bool end_array() + { + return true; + } + + bool parse_error(std::size_t /*unused*/, const std::string& /*unused*/, const detail::exception& /*unused*/) + { + return false; + } +}; + +} // namespace detail +NLOHMANN_JSON_NAMESPACE_END diff --git a/src/mc/include/nlohmann/detail/input/lexer.hpp b/src/mc/include/nlohmann/detail/input/lexer.hpp new file mode 100755 index 0000000..a40790e --- /dev/null +++ b/src/mc/include/nlohmann/detail/input/lexer.hpp @@ -0,0 +1,1643 @@ +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.12.0 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2013 - 2025 Niels Lohmann +// SPDX-License-Identifier: MIT + +#pragma once + +#include // array +#include // localeconv +#include // size_t +#include // snprintf +#include // strtof, strtod, strtold, strtoll, strtoull +#include // initializer_list +#include // char_traits, string +#include // move +#include // vector + +#include +#include +#include +#include + +NLOHMANN_JSON_NAMESPACE_BEGIN +namespace detail +{ + +/////////// +// lexer // +/////////// + +template +class lexer_base +{ + public: + /// token types for the parser + enum class token_type + { + uninitialized, ///< indicating the scanner is uninitialized + literal_true, ///< the `true` literal + literal_false, ///< the `false` literal + literal_null, ///< the `null` literal + value_string, ///< a string -- use get_string() for actual value + value_unsigned, ///< an unsigned integer -- use get_number_unsigned() for actual value + value_integer, ///< a signed integer -- use get_number_integer() for actual value + value_float, ///< an floating point number -- use get_number_float() for actual value + begin_array, ///< the character for array begin `[` + begin_object, ///< the character for object begin `{` + end_array, ///< the character for array end `]` + end_object, ///< the character for object end `}` + name_separator, ///< the name separator `:` + value_separator, ///< the value separator `,` + parse_error, ///< indicating a parse error + end_of_input, ///< indicating the end of the input buffer + literal_or_value ///< a literal or the begin of a value (only for diagnostics) + }; + + /// return name of values of type token_type (only used for errors) + JSON_HEDLEY_RETURNS_NON_NULL + JSON_HEDLEY_CONST + static const char* token_type_name(const token_type t) noexcept + { + switch (t) + { + case token_type::uninitialized: + return ""; + case token_type::literal_true: + return "true literal"; + case token_type::literal_false: + return "false literal"; + case token_type::literal_null: + return "null literal"; + case token_type::value_string: + return "string literal"; + case token_type::value_unsigned: + case token_type::value_integer: + case token_type::value_float: + return "number literal"; + case token_type::begin_array: + return "'['"; + case token_type::begin_object: + return "'{'"; + case token_type::end_array: + return "']'"; + case token_type::end_object: + return "'}'"; + case token_type::name_separator: + return "':'"; + case token_type::value_separator: + return "','"; + case token_type::parse_error: + return ""; + case token_type::end_of_input: + return "end of input"; + case token_type::literal_or_value: + return "'[', '{', or a literal"; + // LCOV_EXCL_START + default: // catch non-enum values + return "unknown token"; + // LCOV_EXCL_STOP + } + } +}; +/*! +@brief lexical analysis + +This class organizes the lexical analysis during JSON deserialization. +*/ +template +class lexer : public lexer_base +{ + using number_integer_t = typename BasicJsonType::number_integer_t; + using number_unsigned_t = typename BasicJsonType::number_unsigned_t; + using number_float_t = typename BasicJsonType::number_float_t; + using string_t = typename BasicJsonType::string_t; + using char_type = typename InputAdapterType::char_type; + using char_int_type = typename char_traits::int_type; + + public: + using token_type = typename lexer_base::token_type; + + explicit lexer(InputAdapterType&& adapter, bool ignore_comments_ = false) noexcept + : ia(std::move(adapter)) + , ignore_comments(ignore_comments_) + , decimal_point_char(static_cast(get_decimal_point())) + {} + + // deleted because of pointer members + lexer(const lexer&) = delete; + lexer(lexer&&) = default; // NOLINT(hicpp-noexcept-move,performance-noexcept-move-constructor) + lexer& operator=(lexer&) = delete; + lexer& operator=(lexer&&) = default; // NOLINT(hicpp-noexcept-move,performance-noexcept-move-constructor) + ~lexer() = default; + + private: + ///////////////////// + // locales + ///////////////////// + + /// return the locale-dependent decimal point + JSON_HEDLEY_PURE + static char get_decimal_point() noexcept + { + const auto* loc = localeconv(); + JSON_ASSERT(loc != nullptr); + return (loc->decimal_point == nullptr) ? '.' : *(loc->decimal_point); + } + + ///////////////////// + // scan functions + ///////////////////// + + /*! + @brief get codepoint from 4 hex characters following `\u` + + For input "\u c1 c2 c3 c4" the codepoint is: + (c1 * 0x1000) + (c2 * 0x0100) + (c3 * 0x0010) + c4 + = (c1 << 12) + (c2 << 8) + (c3 << 4) + (c4 << 0) + + Furthermore, the possible characters '0'..'9', 'A'..'F', and 'a'..'f' + must be converted to the integers 0x0..0x9, 0xA..0xF, 0xA..0xF, resp. The + conversion is done by subtracting the offset (0x30, 0x37, and 0x57) + between the ASCII value of the character and the desired integer value. + + @return codepoint (0x0000..0xFFFF) or -1 in case of an error (e.g. EOF or + non-hex character) + */ + int get_codepoint() + { + // this function only makes sense after reading `\u` + JSON_ASSERT(current == 'u'); + int codepoint = 0; + + const auto factors = { 12u, 8u, 4u, 0u }; + for (const auto factor : factors) + { + get(); + + if (current >= '0' && current <= '9') + { + codepoint += static_cast((static_cast(current) - 0x30u) << factor); + } + else if (current >= 'A' && current <= 'F') + { + codepoint += static_cast((static_cast(current) - 0x37u) << factor); + } + else if (current >= 'a' && current <= 'f') + { + codepoint += static_cast((static_cast(current) - 0x57u) << factor); + } + else + { + return -1; + } + } + + JSON_ASSERT(0x0000 <= codepoint && codepoint <= 0xFFFF); + return codepoint; + } + + /*! + @brief check if the next byte(s) are inside a given range + + Adds the current byte and, for each passed range, reads a new byte and + checks if it is inside the range. If a violation was detected, set up an + error message and return false. Otherwise, return true. + + @param[in] ranges list of integers; interpreted as list of pairs of + inclusive lower and upper bound, respectively + + @pre The passed list @a ranges must have 2, 4, or 6 elements; that is, + 1, 2, or 3 pairs. This precondition is enforced by an assertion. + + @return true if and only if no range violation was detected + */ + bool next_byte_in_range(std::initializer_list ranges) + { + JSON_ASSERT(ranges.size() == 2 || ranges.size() == 4 || ranges.size() == 6); + add(current); + + for (auto range = ranges.begin(); range != ranges.end(); ++range) + { + get(); + if (JSON_HEDLEY_LIKELY(*range <= current && current <= *(++range))) // NOLINT(bugprone-inc-dec-in-conditions) + { + add(current); + } + else + { + error_message = "invalid string: ill-formed UTF-8 byte"; + return false; + } + } + + return true; + } + + /*! + @brief scan a string literal + + This function scans a string according to Sect. 7 of RFC 8259. While + scanning, bytes are escaped and copied into buffer token_buffer. Then the + function returns successfully, token_buffer is *not* null-terminated (as it + may contain \0 bytes), and token_buffer.size() is the number of bytes in the + string. + + @return token_type::value_string if string could be successfully scanned, + token_type::parse_error otherwise + + @note In case of errors, variable error_message contains a textual + description. + */ + token_type scan_string() + { + // reset token_buffer (ignore opening quote) + reset(); + + // we entered the function by reading an open quote + JSON_ASSERT(current == '\"'); + + while (true) + { + // get the next character + switch (get()) + { + // end of file while parsing the string + case char_traits::eof(): + { + error_message = "invalid string: missing closing quote"; + return token_type::parse_error; + } + + // closing quote + case '\"': + { + return token_type::value_string; + } + + // escapes + case '\\': + { + switch (get()) + { + // quotation mark + case '\"': + add('\"'); + break; + // reverse solidus + case '\\': + add('\\'); + break; + // solidus + case '/': + add('/'); + break; + // backspace + case 'b': + add('\b'); + break; + // form feed + case 'f': + add('\f'); + break; + // line feed + case 'n': + add('\n'); + break; + // carriage return + case 'r': + add('\r'); + break; + // tab + case 't': + add('\t'); + break; + + // unicode escapes + case 'u': + { + const int codepoint1 = get_codepoint(); + int codepoint = codepoint1; // start with codepoint1 + + if (JSON_HEDLEY_UNLIKELY(codepoint1 == -1)) + { + error_message = "invalid string: '\\u' must be followed by 4 hex digits"; + return token_type::parse_error; + } + + // check if code point is a high surrogate + if (0xD800 <= codepoint1 && codepoint1 <= 0xDBFF) + { + // expect next \uxxxx entry + if (JSON_HEDLEY_LIKELY(get() == '\\' && get() == 'u')) + { + const int codepoint2 = get_codepoint(); + + if (JSON_HEDLEY_UNLIKELY(codepoint2 == -1)) + { + error_message = "invalid string: '\\u' must be followed by 4 hex digits"; + return token_type::parse_error; + } + + // check if codepoint2 is a low surrogate + if (JSON_HEDLEY_LIKELY(0xDC00 <= codepoint2 && codepoint2 <= 0xDFFF)) + { + // overwrite codepoint + codepoint = static_cast( + // high surrogate occupies the most significant 22 bits + (static_cast(codepoint1) << 10u) + // low surrogate occupies the least significant 15 bits + + static_cast(codepoint2) + // there is still the 0xD800, 0xDC00, and 0x10000 noise + // in the result, so we have to subtract with: + // (0xD800 << 10) + DC00 - 0x10000 = 0x35FDC00 + - 0x35FDC00u); + } + else + { + error_message = "invalid string: surrogate U+D800..U+DBFF must be followed by U+DC00..U+DFFF"; + return token_type::parse_error; + } + } + else + { + error_message = "invalid string: surrogate U+D800..U+DBFF must be followed by U+DC00..U+DFFF"; + return token_type::parse_error; + } + } + else + { + if (JSON_HEDLEY_UNLIKELY(0xDC00 <= codepoint1 && codepoint1 <= 0xDFFF)) + { + error_message = "invalid string: surrogate U+DC00..U+DFFF must follow U+D800..U+DBFF"; + return token_type::parse_error; + } + } + + // the result of the above calculation yields a proper codepoint + JSON_ASSERT(0x00 <= codepoint && codepoint <= 0x10FFFF); + + // translate codepoint into bytes + if (codepoint < 0x80) + { + // 1-byte characters: 0xxxxxxx (ASCII) + add(static_cast(codepoint)); + } + else if (codepoint <= 0x7FF) + { + // 2-byte characters: 110xxxxx 10xxxxxx + add(static_cast(0xC0u | (static_cast(codepoint) >> 6u))); + add(static_cast(0x80u | (static_cast(codepoint) & 0x3Fu))); + } + else if (codepoint <= 0xFFFF) + { + // 3-byte characters: 1110xxxx 10xxxxxx 10xxxxxx + add(static_cast(0xE0u | (static_cast(codepoint) >> 12u))); + add(static_cast(0x80u | ((static_cast(codepoint) >> 6u) & 0x3Fu))); + add(static_cast(0x80u | (static_cast(codepoint) & 0x3Fu))); + } + else + { + // 4-byte characters: 11110xxx 10xxxxxx 10xxxxxx 10xxxxxx + add(static_cast(0xF0u | (static_cast(codepoint) >> 18u))); + add(static_cast(0x80u | ((static_cast(codepoint) >> 12u) & 0x3Fu))); + add(static_cast(0x80u | ((static_cast(codepoint) >> 6u) & 0x3Fu))); + add(static_cast(0x80u | (static_cast(codepoint) & 0x3Fu))); + } + + break; + } + + // other characters after escape + default: + error_message = "invalid string: forbidden character after backslash"; + return token_type::parse_error; + } + + break; + } + + // invalid control characters + case 0x00: + { + error_message = "invalid string: control character U+0000 (NUL) must be escaped to \\u0000"; + return token_type::parse_error; + } + + case 0x01: + { + error_message = "invalid string: control character U+0001 (SOH) must be escaped to \\u0001"; + return token_type::parse_error; + } + + case 0x02: + { + error_message = "invalid string: control character U+0002 (STX) must be escaped to \\u0002"; + return token_type::parse_error; + } + + case 0x03: + { + error_message = "invalid string: control character U+0003 (ETX) must be escaped to \\u0003"; + return token_type::parse_error; + } + + case 0x04: + { + error_message = "invalid string: control character U+0004 (EOT) must be escaped to \\u0004"; + return token_type::parse_error; + } + + case 0x05: + { + error_message = "invalid string: control character U+0005 (ENQ) must be escaped to \\u0005"; + return token_type::parse_error; + } + + case 0x06: + { + error_message = "invalid string: control character U+0006 (ACK) must be escaped to \\u0006"; + return token_type::parse_error; + } + + case 0x07: + { + error_message = "invalid string: control character U+0007 (BEL) must be escaped to \\u0007"; + return token_type::parse_error; + } + + case 0x08: + { + error_message = "invalid string: control character U+0008 (BS) must be escaped to \\u0008 or \\b"; + return token_type::parse_error; + } + + case 0x09: + { + error_message = "invalid string: control character U+0009 (HT) must be escaped to \\u0009 or \\t"; + return token_type::parse_error; + } + + case 0x0A: + { + error_message = "invalid string: control character U+000A (LF) must be escaped to \\u000A or \\n"; + return token_type::parse_error; + } + + case 0x0B: + { + error_message = "invalid string: control character U+000B (VT) must be escaped to \\u000B"; + return token_type::parse_error; + } + + case 0x0C: + { + error_message = "invalid string: control character U+000C (FF) must be escaped to \\u000C or \\f"; + return token_type::parse_error; + } + + case 0x0D: + { + error_message = "invalid string: control character U+000D (CR) must be escaped to \\u000D or \\r"; + return token_type::parse_error; + } + + case 0x0E: + { + error_message = "invalid string: control character U+000E (SO) must be escaped to \\u000E"; + return token_type::parse_error; + } + + case 0x0F: + { + error_message = "invalid string: control character U+000F (SI) must be escaped to \\u000F"; + return token_type::parse_error; + } + + case 0x10: + { + error_message = "invalid string: control character U+0010 (DLE) must be escaped to \\u0010"; + return token_type::parse_error; + } + + case 0x11: + { + error_message = "invalid string: control character U+0011 (DC1) must be escaped to \\u0011"; + return token_type::parse_error; + } + + case 0x12: + { + error_message = "invalid string: control character U+0012 (DC2) must be escaped to \\u0012"; + return token_type::parse_error; + } + + case 0x13: + { + error_message = "invalid string: control character U+0013 (DC3) must be escaped to \\u0013"; + return token_type::parse_error; + } + + case 0x14: + { + error_message = "invalid string: control character U+0014 (DC4) must be escaped to \\u0014"; + return token_type::parse_error; + } + + case 0x15: + { + error_message = "invalid string: control character U+0015 (NAK) must be escaped to \\u0015"; + return token_type::parse_error; + } + + case 0x16: + { + error_message = "invalid string: control character U+0016 (SYN) must be escaped to \\u0016"; + return token_type::parse_error; + } + + case 0x17: + { + error_message = "invalid string: control character U+0017 (ETB) must be escaped to \\u0017"; + return token_type::parse_error; + } + + case 0x18: + { + error_message = "invalid string: control character U+0018 (CAN) must be escaped to \\u0018"; + return token_type::parse_error; + } + + case 0x19: + { + error_message = "invalid string: control character U+0019 (EM) must be escaped to \\u0019"; + return token_type::parse_error; + } + + case 0x1A: + { + error_message = "invalid string: control character U+001A (SUB) must be escaped to \\u001A"; + return token_type::parse_error; + } + + case 0x1B: + { + error_message = "invalid string: control character U+001B (ESC) must be escaped to \\u001B"; + return token_type::parse_error; + } + + case 0x1C: + { + error_message = "invalid string: control character U+001C (FS) must be escaped to \\u001C"; + return token_type::parse_error; + } + + case 0x1D: + { + error_message = "invalid string: control character U+001D (GS) must be escaped to \\u001D"; + return token_type::parse_error; + } + + case 0x1E: + { + error_message = "invalid string: control character U+001E (RS) must be escaped to \\u001E"; + return token_type::parse_error; + } + + case 0x1F: + { + error_message = "invalid string: control character U+001F (US) must be escaped to \\u001F"; + return token_type::parse_error; + } + + // U+0020..U+007F (except U+0022 (quote) and U+005C (backspace)) + case 0x20: + case 0x21: + case 0x23: + case 0x24: + case 0x25: + case 0x26: + case 0x27: + case 0x28: + case 0x29: + case 0x2A: + case 0x2B: + case 0x2C: + case 0x2D: + case 0x2E: + case 0x2F: + case 0x30: + case 0x31: + case 0x32: + case 0x33: + case 0x34: + case 0x35: + case 0x36: + case 0x37: + case 0x38: + case 0x39: + case 0x3A: + case 0x3B: + case 0x3C: + case 0x3D: + case 0x3E: + case 0x3F: + case 0x40: + case 0x41: + case 0x42: + case 0x43: + case 0x44: + case 0x45: + case 0x46: + case 0x47: + case 0x48: + case 0x49: + case 0x4A: + case 0x4B: + case 0x4C: + case 0x4D: + case 0x4E: + case 0x4F: + case 0x50: + case 0x51: + case 0x52: + case 0x53: + case 0x54: + case 0x55: + case 0x56: + case 0x57: + case 0x58: + case 0x59: + case 0x5A: + case 0x5B: + case 0x5D: + case 0x5E: + case 0x5F: + case 0x60: + case 0x61: + case 0x62: + case 0x63: + case 0x64: + case 0x65: + case 0x66: + case 0x67: + case 0x68: + case 0x69: + case 0x6A: + case 0x6B: + case 0x6C: + case 0x6D: + case 0x6E: + case 0x6F: + case 0x70: + case 0x71: + case 0x72: + case 0x73: + case 0x74: + case 0x75: + case 0x76: + case 0x77: + case 0x78: + case 0x79: + case 0x7A: + case 0x7B: + case 0x7C: + case 0x7D: + case 0x7E: + case 0x7F: + { + add(current); + break; + } + + // U+0080..U+07FF: bytes C2..DF 80..BF + case 0xC2: + case 0xC3: + case 0xC4: + case 0xC5: + case 0xC6: + case 0xC7: + case 0xC8: + case 0xC9: + case 0xCA: + case 0xCB: + case 0xCC: + case 0xCD: + case 0xCE: + case 0xCF: + case 0xD0: + case 0xD1: + case 0xD2: + case 0xD3: + case 0xD4: + case 0xD5: + case 0xD6: + case 0xD7: + case 0xD8: + case 0xD9: + case 0xDA: + case 0xDB: + case 0xDC: + case 0xDD: + case 0xDE: + case 0xDF: + { + if (JSON_HEDLEY_UNLIKELY(!next_byte_in_range({0x80, 0xBF}))) + { + return token_type::parse_error; + } + break; + } + + // U+0800..U+0FFF: bytes E0 A0..BF 80..BF + case 0xE0: + { + if (JSON_HEDLEY_UNLIKELY(!(next_byte_in_range({0xA0, 0xBF, 0x80, 0xBF})))) + { + return token_type::parse_error; + } + break; + } + + // U+1000..U+CFFF: bytes E1..EC 80..BF 80..BF + // U+E000..U+FFFF: bytes EE..EF 80..BF 80..BF + case 0xE1: + case 0xE2: + case 0xE3: + case 0xE4: + case 0xE5: + case 0xE6: + case 0xE7: + case 0xE8: + case 0xE9: + case 0xEA: + case 0xEB: + case 0xEC: + case 0xEE: + case 0xEF: + { + if (JSON_HEDLEY_UNLIKELY(!(next_byte_in_range({0x80, 0xBF, 0x80, 0xBF})))) + { + return token_type::parse_error; + } + break; + } + + // U+D000..U+D7FF: bytes ED 80..9F 80..BF + case 0xED: + { + if (JSON_HEDLEY_UNLIKELY(!(next_byte_in_range({0x80, 0x9F, 0x80, 0xBF})))) + { + return token_type::parse_error; + } + break; + } + + // U+10000..U+3FFFF F0 90..BF 80..BF 80..BF + case 0xF0: + { + if (JSON_HEDLEY_UNLIKELY(!(next_byte_in_range({0x90, 0xBF, 0x80, 0xBF, 0x80, 0xBF})))) + { + return token_type::parse_error; + } + break; + } + + // U+40000..U+FFFFF F1..F3 80..BF 80..BF 80..BF + case 0xF1: + case 0xF2: + case 0xF3: + { + if (JSON_HEDLEY_UNLIKELY(!(next_byte_in_range({0x80, 0xBF, 0x80, 0xBF, 0x80, 0xBF})))) + { + return token_type::parse_error; + } + break; + } + + // U+100000..U+10FFFF F4 80..8F 80..BF 80..BF + case 0xF4: + { + if (JSON_HEDLEY_UNLIKELY(!(next_byte_in_range({0x80, 0x8F, 0x80, 0xBF, 0x80, 0xBF})))) + { + return token_type::parse_error; + } + break; + } + + // the remaining bytes (80..C1 and F5..FF) are ill-formed + default: + { + error_message = "invalid string: ill-formed UTF-8 byte"; + return token_type::parse_error; + } + } + } + } + + /*! + * @brief scan a comment + * @return whether comment could be scanned successfully + */ + bool scan_comment() + { + switch (get()) + { + // single-line comments skip input until a newline or EOF is read + case '/': + { + while (true) + { + switch (get()) + { + case '\n': + case '\r': + case char_traits::eof(): + case '\0': + return true; + + default: + break; + } + } + } + + // multi-line comments skip input until */ is read + case '*': + { + while (true) + { + switch (get()) + { + case char_traits::eof(): + case '\0': + { + error_message = "invalid comment; missing closing '*/'"; + return false; + } + + case '*': + { + switch (get()) + { + case '/': + return true; + + default: + { + unget(); + continue; + } + } + } + + default: + continue; + } + } + } + + // unexpected character after reading '/' + default: + { + error_message = "invalid comment; expecting '/' or '*' after '/'"; + return false; + } + } + } + + JSON_HEDLEY_NON_NULL(2) + static void strtof(float& f, const char* str, char** endptr) noexcept + { + f = std::strtof(str, endptr); + } + + JSON_HEDLEY_NON_NULL(2) + static void strtof(double& f, const char* str, char** endptr) noexcept + { + f = std::strtod(str, endptr); + } + + JSON_HEDLEY_NON_NULL(2) + static void strtof(long double& f, const char* str, char** endptr) noexcept + { + f = std::strtold(str, endptr); + } + + /*! + @brief scan a number literal + + This function scans a string according to Sect. 6 of RFC 8259. + + The function is realized with a deterministic finite state machine derived + from the grammar described in RFC 8259. Starting in state "init", the + input is read and used to determined the next state. Only state "done" + accepts the number. State "error" is a trap state to model errors. In the + table below, "anything" means any character but the ones listed before. + + state | 0 | 1-9 | e E | + | - | . | anything + ---------|----------|----------|----------|---------|---------|----------|----------- + init | zero | any1 | [error] | [error] | minus | [error] | [error] + minus | zero | any1 | [error] | [error] | [error] | [error] | [error] + zero | done | done | exponent | done | done | decimal1 | done + any1 | any1 | any1 | exponent | done | done | decimal1 | done + decimal1 | decimal2 | decimal2 | [error] | [error] | [error] | [error] | [error] + decimal2 | decimal2 | decimal2 | exponent | done | done | done | done + exponent | any2 | any2 | [error] | sign | sign | [error] | [error] + sign | any2 | any2 | [error] | [error] | [error] | [error] | [error] + any2 | any2 | any2 | done | done | done | done | done + + The state machine is realized with one label per state (prefixed with + "scan_number_") and `goto` statements between them. The state machine + contains cycles, but any cycle can be left when EOF is read. Therefore, + the function is guaranteed to terminate. + + During scanning, the read bytes are stored in token_buffer. This string is + then converted to a signed integer, an unsigned integer, or a + floating-point number. + + @return token_type::value_unsigned, token_type::value_integer, or + token_type::value_float if number could be successfully scanned, + token_type::parse_error otherwise + + @note The scanner is independent of the current locale. Internally, the + locale's decimal point is used instead of `.` to work with the + locale-dependent converters. + */ + token_type scan_number() // lgtm [cpp/use-of-goto] `goto` is used in this function to implement the number-parsing state machine described above. By design, any finite input will eventually reach the "done" state or return token_type::parse_error. In each intermediate state, 1 byte of the input is appended to the token_buffer vector, and only the already initialized variables token_buffer, number_type, and error_message are manipulated. + { + // reset token_buffer to store the number's bytes + reset(); + + // the type of the parsed number; initially set to unsigned; will be + // changed if minus sign, decimal point, or exponent is read + token_type number_type = token_type::value_unsigned; + + // state (init): we just found out we need to scan a number + switch (current) + { + case '-': + { + add(current); + goto scan_number_minus; + } + + case '0': + { + add(current); + goto scan_number_zero; + } + + case '1': + case '2': + case '3': + case '4': + case '5': + case '6': + case '7': + case '8': + case '9': + { + add(current); + goto scan_number_any1; + } + + // all other characters are rejected outside scan_number() + default: // LCOV_EXCL_LINE + JSON_ASSERT(false); // NOLINT(cert-dcl03-c,hicpp-static-assert,misc-static-assert) LCOV_EXCL_LINE + } + +scan_number_minus: + // state: we just parsed a leading minus sign + number_type = token_type::value_integer; + switch (get()) + { + case '0': + { + add(current); + goto scan_number_zero; + } + + case '1': + case '2': + case '3': + case '4': + case '5': + case '6': + case '7': + case '8': + case '9': + { + add(current); + goto scan_number_any1; + } + + default: + { + error_message = "invalid number; expected digit after '-'"; + return token_type::parse_error; + } + } + +scan_number_zero: + // state: we just parse a zero (maybe with a leading minus sign) + switch (get()) + { + case '.': + { + add(decimal_point_char); + decimal_point_position = token_buffer.size() - 1; + goto scan_number_decimal1; + } + + case 'e': + case 'E': + { + add(current); + goto scan_number_exponent; + } + + default: + goto scan_number_done; + } + +scan_number_any1: + // state: we just parsed a number 0-9 (maybe with a leading minus sign) + switch (get()) + { + case '0': + case '1': + case '2': + case '3': + case '4': + case '5': + case '6': + case '7': + case '8': + case '9': + { + add(current); + goto scan_number_any1; + } + + case '.': + { + add(decimal_point_char); + decimal_point_position = token_buffer.size() - 1; + goto scan_number_decimal1; + } + + case 'e': + case 'E': + { + add(current); + goto scan_number_exponent; + } + + default: + goto scan_number_done; + } + +scan_number_decimal1: + // state: we just parsed a decimal point + number_type = token_type::value_float; + switch (get()) + { + case '0': + case '1': + case '2': + case '3': + case '4': + case '5': + case '6': + case '7': + case '8': + case '9': + { + add(current); + goto scan_number_decimal2; + } + + default: + { + error_message = "invalid number; expected digit after '.'"; + return token_type::parse_error; + } + } + +scan_number_decimal2: + // we just parsed at least one number after a decimal point + switch (get()) + { + case '0': + case '1': + case '2': + case '3': + case '4': + case '5': + case '6': + case '7': + case '8': + case '9': + { + add(current); + goto scan_number_decimal2; + } + + case 'e': + case 'E': + { + add(current); + goto scan_number_exponent; + } + + default: + goto scan_number_done; + } + +scan_number_exponent: + // we just parsed an exponent + number_type = token_type::value_float; + switch (get()) + { + case '+': + case '-': + { + add(current); + goto scan_number_sign; + } + + case '0': + case '1': + case '2': + case '3': + case '4': + case '5': + case '6': + case '7': + case '8': + case '9': + { + add(current); + goto scan_number_any2; + } + + default: + { + error_message = + "invalid number; expected '+', '-', or digit after exponent"; + return token_type::parse_error; + } + } + +scan_number_sign: + // we just parsed an exponent sign + switch (get()) + { + case '0': + case '1': + case '2': + case '3': + case '4': + case '5': + case '6': + case '7': + case '8': + case '9': + { + add(current); + goto scan_number_any2; + } + + default: + { + error_message = "invalid number; expected digit after exponent sign"; + return token_type::parse_error; + } + } + +scan_number_any2: + // we just parsed a number after the exponent or exponent sign + switch (get()) + { + case '0': + case '1': + case '2': + case '3': + case '4': + case '5': + case '6': + case '7': + case '8': + case '9': + { + add(current); + goto scan_number_any2; + } + + default: + goto scan_number_done; + } + +scan_number_done: + // unget the character after the number (we only read it to know that + // we are done scanning a number) + unget(); + + char* endptr = nullptr; // NOLINT(misc-const-correctness,cppcoreguidelines-pro-type-vararg,hicpp-vararg) + errno = 0; + + // try to parse integers first and fall back to floats + if (number_type == token_type::value_unsigned) + { + const auto x = std::strtoull(token_buffer.data(), &endptr, 10); + + // we checked the number format before + JSON_ASSERT(endptr == token_buffer.data() + token_buffer.size()); + + if (errno != ERANGE) + { + value_unsigned = static_cast(x); + if (value_unsigned == x) + { + return token_type::value_unsigned; + } + } + } + else if (number_type == token_type::value_integer) + { + const auto x = std::strtoll(token_buffer.data(), &endptr, 10); + + // we checked the number format before + JSON_ASSERT(endptr == token_buffer.data() + token_buffer.size()); + + if (errno != ERANGE) + { + value_integer = static_cast(x); + if (value_integer == x) + { + return token_type::value_integer; + } + } + } + + // this code is reached if we parse a floating-point number or if an + // integer conversion above failed + strtof(value_float, token_buffer.data(), &endptr); + + // we checked the number format before + JSON_ASSERT(endptr == token_buffer.data() + token_buffer.size()); + + return token_type::value_float; + } + + /*! + @param[in] literal_text the literal text to expect + @param[in] length the length of the passed literal text + @param[in] return_type the token type to return on success + */ + JSON_HEDLEY_NON_NULL(2) + token_type scan_literal(const char_type* literal_text, const std::size_t length, + token_type return_type) + { + JSON_ASSERT(char_traits::to_char_type(current) == literal_text[0]); + for (std::size_t i = 1; i < length; ++i) + { + if (JSON_HEDLEY_UNLIKELY(char_traits::to_char_type(get()) != literal_text[i])) + { + error_message = "invalid literal"; + return token_type::parse_error; + } + } + return return_type; + } + + ///////////////////// + // input management + ///////////////////// + + /// reset token_buffer; current character is beginning of token + void reset() noexcept + { + token_buffer.clear(); + token_string.clear(); + decimal_point_position = std::string::npos; + token_string.push_back(char_traits::to_char_type(current)); + } + + /* + @brief get next character from the input + + This function provides the interface to the used input adapter. It does + not throw in case the input reached EOF, but returns a + `char_traits::eof()` in that case. Stores the scanned characters + for use in error messages. + + @return character read from the input + */ + char_int_type get() + { + ++position.chars_read_total; + ++position.chars_read_current_line; + + if (next_unget) + { + // only reset the next_unget variable and work with current + next_unget = false; + } + else + { + current = ia.get_character(); + } + + if (JSON_HEDLEY_LIKELY(current != char_traits::eof())) + { + token_string.push_back(char_traits::to_char_type(current)); + } + + if (current == '\n') + { + ++position.lines_read; + position.chars_read_current_line = 0; + } + + return current; + } + + /*! + @brief unget current character (read it again on next get) + + We implement unget by setting variable next_unget to true. The input is not + changed - we just simulate ungetting by modifying chars_read_total, + chars_read_current_line, and token_string. The next call to get() will + behave as if the unget character is read again. + */ + void unget() + { + next_unget = true; + + --position.chars_read_total; + + // in case we "unget" a newline, we have to also decrement the lines_read + if (position.chars_read_current_line == 0) + { + if (position.lines_read > 0) + { + --position.lines_read; + } + } + else + { + --position.chars_read_current_line; + } + + if (JSON_HEDLEY_LIKELY(current != char_traits::eof())) + { + JSON_ASSERT(!token_string.empty()); + token_string.pop_back(); + } + } + + /// add a character to token_buffer + void add(char_int_type c) + { + token_buffer.push_back(static_cast(c)); + } + + public: + ///////////////////// + // value getters + ///////////////////// + + /// return integer value + constexpr number_integer_t get_number_integer() const noexcept + { + return value_integer; + } + + /// return unsigned integer value + constexpr number_unsigned_t get_number_unsigned() const noexcept + { + return value_unsigned; + } + + /// return floating-point value + constexpr number_float_t get_number_float() const noexcept + { + return value_float; + } + + /// return current string value (implicitly resets the token; useful only once) + string_t& get_string() + { + // translate decimal points from locale back to '.' (#4084) + if (decimal_point_char != '.' && decimal_point_position != std::string::npos) + { + token_buffer[decimal_point_position] = '.'; + } + return token_buffer; + } + + ///////////////////// + // diagnostics + ///////////////////// + + /// return position of last read token + constexpr position_t get_position() const noexcept + { + return position; + } + + /// return the last read token (for errors only). Will never contain EOF + /// (an arbitrary value that is not a valid char value, often -1), because + /// 255 may legitimately occur. May contain NUL, which should be escaped. + std::string get_token_string() const + { + // escape control characters + std::string result; + for (const auto c : token_string) + { + if (static_cast(c) <= '\x1F') + { + // escape control characters + std::array cs{{}}; + static_cast((std::snprintf)(cs.data(), cs.size(), "", static_cast(c))); // NOLINT(cppcoreguidelines-pro-type-vararg,hicpp-vararg) + result += cs.data(); + } + else + { + // add character as is + result.push_back(static_cast(c)); + } + } + + return result; + } + + /// return syntax error message + JSON_HEDLEY_RETURNS_NON_NULL + constexpr const char* get_error_message() const noexcept + { + return error_message; + } + + ///////////////////// + // actual scanner + ///////////////////// + + /*! + @brief skip the UTF-8 byte order mark + @return true iff there is no BOM or the correct BOM has been skipped + */ + bool skip_bom() + { + if (get() == 0xEF) + { + // check if we completely parse the BOM + return get() == 0xBB && get() == 0xBF; + } + + // the first character is not the beginning of the BOM; unget it to + // process is later + unget(); + return true; + } + + void skip_whitespace() + { + do + { + get(); + } + while (current == ' ' || current == '\t' || current == '\n' || current == '\r'); + } + + token_type scan() + { + // initially, skip the BOM + if (position.chars_read_total == 0 && !skip_bom()) + { + error_message = "invalid BOM; must be 0xEF 0xBB 0xBF if given"; + return token_type::parse_error; + } + + // read the next character and ignore whitespace + skip_whitespace(); + + // ignore comments + while (ignore_comments && current == '/') + { + if (!scan_comment()) + { + return token_type::parse_error; + } + + // skip following whitespace + skip_whitespace(); + } + + switch (current) + { + // structural characters + case '[': + return token_type::begin_array; + case ']': + return token_type::end_array; + case '{': + return token_type::begin_object; + case '}': + return token_type::end_object; + case ':': + return token_type::name_separator; + case ',': + return token_type::value_separator; + + // literals + case 't': + { + std::array true_literal = {{static_cast('t'), static_cast('r'), static_cast('u'), static_cast('e')}}; + return scan_literal(true_literal.data(), true_literal.size(), token_type::literal_true); + } + case 'f': + { + std::array false_literal = {{static_cast('f'), static_cast('a'), static_cast('l'), static_cast('s'), static_cast('e')}}; + return scan_literal(false_literal.data(), false_literal.size(), token_type::literal_false); + } + case 'n': + { + std::array null_literal = {{static_cast('n'), static_cast('u'), static_cast('l'), static_cast('l')}}; + return scan_literal(null_literal.data(), null_literal.size(), token_type::literal_null); + } + + // string + case '\"': + return scan_string(); + + // number + case '-': + case '0': + case '1': + case '2': + case '3': + case '4': + case '5': + case '6': + case '7': + case '8': + case '9': + return scan_number(); + + // end of input (the null byte is needed when parsing from + // string literals) + case '\0': + case char_traits::eof(): + return token_type::end_of_input; + + // error + default: + error_message = "invalid literal"; + return token_type::parse_error; + } + } + + private: + /// input adapter + InputAdapterType ia; + + /// whether comments should be ignored (true) or signaled as errors (false) + const bool ignore_comments = false; + + /// the current character + char_int_type current = char_traits::eof(); + + /// whether the next get() call should just return current + bool next_unget = false; + + /// the start position of the current token + position_t position {}; + + /// raw input token string (for error messages) + std::vector token_string {}; + + /// buffer for variable-length tokens (numbers, strings) + string_t token_buffer {}; + + /// a description of occurred lexer errors + const char* error_message = ""; + + // number values + number_integer_t value_integer = 0; + number_unsigned_t value_unsigned = 0; + number_float_t value_float = 0; + + /// the decimal point + const char_int_type decimal_point_char = '.'; + /// the position of the decimal point in the input + std::size_t decimal_point_position = std::string::npos; +}; + +} // namespace detail +NLOHMANN_JSON_NAMESPACE_END diff --git a/src/mc/include/nlohmann/detail/input/parser.hpp b/src/mc/include/nlohmann/detail/input/parser.hpp new file mode 100755 index 0000000..2ddd38c --- /dev/null +++ b/src/mc/include/nlohmann/detail/input/parser.hpp @@ -0,0 +1,519 @@ +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.12.0 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2013 - 2025 Niels Lohmann +// SPDX-License-Identifier: MIT + +#pragma once + +#include // isfinite +#include // uint8_t +#include // function +#include // string +#include // move +#include // vector + +#include +#include +#include +#include +#include +#include +#include +#include + +NLOHMANN_JSON_NAMESPACE_BEGIN +namespace detail +{ +//////////// +// parser // +//////////// + +enum class parse_event_t : std::uint8_t +{ + /// the parser read `{` and started to process a JSON object + object_start, + /// the parser read `}` and finished processing a JSON object + object_end, + /// the parser read `[` and started to process a JSON array + array_start, + /// the parser read `]` and finished processing a JSON array + array_end, + /// the parser read a key of a value in an object + key, + /// the parser finished reading a JSON value + value +}; + +template +using parser_callback_t = + std::function; + +/*! +@brief syntax analysis + +This class implements a recursive descent parser. +*/ +template +class parser +{ + using number_integer_t = typename BasicJsonType::number_integer_t; + using number_unsigned_t = typename BasicJsonType::number_unsigned_t; + using number_float_t = typename BasicJsonType::number_float_t; + using string_t = typename BasicJsonType::string_t; + using lexer_t = lexer; + using token_type = typename lexer_t::token_type; + + public: + /// a parser reading from an input adapter + explicit parser(InputAdapterType&& adapter, + parser_callback_t cb = nullptr, + const bool allow_exceptions_ = true, + const bool skip_comments = false) + : callback(std::move(cb)) + , m_lexer(std::move(adapter), skip_comments) + , allow_exceptions(allow_exceptions_) + { + // read first token + get_token(); + } + + /*! + @brief public parser interface + + @param[in] strict whether to expect the last token to be EOF + @param[in,out] result parsed JSON value + + @throw parse_error.101 in case of an unexpected token + @throw parse_error.102 if to_unicode fails or surrogate error + @throw parse_error.103 if to_unicode fails + */ + void parse(const bool strict, BasicJsonType& result) + { + if (callback) + { + json_sax_dom_callback_parser sdp(result, callback, allow_exceptions, &m_lexer); + sax_parse_internal(&sdp); + + // in strict mode, input must be completely read + if (strict && (get_token() != token_type::end_of_input)) + { + sdp.parse_error(m_lexer.get_position(), + m_lexer.get_token_string(), + parse_error::create(101, m_lexer.get_position(), + exception_message(token_type::end_of_input, "value"), nullptr)); + } + + // in case of an error, return a discarded value + if (sdp.is_errored()) + { + result = value_t::discarded; + return; + } + + // set top-level value to null if it was discarded by the callback + // function + if (result.is_discarded()) + { + result = nullptr; + } + } + else + { + json_sax_dom_parser sdp(result, allow_exceptions, &m_lexer); + sax_parse_internal(&sdp); + + // in strict mode, input must be completely read + if (strict && (get_token() != token_type::end_of_input)) + { + sdp.parse_error(m_lexer.get_position(), + m_lexer.get_token_string(), + parse_error::create(101, m_lexer.get_position(), exception_message(token_type::end_of_input, "value"), nullptr)); + } + + // in case of an error, return a discarded value + if (sdp.is_errored()) + { + result = value_t::discarded; + return; + } + } + + result.assert_invariant(); + } + + /*! + @brief public accept interface + + @param[in] strict whether to expect the last token to be EOF + @return whether the input is a proper JSON text + */ + bool accept(const bool strict = true) + { + json_sax_acceptor sax_acceptor; + return sax_parse(&sax_acceptor, strict); + } + + template + JSON_HEDLEY_NON_NULL(2) + bool sax_parse(SAX* sax, const bool strict = true) + { + (void)detail::is_sax_static_asserts {}; + const bool result = sax_parse_internal(sax); + + // strict mode: next byte must be EOF + if (result && strict && (get_token() != token_type::end_of_input)) + { + return sax->parse_error(m_lexer.get_position(), + m_lexer.get_token_string(), + parse_error::create(101, m_lexer.get_position(), exception_message(token_type::end_of_input, "value"), nullptr)); + } + + return result; + } + + private: + template + JSON_HEDLEY_NON_NULL(2) + bool sax_parse_internal(SAX* sax) + { + // stack to remember the hierarchy of structured values we are parsing + // true = array; false = object + std::vector states; + // value to avoid a goto (see comment where set to true) + bool skip_to_state_evaluation = false; + + while (true) + { + if (!skip_to_state_evaluation) + { + // invariant: get_token() was called before each iteration + switch (last_token) + { + case token_type::begin_object: + { + if (JSON_HEDLEY_UNLIKELY(!sax->start_object(detail::unknown_size()))) + { + return false; + } + + // closing } -> we are done + if (get_token() == token_type::end_object) + { + if (JSON_HEDLEY_UNLIKELY(!sax->end_object())) + { + return false; + } + break; + } + + // parse key + if (JSON_HEDLEY_UNLIKELY(last_token != token_type::value_string)) + { + return sax->parse_error(m_lexer.get_position(), + m_lexer.get_token_string(), + parse_error::create(101, m_lexer.get_position(), exception_message(token_type::value_string, "object key"), nullptr)); + } + if (JSON_HEDLEY_UNLIKELY(!sax->key(m_lexer.get_string()))) + { + return false; + } + + // parse separator (:) + if (JSON_HEDLEY_UNLIKELY(get_token() != token_type::name_separator)) + { + return sax->parse_error(m_lexer.get_position(), + m_lexer.get_token_string(), + parse_error::create(101, m_lexer.get_position(), exception_message(token_type::name_separator, "object separator"), nullptr)); + } + + // remember we are now inside an object + states.push_back(false); + + // parse values + get_token(); + continue; + } + + case token_type::begin_array: + { + if (JSON_HEDLEY_UNLIKELY(!sax->start_array(detail::unknown_size()))) + { + return false; + } + + // closing ] -> we are done + if (get_token() == token_type::end_array) + { + if (JSON_HEDLEY_UNLIKELY(!sax->end_array())) + { + return false; + } + break; + } + + // remember we are now inside an array + states.push_back(true); + + // parse values (no need to call get_token) + continue; + } + + case token_type::value_float: + { + const auto res = m_lexer.get_number_float(); + + if (JSON_HEDLEY_UNLIKELY(!std::isfinite(res))) + { + return sax->parse_error(m_lexer.get_position(), + m_lexer.get_token_string(), + out_of_range::create(406, concat("number overflow parsing '", m_lexer.get_token_string(), '\''), nullptr)); + } + + if (JSON_HEDLEY_UNLIKELY(!sax->number_float(res, m_lexer.get_string()))) + { + return false; + } + + break; + } + + case token_type::literal_false: + { + if (JSON_HEDLEY_UNLIKELY(!sax->boolean(false))) + { + return false; + } + break; + } + + case token_type::literal_null: + { + if (JSON_HEDLEY_UNLIKELY(!sax->null())) + { + return false; + } + break; + } + + case token_type::literal_true: + { + if (JSON_HEDLEY_UNLIKELY(!sax->boolean(true))) + { + return false; + } + break; + } + + case token_type::value_integer: + { + if (JSON_HEDLEY_UNLIKELY(!sax->number_integer(m_lexer.get_number_integer()))) + { + return false; + } + break; + } + + case token_type::value_string: + { + if (JSON_HEDLEY_UNLIKELY(!sax->string(m_lexer.get_string()))) + { + return false; + } + break; + } + + case token_type::value_unsigned: + { + if (JSON_HEDLEY_UNLIKELY(!sax->number_unsigned(m_lexer.get_number_unsigned()))) + { + return false; + } + break; + } + + case token_type::parse_error: + { + // using "uninitialized" to avoid an "expected" message + return sax->parse_error(m_lexer.get_position(), + m_lexer.get_token_string(), + parse_error::create(101, m_lexer.get_position(), exception_message(token_type::uninitialized, "value"), nullptr)); + } + case token_type::end_of_input: + { + if (JSON_HEDLEY_UNLIKELY(m_lexer.get_position().chars_read_total == 1)) + { + return sax->parse_error(m_lexer.get_position(), + m_lexer.get_token_string(), + parse_error::create(101, m_lexer.get_position(), + "attempting to parse an empty input; check that your input string or stream contains the expected JSON", nullptr)); + } + + return sax->parse_error(m_lexer.get_position(), + m_lexer.get_token_string(), + parse_error::create(101, m_lexer.get_position(), exception_message(token_type::literal_or_value, "value"), nullptr)); + } + case token_type::uninitialized: + case token_type::end_array: + case token_type::end_object: + case token_type::name_separator: + case token_type::value_separator: + case token_type::literal_or_value: + default: // the last token was unexpected + { + return sax->parse_error(m_lexer.get_position(), + m_lexer.get_token_string(), + parse_error::create(101, m_lexer.get_position(), exception_message(token_type::literal_or_value, "value"), nullptr)); + } + } + } + else + { + skip_to_state_evaluation = false; + } + + // we reached this line after we successfully parsed a value + if (states.empty()) + { + // empty stack: we reached the end of the hierarchy: done + return true; + } + + if (states.back()) // array + { + // comma -> next value + if (get_token() == token_type::value_separator) + { + // parse a new value + get_token(); + continue; + } + + // closing ] + if (JSON_HEDLEY_LIKELY(last_token == token_type::end_array)) + { + if (JSON_HEDLEY_UNLIKELY(!sax->end_array())) + { + return false; + } + + // We are done with this array. Before we can parse a + // new value, we need to evaluate the new state first. + // By setting skip_to_state_evaluation to false, we + // are effectively jumping to the beginning of this if. + JSON_ASSERT(!states.empty()); + states.pop_back(); + skip_to_state_evaluation = true; + continue; + } + + return sax->parse_error(m_lexer.get_position(), + m_lexer.get_token_string(), + parse_error::create(101, m_lexer.get_position(), exception_message(token_type::end_array, "array"), nullptr)); + } + + // states.back() is false -> object + + // comma -> next value + if (get_token() == token_type::value_separator) + { + // parse key + if (JSON_HEDLEY_UNLIKELY(get_token() != token_type::value_string)) + { + return sax->parse_error(m_lexer.get_position(), + m_lexer.get_token_string(), + parse_error::create(101, m_lexer.get_position(), exception_message(token_type::value_string, "object key"), nullptr)); + } + + if (JSON_HEDLEY_UNLIKELY(!sax->key(m_lexer.get_string()))) + { + return false; + } + + // parse separator (:) + if (JSON_HEDLEY_UNLIKELY(get_token() != token_type::name_separator)) + { + return sax->parse_error(m_lexer.get_position(), + m_lexer.get_token_string(), + parse_error::create(101, m_lexer.get_position(), exception_message(token_type::name_separator, "object separator"), nullptr)); + } + + // parse values + get_token(); + continue; + } + + // closing } + if (JSON_HEDLEY_LIKELY(last_token == token_type::end_object)) + { + if (JSON_HEDLEY_UNLIKELY(!sax->end_object())) + { + return false; + } + + // We are done with this object. Before we can parse a + // new value, we need to evaluate the new state first. + // By setting skip_to_state_evaluation to false, we + // are effectively jumping to the beginning of this if. + JSON_ASSERT(!states.empty()); + states.pop_back(); + skip_to_state_evaluation = true; + continue; + } + + return sax->parse_error(m_lexer.get_position(), + m_lexer.get_token_string(), + parse_error::create(101, m_lexer.get_position(), exception_message(token_type::end_object, "object"), nullptr)); + } + } + + /// get next token from lexer + token_type get_token() + { + return last_token = m_lexer.scan(); + } + + std::string exception_message(const token_type expected, const std::string& context) + { + std::string error_msg = "syntax error "; + + if (!context.empty()) + { + error_msg += concat("while parsing ", context, ' '); + } + + error_msg += "- "; + + if (last_token == token_type::parse_error) + { + error_msg += concat(m_lexer.get_error_message(), "; last read: '", + m_lexer.get_token_string(), '\''); + } + else + { + error_msg += concat("unexpected ", lexer_t::token_type_name(last_token)); + } + + if (expected != token_type::uninitialized) + { + error_msg += concat("; expected ", lexer_t::token_type_name(expected)); + } + + return error_msg; + } + + private: + /// callback function + const parser_callback_t callback = nullptr; + /// the type of the last read token + token_type last_token = token_type::uninitialized; + /// the lexer + lexer_t m_lexer; + /// whether to throw exceptions in case of errors + const bool allow_exceptions = true; +}; + +} // namespace detail +NLOHMANN_JSON_NAMESPACE_END diff --git a/src/mc/include/nlohmann/detail/input/position_t.hpp b/src/mc/include/nlohmann/detail/input/position_t.hpp new file mode 100755 index 0000000..c26c5f4 --- /dev/null +++ b/src/mc/include/nlohmann/detail/input/position_t.hpp @@ -0,0 +1,37 @@ +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.12.0 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2013 - 2025 Niels Lohmann +// SPDX-License-Identifier: MIT + +#pragma once + +#include // size_t + +#include + +NLOHMANN_JSON_NAMESPACE_BEGIN +namespace detail +{ + +/// struct to capture the start position of the current token +struct position_t +{ + /// the total number of characters read + std::size_t chars_read_total = 0; + /// the number of characters read in the current line + std::size_t chars_read_current_line = 0; + /// the number of lines read + std::size_t lines_read = 0; + + /// conversion to size_t to preserve SAX interface + constexpr operator size_t() const + { + return chars_read_total; + } +}; + +} // namespace detail +NLOHMANN_JSON_NAMESPACE_END diff --git a/src/mc/include/nlohmann/detail/iterators/internal_iterator.hpp b/src/mc/include/nlohmann/detail/iterators/internal_iterator.hpp new file mode 100755 index 0000000..9f3c8e6 --- /dev/null +++ b/src/mc/include/nlohmann/detail/iterators/internal_iterator.hpp @@ -0,0 +1,35 @@ +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.12.0 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2013 - 2025 Niels Lohmann +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +NLOHMANN_JSON_NAMESPACE_BEGIN +namespace detail +{ + +/*! +@brief an iterator value + +@note This structure could easily be a union, but MSVC currently does not allow +unions members with complex constructors, see https://github.com/nlohmann/json/pull/105. +*/ +template struct internal_iterator +{ + /// iterator for JSON objects + typename BasicJsonType::object_t::iterator object_iterator {}; + /// iterator for JSON arrays + typename BasicJsonType::array_t::iterator array_iterator {}; + /// generic iterator for all other types + primitive_iterator_t primitive_iterator {}; +}; + +} // namespace detail +NLOHMANN_JSON_NAMESPACE_END diff --git a/src/mc/include/nlohmann/detail/iterators/iter_impl.hpp b/src/mc/include/nlohmann/detail/iterators/iter_impl.hpp new file mode 100755 index 0000000..c2fbbfa --- /dev/null +++ b/src/mc/include/nlohmann/detail/iterators/iter_impl.hpp @@ -0,0 +1,760 @@ +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.12.0 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2013 - 2025 Niels Lohmann +// SPDX-License-Identifier: MIT + +#pragma once + +#include // iterator, random_access_iterator_tag, bidirectional_iterator_tag, advance, next +#include // conditional, is_const, remove_const + +#include +#include +#include +#include +#include +#include +#include + +NLOHMANN_JSON_NAMESPACE_BEGIN +namespace detail +{ + +// forward declare to be able to friend it later on +template class iteration_proxy; +template class iteration_proxy_value; + +/*! +@brief a template for a bidirectional iterator for the @ref basic_json class +This class implements a both iterators (iterator and const_iterator) for the +@ref basic_json class. +@note An iterator is called *initialized* when a pointer to a JSON value has + been set (e.g., by a constructor or a copy assignment). If the iterator is + default-constructed, it is *uninitialized* and most methods are undefined. + **The library uses assertions to detect calls on uninitialized iterators.** +@requirement The class satisfies the following concept requirements: +- +[BidirectionalIterator](https://en.cppreference.com/w/cpp/named_req/BidirectionalIterator): + The iterator that can be moved can be moved in both directions (i.e. + incremented and decremented). +@since version 1.0.0, simplified in version 2.0.9, change to bidirectional + iterators in version 3.0.0 (see https://github.com/nlohmann/json/issues/593) +*/ +template +class iter_impl // NOLINT(cppcoreguidelines-special-member-functions,hicpp-special-member-functions) +{ + /// the iterator with BasicJsonType of different const-ness + using other_iter_impl = iter_impl::value, typename std::remove_const::type, const BasicJsonType>::type>; + /// allow basic_json to access private members + friend other_iter_impl; + friend BasicJsonType; + friend iteration_proxy; + friend iteration_proxy_value; + + using object_t = typename BasicJsonType::object_t; + using array_t = typename BasicJsonType::array_t; + // make sure BasicJsonType is basic_json or const basic_json + static_assert(is_basic_json::type>::value, + "iter_impl only accepts (const) basic_json"); + // superficial check for the LegacyBidirectionalIterator named requirement + static_assert(std::is_base_of::value + && std::is_base_of::iterator_category>::value, + "basic_json iterator assumes array and object type iterators satisfy the LegacyBidirectionalIterator named requirement."); + + public: + /// The std::iterator class template (used as a base class to provide typedefs) is deprecated in C++17. + /// The C++ Standard has never required user-defined iterators to derive from std::iterator. + /// A user-defined iterator should provide publicly accessible typedefs named + /// iterator_category, value_type, difference_type, pointer, and reference. + /// Note that value_type is required to be non-const, even for constant iterators. + using iterator_category = std::bidirectional_iterator_tag; + + /// the type of the values when the iterator is dereferenced + using value_type = typename BasicJsonType::value_type; + /// a type to represent differences between iterators + using difference_type = typename BasicJsonType::difference_type; + /// defines a pointer to the type iterated over (value_type) + using pointer = typename std::conditional::value, + typename BasicJsonType::const_pointer, + typename BasicJsonType::pointer>::type; + /// defines a reference to the type iterated over (value_type) + using reference = + typename std::conditional::value, + typename BasicJsonType::const_reference, + typename BasicJsonType::reference>::type; + + iter_impl() = default; + ~iter_impl() = default; + iter_impl(iter_impl&&) noexcept = default; + iter_impl& operator=(iter_impl&&) noexcept = default; + + /*! + @brief constructor for a given JSON instance + @param[in] object pointer to a JSON object for this iterator + @pre object != nullptr + @post The iterator is initialized; i.e. `m_object != nullptr`. + */ + explicit iter_impl(pointer object) noexcept : m_object(object) + { + JSON_ASSERT(m_object != nullptr); + + switch (m_object->m_data.m_type) + { + case value_t::object: + { + m_it.object_iterator = typename object_t::iterator(); + break; + } + + case value_t::array: + { + m_it.array_iterator = typename array_t::iterator(); + break; + } + + case value_t::null: + case value_t::string: + case value_t::boolean: + case value_t::number_integer: + case value_t::number_unsigned: + case value_t::number_float: + case value_t::binary: + case value_t::discarded: + default: + { + m_it.primitive_iterator = primitive_iterator_t(); + break; + } + } + } + + /*! + @note The conventional copy constructor and copy assignment are implicitly + defined. Combined with the following converting constructor and + assignment, they support: (1) copy from iterator to iterator, (2) + copy from const iterator to const iterator, and (3) conversion from + iterator to const iterator. However conversion from const iterator + to iterator is not defined. + */ + + /*! + @brief const copy constructor + @param[in] other const iterator to copy from + @note This copy constructor had to be defined explicitly to circumvent a bug + occurring on msvc v19.0 compiler (VS 2015) debug build. For more + information refer to: https://github.com/nlohmann/json/issues/1608 + */ + iter_impl(const iter_impl& other) noexcept + : m_object(other.m_object), m_it(other.m_it) + {} + + /*! + @brief converting assignment + @param[in] other const iterator to copy from + @return const/non-const iterator + @note It is not checked whether @a other is initialized. + */ + iter_impl& operator=(const iter_impl& other) noexcept + { + if (&other != this) + { + m_object = other.m_object; + m_it = other.m_it; + } + return *this; + } + + /*! + @brief converting constructor + @param[in] other non-const iterator to copy from + @note It is not checked whether @a other is initialized. + */ + iter_impl(const iter_impl::type>& other) noexcept + : m_object(other.m_object), m_it(other.m_it) + {} + + /*! + @brief converting assignment + @param[in] other non-const iterator to copy from + @return const/non-const iterator + @note It is not checked whether @a other is initialized. + */ + iter_impl& operator=(const iter_impl::type>& other) noexcept // NOLINT(cert-oop54-cpp) + { + m_object = other.m_object; + m_it = other.m_it; + return *this; + } + + JSON_PRIVATE_UNLESS_TESTED: + /*! + @brief set the iterator to the first value + @pre The iterator is initialized; i.e. `m_object != nullptr`. + */ + void set_begin() noexcept + { + JSON_ASSERT(m_object != nullptr); + + switch (m_object->m_data.m_type) + { + case value_t::object: + { + m_it.object_iterator = m_object->m_data.m_value.object->begin(); + break; + } + + case value_t::array: + { + m_it.array_iterator = m_object->m_data.m_value.array->begin(); + break; + } + + case value_t::null: + { + // set to end so begin()==end() is true: null is empty + m_it.primitive_iterator.set_end(); + break; + } + + case value_t::string: + case value_t::boolean: + case value_t::number_integer: + case value_t::number_unsigned: + case value_t::number_float: + case value_t::binary: + case value_t::discarded: + default: + { + m_it.primitive_iterator.set_begin(); + break; + } + } + } + + /*! + @brief set the iterator past the last value + @pre The iterator is initialized; i.e. `m_object != nullptr`. + */ + void set_end() noexcept + { + JSON_ASSERT(m_object != nullptr); + + switch (m_object->m_data.m_type) + { + case value_t::object: + { + m_it.object_iterator = m_object->m_data.m_value.object->end(); + break; + } + + case value_t::array: + { + m_it.array_iterator = m_object->m_data.m_value.array->end(); + break; + } + + case value_t::null: + case value_t::string: + case value_t::boolean: + case value_t::number_integer: + case value_t::number_unsigned: + case value_t::number_float: + case value_t::binary: + case value_t::discarded: + default: + { + m_it.primitive_iterator.set_end(); + break; + } + } + } + + public: + /*! + @brief return a reference to the value pointed to by the iterator + @pre The iterator is initialized; i.e. `m_object != nullptr`. + */ + reference operator*() const + { + JSON_ASSERT(m_object != nullptr); + + switch (m_object->m_data.m_type) + { + case value_t::object: + { + JSON_ASSERT(m_it.object_iterator != m_object->m_data.m_value.object->end()); + return m_it.object_iterator->second; + } + + case value_t::array: + { + JSON_ASSERT(m_it.array_iterator != m_object->m_data.m_value.array->end()); + return *m_it.array_iterator; + } + + case value_t::null: + JSON_THROW(invalid_iterator::create(214, "cannot get value", m_object)); + + case value_t::string: + case value_t::boolean: + case value_t::number_integer: + case value_t::number_unsigned: + case value_t::number_float: + case value_t::binary: + case value_t::discarded: + default: + { + if (JSON_HEDLEY_LIKELY(m_it.primitive_iterator.is_begin())) + { + return *m_object; + } + + JSON_THROW(invalid_iterator::create(214, "cannot get value", m_object)); + } + } + } + + /*! + @brief dereference the iterator + @pre The iterator is initialized; i.e. `m_object != nullptr`. + */ + pointer operator->() const + { + JSON_ASSERT(m_object != nullptr); + + switch (m_object->m_data.m_type) + { + case value_t::object: + { + JSON_ASSERT(m_it.object_iterator != m_object->m_data.m_value.object->end()); + return &(m_it.object_iterator->second); + } + + case value_t::array: + { + JSON_ASSERT(m_it.array_iterator != m_object->m_data.m_value.array->end()); + return &*m_it.array_iterator; + } + + case value_t::null: + case value_t::string: + case value_t::boolean: + case value_t::number_integer: + case value_t::number_unsigned: + case value_t::number_float: + case value_t::binary: + case value_t::discarded: + default: + { + if (JSON_HEDLEY_LIKELY(m_it.primitive_iterator.is_begin())) + { + return m_object; + } + + JSON_THROW(invalid_iterator::create(214, "cannot get value", m_object)); + } + } + } + + /*! + @brief post-increment (it++) + @pre The iterator is initialized; i.e. `m_object != nullptr`. + */ + iter_impl operator++(int)& // NOLINT(cert-dcl21-cpp) + { + auto result = *this; + ++(*this); + return result; + } + + /*! + @brief pre-increment (++it) + @pre The iterator is initialized; i.e. `m_object != nullptr`. + */ + iter_impl& operator++() + { + JSON_ASSERT(m_object != nullptr); + + switch (m_object->m_data.m_type) + { + case value_t::object: + { + std::advance(m_it.object_iterator, 1); + break; + } + + case value_t::array: + { + std::advance(m_it.array_iterator, 1); + break; + } + + case value_t::null: + case value_t::string: + case value_t::boolean: + case value_t::number_integer: + case value_t::number_unsigned: + case value_t::number_float: + case value_t::binary: + case value_t::discarded: + default: + { + ++m_it.primitive_iterator; + break; + } + } + + return *this; + } + + /*! + @brief post-decrement (it--) + @pre The iterator is initialized; i.e. `m_object != nullptr`. + */ + iter_impl operator--(int)& // NOLINT(cert-dcl21-cpp) + { + auto result = *this; + --(*this); + return result; + } + + /*! + @brief pre-decrement (--it) + @pre The iterator is initialized; i.e. `m_object != nullptr`. + */ + iter_impl& operator--() + { + JSON_ASSERT(m_object != nullptr); + + switch (m_object->m_data.m_type) + { + case value_t::object: + { + std::advance(m_it.object_iterator, -1); + break; + } + + case value_t::array: + { + std::advance(m_it.array_iterator, -1); + break; + } + + case value_t::null: + case value_t::string: + case value_t::boolean: + case value_t::number_integer: + case value_t::number_unsigned: + case value_t::number_float: + case value_t::binary: + case value_t::discarded: + default: + { + --m_it.primitive_iterator; + break; + } + } + + return *this; + } + + /*! + @brief comparison: equal + @pre (1) Both iterators are initialized to point to the same object, or (2) both iterators are value-initialized. + */ + template < typename IterImpl, detail::enable_if_t < (std::is_same::value || std::is_same::value), std::nullptr_t > = nullptr > + bool operator==(const IterImpl& other) const + { + // if objects are not the same, the comparison is undefined + if (JSON_HEDLEY_UNLIKELY(m_object != other.m_object)) + { + JSON_THROW(invalid_iterator::create(212, "cannot compare iterators of different containers", m_object)); + } + + // value-initialized forward iterators can be compared, and must compare equal to other value-initialized iterators of the same type #4493 + if (m_object == nullptr) + { + return true; + } + + switch (m_object->m_data.m_type) + { + case value_t::object: + return (m_it.object_iterator == other.m_it.object_iterator); + + case value_t::array: + return (m_it.array_iterator == other.m_it.array_iterator); + + case value_t::null: + case value_t::string: + case value_t::boolean: + case value_t::number_integer: + case value_t::number_unsigned: + case value_t::number_float: + case value_t::binary: + case value_t::discarded: + default: + return (m_it.primitive_iterator == other.m_it.primitive_iterator); + } + } + + /*! + @brief comparison: not equal + @pre (1) Both iterators are initialized to point to the same object, or (2) both iterators are value-initialized. + */ + template < typename IterImpl, detail::enable_if_t < (std::is_same::value || std::is_same::value), std::nullptr_t > = nullptr > + bool operator!=(const IterImpl& other) const + { + return !operator==(other); + } + + /*! + @brief comparison: smaller + @pre (1) Both iterators are initialized to point to the same object, or (2) both iterators are value-initialized. + */ + bool operator<(const iter_impl& other) const + { + // if objects are not the same, the comparison is undefined + if (JSON_HEDLEY_UNLIKELY(m_object != other.m_object)) + { + JSON_THROW(invalid_iterator::create(212, "cannot compare iterators of different containers", m_object)); + } + + // value-initialized forward iterators can be compared, and must compare equal to other value-initialized iterators of the same type #4493 + if (m_object == nullptr) + { + // the iterators are both value-initialized and are to be considered equal, but this function checks for smaller, so we return false + return false; + } + + switch (m_object->m_data.m_type) + { + case value_t::object: + JSON_THROW(invalid_iterator::create(213, "cannot compare order of object iterators", m_object)); + + case value_t::array: + return (m_it.array_iterator < other.m_it.array_iterator); + + case value_t::null: + case value_t::string: + case value_t::boolean: + case value_t::number_integer: + case value_t::number_unsigned: + case value_t::number_float: + case value_t::binary: + case value_t::discarded: + default: + return (m_it.primitive_iterator < other.m_it.primitive_iterator); + } + } + + /*! + @brief comparison: less than or equal + @pre (1) Both iterators are initialized to point to the same object, or (2) both iterators are value-initialized. + */ + bool operator<=(const iter_impl& other) const + { + return !other.operator < (*this); + } + + /*! + @brief comparison: greater than + @pre (1) Both iterators are initialized to point to the same object, or (2) both iterators are value-initialized. + */ + bool operator>(const iter_impl& other) const + { + return !operator<=(other); + } + + /*! + @brief comparison: greater than or equal + @pre (1) The iterator is initialized; i.e. `m_object != nullptr`, or (2) both iterators are value-initialized. + */ + bool operator>=(const iter_impl& other) const + { + return !operator<(other); + } + + /*! + @brief add to iterator + @pre The iterator is initialized; i.e. `m_object != nullptr`. + */ + iter_impl& operator+=(difference_type i) + { + JSON_ASSERT(m_object != nullptr); + + switch (m_object->m_data.m_type) + { + case value_t::object: + JSON_THROW(invalid_iterator::create(209, "cannot use offsets with object iterators", m_object)); + + case value_t::array: + { + std::advance(m_it.array_iterator, i); + break; + } + + case value_t::null: + case value_t::string: + case value_t::boolean: + case value_t::number_integer: + case value_t::number_unsigned: + case value_t::number_float: + case value_t::binary: + case value_t::discarded: + default: + { + m_it.primitive_iterator += i; + break; + } + } + + return *this; + } + + /*! + @brief subtract from iterator + @pre The iterator is initialized; i.e. `m_object != nullptr`. + */ + iter_impl& operator-=(difference_type i) + { + return operator+=(-i); + } + + /*! + @brief add to iterator + @pre The iterator is initialized; i.e. `m_object != nullptr`. + */ + iter_impl operator+(difference_type i) const + { + auto result = *this; + result += i; + return result; + } + + /*! + @brief addition of distance and iterator + @pre The iterator is initialized; i.e. `m_object != nullptr`. + */ + friend iter_impl operator+(difference_type i, const iter_impl& it) + { + auto result = it; + result += i; + return result; + } + + /*! + @brief subtract from iterator + @pre The iterator is initialized; i.e. `m_object != nullptr`. + */ + iter_impl operator-(difference_type i) const + { + auto result = *this; + result -= i; + return result; + } + + /*! + @brief return difference + @pre The iterator is initialized; i.e. `m_object != nullptr`. + */ + difference_type operator-(const iter_impl& other) const + { + JSON_ASSERT(m_object != nullptr); + + switch (m_object->m_data.m_type) + { + case value_t::object: + JSON_THROW(invalid_iterator::create(209, "cannot use offsets with object iterators", m_object)); + + case value_t::array: + return m_it.array_iterator - other.m_it.array_iterator; + + case value_t::null: + case value_t::string: + case value_t::boolean: + case value_t::number_integer: + case value_t::number_unsigned: + case value_t::number_float: + case value_t::binary: + case value_t::discarded: + default: + return m_it.primitive_iterator - other.m_it.primitive_iterator; + } + } + + /*! + @brief access to successor + @pre The iterator is initialized; i.e. `m_object != nullptr`. + */ + reference operator[](difference_type n) const + { + JSON_ASSERT(m_object != nullptr); + + switch (m_object->m_data.m_type) + { + case value_t::object: + JSON_THROW(invalid_iterator::create(208, "cannot use operator[] for object iterators", m_object)); + + case value_t::array: + return *std::next(m_it.array_iterator, n); + + case value_t::null: + JSON_THROW(invalid_iterator::create(214, "cannot get value", m_object)); + + case value_t::string: + case value_t::boolean: + case value_t::number_integer: + case value_t::number_unsigned: + case value_t::number_float: + case value_t::binary: + case value_t::discarded: + default: + { + if (JSON_HEDLEY_LIKELY(m_it.primitive_iterator.get_value() == -n)) + { + return *m_object; + } + + JSON_THROW(invalid_iterator::create(214, "cannot get value", m_object)); + } + } + } + + /*! + @brief return the key of an object iterator + @pre The iterator is initialized; i.e. `m_object != nullptr`. + */ + const typename object_t::key_type& key() const + { + JSON_ASSERT(m_object != nullptr); + + if (JSON_HEDLEY_LIKELY(m_object->is_object())) + { + return m_it.object_iterator->first; + } + + JSON_THROW(invalid_iterator::create(207, "cannot use key() for non-object iterators", m_object)); + } + + /*! + @brief return the value of an iterator + @pre The iterator is initialized; i.e. `m_object != nullptr`. + */ + reference value() const + { + return operator*(); + } + + JSON_PRIVATE_UNLESS_TESTED: + /// associated JSON instance + pointer m_object = nullptr; + /// the actual iterator of the associated instance + internal_iterator::type> m_it {}; +}; + +} // namespace detail +NLOHMANN_JSON_NAMESPACE_END diff --git a/src/mc/include/nlohmann/detail/iterators/iteration_proxy.hpp b/src/mc/include/nlohmann/detail/iterators/iteration_proxy.hpp new file mode 100755 index 0000000..78e0d8b --- /dev/null +++ b/src/mc/include/nlohmann/detail/iterators/iteration_proxy.hpp @@ -0,0 +1,235 @@ +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.12.0 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2013 - 2025 Niels Lohmann +// SPDX-License-Identifier: MIT + +#pragma once + +#include // size_t +#include // forward_iterator_tag +#include // tuple_size, get, tuple_element +#include // move + +#if JSON_HAS_RANGES + #include // enable_borrowed_range +#endif + +#include +#include +#include +#include + +NLOHMANN_JSON_NAMESPACE_BEGIN +namespace detail +{ + +template class iteration_proxy_value +{ + public: + using difference_type = std::ptrdiff_t; + using value_type = iteration_proxy_value; + using pointer = value_type *; + using reference = value_type &; + using iterator_category = std::forward_iterator_tag; + using string_type = typename std::remove_cv< typename std::remove_reference().key() ) >::type >::type; + + private: + /// the iterator + IteratorType anchor{}; + /// an index for arrays (used to create key names) + std::size_t array_index = 0; + /// last stringified array index + mutable std::size_t array_index_last = 0; + /// a string representation of the array index + mutable string_type array_index_str = "0"; + /// an empty string (to return a reference for primitive values) + string_type empty_str{}; + + public: + explicit iteration_proxy_value() = default; + explicit iteration_proxy_value(IteratorType it, std::size_t array_index_ = 0) + noexcept(std::is_nothrow_move_constructible::value + && std::is_nothrow_default_constructible::value) + : anchor(std::move(it)) + , array_index(array_index_) + {} + + iteration_proxy_value(iteration_proxy_value const&) = default; + iteration_proxy_value& operator=(iteration_proxy_value const&) = default; + // older GCCs are a bit fussy and require explicit noexcept specifiers on defaulted functions + iteration_proxy_value(iteration_proxy_value&&) + noexcept(std::is_nothrow_move_constructible::value + && std::is_nothrow_move_constructible::value) = default; // NOLINT(hicpp-noexcept-move,performance-noexcept-move-constructor,cppcoreguidelines-noexcept-move-operations) + iteration_proxy_value& operator=(iteration_proxy_value&&) + noexcept(std::is_nothrow_move_assignable::value + && std::is_nothrow_move_assignable::value) = default; // NOLINT(hicpp-noexcept-move,performance-noexcept-move-constructor,cppcoreguidelines-noexcept-move-operations) + ~iteration_proxy_value() = default; + + /// dereference operator (needed for range-based for) + const iteration_proxy_value& operator*() const + { + return *this; + } + + /// increment operator (needed for range-based for) + iteration_proxy_value& operator++() + { + ++anchor; + ++array_index; + + return *this; + } + + iteration_proxy_value operator++(int)& // NOLINT(cert-dcl21-cpp) + { + auto tmp = iteration_proxy_value(anchor, array_index); + ++anchor; + ++array_index; + return tmp; + } + + /// equality operator (needed for InputIterator) + bool operator==(const iteration_proxy_value& o) const + { + return anchor == o.anchor; + } + + /// inequality operator (needed for range-based for) + bool operator!=(const iteration_proxy_value& o) const + { + return anchor != o.anchor; + } + + /// return key of the iterator + const string_type& key() const + { + JSON_ASSERT(anchor.m_object != nullptr); + + switch (anchor.m_object->type()) + { + // use integer array index as key + case value_t::array: + { + if (array_index != array_index_last) + { + int_to_string( array_index_str, array_index ); + array_index_last = array_index; + } + return array_index_str; + } + + // use key from the object + case value_t::object: + return anchor.key(); + + // use an empty key for all primitive types + case value_t::null: + case value_t::string: + case value_t::boolean: + case value_t::number_integer: + case value_t::number_unsigned: + case value_t::number_float: + case value_t::binary: + case value_t::discarded: + default: + return empty_str; + } + } + + /// return value of the iterator + typename IteratorType::reference value() const + { + return anchor.value(); + } +}; + +/// proxy class for the items() function +template class iteration_proxy +{ + private: + /// the container to iterate + typename IteratorType::pointer container = nullptr; + + public: + explicit iteration_proxy() = default; + + /// construct iteration proxy from a container + explicit iteration_proxy(typename IteratorType::reference cont) noexcept + : container(&cont) {} + + iteration_proxy(iteration_proxy const&) = default; + iteration_proxy& operator=(iteration_proxy const&) = default; + iteration_proxy(iteration_proxy&&) noexcept = default; + iteration_proxy& operator=(iteration_proxy&&) noexcept = default; + ~iteration_proxy() = default; + + /// return iterator begin (needed for range-based for) + iteration_proxy_value begin() const noexcept + { + return iteration_proxy_value(container->begin()); + } + + /// return iterator end (needed for range-based for) + iteration_proxy_value end() const noexcept + { + return iteration_proxy_value(container->end()); + } +}; + +// Structured Bindings Support +// For further reference see https://blog.tartanllama.xyz/structured-bindings/ +// And see https://github.com/nlohmann/json/pull/1391 +template = 0> +auto get(const nlohmann::detail::iteration_proxy_value& i) -> decltype(i.key()) +{ + return i.key(); +} +// Structured Bindings Support +// For further reference see https://blog.tartanllama.xyz/structured-bindings/ +// And see https://github.com/nlohmann/json/pull/1391 +template = 0> +auto get(const nlohmann::detail::iteration_proxy_value& i) -> decltype(i.value()) +{ + return i.value(); +} + +} // namespace detail +NLOHMANN_JSON_NAMESPACE_END + +// The Addition to the STD Namespace is required to add +// Structured Bindings Support to the iteration_proxy_value class +// For further reference see https://blog.tartanllama.xyz/structured-bindings/ +// And see https://github.com/nlohmann/json/pull/1391 +namespace std +{ + +#if defined(__clang__) + // Fix: https://github.com/nlohmann/json/issues/1401 + #pragma clang diagnostic push + #pragma clang diagnostic ignored "-Wmismatched-tags" +#endif +template +class tuple_size<::nlohmann::detail::iteration_proxy_value> // NOLINT(cert-dcl58-cpp) + : public std::integral_constant {}; + +template +class tuple_element> // NOLINT(cert-dcl58-cpp) +{ + public: + using type = decltype( + get(std::declval < + ::nlohmann::detail::iteration_proxy_value> ())); +}; +#if defined(__clang__) + #pragma clang diagnostic pop +#endif + +} // namespace std + +#if JSON_HAS_RANGES + template + inline constexpr bool ::std::ranges::enable_borrowed_range<::nlohmann::detail::iteration_proxy> = true; +#endif diff --git a/src/mc/include/nlohmann/detail/iterators/iterator_traits.hpp b/src/mc/include/nlohmann/detail/iterators/iterator_traits.hpp new file mode 100755 index 0000000..5ca92a5 --- /dev/null +++ b/src/mc/include/nlohmann/detail/iterators/iterator_traits.hpp @@ -0,0 +1,61 @@ +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.12.0 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2013 - 2025 Niels Lohmann +// SPDX-License-Identifier: MIT + +#pragma once + +#include // random_access_iterator_tag + +#include +#include +#include + +NLOHMANN_JSON_NAMESPACE_BEGIN +namespace detail +{ + +template +struct iterator_types {}; + +template +struct iterator_types < + It, + void_t> +{ + using difference_type = typename It::difference_type; + using value_type = typename It::value_type; + using pointer = typename It::pointer; + using reference = typename It::reference; + using iterator_category = typename It::iterator_category; +}; + +// This is required as some compilers implement std::iterator_traits in a way that +// doesn't work with SFINAE. See https://github.com/nlohmann/json/issues/1341. +template +struct iterator_traits +{ +}; + +template +struct iterator_traits < T, enable_if_t < !std::is_pointer::value >> + : iterator_types +{ +}; + +template +struct iterator_traits::value>> +{ + using iterator_category = std::random_access_iterator_tag; + using value_type = T; + using difference_type = ptrdiff_t; + using pointer = T*; + using reference = T&; +}; + +} // namespace detail +NLOHMANN_JSON_NAMESPACE_END diff --git a/src/mc/include/nlohmann/detail/iterators/json_reverse_iterator.hpp b/src/mc/include/nlohmann/detail/iterators/json_reverse_iterator.hpp new file mode 100755 index 0000000..f979d85 --- /dev/null +++ b/src/mc/include/nlohmann/detail/iterators/json_reverse_iterator.hpp @@ -0,0 +1,130 @@ +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.12.0 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2013 - 2025 Niels Lohmann +// SPDX-License-Identifier: MIT + +#pragma once + +#include // ptrdiff_t +#include // reverse_iterator +#include // declval + +#include + +NLOHMANN_JSON_NAMESPACE_BEGIN +namespace detail +{ + +////////////////////// +// reverse_iterator // +////////////////////// + +/*! +@brief a template for a reverse iterator class + +@tparam Base the base iterator type to reverse. Valid types are @ref +iterator (to create @ref reverse_iterator) and @ref const_iterator (to +create @ref const_reverse_iterator). + +@requirement The class satisfies the following concept requirements: +- +[BidirectionalIterator](https://en.cppreference.com/w/cpp/named_req/BidirectionalIterator): + The iterator that can be moved can be moved in both directions (i.e. + incremented and decremented). +- [OutputIterator](https://en.cppreference.com/w/cpp/named_req/OutputIterator): + It is possible to write to the pointed-to element (only if @a Base is + @ref iterator). + +@since version 1.0.0 +*/ +template +class json_reverse_iterator : public std::reverse_iterator +{ + public: + using difference_type = std::ptrdiff_t; + /// shortcut to the reverse iterator adapter + using base_iterator = std::reverse_iterator; + /// the reference type for the pointed-to element + using reference = typename Base::reference; + + /// create reverse iterator from iterator + explicit json_reverse_iterator(const typename base_iterator::iterator_type& it) noexcept + : base_iterator(it) {} + + /// create reverse iterator from base class + explicit json_reverse_iterator(const base_iterator& it) noexcept : base_iterator(it) {} + + /// post-increment (it++) + json_reverse_iterator operator++(int)& // NOLINT(cert-dcl21-cpp) + { + return static_cast(base_iterator::operator++(1)); + } + + /// pre-increment (++it) + json_reverse_iterator& operator++() + { + return static_cast(base_iterator::operator++()); + } + + /// post-decrement (it--) + json_reverse_iterator operator--(int)& // NOLINT(cert-dcl21-cpp) + { + return static_cast(base_iterator::operator--(1)); + } + + /// pre-decrement (--it) + json_reverse_iterator& operator--() + { + return static_cast(base_iterator::operator--()); + } + + /// add to iterator + json_reverse_iterator& operator+=(difference_type i) + { + return static_cast(base_iterator::operator+=(i)); + } + + /// add to iterator + json_reverse_iterator operator+(difference_type i) const + { + return static_cast(base_iterator::operator+(i)); + } + + /// subtract from iterator + json_reverse_iterator operator-(difference_type i) const + { + return static_cast(base_iterator::operator-(i)); + } + + /// return difference + difference_type operator-(const json_reverse_iterator& other) const + { + return base_iterator(*this) - base_iterator(other); + } + + /// access to successor + reference operator[](difference_type n) const + { + return *(this->operator+(n)); + } + + /// return the key of an object iterator + auto key() const -> decltype(std::declval().key()) + { + auto it = --this->base(); + return it.key(); + } + + /// return the value of an iterator + reference value() const + { + auto it = --this->base(); + return it.operator * (); + } +}; + +} // namespace detail +NLOHMANN_JSON_NAMESPACE_END diff --git a/src/mc/include/nlohmann/detail/iterators/primitive_iterator.hpp b/src/mc/include/nlohmann/detail/iterators/primitive_iterator.hpp new file mode 100755 index 0000000..f1a7356 --- /dev/null +++ b/src/mc/include/nlohmann/detail/iterators/primitive_iterator.hpp @@ -0,0 +1,132 @@ +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.12.0 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2013 - 2025 Niels Lohmann +// SPDX-License-Identifier: MIT + +#pragma once + +#include // ptrdiff_t +#include // numeric_limits + +#include + +NLOHMANN_JSON_NAMESPACE_BEGIN +namespace detail +{ + +/* +@brief an iterator for primitive JSON types + +This class models an iterator for primitive JSON types (boolean, number, +string). Its only purpose is to allow the iterator/const_iterator classes +to "iterate" over primitive values. Internally, the iterator is modeled by +a `difference_type` variable. Value begin_value (`0`) models the begin and +end_value (`1`) models past the end. +*/ +class primitive_iterator_t +{ + private: + using difference_type = std::ptrdiff_t; + static constexpr difference_type begin_value = 0; + static constexpr difference_type end_value = begin_value + 1; + + JSON_PRIVATE_UNLESS_TESTED: + /// iterator as signed integer type + difference_type m_it = (std::numeric_limits::min)(); + + public: + constexpr difference_type get_value() const noexcept + { + return m_it; + } + + /// set iterator to a defined beginning + void set_begin() noexcept + { + m_it = begin_value; + } + + /// set iterator to a defined past the end + void set_end() noexcept + { + m_it = end_value; + } + + /// return whether the iterator can be dereferenced + constexpr bool is_begin() const noexcept + { + return m_it == begin_value; + } + + /// return whether the iterator is at end + constexpr bool is_end() const noexcept + { + return m_it == end_value; + } + + friend constexpr bool operator==(primitive_iterator_t lhs, primitive_iterator_t rhs) noexcept + { + return lhs.m_it == rhs.m_it; + } + + friend constexpr bool operator<(primitive_iterator_t lhs, primitive_iterator_t rhs) noexcept + { + return lhs.m_it < rhs.m_it; + } + + primitive_iterator_t operator+(difference_type n) noexcept + { + auto result = *this; + result += n; + return result; + } + + friend constexpr difference_type operator-(primitive_iterator_t lhs, primitive_iterator_t rhs) noexcept + { + return lhs.m_it - rhs.m_it; + } + + primitive_iterator_t& operator++() noexcept + { + ++m_it; + return *this; + } + + primitive_iterator_t operator++(int)& noexcept // NOLINT(cert-dcl21-cpp) + { + auto result = *this; + ++m_it; + return result; + } + + primitive_iterator_t& operator--() noexcept + { + --m_it; + return *this; + } + + primitive_iterator_t operator--(int)& noexcept // NOLINT(cert-dcl21-cpp) + { + auto result = *this; + --m_it; + return result; + } + + primitive_iterator_t& operator+=(difference_type n) noexcept + { + m_it += n; + return *this; + } + + primitive_iterator_t& operator-=(difference_type n) noexcept + { + m_it -= n; + return *this; + } +}; + +} // namespace detail +NLOHMANN_JSON_NAMESPACE_END diff --git a/src/mc/include/nlohmann/detail/json_custom_base_class.hpp b/src/mc/include/nlohmann/detail/json_custom_base_class.hpp new file mode 100755 index 0000000..91e0c80 --- /dev/null +++ b/src/mc/include/nlohmann/detail/json_custom_base_class.hpp @@ -0,0 +1,39 @@ +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.12.0 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2013 - 2025 Niels Lohmann +// SPDX-License-Identifier: MIT + +#pragma once + +#include // conditional, is_same + +#include + +NLOHMANN_JSON_NAMESPACE_BEGIN +namespace detail +{ + +/*! +@brief Default base class of the @ref basic_json class. + +So that the correct implementations of the copy / move ctors / assign operators +of @ref basic_json do not require complex case distinctions +(no base class / custom base class used as customization point), +@ref basic_json always has a base class. +By default, this class is used because it is empty and thus has no effect +on the behavior of @ref basic_json. +*/ +struct json_default_base {}; + +template +using json_base_class = typename std::conditional < + std::is_same::value, + json_default_base, + T + >::type; + +} // namespace detail +NLOHMANN_JSON_NAMESPACE_END diff --git a/src/mc/include/nlohmann/detail/json_pointer.hpp b/src/mc/include/nlohmann/detail/json_pointer.hpp new file mode 100755 index 0000000..e1b3947 --- /dev/null +++ b/src/mc/include/nlohmann/detail/json_pointer.hpp @@ -0,0 +1,988 @@ +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.12.0 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2013 - 2025 Niels Lohmann +// SPDX-License-Identifier: MIT + +#pragma once + +#include // all_of +#include // isdigit +#include // errno, ERANGE +#include // strtoull +#ifndef JSON_NO_IO + #include // ostream +#endif // JSON_NO_IO +#include // max +#include // accumulate +#include // string +#include // move +#include // vector + +#include +#include +#include +#include +#include + +NLOHMANN_JSON_NAMESPACE_BEGIN + +/// @brief JSON Pointer defines a string syntax for identifying a specific value within a JSON document +/// @sa https://json.nlohmann.me/api/json_pointer/ +template +class json_pointer +{ + // allow basic_json to access private members + NLOHMANN_BASIC_JSON_TPL_DECLARATION + friend class basic_json; + + template + friend class json_pointer; + + template + struct string_t_helper + { + using type = T; + }; + + NLOHMANN_BASIC_JSON_TPL_DECLARATION + struct string_t_helper + { + using type = StringType; + }; + + public: + // for backwards compatibility accept BasicJsonType + using string_t = typename string_t_helper::type; + + /// @brief create JSON pointer + /// @sa https://json.nlohmann.me/api/json_pointer/json_pointer/ + explicit json_pointer(const string_t& s = "") + : reference_tokens(split(s)) + {} + + /// @brief return a string representation of the JSON pointer + /// @sa https://json.nlohmann.me/api/json_pointer/to_string/ + string_t to_string() const + { + return std::accumulate(reference_tokens.begin(), reference_tokens.end(), + string_t{}, + [](const string_t& a, const string_t& b) + { + return detail::concat(a, '/', detail::escape(b)); + }); + } + + /// @brief return a string representation of the JSON pointer + /// @sa https://json.nlohmann.me/api/json_pointer/operator_string/ + JSON_HEDLEY_DEPRECATED_FOR(3.11.0, to_string()) + operator string_t() const + { + return to_string(); + } + +#ifndef JSON_NO_IO + /// @brief write string representation of the JSON pointer to stream + /// @sa https://json.nlohmann.me/api/basic_json/operator_ltlt/ + friend std::ostream& operator<<(std::ostream& o, const json_pointer& ptr) + { + o << ptr.to_string(); + return o; + } +#endif + + /// @brief append another JSON pointer at the end of this JSON pointer + /// @sa https://json.nlohmann.me/api/json_pointer/operator_slasheq/ + json_pointer& operator/=(const json_pointer& ptr) + { + reference_tokens.insert(reference_tokens.end(), + ptr.reference_tokens.begin(), + ptr.reference_tokens.end()); + return *this; + } + + /// @brief append an unescaped reference token at the end of this JSON pointer + /// @sa https://json.nlohmann.me/api/json_pointer/operator_slasheq/ + json_pointer& operator/=(string_t token) + { + push_back(std::move(token)); + return *this; + } + + /// @brief append an array index at the end of this JSON pointer + /// @sa https://json.nlohmann.me/api/json_pointer/operator_slasheq/ + json_pointer& operator/=(std::size_t array_idx) + { + return *this /= std::to_string(array_idx); + } + + /// @brief create a new JSON pointer by appending the right JSON pointer at the end of the left JSON pointer + /// @sa https://json.nlohmann.me/api/json_pointer/operator_slash/ + friend json_pointer operator/(const json_pointer& lhs, + const json_pointer& rhs) + { + return json_pointer(lhs) /= rhs; + } + + /// @brief create a new JSON pointer by appending the unescaped token at the end of the JSON pointer + /// @sa https://json.nlohmann.me/api/json_pointer/operator_slash/ + friend json_pointer operator/(const json_pointer& lhs, string_t token) // NOLINT(performance-unnecessary-value-param) + { + return json_pointer(lhs) /= std::move(token); + } + + /// @brief create a new JSON pointer by appending the array-index-token at the end of the JSON pointer + /// @sa https://json.nlohmann.me/api/json_pointer/operator_slash/ + friend json_pointer operator/(const json_pointer& lhs, std::size_t array_idx) + { + return json_pointer(lhs) /= array_idx; + } + + /// @brief returns the parent of this JSON pointer + /// @sa https://json.nlohmann.me/api/json_pointer/parent_pointer/ + json_pointer parent_pointer() const + { + if (empty()) + { + return *this; + } + + json_pointer res = *this; + res.pop_back(); + return res; + } + + /// @brief remove last reference token + /// @sa https://json.nlohmann.me/api/json_pointer/pop_back/ + void pop_back() + { + if (JSON_HEDLEY_UNLIKELY(empty())) + { + JSON_THROW(detail::out_of_range::create(405, "JSON pointer has no parent", nullptr)); + } + + reference_tokens.pop_back(); + } + + /// @brief return last reference token + /// @sa https://json.nlohmann.me/api/json_pointer/back/ + const string_t& back() const + { + if (JSON_HEDLEY_UNLIKELY(empty())) + { + JSON_THROW(detail::out_of_range::create(405, "JSON pointer has no parent", nullptr)); + } + + return reference_tokens.back(); + } + + /// @brief append an unescaped token at the end of the reference pointer + /// @sa https://json.nlohmann.me/api/json_pointer/push_back/ + void push_back(const string_t& token) + { + reference_tokens.push_back(token); + } + + /// @brief append an unescaped token at the end of the reference pointer + /// @sa https://json.nlohmann.me/api/json_pointer/push_back/ + void push_back(string_t&& token) + { + reference_tokens.push_back(std::move(token)); + } + + /// @brief return whether pointer points to the root document + /// @sa https://json.nlohmann.me/api/json_pointer/empty/ + bool empty() const noexcept + { + return reference_tokens.empty(); + } + + private: + /*! + @param[in] s reference token to be converted into an array index + + @return integer representation of @a s + + @throw parse_error.106 if an array index begins with '0' + @throw parse_error.109 if an array index begins not with a digit + @throw out_of_range.404 if string @a s could not be converted to an integer + @throw out_of_range.410 if an array index exceeds size_type + */ + template + static typename BasicJsonType::size_type array_index(const string_t& s) + { + using size_type = typename BasicJsonType::size_type; + + // error condition (cf. RFC 6901, Sect. 4) + if (JSON_HEDLEY_UNLIKELY(s.size() > 1 && s[0] == '0')) + { + JSON_THROW(detail::parse_error::create(106, 0, detail::concat("array index '", s, "' must not begin with '0'"), nullptr)); + } + + // error condition (cf. RFC 6901, Sect. 4) + if (JSON_HEDLEY_UNLIKELY(s.size() > 1 && !(s[0] >= '1' && s[0] <= '9'))) + { + JSON_THROW(detail::parse_error::create(109, 0, detail::concat("array index '", s, "' is not a number"), nullptr)); + } + + const char* p = s.c_str(); + char* p_end = nullptr; // NOLINT(misc-const-correctness) + errno = 0; // strtoull doesn't reset errno + const unsigned long long res = std::strtoull(p, &p_end, 10); // NOLINT(runtime/int) + if (p == p_end // invalid input or empty string + || errno == ERANGE // out of range + || JSON_HEDLEY_UNLIKELY(static_cast(p_end - p) != s.size())) // incomplete read + { + JSON_THROW(detail::out_of_range::create(404, detail::concat("unresolved reference token '", s, "'"), nullptr)); + } + + // only triggered on special platforms (like 32bit), see also + // https://github.com/nlohmann/json/pull/2203 + if (res >= static_cast((std::numeric_limits::max)())) // NOLINT(runtime/int) + { + JSON_THROW(detail::out_of_range::create(410, detail::concat("array index ", s, " exceeds size_type"), nullptr)); // LCOV_EXCL_LINE + } + + return static_cast(res); + } + + JSON_PRIVATE_UNLESS_TESTED: + json_pointer top() const + { + if (JSON_HEDLEY_UNLIKELY(empty())) + { + JSON_THROW(detail::out_of_range::create(405, "JSON pointer has no parent", nullptr)); + } + + json_pointer result = *this; + result.reference_tokens = {reference_tokens[0]}; + return result; + } + + private: + /*! + @brief create and return a reference to the pointed to value + + @complexity Linear in the number of reference tokens. + + @throw parse_error.109 if array index is not a number + @throw type_error.313 if value cannot be unflattened + */ + template + BasicJsonType& get_and_create(BasicJsonType& j) const + { + auto* result = &j; + + // in case no reference tokens exist, return a reference to the JSON value + // j which will be overwritten by a primitive value + for (const auto& reference_token : reference_tokens) + { + switch (result->type()) + { + case detail::value_t::null: + { + if (reference_token == "0") + { + // start a new array if the reference token is 0 + result = &result->operator[](0); + } + else + { + // start a new object otherwise + result = &result->operator[](reference_token); + } + break; + } + + case detail::value_t::object: + { + // create an entry in the object + result = &result->operator[](reference_token); + break; + } + + case detail::value_t::array: + { + // create an entry in the array + result = &result->operator[](array_index(reference_token)); + break; + } + + /* + The following code is only reached if there exists a reference + token _and_ the current value is primitive. In this case, we have + an error situation, because primitive values may only occur as + a single value; that is, with an empty list of reference tokens. + */ + case detail::value_t::string: + case detail::value_t::boolean: + case detail::value_t::number_integer: + case detail::value_t::number_unsigned: + case detail::value_t::number_float: + case detail::value_t::binary: + case detail::value_t::discarded: + default: + JSON_THROW(detail::type_error::create(313, "invalid value to unflatten", &j)); + } + } + + return *result; + } + + /*! + @brief return a reference to the pointed to value + + @note This version does not throw if a value is not present, but tries to + create nested values instead. For instance, calling this function + with pointer `"/this/that"` on a null value is equivalent to calling + `operator[]("this").operator[]("that")` on that value, effectively + changing the null value to an object. + + @param[in] ptr a JSON value + + @return reference to the JSON value pointed to by the JSON pointer + + @complexity Linear in the length of the JSON pointer. + + @throw parse_error.106 if an array index begins with '0' + @throw parse_error.109 if an array index was not a number + @throw out_of_range.404 if the JSON pointer can not be resolved + */ + template + BasicJsonType& get_unchecked(BasicJsonType* ptr) const + { + for (const auto& reference_token : reference_tokens) + { + // convert null values to arrays or objects before continuing + if (ptr->is_null()) + { + // check if the reference token is a number + const bool nums = + std::all_of(reference_token.begin(), reference_token.end(), + [](const unsigned char x) + { + return std::isdigit(x); + }); + + // change value to an array for numbers or "-" or to object otherwise + *ptr = (nums || reference_token == "-") + ? detail::value_t::array + : detail::value_t::object; + } + + switch (ptr->type()) + { + case detail::value_t::object: + { + // use unchecked object access + ptr = &ptr->operator[](reference_token); + break; + } + + case detail::value_t::array: + { + if (reference_token == "-") + { + // explicitly treat "-" as index beyond the end + ptr = &ptr->operator[](ptr->m_data.m_value.array->size()); + } + else + { + // convert array index to number; unchecked access + ptr = &ptr->operator[](array_index(reference_token)); + } + break; + } + + case detail::value_t::null: + case detail::value_t::string: + case detail::value_t::boolean: + case detail::value_t::number_integer: + case detail::value_t::number_unsigned: + case detail::value_t::number_float: + case detail::value_t::binary: + case detail::value_t::discarded: + default: + JSON_THROW(detail::out_of_range::create(404, detail::concat("unresolved reference token '", reference_token, "'"), ptr)); + } + } + + return *ptr; + } + + /*! + @throw parse_error.106 if an array index begins with '0' + @throw parse_error.109 if an array index was not a number + @throw out_of_range.402 if the array index '-' is used + @throw out_of_range.404 if the JSON pointer can not be resolved + */ + template + BasicJsonType& get_checked(BasicJsonType* ptr) const + { + for (const auto& reference_token : reference_tokens) + { + switch (ptr->type()) + { + case detail::value_t::object: + { + // note: at performs range check + ptr = &ptr->at(reference_token); + break; + } + + case detail::value_t::array: + { + if (JSON_HEDLEY_UNLIKELY(reference_token == "-")) + { + // "-" always fails the range check + JSON_THROW(detail::out_of_range::create(402, detail::concat( + "array index '-' (", std::to_string(ptr->m_data.m_value.array->size()), + ") is out of range"), ptr)); + } + + // note: at performs range check + ptr = &ptr->at(array_index(reference_token)); + break; + } + + case detail::value_t::null: + case detail::value_t::string: + case detail::value_t::boolean: + case detail::value_t::number_integer: + case detail::value_t::number_unsigned: + case detail::value_t::number_float: + case detail::value_t::binary: + case detail::value_t::discarded: + default: + JSON_THROW(detail::out_of_range::create(404, detail::concat("unresolved reference token '", reference_token, "'"), ptr)); + } + } + + return *ptr; + } + + /*! + @brief return a const reference to the pointed to value + + @param[in] ptr a JSON value + + @return const reference to the JSON value pointed to by the JSON + pointer + + @throw parse_error.106 if an array index begins with '0' + @throw parse_error.109 if an array index was not a number + @throw out_of_range.402 if the array index '-' is used + @throw out_of_range.404 if the JSON pointer can not be resolved + */ + template + const BasicJsonType& get_unchecked(const BasicJsonType* ptr) const + { + for (const auto& reference_token : reference_tokens) + { + switch (ptr->type()) + { + case detail::value_t::object: + { + // use unchecked object access + ptr = &ptr->operator[](reference_token); + break; + } + + case detail::value_t::array: + { + if (JSON_HEDLEY_UNLIKELY(reference_token == "-")) + { + // "-" cannot be used for const access + JSON_THROW(detail::out_of_range::create(402, detail::concat("array index '-' (", std::to_string(ptr->m_data.m_value.array->size()), ") is out of range"), ptr)); + } + + // use unchecked array access + ptr = &ptr->operator[](array_index(reference_token)); + break; + } + + case detail::value_t::null: + case detail::value_t::string: + case detail::value_t::boolean: + case detail::value_t::number_integer: + case detail::value_t::number_unsigned: + case detail::value_t::number_float: + case detail::value_t::binary: + case detail::value_t::discarded: + default: + JSON_THROW(detail::out_of_range::create(404, detail::concat("unresolved reference token '", reference_token, "'"), ptr)); + } + } + + return *ptr; + } + + /*! + @throw parse_error.106 if an array index begins with '0' + @throw parse_error.109 if an array index was not a number + @throw out_of_range.402 if the array index '-' is used + @throw out_of_range.404 if the JSON pointer can not be resolved + */ + template + const BasicJsonType& get_checked(const BasicJsonType* ptr) const + { + for (const auto& reference_token : reference_tokens) + { + switch (ptr->type()) + { + case detail::value_t::object: + { + // note: at performs range check + ptr = &ptr->at(reference_token); + break; + } + + case detail::value_t::array: + { + if (JSON_HEDLEY_UNLIKELY(reference_token == "-")) + { + // "-" always fails the range check + JSON_THROW(detail::out_of_range::create(402, detail::concat( + "array index '-' (", std::to_string(ptr->m_data.m_value.array->size()), + ") is out of range"), ptr)); + } + + // note: at performs range check + ptr = &ptr->at(array_index(reference_token)); + break; + } + + case detail::value_t::null: + case detail::value_t::string: + case detail::value_t::boolean: + case detail::value_t::number_integer: + case detail::value_t::number_unsigned: + case detail::value_t::number_float: + case detail::value_t::binary: + case detail::value_t::discarded: + default: + JSON_THROW(detail::out_of_range::create(404, detail::concat("unresolved reference token '", reference_token, "'"), ptr)); + } + } + + return *ptr; + } + + /*! + @throw parse_error.106 if an array index begins with '0' + @throw parse_error.109 if an array index was not a number + */ + template + bool contains(const BasicJsonType* ptr) const + { + for (const auto& reference_token : reference_tokens) + { + switch (ptr->type()) + { + case detail::value_t::object: + { + if (!ptr->contains(reference_token)) + { + // we did not find the key in the object + return false; + } + + ptr = &ptr->operator[](reference_token); + break; + } + + case detail::value_t::array: + { + if (JSON_HEDLEY_UNLIKELY(reference_token == "-")) + { + // "-" always fails the range check + return false; + } + if (JSON_HEDLEY_UNLIKELY(reference_token.size() == 1 && !("0" <= reference_token && reference_token <= "9"))) + { + // invalid char + return false; + } + if (JSON_HEDLEY_UNLIKELY(reference_token.size() > 1)) + { + if (JSON_HEDLEY_UNLIKELY(!('1' <= reference_token[0] && reference_token[0] <= '9'))) + { + // the first char should be between '1' and '9' + return false; + } + for (std::size_t i = 1; i < reference_token.size(); i++) + { + if (JSON_HEDLEY_UNLIKELY(!('0' <= reference_token[i] && reference_token[i] <= '9'))) + { + // other char should be between '0' and '9' + return false; + } + } + } + + const auto idx = array_index(reference_token); + if (idx >= ptr->size()) + { + // index out of range + return false; + } + + ptr = &ptr->operator[](idx); + break; + } + + case detail::value_t::null: + case detail::value_t::string: + case detail::value_t::boolean: + case detail::value_t::number_integer: + case detail::value_t::number_unsigned: + case detail::value_t::number_float: + case detail::value_t::binary: + case detail::value_t::discarded: + default: + { + // we do not expect primitive values if there is still a + // reference token to process + return false; + } + } + } + + // no reference token left means we found a primitive value + return true; + } + + /*! + @brief split the string input to reference tokens + + @note This function is only called by the json_pointer constructor. + All exceptions below are documented there. + + @throw parse_error.107 if the pointer is not empty or begins with '/' + @throw parse_error.108 if character '~' is not followed by '0' or '1' + */ + static std::vector split(const string_t& reference_string) + { + std::vector result; + + // special case: empty reference string -> no reference tokens + if (reference_string.empty()) + { + return result; + } + + // check if a nonempty reference string begins with slash + if (JSON_HEDLEY_UNLIKELY(reference_string[0] != '/')) + { + JSON_THROW(detail::parse_error::create(107, 1, detail::concat("JSON pointer must be empty or begin with '/' - was: '", reference_string, "'"), nullptr)); + } + + // extract the reference tokens: + // - slash: position of the last read slash (or end of string) + // - start: position after the previous slash + for ( + // search for the first slash after the first character + std::size_t slash = reference_string.find_first_of('/', 1), + // set the beginning of the first reference token + start = 1; + // we can stop if start == 0 (if slash == string_t::npos) + start != 0; + // set the beginning of the next reference token + // (will eventually be 0 if slash == string_t::npos) + start = (slash == string_t::npos) ? 0 : slash + 1, + // find next slash + slash = reference_string.find_first_of('/', start)) + { + // use the text between the beginning of the reference token + // (start) and the last slash (slash). + auto reference_token = reference_string.substr(start, slash - start); + + // check reference tokens are properly escaped + for (std::size_t pos = reference_token.find_first_of('~'); + pos != string_t::npos; + pos = reference_token.find_first_of('~', pos + 1)) + { + JSON_ASSERT(reference_token[pos] == '~'); + + // ~ must be followed by 0 or 1 + if (JSON_HEDLEY_UNLIKELY(pos == reference_token.size() - 1 || + (reference_token[pos + 1] != '0' && + reference_token[pos + 1] != '1'))) + { + JSON_THROW(detail::parse_error::create(108, 0, "escape character '~' must be followed with '0' or '1'", nullptr)); + } + } + + // finally, store the reference token + detail::unescape(reference_token); + result.push_back(reference_token); + } + + return result; + } + + private: + /*! + @param[in] reference_string the reference string to the current value + @param[in] value the value to consider + @param[in,out] result the result object to insert values to + + @note Empty objects or arrays are flattened to `null`. + */ + template + static void flatten(const string_t& reference_string, + const BasicJsonType& value, + BasicJsonType& result) + { + switch (value.type()) + { + case detail::value_t::array: + { + if (value.m_data.m_value.array->empty()) + { + // flatten empty array as null + result[reference_string] = nullptr; + } + else + { + // iterate array and use index as a reference string + for (std::size_t i = 0; i < value.m_data.m_value.array->size(); ++i) + { + flatten(detail::concat(reference_string, '/', std::to_string(i)), + value.m_data.m_value.array->operator[](i), result); + } + } + break; + } + + case detail::value_t::object: + { + if (value.m_data.m_value.object->empty()) + { + // flatten empty object as null + result[reference_string] = nullptr; + } + else + { + // iterate object and use keys as reference string + for (const auto& element : *value.m_data.m_value.object) + { + flatten(detail::concat(reference_string, '/', detail::escape(element.first)), element.second, result); + } + } + break; + } + + case detail::value_t::null: + case detail::value_t::string: + case detail::value_t::boolean: + case detail::value_t::number_integer: + case detail::value_t::number_unsigned: + case detail::value_t::number_float: + case detail::value_t::binary: + case detail::value_t::discarded: + default: + { + // add a primitive value with its reference string + result[reference_string] = value; + break; + } + } + } + + /*! + @param[in] value flattened JSON + + @return unflattened JSON + + @throw parse_error.109 if array index is not a number + @throw type_error.314 if value is not an object + @throw type_error.315 if object values are not primitive + @throw type_error.313 if value cannot be unflattened + */ + template + static BasicJsonType + unflatten(const BasicJsonType& value) + { + if (JSON_HEDLEY_UNLIKELY(!value.is_object())) + { + JSON_THROW(detail::type_error::create(314, "only objects can be unflattened", &value)); + } + + BasicJsonType result; + + // iterate the JSON object values + for (const auto& element : *value.m_data.m_value.object) + { + if (JSON_HEDLEY_UNLIKELY(!element.second.is_primitive())) + { + JSON_THROW(detail::type_error::create(315, "values in object must be primitive", &element.second)); + } + + // Assign the value to the reference pointed to by JSON pointer. Note + // that if the JSON pointer is "" (i.e., points to the whole value), + // function get_and_create returns a reference to the result itself. + // An assignment will then create a primitive value. + json_pointer(element.first).get_and_create(result) = element.second; + } + + return result; + } + + // can't use the conversion operator because of ambiguity + json_pointer convert() const& + { + json_pointer result; + result.reference_tokens = reference_tokens; + return result; + } + + json_pointer convert()&& + { + json_pointer result; + result.reference_tokens = std::move(reference_tokens); + return result; + } + + public: +#if JSON_HAS_THREE_WAY_COMPARISON + /// @brief compares two JSON pointers for equality + /// @sa https://json.nlohmann.me/api/json_pointer/operator_eq/ + template + bool operator==(const json_pointer& rhs) const noexcept + { + return reference_tokens == rhs.reference_tokens; + } + + /// @brief compares JSON pointer and string for equality + /// @sa https://json.nlohmann.me/api/json_pointer/operator_eq/ + JSON_HEDLEY_DEPRECATED_FOR(3.11.2, operator==(json_pointer)) + bool operator==(const string_t& rhs) const + { + return *this == json_pointer(rhs); + } + + /// @brief 3-way compares two JSON pointers + template + std::strong_ordering operator<=>(const json_pointer& rhs) const noexcept // *NOPAD* + { + return reference_tokens <=> rhs.reference_tokens; // *NOPAD* + } +#else + /// @brief compares two JSON pointers for equality + /// @sa https://json.nlohmann.me/api/json_pointer/operator_eq/ + template + // NOLINTNEXTLINE(readability-redundant-declaration) + friend bool operator==(const json_pointer& lhs, + const json_pointer& rhs) noexcept; + + /// @brief compares JSON pointer and string for equality + /// @sa https://json.nlohmann.me/api/json_pointer/operator_eq/ + template + // NOLINTNEXTLINE(readability-redundant-declaration) + friend bool operator==(const json_pointer& lhs, + const StringType& rhs); + + /// @brief compares string and JSON pointer for equality + /// @sa https://json.nlohmann.me/api/json_pointer/operator_eq/ + template + // NOLINTNEXTLINE(readability-redundant-declaration) + friend bool operator==(const StringType& lhs, + const json_pointer& rhs); + + /// @brief compares two JSON pointers for inequality + /// @sa https://json.nlohmann.me/api/json_pointer/operator_ne/ + template + // NOLINTNEXTLINE(readability-redundant-declaration) + friend bool operator!=(const json_pointer& lhs, + const json_pointer& rhs) noexcept; + + /// @brief compares JSON pointer and string for inequality + /// @sa https://json.nlohmann.me/api/json_pointer/operator_ne/ + template + // NOLINTNEXTLINE(readability-redundant-declaration) + friend bool operator!=(const json_pointer& lhs, + const StringType& rhs); + + /// @brief compares string and JSON pointer for inequality + /// @sa https://json.nlohmann.me/api/json_pointer/operator_ne/ + template + // NOLINTNEXTLINE(readability-redundant-declaration) + friend bool operator!=(const StringType& lhs, + const json_pointer& rhs); + + /// @brief compares two JSON pointer for less-than + template + // NOLINTNEXTLINE(readability-redundant-declaration) + friend bool operator<(const json_pointer& lhs, + const json_pointer& rhs) noexcept; +#endif + + private: + /// the reference tokens + std::vector reference_tokens; +}; + +#if !JSON_HAS_THREE_WAY_COMPARISON +// functions cannot be defined inside the class due to ODR violations +template +inline bool operator==(const json_pointer& lhs, + const json_pointer& rhs) noexcept +{ + return lhs.reference_tokens == rhs.reference_tokens; +} + +template::string_t> +JSON_HEDLEY_DEPRECATED_FOR(3.11.2, operator==(json_pointer, json_pointer)) +inline bool operator==(const json_pointer& lhs, + const StringType& rhs) +{ + return lhs == json_pointer(rhs); +} + +template::string_t> +JSON_HEDLEY_DEPRECATED_FOR(3.11.2, operator==(json_pointer, json_pointer)) +inline bool operator==(const StringType& lhs, + const json_pointer& rhs) +{ + return json_pointer(lhs) == rhs; +} + +template +inline bool operator!=(const json_pointer& lhs, + const json_pointer& rhs) noexcept +{ + return !(lhs == rhs); +} + +template::string_t> +JSON_HEDLEY_DEPRECATED_FOR(3.11.2, operator!=(json_pointer, json_pointer)) +inline bool operator!=(const json_pointer& lhs, + const StringType& rhs) +{ + return !(lhs == rhs); +} + +template::string_t> +JSON_HEDLEY_DEPRECATED_FOR(3.11.2, operator!=(json_pointer, json_pointer)) +inline bool operator!=(const StringType& lhs, + const json_pointer& rhs) +{ + return !(lhs == rhs); +} + +template +inline bool operator<(const json_pointer& lhs, + const json_pointer& rhs) noexcept +{ + return lhs.reference_tokens < rhs.reference_tokens; +} +#endif + +NLOHMANN_JSON_NAMESPACE_END diff --git a/src/mc/include/nlohmann/detail/json_ref.hpp b/src/mc/include/nlohmann/detail/json_ref.hpp new file mode 100755 index 0000000..a9a68d9 --- /dev/null +++ b/src/mc/include/nlohmann/detail/json_ref.hpp @@ -0,0 +1,78 @@ +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.12.0 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2013 - 2025 Niels Lohmann +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +#include +#include + +NLOHMANN_JSON_NAMESPACE_BEGIN +namespace detail +{ + +template +class json_ref +{ + public: + using value_type = BasicJsonType; + + json_ref(value_type&& value) + : owned_value(std::move(value)) + {} + + json_ref(const value_type& value) + : value_ref(&value) + {} + + json_ref(std::initializer_list init) + : owned_value(init) + {} + + template < + class... Args, + enable_if_t::value, int> = 0 > + json_ref(Args && ... args) + : owned_value(std::forward(args)...) + {} + + // class should be movable only + json_ref(json_ref&&) noexcept = default; + json_ref(const json_ref&) = delete; + json_ref& operator=(const json_ref&) = delete; + json_ref& operator=(json_ref&&) = delete; + ~json_ref() = default; + + value_type moved_or_copied() const + { + if (value_ref == nullptr) + { + return std::move(owned_value); + } + return *value_ref; + } + + value_type const& operator*() const + { + return value_ref ? *value_ref : owned_value; + } + + value_type const* operator->() const + { + return &** this; + } + + private: + mutable value_type owned_value = nullptr; + value_type const* value_ref = nullptr; +}; + +} // namespace detail +NLOHMANN_JSON_NAMESPACE_END diff --git a/src/mc/include/nlohmann/detail/macro_scope.hpp b/src/mc/include/nlohmann/detail/macro_scope.hpp new file mode 100755 index 0000000..cf94997 --- /dev/null +++ b/src/mc/include/nlohmann/detail/macro_scope.hpp @@ -0,0 +1,595 @@ +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.12.0 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2013 - 2025 Niels Lohmann +// SPDX-License-Identifier: MIT + +#pragma once + +#include // declval, pair +#include +#include + +// This file contains all internal macro definitions (except those affecting ABI) +// You MUST include macro_unscope.hpp at the end of json.hpp to undef all of them + +#include + +// exclude unsupported compilers +#if !defined(JSON_SKIP_UNSUPPORTED_COMPILER_CHECK) + #if defined(__clang__) + #if (__clang_major__ * 10000 + __clang_minor__ * 100 + __clang_patchlevel__) < 30400 + #error "unsupported Clang version - see https://github.com/nlohmann/json#supported-compilers" + #endif + #elif defined(__GNUC__) && !(defined(__ICC) || defined(__INTEL_COMPILER)) + #if (__GNUC__ * 10000 + __GNUC_MINOR__ * 100 + __GNUC_PATCHLEVEL__) < 40800 + #error "unsupported GCC version - see https://github.com/nlohmann/json#supported-compilers" + #endif + #endif +#endif + +// C++ language standard detection +// if the user manually specified the used C++ version, this is skipped +#if !defined(JSON_HAS_CPP_23) && !defined(JSON_HAS_CPP_20) && !defined(JSON_HAS_CPP_17) && !defined(JSON_HAS_CPP_14) && !defined(JSON_HAS_CPP_11) + #if (defined(__cplusplus) && __cplusplus > 202002L) || (defined(_MSVC_LANG) && _MSVC_LANG > 202002L) + #define JSON_HAS_CPP_23 + #define JSON_HAS_CPP_20 + #define JSON_HAS_CPP_17 + #define JSON_HAS_CPP_14 + #elif (defined(__cplusplus) && __cplusplus > 201703L) || (defined(_MSVC_LANG) && _MSVC_LANG > 201703L) + #define JSON_HAS_CPP_20 + #define JSON_HAS_CPP_17 + #define JSON_HAS_CPP_14 + #elif (defined(__cplusplus) && __cplusplus > 201402L) || (defined(_HAS_CXX17) && _HAS_CXX17 == 1) // fix for issue #464 + #define JSON_HAS_CPP_17 + #define JSON_HAS_CPP_14 + #elif (defined(__cplusplus) && __cplusplus > 201103L) || (defined(_HAS_CXX14) && _HAS_CXX14 == 1) + #define JSON_HAS_CPP_14 + #endif + // the cpp 11 flag is always specified because it is the minimal required version + #define JSON_HAS_CPP_11 +#endif + +#ifdef __has_include + #if __has_include() + #include + #endif +#endif + +#if !defined(JSON_HAS_FILESYSTEM) && !defined(JSON_HAS_EXPERIMENTAL_FILESYSTEM) + #ifdef JSON_HAS_CPP_17 + #if defined(__cpp_lib_filesystem) + #define JSON_HAS_FILESYSTEM 1 + #elif defined(__cpp_lib_experimental_filesystem) + #define JSON_HAS_EXPERIMENTAL_FILESYSTEM 1 + #elif !defined(__has_include) + #define JSON_HAS_EXPERIMENTAL_FILESYSTEM 1 + #elif __has_include() + #define JSON_HAS_FILESYSTEM 1 + #elif __has_include() + #define JSON_HAS_EXPERIMENTAL_FILESYSTEM 1 + #endif + + // std::filesystem does not work on MinGW GCC 8: https://sourceforge.net/p/mingw-w64/bugs/737/ + #if defined(__MINGW32__) && defined(__GNUC__) && __GNUC__ == 8 + #undef JSON_HAS_FILESYSTEM + #undef JSON_HAS_EXPERIMENTAL_FILESYSTEM + #endif + + // no filesystem support before GCC 8: https://en.cppreference.com/w/cpp/compiler_support + #if defined(__GNUC__) && !defined(__clang__) && __GNUC__ < 8 + #undef JSON_HAS_FILESYSTEM + #undef JSON_HAS_EXPERIMENTAL_FILESYSTEM + #endif + + // no filesystem support before Clang 7: https://en.cppreference.com/w/cpp/compiler_support + #if defined(__clang_major__) && __clang_major__ < 7 + #undef JSON_HAS_FILESYSTEM + #undef JSON_HAS_EXPERIMENTAL_FILESYSTEM + #endif + + // no filesystem support before MSVC 19.14: https://en.cppreference.com/w/cpp/compiler_support + #if defined(_MSC_VER) && _MSC_VER < 1914 + #undef JSON_HAS_FILESYSTEM + #undef JSON_HAS_EXPERIMENTAL_FILESYSTEM + #endif + + // no filesystem support before iOS 13 + #if defined(__IPHONE_OS_VERSION_MIN_REQUIRED) && __IPHONE_OS_VERSION_MIN_REQUIRED < 130000 + #undef JSON_HAS_FILESYSTEM + #undef JSON_HAS_EXPERIMENTAL_FILESYSTEM + #endif + + // no filesystem support before macOS Catalina + #if defined(__MAC_OS_X_VERSION_MIN_REQUIRED) && __MAC_OS_X_VERSION_MIN_REQUIRED < 101500 + #undef JSON_HAS_FILESYSTEM + #undef JSON_HAS_EXPERIMENTAL_FILESYSTEM + #endif + #endif +#endif + +#ifndef JSON_HAS_EXPERIMENTAL_FILESYSTEM + #define JSON_HAS_EXPERIMENTAL_FILESYSTEM 0 +#endif + +#ifndef JSON_HAS_FILESYSTEM + #define JSON_HAS_FILESYSTEM 0 +#endif + +#ifndef JSON_HAS_THREE_WAY_COMPARISON + #if defined(__cpp_impl_three_way_comparison) && __cpp_impl_three_way_comparison >= 201907L \ + && defined(__cpp_lib_three_way_comparison) && __cpp_lib_three_way_comparison >= 201907L + #define JSON_HAS_THREE_WAY_COMPARISON 1 + #else + #define JSON_HAS_THREE_WAY_COMPARISON 0 + #endif +#endif + +#ifndef JSON_HAS_RANGES + // ranges header shipping in GCC 11.1.0 (released 2021-04-27) has a syntax error + #if defined(__GLIBCXX__) && __GLIBCXX__ == 20210427 + #define JSON_HAS_RANGES 0 + #elif defined(__cpp_lib_ranges) + #define JSON_HAS_RANGES 1 + #else + #define JSON_HAS_RANGES 0 + #endif +#endif + +#ifndef JSON_HAS_STATIC_RTTI + #if !defined(_HAS_STATIC_RTTI) || _HAS_STATIC_RTTI != 0 + #define JSON_HAS_STATIC_RTTI 1 + #else + #define JSON_HAS_STATIC_RTTI 0 + #endif +#endif + +#ifdef JSON_HAS_CPP_17 + #define JSON_INLINE_VARIABLE inline +#else + #define JSON_INLINE_VARIABLE +#endif + +#if JSON_HEDLEY_HAS_ATTRIBUTE(no_unique_address) + #define JSON_NO_UNIQUE_ADDRESS [[no_unique_address]] +#else + #define JSON_NO_UNIQUE_ADDRESS +#endif + +// disable documentation warnings on clang +#if defined(__clang__) + #pragma clang diagnostic push + #pragma clang diagnostic ignored "-Wdocumentation" + #pragma clang diagnostic ignored "-Wdocumentation-unknown-command" +#endif + +// allow disabling exceptions +#if (defined(__cpp_exceptions) || defined(__EXCEPTIONS) || defined(_CPPUNWIND)) && !defined(JSON_NOEXCEPTION) + #define JSON_THROW(exception) throw exception + #define JSON_TRY try + #define JSON_CATCH(exception) catch(exception) + #define JSON_INTERNAL_CATCH(exception) catch(exception) +#else + #include + #define JSON_THROW(exception) std::abort() + #define JSON_TRY if(true) + #define JSON_CATCH(exception) if(false) + #define JSON_INTERNAL_CATCH(exception) if(false) +#endif + +// override exception macros +#if defined(JSON_THROW_USER) + #undef JSON_THROW + #define JSON_THROW JSON_THROW_USER +#endif +#if defined(JSON_TRY_USER) + #undef JSON_TRY + #define JSON_TRY JSON_TRY_USER +#endif +#if defined(JSON_CATCH_USER) + #undef JSON_CATCH + #define JSON_CATCH JSON_CATCH_USER + #undef JSON_INTERNAL_CATCH + #define JSON_INTERNAL_CATCH JSON_CATCH_USER +#endif +#if defined(JSON_INTERNAL_CATCH_USER) + #undef JSON_INTERNAL_CATCH + #define JSON_INTERNAL_CATCH JSON_INTERNAL_CATCH_USER +#endif + +// allow overriding assert +#if !defined(JSON_ASSERT) + #include // assert + #define JSON_ASSERT(x) assert(x) +#endif + +// allow accessing some private functions (needed by the test suite) +#if defined(JSON_TESTS_PRIVATE) + #define JSON_PRIVATE_UNLESS_TESTED public +#else + #define JSON_PRIVATE_UNLESS_TESTED private +#endif + +/*! +@brief macro to briefly define a mapping between an enum and JSON +@def NLOHMANN_JSON_SERIALIZE_ENUM +@since version 3.4.0 +*/ +#define NLOHMANN_JSON_SERIALIZE_ENUM(ENUM_TYPE, ...) \ + template \ + inline void to_json(BasicJsonType& j, const ENUM_TYPE& e) \ + { \ + /* NOLINTNEXTLINE(modernize-type-traits) we use C++11 */ \ + static_assert(std::is_enum::value, #ENUM_TYPE " must be an enum!"); \ + /* NOLINTNEXTLINE(modernize-avoid-c-arrays) we don't want to depend on */ \ + static const std::pair m[] = __VA_ARGS__; \ + auto it = std::find_if(std::begin(m), std::end(m), \ + [e](const std::pair& ej_pair) -> bool \ + { \ + return ej_pair.first == e; \ + }); \ + j = ((it != std::end(m)) ? it : std::begin(m))->second; \ + } \ + template \ + inline void from_json(const BasicJsonType& j, ENUM_TYPE& e) \ + { \ + /* NOLINTNEXTLINE(modernize-type-traits) we use C++11 */ \ + static_assert(std::is_enum::value, #ENUM_TYPE " must be an enum!"); \ + /* NOLINTNEXTLINE(modernize-avoid-c-arrays) we don't want to depend on */ \ + static const std::pair m[] = __VA_ARGS__; \ + auto it = std::find_if(std::begin(m), std::end(m), \ + [&j](const std::pair& ej_pair) -> bool \ + { \ + return ej_pair.second == j; \ + }); \ + e = ((it != std::end(m)) ? it : std::begin(m))->first; \ + } + +// Ugly macros to avoid uglier copy-paste when specializing basic_json. They +// may be removed in the future once the class is split. + +#define NLOHMANN_BASIC_JSON_TPL_DECLARATION \ + template class ObjectType, \ + template class ArrayType, \ + class StringType, class BooleanType, class NumberIntegerType, \ + class NumberUnsignedType, class NumberFloatType, \ + template class AllocatorType, \ + template class JSONSerializer, \ + class BinaryType, \ + class CustomBaseClass> + +#define NLOHMANN_BASIC_JSON_TPL \ + basic_json + +// Macros to simplify conversion from/to types + +#define NLOHMANN_JSON_EXPAND( x ) x +#define NLOHMANN_JSON_GET_MACRO(_1, _2, _3, _4, _5, _6, _7, _8, _9, _10, _11, _12, _13, _14, _15, _16, _17, _18, _19, _20, _21, _22, _23, _24, _25, _26, _27, _28, _29, _30, _31, _32, _33, _34, _35, _36, _37, _38, _39, _40, _41, _42, _43, _44, _45, _46, _47, _48, _49, _50, _51, _52, _53, _54, _55, _56, _57, _58, _59, _60, _61, _62, _63, _64, NAME,...) NAME +#define NLOHMANN_JSON_PASTE(...) NLOHMANN_JSON_EXPAND(NLOHMANN_JSON_GET_MACRO(__VA_ARGS__, \ + NLOHMANN_JSON_PASTE64, \ + NLOHMANN_JSON_PASTE63, \ + NLOHMANN_JSON_PASTE62, \ + NLOHMANN_JSON_PASTE61, \ + NLOHMANN_JSON_PASTE60, \ + NLOHMANN_JSON_PASTE59, \ + NLOHMANN_JSON_PASTE58, \ + NLOHMANN_JSON_PASTE57, \ + NLOHMANN_JSON_PASTE56, \ + NLOHMANN_JSON_PASTE55, \ + NLOHMANN_JSON_PASTE54, \ + NLOHMANN_JSON_PASTE53, \ + NLOHMANN_JSON_PASTE52, \ + NLOHMANN_JSON_PASTE51, \ + NLOHMANN_JSON_PASTE50, \ + NLOHMANN_JSON_PASTE49, \ + NLOHMANN_JSON_PASTE48, \ + NLOHMANN_JSON_PASTE47, \ + NLOHMANN_JSON_PASTE46, \ + NLOHMANN_JSON_PASTE45, \ + NLOHMANN_JSON_PASTE44, \ + NLOHMANN_JSON_PASTE43, \ + NLOHMANN_JSON_PASTE42, \ + NLOHMANN_JSON_PASTE41, \ + NLOHMANN_JSON_PASTE40, \ + NLOHMANN_JSON_PASTE39, \ + NLOHMANN_JSON_PASTE38, \ + NLOHMANN_JSON_PASTE37, \ + NLOHMANN_JSON_PASTE36, \ + NLOHMANN_JSON_PASTE35, \ + NLOHMANN_JSON_PASTE34, \ + NLOHMANN_JSON_PASTE33, \ + NLOHMANN_JSON_PASTE32, \ + NLOHMANN_JSON_PASTE31, \ + NLOHMANN_JSON_PASTE30, \ + NLOHMANN_JSON_PASTE29, \ + NLOHMANN_JSON_PASTE28, \ + NLOHMANN_JSON_PASTE27, \ + NLOHMANN_JSON_PASTE26, \ + NLOHMANN_JSON_PASTE25, \ + NLOHMANN_JSON_PASTE24, \ + NLOHMANN_JSON_PASTE23, \ + NLOHMANN_JSON_PASTE22, \ + NLOHMANN_JSON_PASTE21, \ + NLOHMANN_JSON_PASTE20, \ + NLOHMANN_JSON_PASTE19, \ + NLOHMANN_JSON_PASTE18, \ + NLOHMANN_JSON_PASTE17, \ + NLOHMANN_JSON_PASTE16, \ + NLOHMANN_JSON_PASTE15, \ + NLOHMANN_JSON_PASTE14, \ + NLOHMANN_JSON_PASTE13, \ + NLOHMANN_JSON_PASTE12, \ + NLOHMANN_JSON_PASTE11, \ + NLOHMANN_JSON_PASTE10, \ + NLOHMANN_JSON_PASTE9, \ + NLOHMANN_JSON_PASTE8, \ + NLOHMANN_JSON_PASTE7, \ + NLOHMANN_JSON_PASTE6, \ + NLOHMANN_JSON_PASTE5, \ + NLOHMANN_JSON_PASTE4, \ + NLOHMANN_JSON_PASTE3, \ + NLOHMANN_JSON_PASTE2, \ + NLOHMANN_JSON_PASTE1)(__VA_ARGS__)) +#define NLOHMANN_JSON_PASTE2(func, v1) func(v1) +#define NLOHMANN_JSON_PASTE3(func, v1, v2) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE2(func, v2) +#define NLOHMANN_JSON_PASTE4(func, v1, v2, v3) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE3(func, v2, v3) +#define NLOHMANN_JSON_PASTE5(func, v1, v2, v3, v4) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE4(func, v2, v3, v4) +#define NLOHMANN_JSON_PASTE6(func, v1, v2, v3, v4, v5) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE5(func, v2, v3, v4, v5) +#define NLOHMANN_JSON_PASTE7(func, v1, v2, v3, v4, v5, v6) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE6(func, v2, v3, v4, v5, v6) +#define NLOHMANN_JSON_PASTE8(func, v1, v2, v3, v4, v5, v6, v7) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE7(func, v2, v3, v4, v5, v6, v7) +#define NLOHMANN_JSON_PASTE9(func, v1, v2, v3, v4, v5, v6, v7, v8) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE8(func, v2, v3, v4, v5, v6, v7, v8) +#define NLOHMANN_JSON_PASTE10(func, v1, v2, v3, v4, v5, v6, v7, v8, v9) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE9(func, v2, v3, v4, v5, v6, v7, v8, v9) +#define NLOHMANN_JSON_PASTE11(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE10(func, v2, v3, v4, v5, v6, v7, v8, v9, v10) +#define NLOHMANN_JSON_PASTE12(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE11(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11) +#define NLOHMANN_JSON_PASTE13(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE12(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12) +#define NLOHMANN_JSON_PASTE14(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE13(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13) +#define NLOHMANN_JSON_PASTE15(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE14(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14) +#define NLOHMANN_JSON_PASTE16(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE15(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15) +#define NLOHMANN_JSON_PASTE17(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE16(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16) +#define NLOHMANN_JSON_PASTE18(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE17(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17) +#define NLOHMANN_JSON_PASTE19(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE18(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18) +#define NLOHMANN_JSON_PASTE20(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE19(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19) +#define NLOHMANN_JSON_PASTE21(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE20(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20) +#define NLOHMANN_JSON_PASTE22(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE21(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21) +#define NLOHMANN_JSON_PASTE23(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE22(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22) +#define NLOHMANN_JSON_PASTE24(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE23(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23) +#define NLOHMANN_JSON_PASTE25(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE24(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24) +#define NLOHMANN_JSON_PASTE26(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE25(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25) +#define NLOHMANN_JSON_PASTE27(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE26(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26) +#define NLOHMANN_JSON_PASTE28(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE27(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27) +#define NLOHMANN_JSON_PASTE29(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE28(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28) +#define NLOHMANN_JSON_PASTE30(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE29(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29) +#define NLOHMANN_JSON_PASTE31(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE30(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30) +#define NLOHMANN_JSON_PASTE32(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE31(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31) +#define NLOHMANN_JSON_PASTE33(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE32(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32) +#define NLOHMANN_JSON_PASTE34(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE33(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33) +#define NLOHMANN_JSON_PASTE35(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE34(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34) +#define NLOHMANN_JSON_PASTE36(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE35(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35) +#define NLOHMANN_JSON_PASTE37(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE36(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36) +#define NLOHMANN_JSON_PASTE38(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE37(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37) +#define NLOHMANN_JSON_PASTE39(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE38(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38) +#define NLOHMANN_JSON_PASTE40(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE39(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39) +#define NLOHMANN_JSON_PASTE41(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE40(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40) +#define NLOHMANN_JSON_PASTE42(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE41(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41) +#define NLOHMANN_JSON_PASTE43(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE42(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42) +#define NLOHMANN_JSON_PASTE44(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE43(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43) +#define NLOHMANN_JSON_PASTE45(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE44(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44) +#define NLOHMANN_JSON_PASTE46(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE45(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45) +#define NLOHMANN_JSON_PASTE47(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE46(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46) +#define NLOHMANN_JSON_PASTE48(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE47(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47) +#define NLOHMANN_JSON_PASTE49(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE48(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48) +#define NLOHMANN_JSON_PASTE50(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE49(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49) +#define NLOHMANN_JSON_PASTE51(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE50(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50) +#define NLOHMANN_JSON_PASTE52(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE51(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51) +#define NLOHMANN_JSON_PASTE53(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE52(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52) +#define NLOHMANN_JSON_PASTE54(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE53(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53) +#define NLOHMANN_JSON_PASTE55(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE54(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54) +#define NLOHMANN_JSON_PASTE56(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE55(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55) +#define NLOHMANN_JSON_PASTE57(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55, v56) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE56(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55, v56) +#define NLOHMANN_JSON_PASTE58(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55, v56, v57) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE57(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55, v56, v57) +#define NLOHMANN_JSON_PASTE59(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55, v56, v57, v58) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE58(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55, v56, v57, v58) +#define NLOHMANN_JSON_PASTE60(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55, v56, v57, v58, v59) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE59(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55, v56, v57, v58, v59) +#define NLOHMANN_JSON_PASTE61(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55, v56, v57, v58, v59, v60) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE60(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55, v56, v57, v58, v59, v60) +#define NLOHMANN_JSON_PASTE62(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55, v56, v57, v58, v59, v60, v61) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE61(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55, v56, v57, v58, v59, v60, v61) +#define NLOHMANN_JSON_PASTE63(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55, v56, v57, v58, v59, v60, v61, v62) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE62(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55, v56, v57, v58, v59, v60, v61, v62) +#define NLOHMANN_JSON_PASTE64(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55, v56, v57, v58, v59, v60, v61, v62, v63) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE63(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55, v56, v57, v58, v59, v60, v61, v62, v63) + +#define NLOHMANN_JSON_TO(v1) nlohmann_json_j[#v1] = nlohmann_json_t.v1; +#define NLOHMANN_JSON_FROM(v1) nlohmann_json_j.at(#v1).get_to(nlohmann_json_t.v1); +#define NLOHMANN_JSON_FROM_WITH_DEFAULT(v1) nlohmann_json_t.v1 = !nlohmann_json_j.is_null() ? nlohmann_json_j.value(#v1, nlohmann_json_default_obj.v1) : nlohmann_json_default_obj.v1; + +/*! +@brief macro +@def NLOHMANN_DEFINE_TYPE_INTRUSIVE +@since version 3.9.0 +@sa https://json.nlohmann.me/api/macros/nlohmann_define_type_intrusive/ +*/ +#define NLOHMANN_DEFINE_TYPE_INTRUSIVE(Type, ...) \ + template::value, int> = 0> \ + friend void to_json(BasicJsonType& nlohmann_json_j, const Type& nlohmann_json_t) { NLOHMANN_JSON_EXPAND(NLOHMANN_JSON_PASTE(NLOHMANN_JSON_TO, __VA_ARGS__)) } \ + template::value, int> = 0> \ + friend void from_json(const BasicJsonType& nlohmann_json_j, Type& nlohmann_json_t) { NLOHMANN_JSON_EXPAND(NLOHMANN_JSON_PASTE(NLOHMANN_JSON_FROM, __VA_ARGS__)) } + +/*! +@brief macro +@def NLOHMANN_DEFINE_TYPE_INTRUSIVE_WITH_DEFAULT +@since version 3.11.0 +@sa https://json.nlohmann.me/api/macros/nlohmann_define_type_intrusive/ +*/ +#define NLOHMANN_DEFINE_TYPE_INTRUSIVE_WITH_DEFAULT(Type, ...) \ + template::value, int> = 0> \ + friend void to_json(BasicJsonType& nlohmann_json_j, const Type& nlohmann_json_t) { NLOHMANN_JSON_EXPAND(NLOHMANN_JSON_PASTE(NLOHMANN_JSON_TO, __VA_ARGS__)) } \ + template::value, int> = 0> \ + friend void from_json(const BasicJsonType& nlohmann_json_j, Type& nlohmann_json_t) { const Type nlohmann_json_default_obj{}; NLOHMANN_JSON_EXPAND(NLOHMANN_JSON_PASTE(NLOHMANN_JSON_FROM_WITH_DEFAULT, __VA_ARGS__)) } + +/*! +@brief macro +@def NLOHMANN_DEFINE_TYPE_INTRUSIVE_ONLY_SERIALIZE +@since version 3.11.3 +@sa https://json.nlohmann.me/api/macros/nlohmann_define_type_intrusive/ +*/ +#define NLOHMANN_DEFINE_TYPE_INTRUSIVE_ONLY_SERIALIZE(Type, ...) \ + template::value, int> = 0> \ + friend void to_json(BasicJsonType& nlohmann_json_j, const Type& nlohmann_json_t) { NLOHMANN_JSON_EXPAND(NLOHMANN_JSON_PASTE(NLOHMANN_JSON_TO, __VA_ARGS__)) } + +/*! +@brief macro +@def NLOHMANN_DEFINE_TYPE_NON_INTRUSIVE +@since version 3.9.0 +@sa https://json.nlohmann.me/api/macros/nlohmann_define_type_non_intrusive/ +*/ +#define NLOHMANN_DEFINE_TYPE_NON_INTRUSIVE(Type, ...) \ + template::value, int> = 0> \ + void to_json(BasicJsonType& nlohmann_json_j, const Type& nlohmann_json_t) { NLOHMANN_JSON_EXPAND(NLOHMANN_JSON_PASTE(NLOHMANN_JSON_TO, __VA_ARGS__)) } \ + template::value, int> = 0> \ + void from_json(const BasicJsonType& nlohmann_json_j, Type& nlohmann_json_t) { NLOHMANN_JSON_EXPAND(NLOHMANN_JSON_PASTE(NLOHMANN_JSON_FROM, __VA_ARGS__)) } + +/*! +@brief macro +@def NLOHMANN_DEFINE_TYPE_NON_INTRUSIVE_WITH_DEFAULT +@since version 3.11.0 +@sa https://json.nlohmann.me/api/macros/nlohmann_define_type_non_intrusive/ +*/ +#define NLOHMANN_DEFINE_TYPE_NON_INTRUSIVE_WITH_DEFAULT(Type, ...) \ + template::value, int> = 0> \ + void to_json(BasicJsonType& nlohmann_json_j, const Type& nlohmann_json_t) { NLOHMANN_JSON_EXPAND(NLOHMANN_JSON_PASTE(NLOHMANN_JSON_TO, __VA_ARGS__)) } \ + template::value, int> = 0> \ + void from_json(const BasicJsonType& nlohmann_json_j, Type& nlohmann_json_t) { const Type nlohmann_json_default_obj{}; NLOHMANN_JSON_EXPAND(NLOHMANN_JSON_PASTE(NLOHMANN_JSON_FROM_WITH_DEFAULT, __VA_ARGS__)) } + +/*! +@brief macro +@def NLOHMANN_DEFINE_TYPE_NON_INTRUSIVE_ONLY_SERIALIZE +@since version 3.11.3 +@sa https://json.nlohmann.me/api/macros/nlohmann_define_type_non_intrusive/ +*/ +#define NLOHMANN_DEFINE_TYPE_NON_INTRUSIVE_ONLY_SERIALIZE(Type, ...) \ + template::value, int> = 0> \ + void to_json(BasicJsonType& nlohmann_json_j, const Type& nlohmann_json_t) { NLOHMANN_JSON_EXPAND(NLOHMANN_JSON_PASTE(NLOHMANN_JSON_TO, __VA_ARGS__)) } + +/*! +@brief macro +@def NLOHMANN_DEFINE_DERIVED_TYPE_INTRUSIVE +@since version 3.12.0 +@sa https://json.nlohmann.me/api/macros/nlohmann_define_derived_type/ +*/ +#define NLOHMANN_DEFINE_DERIVED_TYPE_INTRUSIVE(Type, BaseType, ...) \ + template::value, int> = 0> \ + friend void to_json(BasicJsonType& nlohmann_json_j, const Type& nlohmann_json_t) { nlohmann::to_json(nlohmann_json_j, static_cast(nlohmann_json_t)); NLOHMANN_JSON_EXPAND(NLOHMANN_JSON_PASTE(NLOHMANN_JSON_TO, __VA_ARGS__)) } \ + template::value, int> = 0> \ + friend void from_json(const BasicJsonType& nlohmann_json_j, Type& nlohmann_json_t) { nlohmann::from_json(nlohmann_json_j, static_cast(nlohmann_json_t)); NLOHMANN_JSON_EXPAND(NLOHMANN_JSON_PASTE(NLOHMANN_JSON_FROM, __VA_ARGS__)) } + +/*! +@brief macro +@def NLOHMANN_DEFINE_DERIVED_TYPE_INTRUSIVE_WITH_DEFAULT +@since version 3.12.0 +@sa https://json.nlohmann.me/api/macros/nlohmann_define_derived_type/ +*/ +#define NLOHMANN_DEFINE_DERIVED_TYPE_INTRUSIVE_WITH_DEFAULT(Type, BaseType, ...) \ + template::value, int> = 0> \ + friend void to_json(BasicJsonType& nlohmann_json_j, const Type& nlohmann_json_t) { nlohmann::to_json(nlohmann_json_j, static_cast(nlohmann_json_t)); NLOHMANN_JSON_EXPAND(NLOHMANN_JSON_PASTE(NLOHMANN_JSON_TO, __VA_ARGS__)) } \ + template::value, int> = 0> \ + friend void from_json(const BasicJsonType& nlohmann_json_j, Type& nlohmann_json_t) { nlohmann::from_json(nlohmann_json_j, static_cast(nlohmann_json_t)); const Type nlohmann_json_default_obj{}; NLOHMANN_JSON_EXPAND(NLOHMANN_JSON_PASTE(NLOHMANN_JSON_FROM_WITH_DEFAULT, __VA_ARGS__)) } + +/*! +@brief macro +@def NLOHMANN_DEFINE_DERIVED_TYPE_INTRUSIVE_ONLY_SERIALIZE +@since version 3.12.0 +@sa https://json.nlohmann.me/api/macros/nlohmann_define_derived_type/ +*/ +#define NLOHMANN_DEFINE_DERIVED_TYPE_INTRUSIVE_ONLY_SERIALIZE(Type, BaseType, ...) \ + template::value, int> = 0> \ + friend void to_json(BasicJsonType& nlohmann_json_j, const Type& nlohmann_json_t) { nlohmann::to_json(nlohmann_json_j, static_cast(nlohmann_json_t)); NLOHMANN_JSON_EXPAND(NLOHMANN_JSON_PASTE(NLOHMANN_JSON_TO, __VA_ARGS__)) } + +/*! +@brief macro +@def NLOHMANN_DEFINE_DERIVED_TYPE_NON_INTRUSIVE +@since version 3.12.0 +@sa https://json.nlohmann.me/api/macros/nlohmann_define_derived_type/ +*/ +#define NLOHMANN_DEFINE_DERIVED_TYPE_NON_INTRUSIVE(Type, BaseType, ...) \ + template::value, int> = 0> \ + void to_json(BasicJsonType& nlohmann_json_j, const Type& nlohmann_json_t) { nlohmann::to_json(nlohmann_json_j, static_cast(nlohmann_json_t)); NLOHMANN_JSON_EXPAND(NLOHMANN_JSON_PASTE(NLOHMANN_JSON_TO, __VA_ARGS__)) } \ + template::value, int> = 0> \ + void from_json(const BasicJsonType& nlohmann_json_j, Type& nlohmann_json_t) { nlohmann::from_json(nlohmann_json_j, static_cast(nlohmann_json_t)); NLOHMANN_JSON_EXPAND(NLOHMANN_JSON_PASTE(NLOHMANN_JSON_FROM, __VA_ARGS__)) } + +/*! +@brief macro +@def NLOHMANN_DEFINE_DERIVED_TYPE_NON_INTRUSIVE_WITH_DEFAULT +@since version 3.12.0 +@sa https://json.nlohmann.me/api/macros/nlohmann_define_derived_type/ +*/ +#define NLOHMANN_DEFINE_DERIVED_TYPE_NON_INTRUSIVE_WITH_DEFAULT(Type, BaseType, ...) \ + template::value, int> = 0> \ + void to_json(BasicJsonType& nlohmann_json_j, const Type& nlohmann_json_t) { nlohmann::to_json(nlohmann_json_j, static_cast(nlohmann_json_t)); NLOHMANN_JSON_EXPAND(NLOHMANN_JSON_PASTE(NLOHMANN_JSON_TO, __VA_ARGS__)) } \ + template::value, int> = 0> \ + void from_json(const BasicJsonType& nlohmann_json_j, Type& nlohmann_json_t) { nlohmann::from_json(nlohmann_json_j, static_cast(nlohmann_json_t)); const Type nlohmann_json_default_obj{}; NLOHMANN_JSON_EXPAND(NLOHMANN_JSON_PASTE(NLOHMANN_JSON_FROM_WITH_DEFAULT, __VA_ARGS__)) } + +/*! +@brief macro +@def NLOHMANN_DEFINE_DERIVED_TYPE_NON_INTRUSIVE_ONLY_SERIALIZE +@since version 3.12.0 +@sa https://json.nlohmann.me/api/macros/nlohmann_define_derived_type/ +*/ +#define NLOHMANN_DEFINE_DERIVED_TYPE_NON_INTRUSIVE_ONLY_SERIALIZE(Type, BaseType, ...) \ + template::value, int> = 0> \ + void to_json(BasicJsonType& nlohmann_json_j, const Type& nlohmann_json_t) { nlohmann::to_json(nlohmann_json_j, static_cast(nlohmann_json_t)); NLOHMANN_JSON_EXPAND(NLOHMANN_JSON_PASTE(NLOHMANN_JSON_TO, __VA_ARGS__)) } + +// inspired from https://stackoverflow.com/a/26745591 +// allows calling any std function as if (e.g., with begin): +// using std::begin; begin(x); +// +// it allows using the detected idiom to retrieve the return type +// of such an expression +#define NLOHMANN_CAN_CALL_STD_FUNC_IMPL(std_name) \ + namespace detail { \ + using std::std_name; \ + \ + template \ + using result_of_##std_name = decltype(std_name(std::declval()...)); \ + } \ + \ + namespace detail2 { \ + struct std_name##_tag \ + { \ + }; \ + \ + template \ + std_name##_tag std_name(T&&...); \ + \ + template \ + using result_of_##std_name = decltype(std_name(std::declval()...)); \ + \ + template \ + struct would_call_std_##std_name \ + { \ + static constexpr auto const value = ::nlohmann::detail:: \ + is_detected_exact::value; \ + }; \ + } /* namespace detail2 */ \ + \ + template \ + struct would_call_std_##std_name : detail2::would_call_std_##std_name \ + { \ + } + +#ifndef JSON_USE_IMPLICIT_CONVERSIONS + #define JSON_USE_IMPLICIT_CONVERSIONS 1 +#endif + +#if JSON_USE_IMPLICIT_CONVERSIONS + #define JSON_EXPLICIT +#else + #define JSON_EXPLICIT explicit +#endif + +#ifndef JSON_DISABLE_ENUM_SERIALIZATION + #define JSON_DISABLE_ENUM_SERIALIZATION 0 +#endif + +#ifndef JSON_USE_GLOBAL_UDLS + #define JSON_USE_GLOBAL_UDLS 1 +#endif diff --git a/src/mc/include/nlohmann/detail/macro_unscope.hpp b/src/mc/include/nlohmann/detail/macro_unscope.hpp new file mode 100755 index 0000000..2edb168 --- /dev/null +++ b/src/mc/include/nlohmann/detail/macro_unscope.hpp @@ -0,0 +1,46 @@ +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.12.0 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2013 - 2025 Niels Lohmann +// SPDX-License-Identifier: MIT + +#pragma once + +// restore clang diagnostic settings +#if defined(__clang__) + #pragma clang diagnostic pop +#endif + +// clean up +#undef JSON_ASSERT +#undef JSON_INTERNAL_CATCH +#undef JSON_THROW +#undef JSON_PRIVATE_UNLESS_TESTED +#undef NLOHMANN_BASIC_JSON_TPL_DECLARATION +#undef NLOHMANN_BASIC_JSON_TPL +#undef JSON_EXPLICIT +#undef NLOHMANN_CAN_CALL_STD_FUNC_IMPL +#undef JSON_INLINE_VARIABLE +#undef JSON_NO_UNIQUE_ADDRESS +#undef JSON_DISABLE_ENUM_SERIALIZATION +#undef JSON_USE_GLOBAL_UDLS + +#ifndef JSON_TEST_KEEP_MACROS + #undef JSON_CATCH + #undef JSON_TRY + #undef JSON_HAS_CPP_11 + #undef JSON_HAS_CPP_14 + #undef JSON_HAS_CPP_17 + #undef JSON_HAS_CPP_20 + #undef JSON_HAS_CPP_23 + #undef JSON_HAS_FILESYSTEM + #undef JSON_HAS_EXPERIMENTAL_FILESYSTEM + #undef JSON_HAS_THREE_WAY_COMPARISON + #undef JSON_HAS_RANGES + #undef JSON_HAS_STATIC_RTTI + #undef JSON_USE_LEGACY_DISCARDED_VALUE_COMPARISON +#endif + +#include diff --git a/src/mc/include/nlohmann/detail/meta/call_std/begin.hpp b/src/mc/include/nlohmann/detail/meta/call_std/begin.hpp new file mode 100755 index 0000000..6e697b5 --- /dev/null +++ b/src/mc/include/nlohmann/detail/meta/call_std/begin.hpp @@ -0,0 +1,17 @@ +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.12.0 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2013 - 2025 Niels Lohmann +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +NLOHMANN_JSON_NAMESPACE_BEGIN + +NLOHMANN_CAN_CALL_STD_FUNC_IMPL(begin); + +NLOHMANN_JSON_NAMESPACE_END diff --git a/src/mc/include/nlohmann/detail/meta/call_std/end.hpp b/src/mc/include/nlohmann/detail/meta/call_std/end.hpp new file mode 100755 index 0000000..4d27914 --- /dev/null +++ b/src/mc/include/nlohmann/detail/meta/call_std/end.hpp @@ -0,0 +1,17 @@ +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.12.0 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2013 - 2025 Niels Lohmann +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +NLOHMANN_JSON_NAMESPACE_BEGIN + +NLOHMANN_CAN_CALL_STD_FUNC_IMPL(end); + +NLOHMANN_JSON_NAMESPACE_END diff --git a/src/mc/include/nlohmann/detail/meta/cpp_future.hpp b/src/mc/include/nlohmann/detail/meta/cpp_future.hpp new file mode 100755 index 0000000..57811b9 --- /dev/null +++ b/src/mc/include/nlohmann/detail/meta/cpp_future.hpp @@ -0,0 +1,171 @@ +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.12.0 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2013 - 2025 Niels Lohmann +// SPDX-FileCopyrightText: 2018 The Abseil Authors +// SPDX-License-Identifier: MIT + +#pragma once + +#include // array +#include // size_t +#include // conditional, enable_if, false_type, integral_constant, is_constructible, is_integral, is_same, remove_cv, remove_reference, true_type +#include // index_sequence, make_index_sequence, index_sequence_for + +#include + +NLOHMANN_JSON_NAMESPACE_BEGIN +namespace detail +{ + +template +using uncvref_t = typename std::remove_cv::type>::type; + +#ifdef JSON_HAS_CPP_14 + +// the following utilities are natively available in C++14 +using std::enable_if_t; +using std::index_sequence; +using std::make_index_sequence; +using std::index_sequence_for; + +#else + +// alias templates to reduce boilerplate +template +using enable_if_t = typename std::enable_if::type; + +// The following code is taken from https://github.com/abseil/abseil-cpp/blob/10cb35e459f5ecca5b2ff107635da0bfa41011b4/absl/utility/utility.h +// which is part of Google Abseil (https://github.com/abseil/abseil-cpp), licensed under the Apache License 2.0. + +//// START OF CODE FROM GOOGLE ABSEIL + +// integer_sequence +// +// Class template representing a compile-time integer sequence. An instantiation +// of `integer_sequence` has a sequence of integers encoded in its +// type through its template arguments (which is a common need when +// working with C++11 variadic templates). `absl::integer_sequence` is designed +// to be a drop-in replacement for C++14's `std::integer_sequence`. +// +// Example: +// +// template< class T, T... Ints > +// void user_function(integer_sequence); +// +// int main() +// { +// // user_function's `T` will be deduced to `int` and `Ints...` +// // will be deduced to `0, 1, 2, 3, 4`. +// user_function(make_integer_sequence()); +// } +template +struct integer_sequence +{ + using value_type = T; + static constexpr std::size_t size() noexcept + { + return sizeof...(Ints); + } +}; + +// index_sequence +// +// A helper template for an `integer_sequence` of `size_t`, +// `absl::index_sequence` is designed to be a drop-in replacement for C++14's +// `std::index_sequence`. +template +using index_sequence = integer_sequence; + +namespace utility_internal +{ + +template +struct Extend; + +// Note that SeqSize == sizeof...(Ints). It's passed explicitly for efficiency. +template +struct Extend, SeqSize, 0> +{ + using type = integer_sequence < T, Ints..., (Ints + SeqSize)... >; +}; + +template +struct Extend, SeqSize, 1> +{ + using type = integer_sequence < T, Ints..., (Ints + SeqSize)..., 2 * SeqSize >; +}; + +// Recursion helper for 'make_integer_sequence'. +// 'Gen::type' is an alias for 'integer_sequence'. +template +struct Gen +{ + using type = + typename Extend < typename Gen < T, N / 2 >::type, N / 2, N % 2 >::type; +}; + +template +struct Gen +{ + using type = integer_sequence; +}; + +} // namespace utility_internal + +// Compile-time sequences of integers + +// make_integer_sequence +// +// This template alias is equivalent to +// `integer_sequence`, and is designed to be a drop-in +// replacement for C++14's `std::make_integer_sequence`. +template +using make_integer_sequence = typename utility_internal::Gen::type; + +// make_index_sequence +// +// This template alias is equivalent to `index_sequence<0, 1, ..., N-1>`, +// and is designed to be a drop-in replacement for C++14's +// `std::make_index_sequence`. +template +using make_index_sequence = make_integer_sequence; + +// index_sequence_for +// +// Converts a typename pack into an index sequence of the same length, and +// is designed to be a drop-in replacement for C++14's +// `std::index_sequence_for()` +template +using index_sequence_for = make_index_sequence; + +//// END OF CODE FROM GOOGLE ABSEIL + +#endif + +// dispatch utility (taken from ranges-v3) +template struct priority_tag : priority_tag < N - 1 > {}; +template<> struct priority_tag<0> {}; + +// taken from ranges-v3 +template +struct static_const +{ + static JSON_INLINE_VARIABLE constexpr T value{}; +}; + +#ifndef JSON_HAS_CPP_17 + template + constexpr T static_const::value; +#endif + +template +constexpr std::array make_array(Args&& ... args) +{ + return std::array {{static_cast(std::forward(args))...}}; +} + +} // namespace detail +NLOHMANN_JSON_NAMESPACE_END diff --git a/src/mc/include/nlohmann/detail/meta/detected.hpp b/src/mc/include/nlohmann/detail/meta/detected.hpp new file mode 100755 index 0000000..c394733 --- /dev/null +++ b/src/mc/include/nlohmann/detail/meta/detected.hpp @@ -0,0 +1,70 @@ +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.12.0 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2013 - 2025 Niels Lohmann +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +#include + +NLOHMANN_JSON_NAMESPACE_BEGIN +namespace detail +{ + +// https://en.cppreference.com/w/cpp/experimental/is_detected +struct nonesuch +{ + nonesuch() = delete; + ~nonesuch() = delete; + nonesuch(nonesuch const&) = delete; + nonesuch(nonesuch const&&) = delete; + void operator=(nonesuch const&) = delete; + void operator=(nonesuch&&) = delete; +}; + +template class Op, + class... Args> +struct detector +{ + using value_t = std::false_type; + using type = Default; +}; + +template class Op, class... Args> +struct detector>, Op, Args...> +{ + using value_t = std::true_type; + using type = Op; +}; + +template class Op, class... Args> +using is_detected = typename detector::value_t; + +template class Op, class... Args> +struct is_detected_lazy : is_detected { }; + +template class Op, class... Args> +using detected_t = typename detector::type; + +template class Op, class... Args> +using detected_or = detector; + +template class Op, class... Args> +using detected_or_t = typename detected_or::type; + +template class Op, class... Args> +using is_detected_exact = std::is_same>; + +template class Op, class... Args> +using is_detected_convertible = + std::is_convertible, To>; + +} // namespace detail +NLOHMANN_JSON_NAMESPACE_END diff --git a/src/mc/include/nlohmann/detail/meta/identity_tag.hpp b/src/mc/include/nlohmann/detail/meta/identity_tag.hpp new file mode 100755 index 0000000..c39dabb --- /dev/null +++ b/src/mc/include/nlohmann/detail/meta/identity_tag.hpp @@ -0,0 +1,21 @@ +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.12.0 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2013 - 2025 Niels Lohmann +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +NLOHMANN_JSON_NAMESPACE_BEGIN +namespace detail +{ + +// dispatching helper struct +template struct identity_tag {}; + +} // namespace detail +NLOHMANN_JSON_NAMESPACE_END diff --git a/src/mc/include/nlohmann/detail/meta/is_sax.hpp b/src/mc/include/nlohmann/detail/meta/is_sax.hpp new file mode 100755 index 0000000..2574f72 --- /dev/null +++ b/src/mc/include/nlohmann/detail/meta/is_sax.hpp @@ -0,0 +1,159 @@ +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.12.0 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2013 - 2025 Niels Lohmann +// SPDX-License-Identifier: MIT + +#pragma once + +#include // size_t +#include // declval +#include // string + +#include +#include +#include + +NLOHMANN_JSON_NAMESPACE_BEGIN +namespace detail +{ + +template +using null_function_t = decltype(std::declval().null()); + +template +using boolean_function_t = + decltype(std::declval().boolean(std::declval())); + +template +using number_integer_function_t = + decltype(std::declval().number_integer(std::declval())); + +template +using number_unsigned_function_t = + decltype(std::declval().number_unsigned(std::declval())); + +template +using number_float_function_t = decltype(std::declval().number_float( + std::declval(), std::declval())); + +template +using string_function_t = + decltype(std::declval().string(std::declval())); + +template +using binary_function_t = + decltype(std::declval().binary(std::declval())); + +template +using start_object_function_t = + decltype(std::declval().start_object(std::declval())); + +template +using key_function_t = + decltype(std::declval().key(std::declval())); + +template +using end_object_function_t = decltype(std::declval().end_object()); + +template +using start_array_function_t = + decltype(std::declval().start_array(std::declval())); + +template +using end_array_function_t = decltype(std::declval().end_array()); + +template +using parse_error_function_t = decltype(std::declval().parse_error( + std::declval(), std::declval(), + std::declval())); + +template +struct is_sax +{ + private: + static_assert(is_basic_json::value, + "BasicJsonType must be of type basic_json<...>"); + + using number_integer_t = typename BasicJsonType::number_integer_t; + using number_unsigned_t = typename BasicJsonType::number_unsigned_t; + using number_float_t = typename BasicJsonType::number_float_t; + using string_t = typename BasicJsonType::string_t; + using binary_t = typename BasicJsonType::binary_t; + using exception_t = typename BasicJsonType::exception; + + public: + static constexpr bool value = + is_detected_exact::value && + is_detected_exact::value && + is_detected_exact::value && + is_detected_exact::value && + is_detected_exact::value && + is_detected_exact::value && + is_detected_exact::value && + is_detected_exact::value && + is_detected_exact::value && + is_detected_exact::value && + is_detected_exact::value && + is_detected_exact::value && + is_detected_exact::value; +}; + +template +struct is_sax_static_asserts +{ + private: + static_assert(is_basic_json::value, + "BasicJsonType must be of type basic_json<...>"); + + using number_integer_t = typename BasicJsonType::number_integer_t; + using number_unsigned_t = typename BasicJsonType::number_unsigned_t; + using number_float_t = typename BasicJsonType::number_float_t; + using string_t = typename BasicJsonType::string_t; + using binary_t = typename BasicJsonType::binary_t; + using exception_t = typename BasicJsonType::exception; + + public: + static_assert(is_detected_exact::value, + "Missing/invalid function: bool null()"); + static_assert(is_detected_exact::value, + "Missing/invalid function: bool boolean(bool)"); + static_assert(is_detected_exact::value, + "Missing/invalid function: bool boolean(bool)"); + static_assert( + is_detected_exact::value, + "Missing/invalid function: bool number_integer(number_integer_t)"); + static_assert( + is_detected_exact::value, + "Missing/invalid function: bool number_unsigned(number_unsigned_t)"); + static_assert(is_detected_exact::value, + "Missing/invalid function: bool number_float(number_float_t, const string_t&)"); + static_assert( + is_detected_exact::value, + "Missing/invalid function: bool string(string_t&)"); + static_assert( + is_detected_exact::value, + "Missing/invalid function: bool binary(binary_t&)"); + static_assert(is_detected_exact::value, + "Missing/invalid function: bool start_object(std::size_t)"); + static_assert(is_detected_exact::value, + "Missing/invalid function: bool key(string_t&)"); + static_assert(is_detected_exact::value, + "Missing/invalid function: bool end_object()"); + static_assert(is_detected_exact::value, + "Missing/invalid function: bool start_array(std::size_t)"); + static_assert(is_detected_exact::value, + "Missing/invalid function: bool end_array()"); + static_assert( + is_detected_exact::value, + "Missing/invalid function: bool parse_error(std::size_t, const " + "std::string&, const exception&)"); +}; + +} // namespace detail +NLOHMANN_JSON_NAMESPACE_END diff --git a/src/mc/include/nlohmann/detail/meta/std_fs.hpp b/src/mc/include/nlohmann/detail/meta/std_fs.hpp new file mode 100755 index 0000000..2f33e32 --- /dev/null +++ b/src/mc/include/nlohmann/detail/meta/std_fs.hpp @@ -0,0 +1,29 @@ +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.12.0 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2013 - 2025 Niels Lohmann +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +#if JSON_HAS_EXPERIMENTAL_FILESYSTEM +#include +NLOHMANN_JSON_NAMESPACE_BEGIN +namespace detail +{ +namespace std_fs = std::experimental::filesystem; +} // namespace detail +NLOHMANN_JSON_NAMESPACE_END +#elif JSON_HAS_FILESYSTEM +#include // NOLINT(build/c++17) +NLOHMANN_JSON_NAMESPACE_BEGIN +namespace detail +{ +namespace std_fs = std::filesystem; +} // namespace detail +NLOHMANN_JSON_NAMESPACE_END +#endif diff --git a/src/mc/include/nlohmann/detail/meta/type_traits.hpp b/src/mc/include/nlohmann/detail/meta/type_traits.hpp new file mode 100755 index 0000000..06dcb63 --- /dev/null +++ b/src/mc/include/nlohmann/detail/meta/type_traits.hpp @@ -0,0 +1,821 @@ +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.12.0 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2013 - 2025 Niels Lohmann +// SPDX-License-Identifier: MIT + +#pragma once + +#include // numeric_limits +#include // char_traits +#include // tuple +#include // false_type, is_constructible, is_integral, is_same, true_type +#include // declval +#if defined(__cpp_lib_byte) && __cpp_lib_byte >= 201603L + #include // byte +#endif +#include +#include +#include +#include +#include +#include +#include + +NLOHMANN_JSON_NAMESPACE_BEGIN +/*! +@brief detail namespace with internal helper functions + +This namespace collects functions that should not be exposed, +implementations of some @ref basic_json methods, and meta-programming helpers. + +@since version 2.1.0 +*/ +namespace detail +{ + +///////////// +// helpers // +///////////// + +// Note to maintainers: +// +// Every trait in this file expects a non-CV-qualified type. +// The only exceptions are in the 'aliases for detected' section +// (i.e., those of the form: decltype(T::member_function(std::declval()))) +// +// In this case, T has to be properly CV-qualified to constraint the function arguments +// (e.g., to_json(BasicJsonType&, const T&)) + +template struct is_basic_json : std::false_type {}; + +NLOHMANN_BASIC_JSON_TPL_DECLARATION +struct is_basic_json : std::true_type {}; + +// used by exceptions create() member functions +// true_type for the pointer to possibly cv-qualified basic_json or std::nullptr_t +// false_type otherwise +template +struct is_basic_json_context : + std::integral_constant < bool, + is_basic_json::type>::type>::value + || std::is_same::value > +{}; + +////////////////////// +// json_ref helpers // +////////////////////// + +template +class json_ref; + +template +struct is_json_ref : std::false_type {}; + +template +struct is_json_ref> : std::true_type {}; + +////////////////////////// +// aliases for detected // +////////////////////////// + +template +using mapped_type_t = typename T::mapped_type; + +template +using key_type_t = typename T::key_type; + +template +using value_type_t = typename T::value_type; + +template +using difference_type_t = typename T::difference_type; + +template +using pointer_t = typename T::pointer; + +template +using reference_t = typename T::reference; + +template +using iterator_category_t = typename T::iterator_category; + +template +using to_json_function = decltype(T::to_json(std::declval()...)); + +template +using from_json_function = decltype(T::from_json(std::declval()...)); + +template +using get_template_function = decltype(std::declval().template get()); + +// trait checking if JSONSerializer::from_json(json const&, udt&) exists +template +struct has_from_json : std::false_type {}; + +// trait checking if j.get is valid +// use this trait instead of std::is_constructible or std::is_convertible, +// both rely on, or make use of implicit conversions, and thus fail when T +// has several constructors/operator= (see https://github.com/nlohmann/json/issues/958) +template +struct is_getable +{ + static constexpr bool value = is_detected::value; +}; + +template +struct has_from_json < BasicJsonType, T, enable_if_t < !is_basic_json::value >> +{ + using serializer = typename BasicJsonType::template json_serializer; + + static constexpr bool value = + is_detected_exact::value; +}; + +// This trait checks if JSONSerializer::from_json(json const&) exists +// this overload is used for non-default-constructible user-defined-types +template +struct has_non_default_from_json : std::false_type {}; + +template +struct has_non_default_from_json < BasicJsonType, T, enable_if_t < !is_basic_json::value >> +{ + using serializer = typename BasicJsonType::template json_serializer; + + static constexpr bool value = + is_detected_exact::value; +}; + +// This trait checks if BasicJsonType::json_serializer::to_json exists +// Do not evaluate the trait when T is a basic_json type, to avoid template instantiation infinite recursion. +template +struct has_to_json : std::false_type {}; + +template +struct has_to_json < BasicJsonType, T, enable_if_t < !is_basic_json::value >> +{ + using serializer = typename BasicJsonType::template json_serializer; + + static constexpr bool value = + is_detected_exact::value; +}; + +template +using detect_key_compare = typename T::key_compare; + +template +struct has_key_compare : std::integral_constant::value> {}; + +// obtains the actual object key comparator +template +struct actual_object_comparator +{ + using object_t = typename BasicJsonType::object_t; + using object_comparator_t = typename BasicJsonType::default_object_comparator_t; + using type = typename std::conditional < has_key_compare::value, + typename object_t::key_compare, object_comparator_t>::type; +}; + +template +using actual_object_comparator_t = typename actual_object_comparator::type; + +///////////////// +// char_traits // +///////////////// + +// Primary template of char_traits calls std char_traits +template +struct char_traits : std::char_traits +{}; + +// Explicitly define char traits for unsigned char since it is not standard +template<> +struct char_traits : std::char_traits +{ + using char_type = unsigned char; + using int_type = uint64_t; + + // Redefine to_int_type function + static int_type to_int_type(char_type c) noexcept + { + return static_cast(c); + } + + static char_type to_char_type(int_type i) noexcept + { + return static_cast(i); + } + + static constexpr int_type eof() noexcept + { + return static_cast(std::char_traits::eof()); + } +}; + +// Explicitly define char traits for signed char since it is not standard +template<> +struct char_traits : std::char_traits +{ + using char_type = signed char; + using int_type = uint64_t; + + // Redefine to_int_type function + static int_type to_int_type(char_type c) noexcept + { + return static_cast(c); + } + + static char_type to_char_type(int_type i) noexcept + { + return static_cast(i); + } + + static constexpr int_type eof() noexcept + { + return static_cast(std::char_traits::eof()); + } +}; + +#if defined(__cpp_lib_byte) && __cpp_lib_byte >= 201603L +template<> +struct char_traits : std::char_traits +{ + using char_type = std::byte; + using int_type = uint64_t; + + static int_type to_int_type(char_type c) noexcept + { + return static_cast(std::to_integer(c)); + } + + static char_type to_char_type(int_type i) noexcept + { + return std::byte(static_cast(i)); + } + + static constexpr int_type eof() noexcept + { + return static_cast(std::char_traits::eof()); + } +}; +#endif + +/////////////////// +// is_ functions // +/////////////////// + +// https://en.cppreference.com/w/cpp/types/conjunction +template struct conjunction : std::true_type { }; +template struct conjunction : B { }; +template +struct conjunction +: std::conditional(B::value), conjunction, B>::type {}; + +// https://en.cppreference.com/w/cpp/types/negation +template struct negation : std::integral_constant < bool, !B::value > { }; + +// Reimplementation of is_constructible and is_default_constructible, due to them being broken for +// std::pair and std::tuple until LWG 2367 fix (see https://cplusplus.github.io/LWG/lwg-defects.html#2367). +// This causes compile errors in e.g., Clang 3.5 or GCC 4.9. +template +struct is_default_constructible : std::is_default_constructible {}; + +template +struct is_default_constructible> + : conjunction, is_default_constructible> {}; + +template +struct is_default_constructible> + : conjunction, is_default_constructible> {}; + +template +struct is_default_constructible> + : conjunction...> {}; + +template +struct is_default_constructible> + : conjunction...> {}; + +template +struct is_constructible : std::is_constructible {}; + +template +struct is_constructible> : is_default_constructible> {}; + +template +struct is_constructible> : is_default_constructible> {}; + +template +struct is_constructible> : is_default_constructible> {}; + +template +struct is_constructible> : is_default_constructible> {}; + +template +struct is_iterator_traits : std::false_type {}; + +template +struct is_iterator_traits> +{ + private: + using traits = iterator_traits; + + public: + static constexpr auto value = + is_detected::value && + is_detected::value && + is_detected::value && + is_detected::value && + is_detected::value; +}; + +template +struct is_range +{ + private: + using t_ref = typename std::add_lvalue_reference::type; + + using iterator = detected_t; + using sentinel = detected_t; + + // to be 100% correct, it should use https://en.cppreference.com/w/cpp/iterator/input_or_output_iterator + // and https://en.cppreference.com/w/cpp/iterator/sentinel_for + // but reimplementing these would be too much work, as a lot of other concepts are used underneath + static constexpr auto is_iterator_begin = + is_iterator_traits>::value; + + public: + static constexpr bool value = !std::is_same::value && !std::is_same::value && is_iterator_begin; +}; + +template +using iterator_t = enable_if_t::value, result_of_begin())>>; + +template +using range_value_t = value_type_t>>; + +// The following implementation of is_complete_type is taken from +// https://blogs.msdn.microsoft.com/vcblog/2015/12/02/partial-support-for-expression-sfinae-in-vs-2015-update-1/ +// and is written by Xiang Fan who agreed to use it in this library. + +template +struct is_complete_type : std::false_type {}; + +template +struct is_complete_type : std::true_type {}; + +template +struct is_compatible_object_type_impl : std::false_type {}; + +template +struct is_compatible_object_type_impl < + BasicJsonType, CompatibleObjectType, + enable_if_t < is_detected::value&& + is_detected::value >> +{ + using object_t = typename BasicJsonType::object_t; + + // macOS's is_constructible does not play well with nonesuch... + static constexpr bool value = + is_constructible::value && + is_constructible::value; +}; + +template +struct is_compatible_object_type + : is_compatible_object_type_impl {}; + +template +struct is_constructible_object_type_impl : std::false_type {}; + +template +struct is_constructible_object_type_impl < + BasicJsonType, ConstructibleObjectType, + enable_if_t < is_detected::value&& + is_detected::value >> +{ + using object_t = typename BasicJsonType::object_t; + + static constexpr bool value = + (is_default_constructible::value && + (std::is_move_assignable::value || + std::is_copy_assignable::value) && + (is_constructible::value && + std::is_same < + typename object_t::mapped_type, + typename ConstructibleObjectType::mapped_type >::value)) || + (has_from_json::value || + has_non_default_from_json < + BasicJsonType, + typename ConstructibleObjectType::mapped_type >::value); +}; + +template +struct is_constructible_object_type + : is_constructible_object_type_impl {}; + +template +struct is_compatible_string_type +{ + static constexpr auto value = + is_constructible::value; +}; + +template +struct is_constructible_string_type +{ + // launder type through decltype() to fix compilation failure on ICPC +#ifdef __INTEL_COMPILER + using laundered_type = decltype(std::declval()); +#else + using laundered_type = ConstructibleStringType; +#endif + + static constexpr auto value = + conjunction < + is_constructible, + is_detected_exact>::value; +}; + +template +struct is_compatible_array_type_impl : std::false_type {}; + +template +struct is_compatible_array_type_impl < + BasicJsonType, CompatibleArrayType, + enable_if_t < + is_detected::value&& + is_iterator_traits>>::value&& +// special case for types like std::filesystem::path whose iterator's value_type are themselves +// c.f. https://github.com/nlohmann/json/pull/3073 + !std::is_same>::value >> +{ + static constexpr bool value = + is_constructible>::value; +}; + +template +struct is_compatible_array_type + : is_compatible_array_type_impl {}; + +template +struct is_constructible_array_type_impl : std::false_type {}; + +template +struct is_constructible_array_type_impl < + BasicJsonType, ConstructibleArrayType, + enable_if_t::value >> + : std::true_type {}; + +template +struct is_constructible_array_type_impl < + BasicJsonType, ConstructibleArrayType, + enable_if_t < !std::is_same::value&& + !is_compatible_string_type::value&& + is_default_constructible::value&& +(std::is_move_assignable::value || + std::is_copy_assignable::value)&& +is_detected::value&& +is_iterator_traits>>::value&& +is_detected::value&& +// special case for types like std::filesystem::path whose iterator's value_type are themselves +// c.f. https://github.com/nlohmann/json/pull/3073 +!std::is_same>::value&& +is_complete_type < +detected_t>::value >> +{ + using value_type = range_value_t; + + static constexpr bool value = + std::is_same::value || + has_from_json::value || + has_non_default_from_json < + BasicJsonType, + value_type >::value; +}; + +template +struct is_constructible_array_type + : is_constructible_array_type_impl {}; + +template +struct is_compatible_integer_type_impl : std::false_type {}; + +template +struct is_compatible_integer_type_impl < + RealIntegerType, CompatibleNumberIntegerType, + enable_if_t < std::is_integral::value&& + std::is_integral::value&& + !std::is_same::value >> +{ + // is there an assert somewhere on overflows? + using RealLimits = std::numeric_limits; + using CompatibleLimits = std::numeric_limits; + + static constexpr auto value = + is_constructible::value && + CompatibleLimits::is_integer && + RealLimits::is_signed == CompatibleLimits::is_signed; +}; + +template +struct is_compatible_integer_type + : is_compatible_integer_type_impl {}; + +template +struct is_compatible_type_impl: std::false_type {}; + +template +struct is_compatible_type_impl < + BasicJsonType, CompatibleType, + enable_if_t::value >> +{ + static constexpr bool value = + has_to_json::value; +}; + +template +struct is_compatible_type + : is_compatible_type_impl {}; + +template +struct is_constructible_tuple : std::false_type {}; + +template +struct is_constructible_tuple> : conjunction...> {}; + +template +struct is_json_iterator_of : std::false_type {}; + +template +struct is_json_iterator_of : std::true_type {}; + +template +struct is_json_iterator_of : std::true_type +{}; + +// checks if a given type T is a template specialization of Primary +template