diff --git a/CHANGELOG.md b/CHANGELOG.md index 5d264d32..9cf6eee8 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,34 +1,167 @@ -# Changelog +# CHANGLOG -## v1.3.2 - 2022-03-01 +## Unreleased + +### Changed + +## 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 version to v1.3.2 +- Fix memory leaks problem +- Fix temperature calculation (for M1 only) + +## v1.4.0 - 2021-11-01 -## v1.3.1 - 2022-01-27 +### 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 -- Support the Ruby 4.0 Lidar -- Add vlan support with the PCAP file -- Add SOME/IP support +- Add vlan support +- Add somip support - Add split frame when pkt_cnt < last_pkt_cnt in mems - Add temperature in mems - Add ROCK support -- Support to bind the receiving socket to the specified ip -- Join multicast group with code instead of shell script -- Allow msop socket and difop socket to receive via same port. ### Fixed - Fix don't get time when PointType doesn't have timestamp member - Fix ROCK light center compensation algorithm -- Fix incorrect delay while playing pcap file with multicast lidars ### Removed - Remove redundance condition code in vec.emplace_back(std::move(point)) in mech lidars - - - ## v1.3.0 - 2020-11-10 ### Added diff --git a/CMakeLists.txt b/CMakeLists.txt index ede57274..648647b5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,30 +1,47 @@ cmake_minimum_required(VERSION 3.5) -cmake_policy(SET CMP0048 NEW) + +cmake_policy(SET CMP0048 NEW) # CMake 3.6 required + if(WIN32) - cmake_policy(SET CMP0074 NEW) + cmake_policy(SET CMP0074 NEW) # CMake 3.12 required endif(WIN32) -project(rs_driver VERSION 1.3.2) + +project(rs_driver VERSION 1.5.7) + +#======================== +# Project setup +#======================== +if (CMAKE_BUILD_TYPE STREQUAL "") + set(CMAKE_BUILD_TYPE Release) +endif() #============================= -# Compile Demos&Tools +# Compile Features #============================= -option(COMPILE_DEMOS "Build rs_driver demos" OFF) -option(COMPILE_TOOLS "Build point cloud visualization tool" OFF) +option(DISABLE_PCAP_PARSE "Disable PCAP file parse" OFF) +option(ENABLE_TRANSFORM "Enable transform functions" OFF) + +option(ENABLE_DOUBLE_RCVBUF "Enable double 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) #============================= -# Compile Demos&Tools +# Compile Demos, Tools, Tests #============================= -option(ENABLE_TRANSFORM "Enable transform functions" OFF) - -#======================== -# Project setup -#======================== -set(CMAKE_BUILD_TYPE Release) +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") @@ -33,29 +50,33 @@ if(MSVC) 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 - ) + 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(MINGW) - set(COMPILER "MINGW Compiler") - elseif(UNIX) + + 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} -Wall -std=c++14") + + 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("-- CMake run for ${COMPILER}") message(=============================================================) #======================== @@ -67,92 +88,117 @@ 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) -#======================== -# Boost -#======================== -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) - -find_package(Boost COMPONENTS system date_time regex REQUIRED) -include_directories(${Boost_INCLUDE_DIRS}) -list(APPEND EXTERNAL_LIBS ${Boost_LIBRARIES}) -list(APPEND EXTERNAL_LIBS "-lpthread") - -# fix pthread missing on ubuntu18.04 or ubuntu20.04 if(WIN32) else() - list(APPEND EXTERNAL_LIBS pthread) + if (CMAKE_SYSTEM_NAME STREQUAL "QNX") + list(APPEND EXTERNAL_LIBS socket) + else() + list(APPEND EXTERNAL_LIBS pthread) + endif() endif(WIN32) #======================== -# PCAP +# Build Features #======================== -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}) + +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() - list(APPEND EXTERNAL_LIBS pcap) -endif(WIN32) + 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}) -#======================== -# Eigen -#======================== if(${ENABLE_TRANSFORM}) + + message(=============================================================) + message("-- Enable Transform Fucntions") + message(=============================================================) + add_definitions("-DENABLE_TRANSFORM") + + # Eigen find_package(Eigen3 REQUIRED) include_directories(${EIGEN3_INCLUDE_DIR}) - message(=============================================================) - message("-- Enable Transform Fcuntions") - message(=============================================================) + endif(${ENABLE_TRANSFORM}) -#======================== -# Build Demos -#======================== +#============================ +# Build Demos, Tools, Tests +#============================ + +if(${ENABLE_DOUBLE_RCVBUF}) + add_definitions("-DENABLE_DOUBLE_RCVBUF") +endif(${ENABLE_DOUBLE_RCVBUF}) + +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(${COMPILE_DEMOS}) add_subdirectory(${CMAKE_CURRENT_LIST_DIR}/demo) endif(${COMPILE_DEMOS}) -if(${COMPILE_TOOLS}) +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_TOOLS}) +endif(${COMPILE_TOOL_VIEWER} OR ${COMPILE_TOOL_PCDSAVER}) -#=========================== -# Append Include Directory -#=========================== -get_property(rs_driver_include_dirs DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} PROPERTY INCLUDE_DIRECTORIES) -foreach(dir ${rs_driver_include_dirs}) - list(APPEND DRIVER_INCLUDE_DIRS ${dir}) -endforeach() +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 -) + ${DRIVER_CMAKE_ROOT}/rs_driverConfig.cmake + @ONLY) configure_file( ${DRIVER_CMAKE_ROOT}/rs_driverConfigVersion.cmake.in - ${DRIVER_CMAKE_ROOT}/rs_driverConfigVersion.cmake @ONLY -) + ${DRIVER_CMAKE_ROOT}/rs_driverConfigVersion.cmake + @ONLY) configure_file ( - ${CMAKE_CURRENT_LIST_DIR}/src/rs_driver/macro/version.h.in - ${CMAKE_CURRENT_LIST_DIR}/src/rs_driver/macro/version.h @ONLY -) + ${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) @@ -162,6 +208,7 @@ 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}) @@ -181,4 +228,6 @@ if(UNIX AND ${CMAKE_SOURCE_DIR} STREQUAL ${CMAKE_CURRENT_SOURCE_DIR}) 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/README.md b/README.md index 6e01e7e1..a06d3a54 100644 --- a/README.md +++ b/README.md @@ -4,170 +4,156 @@ ## 1 Introduction -**rs_driver** is cross-platform driver kernel for RoboSense LiDAR which is easy for users to do advanced development. +**rs_driver** is the driver kernel for the RoboSense LiDARs. -### 1.1 LiDAR type support +## 2 Supported LiDARs + +Below are the supported LiDARS. - RS-LiDAR-16 - RS-LiDAR-32 - RS-Bpearl -- RS-Ruby -- RS-Ruby Lite -- RS-LiDAR-M1 +- RS-Bpearl-V4 - 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 -## 2 Compilation and Installation +## 3 Supported Platforms -**rs_driver** is compatible with the following platforms and compilers: +**rs_driver** is compatible with the following platforms and compilers. Note the compiler should support C++ 14. -- Windows - - MSVC ( tested with VC2017 and VC2019) - - Mingw-w64 (tested with x86_64-8.1.0-posix-seh-rt_v6-rev0 ) - Ubuntu (16.04, 18.04) - gcc (4.8+) -### 2.1 Install dependencies - -**rs_driver** depends on the following third-party libraries. They must be compiled/installed properly in advance: - -- Boost -- pcap -- PCL (optional, only needed if build the visualization tool) -- Eigen3 (optional, only needed if use the internal transformation function) - -#### 2.1.1 Install dependencies in Ubuntu - -```sh -sudo apt-get install libboost-dev libpcap-dev libpcl-dev libeigen3-dev -``` - -#### 2.1.2 Install dependencies in Windows - -##### Boost - -In Windows, Boost needs compiling from source, please refer to the [official guide](https://www.boost.org/doc/libs/1_67_0/more/getting_started/windows.html) for detailed instructions. - -Once finishing installing Boost, add a system environment variable named ```BOOST_ROOT``` which is set to your Boost path. - -If using MSVC as your compiler, these pre-built [binary installers](https://boost.teeks99.com/) may save you some time. - -##### PCAP - -Firstly, install [pcap runtime](https://www.winpcap.org/install/bin/WinPcap_4_1_3.exe). - -Then, download pcap's [developer's pack](https://www.winpcap.org/install/bin/WpdPack_4_1_2.zip) to your favorite location and add the path to ```WpdPack_4_1_2/WpdPack``` folder to the ```Path``` environment variable. - -##### PCL - -*Note: you can skip installing PCL if you don't want to compile visualization tool.* - -(1) MSVC - -Please use the provided official [All-in-one installer](https://github.com/PointCloudLibrary/pcl/releases). - -Select the "Add PCL to the system PATH for xxx" option during installation. - -![](./doc/img/install_pcl.PNG) - -(2) Mingw-w64 +- Windows + - MSVC ( tested with Win10 / VS2019) -Since there'are no installers for mingw-w64 compiler available, PCL needs to be compiled out from source as instructed in this [tutorial](https://pointclouds.org/documentation/tutorials/compiling_pcl_windows.html). +## 4 Dependency Libraries +**rs_driver** depends on the following third-party libraries. Please install them before compiling **rs_driver**. +- 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) -## 3 Usage +## 5 Compile On Ubuntu -### 3.1 Install in advance +### 5.1 Dependency Libraries -*Note: installation is not supported in Windows.* +```sh +sudo apt-get install libpcap-dev libeigen3-dev libboost-dev libpcl-dev +``` - Install the driver. +### 5.2 Compilation ```bash cd rs_driver mkdir build && cd build cmake .. && make -j4 +``` + +### 5.3 Installation + +```bash sudo make install ``` -Then find **rs_driver** package and link to it in your ```CMakeLists.txt```. +### 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(project ${rs_driver_LIBRARIES}) +target_link_libraries(your_project ${rs_driver_LIBRARIES}) ``` -### 3.2 Use rs_driver as a submodule +### 5.5 Use rs_driver as a submodule -Add **rs_driver** as your project's submodule, then find **rs_driver** package and link to it in your ```CMakeLists.txt```. +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(project ${rs_driver_LIBRARIES}) +target_link_libraries(your_project ${rs_driver_LIBRARIES}) ``` +## 6 Compile On Windows +### 6.1 Dependency Libraries -## 4 Quick Start +#### libpcap -[Online connect LiDAR](doc/howto/how_to_online_use_driver.md) +Install [libpcap runtime library](https://www.winpcap.org/install/bin/WinPcap_4_1_3.exe). -[Decode pcap bag](doc/howto/how_to_decode_pcap.md) +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``` . +#### PCL +To compile with VS2019, please use the official installation package [PCL All-in-one installer](https://github.com/PointCloudLibrary/pcl/releases). -## 5 Demo Code & Visualization Tool +Select the "Add PCL to the system PATH for xxx" option during the installation. -### 5.1 Demo Code +![](./img/01_install_pcl.png) -**rs_driver** offers two demo programs in ```rs_driver/demo```： +### 6.2 Installation -- demo_online.cpp -- demo_pcap.cpp +Installation is not supported on Windows. -User can refer to the demo code for usage of api. To build demo programs, set the following option to ```ON``` when configuring using cmake: +## 7 Quick Start -```bash -cmake -DCOMPILE_DEMOS=ON .. -``` +**rs_driver** offers two demo programs in ```rs_driver/demo```. -### 5.2 Visualization Tool +- demo_online.cpp +- demo_pcap.cpp -**rs_driver** offer a visualization tool based on PCL in ```rs_driver/tool```： +Please refer to them for usage of the rs_driver API. -- rs_driver_viewer.cpp +`demo_pcap` is based on libpcap. -To build it, set the following option to ```ON``` when configuring using cmake: +To build `demo_online` and `demo_pcap`, enable the option COMPILE_DEMOS when configure the project. ```bash -cmake -DCOMPILE_TOOLS=ON .. +cmake -DCOMPILE_DEMOS=ON .. ``` -For basic usage of this tool, please refer to [Visualization tool guide](doc/howto/how_to_use_rs_driver_viewer.md) +For more info about how to decode an online Lidar, Please refer to [Decode online LiDAR](doc/howto/how_to_decode_online_lidar.md) +For more info about how to decode a PCAP file, Please refer to [Decode pcap file](doc/howto/how_to_decode_pcap_file.md) -## 6 Coordinate Transformation +## 8 Visualization of Point Cloud -**rs_driver** has the coordinate transformation function built inside and it can output the transformed point cloud directly, which can save the extra time cost of doing transformation after receiving point clouds from rs_driver. To enable this function, set the following option to ```ON``` when executing cmake command: -```bash -cmake -DENABLE_TRANSFORM=ON .. -``` +**rs_driver** offers a visualization tool `rs_driver_viwer` in ```rs_driver/tool``` , which is based on PCL. -For more details about the tool, please refer to [Transformation guide](doc/howto/how_to_use_transformation_function.md) +To build it, enable the option CMOPILE_TOOLS when configuring the project. +```bash +cmake -DCOMPILE_TOOLS=ON .. +``` +For more info about how to use the `rs_driver_viewer`, please refer to [Visualization tool guide](doc/howto/how_to_use_rs_driver_viewer.md) -## 7 Others +## 9 More Topics -Please refer to the following files for more infomation. +For more topics, Please refer to: -- **Parameters definition**: ```rs_driver/src/rs_driver/driver/driver_param.h``` -- **Point Cloud message definition**: ```rs_driver/src/rs_driver/msg/point_cloud_msg.h``` -- **API definition**: ```rs_driver/src/rs_driver/api/lidar_driver.h``` -- **Error code definition**: ```rs_driver/src/rs_driver/common/error_code.h``` +Trasformation function: [Transformation guide](doc/howto/how_to_transform_pointcloud.md) +Online Lidar - Advanced topics: [Online LiDAR - Advanced Topics](doc/howto/online_lidar_advanced_topics.md) +PCAP file - Advanced topics: [PCAP file - Advanced Topics](doc/howto/online_lidar_advanced_topics.md) -Multi-Cast function: [Multi-Cast](doc/howto/how_to_use_multi_cast_function.md) +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``` diff --git a/README_CN.md b/README_CN.md index c805ab0b..e472dba0 100644 --- a/README_CN.md +++ b/README_CN.md @@ -1,105 +1,81 @@ # **rs_driver** -## 1 工程简介 +[English Version](README.md) - **rs_driver**为速腾聚创跨平台的雷达驱动内核，方便用户二次开发使用。 +## 1 简介 -### 1.1 雷达型号支持 +**rs_driver**为RoboSense的雷达驱动。 + +## 2 支持的雷达型号 + +支持的雷达型号如下。 - RS-LiDAR-16 - RS-LiDAR-32 - RS-Bpearl -- RS-Ruby -- RS-Ruby Lite -- RS-LiDAR-M1 +- RS-Bpearl-V4 - 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 -## 2 编译与安装 - -**rs_driver**目前支持下列系统和编译器： - -- Windows - - MSVC (VS2017 & VS2019 已测试) - - Mingw-w64 (x86_64-8.1.0-posix-seh-rt_v6-rev0 已测试) +## 3 支持的操作系统 +支持的操作系统及编译器如下。注意编译器需支持C++14标准。 - Ubuntu (16.04, 18.04, 20.04) - gcc (4.8+) -### 2.1 依赖库的安装 - -**rs_driver** 依赖下列的第三方库，在编译之前需要先安装的它们： +- Windows + - MSVC (Win10 / VS2019 已测试) -- Boost -- pcap -- PCL (非必须，如果不需要可视化工具可忽略) +## 4 依赖的第三方库 -- Eigen3 (非必须，如果不需要内置坐标变换可忽略) +依赖的第三方库如下。 +- libpcap (可选。如不需要解析PCAP文件，可忽略) +- Eigen3 (可选。如不需要内置坐标变换，可忽略) +- PCL (可选。如不需要可视化工具，可忽略) +- Boost (可选。如不需要可视化工具，可忽略) -#### 2.1.1 Ubuntu中的依赖库安装 +## 5 Ubuntu下的编译及安装 +### 5.1 安装第三方库 -```shell -sudo apt-get install libboost-dev libpcap-dev libpcl-dev libeigen3-dev +```bash +sudo apt-get install libpcap-dev libeigen3-dev libboost-dev libpcl-dev ``` +### 5.2 编译 -#### 2.1.2 Windows下的依赖库安装 - -##### Boost - -Windows下需要从源码编译Boost库，请参考[官方指南](https://www.boost.org/doc/libs/1_67_0/more/getting_started/windows.html)。编译安装完成之后，将Boost的路径添加到系统环境变量```BOOST_ROOT```。 - -如果使用MSVC，也可以选择直接下载相应版本的预编译的[安装包](https://boost.teeks99.com/)。 - -##### Pcap - -首先，安装[pcap运行库](https://www.winpcap.org/install/bin/WinPcap_4_1_3.exe)。 - -然后，下载[开发者包](https://www.winpcap.org/install/bin/WpdPack_4_1_2.zip)到任意位置，然后将```WpdPack_4_1_2/WpdPack``` 的路径添加到环境变量```PATH```。 - -##### PCL - -*注：如果不编译可视化工具，可不编译安装PCL库* - -(1) MSVC - -如果使用MSVC编译器，可使用PCL官方提供的[安装包](https://github.com/PointCloudLibrary/pcl/releases)安装。 - -安装过程中选择 “Add PCL to the system PATH for xxx”: - -![](./doc/img/install_pcl.PNG) - -(2) Mingw-w64 - -PCL官方并没有提供mingw编译的库，所以需要按照[官方教程](https://pointclouds.org/documentation/tutorials/compiling_pcl_windows.html), 从源码编译PCL并安装。 - - - - -## 3 使用方式 - -### 3.1 安装使用 - -*注：在Windows中，**rs_driver** 暂不支持安装使用。* - -安装驱动 - -```sh +```bash cd rs_driver mkdir build && cd build cmake .. && make -j4 +``` + +### 5.3 安装 + +```bash sudo make install ``` -使用时，需要在```CMakeLists```文件中使用find_package()指令找到**rs_driver**，然后链接相关库。 +### 5.4 作为第三方库使用 + +配置您的```CMakeLists```文件，使用find_package()指令找到**rs_driver**库，并链接。 ```cmake find_package(rs_driver REQUIRED) include_directories(${rs_driver_INCLUDE_DIRS}) -target_link_libraries(project ${rs_driver_LIBRARIES}) +target_link_libraries(your_project ${rs_driver_LIBRARIES}) ``` -### 3.2 作为子模块使用 +### 5.5 作为子模块使用 + +将**rs_driver**作为子模块添加到您的工程，相应配置您的```CMakeLists```文件。 -在```CMakeLists```文件中将**rs_driver**作为子模块添加到工程内，使用find_package()指令找到**rs_driver**，然后链接相关库。 +使用find_package()指令找到**rs_driver**库，并链接。 ```cmake add_subdirectory(${PROJECT_SOURCE_DIR}/rs_driver) @@ -108,66 +84,73 @@ include_directories(${rs_driver_INCLUDE_DIRS}) target_link_libraries(project ${rs_driver_LIBRARIES}) ``` +## 6 Windows下的编译及安装 +### 6.1 安装第三方库 -## 4 快速上手 +#### libpcap -[在线连接雷达](doc/howto/how_to_online_use_driver.md) +安装[libpcap运行库](https://www.winpcap.org/install/bin/WinPcap_4_1_3.exe)。 -[解析pcap包](doc/howto/how_to_decode_pcap.md) +解压[libpcap开发者包](https://www.winpcap.org/install/bin/WpdPack_4_1_2.zip)到任意位置，并将```WpdPack_4_1_2/WpdPack``` 的路径添加到环境变量```PATH```。 +#### PCL +如果使用MSVC编译器，可使用PCL官方提供的[PCL安装包](https://github.com/PointCloudLibrary/pcl/releases)安装。 -## 5 示例程序 & 可视化工具 +安装过程中选择 “Add PCL to the system PATH for xxx”: -### 5.1 示例程序 +![](./img/01_install_pcl.png) -**rs_driver**提供了两个示例程序，存放于```rs_driver/demo``` 中： +### 6.2 安装 -- demo_online.cpp -- demo_pcap.cpp +Windows下，**rs_driver** 暂不支持安装。 -用户可参考示例程序编写代码调用接口。若希望编译这两个示例程序，执行CMake配置时加上参数： +## 7 快速上手 -```bash -cmake -DCOMPILE_DEMOS=ON .. -``` +**rs_driver**在目录```rs_driver/demo``` 下，提供了两个使用示例程序。 -### 5.2 可视化工具 +- demo_online.cpp +- demo_pcap.cpp -**rs_driver**提供了一个基于PCL的点云可视化工具，存放于```rs_driver/tool``` 中： +`demo_online`解析在线雷达的数据，输出点云， `demo_pcap`解析PCAP文件，输出点云。 -- rs_driver_viewer.cpp +`demo_pcap`基于libpcap库。 -若希望编译可视化工具，执行CMake配置时加上参数： +要编译这两个程序，需使能COMPILE_DEMOS选项。 ```bash -cmake -DCOMPILE_TOOLS=ON .. +cmake -DCOMPILE_DEMOS=ON .. ``` -具体使用请参考[可视化工具操作指南](doc/howto/how_to_use_rs_driver_viewer.md) +关于`demo_online`的更多说明，可以参考[连接在线雷达](doc/howto/how_to_decode_online_lidar.md) +关于`demo_pcap`的更多说明，可以参考[解析PCAP包](doc/howto/how_to_decode_pcap_file.md) +## 8 可视化工具 -## 6 坐标变换 +**rs_driver**在目录```rs_driver/tool``` 下，提供了一个点云可视化工具`rs_driver_viewer`。它基于PCL库。 - **rs_driver**提供了内置的坐标变换功能，可以直接输出经过坐标变换后的点云，节省了用户对点云进行坐标变换的额外操作耗时。若希望启用此功能，执行CMake配置时加上参数： +要编译这个工具，需使能COMPILE_TOOS选项。 ```bash -cmake -DENABLE_TRANSFORM=ON .. +cmake -DCOMPILE_TOOLS=ON .. ``` -具体使用请参考[坐标变换功能简介](doc/howto/how_to_use_transformation_function.md) +关于`rs_driver_viewer`的使用方法，请参考[可视化工具操作指南](doc/howto/how_to_use_rs_driver_viewer.md) +## 9 更多主题 +关于**rs_driver**的其他主题，请参考如下链接。 -## 7 其他资料 +坐标变换: [坐标变换](doc/howto/how_to_transform_pointcloud.md) +在线雷达 - 高级主题: [在线雷达 - 高级主题](doc/howto/online_lidar_advanced_topics_CN.md) +PCAP文件 - 高级主题: [PCAP文件 - 高级主题](doc/howto/online_lidar_advanced_topics.md) -请根据以下路径去查看相关文件。 +**rs_driver**的主要接口文件如下。 -- 参数定义: ```rs_driver/src/rs_driver/driver/driver_param.h``` -- 点云消息定义: ```rs_driver/src/rs_driver/msg/point_cloud_msg.h``` -- 接口定义: ```rs_driver/src/rs_driver/api/lidar_driver.h``` -- 错误码定义: ```rs_driver/src/rs_driver/common/error_code.h``` +- 点云消息定义: ```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``` -组播模式: [组播模式](doc/howto/how_to_use_multi_cast_function.md) \ No newline at end of file diff --git a/demo/CMakeLists.txt b/demo/CMakeLists.txt index 3e15f2b2..6420c2ce 100644 --- a/demo/CMakeLists.txt +++ b/demo/CMakeLists.txt @@ -1,22 +1,42 @@ cmake_minimum_required(VERSION 3.5) + project(rs_driver_demos) + message(=============================================================) message("-- Ready to compile demos") message(=============================================================) + +if (${ENABLE_PCL_POINTCLOUD}) + +find_package(PCL REQUIRED) +include_directories(${PCL_INCLUDE_DIRS}) +link_directories(${PCL_LIBRARY_DIRS}) +add_definitions(${PCL_DEFINITIONS}) + +endif (${ENABLE_PCL_POINTCLOUD}) + include_directories(${DRIVER_INCLUDE_DIRS}) + add_executable(demo_online - demo_online.cpp - ) + demo_online.cpp) + target_link_libraries(demo_online - pthread - ${EXTERNAL_LIBS} -) + ${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 - ) + demo_pcap.cpp) + target_link_libraries(demo_pcap - pthread - ${EXTERNAL_LIBS} -) + ${EXTERNAL_LIBS}) + +endif(NOT ${DISABLE_PCAP_PARSE}) - \ No newline at end of file + diff --git a/demo/demo_online.cpp b/demo/demo_online.cpp index e6e214fb..093962f2 100644 --- a/demo/demo_online.cpp +++ b/demo/demo_online.cpp @@ -30,69 +30,133 @@ WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWIS THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *********************************************************************************************************************/ -#include "rs_driver/api/lidar_driver.h" +#include + +#ifdef ENABLE_PCL_POINTCLOUD +#include +#else +#include +#endif + +//#define ORDERLY_EXIT + +typedef PointXYZI PointT; +typedef PointCloudT PointCloudMsg; + using namespace robosense::lidar; -struct PointXYZI ///< user defined point type +SyncQueue> free_cloud_queue; +SyncQueue> stuffed_cloud_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) { - float x; - float y; - float z; - uint8_t intensity; -}; - -/** - * @brief The point cloud callback function. This function will be registered to lidar driver. - * When the point cloud message is ready, driver can send out messages through this function. - * @param msg The lidar point cloud message. - */ -void pointCloudCallback(const PointCloudMsg& 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. + 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: Please do not put time-consuming operations in the callback function! */ - /* Make a copy of the message and process it in another thread is recommended*/ - RS_MSG << "msg: " << msg.seq << " point cloud size: " << msg.point_cloud_ptr->size() << RS_REND; + // 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 The exception callback function. This function will be registered to lidar driver. - * @param code The error code struct. - */ +// +// @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: Please do not put time-consuming operations in the callback function! */ - /* Make a copy of the error message and process it in another thread is recommended*/ + // 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; } +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; + } + + // 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" << RSLIDAR_VERSION_MAJOR << "." << RSLIDAR_VERSION_MINOR << "." - << RSLIDAR_VERSION_PATCH << RS_REND; + RS_TITLE << " RS_Driver Core Version: v" << getDriverVersion() << RS_REND; RS_TITLE << "------------------------------------------------------" << RS_REND; - LidarDriver driver; ///< Declare the driver object - 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 - param.lidar_type = LidarType::RS16; ///< Set the lidar type. Make sure this type is correct + param.lidar_type = LidarType::RSM1; ///< Set the lidar type. Make sure this type is correct param.print(); - driver.regExceptionCallback(exceptionCallback); ///< Register the exception callback function into the driver - driver.regRecvCallback(pointCloudCallback); ///< Register the point cloud callback function into the driver - if (!driver.init(param)) ///< Call the init function and pass the parameter + 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 (!driver.init(param)) ///< Call the init function { RS_ERROR << "Driver Initialize Error..." << RS_REND; return -1; } + + std::thread cloud_handle_thread = std::thread(processCloud); + 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; -} \ No newline at end of file +} diff --git a/demo/demo_online_multi_lidars.cpp b/demo/demo_online_multi_lidars.cpp new file mode 100644 index 00000000..c8c07fd1 --- /dev/null +++ b/demo/demo_online_multi_lidars.cpp @@ -0,0 +1,190 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the +following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following +disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following +disclaimer in the documentation and/or other materials provided with the distribution. + +3. Neither the names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, +INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*********************************************************************************************************************/ + +#include + +#ifdef ENABLE_PCL_POINTCLOUD +#include +#else +#include +#endif + +//#define ORDERLY_EXIT + +typedef PointXYZI PointT; +typedef PointCloudT PointCloudMsg; + +using namespace robosense::lidar; + +class DriverClient +{ +public: + + DriverClient(const std::string name) + : name_(name) + { + } + + bool init(const RSDriverParam& param) + { + RS_INFO << "------------------------------------------------------" << RS_REND; + RS_INFO << " " << name_ << RS_REND; + RS_INFO << "------------------------------------------------------" << RS_REND; + param.print(); + + driver_.regPointCloudCallback (std::bind(&DriverClient::driverGetPointCloudFromCallerCallback, this), + std::bind(&DriverClient::driverReturnPointCloudToCallerCallback, this, std::placeholders::_1)); + driver_.regExceptionCallback (std::bind(&DriverClient::exceptionCallback, this, std::placeholders::_1)); + + if (!driver_.init(param)) + { + RS_ERROR << name_ << ": Failed to initialize driver." << RS_REND; + return false; + } + + return true; + } + + bool start() + { + to_exit_process_ = false; + cloud_handle_thread_ = std::thread(std::bind(&DriverClient::processCloud, this)); + + driver_.start(); + RS_DEBUG << name_ << ": Started driver." << RS_REND; + + return true; + } + + void stop() + { + driver_.stop(); + + to_exit_process_ = true; + cloud_handle_thread_.join(); + } + +protected: + + std::shared_ptr driverGetPointCloudFromCallerCallback(void) + { + std::shared_ptr msg = free_cloud_queue_.pop(); + if (msg.get() != NULL) + { + return msg; + } + + return std::make_shared(); + } + + void driverReturnPointCloudToCallerCallback(std::shared_ptr msg) + { + stuffed_cloud_queue_.push(msg); + } + + void processCloud(void) + { + while (!to_exit_process_) + { + std::shared_ptr msg = stuffed_cloud_queue_.popWait(); + if (msg.get() == NULL) + { + continue; + } + + RS_MSG << name_ << ": msg: " << msg->seq << " point cloud size: " << msg->points.size() << RS_REND; + + free_cloud_queue_.push(msg); + } + } + + void exceptionCallback(const Error& code) + { + RS_WARNING << name_ << ": " << code.toString() << RS_REND; + } + + std::string name_; + LidarDriver driver_; + bool to_exit_process_; + std::thread cloud_handle_thread_; + SyncQueue> free_cloud_queue_; + SyncQueue> stuffed_cloud_queue_; +}; + +int main(int argc, char* argv[]) +{ + RS_TITLE << "------------------------------------------------------" << RS_REND; + RS_TITLE << " RS_Driver Core Version: v" << getDriverVersion() << RS_REND; + RS_TITLE << "------------------------------------------------------" << RS_REND; + + RSDriverParam param_left; ///< Create a parameter object + param_left.input_type = InputType::ONLINE_LIDAR; + param_left.input_param.msop_port = 6004; ///< Set the lidar msop port number, the default is 6699 + param_left.input_param.difop_port = 7004; ///< Set the lidar difop port number, the default is 7788 + param_left.lidar_type = LidarType::RSM1; ///< Set the lidar type. Make sure this type is correct + + DriverClient client_left("LEFT "); + if (!client_left.init(param_left)) ///< Call the init function + { + return -1; + } + + RSDriverParam param_right; ///< Create a parameter object + param_right.input_type = InputType::ONLINE_LIDAR; + param_right.input_param.msop_port = 6005; ///< Set the lidar msop port number, the default is 6699 + param_right.input_param.difop_port = 7005; ///< Set the lidar difop port number, the default is 7788 + param_right.lidar_type = LidarType::RSM1; ///< Set the lidar type. Make sure this type is correct + + DriverClient client_right("RIGHT"); + if (!client_right.init(param_right)) ///< Call the init function + { + return -1; + } + + client_left.start(); + client_right.start(); + +#ifdef ORDERLY_EXIT + std::this_thread::sleep_for(std::chrono::seconds(10)); + + client_left.stop(); + client_right.stop(); +#else + while (true) + { + std::this_thread::sleep_for(std::chrono::seconds(1)); + } +#endif + + return 0; +} + diff --git a/demo/demo_pcap.cpp b/demo/demo_pcap.cpp index 35c0f03b..fe2bca6c 100644 --- a/demo/demo_pcap.cpp +++ b/demo/demo_pcap.cpp @@ -30,71 +30,135 @@ WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWIS THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *********************************************************************************************************************/ -#include "rs_driver/api/lidar_driver.h" +#include + +#ifdef ENABLE_PCL_POINTCLOUD +#include +#else +#include +#endif + +//#define ORDERLY_EXIT + +typedef PointXYZI PointT; +typedef PointCloudT PointCloudMsg; + using namespace robosense::lidar; -struct PointXYZI ///< user defined point type +SyncQueue> free_cloud_queue; +SyncQueue> stuffed_cloud_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) { - float x; - float y; - float z; - uint8_t intensity; -}; - -/** - * @brief The point cloud callback function. This function will be registered to lidar driver. - * When the point cloud message is ready, driver can send out messages through this function. - * @param msg The lidar point cloud message. - */ -void pointCloudCallback(const PointCloudMsg& 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. + 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: Please do not put time-consuming operations in the callback function! */ - /* Make a copy of the message and process it in another thread is recommended*/ - RS_MSG << "msg: " << msg.seq << " point cloud size: " << msg.point_cloud_ptr->size() << RS_REND; + // 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 The exception callback function. This function will be registered to lidar driver. - * @param code The error code struct. - */ +// +// @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: Please do not put time-consuming operations in the callback function! */ - /* Make a copy of the error message and process it in another thread is recommended*/ + // 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; } +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; + } + + // 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" << RSLIDAR_VERSION_MAJOR << "." << RSLIDAR_VERSION_MINOR << "." - << RSLIDAR_VERSION_PATCH << RS_REND; + RS_TITLE << " RS_Driver Core Version: v" << getDriverVersion() << RS_REND; RS_TITLE << "------------------------------------------------------" << RS_REND; - LidarDriver driver; ///< Declare the driver object - RSDriverParam param; ///< Create a parameter object - param.input_param.read_pcap = true; ///< Set read_pcap to true + 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 - param.lidar_type = LidarType::RS16; ///< Set the lidar type. Make sure this type is correct + param.lidar_type = LidarType::RSM1; ///< Set the lidar type. Make sure this type is correct param.print(); - driver.regExceptionCallback(exceptionCallback); ///< Register the exception callback function into the driver - driver.regRecvCallback(pointCloudCallback); ///< Register the point cloud callback function into the driver - if (!driver.init(param)) ///< Call the init function and pass the parameter + 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 (!driver.init(param)) ///< Call the init function { RS_ERROR << "Driver Initialize Error..." << RS_REND; return -1; } + + std::thread cloud_handle_thread = std::thread(processCloud); + 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; -} \ No newline at end of file +} diff --git a/doc/howto/How_to_port_your_app_from_rs_driver_1.3.x_to_1.5.x.md b/doc/howto/How_to_port_your_app_from_rs_driver_1.3.x_to_1.5.x.md new file mode 100644 index 00000000..ccf7cadc --- /dev/null +++ b/doc/howto/How_to_port_your_app_from_rs_driver_1.3.x_to_1.5.x.md @@ -0,0 +1,203 @@ +# How to port your app from rs_driver v1.3.x to v1.5.x + +## 1 Why to port ? + +This document illustrates how to port your app from rs_driver v1.3.x to v1.5.x. + +Why to port your app from v1.3.x to v1.5.x? Below are several reasons. + ++ Obviously reduce CPU usage. ++ Add compile options to solve packet-loss problems on some platforms. ++ Remove the dependency on the Boost library, help to port rs_driver to other platforms other than x86. ++ Refactory the Network Input part and the Decoder part, to support different use cases. ++ Improve the help documents, including [understand source code of rs_driver](../src_intro/rs_driver_intro_CN.md), to help user understand it. + +## 2 How to port ? + +The interface of rs_driver contains two parts: RSDriverParam and LidarDriver. + +### 2.1 RSDriverParam + +#### 2.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 ///< The LiDAR driver parameter +{ + 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 ///< The LiDAR driver parameter +{ + 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`, or `RAW_PACKET`. ++ 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. ++ 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. + +#### 2.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 +}; +``` + +Below are the changes. ++ The member `device_ip` is not needed for receiving packets, so it is removed. ++ The member `multi_cast_address` is renamed to `group_address`。This is to say, in the multicast mode, the host will join the group to receive packets. ++ The member `read_pcap` is not needed now. `RSDriverParam.input_type` replaces it. ++ The members `use_someip` and `use_custom_proto` is not needed now. `user_layer_bytes` replaces them. + +#### 2.1.3 RSDecoderParam + +RSDecoderParam is as below in rs_driver 1.3.x. + +```c++ +typedef struct RSDecoderParam ///< LiDAR decoder parameter +{ + float max_distance = 200.0f; ///< Max distance of point cloud range + float min_distance = 0.2f; ///< Minimum distance of point cloud range + 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 cut_angle; + ///< 2: Split frames by fixed number of packets; + ///< 3: Split frames by custom number of packets (num_pkts_split) + uint32_t num_pkts_split = 1; ///< Number of packets in one frame, only be used when split_frame_mode=3 + float cut_angle = 0.0f; ///< Cut angle(degree) used to split frame, only be used when split_frame_mode=1 + bool use_lidar_clock = false; ///< true: use LiDAR clock as timestamp; false: use system clock as timestamp + RSTransformParam transform_param; ///< Used to transform points + RSCameraTriggerParam trigger_param; ///< Used to trigger camera +}; +``` + +And it is as below in rs_driver 1.5.x. + +```c++ +typedef struct RSDecoderParam ///< LiDAR decoder parameter +{ + bool config_from_file = false; ///< Internal use only for debugging + std::string angle_path = ""; ///< Internal use only for debugging + bool wait_for_difop = true; ///< true: start sending point cloud until receive difop packet + float min_distance = 0.2f; ///< Minimum distance of point cloud range + float max_distance = 200.0f; ///< Max distance of point cloud range + 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 + 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 + RSTransformParam transform_param; ///< Used to transform points +}; +``` + +Below are the changes. ++ The member `cut_angle` is renamed to `split_angle`, to align to `split_frame_mode`. ++ The member `config_from_file` is added. Only if it is true, the member `angle_path` is valid. ++ The member `num_pkts_split` is renamed to `number_blks_split`. That means to split frames `by blocks (in MSOP packets)` instead of `by MSOP packets`. + +### 2.2 LidarDriver + +LidarDriver contains the functions to run rs_driver. + +LidarDriver is as below in rs_driver 1.3.x. + +```c++ +template +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. + +For details of these two changes, please refer to [Decode online Lidar](./how_to_decode_online_lidar.md). + + + diff --git a/doc/howto/How_to_port_your_app_from_rs_driver_1.3.x_to_1.5.x_CN.md b/doc/howto/How_to_port_your_app_from_rs_driver_1.3.x_to_1.5.x_CN.md new file mode 100644 index 00000000..72c27288 --- /dev/null +++ b/doc/howto/How_to_port_your_app_from_rs_driver_1.3.x_to_1.5.x_CN.md @@ -0,0 +1,202 @@ +# 如何将基于rs_driver v1.3.x的应用程序移植到v1.5.x + +## 1 为什么要移植？ + +本文说明了如何将基于rs_driver v1.3.x的应用程序移植到v1.5.x上。 + +rs_driver v1.5.x是对v1.3.x较大重构后的版本。它主要做了如下改进。 ++ 明显减少CPU资源的占用率 ++ 增加可选的编译选项，解决在某些平台上丢包的问题 ++ 去除对Boost库的依赖，更容易移植到除x86以外的其他平台上。 ++ 重构网络部分和解码部分的代码，完整支持不同场景下的配置 ++ 完善帮助文档，尤其是提供了[源代码解析](../src_intro/rs_driver_intro_CN.md)文档，帮助用户深入理解驱动的实现，以方便功能裁剪。 + +## 2 如何移植？ + +rs_driver的接口包括了两个部分：RSDriverParam和LidarDriver。 + +### 2.1 RSDriverParam + +#### 2.1.1 RSDriverParam + +RSDriverParam包括了一些可以改变rs_driver行为的参数。 + +在v1.3.2中，RSDriverParam定义如下。 + +```c++ +typedef struct RSDriverParam ///< The LiDAR driver parameter +{ + 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 ///< The LiDAR driver parameter +{ + 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`, or `RAW_PACKET`. ++ 成员`wait_for_difop` 指定在处理MSOP包之前是否先等待DIFOP包。对于机械式雷达，如果没有DIFOP包中的垂直角数据，得到的点云就是扁平的。这个选项是与解码相关的，所以把它移到RSDecoderParam中。 ++ 删除成员`save_by_rows`。默认情况下，点在点云中按照扫描的通道次序保存在vector中，一轮扫描的点在竖直的线上，也就是按照列保存。成员`saved_by_rows` 可以指定按照水平的线保存点，也就是按照行保存。删除这个成员有两个原因： + + 在按照列保存点的vector中，按照行的顺序检索点，也是容易的，跳过通道数就可以了。 + + 这个选项的实现方式是将点云重排一遍，但是复制点云的代码太大了。 ++ 成员`angle_path`指定垂直角从指定的文件读取，而不是解析DIFOP包得到。这个选项是关于数据源的，所以移到RSInputParam。 ++ 成员`frame_id` 来自ROS的概念，rs_driver本身和rs_driver的其他使用者并不需要它，所以删除它。适配ROS的驱动 `rslidar_sdk`会创建和处理它。 + +#### 2.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 +}; +``` + +变动如下： ++ 删除成员`device_ip`。rs_driver接收MSOP/DIFOP包时，其实不需要这个值。 ++ 成员`multi_cast_address`改名为`group_address`。这里想表达的含义是，在组播模式下，rs_driver会将主机加入`group_address`指定的组播组。 ++ 删除成员`read_pcap`。 既然`RSDriverParam.input_type` 已经指定了数据源类型，`read_pcap`就不需要了。 ++ 删除成员`use_someip` and `use_custom_proto`。它们的功能已经被新的选项 `RSInputParam.user_layer_bytes`所取代。 + +#### 2.1.3 RSDecoderParam + +在v1.3.x中，RSDecoderParam定义如下。 + +```c++ +typedef struct RSDecoderParam ///< LiDAR decoder parameter +{ + float max_distance = 200.0f; ///< Max distance of point cloud range + float min_distance = 0.2f; ///< Minimum distance of point cloud range + 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 cut_angle; + ///< 2: Split frames by fixed number of packets; + ///< 3: Split frames by custom number of packets (num_pkts_split) + uint32_t num_pkts_split = 1; ///< Number of packets in one frame, only be used when split_frame_mode=3 + float cut_angle = 0.0f; ///< Cut angle(degree) used to split frame, only be used when split_frame_mode=1 + bool use_lidar_clock = false; ///< true: use LiDAR clock as timestamp; false: use system clock as timestamp + RSTransformParam transform_param; ///< Used to transform points + RSCameraTriggerParam trigger_param; ///< Used to trigger camera +}; +``` + +在v1.5.x中，RSDecoderParam定义如下。 + +```c++ +typedef struct RSDecoderParam ///< LiDAR decoder parameter +{ + bool config_from_file = false; ///< Internal use only for debugging + std::string angle_path = ""; ///< Internal use only for debugging + bool wait_for_difop = true; ///< true: start sending point cloud until receive difop packet + float min_distance = 0.2f; ///< Minimum distance of point cloud range + float max_distance = 200.0f; ///< Max distance of point cloud range + 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 + 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 + RSTransformParam transform_param; ///< Used to transform points +}; +``` + +变动如下： ++ 成员`cut_angle`改名为`split_angle`, 以便与成员 `split_frame_mode`的名字保持一致。 ++ 增加成员`config_from_file`。只有这个成员为true，成员`angle_path`才有效。 ++ 成员`num_pkts_split`改名为 `number_blks_split`. 因为在v1.3.x中，分帧的最小单位是packet，而在v1.5.x中，分帧单位改成了更小的block。 + +### 2.2 LidarDriver + +创建LidarDriver实例，并调用它的成员函数，可以运行rs_driver。 + +在v1.3.x中，LidarDriver定义如下。 + +```c++ +template +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时需要一个点云回调函数，用于rs_driver把构建好的点云返回给调用者，在v1.5.x中则需要两个回调函数，除了返回构建好的点云，还需要给rs_driver提供空闲的点云实例。 +关于这两点，可以参考[Decode online Lidar](./how_to_decode_online_lidar_CN.md)，得到更详细的说明。 + + + diff --git a/doc/howto/how_to_decode_online_lidar.md b/doc/howto/how_to_decode_online_lidar.md new file mode 100644 index 00000000..3ad7d1c9 --- /dev/null +++ b/doc/howto/how_to_decode_online_lidar.md @@ -0,0 +1,277 @@ +# How to decode online LiDAR + +## 1 Introduction + +This document illustrates how to decode the MSOP/DIFOP packets from an online Lidar with the driver's API. + +## 2 Design Consideration + +The driver is designed to achieve these objectives. ++ Parallel the construction and the process of point cloud. ++ Avoid to copy point cloud. ++ Avoid to malloc/free point cloud repeatly. + +Below is the supposed interaction between rs_driver and user's code. + +![](./img/11_rs_driver_queue_thread.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. + +## 3 Steps + +Below is the steps to use the driver's API. + +### 3.1 Define Point + +Point is the base unit of Point Cloud. The driver supposes that it may have 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 generated by the Lidar or on the host machine. +- ring -- The ring ID of the point, which represents the row number. e.g. For RS80, 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, otherwise the behaviors might be unexpected. + +### 3.2 Define Point Cloud + + The driver allows user to change the point cloud type, as long as it is compatible to the definition below. + + ```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; + }; + + typedef PointXYZI PointT; + typedef PointCloudT PointCloudMsg; + ``` + + Here user may add new members, and change the order of these members, but should not change or remove them. + +### 3.3 Point in Point Cloud + +For mechanical Lidar, it may be useful to explain how the layout of point cloud is like. + +Here is an example of mechanical 5_lasers Lidar. + +The Lidar scans block by block. It starts from b0c0(block 0 channel/laser 0), b0c1, b0c2, b0c3, b0c4, and then go to b1c0, b1c1, b1c2, b1c3, b1c4, and so on. +![](./img/11_rs_driver_point_cloud.png) + +At the same time, it saves the points in a vector, point by point, as below. +![](./img/11_rs_driver_point_cloud_vector.png) + +### 3.4 Define the driver object + +Define a driver object. + +```c++ +int main() +{ + LidarDriver driver; ///< Declare the driver object + ... +} +``` + +### 3.5 Configure the driver parameter + +Define a RSDriverParam variable and configure it. ++ `InputType::ONLINE_LIDAR` means that the driver get packets from a online Lidar. ++ `LidarType::RS16` means a RS16 Lidar. ++ Also set the local MSOP/DIFOP ports. Please use a 3rd party tool, such as WireShark, to get them. + +```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 + param.lidar_type = LidarType::RS16; ///< Set the lidar type. Make sure this type is correct + ... +} +``` + +### 3.6 Define and register Point Cloud callbacks + ++ As said above, user is supposed to provide free point cloud to the 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(); +} +``` + ++ Also user is supposed to accept stuffed point cloud from the 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; +} +``` + +Note: The driver calls these two callback functions in its `contruct_thread`, so **don't do any time-consuming task** in them. + ++ User process the point cloud in its own thread. + +```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 the driver. + +```c++ +int main() +{ + ... + driver.regPointCloudCallback(driverReturnPointCloudToCallerCallback, + driverReturnPointCloudToCallerCallback); + ... +} +``` + +### 3.7 Define and register exception callbacks + ++ When an error happens, the driver will inform user. User is supposed to get it via a callback function. + +```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; + }; + + typedef PointXYZI PointT; + typedef PointCloudT PointCloudMsg; + ``` + +使用者可以向这个定义增加新成员，改变成员顺序，但是不可以改变成员类型，或者删除成员。 + +### 3.3 点在点云中的排列 + +对于机械式雷达，说明点在点云中是如何排列的，可能是有意义的。 + +为了描述方便，假设有一个5通道的机械式雷达。雷达一个block接一个block地扫描。竖直方向的一轮扫描对应一个block，5个通道各一个点。（雷达每轮扫描的通道次序是不变的，但一般不是严格从上往下。下面的图中是从上往下，只是为了描述简单。） + +如下图，它扫描b0c0(block 0 channel/laser 0), b0c1, b0c2, b0c3, b0c4, 然后扫描b1c0, b1c1, b1c2, b1c3, b1c4, 如此下去。 +![](./img/11_rs_driver_point_cloud.png) + +相应地，rs_driver将对应的点保存在点云的成员points中，这是一个vector。如下图。 + +![](./img/11_rs_driver_point_cloud_vector.png) + +### 3.4 定义LidarDriver对象 + +LidarDriver类是rs_driver的接口类。 + +这里定义一个LidarDriver的实例。 + +```c++ +int main() +{ + LidarDriver driver; ///< Declare the driver object + ... +} +``` + +### 3.5 配置LidarDriver的参数 + +RSDriverParam定义LidarDriver的参数。 + +这里定义RSDriverParam变量，并配置它。 + ++ `InputType::ONLINE_LIDAR`意味着从在线雷达得到MSOP/DIFOP包。 ++ `LidarType::RS16`是雷达类型 ++ 分别设置接收MSOP/DIFO包的端口号。如果不知道端口是多少，可以用第三方工具（如WireShark）先查看一下。 + +```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 + param.lidar_type = LidarType::RS16; ///< Set the lidar type. Make sure this type is correct + ... +} +``` + +### 3.6 定义和注册点云回调函数 + ++ 如前面章节所说，使用者需要向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包不能及时处理。 + ++ 使用者在自己的线程中，处理点云。 + +```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); + ... +} +``` + +### 3.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); /// &msg) -{ - RS_MSG << "msg: " << msg.seq << " point cloud size: " << msg.point_cloud_ptr->size() << RS_REND; -} -``` - -### 2.3 Define a exception callback function - -Define the exception callback function. When driver want to send out infos or error codes, this function will be called. Same as the previous callback function, please **do not add any time-consuming operations in this callback function!** - -```c++ -void exceptionCallback(const Error &code) -{ - RS_WARNING << "Error code : " << code.toString() << RS_REND; -} -``` - -### 2.4 Instantiate the driver object - -Instanciate a driver object. - -```c++ -LidarDriver driver; ///< Declare the driver object -``` - -### 2.5 Define the parameter, configure the parameter - -Define a parameter object and config it. Since we want to decode pcap bag, please set the ```read_pcap``` to ```true``` and set up the correct pcap file directory. The msop port and difop port number of lidar can be got from wireshark(a network socket capture software). The default value is ```msop-6699```, ```difop-7788```. User also need to make sure the ```lidar_type``` is set correctly. - -```c++ -RSDriverParam param; ///< Create a parameter object -param.input_param.read_pcap = true; ///< Set read_pcap to true -param.input_param.pcap_path = "/home/robosense/rs16.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 -param.lidar_type = LidarType::RS16; ///< Set the lidar type. Make sure this type is correct -``` - -### 2.6 Register the point cloud callback and exception callback - -Register the callback functions we defined in 2.2 and 2.3. **The exception callback function must be registered before the init() function is called because error may occur during the initialization**. - -```c++ -driver.regRecvCallback(pointCloudCallback); ///< Register the point cloud callback function into the driver -driver.regExceptionCallback(exceptionCallback); ///at(i) , the next point on the same ring should be msg.point_cloud_ptr->at(i+msg.height). User can set the parameter ```saved_by_rows``` to ```true``` to make the point cloud stored in **row major order**. - - - -### *Congratulations! You have finished the demo tutorial of RoboSense LiDAR driver! You can find the complete demo code in the demo folder under the project directory. Feel free to connect us if you have any question about the driver.* \ No newline at end of file diff --git a/doc/howto/how_to_decode_pcap_file.md b/doc/howto/how_to_decode_pcap_file.md new file mode 100644 index 00000000..cab17f4a --- /dev/null +++ b/doc/howto/how_to_decode_pcap_file.md @@ -0,0 +1,248 @@ +# How to decode PCAP file + +## 1 Introduction + +Before reading this document, please be sure that you have read [Decode online LiDAR](./how_to_decode_online_lidar.md). + +The PCAP file is captured with some the 3rd party tools, such as Wireshark/tcpdump. + +This document illustrates how to decode it with the driver's API. + +## 2 Steps + +Below is the steps to use the driver's API. + +### 2.1 Define Point Type + +Point is the base unit of Point Cloud. The driver supposes that it may have these member variables. +- x -- float type. The x coordinate of point. +- y -- float type. The y coordinate of point. +- z -- float type. The z coordinate of point. +- intensity -- uint8_t type. The intensity of point. +- timestamp -- double type. The timestamp of point. It may generated by the Lidar or on the host machine. +- ring -- The ring ID of the point, which represents the row number. e.g. For RS80, 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, otherwise the behaviors might be unexpected. + +### 2.2 Define Point Cloud + + The driver allows user to change the point cloud type, as long as it is compatible to the definition below. + + ```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; + }; + + typedef PointXYZI PointT; + typedef PointCloudT PointCloudMsg; + ``` + + Here user may add new members, and change the order of these members, but should not change or remove them. + +### 2.3 Define the driver object + +Define a driver object. + +```c++ +int main() +{ + LidarDriver driver; ///< Declare the driver object + ... +} +``` + +### 2.4 Configure the driver parameter + +Define a RSDriverParam variable and configure it. ++ `InputType::PCAP_FILE` means that the driver get packets from a PCAP file, which is `/home/robosense/lidar.pcap`. ++ `LidarType::RS16` means a RS16 Lidar. ++ Also set the local MSOP/DIFOP ports. Please use a 3rd party tool, such as WireShark, to get them. + +```c++ +int main() +{ + ... + RSDriverParam param; ///< Create a parameter object + param.input_type = InputType::PCAP_FILE; /// get packet from the pcap file + param.input_param.pcap_path = "/home/robosense/lidar.pcap"; ///< Set the pcap file path + 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 + ... +} +``` + +### 2.5 Define and register Point Cloud callbacks + ++ User is supposed to provide free point cloud to the 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(); +} +``` + ++ Also user is supposed to accept stuffed point cloud from the 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; +} +``` + +Note: The driver calls these two callback functions in its `construct_thread`, so **don't do any time-consuming task** in them. + ++ User process point cloud in its own thread. + +```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 the callbacks to the driver. + +```c++ +int main() +{ + ... + driver.regPointCloudCallback(driverReturnPointCloudToCallerCallback, + driverReturnPointCloudToCallerCallback); + ... +} +``` + +### 2.6 Define and register exception callbacks + ++ When an error happens, the driver will inform user. User is supposed to accept it. + +```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; + }; + + typedef PointXYZI PointT; + typedef PointCloudT PointCloudMsg; + ``` + +使用者可以向这个定义增加新成员，改变成员顺序，但是不可以改变成员类型，或者删除成员。 + +### 2.3 定义LidarDriver对象 + +LidarDriver类是rs_driver的接口类。 + +这里定义一个LidarDriver的实例。 + +```c++ +int main() +{ + LidarDriver driver; ///< Declare the driver object + ... +} +``` + +### 2.4 配置LidarDriver的参数 + +RSDriverParam定义LidarDriver的参数。 + +这里定义RSDriverParam变量，并配置它。 + ++ `InputType::PCAP_FILE` 意味着从PCAP文件得到MSOP/DIFOP包。这里的PCAP文件是`/home/robosense/lidar.pcap`。 ++ `LidarType::RS16`是雷达类型 ++ 分别设置接收MSOP/DIFO包的端口号。如果不知道端口是多少，可以用第三方工具（如WireShark）先查看一下。 + +```c++ +int main() +{ + ... + RSDriverParam param; ///< Create a parameter object + param.input_type = InputType::PCAP_FILE; /// get packet from the pcap file + param.input_param.pcap_path = "/home/robosense/lidar.pcap"; ///< Set the pcap file path + 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 + ... +} +``` + +### 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包不能及时处理。 + ++ 使用者在自己的线程中，处理点云。 + +```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); + ... +} +``` + +### 2.6 定义和注册异常回调函数 + ++ 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); /// &msg) -{ - RS_MSG << "msg: " << msg.seq << " point cloud size: " << msg.point_cloud_ptr->size() << RS_REND; -} -``` - -### 2.3 Define a exception callback function - -Define the exception callback function. When driver want to send out infos or error codes, this function will be called. Same as the previous callback function, please **do not add any time-consuming operations in this callback function!** - -```c++ -void exceptionCallback(const Error &code) -{ - RS_WARNING << "Error code : " << code.toString() << RS_REND; -} -``` - -### 2.4 Instantiate the driver object - -Instanciate a driver object. - -```c++ -LidarDriver driver; ///< Declare the driver object -``` - -### 2.5 Define the parameter and configure the parameter - - The msop port and difop port number of lidar can be got from wireshark(a network socket capture software). The default value is ```msop-6699``` and ```difop-7788```. User also need to make sure the ```lidar_type``` is set correctly. - -```c++ -RSDriverParam param; ///< Create a parameter object -param.input_param.msop_port = 6699; ///< Set the lidar msop port number the default 6699 -param.input_param.difop_port = 7788; ///< Set the lidar difop port number the default 7788 -param.lidar_type = LidarType::RS16; ///< Set the lidar type. Make sure this type is correct -``` - -### 2.6 Register the point cloud callback and exception callback - -Register the callback functions we defined in 2.2 and 2.3. **The exception callback function must be registered before the init() function is called because error may occur during the initialization**. - -```c++ -driver.regRecvCallback(pointCloudCallback); ///< Register the point cloud callback function into the driver -driver.regExceptionCallback(exceptionCallback); ///at(i) , the next point on the same ring should be msg.point_cloud_ptr->at(i+msg.height). User can set the parameter ```saved_by_rows``` to ```true``` to make the point cloud stored in **row major order**. - - - -### *Congratulations! You have finished the demo tutorial of RoboSense LiDAR driver! You can find the complete demo code in the demo folder under the project directory. Feel free to connect us if you have any question about the driver.* \ No newline at end of file diff --git a/doc/howto/how_to_transform_pointcloud.md b/doc/howto/how_to_transform_pointcloud.md new file mode 100644 index 00000000..78b4d462 --- /dev/null +++ b/doc/howto/how_to_transform_pointcloud.md @@ -0,0 +1,46 @@ +# How to transform point cloud + +## 1 Introduction + +Before reading this document, please be sure that you have read the [Decode online LiDAR](./how_to_decode_online_lidar.md) or [Decode PCAP file](./how_to_decode_pcap_file.md). + +This document illustrate how to transform the point cloud to a different position with the built-in trasform function. + +The rotation order of the transformation is **yaw - pitch - row**. The unit of x, y, z, is ```m```, and the unit of roll, pitch, yaw, is ```radian```. + +Warning: + +This function is only for test purpose. It costs much CPU resources, so **never, never enable this function in your released products**. + +## 2 Steps + +### 2.1 Compile + +To enable the transformation function, compile the driver with the option ENABLE_TRANSFORM=ON. + +```bash +cmake -DENABLE_TRANSFORM=ON .. +``` + +### 2.2 Config parameters + +Configure the transformation parameters. These parameters' default value is ```0```. + +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/doc/howto/how_to_transform_pointcloud_CN.md b/doc/howto/how_to_transform_pointcloud_CN.md new file mode 100644 index 00000000..e5da8787 --- /dev/null +++ b/doc/howto/how_to_transform_pointcloud_CN.md @@ -0,0 +1,46 @@ +# 如何对点云作坐标转换 + +## 1 简介 + +阅读本文前，请先阅读 [连接在线雷达](./how_to_decode_online_lidar_CN.md) or [解码PCAP文件](./how_to_decode_pcap_file_CN.md)。 + +本文说明如何使用坐标转换功能，将点云变换到不同位置，不同角度上去。 + +进行旋转的顺序依次是**yaw - pitch - row**。x, y, z的单位是```米```, roll, pitch, yaw的单位是```度```。 + +警告： + +对点做坐标转化，需要消耗大量的CPU资源。提供这个功能，仅用于测试目的，所以***一定不要在你的产品中使能它***。 + +## 2 步骤 + +### 2.1 编译 + +要使用坐标转换功能，需要使能编译选项```ENABLE_TRANSFORM=ON```。 + +```bash +cmake -DENABLE_TRANSFORM=ON .. +``` + +### 2.2 配置参数 + +配置坐标转换的参数，这些参数的默认值是```0```。 + +如下的例子中，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/doc/howto/how_to_use_multi_cast_function.md b/doc/howto/how_to_use_multi_cast_function.md deleted file mode 100644 index 6c5ea6de..00000000 --- a/doc/howto/how_to_use_multi_cast_function.md +++ /dev/null @@ -1,40 +0,0 @@ -# How to use multi-cast function - -## 1 Introduction - -This document will show you how to use rs_driver to receive point cloud from the LiDAR working in multi-cast mode. - -## 2 Steps (Linux) - -Suppose the multi-cast address of LiDAR is ```224.1.1.1```, and the host address is ```192.168.1.102```. - -The host address should be in the same network with the Lidar. In the other words, it can ping to the Lidar. - -### 2.1 Set up parameters - -Set up the parameter ```multi_cast_address``` and ```host_address```. Here is an example for online lidar. - -```c++ -RSDriverParam param; ///< Create a parameter object -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.input_param.host_address = "192.168.1.102"; ///< Set the host address. -param.input_param.multi_cast_address = "224.1.1.1"; ///< Set the multi-cast address. -param.lidar_type = LidarType::RS16; ///< Set the lidar type. Make sure this type is correct - -``` - -### 2.4 Run - -Run the program. - - - - - - - - - - - diff --git a/doc/howto/how_to_use_rs_driver_viewer.md b/doc/howto/how_to_use_rs_driver_viewer.md index 2b7dcc25..11041047 100644 --- a/doc/howto/how_to_use_rs_driver_viewer.md +++ b/doc/howto/how_to_use_rs_driver_viewer.md @@ -1,48 +1,54 @@ -# How to use visualization tool +# How to visualize point cloud with rs_driver_viewer ## 1 Introduction -This document will show you how to use the visualization tool to watch point cloud from LiDAR. +The rs_driver_viewer is a visualization tool for the point cloud. This document illustrates how to use it. -## 2 Run +![](./img/14_rs_driver_viewer_point_cloud.png) -If user install the **rs_driver** in the previous step, the tool can be start by the following command: +## 2 Compile and Run + +Compile the driver with the option COMPILE_TOOLS=ON. ```bash -rs_driver_viewer +cmake -DCOMPILE_TOOS=ON .. ``` -Otherwise, the tool need to be start with the absolute path: +Run the tool. ```bash -./rs_driver/build/tool/rs_driver_viewer +./tool/rs_driver_viewer ``` -### 2.1 Arguments +### 2.1 Help Menu -- -h/--help +- -h print the argument menu -- -msop +- -type - Msop port number of LiDAR, the default value is *6699* + LiDAR type, the default value is *RS16* -- -difop +- -pcap - Difop port number of LiDAR, the default value is *7788* + Full path of the PCAP file. If this argument is set, the driver read packets from the pcap file, else from online Lidar. -- -host +- -msop - Host address. + MSOP port number of LiDAR, the default value is *6699* -- -multi_cast +- -difop - Destination multi-cast address. + DIFOP port number of LiDAR, the default value is *7788* + +- -group -- -type + the multicast group address - Typer of LiDAR, the default value is *RS16* +- -host + + the host address - -x @@ -68,33 +74,25 @@ Otherwise, the tool need to be start with the absolute path: Transformation parameter, default is 0, unit: radian -- -pcap - - The absolute path of pcap file. If this argument is set, the driver will work in off-line mode and the pcap file. Otherwise the driver work in online mode. +Note: -**The point cloud transformation function can only be used when the cmake option ENABLE_TRANSFORM is set to ON.** +**The point cloud transformation function is available only if the CMake option ENABLE_TRANSFORM is ON.** ## 3 Examples -- Online decode a RS128 LiDAR, which msop port is ```9966``` and difop port is ```8877``` - - ```bash - rs_driver_viewer -msop 9966 -difop 8877 -type RS128 - ``` - -- Online decode a RSM1 LiDAR, which send to a muti-cast group ```224.1.1.1``` and the host address is ```192.168.1.102```. +- Decode from an online RS128 LiDAR. Its MSOP port is ```9966```, and difop port is ```8877``` ```bash - rs_driver_viewer -type RSM1 -host 192.168.1.102 -multi_cast 224.1.1.1 + rs_driver_viewer -type RS128 -msop 9966 -difop 8877 ``` -- Offline decode a RSHELIOS LiDAR with a pcap file. +- Decode from a PCAP file with RSHELIOS LiDAR data. ```bash - rs_driver_viewer -pcap /home/robosense/helios.pcap -type RSHELIOS + rs_driver_viewer -type RSHELIOS -pcap /home/robosense/helios.pcap ``` -- Online decode a RS16 LiDAR with the coordinate transformation parameter x=1.5, y=2, z=0, roll=1.57, pitch=0, yaw=0 +- 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/doc/howto/how_to_use_rs_driver_viewer_CN.md b/doc/howto/how_to_use_rs_driver_viewer_CN.md new file mode 100644 index 00000000..16e2f4cd --- /dev/null +++ b/doc/howto/how_to_use_rs_driver_viewer_CN.md @@ -0,0 +1,102 @@ +# 如何使用rs_driver_viewer工具可视化点云 + +## 1 简介 + +rs_driver_viewer是rs_driver自带的小工具，可以用于显示点云。 + +本文说明如何使用这个工具。 + +![](./img/14_rs_driver_viewer_point_cloud.png) + +## 2 编译和运行 + +要编译rs_driver_viewer，需要使能编译选项COMPILE_TOOLS=ON。 + +```bash +cmake -DCOMPILE_TOOS=ON .. +``` + +运行rs_driver_viewer。 + +```bash +./tool/rs_driver_viewer +``` + +### 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.** + +## 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/doc/howto/how_to_use_transformation_function.md b/doc/howto/how_to_use_transformation_function.md deleted file mode 100644 index 89b2e090..00000000 --- a/doc/howto/how_to_use_transformation_function.md +++ /dev/null @@ -1,40 +0,0 @@ -# How to use transformation function - -## 1 Introduction - -This document will show you how to use the built in coordinate transformation function. Before reading this document, please make sure you have read the [Online connect LiDAR](how_to_online_use_driver.md) or [Offline decode pcap bag](how_to_offline_decode_pcap.md). - -The rotation order of the transformation is **yaw - pitch - row**. - -The unit of x, y, z, is ```m```, and the unit of roll, pitch, yaw, is ```radian```. - -## 2 Steps - -### 2.1 Enable - -To enable the coordinate transformation function, set the following option to ```ON``` when executing cmake command: - -```bash -cmake -DENABLE_TRANSFORM=ON .. -``` - -### 2.2 Set up parameters - -Set up the transformation parameters. The default value of each parameter is ```0```. Here is an example for offline decoding pcap bag ( 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_param.read_pcap = true; ///< Set read_pcap to true -param.input_param.pcap_path = "/home/robosense/rs16.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 -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/doc/howto/img/11_rs_driver_point_cloud.png b/doc/howto/img/11_rs_driver_point_cloud.png new file mode 100644 index 00000000..9848f484 Binary files /dev/null and b/doc/howto/img/11_rs_driver_point_cloud.png differ diff --git a/doc/howto/img/11_rs_driver_point_cloud_vector.png b/doc/howto/img/11_rs_driver_point_cloud_vector.png new file mode 100644 index 00000000..4abee3f1 Binary files /dev/null and b/doc/howto/img/11_rs_driver_point_cloud_vector.png differ diff --git a/doc/howto/img/11_rs_driver_queue_thread.png b/doc/howto/img/11_rs_driver_queue_thread.png new file mode 100644 index 00000000..feb3eaf2 Binary files /dev/null and b/doc/howto/img/11_rs_driver_queue_thread.png differ diff --git a/doc/howto/img/12_broadcast.png b/doc/howto/img/12_broadcast.png new file mode 100644 index 00000000..ebf2f4cb Binary files /dev/null and b/doc/howto/img/12_broadcast.png differ diff --git a/doc/howto/img/12_multi_lidars_ip.png b/doc/howto/img/12_multi_lidars_ip.png new file mode 100644 index 00000000..48e1e138 Binary files /dev/null and b/doc/howto/img/12_multi_lidars_ip.png differ diff --git a/doc/howto/img/12_multi_lidars_port.png b/doc/howto/img/12_multi_lidars_port.png new file mode 100644 index 00000000..c426ca92 Binary files /dev/null and b/doc/howto/img/12_multi_lidars_port.png differ diff --git a/doc/howto/img/12_multicast.png b/doc/howto/img/12_multicast.png new file mode 100644 index 00000000..fc39a8dc Binary files /dev/null and b/doc/howto/img/12_multicast.png differ diff --git a/doc/howto/img/12_unicast.png b/doc/howto/img/12_unicast.png new file mode 100644 index 00000000..65964a26 Binary files /dev/null and b/doc/howto/img/12_unicast.png differ diff --git a/doc/howto/img/12_user_layer.png b/doc/howto/img/12_user_layer.png new file mode 100644 index 00000000..d7b31a08 Binary files /dev/null and b/doc/howto/img/12_user_layer.png differ diff --git a/doc/howto/img/12_vlan.png b/doc/howto/img/12_vlan.png new file mode 100644 index 00000000..55f224b4 Binary files /dev/null and b/doc/howto/img/12_vlan.png differ diff --git a/doc/howto/img/12_vlan_layer.png b/doc/howto/img/12_vlan_layer.png new file mode 100644 index 00000000..8d678fcc Binary files /dev/null and b/doc/howto/img/12_vlan_layer.png differ diff --git a/doc/img/viewer_menu.png b/doc/howto/img/13_viewer_menu.png similarity index 100% rename from doc/img/viewer_menu.png rename to doc/howto/img/13_viewer_menu.png diff --git a/doc/howto/img/14_rs_driver_viewer_point_cloud.png b/doc/howto/img/14_rs_driver_viewer_point_cloud.png new file mode 100644 index 00000000..9e99ad5f Binary files /dev/null and b/doc/howto/img/14_rs_driver_viewer_point_cloud.png differ diff --git a/doc/howto/online_lidar_advanced_topics.md b/doc/howto/online_lidar_advanced_topics.md new file mode 100644 index 00000000..0f3162d1 --- /dev/null +++ b/doc/howto/online_lidar_advanced_topics.md @@ -0,0 +1,193 @@ +# Online Lidar - Advanced Topics + +## 1 Introduction + + The RoboSense LiDAR may work in unicast/multicast/broadcast mode, with VLAN layer and with user layers. + +This document illustrates how to configure the driver in each case. + +Before reading this document, please be sure that you have read [Decode online LiDAR](./how_to_decode_online_lidar.md). + +## 2 Unicast, Multicast and Broadcast + +### 2.1 Broadcast mode + +The simplest way is broadcast mode. + +The Lidar sends MSOP/DIFOP packets to the host machine (The driver 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/12_broadcast.png) + +Below is how to configure RSDriverParam variable. + +```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::RS32; ///< Set the lidar type. +``` + +### 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/12_unicast.png) + +Below is how to configure the RSDriverParam variable. In fact, it is same with the broadcast case. + +```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::RS32; ///< Set the lidar type. +``` + + +### 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/12_multicast.png) + +Below is how to configure the RSDriverParam variable. + +```c++ +RSDriverParam param; ///< Create a parameter object +param.input_type = InputType::ONLINE_LIDAR; ///< get packet from online lidar +param.input_param.group_address = "224.1.1.1"; ///< Set the multicast group address. +param.input_param.host_address = "192.168.1.102"; ///< Set the host address. +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::RS32; ///< Set the lidar type. Make sure this type is correct +``` + +## 3 Multiple Lidars + +### 3.1 Different remote ports + +If you have two 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/12_multi_lidars_port.png) + +Below is how to configure the RSDriverParam variables. + +```c++ +RSDriverParam param1; ///< Create a parameter object for Lidar 192.168.1.200 +param1.input_type = InputType::ONLINE_LIDAR; ///< get packet from online lidar +param1.input_param.msop_port = 6699; ///< Set the lidar msop port number +param1.input_param.difop_port = 7788; ///< Set the lidar difop port number +param1.lidar_type = LidarType::RS32; ///< Set the lidar type. + +RSDriverParam param2; ///< Create a parameter object for Lidar 192.168.1.201 +param2.input_type = InputType::ONLINE_LIDAR; ///< get packet from online lidar +param2.input_param.msop_port = 5599; ///< Set the lidar msop port number +param2.input_param.difop_port = 6688; ///< Set the lidar difop port number +param2.lidar_type = LidarType::RS32; ///< Set the lidar type. +``` + +### 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/12_multi_lidars_ip.png) + +Below is how to configure the RSDriverParam variables. + +```c++ +RSDriverParam param1; ///< Create a parameter object for Lidar 192.168.1.200 +param1.input_type = InputType::ONLINE_LIDAR; ///< get packet from online lidar +param1.input_param.host_address = "192.168.1.102"; ///< Set the host address. +param1.input_param.msop_port = 6699; ///< Set the lidar msop port number +param1.input_param.difop_port = 7788; ///< Set the lidar difop port number +param1.lidar_type = LidarType::RS32; ///< Set the lidar type. + +RSDriverParam param2; ///< Create a parameter object for Lidar 192.168.1.201 +param2.input_type = InputType::ONLINE_LIDAR; ///< get packet from online lidar +param2.input_param.host_address = "192.168.1.103"; ///< Set the host address. +param2.input_param.msop_port = 6699; ///< Set the lidar msop port number +param2.input_param.difop_port = 7788; ///< Set the lidar difop port number +param2.lidar_type = LidarType::RS32; ///< Set the lidar type. +``` + +## 4 VLAN + +In some user cases, The Lidar may work on VLAN. Its packets have a VLAN layer. + +![](./img/12_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/12_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. + +```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::RS32; ///< Set the lidar type. +``` + +## 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/12_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. + +```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.input_param.user_layer_bytes = 8; ///< user layer bytes. there is no user layer if it is 0 +param.input_param.tail_layer_bytes = 4; ///< tail layer bytes. there is no user layer if it is 0 +param.lidar_type = LidarType::RS32; ///< Set the lidar type. +``` + + + + + + + + + + + + + + + diff --git a/doc/howto/online_lidar_advanced_topics_CN.md b/doc/howto/online_lidar_advanced_topics_CN.md new file mode 100644 index 00000000..691043a4 --- /dev/null +++ b/doc/howto/online_lidar_advanced_topics_CN.md @@ -0,0 +1,202 @@ +# 在线雷达 - 高级主题 + +## 1 简介 + +RoboSense雷达可以工作在单播/组播/广播模式下，也可以工作在VLAN环境下，也可以加入用户自己的层。 + +本文说明了在每种场景下如何配置rs_driver的参数。 + +阅读本文之前，请先阅读 [连接在线雷达](./how_to_decode_online_lidar_CN.md). + +## 2 单播、组播、广播 + +### 2.1 广播模式 + +广播模式的配置最简单。 + +下面的图中，雷达发送MSOP/DIFOP包到主机，rs_driver运行在主机上。为了简化，图中没有画DIFOP端口。 ++ 雷达发送到 `255.255.255.255` : `6699`, rs_driver绑定到端口`6699`. + +![](./img/12_broadcast.png) + +如下代码配置RSDriverParam。 + +```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::RS32; ///< Set the lidar type. +``` + +### 2.2 单播模式 + +为减少网络流量，推荐使用单播模式。 + +如下的例子中， + ++ 雷达发送到`192.168.1.102` : `6699`, rs_driver绑定到端口`6699`。 + +![](./img/12_unicast.png) + +如下代码配置RSDriverParam。它与广播模式的配置完全相同。 + +```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::RS32; ///< Set the lidar type. +``` + + +### 2.3 组播模式 + +雷达也可以工作在组播模式。 + +如下的例子中， + ++ 雷达发送到`224.1.1.1`:`6699` ++ rs_driver绑定到端口`6699`。 rs_driver让本地网卡加入组播组`224.1.1.1`. 这个网卡的地址是`192.168.1.102`。 + +![](./img/12_multicast.png) + +如下代码配置RSDriverParam。 + +```c++ +RSDriverParam param; ///< Create a parameter object +param.input_type = InputType::ONLINE_LIDAR; ///< get packet from online lidar +param.input_param.group_address = "224.1.1.1"; ///< Set the multicast group address. +param.input_param.host_address = "192.168.1.102"; ///< Set the host address. +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::RS32; ///< Set the lidar type. Make sure this type is correct +``` + +## 3 多雷达的情况 + +### 3.1 雷达目的端口不同 + +如果要接入两个雷达，推荐给它们配置不同的目的端口。 + +如下的例子中， + ++ 第一个雷达发送到`192.168.1.102`:`6699`，rs_driver的第一个实例绑定到端口`6699`。 ++ 第二个雷达发送到`192.168.1.102`:`5599`，rs_driver的第二个实例绑定到端口`5599`。 + +![](./img/12_multi_lidars_port.png) + + + +```c++ +RSDriverParam param1; ///< Create a parameter object for Lidar 192.168.1.200 +param1.input_type = InputType::ONLINE_LIDAR; ///< get packet from online lidar +param1.input_param.msop_port = 6699; ///< Set the lidar msop port number +param1.input_param.difop_port = 7788; ///< Set the lidar difop port number +param1.lidar_type = LidarType::RS32; ///< Set the lidar type. + +RSDriverParam param2; ///< Create a parameter object for Lidar 192.168.1.201 +param2.input_type = InputType::ONLINE_LIDAR; ///< get packet from online lidar +param2.input_param.msop_port = 5599; ///< Set the lidar msop port number +param2.input_param.difop_port = 6688; ///< Set the lidar difop port number +param2.lidar_type = LidarType::RS32; ///< Set the lidar type. +``` + +### 3.2 雷达的目的IP不同 + +虽然不推荐，也可以给接入的两个雷达配置不同的目的IP。 ++ 主机有两个网卡，地址分别是`192.168.1.102` 和`192.168.1.103`。 ++ 第一个雷达发送到`192.168.1.102`:`6699`，第一个rs_driver实例绑定到`192.168.1.102:6699`。 ++ 第二个雷达发送到`192.168.1.103`:`6699`，第二个rs_driver实例绑定到`192.168.1.103:6699`。 + +![](./img/12_multi_lidars_ip.png) + +如下代码分别配置两个rs_driver实例的RSDriverParam。 + +```c++ +RSDriverParam param1; ///< Create a parameter object for Lidar 192.168.1.200 +param1.input_type = InputType::ONLINE_LIDAR; ///< get packet from online lidar +param1.input_param.host_address = "192.168.1.102"; ///< Set the host address. +param1.input_param.msop_port = 6699; ///< Set the lidar msop port number +param1.input_param.difop_port = 7788; ///< Set the lidar difop port number +param1.lidar_type = LidarType::RS32; ///< Set the lidar type. + +RSDriverParam param2; ///< Create a parameter object for Lidar 192.168.1.201 +param2.input_type = InputType::ONLINE_LIDAR; ///< get packet from online lidar +param2.input_param.host_address = "192.168.1.103"; ///< Set the host address. +param2.input_param.msop_port = 6699; ///< Set the lidar msop port number +param2.input_param.difop_port = 7788; ///< Set the lidar difop port number +param2.lidar_type = LidarType::RS32; ///< Set the lidar type. +``` + +## 4 VLAN + +有些场景下，雷达可以工作在VLAN环境下。这时MSOP/DIFOP包带VLAN层，如下图。 + +![](./img/12_vlan_layer.png) + +rs_driver工作在应用层，接触不到VLAN层。这时需要用户创建一个虚拟网卡来剥除VLAN层。 + +如下是一个例子。 ++ 给雷达分配的VLAN ID是`80`。雷达发送到`192.168.1.102` : `6699`。 发送的包带VLAN层。 ++ 主机上装的物理网卡eno1也在VLAN ID `80`上，它接收雷达发出的带VLAN层的包。 + +![](./img/12_vlan.png) + +要剥除VLAN层，需要用户手工创建一个虚拟网卡。如下的命令，在物理网卡eno1上创建虚拟网卡`eno1.80`，并给它指定IP地址`192.168.1.102` 。 + +``` +sudo apt-get install vlan -y +sudo modprobe 8021q + +sudo vconfig add eno1 80 +sudo ifconfig eno1.80 192.168.1.102 up +``` + +现在rs_driver就可以从eno1.80网卡上接收MSOP/DIFOP包了，这些包不带VLAN层。 + +如下代码配置RSDriverParam。 + +```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::RS32; ///< Set the lidar type. +``` + +## 5 User Layer, Tail Layer + +某些场景下，用户可能在MSOP/DIFOP数据前后加入自己的层。 ++ USER_LAYER 在MSOP/DIFOP数据之前，TAIL_LAYER在MSOP/DIFOP数据之后。 + +![](./img/12_user_layer.png) + +这些层是UDP数据的一部分，所以rs_driver可以自己剥除他们。只需要告诉rs_driver每个层的字节数就可以。 + +如下的例子中，指定USER_LAYER为8字节，TAIL_LAYER为4字节。 + +```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.input_param.user_layer_bytes = 8; ///< user layer bytes. there is no user layer if it is 0 +param.input_param.tail_layer_bytes = 4; ///< tail layer bytes. there is no user layer if it is 0 +param.lidar_type = LidarType::RS32; ///< Set the lidar type. +``` + + + + + + + + + + + + + + + diff --git a/doc/howto/pcap_file_advanced_topics.md b/doc/howto/pcap_file_advanced_topics.md new file mode 100644 index 00000000..0f70dd77 --- /dev/null +++ b/doc/howto/pcap_file_advanced_topics.md @@ -0,0 +1,85 @@ +# PCAP file - Advanced Topics + +## 1 Introduction + + The RoboSense LiDAR may work in unicast/multicast/broadcast mode, with VLAN layer and with user layers. + +This document illustrates how to configure the driver in each case. + +Before reading this document, please be sure that you have read [Online LiDAR - Advanced Topics](./online_lidar_advanced_topics.md). + +## 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. + +```c++ +RSDriverParam param; ///< Create a parameter object +param.input_type = InputType::PCAP_FILE; ///< get packet from online lidar +param.input_param.pcap_path = "/home/robosense/lidar.pcap"; ///< Set the pcap file path +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::RS32; ///< Set the lidar type. +``` + +The only exception is "Multiple Lidars with same ports but different IPs", which is not supported now. + +## 3 VLAN + +In some user cases, The LiDar may work on VLAN. Its packets have a VLAN layer. + +![](./img/12_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`. + +```c++ +RSDriverParam param; ///< Create a parameter object +param.input_type = InputType::PCAP_FILE; ///< get packet from online lidar +param.input_param.pcap_path = "/home/robosense/lidar.pcap"; ///< Set the pcap file path +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.input_param.use_vlan = true; ///< Whether to use VLAN layer. +param.lidar_type = LidarType::RS32; ///< Set the lidar type. +``` + +## 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/12_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. + +```c++ +RSDriverParam param; ///< Create a parameter object +param.input_type = InputType::PCAP_FILE; ///< get packet from online lidar +param.input_param.pcap_path = "/home/robosense/lidar.pcap"; ///< Set the pcap file path +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.input_param.user_layer_bytes = 8; ///< user layer bytes. there is no user layer if it is 0 +param.input_param.tail_layer_bytes = 4; ///< tail layer bytes. there is no user layer if it is 0 +param.lidar_type = LidarType::RS32; ///< Set the lidar type. +``` + + + + + + + + + + + + + + + diff --git a/doc/howto/pcap_file_advanced_topics_CN.md b/doc/howto/pcap_file_advanced_topics_CN.md new file mode 100644 index 00000000..e1cd201c --- /dev/null +++ b/doc/howto/pcap_file_advanced_topics_CN.md @@ -0,0 +1,83 @@ +# PCAP文件 - 高级主题 + +## 1 简介 + +RoboSense雷达可以工作在单播/组播/广播模式下，也可以工作在VLAN环境下，也可以加入用户自己的层。 + +本文说明了在每种场景下如何配置rs_driver的参数。 + +阅读本文之前，请先阅读 [在线雷达-高级主题](./online_lidar_advanced_topics_CN.md). + +## 2 一般场景 + +在下列场景下，使用如下配置代码解码PCAP文件。 ++ 广播/组播/单播模式 ++ PCAP文件中有多个雷达 + +```c++ +RSDriverParam param; ///< Create a parameter object +param.input_type = InputType::PCAP_FILE; ///< get packet from online lidar +param.input_param.pcap_path = "/home/robosense/lidar.pcap"; ///< Set the pcap file path +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::RS32; ///< Set the lidar type. +``` + +一个例外是：PCAP文件中有多个雷达数据，但这些雷达目的端口相同，使用不同的目的IP地址来区分。这种情况不支持。 + +## 3 VLAN + +有些场景下，雷达工作在VLAN环境下。这时MSOP/DIFOP包带VLAN层，如下图。 + +![](./img/12_vlan_layer.png) + +rs_driver使用libpcap库解析PCAP文件，可以得到完整的、包括VLAN层的MSOP/DIFOP包。 + +要剥除VLAN层，只需要设置`use_vlan=true`。 + +```c++ +RSDriverParam param; ///< Create a parameter object +param.input_type = InputType::PCAP_FILE; ///< get packet from online lidar +param.input_param.pcap_path = "/home/robosense/lidar.pcap"; ///< Set the pcap file path +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.input_param.use_vlan = true; ///< Whether to use VLAN layer. +param.lidar_type = LidarType::RS32; ///< Set the lidar type. +``` + +## 4 User Layer, Tail Layer + +某些场景下，用户可能在MSOP/DIFOP数据前后加入自己的层。 ++ USER_LAYER 在MSOP/DIFOP数据之前，TAIL_LAYER在MSOP/DIFOP数据之后。 + +![](./img/12_user_layer.png) + +这些层是UDP数据的一部分，所以rs_driver可以自己剥除他们。只需要告诉它每个层的字节数就可以。 + +如下的例子中，指定USER_LAYER为8字节，TAIL_LAYER为4字节。 + +```c++ +RSDriverParam param; ///< Create a parameter object +param.input_type = InputType::PCAP_FILE; ///< get packet from online lidar +param.input_param.pcap_path = "/home/robosense/lidar.pcap"; ///< Set the pcap file path +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.input_param.user_layer_bytes = 8; ///< user layer bytes. there is no user layer if it is 0 +param.input_param.tail_layer_bytes = 4; ///< tail layer bytes. there is no user layer if it is 0 +param.lidar_type = LidarType::RS32; ///< Set the lidar type. +``` + + + + + + + + + + + + + + + diff --git a/doc/intro/parameter_intro.md b/doc/intro/parameter_intro.md new file mode 100644 index 00000000..6924d65c --- /dev/null +++ b/doc/intro/parameter_intro.md @@ -0,0 +1,159 @@ +# Introduction to rs_driver's Parameters + + +## 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, and RSInputParam. + +## 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 + + There are two classes of Lidars: Mechanical Lidar and MEMS Lidar. Some paramters of RSDecoderParam is only for mechanical Lidars. + +```c++ +enum LidarType +{ + RS16 = 1, + RS32, + RSBP, + RS128, + RS80, + RSHELIOS, + RSROCK, + RSM1 +}; +``` + ++ input_type - Where 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 used for recording/replaying packets. + +```c++ +enum InputType +{ + ONLINE_LIDAR = 1, + PCAP_FILE, + RAW_PACKET +}; +``` + + +## 3 RSDecoderParam + +RSDecoderParam specifies how rs_driver decode Lidar's packets. + +```c++ +typedef struct RSDecoderParam +{ + bool use_lidar_clock = false; + bool dense_points = false; + bool ts_first_point = false; + bool wait_for_difop = true; + RSTransformParam transform_param; + bool config_from_file = false; + std::string angle_path = ""; + float min_distance = 0.2f; + float max_distance = 200.0f; + + // The following parameters are only for mechanical Lidars. + SplitFrameMode split_frame_mode = SplitFrameMode::SPLIT_BY_ANGLE; + float split_angle = 0.0f; + uint16_t num_blks_split = 1; + float start_angle = 0.0f; + float end_angle = 360.0f; + +} RSDecoderParam; +``` + +The following parameters are for all Lidars。 ++ 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; +``` + ++ config_from_file - Where to get Lidar config parameters. From extern files, or from DIFOP packet. Internal use only. ++ angle_path - File of angle calibration. Internal use only. ++ min_distance、max_distance - Measurement Range. Internal use only. + +The following paramters are only for mechanical Lidars. ++ split_frame_mode - How to split frame. + + `SPLIT_BY_ANGLE` is by user requested angle;`SPLIT_BY_FIXED_BLKS` is by blocks/round theologically; `SPLIT_BY_CUSTOM_BLKS` is by user requested blocks/frame. `SPLIT_BY_ANGLE` is default, and the other two values are not suggested. + +```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/frame. + ++ 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`). + +## 4 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. + +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. + +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; + uint16_t difop_port = 7788; + std::string host_address = "0.0.0.0"; + std::string group_address = "0.0.0.0"; + + // The following parameters are only for PCAP_FILE + std::string pcap_path = ""; + bool pcap_repeat = true; + float pcap_rate = 1.0; + bool use_vlan = false; +} RSInputParam; + +``` diff --git a/doc/intro/parameter_intro_CN.md b/doc/intro/parameter_intro_CN.md new file mode 100644 index 00000000..1c17fd69 --- /dev/null +++ b/doc/intro/parameter_intro_CN.md @@ -0,0 +1,157 @@ +# rs_driver 配置参数介绍 + +## 1 参数文件 + +文件`rs_driver/src/rs_driver/driver_param.h`中， 定义了rs_driver的配置参数。 + +rs_driver的参数主要保存在RSDriverParam、RSDecoderParam、和RSInputParam三个结构中。 + +## 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 +{ + RS16 = 1, + RS32, + RSBP, + RS128, + RS80, + RSHELIOS, + RSROCK, + RSM1 +}; +``` + ++ 成员input_type - 指定雷达的数据从哪里来。 + + ONLINE_LIDAR是在线雷达；PCAP_FILE是PCAP文件，使用第三方工具抓包得到；RAW_PACKET用于Packet录制与回放。 + +```c++ +enum InputType +{ + ONLINE_LIDAR = 1, + PCAP_FILE, + RAW_PACKET +}; +``` + + +## 3 RSDecoderParam + +RSDecoderParam指定雷达的Packet解析器如何工作。 + +```c++ +typedef struct RSDecoderParam +{ + bool use_lidar_clock = false; + bool dense_points = false; + bool ts_first_point = false; + bool wait_for_difop = true; + RSTransformParam transform_param; + bool config_from_file = false; + std::string angle_path = ""; + float min_distance = 0.2f; + float max_distance = 200.0f; + + // The following parameters are only for mechanical Lidars. + SplitFrameMode split_frame_mode = SplitFrameMode::SPLIT_BY_ANGLE; + float split_angle = 0.0f; + uint16_t num_blks_split = 1; + float start_angle = 0.0f; + float end_angle = 360.0f; +} RSDecoderParam; +``` + +如下参数针对所有雷达。 ++ use_lidar_clock - 指定使用雷达还是主机的时间戳。 + + 如果`use_lidar_clock`=`true`，则使用雷达写入Packet中的时间戳；如果`use_lidar_clock`=`false`,则使用rs_driver在主机端产生的时间戳。 ++ dense_points - 指定点云是否是dense的。 + + 如果`dense_points`=`false`, 则点云中包含NAN点；如果`dense_points`=`true`,则去除点云中的NAN点。 ++ ts_first_point - 指定点云的时间戳来自点云的第一个点，还是最后第一个点。 + + 如果`ts_first_point`=`true`, 则第一个点的时间作为点云的时间戳；如果`ts_first_point`=`false`,则最后一个点的时间作为点云的时间戳。 ++ 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 - 雷达的角度校准参数文件。仅内部调试使用。 ++ min_distance、max_distance - 测距的最大、最小值。仅内部调试使用。 + +如下参数仅针对机械式雷达。 ++ 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数。 + ++ start_angle、end_angle - 机械式雷达一般输出的点云的水平角在[`0`, `360`]之间，这里可以指定一个更小的范围[`start_angle`, `end_angle`)。 + +## 4 RSInputParam + +RSInputParam指定rs_driver得到有关雷达的Packet源的参数。 + +如下参数针对`ONLINE_LIDAR`和`PCAP_FILE`。 ++ msop_port - 指定主机上的本地UDP端口，接收MSOP Packet ++ difop_port - 指定主机上的本地UDP端口，接收DIFOP Packet + +如下参数仅针对ONLINE_LIDAR。 ++ host_address - 指定本地的IP地址，接收MSOP/DIFOP Packet ++ group_address - 指定一个组播组。rs_driver将`host_address`指定的网卡加入这个组播组，以便接收MSOP/DIFOP Packet。 + +如下参数仅针对PCAP_FILE。 ++ pcap_path - PCAP文件的全路径 ++ pcap_repeat - 指定是否重复播放PCAP文件 ++ pcap_rate - rs_driver按`理论上的雷达帧率`播放PCAP文件。`pcap_rate`可以在这个速度上指定一个比例值，加快/放慢播放速度。 ++ use_vlan - 如果PCAP文件中的Packet包含VLAN层，可以指定`use_vlan`=`true`，跳过这一层。 + +```c++ +typedef struct RSInputParam +{ + uint16_t msop_port = 6699; + uint16_t difop_port = 7788; + std::string host_address = "0.0.0.0"; + std::string group_address = "0.0.0.0"; + + // The following parameters are only for PCAP_FILE + std::string pcap_path = ""; + bool pcap_repeat = true; + float pcap_rate = 1.0; + bool use_vlan = false; +} RSInputParam; + +``` diff --git a/doc/intro/parameter_intro_cn.md b/doc/intro/parameter_intro_cn.md deleted file mode 100644 index bd2d3f23..00000000 --- a/doc/intro/parameter_intro_cn.md +++ /dev/null @@ -1,58 +0,0 @@ -# 参数介绍 -## 1 参数定义 -参数文件`rs_driver/src/rs_driver/driver_param.h`里面详细定义了每一个参数，如下。 - -``` -typedef 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 -} RSTransformParam; - -typedef struct RSDecoderParam ///< LiDAR decoder parameter -{ - float max_distance = 200.0f; ///< Max distance of point cloud range - float min_distance = 0.2f; ///< Minimum distance of point cloud range - 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 cut_angle; - ///< 2: Split frames by fixed number of packets; - ///< 3: Split frames by custom number of packets (num_pkts_split) - uint32_t num_pkts_split = 1; ///< Number of packets in one frame, only be used when split_frame_mode=3 - float cut_angle = 0.0f; ///< Cut angle(degree) used to split frame, only be used when split_frame_mode=1 - bool use_lidar_clock = false; ///< true: use LiDAR clock as timestamp; false: use system clock as timestamp - RSTransformParam transform_param; ///< Used to transform points - RSCameraTriggerParam trigger_param; ///< Used to trigger camera -} RSDecoderParam; - -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 - 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 -} RSInputParam; - -typedef struct RSDriverParam ///< The LiDAR driver parameter -{ - 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) -} RSDriverParam; -``` diff --git a/doc/intro/parameter_intro_en.md b/doc/intro/parameter_intro_en.md deleted file mode 100644 index a9a84fa3..00000000 --- a/doc/intro/parameter_intro_en.md +++ /dev/null @@ -1,58 +0,0 @@ -# Parameters Intorduction -## 1 Parameter Definition -The detail parameters are defined in `rs_driver/src/rs_driver/driver_param.h` file. - -``` -typedef 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 -} RSTransformParam; - -typedef struct RSDecoderParam ///< LiDAR decoder parameter -{ - float max_distance = 200.0f; ///< Max distance of point cloud range - float min_distance = 0.2f; ///< Minimum distance of point cloud range - 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 cut_angle; - ///< 2: Split frames by fixed number of packets; - ///< 3: Split frames by custom number of packets (num_pkts_split) - uint32_t num_pkts_split = 1; ///< Number of packets in one frame, only be used when split_frame_mode=3 - float cut_angle = 0.0f; ///< Cut angle(degree) used to split frame, only be used when split_frame_mode=1 - bool use_lidar_clock = false; ///< true: use LiDAR clock as timestamp; false: use system clock as timestamp - RSTransformParam transform_param; ///< Used to transform points - RSCameraTriggerParam trigger_param; ///< Used to trigger camera -} RSDecoderParam; - -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 - 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 -} RSInputParam; - -typedef struct RSDriverParam ///< The LiDAR driver parameter -{ - 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) -} RSDriverParam; -``` diff --git a/doc/src_intro/img/.xdp_01_components.QEQTH1 b/doc/src_intro/img/.xdp_01_components.QEQTH1 new file mode 100644 index 00000000..6d487375 Binary files /dev/null and b/doc/src_intro/img/.xdp_01_components.QEQTH1 differ diff --git a/doc/src_intro/img/.xdp_components.8OXVH1 b/doc/src_intro/img/.xdp_components.8OXVH1 new file mode 100644 index 00000000..c50b6274 Binary files /dev/null and b/doc/src_intro/img/.xdp_components.8OXVH1 differ diff --git a/doc/src_intro/img/class_azimuth_section.png b/doc/src_intro/img/class_azimuth_section.png new file mode 100644 index 00000000..9334b3ff Binary files /dev/null and b/doc/src_intro/img/class_azimuth_section.png differ diff --git a/doc/src_intro/img/class_chan_angles.png b/doc/src_intro/img/class_chan_angles.png new file mode 100644 index 00000000..e965bd92 Binary files /dev/null and b/doc/src_intro/img/class_chan_angles.png differ diff --git a/doc/src_intro/img/class_decoder.png b/doc/src_intro/img/class_decoder.png new file mode 100644 index 00000000..a2c723e8 Binary files /dev/null and b/doc/src_intro/img/class_decoder.png differ diff --git a/doc/src_intro/img/class_decoder_factory.png b/doc/src_intro/img/class_decoder_factory.png new file mode 100644 index 00000000..4c9d83b6 Binary files /dev/null and b/doc/src_intro/img/class_decoder_factory.png differ diff --git a/doc/src_intro/img/class_decoder_mech.png b/doc/src_intro/img/class_decoder_mech.png new file mode 100644 index 00000000..fe543cde Binary files /dev/null and b/doc/src_intro/img/class_decoder_mech.png differ diff --git a/doc/src_intro/img/class_decoder_rsbp.png b/doc/src_intro/img/class_decoder_rsbp.png new file mode 100644 index 00000000..257b3ec7 Binary files /dev/null and b/doc/src_intro/img/class_decoder_rsbp.png differ diff --git a/doc/src_intro/img/class_decoder_rsm1.png b/doc/src_intro/img/class_decoder_rsm1.png new file mode 100644 index 00000000..c63a8534 Binary files /dev/null and b/doc/src_intro/img/class_decoder_rsm1.png differ diff --git a/doc/src_intro/img/class_distance_section.png b/doc/src_intro/img/class_distance_section.png new file mode 100644 index 00000000..35f2624c Binary files /dev/null and b/doc/src_intro/img/class_distance_section.png differ diff --git a/doc/src_intro/img/class_input.png b/doc/src_intro/img/class_input.png new file mode 100644 index 00000000..42e36655 Binary files /dev/null and b/doc/src_intro/img/class_input.png differ diff --git a/doc/src_intro/img/class_input_factory.png b/doc/src_intro/img/class_input_factory.png new file mode 100644 index 00000000..53af18ab Binary files /dev/null and b/doc/src_intro/img/class_input_factory.png differ diff --git a/doc/src_intro/img/class_input_pcap.png b/doc/src_intro/img/class_input_pcap.png new file mode 100644 index 00000000..808935a8 Binary files /dev/null and b/doc/src_intro/img/class_input_pcap.png differ diff --git a/doc/src_intro/img/class_input_raw.png b/doc/src_intro/img/class_input_raw.png new file mode 100644 index 00000000..2a2a2ae9 Binary files /dev/null and b/doc/src_intro/img/class_input_raw.png differ diff --git a/doc/src_intro/img/class_input_sock.png b/doc/src_intro/img/class_input_sock.png new file mode 100644 index 00000000..e48b2ee2 Binary files /dev/null and b/doc/src_intro/img/class_input_sock.png differ diff --git a/doc/src_intro/img/class_lidar_driver_impl.png b/doc/src_intro/img/class_lidar_driver_impl.png new file mode 100644 index 00000000..731eab8a Binary files /dev/null and b/doc/src_intro/img/class_lidar_driver_impl.png differ diff --git a/doc/src_intro/img/class_trigon.png b/doc/src_intro/img/class_trigon.png new file mode 100644 index 00000000..99b6972f Binary files /dev/null and b/doc/src_intro/img/class_trigon.png differ diff --git a/doc/src_intro/img/classes_block_iterator.png b/doc/src_intro/img/classes_block_iterator.png new file mode 100644 index 00000000..2dec796a Binary files /dev/null and b/doc/src_intro/img/classes_block_iterator.png differ diff --git a/doc/src_intro/img/classes_decoder.png b/doc/src_intro/img/classes_decoder.png new file mode 100644 index 00000000..228f9bf2 Binary files /dev/null and b/doc/src_intro/img/classes_decoder.png differ diff --git a/doc/src_intro/img/classes_input.png b/doc/src_intro/img/classes_input.png new file mode 100644 index 00000000..89be452b Binary files /dev/null and b/doc/src_intro/img/classes_input.png differ diff --git a/doc/src_intro/img/classes_rs16_block_iterator.png b/doc/src_intro/img/classes_rs16_block_iterator.png new file mode 100644 index 00000000..61a4f188 Binary files /dev/null and b/doc/src_intro/img/classes_rs16_block_iterator.png differ diff --git a/doc/src_intro/img/classes_split_strategy.png b/doc/src_intro/img/classes_split_strategy.png new file mode 100644 index 00000000..d83939f6 Binary files /dev/null and b/doc/src_intro/img/classes_split_strategy.png differ diff --git a/doc/src_intro/img/classes_split_strategy_by_seq.png b/doc/src_intro/img/classes_split_strategy_by_seq.png new file mode 100644 index 00000000..89c76dcf Binary files /dev/null and b/doc/src_intro/img/classes_split_strategy_by_seq.png differ diff --git a/doc/src_intro/img/components.png b/doc/src_intro/img/components.png new file mode 100644 index 00000000..f71cd3b0 Binary files /dev/null and b/doc/src_intro/img/components.png differ diff --git a/doc/src_intro/img/fov.png b/doc/src_intro/img/fov.png new file mode 100644 index 00000000..693a49e8 Binary files /dev/null and b/doc/src_intro/img/fov.png differ diff --git a/doc/src_intro/img/msop_dual_return.png b/doc/src_intro/img/msop_dual_return.png new file mode 100644 index 00000000..17688a59 Binary files /dev/null and b/doc/src_intro/img/msop_dual_return.png differ diff --git a/doc/src_intro/img/msop_ruby128.png b/doc/src_intro/img/msop_ruby128.png new file mode 100644 index 00000000..37aceae9 Binary files /dev/null and b/doc/src_intro/img/msop_ruby128.png differ diff --git a/doc/src_intro/img/msop_single_return.png b/doc/src_intro/img/msop_single_return.png new file mode 100644 index 00000000..036fbb5a Binary files /dev/null and b/doc/src_intro/img/msop_single_return.png differ diff --git a/doc/src_intro/img/packet_layers.png b/doc/src_intro/img/packet_layers.png new file mode 100644 index 00000000..acc512ff Binary files /dev/null and b/doc/src_intro/img/packet_layers.png differ diff --git a/doc/src_intro/img/packet_layers_full.png b/doc/src_intro/img/packet_layers_full.png new file mode 100644 index 00000000..6ff501b9 Binary files /dev/null and b/doc/src_intro/img/packet_layers_full.png differ diff --git a/doc/src_intro/img/packet_record_replay.png b/doc/src_intro/img/packet_record_replay.png new file mode 100644 index 00000000..726cd392 Binary files /dev/null and b/doc/src_intro/img/packet_record_replay.png differ diff --git a/doc/src_intro/img/rs16_msop_dual_return.png b/doc/src_intro/img/rs16_msop_dual_return.png new file mode 100644 index 00000000..36ef680d Binary files /dev/null and b/doc/src_intro/img/rs16_msop_dual_return.png differ diff --git a/doc/src_intro/img/rs16_msop_single_return.png b/doc/src_intro/img/rs16_msop_single_return.png new file mode 100644 index 00000000..152dae05 Binary files /dev/null and b/doc/src_intro/img/rs16_msop_single_return.png differ diff --git a/doc/src_intro/img/safe_range.png b/doc/src_intro/img/safe_range.png new file mode 100644 index 00000000..8950f219 Binary files /dev/null and b/doc/src_intro/img/safe_range.png differ diff --git a/doc/src_intro/img/split_angle.png b/doc/src_intro/img/split_angle.png new file mode 100644 index 00000000..ad69e3c4 Binary files /dev/null and b/doc/src_intro/img/split_angle.png differ diff --git a/doc/src_intro/img/split_position.png b/doc/src_intro/img/split_position.png new file mode 100644 index 00000000..85458cd3 Binary files /dev/null and b/doc/src_intro/img/split_position.png differ diff --git a/doc/src_intro/img/trigon_sinss.png b/doc/src_intro/img/trigon_sinss.png new file mode 100644 index 00000000..382f3cc1 Binary files /dev/null and b/doc/src_intro/img/trigon_sinss.png differ diff --git a/doc/src_intro/rs_driver_intro_CN.md b/doc/src_intro/rs_driver_intro_CN.md new file mode 100644 index 00000000..3df7c1ad --- /dev/null +++ b/doc/src_intro/rs_driver_intro_CN.md @@ -0,0 +1,1512 @@ +# rs_driver v1.5.7 源代码解析 + +## 1 基本概念 + +### 1.1 机械式雷达、MEMS雷达 + +rs_driver支持RoboSense的两种雷达： ++ 机械式雷达。如RS16/RS32/RSBP/RSHELIOS/RS80/RS128。机械式雷达有控制激光发射角度的旋转部件，有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，输出给电脑主机。 ++ UCWP (User Configuration Write Protocol)。用户可以修改激光雷达的某些配置参数。 + +rs_driver处理前两类协议的包，也就是MSOP Packet和DIFOP Packet。 + +一般来说，激光雷达与电脑主机通过以太网连接，使用UDP协议。MSOP/DIFOP的格式，不同的雷达可能有较大差异。 + +### 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。后者再将点云传递给用户。 + +## 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 Packet发送到不同的目的Port，所以InputSock创建两个Socket来分别接收它们。 + + 成员变量`fds_[2]`保存这两个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 | host_address | group_address | | +|:-----------:|:----------------------:|:-----------------:|:-------------| +| 6699/7788 | 0.0.0.0 | 0.0.0.0 | 雷达的目的地址可以为广播地址、或电脑主机地址 | +| 6699/7788 | 192.168.1.201 | 0.0.0.0 | 雷达的目的地址可以为电脑主机地址 | +| 6699/7788 | 192.168.1.201 | 239.255.0.1 | 雷达的目的地址可以为组播地址、或电脑主机地址 | + +#### 3.2.2 InputSock::init() + +init() 调用createSocket()，创建两个Socket,分别接收MSOP Packet和DIFOP 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_[2]`中的两个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 | 说明 | +|:-----------:|:-------------:|:-------------| +| 0 | 0 | 如果PCAP文件中只包含一个雷达的Packet | +| 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 Packet的PCAP过滤器。 + + 如果它们使用不同端口，则需要两个过滤器，分别保存在`mosp_filter_`和`difop_filter_`中。 + + 如果使用同一端口，那么`difop_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 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格式中，雷达温度值的解析度。 + +``` +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; +}; +``` + +##### 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::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/雷达的类型如下。 + +``` +num LidarType +{ + RS16 = 1, + RS32, + RSBP, + RS128, + RS80, + RSHELIOS, + RSROCK, + RSM1 = 10 +}; +``` + +##### 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 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 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_`。 ++ 成员函数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/packet_record_replay.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/doc/img/install_pcl.PNG b/img/01_install_pcl.png similarity index 100% rename from doc/img/install_pcl.PNG rename to img/01_install_pcl.png diff --git a/src/rs_driver/api/lidar_driver.h b/src/rs_driver/api/lidar_driver.hpp similarity index 59% rename from src/rs_driver/api/lidar_driver.h rename to src/rs_driver/api/lidar_driver.hpp index d9aa24f2..4e2ec9b9 100644 --- a/src/rs_driver/api/lidar_driver.h +++ b/src/rs_driver/api/lidar_driver.hpp @@ -31,70 +31,31 @@ 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 +template class LidarDriver { public: - /** - * @brief Constructor, instanciate the driver pointer - */ - LidarDriver():driver_ptr_(std::make_shared>()) - { - } - - /** - * @brief Deconstructor, stop all threads - */ - ~LidarDriver() - { - stop(); - } - - /** - * @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 The initialization function which only initialize decoder(not include input module). If lidar packets are - * from ROS or other ways excluding online lidar and pcap, call this function to initialize instead of calling init() - * @param param The custom struct RSDriverParam - */ - inline void initDecoderOnly(const RSDriverParam& param) - { - driver_ptr_->initDecoderOnly(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 Stop all threads + * @brief Constructor, instanciate the driver pointer */ - inline void stop() + LidarDriver() + : driver_ptr_(std::make_shared>()) { - driver_ptr_->stop(); } /** @@ -102,83 +63,80 @@ class LidarDriver * called * @param callback The callback function */ - inline void regRecvCallback(const std::function&)>& callback) + inline void regPointCloudCallback(const std::function(void)>& cb_get_cloud, + const std::function)>& cb_put_cloud) { - driver_ptr_->regRecvCallback(callback); + driver_ptr_->regPointCloudCallback(cb_get_cloud, cb_put_cloud); } /** - * @brief Register the lidar scan message callback function to driver.When lidar scan message is ready, this function - * will be called + * @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 regRecvCallback(const std::function& callback) + inline void regPacketCallback(const std::function& cb_put_pkt) { - driver_ptr_->regRecvCallback(callback); + driver_ptr_->regPacketCallback(cb_put_pkt); } /** - * @brief Register the lidar difop packet message callback function to driver. When lidar difop packet message is - * ready, this function will be called + * @brief Register the exception message callback function to driver. When error occurs, this function will be called * @param callback The callback function */ - inline void regRecvCallback(const std::function& callback) + inline void regExceptionCallback(const std::function& cb_excep) { - driver_ptr_->regRecvCallback(callback); + driver_ptr_->regExceptionCallback(cb_excep); } /** - * @brief Register the camera trigger message callback function to driver. When trigger message is ready, this - * function will be called - * @param callback The callback function + * @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 void regRecvCallback(const std::function& callback) + inline bool init(const RSDriverParam& param) { - driver_ptr_->regRecvCallback(callback); + return driver_ptr_->init(param); } /** - * @brief Register the exception message callback function to driver. When error occurs, this function will be called - * @param callback The callback function + * @brief Start the thread to receive and decode packets + * @return If successful, return true; else return false */ - inline void regExceptionCallback(const std::function& callback) + inline bool start() { - driver_ptr_->regExceptionCallback(callback); + return driver_ptr_->start(); } /** - * @brief Get the current lidar temperature - * @param input_temperature The variable to store lidar temperature - * @return if get temperature successfully, return true; else return false + * @brief Decode lidar msop/difop messages + * @param pkt_msg The lidar msop/difop packet */ - inline bool getLidarTemperature(double& input_temperature) + inline void decodePacket(const Packet& pkt) { - return driver_ptr_->getLidarTemperature(input_temperature); + driver_ptr_->decodePacket(pkt); } /** - * @brief Decode lidar scan messages to point cloud - * @note This function will only work after decodeDifopPkt is called unless wait_for_difop is set to false - * @param pkt_scan_msg The lidar scan message - * @param point_cloud_msg The output point cloud message - * @return if decode successfully, return true; else return false + * @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 decodeMsopScan(const ScanMsg& pkt_scan_msg, PointCloudMsg& point_msg) + inline bool getTemperature(float& temp) { - return driver_ptr_->decodeMsopScan(pkt_scan_msg, point_msg); + return driver_ptr_->getTemperature(temp); } /** - * @brief Decode lidar difop messages - * @param pkt_msg The lidar difop packet + * @brief Stop all threads */ - inline void decodeDifopPkt(const PacketMsg& pkt_msg) + inline void stop() { - driver_ptr_->decodeDifopPkt(pkt_msg); + driver_ptr_->stop(); } private: - std::shared_ptr> driver_ptr_; ///< The driver pointer + std::shared_ptr> driver_ptr_; ///< The driver pointer }; } // namespace lidar diff --git a/src/rs_driver/common/common_header.h b/src/rs_driver/common/common_header.h deleted file mode 100644 index 4a91c5c7..00000000 --- a/src/rs_driver/common/common_header.h +++ /dev/null @@ -1,205 +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 -#ifndef _USE_MATH_DEFINES -#define _USE_MATH_DEFINES -#endif -/*Common*/ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -/*Linux*/ -#ifdef __linux__ -#include -#include -#include -#include -#include -#elif _WIN32 -#include -#include -#endif - -/*Eigen*/ -#ifdef ENABLE_TRANSFORM -#include -#endif - -#if defined(_WIN32) -#include -#include -inline void setConsoleColor(WORD c) -{ - HANDLE hConsole = GetStdHandle(STD_OUTPUT_HANDLE); - SetConsoleTextAttribute(hConsole, c); -} -#endif - -/*Pcap*/ -#include - -/*Camera*/ -typedef std::pair CameraTrigger; - -/*Packet Length*/ -const size_t MECH_PKT_LEN = 1248; -const size_t MEMS_MSOP_LEN = 1210; -const size_t MEMS_DIFOP_LEN = 256; -/*Output style*/ -#ifndef RS_INFOL -#if defined(_WIN32) -inline std::ostream& _RS_INFOL() -{ - setConsoleColor(FOREGROUND_GREEN); - return std::cout; -} -#define RS_INFOL _RS_INFOL() -#else -#define RS_INFOL (std::cout << "\033[32m") -#endif -#endif - -#ifndef RS_INFO -#if defined(_WIN32) -inline std::ostream& _RS_INFO() -{ - setConsoleColor(FOREGROUND_GREEN | FOREGROUND_INTENSITY); - return std::cout; -} -#define RS_INFO _RS_INFO() -#else -#define RS_INFO (std::cout << "\033[1m\033[32m") -#endif -#endif - -#ifndef RS_WARNING -#if defined(_WIN32) -inline std::ostream& _RS_WARNING() -{ - setConsoleColor(FOREGROUND_GREEN | FOREGROUND_RED | FOREGROUND_INTENSITY); - return std::cout; -} -#define RS_WARNING _RS_WARNING() -#else -#define RS_WARNING (std::cout << "\033[1m\033[33m") -#endif -#endif - -#ifndef RS_ERROR -#if defined(_WIN32) -inline std::ostream& _RS_ERROR() -{ - setConsoleColor(FOREGROUND_RED | FOREGROUND_INTENSITY); - return std::cout; -} -#define RS_ERROR _RS_ERROR() -#else -#define RS_ERROR (std::cout << "\033[1m\033[31m") -#endif -#endif - -#ifndef RS_DEBUG -#if defined(_WIN32) -inline std::ostream& _RS_DEBUG() -{ - setConsoleColor(FOREGROUND_GREEN); - return std::cout; -} -#define RS_DEBUG _RS_DEBUG() -#else -#define RS_DEBUG (std::cout << "\033[1m\033[36m") -#endif -#endif - -#ifndef RS_TITLE -#if defined(_WIN32) -inline std::ostream& _RS_TITLE() -{ - setConsoleColor(FOREGROUND_BLUE | FOREGROUND_RED | FOREGROUND_INTENSITY); - return std::cout; -} -#define RS_TITLE _RS_TITLE() -#else -#define RS_TITLE (std::cout << "\033[1m\033[35m") -#endif -#endif - -#ifndef RS_MSG -#if defined(_WIN32) -inline std::ostream& _RS_MSG() -{ - setConsoleColor(FOREGROUND_GREEN | FOREGROUND_RED | FOREGROUND_BLUE | FOREGROUND_INTENSITY); - return std::cout; -} -#define RS_MSG _RS_MSG() -#else -#define RS_MSG (std::cout << "\033[1m\033[37m") -#endif -#endif - -#ifndef RS_REND -#if defined(_WIN32) -inline std::ostream& RS_REND(std::ostream& stream) -{ - stream << std::endl; - setConsoleColor(-1); - return stream; -} -#else -#define RS_REND "\033[0m" << std::endl -#endif -#endif diff --git a/src/rs_driver/common/error_code.h b/src/rs_driver/common/error_code.hpp similarity index 60% rename from src/rs_driver/common/error_code.h rename to src/rs_driver/common/error_code.hpp index 94b386b7..4a56470c 100644 --- a/src/rs_driver/common/error_code.h +++ b/src/rs_driver/common/error_code.hpp @@ -31,7 +31,10 @@ THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *********************************************************************************************************************/ #pragma once -#include + +#include +#include + namespace robosense { namespace lidar @@ -49,38 +52,53 @@ enum class ErrCodeType WARNING_CODE, ///< Program may not work normally ERROR_CODE ///< Program will exit immediately }; + enum ErrCode { - ERRCODE_SUCCESS = 0x00, ///< Normal - ERRCODE_PCAPREPEAT = 0x01, ///< Pcap file will play repeatedly - ERRCODE_PCAPEXIT = 0x02, ///< Pcap thread will exit - ERRCODE_MSOPTIMEOUT = 0x41, ///< Msop packets receive overtime (1 sec) - ERRCODE_DIFOPTIMEOUT = 0x42, ///< Difop packets receive overtime (2 sec) - ERRCODE_MSOPINCOMPLETE = 0x43, ///< Incomplete msop packets received - ERRCODE_DIFOPINCOMPLETE = 0x44, ///< Incomplete difop packets received - ERRCODE_NODIFOPRECV = 0x45, ///< Point cloud decoding process will not start until the difop packet receive - ERRCODE_ZEROPOINTS = 0x46, ///< Size of the point cloud is zero - ERRCODE_STARTBEFOREINIT = 0x47, ///< start() function is called before initializing successfully - ERRCODE_PCAPWRONGPATH = 0x48, ///< Input directory of pcap file is wrong - ERRCODE_MSOPPORTBUZY = 0x49, ///< Input msop port is already used - ERRCODE_DIFOPPORTBUZY = 0x50, ///< Input difop port is already used - ERRCODE_WRONGPKTHEADER = 0x51, ///< Packet header is wrong - ERRCODE_PKTNULL = 0x52, ///< Input packet is null - ERRCODE_PKTBUFOVERFLOW = 0x53, ///< Packet buffer is overflow - ERRCODE_CLOUDOVERFLOW = 0x54 ///< Point Cloud is overflow + // info + ERRCODE_SUCCESS = 0x00, ///< Normal + ERRCODE_PCAPREPEAT = 0x01, ///< Pcap file will play repeatedly + ERRCODE_PCAPEXIT = 0x02, ///< Pcap thread will exit + + // warning + ERRCODE_MSOPTIMEOUT = 0x40, ///< Msop packets receive overtime (1 sec) + ERRCODE_DIFOPTIMEOUT = 0x41, ///< Difop packets receive overtime (2 sec) + ERRCODE_NODIFOPRECV = 0x42, ///< Point cloud decoding process will not start until the difop packet receive + ERRCODE_WRONGMSOPID = 0x43, ///< Packet header is wrong + ERRCODE_WRONGMSOPLEN = 0x44, ///< Packet length is wrong + ERRCODE_WRONGMSOPBLKID = 0x45, ///< Packet header is wrong + ERRCODE_WRONGDIFOPID = 0x46, ///< Packet header is wrong + ERRCODE_WRONGDIFOPLEN = 0x47, ///< Packet length is wrong + ERRCODE_ZEROPOINTS = 0x48, ///< Size of the point cloud is zero + ERRCODE_PKTBUFOVERFLOW = 0x49, ///< Packet buffer is overflow + ERRCODE_CLOUDOVERFLOW = 0x4a, ///< Point cloud buffer is overflow + + // error + ERRCODE_STARTBEFOREINIT = 0x80, ///< start() function is called before initializing successfully + ERRCODE_MSOPPORTBUZY = 0x81, ///< Input msop port is already used + ERRCODE_DIFOPPORTBUZY = 0x82, ///< Input difop port is already used + ERRCODE_PCAPWRONGPATH = 0x83, ///< Input directory of pcap file is wrong + ERRCODE_POINTCLOUDNULL = 0x84, ///< PointCloud buffer is invalid }; struct Error { ErrCode error_code; ErrCodeType error_code_type; - explicit Error(const ErrCode& code) : error_code(code) + + Error () + : error_code(ErrCode::ERRCODE_SUCCESS) { - if (error_code <= 0x40) + } + + explicit Error(const ErrCode& code) + : error_code(code) + { + if (error_code < 0x40) { error_code_type = ErrCodeType::INFO_CODE; } - else if (error_code <= 0x80) + else if (error_code < 0x80) { error_code_type = ErrCodeType::WARNING_CODE; } @@ -93,43 +111,76 @@ struct Error { switch (error_code) { - case ERRCODE_PCAPWRONGPATH: - return "ERRCODE_PCAPWRONGPATH"; - case ERRCODE_MSOPPORTBUZY: - return "ERRCODE_MSOPPORTBUZY"; - case ERRCODE_DIFOPPORTBUZY: - return "ERRCODE_DIFOPPORTBUZY"; + // info case ERRCODE_PCAPREPEAT: return "Info_PcapRepeat"; case ERRCODE_PCAPEXIT: return "Info_PcapExit"; + + // warning case ERRCODE_MSOPTIMEOUT: return "ERRCODE_MSOPTIMEOUT"; case ERRCODE_DIFOPTIMEOUT: return "ERRCODE_DIFOPTIMEOUT"; - case ERRCODE_MSOPINCOMPLETE: - return "ERRCODE_MSOPINCOMPLETE"; - case ERRCODE_DIFOPINCOMPLETE: - return "ERRCODE_DIFOPINCOMPLETE"; 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"; + + // error case ERRCODE_STARTBEFOREINIT: return "ERRCODE_STARTBEFOREINIT"; - case ERRCODE_WRONGPKTHEADER: - return "ERRCODE_WRONGPKTHEADER"; - case ERRCODE_PKTNULL: - return "ERRCODE_PKTNULL"; + case ERRCODE_MSOPPORTBUZY: + return "ERRCODE_MSOPPORTBUZY"; + case ERRCODE_DIFOPPORTBUZY: + return "ERRCODE_DIFOPPORTBUZY"; + case ERRCODE_PCAPWRONGPATH: + return "ERRCODE_PCAPWRONGPATH"; + case ERRCODE_POINTCLOUDNULL: + return "ERRCODE_POINTCLOUDNULL"; case ERRCODE_PKTBUFOVERFLOW: return "ERRCODE_PKTBUFOVERFLOW"; case ERRCODE_CLOUDOVERFLOW: return "ERRCODE_CLOUDOVERFLOW"; + + //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/rs_driver/common/rs_common.hpp b/src/rs_driver/common/rs_common.hpp new file mode 100644 index 00000000..a68b8ced --- /dev/null +++ b/src/rs_driver/common/rs_common.hpp @@ -0,0 +1,68 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the +following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following +disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following +disclaimer in the documentation and/or other materials provided with the distribution. + +3. Neither the names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, +INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*********************************************************************************************************************/ + +#pragma once + +// +// define ntohs() +// +#ifdef _WIN32 +#include +#else //__linux__ +#include +#endif + +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; +} + + +// +// 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) + diff --git a/src/rs_driver/common/rs_log.hpp b/src/rs_driver/common/rs_log.hpp new file mode 100644 index 00000000..94087e56 --- /dev/null +++ b/src/rs_driver/common/rs_log.hpp @@ -0,0 +1,46 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the +following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following +disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following +disclaimer in the documentation and/or other materials provided with the distribution. + +3. Neither the names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, +INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*********************************************************************************************************************/ + +#pragma once + +#include + +#define RS_ERROR std::cout << "\033[1m\033[31m" // bold red +#define RS_WARNING std::cout << "\033[1m\033[33m" // bold yellow +#define RS_INFO std::cout << "\033[1m\033[32m" // bold green +#define RS_INFOL std::cout << "\033[32m" // green +#define RS_DEBUG std::cout << "\033[1m\033[36m" // bold cyan +#define RS_REND "\033[0m" << std::endl + +#define RS_TITLE std::cout << "\033[1m\033[35m" // bold magenta +#define RS_MSG std::cout << "\033[1m\033[37m" // bold white + diff --git a/src/rs_driver/driver/decoder/basic_attr.hpp b/src/rs_driver/driver/decoder/basic_attr.hpp new file mode 100644 index 00000000..74b6e8c2 --- /dev/null +++ b/src/rs_driver/driver/decoder/basic_attr.hpp @@ -0,0 +1,264 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the +following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following +disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following +disclaimer in the documentation and/or other materials provided with the distribution. + +3. Neither the names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, +INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*********************************************************************************************************************/ + +#pragma once + +#include + +#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 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 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 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; +} + +} // namespace lidar +} // namespace robosense diff --git a/src/rs_driver/driver/decoder/block_iterator.hpp b/src/rs_driver/driver/decoder/block_iterator.hpp new file mode 100644 index 00000000..0e504167 --- /dev/null +++ b/src/rs_driver/driver/decoder/block_iterator.hpp @@ -0,0 +1,276 @@ +/********************************************************************************************************************* +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 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/rs_driver/driver/decoder/chan_angles.hpp b/src/rs_driver/driver/decoder/chan_angles.hpp new file mode 100644 index 00000000..5673cef2 --- /dev/null +++ b/src/rs_driver/driver/decoder/chan_angles.hpp @@ -0,0 +1,239 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the +following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following +disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following +disclaimer in the documentation and/or other materials provided with the distribution. + +3. Neither the names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, +INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*********************************************************************************************************************/ + +#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 + 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 < 9000)); + } + + uint16_t chan_num_; + std::vector vert_angles_; + std::vector horiz_angles_; + std::vector user_chans_; +}; + +} // namespace lidar +} // namespace robosense diff --git a/src/rs_driver/driver/decoder/decoder.hpp b/src/rs_driver/driver/decoder/decoder.hpp new file mode 100644 index 00000000..41682c87 --- /dev/null +++ b/src/rs_driver/driver/decoder/decoder.hpp @@ -0,0 +1,433 @@ +/********************************************************************************************************************* +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 + +#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 lidar_type; + uint8_t reserved_2[7]; + 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_firmware_ver[5]; + uint8_t bot_firmware_ver[5]; + 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; +}; + +#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 ~Decoder() = default; + + void processDifopPkt(const uint8_t* pkt, size_t size); + bool processMsopPkt(const uint8_t* pkt, size_t size); + + explicit Decoder(const RSDecoderConstParam& const_param, const RSDecoderParam& param); + + float getTemperature(); + 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); + + std::shared_ptr point_cloud_; // accumulated point cloud currently + +#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_; + 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 + + 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 +}; + +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 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 float Decoder::getTemperature() +{ + return temperature_; +} + +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::processMsopPkt(const uint8_t* pkt, size_t size) +{ + constexpr static int CLOUD_POINT_MAX = 1000000; + + if (this->point_cloud_ && (this->point_cloud_->points.size() > CLOUD_POINT_MAX)) + { + LIMIT_CALL(this->cb_excep_(Error(ERRCODE_CLOUDOVERFLOW)), 1); + } + + 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; + } + + return decodeMsopPkt(pkt, size); +} + +} // namespace lidar +} // namespace robosense diff --git a/src/rs_driver/driver/decoder/decoder_RS128.hpp b/src/rs_driver/driver/decoder/decoder_RS128.hpp index 75b3a70c..bbc21ee7 100644 --- a/src/rs_driver/driver/decoder/decoder_RS128.hpp +++ b/src/rs_driver/driver/decoder/decoder_RS128.hpp @@ -31,7 +31,8 @@ THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *********************************************************************************************************************/ #pragma once -#include +#include + namespace robosense { namespace lidar @@ -39,7 +40,7 @@ namespace lidar #pragma pack(push, 1) typedef struct { - uint8_t id; + uint8_t id[1]; uint8_t ret_id; uint16_t azimuth; RSChannel channels[128]; @@ -47,189 +48,266 @@ typedef struct typedef struct { - RSMsopHeaderNew header; + RSMsopHeaderV2 header; RS128MsopBlock blocks[3]; - unsigned int index; + uint8_t reserved[4]; } RS128MsopPkt; typedef struct { - uint64_t id; + uint8_t id[8]; uint16_t rpm; - RSEthNetNew eth; + RSEthNetV2 eth; RSFOV fov; - uint16_t reserved_0; + uint8_t reserved1[2]; uint16_t phase_lock_angle; - RSVersionNew version; - uint8_t reserved_1[229]; - RSSn sn; + RSVersionV2 version; + uint8_t reserved2[229]; + RSSN sn; uint16_t zero_cali; uint8_t return_mode; RSTimeInfo time_info; - RSStatus status; - uint8_t reserved_2[5]; - RSDiagno diagno; + RSStatusV1 status; + uint8_t reserved3[5]; + RSDiagnoV1 diagno; uint8_t gprmc[86]; - RSCalibrationAngle ver_angle_cali[128]; - RSCalibrationAngle hori_angle_cali[128]; - uint8_t reserved_3[10]; + 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 DecoderBase +template +class DecoderRS128 : public DecoderMech { public: - explicit DecoderRS128(const RSDecoderParam& param, const LidarConstantParameter& lidar_const_param); - RSDecoderResult decodeDifopPkt(const uint8_t* pkt); - RSDecoderResult decodeMsopPkt(const uint8_t* pkt, std::vector& vec, int& height, int& azimuth); - double getLidarTime(const uint8_t* pkt); + 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); + +#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 DecoderRS128::DecoderRS128(const RSDecoderParam& param, const LidarConstantParameter& lidar_const_param) - : DecoderBase(param, lidar_const_param) +template +inline RSDecoderMechConstParam& DecoderRS128::getConstParam() { - this->vert_angle_list_.resize(this->lidar_const_param_.LASER_NUM); - this->hori_angle_list_.resize(this->lidar_const_param_.LASER_NUM); - this->beam_ring_table_.resize(this->lidar_const_param_.LASER_NUM); - if (this->param_.max_distance > 250.0f) + 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, + 332.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++) { - this->param_.max_distance = 250.0f; + param.CHAN_TSS[i] = (double)firing_tss[i] / 1000000; + param.CHAN_AZIS[i] = firing_tss[i] / blk_ts; } - if (this->param_.min_distance < 1.0f || this->param_.min_distance > this->param_.max_distance) + + return param; +} + +template +inline RSEchoMode DecoderRS128::getEchoMode(uint8_t mode) +{ + switch (mode) { - this->param_.min_distance = 1.0f; + case 0x03: // dual return + return RSEchoMode::ECHO_DUAL; + case 0x01: // last return + case 0x02: // strongest return + default: + return RSEchoMode::ECHO_SINGLE; } } -template -inline double DecoderRS128::getLidarTime(const uint8_t* pkt) +template +inline DecoderRS128::DecoderRS128(const RSDecoderParam& param) + : DecoderMech(getConstParam(), param) { - return this->template calculateTimeUTC(pkt, LidarType::RS128); } -template -inline RSDecoderResult DecoderRS128::decodeMsopPkt(const uint8_t* pkt, std::vector& vec, int& height, - int& azimuth) +template +inline void DecoderRS128::decodeDifopPkt(const uint8_t* packet, size_t size) { - height = this->lidar_const_param_.LASER_NUM; - const RS128MsopPkt* mpkt_ptr = reinterpret_cast(pkt); - if (mpkt_ptr->header.id != this->lidar_const_param_.MSOP_ID) + 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 RSDecoderResult::WRONG_PKT_HEADER; + return internDecodeMsopPkt>(pkt, size); } - this->protocol_ver_ = RS_SWAP_SHORT(mpkt_ptr->header.protocol_version); - azimuth = RS_SWAP_SHORT(mpkt_ptr->blocks[0].azimuth); - this->current_temperature_ = this->computeTemperature(mpkt_ptr->header.temp_low, mpkt_ptr->header.temp_high); - double block_timestamp = this->get_point_time_func_(pkt); - this->check_camera_trigger_func_(azimuth, pkt); - float azi_diff = 0; - for (size_t blk_idx = 0; blk_idx < this->lidar_const_param_.BLOCKS_PER_PKT; blk_idx++) + else { - if (mpkt_ptr->blocks[blk_idx].id != this->lidar_const_param_.BLOCK_ID) + 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; + + 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_) { - break; + createTimeUTCWithUs (ts, (RSTimestampUTC*)&pkt.header.timestamp); } - int cur_azi = RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx].azimuth); - if (this->echo_mode_ == ECHO_DUAL) + } + + 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) { - azi_diff = static_cast( - (RS_ONE_ROUND + RS_SWAP_SHORT(mpkt_ptr->blocks[2].azimuth) - RS_SWAP_SHORT(mpkt_ptr->blocks[0].azimuth)) % - RS_ONE_ROUND); - if (RS_SWAP_SHORT(mpkt_ptr->blocks[0].azimuth) == RS_SWAP_SHORT(mpkt_ptr->blocks[1].azimuth)) ///< AAB - { - if (blk_idx == 2) - { - block_timestamp = (azi_diff > 100) ? (block_timestamp + this->fov_time_jump_diff_) : - (block_timestamp + this->time_duration_between_blocks_); - } - } - else ///< ABB - { - if (blk_idx == 1) - { - block_timestamp = (azi_diff > 100) ? (block_timestamp + this->fov_time_jump_diff_) : - (block_timestamp + this->time_duration_between_blocks_); - } - } + // + // Disable error report temporarily + // + //this->cb_excep_(Error(ERRCODE_WRONGMSOPBLKID)); + + break; } - else + + 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)) { - if (blk_idx == 0) - { - azi_diff = static_cast((RS_ONE_ROUND + RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx + 1].azimuth) - cur_azi) % - RS_ONE_ROUND); - } - else - { - azi_diff = static_cast((RS_ONE_ROUND + cur_azi - RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx - 1].azimuth)) % - RS_ONE_ROUND); - block_timestamp = (azi_diff > 100) ? (block_timestamp + this->fov_time_jump_diff_) : - (block_timestamp + this->time_duration_between_blocks_); - } + this->cb_split_frame_(this->const_param_.LASER_NUM, this->cloudTs()); + this->first_point_ts_ = block_ts; + ret = true; } - azi_diff = (azi_diff > 100) ? this->azi_diff_between_block_theoretical_ : azi_diff; - for (size_t channel_idx = 0; channel_idx < this->lidar_const_param_.CHANNELS_PER_BLOCK; channel_idx++) + + for (uint16_t chan = 0; chan < this->const_param_.CHANNELS_PER_BLOCK; chan++) { - int dsr_temp = (channel_idx / 4) % 16; - float azi_channel_ori = RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx].azimuth) + - (azi_diff * static_cast(dsr_temp) * this->lidar_const_param_.DSR_TOFFSET * - this->lidar_const_param_.FIRING_FREQUENCY); - int azi_channel_final = this->azimuthCalibration(azi_channel_ori, channel_idx); - float distance = RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx].channels[channel_idx].distance) * - this->lidar_const_param_.DIS_RESOLUTION; - int angle_horiz = static_cast(azi_channel_ori + RS_ONE_ROUND) % RS_ONE_ROUND; - int angle_vert = ((this->vert_angle_list_[channel_idx]) + RS_ONE_ROUND) % RS_ONE_ROUND; - T_Point point; - if ((distance <= this->param_.max_distance && distance >= this->param_.min_distance) && - ((this->angle_flag_ && azi_channel_final >= this->start_angle_ && azi_channel_final <= this->end_angle_) || - (!this->angle_flag_ && - ((azi_channel_final >= this->start_angle_) || (azi_channel_final <= this->end_angle_))))) + 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 * this->checkCosTable(angle_vert) * this->checkCosTable(azi_channel_final) + - this->lidar_const_param_.RX * this->checkCosTable(angle_horiz); - float y = -distance * this->checkCosTable(angle_vert) * this->checkSinTable(azi_channel_final) - - this->lidar_const_param_.RX * this->checkSinTable(angle_horiz); - float z = distance * this->checkSinTable(angle_vert) + this->lidar_const_param_.RZ; - uint8_t intensity = mpkt_ptr->blocks[blk_idx].channels[channel_idx].intensity; + 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, intensity); + setIntensity(point, channel.intensity); + setTimestamp(point, chan_ts); + setRing(point, this->chan_angles_.toUserChan(chan)); + + this->point_cloud_->points.emplace_back(point); } - else + 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); } - setRing(point, this->beam_ring_table_[channel_idx]); - setTimestamp(point, block_timestamp); - vec.emplace_back(std::move(point)); + + this->prev_point_ts_ = chan_ts; } } - return RSDecoderResult::DECODE_OK; -} -template -inline RSDecoderResult DecoderRS128::decodeDifopPkt(const uint8_t* pkt) -{ - const RS128DifopPkt* dpkt_ptr = reinterpret_cast(pkt); - if (dpkt_ptr->id != this->lidar_const_param_.DIFOP_ID) - { - return RSDecoderResult::WRONG_PKT_HEADER; - } - this->template decodeDifopCommon(pkt, LidarType::RS128); - if (!this->difop_flag_) - { - this->template decodeDifopCalibration(pkt, LidarType::RS128); - } - return RSDecoderResult::DECODE_OK; + this->prev_pkt_ts_ = pkt_ts; + return ret; } } // namespace lidar diff --git a/src/rs_driver/driver/decoder/decoder_RS128_40.hpp b/src/rs_driver/driver/decoder/decoder_RS128_40.hpp deleted file mode 100644 index 5dbca121..00000000 --- a/src/rs_driver/driver/decoder/decoder_RS128_40.hpp +++ /dev/null @@ -1,199 +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 DecoderRS128_40 : public DecoderRS128 -{ -public: - explicit DecoderRS128_40(const RSDecoderParam& param, const LidarConstantParameter& lidar_const_param); - RSDecoderResult decodeMsopPkt(const uint8_t* pkt, std::vector& vec, int& height, int& azimuth); -}; - -template -inline DecoderRS128_40::DecoderRS128_40(const RSDecoderParam& param, const LidarConstantParameter& lidar_const_param) - : DecoderRS128(param, lidar_const_param) -{ -} - -template -inline RSDecoderResult DecoderRS128_40::decodeMsopPkt(const uint8_t* pkt, std::vector& vec, int& height, - int& azimuth) -{ - height = this->lidar_const_param_.LASER_NUM; - - const RS128MsopPkt* mpkt_ptr = reinterpret_cast(pkt); - if (mpkt_ptr->header.id != this->lidar_const_param_.MSOP_ID) - { - return RSDecoderResult::WRONG_PKT_HEADER; - } - - this->protocol_ver_ = RS_SWAP_SHORT(mpkt_ptr->header.protocol_version); - - azimuth = RS_SWAP_SHORT(mpkt_ptr->blocks[0].azimuth); - this->check_camera_trigger_func_(azimuth, pkt); - - this->current_temperature_ = this->computeTemperature(mpkt_ptr->header.temp_low, mpkt_ptr->header.temp_high); - - double block_timestamp = this->get_point_time_func_(pkt); - - float azi_diff = 0; - for (uint16_t blk_idx = 0; blk_idx < this->lidar_const_param_.BLOCKS_PER_PKT; blk_idx++) - { - if (mpkt_ptr->blocks[blk_idx].id != this->lidar_const_param_.BLOCK_ID) - { - break; - } - - int cur_azi = RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx].azimuth); - - if (this->echo_mode_ == ECHO_DUAL) - { - azi_diff = static_cast( - (RS_ONE_ROUND + RS_SWAP_SHORT(mpkt_ptr->blocks[2].azimuth) - RS_SWAP_SHORT(mpkt_ptr->blocks[0].azimuth)) % - RS_ONE_ROUND); - - if (RS_SWAP_SHORT(mpkt_ptr->blocks[0].azimuth) == RS_SWAP_SHORT(mpkt_ptr->blocks[1].azimuth)) ///< AAB - { - if (blk_idx == 2) - { - block_timestamp = (azi_diff > 100) ? (block_timestamp + this->fov_time_jump_diff_) : - (block_timestamp + this->time_duration_between_blocks_); - } - } - else ///< ABB - { - if (blk_idx == 1) - { - block_timestamp = (azi_diff > 100) ? (block_timestamp + this->fov_time_jump_diff_) : - (block_timestamp + this->time_duration_between_blocks_); - } - } - } - else - { - if (blk_idx < (this->lidar_const_param_.BLOCKS_PER_PKT - 1)) - { - azi_diff = static_cast((RS_ONE_ROUND + RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx + 1].azimuth) - cur_azi) % - RS_ONE_ROUND); - } - else - { - azi_diff = static_cast((RS_ONE_ROUND + cur_azi - RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx - 1].azimuth)) % - RS_ONE_ROUND); - } - - if (blk_idx > 0) - { - block_timestamp = (azi_diff > 100) ? (block_timestamp + this->fov_time_jump_diff_) : - (block_timestamp + this->time_duration_between_blocks_); - } - } - - azi_diff = (azi_diff > 100) ? this->azi_diff_between_block_theoretical_ : azi_diff; - - for (uint16_t channel_idx = 0; channel_idx < this->lidar_const_param_.CHANNELS_PER_BLOCK; channel_idx++) - { - static const float tss[128] = - { - 0.24f, 0.24f, 0.24f, 0.24f, 1.084f, 1.084f, 1.084f, 1.084f, - 1.932f, 1.932f, 1.932f, 1.932f, 2.872f, 2.872f, 2.872f, 2.872f, - 3.824f, 3.824f, 3.824f, 3.824f, 4.684f, 4.684f, 4.684f, 4.684f, - 5.540f, 5.540f, 5.540f, 5.540f, 6.496f, 6.496f, 6.496f, 6.496f, - 7.440f, 7.440f, 7.440f, 7.440f, 9.348f, 9.348f, 9.348f, 9.348f, - 11.260f, 11.260f, 11.260f, 11.260f, 13.264f, 13.264f, 13.264f, 13.264f, - 15.280f, 15.280f, 15.280f, 15.280f, 17.204f, 17.204f, 17.204f, 17.204f, - 19.124f, 19.124f, 19.124f, 19.124f, 21.144f, 21.144f, 21.144f, 21.144f, - 23.152f, 23.152f, 23.152f, 23.152f, 25.060f, 25.060f, 25.060f, 25.060f, - 26.972f, 26.972f, 26.972f, 26.972f, 28.976f, 28.976f, 28.976f, 28.976f, - 30.992f, 30.992f, 30.992f, 30.992f, 32.916f, 32.916f, 32.916f, 32.916f, - 34.836f, 34.836f, 34.836f, 34.836f, 36.856f, 36.856f, 36.856f, 36.856f, - 38.864f, 38.864f, 38.864f, 38.864f, 40.272f, 40.272f, 40.272f, 40.272f, - 41.684f, 41.684f, 41.684f, 41.684f, 43.188f, 43.188f, 43.188f, 43.188f, - 44.704f, 44.704f, 44.704f, 44.704f, 46.128f, 46.128f, 46.128f, 46.128f, - 47.548f, 47.548f, 47.548f, 47.548f, 49.068f, 49.068f, 49.068f, 49.068f - }; - static const float blk_ts = 50.984f; - - float azi_channel_ori = cur_azi + azi_diff * tss[channel_idx] / blk_ts; - - int azi_channel_final = this->azimuthCalibration(azi_channel_ori, channel_idx); - float distance = RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx].channels[channel_idx].distance) * - this->lidar_const_param_.DIS_RESOLUTION; - //int angle_horiz = static_cast(azi_channel_ori + RS_ONE_ROUND) % RS_ONE_ROUND; - int angle_horiz_x = static_cast(azi_channel_ori - this->lidar_alph0_ + RS_ONE_ROUND) % RS_ONE_ROUND; - int angle_vert = ((this->vert_angle_list_[channel_idx]) + RS_ONE_ROUND) % RS_ONE_ROUND; - - T_Point point; - if ((distance <= this->param_.max_distance && distance >= this->param_.min_distance) && - ((this->angle_flag_ && azi_channel_final >= this->start_angle_ && azi_channel_final <= this->end_angle_) || - (!this->angle_flag_ && - ((azi_channel_final >= this->start_angle_) || (azi_channel_final <= this->end_angle_))))) - { - float x = distance * this->checkCosTable(angle_vert) * this->checkCosTable(azi_channel_final) + - this->lidar_Rxy_ * this->checkCosTable(angle_horiz_x); - float y = -distance * this->checkCosTable(angle_vert) * this->checkSinTable(azi_channel_final) - - this->lidar_Rxy_ * this->checkSinTable(angle_horiz_x); - float z = distance * this->checkSinTable(angle_vert) + this->lidar_const_param_.RZ; - - uint8_t intensity = mpkt_ptr->blocks[blk_idx].channels[channel_idx].intensity; - this->transformPoint(x, y, z); - setX(point, x); - setY(point, y); - setZ(point, z); - setIntensity(point, intensity); - } - else - { - setX(point, NAN); - setY(point, NAN); - setZ(point, NAN); - setIntensity(point, 0); - } - setRing(point, this->beam_ring_table_[channel_idx]); - setTimestamp(point, block_timestamp); - vec.emplace_back(std::move(point)); - } - } - - return RSDecoderResult::DECODE_OK; -} - -} // namespace lidar -} // namespace robosense diff --git a/src/rs_driver/driver/decoder/decoder_RS16.hpp b/src/rs_driver/driver/decoder/decoder_RS16.hpp index 2f60eb76..be0b2132 100644 --- a/src/rs_driver/driver/decoder/decoder_RS16.hpp +++ b/src/rs_driver/driver/decoder/decoder_RS16.hpp @@ -31,22 +31,24 @@ 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 { - uint16_t id; + uint8_t id[2]; uint16_t azimuth; RSChannel channels[32]; } RS16MsopBlock; typedef struct { - RSMsopHeader header; + RSMsopHeaderV1 header; RS16MsopBlock blocks[12]; unsigned int index; uint16_t tail; @@ -54,199 +56,292 @@ typedef struct typedef struct { - uint64_t id; + uint8_t id[8]; uint16_t rpm; - RSEthNet eth; + RSEthNetV1 eth; RSFOV fov; - uint16_t static_base; + uint8_t reserved0[2]; uint16_t phase_lock_angle; - RSVersion version; - uint8_t reserved_1[242]; - RSSn sn; + RSVersionV1 version; + uint8_t reserved1[242]; + RSSN sn; uint16_t zero_cali; uint8_t return_mode; uint16_t sw_ver; RSTimestampYMD timestamp; - RSStatus status; - uint8_t reserved_2[5]; - RSDiagno diagno; + RSStatusV1 status; + uint8_t reserved2[5]; + RSDiagnoV1 diagno; uint8_t gprmc[86]; - uint8_t static_cali[697]; + uint8_t reserved3[697]; uint8_t pitch_cali[48]; - uint8_t reserved_3[33]; + uint8_t reserved4[33]; uint16_t tail; } RS16DifopPkt; #pragma pack(pop) -template -class DecoderRS16 : public DecoderBase +inline void RS16DifopPkt2Adapter (const RS16DifopPkt& src, AdapterDifopPkt& dst) { -public: - DecoderRS16(const RSDecoderParam& param, const LidarConstantParameter& lidar_const_param); - RSDecoderResult decodeDifopPkt(const uint8_t* pkt); - RSDecoderResult decodeMsopPkt(const uint8_t* pkt, std::vector& vec, int& height, int& azimuth); - double getLidarTime(const uint8_t* pkt); + dst.rpm = src.rpm; + dst.fov = src.fov; + dst.return_mode = src.return_mode; + + 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); + +#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 DecoderRS16::DecoderRS16(const RSDecoderParam& param, const LidarConstantParameter& lidar_const_param) - : DecoderBase(param, lidar_const_param) +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() { - this->vert_angle_list_.resize(this->lidar_const_param_.LASER_NUM); - this->hori_angle_list_.resize(this->lidar_const_param_.LASER_NUM); - this->beam_ring_table_.resize(this->lidar_const_param_.LASER_NUM); - if (this->param_.max_distance > 150.0f) + float blk_ts = 55.50f; + float firing_tss[] = { - this->param_.max_distance = 150.0f; + 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); } - if (this->param_.min_distance < 0.2f || this->param_.min_distance > this->param_.max_distance) + else { - this->param_.min_distance = 0.2f; + Rs16DualReturnBlockIterator::calcChannel(blk_ts, firing_tss, + this->mech_const_param_.CHAN_AZIS, this->mech_const_param_.CHAN_TSS); } } -template -inline double DecoderRS16::getLidarTime(const uint8_t* pkt) +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) { - return this->template calculateTimeYMD(pkt); + 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 -inline RSDecoderResult DecoderRS16::decodeMsopPkt(const uint8_t* pkt, std::vector& vec, int& height, - int& azimuth) +template +template +inline bool DecoderRS16::internDecodeMsopPkt(const uint8_t* packet, size_t size) { - height = this->lidar_const_param_.LASER_NUM; - const RS16MsopPkt* mpkt_ptr = reinterpret_cast(pkt); - if (mpkt_ptr->header.id != this->lidar_const_param_.MSOP_ID) + const RS16MsopPkt& pkt = *(const RS16MsopPkt*)(packet); + bool ret = false; + + this->temperature_ = parseTempInLe(&(pkt.header.temp)) * this->const_param_.TEMPERATURE_RES; + + double pkt_ts = 0; + if (this->param_.use_lidar_clock) { - return RSDecoderResult::WRONG_PKT_HEADER; + pkt_ts = parseTimeYMD(&pkt.header.timestamp) * 1e-6; } - azimuth = RS_SWAP_SHORT(mpkt_ptr->blocks[0].azimuth); - this->current_temperature_ = this->computeTemperature(mpkt_ptr->header.temp_raw); - double block_timestamp = this->get_point_time_func_(pkt); - this->check_camera_trigger_func_(azimuth, pkt); - float azi_diff = 0; - for (size_t blk_idx = 0; blk_idx < this->lidar_const_param_.BLOCKS_PER_PKT; blk_idx++) + else { - if (mpkt_ptr->blocks[blk_idx].id != this->lidar_const_param_.BLOCK_ID) + 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_) { - break; + createTimeYMD (ts, (RSTimestampYMD*)&pkt.header.timestamp); } - int cur_azi = RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx].azimuth); - if (blk_idx == 0) + } + + 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) { - azi_diff = static_cast((RS_ONE_ROUND + RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx + 1].azimuth) - cur_azi) % - RS_ONE_ROUND); + this->cb_excep_(Error(ERRCODE_WRONGMSOPBLKID)); + break; } - else + + 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)) { - azi_diff = static_cast((RS_ONE_ROUND + cur_azi - RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx - 1].azimuth)) % - RS_ONE_ROUND); - block_timestamp = (azi_diff > 100) ? (block_timestamp + this->fov_time_jump_diff_) : - (block_timestamp + this->time_duration_between_blocks_); + this->cb_split_frame_(this->const_param_.LASER_NUM, this->cloudTs()); + this->first_point_ts_ = block_ts; + ret = true; } - azi_diff = (azi_diff > 100) ? this->azi_diff_between_block_theoretical_ : azi_diff; - for (size_t channel_idx = 0; channel_idx < this->lidar_const_param_.CHANNELS_PER_BLOCK; channel_idx++) + + for (uint16_t chan = 0; chan < this->const_param_.CHANNELS_PER_BLOCK; chan++) { - float azi_channel_ori = 0; - if (this->echo_mode_ == ECHO_DUAL) - { - azi_channel_ori = cur_azi + - azi_diff * this->lidar_const_param_.DSR_TOFFSET * this->lidar_const_param_.FIRING_FREQUENCY * - 2.0f * static_cast(channel_idx % 16); - } - else - { - azi_channel_ori = - cur_azi + - azi_diff * ((this->lidar_const_param_.DSR_TOFFSET * this->lidar_const_param_.FIRING_FREQUENCY * - static_cast(channel_idx % 16)) + - static_cast(channel_idx / 16) * 0.5f); - } - int azi_channel_final = this->azimuthCalibration(azi_channel_ori, channel_idx % 16); - float distance = RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx].channels[channel_idx].distance) * - this->lidar_const_param_.DIS_RESOLUTION; - int angle_horiz_ori = static_cast(azi_channel_ori + RS_ONE_ROUND) % RS_ONE_ROUND; - int angle_vert = ((this->vert_angle_list_[channel_idx % 16]) + RS_ONE_ROUND) % RS_ONE_ROUND; - T_Point point; - if ((distance <= this->param_.max_distance && distance >= this->param_.min_distance) && - ((this->angle_flag_ && azi_channel_final >= this->start_angle_ && azi_channel_final <= this->end_angle_) || - (!this->angle_flag_ && - ((azi_channel_final >= this->start_angle_) || (azi_channel_final <= this->end_angle_))))) + 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 * this->checkCosTable(angle_vert) * this->checkCosTable(azi_channel_final) + - this->lidar_const_param_.RX * this->checkCosTable(angle_horiz_ori); - float y = -distance * this->checkCosTable(angle_vert) * this->checkSinTable(azi_channel_final) - - this->lidar_const_param_.RX * this->checkSinTable(angle_horiz_ori); - float z = distance * this->checkSinTable(angle_vert) + this->lidar_const_param_.RZ; - uint8_t intensity = mpkt_ptr->blocks[blk_idx].channels[channel_idx].intensity; + 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, intensity); + setIntensity(point, channel.intensity); + setTimestamp(point, chan_ts); + setRing(point, (this->chan_angles_.toUserChan(laser))); + + this->point_cloud_->points.emplace_back(point); } - else + 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); } - setRing(point, this->beam_ring_table_[channel_idx % 16]); - if (this->echo_mode_ != ECHO_DUAL && channel_idx > 15) - { - setTimestamp(point, block_timestamp + this->time_duration_between_blocks_ / 2); - } - else - { - setTimestamp(point, block_timestamp); - } - vec.emplace_back(std::move(point)); - } - } - return RSDecoderResult::DECODE_OK; -} -template -inline RSDecoderResult DecoderRS16::decodeDifopPkt(const uint8_t* pkt) -{ - const RS16DifopPkt* dpkt_ptr = reinterpret_cast(pkt); - if (dpkt_ptr->id != this->lidar_const_param_.DIFOP_ID) - { - return RSDecoderResult::WRONG_PKT_HEADER; - } - this->template decodeDifopCommon(pkt, LidarType::RS16); - if (!this->difop_flag_) - { - if ((dpkt_ptr->pitch_cali[0] == 0x00 || dpkt_ptr->pitch_cali[0] == 0xFF) && - (dpkt_ptr->pitch_cali[1] == 0x00 || dpkt_ptr->pitch_cali[1] == 0xFF) && - (dpkt_ptr->pitch_cali[2] == 0x00 || dpkt_ptr->pitch_cali[2] == 0xFF) && - (dpkt_ptr->pitch_cali[3] == 0x00 || dpkt_ptr->pitch_cali[3] == 0xFF)) - { - return RSDecoderResult::DECODE_OK; + this->prev_point_ts_ = chan_ts; } - for (size_t i = 0; i < this->lidar_const_param_.LASER_NUM; i++) - { - /* vert angle calibration data */ - union vertical_angle - { - uint8_t data[4]; - uint32_t vertical_angle; - } ver_angle; - ver_angle.data[0] = dpkt_ptr->pitch_cali[i * 3 + 2]; - ver_angle.data[1] = dpkt_ptr->pitch_cali[i * 3 + 1]; - ver_angle.data[2] = dpkt_ptr->pitch_cali[i * 3]; - ver_angle.data[3] = 0; - int neg = i < 8 ? -1 : 1; - this->vert_angle_list_[i] = static_cast(ver_angle.vertical_angle) * neg * 0.01f; - this->hori_angle_list_[i] = 0; - } - this->sortBeamTable(); - this->difop_flag_ = true; } - return RSDecoderResult::DECODE_OK; + + this->prev_pkt_ts_ = pkt_ts; + return ret; } } // namespace lidar diff --git a/src/rs_driver/driver/decoder/decoder_RS32.hpp b/src/rs_driver/driver/decoder/decoder_RS32.hpp index 3b67e157..ffc76c5f 100644 --- a/src/rs_driver/driver/decoder/decoder_RS32.hpp +++ b/src/rs_driver/driver/decoder/decoder_RS32.hpp @@ -31,22 +31,24 @@ 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 { - uint16_t id; + uint8_t id[2]; uint16_t azimuth; RSChannel channels[32]; } RS32MsopBlock; typedef struct { - RSMsopHeader header; + RSMsopHeaderV1 header; RS32MsopBlock blocks[12]; unsigned int index; uint16_t tail; @@ -54,179 +56,274 @@ typedef struct typedef struct { - uint64_t id; + uint8_t id[8]; uint16_t rpm; - RSEthNet eth; + RSEthNetV1 eth; RSFOV fov; - uint16_t reserved0; + uint8_t reserved0[2]; uint16_t phase_lock_angle; - RSVersion version; - uint8_t reserved_1[242]; - RSSn sn; + RSVersionV1 version; + uint8_t reserved1[242]; + RSSN sn; uint16_t zero_cali; uint8_t return_mode; uint16_t sw_ver; RSTimestampYMD timestamp; - RSStatus status; - uint8_t reserved_2[5]; - RSDiagno diagno; + RSStatusV1 status; + uint8_t reserved2[5]; + RSDiagnoV1 diagno; uint8_t gprmc[86]; - RSCalibrationAngle ver_angle_cali[32]; - RSCalibrationAngle hori_angle_cali[32]; - uint8_t reserved_3[586]; + RSCalibrationAngle vert_angle_cali[32]; + RSCalibrationAngle horiz_angle_cali[32]; + uint8_t reserved3[586]; uint16_t tail; } RS32DifopPkt; #pragma pack(pop) -template -class DecoderRS32 : public DecoderBase +inline void RS32DifopPkt2Adapter (const RS32DifopPkt& src, AdapterDifopPkt& dst) +{ + dst.rpm = src.rpm; + dst.fov = src.fov; + dst.return_mode = src.return_mode; + + 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: - explicit DecoderRS32(const RSDecoderParam& param, const LidarConstantParameter& lidar_const_param); - RSDecoderResult decodeDifopPkt(const uint8_t* pkt); - RSDecoderResult decodeMsopPkt(const uint8_t* pkt, std::vector& vec, int& height, int& azimuth); - double getLidarTime(const uint8_t* pkt); + + 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); + +#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 DecoderRS32::DecoderRS32(const RSDecoderParam& param, const LidarConstantParameter& lidar_const_param) - : DecoderBase(param, lidar_const_param) +template +inline RSDecoderMechConstParam& DecoderRS32::getConstParam() { - this->vert_angle_list_.resize(this->lidar_const_param_.LASER_NUM); - this->hori_angle_list_.resize(this->lidar_const_param_.LASER_NUM); - this->beam_ring_table_.resize(this->lidar_const_param_.LASER_NUM); - if (this->param_.max_distance > 200.0f) + 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++) { - this->param_.max_distance = 200.0f; + param.CHAN_TSS[i] = (double)firing_tss[i] / 1000000; + param.CHAN_AZIS[i] = firing_tss[i] / blk_ts; } - if (this->param_.min_distance < 0.4f || this->param_.min_distance > this->param_.max_distance) + + return param; +} + +template +inline RSEchoMode DecoderRS32::getEchoMode(uint8_t mode) +{ + switch (mode) { - this->param_.min_distance = 0.4f; + case 0x00: // dual return + return RSEchoMode::ECHO_DUAL; + case 0x01: // strongest return + case 0x02: // last return + default: + return RSEchoMode::ECHO_SINGLE; } } -template -inline double DecoderRS32::getLidarTime(const uint8_t* pkt) +template +inline DecoderRS32::DecoderRS32(const RSDecoderParam& param) + : DecoderMech(getConstParam(), param) { - return this->template calculateTimeYMD(pkt); } -template -inline RSDecoderResult DecoderRS32::decodeMsopPkt(const uint8_t* pkt, std::vector& vec, int& height, - int& azimuth) +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) { - height = this->lidar_const_param_.LASER_NUM; - const RS32MsopPkt* mpkt_ptr = reinterpret_cast(pkt); - if (mpkt_ptr->header.id != this->lidar_const_param_.MSOP_ID) + const RS32MsopPkt& pkt = *(const RS32MsopPkt*)(packet); + bool ret = false; + + this->temperature_ = parseTempInLe(&(pkt.header.temp)) * this->const_param_.TEMPERATURE_RES; + + double pkt_ts = 0; + if (this->param_.use_lidar_clock) { - return RSDecoderResult::WRONG_PKT_HEADER; + pkt_ts = parseTimeYMD(&pkt.header.timestamp) * 1e-6; } - azimuth = RS_SWAP_SHORT(mpkt_ptr->blocks[0].azimuth); - this->current_temperature_ = this->computeTemperature(mpkt_ptr->header.temp_raw); - double block_timestamp = this->get_point_time_func_(pkt); - this->check_camera_trigger_func_(azimuth, pkt); - float azi_diff = 0; - for (size_t blk_idx = 0; blk_idx < this->lidar_const_param_.BLOCKS_PER_PKT; blk_idx++) + else { - if (mpkt_ptr->blocks[blk_idx].id != this->lidar_const_param_.BLOCK_ID) + 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_) { - break; + createTimeYMD (ts, (RSTimestampYMD*)&pkt.header.timestamp); } - int cur_azi = RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx].azimuth); - if (this->echo_mode_ == ECHO_DUAL) + } + + 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) { - if (blk_idx % 2 == 0) - { - if (blk_idx == 0) - { - azi_diff = static_cast( - (RS_ONE_ROUND + RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx + 2].azimuth) - cur_azi) % RS_ONE_ROUND); - } - else - { - azi_diff = static_cast( - (RS_ONE_ROUND + cur_azi - RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx - 2].azimuth)) % RS_ONE_ROUND); - block_timestamp = (azi_diff > 100) ? (block_timestamp + this->fov_time_jump_diff_) : - (block_timestamp + this->time_duration_between_blocks_); - } - } + this->cb_excep_(Error(ERRCODE_WRONGMSOPID)); + break; } - else + + 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)) { - if (blk_idx == 0) // 12 - { - azi_diff = static_cast((RS_ONE_ROUND + RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx + 1].azimuth) - cur_azi) % - RS_ONE_ROUND); - } - else - { - azi_diff = static_cast((RS_ONE_ROUND + cur_azi - RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx - 1].azimuth)) % - RS_ONE_ROUND); - block_timestamp = (azi_diff > 100) ? (block_timestamp + this->fov_time_jump_diff_) : - (block_timestamp + this->time_duration_between_blocks_); - } + this->cb_split_frame_(this->const_param_.LASER_NUM, this->cloudTs()); + this->first_point_ts_ = block_ts; + ret = true; } - azi_diff = (azi_diff > 100) ? this->azi_diff_between_block_theoretical_ : azi_diff; - for (int channel_idx = 0; channel_idx < this->lidar_const_param_.CHANNELS_PER_BLOCK; channel_idx++) + + for (uint16_t chan = 0; chan < this->const_param_.CHANNELS_PER_BLOCK; chan++) { - float azi_channel_ori = cur_azi + - azi_diff * this->lidar_const_param_.FIRING_FREQUENCY * - this->lidar_const_param_.DSR_TOFFSET * - static_cast(2 * (channel_idx % 16) + (channel_idx / 16)); - int azi_channel_final = this->azimuthCalibration(azi_channel_ori, channel_idx); - float distance = RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx].channels[channel_idx].distance) * - this->lidar_const_param_.DIS_RESOLUTION; - int angle_horiz = static_cast(azi_channel_ori + RS_ONE_ROUND) % RS_ONE_ROUND; - int angle_vert = ((this->vert_angle_list_[channel_idx]) + RS_ONE_ROUND) % RS_ONE_ROUND; - - T_Point point; - if ((distance <= this->param_.max_distance && distance >= this->param_.min_distance) && - ((this->angle_flag_ && azi_channel_final >= this->start_angle_ && azi_channel_final <= this->end_angle_) || - (!this->angle_flag_ && - ((azi_channel_final >= this->start_angle_) || (azi_channel_final <= this->end_angle_))))) + 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 * this->checkCosTable(angle_vert) * this->checkCosTable(azi_channel_final) + - this->lidar_const_param_.RX * this->checkCosTable(angle_horiz); - float y = -distance * this->checkCosTable(angle_vert) * this->checkSinTable(azi_channel_final) - - this->lidar_const_param_.RX * this->checkSinTable(angle_horiz); - float z = distance * this->checkSinTable(angle_vert) + this->lidar_const_param_.RZ; - uint8_t intensity = mpkt_ptr->blocks[blk_idx].channels[channel_idx].intensity; + 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, intensity); + setIntensity(point, channel.intensity); + setTimestamp(point, chan_ts); + setRing(point, this->chan_angles_.toUserChan(chan)); + + this->point_cloud_->points.emplace_back(point); } - else + 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); } - setRing(point, this->beam_ring_table_[channel_idx]); - setTimestamp(point, block_timestamp); - vec.emplace_back(std::move(point)); + + this->prev_point_ts_ = chan_ts; } } - return RSDecoderResult::DECODE_OK; -} -template -inline RSDecoderResult DecoderRS32::decodeDifopPkt(const uint8_t* pkt) -{ - const RS32DifopPkt* dpkt_ptr = reinterpret_cast(pkt); - if (dpkt_ptr->id != this->lidar_const_param_.DIFOP_ID) - { - return RSDecoderResult::WRONG_PKT_HEADER; - } - this->template decodeDifopCommon(pkt, LidarType::RS32); - if (!this->difop_flag_) - { - this->template decodeDifopCalibration(pkt, LidarType::RS32); - } - return RSDecoderResult::DECODE_OK; + this->prev_pkt_ts_ = pkt_ts; + return ret; } } // namespace lidar diff --git a/src/rs_driver/driver/decoder/decoder_RS48.hpp b/src/rs_driver/driver/decoder/decoder_RS48.hpp new file mode 100644 index 00000000..ab6e06c1 --- /dev/null +++ b/src/rs_driver/driver/decoder/decoder_RS48.hpp @@ -0,0 +1,260 @@ +/********************************************************************************************************************* +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); + virtual ~DecoderRS48() = default; + + explicit DecoderRS48(const RSDecoderParam& param); + +#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; + + 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; +} + +} // namespace lidar +} // namespace robosense diff --git a/src/rs_driver/driver/decoder/decoder_RS80.hpp b/src/rs_driver/driver/decoder/decoder_RS80.hpp index 72e8a82c..0c0b6d99 100644 --- a/src/rs_driver/driver/decoder/decoder_RS80.hpp +++ b/src/rs_driver/driver/decoder/decoder_RS80.hpp @@ -31,15 +31,17 @@ 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; + uint8_t id[1]; uint8_t ret_id; uint16_t azimuth; RSChannel channels[80]; @@ -47,187 +49,261 @@ typedef struct typedef struct { - RSMsopHeaderNew header; + RSMsopHeaderV2 header; RS80MsopBlock blocks[4]; - uint8_t reserved[188]; - unsigned int index; + uint8_t reserved[192]; } RS80MsopPkt; typedef struct { - uint64_t id; + uint8_t id[8]; uint16_t rpm; - RSEthNetNew eth; + RSEthNetV2 eth; RSFOV fov; - uint16_t reserved_0; + uint8_t reserved1[2]; uint16_t phase_lock_angle; - RSVersionNew version; - uint8_t reserved_1[229]; - RSSn sn; + RSVersionV2 version; + uint8_t reserved2[229]; + RSSN sn; uint16_t zero_cali; uint8_t return_mode; RSTimeInfo time_info; - RSStatus status; - uint8_t reserved_2[5]; - RSDiagno diagno; + RSStatusV1 status; + uint8_t reserved3[5]; + RSDiagnoV1 diagno; uint8_t gprmc[86]; - RSCalibrationAngle ver_angle_cali[128]; - RSCalibrationAngle hori_angle_cali[128]; - uint8_t reserved_3[10]; + 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 DecoderBase +template +class DecoderRS80 : public DecoderMech { public: - explicit DecoderRS80(const RSDecoderParam& param, const LidarConstantParameter& lidar_const_param); - RSDecoderResult decodeDifopPkt(const uint8_t* pkt); - RSDecoderResult decodeMsopPkt(const uint8_t* pkt, std::vector& vec, int& height, int& azimuth); - double getLidarTime(const uint8_t* pkt); + 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); + +#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 DecoderRS80::DecoderRS80(const RSDecoderParam& param, const LidarConstantParameter& lidar_const_param) - : DecoderBase(param, lidar_const_param) +template +inline RSDecoderMechConstParam& DecoderRS80::getConstParam() { - this->vert_angle_list_.resize(this->lidar_const_param_.LASER_NUM); - this->hori_angle_list_.resize(this->lidar_const_param_.LASER_NUM); - this->beam_ring_table_.resize(this->lidar_const_param_.LASER_NUM); - if (this->param_.max_distance > 230.0f) + 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++) { - this->param_.max_distance = 230.0f; + param.CHAN_TSS[i] = (double)firing_tss[i] / 1000000; + param.CHAN_AZIS[i] = firing_tss[i] / blk_ts; } - if (this->param_.min_distance < 1.0f || this->param_.min_distance > this->param_.max_distance) + + return param; +} + +template +inline RSEchoMode DecoderRS80::getEchoMode(uint8_t mode) +{ + switch (mode) { - this->param_.min_distance = 1.0f; + case 0x03: // dual return + return RSEchoMode::ECHO_DUAL; + case 0x01: // strongest return + case 0x02: // last return + default: + return RSEchoMode::ECHO_SINGLE; } } -template -inline double DecoderRS80::getLidarTime(const uint8_t* pkt) +template +inline DecoderRS80::DecoderRS80(const RSDecoderParam& param) + : DecoderMech(getConstParam(), param) +{ +} + +template +inline void DecoderRS80::decodeDifopPkt(const uint8_t* packet, size_t size) { - return this->template calculateTimeUTC(pkt, LidarType::RS80); + 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 RSDecoderResult DecoderRS80::decodeMsopPkt(const uint8_t* pkt, std::vector& vec, int& height, - int& azimuth) +template +inline bool DecoderRS80::decodeMsopPkt(const uint8_t* pkt, size_t size) { - height = this->lidar_const_param_.LASER_NUM; - const RS80MsopPkt* mpkt_ptr = reinterpret_cast(pkt); - if (mpkt_ptr->header.id != this->lidar_const_param_.MSOP_ID) + if (this->echo_mode_ == RSEchoMode::ECHO_SINGLE) { - return RSDecoderResult::WRONG_PKT_HEADER; + return internDecodeMsopPkt>(pkt, size); } - this->protocol_ver_ = RS_SWAP_SHORT(mpkt_ptr->header.protocol_version); - azimuth = RS_SWAP_SHORT(mpkt_ptr->blocks[0].azimuth); - this->current_temperature_ = this->computeTemperature(mpkt_ptr->header.temp_low, mpkt_ptr->header.temp_high); - double block_timestamp = this->get_point_time_func_(pkt); - this->check_camera_trigger_func_(azimuth, pkt); - float azi_diff = 0; - for (size_t blk_idx = 0; blk_idx < this->lidar_const_param_.BLOCKS_PER_PKT; blk_idx++) + else { - if (mpkt_ptr->blocks[blk_idx].id != this->lidar_const_param_.BLOCK_ID) + 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; + + 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_) { - break; + createTimeUTCWithUs (ts, (RSTimestampUTC*)&pkt.header.timestamp); } - int cur_azi = RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx].azimuth); - if (this->echo_mode_ == ECHO_DUAL) + } + + 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) { - if (blk_idx % 2 == 0) - { - if (blk_idx == 0) - { - azi_diff = static_cast( - (RS_ONE_ROUND + RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx + 2].azimuth) - cur_azi) % RS_ONE_ROUND); - } - else - { - azi_diff = static_cast( - (RS_ONE_ROUND + cur_azi - RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx - 2].azimuth)) % RS_ONE_ROUND); - block_timestamp = (azi_diff > 100) ? (block_timestamp + this->fov_time_jump_diff_) : - (block_timestamp + this->time_duration_between_blocks_); - } - } + // + // Disable error report temporarily + // + //this->cb_excep_(Error(ERRCODE_WRONGMSOPBLKID)); + + break; } - else + + 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)) { - if (blk_idx == 0) - { - azi_diff = static_cast((RS_ONE_ROUND + RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx + 1].azimuth) - cur_azi) % - RS_ONE_ROUND); - } - else - { - azi_diff = static_cast((RS_ONE_ROUND + cur_azi - RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx - 1].azimuth)) % - RS_ONE_ROUND); - block_timestamp = (azi_diff > 100) ? (block_timestamp + this->fov_time_jump_diff_) : - (block_timestamp + this->time_duration_between_blocks_); - } + this->cb_split_frame_(this->const_param_.LASER_NUM, this->cloudTs()); + this->first_point_ts_ = block_ts; + ret = true; } - azi_diff = (azi_diff > 100) ? this->azi_diff_between_block_theoretical_ : azi_diff; - for (size_t channel_idx = 0; channel_idx < this->lidar_const_param_.CHANNELS_PER_BLOCK; channel_idx++) + + for (uint16_t chan = 0; chan < this->const_param_.CHANNELS_PER_BLOCK; chan++) { - int dsr_temp = (channel_idx / 4) % 16; - float azi_channel_ori = RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx].azimuth) + - (azi_diff * static_cast(dsr_temp) * this->lidar_const_param_.DSR_TOFFSET * - this->lidar_const_param_.FIRING_FREQUENCY); - int azi_channel_final = this->azimuthCalibration(azi_channel_ori, channel_idx); - float distance = RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx].channels[channel_idx].distance) * - this->lidar_const_param_.DIS_RESOLUTION; - int angle_horiz = static_cast(azi_channel_ori + RS_ONE_ROUND) % RS_ONE_ROUND; - int angle_vert = ((this->vert_angle_list_[channel_idx]) + RS_ONE_ROUND) % RS_ONE_ROUND; - - T_Point point; - if ((distance <= this->param_.max_distance && distance >= this->param_.min_distance) && - ((this->angle_flag_ && azi_channel_final >= this->start_angle_ && azi_channel_final <= this->end_angle_) || - (!this->angle_flag_ && - ((azi_channel_final >= this->start_angle_) || (azi_channel_final <= this->end_angle_))))) + 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 * this->checkCosTable(angle_vert) * this->checkCosTable(azi_channel_final) + - this->lidar_const_param_.RX * this->checkCosTable(angle_horiz); - float y = -distance * this->checkCosTable(angle_vert) * this->checkSinTable(azi_channel_final) - - this->lidar_const_param_.RX * this->checkSinTable(angle_horiz); - float z = distance * this->checkSinTable(angle_vert) + this->lidar_const_param_.RZ; - uint8_t intensity = mpkt_ptr->blocks[blk_idx].channels[channel_idx].intensity; + 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, intensity); + setIntensity(point, channel.intensity); + setTimestamp(point, chan_ts); + setRing(point, this->chan_angles_.toUserChan(chan)); + + this->point_cloud_->points.emplace_back(point); } - else + 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); } - setRing(point, this->beam_ring_table_[channel_idx]); - setTimestamp(point, block_timestamp); - vec.emplace_back(std::move(point)); + + this->prev_point_ts_ = chan_ts; } } - return RSDecoderResult::DECODE_OK; -} -template -inline RSDecoderResult DecoderRS80::decodeDifopPkt(const uint8_t* pkt) -{ - const RS80DifopPkt* dpkt_ptr = reinterpret_cast(pkt); - if (dpkt_ptr->id != this->lidar_const_param_.DIFOP_ID) - { - return RSDecoderResult::WRONG_PKT_HEADER; - } - this->template decodeDifopCommon(pkt, LidarType::RS80); - if (!this->difop_flag_) - { - this->template decodeDifopCalibration(pkt, LidarType::RS80); - } - return RSDecoderResult::DECODE_OK; + this->prev_pkt_ts_ = pkt_ts; + return ret; } } // namespace lidar diff --git a/src/rs_driver/driver/decoder/decoder_RSBP.hpp b/src/rs_driver/driver/decoder/decoder_RSBP.hpp index 1a6b2867..a3caa249 100644 --- a/src/rs_driver/driver/decoder/decoder_RSBP.hpp +++ b/src/rs_driver/driver/decoder/decoder_RSBP.hpp @@ -31,22 +31,24 @@ 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 { - uint16_t id; + uint8_t id[2]; uint16_t azimuth; RSChannel channels[32]; } RSBPMsopBlock; typedef struct { - RSMsopHeader header; + RSMsopHeaderV1 header; RSBPMsopBlock blocks[12]; unsigned int index; uint16_t tail; @@ -54,181 +56,246 @@ typedef struct typedef struct { - uint64_t id; + uint8_t id[8]; uint16_t rpm; - RSEthNet eth; + RSEthNetV1 eth; RSFOV fov; uint16_t reserved0; uint16_t phase_lock_angle; - RSVersion version; + RSVersionV1 version; uint8_t reserved_1[242]; - RSSn sn; + RSSN sn; uint16_t zero_cali; uint8_t return_mode; uint16_t sw_ver; RSTimestampYMD timestamp; - RSStatus status; + RSStatusV1 status; uint8_t reserved_2[5]; - RSDiagno diagno; + RSDiagnoV1 diagno; uint8_t gprmc[86]; - RSCalibrationAngle ver_angle_cali[32]; - RSCalibrationAngle hori_angle_cali[32]; + 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 DecoderBase +template +class DecoderRSBP : public DecoderMech { public: - explicit DecoderRSBP(const RSDecoderParam& param, const LidarConstantParameter& lidar_const_param); - RSDecoderResult decodeDifopPkt(const uint8_t* pkt); - RSDecoderResult decodeMsopPkt(const uint8_t* pkt, std::vector& vec, int& height, int& azimuth); - double getLidarTime(const uint8_t* pkt); + + 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); + +#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 DecoderRSBP::DecoderRSBP(const RSDecoderParam& param, const LidarConstantParameter& lidar_const_param) - : DecoderBase(param, lidar_const_param) +template +inline RSDecoderMechConstParam& DecoderRSBP::getConstParam() { - this->vert_angle_list_.resize(this->lidar_const_param_.LASER_NUM); - this->hori_angle_list_.resize(this->lidar_const_param_.LASER_NUM); - this->beam_ring_table_.resize(this->lidar_const_param_.LASER_NUM); - if (this->param_.max_distance > 100.0f) + 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++) { - this->param_.max_distance = 100.0f; + param.CHAN_TSS[i] = (double)firing_tss[i] / 1000000; + param.CHAN_AZIS[i] = firing_tss[i] / blk_ts; } - if (this->param_.min_distance < 0.1f || this->param_.min_distance > this->param_.max_distance) + + return param; +} + +template +inline RSEchoMode DecoderRSBP::getEchoMode(uint8_t mode) +{ + switch (mode) { - this->param_.min_distance = 0.1f; + case 0x00: // dual return + return RSEchoMode::ECHO_DUAL; + case 0x01: // strongest return + case 0x02: // last return + default: + return RSEchoMode::ECHO_SINGLE; } } -template -inline double DecoderRSBP::getLidarTime(const uint8_t* pkt) +template +inline DecoderRSBP::DecoderRSBP(const RSDecoderParam& param) + : DecoderMech(getConstParam(), param) +{ +} + +template +inline void DecoderRSBP::decodeDifopPkt(const uint8_t* packet, size_t size) { - return this->template calculateTimeYMD(pkt); + const RSBPDifopPkt& pkt = *(const RSBPDifopPkt*)(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 RSDecoderResult DecoderRSBP::decodeMsopPkt(const uint8_t* pkt, std::vector& vec, int& height, - int& azimuth) +template +inline bool DecoderRSBP::decodeMsopPkt(const uint8_t* pkt, size_t size) { - height = this->lidar_const_param_.LASER_NUM; - const RSBPMsopPkt* mpkt_ptr = reinterpret_cast(pkt); - if (mpkt_ptr->header.id != this->lidar_const_param_.MSOP_ID) + if (this->echo_mode_ == RSEchoMode::ECHO_SINGLE) { - return RSDecoderResult::WRONG_PKT_HEADER; + return internDecodeMsopPkt>(pkt, size); } - azimuth = RS_SWAP_SHORT(mpkt_ptr->blocks[0].azimuth); - this->current_temperature_ = this->computeTemperature(mpkt_ptr->header.temp_raw); - double block_timestamp = this->get_point_time_func_(pkt); - this->check_camera_trigger_func_(azimuth, pkt); - float azi_diff = 0; - for (size_t blk_idx = 0; blk_idx < this->lidar_const_param_.BLOCKS_PER_PKT; blk_idx++) + else { - if (mpkt_ptr->blocks[blk_idx].id != this->lidar_const_param_.BLOCK_ID) + 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; + + 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_) { - break; + createTimeYMD (ts, (RSTimestampYMD*)&pkt.header.timestamp); } - int cur_azi = RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx].azimuth); - if (this->echo_mode_ == ECHO_DUAL) + } + + 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) { - if (blk_idx % 2 == 0) - { - if (blk_idx == 0) - { - azi_diff = static_cast( - (RS_ONE_ROUND + RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx + 2].azimuth) - cur_azi) % RS_ONE_ROUND); - } - else - { - azi_diff = static_cast( - (RS_ONE_ROUND + cur_azi - RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx - 2].azimuth)) % RS_ONE_ROUND); - block_timestamp = (azi_diff > 100) ? (block_timestamp + this->fov_time_jump_diff_) : - (block_timestamp + this->time_duration_between_blocks_); - } - } + this->cb_excep_(Error(ERRCODE_WRONGMSOPBLKID)); + break; } - else + + 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)) { - if (blk_idx == 0) // 12 - { - azi_diff = static_cast((RS_ONE_ROUND + RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx + 1].azimuth) - cur_azi) % - RS_ONE_ROUND); - } - else - { - azi_diff = static_cast((RS_ONE_ROUND + cur_azi - RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx - 1].azimuth)) % - RS_ONE_ROUND); - block_timestamp = (azi_diff > 100) ? (block_timestamp + this->fov_time_jump_diff_) : - (block_timestamp + this->time_duration_between_blocks_); - } + this->cb_split_frame_(this->const_param_.LASER_NUM, this->cloudTs()); + this->first_point_ts_ = block_ts; + ret = true; } - azi_diff = (azi_diff > 100) ? this->azi_diff_between_block_theoretical_ : azi_diff; - for (int channel_idx = 0; channel_idx < this->lidar_const_param_.CHANNELS_PER_BLOCK; channel_idx++) + + for (uint16_t chan = 0; chan < this->const_param_.CHANNELS_PER_BLOCK; chan++) { - // charge + reserved = 5.2f - float azi_channel_ori = cur_azi + - azi_diff * this->lidar_const_param_.FIRING_FREQUENCY * - (this->lidar_const_param_.DSR_TOFFSET * static_cast(2 * (channel_idx % 16) + (channel_idx / 16)) + 5.2f * static_cast(channel_idx / 8 % 2)); - - int azi_channel_final = this->azimuthCalibration(azi_channel_ori, channel_idx); - float distance = RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx].channels[channel_idx].distance) * - this->lidar_const_param_.DIS_RESOLUTION; - int angle_horiz = static_cast(azi_channel_ori + RS_ONE_ROUND) % RS_ONE_ROUND; - int angle_vert = ((this->vert_angle_list_[channel_idx]) + RS_ONE_ROUND) % RS_ONE_ROUND; - - // store to point cloud buffer - T_Point point; - if ((distance <= this->param_.max_distance && distance >= this->param_.min_distance) && - ((this->angle_flag_ && azi_channel_final >= this->start_angle_ && azi_channel_final <= this->end_angle_) || - (!this->angle_flag_ && - ((azi_channel_final >= this->start_angle_) || (azi_channel_final <= this->end_angle_))))) + 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 * this->checkCosTable(angle_vert) * this->checkCosTable(azi_channel_final) + - this->lidar_const_param_.RX * this->checkCosTable(angle_horiz); - float y = -distance * this->checkCosTable(angle_vert) * this->checkSinTable(azi_channel_final) - - this->lidar_const_param_.RX * this->checkSinTable(angle_horiz); - float z = distance * this->checkSinTable(angle_vert) + this->lidar_const_param_.RZ; - uint8_t intensity = mpkt_ptr->blocks[blk_idx].channels[channel_idx].intensity; + 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, intensity); + setIntensity(point, channel.intensity); + setTimestamp(point, chan_ts); + setRing(point, this->chan_angles_.toUserChan(chan)); + + this->point_cloud_->points.emplace_back(point); } - else + 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); } - setRing(point, this->beam_ring_table_[channel_idx]); - setTimestamp(point, block_timestamp); - vec.emplace_back(std::move(point)); + + this->prev_point_ts_ = chan_ts; } } - return RSDecoderResult::DECODE_OK; -} -template -inline RSDecoderResult DecoderRSBP::decodeDifopPkt(const uint8_t* pkt) -{ - const RSBPDifopPkt* dpkt_ptr = reinterpret_cast(pkt); - if (dpkt_ptr->id != this->lidar_const_param_.DIFOP_ID) - { - return RSDecoderResult::WRONG_PKT_HEADER; - } - this->template decodeDifopCommon(pkt, LidarType::RSBP); - if (!this->difop_flag_) - { - this->template decodeDifopCalibration(pkt, LidarType::RSBP); - } - return RSDecoderResult::DECODE_OK; + this->prev_pkt_ts_ = pkt_ts; + return ret; } } // namespace lidar diff --git a/src/rs_driver/driver/decoder/decoder_RSBPV4.hpp b/src/rs_driver/driver/decoder/decoder_RSBPV4.hpp new file mode 100644 index 00000000..29ea86e8 --- /dev/null +++ b/src/rs_driver/driver/decoder/decoder_RSBPV4.hpp @@ -0,0 +1,259 @@ +/********************************************************************************************************************* +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 DecoderRSBPV4 : public DecoderMech +{ +public: + + virtual void decodeDifopPkt(const uint8_t* pkt, size_t size); + virtual bool decodeMsopPkt(const uint8_t* pkt, size_t size); + virtual ~DecoderRSBPV4() = default; + + explicit DecoderRSBPV4(const RSDecoderParam& param); + +#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& DecoderRSBPV4::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.0025f // 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 DecoderRSBPV4::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 DecoderRSBPV4::DecoderRSBPV4(const RSDecoderParam& param) + : DecoderMech(getConstParam(), param) +{ +} + +template +inline void DecoderRSBPV4::decodeDifopPkt(const uint8_t* packet, size_t size) +{ + const RSBPDifopPkt& pkt = *(const RSBPDifopPkt*)(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 DecoderRSBPV4::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 DecoderRSBPV4::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; + + 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 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 (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; +} + +} // namespace lidar +} // namespace robosense diff --git a/src/rs_driver/driver/decoder/decoder_RSEOS.hpp b/src/rs_driver/driver/decoder/decoder_RSEOS.hpp new file mode 100644 index 00000000..abb5bf38 --- /dev/null +++ b/src/rs_driver/driver/decoder/decoder_RSEOS.hpp @@ -0,0 +1,246 @@ +/********************************************************************************************************************* +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; + +#pragma pack(pop) + +template +class DecoderRSEOS : public Decoder +{ +public: + + constexpr static double FRAME_DURATION = 0.1; + constexpr static uint32_t SINGLE_PKT_NUM = 112; + 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 ~DecoderRSEOS() = default; + + explicit DecoderRSEOS(const RSDecoderParam& param); + +private: + + static RSDecoderConstParam& getConstParam(); + RSEchoMode getEchoMode(uint8_t mode); + + SplitStrategyBySeq split_strategy_; +}; + +template +inline RSDecoderConstParam& DecoderRSEOS::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 DecoderRSEOS::DecoderRSEOS(const RSDecoderParam& param) + : Decoder(getConstParam(), param) +{ + this->packet_duration_ = FRAME_DURATION / SINGLE_PKT_NUM; + this->angles_ready_ = true; +} + +template +inline RSEchoMode DecoderRSEOS::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 DecoderRSEOS::decodeDifopPkt(const uint8_t* packet, size_t size) +{ +} + +template +inline bool DecoderRSEOS::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); + + 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/rs_driver/driver/decoder/decoder_RSHELIOS.hpp b/src/rs_driver/driver/decoder/decoder_RSHELIOS.hpp index 384bcd98..5d41b259 100644 --- a/src/rs_driver/driver/decoder/decoder_RSHELIOS.hpp +++ b/src/rs_driver/driver/decoder/decoder_RSHELIOS.hpp @@ -31,34 +31,37 @@ 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 { - uint16_t id; + uint8_t id[2]; uint16_t azimuth; RSChannel channels[32]; } RSHELIOSMsopBlock; typedef struct { - uint32_t id; + uint8_t id[4]; uint16_t protocol_version; - uint8_t reserved_1[14]; + uint8_t reserved1[14]; RSTimestampUTC timestamp; uint8_t lidar_type; - uint8_t reserved_2[7]; - uint16_t temp_raw; - uint8_t reserved_3[2]; -} RSHeliosMsopHeader; + uint8_t reserved2[7]; + RSTemperature temp; + uint8_t reserved3[2]; +} RSHELIOSMsopHeader; typedef struct { - RSHeliosMsopHeader header; + RSHELIOSMsopHeader header; RSHELIOSMsopBlock blocks[12]; unsigned int index; uint16_t tail; @@ -66,181 +69,244 @@ typedef struct typedef struct { - uint64_t id; + uint8_t id[8]; uint16_t rpm; - RSEthNetNew eth; + RSEthNetV2 eth; RSFOV fov; - uint8_t reserved_1[2]; + uint8_t reserved1[2]; uint16_t phase_lock_angle; - RSVersionNew version; - uint8_t reserved_2[229]; - RSSn sn; + RSVersionV2 version; + uint8_t reserved2[229]; + RSSN sn; uint16_t zero_cali; uint8_t return_mode; RSTimeInfo time_info; - RSStatusNew status; - uint8_t reserved_3[5]; - RSDiagnoNew diagno; + RSStatusV2 status; + uint8_t reserved3[5]; + RSDiagnoV2 diagno; uint8_t gprmc[86]; - RSCalibrationAngle ver_angle_cali[32]; - RSCalibrationAngle hori_angle_cali[32]; - uint8_t reserved_4[586]; + 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 DecoderBase +template +class DecoderRSHELIOS : public DecoderMech { public: - explicit DecoderRSHELIOS(const RSDecoderParam& param, const LidarConstantParameter& lidar_const_param); - RSDecoderResult decodeDifopPkt(const uint8_t* pkt); - RSDecoderResult decodeMsopPkt(const uint8_t* pkt, std::vector& vec, int& height, int& azimuth); - double getLidarTime(const uint8_t* pkt); + 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); + +#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 DecoderRSHELIOS::DecoderRSHELIOS(const RSDecoderParam& param, - const LidarConstantParameter& lidar_const_param) - : DecoderBase(param, lidar_const_param) +template +inline RSDecoderMechConstParam& DecoderRSHELIOS::getConstParam() { - this->vert_angle_list_.resize(this->lidar_const_param_.LASER_NUM); - this->hori_angle_list_.resize(this->lidar_const_param_.LASER_NUM); - this->beam_ring_table_.resize(this->lidar_const_param_.LASER_NUM); - if (this->param_.max_distance > 150.0f) + 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.57f, 3.15f, 4.72f, 6.30f, 7.87f, 9.45f, 11.36f, + 13.26f, 15.17f, 17.08f, 18.99f, 20.56f, 22.14f, 23.71f, 25.29f, + 26.53f, 29.01f, 27.77f, 30.25f, 31.49f, 33.98f, 32.73f, 35.22f, + 36.46f, 37.70f, 38.94f, 40.18f, 41.42f, 42.67f, 43.91f, 45.15f + }; + + param.BLOCK_DURATION = blk_ts / 1000000; + for (uint16_t i = 0; i < sizeof(firing_tss)/sizeof(firing_tss[0]); i++) { - this->param_.max_distance = 150.0f; + param.CHAN_TSS[i] = (double)firing_tss[i] / 1000000; + param.CHAN_AZIS[i] = firing_tss[i] / blk_ts; } - if (this->param_.min_distance < 0.1f || this->param_.min_distance > this->param_.max_distance) + + return param; +} + +template +inline RSEchoMode DecoderRSHELIOS::getEchoMode(uint8_t mode) +{ + switch (mode) { - this->param_.min_distance = 0.1f; + 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 double DecoderRSHELIOS::getLidarTime(const uint8_t* pkt) +template +inline DecoderRSHELIOS::DecoderRSHELIOS(const RSDecoderParam& param) + : DecoderMech(getConstParam(), param) +{ +} + +template +inline void DecoderRSHELIOS::decodeDifopPkt(const uint8_t* packet, size_t size) { - return this->template calculateTimeUTC(pkt, LidarType::RSHELIOS); + 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 RSDecoderResult DecoderRSHELIOS::decodeMsopPkt(const uint8_t* pkt, std::vector& vec, - int& height, int& azimuth) +template +inline bool DecoderRSHELIOS::decodeMsopPkt(const uint8_t* pkt, size_t size) { - height = this->lidar_const_param_.LASER_NUM; - const RSHELIOSMsopPkt* mpkt_ptr = reinterpret_cast(pkt); - if (mpkt_ptr->header.id != this->lidar_const_param_.MSOP_ID) + if (this->echo_mode_ == RSEchoMode::ECHO_SINGLE) { - return RSDecoderResult::WRONG_PKT_HEADER; + return internDecodeMsopPkt>(pkt, size); } - this->protocol_ver_ = RS_SWAP_SHORT(mpkt_ptr->header.protocol_version); - azimuth = RS_SWAP_SHORT(mpkt_ptr->blocks[0].azimuth); - this->current_temperature_ = this->computeTemperature(mpkt_ptr->header.temp_raw); - double block_timestamp = this->get_point_time_func_(pkt); - this->check_camera_trigger_func_(azimuth, pkt); - float azi_diff = 0; - for (size_t blk_idx = 0; blk_idx < this->lidar_const_param_.BLOCKS_PER_PKT; blk_idx++) + else { - if (mpkt_ptr->blocks[blk_idx].id != this->lidar_const_param_.BLOCK_ID) + 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; + + 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_) { - break; + createTimeUTCWithUs (ts, (RSTimestampUTC*)&pkt.header.timestamp); } - int cur_azi = RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx].azimuth); - if (this->echo_mode_ == ECHO_DUAL) + } + + 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) { - if (blk_idx % 2 == 0) - { - if (blk_idx == 0) - { - azi_diff = static_cast( - (RS_ONE_ROUND + RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx + 2].azimuth) - cur_azi) % RS_ONE_ROUND); - } - else - { - azi_diff = static_cast( - (RS_ONE_ROUND + cur_azi - RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx - 2].azimuth)) % RS_ONE_ROUND); - block_timestamp = (azi_diff > 100) ? (block_timestamp + this->fov_time_jump_diff_) : - (block_timestamp + this->time_duration_between_blocks_); - } - } + this->cb_excep_(Error(ERRCODE_WRONGMSOPBLKID)); + break; } - else + + 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)) { - if (blk_idx == 0) // 12 - { - azi_diff = static_cast((RS_ONE_ROUND + RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx + 1].azimuth) - cur_azi) % - RS_ONE_ROUND); - } - else - { - azi_diff = static_cast((RS_ONE_ROUND + cur_azi - RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx - 1].azimuth)) % - RS_ONE_ROUND); - block_timestamp = (azi_diff > 100) ? (block_timestamp + this->fov_time_jump_diff_) : - (block_timestamp + this->time_duration_between_blocks_); - } + this->cb_split_frame_(this->const_param_.LASER_NUM, this->cloudTs()); + this->first_point_ts_ = block_ts; + ret = true; } - azi_diff = (azi_diff > 100) ? this->azi_diff_between_block_theoretical_ : azi_diff; - for (int channel_idx = 0; channel_idx < this->lidar_const_param_.CHANNELS_PER_BLOCK; channel_idx++) + + for (uint16_t chan = 0; chan < this->const_param_.CHANNELS_PER_BLOCK; chan++) { - float azi_channel_ori = - cur_azi + - azi_diff * this->lidar_const_param_.DSR_TOFFSET * this->lidar_const_param_.FIRING_FREQUENCY * - ((-0.014f * static_cast(channel_idx) + 1.8965f) * static_cast(channel_idx) - 0.6543f); - int azi_channel_final = this->azimuthCalibration(azi_channel_ori, channel_idx); - float distance = RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx].channels[channel_idx].distance) * - this->lidar_const_param_.DIS_RESOLUTION; - int angle_horiz = (int)(azi_channel_ori + RS_ONE_ROUND) % RS_ONE_ROUND; - int angle_vert = ((this->vert_angle_list_[channel_idx]) + RS_ONE_ROUND) % RS_ONE_ROUND; - - // store to point cloud buffer - T_Point point; - if ((distance <= this->param_.max_distance && distance >= this->param_.min_distance) && - ((this->angle_flag_ && azi_channel_final >= this->start_angle_ && azi_channel_final <= this->end_angle_) || - (!this->angle_flag_ && - ((azi_channel_final >= this->start_angle_) || (azi_channel_final <= this->end_angle_))))) + 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 * this->checkCosTable(angle_vert) * this->checkCosTable(azi_channel_final) + - this->lidar_const_param_.RX * this->checkCosTable(angle_horiz); - float y = -distance * this->checkCosTable(angle_vert) * this->checkSinTable(azi_channel_final) - - this->lidar_const_param_.RX * this->checkSinTable(angle_horiz); - float z = distance * this->checkSinTable(angle_vert) + this->lidar_const_param_.RZ; - uint8_t intensity = mpkt_ptr->blocks[blk_idx].channels[channel_idx].intensity; + 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, intensity); + setIntensity(point, channel.intensity); + setTimestamp(point, chan_ts); + setRing(point, this->chan_angles_.toUserChan(chan)); + + this->point_cloud_->points.emplace_back(point); } - else + 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); } - setRing(point, this->beam_ring_table_[channel_idx]); - setTimestamp(point, block_timestamp); - vec.emplace_back(std::move(point)); + + this->prev_point_ts_ = chan_ts; } } - return RSDecoderResult::DECODE_OK; -} -template -inline RSDecoderResult DecoderRSHELIOS::decodeDifopPkt(const uint8_t* pkt) -{ - RSHELIOSDifopPkt* dpkt_ptr = (RSHELIOSDifopPkt*)pkt; - if (dpkt_ptr->id != this->lidar_const_param_.DIFOP_ID) - { - return RSDecoderResult::WRONG_PKT_HEADER; - } - this->template decodeDifopCommon(pkt, LidarType::RSHELIOS); - if (!this->difop_flag_) - { - this->template decodeDifopCalibration(pkt, LidarType::RSHELIOS); - } - return RSDecoderResult::DECODE_OK; + this->prev_pkt_ts_ = pkt_ts; + return ret; } } // namespace lidar diff --git a/src/rs_driver/driver/decoder/decoder_RSHELIOS_16P.hpp b/src/rs_driver/driver/decoder/decoder_RSHELIOS_16P.hpp new file mode 100644 index 00000000..1f64c635 --- /dev/null +++ b/src/rs_driver/driver/decoder/decoder_RSHELIOS_16P.hpp @@ -0,0 +1,280 @@ +/********************************************************************************************************************* +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); + +#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, 3.15f, 6.30f, 9.45f, 13.26f, 17.08f, 20.56f, 23.71f, + 26.53f, 27.77f, 31.49f, 32.73f, 36.46f, 38.94f, 41.42f, 43.91f, + 55.56f, 58.70f, 61.85f, 65.00f, 68.82f, 72.64f, 76.12f, 79.27f, + 82.08f, 83.32f, 87.05f, 88.29f, 92.01f, 94.50f, 96.98f, 99.46f + }; + + 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; + + 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; +} + +} // namespace lidar +} // namespace robosense diff --git a/src/rs_driver/driver/decoder/decoder_RSM1.hpp b/src/rs_driver/driver/decoder/decoder_RSM1.hpp index 08914b79..47054083 100644 --- a/src/rs_driver/driver/decoder/decoder_RSM1.hpp +++ b/src/rs_driver/driver/decoder/decoder_RSM1.hpp @@ -31,17 +31,27 @@ THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *********************************************************************************************************************/ #pragma once -#include +#include namespace robosense { namespace lidar { -const uint32_t SINGLE_PKT_NUM = 630; -const uint32_t DUAL_PKT_NUM = 1260; -const int ANGLE_OFFSET = 32768; #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; @@ -59,19 +69,6 @@ typedef struct RSM1Channel channel[5]; } RSM1Block; -typedef struct -{ - uint32_t id; - uint16_t pkt_cnt; - uint16_t protocol_version; - uint8_t return_mode; - uint8_t time_mode; - RSTimestampUTC timestamp; - uint8_t reserved[10]; - uint8_t lidar_type; - int8_t temperature; -} RSM1MsopHeader; - typedef struct { RSM1MsopHeader header; @@ -119,177 +116,185 @@ typedef struct typedef struct { - uint64_t id; - uint8_t reserved_1; + uint8_t id[8]; + uint8_t reserved1[1]; uint8_t frame_rate; RSM1DifopEther ether; - RSM1DifopFov fov_setting; + RSM1DifopFov fov; RSM1DifopVerInfo ver_info; - RSSn sn; + RSSN sn; uint8_t return_mode; RSTimeInfo time_info; RSM1DifopRunSts status; - uint8_t diag_reserved[40]; + uint8_t reserved2[40]; RSM1DifopCalibration cali_param[20]; - uint8_t reserved_2[71]; + uint8_t reserved3[71]; } RSM1DifopPkt; + #pragma pack(pop) -template -class DecoderRSM1 : public DecoderBase +template +class DecoderRSM1 : public Decoder { public: - DecoderRSM1(const RSDecoderParam& param, const LidarConstantParameter& lidar_const_param); - RSDecoderResult decodeDifopPkt(const uint8_t* pkt); - RSDecoderResult decodeMsopPkt(const uint8_t* pkt, std::vector& vec, int& height, int& azimuth); - double getLidarTime(const uint8_t* pkt); - RSDecoderResult processMsopPkt(const uint8_t* pkt, std::vector& pointcloud_vec, int& height); + + 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: - uint32_t last_pkt_cnt_; - uint32_t max_pkt_num_; - double last_pkt_time_; + + static RSDecoderConstParam& getConstParam(); + RSEchoMode getEchoMode(uint8_t mode); + + SplitStrategyBySeq split_strategy_; }; -template -inline DecoderRSM1::DecoderRSM1(const RSDecoderParam& param, const LidarConstantParameter& lidar_const_param) - : DecoderBase(param, lidar_const_param), last_pkt_cnt_(1), max_pkt_num_(SINGLE_PKT_NUM), last_pkt_time_(0) +template +inline RSDecoderConstParam& DecoderRSM1::getConstParam() { - if (this->param_.max_distance > 200.0f) - { - this->param_.max_distance = 200.0f; - } - if (this->param_.min_distance < 0.2f || this->param_.min_distance > this->param_.max_distance) + static RSDecoderConstParam param = { - this->param_.min_distance = 0.2f; - } - this->time_duration_between_blocks_ = 5 * 1e-6; + 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 double DecoderRSM1::getLidarTime(const uint8_t* pkt) +template +inline DecoderRSM1::DecoderRSM1(const RSDecoderParam& param) + : Decoder(getConstParam(), param) { - return this->template calculateTimeUTC(pkt, LidarType::RSM1); + this->packet_duration_ = FRAME_DURATION / SINGLE_PKT_NUM; + this->angles_ready_ = true; } -template -inline RSDecoderResult DecoderRSM1::processMsopPkt(const uint8_t* pkt, std::vector& pointcloud_vec, - int& height) +template +inline RSEchoMode DecoderRSM1::getEchoMode(uint8_t mode) { - int azimuth = 0; - RSDecoderResult ret = decodeMsopPkt(pkt, pointcloud_vec, height, azimuth); - this->pkt_count_++; - switch (this->param_.split_frame_mode) + switch (mode) { - case SplitFrameMode::SPLIT_BY_ANGLE: - case SplitFrameMode::SPLIT_BY_FIXED_PKTS: - return ret; - case SplitFrameMode::SPLIT_BY_CUSTOM_PKTS: - if (this->pkt_count_ >= this->param_.num_pkts_split) - { - this->pkt_count_ = 0; - this->trigger_index_ = 0; - this->prev_angle_diff_ = RS_ONE_ROUND; - return FRAME_SPLIT; - } - break; + case 0x00: // dual return + return RSEchoMode::ECHO_DUAL; + case 0x04: // strongest return + case 0x05: // last return + case 0x06: // first return default: - break; + return RSEchoMode::ECHO_SINGLE; } - return DECODE_OK; } -template -inline RSDecoderResult DecoderRSM1::decodeMsopPkt(const uint8_t* pkt, std::vector& vec, int& height, - int& azimuth) +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); +} + +template +inline bool DecoderRSM1::decodeMsopPkt(const uint8_t* packet, size_t size) { - height = this->lidar_const_param_.LASER_NUM; - RSM1MsopPkt* mpkt_ptr = (RSM1MsopPkt*)pkt; - if (mpkt_ptr->header.id != this->lidar_const_param_.MSOP_ID) + const RSM1MsopPkt& pkt = *(RSM1MsopPkt*)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 { - return RSDecoderResult::WRONG_PKT_HEADER; + 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); + } } - this->current_temperature_ = static_cast(mpkt_ptr->header.temperature - 80); - this->protocol_ver_ = RS_SWAP_SHORT(mpkt_ptr->header.protocol_version); - double pkt_timestamp = 0; - switch (mpkt_ptr->blocks[0].return_seq) + + uint16_t pkt_seq = ntohs(pkt.header.pkt_seq); + if (split_strategy_.newPacket(pkt_seq)) { - case 0: - pkt_timestamp = this->get_point_time_func_(pkt); - break; - case 1: - pkt_timestamp = this->get_point_time_func_(pkt); - last_pkt_time_ = pkt_timestamp; - break; - case 2: - pkt_timestamp = last_pkt_time_; - break; + this->cb_split_frame_(this->const_param_.LASER_NUM, this->cloudTs()); + this->first_point_ts_ = pkt_ts; + ret = true; } - for (size_t blk_idx = 0; blk_idx < this->lidar_const_param_.BLOCKS_PER_PKT; blk_idx++) + for (uint16_t blk = 0; blk < this->const_param_.BLOCKS_PER_PKT; blk++) { - const RSM1Block& blk = mpkt_ptr->blocks[blk_idx]; - double point_time = pkt_timestamp + blk.time_offset * 1e-6; - for (size_t channel_idx = 0; channel_idx < this->lidar_const_param_.CHANNELS_PER_BLOCK; channel_idx++) + 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++) { - T_Point point; - float distance = RS_SWAP_SHORT(blk.channel[channel_idx].distance) * this->lidar_const_param_.DIS_RESOLUTION; - if (distance <= this->param_.max_distance && distance >= this->param_.min_distance) + const RSM1Channel& channel = block.channel[chan]; + + float distance = ntohs(channel.distance) * this->const_param_.DISTANCE_RES; + + if (this->distance_section_.in(distance)) { - int pitch = RS_SWAP_SHORT(blk.channel[channel_idx].pitch) - ANGLE_OFFSET; - int yaw = RS_SWAP_SHORT(blk.channel[channel_idx].yaw) - ANGLE_OFFSET; - uint8_t intensity = blk.channel[channel_idx].intensity; - float x = distance * this->checkCosTable(pitch) * this->checkCosTable(yaw); - float y = distance * this->checkCosTable(pitch) * this->checkSinTable(yaw); - float z = distance * this->checkSinTable(pitch); + 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, intensity); + setIntensity(point, channel.intensity); + setTimestamp(point, point_time); + setRing(point, chan); + + this->point_cloud_->points.emplace_back(point); } - else + 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); } - setTimestamp(point, point_time); - setRing(point, channel_idx + 1); - vec.emplace_back(std::move(point)); } - } - unsigned int pkt_cnt = RS_SWAP_SHORT(mpkt_ptr->header.pkt_cnt); - // TODO whatif packet loss or seq unorder - if (pkt_cnt == max_pkt_num_ || pkt_cnt < last_pkt_cnt_) - { - last_pkt_cnt_ = 1; - return RSDecoderResult::FRAME_SPLIT; + this->prev_point_ts_ = point_time; } - last_pkt_cnt_ = pkt_cnt; - return RSDecoderResult::DECODE_OK; -} -template -inline RSDecoderResult DecoderRSM1::decodeDifopPkt(const uint8_t* pkt) -{ - RSM1DifopPkt* dpkt_ptr = (RSM1DifopPkt*)pkt; - if (dpkt_ptr->id != this->lidar_const_param_.DIFOP_ID) - { - return RSDecoderResult::WRONG_PKT_HEADER; - } - if (!this->difop_flag_) - { - this->echo_mode_ = this->getEchoMode(LidarType::RSM1, dpkt_ptr->return_mode); - if (this->echo_mode_ == RSEchoMode::ECHO_DUAL) - { - max_pkt_num_ = DUAL_PKT_NUM; - } - this->difop_flag_ = true; - } - return RSDecoderResult::DECODE_OK; + this->prev_pkt_ts_ = pkt_ts; + return ret; } } // namespace lidar diff --git a/src/rs_driver/driver/decoder/decoder_RSM1_Jumbo.hpp b/src/rs_driver/driver/decoder/decoder_RSM1_Jumbo.hpp new file mode 100644 index 00000000..ec06433c --- /dev/null +++ b/src/rs_driver/driver/decoder/decoder_RSM1_Jumbo.hpp @@ -0,0 +1,262 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the +following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following +disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following +disclaimer in the documentation and/or other materials provided with the distribution. + +3. Neither the names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, +INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*********************************************************************************************************************/ + +#pragma once +#include + +namespace robosense +{ +namespace lidar +{ +#pragma pack(push, 1) + +typedef struct +{ + uint16_t distance; + uint16_t pitch; + uint16_t yaw; + uint8_t intensity; +} RSM1_Jumbo_Channel; + +typedef struct +{ + uint8_t time_offset; + uint8_t return_seq; + RSM1_Jumbo_Channel channel[5]; +} RSM1_Jumbo_Block; + +typedef struct +{ + RSM1MsopHeader header; + RSM1_Jumbo_Block blocks[25]; + uint8_t reserved1[11]; + uint8_t reserved2[16]; +} RSM1_Jumbo_MsopPkt; + +typedef struct +{ + RSM1_Jumbo_MsopPkt pkts[63]; + uint8_t reserved[160]; +} RSM1_Jumbo; + +#pragma pack(pop) + +template +class DecoderRSM1_Jumbo : public Decoder +{ +public: + + constexpr static double FRAME_DURATION = 0.1; + constexpr static uint32_t SINGLE_PKT_NUM = 630; + constexpr static int ANGLE_OFFSET = 32768; + + virtual void decodeDifopPkt(const uint8_t* pkt, size_t size); + virtual bool decodeMsopPkt(const uint8_t* pkt, size_t size); + virtual ~DecoderRSM1_Jumbo() = default; + + explicit DecoderRSM1_Jumbo(const RSDecoderParam& param); + +private: + + static RSDecoderConstParam& getConstParam(); + RSEchoMode getEchoMode(uint8_t mode); + + bool internDecodeMsopPkt(const uint8_t* pkt, size_t size); + SplitStrategyBySeq split_strategy_; +}; + +template +inline RSDecoderConstParam& DecoderRSM1_Jumbo::getConstParam() +{ + static RSDecoderConstParam param = + { + 62152 // msop len + , 256 // difop len + , 4 // msop id len + , 8 // difop id len + , {0x55, 0xAA, 0x5A, 0xA5} // msop id + , {0xA5, 0xFF, 0x00, 0x5A, 0x11, 0x11, 0x55, 0x55} // difop id + , {0x00, 0x00} + , 5 // laser number + , 25 // blocks per packet + , 5 // channels per block + , 0.2f // distance min + , 200.0f // distance max + , 0.005f // distance resolution + , 80.0f // initial value of temperature + }; + + return param; +} + +template +inline DecoderRSM1_Jumbo::DecoderRSM1_Jumbo(const RSDecoderParam& param) + : Decoder(getConstParam(), param) +{ + this->packet_duration_ = FRAME_DURATION / SINGLE_PKT_NUM; + this->angles_ready_ = true; +} + +template +inline RSEchoMode DecoderRSM1_Jumbo::getEchoMode(uint8_t mode) +{ + switch (mode) + { + case 0x00: // dual return + return RSEchoMode::ECHO_DUAL; + case 0x04: // strongest return + case 0x05: // last return + case 0x06: // first return + default: + return RSEchoMode::ECHO_SINGLE; + } +} + +template +inline void DecoderRSM1_Jumbo::decodeDifopPkt(const uint8_t* packet, size_t size) +{ + const RSM1DifopPkt& pkt = *(RSM1DifopPkt*)packet; + this->echo_mode_ = this->getEchoMode(pkt.return_mode); +} + +template +inline bool DecoderRSM1_Jumbo::decodeMsopPkt(const uint8_t* packet, size_t size) +{ + if (size != sizeof(RSM1_Jumbo)) + return false; + + constexpr static int PACKET_NUM = 63; + + const RSM1_Jumbo* jumbo = (const RSM1_Jumbo *)packet; + bool ret = false; + + for (size_t i = 0; i < PACKET_NUM; i++) + { + bool r = internDecodeMsopPkt((const uint8_t*)&(jumbo->pkts[i]), sizeof(RSM1_Jumbo_MsopPkt)); + ret = (ret || r); + } + + return ret; +} + +template +inline bool DecoderRSM1_Jumbo::internDecodeMsopPkt(const uint8_t* packet, size_t size) +{ + const RSM1_Jumbo_MsopPkt& pkt = *(RSM1_Jumbo_MsopPkt*)packet; + bool ret = false; + + if (memcmp(packet, this->const_param_.MSOP_ID, this->const_param_.MSOP_ID_LEN) != 0) + return false; + + this->temperature_ = static_cast((int)pkt.header.temperature - this->const_param_.TEMPERATURE_RES); + + 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/rs_driver/driver/decoder/decoder_RSM2.hpp b/src/rs_driver/driver/decoder/decoder_RSM2.hpp new file mode 100644 index 00000000..4d8824c6 --- /dev/null +++ b/src/rs_driver/driver/decoder/decoder_RSM2.hpp @@ -0,0 +1,251 @@ +/********************************************************************************************************************* +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 + , 200.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); +} + +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); + + 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/rs_driver/driver/decoder/decoder_RSP128.hpp b/src/rs_driver/driver/decoder/decoder_RSP128.hpp new file mode 100644 index 00000000..078464a7 --- /dev/null +++ b/src/rs_driver/driver/decoder/decoder_RSP128.hpp @@ -0,0 +1,316 @@ +/********************************************************************************************************************* +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); + +#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.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, + 34.347f, 34.347f, 34.347f, 34.347f, 36.498f, 36.498f, 36.498f, 36.498f, + 38.648f, 38.648f, 38.648f, 38.648f, 40.666f, 40.666f, 40.666f, 40.666f, + + 42.683f, 42.683f, 42.683f, 42.683f, 44.267f, 44.267f, 44.267f, 44.267f, + 45.851f, 45.851f, 45.851f, 45.851f, 47.435f, 47.435f, 47.435f, 47.435f, + 49.019f, 49.019f, 49.019f, 49.019f, 50.603f, 50.603f, 50.603f, 50.603f, + 52.187f, 52.187f, 52.187f, 52.187f, 53.771f, 53.771f, 53.771f, 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; + + 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; +} + +} // namespace lidar +} // namespace robosense diff --git a/src/rs_driver/driver/decoder/decoder_RSP48.hpp b/src/rs_driver/driver/decoder/decoder_RSP48.hpp new file mode 100644 index 00000000..75dd1c17 --- /dev/null +++ b/src/rs_driver/driver/decoder/decoder_RSP48.hpp @@ -0,0 +1,305 @@ +/********************************************************************************************************************* +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); + +#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; + + 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; +} + +} // namespace lidar +} // namespace robosense diff --git a/src/rs_driver/driver/decoder/decoder_RSP80.hpp b/src/rs_driver/driver/decoder/decoder_RSP80.hpp new file mode 100644 index 00000000..765bc871 --- /dev/null +++ b/src/rs_driver/driver/decoder/decoder_RSP80.hpp @@ -0,0 +1,349 @@ +/********************************************************************************************************************* +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); + +#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; + + 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; +} + +} // namespace lidar +} // namespace robosense diff --git a/src/rs_driver/driver/decoder/decoder_RSROCK.hpp b/src/rs_driver/driver/decoder/decoder_RSROCK.hpp deleted file mode 100644 index c96dd831..00000000 --- a/src/rs_driver/driver/decoder/decoder_RSROCK.hpp +++ /dev/null @@ -1,226 +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 azimuth; - RSChannel channels[4]; -} RSROCKChannel; - -typedef struct -{ - uint16_t id; - RSROCKChannel channels[14]; -} RSROCKMsopBlock; - -typedef struct -{ - RSMsopHeader header; - RSROCKMsopBlock blocks[6]; - unsigned int index; - uint16_t tail; -} RSROCKMsopPkt; - -// TODO -typedef struct -{ - uint64_t id; - uint16_t rpm; - RSEthNet eth; - RSFOV fov; - uint16_t reserved0; - uint16_t phase_lock_angle; - RSVersion version; - uint8_t reserved_1[242]; - RSSn sn; - uint16_t zero_cali; - uint8_t return_mode; - uint16_t sw_ver; - RSTimestampYMD timestamp; - RSStatus status; - uint8_t reserved_2[5]; - RSDiagno diagno; - uint8_t gprmc[86]; - RSCalibrationAngle ver_angle_cali[32]; - RSCalibrationAngle hori_angle_cali[32]; - uint8_t reserved_3[586]; - uint16_t tail; -} RSROCKDifopPkt; - -#pragma pack(pop) - -template -class DecoderRSROCK : public DecoderBase -{ -public: - explicit DecoderRSROCK(const RSDecoderParam& param, const LidarConstantParameter& lidar_const_param); - RSDecoderResult decodeDifopPkt(const uint8_t* pkt); - RSDecoderResult decodeMsopPkt(const uint8_t* pkt, std::vector& vec, int& height, int& azimuth); - double getLidarTime(const uint8_t* pkt); -}; - -template -inline DecoderRSROCK::DecoderRSROCK(const RSDecoderParam& param, - const LidarConstantParameter& lidar_const_param) - : DecoderBase(param, lidar_const_param) -{ - this->vert_angle_list_.resize(this->lidar_const_param_.LASER_NUM); - this->hori_angle_list_.resize(this->lidar_const_param_.LASER_NUM); - this->beam_ring_table_.resize(this->lidar_const_param_.LASER_NUM); - - // TODO temp - this->vert_angle_list_[0] = 5 * 100; - this->vert_angle_list_[1] = 2.5 * 100; - this->vert_angle_list_[2] = 0 * 100; - this->vert_angle_list_[3] = -2.5 * 100; - - this->hori_angle_list_[0] = -0.2 * 100; - this->hori_angle_list_[1] = -0.2 * 100; - this->hori_angle_list_[2] = -0.2 * 100; - this->hori_angle_list_[3] = -0.2 * 100; - - if (this->param_.max_distance > 200.0f) - { - this->param_.max_distance = 200.0f; - } - if (this->param_.min_distance < 0.4f || this->param_.min_distance > this->param_.max_distance) - { - this->param_.min_distance = 0.4f; - } -} - -template -inline double DecoderRSROCK::getLidarTime(const uint8_t* pkt) -{ - return this->template calculateTimeYMD(pkt); -} - -template -inline RSDecoderResult DecoderRSROCK::decodeMsopPkt(const uint8_t* pkt, std::vector& vec, int& height, - int& azimuth) -{ - height = this->lidar_const_param_.LASER_NUM; - const RSROCKMsopPkt* mpkt_ptr = reinterpret_cast(pkt); - if (mpkt_ptr->header.id != this->lidar_const_param_.MSOP_ID) - { - return RSDecoderResult::WRONG_PKT_HEADER; - } - - azimuth = RS_SWAP_SHORT(mpkt_ptr->blocks[0].channels[0].azimuth); - this->current_temperature_ = this->computeTemperature(mpkt_ptr->header.temp_raw); - double block_timestamp = this->get_point_time_func_(pkt); - this->check_camera_trigger_func_(azimuth, pkt); - float azi_diff = 0; - for (size_t blk_idx = 0; blk_idx < this->lidar_const_param_.BLOCKS_PER_PKT; blk_idx++) - { - if (mpkt_ptr->blocks[blk_idx].id != this->lidar_const_param_.BLOCK_ID) - { - break; - } - - for (size_t firing_idx = 0; - firing_idx < this->lidar_const_param_.CHANNELS_PER_BLOCK / this->lidar_const_param_.LASER_NUM; firing_idx++) - { - int cur_azi = RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx].channels[firing_idx].azimuth); - - azi_diff = 0.0; // TODO - for (int channel_idx = 0; channel_idx < this->lidar_const_param_.LASER_NUM; channel_idx++) - { - float azi_channel_ori = cur_azi + azi_diff * 1; // this->lidar_const_param_.FIRING_FREQUENCY * - // this->lidar_const_param_.DSR_TOFFSET * - // static_cast(2 * (channel_idx % 16) + (channel_idx / 16)); // TODO - int azi_channel_final = this->azimuthCalibration(azi_channel_ori, channel_idx); - float distance = RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx].channels[firing_idx].channels[channel_idx].distance) * - this->lidar_const_param_.DIS_RESOLUTION; - int angle_horiz = static_cast(azi_channel_ori + RS_ONE_ROUND) % RS_ONE_ROUND; - int angle_vert = ((this->vert_angle_list_[channel_idx]) + RS_ONE_ROUND) % RS_ONE_ROUND; - - T_Point point; - if ((distance <= this->param_.max_distance && distance >= this->param_.min_distance) && - ((this->angle_flag_ && azi_channel_final >= this->start_angle_ && azi_channel_final <= this->end_angle_) || - (!this->angle_flag_ && - ((azi_channel_final >= this->start_angle_) || (azi_channel_final <= this->end_angle_))))) - { - float x = distance * this->checkCosTable(angle_vert) * this->checkCosTable(azi_channel_final) + - this->lidar_Rxy_ * this->checkCosTable(angle_horiz - this->lidar_alph0_); - float y = -distance * this->checkCosTable(angle_vert) * this->checkSinTable(azi_channel_final) - - this->lidar_Rxy_ * this->checkSinTable(angle_horiz - this->lidar_alph0_); - float z = distance * this->checkSinTable(angle_vert) + this->lidar_const_param_.RZ; - uint8_t intensity = mpkt_ptr->blocks[blk_idx].channels[firing_idx].channels[channel_idx].intensity; - this->transformPoint(x, y, z); - setX(point, x); - setY(point, y); - setZ(point, z); - setIntensity(point, intensity); - } - else - { - setX(point, NAN); - setY(point, NAN); - setZ(point, NAN); - setIntensity(point, 0); - } - setRing(point, this->beam_ring_table_[channel_idx]); - setTimestamp(point, block_timestamp); - vec.emplace_back(std::move(point)); - } - } - } - return RSDecoderResult::DECODE_OK; -} - -template -inline RSDecoderResult DecoderRSROCK::decodeDifopPkt(const uint8_t* pkt) -{ - const RSROCKDifopPkt* dpkt_ptr = reinterpret_cast(pkt); - if (dpkt_ptr->id != this->lidar_const_param_.DIFOP_ID) - { - return RSDecoderResult::WRONG_PKT_HEADER; - } - this->template decodeDifopCommon(pkt, LidarType::RSROCK); - if (!this->difop_flag_) - { - this->template decodeDifopCalibration(pkt, LidarType::RSROCK); - } - return RSDecoderResult::DECODE_OK; -} - -} // namespace lidar -} // namespace robosense diff --git a/src/rs_driver/driver/decoder/decoder_base.hpp b/src/rs_driver/driver/decoder/decoder_base.hpp deleted file mode 100644 index 5a1ce2b1..00000000 --- a/src/rs_driver/driver/decoder/decoder_base.hpp +++ /dev/null @@ -1,931 +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 -{ -#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 RS_HAS_MEMBER(C, member) has_##member::value -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 RS_SWAP_SHORT(x) ((((x)&0xFF) << 8) | (((x)&0xFF00) >> 8)) -#define RS_SWAP_LONG(x) ((((x)&0xFF) << 24) | (((x)&0xFF00) << 8) | (((x)&0xFF0000) >> 8) | (((x)&0xFF000000) >> 24)) -#define RS_TO_RADS(x) ((x) * (M_PI) / 180) -constexpr float RS_ANGLE_RESOLUTION = 0.01; -constexpr float MICRO = 1000000.0; -constexpr float NANO = 1000000000.0; -constexpr int RS_ONE_ROUND = 36000; -constexpr uint16_t PROTOCOL_VER_0 = 0x00; -/* Echo mode definition */ -enum RSEchoMode -{ - ECHO_SINGLE, - ECHO_DUAL -}; - -/* Decode result definition */ -enum RSDecoderResult -{ - DECODE_OK = 0, - FRAME_SPLIT = 1, - WRONG_PKT_HEADER = -1, - PKT_NULL = -2, - DISCARD_PKT = -3 -}; - -#pragma pack(push, 1) -typedef struct -{ - uint64_t MSOP_ID; - uint64_t DIFOP_ID; - uint64_t BLOCK_ID; - uint32_t PKT_RATE; - uint16_t BLOCKS_PER_PKT; - uint16_t CHANNELS_PER_BLOCK; - uint16_t LASER_NUM; - float DSR_TOFFSET; - float FIRING_FREQUENCY; - float DIS_RESOLUTION; - float RX; - float RY; - float RZ; -} LidarConstantParameter; - -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]; - uint32_t us; -} RSTimestampUTC; - -typedef struct -{ - uint8_t sync_mode; - uint8_t sync_sts; - RSTimestampUTC timestamp; -} RSTimeInfo; - -typedef struct -{ - uint64_t id; - uint8_t reserved_1[12]; - RSTimestampYMD timestamp; - uint8_t lidar_type; - uint8_t reserved_2[7]; - uint16_t temp_raw; - uint8_t reserved_3[2]; -} RSMsopHeader; - -typedef struct -{ - uint32_t id; - uint16_t protocol_version; - uint8_t reserved_1; - uint8_t wave_mode; - uint8_t temp_low; - uint8_t temp_high; - RSTimestampUTC timestamp; - uint8_t reserved_2[10]; - uint8_t lidar_type; - uint8_t reserved_3[49]; -} RSMsopHeaderNew; - -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; -} RSEthNet; - -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; -} RSEthNetNew; - -typedef struct -{ - uint16_t start_angle; - uint16_t end_angle; -} RSFOV; - -typedef struct -{ - uint8_t sign; - uint16_t value; -} RSCalibrationAngle; - -typedef struct -{ - uint16_t distance; - uint8_t intensity; -} RSChannel; - -typedef struct -{ - uint8_t top_ver[5]; - uint8_t bottom_ver[5]; -} RSVersion; - -typedef struct -{ - uint8_t top_firmware_ver[5]; - uint8_t bot_firmware_ver[5]; - uint8_t bot_soft_ver[5]; - uint8_t motor_firmware_ver[5]; - uint8_t hw_ver[3]; -} RSVersionNew; - -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; -} RSStatus; - -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]; -} RSStatusNew; - -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]; -} RSDiagno; - -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]; -} RSDiagnoNew; - -#pragma pack(pop) - -//----------------- Decoder --------------------- -template -class DecoderBase -{ -public: - -#ifdef ENABLE_TRANSFORM - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -#endif - - explicit DecoderBase(const RSDecoderParam& param, const LidarConstantParameter& lidar_const_param); - DecoderBase(const DecoderBase&) = delete; - DecoderBase& operator=(const DecoderBase&) = delete; - virtual ~DecoderBase() = default; - virtual RSDecoderResult processMsopPkt(const uint8_t* pkt, std::vector& point_cloud_vec, int& height); - virtual RSDecoderResult processDifopPkt(const uint8_t* pkt); - virtual void loadAngleFile(const std::string& angle_path); - virtual void regRecvCallback(const std::function& callback); ///< Camera trigger - virtual double getLidarTemperature(); - virtual double getLidarTime(const uint8_t* pkt) = 0; - -protected: - virtual float computeTemperature(const uint16_t& temp_raw); - virtual float computeTemperature(const uint8_t& temp_low, const uint8_t& temp_high); - virtual int azimuthCalibration(const float& azimuth, const int& channel); - virtual void checkTriggerAngle(const int& angle, const double& timestamp); - virtual RSDecoderResult decodeMsopPkt(const uint8_t* pkt, std::vector& vec, int& height, int& azimuth) = 0; - virtual RSDecoderResult decodeDifopPkt(const uint8_t* pkt) = 0; - RSEchoMode getEchoMode(const LidarType& type, const uint8_t& return_mode); - template - double calculateTimeUTC(const uint8_t* pkt, const LidarType& type); - template - double calculateTimeYMD(const uint8_t* pkt); - template - void decodeDifopCommon(const uint8_t* pkt, const LidarType& type); - template - void decodeDifopCalibration(const uint8_t* pkt, const LidarType& type); - void transformPoint(float& x, float& y, float& z); - float checkCosTable(const int& angle); - float checkSinTable(const int& angle); - void sortBeamTable(); - -private: - std::vector initTrigonometricLookupTable(const std::function& func); - -protected: - LidarConstantParameter lidar_const_param_; - RSDecoderParam param_; - RSEchoMode echo_mode_; - unsigned int pkts_per_frame_; - unsigned int pkt_count_; - unsigned int trigger_index_; - unsigned int prev_angle_diff_; - unsigned int rpm_; - unsigned int protocol_ver_; - int start_angle_; - int end_angle_; - int cut_angle_; - int last_azimuth_; - bool angle_flag_; - bool difop_flag_; - float fov_time_jump_diff_; - float time_duration_between_blocks_; - float current_temperature_; - float azi_diff_between_block_theoretical_; - std::vector vert_angle_list_; - std::vector hori_angle_list_; - std::vector beam_ring_table_; - std::vector> camera_trigger_cb_vec_; - std::function get_point_time_func_; - std::function check_camera_trigger_func_; - int lidar_alph0_; // atan2(Ry, Rx) * 180 / M_PI * 100 - float lidar_Rxy_; // sqrt(Rx*Rx + Ry*Ry) - -private: - std::vector cos_lookup_table_; - std::vector sin_lookup_table_; -}; - -template -inline DecoderBase::DecoderBase(const RSDecoderParam& param, const LidarConstantParameter& lidar_const_param) - : lidar_const_param_(lidar_const_param) - , param_(param) - , echo_mode_(ECHO_SINGLE) - , pkts_per_frame_(lidar_const_param.PKT_RATE / 10) - , pkt_count_(0) - , trigger_index_(0) - , prev_angle_diff_(RS_ONE_ROUND) - , rpm_(600) - , protocol_ver_(0) - , start_angle_(param.start_angle * 100) - , end_angle_(param.end_angle * 100) - , cut_angle_(param.cut_angle * 100) - , last_azimuth_(-36001) - , angle_flag_(true) - , difop_flag_(false) - , fov_time_jump_diff_(0) - , time_duration_between_blocks_(0) - , current_temperature_(0) - , azi_diff_between_block_theoretical_(20) -{ - if (cut_angle_ > RS_ONE_ROUND) - { - cut_angle_ = 0; - } - if (this->start_angle_ > RS_ONE_ROUND || this->start_angle_ < 0 || this->end_angle_ > RS_ONE_ROUND || - this->end_angle_ < 0) - { - RS_WARNING << "start_angle & end_angle should be in range 0~360° " << RS_REND; - this->start_angle_ = 0; - this->end_angle_ = RS_ONE_ROUND; - } - if (this->start_angle_ > this->end_angle_) - { - this->angle_flag_ = false; - } - - // even though T_Point does not have timestamp, it gives the timestamp - /* Point time function*/ - // if (RS_HAS_MEMBER(T_Point, timestamp)) ///< return the timestamp of the first block in one packet - // { - if (this->param_.use_lidar_clock) - { - get_point_time_func_ = [this](const uint8_t* pkt) { return getLidarTime(pkt); }; - } - else - { - get_point_time_func_ = [this](const uint8_t* pkt) { - double ret_time = getTime() - (this->lidar_const_param_.BLOCKS_PER_PKT - 1) * this->time_duration_between_blocks_; - return ret_time; - }; - } - // } - // else - // { - // get_point_time_func_ = [this](const uint8_t* pkt) { return 0; }; - // } - /*Camera trigger function*/ - if (param.trigger_param.trigger_map.size() != 0) - { - if (this->param_.use_lidar_clock) - { - check_camera_trigger_func_ = [this](const int& azimuth, const uint8_t* pkt) { - checkTriggerAngle(azimuth, getLidarTime(pkt)); - }; - } - else - { - check_camera_trigger_func_ = [this](const int& azimuth, const uint8_t* pkt) { - checkTriggerAngle(azimuth, getTime()); - }; - } - } - else - { - check_camera_trigger_func_ = [this](const int& azimuth, const uint8_t* pkt) { return; }; - } - - /* Cos & Sin look-up table*/ - cos_lookup_table_ = initTrigonometricLookupTable([](const double rad) -> double { return std::cos(rad); }); - sin_lookup_table_ = initTrigonometricLookupTable([](const double rad) -> double { return std::sin(rad); }); - - /* Calulate the lidar_alph0 and lidar_Rxy */ - lidar_alph0_ = std::atan2(lidar_const_param_.RY, lidar_const_param_.RX) * 180 / M_PI * 100; - lidar_Rxy_ = std::sqrt(lidar_const_param_.RX * lidar_const_param_.RX + lidar_const_param_.RY * lidar_const_param_.RY); -} - -template -inline RSDecoderResult DecoderBase::processDifopPkt(const uint8_t* pkt) -{ - if (pkt == NULL) - { - return PKT_NULL; - } - return decodeDifopPkt(pkt); -} - -template -inline RSDecoderResult DecoderBase::processMsopPkt(const uint8_t* pkt, std::vector& point_cloud_vec, - int& height) -{ - if (pkt == NULL) - { - return PKT_NULL; - } - int azimuth = 0; - RSDecoderResult ret = decodeMsopPkt(pkt, point_cloud_vec, height, azimuth); - if (ret != RSDecoderResult::DECODE_OK) - { - return ret; - } - this->pkt_count_++; - switch (this->param_.split_frame_mode) - { - case SplitFrameMode::SPLIT_BY_ANGLE: - if (azimuth < this->last_azimuth_) - { - this->last_azimuth_ -= RS_ONE_ROUND; - } - if (this->last_azimuth_ != -36001 && this->last_azimuth_ < this->cut_angle_ && azimuth >= this->cut_angle_) - { - this->last_azimuth_ = azimuth; - this->pkt_count_ = 0; - this->trigger_index_ = 0; - this->prev_angle_diff_ = RS_ONE_ROUND; - return FRAME_SPLIT; - } - this->last_azimuth_ = azimuth; - break; - case SplitFrameMode::SPLIT_BY_FIXED_PKTS: - if (this->pkt_count_ >= this->pkts_per_frame_) - { - this->pkt_count_ = 0; - this->trigger_index_ = 0; - this->prev_angle_diff_ = RS_ONE_ROUND; - return FRAME_SPLIT; - } - break; - case SplitFrameMode::SPLIT_BY_CUSTOM_PKTS: - if (this->pkt_count_ >= this->param_.num_pkts_split) - { - this->pkt_count_ = 0; - this->trigger_index_ = 0; - this->prev_angle_diff_ = RS_ONE_ROUND; - return FRAME_SPLIT; - } - break; - default: - break; - } - return DECODE_OK; -} - -template -inline void DecoderBase::regRecvCallback(const std::function& callback) -{ - camera_trigger_cb_vec_.emplace_back(callback); -} - -template -inline double DecoderBase::getLidarTemperature() -{ - return current_temperature_; -} - -template -inline void DecoderBase::loadAngleFile(const std::string& angle_path) -{ - std::string line_str; - std::ifstream fd_angle(angle_path.c_str(), std::ios::in); - if (fd_angle.is_open()) - { - unsigned int row_index = 0; - while (std::getline(fd_angle, line_str)) - { - std::stringstream ss(line_str); - std::string str; - std::vector vect_str; - while (std::getline(ss, str, ',')) - { - vect_str.emplace_back(str); - } - try - { - this->vert_angle_list_[row_index] = std::stof(vect_str.at(0)) * 100; // degree - this->hori_angle_list_[row_index] = std::stof(vect_str.at(1)) * 100; // degree - } - catch (...) - { - RS_WARNING << "Wrong calibration file format! Please check your angle.csv file!" << RS_REND; - break; - } - row_index++; - if (row_index >= this->lidar_const_param_.LASER_NUM) - { - this->sortBeamTable(); - break; - } - } - fd_angle.close(); - } -} - -template -inline void DecoderBase::checkTriggerAngle(const int& angle, const double& timestamp) -{ - std::map::iterator iter = param_.trigger_param.trigger_map.begin(); - for (size_t i = 0; i < trigger_index_; i++) - { - iter++; - } - if (iter != param_.trigger_param.trigger_map.end()) - { - unsigned int angle_diff = std::abs(iter->first * 100 - angle); - if (angle_diff < prev_angle_diff_) - { - prev_angle_diff_ = angle_diff; - return; - } - else - { - trigger_index_++; - prev_angle_diff_ = RS_ONE_ROUND; - for (auto cb : camera_trigger_cb_vec_) - { - cb(std::make_pair(iter->second, timestamp)); - } - } - } -} - -/* 16, 32, BP & RSHELIOS */ -template -inline float DecoderBase::computeTemperature(const uint16_t& temp_raw) -{ - uint8_t neg_flag = (temp_raw >> 8) & 0x80; - float msb = (temp_raw >> 8) & 0x7F; - float lsb = (temp_raw & 0x00FF) >> 3; - float temp; - if (neg_flag == 0x80) - { - temp = -1 * (msb * 32 + lsb) * 0.0625f; - } - else - { - temp = (msb * 32 + lsb) * 0.0625f; - } - - return temp; -} - -/* 128 & 80 */ -template -inline float DecoderBase::computeTemperature(const uint8_t& temp_low, const uint8_t& temp_high) -{ - uint8_t neg_flag = temp_low & 0x80; - float msb = temp_low & 0x7F; - float lsb = temp_high >> 4; - float temp; - if (neg_flag == 0x80) - { - temp = -1 * (msb * 16 + lsb) * 0.0625f; - } - else - { - temp = (msb * 16 + lsb) * 0.0625f; - } - - return temp; -} - -template -inline int DecoderBase::azimuthCalibration(const float& azimuth, const int& channel) -{ - return (static_cast(azimuth) + this->hori_angle_list_[channel] + RS_ONE_ROUND) % RS_ONE_ROUND; -} - -template -template -inline void DecoderBase::decodeDifopCommon(const uint8_t* pkt, const LidarType& type) -{ - const T_Difop* dpkt_ptr = reinterpret_cast(pkt); - this->echo_mode_ = this->getEchoMode(type, dpkt_ptr->return_mode); - this->rpm_ = RS_SWAP_SHORT(dpkt_ptr->rpm); - if (this->rpm_ == 0) - { - RS_WARNING << "LiDAR RPM is 0" << RS_REND; - this->rpm_ = 600; - } - this->time_duration_between_blocks_ = - (60 / static_cast(this->rpm_)) / - ((this->lidar_const_param_.PKT_RATE * 60 / this->rpm_) * this->lidar_const_param_.BLOCKS_PER_PKT); - int fov_start_angle = RS_SWAP_SHORT(dpkt_ptr->fov.start_angle); - int fov_end_angle = RS_SWAP_SHORT(dpkt_ptr->fov.end_angle); - int fov_range = (fov_start_angle < fov_end_angle) ? (fov_end_angle - fov_start_angle) : - (RS_ONE_ROUND - fov_start_angle + fov_end_angle); - int blocks_per_round = - (this->lidar_const_param_.PKT_RATE / (this->rpm_ / 60)) * this->lidar_const_param_.BLOCKS_PER_PKT; - - this->fov_time_jump_diff_ = - this->time_duration_between_blocks_ * (fov_range / (RS_ONE_ROUND / static_cast(blocks_per_round))); - - if (this->echo_mode_ == RSEchoMode::ECHO_DUAL) - { - this->pkts_per_frame_ = ceil(2 * this->lidar_const_param_.PKT_RATE * 60 / this->rpm_); - } - else - { - this->pkts_per_frame_ = ceil(this->lidar_const_param_.PKT_RATE * 60 / this->rpm_); - } - this->azi_diff_between_block_theoretical_ = - (RS_ONE_ROUND / this->lidar_const_param_.BLOCKS_PER_PKT) / - static_cast(this->pkts_per_frame_); ///< ((rpm/60)*360)/pkts_rate/blocks_per_pkt -} - -template -template -inline void DecoderBase::decodeDifopCalibration(const uint8_t* pkt, const LidarType& type) -{ - const T_Difop* dpkt_ptr = reinterpret_cast(pkt); - const uint8_t* p_ver_cali = reinterpret_cast(dpkt_ptr->ver_angle_cali); - if ((p_ver_cali[0] == 0x00 || p_ver_cali[0] == 0xFF) && (p_ver_cali[1] == 0x00 || p_ver_cali[1] == 0xFF) && - (p_ver_cali[2] == 0x00 || p_ver_cali[2] == 0xFF) && (p_ver_cali[3] == 0x00 || p_ver_cali[3] == 0xFF)) - { - return; - } - int neg = 1; - for (size_t i = 0; i < this->lidar_const_param_.LASER_NUM; i++) - { - /* vert angle calibration data */ - neg = static_cast(dpkt_ptr->ver_angle_cali[i].sign) == 0 ? 1 : -1; - this->vert_angle_list_[i] = static_cast(RS_SWAP_SHORT(dpkt_ptr->ver_angle_cali[i].value)) * neg; - /* horizon angle calibration data */ - neg = static_cast(dpkt_ptr->hori_angle_cali[i].sign) == 0 ? 1 : -1; - this->hori_angle_list_[i] = static_cast(RS_SWAP_SHORT(dpkt_ptr->hori_angle_cali[i].value)) * neg; - if (type == LidarType::RS32) - { - this->vert_angle_list_[i] *= 0.1f; - this->hori_angle_list_[i] *= 0.1f; - } - } - this->sortBeamTable(); - this->difop_flag_ = true; -} - -template -template -inline double DecoderBase::calculateTimeUTC(const uint8_t* pkt, const LidarType& type) -{ - const T_Msop* mpkt_ptr = reinterpret_cast(pkt); - union u_ts - { - uint8_t data[8]; - uint64_t ts; - } timestamp; - timestamp.data[7] = 0; - timestamp.data[6] = 0; - timestamp.data[5] = mpkt_ptr->header.timestamp.sec[0]; - timestamp.data[4] = mpkt_ptr->header.timestamp.sec[1]; - timestamp.data[3] = mpkt_ptr->header.timestamp.sec[2]; - timestamp.data[2] = mpkt_ptr->header.timestamp.sec[3]; - timestamp.data[1] = mpkt_ptr->header.timestamp.sec[4]; - timestamp.data[0] = mpkt_ptr->header.timestamp.sec[5]; - - if ((type == LidarType::RS80 || type == LidarType::RS128) && this->protocol_ver_ == PROTOCOL_VER_0) - { - return static_cast(timestamp.ts) + - (static_cast(RS_SWAP_LONG(mpkt_ptr->header.timestamp.us))) / NANO; - } - - return static_cast(timestamp.ts) + (static_cast(RS_SWAP_LONG(mpkt_ptr->header.timestamp.us))) / MICRO; -} - -template -template -inline double DecoderBase::calculateTimeYMD(const uint8_t* pkt) -{ -#ifdef _MSC_VER - long timezone = 0; - _get_timezone(&timezone); -#else - tzset(); -#endif - const T_Msop* mpkt_ptr = reinterpret_cast(pkt); - std::tm stm; - memset(&stm, 0, sizeof(stm)); - stm.tm_year = mpkt_ptr->header.timestamp.year + 100; - stm.tm_mon = mpkt_ptr->header.timestamp.month - 1; - stm.tm_mday = mpkt_ptr->header.timestamp.day; - stm.tm_hour = mpkt_ptr->header.timestamp.hour; - stm.tm_min = mpkt_ptr->header.timestamp.minute; - stm.tm_sec = mpkt_ptr->header.timestamp.second; - return std::mktime(&stm) + static_cast(RS_SWAP_SHORT(mpkt_ptr->header.timestamp.ms)) / 1000.0 + - static_cast(RS_SWAP_SHORT(mpkt_ptr->header.timestamp.us)) / 1000000.0 - static_cast(timezone); -} - -template -inline void DecoderBase::transformPoint(float& x, float& y, float& z) -{ -#ifdef ENABLE_TRANSFORM - static Eigen::AngleAxisd current_rotation_x(param_.transform_param.roll, Eigen::Vector3d::UnitX()); - static Eigen::AngleAxisd current_rotation_y(param_.transform_param.pitch, Eigen::Vector3d::UnitY()); - static Eigen::AngleAxisd current_rotation_z(param_.transform_param.yaw, Eigen::Vector3d::UnitZ()); - static Eigen::Translation3d current_translation(param_.transform_param.x, param_.transform_param.y, - param_.transform_param.z); - static Eigen::Matrix4d trans = (current_translation * current_rotation_z * current_rotation_y * current_rotation_x).matrix(); - 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 DecoderBase::sortBeamTable() -{ - std::vector sorted_idx(this->lidar_const_param_.LASER_NUM); - std::iota(sorted_idx.begin(), sorted_idx.end(), 0); - std::sort(sorted_idx.begin(), sorted_idx.end(), [this](std::size_t i1, std::size_t i2) -> bool { - return this->vert_angle_list_[i1] < this->vert_angle_list_[i2]; - }); - for (size_t i = 0; i < this->lidar_const_param_.LASER_NUM; i++) - { - this->beam_ring_table_[sorted_idx[i]] = i; - } -} - -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 RSEchoMode DecoderBase::getEchoMode(const LidarType& type, const uint8_t& return_mode) -{ - switch (type) - { - case LidarType::RS128_40: - switch (return_mode) - { - case 0x00: - case 0x01: - case 0x02: - return RSEchoMode::ECHO_SINGLE; - case 0x03: - case 0x04: - case 0x05: - default: - return RSEchoMode::ECHO_DUAL; - } - case LidarType::RS128: - case LidarType::RS80: - case LidarType::RSHELIOS: - switch (return_mode) - { - case 0x00: - case 0x03: - return RSEchoMode::ECHO_DUAL; - default: - return RSEchoMode::ECHO_SINGLE; - } - case LidarType::RS16: - case LidarType::RS32: - case LidarType::RSBP: - switch (return_mode) - { - case 0x00: - return RSEchoMode::ECHO_DUAL; - default: - return RSEchoMode::ECHO_SINGLE; - } - case LidarType::RSM1: - switch (return_mode) - { - case 0x00: - case 0x01: - case 0x02: - case 0x03: - return RSEchoMode::ECHO_DUAL; - default: - return RSEchoMode::ECHO_SINGLE; - } - case LidarType::RSROCK: // TODO - return RSEchoMode::ECHO_SINGLE; - } - return RSEchoMode::ECHO_SINGLE; -} - -template -inline float DecoderBase::checkCosTable(const int& angle) -{ - return cos_lookup_table_[angle + RS_ONE_ROUND]; -} -template -inline float DecoderBase::checkSinTable(const int& angle) -{ - return sin_lookup_table_[angle + RS_ONE_ROUND]; -} - -template -inline std::vector -DecoderBase::initTrigonometricLookupTable(const std::function& func) -{ - std::vector temp_table = std::vector(2 * RS_ONE_ROUND, 0.0); - for (int i = 0; i < 2 * RS_ONE_ROUND; i++) - { - const double rad = RS_TO_RADS(static_cast(i - RS_ONE_ROUND) * RS_ANGLE_RESOLUTION); - temp_table[i] = func(rad); - } - return temp_table; -} - -} // namespace lidar -} // namespace robosense diff --git a/src/rs_driver/driver/decoder/decoder_factory.hpp b/src/rs_driver/driver/decoder/decoder_factory.hpp index 181408ee..7c8bca04 100644 --- a/src/rs_driver/driver/decoder/decoder_factory.hpp +++ b/src/rs_driver/driver/decoder/decoder_factory.hpp @@ -30,255 +30,102 @@ WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWIS 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 +#include +#include +#include +#include +#include namespace robosense { namespace lidar { -template + +template class DecoderFactory { public: - DecoderFactory() = default; - ~DecoderFactory() = default; - static std::shared_ptr> createDecoder(const RSDriverParam& param); -private: - static const LidarConstantParameter getRS16ConstantParam(); - static const LidarConstantParameter getRS32ConstantParam(); - static const LidarConstantParameter getRSBPConstantParam(); - static const LidarConstantParameter getRS80ConstantParam(); - static const LidarConstantParameter getRS128ConstantParam(); - static const LidarConstantParameter getRS128_40ConstantParam(); - static const LidarConstantParameter getRSM1ConstantParam(); - static const LidarConstantParameter getRSHELIOSConstantParam(); - static const LidarConstantParameter getRSROCKConstantParam(); + static std::shared_ptr> createDecoder( + LidarType type, const RSDecoderParam& param); }; -template -inline std::shared_ptr> DecoderFactory::createDecoder(const RSDriverParam& param) +template +inline std::shared_ptr> DecoderFactory::createDecoder( + LidarType type, const RSDecoderParam& param) { - std::shared_ptr> ret_ptr; - switch (param.lidar_type) + std::shared_ptr> ret_ptr; + + switch (type) { case LidarType::RS16: - ret_ptr = std::make_shared>(param.decoder_param, getRS16ConstantParam()); + ret_ptr = std::make_shared>(param); break; case LidarType::RS32: - ret_ptr = std::make_shared>(param.decoder_param, getRS32ConstantParam()); + ret_ptr = std::make_shared>(param); break; case LidarType::RSBP: - ret_ptr = std::make_shared>(param.decoder_param, getRSBPConstantParam()); + ret_ptr = std::make_shared>(param); break; - case LidarType::RS128: - ret_ptr = std::make_shared>(param.decoder_param, getRS128ConstantParam()); + case LidarType::RSBPV4: + ret_ptr = std::make_shared>(param); + break; + case LidarType::RSHELIOS: + ret_ptr = std::make_shared>(param); break; - case LidarType::RS128_40: - ret_ptr = std::make_shared>(param.decoder_param, getRS128_40ConstantParam()); + 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.decoder_param, getRS80ConstantParam()); + 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.decoder_param, getRSM1ConstantParam()); + ret_ptr = std::make_shared>(param); break; - case LidarType::RSHELIOS: - ret_ptr = std::make_shared>(param.decoder_param, getRSHELIOSConstantParam()); + case LidarType::RSM2: + ret_ptr = std::make_shared>(param); + break; + case LidarType::RSEOS: + ret_ptr = std::make_shared>(param); break; - case LidarType::RSROCK: - ret_ptr = std::make_shared>(param.decoder_param, getRSROCKConstantParam()); + case LidarType::RSM1_JUMBO: + ret_ptr = std::make_shared>(param); break; default: RS_ERROR << "Wrong LiDAR Type. Please check your LiDAR Version! " << RS_REND; exit(-1); } - ret_ptr->loadAngleFile(param.angle_path); - return ret_ptr; -} - -template -inline const LidarConstantParameter DecoderFactory::getRS16ConstantParam() -{ - LidarConstantParameter ret_param; - ret_param.MSOP_ID = 0xA050A55A0A05AA55; - ret_param.DIFOP_ID = 0x555511115A00FFA5; - ret_param.BLOCK_ID = 0xEEFF; - ret_param.PKT_RATE = 750; - ret_param.BLOCKS_PER_PKT = 12; - ret_param.CHANNELS_PER_BLOCK = 32; - ret_param.LASER_NUM = 16; - ret_param.DSR_TOFFSET = 2.8; - ret_param.FIRING_FREQUENCY = 0.009; - ret_param.DIS_RESOLUTION = 0.005; - ret_param.RX = 0.03825; - ret_param.RY = -0.01088; - ret_param.RZ = 0; - return ret_param; -} - -template -inline const LidarConstantParameter DecoderFactory::getRS32ConstantParam() -{ - LidarConstantParameter ret_param; - ret_param.MSOP_ID = 0xA050A55A0A05AA55; - ret_param.DIFOP_ID = 0x555511115A00FFA5; - ret_param.BLOCK_ID = 0xEEFF; - ret_param.PKT_RATE = 1500; - ret_param.BLOCKS_PER_PKT = 12; - ret_param.CHANNELS_PER_BLOCK = 32; - ret_param.LASER_NUM = 32; - ret_param.DSR_TOFFSET = 1.44; - ret_param.FIRING_FREQUENCY = 0.018; - ret_param.DIS_RESOLUTION = 0.005; - ret_param.RX = 0.03997; - ret_param.RY = -0.01087; - ret_param.RZ = 0; - return ret_param; -} - -template -inline const LidarConstantParameter DecoderFactory::getRSBPConstantParam() -{ - LidarConstantParameter ret_param; - ret_param.MSOP_ID = 0xA050A55A0A05AA55; - ret_param.DIFOP_ID = 0x555511115A00FFA5; - ret_param.BLOCK_ID = 0xEEFF; - ret_param.PKT_RATE = 1500; - ret_param.BLOCKS_PER_PKT = 12; - ret_param.CHANNELS_PER_BLOCK = 32; - ret_param.LASER_NUM = 32; - ret_param.DSR_TOFFSET = 1.28; - ret_param.FIRING_FREQUENCY = 0.018; - ret_param.DIS_RESOLUTION = 0.005; - ret_param.RX = 0.01473; - ret_param.RY = 0.0085; - ret_param.RZ = 0.09427; - return ret_param; -} - -template -inline const LidarConstantParameter DecoderFactory::getRS80ConstantParam() -{ - LidarConstantParameter ret_param; - ret_param.MSOP_ID = 0x5A05AA55; - ret_param.DIFOP_ID = 0x555511115A00FFA5; - ret_param.BLOCK_ID = 0xFE; - ret_param.PKT_RATE = 4500; - ret_param.BLOCKS_PER_PKT = 4; - ret_param.CHANNELS_PER_BLOCK = 80; - ret_param.LASER_NUM = 80; - ret_param.DSR_TOFFSET = 3.236; - ret_param.FIRING_FREQUENCY = 0.018; - ret_param.DIS_RESOLUTION = 0.005; - ret_param.RX = 0.03615; - ret_param.RY = -0.017; - ret_param.RZ = 0; - return ret_param; -} - -template -inline const LidarConstantParameter DecoderFactory::getRS128ConstantParam() -{ - LidarConstantParameter ret_param; - ret_param.MSOP_ID = 0x5A05AA55; - ret_param.DIFOP_ID = 0x555511115A00FFA5; - ret_param.BLOCK_ID = 0xFE; - ret_param.PKT_RATE = 6000; - ret_param.BLOCKS_PER_PKT = 3; - ret_param.CHANNELS_PER_BLOCK = 128; - ret_param.LASER_NUM = 128; - ret_param.DSR_TOFFSET = 3.236; - ret_param.FIRING_FREQUENCY = 0.018; - ret_param.DIS_RESOLUTION = 0.005; - ret_param.RX = 0.03615; - ret_param.RY = -0.017; - ret_param.RZ = 0; - return ret_param; -} - -template -inline const LidarConstantParameter DecoderFactory::getRS128_40ConstantParam() -{ - LidarConstantParameter ret_param; - ret_param.MSOP_ID = 0x5A05AA55; - ret_param.DIFOP_ID = 0x555511115A00FFA5; - ret_param.BLOCK_ID = 0xFE; - ret_param.PKT_RATE = 6000; - ret_param.BLOCKS_PER_PKT = 3; - ret_param.CHANNELS_PER_BLOCK = 128; - ret_param.LASER_NUM = 128; - ret_param.DSR_TOFFSET = 3.236; - ret_param.FIRING_FREQUENCY = 0.018; - ret_param.DIS_RESOLUTION = 0.005; - ret_param.RX = 0.02892; //0.03615; - ret_param.RY = -0.013; //-0.017; - ret_param.RZ = 0;//0.0735; //0; - return ret_param; -} - -template -inline const LidarConstantParameter DecoderFactory::getRSM1ConstantParam() -{ - LidarConstantParameter ret_param; - ret_param.MSOP_ID = 0xA55AAA55; - ret_param.DIFOP_ID = 0x555511115A00FFA5; - ret_param.BLOCKS_PER_PKT = 25; - ret_param.CHANNELS_PER_BLOCK = 5; - ret_param.LASER_NUM = 5; - ret_param.DIS_RESOLUTION = 0.005; - return ret_param; -} - -template -inline const LidarConstantParameter DecoderFactory::getRSHELIOSConstantParam() -{ - LidarConstantParameter ret_param; - ret_param.MSOP_ID = 0x5A05AA55; - ret_param.DIFOP_ID = 0x555511115A00FFA5; - ret_param.BLOCK_ID = 0xEEFF; - ret_param.PKT_RATE = 1500; - ret_param.BLOCKS_PER_PKT = 12; - ret_param.CHANNELS_PER_BLOCK = 32; - ret_param.LASER_NUM = 32; - ret_param.DSR_TOFFSET = 1.0; - ret_param.FIRING_FREQUENCY = 0.018; - ret_param.DIS_RESOLUTION = 0.0025; - ret_param.RX = 0.03498; - ret_param.RY = -0.015; - ret_param.RZ = 0.0; - return ret_param; -} -// TODO -template -inline const LidarConstantParameter DecoderFactory::getRSROCKConstantParam() -{ - LidarConstantParameter ret_param; - ret_param.MSOP_ID = 0x000001005A05AA55; - ret_param.DIFOP_ID = 0x555511115A00FFA5; - ret_param.BLOCK_ID = 0xEEFF; - ret_param.PKT_RATE = 1071; // TODO - ret_param.BLOCKS_PER_PKT = 6; - ret_param.CHANNELS_PER_BLOCK = 14 * 4; // TODO - ret_param.LASER_NUM = 4; - ret_param.DSR_TOFFSET = 1.0; - ret_param.FIRING_FREQUENCY = 0.018; - ret_param.DIS_RESOLUTION = 0.0025; - ret_param.RX = 0.07526; - ret_param.RY = 0.00968; - ret_param.RZ = 0.0; - return ret_param; + return ret_ptr; } } // namespace lidar diff --git a/src/rs_driver/driver/decoder/decoder_mech.hpp b/src/rs_driver/driver/decoder/decoder_mech.hpp new file mode 100644 index 00000000..92743256 --- /dev/null +++ b/src/rs_driver/driver/decoder/decoder_mech.hpp @@ -0,0 +1,203 @@ +/********************************************************************************************************************* +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 +{ + +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; + RSFOV fov; + uint8_t return_mode; + 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 + + 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) +}; + +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_); + break; + + case SplitFrameMode::SPLIT_BY_CUSTOM_BLKS: + 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); + 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; + } + } +} + +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); + } +} + +} // namespace lidar +} // namespace robosense diff --git a/src/rs_driver/driver/decoder/member_checker.hpp b/src/rs_driver/driver/decoder/member_checker.hpp new file mode 100644 index 00000000..cd5595ed --- /dev/null +++ b/src/rs_driver/driver/decoder/member_checker.hpp @@ -0,0 +1,125 @@ +/********************************************************************************************************************* +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 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; +} + diff --git a/src/rs_driver/utility/lock_queue.h b/src/rs_driver/driver/decoder/section.hpp similarity index 69% rename from src/rs_driver/utility/lock_queue.h rename to src/rs_driver/driver/decoder/section.hpp index 7f33e118..9865a157 100644 --- a/src/rs_driver/utility/lock_queue.h +++ b/src/rs_driver/driver/decoder/section.hpp @@ -31,71 +31,68 @@ THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *********************************************************************************************************************/ #pragma once -#include + namespace robosense { namespace lidar { -template -class Queue + +class AzimuthSection { public: - inline Queue():is_task_finished_(true) + AzimuthSection(int32_t start, int32_t end) { - } + full_round_ = (start == 0) && (end == 36000); - inline T front() - { - std::lock_guard lock(mutex_); - return queue_.front(); + start_ = start % 36000; + end_ = end % 36000; + cross_zero_ = (start_ > end_); } - inline void push(const T& value) + bool in(int32_t angle) { - std::lock_guard lock(mutex_); - queue_.push(value); - } + if (full_round_) + return true; - inline void pop() - { - std::lock_guard lock(mutex_); - if (!queue_.empty()) + if (cross_zero_) { - queue_.pop(); + return (angle >= start_) || (angle < end_); } - } - - inline T popFront() - { - T value; - std::lock_guard lock(mutex_); - if (!queue_.empty()) + else { - value = std::move(queue_.front()); - queue_.pop(); + return (angle >= start_) && (angle < end_); } - return value; } - inline void clear() +#ifndef UNIT_TEST +private: +#endif + 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_((usr_min > min) ? usr_min : min), max_((usr_max < max) ? usr_max : max) { - std::queue empty; - std::lock_guard lock(mutex_); - swap(empty, queue_); } - inline size_t size() + bool in(float distance) { - std::lock_guard lock(mutex_); - return queue_.size(); + return ((min_ <= distance) && (distance <= max_)); } -public: - std::queue queue_; - std::atomic is_task_finished_; - +#ifndef UNIT_TEST private: - mutable std::mutex mutex_; +#endif + + float min_; + float max_; }; + } // namespace lidar -} // namespace robosense \ No newline at end of file +} // namespace robosense diff --git a/src/rs_driver/utility/thread_pool.hpp b/src/rs_driver/driver/decoder/split_strategy.hpp similarity index 53% rename from src/rs_driver/utility/thread_pool.hpp rename to src/rs_driver/driver/decoder/split_strategy.hpp index a581cd1e..e8200f5d 100644 --- a/src/rs_driver/utility/thread_pool.hpp +++ b/src/rs_driver/driver/decoder/split_strategy.hpp @@ -31,82 +31,139 @@ THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *********************************************************************************************************************/ #pragma once -#include + namespace robosense { namespace lidar { -constexpr uint16_t MAX_THREAD_NUM = 4; -struct Thread + +class SplitStrategy { - Thread() : start_(false) - { - } - std::shared_ptr thread_; - std::atomic start_; +public: + virtual bool newBlock(int32_t angle) = 0; + virtual ~SplitStrategy() = default; }; -class ThreadPool + +class SplitStrategyByAngle : public SplitStrategy { public: - typedef std::shared_ptr Ptr; - inline ThreadPool() : stop_flag_(false), idl_thr_num_(MAX_THREAD_NUM) + SplitStrategyByAngle (int32_t split_angle) + : split_angle_(split_angle), prev_angle_(split_angle) { - for (int i = 0; i < idl_thr_num_; ++i) + } + + 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) { - pool_.emplace_back([this] { - while (!this->stop_flag_) - { - std::function task; - { - std::unique_lock lock{ this->mutex_ }; - this->cv_task_.wait(lock, [this] { return this->stop_flag_.load() || !this->tasks_.empty(); }); - if (this->stop_flag_ && this->tasks_.empty()) - return; - task = std::move(this->tasks_.front()); - this->tasks_.pop(); - } - idl_thr_num_--; - task(); - idl_thr_num_++; - } - }); + 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) + { } - inline ~ThreadPool() + virtual ~SplitStrategyByNum() = default; + + virtual bool newBlock(int32_t) { - stop_flag_.store(true); - cv_task_.notify_all(); - for (std::thread& thread : pool_) + blks_++; + if (blks_ >= *max_blks_) { - thread.join(); + blks_ = 0; + return true; } + + return false; } +#ifndef UNIT_TEST +private: +#endif + uint16_t* max_blks_; + uint16_t blks_; +}; + +class SplitStrategyBySeq +{ public: - template - inline void commit(F&& f, Args&&... args) + + SplitStrategyBySeq() + : prev_seq_(0) + { + setSafeRange(); + } + + bool newPacket(uint16_t seq) { - if (stop_flag_.load()) - throw std::runtime_error("Commit on LiDAR threadpool is stopped."); - using RetType = decltype(f(args...)); - auto task = - std::make_shared>(std::bind(std::forward(f), std::forward(args)...)); + bool split = false; + + if (seq < safe_seq_min_) // rewind { - std::lock_guard lock{ mutex_ }; - tasks_.emplace([task]() { (*task)(); }); + prev_seq_ = seq; + split = true; } - cv_task_.notify_one(); + 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; } +#ifndef UNIT_TEST private: - using Task = std::function; - std::vector pool_; - std::queue tasks_; - std::mutex mutex_; - std::condition_variable cv_task_; - std::atomic stop_flag_; - std::atomic idl_thr_num_; +#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_; }; + } // namespace lidar -} // namespace robosense \ No newline at end of file +} // namespace robosense diff --git a/src/rs_driver/driver/decoder/trigon.hpp b/src/rs_driver/driver/decoder/trigon.hpp new file mode 100644 index 00000000..b2d73a90 --- /dev/null +++ b/src/rs_driver/driver/decoder/trigon.hpp @@ -0,0 +1,133 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the +following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following +disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following +disclaimer in the documentation and/or other materials provided with the distribution. + +3. Neither the names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, +INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*********************************************************************************************************************/ + +#pragma once + +#include + +#include + +namespace robosense +{ +namespace lidar +{ + +//#define DBG + +class Trigon +{ +public: + + constexpr static int32_t ANGLE_MIN = -9000; + constexpr static int32_t ANGLE_MAX = 45000; + + Trigon() + { + int32_t range = ANGLE_MAX - ANGLE_MIN; +#ifdef DBG + o_angles_ = (int32_t*)malloc(range * sizeof(int32_t)); +#endif + o_sins_ = (float*)malloc(range * sizeof(float)); + o_coss_ = (float*)malloc(range * sizeof(float)); + + for (int32_t i = ANGLE_MIN, j = 0; i < ANGLE_MAX; i++, j++) + { + double rad = DEGREE_TO_RADIAN(static_cast(i) * 0.01); + +#ifdef DBG + o_angles_[j] = i; +#endif + o_sins_[j] = (float)std::sin(rad); + o_coss_[j] = (float)std::cos(rad); + } + +#ifdef DBG + angles_ = o_angles_ - ANGLE_MIN; +#endif + sins_ = o_sins_ - ANGLE_MIN; + coss_ = o_coss_ - ANGLE_MIN; + } + + ~Trigon() + { + free(o_coss_); + free(o_sins_); +#ifdef DBG + free(o_angles_); +#endif + } + + float sin(int32_t angle) + { + if (angle < ANGLE_MIN || angle >= ANGLE_MAX) + { + angle = 0; + } + + return sins_[angle]; + } + + float cos(int32_t angle) + { + if (angle < ANGLE_MIN || angle >= ANGLE_MAX) + { + angle = 0; + } + + return coss_[angle]; + } + + void print() + { + for (int32_t i = -10; i < 10; i++) + { + std::cout << +#ifdef DBG + angles_[i] << "\t" << +#endif + sins_[i] << "\t" << coss_[i] << std::endl; + } + } + +private: +#ifdef DBG + int32_t* o_angles_; + int32_t* angles_; +#endif + float* o_sins_; + float* o_coss_; + float* sins_; + float* coss_; +}; + +} // namespace lidar +} // namespace robosense diff --git a/src/rs_driver/driver/driver_param.h b/src/rs_driver/driver/driver_param.h deleted file mode 100644 index e5cb7164..00000000 --- a/src/rs_driver/driver/driver_param.h +++ /dev/null @@ -1,269 +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 LidarType ///< LiDAR type -{ - RS16 = 1, - RS32, - RSBP, - RS128, - RS128_40, - RS80, - RSHELIOS, - RSROCK, - RSM1 = 10 -}; - -enum SplitFrameMode -{ - SPLIT_BY_ANGLE = 1, - SPLIT_BY_FIXED_PKTS, - SPLIT_BY_CUSTOM_PKTS -}; - -typedef struct RSCameraTriggerParam ///< Camera trigger parameters -{ - std::map trigger_map; ///< Map stored the trigger angle and camera frame id - void print() const - { - RS_INFO << "------------------------------------------------------" << RS_REND; - RS_INFO << " RoboSense Camera Trigger Parameters " << RS_REND; - for (auto iter : trigger_map) - { - RS_INFOL << "camera_frame_id: " << iter.second << " trigger_angle : " << iter.first << RS_REND; - } - RS_INFO << "------------------------------------------------------" << RS_REND; - } -} RSCameraTriggerParam; - -typedef 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; - } -} RSTransformParam; - -typedef struct RSDecoderParam ///< LiDAR decoder parameter -{ - float max_distance = 200.0f; ///< Max distance of point cloud range - float min_distance = 0.2f; ///< Minimum distance of point cloud range - 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 cut_angle; - ///< 2: Split frames by fixed number of packets; - ///< 3: Split frames by custom number of packets (num_pkts_split) - uint32_t num_pkts_split = 1; ///< Number of packets in one frame, only be used when split_frame_mode=3 - float cut_angle = 0.0f; ///< Cut angle(degree) used to split frame, only be used when split_frame_mode=1 - bool use_lidar_clock = false; ///< true: use LiDAR clock as timestamp; false: use system clock as timestamp - RSTransformParam transform_param; ///< Used to transform points - RSCameraTriggerParam trigger_param; ///< Used to trigger camera - void print() const - { - transform_param.print(); - trigger_param.print(); - RS_INFO << "------------------------------------------------------" << RS_REND; - RS_INFO << " RoboSense Decoder Parameters " << RS_REND; - RS_INFOL << "max_distance: " << max_distance << RS_REND; - RS_INFOL << "min_distance: " << min_distance << RS_REND; - RS_INFOL << "start_angle: " << start_angle << RS_REND; - RS_INFOL << "end_angle: " << end_angle << RS_REND; - RS_INFOL << "use_lidar_clock: " << use_lidar_clock << RS_REND; - RS_INFOL << "split_frame_mode: " << split_frame_mode << RS_REND; - RS_INFOL << "num_pkts_split: " << num_pkts_split << RS_REND; - RS_INFOL << "cut_angle: " << cut_angle << RS_REND; - RS_INFOL << "use_lidar_clock: " << use_lidar_clock << RS_REND; - RS_INFO << "------------------------------------------------------" << RS_REND; - } -} RSDecoderParam; - -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 - void print() const - { - RS_INFO << "------------------------------------------------------" << RS_REND; - RS_INFO << " RoboSense Input Parameters " << RS_REND; - RS_INFOL << "multi_cast_address: " << multi_cast_address << RS_REND; - RS_INFOL << "host_address: " << host_address << RS_REND; - RS_INFOL << "msop_port: " << msop_port << RS_REND; - RS_INFOL << "difop_port: " << difop_port << RS_REND; - RS_INFOL << "read_pcap: " << read_pcap << RS_REND; - RS_INFOL << "pcap_repeat: " << pcap_repeat << RS_REND; - RS_INFOL << "pcap_path: " << pcap_path << RS_REND; - RS_INFOL << "use_vlan: " << use_vlan << RS_REND; - RS_INFOL << "use_someip: " << use_someip << RS_REND; - RS_INFO << "------------------------------------------------------" << RS_REND; - } -} RSInputParam; - -typedef struct RSDriverParam ///< The LiDAR driver parameter -{ - 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) - void print() const - { - input_param.print(); - decoder_param.print(); - RS_INFO << "------------------------------------------------------" << RS_REND; - RS_INFOL << " RoboSense Driver Parameters " << RS_REND; - RS_INFOL << "angle_path: " << angle_path << RS_REND; - RS_INFOL << "frame_id: " << frame_id << RS_REND; - RS_INFOL << "wait_for_difop: " << wait_for_difop << RS_REND; - RS_INFOL << "lidar_type: "; - RS_INFO << lidarTypeToStr(lidar_type) << RS_REND; - RS_INFOL << "------------------------------------------------------" << RS_REND; - } - static std::string lidarTypeToStr(const LidarType& type) - { - std::string str = ""; - switch (type) - { - case LidarType::RS16: - str = "RS16"; - break; - case LidarType::RS32: - str = "RS32"; - break; - case LidarType::RSBP: - str = "RSBP"; - break; - case LidarType::RS128: - str = "RS128"; - break; - case LidarType::RS128_40: - str = "RS128_40"; - break; - case LidarType::RS80: - str = "RS80"; - break; - case LidarType::RSM1: - str = "RSM1"; - break; - case LidarType::RSHELIOS: - str = "RSHELIOS"; - break; - case LidarType::RSROCK: - str = "RSROCK"; - break; - default: - str = "ERROR"; - RS_ERROR << "RS_ERROR" << RS_REND; - } - return str; - } - static LidarType strToLidarType(const std::string& type) - { - if (type == "RS16") - { - return lidar::LidarType::RS16; - } - else if (type == "RS32") - { - return lidar::LidarType::RS32; - } - else if (type == "RSBP") - { - return lidar::LidarType::RSBP; - } - else if (type == "RS128") - { - return lidar::LidarType::RS128; - } - else if (type == "RS128_40") - { - return lidar::LidarType::RS128_40; - } - else if (type == "RS80") - { - return lidar::LidarType::RS80; - } - else if (type == "RSM1") - { - return lidar::LidarType::RSM1; - } - else if (type == "RSHELIOS") - { - return lidar::LidarType::RSHELIOS; - } - else if (type == "RSROCK") - { - return lidar::LidarType::RSROCK; - } - else - { - RS_ERROR << "Wrong lidar type: " << type << RS_REND; - RS_ERROR << "Please setup the correct type: RS16, RS32, RSBP, RS128, RS80, RSM1, RSHELIOS" << RS_REND; - exit(-1); - } - } -} RSDriverParam; -} // namespace lidar -} // namespace robosense diff --git a/src/rs_driver/driver/driver_param.hpp b/src/rs_driver/driver/driver_param.hpp new file mode 100644 index 00000000..4a1bcc52 --- /dev/null +++ b/src/rs_driver/driver/driver_param.hpp @@ -0,0 +1,376 @@ +/********************************************************************************************************************* +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 +{ +enum LidarType ///< LiDAR type +{ + // mechanical + RS_MECH = 0x01, + RS16 = RS_MECH, + RS32, + RSBP, + RSBPV4, + RSHELIOS, + RSHELIOS_16P, + RS128, + RS80, + RS48, + RSP128, + RSP80, + RSP48, + + // mems + RS_MEMS = 0x20, + RSM1 = RS_MEMS, + RSM2, + RSEOS, + + // 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) +{ + std::string str = ""; + switch (type) + { + case LidarType::RS16: + str = "RS16"; + break; + case LidarType::RS32: + str = "RS32"; + break; + case LidarType::RSBP: + str = "RSBP"; + break; + case LidarType::RSBPV4: + str = "RSBPV4"; + break; + case LidarType::RSHELIOS: + str = "RSHELIOS"; + break; + case LidarType::RSHELIOS_16P: + str = "RSHELIOS_16P"; + break; + case LidarType::RS128: + str = "RS128"; + break; + case LidarType::RS80: + str = "RS80"; + break; + case LidarType::RS48: + str = "RS48"; + break; + case LidarType::RSP128: + str = "RSP128"; + break; + case LidarType::RSP80: + str = "RSP80"; + break; + case LidarType::RSP48: + str = "RSP48"; + break; + case LidarType::RSM1: + str = "RSM1"; + break; + case LidarType::RSM2: + str = "RSM2"; + break; + case LidarType::RSEOS: + str = "RSEOS"; + break; + case LidarType::RSM1_JUMBO: + str = "RSM1_JUMBO"; + break; + default: + str = "ERROR"; + RS_ERROR << "RS_ERROR" << RS_REND; + } + return str; +} + +inline LidarType strToLidarType(const std::string& type) +{ + if (type == "RS16") + { + return LidarType::RS16; + } + else if (type == "RS32") + { + return LidarType::RS32; + } + else if (type == "RSBP") + { + return LidarType::RSBP; + } + else if (type == "RSBPV4") + { + return LidarType::RSBPV4; + } + else if (type == "RSHELIOS") + { + return LidarType::RSHELIOS; + } + else if (type == "RSHELIOS_16P") + { + return LidarType::RSHELIOS_16P; + } + else if (type == "RS128") + { + return LidarType::RS128; + } + else if (type == "RS80") + { + return LidarType::RS80; + } + else if (type == "RS48") + { + return LidarType::RS48; + } + else if (type == "RSP128") + { + return LidarType::RSP128; + } + else if (type == "RSP80") + { + return LidarType::RSP80; + } + else if (type == "RSP48") + { + return lidar::LidarType::RSP48; + } + else if (type == "RSM1") + { + return lidar::LidarType::RSM1; + } + else if (type == "RSM2") + { + return lidar::LidarType::RSM2; + } + else if (type == "RSEOS") + { + return LidarType::RSEOS; + } + else if (type == "RSM1_JUMBO") + { + return LidarType::RSM1_JUMBO; + } + 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, RSM2, RSEOS, RSM1_JUMBO." + << 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 +{ + bool config_from_file = false; ///< Internal use only for debugging + std::string angle_path = ""; ///< Internal use only for debugging + bool wait_for_difop = true; ///< true: start sending point cloud until receive difop packet + float min_distance = 0.2f; ///< Minimum distance of point cloud range + float max_distance = 200.0f; ///< Max distance of point cloud range + 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 + 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; + RSTransformParam transform_param; ///< Used to transform points + + void print() const + { + RS_INFO << "------------------------------------------------------" << RS_REND; + RS_INFO << " RoboSense Decoder Parameters " << RS_REND; + RS_INFOL << "wait_for_difop: " << wait_for_difop << RS_REND; + RS_INFOL << "min_distance: " << min_distance << RS_REND; + RS_INFOL << "max_distance: " << max_distance << RS_REND; + RS_INFOL << "start_angle: " << start_angle << RS_REND; + RS_INFOL << "end_angle: " << end_angle << RS_REND; + RS_INFOL << "use_lidar_clock: " << use_lidar_clock << RS_REND; + RS_INFOL << "dense_points: " << dense_points << RS_REND; + RS_INFOL << "config_from_file: " << config_from_file << RS_REND; + RS_INFOL << "angle_path: " << angle_path << 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 + 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 + + void print() const + { + RS_INFO << "------------------------------------------------------" << RS_REND; + RS_INFO << " RoboSense Input Parameters " << RS_REND; + RS_INFOL << "msop_port: " << msop_port << RS_REND; + RS_INFOL << "difop_port: " << difop_port << RS_REND; + RS_INFOL << "host_address: " << host_address << RS_REND; + RS_INFOL << "group_address: " << group_address << RS_REND; + RS_INFOL << "pcap_path: " << pcap_path << RS_REND; + RS_INFOL << "pcap_rate: " << pcap_rate << RS_REND; + RS_INFOL << "pcap_repeat: " << pcap_repeat << RS_REND; + RS_INFOL << "use_vlan: " << use_vlan << RS_REND; + RS_INFOL << "user_layer_bytes: " << user_layer_bytes << RS_REND; + RS_INFOL << "tail_layer_bytes: " << tail_layer_bytes << RS_REND; + RS_INFO << "------------------------------------------------------" << RS_REND; + } + +}; + +struct RSDriverParam ///< The LiDAR driver parameter +{ + 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 + + void print() const + { + RS_INFO << "------------------------------------------------------" << RS_REND; + RS_INFO << " RoboSense Driver Parameters " << RS_REND; + RS_INFOL << "input type: " << inputTypeToStr(input_type) << RS_REND; + RS_INFOL << "lidar_type: " << lidarTypeToStr(lidar_type) << RS_REND; + RS_INFOL << "------------------------------------------------------" << RS_REND; + + input_param.print(); + decoder_param.print(); + } + +}; + +} // namespace lidar +} // namespace robosense diff --git a/src/rs_driver/driver/input.hpp b/src/rs_driver/driver/input.hpp deleted file mode 100644 index fb97385d..00000000 --- a/src/rs_driver/driver/input.hpp +++ /dev/null @@ -1,562 +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 -///< 1.0 second / 10 Hz / (360 degree / horiz angle resolution / column per msop packet) * (s to us) -constexpr double RS16_PCAP_SLEEP_DURATION = 1200; ///< us -constexpr double RS32_PCAP_SLEEP_DURATION = 530; ///< us -constexpr double RSBP_PCAP_SLEEP_DURATION = 530; ///< us -constexpr double RS128_PCAP_SLEEP_DURATION = 100; ///< us -constexpr double RS80_PCAP_SLEEP_DURATION = 135; ///< us -constexpr double RSM1_PCAP_SLEEP_DURATION = 90; ///< us -constexpr double RSHELIOS_PCAP_SLEEP_DURATION = 530; ///< us -constexpr double RSROCK_PCAP_SLEEP_DURATION = 530; ///< us TODO -using boost::asio::deadline_timer; -using boost::asio::ip::address; -using boost::asio::ip::udp; - -namespace robosense -{ -namespace lidar -{ -class Input -{ -public: - Input(const LidarType& type, const RSInputParam& input_param, const std::function& excb); - ~Input(); - bool init(); - bool start(); - void stop(); - void regRecvMsopCallback(const std::function& callback); - void regRecvDifopCallback(const std::function& callback); - -private: - inline bool setSocket(const std::string& pkt_type); - inline void getMsopPacket(); - inline void getDifopPacket(); - inline void getPcapPacket(); - inline void checkDifopDeadline(); - inline void checkMsopDeadline(); - static void handleReceive(const boost::system::error_code& ec, std::size_t length, boost::system::error_code* out_ec, - std::size_t* out_length); - -private: - LidarType lidar_type_; - RSInputParam input_param_; - std::function excb_; - bool init_flag_; - uint32_t msop_pkt_length_; - uint32_t difop_pkt_length_; - /* pcap file parse */ - pcap_t* pcap_; - bpf_program pcap_msop_filter_; - bpf_program pcap_difop_filter_; - std::chrono::time_point last_packet_time_; - /* live socket */ - std::unique_ptr msop_sock_ptr_; - std::unique_ptr difop_sock_ptr_; - std::unique_ptr msop_deadline_; - std::unique_ptr difop_deadline_; - Thread msop_thread_; - Thread difop_thread_; - Thread pcap_thread_; - int vlan_offset_; - int someip_offset_; - int custom_proto_offset_; - int pcap_offset_; - boost::asio::io_service msop_io_service_; - boost::asio::io_service difop_io_service_; - std::vector> difop_cb_; - std::vector> msop_cb_; -}; - -inline Input::Input(const LidarType& type, const RSInputParam& input_param, - const std::function& excb) - : lidar_type_(type) - , input_param_(input_param) - , excb_(excb) - , init_flag_(false) - , pcap_(nullptr) - , vlan_offset_(0) - , someip_offset_(0) - , custom_proto_offset_(0) - , pcap_offset_(42) -{ - last_packet_time_ = std::chrono::system_clock::now(); - input_param_.pcap_rate = input_param_.pcap_rate < 0.1 ? 0.1 : input_param_.pcap_rate; - switch (type) - { - case LidarType::RSM1: - msop_pkt_length_ = MEMS_MSOP_LEN; - difop_pkt_length_ = MEMS_DIFOP_LEN; - break; - case LidarType::RSROCK: - msop_pkt_length_ = 1236; // TODO - difop_pkt_length_ = MECH_PKT_LEN; - break; - default: - msop_pkt_length_ = MECH_PKT_LEN; - difop_pkt_length_ = MECH_PKT_LEN; - break; - } - if (input_param_.use_vlan) - { - vlan_offset_ = 4; - } - if (input_param_.use_someip) - { - someip_offset_ = 16; - } - if (input_param_.use_custom_proto) - { - custom_proto_offset_ = 8; - } -} - -inline Input::~Input() -{ - stop(); - if (pcap_ != NULL) - { - pcap_close(pcap_); - } - msop_sock_ptr_.reset(); - difop_sock_ptr_.reset(); - msop_deadline_.reset(); - difop_deadline_.reset(); -} - -inline bool Input::init() -{ - if (input_param_.read_pcap) - { - char errbuf[PCAP_ERRBUF_SIZE]; - if ((pcap_ = pcap_open_offline(input_param_.pcap_path.c_str(), errbuf)) == NULL) - { - excb_(Error(ERRCODE_PCAPWRONGPATH)); - return false; - } - else - { - std::stringstream msop_filter; - std::stringstream difop_filter; - /*ip address filter*/ - // msop_filter << "src host " << input_param_.device_ip << " && "; - // difop_filter << "src host " << input_param_.device_ip << " && "; - if (input_param_.use_vlan) - { - msop_filter << "vlan && "; - difop_filter << "vlan && "; - } - msop_filter << "udp dst port " << input_param_.msop_port; - difop_filter << "udp dst port " << input_param_.difop_port; - pcap_compile(pcap_, &pcap_msop_filter_, msop_filter.str().c_str(), 1, 0xFFFFFFFF); - pcap_compile(pcap_, &pcap_difop_filter_, difop_filter.str().c_str(), 1, 0xFFFFFFFF); - pcap_offset_ += vlan_offset_ + someip_offset_ + custom_proto_offset_; - } - } - else - { - if (!setSocket("msop")) - { - return false; - } - if ((input_param_.difop_port > 0) && !setSocket("difop")) - { - return false; - } - } - init_flag_ = true; - return true; -} - -inline bool Input::start() -{ - if (!init_flag_) - { - excb_(Error(ERRCODE_STARTBEFOREINIT)); - return false; - } - if (!input_param_.read_pcap) - { - msop_thread_.start_.store(true); - msop_thread_.thread_.reset(new std::thread([this]() { getMsopPacket(); })); - - if (input_param_.difop_port > 0) - { - difop_thread_.start_.store(true); - difop_thread_.thread_.reset(new std::thread([this]() { getDifopPacket(); })); - } - } - else - { - pcap_thread_.start_.store(true); - pcap_thread_.thread_.reset(new std::thread([this]() { getPcapPacket(); })); - } - return true; -} - -inline void Input::stop() -{ - if (!input_param_.read_pcap) - { - msop_thread_.start_.store(false); - difop_thread_.start_.store(false); - if (msop_thread_.thread_ != nullptr && msop_thread_.thread_->joinable()) - { - msop_thread_.thread_->join(); - } - if (difop_thread_.thread_ != nullptr && difop_thread_.thread_->joinable()) - { - difop_thread_.thread_->join(); - } - } - else - { - pcap_thread_.start_.store(false); - if (pcap_thread_.thread_ != nullptr && pcap_thread_.thread_->joinable()) - { - pcap_thread_.thread_->join(); - } - } -} - -inline void Input::regRecvMsopCallback(const std::function& callback) -{ - msop_cb_.emplace_back(callback); -} - -inline void Input::regRecvDifopCallback(const std::function& callback) -{ - difop_cb_.emplace_back(callback); -} - -inline bool Input::setSocket(const std::string& pkt_type) -{ - boost::asio::ip::address_v4 local_address; - if (input_param_.multi_cast_address == "0.0.0.0" && input_param_.host_address != "0.0.0.0") - { - local_address = boost::asio::ip::address_v4::from_string(input_param_.host_address); - } - else - { - local_address = boost::asio::ip::address_v4(); - } - - if (pkt_type == "msop") - { - try - { - msop_sock_ptr_.reset(new udp::socket(msop_io_service_)); - - udp::endpoint ep(local_address, input_param_.msop_port); - msop_sock_ptr_->open (ep.protocol()); - msop_sock_ptr_->bind (ep); - - if (input_param_.multi_cast_address != "0.0.0.0") - { - boost::asio::ip::address_v4 multi_cast_address = address::from_string(input_param_.multi_cast_address).to_v4(); - boost::asio::ip::address_v4 host_address = address::from_string(input_param_.host_address).to_v4(); - msop_sock_ptr_->set_option(boost::asio::ip::multicast::join_group(multi_cast_address, host_address)); - } - - msop_deadline_.reset(new deadline_timer(msop_io_service_)); - } - catch (...) - { - excb_(Error(ERRCODE_MSOPPORTBUZY)); - return false; - } - - msop_deadline_->expires_at(boost::posix_time::pos_infin); - checkMsopDeadline(); - } - else if (pkt_type == "difop") - { - try - { - difop_sock_ptr_.reset(new udp::socket(difop_io_service_)); - - udp::endpoint ep(local_address, input_param_.difop_port); - difop_sock_ptr_->open (ep.protocol()); - difop_sock_ptr_->bind (ep); - - if (input_param_.multi_cast_address != "0.0.0.0") - { - boost::asio::ip::address_v4 multi_cast_address = address::from_string(input_param_.multi_cast_address).to_v4(); - boost::asio::ip::address_v4 host_address = address::from_string(input_param_.host_address).to_v4(); - difop_sock_ptr_->set_option(boost::asio::ip::multicast::join_group(multi_cast_address, host_address)); - } - - difop_deadline_.reset(new deadline_timer(difop_io_service_)); - } - catch (...) - { - excb_(Error(ERRCODE_DIFOPPORTBUZY)); - return false; - } - - difop_deadline_->expires_at(boost::posix_time::pos_infin); - checkDifopDeadline(); - } - return true; -} - -inline void Input::getMsopPacket() -{ - char* precv_buffer = (char*)malloc(msop_pkt_length_ + someip_offset_ + custom_proto_offset_); - while (msop_thread_.start_.load()) - { - msop_deadline_->expires_from_now(boost::posix_time::seconds(1)); - boost::system::error_code ec = boost::asio::error::would_block; - std::size_t ret = 0; - - msop_sock_ptr_->async_receive(boost::asio::buffer(precv_buffer, msop_pkt_length_ + someip_offset_ + custom_proto_offset_), - boost::bind(&Input::handleReceive, _1, _2, &ec, &ret)); - do - { - msop_io_service_.run_one(); - } while (ec == boost::asio::error::would_block); - if (ec) - { - excb_(Error(ERRCODE_MSOPTIMEOUT)); - continue; - } - - // use_someip, the msop and difop port are the same. - if ((input_param_.use_someip || input_param_.use_custom_proto) && ret == (difop_pkt_length_ + someip_offset_ + custom_proto_offset_)) - { - PacketMsg msg(difop_pkt_length_); - memcpy(msg.packet.data(), precv_buffer + someip_offset_ + custom_proto_offset_, difop_pkt_length_); - for (auto& iter : difop_cb_) - { - iter(msg); - } - continue; - } - - if (ret < (msop_pkt_length_ + someip_offset_ + custom_proto_offset_)) - { - excb_(Error(ERRCODE_MSOPINCOMPLETE)); - continue; - } - - PacketMsg msg(msop_pkt_length_); - memcpy(msg.packet.data(), precv_buffer + someip_offset_ + custom_proto_offset_, msop_pkt_length_); - for (auto& iter : msop_cb_) - { - iter(msg); - } - } - free(precv_buffer); -} - -inline void Input::getDifopPacket() -{ - if (!input_param_.use_someip) - { - char* precv_buffer = (char*)malloc(difop_pkt_length_ + custom_proto_offset_); - while (difop_thread_.start_.load()) - { - difop_deadline_->expires_from_now(boost::posix_time::seconds(5)); - boost::system::error_code ec = boost::asio::error::would_block; - std::size_t ret = 0; - - difop_sock_ptr_->async_receive(boost::asio::buffer(precv_buffer, difop_pkt_length_ + custom_proto_offset_), - boost::bind(&Input::handleReceive, _1, _2, &ec, &ret)); - do - { - difop_io_service_.run_one(); - } while (ec == boost::asio::error::would_block); - if (ec) - { - excb_(Error(ERRCODE_DIFOPTIMEOUT)); - continue; - } - if (ret < (difop_pkt_length_ + custom_proto_offset_)) - { - excb_(Error(ERRCODE_DIFOPINCOMPLETE)); - continue; - } - PacketMsg msg(difop_pkt_length_); - memcpy(msg.packet.data(), precv_buffer + custom_proto_offset_, difop_pkt_length_); - for (auto& iter : difop_cb_) - { - iter(msg); - } - } - free(precv_buffer); - } -} - -inline void Input::getPcapPacket() -{ - while (pcap_thread_.start_.load()) - { - struct pcap_pkthdr* header; - const u_char* pkt_data; - if (pcap_next_ex(pcap_, &header, &pkt_data) >= 0) - { - if (pcap_offline_filter(&pcap_msop_filter_, header, pkt_data) != 0) - { - if (header->len == msop_pkt_length_ + pcap_offset_) - { - PacketMsg msg(msop_pkt_length_); - memcpy(msg.packet.data(), pkt_data + pcap_offset_, msop_pkt_length_); - for (auto& iter : msop_cb_) - { - iter(msg); - } - } - else if ((input_param_.use_someip || input_param_.use_custom_proto) && (header->len == difop_pkt_length_ + pcap_offset_)) // the someip case - { - PacketMsg msg(difop_pkt_length_); - memcpy(msg.packet.data(), pkt_data + pcap_offset_, difop_pkt_length_); - for (auto& iter : difop_cb_) - { - iter(msg); - } - } - else - { - continue; - } - } - else if (pcap_offline_filter(&pcap_difop_filter_, header, pkt_data) != 0) - { - PacketMsg msg(difop_pkt_length_); - memcpy(msg.packet.data(), pkt_data + pcap_offset_, difop_pkt_length_); - for (auto& iter : difop_cb_) - { - iter(msg); - } - } - else - { - continue; - } - } - else - { - pcap_close(pcap_); - - if (input_param_.pcap_repeat) - { - excb_(Error(ERRCODE_PCAPREPEAT)); - char errbuf[PCAP_ERRBUF_SIZE]; - pcap_ = pcap_open_offline(input_param_.pcap_path.c_str(), errbuf); - } - else - { - excb_(Error(ERRCODE_PCAPEXIT)); - break; - } - } - - if (!pcap_thread_.start_.load()) - { - break; - } - - auto time2go = last_packet_time_; - switch (lidar_type_) - { - case LidarType::RS16: - time2go += std::chrono::microseconds(static_cast(RS16_PCAP_SLEEP_DURATION / input_param_.pcap_rate)); - break; - case LidarType::RS32: - time2go += std::chrono::microseconds(static_cast(RS32_PCAP_SLEEP_DURATION / input_param_.pcap_rate)); - break; - case LidarType::RSBP: - time2go += std::chrono::microseconds(static_cast(RSBP_PCAP_SLEEP_DURATION / input_param_.pcap_rate)); - break; - case LidarType::RS128: - case LidarType::RS128_40: - time2go += - std::chrono::microseconds(static_cast(RS128_PCAP_SLEEP_DURATION / input_param_.pcap_rate)); - break; - case LidarType::RS80: - time2go += std::chrono::microseconds(static_cast(RS80_PCAP_SLEEP_DURATION / input_param_.pcap_rate)); - break; - case LidarType::RSM1: - time2go += std::chrono::microseconds(static_cast(RSM1_PCAP_SLEEP_DURATION / input_param_.pcap_rate)); - break; - case LidarType::RSHELIOS: - time2go += - std::chrono::microseconds(static_cast(RSHELIOS_PCAP_SLEEP_DURATION / input_param_.pcap_rate)); - case LidarType::RSROCK: - time2go += - std::chrono::microseconds(static_cast(RSROCK_PCAP_SLEEP_DURATION / input_param_.pcap_rate)); - break; - - default: - break; - } - std::this_thread::sleep_until(time2go); - last_packet_time_ = std::chrono::system_clock::now(); - } -} // namespace lidar - -inline void Input::checkDifopDeadline() -{ - if (difop_deadline_->expires_at() <= deadline_timer::traits_type::now()) - { - difop_sock_ptr_->cancel(); - difop_deadline_->expires_at(boost::posix_time::pos_infin); - } - difop_deadline_->async_wait(boost::bind(&Input::checkDifopDeadline, this)); -} - -inline void Input::checkMsopDeadline() -{ - if (msop_deadline_->expires_at() <= deadline_timer::traits_type::now()) - { - msop_sock_ptr_->cancel(); - msop_deadline_->expires_at(boost::posix_time::pos_infin); - } - msop_deadline_->async_wait(boost::bind(&Input::checkMsopDeadline, this)); -} - -inline void Input::handleReceive(const boost::system::error_code& ec, std::size_t length, - boost::system::error_code* out_ec, std::size_t* out_length) -{ - *out_ec = ec; - *out_length = length; -} - -} // namespace lidar -} // namespace robosense diff --git a/src/rs_driver/driver/input/input.hpp b/src/rs_driver/driver/input/input.hpp new file mode 100644 index 00000000..610c6047 --- /dev/null +++ b/src/rs_driver/driver/input/input.hpp @@ -0,0 +1,115 @@ +/********************************************************************************************************************* +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 + +#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); + + 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::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); +} + +} // namespace lidar +} // namespace robosense diff --git a/src/rs_driver/driver/input/input_factory.hpp b/src/rs_driver/driver/input/input_factory.hpp new file mode 100644 index 00000000..10b6e0ee --- /dev/null +++ b/src/rs_driver/driver/input/input_factory.hpp @@ -0,0 +1,117 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the +following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following +disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following +disclaimer in the documentation and/or other materials provided with the distribution. + +3. Neither the names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, +INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*********************************************************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include + +#ifndef DISABLE_PCAP_PARSE +#include +#include +#endif + +namespace robosense +{ +namespace lidar +{ + +class InputFactory +{ +public: + static std::shared_ptr createInput(InputType type, const RSInputParam& param, bool isJumbo, + double sec_to_delay, std::function& cb_feed_pkt); +}; + +inline std::shared_ptr InputFactory::createInput(InputType type, const RSInputParam& param, bool isJumbo, + double sec_to_delay, std::function& cb_feed_pkt) +{ + std::shared_ptr input; + + switch(type) + { + case InputType::ONLINE_LIDAR: + { + if (isJumbo) + input = std::make_shared(param); + else + input = std::make_shared(param); + } + break; + +#ifndef DISABLE_PCAP_PARSE + case InputType::PCAP_FILE: + { + if (isJumbo) + input = std::make_shared(param, sec_to_delay); + else + input = std::make_shared(param, sec_to_delay); + } + break; +#endif + + case InputType::RAW_PACKET: + { + std::shared_ptr inputRaw; + + if (isJumbo) + inputRaw = std::make_shared(param); + else + inputRaw = std::make_shared(param); + + cb_feed_pkt = std::bind(&InputRaw::feedPacket, inputRaw, + std::placeholders::_1, std::placeholders::_2); + + input = inputRaw; + } + break; + + default: + + RS_ERROR << "Wrong Input Type " << type << "." << RS_REND; + + if (type == InputType::PCAP_FILE) + { + RS_ERROR << "To use InputType::PCAP_FILE, please do not specify the make option DISABLE_PCAP_PARSE." << RS_REND; + } + + exit(-1); + } + + return input; +} + +} // namespace lidar +} // namespace robosense diff --git a/src/rs_driver/driver/input/input_pcap.hpp b/src/rs_driver/driver/input/input_pcap.hpp new file mode 100644 index 00000000..09e96f9a --- /dev/null +++ b/src/rs_driver/driver/input/input_pcap.hpp @@ -0,0 +1,202 @@ +/********************************************************************************************************************* +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), + 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, difop_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; + + msop_filter_str_ = msop_stream.str(); + difop_filter_str_ = difop_stream.str(); + } + + 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_; + bpf_program msop_filter_; + bpf_program difop_filter_; + bool difop_filter_valid_; + uint64_t msec_to_delay_; +}; + +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; + } + + 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() +{ + 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) + { + std::shared_ptr pkt = cb_get_pkt_(ETH_LEN); + memcpy(pkt->data(), pkt_data + pcap_offset_, header->len - pcap_offset_ - pcap_tail_); + pkt->setData(0, header->len - pcap_offset_ - pcap_tail_); + pushPacket(pkt); + } + else if (difop_filter_valid_ && (pcap_offline_filter(&difop_filter_, header, pkt_data) != 0)) + { + std::shared_ptr pkt = cb_get_pkt_(ETH_LEN); + memcpy(pkt->data(), pkt_data + pcap_offset_, header->len - pcap_offset_ - pcap_tail_); + pkt->setData(0, header->len - pcap_offset_ - pcap_tail_); + pushPacket(pkt); + } + else + { + continue; + } + + std::this_thread::sleep_for(std::chrono::microseconds(msec_to_delay_)); + } +} + +} // namespace lidar +} // namespace robosense diff --git a/src/rs_driver/driver/input/input_pcap_jumbo.hpp b/src/rs_driver/driver/input/input_pcap_jumbo.hpp new file mode 100644 index 00000000..b1d0b673 --- /dev/null +++ b/src/rs_driver/driver/input/input_pcap_jumbo.hpp @@ -0,0 +1,198 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the +following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following +disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following +disclaimer in the documentation and/or other materials provided with the distribution. + +3. Neither the names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, +INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*********************************************************************************************************************/ + +#pragma once +#include +#include + +#include + +#ifdef _WIN32 +#define WIN32 +#else //__linux__ +#endif + +#include + +namespace robosense +{ +namespace lidar +{ +class InputPcapJumbo : public Input +{ +public: + InputPcapJumbo(const RSInputParam& input_param, double sec_to_delay) + : Input(input_param), pcap_(NULL), pcap_offset_(ETH_HDR_LEN), pcap_tail_(0), difop_filter_valid_(false), + msec_to_delay_((uint64_t)(sec_to_delay / input_param.pcap_rate * 1000000)) + { + if (input_param.use_vlan) + { + pcap_offset_ += VLAN_HDR_LEN; + } + + pcap_offset_ += input_param.user_layer_bytes; + pcap_tail_ += input_param.tail_layer_bytes; + + std::stringstream msop_stream; + if (input_param_.use_vlan) + { + msop_stream << "vlan && "; + } + + msop_stream << "udp"; + + msop_filter_str_ = msop_stream.str(); + } + + virtual bool init(); + virtual bool start(); + virtual ~InputPcapJumbo(); + +private: + void recvPacket(); + +private: + pcap_t* pcap_; + size_t pcap_offset_; + size_t pcap_tail_; + std::string msop_filter_str_; + bpf_program msop_filter_; + bpf_program difop_filter_; + bool difop_filter_valid_; + uint64_t msec_to_delay_; + + Jumbo jumbo_; +}; + +inline bool InputPcapJumbo::init() +{ + if (init_flag_) + return true; + + char errbuf[PCAP_ERRBUF_SIZE]; + pcap_ = pcap_open_offline(input_param_.pcap_path.c_str(), errbuf); + if (pcap_ == NULL) + { + cb_excep_(Error(ERRCODE_PCAPWRONGPATH)); + return false; + } + + pcap_compile(pcap_, &msop_filter_, msop_filter_str_.c_str(), 1, 0xFFFFFFFF); + + init_flag_ = true; + return true; +} + +inline bool InputPcapJumbo::start() +{ + if (start_flag_) + return true; + + if (!init_flag_) + { + cb_excep_(Error(ERRCODE_STARTBEFOREINIT)); + return false; + } + + to_exit_recv_ = false; + recv_thread_ = std::thread(std::bind(&InputPcapJumbo::recvPacket, this)); + + start_flag_ = true; + return true; +} + +inline InputPcapJumbo::~InputPcapJumbo() +{ + stop(); + + if (pcap_ != NULL) + { + pcap_close(pcap_); + pcap_ = NULL; + } +} + +inline void InputPcapJumbo::recvPacket() +{ + while (!to_exit_recv_) + { + struct pcap_pkthdr* header; + const uint8_t* pkt_data; + int ret = pcap_next_ex(pcap_, &header, &pkt_data); + if (ret < 0) // reach file end. + { + pcap_close(pcap_); + pcap_ = NULL; + + if (input_param_.pcap_repeat) + { + cb_excep_(Error(ERRCODE_PCAPREPEAT)); + + char errbuf[PCAP_ERRBUF_SIZE]; + pcap_ = pcap_open_offline(input_param_.pcap_path.c_str(), errbuf); + continue; + } + else + { + cb_excep_(Error(ERRCODE_PCAPEXIT)); + break; + } + } + + if (pcap_offline_filter(&msop_filter_, header, pkt_data) != 0) + { + uint16_t udp_port = 0; + const uint8_t* udp_data = NULL; + size_t udp_data_len = 0; + bool new_pkt = jumbo_.new_fragment(pkt_data, header->len, &udp_port, &udp_data, &udp_data_len); + if (new_pkt) + { + if ((udp_port == input_param_.msop_port) || (udp_port == input_param_.difop_port)) + { + std::shared_ptr pkt = cb_get_pkt_(IP_LEN); + memcpy(pkt->data(), udp_data, udp_data_len); + pkt->setData(0, udp_data_len); + pushPacket(pkt); + } + } + } + else + { + continue; + } + + std::this_thread::sleep_for(std::chrono::microseconds(msec_to_delay_)); + } +} + +} // namespace lidar +} // namespace robosense diff --git a/src/rs_driver/msg/point_cloud_msg.h b/src/rs_driver/driver/input/input_raw.hpp similarity index 67% rename from src/rs_driver/msg/point_cloud_msg.h rename to src/rs_driver/driver/input/input_raw.hpp index 884b2a82..201e02c4 100644 --- a/src/rs_driver/msg/point_cloud_msg.h +++ b/src/rs_driver/driver/input/input_raw.hpp @@ -31,34 +31,47 @@ THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *********************************************************************************************************************/ #pragma once -#include + +#include + namespace robosense { namespace lidar { -template -#ifdef _MSC_VER -struct __declspec(align(16)) PointCloudMsg -#elif __GNUC__ -struct __attribute__((aligned(16))) PointCloudMsg -#endif +class InputRaw : public Input { - typedef std::vector PointCloud; - typedef std::shared_ptr PointCloudPtr; - typedef std::shared_ptr PointCloudConstPtr; - double timestamp = 0.0; - std::string frame_id = ""; ///< Point cloud frame id - uint32_t seq = 0; ///< Sequence number of message - uint32_t height = 0; ///< Height of point cloud - uint32_t width = 0; ///< Width of point cloud - bool is_dense = false; ///< If is_dense=true, the point cloud does not contain NAN points - PointCloudPtr point_cloud_ptr; ///< Point cloud pointer - PointCloudMsg() = default; - explicit PointCloudMsg(const PointCloudPtr& ptr) : point_cloud_ptr(ptr) - { - } - typedef std::shared_ptr Ptr; - typedef std::shared_ptr ConstPtr; +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/rs_driver/utility/time.h b/src/rs_driver/driver/input/input_raw_jumbo.hpp similarity index 87% rename from src/rs_driver/utility/time.h rename to src/rs_driver/driver/input/input_raw_jumbo.hpp index ffbb8778..1c9ca0ff 100644 --- a/src/rs_driver/utility/time.h +++ b/src/rs_driver/driver/input/input_raw_jumbo.hpp @@ -31,16 +31,23 @@ THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *********************************************************************************************************************/ #pragma once -#include + +#include + namespace robosense { namespace lidar { -inline double getTime(void) +class InputRawJumbo : public InputRaw { - const auto t = std::chrono::system_clock::now(); - const auto t_sec = std::chrono::duration_cast>(t.time_since_epoch()); - return (double)t_sec.count(); -} +public: + + InputRawJumbo(const RSInputParam& input_param) + : InputRaw(input_param) + { + pkt_buf_len_ = IP_LEN; + } +}; + } // namespace lidar -} // namespace robosense \ No newline at end of file +} // namespace robosense diff --git a/src/rs_driver/driver/input/input_sock.hpp b/src/rs_driver/driver/input/input_sock.hpp new file mode 100644 index 00000000..a01dd60d --- /dev/null +++ b/src/rs_driver/driver/input/input_sock.hpp @@ -0,0 +1,17 @@ + +#pragma once + +#ifdef _WIN32 + +#include + +#else //__linux__ + +#ifdef ENABLE_EPOLL_RECEIVE +#include +#else +#include +#endif + +#endif + diff --git a/src/rs_driver/driver/input/input_sock_jumbo.hpp b/src/rs_driver/driver/input/input_sock_jumbo.hpp new file mode 100644 index 00000000..cc197fad --- /dev/null +++ b/src/rs_driver/driver/input/input_sock_jumbo.hpp @@ -0,0 +1,23 @@ + +#pragma once + +#include + +namespace robosense +{ +namespace lidar +{ + +class InputSockJumbo : public InputSock +{ +public: + + InputSockJumbo(const RSInputParam& input_param) + : InputSock(input_param) + { + pkt_buf_len_ = IP_LEN; + } +}; + +} // namespace lidar +} // namespace robosense diff --git a/src/rs_driver/driver/input/jumbo.hpp b/src/rs_driver/driver/input/jumbo.hpp new file mode 100644 index 00000000..a25f76ae --- /dev/null +++ b/src/rs_driver/driver/input/jumbo.hpp @@ -0,0 +1,190 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the +following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following +disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following +disclaimer in the documentation and/or other materials provided with the distribution. + +3. Neither the names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, +INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*********************************************************************************************************************/ + +#pragma once + +#include + +namespace robosense +{ +namespace lidar +{ + +#pragma pack(push, 1) + +struct iphdr +{ + uint8_t version; + uint8_t tos; + uint16_t tot_len; + + uint16_t id; + uint16_t frag_off; + + uint8_t ttl; + uint8_t protocol; + uint16_t check; + + uint32_t saddr; + uint32_t daddr; +}; + +struct udphdr +{ + uint16_t source; + uint16_t dest; + uint16_t len; + uint16_t check; +}; + +#pragma pack(pop) + +class Jumbo +{ +public: + + Jumbo() + : ip_id_(0), buf_off_(0) + { + } + + bool new_fragment(const uint8_t* pkt_data, size_t pkt_data_size, + uint16_t* udp_port, const uint8_t**ip_data, size_t* ip_data_len); + +private: + + uint16_t dst_port(const uint8_t* buf); + + uint16_t ip_id_; + uint8_t buf_[IP_LEN]; + uint16_t buf_off_; +}; + +inline bool Jumbo::new_fragment(const uint8_t* pkt_data, size_t pkt_data_size, + uint16_t* udp_port, const uint8_t** udp_data, size_t* udp_data_len) +{ + // Is it an ip packet ? + const uint16_t* eth_type = (const uint16_t*)(pkt_data + 12); + if (ntohs(*eth_type) != 0x0800) + return false; + + // is it a udp packet? + const struct iphdr* ip_hdr = (const struct iphdr*)(pkt_data + 14); + if (ip_hdr->protocol != 0x11) + return false; + + // ip data + uint16_t ip_hdr_size = (ip_hdr->version & 0xf) * 4; + uint16_t ip_len = ntohs(ip_hdr->tot_len); + + const uint8_t* ip_data = pkt_data + 14 + ip_hdr_size; + uint16_t ip_data_len = ip_len - ip_hdr_size; + + // ip fragment + uint16_t ip_id = ntohs (ip_hdr->id); + uint16_t f_off = ntohs(ip_hdr->frag_off); + uint16_t frag_flags = (f_off >> 13); + uint16_t frag_off = (f_off & 0x1fff) * 8; // 8 octet boudary + +#define MORE_FRAGS(flags) ((flags & 0x01) != 0) + + if (ip_id == ip_id_) + { + if (frag_off == buf_off_) + { + memcpy (buf_ + buf_off_, ip_data, ip_data_len); + buf_off_ += ip_data_len; + + if (!MORE_FRAGS(frag_flags)) + { +#if 0 + printf ("--- end new packet. ip_id:0x%x, len:%d\n", ip_id_, buf_off_); + + for (uint16_t off = UDP_HDR_LEN, i = 0; off < buf_len(); off += 984, i++) + { + char title[32]; + sprintf (title, "jumbo %d ", i); + + hexdump (buf() + off, 32, title); + } +#endif + + ip_id_ = 0; + + *udp_port = dst_port (buf_); + *udp_data = buf_ + UDP_HDR_LEN; + *udp_data_len = buf_off_ - UDP_HDR_LEN; + return true; + } + } + } + else + { + if (frag_off == 0) + { + if (MORE_FRAGS(frag_flags)) + { + ip_id_ = ip_id; + buf_off_ = 0; + + memcpy (buf_ + buf_off_, ip_data, ip_data_len); + buf_off_ += ip_data_len; + +#if 0 + printf ("+++ start packet 0x%x\n", ip_id_); +#endif + } + else + { +#if 0 + printf ("+++ non-fragment packet 0x%x\n", ip_id); +#endif + + *udp_port = dst_port (ip_data); + *udp_data = ip_data + UDP_HDR_LEN; + *udp_data_len = ip_data_len - UDP_HDR_LEN; + return true; + } + } + } + + return false; +} + +inline uint16_t Jumbo::dst_port(const uint8_t* buf) +{ + const struct udphdr* udp_hdr = (const struct udphdr*)buf; + return ntohs(udp_hdr->dest); +} + +} // namespace lidar +} // namespace robosense diff --git a/src/rs_driver/driver/input/unix/input_sock_epoll.hpp b/src/rs_driver/driver/input/unix/input_sock_epoll.hpp new file mode 100644 index 00000000..d6f2c9c9 --- /dev/null +++ b/src/rs_driver/driver/input/unix/input_sock_epoll.hpp @@ -0,0 +1,285 @@ +/********************************************************************************************************************* +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_[2]; + size_t sock_offset_; + size_t sock_tail_; +}; + +inline bool InputSock::init() +{ + if (init_flag_) + { + return true; + } + + int msop_fd = -1, difop_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; + + init_flag_ = true; + return true; + +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]); + + 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: "); + 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: "); + 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/rs_driver/driver/input/unix/input_sock_select.hpp b/src/rs_driver/driver/input/unix/input_sock_select.hpp new file mode 100644 index 00000000..248eace9 --- /dev/null +++ b/src/rs_driver/driver/input/unix/input_sock_select.hpp @@ -0,0 +1,290 @@ +/********************************************************************************************************************* +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_[2]; + size_t sock_offset_; + size_t sock_tail_; +}; + +inline bool InputSock::init() +{ + if (init_flag_) + { + return true; + } + + int msop_fd = -1, difop_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; + + init_flag_ = true; + return true; + +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]); +} + +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: "); + 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: "); + goto failGroup; + } + } + +#ifdef ENABLE_DOUBLE_RCVBUF + { + uint32_t opt_val; + socklen_t opt_len = sizeof(uint32_t); + getsockopt(fd, SOL_SOCKET, SO_RCVBUF, (char*)&opt_val, &opt_len); + opt_val *= 4; + setsockopt(fd, SOL_SOCKET, SO_RCVBUF, (char*)&opt_val, opt_len); + } +#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 = ((fds_[0] > fds_[1]) ? fds_[0] : fds_[1]); + bool msop_recv = true; + + while (!to_exit_recv_) + { + fd_set rfds; + FD_ZERO(&rfds); + FD_SET(fds_[0], &rfds); + if (fds_[1] >= 0) + FD_SET(fds_[1], &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 < 2; 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); + } + } + } + + if (FD_ISSET(fds_[0], &rfds)) // receive msop + { + msop_recv = true; + } + else // receive difop + { + if (!msop_recv) + { + cb_excep_(Error(ERRCODE_MSOPTIMEOUT)); + } + + msop_recv = false; + } + } +} + +} // namespace lidar +} // namespace robosense diff --git a/src/rs_driver/driver/input/win/input_sock_select.hpp b/src/rs_driver/driver/input/win/input_sock_select.hpp new file mode 100644 index 00000000..3de8e185 --- /dev/null +++ b/src/rs_driver/driver/input/win/input_sock_select.hpp @@ -0,0 +1,295 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the +following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following +disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following +disclaimer in the documentation and/or other materials provided with the distribution. + +3. Neither the names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, +INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*********************************************************************************************************************/ + +#pragma once + +#include + +#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_[2]; + size_t sock_offset_; + size_t sock_tail_; +}; + +inline bool InputSock::init() +{ + if (init_flag_) + { + return true; + } + + int msop_fd = -1, difop_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; + } + + fds_[0] = msop_fd; + fds_[1] = difop_fd; + + init_flag_ = true; + return true; + +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]); +} + +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: "); + 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: "); + goto failGroup; + } + } + +#ifdef ENABLE_DOUBLE_RCVBUF + { + uint32_t opt_val; + socklen_t opt_len = sizeof(uint32_t); + getsockopt(fd, SOL_SOCKET, SO_RCVBUF, (char*)&opt_val, &opt_len); + opt_val *= 4; + setsockopt(fd, SOL_SOCKET, SO_RCVBUF, (char*)&opt_val, opt_len); + } +#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 = ((fds_[0] > fds_[1]) ? fds_[0] : fds_[1]); + bool msop_recv = true; + + while (!to_exit_recv_) + { + fd_set rfds; + FD_ZERO(&rfds); + FD_SET(fds_[0], &rfds); + if (fds_[1] >= 0) + FD_SET(fds_[1], &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 < 2; 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); + } + } + } + + if (FD_ISSET(fds_[0], &rfds)) // receive msop + { + msop_recv = true; + } + else // receive difop + { + if (!msop_recv) + { + cb_excep_(Error(ERRCODE_MSOPTIMEOUT)); + } + + msop_recv = false; + } + } +} + +} // namespace lidar +} // namespace robosense diff --git a/src/rs_driver/driver/lidar_driver_impl.hpp b/src/rs_driver/driver/lidar_driver_impl.hpp index 85d8153d..e533712e 100644 --- a/src/rs_driver/driver/lidar_driver_impl.hpp +++ b/src/rs_driver/driver/lidar_driver_impl.hpp @@ -31,498 +31,362 @@ 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 -constexpr size_t MAX_PACKETS_BUFFER_SIZE = 100000; + +#include + namespace robosense { namespace lidar { -template + +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 regExceptionCallback(const std::function& cb_excep); + bool init(const RSDriverParam& param); - void initDecoderOnly(const RSDriverParam& param); bool start(); void stop(); - void regRecvCallback(const std::function&)>& callback); - void regRecvCallback(const std::function& callback); - void regRecvCallback(const std::function& callback); - void regRecvCallback(const std::function& callback); - void regExceptionCallback(const std::function& callback); - bool getLidarTemperature(double& input_temperature); - bool decodeMsopScan(const ScanMsg& scan_msg, PointCloudMsg& point_cloud_msg); - void decodeDifopPkt(const PacketMsg& msg); -private: - void runCallBack(const ScanMsg& msg); - void runCallBack(const PacketMsg& msg); - void runCallBack(const PointCloudMsg& msg); - void reportError(const Error& error); - void msopCallback(const PacketMsg& msg); - void difopCallback(const PacketMsg& msg); - void processMsop(); - void processDifop(); - void localCameraTriggerCallback(const CameraTrigger& msg); - void initPointCloudTransFunc(); - void setScanMsgHeader(ScanMsg& msg); - void setPointCloudMsgHeader(PointCloudMsg& msg); + void decodePacket(const Packet& pkt); + bool getTemperature(float& temp); private: - Queue msop_pkt_queue_; - Queue difop_pkt_queue_; - std::vector> msop_pkt_cb_vec_; - std::vector> difop_pkt_cb_vec_; - std::vector&)>> point_cloud_cb_vec_; - std::vector> camera_trigger_cb_vec_; - std::vector> excb_; - std::shared_ptr lidar_thread_ptr_; - std::shared_ptr> lidar_decoder_ptr_; + + 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(); + + std::shared_ptr getPointCloud(); + void splitFrame(uint16_t height, double ts); + void setPointCloudHeader(std::shared_ptr msg, uint16_t height, double chan_ts); + + RSDriverParam driver_param_; + std::function(void)> cb_get_cloud_; + std::function)> cb_put_cloud_; + std::function cb_put_pkt_; + std::function cb_excep_; + std::function cb_feed_pkt_; + std::shared_ptr input_ptr_; - std::shared_ptr scan_ptr_; - std::shared_ptr thread_pool_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_; - bool difop_flag_; - uint32_t point_cloud_seq_; - uint32_t scan_seq_; - uint32_t ndifop_count_; - RSDriverParam driver_param_; - std::function::PointCloudPtr(const typename PointCloudMsg::PointCloudPtr, - const size_t& height)> - point_cloud_transform_func_; - typename PointCloudMsg::PointCloudPtr point_cloud_ptr_; }; -template -inline LidarDriverImpl::LidarDriverImpl() - : init_flag_(false), start_flag_(false), difop_flag_(false), point_cloud_seq_(0), scan_seq_(0), ndifop_count_(0) +template +inline LidarDriverImpl::LidarDriverImpl() + : pkt_seq_(0), point_cloud_seq_(0), init_flag_(false), start_flag_(false) { - thread_pool_ptr_ = std::make_shared(); - point_cloud_ptr_ = std::make_shared::PointCloud>(); - scan_ptr_ = std::make_shared(); } -template -inline LidarDriverImpl::~LidarDriverImpl() +template +inline LidarDriverImpl::~LidarDriverImpl() { stop(); - thread_pool_ptr_.reset(); - input_ptr_.reset(); } -template -inline bool LidarDriverImpl::init(const RSDriverParam& param) +template +std::shared_ptr LidarDriverImpl::getPointCloud() { - if (init_flag_) + while (1) { - return false; - } - driver_param_ = param; - input_ptr_ = std::make_shared(driver_param_.lidar_type, driver_param_.input_param, - std::bind(&LidarDriverImpl::reportError, this, std::placeholders::_1)); - input_ptr_->regRecvMsopCallback(std::bind(&LidarDriverImpl::msopCallback, this, std::placeholders::_1)); - input_ptr_->regRecvDifopCallback(std::bind(&LidarDriverImpl::difopCallback, this, std::placeholders::_1)); - if (!input_ptr_->init()) - { - return false; + std::shared_ptr cloud = cb_get_cloud_(); + if (cloud) + { + cloud->points.resize(0); + return cloud; + } + + LIMIT_CALL(runExceptionCallback(Error(ERRCODE_POINTCLOUDNULL)), 1); } - lidar_decoder_ptr_ = DecoderFactory::createDecoder(driver_param_); - lidar_decoder_ptr_->regRecvCallback( - std::bind(&LidarDriverImpl::localCameraTriggerCallback, this, std::placeholders::_1)); - init_flag_ = true; - initPointCloudTransFunc(); - return true; } -template -inline void LidarDriverImpl::initDecoderOnly(const RSDriverParam& param) +template +void LidarDriverImpl::regPointCloudCallback( + const std::function(void)>& cb_get_cloud, + const std::function)>& cb_put_cloud) { - if (init_flag_) - { - return; - } - driver_param_ = param; - lidar_decoder_ptr_ = DecoderFactory::createDecoder(driver_param_); - lidar_decoder_ptr_->regRecvCallback( - std::bind(&LidarDriverImpl::localCameraTriggerCallback, this, std::placeholders::_1)); - init_flag_ = true; - initPointCloudTransFunc(); + cb_get_cloud_ = cb_get_cloud; + cb_put_cloud_ = cb_put_cloud; } -template -inline void LidarDriverImpl::initPointCloudTransFunc() +template +inline void LidarDriverImpl::regPacketCallback( + const std::function& cb_put_pkt) { - if (driver_param_.saved_by_rows) - { - point_cloud_transform_func_ = [](const typename PointCloudMsg::PointCloudPtr input_ptr, - const size_t& height) -> typename PointCloudMsg::PointCloudPtr - { - typename PointCloudMsg::PointCloudPtr row_major_ptr = - std::make_shared::PointCloud>(); - row_major_ptr->resize(input_ptr->size()); - size_t width = input_ptr->size() / height; - for (int i = 0; i < static_cast(height); i++) - { - for (int j = 0; j < static_cast(width); j++) - { - row_major_ptr->at(i * width + j) = input_ptr->at(j * height + i); - } - } - return row_major_ptr; - }; - } - else - { - point_cloud_transform_func_ = [](const typename PointCloudMsg::PointCloudPtr input_ptr, - const size_t& height) -> typename PointCloudMsg::PointCloudPtr - { - return input_ptr; - }; - } + cb_put_pkt_ = cb_put_pkt; } -template -inline bool LidarDriverImpl::start() +template +inline void LidarDriverImpl::regExceptionCallback( + const std::function& cb_excep) { - if (start_flag_ || input_ptr_ == nullptr) - { - return false; - } - start_flag_ = true; - return input_ptr_->start(); + cb_excep_ = cb_excep; } -template -inline void LidarDriverImpl::stop() +template +inline bool LidarDriverImpl::init(const RSDriverParam& param) { - if (input_ptr_ != nullptr) - { - input_ptr_->stop(); - } - start_flag_ = false; - if (!msop_pkt_cb_vec_.empty() || !difop_pkt_cb_vec_.empty()) + if (init_flag_) { - std::this_thread::sleep_for(std::chrono::microseconds(10)); + return true; } -} -template -inline void -LidarDriverImpl::regRecvCallback(const std::function&)>& callback) -{ - point_cloud_cb_vec_.emplace_back(callback); -} + // + // 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)); + + 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)); -template -inline void LidarDriverImpl::regRecvCallback(const std::function& callback) -{ - msop_pkt_cb_vec_.emplace_back(callback); -} - -template -inline void LidarDriverImpl::regRecvCallback(const std::function& callback) -{ - difop_pkt_cb_vec_.emplace_back(callback); -} + if (!input_ptr_->init()) + { + goto failInputInit; + } -template -inline void LidarDriverImpl::regRecvCallback(const std::function& callback) -{ - camera_trigger_cb_vec_.emplace_back(callback); -} + driver_param_ = param; + init_flag_ = true; + return true; -template -inline void LidarDriverImpl::regExceptionCallback(const std::function& callback) -{ - excb_.emplace_back(callback); +failInputInit: + input_ptr_.reset(); + decoder_ptr_.reset(); + return false; } -template -inline bool LidarDriverImpl::getLidarTemperature(double& input_temperature) +template +inline bool LidarDriverImpl::start() { - if (lidar_decoder_ptr_ != nullptr) + if (start_flag_) { - input_temperature = lidar_decoder_ptr_->getLidarTemperature(); return true; } - return false; -} -template -inline bool LidarDriverImpl::decodeMsopScan(const ScanMsg& scan_msg, PointCloudMsg& point_cloud_msg) -{ - typename PointCloudMsg::PointCloudPtr output_point_cloud_ptr = - std::make_shared::PointCloud>(); - if (!difop_flag_ && driver_param_.wait_for_difop) + if (!init_flag_) { - ndifop_count_++; - if (ndifop_count_ > 20) - { - reportError(Error(ERRCODE_NODIFOPRECV)); - ndifop_count_ = 0; - } - point_cloud_msg.point_cloud_ptr = output_point_cloud_ptr; - std::this_thread::sleep_for(std::chrono::milliseconds(10)); return false; } - std::vector> pointcloud_one_frame; - int height = 1; - pointcloud_one_frame.resize(scan_msg.packets.size()); - for (int i = 0; i < static_cast(scan_msg.packets.size()); i++) - { - std::vector pointcloud_one_packet; - RSDecoderResult ret = - lidar_decoder_ptr_->processMsopPkt(scan_msg.packets[i].packet.data(), pointcloud_one_packet, height); - switch (ret) - { - case RSDecoderResult::DECODE_OK: - case RSDecoderResult::FRAME_SPLIT: - pointcloud_one_frame[i] = std::move(pointcloud_one_packet); - break; - case RSDecoderResult::WRONG_PKT_HEADER: - reportError(Error(ERRCODE_WRONGPKTHEADER)); - break; - case RSDecoderResult::PKT_NULL: - reportError(Error(ERRCODE_PKTNULL)); - break; - default: - break; - } - } - for (auto iter : pointcloud_one_frame) - { - output_point_cloud_ptr->insert(output_point_cloud_ptr->end(), iter.begin(), iter.end()); - } - point_cloud_msg.point_cloud_ptr = point_cloud_transform_func_(output_point_cloud_ptr, height); - point_cloud_msg.height = height; - point_cloud_msg.width = point_cloud_msg.point_cloud_ptr->size() / point_cloud_msg.height; -#if 0 - size_t iter_move = point_cloud_msg.point_cloud_ptr->size() - point_cloud_msg.height * point_cloud_msg.width; - for (size_t i = 0; i < iter_move; i++) - { - point_cloud_msg.point_cloud_ptr->erase(point_cloud_msg.point_cloud_ptr->end()); - } -#endif - setPointCloudMsgHeader(point_cloud_msg); - point_cloud_msg.timestamp = scan_msg.timestamp; - if (point_cloud_msg.point_cloud_ptr->size() == 0) + 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_) { - reportError(Error(ERRCODE_ZEROPOINTS)); - return false; + return; } - return true; + + input_ptr_->stop(); + + to_exit_handle_ = true; + handle_thread_.join(); + + start_flag_ = false; } -template -inline void LidarDriverImpl::decodeDifopPkt(const PacketMsg& msg) +template +inline void LidarDriverImpl::decodePacket(const Packet& pkt) { - lidar_decoder_ptr_->processDifopPkt(msg.packet.data()); - difop_flag_ = true; + cb_feed_pkt_(pkt.buf_.data(), pkt.buf_.size()); } -template -inline void LidarDriverImpl::runCallBack(const ScanMsg& msg) +template +inline bool LidarDriverImpl::getTemperature(float& temp) { - if (msg.seq != 0) + if (decoder_ptr_ == nullptr) { - for (auto& it : msop_pkt_cb_vec_) - { - it(msg); - } + return false; } + + temp = decoder_ptr_->getTemperature(); + return true; } -template -inline void LidarDriverImpl::runCallBack(const PacketMsg& msg) +template +inline void LidarDriverImpl::runPacketCallBack(uint8_t* data, size_t data_size, + double timestamp, uint8_t is_difop, uint8_t is_frame_begin) { - for (auto& it : difop_pkt_cb_vec_) + if (cb_put_pkt_) { - it(msg); + Packet pkt; + pkt.timestamp = timestamp; + pkt.is_difop = is_difop; + pkt.is_frame_begin = is_frame_begin; + pkt.seq = pkt_seq_++; + + pkt.buf_.resize(data_size); + memcpy (pkt.buf_.data(), data, data_size); + cb_put_pkt_(pkt); } } -template -inline void LidarDriverImpl::runCallBack(const PointCloudMsg& msg) +template +inline void LidarDriverImpl::runExceptionCallback(const Error& error) { - if (msg.seq != 0) + if (cb_excep_) { - for (auto& it : point_cloud_cb_vec_) - { - it(msg); - } + cb_excep_(error); } } -template -inline void LidarDriverImpl::reportError(const Error& error) +template +inline std::shared_ptr LidarDriverImpl::packetGet(size_t size) { - for (auto& it : excb_) + std::shared_ptr pkt = free_pkt_queue_.pop(); + if (pkt.get() != NULL) { - it(error); + return pkt; } + + return std::make_shared(size); } -template -inline void LidarDriverImpl::msopCallback(const PacketMsg& msg) +template +inline void LidarDriverImpl::packetPut(std::shared_ptr pkt, bool stuffed) { - if (msop_pkt_queue_.size() > MAX_PACKETS_BUFFER_SIZE) - { - reportError(Error(ERRCODE_PKTBUFOVERFLOW)); - msop_pkt_queue_.clear(); - } - msop_pkt_queue_.push(msg); - if (msop_pkt_queue_.is_task_finished_.load()) + constexpr static int PACKET_POOL_MAX = 1024; + + if (!stuffed) { - msop_pkt_queue_.is_task_finished_.store(false); - thread_pool_ptr_->commit([this]() { processMsop(); }); + free_pkt_queue_.push(pkt); + return; } -} -template -inline void LidarDriverImpl::difopCallback(const PacketMsg& msg) -{ - difop_pkt_queue_.push(msg); - if (difop_pkt_queue_.is_task_finished_.load()) + size_t sz = pkt_queue_.push(pkt); + if (sz > PACKET_POOL_MAX) { - difop_pkt_queue_.is_task_finished_.store(false); - thread_pool_ptr_->commit([this]() { processDifop(); }); + LIMIT_CALL(runExceptionCallback(Error(ERRCODE_PKTBUFOVERFLOW)), 1); + pkt_queue_.clear(); } } -template -inline void LidarDriverImpl::processMsop() +template +inline void LidarDriverImpl::processPacket() { - if (!difop_flag_ && driver_param_.wait_for_difop) + while (!to_exit_handle_) { - ndifop_count_++; - if (ndifop_count_ > 120) + std::shared_ptr pkt = pkt_queue_.popWait(500000); + if (pkt.get() == NULL) { - reportError(Error(ERRCODE_NODIFOPRECV)); - ndifop_count_ = 0; + continue; } - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - msop_pkt_queue_.clear(); - msop_pkt_queue_.is_task_finished_.store(true); - return; - } - while (msop_pkt_queue_.size() > 0) - { - PacketMsg pkt = msop_pkt_queue_.popFront(); - int height = 1; - int ret = lidar_decoder_ptr_->processMsopPkt(pkt.packet.data(), *point_cloud_ptr_, height); - scan_ptr_->packets.emplace_back(pkt); - if ((ret == DECODE_OK || ret == FRAME_SPLIT)) - { - if (ret == FRAME_SPLIT) - { - PointCloudMsg msg(point_cloud_transform_func_(point_cloud_ptr_, height)); - msg.height = height; - msg.width = point_cloud_ptr_->size() / msg.height; -#if 0 - size_t iter_move = point_cloud_ptr_->size() - msg.height * msg.width; - for (size_t i = 0; i < iter_move; i++) - { - point_cloud_ptr_->erase(point_cloud_ptr_->end()); - } -#endif - setPointCloudMsgHeader(msg); - if (driver_param_.decoder_param.use_lidar_clock == true) - { - msg.timestamp = lidar_decoder_ptr_->getLidarTime(pkt.packet.data()); - } - else - { - msg.timestamp = getTime(); - } - if (msg.point_cloud_ptr->size() == 0) - { - reportError(Error(ERRCODE_ZEROPOINTS)); - } - else - { - runCallBack(msg); - } - setScanMsgHeader(*scan_ptr_); - runCallBack(*scan_ptr_); - point_cloud_ptr_.reset(new typename PointCloudMsg::PointCloud); - scan_ptr_.reset(new ScanMsg); - } - } - else if (ret == DISCARD_PKT) + + uint8_t* id = pkt->data(); + if (*id == 0x55) { - scan_ptr_->packets.clear(); - point_cloud_ptr_.reset(new typename PointCloudMsg::PointCloud); + 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 + else if (*id == 0xA5) { - reportError(Error(ERRCODE_WRONGPKTHEADER)); - // msop_pkt_queue_.clear(); - // std::this_thread::sleep_for(std::chrono::milliseconds(100)); + decoder_ptr_->processDifopPkt(pkt->data(), pkt->dataSize()); + runPacketCallBack(pkt->data(), pkt->dataSize(), 0, true, false); // difop packet } - constexpr static int CLOUD_POINT_MAX = 1000000; - if (point_cloud_ptr_->size() > CLOUD_POINT_MAX) - { - scan_ptr_->packets.clear(); - point_cloud_ptr_.reset(new typename PointCloudMsg::PointCloud); - - reportError(Error(ERRCODE_CLOUDOVERFLOW)); - } + free_pkt_queue_.push(pkt); } - msop_pkt_queue_.is_task_finished_.store(true); } -template -inline void LidarDriverImpl::localCameraTriggerCallback(const CameraTrigger& msg) +template +void LidarDriverImpl::splitFrame(uint16_t height, double ts) { - for (auto& it : camera_trigger_cb_vec_) + std::shared_ptr cloud = decoder_ptr_->point_cloud_; + if (cloud->points.size() > 0) { - it(msg); + setPointCloudHeader(cloud, height, ts); + cb_put_cloud_(cloud); + decoder_ptr_->point_cloud_ = getPointCloud(); } -} - -template -inline void LidarDriverImpl::processDifop() -{ - while (difop_pkt_queue_.size() > 0) + else { - PacketMsg pkt = difop_pkt_queue_.popFront(); - decodeDifopPkt(pkt); - runCallBack(pkt); + runExceptionCallback(Error(ERRCODE_ZEROPOINTS)); } - difop_pkt_queue_.is_task_finished_.store(true); } -template -inline void LidarDriverImpl::setScanMsgHeader(ScanMsg& msg) +template +void LidarDriverImpl::setPointCloudHeader(std::shared_ptr msg, + uint16_t height, double ts) { - msg.timestamp = getTime(); - if (driver_param_.decoder_param.use_lidar_clock == true) + msg->seq = point_cloud_seq_++; + msg->timestamp = ts; + msg->is_dense = driver_param_.decoder_param.dense_points; + if (msg->is_dense) { - msg.timestamp = lidar_decoder_ptr_->getLidarTime(msg.packets.back().packet.data()); + msg->height = 1; + msg->width = (uint32_t)msg->points.size(); + } + else + { + msg->height = height; + msg->width = (uint32_t)msg->points.size() / msg->height; } - msg.seq = scan_seq_++; - msg.frame_id = driver_param_.frame_id; -} - -template -inline void LidarDriverImpl::setPointCloudMsgHeader(PointCloudMsg& msg) -{ - msg.seq = point_cloud_seq_++; - msg.frame_id = driver_param_.frame_id; - msg.is_dense = false; } } // namespace lidar diff --git a/src/rs_driver/macro/version.hpp b/src/rs_driver/macro/version.hpp new file mode 100644 index 00000000..051b8468 --- /dev/null +++ b/src/rs_driver/macro/version.hpp @@ -0,0 +1,3 @@ +#define RSLIDAR_VERSION_MAJOR 1 +#define RSLIDAR_VERSION_MINOR 5 +#define RSLIDAR_VERSION_PATCH 7 diff --git a/src/rs_driver/macro/version.h.in b/src/rs_driver/macro/version.hpp.in similarity index 100% rename from src/rs_driver/macro/version.h.in rename to src/rs_driver/macro/version.hpp.in diff --git a/src/rs_driver/msg/scan_msg.h b/src/rs_driver/msg/packet.hpp similarity index 86% rename from src/rs_driver/msg/scan_msg.h rename to src/rs_driver/msg/packet.hpp index fd48b521..4355bdde 100644 --- a/src/rs_driver/msg/scan_msg.h +++ b/src/rs_driver/msg/packet.hpp @@ -31,22 +31,35 @@ THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *********************************************************************************************************************/ #pragma once -#include -#include + +#include +#include +#include + namespace robosense { namespace lidar { -#ifdef _MSC_VER -struct __declspec(align(16)) ScanMsg -#elif __GNUC__ -struct __attribute__((aligned(16))) ScanMsg -#endif + +struct Packet { double timestamp = 0.0; uint32_t seq = 0; - std::string frame_id = ""; - std::vector packets; ///< A vector which store a scan of packets (the size of the vector is not fix) + uint8_t is_difop = 0; + uint8_t is_frame_begin = 0; + + 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/rs_driver/msg/pcl_point_cloud_msg.hpp b/src/rs_driver/msg/pcl_point_cloud_msg.hpp new file mode 100644 index 00000000..37a53c2c --- /dev/null +++ b/src/rs_driver/msg/pcl_point_cloud_msg.hpp @@ -0,0 +1,62 @@ +/********************************************************************************************************************* +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; + uint8_t intensity; + uint16_t ring = 0; + double timestamp = 0; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +} EIGEN_ALIGN16; + +POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZIRT, (float, x, x)(float, y, y)(float, z, z) + (std::uint8_t, 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 +}; + diff --git a/src/rs_driver/msg/point_cloud_msg.hpp b/src/rs_driver/msg/point_cloud_msg.hpp new file mode 100644 index 00000000..0904fda0 --- /dev/null +++ b/src/rs_driver/msg/point_cloud_msg.hpp @@ -0,0 +1,71 @@ +/********************************************************************************************************************* +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 + +struct PointXYZI +{ + float x; + float y; + float z; + uint8_t intensity; +}; + +struct PointXYZIRT +{ + float x; + float y; + float z; + uint8_t intensity; + uint16_t ring; + double timestamp; +}; + +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 + + VectorT points; +}; + diff --git a/src/rs_driver/utility/buffer.hpp b/src/rs_driver/utility/buffer.hpp new file mode 100644 index 00000000..32eb0a0f --- /dev/null +++ b/src/rs_driver/utility/buffer.hpp @@ -0,0 +1,89 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the +following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following +disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following +disclaimer in the documentation and/or other materials provided with the distribution. + +3. Neither the names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, +INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*********************************************************************************************************************/ + +#pragma once + +#include +#include +#include + +namespace robosense +{ +namespace lidar +{ +class Buffer +{ +public: + + Buffer(size_t buf_size) + : data_off_(0), data_size_(0) + { + buf_.resize(buf_size); + buf_size_ = buf_size; + } + + ~Buffer() = default; + + uint8_t* buf() + { + return buf_.data(); + } + + size_t bufSize() const + { + return buf_size_; + } + + uint8_t* data() + { + return buf() + data_off_; + } + + size_t dataSize() const + { + return data_size_; + } + + void setData(size_t data_off, size_t data_size) + { + data_off_ = data_off; + data_size_ = data_size; + } + +private: + std::vector buf_; + size_t buf_size_; + size_t data_off_; + size_t data_size_; +}; +} // namespace lidar +} // namespace robosense diff --git a/src/rs_driver/msg/packet_msg.h b/src/rs_driver/utility/dbg.hpp similarity index 81% rename from src/rs_driver/msg/packet_msg.h rename to src/rs_driver/utility/dbg.hpp index a9ed4a22..4e6e3fbd 100644 --- a/src/rs_driver/msg/packet_msg.h +++ b/src/rs_driver/utility/dbg.hpp @@ -31,35 +31,26 @@ THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *********************************************************************************************************************/ #pragma once -#include +#include namespace robosense { namespace lidar { -enum PktType -{ - MSOP = 0, - DIFOP -}; -#ifdef _MSC_VER -struct __declspec(align(16)) PacketMsg -#elif __GNUC__ -struct __attribute__((aligned(16))) PacketMsg ///< LiDAR single packet message -#endif + +inline void hexdump(const uint8_t* data, size_t size, const char* desc = NULL) { - std::vector packet; - PacketMsg() - { - } - PacketMsg(const PacketMsg& msg) - { - this->packet.assign(msg.packet.begin(), msg.packet.end()); - } - PacketMsg(const size_t& pkt_length) + printf("\n---------------%s(size:%d)------------------", (desc ? desc : ""), (int)size); + + for (size_t i = 0; i < size; i++) { - packet.resize(pkt_length); + if (i % 16 == 0) + printf("\n"); + printf("%02x ", data[i]); } -}; + + printf("\n---------------------------------\n"); +} + } // namespace lidar } // namespace robosense diff --git a/src/rs_driver/utility/sync_queue.hpp b/src/rs_driver/utility/sync_queue.hpp new file mode 100644 index 00000000..f47388be --- /dev/null +++ b/src/rs_driver/utility/sync_queue.hpp @@ -0,0 +1,139 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the +following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following +disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following +disclaimer in the documentation and/or other materials provided with the distribution. + +3. Neither the names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, +INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*********************************************************************************************************************/ + +#pragma once + +#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_); + } + +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/test/CMakeLists.txt b/test/CMakeLists.txt new file mode 100644 index 00000000..b32cb327 --- /dev/null +++ b/test/CMakeLists.txt @@ -0,0 +1,34 @@ + +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) + +target_link_libraries(rs_driver_test + ${GTEST_LIBRARIES} + ${EXTERNAL_LIBS}) + diff --git a/test/ab_dual_return_block_iterator_test.cpp b/test/ab_dual_return_block_iterator_test.cpp new file mode 100644 index 00000000..fdde7e06 --- /dev/null +++ b/test/ab_dual_return_block_iterator_test.cpp @@ -0,0 +1,136 @@ + +#include + +#include +#include + +using namespace robosense::lidar; + +typedef struct +{ + uint16_t distance; + uint8_t intensity; +} MyChannel; + +typedef struct +{ + uint16_t azimuth; + MyChannel channels[2]; +} MyBlock; + +typedef struct +{ + MyBlock blocks[3]; +} MyPacket; + +TEST(TestABDualPacketTraverser, ctor) +{ + { + // AAB + MyPacket pkt = + { + htons(1), 0x00, 0x00, 0x00, 0x00 + , htons(1), 0x00, 0x00, 0x00, 0x00 + , htons(21), 0x00, 0x00, 0x00, 0x00 + }; + + ABDualReturnBlockIterator iter(pkt, + 3, // blocks per packet + 0.5f, // block_duration + 25, // block_az_duraton + 2.0f); // fov_blind_duration + + int32_t az_diff; + double ts; + + // first block + iter.get (0, az_diff, ts); + ASSERT_EQ(az_diff, 20); + ASSERT_EQ(ts, 0.0f); + + // second block + iter.get (1, az_diff, ts); + ASSERT_EQ(az_diff, 20); + ASSERT_EQ(ts, 0.0f); + + // last block + iter.get (2, az_diff, ts); + ASSERT_EQ(az_diff, 25); + ASSERT_EQ(ts, 0.5f); + } + + { + // ABB + MyPacket pkt = + { + htons(1), 0x00, 0x00, 0x00, 0x00 + , htons(21), 0x00, 0x00, 0x00, 0x00 + , htons(21), 0x00, 0x00, 0x00, 0x00 + }; + + ABDualReturnBlockIterator iter(pkt, + 3, // blocks per packet + 0.5f, // block_duration + 25, // block_az_duraton + 2.0f); // fov_blind_duration + + int32_t az_diff; + double ts; + + // first block + iter.get (0, az_diff, ts); + ASSERT_EQ(az_diff, 20); + ASSERT_EQ(ts, 0.0f); + + // second block + iter.get (1, az_diff, ts); + ASSERT_EQ(az_diff, 25); + ASSERT_EQ(ts, 0.5f); + + // last block + iter.get (2, az_diff, ts); + ASSERT_EQ(az_diff, 25); + ASSERT_EQ(ts, 0.5f); + } + +} + +TEST(TestABDualPacketTraverser, ctor_fov) +{ + { + // AAB + MyPacket pkt = + { + htons(1), 0x00, 0x00, 0x00, 0x00 + , htons(1), 0x00, 0x00, 0x00, 0x00 + , htons(121), 0x00, 0x00, 0x00, 0x00 + }; + + ABDualReturnBlockIterator iter(pkt, + 3, // blocks per packet + 0.5f, // block_duration + 25, // block_az_duraton + 2.0f); // fov_blind_duration + + int32_t az_diff; + double ts; + + // first block + iter.get (0, az_diff, ts); + ASSERT_EQ(az_diff, 25); + ASSERT_EQ(ts, 0.0f); + + // second block + iter.get (1, az_diff, ts); + ASSERT_EQ(az_diff, 25); + ASSERT_EQ(ts, 0.0f); + + // last block + iter.get (2, az_diff, ts); + ASSERT_EQ(az_diff, 25); + ASSERT_EQ(ts, 2.0f); + } + +} + + diff --git a/test/basic_attr_test.cpp b/test/basic_attr_test.cpp new file mode 100644 index 00000000..7b157b25 --- /dev/null +++ b/test/basic_attr_test.cpp @@ -0,0 +1,76 @@ + +#include + +#include +#include + +using namespace robosense::lidar; + +TEST(TestParseTime, parseTimeYMD) +{ + uint8_t ts1[] = {0x15, 0x0a, 0x01, 0x01, 0x02, 0x03, 0x01, 0x11, 0x02, 0x22}; + uint8_t ts2[10]; + + ASSERT_EQ(parseTimeYMD((RSTimestampYMD*)ts1), 1633021323273546); + + createTimeYMD(1633021323273546, (RSTimestampYMD*)ts2); + ASSERT_EQ(memcmp(ts2, ts1, 10), 0); +} + +#if 0 +TEST(TestParseTime, parseTimeUTCWithNs) +{ + RSTimestampUTC ts1 = + {{0x01, 0x02, 0x03, 0x04, 0x05, 0x06}, {0x06, 0xA1, 0x1C, 0xF0}}; + RSTimestampUTC ts2; + + ASSERT_EQ(parseTimeUTCWithNs(&ts1), 0x010203040506 * 1000000 + 0x06A11CF0/1000); + + createTimeUTCWithNs(0x010203040506 * 1000000 + 0x06A11CF0/1000, &ts2); + ASSERT_EQ(memcmp(&ts2, &ts1, sizeof(ts1)), 0); +} +#endif + +TEST(TestParseTime, parseTimeUTCWithUs) +{ + RSTimestampUTC ts1 = + {{0x01, 0x02, 0x03, 0x04, 0x05, 0x06}, {0x00, 0x02, 0x33, 0x44}}; + RSTimestampUTC ts2; + + ASSERT_EQ(parseTimeUTCWithUs(&ts1), 0x010203040506 * 1000000 + 0x00023344); + + createTimeUTCWithUs(0x010203040506 * 1000000 + 0x00023344, &ts2); + ASSERT_EQ(memcmp(&ts2, &ts1, sizeof(ts1)), 0); +} + +TEST(TestParseTime, getTimeHost) +{ + getTimeHost(); +} + +TEST(TestParseTemp, parseTempInLe) +{ + { + uint8_t temp[] = {0x88, 0x11}; + ASSERT_EQ(parseTempInLe((RSTemperature*)&temp), 561); + } + + { + uint8_t temp[] = {0x88, 0x91}; + ASSERT_EQ(parseTempInLe((RSTemperature*)&temp), -561); + } +} + +TEST(TestParseTemp, parseTempInBe) +{ + { + uint8_t temp[] = {0x23, 0x10}; + ASSERT_EQ(parseTempInBe((RSTemperature*)&temp), 561); + } + + { + uint8_t temp[] = {0xA3, 0x10}; + ASSERT_EQ(parseTempInBe((RSTemperature*)&temp), -561); + } +} + diff --git a/test/buffer_test.cpp b/test/buffer_test.cpp new file mode 100644 index 00000000..e69bdd1b --- /dev/null +++ b/test/buffer_test.cpp @@ -0,0 +1,22 @@ + +#include + +#include + +using namespace robosense::lidar; + +TEST(TestBuffer, ctor) +{ + Buffer pkt(100); + + ASSERT_TRUE(pkt.buf() != NULL); + ASSERT_EQ(pkt.bufSize(), 100); + + ASSERT_EQ(pkt.data(), pkt.buf()); + ASSERT_EQ(pkt.dataSize(), 0); + + pkt.setData(5, 10); + ASSERT_EQ(pkt.data(), pkt.buf()+5); + ASSERT_EQ(pkt.dataSize(), 10); +} + diff --git a/test/chan_angles_test.cpp b/test/chan_angles_test.cpp new file mode 100644 index 00000000..e1beb052 --- /dev/null +++ b/test/chan_angles_test.cpp @@ -0,0 +1,223 @@ + +#include + +#include + +using namespace robosense::lidar; + +TEST(TestChanAngles, genUserChan) +{ + std::vector vert_angles; + std::vector user_chans; + + vert_angles.push_back(100); + vert_angles.push_back(0); + vert_angles.push_back(-100); + vert_angles.push_back(200); + + ChanAngles::genUserChan (vert_angles, user_chans); + ASSERT_EQ(user_chans.size(), 4); + ASSERT_EQ(user_chans[0], 2); + ASSERT_EQ(user_chans[1], 1); + ASSERT_EQ(user_chans[2], 0); + ASSERT_EQ(user_chans[3], 3); +} + +TEST(TestChanAngles, loadFromFile) +{ + std::vector vert_angles, horiz_angles; + + // load + ASSERT_EQ(ChanAngles::loadFromFile ("../rs_driver/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 ("../rs_driver/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 ("../rs_driver/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 ("../rs_driver/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 ("../rs_driver/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 < 9000 + 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, 0x23, 0x28 + }; + + // 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/test/decoder_rs16_test.cpp b/test/decoder_rs16_test.cpp new file mode 100644 index 00000000..2ce99185 --- /dev/null +++ b/test/decoder_rs16_test.cpp @@ -0,0 +1,52 @@ + +#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; + 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/test/decoder_rs32_test.cpp b/test/decoder_rs32_test.cpp new file mode 100644 index 00000000..800004ac --- /dev/null +++ b/test/decoder_rs32_test.cpp @@ -0,0 +1,206 @@ + +#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; + 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)); + ASSERT_EQ(decoder.getTemperature(), 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/test/decoder_rsbp_test.cpp b/test/decoder_rsbp_test.cpp new file mode 100644 index 00000000..4937f64c --- /dev/null +++ b/test/decoder_rsbp_test.cpp @@ -0,0 +1,173 @@ + +#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)); + ASSERT_EQ(decoder.getTemperature(), 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/test/decoder_test.cpp b/test/decoder_test.cpp new file mode 100644 index 00000000..cd49e6b7 --- /dev/null +++ b/test/decoder_test.cpp @@ -0,0 +1,287 @@ + +#include + +#include +#include +#include + +using namespace robosense::lidar; + +typedef PointXYZI PointT; +typedef PointCloudT PointCloud; + +#pragma pack(push, 1) +struct MyMsopPkt +{ + uint8_t id[8]; +}; + +struct MyDifopPkt +{ + uint8_t id[8]; + uint16_t rpm; + RSFOV fov; + RSCalibrationAngle vert_angle_cali[2]; + RSCalibrationAngle horiz_angle_cali[2]; +}; +#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 = "../rs_driver/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 = "../rs_driver/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)); + ASSERT_EQ(errCode, ERRCODE_SUCCESS); +} + diff --git a/test/dual_return_block_iterator_test.cpp b/test/dual_return_block_iterator_test.cpp new file mode 100644 index 00000000..14223d8b --- /dev/null +++ b/test/dual_return_block_iterator_test.cpp @@ -0,0 +1,124 @@ + +#include + +#include +#include + +using namespace robosense::lidar; + +typedef struct +{ + uint16_t distance; + uint8_t intensity; +} MyChannel; + +typedef struct +{ + uint16_t azimuth; + MyChannel channels[2]; +} MyBlock; + +typedef struct +{ + MyBlock blocks[6]; +} MyPacket; + +TEST(TestDualPacketTraverser, ctor) +{ + MyPacket pkt = + { + htons(1), 0x00, 0x00, 0x00, 0x00 + , htons(1), 0x00, 0x00, 0x00, 0x00 + , htons(21), 0x00, 0x00, 0x00, 0x00 + , htons(21), 0x00, 0x00, 0x00, 0x00 + , htons(51), 0x00, 0x00, 0x00, 0x00 + , htons(51), 0x00, 0x00, 0x00, 0x00 + }; + + DualReturnBlockIterator iter(pkt, + 6, // blocks per packet + 0.5f, // block_duration + 25, // block_az_duraton + 2.0f); // fov_blind_duration + + int32_t az_diff; + double ts; + + // first block + iter.get (0, az_diff, ts); + ASSERT_EQ(az_diff, 20); + ASSERT_EQ(ts, 0.0f); + + // second block + iter.get (1, az_diff, ts); + ASSERT_EQ(az_diff, 20); + ASSERT_EQ(ts, 0.0f); + + // third block + iter.get (2, az_diff, ts); + ASSERT_EQ(az_diff, 30); + ASSERT_EQ(ts, 0.5f); + + // last block + iter.get (4, az_diff, ts); + ASSERT_EQ(az_diff, 25); + ASSERT_EQ(ts, 1.0f); + + // last block + iter.get (5, az_diff, ts); + ASSERT_EQ(az_diff, 25); + ASSERT_EQ(ts, 1.0f); +} + +TEST(TestDualPacketTraverser, ctor_fov) +{ + MyPacket pkt = + { + htons(1), 0x00, 0x00, 0x00, 0x00 + , htons(1), 0x00, 0x00, 0x00, 0x00 + , htons(21), 0x00, 0x00, 0x00, 0x00 + , htons(21), 0x00, 0x00, 0x00, 0x00 + , htons(141), 0x00, 0x00, 0x00, 0x00 + , htons(141), 0x00, 0x00, 0x00, 0x00 + }; + + DualReturnBlockIterator iter(pkt, + 6, // blocks per packet + 0.5f, // block_duration + 25, // block_az_duraton + 2.0f); // fov_blind_duration + + int32_t az_diff; + double ts; + + // first block + iter.get (0, az_diff, ts); + ASSERT_EQ(az_diff, 20); + ASSERT_EQ(ts, 0.0f); + + // second block + iter.get (1, az_diff, ts); + ASSERT_EQ(az_diff, 20); + ASSERT_EQ(ts, 0.0f); + + // third block + iter.get (2, az_diff, ts); + ASSERT_EQ(az_diff, 25); + ASSERT_EQ(ts, 0.5f); + + // fourth block + iter.get (2, az_diff, ts); + ASSERT_EQ(az_diff, 25); + ASSERT_EQ(ts, 0.5f); + + // last block + iter.get (4, az_diff, ts); + ASSERT_EQ(az_diff, 25); + ASSERT_EQ(ts, 2.5f); + + // last block + iter.get (5, az_diff, ts); + ASSERT_EQ(az_diff, 25); + ASSERT_EQ(ts, 2.5f); +} + diff --git a/test/res/angle.csv b/test/res/angle.csv new file mode 100644 index 00000000..0c02c837 --- /dev/null +++ b/test/res/angle.csv @@ -0,0 +1,4 @@ +5,0.1 +2.5,-0.2 +0,0 +-2.5,-1 diff --git a/test/rs16_dual_return_block_iterator_test.cpp b/test/rs16_dual_return_block_iterator_test.cpp new file mode 100644 index 00000000..29cd9335 --- /dev/null +++ b/test/rs16_dual_return_block_iterator_test.cpp @@ -0,0 +1,92 @@ + +#include + +#include +#include + +using namespace robosense::lidar; + +typedef struct +{ + uint16_t distance; + uint8_t intensity; +} MyChannel; + +typedef struct +{ + uint16_t azimuth; + MyChannel channels[2]; +} MyBlock; + +typedef struct +{ + MyBlock blocks[3]; +} MyPacket; + +TEST(TestRs16DualReturnBlockIterator, ctor) +{ + MyPacket pkt = + { + htons(1), 0x00, 0x00, 0x00, 0x00 + , htons(21), 0x00, 0x00, 0x00, 0x00 + , htons(51), 0x00, 0x00, 0x00, 0x00 + }; + + Rs16DualReturnBlockIterator iter(pkt, + 3, // blocks per packet + 0.5f, // block_duration + 25, // block_az_duraton + 2.0f); // fov_blind_duration + + int32_t az_diff; + double ts; + + // first block + iter.get (0, az_diff, ts); + ASSERT_EQ(az_diff, 20); + ASSERT_EQ(ts, 0.0f); + + // second block + iter.get (1, az_diff, ts); + ASSERT_EQ(az_diff, 30); + ASSERT_EQ(ts, 0.5f); + + // third block + iter.get (2, az_diff, ts); + ASSERT_EQ(az_diff, 25); + ASSERT_EQ(ts, 1.0f); +} + +TEST(TestRs16DualReturnBlockIterator, ctor_fov) +{ + MyPacket pkt = + { + htons(1), 0x00, 0x00, 0x00, 0x00 + , htons(21), 0x00, 0x00, 0x00, 0x00 + , htons(141), 0x00, 0x00, 0x00, 0x00 + }; + + Rs16DualReturnBlockIterator iter(pkt, + 3, // blocks per packet + 0.5f, // block_duration 25, // block_az_duraton + 25, // block_az_duraton + 2.0f); // fov_blind_duration + + int32_t az_diff; + double ts; + + // first block + iter.get (0, az_diff, ts); + ASSERT_EQ(az_diff, 20); + ASSERT_EQ(ts, 0.0f); + + // second block + iter.get (1, az_diff, ts); + ASSERT_EQ(az_diff, 25); + ASSERT_EQ(ts, 0.5f); + + // third block + iter.get (2, az_diff, ts); + ASSERT_EQ(az_diff, 25); + ASSERT_EQ(ts, 2.5f); +} diff --git a/test/rs16_single_return_block_iterator_test.cpp b/test/rs16_single_return_block_iterator_test.cpp new file mode 100644 index 00000000..0174c797 --- /dev/null +++ b/test/rs16_single_return_block_iterator_test.cpp @@ -0,0 +1,93 @@ + +#include + +#include +#include + +using namespace robosense::lidar; + +typedef struct +{ + uint16_t distance; + uint8_t intensity; +} MyChannel; + +typedef struct +{ + uint16_t azimuth; + MyChannel channels[2]; +} MyBlock; + +typedef struct +{ + MyBlock blocks[3]; +} MyPacket; + +TEST(TestRs16SingleReturnBlockIterator, ctor) +{ + MyPacket pkt = + { + htons(1), 0x00, 0x00, 0x00, 0x00 + , htons(21), 0x00, 0x00, 0x00, 0x00 + , htons(51), 0x00, 0x00, 0x00, 0x00 + }; + + Rs16SingleReturnBlockIterator iter(pkt, + 3, // blocks per packet + 0.5f, // block_duration + 25, // block_az_duraton + 2.0f); // fov_blind_duration + + int32_t az_diff; + double ts; + + // first block + iter.get (0, az_diff, ts); + ASSERT_EQ(az_diff, 20); + ASSERT_EQ(ts, 0.0f); + + // second block + iter.get (1, az_diff, ts); + ASSERT_EQ(az_diff, 30); + ASSERT_EQ(ts, 1.0f); + + // third block + iter.get (2, az_diff, ts); + ASSERT_EQ(az_diff, 50); + ASSERT_EQ(ts, 2.0f); +} + +TEST(TestRs16SingleReturnBlockIterator, ctor_fov) +{ + MyPacket pkt = + { + htons(1), 0x00, 0x00, 0x00, 0x00 + , htons(21), 0x00, 0x00, 0x00, 0x00 + , htons(141), 0x00, 0x00, 0x00, 0x00 + }; + + Rs16SingleReturnBlockIterator iter(pkt, + 3, // blocks per packet + 0.5f, // block_duration 25, // block_az_duraton + 25, // block_az_duraton + 2.0f); // fov_blind_duration + + int32_t az_diff; + double ts; + + // first block + iter.get (0, az_diff, ts); + ASSERT_EQ(az_diff, 20); + ASSERT_EQ(ts, 0.0f); + + // second block + iter.get (1, az_diff, ts); + ASSERT_EQ(az_diff, 50); + ASSERT_EQ(ts, 1.0f); + + // third block + iter.get (2, az_diff, ts); + ASSERT_EQ(az_diff, 50); + ASSERT_EQ(ts, 3.0f); +} + diff --git a/test/rs_driver_test.cpp b/test/rs_driver_test.cpp new file mode 100644 index 00000000..9efd6294 --- /dev/null +++ b/test/rs_driver_test.cpp @@ -0,0 +1,8 @@ + +#include + +int main(int argc, char** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/section_test.cpp b/test/section_test.cpp new file mode 100644 index 00000000..8b3d26b3 --- /dev/null +++ b/test/section_test.cpp @@ -0,0 +1,68 @@ + +#include + +#include + +using namespace robosense::lidar; + +TEST(TestAzimuthSection, ctorFull) +{ + AzimuthSection sec(0, 36000); + ASSERT_TRUE(sec.in(0)); + ASSERT_TRUE(sec.in(10)); + ASSERT_TRUE(sec.in(36000)); +} + +TEST(TestAzimuthSection, ctor) +{ + AzimuthSection sec(10, 20); + 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, ctorNoUseBlock) +{ + DistanceSection sec(0.5, 200, 0.0, 200.5); + ASSERT_EQ(sec.min_, 0.5); + ASSERT_EQ(sec.max_, 200); +} + diff --git a/test/single_return_block_iterator_test.cpp b/test/single_return_block_iterator_test.cpp new file mode 100644 index 00000000..e3899909 --- /dev/null +++ b/test/single_return_block_iterator_test.cpp @@ -0,0 +1,89 @@ + +#include + +#include +#include + +using namespace robosense::lidar; + +typedef struct +{ + uint16_t distance; + uint8_t intensity; +} MyChannel; + +typedef struct +{ + uint16_t azimuth; + MyChannel channels[2]; +} MyBlock; + +typedef struct +{ + MyBlock blocks[3]; +} MyPacket; + +TEST(TestSingleReturnBlockIterator, ctor) +{ + MyPacket pkt = + { + htons(1), 0x00, 0x00, 0x00, 0x00 + , htons(21), 0x00, 0x00, 0x00, 0x00 + , htons(51), 0x00, 0x00, 0x00, 0x00 + }; + + SingleReturnBlockIterator iter(pkt, + 3, // blocks per packet + 0.5f, // block_duration + 25, // block_az_duraton + 2.0f); // fov_blind_duration + + int32_t az_diff; + double ts; + + // first block + iter.get (0, az_diff, ts); + ASSERT_EQ(az_diff, 20); + ASSERT_EQ(ts, 0.0f); + + iter.get (1, az_diff, ts); + ASSERT_EQ(az_diff, 30); + ASSERT_EQ(ts, 0.5f); + + iter.get (2, az_diff, ts); + ASSERT_EQ(az_diff, 25); + ASSERT_EQ(ts, 1.0f); +} + +TEST(TestSingleReturnBlockIterator, ctor_fov) +{ + MyPacket pkt = + { + htons(1), 0x00, 0x00, 0x00, 0x00 + , htons(21), 0x00, 0x00, 0x00, 0x00 + , htons(141), 0x00, 0x00, 0x00, 0x00 + }; + + SingleReturnBlockIterator iter(pkt, + 3, // blocks per packet + 0.5f, // block_duration + 25, // block_az_duraton + 2.0f); // fov_blind_duration + + int32_t az_diff; + double ts; + + // first block + iter.get (0, az_diff, ts); + ASSERT_EQ(az_diff, 20); + ASSERT_EQ(ts, 0.0f); + + iter.get (1, az_diff, ts); + ASSERT_EQ(az_diff, 25); + ASSERT_EQ(ts, 0.5f); + + iter.get (2, az_diff, ts); + ASSERT_EQ(az_diff, 25); + ASSERT_EQ(ts, 2.5f); +} + diff --git a/test/split_strategy_test.cpp b/test/split_strategy_test.cpp new file mode 100644 index 00000000..cc290649 --- /dev/null +++ b/test/split_strategy_test.cpp @@ -0,0 +1,137 @@ + +#include + +#include + +using namespace robosense::lidar; + +TEST(TestSplitStrategyByAngle, newBlock) +{ + { + SplitStrategyByAngle sa(10); + ASSERT_FALSE(sa.newBlock(5)); + ASSERT_TRUE(sa.newBlock(15)); + } + + { + SplitStrategyByAngle sa(10); + ASSERT_FALSE(sa.newBlock(5)); + ASSERT_TRUE(sa.newBlock(10)); + ASSERT_FALSE(sa.newBlock(15)); + } + + { + SplitStrategyByAngle sa(10); + ASSERT_FALSE(sa.newBlock(10)); + ASSERT_FALSE(sa.newBlock(15)); + } +} + +TEST(TestSplitStrategyByAngle, newBlock_Zero) +{ + { + SplitStrategyByAngle sa(0); + ASSERT_FALSE(sa.newBlock(35999)); + ASSERT_TRUE(sa.newBlock(1)); + ASSERT_FALSE(sa.newBlock(2)); + } + + { + SplitStrategyByAngle sa(0); + ASSERT_FALSE(sa.newBlock(35999)); + ASSERT_TRUE(sa.newBlock(0)); + ASSERT_FALSE(sa.newBlock(2)); + } + + { + SplitStrategyByAngle sa(0); + ASSERT_FALSE(sa.newBlock(0)); + ASSERT_FALSE(sa.newBlock(2)); + } +} + +TEST(TestSplitStrategyByNum, newBlock) +{ + uint16_t max_blks = 2; + SplitStrategyByNum sn(&max_blks); + ASSERT_FALSE(sn.newBlock(0)); + ASSERT_TRUE(sn.newBlock(0)); + ASSERT_FALSE(sn.newBlock(0)); + ASSERT_TRUE(sn.newBlock(0)); + + max_blks = 3; + ASSERT_FALSE(sn.newBlock(0)); + ASSERT_FALSE(sn.newBlock(0)); + ASSERT_TRUE(sn.newBlock(0)); +} + +TEST(TestSplitStrategyBySeq, newPacket_by_seq) +{ + SplitStrategyBySeq sn; + ASSERT_EQ(sn.prev_seq_, 0); + ASSERT_EQ(sn.safe_seq_min_, 0); + ASSERT_EQ(sn.safe_seq_max_, 10); + + // init value + ASSERT_FALSE(sn.newPacket(1)); + ASSERT_EQ(sn.prev_seq_, 1); + ASSERT_EQ(sn.safe_seq_min_, 0); + ASSERT_EQ(sn.safe_seq_max_, 11); + + // too big value + ASSERT_FALSE(sn.newPacket(12)); + ASSERT_EQ(sn.prev_seq_, 1); + ASSERT_EQ(sn.safe_seq_min_, 0); + ASSERT_EQ(sn.safe_seq_max_, 11); + + ASSERT_FALSE(sn.newPacket(9)); + + ASSERT_FALSE(sn.newPacket(12)); + ASSERT_EQ(sn.prev_seq_, 12); + ASSERT_EQ(sn.safe_seq_min_, 2); + ASSERT_EQ(sn.safe_seq_max_, 22); + + ASSERT_FALSE(sn.newPacket(11)); + ASSERT_EQ(sn.prev_seq_, 12); + ASSERT_EQ(sn.safe_seq_min_, 2); + ASSERT_EQ(sn.safe_seq_max_, 22); +} + +TEST(TestSplitStrategyBySeq, newPacket_prev_seq) +{ + SplitStrategyBySeq sn; + ASSERT_EQ(sn.prev_seq_, 0); + ASSERT_EQ(sn.safe_seq_min_, 0); + ASSERT_EQ(sn.safe_seq_max_, 10); + + // init value + ASSERT_FALSE(sn.newPacket(15)); + ASSERT_EQ(sn.prev_seq_, 15); + ASSERT_EQ(sn.safe_seq_min_, 5); + ASSERT_EQ(sn.safe_seq_max_, 25); +} + +TEST(TestSplitStrategyBySeq, newPacket_rewind) +{ + SplitStrategyBySeq sn; + ASSERT_EQ(sn.prev_seq_, 0); + ASSERT_EQ(sn.safe_seq_min_, 0); + ASSERT_EQ(sn.safe_seq_max_, 10); + + // init value + ASSERT_FALSE(sn.newPacket(2)); + ASSERT_EQ(sn.prev_seq_, 2); + ASSERT_EQ(sn.safe_seq_min_, 0); + ASSERT_EQ(sn.safe_seq_max_, 12); + + ASSERT_FALSE(sn.newPacket(10)); + ASSERT_FALSE(sn.newPacket(14)); + ASSERT_EQ(sn.prev_seq_, 14); + ASSERT_EQ(sn.safe_seq_min_, 4); + ASSERT_EQ(sn.safe_seq_max_, 24); + + ASSERT_TRUE(sn.newPacket(1)); + ASSERT_EQ(sn.prev_seq_, 1); + ASSERT_EQ(sn.safe_seq_min_, 0); + ASSERT_EQ(sn.safe_seq_max_, 11); +} diff --git a/test/sync_queue_test.cpp b/test/sync_queue_test.cpp new file mode 100644 index 00000000..f31b8038 --- /dev/null +++ b/test/sync_queue_test.cpp @@ -0,0 +1,59 @@ + +#include + +#include + +using namespace robosense::lidar; + +TEST(TestSyncQueue, emptyPop) +{ + SyncQueue> queue; + + ASSERT_TRUE(queue.pop().get() == NULL); + ASSERT_TRUE(queue.popWait(1000).get() == NULL); +} + +TEST(TestSyncQueue, nulPtrPop) +{ + SyncQueue> queue; + + { + std::shared_ptr n_ptr; + ASSERT_EQ(queue.push(n_ptr), 1); + ASSERT_TRUE(queue.pop().get() == NULL); + } + + { + std::shared_ptr n_ptr; + ASSERT_EQ(queue.push(n_ptr), 1); + ASSERT_TRUE(queue.popWait(1000).get() == NULL); + } +} + +TEST(TestSyncQueue, valPtrPop) +{ + SyncQueue> queue; + + { + std::shared_ptr v_ptr = std::make_shared(100); + ASSERT_EQ(queue.push(v_ptr), 1); + ASSERT_TRUE(queue.pop().get() != NULL); + } + + { + std::shared_ptr v_ptr = std::make_shared(100); + ASSERT_EQ(queue.push(v_ptr), 1); + ASSERT_TRUE(queue.popWait(1000).get() != NULL); + } +} + +TEST(TestSyncQueue, clear) +{ + SyncQueue> queue; + + std::shared_ptr v_ptr = std::make_shared(100); + ASSERT_EQ(queue.push(v_ptr), 1); + ASSERT_EQ(queue.push(v_ptr), 2); + queue.clear(); + ASSERT_EQ(queue.push(v_ptr), 1); +} diff --git a/test/trigon_test.cpp b/test/trigon_test.cpp new file mode 100644 index 00000000..63aa30fe --- /dev/null +++ b/test/trigon_test.cpp @@ -0,0 +1,32 @@ + +#include + +#include + +using namespace robosense::lidar; + +TEST(TestTrigon, ctor) +{ + Trigon trigon; +#if 0 + trigon.print(); +#endif + + ASSERT_EQ(trigon.sin(-9000), -1.0f); + ASSERT_LT(trigon.cos(-9000), 0.0001f); + + ASSERT_EQ(trigon.sin(0), 0.0f); + ASSERT_EQ(trigon.cos(0), 1.0f); + + ASSERT_EQ(trigon.sin(3000), 0.5f); + ASSERT_EQ(trigon.cos(6000), 0.5f); + + trigon.sin(44999); + trigon.cos(44999); + +#if 0 + trigon.sin(45000); + trigon.cos(45000); +#endif +} + diff --git a/tool/CMakeLists.txt b/tool/CMakeLists.txt index fe35e749..8e412da9 100644 --- a/tool/CMakeLists.txt +++ b/tool/CMakeLists.txt @@ -1,10 +1,20 @@ + cmake_minimum_required(VERSION 3.5) + project(rs_driver_viewer) + message(=============================================================) message("-- Ready to compile tools") message(=============================================================) + include_directories(${DRIVER_INCLUDE_DIRS}) -set(CMAKE_BUILD_TYPE Release) + +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") @@ -13,22 +23,40 @@ if(WIN32) 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) 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 - ) + rs_driver_viewer.cpp) + target_link_libraries(rs_driver_viewer ${EXTERNAL_LIBS} - ${PCL_LIBRARIES} -) + ${PCL_LIBRARIES}) + else() + message("PCL Not found! Can not compile rs_driver_viewer!") + endif() install(TARGETS rs_driver_viewer - RUNTIME DESTINATION /usr/bin -) + 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/tool/rs_driver_pcdsaver.cpp b/tool/rs_driver_pcdsaver.cpp new file mode 100644 index 00000000..ec59c4fb --- /dev/null +++ b/tool/rs_driver_pcdsaver.cpp @@ -0,0 +1,283 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the +following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following +disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following +disclaimer in the documentation and/or other materials provided with the distribution. + +3. Neither the names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, +INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*********************************************************************************************************************/ + +#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, RS128, RS80, RSM1)" << 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/tool/rs_driver_viewer.cpp b/tool/rs_driver_viewer.cpp index feaa5188..e70c0174 100644 --- a/tool/rs_driver_viewer.cpp +++ b/tool/rs_driver_viewer.cpp @@ -30,13 +30,22 @@ WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWIS THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *********************************************************************************************************************/ +#include +#include + #include #include -#include "rs_driver/api/lidar_driver.h" + using namespace robosense::lidar; using namespace pcl::visualization; + +typedef PointCloudT PointCloudMsg; + std::shared_ptr pcl_viewer; -std::mutex mex_viewer; +std::mutex mtx_viewer; + +SyncQueue> free_cloud_queue; +SyncQueue> stuffed_cloud_queue; bool checkKeywordExist(int argc, const char* const* argv, const char* str) { @@ -53,6 +62,7 @@ bool checkKeywordExist(int argc, const char* const* argv, const char* str) 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) @@ -60,201 +70,216 @@ bool parseArgument(int argc, const char* const* argv, const char* str, std::stri index = i + 1; } } + if (index > 0 && index < argc) { val = argv[index]; return true; } + return false; } void parseParam(int argc, char* argv[], RSDriverParam& param) { - param.wait_for_difop = false; std::string result_str; - if (parseArgument(argc, argv, "-type", 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.lidar_type = param.strToLidarType(result_str); + 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); - parseArgument(argc, argv, "-multi_cast", param.input_param.multi_cast_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); } - if (parseArgument(argc, argv, "-pcap", param.input_param.pcap_path)) - { - param.input_param.read_pcap = true; - } } void printHelpMenu() { - RS_MSG << "Arguments are: " << 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 << " -host = LiDAR destination address." << RS_REND; - RS_MSG << " -multi_cast = LiDAR destination multicast group address." << RS_REND; - RS_MSG << " -type = LiDAR type( RS16, RS32, RSBP, RS128, RS80, RSM1, RSHELIOS, RSROCK ), the " - "default " - "value is RS16" - << 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; - RS_MSG << " -pcap = The path of the pcap file, if this argument is set, the driver " - "will work in off-line mode and read the pcap file. Otherwise the driver work in online mode." - << RS_REND; + RS_MSG << "Arguments: " << RS_REND; + RS_MSG << " -type = LiDAR type(RS16, RS32, RSBP, RSHELIOS, RS128, RS80, RSM1)" << 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 printParam(const RSDriverParam& param) +void exceptionCallback(const Error& code) { - if (param.input_param.read_pcap) - { - RS_INFOL << "Working mode: "; - RS_INFO << "Offline Pcap " << RS_REND; - RS_INFOL << "Pcap Path: "; - RS_INFO << param.input_param.pcap_path << RS_REND; - } - else - { - RS_INFOL << "Working mode: "; - RS_INFO << "Online LiDAR " << RS_REND; - } - RS_INFOL << "MSOP Port: "; - RS_INFO << param.input_param.msop_port << RS_REND; - RS_INFOL << "DIFOP Port: "; - RS_INFO << param.input_param.difop_port << RS_REND; - RS_INFOL << "Host Adress: "; - RS_INFO << param.input_param.host_address << RS_REND; - RS_INFOL << "Multi-cast Group Adress: "; - RS_INFO << param.input_param.multi_cast_address << RS_REND; - RS_INFOL << "LiDAR Type: "; - RS_INFO << param.lidarTypeToStr(param.lidar_type) << RS_REND; - RS_INFOL << "Transformation Parameters (x, y, z, roll, pitch, yaw): " << RS_REND; - RS_INFOL << "x: "; - RS_INFO << std::fixed << param.decoder_param.transform_param.x << RS_REND; - RS_INFOL << "y: "; - RS_INFO << std::fixed << param.decoder_param.transform_param.y << RS_REND; - RS_INFOL << "z: "; - RS_INFO << std::fixed << param.decoder_param.transform_param.z << RS_REND; - RS_INFOL << "roll: "; - RS_INFO << std::fixed << param.decoder_param.transform_param.roll << RS_REND; - RS_INFOL << "pitch: "; - RS_INFO << std::fixed << param.decoder_param.transform_param.pitch << RS_REND; - RS_INFOL << "yaw: "; - RS_INFO << std::fixed << param.decoder_param.transform_param.yaw << RS_REND; + RS_WARNING << code.toString() << RS_REND; } -/** - * @brief The point cloud callback function. This function will be registered to lidar driver. - * When the point cloud message is ready, driver can send out messages through this function. - * @param msg The lidar point cloud message. - */ -void pointCloudCallback(const PointCloudMsg& msg) + +std::shared_ptr pointCloudGetCallback(void) { - /* Note: Please do not put time-consuming operations in the callback function! */ - /* Make a copy of the message and process it in another thread is recommended*/ - pcl::PointCloud::Ptr pcl_pointcloud(new pcl::PointCloud); - pcl_pointcloud->points.assign(msg.point_cloud_ptr->begin(), msg.point_cloud_ptr->end()); - pcl_pointcloud->height = msg.height; - pcl_pointcloud->width = msg.width; - pcl_pointcloud->is_dense = false; - PointCloudColorHandlerGenericField point_color_handle(pcl_pointcloud, "intensity"); + std::shared_ptr msg = free_cloud_queue.pop(); + if (msg.get() != NULL) { - const std::lock_guard lock(mex_viewer); - pcl_viewer->updatePointCloud(pcl_pointcloud, point_color_handle, "rslidar"); + return msg; } + + return std::make_shared(); } -/** - * @brief The exception callback function. This function will be registered to lidar driver. - * @param code The error code struct. - */ -void exceptionCallback(const Error& code) +void pointCloudPutCallback(std::shared_ptr msg) { - /* Note: Please do not put time-consuming operations in the callback function! */ - /* Make a copy of the error message and process it in another thread is recommended*/ - RS_WARNING << code.toString() << RS_REND; + 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" << RSLIDAR_VERSION_MAJOR << "." << RSLIDAR_VERSION_MINOR << "." - << RSLIDAR_VERSION_PATCH << RS_REND; + RS_TITLE << " RS_Driver Viewer Version: v" << getDriverVersion() << RS_REND; RS_TITLE << "------------------------------------------------------" << RS_REND; if (argc < 2) { - RS_INFOL << "Use 'rs_driver_viewer -h/--help' to check the argument menu..." << RS_REND; + printHelpMenu(); + return 0; } + if (checkKeywordExist(argc, argv, "-h") || checkKeywordExist(argc, argv, "--help")) { printHelpMenu(); return 0; } - pcl_viewer = std::make_shared("RSPointCloudViewer"); + 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; ///< Declare the driver object - RSDriverParam param; ///< Create a parameter object - parseParam(argc, argv, param); - printParam(param); - driver.regExceptionCallback(exceptionCallback); ///< Register the exception callback function into the driver - driver.regRecvCallback(pointCloudCallback); ///< Register the point cloud callback function into the driver - if (!driver.init(param)) ///< Call the init function and pass the parameter + LidarDriver driver; + driver.regExceptionCallback(exceptionCallback); + driver.regPointCloudCallback(pointCloudGetCallback, pointCloudPutCallback); + if (!driver.init(param)) { RS_ERROR << "Driver Initialize Error..." << RS_REND; return -1; } - driver.start(); ///< The driver thread will start + RS_INFO << "RoboSense Lidar-Driver Viewer start......" << RS_REND; + driver.start(); + while (!pcl_viewer->wasStopped()) { { - const std::lock_guard lock(mex_viewer); + 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/win/demo_online/demo_online.vcxproj b/win/demo_online/demo_online.vcxproj new file mode 100644 index 00000000..1e17ae81 --- /dev/null +++ b/win/demo_online/demo_online.vcxproj @@ -0,0 +1,144 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + + + + 15.0 + {A30B3B76-84F3-4A17-B42B-6284FCC3138F} + demoonline + 10.0 + + + + Application + true + v142 + MultiByte + + + Application + false + v142 + true + MultiByte + + + Application + true + v142 + MultiByte + + + Application + false + v142 + true + MultiByte + + + + + + + + + + + + + + + + + + + + + C:\Program Files\WpdPack_4_1_2\WpdPack\Include;$(IncludePath) + C:\Program Files\WpdPack_4_1_2\WpdPack\Lib\x64;$(LibraryPath) + + + + Level3 + Disabled + true + true + ../../../src/ + _CRT_SECURE_NO_WARNINGS;%(PreprocessorDefinitions) + + + Console + kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies);Ws2_32.lib + + + + + Level3 + Disabled + true + true + ../../src;../../../3th-party/libpcap/Include + _MBCS;%(PreprocessorDefinitions);_CRT_SECURE_NO_WARNINGS + + + Console + kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies);Ws2_32.lib;wpcap.lib + ../../../3th-party/libpcap/Lib/x64 + + + + + Level3 + MaxSpeed + true + true + true + true + + + Console + true + true + + + + + Level3 + MaxSpeed + true + true + true + true + ../../src + _CRT_SECURE_NO_WARNINGS;%(PreprocessorDefinitions) + + + Console + true + true + wpcap.lib;ws2_32.lib;%(AdditionalDependencies) + + + + + + \ No newline at end of file diff --git a/win/demo_pcap/demo_pcap.vcxproj b/win/demo_pcap/demo_pcap.vcxproj new file mode 100644 index 00000000..2a7052ec --- /dev/null +++ b/win/demo_pcap/demo_pcap.vcxproj @@ -0,0 +1,141 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + 15.0 + {E7971DE6-C89C-4572-A3A4-07BE68897D30} + demopcap + 10.0 + + + + Application + true + v142 + MultiByte + + + Application + false + v142 + true + MultiByte + + + Application + true + v142 + MultiByte + + + Application + false + v142 + true + MultiByte + + + + + + + + + + + + + + + + + + + + + C:\Program Files\WpdPack_4_1_2\WpdPack\Lib\x64;$(LibraryPath) + C:\Program Files\WpdPack_4_1_2\WpdPack\Include;$(IncludePath) + + + + Level3 + Disabled + true + true + ../../src;;../../../3th-party/libpcap/Include + _MBCS;%(PreprocessorDefinitions);_CRT_SECURE_NO_WARNINGS;ENABLE_PCAP_PARSE + + + Console + kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies);Ws2_32.lib;wpcap.lib + ../../../3th-party/libpcap/Lib/x64 + + + + + Level3 + Disabled + true + true + + + Console + + + + + Level3 + MaxSpeed + true + true + true + true + + + Console + true + true + + + + + Level3 + MaxSpeed + true + true + true + true + ../../src + _CRT_SECURE_NO_WARNINGS;%(PreprocessorDefinitions) + + + Console + true + true + wpcap.lib;ws2_32.lib;%(AdditionalDependencies) + + + + + + + + + \ No newline at end of file diff --git a/win/rs_driver.sln b/win/rs_driver.sln new file mode 100644 index 00000000..c9d9963e --- /dev/null +++ b/win/rs_driver.sln @@ -0,0 +1,51 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Version 16 +VisualStudioVersion = 16.0.32602.291 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "demo_online", "demo_online\demo_online.vcxproj", "{A30B3B76-84F3-4A17-B42B-6284FCC3138F}" +EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "demo_pcap", "demo_pcap\demo_pcap.vcxproj", "{E7971DE6-C89C-4572-A3A4-07BE68897D30}" +EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "rs_driver_viewer", "rs_driver_viewer\rs_driver_viewer.vcxproj", "{F15B6ACB-D381-48D2-B6F1-E4817FADF216}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|x64 = Debug|x64 + Debug|x86 = Debug|x86 + Release|x64 = Release|x64 + Release|x86 = Release|x86 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {A30B3B76-84F3-4A17-B42B-6284FCC3138F}.Debug|x64.ActiveCfg = Debug|x64 + {A30B3B76-84F3-4A17-B42B-6284FCC3138F}.Debug|x64.Build.0 = Debug|x64 + {A30B3B76-84F3-4A17-B42B-6284FCC3138F}.Debug|x86.ActiveCfg = Debug|Win32 + {A30B3B76-84F3-4A17-B42B-6284FCC3138F}.Debug|x86.Build.0 = Debug|Win32 + {A30B3B76-84F3-4A17-B42B-6284FCC3138F}.Release|x64.ActiveCfg = Release|x64 + {A30B3B76-84F3-4A17-B42B-6284FCC3138F}.Release|x64.Build.0 = Release|x64 + {A30B3B76-84F3-4A17-B42B-6284FCC3138F}.Release|x86.ActiveCfg = Release|Win32 + {A30B3B76-84F3-4A17-B42B-6284FCC3138F}.Release|x86.Build.0 = Release|Win32 + {E7971DE6-C89C-4572-A3A4-07BE68897D30}.Debug|x64.ActiveCfg = Debug|x64 + {E7971DE6-C89C-4572-A3A4-07BE68897D30}.Debug|x64.Build.0 = Debug|x64 + {E7971DE6-C89C-4572-A3A4-07BE68897D30}.Debug|x86.ActiveCfg = Debug|Win32 + {E7971DE6-C89C-4572-A3A4-07BE68897D30}.Debug|x86.Build.0 = Debug|Win32 + {E7971DE6-C89C-4572-A3A4-07BE68897D30}.Release|x64.ActiveCfg = Release|x64 + {E7971DE6-C89C-4572-A3A4-07BE68897D30}.Release|x64.Build.0 = Release|x64 + {E7971DE6-C89C-4572-A3A4-07BE68897D30}.Release|x86.ActiveCfg = Release|Win32 + {E7971DE6-C89C-4572-A3A4-07BE68897D30}.Release|x86.Build.0 = Release|Win32 + {F15B6ACB-D381-48D2-B6F1-E4817FADF216}.Debug|x64.ActiveCfg = Debug|x64 + {F15B6ACB-D381-48D2-B6F1-E4817FADF216}.Debug|x64.Build.0 = Debug|x64 + {F15B6ACB-D381-48D2-B6F1-E4817FADF216}.Debug|x86.ActiveCfg = Debug|Win32 + {F15B6ACB-D381-48D2-B6F1-E4817FADF216}.Debug|x86.Build.0 = Debug|Win32 + {F15B6ACB-D381-48D2-B6F1-E4817FADF216}.Release|x64.ActiveCfg = Release|x64 + {F15B6ACB-D381-48D2-B6F1-E4817FADF216}.Release|x64.Build.0 = Release|x64 + {F15B6ACB-D381-48D2-B6F1-E4817FADF216}.Release|x86.ActiveCfg = Release|Win32 + {F15B6ACB-D381-48D2-B6F1-E4817FADF216}.Release|x86.Build.0 = Release|Win32 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection + GlobalSection(ExtensibilityGlobals) = postSolution + SolutionGuid = {56237513-8A68-4EC5-9B6B-BAB05BA945B4} + EndGlobalSection +EndGlobal diff --git a/win/rs_driver_viewer/rs_driver_viewer.vcxproj b/win/rs_driver_viewer/rs_driver_viewer.vcxproj new file mode 100644 index 00000000..5de8a52d --- /dev/null +++ b/win/rs_driver_viewer/rs_driver_viewer.vcxproj @@ -0,0 +1,155 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + 16.0 + Win32Proj + {f15b6acb-d381-48d2-b6f1-e4817fadf216} + rsdriverviewer + 10.0 + + + + Application + true + v142 + Unicode + + + Application + false + v142 + true + Unicode + + + Application + true + v142 + Unicode + + + Application + false + v142 + true + Unicode + + + + + + + + + + + + + + + + + + + + + true + + + false + + + true + C:\Program Files\WpdPack_4_1_2\WpdPack\Include;C:\Program Files\PCL 1.11.1\include\pcl-1.11;C:\Program Files\PCL 1.11.1\3rdParty\Boost\include\boost-1_74;C:\Program Files\PCL 1.11.1\3rdParty\Eigen\eigen3;C:\Program Files\PCL 1.11.1\3rdParty\FLANN\include;C:\Program Files\PCL 1.11.1\3rdParty\Qhull\include;C:\Program Files\PCL 1.11.1\3rdParty\VTK\include\vtk-8.2;C:\Program Files\OpenNI2\Include;$(IncludePath) + C:\Program Files\WpdPack_4_1_2\WpdPack\Lib\x64;C:\Program Files\PCL 1.11.1\lib;C:\Program Files\PCL 1.11.1\3rdParty\Boost\lib;C:\Program Files\PCL 1.11.1\3rdParty\FLANN\lib;C:\Program Files\PCL 1.11.1\3rdParty\Qhull\lib;C:\Program Files\PCL 1.11.1\3rdParty\VTK\lib;C:\Program Files\OpenNI2\Lib;$(LibraryPath) + + + false + C:\Program Files\WpdPack_4_1_2\WpdPack\Include;C:\Program Files\PCL 1.11.1\include\pcl-1.11;C:\Program Files\PCL 1.11.1\3rdParty\Boost\include\boost-1_74;C:\Program Files\PCL 1.11.1\3rdParty\Eigen\eigen3;C:\Program Files\PCL 1.11.1\3rdParty\FLANN\include;C:\Program Files\PCL 1.11.1\3rdParty\Qhull\include;C:\Program Files\PCL 1.11.1\3rdParty\VTK\include\vtk-8.2;C:\Program Files\OpenNI2\Include;$(IncludePath) + C:\Program Files\WpdPack_4_1_2\WpdPack\Lib\x64;C:\Program Files\PCL 1.11.1\lib;C:\Program Files\PCL 1.11.1\3rdParty\Boost\lib;C:\Program Files\PCL 1.11.1\3rdParty\FLANN\lib;C:\Program Files\PCL 1.11.1\3rdParty\Qhull\lib;C:\Program Files\PCL 1.11.1\3rdParty\VTK\lib;C:\Program Files\OpenNI2\Lib;$(LibraryPath) + + + + Level3 + true + WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + + + Console + true + + + + + Level3 + true + true + true + WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + + + Console + true + true + true + + + + + Level3 + false + BOOST_USE_WINDOWS_H;NOMINMAX;_CRT_SECURE_NO_DEPRECATE;_CRT_SECURE_NO_WARNINGS;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + ../../src + + + Console + true + wpcap.lib;ws2_32.lib;vtkChartsCore-8.2.lib;vtkCommonColor-8.2.lib;vtkCommonComputationalGeometry-8.2.lib;vtkCommonCore-8.2.lib;vtkCommonDataModel-8.2.lib;vtkCommonExecutionModel-8.2.lib;vtkCommonMath-8.2.lib;vtkCommonMisc-8.2.lib;vtkCommonSystem-8.2.lib;vtkCommonTransforms-8.2.lib;vtkDICOMParser-8.2.lib;vtkDomainsChemistry-8.2.lib;vtkDomainsChemistryOpenGL2-8.2.lib;vtkdoubleconversion-8.2.lib;vtkexodusII-8.2.lib;vtkexpat-8.2.lib;vtkFiltersAMR-8.2.lib;vtkFiltersCore-8.2.lib;vtkFiltersExtraction-8.2.lib;vtkFiltersFlowPaths-8.2.lib;vtkFiltersGeneral-8.2.lib;vtkFiltersGeneric-8.2.lib;vtkFiltersGeometry-8.2.lib;vtkFiltersHybrid-8.2.lib;vtkFiltersHyperTree-8.2.lib;vtkFiltersImaging-8.2.lib;vtkFiltersModeling-8.2.lib;vtkFiltersParallel-8.2.lib;vtkFiltersParallelImaging-8.2.lib;vtkFiltersPoints-8.2.lib;vtkFiltersProgrammable-8.2.lib;vtkFiltersSelection-8.2.lib;vtkFiltersSMP-8.2.lib;vtkFiltersSources-8.2.lib;vtkFiltersStatistics-8.2.lib;vtkFiltersTexture-8.2.lib;vtkFiltersTopology-8.2.lib;vtkFiltersVerdict-8.2.lib;vtkfreetype-8.2.lib;vtkGeovisCore-8.2.lib;vtkgl2ps-8.2.lib;vtkglew-8.2.lib;vtkGUISupportMFC-8.2.lib;vtkhdf5-8.2.lib;vtkhdf5_hl-8.2.lib;vtkImagingColor-8.2.lib;vtkImagingCore-8.2.lib;vtkImagingFourier-8.2.lib;vtkImagingGeneral-8.2.lib;vtkImagingHybrid-8.2.lib;vtkImagingMath-8.2.lib;vtkImagingMorphological-8.2.lib;vtkImagingSources-8.2.lib;vtkImagingStatistics-8.2.lib;vtkImagingStencil-8.2.lib;vtkInfovisCore-8.2.lib;vtkInfovisLayout-8.2.lib;vtkInteractionImage-8.2.lib;vtkInteractionStyle-8.2.lib;vtkInteractionWidgets-8.2.lib;vtkIOAMR-8.2.lib;vtkIOAsynchronous-8.2.lib;vtkIOCityGML-8.2.lib;vtkIOCore-8.2.lib;vtkIOEnSight-8.2.lib;vtkIOExodus-8.2.lib;vtkIOExport-8.2.lib;vtkIOExportOpenGL2-8.2.lib;vtkIOExportPDF-8.2.lib;vtkIOGeometry-8.2.lib;vtkIOImage-8.2.lib;vtkIOImport-8.2.lib;vtkIOInfovis-8.2.lib;vtkIOLegacy-8.2.lib;vtkIOLSDyna-8.2.lib;vtkIOMINC-8.2.lib;vtkIOMovie-8.2.lib;vtkIONetCDF-8.2.lib;vtkIOParallel-8.2.lib;vtkIOParallelXML-8.2.lib;vtkIOPLY-8.2.lib;vtkIOSegY-8.2.lib;vtkIOSQL-8.2.lib;vtkIOTecplotTable-8.2.lib;vtkIOVeraOut-8.2.lib;vtkIOVideo-8.2.lib;vtkIOXML-8.2.lib;vtkIOXMLParser-8.2.lib;vtkjpeg-8.2.lib;vtkjsoncpp-8.2.lib;vtklibharu-8.2.lib;vtklibxml2-8.2.lib;vtklz4-8.2.lib;vtklzma-8.2.lib;vtkmetaio-8.2.lib;vtkNetCDF-8.2.lib;vtkogg-8.2.lib;vtkParallelCore-8.2.lib;vtkpng-8.2.lib;vtkproj-8.2.lib;vtkpugixml-8.2.lib;vtkRenderingAnnotation-8.2.lib;vtkRenderingContext2D-8.2.lib;vtkRenderingContextOpenGL2-8.2.lib;vtkRenderingCore-8.2.lib;vtkRenderingExternal-8.2.lib;vtkRenderingFreeType-8.2.lib;vtkRenderingGL2PSOpenGL2-8.2.lib;vtkRenderingImage-8.2.lib;vtkRenderingLabel-8.2.lib;vtkRenderingLOD-8.2.lib;vtkRenderingOpenGL2-8.2.lib;vtkRenderingVolume-8.2.lib;vtkRenderingVolumeOpenGL2-8.2.lib;vtksqlite-8.2.lib;vtksys-8.2.lib;vtktheora-8.2.lib;vtktiff-8.2.lib;vtkverdict-8.2.lib;vtkViewsContext2D-8.2.lib;vtkViewsCore-8.2.lib;vtkViewsInfovis-8.2.lib;vtkzlib-8.2.lib;pcl_common.lib;pcl_commond.lib;pcl_features.lib;pcl_featuresd.lib;pcl_filters.lib;pcl_filtersd.lib;pcl_io.lib;pcl_iod.lib;pcl_io_ply.lib;pcl_io_plyd.lib;pcl_kdtree.lib;pcl_kdtreed.lib;pcl_keypoints.lib;pcl_keypointsd.lib;pcl_ml.lib;pcl_mld.lib;pcl_octree.lib;pcl_octreed.lib;pcl_outofcore.lib;pcl_outofcored.lib;pcl_people.lib;pcl_peopled.lib;pcl_recognition.lib;pcl_recognitiond.lib;pcl_registration.lib;pcl_registrationd.lib;pcl_sample_consensus.lib;pcl_sample_consensusd.lib;pcl_search.lib;pcl_searchd.lib;pcl_segmentation.lib;pcl_segmentationd.lib;pcl_stereo.lib;pcl_stereod.lib;pcl_surface.lib;pcl_surfaced.lib;pcl_tracking.lib;pcl_trackingd.lib;pcl_visualization.lib;pcl_visualizationd.lib;%(AdditionalDependencies) + + + + + Level3 + true + true + false + BOOST_USE_WINDOWS_H;NOMINMAX;_CRT_SECURE_NO_DEPRECATE;_CRT_SECURE_NO_WARNINGS;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + ../../src + + + Console + true + true + true + wpcap.lib;ws2_32.lib;vtkChartsCore-8.2.lib;vtkCommonColor-8.2.lib;vtkCommonComputationalGeometry-8.2.lib;vtkCommonCore-8.2.lib;vtkCommonDataModel-8.2.lib;vtkCommonExecutionModel-8.2.lib;vtkCommonMath-8.2.lib;vtkCommonMisc-8.2.lib;vtkCommonSystem-8.2.lib;vtkCommonTransforms-8.2.lib;vtkDICOMParser-8.2.lib;vtkDomainsChemistry-8.2.lib;vtkDomainsChemistryOpenGL2-8.2.lib;vtkdoubleconversion-8.2.lib;vtkexodusII-8.2.lib;vtkexpat-8.2.lib;vtkFiltersAMR-8.2.lib;vtkFiltersCore-8.2.lib;vtkFiltersExtraction-8.2.lib;vtkFiltersFlowPaths-8.2.lib;vtkFiltersGeneral-8.2.lib;vtkFiltersGeneric-8.2.lib;vtkFiltersGeometry-8.2.lib;vtkFiltersHybrid-8.2.lib;vtkFiltersHyperTree-8.2.lib;vtkFiltersImaging-8.2.lib;vtkFiltersModeling-8.2.lib;vtkFiltersParallel-8.2.lib;vtkFiltersParallelImaging-8.2.lib;vtkFiltersPoints-8.2.lib;vtkFiltersProgrammable-8.2.lib;vtkFiltersSelection-8.2.lib;vtkFiltersSMP-8.2.lib;vtkFiltersSources-8.2.lib;vtkFiltersStatistics-8.2.lib;vtkFiltersTexture-8.2.lib;vtkFiltersTopology-8.2.lib;vtkFiltersVerdict-8.2.lib;vtkfreetype-8.2.lib;vtkGeovisCore-8.2.lib;vtkgl2ps-8.2.lib;vtkglew-8.2.lib;vtkGUISupportMFC-8.2.lib;vtkhdf5-8.2.lib;vtkhdf5_hl-8.2.lib;vtkImagingColor-8.2.lib;vtkImagingCore-8.2.lib;vtkImagingFourier-8.2.lib;vtkImagingGeneral-8.2.lib;vtkImagingHybrid-8.2.lib;vtkImagingMath-8.2.lib;vtkImagingMorphological-8.2.lib;vtkImagingSources-8.2.lib;vtkImagingStatistics-8.2.lib;vtkImagingStencil-8.2.lib;vtkInfovisCore-8.2.lib;vtkInfovisLayout-8.2.lib;vtkInteractionImage-8.2.lib;vtkInteractionStyle-8.2.lib;vtkInteractionWidgets-8.2.lib;vtkIOAMR-8.2.lib;vtkIOAsynchronous-8.2.lib;vtkIOCityGML-8.2.lib;vtkIOCore-8.2.lib;vtkIOEnSight-8.2.lib;vtkIOExodus-8.2.lib;vtkIOExport-8.2.lib;vtkIOExportOpenGL2-8.2.lib;vtkIOExportPDF-8.2.lib;vtkIOGeometry-8.2.lib;vtkIOImage-8.2.lib;vtkIOImport-8.2.lib;vtkIOInfovis-8.2.lib;vtkIOLegacy-8.2.lib;vtkIOLSDyna-8.2.lib;vtkIOMINC-8.2.lib;vtkIOMovie-8.2.lib;vtkIONetCDF-8.2.lib;vtkIOParallel-8.2.lib;vtkIOParallelXML-8.2.lib;vtkIOPLY-8.2.lib;vtkIOSegY-8.2.lib;vtkIOSQL-8.2.lib;vtkIOTecplotTable-8.2.lib;vtkIOVeraOut-8.2.lib;vtkIOVideo-8.2.lib;vtkIOXML-8.2.lib;vtkIOXMLParser-8.2.lib;vtkjpeg-8.2.lib;vtkjsoncpp-8.2.lib;vtklibharu-8.2.lib;vtklibxml2-8.2.lib;vtklz4-8.2.lib;vtklzma-8.2.lib;vtkmetaio-8.2.lib;vtkNetCDF-8.2.lib;vtkogg-8.2.lib;vtkParallelCore-8.2.lib;vtkpng-8.2.lib;vtkproj-8.2.lib;vtkpugixml-8.2.lib;vtkRenderingAnnotation-8.2.lib;vtkRenderingContext2D-8.2.lib;vtkRenderingContextOpenGL2-8.2.lib;vtkRenderingCore-8.2.lib;vtkRenderingExternal-8.2.lib;vtkRenderingFreeType-8.2.lib;vtkRenderingGL2PSOpenGL2-8.2.lib;vtkRenderingImage-8.2.lib;vtkRenderingLabel-8.2.lib;vtkRenderingLOD-8.2.lib;vtkRenderingOpenGL2-8.2.lib;vtkRenderingVolume-8.2.lib;vtkRenderingVolumeOpenGL2-8.2.lib;vtksqlite-8.2.lib;vtksys-8.2.lib;vtktheora-8.2.lib;vtktiff-8.2.lib;vtkverdict-8.2.lib;vtkViewsContext2D-8.2.lib;vtkViewsCore-8.2.lib;vtkViewsInfovis-8.2.lib;vtkzlib-8.2.lib;pcl_common.lib;pcl_commond.lib;pcl_features.lib;pcl_featuresd.lib;pcl_filters.lib;pcl_filtersd.lib;pcl_io.lib;pcl_iod.lib;pcl_io_ply.lib;pcl_io_plyd.lib;pcl_kdtree.lib;pcl_kdtreed.lib;pcl_keypoints.lib;pcl_keypointsd.lib;pcl_ml.lib;pcl_mld.lib;pcl_octree.lib;pcl_octreed.lib;pcl_outofcore.lib;pcl_outofcored.lib;pcl_people.lib;pcl_peopled.lib;pcl_recognition.lib;pcl_recognitiond.lib;pcl_registration.lib;pcl_registrationd.lib;pcl_sample_consensus.lib;pcl_sample_consensusd.lib;pcl_search.lib;pcl_searchd.lib;pcl_segmentation.lib;pcl_segmentationd.lib;pcl_stereo.lib;pcl_stereod.lib;pcl_surface.lib;pcl_surfaced.lib;pcl_tracking.lib;pcl_trackingd.lib;pcl_visualization.lib;pcl_visualizationd.lib;%(AdditionalDependencies) + + + + + + + + + \ No newline at end of file