diff --git a/src/airy/rslidar_msg-master/CMakeLists.txt b/src/airy/rslidar_msg-master/CMakeLists.txt deleted file mode 100644 index a825404..0000000 --- a/src/airy/rslidar_msg-master/CMakeLists.txt +++ /dev/null @@ -1,41 +0,0 @@ -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 deleted file mode 100644 index 8b1fb13..0000000 --- a/src/airy/rslidar_msg-master/LICENSE +++ /dev/null @@ -1,19 +0,0 @@ -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 deleted file mode 100644 index 2164d00..0000000 --- a/src/airy/rslidar_msg-master/README.md +++ /dev/null @@ -1,5 +0,0 @@ -## 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 deleted file mode 100644 index b6a05f5..0000000 --- a/src/airy/rslidar_msg-master/msg/RslidarPacket.msg +++ /dev/null @@ -1,4 +0,0 @@ -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 deleted file mode 100644 index b8a8309..0000000 --- a/src/airy/rslidar_msg-master/package.xml +++ /dev/null @@ -1,29 +0,0 @@ - - - - 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 deleted file mode 100644 index 320fb43..0000000 --- a/src/airy/rslidar_msg-master/ros1/CMakeLists.txt +++ /dev/null @@ -1,41 +0,0 @@ -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 deleted file mode 100644 index 831a653..0000000 --- a/src/airy/rslidar_msg-master/ros1/package.xml +++ /dev/null @@ -1,22 +0,0 @@ - - - - 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 deleted file mode 100644 index a825404..0000000 --- a/src/airy/rslidar_msg-master/ros2/CMakeLists.txt +++ /dev/null @@ -1,41 +0,0 @@ -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 deleted file mode 100644 index b8a8309..0000000 --- a/src/airy/rslidar_msg-master/ros2/package.xml +++ /dev/null @@ -1,29 +0,0 @@ - - - - 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 deleted file mode 100644 index eca6051..0000000 --- a/src/airy/rslidar_sdk-main/.clang-format +++ /dev/null @@ -1,66 +0,0 @@ ---- -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 deleted file mode 100644 index c4d6e45..0000000 --- a/src/airy/rslidar_sdk-main/CHANGELOG.md +++ /dev/null @@ -1,181 +0,0 @@ -# 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 deleted file mode 100644 index e0d545d..0000000 --- a/src/airy/rslidar_sdk-main/CMakeLists.txt +++ /dev/null @@ -1,210 +0,0 @@ -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 deleted file mode 100644 index 8b1fb13..0000000 --- a/src/airy/rslidar_sdk-main/LICENSE +++ /dev/null @@ -1,19 +0,0 @@ -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 deleted file mode 100644 index ed50e37..0000000 --- a/src/airy/rslidar_sdk-main/README.md +++ /dev/null @@ -1,186 +0,0 @@ -# 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 deleted file mode 100644 index 23b7e71..0000000 --- a/src/airy/rslidar_sdk-main/README_CN.md +++ /dev/null @@ -1,180 +0,0 @@ -# 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 deleted file mode 100644 index a7057d6..0000000 --- a/src/airy/rslidar_sdk-main/config/config.yaml +++ /dev/null @@ -1,50 +0,0 @@ -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 deleted file mode 100644 index 63ed49d..0000000 --- a/src/airy/rslidar_sdk-main/config/config_double.yaml +++ /dev/null @@ -1,92 +0,0 @@ -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 deleted file mode 100644 index 4dab7f4..0000000 --- a/src/airy/rslidar_sdk-main/config/config_front.yaml +++ /dev/null @@ -1,50 +0,0 @@ -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 deleted file mode 100644 index 2142f52..0000000 --- a/src/airy/rslidar_sdk-main/config/config_rear.yaml +++ /dev/null @@ -1,50 +0,0 @@ -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 deleted file mode 100755 index d432b89..0000000 --- a/src/airy/rslidar_sdk-main/create_debian.sh +++ /dev/null @@ -1,47 +0,0 @@ -#! /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 deleted file mode 100644 index 50bf6b6..0000000 --- a/src/airy/rslidar_sdk-main/doc/howto/04_how_to_work_along_with_rslidar_sdk_node_v1.3.x_CN.md +++ /dev/null @@ -1,115 +0,0 @@ -# 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 deleted file mode 100644 index 9de05b0..0000000 --- a/src/airy/rslidar_sdk-main/doc/howto/05_how_to_change_point_type.md +++ /dev/null @@ -1,165 +0,0 @@ -# 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 deleted file mode 100644 index f7c00d7..0000000 --- a/src/airy/rslidar_sdk-main/doc/howto/05_how_to_change_point_type_CN.md +++ /dev/null @@ -1,162 +0,0 @@ -# 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 deleted file mode 100644 index 6403345..0000000 --- a/src/airy/rslidar_sdk-main/doc/howto/06_how_to_decode_online_lidar.md +++ /dev/null @@ -1,70 +0,0 @@ -# 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 deleted file mode 100644 index 7590739..0000000 --- a/src/airy/rslidar_sdk-main/doc/howto/06_how_to_decode_online_lidar_CN.md +++ /dev/null @@ -1,74 +0,0 @@ -# 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 deleted file mode 100644 index d5d529c..0000000 --- a/src/airy/rslidar_sdk-main/doc/howto/07_online_lidar_advanced_topics.md +++ /dev/null @@ -1,200 +0,0 @@ -# 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 deleted file mode 100644 index ea24357..0000000 --- a/src/airy/rslidar_sdk-main/doc/howto/07_online_lidar_advanced_topics_CN.md +++ /dev/null @@ -1,199 +0,0 @@ -# 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 deleted file mode 100644 index 8f3d3d3..0000000 --- a/src/airy/rslidar_sdk-main/doc/howto/08_how_to_decode_pcap_file.md +++ /dev/null @@ -1,79 +0,0 @@ -# 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 deleted file mode 100644 index 2704bf1..0000000 --- a/src/airy/rslidar_sdk-main/doc/howto/08_how_to_decode_pcap_file_CN.md +++ /dev/null @@ -1,74 +0,0 @@ -# 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 deleted file mode 100644 index d17e39f..0000000 --- a/src/airy/rslidar_sdk-main/doc/howto/09_pcap_file_advanced_topics.md +++ /dev/null @@ -1,94 +0,0 @@ -# 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 deleted file mode 100644 index b6d4bf2..0000000 --- a/src/airy/rslidar_sdk-main/doc/howto/09_pcap_file_advanced_topics_CN.md +++ /dev/null @@ -1,92 +0,0 @@ -# 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 deleted file mode 100644 index ce5c5ec..0000000 --- a/src/airy/rslidar_sdk-main/doc/howto/10_how_to_use_coordinate_transformation.md +++ /dev/null @@ -1,80 +0,0 @@ -# 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 deleted file mode 100644 index 6af570e..0000000 --- a/src/airy/rslidar_sdk-main/doc/howto/10_how_to_use_coordinate_transformation_CN.md +++ /dev/null @@ -1,80 +0,0 @@ -# 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 deleted file mode 100644 index 4cd0942..0000000 --- a/src/airy/rslidar_sdk-main/doc/howto/11_how_to_record_replay_packet_rosbag.md +++ /dev/null @@ -1,110 +0,0 @@ -# 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 deleted file mode 100644 index eb69586..0000000 --- a/src/airy/rslidar_sdk-main/doc/howto/11_how_to_record_replay_packet_rosbag_CN.md +++ /dev/null @@ -1,113 +0,0 @@ -# 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 deleted file mode 100644 index b183eaa..0000000 --- a/src/airy/rslidar_sdk-main/doc/howto/12_how_to_create_deb.md +++ /dev/null @@ -1,35 +0,0 @@ -# 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 deleted file mode 100644 index 90d823b..0000000 Binary files a/src/airy/rslidar_sdk-main/doc/howto/img/04_01_packet_rosbag.png and /dev/null 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 deleted file mode 100644 index ebf2f4c..0000000 Binary files a/src/airy/rslidar_sdk-main/doc/howto/img/07_01_broadcast.png and /dev/null 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 deleted file mode 100644 index 65964a2..0000000 Binary files a/src/airy/rslidar_sdk-main/doc/howto/img/07_02_unicast.png and /dev/null 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 deleted file mode 100644 index fc39a8d..0000000 Binary files a/src/airy/rslidar_sdk-main/doc/howto/img/07_03_multicast.png and /dev/null 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 deleted file mode 100644 index c426ca9..0000000 Binary files a/src/airy/rslidar_sdk-main/doc/howto/img/07_04_multi_lidars_port.png and /dev/null 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 deleted file mode 100644 index 48e1e13..0000000 Binary files a/src/airy/rslidar_sdk-main/doc/howto/img/07_05_multi_lidars_ip.png and /dev/null 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 deleted file mode 100644 index 8d678fc..0000000 Binary files a/src/airy/rslidar_sdk-main/doc/howto/img/07_06_vlan_layer.png and /dev/null 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 deleted file mode 100644 index 55f224b..0000000 Binary files a/src/airy/rslidar_sdk-main/doc/howto/img/07_07_vlan.png and /dev/null 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 deleted file mode 100644 index d7b31a0..0000000 Binary files a/src/airy/rslidar_sdk-main/doc/howto/img/07_08_user_layer.png and /dev/null 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 deleted file mode 100644 index dec4886..0000000 --- a/src/airy/rslidar_sdk-main/doc/intro/02_parameter_intro.md +++ /dev/null @@ -1,272 +0,0 @@ -# 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 deleted file mode 100644 index 3242796..0000000 --- a/src/airy/rslidar_sdk-main/doc/intro/02_parameter_intro_CN.md +++ /dev/null @@ -1,276 +0,0 @@ -# 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 deleted file mode 100644 index 7675ccf..0000000 --- a/src/airy/rslidar_sdk-main/doc/intro/03_hiding_parameters_intro.md +++ /dev/null @@ -1,89 +0,0 @@ -# 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 deleted file mode 100644 index d9dfa58..0000000 --- a/src/airy/rslidar_sdk-main/doc/intro/03_hiding_parameters_intro_CN.md +++ /dev/null @@ -1,86 +0,0 @@ -# 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 deleted file mode 100644 index b38f89e..0000000 Binary files a/src/airy/rslidar_sdk-main/doc/src_intro/img/class_destination_packet.png and /dev/null 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 deleted file mode 100644 index 102bf56..0000000 Binary files a/src/airy/rslidar_sdk-main/doc/src_intro/img/class_destination_pointcloud.png and /dev/null 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 deleted file mode 100644 index 2729d40..0000000 Binary files a/src/airy/rslidar_sdk-main/doc/src_intro/img/class_node_manager.png and /dev/null 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 deleted file mode 100644 index 2dd63e5..0000000 Binary files a/src/airy/rslidar_sdk-main/doc/src_intro/img/class_source_destination.png and /dev/null 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 deleted file mode 100644 index d2a71ea..0000000 Binary files a/src/airy/rslidar_sdk-main/doc/src_intro/img/class_source_driver.png and /dev/null 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 deleted file mode 100644 index 49bd2a1..0000000 Binary files a/src/airy/rslidar_sdk-main/doc/src_intro/img/class_source_packet_ros.png and /dev/null 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 deleted file mode 100644 index d34a6b9..0000000 --- a/src/airy/rslidar_sdk-main/doc/src_intro/rslidar_sdk_intro_CN.md +++ /dev/null @@ -1,250 +0,0 @@ -# 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 deleted file mode 100644 index 0ad5469..0000000 Binary files a/src/airy/rslidar_sdk-main/img/01_01_download_page.png and /dev/null differ diff --git a/src/airy/rslidar_sdk-main/launch/elequent_start.py b/src/airy/rslidar_sdk-main/launch/elequent_start.py deleted file mode 100755 index 98cc9e0..0000000 --- a/src/airy/rslidar_sdk-main/launch/elequent_start.py +++ /dev/null @@ -1,21 +0,0 @@ -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 deleted file mode 100755 index 849e3b2..0000000 --- a/src/airy/rslidar_sdk-main/launch/start.launch +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/src/airy/rslidar_sdk-main/launch/start.py b/src/airy/rslidar_sdk-main/launch/start.py deleted file mode 100755 index 95f915f..0000000 --- a/src/airy/rslidar_sdk-main/launch/start.py +++ /dev/null @@ -1,28 +0,0 @@ -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 deleted file mode 100755 index 4ba045b..0000000 --- a/src/airy/rslidar_sdk-main/launch/start_double.launch.py +++ /dev/null @@ -1,61 +0,0 @@ -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 deleted file mode 100644 index 49d543b..0000000 --- a/src/airy/rslidar_sdk-main/node/rslidar_sdk_node.cpp +++ /dev/null @@ -1,136 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index 02ad159..0000000 --- a/src/airy/rslidar_sdk-main/package.xml +++ /dev/null @@ -1,25 +0,0 @@ - - - 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 deleted file mode 100644 index e3b70a8..0000000 --- a/src/airy/rslidar_sdk-main/rviz/rviz.rviz +++ /dev/null @@ -1,164 +0,0 @@ -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 deleted file mode 100644 index 719b7a8..0000000 --- a/src/airy/rslidar_sdk-main/rviz/rviz2.rviz +++ /dev/null @@ -1,184 +0,0 @@ -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 deleted file mode 100644 index 3646fb3..0000000 --- a/src/airy/rslidar_sdk-main/src/manager/node_manager.cpp +++ /dev/null @@ -1,167 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index 39e0bc8..0000000 --- a/src/airy/rslidar_sdk-main/src/manager/node_manager.hpp +++ /dev/null @@ -1,61 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index a912af6..0000000 --- a/src/airy/rslidar_sdk-main/src/msg/ros_msg/RsCompressedImage.msg +++ /dev/null @@ -1,3 +0,0 @@ -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 deleted file mode 100644 index b6a05f5..0000000 --- a/src/airy/rslidar_sdk-main/src/msg/ros_msg/RslidarPacket.msg +++ /dev/null @@ -1,4 +0,0 @@ -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 deleted file mode 100644 index f0a391d..0000000 --- a/src/airy/rslidar_sdk-main/src/msg/ros_msg/rs_compressed_image.hpp +++ /dev/null @@ -1,237 +0,0 @@ -// 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 deleted file mode 100644 index 4a1a290..0000000 --- a/src/airy/rslidar_sdk-main/src/msg/ros_msg/rslidar_packet.hpp +++ /dev/null @@ -1,247 +0,0 @@ -// 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 deleted file mode 100644 index fbd820b..0000000 --- a/src/airy/rslidar_sdk-main/src/msg/ros_msg/rslidar_packet_legacy.hpp +++ /dev/null @@ -1,196 +0,0 @@ -// 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 deleted file mode 100644 index b9837e1..0000000 --- a/src/airy/rslidar_sdk-main/src/msg/ros_msg/rslidar_scan_legacy.hpp +++ /dev/null @@ -1,228 +0,0 @@ -// 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 deleted file mode 100644 index 96b4c8b..0000000 --- a/src/airy/rslidar_sdk-main/src/msg/rs_msg/lidar_point_cloud_msg.hpp +++ /dev/null @@ -1,50 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index 2fc5d9e..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver.zip and /dev/null 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 deleted file mode 100644 index a30530e..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/.clang-format +++ /dev/null @@ -1,66 +0,0 @@ ---- -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 deleted file mode 100644 index ac1c020..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/CHANGELOG.md +++ /dev/null @@ -1,376 +0,0 @@ -# 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 deleted file mode 100644 index 025fce0..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/CMakeLists.txt +++ /dev/null @@ -1,246 +0,0 @@ -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 deleted file mode 100644 index 8b1fb13..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/LICENSE +++ /dev/null @@ -1,19 +0,0 @@ -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 deleted file mode 100644 index e60317b..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/README.md +++ /dev/null @@ -1,260 +0,0 @@ -# 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 deleted file mode 100644 index ef43c78..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/README_CN.md +++ /dev/null @@ -1,268 +0,0 @@ -# 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 deleted file mode 100644 index 3da93a3..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/cmake/FindPCAP.cmake +++ /dev/null @@ -1,86 +0,0 @@ -# - 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 deleted file mode 100644 index ac47a0f..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/cmake/cmake_uninstall.cmake.in +++ /dev/null @@ -1,23 +0,0 @@ -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 deleted file mode 100644 index 9d38c5f..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/cmake/rs_driverConfig.cmake +++ /dev/null @@ -1,29 +0,0 @@ -# - 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 deleted file mode 100644 index 078057c..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/cmake/rs_driverConfig.cmake.in +++ /dev/null @@ -1,29 +0,0 @@ -# - 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 deleted file mode 100644 index abaef6a..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/cmake/rs_driverConfigVersion.cmake +++ /dev/null @@ -1,14 +0,0 @@ -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 deleted file mode 100644 index 4c2ff44..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/cmake/rs_driverConfigVersion.cmake.in +++ /dev/null @@ -1,14 +0,0 @@ -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 deleted file mode 100644 index 6420c2c..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/demo/CMakeLists.txt +++ /dev/null @@ -1,42 +0,0 @@ -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 deleted file mode 100644 index 98a5934..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/demo/demo_online.cpp +++ /dev/null @@ -1,237 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index c8c07fd..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/demo/demo_online_multi_lidars.cpp +++ /dev/null @@ -1,190 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index e1fc56c..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/demo/demo_pcap.cpp +++ /dev/null @@ -1,239 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index 0fda27e..0000000 --- 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 +++ /dev/null @@ -1,236 +0,0 @@ -# 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 deleted file mode 100644 index 50792b6..0000000 --- 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 +++ /dev/null @@ -1,256 +0,0 @@ -# 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 deleted file mode 100644 index d7d8c07..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/08_how_to_decode_online_lidar.md +++ /dev/null @@ -1,348 +0,0 @@ -# 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 deleted file mode 100644 index ae3abaa..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/13_how_to_capture_pcap_file_CN.md +++ /dev/null @@ -1,90 +0,0 @@ -# 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 deleted file mode 100644 index 4ead255..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/14_how_to_use_rs_driver_viewer.md +++ /dev/null @@ -1,113 +0,0 @@ -# 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 deleted file mode 100644 index 192a8a3..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/14_how_to_use_rs_driver_viewer_CN.md +++ /dev/null @@ -1,111 +0,0 @@ -# 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 deleted file mode 100644 index 9aaed8b..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/15_how_to_transform_pointcloud.md +++ /dev/null @@ -1,52 +0,0 @@ -# 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 deleted file mode 100644 index 70b3e9a..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/15_how_to_transform_pointcloud_CN.md +++ /dev/null @@ -1,47 +0,0 @@ -# 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 deleted file mode 100644 index 43a0db2..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/16_how_to_compile_on_windows.md +++ /dev/null @@ -1,332 +0,0 @@ -# 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 deleted file mode 100644 index 5bbf88f..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/16_how_to_compile_on_windows_CN.md +++ /dev/null @@ -1,330 +0,0 @@ -# 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 deleted file mode 100644 index 54bb352..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/17_how_to_avoid_packet_loss.md +++ /dev/null @@ -1,68 +0,0 @@ -# 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 deleted file mode 100644 index 985c17e..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/17_how_to_avoid_packet_loss_CN.md +++ /dev/null @@ -1,68 +0,0 @@ -# **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 deleted file mode 100644 index d3eaa5e..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/18_about_point_layout.md +++ /dev/null @@ -1,187 +0,0 @@ - -# 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 deleted file mode 100644 index 4133dcf..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/18_about_point_layout_CN.md +++ /dev/null @@ -1,184 +0,0 @@ - -# 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 deleted file mode 100644 index 5fab6a2..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/19_about_splitting_frame.md +++ /dev/null @@ -1,121 +0,0 @@ -# 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 deleted file mode 100644 index 0554ad7..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/19_about_splitting_frame_CN.md +++ /dev/null @@ -1,121 +0,0 @@ -# 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 deleted file mode 100644 index b2975ca..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/20_about_usage_of_cpu_and_memory.md +++ /dev/null @@ -1,131 +0,0 @@ -# 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 deleted file mode 100644 index fe3eda1..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/20_about_usage_of_cpu_and_memory_CN.md +++ /dev/null @@ -1,121 +0,0 @@ -# 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 deleted file mode 100644 index 73b0ebc..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/21_how_to_parse_difop.md +++ /dev/null @@ -1,363 +0,0 @@ -# 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 deleted file mode 100644 index 4125a14..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/21_how_to_parse_difop_CN.md +++ /dev/null @@ -1,360 +0,0 @@ -# 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 deleted file mode 100644 index 28ee798..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/07_01_branches.png and /dev/null 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 deleted file mode 100644 index 1101a32..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/07_02_cpu_usage_dev.png and /dev/null 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 deleted file mode 100644 index 6ba35b7..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/07_03_cpu_usage_dev_opt.png and /dev/null 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 deleted file mode 100644 index ebf2f4c..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/09_01_broadcast.png and /dev/null 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 deleted file mode 100644 index 65964a2..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/09_02_unicast.png and /dev/null 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 deleted file mode 100644 index 3134a59..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/09_03_multicast.png and /dev/null 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 deleted file mode 100644 index c426ca9..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/09_04_multi_lidars_port.png and /dev/null 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 deleted file mode 100644 index 48e1e13..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/09_05_multi_lidars_ip.png and /dev/null 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 deleted file mode 100644 index 8d678fc..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/09_06_vlan_layer.png and /dev/null 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 deleted file mode 100644 index 899496d..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/09_07_vlan.png and /dev/null 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 deleted file mode 100644 index d7b31a0..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/09_08_user_layer.png and /dev/null 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 deleted file mode 100644 index c38a07a..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/12_01_select_by_port.png and /dev/null 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 deleted file mode 100644 index 47df223..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/12_02_select_by_non_port.png and /dev/null 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 deleted file mode 100644 index bbebaaf..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/12_03_rs32_msop_packet.png and /dev/null 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 deleted file mode 100644 index db081a7..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/12_04_rs32_difop_packet.png and /dev/null 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 deleted file mode 100644 index 75415c9..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/12_05_with_vlan.png and /dev/null 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 deleted file mode 100644 index dc4e96e..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/12_06_with_user_layer.png and /dev/null 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 deleted file mode 100644 index e927020..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/12_07_with_tail_layer.png and /dev/null 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 deleted file mode 100644 index dad05e8..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/12_08_not_supported.png and /dev/null 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 deleted file mode 100644 index 67996d0..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/13_01_wireshark_select_nic.png and /dev/null 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 deleted file mode 100644 index cf02395..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/13_02_wireshark_capture.png and /dev/null 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 deleted file mode 100644 index c5e9e2c..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/13_03_wireshark_filter_by_port.png and /dev/null 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 deleted file mode 100644 index 50f89c5..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/13_04_wireshark_export_all.png and /dev/null 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 deleted file mode 100644 index 5aaa1f6..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/13_05_wireshark_export_range.png and /dev/null 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 deleted file mode 100644 index 6e60dab..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/13_06_wireshark_mark.png and /dev/null 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 deleted file mode 100644 index 42bfd28..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/13_07_wireshark_export_marked.png and /dev/null 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 deleted file mode 100644 index 651a52d..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/13_08_wireshark_export_marked_range.png and /dev/null 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 deleted file mode 100644 index dff4b05..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/14_01_rs_driver_viewer_point_cloud.png and /dev/null 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 deleted file mode 100644 index 37e3ce6..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_01_demo_project.png and /dev/null 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 deleted file mode 100644 index 3db993e..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_02_demo_use_cpp14.png and /dev/null 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 deleted file mode 100644 index 7d41659..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_03_demo_extra_include.png and /dev/null 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 deleted file mode 100644 index 2195631..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_04_demo_include_path.png and /dev/null 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 deleted file mode 100644 index 5e877c4..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_05_demo_lib_path.png and /dev/null 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 deleted file mode 100644 index d596373..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_06_demo_lib.png and /dev/null 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 deleted file mode 100644 index 6a52cb0..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_07_demo_precompile_macro.png and /dev/null 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 deleted file mode 100644 index 85882b3..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_08_viewer_install_pcl.png and /dev/null 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 deleted file mode 100644 index 1f0ec96..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_09_viewer_install_pcl_dep.png and /dev/null 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 deleted file mode 100644 index 826fa4e..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_10_viewer_add_env_var.png and /dev/null 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 deleted file mode 100644 index 454b98b..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_11_viewer_project.png and /dev/null 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 deleted file mode 100644 index 5f83fc8..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_12_viewer_sdl_check.png and /dev/null 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 deleted file mode 100644 index ced43ef..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_13_viewer_include_path.png and /dev/null 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 deleted file mode 100644 index ed0031b..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_14_viewer_lib_path.png and /dev/null 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 deleted file mode 100644 index 316f8c2..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_15_viewer_lib.png and /dev/null 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 deleted file mode 100644 index 1d78ece..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/16_16_viewer_precompile_macro.png and /dev/null 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 deleted file mode 100644 index e3e13d7..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/18_01_mech_lasers.png and /dev/null 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 deleted file mode 100644 index 6602f74..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/18_02_mems_lasers.png and /dev/null 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 deleted file mode 100644 index bb21578..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/18_03_mech_lasers_and_points.png and /dev/null 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 deleted file mode 100644 index 3660963..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/18_04_mech_points.png and /dev/null 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 deleted file mode 100644 index f9f4f00..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/18_05_mems_lasers_and_points.png and /dev/null 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 deleted file mode 100644 index 0f9c368..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/18_06_mems_points.png and /dev/null 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 deleted file mode 100644 index ad69e3c..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/19_01_split_angle.png and /dev/null 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 deleted file mode 100644 index 8950f21..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/19_02_safe_range.png and /dev/null 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 deleted file mode 100644 index 1f088f6..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/howto/img/20_01_components_and_threads.png and /dev/null 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 deleted file mode 100644 index b831935..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/doc/intro/02_directories_and_files.md +++ /dev/null @@ -1,44 +0,0 @@ -# 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 deleted file mode 100644 index 4634772..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/doc/intro/02_directories_and_files_CN.md +++ /dev/null @@ -1,44 +0,0 @@ -# 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 deleted file mode 100644 index 415d50c..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/doc/intro/03_thread_model.md +++ /dev/null @@ -1,64 +0,0 @@ -# 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 deleted file mode 100644 index f778c73..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/doc/intro/03_thread_model_CN.md +++ /dev/null @@ -1,69 +0,0 @@ -# 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 deleted file mode 100644 index fbca7f0..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/doc/intro/04_parameter_intro.md +++ /dev/null @@ -1,242 +0,0 @@ -# 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 deleted file mode 100644 index 367b5c9..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/doc/intro/04_parameter_intro_CN.md +++ /dev/null @@ -1,238 +0,0 @@ -# 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 deleted file mode 100644 index 38857c4..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/doc/intro/05_cmake_macros_intro.md +++ /dev/null @@ -1,166 +0,0 @@ -# 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 deleted file mode 100644 index 9cc87ba..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/doc/intro/05_cmake_macros_intro_CN.md +++ /dev/null @@ -1,168 +0,0 @@ -# 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 deleted file mode 100644 index b5b445b..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/doc/intro/06_error_code_intro.md +++ /dev/null @@ -1,115 +0,0 @@ -# 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 deleted file mode 100644 index 313c5b8..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/doc/intro/06_error_code_intro_CN.md +++ /dev/null @@ -1,110 +0,0 @@ -# 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 deleted file mode 100644 index 1f088f6..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/intro/img/03_01_components_and_threads.png and /dev/null 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 deleted file mode 100644 index 369e864..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/intro/img/03_02_interface_with_threads.png and /dev/null 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 deleted file mode 100644 index 6d48737..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/.xdp_01_components.QEQTH1 and /dev/null 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 deleted file mode 100644 index c50b627..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/.xdp_components.8OXVH1 and /dev/null 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 deleted file mode 100644 index 9334b3f..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_azimuth_section.png and /dev/null 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 deleted file mode 100644 index e965bd9..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_chan_angles.png and /dev/null 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 deleted file mode 100644 index a2c723e..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_decoder.png and /dev/null 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 deleted file mode 100644 index 4c9d83b..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_decoder_factory.png and /dev/null 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 deleted file mode 100644 index fe543cd..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_decoder_mech.png and /dev/null 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 deleted file mode 100644 index 257b3ec..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_decoder_rsbp.png and /dev/null 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 deleted file mode 100644 index c63a853..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_decoder_rsm1.png and /dev/null 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 deleted file mode 100644 index 35f2624..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_distance_section.png and /dev/null 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 deleted file mode 100644 index 42e3665..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_input.png and /dev/null 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 deleted file mode 100644 index 53af18a..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_input_factory.png and /dev/null 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 deleted file mode 100644 index 808935a..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_input_pcap.png and /dev/null 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 deleted file mode 100644 index 2a2a2ae..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_input_raw.png and /dev/null 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 deleted file mode 100644 index 2671c23..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_input_sock.png and /dev/null 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 deleted file mode 100644 index d6b79b7..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_lidar_driver_impl.png and /dev/null 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 deleted file mode 100644 index 99b6972..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/class_trigon.png and /dev/null 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 deleted file mode 100644 index 2dec796..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/classes_block_iterator.png and /dev/null 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 deleted file mode 100644 index 228f9bf..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/classes_decoder.png and /dev/null 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 deleted file mode 100644 index 89be452..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/classes_input.png and /dev/null 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 deleted file mode 100644 index 61a4f18..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/classes_rs16_block_iterator.png and /dev/null 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 deleted file mode 100644 index d83939f..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/classes_split_strategy.png and /dev/null 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 deleted file mode 100644 index 89c76dc..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/classes_split_strategy_by_seq.png and /dev/null 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 deleted file mode 100644 index f71cd3b..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/components.png and /dev/null 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 deleted file mode 100644 index 693a49e..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/fov.png and /dev/null 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 deleted file mode 100644 index 17688a5..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/msop_dual_return.png and /dev/null 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 deleted file mode 100644 index 37aceae..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/msop_ruby128.png and /dev/null 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 deleted file mode 100644 index 036fbb5..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/msop_single_return.png and /dev/null 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 deleted file mode 100644 index acc512f..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/packet_layers.png and /dev/null 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 deleted file mode 100644 index 6ff501b..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/packet_layers_full.png and /dev/null 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 deleted file mode 100644 index 726cd39..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/packet_record_replay.png and /dev/null 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 deleted file mode 100644 index 36ef680..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/rs16_msop_dual_return.png and /dev/null 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 deleted file mode 100644 index 152dae0..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/rs16_msop_single_return.png and /dev/null 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 deleted file mode 100644 index 8950f21..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/safe_range.png and /dev/null 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 deleted file mode 100644 index ad69e3c..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/split_angle.png and /dev/null 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 deleted file mode 100644 index 85458cd..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/split_position.png and /dev/null 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 deleted file mode 100644 index 382f3cc..0000000 Binary files a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/img/trigon_sinss.png and /dev/null 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 deleted file mode 100644 index fb69360..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/doc/src_intro/rs_driver_intro_CN.md +++ /dev/null @@ -1,1557 +0,0 @@ -# **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 deleted file mode 100644 index 47cfe5c..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/api/lidar_driver.hpp +++ /dev/null @@ -1,173 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index c4c97ec..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/common/error_code.hpp +++ /dev/null @@ -1,186 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index 8fb2bef..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/common/rs_common.hpp +++ /dev/null @@ -1,92 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index 94087e5..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/common/rs_log.hpp +++ /dev/null @@ -1,46 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index aa53be7..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/basic_attr.hpp +++ /dev/null @@ -1,426 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index 6662517..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/block_iterator.hpp +++ /dev/null @@ -1,346 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index 6f2e882..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/chan_angles.hpp +++ /dev/null @@ -1,238 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index 52676a5..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder.hpp +++ /dev/null @@ -1,533 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index bb8f4c1..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RS128.hpp +++ /dev/null @@ -1,338 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index 3e81b42..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RS16.hpp +++ /dev/null @@ -1,374 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index 5c3a471..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RS32.hpp +++ /dev/null @@ -1,355 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index c1afb9a..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RS48.hpp +++ /dev/null @@ -1,295 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index 22b53a2..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RS80.hpp +++ /dev/null @@ -1,331 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index c7b28d9..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSAIRY.hpp +++ /dev/null @@ -1,540 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index 9b74b51..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSBP.hpp +++ /dev/null @@ -1,374 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index fae81bc..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSE1.hpp +++ /dev/null @@ -1,294 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index 3b17b31..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSHELIOS.hpp +++ /dev/null @@ -1,338 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index 2cea047..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSHELIOS_16P.hpp +++ /dev/null @@ -1,306 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index 8190fd7..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSM1.hpp +++ /dev/null @@ -1,317 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index 52cf6e2..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSM1_Jumbo.hpp +++ /dev/null @@ -1,262 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index 17417a3..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSM2.hpp +++ /dev/null @@ -1,264 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index 1088321..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSM3.hpp +++ /dev/null @@ -1,233 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index 0b5a15b..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSMX.hpp +++ /dev/null @@ -1,311 +0,0 @@ -/********************************************************************************************************************* - -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 deleted file mode 100644 index bef3c54..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSP128.hpp +++ /dev/null @@ -1,338 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index 983c26d..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSP48.hpp +++ /dev/null @@ -1,341 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index ac82169..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_RSP80.hpp +++ /dev/null @@ -1,372 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index a34fe03..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_factory.hpp +++ /dev/null @@ -1,140 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index 65f09cb..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/decoder_mech.hpp +++ /dev/null @@ -1,219 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index 4f9591c..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/member_checker.hpp +++ /dev/null @@ -1,139 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index 969d481..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/section.hpp +++ /dev/null @@ -1,113 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index 43ff2de..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/split_strategy.hpp +++ /dev/null @@ -1,184 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index b2d73a9..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/decoder/trigon.hpp +++ /dev/null @@ -1,133 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index 48f01b6..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/driver_param.hpp +++ /dev/null @@ -1,409 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index 10b6e0e..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/input/input_factory.hpp +++ /dev/null @@ -1,117 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index ba9ff7c..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/input/input_pcap.hpp +++ /dev/null @@ -1,283 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index b1d0b67..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/input/input_pcap_jumbo.hpp +++ /dev/null @@ -1,198 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index 201e02c..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/input/input_raw.hpp +++ /dev/null @@ -1,77 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index 1c9ca0f..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/input/input_raw_jumbo.hpp +++ /dev/null @@ -1,53 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index a01dd60..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/input/input_sock.hpp +++ /dev/null @@ -1,17 +0,0 @@ - -#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 deleted file mode 100644 index cc197fa..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/input/input_sock_jumbo.hpp +++ /dev/null @@ -1,23 +0,0 @@ - -#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 deleted file mode 100644 index a25f76a..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/input/jumbo.hpp +++ /dev/null @@ -1,190 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index b0ac884..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/input/unix/input_sock_epoll.hpp +++ /dev/null @@ -1,300 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index 3a73f8e..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/input/unix/input_sock_select.hpp +++ /dev/null @@ -1,321 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index 1d39d65..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/input/win/input_sock_select.hpp +++ /dev/null @@ -1,317 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index 2fcb56c..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/driver/lidar_driver_impl.hpp +++ /dev/null @@ -1,498 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index 160a1b2..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/macro/version.hpp +++ /dev/null @@ -1,3 +0,0 @@ -#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 deleted file mode 100644 index d54a6ba..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/macro/version.hpp.in +++ /dev/null @@ -1,3 +0,0 @@ -#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 deleted file mode 100644 index 9441b0f..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/msg/imu_data_msg.hpp +++ /dev/null @@ -1,118 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index 869237b..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/msg/packet.hpp +++ /dev/null @@ -1,66 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index 86d76ac..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/msg/pcl_point_cloud_msg.hpp +++ /dev/null @@ -1,68 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index 895b2ef..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/msg/point_cloud_msg.hpp +++ /dev/null @@ -1,80 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index 32eb0a0..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/utility/buffer.hpp +++ /dev/null @@ -1,89 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index 4e6e3fb..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/utility/dbg.hpp +++ /dev/null @@ -1,56 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index 378609b..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/src/rs_driver/utility/sync_queue.hpp +++ /dev/null @@ -1,149 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index 75e53e1..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/test/CMakeLists.txt +++ /dev/null @@ -1,36 +0,0 @@ - -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 deleted file mode 100644 index fdde7e0..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/test/ab_dual_return_block_iterator_test.cpp +++ /dev/null @@ -1,136 +0,0 @@ - -#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 deleted file mode 100644 index 7b157b2..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/test/basic_attr_test.cpp +++ /dev/null @@ -1,76 +0,0 @@ - -#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 deleted file mode 100644 index e69bdd1..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/test/buffer_test.cpp +++ /dev/null @@ -1,22 +0,0 @@ - -#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 deleted file mode 100644 index 19233db..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/test/chan_angles_test.cpp +++ /dev/null @@ -1,223 +0,0 @@ - -#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 deleted file mode 100644 index d9e122d..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/test/dbg_test.cpp +++ /dev/null @@ -1,13 +0,0 @@ -#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 deleted file mode 100644 index b116032..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/test/decoder_rs16_test.cpp +++ /dev/null @@ -1,56 +0,0 @@ - -#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 deleted file mode 100644 index 7948361..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/test/decoder_rs32_test.cpp +++ /dev/null @@ -1,213 +0,0 @@ - -#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 deleted file mode 100644 index 1086fb9..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/test/decoder_rsbp_test.cpp +++ /dev/null @@ -1,175 +0,0 @@ - -#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 deleted file mode 100644 index e2ef49d..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/test/decoder_test.cpp +++ /dev/null @@ -1,296 +0,0 @@ - -#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 deleted file mode 100644 index 14223d8..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/test/dual_return_block_iterator_test.cpp +++ /dev/null @@ -1,124 +0,0 @@ - -#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 deleted file mode 100644 index 0c02c83..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/test/res/angle.csv +++ /dev/null @@ -1,4 +0,0 @@ -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 deleted file mode 100644 index 29cd933..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/test/rs16_dual_return_block_iterator_test.cpp +++ /dev/null @@ -1,92 +0,0 @@ - -#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 deleted file mode 100644 index 0174c79..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/test/rs16_single_return_block_iterator_test.cpp +++ /dev/null @@ -1,93 +0,0 @@ - -#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 deleted file mode 100644 index a7b01be..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/test/rs_common_test.cpp +++ /dev/null @@ -1,53 +0,0 @@ -#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 deleted file mode 100644 index 9efd629..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/test/rs_driver_test.cpp +++ /dev/null @@ -1,8 +0,0 @@ - -#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 deleted file mode 100644 index 8c3c189..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/test/section_test.cpp +++ /dev/null @@ -1,86 +0,0 @@ - -#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 deleted file mode 100644 index e389990..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/test/single_return_block_iterator_test.cpp +++ /dev/null @@ -1,89 +0,0 @@ - -#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 deleted file mode 100644 index cc29064..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/test/split_strategy_test.cpp +++ /dev/null @@ -1,137 +0,0 @@ - -#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 deleted file mode 100644 index f31b803..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/test/sync_queue_test.cpp +++ /dev/null @@ -1,59 +0,0 @@ - -#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 deleted file mode 100644 index 63aa30f..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/test/trigon_test.cpp +++ /dev/null @@ -1,32 +0,0 @@ - -#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 deleted file mode 100644 index 8744eab..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/tool/CMakeLists.txt +++ /dev/null @@ -1,64 +0,0 @@ - -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 deleted file mode 100644 index 50a68f9..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/tool/rs_driver_pcdsaver.cpp +++ /dev/null @@ -1,284 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index 75bac92..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/tool/rs_driver_viewer.cpp +++ /dev/null @@ -1,286 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index 1e17ae8..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/win/demo_online/demo_online.vcxproj +++ /dev/null @@ -1,144 +0,0 @@ - - - - - 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 deleted file mode 100644 index 2a7052e..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/win/demo_pcap/demo_pcap.vcxproj +++ /dev/null @@ -1,141 +0,0 @@ - - - - - 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 deleted file mode 100644 index c9d9963..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/win/rs_driver.sln +++ /dev/null @@ -1,51 +0,0 @@ - -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 deleted file mode 100644 index 5de8a52..0000000 --- a/src/airy/rslidar_sdk-main/src/rs_driver/win/rs_driver_viewer/rs_driver_viewer.vcxproj +++ /dev/null @@ -1,155 +0,0 @@ - - - - - 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 deleted file mode 100644 index 1a9eff4..0000000 --- a/src/airy/rslidar_sdk-main/src/source/source.hpp +++ /dev/null @@ -1,146 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index 9b42c47..0000000 --- a/src/airy/rslidar_sdk-main/src/source/source_driver.hpp +++ /dev/null @@ -1,283 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index 6bebc9a..0000000 --- a/src/airy/rslidar_sdk-main/src/source/source_packet_ros.hpp +++ /dev/null @@ -1,350 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index 06ccf3a..0000000 --- a/src/airy/rslidar_sdk-main/src/source/source_pointcloud_ros.hpp +++ /dev/null @@ -1,501 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index fa96c83..0000000 --- a/src/airy/rslidar_sdk-main/src/utility/common.hpp +++ /dev/null @@ -1,35 +0,0 @@ -/********************************************************************************************************************* -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 deleted file mode 100644 index 4c34609..0000000 --- a/src/airy/rslidar_sdk-main/src/utility/yaml_reader.hpp +++ /dev/null @@ -1,86 +0,0 @@ -/********************************************************************************************************************* -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/mc/include/mc/can_struct.h b/src/mc/include/mc/can_struct.h index 197254d..70288d6 100644 --- a/src/mc/include/mc/can_struct.h +++ b/src/mc/include/mc/can_struct.h @@ -12,27 +12,29 @@ union can_MCU_cmd_union { struct { - // Byte 0-1 - uint8_t reserve1; - uint8_t reserve2; + // Byte 0: 设定转速低字节 + uint8_t speed_low; - // Byte 2 - uint8_t rpm; + // Byte 1: 设定转速高字节 + uint8_t speed_high; - // Byte 3 - uint8_t gear; + // Byte 2: CAN使能信号 (1使能，其他值不使能) + uint8_t can_enabled; - // Byte 4 - uint8_t work_enabled; + // Byte 3: 开关信号状态 + struct + { + uint8_t cmd_status : 2; // BIT0-1: 命令状态(0空挡,1前进,2后退,3保留) + uint8_t brake : 1; // BIT2: 刹车开关(1=开) + uint8_t reserved_bit3 : 1; // BIT3: 保留 + uint8_t reserved_bits47 : 4; // BIT4-7: 保留 + } switch_status; - // Byte 5 - uint8_t brake; - - // Byte 6 - uint8_t reserve3; - - // Byte 7 - uint8_t ctrl_enabled; + // Byte 4-7: 保留 + uint8_t reserve4; + uint8_t reserve5; + uint8_t reserve6; + uint8_t reserve7; } fields; uint8_t bytes[8]; @@ -57,55 +59,50 @@ union can_EPS_cmd_union #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; + // 根据报文信息ID (0x0CF1011E，其中01部分为设备ID) + static constexpr uint32_t CMD_ID = 0x0CF1011E; 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; + // 初始化默认值 + data.fields.can_enabled = 0; // 默认不使能 + data.fields.switch_status.cmd_status = 0; // 默认空挡 + data.fields.switch_status.brake = 1; // 默认刹车 } + // 设置使能状态 void setEnabled(bool en) { - data.fields.work_enabled = en ? 1 : 0; + data.fields.can_enabled = en ? 1 : 0; } + // 设置档位 (0空挡,1前进,2后退,3保留) void setGear(uint8_t gear) { - data.fields.gear = gear & 0x01; + data.fields.switch_status.cmd_status = gear & 0x03; // 只保留低2位 } - void setBrake(uint8_t brake) + // 设置刹车状态 + void setBrake(bool brake_on) { - data.fields.brake = brake & 0x01; + data.fields.switch_status.brake = brake_on ? 1 : 0; } - void setRPM(uint8_t rpm) + // 设置转速 (0-6000rpm) + void setRPM(uint16_t rpm) { - data.fields.rpm = std::clamp(rpm, static_cast(0), static_cast(100)); + // 限制转速范围0-6000 + uint16_t clamped_rpm = std::clamp(rpm, static_cast(0), static_cast(6000)); + data.fields.speed_low = clamped_rpm & 0xFF; // 低字节 + data.fields.speed_high = (clamped_rpm >> 8) & 0xFF; // 高字节 } CANFrame toFrame() const @@ -347,53 +344,10 @@ struct can_VCU_motor2_cmd } }; -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_enable_cmd vcu_enable_cmd; extern can_VCU_motor1_cmd vcu_motor1_cmd; extern can_VCU_motor2_cmd vcu_motor2_cmd; -extern can_DBS_cmd dbs_cmd; #endif diff --git a/src/mc/include/mc/vcu_wakeup.hpp b/src/mc/include/mc/vcu_wakeup.hpp deleted file mode 100644 index d5a5f12..0000000 --- a/src/mc/include/mc/vcu_wakeup.hpp +++ /dev/null @@ -1,4 +0,0 @@ -#pragma once -#include "mc/can_struct.h" - -void VCUWakeUp(CANDriver &driver); // 发送唤醒帧 diff --git a/src/mc/src/can_struct.cpp b/src/mc/src/can_struct.cpp index 9d03410..4a29203 100644 --- a/src/mc/src/can_struct.cpp +++ b/src/mc/src/can_struct.cpp @@ -4,5 +4,4 @@ can_MCU_cmd mcu_cmd; can_EPS_cmd eps_cmd; can_VCU_enable_cmd vcu_enable_cmd; can_VCU_motor1_cmd vcu_motor1_cmd; -can_VCU_motor2_cmd vcu_motor2_cmd; -can_DBS_cmd dbs_cmd; +can_VCU_motor2_cmd vcu_motor2_cmd; \ No newline at end of file diff --git a/src/mc/src/can_utils.cpp b/src/mc/src/can_utils.cpp index 30e9179..12ff860 100644 --- a/src/mc/src/can_utils.cpp +++ b/src/mc/src/can_utils.cpp @@ -3,9 +3,6 @@ #include bool g_can_print_enable = false; -extern std::atomic vcu_msg_received; -extern std::atomic vcu_awake; -extern rclcpp::Time last_vcu_msg_time; void receiveHandler(const CANFrame &frame, void *userData) { @@ -29,11 +26,4 @@ void receiveHandler(const CANFrame &frame, void *userData) ss << std::setw(2) << std::setfill('0') << std::hex << (int)frame.data[i] << " "; RCLCPP_INFO(node->get_logger(), "%s", ss.str().c_str()); } - - if (frame.id == 0x18FA0121) - { - last_vcu_msg_time = now; - vcu_msg_received.store(true); - vcu_awake.store(true); - } -} +} \ No newline at end of file diff --git a/src/mc/src/mc.cpp b/src/mc/src/mc.cpp index 15e8cc2..0832f7f 100644 --- a/src/mc/src/mc.cpp +++ b/src/mc/src/mc.cpp @@ -4,7 +4,6 @@ #include "mc/can_utils.hpp" #include "mc/control_cache.hpp" #include "mc/timer_tasks.hpp" -#include "mc/vcu_wakeup.hpp" #include "sweeper_interfaces/msg/can_frame.hpp" #include "sweeper_interfaces/msg/mc_ctrl.hpp" @@ -13,9 +12,6 @@ namespace sweeperMsg = sweeper_interfaces::msg; CANDriver canctl; -std::atomic vcu_msg_received = false; -std::atomic vcu_awake = false; -rclcpp::Time last_vcu_msg_time; void mcCtrlCallback(const sweeperMsg::McCtrl::SharedPtr msg) { @@ -23,8 +19,6 @@ void mcCtrlCallback(const sweeperMsg::McCtrl::SharedPtr msg) << "\n 电磁刹: " << static_cast(msg->brake) << "\n 挡位: " << static_cast(msg->gear) << "\n 转速: " << static_cast(msg->rpm) - << "\n ehb使能: " << static_cast(msg->ehb_anable) - << "\n ehb制动压力: " << static_cast(msg->ehb_brake_pressure) << "\n 轮端转向角度: " << msg->angle << "\n 转向角速度: " << msg->angle_speed << "\n 滚刷&吸风电机方向: " << static_cast(msg->roll_brush_suction_direction) @@ -47,7 +41,6 @@ int main(int argc, char **argv) auto pub = node->create_publisher("can_data", 10); auto sub = node->create_subscription("mc_ctrl", 10, mcCtrlCallback); - last_vcu_msg_time = rclcpp::Time(0, 0, node->get_clock()->get_clock_type()); Config mc_config; load_config(mc_config); diff --git a/src/mc/src/timer_tasks.cpp b/src/mc/src/timer_tasks.cpp index a902345..0859741 100644 --- a/src/mc/src/timer_tasks.cpp +++ b/src/mc/src/timer_tasks.cpp @@ -1,48 +1,7 @@ #include "mc/timer_tasks.hpp" #include "mc/control_cache.hpp" -#include "mc/vcu_wakeup.hpp" #include "mc/can_struct.h" -extern std::atomic vcu_awake; -extern std::atomic vcu_msg_received; -extern rclcpp::Time last_vcu_msg_time; - -// 定时器回调：VCU 唤醒 -void wakeupTimerTask(const rclcpp::Node::SharedPtr &node, CANDriver &canctl) -{ - if (!vcu_awake.load()) - { - RCLCPP_INFO(node->get_logger(), "[TIMER][VCU] Not awake, sending wake-up frame..."); - VCUWakeUp(canctl); - } -} - -// 定时器回调：VCU watchdog -void vcuWatchdogTask(const rclcpp::Node::SharedPtr &node) -{ - auto now = node->now(); - if (vcu_msg_received.load() && (now - last_vcu_msg_time > rclcpp::Duration::from_seconds(0.5))) - { - RCLCPP_WARN(node->get_logger(), "[VCU] Timeout, resetting state."); - vcu_awake.store(false); - vcu_msg_received.store(false); - } - - // if (vcu_awake.load()) - // { - // auto msg = get_safe_control(); - // } -} - -// 定时器回调：DBS 控制 -void dbsTimerTask(CANDriver &canctl) -{ - auto msg = get_safe_control(); - dbs_cmd.setEnabled(msg.ehb_anable); - dbs_cmd.setPressure(msg.ehb_brake_pressure); - canctl.sendFrame(dbs_cmd.toFrame()); -} - // 定时器回调：MCU 控制 void mcuTimerTask(CANDriver &canctl) { @@ -65,7 +24,7 @@ void epsTimerTask(CANDriver &canctl) canctl.sendFrame(eps_cmd.toFrame()); } -// 定时器回调：VCU 扫地控制（使用新的报文结构） +// 定时器回调：VCU 扫地控制 void vcuTimerTask(CANDriver &canctl) { static bool vcu_initialized = false; @@ -137,18 +96,8 @@ void vcuTimerTask(CANDriver &canctl) // 注册所有定时器任务 void setupTimers(rclcpp::Node::SharedPtr node, CANDriver &canctl) { - static auto timer_wakeup = node->create_wall_timer( - std::chrono::seconds(1), - [node, &canctl]() - { wakeupTimerTask(node, canctl); }); - - static auto timer_dbs = node->create_wall_timer( - std::chrono::milliseconds(10), - [&canctl]() - { dbsTimerTask(canctl); }); - static auto timer_mcu = node->create_wall_timer( - std::chrono::milliseconds(20), + std::chrono::milliseconds(50), [&canctl]() { mcuTimerTask(canctl); }); @@ -162,10 +111,5 @@ void setupTimers(rclcpp::Node::SharedPtr node, CANDriver &canctl) [&canctl]() { vcuTimerTask(canctl); }); - static auto timer_vcu_watchdog = node->create_wall_timer( - std::chrono::milliseconds(200), - [node]() - { vcuWatchdogTask(node); }); - RCLCPP_INFO(node->get_logger(), "[TIMER] All timers setup completed"); } \ No newline at end of file diff --git a/src/mc/src/vcu_wakeup.cpp b/src/mc/src/vcu_wakeup.cpp deleted file mode 100644 index f48a33d..0000000 --- a/src/mc/src/vcu_wakeup.cpp +++ /dev/null @@ -1,21 +0,0 @@ -#include "mc/vcu_wakeup.hpp" -#include - -void VCUWakeUp(CANDriver &driver) -{ - CANFrame frame; - frame.id = 0x18FF8017; - frame.dlc = 8; - frame.ext = true; - frame.rtr = false; - - frame.data[0] = 0x55; - frame.data[1] = 0x21; - std::fill(frame.data + 2, frame.data + 8, 0xFF); - driver.sendFrame(frame); - - std::this_thread::sleep_for(std::chrono::milliseconds(50)); - - frame.data[0] = 0xAA; - driver.sendFrame(frame); -} diff --git a/src/mqtt_report/include/mqtt_report/fault_codes.h b/src/mqtt_report/include/mqtt_report/fault_codes.h index 7f53e4f..c5e53fb 100644 --- a/src/mqtt_report/include/mqtt_report/fault_codes.h +++ b/src/mqtt_report/include/mqtt_report/fault_codes.h @@ -58,7 +58,6 @@ private: // 故障字典声明（在 .cpp 实现里定义） extern std::unordered_map faultMap; -extern const std::map mcuErrorMap; extern const uint8_t epsErrorCode[18]; extern ErrorCodeSet vehicle_error_code; diff --git a/src/mqtt_report/include/mqtt_report/report_struct.h b/src/mqtt_report/include/mqtt_report/report_struct.h index 01416b6..2f7b0e7 100644 --- a/src/mqtt_report/include/mqtt_report/report_struct.h +++ b/src/mqtt_report/include/mqtt_report/report_struct.h @@ -15,10 +15,9 @@ struct GeneralMsg { float power = 0.0f; // 电量 % bool chargeStatus = false; // 是否在充电 - int gear = -1; // 档位 + int gear = 0; // 档位 float speed = 0.0f; // 当前速度 km/h - float steeringAngle = 0.0f; // 当前速度 km/h - int waterLevel = 0; // 水位 + float steeringAngle = 0.0f; // 当前角度 int motorTemp = 0; // 电机温度 °C int controllerTemp = 0; // 控制器温度 °C int64_t timestamp = 0; // 时间戳 diff --git a/src/mqtt_report/src/fault_codes.cpp b/src/mqtt_report/src/fault_codes.cpp index 8308821..8f3c15d 100644 --- a/src/mqtt_report/src/fault_codes.cpp +++ b/src/mqtt_report/src/fault_codes.cpp @@ -37,21 +37,6 @@ bool updateFaultLevel(int code, int newLevel) return true; } -// MCU 错误码映射表：原始值（msg->data[7]）→ 故障码（1101 ~ 1111） -const std::map mcuErrorMap = { - {1, 1101}, // 过压 - {2, 1102}, // 欠压 - {4, 1103}, // 电机过温 - {5, 1104}, // 母线过流 - {6, 1105}, // 挡位故障 - {7, 1106}, // EEPROM故障 - {8, 1107}, // 相电流瞬时过流 - {9, 1108}, // 霍尔故障 - {10, 1109}, // CAN故障 - {12, 1110}, // 控制器过温 - {13, 1111} // 油门故障 -}; - const uint8_t epsErrorCode[18] = {0x10, 0x14, 0x12, 0x21, 0x22, 0x23, 0x24, 0x25, 0x32, 0x33, 0x35, 0x37, 0x43, 0x46, 0x51, 0x55, 0x61, 0x62}; std::unordered_map faultMap = { @@ -84,17 +69,18 @@ std::unordered_map faultMap = { {1027, {"BMS", "BMS_CHAR_STATUS_ERR", "充电枪状态", 0}}, {1028, {"BMS", "BMS_HALL_ZERO_ERR", "霍尔零点故障", 0}}, - {1101, {"MCU", "MCU_OVER_VOL_ERR", "过压", 0}}, - {1102, {"MCU", "MCU_UNDER_VOL_ERR", "欠压", 0}}, - {1103, {"MCU", "MCU_MOTOR_OVER_TEMP_ERR", "电机过温", 0}}, - {1104, {"MCU", "MCU_DC_BUS_OVER_CUR_ERR", "母线过流", 0}}, - {1105, {"MCU", "MCU_GEAR_ERR", "挡位故障", 0}}, - {1106, {"MCU", "MCU_EEPROM_ERR", "EEPROM故障", 0}}, - {1107, {"MCU", "MCU_PHASE_CUR_SPIKE_ERR", "相电流瞬时过流", 0}}, - {1108, {"MCU", "MCU_HALL_SIGNAL_ERR", "霍尔故障", 0}}, - {1109, {"MCU", "MCU_CAN_ERR", "CAN故障", 0}}, - {1110, {"MCU", "MCU_CONT_OVER_TEMP_ERR", "控制器过温", 0}}, - {1111, {"MCU", "MCU_ACCEL_ERR", "油门故障", 0}}, + {1101, {"MCU", "MCU_ANGLE_SENSOR_ERR", "控制器辨识角度传感器失败", 0}}, + {1102, {"MCU", "MCU_OVER_VOL_ERR", "过压", 0}}, + {1103, {"MCU", "MCU_UNDER_VOL_ERR", "低压", 0}}, + {1104, {"MCU", "MCU_STALL_ERR", "堵转", 0}}, + {1105, {"MCU", "MCU_VOLTAGE_ERR", "电压错误", 0}}, + {1106, {"MCU", "MCU_OVER_TEMP_ERR", "控制器过温错误", 0}}, + {1107, {"MCU", "MCU_PEDAL_START_ERR", "启动时踏板错误", 0}}, + {1108, {"MCU", "MCU_RESET_ERR", "控制器内部进行了一次复位", 0}}, + {1109, {"MCU", "MCU_THROTTLE_ERR", "控制器启动时或者运行过程中霍尔式油门踏板短路或断路", 0}}, + {1110, {"MCU", "MCU_ANGLE_SENSOR_ERR", "角度传感器故障", 0}}, + {1111, {"MCU", "MCU_MOTOR_OVER_TEMP_ERR", "电机温度过温", 0}}, + {1112, {"MCU", "MCU_CUR_SENSOR_ERR", "电流传感器故障", 0}}, {1201, {"EPS", "EPS_NO_EXTER_OSC_ERR", "没有外部晶振", 0}}, {1202, {"EPS", "EPS_EEPROM_ERR", "EEPROM读写故障", 0}}, @@ -115,22 +101,6 @@ std::unordered_map faultMap = { {1217, {"EPS", "EPS_MIN_GEAR_BREAK_ERR", "小齿轮角度传感器断开", 0}}, {1218, {"EPS", "EPS_MED_GEAR_BREAK_ERR", "中齿轮角度传感器断开", 0}}, - {1501, {"DBS", "DBS_OVERLOAD_ERR", "过载故障", 0}}, - {1502, {"DBS", "DBS_OVER_TEMP_VOL_ERR", "过温或过压故障", 0}}, - {1503, {"DBS", "DBS_MOS_SHORT_ERR", "MOS短路故障", 0}}, - {1504, {"DBS", "DBS_UNDER_VOL_ERR", "电源欠压故障", 0}}, - {1505, {"DBS", "DBS_OVER_VOL_ERR", "电源过压故障", 0}}, - {1506, {"DBS", "DBS_PRESSURE_LOW_ERR", "压力不足", 0}}, - {1507, {"DBS", "DBS_PHASE_LOSS_ERR", "缺相故障", 0}}, - {1508, {"DBS", "DBS_COMM_ERR", "通讯故障", 0}}, - {1509, {"DBS", "DBS_CURR_SAMPLE_ERR", "电流采样故障", 0}}, - {1510, {"DBS", "DBS_DRIVE_ERR", "驱动故障", 0}}, - {1511, {"DBS", "DBS_ENCODER_ERR", "磁编故障", 0}}, - {1512, {"DBS", "DBS_PRESSURE_SENSOR_ERR", "压力传感器故障", 0}}, - {1513, {"DBS", "DBS_PEDAL_SENSOR_ERR", "踏板位置传感器故障", 0}}, - {1514, {"DBS", "DBS_MECHANICAL_ERR", "机械故障", 0}}, - {1515, {"DBS", "DBS_OTHER_ERR", "其他故障", 0}}, - {1701, {"VCU", "VCU_EMERGENCY_STOP_ERR", "急停信号", 0}}, {1702, {"VCU", "VCU_EEPROM_ERR", "EEPROM故障", 0}}, {1703, {"VCU", "VCU_BATTERY_VOL_VERY_LOW_ERR", "电池电压非常低错误", 0}}, diff --git a/src/mqtt_report/src/mqtt_report.cpp b/src/mqtt_report/src/mqtt_report.cpp index 585c03a..b2b3863 100644 --- a/src/mqtt_report/src/mqtt_report.cpp +++ b/src/mqtt_report/src/mqtt_report.cpp @@ -14,26 +14,6 @@ Config config; // 清扫车配置文件 GeneralMsg info_report; // 常规消息上报 -std::deque waterLevelHistory; -const size_t maxSamples = 10; - -void updateWaterLevel(float newLevel) -{ - waterLevelHistory.push_back(newLevel); - if (waterLevelHistory.size() > maxSamples) - { - waterLevelHistory.pop_front(); - } - - float sum = 0; - for (float v : waterLevelHistory) - sum += v; - float filteredLevel = sum / waterLevelHistory.size(); - - filteredLevel = std::clamp(filteredLevel, 0.0f, 180.0f); - info_report.waterLevel = static_cast(std::round(filteredLevel / 1.8f)); -} - // 解析can报文，做消息上报 void Msg_Handler(const sweeperMsg::CanFrame::SharedPtr msg) { @@ -49,88 +29,6 @@ void Msg_Handler(const sweeperMsg::CanFrame::SharedPtr msg) break; } - // MCU_Status_2 and MCU_Fault - case 0x0CFF15EF: - { - // 电机转速：Byte1 高位，Byte0 低位，有符号16位 - int16_t motor_rpm_raw = (static_cast(msg->data[1]) << 8) | msg->data[0]; - float gear_ratio = 16.14; - float wheel_radius = 0.20f; // 轮胎半径，单位：米 - float vehicle_speed_kmh = (2 * 3.1416f * wheel_radius * (motor_rpm_raw / gear_ratio) * 60.0f) / 1000.0f; - vehicle_speed_kmh = std::round(vehicle_speed_kmh * 100.0f) / 100.0f; - info_report.speed = vehicle_speed_kmh; - // std::cout << "[0x0CFF15EF] speed : " << info_report.speed << std::endl; - - // 电机温度(Byte4)，偏移 -40 - // info_report.motorTemp = static_cast(msg->data[4]) - 40; - - // ===================== 使用 控制器温度 代 电机温度，之后小程序适配了改回来 ===================== - info_report.motorTemp = static_cast(msg->data[5]) - 40; - // ===================== 使用 控制器温度 代 电机温度，之后小程序适配了改回来 ===================== - - // 控制器温度(Byte5)，偏移 -40 - // info_report.controllerTemp = static_cast(msg->data[5]) - 40; - - // 十进制输出CAN帧数据 - // std::cout << "[0x0CFF15EF] msg data : "; - // for (int i = 0; i < 8; i++) - // { - // std::cout << static_cast(msg->data[i]) << " "; - // } - // std::cout << std::endl; - // std::cout << "[0x0CFF15EF] motorTemp : " << info_report.motorTemp << std::endl; - // std::cout << "[0x0CFF15EF] controllerTemp : " << info_report.controllerTemp << std::endl; - // std::cout << std::endl; - - // 清空所有 MCU 错误码 - for (int code = 1101; code <= 1111; ++code) - { - vehicle_error_code.removeErrorCode(code); - } - - // 添加当前报文中的错误码（msg->data[7]） - auto it = mcuErrorMap.find(msg->data[7]); - if (it != mcuErrorMap.end()) - { - vehicle_error_code.addErrorCode(it->second); - } - - break; - } - - // MCU_Status_1 - case 0x0CFF16EF: - { - uint8_t dir = msg->data[1]; - // 按位判断 - if (dir & (1 << 0)) // bit1为1(0速) - info_report.gear = 0; - else if (dir & (1 << 1)) // bit2为1(前进) - info_report.gear = 1; - else if (dir & (1 << 2)) // bit3为1(后退) - info_report.gear = 2; - else - info_report.gear = -1; // 默认-1 - - // std::bitset<8> binary_dir(dir); - // std::cout << "[0x0CFF16EF] dir (binary) : " << binary_dir << std::endl; - // std::cout << "[0x0CFF16EF] gear : " << info_report.gear << std::endl; - break; - } - - // case 0x18FF0121: - // { - // uint8_t WaterValue_h = (msg->data[2]); - // uint8_t WaterValue_l = (msg->data[1]); - // uint16_t WaterValue = (WaterValue_h & 0x3f) * 16 + (WaterValue_l & 0x0f); - // float WaterLevel = WaterValue * (-0.235) + 199.75; - // WaterLevel = std::clamp(WaterLevel, 0.0f, 180.0f); - // updateWaterLevel(WaterLevel); - // std::cout << "[0x18FF0121] waterLevel : " << info_report.waterLevel << std::endl; - // std::cout << std::endl; - // break; - // } - // BMS_Fault case 0x1825E5F1: { @@ -155,6 +53,70 @@ void Msg_Handler(const sweeperMsg::CanFrame::SharedPtr msg) break; } + // MCU_Status_1 和 MCU_Fault（基础ID：0x0CF11E00，叠加设备ID） + case 0x0CF11E00: + { + // 解析电机实际转速（data[0]：低字节，data[1]：高字节） + uint16_t raw_speed = (static_cast(msg->data[1]) << 8) | msg->data[0]; + float actual_speed_rpm = static_cast(raw_speed); // 量程0-6000rpm，1rpm/bit + // 转换为km/h + float gear_ratio = 32.0; // 电机速比 + float wheel_radius = 0.3; // 轮胎直径(m) + info_report.speed = (actual_speed_rpm * 60 * 3.1416 * wheel_radius) / (1000 * gear_ratio); + std::cout << "[0x0CF11E00] speed : " << info_report.speed << std::endl; + + // 故障码（data[6]：低字节，data[7]：高字节） + uint16_t mcu_fault_code = (static_cast(msg->data[7]) << 8) | msg->data[6]; + // 清空原有MCU故障码（1101-1116，对应ERR0-ERR15） + for (int code = 1101; code <= 1116; ++code) + { + vehicle_error_code.removeErrorCode(code); + } + // 遍历16个故障位（ERR0-ERR15） + for (int i = 0; i < 16; ++i) + { + bool is_fault = (mcu_fault_code >> i) & 0x01; // 提取第i位故障状态 + int error_code = 1101 + i; // 自定义故障码（1101=ERR0，1116=ERR15） + + if (is_fault && faultMap.count(error_code)) // 故障存在且在映射表中 + { + vehicle_error_code.addErrorCode(error_code); + updateFaultLevel(error_code, 1); // 协议未定义故障等级，默认1级（可根据需求调整） + } + else // 故障不存在 + { + vehicle_error_code.removeErrorCode(error_code); + } + } + + break; + } + + // MCU_Status_2（基础ID：0x0CF11F00，叠加设备ID） + case 0x0CF11F00: + { + // 解析控制器温度（data[2]，偏移量40℃，1℃/bit） + int raw_ctrl_temp = static_cast(msg->data[2]); + info_report.controllerTemp = raw_ctrl_temp - 40; // 实际温度=原始值-40，量程：-40~215℃ + + // 解析电机温度（data[3]，偏移量30℃，1℃/bit） + int raw_motor_temp = static_cast(msg->data[3]); + info_report.motorTemp = raw_motor_temp - 30; // 实际温度=原始值-30，量程：-30~225℃ + + // 解析控制器状态（data[4]，BIT1-BIT0：命令状态；BIT3-BIT2：反馈状态） + uint8_t ctrl_status = msg->data[4]; + // 反馈状态（BIT3-BIT2）：0=空挡，1=前进，2=后退，3=保留 + int feedback_gear = (ctrl_status >> 2) & 0x03; + // 档位优先级：命令状态优先（空挡时以反馈状态为准，可根据需求调整） + info_report.gear = feedback_gear; + + std::cout << "[0x0CF11F00] controllerTemp : " << info_report.controllerTemp << std::endl; + std::cout << "[0x0CF11F00] motorTemp : " << info_report.motorTemp << std::endl; + std::cout << "[0x0CF11F00] gear : " << info_report.gear << std::endl; + + break; + } + // EPS_Status and EPS_Fault case 0x401: { @@ -185,32 +147,6 @@ void Msg_Handler(const sweeperMsg::CanFrame::SharedPtr msg) break; } - // DBS_Fault - case 0x143: - { - // 提取 FaultCode（3字节）：msg->data[0] ~ msg->data[2] - uint32_t fault_bits = msg->data[0] | - (msg->data[1] << 8) | - (msg->data[2] << 16); - - // 清空 DBS 故障码（1501 ~ 1515） - for (int code = 1501; code <= 1515; ++code) - { - vehicle_error_code.removeErrorCode(code); - } - - // 添加故障码（bit0 ~ bit14 → 1501 ~ 1515） - for (uint8_t bit = 0; bit <= 14; ++bit) - { - if ((fault_bits >> bit) & 0x01) - { - vehicle_error_code.addErrorCode(1501 + bit); - } - } - - break; - } - // VCU_Fault_1和Fault_2报文解析 (ID: 0x216) case 0x216: { @@ -305,8 +241,9 @@ std::string pack_general_info_to_json(const GeneralMsg &msg) j["chargeStatus"] = msg.chargeStatus; j["gear"] = msg.gear; j["speed"] = msg.speed; - j["waterLevel"] = msg.waterLevel; + j["steeringAngle"] = msg.steeringAngle; j["motorTemp"] = msg.motorTemp; + j["controllerTemp"] = msg.controllerTemp; j["timestamp"] = msg.timestamp; return j.dump(4); // 输出有序 JSON 字符串 diff --git a/src/radio_ctrl/config/config.json b/src/radio_ctrl/config/config.json index 7108c0b..1532f5f 100644 --- a/src/radio_ctrl/config/config.json +++ b/src/radio_ctrl/config/config.json @@ -2,7 +2,7 @@ "serial_port": "/dev/ttyUSB0", "baudrate": 100000, "rmoctl_para": { - "mcu_rpm_max": 50, + "mcu_rpm_max": 3000, "eps_angle_max": 40.0 } } \ No newline at end of file diff --git a/src/radio_ctrl/src/radio_ctrl.cpp b/src/radio_ctrl/src/radio_ctrl.cpp index 56f146f..cd54ead 100644 --- a/src/radio_ctrl/src/radio_ctrl.cpp +++ b/src/radio_ctrl/src/radio_ctrl.cpp @@ -9,7 +9,7 @@ namespace sweeperMsg = sweeper_interfaces::msg; constexpr float DEG_PER_SEC_TO_RPM = 1.0f / 6.0f; -constexpr float GEAR_RATIO = 7.0f; +constexpr float GEAR_RATIO = 16.5f; constexpr float DELTA_T = 0.02f; // 20ms class SBUSNode : public rclcpp::Node @@ -79,104 +79,39 @@ private: } // printf("\n"); - uint16_t ctrl = ch_data[4]; // 手刹 uint16_t gear = ch_data[5]; // 挡位 uint16_t sweep = ch_data[6]; // 清扫 int16_t speed = ch_data[1] - 992; //[-800,800] - static uint16_t last_ctrl = 992; // 初始手刹值 - static int current_brake = 1; // 默认上电是拉手刹（1） - - // 判断是否是上拨触发（192） - if (last_ctrl == 992 && ctrl == 192) + // 挡位选择 + if (gear == 192) { - // 上拨 → 松手刹 - current_brake = 0; + msg.gear = 1; // D挡 } - // 判断是否是下拨触发（1792） - else if (last_ctrl == 992 && ctrl == 1792) + else if (gear == 992) { - // 下拨 → 拉手刹 - current_brake = 1; + msg.gear = 0; // N挡 + } + else if (gear == 1792) + { + msg.gear = 2; // R挡 } - last_ctrl = ctrl; - msg.brake = current_brake; - - if (msg.brake == 0) // 手刹释放 - { - // 挡位选择 - if (gear == 192) - { - msg.gear = 0; // D挡 - } - else if (gear == 1792) - { - msg.gear = 1; // R挡 - } - - // 油门 / 刹车逻辑 - if (speed <= 0) - { - msg.ehb_anable = true; - msg.ehb_brake_pressure = -0.01f * std::clamp(static_cast(speed), -800, 0); - msg.rpm = 0; - } - else - { - msg.ehb_anable = false; - msg.ehb_brake_pressure = 0; - msg.rpm = static_cast(std::clamp(MCU_RPM_MAX * speed / 800, 0, MCU_RPM_MAX)); - } - - // 一键清扫 - if (sweep == 1792) - { - msg.roll_brush_suction_direction = 0; - msg.roll_brush_suction = 0; - msg.side_brush = 0; - msg.dust_shake = 0; - msg.brush_deck_pusher = 0; - msg.suction_squeeqee_pusher = 0; - msg.water_spray = 0; - } - else if (sweep == 192) - { - msg.roll_brush_suction_direction = 1; - msg.roll_brush_suction = 100; - msg.side_brush = 100; - msg.dust_shake = 100; - msg.brush_deck_pusher = 100; - msg.suction_squeeqee_pusher = 100; - msg.water_spray = 100; - } - - // 转向逻辑 - float target_angle = (static_cast(ch_data[3]) - 992) / 800 * EPS_ANGLE_MAX; - - // 角速度（度/秒） - float angle_speed = fabs(target_angle - current_feedback_angle) / DELTA_T; - - // 电机转速（单位：rpm） - float motor_rpm = angle_speed * DEG_PER_SEC_TO_RPM * GEAR_RATIO; - - // 限制电机转速到 [120, 1500] 范围，防止过小/过大 - uint16_t can_speed = std::clamp( - static_cast(motor_rpm), - static_cast(120), - static_cast(1500)); - - msg.angle = target_angle; - msg.angle_speed = can_speed; - } - else // 未释放，发送安全默认控制指令 + // 油门 / 刹车逻辑 + if (speed <= 0) { msg.brake = 1; - msg.gear = 0; msg.rpm = 0; - msg.angle = 0; - msg.angle_speed = 120; + } + else + { + msg.brake = 0; + msg.rpm = static_cast(std::clamp(MCU_RPM_MAX * speed / 800, 0, MCU_RPM_MAX)); + } + // 一键清扫 + if (sweep == 1792) + { msg.roll_brush_suction_direction = 0; msg.roll_brush_suction = 0; msg.side_brush = 0; @@ -185,6 +120,34 @@ private: msg.suction_squeeqee_pusher = 0; msg.water_spray = 0; } + else if (sweep == 192) + { + msg.roll_brush_suction_direction = 1; + msg.roll_brush_suction = 100; + msg.side_brush = 100; + msg.dust_shake = 100; + msg.brush_deck_pusher = 100; + msg.suction_squeeqee_pusher = 100; + msg.water_spray = 100; + } + + // 转向逻辑 + float target_angle = (static_cast(ch_data[3]) - 992) / 800 * EPS_ANGLE_MAX; + + // 角速度（度/秒） + float angle_speed = fabs(target_angle - current_feedback_angle) / DELTA_T; + + // 电机转速（单位：rpm） + float motor_rpm = angle_speed * DEG_PER_SEC_TO_RPM * GEAR_RATIO; + + // 限制电机转速到 [120, 1500] 范围，防止过小/过大 + uint16_t can_speed = std::clamp( + static_cast(motor_rpm), + static_cast(120), + static_cast(1500)); + + msg.angle = target_angle; + msg.angle_speed = can_speed; // 发布控制消息 pub_->publish(msg); @@ -193,8 +156,6 @@ private: << "\n 电磁刹: " << static_cast(msg.brake) << "\n 挡位: " << static_cast(msg.gear) << "\n 转速: " << static_cast(msg.rpm) - << "\n ehb使能: " << static_cast(msg.ehb_anable) - << "\n ehb制动压力: " << static_cast(msg.ehb_brake_pressure) << "\n 轮端转向角度: " << msg.angle << "\n 转向角速度: " << msg.angle_speed << "\n 滚刷&吸风电机方向: " << static_cast(msg.roll_brush_suction_direction) diff --git a/src/rslidar_pointcloud_merger/CMakeLists.txt b/src/rslidar_pointcloud_merger/CMakeLists.txt deleted file mode 100644 index 811ca18..0000000 --- a/src/rslidar_pointcloud_merger/CMakeLists.txt +++ /dev/null @@ -1,39 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -cmake_policy(SET CMP0074 NEW) -project(rslidar_pointcloud_merger) - -# 1. 默认开启 C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(pcl_conversions REQUIRED) -find_package(pcl_ros REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(tf2_sensor_msgs REQUIRED) -find_package(PCL REQUIRED COMPONENTS common io) - -include_directories( - include - ${PCL_INCLUDE_DIRS} -) - -add_executable(merge_two_lidars src/merge_two_lidars.cpp) -ament_target_dependencies(merge_two_lidars - rclcpp sensor_msgs pcl_conversions pcl_ros - tf2 tf2_ros tf2_sensor_msgs -) - -target_link_libraries(merge_two_lidars ${PCL_LIBRARIES}) - -install(TARGETS merge_two_lidars - DESTINATION lib/${PROJECT_NAME}) - -install(DIRECTORY launch - DESTINATION share/${PROJECT_NAME}) - -ament_package() diff --git a/src/rslidar_pointcloud_merger/launch/merge_two_lidars.launch.py b/src/rslidar_pointcloud_merger/launch/merge_two_lidars.launch.py deleted file mode 100644 index 88fd1c7..0000000 --- a/src/rslidar_pointcloud_merger/launch/merge_two_lidars.launch.py +++ /dev/null @@ -1,93 +0,0 @@ -from launch import LaunchDescription -from launch_ros.actions import Node -from ament_index_python.packages import get_package_share_directory -from pathlib import Path - - -def generate_launch_description(): - pkg_share = get_package_share_directory("rslidar_pointcloud_merger") - - # front_lidar（右前角） -> rslidar（车辆中心） - # [x, y, z, yaw, pitch, roll] - tf_front = [ - 1.15, # x 前方 1.15 m - 0.66, # y 右侧 0.66 m - 0.0, # z 高度 1.7 m - 0.7854, # yaw π/4 ≈ 0.785 rad (水平右前 45°) - 0.0, # pitch - 0.0, # roll - ] - - # rear_lidar（左后角） -> rslidar（车辆中心） - # [x, y, z, yaw, pitch, roll] - tf_rear = [ - -1.15, # x 后方 1.15 m - -0.66, # y 左侧 0.66 m - 0.0, # z 高度 1.7 m - 3.9270, # yaw +5π/4 ≈ +3.927rad (水平后左 135°) - 0.0, # pitch - 0.0, # roll - ] - - return LaunchDescription( - [ - # ---------- 静态 TF (front) ---------- - Node( - package="tf2_ros", - executable="static_transform_publisher", - name="static_tf_front", - arguments=[*map(str, tf_front), "rslidar", "front_lidar"], - # arguments=[*map(str, tf_front), "rslidar", "front_lidar", "100"], - output="log", - ), - # ---------- 静态 TF (rear) ---------- - Node( - package="tf2_ros", - executable="static_transform_publisher", - name="static_tf_rear", - arguments=[*map(str, tf_rear), "rslidar", "rear_lidar"], - # arguments=[*map(str, tf_rear), "rslidar", "rear_lidar", "100"], - output="log", - ), - # ---------- 点云合并节点 ---------- - Node( - package="rslidar_pointcloud_merger", - executable="merge_two_lidars", - name="lidar_merger", - parameters=[ - { - "front_topic": "/rslidar_points/front_lidar", - "rear_topic": "/rslidar_points/rear_lidar", - "output_topic": "/rslidar_points", - "frame_id": "rslidar", - "queue_size": 3, - "cache_size": 10, - "time_tolerance": 0.1, - "max_wait_time": 1.0, - "enable_debug": False, - # 点云处理参数 - "filter_min_x": -5.0, # X轴最小坐标（米） - "filter_max_x": 10.0, # X轴最大坐标（米） - "filter_min_y": -5.0, # Y轴最小坐标（米） - "filter_max_y": 5.0, # Y轴最大坐标（米） - "filter_min_z": 0.0, # Z轴最小坐标（米） - "filter_max_z": 1.65, # Z轴最大坐标（米） - "voxel_size": 0.1, # 体素网格大小（米） 0.1~0.3m - "stat_mean_k": 30, # 计算点的平均距离时考虑的邻居数 - "stat_std_thresh": 1.5, # 标准差倍数阈值 噪声较多 ：0.5~1.0。 噪声较少 ：1.0~2.0。 - "grid_size": 50, # 栅格矩阵的边长（单元格数） - "grid_range": 15.0, # 栅格覆盖的实际空间范围（米） - # 单元格尺寸：由 grid_range / grid_size 决定 - "enable_print": False, # 是否打印栅格 - # 车身过滤参数 - "filter_car": True, # 是否启用车身过滤 - "car_length": 2.4, # 车长（米） - "car_width": 1.4, # 车宽（米） - "car_lidar_offset_x": 0.0, # LiDAR在x轴的安装偏移 - "car_lidar_offset_y": 0.0, # LiDAR在y轴的安装偏移 - } - ], - output="screen", - ), - ] - ) diff --git a/src/rslidar_pointcloud_merger/package.xml b/src/rslidar_pointcloud_merger/package.xml deleted file mode 100644 index 2f90f3a..0000000 --- a/src/rslidar_pointcloud_merger/package.xml +++ /dev/null @@ -1,26 +0,0 @@ - - - rslidar_pointcloud_merger - 0.1.0 - Merge two RSLiDAR point clouds into one topic. - - Your Name - Apache-2.0 - - ament_cmake - - - rclcpp - sensor_msgs - pcl_conversions - pcl_ros - tf2 - tf2_ros - tf2_sensor_msgs - - rviz2 - - - ament_cmake - - \ No newline at end of file diff --git a/src/rslidar_pointcloud_merger/src/merge_two_lidars.cpp b/src/rslidar_pointcloud_merger/src/merge_two_lidars.cpp deleted file mode 100644 index af8791f..0000000 --- a/src/rslidar_pointcloud_merger/src/merge_two_lidars.cpp +++ /dev/null @@ -1,913 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include -#include - -using sensor_msgs::msg::PointCloud2; -using std::placeholders::_1; - -// 点云帧结构体 -struct CloudFrame -{ - std::shared_ptr cloud; - rclcpp::Time stamp; - rclcpp::Time received_time; // 添加接收时间用于延时分析 - std::string source; -}; - -// 内存池实现 -class PointCloud2Pool -{ -public: - std::shared_ptr acquire() - { - std::lock_guard lock(mutex_); - if (pool_.empty()) - { - return std::make_shared(); - } - else - { - auto cloud = std::move(pool_.back()); - pool_.pop_back(); - return cloud; - } - } - - void release(std::shared_ptr cloud) - { - if (!cloud) - return; - - cloud->data.clear(); - cloud->width = 0; - cloud->height = 0; - cloud->row_step = 0; - - std::lock_guard lock(mutex_); - if (pool_.size() < 10) // 限制池大小 - { - pool_.push_back(cloud); - } - } - -private: - std::vector> pool_; - std::mutex mutex_; -}; - -class LidarMerger : public rclcpp::Node -{ -public: - LidarMerger() : Node("lidar_merger") - { - /* ---------- 参数 ---------- */ - front_topic_ = declare_parameter("front_topic", "/rslidar_points/front_lidar"); - rear_topic_ = declare_parameter("rear_topic", "/rslidar_points/rear_lidar"); - output_topic_ = declare_parameter("output_topic", "/rslidar_points"); - target_frame_ = declare_parameter("frame_id", "rslidar"); - queue_size_ = declare_parameter("queue_size", 3); // 减小队列大小 - cache_size_ = declare_parameter("cache_size", 10); // 增加缓存以应对延时 - time_tolerance_ = declare_parameter("time_tolerance", 0.1); // 放宽时间容差 - max_wait_time_ = declare_parameter("max_wait_time", 1.0); // 增加到1.0s以适应实际延时 - enable_debug_ = declare_parameter("enable_debug", false); // 默认关闭调试减少日志开销 - - // 点云处理参数 - filter_min_x_ = declare_parameter("filter_min_x", -10.0f); - filter_max_x_ = declare_parameter("filter_max_x", 10.0f); - filter_min_y_ = declare_parameter("filter_min_y", -10.0f); - filter_max_y_ = declare_parameter("filter_max_y", 10.0f); - filter_min_z_ = declare_parameter("filter_min_z", -2.0f); - filter_max_z_ = declare_parameter("filter_max_z", 2.0f); - voxel_size_ = declare_parameter("voxel_size", 0.1f); // 降采样体素大小 - stat_mean_k_ = declare_parameter("stat_mean_k", 30); // 统计离群点去除的k值 - stat_std_thresh_ = declare_parameter("stat_std_thresh", 1.0f); // 统计离群点去除的标准差阈值 - grid_size_ = declare_parameter("grid_size", 100); // 栅格大小 - grid_range_ = declare_parameter("grid_range", 20.0f); // 栅格范围 - enable_print_ = declare_parameter("enable_print", true); - - // 车身过滤参数 - filter_car_ = declare_parameter("filter_car", true); // 是否启用车身过滤 - car_length_ = declare_parameter("car_length", 2.3f); // 车长（米） - car_width_ = declare_parameter("car_width", 1.32f); // 车宽（米） - car_lidar_offset_x_ = declare_parameter("car_lidar_offset_x", 0.0f); // LiDAR在x轴的安装偏移 - car_lidar_offset_y_ = declare_parameter("car_lidar_offset_y", 0.0f); // LiDAR在y轴的安装偏移 - - // 打印所有参数值（添加到构造函数末尾） - RCLCPP_INFO_STREAM(this->get_logger(), - "\n\n---------- 点云融合节点参数配置 ----------" - << "\n [输入输出]" - << "\n 前雷达话题: " << front_topic_ - << "\n 后雷达话题: " << rear_topic_ - << "\n 输出话题: " << output_topic_ - << "\n 目标坐标系: " << target_frame_ - << "\n [同步参数]" - << "\n 队列大小: " << queue_size_ - << "\n 缓存大小: " << cache_size_ - << "\n 时间容差(s): " << time_tolerance_ - << "\n 最大等待时间(s): " << max_wait_time_ - << "\n [调试选项]" - << "\n 调试模式: " << (enable_debug_ ? "开启" : "关闭") - << "\n [点云处理]" - << "\n 空间过滤范围(m): [" << filter_min_x_ << ", " << filter_max_x_ - << "] x [" << filter_min_y_ << ", " << filter_max_y_ - << "] x [" << filter_min_z_ << ", " << filter_max_z_ << "]" - << "\n 体素大小(m): " << voxel_size_ - << "\n 离群点k值: " << stat_mean_k_ - << "\n 离群点标准差阈值: " << stat_std_thresh_ - << "\n 栅格大小: " << grid_size_ << "x" << grid_size_ - << "\n 栅格范围(m): " << grid_range_ - << "\n 打印栅格: " << (enable_print_ ? "开启" : "关闭") - << "\n [车身过滤]" - << "\n 启用车身过滤: " << (filter_car_ ? "是" : "否") - << "\n 车身尺寸(m): " << car_length_ << " x " << car_width_ - << "\n LiDAR偏移(m): (" << car_lidar_offset_x_ << ", " << car_lidar_offset_y_ << ")" - << "\n--------------------------------------------\n"); - - /* ---------- TF ---------- */ - tf_buffer_ = std::make_shared(get_clock()); - tf_listener_ = std::make_shared(*tf_buffer_); - - /* ---------- QoS - 优化为低延时 ---------- */ - auto qos = rclcpp::QoS(rclcpp::KeepLast(queue_size_)) - .best_effort() - .durability_volatile(); - // 移除deadline约束，避免因延时导致的消息丢弃 - - /* ---------- 订阅 ---------- */ - sub_front_ = create_subscription( - front_topic_, qos, std::bind(&LidarMerger::frontCB, this, _1)); - sub_rear_ = create_subscription( - rear_topic_, qos, std::bind(&LidarMerger::rearCB, this, _1)); - - /* ---------- 发布 ---------- */ - auto pub_qos = rclcpp::QoS(rclcpp::KeepLast(queue_size_)) - .reliable() - .durability_volatile(); - pub_ = create_publisher(output_topic_, pub_qos); - - // 创建栅格发布者 - grid_pub_ = this->create_publisher("grid_raw", 10); - - // 统计信息 - last_stats_time_ = now(); - front_count_ = 0; - rear_count_ = 0; - merged_count_ = 0; - - RCLCPP_INFO(get_logger(), "LidarMerger started with cache_size=%d, time_tolerance=%.3fs, max_wait=%.3fs", - cache_size_, time_tolerance_, max_wait_time_); - } - -private: - /* ---------- 回调函数 - 同步处理 ---------- */ - void frontCB(const PointCloud2::SharedPtr msg) - { - auto receive_time = now(); - front_count_++; - - if (enable_debug_) - { - auto data_delay = (receive_time - rclcpp::Time(msg->header.stamp)).seconds(); - RCLCPP_DEBUG(get_logger(), "Front cloud received, data_delay=%.3fs", data_delay); - } - - processCloudSync(msg, "front", receive_time); - } - - void rearCB(const PointCloud2::SharedPtr msg) - { - auto receive_time = now(); - rear_count_++; - - if (enable_debug_) - { - auto data_delay = (receive_time - rclcpp::Time(msg->header.stamp)).seconds(); - RCLCPP_DEBUG(get_logger(), "Rear cloud received, data_delay=%.3fs", data_delay); - } - - processCloudSync(msg, "rear", receive_time); - } - - /* ---------- 同步处理点云 ---------- */ - void processCloudSync(const PointCloud2::SharedPtr msg, const std::string &source, - const rclcpp::Time &receive_time) - { - // 立即进行坐标变换 - auto transformed_cloud = transformCloud(*msg); - if (!transformed_cloud) - { - RCLCPP_WARN(get_logger(), "Failed to transform %s cloud", source.c_str()); - return; - } - - // 创建CloudFrame - auto cloud_frame = std::make_shared(CloudFrame{ - transformed_cloud, - rclcpp::Time(msg->header.stamp), - receive_time, - source}); - - // 添加到缓存并立即尝试合并 - { - std::lock_guard lock(cache_mutex_); - addToCache(cloud_frame); - - // 立即尝试合并，无需等待定时器 - tryMerge(); - } - - // 定期输出统计信息 - printStats(); - } - - /* ---------- 添加到缓存 ---------- */ - void addToCache(std::shared_ptr frame) - { - auto &cache = (frame->source == "front") ? front_clouds_ : rear_clouds_; - - // 保持时间排序（最新的在前） - auto it = cache.begin(); - while (it != cache.end() && (*it)->stamp > frame->stamp) - { - ++it; - } - cache.insert(it, frame); - - // 限制缓存大小 - while (cache.size() > cache_size_) - { - cloud_pool_.release(cache.back()->cloud); - cache.pop_back(); - } - } - - /* ---------- 尝试合并 - 触发式 ---------- */ - void tryMerge() - { - if (front_clouds_.empty() || rear_clouds_.empty()) - return; - - // 查找最佳匹配 - auto [front_frame, rear_frame] = findBestMatchOptimized(); - - if (!front_frame || !rear_frame) - return; - - // 自适应延时检查 - 基于实际网络条件调整 - auto now_time = now(); - auto front_age = (now_time - front_frame->received_time).seconds(); - auto rear_age = (now_time - rear_frame->received_time).seconds(); - - // 动态调整max_wait_time基于观察到的延时 - static double observed_max_delay = 0.0; - double current_max_delay = std::max(front_age, rear_age); - observed_max_delay = std::max(observed_max_delay * 0.95, current_max_delay); // 指数衰减 - - // 使用观察到的延时来决定是否处理，而不是固定阈值 - double adaptive_threshold = std::min(max_wait_time_, observed_max_delay + 0.1); - - if (front_age > adaptive_threshold || rear_age > adaptive_threshold) - { - if (enable_debug_) - { - RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, - "Data age check: front_age=%.3fs, rear_age=%.3fs, threshold=%.3fs", - front_age, rear_age, adaptive_threshold); - } - - // 不直接返回，而是清理最旧的数据后重试 - cleanupExpiredClouds(); - if (!front_clouds_.empty() && !rear_clouds_.empty()) - { - // 递归重试一次 - static int retry_count = 0; - if (retry_count < 2) - { - retry_count++; - tryMerge(); - retry_count--; - } - } - return; - } - - // 合并点云 - auto merge_start = std::chrono::high_resolution_clock::now(); - auto merged = mergeClouds(*front_frame, *rear_frame); - auto merge_end = std::chrono::high_resolution_clock::now(); - - if (!merged) - return; - - // 设置时间戳和frame_id - auto front_time = front_frame->stamp.nanoseconds(); - auto rear_time = rear_frame->stamp.nanoseconds(); - merged->header.stamp = rclcpp::Time((front_time + rear_time) / 2, RCL_ROS_TIME); - merged->header.frame_id = target_frame_; - - // 发布合并后的点云 - // pub_->publish(*merged); - merged_count_++; - - // 将ROS点云消息转换为PCL点云 - pcl::PointCloud::Ptr merged_pcl(new pcl::PointCloud); - pcl::fromROSMsg(*merged, *merged_pcl); - - // ========================= 点云 ========================= - auto processed_pcl = processPointCloud(merged_pcl); - if (!processed_pcl) - return; - - // 将处理后的PCL点云转回ROS消息 - auto processed = std::make_shared(); - pcl::toROSMsg(*processed_pcl, *processed); - processed->header = merged->header; - - // 发布处理后的点云 - pub_->publish(*processed); - // ========================= 点云 ========================= - // ========================= 栅格 ========================= - auto grid = processPointCloud_grid(merged_pcl); - if (grid.empty()) - return; - - // 可视化栅格 - if (enable_print_) - visualizeGridInTerminal(grid); - - // 发布栅格到ROS话题 - publishGrid(grid); - // ========================= 栅格 ========================= - - // 延时分析（降低频率） - if (enable_debug_ && merged_count_ % 10 == 0) // 每10次输出一次 - { - auto total_delay = (now_time - processed->header.stamp).seconds(); - auto process_time = std::chrono::duration(std::chrono::high_resolution_clock::now() - merge_start).count(); - - RCLCPP_INFO(get_logger(), - "Merged #%d: points=%zu → %zu, total_delay=%.3fs, process_time=%.3fms, adaptive_threshold=%.3fs", - merged_count_, merged->data.size() / merged->point_step, - processed->data.size() / processed->point_step, - total_delay, process_time * 1000, adaptive_threshold); - } - - // 清理已使用的点云 - removeUsedClouds(front_frame, rear_frame); - } - - /* ---------- 点云处理流程 ---------- */ - pcl::PointCloud::Ptr processPointCloud(const pcl::PointCloud::Ptr &cloud) - { - if (!cloud || cloud->empty()) - return nullptr; - - // 1. 条件过滤 - auto filtered = applyConditionalFiltering(cloud); - if (filtered->empty()) - { - RCLCPP_WARN(get_logger(), "[processPointCloud] Filtered cloud is empty!"); - return cloud; // 返回原始云而不是nullptr - } - - // 2. 降采样 - auto downsampled = applyVoxelGridFiltering(filtered); - if (downsampled->empty()) - { - RCLCPP_WARN(get_logger(), "[processPointCloud] Downsampled cloud is empty!"); - return filtered; - } - - // 3. 离群点去除 - auto outlier_removed = applyStatisticalOutlierRemoval(downsampled); - if (outlier_removed->empty()) - { - RCLCPP_WARN(get_logger(), "[processPointCloud] Outlier-removed cloud is empty!"); - return downsampled; - } - - return outlier_removed; - } - std::vector> processPointCloud_grid(const pcl::PointCloud::Ptr &cloud) - { - if (!cloud || cloud->empty()) - return {}; // 返回空栅格 - - // 1. 条件过滤 - auto filtered = applyConditionalFiltering(cloud); - if (filtered->empty()) - { - RCLCPP_WARN(get_logger(), "[processPointCloud_grid] Filtered cloud is empty!"); - return {}; - } - - // 2. 降采样 - auto downsampled = applyVoxelGridFiltering(filtered); - if (downsampled->empty()) - { - RCLCPP_WARN(get_logger(), "[processPointCloud_grid] Downsampled cloud is empty!"); - return {}; - } - - // 3. 离群点去除 - auto outlier_removed = applyStatisticalOutlierRemoval(downsampled); - if (outlier_removed->empty()) - { - RCLCPP_WARN(get_logger(), "[processPointCloud_grid] Outlier-removed cloud is empty!"); - return {}; - } - - // 4. 栅格化处理并返回栅格结果 - return generateOccupancyGrid(outlier_removed); - } - - /* ---------- 条件过滤 ---------- */ - pcl::PointCloud::Ptr applyConditionalFiltering(const pcl::PointCloud::Ptr &cloud) - { - pcl::PointCloud::Ptr filtered(new pcl::PointCloud); - - for (const auto &point : cloud->points) - { - // 自定义过滤条件：保留特定区域内的点 - if (point.x > filter_min_x_ && point.x < filter_max_x_ && - point.y > filter_min_y_ && point.y < filter_max_y_ && - point.z > filter_min_z_ && point.z < filter_max_z_) - { - filtered->points.push_back(point); - } - } - - filtered->width = filtered->points.size(); - filtered->height = 1; - filtered->is_dense = true; - - // 不启用车身过滤，直接返回 - if (!filter_car_) - return filtered; - // RCLCPP_INFO(get_logger(), "启用车身过滤"); - - // 使用CropBox移除车身区域 - pcl::CropBox crop; - crop.setInputCloud(filtered); - - // 设置车身区域（最小点和最大点） - Eigen::Vector4f min_point(car_lidar_offset_x_ - car_length_ / 2.0f, - car_lidar_offset_y_ - car_width_ / 2.0f, - filter_min_z_, 1.0); - - Eigen::Vector4f max_point(car_lidar_offset_x_ + car_length_ / 2.0f, - car_lidar_offset_y_ + car_width_ / 2.0f, - filter_max_z_, 1.0); - - crop.setMin(min_point); - crop.setMax(max_point); - crop.setNegative(true); // true表示保留外部点，false表示保留内部点 - - pcl::PointCloud::Ptr output(new pcl::PointCloud); - crop.filter(*output); - - // return output; - - // 右侧主刷 (x:0.85~1.25, y:0.6~0.96, z:1.2~1.65) - pcl::CropBox crop1; - crop1.setInputCloud(output); - Eigen::Vector4f min1(0.85f, 0.6f, 1.2f, 1.0); // 右侧主刷最小点 - Eigen::Vector4f max1(1.25f, 0.96f, 1.65f, 1.0); // 右侧主刷最大点 - crop1.setMin(min1); - crop1.setMax(max1); - crop1.setNegative(true); // 剔除右侧主刷内的点 - - pcl::PointCloud::Ptr output1(new pcl::PointCloud); - crop1.filter(*output1); - - // 左侧主刷 - 右侧主刷的X轴对称（y坐标取相反数） - pcl::CropBox crop2; - crop2.setInputCloud(output1); - // X轴对称：将y坐标取相反数，保持x和z坐标不变 - Eigen::Vector4f min2(0.85f, -0.96f, 1.2f, 1.0); // 左侧主刷最小点（y取反后注意交换大小） - Eigen::Vector4f max2(1.25f, -0.6f, 1.65f, 1.0); // 左侧主刷最大点（y取反后注意交换大小） - crop2.setMin(min2); - crop2.setMax(max2); - crop2.setNegative(true); // 剔除左侧主刷内的点 - - pcl::PointCloud::Ptr final_output(new pcl::PointCloud); - crop2.filter(*final_output); - - return final_output; // 剔除两侧主刷 - } - - /* ---------- 降采样（体素网格过滤） ---------- */ - pcl::PointCloud::Ptr applyVoxelGridFiltering(const pcl::PointCloud::Ptr &cloud) - { - pcl::PointCloud::Ptr downsampled(new pcl::PointCloud); - pcl::VoxelGrid vg; - - vg.setInputCloud(cloud); - vg.setLeafSize(voxel_size_, voxel_size_, voxel_size_); - vg.filter(*downsampled); - - return downsampled; - } - - /* ---------- 离群点去除 ---------- */ - pcl::PointCloud::Ptr applyStatisticalOutlierRemoval(const pcl::PointCloud::Ptr &cloud) - { - pcl::PointCloud::Ptr filtered(new pcl::PointCloud); - pcl::StatisticalOutlierRemoval sor; - - sor.setInputCloud(cloud); - sor.setMeanK(stat_mean_k_); // 考虑每个点的k个最近邻 - sor.setStddevMulThresh(stat_std_thresh_); // 标准差倍数阈值 - sor.filter(*filtered); - - return filtered; - } - - /* ---------- 栅格化处理 ---------- */ - std::vector> generateOccupancyGrid(const pcl::PointCloud::Ptr &cloud) - { - // 0:空 1:障碍物 2:车体 - std::vector> grid(grid_size_, std::vector(grid_size_, 0)); - float resolution = grid_range_ / static_cast(grid_size_); - - // 标记车体区域 - if (filter_car_) - { - // 计算车体在栅格中的位置 - int car_min_x = static_cast((grid_range_ / 2 - (car_lidar_offset_x_ + car_length_ / 2.0f)) / resolution); - int car_max_x = static_cast((grid_range_ / 2 - (car_lidar_offset_x_ - car_length_ / 2.0f)) / resolution); - int car_min_y = static_cast((-car_lidar_offset_y_ - car_width_ / 2.0f + grid_range_ / 2) / resolution); - int car_max_y = static_cast((-car_lidar_offset_y_ + car_width_ / 2.0f + grid_range_ / 2) / resolution); - - // 确保索引在有效范围内 - car_min_x = std::max(0, std::min(car_min_x, grid_size_ - 1)); - car_max_x = std::max(0, std::min(car_max_x, grid_size_ - 1)); - car_min_y = std::max(0, std::min(car_min_y, grid_size_ - 1)); - car_max_y = std::max(0, std::min(car_max_y, grid_size_ - 1)); - - // 在栅格中标记车体区域 - for (int x = car_min_x; x <= car_max_x; ++x) - { - for (int y = car_min_y; y <= car_max_y; ++y) - { - grid[x][y] = 2; - } - } - } - - // 标记点云障碍物 - for (const auto &point : cloud->points) - { - // X轴（前向）映射到行索引（从上到下为正X到负X） - int grid_x = static_cast((grid_range_ / 2 - point.x) / resolution); - - // Y轴（右向）映射到列索引（从左到右为负Y到正Y） - int grid_y = static_cast((point.y + grid_range_ / 2) / resolution); - - if (grid_x >= 0 && grid_x < grid_size_ && grid_y >= 0 && grid_y < grid_size_) - { - // 只有当该位置不是车体时才标记为障碍物 - if (grid[grid_x][grid_y] != 2) - { - grid[grid_x][grid_y] = 1; - } - } - } - - return grid; - } - - /* ---------- 可视化栅格 - 终端打印 ---------- */ - void visualizeGridInTerminal(const std::vector> &grid) - { - std::system("clear"); - - std::cout << " -----------------------------障碍物矩阵------------------------------- " << std::endl; - - // 打印顶部边框 - std::cout << " " << std::string(grid[0].size(), '-') << std::endl; - - // 打印列号（仅0-9） - std::cout << " "; - for (size_t i = 0; i < grid[0].size(); i++) - { - std::cout << (i % 10); - } - std::cout << std::endl; - - // 打印栅格内容（行号 + 栅格） - for (size_t i = 0; i < grid.size(); i++) - { - // 行号显示（两位数，右对齐） - std::cout << (i < 10 ? " " : "") << i << "|"; - - // 栅格内容 - for (int cell : grid[i]) - { - // 根据栅格值选择显示符号 - std::cout << (cell == 1 ? "@" : (cell == 2 ? " " : ".")); - } - - std::cout << "|" << std::endl; - } - - // 打印底部边框 - std::cout << " " << std::string(grid[0].size(), '-') << std::endl; - } - - /* ---------- 发布栅格 ---------- */ - void publishGrid(const std::vector> &grid) - { - // 创建Int32MultiArray消息 - auto msg = std_msgs::msg::Int32MultiArray(); - - // 设置维度信息 - msg.layout.dim.resize(2); - msg.layout.dim[0].label = "height"; // 行（对应X轴） - msg.layout.dim[0].size = grid.size(); - msg.layout.dim[0].stride = grid.size() * grid[0].size(); - - msg.layout.dim[1].label = "width"; // 列（对应Y轴） - msg.layout.dim[1].size = grid[0].size(); - msg.layout.dim[1].stride = grid[0].size(); - - // 设置数据（按行优先展开） - msg.data.clear(); - for (const auto &row : grid) - { - msg.data.insert(msg.data.end(), row.begin(), row.end()); - } - - // 发布消息 - grid_pub_->publish(msg); - } - /* ---------- 优化的匹配算法 ---------- */ - std::pair, std::shared_ptr> findBestMatchOptimized() - { - if (front_clouds_.empty() || rear_clouds_.empty()) - return {nullptr, nullptr}; - - // 快速策略：优先使用最新的点云进行匹配 - auto front_candidate = front_clouds_.front(); - auto rear_candidate = rear_clouds_.front(); - - // 检查时间差 - auto time_diff = std::abs((front_candidate->stamp - rear_candidate->stamp).seconds()); - - if (time_diff <= time_tolerance_) - { - return {front_candidate, rear_candidate}; - } - - // 如果最新的不匹配，寻找更好的组合（但限制搜索范围） - std::shared_ptr best_front = nullptr; - std::shared_ptr best_rear = nullptr; - double min_diff = std::numeric_limits::max(); - - // 限制搜索范围到前3个元素 - size_t search_limit = std::min(3, std::min(front_clouds_.size(), rear_clouds_.size())); - - for (size_t i = 0; i < search_limit && i < front_clouds_.size(); ++i) - { - for (size_t j = 0; j < search_limit && j < rear_clouds_.size(); ++j) - { - auto diff = std::abs((front_clouds_[i]->stamp - rear_clouds_[j]->stamp).seconds()); - if (diff < min_diff) - { - min_diff = diff; - best_front = front_clouds_[i]; - best_rear = rear_clouds_[j]; - } - } - } - - if (min_diff <= time_tolerance_) - { - return {best_front, best_rear}; - } - - return {nullptr, nullptr}; - } - - /* ---------- 移除已使用的点云 ---------- */ - void removeUsedClouds(std::shared_ptr used_front, - std::shared_ptr used_rear) - { - // 从front_clouds_中移除 - auto front_it = std::find(front_clouds_.begin(), front_clouds_.end(), used_front); - if (front_it != front_clouds_.end()) - { - cloud_pool_.release((*front_it)->cloud); - front_clouds_.erase(front_it); - } - - // 从rear_clouds_中移除 - auto rear_it = std::find(rear_clouds_.begin(), rear_clouds_.end(), used_rear); - if (rear_it != rear_clouds_.end()) - { - cloud_pool_.release((*rear_it)->cloud); - rear_clouds_.erase(rear_it); - } - - // 清理过期的点云 - cleanupExpiredClouds(); - } - - /* ---------- 清理过期点云 ---------- */ - void cleanupExpiredClouds() - { - auto now_time = now(); - auto cleanup_threshold = max_wait_time_; // 使用配置的等待时间 - - // 清理front_clouds_中过期的数据 - while (!front_clouds_.empty()) - { - auto age = (now_time - front_clouds_.back()->received_time).seconds(); - if (age > cleanup_threshold) - { - if (enable_debug_) - { - RCLCPP_DEBUG(get_logger(), "Cleaning up old front cloud, age=%.3fs", age); - } - cloud_pool_.release(front_clouds_.back()->cloud); - front_clouds_.pop_back(); - } - else - { - break; - } - } - - // 清理rear_clouds_中过期的数据 - while (!rear_clouds_.empty()) - { - auto age = (now_time - rear_clouds_.back()->received_time).seconds(); - if (age > cleanup_threshold) - { - if (enable_debug_) - { - RCLCPP_DEBUG(get_logger(), "Cleaning up old rear cloud, age=%.3fs", age); - } - cloud_pool_.release(rear_clouds_.back()->cloud); - rear_clouds_.pop_back(); - } - else - { - break; - } - } - } - - /* ---------- 合并点云 ---------- */ - std::shared_ptr mergeClouds(const CloudFrame &front, const CloudFrame &rear) - { - size_t front_points = front.cloud->data.size() / front.cloud->point_step; - size_t rear_points = rear.cloud->data.size() / rear.cloud->point_step; - - if (front.cloud->fields != rear.cloud->fields || - front.cloud->point_step != rear.cloud->point_step || - front.cloud->is_bigendian != rear.cloud->is_bigendian) - { - RCLCPP_ERROR(get_logger(), "PointCloud2 formats do not match!"); - return nullptr; - } - - auto merged = cloud_pool_.acquire(); - - // 复制元数据 - merged->header = front.cloud->header; - merged->fields = front.cloud->fields; - merged->is_bigendian = front.cloud->is_bigendian; - merged->point_step = front.cloud->point_step; - merged->height = 1; - merged->width = front_points + rear_points; - - // 高效合并数据 - merged->data.clear(); - merged->data.reserve(front.cloud->data.size() + rear.cloud->data.size()); - merged->data.insert(merged->data.end(), front.cloud->data.begin(), front.cloud->data.end()); - merged->data.insert(merged->data.end(), rear.cloud->data.begin(), rear.cloud->data.end()); - merged->row_step = merged->data.size(); - - return merged; - } - - /* ---------- 坐标变换 - 简化版 ---------- */ - std::shared_ptr transformCloud(const PointCloud2 &in) - { - if (in.header.frame_id == target_frame_) - { - auto out = cloud_pool_.acquire(); - *out = in; - return out; - } - - try - { - // 使用最新可用的变换，不等待 - auto tf = tf_buffer_->lookupTransform( - target_frame_, in.header.frame_id, - tf2::TimePointZero); - - auto out = cloud_pool_.acquire(); - tf2::doTransform(in, *out, tf); - return out; - } - catch (const tf2::TransformException &e) - { - RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, - "TF failed (%s -> %s): %s", - in.header.frame_id.c_str(), - target_frame_.c_str(), e.what()); - return nullptr; - } - } - - /* ---------- 统计信息 ---------- */ - void printStats() - { - auto current_time = now(); - if ((current_time - last_stats_time_).seconds() >= 5.0) // 每5秒输出一次 - { - RCLCPP_INFO(get_logger(), - "Stats: Front=%d, Rear=%d, Merged=%d, Cache(F:%zu,R:%zu)", - front_count_, rear_count_, merged_count_, - front_clouds_.size(), rear_clouds_.size()); - - last_stats_time_ = current_time; - } - } - - /* ---------- 成员变量 ---------- */ - std::string front_topic_, rear_topic_, output_topic_, target_frame_; - int queue_size_, cache_size_; - double time_tolerance_, max_wait_time_; - bool enable_debug_; - - // 点云处理参数 - float filter_min_x_, filter_max_x_; - float filter_min_y_, filter_max_y_; - float filter_min_z_, filter_max_z_; - float voxel_size_; // 降采样体素大小 - int stat_mean_k_; // 统计离群点去除的k值 - float stat_std_thresh_; // 统计离群点去除的标准差阈值 - int grid_size_; // 栅格大小 - float grid_range_; // 栅格范围 - bool enable_print_; - - // 车身过滤参数 - bool filter_car_{true}; - float car_length_{2.3f}; - float car_width_{1.32f}; - float car_lidar_offset_x_{0.0f}; - float car_lidar_offset_y_{0.0f}; - - rclcpp::Subscription::SharedPtr sub_front_, sub_rear_; - rclcpp::Publisher::SharedPtr pub_; - - rclcpp::Publisher::SharedPtr grid_pub_; - - std::shared_ptr tf_buffer_; - std::shared_ptr tf_listener_; - - // 点云缓存 - std::deque> front_clouds_; - std::deque> rear_clouds_; - std::mutex cache_mutex_; - - // 内存池 - PointCloud2Pool cloud_pool_; - - // 统计信息 - rclcpp::Time last_stats_time_; - int front_count_, rear_count_, merged_count_; -}; - -int main(int argc, char **argv) -{ - rclcpp::init(argc, argv); - - // 设置线程优先级 - auto node = std::make_shared(); - - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} \ No newline at end of file diff --git a/src/sweeper_interfaces/msg/McCtrl.msg b/src/sweeper_interfaces/msg/McCtrl.msg index b462268..681b2ac 100644 --- a/src/sweeper_interfaces/msg/McCtrl.msg +++ b/src/sweeper_interfaces/msg/McCtrl.msg @@ -1,13 +1,9 @@ #清扫车控制指令 #mcu部分 -uint8 brake #电磁刹指令 0 放开 1 抱死 -uint8 gear #挡位 0x00:D档 0x01:R档 -uint8 rpm #转速占空比 0-100 - -#ehb部分 -bool ehb_anable #ehb使能 -float32 ehb_brake_pressure #ehb制动压力 0-8MPa +uint8 brake #电磁刹指令 0开;1关 +uint8 gear #挡位 0空挡;1前进;2后退;3保留 +uint8 rpm #转速 量程：0-6000，对应实际电机转速0-6000rpm #eps部分 float32 angle #轮端转向角度 分辨率0.2° [-66.0,66.0] 适当缩减