├── .gitignore ├── .gitmodules ├── CMakeLists.txt ├── LICENSE ├── config ├── calibration.yml ├── calibration.yml.bak └── correction.csv ├── launch └── p40.launch ├── package.xml ├── readme.md └── src └── main.cc /.gitignore: -------------------------------------------------------------------------------- 1 | build -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "src/HesaiLidarSDK"] 2 | path = src/HesaiLidarSDK 3 | url = https://github.com/HesaiTechnology/HesaiLidarSDK.git 4 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(hesai_lidar) 3 | 4 | # Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 6 | 7 | 8 | 9 | find_package(catkin REQUIRED COMPONENTS 10 | cv_bridge 11 | image_transport 12 | sensor_msgs 13 | ) 14 | find_package( Boost REQUIRED ) 15 | find_package(PCL REQUIRED ) 16 | 17 | 18 | 19 | catkin_package() 20 | 21 | add_subdirectory(src/HesaiLidarSDK) 22 | add_executable(hesai_lidar_node 23 | src/main.cc 24 | ) 25 | 26 | include_directories( 27 | src/HesaiLidarSDK/include 28 | ${PCL_INCLUDE_DIRS} 29 | ${catkin_INCLUDE_DIRS} 30 | ${Boost_INCLUDE_DIRS} 31 | ) 32 | 33 | 34 | 35 | target_link_libraries(hesai_lidar_node 36 | ${catkin_LIBRARIES} 37 | hesaiLidarSDK 38 | ) 39 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2018, HesaiTechnology 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /config/calibration.yml: -------------------------------------------------------------------------------- 1 | 3: 2 | K: [587.9607639513098, 0, 641.1987547312688, 0, 589.7411916647718, 350.4665310559093, 0, 0, 1] 3 | D: [-0.3417634136072756, 0.163717393424974, 3.550306333579257e-05, -2.269502070408432e-05, -0.0467970999032102] 4 | 2: 5 | K: [588.2508959163868, 0, 647.292862980063, 0, 590.1733006978851, 357.5000621554299, 0, 0, 1] 6 | D: [-0.33792847429469, 0.15497147097907, 6.687273279031831e-05, -6.474133626990794e-06, -0.04056624381332481] 7 | 1: 8 | K: [586.3259663840066, 0, 636.2924066352238, 0, 588.4623137129338, 366.0670100830787, 0, 0, 1] 9 | D: [-0.3372234945379695, 0.1539569085130269, 8.516331936573671e-06, -2.37919172198999e-05, -0.04026038638740984] 10 | 4: 11 | K: [586.9208412885432, 0, 639.4036213157253, 0, 588.9533517337975, 372.2792209901052, 0, 0, 1] 12 | D: [-0.3385481906291727, 0.1560329273732019, -0.0002525352067705174, 0.0001712995534017639, -0.04111459154475255] 13 | 0: 14 | K: [1411.077090949719, 0, 615.9434135330476, 0, 1414.323243925144, 334.4885997587672, 0, 0, 1] 15 | D: [-0.4576995011452611, 0.2804187482132228, -0.0008824335077850593, -0.002560567254523004, -0.3683570521877889] 16 | 17 | -------------------------------------------------------------------------------- /config/calibration.yml.bak: -------------------------------------------------------------------------------- 1 | 0: 2 | K: [1411.077090949719, 0, 615.9434135330476, 0, 1414.323243925144, 334.4885997587672, 0, 0, 1] 3 | D: [-0.4576995011452611, 0.2804187482132228, -0.0008824335077850593, -0.002560567254523004, -0.3683570521877889] 4 | 1: 5 | K: [342.5400328255074, 0, 640.460699031343, 0, 339.4331965071486, 356.3038964572869, 0, 0, 1] 6 | D: [-0.1753591484498331, 0.02762625329264434, 1.990356823116969e-05, 0.0005212335592411709, -0.001774133391009368] 7 | 2: 8 | K: [330.0496139729793, 0, 637.0246798503467, 0, 326.7090363074088, 350.7881295175944, 0, 0, 1] 9 | D: [-0.1846577791624761, 0.03366866480811935, 0.0003070812977140898, 0.0003952438640611272, -0.002608423745777319] 10 | 3: 11 | K: [373.8054674494855, 0, 637.7349082069506, 0, 369.6542050863838, 349.0390592833846, 0, 0, 1] 12 | D: [-0.2458410838367304, 0.06611616265915085, 0.0003872523163950329, 0.0002377539327965119, -0.007809619928009832] 13 | 4: 14 | K: [363.9737360031555, 0, 652.4764949793846, 0, 359.6935815957517, 345.7881319794952, 0, 0, 1] 15 | D: [-0.2190493660809663, 0.04707086193025265, 0.0004825870412562302, -0.0004784635496621562, -0.004180118405907018] 16 | -------------------------------------------------------------------------------- /config/correction.csv: -------------------------------------------------------------------------------- 1 | Laser id,Elevation,Azimuth 2 | 1,7.143,-0.025 3 | 2,6.142,-0.025 4 | 3,5.136,-0.026 5 | 4,4.128,-0.026 6 | 5,3.115,-2.54 7 | 6,2.102,-2.541 8 | 7,1.767,2.49 9 | 8,1.421,-5.042 10 | 9,1.087,-2.541 11 | 10,0.752,2.49 12 | 11,0.406,-5.042 13 | 12,0.071,-2.541 14 | 13,-0.268,2.49 15 | 14,-0.606,-5.042 16 | 15,-0.945,-0.026 17 | 16,-1.284,2.49 18 | 17,-1.621,-5.042 19 | 18,-1.96,-0.026 20 | 19,-2.298,4.998 21 | 20,-2.636,-2.541 22 | 21,-2.974,-0.026 23 | 22,-3.311,4.998 24 | 23,-3.648,-2.54 25 | 24,-3.986,-0.026 26 | 25,-4.319,4.997 27 | 26,-4.661,-2.54 28 | 27,-4.995,2.49 29 | 28,-5.326,4.997 30 | 29,-5.667,-2.539 31 | 30,-6,2.49 32 | 31,-7.001,2.49 33 | 32,-7.997,2.49 34 | 33,-8.987,-0.024 35 | 34,-9.971,-0.023 36 | 35,-10.948,-0.023 37 | 36,-11.917,-0.022 38 | 37,-12.874,-2.528 39 | 38,-13.825,-2.526 40 | 39,-14.766,-2.523 41 | 40,-15.696,-2.521 42 | -------------------------------------------------------------------------------- /launch/p40.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 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | hesai_lidar 4 | 0.0.0 5 | The hesai_lidar package 6 | 7 | 8 | 9 | 10 | yy 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | cv_bridge 44 | image_transport 45 | roscpp 46 | sensor_msgs 47 | std_msgs 48 | pandar_msgs 49 | pandar_msgs 50 | message_runtime 51 | cv_bridge 52 | image_transport 53 | roscpp 54 | sensor_msgs 55 | std_msgs 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | -------------------------------------------------------------------------------- /readme.md: -------------------------------------------------------------------------------- 1 | # Pandar40_ROS 2 | 3 | ## About the project 4 | Pandar40_ROS project includes the ROS Driver for **Pandar40** LiDAR sensor manufactured by Hesai Technology. 5 | Note: For the driver of Pandar40P and other modules, please go to [HesaiLidar_General_ROS](https://github.com/HesaiTechnology/HesaiLidar_General_ROS) 6 | 7 | Developed based on [Pandar40_SDK](https://github.com/HesaiTechnology/Pandar40_SDK), after launched, the project will monitor UDP packets from Pandar40 Lidar, parse data and publish point cloud frames into ROS under topic: ```/pandar```. It can also be used as an official demo showing how to work with Pandar40_SDK. 8 | 9 | ## Environment and Dependencies 10 | **System environment requirement: Linux + ROS** 11 | 12 | 　Recommanded: 13 | 　Ubuntu 16.04 - with ROS kinetic desktop-full installed or 14 | 　Ubuntu 18.04 - with ROS melodic desktop-full installed 15 | 　Check resources on http://ros.org for installation guide 16 | 17 | **Library Dependencies: libpcap-dev + libyaml-cpp-dev** 18 | ``` 19 | $sudo apt install libpcap-dev libyaml-cpp-dev 20 | ``` 21 | 22 | ## Download and Build 23 | 24 | **Install `catkin_tools`** 25 | ``` 26 | $ sudo apt-get update 27 | $ sudo apt-get install python-catkin-tools 28 | ``` 29 | **Download code** 30 | ``` 31 | $ mkdir -p rosworkspace/src 32 | $ cd rosworkspace/src 33 | $ git clone https://github.com/HesaiTechnology/Pandar40_ROS.git --recursive 34 | ``` 35 | **Build** 36 | ``` 37 | $ cd .. 38 | $ catkin_make 39 | ``` 40 | 41 | ## Configuration 42 | ``` 43 | $ gedit src/launch/p40.launch 44 | ``` 45 | **Reciving data from connected Lidar: config lidar ip&port, leave the pcap_file empty** 46 | |Parameter | Default Value| 47 | |---------|---------------| 48 | |server_ip |192.168.1.201| 49 | |lidar_recv_port |2368| 50 | |gps_recv_port |10110| 51 | |laser_return_type| 0 | 52 | |pcap_file ||　　 53 | 54 | Data source will be from connected Lidar when "pcap_file" set to empty 55 | Make sure the parameters above set to the same with Lidar setting 56 | Note: (laser_return_type: for single return set to 1, for dural return set to 1) 57 | 58 | **Reciving data from pcap file: config pcap_file and correction file path** 59 | |Parameter | Value| 60 | |---------|---------------| 61 | |pcap_file |pcap file path| 62 | |lidar_correction_file |lidar correction file path| 63 | |laser_return_type| return type of lidar in pcap file | 64 | 65 | Data source will be from pcap file once "pcap_file" not empty 66 | 67 | 68 | ## Run 69 | 70 | 1. Make sure current path in the `rosworkspace` directory 71 | ``` 72 | $ source devel/setup.bash 73 | $ roslaunch hesai_lidar p40.launch 74 | ``` 75 | 2. The driver will publish PointCloud messages to the topic `/pandar_points` 76 | 3. Open Rviz and add display by topic 77 | 4. Change fixed frame to `pandar` to view published point clouds 78 | -------------------------------------------------------------------------------- /src/main.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include "hesaiLidarSDK.h" 8 | using namespace std; 9 | 10 | class HesaiLidarClient 11 | { 12 | public: 13 | HesaiLidarClient(ros::NodeHandle node, ros::NodeHandle nh) 14 | { 15 | lidarPublisher = node.advertise("pandar_points", 10); 16 | 17 | string serverIp; 18 | int serverPort; 19 | string calibrationFile; 20 | int lidarRecvPort; 21 | int gpsPort; 22 | double startAngle; 23 | string lidarCorrectionFile; 24 | int laserReturnType; 25 | int laserCount; 26 | int pclDataType; 27 | string pcapFile; 28 | image_transport::ImageTransport it(nh); 29 | 30 | 31 | nh.getParam("pcap_file", pcapFile); 32 | nh.getParam("server_ip", serverIp); 33 | nh.getParam("server_port", serverPort); 34 | nh.getParam("calibration_file", calibrationFile); 35 | nh.getParam("lidar_recv_port", lidarRecvPort); 36 | nh.getParam("gps_port", gpsPort); 37 | nh.getParam("start_angle", startAngle); 38 | nh.getParam("lidar_correction_file", lidarCorrectionFile); 39 | nh.getParam("laser_return_type", laserReturnType); 40 | nh.getParam("laser_count", laserCount); 41 | nh.getParam("pcldata_type", pclDataType); 42 | 43 | // pcapFile = "/home/pandora/Desktop/pandar40p.pcap"; 44 | 45 | if(!pcapFile.empty()) 46 | { 47 | hsdk = new HesaiLidarSDK(pcapFile, lidarCorrectionFile, (HesaiLidarRawDataSturct)laserReturnType, laserCount, (HesaiLidarPCLDataType)pclDataType, 48 | boost::bind(&HesaiLidarClient::lidarCallback, this, _1, _2)); 49 | } 50 | else if(!serverIp.empty()) 51 | { 52 | 53 | hsdk = new HesaiLidarSDK(lidarRecvPort, gpsPort, startAngle, lidarCorrectionFile, 54 | boost::bind(&HesaiLidarClient::lidarCallback, this, _1, _2), 55 | NULL, (HesaiLidarRawDataSturct)laserReturnType, laserCount, (HesaiLidarPCLDataType)pclDataType); 56 | } 57 | 58 | hsdk->start(); 59 | } 60 | 61 | void cameraCallback(boost::shared_ptr matp, double timestamp, int pic_id) 62 | { 63 | 64 | sensor_msgs::ImagePtr imgMsg; 65 | 66 | switch (pic_id) 67 | { 68 | case 0: 69 | imgMsg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", *matp).toImageMsg(); 70 | break; 71 | case 1: 72 | case 2: 73 | case 3: 74 | case 4: 75 | imgMsg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", *matp).toImageMsg(); 76 | break; 77 | default: 78 | ROS_INFO("picid wrong in getImageToPub"); 79 | return; 80 | } 81 | imgMsg->header.stamp = ros::Time(timestamp); 82 | imgPublishers[pic_id].publish(imgMsg); 83 | } 84 | 85 | void lidarCallback(boost::shared_ptr cld, double timestamp) 86 | { 87 | pcl_conversions::toPCL(ros::Time(timestamp), cld->header.stamp); 88 | sensor_msgs::PointCloud2 output; 89 | pcl::toROSMsg(*cld, output); 90 | lidarPublisher.publish(output); 91 | } 92 | 93 | 94 | private: 95 | ros::Publisher lidarPublisher; 96 | image_transport::Publisher imgPublishers[5]; 97 | HesaiLidarSDK* hsdk; 98 | }; 99 | 100 | 101 | int main(int argc, char **argv) 102 | { 103 | ros::init(argc, argv, "pandora_ros"); 104 | ros::NodeHandle nh("~"); 105 | ros::NodeHandle node; 106 | HesaiLidarClient pandoraClient(node, nh); 107 | 108 | ros::spin(); 109 | return 0; 110 | } 111 | --------------------------------------------------------------------------------