├── src ├── msg │ ├── ros_msg │ │ ├── RsCompressedImage.msg │ │ ├── RslidarPacket.msg │ │ ├── rslidar_packet_legacy.hpp │ │ ├── rs_compressed_image.hpp │ │ ├── rslidar_scan_legacy.hpp │ │ └── rslidar_packet.hpp │ └── rs_msg │ │ └── lidar_point_cloud_msg.hpp ├── utility │ ├── common.hpp │ └── yaml_reader.hpp ├── manager │ ├── node_manager.hpp │ └── node_manager.cpp └── source │ ├── source.hpp │ ├── source_driver.hpp │ ├── source_packet_ros.hpp │ └── source_pointcloud_ros.hpp ├── img └── 01_01_download_page.png ├── doc ├── howto │ ├── img │ │ ├── 07_07_vlan.png │ │ ├── 07_02_unicast.png │ │ ├── 07_01_broadcast.png │ │ ├── 07_03_multicast.png │ │ ├── 07_06_vlan_layer.png │ │ ├── 07_08_user_layer.png │ │ ├── 04_01_packet_rosbag.png │ │ ├── 07_05_multi_lidars_ip.png │ │ └── 07_04_multi_lidars_port.png │ ├── 12_how_to_create_deb.md │ ├── 10_how_to_use_coordinate_transformation_CN.md │ ├── 06_how_to_decode_online_lidar_CN.md │ ├── 08_how_to_decode_pcap_file_CN.md │ ├── 09_pcap_file_advanced_topics_CN.md │ ├── 10_how_to_use_coordinate_transformation.md │ ├── 06_how_to_decode_online_lidar.md │ ├── 05_how_to_change_point_type_CN.md │ ├── 08_how_to_decode_pcap_file.md │ ├── 05_how_to_change_point_type.md │ ├── 09_pcap_file_advanced_topics.md │ ├── 11_how_to_record_replay_packet_rosbag_CN.md │ ├── 11_how_to_record_replay_packet_rosbag.md │ ├── 07_online_lidar_advanced_topics_CN.md │ ├── 04_how_to_work_along_with_rslidar_sdk_node_v1.3.x_CN.md │ └── 07_online_lidar_advanced_topics.md ├── src_intro │ ├── img │ │ ├── class_node_manager.png │ │ ├── class_source_driver.png │ │ ├── class_destination_packet.png │ │ ├── class_source_destination.png │ │ ├── class_source_packet_ros.png │ │ └── class_destination_pointcloud.png │ └── rslidar_sdk_intro_CN.md └── intro │ ├── 03_hiding_parameters_intro_CN.md │ ├── 03_hiding_parameters_intro.md │ ├── 02_parameter_intro_CN.md │ └── 02_parameter_intro.md ├── .gitmodules ├── .gitignore ├── launch ├── start.launch ├── start.py └── elequent_start.py ├── package.xml ├── create_debian.sh ├── LICENSE ├── config ├── config_one_lidar.yaml └── config.yaml ├── .clang-format ├── CHANGELOG.md ├── README_CN.md ├── README.md ├── node └── rslidar_sdk_node.cpp ├── rviz ├── rviz2.rviz └── rviz.rviz └── CMakeLists.txt /src/msg/ros_msg/RsCompressedImage.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | uint8 type 3 | uint8[] data 4 | -------------------------------------------------------------------------------- /img/01_01_download_page.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/frozen-dev71/dual-lidar/main/img/01_01_download_page.png -------------------------------------------------------------------------------- /doc/howto/img/07_07_vlan.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/frozen-dev71/dual-lidar/main/doc/howto/img/07_07_vlan.png -------------------------------------------------------------------------------- /doc/howto/img/07_02_unicast.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/frozen-dev71/dual-lidar/main/doc/howto/img/07_02_unicast.png -------------------------------------------------------------------------------- /src/msg/ros_msg/RslidarPacket.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | uint8 is_difop 3 | uint8 is_frame_begin 4 | uint8[] data 5 | -------------------------------------------------------------------------------- /doc/howto/img/07_01_broadcast.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/frozen-dev71/dual-lidar/main/doc/howto/img/07_01_broadcast.png -------------------------------------------------------------------------------- /doc/howto/img/07_03_multicast.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/frozen-dev71/dual-lidar/main/doc/howto/img/07_03_multicast.png -------------------------------------------------------------------------------- /doc/howto/img/07_06_vlan_layer.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/frozen-dev71/dual-lidar/main/doc/howto/img/07_06_vlan_layer.png -------------------------------------------------------------------------------- /doc/howto/img/07_08_user_layer.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/frozen-dev71/dual-lidar/main/doc/howto/img/07_08_user_layer.png -------------------------------------------------------------------------------- /doc/howto/img/04_01_packet_rosbag.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/frozen-dev71/dual-lidar/main/doc/howto/img/04_01_packet_rosbag.png -------------------------------------------------------------------------------- /doc/howto/img/07_05_multi_lidars_ip.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/frozen-dev71/dual-lidar/main/doc/howto/img/07_05_multi_lidars_ip.png -------------------------------------------------------------------------------- /doc/howto/img/07_04_multi_lidars_port.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/frozen-dev71/dual-lidar/main/doc/howto/img/07_04_multi_lidars_port.png -------------------------------------------------------------------------------- /doc/src_intro/img/class_node_manager.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/frozen-dev71/dual-lidar/main/doc/src_intro/img/class_node_manager.png -------------------------------------------------------------------------------- /doc/src_intro/img/class_source_driver.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/frozen-dev71/dual-lidar/main/doc/src_intro/img/class_source_driver.png -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "src/rs_driver"] 2 | path = src/rs_driver 3 | url = https://github.com/RoboSense-LiDAR/rs_driver.git 4 | branch = main 5 | -------------------------------------------------------------------------------- /doc/src_intro/img/class_destination_packet.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/frozen-dev71/dual-lidar/main/doc/src_intro/img/class_destination_packet.png -------------------------------------------------------------------------------- /doc/src_intro/img/class_source_destination.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/frozen-dev71/dual-lidar/main/doc/src_intro/img/class_source_destination.png -------------------------------------------------------------------------------- /doc/src_intro/img/class_source_packet_ros.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/frozen-dev71/dual-lidar/main/doc/src_intro/img/class_source_packet_ros.png -------------------------------------------------------------------------------- /doc/src_intro/img/class_destination_pointcloud.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/frozen-dev71/dual-lidar/main/doc/src_intro/img/class_destination_pointcloud.png -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.cproject 2 | *.project 3 | *~ 4 | *.swp 5 | *.cur_trans 6 | build 7 | .vscode 8 | *.csv 9 | build/* 10 | cmake-build-debug/ 11 | .idea/ 12 | version.h 13 | *.pb.cc 14 | *.pb.h 15 | -------------------------------------------------------------------------------- /launch/start.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /launch/start.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch_ros.actions import Node 3 | from ament_index_python.packages import get_package_share_directory 4 | 5 | def generate_launch_description(): 6 | 7 | rviz_config=get_package_share_directory('rslidar_sdk')+'/rviz/rviz2.rviz' 8 | 9 | config_file = '' # your config file path 10 | 11 | return LaunchDescription([ 12 | Node(namespace='rslidar_sdk', package='rslidar_sdk', executable='rslidar_sdk_node', output='screen', parameters=[{'config_path': config_file}]), 13 | Node(namespace='rviz2', package='rviz2', executable='rviz2', arguments=['-d',rviz_config]) 14 | ]) 15 | -------------------------------------------------------------------------------- /launch/elequent_start.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch_ros.actions import Node 3 | from ament_index_python.packages import get_package_share_directory 4 | def generate_launch_description(): 5 | rviz_config=get_package_share_directory('rslidar_sdk')+'/rviz/rviz2.rviz' 6 | return LaunchDescription([ 7 | Node( 8 | package='rslidar_sdk', 9 | node_namespace='rslidar_sdk', 10 | node_name='rslidar_sdk_node', 11 | node_executable='rslidar_sdk_node', 12 | output='screen' 13 | ), 14 | Node( 15 | package='rviz2', 16 | node_namespace='rviz2', 17 | node_name='rviz2', 18 | node_executable='rviz2', 19 | arguments=['-d',rviz_config] 20 | ) 21 | ]) 22 | -------------------------------------------------------------------------------- /doc/howto/12_how_to_create_deb.md: -------------------------------------------------------------------------------- 1 | # 12 How to create deb 2 | 3 | 4 | 5 | ## 12.1 Introduction 6 | 7 | Generating a ".deb" installable file is useful. 8 | 9 | 10 | 11 | ## 12.2 Create deb 12 | 13 | Just run the shell script: 14 | ``` 15 | ./create_debian.sh 16 | ``` 17 | The deb file will be generated in the parent directory of `rslidar_sdk`. 18 | 19 | 20 | ## 12.3 Use the deb 21 | 22 | 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. 23 | 24 | ``` 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | ``` 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rslidar_sdk 4 | 1.5.16 5 | The rslidar_sdk package 6 | robosense 7 | BSD 8 | 9 | catkin 10 | ament_cmake 11 | 12 | libpcap 13 | pcl_ros 14 | rclcpp 15 | roscpp 16 | roslib 17 | rslidar_msg 18 | sensor_msgs 19 | std_msgs 20 | yaml-cpp 21 | 22 | 23 | ament_cmake 24 | 25 | 26 | -------------------------------------------------------------------------------- /create_debian.sh: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env bash 2 | 3 | ## install the python-bloom fakeroot 4 | 5 | function install_pkg() 6 | { 7 | pkg_found=$(dpkg -l | grep ${1}) 8 | if [[ $? -eq 0 ]] 9 | then 10 | echo "${1} already installed." 11 | else 12 | echo "Install ${1} ..." 13 | sudo apt-get install ${1} -y 14 | fi 15 | } 16 | 17 | install_pkg python-bloom 18 | install_pkg fakeroot 19 | 20 | echo -e "\n\033[1;32m ~~ (1). Delete old debian folders in the directory...\033[0m" 21 | rm -rf debian/ obj-x86_64-linux-gnu/ 22 | 23 | echo -e "\n\033[1;32m ~~ (2). Delete any backup files...\033[0m" 24 | find . -type f -name '*~' -delete 25 | 26 | echo -e "\n\033[1;32m ~~ (3). Create debian packages...\033[0m\n" 27 | bloom-generate rosdebian --os-name ubuntu --os-version `echo $(lsb_release -sc)` --ros-distro `echo ${ROS_DISTRO}` 28 | 29 | # sed 's#dh_shlibdeps*#dh_shlibdeps --dpkg-shlibdeps-params=--ignore-missing-info#g' debian/rules 30 | ## when dpkg-shlibdeps: error: no dependency information found for 31 | ## adding bellow code to debian/rules 32 | ## 33 | ## override_dh_shlibdeps: 34 | ## dh_shlibdeps --dpkg-shlibdeps-params=--ignore-missing-info 35 | ## It is necessary to insert a TAB instead of spaces before "dh_shlibdeps ..." 36 | 37 | target_string=$(grep "dh_shlibdeps " debian/rules) 38 | replace_string=" dh_shlibdeps --dpkg-shlibdeps-params=--ignore-missing-info" 39 | sed -i "s#${target_string}#${replace_string}#g" debian/rules 40 | 41 | fakeroot debian/rules binary 42 | 43 | echo -e "\n\033[1;32m ~~ (4). Delete old debian folders in the directory...\033[0m" 44 | rm -rf debian/ obj-x86_64-linux-gnu/ 45 | 46 | echo -e "\n\033[1;32m ~~ (5). Delete any backup files...\033[0m" 47 | find . -type f -name '*~' -delete 48 | -------------------------------------------------------------------------------- /doc/howto/10_how_to_use_coordinate_transformation_CN.md: -------------------------------------------------------------------------------- 1 | # 10 如何使用坐标变换功能 2 | 3 | 4 | 5 | ## 10.1 简介 6 | 7 | rslidar_sdk支持对点云进行坐标变换，本文档展示如何作这种变换。 8 | 9 | 在阅读本文档之前，请确保已阅读雷达用户手册和[隐藏参数介绍](../intro/03_hiding_parameters_intro_CN.md)。 10 | 11 | 12 | 13 | ## 10.2 依赖库 14 | 15 | rslidar_sdk的坐标变换基于libeigen库，所以要先安装它。 16 | 17 | ```bash 18 | sudo apt-get install libeigen3-dev 19 | ``` 20 | 21 | 22 | 23 | ## 10.3 编译 24 | 25 | 要启用坐标变换，编译rslidar_sdk时，需要将```ENABLE_TRANSFORM```选项设置为```ON```. 26 | 27 | - 直接编译 28 | 29 | ```bash 30 | cmake -DENABLE_TRANSFORM=ON .. 31 | ``` 32 | 33 | - ROS 34 | 35 | ```bash 36 | catkin_make -DENABLE_TRANSFORM=ON 37 | ``` 38 | 39 | - ROS2 40 | 41 | ```bash 42 | colcon build --cmake-args '-DENABLE_TRANSFORM=ON' 43 | ``` 44 | 45 | 46 | 47 | ## 10.4 设置雷达参数 48 | 49 | 在`config.yaml`中，设置`lidar-lidar`部分的参数`x`、, `y`、 `z`、 `roll`、 `pitch` 、`yaw`。 50 | 51 | ```yaml 52 | common: 53 | msg_source: 1 54 | send_packet_ros: false 55 | send_point_cloud_ros: true 56 | send_packet_proto: false 57 | send_point_cloud_proto: false 58 | lidar: 59 | - driver: 60 | lidar_type: RS128 61 | msop_port: 6699 62 | difop_port: 7788 63 | start_angle: 0 64 | end_angle: 360 65 | min_distance: 0.2 66 | max_distance: 200 67 | use_lidar_clock: false 68 | pcap_path: /home/robosense/lidar.pcap 69 | x: 1 70 | y: 0 71 | z: 2.5 72 | roll: 0.1 73 | pitch: 0.2 74 | yaw: 1.57 75 | ``` 76 | 77 | 78 | 79 | ## 10.5 运行 80 | 81 | 运行程序。 82 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2020 RoboSense 2 | All rights reserved 3 | 4 | By downloading, copying, installing or using the software you agree to this license. If you do not agree to this license, do not download, install, copy or use the software. 5 | 6 | License Agreement 7 | For RoboSense LiDAR SDK Library 8 | (3-clause BSD License) 9 | 10 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 11 | 12 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 13 | 14 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 15 | 16 | 3. Neither the names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used to endorse or promote products derived from this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 19 | 20 | -------------------------------------------------------------------------------- /doc/howto/06_how_to_decode_online_lidar_CN.md: -------------------------------------------------------------------------------- 1 | # 6 如何连接在线雷达 2 | 3 | 4 | 5 | ## 6.1 简介 6 | 7 | 本文档描述如何连接在线雷达，并发送点云数据到ROS。 8 | 9 | 在阅读本文档之前， 请确保已经阅读过雷达用户手册和[参数简介](../intro/02_parameter_intro_CN.md) 。 10 | 11 | 12 | 13 | ## 6.2 步骤 14 | 15 | ### 6.2.1 获取数据端口号 16 | 17 | 根据雷达用户手册连接雷达, 并设置好您的电脑的IP地址。 18 | 19 | 请参考雷达用户手册，或使用第三方工具（如WireShark等）得到雷达的MSOP端口号和DIFOP端口号。端口的默认值分别为```6699```和```7788```。 20 | 21 | ### 6.2.2 设置参数文件 22 | 23 | 设置参数文件```config.yaml```。 24 | 25 | #### 6.2.2.1 common部分 26 | 27 | ```yaml 28 | common: 29 | msg_source: 1 30 | send_packet_ros: false 31 | send_point_cloud_ros: true 32 | send_packet_proto: false 33 | send_point_cloud_proto: false 34 | ``` 35 | 36 | 消息来源于在线雷达，因此请设置```msg_source=1```。 37 | 38 | 将点云发送到ROS以便查看，因此设置 ```send_point_cloud_ros = true``` 。 39 | 40 | #### 6.2.2.2 lidar-driver部分 41 | 42 | ```yaml 43 | lidar: 44 | - driver: 45 | lidar_type: RS128 46 | msop_port: 6699 47 | difop_port: 7788 48 | start_angle: 0 49 | end_angle: 360 50 | min_distance: 0.2 51 | max_distance: 200 52 | use_lidar_clock: false 53 | ``` 54 | 55 | 将 ```lidar_type``` 设置为LiDAR类型 。 56 | 57 | 设置 ```msop_port``` 和 ```difop_port``` 为雷达数据端口号。 58 | 59 | #### 6.2.2.3 lidar-ros部分 60 | 61 | ```yaml 62 | ros: 63 | ros_frame_id: rslidar 64 | ros_recv_packet_topic: /rslidar_packets 65 | ros_send_packet_topic: /rslidar_packets 66 | ros_send_point_cloud_topic: /rslidar_points 67 | ``` 68 | 69 | 将 ```ros_send_point_cloud_topic``` 设置为发送点云的话题。 70 | 71 | ### 6.2.3 运行 72 | 73 | 运行程序。 74 | 75 | -------------------------------------------------------------------------------- /doc/howto/08_how_to_decode_pcap_file_CN.md: -------------------------------------------------------------------------------- 1 | # 8 如何解码PCAP文件 2 | 3 | 4 | 5 | ## 8.1 简介 6 | 7 | 本文档展示如何解码PCAP文件, 并发送点云数据到ROS。 8 | 9 | 在阅读本文档之前，请确保已阅读雷达用户手册和 [参数简介](../intro/02_parameter_intro_CN.md) 。 10 | 11 | 12 | 13 | ## 8.2 步骤 14 | 15 | ### 8.2.1 获取数据的端口号 16 | 17 | 请参考雷达用户手册，或者使用第三方工具（WireShark等）抓包，得到雷达的目标MSOP端口和目标DIFOP端口。端口的默认值分别为`6699`和`7788`。 18 | 19 | ### 8.2.2 设置参数文件 20 | 21 | 设置参数文件```config.yaml```。 22 | 23 | #### 8.2.2.1 common部分 24 | 25 | ```yaml 26 | common: 27 | msg_source: 3 28 | send_packet_ros: false 29 | send_point_cloud_ros: true 30 | send_packet_proto: false 31 | send_point_cloud_proto: false 32 | ``` 33 | 34 | 消息来自PCAP包，所以设置 ```msg_source = 3``` 。 35 | 36 | 将点云发送到ROS以便查看，所以设置 ```send_point_cloud_ros = true``` 。 37 | 38 | #### 8.2.2.2 lidar-driver部分 39 | 40 | ```yaml 41 | lidar: 42 | - driver: 43 | lidar_type: RS128 44 | msop_port: 6699 45 | difop_port: 7788 46 | start_angle: 0 47 | end_angle: 360 48 | min_distance: 0.2 49 | max_distance: 200 50 | use_lidar_clock: false 51 | pcap_path: /home/robosense/lidar.pcap 52 | ``` 53 | 54 | 将```pcap_path``` 设置为PCAP文件的全路径。 55 | 56 | 将 ```lidar_type``` 设置为LiDAR类型。 57 | 58 | 设置 ```msop_port``` 和 ```difop_port``` 为雷达数据的目标端口号，这里分别是6699和7788。 59 | 60 | #### 8.2.2.3 lidar-ros部分 61 | 62 | ```yaml 63 | ros: 64 | ros_frame_id: rslidar 65 | ros_recv_packet_topic: /rslidar_packets 66 | ros_send_packet_topic: /rslidar_packets 67 | ros_send_point_cloud_topic: /rslidar_points 68 | ``` 69 | 70 | 将 ```ros_send_point_cloud_topic``` 设置为发送点云的话题，这里是/rslidar_points。 71 | 72 | ### 8.2.3 运行 73 | 74 | 运行程序。 75 | -------------------------------------------------------------------------------- /doc/howto/09_pcap_file_advanced_topics_CN.md: -------------------------------------------------------------------------------- 1 | # 9 PCAP文件 - 高级主题 2 | 3 | 4 | 5 | ## 9.1 简介 6 | 7 | RoboSense雷达可以工作在如下场景。 8 | + 单播/组播/广播模式 9 | + 运行在VLAN协议上 10 | + 向Packet中加入用户自己的层 11 | + 接入多个雷达 12 | 13 | 本文说明在每种场景下，如何配置rslidar_sdk。 14 | 15 | 在阅读本文之前，请先阅读： 16 | + 雷达用户使用手册 17 | + [参数介绍](../intro/02_parameter_intro_CN.md) 18 | + [在线雷达-高级主题](./07_online_lidar_advanced_topics_CN.md) 19 | 20 | 21 | 22 | ## 9.2 一般场景 23 | 24 | 在下列场景下，使用如下配置解码PCAP文件。 25 | + 广播/组播/单播模式 26 | + PCAP文件中有多个雷达 27 | 28 | ```yaml 29 | common: 30 | msg_source: 3 31 | send_point_cloud_ros: true 32 | 33 | lidar: 34 | - driver: 35 | lidar_type: RS32 36 | pcap_path: /home/robosense/lidar.pcap 37 | msop_port: 6699 38 | difop_port: 7788 39 | ros: 40 | ros_frame_id: rslidar 41 | ros_send_point_cloud_topic: /rslidar_points 42 | ``` 43 | 44 | 一个例外是：PCAP文件中有多个雷达数据，但这些雷达目的端口相同，使用不同的目的IP地址来区分。这种情况不支持。 45 | 46 | 47 | 48 | ## 9.3 VLAN 49 | 50 | 有些场景下，雷达工作在VLAN环境下。这时MSOP/DIFOP包带VLAN层，如下图。 51 | ![](./img/07_06_vlan_layer.png) 52 | 53 | rs_driver使用libpcap库解析PCAP文件，可以得到完整的、包括VLAN层的MSOP/DIFOP包。 54 | 55 | 要剥除VLAN层，只需要设置`use_vlan: true`。 56 | 57 | ```yaml 58 | lidar: 59 | - driver: 60 | lidar_type: RS32 61 | pcap_path: /home/robosense/lidar.pcap 62 | msop_port: 6699 63 | difop_port: 7788 64 | use_vlan: true 65 | ``` 66 | 67 | 68 | 69 | ## 9.4 User Layer, Tail Layer 70 | 71 | 某些场景下，用户可能在MSOP/DIFOP数据前后加入自己的层。 72 | + USER_LAYER 在MSOP/DIFOP数据之前，TAIL_LAYER在MSOP/DIFOP数据之后。 73 | ![](./img/07_08_user_layer.png) 74 | 75 | 这些层是UDP数据的一部分，所以rs_driver可以自己剥除他们。只需要告诉它每个层的字节数就可以。 76 | 77 | 如下的例子中，指定USER_LAYER为8字节，TAIL_LAYER为4字节。 78 | 79 | ```yaml 80 | lidar: 81 | - driver: 82 | lidar_type: RS32 83 | pcap_path: /home/robosense/lidar.pcap 84 | msop_port: 6699 85 | difop_port: 7788 86 | user_layer_bytes: 8 87 | tail_layer_bytes: 4 88 | ``` 89 | 90 | -------------------------------------------------------------------------------- /config/config_one_lidar.yaml: -------------------------------------------------------------------------------- 1 | common: 2 | msg_source: 1 #0: not use Lidar 3 | #1: packet message comes from online Lidar 4 | #2: packet message comes from ROS or ROS2 5 | #3: packet message comes from Pcap file 6 | send_packet_ros: false #true: Send packets through ROS or ROS2(Used to record packet) 7 | send_point_cloud_ros: true #true: Send point cloud through ROS or ROS2 8 | lidar: 9 | - driver: 10 | lidar_type: RSBP #LiDAR type - RS16, RS32, RSBP, RSHELIOS, RSHELIOS_16P, RS128, RS80, RS48, RSP128, RSP80, RSP48, 11 | # RSM1, RSM1_JUMBO, RSM2, RSM3, RSE1, RSMX. 12 | msop_port: 6999 #Msop port of lidar 13 | difop_port: 7888 #Difop port of lidar 14 | start_angle: 0 #Start angle of point cloud 15 | end_angle: 360 #End angle of point cloud 16 | wait_for_difop: true 17 | min_distance: 0.2 #Minimum distance of point cloud 18 | max_distance: 200 #Maximum distance of point cloud 19 | use_lidar_clock: false #True--Use the lidar clock as the message timestamp 20 | #False-- Use the system clock as the timestamp 21 | pcap_path: /home/robosense/lidar.pcap #The path of pcap file 22 | ros: 23 | ros_frame_id: rslidar #Frame id of packet message and point cloud message 24 | ros_recv_packet_topic: /rslidar_packets #Topic used to receive lidar packets from ROS 25 | ros_send_packet_topic: /rslidar_packets #Topic used to send lidar packets through ROS 26 | ros_send_point_cloud_topic: /rslidar_points #Topic used to send point cloud through ROS 27 | -------------------------------------------------------------------------------- /doc/howto/10_how_to_use_coordinate_transformation.md: -------------------------------------------------------------------------------- 1 | # 10 How to use coordinate transformation 2 | 3 | 4 | 5 | ## 10.1 Introduction 6 | 7 | rslidar_sdk can transform the coordinate of point cloud. This document illustrate how to do so. 8 | 9 | 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 | 11 | 12 | 13 | ## 10.2 Dependencies 14 | 15 | rslidar_sdk depends on the libeigen library to do coordinate transformation. Please install it first. 16 | 17 | ```bash 18 | sudo apt-get install libeigen3-dev 19 | ``` 20 | 21 | 22 | 23 | ## 10.3 Compile 24 | 25 | To enable transformation, set the CMake option ```ENABLE_TRANSFORM```to be ```ON```. 26 | 27 | - Compile directly 28 | 29 | ```bash 30 | cmake -DENABLE_TRANSFORM=ON .. 31 | ``` 32 | 33 | - ROS 34 | 35 | ```bash 36 | catkin_make -DENABLE_TRANSFORM=ON 37 | ``` 38 | 39 | - ROS2 40 | 41 | ```bash 42 | colcon build --cmake-args '-DENABLE_TRANSFORM=ON' 43 | ``` 44 | 45 | 46 | 47 | ## 10.4 Set LiDAR parameters 48 | 49 | In the `lidar-driver` part of `config.yaml`, set the hiding parameter`x`, `y`, `z`, `roll`, `pitch` ,`yaw`. 50 | 51 | ```yaml 52 | common: 53 | msg_source: 1 54 | send_packet_ros: false 55 | send_point_cloud_ros: true 56 | send_packet_proto: false 57 | send_point_cloud_proto: false 58 | lidar: 59 | - driver: 60 | lidar_type: RS128 61 | msop_port: 6699 62 | difop_port: 7788 63 | start_angle: 0 64 | end_angle: 360 65 | min_distance: 0.2 66 | max_distance: 200 67 | use_lidar_clock: false 68 | pcap_path: /home/robosense/lidar.pcap 69 | x: 1 70 | y: 0 71 | z: 2.5 72 | roll: 0.1 73 | pitch: 0.2 74 | yaw: 1.57 75 | ``` 76 | 77 | 78 | 79 | ## 10.5 Run 80 | 81 | Run the program. 82 | -------------------------------------------------------------------------------- /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | BasedOnStyle: Google 3 | AccessModifierOffset: -2 4 | ConstructorInitializerIndentWidth: 2 5 | AlignEscapedNewlinesLeft: false 6 | AlignTrailingComments: true 7 | AllowAllParametersOfDeclarationOnNextLine: false 8 | AllowShortIfStatementsOnASingleLine: false 9 | AllowShortLoopsOnASingleLine: false 10 | AllowShortFunctionsOnASingleLine: None 11 | AllowShortLoopsOnASingleLine: false 12 | AlwaysBreakTemplateDeclarations: true 13 | AlwaysBreakBeforeMultilineStrings: false 14 | BreakBeforeBinaryOperators: false 15 | BreakBeforeTernaryOperators: false 16 | BreakConstructorInitializersBeforeComma: true 17 | BinPackParameters: true 18 | ColumnLimit: 120 19 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 20 | DerivePointerBinding: false 21 | PointerBindsToType: true 22 | ExperimentalAutoDetectBinPacking: false 23 | IndentCaseLabels: true 24 | MaxEmptyLinesToKeep: 1 25 | NamespaceIndentation: None 26 | ObjCSpaceBeforeProtocolList: true 27 | PenaltyBreakBeforeFirstCallParameter: 19 28 | PenaltyBreakComment: 60 29 | PenaltyBreakString: 1 30 | PenaltyBreakFirstLessLess: 1000 31 | PenaltyExcessCharacter: 1000 32 | PenaltyReturnTypeOnItsOwnLine: 90 33 | SpacesBeforeTrailingComments: 2 34 | Cpp11BracedListStyle: false 35 | Standard: Auto 36 | IndentWidth: 2 37 | TabWidth: 2 38 | UseTab: Never 39 | IndentFunctionDeclarationAfterType: false 40 | SpacesInParentheses: false 41 | SpacesInAngles: false 42 | SpaceInEmptyParentheses: false 43 | SpacesInCStyleCastParentheses: false 44 | SpaceAfterControlStatementKeyword: true 45 | SpaceBeforeAssignmentOperators: true 46 | ContinuationIndentWidth: 4 47 | SortIncludes: false 48 | SpaceAfterCStyleCast: false 49 | 50 | # Configure each individual brace in BraceWrapping 51 | BreakBeforeBraces: Custom 52 | 53 | # Control of individual brace wrapping cases 54 | BraceWrapping: { 55 | AfterClass: 'true' 56 | AfterControlStatement: 'true' 57 | AfterEnum : 'true' 58 | AfterFunction : 'true' 59 | AfterNamespace : 'true' 60 | AfterStruct : 'true' 61 | AfterUnion : 'true' 62 | BeforeCatch : 'true' 63 | BeforeElse : 'true' 64 | IndentBraces : 'false' 65 | } 66 | ... 67 | -------------------------------------------------------------------------------- /src/utility/common.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************************************************* 2 | Copyright (c) 2020 RoboSense 3 | All rights reserved 4 | 5 | By downloading, copying, installing or using the software you agree to this license. If you do not agree to this 6 | license, do not download, install, copy or use the software. 7 | 8 | License Agreement 9 | For RoboSense LiDAR SDK Library 10 | (3-clause BSD License) 11 | 12 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the 13 | following conditions are met: 14 | 15 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following 16 | disclaimer. 17 | 18 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following 19 | disclaimer in the documentation and/or other materials provided with the distribution. 20 | 21 | 3. Neither the names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used 22 | to endorse or promote products derived from this software without specific prior written permission. 23 | 24 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, 25 | INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 26 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 27 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 29 | WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 30 | THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | *********************************************************************************************************************/ 32 | 33 | #pragma once 34 | 35 | #include "rs_driver/common/rs_log.hpp" 36 | -------------------------------------------------------------------------------- /doc/howto/06_how_to_decode_online_lidar.md: -------------------------------------------------------------------------------- 1 | # 6 How to decode on-line LiDAR 2 | 3 | ## 6.1 Introduction 4 | 5 | This document illustrates how to connect to an on-line LiDAR, and send point cloud to ROS. 6 | 7 | Please make sure you have read the LiDAR user-guide and [Intro to parameters](../intro/02_parameter_intro.md) before reading this document. 8 | 9 | 10 | 11 | ## 6.2 Steps 12 | 13 | ### 6.2.1 Get the LiDAR port number 14 | 15 | Please follow the instructions in LiDAR user-guide, to connect the LiDAR, and set up your computer's ip address. 16 | 17 | 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```. 18 | 19 | ### 6.2.2 Set up the configuration file 20 | 21 | #### 6.2.2.1 common part 22 | 23 | ```yaml 24 | common: 25 | msg_source: 1 26 | send_packet_ros: false 27 | send_point_cloud_ros: true 28 | send_packet_proto: false 29 | send_point_cloud_proto: false 30 | ``` 31 | 32 | The message come from the LiDAR, so set ```msg_source = 1```. 33 | 34 | Send point cloud to ROS, so set ```send_point_cloud_ros = true```. 35 | 36 | #### 6.2.2.2 lidar-driver part 37 | 38 | ```yaml 39 | lidar: 40 | - driver: 41 | lidar_type: RS128 42 | msop_port: 6699 43 | difop_port: 7788 44 | start_angle: 0 45 | end_angle: 360 46 | min_distance: 0.2 47 | max_distance: 200 48 | use_lidar_clock: false 49 | ``` 50 | 51 | Set the ```lidar_type``` to your LiDAR type. 52 | 53 | Set the ```msop_port``` and ```difop_port``` to your LiDAR's port number. 54 | 55 | #### 6.2.2.3 lidar-ros part 56 | 57 | ```yaml 58 | ros: 59 | ros_frame_id: rslidar 60 | ros_recv_packet_topic: /rslidar_packets 61 | ros_send_packet_topic: /rslidar_packets 62 | ros_send_point_cloud_topic: /rslidar_points 63 | ``` 64 | 65 | Set the ```ros_send_point_cloud_topic``` to the topic you want to send to. 66 | 67 | ### 6.2.3 Run 68 | 69 | Run the program. 70 | 71 | -------------------------------------------------------------------------------- /doc/howto/05_how_to_change_point_type_CN.md: -------------------------------------------------------------------------------- 1 | # 5 如何改变点类型的定义 2 | 3 | 4 | 5 | ## 5.1 简介 6 | 7 | 本文档介绍如何改变点类型的定义。 8 | 9 | 在项目的```CMakeLists.txt```文件中设置`POINT_TYPE`变量。修改后，需要重新编译整个工程。 10 | 11 | ```cmake 12 | #======================================= 13 | # Custom Point Type (XYZI, XYZIRT) 14 | #======================================= 15 | set(POINT_TYPE XYZI) 16 | ``` 17 | 18 | 19 | 20 | ## 5.2 XYZI 21 | 22 | `POINT_TYPE`为`XYZI`时，rslidar_sdk使用RoboSense自定义的点类型```PointXYZI```. 23 | 24 | ```c++ 25 | struct PointXYZI 26 | { 27 | float x; 28 | float y; 29 | float z; 30 | uint8_t intensity; 31 | }; 32 | ``` 33 | 34 | rslidar_sdk将基于`PointXYZI`的点云，转换为ROS的`PointCloud2`消息，再发布出去。 35 | 36 | ```c++ 37 | sensor_msgs::PointCloud2 ros_msg; 38 | addPointField(ros_msg, "x", 1, sensor_msgs::PointField::FLOAT32, offset); 39 | addPointField(ros_msg, "y", 1, sensor_msgs::PointField::FLOAT32, offset); 40 | addPointField(ros_msg, "z", 1, sensor_msgs::PointField::FLOAT32, offset); 41 | addPointField(ros_msg, "intensity", 1, sensor_msgs::PointField::FLOAT32, offset); 42 | 43 | // 44 | // copy points from point cloud of `PointXYZI` to `PointCloud2` 45 | // 46 | ... 47 | ``` 48 | 49 | 这里`PointCloud2`中的`intensity`是`float`类型，而不是`uint8_t`类型。这是因为大多数基于ROS的程序都希望`float`类型的`intensity`。 50 | 51 | 52 | 53 | ## 5.3 XYZIRT 54 | 55 | `POINT_TYPE`为`XYZIRT`时，rslidar_sdk使用RoboSense自定义的点类型```PointXYZRT```。 56 | 57 | ```c++ 58 | struct PointXYZIRT 59 | { 60 | float x; 61 | float y; 62 | float z; 63 | uint8_t intensity; 64 | uint16_t ring; 65 | double timestamp; 66 | }; 67 | ``` 68 | 69 | rslidar_sdk将基于`PointXYZIRT`的点云，转换为ROS的PointCloud2消息，再发布出去。 70 | 71 | ```c++ 72 | sensor_msgs::PointCloud2 ros_msg; 73 | addPointField(ros_msg, "x", 1, sensor_msgs::PointField::FLOAT32, offset); 74 | addPointField(ros_msg, "y", 1, sensor_msgs::PointField::FLOAT32, offset); 75 | addPointField(ros_msg, "z", 1, sensor_msgs::PointField::FLOAT32, offset); 76 | addPointField(ros_msg, "intensity", 1, sensor_msgs::PointField::FLOAT32, offset); 77 | #ifdef POINT_TYPE_XYZIRT 78 | sensor_msgs::PointCloud2Iterator iter_ring_(ros_msg, "ring"); 79 | sensor_msgs::PointCloud2Iterator iter_timestamp_(ros_msg, "timestamp"); 80 | #endif 81 | 82 | // 83 | // copy points from point cloud of `PointXYZIRT` to `PointCloud2` 84 | // 85 | ... 86 | ``` 87 | 88 | -------------------------------------------------------------------------------- /doc/howto/08_how_to_decode_pcap_file.md: -------------------------------------------------------------------------------- 1 | # 8 How to decode PCAP file 2 | 3 | 4 | 5 | ## 8.1 Introduction 6 | 7 | This document illustrates how to decode PCAP file, and send point cloud to ROS. 8 | 9 | Please make sure you have read the LiDAR user-guide and [Intro to parameters](../intro/02_parameter_intro.md) before reading this document. 10 | 11 | 12 | 13 | ## 8.2 Steps 14 | 15 | ### 8.2.1 Get the LiDAR port number 16 | 17 | 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```. 18 | 19 | ### 8.2.2 Set up the configuration file 20 | 21 | Set up the configuration file `config.yaml`. 22 | 23 | #### 8.2.2.1 common part 24 | 25 | ```yaml 26 | common: 27 | msg_source: 3 28 | send_packet_ros: false 29 | send_point_cloud_ros: true 30 | send_packet_proto: false 31 | send_point_cloud_proto: false 32 | ``` 33 | 34 | The messages come from the PCAP bag, so set ```msg_source = 3```. 35 | 36 | Send point cloud to ROS, so set ```send_point_cloud_ros = true```. 37 | 38 | #### 8.2.2.2 lidar-driver part 39 | 40 | ```yaml 41 | lidar: 42 | - driver: 43 | lidar_type: RS128 44 | msop_port: 6699 45 | difop_port: 7788 46 | start_angle: 0 47 | end_angle: 360 48 | min_distance: 0.2 49 | max_distance: 200 50 | use_lidar_clock: false 51 | pcap_path: /home/robosense/lidar.pcap 52 | ``` 53 | 54 | Set the ```pcap_path``` to the absolute path of the PCAP file. 55 | 56 | Set the ```lidar_type``` to your LiDAR type. 57 | 58 | Set the ```msop_port``` and ```difop_port``` to the port numbers of your LiDAR. 59 | 60 | #### 8.2.2.3 lidar-ros part 61 | 62 | ```yaml 63 | ros: 64 | ros_frame_id: rslidar 65 | ros_recv_packet_topic: /rslidar_packets 66 | ros_send_packet_topic: /rslidar_packets 67 | ros_send_point_cloud_topic: /rslidar_points 68 | ``` 69 | 70 | Set the ```ros_send_point_cloud_topic``` to the topic you want to send to. 71 | 72 | ### 8.2.3 Run 73 | 74 | Run the program. 75 | 76 | 77 | 78 | 79 | 80 | -------------------------------------------------------------------------------- /src/msg/rs_msg/lidar_point_cloud_msg.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************************************************* 2 | Copyright (c) 2020 RoboSense 3 | All rights reserved 4 | 5 | By downloading, copying, installing or using the software you agree to this license. If you do not agree to this 6 | license, do not download, install, copy or use the software. 7 | 8 | License Agreement 9 | For RoboSense LiDAR SDK Library 10 | (3-clause BSD License) 11 | 12 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the 13 | following conditions are met: 14 | 15 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following 16 | disclaimer. 17 | 18 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following 19 | disclaimer in the documentation and/or other materials provided with the distribution. 20 | 21 | 3. Neither the names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used 22 | to endorse or promote products derived from this software without specific prior written permission. 23 | 24 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, 25 | INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 26 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 27 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 29 | WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 30 | THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | *********************************************************************************************************************/ 32 | 33 | #pragma once 34 | 35 | #include "rs_driver/msg/point_cloud_msg.hpp" 36 | 37 | #ifdef POINT_TYPE_XYZIRT 38 | typedef PointCloudT LidarPointCloudMsg; 39 | #else 40 | typedef PointCloudT LidarPointCloudMsg; 41 | #endif 42 | 43 | -------------------------------------------------------------------------------- /src/manager/node_manager.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************************************************* 2 | Copyright (c) 2020 RoboSense 3 | All rights reserved 4 | 5 | By downloading, copying, installing or using the software you agree to this license. If you do not agree to this 6 | license, do not download, install, copy or use the software. 7 | 8 | License Agreement 9 | For RoboSense LiDAR SDK Library 10 | (3-clause BSD License) 11 | 12 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the 13 | following conditions are met: 14 | 15 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following 16 | disclaimer. 17 | 18 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following 19 | disclaimer in the documentation and/or other materials provided with the distribution. 20 | 21 | 3. Neither the names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used 22 | to endorse or promote products derived from this software without specific prior written permission. 23 | 24 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, 25 | INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 26 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 27 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 29 | WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 30 | THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | *********************************************************************************************************************/ 32 | 33 | #pragma once 34 | 35 | #include "utility/yaml_reader.hpp" 36 | #include "source/source.hpp" 37 | 38 | namespace robosense 39 | { 40 | namespace lidar 41 | { 42 | 43 | class NodeManager 44 | { 45 | public: 46 | 47 | void init(const YAML::Node& config); 48 | void start(); 49 | void stop(); 50 | 51 | ~NodeManager(); 52 | NodeManager() = default; 53 | 54 | private: 55 | 56 | std::vector sources_; 57 | }; 58 | 59 | } // namespace lidar 60 | } // namespace robosense 61 | 62 | -------------------------------------------------------------------------------- /doc/howto/05_how_to_change_point_type.md: -------------------------------------------------------------------------------- 1 | # 5 How to change point type 2 | 3 | 4 | 5 | ## 5.1 Introduction 6 | 7 | This document illustrates how to change the point type. 8 | 9 | In ```CMakeLists.txt``` of the project, change the variable `POINT_TYPE`. Remember to **rebuild** the project after changing it. 10 | 11 | ```cmake 12 | #======================================= 13 | # Custom Point Type (XYZI, XYZIRT) 14 | #======================================= 15 | set(POINT_TYPE XYZI) 16 | ``` 17 | 18 | 19 | 20 | ## 5.2 XYZI 21 | 22 | If `POINT_TYPE` is `XYZI`, rslidar_sdk uses the RoboSense defined type as below. 23 | 24 | ```c++ 25 | struct PointXYZI 26 | { 27 | float x; 28 | float y; 29 | float z; 30 | uint8_t intensity; 31 | }; 32 | ``` 33 | 34 | rslidar_sdk transforms point cloud of `PointXYZI` to ROS message of `PointCloud2`，and publish it. 35 | 36 | ```c++ 37 | sensor_msgs::PointCloud2 ros_msg; 38 | addPointField(ros_msg, "x", 1, sensor_msgs::PointField::FLOAT32, offset); 39 | addPointField(ros_msg, "y", 1, sensor_msgs::PointField::FLOAT32, offset); 40 | addPointField(ros_msg, "z", 1, sensor_msgs::PointField::FLOAT32, offset); 41 | addPointField(ros_msg, "intensity", 1, sensor_msgs::PointField::FLOAT32, offset); 42 | 43 | // 44 | // copy points from point cloud of `PointXYZI` to `PointCloud2` 45 | // 46 | ... 47 | 48 | ``` 49 | 50 | Here `intensity` of `PointCloud2` is `float` type, not `uint8_t`. This is because most ROS based applications require `intensity` of `float` type. 51 | 52 | 53 | 54 | ## 5.3 XYZIRT 55 | 56 | If `POINT_TYPE` is `XYZIRT`, rslidar_sdk uses the RoboSense defined type as below. 57 | 58 | ```c++ 59 | struct PointXYZIRT 60 | { 61 | float x; 62 | float y; 63 | float z; 64 | uint8_t intensity; 65 | uint16_t ring; 66 | double timestamp; 67 | }; 68 | ``` 69 | 70 | rslidar_sdk transforms point cloud of `PointXYZIRT` to ROS message of `PointCloud2`，and publish it. 71 | 72 | ```c++ 73 | sensor_msgs::PointCloud2 ros_msg; 74 | addPointField(ros_msg, "x", 1, sensor_msgs::PointField::FLOAT32, offset); 75 | addPointField(ros_msg, "y", 1, sensor_msgs::PointField::FLOAT32, offset); 76 | addPointField(ros_msg, "z", 1, sensor_msgs::PointField::FLOAT32, offset); 77 | addPointField(ros_msg, "intensity", 1, sensor_msgs::PointField::FLOAT32, offset); 78 | #ifdef POINT_TYPE_XYZIRT 79 | sensor_msgs::PointCloud2Iterator iter_ring_(ros_msg, "ring"); 80 | sensor_msgs::PointCloud2Iterator iter_timestamp_(ros_msg, "timestamp"); 81 | #endif 82 | 83 | // 84 | // copy points from point cloud of `PointXYZIRT` to `PointCloud2` 85 | // 86 | ... 87 | 88 | ``` 89 | 90 | -------------------------------------------------------------------------------- /doc/howto/09_pcap_file_advanced_topics.md: -------------------------------------------------------------------------------- 1 | # 9 PCAP File - Advanced Topics 2 | 3 | 4 | 5 | ## 9.1 Introduction 6 | 7 | The RoboSense LiDAR may work 8 | + in unicast/multicast/broadcast mode, 9 | + with VLAN layer 10 | + with user layers. 11 | + Also rslidar_sdk supports multi-LiDARs. 12 | 13 | This document illustrates how to configure rslidar_sdk in each case. 14 | 15 | Before reading this document, please be sure that you have read: 16 | + LiDAR user-guide 17 | + [Intro to parameters](../intro/02_parameter_intro.md) 18 | + [Online LiDAR - Advanced Topics](./07_online_lidar_advanced_topics.md) 19 | 20 | 21 | 22 | ## 9.2 General Case 23 | 24 | Generally, below code is for decoding a PCAP file in these cases. 25 | + Broadcast/multicast/unicast mode 26 | + There are multiple LiDars in a file. 27 | 28 | ```yaml 29 | common: 30 | msg_source: 3 31 | send_point_cloud_ros: true 32 | 33 | lidar: 34 | - driver: 35 | lidar_type: RS32 36 | pcap_path: /home/robosense/lidar.pcap 37 | msop_port: 6699 38 | difop_port: 7788 39 | ros: 40 | ros_frame_id: rslidar 41 | ros_send_point_cloud_topic: /rslidar_points 42 | ``` 43 | 44 | The only exception is "Multiple Lidars with same ports but different IPs", which is not supported now. 45 | 46 | 47 | 48 | ## 9.3 VLAN 49 | 50 | In some user cases, The LiDar may work on VLAN. Its packets have a VLAN layer. 51 | ![](./img/07_06_vlan_layer.png) 52 | 53 | rs_driver decodes PCAP file and gets all parts of MSOP packets, including the VLAN layer. 54 | 55 | To strip the VLAN layer, just set `use_vlan: true`. 56 | 57 | ```yaml 58 | lidar: 59 | - driver: 60 | lidar_type: RS32 61 | pcap_path: /home/robosense/lidar.pcap 62 | msop_port: 6699 63 | difop_port: 7788 64 | use_vlan: true 65 | ``` 66 | 67 | 68 | 69 | ## 9.4 User Layer, Tail Layer 70 | 71 | In some user cases, User may add extra layers before or/and after the MSOP/DIFOP packet. 72 | + USER_LAYER is before the packet and TAIL_LAYER is after it. 73 | ![](./img/07_08_user_layer.png) 74 | 75 | These extra layers are parts of UDP data. The driver can strip them. 76 | 77 | To strip them, just give their lengths in bytes. 78 | 79 | In the following example, USER_LAYER is 8 bytes, and TAIL_LAYER is 4 bytes. 80 | 81 | ```yaml 82 | lidar: 83 | - driver: 84 | lidar_type: RS32 85 | pcap_path: /home/robosense/lidar.pcap 86 | msop_port: 6699 87 | difop_port: 7788 88 | user_layer_bytes: 8 89 | tail_layer_bytes: 4 90 | ``` 91 | 92 | -------------------------------------------------------------------------------- /doc/intro/03_hiding_parameters_intro_CN.md: -------------------------------------------------------------------------------- 1 | # 3 隐藏参数介绍 2 | 3 | 为了使配置文件config.yaml尽可能简洁，我们隐藏了部分不常用的参数，在代码中使用默认值。 4 | 5 | 本文档将详细介绍这些隐藏参数。用户可根据需要，将它们加入参数文件，重新设置。 6 | 7 | 8 | 9 | ## 3.1 common 10 | 11 | ```yaml 12 | common: 13 | msg_source: 1 14 | send_packet_ros: false 15 | send_point_cloud_ros: false 16 | ``` 17 | 18 | 19 | 20 | ## 3.2 lidar 21 | 22 | ```yaml 23 | lidar: 24 | - driver: 25 | lidar_type: RS128 26 | msop_port: 6699 27 | difop_port: 7788 28 | pcap_path: /home/robosense/lidar.pcap 29 | pcap_repeat: true 30 | pcap_rate: 1 31 | start_angle: 0 32 | end_angle: 360 33 | min_distance: 0.2 34 | max_distance: 200 35 | use_lidar_clock: false 36 | config_from_file: false 37 | angle_path: /home/robosense/angle.csv 38 | dense_points: false 39 | ts_first_point: false 40 | split_frame_mode: 1 41 | split_angle: 0 42 | num_blks_split: 1 43 | wait_for_difop: true 44 | group_address: 0.0.0.0 45 | host_address: 0.0.0.0 46 | x: 0 47 | y: 0 48 | z: 0 49 | roll: 0 50 | pitch: 0 51 | yaw: 0 52 | use_vlan: false 53 | ``` 54 | 55 | - ```pcap_repeat``` -- 默认值为true， 用户可将其设置为false来禁用pcap循环播放功能。 56 | - ```pcap_rate``` -- 默认值为1，点云频率约为10hz。 用户可调节此参数来控制pcap播放速度，设置的值越大，pcap播放速度越快。 57 | - ```config_from_file``` -- 默认值为false, 是否从外参文件读入雷达配置信息，仅用于调试，可忽略。 58 | - ```angle_path``` -- angle.csv外参文件的路径，仅用于调试，可忽略。 59 | - ```ts_first_point``` -- 默认值为false。点云的时间戳是否第一个点的时间。```true```为第一个点的时间，```false```为最后一个点的时间。 60 | - ```split_frame_mode``` -- 分帧模式设置，默认值为```1```。 61 | - 1 -- 角度分帧 62 | - 2 -- 固定block数分帧 63 | - 3 -- 自定义block数分帧 64 | - ```split_angle``` -- 用于分帧的角度(单位为度)， 在```split_frame_mode = 1``` 时才生效，默认值为```0```。 65 | - ```num_blks_split``` -- 用于分帧的包数，在 ```split_frame_mode = 3```时才生效，默认值为1。 66 | - ```wait_for_difop``` -- 若设置为false， 驱动将不会等待DIFOP包（包含配置数据，尤其是角度信息），而是立即解析MSOP包并发出点云。 默认值为```true```，也就是必须要有DIFOP包才会进行点云解析。 67 | - ```group_address``` -- 如果雷达为组播模式，此参数需要被设置为组播的地址。具体使用方式可以参考[在线雷达 - 高级主题](../howto/07_online_lidar_advanced_topics_CN.md) 。 68 | - ```host_address``` -- 有两种情况需要这个选项。如果主机上通过多个IP地址接收多个雷达的数据，则可以将此参数指定为雷达的目标IP；如果设置了group_address，那也需要设置host_address，以便将这个IP地址的网卡加入组播组。 69 | - ```x, y, z, roll, pitch, yaw ``` -- 坐标变换参数，若启用了内核的坐标变换功能，将会使用此参数输出经过变换后的点云。x, y, z, 单位为```米```, roll, pitch, yaw, 单位为```弧度```。具体使用方式可以参考 [坐标变换功能](../howto/10_how_to_use_coordinate_transformation_CN.md) 。 70 | - ```use_vlan``` -- 默认为false，指定是否使用vlan。如果pcap文件中的packet带vlan层，则需要设置这个选项为true。其他情况下不需要。在线雷达的情况下，协议层到达驱动时，已经剥离vlan层，所以不需要设置这个选项。 71 | -------------------------------------------------------------------------------- /doc/howto/11_how_to_record_replay_packet_rosbag_CN.md: -------------------------------------------------------------------------------- 1 | # 11 如何录制与回放 Packet rosbag 2 | 3 | 4 | 5 | ## 11.1 简介 6 | 7 | 本文档展示如何记录与回放MSOP/DIFOP rosbag。 8 | 9 | 使用ROS可以录制点云rosbag消息并回放，但点云包非常大，所以rslidar_sdk提供更好的选择，也就是录制Packet rosbag并回放。 10 | 11 | 在阅读本文档之前, 请先阅读雷达用户手册和 [连接在线雷达并发送点云到ROS](./06_how_to_decode_online_lidar_CN.md) 。 12 | 13 | 14 | 15 | ## 11.2 录制 16 | 17 | ### 11.2.1 将MSOP/DIFOP Packet发送至ROS 18 | 19 | 这里假设已经连接在线雷达，并能发送点云到ROS。 20 | 21 | ```yaml 22 | common: 23 | msg_source: 1 24 | send_packet_ros: true 25 | send_point_cloud_ros: true 26 | send_packet_proto: false 27 | send_point_cloud_proto: false 28 | ``` 29 | 30 | 要录制Packet, 设置 ```send_packet_ros = true```。 31 | 32 | ### 11.2.2 根据话题录制rosbag 33 | 34 | 修改```ros_send_packet_topic```, 来改变发送的话题。这个话题包括MSOP Packet和DIFOP Packet。 35 | 36 | ```yaml 37 | ros: 38 | ros_frame_id: rslidar 39 | ros_recv_packet_topic: /rslidar_packets 40 | ros_send_packet_topic: /rslidar_packets 41 | ros_send_point_cloud_topic: /rslidar_points 42 | ``` 43 | 44 | ROS录制rosbag的指令如下。 45 | 46 | ```bash 47 | rosbag record /rslidar_packets -O bag 48 | ``` 49 | 50 | 51 | 52 | ## 11.3 回放 53 | 54 | 假设录制了一个rosbag，其中包含话题为 `/rslidar_packets` 的MSOP/DIFOP Packet。 55 | 56 | ### 11.3.1 设置Packet源 57 | 58 | 配置`config.yaml`的`common`部分。 59 | 60 | ```yaml 61 | common: 62 | msg_source: 2 63 | send_packet_ros: false 64 | send_point_cloud_ros: true 65 | send_packet_proto: false 66 | send_point_cloud_proto: false 67 | ``` 68 | 69 | MSOP/DIFOP Packet来自ROS rosbag，因此设置 ```msg_source = 2``` 。 70 | 71 | 将点云发送到ROS，因此设置 ```send_point_cloud_ros = true```。 72 | 73 | ### 11.3.2 设置雷达参数 74 | 75 | 配置`config.yaml`的`lidar-driver`部分。 76 | 77 | ```yaml 78 | lidar: 79 | - driver: 80 | lidar_type: RS128 81 | msop_port: 6699 82 | difop_port: 7788 83 | start_angle: 0 84 | end_angle: 360 85 | min_distance: 0.2 86 | max_distance: 200 87 | use_lidar_clock: false 88 | ``` 89 | 90 | 将 ```lidar_type``` 设置为LiDAR类型 。 91 | 92 | ### 11.3.3 设置接收Packet的主题 93 | 94 | 设置`config.yaml`的`lidar-ros`部分。 95 | 96 | ```yaml 97 | ros: 98 | ros_frame_id: rslidar 99 | ros_recv_packet_topic: /rslidar_packets 100 | ros_send_packet_topic: /rslidar_packets 101 | ros_send_point_cloud_topic: /rslidar_points 102 | ``` 103 | 104 | 将 ```ros_recv_packet_topic``` 设置为rosbag中MSOP/DIFOP Packet的话题。 105 | 106 | 107 | 108 | ### 11.3.4 运行 109 | 110 | 运行程序，回放rosbag。 111 | 112 | 113 | 114 | 115 | -------------------------------------------------------------------------------- /config/config.yaml: -------------------------------------------------------------------------------- 1 | common: 2 | msg_source: 1 #0: not use Lidar 3 | #1: packet message comes from online Lidar 4 | #2: packet message comes from ROS or ROS2 5 | #3: packet message comes from Pcap file 6 | send_packet_ros: false #true: Send packets through ROS or ROS2(Used to record packet) 7 | send_point_cloud_ros: true #true: Send point cloud through ROS or ROS2 8 | lidar: 9 | - driver: 10 | lidar_type: RSBP #LiDAR type - RS16, RS32, RSBP, RSHELIOS, RSHELIOS_16P, RS128, RS80, RS48, RSP128, RSP80, RSP48, 11 | # RSM1, RSM1_JUMBO, RSM2, RSM3, RSE1, RSMX. 12 | msop_port: 6999 #Msop port of lidar 13 | difop_port: 7888 #Difop port of lidar 14 | start_angle: 0 #Start angle of point cloud 15 | end_angle: 360 #End angle of point cloud 16 | wait_for_difop: true 17 | min_distance: 0.2 #Minimum distance of point cloud 18 | max_distance: 200 #Maximum distance of point cloud 19 | use_lidar_clock: false #True--Use the lidar clock as the message timestamp 20 | #False-- Use the system clock as the timestamp 21 | pcap_path: /home/robosense/lidar.pcap #The path of pcap file 22 | x: 0 23 | y: -0.15 24 | z: 0 25 | roll: 0 26 | pitch: -1.57 27 | yaw: 3 28 | ros: 29 | ros_frame_id: rslidar #Frame id of packet message and point cloud message 30 | ros_recv_packet_topic: /vertical_rslidar_packets #Topic used to receive lidar packets from ROS 31 | ros_send_packet_topic: /vertical_rslidar_packets #Topic used to send lidar packets through ROS 32 | ros_send_point_cloud_topic: /vertical_rslidar_points #Topic used to send point cloud through ROS 33 | - driver: 34 | lidar_type: RSBP 35 | msop_port: 6699 36 | difop_port: 7788 37 | start_angle: 0 38 | end_angle: 360 39 | min_distance: 0.2 40 | max_distance: 200 41 | use_lidar_clock: false 42 | pcap_path: /home/robosense/lidar.pcap 43 | x: 0 44 | y: 0 45 | z: -0.1 46 | roll: -0.2 47 | pitch: 0 48 | yaw: 0 49 | ros: 50 | ros_frame_id: rslidar #Frame id of packet message and point cloud message 51 | ros_recv_packet_topic: /horizontal_rslidar_packets #Topic used to receive lidar packets from ROS 52 | ros_send_packet_topic: /horizontal_slidar_packets #Topic used to send lidar packets through ROS 53 | ros_send_point_cloud_topic: /horizontal_rslidar_points #Topic used to send point cloud through ROS 54 | -------------------------------------------------------------------------------- /src/utility/yaml_reader.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************************************************* 2 | Copyright (c) 2020 RoboSense 3 | All rights reserved 4 | 5 | By downloading, copying, installing or using the software you agree to this license. If you do not agree to this 6 | license, do not download, install, copy or use the software. 7 | 8 | License Agreement 9 | For RoboSense LiDAR SDK Library 10 | (3-clause BSD License) 11 | 12 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the 13 | following conditions are met: 14 | 15 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following 16 | disclaimer. 17 | 18 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following 19 | disclaimer in the documentation and/or other materials provided with the distribution. 20 | 21 | 3. Neither the names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used 22 | to endorse or promote products derived from this software without specific prior written permission. 23 | 24 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, 25 | INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 26 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 27 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 29 | WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 30 | THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | *********************************************************************************************************************/ 32 | 33 | #pragma once 34 | 35 | #include 36 | 37 | #include "utility/common.hpp" 38 | 39 | namespace robosense 40 | { 41 | namespace lidar 42 | { 43 | 44 | template 45 | inline void yamlReadAbort(const YAML::Node& yaml, const std::string& key, T& out_val) 46 | { 47 | if (!yaml[key] || yaml[key].Type() == YAML::NodeType::Null) 48 | { 49 | RS_ERROR << " : Not set " << key; 50 | RS_ERROR << " value, Aborting!!!" << RS_REND; 51 | exit(-1); 52 | } 53 | else 54 | { 55 | out_val = yaml[key].as(); 56 | } 57 | } 58 | 59 | template 60 | inline bool yamlRead(const YAML::Node& yaml, const std::string& key, T& out_val, const T& default_val) 61 | { 62 | if (!yaml[key] || yaml[key].Type() == YAML::NodeType::Null) 63 | { 64 | out_val = default_val; 65 | return false; 66 | } 67 | else 68 | { 69 | out_val = yaml[key].as(); 70 | return true; 71 | } 72 | } 73 | 74 | inline YAML::Node yamlSubNodeAbort(const YAML::Node& yaml, const std::string& node) 75 | { 76 | YAML::Node ret = yaml[node.c_str()]; 77 | if (!ret) 78 | { 79 | RS_ERROR << " : Cannot find subnode " << node << ". Aborting!!!" << RS_REND; 80 | exit(-1); 81 | } 82 | return ret; 83 | } 84 | 85 | } // namespace lidar 86 | } // namespace robosense 87 | -------------------------------------------------------------------------------- /doc/howto/11_how_to_record_replay_packet_rosbag.md: -------------------------------------------------------------------------------- 1 | # 11 How to record and replay Packet rosbag 2 | 3 | 4 | 5 | ## 11.1 Introduction 6 | 7 | This document illustrates how to record and replay MSOP/DIFOP Packet rosbag. 8 | 9 | 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. 10 | 11 | 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). 12 | 13 | 14 | 15 | ## 11.2 Record 16 | 17 | ### 11.2.1 Send packet to ROS 18 | 19 | Here suppose that you have connected to an on-line LiDAR, and have sent the point cloud to ROS. 20 | 21 | 22 | ```yaml 23 | common: 24 | msg_source: 1 25 | send_packet_ros: true 26 | send_point_cloud_ros: true 27 | send_packet_proto: false 28 | send_point_cloud_proto: false 29 | ``` 30 | 31 | To record packets, set ```send_packet_ros = true```. 32 | 33 | ### 11.2.2 Record the topic of packet 34 | 35 | To change the topic of packet, change ```ros_send_packet_topic```. This topic sends out both MSOP and DIFOP packets. 36 | 37 | ```yaml 38 | ros: 39 | ros_frame_id: rslidar 40 | ros_recv_packet_topic: /rslidar_packets 41 | ros_send_packet_topic: /rslidar_packets 42 | ros_send_point_cloud_topic: /rslidar_points 43 | ``` 44 | 45 | Record rosbag as below. 46 | 47 | ```sh 48 | rosbag record /rslidar_packets -O bag 49 | ``` 50 | 51 | 52 | 53 | ## 11.3 Replay 54 | 55 | Suppose you have recorded a rosbag, which contains MSOP/DIFOP packets with the topic ```/rslidar_packets```. 56 | 57 | ### 11.3.1 Set Packet Source 58 | 59 | In `config.yaml`, set the `common` part. 60 | 61 | ```yaml 62 | common: 63 | msg_source: 2 64 | send_packet_ros: false 65 | send_point_cloud_ros: true 66 | send_packet_proto: false 67 | send_point_cloud_proto: false 68 | ``` 69 | 70 | Packet is from the ROS, so set ```msg_source = 2```. 71 | 72 | To send point cloud to ROS, set ```send_point_cloud_ros = true```. 73 | 74 | ### 11.3.2 Set parameters of Lidar 75 | 76 | In `config.yaml`, set the `lidar-driver` part. 77 | 78 | ```yaml 79 | lidar: 80 | - driver: 81 | lidar_type: RS128 82 | msop_port: 6699 83 | difop_port: 7788 84 | start_angle: 0 85 | end_angle: 360 86 | min_distance: 0.2 87 | max_distance: 200 88 | use_lidar_clock: false 89 | ``` 90 | 91 | Set the ```lidar_type``` to your LiDAR type. 92 | 93 | ### 11.3.3 Set Topic of packet. 94 | 95 | In `config.yaml`, set the `lidar-ros` part. 96 | 97 | ```yaml 98 | ros: 99 | ros_frame_id: rslidar 100 | ros_recv_packet_topic: /rslidar_packets 101 | ros_send_packet_topic: /rslidar_packets 102 | ros_send_point_cloud_topic: /rslidar_points 103 | ``` 104 | 105 | To receive MSOP/DIFOP packest, set ```ros_recv_packet_topic``` to the topic in the rosbag. 106 | 107 | ### 11.3.4 Run 108 | 109 | Run the demo, and replay rosbag. 110 | 111 | 112 | -------------------------------------------------------------------------------- /CHANGELOG.md: -------------------------------------------------------------------------------- 1 | # CHANGELOG 2 | 3 | ## v1.5.16 2024-08-27 4 | ### Added 5 | - Load config path frome ros2 param. 6 | ### Changed 7 | - Remove the original compilation method. 8 | ### Fixed 9 | - Use single package.xml file for both ROS1 and ROS2 @Timple. 10 | - Update msop protocol of RSMX. 11 | 12 | ## v1.5.15 2024-08-07 13 | ### Added 14 | - Support RSM3. 15 | 16 | ## v1.5.14 2024-07-15 17 | ### Added 18 | - Support multiple lidars with different multicast addresses and the same port. 19 | ### Fixed 20 | - Fixed the bug that only one lidar was parsed correctly when multiple bp4.0 were used. 21 | - Fix version number in the package.xml by @Timple. 22 | 23 | ## v1.5.13 2024-05-10 24 | ### Added 25 | - Support RSMX. 26 | ### Fixed 27 | - Update timestamp parsing unit and the number of packets per frame in decoder_RSE1. 28 | - Update firing_tss of Helios/Helios16P/RubyPlus. 29 | - Fix compilation bug of unit test. 30 | - Remove duplicate text "/rslidar_packets" by @luhuadong. 31 | 32 | ## v1.5.12 2023-12-28 33 | ### Fixed 34 | - Fix bug in getting device info and status. 35 | - Fix bug in getting device temperature. 36 | 37 | ## v1.5.11 2023-12-18 38 | 39 | ### Changed 40 | - Enable modify socket buffer size. 41 | 42 | ## v1.5.10 - 2023-02-17 43 | 44 | ### Changed 45 | - Merge RSBPV4 into RSBP 46 | 47 | 48 | ## v1.5.9 - 2023-02-17 49 | 50 | ### Changed 51 | - Increase sending DDS buffer queue to avoid packet loss 52 | 53 | 54 | ## v1.5.8 - 2022-12-09 55 | 56 | ### Added 57 | - Support ROS2/Humble Hawksbill 58 | - rename RSEOS as RSE1 59 | 60 | ### Fixed 61 | - Fix wrong widthxheight while ros_send_by_rows=true 62 | 63 | 64 | ## v1.5.7 - 2022-10-09 65 | 66 | ### Added 67 | - Seperate RSBPV4 from RSBP 68 | - Support to receive MSOP/DIFOP packet from rosbag v1.3.x 69 | - Support option ros_send_by_rows 70 | 71 | ## v1.5.6 - 2022-09-01 72 | 73 | ### Added 74 | + Add a few build options according to rs_driver 75 | + Update help documents 76 | 77 | ## v1.5.5 - 2022-08-01 78 | 79 | ### Changed 80 | - Output intensity in point cloud as float32 81 | 82 | ### Fixed 83 | - Fix compiling and runtime error on ROS2 Elequent 84 | - Fix frame_id in help docs 85 | 86 | ## v1.5.4 - 2022-07-01 87 | 88 | ### Added 89 | - Support the option to stamp the point cloud with the first point 90 | 91 | ### Changed 92 | - Remove the dependency on the protobuf library 93 | 94 | ## v1.5.3 - 2022-06-01 95 | 96 | ### Added 97 | - Support Jumbo Mode 98 | 99 | ### Fixed 100 | - Fix compiling error when protobuf is unavailable 101 | 102 | ## v1.5.0 103 | 104 | ### Changed 105 | - refactory the project 106 | 107 | ### Added 108 | - support user_layer_bytes and tail_layer_bytes 109 | - support M2 110 | - replace point with point cloud, as rs_driver's template parameter 111 | - handle point cloud in rs_driver's thread 112 | 113 | ## v1.3.0 - 2020-11-10 114 | 115 | ### Added 116 | 117 | - Add multi-cast support 118 | - Add saved_by_rows argument 119 | - Add different point types( XYZI & XYZIRT) 120 | 121 | ### Changed 122 | - Update driver core, please refer to CHANGELOG in rs_driver for details 123 | - Update some documents 124 | - Change angle_path argument to hiding parameter 125 | 126 | ### Removed 127 | 128 | - Remove RSAUTO for lidar type 129 | - Remove device_ip argument 130 | 131 | ## v1.2.1 - 2020-09-04 132 | 133 | ### Fixed 134 | - Fix bug in driver core, please refer to changelog in rs_driver for details. 135 | 136 | ## v1.2.0 - 2020-09-01 137 | 138 | ### Added 139 | - Add camera software trigger (base on target angle) 140 | 141 | ### Changed 142 | - Update driver core, please refer to changelog in rs_driver for details 143 | - Update the compiler version from C++11 to C++14 144 | 145 | ## v1.1.0 - 2020-07-01 146 | 147 | ### Added 148 | - Add ROS2 support 149 | 150 | ### Changed 151 | - Replace while loop with cv.wait 152 | - Update the vector copy part 153 | - Update the program structure 154 | 155 | ### Removed 156 | - Remove some unused variables in message struct 157 | 158 | ## v1.0.0 - 2020-06-01 159 | 160 | ### Added 161 | - New program structure 162 | - Support ROS & Protobuf-UDP functions 163 | 164 | 165 | -------------------------------------------------------------------------------- /doc/intro/03_hiding_parameters_intro.md: -------------------------------------------------------------------------------- 1 | # 3 Introduction to hidden parameters 2 | 3 | In order to make the configuration file as simple as possible, we hide some parameters and use default values for them. 4 | 5 | This document explains the meanings of these hidden parameters. 6 | 7 | 8 | 9 | ## 3.1 common 10 | 11 | ```yaml 12 | common: 13 | msg_source: 1 14 | send_packet_ros: false 15 | send_point_cloud_ros: false 16 | send_packet_proto: false 17 | send_point_cloud_proto: false 18 | ``` 19 | 20 | 21 | 22 | ## 3.2 lidar 23 | 24 | ```yaml 25 | lidar: 26 | - driver: 27 | lidar_type: RS128 28 | msop_port: 6699 29 | difop_port: 7788 30 | start_angle: 0 31 | end_angle: 360 32 | min_distance: 0.2 33 | max_distance: 200 34 | use_lidar_clock: false 35 | pcap_path: /home/robosense/lidar.pcap 36 | pcap_repeat: true 37 | pcap_rate: 1 38 | config_from_file: false 39 | angle_path: /home/robosense/angle.csv 40 | dense_points: false 41 | ts_first_point: false 42 | split_frame_mode: 1 43 | split_angle: 0 44 | num_blks_split: 1 45 | wait_for_difop: true 46 | group_address: 0.0.0.0 47 | host_address: 0.0.0.0 48 | x: 0 49 | y: 0 50 | z: 0 51 | roll: 0 52 | pitch: 0 53 | yaw: 0 54 | use_vlan: false 55 | ``` 56 | 57 | - ```pcap_repeat``` -- The default value is ```true```. Set it to ```false``` to prevent play pcap repeatedly. 58 | - ```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. 59 | - ```config_from_file``` -- Whether to read Lidar configuration from file. Only used for debug purpose, and can be ignored. 60 | - ```angle_path``` -- The path of the angle.csv. Only used for debug purpose and can be ignored. 61 | - ```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```. 62 | - ```split_frame_mode``` -- The way to split the LiDAR frames. Default value is ```1```. 63 | - 1 -- Split frame depending on the split_angle 64 | - 2 -- Split frame depending on a fixed number of blocks 65 | - 3 -- Split frame depending on num_blks_split 66 | 67 | - ```split_angle``` -- The angle(in degree) to split frames. Only be used when ```split_frame_mode = 1```. The default value is ```0```. 68 | - ```num_blks_split``` -- The number of blocks in one frame. Only be used when ```split_frame_mode = 3```. 69 | - ```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```. 70 | - ```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) 71 | - ```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. 72 | - ```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) 73 | - ```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. 74 | 75 | -------------------------------------------------------------------------------- /README_CN.md: -------------------------------------------------------------------------------- 1 | # 1 **rslidar_sdk** 2 | 3 | [English Version](README.md) 4 | 5 | 6 | 7 | ## 1.1 工程简介 8 | 9 | **rslidar_sdk** 是速腾聚创在Ubuntu环境下的雷达驱动软件包。它包括： 10 | + 雷达驱动内核[rs_driver](https://github.com/RoboSense-LiDAR/rs_driver)， 11 | + ROS拓展功能， 12 | + ROS2拓展功能 13 | 14 | 如果希望基于ROS/ROS2进行二次开发，可以使用本软件包。配合ROS/ROS2自带的可视化工具rviz，可以查看点云。 15 | 16 | 如果希望将雷达驱动集成到自己的工程，作更深一步的二次开发，请基于rs_driver进行开发。 17 | 18 | ### 1.1.1 支持的雷达型号 19 | 20 | - RS-LiDAR-16 21 | - RS-LiDAR-32 22 | - RS-Bpearl 23 | - RS-Helios 24 | - RS-Helios-16P 25 | - RS-Ruby-128 26 | - RS-Ruby-80 27 | - RS-Ruby-48 28 | - RS-Ruby-Plus-128 29 | - RS-Ruby-Plus-80 30 | - RS-Ruby-Plus-48 31 | - RS-LiDAR-M1 32 | - RS-LiDAR-M2 33 | - RS-LiDAR-M3 34 | - RS-LiDAR-E1 35 | - RS-LiDAR-MX 36 | 37 | ### 1.1.2 支持的点类型 38 | 39 | - XYZI - x, y, z, intensity 40 | - XYZIRT - x, y, z, intensity, ring, timestamp 41 | 42 | 43 | 44 | ## 1.2 下载 45 | 46 | ### 1.2.1 使用 git clone 47 | 48 | rslidar_sdk项目包含子模块驱动内核rs_driver。在执行git clone后，还需要执行相关指令，初始化并更新子模块。 49 | 50 | ```sh 51 | git clone https://github.com/RoboSense-LiDAR/rslidar_sdk.git 52 | cd rslidar_sdk 53 | git submodule init 54 | git submodule update 55 | ``` 56 | 57 | ### 1.2.2 直接下载 58 | 59 | 用户可以直接访问 [rslidar_sdk_release](https://github.com/RoboSense-LiDAR/rslidar_sdk/releases) 下载最新版本的rslidar_sdk. 60 | 61 | 请下载 **rslidar_sdk.tar.gz** 压缩包，不要下载Source code。因为Source code压缩包内不包含子模块rs_driver的代码， 用户还需自行下载rs_driver的代码放入其中才行。 62 | ![](./img/01_01_download_page.png) 63 | 64 | 65 | 66 | ## 1.3 依赖介绍 67 | 68 | ### 1.3.1 ROS 69 | 70 | 在ROS环境下使用雷达驱动，需要安装ROS相关依赖库。 71 | + Ubuntu 16.04 - ROS Kinetic desktop 72 | + Ubuntu 18.04 - ROS Melodic desktop 73 | + Ubuntu 20.04 - ROS Noetic desktop 74 | 75 | 安装方法请参考 http://wiki.ros.org。 76 | 77 | **强烈建议安装ROS desktop-full版。这个过程会自动安装一些兼容版本的依赖库，如PCL库等。这样可以避免花大量时间，去逐个安装和配置它们**。 78 | 79 | ### 1.3.2 ROS2 80 | 81 | 在ROS2环境下使用雷达驱动，需要安装ROS2相关依赖库。 82 | + Ubuntu 16.04 - 不支持 83 | + Ubuntu 18.04 - ROS2 Eloquent desktop 84 | + Ubuntu 20.04 - ROS2 Galactic desktop 85 | + Ubuntu 22.04 - ROS2 Humble desktop 86 | 87 | 安装方法请参考 https://index.ros.org/doc/ros2/Installation/Eloquent/Linux-Install-Debians/ 88 | 89 | **请不要在一台电脑上同时安装ROS和ROS2，以避免可能的版本冲突，和手工安装其他库（如Yaml）的麻烦。** 90 | 91 | ### 1.3.3 Yaml (必需) 92 | 93 | 版本号: >= v0.5.2 94 | 95 | *若已安装ROS desktop-full, 可跳过* 96 | 97 | 安装方法如下: 98 | 99 | ```sh 100 | sudo apt-get update 101 | sudo apt-get install -y libyaml-cpp-dev 102 | ``` 103 | 104 | ### 1.3.4 libpcap (必需) 105 | 106 | 版本号： >= v1.7.4 107 | 108 | 安装方法如下： 109 | 110 | ```sh 111 | sudo apt-get install -y libpcap-dev 112 | ``` 113 | 114 | 115 | 116 | ## 1.4 编译、运行 117 | 118 | ### 1.4.1 依赖于ROS-catkin编译 119 | 120 | (1) 新建一个文件夹作为工作空间，然后再新建一个名为*src*的文件夹, 将rslidar_sdk工程放入*src*文件夹内。 121 | 122 | (2) 返回工作空间目录，执行以下命令即可编译&运行(若使用.zsh,将第二句指令替换为 *source devel/setup.zsh*)。 123 | 124 | ```sh 125 | catkin_make 126 | source devel/setup.bash 127 | roslaunch rslidar_sdk start.launch 128 | ``` 129 | 130 | ### 1.4.2 依赖于ROS2-colcon编译 131 | 132 | (1) 新建一个文件夹作为工作空间，然后再新建一个名为*src*的文件夹, 将rslidar_sdk工程放入*src*文件夹内。 133 | 134 | (2) 通过[链接](https://github.com/RoboSense-LiDAR/rslidar_msg)，下载ROS2环境下的雷达packet消息定义，将rslidar_msg工程也放在刚刚新建的*src*文件夹内，与rslidar_sdk并列。 135 | 136 | (3) 返回工作空间目录，执行以下命令即可编译、运行。如果使用.zsh，将第二行替换为*source install/setup.zsh*。 137 | 138 | ```sh 139 | colcon build 140 | source install/setup.bash 141 | ros2 launch rslidar_sdk start.py 142 | ``` 143 | 144 | 不同ROS2版本start.py的格式可能不同，请使用对应版本的start.py。如ROS2 Elequent，请使用elequent_start.py。 145 | 146 | 147 | 148 | ## 1.5 参数介绍 149 | 150 | rslidar_sdk的功能通过配置参数文件来实现，请仔细阅读。 151 | 152 | [参数介绍](doc/intro/02_parameter_intro_CN.md) 153 | 154 | [隐藏参数介绍](doc/intro/03_hiding_parameters_intro_CN.md) 155 | 156 | 157 | 158 | ## 1.6 快速上手 159 | 160 | 以下是一些常用功能的使用指南。 161 | 162 | [连接在线雷达数据并发送点云到ROS](doc/howto/06_how_to_decode_online_lidar_CN.md) 163 | 164 | [解析PCAP包并发送点云到ROS](doc/howto/08_how_to_decode_pcap_file_CN.md) 165 | 166 | [切换点类型](doc/howto/05_how_to_change_point_type_CN.md) 167 | 168 | 169 | 170 | ## 1.7 使用进阶 171 | 172 | [在线雷达-高级主题](doc/howto/07_online_lidar_advanced_topics_CN.md) 173 | 174 | [PCAP文件-高级主题](doc/howto/09_pcap_file_advanced_topics_CN.md) 175 | 176 | [点云坐标变换](doc/howto/10_how_to_use_coordinate_transformation_CN.md) 177 | 178 | [录制ROS数据包然后播放它](doc/howto/11_how_to_record_replay_packet_rosbag_CN.md) 179 | 180 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # 1 **rslidar_sdk** 2 | 3 | [中文介绍](README_CN.md) 4 | 5 | 6 | 7 | ## 1 Introduction 8 | 9 | **rslidar_sdk** is the Software Development Kit of the RoboSense Lidar based on Ubuntu. It contains: 10 | 11 | + The lidar driver core [rs_driver](https://github.com/RoboSense-LiDAR/rs_driver), 12 | + The ROS support, 13 | + The ROS2 support, 14 | 15 | To get point cloud through ROS/ROS2, please just use this SDK. 16 | 17 | To integrate the Lidar driver into your own projects, please use the rs_driver. 18 | 19 | ### 1.1 LiDAR Supported 20 | 21 | - RS-LiDAR-16 22 | - RS-LiDAR-32 23 | - RS-Bpearl 24 | - RS-Helios 25 | - RS-Helios-16P 26 | - RS-Ruby-128 27 | - RS-Ruby-80 28 | - RS-Ruby-48 29 | - RS-Ruby-Plus-128 30 | - RS-Ruby-Plus-80 31 | - RS-Ruby-Plus-48 32 | - RS-LiDAR-M1 33 | - RS-LiDAR-M2 34 | - RS-LiDAR-M3 35 | - RS-LiDAR-E1 36 | - RS-LiDAR-MX 37 | 38 | ### 1.2 Point Type Supported 39 | 40 | - XYZI - x, y, z, intensity 41 | - XYZIRT - x, y, z, intensity, ring, timestamp 42 | 43 | 44 | 45 | ## 2 Download 46 | 47 | ### 2.1 Download via Git 48 | 49 | Download the rslidar_sdk as below. Since it contains the submodule rs_driver, please also use `git submodule` to download the submodule properly. 50 | 51 | 52 | ```sh 53 | git clone https://github.com/RoboSense-LiDAR/rslidar_sdk.git 54 | cd rslidar_sdk 55 | git submodule init 56 | git submodule update 57 | ``` 58 | 59 | ### 2.2 Download directly 60 | 61 | 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. 62 | 63 | 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. 64 | ![](./img/01_01_download_page.png) 65 | 66 | 67 | 68 | ## 3 Dependencies 69 | 70 | ### 3.1 ROS 71 | 72 | To run rslidar_sdk in the ROS environment, please install below libraries. 73 | + Ubuntu 16.04 - ROS Kinetic desktop 74 | + Ubuntu 18.04 - ROS Melodic desktop 75 | + Ubuntu 20.04 - ROS Noetic desktop 76 | 77 | For installation, please refer to http://wiki.ros.org. 78 | 79 | **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. 80 | 81 | This brings a lot of convenience, since you don't have to handle version conflict. 82 | 83 | 84 | **Please do not install ROS and ROS2 on the same computer, to avoid possible conflict and manually install some libraries, such as Yaml.** 85 | 86 | ### 3.2 Yaml (Essential) 87 | 88 | version: >= v0.5.2 89 | 90 | *If ros-distro-desktop-full is installed, this step can be skipped* 91 | 92 | Installation: 93 | 94 | ```sh 95 | sudo apt-get update 96 | sudo apt-get install -y libyaml-cpp-dev 97 | ``` 98 | 99 | ### 3.3 libpcap (Essential) 100 | 101 | version: >= v1.7.4 102 | 103 | Installation: 104 | 105 | ```sh 106 | sudo apt-get install -y libpcap-dev 107 | ``` 108 | 109 | 110 | 111 | ## 4 Compile & Run 112 | 113 | 114 | ### 4.1 Compile with ROS catkin tools 115 | 116 | (1) Create a new workspace folder, and create a *src* folder in it. Then put the rslidar_sdk project into the *src* folder. 117 | 118 | (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*). 119 | 120 | ```sh 121 | catkin_make -DENABLE_TRANSFORM=ON 122 | source devel/setup.bash 123 | roslaunch rslidar_sdk start.launch 124 | ``` 125 | 126 | 127 | ## 5 Introduction to parameters 128 | 129 | To change behaviors of rslidar_sdk, change its parameters. please read the following links for detail information. 130 | 131 | [Intro to parameters](doc/intro/02_parameter_intro.md) 132 | 133 | [Intro to hidden parameters](doc/intro/03_hiding_parameters_intro.md) 134 | 135 | 136 | 137 | ## 6 Quick start 138 | 139 | Below are some quick guides to use rslidar_sdk. 140 | 141 | [Connect to online LiDAR and send point cloud through ROS](doc/howto/06_how_to_decode_online_lidar.md) 142 | 143 | [Decode PCAP file and send point cloud through ROS](doc/howto/08_how_to_decode_pcap_file.md) 144 | 145 | [Change Point Type](doc/howto/05_how_to_change_point_type.md) 146 | 147 | 148 | 149 | ## 7 Advanced Topics 150 | 151 | [Online Lidar - Advanced topics](doc/howto/07_online_lidar_advanced_topics.md) 152 | 153 | [PCAP file - Advanced topics](doc/howto/09_pcap_file_advanced_topics.md) 154 | 155 | [Coordinate Transformation](doc/howto/10_how_to_use_coordinate_transformation.md) 156 | 157 | [Record rosbag & Replay it](doc/howto/11_how_to_record_replay_packet_rosbag.md) 158 | 159 | 160 | 161 | -------------------------------------------------------------------------------- /src/source/source.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************************************************* 2 | Copyright (c) 2020 RoboSense 3 | All rights reserved 4 | 5 | By downloading, copying, installing or using the software you agree to this license. If you do not agree to this 6 | license, do not download, install, copy or use the software. 7 | 8 | License Agreement 9 | For RoboSense LiDAR SDK Library 10 | (3-clause BSD License) 11 | 12 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the 13 | following conditions are met: 14 | 15 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following 16 | disclaimer. 17 | 18 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following 19 | disclaimer in the documentation and/or other materials provided with the distribution. 20 | 21 | 3. Neither the names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used 22 | to endorse or promote products derived from this software without specific prior written permission. 23 | 24 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, 25 | INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 26 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 27 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 29 | WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 30 | THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | *********************************************************************************************************************/ 32 | 33 | #pragma once 34 | 35 | #include "msg/rs_msg/lidar_point_cloud_msg.hpp" 36 | 37 | #include "utility/yaml_reader.hpp" 38 | 39 | #include 40 | 41 | namespace robosense 42 | { 43 | namespace lidar 44 | { 45 | 46 | class DestinationPointCloud 47 | { 48 | public: 49 | typedef std::shared_ptr Ptr; 50 | 51 | virtual void init(const YAML::Node& config){} 52 | virtual void start() {} 53 | virtual void stop() {} 54 | virtual void sendPointCloud(const LidarPointCloudMsg& msg) = 0; 55 | virtual ~DestinationPointCloud() = default; 56 | }; 57 | 58 | class DestinationPacket 59 | { 60 | public: 61 | typedef std::shared_ptr Ptr; 62 | 63 | virtual void init(const YAML::Node& config){} 64 | virtual void start() {} 65 | virtual void stop() {} 66 | virtual void sendPacket(const Packet& msg) = 0; 67 | virtual ~DestinationPacket() = default; 68 | }; 69 | 70 | enum SourceType 71 | { 72 | MSG_FROM_LIDAR = 1, 73 | MSG_FROM_ROS_PACKET = 2, 74 | MSG_FROM_PCAP = 3, 75 | MSG_FROM_PROTO_PACKET = 4, 76 | MSG_FROM_PROTO_POINTCLOUD = 5 77 | }; 78 | 79 | class Source 80 | { 81 | public: 82 | typedef std::shared_ptr Ptr; 83 | 84 | virtual void init(const YAML::Node& config) {} 85 | virtual void start() {} 86 | virtual void stop() {} 87 | virtual void regPointCloudCallback(DestinationPointCloud::Ptr dst); 88 | virtual void regPacketCallback(DestinationPacket::Ptr dst); 89 | virtual ~Source() = default; 90 | Source(SourceType src_type); 91 | 92 | protected: 93 | 94 | void sendPacket(const Packet& msg); 95 | void sendPointCloud(std::shared_ptr msg); 96 | 97 | SourceType src_type_; 98 | std::vector pc_cb_vec_; 99 | std::vector pkt_cb_vec_; 100 | }; 101 | 102 | inline Source::Source(SourceType src_type) 103 | : src_type_(src_type) 104 | { 105 | } 106 | 107 | inline void Source::regPacketCallback(DestinationPacket::Ptr dst) 108 | { 109 | pkt_cb_vec_.emplace_back(dst); 110 | } 111 | 112 | inline void Source::regPointCloudCallback(DestinationPointCloud::Ptr dst) 113 | { 114 | pc_cb_vec_.emplace_back(dst); 115 | } 116 | 117 | inline void Source::sendPacket(const Packet& msg) 118 | { 119 | for (auto iter : pkt_cb_vec_) 120 | { 121 | iter->sendPacket(msg); 122 | } 123 | } 124 | 125 | inline void Source::sendPointCloud(std::shared_ptr msg) 126 | { 127 | for (auto iter : pc_cb_vec_) 128 | { 129 | iter->sendPointCloud(*msg); 130 | } 131 | } 132 | 133 | 134 | } // namespace lidar 135 | } // namespace robosense 136 | -------------------------------------------------------------------------------- /doc/howto/07_online_lidar_advanced_topics_CN.md: -------------------------------------------------------------------------------- 1 | # 7 在线雷达 - 高级主题 2 | 3 | 4 | 5 | ## 7.1 简介 6 | 7 | RoboSense雷达可以工作在如下的场景。 8 | 9 | + 单播/组播/广播模式。 10 | + 运行在VLAN协议上。 11 | + 向Packet加入用户自己的层。 12 | + 接入多个雷达。 13 | 14 | 本文描述在这些场景下如何配置rslidar_sdk。 15 | 16 | 在阅读本文档之前， 请确保已经阅读过： 17 | + 雷达用户手册 18 | + [参数介绍](../intro/02_parameter_intro_CN.md) 19 | + [连接在线雷达](./06_how_to_decode_online_lidar_CN.md) 20 | 21 | 22 | 23 | ## 7.2 单播、组播、广播 24 | 25 | ### 7.2.1 广播 26 | 27 | 雷达发送 MSOP/DIFOP Packet到电脑主机。为简单起见，如下的图没有显示DIFOP端口。 28 | + 雷达发送Packet到 `255.255.255.255` : `6699`, rslidar_sdk绑定到主机的端口 `6699`. 29 | ![](./img/07_01_broadcast.png) 30 | 31 | 如下是配置`config.yaml`的方式。 32 | 33 | ```yaml 34 | common: 35 | msg_source: 1 36 | send_point_cloud_ros: true 37 | 38 | lidar: 39 | - driver: 40 | lidar_type: RS32 41 | msop_port: 6699 42 | difop_port: 7788 43 | ros: 44 | ros_frame_id: rslidar 45 | ros_send_point_cloud_topic: /rslidar_points 46 | ``` 47 | 48 | 这里列出了`common`部分和`lidar-ros`部分的设置。这两部分设置将在本文中后面的例子沿用，不再列出。 49 | 50 | ### 7.2.2 单播 51 | 52 | 为了减少网络负载，建议雷达使用单播模式。 53 | + 雷达发送Packet到 `192.168.1.102` : `6699`, rslidar_sdk绑定端口 `6699`。 54 | ![](./img/07_02_unicast.png) 55 | 56 | 如下是配置`config.yaml`的方式。这实际上与广播的方式一样。 57 | 58 | ```yaml 59 | lidar: 60 | - driver: 61 | lidar_type: RS32 62 | msop_port: 6699 63 | difop_port: 7788 64 | ``` 65 | 66 | ### 7.2.3 组播 67 | 68 | 雷达也可以工作在组播模式。 69 | + 雷达发送Packet到 `224.1.1.1`:`6699` 70 | + rslidar_sdk绑定到端口 `6699`。同时它将IP地址为`192.168.1.102`的本地网络接口加入组播组`224.1.1.1`。 71 | ![](./img/07_03_multicast.png) 72 | 73 | 如下是配置`config.yaml`的方式。 74 | 75 | ```yaml 76 | lidar: 77 | - driver: 78 | lidar_type: RS32 79 | msop_port: 6699 80 | difop_port: 7788 81 | group_address: 224.1.1.1 82 | host_address: 192.168.1.102 83 | ``` 84 | 85 | 86 | 87 | ## 7.3 多个雷达的情况 88 | 89 | ### 7.3.1 不同的目标端口 90 | 91 | 如果有两个或多个雷达，首选的配置是让它们有不同的目标端口。 92 | + 第一个雷达发送Packet到 `192.168.1.102`:`6699`, 给rslidar_sdk配置的第一个driver节点绑定到`6699`。 93 | + 第二个雷达发送Packet到 `192.168.1.102`:`5599`, 给rslidar_sdk配置的第二个driver节点绑定到`5599`。 94 | ![](./img/07_04_multi_lidars_port.png) 95 | 96 | 如下是配置`config.yaml`的方式。 97 | 98 | ```yaml 99 | lidar: 100 | - driver: 101 | lidar_type: RS32 102 | msop_port: 6699 103 | difop_port: 7788 104 | - driver: 105 | lidar_type: RS32 106 | msop_port: 5599 107 | difop_port: 6688 108 | ``` 109 | 110 | ### 7.3.2 不同的目标IP 111 | 112 | 也可以让多个雷达使用不同的目标IP。 113 | + 主机有两个网卡， IP地址分别为`192.168.1.102` 和 `192.168.1.103`。 114 | + 第一个雷达发送Packet到 `192.168.1.102`:`6699`, 给rslidar_sdk配置的第一个driver节点绑定到`192.168.1.102:6699`。 115 | + 第二个雷达发送Packet到 `192.168.1.103`:`6699`, 给rslidar_sdk配置的第二个driver节点绑定到`192.168.1.103:6699`。 116 | ![](./img/07_05_multi_lidars_ip.png) 117 | 118 | 如下是配置`config.yaml`的方式。 119 | 120 | ```yaml 121 | lidar: 122 | - driver: 123 | lidar_type: RS32 124 | msop_port: 6699 125 | difop_port: 7788 126 | host_address: 192.168.1.102 127 | - driver: 128 | lidar_type: RS32 129 | msop_port: 6699 130 | difop_port: 7788 131 | host_address: 192.168.1.103 132 | ``` 133 | 134 | 135 | 136 | ## 7.4 VLAN 137 | 138 | 在某些场景下，雷达工作在VLAN层之上。MSOP/DIFOP Packet有VLAN层，如下图。 139 | ![](./img/07_06_vlan_layer.png) 140 | 141 | rslidar_sdk不能解析VLAN层。 142 | 143 | 需要一个虚拟网卡来剥除掉这一层。举例如下。 144 | 145 | + 雷达工作在VLAN id为`80`的VLAN层上。它发送Packet到`192.168.1.102` : `6699`，Packet有VLAN层。 146 | + 假设主机上有一个支持VLAN的物理网卡`eno1`. 它接收带VLAN层的Packet。 147 | ![](./img/07_07_vlan.png) 148 | 149 | 要剥离VLAN层，需要基于`eno1`，创建一个虚拟网卡`eno1.80`, 并且将它的IP设置为`192.168.1.102`。 150 | 151 | ```shell 152 | sudo apt-get install vlan -y 153 | sudo modprobe 8021q 154 | 155 | sudo vconfig add eno1 80 156 | sudo ifconfig eno1.80 192.168.1.102 up 157 | ``` 158 | 159 | 现在，rslidar_sdk可以将`eno1.80`当做一个一般的网卡来处理，从这里接收不带VLAN层的Packet. 160 | 161 | ```yaml 162 | lidar: 163 | - driver: 164 | lidar_type: RS32 165 | msop_port: 6699 166 | difop_port: 7788 167 | ``` 168 | 169 | 170 | 171 | ## 7.5 User Layer, Tail Layer 172 | 173 | 在某些场景下，用户可能在MSOP/DIFOP数据前后加入自己的层。 174 | + 在前面的是USER_LAYER，在后面的是TAIL_LAYER。 175 | ![](./img/07_08_user_layer.png) 176 | 177 | 这两个层是UDP数据的一部分，所以rslidar_sdk可以自己剥除它们。只需要指出这两个层的长度就可以了。 178 | 179 | 在下面的例子中，USER_LAYER是8字节，TAIL_LAYER是4字节。 180 | 181 | ```yaml 182 | lidar: 183 | - driver: 184 | lidar_type: RS32 185 | msop_port: 6699 186 | difop_port: 7788 187 | user_layer_bytes: 8 188 | tail_layer_bytes: 4 189 | ``` 190 | 191 | -------------------------------------------------------------------------------- /node/rslidar_sdk_node.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************************************************* 2 | Copyright (c) 2020 RoboSense 3 | All rights reserved 4 | 5 | By downloading, copying, installing or using the software you agree to this license. If you do not agree to this 6 | license, do not download, install, copy or use the software. 7 | 8 | License Agreement 9 | For RoboSense LiDAR SDK Library 10 | (3-clause BSD License) 11 | 12 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the 13 | following conditions are met: 14 | 15 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following 16 | disclaimer. 17 | 18 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following 19 | disclaimer in the documentation and/or other materials provided with the distribution. 20 | 21 | 3. Neither the names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used 22 | to endorse or promote products derived from this software without specific prior written permission. 23 | 24 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, 25 | INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 26 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 27 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 29 | WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 30 | THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | *********************************************************************************************************************/ 32 | 33 | #include "manager/node_manager.hpp" 34 | 35 | #include 36 | #include 37 | 38 | #ifdef ROS_FOUND 39 | #include 40 | #include 41 | #elif ROS2_FOUND 42 | #include 43 | #endif 44 | 45 | using namespace robosense::lidar; 46 | 47 | #ifdef ROS2_FOUND 48 | std::mutex g_mtx; 49 | std::condition_variable g_cv; 50 | #endif 51 | 52 | static void sigHandler(int sig) 53 | { 54 | RS_MSG << "RoboSense-LiDAR-Driver is stopping....." << RS_REND; 55 | 56 | #ifdef ROS_FOUND 57 | ros::shutdown(); 58 | #elif ROS2_FOUND 59 | g_cv.notify_all(); 60 | #endif 61 | } 62 | 63 | int main(int argc, char** argv) 64 | { 65 | signal(SIGINT, sigHandler); ///< bind ctrl+c signal with the sigHandler function 66 | 67 | RS_TITLE << "********************************************************" << RS_REND; 68 | RS_TITLE << "********** **********" << RS_REND; 69 | RS_TITLE << "********** RSLidar_SDK Version: v" << RSLIDAR_VERSION_MAJOR 70 | << "." << RSLIDAR_VERSION_MINOR 71 | << "." << RSLIDAR_VERSION_PATCH << " **********" << RS_REND; 72 | RS_TITLE << "********** **********" << RS_REND; 73 | RS_TITLE << "********************************************************" << RS_REND; 74 | 75 | #ifdef ROS_FOUND 76 | ros::init(argc, argv, "rslidar_sdk_node", ros::init_options::NoSigintHandler); 77 | #elif ROS2_FOUND 78 | rclcpp::init(argc, argv); 79 | #endif 80 | 81 | std::string config_path; 82 | 83 | #ifdef RUN_IN_ROS_WORKSPACE 84 | config_path = ros::package::getPath("rslidar_sdk"); 85 | #else 86 | config_path = (std::string)PROJECT_PATH; 87 | #endif 88 | 89 | config_path += "/config/config.yaml"; 90 | 91 | #ifdef ROS_FOUND 92 | ros::NodeHandle priv_hh("~"); 93 | std::string path; 94 | priv_hh.param("config_path", path, std::string("")); 95 | #elif ROS2_FOUND 96 | std::shared_ptr nd = rclcpp::Node::make_shared("param_handle"); 97 | std::string path = nd->declare_parameter("config_path", ""); 98 | #endif 99 | 100 | #if defined(ROS_FOUND) || defined(ROS2_FOUND) 101 | if (!path.empty()) 102 | { 103 | config_path = path; 104 | } 105 | #endif 106 | 107 | YAML::Node config; 108 | try 109 | { 110 | config = YAML::LoadFile(config_path); 111 | RS_INFO << "--------------------------------------------------------" << RS_REND; 112 | RS_INFO << "Config loaded from PATH:" << RS_REND; 113 | RS_INFO << config_path << RS_REND; 114 | RS_INFO << "--------------------------------------------------------" << RS_REND; 115 | } 116 | catch (...) 117 | { 118 | RS_ERROR << "The format of config file " << config_path << " is wrong. Please check (e.g. indentation)." << RS_REND; 119 | return -1; 120 | } 121 | 122 | std::shared_ptr demo_ptr = std::make_shared(); 123 | demo_ptr->init(config); 124 | demo_ptr->start(); 125 | 126 | RS_MSG << "RoboSense-LiDAR-Driver is running....." << RS_REND; 127 | 128 | #ifdef ROS_FOUND 129 | ros::spin(); 130 | #elif ROS2_FOUND 131 | std::unique_lock lck(g_mtx); 132 | g_cv.wait(lck); 133 | #endif 134 | 135 | return 0; 136 | } 137 | -------------------------------------------------------------------------------- /doc/howto/04_how_to_work_along_with_rslidar_sdk_node_v1.3.x_CN.md: -------------------------------------------------------------------------------- 1 | # 4 如何与rslidar_sdk_node v1.3.x共存？ 2 | 3 | 4 | 5 | ## 4.1 问题描述 6 | 7 | `rslidar_sdk_node` `v1.3.x`和`v1.5.x`的配置方式不同。除了如下两个可能有交互的场景外， 两者各自运行，没有关系。 8 | 9 | + `rslidar_sdk_node`在主题`/rslidar_points`下发布点云，rosbag订阅并录制到一个cloud rosbag文件。后面rosbag又会回放这个文件，发布到`/rslidar_points`，`rslidar_sdk_node`订阅并播放它。 10 | 11 | + `rslidar_sdk_node`在主题`/rslidar_packets`下发布原始的`MSOP/DIFOP Packet`，rosbag订阅并录制到一个packet rosbag文件。后面rosbag又会回放这个文件到`/rslidar_packets`，`rslidar_sdk_node`订阅并播放它。 12 | 13 | 第一种场景下，`v1.3.x`和`v1.5.x`发布的点云格式相同，所以`v1.3.x`录制的点云，在`v1.5.x`上播放是没有问题的。 14 | 15 | 第二种场景下，`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不匹配并报错。 16 | 17 | 本文说明如何配置`rslidar_sdk` `v1.5.x`，让它在第二种场景下可以同时播放`v1.3.x`和`v1.5.x`的packet rosbag。 18 | 19 | 20 | 21 | ## 4.2 场景说明 22 | 23 | 场景说明如下。 24 | + 2个雷达，`Lidar1`是运行`v1.3.x`的雷达，`Lidar2`是运行`v1.5.x`的雷达。 25 | + 1台主机，用于分析`Lidar1`和`Lidar2`的数据。 26 | 27 | ![](./img/04_01_packet_rosbag.png) 28 | 29 | ## 4.3 步骤 30 | 31 | 32 | ### 4.3.1 配置 v1.3.x 雷达 33 | 34 | 使用`v1.3.x` `rslidar_sdk_node`录制pacekt rosbag。 35 | 36 | 按照默认的`config.yaml`的配置，消息发布到主题`/rslidar_packets`和`/rslidar_packets_difop`下。 37 | 38 | ``` 39 | common: 40 | msg_source: 1 #0: not use Lidar 41 | #1: packet message comes from online Lidar 42 | #2: packet message comes from ROS or ROS2 43 | #3: packet message comes from Pcap file 44 | #4: packet message comes from Protobuf-UDP 45 | #5: point cloud comes from Protobuf-UDP 46 | send_packet_ros: true #true: Send packets through ROS or ROS2(Used to record packet) 47 | send_point_cloud_ros: true #true: Send point cloud through ROS or ROS2 48 | lidar: 49 | - driver: 50 | lidar_type: RSM1 #LiDAR type - RS16, RS32, RSBP, RSHELIOS, RS128, RS80, RSM1 51 | msop_port: 6699 #Msop port of lidar 52 | difop_port: 7788 #Difop port of lidar 53 | ros: 54 | ros_send_packet_topic: /rslidar_packets #Topic used to send lidar packets through ROS 55 | ros_send_point_cloud_topic: /rslidar_points #Topic used to send point cloud through ROS 56 | ``` 57 | 58 | ### 4.3.2 配置 v1.5.x 雷达 59 | 60 | 使用`v1.5.6` `rslidar_sdk_node`录制packet rosbag。 61 | 62 | 为了与`v1.3.2`的消息区别，将消息输出到主题`/rslidar_packets_v2`下。 63 | 64 | ``` 65 | common: 66 | msg_source: 1 #0: not use Lidar 67 | #1: packet message comes from online Lidar 68 | #2: packet message comes from ROS or ROS2 69 | #3: packet message comes from Pcap file 70 | send_packet_ros: true #true: Send packets through ROS or ROS2(Used to record packet) 71 | send_point_cloud_ros: true #true: Send point cloud through ROS or ROS2 72 | lidar: 73 | - driver: 74 | lidar_type: RSM1 #LiDAR type - RS16, RS32, RSBP, RSHELIOS, RS128, RS80, RS48, RSM1 75 | msop_port: 6699 #Msop port of lidar 76 | difop_port: 7788 #Difop port of lidar 77 | ros: 78 | ros_send_packet_topic: /rslidar_packets_v2 #Topic used to send lidar packets through ROS 79 | ros_send_point_cloud_topic: /rslidar_points #Topic used to send point cloud through ROS 80 | ``` 81 | 82 | 83 | ### 4.3.3 配置 v1.5.x 主机 84 | 85 | + 打开CMake编译选项`ENABLE_SOURCE_PACKET_LEGACY=ON`，编译`rslidar_sdk`。 86 | 87 | ``` 88 | # CMakeLists.txt 89 | 90 | option(ENABLE_SOURCE_PACKET_LEGACY "Enable ROS Source of MSOP/DIFOP Packet v1.3.x" ON) 91 | ``` 92 | 93 | + 在`config.yaml`中，增加一个配置项`ros_recv_packet_legacy_topic`: `/rslidar_packets`。这样`rslidar_sdk_node`将同时订阅两个主题。 94 | + 订阅`/rslidar_packets`和`/rslidar_packets_difop`，读入`v1.3.x`的消息 95 | + 订阅`/rslidar_packets_v2`，读入`v1.5.x`的消息 96 | 97 | ``` 98 | common: 99 | msg_source: 1 #0: not use Lidar 100 | #1: packet message comes from online Lidar 101 | #2: packet message comes from ROS or ROS2 102 | #3: packet message comes from Pcap file 103 | send_packet_ros: false #true: Send packets through ROS or ROS2(Used to record packet) 104 | send_point_cloud_ros: true #true: Send point cloud through ROS or ROS2 105 | lidar: 106 | - driver: 107 | lidar_type: RSM1 #LiDAR type - RS16, RS32, RSBP, RSHELIOS, RS128, RS80, RS48, RSM1 108 | msop_port: 6699 #Msop port of lidar 109 | difop_port: 7788 #Difop port of lidar 110 | ros: 111 | ros_recv_packet_legacy_topic: /rslidar_packets #Topic used to receive lidar packets from ROS 112 | ros_recv_packet_topic: /rslidar_packets_v2 #Topic used to receive lidar packets from ROS 113 | ros_send_point_cloud_topic: /rslidar_points #Topic used to send point cloud through ROS 114 | ``` 115 | 116 | -------------------------------------------------------------------------------- /rviz/rviz2.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: ~ 7 | Splitter Ratio: 0.5 8 | Tree Height: 796 9 | - Class: rviz_common/Selection 10 | Name: Selection 11 | - Class: rviz_common/Tool Properties 12 | Expanded: 13 | - /2D Goal Pose1 14 | - /Publish Point1 15 | Name: Tool Properties 16 | Splitter Ratio: 0.5886790156364441 17 | - Class: rviz_common/Views 18 | Expanded: 19 | - /Current View1 20 | Name: Views 21 | Splitter Ratio: 0.5 22 | Visualization Manager: 23 | Class: "" 24 | Displays: 25 | - Alpha: 0.5 26 | Cell Size: 1 27 | Class: rviz_default_plugins/Grid 28 | Color: 160; 160; 164 29 | Enabled: true 30 | Line Style: 31 | Line Width: 0.029999999329447746 32 | Value: Lines 33 | Name: Grid 34 | Normal Cell Count: 0 35 | Offset: 36 | X: 0 37 | Y: 0 38 | Z: 0 39 | Plane: XY 40 | Plane Cell Count: 10 41 | Reference Frame: 42 | Value: true 43 | - Alpha: 1 44 | Autocompute Intensity Bounds: true 45 | Autocompute Value Bounds: 46 | Max Value: 30.831819534301758 47 | Min Value: -6.263338565826416 48 | Value: true 49 | Axis: Z 50 | Channel Name: intensity 51 | Class: rviz_default_plugins/PointCloud2 52 | Color: 255; 255; 255 53 | Color Transformer: Intensity 54 | Decay Time: 0 55 | Enabled: true 56 | Invert Rainbow: false 57 | Max Color: 255; 255; 255 58 | Max Intensity: 241 59 | Min Color: 0; 0; 0 60 | Min Intensity: 1 61 | Name: PointCloud2 62 | Position Transformer: XYZ 63 | Selectable: true 64 | Size (Pixels): 1 65 | Size (m): 0.05000000074505806 66 | Style: Points 67 | Topic: 68 | Depth: 5 69 | Durability Policy: Volatile 70 | History Policy: Keep Last 71 | Reliability Policy: Reliable 72 | Value: /rslidar_points 73 | Use Fixed Frame: true 74 | Use rainbow: true 75 | Value: true 76 | Enabled: true 77 | Global Options: 78 | Background Color: 48; 48; 48 79 | Fixed Frame: rslidar 80 | Frame Rate: 30 81 | Name: root 82 | Tools: 83 | - Class: rviz_default_plugins/Interact 84 | Hide Inactive Objects: true 85 | - Class: rviz_default_plugins/MoveCamera 86 | - Class: rviz_default_plugins/Select 87 | - Class: rviz_default_plugins/FocusCamera 88 | - Class: rviz_default_plugins/Measure 89 | Line color: 128; 128; 0 90 | - Class: rviz_default_plugins/SetInitialPose 91 | Topic: 92 | Depth: 5 93 | Durability Policy: Volatile 94 | History Policy: Keep Last 95 | Reliability Policy: Reliable 96 | Value: /initialpose 97 | - Class: rviz_default_plugins/SetGoal 98 | Topic: 99 | Depth: 5 100 | Durability Policy: Volatile 101 | History Policy: Keep Last 102 | Reliability Policy: Reliable 103 | Value: /goal_pose 104 | - Class: rviz_default_plugins/PublishPoint 105 | Single click: true 106 | Topic: 107 | Depth: 5 108 | Durability Policy: Volatile 109 | History Policy: Keep Last 110 | Reliability Policy: Reliable 111 | Value: /clicked_point 112 | Transformation: 113 | Current: 114 | Class: rviz_default_plugins/TF 115 | Value: true 116 | Views: 117 | Current: 118 | Class: rviz_default_plugins/Orbit 119 | Distance: 73.69597625732422 120 | Enable Stereo Rendering: 121 | Stereo Eye Separation: 0.05999999865889549 122 | Stereo Focal Distance: 1 123 | Swap Stereo Eyes: false 124 | Value: false 125 | Focal Point: 126 | X: 0.3094900846481323 127 | Y: 6.032918930053711 128 | Z: -1.6452505588531494 129 | Focal Shape Fixed Size: true 130 | Focal Shape Size: 0.05000000074505806 131 | Invert Z Axis: false 132 | Name: Current View 133 | Near Clip Distance: 0.009999999776482582 134 | Pitch: 0.505397617816925 135 | Target Frame: 136 | Value: Orbit (rviz) 137 | Yaw: 3.0422189235687256 138 | Saved: ~ 139 | Window Geometry: 140 | Displays: 141 | collapsed: false 142 | Height: 1025 143 | Hide Left Dock: false 144 | Hide Right Dock: false 145 | QMainWindow State: 000000ff00000000fd000000040000000000000156000003a7fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003a7000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003a7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000003a7000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004cc000003a700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 146 | Selection: 147 | collapsed: false 148 | Tool Properties: 149 | collapsed: false 150 | Views: 151 | collapsed: false 152 | Width: 1853 153 | X: 67 154 | Y: 27 155 | -------------------------------------------------------------------------------- /doc/howto/07_online_lidar_advanced_topics.md: -------------------------------------------------------------------------------- 1 | # 7 Online LiDAR - Advanced Topics 2 | 3 | 4 | 5 | ## 7.1 Introduction 6 | 7 | The RoboSense LiDAR may work 8 | 9 | + in unicast/multicast/broadcast mode, 10 | + with VLAN layer 11 | + with user layers. 12 | + Also rslidar_sdk supports multi-LiDARs. 13 | 14 | This document illustrates how to configure rslidar_sdk in each case. 15 | 16 | Before reading this document, please be sure that you have read: 17 | + LiDAR user-guide 18 | + [Intro to parameters](../intro/02_parameter_intro.md) 19 | + [Decode online LiDAR](./06_how_to_decode_online_lidar.md) 20 | 21 | 22 | 23 | ## 7.2 Unicast, Multicast and Broadcast 24 | 25 | ### 7.2.1 Broadcast mode 26 | 27 | The Lidar sends MSOP/DIFOP packets to the host machine (rslidar_sdk runs on it). For simplicity, the DIFOP port is ommited here. 28 | + The Lidar sends to `255.255.255.255` : `6699`, and the host binds to port `6699`. 29 | ![](./img/07_01_broadcast.png) 30 | 31 | Below is how to configure `config.yaml`. 32 | 33 | ```yaml 34 | common: 35 | msg_source: 1 36 | send_point_cloud_ros: true 37 | 38 | lidar: 39 | - driver: 40 | lidar_type: RS32 41 | msop_port: 6699 42 | difop_port: 7788 43 | ros: 44 | ros_frame_id: rslidar 45 | ros_send_point_cloud_topic: /rslidar_points 46 | ``` 47 | 48 | The `common` part and the `lidar-ros` part is listed here. They will be ommited in the following examples, since they are not changed. 49 | 50 | ### 7.2.2 Unicast mode 51 | 52 | To reduce the network load, the Lidar is suggested to work in unicast mode. 53 | + The Lidar sends to `192.168.1.102` : `6699`, and the host binds to port `6699`. 54 | ![](./img/07_02_unicast.png) 55 | 56 | Below is how to configure `config.yaml`. In fact, it same with the broadcast mode. 57 | 58 | ```yaml 59 | lidar: 60 | - driver: 61 | lidar_type: RS32 62 | msop_port: 6699 63 | difop_port: 7788 64 | ``` 65 | 66 | ### 7.2.3 Multicast mode 67 | 68 | The Lidar may also works in multicast mode. 69 | + The lidar sends to `224.1.1.1`:`6699` 70 | + 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`. 71 | ![](./img/07_03_multicast.png) 72 | 73 | Below is how to configure `config.yaml`. 74 | 75 | ```yaml 76 | lidar: 77 | - driver: 78 | lidar_type: RS32 79 | msop_port: 6699 80 | difop_port: 7788 81 | group_address: 224.1.1.1 82 | host_address: 192.168.1.102 83 | ``` 84 | 85 | 86 | 87 | ## 7.3 Multiple LiDARs 88 | 89 | ### 7.3.1 Different remote ports 90 | 91 | If you have two or more Lidars, it is suggested to set different remote ports. 92 | + First Lidar sends to `192.168.1.102`:`6699`, and the first driver instance binds to `6699`. 93 | + Second Lidar sends to `192.168.1.102`:`5599`, and the second driver instance binds to `5599`. 94 | ![](./img/07_04_multi_lidars_port.png) 95 | 96 | Below is how to configure `config.yaml`. 97 | 98 | ```yaml 99 | lidar: 100 | - driver: 101 | lidar_type: RS32 102 | msop_port: 6699 103 | difop_port: 7788 104 | - driver: 105 | lidar_type: RS32 106 | msop_port: 5599 107 | difop_port: 6688 108 | ``` 109 | 110 | ### 7.3.2 Different remote IPs 111 | 112 | An alternate way is to set different remote IPs. 113 | + The host has two NICs: `192.168.1.102` and `192.168.1.103`. 114 | + First Lidar sends to `192.168.1.102`:`6699`, and the first driver instance binds to `192.168.1.102:6699`. 115 | + Second Lidar sends to `192.168.1.103`:`6699`, and the second driver instance binds to `192.168.1.103:6699`. 116 | ![](./img/07_05_multi_lidars_ip.png) 117 | 118 | Below is how to configure `config.yaml`. 119 | 120 | ```yaml 121 | lidar: 122 | - driver: 123 | lidar_type: RS32 124 | msop_port: 6699 125 | difop_port: 7788 126 | host_address: 192.168.1.102 127 | - driver: 128 | lidar_type: RS32 129 | msop_port: 6699 130 | difop_port: 7788 131 | host_address: 192.168.1.103 132 | ``` 133 | 134 | 135 | 136 | ## 7.4 VLAN 137 | 138 | In some user cases, The Lidar may work on VLAN. Its packets have a VLAN layer. 139 | ![](./img/07_06_vlan_layer.png) 140 | 141 | The driver cannot parse this packet. Instead, it depends on a virtual NIC to strip the VLAN layer. 142 | 143 | Below is an example. 144 | + The Lidar works on VLAN `80`. It sends packets to `192.168.1.102` : `6699`. The packet has a VLAN layer. 145 | + Suppose there is a physical NIC `eno1` on the host. It receives packets with VLAN layer. 146 | ![](./img/07_07_vlan.png) 147 | 148 | To strip the VLAN layer, create a virtual NIC `eno1.80` on `eno1`, and assign IP `192.168.1.102` to it. 149 | 150 | ``` 151 | sudo apt-get install vlan -y 152 | sudo modprobe 8021q 153 | 154 | sudo vconfig add eno1 80 155 | sudo ifconfig eno1.80 192.168.1.102 up 156 | ``` 157 | 158 | Now the driver may take `eno1.80` as a general NIC, and receives packets without VLAN layer. 159 | 160 | ```yaml 161 | lidar: 162 | - driver: 163 | lidar_type: RS32 164 | msop_port: 6699 165 | difop_port: 7788 166 | ``` 167 | 168 | 169 | 170 | ## 7.5 User Layer, Tail Layer 171 | 172 | In some user cases, User may add extra layers before or after the MSOP/DIFOP packet. 173 | + USER_LAYER is before the packet and TAIL_LAYER is after it. 174 | ![](./img/07_08_user_layer.png) 175 | 176 | These extra layers are parts of UDP data. The driver can strip them. 177 | 178 | To strip them, just give their lengths in bytes. 179 | 180 | In the following example, USER_LAYER is 8 bytes, and TAIL_LAYER is 4 bytes. 181 | 182 | ```yaml 183 | lidar: 184 | - driver: 185 | lidar_type: RS32 186 | msop_port: 6699 187 | difop_port: 7788 188 | user_layer_bytes: 8 189 | tail_layer_bytes: 4 190 | ``` 191 | 192 | -------------------------------------------------------------------------------- /doc/intro/02_parameter_intro_CN.md: -------------------------------------------------------------------------------- 1 | # 2 参数介绍 2 | 3 | rslidar_sdk读取配置文件 ```config.yaml```，得到所有的参数。```config.yaml```在```rslidar_sdk/config```文件夹中。 4 | 5 | **config.yaml遵循YAML格式。该格式对缩进有严格要求。修改config.yaml之后，请确保每行开头的缩进仍保持一致！** 6 | 7 | config.yaml包括两部分：common部分 和 lidar部分。 8 | 9 | rslidar_sdk支持多个雷达。common部分为所有雷达共享。lidar部分，每一个子节点对应一个雷达，针对这个雷达的实际情况分别设置。 10 | 11 | 12 | 13 | ## 2.1 common部分 14 | 15 | common部分设置雷达消息的源（Packet或点云从哪来）和目标（Packet或点云发布到哪去）。 16 | 17 | ```yaml 18 | common: 19 | msg_source: 1 20 | send_packet_ros: false 21 | send_point_cloud_ros: false 22 | ``` 23 | 24 | - msg_source 25 | 26 | - 1 -- 连接在线雷达。更多使用细节，请参考[连接在线雷达并发送点云到ROS](../howto/06_how_to_decode_online_lidar_CN.md)。 27 | 28 | - 2 -- 离线解析ROS/ROS2的Packet包。更多使用细节，请参考 [录制ROS数据包然后播放它](../howto/11_how_to_record_replay_packet_rosbag_CN.md)。 29 | 30 | - 3 -- 离线解析PCAP包。更多使用细节，请参考[离线解析PCAP包并发送点云到ROS](../howto/08_how_to_decode_pcap_file_CN.md)。 31 | 32 | - send_packet_ros 33 | 34 | - true -- 雷达Packet消息将通过ROS/ROS2发出 35 | 36 | *雷达ROS packet消息为速腾聚创自定义ROS消息，用户使用ROS/ROS2 echo命令不能查看消息的具体内容。这个功能用于录制ROS/ROS2的Packet包，更多使用细节，请参考msg_source=2的情况。 37 | 38 | - send_point_cloud_ros 39 | 40 | - true -- 雷达点云消息将通过ROS/ROS2发出 41 | 42 | *点云消息的类型为ROS官方定义的点云类型sensor_msgs/PointCloud2, 用户可以使用Rviz直接查看点云。用户可以录制ROS/ROS2的点云包，但点云包的体积非常大，所以不建议这么做。更好的方式是录制Packet包，请参考send_packet_ros=true的情况。* 43 | 44 | 45 | 46 | 47 | 48 | ## 2.2 lidar部分 49 | 50 | lidar部分根据每个雷达的实际情况进行设置。 51 | 52 | ```yaml 53 | lidar: 54 | - driver: 55 | lidar_type: RS128 56 | msop_port: 6699 57 | difop_port: 7788 58 | start_angle: 0 59 | end_angle: 360 60 | min_distance: 0.2 61 | max_distance: 200 62 | use_lidar_clock: false 63 | dense_points: false 64 | pcap_path: /home/robosense/lidar.pcap 65 | ros: 66 | ros_send_by_rows: false 67 | ros_frame_id: rslidar 68 | ros_recv_packet_topic: /rslidar_packets 69 | ros_send_packet_topic: /rslidar_packets 70 | ros_send_point_cloud_topic: /rslidar_points 71 | ``` 72 | 73 | - lidar_type 74 | 75 | 支持的雷达型号在rslidar_sdk的README文件中列出。 76 | 77 | - msop_port, difop_port 78 | 79 | 接收MSOP/DIFOP Packet的msop端口号和difop端口号。 *若收不到消息，请优先确认这两个参数是否配置正确。* 80 | 81 | - start_angle, end_angle 82 | 83 | 点云消息的起始角度和结束角度。这个设置是软件屏蔽，将区域外的点设置为NAN点，不会减小每帧点云的体积。 start_angle和end_angle的范围是0~360°，**起始角可以大于结束角**. 84 | 85 | - min_distance, max_distance 86 | 87 | 点云的最小距离和最大距离。这个设置是软件屏蔽，会将区域外的点设置为NAN点，不会减小每帧点云的体积。 88 | 89 | - use_lidar_clock 90 | 91 | - true -- 使用雷达时间作为消息时间戳。 92 | - false -- 使用电脑主机时间作为消息时间戳。 93 | 94 | - dense_points 95 | 96 | 输出的点云中是否剔除NAN points。默认值为false。 97 | - true 为剔除， 98 | - false为不剔除。 99 | 100 | - pcap_path 101 | 102 | pcap包的路径。当 msg_source=3 时有效。 103 | 104 | - ros_send_by_rows 105 | 106 | 只对机械式雷达有意义，且只有当dense_points = false时才有效。 107 | - true -- 发送点云时，按照一行一行的顺序排列点 108 | - false -- 发送点云时，按照一列一列的顺序排列点 109 | 110 | 111 | 112 | ## 2.3 示例 113 | 114 | ### 2.3.1 单台雷达 115 | 116 | 在线连接1台RS128雷达，并发送点云到ROS。 117 | 118 | ```yaml 119 | common: 120 | msg_source: 1 121 | send_packet_ros: false 122 | send_point_cloud_ros: true 123 | lidar: 124 | - driver: 125 | lidar_type: RS128 126 | msop_port: 6699 127 | difop_port: 7788 128 | start_angle: 0 129 | end_angle: 360 130 | min_distance: 0.2 131 | max_distance: 200 132 | use_lidar_clock: false 133 | ros: 134 | ros_frame_id: rslidar 135 | ros_recv_packet_topic: /rslidar_packets 136 | ros_send_packet_topic: /rslidar_packets 137 | ros_send_point_cloud_topic: /rslidar_points 138 | ``` 139 | 140 | ### 2.3.2 单台雷达 141 | 142 | 在线连接1台RS128雷达和2台RSBP雷达，发送点云到ROS。 143 | 144 | *注意lidar部分参数的缩进* 145 | 146 | ```yaml 147 | common: 148 | msg_source: 1 149 | send_packet_ros: false 150 | send_point_cloud_ros: true 151 | lidar: 152 | - driver: 153 | lidar_type: RS128 154 | msop_port: 6699 155 | difop_port: 7788 156 | start_angle: 0 157 | end_angle: 360 158 | min_distance: 0.2 159 | max_distance: 200 160 | use_lidar_clock: false 161 | ros: 162 | ros_frame_id: rslidar 163 | ros_recv_packet_topic: /middle/rslidar_packets 164 | ros_send_packet_topic: /middle/rslidar_packets 165 | ros_send_point_cloud_topic: /middle/rslidar_points 166 | - driver: 167 | lidar_type: RSBP 168 | msop_port: 1990 169 | difop_port: 1991 170 | start_angle: 0 171 | end_angle: 360 172 | min_distance: 0.2 173 | max_distance: 200 174 | use_lidar_clock: false 175 | ros: 176 | ros_frame_id: rslidar 177 | ros_recv_packet_topic: /left/rslidar_packets 178 | ros_send_packet_topic: /left/rslidar_packets 179 | ros_send_point_cloud_topic: /left/rslidar_points 180 | - driver: 181 | lidar_type: RSBP 182 | msop_port: 2010 183 | difop_port: 2011 184 | start_angle: 0 185 | end_angle: 360 186 | min_distance: 0.2 187 | max_distance: 200 188 | use_lidar_clock: false 189 | ros: 190 | ros_frame_id: rslidar 191 | ros_recv_packet_topic: /right/rslidar_packets 192 | ros_send_packet_topic: /right/rslidar_packets 193 | ros_send_point_cloud_topic: /right/rslidar_points 194 | ``` 195 | 196 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | cmake_policy(SET CMP0048 NEW) 3 | project(rslidar_sdk) 4 | 5 | #======================================= 6 | # Custom Point Type (XYZI, XYZIRT) 7 | #======================================= 8 | set(POINT_TYPE XYZI) 9 | 10 | option(ENABLE_TRANSFORM "Enable transform functions" OFF) 11 | if(${ENABLE_TRANSFORM}) 12 | add_definitions("-DENABLE_TRANSFORM") 13 | 14 | find_package(Eigen3 REQUIRED) 15 | include_directories(${EIGEN3_INCLUDE_DIR}) 16 | endif(${ENABLE_TRANSFORM}) 17 | 18 | option(ENABLE_EPOLL_RECEIVE "Receive packets with epoll() instead of select()" OFF) 19 | if(${ENABLE_EPOLL_RECEIVE}) 20 | add_definitions("-DENABLE_EPOLL_RECEIVE") 21 | endif(${ENABLE_EPOLL_RECEIVE}) 22 | 23 | option(ENABLE_MODIFY_RECVBUF "Enable modify size of RECVBUF" ON) 24 | if(${ENABLE_MODIFY_RECVBUF}) 25 | add_definitions("-DENABLE_MODIFY_RECVBUF") 26 | endif(${ENABLE_MODIFY_RECVBUF}) 27 | 28 | option(ENABLE_WAIT_IF_QUEUE_EMPTY "Enable waiting for a while in handle thread if the queue is empty" OFF) 29 | if(${ENABLE_WAIT_IF_QUEUE_EMPTY}) 30 | add_definitions("-DENABLE_WAIT_IF_QUEUE_EMPTY") 31 | endif(${ENABLE_WAIT_IF_QUEUE_EMPTY}) 32 | 33 | option(ENABLE_DIFOP_PARSE "Enable parsing DIFOP Packet" ON) 34 | if(${ENABLE_DIFOP_PARSE}) 35 | add_definitions("-DENABLE_DIFOP_PARSE") 36 | endif(${ENABLE_DIFOP_PARSE}) 37 | 38 | option(ENABLE_STAMP_WITH_LOCAL "Enable stamp point cloud with local time" OFF) 39 | if(${ENABLE_STAMP_WITH_LOCAL}) 40 | add_definitions("-DENABLE_STAMP_WITH_LOCAL") 41 | endif(${ENABLE_STAMP_WITH_LOCAL}) 42 | 43 | 44 | option(ENABLE_SOURCE_PACKET_LEGACY "Enable ROS Source of MSOP/DIFOP Packet v1.3.x" OFF) 45 | if(${ENABLE_SOURCE_PACKET_LEGACY}) 46 | add_definitions("-DENABLE_SOURCE_PACKET_LEGACY") 47 | endif(${ENABLE_SOURCE_PACKET_LEGACY}) 48 | 49 | #======================== 50 | # Project details / setup 51 | #======================== 52 | set(PROJECT_NAME rslidar_sdk) 53 | 54 | add_definitions(-DPROJECT_PATH="${PROJECT_SOURCE_DIR}") 55 | 56 | if (CMAKE_BUILD_TYPE STREQUAL "") 57 | set(CMAKE_BUILD_TYPE Release) 58 | add_definitions(-O3) 59 | endif() 60 | 61 | add_definitions(-std=c++17) 62 | add_compile_options(-Wall) 63 | 64 | #======================== 65 | # Point Type Definition 66 | #======================== 67 | if(${POINT_TYPE} STREQUAL "XYZI") 68 | add_definitions(-DPOINT_TYPE_XYZI) 69 | elseif(${POINT_TYPE} STREQUAL "XYZIRT") 70 | add_definitions(-DPOINT_TYPE_XYZIRT) 71 | endif() 72 | 73 | message(=============================================================) 74 | message("-- POINT_TYPE is ${POINT_TYPE}") 75 | message(=============================================================) 76 | 77 | #======================== 78 | # Dependencies Setup 79 | #======================== 80 | 81 | #ROS# 82 | find_package(roscpp 1.12 QUIET) 83 | 84 | if(roscpp_FOUND) 85 | 86 | message(=============================================================) 87 | message("-- ROS Found. ROS Support is turned On.") 88 | message(=============================================================) 89 | 90 | add_definitions(-DROS_FOUND) 91 | 92 | find_package(roslib QUIET) 93 | include_directories(${roscpp_INCLUDE_DIRS} ${roslib_INCLUDE_DIRS}) 94 | set(ROS_LIBS ${roscpp_LIBRARIES} ${roslib_LIBRARIES}) 95 | 96 | add_definitions(-DRUN_IN_ROS_WORKSPACE) 97 | 98 | find_package(catkin REQUIRED COMPONENTS 99 | roscpp 100 | sensor_msgs 101 | roslib) 102 | 103 | catkin_package(CATKIN_DEPENDS 104 | sensor_msgs 105 | roslib) 106 | 107 | else(roscpp_FOUND) 108 | 109 | message(=============================================================) 110 | message("-- ROS Not Found. ROS Support is turned Off.") 111 | message(=============================================================) 112 | 113 | endif(roscpp_FOUND) 114 | 115 | #ROS2# 116 | find_package(rclcpp QUIET) 117 | 118 | if(rclcpp_FOUND) 119 | message(=============================================================) 120 | message("-- ROS2 Found. ROS2 Support is turned On.") 121 | message(=============================================================) 122 | 123 | add_definitions(-DROS2_FOUND) 124 | include_directories(${rclcpp_INCLUDE_DIRS}) 125 | set(CMAKE_CXX_STANDARD 14) 126 | 127 | find_package(ament_cmake REQUIRED) 128 | find_package(sensor_msgs REQUIRED) 129 | find_package(rslidar_msg REQUIRED) 130 | find_package(std_msgs REQUIRED) 131 | 132 | else(rclcpp_FOUND) 133 | message(=============================================================) 134 | message("-- ROS2 Not Found. ROS2 Support is turned Off.") 135 | message(=============================================================) 136 | endif(rclcpp_FOUND) 137 | 138 | #Others# 139 | find_package(yaml-cpp REQUIRED) 140 | 141 | #Include directory# 142 | include_directories(${PROJECT_SOURCE_DIR}/src) 143 | 144 | #Driver core# 145 | add_subdirectory(src/rs_driver) 146 | find_package(rs_driver REQUIRED) 147 | include_directories(${rs_driver_INCLUDE_DIRS}) 148 | 149 | #======================== 150 | # Build Setup 151 | #======================== 152 | 153 | add_executable(rslidar_sdk_node 154 | node/rslidar_sdk_node.cpp 155 | src/manager/node_manager.cpp) 156 | 157 | target_link_libraries(rslidar_sdk_node 158 | ${YAML_CPP_LIBRARIES} 159 | ${rs_driver_LIBRARIES}) 160 | 161 | #Ros# 162 | if(roscpp_FOUND) 163 | 164 | target_link_libraries(rslidar_sdk_node 165 | ${ROS_LIBS}) 166 | 167 | install(TARGETS rslidar_sdk_node 168 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 169 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 170 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) 171 | 172 | endif(roscpp_FOUND) 173 | 174 | #Ros2# 175 | if(rclcpp_FOUND) 176 | 177 | ament_target_dependencies(rslidar_sdk_node 178 | rclcpp 179 | std_msgs 180 | sensor_msgs 181 | rslidar_msg) 182 | 183 | install(TARGETS 184 | rslidar_sdk_node 185 | DESTINATION lib/${PROJECT_NAME}) 186 | 187 | install(DIRECTORY 188 | launch 189 | rviz 190 | DESTINATION share/${PROJECT_NAME}) 191 | 192 | ament_package() 193 | 194 | endif(rclcpp_FOUND) 195 | 196 | -------------------------------------------------------------------------------- /src/msg/ros_msg/rslidar_packet_legacy.hpp: -------------------------------------------------------------------------------- 1 | // Generated by gencpp from file rslidar_msgs/rslidarPacket.msg 2 | // DO NOT EDIT! 3 | 4 | #pragma once 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | namespace rslidar_msgs 16 | { 17 | template 18 | struct rslidarPacket_ 19 | { 20 | typedef rslidarPacket_ Type; 21 | 22 | rslidarPacket_() : stamp(), data() 23 | { 24 | data.assign(0); 25 | } 26 | rslidarPacket_(const ContainerAllocator& _alloc) : stamp(), data() 27 | { 28 | (void)_alloc; 29 | data.assign(0); 30 | } 31 | 32 | typedef ros::Time _stamp_type; 33 | _stamp_type stamp; 34 | 35 | typedef boost::array _data_type; 36 | _data_type data; 37 | 38 | typedef boost::shared_ptr< ::rslidar_msgs::rslidarPacket_ > Ptr; 39 | typedef boost::shared_ptr< ::rslidar_msgs::rslidarPacket_ const> ConstPtr; 40 | 41 | }; // struct rslidarPacket_ 42 | 43 | typedef ::rslidar_msgs::rslidarPacket_ > rslidarPacket; 44 | 45 | typedef boost::shared_ptr< ::rslidar_msgs::rslidarPacket> rslidarPacketPtr; 46 | typedef boost::shared_ptr< ::rslidar_msgs::rslidarPacket const> rslidarPacketConstPtr; 47 | 48 | // constants requiring out of line definition 49 | 50 | template 51 | std::ostream& operator<<(std::ostream& s, const ::rslidar_msgs::rslidarPacket_& v) 52 | { 53 | ros::message_operations::Printer< ::rslidar_msgs::rslidarPacket_ >::stream(s, "", v); 54 | return s; 55 | } 56 | 57 | } // namespace rslidar_msgs 58 | 59 | namespace ros 60 | { 61 | namespace message_traits 62 | { 63 | // BOOLTRAITS {'IsFixedSize': true, 'IsMessage': true, 'HasHeader': false} 64 | // {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'rslidar_msgs': 65 | // ['/home/xzd/catkin_ws/src/rs_driver/rslidar_msgs/msg']} 66 | 67 | // !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', 68 | // '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', 69 | // '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 70 | // 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] 71 | 72 | template 73 | struct IsFixedSize< ::rslidar_msgs::rslidarPacket_ > : TrueType 74 | { 75 | }; 76 | 77 | template 78 | struct IsFixedSize< ::rslidar_msgs::rslidarPacket_ const> : TrueType 79 | { 80 | }; 81 | 82 | template 83 | struct IsMessage< ::rslidar_msgs::rslidarPacket_ > : TrueType 84 | { 85 | }; 86 | 87 | template 88 | struct IsMessage< ::rslidar_msgs::rslidarPacket_ const> : TrueType 89 | { 90 | }; 91 | 92 | template 93 | struct HasHeader< ::rslidar_msgs::rslidarPacket_ > : FalseType 94 | { 95 | }; 96 | 97 | template 98 | struct HasHeader< ::rslidar_msgs::rslidarPacket_ const> : FalseType 99 | { 100 | }; 101 | 102 | template 103 | struct MD5Sum< ::rslidar_msgs::rslidarPacket_ > 104 | { 105 | static const char* value() 106 | { 107 | return "1e4288e00b9222ea477b73350bf24f51"; 108 | } 109 | 110 | static const char* value(const ::rslidar_msgs::rslidarPacket_&) 111 | { 112 | return value(); 113 | } 114 | static const uint64_t static_value1 = 0x1e4288e00b9222eaULL; 115 | static const uint64_t static_value2 = 0x477b73350bf24f51ULL; 116 | }; 117 | 118 | template 119 | struct DataType< ::rslidar_msgs::rslidarPacket_ > 120 | { 121 | static const char* value() 122 | { 123 | return "rslidar_msgs/rslidarPacket"; 124 | } 125 | 126 | static const char* value(const ::rslidar_msgs::rslidarPacket_&) 127 | { 128 | return value(); 129 | } 130 | }; 131 | 132 | template 133 | struct Definition< ::rslidar_msgs::rslidarPacket_ > 134 | { 135 | static const char* value() 136 | { 137 | return "# Raw LIDAR packet.\n\ 138 | \n\ 139 | timestamp # packet timestamp\n\ 140 | uint8[1248] data # packet contents\n\ 141 | \n\ 142 | "; 143 | } 144 | 145 | static const char* value(const ::rslidar_msgs::rslidarPacket_&) 146 | { 147 | return value(); 148 | } 149 | }; 150 | 151 | } // namespace message_traits 152 | } // namespace ros 153 | 154 | namespace ros 155 | { 156 | namespace serialization 157 | { 158 | template 159 | struct Serializer< ::rslidar_msgs::rslidarPacket_ > 160 | { 161 | template 162 | inline static void allInOne(Stream& stream, T m) 163 | { 164 | stream.next(m.stamp); 165 | stream.next(m.data); 166 | } 167 | 168 | ROS_DECLARE_ALLINONE_SERIALIZER 169 | }; // struct rslidarPacket_ 170 | 171 | } // namespace serialization 172 | } // namespace ros 173 | 174 | namespace ros 175 | { 176 | namespace message_operations 177 | { 178 | template 179 | struct Printer< ::rslidar_msgs::rslidarPacket_ > 180 | { 181 | template 182 | static void stream(Stream& s, const std::string& indent, const ::rslidar_msgs::rslidarPacket_& v) 183 | { 184 | s << indent << "stamp: "; 185 | Printer::stream(s, indent + " ", v.stamp); 186 | s << indent << "data[]" << std::endl; 187 | for (size_t i = 0; i < v.data.size(); ++i) 188 | { 189 | s << indent << " data[" << i << "]: "; 190 | Printer::stream(s, indent + " ", v.data[i]); 191 | } 192 | } 193 | }; 194 | 195 | } // namespace message_operations 196 | } // namespace ros 197 | -------------------------------------------------------------------------------- /src/manager/node_manager.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************************************************* 2 | Copyright (c) 2020 RoboSense 3 | All rights reserved 4 | 5 | By downloading, copying, installing or using the software you agree to this license. If you do not agree to this 6 | license, do not download, install, copy or use the software. 7 | 8 | License Agreement 9 | For RoboSense LiDAR SDK Library 10 | (3-clause BSD License) 11 | 12 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the 13 | following conditions are met: 14 | 15 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following 16 | disclaimer. 17 | 18 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following 19 | disclaimer in the documentation and/or other materials provided with the distribution. 20 | 21 | 3. Neither the names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used 22 | to endorse or promote products derived from this software without specific prior written permission. 23 | 24 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, 25 | INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 26 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 27 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 29 | WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 30 | THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | *********************************************************************************************************************/ 32 | 33 | #include "manager/node_manager.hpp" 34 | #include "source/source_driver.hpp" 35 | #include "source/source_pointcloud_ros.hpp" 36 | #include "source/source_packet_ros.hpp" 37 | 38 | namespace robosense 39 | { 40 | namespace lidar 41 | { 42 | 43 | void NodeManager::init(const YAML::Node& config) 44 | { 45 | YAML::Node common_config = yamlSubNodeAbort(config, "common"); 46 | 47 | int msg_source = 0; 48 | yamlRead(common_config, "msg_source", msg_source, 0); 49 | 50 | bool send_packet_ros; 51 | yamlRead(common_config, "send_packet_ros", send_packet_ros, false); 52 | 53 | bool send_point_cloud_ros; 54 | yamlRead(common_config, "send_point_cloud_ros", send_point_cloud_ros, false); 55 | 56 | bool send_point_cloud_proto; 57 | yamlRead(common_config, "send_point_cloud_proto", send_point_cloud_proto, false); 58 | 59 | bool send_packet_proto; 60 | yamlRead(common_config, "send_packet_proto", send_packet_proto, false); 61 | 62 | YAML::Node lidar_config = yamlSubNodeAbort(config, "lidar"); 63 | 64 | for (uint8_t i = 0; i < lidar_config.size(); ++i) 65 | { 66 | std::shared_ptr source; 67 | 68 | switch (msg_source) 69 | { 70 | case SourceType::MSG_FROM_LIDAR: // online lidar 71 | 72 | RS_INFO << "------------------------------------------------------" << RS_REND; 73 | RS_INFO << "Receive Packets From : Online LiDAR" << RS_REND; 74 | RS_INFO << "Msop Port: " << lidar_config[i]["driver"]["msop_port"].as() << RS_REND; 75 | RS_INFO << "Difop Port: " << lidar_config[i]["driver"]["difop_port"].as() << RS_REND; 76 | RS_INFO << "------------------------------------------------------" << RS_REND; 77 | 78 | source = std::make_shared(SourceType::MSG_FROM_LIDAR); 79 | source->init(lidar_config[i]); 80 | break; 81 | 82 | case SourceType::MSG_FROM_ROS_PACKET: // pkt from ros 83 | 84 | RS_INFO << "------------------------------------------------------" << RS_REND; 85 | RS_INFO << "Receive Packets From : ROS" << RS_REND; 86 | RS_INFO << "Msop Topic: " << lidar_config[i]["ros"]["ros_recv_packet_topic"].as() << RS_REND; 87 | RS_INFO << "------------------------------------------------------" << RS_REND; 88 | 89 | source = std::make_shared(); 90 | source->init(lidar_config[i]); 91 | break; 92 | 93 | case SourceType::MSG_FROM_PCAP: // pcap 94 | 95 | RS_INFO << "------------------------------------------------------" << RS_REND; 96 | RS_INFO << "Receive Packets From : Pcap" << RS_REND; 97 | RS_INFO << "Msop Port: " << lidar_config[i]["driver"]["msop_port"].as() << RS_REND; 98 | RS_INFO << "Difop Port: " << lidar_config[i]["driver"]["difop_port"].as() << RS_REND; 99 | RS_INFO << "------------------------------------------------------" << RS_REND; 100 | 101 | source = std::make_shared(SourceType::MSG_FROM_PCAP); 102 | source->init(lidar_config[i]); 103 | break; 104 | 105 | default: 106 | RS_ERROR << "Unsupported LiDAR message source:" << msg_source << "." << RS_REND; 107 | exit(-1); 108 | } 109 | 110 | if (send_packet_ros) 111 | { 112 | RS_DEBUG << "------------------------------------------------------" << RS_REND; 113 | RS_DEBUG << "Send Packets To : ROS" << RS_REND; 114 | RS_DEBUG << "Msop Topic: " << lidar_config[i]["ros"]["ros_send_packet_topic"].as() << RS_REND; 115 | RS_DEBUG << "------------------------------------------------------" << RS_REND; 116 | 117 | std::shared_ptr dst = std::make_shared(); 118 | dst->init(lidar_config[i]); 119 | source->regPacketCallback(dst); 120 | } 121 | 122 | if (send_point_cloud_ros) 123 | { 124 | RS_DEBUG << "------------------------------------------------------" << RS_REND; 125 | RS_DEBUG << "Send PointCloud To : ROS" << RS_REND; 126 | RS_DEBUG << "PointCloud Topic: " << lidar_config[i]["ros"]["ros_send_point_cloud_topic"].as() 127 | << RS_REND; 128 | RS_DEBUG << "------------------------------------------------------" << RS_REND; 129 | 130 | std::shared_ptr dst = std::make_shared(); 131 | dst->init(lidar_config[i]); 132 | source->regPointCloudCallback(dst); 133 | } 134 | 135 | sources_.emplace_back(source); 136 | } 137 | } 138 | 139 | void NodeManager::start() 140 | { 141 | for (auto& iter : sources_) 142 | { 143 | if (iter != nullptr) 144 | { 145 | iter->start(); 146 | } 147 | } 148 | } 149 | 150 | void NodeManager::stop() 151 | { 152 | for (auto& iter : sources_) 153 | { 154 | if (iter != nullptr) 155 | { 156 | iter->stop(); 157 | } 158 | } 159 | } 160 | 161 | NodeManager::~NodeManager() 162 | { 163 | stop(); 164 | } 165 | 166 | } // namespace lidar 167 | } // namespace robosense 168 | -------------------------------------------------------------------------------- /rviz/rviz.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 75 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Point Cloud1 9 | - /PointCloud21 10 | Splitter Ratio: 0.5 11 | Tree Height: 790 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: 16 | - /2D Pose Estimate1 17 | - /2D Nav Goal1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.5886790156364441 21 | - Class: rviz/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz/Time 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: Point Cloud 30 | - Class: rviz/Displays 31 | Help Height: 75 32 | Name: Displays 33 | Property Tree Widget: 34 | Expanded: ~ 35 | Splitter Ratio: 0.5 36 | Tree Height: 366 37 | Preferences: 38 | PromptSaveOnExit: true 39 | Toolbars: 40 | toolButtonStyle: 2 41 | Visualization Manager: 42 | Class: "" 43 | Displays: 44 | - Alpha: 1 45 | Autocompute Intensity Bounds: true 46 | Autocompute Value Bounds: 47 | Max Value: 10 48 | Min Value: -10 49 | Value: true 50 | Axis: Z 51 | Channel Name: intensity 52 | Class: rviz/PointCloud2 53 | Color: 255; 255; 255 54 | Color Transformer: Intensity 55 | Decay Time: 0 56 | Enabled: true 57 | Invert Rainbow: false 58 | Max Color: 255; 255; 255 59 | Min Color: 0; 0; 0 60 | Name: Point Cloud 61 | Position Transformer: XYZ 62 | Queue Size: 10 63 | Selectable: true 64 | Size (Pixels): 1.5 65 | Size (m): 0.009999999776482582 66 | Style: Squares 67 | Topic: /horizontal_rslidar_points 68 | Unreliable: false 69 | Use Fixed Frame: true 70 | Use rainbow: true 71 | Value: true 72 | - Alpha: 1 73 | Autocompute Intensity Bounds: true 74 | Autocompute Value Bounds: 75 | Max Value: 10 76 | Min Value: -10 77 | Value: true 78 | Axis: Z 79 | Channel Name: intensity 80 | Class: rviz/PointCloud2 81 | Color: 255; 255; 255 82 | Color Transformer: Intensity 83 | Decay Time: 0 84 | Enabled: true 85 | Invert Rainbow: false 86 | Max Color: 255; 255; 255 87 | Min Color: 0; 0; 0 88 | Name: PointCloud2 89 | Position Transformer: XYZ 90 | Queue Size: 10 91 | Selectable: true 92 | Size (Pixels): 3 93 | Size (m): 0.009999999776482582 94 | Style: Flat Squares 95 | Topic: /vertical_rslidar_points 96 | Unreliable: false 97 | Use Fixed Frame: true 98 | Use rainbow: true 99 | Value: true 100 | - Alpha: 0.5 101 | Cell Size: 10 102 | Class: rviz/Grid 103 | Color: 160; 160; 164 104 | Enabled: true 105 | Line Style: 106 | Line Width: 0.029999999329447746 107 | Value: Lines 108 | Name: Grid 109 | Normal Cell Count: 0 110 | Offset: 111 | X: 0 112 | Y: 0 113 | Z: 0 114 | Plane: XY 115 | Plane Cell Count: 50 116 | Reference Frame: 117 | Value: true 118 | - Alpha: 1 119 | Class: rviz/Axes 120 | Enabled: true 121 | Length: 1 122 | Name: Axes 123 | Radius: 0.10000000149011612 124 | Reference Frame: 125 | Show Trail: false 126 | Value: true 127 | Enabled: true 128 | Global Options: 129 | Background Color: 48; 48; 48 130 | Default Light: true 131 | Fixed Frame: rslidar 132 | Frame Rate: 30 133 | Name: root 134 | Tools: 135 | - Class: rviz/Interact 136 | Hide Inactive Objects: true 137 | - Class: rviz/MoveCamera 138 | - Class: rviz/Select 139 | - Class: rviz/FocusCamera 140 | - Class: rviz/Measure 141 | - Class: rviz/SetInitialPose 142 | Theta std deviation: 0.2617993950843811 143 | Topic: /initialpose 144 | X std deviation: 0.5 145 | Y std deviation: 0.5 146 | - Class: rviz/SetGoal 147 | Topic: /move_base_simple/goal 148 | - Class: rviz/PublishPoint 149 | Single click: true 150 | Topic: /clicked_point 151 | Value: true 152 | Views: 153 | Current: 154 | Class: rviz/Orbit 155 | Distance: 22.780855178833008 156 | Enable Stereo Rendering: 157 | Stereo Eye Separation: 0.05999999865889549 158 | Stereo Focal Distance: 1 159 | Swap Stereo Eyes: false 160 | Value: false 161 | Field of View: 0.7853981852531433 162 | Focal Point: 163 | X: 1.4133121967315674 164 | Y: -0.1708243489265442 165 | Z: 1.6932628154754639 166 | Focal Shape Fixed Size: true 167 | Focal Shape Size: 0.05000000074505806 168 | Invert Z Axis: false 169 | Name: Current View 170 | Near Clip Distance: 0.009999999776482582 171 | Pitch: 0.9697965979576111 172 | Target Frame: 173 | Yaw: 3.20500111579895 174 | Saved: ~ 175 | Window Geometry: 176 | Displays: 177 | collapsed: false 178 | Height: 1016 179 | Hide Left Dock: false 180 | Hide Right Dock: true 181 | QMainWindow State: 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 182 | Selection: 183 | collapsed: false 184 | Time: 185 | collapsed: false 186 | Tool Properties: 187 | collapsed: false 188 | Views: 189 | collapsed: false 190 | Width: 1848 191 | X: 72 192 | Y: 27 193 | -------------------------------------------------------------------------------- /doc/src_intro/rslidar_sdk_intro_CN.md: -------------------------------------------------------------------------------- 1 | # rslidar_sdk v1.5.9 源代码解析 2 | 3 | 4 | 5 | ## 1 简介 6 | 7 | rslidar_sdk是基于ROS/ROS2的雷达驱动。rslidar_sdk依赖rs_driver接收和解析MSOP/DIFOP Packet。 8 | 9 | rslidar_sdk的基本功能如下： 10 | + 从在线雷达或PCAP文件得到点云，通过ROS主题`/rslidar_points`发布。使用者可以订阅这个主题，在rviz中看到点云。 11 | + 从在线雷达得到原始的MSOP/DIFOP Packet，通过ROS主题`/rslidar_packets`发布。使用者可以订阅这个主题，将Packet记录到rosbag文件。 12 | + 从ROS主题`/rslidar_packets`得到MSOP/DIFOP Packet，解析得到点云，再发布到主题`/rslidar_points`。 13 | + 这里的主题`/rslidar_packets`，由使用者通过回放Packet rosbag文件发布。 14 | 15 | 16 | 17 | ## 2 Source 与 Destination 18 | 19 | 如前面所说，rslidar_sdk从在线雷达、PCAP文件、ROS主题这三种源得到MSOP/DIFOP Packet，将Packet发布到ROS主题`/rslidar_packets`，将点云发布到目标 - ROS主题`/rslidar_points`。 20 | + Source定义源接口 21 | + DestinationPointCloud定义发送点云的目标接口。 22 | + DestinationPacket定义发送MSOP/DIFOP Packet的目标接口。 23 | 24 | ![source](./img/class_source_destination.png) 25 | 26 | ### 2.1 DestinationPointCloud 27 | 28 | DestinationPointCloud定义发送点云的接口。 29 | + 虚拟成员函数init()对DestinationPointCloud实例初始化 30 | + 虚拟成员函数start()启动实例 31 | + 虚拟成员函数sendPointCloud()发送PointCloud消息 32 | 33 | ### 2.2 DestinationPacket 34 | 35 | DestinationPacket定义发送MSOP/DIFOP Packet的接口。 36 | + 虚拟成员函数init()对DestinationPacket实例初始化 37 | + 虚拟成员函数start()启动实例 38 | + 虚拟成员函数sendPacket()启动发送Packet消息 39 | 40 | ### 2.3 Source 41 | 42 | Source是定义源的接口。 43 | 44 | + 成员`src_type_`是源的类型 45 | 46 | ``` 47 | enum SourceType 48 | { 49 | MSG_FROM_LIDAR = 1, 50 | MSG_FROM_ROS_PACKET = 2, 51 | MSG_FROM_PCAP = 3, 52 | }; 53 | ``` 54 | 55 | + 成员`pc_cb_vec_[]`中是一组DestinationPointCloud的实例。 56 | + 成员函数sendPointCloud()调用`point_cb_vec_[]`中的实例，发送点云消息。 57 | + 成员`pkt_cb_vec_[]`中是一组DestinationPacket实例。 58 | + 成员函数sendPacket()将Packet消息发送到`pkt_cb_vec_[]`中的实例中。 59 | 60 | + 虚拟成员函数init()初始化Source实例 61 | + 虚拟成员函数start()启动实例 62 | + 虚拟成员函数regPointCloudCallback()将PointCloudDestination实例注册到`point_cb_vec_[]`。 63 | + 虚拟成员函数regPacketCallback()将PacketDestination实例注册到`packet_cb_vec_[]`。 64 | 65 | ### 2.4 DestinationPointCloudRos 66 | 67 | DestinationPointCloudRos在ROS主题`/rslidar_points`发布点云。 68 | + 成员`pkt_pub_`是ROS主题发布器。 69 | + 成员`frame_id_`保存`frame_id`。`frame_id`是坐标系名字。 70 | 71 | ![destination pointcloud ros](./img/class_destination_pointcloud.png) 72 | 73 | #### 2.4.1 DestinationPointCloudRos::init() 74 | 75 | init()初始化DestinationPointCloudRos实例。 76 | + 从YAML文件读入用户配置参数。 77 | + 读入`frame_id`，保存在成员`frame_id_`，默认值是`rslidar`。 78 | + 读入ROS主题，保存在本地变量`ros_send_topic_`，默认值是`/rslidar_points`。 79 | + 创建ROS主题发布器，保存在成员`pkt_sub_`. 80 | 81 | #### 2.4.2 DestinationPointCloudRos::sendPointCloud() 82 | 83 | sendPacket()在ROS主题`/rslidar_points`发布点云。 84 | + 调用Publisher::publish()发布ROS格式的点云消息。 85 | 86 | ### 2.5 DestinationPacketRos 87 | 88 | DestinationPacketRos在ROS主题`/rslidar_packets`发布MSOP/DIFOP Packet。 89 | + 成员`pkt_sub_`是ROS主题发布器。 90 | + 成员`frame_id_`保存`frame_id`。`frame_id`是坐标系名字。 91 | 92 | ![destination packet ros](./img/class_destination_packet.png) 93 | 94 | #### 2.5.1 DestinationPacketRos::init() 95 | 96 | init()初始化DestinationPacketRos实例。 97 | + 从YAML文件读入用户配置参数。 98 | + 读入`frame_id`，保存在成员`frame_id_`，默认值是`rslidar` 99 | + 读入ROS主题，保存在本地变量`ros_send_topic_`，默认值是`/rslidar_packets`。 100 | + 创建ROS主题发布器，保存在成员`pkt_sub_`. 101 | 102 | #### 2.5.2 DestinationPacketRos::sendPacket() 103 | 104 | sendPacket()在ROS主题`/rslidar_packets`发布MOSP/DIFOP packet。 105 | + 调用Publisher::publish()发布ROS格式的Packet消息。 106 | 107 | ### 2.6 SourceDriver 108 | 109 | SourceDriver从在线雷达和PCAP文件得到MSOP/DIFOP Packet，并解析得到点云。 110 | + 成员`driver_ptr_`是rs_driver驱动的实例，也就是LidarDriver。 111 | + 成员`free_point_cloud_queue_`和`point_cloud_queue_`，分别是空闲点云的队列和待处理点云的队列。 112 | + 成员`point_cloud_handle_thread_`是点云的处理线程。 113 | 114 | ![source driver](./img/class_source_driver.png) 115 | 116 | #### 2.6.1 SourceDriver::init() 117 | 118 | init()初始化SourceDriver实例。 119 | + 读取YAML配置文件，得到雷达的用户配置参数。 120 | + 根据源类型，也就是成员`src_type_`，创建相应类型的LidarDriver实例，也就是成员`driver_ptr_`。 121 | + `src_type_`是在SourceDriver中的构造函数中指定的。 122 | + 调用LidarDriver::regPointCloudCallback()，注册回调函数。这里是getPointCloud()和putPointCloud()。前者给`driver_ptr_`提供空闲点云，后者从`driver_ptr_`得到填充好的点云。 123 | + 注意，这里没有注册MSOP/DIFOP Packet的回调函数，因为Packet是按需获取的。这时为了避免不必要地消耗CPU资源。 124 | + 调用LidarDriver::init()，初始化`driver_ptr_`。 125 | + 创建、启动点云处理线程`point_cloud_handle_thread_`， 线程函数是processPointCloud()。 126 | 127 | #### 2.6.2 SourceDriver::getPointCloud() 128 | 129 | getPointCloud()给成员`driver_ptr_`提供空闲的点云。 130 | + 优先从成员`free_point_cloud_queue_`得到点云。 131 | + 如果得不到，分配新的点云。 132 | 133 | #### 2.6.3 SourceDriver::putPointCloud() 134 | 135 | putPointCloud()给从成员`driver_ptr_`得到填充好的点云。 136 | + 将得到的点云推送到成员`point_cloud_queue_`，等待处理。 137 | 138 | #### 2.6.4 SourceDriver::processPointCloud() 139 | 140 | processPointCloud()处理点云。在while循环中， 141 | + 从待处理点云的队列`point_cloud_queue_`，得到点云， 142 | + 调用sendPointCloud()，其中调用成员`pc_cb_vec_[]`中的DestinationPointCloud实例，发送点云。 143 | + 回收点云，放入空闲点云的队列`free_cloud_queue_`，待下次使用。 144 | 145 | #### 2.6.5 SourceDriver::regPacketCallback() 146 | 147 | regPacketCallback()用来注册DestinationPacket。 148 | + 调用Source::regPacketCallback()，将DestinationPacket实例，加入成员`pkt_cb_vec_[]`。 149 | + 如果这是首次要求Packet(`pkt_cb_vec_[]`的第1个实例)，调用LidarDriver::regPacketCallback()，向`driver_ptr_`注册Packet回调函数，开始接收Packet。回调函数是putPacket()。 150 | 151 | #### 2.6.6 SourceDriver::putPacket() 152 | 153 | putPacket()调用sendPacket()，其中调用成员`pkt_cb_vec_[]`中的所有实例，发送MSOP/DIFOP Packet。 154 | 155 | ### 2.7 SourcePacketRos 156 | 157 | SourcePacketRos在ROS主题`/rslidar_packets`得到MSOP/DIFOP Packet，解析后得到点云。 158 | + SourcePacketRos从SourceDriver派生，而不是直接从Source派生，是因为它用SourceDriver解析Packet得到点云。 159 | + 成员`pkt_sub_`，是ROS主题`/rslidar_packets`的订阅器。 160 | 161 | ![source](./img/class_source_packet_ros.png) 162 | 163 | #### 2.7.1 SourcePacketRos::init() 164 | 165 | init()初始化SourcePacketRos实例。 166 | + 调用SourceDriver::init()初始化成员`driver_ptr_`。 167 | + 在SourcePacketRos的构造函数中，SourceType设置为`SourceType::MSG_FROM_ROS_PACKET`。这样，在SourceDriver::init()中，`driver_ptr_`的`input_type`就是`InputType::RAW_PACKET`，它通过LidarDriver::feedPacket接收Packet作为源。 168 | + 解析YAML文件得到雷达的用户配置参数 169 | + 得到接收Packet的主题，默认值为`/rslidar_packets`。 170 | + 创建Packet主题的订阅器，也就是成员`pkt_sub_`，接收函数是putPacket()。 171 | 172 | #### 2.7.2 SourcePacketRos::putPacket() 173 | 174 | putPacket()接收Packet，送到`driver_ptr_`解析。 175 | + 调用LidarDriver::decodePacket()，将Packet喂给`driver_ptr_`。 176 | + 点云的接收，使用SourceDriver的已有实现。 177 | 178 | 179 | 180 | ## 3 NodeManager 181 | 182 | NodeManager管理Source实例，包括创建、初始化、启动、停止Source。它支持多个源，但是这些源的类型必须相同。 183 | + 成员`sources_[]`是一个Source实例的数组。 184 | 185 | ![node_manager](./img/class_node_manager.png) 186 | 187 | ### 3.1 NodeManager::init() 188 | 189 | init()初始化NodeManger实例。 190 | + 从config.yaml文件得到用户配置参数 191 | + 本地变量`msg_source`，数据源类型 192 | + 本地变量`send_point_cloud_ros`， 是否在ROS主题发送点云。 193 | + 本地变量`send_packet_ros`，是否在ROS主题发送MSOP/DIFOP packet， 194 | 195 | + 在.yaml文件中遍历数据源。在循环中， 196 | + 根据`msg_source`创建Source实例。 197 | + 如果是在线雷达（`SourceType::MSG_FROM_LIDAR`)，创建SourceDriver实例并初始化， 源类型为`MSG_FROM_LIDAR`。 198 | + 如果是PCAP文件(`SourceType::MSG_FROM_PCAP`)，创建SourceDriver实例并初始化，源类型为`MSG_FROM_PCAP`。 199 | + 如果是ROS主题(`SourceType::MSG_FROM_ROS_PACKET`), 创建SourcePacketRos并初始化。SourcePacketRos构造函数已将源类型设置为`MSG_FROM_ROS_PACKET` 200 | + 如果在ROS主题发送点云(`send_point_cloud_ros` = `true`)，则创建DestinationPointCloudRos实例、初始化，调用Source::regPointCloudCallback(),将它加入Source的`pc_cb_vec_[]`。 201 | + 如果在ROS主题发送Packet(`send_packet_ros` = `true`)，则创建DestinationPacketRos实例、初始化，调用Source::regPacketCallback()将它加入Source的`pkt_cb_vec_[]`。 202 | + 将Source实例，加入成员`sources_[]`。 203 | 204 | ### 3.2 NodeManager::start() 205 | 206 | start()启动成员`sources_[]`中的所有实例。 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | -------------------------------------------------------------------------------- /src/msg/ros_msg/rs_compressed_image.hpp: -------------------------------------------------------------------------------- 1 | // Generated by gencpp from file rscamera_msg/RsCompressedImage.msg 2 | // DO NOT EDIT! 3 | 4 | 5 | #ifndef RSCAMERA_MESSAGE_RSCOMPRESSEDIMAGE_H 6 | #define RSCAMERA_MESSAGE_RSCOMPRESSEDIMAGE_H 7 | 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | 20 | namespace rscamera_msg 21 | { 22 | template 23 | struct RsCompressedImage_ 24 | { 25 | typedef RsCompressedImage_ Type; 26 | 27 | RsCompressedImage_() 28 | : header() 29 | , type(0) 30 | , data() { 31 | } 32 | RsCompressedImage_(const ContainerAllocator& _alloc) 33 | : header(_alloc) 34 | , type(0) 35 | , data(_alloc) { 36 | (void)_alloc; 37 | } 38 | 39 | 40 | 41 | typedef ::std_msgs::Header_ _header_type; 42 | _header_type header; 43 | 44 | typedef uint8_t _type_type; 45 | _type_type type; 46 | 47 | typedef std::vector::template rebind_alloc> _data_type; 48 | _data_type data; 49 | 50 | 51 | 52 | 53 | 54 | typedef boost::shared_ptr< ::rscamera_msg::RsCompressedImage_ > Ptr; 55 | typedef boost::shared_ptr< ::rscamera_msg::RsCompressedImage_ const> ConstPtr; 56 | 57 | }; // struct RsCompressedImage_ 58 | 59 | typedef ::rscamera_msg::RsCompressedImage_ > RsCompressedImage; 60 | 61 | typedef boost::shared_ptr< ::rscamera_msg::RsCompressedImage > RsCompressedImagePtr; 62 | typedef boost::shared_ptr< ::rscamera_msg::RsCompressedImage const> RsCompressedImageConstPtr; 63 | 64 | // constants requiring out of line definition 65 | 66 | 67 | 68 | template 69 | std::ostream& operator<<(std::ostream& s, const ::rscamera_msg::RsCompressedImage_ & v) 70 | { 71 | ros::message_operations::Printer< ::rscamera_msg::RsCompressedImage_ >::stream(s, "", v); 72 | return s; 73 | } 74 | 75 | 76 | template 77 | bool operator==(const ::rscamera_msg::RsCompressedImage_ & lhs, const ::rscamera_msg::RsCompressedImage_ & rhs) 78 | { 79 | return lhs.header == rhs.header && 80 | lhs.type == rhs.type && 81 | lhs.data == rhs.data; 82 | } 83 | 84 | template 85 | bool operator!=(const ::rscamera_msg::RsCompressedImage_ & lhs, const ::rscamera_msg::RsCompressedImage_ & rhs) 86 | { 87 | return !(lhs == rhs); 88 | } 89 | 90 | 91 | } // namespace rscamera_msg 92 | 93 | namespace ros 94 | { 95 | namespace message_traits 96 | { 97 | 98 | 99 | 100 | 101 | 102 | template 103 | struct IsMessage< ::rscamera_msg::RsCompressedImage_ > 104 | : TrueType 105 | { }; 106 | 107 | template 108 | struct IsMessage< ::rscamera_msg::RsCompressedImage_ const> 109 | : TrueType 110 | { }; 111 | 112 | template 113 | struct IsFixedSize< ::rscamera_msg::RsCompressedImage_ > 114 | : FalseType 115 | { }; 116 | 117 | template 118 | struct IsFixedSize< ::rscamera_msg::RsCompressedImage_ const> 119 | : FalseType 120 | { }; 121 | 122 | template 123 | struct HasHeader< ::rscamera_msg::RsCompressedImage_ > 124 | : TrueType 125 | { }; 126 | 127 | template 128 | struct HasHeader< ::rscamera_msg::RsCompressedImage_ const> 129 | : TrueType 130 | { }; 131 | 132 | 133 | template 134 | struct MD5Sum< ::rscamera_msg::RsCompressedImage_ > 135 | { 136 | static const char* value() 137 | { 138 | return "4044c7c7aa754eb9dc4031aaf0ca91a7"; 139 | } 140 | 141 | static const char* value(const ::rscamera_msg::RsCompressedImage_&) { return value(); } 142 | static const uint64_t static_value1 = 0x4044c7c7aa754eb9ULL; 143 | static const uint64_t static_value2 = 0xdc4031aaf0ca91a7ULL; 144 | }; 145 | 146 | template 147 | struct DataType< ::rscamera_msg::RsCompressedImage_ > 148 | { 149 | static const char* value() 150 | { 151 | return "rscamera_msg/RsCompressedImage"; 152 | } 153 | 154 | static const char* value(const ::rscamera_msg::RsCompressedImage_&) { return value(); } 155 | }; 156 | 157 | template 158 | struct Definition< ::rscamera_msg::RsCompressedImage_ > 159 | { 160 | static const char* value() 161 | { 162 | return "std_msgs/Header header\n" 163 | "uint8 type\n" 164 | "uint8[] data\n" 165 | "\n" 166 | "================================================================================\n" 167 | "MSG: std_msgs/Header\n" 168 | "# Standard metadata for higher-level stamped data types.\n" 169 | "# This is generally used to communicate timestamped data \n" 170 | "# in a particular coordinate frame.\n" 171 | "# \n" 172 | "# sequence ID: consecutively increasing ID \n" 173 | "uint32 seq\n" 174 | "#Two-integer timestamp that is expressed as:\n" 175 | "# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n" 176 | "# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n" 177 | "# time-handling sugar is provided by the client library\n" 178 | "time stamp\n" 179 | "#Frame this data is associated with\n" 180 | "string frame_id\n" 181 | ; 182 | } 183 | 184 | static const char* value(const ::rscamera_msg::RsCompressedImage_&) { return value(); } 185 | }; 186 | 187 | } // namespace message_traits 188 | } // namespace ros 189 | 190 | namespace ros 191 | { 192 | namespace serialization 193 | { 194 | 195 | template struct Serializer< ::rscamera_msg::RsCompressedImage_ > 196 | { 197 | template inline static void allInOne(Stream& stream, T m) 198 | { 199 | stream.next(m.header); 200 | stream.next(m.type); 201 | stream.next(m.data); 202 | } 203 | 204 | ROS_DECLARE_ALLINONE_SERIALIZER 205 | }; // struct RsCompressedImage_ 206 | 207 | } // namespace serialization 208 | } // namespace ros 209 | 210 | namespace ros 211 | { 212 | namespace message_operations 213 | { 214 | 215 | template 216 | struct Printer< ::rscamera_msg::RsCompressedImage_ > 217 | { 218 | template static void stream(Stream& s, const std::string& indent, const ::rscamera_msg::RsCompressedImage_& v) 219 | { 220 | s << indent << "header: "; 221 | s << std::endl; 222 | Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); 223 | s << indent << "type: "; 224 | Printer::stream(s, indent + " ", v.type); 225 | s << indent << "data[]" << std::endl; 226 | for (size_t i = 0; i < v.data.size(); ++i) 227 | { 228 | s << indent << " data[" << i << "]: "; 229 | Printer::stream(s, indent + " ", v.data[i]); 230 | } 231 | } 232 | }; 233 | 234 | } // namespace message_operations 235 | } // namespace ros 236 | 237 | #endif // RSCAMERA_MESSAGE_RSCOMPRESSEDIMAGE_H 238 | -------------------------------------------------------------------------------- /src/msg/ros_msg/rslidar_scan_legacy.hpp: -------------------------------------------------------------------------------- 1 | // Generated by gencpp from file rslidar_msgs/rslidarScan.msg 2 | // DO NOT EDIT! 3 | 4 | #pragma once 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | #include "msg/ros_msg/rslidar_packet_legacy.hpp" 17 | 18 | namespace rslidar_msgs 19 | { 20 | template 21 | struct rslidarScan_ 22 | { 23 | typedef rslidarScan_ Type; 24 | 25 | rslidarScan_() : header(), packets() 26 | { 27 | } 28 | rslidarScan_(const ContainerAllocator& _alloc) : header(_alloc), packets(_alloc) 29 | { 30 | (void)_alloc; 31 | } 32 | 33 | typedef ::std_msgs::Header_ _header_type; 34 | _header_type header; 35 | 36 | typedef std::vector< 37 | ::rslidar_msgs::rslidarPacket_, 38 | typename ContainerAllocator::template rebind<::rslidar_msgs::rslidarPacket_>::other> 39 | _packets_type; 40 | _packets_type packets; 41 | 42 | typedef boost::shared_ptr<::rslidar_msgs::rslidarScan_> Ptr; 43 | typedef boost::shared_ptr<::rslidar_msgs::rslidarScan_ const> ConstPtr; 44 | 45 | }; // struct rslidarScan_ 46 | 47 | typedef ::rslidar_msgs::rslidarScan_> rslidarScan; 48 | 49 | typedef boost::shared_ptr<::rslidar_msgs::rslidarScan> rslidarScanPtr; 50 | typedef boost::shared_ptr<::rslidar_msgs::rslidarScan const> rslidarScanConstPtr; 51 | 52 | // constants requiring out of line definition 53 | 54 | template 55 | std::ostream& operator<<(std::ostream& s, const ::rslidar_msgs::rslidarScan_& v) 56 | { 57 | ros::message_operations::Printer<::rslidar_msgs::rslidarScan_>::stream(s, "", v); 58 | return s; 59 | } 60 | 61 | } // namespace rslidar_msgs 62 | 63 | namespace ros 64 | { 65 | namespace message_traits 66 | { 67 | // BOOLTRAITS {'IsFixedSize': false, 'IsMessage': true, 'HasHeader': true} 68 | // {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'rslidar_msgs': 69 | // ['/home/xzd/catkin_ws/src/rs_driver/rslidar_msgs/msg']} 70 | 71 | // !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', 72 | // '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', 73 | // '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 74 | // 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types'] 75 | 76 | template 77 | struct IsFixedSize<::rslidar_msgs::rslidarScan_> : FalseType 78 | { 79 | }; 80 | 81 | template 82 | struct IsFixedSize<::rslidar_msgs::rslidarScan_ const> : FalseType 83 | { 84 | }; 85 | 86 | template 87 | struct IsMessage<::rslidar_msgs::rslidarScan_> : TrueType 88 | { 89 | }; 90 | 91 | template 92 | struct IsMessage<::rslidar_msgs::rslidarScan_ const> : TrueType 93 | { 94 | }; 95 | 96 | template 97 | struct HasHeader<::rslidar_msgs::rslidarScan_> : TrueType 98 | { 99 | }; 100 | 101 | template 102 | struct HasHeader<::rslidar_msgs::rslidarScan_ const> : TrueType 103 | { 104 | }; 105 | 106 | template 107 | struct MD5Sum<::rslidar_msgs::rslidarScan_> 108 | { 109 | static const char* value() 110 | { 111 | return "ff6baa58985b528481871cbaf1bb342f"; 112 | } 113 | 114 | static const char* value(const ::rslidar_msgs::rslidarScan_&) 115 | { 116 | return value(); 117 | } 118 | static const uint64_t static_value1 = 0xff6baa58985b5284ULL; 119 | static const uint64_t static_value2 = 0x81871cbaf1bb342fULL; 120 | }; 121 | 122 | template 123 | struct DataType<::rslidar_msgs::rslidarScan_> 124 | { 125 | static const char* value() 126 | { 127 | return "rslidar_msgs/rslidarScan"; 128 | } 129 | 130 | static const char* value(const ::rslidar_msgs::rslidarScan_&) 131 | { 132 | return value(); 133 | } 134 | }; 135 | 136 | template 137 | struct Definition<::rslidar_msgs::rslidarScan_> 138 | { 139 | static const char* value() 140 | { 141 | return "# LIDAR scan packets.\n\ 142 | \n\ 143 | Header header # standard ROS message header\n\ 144 | rslidarPacket[] packets # vector of raw packets\n\ 145 | \n\ 146 | ================================================================================\n\ 147 | MSG: std_msgs/Header\n\ 148 | # Standard metadata for higher-level stamped data types.\n\ 149 | # This is generally used to communicate timestamped data \n\ 150 | # in a particular coordinate frame.\n\ 151 | # \n\ 152 | # sequence ID: consecutively increasing ID \n\ 153 | uint32 seq\n\ 154 | #Two-integer timestamp that is expressed as:\n\ 155 | # * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\ 156 | # * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\ 157 | # time-handling sugar is provided by the client library\n\ 158 | timestamp\n\ 159 | #Frame this data is associated with\n\ 160 | # 0: no frame\n\ 161 | # 1: global frame\n\ 162 | string frame_id\n\ 163 | \n\ 164 | ================================================================================\n\ 165 | MSG: rslidar_msgs/rslidarPacket\n\ 166 | # Raw LIDAR packet.\n\ 167 | \n\ 168 | timestamp # packet timestamp\n\ 169 | uint8[1248] data # packet contents\n\ 170 | \n\ 171 | "; 172 | } 173 | 174 | static const char* value(const ::rslidar_msgs::rslidarScan_&) 175 | { 176 | return value(); 177 | } 178 | }; 179 | 180 | } // namespace message_traits 181 | } // namespace ros 182 | 183 | namespace ros 184 | { 185 | namespace serialization 186 | { 187 | template 188 | struct Serializer<::rslidar_msgs::rslidarScan_> 189 | { 190 | template 191 | inline static void allInOne(Stream& stream, T m) 192 | { 193 | stream.next(m.header); 194 | stream.next(m.packets); 195 | } 196 | 197 | ROS_DECLARE_ALLINONE_SERIALIZER 198 | }; // struct rslidarScan_ 199 | 200 | } // namespace serialization 201 | } // namespace ros 202 | 203 | namespace ros 204 | { 205 | namespace message_operations 206 | { 207 | template 208 | struct Printer<::rslidar_msgs::rslidarScan_> 209 | { 210 | template 211 | static void stream(Stream& s, const std::string& indent, const ::rslidar_msgs::rslidarScan_& v) 212 | { 213 | s << indent << "header: "; 214 | s << std::endl; 215 | Printer<::std_msgs::Header_>::stream(s, indent + " ", v.header); 216 | s << indent << "packets[]" << std::endl; 217 | for (size_t i = 0; i < v.packets.size(); ++i) 218 | { 219 | s << indent << " packets[" << i << "]: "; 220 | s << std::endl; 221 | s << indent; 222 | Printer<::rslidar_msgs::rslidarPacket_>::stream(s, indent + " ", v.packets[i]); 223 | } 224 | } 225 | }; 226 | 227 | } // namespace message_operations 228 | } // namespace ros 229 | -------------------------------------------------------------------------------- /doc/intro/02_parameter_intro.md: -------------------------------------------------------------------------------- 1 | # 2 Introduction to Parameters 2 | 3 | rslidar_sdk reads parameters from the configuration file ```config.yaml```, which is stored in ```rslidar_sdk/config```. 4 | 5 | `config.yaml` contains two parts, the `common` part and the `lidar` part. 6 | 7 | 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. 8 | 9 | **config.yaml is indentation sensitive! Please make sure the indentation is not changed after adjusting the parameters!** 10 | 11 | 12 | 13 | ## 2.1 Common 14 | 15 | The `common` part specifies the source of LiDAR packets, and where to publish point clouds and packets. 16 | 17 | ```yaml 18 | common: 19 | msg_source: 1 20 | send_packet_ros: false 21 | send_point_cloud_ros: true 22 | ``` 23 | 24 | - msg_source 25 | 26 | - 0 -- Unused. Never set this parameter to 0. 27 | 28 | - 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) 29 | 30 | - 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) 31 | 32 | - 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) 33 | 34 | - send_packet_ros 35 | 36 | - true -- LiDAR packets will be sent to ROS/ROS2. 37 | 38 | *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.* 39 | 40 | - send_point_cloud_ros 41 | 42 | - true -- The LiDAR point cloud will be sent to ROS/ROS2. 43 | 44 | *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.* 45 | 46 | 47 | 48 | ## 2.2 lidar 49 | 50 | The `lidar` part needs to be adjusted for every LiDAR seperately. 51 | 52 | ```yaml 53 | lidar: 54 | - driver: 55 | lidar_type: RSBP 56 | msop_port: 6699 57 | difop_port: 7788 58 | start_angle: 0 59 | end_angle: 360 60 | min_distance: 0.2 61 | max_distance: 200 62 | use_lidar_clock: false 63 | dense_points: false 64 | pcap_path: /home/robosense/lidar.pcap 65 | ros: 66 | ros_frame_id: rslidar 67 | ros_recv_packet_topic: /rslidar_packets 68 | ros_send_packet_topic: /rslidar_packets 69 | ros_send_point_cloud_topic: /rslidar_points 70 | ``` 71 | 72 | - lidar_type 73 | 74 | Supported LiDAR types are listed in the README file. 75 | 76 | - msop_port, difop_port 77 | 78 | The MSOP port and DIFOP port to receive LiDAR packets. *If no data is received, please check these parameters first.* 79 | 80 | - start_angle, end_angle 81 | 82 | 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`*. 83 | 84 | - min_distance, max_distance 85 | 86 | The minimum distance and maximum distance of the point cloud. 87 | 88 | - use_lidar_clock 89 | 90 | - true -- Use the Lidar clock as the message timestamp 91 | - false -- Use the host machine clock as the message timestamp 92 | 93 | - dense_points 94 | 95 | Whether to discard NAN points. The default value is ```false```. 96 | - Discard if ```true``` 97 | - reserve if ```false```. 98 | 99 | - pcap_path 100 | 101 | The full path of the PCAP file. Valid if msg_source = 3. 102 | 103 | - ros_send_by_rows 104 | 105 | Meaningful only for Mechanical Lidars, and valid if dense_points = false。 106 | - true -- send point cloud row by row 107 | - false -- send point cloud clolumn by column 108 | 109 | 110 | 111 | ## 2.3 Examples 112 | 113 | ### 2.3.1 Single Lidar Case 114 | 115 | Connect to 1 LiDAR of RS128, and send point cloud to ROS. 116 | 117 | ```yaml 118 | common: 119 | msg_source: 1 120 | send_packet_ros: false 121 | send_point_cloud_ros: true 122 | lidar: 123 | - driver: 124 | lidar_type: RSBP 125 | msop_port: 6699 126 | difop_port: 7788 127 | start_angle: 0 128 | end_angle: 360 129 | min_distance: 0.2 130 | max_distance: 200 131 | use_lidar_clock: false 132 | ros: 133 | ros_frame_id: rslidar 134 | ros_recv_packet_topic: /rslidar_packets 135 | ros_send_packet_topic: /rslidar_packets 136 | ros_send_point_cloud_topic: /rslidar_points 137 | ``` 138 | 139 | ### 2.3.2 Multi Lidar Case 140 | 141 | Connect to 2 LiDARs of RSBP, and send point cloud to ROS. 142 | 143 | *First of all you must configure Lidar cmaeras accordingly.* 144 | ```text 145 | Connecting 2 Lidars simutaneously! 146 | 147 | The default IP address of the RS-BPearl Lidar camera's ip address is 192.168.1.200 148 | 1. You must configure your IP address of IP adapter(Lidar connected) as 192.168.1.x/24 149 | 2. Set Lidars' IP address differently. (with 192.168.1.200 and 192.168.1.201) 150 | Plug only one lidar to the Ethernet and set IP address as 192.168.1.201. (Via web interface of Lidar at http://192.168.1.200) 151 | Plug in another lidar to the different IP adapter. 152 | 3. Set MSOP and DIFOP port accordingly. 153 | Default ports are 6699 and 7788 so if you plugged in dual lidar they are all set as 6699 and 7799. So you need to set one differently 154 | In the example used 6999 and 7888. 155 | Via webinterface of Lidar camera as mentioned in the second step. 156 | ``` 157 | All the connection is done. 158 | Then let's dive into the lidar sdk configuration. 159 | 160 | *Pay attention to the indentation of the `lidar` part* 161 | 162 | ```yaml 163 | common: 164 | msg_source: 1 165 | send_packet_ros: false 166 | send_point_cloud_ros: true 167 | lidar: 168 | - driver: 169 | lidar_type: RSBP 170 | msop_port: 6699 171 | difop_port: 7788 172 | start_angle: 0 173 | end_angle: 360 174 | min_distance: 0.2 175 | max_distance: 200 176 | use_lidar_clock: false 177 | ros: 178 | ros_frame_id: rslidar 179 | ros_recv_packet_topic: /left/rslidar_packets 180 | ros_send_packet_topic: /left/rslidar_packets 181 | ros_send_point_cloud_topic: /left/rslidar_points 182 | - driver: 183 | lidar_type: RSBP 184 | msop_port: 6999 185 | difop_port: 7888 186 | start_angle: 0 187 | end_angle: 360 188 | min_distance: 0.2 189 | max_distance: 200 190 | use_lidar_clock: false 191 | ros: 192 | ros_frame_id: rslidar 193 | ros_recv_packet_topic: /right/rslidar_packets 194 | ros_send_packet_topic: /right/rslidar_packets 195 | ros_send_point_cloud_topic: /right/rslidar_points 196 | ``` 197 | 198 | -------------------------------------------------------------------------------- /src/msg/ros_msg/rslidar_packet.hpp: -------------------------------------------------------------------------------- 1 | // Generated by gencpp from file rslidar_msg/RslidarPacket.msg 2 | // DO NOT EDIT! 3 | 4 | 5 | #ifndef RSLIDAR_MSG_MESSAGE_RSLIDARPACKET_H 6 | #define RSLIDAR_MSG_MESSAGE_RSLIDARPACKET_H 7 | 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | 20 | namespace rslidar_msg 21 | { 22 | template 23 | struct RslidarPacket_ 24 | { 25 | typedef RslidarPacket_ Type; 26 | 27 | RslidarPacket_() 28 | : header() 29 | , is_difop(0) 30 | , is_frame_begin(0) 31 | , data() { 32 | } 33 | RslidarPacket_(const ContainerAllocator& _alloc) 34 | : header(_alloc) 35 | , is_difop(0) 36 | , is_frame_begin(0) 37 | , data(_alloc) { 38 | (void)_alloc; 39 | } 40 | 41 | 42 | 43 | typedef ::std_msgs::Header_ _header_type; 44 | _header_type header; 45 | 46 | typedef uint8_t _is_difop_type; 47 | _is_difop_type is_difop; 48 | 49 | typedef uint8_t _is_frame_begin_type; 50 | _is_frame_begin_type is_frame_begin; 51 | 52 | typedef std::vector::other > _data_type; 53 | _data_type data; 54 | 55 | 56 | 57 | 58 | 59 | typedef boost::shared_ptr< ::rslidar_msg::RslidarPacket_ > Ptr; 60 | typedef boost::shared_ptr< ::rslidar_msg::RslidarPacket_ const> ConstPtr; 61 | 62 | }; // struct RslidarPacket_ 63 | 64 | typedef ::rslidar_msg::RslidarPacket_ > RslidarPacket; 65 | 66 | typedef boost::shared_ptr< ::rslidar_msg::RslidarPacket > RslidarPacketPtr; 67 | typedef boost::shared_ptr< ::rslidar_msg::RslidarPacket const> RslidarPacketConstPtr; 68 | 69 | // constants requiring out of line definition 70 | 71 | 72 | 73 | template 74 | std::ostream& operator<<(std::ostream& s, const ::rslidar_msg::RslidarPacket_ & v) 75 | { 76 | ros::message_operations::Printer< ::rslidar_msg::RslidarPacket_ >::stream(s, "", v); 77 | return s; 78 | } 79 | 80 | 81 | template 82 | bool operator==(const ::rslidar_msg::RslidarPacket_ & lhs, const ::rslidar_msg::RslidarPacket_ & rhs) 83 | { 84 | return lhs.header == rhs.header && 85 | lhs.is_difop == rhs.is_difop && 86 | lhs.is_frame_begin == rhs.is_frame_begin && 87 | lhs.data == rhs.data; 88 | } 89 | 90 | template 91 | bool operator!=(const ::rslidar_msg::RslidarPacket_ & lhs, const ::rslidar_msg::RslidarPacket_ & rhs) 92 | { 93 | return !(lhs == rhs); 94 | } 95 | 96 | 97 | } // namespace rslidar_msg 98 | 99 | namespace ros 100 | { 101 | namespace message_traits 102 | { 103 | 104 | 105 | 106 | 107 | 108 | template 109 | struct IsMessage< ::rslidar_msg::RslidarPacket_ > 110 | : TrueType 111 | { }; 112 | 113 | template 114 | struct IsMessage< ::rslidar_msg::RslidarPacket_ const> 115 | : TrueType 116 | { }; 117 | 118 | template 119 | struct IsFixedSize< ::rslidar_msg::RslidarPacket_ > 120 | : FalseType 121 | { }; 122 | 123 | template 124 | struct IsFixedSize< ::rslidar_msg::RslidarPacket_ const> 125 | : FalseType 126 | { }; 127 | 128 | template 129 | struct HasHeader< ::rslidar_msg::RslidarPacket_ > 130 | : TrueType 131 | { }; 132 | 133 | template 134 | struct HasHeader< ::rslidar_msg::RslidarPacket_ const> 135 | : TrueType 136 | { }; 137 | 138 | 139 | template 140 | struct MD5Sum< ::rslidar_msg::RslidarPacket_ > 141 | { 142 | static const char* value() 143 | { 144 | return "4b1cc155a9097c0cb935a7abf46d6eef"; 145 | } 146 | 147 | static const char* value(const ::rslidar_msg::RslidarPacket_&) { return value(); } 148 | static const uint64_t static_value1 = 0x4b1cc155a9097c0cULL; 149 | static const uint64_t static_value2 = 0xb935a7abf46d6eefULL; 150 | }; 151 | 152 | template 153 | struct DataType< ::rslidar_msg::RslidarPacket_ > 154 | { 155 | static const char* value() 156 | { 157 | return "rslidar_msg/RslidarPacket"; 158 | } 159 | 160 | static const char* value(const ::rslidar_msg::RslidarPacket_&) { return value(); } 161 | }; 162 | 163 | template 164 | struct Definition< ::rslidar_msg::RslidarPacket_ > 165 | { 166 | static const char* value() 167 | { 168 | return "std_msgs/Header header\n" 169 | "uint8 is_difop\n" 170 | "uint8 is_frame_begin\n" 171 | "uint8[] data\n" 172 | "\n" 173 | "================================================================================\n" 174 | "MSG: std_msgs/Header\n" 175 | "# Standard metadata for higher-level stamped data types.\n" 176 | "# This is generally used to communicate timestamped data \n" 177 | "# in a particular coordinate frame.\n" 178 | "# \n" 179 | "# sequence ID: consecutively increasing ID \n" 180 | "uint32 seq\n" 181 | "#Two-integer timestamp that is expressed as:\n" 182 | "# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n" 183 | "# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n" 184 | "# time-handling sugar is provided by the client library\n" 185 | "time stamp\n" 186 | "#Frame this data is associated with\n" 187 | "string frame_id\n" 188 | ; 189 | } 190 | 191 | static const char* value(const ::rslidar_msg::RslidarPacket_&) { return value(); } 192 | }; 193 | 194 | } // namespace message_traits 195 | } // namespace ros 196 | 197 | namespace ros 198 | { 199 | namespace serialization 200 | { 201 | 202 | template struct Serializer< ::rslidar_msg::RslidarPacket_ > 203 | { 204 | template inline static void allInOne(Stream& stream, T m) 205 | { 206 | stream.next(m.header); 207 | stream.next(m.is_difop); 208 | stream.next(m.is_frame_begin); 209 | stream.next(m.data); 210 | } 211 | 212 | ROS_DECLARE_ALLINONE_SERIALIZER 213 | }; // struct RslidarPacket_ 214 | 215 | } // namespace serialization 216 | } // namespace ros 217 | 218 | namespace ros 219 | { 220 | namespace message_operations 221 | { 222 | 223 | template 224 | struct Printer< ::rslidar_msg::RslidarPacket_ > 225 | { 226 | template static void stream(Stream& s, const std::string& indent, const ::rslidar_msg::RslidarPacket_& v) 227 | { 228 | s << indent << "header: "; 229 | s << std::endl; 230 | Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); 231 | s << indent << "is_difop: "; 232 | Printer::stream(s, indent + " ", v.is_difop); 233 | s << indent << "is_frame_begin: "; 234 | Printer::stream(s, indent + " ", v.is_frame_begin); 235 | s << indent << "data[]" << std::endl; 236 | for (size_t i = 0; i < v.data.size(); ++i) 237 | { 238 | s << indent << " data[" << i << "]: "; 239 | Printer::stream(s, indent + " ", v.data[i]); 240 | } 241 | } 242 | }; 243 | 244 | } // namespace message_operations 245 | } // namespace ros 246 | 247 | #endif // RSLIDAR_MSG_MESSAGE_RSLIDARPACKET_H 248 | -------------------------------------------------------------------------------- /src/source/source_driver.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************************************************* 2 | Copyright (c) 2020 RoboSense 3 | All rights reserved 4 | 5 | By downloading, copying, installing or using the software you agree to this license. If you do not agree to this 6 | license, do not download, install, copy or use the software. 7 | 8 | License Agreement 9 | For RoboSense LiDAR SDK Library 10 | (3-clause BSD License) 11 | 12 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the 13 | following conditions are met: 14 | 15 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following 16 | disclaimer. 17 | 18 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following 19 | disclaimer in the documentation and/or other materials provided with the distribution. 20 | 21 | 3. Neither the names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used 22 | to endorse or promote products derived from this software without specific prior written permission. 23 | 24 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, 25 | INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 26 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 27 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 29 | WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 30 | THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | *********************************************************************************************************************/ 32 | 33 | #pragma once 34 | 35 | #include "source/source.hpp" 36 | 37 | #include 38 | #include 39 | 40 | namespace robosense 41 | { 42 | namespace lidar 43 | { 44 | 45 | class SourceDriver : public Source 46 | { 47 | public: 48 | 49 | virtual void init(const YAML::Node& config); 50 | virtual void start(); 51 | virtual void stop(); 52 | virtual void regPacketCallback(DestinationPacket::Ptr dst); 53 | virtual ~SourceDriver(); 54 | 55 | SourceDriver(SourceType src_type); 56 | 57 | protected: 58 | 59 | std::shared_ptr getPointCloud(void); 60 | void putPointCloud(std::shared_ptr msg); 61 | void putPacket(const Packet& msg); 62 | void putException(const lidar::Error& msg); 63 | void processPointCloud(); 64 | 65 | std::shared_ptr> driver_ptr_; 66 | SyncQueue> free_point_cloud_queue_; 67 | SyncQueue> point_cloud_queue_; 68 | std::thread point_cloud_process_thread_; 69 | bool to_exit_process_; 70 | }; 71 | 72 | SourceDriver::SourceDriver(SourceType src_type) 73 | : Source(src_type), to_exit_process_(false) 74 | { 75 | } 76 | 77 | inline void SourceDriver::init(const YAML::Node& config) 78 | { 79 | YAML::Node driver_config = yamlSubNodeAbort(config, "driver"); 80 | lidar::RSDriverParam driver_param; 81 | 82 | // input related 83 | yamlRead(driver_config, "msop_port", driver_param.input_param.msop_port, 6699); 84 | yamlRead(driver_config, "difop_port", driver_param.input_param.difop_port, 7788); 85 | yamlRead(driver_config, "host_address", driver_param.input_param.host_address, "0.0.0.0"); 86 | yamlRead(driver_config, "group_address", driver_param.input_param.group_address, "0.0.0.0"); 87 | yamlRead(driver_config, "use_vlan", driver_param.input_param.use_vlan, false); 88 | yamlRead(driver_config, "pcap_path", driver_param.input_param.pcap_path, ""); 89 | yamlRead(driver_config, "pcap_rate", driver_param.input_param.pcap_rate, 1); 90 | yamlRead(driver_config, "pcap_repeat", driver_param.input_param.pcap_repeat, true); 91 | yamlRead(driver_config, "user_layer_bytes", driver_param.input_param.user_layer_bytes, 0); 92 | yamlRead(driver_config, "tail_layer_bytes", driver_param.input_param.tail_layer_bytes, 0); 93 | yamlRead(driver_config, "socket_recv_buf", driver_param.input_param.socket_recv_buf, 106496); 94 | // decoder related 95 | std::string lidar_type; 96 | yamlReadAbort(driver_config, "lidar_type", lidar_type); 97 | driver_param.lidar_type = strToLidarType(lidar_type); 98 | 99 | // decoder 100 | yamlRead(driver_config, "wait_for_difop", driver_param.decoder_param.wait_for_difop, true); 101 | yamlRead(driver_config, "use_lidar_clock", driver_param.decoder_param.use_lidar_clock, false); 102 | yamlRead(driver_config, "min_distance", driver_param.decoder_param.min_distance, 0.2); 103 | yamlRead(driver_config, "max_distance", driver_param.decoder_param.max_distance, 200); 104 | yamlRead(driver_config, "start_angle", driver_param.decoder_param.start_angle, 0); 105 | yamlRead(driver_config, "end_angle", driver_param.decoder_param.end_angle, 360); 106 | yamlRead(driver_config, "dense_points", driver_param.decoder_param.dense_points, false); 107 | yamlRead(driver_config, "ts_first_point", driver_param.decoder_param.ts_first_point, false); 108 | 109 | // mechanical decoder 110 | yamlRead(driver_config, "config_from_file", driver_param.decoder_param.config_from_file, false); 111 | yamlRead(driver_config, "angle_path", driver_param.decoder_param.angle_path, ""); 112 | 113 | uint16_t split_frame_mode; 114 | yamlRead(driver_config, "split_frame_mode", split_frame_mode, 1); 115 | driver_param.decoder_param.split_frame_mode = SplitFrameMode(split_frame_mode); 116 | 117 | yamlRead(driver_config, "split_angle", driver_param.decoder_param.split_angle, 0); 118 | yamlRead(driver_config, "num_blks_split", driver_param.decoder_param.num_blks_split, 0); 119 | 120 | // transform 121 | yamlRead(driver_config, "x", driver_param.decoder_param.transform_param.x, 0); 122 | yamlRead(driver_config, "y", driver_param.decoder_param.transform_param.y, 0); 123 | yamlRead(driver_config, "z", driver_param.decoder_param.transform_param.z, 0); 124 | yamlRead(driver_config, "roll", driver_param.decoder_param.transform_param.roll, 0); 125 | yamlRead(driver_config, "pitch", driver_param.decoder_param.transform_param.pitch, 0); 126 | yamlRead(driver_config, "yaw", driver_param.decoder_param.transform_param.yaw, 0); 127 | 128 | switch (src_type_) 129 | { 130 | case SourceType::MSG_FROM_LIDAR: 131 | driver_param.input_type = InputType::ONLINE_LIDAR; 132 | break; 133 | case SourceType::MSG_FROM_PCAP: 134 | driver_param.input_type = InputType::PCAP_FILE; 135 | break; 136 | default: 137 | driver_param.input_type = InputType::RAW_PACKET; 138 | break; 139 | } 140 | 141 | driver_param.print(); 142 | 143 | driver_ptr_.reset(new lidar::LidarDriver()); 144 | driver_ptr_->regPointCloudCallback(std::bind(&SourceDriver::getPointCloud, this), 145 | std::bind(&SourceDriver::putPointCloud, this, std::placeholders::_1)); 146 | driver_ptr_->regExceptionCallback( 147 | std::bind(&SourceDriver::putException, this, std::placeholders::_1)); 148 | point_cloud_process_thread_ = std::thread(std::bind(&SourceDriver::processPointCloud, this)); 149 | 150 | if (!driver_ptr_->init(driver_param)) 151 | { 152 | RS_ERROR << "Driver Initialize Error...." << RS_REND; 153 | exit(-1); 154 | } 155 | } 156 | 157 | inline void SourceDriver::start() 158 | { 159 | driver_ptr_->start(); 160 | } 161 | 162 | inline SourceDriver::~SourceDriver() 163 | { 164 | stop(); 165 | } 166 | 167 | inline void SourceDriver::stop() 168 | { 169 | driver_ptr_->stop(); 170 | 171 | to_exit_process_ = true; 172 | point_cloud_process_thread_.join(); 173 | } 174 | 175 | inline std::shared_ptr SourceDriver::getPointCloud(void) 176 | { 177 | std::shared_ptr point_cloud = free_point_cloud_queue_.pop(); 178 | if (point_cloud.get() != NULL) 179 | { 180 | return point_cloud; 181 | } 182 | 183 | return std::make_shared(); 184 | } 185 | 186 | inline void SourceDriver::regPacketCallback(DestinationPacket::Ptr dst) 187 | { 188 | Source::regPacketCallback(dst); 189 | 190 | if (pkt_cb_vec_.size() == 1) 191 | { 192 | driver_ptr_->regPacketCallback( 193 | std::bind(&SourceDriver::putPacket, this, std::placeholders::_1)); 194 | } 195 | } 196 | 197 | inline void SourceDriver::putPacket(const Packet& msg) 198 | { 199 | sendPacket(msg); 200 | } 201 | 202 | void SourceDriver::putPointCloud(std::shared_ptr msg) 203 | { 204 | point_cloud_queue_.push(msg); 205 | } 206 | 207 | void SourceDriver::processPointCloud() 208 | { 209 | while (!to_exit_process_) 210 | { 211 | std::shared_ptr msg = point_cloud_queue_.popWait(1000); 212 | if (msg.get() == NULL) 213 | { 214 | continue; 215 | } 216 | sendPointCloud(msg); 217 | 218 | free_point_cloud_queue_.push(msg); 219 | } 220 | } 221 | 222 | inline void SourceDriver::putException(const lidar::Error& msg) 223 | { 224 | switch (msg.error_code_type) 225 | { 226 | case lidar::ErrCodeType::INFO_CODE: 227 | RS_INFO << msg.toString() << RS_REND; 228 | break; 229 | case lidar::ErrCodeType::WARNING_CODE: 230 | RS_WARNING << msg.toString() << RS_REND; 231 | break; 232 | case lidar::ErrCodeType::ERROR_CODE: 233 | RS_ERROR << msg.toString() << RS_REND; 234 | break; 235 | } 236 | } 237 | 238 | } // namespace lidar 239 | } // namespace robosense 240 | -------------------------------------------------------------------------------- /src/source/source_packet_ros.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************************************************* 2 | Copyright (c) 2020 RoboSense 3 | All rights reserved 4 | 5 | By downloading, copying, installing or using the software you agree to this license. If you do not agree to this 6 | license, do not download, install, copy or use the software. 7 | 8 | License Agreement 9 | For RoboSense LiDAR SDK Library 10 | (3-clause BSD License) 11 | 12 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the 13 | following conditions are met: 14 | 15 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following 16 | disclaimer. 17 | 18 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following 19 | disclaimer in the documentation and/or other materials provided with the distribution. 20 | 21 | 3. Neither the names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used 22 | to endorse or promote products derived from this software without specific prior written permission. 23 | 24 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, 25 | INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 26 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 27 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 29 | WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 30 | THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | *********************************************************************************************************************/ 32 | 33 | #pragma once 34 | 35 | #include "source/source_driver.hpp" 36 | 37 | #ifdef ROS_FOUND 38 | 39 | #ifdef ENABLE_SOURCE_PACKET_LEGACY 40 | #include "msg/ros_msg/rslidar_packet_legacy.hpp" 41 | #include "msg/ros_msg/rslidar_scan_legacy.hpp" 42 | #endif 43 | 44 | #include "msg/ros_msg/rslidar_packet.hpp" 45 | #include 46 | 47 | namespace robosense 48 | { 49 | namespace lidar 50 | { 51 | 52 | inline Packet toRsMsg(const rslidar_msg::RslidarPacket& ros_msg) 53 | { 54 | Packet rs_msg; 55 | rs_msg.timestamp = ros_msg.header.stamp.toSec(); 56 | rs_msg.seq = ros_msg.header.seq; 57 | rs_msg.is_difop = ros_msg.is_difop; 58 | rs_msg.is_frame_begin = ros_msg.is_frame_begin; 59 | 60 | for (size_t i = 0; i < ros_msg.data.size(); i++) 61 | { 62 | rs_msg.buf_.emplace_back(ros_msg.data[i]); 63 | } 64 | 65 | return rs_msg; 66 | } 67 | 68 | class SourcePacketRos : public SourceDriver 69 | { 70 | public: 71 | 72 | virtual void init(const YAML::Node& config); 73 | 74 | SourcePacketRos(); 75 | 76 | private: 77 | 78 | #ifdef ENABLE_SOURCE_PACKET_LEGACY 79 | void putPacketLegacy(const rslidar_msgs::rslidarScan& msg); 80 | void putPacketDifopLegacy(const rslidar_msgs::rslidarPacket& msg); 81 | ros::Subscriber pkt_sub_legacy_; 82 | ros::Subscriber pkt_sub_difop_legacy_; 83 | LidarType lidar_type_; 84 | #endif 85 | 86 | void putPacket(const rslidar_msg::RslidarPacket& msg); 87 | ros::Subscriber pkt_sub_; 88 | 89 | std::unique_ptr nh_; 90 | }; 91 | 92 | SourcePacketRos::SourcePacketRos() 93 | : SourceDriver(SourceType::MSG_FROM_ROS_PACKET) 94 | { 95 | } 96 | 97 | void SourcePacketRos::init(const YAML::Node& config) 98 | { 99 | SourceDriver::init(config); 100 | 101 | nh_ = std::unique_ptr(new ros::NodeHandle()); 102 | 103 | #ifdef ENABLE_SOURCE_PACKET_LEGACY 104 | std::string lidar_type; 105 | yamlReadAbort(config["driver"], "lidar_type", lidar_type); 106 | lidar_type_ = strToLidarType(lidar_type); 107 | 108 | std::string ros_recv_topic_legacy; 109 | yamlRead(config["ros"], "ros_recv_packet_legacy_topic", 110 | ros_recv_topic_legacy, "rslidar_packets"); 111 | 112 | pkt_sub_legacy_ = nh_->subscribe(ros_recv_topic_legacy, 1, &SourcePacketRos::putPacketLegacy, this); 113 | pkt_sub_difop_legacy_ = nh_->subscribe(ros_recv_topic_legacy + "_difop", 1, &SourcePacketRos::putPacketDifopLegacy, this); 114 | #endif 115 | 116 | std::string ros_recv_topic; 117 | yamlRead(config["ros"], "ros_recv_packet_topic", 118 | ros_recv_topic, "rslidar_packets"); 119 | 120 | pkt_sub_ = nh_->subscribe(ros_recv_topic, 100, &SourcePacketRos::putPacket, this); 121 | 122 | } 123 | 124 | #ifdef ENABLE_SOURCE_PACKET_LEGACY 125 | inline Packet toRsMsg(const rslidar_msgs::rslidarPacket& ros_msg, LidarType lidar_type, bool isDifop) 126 | { 127 | size_t pkt_len = 1248; 128 | if (lidar_type == LidarType::RSM1) 129 | { 130 | pkt_len = isDifop ? 256 : 1210; 131 | } 132 | 133 | Packet rs_msg; 134 | for (size_t i = 0; i < pkt_len; i++) 135 | { 136 | rs_msg.buf_.emplace_back(ros_msg.data[i]); 137 | } 138 | 139 | return rs_msg; 140 | } 141 | 142 | void SourcePacketRos::putPacketLegacy(const rslidar_msgs::rslidarScan& msg) 143 | { 144 | for (uint32_t i = 0; i < msg.packets.size(); i++) 145 | { 146 | const rslidar_msgs::rslidarPacket& packet = msg.packets[i]; 147 | driver_ptr_->decodePacket(toRsMsg(packet, lidar_type_, false)); 148 | } 149 | } 150 | 151 | void SourcePacketRos::putPacketDifopLegacy(const rslidar_msgs::rslidarPacket& msg) 152 | { 153 | driver_ptr_->decodePacket(toRsMsg(msg, lidar_type_, true)); 154 | } 155 | #endif 156 | 157 | void SourcePacketRos::putPacket(const rslidar_msg::RslidarPacket& msg) 158 | { 159 | driver_ptr_->decodePacket(toRsMsg(msg)); 160 | } 161 | 162 | inline rslidar_msg::RslidarPacket toRosMsg(const Packet& rs_msg, const std::string& frame_id) 163 | { 164 | rslidar_msg::RslidarPacket ros_msg; 165 | ros_msg.header.stamp = ros_msg.header.stamp.fromSec(rs_msg.timestamp); 166 | ros_msg.header.seq = rs_msg.seq; 167 | ros_msg.header.frame_id = frame_id; 168 | ros_msg.is_difop = rs_msg.is_difop; 169 | ros_msg.is_frame_begin = rs_msg.is_frame_begin; 170 | 171 | for (size_t i = 0; i < rs_msg.buf_.size(); i++) 172 | { 173 | ros_msg.data.emplace_back(rs_msg.buf_[i]); 174 | } 175 | 176 | return ros_msg; 177 | } 178 | 179 | class DestinationPacketRos : public DestinationPacket 180 | { 181 | public: 182 | 183 | virtual void init(const YAML::Node& config); 184 | virtual void sendPacket(const Packet& msg); 185 | virtual ~DestinationPacketRos() = default; 186 | 187 | private: 188 | 189 | std::unique_ptr nh_; 190 | ros::Publisher pkt_pub_; 191 | std::string frame_id_; 192 | }; 193 | 194 | inline void DestinationPacketRos::init(const YAML::Node& config) 195 | { 196 | yamlRead(config["ros"], 197 | "ros_frame_id", frame_id_, "rslidar"); 198 | 199 | std::string ros_send_topic; 200 | yamlRead(config["ros"], "ros_send_packet_topic", 201 | ros_send_topic, "rslidar_packets"); 202 | 203 | nh_ = std::unique_ptr(new ros::NodeHandle()); 204 | pkt_pub_ = nh_->advertise(ros_send_topic, 100); 205 | } 206 | 207 | inline void DestinationPacketRos::sendPacket(const Packet& msg) 208 | { 209 | pkt_pub_.publish(toRosMsg(msg, frame_id_)); 210 | } 211 | 212 | } // namespace lidar 213 | } // namespace robosense 214 | 215 | #endif // ROS_FOUND 216 | 217 | #ifdef ROS2_FOUND 218 | #include "rslidar_msg/msg/rslidar_packet.hpp" 219 | #include 220 | #include 221 | 222 | namespace robosense 223 | { 224 | namespace lidar 225 | { 226 | 227 | inline Packet toRsMsg(const rslidar_msg::msg::RslidarPacket& ros_msg) 228 | { 229 | Packet rs_msg; 230 | rs_msg.timestamp = ros_msg.header.stamp.sec + double(ros_msg.header.stamp.nanosec) / 1e9; 231 | //rs_msg.seq = ros_msg.header.seq; 232 | rs_msg.is_difop = ros_msg.is_difop; 233 | rs_msg.is_frame_begin = ros_msg.is_frame_begin; 234 | 235 | for (size_t i = 0; i < ros_msg.data.size(); i++) 236 | { 237 | rs_msg.buf_.emplace_back(ros_msg.data[i]); 238 | } 239 | 240 | return rs_msg; 241 | } 242 | 243 | class SourcePacketRos : public SourceDriver 244 | { 245 | public: 246 | 247 | virtual void init(const YAML::Node& config); 248 | 249 | SourcePacketRos(); 250 | 251 | private: 252 | 253 | void putPacket(const rslidar_msg::msg::RslidarPacket::SharedPtr msg) const; 254 | 255 | std::shared_ptr node_ptr_; 256 | rclcpp::Subscription::SharedPtr pkt_sub_; 257 | }; 258 | 259 | SourcePacketRos::SourcePacketRos() 260 | : SourceDriver(SourceType::MSG_FROM_ROS_PACKET) 261 | { 262 | } 263 | 264 | void SourcePacketRos::init(const YAML::Node& config) 265 | { 266 | SourceDriver::init(config); 267 | 268 | std::string ros_recv_topic; 269 | yamlRead(config["ros"], "ros_recv_packet_topic", 270 | ros_recv_topic, "rslidar_packets"); 271 | 272 | static int node_index = 0; 273 | std::stringstream node_name; 274 | node_name << "rslidar_packets_source_" << node_index++; 275 | 276 | node_ptr_.reset(new rclcpp::Node(node_name.str())); 277 | pkt_sub_ = node_ptr_->create_subscription(ros_recv_topic, 100, 278 | std::bind(&SourcePacketRos::putPacket, this, std::placeholders::_1)); 279 | } 280 | 281 | void SourcePacketRos::putPacket(const rslidar_msg::msg::RslidarPacket::SharedPtr msg) const 282 | { 283 | driver_ptr_->decodePacket(toRsMsg(*msg)); 284 | } 285 | 286 | inline rslidar_msg::msg::RslidarPacket toRosMsg(const Packet& rs_msg, const std::string& frame_id) 287 | { 288 | rslidar_msg::msg::RslidarPacket ros_msg; 289 | ros_msg.header.stamp.sec = (uint32_t)floor(rs_msg.timestamp); 290 | ros_msg.header.stamp.nanosec = (uint32_t)round((rs_msg.timestamp - ros_msg.header.stamp.sec) * 1e9); 291 | //ros_msg.header.seq = rs_msg.seq; 292 | ros_msg.header.frame_id = frame_id; 293 | ros_msg.is_difop = rs_msg.is_difop; 294 | ros_msg.is_frame_begin = rs_msg.is_frame_begin; 295 | 296 | for (size_t i = 0; i < rs_msg.buf_.size(); i++) 297 | { 298 | ros_msg.data.emplace_back(rs_msg.buf_[i]); 299 | } 300 | 301 | return ros_msg; 302 | } 303 | 304 | class DestinationPacketRos : public DestinationPacket 305 | { 306 | public: 307 | 308 | virtual void init(const YAML::Node& config); 309 | virtual void sendPacket(const Packet& msg); 310 | virtual ~DestinationPacketRos() = default; 311 | 312 | private: 313 | 314 | std::shared_ptr node_ptr_; 315 | rclcpp::Publisher::SharedPtr pkt_pub_; 316 | std::string frame_id_; 317 | }; 318 | 319 | inline void DestinationPacketRos::init(const YAML::Node& config) 320 | { 321 | yamlRead(config["ros"], 322 | "ros_frame_id", frame_id_, "rslidar"); 323 | 324 | std::string ros_send_topic; 325 | yamlRead(config["ros"], "ros_send_packet_topic", 326 | ros_send_topic, "rslidar_packets"); 327 | 328 | static int node_index = 0; 329 | std::stringstream node_name; 330 | node_name << "rslidar_packets_destination_" << node_index++; 331 | 332 | node_ptr_.reset(new rclcpp::Node(node_name.str())); 333 | pkt_pub_ = node_ptr_->create_publisher(ros_send_topic, 100); 334 | } 335 | 336 | inline void DestinationPacketRos::sendPacket(const Packet& msg) 337 | { 338 | pkt_pub_->publish(toRosMsg(msg, frame_id_)); 339 | } 340 | 341 | } // namespace lidar 342 | } // namespace robosense 343 | 344 | #endif // ROS2_FOUND 345 | 346 | 347 | -------------------------------------------------------------------------------- /src/source/source_pointcloud_ros.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************************************************* 2 | Copyright (c) 2020 RoboSense 3 | All rights reserved 4 | 5 | By downloading, copying, installing or using the software you agree to this license. If you do not agree to this 6 | license, do not download, install, copy or use the software. 7 | 8 | License Agreement 9 | For RoboSense LiDAR SDK Library 10 | (3-clause BSD License) 11 | 12 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the 13 | following conditions are met: 14 | 15 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following 16 | disclaimer. 17 | 18 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following 19 | disclaimer in the documentation and/or other materials provided with the distribution. 20 | 21 | 3. Neither the names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used 22 | to endorse or promote products derived from this software without specific prior written permission. 23 | 24 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, 25 | INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 26 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 27 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 29 | WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 30 | THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | *********************************************************************************************************************/ 32 | 33 | #pragma once 34 | 35 | #include "source/source.hpp" 36 | 37 | #ifdef ROS_FOUND 38 | #include 39 | #include 40 | 41 | namespace robosense 42 | { 43 | namespace lidar 44 | { 45 | 46 | inline sensor_msgs::PointCloud2 toRosMsg(const LidarPointCloudMsg& rs_msg, const std::string& frame_id, bool send_by_rows) 47 | { 48 | sensor_msgs::PointCloud2 ros_msg; 49 | 50 | int fields = 4; 51 | #ifdef POINT_TYPE_XYZIRT 52 | fields = 6; 53 | #endif 54 | ros_msg.fields.clear(); 55 | ros_msg.fields.reserve(fields); 56 | 57 | if (send_by_rows) 58 | { 59 | ros_msg.width = rs_msg.width; 60 | ros_msg.height = rs_msg.height; 61 | } 62 | else 63 | { 64 | ros_msg.width = rs_msg.height; // exchange width and height to be compatible with pcl::PointCloud<> 65 | ros_msg.height = rs_msg.width; 66 | } 67 | 68 | int offset = 0; 69 | offset = addPointField(ros_msg, "x", 1, sensor_msgs::PointField::FLOAT32, offset); 70 | offset = addPointField(ros_msg, "y", 1, sensor_msgs::PointField::FLOAT32, offset); 71 | offset = addPointField(ros_msg, "z", 1, sensor_msgs::PointField::FLOAT32, offset); 72 | offset = addPointField(ros_msg, "intensity", 1, sensor_msgs::PointField::FLOAT32, offset); 73 | #ifdef POINT_TYPE_XYZIRT 74 | offset = addPointField(ros_msg, "ring", 1, sensor_msgs::PointField::UINT16, offset); 75 | offset = addPointField(ros_msg, "timestamp", 1, sensor_msgs::PointField::FLOAT64, offset); 76 | #endif 77 | 78 | #if 0 79 | std::cout << "off:" << offset << std::endl; 80 | #endif 81 | 82 | ros_msg.point_step = offset; 83 | ros_msg.row_step = ros_msg.width * ros_msg.point_step; 84 | ros_msg.is_dense = rs_msg.is_dense; 85 | ros_msg.data.resize(ros_msg.point_step * ros_msg.width * ros_msg.height); 86 | 87 | sensor_msgs::PointCloud2Iterator iter_x_(ros_msg, "x"); 88 | sensor_msgs::PointCloud2Iterator iter_y_(ros_msg, "y"); 89 | sensor_msgs::PointCloud2Iterator iter_z_(ros_msg, "z"); 90 | sensor_msgs::PointCloud2Iterator iter_intensity_(ros_msg, "intensity"); 91 | #ifdef POINT_TYPE_XYZIRT 92 | sensor_msgs::PointCloud2Iterator iter_ring_(ros_msg, "ring"); 93 | sensor_msgs::PointCloud2Iterator iter_timestamp_(ros_msg, "timestamp"); 94 | #endif 95 | 96 | if (send_by_rows) 97 | { 98 | for (size_t i = 0; i < rs_msg.height; i++) 99 | { 100 | for (size_t j = 0; j < rs_msg.width; j++) 101 | { 102 | const LidarPointCloudMsg::PointT& point = rs_msg.points[i + j * rs_msg.height]; 103 | 104 | *iter_x_ = point.x; 105 | *iter_y_ = point.y; 106 | *iter_z_ = point.z; 107 | *iter_intensity_ = point.intensity; 108 | 109 | ++iter_x_; 110 | ++iter_y_; 111 | ++iter_z_; 112 | ++iter_intensity_; 113 | 114 | #ifdef POINT_TYPE_XYZIRT 115 | *iter_ring_ = point.ring; 116 | *iter_timestamp_ = point.timestamp; 117 | 118 | ++iter_ring_; 119 | ++iter_timestamp_; 120 | #endif 121 | } 122 | } 123 | } 124 | else 125 | { 126 | for (size_t i = 0; i < rs_msg.points.size(); i++) 127 | { 128 | const LidarPointCloudMsg::PointT& point = rs_msg.points[i]; 129 | 130 | *iter_x_ = point.x; 131 | *iter_y_ = point.y; 132 | *iter_z_ = point.z; 133 | *iter_intensity_ = point.intensity; 134 | 135 | ++iter_x_; 136 | ++iter_y_;; 137 | ++iter_z_; 138 | ++iter_intensity_; 139 | 140 | #ifdef POINT_TYPE_XYZIRT 141 | *iter_ring_ = point.ring; 142 | *iter_timestamp_ = point.timestamp; 143 | 144 | ++iter_ring_; 145 | ++iter_timestamp_; 146 | #endif 147 | } 148 | } 149 | 150 | ros_msg.header.seq = rs_msg.seq; 151 | ros_msg.header.stamp = ros_msg.header.stamp.fromSec(rs_msg.timestamp); 152 | ros_msg.header.frame_id = frame_id; 153 | 154 | return ros_msg; 155 | } 156 | 157 | class DestinationPointCloudRos : public DestinationPointCloud 158 | { 159 | public: 160 | 161 | virtual void init(const YAML::Node& config); 162 | virtual void sendPointCloud(const LidarPointCloudMsg& msg); 163 | virtual ~DestinationPointCloudRos() = default; 164 | 165 | private: 166 | std::shared_ptr nh_; 167 | ros::Publisher pub_; 168 | std::string frame_id_; 169 | bool send_by_rows_; 170 | }; 171 | 172 | inline void DestinationPointCloudRos::init(const YAML::Node& config) 173 | { 174 | yamlRead(config["ros"], 175 | "ros_send_by_rows", send_by_rows_, false); 176 | 177 | bool dense_points; 178 | yamlRead(config["driver"], "dense_points", dense_points, false); 179 | if (dense_points) 180 | send_by_rows_ = false; 181 | 182 | yamlRead(config["ros"], 183 | "ros_frame_id", frame_id_, "rslidar"); 184 | 185 | std::string ros_send_topic; 186 | yamlRead(config["ros"], 187 | "ros_send_point_cloud_topic", ros_send_topic, "rslidar_points"); 188 | 189 | nh_ = std::unique_ptr(new ros::NodeHandle()); 190 | pub_ = nh_->advertise(ros_send_topic, 10); 191 | } 192 | 193 | inline void DestinationPointCloudRos::sendPointCloud(const LidarPointCloudMsg& msg) 194 | { 195 | pub_.publish(toRosMsg(msg, frame_id_, send_by_rows_)); 196 | } 197 | 198 | } // namespace lidar 199 | } // namespace robosense 200 | 201 | #endif // ROS_FOUND 202 | 203 | #ifdef ROS2_FOUND 204 | #include 205 | #include 206 | #include 207 | 208 | namespace robosense 209 | { 210 | namespace lidar 211 | { 212 | 213 | inline sensor_msgs::msg::PointCloud2 toRosMsg(const LidarPointCloudMsg& rs_msg, const std::string& frame_id, bool send_by_rows) 214 | { 215 | sensor_msgs::msg::PointCloud2 ros_msg; 216 | 217 | int fields = 4; 218 | #ifdef POINT_TYPE_XYZIRT 219 | fields = 6; 220 | #endif 221 | ros_msg.fields.clear(); 222 | ros_msg.fields.reserve(fields); 223 | 224 | if (send_by_rows) 225 | { 226 | ros_msg.width = rs_msg.width; 227 | ros_msg.height = rs_msg.height; 228 | } 229 | else 230 | { 231 | ros_msg.width = rs_msg.height; // exchange width and height to be compatible with pcl::PointCloud<> 232 | ros_msg.height = rs_msg.width; 233 | } 234 | 235 | int offset = 0; 236 | offset = addPointField(ros_msg, "x", 1, sensor_msgs::msg::PointField::FLOAT32, offset); 237 | offset = addPointField(ros_msg, "y", 1, sensor_msgs::msg::PointField::FLOAT32, offset); 238 | offset = addPointField(ros_msg, "z", 1, sensor_msgs::msg::PointField::FLOAT32, offset); 239 | offset = addPointField(ros_msg, "intensity", 1, sensor_msgs::msg::PointField::FLOAT32, offset); 240 | #ifdef POINT_TYPE_XYZIRT 241 | offset = addPointField(ros_msg, "ring", 1, sensor_msgs::msg::PointField::UINT16, offset); 242 | offset = addPointField(ros_msg, "timestamp", 1, sensor_msgs::msg::PointField::FLOAT64, offset); 243 | #endif 244 | 245 | #if 0 246 | std::cout << "off:" << offset << std::endl; 247 | #endif 248 | 249 | ros_msg.point_step = offset; 250 | ros_msg.row_step = ros_msg.width * ros_msg.point_step; 251 | ros_msg.is_dense = rs_msg.is_dense; 252 | ros_msg.data.resize(ros_msg.point_step * ros_msg.width * ros_msg.height); 253 | 254 | sensor_msgs::PointCloud2Iterator iter_x_(ros_msg, "x"); 255 | sensor_msgs::PointCloud2Iterator iter_y_(ros_msg, "y"); 256 | sensor_msgs::PointCloud2Iterator iter_z_(ros_msg, "z"); 257 | sensor_msgs::PointCloud2Iterator iter_intensity_(ros_msg, "intensity"); 258 | #ifdef POINT_TYPE_XYZIRT 259 | sensor_msgs::PointCloud2Iterator iter_ring_(ros_msg, "ring"); 260 | sensor_msgs::PointCloud2Iterator iter_timestamp_(ros_msg, "timestamp"); 261 | #endif 262 | 263 | if (send_by_rows) 264 | { 265 | for (size_t i = 0; i < rs_msg.height; i++) 266 | { 267 | for (size_t j = 0; j < rs_msg.width; j++) 268 | { 269 | const LidarPointCloudMsg::PointT& point = rs_msg.points[i + j * rs_msg.height]; 270 | 271 | *iter_x_ = point.x; 272 | *iter_y_ = point.y; 273 | *iter_z_ = point.z; 274 | *iter_intensity_ = point.intensity; 275 | 276 | ++iter_x_; 277 | ++iter_y_; 278 | ++iter_z_; 279 | ++iter_intensity_; 280 | 281 | #ifdef POINT_TYPE_XYZIRT 282 | *iter_ring_ = point.ring; 283 | *iter_timestamp_ = point.timestamp; 284 | 285 | ++iter_ring_; 286 | ++iter_timestamp_; 287 | #endif 288 | } 289 | } 290 | } 291 | else 292 | { 293 | for (size_t i = 0; i < rs_msg.points.size(); i++) 294 | { 295 | const LidarPointCloudMsg::PointT& point = rs_msg.points[i]; 296 | 297 | *iter_x_ = point.x; 298 | *iter_y_ = point.y; 299 | *iter_z_ = point.z; 300 | *iter_intensity_ = point.intensity; 301 | 302 | ++iter_x_; 303 | ++iter_y_; 304 | ++iter_z_; 305 | ++iter_intensity_; 306 | 307 | #ifdef POINT_TYPE_XYZIRT 308 | *iter_ring_ = point.ring; 309 | *iter_timestamp_ = point.timestamp; 310 | 311 | ++iter_ring_; 312 | ++iter_timestamp_; 313 | #endif 314 | } 315 | } 316 | 317 | ros_msg.header.stamp.sec = (uint32_t)floor(rs_msg.timestamp); 318 | ros_msg.header.stamp.nanosec = (uint32_t)round((rs_msg.timestamp - ros_msg.header.stamp.sec) * 1e9); 319 | ros_msg.header.frame_id = frame_id; 320 | 321 | return ros_msg; 322 | } 323 | 324 | class DestinationPointCloudRos : virtual public DestinationPointCloud 325 | { 326 | public: 327 | 328 | virtual void init(const YAML::Node& config); 329 | virtual void sendPointCloud(const LidarPointCloudMsg& msg); 330 | virtual ~DestinationPointCloudRos() = default; 331 | 332 | private: 333 | std::shared_ptr node_ptr_; 334 | rclcpp::Publisher::SharedPtr pub_; 335 | std::string frame_id_; 336 | bool send_by_rows_; 337 | }; 338 | 339 | inline void DestinationPointCloudRos::init(const YAML::Node& config) 340 | { 341 | yamlRead(config["ros"], 342 | "ros_send_by_rows", send_by_rows_, false); 343 | 344 | bool dense_points; 345 | yamlRead(config["driver"], "dense_points", dense_points, false); 346 | if (dense_points) 347 | send_by_rows_ = false; 348 | 349 | yamlRead(config["ros"], 350 | "ros_frame_id", frame_id_, "rslidar"); 351 | 352 | std::string ros_send_topic; 353 | yamlRead(config["ros"], 354 | "ros_send_point_cloud_topic", ros_send_topic, "rslidar_points"); 355 | 356 | static int node_index = 0; 357 | std::stringstream node_name; 358 | node_name << "rslidar_points_destination_" << node_index++; 359 | 360 | node_ptr_.reset(new rclcpp::Node(node_name.str())); 361 | pub_ = node_ptr_->create_publisher(ros_send_topic, 100); 362 | } 363 | 364 | inline void DestinationPointCloudRos::sendPointCloud(const LidarPointCloudMsg& msg) 365 | { 366 | pub_->publish(toRosMsg(msg, frame_id_, send_by_rows_)); 367 | } 368 | 369 | } // namespace lidar 370 | } // namespace robosense 371 | 372 | #endif 373 | 374 | --------------------------------------------------------------------------------