├── .clang-format ├── .gitignore ├── README.md ├── changelog_cn.md ├── changelog_en.md ├── doc ├── readme.md └── readme_cn.md ├── license ├── rslidar ├── CHANGELOG.rst ├── CMakeLists.txt └── package.xml ├── rslidar_driver ├── CMakeLists.txt ├── cfg │ └── rslidarNode.cfg ├── nodelet_rslidar.xml ├── package.xml └── src │ ├── CMakeLists.txt │ ├── input.cc │ ├── input.h │ ├── nodelet.cc │ ├── rsdriver.cpp │ ├── rsdriver.h │ └── rslidar_node.cpp ├── rslidar_msgs ├── CMakeLists.txt ├── msg │ ├── rslidarPacket.msg │ └── rslidarScan.msg └── package.xml ├── rslidar_pointcloud ├── CMakeLists.txt ├── cfg │ └── CloudNode.cfg ├── data │ ├── lidar1 │ │ ├── ChannelNum.csv │ │ ├── angle.csv │ │ └── curves.csv │ ├── lidar2 │ │ ├── ChannelNum.csv │ │ ├── angle.csv │ │ └── curves.csv │ ├── rs_bpearl │ │ ├── ChannelNum.csv │ │ └── angle.csv │ ├── rs_lidar_16 │ │ ├── ChannelNum.csv │ │ ├── angle.csv │ │ └── curves.csv │ └── rs_lidar_32 │ │ ├── ChannelNum.csv │ │ ├── CurveRate.csv │ │ ├── angle.csv │ │ └── curves.csv ├── launch │ ├── cloud_nodelet.launch │ ├── rs_bpearl.launch │ ├── rs_lidar_16.launch │ ├── rs_lidar_32.launch │ └── two_lidar.launch ├── nodelets.xml ├── package.xml ├── rviz_cfg │ └── rslidar.rviz └── src │ ├── CMakeLists.txt │ ├── cloud_node.cc │ ├── cloud_nodelet.cc │ ├── convert.cc │ ├── convert.h │ ├── rawdata.cc │ └── rawdata.h ├── rslidar_sync ├── CMakeLists.txt ├── README.md ├── launch │ ├── left_lidar.launch │ ├── middle_lidar.launch │ ├── right_lidar.launch │ ├── rslidar_sync_2lidar.launch │ └── rslidar_sync_3lidar.launch ├── package.xml ├── rviz_cfg │ └── time_sync.rviz └── src │ ├── timesync_2lidar.cpp │ └── timesync_3lidar.cpp └── test_doc └── test_guide_cn.md /.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 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # GNU global tags 2 | GTAGS 3 | GRTAGS 4 | GSYMS 5 | GPATH 6 | 7 | # Python byte-compile file 8 | __pycache__/ 9 | *.py[co] 10 | .ycm_extra_conf.py 11 | # editor backup files 12 | *~ 13 | \#*\# 14 | 15 | # backup files 16 | *.bak 17 | 18 | # Eclipse 19 | .cproject 20 | .project 21 | .pydevproject 22 | .settings/ 23 | 24 | # Visual Studio Code 25 | .vscode/ 26 | 27 | # CLion 28 | .idea/ 29 | cmake-build-debug/ 30 | 31 | # clang-format 32 | #.clang-format 33 | 34 | # ROSBag files 35 | *.bag 36 | 37 | # Environments 38 | .env 39 | .venv 40 | env/ 41 | venv/ 42 | ENV/ 43 | 44 | # Autoware Resources 45 | *.pcap 46 | *.pcd 47 | *.bag 48 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | This project is no longer maintained. Please use our new driver sdk [rslidar_sdk](https://github.com/RoboSense-LiDAR/rslidar_sdk). 2 | 3 | [rslidar_sdk](https://github.com/RoboSense-LiDAR/rslidar_sdk) supports ROS, ROS2, protobuf, and you can even use driver core [rs_driver](https://github.com/RoboSense-LiDAR/rs_driver) in Windows. 4 | 5 | [rs_driver](https://github.com/RoboSense-LiDAR/rs_driver) supports all kind of RoboSense's lidars by now. 6 | -------------------------------------------------------------------------------- /changelog_cn.md: -------------------------------------------------------------------------------- 1 | # RSLidar ROS驱动修改记录 2 | 3 | ### V1.0.0(2017-09-29) 4 | --- 5 | 初版提交 6 | 7 | ### V2.0.0(2017-11-23) 8 | --- 9 | * 1.加入距离随温度的补偿功能。 10 | * 2.移除OpenCV相关的代码 11 | 12 | ### V3.0.0(2018-05-10) 13 | --- 14 | * 1.加入RS-LiDAR-32的功能支持。 15 | * 2.将温度补偿更新到30~71℃。 16 | * 3.使用新的代码架构。 17 | * 4.改变反射率的计算方法。 18 | * 5.兼容不同的标定文件格式。 19 | * 6.加入多雷达时间同步功能。 20 | 21 | ### V3.1.0(2018-07-14) 22 | --- 23 | * 1.在反射率计算中加入温度补偿功能。 24 | * 2.移除package.xml中多余的依赖项。 25 | 26 | ### V4.0.0(2019-04-11) 27 | --- 28 | * 1.新增距离参数**max_distance**和**min_distance**到launch文件设置功能。 29 | * 2.加入difop包解析功能。 30 | * 3.兼容适配0.5cm分辨率雷达。 31 | * 4.修复1cm分辨率雷达解析错误问题。 32 | * 5.增加反射率计算mode=3，直接输出反射强度值，不需要计算标定公式计算。 33 | * 6.新增支持解析雷达**双回波模式**数据。 34 | * 7.将多雷达同步功能的获取时间戳代码从input.cc移到rsdriver.cpp。 35 | * 8.增加**resolution**和**intensity**通过launch文件设置，可以适配bag数据中没有记录difop包的情况。 36 | * 9.增加因为坐标变换引起的XYZ补偿代码。 37 | * 10.规范化代码。 38 | * 11.增加根据水平角度分帧的功能。 39 | 40 | ### V4.1.0(2019-09-11) 41 | --- 42 | * 1.修复不同回波模式下packet_rate参数计算错误问题。 43 | * 2.新增从RS32线的difop包中读取角度标定值。 44 | * 3.更改打印信息输出内容，便于识别打印信息来源。 45 | * 4.更改三角函数计算到查表方式，优化代码执行效率。 46 | * 5.更改不同雷达镜头到转体中心的坐标换。 47 | * 6.修改默认分帧方式到角度分帧，且修改包数分帧的报数对应18K点频设备。 48 | * 7.新增RSBPEARL配置。 49 | 50 | ### V4.1.0(2019-09-11) 51 | --- 52 | * 1.修复BPearl的角度解析错误。 53 | * 2.修复当LiDAR被设置了FOV后出现的角度差值计算错误。 -------------------------------------------------------------------------------- /changelog_en.md: -------------------------------------------------------------------------------- 1 | ## RSLidar ROS Driver Changelog 2 | 3 | ### V1.0.0(2017-09-29) 4 | --- 5 | first release for mutil node version 6 | 7 | ### V2.0.0(2017-11-23) 8 | --- 9 | * 1.Add the temperature compensation for distance. 10 | * 2.Remove the OpenCV code. 11 | 12 | ### V3.0.0(2018-05-10) 13 | --- 14 | * 1.Add new feature: add the RS-LiDAR-32 support 15 | * 2.Fix bug: modify the temperature compensation range to 30~71 degree centigrade. 16 | * 3.Use the new code structure. 17 | * 4.Add new feature: modify the intensity calculation formula. 18 | * 5.Add new feature: compatible with the different calibration files. 19 | * 6.Add new feature: add the multi lidar synchronization function. 20 | 21 | ### V3.1.0(2018-07-14) 22 | --- 23 | * 1.Add new feature: Add temperature compensation in internsity caculation. 24 | * 2.Remove uncessary dependency in package.xml 25 | 26 | ### V4.0.0(2019-04-11) 27 | --- 28 | * 1.Add new feature: add new parameter name and value in launch file where max_distance is used to set maximum distance and min_distance is used to set minimum distance. 29 | * 2.Add the difop packets parser code. 30 | * 3.Add new feature: compatible with firmware which's output distance resolution is 0.5cm. 31 | * 4.Fix bug: fix the wrong distance and coordination of point cloud when output distance resolution of firmware is 1cm. 32 | * 5.Add new feature: compatible with firmware which's output intensity directly without calibration formula (intensity_mode = 3). 33 | * 6.Add new feature: add 'Dual Return' packet parser support 34 | * 7.Move timestamp getting code for multiple lidar 's synchronization from input.cc to rsdriver.cpp 35 | * 8.Add 'Resolution' and 'intensity_mode' parameter into launch file so that it can compatible with the bag files that did not record the difop packets. 36 | * 9.Fix bug: add coordinate transformation compensation into the XYZ calculation 37 | * 10.Normalize the code 38 | * 11.Add the horizontal angle framing code 39 | 40 | ### V4.1.0(2019-09-11) 41 | --- 42 | * 1.Fix bug: fix the packet_rate calculation bug for different return mode. 43 | * 2.Add the function for reading the calibrated angle from difop of RS32. 44 | * 3.Update the log format to make it more clear to know the source of the information. 45 | * 4.Change the trigonometric function calculation to look-up table, this can improve the code performance efficiency. 46 | * 5.Change the coordinate transformation compensation for different lidars. 47 | * 6.Change the scan split way to angle by default. Change the packet_rate value corresponding to 18K sampling rate. 48 | * 7.Add RSBPEARL configuration support. 49 | 50 | ### V4.1.1(2019-11-29) 51 | --- 52 | * 1. Fix bug: fix the angle parsing bug for BPearl 53 | * 2. Fix bug: fix the angle difference error when we set the specific FOV of LiDAR 54 | -------------------------------------------------------------------------------- /doc/readme.md: -------------------------------------------------------------------------------- 1 | #### 1. Prerequisites 2 | (1) Install a ubuntu PC. We suggested Ubuntu 14.04 or Ubuntu 16.04. Please do not use virtual machine. 3 | (2) Install ros full-desktop version. We tried Indigo and Kinect. 4 | (3) Please also install libpcap-dev. 5 | 6 | #### 2. Install 7 | (1). Copy the whole rslidar ROS driver directory into ROS workspace, i.e "~/catkin_ws/src". 8 | 9 | (2). Check the file attributes: 10 | 11 | ``` 12 | cd ~/catkin_ws/src/ros_rslidar/rslidar_drvier 13 | chmod 777 cfg/* 14 | cd ~/catkin_ws/src/ros_rslidar/rslidar_pointcloud 15 | chmod 777 cfg/* 16 | ``` 17 | 18 | (3). Then to compile the source code and to install it: 19 | 20 | ``` 21 | cd ~/catkin_ws 22 | catkin_make 23 | ``` 24 | #### 3. Configure PC IP 25 | By default, the RSLIDAR is configured to **192.168.1.200** as its device IP and **192.168.1.102** as PC IP that it would communicate. The default **MSOP port is 6699** while **DIFOP port is 7788**. 26 | So you need configure your PC IP as a static one **192.168.1.102**. 27 | 28 | #### 4. Run as independent node 29 | We have provide example launch files under rslidar_pointcloud/launch, we can run the launch file to view the point cloud data. For example, if we want to view RS-LiDAR-16 real time data: 30 | (1). Open a new terminal and run: 31 | 32 | ``` 33 | cd ~/catkin_ws 34 | source devel/setup.bash 35 | roslaunch rslidar_pointcloud rs_lidar_16.launch 36 | ``` 37 | 38 | (2). Open a new terminal and run: 39 | 40 | ``` 41 | rviz 42 | ``` 43 | Set the Fixed Frame to "**rslidar**". 44 | Add a Pointcloud2 type and set the topic to "**rslidar_points**". 45 | 46 | #### 5. Run as nodelet 47 | We can also run the driver node and cloud node as a nodelet. 48 | Open a new terminal and run: 49 | 50 | ``` 51 | cd ~/catkin_ws 52 | source devel/setup.bash 53 | roslaunch rslidar_pointcloud cloud_nodelet.launch 54 | ``` 55 | Then we can run view the pointcloud via "rviz" 56 | 57 | #### 6. About the lidar calibration parameters 58 | Under "**rslidar_pointcloud/data**" directory, you can find the lidar calibration parameters files for the exact sensor. By default the launch file "rs_lidar_16.launch" load the three files: 59 | - rslidar_pointcloud/data/rs_lidar_16/angle.csv 60 | - rslidar_pointcloud/data/rs_lidar_16/ChannelNum.csv 61 | - rslidar_pointcloud/data/rs_lidar_16/curves.csv. 62 | 63 | If you have more than one RSLIDAR, you can create new sub-directories under the "**rslidar_pointcloud/data/**", and put the data files into it.Then you need rewrite the launch file to start you lidar. We have put an example launch file "two_lidar.launch" to load two lidars together for reference. 64 | 65 | **Begin from 2018.09.01** 66 | In the launch file, we could set the MSOP port and DIFOP port. 67 | MSOP port is used for receive the main point cloud data, while DIFOP port is used for receive the device information data. If we set the right DIFOP port, we will get the lidar calibration parameters from the DIFOP packets not from the files, so can ignore the local lidar calibration files. About how to check the DIFOP port, please reference the Appendix G in RS-LiDARR manual. 68 | 69 | ### 3.Parameters instruction 70 | There are some parameters need to be set in the launch files for operating LiDAR correctly. 71 | * model: Assign the LiDAR model.There are three selection:RS32,RS16 and RSBPEARL. 72 | * device_ip：Set the LiDAR network ip address. 73 | * msop_port：LiDAR MSOP network port number. 74 | * difop_port：LiDAR DIFOP network port number 75 | * lidar_param_path：The location for the calibration files. 76 | * pcap：The location for the pcap which needs to be parse. After setting this parameter, the online connection for LiDAR will stop. 77 | * curves_path：Assign the location for reflectivity calibration files. Just valid for intensity_mode=1 and intensity_mode=2. 78 | * angle_path：Assign the location for angle calibration files. 79 | * channel_path：Assign the location for distance calibration files. 80 | * curves_rate_path：Assign the location for reflectivity coefficient. This can be set to null by default. 81 | * max_distance：The upper limit for distance. 82 | * min_distance：The lower limit for distance. 83 | * resolution_type：The LSB for distance."0.5cm" or "1cm" . 84 | * intensity_mode：The mode for reflectivity.1, 2 or 3. 85 | * cut_angle：The scan split way for using angle. -------------------------------------------------------------------------------- /doc/readme_cn.md: -------------------------------------------------------------------------------- 1 | # RSLidar ROS 驱动说明 2 | 3 | ### 1. 依赖 4 | * 1.安装ubuntu系统的电脑，推荐Ubuntu14.04或Ubuntu16.04，不要使用虚拟机 5 | * 2.安装 ROS full-desktop版本。建议安装Indigo或Kinect 6 | * 3.安装libpcap-dev 7 | 8 | ### 2. 安装和使用 9 | * 1.将这个rslidar驱动拷贝到ROS工作区，例如“~/catkin_ws/src" 10 | * 2.设置文件访问属性 11 | ``` 12 | cd ~/catkin_ws/src/ros_rslidar/rslidar_drvier 13 | chmod 777 cfg/* 14 | cd ~/catkin_ws/src/ros_rslidar/rslidar_pointcloud 15 | chmod 777 cfg/* 16 | ``` 17 | * 3.编译和安装 18 | ``` 19 | cd ~/catkin_ws 20 | catkin_make 21 | ``` 22 | 23 | * 4.设置PC ip 24 | 默认情况下，RSLIDAR雷达设备ip设置为**192.168.1.200**，而PC ip则设置为**192.168.1.102**；同时默认**MSOP**端口为**6699**，**DIFOP**端口为**7788**. 所以需将PC ip设置为静态ip **192.168.1.102**才能和雷达通信。 25 | 26 | * 5.以node方式运行 27 | 这里提供示例launch文件，可以运行launch文件来观看点云数据。观看RS-LIDAR-16实时点云数据例子操作如下： 28 | 29 | * 1)打开一个终端并执行以下命令 30 | ``` 31 | cd ~/catkin_ws 32 | source devel/setup.bash 33 | roslaunch rslidar_pointcloud rs_lidar_16.launch 34 | ``` 35 | * 2)新开一个终端并执行命令： 36 | ``` 37 | rviz 38 | ``` 39 | 然后设置Fixed Frame为**“rslidar”**，接着添加Pointcloud2 类型和设置topic为**“rslidar_points"**。 40 | 41 | * 6.以nodelet方式运行 42 | 同时支持driver node和cloud node以nodelet方式运行。打开终端并执行以下命令： 43 | ``` 44 | cd ~/catkin_ws 45 | source devel/setup.bash 46 | roslaunch rslidar_pointcloud cloud_nodelet.launch 47 | ``` 48 | 然后运行“rviz”来观看点云。 49 | 50 | * 7.雷达校准参数 51 | **“rslidar_pointcloud/data”**目录下，存放雷达的校准参数文件。通过默认launch文件“rs_lidar_16.launch”加载以下3个文件： 52 | 53 | * rslidar_pointcloud/data/rs_lidar_16/angle.csv 54 | * rslidar_pointcloud/data/rs_lidar_16/ChannelNum.csv 55 | * rslidar_pointcloud/data/rs_lidar_16/curves.csv. 56 | 57 | 如果要新增加雷达，可在**“rslidar_pointcloud/data/”**新建子目录，然后将校准参数文件放到子目录，接着修改launch文件，启动雷达雷达即可。launch文件修改可参考two_lidar.launch文件。 58 | 59 | **2018.09.01**之后在launch文件，可实现设置MSOP和DIFOP的端口。MSOP端口用于接收点云数据；而DIFOP端口用于接收雷达信息数据。如果DIFOP端口设置正确，可通过DIFOP数据包获取雷达的校准参数，而不用再通过校准文件，那可以忽略本地的雷达校准文件。如何检查DIFOP端口，可参考RS_LiDAR用户手册的附录G章节。 60 | 61 | ### 3.参数说明 62 | 雷达有一些参数需要设置，需要通过launch文件进行传递。 63 | * model: 指定雷达类型。目前三个选项 RS32、RS16 和RSBPEARL，分别指32线、16线、补盲雷达 64 | * device_ip：雷达ip 65 | * msop_port：MSOP数据包接收端口 66 | * difop_port：DIFOP数据包接收端口 67 | * lidar_param_path：雷达标定参数文件存放位置 68 | * pcap：要解析的pcap包存放路径。注意指定此参数，则驱动就不会接收从雷达发出的数据 69 | * curves_path：指定反射率标定文件curves.csv完整路径。针对intensity_mode为1和2。 70 | * angle_path：指定角度标定文件angle.csv完整路径 71 | * channel_path：指定距离补偿文件ChannelNum.csv完整路径。一般使用默认全0参数即可。 72 | * curves_rate_path：指定反射率洗漱标定文件CurveRate.csv完整路径。一般不需要 73 | * max_distance：指定点云距离上限 74 | * min_distance：指定点云距离下限 75 | * resolution_type：指定雷达距离解析分辨类型，"0.5cm" 和 "1cm" 二选一 76 | * intensity_mode：指定雷达反射率模式，分别为1,2,3 三选一 77 | * cut_angle：采用角度分帧方式 -------------------------------------------------------------------------------- /license: -------------------------------------------------------------------------------- 1 | Copyright (c) 2018-2020 RoboSense, and others 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 ROS 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 | -------------------------------------------------------------------------------- /rslidar/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | Change history 2 | ============== 3 | 4 | 1.0.0 (2018-12-03): first release for mutil node version 5 | -------------------------------------------------------------------------------- /rslidar/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(rslidar) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /rslidar/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rslidar 4 | 1.1.0 5 | 6 | Basic ROS support for the Robosense 3D LIDARs. 7 | 8 | Tony Zhang 9 | Tony Zhang 10 | BSD 11 | 12 | catkin 13 | 14 | rslidar_driver 15 | rslidar_msgs 16 | rslidar_pointcloud 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /rslidar_driver/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(rslidar_driver) 3 | 4 | add_compile_options(-std=c++11) 5 | set(CMAKE_BUILD_TYPE Release)#RelWithDebInfo 6 | ## Find catkin macros and libraries 7 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 8 | ## is used, also find other catkin packages 9 | set(${PROJECT_NAME}_CATKIN_DEPS 10 | angles 11 | pcl_ros 12 | roscpp 13 | roslib 14 | sensor_msgs 15 | tf 16 | dynamic_reconfigure 17 | diagnostic_updater 18 | rslidar_msgs 19 | nodelet 20 | ) 21 | 22 | set(libpcap_LIBRARIES -lpcap) 23 | 24 | find_package(catkin REQUIRED COMPONENTS 25 | ${${PROJECT_NAME}_CATKIN_DEPS} 26 | pcl_conversions 27 | rospy 28 | std_msgs 29 | genmsg 30 | cv_bridge 31 | message_generation 32 | ) 33 | 34 | find_package(Boost COMPONENTS signals) 35 | find_package(Boost REQUIRED COMPONENTS thread) 36 | find_package(PkgConfig REQUIRED) 37 | 38 | 39 | include_directories(${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS} 40 | ${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake) 41 | 42 | generate_dynamic_reconfigure_options(cfg/rslidarNode.cfg) 43 | 44 | catkin_package( 45 | CATKIN_DEPENDS ${${PROJECT_NAME}_CATKIN_DEPS} 46 | CATKIN_DEPENDS message_runtime std_msgs 47 | ) 48 | 49 | add_subdirectory(src) 50 | 51 | -------------------------------------------------------------------------------- /rslidar_driver/cfg/rslidarNode.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "rslidar_driver" 3 | NODE_NAME = "fullscan_puber" 4 | PARAMS_NAME = "rslidarNode" 5 | 6 | from math import pi 7 | from dynamic_reconfigure.parameter_generator_catkin import * 8 | 9 | gen = ParameterGenerator() 10 | 11 | gen.add("time_offset", double_t, 0, "A manually calibrated offset (in seconds) to add to the timestamp before publication of a message.", 12 | 0.0, -1.0, 1.0) 13 | 14 | exit(gen.generate(PACKAGE, NODE_NAME, PARAMS_NAME)) 15 | -------------------------------------------------------------------------------- /rslidar_driver/nodelet_rslidar.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | Publish raw rslidar data packets. 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /rslidar_driver/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rslidar_driver 4 | 1.0.0 5 | segmentation 6 | 7 | Tony 8 | Tony Zhang 9 | BSD 10 | 11 | catkin 12 | 13 | angles 14 | nodelet 15 | pcl_conversions 16 | pcl_ros 17 | pluginlib 18 | roscpp 19 | roslib 20 | sensor_msgs 21 | tf 22 | dynamic_reconfigure 23 | roslaunch 24 | rostest 25 | tf2_ros 26 | message_generation 27 | rospy 28 | std_msgs 29 | rslidar_input 30 | pcl_conversions 31 | pcl_ros 32 | libpcl-all-dev 33 | rslidar_msgs 34 | diagnostic_updater 35 | 36 | 37 | angles 38 | pcl_ros 39 | nodelet 40 | pluginlib 41 | 42 | roscpp 43 | roslib 44 | rospy 45 | sensor_msgs 46 | tf 47 | 48 | dynamic_reconfigure 49 | message_runtime 50 | pcl_conversions 51 | pcl_ros 52 | libpcl-all 53 | std_msgs 54 | rslidar_msgs 55 | diagnostic_updater 56 | 57 | 58 | 59 | 60 | 61 | 62 | -------------------------------------------------------------------------------- /rslidar_driver/src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(rslidar_input input.cc) 2 | target_link_libraries(rslidar_input 3 | ${catkin_LIBRARIES} 4 | ${libpcap_LIBRARIES}) 5 | 6 | add_library(rslidar_driver rsdriver.cpp) 7 | target_link_libraries(rslidar_driver 8 | rslidar_input 9 | ${catkin_LIBRARIES}) 10 | 11 | # build the nodelet version 12 | add_library(driver_nodelet nodelet.cc rsdriver.cpp) 13 | target_link_libraries(driver_nodelet 14 | rslidar_input 15 | ${catkin_LIBRARIES} 16 | ) 17 | 18 | add_executable(rslidar_node rslidar_node.cpp) 19 | 20 | if(catkin_EXPORTED_TARGETS) 21 | add_dependencies(rslidar_input ${catkin_EXPORTED_TARGETS}) 22 | endif() 23 | 24 | target_link_libraries(rslidar_node 25 | rslidar_driver 26 | rslidar_input 27 | ${catkin_LIBRARIES} 28 | ${libpcap_LIBRARIES} 29 | ) 30 | 31 | -------------------------------------------------------------------------------- /rslidar_driver/src/input.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2018-2020 Robosense Authors 3 | * License: Modified BSD Software License Agreement 4 | * 5 | * $Id$ 6 | */ 7 | 8 | /** \file 9 | * 10 | * Input classes for the Robosense 3D LIDAR: 11 | * 12 | * Input -- base class used to access the data independently of 13 | * its source 14 | * 15 | * InputSocket -- derived class reads live data from the device 16 | * via a UDP socket 17 | * 18 | * InputPCAP -- derived class provides a similar interface from a 19 | * PCAP dump 20 | */ 21 | #include "input.h" 22 | 23 | extern volatile sig_atomic_t flag; 24 | namespace rslidar_driver 25 | { 26 | static const size_t packet_size = sizeof(rslidar_msgs::rslidarPacket().data); 27 | 28 | //////////////////////////////////////////////////////////////////////// 29 | // Input base class implementation 30 | //////////////////////////////////////////////////////////////////////// 31 | 32 | /** @brief constructor 33 | * 34 | * @param private_nh ROS private handle for calling node. 35 | * @param port UDP port number. 36 | */ 37 | Input::Input(ros::NodeHandle private_nh, uint16_t port) : private_nh_(private_nh), port_(port) 38 | { 39 | npkt_update_flag_ = false; 40 | cur_rpm_ = 600; 41 | return_mode_ = 1; 42 | 43 | private_nh.param("device_ip", devip_str_, std::string("")); 44 | if (!devip_str_.empty()) 45 | { 46 | ROS_INFO_STREAM("[driver][input] accepting packets from IP address: " << devip_str_); 47 | } 48 | } 49 | 50 | int Input::getRpm(void) 51 | { 52 | return cur_rpm_; 53 | } 54 | 55 | int Input::getReturnMode(void) 56 | { 57 | return return_mode_; 58 | } 59 | 60 | bool Input::getUpdateFlag(void) 61 | { 62 | return npkt_update_flag_; 63 | } 64 | 65 | void Input::clearUpdateFlag(void) 66 | { 67 | npkt_update_flag_ = false; 68 | } 69 | //////////////////////////////////////////////////////////////////////// 70 | // InputSocket class implementation 71 | //////////////////////////////////////////////////////////////////////// 72 | 73 | /** @brief constructor 74 | * 75 | * @param private_nh ROS private handle for calling node. 76 | * @param port UDP port number 77 | */ 78 | InputSocket::InputSocket(ros::NodeHandle private_nh, uint16_t port) : Input(private_nh, port) 79 | { 80 | sockfd_ = -1; 81 | 82 | if (!devip_str_.empty()) 83 | { 84 | inet_aton(devip_str_.c_str(), &devip_); 85 | } 86 | 87 | ROS_INFO_STREAM("[driver][socket] Opening UDP socket: port " << port); 88 | sockfd_ = socket(PF_INET, SOCK_DGRAM, 0); 89 | if (sockfd_ == -1) 90 | { 91 | ROS_ERROR("[driver][socket] create socket fail"); 92 | return; 93 | } 94 | 95 | int opt = 1; 96 | if (setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (const void*)&opt, sizeof(opt))) 97 | { 98 | ROS_ERROR("[driver][socket] setsockopt fail"); 99 | return; 100 | } 101 | 102 | sockaddr_in my_addr; // my address information 103 | memset(&my_addr, 0, sizeof(my_addr)); // initialize to zeros 104 | my_addr.sin_family = AF_INET; // host byte order 105 | my_addr.sin_port = htons(port); // port in network byte order 106 | my_addr.sin_addr.s_addr = INADDR_ANY; // automatically fill in my IP 107 | 108 | if (bind(sockfd_, (sockaddr*)&my_addr, sizeof(sockaddr)) == -1) 109 | { 110 | ROS_ERROR("[driver][socket] socket bind fail"); 111 | return; 112 | } 113 | 114 | if (fcntl(sockfd_, F_SETFL, O_NONBLOCK | FASYNC) < 0) 115 | { 116 | ROS_ERROR("[driver][socket] fcntl fail"); 117 | return; 118 | } 119 | } 120 | 121 | /** @brief destructor */ 122 | InputSocket::~InputSocket(void) 123 | { 124 | (void)close(sockfd_); 125 | } 126 | 127 | /** @brief Get one rslidar packet. */ 128 | int InputSocket::getPacket(rslidar_msgs::rslidarPacket* pkt, const double time_offset) 129 | { 130 | double time1 = ros::Time::now().toSec(); 131 | struct pollfd fds[1]; 132 | fds[0].fd = sockfd_; 133 | fds[0].events = POLLIN; 134 | static const int POLL_TIMEOUT = 1000; // one second (in msec) 135 | 136 | sockaddr_in sender_address; 137 | socklen_t sender_address_len = sizeof(sender_address); 138 | while (flag == 1) 139 | { 140 | // Receive packets that should now be available from the 141 | // socket using a blocking read. 142 | // poll() until input available 143 | do 144 | { 145 | int retval = poll(fds, 1, POLL_TIMEOUT); 146 | if (retval < 0) // poll() error? 147 | { 148 | if (errno != EINTR) 149 | { 150 | ROS_ERROR("[driver][socket] poll() error: %s", strerror(errno)); 151 | } 152 | return 1; 153 | } 154 | if (retval == 0) // poll() timeout? 155 | { 156 | ROS_WARN("[driver][socket] Rslidar poll() timeout"); 157 | 158 | char buffer_data[8] = "re-con"; 159 | memset(&sender_address, 0, sender_address_len); // initialize to zeros 160 | sender_address.sin_family = AF_INET; // host byte order 161 | sender_address.sin_port = htons(MSOP_DATA_PORT_NUMBER); // port in network byte order, set any value 162 | sender_address.sin_addr.s_addr = devip_.s_addr; // automatically fill in my IP 163 | sendto(sockfd_, &buffer_data, strlen(buffer_data), 0, (sockaddr*)&sender_address, sender_address_len); 164 | return 1; 165 | } 166 | if ((fds[0].revents & POLLERR) || (fds[0].revents & POLLHUP) || (fds[0].revents & POLLNVAL)) // device error? 167 | { 168 | ROS_ERROR("[driver][socket] poll() reports Rslidar error"); 169 | return 1; 170 | } 171 | } while ((fds[0].revents & POLLIN) == 0); 172 | ssize_t nbytes = recvfrom(sockfd_, &pkt->data[0], packet_size, 0, (sockaddr*)&sender_address, &sender_address_len); 173 | 174 | if (nbytes < 0) 175 | { 176 | if (errno != EWOULDBLOCK) 177 | { 178 | ROS_ERROR("[driver][socket] recvfail"); 179 | return 1; 180 | } 181 | } 182 | else if ((size_t)nbytes == packet_size) 183 | { 184 | if (devip_str_ != "" && sender_address.sin_addr.s_addr != devip_.s_addr) 185 | { 186 | continue; 187 | } 188 | else 189 | { 190 | break; // done 191 | } 192 | } 193 | 194 | ROS_WARN_STREAM("[driver][socket] incomplete rslidar packet read: " << nbytes << " bytes"); 195 | } 196 | if (flag == 0) 197 | { 198 | abort(); 199 | } 200 | 201 | if (pkt->data[0] == 0xA5 && pkt->data[1] == 0xFF && pkt->data[2] == 0x00 && pkt->data[3] == 0x5A) 202 | {//difop 203 | int rpm = (pkt->data[8]<<8)|pkt->data[9]; 204 | int mode = 1; 205 | 206 | if ((pkt->data[45] == 0x08 && pkt->data[46] == 0x02 && pkt->data[47] >= 0x09) || (pkt->data[45] > 0x08) 207 | || (pkt->data[45] == 0x08 && pkt->data[46] > 0x02)) 208 | { 209 | if (pkt->data[300] != 0x01 && pkt->data[300] != 0x02) 210 | { 211 | mode = 0; 212 | } 213 | } 214 | 215 | if (cur_rpm_ != rpm || return_mode_ != mode) 216 | { 217 | cur_rpm_ = rpm; 218 | return_mode_ = mode; 219 | 220 | npkt_update_flag_ = true; 221 | } 222 | } 223 | // Average the times at which we begin and end reading. Use that to 224 | // estimate when the scan occurred. Add the time offset. 225 | double time2 = ros::Time::now().toSec(); 226 | pkt->stamp = ros::Time((time2 + time1) / 2.0 + time_offset); 227 | 228 | return 0; 229 | } 230 | 231 | //////////////////////////////////////////////////////////////////////// 232 | // InputPCAP class implementation 233 | //////////////////////////////////////////////////////////////////////// 234 | 235 | /** @brief constructor 236 | * 237 | * @param private_nh ROS private handle for calling node. 238 | * @param port UDP port number 239 | * @param packet_rate expected device packet frequency (Hz) 240 | * @param filename PCAP dump file name 241 | */ 242 | InputPCAP::InputPCAP(ros::NodeHandle private_nh, uint16_t port, double packet_rate, std::string filename, 243 | bool read_once, bool read_fast, double repeat_delay) 244 | : Input(private_nh, port), packet_rate_(packet_rate), filename_(filename) 245 | { 246 | pcap_ = NULL; 247 | empty_ = true; 248 | 249 | // get parameters using private node handle 250 | private_nh.param("read_once", read_once_, false); 251 | private_nh.param("read_fast", read_fast_, false); 252 | private_nh.param("repeat_delay", repeat_delay_, 0.0); 253 | 254 | if (read_once_) 255 | { 256 | ROS_INFO("[driver][pcap] Read input file only once."); 257 | } 258 | if (read_fast_) 259 | { 260 | ROS_INFO("[driver][pcap] Read input file as quickly as possible."); 261 | } 262 | if (repeat_delay_ > 0.0) 263 | { 264 | ROS_INFO("[driver][pcap] Delay %.3f seconds before repeating input file.", repeat_delay_); 265 | } 266 | 267 | // Open the PCAP dump file 268 | // ROS_INFO("Opening PCAP file \"%s\"", filename_.c_str()); 269 | ROS_INFO_STREAM("[driver][pcap] Opening PCAP file " << filename_); 270 | if ((pcap_ = pcap_open_offline(filename_.c_str(), errbuf_)) == NULL) 271 | { 272 | ROS_FATAL("[driver][pcap] Error opening rslidar socket dump file."); 273 | return; 274 | } 275 | 276 | std::stringstream filter; 277 | if (devip_str_ != "") // using specific IP? 278 | { 279 | filter << "src host " << devip_str_ << " && "; 280 | } 281 | filter << "udp dst port " << port; 282 | int ret = pcap_compile(pcap_, &pcap_packet_filter_, filter.str().c_str(), 1, PCAP_NETMASK_UNKNOWN); 283 | if (ret < 0) 284 | { 285 | ROS_FATAL_STREAM("[driver][pcap] pcap compile filter fail. filter: "<= 0) 306 | { 307 | // Skip packets not for the correct port and from the 308 | // selected IP address. 309 | if (!devip_str_.empty() && (0 == pcap_offline_filter(&pcap_packet_filter_, header, pkt_data))) 310 | continue; 311 | 312 | // Keep the reader from blowing through the file. 313 | if (read_fast_ == false) 314 | packet_rate_.sleep(); 315 | 316 | 317 | memcpy(&pkt->data[0], pkt_data + 42, packet_size); 318 | 319 | if (pkt->data[0] == 0xA5 && pkt->data[1] == 0xFF && pkt->data[2] == 0x00 && pkt->data[3] == 0x5A) 320 | {//difop 321 | int rpm = (pkt->data[8]<<8)|pkt->data[9]; 322 | int mode = 1; 323 | 324 | if ((pkt->data[45] == 0x08 && pkt->data[46] == 0x02 && pkt->data[47] >= 0x09) || (pkt->data[45] > 0x08) 325 | || (pkt->data[45] == 0x08 && pkt->data[46] > 0x02)) 326 | { 327 | if (pkt->data[300] != 0x01 && pkt->data[300] != 0x02) 328 | { 329 | mode = 0; 330 | } 331 | } 332 | 333 | if (cur_rpm_ != rpm || return_mode_ != mode) 334 | { 335 | cur_rpm_ = rpm; 336 | return_mode_ = mode; 337 | 338 | npkt_update_flag_ = true; 339 | } 340 | } 341 | 342 | pkt->stamp = ros::Time::now(); // time_offset not considered here, as no 343 | // synchronization required 344 | empty_ = false; 345 | return 0; // success 346 | } 347 | 348 | if (empty_) // no data in file? 349 | { 350 | ROS_ERROR("[driver][pcap] Error %d reading rslidar packet: %s", res, pcap_geterr(pcap_)); 351 | return -1; 352 | } 353 | 354 | if (read_once_) 355 | { 356 | ROS_INFO("[driver][pcap] end of file reached -- done reading."); 357 | return -1; 358 | } 359 | 360 | if (repeat_delay_ > 0.0) 361 | { 362 | ROS_INFO("[driver][pcap] end of file reached -- delaying %.3f seconds.", repeat_delay_); 363 | usleep(rint(repeat_delay_ * 1000000.0)); 364 | } 365 | 366 | ROS_INFO("[driver][pcap] replaying rslidar dump file"); 367 | 368 | // I can't figure out how to rewind the file, because it 369 | // starts with some kind of header. So, close the file 370 | // and reopen it with pcap. 371 | pcap_close(pcap_); 372 | pcap_ = pcap_open_offline(filename_.c_str(), errbuf_); 373 | empty_ = true; // maybe the file disappeared? 374 | } // loop back and try again 375 | 376 | if (flag == 0) 377 | { 378 | abort(); 379 | } 380 | } 381 | } 382 | -------------------------------------------------------------------------------- /rslidar_driver/src/input.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2018-2020 Robosense Authors 3 | * License: Modified BSD Software License Agreement 4 | * 5 | * $Id$ 6 | */ 7 | 8 | /** \file 9 | * 10 | * Input classes for the Robosense LIDAR: 11 | * 12 | * Input -- base class used to access the data independently of 13 | * its source 14 | * 15 | * InputSocket -- derived class reads live data from the device 16 | * via a UDP socket 17 | * 18 | * InputPCAP -- derived class provides a similar interface from a 19 | * PCAP dump 20 | */ 21 | 22 | #ifndef __RSLIDAR_INPUT_H_ 23 | #define __RSLIDAR_INPUT_H_ 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | namespace rslidar_driver 43 | { 44 | static uint16_t MSOP_DATA_PORT_NUMBER = 6699; // rslidar default data port on PC 45 | static uint16_t DIFOP_DATA_PORT_NUMBER = 7788; // rslidar default difop data port on PC 46 | /** 47 | * 从在线的网络数据或离线的网络抓包数据（pcap文件）中提取出lidar的原始数据，即packet数据包 48 | * @brief The Input class, 49 | * 50 | * @param private_nh 一个NodeHandled,用于通过节点传递参数 51 | * @param port 52 | * @returns 0 if successful, 53 | * -1 if end of file 54 | * >0 if incomplete packet (is this possible?) 55 | */ 56 | class Input 57 | { 58 | public: 59 | Input(ros::NodeHandle private_nh, uint16_t port); 60 | 61 | virtual ~Input() 62 | { 63 | } 64 | 65 | virtual int getPacket(rslidar_msgs::rslidarPacket* pkt, const double time_offset) = 0; 66 | 67 | int getRpm(void); 68 | int getReturnMode(void); 69 | bool getUpdateFlag(void); 70 | void clearUpdateFlag(void); 71 | 72 | protected: 73 | ros::NodeHandle private_nh_; 74 | uint16_t port_; 75 | std::string devip_str_; 76 | int cur_rpm_; 77 | int return_mode_; 78 | bool npkt_update_flag_; 79 | }; 80 | 81 | /** @brief Live rslidar input from socket. */ 82 | class InputSocket : public Input 83 | { 84 | public: 85 | InputSocket(ros::NodeHandle private_nh, uint16_t port = MSOP_DATA_PORT_NUMBER); 86 | 87 | virtual ~InputSocket(); 88 | 89 | virtual int getPacket(rslidar_msgs::rslidarPacket* pkt, const double time_offset); 90 | 91 | private: 92 | private: 93 | int sockfd_; 94 | in_addr devip_; 95 | 96 | }; 97 | 98 | /** @brief rslidar input from PCAP dump file. 99 | * 100 | * Dump files can be grabbed by libpcap 101 | */ 102 | class InputPCAP : public Input 103 | { 104 | public: 105 | InputPCAP(ros::NodeHandle private_nh, uint16_t port = MSOP_DATA_PORT_NUMBER, double packet_rate = 0.0, 106 | std::string filename = "", bool read_once = false, bool read_fast = false, double repeat_delay = 0.0); 107 | 108 | virtual ~InputPCAP(); 109 | 110 | virtual int getPacket(rslidar_msgs::rslidarPacket* pkt, const double time_offset); 111 | 112 | private: 113 | ros::Rate packet_rate_; 114 | std::string filename_; 115 | pcap_t* pcap_; 116 | bpf_program pcap_packet_filter_; 117 | char errbuf_[PCAP_ERRBUF_SIZE]; 118 | bool empty_; 119 | bool read_once_; 120 | bool read_fast_; 121 | double repeat_delay_; 122 | }; 123 | } 124 | 125 | #endif // __RSLIDAR_INPUT_H 126 | -------------------------------------------------------------------------------- /rslidar_driver/src/nodelet.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2018-2020 Robosense Authors 3 | * License: Modified BSD Software License Agreement 4 | * 5 | * $Id$ 6 | */ 7 | 8 | /** \file 9 | * 10 | * ROS driver nodelet for the Robosense 3D LIDARs 11 | */ 12 | 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | 20 | #include "rsdriver.h" 21 | 22 | volatile sig_atomic_t flag = 1; 23 | 24 | namespace rslidar_driver 25 | { 26 | class DriverNodelet : public nodelet::Nodelet 27 | { 28 | public: 29 | DriverNodelet() : running_(false) 30 | { 31 | } 32 | 33 | ~DriverNodelet() 34 | { 35 | if (running_) 36 | { 37 | NODELET_INFO("[driver][nodelet] shutting down driver thread"); 38 | running_ = false; 39 | deviceThread_->join(); 40 | NODELET_INFO("[driver][nodelet] sdriver thread stopped"); 41 | } 42 | } 43 | 44 | private: 45 | virtual void onInit(void); 46 | virtual void devicePoll(void); 47 | 48 | volatile bool running_; ///< device thread is running 49 | boost::shared_ptr deviceThread_; 50 | 51 | boost::shared_ptr dvr_; ///< driver implementation class 52 | }; 53 | 54 | void DriverNodelet::onInit() 55 | { 56 | // start the driver 57 | dvr_.reset(new rslidarDriver(getNodeHandle(), getPrivateNodeHandle())); 58 | 59 | // spawn device poll thread 60 | running_ = true; 61 | deviceThread_ = boost::shared_ptr(new boost::thread(boost::bind(&DriverNodelet::devicePoll, this))); 62 | // NODELET_INFO("DriverNodelet onInit"); 63 | } 64 | 65 | /** @brief Device poll thread main loop. */ 66 | void DriverNodelet::devicePoll() 67 | { 68 | while (ros::ok() && dvr_->poll()) 69 | { 70 | ros::spinOnce(); 71 | } 72 | } 73 | } 74 | 75 | // Register this plugin with pluginlib. Names must match nodelet_rslidar.xml. 76 | // 77 | // parameters are: class type, base class type 78 | PLUGINLIB_EXPORT_CLASS(rslidar_driver::DriverNodelet, nodelet::Nodelet) 79 | -------------------------------------------------------------------------------- /rslidar_driver/src/rsdriver.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2018-2020 Robosense Authors 3 | * License: Modified BSD Software License Agreement 4 | * 5 | * $Id$ 6 | */ 7 | 8 | /** \file 9 | * 10 | * ROS driver implementation for the Robosense 3D LIDARs 11 | */ 12 | #include "rsdriver.h" 13 | #include 14 | 15 | namespace rslidar_driver 16 | { 17 | static const unsigned int POINTS_ONE_CHANNEL_PER_SECOND = 18000; 18 | static const unsigned int BLOCKS_ONE_CHANNEL_PER_PKT = 12; 19 | 20 | rslidarDriver::rslidarDriver(ros::NodeHandle node, ros::NodeHandle private_nh) 21 | { 22 | skip_num_ = 0; 23 | // use private node handle to get parameters 24 | private_nh.param("frame_id", config_.frame_id, std::string("rslidar")); 25 | 26 | std::string tf_prefix = tf::getPrefixParam(private_nh); 27 | ROS_DEBUG_STREAM("tf_prefix: " << tf_prefix); 28 | config_.frame_id = tf::resolve(tf_prefix, config_.frame_id); 29 | 30 | // get model name, validate string, determine packet rate 31 | private_nh.param("model", config_.model, std::string("RS16")); 32 | double packet_rate; // packet frequency (Hz) 33 | std::string model_full_name; 34 | 35 | // product model 36 | if (config_.model == "RS16") 37 | { 38 | //for 0.18 degree horizontal angle resolution 39 | //packet_rate = 840; 40 | //for 0.2 degree horizontal angle resolution 41 | packet_rate = 750; 42 | model_full_name = "RS-LiDAR-16"; 43 | } 44 | else if (config_.model == "RS32") 45 | { 46 | //for 0.18 degree horizontal angle resolution 47 | //packet_rate = 1690; 48 | //for 0.2 degree horizontal angle resolution 49 | packet_rate = 1500; 50 | model_full_name = "RS-LiDAR-32"; 51 | } 52 | else if (config_.model == "RSBPEARL") 53 | { 54 | packet_rate = 1500; 55 | model_full_name = "RSBPEARL"; 56 | } 57 | else if (config_.model == "RSBPEARL_MINI") 58 | { 59 | packet_rate = 1500; 60 | model_full_name = "RSBPEARL_MINI"; 61 | } 62 | else 63 | { 64 | ROS_ERROR_STREAM("[driver] unknown LIDAR model: " << config_.model); 65 | packet_rate = 2600.0; 66 | } 67 | std::string deviceName(std::string("Robosense ") + model_full_name); 68 | 69 | private_nh.param("rpm", config_.rpm, 600.0); 70 | double frequency = (config_.rpm / 60.0); // expected Hz rate 71 | 72 | // default number of packets for each scan is a single revolution 73 | // (fractions rounded up) 74 | 75 | int npackets = (int)ceil(packet_rate / frequency); 76 | private_nh.param("npackets", config_.npackets, npackets); 77 | ROS_INFO_STREAM("[driver] publishing " << config_.npackets << " packets per scan"); 78 | 79 | std::string dump_file; 80 | private_nh.param("pcap", dump_file, std::string("")); 81 | 82 | int msop_udp_port; 83 | private_nh.param("msop_port", msop_udp_port, (int)MSOP_DATA_PORT_NUMBER); 84 | int difop_udp_port; 85 | private_nh.param("difop_port", difop_udp_port, (int)DIFOP_DATA_PORT_NUMBER); 86 | 87 | double cut_angle; 88 | private_nh.param("cut_angle", cut_angle, -0.01); 89 | if (cut_angle < 0.0) 90 | { 91 | ROS_INFO_STREAM("[driver] Cut at specific angle feature deactivated."); 92 | } 93 | else if (cut_angle < 360) 94 | { 95 | ROS_INFO_STREAM("[driver] Cut at specific angle feature activated. " 96 | "Cutting rslidar points always at " 97 | << cut_angle << " degree."); 98 | } 99 | else 100 | { 101 | ROS_ERROR_STREAM("[driver] cut_angle parameter is out of range. Allowed range is " 102 | << "between 0.0 and 360 negative values to deactivate this feature."); 103 | cut_angle = -0.01; 104 | } 105 | 106 | // Convert cut_angle from radian to one-hundredth degree, 107 | // which is used in rslidar packets 108 | config_.cut_angle = static_cast(cut_angle * 100); 109 | 110 | // Initialize dynamic reconfigure 111 | srv_ = boost::make_shared >(private_nh); 112 | dynamic_reconfigure::Server::CallbackType f; 113 | f = boost::bind(&rslidarDriver::callback, this, _1, _2); 114 | srv_->setCallback(f); // Set callback function und call initially 115 | 116 | // initialize diagnostics 117 | diagnostics_.setHardwareID(deviceName); 118 | const double diag_freq = packet_rate / config_.npackets; 119 | diag_max_freq_ = diag_freq; 120 | diag_min_freq_ = diag_freq; 121 | // ROS_INFO("expected frequency: %.3f (Hz)", diag_freq); 122 | 123 | using namespace diagnostic_updater; 124 | diag_topic_.reset(new TopicDiagnostic("rslidar_packets", diagnostics_, 125 | FrequencyStatusParam(&diag_min_freq_, &diag_max_freq_, 0.1, 10), 126 | TimeStampStatusParam())); 127 | 128 | // open rslidar input device or file 129 | if (dump_file != "") // have PCAP file? 130 | { 131 | // read data from packet capture file 132 | msop_input_.reset(new rslidar_driver::InputPCAP(private_nh, msop_udp_port, packet_rate, dump_file)); 133 | difop_input_.reset(new rslidar_driver::InputPCAP(private_nh, difop_udp_port, packet_rate, dump_file)); 134 | } 135 | else 136 | { 137 | // read data from live socket 138 | msop_input_.reset(new rslidar_driver::InputSocket(private_nh, msop_udp_port)); 139 | difop_input_.reset(new rslidar_driver::InputSocket(private_nh, difop_udp_port)); 140 | } 141 | 142 | // raw packet output topic 143 | std::string output_packets_topic; 144 | private_nh.param("output_packets_topic", output_packets_topic, std::string("rslidar_packets")); 145 | msop_output_ = node.advertise(output_packets_topic, 10); 146 | 147 | std::string output_difop_topic; 148 | private_nh.param("output_difop_topic", output_difop_topic, std::string("rslidar_packets_difop")); 149 | difop_output_ = node.advertise(output_difop_topic, 10); 150 | 151 | difop_thread_ = boost::shared_ptr(new boost::thread(boost::bind(&rslidarDriver::difopPoll, this))); 152 | 153 | private_nh.param("time_synchronization", time_synchronization_, false); 154 | 155 | if (time_synchronization_) 156 | { 157 | output_sync_ = node.advertise("sync_header", 1); 158 | skip_num_sub_ = node.subscribe("skippackets_num", 1, &rslidarDriver::skipNumCallback, 159 | (rslidarDriver*)this, ros::TransportHints().tcpNoDelay(true)); 160 | } 161 | } 162 | 163 | /** poll the device 164 | * 165 | * @returns true unless end of file reached 166 | */ 167 | bool rslidarDriver::poll(void) 168 | { // Allocate a new shared pointer for zero-copy sharing with other nodelets. 169 | rslidar_msgs::rslidarScanPtr scan(new rslidar_msgs::rslidarScan); 170 | 171 | // Since the rslidar delivers data at a very high rate, keep 172 | // reading and publishing scans as fast as possible. 173 | if (config_.cut_angle >= 0) // Cut at specific angle feature enabled 174 | { 175 | scan->packets.reserve(config_.npackets); 176 | rslidar_msgs::rslidarPacket tmp_packet; 177 | while (true) 178 | { 179 | while (true) 180 | { 181 | int rc = msop_input_->getPacket(&tmp_packet, config_.time_offset); 182 | if (rc == 0) 183 | break; // got a full packet? 184 | if (rc < 0) 185 | return false; // end of file reached? 186 | } 187 | scan->packets.push_back(tmp_packet); 188 | 189 | static int ANGLE_HEAD = -36001; // note: cannot be set to -1, or stack smashing 190 | static int last_azimuth = ANGLE_HEAD; 191 | 192 | int azimuth = 256 * tmp_packet.data[44] + tmp_packet.data[45]; 193 | // int azimuth = *( (u_int16_t*) (&tmp_packet.data[azimuth_data_pos])); 194 | 195 | // Handle overflow 35999->0 196 | if (azimuth < last_azimuth) 197 | { 198 | last_azimuth -= 36000; 199 | } 200 | // Check if currently passing cut angle 201 | if (last_azimuth != ANGLE_HEAD && last_azimuth < config_.cut_angle && azimuth >= config_.cut_angle) 202 | { 203 | last_azimuth = azimuth; 204 | break; // Cut angle passed, one full revolution collected 205 | } 206 | last_azimuth = azimuth; 207 | } 208 | } 209 | else // standard behaviour 210 | { 211 | if (difop_input_->getUpdateFlag()) 212 | { 213 | int packets_rate = ceil(POINTS_ONE_CHANNEL_PER_SECOND/BLOCKS_ONE_CHANNEL_PER_PKT); 214 | int mode = difop_input_->getReturnMode(); 215 | if (config_.model == "RS16" && (mode == 1 || mode == 2)) 216 | { 217 | packets_rate = ceil(packets_rate/2); 218 | } 219 | else if ((config_.model == "RS32" || config_.model == "RSBPEARL" || config_.model == "RSBPEARL_MINI") && (mode == 0)) 220 | { 221 | packets_rate = packets_rate*2; 222 | } 223 | config_.rpm = difop_input_->getRpm(); 224 | config_.npackets = ceil(packets_rate*60/config_.rpm); 225 | 226 | difop_input_->clearUpdateFlag(); 227 | 228 | ROS_INFO_STREAM("[driver] update npackets. rpm: "<packets.resize(config_.npackets); 231 | // use in standard behaviour only 232 | while (skip_num_) 233 | { 234 | while (true) 235 | { 236 | // keep reading until full packet received 237 | int rc = msop_input_->getPacket(&scan->packets[0], config_.time_offset); 238 | if (rc == 0) 239 | break; // got a full packet? 240 | if (rc < 0) 241 | return false; // end of file reached? 242 | } 243 | --skip_num_; 244 | } 245 | 246 | for (int i = 0; i < config_.npackets; ++i) 247 | { 248 | while (true) 249 | { 250 | // keep reading until full packet received 251 | int rc = msop_input_->getPacket(&scan->packets[i], config_.time_offset); 252 | if (rc == 0) 253 | break; // got a full packet? 254 | if (rc < 0) 255 | return false; // end of file reached? 256 | } 257 | } 258 | 259 | if (time_synchronization_) 260 | { 261 | sensor_msgs::TimeReference sync_header; 262 | // it is already the msop msg 263 | // if (pkt->data[0] == 0x55 && pkt->data[1] == 0xaa && pkt->data[2] == 0x05 && pkt->data[3] == 0x0a) 264 | // use the first packets 265 | rslidar_msgs::rslidarPacket pkt = scan->packets[0]; 266 | struct tm stm; 267 | memset(&stm, 0, sizeof(stm)); 268 | stm.tm_year = (int)pkt.data[20] + 100; 269 | stm.tm_mon = (int)pkt.data[21] - 1; 270 | stm.tm_mday = (int)pkt.data[22]; 271 | stm.tm_hour = (int)pkt.data[23]; 272 | stm.tm_min = (int)pkt.data[24]; 273 | stm.tm_sec = (int)pkt.data[25]; 274 | double stamp_double = mktime(&stm) + 0.001 * (256 * pkt.data[26] + pkt.data[27]) + 275 | 0.000001 * (256 * pkt.data[28] + pkt.data[29]); 276 | sync_header.header.stamp = ros::Time(stamp_double); 277 | 278 | output_sync_.publish(sync_header); 279 | } 280 | } 281 | 282 | // publish message using time of last packet read 283 | // ROS_DEBUG("[driver] Publishing a full rslidar scan."); 284 | scan->header.stamp = scan->packets.back().stamp; 285 | scan->header.frame_id = config_.frame_id; 286 | msop_output_.publish(scan); 287 | 288 | // notify diagnostics that a message has been published, updating its status 289 | diag_topic_->tick(scan->header.stamp); 290 | diagnostics_.update(); 291 | 292 | return true; 293 | } 294 | 295 | void rslidarDriver::difopPoll(void) 296 | { 297 | // reading and publishing scans as fast as possible. 298 | rslidar_msgs::rslidarPacketPtr difop_packet_ptr(new rslidar_msgs::rslidarPacket); 299 | while (ros::ok()) 300 | { 301 | // keep reading 302 | rslidar_msgs::rslidarPacket difop_packet_msg; 303 | int rc = difop_input_->getPacket(&difop_packet_msg, config_.time_offset); 304 | if (rc == 0) 305 | { 306 | // ROS_DEBUG("[driver] Publishing a difop data."); 307 | *difop_packet_ptr = difop_packet_msg; 308 | difop_output_.publish(difop_packet_ptr); 309 | } 310 | if (rc < 0) 311 | return; // end of file reached? 312 | ros::spinOnce(); 313 | } 314 | } 315 | 316 | void rslidarDriver::callback(rslidar_driver::rslidarNodeConfig& config, uint32_t level) 317 | { 318 | // ROS_INFO("[driver] Reconfigure Request"); 319 | config_.time_offset = config.time_offset; 320 | } 321 | 322 | // add for time synchronization 323 | void rslidarDriver::skipNumCallback(const std_msgs::Int32::ConstPtr& skip_num) 324 | { 325 | // std::cout << "Enter skipNumCallback: " << skip_num->data << std::endl; 326 | skip_num_ = skip_num->data; 327 | } 328 | } // namespace rslidar_driver 329 | -------------------------------------------------------------------------------- /rslidar_driver/src/rsdriver.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2018-2020 Robosense Authors 3 | * License: Modified BSD Software License Agreement 4 | * 5 | * $Id$ 6 | */ 7 | 8 | /** \file 9 | * 10 | * ROS driver interface for the Robosense 3D LIDARs 11 | */ 12 | #ifndef _RSDRIVER_H_ 13 | #define _RSDRIVER_H_ 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include "input.h" 27 | 28 | namespace rslidar_driver 29 | { 30 | class rslidarDriver 31 | { 32 | public: 33 | /** 34 | * @brief rslidarDriver 35 | * @param node raw packet output topic 36 | * @param private_nh 通过这个节点传参数 37 | */ 38 | rslidarDriver(ros::NodeHandle node, ros::NodeHandle private_nh); 39 | 40 | ~rslidarDriver() 41 | { 42 | } 43 | 44 | bool poll(void); 45 | void difopPoll(void); 46 | 47 | private: 48 | /// Callback for dynamic reconfigure 49 | void callback(rslidar_driver::rslidarNodeConfig& config, uint32_t level); 50 | /// Callback for skip num for time synchronization 51 | void skipNumCallback(const std_msgs::Int32::ConstPtr& skip_num); 52 | 53 | /// Pointer to dynamic reconfigure service srv_ 54 | boost::shared_ptr > srv_; 55 | 56 | // configuration parameters 57 | struct 58 | { 59 | std::string frame_id; ///< tf frame ID 60 | std::string model; ///< device model name 61 | int npackets; ///< number of packets to collect 62 | double rpm; ///< device rotation rate (RPMs) 63 | double time_offset; ///< time in seconds added to each time stamp 64 | int cut_angle; 65 | } config_; 66 | 67 | boost::shared_ptr msop_input_; 68 | boost::shared_ptr difop_input_; 69 | ros::Publisher msop_output_; 70 | ros::Publisher difop_output_; 71 | ros::Publisher output_sync_; 72 | // Converter convtor_; 73 | /** diagnostics updater */ 74 | diagnostic_updater::Updater diagnostics_; 75 | double diag_min_freq_; 76 | double diag_max_freq_; 77 | boost::shared_ptr diag_topic_; 78 | boost::shared_ptr difop_thread_; 79 | 80 | // add for time synchronization 81 | bool time_synchronization_; 82 | uint32_t skip_num_; 83 | ros::Subscriber skip_num_sub_; 84 | }; 85 | 86 | } // namespace rslidar_driver 87 | 88 | #endif 89 | -------------------------------------------------------------------------------- /rslidar_driver/src/rslidar_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2018-2020 Robosense Authors 3 | * License: Modified BSD Software License Agreement 4 | * 5 | * $Id$ 6 | */ 7 | 8 | /** \file 9 | * 10 | * ROS driver node for the Robosense 3D LIDARs. 11 | */ 12 | #include 13 | #include "rsdriver.h" 14 | #include "std_msgs/String.h" 15 | 16 | using namespace rslidar_driver; 17 | volatile sig_atomic_t flag = 1; 18 | 19 | static void my_handler(int sig) 20 | { 21 | flag = 0; 22 | } 23 | 24 | int main(int argc, char** argv) 25 | { 26 | ros::init(argc, argv, "rsdriver"); 27 | ros::NodeHandle node; 28 | ros::NodeHandle private_nh("~"); 29 | 30 | signal(SIGINT, my_handler); 31 | 32 | // start the driver 33 | rslidar_driver::rslidarDriver dvr(node, private_nh); 34 | // loop until shut down or end of file 35 | while (ros::ok() && dvr.poll()) 36 | { 37 | ros::spinOnce(); 38 | } 39 | 40 | return 0; 41 | } 42 | -------------------------------------------------------------------------------- /rslidar_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(rslidar_msgs) 3 | 4 | find_package(catkin REQUIRED COMPONENTS message_generation std_msgs) 5 | 6 | add_message_files( 7 | DIRECTORY msg 8 | FILES 9 | rslidarPacket.msg 10 | rslidarScan.msg 11 | ) 12 | generate_messages(DEPENDENCIES std_msgs) 13 | 14 | catkin_package( 15 | CATKIN_DEPENDS message_runtime std_msgs 16 | ) 17 | -------------------------------------------------------------------------------- /rslidar_msgs/msg/rslidarPacket.msg: -------------------------------------------------------------------------------- 1 | # Raw LIDAR packet. 2 | 3 | time stamp # packet timestamp 4 | uint8[1248] data # packet contents 5 | 6 | -------------------------------------------------------------------------------- /rslidar_msgs/msg/rslidarScan.msg: -------------------------------------------------------------------------------- 1 | # LIDAR scan packets. 2 | 3 | Header header # standard ROS message header 4 | rslidarPacket[] packets # vector of raw packets 5 | -------------------------------------------------------------------------------- /rslidar_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rslidar_msgs 4 | 1.0.0 5 | 6 | ROS message definitions for Rslidar 3D LIDARs. 7 | 8 | Tony Zhang 9 | Tony Zhang 10 | BSD 11 | 12 | 13 | catkin 14 | 15 | message_generation 16 | std_msgs 17 | 18 | message_runtime 19 | std_msgs 20 | 21 | 22 | -------------------------------------------------------------------------------- /rslidar_pointcloud/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(rslidar_pointcloud) 3 | 4 | add_compile_options(-std=c++11) 5 | set(CMAKE_BUILD_TYPE Release)#RelWithDebInfo 6 | set(${PROJECT_NAME}_CATKIN_DEPS 7 | angles 8 | nodelet 9 | pcl_ros 10 | roscpp 11 | roslib 12 | sensor_msgs 13 | tf 14 | rslidar_driver 15 | rslidar_msgs 16 | dynamic_reconfigure 17 | ) 18 | 19 | find_package(catkin REQUIRED COMPONENTS 20 | ${${PROJECT_NAME}_CATKIN_DEPS} pcl_conversions) 21 | find_package(Boost COMPONENTS signals) 22 | find_package(PkgConfig REQUIRED) 23 | include_directories(${catkin_INCLUDE_DIRS} 24 | ${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake 25 | ) 26 | 27 | generate_dynamic_reconfigure_options(cfg/CloudNode.cfg) 28 | 29 | catkin_package( 30 | CATKIN_DEPENDS ${${PROJECT_NAME}_CATKIN_DEPS} 31 | ) 32 | 33 | add_subdirectory(src) 34 | -------------------------------------------------------------------------------- /rslidar_pointcloud/cfg/CloudNode.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "rslidar_pointcloud" 3 | 4 | from math import pi 5 | from dynamic_reconfigure.parameter_generator_catkin import * 6 | 7 | gen = ParameterGenerator() 8 | 9 | gen.add("time_offset", double_t, 0, "A manually calibrated offset (in seconds) to add to the timestamp before publication of a message.", 10 | 0.0, -1.0, 1.0) 11 | 12 | 13 | exit(gen.generate(PACKAGE, "cloud_node", "CloudNode")) 14 | -------------------------------------------------------------------------------- /rslidar_pointcloud/data/lidar1/ChannelNum.csv: -------------------------------------------------------------------------------- 1 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 2 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 3 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 4 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 5 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 6 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 7 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 8 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 9 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 10 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 11 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 12 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 13 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 14 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 15 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 16 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 17 | -------------------------------------------------------------------------------- /rslidar_pointcloud/data/lidar1/angle.csv: -------------------------------------------------------------------------------- 1 | -14.9559 2 | -12.9460 3 | -10.9169 4 | -8.9541 5 | -6.9592 6 | -4.9829 7 | -2.9588 8 | -0.9274 9 | 15.0228 10 | 13.0276 11 | 10.9686 12 | 8.9960 13 | 7.0298 14 | 5.0611 15 | 3.0160 16 | 1.0455 17 | -------------------------------------------------------------------------------- /rslidar_pointcloud/data/lidar1/curves.csv: -------------------------------------------------------------------------------- 1 | 20.26,21.02,21.47,19.81,20.92,16.34,18.38,15,15.33,14.47,15.02,14.75,15.6,15.65,15.46,15.18 2 | 3.236,3.362,3.43,3.171,3.346,2.614,2.941,2.394,2.454,2.313,2.405,2.363,2.515,2.504,2.46,2.436 3 | 1.186,1.354,2.171,1.237,1.271,1.57,1.145,1.57,1.608,1.533,1.51,1.353,1.106,1.39,1.673,1.599 4 | 14.23,25.42,15.47,6.734,21.44,8.996,5.323,4.386,5.528,3.97,5.492,5.227,3.239,5.394,5.955,4.877 5 | 0.07468,0.0859,0.06859,0.07038,0.07101,0.05011,0.05272,0.04714,0.05424,0.05364,0.05385,0.05084,0.04908,0.04862,0.04714,0.04714 6 | -1.235,-1.606,-1.112,-1.006,-1.223,-0.7429,-0.8428,-0.6681,-0.8384,-0.889,-0.8348,-0.7302,-0.7143,-0.7469,-0.6935,-0.6681 7 | 14.77,20.7,13.32,11.86,17.38,9.332,12.01,7.657,9.517,8.638,9.538,8.7,9.396,8.464,8.901,7.657 8 | -------------------------------------------------------------------------------- /rslidar_pointcloud/data/lidar2/ChannelNum.csv: -------------------------------------------------------------------------------- 1 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 2 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 3 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 4 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 5 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 6 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 7 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 8 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 9 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 10 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 11 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 12 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 13 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 14 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 15 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 16 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 17 | -------------------------------------------------------------------------------- /rslidar_pointcloud/data/lidar2/angle.csv: -------------------------------------------------------------------------------- 1 | -15.0762 2 | -13.0208 3 | -10.9997 4 | -9.0204 5 | -7.0192 6 | -5.0006 7 | -3.0410 8 | -1.0384 9 | 15.0094 10 | 12.9868 11 | 10.9307 12 | 8.9855 13 | 6.9487 14 | 5.0006 15 | 2.9696 16 | 1.0026 17 | -------------------------------------------------------------------------------- /rslidar_pointcloud/data/lidar2/curves.csv: -------------------------------------------------------------------------------- 1 | 20.26,21.02,21.47,19.81,20.92,16.34,18.38,15,15.33,14.47,15.02,14.75,15.6,15.65,15.46,15.18 2 | 3.236,3.362,3.43,3.171,3.346,2.614,2.941,2.394,2.454,2.313,2.405,2.363,2.515,2.504,2.46,2.436 3 | 1.186,1.354,2.171,1.237,1.271,1.57,1.145,1.57,1.608,1.533,1.51,1.353,1.106,1.39,1.673,1.599 4 | 14.23,25.42,15.47,6.734,21.44,8.996,5.323,4.386,5.528,3.97,5.492,5.227,3.239,5.394,5.955,4.877 5 | 0.07468,0.0859,0.06859,0.07038,0.07101,0.05011,0.05272,0.04714,0.05424,0.05364,0.05385,0.05084,0.04908,0.04862,0.04714,0.04714 6 | -1.235,-1.606,-1.112,-1.006,-1.223,-0.7429,-0.8428,-0.6681,-0.8384,-0.889,-0.8348,-0.7302,-0.7143,-0.7469,-0.6935,-0.6681 7 | 14.77,20.7,13.32,11.86,17.38,9.332,12.01,7.657,9.517,8.638,9.538,8.7,9.396,8.464,8.901,7.657 8 | -------------------------------------------------------------------------------- /rslidar_pointcloud/data/rs_bpearl/ChannelNum.csv: -------------------------------------------------------------------------------- 1 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 2 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 3 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 4 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 5 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 6 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 7 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 8 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 9 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 10 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 11 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 12 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 13 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 14 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 15 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 16 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 17 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 18 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 19 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 20 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 21 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 22 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 23 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 24 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 25 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 26 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 27 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 28 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 29 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 30 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 31 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 32 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 33 | 31,32,33,34,35,36,37,38,39,40,41,42,43,44,45,46,47,48,49,50,51,52,53,54,55,56,57,58,59,60,61,62,63,64,65,66,67,68,69,70,71,72,73,74,75,76,77,78,79,80,81 34 | -------------------------------------------------------------------------------- /rslidar_pointcloud/data/rs_bpearl/angle.csv: -------------------------------------------------------------------------------- 1 | 89.5,0 2 | 86.6875,0 3 | 83.875,0 4 | 81.0625,0 5 | 78.25,0 6 | 75.4375,0 7 | 72.625,0 8 | 69.8125,0 9 | 67,0 10 | 64.1875,0 11 | 61.375,0 12 | 58.5625,0 13 | 55.75,0 14 | 52.9375,0 15 | 50.125,0 16 | 47.3125,0 17 | 44.5,0 18 | 41.6875,0 19 | 38.875,0 20 | 36.0625,0 21 | 33.25,0 22 | 30.4375,0 23 | 27.625,0 24 | 24.8125,0 25 | 22,0 26 | 19.1875,0 27 | 16.375,0 28 | 13.5625,0 29 | 10.75,0 30 | 7.9375,0 31 | 5.125,0 32 | 2.3125,0 33 | -------------------------------------------------------------------------------- /rslidar_pointcloud/data/rs_lidar_16/ChannelNum.csv: -------------------------------------------------------------------------------- 1 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 2 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 3 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 4 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 5 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 6 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 7 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 8 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 9 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 10 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 11 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 12 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 13 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 14 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 15 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 16 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 17 | -------------------------------------------------------------------------------- /rslidar_pointcloud/data/rs_lidar_16/angle.csv: -------------------------------------------------------------------------------- 1 | -14.9994 2 | -12.9766 3 | -10.9894 4 | -8.9750 5 | -6.9734 6 | -4.9900 7 | -2.9946 8 | -0.9918 9 | 14.9860 10 | 12.9630 11 | 10.9756 12 | 8.9960 13 | 6.9945 14 | 5.0113 15 | 2.9803 16 | 1.0133 -------------------------------------------------------------------------------- /rslidar_pointcloud/data/rs_lidar_16/curves.csv: -------------------------------------------------------------------------------- 1 | 20.26,21.02,21.47,19.81,20.92,16.34,18.38,15,15.33,14.47,15.02,14.75,15.6,15.65,15.46,15.18 2 | 3.236,3.362,3.43,3.171,3.346,2.614,2.941,2.394,2.454,2.313,2.405,2.363,2.515,2.504,2.46,2.436 3 | 1.186,1.354,2.171,1.237,1.271,1.57,1.145,1.57,1.608,1.533,1.51,1.353,1.106,1.39,1.673,1.599 4 | 14.23,25.42,15.47,6.734,21.44,8.996,5.323,4.386,5.528,3.97,5.492,5.227,3.239,5.394,5.955,4.877 5 | 0.07468,0.0859,0.06859,0.07038,0.07101,0.05011,0.05272,0.04714,0.05424,0.05364,0.05385,0.05084,0.04908,0.04862,0.04714,0.04714 6 | -1.235,-1.606,-1.112,-1.006,-1.223,-0.7429,-0.8428,-0.6681,-0.8384,-0.889,-0.8348,-0.7302,-0.7143,-0.7469,-0.6935,-0.6681 7 | 14.77,20.7,13.32,11.86,17.38,9.332,12.01,7.657,9.517,8.638,9.538,8.7,9.396,8.464,8.901,7.657 8 | -------------------------------------------------------------------------------- /rslidar_pointcloud/data/rs_lidar_32/ChannelNum.csv: -------------------------------------------------------------------------------- 1 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 2 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 3 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 4 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 5 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 6 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 7 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 8 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 9 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 10 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 11 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 12 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 13 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 14 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 15 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 16 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 17 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 18 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 19 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 20 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 21 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 22 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 23 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 24 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 25 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 26 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 27 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 28 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 29 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 30 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 31 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 32 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 33 | 31,32,33,34,35,36,37,38,39,40,41,42,43,44,45,46,47,48,49,50,51,52,53,54,55,56,57,58,59,60,61,62,63,64,65,66,67,68,69,70,71,72,73,74,75,76,77,78,79,80,81 34 | -------------------------------------------------------------------------------- /rslidar_pointcloud/data/rs_lidar_32/CurveRate.csv: -------------------------------------------------------------------------------- 1 | 1 2 | 1 3 | 1 4 | 1 5 | 1 6 | 1 7 | 1 8 | 1 9 | 1 10 | 1 11 | 1 12 | 1 13 | 1 14 | 1 15 | 1 16 | 1 17 | 1 18 | 1 19 | 1 20 | 1 21 | 1 22 | 1 23 | 1 24 | 1 25 | 1 26 | 1 27 | 1 28 | 1 29 | 1 30 | 1 31 | 1 32 | 1 33 | -------------------------------------------------------------------------------- /rslidar_pointcloud/data/rs_lidar_32/angle.csv: -------------------------------------------------------------------------------- 1 | -10.2637,9.1205 2 | 0.2972,-7.1203 3 | -6.4063,8.9109 4 | -0.0179,-1.7781 5 | 2.2794,9.1205 6 | -0.3330,3.5716 7 | 3.2973,-6.8380 8 | -0.6670,8.7856 9 | 4.6136,9.1205 10 | 1.6849,-7.1555 11 | 7.0176,-6.7674 12 | 1.2972,-1.8139 13 | 10.2983,9.0507 14 | 1.0000,3.4859 15 | 15.0167,-6.9439 16 | 0.7028,8.9007 17 | -25.0000,-6.6968 18 | -2.2794,-7.3317 19 | -14.6380,-6.6614 20 | -2.6670,-2.0000 21 | -7.9100,-6.7674 22 | -3.0179,3.3932 23 | -5.4070,-6.8380 24 | -3.2973,8.8060 25 | -3.6492,-7.1908 26 | -1.0179,-7.2965 27 | -4.0534,-1.8643 28 | -1.3330,-1.8139 29 | -4.3864,3.4932 30 | -1.6312,3.5716 31 | -4.6492,8.8060 32 | -1.9821,8.7308 -------------------------------------------------------------------------------- /rslidar_pointcloud/data/rs_lidar_32/curves.csv: -------------------------------------------------------------------------------- 1 | 9.146,14.59,9.765,11.67,10.89,6.767,12.25,6.767,9.641,14.28,11.4,18.12,10.25,19.72,18.15,19.6,16.01,13.77,14.51,13.82,9.894,12.95,8.204,11.97,13.65,15.78,15.14,14.57,11.65,12.28,3.5,12.31 2 | 1.471,2.336,1.564,1.868,1.74,1.084,1.957,1.084,1.543,2.286,1.82,3.231,1.64,3.157,2.884,3.135,2.558,2.203,2.321,2.211,1.584,2.071,1.299,1.916,2.182,2.524,2.423,2.333,1.862,1.972,0.5675,1.972 3 | 4.091,1.158,2.611,2.033,1.861,5.428,1.166,5.428,2.306,1.546,1.328,3.672,2.915,3.027,0.8046,2.81,1.283,0.9306,1.263,1.102,1.683,2.184,2.304,5.983,1.695,1.106,1.08,2.36,3.328,2.908,2.633,3.482 4 | 6.06,4.316,4.457,4.847,6.098,5.203,4.178,5.203,6.216,7.532,4.563,8.926,6.743,7.91,7.246e-08,7.586,8.587,4.376,3.76,5.592,6.678,8.152,3.622,6.286,6.715,7.819,5.758,4.781,8.349,6.168,6.162,6.204 5 | 0.07359,0.05948,0.05485,0.08061,0.05223,0.1015,0.05232,0.1015,0.06199,0.06027,0.06084,0.07249,0.08784,0.07249,0.0688,0.07249,0.2397,0.06514,0.05794,0.05276,0.06672,0.06231,0.06167,0.07097,0.0631,0.06867,0.05846,0.06226,0.0602,0.0594,0.06162,0.1272 6 | -1.207,-0.4436,-0.5708,-1.042,-0.8002,-1.576,-0.6504,-1.576,-1.063,-0.5388,-0.4561,-0.8357,-1.002,-0.8357,-0.696,-0.8357,-3.747,-0.6581,-0.6612,-0.6709,-0.6868,-0.7484,-0.9373,-0.9974,-0.5119,-0.4602,-0.6081,-0.8116,-0.6385,-0.6696,-0.6901,-2.445 7 | 13.02,9.64,6.718,10.79,11.49,14.06,8.267,14.06,12.36,10.2,6.484,8.279,11.72,8.279,7.048,8.279,32.67,11.99,7.598,10.62,9.679,11.77,8.828,10.03,9.267,11.96,10.68,9.459,10.36,9.944,9.401,21.9 8 | -------------------------------------------------------------------------------- /rslidar_pointcloud/launch/cloud_nodelet.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /rslidar_pointcloud/launch/rs_bpearl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /rslidar_pointcloud/launch/rs_lidar_16.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /rslidar_pointcloud/launch/rs_lidar_32.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /rslidar_pointcloud/launch/two_lidar.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /rslidar_pointcloud/nodelets.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | Aggregates points from multiple packets, publishing PointCloud2. 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /rslidar_pointcloud/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rslidar_pointcloud 4 | 1.0.0 5 | 6 | Point cloud conversions for rslidar 3D LIDARs. 7 | 8 | Tony 9 | Tony Zhang 10 | George Wang 11 | Piyush Khandelwal 12 | Jesse Vera 13 | BSD 14 | 15 | catkin 16 | 17 | angles 18 | nodelet 19 | pcl_conversions 20 | pcl_ros 21 | pluginlib 22 | roscpp 23 | roslib 24 | sensor_msgs 25 | tf 26 | rslidar_driver 27 | rslidar_msgs 28 | dynamic_reconfigure 29 | 30 | 31 | roslaunch 32 | rostest 33 | tf2_ros 34 | 35 | angles 36 | nodelet 37 | pcl_ros 38 | pluginlib 39 | 40 | roscpp 41 | roslib 42 | sensor_msgs 43 | tf 44 | rslidar_driver 45 | rslidar_msgs 46 | 47 | dynamic_reconfigure 48 | 49 | 50 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /rslidar_pointcloud/rviz_cfg/rslidar.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 89 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Grid1 10 | - /PointCloud21 11 | - /PointCloud21/Autocompute Value Bounds1 12 | - /Axes1 13 | Splitter Ratio: 0.5 14 | Tree Height: 733 15 | - Class: rviz/Selection 16 | Name: Selection 17 | - Class: rviz/Tool Properties 18 | Expanded: 19 | - /2D Pose Estimate1 20 | - /2D Nav Goal1 21 | - /Publish Point1 22 | Name: Tool Properties 23 | Splitter Ratio: 0.588679016 24 | - Class: rviz/Views 25 | Expanded: 26 | - /Current View1 27 | Name: Views 28 | Splitter Ratio: 0.5 29 | - Class: rviz/Time 30 | Experimental: false 31 | Name: Time 32 | SyncMode: 0 33 | SyncSource: PointCloud2 34 | Visualization Manager: 35 | Class: "" 36 | Displays: 37 | - Alpha: 0.5 38 | Cell Size: 10 39 | Class: rviz/Grid 40 | Color: 160; 160; 164 41 | Enabled: true 42 | Line Style: 43 | Line Width: 0.0299999993 44 | Value: Lines 45 | Name: Grid 46 | Normal Cell Count: 0 47 | Offset: 48 | X: 0 49 | Y: 0 50 | Z: 0 51 | Plane: XY 52 | Plane Cell Count: 20 53 | Reference Frame: 54 | Value: true 55 | - Alpha: 1 56 | Autocompute Intensity Bounds: true 57 | Autocompute Value Bounds: 58 | Max Value: 135.535004 59 | Min Value: -69.2572021 60 | Value: false 61 | Axis: X 62 | Channel Name: intensity 63 | Class: rviz/PointCloud2 64 | Color: 255; 255; 255 65 | Color Transformer: Intensity 66 | Decay Time: 0 67 | Enabled: true 68 | Invert Rainbow: false 69 | Max Color: 255; 255; 255 70 | Max Intensity: 226 71 | Min Color: 0; 0; 0 72 | Min Intensity: 1 73 | Name: PointCloud2 74 | Position Transformer: XYZ 75 | Queue Size: 255 76 | Selectable: true 77 | Size (Pixels): 2 78 | Size (m): 0.00999999978 79 | Style: Points 80 | Topic: /rslidar_points 81 | Unreliable: true 82 | Use Fixed Frame: true 83 | Use rainbow: true 84 | Value: true 85 | - Class: rviz/Axes 86 | Enabled: true 87 | Length: 1 88 | Name: Axes 89 | Radius: 0.100000001 90 | Reference Frame: 91 | Value: true 92 | Enabled: true 93 | Global Options: 94 | Background Color: 48; 48; 48 95 | Default Light: true 96 | Fixed Frame: rslidar 97 | Frame Rate: 10 98 | Name: root 99 | Tools: 100 | - Class: rviz/Interact 101 | Hide Inactive Objects: true 102 | - Class: rviz/MoveCamera 103 | - Class: rviz/Select 104 | - Class: rviz/FocusCamera 105 | - Class: rviz/Measure 106 | - Class: rviz/SetInitialPose 107 | Topic: /initialpose 108 | - Class: rviz/SetGoal 109 | Topic: /move_base_simple/goal 110 | - Class: rviz/PublishPoint 111 | Single click: true 112 | Topic: /clicked_point 113 | Value: true 114 | Views: 115 | Current: 116 | Class: rviz/ThirdPersonFollower 117 | Distance: 49.6928291 118 | Enable Stereo Rendering: 119 | Stereo Eye Separation: 0.0599999987 120 | Stereo Focal Distance: 1 121 | Swap Stereo Eyes: false 122 | Value: false 123 | Focal Point: 124 | X: 4.06498575 125 | Y: -10.8205557 126 | Z: -2.45250994e-05 127 | Focal Shape Fixed Size: true 128 | Focal Shape Size: 0.0500000007 129 | Invert Z Axis: false 130 | Name: Current View 131 | Near Clip Distance: 0.00999999978 132 | Pitch: 1.14480078 133 | Target Frame: aft_mapped 134 | Value: ThirdPersonFollower (rviz) 135 | Yaw: 4.18355608 136 | Saved: ~ 137 | Window Geometry: 138 | Displays: 139 | collapsed: false 140 | Height: 1027 141 | Hide Left Dock: false 142 | Hide Right Dock: true 143 | QMainWindow State: 000000ff00000000fd00000004000000000000016a00000377fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000377000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000394fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000394000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f00000040fc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000005cf0000037700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 144 | Selection: 145 | collapsed: false 146 | Time: 147 | collapsed: false 148 | Tool Properties: 149 | collapsed: false 150 | Views: 151 | collapsed: true 152 | Width: 1855 153 | X: 65 154 | Y: 43 155 | -------------------------------------------------------------------------------- /rslidar_pointcloud/src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(rslidar_data rawdata.cc) 2 | target_link_libraries(rslidar_data 3 | ${catkin_LIBRARIES} 4 | ${libpcap_LIBRARIES}) 5 | 6 | add_library(rslidar_point convert.cc) 7 | target_link_libraries(rslidar_point 8 | rslidar_data 9 | ${catkin_LIBRARIES}) 10 | 11 | 12 | add_library(cloud_nodelet cloud_nodelet.cc) 13 | target_link_libraries(cloud_nodelet rslidar_point 14 | ${catkin_LIBRARIES}) 15 | 16 | add_executable(cloud_node cloud_node.cc) 17 | 18 | if(catkin_EXPORTED_TARGETS) 19 | add_dependencies(rslidar_data ${catkin_EXPORTED_TARGETS}) 20 | endif() 21 | 22 | target_link_libraries(cloud_node 23 | rslidar_point 24 | ${catkin_LIBRARIES} 25 | ${libpcap_LIBRARIES}) 26 | -------------------------------------------------------------------------------- /rslidar_pointcloud/src/cloud_node.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2018-2020 Robosense Authors 3 | * License: Modified BSD Software License Agreement 4 | * 5 | * $Id$ 6 | */ 7 | 8 | /** \file 9 | 10 | This ROS node converts raw Robosense 3D LIDAR packets to PointCloud2. 11 | 12 | */ 13 | #include "convert.h" 14 | 15 | /** Main node entry point. */ 16 | int main(int argc, char** argv) 17 | { 18 | ros::init(argc, argv, "cloud_node"); 19 | ros::NodeHandle node; 20 | ros::NodeHandle priv_nh("~"); 21 | 22 | // create conversion class, which subscribes to raw data 23 | rslidar_pointcloud::Convert conv(node, priv_nh); 24 | 25 | // handle callbacks until shut down 26 | ros::spin(); 27 | 28 | return 0; 29 | } 30 | -------------------------------------------------------------------------------- /rslidar_pointcloud/src/cloud_nodelet.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2018-2020 Robosense Authors 3 | * License: Modified BSD Software License Agreement 4 | * 5 | * $Id$ 6 | */ 7 | 8 | /** @file 9 | 10 | This ROS nodelet converts raw Robosense 3D LIDAR packets to a 11 | PointCloud2. 12 | 13 | */ 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | #include "convert.h" 20 | 21 | namespace rslidar_pointcloud 22 | { 23 | class CloudNodelet : public nodelet::Nodelet 24 | { 25 | public: 26 | CloudNodelet() 27 | { 28 | } 29 | ~CloudNodelet() 30 | { 31 | } 32 | 33 | private: 34 | virtual void onInit(); 35 | boost::shared_ptr conv_; 36 | }; 37 | 38 | /** @brief Nodelet initialization. */ 39 | void CloudNodelet::onInit() 40 | { 41 | conv_.reset(new Convert(getNodeHandle(), getPrivateNodeHandle())); 42 | } 43 | 44 | } // namespace rslidar_pointcloud 45 | 46 | // Register this plugin with pluginlib. Names must match nodelets.xml. 47 | // 48 | // parameters: class type, base class type 49 | PLUGINLIB_EXPORT_CLASS(rslidar_pointcloud::CloudNodelet, nodelet::Nodelet) 50 | -------------------------------------------------------------------------------- /rslidar_pointcloud/src/convert.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2018-2020 Robosense Authors 3 | * License: Modified BSD Software License Agreement 4 | * 5 | * $Id$ 6 | */ 7 | 8 | /** @file 9 | 10 | This class converts raw Robosense 3D LIDAR packets to PointCloud2. 11 | 12 | */ 13 | #include "convert.h" 14 | #include 15 | 16 | namespace rslidar_pointcloud 17 | { 18 | std::string model; 19 | 20 | /** @brief Constructor. */ 21 | Convert::Convert(ros::NodeHandle node, ros::NodeHandle private_nh) : data_(new rslidar_rawdata::RawData()) 22 | { 23 | data_->loadConfigFile(node, private_nh); // load lidar parameters 24 | private_nh.param("model", model, std::string("RS16")); 25 | 26 | // advertise output point cloud (before subscribing to input data) 27 | std::string output_points_topic; 28 | private_nh.param("output_points_topic", output_points_topic, std::string("rslidar_points")); 29 | output_ = node.advertise(output_points_topic, 10); 30 | 31 | srv_ = boost::make_shared >(private_nh); 32 | dynamic_reconfigure::Server::CallbackType f; 33 | f = boost::bind(&Convert::callback, this, _1, _2); 34 | srv_->setCallback(f); 35 | 36 | // subscribe to rslidarScan packets 37 | std::string input_packets_topic; 38 | private_nh.param("input_packets_topic", input_packets_topic, std::string("rslidar_packets")); 39 | rslidar_scan_ = node.subscribe(input_packets_topic, 10, &Convert::processScan, (Convert*)this, 40 | ros::TransportHints().tcpNoDelay(true)); 41 | } 42 | 43 | void Convert::callback(rslidar_pointcloud::CloudNodeConfig& config, uint32_t level) 44 | { 45 | // ROS_INFO("[cloud][convert] Reconfigure Request"); 46 | // config_.time_offset = config.time_offset; 47 | } 48 | 49 | /** @brief Callback for raw scan messages. */ 50 | void Convert::processScan(const rslidar_msgs::rslidarScan::ConstPtr& scanMsg) 51 | { 52 | pcl::PointCloud::Ptr outPoints(new pcl::PointCloud); 53 | outPoints->header.stamp = pcl_conversions::toPCL(scanMsg->header).stamp; 54 | outPoints->header.frame_id = scanMsg->header.frame_id; 55 | outPoints->clear(); 56 | if (model == "RS16") 57 | { 58 | outPoints->height = 16; 59 | outPoints->width = 24 * (int)scanMsg->packets.size(); 60 | outPoints->is_dense = false; 61 | outPoints->resize(outPoints->height * outPoints->width); 62 | } 63 | else if (model == "RS32" || model == "RSBPEARL" || model == "RSBPEARL_MINI") 64 | { 65 | outPoints->height = 32; 66 | outPoints->width = 12 * (int)scanMsg->packets.size(); 67 | outPoints->is_dense = false; 68 | outPoints->resize(outPoints->height * outPoints->width); 69 | } 70 | 71 | // process each packet provided by the driver 72 | 73 | data_->block_num = 0; 74 | for (size_t i = 0; i < scanMsg->packets.size(); ++i) 75 | { 76 | data_->unpack(scanMsg->packets[i], outPoints); 77 | } 78 | sensor_msgs::PointCloud2 outMsg; 79 | pcl::toROSMsg(*outPoints, outMsg); 80 | 81 | output_.publish(outMsg); 82 | } 83 | } // namespace rslidar_pointcloud 84 | -------------------------------------------------------------------------------- /rslidar_pointcloud/src/convert.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2018-2020 Robosense Authors 3 | * License: Modified BSD Software License Agreement 4 | * 5 | * $Id$ 6 | */ 7 | 8 | /** @file 9 | 10 | This class converts raw Robosense 3D LIDAR packets to PointCloud2. 11 | 12 | */ 13 | #ifndef _CONVERT_H_ 14 | #define _CONVERT_H_ 15 | 16 | #include 17 | #include 18 | #include 19 | #include "rawdata.h" 20 | 21 | namespace rslidar_pointcloud 22 | { 23 | class Convert 24 | { 25 | public: 26 | Convert(ros::NodeHandle node, ros::NodeHandle private_nh); 27 | 28 | ~Convert() 29 | { 30 | } 31 | 32 | private: 33 | void callback(rslidar_pointcloud::CloudNodeConfig& config, uint32_t level); 34 | 35 | void processScan(const rslidar_msgs::rslidarScan::ConstPtr& scanMsg); 36 | 37 | /// Pointer to dynamic reconfigure service srv_ 38 | boost::shared_ptr > srv_; 39 | 40 | boost::shared_ptr data_; 41 | ros::Subscriber rslidar_scan_; 42 | ros::Publisher output_; 43 | }; 44 | 45 | } // namespace rslidar_pointcloud 46 | #endif 47 | -------------------------------------------------------------------------------- /rslidar_pointcloud/src/rawdata.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2018-2020 Robosense Authors 3 | * License: Modified BSD Software License Agreement 4 | * 5 | * $Id$ 6 | */ 7 | 8 | /** 9 | * @file 10 | * 11 | * RSLIDAR 3D LIDAR data accessor class implementation. 12 | * 13 | * Class for unpacking raw Robosense 3D LIDAR packets into useful 14 | * formats. 15 | * 16 | */ 17 | #include "rawdata.h" 18 | 19 | namespace rslidar_rawdata 20 | { 21 | RawData::RawData() 22 | { 23 | this->is_init_angle_ = false; 24 | this->is_init_curve_ = false; 25 | this->is_init_top_fw_ = false; 26 | } 27 | 28 | void RawData::loadConfigFile(ros::NodeHandle node, ros::NodeHandle private_nh) 29 | { 30 | std::string anglePath, curvesPath, channelPath, curvesRatePath; 31 | std::string model; 32 | std::string sub_model; 33 | std::string resolution_param; 34 | private_nh.param("model", model, std::string("RS16")); 35 | private_nh.param("curves_path", curvesPath, std::string("")); 36 | private_nh.param("angle_path", anglePath, std::string("")); 37 | private_nh.param("channel_path", channelPath, std::string("")); 38 | private_nh.param("curves_rate_path", curvesRatePath, std::string("")); 39 | private_nh.param("start_angle", start_angle_, int(0)); 40 | private_nh.param("end_angle", end_angle_, int(360)); 41 | 42 | ROS_INFO_STREAM("[cloud][rawdata] lidar model: " << model); 43 | if (start_angle_ < 0 || start_angle_ > 360 || end_angle_ < 0 || end_angle_ > 360) 44 | { 45 | start_angle_ = 0; 46 | end_angle_ = 360; 47 | ROS_INFO_STREAM("[cloud][rawdata] start and end angle select feature deactivated."); 48 | } 49 | else 50 | { 51 | ROS_INFO_STREAM("[cloud][rawdata] start and end angle feature activated."); 52 | } 53 | 54 | angle_flag_ = true; 55 | if (start_angle_ > end_angle_) 56 | { 57 | angle_flag_ = false; 58 | // ROS_INFO_STREAM("[cloud][rawdata] Start angle is smaller than end angle, not the normal state!"); 59 | } 60 | 61 | ROS_INFO_STREAM("[cloud][rawdata] start_angle: " << start_angle_ << " end_angle: " << end_angle_ 62 | << " angle_flag: " << angle_flag_); 63 | start_angle_ = start_angle_ * 100; 64 | end_angle_ = end_angle_ * 100; 65 | 66 | private_nh.param("max_distance", max_distance_, 200.0f); 67 | private_nh.param("min_distance", min_distance_, 0.4f); 68 | 69 | ROS_INFO_STREAM("[cloud][rawdata] distance threshlod, max: " << max_distance_ << " m, min: " << min_distance_ 70 | << " m"); 71 | 72 | intensity_mode_ = 3; 73 | info_print_flag_ = false; 74 | private_nh.param("resolution_type", resolution_param, std::string("0.5cm")); 75 | private_nh.param("intensity_mode", intensity_mode_, 1); 76 | 77 | if (resolution_param == "0.5cm") 78 | { 79 | dis_resolution_mode_ = 0; 80 | } 81 | else 82 | { 83 | dis_resolution_mode_ = 1; 84 | } 85 | 86 | ROS_INFO_STREAM("[cloud][rawdata] initialize resolution type: " << (dis_resolution_mode_ ? "1 cm" : "0.5 cm") 87 | << ", intensity mode: " << intensity_mode_); 88 | 89 | if (model == "RS16") 90 | { 91 | numOfLasers = 16; 92 | Rx_ = 0.03825; 93 | Ry_ = -0.01088; 94 | Rz_ = 0; 95 | } 96 | else if (model == "RS32") 97 | { 98 | numOfLasers = 32; 99 | TEMPERATURE_RANGE = 50; 100 | Rx_ = 0.03997; 101 | Ry_ = -0.01087; 102 | Rz_ = 0; 103 | isBpearlLidar_ = false; 104 | } 105 | else if(model == "RSBPEARL") 106 | { 107 | numOfLasers = 32; 108 | TEMPERATURE_RANGE = 50; 109 | Rx_ = 0.01697; 110 | Ry_ = -0.0085; 111 | Rz_ = 0.12644; 112 | isBpearlLidar_ = true; 113 | } 114 | else if (model == "RSBPEARL_MINI") 115 | { 116 | numOfLasers = 32; 117 | TEMPERATURE_RANGE = 50; 118 | Rx_ = 0.01473; 119 | Ry_ = 0.0085; 120 | Rz_ = 0.09427; 121 | isBpearlLidar_ = true; 122 | } 123 | else 124 | { 125 | std::cout << "Bad model!" << std::endl; 126 | } 127 | 128 | intensityFactor = 51; 129 | 130 | // return mode default 131 | return_mode_ = 1; 132 | 133 | // lookup table init 134 | this->cos_lookup_table_.resize(36000); 135 | this->sin_lookup_table_.resize(36000); 136 | for (unsigned int i = 0; i < 36000; i++) 137 | { 138 | double rad = RS_TO_RADS(i / 100.0f); 139 | 140 | this->cos_lookup_table_[i] = std::cos(rad); 141 | this->sin_lookup_table_[i] = std::sin(rad); 142 | } 143 | 144 | /// 读参数文件 2017-02-27 145 | FILE* f_inten = fopen(curvesPath.c_str(), "r"); 146 | int loopi = 0; 147 | int loopj = 0; 148 | int loop_num; 149 | if (!f_inten) 150 | { 151 | ROS_WARN_STREAM("[cloud][rawdata] " << curvesPath << " does not exist"); 152 | } 153 | else 154 | { 155 | fseek(f_inten, 0, SEEK_END); //定位到文件末 156 | int size = ftell(f_inten); //文件长度 157 | 158 | if (size > 10000) //老版的curve 159 | { 160 | Curvesis_new = false; 161 | loop_num = 1600; 162 | } 163 | else 164 | { 165 | Curvesis_new = true; 166 | loop_num = 7; 167 | } 168 | fseek(f_inten, 0, SEEK_SET); 169 | while (!feof(f_inten)) 170 | { 171 | float a[32]; 172 | loopi++; 173 | 174 | if (loopi > loop_num) 175 | break; 176 | if (numOfLasers == 16) 177 | { 178 | int tmp = fscanf(f_inten, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n", &a[0], &a[1], &a[2], &a[3], 179 | &a[4], &a[5], &a[6], &a[7], &a[8], &a[9], &a[10], &a[11], &a[12], &a[13], &a[14], &a[15]); 180 | } 181 | else if (numOfLasers == 32) 182 | { 183 | int tmp = fscanf(f_inten, 184 | "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f," 185 | "%f,%f,%f,%f\n", 186 | &a[0], &a[1], &a[2], &a[3], &a[4], &a[5], &a[6], &a[7], &a[8], &a[9], &a[10], &a[11], &a[12], 187 | &a[13], &a[14], &a[15], &a[16], &a[17], &a[18], &a[19], &a[20], &a[21], &a[22], &a[23], &a[24], 188 | &a[25], &a[26], &a[27], &a[28], &a[29], &a[30], &a[31]); 189 | } 190 | if (Curvesis_new) 191 | { 192 | for (loopj = 0; loopj < numOfLasers; loopj++) 193 | { 194 | aIntensityCal[loopi - 1][loopj] = a[loopj]; 195 | } 196 | } 197 | else 198 | { 199 | for (loopj = 0; loopj < numOfLasers; loopj++) 200 | { 201 | aIntensityCal_old[loopi - 1][loopj] = a[loopj]; 202 | } 203 | } 204 | // ROS_INFO_STREAM("new is " << a[0]); 205 | } 206 | fclose(f_inten); 207 | } 208 | //============================================================= 209 | FILE* f_angle = fopen(anglePath.c_str(), "r"); 210 | if (!f_angle) 211 | { 212 | ROS_WARN_STREAM("[cloud][rawdata] " << anglePath << " does not exist"); 213 | } 214 | else 215 | { 216 | float b[32], d[32]; 217 | int loopk = 0; 218 | int loopn = 0; 219 | while (!feof(f_angle)) 220 | { 221 | int tmp = fscanf(f_angle, "%f,%f\n", &b[loopk], &d[loopk]); 222 | loopk++; 223 | if (loopk > (numOfLasers - 1)) 224 | break; 225 | } 226 | for (loopn = 0; loopn < numOfLasers; loopn++) 227 | { 228 | VERT_ANGLE[loopn] = b[loopn] * 100; 229 | HORI_ANGLE[loopn] = d[loopn] * 100; 230 | } 231 | 232 | if (numOfLasers == 16) 233 | { 234 | memset(HORI_ANGLE, 0, sizeof(HORI_ANGLE)); 235 | } 236 | 237 | fclose(f_angle); 238 | } 239 | 240 | //============================================================= 241 | FILE* f_channel = fopen(channelPath.c_str(), "r"); 242 | if (!f_channel) 243 | { 244 | ROS_WARN_STREAM("[cloud][rawdata] " << channelPath << " does not exist"); 245 | } 246 | else 247 | { 248 | int loopl = 0; 249 | int loopm = 0; 250 | int c[51]; 251 | int tempMode = 1; 252 | while (!feof(f_channel)) 253 | { 254 | if (numOfLasers == 16) 255 | { 256 | int tmp = fscanf(f_channel, 257 | "%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%" 258 | "d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n", 259 | &c[0], &c[1], &c[2], &c[3], &c[4], &c[5], &c[6], &c[7], &c[8], &c[9], &c[10], &c[11], &c[12], 260 | &c[13], &c[14], &c[15], &c[16], &c[17], &c[18], &c[19], &c[20], &c[21], &c[22], &c[23], &c[24], 261 | &c[25], &c[26], &c[27], &c[28], &c[29], &c[30], &c[31], &c[32], &c[33], &c[34], &c[35], &c[36], 262 | &c[37], &c[38], &c[39], &c[40]); 263 | } 264 | else 265 | { 266 | int tmp = fscanf(f_channel, 267 | "%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%" 268 | "d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n", 269 | &c[0], &c[1], &c[2], &c[3], &c[4], &c[5], &c[6], &c[7], &c[8], &c[9], &c[10], &c[11], &c[12], 270 | &c[13], &c[14], &c[15], &c[16], &c[17], &c[18], &c[19], &c[20], &c[21], &c[22], &c[23], &c[24], 271 | &c[25], &c[26], &c[27], &c[28], &c[29], &c[30], &c[31], &c[32], &c[33], &c[34], &c[35], &c[36], 272 | &c[37], &c[38], &c[39], &c[40], &c[41], &c[42], &c[43], &c[44], &c[45], &c[46], &c[47], &c[48], 273 | &c[49], &c[50]); 274 | } 275 | for (loopl = 0; loopl < TEMPERATURE_RANGE + 1; loopl++) 276 | { 277 | g_ChannelNum[loopm][loopl] = c[tempMode * loopl]; 278 | } 279 | loopm++; 280 | if (loopm > (numOfLasers - 1)) 281 | { 282 | break; 283 | } 284 | } 285 | fclose(f_channel); 286 | } 287 | 288 | if (numOfLasers == 32) 289 | { 290 | FILE* f_curvesRate = fopen(curvesRatePath.c_str(), "r"); 291 | if (!f_curvesRate) 292 | { 293 | ROS_WARN_STREAM("[cloud][rawdata] " << curvesRatePath << " does not exist"); 294 | for (int i = 0; i < 32; ++i) 295 | { 296 | CurvesRate[i] = 1.0; 297 | } 298 | } 299 | else 300 | { 301 | int loopk = 0; 302 | while (!feof(f_curvesRate)) 303 | { 304 | int tmp = fscanf(f_curvesRate, "%f\n", &CurvesRate[loopk]); 305 | loopk++; 306 | if (loopk > (numOfLasers - 1)) 307 | break; 308 | } 309 | fclose(f_curvesRate); 310 | } 311 | } 312 | 313 | // receive difop data 314 | // subscribe to difop rslidar packets, if not right correct data in difop, it will not revise the correct data in the 315 | // VERT_ANGLE, HORI_ANGLE etc. 316 | difop_sub_ = node.subscribe("rslidar_packets_difop", 10, &RawData::processDifop, (RawData*)this); 317 | temperature_pub_ = node.advertise("temperature", 10); 318 | } 319 | 320 | void RawData::processDifop(const rslidar_msgs::rslidarPacket::ConstPtr& difop_msg) 321 | { 322 | // std::cout << "Enter difop callback!" << std::endl; 323 | const uint8_t* data = &difop_msg->data[0]; 324 | bool is_support_dual_return = false; 325 | 326 | // check header 327 | if (data[0] != 0xa5 || data[1] != 0xff || data[2] != 0x00 || data[3] != 0x5a) 328 | { 329 | return; 330 | } 331 | 332 | // uint8_t save_return_mode = return_mode_; 333 | // return mode check 334 | if ((data[45] == 0x08 && data[46] == 0x02 && data[47] >= 0x09) || (data[45] > 0x08) || 335 | (data[45] == 0x08 && data[46] > 0x02)) 336 | { 337 | is_support_dual_return = true; 338 | if (data[300] == 0x01 || data[300] == 0x02) 339 | { 340 | return_mode_ = data[300]; 341 | } 342 | else 343 | { 344 | return_mode_ = 0; 345 | } 346 | } 347 | else 348 | { 349 | is_support_dual_return = false; 350 | return_mode_ = 1; 351 | } 352 | // 353 | if (!this->is_init_top_fw_) 354 | { 355 | if ((data[41] == 0x00 && data[42] == 0x00 && data[43] == 0x00) || 356 | (data[41] == 0xff && data[42] == 0xff && data[43] == 0xff) || 357 | (data[41] == 0x55 && data[42] == 0xaa && data[43] == 0x5a) || 358 | (data[41] == 0xe9 && data[42] == 0x01 && data[43] == 0x00)) 359 | { 360 | dis_resolution_mode_ = 1; // 1cm resolution 361 | // std::cout << "The distance resolution is 1cm" << std::endl; 362 | } 363 | else 364 | { 365 | dis_resolution_mode_ = 0; // 0.5cm resolution 366 | // std::cout << "The distance resolution is 0.5cm" << std::endl; 367 | } 368 | this->is_init_top_fw_ = true; 369 | } 370 | 371 | if (!this->is_init_curve_) 372 | { 373 | bool curve_flag = true; 374 | // check difop reigon has beed flashed the right data 375 | if ((data[50] == 0x00 || data[50] == 0xff) && (data[51] == 0x00 || data[51] == 0xff) && 376 | (data[52] == 0x00 || data[52] == 0xff) && (data[53] == 0x00 || data[53] == 0xff)) 377 | { 378 | curve_flag = false; 379 | } 380 | 381 | // TODO check why rsview here no 32 laser, be more careful the new, old version 382 | // Init curves 383 | if (curve_flag) 384 | { 385 | unsigned char checkbit; 386 | int bit1, bit2; 387 | for (int loopn = 0; loopn < numOfLasers; ++loopn) 388 | { 389 | // check the curves' parameter in difop 390 | checkbit = *(data + 50 + loopn * 15) ^ *(data + 50 + loopn * 15 + 1); 391 | for (int loopm = 1; loopm < 7; ++loopm) 392 | { 393 | checkbit = checkbit ^ (*(data + 50 + loopn * 15 + loopm * 2)) ^ (*(data + 50 + loopn * 15 + loopm * 2 + 1)); 394 | } 395 | if (checkbit != *(data + 50 + loopn * 15 + 14)) 396 | { 397 | return; 398 | } 399 | } 400 | for (int loopn = 0; loopn < numOfLasers; ++loopn) 401 | { 402 | // calculate curves' parameters 403 | bit1 = static_cast(*(data + 50 + loopn * 15)); 404 | bit2 = static_cast(*(data + 50 + loopn * 15 + 1)); 405 | aIntensityCal[0][loopn] = (bit1 * 256 + bit2) * 0.001; 406 | bit1 = static_cast(*(data + 50 + loopn * 15 + 2)); 407 | bit2 = static_cast(*(data + 50 + loopn * 15 + 3)); 408 | aIntensityCal[1][loopn] = (bit1 * 256 + bit2) * 0.001; 409 | bit1 = static_cast(*(data + 50 + loopn * 15 + 4)); 410 | bit2 = static_cast(*(data + 50 + loopn * 15 + 5)); 411 | aIntensityCal[2][loopn] = (bit1 * 256 + bit2) * 0.001; 412 | bit1 = static_cast(*(data + 50 + loopn * 15 + 6)); 413 | bit2 = static_cast(*(data + 50 + loopn * 15 + 7)); 414 | aIntensityCal[3][loopn] = (bit1 * 256 + bit2) * 0.001; 415 | bit1 = static_cast(*(data + 50 + loopn * 15 + 8)); 416 | bit2 = static_cast(*(data + 50 + loopn * 15 + 9)); 417 | aIntensityCal[4][loopn] = (bit1 * 256 + bit2) * 0.00001; 418 | bit1 = static_cast(*(data + 50 + loopn * 15 + 10)); 419 | bit2 = static_cast(*(data + 50 + loopn * 15 + 11)); 420 | aIntensityCal[5][loopn] = -(bit1 * 256 + bit2) * 0.0001; 421 | bit1 = static_cast(*(data + 50 + loopn * 15 + 12)); 422 | bit2 = static_cast(*(data + 50 + loopn * 15 + 13)); 423 | aIntensityCal[6][loopn] = (bit1 * 256 + bit2) * 0.001; 424 | } 425 | this->is_init_curve_ = true; 426 | ROS_INFO_STREAM("[cloud][rawdata] curves data is wrote in difop packet!"); 427 | Curvesis_new = true; 428 | } 429 | 430 | if ((data[290] != 0x00) && (data[290] != 0xff)) 431 | { 432 | intensityFactor = static_cast(*(data + 290)); // intensity factor introduced since than 20181115 433 | } 434 | 435 | if ((data[291] == 0x00) || (data[291] == 0xff) || (data[291] == 0xa1)) 436 | { 437 | intensity_mode_ = 1; // mode for the top firmware lower than T6R23V8(16) or T9R23V6(32) 438 | } 439 | else if (data[291] == 0xb1) 440 | { 441 | intensity_mode_ = 2; // mode for the top firmware higher than T6R23V8(16) or T9R23V6(32) 442 | } 443 | else if (data[291] == 0xc1) 444 | { 445 | intensity_mode_ = 3; // mode for the top firmware higher than T6R23V9 446 | } 447 | 448 | } 449 | 450 | if (!this->is_init_angle_) 451 | { 452 | // check header 453 | { 454 | bool angle_flag = true; 455 | if (numOfLasers == 16) 456 | { 457 | // check difop reigon has beed flashed the right data 458 | if ((data[1165] == 0x00 || data[1165] == 0xff) && (data[1166] == 0x00 || data[1166] == 0xff) && 459 | (data[1167] == 0x00 || data[1167] == 0xff) && (data[1168] == 0x00 || data[1168] == 0xff)) 460 | { 461 | angle_flag = false; 462 | } 463 | } 464 | else if (numOfLasers == 32) 465 | { 466 | if ((data[468] == 0x00 || data[468] == 0xff) && (data[469] == 0x00 || data[469] == 0xff) && 467 | (data[470] == 0x00 || data[470] == 0xff)) 468 | { 469 | angle_flag = false; 470 | } 471 | } 472 | 473 | // angle 474 | if (angle_flag) 475 | { 476 | // TODO check the HORI_ANGLE 477 | int bit1, bit2, bit3, symbolbit; 478 | if (numOfLasers == 16) 479 | { 480 | for (int loopn = 0; loopn < 16; ++loopn) 481 | { 482 | if (loopn < 8) 483 | { 484 | symbolbit = -1; 485 | } 486 | else 487 | { 488 | symbolbit = 1; 489 | } 490 | bit1 = static_cast(*(data + 1165 + loopn * 3)); 491 | bit2 = static_cast(*(data + 1165 + loopn * 3 + 1)); 492 | bit3 = static_cast(*(data + 1165 + loopn * 3 + 2)); 493 | VERT_ANGLE[loopn] = (bit1 * 256 * 256 + bit2 * 256 + bit3) * symbolbit * 0.01f; 494 | // std::cout << VERT_ANGLE[loopn] << std::endl; 495 | // TODO 496 | HORI_ANGLE[loopn] = 0; 497 | } 498 | } 499 | else if (numOfLasers == 32) 500 | { 501 | for (int loopn = 0; loopn < 32; ++loopn) 502 | { 503 | // vertical angle 504 | bit1 = static_cast(*(data + 468 + loopn * 3)); 505 | if (bit1 == 0) 506 | symbolbit = 1; 507 | else if (bit1 == 1) 508 | symbolbit = -1; 509 | bit2 = static_cast(*(data + 468 + loopn * 3 + 1)); 510 | bit3 = static_cast(*(data + 468 + loopn * 3 + 2)); 511 | if (isBpearlLidar_) 512 | VERT_ANGLE[loopn] = (bit2 * 256 + bit3) * symbolbit * 0.01f * 100; 513 | else 514 | VERT_ANGLE[loopn] = (bit2 * 256 + bit3) * symbolbit * 0.001f * 100; 515 | // horizontal offset angle 516 | bit1 = static_cast(*(data + 564 + loopn * 3)); 517 | if (bit1 == 0) 518 | symbolbit = 1; 519 | else if (bit1 == 1) 520 | symbolbit = -1; 521 | bit2 = static_cast(*(data + 564 + loopn * 3 + 1)); 522 | bit3 = static_cast(*(data + 564 + loopn * 3 + 2)); 523 | if (isBpearlLidar_) 524 | HORI_ANGLE[loopn] = (bit2 * 256 + bit3) * symbolbit * 0.01f * 100; 525 | else 526 | HORI_ANGLE[loopn] = (bit2 * 256 + bit3) * symbolbit * 0.001f * 100; 527 | } 528 | } 529 | 530 | this->is_init_angle_ = true; 531 | ROS_INFO_STREAM("[cloud][rawdata] angle data is wrote in difop packet!"); 532 | } 533 | } 534 | } 535 | 536 | if (!info_print_flag_) 537 | { 538 | info_print_flag_ = true; 539 | 540 | // print info 541 | float prin_dis; 542 | if (dis_resolution_mode_ == 0) 543 | { 544 | prin_dis = 0.5; 545 | } 546 | else 547 | { 548 | prin_dis = 1; 549 | } 550 | ROS_INFO_STREAM("[cloud][rawdata] distance resolution is: " << prin_dis << " cm, intensity mode is: Mode " 551 | << intensity_mode_); 552 | if (is_support_dual_return == false) 553 | { 554 | ROS_INFO_STREAM("[cloud][rawdata] lidar only support single return wave!"); 555 | } 556 | else 557 | { 558 | if (return_mode_ == 0) 559 | { 560 | ROS_INFO_STREAM("[cloud][rawdata] lidar support dual return wave, the current mode is: dual"); 561 | } 562 | else if (return_mode_ == 1) 563 | { 564 | ROS_INFO_STREAM("[cloud][rawdata] lidar support dual return wave, the current mode is: strongest"); 565 | } 566 | else if (return_mode_ == 2) 567 | { 568 | ROS_INFO_STREAM("[cloud][rawdata] lidar support dual return wave, the current mode is: last"); 569 | } 570 | } 571 | 572 | ROS_INFO_STREAM("[cloud][rawdata] difop intensity mode: "< 0.0 && azimuth_f < 3000.0) 595 | { 596 | azimuth_f = azimuth_f + HORI_ANGLE[passageway] + 36000.0f; 597 | } 598 | else 599 | { 600 | azimuth_f = azimuth_f + HORI_ANGLE[passageway]; 601 | } 602 | azimuth = (int)azimuth_f; 603 | azimuth = ((azimuth%36000)+36000)%36000; 604 | 605 | return azimuth; 606 | } 607 | 608 | //------------------------------------------------------------ 609 | //校准反射强度值 610 | float RawData::calibrateIntensity(float intensity, int calIdx, int distance) 611 | { 612 | if (intensity_mode_ == 3) 613 | { 614 | return intensity; 615 | } 616 | else 617 | { 618 | int algDist; 619 | int sDist; 620 | float realPwr; 621 | float refPwr; 622 | float tempInten; 623 | float distance_f; 624 | float endOfSection1, endOfSection2; 625 | int temp = estimateTemperature(temper); 626 | 627 | realPwr = std::max((float)(intensity / (1 + (temp - TEMPERATURE_MIN) / 24.0f)), 1.0f); 628 | 629 | if (intensity_mode_ == 1) 630 | { 631 | // transform the one byte intensity value to two byte 632 | if ((int)realPwr < 126) 633 | realPwr = realPwr * 4.0f; 634 | else if ((int)realPwr >= 126 && (int)realPwr < 226) 635 | realPwr = (realPwr - 125.0f) * 16.0f + 500.0f; 636 | else 637 | realPwr = (realPwr - 225.0f) * 256.0f + 2100.0f; 638 | } 639 | else if (intensity_mode_ == 2) 640 | { 641 | // the caculation for the firmware after T6R23V8(16) and T9R23V6(32) 642 | if ((int)realPwr < 64) 643 | realPwr = realPwr; 644 | else if ((int)realPwr >= 64 && (int)realPwr < 176) 645 | realPwr = (realPwr - 64.0f) * 4.0f + 64.0f; 646 | else 647 | realPwr = (realPwr - 176.0f) * 16.0f + 512.0f; 648 | } 649 | else 650 | { 651 | ROS_WARN("[cloud][rawdata] The intensity mode is not right"); 652 | } 653 | 654 | int indexTemper = estimateTemperature(temper) - TEMPERATURE_MIN; 655 | 656 | // limit sDist 657 | sDist = (distance > g_ChannelNum[calIdx][indexTemper]) ? distance : g_ChannelNum[calIdx][indexTemper]; 658 | 659 | // minus the static offset (this data is For the intensity cal useage only) 660 | algDist = sDist - g_ChannelNum[calIdx][indexTemper]; 661 | if (dis_resolution_mode_ == 0) 662 | { 663 | distance_f = (float)algDist * DISTANCE_RESOLUTION_NEW; 664 | } 665 | else 666 | { 667 | distance_f = (float)algDist * DISTANCE_RESOLUTION; 668 | } 669 | distance_f = (distance_f > this->max_distance_) ? this->max_distance_ : distance_f; 670 | 671 | // calculate intensity ref curves 672 | float refPwr_temp = 0.0f; 673 | int order = 3; 674 | endOfSection1 = 5.0f; 675 | endOfSection2 = 40.0; 676 | 677 | if (intensity_mode_ == 1) 678 | { 679 | if (distance_f <= endOfSection1) 680 | { 681 | refPwr_temp = aIntensityCal[0][calIdx] * exp(aIntensityCal[1][calIdx] - aIntensityCal[2][calIdx] * distance_f) + 682 | aIntensityCal[3][calIdx]; 683 | } 684 | else 685 | { 686 | for (int i = 0; i < order; i++) 687 | { 688 | refPwr_temp += aIntensityCal[i + 4][calIdx] * (pow(distance_f, order - 1 - i)); 689 | } 690 | } 691 | } 692 | else if (intensity_mode_ == 2) 693 | { 694 | if (distance_f <= endOfSection1) 695 | { 696 | refPwr_temp = aIntensityCal[0][calIdx] * exp(aIntensityCal[1][calIdx] - aIntensityCal[2][calIdx] * distance_f) + 697 | aIntensityCal[3][calIdx]; 698 | } 699 | else if (distance_f > endOfSection1 && distance_f <= endOfSection2) 700 | { 701 | for (int i = 0; i < order; i++) 702 | { 703 | refPwr_temp += aIntensityCal[i + 4][calIdx] * (pow(distance_f, order - 1 - i)); 704 | } 705 | } 706 | else 707 | { 708 | float refPwr_temp0 = 0.0f; 709 | float refPwr_temp1 = 0.0f; 710 | for (int i = 0; i < order; i++) 711 | { 712 | refPwr_temp0 += aIntensityCal[i + 4][calIdx] * (pow(40.0f, order - 1 - i)); 713 | refPwr_temp1 += aIntensityCal[i + 4][calIdx] * (pow(39.0f, order - 1 - i)); 714 | } 715 | refPwr_temp = 0.3f * (refPwr_temp0 - refPwr_temp1) * distance_f + refPwr_temp0; 716 | } 717 | } 718 | else 719 | { 720 | ROS_WARN("[cloud][rawdata] The intensity mode is not right"); 721 | } 722 | 723 | refPwr = std::max(std::min(refPwr_temp, 500.0f), 4.0f); 724 | 725 | tempInten = (intensityFactor * refPwr) / realPwr; 726 | if (numOfLasers == 32) 727 | { 728 | tempInten = tempInten * CurvesRate[calIdx]; 729 | } 730 | tempInten = (int)tempInten > 255 ? 255.0f : tempInten; 731 | return tempInten; 732 | } 733 | } 734 | 735 | //------------------------------------------------------------ 736 | //校准反射强度值 old 737 | float RawData::calibrateIntensity_old(float intensity, int calIdx, int distance) 738 | { 739 | int algDist; 740 | int sDist; 741 | int uplimitDist; 742 | float realPwr; 743 | float refPwr; 744 | float tempInten; 745 | 746 | int temp = estimateTemperature(temper); 747 | realPwr = std::max((float)(intensity / (1 + (temp - TEMPERATURE_MIN) / 24.0f)), 1.0f); 748 | // realPwr = intensity; 749 | 750 | if ((int)realPwr < 126) 751 | realPwr = realPwr * 4.0f; 752 | else if ((int)realPwr >= 126 && (int)realPwr < 226) 753 | realPwr = (realPwr - 125.0f) * 16.0f + 500.0f; 754 | else 755 | realPwr = (realPwr - 225.0f) * 256.0f + 2100.0f; 756 | 757 | int indexTemper = estimateTemperature(temper) - TEMPERATURE_MIN; 758 | uplimitDist = g_ChannelNum[calIdx][indexTemper] + 1400; 759 | sDist = (distance > g_ChannelNum[calIdx][indexTemper]) ? distance : g_ChannelNum[calIdx][indexTemper]; 760 | sDist = (sDist < uplimitDist) ? sDist : uplimitDist; 761 | // minus the static offset (this data is For the intensity cal useage only) 762 | algDist = sDist - g_ChannelNum[calIdx][indexTemper]; 763 | // algDist = algDist < 1400? algDist : 1399; 764 | refPwr = aIntensityCal_old[algDist][calIdx]; 765 | 766 | tempInten = (51 * refPwr) / realPwr; 767 | if (numOfLasers == 32) 768 | { 769 | tempInten = tempInten * CurvesRate[calIdx]; 770 | } 771 | tempInten = (int)tempInten > 255 ? 255.0f : tempInten; 772 | return tempInten; 773 | } 774 | 775 | //------------------------------------------------------------ 776 | int RawData::isABPacket(int distance) 777 | { 778 | int ABflag = 0; 779 | if ((distance & 32768) != 0) 780 | { 781 | ABflag = 1; // B 782 | } 783 | else 784 | { 785 | ABflag = 0; // A 786 | } 787 | return ABflag; 788 | } 789 | 790 | //------------------------------------------------------------ 791 | float RawData::computeTemperature(unsigned char bit1, unsigned char bit2) 792 | { 793 | float Temp; 794 | float bitneg = bit2 & 128; // 10000000 795 | float highbit = bit2 & 127; // 01111111 796 | float lowbit = bit1 >> 3; 797 | if (bitneg == 128) 798 | { 799 | Temp = -1 * (highbit * 32 + lowbit) * 0.0625f; 800 | } 801 | else 802 | { 803 | Temp = (highbit * 32 + lowbit) * 0.0625f; 804 | } 805 | 806 | return Temp; 807 | } 808 | 809 | //------------------------------------------------------------ 810 | int RawData::estimateTemperature(float Temper) 811 | { 812 | int temp = (int)floor(Temper + 0.5); 813 | if (temp < TEMPERATURE_MIN) 814 | { 815 | temp = TEMPERATURE_MIN; 816 | } 817 | else if (temp > TEMPERATURE_MIN + TEMPERATURE_RANGE) 818 | { 819 | temp = TEMPERATURE_MIN + TEMPERATURE_RANGE; 820 | } 821 | 822 | return temp; 823 | } 824 | //------------------------------------------------------------ 825 | 826 | /** @brief convert raw packet to point cloud 827 | * 828 | * @param pkt raw packet to unpack 829 | * @param pc shared pointer to point cloud (points are appended) 830 | */ 831 | void RawData::unpack(const rslidar_msgs::rslidarPacket& pkt, pcl::PointCloud::Ptr pointcloud) 832 | { 833 | // check pkt header 834 | if (pkt.data[0] != 0x55 || pkt.data[1] != 0xAA || pkt.data[2] != 0x05 || pkt.data[3] != 0x0A) 835 | { 836 | return; 837 | } 838 | 839 | if (numOfLasers == 32) 840 | { 841 | unpack_RS32(pkt, pointcloud); 842 | return; 843 | } 844 | float azimuth; // 0.01 dgree 845 | float intensity; 846 | float azimuth_diff; 847 | float azimuth_corrected_f; 848 | int azimuth_corrected; 849 | 850 | const raw_packet_t* raw = (const raw_packet_t*)&pkt.data[42]; 851 | 852 | if (tempPacketNum < 75 && tempPacketNum > 0) 853 | { 854 | tempPacketNum++; 855 | } 856 | else 857 | { 858 | temper = computeTemperature(pkt.data[38], pkt.data[39]); 859 | tempPacketNum = 1; 860 | std_msgs::Float32 temperature_msgs; 861 | temperature_msgs.data = temper; 862 | temperature_pub_.publish(temperature_msgs); 863 | } 864 | 865 | for (int block = 0; block < BLOCKS_PER_PACKET; block++, this->block_num++) // 1 packet:12 data blocks 866 | { 867 | if (UPPER_BANK != raw->blocks[block].header) 868 | { 869 | ROS_INFO_STREAM_THROTTLE(180, "[cloud][rawdata] skipping RSLIDAR DIFOP packet"); 870 | break; 871 | } 872 | 873 | 874 | azimuth = (float)(256 * raw->blocks[block].rotation_1 + raw->blocks[block].rotation_2); 875 | 876 | int azi1, azi2; 877 | if (block < (BLOCKS_PER_PACKET - 1)) // 12 878 | { 879 | // int azi1, azi2; 880 | azi1 = 256 * raw->blocks[block + 1].rotation_1 + raw->blocks[block + 1].rotation_2; 881 | azi2 = 256 * raw->blocks[block].rotation_1 + raw->blocks[block].rotation_2; 882 | } 883 | else 884 | { 885 | 886 | azi1 = 256 * raw->blocks[block].rotation_1 + raw->blocks[block].rotation_2; 887 | azi2 = 256 * raw->blocks[block - 1].rotation_1 + raw->blocks[block - 1].rotation_2; 888 | 889 | } 890 | uint16_t diff = (36000 + azi1 - azi2) % 36000; 891 | if (diff > 100) //to avoid when the lidar is set to specific FOV that cause the big difference between angle 892 | { 893 | diff = 0; 894 | } 895 | azimuth_diff = (float)(diff); 896 | 897 | for (int firing = 0, k = 0; firing < RS16_FIRINGS_PER_BLOCK; firing++) // 2 898 | { 899 | for (int dsr = 0; dsr < RS16_SCANS_PER_FIRING; dsr++, k += RAW_SCAN_SIZE) // 16 3 900 | { 901 | if (0 == return_mode_) 902 | { 903 | azimuth_corrected_f = azimuth + (azimuth_diff * (dsr * RS16_DSR_TOFFSET)) / RS16_FIRING_TOFFSET; 904 | } 905 | else 906 | { 907 | azimuth_corrected_f = azimuth + (azimuth_diff * ((dsr * RS16_DSR_TOFFSET) + (firing * RS16_FIRING_TOFFSET)) / 908 | RS16_BLOCK_TDURATION); 909 | } 910 | azimuth_corrected = ((int)round(azimuth_corrected_f)) % 36000; // convert to integral value... 911 | 912 | union two_bytes tmp; 913 | tmp.bytes[1] = raw->blocks[block].data[k]; 914 | tmp.bytes[0] = raw->blocks[block].data[k + 1]; 915 | int distance = tmp.uint; 916 | 917 | // read intensity 918 | intensity = raw->blocks[block].data[k + 2]; 919 | if (Curvesis_new) 920 | intensity = calibrateIntensity(intensity, dsr, distance); 921 | else 922 | intensity = calibrateIntensity_old(intensity, dsr, distance); 923 | 924 | float distance2 = pixelToDistance(distance, dsr); 925 | if (dis_resolution_mode_ == 0) // distance resolution is 0.5cm 926 | { 927 | distance2 = distance2 * DISTANCE_RESOLUTION_NEW; 928 | } 929 | else 930 | { 931 | distance2 = distance2 * DISTANCE_RESOLUTION; 932 | } 933 | 934 | int arg_horiz = (azimuth_corrected + 36000) % 36000; 935 | int arg_horiz_orginal = arg_horiz; 936 | int arg_vert = ((VERT_ANGLE[dsr]) % 36000 + 36000) % 36000; 937 | 938 | pcl::PointXYZI point; 939 | 940 | if (distance2 > max_distance_ || distance2 < min_distance_ || 941 | (angle_flag_ && (arg_horiz < start_angle_ || arg_horiz > end_angle_)) || 942 | (!angle_flag_ && (arg_horiz > end_angle_ && arg_horiz < start_angle_))) // invalid distance 943 | { 944 | point.x = NAN; 945 | point.y = NAN; 946 | point.z = NAN; 947 | point.intensity = 0; 948 | pointcloud->at(2 * this->block_num + firing, dsr) = point; 949 | } 950 | else 951 | { 952 | // If you want to fix the rslidar X aixs to the front side of the cable, please use the two line below 953 | 954 | point.x = distance2 * this->cos_lookup_table_[arg_vert] * this->cos_lookup_table_[arg_horiz] + 955 | Rx_ * this->cos_lookup_table_[arg_horiz_orginal]; 956 | point.y = -distance2 * this->cos_lookup_table_[arg_vert] * this->sin_lookup_table_[arg_horiz] - 957 | Rx_ * this->sin_lookup_table_[arg_horiz_orginal]; 958 | point.z = distance2 * this->sin_lookup_table_[arg_vert] + Rz_; 959 | point.intensity = intensity; 960 | pointcloud->at(2 * this->block_num + firing, dsr) = point; 961 | } 962 | } 963 | } 964 | } 965 | } 966 | 967 | void RawData::unpack_RS32(const rslidar_msgs::rslidarPacket& pkt, pcl::PointCloud::Ptr pointcloud) 968 | { 969 | float azimuth; // 0.01 dgree 970 | float intensity; 971 | float azimuth_diff; 972 | float azimuth_corrected_f; 973 | int azimuth_corrected; 974 | 975 | const raw_packet_t* raw = (const raw_packet_t*)&pkt.data[42]; 976 | 977 | if (tempPacketNum < 150 && tempPacketNum > 0) 978 | { 979 | tempPacketNum++; 980 | } 981 | else 982 | { 983 | temper = computeTemperature(pkt.data[38], pkt.data[39]); 984 | tempPacketNum = 1; 985 | std_msgs::Float32 temperature_msgs; 986 | temperature_msgs.data = temper; 987 | temperature_pub_.publish(temperature_msgs); 988 | } 989 | 990 | for (int block = 0; block < BLOCKS_PER_PACKET; block++, this->block_num++) // 1 packet:12 data blocks 991 | { 992 | if (UPPER_BANK != raw->blocks[block].header) 993 | { 994 | ROS_INFO_STREAM_THROTTLE(180, "[cloud][rawdata] skipping RSLIDAR DIFOP packet"); 995 | break; 996 | } 997 | 998 | azimuth = (float)(256 * raw->blocks[block].rotation_1 + raw->blocks[block].rotation_2); 999 | 1000 | int azi1, azi2; 1001 | if (0 == return_mode_) 1002 | { // dual return mode 1003 | if (block < (BLOCKS_PER_PACKET - 2)) // 12 1004 | { 1005 | azi1 = 256 * raw->blocks[block + 2].rotation_1 + raw->blocks[block + 2].rotation_2; 1006 | azi2 = 256 * raw->blocks[block].rotation_1 + raw->blocks[block].rotation_2; 1007 | } 1008 | else 1009 | { 1010 | azi1 = 256 * raw->blocks[block].rotation_1 + raw->blocks[block].rotation_2; 1011 | azi2 = 256 * raw->blocks[block - 2].rotation_1 + raw->blocks[block - 2].rotation_2; 1012 | } 1013 | } 1014 | else 1015 | { 1016 | if (block < (BLOCKS_PER_PACKET - 1)) // 12 1017 | { 1018 | azi1 = 256 * raw->blocks[block + 1].rotation_1 + raw->blocks[block + 1].rotation_2; 1019 | azi2 = 256 * raw->blocks[block].rotation_1 + raw->blocks[block].rotation_2; 1020 | } 1021 | else 1022 | { 1023 | azi1 = 256 * raw->blocks[block].rotation_1 + raw->blocks[block].rotation_2; 1024 | azi2 = 256 * raw->blocks[block - 1].rotation_1 + raw->blocks[block - 1].rotation_2; 1025 | } 1026 | } 1027 | uint16_t diff = (36000 + azi1 - azi2) % 36000; 1028 | if (diff > 100) //to avoid when the lidar is set to specific FOV that cause the big difference between angle 1029 | { 1030 | diff = 0; 1031 | } 1032 | azimuth_diff = (float)(diff); 1033 | 1034 | if (dis_resolution_mode_ == 0) // distance resolution is 0.5cm and delete the AB packet mechanism 1035 | { 1036 | for (int dsr = 0, k = 0; dsr < RS32_SCANS_PER_FIRING * RS32_FIRINGS_PER_BLOCK; dsr++, k += RAW_SCAN_SIZE) // 16 3 1037 | { 1038 | int dsr_temp; 1039 | if (dsr >= 16) 1040 | { 1041 | dsr_temp = dsr - 16; 1042 | } 1043 | else 1044 | { 1045 | dsr_temp = dsr; 1046 | } 1047 | azimuth_corrected_f = azimuth + (azimuth_diff * ((dsr_temp * RS32_DSR_TOFFSET)) / RS32_BLOCK_TDURATION); 1048 | azimuth_corrected = correctAzimuth(azimuth_corrected_f, dsr); 1049 | 1050 | union two_bytes tmp; 1051 | tmp.bytes[1] = raw->blocks[block].data[k]; 1052 | tmp.bytes[0] = raw->blocks[block].data[k + 1]; 1053 | int distance = tmp.uint; 1054 | 1055 | // read intensity 1056 | intensity = (float)raw->blocks[block].data[k + 2]; 1057 | intensity = calibrateIntensity(intensity, dsr, distance); 1058 | 1059 | float distance2 = pixelToDistance(distance, dsr); 1060 | distance2 = distance2 * DISTANCE_RESOLUTION_NEW; 1061 | 1062 | int arg_horiz_orginal = (int)azimuth_corrected_f % 36000; 1063 | int arg_horiz = azimuth_corrected; 1064 | int arg_vert = ((VERT_ANGLE[dsr]) % 36000 + 36000) % 36000; 1065 | pcl::PointXYZI point; 1066 | 1067 | if (distance2 > max_distance_ || distance2 < min_distance_ || 1068 | (angle_flag_ && (arg_horiz < start_angle_ || arg_horiz > end_angle_)) || 1069 | (!angle_flag_ && (arg_horiz > end_angle_ && arg_horiz < start_angle_))) // invalid distance 1070 | { 1071 | point.x = NAN; 1072 | point.y = NAN; 1073 | point.z = NAN; 1074 | point.intensity = 0; 1075 | pointcloud->at(this->block_num, dsr) = point; 1076 | } 1077 | else 1078 | { 1079 | // If you want to fix the rslidar X aixs to the front side of the cable, please use the two line below 1080 | point.x = distance2 * this->cos_lookup_table_[arg_vert] * this->cos_lookup_table_[arg_horiz] + 1081 | Rx_ * this->cos_lookup_table_[arg_horiz_orginal]; 1082 | point.y = -distance2 * this->cos_lookup_table_[arg_vert] * this->sin_lookup_table_[arg_horiz] - 1083 | Rx_ * this->sin_lookup_table_[arg_horiz_orginal]; 1084 | point.z = distance2 * this->sin_lookup_table_[arg_vert] + Rz_; 1085 | point.intensity = intensity; 1086 | pointcloud->at(this->block_num, dsr) = point; 1087 | } 1088 | } 1089 | } 1090 | else 1091 | { 1092 | // Estimate the type of packet 1093 | union two_bytes tmp_flag; 1094 | tmp_flag.bytes[1] = raw->blocks[block].data[0]; 1095 | tmp_flag.bytes[0] = raw->blocks[block].data[1]; 1096 | int ABflag = isABPacket(tmp_flag.uint); 1097 | 1098 | int k = 0; 1099 | int index; 1100 | for (int dsr = 0; dsr < RS32_SCANS_PER_FIRING * RS32_FIRINGS_PER_BLOCK; dsr++, k += RAW_SCAN_SIZE) // 16 3 1101 | { 1102 | if (ABflag == 1 && dsr < 16) 1103 | { 1104 | index = k + 48; 1105 | } 1106 | else if (ABflag == 1 && dsr >= 16) 1107 | { 1108 | index = k - 48; 1109 | } 1110 | else 1111 | { 1112 | index = k; 1113 | } 1114 | 1115 | int dsr_temp; 1116 | if (dsr >= 16) 1117 | { 1118 | dsr_temp = dsr - 16; 1119 | } 1120 | else 1121 | { 1122 | dsr_temp = dsr; 1123 | } 1124 | azimuth_corrected_f = azimuth + (azimuth_diff * ((dsr_temp * RS32_DSR_TOFFSET)) / RS32_BLOCK_TDURATION); 1125 | azimuth_corrected = correctAzimuth(azimuth_corrected_f, dsr); 1126 | 1127 | union two_bytes tmp; 1128 | tmp.bytes[1] = raw->blocks[block].data[index]; 1129 | tmp.bytes[0] = raw->blocks[block].data[index + 1]; 1130 | int ab_flag_in_block = isABPacket(tmp.uint); 1131 | int distance = tmp.uint - ab_flag_in_block * 32768; 1132 | 1133 | // read intensity 1134 | intensity = (float)raw->blocks[block].data[index + 2]; 1135 | intensity = calibrateIntensity(intensity, dsr, distance); 1136 | 1137 | float distance2 = pixelToDistance(distance, dsr); 1138 | distance2 = distance2 * DISTANCE_RESOLUTION; 1139 | 1140 | int arg_horiz_orginal = (int)azimuth_corrected_f % 36000; 1141 | int arg_horiz = azimuth_corrected; 1142 | int arg_vert = ((VERT_ANGLE[dsr]) % 36000 + 36000) % 36000; 1143 | 1144 | pcl::PointXYZI point; 1145 | 1146 | if (distance2 > max_distance_ || distance2 < min_distance_ || 1147 | (angle_flag_ && (arg_horiz < start_angle_ || arg_horiz > end_angle_)) || 1148 | (!angle_flag_ && (arg_horiz > end_angle_ && arg_horiz < start_angle_))) // invalid distance 1149 | { 1150 | point.x = NAN; 1151 | point.y = NAN; 1152 | point.z = NAN; 1153 | point.intensity = 0; 1154 | pointcloud->at(this->block_num, dsr) = point; 1155 | } 1156 | else 1157 | { 1158 | // If you want to fix the rslidar X aixs to the front side of the cable, please use the two line below 1159 | point.x = distance2 * this->cos_lookup_table_[arg_vert] * this->cos_lookup_table_[arg_horiz] + 1160 | Rx_ * this->cos_lookup_table_[arg_horiz_orginal]; 1161 | point.y = -distance2 * this->cos_lookup_table_[arg_vert] * this->sin_lookup_table_[arg_horiz] - 1162 | Rx_ * this->sin_lookup_table_[arg_horiz_orginal]; 1163 | point.z = distance2 * this->sin_lookup_table_[arg_vert] + Rz_; 1164 | point.intensity = intensity; 1165 | pointcloud->at(this->block_num, dsr) = point; 1166 | } 1167 | } 1168 | } 1169 | } 1170 | } 1171 | 1172 | } // namespace rslidar_rawdata 1173 | -------------------------------------------------------------------------------- /rslidar_pointcloud/src/rawdata.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2018-2020 Robosense Authors 3 | * License: Modified BSD Software License Agreement 4 | * 5 | * $Id$ 6 | */ 7 | 8 | /** @file 9 | * 10 | * @brief Interfaces for interpreting raw packets from the Robosense 3D LIDAR. 11 | * 12 | */ 13 | 14 | #ifndef _RAWDATA_H 15 | #define _RAWDATA_H 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include "std_msgs/String.h" 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | namespace rslidar_rawdata 29 | { 30 | // static const float ROTATION_SOLUTION_ = 0.18f; //水平角分辨率 10hz 31 | static const int SIZE_BLOCK = 100; 32 | static const int RAW_SCAN_SIZE = 3; 33 | static const int SCANS_PER_BLOCK = 32; 34 | static const int BLOCK_DATA_SIZE = (SCANS_PER_BLOCK * RAW_SCAN_SIZE); // 96 35 | 36 | static const float ROTATION_RESOLUTION = 0.01f; /**< degrees 旋转角分辨率*/ 37 | static const uint16_t ROTATION_MAX_UNITS = 36000; /**< hundredths of degrees */ 38 | 39 | static const float DISTANCE_MAX = 200.0f; /**< meters */ 40 | static const float DISTANCE_MIN = 0.4f; /**< meters */ 41 | static const float DISTANCE_RESOLUTION = 0.01f; /**< meters */ 42 | static const float DISTANCE_RESOLUTION_NEW = 0.005f; /**< meters */ 43 | static const float DISTANCE_MAX_UNITS = (DISTANCE_MAX / DISTANCE_RESOLUTION + 1.0f); 44 | /** @todo make this work for both big and little-endian machines */ 45 | static const uint16_t UPPER_BANK = 0xeeff; // 46 | static const uint16_t LOWER_BANK = 0xddff; 47 | 48 | /** Special Defines for RS16 support **/ 49 | static const int RS16_FIRINGS_PER_BLOCK = 2; 50 | static const int RS16_SCANS_PER_FIRING = 16; 51 | static const float RS16_BLOCK_TDURATION = 100.0f; // [µs] 52 | static const float RS16_DSR_TOFFSET = 3.0f; // [µs] 53 | static const float RS16_FIRING_TOFFSET = 50.0f; // [µs] 54 | 55 | /** Special Defines for RS32 support **/ 56 | static const int RS32_FIRINGS_PER_BLOCK = 1; 57 | static const int RS32_SCANS_PER_FIRING = 32; 58 | static const float RS32_BLOCK_TDURATION = 50.0f; // [µs] 59 | static const float RS32_DSR_TOFFSET = 3.0f; // [µs] 60 | static const float RL32_FIRING_TOFFSET = 50.0f; // [µs] 61 | 62 | static const int TEMPERATURE_MIN = 31; 63 | 64 | #define RS_TO_RADS(x) ((x) * (M_PI) / 180) 65 | /** \brief Raw rslidar data block. 66 | * 67 | * Each block contains data from either the upper or lower laser 68 | * bank. The device returns three times as many upper bank blocks. 69 | * 70 | * use stdint.h types, so things work with both 64 and 32-bit machines 71 | */ 72 | // block 73 | typedef struct raw_block 74 | { 75 | uint16_t header; ///< UPPER_BANK or LOWER_BANK 76 | uint8_t rotation_1; 77 | uint8_t rotation_2; /// combine rotation1 and rotation2 together to get 0-35999, divide by 100 to get degrees 78 | uint8_t data[BLOCK_DATA_SIZE]; // 96 79 | } raw_block_t; 80 | 81 | /** used for unpacking the first two data bytes in a block 82 | * 83 | * They are packed into the actual data stream misaligned. I doubt 84 | * this works on big endian machines. 85 | */ 86 | union two_bytes 87 | { 88 | uint16_t uint; 89 | uint8_t bytes[2]; 90 | }; 91 | 92 | static const int PACKET_SIZE = 1248; 93 | static const int BLOCKS_PER_PACKET = 12; 94 | static const int PACKET_STATUS_SIZE = 4; 95 | static const int SCANS_PER_PACKET = (SCANS_PER_BLOCK * BLOCKS_PER_PACKET); 96 | 97 | /** \brief Raw Rsldar packet. 98 | * 99 | * revolution is described in the device manual as incrementing 100 | * (mod 65536) for each physical turn of the device. Our device 101 | * seems to alternate between two different values every third 102 | * packet. One value increases, the other decreases. 103 | * 104 | * \todo figure out if revolution is only present for one of the 105 | * two types of status fields 106 | * 107 | * status has either a temperature encoding or the microcode level 108 | */ 109 | typedef struct raw_packet 110 | { 111 | raw_block_t blocks[BLOCKS_PER_PACKET]; 112 | uint16_t revolution; 113 | uint8_t status[PACKET_STATUS_SIZE]; 114 | } raw_packet_t; 115 | 116 | /** \brief RSLIDAR data conversion class */ 117 | class RawData 118 | { 119 | public: 120 | RawData(); 121 | 122 | ~RawData() 123 | { 124 | this->cos_lookup_table_.clear(); 125 | this->sin_lookup_table_.clear(); 126 | } 127 | 128 | /*load the cablibrated files: angle, distance, intensity*/ 129 | void loadConfigFile(ros::NodeHandle node, ros::NodeHandle private_nh); 130 | 131 | /*unpack the RS16 UDP packet and opuput PCL PointXYZI type*/ 132 | void unpack(const rslidar_msgs::rslidarPacket& pkt, pcl::PointCloud::Ptr pointcloud); 133 | 134 | /*unpack the RS32 UDP packet and opuput PCL PointXYZI type*/ 135 | void unpack_RS32(const rslidar_msgs::rslidarPacket& pkt, pcl::PointCloud::Ptr pointcloud); 136 | 137 | /*compute temperature*/ 138 | float computeTemperature(unsigned char bit1, unsigned char bit2); 139 | 140 | /*estimate temperature*/ 141 | int estimateTemperature(float Temper); 142 | 143 | /*calibrated the disctance*/ 144 | float pixelToDistance(int pixelValue, int passageway); 145 | 146 | /*calibrated the azimuth*/ 147 | int correctAzimuth(float azimuth_f, int passageway); 148 | 149 | /*calibrated the intensity*/ 150 | float calibrateIntensity(float inten, int calIdx, int distance); 151 | float calibrateIntensity_old(float inten, int calIdx, int distance); 152 | 153 | /*estimate the packet type*/ 154 | int isABPacket(int distance); 155 | 156 | void processDifop(const rslidar_msgs::rslidarPacket::ConstPtr& difop_msg); 157 | ros::Subscriber difop_sub_; 158 | ros::Publisher temperature_pub_; 159 | bool is_init_curve_; 160 | bool is_init_angle_; 161 | bool is_init_top_fw_; 162 | int block_num = 0; 163 | int intensity_mode_; 164 | int intensityFactor; 165 | 166 | private: 167 | float Rx_; // the optical center position in the lidar coordination in x direction 168 | float Ry_; // the optical center position in the lidar coordination in y direction, for now not used 169 | float Rz_; // the optical center position in the lidar coordination in z direction 170 | int start_angle_; 171 | int end_angle_; 172 | float max_distance_; 173 | float min_distance_; 174 | int dis_resolution_mode_; 175 | int return_mode_; 176 | bool info_print_flag_; 177 | bool isBpearlLidar_; 178 | bool angle_flag_; 179 | 180 | /* cos/sin lookup table */ 181 | std::vector cos_lookup_table_; 182 | std::vector sin_lookup_table_; 183 | }; 184 | 185 | static int VERT_ANGLE[32]; 186 | static int HORI_ANGLE[32]; 187 | static float aIntensityCal[7][32]; 188 | static float aIntensityCal_old[1600][32]; 189 | static bool Curvesis_new = true; 190 | static int g_ChannelNum[32][51]; 191 | static float CurvesRate[32]; 192 | 193 | static float temper = 31.0; 194 | static int tempPacketNum = 0; 195 | static int numOfLasers = 16; 196 | static int TEMPERATURE_RANGE = 40; 197 | } // namespace rslidar_rawdata 198 | 199 | #endif // __RAWDATA_H 200 | -------------------------------------------------------------------------------- /rslidar_sync/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(rslidar_sync) 3 | find_package(catkin REQUIRED roscpp roslib std_msgs sensor_msgs message_filters) 4 | catkin_package() 5 | 6 | include_directories(include 7 | ${catkin_INCLUDE_DIRS}) 8 | 9 | add_executable(rslidar_sync_2lidar_node src/timesync_2lidar.cpp) 10 | target_link_libraries(rslidar_sync_2lidar_node 11 | ${catkin_LIBRARIES} 12 | ) 13 | 14 | add_executable(rslidar_sync_3lidar_node src/timesync_3lidar.cpp) 15 | target_link_libraries(rslidar_sync_3lidar_node 16 | ${catkin_LIBRARIES} 17 | ) 18 | -------------------------------------------------------------------------------- /rslidar_sync/README.md: -------------------------------------------------------------------------------- 1 | # rslidar_sync multilidar time synchronization package 2 | 3 | # How to use 4 | 5 | + Connect multiple lidars with the same GPS. The system time of the lidars will be the same with the GPS time. 6 | 7 | + Modify the lidar ip, port and namespace in the launch/rslidar_sync_nlidar.launch (n = 2 or 3). Please make sure the time_synchronization param value is true. 8 | 9 | + Build and launch the rslidar_sync_nlidar.launch. 10 | ``` 11 | roslaunch rslidar_sync rslidar_sync_2lidar.launch 12 | ``` 13 | or 14 | ``` 15 | roslaunch rslidar_sync rslidar_sync_3lidar.launch 16 | ``` 17 | 18 | + Check the time synchronization result. 19 | ``` 20 | rostopic echo /sync_packet_diff 21 | ``` 22 | It shows like this, n < 4 says good time synchronization. n >= 4 says bad result. 23 | ``` 24 | sync diff packets: n 25 | ``` 26 | If it doesn't show any message, please check the synchronization board or GPS connection. And you may also check the scanx_topic timestamp are near enough (no more than 0.1s). 27 | ``` 28 | rostopic echo /left/sync_header 29 | rostopic echo /right/sync_header 30 | rostopic echo /middle/sync_header 31 | ``` 32 | -------------------------------------------------------------------------------- /rslidar_sync/launch/left_lidar.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /rslidar_sync/launch/middle_lidar.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /rslidar_sync/launch/right_lidar.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /rslidar_sync/launch/rslidar_sync_2lidar.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /rslidar_sync/launch/rslidar_sync_3lidar.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /rslidar_sync/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rslidar_sync 4 | 1.0.0 5 | synchronizing lidar data with time stamps 6 | 7 | baoxianzhang 8 | baoxianzhang 9 | BSD 10 | 11 | catkin 12 | roscpp 13 | sensor_msgs 14 | std_msgs 15 | message_filters 16 | 17 | roscpp 18 | sensor_msgs 19 | std_msgs 20 | message_filters 21 | 22 | -------------------------------------------------------------------------------- /rslidar_sync/rviz_cfg/time_sync.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Lidar11 10 | - /Lidar21 11 | Splitter Ratio: 0.5 12 | Tree Height: 775 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.588679016 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: Lidar1 32 | Visualization Manager: 33 | Class: "" 34 | Displays: 35 | - Alpha: 0.5 36 | Cell Size: 1 37 | Class: rviz/Grid 38 | Color: 160; 160; 164 39 | Enabled: true 40 | Line Style: 41 | Line Width: 0.0299999993 42 | Value: Lines 43 | Name: Grid 44 | Normal Cell Count: 0 45 | Offset: 46 | X: 0 47 | Y: 0 48 | Z: 0 49 | Plane: XY 50 | Plane Cell Count: 10 51 | Reference Frame: 52 | Value: true 53 | - Alpha: 1 54 | Autocompute Intensity Bounds: true 55 | Autocompute Value Bounds: 56 | Max Value: 10 57 | Min Value: -10 58 | Value: true 59 | Axis: Z 60 | Channel Name: intensity 61 | Class: rviz/PointCloud2 62 | Color: 255; 255; 255 63 | Color Transformer: Intensity 64 | Decay Time: 0 65 | Enabled: true 66 | Invert Rainbow: false 67 | Max Color: 255; 255; 255 68 | Max Intensity: 255.949997 69 | Min Color: 0; 0; 0 70 | Min Intensity: 0.251422614 71 | Name: Lidar1 72 | Position Transformer: XYZ 73 | Queue Size: 10 74 | Selectable: true 75 | Size (Pixels): 3 76 | Size (m): 0.0500000007 77 | Style: Flat Squares 78 | Topic: /ns1/rslidar_points 79 | Unreliable: false 80 | Use Fixed Frame: true 81 | Use rainbow: true 82 | Value: true 83 | - Alpha: 1 84 | Autocompute Intensity Bounds: true 85 | Autocompute Value Bounds: 86 | Max Value: 10 87 | Min Value: -10 88 | Value: true 89 | Axis: Z 90 | Channel Name: intensity 91 | Class: rviz/PointCloud2 92 | Color: 255; 255; 255 93 | Color Transformer: Intensity 94 | Decay Time: 0 95 | Enabled: true 96 | Invert Rainbow: false 97 | Max Color: 255; 255; 255 98 | Max Intensity: 255.389999 99 | Min Color: 0; 0; 0 100 | Min Intensity: 0.26383543 101 | Name: Lidar2 102 | Position Transformer: XYZ 103 | Queue Size: 10 104 | Selectable: true 105 | Size (Pixels): 3 106 | Size (m): 0.0500000007 107 | Style: Flat Squares 108 | Topic: /ns2/rslidar_points 109 | Unreliable: false 110 | Use Fixed Frame: true 111 | Use rainbow: true 112 | Value: true 113 | Enabled: true 114 | Global Options: 115 | Background Color: 48; 48; 48 116 | Default Light: true 117 | Fixed Frame: rslidar 118 | Frame Rate: 30 119 | Name: root 120 | Tools: 121 | - Class: rviz/Interact 122 | Hide Inactive Objects: true 123 | - Class: rviz/MoveCamera 124 | - Class: rviz/Select 125 | - Class: rviz/FocusCamera 126 | - Class: rviz/Measure 127 | - Class: rviz/SetInitialPose 128 | Topic: /initialpose 129 | - Class: rviz/SetGoal 130 | Topic: /move_base_simple/goal 131 | - Class: rviz/PublishPoint 132 | Single click: true 133 | Topic: /clicked_point 134 | Value: true 135 | Views: 136 | Current: 137 | Class: rviz/Orbit 138 | Distance: 39.3154106 139 | Enable Stereo Rendering: 140 | Stereo Eye Separation: 0.0599999987 141 | Stereo Focal Distance: 1 142 | Swap Stereo Eyes: false 143 | Value: false 144 | Focal Point: 145 | X: -4.96815443 146 | Y: -4.25982523 147 | Z: 0.00407630112 148 | Focal Shape Fixed Size: true 149 | Focal Shape Size: 0.0500000007 150 | Invert Z Axis: false 151 | Name: Current View 152 | Near Clip Distance: 0.00999999978 153 | Pitch: 1.56979632 154 | Target Frame: 155 | Value: Orbit (rviz) 156 | Yaw: 5.79858398 157 | Saved: ~ 158 | Window Geometry: 159 | Displays: 160 | collapsed: false 161 | Height: 1056 162 | Hide Left Dock: false 163 | Hide Right Dock: false 164 | QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004ba0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 165 | Selection: 166 | collapsed: false 167 | Time: 168 | collapsed: false 169 | Tool Properties: 170 | collapsed: false 171 | Views: 172 | collapsed: false 173 | Width: 1855 174 | X: 1985 175 | Y: 24 176 | -------------------------------------------------------------------------------- /rslidar_sync/src/timesync_2lidar.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2018-2020 Robosense Authors 3 | * License: Modified BSD Software License Agreement 4 | * 5 | * $Id$ 6 | */ 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | ros::Publisher g_skippackets_num_pub1; 17 | ros::Publisher g_skippackets_num_pub2; 18 | ros::Publisher g_maxnum_diff_packetnum_pub; 19 | int g_packet_duration_us = 1200; 20 | bool comparePair(const std::pair a, const std::pair b) 21 | { 22 | return a.second < b.second; 23 | } 24 | 25 | void scanCallback(const sensor_msgs::TimeReference::ConstPtr& scan_msg1, 26 | const sensor_msgs::TimeReference::ConstPtr& scan_msg2) 27 | { 28 | // calculate the first packet timestamp difference (us) 29 | double time1 = scan_msg1->header.stamp.sec * 1e6 + scan_msg1->header.stamp.nsec * 0.001; 30 | double time2 = scan_msg2->header.stamp.sec * 1e6 + scan_msg2->header.stamp.nsec * 0.001; 31 | 32 | std::vector > lidar_vector; 33 | lidar_vector.push_back(std::make_pair(1, time1)); 34 | lidar_vector.push_back(std::make_pair(2, time2)); 35 | std::sort(lidar_vector.begin(), lidar_vector.end(), comparePair); 36 | unsigned delta_skip = (lidar_vector[1].second - lidar_vector[0].second) / g_packet_duration_us; 37 | 38 | std_msgs::String msg; 39 | std::stringstream ss; 40 | ss << "sync diff packets: " << delta_skip; 41 | ; 42 | msg.data = ss.str(); 43 | g_maxnum_diff_packetnum_pub.publish(msg); 44 | 45 | std_msgs::Int32 skip_num; 46 | skip_num.data = 1; 47 | if (delta_skip > 0) 48 | { 49 | if (lidar_vector[0].first == 1) 50 | { 51 | g_skippackets_num_pub1.publish(skip_num); 52 | } 53 | else if (lidar_vector[0].first == 2) 54 | { 55 | g_skippackets_num_pub2.publish(skip_num); 56 | } 57 | } 58 | else 59 | { 60 | // std::cout << "Synchronizer!" << std::endl; 61 | } 62 | } 63 | 64 | int main(int argc, char** argv) 65 | { 66 | ros::init(argc, argv, "rs_sync"); 67 | ros::NodeHandle nh; 68 | ros::NodeHandle nh_private("~"); 69 | 70 | // get the topic parameters 71 | std::string scan1_topic("/left/sync_header"); 72 | std::string scan2_topic("/right/sync_header"); 73 | if (!nh_private.getParam(std::string("scan1_topic"), scan1_topic)) 74 | { 75 | ROS_ERROR_STREAM("[sync-2] Can't get scan1_topic, use the default scan1_topic: "< scan_sub1(nh, scan1_topic, 1); 130 | message_filters::Subscriber scan_sub2(nh, scan2_topic, 1); 131 | typedef message_filters::sync_policies::ApproximateTime 132 | ScanSyncPolicy; 133 | message_filters::Synchronizer scan_sync(ScanSyncPolicy(10), scan_sub1, scan_sub2); 134 | scan_sync.registerCallback(boost::bind(&scanCallback, _1, _2)); 135 | 136 | g_skippackets_num_pub1 = nh.advertise(skippackets1_topic, 1, true); 137 | g_skippackets_num_pub2 = nh.advertise(skippackets2_topic, true); 138 | 139 | std::string sync_packet_diff_topic("/sync_packet_diff"); 140 | if (!nh_private.getParam(std::string("sync_packet_diff_topic"), sync_packet_diff_topic)) 141 | { 142 | ROS_ERROR_STREAM("[sync-2] Can't get sync_packet_diff_topic, use the default sync_packet_diff_topic: " 143 | <(sync_packet_diff_topic, 1, true); 151 | 152 | ros::spin(); 153 | return 0; 154 | } 155 | -------------------------------------------------------------------------------- /rslidar_sync/src/timesync_3lidar.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2018-2020 Robosense Authors 3 | * License: Modified BSD Software License Agreement 4 | * 5 | * $Id$ 6 | */ 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | ros::Publisher g_skippackets_num_pub1; 17 | ros::Publisher g_skippackets_num_pub2; 18 | ros::Publisher g_skippackets_num_pub3; 19 | ros::Publisher g_maxnum_diff_packetnum_pub; 20 | int g_packet_duration_us = 1200; 21 | bool comparePair(const std::pair a, const std::pair b) 22 | { 23 | return a.second < b.second; 24 | } 25 | 26 | void scanCallback(const sensor_msgs::TimeReference::ConstPtr& scan_msg1, 27 | const sensor_msgs::TimeReference::ConstPtr& scan_msg2, 28 | const sensor_msgs::TimeReference::ConstPtr scan_msg3) 29 | { 30 | // calculate the first packet timestamp difference (us) 31 | double time1 = scan_msg1->header.stamp.sec * 1e6 + scan_msg1->header.stamp.nsec * 0.001; 32 | double time2 = scan_msg2->header.stamp.sec * 1e6 + scan_msg2->header.stamp.nsec * 0.001; 33 | double time3 = scan_msg3->header.stamp.sec * 1e6 + scan_msg3->header.stamp.nsec * 0.001; 34 | 35 | std::vector > lidar_vector; 36 | lidar_vector.push_back(std::make_pair(1, time1)); 37 | lidar_vector.push_back(std::make_pair(2, time2)); 38 | lidar_vector.push_back(std::make_pair(3, time3)); 39 | 40 | std::sort(lidar_vector.begin(), lidar_vector.end(), comparePair); 41 | 42 | unsigned skip_big = (lidar_vector[2].second - lidar_vector[0].second) / g_packet_duration_us; 43 | unsigned skip_small = (lidar_vector[1].second - lidar_vector[0].second) / g_packet_duration_us; 44 | 45 | std_msgs::String msg; 46 | std::stringstream ss; 47 | ss << "sync diff packets: " << skip_big; 48 | msg.data = ss.str(); 49 | g_maxnum_diff_packetnum_pub.publish(msg); 50 | 51 | std_msgs::Int32 skip_num; 52 | if (skip_big > 1 && skip_small > 0) 53 | { 54 | // oldest skip 2 packets 55 | skip_num.data = 2; 56 | if (lidar_vector[0].first == 1) 57 | { 58 | g_skippackets_num_pub1.publish(skip_num); 59 | } 60 | else if (lidar_vector[0].first == 2) 61 | { 62 | g_skippackets_num_pub2.publish(skip_num); 63 | } 64 | else if (lidar_vector[0].first == 3) 65 | { 66 | g_skippackets_num_pub3.publish(skip_num); 67 | } 68 | 69 | // middle skip 1 packets 70 | skip_num.data = 1; 71 | if (lidar_vector[1].first == 1) 72 | { 73 | g_skippackets_num_pub1.publish(skip_num); 74 | } 75 | else if (lidar_vector[1].first == 2) 76 | { 77 | g_skippackets_num_pub2.publish(skip_num); 78 | } 79 | else if (lidar_vector[1].first == 3) 80 | { 81 | g_skippackets_num_pub3.publish(skip_num); 82 | } 83 | } 84 | else if (skip_big > 0 && skip_small == 0) 85 | { 86 | // oldest skip 2 packets 87 | skip_num.data = 1; 88 | if (lidar_vector[0].first == 1) 89 | { 90 | g_skippackets_num_pub1.publish(skip_num); 91 | } 92 | else if (lidar_vector[0].first == 2) 93 | { 94 | g_skippackets_num_pub2.publish(skip_num); 95 | } 96 | else if (lidar_vector[0].first == 3) 97 | { 98 | g_skippackets_num_pub3.publish(skip_num); 99 | } 100 | 101 | // middle skip 1 packets 102 | skip_num.data = 1; 103 | if (lidar_vector[1].first == 1) 104 | { 105 | g_skippackets_num_pub1.publish(skip_num); 106 | } 107 | else if (lidar_vector[1].first == 2) 108 | { 109 | g_skippackets_num_pub2.publish(skip_num); 110 | } 111 | else if (lidar_vector[1].first == 3) 112 | { 113 | g_skippackets_num_pub3.publish(skip_num); 114 | } 115 | } 116 | else if (skip_small > 0) 117 | { 118 | skip_num.data = 1; 119 | if (lidar_vector[0].first == 1) 120 | { 121 | g_skippackets_num_pub1.publish(skip_num); 122 | } 123 | else if (lidar_vector[0].first == 2) 124 | { 125 | g_skippackets_num_pub2.publish(skip_num); 126 | } 127 | else if (lidar_vector[0].first == 3) 128 | { 129 | g_skippackets_num_pub3.publish(skip_num); 130 | } 131 | else 132 | { 133 | ROS_ERROR("[sync-3] Oldest lidar skip one packet!"); 134 | // std::cerr << "Oldest lidar skip one packet!" << std::endl; 135 | } 136 | } 137 | if (skip_big == 0) 138 | { 139 | // std::cout << "3 lidar synchronizer!" << std::endl; 140 | } 141 | // std::cout << std::endl; 142 | } 143 | 144 | int main(int argc, char** argv) 145 | { 146 | ros::init(argc, argv, "rs_sync"); 147 | ros::NodeHandle nh; 148 | ros::NodeHandle nh_private("~"); 149 | 150 | // get the topic parameters 151 | std::string scan1_topic("/left/sync_header"); 152 | std::string scan2_topic("/right/sync_header"); 153 | std::string scan3_topic("/middle/sync_header"); 154 | if (!nh_private.getParam(std::string("scan1_topic"), scan1_topic)) 155 | { 156 | ROS_ERROR_STREAM("[sync-3] Can't get scan1_topic, use the default scan1_topic: "< scan_sub1(nh, scan1_topic, 1); 227 | message_filters::Subscriber scan_sub2(nh, scan2_topic, 1); 228 | message_filters::Subscriber scan_sub3(nh, scan3_topic, 1); 229 | typedef message_filters::sync_policies::ApproximateTime 231 | ScanSyncPolicy; 232 | message_filters::Synchronizer scan_sync(ScanSyncPolicy(10), scan_sub1, scan_sub2, scan_sub3); 233 | scan_sync.registerCallback(boost::bind(&scanCallback, _1, _2, _3)); 234 | 235 | g_skippackets_num_pub1 = nh.advertise(skippackets1_topic.c_str(), 1, true); 236 | g_skippackets_num_pub2 = nh.advertise(skippackets2_topic.c_str(), 1, true); 237 | g_skippackets_num_pub3 = nh.advertise(skippackets3_topic.c_str(), 1, true); 238 | 239 | std::string sync_packet_diff_topic("/sync_packet_diff"); 240 | if (!nh_private.getParam(std::string("sync_packet_diff_topic"), sync_packet_diff_topic)) 241 | { 242 | ROS_ERROR_STREAM("[sync-3] Can't get sync_packet_diff_topic, use the default sync_packet_diff_topic: " 243 | <(sync_packet_diff_topic, 1, true); 250 | 251 | ros::spin(); 252 | return 0; 253 | } 254 | -------------------------------------------------------------------------------- /test_doc/test_guide_cn.md: -------------------------------------------------------------------------------- 1 | # RSLidar ROS 驱动测试说明 2 | 3 | **修订日期：2019-03-18** 4 | 5 | 6 | ## 1. 版本号 7 | **v1.1.0** 8 | 9 | 10 | ## 2. 版本更新内容 11 | | 序号 | 简要 | 详细 | 备注 | 12 | | ---- | ---------------------------------------------------- | ----------------------- | ---- | 13 | | 1 | 新增：Launch文件可设置点云极限距离| 新增launch文件的配置参数：max_distance和min_distance来设置点云显示的最大距离和最小距离，如果没有设置此参数，则使用默认值：200m(最大), 0.2m(最小) | 无 | 14 | | 2 | Bug修复：点云坐标计算错误问题| 修复在1cm距离精度的固件中，点云距离和点云坐标错误的问题 | 无 | 15 | | 3 | 新增：兼容0.5cm距离精度固件 | 兼容0.5cm距离精度的固件| 无 | 16 | | 4 | 新增：增加对新反射率模式：Mode3的支持|由于新增反射率模式：Mode3，驱动增加对此支持解析| 无 | 17 | | 5 | 新增：增加双回波功能支持|驱动自动识别雷达的回波模式，并进行数据包相应解析处理|无| 18 | | 6 | 新增：支持resolution和intensity launch文件设置|在launch文件里设置指定，驱动根据参数进行设置|无| 19 | 20 | ## 3. 版本遗留问题 21 | | 序号 | 简要 | 详细 | 规避 | 效果 | 22 | | ---- | -------------- | ------------------------------ | --------------------------------- | -------------------------------- | 23 | 24 | 25 | ## 4. 版本自测概要 26 | 27 | |序号 | 操作 | 结果 | 备注 | 28 | |----|--------|----|------------------------------| 29 | | 1 | 用不同线数、不同距离精度的Pcap测试实际识别的距离精度，将点云距离与实际距离做比对 | Y | 环境为ubuntu_16.04+ROS_Kinetic | 30 | | 2 | 用不同线数、不同距离精度的Pcap测试实际识别的反射率模式，并与DIFOP包中第291Byte比对|Y | 环境为ubuntu_16.04+ROS_Kinetic | 31 | | 3 | 用不同线数、不同距离精度的Pcap测试极限距离，通过设置max_distance和min_distance的值，观察点云显示是否正常|Y | 环境为ubuntu_16.04+ROS_Kinetic | 32 | | 4 | 拿支持双回波的雷达，查看双回波模式下点云是否正常 | Y |目前只有16线雷达有双回波的正式版本| 33 | | 5 | launch文件设置不同resolution和intensity，看驱动是否生效 | Y |无| 34 | 35 | ## 5. 版本测试建议 36 | | 序号 | 测试点 | 建议 | 依据 | 备注 | 37 | | ---- | -------------- | ---------------------------------------------- | ------------------------------------------------------------ | ---- | 38 | | 1 | 使用不同线数，不同距离精度的Pcap或者雷达对距离精度、反射率模式、极限距离进行测试 | | 无 |无| 39 | | 2 | 双回波功能测试|双回波功能比较难测，可咨询固件同事协助 | 无 | 无| 40 | | 3 | 多个雷达同步功能的代码有调整，请测试功能是否正常 || 无 | 无| 41 | 42 | --------------------------------------------------------------------------------