diff --git a/.github/Jenkinsfile b/.github/Jenkinsfile new file mode 100644 index 00000000..4b59e7b0 --- /dev/null +++ b/.github/Jenkinsfile @@ -0,0 +1 @@ +buildPackage() diff --git a/.gitignore b/.gitignore index 01b13ecd..581c6ddb 100644 --- a/.gitignore +++ b/.gitignore @@ -12,4 +12,3 @@ cmake-build-debug/ version.h *.pb.cc *.pb.h -package.xml diff --git a/.gitmodules b/.gitmodules index 3864e32a..40552b95 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,4 @@ [submodule "src/rs_driver"] path = src/rs_driver url = https://github.com/RoboSense-LiDAR/rs_driver.git + branch = main diff --git a/CHANGELOG.md b/CHANGELOG.md index 9228a799..505bb240 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,4 +1,114 @@ -# Changelog +# CHANGELOG + +## v1.5.16 2024-08-27 +### Added +- Load config path frome ros2 param. +### Changed +- Remove the original compilation method. +### Fixed +- Use single package.xml file for both ROS1 and ROS2 @Timple. +- Update msop protocol of RSMX. + +## v1.5.15 2024-08-07 +### Added +- Support RSM3. + +## v1.5.14 2024-07-15 +### Added +- Support multiple lidars with different multicast addresses and the same port. +### Fixed +- Fixed the bug that only one lidar was parsed correctly when multiple bp4.0 were used. +- Fix version number in the package.xml by @Timple. + +## v1.5.13 2024-05-10 +### Added +- Support RSMX. +### Fixed +- Update timestamp parsing unit and the number of packets per frame in decoder_RSE1. +- Update firing_tss of Helios/Helios16P/RubyPlus. +- Fix compilation bug of unit test. +- Remove duplicate text "/rslidar_packets" by @luhuadong. + +## v1.5.12 2023-12-28 +### Fixed +- Fix bug in getting device info and status. +- Fix bug in getting device temperature. + +## v1.5.11 2023-12-18 + +### Changed +- Enable modify socket buffer size. + +## v1.5.10 - 2023-02-17 + +### Changed +- Merge RSBPV4 into RSBP + + +## v1.5.9 - 2023-02-17 + +### Changed +- Increase sending DDS buffer queue to avoid packet loss + + +## v1.5.8 - 2022-12-09 + +### Added +- Support ROS2/Humble Hawksbill +- rename RSEOS as RSE1 + +### Fixed +- Fix wrong widthxheight while ros_send_by_rows=true + + +## v1.5.7 - 2022-10-09 + +### Added +- Seperate RSBPV4 from RSBP +- Support to receive MSOP/DIFOP packet from rosbag v1.3.x +- Support option ros_send_by_rows + +## v1.5.6 - 2022-09-01 + +### Added ++ Add a few build options according to rs_driver ++ Update help documents + +## v1.5.5 - 2022-08-01 + +### Changed +- Output intensity in point cloud as float32 + +### Fixed +- Fix compiling and runtime error on ROS2 Elequent +- Fix frame_id in help docs + +## v1.5.4 - 2022-07-01 + +### Added +- Support the option to stamp the point cloud with the first point + +### Changed +- Remove the dependency on the protobuf library + +## v1.5.3 - 2022-06-01 + +### Added +- Support Jumbo Mode + +### Fixed +- Fix compiling error when protobuf is unavailable + +## v1.5.0 + +### Changed +- refactory the project + +### Added +- support user_layer_bytes and tail_layer_bytes +- support M2 +- replace point with point cloud, as rs_driver's template parameter +- handle point cloud in rs_driver's thread ## v1.3.0 - 2020-11-10 @@ -9,7 +119,6 @@ - Add different point types( XYZI & XYZIRT) ### Changed - - Update driver core, please refer to CHANGELOG in rs_driver for details - Update some documents - Change angle_path argument to hiding parameter @@ -19,15 +128,11 @@ - Remove RSAUTO for lidar type - Remove device_ip argument - - ## v1.2.1 - 2020-09-04 ### Fixed - - Fix bug in driver core, please refer to changelog in rs_driver for details. - ## v1.2.0 - 2020-09-01 ### Added @@ -37,11 +142,9 @@ - Update driver core, please refer to changelog in rs_driver for details - Update the compiler version from C++11 to C++14 - ## v1.1.0 - 2020-07-01 ### Added - - Add ROS2 support ### Changed @@ -55,9 +158,7 @@ ## v1.0.0 - 2020-06-01 ### Added - - New program structure - - Support ROS & Protobuf-UDP functions - \ No newline at end of file + diff --git a/CMakeLists.txt b/CMakeLists.txt index e3d848ab..be0a1d47 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,165 +2,194 @@ cmake_minimum_required(VERSION 3.5) cmake_policy(SET CMP0048 NEW) project(rslidar_sdk) -#======================================= -# Compile setup (ORIGINAL, CATKIN, COLCON) -#======================================= -set(COMPILE_METHOD COLCON) - #======================================= # Custom Point Type (XYZI, XYZIRT) #======================================= -set(POINT_TYPE XYZI) +set(POINT_TYPE XYZIRT) + +option(ENABLE_TRANSFORM "Enable transform functions" OFF) +if(${ENABLE_TRANSFORM}) + add_definitions("-DENABLE_TRANSFORM") + + find_package(Eigen3 REQUIRED) + include_directories(${EIGEN3_INCLUDE_DIR}) +endif(${ENABLE_TRANSFORM}) + +option(ENABLE_EPOLL_RECEIVE "Receive packets with epoll() instead of select()" OFF) +if(${ENABLE_EPOLL_RECEIVE}) + add_definitions("-DENABLE_EPOLL_RECEIVE") +endif(${ENABLE_EPOLL_RECEIVE}) + +option(ENABLE_MODIFY_RECVBUF "Enable modify size of RECVBUF" ON) +if(${ENABLE_MODIFY_RECVBUF}) + add_definitions("-DENABLE_MODIFY_RECVBUF") +endif(${ENABLE_MODIFY_RECVBUF}) + +option(ENABLE_WAIT_IF_QUEUE_EMPTY "Enable waiting for a while in handle thread if the queue is empty" OFF) +if(${ENABLE_WAIT_IF_QUEUE_EMPTY}) + add_definitions("-DENABLE_WAIT_IF_QUEUE_EMPTY") +endif(${ENABLE_WAIT_IF_QUEUE_EMPTY}) + +option(ENABLE_DIFOP_PARSE "Enable parsing DIFOP Packet" ON) +if(${ENABLE_DIFOP_PARSE}) + add_definitions("-DENABLE_DIFOP_PARSE") +endif(${ENABLE_DIFOP_PARSE}) + +option(ENABLE_STAMP_WITH_LOCAL "Enable stamp point cloud with local time" OFF) +if(${ENABLE_STAMP_WITH_LOCAL}) + add_definitions("-DENABLE_STAMP_WITH_LOCAL") +endif(${ENABLE_STAMP_WITH_LOCAL}) + + +option(ENABLE_SOURCE_PACKET_LEGACY "Enable ROS Source of MSOP/DIFOP Packet v1.3.x" OFF) +if(${ENABLE_SOURCE_PACKET_LEGACY}) + add_definitions("-DENABLE_SOURCE_PACKET_LEGACY") +endif(${ENABLE_SOURCE_PACKET_LEGACY}) #======================== # Project details / setup #======================== set(PROJECT_NAME rslidar_sdk) + add_definitions(-DPROJECT_PATH="${PROJECT_SOURCE_DIR}") -set(CMAKE_BUILD_TYPE RELEASE) -add_definitions(-O3) -add_definitions(-std=c++14) + +if (CMAKE_BUILD_TYPE STREQUAL "") + set(CMAKE_BUILD_TYPE Release) + add_definitions(-O3) +endif() + +add_definitions(-std=c++17) add_compile_options(-Wall) +#======================== +# Point Type Definition +#======================== +if(${POINT_TYPE} STREQUAL "XYZI") + add_definitions(-DPOINT_TYPE_XYZI) +elseif(${POINT_TYPE} STREQUAL "XYZIRT") + add_definitions(-DPOINT_TYPE_XYZIRT) +endif() + +message(=============================================================) +message("-- POINT_TYPE is ${POINT_TYPE}") +message(=============================================================) + #======================== # Dependencies Setup #======================== + #ROS# find_package(roscpp 1.12 QUIET) + if(roscpp_FOUND) + message(=============================================================) - message("-- ROS Found, Ros Support is turned On!") + message("-- ROS Found. ROS Support is turned On.") message(=============================================================) - configure_file(${CMAKE_CURRENT_SOURCE_DIR}/package_ros1.xml ${CMAKE_CURRENT_SOURCE_DIR}/package.xml COPYONLY) + add_definitions(-DROS_FOUND) - include_directories(${roscpp_INCLUDE_DIRS}) + + find_package(roslib QUIET) + include_directories(${roscpp_INCLUDE_DIRS} ${roslib_INCLUDE_DIRS}) + set(ROS_LIBS ${roscpp_LIBRARIES} ${roslib_LIBRARIES}) + + add_definitions(-DRUN_IN_ROS_WORKSPACE) + + find_package(catkin REQUIRED COMPONENTS + roscpp + sensor_msgs + roslib) + + catkin_package(CATKIN_DEPENDS + sensor_msgs + roslib) + else(roscpp_FOUND) + message(=============================================================) - message("-- ROS Not Found, Ros Support is turned Off!") + message("-- ROS Not Found. ROS Support is turned Off.") message(=============================================================) + endif(roscpp_FOUND) + #ROS2# find_package(rclcpp QUIET) -if(rclcpp_FOUND AND ${COMPILE_METHOD} STREQUAL "COLCON") - find_package(ament_cmake REQUIRED) - find_package(sensor_msgs REQUIRED) - find_package(rslidar_msg REQUIRED) - find_package(std_msgs REQUIRED) - set(CMAKE_CXX_STANDARD 14) + +if(rclcpp_FOUND) message(=============================================================) - message("-- ROS2 Found, Ros2 Support is turned On!") + message("-- ROS2 Found. ROS2 Support is turned On.") message(=============================================================) - configure_file(${CMAKE_CURRENT_SOURCE_DIR}/package_ros2.xml ${CMAKE_CURRENT_SOURCE_DIR}/package.xml COPYONLY) + add_definitions(-DROS2_FOUND) include_directories(${rclcpp_INCLUDE_DIRS}) -else(rclcpp_FOUND AND ${COMPILE_METHOD} STREQUAL "COLCON") - message(=============================================================) - message("-- ROS2 Not Found, Ros2 Support is turned Off!") - message(=============================================================) -endif(rclcpp_FOUND AND ${COMPILE_METHOD} STREQUAL "COLCON") -#Protobuf# -find_package(Protobuf QUIET) -find_program(PROTOC protoc) -if(NOT PROTOC MATCHES "NOTFOUND" AND Protobuf_FOUND) - message(=============================================================) - message("-- Protobuf Found, Protobuf Support is turned On!") - message(=============================================================) - add_definitions(-DPROTO_FOUND) - include_directories(${PROTOBUF_INCLUDE_DIRS}) - SET(PROTO_FILE_PATH ${PROJECT_SOURCE_DIR}/src/msg/proto_msg) - file(GLOB PROTOBUF_FILELIST ${PROTO_FILE_PATH}/*.proto) - foreach(proto_file ${PROTOBUF_FILELIST}) - message(STATUS "COMPILING ${proto_file} USING ${PROTOBUF_PROTOC_EXECUTABLE}") - execute_process(COMMAND ${PROTOBUF_PROTOC_EXECUTABLE} - --proto_path=${PROTO_FILE_PATH} - --cpp_out=${PROTO_FILE_PATH} - ${proto_file}) - endforeach() -else(NOT PROTOC MATCHES "NOTFOUND" AND Protobuf_FOUND) + set(CMAKE_CXX_STANDARD 14) + + find_package(ament_cmake REQUIRED) + find_package(sensor_msgs REQUIRED) + find_package(rslidar_msg REQUIRED) + find_package(std_msgs REQUIRED) + +else(rclcpp_FOUND) message(=============================================================) - message("-- Protobuf Not Found, Protobuf Support is turned Off!") + message("-- ROS2 Not Found. ROS2 Support is turned Off.") message(=============================================================) -endif(NOT PROTOC MATCHES "NOTFOUND" AND Protobuf_FOUND) +endif(rclcpp_FOUND) + #Others# find_package(yaml-cpp REQUIRED) -find_package(PCL QUIET REQUIRED) -include_directories(${PCL_INCLUDE_DIRS}) -link_directories(${PCL_LIBRARY_DIRS}) -add_definitions(${PCL_DEFINITIONS}) -#Catkin# -if(${COMPILE_METHOD} STREQUAL "CATKIN") - find_package(catkin REQUIRED COMPONENTS - roscpp - sensor_msgs - ) - - catkin_package( - CATKIN_DEPENDS sensor_msgs - ) -endif(${COMPILE_METHOD} STREQUAL "CATKIN") + #Include directory# include_directories(${PROJECT_SOURCE_DIR}/src) + #Driver core# add_subdirectory(src/rs_driver) find_package(rs_driver REQUIRED) include_directories(${rs_driver_INCLUDE_DIRS}) -#======================== -# Point Type Definition -#======================== -if(${POINT_TYPE} STREQUAL "XYZI") -add_definitions(-DPOINT_TYPE_XYZI) -elseif(${POINT_TYPE} STREQUAL "XYZIRT") -add_definitions(-DPOINT_TYPE_XYZIRT) -endif() -message(=============================================================) -message("-- POINT_TYPE is ${POINT_TYPE}") -message(=============================================================) - #======================== # Build Setup #======================== -#Protobuf# -if(NOT PROTOC MATCHES "NOTFOUND" AND Protobuf_FOUND) - add_executable(rslidar_sdk_node - node/rslidar_sdk_node.cpp - src/manager/adapter_manager.cpp - ${PROTO_FILE_PATH}/packet.pb.cc - ${PROTO_FILE_PATH}/scan.pb.cc - ${PROTO_FILE_PATH}/point_cloud.pb.cc - ) - target_link_libraries(rslidar_sdk_node - ${PCL_LIBRARIES} - ${YAML_CPP_LIBRARIES} - ${PROTOBUF_LIBRARY} - ${rs_driver_LIBRARIES} - ) -else(NOT PROTOC MATCHES "NOTFOUND" AND Protobuf_FOUND) - add_executable(rslidar_sdk_node - node/rslidar_sdk_node.cpp - src/manager/adapter_manager.cpp - ) - target_link_libraries(rslidar_sdk_node - ${PCL_LIBRARIES} + +add_executable(rslidar_sdk_node + node/rslidar_sdk_node.cpp + src/manager/node_manager.cpp) + +target_link_libraries(rslidar_sdk_node ${YAML_CPP_LIBRARIES} - ${rs_driver_LIBRARIES} - ) -endif(NOT PROTOC MATCHES "NOTFOUND" AND Protobuf_FOUND) + ${rs_driver_LIBRARIES}) + #Ros# if(roscpp_FOUND) - target_link_libraries(rslidar_sdk_node ${roscpp_LIBRARIES}) + + target_link_libraries(rslidar_sdk_node + ${ROS_LIBS}) + + install(TARGETS rslidar_sdk_node + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + endif(roscpp_FOUND) + #Ros2# -if(rclcpp_FOUND AND ${COMPILE_METHOD} STREQUAL "COLCON") - ament_target_dependencies(rslidar_sdk_node rclcpp sensor_msgs std_msgs rslidar_msg) +if(rclcpp_FOUND) + + ament_target_dependencies(rslidar_sdk_node + rclcpp + std_msgs + sensor_msgs + rslidar_msg) + install(TARGETS rslidar_sdk_node - DESTINATION lib/${PROJECT_NAME} - ) + DESTINATION lib/${PROJECT_NAME}) + install(DIRECTORY launch rviz - DESTINATION share/${PROJECT_NAME} - ) + DESTINATION share/${PROJECT_NAME}) + ament_package() -endif(rclcpp_FOUND AND ${COMPILE_METHOD} STREQUAL "COLCON") + +endif(rclcpp_FOUND) + diff --git a/README.md b/README.md index 7feb51a6..98106616 100644 --- a/README.md +++ b/README.md @@ -1,31 +1,53 @@ -# **rslidar_sdk** +# 1 **rslidar_sdk** [中文介绍](README_CN.md) + + ## 1 Introduction -**rslidar_sdk** is the lidar driver software development kit running on Ubuntu operating system, which contains the lidar driver core, ROS support, ROS2 support and Protobuf-UDP communication functions. For users who want to get point cloud through ROS or ROS2, this software development kit can be used directly. For those who want to do advanced development or integrate the lidar driver into their own projects, please refer to the lidar driver core [rs_driver](https://github.com/RoboSense-LiDAR/rs_driver). +**rslidar_sdk** is the Software Development Kit of the RoboSense Lidar based on Ubuntu. It contains: + ++ The lidar driver core [rs_driver](https://github.com/RoboSense-LiDAR/rs_driver), ++ The ROS support, ++ The ROS2 support, + +To get point cloud through ROS/ROS2, please just use this SDK. + +To integrate the Lidar driver into your own projects, please use the rs_driver. -### **1.1 LiDAR Support** +### 1.1 LiDAR Supported -- RS16 -- RS32 -- RSBP -- RS128 -- RS80 -- RSM1-B3 -- RSHELIOS +- RS-LiDAR-16 +- RS-LiDAR-32 +- RS-Bpearl +- RS-Helios +- RS-Helios-16P +- RS-Ruby-128 +- RS-Ruby-80 +- RS-Ruby-48 +- RS-Ruby-Plus-128 +- RS-Ruby-Plus-80 +- RS-Ruby-Plus-48 +- RS-LiDAR-M1 +- RS-LiDAR-M2 +- RS-LiDAR-M3 +- RS-LiDAR-E1 +- RS-LiDAR-MX -### 1.2 Point type support +### 1.2 Point Type Supported - XYZI - x, y, z, intensity - XYZIRT - x, y, z, intensity, ring, timestamp + + ## 2 Download ### 2.1 Download via Git -Since rslidar_sdk project includes a submodule --- rs_driver, user needs to run the following commands after git clone to download the submodule properly. +Download the rslidar_sdk as below. Since it contains the submodule rs_driver, please also use `git submodule` to download the submodule properly. + ```sh git clone https://github.com/RoboSense-LiDAR/rslidar_sdk.git @@ -36,41 +58,45 @@ git submodule update ### 2.2 Download directly -Instead of using Git, user can also access [rslidar_sdk_release](https://github.com/RoboSense-LiDAR/rslidar_sdk/releases) to download the latest version of rslidar_sdk. Please download the **rslidar_sdk.tar.gz** archive instead of Source code because the Source code zip file does not contain the submodule(rs_driver), which should be downloaded manaully. +Instead of using Git, user can also access [rslidar_sdk_release](https://github.com/RoboSense-LiDAR/rslidar_sdk/releases) to download the latest version of rslidar_sdk. + +Please download the **rslidar_sdk.tar.gz** archive instead of Source code. The Source code zip file does not contain the submodule rs_driver, so it has to be downloaded manaully. +![](./img/01_01_download_page.png) + -![](doc/img/download_page.png) ## 3 Dependencies ### 3.1 ROS -To run rslidar_sdk in ROS environment, ROS related libraries need to be installed. +To run rslidar_sdk in the ROS environment, please install below libraries. ++ Ubuntu 16.04 - ROS Kinetic desktop ++ Ubuntu 18.04 - ROS Melodic desktop ++ Ubuntu 20.04 - ROS Noetic desktop -**Ubuntu 16.04**: ros-kinetic-desktop-full +For installation, please refer to http://wiki.ros.org. -**Ubuntu 18.04**: ros-melodic-desktop-full +**It's highly recommanded to install ros-distro-desktop-full**. If you do so, the corresponding libraries, such as PCL, will be installed at the same time. -**Installation**: please refer to http://wiki.ros.org - -If you install ros-kinetic-desktop-full or ros-melodic-desktop-full， the corresponding PCL and Boost will be installed at the same time. It will bring you a lot of convenience since you don't need to handle the version confliction. Thus, **it's highly recommanded to install ros-distro-desktop-full**. +This brings a lot of convenience, since you don't have to handle version conflict. ### 3.2 ROS2 -If use rslidar_sdk in ROS2 environment, ROS2 related libraries need to be installed. - -**Ubuntu 16.04**: Not supportted - -**Ubuntu 18.04**: ROS2 eloquent desktop +To use rslidar_sdk in the ROS2 environment, please install below libraries. ++ Ubuntu 16.04 - Not supported ++ Ubuntu 18.04 - ROS2 Eloquent desktop ++ Ubuntu 20.04 - ROS2 Galactic desktop ++ Ubuntu 22.04 - ROS2 Humble desktop -**Installation**: please refer to https://index.ros.org/doc/ros2/Installation/Eloquent/Linux-Install-Debians/ +For installation, please refer to https://index.ros.org/doc/ros2/Installation/Eloquent/Linux-Install-Debians/ -**Note! Please avoid installing ROS and ROS2 on the same computer! This may cause conflict! Also you may need to install Yaml manually.** +**Please do not install ROS and ROS2 on the same computer, to avoid possible conflict and manually install some libraries, such as Yaml.** -### 3.3 Yaml(Essential) +### 3.3 Yaml (Essential) version: >= v0.5.2 -*If ros-distro-desktop-full is installed, this section can be skipped* +*If ros-distro-desktop-full is installed, this step can be skipped* Installation: @@ -79,61 +105,26 @@ sudo apt-get update sudo apt-get install -y libyaml-cpp-dev ``` -### 3.4 Pcap(Essential) +### 3.4 libpcap (Essential) -version: >=v1.7.4 +version: >= v1.7.4 -Installation： +Installation: ```sh sudo apt-get install -y libpcap-dev ``` -### 3.5 Protobuf(Optional) - -version:>=v2.6.1 - -Installation : - -```sh -sudo apt-get install -y libprotobuf-dev protobuf-compiler -``` - ## 4 Compile & Run -We offer three ways to compile and run the driver. -### 4.1 Compile directly +### 4.1 Compile with ROS catkin tools - In this way, user can use ROS facilities(not ROS2) if ROS master node is launched in advance via ```roscore```. Also, rviz can be launched seperately for visualization. +(1) Create a new workspace folder, and create a *src* folder in it. Then put the rslidar_sdk project into the *src* folder. -```sh -cd rslidar_sdk -mkdir build && cd build -cmake .. && make -j4 -./rslidar_sdk_node -``` - - - -### 4.2 Compile with ROS catkin tools - -(1) Open the *CMakeLists.txt* in the project，modify the line on top of the file **set(COMPILE_METHOD ORIGINAL)** to **set(COMPILE_METHOD CATKIN)**. - -```cmake -#======================================= -# Compile setup (ORIGINAL,CATKIN,COLCON) -#======================================= -set(COMPILE_METHOD CATKIN) -``` - -(2) Rename the file *package_ros1.xml* in the rslidar_sdk to *package.xml* - -(3) Create a new workspace folder, and create a *src* folder in it. Then put the rslidar_sdk project into the *src* folder. - -(4) Go back to the root of workspace, run the following commands to compile and run. (if using zsh, replace the 2nd command with *source devel/setup.zsh*). +(2) Go back to the root of workspace, run the following commands to compile and run. (if using zsh, replace the 2nd command with *source devel/setup.zsh*). ```sh catkin_make @@ -141,63 +132,54 @@ source devel/setup.bash roslaunch rslidar_sdk start.launch ``` -### 4.3 Compile with ROS2-colcon - -(1) Open the *CMakeLists.txt* in the project，modify the line on top of the file **set(COMPILE_METHOD ORIGINAL)** to **set(COMPILE_METHOD COLCON)**. - -```cmake -#======================================= -# Compile setup (ORIGINAL,CATKIN,COLCON) -#======================================= -set(COMPILE_METHOD COLCON) -``` - -(2) Rename the file *package_ros2.xml* in the rslidar_sdk to *package.xml* - -(3) Create a new workspace folder, and create a *src* folder in it. Then put the rslidar_sdk project in the *src* folder. +### 4.2 Compile with ROS2 colcon -(4) Download the packet definition project in ROS2 through [link](https://github.com/RoboSense-LiDAR/rslidar_msg), then put the project rslidar_msg in the *src* folder you just created. +(1) Create a new workspace folder, and create a *src* folder in it. Then put the rslidar_sdk project in the *src* folder. -(5) Go back to the root of workspace, run the following commands to compile and run. (if using zsh, replace the 2nd command with *source install/setup.zsh*). +(2) Download the packet definition project in ROS2 through [link](https://github.com/RoboSense-LiDAR/rslidar_msg), then put the project rslidar_msg in the *src* folder you just created. +(3) Go back to the root of workspace, run the following commands to compile and run. (if using zsh, replace the 2nd command with *source install/setup.zsh*). ```sh colcon build source install/setup.bash ros2 launch rslidar_sdk start.py ``` +Another version of start.py may be used, since it is different on different versios of ROS2. For example, elequent_start.py is used instead for ROS2 elequent. + ## 5 Introduction to parameters -This section is very important since all functions and features provided by rslidar_sdk is configured via parameter modification. So please read the following links carefully. +To change behaviors of rslidar_sdk, change its parameters. please read the following links for detail information. -[Intro to parameters](doc/intro/parameter_intro.md) +[Intro to parameters](doc/intro/02_parameter_intro.md) -[Intro to hidden parameters](doc/intro/hiding_parameters_intro.md) +[Intro to hidden parameters](doc/intro/03_hiding_parameters_intro.md) ## 6 Quick start -The following documents are some quick guides for using some of the most common features of the rslidar_sdk. The rslidar_sdk are not limited to the following modes of operation and users can use rslidar_sdk in their own way by modifying parameters. +Below are some quick guides to use rslidar_sdk. + +[Connect to online LiDAR and send point cloud through ROS](doc/howto/06_how_to_decode_online_lidar.md) + +[Decode PCAP file and send point cloud through ROS](doc/howto/08_how_to_decode_pcap_file.md) -[Online connect lidar and send point cloud through ROS](doc/howto/how_to_online_send_point_cloud_ros.md) +[Change Point Type](doc/howto/05_how_to_change_point_type.md) -[Record rosbag & Offline decode rosbag](doc/howto/how_to_record_and_offline_decode_rosbag.md) -[Decode pcap bag and send point cloud through ROS](doc/howto/how_to_offline_decode_pcap.md) +## 7 Advanced Topics +[Online Lidar - Advanced topics](doc/howto/07_online_lidar_advanced_topics.md) -## 7 Advanced +[PCAP file - Advanced topics](doc/howto/09_pcap_file_advanced_topics.md) -[Send & Receive via Protobuf](doc/howto/how_to_use_protobuf_function.md) +[Coordinate Transformation](doc/howto/10_how_to_use_coordinate_transformation.md) -[Multi-LiDARs](doc/howto/how_to_use_multi_lidars.md) +[Record rosbag & Replay it](doc/howto/11_how_to_record_replay_packet_rosbag.md) -[Switch Point Type](doc/howto/how_to_switch_point_type.md) -[Coordinate Transformation](doc/howto/how_to_use_coordinate_transformation.md) -[Multi-Cast](doc/howto/how_to_use_multi_cast_function.md) \ No newline at end of file diff --git a/README_CN.md b/README_CN.md index 3f281581..7ac9a82b 100644 --- a/README_CN.md +++ b/README_CN.md @@ -1,28 +1,51 @@ -# **rslidar_sdk** +# 1 **rslidar_sdk** -## 1 工程简介 - **rslidar_sdk** 为速腾聚创在Ubuntu环境下的雷达驱动软件包，包括了雷达驱动内核， ROS拓展功能，ROS2拓展功能，Protobuf-UDP通信拓展功能。对于没有二次开发需求的用户，或是想直接使用ROS或ROS2进行二次开发的用户，可直接使用本软件包， 配合ROS或ROS2自带的RVIZ可视化工具即可查看点云。 对于有更深一步二次开发需求，想将雷达驱动集成到自己工程内的客户， 请参考雷达驱动内核的相关文档，直接使用内核[rs_driver](https://github.com/RoboSense-LiDAR/rs_driver)进行二次开发。 +[English Version](README.md) -### **1.1 雷达型号支持** -- RS16 -- RS32 -- RSBP -- RS128 -- RS80 -- RSM1-B3 -- RSHELIOS -### 1.2 点的类型支持 +## 1.1 工程简介 + + **rslidar_sdk** 是速腾聚创在Ubuntu环境下的雷达驱动软件包。它包括： + + 雷达驱动内核[rs_driver](https://github.com/RoboSense-LiDAR/rs_driver)， + + ROS拓展功能， + + ROS2拓展功能 + +如果希望基于ROS/ROS2进行二次开发，可以使用本软件包。配合ROS/ROS2自带的可视化工具rviz，可以查看点云。 + +如果希望将雷达驱动集成到自己的工程，作更深一步的二次开发，请基于rs_driver进行开发。 + +### 1.1.1 支持的雷达型号 + +- RS-LiDAR-16 +- RS-LiDAR-32 +- RS-Bpearl +- RS-Helios +- RS-Helios-16P +- RS-Ruby-128 +- RS-Ruby-80 +- RS-Ruby-48 +- RS-Ruby-Plus-128 +- RS-Ruby-Plus-80 +- RS-Ruby-Plus-48 +- RS-LiDAR-M1 +- RS-LiDAR-M2 +- RS-LiDAR-M3 +- RS-LiDAR-E1 +- RS-LiDAR-MX + +### 1.1.2 支持的点类型 - XYZI - x, y, z, intensity - XYZIRT - x, y, z, intensity, ring, timestamp -## 2 下载 -### 2.1 使用git clone - 由于rslidar_sdk项目中包含子模块驱动内核rs_driver, 因此在执行git clone 后还需要执行相关指令初始化并更新子模块。 +## 1.2 下载 + +### 1.2.1 使用 git clone + +rslidar_sdk项目包含子模块驱动内核rs_driver。在执行git clone后，还需要执行相关指令，初始化并更新子模块。 ```sh git clone https://github.com/RoboSense-LiDAR/rslidar_sdk.git @@ -31,106 +54,72 @@ git submodule init git submodule update ``` -### 2.2 直接下载 - -用户可以直接访问 [rslidar_sdk_release](https://github.com/RoboSense-LiDAR/rslidar_sdk/releases) 下载最新版本的rslidar_sdk. 请下载 **rslidar_sdk.tar.gz** 压缩包， 不要下载Source code。 因为Source code压缩包内不包含子模块rs_driver的代码， 用户还需自行下载rs_driver的代码放入其中才行。 +### 1.2.2 直接下载 -![](doc/img/download_page.png) +用户可以直接访问 [rslidar_sdk_release](https://github.com/RoboSense-LiDAR/rslidar_sdk/releases) 下载最新版本的rslidar_sdk. +请下载 **rslidar_sdk.tar.gz** 压缩包，不要下载Source code。因为Source code压缩包内不包含子模块rs_driver的代码， 用户还需自行下载rs_driver的代码放入其中才行。 +![](./img/01_01_download_page.png) -## 3 依赖介绍 -### 3.1 ROS +## 1.3 依赖介绍 -*若需在ROS环境下使用雷达驱动，则需安装ROS相关依赖库* +### 1.3.1 ROS -Ubuntu 16.04 - ROS kinetic desktop-full +在ROS环境下使用雷达驱动，需要安装ROS相关依赖库。 ++ Ubuntu 16.04 - ROS Kinetic desktop ++ Ubuntu 18.04 - ROS Melodic desktop ++ Ubuntu 20.04 - ROS Noetic desktop -Ubuntu 18.04 - ROS melodic desktop-full +安装方法请参考 http://wiki.ros.org。 -安装方式： 参考 http://wiki.ros.org +**强烈建议安装ROS desktop-full版。这个过程会自动安装一些兼容版本的依赖库，如PCL库等。这样可以避免花大量时间，去逐个安装和配置它们**。 -**如果安装了ROS kinetic desktop-full版或ROS melodic desktop-full版，那么兼容版本其他依赖库也应该同时被安装了，所以不需要重新安装它们以避免多个版本冲突引起的问题, 因此，强烈建议安装desktop-full版，这将节省大量的时间来逐个安装和配置库**。 +### 1.3.2 ROS2 -### 3.2 ROS2 +在ROS2环境下使用雷达驱动，需要安装ROS2相关依赖库。 ++ Ubuntu 16.04 - 不支持 ++ Ubuntu 18.04 - ROS2 Eloquent desktop ++ Ubuntu 20.04 - ROS2 Galactic desktop ++ Ubuntu 22.04 - ROS2 Humble desktop -*若需在ROS2环境下使用雷达驱动，则需安装ROS2相关依赖库* +安装方法请参考 https://index.ros.org/doc/ros2/Installation/Eloquent/Linux-Install-Debians/ -Ubuntu 16.04 - 不支持 +**请不要在一台电脑上同时安装ROS和ROS2，以避免可能的版本冲突，和手工安装其他库（如Yaml）的麻烦。** -Ubuntu 18.04 - ROS2 Eloquent desktop - -安装方式：参考 https://index.ros.org/doc/ros2/Installation/Eloquent/Linux-Install-Debians/ - -**注意！ 请避免在同一台电脑上同时安装ROS和ROS2， 这可能会产生冲突！ 同时还需要手动安装Yaml库。** - -### 3.3 Yaml (必需) +### 1.3.3 Yaml (必需) 版本号: >= v0.5.2 *若已安装ROS desktop-full, 可跳过* -安装方式: +安装方法如下: ```sh sudo apt-get update sudo apt-get install -y libyaml-cpp-dev ``` -### 3.4 Pcap (必需) +### 1.3.4 libpcap (必需) -版本号： >=v1.7.4 +版本号： >= v1.7.4 -安装方式： +安装方法如下： ```sh sudo apt-get install -y libpcap-dev ``` -### 3.5 Protobuf (可选) - -版本号： >=v2.6.1 - -安装方式: - -```sh -sudo apt-get install -y libprotobuf-dev protobuf-compiler -``` - -## 4 编译 & 运行 +## 1.4 编译、运行 -我们提供三种编译&运行方式。 +### 1.4.1 依赖于ROS-catkin编译 -### 4.1 直接编译 +(1) 新建一个文件夹作为工作空间，然后再新建一个名为*src*的文件夹, 将rslidar_sdk工程放入*src*文件夹内。 -按照如下指令即可编译运行程序。 直接编译也可以使用ROS相关功能(不包括ROS2)，但需要在程序启动前**手动启动roscore**，启动后**手动打开rviz**才能看到可视化点云结果。 - -```sh -cd rslidar_sdk -mkdir build && cd build -cmake .. && make -j4 -./rslidar_sdk_node -``` - -### 4.2 依赖于ROS-catkin编译 - -(1) 打开工程内的*CMakeLists.txt*文件，将文件顶部的**set(COMPILE_METHOD ORIGINAL)**改为**set(COMPILE_METHOD CATKIN)**。 - -```cmake -#======================================= -# Compile setup (ORIGINAL,CATKIN,COLCON) -#======================================= -set(COMPILE_METHOD CATKIN) -``` - -(2) 将rslidar_sdk工程目录下的*package_ros1.xml*文件重命名为*package.xml*。 - -(3) 新建一个文件夹作为工作空间，然后再新建一个名为*src*的文件夹, 将rslidar_sdk工程放入*src*文件夹内。 - -(4) 返回工作空间目录，执行以下命令即可编译&运行(若使用.zsh,将第二句指令替换为 *source devel/setup.zsh*)。 +(2) 返回工作空间目录，执行以下命令即可编译&运行(若使用.zsh,将第二句指令替换为 *source devel/setup.zsh*)。 ```sh catkin_make @@ -138,24 +127,13 @@ source devel/setup.bash roslaunch rslidar_sdk start.launch ``` -### 4.3 依赖于ROS2-colcon编译 - -(1) 打开工程内的*CMakeLists.txt*文件，将文件顶部的**set(COMPILE_METHOD ORIGINAL)**改为**set(COMPILE_METHOD COLCON)**。 - -```cmake -#======================================= -# Compile setup (ORIGINAL,CATKIN,COLCON) -#======================================= -set(COMPILE_METHOD COLCON) -``` - -(2) 将rslidar_sdk工程目录下的*package_ros2.xml*文件重命名为*package.xml*。 +### 1.4.2 依赖于ROS2-colcon编译 -(3) 新建一个文件夹作为工作空间，然后再新建一个名为*src*的文件夹, 将rslidar_sdk工程放入*src*文件夹内。 +(1) 新建一个文件夹作为工作空间，然后再新建一个名为*src*的文件夹, 将rslidar_sdk工程放入*src*文件夹内。 -(4) 通过[链接](https://github.com/RoboSense-LiDAR/rslidar_msg)下载ROS2环境下的雷达packet消息定义， 将rslidar_msg工程也放在刚刚新建的*src*文件夹内，与rslidar_sdk并列。 +(2) 通过[链接](https://github.com/RoboSense-LiDAR/rslidar_msg)，下载ROS2环境下的雷达packet消息定义，将rslidar_msg工程也放在刚刚新建的*src*文件夹内，与rslidar_sdk并列。 -(5) 返回工作空间目录，执行以下命令即可编译&运行(若使用.zsh,将第二句指令替换为 *source install/setup.zsh*)。 +(3) 返回工作空间目录，执行以下命令即可编译、运行。如果使用.zsh，将第二行替换为*source install/setup.zsh*。 ```sh colcon build @@ -163,38 +141,39 @@ source install/setup.bash ros2 launch rslidar_sdk start.py ``` +不同ROS2版本start.py的格式可能不同，请使用对应版本的start.py。如ROS2 Elequent，请使用elequent_start.py。 + -## 5 参数介绍 +## 1.5 参数介绍 -参数介绍非常重要，请仔细阅读。 本软件包的所有功能都将通过配置参数文件来实现。 +rslidar_sdk的功能通过配置参数文件来实现，请仔细阅读。 -[参数介绍](doc/intro/parameter_intro_cn.md) +[参数介绍](doc/intro/02_parameter_intro_CN.md) -[隐藏参数介绍](doc/intro/hiding_parameters_intro_cn.md) +[隐藏参数介绍](doc/intro/03_hiding_parameters_intro_CN.md) -## 6 快速上手 +## 1.6 快速上手 -以下仅为一些常用功能的快速使用指南， 实际使用时并不仅限于以下几种工作模式， 可通过配置参数改变不同的工作模式。 +以下是一些常用功能的使用指南。 -[在线读取雷达数据发送到ROS](doc/howto/how_to_online_send_point_cloud_ros_cn.md) +[连接在线雷达数据并发送点云到ROS](doc/howto/06_how_to_decode_online_lidar_CN.md) -[录制ROS数据包&离线解析ROS数据包](doc/howto/how_to_record_and_offline_decode_rosbag_cn.md) +[解析PCAP包并发送点云到ROS](doc/howto/08_how_to_decode_pcap_file_CN.md) -[离线解析Pcap包发送到ROS](doc/howto/how_to_offline_decode_pcap_cn.md) +[切换点类型](doc/howto/05_how_to_change_point_type_CN.md) -## 7 使用进阶 +## 1.7 使用进阶 -[使用Protobuf发送&接收](doc/howto/how_to_use_protobuf_function_cn.md) +[在线雷达-高级主题](doc/howto/07_online_lidar_advanced_topics_CN.md) -[多雷达](doc/howto/how_to_use_multi_lidars_cn.md) +[PCAP文件-高级主题](doc/howto/09_pcap_file_advanced_topics_CN.md) -[切换点的类型](doc/howto/how_to_switch_point_type_cn.md) +[点云坐标变换](doc/howto/10_how_to_use_coordinate_transformation_CN.md) -[坐标变换功能](doc/howto/how_to_use_coordinate_transformation_cn.md) +[录制ROS数据包然后播放它](doc/howto/11_how_to_record_replay_packet_rosbag_CN.md) -[组播模式](doc/howto/how_to_use_multi_cast_function_cn.md) \ No newline at end of file diff --git a/config/config.yaml b/config/config.yaml index 739d893e..6757335d 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -3,39 +3,24 @@ common: #1: packet message comes from online Lidar #2: packet message comes from ROS or ROS2 #3: packet message comes from Pcap file - #4: packet message comes from Protobuf-UDP - #5: point cloud comes from Protobuf-UDP send_packet_ros: false #true: Send packets through ROS or ROS2(Used to record packet) send_point_cloud_ros: true #true: Send point cloud through ROS or ROS2 - send_packet_proto: false #true: Send packets through Protobuf-UDP - send_point_cloud_proto: false #true: Send point cloud through Protobuf-UDP - pcap_path: /home/robosense/lidar.pcap #The path of pcap file - lidar: - driver: - lidar_type: RS128 #LiDAR type - RS16, RS32, RSBP, RS128, RS80, RSM1, RSHELIOS - frame_id: /rslidar #Frame id of message + lidar_type: RSHELIOS_16P #LiDAR type - RS16, RS32, RSBP, RSHELIOS, RSHELIOS_16P, RS128, RS80, RS48, RSP128, RSP80, RSP48, + # RSM1, RSM1_JUMBO, RSM2, RSM3, RSE1, RSMX. msop_port: 6699 #Msop port of lidar difop_port: 7788 #Difop port of lidar start_angle: 0 #Start angle of point cloud end_angle: 360 #End angle of point cloud + wait_for_difop: true min_distance: 0.2 #Minimum distance of point cloud max_distance: 200 #Maximum distance of point cloud use_lidar_clock: false #True--Use the lidar clock as the message timestamp - #False-- Use the system clock as the timestamp + #False-- Use the system clock as the timestamp + pcap_path: /home/robosense/lidar.pcap #The path of pcap file ros: + ros_frame_id: rslidar #Frame id of packet message and point cloud message ros_recv_packet_topic: /rslidar_packets #Topic used to receive lidar packets from ROS ros_send_packet_topic: /rslidar_packets #Topic used to send lidar packets through ROS ros_send_point_cloud_topic: /rslidar_points #Topic used to send point cloud through ROS - proto: - point_cloud_recv_port: 60021 #Port number used for receiving point cloud - point_cloud_send_port: 60021 #Port number which the point cloud will be send to - msop_recv_port: 60022 #Port number used for receiving lidar msop packets - msop_send_port: 60022 #Port number which the msop packets will be send to - difop_recv_port: 60023 #Port number used for receiving lidar difop packets - difop_send_port: 60023 #Port number which the difop packets will be send to - point_cloud_send_ip: 127.0.0.1 #Ip address which the point cloud will be send to - packet_send_ip: 127.0.0.1 #Ip address which the lidar packets will be send to - - - diff --git a/create_debian.sh b/create_debian.sh new file mode 100755 index 00000000..d432b89a --- /dev/null +++ b/create_debian.sh @@ -0,0 +1,47 @@ +#! /usr/bin/env bash + +## install the python-bloom fakeroot + +function install_pkg() +{ + pkg_found=$(dpkg -l | grep ${1}) + if [[ $? -eq 0 ]] + then + echo "${1} already installed." + else + echo "Install ${1} ..." + sudo apt-get install ${1} -y + fi +} + +install_pkg python-bloom +install_pkg fakeroot + +echo -e "\n\033[1;32m ~~ (1). Delete old debian folders in the directory...\033[0m" +rm -rf debian/ obj-x86_64-linux-gnu/ + +echo -e "\n\033[1;32m ~~ (2). Delete any backup files...\033[0m" +find . -type f -name '*~' -delete + +echo -e "\n\033[1;32m ~~ (3). Create debian packages...\033[0m\n" +bloom-generate rosdebian --os-name ubuntu --os-version `echo $(lsb_release -sc)` --ros-distro `echo ${ROS_DISTRO}` + +# sed 's#dh_shlibdeps*#dh_shlibdeps --dpkg-shlibdeps-params=--ignore-missing-info#g' debian/rules +## when dpkg-shlibdeps: error: no dependency information found for +## adding bellow code to debian/rules +## +## override_dh_shlibdeps: +## dh_shlibdeps --dpkg-shlibdeps-params=--ignore-missing-info +## It is necessary to insert a TAB instead of spaces before "dh_shlibdeps ..." + +target_string=$(grep "dh_shlibdeps " debian/rules) +replace_string=" dh_shlibdeps --dpkg-shlibdeps-params=--ignore-missing-info" +sed -i "s#${target_string}#${replace_string}#g" debian/rules + +fakeroot debian/rules binary + +echo -e "\n\033[1;32m ~~ (4). Delete old debian folders in the directory...\033[0m" +rm -rf debian/ obj-x86_64-linux-gnu/ + +echo -e "\n\033[1;32m ~~ (5). Delete any backup files...\033[0m" +find . -type f -name '*~' -delete diff --git a/doc/howto/04_how_to_work_along_with_rslidar_sdk_node_v1.3.x_CN.md b/doc/howto/04_how_to_work_along_with_rslidar_sdk_node_v1.3.x_CN.md new file mode 100644 index 00000000..50bf6b61 --- /dev/null +++ b/doc/howto/04_how_to_work_along_with_rslidar_sdk_node_v1.3.x_CN.md @@ -0,0 +1,115 @@ +# 4 如何与rslidar_sdk_node v1.3.x共存？ + + + +## 4.1 问题描述 + +`rslidar_sdk_node` `v1.3.x`和`v1.5.x`的配置方式不同。除了如下两个可能有交互的场景外， 两者各自运行，没有关系。 + ++ `rslidar_sdk_node`在主题`/rslidar_points`下发布点云，rosbag订阅并录制到一个cloud rosbag文件。后面rosbag又会回放这个文件，发布到`/rslidar_points`，`rslidar_sdk_node`订阅并播放它。 + ++ `rslidar_sdk_node`在主题`/rslidar_packets`下发布原始的`MSOP/DIFOP Packet`，rosbag订阅并录制到一个packet rosbag文件。后面rosbag又会回放这个文件到`/rslidar_packets`，`rslidar_sdk_node`订阅并播放它。 + +第一种场景下，`v1.3.x`和`v1.5.x`发布的点云格式相同，所以`v1.3.x`录制的点云，在`v1.5.x`上播放是没有问题的。 + +第二种场景下，`v1.3.x`将MSOP/DIFOP Packet分别发布在两个主题`/rslidar_packets`和`/rslidar_packets_difop`下，而`v1.5.x`将MSOP/DIFOP Packet发布在单个主题`/rslidar_packets`下，而且`v1.3.x`和`v1.5.x`的消息定义也不同，所以`v1.3.x`录制的packet rosbag在`v1.5.x`上不能播放。ROS会检测出这两种格式的MD5 Checksum不匹配并报错。 + +本文说明如何配置`rslidar_sdk` `v1.5.x`，让它在第二种场景下可以同时播放`v1.3.x`和`v1.5.x`的packet rosbag。 + + + +## 4.2 场景说明 + +场景说明如下。 ++ 2个雷达，`Lidar1`是运行`v1.3.x`的雷达，`Lidar2`是运行`v1.5.x`的雷达。 ++ 1台主机，用于分析`Lidar1`和`Lidar2`的数据。 + +![](./img/04_01_packet_rosbag.png) + +## 4.3 步骤 + + +### 4.3.1 配置 v1.3.x 雷达 + +使用`v1.3.x` `rslidar_sdk_node`录制pacekt rosbag。 + +按照默认的`config.yaml`的配置，消息发布到主题`/rslidar_packets`和`/rslidar_packets_difop`下。 + +``` +common: + msg_source: 1 #0: not use Lidar + #1: packet message comes from online Lidar + #2: packet message comes from ROS or ROS2 + #3: packet message comes from Pcap file + #4: packet message comes from Protobuf-UDP + #5: point cloud comes from Protobuf-UDP + send_packet_ros: true #true: Send packets through ROS or ROS2(Used to record packet) + send_point_cloud_ros: true #true: Send point cloud through ROS or ROS2 +lidar: + - driver: + lidar_type: RSM1 #LiDAR type - RS16, RS32, RSBP, RSHELIOS, RS128, RS80, RSM1 + msop_port: 6699 #Msop port of lidar + difop_port: 7788 #Difop port of lidar + ros: + ros_send_packet_topic: /rslidar_packets #Topic used to send lidar packets through ROS + ros_send_point_cloud_topic: /rslidar_points #Topic used to send point cloud through ROS +``` + +### 4.3.2 配置 v1.5.x 雷达 + +使用`v1.5.6` `rslidar_sdk_node`录制packet rosbag。 + +为了与`v1.3.2`的消息区别，将消息输出到主题`/rslidar_packets_v2`下。 + +``` +common: + msg_source: 1 #0: not use Lidar + #1: packet message comes from online Lidar + #2: packet message comes from ROS or ROS2 + #3: packet message comes from Pcap file + send_packet_ros: true #true: Send packets through ROS or ROS2(Used to record packet) + send_point_cloud_ros: true #true: Send point cloud through ROS or ROS2 +lidar: + - driver: + lidar_type: RSM1 #LiDAR type - RS16, RS32, RSBP, RSHELIOS, RS128, RS80, RS48, RSM1 + msop_port: 6699 #Msop port of lidar + difop_port: 7788 #Difop port of lidar + ros: + ros_send_packet_topic: /rslidar_packets_v2 #Topic used to send lidar packets through ROS + ros_send_point_cloud_topic: /rslidar_points #Topic used to send point cloud through ROS +``` + + +### 4.3.3 配置 v1.5.x 主机 + ++ 打开CMake编译选项`ENABLE_SOURCE_PACKET_LEGACY=ON`，编译`rslidar_sdk`。 + +``` +# CMakeLists.txt + +option(ENABLE_SOURCE_PACKET_LEGACY "Enable ROS Source of MSOP/DIFOP Packet v1.3.x" ON) +``` + ++ 在`config.yaml`中，增加一个配置项`ros_recv_packet_legacy_topic`: `/rslidar_packets`。这样`rslidar_sdk_node`将同时订阅两个主题。 + + 订阅`/rslidar_packets`和`/rslidar_packets_difop`，读入`v1.3.x`的消息 + + 订阅`/rslidar_packets_v2`，读入`v1.5.x`的消息 + +``` +common: + msg_source: 1 #0: not use Lidar + #1: packet message comes from online Lidar + #2: packet message comes from ROS or ROS2 + #3: packet message comes from Pcap file + send_packet_ros: false #true: Send packets through ROS or ROS2(Used to record packet) + send_point_cloud_ros: true #true: Send point cloud through ROS or ROS2 +lidar: + - driver: + lidar_type: RSM1 #LiDAR type - RS16, RS32, RSBP, RSHELIOS, RS128, RS80, RS48, RSM1 + msop_port: 6699 #Msop port of lidar + difop_port: 7788 #Difop port of lidar + ros: + ros_recv_packet_legacy_topic: /rslidar_packets #Topic used to receive lidar packets from ROS + ros_recv_packet_topic: /rslidar_packets_v2 #Topic used to receive lidar packets from ROS + ros_send_point_cloud_topic: /rslidar_points #Topic used to send point cloud through ROS +``` + diff --git a/doc/howto/05_how_to_change_point_type.md b/doc/howto/05_how_to_change_point_type.md new file mode 100644 index 00000000..5dd6924a --- /dev/null +++ b/doc/howto/05_how_to_change_point_type.md @@ -0,0 +1,89 @@ +# 5 How to change point type + + + +## 5.1 Introduction + +This document illustrates how to change the point type. + +In ```CMakeLists.txt``` of the project, change the variable `POINT_TYPE`. Remember to **rebuild** the project after changing it. + +```cmake +#======================================= +# Custom Point Type (XYZI, XYZIRT) +#======================================= +set(POINT_TYPE XYZI) +``` + + + +## 5.2 XYZI + +If `POINT_TYPE` is `XYZI`, rslidar_sdk uses the RoboSense defined type as below. + +```c++ +struct PointXYZI +{ + float x; + float y; + float z; + uint8_t intensity; +}; +``` + +rslidar_sdk transforms point cloud of `PointXYZI` to ROS message of `PointCloud2`，and publish it. + +```c++ + sensor_msgs::PointCloud2 ros_msg; + addPointField(ros_msg, "x", 1, sensor_msgs::PointField::FLOAT32, offset); + addPointField(ros_msg, "y", 1, sensor_msgs::PointField::FLOAT32, offset); + addPointField(ros_msg, "z", 1, sensor_msgs::PointField::FLOAT32, offset); + addPointField(ros_msg, "intensity", 1, sensor_msgs::PointField::FLOAT32, offset); + + // + // copy points from point cloud of `PointXYZI` to `PointCloud2` + // + ... + +``` + +Here `intensity` of `PointCloud2` is `float` type, not `uint8_t`. This is because most ROS based applications require `intensity` of `float` type. + + + +## 5.3 XYZIRT + +If `POINT_TYPE` is `XYZIRT`, rslidar_sdk uses the RoboSense defined type as below. + +```c++ +struct PointXYZIRT +{ + float x; + float y; + float z; + uint8_t intensity; + uint16_t ring; + double timestamp; +}; +``` + +rslidar_sdk transforms point cloud of `PointXYZIRT` to ROS message of `PointCloud2`，and publish it. + +```c++ + sensor_msgs::PointCloud2 ros_msg; + addPointField(ros_msg, "x", 1, sensor_msgs::PointField::FLOAT32, offset); + addPointField(ros_msg, "y", 1, sensor_msgs::PointField::FLOAT32, offset); + addPointField(ros_msg, "z", 1, sensor_msgs::PointField::FLOAT32, offset); + addPointField(ros_msg, "intensity", 1, sensor_msgs::PointField::FLOAT32, offset); +#ifdef POINT_TYPE_XYZIRT + sensor_msgs::PointCloud2Iterator iter_ring_(ros_msg, "ring"); + sensor_msgs::PointCloud2Iterator iter_timestamp_(ros_msg, "timestamp"); +#endif + + // + // copy points from point cloud of `PointXYZIRT` to `PointCloud2` + // + ... + +``` + diff --git a/doc/howto/05_how_to_change_point_type_CN.md b/doc/howto/05_how_to_change_point_type_CN.md new file mode 100644 index 00000000..08eca6ca --- /dev/null +++ b/doc/howto/05_how_to_change_point_type_CN.md @@ -0,0 +1,87 @@ +# 5 如何改变点类型的定义 + + + +## 5.1 简介 + +本文档介绍如何改变点类型的定义。 + +在项目的```CMakeLists.txt```文件中设置`POINT_TYPE`变量。修改后，需要重新编译整个工程。 + +```cmake +#======================================= +# Custom Point Type (XYZI, XYZIRT) +#======================================= +set(POINT_TYPE XYZI) +``` + + + +## 5.2 XYZI + +`POINT_TYPE`为`XYZI`时，rslidar_sdk使用RoboSense自定义的点类型```PointXYZI```. + +```c++ +struct PointXYZI +{ + float x; + float y; + float z; + uint8_t intensity; +}; +``` + +rslidar_sdk将基于`PointXYZI`的点云，转换为ROS的`PointCloud2`消息，再发布出去。 + +```c++ + sensor_msgs::PointCloud2 ros_msg; + addPointField(ros_msg, "x", 1, sensor_msgs::PointField::FLOAT32, offset); + addPointField(ros_msg, "y", 1, sensor_msgs::PointField::FLOAT32, offset); + addPointField(ros_msg, "z", 1, sensor_msgs::PointField::FLOAT32, offset); + addPointField(ros_msg, "intensity", 1, sensor_msgs::PointField::FLOAT32, offset); + + // + // copy points from point cloud of `PointXYZI` to `PointCloud2` + // + ... +``` + +这里`PointCloud2`中的`intensity`是`float`类型，而不是`uint8_t`类型。这是因为大多数基于ROS的程序都希望`float`类型的`intensity`。 + + + +## 5.3 XYZIRT + +`POINT_TYPE`为`XYZIRT`时，rslidar_sdk使用RoboSense自定义的点类型```PointXYZRT```。 + +```c++ +struct PointXYZIRT +{ + float x; + float y; + float z; + uint8_t intensity; + uint16_t ring; + double timestamp; +}; +``` + +rslidar_sdk将基于`PointXYZIRT`的点云，转换为ROS的PointCloud2消息，再发布出去。 + +```c++ + sensor_msgs::PointCloud2 ros_msg; + addPointField(ros_msg, "x", 1, sensor_msgs::PointField::FLOAT32, offset); + addPointField(ros_msg, "y", 1, sensor_msgs::PointField::FLOAT32, offset); + addPointField(ros_msg, "z", 1, sensor_msgs::PointField::FLOAT32, offset); + addPointField(ros_msg, "intensity", 1, sensor_msgs::PointField::FLOAT32, offset); +#ifdef POINT_TYPE_XYZIRT + sensor_msgs::PointCloud2Iterator iter_ring_(ros_msg, "ring"); + sensor_msgs::PointCloud2Iterator iter_timestamp_(ros_msg, "timestamp"); +#endif + + // + // copy points from point cloud of `PointXYZIRT` to `PointCloud2` + // + ... +``` + diff --git a/doc/howto/06_how_to_decode_online_lidar.md b/doc/howto/06_how_to_decode_online_lidar.md new file mode 100644 index 00000000..905afca1 --- /dev/null +++ b/doc/howto/06_how_to_decode_online_lidar.md @@ -0,0 +1,70 @@ +# 6 How to decode on-line LiDAR + +## 6.1 Introduction + +This document illustrates how to connect to an on-line LiDAR, and send point cloud to ROS. + +Please make sure you have read the LiDAR user-guide and [Intro to parameters](../intro/02_parameter_intro.md) before reading this document. + + + +## 6.2 Steps + +### 6.2.1 Get the LiDAR port number + +Please follow the instructions in LiDAR user-guide, to connect the LiDAR, and set up your computer's ip address. + +Please check the LiDAR user-guide, or use the 3rd-party tool(such as WireShark), to get your LiDAR's MSOP port number and DIFOP port number. The default values are ```msop-6699, difop-7788```. + +### 6.2.2 Set up the configuration file + +#### 6.2.2.1 common part + +```yaml +common: + msg_source: 1 + send_packet_ros: false + send_point_cloud_ros: true + send_packet_proto: false + send_point_cloud_proto: false +``` + +The message come from the LiDAR, so set ```msg_source = 1```. + +Send point cloud to ROS, so set ```send_point_cloud_ros = true```. + +#### 6.2.2.2 lidar-driver part + +```yaml +lidar: + - driver: + lidar_type: RS128 + msop_port: 6699 + difop_port: 7788 + start_angle: 0 + end_angle: 360 + min_distance: 0.2 + max_distance: 200 + use_lidar_clock: false +``` + +Set the ```lidar_type``` to your LiDAR type. + +Set the ```msop_port``` and ```difop_port``` to your LiDAR's port number. + +#### 6.2.2.3 lidar-ros part + +```yaml +ros: + ros_frame_id: rslidar + ros_recv_packet_topic: /rslidar_packets + ros_send_packet_topic: /rslidar_packets + ros_send_point_cloud_topic: /rslidar_points +``` + +Set the ```ros_send_point_cloud_topic``` to the topic you want to send to. + +### 6.2.3 Run + +Run the program. + diff --git a/doc/howto/how_to_online_send_point_cloud_ros_cn.md b/doc/howto/06_how_to_decode_online_lidar_CN.md similarity index 51% rename from doc/howto/how_to_online_send_point_cloud_ros_cn.md rename to doc/howto/06_how_to_decode_online_lidar_CN.md index 0b846b89..6dda0f5b 100644 --- a/doc/howto/how_to_online_send_point_cloud_ros_cn.md +++ b/doc/howto/06_how_to_decode_online_lidar_CN.md @@ -1,16 +1,28 @@ -# 如何在线连接雷达并发送点云数据到ROS +# 6 如何连接在线雷达 -## 1 简介 -本文档描述了如何在线连接雷达并发送点云数据到ROS。在阅读本文档之前， 请确保已经阅读过雷达用户手册和[参数简介](../intro/parameter_intro.md) 。 -## 2 步骤 +## 6.1 简介 -### 2.1 获取数据端口号 +本文档描述如何连接在线雷达，并发送点云数据到ROS。 -首先根据雷达用户手册连接雷达并设置好您的电脑的IP地址。此时应该已知雷达的msop端口号和difop端口号，默认值为```msop-6699, difop-7788```。 如果不清楚上述内容，请查看雷达用户手册。 +在阅读本文档之前， 请确保已经阅读过雷达用户手册和[参数简介](../intro/02_parameter_intro_CN.md) 。 -### 2.2 设置参数文件的common部分 + + +## 6.2 步骤 + +### 6.2.1 获取数据端口号 + +根据雷达用户手册连接雷达, 并设置好您的电脑的IP地址。 + +请参考雷达用户手册，或使用第三方工具（如WireShark等）得到雷达的MSOP端口号和DIFOP端口号。端口的默认值分别为```6699```和```7788```。 + +### 6.2.2 设置参数文件 + +设置参数文件```config.yaml```。 + +#### 6.2.2.1 common部分 ```yaml common: @@ -19,20 +31,18 @@ common: send_point_cloud_ros: true send_packet_proto: false send_point_cloud_proto: false - pcap_path: /home/robosense/lidar.pcap ``` -由于消息来源于在线雷达，因此请设置```msg_source=1```。 +消息来源于在线雷达，因此请设置```msg_source=1```。 -将点云发送到ROS以查看，因此设置 ```send_point_cloud_ros = true``` 。 +将点云发送到ROS以便查看，因此设置 ```send_point_cloud_ros = true``` 。 -### 2.3 设置参数文件的 lidar-driver部分 +#### 6.2.2.2 lidar-driver部分 ```yaml lidar: - driver: lidar_type: RS128 - frame_id: /rslidar msop_port: 6699 difop_port: 7788 start_angle: 0 @@ -44,13 +54,13 @@ lidar: 将 ```lidar_type``` 设置为LiDAR类型 。 - 设置 ```msop_port``` 和 ```difop_port``` 为雷达数据端口号。 -### 2.4设置配置文件的lidar-ros部分 +#### 6.2.2.3 lidar-ros部分 ```yaml ros: + ros_frame_id: rslidar ros_recv_packet_topic: /rslidar_packets ros_send_packet_topic: /rslidar_packets ros_send_point_cloud_topic: /rslidar_points @@ -58,8 +68,7 @@ ros: 将 ```ros_send_point_cloud_topic``` 设置为发送点云的话题。 -### 2.5 运行 +### 6.2.3 运行 运行程序。 - diff --git a/doc/howto/07_online_lidar_advanced_topics.md b/doc/howto/07_online_lidar_advanced_topics.md new file mode 100644 index 00000000..62baf8ec --- /dev/null +++ b/doc/howto/07_online_lidar_advanced_topics.md @@ -0,0 +1,191 @@ +# 7 Online LiDAR - Advanced Topics + + + +## 7.1 Introduction + +The RoboSense LiDAR may work + ++ in unicast/multicast/broadcast mode, ++ with VLAN layer ++ with user layers. ++ Also rslidar_sdk supports multi-LiDARs. + +This document illustrates how to configure rslidar_sdk in each case. + +Before reading this document, please be sure that you have read: ++ LiDAR user-guide ++ [Intro to parameters](../intro/02_parameter_intro.md) ++ [Decode online LiDAR](./06_how_to_decode_online_lidar.md) + + + +## 7.2 Unicast, Multicast and Broadcast + +### 7.2.1 Broadcast mode + +The Lidar sends MSOP/DIFOP packets to the host machine (rslidar_sdk runs on it). For simplicity, the DIFOP port is ommited here. ++ The Lidar sends to `255.255.255.255` : `6699`, and the host binds to port `6699`. +![](./img/07_01_broadcast.png) + +Below is how to configure `config.yaml`. + +```yaml +common: + msg_source: 1 + send_point_cloud_ros: true + +lidar: + - driver: + lidar_type: RS32 + msop_port: 6699 + difop_port: 7788 + ros: + ros_frame_id: rslidar + ros_send_point_cloud_topic: /rslidar_points +``` + +The `common` part and the `lidar-ros` part is listed here. They will be ommited in the following examples, since they are not changed. + +### 7.2.2 Unicast mode + +To reduce the network load, the Lidar is suggested to work in unicast mode. ++ The Lidar sends to `192.168.1.102` : `6699`, and the host binds to port `6699`. +![](./img/07_02_unicast.png) + +Below is how to configure `config.yaml`. In fact, it same with the broadcast mode. + +```yaml +lidar: + - driver: + lidar_type: RS32 + msop_port: 6699 + difop_port: 7788 +``` + +### 7.2.3 Multicast mode + +The Lidar may also works in multicast mode. ++ The lidar sends to `224.1.1.1`:`6699` ++ The host binds to port `6699`. And it makes local NIC (Network Interface Card) join the multicast group `224.1.1.1`. The local NIC's IP is `192.168.1.102`. +![](./img/07_03_multicast.png) + +Below is how to configure `config.yaml`. + +```yaml +lidar: + - driver: + lidar_type: RS32 + msop_port: 6699 + difop_port: 7788 + group_address: 224.1.1.1 + host_address: 192.168.1.102 +``` + + + +## 7.3 Multiple LiDARs + +### 7.3.1 Different remote ports + +If you have two or more Lidars, it is suggested to set different remote ports. ++ First Lidar sends to `192.168.1.102`:`6699`, and the first driver instance binds to `6699`. ++ Second Lidar sends to `192.168.1.102`:`5599`, and the second driver instance binds to `5599`. +![](./img/07_04_multi_lidars_port.png) + +Below is how to configure `config.yaml`. + +```yaml +lidar: + - driver: + lidar_type: RS32 + msop_port: 6699 + difop_port: 7788 + - driver: + lidar_type: RS32 + msop_port: 5599 + difop_port: 6688 +``` + +### 7.3.2 Different remote IPs + +An alternate way is to set different remote IPs. ++ The host has two NICs: `192.168.1.102` and `192.168.1.103`. ++ First Lidar sends to `192.168.1.102`:`6699`, and the first driver instance binds to `192.168.1.102:6699`. ++ Second Lidar sends to `192.168.1.103`:`6699`, and the second driver instance binds to `192.168.1.103:6699`. +![](./img/07_05_multi_lidars_ip.png) + +Below is how to configure `config.yaml`. + +```yaml +lidar: + - driver: + lidar_type: RS32 + msop_port: 6699 + difop_port: 7788 + host_address: 192.168.1.102 + - driver: + lidar_type: RS32 + msop_port: 6699 + difop_port: 7788 + host_address: 192.168.1.103 +``` + + + +## 7.4 VLAN + +In some user cases, The Lidar may work on VLAN. Its packets have a VLAN layer. +![](./img/07_06_vlan_layer.png) + +The driver cannot parse this packet. Instead, it depends on a virtual NIC to strip the VLAN layer. + +Below is an example. ++ The Lidar works on VLAN `80`. It sends packets to `192.168.1.102` : `6699`. The packet has a VLAN layer. ++ Suppose there is a physical NIC `eno1` on the host. It receives packets with VLAN layer. +![](./img/07_07_vlan.png) + +To strip the VLAN layer, create a virtual NIC `eno1.80` on `eno1`, and assign IP `192.168.1.102` to it. + +``` +sudo apt-get install vlan -y +sudo modprobe 8021q + +sudo vconfig add eno1 80 +sudo ifconfig eno1.80 192.168.1.102 up +``` + +Now the driver may take `eno1.80` as a general NIC, and receives packets without VLAN layer. + +```yaml +lidar: + - driver: + lidar_type: RS32 + msop_port: 6699 + difop_port: 7788 +``` + + + +## 7.5 User Layer, Tail Layer + +In some user cases, User may add extra layers before or after the MSOP/DIFOP packet. ++ USER_LAYER is before the packet and TAIL_LAYER is after it. +![](./img/07_08_user_layer.png) + +These extra layers are parts of UDP data. The driver can strip them. + +To strip them, just give their lengths in bytes. + +In the following example, USER_LAYER is 8 bytes, and TAIL_LAYER is 4 bytes. + +```yaml +lidar: + - driver: + lidar_type: RS32 + msop_port: 6699 + difop_port: 7788 + user_layer_bytes: 8 + tail_layer_bytes: 4 +``` + diff --git a/doc/howto/07_online_lidar_advanced_topics_CN.md b/doc/howto/07_online_lidar_advanced_topics_CN.md new file mode 100644 index 00000000..a6ece182 --- /dev/null +++ b/doc/howto/07_online_lidar_advanced_topics_CN.md @@ -0,0 +1,190 @@ +# 7 在线雷达 - 高级主题 + + + +## 7.1 简介 + +RoboSense雷达可以工作在如下的场景。 + ++ 单播/组播/广播模式。 ++ 运行在VLAN协议上。 ++ 向Packet加入用户自己的层。 ++ 接入多个雷达。 + +本文描述在这些场景下如何配置rslidar_sdk。 + +在阅读本文档之前， 请确保已经阅读过： ++ 雷达用户手册 ++ [参数介绍](../intro/02_parameter_intro_CN.md) ++ [连接在线雷达](./06_how_to_decode_online_lidar_CN.md) + + + +## 7.2 单播、组播、广播 + +### 7.2.1 广播 + +雷达发送 MSOP/DIFOP Packet到电脑主机。为简单起见，如下的图没有显示DIFOP端口。 ++ 雷达发送Packet到 `255.255.255.255` : `6699`, rslidar_sdk绑定到主机的端口 `6699`. +![](./img/07_01_broadcast.png) + +如下是配置`config.yaml`的方式。 + +```yaml +common: + msg_source: 1 + send_point_cloud_ros: true + +lidar: + - driver: + lidar_type: RS32 + msop_port: 6699 + difop_port: 7788 + ros: + ros_frame_id: rslidar + ros_send_point_cloud_topic: /rslidar_points +``` + +这里列出了`common`部分和`lidar-ros`部分的设置。这两部分设置将在本文中后面的例子沿用，不再列出。 + +### 7.2.2 单播 + +为了减少网络负载，建议雷达使用单播模式。 ++ 雷达发送Packet到 `192.168.1.102` : `6699`, rslidar_sdk绑定端口 `6699`。 +![](./img/07_02_unicast.png) + +如下是配置`config.yaml`的方式。这实际上与广播的方式一样。 + +```yaml +lidar: + - driver: + lidar_type: RS32 + msop_port: 6699 + difop_port: 7788 +``` + +### 7.2.3 组播 + +雷达也可以工作在组播模式。 ++ 雷达发送Packet到 `224.1.1.1`:`6699` ++ rslidar_sdk绑定到端口 `6699`。同时它将IP地址为`192.168.1.102`的本地网络接口加入组播组`224.1.1.1`。 +![](./img/07_03_multicast.png) + +如下是配置`config.yaml`的方式。 + +```yaml +lidar: + - driver: + lidar_type: RS32 + msop_port: 6699 + difop_port: 7788 + group_address: 224.1.1.1 + host_address: 192.168.1.102 +``` + + + +## 7.3 多个雷达的情况 + +### 7.3.1 不同的目标端口 + +如果有两个或多个雷达，首选的配置是让它们有不同的目标端口。 ++ 第一个雷达发送Packet到 `192.168.1.102`:`6699`, 给rslidar_sdk配置的第一个driver节点绑定到`6699`。 ++ 第二个雷达发送Packet到 `192.168.1.102`:`5599`, 给rslidar_sdk配置的第二个driver节点绑定到`5599`。 +![](./img/07_04_multi_lidars_port.png) + +如下是配置`config.yaml`的方式。 + +```yaml +lidar: + - driver: + lidar_type: RS32 + msop_port: 6699 + difop_port: 7788 + - driver: + lidar_type: RS32 + msop_port: 5599 + difop_port: 6688 +``` + +### 7.3.2 不同的目标IP + +也可以让多个雷达使用不同的目标IP。 ++ 主机有两个网卡， IP地址分别为`192.168.1.102` 和 `192.168.1.103`。 ++ 第一个雷达发送Packet到 `192.168.1.102`:`6699`, 给rslidar_sdk配置的第一个driver节点绑定到`192.168.1.102:6699`。 ++ 第二个雷达发送Packet到 `192.168.1.103`:`6699`, 给rslidar_sdk配置的第二个driver节点绑定到`192.168.1.103:6699`。 +![](./img/07_05_multi_lidars_ip.png) + +如下是配置`config.yaml`的方式。 + +```yaml +lidar: + - driver: + lidar_type: RS32 + msop_port: 6699 + difop_port: 7788 + host_address: 192.168.1.102 + - driver: + lidar_type: RS32 + msop_port: 6699 + difop_port: 7788 + host_address: 192.168.1.103 +``` + + + +## 7.4 VLAN + +在某些场景下，雷达工作在VLAN层之上。MSOP/DIFOP Packet有VLAN层，如下图。 +![](./img/07_06_vlan_layer.png) + +rslidar_sdk不能解析VLAN层。 + +需要一个虚拟网卡来剥除掉这一层。举例如下。 + ++ 雷达工作在VLAN id为`80`的VLAN层上。它发送Packet到`192.168.1.102` : `6699`，Packet有VLAN层。 ++ 假设主机上有一个支持VLAN的物理网卡`eno1`. 它接收带VLAN层的Packet。 +![](./img/07_07_vlan.png) + +要剥离VLAN层，需要基于`eno1`，创建一个虚拟网卡`eno1.80`, 并且将它的IP设置为`192.168.1.102`。 + +```shell +sudo apt-get install vlan -y +sudo modprobe 8021q + +sudo vconfig add eno1 80 +sudo ifconfig eno1.80 192.168.1.102 up +``` + +现在，rslidar_sdk可以将`eno1.80`当做一个一般的网卡来处理，从这里接收不带VLAN层的Packet. + +```yaml +lidar: + - driver: + lidar_type: RS32 + msop_port: 6699 + difop_port: 7788 +``` + + + +## 7.5 User Layer, Tail Layer + +在某些场景下，用户可能在MSOP/DIFOP数据前后加入自己的层。 ++ 在前面的是USER_LAYER，在后面的是TAIL_LAYER。 +![](./img/07_08_user_layer.png) + +这两个层是UDP数据的一部分，所以rslidar_sdk可以自己剥除它们。只需要指出这两个层的长度就可以了。 + +在下面的例子中，USER_LAYER是8字节，TAIL_LAYER是4字节。 + +```yaml +lidar: + - driver: + lidar_type: RS32 + msop_port: 6699 + difop_port: 7788 + user_layer_bytes: 8 + tail_layer_bytes: 4 +``` + diff --git a/doc/howto/08_how_to_decode_pcap_file.md b/doc/howto/08_how_to_decode_pcap_file.md new file mode 100644 index 00000000..a6d2661c --- /dev/null +++ b/doc/howto/08_how_to_decode_pcap_file.md @@ -0,0 +1,79 @@ +# 8 How to decode PCAP file + + + +## 8.1 Introduction + +This document illustrates how to decode PCAP file, and send point cloud to ROS. + +Please make sure you have read the LiDAR user-guide and [Intro to parameters](../intro/02_parameter_intro.md) before reading this document. + + + +## 8.2 Steps + +### 8.2.1 Get the LiDAR port number + +Please check the LiDAR user-guide, or use the 3rd-party tool(such as WireShark), to get your LiDAR's MSOP port number and DIFOP port number. The default values are ```msop-6699, difop-7788```. + +### 8.2.2 Set up the configuration file + +Set up the configuration file `config.yaml`. + +#### 8.2.2.1 common part + +```yaml +common: + msg_source: 3 + send_packet_ros: false + send_point_cloud_ros: true + send_packet_proto: false + send_point_cloud_proto: false +``` + +The messages come from the PCAP bag, so set ```msg_source = 3```. + +Send point cloud to ROS, so set ```send_point_cloud_ros = true```. + +#### 8.2.2.2 lidar-driver part + +```yaml +lidar: + - driver: + lidar_type: RS128 + msop_port: 6699 + difop_port: 7788 + start_angle: 0 + end_angle: 360 + min_distance: 0.2 + max_distance: 200 + use_lidar_clock: false + pcap_path: /home/robosense/lidar.pcap +``` + +Set the ```pcap_path``` to the absolute path of the PCAP file. + +Set the ```lidar_type``` to your LiDAR type. + +Set the ```msop_port``` and ```difop_port``` to the port numbers of your LiDAR. + +#### 8.2.2.3 lidar-ros part + +```yaml +ros: + ros_frame_id: rslidar + ros_recv_packet_topic: /rslidar_packets + ros_send_packet_topic: /rslidar_packets + ros_send_point_cloud_topic: /rslidar_points +``` + +Set the ```ros_send_point_cloud_topic``` to the topic you want to send to. + +### 8.2.3 Run + +Run the program. + + + + + diff --git a/doc/howto/08_how_to_decode_pcap_file_CN.md b/doc/howto/08_how_to_decode_pcap_file_CN.md new file mode 100644 index 00000000..c5fd4cb9 --- /dev/null +++ b/doc/howto/08_how_to_decode_pcap_file_CN.md @@ -0,0 +1,74 @@ +# 8 如何解码PCAP文件 + + + +## 8.1 简介 + +本文档展示如何解码PCAP文件, 并发送点云数据到ROS。 + +在阅读本文档之前，请确保已阅读雷达用户手册和 [参数简介](../intro/02_parameter_intro_CN.md) 。 + + + +## 8.2 步骤 + +### 8.2.1 获取数据的端口号 + +请参考雷达用户手册，或者使用第三方工具（WireShark等）抓包，得到雷达的目标MSOP端口和目标DIFOP端口。端口的默认值分别为`6699`和`7788`。 + +### 8.2.2 设置参数文件 + +设置参数文件```config.yaml```。 + +#### 8.2.2.1 common部分 + +```yaml +common: + msg_source: 3 + send_packet_ros: false + send_point_cloud_ros: true + send_packet_proto: false + send_point_cloud_proto: false +``` + +消息来自PCAP包，所以设置 ```msg_source = 3``` 。 + +将点云发送到ROS以便查看，所以设置 ```send_point_cloud_ros = true``` 。 + +#### 8.2.2.2 lidar-driver部分 + +```yaml +lidar: + - driver: + lidar_type: RS128 + msop_port: 6699 + difop_port: 7788 + start_angle: 0 + end_angle: 360 + min_distance: 0.2 + max_distance: 200 + use_lidar_clock: false + pcap_path: /home/robosense/lidar.pcap +``` + +将```pcap_path``` 设置为PCAP文件的全路径。 + +将 ```lidar_type``` 设置为LiDAR类型。 + +设置 ```msop_port``` 和 ```difop_port``` 为雷达数据的目标端口号，这里分别是6699和7788。 + +#### 8.2.2.3 lidar-ros部分 + +```yaml +ros: + ros_frame_id: rslidar + ros_recv_packet_topic: /rslidar_packets + ros_send_packet_topic: /rslidar_packets + ros_send_point_cloud_topic: /rslidar_points +``` + +将 ```ros_send_point_cloud_topic``` 设置为发送点云的话题，这里是/rslidar_points。 + +### 8.2.3 运行 + +运行程序。 diff --git a/doc/howto/09_pcap_file_advanced_topics.md b/doc/howto/09_pcap_file_advanced_topics.md new file mode 100644 index 00000000..cb939a6c --- /dev/null +++ b/doc/howto/09_pcap_file_advanced_topics.md @@ -0,0 +1,91 @@ +# 9 PCAP File - Advanced Topics + + + +## 9.1 Introduction + +The RoboSense LiDAR may work ++ in unicast/multicast/broadcast mode, ++ with VLAN layer ++ with user layers. ++ Also rslidar_sdk supports multi-LiDARs. + +This document illustrates how to configure rslidar_sdk in each case. + +Before reading this document, please be sure that you have read: ++ LiDAR user-guide ++ [Intro to parameters](../intro/02_parameter_intro.md) ++ [Online LiDAR - Advanced Topics](./07_online_lidar_advanced_topics.md) + + + +## 9.2 General Case + +Generally, below code is for decoding a PCAP file in these cases. ++ Broadcast/multicast/unicast mode ++ There are multiple LiDars in a file. + +```yaml +common: + msg_source: 3 + send_point_cloud_ros: true + +lidar: + - driver: + lidar_type: RS32 + pcap_path: /home/robosense/lidar.pcap + msop_port: 6699 + difop_port: 7788 + ros: + ros_frame_id: rslidar + ros_send_point_cloud_topic: /rslidar_points +``` + +The only exception is "Multiple Lidars with same ports but different IPs", which is not supported now. + + + +## 9.3 VLAN + +In some user cases, The LiDar may work on VLAN. Its packets have a VLAN layer. +![](./img/07_06_vlan_layer.png) + +rs_driver decodes PCAP file and gets all parts of MSOP packets, including the VLAN layer. + +To strip the VLAN layer, just set `use_vlan: true`. + +```yaml +lidar: + - driver: + lidar_type: RS32 + pcap_path: /home/robosense/lidar.pcap + msop_port: 6699 + difop_port: 7788 + use_vlan: true +``` + + + +## 9.4 User Layer, Tail Layer + +In some user cases, User may add extra layers before or/and after the MSOP/DIFOP packet. ++ USER_LAYER is before the packet and TAIL_LAYER is after it. +![](./img/07_08_user_layer.png) + +These extra layers are parts of UDP data. The driver can strip them. + +To strip them, just give their lengths in bytes. + +In the following example, USER_LAYER is 8 bytes, and TAIL_LAYER is 4 bytes. + +```yaml +lidar: + - driver: + lidar_type: RS32 + pcap_path: /home/robosense/lidar.pcap + msop_port: 6699 + difop_port: 7788 + user_layer_bytes: 8 + tail_layer_bytes: 4 +``` + diff --git a/doc/howto/09_pcap_file_advanced_topics_CN.md b/doc/howto/09_pcap_file_advanced_topics_CN.md new file mode 100644 index 00000000..1a3dbf1f --- /dev/null +++ b/doc/howto/09_pcap_file_advanced_topics_CN.md @@ -0,0 +1,89 @@ +# 9 PCAP文件 - 高级主题 + + + +## 9.1 简介 + +RoboSense雷达可以工作在如下场景。 ++ 单播/组播/广播模式 ++ 运行在VLAN协议上 ++ 向Packet中加入用户自己的层 ++ 接入多个雷达 + +本文说明在每种场景下，如何配置rslidar_sdk。 + +在阅读本文之前，请先阅读： ++ 雷达用户使用手册 ++ [参数介绍](../intro/02_parameter_intro_CN.md) ++ [在线雷达-高级主题](./07_online_lidar_advanced_topics_CN.md) + + + +## 9.2 一般场景 + +在下列场景下，使用如下配置解码PCAP文件。 ++ 广播/组播/单播模式 ++ PCAP文件中有多个雷达 + +```yaml +common: + msg_source: 3 + send_point_cloud_ros: true + +lidar: + - driver: + lidar_type: RS32 + pcap_path: /home/robosense/lidar.pcap + msop_port: 6699 + difop_port: 7788 + ros: + ros_frame_id: rslidar + ros_send_point_cloud_topic: /rslidar_points +``` + +一个例外是：PCAP文件中有多个雷达数据，但这些雷达目的端口相同，使用不同的目的IP地址来区分。这种情况不支持。 + + + +## 9.3 VLAN + +有些场景下，雷达工作在VLAN环境下。这时MSOP/DIFOP包带VLAN层，如下图。 +![](./img/07_06_vlan_layer.png) + +rs_driver使用libpcap库解析PCAP文件，可以得到完整的、包括VLAN层的MSOP/DIFOP包。 + +要剥除VLAN层，只需要设置`use_vlan: true`。 + +```yaml +lidar: + - driver: + lidar_type: RS32 + pcap_path: /home/robosense/lidar.pcap + msop_port: 6699 + difop_port: 7788 + use_vlan: true +``` + + + +## 9.4 User Layer, Tail Layer + +某些场景下，用户可能在MSOP/DIFOP数据前后加入自己的层。 ++ USER_LAYER 在MSOP/DIFOP数据之前，TAIL_LAYER在MSOP/DIFOP数据之后。 +![](./img/07_08_user_layer.png) + +这些层是UDP数据的一部分，所以rs_driver可以自己剥除他们。只需要告诉它每个层的字节数就可以。 + +如下的例子中，指定USER_LAYER为8字节，TAIL_LAYER为4字节。 + +```yaml +lidar: + - driver: + lidar_type: RS32 + pcap_path: /home/robosense/lidar.pcap + msop_port: 6699 + difop_port: 7788 + user_layer_bytes: 8 + tail_layer_bytes: 4 +``` + diff --git a/doc/howto/10_how_to_use_coordinate_transformation.md b/doc/howto/10_how_to_use_coordinate_transformation.md new file mode 100644 index 00000000..d38e24ae --- /dev/null +++ b/doc/howto/10_how_to_use_coordinate_transformation.md @@ -0,0 +1,81 @@ +# 10 How to use coordinate transformation + + + +## 10.1 Introduction + +rslidar_sdk can transform the coordinate of point cloud. This document illustrate how to do so. + +Please check the [Intro to hiding parameters](../intro/03_hiding_parameters_intro.md) for more details. Here is an example of the config.yaml. + + + +## 10.2 Dependencies + +rslidar_sdk depends on the libeigen library to do coordinate transformation. Please install it first. + +```bash +sudo apt-get install libeigen3-dev +``` + + + +## 10.3 Compile + +To enable transformation, set the CMake option ```ENABLE_TRANSFORM```to be ```ON```. + +- Compile directly + + ```bash + cmake -DENABLE_TRANSFORM=ON .. + ``` + +- ROS + + ```bash + catkin_make -DENABLE_TRANSFORM=ON + ``` + +- ROS2 + + ```bash + colcon build --cmake-args '-DENABLE_TRANSFORM=ON' + ``` + + + +## 10.4 Set LiDAR parameters + +In the `lidar-driver` part of `config.yaml`, set the hiding parameter`x`, `y`, `z`, `roll`, `pitch` ,`yaw`. + +```yaml +common: + msg_source: 1 + send_packet_ros: false + send_point_cloud_ros: true + send_packet_proto: false + send_point_cloud_proto: false +lidar: + - driver: + lidar_type: RS128 + msop_port: 6699 + difop_port: 7788 + start_angle: 0 + end_angle: 360 + min_distance: 0.2 + max_distance: 200 + use_lidar_clock: false + pcap_path: /home/robosense/lidar.pcap + x: 1 + y: 0 + z: 2.5 + roll: 0.1 + pitch: 0.2 + yaw: 1.57 +``` + + + +## 10.5 Run + +Run the program. diff --git a/doc/howto/10_how_to_use_coordinate_transformation_CN.md b/doc/howto/10_how_to_use_coordinate_transformation_CN.md new file mode 100644 index 00000000..3895a2be --- /dev/null +++ b/doc/howto/10_how_to_use_coordinate_transformation_CN.md @@ -0,0 +1,81 @@ +# 10 如何使用坐标变换功能 + + + +## 10.1 简介 + +rslidar_sdk支持对点云进行坐标变换，本文档展示如何作这种变换。 + +在阅读本文档之前，请确保已阅读雷达用户手册和[隐藏参数介绍](../intro/03_hiding_parameters_intro_CN.md)。 + + + +## 10.2 依赖库 + +rslidar_sdk的坐标变换基于libeigen库，所以要先安装它。 + +```bash +sudo apt-get install libeigen3-dev +``` + + + +## 10.3 编译 + +要启用坐标变换，编译rslidar_sdk时，需要将```ENABLE_TRANSFORM```选项设置为```ON```. + +- 直接编译 + + ```bash + cmake -DENABLE_TRANSFORM=ON .. + ``` + +- ROS + + ```bash + catkin_make -DENABLE_TRANSFORM=ON + ``` + +- ROS2 + + ```bash + colcon build --cmake-args '-DENABLE_TRANSFORM=ON' + ``` + + + +## 10.4 设置雷达参数 + +在`config.yaml`中，设置`lidar-lidar`部分的参数`x`、, `y`、 `z`、 `roll`、 `pitch` 、`yaw`。 + +```yaml +common: + msg_source: 1 + send_packet_ros: false + send_point_cloud_ros: true + send_packet_proto: false + send_point_cloud_proto: false +lidar: + - driver: + lidar_type: RS128 + msop_port: 6699 + difop_port: 7788 + start_angle: 0 + end_angle: 360 + min_distance: 0.2 + max_distance: 200 + use_lidar_clock: false + pcap_path: /home/robosense/lidar.pcap + x: 1 + y: 0 + z: 2.5 + roll: 0.1 + pitch: 0.2 + yaw: 1.57 +``` + + + +## 10.5 运行 + +运行程序。 diff --git a/doc/howto/11_how_to_record_replay_packet_rosbag.md b/doc/howto/11_how_to_record_replay_packet_rosbag.md new file mode 100644 index 00000000..32a307cc --- /dev/null +++ b/doc/howto/11_how_to_record_replay_packet_rosbag.md @@ -0,0 +1,111 @@ +# 11 How to record and replay Packet rosbag + + + +## 11.1 Introduction + +This document illustrates how to record and replay MSOP/DIFOP Packet rosbag. + +It is possible to record the point cloud message into a rosbag and replay it, but the point cloud rosbag is very large. rslidar_sdk provides a better way - record packet rosbag and replay it. + +Please be sure you have read the LiDAR user-guide and [Connect to online LiDAR and send point cloud through ROS](./06_how_to_decode_online_lidar.md). + + + +## 11.2 Record + +### 11.2.1 Send packet to ROS + +Here suppose that you have connected to an on-line LiDAR, and have sent the point cloud to ROS. + + +```yaml +common: + msg_source: 1 + send_packet_ros: true + send_point_cloud_ros: true + send_packet_proto: false + send_point_cloud_proto: false +``` + +To record packets, set ```send_packet_ros = true```. + +### 11.2.2 Record the topic of packet + +To change the topic of packet, change ```ros_send_packet_topic```. This topic sends out both MSOP and DIFOP packets. + +```yaml +ros: + ros_frame_id: rslidar + ros_recv_packet_topic: /rslidar_packets + ros_send_packet_topic: /rslidar_packets + ros_send_point_cloud_topic: /rslidar_points +``` + +Record rosbag as below. + +```sh +rosbag record /rslidar_packets -O bag +``` + + + +## 11.3 Replay + +Suppose you have recorded a rosbag, which contains MSOP/DIFOP packets with the topic ```/rslidar_packets```. + +### 11.3.1 Set Packet Source + +In `config.yaml`, set the `common` part. + +```yaml +common: + msg_source: 2 + send_packet_ros: false + send_point_cloud_ros: true + send_packet_proto: false + send_point_cloud_proto: false +``` + +Packet is from the ROS, so set ```msg_source = 2```. + +To send point cloud to ROS, set ```send_point_cloud_ros = true```. + +### 11.3.2 Set parameters of Lidar + +In `config.yaml`, set the `lidar-driver` part. + +```yaml +lidar: + - driver: + lidar_type: RS128 + msop_port: 6699 + difop_port: 7788 + start_angle: 0 + end_angle: 360 + min_distance: 0.2 + max_distance: 200 + use_lidar_clock: false +``` + +Set the ```lidar_type``` to your LiDAR type. + +### 11.3.3 Set Topic of packet. + +In `config.yaml`, set the `lidar-ros` part. + +```yaml +ros: + ros_frame_id: rslidar + ros_recv_packet_topic: /rslidar_packets + ros_send_packet_topic: /rslidar_packets + ros_send_point_cloud_topic: /rslidar_points +``` + +To receive MSOP/DIFOP packest, set ```ros_recv_packet_topic``` to the topic in the rosbag. + +### 11.3.4 Run + +Run the demo, and replay rosbag. + + diff --git a/doc/howto/11_how_to_record_replay_packet_rosbag_CN.md b/doc/howto/11_how_to_record_replay_packet_rosbag_CN.md new file mode 100644 index 00000000..d2d93e1f --- /dev/null +++ b/doc/howto/11_how_to_record_replay_packet_rosbag_CN.md @@ -0,0 +1,114 @@ +# 11 如何录制与回放 Packet rosbag + + + +## 11.1 简介 + +本文档展示如何记录与回放MSOP/DIFOP rosbag。 + +使用ROS可以录制点云rosbag消息并回放，但点云包非常大，所以rslidar_sdk提供更好的选择，也就是录制Packet rosbag并回放。 + +在阅读本文档之前, 请先阅读雷达用户手册和 [连接在线雷达并发送点云到ROS](./06_how_to_decode_online_lidar_CN.md) 。 + + + +## 11.2 录制 + +### 11.2.1 将MSOP/DIFOP Packet发送至ROS + +这里假设已经连接在线雷达，并能发送点云到ROS。 + +```yaml +common: + msg_source: 1 + send_packet_ros: true + send_point_cloud_ros: true + send_packet_proto: false + send_point_cloud_proto: false +``` + +要录制Packet, 设置 ```send_packet_ros = true```。 + +### 11.2.2 根据话题录制rosbag + +修改```ros_send_packet_topic```, 来改变发送的话题。这个话题包括MSOP Packet和DIFOP Packet。 + +```yaml +ros: + ros_frame_id: rslidar + ros_recv_packet_topic: /rslidar_packets + ros_send_packet_topic: /rslidar_packets + ros_send_point_cloud_topic: /rslidar_points +``` + +ROS录制rosbag的指令如下。 + +```bash +rosbag record /rslidar_packets -O bag +``` + + + +## 11.3 回放 + +假设录制了一个rosbag，其中包含话题为 `/rslidar_packets` 的MSOP/DIFOP Packet。 + +### 11.3.1 设置Packet源 + +配置`config.yaml`的`common`部分。 + +```yaml +common: + msg_source: 2 + send_packet_ros: false + send_point_cloud_ros: true + send_packet_proto: false + send_point_cloud_proto: false +``` + +MSOP/DIFOP Packet来自ROS rosbag，因此设置 ```msg_source = 2``` 。 + +将点云发送到ROS，因此设置 ```send_point_cloud_ros = true```。 + +### 11.3.2 设置雷达参数 + +配置`config.yaml`的`lidar-driver`部分。 + +```yaml +lidar: + - driver: + lidar_type: RS128 + msop_port: 6699 + difop_port: 7788 + start_angle: 0 + end_angle: 360 + min_distance: 0.2 + max_distance: 200 + use_lidar_clock: false +``` + +将 ```lidar_type``` 设置为LiDAR类型 。 + +### 11.3.3 设置接收Packet的主题 + +设置`config.yaml`的`lidar-ros`部分。 + +```yaml +ros: + ros_frame_id: rslidar + ros_recv_packet_topic: /rslidar_packets + ros_send_packet_topic: /rslidar_packets + ros_send_point_cloud_topic: /rslidar_points +``` + +将 ```ros_recv_packet_topic``` 设置为rosbag中MSOP/DIFOP Packet的话题。 + + + +### 11.3.4 运行 + +运行程序，回放rosbag。 + + + + diff --git a/doc/howto/12_how_to_create_deb.md b/doc/howto/12_how_to_create_deb.md new file mode 100644 index 00000000..b183eaaf --- /dev/null +++ b/doc/howto/12_how_to_create_deb.md @@ -0,0 +1,35 @@ +# 12 How to create deb + + + +## 12.1 Introduction + +Generating a ".deb" installable file is useful. + + + +## 12.2 Create deb + +Just run the shell script: +``` +./create_debian.sh +``` +The deb file will be generated in the parent directory of `rslidar_sdk`. + + +## 12.3 Use the deb + +Install the deb and set the right config_path. If leave the config_path empty, it will use the `config.yaml` in the ros package path. + +``` + + + + + + + +``` + + + diff --git a/doc/howto/how_to_offline_decode_pcap.md b/doc/howto/how_to_offline_decode_pcap.md deleted file mode 100644 index 6a77a978..00000000 --- a/doc/howto/how_to_offline_decode_pcap.md +++ /dev/null @@ -1,70 +0,0 @@ -# How to decode pcap bag and send point cloud to ROS - -## 1 Introduction - -This document will show you how to decode pcap bag and send point cloud to ROS. Please make sure you have read the LiDAR user-guide and [Intro to parameters](doc/intro/parameter_intro.md) before reading this document. - -## 2 Steps - -### 2.1 Get the LiDAR port number - -Please follow the instructions in LiDAR user-guide to connect the LiDAR and set up your computer's ip address. At this time, you should have already known your LiDAR's msop port number and difop port number. The default is ```msop-6699, difop-7788```. If you have no idea about what it is, please check the LiDAR user-guide first. - -### 2.2 Set up the common part of the config file - -```yaml -common: - msg_source: 3 - send_packet_ros: false - send_point_cloud_ros: true - send_packet_proto: false - send_point_cloud_proto: false - pcap_path: /home/robosense/lidar.pcap -``` - -Since the message come from the pcap bag, set ```msg_source = 3```. - -Send point cloud to ROS so set ```send_point_cloud_ros = true```. - -Make sure the ```pcap_path``` is correct. - -### 2.3 Set up the lidar-driver part of the config file - -```yaml -lidar: - - driver: - lidar_type: RS128 - frame_id: /rslidar - msop_port: 6699 - difop_port: 7788 - start_angle: 0 - end_angle: 360 - min_distance: 0.2 - max_distance: 200 - use_lidar_clock: false -``` - -Set the ```lidar_type``` to your LiDAR type. - -Set the ```msop_port``` and ```difop_port``` to your LiDAR's port number. - -### 2.4 Set up the lidar-ros part of the config file - -```yaml -ros: - ros_recv_packet_topic: /rslidar_packets - ros_send_packet_topic: /rslidar_packets - ros_send_point_cloud_topic: /rslidar_points -``` - -Set the ```ros_send_point_cloud_topic``` to the topic you want to send. - -### 2.5 Run - -Run the program. - - - - - - diff --git a/doc/howto/how_to_offline_decode_pcap_cn.md b/doc/howto/how_to_offline_decode_pcap_cn.md deleted file mode 100644 index b418d166..00000000 --- a/doc/howto/how_to_offline_decode_pcap_cn.md +++ /dev/null @@ -1,65 +0,0 @@ -# 如何解码pcap包并发送点云数据到ROS - -## 1 简介 - -本文档将展示如何解码pcap包并发送点云数据到ROS。 在阅读本文档之前，请确保已阅读雷达用户手册和 [参数简介](../intro/parameter_intro.md) 。 - -## 2 步骤 - -### 2.1 获取数据端口号 - -首先根据雷达用户手册连接雷达并设置好您的电脑的IP地址。此时应该已知雷达的msop端口号和difop端口号，默认值为```msop-6699, difop-7788```。 如果不清楚上述内容，请查看雷达用户手册。 - -### 2.2 设置参数文件的common部分 - -```yaml -common: - msg_source: 3 - send_packet_ros: false - send_point_cloud_ros: true - send_packet_proto: false - send_point_cloud_proto: false - pcap_path: /home/robosense/lidar.pcap -``` - -由于消息来自pcap包，因此请设置 ```msg_source = 3``` 。 - -将点云发送到ROS以查看，因此设置 ```send_point_cloud_ros = true``` 。 - -请确保路径```pcap_path``` 是正确的。 - -### 2.3 设置参数文件的 lidar-driver部分 - -```yaml -lidar: - - driver: - lidar_type: RS128 - frame_id: /rslidar - msop_port: 6699 - difop_port: 7788 - start_angle: 0 - end_angle: 360 - min_distance: 0.2 - max_distance: 200 - use_lidar_clock: false -``` - -将 ```lidar_type``` 设置为LiDAR类型 。 - - -设置 ```msop_port``` 和 ```difop_port``` 为雷达数据端口号。 - -### 2.4设置配置文件的lidar-ros部分 - -```yaml -ros: - ros_recv_packet_topic: /rslidar_packets - ros_send_packet_topic: /rslidar_packets - ros_send_point_cloud_topic: /rslidar_points -``` - -将 ```ros_send_point_cloud_topic``` 设置为发送点云的话题。 - -#### 2.5 运行 - -运行程序。 diff --git a/doc/howto/how_to_online_send_point_cloud_ros.md b/doc/howto/how_to_online_send_point_cloud_ros.md deleted file mode 100644 index 81c9b12a..00000000 --- a/doc/howto/how_to_online_send_point_cloud_ros.md +++ /dev/null @@ -1,70 +0,0 @@ -# How to online connect lidar and send point cloud to ROS - -## 1 Introduction - -This document will show you how to online connect a LiDAR and send point cloud to ROS. Please make sure you have read the LiDAR user-guide and [Intro to parameters](../intro/parameter_intro.md) before reading this document. - -## 2 Steps - -### 2.1 Get the LiDAR port number - -Please follow the instructions in LiDAR user-guide to connect the LiDAR and set up your computer's ip address. At this time, you should have already known your LiDAR's msop port number and difop port number. The default is ```msop-6699, difop-7788```. If you have no idea about what it is, please check the LiDAR user-guide first. - -### 2.2 Set up the common part of the config file - -```yaml -common: - msg_source: 1 - send_packet_ros: false - send_point_cloud_ros: true - send_packet_proto: false - send_point_cloud_proto: false - pcap_path: /home/robosense/lidar.pcap -``` - -Since the message come from the LiDAR, set ```msg_source = 1```. - -Send point cloud to ROS so set ```send_point_cloud_ros = true```. - -### 2.3 Set up the lidar-driver part of the config file - -```yaml -lidar: - - driver: - lidar_type: RS128 - frame_id: /rslidar - msop_port: 6699 - difop_port: 7788 - start_angle: 0 - Run the demo & play the rosbag. - - end_angle: 360 - min_distance: 0.2 - max_distance: 200 - use_lidar_clock: false -``` - -Set the ```lidar_type``` to your LiDAR type. - -Set the ```msop_port``` and ```difop_port``` to your LiDAR's port number. - -### 2.4 Set up the lidar-ros part of the config file - -```yaml -ros: - ros_recv_packet_topic: /rslidar_packets - ros_send_packet_topic: /rslidar_packets - ros_send_point_cloud_topic: /rslidar_points -``` - -Set the ```ros_send_point_cloud_topic``` to the topic you want to send. - -### 2.5 Run - -Run the program. - - - - - - diff --git a/doc/howto/how_to_record_and_offline_decode_rosbag.md b/doc/howto/how_to_record_and_offline_decode_rosbag.md deleted file mode 100644 index 3c9f6a47..00000000 --- a/doc/howto/how_to_record_and_offline_decode_rosbag.md +++ /dev/null @@ -1,97 +0,0 @@ -# How to record and decode rosbag - -## 1 Introduction - -This document will show you how to record and decode rosbag. Please make sure you have read the LiDAR user-guide and [Intro to parameters](../intro/parameter_intro.md) before reading this document. - -## 2 Record - -### 2.1 Send packet to ROS - -We suppose you are connecting an online LiDAR and you have already sent out the point cloud to ROS. If you have no idea about this, please read [Online connect lidar and send point cloud through ROS](how_to_online_send_point_cloud_ros.md) first. - -Actually, you can record the point cloud message now and you don't need to run the driver again when you off-line play the bag. But the disadvantage is also obvious that the point cloud message is very large. So normally we recommend to record packets rather than point cloud message. - -```yaml -common: - msg_source: 1 - send_packet_ros: true - send_point_cloud_ros: true - send_packet_proto: false - send_point_cloud_proto: false - pcap_path: /home/robosense/lidar.pcap -``` - -In order to record packets, set ```send_packet_ros = true```. - -### 2.2 Record according to the topic - -```yaml -ros: - ros_recv_packet_topic: /rslidar_packets - ros_send_packet_topic: /rslidar_packets - ros_send_point_cloud_topic: /rslidar_points -``` - -User can also adjust the packets topic by adjust the ```ros_send_packet_topic``` in *lidar-ros* part of the config file. This topic represent the topic of the msop, and the topic of the difop will be ```msop-topic_difop```. e.g., the default topic value is set to ```rslidar_packets```, so the msop topic is ```rslidar_packets``` and the difop topic is ```rslidar_packets_difop```. The command to record bag is listed below. - -```sh -rosbag record /rslidar_packets /rslidar_packets_difop -O bag -``` - -**If you set send_packet_ros = true, both two kinds of packets will be sent to ROS. And you must record both of these two kinds of packets.** - -## 3 Offline Decode - -We suppose you have recorded a rosbag which contains msop packets with the topic ```rslidar_packets``` and difop packets with the topic ```rslidar_packets_difop```. - -### 3.1 Set up the common part of the config file - -```yaml -common: - msg_source: 2 - send_packet_ros: false - send_point_cloud_ros: true - send_packet_proto: false - send_point_cloud_proto: false - pcap_path: /home/robosense/lidar.pcap -``` - -Since the packets message come from the ROS, set ```msg_source = 2```. - -We want to send point cloud to ROS so set ```send_point_cloud_ros = true```. - -### 3.2 Set up the lidar-driver part of the config file - -```yaml -lidar: - - driver: - lidar_type: RS128 - frame_id: /rslidar - msop_port: 6699 - difop_port: 7788 - start_angle: 0 - end_angle: 360 - min_distance: 0.2 - max_distance: 200 - use_lidar_clock: false -``` - -Set the ```lidar_type``` to your LiDAR type. - -### 3.3 Set the lidar-ros part of the config file - -```yaml -ros: - ros_recv_packet_topic: /rslidar_packets - ros_send_packet_topic: /rslidar_packets - ros_send_point_cloud_topic: /rslidar_points -``` - -Set up the ```ros_recv_packet_topic``` to the ```msop``` topic in the rosbag. - -### 3.4 Run - -Run the program & play rosbag. - - diff --git a/doc/howto/how_to_record_and_offline_decode_rosbag_cn.md b/doc/howto/how_to_record_and_offline_decode_rosbag_cn.md deleted file mode 100644 index 04f8af5a..00000000 --- a/doc/howto/how_to_record_and_offline_decode_rosbag_cn.md +++ /dev/null @@ -1,99 +0,0 @@ -# 如何录制与解码rosbag - -## 1 简介 - -本文档将展示如何记录与解码rosbag。 在阅读这本文档之前请先阅读雷达用户手册与[参数简介](../intro/parameter_intro.md) 。 - -## 2 录包 - -### 2.1 将packet发送至ROS - -首先在线连接雷达并将点云发送至ROS。如果对此不太了解, 请先阅读 [在线连接雷达并发送点云到ROS](how_to_online_send_point_cloud_ros_cn.md) 。 - -现在可以直接录制点云消息，这样在离线播包时不需要再另外运行驱动程序解包。但这种方法会导致录制的包非常大。 因此，通常建议记录雷达packet数据，而不是记录点云数据。 - -```yaml -common: - msg_source: 1 - send_packet_ros: true - send_point_cloud_ros: true - send_packet_proto: false - send_point_cloud_proto: false - pcap_path: /home/robosense/lidar.pcap -``` - -​ 为了记录雷达packet, 需要设置 ```send_packet_ros = true```。 - -### 2.2 根据对应话题录包 - -```yaml -ros: - ros_recv_packet_topic: /rslidar_packets - ros_send_packet_topic: /rslidar_packets - ros_send_point_cloud_topic: /rslidar_points -``` - -用户可以通过调整参数文件的 *lidar-ros* 部分中的 ```ros_send_packet_topic``` 来调整发送的话题。 该话题表示msop的话题，而difop的话题为``` msop-topic_difop```。 例： 默认话题设置为 ```rslidar_packets```，因此msop话题为 ```rslidar_packets```，而difop的话题为 ```rslidar_packets_difop```。录包的指令如下所示。 - -```bash -rosbag record /rslidar_packets /rslidar_packets_difop -O bag -``` - -**如果将send_packet_ros设置为true，则两种数据包都将发送到ROS。 录包时必须同时记录这两种数据。** - -## 3 离线解码 - -假设录制了一个rosbag，其中包含话题为 *rslidar_packets* 的msop数据包和话题为 *rslidar_packets_difop*的difop数据包。 - -### 3.1 设置文件的 common部分 - -```yaml -common: - msg_source: 2 - send_packet_ros: false - send_point_cloud_ros: true - send_packet_proto: false - send_point_cloud_proto: false - pcap_path: /home/robosense/lidar.pcap -``` - -由于数据包消息来自ROS，因此设置 ```msg_source = 2``` 。 - -将点云发送到ROS，因此设置 ```send_point_cloud_ros = true```。 - -### 3.2 设置配置文件的lidar-driver部分 - -```yaml -lidar: - - driver: - lidar_type: RS128 - frame_id: /rslidar - msop_port: 6699 - difop_port: 7788 - start_angle: 0 - end_angle: 360 - min_distance: 0.2 - max_distance: 200 - use_lidar_clock: false -``` - -将 ```lidar_type``` 设置为LiDAR类型 。 - -### 3.3 设置配置文件的lidar-ros部分 - -```yaml -ros: - ros_recv_packet_topic: /rslidar_packets - ros_send_packet_topic: /rslidar_packets - ros_send_point_cloud_topic: /rslidar_points -``` - -将 ```ros_recv_packet_topic``` 设置为rosbag中的```msop```数据的话题。 - -### 3.4 运行 - -运行程序并播放rosbag。 - - - - diff --git a/doc/howto/how_to_switch_point_type.md b/doc/howto/how_to_switch_point_type.md deleted file mode 100644 index ed90b8c9..00000000 --- a/doc/howto/how_to_switch_point_type.md +++ /dev/null @@ -1,52 +0,0 @@ -# How to switch point type - -## 1 Introduction - -This document will show you how to switch the point type. The supported point type is listed in README. To switch the type, open the ```CMakeLists.txt``` in the project and check the top of the file. Remember to **rebuild** the project after changing the point type. - -```cmake -#======================================= -# Custom Point Type (XYZI, XYZIRT) -#======================================= -set(POINT_TYPE XYZI) -``` - - - -## 2 XYZI - -The default point type is the pcl official type```pcl::PointXYZI```. Here is an example of transforming the ros point cloud to pcl point cloud. User can take this as an reference in their receiving program. - -```c++ -pcl::PointCloud::Ptr cloud_ptr (new pcl::PointCloud); -pcl::fromROSMsg(ros_msg, *cloud_ptr); -``` - - - -## 3 XYZIRT - -Since this is the custom pcl point type, user needs to add the definition of the point in the receiving program. The definition is shown below. - -```c++ -#include -#include -struct RsPointXYZIRT -{ - 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(RsPointXYZIRT, (float, x, x)(float, y, y)(float, z, z)(uint8_t, intensity, intensity)(uint16_t, ring, ring)(double, timestamp, timestamp)) - -``` - -Then user can transform the ros point cloud to pcl point cloud. - -```c++ -pcl::PointCloud::Ptr cloud_ptr (new pcl::PointCloud); -pcl::fromROSMsg(ros_msg, *cloud_ptr); -``` - diff --git a/doc/howto/how_to_switch_point_type_cn.md b/doc/howto/how_to_switch_point_type_cn.md deleted file mode 100644 index e55a43b1..00000000 --- a/doc/howto/how_to_switch_point_type_cn.md +++ /dev/null @@ -1,52 +0,0 @@ -# 如何切换点的类型 - -## 1 简介 - -本文档将会介绍如何切换点的类型。目前支持的类型在README中有列出。切换点的类型时，首先打开项目根目录下的```CMakeLists.txt```文件，在文件顶部便可以选择点的类型。在改变类型后，需要重新编译整个工程。 - -```cmake -#======================================= -# Custom Point Type (XYZI, XYZIRT) -#======================================= -set(POINT_TYPE XYZI) -``` - - - -## 2 XYZI - -默认的点的类型是pcl的官方类型```pcl::PointXYZI```. 此处给出了一个将ros点云消息转换成pcl点云的例子，用户在编写ros点云接收端时可以作为参考。 - -```c++ -pcl::PointCloud::Ptr cloud_ptr (new pcl::PointCloud); -pcl::fromROSMsg(ros_msg, *cloud_ptr); -``` - - - -## 3 XYZIRT - -由于这是速腾自定义的点的类型，用户需要在接收端程序添加对应的点的定义，点的定义如下所示。 - -```c++ -#include -#include -struct RsPointXYZIRT -{ - 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(RsPointXYZIRT, (float, x, x)(float, y, y)(float, z, z)(uint8_t, intensity, intensity)(uint16_t, ring, ring)(double, timestamp, timestamp)) - -``` - -然后即可调用pcl的转换函数将ros点云转换为pcl点云。 - -```c++ -pcl::PointCloud::Ptr cloud_ptr (new pcl::PointCloud); -pcl::fromROSMsg(ros_msg, *cloud_ptr); -``` - diff --git a/doc/howto/how_to_use_coordinate_transformation.md b/doc/howto/how_to_use_coordinate_transformation.md deleted file mode 100644 index fdc411a5..00000000 --- a/doc/howto/how_to_use_coordinate_transformation.md +++ /dev/null @@ -1,73 +0,0 @@ -# How to use the coordinate transformation function - -## 1 Introduction - - **rslidar_sdk** has the coordinate transformation function built inside and it can output the transformed point cloud directly, which can help users saving time to do transformation for point cloud (e.g. for RS128, it costs about 3~5ms to do transformation for one frame point cloud). This document will show you how to use the transformation function. - -## 2 Dependency - -- Eigen3 - - Installation： - - ```bash - sudo apt-get install libeigen3-dev - ``` - -## 3 Compile - -To enable the transformation function, the option ```ENABLE_TRANSFORM``` needs to be set to ```ON``` during the cmake process. - -- Compile directly - - ```bash - cmake -DENABLE_TRANSFORM=ON .. - make -j4 - ``` - -- ROS - - ```bash - catkin_make -DENABLE_TRANSFORM=ON - ``` - -- ROS2 - - ```bash - colcon build --cmake-args '-DENABLE_TRANSFORM=ON' - ``` - -## 4 Set up parameters - -User needs to set up the hiding parameter```x, y, z, roll, pitch ,yaw ``` in lidar part of the config.yaml. Please check the [Intro to hiding parameters](../intro/hiding_parameters_intro.md) for more details. Here is an example of the config.yaml. - -```yaml -common: - msg_source: 1 - send_packet_ros: false - send_point_cloud_ros: true - send_packet_proto: false - send_point_cloud_proto: false - pcap_path: /home/robosense/lidar.pcap -lidar: - - driver: - lidar_type: RS128 - frame_id: /rslidar - msop_port: 6699 - difop_port: 7788 - start_angle: 0 - end_angle: 360 - min_distance: 0.2 - max_distance: 200 - use_lidar_clock: false - x: 1 - y: 0 - z: 2.5 - roll: 0.1 - pitch: 0.2 - yaw: 1.57 -``` - -## 5 Run - -Run the program. \ No newline at end of file diff --git a/doc/howto/how_to_use_coordinate_transformation_cn.md b/doc/howto/how_to_use_coordinate_transformation_cn.md deleted file mode 100644 index 7c89a761..00000000 --- a/doc/howto/how_to_use_coordinate_transformation_cn.md +++ /dev/null @@ -1,77 +0,0 @@ -# 如何使用坐标变换功能 - -## 1 简介 - -**rslidar_sdk** 提供了内置的坐标变换功能，可以直接输出经过坐标变换后的点云，显著节省了用户对点云进行坐标变换的操作耗时（128线雷达一帧点云坐标变换耗时约3~5ms）。本文档将指导用户如何使用rslidar_sdk的内置坐标变换功能， 输出经过变换后的点云。 - - - -## 2 依赖介绍 - -若希望启用坐标变换功能，需要安装以下依赖。 - -- Eigen3 - - 安装方式： - - ```bash - sudo apt-get install libeigen3-dev - ``` - -## 3 编译 - -若希望启用坐标变换的功能，在编译程序时需要将```ENABLE_TRANSFORM```选项设置为```ON```. - -- 直接编译 - - ```bash - cmake -DENABLE_TRANSFORM=ON .. - make -j4 - ``` - -- ROS - - ```bash - catkin_make -DENABLE_TRANSFORM=ON - ``` - -- ROS2 - - ```bash - colcon build --cmake-args '-DENABLE_TRANSFORM=ON' - ``` - -## 4 参数设置 - -用户需要设置lidar部分的隐藏参数```x, y, z, roll, pitch ,yaw ``` ，更多的细节可以参考[隐藏参数介绍](../intro/hiding_parameters_intro.md)。此处为参数文件的一个示例，用户可根据实际情况配置。 - -```yaml -common: - msg_source: 1 - send_packet_ros: false - send_point_cloud_ros: true - send_packet_proto: false - send_point_cloud_proto: false - pcap_path: /home/robosense/lidar.pcap -lidar: - - driver: - lidar_type: RS128 - frame_id: /rslidar - msop_port: 6699 - difop_port: 7788 - start_angle: 0 - end_angle: 360 - min_distance: 0.2 - max_distance: 200 - use_lidar_clock: false - x: 1 - y: 0 - z: 2.5 - roll: 0.1 - pitch: 0.2 - yaw: 1.57 -``` - -## 5 运行 - -运行程序。 \ No newline at end of file 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 821bb8a2..00000000 --- a/doc/howto/how_to_use_multi_cast_function.md +++ /dev/null @@ -1,73 +0,0 @@ -# How to use multi-cast function - -## 1 Introduction - -This document will show you how to use rslidar_sdk 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```. - -### 2.1 Set up hiding parameters - -User need to set up the hiding parameter ```multi_cast_address``` in lidar part of the config.yaml. Please check the [Intro to hiding parameters](../intro/hiding_parameters_intro.md) for more details. Here is an example of the config.yaml. - -```yaml -common: - msg_source: 1 - send_packet_ros: false - send_point_cloud_ros: true - send_packet_proto: false - send_point_cloud_proto: false - pcap_path: /home/robosense/lidar.pcap -lidar: - - driver: - lidar_type: RS128 - frame_id: /rslidar - multi_cast_address: 224.1.1.1 - msop_port: 6699 - difop_port: 7788 - start_angle: 0 - end_angle: 360 - min_distance: 0.2 - max_distance: 200 - use_lidar_clock: false - -``` - - - -### 2.2 Set up the PC ip address - -In multi-cast case, the ip address of PC has no limit but user needs to make sure **the PC and LiDAR are in the same net work**, which means PC can ping to LiDAR. - -### 2.3 Add the PC to the group - -Use the following command to check the PC's ethernet card name: - -```bash -ifconfig -``` - -![ethernet](../img/ethernet.png) - -As the figure shows, the ethernet card name is ```enp2s0```. Then execute the following command to add the PC to the group (replace the ```enp2s0``` with your ethernet card name): - -``` -sudo route add -net 224.0.0.0/4 dev enp2s0 -``` - -### 2.4 Run - -Run the program. - - - - - - - - - - - diff --git a/doc/howto/how_to_use_multi_cast_function_cn.md b/doc/howto/how_to_use_multi_cast_function_cn.md deleted file mode 100644 index b7eee5f8..00000000 --- a/doc/howto/how_to_use_multi_cast_function_cn.md +++ /dev/null @@ -1,73 +0,0 @@ -# 如何使用组播功能 - -## 1 简介 - -本文档将演示如何使用rslidar_sdk来接收组播模式下的雷达点云。 - -## 2 步骤 (Linux) - -假设雷达的组播地址设置为```224.1.1.1```。 - -### 2.1 设置隐藏参数 - -用户首先需要将lidar部分的隐藏参数```multi_cast_address```设置为雷达的组播地址， 更多的细节可以参考[隐藏参数介绍](../intro/hiding_parameters_intro.md)，此处为参数文件的一个示例，用户可根据实际情况配置。 - -```yaml -common: - msg_source: 1 - send_packet_ros: false - send_point_cloud_ros: true - send_packet_proto: false - send_point_cloud_proto: false - pcap_path: /home/robosense/lidar.pcap -lidar: - - driver: - lidar_type: RS128 - frame_id: /rslidar - multi_cast_address: 224.1.1.1 - msop_port: 6699 - difop_port: 7788 - start_angle: 0 - end_angle: 360 - min_distance: 0.2 - max_distance: 200 - use_lidar_clock: false - -``` - - - -### 2.2 设置电脑ip地址 - -在组播模式下，电脑的ip地址并没有严格限制，但用户需要确保**电脑与雷达的网络互通**， 也就是电脑必须能够ping通雷达。 - -### 2.3 将PC添加到组内 - -使用以下指令查看电脑的网卡名: - -```bash -ifconfig -``` - -![ethernet](../img/ethernet.png) - -如图所示，网卡名为```enp2s0```。 然后运行以下指令将电脑加入组内（将指令中的```enp2s0```替换为用户电脑的网卡名: - -``` -sudo route add -net 224.0.0.0/4 dev enp2s0 -``` - -### 2.4 运行 - -运行程序。 - - - - - - - - - - - diff --git a/doc/howto/how_to_use_multi_lidars.md b/doc/howto/how_to_use_multi_lidars.md deleted file mode 100644 index 5e2eec3d..00000000 --- a/doc/howto/how_to_use_multi_lidars.md +++ /dev/null @@ -1,215 +0,0 @@ -# How to use multi-LiDARs - -## 1 Introduction - -This document will show you how to send out multi-LiDARs point cloud with only one driver running. Theoretically, one driver can decoder unlimited number of LiDARs at the same time. For convenient, we will use three LiDARs as an example. Please make sure you have read the LiDAR user-guide and [Intro to parameters](../intro/parameter_intro.md) before reading this document. - -## 2 Online connect with multi-LiDARs - -### 2.1 Get the data port number - -Please follow the instructions in LiDAR user-guide to connect the LiDAR and set up your computer's ip address. At this time, you should have already known msop port number and difop port number for each LiDAR. If you have no idea about what it is, please check the LiDAR user-guide first. - -### 2.2 Set up the common part of the config file - -```yaml -common: - msg_source: 1 - send_packet_ros: false - send_point_cloud_ros: true - send_packet_proto: false - send_point_cloud_proto: false - pcap_path: /home/robosense/lidar.pcap -``` - -Since the message come from the LiDAR, set ```msg_source = 1```. - -Send point cloud to ROS so set ```send_point_cloud_ros = true```. - -### 2.3 Set up the lidar part of the config file - -```yaml -lidar: - - driver: - lidar_type: RS128 - frame_id: /rslidar - msop_port: 6699 - difop_port: 7788 - start_angle: 0 - end_angle: 360 - min_distance: 0.2 - max_distance: 200 - use_lidar_clock: false - ros: - ros_recv_packet_topic: /middle/rslidar_packets - ros_send_packet_topic: /middle/rslidar_packets - ros_send_point_cloud_topic: /middle/rslidar_points - proto: - point_cloud_recv_port: 60021 - point_cloud_send_port: 60021 - msop_recv_port: 60022 - msop_send_port: 60022 - difop_recv_port: 60023 - difop_send_port: 60023 - point_cloud_send_ip: 127.0.0.1 - packet_send_ip: 127.0.0.1 - - driver: - lidar_type: RSBP - frame_id: /rslidar - msop_port: 1990 - difop_port: 1991 - start_angle: 0 - end_angle: 360 - min_distance: 0.2 - max_distance: 200 - use_lidar_clock: false - ros: - ros_recv_packet_topic: /left/rslidar_packets - ros_send_packet_topic: /left/rslidar_packets - ros_send_point_cloud_topic: /left/rslidar_points - proto: - point_cloud_recv_port: 60024 - point_cloud_send_port: 60024 - msop_recv_port: 60025 - msop_send_port: 60025 - difop_recv_port: 60026 - difop_send_port: 60026 - point_cloud_send_ip: 127.0.0.1 - packet_send_ip: 127.0.0.1 - - driver: - lidar_type: RSBP - frame_id: /rslidar - msop_port: 2010 - difop_port: 2011 - start_angle: 0 - end_angle: 360 - min_distance: 0.2 - max_distance: 200 - use_lidar_clock: false - ros: - ros_recv_packet_topic: /right/rslidar_packets - ros_send_packet_topic: /right/rslidar_packets - ros_send_point_cloud_topic: /right/rslidar_points - proto: - point_cloud_recv_port: 60027 - point_cloud_send_port: 60027 - msop_recv_port: 60028 - msop_send_port: 60028 - difop_recv_port: 60029 - difop_send_port: 60029 - point_cloud_send_ip: 127.0.0.1 - packet_send_ip: 127.0.0.1 -``` - -Set ```lidar_type``` for each LiDAR. - -Set ```msop_port``` and ```difop_port``` for each LiDAR. - -Set ```ros_send_point_cloud_topic``` for each LiDAR. - -### 2.4 Run - -Run the demo. - -## 3 Offline use rosbag with multi-LiDARs - -### 3.1 Set up the common part of the config file - -```yaml -common: - msg_source: 2 - send_packet_ros: false - send_point_cloud_ros: true - send_packet_proto: false - send_point_cloud_proto: false - pcap_path: /home/robosense/lidar.pcap -``` - -Since the packets message come from the ROS, set ```msg_source = 2```. - -We want to send point cloud to ROS so set ```send_point_cloud_ros = true```. - -### 3.2 Set up the lidar part of the config file - -```yaml -lidar: - - driver: - lidar_type: RS128 - frame_id: /rslidar - msop_port: 6699 - difop_port: 7788 - start_angle: 0 - end_angle: 360 - min_distance: 0.2 - max_distance: 200 - use_lidar_clock: false - ros: - ros_recv_packet_topic: /middle/rslidar_packets - ros_send_packet_topic: /middle/rslidar_packets - ros_send_point_cloud_topic: /middle/rslidar_points - proto: - point_cloud_recv_port: 60021 - point_cloud_send_port: 60021 - msop_recv_port: 60022 - msop_send_port: 60022 - difop_recv_port: 60023 - difop_send_port: 60023 - point_cloud_send_ip: 127.0.0.1 - packet_send_ip: 127.0.0.1 - - driver: - lidar_type: RSBP - frame_id: /rslidar - msop_port: 1990 - difop_port: 1991 - start_angle: 0 - end_angle: 360 - min_distance: 0.2 - max_distance: 200 - use_lidar_clock: false - ros: - ros_recv_packet_topic: /left/rslidar_packets - ros_send_packet_topic: /left/rslidar_packets - ros_send_point_cloud_topic: /left/rslidar_points - proto: - point_cloud_recv_port: 60024 - point_cloud_send_port: 60024 - msop_recv_port: 60025 - msop_send_port: 60025 - difop_recv_port: 60026 - difop_send_port: 60026 - point_cloud_send_ip: 127.0.0.1 - packet_send_ip: 127.0.0.1 - - driver: - lidar_type: RSBP - frame_id: /rslidar - msop_port: 2010 - difop_port: 2011 - start_angle: 0 - end_angle: 360 - min_distance: 0.2 - max_distance: 200 - use_lidar_clock: false - ros: - ros_recv_packet_topic: /right/rslidar_packets - ros_send_packet_topic: /right/rslidar_packets - ros_send_point_cloud_topic: /right/rslidar_points - proto: - point_cloud_recv_port: 60027 - point_cloud_send_port: 60027 - msop_recv_port: 60028 - msop_send_port: 60028 - difop_recv_port: 60029 - difop_send_port: 60029 - point_cloud_send_ip: 127.0.0.1 - packet_send_ip: 127.0.0.1 -``` - -Set ```lidar_type``` for each LiDAR. - -Set the ```ros_recv_packet_topic``` for each LiDAR which need to corresbond to the topic names in rosbag. - -Set ```ros_send_point_cloud_topic``` for each LiDAR. - -### 3.3 Run - -Run the program & play rosbag. diff --git a/doc/howto/how_to_use_multi_lidars_cn.md b/doc/howto/how_to_use_multi_lidars_cn.md deleted file mode 100644 index a9172d79..00000000 --- a/doc/howto/how_to_use_multi_lidars_cn.md +++ /dev/null @@ -1,215 +0,0 @@ -# 如何使用多雷达 - -## 1 简介 - -本文档将展示如何在仅运行一个驱动程序的情况解析并发送多台雷达的点云。理论上，一个驱动可以同时解码无限数量的雷达。为了方便起见，本文档将会使用三个雷达作为示例。在阅读本文档之前， 请确保已经阅读过雷达用户手册和[参数简介](../intro/parameter_intro.md) 。 - -## 2 在线解析多雷达 - -### 2.1 获取数据端口号 - -首先将三个雷达与计算机正确连接，此时应已知每个LiDAR的msop端口号 与 difop端口号。 如果不清楚上述内容，请查看雷达用户手册。 - -### 2.2 设置参数文件的common部分 - -```yaml -common: - msg_source: 1 - send_packet_ros: false - send_point_cloud_ros: true - send_packet_proto: false - send_point_cloud_proto: false - pcap_path: /home/robosense/lidar.pcap -``` - -由于消息来源于在线雷达，因此请设置```msg_source=1```。 - -将点云发送到ROS以查看，因此设置 ```send_point_cloud_ros = true``` 。 - -### 2.3 设置配置文件中的lidar部分 - -```yaml -lidar: - - driver: - lidar_type: RS128 - frame_id: /rslidar - msop_port: 6699 - difop_port: 7788 - start_angle: 0 - end_angle: 360 - min_distance: 0.2 - max_distance: 200 - use_lidar_clock: false - ros: - ros_recv_packet_topic: /middle/rslidar_packets - ros_send_packet_topic: /middle/rslidar_packets - ros_send_point_cloud_topic: /middle/rslidar_points - proto: - point_cloud_recv_port: 60021 - point_cloud_send_port: 60021 - msop_recv_port: 60022 - msop_send_port: 60022 - difop_recv_port: 60023 - difop_send_port: 60023 - point_cloud_send_ip: 127.0.0.1 - packet_send_ip: 127.0.0.1 - - driver: - lidar_type: RSBP - frame_id: /rslidar - msop_port: 1990 - difop_port: 1991 - start_angle: 0 - end_angle: 360 - min_distance: 0.2 - max_distance: 200 - use_lidar_clock: false - ros: - ros_recv_packet_topic: /left/rslidar_packets - ros_send_packet_topic: /left/rslidar_packets - ros_send_point_cloud_topic: /left/rslidar_points - proto: - point_cloud_recv_port: 60024 - point_cloud_send_port: 60024 - msop_recv_port: 60025 - msop_send_port: 60025 - difop_recv_port: 60026 - difop_send_port: 60026 - point_cloud_send_ip: 127.0.0.1 - packet_send_ip: 127.0.0.1 - - driver: - lidar_type: RSBP - frame_id: /rslidar - msop_port: 2010 - difop_port: 2011 - start_angle: 0 - end_angle: 360 - min_distance: 0.2 - max_distance: 200 - use_lidar_clock: false - ros: - ros_recv_packet_topic: /right/rslidar_packets - ros_send_packet_topic: /right/rslidar_packets - ros_send_point_cloud_topic: /right/rslidar_points - proto: - point_cloud_recv_port: 60027 - point_cloud_send_port: 60027 - msop_recv_port: 60028 - msop_send_port: 60028 - difop_recv_port: 60029 - difop_send_port: 60029 - point_cloud_send_ip: 127.0.0.1 - packet_send_ip: 127.0.0.1 -``` - -为每个雷达类型设置型号```lidar_type```。 - -为每个雷达设置对应的端口号 ```msop_port``` 和```difop_port``` 。 - -为每个雷达设置点云发送的话题```ros_send_point_cloud_topic```。 - -### 2.4 运行 - -运行程序。 - -## 3 离线解析多雷达rosbag - -### 3.1 设置配置文件中的common部分 - -```yaml -common: - msg_source: 2 - send_packet_ros: false - send_point_cloud_ros: true - send_packet_proto: false - send_point_cloud_proto: false - pcap_path: /home/robosense/lidar.pcap -``` - -由于数据包消息来自ROS，因此设置 ```msg_source = 2``` 。 - -将点云发送到ROS，因此设置 ```send_point_cloud_ros = true```。 - -### 3.2 设置配置文件中的lidar部分 - -```yaml -lidar: - - driver: - lidar_type: RS128 - frame_id: /rslidar - msop_port: 6699 - difop_port: 7788 - start_angle: 0 - end_angle: 360 - min_distance: 0.2 - max_distance: 200 - use_lidar_clock: false - ros: - ros_recv_packet_topic: /middle/rslidar_packets - ros_send_packet_topic: /middle/rslidar_packets - ros_send_point_cloud_topic: /middle/rslidar_points - proto: - point_cloud_recv_port: 60021 - point_cloud_send_port: 60021 - msop_recv_port: 60022 - msop_send_port: 60022 - difop_recv_port: 60023 - difop_send_port: 60023 - point_cloud_send_ip: 127.0.0.1 - packet_send_ip: 127.0.0.1 - - driver: - lidar_type: RSBP - frame_id: /rslidar - msop_port: 1990 - difop_port: 1991 - start_angle: 0 - end_angle: 360 - min_distance: 0.2 - max_distance: 200 - use_lidar_clock: false - ros: - ros_recv_packet_topic: /left/rslidar_packets - ros_send_packet_topic: /left/rslidar_packets - ros_send_point_cloud_topic: /left/rslidar_points - proto: - point_cloud_recv_port: 60024 - point_cloud_send_port: 60024 - msop_recv_port: 60025 - msop_send_port: 60025 - difop_recv_port: 60026 - difop_send_port: 60026 - point_cloud_send_ip: 127.0.0.1 - packet_send_ip: 127.0.0.1 - - driver: - lidar_type: RSBP - frame_id: /rslidar - msop_port: 2010 - difop_port: 2011 - start_angle: 0 - end_angle: 360 - min_distance: 0.2 - max_distance: 200 - use_lidar_clock: false - ros: - ros_recv_packet_topic: /right/rslidar_packets - ros_send_packet_topic: /right/rslidar_packets - ros_send_point_cloud_topic: /right/rslidar_points - proto: - point_cloud_recv_port: 60027 - point_cloud_send_port: 60027 - msop_recv_port: 60028 - msop_send_port: 60028 - difop_recv_port: 60029 - difop_send_port: 60029 - point_cloud_send_ip: 127.0.0.1 - packet_send_ip: 127.0.0.1 -``` - -为每个雷达类型设置型号```lidar_type```。 - -为每个雷达设置接收的packet话题名```ros_recv_packet_topic```。 - -为每个雷达设置点云发送的话题```ros_send_point_cloud_topic```。 - -### 3.3 运行 - -运行程序并播放rosbag。 diff --git a/doc/howto/how_to_use_protobuf_function.md b/doc/howto/how_to_use_protobuf_function.md deleted file mode 100644 index d36101e5..00000000 --- a/doc/howto/how_to_use_protobuf_function.md +++ /dev/null @@ -1,224 +0,0 @@ -# How to use protobuf functions to send & receive message - -## 1 Introduction - -Suppose there are two computers, PC-A and PC-B and they are far away from each other. You connect LiDAR with PC-A and for some reasons, you want to use point cloud message in PC-B. At this time, you may need to use the protobuf functions. Typically, there are two ways to achieve this goal. - -- PC-A send out the packets message to PC-B. PC-B receive the packet message and decode it , then PC-B get the point cloud message and use it. - -- PC-A decode the packets message, get the point cloud and send out the point cloud message to PC-B. PC-B receive the point cloud message and use it directly. - -We offer both of these two ways but we recommend first method rather than second method because the point cloud message is very large and it may take up your bandwidth. - -## 2 Send & Receive packets through Protobuf-UDP - - We suppose you have already read [Intro to parameters](../intro/parameter_intro.md) and you already have a basic idea about the config file. - -### 2.1 PC-A(Sender) - -```yaml -common: - msg_source: 1 - send_packet_ros: false - send_point_cloud_ros: false - send_packet_proto: true - send_point_cloud_proto: false - pcap_path: /home/robosense/lidar.pcap -``` - -Since the message come from the LiDAR, set ```msg_source = 1```. - -Send packets through Protobuf-UDP so set ```send_packet_proto = true```. - -```yaml -lidar: - - driver: - lidar_type: RS128 - frame_id: /rslidar - msop_port: 6699 - difop_port: 7788 - start_angle: 0 - end_angle: 360 - min_distance: 0.2 - max_distance: 200 - use_lidar_clock: false - ros: - ros_recv_packet_topic: /rslidar_packets - ros_send_packet_topic: /rslidar_packets - ros_send_point_cloud_topic: /rslidar_points - proto: - point_cloud_recv_port: 60021 - point_cloud_send_port: 60021 - msop_recv_port: 60022 - msop_send_port: 60022 - difop_recv_port: 60023 - difop_send_port: 60023 - point_cloud_send_ip: 127.0.0.1 - packet_send_ip: 127.0.0.1 -``` - -Set the ```lidar_type``` to your LiDAR type. - -Set the ```msop_port``` and ```difop_port``` to your LiDAR's port number. - -Set the ```msop_send_port```, ```difop_send_port```, and ```packet_send_ip```. - -### 2.2 PC-B(Receiver) - -```yaml -common: - msg_source: 4 - send_packet_ros: false - send_point_cloud_ros: true - send_packet_proto: false - send_point_cloud_proto: false - pcap_path: /home/robosense/lidar.pcap -``` - -Since the packets message come from protobuf-UDP, set ```msg_source = 4```. - -Send point cloud to ROS so set ```send_point_cloud_ros = true```. - -```yaml -lidar: - - driver: - lidar_type: RS128 - frame_id: /rslidar - msop_port: 6699 - difop_port: 7788 - start_angle: 0 - end_angle: 360 - min_distance: 0.2 - max_distance: 200 - use_lidar_clock: false - ros: - ros_recv_packet_topic: /rslidar_packets - ros_send_packet_topic: /rslidar_packets - ros_send_point_cloud_topic: /rslidar_points - proto: - point_cloud_recv_port: 60021 - point_cloud_send_port: 60021 - msop_recv_port: 60022 - msop_send_port: 60022 - difop_recv_port: 60023 - difop_send_port: 60023 - point_cloud_send_ip: 127.0.0.1 - packet_send_ip: 127.0.0.1 -``` - -Set the ```lidar_type``` to your LiDAR type. - -Make sure the PC-B's ip address is same as the ```packet_send_ip``` set in PC-A's config.yaml. - -Set the ```msop_recv_port``` and ```difop_recv_port``` to be same with the ```msop_send_port``` and ```difop_send_port``` set in PC-A's config.yaml. - -## 3 Send & receive point cloud through Protobuf-UDP - -We suppose you have already read [Intro to parameters](../intro/parameter_intro.md) and you already have a basic idea about the config file. - -### 3.1 PC-A(Sender) - -```yaml -common: - msg_source: 1 - send_packet_ros: false - send_point_cloud_ros: false - send_packet_proto: false - send_point_cloud_proto: true - pcap_path: /home/robosense/lidar.pcap -``` - -Since the message come from the LiDAR, set ```msg_source = 1```. - -Send point cloud through Protobuf-UDP so set ```send_point_cloud_proto = true```. - -```yaml -lidar: - - driver: - lidar_type: RS128 - frame_id: /rslidar - msop_port: 6699 - difop_port: 7788 - start_angle: 0 - end_angle: 360 - min_distance: 0.2 - max_distance: 200 - use_lidar_clock: false - ros: - ros_recv_packet_topic: /rslidar_packets - ros_send_packet_topic: /rslidar_packets - ros_send_point_cloud_topic: /rslidar_points - proto: - point_cloud_recv_port: 60021 - point_cloud_send_port: 60021 - msop_recv_port: 60022 - msop_send_port: 60022 - difop_recv_port: 60023 - difop_send_port: 60023 - point_cloud_send_ip: 127.0.0.1 - packet_send_ip: 127.0.0.1 -``` - -Set the ```lidar_type``` to your LiDAR type. - -Set the ```msop_port``` and ```difop_port``` to your LiDAR's port number. - -Set the ```point_cloud_send_port``` and ```point_cloud_send_ip``. - -### 3.2 PC-B(Receiver) - -```yaml -common: - msg_source: 5 - send_packet_ros: false - send_point_cloud_ros: true - send_packet_proto: false - send_point_cloud_proto: false - pcap_path: /home/robosense/lidar.pcap -``` - -Since the point cloud message come from protobuf-UDP, set ```msg_source = 5```. - -Send point cloud to ROS so set ```send_point_cloud_ros = true```. - -```yaml -lidar: - - driver: - lidar_type: RS128 - frame_id: /rslidar - msop_port: 6699 - difop_port: 7788 - start_angle: 0 - end_angle: 360 - min_distance: 0.2 - max_distance: 200 - use_lidar_clock: false - ros: - ros_recv_packet_topic: /rslidar_packets - ros_send_packet_topic: /rslidar_packets - ros_send_point_cloud_topic: /rslidar_points - proto: - point_cloud_recv_port: 60021 - point_cloud_send_port: 60021 - msop_recv_port: 60022 - msop_send_port: 60022 - difop_recv_port: 60023 - difop_send_port: 60023 - point_cloud_send_ip: 127.0.0.1 - packet_send_ip: 127.0.0.1 -``` - -Make sure the PC-B's ip address is same as the ```point_cloud_send_ip``` set in PC-A's config.yaml. - -Set the ```point_cloud_recv_port``` to be same with the ```point_cloud_send_port``` set in PC-A's config.yaml. - - - - - - - - - - - diff --git a/doc/howto/how_to_use_protobuf_function_cn.md b/doc/howto/how_to_use_protobuf_function_cn.md deleted file mode 100644 index ffb8dde3..00000000 --- a/doc/howto/how_to_use_protobuf_function_cn.md +++ /dev/null @@ -1,228 +0,0 @@ -# 如何使用Protobuf函数发送和接收消息 - -## 1 简介 - -假设有两台计算机，PC-A和PC-B，并且它们彼此相距很远。 将LiDAR与PC-A连接，由于某些原因，用户想在PC-B中使用点云消息。 此时，可能需要使用protobuf功能。 通常，有两种方法可以实现此目标。 - -- PC-A将雷达packet消息发送到PC-B。 PC-B收到雷达packet消息并对其进行解码，然后PC-B获得点云消息并使用它。 - -- PC-A解码雷达packet消息，获取点云并将点云消息发送到PC-B。 PC-B收到点云消息并直接使用。 - -rslidar_sdk提供这两种方式，但是通常建议使用第一种方法，因为点云消息非常大，对带宽有较高要求。 - -## 2 通过Protobuf-UDP发送和接收 packets - -​ 首先请阅读[参数简介](.. / intro / parameter_intro.md)，了解基本的参数配置。 - -### 2.1 PC-A(发送端) - -```yaml -common: - msg_source: 1 - send_packet_ros: false - send_point_cloud_ros: false - send_packet_proto: true - send_point_cloud_proto: false - pcap_path: /home/robosense/lidar.pcap -``` - -由于消息来源于在线雷达，因此请设置```msg_source=1```。 - -将packet数据通过Protobuf-UDP发出，因此设置 ```send_packet_proto = true``` 。 - -```yaml -lidar: - - driver: - lidar_type: RS128 - frame_id: /rslidar - msop_port: 6699 - difop_port: 7788 - start_angle: 0 - end_angle: 360 - min_distance: 0.2 - max_distance: 200 - use_lidar_clock: false - ros: - ros_recv_packet_topic: /rslidar_packets - ros_send_packet_topic: /rslidar_packets - ros_send_point_cloud_topic: /rslidar_points - proto: - point_cloud_recv_port: 60021 - point_cloud_send_port: 60021 - msop_recv_port: 60022 - msop_send_port: 60022 - difop_recv_port: 60023 - difop_send_port: 60023 - point_cloud_send_ip: 127.0.0.1 - packet_send_ip: 127.0.0.1 -``` - -将 ```lidar_type``` 设置为LiDAR类型 。 - - -设置 ```msop_port``` 和 ```difop_port``` 为雷达数据端口号。 - -设置 ```msop_send_port```, ```difop_send_port```, 和 ```packet_send_ip```. - -### 2.2 PC-B(接收端) - -```yaml -common: - msg_source: 4 - send_packet_ros: false - send_point_cloud_ros: true - send_packet_proto: false - send_point_cloud_proto: false - pcap_path: /home/robosense/lidar.pcap -``` - -由于packet消息来源于protobuf-UDP，因此设置 ```msg_source = 4``` 。 - -将点云发送到ROS以查看，因此设置 ```send_point_cloud_ros = true``` 。 - -```yaml -lidar: - - driver: - lidar_type: RS128 - frame_id: /rslidar - msop_port: 6699 - difop_port: 7788 - start_angle: 0 - end_angle: 360 - min_distance: 0.2 - max_distance: 200 - use_lidar_clock: false - ros: - ros_recv_packet_topic: /rslidar_packets - ros_send_packet_topic: /rslidar_packets - ros_send_point_cloud_topic: /rslidar_points - proto: - point_cloud_recv_port: 60021 - point_cloud_send_port: 60021 - msop_recv_port: 60022 - msop_send_port: 60022 - difop_recv_port: 60023 - difop_send_port: 60023 - point_cloud_send_ip: 127.0.0.1 - packet_send_ip: 127.0.0.1 -``` - -将 ```lidar_type``` 设置为LiDAR类型 。 - -确认PC-B的ip地址与PC-A的配置文件中设置的```packet_send_ip```一致。 - -将```msop_recv_port```和```difop_recv_port```与PC-A的配置文件中设置的```msop_send_port```和```difop_send_port```设置为一致。 - -## 3 通过Protobuf-UDP发送和接收点云 - -首先请阅读[参数简介](... / intro / parameter_intro.md)，了解基本的参数配置。 - -### 3.1 PC-A(发送端) - -```yaml -common: - msg_source: 1 - send_packet_ros: false - send_point_cloud_ros: false - send_packet_proto: false - send_point_cloud_proto: true - pcap_path: /home/robosense/lidar.pcap -``` - -由于消息来源于在线雷达，因此请设置```msg_source=1```。 - -将点云数据通过Protobuf-UDP发出，因此设置 ```send_point_cloud_proto = true``` 。 - -```yaml -lidar: - - driver: - lidar_type: RS128 - frame_id: /rslidar - msop_port: 6699 - difop_port: 7788 - start_angle: 0 - end_angle: 360 - min_distance: 0.2 - max_distance: 200 - use_lidar_clock: false - ros: - ros_recv_packet_topic: /rslidar_packets - ros_send_packet_topic: /rslidar_packets - ros_send_point_cloud_topic: /rslidar_points - proto: - point_cloud_recv_port: 60021 - point_cloud_send_port: 60021 - msop_recv_port: 60022 - msop_send_port: 60022 - difop_recv_port: 60023 - difop_send_port: 60023 - point_cloud_send_ip: 127.0.0.1 - packet_send_ip: 127.0.0.1 -``` - -将 ```lidar_type``` 设置为LiDAR类型 。 - - -设置 ```msop_port``` 和 ```difop_port``` 为雷达数据端口号。 - -设置 ```point_cloud_send_port``` 和 ```point_cloud_send_ip```. - -### 3.2 PC-B(接收端) - -```yaml -common: - msg_source: 5 - send_packet_ros: false - send_point_cloud_ros: true - send_packet_proto: false - send_point_cloud_proto: false - pcap_path: /home/robosense/lidar.pcap -``` - -由于点云消息来源于protobuf-UDP，因此设置 ```msg_source = 5``` 。 - -将点云发送到ROS以查看，因此设置 ```send_point_cloud_ros = true``` 。 - -```yaml -lidar: - - driver: - lidar_type: RS128 - frame_id: /rslidar - msop_port: 6699 - difop_port: 7788 - start_angle: 0 - end_angle: 360 - min_distance: 0.2 - max_distance: 200 - use_lidar_clock: false - ros: - ros_recv_packet_topic: /rslidar_packets - ros_send_packet_topic: /rslidar_packets - ros_send_point_cloud_topic: /rslidar_points - proto: - point_cloud_recv_port: 60021 - point_cloud_send_port: 60021 - msop_recv_port: 60022 - msop_send_port: 60022 - difop_recv_port: 60023 - difop_send_port: 60023 - point_cloud_send_ip: 127.0.0.1 - packet_send_ip: 127.0.0.1 -``` - -确认PC-B的ip地址与PC-A的配置文件中设置的```point_cloud_send_ip```一致。 - -将```point_cloud_recv_port```与PC-A的配置文件中设置的```point_cloud_send_port```设置为一致。 - - - - - - - - - - - - - diff --git a/doc/howto/img/04_01_packet_rosbag.png b/doc/howto/img/04_01_packet_rosbag.png new file mode 100644 index 00000000..90d823b4 Binary files /dev/null and b/doc/howto/img/04_01_packet_rosbag.png differ diff --git a/doc/howto/img/07_01_broadcast.png b/doc/howto/img/07_01_broadcast.png new file mode 100644 index 00000000..ebf2f4cb Binary files /dev/null and b/doc/howto/img/07_01_broadcast.png differ diff --git a/doc/howto/img/07_02_unicast.png b/doc/howto/img/07_02_unicast.png new file mode 100644 index 00000000..65964a26 Binary files /dev/null and b/doc/howto/img/07_02_unicast.png differ diff --git a/doc/howto/img/07_03_multicast.png b/doc/howto/img/07_03_multicast.png new file mode 100644 index 00000000..fc39a8dc Binary files /dev/null and b/doc/howto/img/07_03_multicast.png differ diff --git a/doc/howto/img/07_04_multi_lidars_port.png b/doc/howto/img/07_04_multi_lidars_port.png new file mode 100644 index 00000000..c426ca92 Binary files /dev/null and b/doc/howto/img/07_04_multi_lidars_port.png differ diff --git a/doc/howto/img/07_05_multi_lidars_ip.png b/doc/howto/img/07_05_multi_lidars_ip.png new file mode 100644 index 00000000..48e1e138 Binary files /dev/null and b/doc/howto/img/07_05_multi_lidars_ip.png differ diff --git a/doc/howto/img/07_06_vlan_layer.png b/doc/howto/img/07_06_vlan_layer.png new file mode 100644 index 00000000..8d678fcc Binary files /dev/null and b/doc/howto/img/07_06_vlan_layer.png differ diff --git a/doc/howto/img/07_07_vlan.png b/doc/howto/img/07_07_vlan.png new file mode 100644 index 00000000..55f224b4 Binary files /dev/null and b/doc/howto/img/07_07_vlan.png differ diff --git a/doc/howto/img/07_08_user_layer.png b/doc/howto/img/07_08_user_layer.png new file mode 100644 index 00000000..d7b31a08 Binary files /dev/null and b/doc/howto/img/07_08_user_layer.png differ diff --git a/doc/img/ethernet.png b/doc/img/ethernet.png deleted file mode 100644 index 329d7c81..00000000 Binary files a/doc/img/ethernet.png and /dev/null differ diff --git a/doc/intro/02_parameter_intro.md b/doc/intro/02_parameter_intro.md new file mode 100644 index 00000000..8b92ab8e --- /dev/null +++ b/doc/intro/02_parameter_intro.md @@ -0,0 +1,194 @@ +# 2 Introduction to Parameters + +rslidar_sdk reads parameters from the configuration file ```config.yaml```, which is stored in ```rslidar_sdk/config```. + +`config.yaml` contains two parts, the `common` part and the `lidar` part. + +rslidar_sdk supports multi-LiDARs case. The `common` part is shared by all LiDARs, while in the `lidar` part, each child node is for an individual Lidar. + +**config.yaml is indentation sensitive! Please make sure the indentation is not changed after adjusting the parameters!** + + + +## 2.1 Common + +The `common` part specifies the source of LiDAR packets, and where to publish point clouds and packets. + +```yaml +common: + msg_source: 1 + send_packet_ros: false + send_point_cloud_ros: false +``` + +- msg_source + + - 0 -- Unused. Never set this parameter to 0. + + - 1 -- LiDAR packets come from on-line LiDARs. For more details, please refer to [Connect to online LiDAR and send point cloud through ROS](../howto/06_how_to_decode_online_lidar.md) + + - 2 -- LiDAR packets come from ROS/ROS2. It is used to decode from an off-line rosbag. For more details, please refer to [Record rosbag & Replay it](../howto/11_how_to_record_replay_packet_rosbag.md) + + - 3 -- LiDAR packets come from a PCAP bag. For more details, please refer to [Decode PCAP file and send point cloud through ROS](../howto/08_how_to_decode_pcap_file.md) + +- send_packet_ros + + - true -- LiDAR packets will be sent to ROS/ROS2. + +*The ROS Packet message is of a customized message type, so you can't print its content via the ROS `echo` command. This option is used to record off-line Packet rosbags. For more details, please refer to the case of msg_source=2.* + +- send_point_cloud_ros + + - true -- The LiDAR point cloud will be sent to ROS/ROS2. + + *The ROS point cloud type is the ROS official defined type -- sensor_msgs/PointCloud2, so it can be visualized on the ROS `rviz` tool directly. It is not suggested to record the point cloud to rosbag, because its size may be very large. Please record Packets instead. Refer to the case of msg_source=2.* + + + +## 2.2 lidar + +The `lidar` part needs to be adjusted for every LiDAR seperately. + +```yaml +lidar: + - driver: + lidar_type: RS128 + msop_port: 6699 + difop_port: 7788 + start_angle: 0 + end_angle: 360 + min_distance: 0.2 + max_distance: 200 + use_lidar_clock: false + dense_points: false + pcap_path: /home/robosense/lidar.pcap + ros: + ros_frame_id: rslidar + ros_recv_packet_topic: /rslidar_packets + ros_send_packet_topic: /rslidar_packets + ros_send_point_cloud_topic: /rslidar_points +``` + +- lidar_type + + Supported LiDAR types are listed in the README file. + +- msop_port, difop_port + + The MSOP port and DIFOP port to receive LiDAR packets. *If no data is received, please check these parameters first.* + +- start_angle, end_angle + + The start angle and end angle of the point cloud, which should be in the range of 0~360°. *`start_angle` can be larger than `end_angle`*. + +- min_distance, max_distance + + The minimum distance and maximum distance of the point cloud. + +- use_lidar_clock + + - true -- Use the Lidar clock as the message timestamp + - false -- Use the host machine clock as the message timestamp + +- dense_points + + Whether to discard NAN points. The default value is ```false```. + - Discard if ```true``` + - reserve if ```false```. + +- pcap_path + + The full path of the PCAP file. Valid if msg_source = 3. + +- ros_send_by_rows + + Meaningful only for Mechanical Lidars, and valid if dense_points = false。 + - true -- send point cloud row by row + - false -- send point cloud clolumn by column + + + +## 2.3 Examples + +### 2.3.1 Single Lidar Case + +Connect to 1 LiDAR of RS128, and send point cloud to ROS. + +```yaml +common: + msg_source: 1 + send_packet_ros: false + send_point_cloud_ros: true +lidar: + - driver: + lidar_type: RS128 + msop_port: 6699 + difop_port: 7788 + start_angle: 0 + end_angle: 360 + min_distance: 0.2 + max_distance: 200 + use_lidar_clock: false + ros: + ros_frame_id: rslidar + ros_recv_packet_topic: /rslidar_packets + ros_send_packet_topic: /rslidar_packets + ros_send_point_cloud_topic: /rslidar_points +``` + +### 2.3.2 Multi Lidar Case + +Connect to 1 LiDAR of RS128, and 2 LiDARs of RSBP, and send point cloud to ROS. + +*Pay attention to the indentation of the `lidar` part* + +```yaml +common: + msg_source: 1 + send_packet_ros: false + send_point_cloud_ros: true +lidar: + - driver: + lidar_type: RS128 + msop_port: 6699 + difop_port: 7788 + start_angle: 0 + end_angle: 360 + min_distance: 0.2 + max_distance: 200 + use_lidar_clock: false + ros: + ros_frame_id: rslidar + ros_recv_packet_topic: /middle/rslidar_packets + ros_send_packet_topic: /middle/rslidar_packets + ros_send_point_cloud_topic: /middle/rslidar_points + - driver: + lidar_type: RSBP + msop_port: 1990 + difop_port: 1991 + start_angle: 0 + end_angle: 360 + min_distance: 0.2 + max_distance: 200 + use_lidar_clock: false + ros: + ros_frame_id: rslidar + ros_recv_packet_topic: /left/rslidar_packets + ros_send_packet_topic: /left/rslidar_packets + ros_send_point_cloud_topic: /left/rslidar_points + - driver: + lidar_type: RSBP + msop_port: 2010 + difop_port: 2011 + start_angle: 0 + end_angle: 360 + min_distance: 0.2 + max_distance: 200 + use_lidar_clock: false + ros: + ros_frame_id: rslidar + ros_recv_packet_topic: /right/rslidar_packets + ros_send_packet_topic: /right/rslidar_packets + ros_send_point_cloud_topic: /right/rslidar_points +``` + diff --git a/doc/intro/02_parameter_intro_CN.md b/doc/intro/02_parameter_intro_CN.md new file mode 100644 index 00000000..f96389b3 --- /dev/null +++ b/doc/intro/02_parameter_intro_CN.md @@ -0,0 +1,195 @@ +# 2 参数介绍 + +rslidar_sdk读取配置文件 ```config.yaml```，得到所有的参数。```config.yaml```在```rslidar_sdk/config```文件夹中。 + +**config.yaml遵循YAML格式。该格式对缩进有严格要求。修改config.yaml之后，请确保每行开头的缩进仍保持一致！** + +config.yaml包括两部分：common部分 和 lidar部分。 + +rslidar_sdk支持多个雷达。common部分为所有雷达共享。lidar部分，每一个子节点对应一个雷达，针对这个雷达的实际情况分别设置。 + + + +## 2.1 common部分 + +common部分设置雷达消息的源（Packet或点云从哪来）和目标（Packet或点云发布到哪去）。 + +```yaml +common: + msg_source: 1 + send_packet_ros: false + send_point_cloud_ros: false +``` + +- msg_source + + - 1 -- 连接在线雷达。更多使用细节，请参考[连接在线雷达并发送点云到ROS](../howto/06_how_to_decode_online_lidar_CN.md)。 + + - 2 -- 离线解析ROS/ROS2的Packet包。更多使用细节，请参考 [录制ROS数据包然后播放它](../howto/11_how_to_record_replay_packet_rosbag_CN.md)。 + + - 3 -- 离线解析PCAP包。更多使用细节，请参考[离线解析PCAP包并发送点云到ROS](../howto/08_how_to_decode_pcap_file_CN.md)。 + +- send_packet_ros + + - true -- 雷达Packet消息将通过ROS/ROS2发出 + + *雷达ROS packet消息为速腾聚创自定义ROS消息，用户使用ROS/ROS2 echo命令不能查看消息的具体内容。这个功能用于录制ROS/ROS2的Packet包，更多使用细节，请参考msg_source=2的情况。 + +- send_point_cloud_ros + + - true -- 雷达点云消息将通过ROS/ROS2发出 + + *点云消息的类型为ROS官方定义的点云类型sensor_msgs/PointCloud2, 用户可以使用Rviz直接查看点云。用户可以录制ROS/ROS2的点云包，但点云包的体积非常大，所以不建议这么做。更好的方式是录制Packet包，请参考send_packet_ros=true的情况。* + + + + + +## 2.2 lidar部分 + +lidar部分根据每个雷达的实际情况进行设置。 + +```yaml +lidar: + - driver: + lidar_type: RS128 + msop_port: 6699 + difop_port: 7788 + start_angle: 0 + end_angle: 360 + min_distance: 0.2 + max_distance: 200 + use_lidar_clock: false + dense_points: false + pcap_path: /home/robosense/lidar.pcap + ros: + ros_send_by_rows: false + ros_frame_id: rslidar + ros_recv_packet_topic: /rslidar_packets + ros_send_packet_topic: /rslidar_packets + ros_send_point_cloud_topic: /rslidar_points +``` + +- lidar_type + + 支持的雷达型号在rslidar_sdk的README文件中列出。 + +- msop_port, difop_port + + 接收MSOP/DIFOP Packet的msop端口号和difop端口号。 *若收不到消息，请优先确认这两个参数是否配置正确。* + +- start_angle, end_angle + + 点云消息的起始角度和结束角度。这个设置是软件屏蔽，将区域外的点设置为NAN点，不会减小每帧点云的体积。 start_angle和end_angle的范围是0~360°，**起始角可以大于结束角**. + +- min_distance, max_distance + + 点云的最小距离和最大距离。这个设置是软件屏蔽，会将区域外的点设置为NAN点，不会减小每帧点云的体积。 + +- use_lidar_clock + + - true -- 使用雷达时间作为消息时间戳。 + - false -- 使用电脑主机时间作为消息时间戳。 + +- dense_points + + 输出的点云中是否剔除NAN points。默认值为false。 + - true 为剔除， + - false为不剔除。 + +- pcap_path + + pcap包的路径。当 msg_source=3 时有效。 + +- ros_send_by_rows + + 只对机械式雷达有意义，且只有当dense_points = false时才有效。 + - true -- 发送点云时，按照一行一行的顺序排列点 + - false -- 发送点云时，按照一列一列的顺序排列点 + + + +## 2.3 示例 + +### 2.3.1 单台雷达 + +在线连接1台RS128雷达，并发送点云到ROS。 + +```yaml +common: + msg_source: 1 + send_packet_ros: false + send_point_cloud_ros: true +lidar: + - driver: + lidar_type: RS128 + msop_port: 6699 + difop_port: 7788 + start_angle: 0 + end_angle: 360 + min_distance: 0.2 + max_distance: 200 + use_lidar_clock: false + ros: + ros_frame_id: rslidar + ros_recv_packet_topic: /rslidar_packets + ros_send_packet_topic: /rslidar_packets + ros_send_point_cloud_topic: /rslidar_points +``` + +### 2.3.2 单台雷达 + +在线连接1台RS128雷达和2台RSBP雷达，发送点云到ROS。 + +*注意lidar部分参数的缩进* + +```yaml +common: + msg_source: 1 + send_packet_ros: false + send_point_cloud_ros: true +lidar: + - driver: + lidar_type: RS128 + msop_port: 6699 + difop_port: 7788 + start_angle: 0 + end_angle: 360 + min_distance: 0.2 + max_distance: 200 + use_lidar_clock: false + ros: + ros_frame_id: rslidar + ros_recv_packet_topic: /middle/rslidar_packets + ros_send_packet_topic: /middle/rslidar_packets + ros_send_point_cloud_topic: /middle/rslidar_points + - driver: + lidar_type: RSBP + msop_port: 1990 + difop_port: 1991 + start_angle: 0 + end_angle: 360 + min_distance: 0.2 + max_distance: 200 + use_lidar_clock: false + ros: + ros_frame_id: rslidar + ros_recv_packet_topic: /left/rslidar_packets + ros_send_packet_topic: /left/rslidar_packets + ros_send_point_cloud_topic: /left/rslidar_points + - driver: + lidar_type: RSBP + msop_port: 2010 + difop_port: 2011 + start_angle: 0 + end_angle: 360 + min_distance: 0.2 + max_distance: 200 + use_lidar_clock: false + ros: + ros_frame_id: rslidar + ros_recv_packet_topic: /right/rslidar_packets + ros_send_packet_topic: /right/rslidar_packets + ros_send_point_cloud_topic: /right/rslidar_points +``` + diff --git a/doc/intro/03_hiding_parameters_intro.md b/doc/intro/03_hiding_parameters_intro.md new file mode 100644 index 00000000..0e2388ef --- /dev/null +++ b/doc/intro/03_hiding_parameters_intro.md @@ -0,0 +1,74 @@ +# 3 Introduction to hidden parameters + +In order to make the configuration file as simple as possible, we hide some parameters and use default values for them. + +This document explains the meanings of these hidden parameters. + + + +## 3.1 common + +```yaml +common: + msg_source: 1 + send_packet_ros: false + send_point_cloud_ros: false + send_packet_proto: false + send_point_cloud_proto: false +``` + + + +## 3.2 lidar + +```yaml +lidar: + - driver: + lidar_type: RS128 + msop_port: 6699 + difop_port: 7788 + start_angle: 0 + end_angle: 360 + min_distance: 0.2 + max_distance: 200 + use_lidar_clock: false + pcap_path: /home/robosense/lidar.pcap + pcap_repeat: true + pcap_rate: 1 + config_from_file: false + angle_path: /home/robosense/angle.csv + dense_points: false + ts_first_point: false + split_frame_mode: 1 + split_angle: 0 + num_blks_split: 1 + wait_for_difop: true + group_address: 0.0.0.0 + host_address: 0.0.0.0 + x: 0 + y: 0 + z: 0 + roll: 0 + pitch: 0 + yaw: 0 + use_vlan: false +``` + +- ```pcap_repeat``` -- The default value is ```true```. Set it to ```false``` to prevent play pcap repeatedly. +- ```pcap_rate``` -- The default value is ```1```. The frequency of point cloud is about 10hz. The larger the value is, the faster the pcap bag is played. +- ```config_from_file``` -- Whether to read Lidar configuration from file. Only used for debug purpose, and can be ignored. +- ```angle_path``` -- The path of the angle.csv. Only used for debug purpose and can be ignored. +- ```ts_first_point``` -- Stamp the point cloud with the first point or the last one. Stamp with the first point if ```true```, else stamp with the last point if ```false```. The default value is ```false```. +- ```split_frame_mode``` -- The way to split the LiDAR frames. Default value is ```1```. + - 1 -- Split frame depending on the split_angle + - 2 -- Split frame depending on a fixed number of blocks + - 3 -- Split frame depending on num_blks_split + +- ```split_angle``` -- The angle(in degree) to split frames. Only be used when ```split_frame_mode = 1```. The default value is ```0```. +- ```num_blks_split``` -- The number of blocks in one frame. Only be used when ```split_frame_mode = 3```. +- ```wait_for_difop``` -- If ```false```, the driver will not wait for difop packet(including lidar configuration data, especially angle data to calculate x, y, z), and send out the point cloud immediately. The default value is ```true```. +- ```group_address``` -- If use multi-cast function, this parameter needs to be set correctly. For more details, please refer to [Online LiDAR - Advanced Topics](../howto/07_online_lidar_advanced_topics.md) +- ```host_address``` -- Needed in two conditions. If the host receives packets from multiple Lidars via different IP addresses, use this parameter to specify destination IPs of the Lidars; If group_address is set, it should be set, so it will be joined into the multicast group. +- ```x, y, z, roll, pitch, yaw ``` -- The parameters to do coordinate transformation. If the coordinate transformation function is enabled in driver core, the output point cloud will be transformed based on these parameters. For more details, please refer to [Coordinate Transformation](../howto/10_how_to_use_coordinate_transformation.md) +- ```use_vlan``` -- Whether to use VLAN. The default value is ```false```. This parameter is only needed for pcap file. If it contains packets with VLAN layer, ```use_vlan``` should set to true. In the case of online Lidar, the VLAN layer is stripped by the protocol layer, so use_vlan can be ignored. + diff --git a/doc/intro/03_hiding_parameters_intro_CN.md b/doc/intro/03_hiding_parameters_intro_CN.md new file mode 100644 index 00000000..0a1900ad --- /dev/null +++ b/doc/intro/03_hiding_parameters_intro_CN.md @@ -0,0 +1,70 @@ +# 3 隐藏参数介绍 + +为了使配置文件config.yaml尽可能简洁，我们隐藏了部分不常用的参数，在代码中使用默认值。 + +本文档将详细介绍这些隐藏参数。用户可根据需要，将它们加入参数文件，重新设置。 + + + +## 3.1 common + +```yaml +common: + msg_source: 1 + send_packet_ros: false + send_point_cloud_ros: false +``` + + + +## 3.2 lidar + +```yaml +lidar: + - driver: + lidar_type: RS128 + msop_port: 6699 + difop_port: 7788 + pcap_path: /home/robosense/lidar.pcap + pcap_repeat: true + pcap_rate: 1 + start_angle: 0 + end_angle: 360 + min_distance: 0.2 + max_distance: 200 + use_lidar_clock: false + config_from_file: false + angle_path: /home/robosense/angle.csv + dense_points: false + ts_first_point: false + split_frame_mode: 1 + split_angle: 0 + num_blks_split: 1 + wait_for_difop: true + group_address: 0.0.0.0 + host_address: 0.0.0.0 + x: 0 + y: 0 + z: 0 + roll: 0 + pitch: 0 + yaw: 0 + use_vlan: false +``` + +- ```pcap_repeat``` -- 默认值为true， 用户可将其设置为false来禁用pcap循环播放功能。 +- ```pcap_rate``` -- 默认值为1，点云频率约为10hz。 用户可调节此参数来控制pcap播放速度，设置的值越大，pcap播放速度越快。 +- ```config_from_file``` -- 默认值为false, 是否从外参文件读入雷达配置信息，仅用于调试，可忽略。 +- ```angle_path``` -- angle.csv外参文件的路径，仅用于调试，可忽略。 +- ```ts_first_point``` -- 默认值为false。点云的时间戳是否第一个点的时间。```true```为第一个点的时间，```false```为最后一个点的时间。 +- ```split_frame_mode``` -- 分帧模式设置，默认值为```1```。 + - 1 -- 角度分帧 + - 2 -- 固定block数分帧 + - 3 -- 自定义block数分帧 +- ```split_angle``` -- 用于分帧的角度(单位为度)， 在```split_frame_mode = 1``` 时才生效，默认值为```0```。 +- ```num_blks_split``` -- 用于分帧的包数，在 ```split_frame_mode = 3```时才生效，默认值为1。 +- ```wait_for_difop``` -- 若设置为false， 驱动将不会等待DIFOP包（包含配置数据，尤其是角度信息），而是立即解析MSOP包并发出点云。 默认值为```true```，也就是必须要有DIFOP包才会进行点云解析。 +- ```group_address``` -- 如果雷达为组播模式，此参数需要被设置为组播的地址。具体使用方式可以参考[在线雷达 - 高级主题](../howto/07_online_lidar_advanced_topics_CN.md) 。 +- ```host_address``` -- 有两种情况需要这个选项。如果主机上通过多个IP地址接收多个雷达的数据，则可以将此参数指定为雷达的目标IP；如果设置了group_address，那也需要设置host_address，以便将这个IP地址的网卡加入组播组。 +- ```x, y, z, roll, pitch, yaw ``` -- 坐标变换参数，若启用了内核的坐标变换功能，将会使用此参数输出经过变换后的点云。x, y, z, 单位为```米```, roll, pitch, yaw, 单位为```弧度```。具体使用方式可以参考 [坐标变换功能](../howto/10_how_to_use_coordinate_transformation_CN.md) 。 +- ```use_vlan``` -- 默认为false，指定是否使用vlan。如果pcap文件中的packet带vlan层，则需要设置这个选项为true。其他情况下不需要。在线雷达的情况下，协议层到达驱动时，已经剥离vlan层，所以不需要设置这个选项。 diff --git a/doc/intro/hiding_parameters_intro.md b/doc/intro/hiding_parameters_intro.md deleted file mode 100644 index 6d2cb532..00000000 --- a/doc/intro/hiding_parameters_intro.md +++ /dev/null @@ -1,67 +0,0 @@ -# Introduction to hidden parameters - -In order to make the config file as simple as possible, we selectively hide some of the parameters and give them a default value in the program. If not added to ```config.yaml```, the default values will be used. This document explains the meanings of these these hidden parameters. - - - -## 1 common - -```yaml -common: - msg_source: 1 - send_packet_ros: false - send_point_cloud_ros: false - send_packet_proto: false - send_point_cloud_proto: false - pcap_path: /home/robosense/lidar.pcap - pcap_repeat: true - pcap_rate: 1 -``` - -- ```pcap_repeat``` -- The default value is ```true``` , you can add it back and set to false to prevent pcap read repeatedly. - -- ```pcap_rate``` -- The default value is ```1``` and the point cloud frequency is about 10hz. The larger the value is set, the faster the pcap bag is played. - - - -## 2 lidar - -```yaml -lidar: - - driver: - lidar_type: RS128 - frame_id: /rslidar - msop_port: 6699 - difop_port: 7788 - start_angle: 0 - end_angle: 360 - min_distance: 0.2 - max_distance: 200 - use_lidar_clock: false - angle_path: /home/robosense/angle.csv - split_frame_mode: 1 - cut_angle: 0 - num_pkts_split: 1 - wait_for_difop: true - saved_by_rows: false - multi_cast_address: 0.0.0.0 - x: 0 - y: 0 - z: 0 - roll: 0 - pitch: 0 - yaw: 0 -``` - -- ```angle_path``` -- The path of the angle.csv. For latest version of LiDARs, this parameter can be ignored. -- ```split_frame_mode``` -- The mode to split the LiDAR frames. Default value is ```1```. - - 1 -- Spliting frames depending on the cut_angle - - 2 -- Spliting frames depending on a fixed number of packets - - 3 -- Spliting frames depending on num_pkts_split -- ```cut_angle``` -- The angle(degree) to split frames. Only be used when ```split_frame_mode = 1```. The default value is ```0```. -- ```num_pkts_split``` -- The number of packets in one frame. Only be used when ```split_frame_mode = 3```. -- ```wait_for_difop``` -- If set to false, the driver will not wait for difop packet and send out the point cloud immediately. The default value is ```true```. -- ```saved_by_rows``` -- The default point cloud is stored in **column major order**, which means if there is a point msg.point_cloud_ptr->at(i) , the next point on the same ring should be msg.point_cloud_ptr->at(i+msg.height). If this parameter is set to ```true``` , the point cloud will be stored in **row major order**. -- ```multi_cast_address``` -- If use multi-cast function, this parameter need to be set correctly. For more details, please refer to [Multi-Cast](../howto/how_to_use_multi_cast_function.md) - -- ```x, y, z, roll, pitch, yaw ``` -- The parameters to do cooridiante transformation. If the coordinate transformation function is enabled in driver core, the output point cloud will be transformed based on these parameters. For more details, please refer to [Coordinate Transformation](../howto/how_to_use_coordinate_transformation.md) \ No newline at end of file diff --git a/doc/intro/hiding_parameters_intro_cn.md b/doc/intro/hiding_parameters_intro_cn.md deleted file mode 100644 index 3cb9738d..00000000 --- a/doc/intro/hiding_parameters_intro_cn.md +++ /dev/null @@ -1,65 +0,0 @@ -# 隐藏参数介绍 - -为了让参数配置文件尽可能简洁，我们选择隐藏了部分用户不常用到的参数并在程序内给予了它们默认值。 本文档将详细介绍这些隐藏参数，用户可自行选择是否要将它们添加回参数文件内。 - -## 1 common - -```yaml -common: - msg_source: 1 - send_packet_ros: false - send_point_cloud_ros: false - send_packet_proto: false - send_point_cloud_proto: false - pcap_path: /home/robosense/lidar.pcap - pcap_repeat: true - pcap_rate: 1 -``` - -- ```pcap_repeat``` -- 默认值为true， 用户可将其设置为false来禁用pcap循环播放功能。 - -- ```pcap_rate``` -- 默认值为1，点云频率约为10hz。 用户可调节此参数来控制pcap播放速度，设置的值越大，pcap播放速度越快。 - - - -## 2 lidar - -```yaml -lidar: - - driver: - lidar_type: RS128 - frame_id: /rslidar - msop_port: 6699 - difop_port: 7788 - start_angle: 0 - end_angle: 360 - min_distance: 0.2 - max_distance: 200 - use_lidar_clock: false - angle_path: /home/robosense/angle.csv - split_frame_mode: 1 - cut_angle: 0 - num_pkts_split: 1 - wait_for_difop: true - saved_by_rows: false - multi_cast_address: 0.0.0.0 - x: 0 - y: 0 - z: 0 - roll: 0 - pitch: 0 - yaw: 0 -``` - -- ```angle_path``` -- angle.csv外参文件的路径，仅用于调试，可忽略。 -- ```split_frame_mode``` -- 分帧模式设置，默认值为```1```。 - - 1 -- 角度分帧 - - 2 -- 固定包数分帧 - - 3 -- 自定义包数分帧 -- ```cut_angle``` -- 用于分帧的角度(单位为度)， 在```split_frame_mode = 1``` 时才生效，默认值为```0```。 -- ```num_pkts_split``` -- 用于分帧的包数，在 ```split_frame_mode = 3```时才生效，默认值为1。 -- ```wait_for_difop``` -- 若设置为false， 驱动将不会等待difop包而是立即解析并发出点云。 默认值为```true```，也就是必须要有difop包才会进行点云解析。 -- ```saved_by_rows``` -- 点云的默认储存方式为```按列储存```，也就是说假设有一个点msg.point_cloud_ptr->at(i) ，那么与这个点同一行的下一个点应该为msg.point_cloud_ptr->at(i+msg.height)。如果此参数设置为```true```,那么输出的点云将会```按行储存```。 -- ```multi_cast_address``` -- 如果雷达为组播模式，此参数需要被设置为组播的地址。具体使用方式可以参考 [组播模式](../howto/how_to_use_multi_cast_function_cn.md) - -- ```x, y, z, roll, pitch, yaw ``` -- 坐标变换参数，若启用了内核的坐标变换功能，将会使用此参数输出经过变换后的点云。x, y, z, 单位为```米```, roll, pitch, yaw, 单位为```弧度```。具体使用方式可以参考 [坐标变换功能](../howto/how_to_use_coordinate_transformation_cn.md) \ No newline at end of file diff --git a/doc/intro/parameter_intro.md b/doc/intro/parameter_intro.md deleted file mode 100644 index 526dbfd5..00000000 --- a/doc/intro/parameter_intro.md +++ /dev/null @@ -1,251 +0,0 @@ -# Parameters Introduction - -There is only one configure file ```config.yaml```, which is stored in ```rslidar_sdk/config```. The ```config.yaml``` can be divided into two parts, the common part and the lidar part . - -*In multi-LiDARs case, the parameters in common part will be shared by all LiDARs, while the parameters in lidar part need to be adjust for each LiDAR.* - -**config.yaml is indentation sensitive! Please make sure the indentation is not changed when adjusting the parameters!** - - - -## 1 Common - -This part is used to decide the source of LiDAR data, and whether to publish point clouds or not. - -```yaml -common: - msg_source: 1 - send_packet_ros: false - send_point_cloud_ros: false - send_packet_proto: false - send_point_cloud_proto: false - pcap_path: /home/robosense/lidar.pcap -``` - -- msg_source - - - 0 -- Not use lidar. Basically you will never set this parameter to 0. - - - 1 -- When connecting with a running lidar, set to 1. For more details, please refer to [Online connect lidar and send point cloud through ROS](../howto/how_to_online_send_point_cloud_ros.md) - - - 2 -- The lidar packet come from ROS or ROS2. This will be used in offline decode rosbag. For more details, please refer to [Record rosbag & Offline decode rosbag](../howto/how_to_record_and_offline_decode_rosbag.md) - - - 3 -- The lidar packet come from offline pcap bag. For more details, please refer to [Decode pcap bag and send point cloud through ROS](../howto/how_to_offline_decode_pcap.md) - - - 4 -- The lidar packet come from Protobuf-UDP. For more details, please refer to [Use protobuf send & receive](../howto/how_to_use_protobuf_function.md) - - - 5 -- The lidar point cloud come from Protobuf-UDP. For more details, please refer to [Use protobuf send & receive](../howto/how_to_use_protobuf_function.md) - -- send_packet_ros - - - true -- The lidar packets will be sent to ROS or ROS2. - - -*Since the ROS packet message is of a customized message type, you can't directly echo the topic through ROS. Mostly the packets are only used to record offline bag because the size is much smaller than point cloud.* - -- send_point_cloud_ros - - - true -- The lidar point cloud will be sent to ROS or ROS2. - - - *The ROS point cloud type is the ROS official defined type -- sensor_msgs/PointCloud2, which means the point cloud can be visualized on ROS-Rviz directly. Also you can record the point cloud to rosbag but its size may be very large, that's why we suggest to record packets.* - -- send_packet_proto - - - true -- The lidar packets will be sent out as protobuf message through ethernet by UDP protocal. - -- send_point_cloud_proto - - - true -- The lidar point cloud will be sent out as protobuf message through ethernet in UDP protocal. - - -*We suggest sending packets rather than point clouds through ethernet by protobuf because point cloud size is too large which may take up a lot of bandwidth.* - -- pcap_path - - If the msg_source = 3, please make sure the pcap_path is correct, otherwise this paramter can be igonred. - - - -## 2 lidar - -This part needs to be adjusted for every LiDAR seperately (in multi-LiDARs case). - -```yaml -lidar: - - driver: - lidar_type: RS128 - frame_id: /rslidar - msop_port: 6699 - difop_port: 7788 - start_angle: 0 - end_angle: 360 - min_distance: 0.2 - max_distance: 200 - use_lidar_clock: false - ros: - ros_recv_packet_topic: /rslidar_packets - ros_send_packet_topic: /rslidar_packets - ros_send_point_cloud_topic: /rslidar_points - proto: - point_cloud_recv_port: 60021 - point_cloud_send_port: 60021 - msop_recv_port: 60022 - msop_send_port: 60022 - difop_recv_port: 60023 - difop_send_port: 60023 - point_cloud_send_ip: 127.0.0.1 - packet_send_ip: 127.0.0.1 -``` - -- lidar_type - - Supported types of LiDAR are listed in README. - -- frame_id - - The frame id of the point cloud message. - -- msop_port, difop_port - - The msop port and difop_port of LiDAR. *Please check these parameters first is no data received.* - -- start_angle, end_angle - - The start angle and end angle of the point cloud, which should be set in range of 0~360°. (*start_angle **can** be larger than end_angle*). - -- min_distance, max_distance - - The minimum distance and maximum distance of the point cloud. - -- use_lidar_clock - - - true -- Use the lidar internal clock as the message timestamp - - false -- Use the system clock as the message timestamp - - - -## 3 Example - -Here are two examples. The first configure file is for single LiDAR case and second is used for the case where there are 3 LiDARs. Please adjust the specific parameters according to your own case. - -- Online connection to single LiDAR & Send point cloud to ROS - -```yaml -common: - msg_source: 1 - send_packet_ros: false - send_point_cloud_ros: true - send_packet_proto: false - send_point_cloud_proto: false - pcap_path: /home/robosense/lidar.pcap -lidar: - - driver: - lidar_type: RS128 - frame_id: /rslidar - msop_port: 6699 - difop_port: 7788 - start_angle: 0 - end_angle: 360 - min_distance: 0.2 - max_distance: 200 - use_lidar_clock: false - ros: - ros_recv_packet_topic: /rslidar_packets - ros_send_packet_topic: /rslidar_packets - ros_send_point_cloud_topic: /rslidar_points - proto: - point_cloud_recv_port: 60021 - point_cloud_send_port: 60021 - msop_recv_port: 60022 - msop_send_port: 60022 - difop_recv_port: 60023 - difop_send_port: 60023 - point_cloud_send_ip: 127.0.0.1 - packet_send_ip: 127.0.0.1 -``` - -- Online connection to three LiDARs & Send point cloud to ROS - -*Pay attention to the indentation of lidar part* - -```yaml -common: - msg_source: 1 - send_packet_ros: false - send_point_cloud_ros: true - send_packet_proto: false - send_point_cloud_proto: false - pcap_path: /home/robosense/lidar.pcap -lidar: - - driver: - lidar_type: RS128 - frame_id: /rslidar - msop_port: 6699 - difop_port: 7788 - start_angle: 0 - end_angle: 360 - min_distance: 0.2 - max_distance: 200 - use_lidar_clock: false - ros: - ros_recv_packet_topic: /middle/rslidar_packets - ros_send_packet_topic: /middle/rslidar_packets - ros_send_point_cloud_topic: /middle/rslidar_points - proto: - point_cloud_recv_port: 60021 - point_cloud_send_port: 60021 - msop_recv_port: 60022 - msop_send_port: 60022 - difop_recv_port: 60023 - difop_send_port: 60023 - point_cloud_send_ip: 127.0.0.1 - packet_send_ip: 127.0.0.1 - - driver: - lidar_type: RSBP - frame_id: /rslidar - msop_port: 1990 - difop_port: 1991 - start_angle: 0 - end_angle: 360 - min_distance: 0.2 - max_distance: 200 - use_lidar_clock: false - ros: - ros_recv_packet_topic: /left/rslidar_packets - ros_send_packet_topic: /left/rslidar_packets - ros_send_point_cloud_topic: /left/rslidar_points - proto: - point_cloud_recv_port: 60024 - point_cloud_send_port: 60024 - msop_recv_port: 60025 - msop_send_port: 60025 - difop_recv_port: 60026 - difop_send_port: 60026 - point_cloud_send_ip: 127.0.0.1 - packet_send_ip: 127.0.0.1 - - driver: - lidar_type: RSBP - frame_id: /rslidar - msop_port: 2010 - difop_port: 2011 - start_angle: 0 - end_angle: 360 - min_distance: 0.2 - max_distance: 200 - use_lidar_clock: false - ros: - ros_recv_packet_topic: /right/rslidar_packets - ros_send_packet_topic: /right/rslidar_packets - ros_send_point_cloud_topic: /right/rslidar_points - proto: - point_cloud_recv_port: 60027 - point_cloud_send_port: 60027 - msop_recv_port: 60028 - msop_send_port: 60028 - difop_recv_port: 60029 - difop_send_port: 60029 - point_cloud_send_ip: 127.0.0.1 - packet_send_ip: 127.0.0.1 -``` - diff --git a/doc/intro/parameter_intro_cn.md b/doc/intro/parameter_intro_cn.md deleted file mode 100644 index ae2fcbc7..00000000 --- a/doc/intro/parameter_intro_cn.md +++ /dev/null @@ -1,240 +0,0 @@ -# 参数介绍 - -本工程只有一份参数文件 ```config.yaml```， 储存于```rslidar_sdk/config```文件夹内。 整个参数文件可以被分为两部分，common部分以及lidar部分。 *在多雷达情况下，common部分的参数设置将会被所有雷达共享，而lidar部分需要根据每台雷达实际情况分别进行设置。* - -**参数文件config.yaml对缩进有严格的要求！请确保修改参数之后每行开头的缩进仍保持一致！** - -## 1 Common - -此部分用于设置雷达的消息来源，以及是否将结果发布。 - -```yaml -common: - msg_source: 1 - send_packet_ros: false - send_point_cloud_ros: false - send_packet_proto: false - send_point_cloud_proto: false - pcap_path: /home/robosense/lidar.pcap -``` - -- msg_source - - - 1 -- 连接在线雷达. 更多使用细节请参考[在线读取雷达数据发送到ROS](../howto/how_to_online_send_point_cloud_ros_cn.md)。 - - - 2 -- 离线解析ROS或ROS2的packet包。更多使用细节请参考 [录制ROS数据包&离线解析ROS数据包](doc/howto/how_to_record_and_offline_decode_rosbag_cn.md)。 - - - 3 -- 离线解析pcap包。更多使用细节请参考[离线解析Pcap包发送到ROS](doc/howto/how_to_offline_decode_pcap_cn.md)。 - - - 4 -- 雷达消息来源为Protobuf-UDP的packet消息，更多使用细节请参考 [使用Protobuf发送&接收](../howto/how_to_use_protobuf_function_cn.md)。 - - - 5 -- 雷达消息来源为Protobuf-UDP的点云消息，更多使用细节请参考 [使用Protobuf发送&接收](../howto/how_to_use_protobuf_function_cn.md)。 - -- send_packet_ros - - - true -- 雷达packet消息将通过ROS或ROS2发出 - - *由于雷达ROS packet消息为速腾聚创自定义ROS消息，因此用户无法直接echo话题查看消息具体内容。实际上packet主要用于录制离线ROS包，因为packet的体积小于点云。* - -- send_point_cloud_ros - - - true -- 雷达点云消息将通过ROS或ROS2发出 - - *点云消息类型为ROS官方定义的点云类型sensor_msgs/PointCloud2, 因此用户可以直接使用Rviz查看点云。同时，用户也可以选择录包时直接录制点云，但这样做包的体积会非常大，因此我们建议离线录制ROS包时录制packet消息。* - -- send_packet_proto - - - true -- 雷达packet消息将通过Protobuf-UDP发出 - -- send_point_cloud_proto - - - true -- 雷达点云消息将通过Protobuf-UDP发出 - - *我们建议发送packet消息而不是点云，因为点云消息体积过大，对带宽有较高的要求。.* - -- pcap_path - - 如果msg_dource = 3, 请确保此参数设置为正确的pcap包的路径。 - - - -## 2 lidar - -本部分需要根据不同的雷达进行设置（多雷达时）。 - -```yaml -lidar: - - driver: - lidar_type: RS128 - frame_id: /rslidar - msop_port: 6699 - difop_port: 7788 - start_angle: 0 - end_angle: 360 - min_distance: 0.2 - max_distance: 200 - use_lidar_clock: false - ros: - ros_recv_packet_topic: /rslidar_packets - ros_send_packet_topic: /rslidar_packets - ros_send_point_cloud_topic: /rslidar_points - proto: - point_cloud_recv_port: 60021 - point_cloud_send_port: 60021 - msop_recv_port: 60022 - msop_send_port: 60022 - difop_recv_port: 60023 - difop_send_port: 60023 - point_cloud_send_ip: 127.0.0.1 - packet_send_ip: 127.0.0.1 -``` - -- lidar_type - - 目前支持的雷达型号已在README中列出。 - -- frame_id - - 点云消息的frame_id。 - -- msop_port, difop_port - - 点云的msop端口号和difop端口号。 *若收不到消息，请优先确认这两个参数是否配置正确。* - -- start_angle, end_angle - - 点云消息的起始角度和结束角度，此处设置为软件屏蔽，无法减小每帧点云的体积，只会将区域外的点设置为NAN点。 起始角和结束角的范围应在0~360°之间。(**起始角可以大于结束角**). - -- min_distance, max_distance - - 点云的最小距离和最大距离，此处设置为软件屏蔽，无法减小每帧点云的体积，只会将区域外的点设置为NAN点。 - -- use_lidar_clock - - - true -- 使用雷达时间作为消息时间戳。 - - false -- 使用系统时间作为消息时间戳。 - - - -## 3 示例 - -在线连接一台雷达，并发送点云到ROS。 - -```yaml -common: - msg_source: 1 - send_packet_ros: false - send_point_cloud_ros: true - send_packet_proto: false - send_point_cloud_proto: false - pcap_path: /home/robosense/lidar.pcap -lidar: - - driver: - lidar_type: RS128 - frame_id: /rslidar - msop_port: 6699 - difop_port: 7788 - start_angle: 0 - end_angle: 360 - min_distance: 0.2 - max_distance: 200 - use_lidar_clock: false - ros: - ros_recv_packet_topic: /rslidar_packets - ros_send_packet_topic: /rslidar_packets - ros_send_point_cloud_topic: /rslidar_points - proto: - point_cloud_recv_port: 60021 - point_cloud_send_port: 60021 - msop_recv_port: 60022 - msop_send_port: 60022 - difop_recv_port: 60023 - difop_send_port: 60023 - point_cloud_send_ip: 127.0.0.1 - packet_send_ip: 127.0.0.1 -``` - -在线连接3台雷达，并发送点云到ROS。 - -*注意lidar部分参数的缩进* - -```yaml -common: - msg_source: 1 - send_packet_ros: false - send_point_cloud_ros: true - send_packet_proto: false - send_point_cloud_proto: false - pcap_path: /home/robosense/lidar.pcap -lidar: - - driver: - lidar_type: RS128 - frame_id: /rslidar - msop_port: 6699 - difop_port: 7788 - start_angle: 0 - end_angle: 360 - min_distance: 0.2 - max_distance: 200 - use_lidar_clock: false - ros: - ros_recv_packet_topic: /middle/rslidar_packets - ros_send_packet_topic: /middle/rslidar_packets - ros_send_point_cloud_topic: /middle/rslidar_points - proto: - point_cloud_recv_port: 60021 - point_cloud_send_port: 60021 - msop_recv_port: 60022 - msop_send_port: 60022 - difop_recv_port: 60023 - difop_send_port: 60023 - point_cloud_send_ip: 127.0.0.1 - packet_send_ip: 127.0.0.1 - - driver: - lidar_type: RSBP - frame_id: /rslidar - msop_port: 1990 - difop_port: 1991 - start_angle: 0 - end_angle: 360 - min_distance: 0.2 - max_distance: 200 - use_lidar_clock: false - ros: - ros_recv_packet_topic: /left/rslidar_packets - ros_send_packet_topic: /left/rslidar_packets - ros_send_point_cloud_topic: /left/rslidar_points - proto: - point_cloud_recv_port: 60024 - point_cloud_send_port: 60024 - msop_recv_port: 60025 - msop_send_port: 60025 - difop_recv_port: 60026 - difop_send_port: 60026 - point_cloud_send_ip: 127.0.0.1 - packet_send_ip: 127.0.0.1 - - driver: - lidar_type: RSBP - frame_id: /rslidar - msop_port: 2010 - difop_port: 2011 - start_angle: 0 - end_angle: 360 - min_distance: 0.2 - max_distance: 200 - use_lidar_clock: false - ros: - ros_recv_packet_topic: /right/rslidar_packets - ros_send_packet_topic: /right/rslidar_packets - ros_send_point_cloud_topic: /right/rslidar_points - proto: - point_cloud_recv_port: 60027 - point_cloud_send_port: 60027 - msop_recv_port: 60028 - msop_send_port: 60028 - difop_recv_port: 60029 - difop_send_port: 60029 - point_cloud_send_ip: 127.0.0.1 - packet_send_ip: 127.0.0.1 -``` - diff --git a/doc/src_intro/img/class_destination_packet.png b/doc/src_intro/img/class_destination_packet.png new file mode 100644 index 00000000..b38f89ec Binary files /dev/null and b/doc/src_intro/img/class_destination_packet.png differ diff --git a/doc/src_intro/img/class_destination_pointcloud.png b/doc/src_intro/img/class_destination_pointcloud.png new file mode 100644 index 00000000..6f2383e6 Binary files /dev/null and b/doc/src_intro/img/class_destination_pointcloud.png differ diff --git a/doc/src_intro/img/class_node_manager.png b/doc/src_intro/img/class_node_manager.png new file mode 100644 index 00000000..2729d406 Binary files /dev/null and b/doc/src_intro/img/class_node_manager.png differ diff --git a/doc/src_intro/img/class_source_destination.png b/doc/src_intro/img/class_source_destination.png new file mode 100644 index 00000000..a7ed0a2a Binary files /dev/null and b/doc/src_intro/img/class_source_destination.png differ diff --git a/doc/src_intro/img/class_source_driver.png b/doc/src_intro/img/class_source_driver.png new file mode 100644 index 00000000..26e19165 Binary files /dev/null and b/doc/src_intro/img/class_source_driver.png differ diff --git a/doc/src_intro/img/class_source_packet_ros.png b/doc/src_intro/img/class_source_packet_ros.png new file mode 100644 index 00000000..91da7af2 Binary files /dev/null and b/doc/src_intro/img/class_source_packet_ros.png differ diff --git a/doc/src_intro/rslidar_sdk_intro_CN.md b/doc/src_intro/rslidar_sdk_intro_CN.md new file mode 100644 index 00000000..e56337c7 --- /dev/null +++ b/doc/src_intro/rslidar_sdk_intro_CN.md @@ -0,0 +1,217 @@ +# rslidar_sdk v1.5.9 源代码解析 + + + +## 1 简介 + +rslidar_sdk是基于ROS/ROS2的雷达驱动。rslidar_sdk依赖rs_driver接收和解析MSOP/DIFOP Packet。 + +rslidar_sdk的基本功能如下： ++ 从在线雷达或PCAP文件得到点云，通过ROS主题`/rslidar_points`发布。使用者可以订阅这个主题，在rviz中看到点云。 ++ 从在线雷达得到原始的MSOP/DIFOP Packet，通过ROS主题`/rslidar_packets`发布。使用者可以订阅这个主题，将Packet记录到rosbag文件。 ++ 从ROS主题`/rslidar_packets`得到MSOP/DIFOP Packet，解析得到点云，再发布到主题`/rslidar_points`。 + + 这里的主题`/rslidar_packets`，由使用者通过回放Packet rosbag文件发布。 + + + +## 2 Source 与 Destination + +如前面所说，rslidar_sdk从在线雷达、PCAP文件、ROS主题这三种源得到MSOP/DIFOP Packet，将Packet发布到ROS主题`/rslidar_packets`，将点云发布到目标 - ROS主题`/rslidar_points`。 ++ Source定义源接口 ++ DestinationPointCloud定义发送点云的目标接口。 ++ DestinationPacket定义发送MSOP/DIFOP Packet的目标接口。 + +![source](./img/class_source_destination.png) + +### 2.1 DestinationPointCloud + +DestinationPointCloud定义发送点云的接口。 ++ 虚拟成员函数init()对DestinationPointCloud实例初始化 ++ 虚拟成员函数start()启动实例 ++ 虚拟成员函数sendPointCloud()发送PointCloud消息 + +### 2.2 DestinationPacket + +DestinationPacket定义发送MSOP/DIFOP Packet的接口。 ++ 虚拟成员函数init()对DestinationPacket实例初始化 ++ 虚拟成员函数start()启动实例 ++ 虚拟成员函数sendPacket()启动发送Packet消息 + +### 2.3 Source + +Source是定义源的接口。 + ++ 成员`src_type_`是源的类型 + + ``` + enum SourceType + { + MSG_FROM_LIDAR = 1, + MSG_FROM_ROS_PACKET = 2, + MSG_FROM_PCAP = 3, + }; + ``` + ++ 成员`pc_cb_vec_[]`中是一组DestinationPointCloud的实例。 + + 成员函数sendPointCloud()调用`point_cb_vec_[]`中的实例，发送点云消息。 ++ 成员`pkt_cb_vec_[]`中是一组DestinationPacket实例。 + + 成员函数sendPacket()将Packet消息发送到`pkt_cb_vec_[]`中的实例中。 + ++ 虚拟成员函数init()初始化Source实例 ++ 虚拟成员函数start()启动实例 ++ 虚拟成员函数regPointCloudCallback()将PointCloudDestination实例注册到`point_cb_vec_[]`。 ++ 虚拟成员函数regPacketCallback()将PacketDestination实例注册到`packet_cb_vec_[]`。 + +### 2.4 DestinationPointCloudRos + +DestinationPointCloudRos在ROS主题`/rslidar_points`发布点云。 ++ 成员`pkt_pub_`是ROS主题发布器。 ++ 成员`frame_id_`保存`frame_id`。`frame_id`是坐标系名字。 + +![destination pointcloud ros](./img/class_destination_pointcloud.png) + +#### 2.4.1 DestinationPointCloudRos::init() + +init()初始化DestinationPointCloudRos实例。 ++ 从YAML文件读入用户配置参数。 + + 读入`frame_id`，保存在成员`frame_id_`，默认值是`rslidar`。 + + 读入ROS主题，保存在本地变量`ros_send_topic_`，默认值是`/rslidar_points`。 ++ 创建ROS主题发布器，保存在成员`pkt_sub_`. + +#### 2.4.2 DestinationPointCloudRos::sendPointCloud() + +sendPacket()在ROS主题`/rslidar_points`发布点云。 ++ 调用Publisher::publish()发布ROS格式的点云消息。 + +### 2.5 DestinationPacketRos + +DestinationPacketRos在ROS主题`/rslidar_packets`发布MSOP/DIFOP Packet。 ++ 成员`pkt_sub_`是ROS主题发布器。 ++ 成员`frame_id_`保存`frame_id`。`frame_id`是坐标系名字。 + +![destination packet ros](./img/class_destination_packet.png) + +#### 2.5.1 DestinationPacketRos::init() + +init()初始化DestinationPacketRos实例。 ++ 从YAML文件读入用户配置参数。 + + 读入`frame_id`，保存在成员`frame_id_`，默认值是`rslidar` + + 读入ROS主题，保存在本地变量`ros_send_topic_`，默认值是`/rslidar_packets`。 ++ 创建ROS主题发布器，保存在成员`pkt_sub_`. + +#### 2.5.2 DestinationPacketRos::sendPacket() + +sendPacket()在ROS主题`/rslidar_packets`发布MOSP/DIFOP packet。 ++ 调用Publisher::publish()发布ROS格式的Packet消息。 + +### 2.6 SourceDriver + +SourceDriver从在线雷达和PCAP文件得到MSOP/DIFOP Packet，并解析得到点云。 ++ 成员`driver_ptr_`是rs_driver驱动的实例，也就是LidarDriver。 ++ 成员`free_point_cloud_queue_`和`point_cloud_queue_`，分别是空闲点云的队列和待处理点云的队列。 ++ 成员`point_cloud_handle_thread_`是点云的处理线程。 + +![source driver](./img/class_source_driver.png) + +#### 2.6.1 SourceDriver::init() + +init()初始化SourceDriver实例。 ++ 读取YAML配置文件，得到雷达的用户配置参数。 ++ 根据源类型，也就是成员`src_type_`，创建相应类型的LidarDriver实例，也就是成员`driver_ptr_`。 + + `src_type_`是在SourceDriver中的构造函数中指定的。 ++ 调用LidarDriver::regPointCloudCallback()，注册回调函数。这里是getPointCloud()和putPointCloud()。前者给`driver_ptr_`提供空闲点云，后者从`driver_ptr_`得到填充好的点云。 + + 注意，这里没有注册MSOP/DIFOP Packet的回调函数，因为Packet是按需获取的。这时为了避免不必要地消耗CPU资源。 ++ 调用LidarDriver::init()，初始化`driver_ptr_`。 ++ 创建、启动点云处理线程`point_cloud_handle_thread_`， 线程函数是processPointCloud()。 + +#### 2.6.2 SourceDriver::getPointCloud() + +getPointCloud()给成员`driver_ptr_`提供空闲的点云。 ++ 优先从成员`free_point_cloud_queue_`得到点云。 ++ 如果得不到，分配新的点云。 + +#### 2.6.3 SourceDriver::putPointCloud() + +putPointCloud()给从成员`driver_ptr_`得到填充好的点云。 ++ 将得到的点云推送到成员`point_cloud_queue_`，等待处理。 + +#### 2.6.4 SourceDriver::processPointCloud() + +processPointCloud()处理点云。在while循环中， ++ 从待处理点云的队列`point_cloud_queue_`，得到点云， ++ 调用sendPointCloud()，其中调用成员`pc_cb_vec_[]`中的DestinationPointCloud实例，发送点云。 ++ 回收点云，放入空闲点云的队列`free_cloud_queue_`，待下次使用。 + +#### 2.6.5 SourceDriver::regPacketCallback() + +regPacketCallback()用来注册DestinationPacket。 ++ 调用Source::regPacketCallback()，将DestinationPacket实例，加入成员`pkt_cb_vec_[]`。 ++ 如果这是首次要求Packet(`pkt_cb_vec_[]`的第1个实例)，调用LidarDriver::regPacketCallback()，向`driver_ptr_`注册Packet回调函数，开始接收Packet。回调函数是putPacket()。 + +#### 2.6.6 SourceDriver::putPacket() + +putPacket()调用sendPacket()，其中调用成员`pkt_cb_vec_[]`中的所有实例，发送MSOP/DIFOP Packet。 + +### 2.7 SourcePacketRos + +SourcePacketRos在ROS主题`/rslidar_packets`得到MSOP/DIFOP Packet，解析后得到点云。 ++ SourcePacketRos从SourceDriver派生，而不是直接从Source派生，是因为它用SourceDriver解析Packet得到点云。 ++ 成员`pkt_sub_`，是ROS主题`/rslidar_packets`的订阅器。 + +![source](./img/class_source_packet_ros.png) + +#### 2.7.1 SourcePacketRos::init() + +init()初始化SourcePacketRos实例。 ++ 调用SourceDriver::init()初始化成员`driver_ptr_`。 + + 在SourcePacketRos的构造函数中，SourceType设置为`SourceType::MSG_FROM_ROS_PACKET`。这样，在SourceDriver::init()中，`driver_ptr_`的`input_type`就是`InputType::RAW_PACKET`，它通过LidarDriver::feedPacket接收Packet作为源。 ++ 解析YAML文件得到雷达的用户配置参数 + + 得到接收Packet的主题，默认值为`/rslidar_packets`。 ++ 创建Packet主题的订阅器，也就是成员`pkt_sub_`，接收函数是putPacket()。 + +#### 2.7.2 SourcePacketRos::putPacket() + +putPacket()接收Packet，送到`driver_ptr_`解析。 ++ 调用LidarDriver::decodePacket()，将Packet喂给`driver_ptr_`。 ++ 点云的接收，使用SourceDriver的已有实现。 + + + +## 3 NodeManager + +NodeManager管理Source实例，包括创建、初始化、启动、停止Source。它支持多个源，但是这些源的类型必须相同。 ++ 成员`sources_[]`是一个Source实例的数组。 + +![node_manager](./img/class_node_manager.png) + +### 3.1 NodeManager::init() + +init()初始化NodeManger实例。 ++ 从config.yaml文件得到用户配置参数 + + 本地变量`msg_source`，数据源类型 + + 本地变量`send_point_cloud_ros`， 是否在ROS主题发送点云。 + + 本地变量`send_packet_ros`，是否在ROS主题发送MSOP/DIFOP packet， + ++ 在.yaml文件中遍历数据源。在循环中， + + 根据`msg_source`创建Source实例。 + + 如果是在线雷达（`SourceType::MSG_FROM_LIDAR`)，创建SourceDriver实例并初始化， 源类型为`MSG_FROM_LIDAR`。 + + 如果是PCAP文件(`SourceType::MSG_FROM_PCAP`)，创建SourceDriver实例并初始化，源类型为`MSG_FROM_PCAP`。 + + 如果是ROS主题(`SourceType::MSG_FROM_ROS_PACKET`), 创建SourcePacketRos并初始化。SourcePacketRos构造函数已将源类型设置为`MSG_FROM_ROS_PACKET` + + 如果在ROS主题发送点云(`send_point_cloud_ros` = `true`)，则创建DestinationPointCloudRos实例、初始化，调用Source::regPointCloudCallback(),将它加入Source的`pc_cb_vec_[]`。 + + 如果在ROS主题发送Packet(`send_packet_ros` = `true`)，则创建DestinationPacketRos实例、初始化，调用Source::regPacketCallback()将它加入Source的`pkt_cb_vec_[]`。 + + 将Source实例，加入成员`sources_[]`。 + +### 3.2 NodeManager::start() + +start()启动成员`sources_[]`中的所有实例。 + + + + + + + + + + + diff --git a/doc/img/download_page.png b/img/01_01_download_page.png similarity index 100% rename from doc/img/download_page.png rename to img/01_01_download_page.png diff --git a/launch/elequent_start.py b/launch/elequent_start.py new file mode 100755 index 00000000..98cc9e05 --- /dev/null +++ b/launch/elequent_start.py @@ -0,0 +1,21 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +def generate_launch_description(): + rviz_config=get_package_share_directory('rslidar_sdk')+'/rviz/rviz2.rviz' + return LaunchDescription([ + Node( + package='rslidar_sdk', + node_namespace='rslidar_sdk', + node_name='rslidar_sdk_node', + node_executable='rslidar_sdk_node', + output='screen' + ), + Node( + package='rviz2', + node_namespace='rviz2', + node_name='rviz2', + node_executable='rviz2', + arguments=['-d',rviz_config] + ) + ]) diff --git a/launch/start.launch b/launch/start.launch index dbf20789..25b89068 100755 --- a/launch/start.launch +++ b/launch/start.launch @@ -1,5 +1,8 @@ + + + diff --git a/launch/start.py b/launch/start.py index 8e023706..ecc86a79 100755 --- a/launch/start.py +++ b/launch/start.py @@ -1,21 +1,14 @@ from launch import LaunchDescription from launch_ros.actions import Node from ament_index_python.packages import get_package_share_directory + def generate_launch_description(): + rviz_config=get_package_share_directory('rslidar_sdk')+'/rviz/rviz2.rviz' + + config_file = '' # your config file path + return LaunchDescription([ - Node( - package='rslidar_sdk', - node_namespace='rslidar_sdk', - node_executable='rslidar_sdk_node', - output='screen', - node_name='rslidar_sdk_node' - ), - Node( - package='rviz2', - node_namespace='rviz2', - node_executable='rviz2', - node_name='rviz2', - arguments=['-d',rviz_config] - ) - ]) \ No newline at end of file + Node(namespace='rslidar_sdk', package='rslidar_sdk', executable='rslidar_sdk_node', output='screen', parameters=[{'config_path': config_file}]), + Node(namespace='rviz2', package='rviz2', executable='rviz2', arguments=['-d',rviz_config]) + ]) diff --git a/node/rslidar_sdk_node.cpp b/node/rslidar_sdk_node.cpp index 336416a9..084b00d3 100644 --- a/node/rslidar_sdk_node.cpp +++ b/node/rslidar_sdk_node.cpp @@ -30,59 +30,127 @@ WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWIS THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *********************************************************************************************************************/ +#include "manager/node_manager.hpp" + +#include #include -#include "manager/adapter_manager.h" + +#ifdef ROS_FOUND +#include +#include +#elif ROS2_FOUND +#include +#endif + using namespace robosense::lidar; + +#ifdef ROS2_FOUND std::mutex g_mtx; std::condition_variable g_cv; +#endif + static void sigHandler(int sig) { RS_MSG << "RoboSense-LiDAR-Driver is stopping....." << RS_REND; + #ifdef ROS_FOUND ros::shutdown(); -#endif +#elif ROS2_FOUND g_cv.notify_all(); +#endif } int main(int argc, char** argv) { signal(SIGINT, sigHandler); ///< bind ctrl+c signal with the sigHandler function + RS_TITLE << "********************************************************" << RS_REND; RS_TITLE << "********** **********" << RS_REND; - RS_TITLE << "********** RSLidar_SDK Version: v" << RSLIDAR_VERSION_MAJOR << "." << RSLIDAR_VERSION_MINOR << "." - << RSLIDAR_VERSION_PATCH << " **********" << RS_REND; + RS_TITLE << "********** RSLidar_SDK Version: v" << RSLIDAR_VERSION_MAJOR + << "." << RSLIDAR_VERSION_MINOR + << "." << RSLIDAR_VERSION_PATCH << " **********" << RS_REND; RS_TITLE << "********** **********" << RS_REND; RS_TITLE << "********************************************************" << RS_REND; - std::shared_ptr demo_ptr = std::make_shared(); +#ifdef ROS_FOUND + ros::init(argc, argv, "rslidar_sdk_node", ros::init_options::NoSigintHandler); +#elif ROS2_FOUND + rclcpp::init(argc, argv); +#endif + + std::string config_path; + +#ifdef RUN_IN_ROS_WORKSPACE + config_path = ros::package::getPath("rslidar_sdk"); +#else + config_path = (std::string)PROJECT_PATH; +#endif + + //config_path += "/config/config.yaml"; + std::string rslidar_config_file = (std::string)PROJECT_PATH + "/config/config.yaml"; + ros::param::get("rslidar_config_file", rslidar_config_file); + +#ifdef ROS_FOUND + ros::NodeHandle priv_hh("~"); + std::string path; + priv_hh.param("config_path", path, std::string("")); +#elif ROS2_FOUND + std::shared_ptr nd = rclcpp::Node::make_shared("param_handle"); + std::string path = nd->declare_parameter("config_path", ""); +#endif + +#if defined(ROS_FOUND) || defined(ROS2_FOUND) + if (!path.empty()) + { + config_path = path; + } +#endif + YAML::Node config; try { - config = YAML::LoadFile((std::string)PROJECT_PATH + "/config/config.yaml"); + //config = YAML::LoadFile(config_path); + config = YAML::LoadFile(rslidar_config_file); + RS_INFO << "--------------------------------------------------------" << RS_REND; + RS_INFO << "Config loaded from PATH:" << RS_REND; + RS_INFO << config_path << RS_REND; + RS_INFO << "--------------------------------------------------------" << RS_REND; } catch (...) { - RS_ERROR << "Config file format wrong! Please check the format(e.g. indentation) " << RS_REND; + RS_ERROR << "The format of config file " << config_path << " is wrong. Please check (e.g. indentation)." << RS_REND; return -1; } + + std::string lidar_type = ""; + priv_hh.param("lidar_type", lidar_type, lidar_type); + if (lidar_type != "") + config["lidar"][0]["driver"]["lidar_type"] = lidar_type; + int msop_port = -1; + priv_hh.param("msop_port", msop_port, msop_port); + if (msop_port != -1) + config["lidar"][0]["driver"]["msop_port"] = msop_port; + int difop_port = -1; + priv_hh.param("difop_port", difop_port, difop_port); + if (difop_port != -1) + config["lidar"][0]["driver"]["difop_port"] = difop_port; + std::string ros_frame_id = ""; + priv_hh.param("frame_id", ros_frame_id, ros_frame_id); + if (ros_frame_id != "") + config["lidar"][0]["ros"]["ros_frame_id"] = ros_frame_id; -#ifdef ROS_FOUND ///< if ROS is found, call the ros::init function - ros::init(argc, argv, "rslidar_sdk_node", ros::init_options::NoSigintHandler); -#endif - -#ifdef ROS2_FOUND ///< if ROS2 is found, call the rclcpp::init function - rclcpp::init(argc, argv); -#endif - + std::shared_ptr demo_ptr = std::make_shared(); demo_ptr->init(config); demo_ptr->start(); + RS_MSG << "RoboSense-LiDAR-Driver is running....." << RS_REND; #ifdef ROS_FOUND ros::spin(); -#else +#elif ROS2_FOUND std::unique_lock lck(g_mtx); g_cv.wait(lck); #endif + return 0; -} \ No newline at end of file +} diff --git a/package.xml b/package.xml new file mode 100644 index 00000000..02ad159d --- /dev/null +++ b/package.xml @@ -0,0 +1,25 @@ + + + rslidar_sdk + 1.5.16 + The rslidar_sdk package + robosense + BSD + + catkin + ament_cmake + + libpcap + pcl_ros + rclcpp + roscpp + roslib + rslidar_msg + sensor_msgs + std_msgs + yaml-cpp + + + ament_cmake + + diff --git a/package_ros1.xml b/package_ros1.xml deleted file mode 100644 index c354985b..00000000 --- a/package_ros1.xml +++ /dev/null @@ -1,15 +0,0 @@ - - - rslidar_sdk - 1.3.0 - The rslidar_sdk package - robosense - BSD - catkin - - roscpp - sensor_msgs - - roscpp - sensor_msgs - \ No newline at end of file diff --git a/package_ros2.xml b/package_ros2.xml deleted file mode 100644 index 8682ce49..00000000 --- a/package_ros2.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - rslidar_sdk - 1.3.0 - The rslidar_sdk package - robosense - BSD - ament_cmake - - rclcpp - sensor_msgs - std_msgs - rclcpp - sensor_msgs - std_msgs - rslidar_msg - - ament_cmake - - - - - - diff --git a/rviz/rviz.rviz b/rviz/rviz.rviz index 4a44e95a..e3b70a87 100644 --- a/rviz/rviz.rviz +++ b/rviz/rviz.rviz @@ -3,9 +3,11 @@ Panels: Help Height: 75 Name: Displays Property Tree Widget: - Expanded: ~ + Expanded: + - /Global Options1 + - /Point Cloud1 Splitter Ratio: 0.5 - Tree Height: 846 + Tree Height: 790 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -14,7 +16,7 @@ Panels: - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties - Splitter Ratio: 0.588679016 + Splitter Ratio: 0.5886790156364441 - Class: rviz/Views Expanded: - /Current View1 @@ -31,7 +33,9 @@ Panels: Property Tree Widget: Expanded: ~ Splitter Ratio: 0.5 - Tree Height: 362 + Tree Height: 366 +Preferences: + PromptSaveOnExit: true Toolbars: toolButtonStyle: 2 Visualization Manager: @@ -52,16 +56,14 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 241 Min Color: 0; 0; 0 - Min Intensity: 1 Name: Point Cloud Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 1.5 - Size (m): 0.00999999978 - Style: Points + Size (m): 0.10000000149011612 + Style: Squares Topic: /rslidar_points Unreliable: false Use Fixed Frame: true @@ -73,7 +75,7 @@ Visualization Manager: Color: 160; 160; 164 Enabled: true Line Style: - Line Width: 0.0299999993 + Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 @@ -85,18 +87,20 @@ Visualization Manager: Plane Cell Count: 50 Reference Frame: Value: true - - Class: rviz/Axes + - Alpha: 1 + Class: rviz/Axes Enabled: true Length: 1 Name: Axes - Radius: 0.100000001 + Radius: 0.10000000149011612 Reference Frame: + Show Trail: false Value: true Enabled: true Global Options: Background Color: 48; 48; 48 Default Light: true - Fixed Frame: rslidar + Fixed Frame: /rslidar Frame Rate: 30 Name: root Tools: @@ -107,7 +111,10 @@ Visualization Manager: - Class: rviz/FocusCamera - Class: rviz/Measure - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 - Class: rviz/SetGoal Topic: /move_base_simple/goal - Class: rviz/PublishPoint @@ -117,33 +124,33 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 34.5997963 + Distance: 107.46170806884766 Enable Stereo Rendering: - Stereo Eye Separation: 0.0599999987 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false + Field of View: 0.7853981852531433 Focal Point: - X: 1.4133122 - Y: -0.170824349 - Z: 1.69326282 + X: 1.4133121967315674 + Y: -0.1708243489265442 + Z: 1.6932628154754639 Focal Shape Fixed Size: true - Focal Shape Size: 0.0500000007 + Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View - Near Clip Distance: 0.00999999978 - Pitch: 0.359796703 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.634796679019928 Target Frame: - Value: Orbit (rviz) - Yaw: 3.12499475 + Yaw: 3.9649996757507324 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1056 + Height: 1016 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 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 + QMainWindow State: 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 Selection: collapsed: false Time: @@ -152,6 +159,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 2495 - X: 65 - Y: 24 + Width: 2504 + X: 56 + Y: 27 diff --git a/src/adapter/driver_adapter.hpp b/src/adapter/driver_adapter.hpp deleted file mode 100644 index 102af338..00000000 --- a/src/adapter/driver_adapter.hpp +++ /dev/null @@ -1,267 +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 "adapter/adapter_base.hpp" -#include "rs_driver/api/lidar_driver.h" - -namespace robosense -{ -namespace lidar -{ -class DriverAdapter : virtual public AdapterBase -{ -public: - DriverAdapter(); - ~DriverAdapter(); - void init(const YAML::Node& config); - void start(); - void stop(); - inline void regRecvCallback(const std::function& callback); - inline void regRecvCallback(const std::function& callback); - inline void regRecvCallback(const std::function& callback); - inline void regRecvCallback(const std::function& callback); - void decodeScan(const ScanMsg& msg); - void decodePacket(const PacketMsg& msg); - -private: - void localPointsCallback(const PointCloudMsg& msg); - void localScanCallback(const ScanMsg& msg); - void localPacketCallback(const PacketMsg& msg); - void localCameraTriggerCallback(const CameraTrigger& msg); - void localExceptionCallback(const lidar::Error& msg); - LidarPointCloudMsg core2SDK(const lidar::PointCloudMsg& msg); - -private: - std::shared_ptr> driver_ptr_; - std::vector> point_cloud_cb_vec_; - std::vector> scan_cb_vec_; - std::vector> packet_cb_vec_; - std::vector> camera_trigger_cb_vec_; - lidar::ThreadPool::Ptr thread_pool_ptr_; -}; - -inline DriverAdapter::DriverAdapter() -{ - driver_ptr_.reset(new lidar::LidarDriver()); - thread_pool_ptr_.reset(new lidar::ThreadPool()); - driver_ptr_->regExceptionCallback(std::bind(&DriverAdapter::localExceptionCallback, this, std::placeholders::_1)); -} - -inline DriverAdapter::~DriverAdapter() -{ - driver_ptr_->stop(); -} - -inline void DriverAdapter::init(const YAML::Node& config) -{ - lidar::RSDriverParam driver_param; - int msg_source; - std::string lidar_type; - uint16_t split_frame_mode; - YAML::Node driver_config = yamlSubNodeAbort(config, "driver"); - yamlReadAbort(config, "msg_source", msg_source); - yamlRead(driver_config, "frame_id", driver_param.frame_id, "rslidar"); - yamlRead(driver_config, "angle_path", driver_param.angle_path, ""); - yamlReadAbort(driver_config, "lidar_type", lidar_type); - yamlRead(driver_config, "wait_for_difop", driver_param.wait_for_difop, true); - yamlRead(driver_config, "saved_by_rows", driver_param.saved_by_rows, false); - yamlRead(driver_config, "use_lidar_clock", driver_param.decoder_param.use_lidar_clock, false); - yamlRead(driver_config, "min_distance", driver_param.decoder_param.min_distance, 0.2); - yamlRead(driver_config, "max_distance", driver_param.decoder_param.max_distance, 200); - yamlRead(driver_config, "start_angle", driver_param.decoder_param.start_angle, 0); - yamlRead(driver_config, "end_angle", driver_param.decoder_param.end_angle, 360); - yamlRead(driver_config, "split_frame_mode", split_frame_mode, 1); - yamlRead(driver_config, "num_pkts_split", driver_param.decoder_param.num_pkts_split, 0); - yamlRead(driver_config, "cut_angle", driver_param.decoder_param.cut_angle, 0); - yamlRead(driver_config, "device_ip", driver_param.input_param.device_ip, "192.168.1.200"); - yamlRead(driver_config, "multi_cast_address", driver_param.input_param.multi_cast_address, "0.0.0.0"); - yamlRead(driver_config, "msop_port", driver_param.input_param.msop_port, 6699); - yamlRead(driver_config, "difop_port", driver_param.input_param.difop_port, 7788); - yamlRead(driver_config, "read_pcap", driver_param.input_param.read_pcap, false); - yamlRead(driver_config, "pcap_rate", driver_param.input_param.pcap_rate, 1); - yamlRead(driver_config, "pcap_repeat", driver_param.input_param.pcap_repeat, false); - yamlRead(driver_config, "pcap_path", driver_param.input_param.pcap_path, ""); - yamlRead(driver_config, "x", driver_param.decoder_param.transform_param.x, 0); - yamlRead(driver_config, "y", driver_param.decoder_param.transform_param.y, 0); - yamlRead(driver_config, "z", driver_param.decoder_param.transform_param.z, 0); - yamlRead(driver_config, "roll", driver_param.decoder_param.transform_param.roll, 0); - yamlRead(driver_config, "pitch", driver_param.decoder_param.transform_param.pitch, 0); - yamlRead(driver_config, "yaw", driver_param.decoder_param.transform_param.yaw, 0); - driver_param.lidar_type = driver_param.strToLidarType(lidar_type); - driver_param.decoder_param.split_frame_mode = SplitFrameMode(split_frame_mode); - if (config["camera"] && config["camera"].Type() != YAML::NodeType::Null) - { - for (size_t i = 0; i < config["camera"].size(); i++) - { - double trigger_angle; - std::string frame_id; - yamlRead(config["camera"][i], "trigger_angle", trigger_angle, 0); - yamlRead(config["camera"][i], "frame_id", frame_id, "rs_camera"); - auto iter = driver_param.decoder_param.trigger_param.trigger_map.find(trigger_angle); - if (iter != driver_param.decoder_param.trigger_param.trigger_map.end()) - { - trigger_angle += (double)i / 1000.0; - driver_param.decoder_param.trigger_param.trigger_map.emplace(trigger_angle, frame_id); - } - else - { - driver_param.decoder_param.trigger_param.trigger_map.emplace(trigger_angle, frame_id); - } - } - } - if (msg_source == MsgSource::MSG_FROM_LIDAR || msg_source == MsgSource::MSG_FROM_PCAP) - { - if (!driver_ptr_->init(driver_param)) - { - RS_ERROR << "Driver Initialize Error...." << RS_REND; - exit(-1); - } - } - else - { - driver_ptr_->initDecoderOnly(driver_param); - } - driver_ptr_->regRecvCallback(std::bind(&DriverAdapter::localPointsCallback, this, std::placeholders::_1)); - driver_ptr_->regRecvCallback(std::bind(&DriverAdapter::localScanCallback, this, std::placeholders::_1)); - driver_ptr_->regRecvCallback(std::bind(&DriverAdapter::localPacketCallback, this, std::placeholders::_1)); - driver_ptr_->regRecvCallback(std::bind(&DriverAdapter::localCameraTriggerCallback, this, std::placeholders::_1)); -} - -inline void DriverAdapter::start() -{ - driver_ptr_->start(); -} - -inline void DriverAdapter::stop() -{ - driver_ptr_->stop(); -} - -inline void DriverAdapter::regRecvCallback(const std::function& callback) -{ - point_cloud_cb_vec_.emplace_back(callback); -} - -inline void DriverAdapter::regRecvCallback(const std::function& callback) -{ - scan_cb_vec_.emplace_back(callback); -} - -inline void DriverAdapter::regRecvCallback(const std::function& callback) -{ - packet_cb_vec_.emplace_back(callback); -} - -inline void DriverAdapter::regRecvCallback(const std::function& callback) -{ - camera_trigger_cb_vec_.emplace_back(callback); -} - -inline void DriverAdapter::decodeScan(const ScanMsg& msg) -{ - lidar::PointCloudMsg point_cloud_msg; - if (driver_ptr_->decodeMsopScan(msg, point_cloud_msg)) - { - localPointsCallback(point_cloud_msg); - } -} - -inline void DriverAdapter::decodePacket(const PacketMsg& msg) -{ - driver_ptr_->decodeDifopPkt(msg); -} - -inline void DriverAdapter::localPointsCallback(const PointCloudMsg& msg) -{ - for (auto iter : point_cloud_cb_vec_) - { - thread_pool_ptr_->commit([this, msg, iter]() { iter(core2SDK(msg)); }); - } -} - -inline void DriverAdapter::localScanCallback(const ScanMsg& msg) -{ - for (auto iter : scan_cb_vec_) - { - thread_pool_ptr_->commit([this, msg, iter]() { iter(msg); }); - } -} - -inline void DriverAdapter::localPacketCallback(const PacketMsg& msg) -{ - for (auto iter : packet_cb_vec_) - { - thread_pool_ptr_->commit([this, msg, iter]() { iter(msg); }); - } -} - -inline void DriverAdapter::localCameraTriggerCallback(const CameraTrigger& msg) -{ - for (auto iter : camera_trigger_cb_vec_) - { - thread_pool_ptr_->commit([this, msg, iter]() { iter(msg); }); - } -} - -inline void DriverAdapter::localExceptionCallback(const lidar::Error& msg) -{ - switch (msg.error_code_type) - { - case lidar::ErrCodeType::INFO_CODE: - RS_INFO << msg.toString() << RS_REND; - break; - case lidar::ErrCodeType::WARNING_CODE: - RS_WARNING << msg.toString() << RS_REND; - break; - case lidar::ErrCodeType::ERROR_CODE: - RS_ERROR << msg.toString() << RS_REND; - break; - } -} - -inline LidarPointCloudMsg DriverAdapter::core2SDK(const lidar::PointCloudMsg& msg) -{ - LidarPointCloudMsg::PointCloudPtr point_cloud(new LidarPointCloudMsg::PointCloud); - point_cloud->points.assign(msg.point_cloud_ptr->begin(), msg.point_cloud_ptr->end()); - point_cloud->height = msg.height; - point_cloud->width = msg.width; - point_cloud->is_dense = msg.is_dense; - LidarPointCloudMsg point_cloud_msg(point_cloud); - point_cloud_msg.frame_id = msg.frame_id; - point_cloud_msg.timestamp = msg.timestamp; - point_cloud_msg.seq = msg.seq; - return std::move(point_cloud_msg); -} - -} // namespace lidar -} // namespace robosense diff --git a/src/adapter/packet_protobuf_adapter.hpp b/src/adapter/packet_protobuf_adapter.hpp deleted file mode 100644 index 7a301705..00000000 --- a/src/adapter/packet_protobuf_adapter.hpp +++ /dev/null @@ -1,325 +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 -#ifdef PROTO_FOUND -#include "adapter/adapter_base.hpp" -#include "utility/protobuf_communicator.hpp" -#include "msg/proto_msg_translator.h" -constexpr size_t PKT_RECEIVE_BUF_SIZE = 2000000; - -namespace robosense -{ -namespace lidar -{ -class PacketProtoAdapter : virtual public AdapterBase -{ -public: - PacketProtoAdapter(); - ~PacketProtoAdapter(); - void init(const YAML::Node& config); - void start(); - void stop(); - void regRecvCallback(const std::function& callback); - void regRecvCallback(const std::function& callback); - void sendScan(const ScanMsg& msg); - void sendPacket(const PacketMsg& msg); - -private: - void localMsopCallback(const ScanMsg& msg); - void recvMsopPkts(); - void spliceMsopPkts(); - void sendMsop(); - void localDifopCallback(const PacketMsg& msg); - void recvDifopPkts(); - void spliceDifopPkts(); - void sendDifop(); - -private: - std::vector> scan_cb_vec_; - std::vector> packet_cb_vec_; - std::unique_ptr scan_proto_com_ptr_; - std::unique_ptr packet_proto_com_ptr_; - lidar::ThreadPool::Ptr thread_pool_ptr_; - lidar::Queue scan_send_queue_; - lidar::Queue packet_send_queue_; - lidar::Queue> scan_recv_queue_; - lidar::Queue> packet_recv_queue_; - lidar::Thread scan_recv_thread_; - lidar::Thread packet_recv_thread_; - int old_frmnum_; - int new_frmnum_; - void* scan_buff_; -}; - -inline PacketProtoAdapter::PacketProtoAdapter() : old_frmnum_(0), new_frmnum_(0) -{ - thread_pool_ptr_.reset(new ThreadPool()); -} - -inline PacketProtoAdapter::~PacketProtoAdapter() -{ - stop(); -} - -inline void PacketProtoAdapter::init(const YAML::Node& config) -{ - bool send_packet_proto; - int msg_source = 0; - std::string packet_send_ip; - std::string msop_send_port; - std::string difop_send_port; - uint16_t msop_recv_port; - uint16_t difop_recv_port; - yamlReadAbort(config, "msg_source", msg_source); - yamlRead(config, "send_packet_proto", send_packet_proto, false); - yamlReadAbort(config["proto"], "packet_send_ip", packet_send_ip); - yamlReadAbort(config["proto"], "msop_send_port", msop_send_port); - yamlReadAbort(config["proto"], "difop_send_port", difop_send_port); - yamlReadAbort(config["proto"], "msop_recv_port", msop_recv_port); - yamlReadAbort(config["proto"], "difop_recv_port", difop_recv_port); - scan_proto_com_ptr_.reset(new ProtoCommunicator); - packet_proto_com_ptr_.reset(new ProtoCommunicator); - if (msg_source == MsgSource::MSG_FROM_PROTO_PACKET) - { - if ((scan_proto_com_ptr_->initReceiver(msop_recv_port) == -1) || - (packet_proto_com_ptr_->initReceiver(difop_recv_port) == -1)) - { - RS_ERROR << "LidarPacketsReceiver: Create UDP Receiver Socket failed or Bind Network failed!" << RS_REND; - exit(-1); - } - send_packet_proto = false; - } - if (send_packet_proto) - { - if ((scan_proto_com_ptr_->initSender(msop_send_port, packet_send_ip) == -1) || - (packet_proto_com_ptr_->initSender(difop_send_port, packet_send_ip) == -1)) - { - RS_ERROR << "LidarPacketsReceiver: Create UDP Sender Socket failed ! " << RS_REND; - exit(-1); - } - } -} - -inline void PacketProtoAdapter::start() -{ - scan_buff_ = malloc(PKT_RECEIVE_BUF_SIZE); - scan_recv_thread_.start_ = true; - scan_recv_thread_.thread_.reset(new std::thread([this]() { recvMsopPkts(); })); - packet_recv_thread_.start_ = true; - packet_recv_thread_.thread_.reset(new std::thread([this]() { recvDifopPkts(); })); -} - -inline void PacketProtoAdapter::stop() -{ - if (scan_recv_thread_.start_.load()) - { - scan_recv_thread_.start_.store(false); - scan_recv_thread_.thread_->join(); - free(scan_buff_); - } - if (packet_recv_thread_.start_.load()) - { - packet_recv_thread_.start_.store(false); - packet_recv_thread_.thread_->join(); - } -} - -inline void PacketProtoAdapter::regRecvCallback(const std::function& callback) -{ - scan_cb_vec_.emplace_back(callback); -} - -inline void PacketProtoAdapter::regRecvCallback(const std::function& callback) -{ - packet_cb_vec_.emplace_back(callback); -} - -inline void PacketProtoAdapter::sendScan(const ScanMsg& msg) -{ - scan_send_queue_.push(msg); - if (scan_send_queue_.is_task_finished_.load()) - { - scan_send_queue_.is_task_finished_.store(false); - thread_pool_ptr_->commit([this]() { sendMsop(); }); - } -} - -inline void PacketProtoAdapter::sendPacket(const PacketMsg& msg) -{ - packet_send_queue_.push(msg); - if (packet_send_queue_.is_task_finished_.load()) - { - packet_send_queue_.is_task_finished_.store(false); - thread_pool_ptr_->commit([this]() { sendDifop(); }); - } -} - -inline void PacketProtoAdapter::localMsopCallback(const ScanMsg& msg) -{ - for (auto& cb : scan_cb_vec_) - { - cb(msg); - } -} - -inline void PacketProtoAdapter::recvMsopPkts() -{ - bool start_check = true; - while (scan_recv_thread_.start_.load()) - { - void* p_data = malloc(MAX_RECEIVE_LENGTH); - ProtoMsgHeader tmp_header; - int ret = scan_proto_com_ptr_->receiveProtoMsg(p_data, MAX_RECEIVE_LENGTH, tmp_header); - if (start_check) - { - if (tmp_header.msg_id == 0) - { - start_check = false; - } - else - { - continue; - } - } - if (ret == -1) - { - RS_WARNING << "Packets Protobuf receiving error" << RS_REND; - continue; - } - scan_recv_queue_.push(std::make_pair(p_data, tmp_header)); - if (scan_recv_queue_.is_task_finished_.load()) - { - scan_recv_queue_.is_task_finished_.store(false); - thread_pool_ptr_->commit([&]() { spliceMsopPkts(); }); - } - } -} - -inline void PacketProtoAdapter::spliceMsopPkts() -{ - while (scan_recv_queue_.size() > 0) - { - if (scan_recv_thread_.start_.load()) - { - auto pair = scan_recv_queue_.front(); - old_frmnum_ = new_frmnum_; - new_frmnum_ = pair.second.frame_num; - memcpy((uint8_t*)scan_buff_ + pair.second.msg_id * SPLIT_SIZE, pair.first, SPLIT_SIZE); - if ((old_frmnum_ == new_frmnum_) && (pair.second.msg_id == pair.second.total_msg_cnt - 1)) - { - proto_msg::LidarScan proto_msg; - proto_msg.ParseFromArray(scan_buff_, pair.second.total_msg_length); - localMsopCallback(toRsMsg(proto_msg)); - } - } - free(scan_recv_queue_.front().first); - scan_recv_queue_.pop(); - } - scan_recv_queue_.is_task_finished_.store(true); -} - -inline void PacketProtoAdapter::sendMsop() -{ - while (scan_send_queue_.size() > 0) - { - proto_msg::LidarScan proto_msg = toProtoMsg(scan_send_queue_.popFront()); - if (!scan_proto_com_ptr_->sendSplitMsg(proto_msg)) - { - RS_WARNING << "Msop packets Protobuf sending error" << RS_REND; - } - } - scan_send_queue_.is_task_finished_.store(true); -} - -inline void PacketProtoAdapter::localDifopCallback(const PacketMsg& msg) -{ - for (auto& cb : packet_cb_vec_) - { - cb(msg); - } -} - -inline void PacketProtoAdapter::recvDifopPkts() -{ - while (packet_recv_thread_.start_.load()) - { - void* p_data = malloc(MAX_RECEIVE_LENGTH); - ProtoMsgHeader tmp_header; - int ret = packet_proto_com_ptr_->receiveProtoMsg(p_data, MAX_RECEIVE_LENGTH, tmp_header); - - if (ret == -1) - { - continue; - } - packet_recv_queue_.push(std::make_pair(p_data, tmp_header)); - if (packet_recv_queue_.is_task_finished_.load()) - { - packet_recv_queue_.is_task_finished_.store(false); - thread_pool_ptr_->commit([&]() { spliceDifopPkts(); }); - } - } -} - -inline void PacketProtoAdapter::spliceDifopPkts() -{ - while (packet_recv_queue_.size() > 0) - { - if (packet_recv_thread_.start_.load()) - { - auto pair = packet_recv_queue_.front(); - proto_msg::LidarPacket protomsg; - protomsg.ParseFromArray(pair.first, pair.second.msg_length); - localDifopCallback(toRsMsg(protomsg)); - } - free(packet_recv_queue_.front().first); - packet_recv_queue_.pop(); - } - packet_recv_queue_.is_task_finished_.store(true); -} - -inline void PacketProtoAdapter::sendDifop() -{ - while (packet_send_queue_.size() > 0) - { - proto_msg::LidarPacket proto_msg = toProtoMsg(packet_send_queue_.popFront()); - if (!packet_proto_com_ptr_->sendSingleMsg(proto_msg)) - { - RS_WARNING << "Difop packets Protobuf sending error" << RS_REND; - } - } - packet_send_queue_.is_task_finished_.store(true); -} - -} // namespace lidar -} // namespace robosense -#endif // PROTO_FOUND \ No newline at end of file diff --git a/src/adapter/packet_ros_adapter.hpp b/src/adapter/packet_ros_adapter.hpp deleted file mode 100644 index e134fcbc..00000000 --- a/src/adapter/packet_ros_adapter.hpp +++ /dev/null @@ -1,247 +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 -#ifdef ROS_FOUND -#include -#include "adapter/adapter_base.hpp" -#include "msg/ros_msg_translator.h" - -namespace robosense -{ -namespace lidar -{ -class PacketRosAdapter : virtual public AdapterBase -{ -public: - PacketRosAdapter() = default; - ~PacketRosAdapter() = default; - void init(const YAML::Node& config); - void regRecvCallback(const std::function& callback); - void regRecvCallback(const std::function& callback); - void sendScan(const ScanMsg& msg); - void sendPacket(const PacketMsg& msg); - -private: - void localMsopCallback(const rslidar_msgs::rslidarScan& msg); - void localDifopCallback(const rslidar_msgs::rslidarPacket& msg); - -private: - std::unique_ptr nh_; - std::vector> scan_cb_vec_; - std::vector> packet_cb_vec_; - ros::Publisher scan_pub_; - ros::Publisher packet_pub_; - ros::Subscriber scan_sub_; - ros::Subscriber packet_sub_; - LidarType lidar_type_; -}; - -inline void PacketRosAdapter::init(const YAML::Node& config) -{ - int msg_source; - bool send_packet_ros; - std::string ros_recv_topic; - std::string ros_send_topic; - std::string lidar_type_str; - nh_ = std::unique_ptr(new ros::NodeHandle()); - yamlReadAbort(config, "msg_source", msg_source); - yamlRead(config, "send_packet_ros", send_packet_ros, false); - yamlRead(config["driver"], "lidar_type", lidar_type_str, "RS16"); - yamlRead(config["ros"], "ros_recv_packet_topic", ros_recv_topic, "rslidar_packets"); - yamlRead(config["ros"], "ros_send_packet_topic", ros_send_topic, "rslidar_packets"); - lidar_type_ = RSDriverParam::strToLidarType(lidar_type_str); - if (msg_source == MsgSource::MSG_FROM_ROS_PACKET) - { - packet_sub_ = nh_->subscribe(ros_recv_topic + "_difop", 1, &PacketRosAdapter::localDifopCallback, this); - scan_sub_ = nh_->subscribe(ros_recv_topic, 1, &PacketRosAdapter::localMsopCallback, this); - send_packet_ros = false; - } - if (send_packet_ros) - { - packet_pub_ = nh_->advertise(ros_send_topic + "_difop", 10); - scan_pub_ = nh_->advertise(ros_send_topic, 10); - } -} - -inline void PacketRosAdapter::regRecvCallback(const std::function& callback) -{ - scan_cb_vec_.emplace_back(callback); -} - -inline void PacketRosAdapter::regRecvCallback(const std::function& callback) -{ - packet_cb_vec_.emplace_back(callback); -} - -inline void PacketRosAdapter::sendScan(const ScanMsg& msg) -{ - scan_pub_.publish(toRosMsg(msg)); -} - -inline void PacketRosAdapter::sendPacket(const PacketMsg& msg) -{ - packet_pub_.publish(toRosMsg(msg)); -} - -inline void PacketRosAdapter::localMsopCallback(const rslidar_msgs::rslidarScan& msg) -{ - for (auto& cb : scan_cb_vec_) - { - cb(toRsMsg(lidar_type_, PktType::MSOP, msg)); - } -} - -inline void PacketRosAdapter::localDifopCallback(const rslidar_msgs::rslidarPacket& msg) -{ - for (auto& cb : packet_cb_vec_) - { - cb(toRsMsg(lidar_type_, PktType::DIFOP, msg)); - } -} - -} // namespace lidar -} // namespace robosense -#endif // ROS_FOUND - -#ifdef ROS2_FOUND -#include -#include "adapter/adapter_base.hpp" -#include "msg/ros_msg_translator.h" -namespace robosense -{ -namespace lidar -{ -class PacketRosAdapter : virtual public AdapterBase -{ -public: - PacketRosAdapter() = default; - ~PacketRosAdapter(); - void init(const YAML::Node& config); - void start(); - void regRecvCallback(const std::function& callback); - void regRecvCallback(const std::function& callback); - void sendScan(const ScanMsg& msg); - void sendPacket(const PacketMsg& msg); - -private: - void localMsopCallback(const rslidar_msg::msg::RslidarScan::SharedPtr msg); - void localDifopCallback(const rslidar_msg::msg::RslidarPacket::SharedPtr msg); - -private: - std::shared_ptr node_ptr_; - rclcpp::Publisher::SharedPtr scan_pub_; - rclcpp::Publisher::SharedPtr packet_pub_; - rclcpp::Subscription::SharedPtr scan_sub_; - rclcpp::Subscription::SharedPtr packet_sub_; - std::vector> scan_cb_vec_; - std::vector> packet_cb_vec_; - LidarType lidar_type_; -}; -inline PacketRosAdapter::~PacketRosAdapter() -{ - stop(); -} - -inline void PacketRosAdapter::init(const YAML::Node& config) -{ - int msg_source; - bool send_packet_ros; - std::string ros_recv_topic; - std::string ros_send_topic; - std::string lidar_type_str; - node_ptr_.reset(new rclcpp::Node("rslidar_packets_adapter")); - yamlReadAbort(config, "msg_source", msg_source); - yamlRead(config, "send_packet_ros", send_packet_ros, false); - yamlRead(config["driver"], "lidar_type", lidar_type_str, "RS16"); - yamlRead(config["ros"], "ros_recv_packet_topic", ros_recv_topic, "rslidar_packets"); - yamlRead(config["ros"], "ros_send_packet_topic", ros_send_topic, "rslidar_packets"); - lidar_type_ = RSDriverParam::strToLidarType(lidar_type_str); - if (msg_source == MsgSource::MSG_FROM_ROS_PACKET) - { - packet_sub_ = node_ptr_->create_subscription( - ros_recv_topic + "_difop", 10, - [this](const rslidar_msg::msg::RslidarPacket::SharedPtr msg) { localDifopCallback(msg); }); - scan_sub_ = node_ptr_->create_subscription( - ros_recv_topic, 10, [this](const rslidar_msg::msg::RslidarScan::SharedPtr msg) { localMsopCallback(msg); }); - } - if (send_packet_ros) - { - packet_pub_ = node_ptr_->create_publisher(ros_send_topic + "_difop", 10); - scan_pub_ = node_ptr_->create_publisher(ros_send_topic, 10); - } -} - -inline void PacketRosAdapter::start() -{ - std::thread t([this]() { rclcpp::spin(node_ptr_); }); - t.detach(); -} - -inline void PacketRosAdapter::regRecvCallback(const std::function& callback) -{ - scan_cb_vec_.emplace_back(callback); -} - -inline void PacketRosAdapter::regRecvCallback(const std::function& callback) -{ - packet_cb_vec_.emplace_back(callback); -} - -inline void PacketRosAdapter::sendScan(const ScanMsg& msg) -{ - scan_pub_->publish(toRosMsg(msg)); -} - -inline void PacketRosAdapter::sendPacket(const PacketMsg& msg) -{ - packet_pub_->publish(toRosMsg(msg)); -} - -inline void PacketRosAdapter::localMsopCallback(const rslidar_msg::msg::RslidarScan::SharedPtr msg) -{ - for (auto& cb : scan_cb_vec_) - { - cb(toRsMsg(lidar_type_, PktType::MSOP, *msg)); - } -} - -inline void PacketRosAdapter::localDifopCallback(const rslidar_msg::msg::RslidarPacket::SharedPtr msg) -{ - for (auto& cb : packet_cb_vec_) - { - cb(toRsMsg(lidar_type_, PktType::DIFOP, *msg)); - } -} -} // namespace lidar -} // namespace robosense -#endif // ROS2_FOUND \ No newline at end of file diff --git a/src/adapter/point_cloud_protobuf_adapter.hpp b/src/adapter/point_cloud_protobuf_adapter.hpp deleted file mode 100644 index 4bf42ec0..00000000 --- a/src/adapter/point_cloud_protobuf_adapter.hpp +++ /dev/null @@ -1,233 +0,0 @@ -/********************************************************************************************************************* -Copyright (c) 2020 RoboSense -All rights reserved - -By downloading, copying, installing or using the software you agree to this license. If you do not agree to this -license, do not download, install, copy or use the software. - -License Agreement -For RoboSense LiDAR SDK Library -(3-clause BSD License) - -Redistribution and use in source and binary forms, with or without modification, are permitted provided that the -following conditions are met: - -1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following -disclaimer. - -2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following -disclaimer in the documentation and/or other materials provided with the distribution. - -3. Neither the names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used -to endorse or promote products derived from this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, -INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, -SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, -WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF -THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*********************************************************************************************************************/ - -#pragma once -#ifdef PROTO_FOUND -#include "adapter/adapter_base.hpp" -#include "utility/protobuf_communicator.hpp" -#include "msg/proto_msg_translator.h" -constexpr size_t RECEIVE_BUF_SIZE = 10000000; - -namespace robosense -{ -namespace lidar -{ -class PointCloudProtoAdapter : virtual public AdapterBase -{ -public: - PointCloudProtoAdapter(); - ~PointCloudProtoAdapter(); - void init(const YAML::Node& config); - void start(); - void stop(); - void regRecvCallback(const std::function& callback); - void sendPointCloud(const LidarPointCloudMsg& msg); - -private: - void localCallback(const LidarPointCloudMsg& msg); - void sendPoints(); - void recvPoints(); - void splicePoints(); - -private: - std::vector> point_cloud_cb_vec_; - lidar::Queue point_cloud_send_queue_; - lidar::Queue> point_cloud_recv_queue_; - std::unique_ptr proto_com_ptr_; - lidar::ThreadPool::Ptr thread_pool_ptr_; - lidar::Thread recv_thread_; - int old_frmnum_; - int new_frmnum_; - void* buff_; -}; - -inline PointCloudProtoAdapter::PointCloudProtoAdapter() : old_frmnum_(0), new_frmnum_(0) -{ - thread_pool_ptr_.reset(new lidar::ThreadPool()); -} - -inline PointCloudProtoAdapter::~PointCloudProtoAdapter() -{ - stop(); -} - -inline void PointCloudProtoAdapter::init(const YAML::Node& config) -{ - int msg_source = 0; - bool send_point_cloud_proto; - std::string point_cloud_send_port; - std::string point_cloud_send_ip; - uint16_t point_cloud_recv_port; - proto_com_ptr_.reset(new ProtoCommunicator); - yamlReadAbort(config, "msg_source", msg_source); - yamlRead(config, "send_point_cloud_proto", send_point_cloud_proto, false); - yamlReadAbort(config["proto"], "point_cloud_send_port", point_cloud_send_port); - yamlReadAbort(config["proto"], "point_cloud_send_ip", point_cloud_send_ip); - yamlReadAbort(config["proto"], "point_cloud_recv_port", point_cloud_recv_port); - if (msg_source == MsgSource::MSG_FROM_PROTO_POINTCLOUD) - { - if (proto_com_ptr_->initReceiver(point_cloud_recv_port) == -1) - { - RS_ERROR << "PointCloudProtoAdapter: Create UDP Receiver Socket failed or Bind Network failed!" << RS_REND; - exit(-1); - } - send_point_cloud_proto = false; - } - if (send_point_cloud_proto) - { - if (proto_com_ptr_->initSender(point_cloud_send_port, point_cloud_send_ip) == -1) - { - RS_ERROR << "PointCloudProtoAdapter: Create UDP Sender Socket failed ! " << RS_REND; - exit(-1); - } - } -} - -inline void PointCloudProtoAdapter::start() -{ - buff_ = malloc(RECEIVE_BUF_SIZE); - recv_thread_.start_ = true; - recv_thread_.thread_.reset(new std::thread([this]() { recvPoints(); })); -} - -inline void PointCloudProtoAdapter::stop() -{ - if (recv_thread_.start_.load()) - { - recv_thread_.start_.store(false); - recv_thread_.thread_->join(); - free(buff_); - } -} - -inline void PointCloudProtoAdapter::regRecvCallback(const std::function& callback) -{ - point_cloud_cb_vec_.emplace_back(callback); -} - -inline void PointCloudProtoAdapter::sendPointCloud(const LidarPointCloudMsg& msg) -{ - if (point_cloud_send_queue_.size() > 10) - { - RS_WARNING << "Point Cloud Protobuf Sender buffer over flow!" << RS_REND; - point_cloud_send_queue_.clear(); - } - point_cloud_send_queue_.push(msg); - if (point_cloud_send_queue_.is_task_finished_.load()) - { - point_cloud_send_queue_.is_task_finished_.store(false); - thread_pool_ptr_->commit([this]() { sendPoints(); }); - } -} - -inline void PointCloudProtoAdapter::localCallback(const LidarPointCloudMsg& rs_msg) -{ - for (auto& cb : point_cloud_cb_vec_) - { - cb(rs_msg); - } -} - -inline void PointCloudProtoAdapter::sendPoints() -{ - while (point_cloud_send_queue_.size() > 0) - { - proto_msg::LidarPointCloud proto_msg = toProtoMsg(point_cloud_send_queue_.popFront()); - if (!proto_com_ptr_->sendSplitMsg(proto_msg)) - { - RS_WARNING << "PointCloud Protobuf sending error" << RS_REND; - } - } - point_cloud_send_queue_.is_task_finished_.store(true); -} - -inline void PointCloudProtoAdapter::recvPoints() -{ - bool start_check = true; - while (recv_thread_.start_.load()) - { - void* p_data = malloc(MAX_RECEIVE_LENGTH); - ProtoMsgHeader tmp_header; - int ret = proto_com_ptr_->receiveProtoMsg(p_data, MAX_RECEIVE_LENGTH, tmp_header); - if (start_check) - { - if (tmp_header.msg_id == 0) - { - start_check = false; - } - else - { - continue; - } - } - if (ret == -1) - { - RS_WARNING << "PointCloud Protobuf receiving error" << RS_REND; - continue; - } - - point_cloud_recv_queue_.push(std::make_pair(p_data, tmp_header)); - - if (point_cloud_recv_queue_.is_task_finished_.load()) - { - point_cloud_recv_queue_.is_task_finished_.store(false); - thread_pool_ptr_->commit([&]() { splicePoints(); }); - } - } -} - -inline void PointCloudProtoAdapter::splicePoints() -{ - while (point_cloud_recv_queue_.size() > 0) - { - if (recv_thread_.start_.load()) - { - auto pair = point_cloud_recv_queue_.front(); - old_frmnum_ = new_frmnum_; - new_frmnum_ = pair.second.frame_num; - memcpy((uint8_t*)buff_ + pair.second.msg_id * SPLIT_SIZE, pair.first, SPLIT_SIZE); - if ((old_frmnum_ == new_frmnum_) && (pair.second.msg_id == pair.second.total_msg_cnt - 1)) - { - proto_msg::LidarPointCloud proto_msg; - proto_msg.ParseFromArray(buff_, pair.second.total_msg_length); - localCallback(toRsMsg(proto_msg)); - } - } - free(point_cloud_recv_queue_.front().first); - point_cloud_recv_queue_.pop(); - } - point_cloud_recv_queue_.is_task_finished_.store(true); -} - -} // namespace lidar -} // namespace robosense -#endif // PROTO_FOUND \ No newline at end of file diff --git a/src/adapter/point_cloud_ros_adapter.hpp b/src/adapter/point_cloud_ros_adapter.hpp deleted file mode 100644 index c7e0415c..00000000 --- a/src/adapter/point_cloud_ros_adapter.hpp +++ /dev/null @@ -1,119 +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 -#ifdef ROS_FOUND -#include -#include "adapter/adapter_base.hpp" -#include "msg/ros_msg_translator.h" - -namespace robosense -{ -namespace lidar -{ -class PointCloudRosAdapter : virtual public AdapterBase -{ -public: - PointCloudRosAdapter() = default; - ~PointCloudRosAdapter() = default; - void init(const YAML::Node& config); - void sendPointCloud(const LidarPointCloudMsg& msg); - -private: - std::shared_ptr nh_; - ros::Publisher point_cloud_pub_; -}; - -inline void PointCloudRosAdapter::init(const YAML::Node& config) -{ - bool send_point_cloud_ros; - std::string ros_send_topic; - nh_ = std::unique_ptr(new ros::NodeHandle()); - yamlRead(config, "send_point_cloud_ros", send_point_cloud_ros, false); - yamlRead(config["ros"], "ros_send_point_cloud_topic", ros_send_topic, "rslidar_points"); - if (send_point_cloud_ros) - { - point_cloud_pub_ = nh_->advertise(ros_send_topic, 10); - } -} - -inline void PointCloudRosAdapter::sendPointCloud(const LidarPointCloudMsg& msg) -{ - point_cloud_pub_.publish(toRosMsg(msg)); -} - -} // namespace lidar -} // namespace robosense -#endif // ROS_FOUND - -#ifdef ROS2_FOUND -#include -#include "adapter/adapter_base.hpp" -#include "msg/ros_msg_translator.h" -namespace robosense -{ -namespace lidar -{ -class PointCloudRosAdapter : virtual public AdapterBase -{ -public: - PointCloudRosAdapter() = default; - ~PointCloudRosAdapter() = default; - void init(const YAML::Node& config); - void sendPointCloud(const LidarPointCloudMsg& msg); - -private: - std::shared_ptr node_ptr_; - rclcpp::Publisher::SharedPtr point_cloud_pub_; -}; - -inline void PointCloudRosAdapter::init(const YAML::Node& config) -{ - bool send_point_cloud_ros; - std::string ros_send_topic; - node_ptr_.reset(new rclcpp::Node("rslidar_points_adapter")); - yamlRead(config, "send_point_cloud_ros", send_point_cloud_ros, false); - yamlRead(config["ros"], "ros_send_point_cloud_topic", ros_send_topic, "rslidar_points"); - if (send_point_cloud_ros) - { - point_cloud_pub_ = node_ptr_->create_publisher(ros_send_topic, 1); - } -} - -inline void PointCloudRosAdapter::sendPointCloud(const LidarPointCloudMsg& msg) -{ - point_cloud_pub_->publish(toRosMsg(msg)); -} - -} // namespace lidar -} // namespace robosense -#endif // ROS2_FOUND \ No newline at end of file diff --git a/src/manager/adapter_manager.cpp b/src/manager/adapter_manager.cpp deleted file mode 100644 index 7d51a6c3..00000000 --- a/src/manager/adapter_manager.cpp +++ /dev/null @@ -1,403 +0,0 @@ -/********************************************************************************************************************* -Copyright (c) 2020 RoboSense -All rights reserved - -By downloading, copying, installing or using the software you agree to this license. If you do not agree to this -license, do not download, install, copy or use the software. - -License Agreement -For RoboSense LiDAR SDK Library -(3-clause BSD License) - -Redistribution and use in source and binary forms, with or without modification, are permitted provided that the -following conditions are met: - -1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following -disclaimer. - -2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following -disclaimer in the documentation and/or other materials provided with the distribution. - -3. Neither the names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used -to endorse or promote products derived from this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, -INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, -SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, -WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF -THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*********************************************************************************************************************/ - -#include "manager/adapter_manager.h" -namespace robosense -{ -namespace lidar -{ -AdapterManager::~AdapterManager() -{ - stop(); -} - -void AdapterManager::init(const YAML::Node& config) -{ - packet_thread_flag_ = false; - point_cloud_thread_flag_ = false; - int msg_source = 0; - bool send_point_cloud_ros; - bool send_packet_ros; - bool send_point_cloud_proto; - bool send_packet_proto; - bool send_camera_trigger_ros; - std::string pcap_dir; - double pcap_rate; - bool pcap_repeat; - YAML::Node common_config = yamlSubNodeAbort(config, "common"); - yamlRead(common_config, "msg_source", msg_source, 0); - yamlRead(common_config, "send_packet_ros", send_packet_ros, false); - yamlRead(common_config, "send_point_cloud_ros", send_point_cloud_ros, false); - yamlRead(common_config, "send_point_cloud_proto", send_point_cloud_proto, false); - yamlRead(common_config, "send_packet_proto", send_packet_proto, false); - yamlRead(common_config, "send_camera_trigger_ros", send_camera_trigger_ros, false); - yamlRead(common_config, "pcap_path", pcap_dir, ""); - yamlRead(common_config, "pcap_rate", pcap_rate, 1); - yamlRead(common_config, "pcap_repeat", pcap_repeat, true); - YAML::Node lidar_config = yamlSubNodeAbort(config, "lidar"); - for (uint8_t i = 0; i < lidar_config.size(); ++i) - { - AdapterBase::Ptr recv_ptr; - /*Receiver*/ - switch (msg_source) - { - case MsgSource::MSG_FROM_LIDAR: // use driver - RS_INFO << "------------------------------------------------------" << RS_REND; - RS_INFO << "Receive Packets From : Online LiDAR" << RS_REND; - RS_INFO << "Msop Port: " << lidar_config[i]["driver"]["msop_port"].as() << RS_REND; - RS_INFO << "Difop Port: " << lidar_config[i]["driver"]["difop_port"].as() << RS_REND; - RS_INFO << "------------------------------------------------------" << RS_REND; - point_cloud_thread_flag_ = true; - lidar_config[i]["msg_source"] = (int)MsgSource::MSG_FROM_LIDAR; - recv_ptr = createReceiver(lidar_config[i], AdapterType::DriverAdapter); - point_cloud_receive_adapter_vec_.push_back(recv_ptr); - if (send_packet_ros || send_packet_proto) - { - packet_receive_adapter_vec_.push_back(recv_ptr); - packet_thread_flag_ = true; - } - break; - - case MsgSource::MSG_FROM_ROS_PACKET: // pkt from ros - RS_INFO << "------------------------------------------------------" << RS_REND; - RS_INFO << "Receive Packets From : ROS" << RS_REND; - RS_INFO << "Msop Topic: " << lidar_config[i]["ros"]["ros_recv_packet_topic"].as() << RS_REND; - RS_INFO << "Difop Topic: " << lidar_config[i]["ros"]["ros_recv_packet_topic"].as() << "_difop" - << RS_REND; - RS_INFO << "------------------------------------------------------" << RS_REND; - point_cloud_thread_flag_ = false; - packet_thread_flag_ = true; - lidar_config[i]["msg_source"] = (int)MsgSource::MSG_FROM_ROS_PACKET; - send_packet_ros = false; - point_cloud_receive_adapter_vec_.emplace_back(createReceiver(lidar_config[i], AdapterType::DriverAdapter)); - packet_receive_adapter_vec_.emplace_back(createReceiver(lidar_config[i], AdapterType::PacketRosAdapter)); - packet_receive_adapter_vec_[i]->regRecvCallback( - std::bind(&AdapterBase::decodeScan, point_cloud_receive_adapter_vec_[i], std::placeholders::_1)); - packet_receive_adapter_vec_[i]->regRecvCallback( - std::bind(&AdapterBase::decodePacket, point_cloud_receive_adapter_vec_[i], std::placeholders::_1)); - break; - - case MsgSource::MSG_FROM_PCAP: // pcap - RS_INFO << "------------------------------------------------------" << RS_REND; - RS_INFO << "Receive Packets From : Pcap" << RS_REND; - RS_INFO << "Msop Port: " << lidar_config[i]["driver"]["msop_port"].as() << RS_REND; - RS_INFO << "Difop Port: " << lidar_config[i]["driver"]["difop_port"].as() << RS_REND; - RS_INFO << "------------------------------------------------------" << RS_REND; - point_cloud_thread_flag_ = true; - lidar_config[i]["msg_source"] = (int)MsgSource::MSG_FROM_PCAP; - lidar_config[i]["driver"]["read_pcap"] = true; - lidar_config[i]["driver"]["pcap_path"] = pcap_dir; - lidar_config[i]["driver"]["pcap_rate"] = pcap_rate; - lidar_config[i]["driver"]["pcap_repeat"] = pcap_repeat; - recv_ptr = createReceiver(lidar_config[i], AdapterType::DriverAdapter); - point_cloud_receive_adapter_vec_.push_back(recv_ptr); - if (send_packet_ros || send_packet_proto) - { - packet_receive_adapter_vec_.push_back(recv_ptr); - packet_thread_flag_ = true; - } - break; - - case MsgSource::MSG_FROM_PROTO_PACKET: // packets from proto - RS_INFO << "------------------------------------------------------" << RS_REND; - RS_INFO << "Receive Packets From : Protobuf-UDP" << RS_REND; - RS_INFO << "Msop Port: " << lidar_config[i]["proto"]["msop_recv_port"].as() << RS_REND; - RS_INFO << "Difop Port: " << lidar_config[i]["proto"]["difop_recv_port"].as() << RS_REND; - RS_INFO << "------------------------------------------------------" << RS_REND; - point_cloud_thread_flag_ = false; - packet_thread_flag_ = true; - lidar_config[i]["msg_source"] = (int)MsgSource::MSG_FROM_PROTO_PACKET; - send_packet_proto = false; - point_cloud_receive_adapter_vec_.emplace_back(createReceiver(lidar_config[i], AdapterType::DriverAdapter)); - packet_receive_adapter_vec_.emplace_back(createReceiver(lidar_config[i], AdapterType::PacketProtoAdapter)); - packet_receive_adapter_vec_[i]->regRecvCallback( - std::bind(&AdapterBase::decodeScan, point_cloud_receive_adapter_vec_[i], std::placeholders::_1)); - packet_receive_adapter_vec_[i]->regRecvCallback( - std::bind(&AdapterBase::decodePacket, point_cloud_receive_adapter_vec_[i], std::placeholders::_1)); - break; - - case MsgSource::MSG_FROM_PROTO_POINTCLOUD: // point cloud from proto - RS_INFO << "------------------------------------------------------" << RS_REND; - RS_INFO << "Receive PointCloud From : Protobuf-UDP" << RS_REND; - RS_INFO << "PointCloud Port: " << lidar_config[i]["proto"]["point_cloud_recv_port"].as() << RS_REND; - RS_INFO << "------------------------------------------------------" << RS_REND; - point_cloud_thread_flag_ = true; - packet_thread_flag_ = false; - lidar_config[i]["msg_source"] = (int)MsgSource::MSG_FROM_PROTO_POINTCLOUD; - send_point_cloud_proto = false; - send_packet_ros = false; - send_packet_proto = false; - send_camera_trigger_ros = false; - point_cloud_receive_adapter_vec_.emplace_back( - createReceiver(lidar_config[i], AdapterType::PointCloudProtoAdapter)); - packet_receive_adapter_vec_.emplace_back(nullptr); - break; - - default: - RS_ERROR << "Not use LiDAR or Wrong LiDAR message source! Abort!" << RS_REND; - exit(-1); - } - - /*Transmitter*/ - if (send_packet_ros) - { - RS_DEBUG << "------------------------------------------------------" << RS_REND; - RS_DEBUG << "Send Packets To : ROS" << RS_REND; - RS_DEBUG << "Msop Topic: " << lidar_config[i]["ros"]["ros_send_packet_topic"].as() << RS_REND; - RS_DEBUG << "Difop Topic: " << lidar_config[i]["ros"]["ros_send_packet_topic"].as() << "_difop" - << RS_REND; - RS_DEBUG << "------------------------------------------------------" << RS_REND; - lidar_config[i]["send_packet_ros"] = true; - AdapterBase::Ptr transmitter_ptr = createTransmitter(lidar_config[i], AdapterType::PacketRosAdapter); - packet_transmit_adapter_vec_.emplace_back(transmitter_ptr); - packet_receive_adapter_vec_[i]->regRecvCallback( - std::bind(&AdapterBase::sendScan, transmitter_ptr, std::placeholders::_1)); - packet_receive_adapter_vec_[i]->regRecvCallback( - std::bind(&AdapterBase::sendPacket, transmitter_ptr, std::placeholders::_1)); - } - if (send_packet_proto) - { - RS_DEBUG << "------------------------------------------------------" << RS_REND; - RS_DEBUG << "Send Packets To : Protobuf-UDP" << RS_REND; - RS_DEBUG << "Msop Port: " << lidar_config[i]["proto"]["msop_send_port"].as() << RS_REND; - RS_DEBUG << "Difop Port: " << lidar_config[i]["proto"]["difop_send_port"].as() << RS_REND; - RS_DEBUG << "Target IP: " << lidar_config[i]["proto"]["packet_send_ip"].as() << RS_REND; - RS_DEBUG << "------------------------------------------------------" << RS_REND; - lidar_config[i]["send_packet_proto"] = true; - AdapterBase::Ptr transmitter_ptr = createTransmitter(lidar_config[i], AdapterType::PacketProtoAdapter); - packet_transmit_adapter_vec_.emplace_back(transmitter_ptr); - packet_receive_adapter_vec_[i]->regRecvCallback( - std::bind(&AdapterBase::sendScan, transmitter_ptr, std::placeholders::_1)); - packet_receive_adapter_vec_[i]->regRecvCallback( - std::bind(&AdapterBase::sendPacket, transmitter_ptr, std::placeholders::_1)); - } - if (send_point_cloud_ros) - { - RS_DEBUG << "------------------------------------------------------" << RS_REND; - RS_DEBUG << "Send PointCloud To : ROS" << RS_REND; - RS_DEBUG << "PointCloud Topic: " << lidar_config[i]["ros"]["ros_send_point_cloud_topic"].as() - << RS_REND; - RS_DEBUG << "------------------------------------------------------" << RS_REND; - lidar_config[i]["send_point_cloud_ros"] = true; - AdapterBase::Ptr transmitter_ptr = createTransmitter(lidar_config[i], AdapterType::PointCloudRosAdapter); - point_cloud_transmit_adapter_vec_.emplace_back(transmitter_ptr); - point_cloud_receive_adapter_vec_[i]->regRecvCallback( - std::bind(&AdapterBase::sendPointCloud, transmitter_ptr, std::placeholders::_1)); - } - if (send_point_cloud_proto) - { - RS_DEBUG << "------------------------------------------------------" << RS_REND; - RS_DEBUG << "Send PointCloud To : Protobuf-UDP" << RS_REND; - RS_DEBUG << "PointCloud Port: " << lidar_config[i]["proto"]["point_cloud_send_port"].as() << RS_REND; - RS_DEBUG << "Target IP: " << lidar_config[i]["proto"]["point_cloud_send_ip"].as() << RS_REND; - RS_DEBUG << "------------------------------------------------------" << RS_REND; - lidar_config[i]["send_point_cloud_proto"] = true; - AdapterBase::Ptr transmitter_ptr = createTransmitter(lidar_config[i], AdapterType::PointCloudProtoAdapter); - point_cloud_transmit_adapter_vec_.emplace_back(transmitter_ptr); - point_cloud_receive_adapter_vec_[i]->regRecvCallback( - std::bind(&AdapterBase::sendPointCloud, transmitter_ptr, std::placeholders::_1)); - } - if (send_camera_trigger_ros) - { - RS_DEBUG << "------------------------------------------------------" << RS_REND; - RS_DEBUG << "Send Camera Trigger To : ROS" << RS_REND; - for (auto iter : lidar_config[i]["camera"]) - { - RS_DEBUG << "Camera : " << iter["frame_id"].as() - << " Trigger Angle : " << iter["trigger_angle"].as() << RS_REND; - } - RS_DEBUG << "------------------------------------------------------" << RS_REND; - AdapterBase::Ptr transmitter_ptr = createTransmitter(lidar_config[i], AdapterType::CameraTriggerRosAdapter); - point_cloud_receive_adapter_vec_[i]->regRecvCallback( - std::bind(&AdapterBase::sendCameraTrigger, transmitter_ptr, std::placeholders::_1)); - } - } -} - -void AdapterManager::start() -{ - if (point_cloud_thread_flag_) - { - for (auto& iter : point_cloud_receive_adapter_vec_) - { - if (iter != nullptr) - iter->start(); - } - } - if (packet_thread_flag_) - { - for (auto& iter : packet_receive_adapter_vec_) - { - if (iter != nullptr) - { - iter->start(); - } - } - } -} - -void AdapterManager::stop() -{ - if (point_cloud_thread_flag_) - { - for (auto& iter : point_cloud_receive_adapter_vec_) - { - if (iter != nullptr) - iter->stop(); - } - } - if (packet_thread_flag_) - { - for (auto& iter : packet_receive_adapter_vec_) - { - if (iter != nullptr) - { - iter->stop(); - } - } - } -} - -std::shared_ptr AdapterManager::createReceiver(const YAML::Node& config, const AdapterType& adapter_type) -{ - std::shared_ptr receiver; - switch (adapter_type) - { - case AdapterType::DriverAdapter: - receiver = std::dynamic_pointer_cast(std::make_shared()); - receiver->init(config); - break; - - case AdapterType::PacketRosAdapter: -#if (ROS_FOUND || ROS2_FOUND) - receiver = std::dynamic_pointer_cast(std::make_shared()); - receiver->init(config); - break; -#else - RS_ERROR << "ROS not found! Could not use ROS functions!" << RS_REND; - exit(-1); -#endif - - case AdapterType::PacketProtoAdapter: -#ifdef PROTO_FOUND - receiver = std::dynamic_pointer_cast(std::make_shared()); - receiver->init(config); - break; -#else - RS_ERROR << "Protobuf not found! Could not use Protobuf functions!" << RS_REND; - exit(-1); -#endif - - case AdapterType::PointCloudProtoAdapter: -#ifdef PROTO_FOUND - receiver = std::dynamic_pointer_cast(std::make_shared()); - receiver->init(config); - break; -#else - RS_ERROR << "Protobuf not found! Could not use Protobuf functions!" << RS_REND; - exit(-1); -#endif - - default: - RS_ERROR << "Create receiver failed. Abort!" << RS_REND; - exit(-1); - } - - return receiver; -} - -std::shared_ptr AdapterManager::createTransmitter(const YAML::Node& config, - const AdapterType& adapter_type) -{ - std::shared_ptr transmitter; - switch (adapter_type) - { - case AdapterType::PacketRosAdapter: -#if (ROS_FOUND || ROS2_FOUND) - transmitter = std::dynamic_pointer_cast(std::make_shared()); - transmitter->init(config); - break; -#else - RS_ERROR << "ROS not found! Could not use ROS functions!" << RS_REND; - exit(-1); -#endif - - case AdapterType::PacketProtoAdapter: -#ifdef PROTO_FOUND - transmitter = std::dynamic_pointer_cast(std::make_shared()); - transmitter->init(config); - break; -#else - RS_ERROR << "Protobuf not found! Could not use Protobuf functions!" << RS_REND; - exit(-1); -#endif - - case AdapterType::PointCloudRosAdapter: -#if (ROS_FOUND || ROS2_FOUND) - transmitter = std::dynamic_pointer_cast(std::make_shared()); - transmitter->init(config); - break; -#else - RS_ERROR << "ROS not found! Could not use ROS functions!" << RS_REND; - exit(-1); -#endif - - case AdapterType::PointCloudProtoAdapter: -#ifdef PROTO_FOUND - transmitter = std::dynamic_pointer_cast(std::make_shared()); - transmitter->init(config); - break; -#else - RS_ERROR << "Protobuf not found! Could not use Protobuf functions!" << RS_REND; - exit(-1); -#endif - - case AdapterType::CameraTriggerRosAdapter: -#if (ROS_FOUND) ///< Camera trigger not support ROS2 yet - transmitter = std::dynamic_pointer_cast(std::make_shared()); - transmitter->init(config); - break; -#else - RS_ERROR << "ROS not found! Could not use ROS functions!" << RS_REND; - exit(-1); -#endif - - default: - RS_ERROR << "Create transmitter failed. Abort!" << RS_REND; - exit(-1); - } - - return transmitter; -} - -} // namespace lidar -} // namespace robosense diff --git a/src/manager/adapter_manager.h b/src/manager/adapter_manager.h deleted file mode 100644 index 09ce5897..00000000 --- a/src/manager/adapter_manager.h +++ /dev/null @@ -1,107 +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. -*********************************************************************************************************************/ -/******************************************************************************************************************** - * Basic struct: - * - * - * AdapterBase - * / | \ - * / | \ - * ROS: PacketRosAdapter | PointCloudRosAdapter - * Protobuf: PacketProtoAdapter | PointCloudProtoAdapter - * Driver: \ DriverAdapter / - * \ / \ / - * \ / \ / - * \ / \ / - * Packet PointCloud - * - * - * AdapterManager: - * - * step1 - * -msg_source=1 -> packet receiver: DriverAdapter; point cloud receiver: DriverAdapter - * -msg_source=2 -> packet receiver: PacketRosAdapter; point cloud receiver: DriverAdapter - * createReceiver ->-msg_source=3 -> packet receiver: DriverAdapter; point cloud receiver: DriverAdapter - * -msg_source=4 -> packet receiver: PacketProtoAdapter; point cloud receiver: DriverAdapter - * -msg_source=5 -> packet receiver: None; point cloud receiver: PointCloudProtoAdapter - * - * step2 - * - * -send_packet_ros=true -> PacketRosAdapter - * createTransmitter -> -send_point_cloud_ros=true -> PointCloudRosAdapter - * -send_packet_proto=true -> PacketProtoAdapter - * -send_point_cloud_proto=true -> PointCloudProtoAdapter - * - * step3 - * Register the transmitter's sending functions into the receiver - * - * step4 - * Start all the receivers - * - * ******************************************************************************************************************/ - -#pragma once -#include "utility/yaml_reader.hpp" -#include "adapter/driver_adapter.hpp" -#include "adapter/packet_ros_adapter.hpp" -#include "adapter/point_cloud_ros_adapter.hpp" -#include "adapter/packet_protobuf_adapter.hpp" -#include "adapter/point_cloud_protobuf_adapter.hpp" -#include "adapter/camera_trigger_adapter.hpp" - -namespace robosense -{ -namespace lidar -{ -class AdapterManager -{ -public: - AdapterManager() = default; - ~AdapterManager(); - void init(const YAML::Node& config); - void start(); - void stop(); - -private: - std::shared_ptr createReceiver(const YAML::Node& config, const AdapterType& adapter_type); - std::shared_ptr createTransmitter(const YAML::Node& config, const AdapterType& adapter_type); - -private: - bool packet_thread_flag_; - bool point_cloud_thread_flag_; - std::vector packet_receive_adapter_vec_; - std::vector point_cloud_receive_adapter_vec_; - std::vector packet_transmit_adapter_vec_; - std::vector point_cloud_transmit_adapter_vec_; -}; -} // namespace lidar -} // namespace robosense diff --git a/src/manager/node_manager.cpp b/src/manager/node_manager.cpp new file mode 100644 index 00000000..3646fb31 --- /dev/null +++ b/src/manager/node_manager.cpp @@ -0,0 +1,167 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the +following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following +disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following +disclaimer in the documentation and/or other materials provided with the distribution. + +3. Neither the names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, +INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*********************************************************************************************************************/ + +#include "manager/node_manager.hpp" +#include "source/source_driver.hpp" +#include "source/source_pointcloud_ros.hpp" +#include "source/source_packet_ros.hpp" + +namespace robosense +{ +namespace lidar +{ + +void NodeManager::init(const YAML::Node& config) +{ + YAML::Node common_config = yamlSubNodeAbort(config, "common"); + + int msg_source = 0; + yamlRead(common_config, "msg_source", msg_source, 0); + + bool send_packet_ros; + yamlRead(common_config, "send_packet_ros", send_packet_ros, false); + + bool send_point_cloud_ros; + yamlRead(common_config, "send_point_cloud_ros", send_point_cloud_ros, false); + + bool send_point_cloud_proto; + yamlRead(common_config, "send_point_cloud_proto", send_point_cloud_proto, false); + + bool send_packet_proto; + yamlRead(common_config, "send_packet_proto", send_packet_proto, false); + + YAML::Node lidar_config = yamlSubNodeAbort(config, "lidar"); + + for (uint8_t i = 0; i < lidar_config.size(); ++i) + { + std::shared_ptr source; + + switch (msg_source) + { + case SourceType::MSG_FROM_LIDAR: // online lidar + + RS_INFO << "------------------------------------------------------" << RS_REND; + RS_INFO << "Receive Packets From : Online LiDAR" << RS_REND; + RS_INFO << "Msop Port: " << lidar_config[i]["driver"]["msop_port"].as() << RS_REND; + RS_INFO << "Difop Port: " << lidar_config[i]["driver"]["difop_port"].as() << RS_REND; + RS_INFO << "------------------------------------------------------" << RS_REND; + + source = std::make_shared(SourceType::MSG_FROM_LIDAR); + source->init(lidar_config[i]); + break; + + case SourceType::MSG_FROM_ROS_PACKET: // pkt from ros + + RS_INFO << "------------------------------------------------------" << RS_REND; + RS_INFO << "Receive Packets From : ROS" << RS_REND; + RS_INFO << "Msop Topic: " << lidar_config[i]["ros"]["ros_recv_packet_topic"].as() << RS_REND; + RS_INFO << "------------------------------------------------------" << RS_REND; + + source = std::make_shared(); + source->init(lidar_config[i]); + break; + + case SourceType::MSG_FROM_PCAP: // pcap + + RS_INFO << "------------------------------------------------------" << RS_REND; + RS_INFO << "Receive Packets From : Pcap" << RS_REND; + RS_INFO << "Msop Port: " << lidar_config[i]["driver"]["msop_port"].as() << RS_REND; + RS_INFO << "Difop Port: " << lidar_config[i]["driver"]["difop_port"].as() << RS_REND; + RS_INFO << "------------------------------------------------------" << RS_REND; + + source = std::make_shared(SourceType::MSG_FROM_PCAP); + source->init(lidar_config[i]); + break; + + default: + RS_ERROR << "Unsupported LiDAR message source:" << msg_source << "." << RS_REND; + exit(-1); + } + + if (send_packet_ros) + { + RS_DEBUG << "------------------------------------------------------" << RS_REND; + RS_DEBUG << "Send Packets To : ROS" << RS_REND; + RS_DEBUG << "Msop Topic: " << lidar_config[i]["ros"]["ros_send_packet_topic"].as() << RS_REND; + RS_DEBUG << "------------------------------------------------------" << RS_REND; + + std::shared_ptr dst = std::make_shared(); + dst->init(lidar_config[i]); + source->regPacketCallback(dst); + } + + if (send_point_cloud_ros) + { + RS_DEBUG << "------------------------------------------------------" << RS_REND; + RS_DEBUG << "Send PointCloud To : ROS" << RS_REND; + RS_DEBUG << "PointCloud Topic: " << lidar_config[i]["ros"]["ros_send_point_cloud_topic"].as() + << RS_REND; + RS_DEBUG << "------------------------------------------------------" << RS_REND; + + std::shared_ptr dst = std::make_shared(); + dst->init(lidar_config[i]); + source->regPointCloudCallback(dst); + } + + sources_.emplace_back(source); + } +} + +void NodeManager::start() +{ + for (auto& iter : sources_) + { + if (iter != nullptr) + { + iter->start(); + } + } +} + +void NodeManager::stop() +{ + for (auto& iter : sources_) + { + if (iter != nullptr) + { + iter->stop(); + } + } +} + +NodeManager::~NodeManager() +{ + stop(); +} + +} // namespace lidar +} // namespace robosense diff --git a/src/adapter/camera_trigger_adapter.hpp b/src/manager/node_manager.hpp similarity index 60% rename from src/adapter/camera_trigger_adapter.hpp rename to src/manager/node_manager.hpp index ddf6d432..39e0bc84 100644 --- a/src/adapter/camera_trigger_adapter.hpp +++ b/src/manager/node_manager.hpp @@ -31,66 +31,31 @@ THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *********************************************************************************************************************/ #pragma once -#ifdef ROS_FOUND -#include "adapter/adapter_base.hpp" -#include "msg/ros_msg_translator.h" -#include -#include + +#include "utility/yaml_reader.hpp" +#include "source/source.hpp" namespace robosense { namespace lidar { -class CameraTriggerRosAdapter : virtual public AdapterBase + +class NodeManager { public: - CameraTriggerRosAdapter() = default; - ~CameraTriggerRosAdapter() = default; + void init(const YAML::Node& config); - void sendCameraTrigger(const CameraTrigger& msg); + void start(); + void stop(); + + ~NodeManager(); + NodeManager() = default; private: - std::shared_ptr nh_; - std::map pub_map_; -}; -inline void CameraTriggerRosAdapter::init(const YAML::Node& config) -{ - nh_ = std::unique_ptr(new ros::NodeHandle()); - if (config["camera"] && config["camera"].Type() != YAML::NodeType::Null) - { - for (auto iter : config["camera"]) - { - std::string frame_id; - yamlRead(iter, "frame_id", frame_id, "rs_camera"); - ros::Publisher pub = nh_->advertise(frame_id + "_trigger", 10); - pub_map_.emplace(frame_id, pub); - } - } -} - -inline void CameraTriggerRosAdapter::sendCameraTrigger(const CameraTrigger& msg) -{ - auto iter = pub_map_.find(msg.first); - if (iter != pub_map_.end()) - { - iter->second.publish(toRosMsg(msg)); - } -} + std::vector sources_; +}; } // namespace lidar } // namespace robosense -#endif // ROS_FOUND -#ifdef ROS2_FOUND -#include "adapter/adapter_base.hpp" -#include "msg/ros_msg_translator.h" -#include "rclcpp/rclcpp.hpp" -namespace robosense -{ -namespace lidar -{ -///< Add in the future -} // namespace lidar -} // namespace robosense -#endif // ROS2_FOUND \ No newline at end of file diff --git a/src/msg/proto_msg/packet.proto b/src/msg/proto_msg/packet.proto deleted file mode 100644 index 783636d1..00000000 --- a/src/msg/proto_msg/packet.proto +++ /dev/null @@ -1,9 +0,0 @@ -syntax="proto2"; -package proto_msg; - -message LidarPacket -{ - optional bytes data = 1; -} - - diff --git a/src/msg/proto_msg/point_cloud.proto b/src/msg/proto_msg/point_cloud.proto deleted file mode 100644 index 0f3d7635..00000000 --- a/src/msg/proto_msg/point_cloud.proto +++ /dev/null @@ -1,16 +0,0 @@ -syntax="proto2"; -package proto_msg; - -message LidarPointCloud -{ - optional double timestamp = 1; - optional uint32 seq = 2; - optional string frame_id = 3; - optional uint32 height=4; - optional uint32 width=5; - optional bool is_dense=6; - repeated float data = 7; - -} - - diff --git a/src/msg/proto_msg/scan.proto b/src/msg/proto_msg/scan.proto deleted file mode 100644 index d430f258..00000000 --- a/src/msg/proto_msg/scan.proto +++ /dev/null @@ -1,11 +0,0 @@ -syntax="proto2"; -package proto_msg; - -message LidarScan -{ - optional double timestamp = 1; - optional uint32 seq=2; - repeated bytes data = 3; -} - - diff --git a/src/msg/proto_msg_translator.h b/src/msg/proto_msg_translator.h deleted file mode 100644 index 32f1e61d..00000000 --- a/src/msg/proto_msg_translator.h +++ /dev/null @@ -1,142 +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 -#ifdef PROTO_FOUND -#include "msg/rs_msg/lidar_point_cloud_msg.h" -#include "rs_driver/msg/packet_msg.h" -#include "rs_driver/msg/scan_msg.h" -#include "msg/proto_msg/point_cloud.pb.h" -#include "msg/proto_msg/packet.pb.h" -#include "msg/proto_msg/scan.pb.h" -namespace robosense -{ -namespace lidar -{ -/************************************************************************/ -/**Translation functions between RoboSense message and protobuf message**/ -/************************************************************************/ -inline proto_msg::LidarPointCloud toProtoMsg(const LidarPointCloudMsg& rs_msg) -{ - proto_msg::LidarPointCloud proto_msg; - proto_msg.set_timestamp(rs_msg.timestamp); - proto_msg.set_seq(rs_msg.seq); - proto_msg.set_frame_id(rs_msg.frame_id); - proto_msg.set_height(rs_msg.point_cloud_ptr->height); - proto_msg.set_width(rs_msg.point_cloud_ptr->width); - proto_msg.set_is_dense(rs_msg.point_cloud_ptr->is_dense); - - for (size_t i = 0; i < rs_msg.point_cloud_ptr->size(); i++) - { - proto_msg.add_data(rs_msg.point_cloud_ptr->points[i].x); - proto_msg.add_data(rs_msg.point_cloud_ptr->points[i].y); - proto_msg.add_data(rs_msg.point_cloud_ptr->points[i].z); - proto_msg.add_data(rs_msg.point_cloud_ptr->points[i].intensity); - } - - return std::move(proto_msg); -} - -inline LidarPointCloudMsg toRsMsg(const proto_msg::LidarPointCloud& proto_msg) -{ - LidarPointCloudMsg rs_msg; - rs_msg.timestamp = proto_msg.timestamp(); - rs_msg.seq = proto_msg.seq(); - rs_msg.frame_id = proto_msg.frame_id(); - LidarPointCloudMsg::PointCloud* ptr_tmp = new LidarPointCloudMsg::PointCloud(); - for (int i = 0; i < proto_msg.data_size(); i += 4) - { - PointT point; - point.x = proto_msg.data(i); - point.y = proto_msg.data(i + 1); - point.z = proto_msg.data(i + 2); - point.intensity = proto_msg.data(i + 3); - ptr_tmp->push_back(point); - } - ptr_tmp->height = proto_msg.height(); - ptr_tmp->width = proto_msg.width(); - ptr_tmp->is_dense = proto_msg.is_dense(); - rs_msg.point_cloud_ptr.reset(ptr_tmp); - return rs_msg; -} - -inline proto_msg::LidarScan toProtoMsg(const ScanMsg& rs_msg) -{ - proto_msg::LidarScan proto_msg; - proto_msg.set_timestamp(rs_msg.timestamp); - proto_msg.set_seq(rs_msg.seq); - for (auto iter : rs_msg.packets) - { - void* data_ptr = malloc(iter.packet.size()); - memcpy(data_ptr, iter.packet.data(), iter.packet.size()); - proto_msg.add_data(data_ptr, iter.packet.size()); - free(data_ptr); - } - return proto_msg; -} - -inline ScanMsg toRsMsg(const proto_msg::LidarScan& proto_msg) -{ - ScanMsg rs_msg; - rs_msg.timestamp = proto_msg.timestamp(); - rs_msg.seq = proto_msg.seq(); - for (int i = 0; i < proto_msg.data_size(); i++) - { - std::string data_str = proto_msg.data(i); - PacketMsg tmp_pkt(data_str.size()); - memcpy(tmp_pkt.packet.data(), data_str.data(), data_str.size()); - rs_msg.packets.emplace_back(std::move(tmp_pkt)); - } - return rs_msg; -} - -inline proto_msg::LidarPacket toProtoMsg(const PacketMsg& rs_msg) -{ - proto_msg::LidarPacket proto_msg; - void* data_ptr = malloc(rs_msg.packet.size()); - memcpy(data_ptr, rs_msg.packet.data(), rs_msg.packet.size()); - proto_msg.set_data(data_ptr, rs_msg.packet.size()); - free(data_ptr); - return proto_msg; -} - -inline PacketMsg toRsMsg(const proto_msg::LidarPacket& proto_msg) -{ - std::string data_str = proto_msg.data(); - PacketMsg rs_msg(data_str.size()); - memcpy(rs_msg.packet.data(), data_str.data(), data_str.size()); - return rs_msg; -} - -} // namespace lidar -} // namespace robosense -#endif // PROTO_FOUND diff --git a/src/msg/ros_msg/RsCompressedImage.msg b/src/msg/ros_msg/RsCompressedImage.msg new file mode 100644 index 00000000..a912af68 --- /dev/null +++ b/src/msg/ros_msg/RsCompressedImage.msg @@ -0,0 +1,3 @@ +std_msgs/Header header +uint8 type +uint8[] data diff --git a/src/msg/ros_msg/RslidarPacket.msg b/src/msg/ros_msg/RslidarPacket.msg new file mode 100644 index 00000000..b6a05f5e --- /dev/null +++ b/src/msg/ros_msg/RslidarPacket.msg @@ -0,0 +1,4 @@ +std_msgs/Header header +uint8 is_difop +uint8 is_frame_begin +uint8[] data diff --git a/src/msg/ros_msg/rs_compressed_image.hpp b/src/msg/ros_msg/rs_compressed_image.hpp new file mode 100644 index 00000000..f0a391d3 --- /dev/null +++ b/src/msg/ros_msg/rs_compressed_image.hpp @@ -0,0 +1,237 @@ +// Generated by gencpp from file rscamera_msg/RsCompressedImage.msg +// DO NOT EDIT! + + +#ifndef RSCAMERA_MESSAGE_RSCOMPRESSEDIMAGE_H +#define RSCAMERA_MESSAGE_RSCOMPRESSEDIMAGE_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace rscamera_msg +{ +template +struct RsCompressedImage_ +{ + typedef RsCompressedImage_ Type; + + RsCompressedImage_() + : header() + , type(0) + , data() { + } + RsCompressedImage_(const ContainerAllocator& _alloc) + : header(_alloc) + , type(0) + , data(_alloc) { + (void)_alloc; + } + + + + typedef ::std_msgs::Header_ _header_type; + _header_type header; + + typedef uint8_t _type_type; + _type_type type; + + typedef std::vector::template rebind_alloc> _data_type; + _data_type data; + + + + + + typedef boost::shared_ptr< ::rscamera_msg::RsCompressedImage_ > Ptr; + typedef boost::shared_ptr< ::rscamera_msg::RsCompressedImage_ const> ConstPtr; + +}; // struct RsCompressedImage_ + +typedef ::rscamera_msg::RsCompressedImage_ > RsCompressedImage; + +typedef boost::shared_ptr< ::rscamera_msg::RsCompressedImage > RsCompressedImagePtr; +typedef boost::shared_ptr< ::rscamera_msg::RsCompressedImage const> RsCompressedImageConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::rscamera_msg::RsCompressedImage_ & v) +{ +ros::message_operations::Printer< ::rscamera_msg::RsCompressedImage_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::rscamera_msg::RsCompressedImage_ & lhs, const ::rscamera_msg::RsCompressedImage_ & rhs) +{ + return lhs.header == rhs.header && + lhs.type == rhs.type && + lhs.data == rhs.data; +} + +template +bool operator!=(const ::rscamera_msg::RsCompressedImage_ & lhs, const ::rscamera_msg::RsCompressedImage_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace rscamera_msg + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsMessage< ::rscamera_msg::RsCompressedImage_ > + : TrueType + { }; + +template +struct IsMessage< ::rscamera_msg::RsCompressedImage_ const> + : TrueType + { }; + +template +struct IsFixedSize< ::rscamera_msg::RsCompressedImage_ > + : FalseType + { }; + +template +struct IsFixedSize< ::rscamera_msg::RsCompressedImage_ const> + : FalseType + { }; + +template +struct HasHeader< ::rscamera_msg::RsCompressedImage_ > + : TrueType + { }; + +template +struct HasHeader< ::rscamera_msg::RsCompressedImage_ const> + : TrueType + { }; + + +template +struct MD5Sum< ::rscamera_msg::RsCompressedImage_ > +{ + static const char* value() + { + return "4044c7c7aa754eb9dc4031aaf0ca91a7"; + } + + static const char* value(const ::rscamera_msg::RsCompressedImage_&) { return value(); } + static const uint64_t static_value1 = 0x4044c7c7aa754eb9ULL; + static const uint64_t static_value2 = 0xdc4031aaf0ca91a7ULL; +}; + +template +struct DataType< ::rscamera_msg::RsCompressedImage_ > +{ + static const char* value() + { + return "rscamera_msg/RsCompressedImage"; + } + + static const char* value(const ::rscamera_msg::RsCompressedImage_&) { return value(); } +}; + +template +struct Definition< ::rscamera_msg::RsCompressedImage_ > +{ + static const char* value() + { + return "std_msgs/Header header\n" +"uint8 type\n" +"uint8[] data\n" +"\n" +"================================================================================\n" +"MSG: std_msgs/Header\n" +"# Standard metadata for higher-level stamped data types.\n" +"# This is generally used to communicate timestamped data \n" +"# in a particular coordinate frame.\n" +"# \n" +"# sequence ID: consecutively increasing ID \n" +"uint32 seq\n" +"#Two-integer timestamp that is expressed as:\n" +"# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n" +"# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n" +"# time-handling sugar is provided by the client library\n" +"time stamp\n" +"#Frame this data is associated with\n" +"string frame_id\n" +; + } + + static const char* value(const ::rscamera_msg::RsCompressedImage_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::rscamera_msg::RsCompressedImage_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.header); + stream.next(m.type); + stream.next(m.data); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct RsCompressedImage_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::rscamera_msg::RsCompressedImage_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::rscamera_msg::RsCompressedImage_& v) + { + s << indent << "header: "; + s << std::endl; + Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); + s << indent << "type: "; + Printer::stream(s, indent + " ", v.type); + s << indent << "data[]" << std::endl; + for (size_t i = 0; i < v.data.size(); ++i) + { + s << indent << " data[" << i << "]: "; + Printer::stream(s, indent + " ", v.data[i]); + } + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // RSCAMERA_MESSAGE_RSCOMPRESSEDIMAGE_H diff --git a/src/msg/ros_msg/rslidar_packet.hpp b/src/msg/ros_msg/rslidar_packet.hpp new file mode 100644 index 00000000..4a1a2906 --- /dev/null +++ b/src/msg/ros_msg/rslidar_packet.hpp @@ -0,0 +1,247 @@ +// Generated by gencpp from file rslidar_msg/RslidarPacket.msg +// DO NOT EDIT! + + +#ifndef RSLIDAR_MSG_MESSAGE_RSLIDARPACKET_H +#define RSLIDAR_MSG_MESSAGE_RSLIDARPACKET_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace rslidar_msg +{ +template +struct RslidarPacket_ +{ + typedef RslidarPacket_ Type; + + RslidarPacket_() + : header() + , is_difop(0) + , is_frame_begin(0) + , data() { + } + RslidarPacket_(const ContainerAllocator& _alloc) + : header(_alloc) + , is_difop(0) + , is_frame_begin(0) + , data(_alloc) { + (void)_alloc; + } + + + + typedef ::std_msgs::Header_ _header_type; + _header_type header; + + typedef uint8_t _is_difop_type; + _is_difop_type is_difop; + + typedef uint8_t _is_frame_begin_type; + _is_frame_begin_type is_frame_begin; + + typedef std::vector::other > _data_type; + _data_type data; + + + + + + typedef boost::shared_ptr< ::rslidar_msg::RslidarPacket_ > Ptr; + typedef boost::shared_ptr< ::rslidar_msg::RslidarPacket_ const> ConstPtr; + +}; // struct RslidarPacket_ + +typedef ::rslidar_msg::RslidarPacket_ > RslidarPacket; + +typedef boost::shared_ptr< ::rslidar_msg::RslidarPacket > RslidarPacketPtr; +typedef boost::shared_ptr< ::rslidar_msg::RslidarPacket const> RslidarPacketConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::rslidar_msg::RslidarPacket_ & v) +{ +ros::message_operations::Printer< ::rslidar_msg::RslidarPacket_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::rslidar_msg::RslidarPacket_ & lhs, const ::rslidar_msg::RslidarPacket_ & rhs) +{ + return lhs.header == rhs.header && + lhs.is_difop == rhs.is_difop && + lhs.is_frame_begin == rhs.is_frame_begin && + lhs.data == rhs.data; +} + +template +bool operator!=(const ::rslidar_msg::RslidarPacket_ & lhs, const ::rslidar_msg::RslidarPacket_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace rslidar_msg + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsMessage< ::rslidar_msg::RslidarPacket_ > + : TrueType + { }; + +template +struct IsMessage< ::rslidar_msg::RslidarPacket_ const> + : TrueType + { }; + +template +struct IsFixedSize< ::rslidar_msg::RslidarPacket_ > + : FalseType + { }; + +template +struct IsFixedSize< ::rslidar_msg::RslidarPacket_ const> + : FalseType + { }; + +template +struct HasHeader< ::rslidar_msg::RslidarPacket_ > + : TrueType + { }; + +template +struct HasHeader< ::rslidar_msg::RslidarPacket_ const> + : TrueType + { }; + + +template +struct MD5Sum< ::rslidar_msg::RslidarPacket_ > +{ + static const char* value() + { + return "4b1cc155a9097c0cb935a7abf46d6eef"; + } + + static const char* value(const ::rslidar_msg::RslidarPacket_&) { return value(); } + static const uint64_t static_value1 = 0x4b1cc155a9097c0cULL; + static const uint64_t static_value2 = 0xb935a7abf46d6eefULL; +}; + +template +struct DataType< ::rslidar_msg::RslidarPacket_ > +{ + static const char* value() + { + return "rslidar_msg/RslidarPacket"; + } + + static const char* value(const ::rslidar_msg::RslidarPacket_&) { return value(); } +}; + +template +struct Definition< ::rslidar_msg::RslidarPacket_ > +{ + static const char* value() + { + return "std_msgs/Header header\n" +"uint8 is_difop\n" +"uint8 is_frame_begin\n" +"uint8[] data\n" +"\n" +"================================================================================\n" +"MSG: std_msgs/Header\n" +"# Standard metadata for higher-level stamped data types.\n" +"# This is generally used to communicate timestamped data \n" +"# in a particular coordinate frame.\n" +"# \n" +"# sequence ID: consecutively increasing ID \n" +"uint32 seq\n" +"#Two-integer timestamp that is expressed as:\n" +"# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n" +"# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n" +"# time-handling sugar is provided by the client library\n" +"time stamp\n" +"#Frame this data is associated with\n" +"string frame_id\n" +; + } + + static const char* value(const ::rslidar_msg::RslidarPacket_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::rslidar_msg::RslidarPacket_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.header); + stream.next(m.is_difop); + stream.next(m.is_frame_begin); + stream.next(m.data); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct RslidarPacket_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::rslidar_msg::RslidarPacket_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::rslidar_msg::RslidarPacket_& v) + { + s << indent << "header: "; + s << std::endl; + Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); + s << indent << "is_difop: "; + Printer::stream(s, indent + " ", v.is_difop); + s << indent << "is_frame_begin: "; + Printer::stream(s, indent + " ", v.is_frame_begin); + s << indent << "data[]" << std::endl; + for (size_t i = 0; i < v.data.size(); ++i) + { + s << indent << " data[" << i << "]: "; + Printer::stream(s, indent + " ", v.data[i]); + } + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // RSLIDAR_MSG_MESSAGE_RSLIDARPACKET_H diff --git a/src/msg/ros_msg/lidar_packet_ros.h b/src/msg/ros_msg/rslidar_packet_legacy.hpp similarity index 100% rename from src/msg/ros_msg/lidar_packet_ros.h rename to src/msg/ros_msg/rslidar_packet_legacy.hpp diff --git a/src/msg/ros_msg/lidar_scan_ros.h b/src/msg/ros_msg/rslidar_scan_legacy.hpp similarity index 99% rename from src/msg/ros_msg/lidar_scan_ros.h rename to src/msg/ros_msg/rslidar_scan_legacy.hpp index 371197d8..b9837e11 100644 --- a/src/msg/ros_msg/lidar_scan_ros.h +++ b/src/msg/ros_msg/rslidar_scan_legacy.hpp @@ -13,7 +13,7 @@ #include #include -#include "msg/ros_msg/lidar_packet_ros.h" +#include "msg/ros_msg/rslidar_packet_legacy.hpp" namespace rslidar_msgs { diff --git a/src/msg/ros_msg_translator.h b/src/msg/ros_msg_translator.h deleted file mode 100644 index 5d02cde0..00000000 --- a/src/msg/ros_msg_translator.h +++ /dev/null @@ -1,224 +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 -#ifdef ROS_FOUND -#include -#include -#include -#include -#include -#include -#include -#include "msg/rs_msg/lidar_point_cloud_msg.h" -#include "msg/ros_msg/lidar_packet_ros.h" -#include "msg/ros_msg/lidar_scan_ros.h" -#include "rs_driver/msg/packet_msg.h" -#include "rs_driver/msg/scan_msg.h" -#include "rs_driver/driver/driver_param.h" - -namespace robosense -{ -namespace lidar -{ -/************************************************************************/ -/**Translation functions between RoboSense message and ROS message**/ -/************************************************************************/ -inline sensor_msgs::PointCloud2 toRosMsg(const LidarPointCloudMsg& rs_msg) -{ - sensor_msgs::PointCloud2 ros_msg; - pcl::toROSMsg(*rs_msg.point_cloud_ptr, ros_msg); - ros_msg.header.stamp = ros_msg.header.stamp.fromSec(rs_msg.timestamp); - ros_msg.header.frame_id = rs_msg.frame_id; - ros_msg.header.seq = rs_msg.seq; - return std::move(ros_msg); -} -inline PacketMsg toRsMsg(const LidarType& lidar_type, const PktType& pkt_type, - const rslidar_msgs::rslidarPacket& ros_msg) -{ - size_t pkt_length; - switch (lidar_type) - { - case LidarType::RSM1: - if (pkt_type == PktType::MSOP) - { - pkt_length = MEMS_MSOP_LEN; - } - else - { - pkt_length = MEMS_DIFOP_LEN; - } - break; - default: - pkt_length = MECH_PKT_LEN; - break; - } - PacketMsg rs_msg(pkt_length); - for (size_t i = 0; i < pkt_length; i++) - { - rs_msg.packet[i] = ros_msg.data[i]; - } - return std::move(rs_msg); -} -inline rslidar_msgs::rslidarPacket toRosMsg(const PacketMsg& rs_msg) -{ - rslidar_msgs::rslidarPacket ros_msg; - for (size_t i = 0; i < rs_msg.packet.size(); i++) - { - ros_msg.data[i] = rs_msg.packet[i]; - } - return std::move(ros_msg); -} -inline ScanMsg toRsMsg(const LidarType& lidar_type, const PktType& pkt_type, const rslidar_msgs::rslidarScan& ros_msg) -{ - ScanMsg rs_msg; - rs_msg.seq = ros_msg.header.seq; - rs_msg.timestamp = ros_msg.header.stamp.toSec(); - rs_msg.frame_id = ros_msg.header.frame_id; - - for (uint32_t i = 0; i < ros_msg.packets.size(); i++) - { - PacketMsg tmp = toRsMsg(lidar_type, pkt_type, ros_msg.packets[i]); - rs_msg.packets.emplace_back(std::move(tmp)); - } - return std::move(rs_msg); -} -inline rslidar_msgs::rslidarScan toRosMsg(const ScanMsg& rs_msg) -{ - rslidar_msgs::rslidarScan ros_msg; - ros_msg.header.stamp = ros_msg.header.stamp.fromSec(rs_msg.timestamp); - ros_msg.header.frame_id = rs_msg.frame_id; - ros_msg.header.seq = rs_msg.seq; - for (uint32_t i = 0; i < rs_msg.packets.size(); i++) - { - rslidar_msgs::rslidarPacket tmp = toRosMsg(rs_msg.packets[i]); - ros_msg.packets.emplace_back(std::move(tmp)); - } - return std::move(ros_msg); -} -inline std_msgs::Time toRosMsg(const CameraTrigger& rs_msg) -{ - std_msgs::Time ros_msg; - ros_msg.data = ros_msg.data.fromSec(rs_msg.second); - return ros_msg; -} - -} // namespace lidar -} // namespace robosense -#endif // ROS_FOUND - -#ifdef ROS2_FOUND -#include -#include -#include "rslidar_msg/msg/rslidar_packet.hpp" -#include "rslidar_msg/msg/rslidar_scan.hpp" -namespace robosense -{ -namespace lidar -{ -/************************************************************************/ -/**Translation functions between RoboSense message and ROS2 message**/ -/************************************************************************/ -inline sensor_msgs::msg::PointCloud2 toRosMsg(const LidarPointCloudMsg& rs_msg) -{ - sensor_msgs::msg::PointCloud2 ros_msg; - pcl::toROSMsg(*rs_msg.point_cloud_ptr, ros_msg); - ros_msg.header.stamp.sec = (uint32_t)floor(rs_msg.timestamp); - ros_msg.header.stamp.nanosec = (uint32_t)round((rs_msg.timestamp - ros_msg.header.stamp.sec) * 1e9); - ros_msg.header.frame_id = rs_msg.frame_id; - return std::move(ros_msg); -} -inline PacketMsg toRsMsg(const LidarType& lidar_type, const PktType& pkt_type, - const rslidar_msg::msg::RslidarPacket& ros_msg) -{ - size_t pkt_length; - switch (lidar_type) - { - case LidarType::RSM1: - if (pkt_type == PktType::MSOP) - { - pkt_length = MEMS_MSOP_LEN; - } - else - { - pkt_length = MEMS_DIFOP_LEN; - } - break; - default: - pkt_length = MECH_PKT_LEN; - break; - } - PacketMsg rs_msg(pkt_length); - for (size_t i = 0; i < pkt_length; i++) - { - rs_msg.packet[i] = ros_msg.data[i]; - } - return std::move(rs_msg); -} -inline rslidar_msg::msg::RslidarPacket toRosMsg(const PacketMsg& rs_msg) -{ - rslidar_msg::msg::RslidarPacket ros_msg; - for (size_t i = 0; i < rs_msg.packet.size(); i++) - { - ros_msg.data[i] = rs_msg.packet[i]; - } - return std::move(ros_msg); -} -inline ScanMsg toRsMsg(const LidarType& lidar_type, const PktType& pkt_type, - const rslidar_msg::msg::RslidarScan& ros_msg) -{ - ScanMsg rs_msg; - rs_msg.timestamp = ros_msg.header.stamp.sec + double(ros_msg.header.stamp.nanosec) / 1e9; - rs_msg.frame_id = ros_msg.header.frame_id; - for (uint32_t i = 0; i < ros_msg.packets.size(); i++) - { - PacketMsg tmp = toRsMsg(lidar_type, pkt_type, ros_msg.packets[i]); - rs_msg.packets.emplace_back(std::move(tmp)); - } - return rs_msg; -} -inline rslidar_msg::msg::RslidarScan toRosMsg(const ScanMsg& rs_msg) -{ - rslidar_msg::msg::RslidarScan ros_msg; - ros_msg.header.stamp.sec = (uint32_t)floor(rs_msg.timestamp); - ros_msg.header.stamp.nanosec = (uint32_t)round((rs_msg.timestamp - ros_msg.header.stamp.sec) * 1e9); - ros_msg.header.frame_id = rs_msg.frame_id; - for (uint32_t i = 0; i < rs_msg.packets.size(); i++) - { - rslidar_msg::msg::RslidarPacket tmp = toRosMsg(rs_msg.packets[i]); - ros_msg.packets.emplace_back(std::move(tmp)); - } - return ros_msg; -} -} // namespace lidar -} // namespace robosense -#endif // ROS2_FOUND \ No newline at end of file diff --git a/src/msg/rs_msg/lidar_point_cloud_msg.h b/src/msg/rs_msg/lidar_point_cloud_msg.hpp similarity index 55% rename from src/msg/rs_msg/lidar_point_cloud_msg.h rename to src/msg/rs_msg/lidar_point_cloud_msg.hpp index 412e2c89..476ad4cb 100644 --- a/src/msg/rs_msg/lidar_point_cloud_msg.h +++ b/src/msg/rs_msg/lidar_point_cloud_msg.hpp @@ -31,56 +31,12 @@ THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *********************************************************************************************************************/ #pragma once -#include -#include -#include -#include -struct RsPointXYZIRT -{ - 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(RsPointXYZIRT, (float, x, x)(float, y, y)(float, z, z)(uint8_t, intensity, intensity)( - uint16_t, ring, ring)(double, timestamp, timestamp)) -#ifdef POINT_TYPE_XYZI -typedef pcl::PointXYZI PointT; -#elif POINT_TYPE_XYZIRT -typedef RsPointXYZIRT PointT; -#endif - -namespace robosense -{ -namespace lidar -{ -/** - * @brief Point cloud message for RoboSense SDK. - * @detail RoboSense LidarPointCloudMsg is defined for passing lidar point cloud accross different modules - * If ROS is turned on , we provide translation functions between ROS message and RoboSense message - * If Proto is turned on , we provide translation functions between Protobuf message and RoboSense message - */ - -struct alignas(16) LidarPointCloudMsg -{ - typedef pcl::PointCloud PointCloud; - typedef pcl::PointCloud::Ptr PointCloudPtr; - typedef pcl::PointCloud::ConstPtr PointCloudConstPtr; - double timestamp = 0.0; - uint32_t seq = 0; - std::string frame_id = ""; +#include "rs_driver/msg/point_cloud_msg.hpp" - PointCloudConstPtr point_cloud_ptr; ///< the point cloud pointer - - LidarPointCloudMsg() = default; - LidarPointCloudMsg(const PointCloudPtr& ptr) : point_cloud_ptr(ptr) - { - } - typedef std::shared_ptr Ptr; - typedef std::shared_ptr ConstPtr; -}; +#ifdef POINT_TYPE_XYZIRT +typedef PointCloudT LidarPointCloudMsg; +#else +typedef PointCloudT LidarPointCloudMsg; +#endif -} // namespace lidar -} // namespace robosense diff --git a/src/rs_driver b/src/rs_driver index 651fdd8c..ba07ba49 160000 --- a/src/rs_driver +++ b/src/rs_driver @@ -1 +1 @@ -Subproject commit 651fdd8ce94acde7cad97068b81a60f6bac895af +Subproject commit ba07ba49565c8f1df745c8effaf64f39ac83a0ef diff --git a/src/adapter/adapter_base.hpp b/src/source/source.hpp similarity index 51% rename from src/adapter/adapter_base.hpp rename to src/source/source.hpp index 76462c2c..ddb27aad 100644 --- a/src/adapter/adapter_base.hpp +++ b/src/source/source.hpp @@ -31,118 +31,105 @@ THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *********************************************************************************************************************/ #pragma once -#include "utility/common.h" + +#include "msg/rs_msg/lidar_point_cloud_msg.hpp" + #include "utility/yaml_reader.hpp" -#include "rs_driver/msg/packet_msg.h" -#include "rs_driver/msg/scan_msg.h" -#include "msg/rs_msg/lidar_point_cloud_msg.h" + +#include namespace robosense { namespace lidar { -enum MsgSource -{ - MSG_FROM_LIDAR = 1, - MSG_FROM_ROS_PACKET = 2, - MSG_FROM_PCAP = 3, - MSG_FROM_PROTO_PACKET = 4, - MSG_FROM_PROTO_POINTCLOUD = 5 -}; -enum class AdapterType -{ - DriverAdapter, - PointCloudRosAdapter, - PointCloudProtoAdapter, - PacketRosAdapter, - PacketProtoAdapter, - CameraTriggerRosAdapter -}; -class AdapterBase + +class DestinationPointCloud { public: - typedef std::shared_ptr Ptr; - AdapterBase() = default; - virtual ~AdapterBase(); - virtual void init(const YAML::Node& config) = 0; - virtual void start(); - virtual void stop(); - virtual void sendScan(const ScanMsg& msg); - virtual void sendPacket(const PacketMsg& msg); - virtual void sendPointCloud(const LidarPointCloudMsg& msg); - virtual void sendCameraTrigger(const CameraTrigger& msg); - virtual void regRecvCallback(const std::function& callback); - virtual void regRecvCallback(const std::function& callback); - virtual void regRecvCallback(const std::function& callback); - virtual void regRecvCallback(const std::function& callback); - virtual void decodeScan(const ScanMsg& msg); - virtual void decodePacket(const PacketMsg& msg); + typedef std::shared_ptr Ptr; + + virtual void init(const YAML::Node& config){} + virtual void start() {} + virtual void stop() {} + virtual void sendPointCloud(const LidarPointCloudMsg& msg) = 0; + virtual ~DestinationPointCloud() = default; }; -inline AdapterBase::~AdapterBase() +class DestinationPacket { - stop(); -} +public: + typedef std::shared_ptr Ptr; -inline void AdapterBase::start() -{ - return; -} + virtual void init(const YAML::Node& config){} + virtual void start() {} + virtual void stop() {} + virtual void sendPacket(const Packet& msg) = 0; + virtual ~DestinationPacket() = default; +}; -inline void AdapterBase::stop() +enum SourceType { - return; -} + MSG_FROM_LIDAR = 1, + MSG_FROM_ROS_PACKET = 2, + MSG_FROM_PCAP = 3, + MSG_FROM_PROTO_PACKET = 4, + MSG_FROM_PROTO_POINTCLOUD = 5 +}; -inline void AdapterBase::sendScan(const ScanMsg& msg) +class Source { - return; -} +public: + typedef std::shared_ptr Ptr; -inline void AdapterBase::sendPacket(const PacketMsg& msg) -{ - return; -} + virtual void init(const YAML::Node& config) {} + virtual void start() {} + virtual void stop() {} + virtual void regPointCloudCallback(DestinationPointCloud::Ptr dst); + virtual void regPacketCallback(DestinationPacket::Ptr dst); + virtual ~Source() = default; + Source(SourceType src_type); -inline void AdapterBase::sendPointCloud(const LidarPointCloudMsg& msg) -{ - return; -} +protected: -inline void AdapterBase::sendCameraTrigger(const CameraTrigger& msg) -{ - return; -} + void sendPacket(const Packet& msg); + void sendPointCloud(std::shared_ptr msg); -inline void AdapterBase::regRecvCallback(const std::function& callback) -{ - return; -} + SourceType src_type_; + std::vector pc_cb_vec_; + std::vector pkt_cb_vec_; +}; -inline void AdapterBase::regRecvCallback(const std::function& callback) +inline Source::Source(SourceType src_type) + : src_type_(src_type) { - return; } -inline void AdapterBase::regRecvCallback(const std::function& callback) +inline void Source::regPacketCallback(DestinationPacket::Ptr dst) { - return; + pkt_cb_vec_.emplace_back(dst); } -inline void AdapterBase::regRecvCallback(const std::function& callback) +inline void Source::regPointCloudCallback(DestinationPointCloud::Ptr dst) { - return; + pc_cb_vec_.emplace_back(dst); } -inline void AdapterBase::decodeScan(const ScanMsg& msg) +inline void Source::sendPacket(const Packet& msg) { - return; + for (auto iter : pkt_cb_vec_) + { + iter->sendPacket(msg); + } } -inline void AdapterBase::decodePacket(const PacketMsg& msg) +inline void Source::sendPointCloud(std::shared_ptr msg) { - return; + for (auto iter : pc_cb_vec_) + { + iter->sendPointCloud(*msg); + } } + } // namespace lidar } // namespace robosense diff --git a/src/source/source_driver.hpp b/src/source/source_driver.hpp new file mode 100644 index 00000000..f41c09aa --- /dev/null +++ b/src/source/source_driver.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 "source/source.hpp" + +#include +#include + +namespace robosense +{ +namespace lidar +{ + +class SourceDriver : public Source +{ +public: + + virtual void init(const YAML::Node& config); + virtual void start(); + virtual void stop(); + virtual void regPacketCallback(DestinationPacket::Ptr dst); + virtual ~SourceDriver(); + + SourceDriver(SourceType src_type); + +protected: + + std::shared_ptr getPointCloud(void); + void putPointCloud(std::shared_ptr msg); + void putPacket(const Packet& msg); + void putException(const lidar::Error& msg); + void processPointCloud(); + + std::shared_ptr> driver_ptr_; + SyncQueue> free_point_cloud_queue_; + SyncQueue> point_cloud_queue_; + std::thread point_cloud_process_thread_; + bool to_exit_process_; +}; + +SourceDriver::SourceDriver(SourceType src_type) + : Source(src_type), to_exit_process_(false) +{ +} + +inline void SourceDriver::init(const YAML::Node& config) +{ + YAML::Node driver_config = yamlSubNodeAbort(config, "driver"); + lidar::RSDriverParam driver_param; + + // input related + yamlRead(driver_config, "msop_port", driver_param.input_param.msop_port, 6699); + yamlRead(driver_config, "difop_port", driver_param.input_param.difop_port, 7788); + yamlRead(driver_config, "host_address", driver_param.input_param.host_address, "0.0.0.0"); + yamlRead(driver_config, "group_address", driver_param.input_param.group_address, "0.0.0.0"); + yamlRead(driver_config, "use_vlan", driver_param.input_param.use_vlan, false); + yamlRead(driver_config, "pcap_path", driver_param.input_param.pcap_path, ""); + yamlRead(driver_config, "pcap_rate", driver_param.input_param.pcap_rate, 1); + yamlRead(driver_config, "pcap_repeat", driver_param.input_param.pcap_repeat, true); + yamlRead(driver_config, "user_layer_bytes", driver_param.input_param.user_layer_bytes, 0); + yamlRead(driver_config, "tail_layer_bytes", driver_param.input_param.tail_layer_bytes, 0); + yamlRead(driver_config, "socket_recv_buf", driver_param.input_param.socket_recv_buf, 106496); + // decoder related + std::string lidar_type; + yamlReadAbort(driver_config, "lidar_type", lidar_type); + driver_param.lidar_type = strToLidarType(lidar_type); + + // decoder + yamlRead(driver_config, "wait_for_difop", driver_param.decoder_param.wait_for_difop, true); + yamlRead(driver_config, "use_lidar_clock", driver_param.decoder_param.use_lidar_clock, false); + yamlRead(driver_config, "min_distance", driver_param.decoder_param.min_distance, 0.2); + yamlRead(driver_config, "max_distance", driver_param.decoder_param.max_distance, 200); + yamlRead(driver_config, "start_angle", driver_param.decoder_param.start_angle, 0); + yamlRead(driver_config, "end_angle", driver_param.decoder_param.end_angle, 360); + yamlRead(driver_config, "dense_points", driver_param.decoder_param.dense_points, false); + yamlRead(driver_config, "ts_first_point", driver_param.decoder_param.ts_first_point, false); + + // mechanical decoder + yamlRead(driver_config, "config_from_file", driver_param.decoder_param.config_from_file, false); + yamlRead(driver_config, "angle_path", driver_param.decoder_param.angle_path, ""); + + uint16_t split_frame_mode; + yamlRead(driver_config, "split_frame_mode", split_frame_mode, 1); + driver_param.decoder_param.split_frame_mode = SplitFrameMode(split_frame_mode); + + yamlRead(driver_config, "split_angle", driver_param.decoder_param.split_angle, 0); + yamlRead(driver_config, "num_blks_split", driver_param.decoder_param.num_blks_split, 0); + + // transform + yamlRead(driver_config, "x", driver_param.decoder_param.transform_param.x, 0); + yamlRead(driver_config, "y", driver_param.decoder_param.transform_param.y, 0); + yamlRead(driver_config, "z", driver_param.decoder_param.transform_param.z, 0); + yamlRead(driver_config, "roll", driver_param.decoder_param.transform_param.roll, 0); + yamlRead(driver_config, "pitch", driver_param.decoder_param.transform_param.pitch, 0); + yamlRead(driver_config, "yaw", driver_param.decoder_param.transform_param.yaw, 0); + + switch (src_type_) + { + case SourceType::MSG_FROM_LIDAR: + driver_param.input_type = InputType::ONLINE_LIDAR; + break; + case SourceType::MSG_FROM_PCAP: + driver_param.input_type = InputType::PCAP_FILE; + break; + default: + driver_param.input_type = InputType::RAW_PACKET; + break; + } + + driver_param.print(); + + driver_ptr_.reset(new lidar::LidarDriver()); + driver_ptr_->regPointCloudCallback(std::bind(&SourceDriver::getPointCloud, this), + std::bind(&SourceDriver::putPointCloud, this, std::placeholders::_1)); + driver_ptr_->regExceptionCallback( + std::bind(&SourceDriver::putException, this, std::placeholders::_1)); + point_cloud_process_thread_ = std::thread(std::bind(&SourceDriver::processPointCloud, this)); + + if (!driver_ptr_->init(driver_param)) + { + RS_ERROR << "Driver Initialize Error...." << RS_REND; + exit(-1); + } +} + +inline void SourceDriver::start() +{ + driver_ptr_->start(); +} + +inline SourceDriver::~SourceDriver() +{ + stop(); +} + +inline void SourceDriver::stop() +{ + driver_ptr_->stop(); + + to_exit_process_ = true; + point_cloud_process_thread_.join(); +} + +inline std::shared_ptr SourceDriver::getPointCloud(void) +{ + std::shared_ptr point_cloud = free_point_cloud_queue_.pop(); + if (point_cloud.get() != NULL) + { + return point_cloud; + } + + return std::make_shared(); +} + +inline void SourceDriver::regPacketCallback(DestinationPacket::Ptr dst) +{ + Source::regPacketCallback(dst); + + if (pkt_cb_vec_.size() == 1) + { + driver_ptr_->regPacketCallback( + std::bind(&SourceDriver::putPacket, this, std::placeholders::_1)); + } +} + +inline void SourceDriver::putPacket(const Packet& msg) +{ + sendPacket(msg); +} + +void SourceDriver::putPointCloud(std::shared_ptr msg) +{ + point_cloud_queue_.push(msg); +} + +void SourceDriver::processPointCloud() +{ + while (!to_exit_process_) + { + std::shared_ptr msg = point_cloud_queue_.popWait(1000); + if (msg.get() == NULL) + { + continue; + } + sendPointCloud(msg); + + free_point_cloud_queue_.push(msg); + } +} + +inline void SourceDriver::putException(const lidar::Error& msg) +{ + switch (msg.error_code_type) + { + case lidar::ErrCodeType::INFO_CODE: + RS_INFO << msg.toString() << RS_REND; + break; + case lidar::ErrCodeType::WARNING_CODE: + RS_WARNING << msg.toString() << RS_REND; + break; + case lidar::ErrCodeType::ERROR_CODE: + RS_ERROR << msg.toString() << RS_REND; + break; + } +} + +} // namespace lidar +} // namespace robosense diff --git a/src/source/source_packet_ros.hpp b/src/source/source_packet_ros.hpp new file mode 100644 index 00000000..00ab6d98 --- /dev/null +++ b/src/source/source_packet_ros.hpp @@ -0,0 +1,346 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the +following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following +disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following +disclaimer in the documentation and/or other materials provided with the distribution. + +3. Neither the names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, +INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*********************************************************************************************************************/ + +#pragma once + +#include "source/source_driver.hpp" + +#ifdef ROS_FOUND + +#ifdef ENABLE_SOURCE_PACKET_LEGACY +#include "msg/ros_msg/rslidar_packet_legacy.hpp" +#include "msg/ros_msg/rslidar_scan_legacy.hpp" +#endif + +#include "msg/ros_msg/rslidar_packet.hpp" +#include + +namespace robosense +{ +namespace lidar +{ + +inline Packet toRsMsg(const rslidar_msg::RslidarPacket& ros_msg) +{ + Packet rs_msg; + rs_msg.timestamp = ros_msg.header.stamp.toSec(); + rs_msg.seq = ros_msg.header.seq; + rs_msg.is_difop = ros_msg.is_difop; + rs_msg.is_frame_begin = ros_msg.is_frame_begin; + + for (size_t i = 0; i < ros_msg.data.size(); i++) + { + rs_msg.buf_.emplace_back(ros_msg.data[i]); + } + + return rs_msg; +} + +class SourcePacketRos : public SourceDriver +{ +public: + + virtual void init(const YAML::Node& config); + + SourcePacketRos(); + +private: + +#ifdef ENABLE_SOURCE_PACKET_LEGACY + void putPacketLegacy(const rslidar_msgs::rslidarScan& msg); + void putPacketDifopLegacy(const rslidar_msgs::rslidarPacket& msg); + ros::Subscriber pkt_sub_legacy_; + ros::Subscriber pkt_sub_difop_legacy_; + LidarType lidar_type_; +#endif + + void putPacket(const rslidar_msg::RslidarPacket& msg); + ros::Subscriber pkt_sub_; + + std::unique_ptr nh_; +}; + +SourcePacketRos::SourcePacketRos() + : SourceDriver(SourceType::MSG_FROM_ROS_PACKET) +{ +} + +void SourcePacketRos::init(const YAML::Node& config) +{ + SourceDriver::init(config); + + nh_ = std::unique_ptr(new ros::NodeHandle()); + +#ifdef ENABLE_SOURCE_PACKET_LEGACY + std::string lidar_type; + yamlReadAbort(config["driver"], "lidar_type", lidar_type); + lidar_type_ = strToLidarType(lidar_type); + + std::string ros_recv_topic_legacy; + yamlRead(config["ros"], "ros_recv_packet_legacy_topic", + ros_recv_topic_legacy, "rslidar_packets"); + + pkt_sub_legacy_ = nh_->subscribe(ros_recv_topic_legacy, 1, &SourcePacketRos::putPacketLegacy, this); + pkt_sub_difop_legacy_ = nh_->subscribe(ros_recv_topic_legacy + "_difop", 1, &SourcePacketRos::putPacketDifopLegacy, this); +#endif + + std::string ros_recv_topic; + yamlRead(config["ros"], "ros_recv_packet_topic", + ros_recv_topic, "rslidar_packets"); + + pkt_sub_ = nh_->subscribe(ros_recv_topic, 100, &SourcePacketRos::putPacket, this); + +} + +#ifdef ENABLE_SOURCE_PACKET_LEGACY +inline Packet toRsMsg(const rslidar_msgs::rslidarPacket& ros_msg, LidarType lidar_type, bool isDifop) +{ + size_t pkt_len = 1248; + if (lidar_type == LidarType::RSM1) + { + pkt_len = isDifop ? 256 : 1210; + } + + Packet rs_msg; + for (size_t i = 0; i < pkt_len; i++) + { + rs_msg.buf_.emplace_back(ros_msg.data[i]); + } + + return rs_msg; +} + +void SourcePacketRos::putPacketLegacy(const rslidar_msgs::rslidarScan& msg) +{ + for (uint32_t i = 0; i < msg.packets.size(); i++) + { + const rslidar_msgs::rslidarPacket& packet = msg.packets[i]; + driver_ptr_->decodePacket(toRsMsg(packet, lidar_type_, false)); + } +} + +void SourcePacketRos::putPacketDifopLegacy(const rslidar_msgs::rslidarPacket& msg) +{ + driver_ptr_->decodePacket(toRsMsg(msg, lidar_type_, true)); +} +#endif + +void SourcePacketRos::putPacket(const rslidar_msg::RslidarPacket& msg) +{ + driver_ptr_->decodePacket(toRsMsg(msg)); +} + +inline rslidar_msg::RslidarPacket toRosMsg(const Packet& rs_msg, const std::string& frame_id) +{ + rslidar_msg::RslidarPacket ros_msg; + ros_msg.header.stamp = ros_msg.header.stamp.fromSec(rs_msg.timestamp); + ros_msg.header.seq = rs_msg.seq; + ros_msg.header.frame_id = frame_id; + ros_msg.is_difop = rs_msg.is_difop; + ros_msg.is_frame_begin = rs_msg.is_frame_begin; + + for (size_t i = 0; i < rs_msg.buf_.size(); i++) + { + ros_msg.data.emplace_back(rs_msg.buf_[i]); + } + + return ros_msg; +} + +class DestinationPacketRos : public DestinationPacket +{ +public: + + virtual void init(const YAML::Node& config); + virtual void sendPacket(const Packet& msg); + virtual ~DestinationPacketRos() = default; + +private: + + std::unique_ptr nh_; + ros::Publisher pkt_pub_; + std::string frame_id_; +}; + +inline void DestinationPacketRos::init(const YAML::Node& config) +{ + yamlRead(config["ros"], + "ros_frame_id", frame_id_, "rslidar"); + + std::string ros_send_topic; + yamlRead(config["ros"], "ros_send_packet_topic", + ros_send_topic, "rslidar_packets"); + + nh_ = std::unique_ptr(new ros::NodeHandle()); + pkt_pub_ = nh_->advertise(ros_send_topic, 100); +} + +inline void DestinationPacketRos::sendPacket(const Packet& msg) +{ + pkt_pub_.publish(toRosMsg(msg, frame_id_)); +} + +} // namespace lidar +} // namespace robosense + +#endif // ROS_FOUND + +#ifdef ROS2_FOUND +#include "rslidar_msg/msg/rslidar_packet.hpp" +#include +#include + +namespace robosense +{ +namespace lidar +{ + +inline Packet toRsMsg(const rslidar_msg::msg::RslidarPacket& ros_msg) +{ + Packet rs_msg; + rs_msg.timestamp = ros_msg.header.stamp.sec + double(ros_msg.header.stamp.nanosec) / 1e9; + //rs_msg.seq = ros_msg.header.seq; + rs_msg.is_difop = ros_msg.is_difop; + rs_msg.is_frame_begin = ros_msg.is_frame_begin; + + for (size_t i = 0; i < ros_msg.data.size(); i++) + { + rs_msg.buf_.emplace_back(ros_msg.data[i]); + } + + return rs_msg; +} + +class SourcePacketRos : public SourceDriver +{ +public: + + virtual void init(const YAML::Node& config); + + SourcePacketRos(); + +private: + + void putPacket(const rslidar_msg::msg::RslidarPacket::SharedPtr msg) const; + + std::shared_ptr node_ptr_; + rclcpp::Subscription::SharedPtr pkt_sub_; +}; + +SourcePacketRos::SourcePacketRos() + : SourceDriver(SourceType::MSG_FROM_ROS_PACKET) +{ +} + +void SourcePacketRos::init(const YAML::Node& config) +{ + SourceDriver::init(config); + + std::string ros_recv_topic; + yamlRead(config["ros"], "ros_recv_packet_topic", + ros_recv_topic, "rslidar_packets"); + + static int node_index = 0; + std::stringstream node_name; + node_name << "rslidar_packets_source_" << node_index++; + + node_ptr_.reset(new rclcpp::Node(node_name.str())); + pkt_sub_ = node_ptr_->create_subscription(ros_recv_topic, 100, + std::bind(&SourcePacketRos::putPacket, this, std::placeholders::_1)); +} + +void SourcePacketRos::putPacket(const rslidar_msg::msg::RslidarPacket::SharedPtr msg) const +{ + driver_ptr_->decodePacket(toRsMsg(*msg)); +} + +inline rslidar_msg::msg::RslidarPacket toRosMsg(const Packet& rs_msg, const std::string& frame_id) +{ + rslidar_msg::msg::RslidarPacket ros_msg; + ros_msg.header.stamp.sec = (uint32_t)floor(rs_msg.timestamp); + ros_msg.header.stamp.nanosec = (uint32_t)round((rs_msg.timestamp - ros_msg.header.stamp.sec) * 1e9); + //ros_msg.header.seq = rs_msg.seq; + ros_msg.header.frame_id = frame_id; + ros_msg.is_difop = rs_msg.is_difop; + ros_msg.is_frame_begin = rs_msg.is_frame_begin; + + for (size_t i = 0; i < rs_msg.buf_.size(); i++) + { + ros_msg.data.emplace_back(rs_msg.buf_[i]); + } + + return ros_msg; +} + +class DestinationPacketRos : public DestinationPacket +{ +public: + + virtual void init(const YAML::Node& config); + virtual void sendPacket(const Packet& msg); + virtual ~DestinationPacketRos() = default; + +private: + + std::shared_ptr node_ptr_; + rclcpp::Publisher::SharedPtr pkt_pub_; + std::string frame_id_; +}; + +inline void DestinationPacketRos::init(const YAML::Node& config) +{ + yamlRead(config["ros"], + "ros_frame_id", frame_id_, "rslidar"); + + std::string ros_send_topic; + yamlRead(config["ros"], "ros_send_packet_topic", + ros_send_topic, "rslidar_packets"); + + static int node_index = 0; + std::stringstream node_name; + node_name << "rslidar_packets_destination_" << node_index++; + + node_ptr_.reset(new rclcpp::Node(node_name.str())); + pkt_pub_ = node_ptr_->create_publisher(ros_send_topic, 100); +} + +inline void DestinationPacketRos::sendPacket(const Packet& msg) +{ + pkt_pub_->publish(toRosMsg(msg, frame_id_)); +} + +} // namespace lidar +} // namespace robosense + +#endif // ROS2_FOUND + + diff --git a/src/source/source_pointcloud_ros.hpp b/src/source/source_pointcloud_ros.hpp new file mode 100644 index 00000000..a756d289 --- /dev/null +++ b/src/source/source_pointcloud_ros.hpp @@ -0,0 +1,373 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the +following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following +disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following +disclaimer in the documentation and/or other materials provided with the distribution. + +3. Neither the names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, +INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*********************************************************************************************************************/ + +#pragma once + +#include "source/source.hpp" + +#ifdef ROS_FOUND +#include +#include + +namespace robosense +{ +namespace lidar +{ + +inline sensor_msgs::PointCloud2 toRosMsg(const LidarPointCloudMsg& rs_msg, const std::string& frame_id, bool send_by_rows) +{ + sensor_msgs::PointCloud2 ros_msg; + + int fields = 4; +#ifdef POINT_TYPE_XYZIRT + fields = 6; +#endif + ros_msg.fields.clear(); + ros_msg.fields.reserve(fields); + + if (send_by_rows) + { + ros_msg.width = rs_msg.width; + ros_msg.height = rs_msg.height; + } + else + { + ros_msg.width = rs_msg.height; // exchange width and height to be compatible with pcl::PointCloud<> + ros_msg.height = rs_msg.width; + } + + int offset = 0; + offset = addPointField(ros_msg, "x", 1, sensor_msgs::PointField::FLOAT32, offset); + offset = addPointField(ros_msg, "y", 1, sensor_msgs::PointField::FLOAT32, offset); + offset = addPointField(ros_msg, "z", 1, sensor_msgs::PointField::FLOAT32, offset); + offset = addPointField(ros_msg, "intensity", 1, sensor_msgs::PointField::FLOAT32, offset); +#ifdef POINT_TYPE_XYZIRT + offset = addPointField(ros_msg, "ring", 1, sensor_msgs::PointField::UINT16, offset); + offset = addPointField(ros_msg, "timestamp", 1, sensor_msgs::PointField::FLOAT64, offset); +#endif + +#if 0 + std::cout << "off:" << offset << std::endl; +#endif + + ros_msg.point_step = offset; + ros_msg.row_step = ros_msg.width * ros_msg.point_step; + ros_msg.is_dense = rs_msg.is_dense; + ros_msg.data.resize(ros_msg.point_step * ros_msg.width * ros_msg.height); + + sensor_msgs::PointCloud2Iterator iter_x_(ros_msg, "x"); + sensor_msgs::PointCloud2Iterator iter_y_(ros_msg, "y"); + sensor_msgs::PointCloud2Iterator iter_z_(ros_msg, "z"); + sensor_msgs::PointCloud2Iterator iter_intensity_(ros_msg, "intensity"); +#ifdef POINT_TYPE_XYZIRT + sensor_msgs::PointCloud2Iterator iter_ring_(ros_msg, "ring"); + sensor_msgs::PointCloud2Iterator iter_timestamp_(ros_msg, "timestamp"); +#endif + + if (send_by_rows) + { + for (size_t i = 0; i < rs_msg.height; i++) + { + for (size_t j = 0; j < rs_msg.width; j++) + { + const LidarPointCloudMsg::PointT& point = rs_msg.points[i + j * rs_msg.height]; + + *iter_x_ = point.x; + *iter_y_ = point.y; + *iter_z_ = point.z; + *iter_intensity_ = point.intensity; + + ++iter_x_; + ++iter_y_; + ++iter_z_; + ++iter_intensity_; + +#ifdef POINT_TYPE_XYZIRT + *iter_ring_ = point.ring; + *iter_timestamp_ = point.timestamp; + + ++iter_ring_; + ++iter_timestamp_; +#endif + } + } + } + else + { + for (size_t i = 0; i < rs_msg.points.size(); i++) + { + const LidarPointCloudMsg::PointT& point = rs_msg.points[i]; + + *iter_x_ = point.x; + *iter_y_ = point.y; + *iter_z_ = point.z; + *iter_intensity_ = point.intensity; + + ++iter_x_; + ++iter_y_;; + ++iter_z_; + ++iter_intensity_; + +#ifdef POINT_TYPE_XYZIRT + *iter_ring_ = point.ring; + *iter_timestamp_ = point.timestamp; + + ++iter_ring_; + ++iter_timestamp_; +#endif + } + } + + ros_msg.header.seq = rs_msg.seq; + ros_msg.header.stamp = ros_msg.header.stamp.fromSec(rs_msg.timestamp); + ros_msg.header.frame_id = frame_id; + + return ros_msg; +} + +class DestinationPointCloudRos : public DestinationPointCloud +{ +public: + + virtual void init(const YAML::Node& config); + virtual void sendPointCloud(const LidarPointCloudMsg& msg); + virtual ~DestinationPointCloudRos() = default; + +private: + std::shared_ptr nh_; + ros::Publisher pub_; + std::string frame_id_; + bool send_by_rows_; +}; + +inline void DestinationPointCloudRos::init(const YAML::Node& config) +{ + yamlRead(config["ros"], + "ros_send_by_rows", send_by_rows_, false); + + bool dense_points; + yamlRead(config["driver"], "dense_points", dense_points, false); + if (dense_points) + send_by_rows_ = false; + + yamlRead(config["ros"], + "ros_frame_id", frame_id_, "rslidar"); + + std::string ros_send_topic; + yamlRead(config["ros"], + "ros_send_point_cloud_topic", ros_send_topic, "rslidar_points"); + + nh_ = std::unique_ptr(new ros::NodeHandle()); + pub_ = nh_->advertise(ros_send_topic, 10); +} + +inline void DestinationPointCloudRos::sendPointCloud(const LidarPointCloudMsg& msg) +{ + pub_.publish(toRosMsg(msg, frame_id_, send_by_rows_)); +} + +} // namespace lidar +} // namespace robosense + +#endif // ROS_FOUND + +#ifdef ROS2_FOUND +#include +#include +#include + +namespace robosense +{ +namespace lidar +{ + +inline sensor_msgs::msg::PointCloud2 toRosMsg(const LidarPointCloudMsg& rs_msg, const std::string& frame_id, bool send_by_rows) +{ + sensor_msgs::msg::PointCloud2 ros_msg; + + int fields = 4; +#ifdef POINT_TYPE_XYZIRT + fields = 6; +#endif + ros_msg.fields.clear(); + ros_msg.fields.reserve(fields); + + if (send_by_rows) + { + ros_msg.width = rs_msg.width; + ros_msg.height = rs_msg.height; + } + else + { + ros_msg.width = rs_msg.height; // exchange width and height to be compatible with pcl::PointCloud<> + ros_msg.height = rs_msg.width; + } + + int offset = 0; + offset = addPointField(ros_msg, "x", 1, sensor_msgs::msg::PointField::FLOAT32, offset); + offset = addPointField(ros_msg, "y", 1, sensor_msgs::msg::PointField::FLOAT32, offset); + offset = addPointField(ros_msg, "z", 1, sensor_msgs::msg::PointField::FLOAT32, offset); + offset = addPointField(ros_msg, "intensity", 1, sensor_msgs::msg::PointField::FLOAT32, offset); +#ifdef POINT_TYPE_XYZIRT + offset = addPointField(ros_msg, "ring", 1, sensor_msgs::msg::PointField::UINT16, offset); + offset = addPointField(ros_msg, "timestamp", 1, sensor_msgs::msg::PointField::FLOAT64, offset); +#endif + +#if 0 + std::cout << "off:" << offset << std::endl; +#endif + + ros_msg.point_step = offset; + ros_msg.row_step = ros_msg.width * ros_msg.point_step; + ros_msg.is_dense = rs_msg.is_dense; + ros_msg.data.resize(ros_msg.point_step * ros_msg.width * ros_msg.height); + + sensor_msgs::PointCloud2Iterator iter_x_(ros_msg, "x"); + sensor_msgs::PointCloud2Iterator iter_y_(ros_msg, "y"); + sensor_msgs::PointCloud2Iterator iter_z_(ros_msg, "z"); + sensor_msgs::PointCloud2Iterator iter_intensity_(ros_msg, "intensity"); +#ifdef POINT_TYPE_XYZIRT + sensor_msgs::PointCloud2Iterator iter_ring_(ros_msg, "ring"); + sensor_msgs::PointCloud2Iterator iter_timestamp_(ros_msg, "timestamp"); +#endif + + if (send_by_rows) + { + for (size_t i = 0; i < rs_msg.height; i++) + { + for (size_t j = 0; j < rs_msg.width; j++) + { + const LidarPointCloudMsg::PointT& point = rs_msg.points[i + j * rs_msg.height]; + + *iter_x_ = point.x; + *iter_y_ = point.y; + *iter_z_ = point.z; + *iter_intensity_ = point.intensity; + + ++iter_x_; + ++iter_y_; + ++iter_z_; + ++iter_intensity_; + +#ifdef POINT_TYPE_XYZIRT + *iter_ring_ = point.ring; + *iter_timestamp_ = point.timestamp; + + ++iter_ring_; + ++iter_timestamp_; +#endif + } + } + } + else + { + for (size_t i = 0; i < rs_msg.points.size(); i++) + { + const LidarPointCloudMsg::PointT& point = rs_msg.points[i]; + + *iter_x_ = point.x; + *iter_y_ = point.y; + *iter_z_ = point.z; + *iter_intensity_ = point.intensity; + + ++iter_x_; + ++iter_y_; + ++iter_z_; + ++iter_intensity_; + +#ifdef POINT_TYPE_XYZIRT + *iter_ring_ = point.ring; + *iter_timestamp_ = point.timestamp; + + ++iter_ring_; + ++iter_timestamp_; +#endif + } + } + + ros_msg.header.stamp.sec = (uint32_t)floor(rs_msg.timestamp); + ros_msg.header.stamp.nanosec = (uint32_t)round((rs_msg.timestamp - ros_msg.header.stamp.sec) * 1e9); + ros_msg.header.frame_id = frame_id; + + return ros_msg; +} + +class DestinationPointCloudRos : virtual public DestinationPointCloud +{ +public: + + virtual void init(const YAML::Node& config); + virtual void sendPointCloud(const LidarPointCloudMsg& msg); + virtual ~DestinationPointCloudRos() = default; + +private: + std::shared_ptr node_ptr_; + rclcpp::Publisher::SharedPtr pub_; + std::string frame_id_; + bool send_by_rows_; +}; + +inline void DestinationPointCloudRos::init(const YAML::Node& config) +{ + yamlRead(config["ros"], + "ros_send_by_rows", send_by_rows_, false); + + bool dense_points; + yamlRead(config["driver"], "dense_points", dense_points, false); + if (dense_points) + send_by_rows_ = false; + + yamlRead(config["ros"], + "ros_frame_id", frame_id_, "rslidar"); + + std::string ros_send_topic; + yamlRead(config["ros"], + "ros_send_point_cloud_topic", ros_send_topic, "rslidar_points"); + + static int node_index = 0; + std::stringstream node_name; + node_name << "rslidar_points_destination_" << node_index++; + + node_ptr_.reset(new rclcpp::Node(node_name.str())); + pub_ = node_ptr_->create_publisher(ros_send_topic, 100); +} + +inline void DestinationPointCloudRos::sendPointCloud(const LidarPointCloudMsg& msg) +{ + pub_->publish(toRosMsg(msg, frame_id_, send_by_rows_)); +} + +} // namespace lidar +} // namespace robosense + +#endif + diff --git a/src/utility/common.h b/src/utility/common.hpp similarity index 92% rename from src/utility/common.h rename to src/utility/common.hpp index a1de6592..fa96c83c 100644 --- a/src/utility/common.h +++ b/src/utility/common.hpp @@ -31,7 +31,5 @@ THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *********************************************************************************************************************/ #pragma once -#include "rs_driver/common/common_header.h" -#include "rs_driver/utility/time.h" -#include "rs_driver/utility/thread_pool.hpp" -#include "rs_driver/utility/lock_queue.h" + +#include "rs_driver/common/rs_log.hpp" diff --git a/src/utility/protobuf_communicator.hpp b/src/utility/protobuf_communicator.hpp deleted file mode 100644 index 240f9fb0..00000000 --- a/src/utility/protobuf_communicator.hpp +++ /dev/null @@ -1,402 +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 -#ifdef PROTO_FOUND -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#define SPLIT_SIZE 5000 -#define MAX_RECEIVE_LENGTH 5200 - -namespace robosense -{ -namespace lidar -{ -enum class DataEndianType -{ - RS_BIG_ENDIAN = 0, - RS_SMALL_ENDIAN = 1 -}; -template -class CRSEndian -{ -public: - typedef typename std::shared_ptr Ptr; - typedef typename std::shared_ptr ConstPtr; - -public: - CRSEndian() - { - unsigned char* p_char = (unsigned char*)(&magic_data_); - if (*p_char == magic_big_endian_[0] && (*(p_char + 1)) == magic_big_endian_[1] && - (*(p_char + 2)) == magic_big_endian_[2] && (*(p_char + 3)) == magic_big_endian_[3]) - { - host_endian_type_ = DataEndianType::RS_BIG_ENDIAN; - } - else - { - host_endian_type_ = DataEndianType::RS_SMALL_ENDIAN; - } - } - -public: - DataEndianType getHostEndian() - { - return host_endian_type_; - } - - int toTargetEndianArray(T t, void* p_array, const unsigned int& max_size, const DataEndianType& dst_endian) - { - unsigned int t_size = sizeof(T); - assert(t_size <= max_size); - assert(p_array != nullptr); - if (host_endian_type_ != dst_endian) - { - char* p_data_ = (char*)(&t); - unsigned int t_size_h = t_size / 2; - for (unsigned int idx = 0; idx < t_size_h; ++idx) - { - char tmp = p_data_[idx]; - unsigned int swapIdx = t_size - idx - 1; - p_data_[idx] = p_data_[swapIdx]; - p_data_[swapIdx] = tmp; - } - } - memcpy(p_array, &t, t_size); - return 0; - } - - int toHostEndianValue(T& t, const void* p_array, const unsigned int& max_size, const DataEndianType& src_endian) - { - unsigned int t_size = sizeof(T); - assert(p_array != nullptr); - assert(t_size <= max_size); - if (src_endian != host_endian_type_) - { - char* p_data_ = (char*)(p_array); - unsigned int t_size_h = t_size / 2; - for (unsigned idx = 0; idx < t_size_h; ++idx) - { - char tmp = p_data_[idx]; - unsigned int swapIdx = t_size - idx - 1; - p_data_[idx] = p_data_[swapIdx]; - p_data_[swapIdx] = tmp; - } - } - memcpy(&t, p_array, t_size); - return 0; - } - -private: - DataEndianType host_endian_type_; - const unsigned int magic_data_ = 0xA1B2C3D4; - const unsigned char magic_small_endian_[4] = { (unsigned char)0xD4, (unsigned char)0xC3, (unsigned char)0xB2, - (unsigned char)0xA1 }; - const unsigned char magic_big_endian_[4] = { (unsigned char)0xA1, (unsigned char)0xB2, (unsigned char)0xC3, - (unsigned char)0xD4 }; -}; - -struct alignas(16) ProtoMsgHeader -{ - unsigned int frame_num; - unsigned int total_msg_cnt; - unsigned int msg_id; - unsigned int msg_length; - unsigned int total_msg_length; - - ProtoMsgHeader() - { - frame_num = 0; - total_msg_cnt = 0; - msg_id = 0; - msg_length = 0; - total_msg_length = 0; - } - bool toTargetEndianArray(char* p_array, const unsigned int maxArraySize, - const DataEndianType dst_endian = DataEndianType::RS_BIG_ENDIAN) const - { - if (p_array == nullptr || maxArraySize < sizeof(ProtoMsgHeader)) - { - return false; - } - - CRSEndian uint32Endian; - CRSEndian uint64Endian; - - int offset = 0; - uint32Endian.toTargetEndianArray(frame_num, p_array + offset, maxArraySize - offset, dst_endian); - - offset += sizeof(unsigned int); - uint32Endian.toTargetEndianArray(total_msg_cnt, p_array + offset, maxArraySize - offset, dst_endian); - - offset += sizeof(unsigned int); - uint32Endian.toTargetEndianArray(msg_id, p_array + offset, maxArraySize - offset, dst_endian); - - offset += sizeof(unsigned int); - uint32Endian.toTargetEndianArray(msg_length, p_array + offset, maxArraySize - offset, dst_endian); - - offset += sizeof(unsigned int); - uint32Endian.toTargetEndianArray(total_msg_length, p_array + offset, maxArraySize - offset, dst_endian); - return true; - } - bool toHostEndianValue(const char* p_array, const unsigned int arraySize, - const DataEndianType src_endian = DataEndianType::RS_BIG_ENDIAN) - { - if (p_array == nullptr || arraySize < sizeof(ProtoMsgHeader)) - { - return false; - } - CRSEndian uint32Endian; - CRSEndian uint64Endian; - - int offset = 0; - uint32Endian.toHostEndianValue(frame_num, p_array + offset, arraySize - offset, src_endian); - - offset += sizeof(unsigned int); - uint32Endian.toHostEndianValue(total_msg_cnt, p_array + offset, arraySize - offset, src_endian); - - offset += sizeof(unsigned int); - uint32Endian.toHostEndianValue(msg_id, p_array + offset, arraySize - offset, src_endian); - - offset += sizeof(unsigned int); - uint32Endian.toHostEndianValue(msg_length, p_array + offset, arraySize - offset, src_endian); - - offset += sizeof(unsigned int); - uint32Endian.toHostEndianValue(total_msg_length, p_array + offset, arraySize - offset, src_endian); - return true; - } -}; -using boost::asio::deadline_timer; -using boost::asio::ip::udp; -/** - * @brief Protobuf UDP transmission basic class - * @note Initialize socket sender and receiver, and define send function and receive function - */ -class ProtoCommunicator -{ -public: - ProtoCommunicator() = default; - ~ProtoCommunicator() = default; - - /** - * @brief intilize the socket sender - * @param proto_send_port: destination port - * @param proto_send_ip: destination IP address - * @retval 0: success -1: failed - */ - inline int initSender(std::string proto_send_port, std::string proto_send_ip) - { - try - { - send_sock_ptr_.reset(new udp::socket(io_service_, udp::endpoint(udp::v4(), 0))); - boost::asio::socket_base::broadcast option(true); - send_sock_ptr_->set_option(option); - udp::resolver resolver(io_service_); - udp::resolver::query query(udp::v4(), proto_send_ip, proto_send_port); - iterator_ = resolver.resolve(query); - } - catch (...) - { - RS_ERROR << "Proto Sender Port is already used! Abort!" << RS_REND; - exit(-1); - } - return 0; - } - /** - * @brief Initialize the socket receiver - * @param proto_recv_port: the receiver port number - * @retval 0:success -1:failed - */ - inline int initReceiver(uint16_t proto_recv_port) - { - try - { - recv_sock_ptr_.reset(new udp::socket(io_service_, udp::endpoint(udp::v4(), proto_recv_port))); - deadline_.reset(new deadline_timer(io_service_)); - } - catch (...) - { - RS_ERROR << "Proto Receiver Port is already used! Abort!" << RS_REND; - exit(-1); - } - deadline_->expires_at(boost::posix_time::pos_infin); - checkDeadline(); - return 0; - } - - /* message send function & message receive function */ - /** - * @brief the message send function - * @note serialize the message to protobuf and send it - * @param *pMsgData: data - * @param &header: header - * @retval >=0 : success -1: failed - */ - inline int sendProtoMsg(const void* pMsgData, const ProtoMsgHeader& header) - { - int sendLen = header.msg_length + sizeof(ProtoMsgHeader); - char* sendBuffer = (char*)malloc(sendLen); - memset(sendBuffer, 0, sendLen); - header.toTargetEndianArray(sendBuffer, sizeof(ProtoMsgHeader), DataEndianType::RS_BIG_ENDIAN); - if (header.msg_length > 0) - { - memcpy(sendBuffer + sizeof(ProtoMsgHeader), pMsgData, header.msg_length); - } - int ret = send_sock_ptr_->send_to(boost::asio::buffer(sendBuffer, sendLen), *iterator_); - free(sendBuffer); - return ret; - } - /** - * @brief message receive function - * @note receive the message and deserilize it - * @param *pMsgData: the output message - * @param msgMaxLen: max receive message length - * @param &header: output header - * @retval >=0: success -1: failed - */ - inline int receiveProtoMsg(void* pMsgData, const int msgMaxLen, ProtoMsgHeader& header) - { - deadline_->expires_from_now(boost::posix_time::seconds(1)); - boost::system::error_code ec = boost::asio::error::would_block; - std::size_t ret = 0; - char* pRecvBuffer = (char*)malloc(msgMaxLen + sizeof(ProtoMsgHeader)); - recv_sock_ptr_->async_receive(boost::asio::buffer(pRecvBuffer, msgMaxLen + sizeof(ProtoMsgHeader)), - boost::bind(&ProtoCommunicator::handleReceive, _1, _2, &ec, &ret)); - do - { - io_service_.run_one(); - } while (ec == boost::asio::error::would_block); - if (ec) - { - free(pRecvBuffer); - return -1; - } - header.toHostEndianValue(pRecvBuffer, sizeof(ProtoMsgHeader), DataEndianType::RS_BIG_ENDIAN); - if (ret < (std::size_t)(header.msg_length + sizeof(ProtoMsgHeader))) - { - free(pRecvBuffer); - return -1; - } - if (header.msg_length > 0) - { - memcpy(pMsgData, pRecvBuffer + sizeof(ProtoMsgHeader), header.msg_length); - } - free(pRecvBuffer); - return ret; - } - - template - bool sendSplitMsg(const T& msg) - { - void* buf = malloc(msg.ByteSize() + SPLIT_SIZE); - msg.SerializeToArray(buf, msg.ByteSize()); - int pkt_num = ceil(1.0 * msg.ByteSize() / SPLIT_SIZE); - ProtoMsgHeader tmp_header; - tmp_header.frame_num = msg.seq(); - tmp_header.msg_length = SPLIT_SIZE; - tmp_header.total_msg_cnt = pkt_num; - tmp_header.total_msg_length = msg.ByteSize(); - for (int i = 0; i < pkt_num; i++) - { - tmp_header.msg_id = i; - void* tmp_buf = malloc(SPLIT_SIZE); - memcpy(tmp_buf, (uint8_t*)buf + i * SPLIT_SIZE, SPLIT_SIZE); - if (sendProtoMsg(tmp_buf, tmp_header) == -1) - { - free(tmp_buf); - free(buf); - return false; - } - free(tmp_buf); - usleep(2); - } - free(buf); - return true; - } - - template - bool sendSingleMsg(const T& msg) - { - ProtoMsgHeader tmp_header; - tmp_header.msg_length = msg.ByteSize(); - void* buf = malloc(tmp_header.msg_length); - msg.SerializeToArray(buf, tmp_header.msg_length); - if (sendProtoMsg(buf, tmp_header) == -1) - { - free(buf); - return false; - } - free(buf); - return true; - } - -private: - inline void checkDeadline() - { - if (deadline_->expires_at() <= deadline_timer::traits_type::now()) - { - recv_sock_ptr_->cancel(); - deadline_->expires_at(boost::posix_time::pos_infin); - } - deadline_->async_wait(boost::bind(&ProtoCommunicator::checkDeadline, this)); - } - static void 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; - } - -private: - /* UDP socket related varibles */ - boost::asio::io_service io_service_; - udp::resolver::iterator iterator_; - std::unique_ptr send_sock_ptr_; - std::unique_ptr recv_sock_ptr_; - std::unique_ptr deadline_; -}; - -} // namespace lidar -} // namespace robosense -#endif // PROTO_FOUND \ No newline at end of file diff --git a/src/utility/yaml_reader.hpp b/src/utility/yaml_reader.hpp index d01a680a..4c34609d 100644 --- a/src/utility/yaml_reader.hpp +++ b/src/utility/yaml_reader.hpp @@ -31,12 +31,16 @@ THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *********************************************************************************************************************/ #pragma once + #include -#include "utility/common.h" + +#include "utility/common.hpp" + namespace robosense { namespace lidar { + template inline void yamlReadAbort(const YAML::Node& yaml, const std::string& key, T& out_val) { @@ -75,7 +79,7 @@ inline YAML::Node yamlSubNodeAbort(const YAML::Node& yaml, const std::string& no RS_ERROR << " : Cannot find subnode " << node << ". Aborting!!!" << RS_REND; exit(-1); } - return std::move(ret); + return ret; } } // namespace lidar