├── .gitignore ├── .gitmodules ├── CMakeLists.txt ├── LICENSE ├── README.md ├── include ├── calibration.h ├── hesaiLidarSDK.h ├── hesaiLidarSDK_IN.h ├── input.h ├── pandora_types.h ├── point_types.h ├── rawdata.h └── utilities.h ├── sample ├── pandar40 │ ├── CMakeLists.txt │ └── pandar40.cc ├── pandar40p │ ├── CMakeLists.txt │ └── pandar40p.cc └── pandora │ ├── CMakeLists.txt │ └── pandora.cc └── src ├── calibration.cc ├── hesaiLidarSDK.cc ├── hesaiLidarSDK_IN.cc ├── input.cc ├── rawdata.cc ├── test.cc └── utilities.cc /.gitignore: -------------------------------------------------------------------------------- 1 | build 2 | test/build -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "src/pandora_camera_client"] 2 | path = src/pandora_camera_client 3 | url = https://github.com/HesaiTechnology/PandoraCameraClient.git 4 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(hesaiLidarSDK) 3 | 4 | 5 | find_package(Boost REQUIRED ) 6 | find_package(PCL REQUIRED ) 7 | 8 | set (LINK_LIBRARY_PLUS "") 9 | 10 | if(ENABLE_SAMPLE) 11 | option(Camera_Enable "Enable Camera Driver, For Pandora" ON) 12 | else(NOT ENABLE_SAMPLE) 13 | option(Camera_Enable "Enable Camera Driver, For Pandora" OFF) 14 | endif() 15 | 16 | option(Debug_Enable "Enable Camera Driver, For Pandora" OFF) 17 | 18 | set (CMAKE_CXX_FLAGS "-fPIC -std=c++11") 19 | set (CMAKE_CXX_STANDARD 11) 20 | 21 | set (SRC 22 | src/calibration.cc 23 | src/input.cc 24 | src/rawdata.cc 25 | src/hesaiLidarSDK_IN.cc 26 | src/hesaiLidarSDK.cc 27 | ) 28 | 29 | #add_custom_target(build-time-make-directory ALL 30 | # COMMAND ${CMAKE_COMMAND} -E make_directory ${CMAKE_BINARY_DIR}/lib) 31 | #message (STATUS "Target library dir == ${CMAKE_BINARY_DIR}/lib") 32 | #message (STATUS "build-time-make-directory == ${build-time-make-directory}") 33 | 34 | add_subdirectory(src/pandora_camera_client) 35 | if(Camera_Enable) 36 | message("Build Camera") 37 | find_package( OpenCV REQUIRED ) 38 | add_definitions("-DHESAI_WITH_CAMERA") 39 | set (LINK_LIBRARY_PLUS "${OpenCV_LIBS}") 40 | set (JPEG_LIB "jpeg") 41 | set (SRC 42 | ${SRC} 43 | src/utilities.cc 44 | ) 45 | endif(Camera_Enable) 46 | 47 | if(Debug_Enable) 48 | message("Build Debug") 49 | add_definitions("-DDebug") 50 | set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g") 51 | endif(Debug_Enable) 52 | 53 | 54 | include_directories( 55 | include 56 | src/pandora_camera_client/include 57 | ${Boost_INCLUDE_DIRS} 58 | ${PCL_INCLUDE_DIRS} 59 | ) 60 | 61 | set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib) 62 | set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib) 63 | set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin) 64 | 65 | add_library( ${PROJECT_NAME} SHARED 66 | ${SRC} 67 | ) 68 | 69 | target_link_libraries(${PROJECT_NAME} 70 | pandora_client 71 | pcap 72 | yaml-cpp 73 | ${JPEG_LIB} 74 | ${Boost_LIBRARIES} 75 | ${PCL_COMMON_LIBRARIES} 76 | ${PCL_IO_LIBRARIES} 77 | ${LINK_LIBRARY_PLUS} 78 | ) 79 | 80 | if(${CMAKE_SOURCE_DIR} STREQUAL ${CMAKE_CURRENT_SOURCE_DIR}) 81 | 82 | 83 | add_executable(pandora_sdk_test 84 | src/test.cc 85 | ) 86 | target_link_libraries(pandora_sdk_test 87 | ${PROJECT_NAME} 88 | ${Boost_LIBRARIES} 89 | ${PCL_COMMON_LIBRARIES} ${PCL_IO_LIBRARIES} 90 | ${LINK_LIBRARY_PLUS} 91 | ) 92 | 93 | endif(${CMAKE_SOURCE_DIR} STREQUAL ${CMAKE_CURRENT_SOURCE_DIR}) 94 | 95 | if (ENABLE_SAMPLE) 96 | add_subdirectory(sample/pandar40) 97 | 98 | add_subdirectory(sample/pandar40p) 99 | 100 | add_subdirectory(sample/pandora) 101 | 102 | endif() 103 | 104 | #set(CMAKE_INSTALL_PREFIX ${CMAKE_BINARY_DIR}/SDK) 105 | 106 | install(PROGRAMS include/hesaiLidarSDK.h DESTINATION include) 107 | install(PROGRAMS include/point_types.h DESTINATION include) 108 | install(DIRECTORY sample DESTINATION .) 109 | install(PROGRAMS ${CMAKE_BINARY_DIR}/lib/lib${PROJECT_NAME}.so DESTINATION lib) 110 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 2-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 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # HesaiLidar_General_SDK 2 | 3 | ## About the project 4 | HesaiLidar_General_SDK project is the software development kit for **Pandar40** LiDAR manufactured by Hesai Technology. 5 | ## Environment and Dependencies 6 | **System environment requirement: Linux + G++ 7.0 or above** 7 | **Library Dependencies: libpcap-dev + libyaml-cpp-dev** 8 | ``` 9 | $ sudo apt install libpcap-dev libyaml-cpp-dev 10 | ``` 11 | ## Clone 12 | ``` 13 | $ git clone https://github.com/HesaiTechnology/Pandar40_SDK.git --recursive 14 | ``` 15 | ## Build 16 | ``` 17 | $ cd Pandar40_SDK 18 | $ mkdir build 19 | $ cd build 20 | $ cmake .. 21 | $ make 22 | ``` 23 | # Add to your project 24 | ``` 25 | add_subdirectory(Pandar40_SDK) 26 | 27 | target_link_libraries(${YOUR_PROJECT_NAME} 28 | ... 29 | Pandar40_SDK 30 | ... 31 | ) 32 | 33 | ``` 34 | -------------------------------------------------------------------------------- /include/calibration.h: -------------------------------------------------------------------------------- 1 | #ifndef __PANDAR_CALIBRATION_H 2 | #define __PANDAR_CALIBRATION_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #ifndef M_PI 9 | #define M_PI 3.14159265358979323846 10 | #endif 11 | 12 | namespace angles 13 | { 14 | double degreeToRadian(double degrees); 15 | } 16 | 17 | namespace pandar_pointcloud { 18 | 19 | struct PandarLaserCorrection 20 | { 21 | double azimuthCorrection; 22 | double verticalCorrection; 23 | double distanceCorrection; 24 | double verticalOffsetCorrection; 25 | double horizontalOffsetCorrection; 26 | double sinVertCorrection; 27 | double cosVertCorrection; 28 | double sinVertOffsetCorrection; 29 | double cosVertOffsetCorrection; 30 | }; 31 | 32 | class Calibration { 33 | 34 | public: 35 | Calibration(): initialized(false) 36 | {} 37 | Calibration(const std::string& calibrationFile) 38 | { 39 | read(calibrationFile); 40 | } 41 | 42 | void read(const std::string& calibrationFile); 43 | static const int laserNumber = 40; 44 | PandarLaserCorrection laserCorrections[laserNumber]; 45 | bool initialized; 46 | 47 | private: 48 | void setDefaultCorrections (); 49 | }; 50 | 51 | } /* pandar_pointcloud */ 52 | 53 | #endif /* end of include guard: __PANDAR_CALIBRATION_H */ 54 | -------------------------------------------------------------------------------- /include/hesaiLidarSDK.h: -------------------------------------------------------------------------------- 1 | #ifndef __PANDORA_SDK_H 2 | #define __PANDORA_SDK_H 3 | 4 | #include 5 | #include 6 | #ifdef HESAI_WITH_CAMERA 7 | #include 8 | #include 9 | #endif 10 | #include 11 | #include 12 | #include "point_types.h" 13 | 14 | typedef enum{ 15 | HESAI_LIDAR_PCL_DATA_TYPE_REDUCED = 0, // Don't include the useless points such as xyz(0,0,0) or xyz(na/na/na) 16 | HESAI_LIDAR_PCL_DATA_TYPE_ALIGNMENT = 1, // Align the points , such as 1800x40 (1800 indicates the frequency of each laser one circle) 17 | }HesaiLidarPCLDataType; 18 | 19 | typedef enum{ 20 | HESAI_LIDAR_RAW_DATA_STRCUT_SINGLE_RETURN = 0, // Pandar40 21 | HESAI_LIDAR_RAW_DATA_STRCUT_DUAL_RETURN = 1, // Pandar40-AC , Pandar40P 22 | }HesaiLidarRawDataSturct; 23 | 24 | class HesaiLidarSDK_internal; 25 | 26 | class HesaiLidarSDK 27 | { 28 | public: 29 | #ifdef HESAI_WITH_CAMERA 30 | HesaiLidarSDK( 31 | const std::string pandoraIP, // pandora的ip 32 | const unsigned short pandoraCameraPort, // pandora的port 33 | const std::string intrinsicFile, // 摄像头的内参文件路径，为空时，输出的图像是未经过矫正的 34 | boost::function matp, double timestamp, int picid)> cameraCallback, // 摄像头的callback函数 35 | 36 | const unsigned short lidarRecvPort, // lidar的数据接收端口， 默认为8080 37 | const unsigned short gpsRecvPort, // gps数据接收端口， 默认为10110 38 | const double startAngle, // lidar的旋转起始角度，默认为0,单位是度，例如135度 39 | const std::string lidarCorrectionFile, // lidar的标定文件路径，为空时，将使用默认参数 40 | boost::function pcloudp, double timestamp)> lidarCallback, // lidar的callback函数 41 | boost::function gpsCallback, // gps数据的callback函数，可为NULL.timestamp为当前gps的时间戳 42 | const HesaiLidarRawDataSturct dataStruct, // 回波类型,0为单回波,1为双回波. 43 | const unsigned int laserCount, // lidar的线数量 44 | const HesaiLidarPCLDataType pclDataType); // 返回给lidarcallback的pcl数据的格式类型.0是以block遍历生成的数据,1是以laserId遍历生成的数据 45 | #endif 46 | 47 | HesaiLidarSDK( 48 | #ifdef HESAI_WITH_CAMERA 49 | const std::string pandoraIP, //此时,lidar的数据接收端口默认为8080, gps数据接收端口默认为10110, lidar的旋转起始角度默认为0, 输出的图像是未经过矫正的, lidar的标定参数使用默认值 50 | const unsigned short pandoraCameraPort, 51 | boost::function matp, double timestamp, int picid)> cameraCallback, 52 | #endif 53 | boost::function pcloudp, double timestamp)> lidarCallback); 54 | 55 | HesaiLidarSDK( 56 | const unsigned short lidarRecvPort, 57 | const unsigned short gpsRecvPort, 58 | const double startAngle, 59 | const std::string lidarCorrectionFile, 60 | boost::function cld, double timestamp)> lidarCallback, 61 | boost::function gpsCallback, 62 | const HesaiLidarRawDataSturct laserReturnType, 63 | const unsigned int laserCount, 64 | const HesaiLidarPCLDataType pclDataType); 65 | 66 | 67 | HesaiLidarSDK( 68 | const std::string pcapPath, 69 | const std::string lidarCorrectionFile, 70 | const HesaiLidarRawDataSturct laserReturnType, 71 | const unsigned int laserCount, 72 | const HesaiLidarPCLDataType pclDataType, 73 | boost::function pcloudp, double timestamp)> lidarCallback); 74 | 75 | HesaiLidarSDK(){}; 76 | 77 | ~HesaiLidarSDK(); 78 | int start(); //开始数据接收和传输任务,成功时返回0,否则返回-1 79 | void stop(); //停止数据接收和传输任务 80 | private: 81 | HesaiLidarSDK_internal *psi; 82 | }; 83 | 84 | #endif 85 | -------------------------------------------------------------------------------- /include/hesaiLidarSDK_IN.h: -------------------------------------------------------------------------------- 1 | #ifndef __PANDORA_SDK_IN_H 2 | #define __PANDORA_SDK_IN_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #ifdef HESAI_WITH_CAMERA 10 | #include 11 | #include 12 | using namespace cv; 13 | 14 | 15 | #define HesaiLidarSDK_CAMERA_NUM 5 16 | #define HesaiLidarSDK_IMAGE_WIDTH 1280 17 | #define HesaiLidarSDK_IMAGE_HEIGHT 720 18 | 19 | #include "pandora_client.h" 20 | #endif 21 | 22 | #include "input.h" 23 | #include "rawdata.h" 24 | 25 | #define HesaiLidarSDK_DEFAULT_START_ANGLE 0.0 26 | typedef struct HS_LIDAR_L40_GPS_PACKET_s 27 | { 28 | unsigned short flag; 29 | unsigned short year; 30 | unsigned short month; 31 | unsigned short day; 32 | unsigned short second; 33 | unsigned short minute; 34 | unsigned short hour; 35 | unsigned int fineTime; 36 | } HS_LIDAR_L40_GPS_Packet; 37 | 38 | #define HS_LIDAR_L40_GPS_PACKET_SIZE (512) 39 | #define HS_LIDAR_L40_GPS_PACKET_FLAG_SIZE (2) 40 | #define HS_LIDAR_L40_GPS_PACKET_YEAR_SIZE (2) 41 | #define HS_LIDAR_L40_GPS_PACKET_MONTH_SIZE (2) 42 | #define HS_LIDAR_L40_GPS_PACKET_DAY_SIZE (2) 43 | #define HS_LIDAR_L40_GPS_PACKET_HOUR_SIZE (2) 44 | #define HS_LIDAR_L40_GPS_PACKET_MINUTE_SIZE (2) 45 | #define HS_LIDAR_L40_GPS_PACKET_SECOND_SIZE (2) 46 | #define HS_LIDAR_L40_GPS_ITEM_NUM (7) 47 | 48 | #define HesaiLidarSDK_DEFAULT_LIDAR_RECV_PORT 8080 49 | #define HesaiLidarSDK_DEFAULT_GPS_RECV_PORT 10110 50 | 51 | 52 | 53 | 54 | // 10000 us = 100 ms = 0.1s per packet 55 | #define HesaiLidarSDK_PCAP_TIME_INTERVAL (100000 / 3000) 56 | 57 | enum SDKUseMode 58 | { 59 | PandoraUseModeOnlyLidar, // 仅使用lidar 60 | PandoraUseModeReadPcap, // 仅读取lidar的pcap 61 | PandoraUseModeLidarAndCamera //使用lidar和camera 62 | }; 63 | 64 | class HesaiLidarSDK_internal 65 | { 66 | public: 67 | HesaiLidarSDK_internal( 68 | #ifdef HESAI_WITH_CAMERA 69 | const std::string pandoraIP, 70 | const unsigned short pandoraCameraPort, 71 | const std::string intrinsicFile, 72 | boost::function matp, double timestamp, int pic_id)> cameraCallback, 73 | #endif 74 | const unsigned short lidarRecvPort, 75 | const unsigned short gpsRecvPort, 76 | const double startAngle, 77 | const std::string lidarCorrectionFile, 78 | boost::function cld, double timestamp)> lidarCallback, 79 | boost::function gpsCallback, 80 | const unsigned int laserReturnType, 81 | const unsigned int laserCount, 82 | const unsigned int pclDataType); 83 | 84 | HesaiLidarSDK_internal( 85 | #ifdef HESAI_WITH_CAMERA 86 | const std::string pandoraIP, 87 | const unsigned short pandoraCameraPort, 88 | boost::function matp, double timestamp, int pic_id)> cameraCallback, 89 | #endif 90 | boost::function cld, double timestamp)> lidarCallback); 91 | 92 | 93 | HesaiLidarSDK_internal( 94 | const std::string pcapPath, 95 | const std::string lidarCorrectionFile, 96 | const unsigned int laserReturnType, 97 | const unsigned int laserCount, 98 | const unsigned int pclDataType, 99 | boost::function pcloudp, double timestamp)> lidarCallback); 100 | 101 | ~HesaiLidarSDK_internal(); 102 | int start(); 103 | void stop(); 104 | #ifdef HESAI_WITH_CAMERA 105 | void pushPicture(PandoraPic *pic); 106 | #endif 107 | 108 | private: 109 | void init( 110 | #ifdef HESAI_WITH_CAMERA 111 | const std::string pandoraIP, 112 | const unsigned short pandoraCameraPort, 113 | const std::string intrinsicFile, 114 | boost::function matp, double timestamp, int pic_id)> cameraCallback, 115 | #endif 116 | const unsigned short lidarRecvPort, 117 | const unsigned short gpsRecvPort, 118 | const double startAngle, 119 | const std::string lidarCorrectionFile, 120 | boost::function cld, double timestamp)> lidarCallback, 121 | boost::function gpsCallback, 122 | const unsigned int laserReturnType, 123 | const unsigned int laserCount, 124 | const unsigned int pclDataType); 125 | void init( 126 | const std::string pcapPath, 127 | const std::string lidarCorrectionFile, 128 | const unsigned int laserReturnType, 129 | const unsigned int laserCount, 130 | const unsigned int pclDataType, 131 | boost::function pcloudp, double timestamp)> lidarCallback); 132 | #ifdef HESAI_WITH_CAMERA 133 | void setupCameraClient(); 134 | void processPic(); 135 | bool loadIntrinsics(const std::string &intrinsicFile); 136 | 137 | 138 | pthread_mutex_t picLock; 139 | sem_t picSem; 140 | boost::thread *processPicThread; 141 | bool continueProcessPic; 142 | bool needRemapPicMat; 143 | std::string ip; 144 | unsigned short cport; 145 | void *pandoraCameraClient; 146 | std::list pictureList; 147 | std::vector mapxList; 148 | std::vector mapyList; 149 | boost::function matp, double timestamp, int pic_id)> userCameraCallback; 150 | unsigned int cameraLastGPSSecond; 151 | #endif 152 | void setupLidarClient(); 153 | void setupReadPcap(); 154 | void lidarRecvTask(); 155 | void readPcapFile(); 156 | void processGps(HS_LIDAR_L40_GPS_Packet &gpsMsg); 157 | void processLiarPacket(); 158 | void pushLiDARData(PandarPacket packet); 159 | void internalFuncForGPS(const unsigned int &gpsstamp); 160 | 161 | 162 | pthread_mutex_t lidarLock, lidarGpsLock; 163 | sem_t lidarSem; 164 | boost::thread *lidarRecvThread; 165 | boost::thread *lidarProcessThread; 166 | boost::thread *readPacpThread; 167 | bool continueLidarRecvThread; 168 | bool continueProcessLidarPacket; 169 | bool continueReadPcap; 170 | unsigned short lport; 171 | int lidarRotationStartAngle; 172 | HS_LIDAR_L40_GPS_Packet hesaiGps; 173 | 174 | std::list lidarPacketList; 175 | 176 | boost::shared_ptr input; 177 | boost::function cld, double timestamp)> userLidarCallback; 178 | boost::function userGpsCallback; 179 | boost::shared_ptr data_; 180 | time_t gps1; 181 | GPS_STRUCT_T gps2; 182 | unsigned int lidarLastGPSSecond; 183 | unsigned int lidarLastGPSHourSecond; 184 | 185 | SDKUseMode useMode; 186 | }; 187 | 188 | #endif -------------------------------------------------------------------------------- /include/input.h: -------------------------------------------------------------------------------- 1 | #ifndef __PANDAR_INPUT_H 2 | #define __PANDAR_INPUT_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include "pandora_types.h" 9 | #include "pcap.h" 10 | 11 | namespace pandar_pointcloud 12 | { 13 | 14 | class Input 15 | { 16 | public: 17 | Input(uint16_t port, uint16_t gpsPort); 18 | ~Input(); 19 | Input(std::string filePath, int type); 20 | int getPacket(PandarPacket *pkt); 21 | int getPacketFromPcap(PandarPacket *pkt); 22 | private: 23 | int socketForLidar; 24 | int socketForGPS; 25 | int socketNumber; 26 | pcap_t *pcap_; 27 | std::string pcapFilePath; 28 | char pcapErrorBuffer[PCAP_ERRBUF_SIZE]; 29 | int realLidarPacketSize; 30 | struct timeval getPacketStartTime, getPacketStopTime; 31 | }; 32 | 33 | } // pandar_driver namespace 34 | 35 | #endif // __PANDAR_INPUT_H 36 | -------------------------------------------------------------------------------- /include/pandora_types.h: -------------------------------------------------------------------------------- 1 | #ifndef __PANDORA_TYPES_H 2 | #define __PANDORA_TYPES_H 3 | 4 | #include 5 | #define ETHERNET_MTU (1500) 6 | 7 | typedef struct PandarPacket_s 8 | { 9 | double stamp; 10 | uint8_t data[ETHERNET_MTU]; 11 | uint32_t size; 12 | }PandarPacket; 13 | 14 | 15 | 16 | typedef struct GPS_STRUCT_{ 17 | int used; 18 | time_t gps; 19 | int usedHour; 20 | struct tm t; 21 | }GPS_STRUCT_T; 22 | 23 | enum DeviceType 24 | { 25 | deviceTypeSingleReturn, 26 | deviceTypeDualReturn 27 | }; 28 | 29 | #define PANDORA_GPS_PACKET_SIZE 512 30 | 31 | #endif -------------------------------------------------------------------------------- /include/point_types.h: -------------------------------------------------------------------------------- 1 | #ifndef __PANDAR_POINTCLOUD_POINT_TYPES_H 2 | #define __PANDAR_POINTCLOUD_POINT_TYPES_H 3 | 4 | #include 5 | 6 | namespace pandar_pointcloud 7 | { 8 | struct PointXYZIT { 9 | PCL_ADD_POINT4D 10 | float intensity; 11 | double timestamp; 12 | uint16_t ring; ///< laser ring number 13 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned 14 | } EIGEN_ALIGN16; 15 | 16 | }; // namespace pandar_pointcloud 17 | 18 | POINT_CLOUD_REGISTER_POINT_STRUCT(pandar_pointcloud::PointXYZIT, 19 | (float, x, x)(float, y, y)(float, z, z) 20 | (float, intensity, intensity)(double, timestamp, timestamp)(uint16_t, ring, ring)) 21 | 22 | typedef pandar_pointcloud::PointXYZIT PPoint; 23 | typedef pcl::PointCloud PPointCloud; 24 | 25 | #endif 26 | -------------------------------------------------------------------------------- /include/rawdata.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef __PANDAR_RAWDATA_H 3 | #define __PANDAR_RAWDATA_H 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include "point_types.h" 12 | #include "calibration.h" 13 | #include "pandora_types.h" 14 | 15 | namespace pandar_rawdata 16 | { 17 | 18 | #define SOB_ANGLE_SIZE 4 19 | #define RAW_MEASURE_SIZE 5 20 | #define LASER_COUNT 40 21 | #define BLOCKS_PER_PACKET 6 22 | #define BLOCK_SIZE (RAW_MEASURE_SIZE * LASER_COUNT + SOB_ANGLE_SIZE) 23 | #define TIMESTAMP_SIZE 4 24 | #define FACTORY_ID_SIZE 2 25 | #define RESERVE_SIZE 8 26 | #define REVOLUTION_SIZE 2 27 | #define INFO_SIZE (TIMESTAMP_SIZE + FACTORY_ID_SIZE + RESERVE_SIZE + REVOLUTION_SIZE) 28 | #define PACKET_SIZE (BLOCK_SIZE * BLOCKS_PER_PACKET + INFO_SIZE) 29 | #define ROTATION_MAX_UNITS 36001 30 | #define ROTATION_RESOLUTION 0.01 31 | 32 | #define PandoraSDK_MIN_RANGE 0.5 33 | #define PandoraSDK_MAX_RANGE 250.0 34 | 35 | #define LASER_RETURN_TO_DISTANCE_RATE 0.002 36 | 37 | #define MAX_LASER_COUNT (128) 38 | #define MAX_BLOCK_SIZE (20) 39 | // DUAL-VERSION 40 | #define DUAL_VERSION_SOB_ANGLE_SIZE 4 41 | #define DUAL_VERSION_RAW_MEASURE_SIZE 3 42 | #define DUAL_VERSION_LASER_COUNT 40 43 | #define DUAL_VERSION_BLOCKS_PER_PACKET 10 44 | #define DUAL_VERSION_BLOCK_SIZE (DUAL_VERSION_RAW_MEASURE_SIZE * DUAL_VERSION_LASER_COUNT + DUAL_VERSION_SOB_ANGLE_SIZE) 45 | #define DUAL_VERSION_TIMESTAMP_SIZE 4 46 | #define DUAL_VERSION_FACTORY_INFO_SIZE 1 47 | #define DUAL_VERSION_ECHO_SIZE 1 48 | #define DUAL_VERSION_RESERVE_SIZE 8 49 | #define DUAL_VERSION_REVOLUTION_SIZE 2 50 | #define DUAL_VERSION_INFO_SIZE (DUAL_VERSION_TIMESTAMP_SIZE + DUAL_VERSION_FACTORY_INFO_SIZE + DUAL_VERSION_ECHO_SIZE + DUAL_VERSION_RESERVE_SIZE + DUAL_VERSION_REVOLUTION_SIZE) 51 | #define DUAL_VERSION_PACKET_SIZE (DUAL_VERSION_BLOCK_SIZE * DUAL_VERSION_BLOCKS_PER_PACKET + DUAL_VERSION_INFO_SIZE) 52 | #define DUAL_VERSION_LASER_RETURN_TO_DISTANCE_RATE 0.004 53 | 54 | // typedef struct RAW_MEASURE_{ 55 | // uint32_t range; 56 | // uint16_t reflectivity; 57 | // } RAW_MEASURE_T; 58 | 59 | // typedef struct RAW_BLOCK 60 | // { 61 | // uint16_t sob; 62 | // uint16_t azimuth; 63 | // RAW_MEASURE_T measures[LASER_COUNT]; 64 | // } RAW_BLOCK_T; 65 | 66 | // typedef struct RAW_PACKET 67 | // { 68 | // RAW_BLOCK_T blocks[BLOCKS_PER_PACKET]; 69 | // uint8_t reserved[RESERVE_SIZE]; 70 | // uint16_t revolution; 71 | // uint32_t timestamp; 72 | // uint8_t factory[2]; 73 | // double recv_time; 74 | // } RAW_PACKET_T; 75 | 76 | typedef struct RAW_MEASURE_{ 77 | double range; 78 | uint16_t reflectivity; 79 | } RAW_MEASURE_T; 80 | 81 | typedef struct RAW_BLOCK 82 | { 83 | uint16_t sob; 84 | uint16_t azimuth; 85 | RAW_MEASURE_T measures[MAX_LASER_COUNT]; 86 | // int laserCount; 87 | } RAW_BLOCK_T; 88 | 89 | typedef struct RAW_PACKET 90 | { 91 | RAW_BLOCK_T blocks[MAX_BLOCK_SIZE]; 92 | int blockAmount; 93 | uint32_t timestamp; 94 | unsigned int echo; 95 | } RAW_PACKET_T; 96 | 97 | // dual-return version 98 | typedef struct RAW_MEASURE_DUAL_{ 99 | unsigned int range; 100 | unsigned short reflectivity; 101 | } RAW_MEASURE_DUAL_T; 102 | 103 | typedef struct RAW_BLOCK_DUAL 104 | { 105 | uint16_t sob; 106 | uint16_t azimuth; 107 | RAW_MEASURE_DUAL_T measures[DUAL_VERSION_LASER_COUNT]; 108 | } RAW_BLOCK_DUAL_T; 109 | 110 | typedef struct RAW_PACKET_DUAL 111 | { 112 | RAW_BLOCK_DUAL_T blocks[DUAL_VERSION_BLOCKS_PER_PACKET]; 113 | uint8_t reserved[DUAL_VERSION_RESERVE_SIZE]; 114 | uint16_t revolution; 115 | uint32_t timestamp; 116 | unsigned int echo; 117 | unsigned char factory; 118 | } RAW_PACKET_DUAL_T; 119 | 120 | class RawData 121 | { 122 | public: 123 | 124 | RawData(const std::string& correctionFile, 125 | const unsigned int laserReturnType, 126 | const unsigned int laserNumber, 127 | const unsigned int pclType); 128 | ~RawData() {} 129 | int setup(); 130 | int unpack( 131 | PandarPacket &packet, 132 | PPointCloud &pc, 133 | time_t& gps1 , 134 | GPS_STRUCT_T &gps2, 135 | int& lidarRotationStartAngle); 136 | // int unpackSingleReturn( 137 | // PandarPacket &packet, 138 | // PPointCloud &pc, 139 | // time_t& gps1 , 140 | // GPS_STRUCT_T &gps2, 141 | // double& firstStamp, 142 | // int& lidarRotationStartAngle); 143 | // int unpackDualReturn( 144 | // PandarPacket &packet, 145 | // PPointCloud &pc, 146 | // time_t& gps1 , 147 | // GPS_STRUCT_T &gps2, 148 | // double& firstStamp, 149 | // int& lidarRotationStartAngle); 150 | 151 | private: 152 | 153 | typedef struct { 154 | std::string calibrationFile; ///< calibration file name 155 | double maxRange; ///< maximum range to publish 156 | double minRange; ///< minimum range to publish 157 | int minAngle; ///< minimum angle to publish 158 | int maxAngle; ///< maximum angle to publish 159 | } CONFIG_T; 160 | CONFIG_T config_; 161 | 162 | pandar_pointcloud::Calibration calibration_; 163 | float sin_lookup_table_[ROTATION_MAX_UNITS]; 164 | float cos_lookup_table_[ROTATION_MAX_UNITS]; 165 | RAW_PACKET_T *bufferPacket; 166 | 167 | int parseRawData(RAW_PACKET_T* packet, const uint8_t* buf, const int len); 168 | void toPointClouds (RAW_PACKET_T* packet,int laser, PPointCloud& pc, double stamp); 169 | 170 | // int parseRawData(RAW_PACKET_DUAL_T* packet, const uint8_t* buf, const int len); 171 | // void toPointClouds (RAW_PACKET_DUAL_T* packet,int laser, PPointCloud& pc, double stamp, double& firstStamp); 172 | 173 | void toPointClouds(RAW_PACKET_T* packet,int laser, int block,PPointCloud &pc,double blockstamp); 174 | // void toPointClouds(RAW_PACKET_DUAL_T* packet,int laser, int block,PPointCloud &pc,double blockstamp); 175 | 176 | void computeXYZIR( 177 | PPoint& point, 178 | int azimuth, 179 | const RAW_MEASURE_T& laserReturn, 180 | const pandar_pointcloud::PandarLaserCorrection& correction); 181 | // void computeXYZIR( 182 | // PPoint& point, 183 | // int azimuth, 184 | // const RAW_MEASURE_DUAL_T& laserReturn, 185 | // const pandar_pointcloud::PandarLaserCorrection& correction); 186 | 187 | int lastBlockEnd; 188 | int bufferPacketSize; 189 | int currentPacketStart; 190 | unsigned int lastTimestamp; 191 | int lastAzumith; 192 | DeviceType deviceType; 193 | int laserCountType; 194 | unsigned int pclDataType; 195 | int pandarEnableList[MAX_LASER_COUNT]; 196 | }; 197 | 198 | } // namespace pandar_rawdata 199 | 200 | #endif // __PANDAR_RAWDATA_H 201 | -------------------------------------------------------------------------------- /include/utilities.h: -------------------------------------------------------------------------------- 1 | #ifndef PANDORA_UTILITY_H 2 | #define PANDORA_UTILITY_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | void yuvToRgb(const int &iY, const int &iU, const int &iV, int &iR, int &iG, int &iB); 9 | void yuv400ToCvmat(cv::Mat &dst, void *pYUV400, int nWidth, int nHeight, int bitDepth); 10 | void yuv422ToRgb24(unsigned char *uyvy422, unsigned char *rgb24, int width, int height); 11 | void yuv422ToCvmat(cv::Mat &dst, void *pYUV422, int nWidth, int nHeight, int bitDepth); 12 | bool loadCameraIntrinsics(const std::string &filename, std::vector &intrinsicKList, std::vector &intrinsicDList); 13 | int decompressJpeg(unsigned char *jpgBuffer, unsigned long jpgSize, unsigned char * &bmp, unsigned long& bmpSize); 14 | 15 | #endif -------------------------------------------------------------------------------- /sample/pandar40/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | 3 | find_package(Boost REQUIRED ) 4 | find_package(PCL REQUIRED ) 5 | 6 | 7 | set (CMAKE_CXX_STANDARD 11) 8 | 9 | find_file(HAVE_HESAILIDARSDK_H_ PATHS "/usr/local/include" "/usr/include") 10 | message(STATUS "HAVE_HESAILIDARSDK_H_ == ${HAVE_HESAILIDARSDK_H_}") # might be HAVE_HESAILIDARSDK_H_-NOTFOUND 11 | 12 | macro(FIND_IN_PATH TARGET_H TARGET_H_IN_PATH) 13 | find_path( 14 | ${TARGET_H_IN_PATH} 15 | NAMES ${TARGET_H} 16 | PATHS "${CMAKE_SYSTEM_INCLUDE_PATH}/include" 17 | ) 18 | 19 | #message(STATUS "System include == ${CMAKE_SYSTEM_INCLUDE_PATH}") 20 | message(STATUS "\"${TARGET_H}\" in path == ${${TARGET_H_IN_PATH}}") 21 | endmacro() 22 | 23 | #set(CMAKE_REQUIRED_FLAGS "-std=c++11") 24 | #set(CMAKE_REQUIRED_INCLUDES "/usr/local/include") 25 | 26 | include(CheckIncludeFileCXX) 27 | #include(CheckIncludeFile) 28 | #include(CheckIncludeFiles) 29 | macro(CHECK_INCLUDE_FILE_CXX_ERROR INCLUDE_FILE HAVE_FILE) 30 | CHECK_INCLUDE_FILE_CXX(${INCLUDE_FILE} ${HAVE_FILE}) 31 | if(NOT ${HAVE_FILE}) 32 | unset(HAVE_FILE CACHE) 33 | message( FATAL_ERROR "${INCLUDE_FILE} is not found" ) 34 | endif() 35 | endmacro() 36 | 37 | #CHECK_INCLUDE_FILE_CXX("hesaiLidarSDK.h" HAVE_HESAILIDARSDK_H "-I/usr/local/include") 38 | CHECK_INCLUDE_FILE_CXX("hesaiLidarSDK.h" HAVE_HESAILIDARSDK_H) 39 | #CHECK_INCLUDE_FILE("hesaiLidarSDK.h" HAVE_HESAILIDARSDK_H) 40 | #CHECK_INCLUDE_FILES("hesaiLidarSDK.h" HAVE_HESAILIDARSDK_H) 41 | message(STATUS "HAVE_HESAILIDARSDK_H == ${HAVE_HESAILIDARSDK_H}") 42 | message(STATUS "HESAILIDARSDK_H_PATH == ${CMAKE_REQUIRED_INCLUDES}") 43 | unset(HAVE_HESAILIDARSDK_H CACHE) 44 | 45 | #CHECK_INCLUDE_FILE_CXX_ERROR("hesaiLidarSDK.h" HAVE_HESAILIDARSDK_H) 46 | #CHECK_INCLUDE_FILE_CXX_ERROR("point_types.h" HAVE_POINT_TYPES_H) 47 | 48 | FIND_IN_PATH("hesaiLidarSDK.h" HAVE_HESAILIDARSDK_H_IN_PATH) 49 | FIND_IN_PATH("point_types.h" HAVE_POINT_TYPES_H_IN_PATH) 50 | message(STATUS "\"hesaiLidarSDK.h\" in path == ${HAVE_HESAILIDARSDK_H_IN_PATH}") 51 | message(STATUS "\"point_types.h\" in path == ${HAVE_POINT_TYPES_H_IN_PATH}") 52 | 53 | if((NOT ${HAVE_HESAILIDARSDK_H_IN_PATH} STREQUAL "") AND (NOT ${HAVE_POINT_TYPES_H_IN_PATH} STREQUAL "")) 54 | include_directories( 55 | # ${CMAKE_CURRENT_SOURCE_DIR}/../../include # Use a command line parameter to provide the include files directory is feasible 56 | ${HAVE_HESAILIDARSDK_H_IN_PATH} 57 | ${HAVE_POINT_TYPES_H_IN_PATH} 58 | ${Boost_INCLUDE_DIRS} 59 | ${PCL_INCLUDE_DIRS} 60 | ) 61 | endif() 62 | 63 | message (STATUS "Debug include dir == ${CMAKE_CURRENT_SOURCE_DIR}/../../include") 64 | message (STATUS "Debug boost include dir == ${Boost_INCLUDE_DIRS}") 65 | message (STATUS "Debug pcl include dir ==${PCL_INCLUDE_DIRS}") 66 | message (STATUS "Debug hesaiLidarSDK dir ==${hesaiLidarSDK}") 67 | message (STATUS "Debug boost library dir == ${Boost_LIBRARIES}") 68 | message (STATUS "Debug pcl library dir == ${PCL_LIBRARIES}") 69 | message (STATUS "_____________________________________________________________________\ 70 | ") 71 | 72 | add_executable(pandar40 73 | pandar40.cc 74 | ) 75 | 76 | message (STATUS "HesaiLidarSDK_LIBS before initialized == ${HesaiLidarSDK_LIBS}") 77 | 78 | find_library(HesaiLidarSDK_LIBS hesaiLidarSDK) 79 | 80 | message (STATUS "HesaiLidarSDK_LIBS after initialized == ${HesaiLidarSDK_LIBS}") 81 | if( NOT HesaiLidarSDK_LIBS STREQUAL "HesaiLidarSDK_LIBS-NOTFOUND") 82 | # message( SEND_ERROR "You can't do that" ) 83 | target_link_libraries(pandar40 84 | hesaiLidarSDK 85 | ${Boost_LIBRARIES} 86 | ${PCL_LIBRARIES} 87 | ) 88 | elseif(HesaiLidarSDK_LIBS STREQUAL "HesaiLidarSDK_LIBS-NOTFOUND") 89 | message( FATAL_ERROR "HesaiLidarSDK_LIBS not found, CMake will exit." ) 90 | endif() 91 | 92 | 93 | -------------------------------------------------------------------------------- /sample/pandar40/pandar40.cc: -------------------------------------------------------------------------------- 1 | #include "hesaiLidarSDK.h" 2 | 3 | void gpsCallback(int timestamp) 4 | { 5 | printf("gps: %d" , timestamp); 6 | } 7 | 8 | void lidarCallback(boost::shared_ptr cld, double timestamp) 9 | { 10 | printf("lidar: time %lf , points %d\n", timestamp , cld->points.size()); 11 | } 12 | 13 | int main(int argc, char **argv) 14 | { 15 | HesaiLidarSDK psdk( 16 | 8080 /* lidar data port */, 17 | 8308 /* gps data port */, 18 | 0 /* start angle*/, 19 | std::string("correction.csv") /* calibration file of lidar */, 20 | lidarCallback /* point cloud data call back */, 21 | gpsCallback /* gps data callback */, 22 | HESAI_LIDAR_RAW_DATA_STRCUT_SINGLE_RETURN/* Return Mode: Single Return data structure */, 23 | 40 /* laser counter */, 24 | HESAI_LIDAR_PCL_DATA_TYPE_REDUCED/* pcl data alignment */ 25 | ); 26 | psdk.start(); 27 | while(true) 28 | { 29 | sleep(100); 30 | } 31 | } 32 | -------------------------------------------------------------------------------- /sample/pandar40p/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | 3 | find_package(Boost REQUIRED ) 4 | find_package(PCL REQUIRED ) 5 | 6 | 7 | set (CMAKE_CXX_STANDARD 11) 8 | 9 | macro(FIND_IN_PATH TARGET_H TARGET_H_IN_PATH) 10 | find_path( 11 | ${TARGET_H_IN_PATH} 12 | NAMES ${TARGET_H} 13 | PATHS "${CMAKE_SYSTEM_INCLUDE_PATH}/include" 14 | ) 15 | 16 | #message(STATUS "System include == ${CMAKE_SYSTEM_INCLUDE_PATH}") 17 | message(STATUS "\"${TARGET_H}\" in path == ${${TARGET_H_IN_PATH}}") 18 | endmacro() 19 | 20 | FIND_IN_PATH("hesaiLidarSDK.h" HAVE_HESAILIDARSDK_H_IN_PATH) 21 | FIND_IN_PATH("point_types.h" HAVE_POINT_TYPES_H_IN_PATH) 22 | message(STATUS "\"hesaiLidarSDK.h\" in path == ${HAVE_HESAILIDARSDK_H_IN_PATH}") 23 | message(STATUS "\"point_types.h\" in path == ${HAVE_POINT_TYPES_H_IN_PATH}") 24 | 25 | if((NOT ${HAVE_HESAILIDARSDK_H_IN_PATH} STREQUAL "") AND (NOT ${HAVE_POINT_TYPES_H_IN_PATH} STREQUAL "")) 26 | include_directories( 27 | # ${CMAKE_CURRENT_SOURCE_DIR}/../../include # Use a command line parameter to provide the include files directory is feasible 28 | ${HAVE_HESAILIDARSDK_H_IN_PATH} 29 | ${HAVE_POINT_TYPES_H_IN_PATH} 30 | ${Boost_INCLUDE_DIRS} 31 | ${PCL_INCLUDE_DIRS} 32 | ) 33 | endif() 34 | 35 | add_executable(pandar40p 36 | pandar40p.cc 37 | ) 38 | 39 | message (STATUS "HesaiLidarSDK_LIBS before initialized == ${HesaiLidarSDK_LIBS}") 40 | 41 | find_library(HesaiLidarSDK_LIBS hesaiLidarSDK) 42 | 43 | message (STATUS "HesaiLidarSDK_LIBS after initialized == ${HesaiLidarSDK_LIBS}") 44 | if( NOT HesaiLidarSDK_LIBS STREQUAL "HesaiLidarSDK_LIBS-NOTFOUND") 45 | target_link_libraries(pandar40p 46 | hesaiLidarSDK 47 | ${Boost_LIBRARIES} 48 | ${PCL_LIBRARIES} 49 | ) 50 | elseif(HesaiLidarSDK_LIBS STREQUAL "HesaiLidarSDK_LIBS-NOTFOUND") 51 | message( FATAL_ERROR "HesaiLidarSDK_LIBS not found, CMake will exit." ) 52 | endif() 53 | 54 | 55 | -------------------------------------------------------------------------------- /sample/pandar40p/pandar40p.cc: -------------------------------------------------------------------------------- 1 | #include "hesaiLidarSDK.h" 2 | 3 | void gpsCallback(int timestamp) 4 | { 5 | printf("gps: %d" , timestamp); 6 | } 7 | 8 | void lidarCallback(boost::shared_ptr cld, double timestamp) 9 | { 10 | printf("lidar: time %lf , points %d\n", timestamp , cld->points.size()); 11 | } 12 | 13 | int main(int argc, char **argv) 14 | { 15 | HesaiLidarSDK psdk( 16 | 2368 /* lidar data port */, 17 | 10110 /* gps data port */, 18 | std::string("correction.csv") /* calibration file of lidar */, 19 | lidarCallback /* point cloud data call back */, 20 | gpsCallback /* gps data callback */, 21 | HESAI_LIDAR_RAW_DATA_STRCUT_DUAL_RETURN/* Return Mode: Dual Return data structure */, 22 | 40 /* laser counter */, 23 | HESAI_LIDAR_PCL_DATA_TYPE_REDUCED/* pcl data alignment */ 24 | ); 25 | psdk.start(); 26 | while(true) 27 | { 28 | sleep(100); 29 | } 30 | } -------------------------------------------------------------------------------- /sample/pandora/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | 3 | find_package(Boost REQUIRED ) 4 | find_package(PCL REQUIRED ) 5 | find_package(OpenCV REQUIRED ) 6 | 7 | 8 | set (CMAKE_CXX_STANDARD 11) 9 | 10 | # When sdk compiled with Camera_Enable , This definition must be add to cmakelists 11 | add_definitions("-DHESAI_WITH_CAMERA") 12 | 13 | macro(FIND_IN_PATH TARGET_H TARGET_H_IN_PATH) 14 | find_path( 15 | ${TARGET_H_IN_PATH} 16 | NAMES ${TARGET_H} 17 | PATHS "${CMAKE_SYSTEM_INCLUDE_PATH}/include" 18 | ) 19 | 20 | #message(STATUS "System include == ${CMAKE_SYSTEM_INCLUDE_PATH}") 21 | message(STATUS "\"${TARGET_H}\" in path == ${${TARGET_H_IN_PATH}}") 22 | endmacro() 23 | 24 | FIND_IN_PATH("hesaiLidarSDK.h" HAVE_HESAILIDARSDK_H_IN_PATH) 25 | FIND_IN_PATH("point_types.h" HAVE_POINT_TYPES_H_IN_PATH) 26 | message(STATUS "\"hesaiLidarSDK.h\" in path == ${HAVE_HESAILIDARSDK_H_IN_PATH}") 27 | message(STATUS "\"point_types.h\" in path == ${HAVE_POINT_TYPES_H_IN_PATH}") 28 | 29 | if((NOT ${HAVE_HESAILIDARSDK_H_IN_PATH} STREQUAL "") AND (NOT ${HAVE_POINT_TYPES_H_IN_PATH} STREQUAL "")) 30 | include_directories( 31 | # ${CMAKE_CURRENT_SOURCE_DIR}/../../include # Use a command line parameter to provide the include files directory is feasible 32 | ${HAVE_HESAILIDARSDK_H_IN_PATH} 33 | ${HAVE_POINT_TYPES_H_IN_PATH} 34 | ${Boost_INCLUDE_DIRS} 35 | ${PCL_INCLUDE_DIRS} 36 | ) 37 | endif() 38 | 39 | add_executable(pandora 40 | pandora.cc 41 | ) 42 | 43 | message (STATUS "HesaiLidarSDK_LIBS before initialized == ${HesaiLidarSDK_LIBS}") 44 | find_library(HesaiLidarSDK_LIBS hesaiLidarSDK) 45 | message (STATUS "HesaiLidarSDK_LIBS after initialized == ${HesaiLidarSDK_LIBS}") 46 | 47 | if( NOT HesaiLidarSDK_LIBS STREQUAL "HesaiLidarSDK_LIBS-NOTFOUND") 48 | target_link_libraries(pandora 49 | hesaiLidarSDK 50 | ${Boost_LIBRARIES} 51 | ${PCL_LIBRARIES} 52 | ${OpenCV_LIBS} 53 | jpeg 54 | ) 55 | elseif(HesaiLidarSDK_LIBS STREQUAL "HesaiLidarSDK_LIBS-NOTFOUND") 56 | message( FATAL_ERROR "HesaiLidarSDK_LIBS not found, CMake will exit." ) 57 | endif() 58 | 59 | -------------------------------------------------------------------------------- /sample/pandora/pandora.cc: -------------------------------------------------------------------------------- 1 | #include "hesaiLidarSDK.h" 2 | 3 | 4 | void cameraCallback(boost::shared_ptr matp, double timestamp, int pic_id) 5 | { 6 | printf("cameraid: %d, timestamp: %lf\n", pic_id, timestamp); 7 | } 8 | 9 | void gpsCallback(int timestamp) 10 | { 11 | printf("gps: %d\n", timestamp); 12 | } 13 | 14 | 15 | 16 | void lidarCallback(boost::shared_ptr cld, double timestamp) 17 | { 18 | printf("lidar: %lf\n", timestamp); 19 | } 20 | 21 | int main(int argc, char **argv) 22 | { 23 | HesaiLidarSDK psdk( 24 | std::string("192.168.20.51"), 25 | 9870, 26 | std::string("calibration.yml"), 27 | cameraCallback, 28 | 2368, 10110, 0, 29 | std::string("correction.csv"), 30 | lidarCallback, gpsCallback, 31 | HESAI_LIDAR_RAW_DATA_STRCUT_DUAL_RETURN, 40, HESAI_LIDAR_PCL_DATA_TYPE_REDUCED); 32 | 33 | psdk.start(); 34 | while(true) 35 | { 36 | sleep(100); 37 | } 38 | } -------------------------------------------------------------------------------- /src/calibration.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include "calibration.h" 7 | 8 | namespace angles 9 | { 10 | double degreeToRadian(double degree) 11 | { 12 | return degree * M_PI / 180; 13 | } 14 | } 15 | 16 | namespace pandar_pointcloud 17 | { 18 | 19 | // elevation angle of each line for HS Line 40 Lidar, Line 1 - Line 40 20 | static const float hesai_elev_angle_map[] = { 21 | 6.96, 5.976, 4.988, 3.996, 22 | 2.999, 2.001, 1.667, 1.333, 23 | 1.001, 0.667, 0.333, 0, 24 | -0.334, -0.667, -1.001, -1.334, 25 | -1.667, -2.001, -2.331, -2.667, 26 | -3, -3.327, -3.663, -3.996, 27 | -4.321, -4.657, -4.986, -5.311, 28 | -5.647, -5.974, -6.957, -7.934, 29 | -8.908, -9.871, -10.826, -11.772, 30 | -12.705, -13.63, -14.543, -15.444}; 31 | 32 | // Line 40 Lidar azimuth Horizatal offset , Line 1 - Line 40 33 | static const float hesai_horizatal_azimuth_offset_map[] = { 34 | 0.005, 0.006, 0.006, 0.006, 35 | -2.479, -2.479, 2.491, -4.953, 36 | -2.479, 2.492, -4.953, -2.479, 37 | 2.492, -4.953, 0.007, 2.491, 38 | -4.953, 0.006, 4.961, -2.479, 39 | 0.006, 4.96, -2.478, 0.006, 40 | 4.958, -2.478, 2.488, 4.956, 41 | -2.477, 2.487, 2.485, 2.483, 42 | 0.004, 0.004, 0.003, 0.003, 43 | -2.466, -2.463, -2.46, -2.457}; 44 | 45 | void Calibration::setDefaultCorrections() 46 | { 47 | for (int i = 0; i < laserNumber; i++) 48 | { 49 | laserCorrections[i].azimuthCorrection = hesai_horizatal_azimuth_offset_map[i]; 50 | laserCorrections[i].distanceCorrection = 0.0; 51 | laserCorrections[i].horizontalOffsetCorrection = 0.0; 52 | laserCorrections[i].verticalOffsetCorrection = 0.0; 53 | laserCorrections[i].verticalCorrection = hesai_elev_angle_map[i]; 54 | laserCorrections[i].sinVertCorrection = std::sin(hesai_elev_angle_map[i] / 180.0 * M_PI); 55 | laserCorrections[i].cosVertCorrection = std::cos(hesai_elev_angle_map[i] / 180.0 * M_PI); 56 | } 57 | } 58 | 59 | void Calibration::read(const std::string &calibrationFile) 60 | { 61 | initialized = true; 62 | setDefaultCorrections(); 63 | if (calibrationFile.empty()) 64 | return; 65 | 66 | boost::filesystem::path filePath(calibrationFile); 67 | if (!boost::filesystem::is_regular(filePath)) 68 | { printf("invalid lidar correction file, use default values\n"); 69 | return; 70 | } 71 | 72 | std::ifstream ifs(filePath.string()); 73 | if (!ifs.is_open()) 74 | return; 75 | 76 | std::string line; 77 | if (std::getline(ifs, line)) 78 | { // first line "Laser id,Elevation,Azimuth" 79 | std::cout << "parsing correction file." << std::endl; 80 | } 81 | 82 | int lineCounter = 0; 83 | while (std::getline(ifs, line)) 84 | { 85 | if (lineCounter++ >= 40) 86 | break; 87 | 88 | int lineId = 0; 89 | double elev, azimuth; 90 | 91 | std::stringstream ss(line); 92 | std::string subline; 93 | std::getline(ss, subline, ','); 94 | std::stringstream(subline) >> lineId; 95 | std::getline(ss, subline, ','); 96 | std::stringstream(subline) >> elev; 97 | std::getline(ss, subline, ','); 98 | std::stringstream(subline) >> azimuth; 99 | lineId--; 100 | laserCorrections[lineId].azimuthCorrection = azimuth; 101 | laserCorrections[lineId].distanceCorrection = 0.0; 102 | laserCorrections[lineId].horizontalOffsetCorrection = 0.0; 103 | laserCorrections[lineId].verticalOffsetCorrection = 0.0; 104 | laserCorrections[lineId].verticalCorrection = elev; 105 | laserCorrections[lineId].sinVertCorrection = std::sin(angles::degreeToRadian(elev)); 106 | laserCorrections[lineId].cosVertCorrection = std::cos(angles::degreeToRadian(elev)); 107 | } 108 | } 109 | 110 | } /* pandar_pointcloud */ 111 | -------------------------------------------------------------------------------- /src/hesaiLidarSDK.cc: -------------------------------------------------------------------------------- 1 | #include "hesaiLidarSDK.h" 2 | #include "hesaiLidarSDK_IN.h" 3 | 4 | #ifdef HESAI_WITH_CAMERA 5 | HesaiLidarSDK::HesaiLidarSDK( 6 | const std::string pandoraIP, 7 | const unsigned short pandoraCameraPort, 8 | const std::string intrinsicFile, 9 | boost::function matp, double timestamp, int pic_id)> cameraCallback, 10 | const unsigned short lidarRecvPort, 11 | const unsigned short gpsRecvPort, 12 | const double startAngle, 13 | const std::string lidarCorrectionFile, 14 | 15 | boost::function cld, double timestamp)> lidarCallback, 16 | boost::function gpsCallback, 17 | const HesaiLidarRawDataSturct laserReturnType, 18 | const unsigned int laserCount, 19 | const HesaiLidarPCLDataType pclDataType) 20 | { 21 | psi = new HesaiLidarSDK_internal( 22 | pandoraIP, 23 | pandoraCameraPort, 24 | intrinsicFile, 25 | cameraCallback, 26 | lidarRecvPort, 27 | gpsRecvPort, 28 | startAngle, 29 | lidarCorrectionFile, 30 | lidarCallback, 31 | gpsCallback, 32 | (unsigned int)laserReturnType, laserCount, (unsigned int)pclDataType); 33 | } 34 | 35 | #endif 36 | 37 | HesaiLidarSDK::HesaiLidarSDK( 38 | #ifdef HESAI_WITH_CAMERA 39 | const std::string pandoraIP, 40 | const unsigned short pandoraCameraPort, 41 | boost::function matp, double timestamp, int pic_id)> cameraCallback, 42 | #endif 43 | boost::function cld, double timestamp)> lidarCallback) 44 | { 45 | psi = new HesaiLidarSDK_internal( 46 | #ifdef HESAI_WITH_CAMERA 47 | pandoraIP, 48 | pandoraCameraPort, 49 | cameraCallback, 50 | #endif 51 | lidarCallback); 52 | } 53 | 54 | HesaiLidarSDK::HesaiLidarSDK( 55 | const unsigned short lidarRecvPort, 56 | const unsigned short gpsRecvPort, 57 | const double startAngle, 58 | const std::string lidarCorrectionFile, 59 | boost::function cld, double timestamp)> lidarCallback, 60 | boost::function gpsCallback, 61 | const HesaiLidarRawDataSturct laserReturnType, 62 | const unsigned int laserCount, 63 | const HesaiLidarPCLDataType pclDataType) 64 | { 65 | psi = new HesaiLidarSDK_internal( 66 | #ifdef HESAI_WITH_CAMERA 67 | std::string(""), 68 | 9870, 69 | std::string(""), 70 | NULL, 71 | #endif 72 | lidarRecvPort, 73 | gpsRecvPort, 74 | startAngle, 75 | lidarCorrectionFile, 76 | lidarCallback, 77 | gpsCallback, 78 | (unsigned int)laserReturnType, laserCount, (unsigned int)pclDataType); 79 | } 80 | 81 | HesaiLidarSDK::HesaiLidarSDK( 82 | const std::string pcapPath, 83 | const std::string lidarCorrectionFile, 84 | const HesaiLidarRawDataSturct laserReturnType, 85 | const unsigned int laserCount, 86 | const HesaiLidarPCLDataType pclDataType, 87 | boost::function pcloudp, double timestamp)> lidarCallback) 88 | { 89 | psi = new HesaiLidarSDK_internal( 90 | pcapPath, 91 | lidarCorrectionFile, 92 | (unsigned int)laserReturnType, laserCount, (unsigned int)pclDataType, 93 | lidarCallback); 94 | } 95 | 96 | HesaiLidarSDK::~HesaiLidarSDK() 97 | { 98 | delete psi; 99 | } 100 | 101 | int HesaiLidarSDK::start() 102 | { 103 | psi->start(); 104 | } 105 | void HesaiLidarSDK::stop() 106 | { 107 | psi->stop(); 108 | } 109 | -------------------------------------------------------------------------------- /src/hesaiLidarSDK_IN.cc: -------------------------------------------------------------------------------- 1 | 2 | 3 | #include "hesaiLidarSDK_IN.h" 4 | #ifdef HESAI_WITH_CAMERA 5 | #include "utilities.h" 6 | 7 | Size HesaiLidarSDK_IMAGE_SIZE(HesaiLidarSDK_IMAGE_WIDTH, HesaiLidarSDK_IMAGE_HEIGHT); 8 | #endif 9 | 10 | int HS_L40_GPS_Parse(HS_LIDAR_L40_GPS_Packet *packet, const unsigned char *recvbuf) 11 | { 12 | int index = 0; 13 | packet->flag = (recvbuf[index] & 0xff) | ((recvbuf[index + 1] & 0xff) << 8); 14 | index += HS_LIDAR_L40_GPS_PACKET_FLAG_SIZE; 15 | packet->year = (recvbuf[index] & 0xff - 0x30) + (recvbuf[index + 1] & 0xff - 0x30) * 10; 16 | index += HS_LIDAR_L40_GPS_PACKET_YEAR_SIZE; 17 | packet->month = (recvbuf[index] & 0xff - 0x30) + (recvbuf[index + 1] & 0xff - 0x30) * 10; 18 | index += HS_LIDAR_L40_GPS_PACKET_MONTH_SIZE; 19 | packet->day = (recvbuf[index] & 0xff - 0x30) + (recvbuf[index + 1] & 0xff - 0x30) * 10; 20 | index += HS_LIDAR_L40_GPS_PACKET_DAY_SIZE; 21 | packet->second = (recvbuf[index] & 0xff - 0x30) + (recvbuf[index + 1] & 0xff - 0x30) * 10; 22 | index += HS_LIDAR_L40_GPS_PACKET_SECOND_SIZE; 23 | packet->minute = (recvbuf[index] & 0xff - 0x30) + (recvbuf[index + 1] & 0xff - 0x30) * 10; 24 | index += HS_LIDAR_L40_GPS_PACKET_MINUTE_SIZE; 25 | packet->hour = (recvbuf[index] & 0xff - 0x30) + (recvbuf[index + 1] & 0xff - 0x30) * 10; 26 | index += HS_LIDAR_L40_GPS_PACKET_HOUR_SIZE; 27 | packet->fineTime = (recvbuf[index] & 0xff) | (recvbuf[index + 1] & 0xff) << 8 | 28 | ((recvbuf[index + 2] & 0xff) << 16) | ((recvbuf[index + 3] & 0xff) << 24); 29 | #ifdef DEBUG 30 | printf("error gps\n"); 31 | if(packet->year != 18) 32 | { 33 | char str[128]; 34 | int fd = open("/var/tmp/error_gps.txt" , O_RDWR | O_CREAT , 0666); 35 | lseek(fd , 0 , SEEK_END); 36 | int i =0; 37 | for(i = 0 ; i < 512 ; i ++) 38 | { 39 | sprintf(str , "%02x " , recvbuf[i]); 40 | write(fd , str , strlen(str)); 41 | } 42 | write(fd , "\n" , 1); 43 | close(fd); 44 | } 45 | #endif 46 | 47 | return 0; 48 | } 49 | 50 | 51 | void HesaiLidarSDK_internal::init( 52 | const std::string pcapPath, 53 | const std::string lidarCorrectionFile, 54 | const unsigned int laserReturnType, 55 | const unsigned int laserCount, 56 | const unsigned int pclDataType, 57 | boost::function pcloudp, double timestamp)> lidarCallback) 58 | { 59 | useMode = PandoraUseModeReadPcap; 60 | input.reset(new pandar_pointcloud::Input(pcapPath, laserReturnType)); 61 | data_.reset(new pandar_rawdata::RawData(lidarCorrectionFile, laserReturnType, laserCount, pclDataType)); 62 | readPacpThread = 0; 63 | lidarRecvThread = 0; 64 | lidarProcessThread = 0; 65 | lidarRotationStartAngle = 0; 66 | gps1 = 0; 67 | gps2.gps = 0; 68 | gps2.used = 1; 69 | gps2.usedHour = 1; 70 | userLidarCallback = lidarCallback; 71 | #ifdef HESAI_WITH_CAMERA 72 | pandoraCameraClient = NULL; 73 | processPicThread = 0; 74 | #endif 75 | 76 | sem_init(&lidarSem, 0, 0); 77 | pthread_mutex_init(&lidarLock, NULL); 78 | pthread_mutex_init(&lidarGpsLock, NULL); 79 | } 80 | 81 | void HesaiLidarSDK_internal::init( 82 | #ifdef HESAI_WITH_CAMERA 83 | const std::string pandoraIP, 84 | const unsigned short pandoraCameraPort, 85 | const std::string intrinsicFile, 86 | boost::function matp, double timestamp, int pic_id)> cameraCallback, 87 | #endif 88 | const unsigned short lidarRecvPort, 89 | const unsigned short gpsRecvPort, 90 | const double startAngle, 91 | const std::string lidarCorrectionFile, 92 | boost::function cld, double timestamp)> lidarCallback, 93 | boost::function gpsCallback, 94 | const unsigned int laserReturnType, 95 | const unsigned int laserCount, 96 | const unsigned int pclDataType) 97 | { 98 | #ifdef HESAI_WITH_CAMERA 99 | ip = pandoraIP; 100 | cport = pandoraCameraPort; 101 | userCameraCallback = cameraCallback; 102 | if (ip.empty()) 103 | useMode = PandoraUseModeOnlyLidar; 104 | else 105 | useMode = PandoraUseModeLidarAndCamera; 106 | pthread_mutex_init(&picLock, NULL); 107 | processPicThread = 0; 108 | sem_init(&picSem, 0, 0); 109 | 110 | if (intrinsicFile.empty()) 111 | { 112 | needRemapPicMat = false; 113 | } 114 | else 115 | { 116 | if (loadIntrinsics(intrinsicFile)) 117 | needRemapPicMat = true; 118 | else 119 | needRemapPicMat = false; 120 | } 121 | pandoraCameraClient = NULL; 122 | #else 123 | useMode = PandoraUseModeOnlyLidar; 124 | #endif 125 | 126 | userLidarCallback = lidarCallback; 127 | userGpsCallback = gpsCallback; 128 | lidarRotationStartAngle = static_cast(startAngle * 100); 129 | 130 | gps1 = 0; 131 | gps2.gps = 0; 132 | gps2.used = 1; 133 | gps2.usedHour = 1; 134 | lidarLastGPSHourSecond = 0; 135 | 136 | sem_init(&lidarSem, 0, 0); 137 | 138 | pthread_mutex_init(&lidarLock, NULL); 139 | pthread_mutex_init(&lidarGpsLock, NULL); 140 | 141 | lidarRecvThread = 0; 142 | lidarProcessThread = 0; 143 | readPacpThread = 0; 144 | 145 | 146 | 147 | input.reset(new pandar_pointcloud::Input(lidarRecvPort, gpsRecvPort)); 148 | data_.reset(new pandar_rawdata::RawData(lidarCorrectionFile, laserReturnType, laserCount, pclDataType)); 149 | } 150 | 151 | HesaiLidarSDK_internal::HesaiLidarSDK_internal( 152 | #ifdef HESAI_WITH_CAMERA 153 | const std::string pandoraIP, 154 | const unsigned short pandoraCameraPort, 155 | const std::string intrinsicFile, 156 | boost::function matp, double timestamp, int pic_id)> cameraCallback, 157 | #endif 158 | const unsigned short lidarRecvPort, 159 | const unsigned short gpsRecvPort, 160 | const double startAngle, 161 | const std::string lidarCorrectionFile, 162 | boost::function cld, double timestamp)> lidarCallback, 163 | boost::function gpsCallback, 164 | const unsigned int laserReturnType, 165 | const unsigned int laserCount, 166 | const unsigned int pclDataType) 167 | 168 | { 169 | init( 170 | #ifdef HESAI_WITH_CAMERA 171 | pandoraIP, 172 | pandoraCameraPort, 173 | intrinsicFile, 174 | cameraCallback, 175 | #endif 176 | lidarRecvPort, 177 | gpsRecvPort, 178 | startAngle, 179 | 180 | lidarCorrectionFile, 181 | 182 | lidarCallback, 183 | gpsCallback, 184 | laserReturnType, laserCount, pclDataType); 185 | } 186 | 187 | HesaiLidarSDK_internal::HesaiLidarSDK_internal( 188 | #ifdef HESAI_WITH_CAMERA 189 | const std::string pandoraIP, 190 | const unsigned short pandoraCameraPort, 191 | boost::function matp, double timestamp, int pic_id)> cameraCallback, 192 | #endif 193 | boost::function cld, double timestamp)> lidarCallback) 194 | { 195 | init( 196 | #ifdef HESAI_WITH_CAMERA 197 | pandoraIP, 198 | pandoraCameraPort, 199 | std::string(""), 200 | cameraCallback, 201 | #endif 202 | HesaiLidarSDK_DEFAULT_LIDAR_RECV_PORT, 203 | HesaiLidarSDK_DEFAULT_GPS_RECV_PORT, 204 | HesaiLidarSDK_DEFAULT_START_ANGLE, 205 | std::string(""), 206 | lidarCallback, 207 | NULL, 0, 40, 0); 208 | } 209 | 210 | HesaiLidarSDK_internal::HesaiLidarSDK_internal( 211 | const std::string pcapPath, 212 | const std::string lidarCorrectionFile, 213 | const unsigned int laserReturnType, 214 | const unsigned int laserCount, 215 | const unsigned int pclDataType, 216 | boost::function pcloudp, double timestamp)> lidarCallback) 217 | { 218 | init( 219 | pcapPath, 220 | lidarCorrectionFile, 221 | laserReturnType, 222 | laserCount, 223 | pclDataType, 224 | lidarCallback); 225 | } 226 | 227 | HesaiLidarSDK_internal::~HesaiLidarSDK_internal() 228 | { 229 | stop(); 230 | } 231 | 232 | int HesaiLidarSDK_internal::start() 233 | { 234 | stop(); 235 | switch (useMode) 236 | { 237 | case PandoraUseModeReadPcap: 238 | { 239 | setupReadPcap(); 240 | break; 241 | } 242 | 243 | case PandoraUseModeOnlyLidar: 244 | { 245 | setupLidarClient(); 246 | break; 247 | } 248 | #ifdef HESAI_WITH_CAMERA 249 | case PandoraUseModeLidarAndCamera: 250 | { 251 | 252 | setupCameraClient(); 253 | setupLidarClient(); 254 | break; 255 | } 256 | #endif 257 | default: 258 | { 259 | printf("wrong useMode\n"); 260 | return -1; 261 | } 262 | } 263 | 264 | return 0; 265 | } 266 | 267 | void HesaiLidarSDK_internal::setupReadPcap() 268 | { 269 | continueReadPcap = true; 270 | readPacpThread = new boost::thread(boost::bind(&HesaiLidarSDK_internal::readPcapFile, this)); 271 | continueProcessLidarPacket = true; 272 | lidarProcessThread = new boost::thread(boost::bind(&HesaiLidarSDK_internal::processLiarPacket, this)); 273 | } 274 | 275 | void HesaiLidarSDK_internal::setupLidarClient() 276 | { 277 | continueLidarRecvThread = true; 278 | continueProcessLidarPacket = true; 279 | lidarProcessThread = new boost::thread(boost::bind(&HesaiLidarSDK_internal::processLiarPacket, this)); 280 | lidarRecvThread = new boost::thread(boost::bind(&HesaiLidarSDK_internal::lidarRecvTask, this)); 281 | } 282 | 283 | void HesaiLidarSDK_internal::readPcapFile() 284 | { 285 | while (continueReadPcap) 286 | { 287 | PandarPacket pkt; 288 | int rc = input->getPacketFromPcap(&pkt); // todo 289 | usleep(HesaiLidarSDK_PCAP_TIME_INTERVAL); 290 | 291 | if (rc == -1) 292 | { 293 | usleep(HesaiLidarSDK_PCAP_TIME_INTERVAL); 294 | continue; 295 | } 296 | if (rc == 1) 297 | { 298 | // gps packet; 299 | HS_L40_GPS_Parse(&hesaiGps, &pkt.data[0]); 300 | processGps(hesaiGps); 301 | continue; 302 | } 303 | // data packet 304 | pushLiDARData(pkt); 305 | // 10 ms for read 306 | } 307 | } 308 | 309 | void HesaiLidarSDK_internal::stop() 310 | { 311 | continueLidarRecvThread = false; 312 | continueProcessLidarPacket = false; 313 | 314 | continueReadPcap = false; 315 | 316 | if (lidarProcessThread) 317 | { 318 | lidarProcessThread->join(); 319 | delete lidarProcessThread; 320 | lidarProcessThread = 0; 321 | } 322 | if (lidarRecvThread) 323 | { 324 | lidarRecvThread->join(); 325 | delete lidarRecvThread; 326 | lidarRecvThread = 0; 327 | } 328 | #ifdef HESAI_WITH_CAMERA 329 | continueProcessPic = false; 330 | if (pandoraCameraClient) 331 | { 332 | PandoraClientDestroy(pandoraCameraClient); 333 | pandoraCameraClient = NULL; 334 | } 335 | 336 | if (processPicThread) 337 | { 338 | processPicThread->join(); 339 | delete processPicThread; 340 | processPicThread = 0; 341 | } 342 | #endif 343 | if (readPacpThread) 344 | { 345 | readPacpThread->join(); 346 | readPacpThread = 0; 347 | } 348 | } 349 | 350 | 351 | 352 | 353 | 354 | 355 | void HesaiLidarSDK_internal::lidarRecvTask() 356 | { 357 | while (continueLidarRecvThread) 358 | { 359 | PandarPacket pkt; 360 | int rc = input->getPacket(&pkt); 361 | if (rc == -1) 362 | { 363 | continue; 364 | } 365 | if (rc == 1) 366 | { 367 | // gps packet; 368 | HS_L40_GPS_Parse(&hesaiGps, &pkt.data[0]); 369 | processGps(hesaiGps); 370 | continue; 371 | } 372 | pushLiDARData(pkt); 373 | // internalAnalysisPacket(pkt); 374 | } 375 | } 376 | 377 | void HesaiLidarSDK_internal::processLiarPacket() 378 | { 379 | double lastTimestamp = 0.0f; 380 | struct timespec ts; 381 | 382 | while (continueProcessLidarPacket) 383 | { 384 | boost::shared_ptr outMsg(new PPointCloud()); 385 | if (clock_gettime(CLOCK_REALTIME, &ts) == -1) 386 | { 387 | printf("get time error\n"); 388 | } 389 | 390 | ts.tv_sec += 1; 391 | if (sem_timedwait(&lidarSem, &ts) == -1) 392 | { 393 | continue; 394 | } 395 | pthread_mutex_lock(&lidarLock); 396 | PandarPacket packet = lidarPacketList.front(); 397 | lidarPacketList.pop_front(); 398 | pthread_mutex_unlock(&lidarLock); 399 | outMsg->header.frame_id = "pandar"; 400 | outMsg->height = 1; 401 | 402 | pthread_mutex_lock(&lidarGpsLock); 403 | // printf("gps: %d\n", gps1); 404 | int ret = data_->unpack(packet, *outMsg, gps1, gps2, lidarRotationStartAngle); 405 | pthread_mutex_unlock(&lidarGpsLock); 406 | if (ret == 1 && outMsg->points.size() > 0) 407 | { 408 | lastTimestamp = outMsg->points[0].timestamp; 409 | if (userLidarCallback) 410 | userLidarCallback(outMsg, lastTimestamp); 411 | } 412 | } 413 | } 414 | 415 | void HesaiLidarSDK_internal::pushLiDARData(PandarPacket packet) 416 | { 417 | pthread_mutex_lock(&lidarLock); 418 | lidarPacketList.push_back(packet); 419 | if (lidarPacketList.size() > 6) 420 | { 421 | sem_post(&lidarSem); 422 | } 423 | pthread_mutex_unlock(&lidarLock); 424 | } 425 | 426 | void HesaiLidarSDK_internal::processGps(HS_LIDAR_L40_GPS_Packet &gpsMsg) 427 | { 428 | struct tm t; 429 | 430 | t.tm_sec = 0; 431 | t.tm_min = 0; 432 | 433 | t.tm_hour = gpsMsg.hour; 434 | t.tm_mday = gpsMsg.day; 435 | t.tm_mon = gpsMsg.month - 1; 436 | t.tm_year = gpsMsg.year + 2000 - 1900; 437 | t.tm_isdst = 0; 438 | 439 | int curlidarLastGPSHourSecond = mktime(&t); 440 | 441 | t.tm_sec = gpsMsg.second; 442 | t.tm_min = gpsMsg.minute; 443 | 444 | if (lidarLastGPSSecond != (mktime(&t) + 1)) 445 | { 446 | // Send the last GPS Time when the PPS occurs 447 | lidarLastGPSSecond = mktime(&t) + 1; 448 | pthread_mutex_lock(&lidarGpsLock); 449 | gps2.gps = lidarLastGPSSecond; 450 | gps2.used = 0; 451 | if (curlidarLastGPSHourSecond != lidarLastGPSHourSecond) 452 | { 453 | lidarLastGPSHourSecond = curlidarLastGPSHourSecond; 454 | gps2.usedHour = 0; 455 | } 456 | 457 | // TODO: if the jump is too big(24 * 3600 = one day) , We regard this GPS is reliable. use it. 458 | if((curlidarLastGPSHourSecond - lidarLastGPSHourSecond ) > (24 * 3600)) 459 | { 460 | gps1 = 0; 461 | gps2.usedHour = 0; 462 | } 463 | 464 | memcpy(&gps2.t, &t, sizeof(struct tm)); 465 | gps2.t.tm_min = 0; 466 | gps2.t.tm_sec = 0; 467 | 468 | pthread_mutex_unlock(&lidarGpsLock); 469 | } 470 | if (userGpsCallback) 471 | userGpsCallback(lidarLastGPSSecond); 472 | } 473 | 474 | 475 | #ifdef HESAI_WITH_CAMERA 476 | 477 | static int cameraClientCallback(void *handle, int cmd, void *param, void *userp) 478 | { 479 | PandoraPic *pic = (PandoraPic *)param; 480 | HesaiLidarSDK_internal *pSDK = (HesaiLidarSDK_internal *)userp; 481 | pSDK->pushPicture(pic); 482 | return 0; 483 | } 484 | 485 | void HesaiLidarSDK_internal::setupCameraClient() 486 | { 487 | continueProcessPic = true; 488 | pandoraCameraClient = PandoraClientNew(ip.c_str(), cport, cameraClientCallback, this); 489 | processPicThread = new boost::thread(boost::bind(&HesaiLidarSDK_internal::processPic, this)); 490 | } 491 | 492 | void HesaiLidarSDK_internal::pushPicture(PandoraPic *pic) 493 | { 494 | pthread_mutex_lock(&picLock); 495 | pictureList.push_back(pic); 496 | pthread_mutex_unlock(&picLock); 497 | sem_post(&picSem); 498 | } 499 | 500 | void HesaiLidarSDK_internal::processPic() 501 | { 502 | while (continueProcessPic) 503 | { 504 | struct timespec ts; 505 | if (clock_gettime(CLOCK_REALTIME, &ts) == -1) 506 | { 507 | printf("processPic, get time error\n"); 508 | } 509 | 510 | ts.tv_sec += 1; 511 | if (sem_timedwait(&picSem, &ts) == -1) 512 | { 513 | continue; 514 | } 515 | 516 | pthread_mutex_lock(&picLock); 517 | PandoraPic *pic = pictureList.front(); 518 | pictureList.pop_front(); 519 | pthread_mutex_unlock(&picLock); 520 | 521 | if (pic == NULL) 522 | { 523 | printf("pic is NULL\n"); 524 | return; 525 | } 526 | 527 | struct tm t; 528 | t.tm_sec = pic->header.UTC_Time.UTC_Second; 529 | t.tm_min = pic->header.UTC_Time.UTC_Minute; 530 | t.tm_hour = pic->header.UTC_Time.UTC_Hour; 531 | t.tm_mday = pic->header.UTC_Time.UTC_Day; 532 | t.tm_mon = pic->header.UTC_Time.UTC_Month - 1; 533 | t.tm_year = pic->header.UTC_Time.UTC_Year + 2000 - 1900; 534 | t.tm_isdst = 0; 535 | double timestamp = mktime(&t) + pic->header.timestamp / 1000000.0; 536 | boost::shared_ptr cvMatPic(new cv::Mat()); 537 | switch (pic->header.pic_id) 538 | { 539 | case 0: 540 | yuv422ToCvmat(*cvMatPic, pic->yuv, pic->header.width, pic->header.height, 8); 541 | if (needRemapPicMat) 542 | remap(cvMatPic->clone(), *cvMatPic, mapxList[pic->header.pic_id], mapyList[pic->header.pic_id], INTER_LINEAR); 543 | break; 544 | case 1: 545 | case 2: 546 | case 3: 547 | case 4: 548 | unsigned char *bmp; 549 | unsigned long bmpSize; 550 | decompressJpeg((unsigned char *)pic->yuv, pic->header.len, bmp, bmpSize); 551 | 552 | *cvMatPic = cv::Mat(HesaiLidarSDK_IMAGE_HEIGHT, HesaiLidarSDK_IMAGE_WIDTH, CV_8UC3, bmp).clone(); 553 | if (needRemapPicMat) 554 | remap(cvMatPic->clone(), *cvMatPic, mapxList[pic->header.pic_id], mapyList[pic->header.pic_id], INTER_LINEAR); 555 | 556 | free(bmp); 557 | break; 558 | 559 | default: 560 | free(pic->yuv); 561 | free(pic); 562 | printf("wrong pic id\n"); 563 | return; 564 | } 565 | if (userCameraCallback) 566 | { 567 | // Add 2 seconds to correct the timestamp, Reason? No reason. liuxingwei@hesaitech.com 568 | userCameraCallback(cvMatPic, timestamp + 2, pic->header.pic_id); 569 | } 570 | free(pic->yuv); 571 | free(pic); 572 | pic->yuv = NULL; 573 | pic = NULL; 574 | } 575 | } 576 | 577 | bool HesaiLidarSDK_internal::loadIntrinsics(const std::string &intrinsicFile) 578 | { 579 | std::vector cameraKList; 580 | std::vector cameraDList; 581 | if (!loadCameraIntrinsics(intrinsicFile.c_str(), cameraKList, cameraDList)) 582 | return false; 583 | for (int i = 0; i < HesaiLidarSDK_CAMERA_NUM; i++) 584 | { 585 | Mat mapx = Mat(HesaiLidarSDK_IMAGE_SIZE, CV_32FC1); 586 | Mat mapy = Mat(HesaiLidarSDK_IMAGE_SIZE, CV_32FC1); 587 | Mat R = Mat::eye(3, 3, CV_32F); 588 | initUndistortRectifyMap(cameraKList[i], cameraDList[i], R, cameraKList[i], HesaiLidarSDK_IMAGE_SIZE, CV_32FC1, mapx, mapy); 589 | mapxList.push_back(mapx); 590 | mapyList.push_back(mapy); 591 | } 592 | return true; 593 | } 594 | 595 | 596 | 597 | 598 | #endif 599 | -------------------------------------------------------------------------------- /src/input.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include "input.h" 12 | 13 | namespace pandar_pointcloud 14 | { 15 | const int lidarPacketSize = ETHERNET_MTU; 16 | 17 | 18 | Input::Input(std::string filePath, int type) 19 | { 20 | if(type == 0) 21 | realLidarPacketSize = 1240; 22 | else if(type == 1) 23 | realLidarPacketSize = 1256; 24 | pcapFilePath = filePath; 25 | if ((pcap_ = pcap_open_offline(pcapFilePath.c_str(), pcapErrorBuffer) ) == NULL) 26 | { 27 | printf("read pcap file wrong\n"); 28 | } 29 | } 30 | 31 | 32 | Input::Input(uint16_t port, uint16_t gpsPort) 33 | { 34 | socketForLidar = -1; 35 | socketForLidar = socket(PF_INET, SOCK_DGRAM, 0); 36 | if (socketForLidar == -1) 37 | { 38 | perror("socket"); // TODO: perror errno 39 | return; 40 | } 41 | 42 | sockaddr_in myAddress; // my address information 43 | memset(&myAddress, 0, sizeof(myAddress)); // initialize to zeros 44 | myAddress.sin_family = AF_INET; // host byte order 45 | myAddress.sin_port = htons(port); // port in network byte order 46 | myAddress.sin_addr.s_addr = INADDR_ANY; // automatically fill in my IP 47 | 48 | if (bind(socketForLidar, (sockaddr *)&myAddress, sizeof(sockaddr)) == -1) 49 | { 50 | perror("bind"); // TODO: perror errno 51 | return; 52 | } 53 | 54 | if (fcntl(socketForLidar, F_SETFL, O_NONBLOCK | FASYNC) < 0) 55 | { 56 | perror("non-block"); 57 | return; 58 | } 59 | 60 | if (port == gpsPort) 61 | { 62 | socketNumber = 1; 63 | return; 64 | } 65 | //gps socket 66 | socketForGPS = -1; 67 | socketForGPS = socket(PF_INET, SOCK_DGRAM, 0); 68 | if (socketForGPS == -1) 69 | { 70 | perror("socket"); // TODO: perror errno 71 | return; 72 | } 73 | 74 | sockaddr_in myAddressGPS; // my address information 75 | memset(&myAddressGPS, 0, sizeof(myAddressGPS)); // initialize to zeros 76 | myAddressGPS.sin_family = AF_INET; // host byte order 77 | myAddressGPS.sin_port = htons(gpsPort); // port in network byte order 78 | myAddressGPS.sin_addr.s_addr = INADDR_ANY; // automatically fill in my IP 79 | 80 | if (bind(socketForGPS, (sockaddr *)&myAddressGPS, sizeof(sockaddr)) == -1) 81 | { 82 | perror("bind"); // TODO: perror errno 83 | return; 84 | } 85 | 86 | if (fcntl(socketForGPS, F_SETFL, O_NONBLOCK | FASYNC) < 0) 87 | { 88 | perror("non-block"); 89 | return; 90 | } 91 | socketNumber = 2; 92 | } 93 | 94 | Input::~Input(void) 95 | { 96 | if (socketForGPS > 0) 97 | close(socketForGPS); 98 | if (socketForLidar > 0) 99 | (void)close(socketForLidar); 100 | } 101 | 102 | // return : 0 - lidar 103 | // 1 - gps 104 | // -1 - error 105 | int Input::getPacket(PandarPacket *pkt) 106 | { 107 | gettimeofday(&getPacketStartTime, NULL); 108 | struct pollfd fds[socketNumber]; 109 | if (socketNumber == 2) 110 | { 111 | fds[0].fd = socketForGPS; 112 | fds[0].events = POLLIN; 113 | 114 | fds[1].fd = socketForLidar; 115 | fds[1].events = POLLIN; 116 | } 117 | else if (socketNumber == 1) 118 | { 119 | fds[0].fd = socketForLidar; 120 | fds[0].events = POLLIN; 121 | } 122 | static const int POLL_TIMEOUT = 1000; // one second (in msec) 123 | 124 | sockaddr_in senderAddress; 125 | socklen_t senderAddressLen = sizeof(senderAddress); 126 | int retval = poll(fds, socketNumber, POLL_TIMEOUT); 127 | if (retval < 0) // poll() error? 128 | { 129 | if (errno != EINTR) 130 | printf("poll() error: %s", strerror(errno)); 131 | return -1; 132 | } 133 | if (retval == 0) // poll() timeout? 134 | { 135 | return -1; 136 | } 137 | if ((fds[0].revents & POLLERR) || (fds[0].revents & POLLHUP) || (fds[0].revents & POLLNVAL)) // device error? 138 | { 139 | perror("poll() reports Pandar error"); 140 | return -1; 141 | } 142 | 143 | senderAddressLen = sizeof(senderAddress); 144 | ssize_t nbytes; 145 | for (int i = 0; i != socketNumber; ++i) 146 | { 147 | if (fds[i].revents & POLLIN) 148 | { 149 | nbytes = recvfrom(fds[i].fd, &pkt->data[0], 150 | lidarPacketSize, 0, 151 | (sockaddr *)&senderAddress, 152 | &senderAddressLen); 153 | break; 154 | } 155 | } 156 | 157 | if (nbytes < 0) 158 | { 159 | if (errno != EWOULDBLOCK) 160 | { 161 | perror("recvfail"); 162 | return -1; 163 | } 164 | } 165 | pkt->size = nbytes; 166 | 167 | gettimeofday(&getPacketStopTime, NULL); 168 | pkt->stamp = static_cast(getPacketStartTime.tv_sec + static_cast(getPacketStartTime.tv_usec) / 1000000 + getPacketStopTime.tv_sec + static_cast(getPacketStopTime.tv_usec) / 1000000) / 2; 169 | 170 | if ((size_t)nbytes == PANDORA_GPS_PACKET_SIZE) // gps 171 | return 1; 172 | 173 | return 0; 174 | } 175 | 176 | int Input::getPacketFromPcap(PandarPacket *pkt) 177 | { 178 | struct pcap_pkthdr *header; 179 | const u_char *pcapData; 180 | 181 | int ret = pcap_next_ex(pcap_, &header, &pcapData); 182 | 183 | if ( ret == 1) 184 | { 185 | if (header->caplen == (PANDORA_GPS_PACKET_SIZE + 42)) // gps packet 186 | { 187 | memcpy(&pkt->data[0], pcapData + 42, PANDORA_GPS_PACKET_SIZE); 188 | return 1; 189 | } 190 | 191 | else if (header->caplen == (realLidarPacketSize + 42)) 192 | { 193 | memcpy(&pkt->data[0], pcapData + 42, realLidarPacketSize); 194 | gettimeofday(&getPacketStartTime, NULL); 195 | pkt->stamp = getPacketStartTime.tv_sec + static_cast(getPacketStartTime.tv_usec) / 1000000; 196 | pkt->size = realLidarPacketSize; 197 | return 0; // success 198 | } 199 | return -1; 200 | } 201 | 202 | else if (ret == -2) // there are no more packets to read from the savefile, close and reopen file 203 | { 204 | pcap_close(pcap_); 205 | pcap_ = pcap_open_offline(pcapFilePath.c_str(), pcapErrorBuffer); 206 | } 207 | 208 | else if (ret == -1) // an error occurred while reading the packet 209 | { 210 | perror("read pcap file wrong: "); 211 | } 212 | } 213 | 214 | } 215 | -------------------------------------------------------------------------------- /src/rawdata.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "rawdata.h" 4 | 5 | namespace pandar_rawdata 6 | { 7 | 8 | static int pandarEnableListP16[40] = { 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 | 0, 26 | 0, 27 | 0, 28 | 0, 29 | 0, 30 | 0, 31 | 0, 32 | 0, 33 | 0, 34 | 0, 35 | 0, 36 | 0, 37 | 0, 38 | 0, 39 | 0, 40 | 0, 41 | 0, 42 | 0, 43 | 0, 44 | 0, 45 | 0, 46 | 0, 47 | 0, 48 | 0, 49 | }; 50 | 51 | 52 | static int pandarEnableListP20[40] = { 53 | 0, 54 | 1, 55 | 0, 56 | 1, 57 | 0, 58 | 1, 59 | 0, 60 | 1, 61 | 0, 62 | 1, 63 | 0, 64 | 1, 65 | 0, 66 | 1, 67 | 0, 68 | 1, 69 | 0, 70 | 1, 71 | 0, 72 | 1, 73 | 0, 74 | 1, 75 | 0, 76 | 1, 77 | 0, 78 | 1, 79 | 0, 80 | 1, 81 | 0, 82 | 1, 83 | 0, 84 | 1, 85 | 0, 86 | 1, 87 | 0, 88 | 1, 89 | 0, 90 | 1, 91 | 0, 92 | 1}; 93 | 94 | static int pandarEnableListP40[40] = { 95 | 1, 96 | 1, 97 | 1, 98 | 1, 99 | 1, 100 | 1, 101 | 1, 102 | 1, 103 | 1, 104 | 1, 105 | 1, 106 | 1, 107 | 1, 108 | 1, 109 | 1, 110 | 1, 111 | 1, 112 | 1, 113 | 1, 114 | 1, 115 | 1, 116 | 1, 117 | 1, 118 | 1, 119 | 1, 120 | 1, 121 | 1, 122 | 1, 123 | 1, 124 | 1, 125 | 1, 126 | 1, 127 | 1, 128 | 1, 129 | 1, 130 | 1, 131 | 1, 132 | 1, 133 | 1, 134 | 1}; 135 | 136 | static double blockOffset[BLOCKS_PER_PACKET]; 137 | static double laserOffset[MAX_LASER_COUNT]; 138 | 139 | static double blockOffsetDualVersion[MAX_LASER_COUNT]; 140 | 141 | RawData::RawData(const std::string &correctionFile, 142 | const unsigned int laserReturnType, 143 | const unsigned int laserNumber, 144 | const unsigned int pclType) 145 | { 146 | config_.calibrationFile = correctionFile; 147 | config_.minRange = PandoraSDK_MIN_RANGE; 148 | config_.maxRange = PandoraSDK_MAX_RANGE; 149 | 150 | pclDataType = pclType; 151 | 152 | laserCountType = laserNumber; 153 | switch (laserCountType) 154 | { 155 | case 16: 156 | for (int i = 0; i != LASER_COUNT; i++) 157 | pandarEnableList[i] = pandarEnableListP16[i]; 158 | break; 159 | case 20: 160 | for (int i = 0; i != LASER_COUNT; i++) 161 | pandarEnableList[i] = pandarEnableListP20[i]; 162 | break; 163 | case 40: 164 | for (int i = 0; i != LASER_COUNT; i++) 165 | pandarEnableList[i] = pandarEnableListP40[i]; 166 | break; 167 | } 168 | 169 | if (laserReturnType == 0) 170 | deviceType = deviceTypeSingleReturn; 171 | else if (laserReturnType == 1) 172 | deviceType = deviceTypeDualReturn; 173 | 174 | 175 | bufferPacket = new RAW_PACKET_T[1000]; 176 | 177 | bufferPacketSize = 0; 178 | lastBlockEnd = 0; 179 | lastTimestamp = 0; 180 | 181 | blockOffset[5] = 55.1f * 0.0 + 45.18f; 182 | blockOffset[4] = 55.1f * 1.0 + 45.18f; 183 | blockOffset[3] = 55.1f * 2.0 + 45.18f; 184 | blockOffset[2] = 55.1f * 3.0 + 45.18f; 185 | blockOffset[1] = 55.1f * 4.0 + 45.18f; 186 | blockOffset[0] = 55.1f * 5.0 + 45.18f; 187 | 188 | blockOffsetDualVersion[9] = 55.1f * 0.0 + 45.18f; 189 | blockOffsetDualVersion[8] = 55.1f * 1.0 + 45.18f; 190 | blockOffsetDualVersion[7] = 55.1f * 2.0 + 45.18f; 191 | blockOffsetDualVersion[6] = 55.1f * 3.0 + 45.18f; 192 | blockOffsetDualVersion[5] = 55.1f * 4.0 + 45.18f; 193 | blockOffsetDualVersion[4] = 55.1f * 5.0 + 45.18f; 194 | blockOffsetDualVersion[3] = 55.1f * 6.0 + 45.18f; 195 | blockOffsetDualVersion[2] = 55.1f * 7.0 + 45.18f; 196 | blockOffsetDualVersion[1] = 55.1f * 8.0 + 45.18f; 197 | blockOffsetDualVersion[0] = 55.1f * 9.0 + 45.18f; 198 | 199 | laserOffset[3] = 0.93f * 1.0f; 200 | laserOffset[35] = 0.93f * 2.0f; 201 | laserOffset[39] = 0.93f * 3.0f; 202 | laserOffset[23] = 0.93f * 3.0f + 1.6f * 1.0f; 203 | laserOffset[16] = 0.93f * 3.0f + 1.6f * 2.0f; 204 | laserOffset[27] = 0.93f * 4.0f + 1.6f * 2.0f; 205 | laserOffset[11] = 0.93f * 4.0f + 1.6f * 3.0f; 206 | laserOffset[31] = 0.93f * 5.0f + 1.6f * 3.0f; 207 | laserOffset[28] = 0.93f * 6.0f + 1.6f * 3.0f; 208 | laserOffset[15] = 0.93f * 6.0f + 1.6f * 4.0f; 209 | laserOffset[2] = 0.93f * 7.0f + 1.6f * 4.0f; 210 | laserOffset[34] = 0.93f * 8.0f + 1.6f * 4.0f; 211 | laserOffset[38] = 0.93f * 9.0f + 1.6f * 4.0f; 212 | laserOffset[20] = 0.93f * 9.0f + 1.6f * 5.0f; 213 | laserOffset[13] = 0.93f * 9.0f + 1.6f * 6.0f; 214 | laserOffset[24] = 0.93f * 9.0f + 1.6f * 7.0f; 215 | laserOffset[8] = 0.93f * 9.0f + 1.6f * 8.0f; 216 | laserOffset[30] = 0.93f * 10.0f + 1.6f * 8.0f; 217 | laserOffset[25] = 0.93f * 11.0f + 1.6f * 8.0f; 218 | laserOffset[12] = 0.93f * 11.0f + 1.6f * 9.0f; 219 | laserOffset[1] = 0.93f * 12.0f + 1.6f * 9.0f; 220 | laserOffset[33] = 0.93f * 13.0f + 1.6f * 9.0f; 221 | laserOffset[37] = 0.93f * 14.0f + 1.6f * 9.0f; 222 | laserOffset[17] = 0.93f * 14.0f + 1.6f * 10.0f; 223 | laserOffset[10] = 0.93f * 14.0f + 1.6f * 11.0f; 224 | laserOffset[21] = 0.93f * 14.0f + 1.6f * 12.0f; 225 | laserOffset[5] = 0.93f * 14.0f + 1.6f * 13.0f; 226 | laserOffset[29] = 0.93f * 15.0f + 1.6f * 13.0f; 227 | laserOffset[22] = 0.93f * 15.0f + 1.6f * 14.0f; 228 | laserOffset[9] = 0.93f * 15.0f + 1.6f * 15.0f; 229 | laserOffset[0] = 0.93f * 16.0f + 1.6f * 15.0f; 230 | laserOffset[32] = 0.93f * 17.0f + 1.6f * 15.0f; 231 | laserOffset[36] = 0.93f * 18.0f + 1.6f * 15.0f; 232 | laserOffset[14] = 0.93f * 18.0f + 1.6f * 16.0f; 233 | laserOffset[7] = 0.93f * 18.0f + 1.6f * 17.0f; 234 | laserOffset[18] = 0.93f * 18.0f + 1.6f * 18.0f; 235 | laserOffset[4] = 0.93f * 19.0f + 1.6f * 18.0f; 236 | laserOffset[26] = 0.93f * 20.0f + 1.6f * 18.0f; 237 | laserOffset[19] = 0.93f * 20.0f + 1.6f * 19.0f; 238 | laserOffset[6] = 0.93f * 20.0f + 1.6f * 20.0f; 239 | setup(); 240 | } 241 | 242 | int RawData::setup() 243 | { 244 | calibration_.read(config_.calibrationFile); 245 | if (!calibration_.initialized) 246 | { 247 | printf("Unable to open calibration file: %s", config_.calibrationFile.c_str()); 248 | return -1; 249 | } 250 | 251 | for (uint16_t rotIndex = 0; rotIndex < ROTATION_MAX_UNITS; ++rotIndex) 252 | { 253 | float rotation = angles::degreeToRadian(ROTATION_RESOLUTION * rotIndex); 254 | cos_lookup_table_[rotIndex] = cosf(rotation); 255 | sin_lookup_table_[rotIndex] = sinf(rotation); 256 | } 257 | return 0; 258 | } 259 | 260 | int RawData::parseRawData(RAW_PACKET_T *packet, const uint8_t *buf, const int len) 261 | { 262 | 263 | if(deviceType == deviceTypeSingleReturn) 264 | { 265 | if (len != PACKET_SIZE) 266 | { 267 | printf("packet size mismatch!\n"); 268 | return -1; 269 | } 270 | 271 | int index = 0; 272 | // 6 BLOCKs 273 | for (int i = 0; i < BLOCKS_PER_PACKET; i++) 274 | { 275 | RAW_BLOCK_T &block = packet->blocks[i]; 276 | // block.laserCount = laserCount; 277 | block.sob = (buf[index] & 0xff) | ((buf[index + 1] & 0xff) << 8); 278 | block.azimuth = (buf[index + 2] & 0xff) | ((buf[index + 3] & 0xff) << 8); 279 | index += SOB_ANGLE_SIZE; 280 | // 40x measures 281 | for (int j = 0; j < LASER_COUNT; j++) 282 | { 283 | RAW_MEASURE_T &measure = block.measures[j]; 284 | unsigned int range = (buf[index] & 0xff) | ((buf[index + 1] & 0xff) << 8) | ((buf[index + 2] & 0xff) << 16); 285 | measure.range = ((double)range) * LASER_RETURN_TO_DISTANCE_RATE; 286 | // printf("%d, %d, %d\n", buf[index], buf[index + 1], buf[index + 2]); 287 | // printf("parseRawData measure.range: %d, %d\n", j, measure.range); 288 | measure.reflectivity = (buf[index + 3] & 0xff) | ((buf[index + 4] & 0xff) << 8); 289 | // TODO: Filtering wrong data for LiDAR Bugs. 290 | if ((measure.range == 0x010101 && measure.reflectivity == 0x0101) || measure.range > (200 * 1000 / 2 /* 200m -> 2mm */)) 291 | { 292 | measure.range = 0; 293 | measure.reflectivity = 0; 294 | } 295 | index += RAW_MEASURE_SIZE; 296 | } 297 | } 298 | index += RESERVE_SIZE; // skip reserved bytes 299 | 300 | index += REVOLUTION_SIZE; 301 | 302 | packet->timestamp = (buf[index] & 0xff) | (buf[index + 1] & 0xff) << 8 | 303 | ((buf[index + 2] & 0xff) << 16) | ((buf[index + 3] & 0xff) << 24); 304 | // printf("packet timestamp: %ld\n", packet->timestamp); 305 | 306 | packet->blockAmount = BLOCKS_PER_PACKET; 307 | } 308 | else if( deviceType == deviceTypeDualReturn) 309 | { 310 | if (len != DUAL_VERSION_PACKET_SIZE) 311 | { 312 | printf("packet size mismatch dual, %d, %d\n", len, DUAL_VERSION_PACKET_SIZE); 313 | return -1; 314 | } 315 | 316 | int index = 0; 317 | // 10 BLOCKs 318 | for (int i = 0; i < DUAL_VERSION_BLOCKS_PER_PACKET; i++) 319 | { 320 | RAW_BLOCK_T &block = packet->blocks[i]; 321 | 322 | // block.laserCount = laserCount; 323 | block.sob = (buf[index] & 0xff) | ((buf[index + 1] & 0xff) << 8); 324 | block.azimuth = (buf[index + 2] & 0xff) | ((buf[index + 3] & 0xff) << 8); 325 | index += DUAL_VERSION_SOB_ANGLE_SIZE; 326 | // 40x measures 327 | for (int j = 0; j < LASER_COUNT; j++) 328 | { 329 | RAW_MEASURE_T &measure = block.measures[j]; 330 | unsigned int range = (buf[index] & 0xff) | ((buf[index + 1] & 0xff) << 8); 331 | 332 | measure.range = ((double)range) * DUAL_VERSION_LASER_RETURN_TO_DISTANCE_RATE; 333 | // printf("%d, %d, %d\n", buf[index], buf[index + 1], buf[index + 2]); 334 | // printf("parseRawData measure.range: %d, %d\n", j, measure.range); 335 | measure.reflectivity = (buf[index + 2] & 0xff); 336 | // printf("intensity: %d\n", measure.reflectivity); 337 | 338 | // TODO: Filtering wrong data for LiDAR Bugs. 339 | if ((measure.range == 0x010101 && measure.reflectivity == 0x0101) || measure.range > (200 * 1000 / 2 /* 200m -> 2mm */)) 340 | { 341 | measure.range = 0; 342 | measure.reflectivity = 0; 343 | } 344 | index += DUAL_VERSION_RAW_MEASURE_SIZE; 345 | } 346 | } 347 | index += DUAL_VERSION_RESERVE_SIZE; // skip reserved bytes 348 | 349 | index += DUAL_VERSION_REVOLUTION_SIZE; 350 | 351 | packet->timestamp = (buf[index] & 0xff) | (buf[index + 1] & 0xff) << 8 | 352 | ((buf[index + 2] & 0xff) << 16) | ((buf[index + 3] & 0xff) << 24); 353 | 354 | index += DUAL_VERSION_TIMESTAMP_SIZE; 355 | packet->echo = buf[index] & 0xff; 356 | packet->blockAmount = DUAL_VERSION_BLOCKS_PER_PACKET; 357 | } 358 | return 0; 359 | } 360 | 361 | // int RawData::parseRawData(RAW_PACKET_DUAL_T *packet, const uint8_t *buf, const int len) 362 | // { 363 | // if (len != DUAL_VERSION_PACKET_SIZE) 364 | // { 365 | // printf("packet size mismatch dual, %d, %d\n", len, DUAL_VERSION_PACKET_SIZE); 366 | // return -1; 367 | // } 368 | 369 | // int index = 0; 370 | // // 10 BLOCKs 371 | // for (int i = 0; i < DUAL_VERSION_BLOCKS_PER_PACKET; i++) 372 | // { 373 | // RAW_BLOCK_DUAL_T &block = packet->blocks[i]; 374 | // block.sob = (buf[index] & 0xff) | ((buf[index + 1] & 0xff) << 8); 375 | // block.azimuth = (buf[index + 2] & 0xff) | ((buf[index + 3] & 0xff) << 8); 376 | // index += DUAL_VERSION_SOB_ANGLE_SIZE; 377 | // // 40x measures 378 | // for (int j = 0; j < DUAL_VERSION_LASER_COUNT; j++) 379 | // { 380 | // RAW_MEASURE_DUAL_T &measure = block.measures[j]; 381 | // measure.range = (buf[index] & 0xff) | ((buf[index + 1] & 0xff) << 8); 382 | // // printf("%d, %d, %d\n", buf[index], buf[index + 1], buf[index + 2]); 383 | // // printf("parseRawData measure.range: %d, %d\n", j, measure.range); 384 | // measure.reflectivity = (buf[index + 2] & 0xff); 385 | 386 | // // TODO: Filtering wrong data for LiDAR Bugs. 387 | // if ((measure.range == 0x010101 && measure.reflectivity == 0x0101) || measure.range > (200 * 1000 / 2 /* 200m -> 2mm */)) 388 | // { 389 | // measure.range = 0; 390 | // measure.reflectivity = 0; 391 | // } 392 | // index += DUAL_VERSION_RAW_MEASURE_SIZE; 393 | // } 394 | // } 395 | // index += DUAL_VERSION_RESERVE_SIZE; // skip reserved bytes 396 | 397 | // packet->revolution = (buf[index] & 0xff) | (buf[index + 1] & 0xff) << 8; 398 | // index += DUAL_VERSION_REVOLUTION_SIZE; 399 | 400 | // packet->timestamp = (buf[index] & 0xff) | (buf[index + 1] & 0xff) << 8 | 401 | // ((buf[index + 2] & 0xff) << 16) | ((buf[index + 3] & 0xff) << 24); 402 | // index += DUAL_VERSION_TIMESTAMP_SIZE; 403 | // packet->echo = buf[index] & 0xff; 404 | // packet->factory = buf[index + 1] & 0xff; 405 | // return 0; 406 | // } 407 | 408 | void RawData::computeXYZIR( 409 | PPoint &point, 410 | int azimuth, 411 | const RAW_MEASURE_T &laserReturn, 412 | const pandar_pointcloud::PandarLaserCorrection &correction) 413 | { 414 | double cos_azimuth, sin_azimuth; 415 | double distanceM = laserReturn.range; 416 | 417 | if(deviceType == deviceTypeSingleReturn) 418 | point.intensity = static_cast(laserReturn.reflectivity >> 8); 419 | else if(deviceType == deviceTypeDualReturn) 420 | point.intensity = static_cast(laserReturn.reflectivity); 421 | if (distanceM < config_.minRange || distanceM > config_.maxRange) 422 | { 423 | point.x = point.y = point.z = std::numeric_limits::quiet_NaN(); 424 | return; 425 | } 426 | if (correction.azimuthCorrection == 0) 427 | { 428 | cos_azimuth = cos_lookup_table_[azimuth]; 429 | sin_azimuth = sin_lookup_table_[azimuth]; 430 | } 431 | else 432 | { 433 | double azimuthInRadians = angles::degreeToRadian((static_cast(azimuth) / 100.0) + correction.azimuthCorrection); 434 | cos_azimuth = std::cos(azimuthInRadians); 435 | sin_azimuth = std::sin(azimuthInRadians); 436 | } 437 | 438 | distanceM += correction.distanceCorrection; 439 | double xyDistance = distanceM * correction.cosVertCorrection; 440 | 441 | point.x = static_cast(xyDistance * sin_azimuth - correction.horizontalOffsetCorrection * cos_azimuth); 442 | point.y = static_cast(xyDistance * cos_azimuth + correction.horizontalOffsetCorrection * sin_azimuth); 443 | point.z = static_cast(distanceM * correction.sinVertCorrection + correction.verticalOffsetCorrection); 444 | 445 | if (point.x == 0 && point.y == 0 && point.z == 0) 446 | { 447 | point.x = point.y = point.z = std::numeric_limits::quiet_NaN(); 448 | } 449 | } 450 | 451 | // void RawData::computeXYZIR( 452 | // PPoint &point, 453 | // int azimuth, 454 | // const RAW_MEASURE_DUAL_T &laserReturn, 455 | // const pandar_pointcloud::PandarLaserCorrection &correction) 456 | // { 457 | // double cos_azimuth, sin_azimuth; 458 | // double distanceM = laserReturn.range * DUAL_VERSION_LASER_RETURN_TO_DISTANCE_RATE; 459 | 460 | // point.intensity = static_cast(laserReturn.reflectivity >> 8); 461 | // if (distanceM < config_.minRange || distanceM > config_.maxRange) 462 | // { 463 | // point.x = point.y = point.z = std::numeric_limits::quiet_NaN(); 464 | // return; 465 | // } 466 | // if (correction.azimuthCorrection == 0) 467 | // { 468 | // cos_azimuth = cos_lookup_table_[azimuth]; 469 | // sin_azimuth = sin_lookup_table_[azimuth]; 470 | // } 471 | // else 472 | // { 473 | // double azimuthInRadians = angles::degreeToRadian((static_cast(azimuth) / 100.0) + correction.azimuthCorrection); 474 | // cos_azimuth = std::cos(azimuthInRadians); 475 | // sin_azimuth = std::sin(azimuthInRadians); 476 | // } 477 | 478 | // distanceM += correction.distanceCorrection; 479 | // double xyDistance = distanceM * correction.cosVertCorrection; 480 | 481 | // point.x = static_cast(xyDistance * sin_azimuth - correction.horizontalOffsetCorrection * cos_azimuth); 482 | // point.y = static_cast(xyDistance * cos_azimuth + correction.horizontalOffsetCorrection * sin_azimuth); 483 | // point.z = static_cast(distanceM * correction.sinVertCorrection + correction.verticalOffsetCorrection); 484 | 485 | // if (point.x == 0 && point.y == 0 && point.z == 0) 486 | // { 487 | // point.x = point.y = point.z = std::numeric_limits::quiet_NaN(); 488 | // } 489 | // } 490 | 491 | void RawData::toPointClouds( 492 | RAW_PACKET_T *packet, 493 | int block, PPointCloud &pc, 494 | double stamp) 495 | { 496 | const RAW_BLOCK_T &firingData = packet->blocks[block]; 497 | for (int i = 0; i < LASER_COUNT; i++) 498 | { 499 | if(pandarEnableList[i] != 1) 500 | continue; 501 | PPoint xyzir; 502 | computeXYZIR(xyzir, firingData.azimuth, 503 | firingData.measures[i], calibration_.laserCorrections[i]); 504 | if (pcl_isnan(xyzir.x) || pcl_isnan(xyzir.y) || pcl_isnan(xyzir.z)) 505 | { 506 | continue; 507 | } 508 | 509 | if(deviceType == deviceTypeSingleReturn) 510 | xyzir.timestamp = stamp - ((double)(blockOffset[block] + laserOffset[i]) / 1000000.0f); 511 | else if(deviceType == deviceTypeDualReturn) 512 | { 513 | if(packet->echo == 0x39) 514 | xyzir.timestamp = stamp - ((double)(blockOffsetDualVersion[block / 2] + laserOffset[i / 2]) / 1000000.0f); 515 | else if(packet->echo == 0x37 || packet->echo == 0x38) 516 | xyzir.timestamp = stamp - ((double)(blockOffsetDualVersion[block] + laserOffset[i]) / 1000000.0f); 517 | 518 | } 519 | xyzir.ring = i; 520 | pc.points.push_back(xyzir); 521 | pc.width++; 522 | } 523 | } 524 | 525 | void RawData::toPointClouds( 526 | RAW_PACKET_T *packet, 527 | int laser, int block, 528 | PPointCloud &pc, 529 | double blockstamp) 530 | { 531 | int i = block; 532 | { 533 | const RAW_BLOCK_T &firing_data = packet->blocks[i]; 534 | PPoint xyzir; 535 | computeXYZIR(xyzir, firing_data.azimuth, 536 | firing_data.measures[laser], calibration_.laserCorrections[laser]); 537 | 538 | if(deviceType == deviceTypeSingleReturn) 539 | xyzir.timestamp = blockstamp - ((double)(blockOffset[block] + laserOffset[i]) / 1000000.0f); 540 | else if(deviceType == deviceTypeDualReturn) 541 | { 542 | if(packet->echo == 0x39) 543 | xyzir.timestamp = blockstamp - ((double)(blockOffsetDualVersion[block / 2] + laserOffset[i / 2]) / 1000000.0f); 544 | else if(packet->echo == 0x37 || packet->echo == 0x37) 545 | xyzir.timestamp = blockstamp - ((double)(blockOffsetDualVersion[block] + laserOffset[i]) / 1000000.0f); 546 | 547 | } 548 | // if (pcl_isnan (xyzir.x) || pcl_isnan (xyzir.y) || pcl_isnan (xyzir.z)) 549 | // { 550 | // return; 551 | // } 552 | // xyzir.ring = laser; 553 | // xyzir.timestamp = blockstamp - ((blockOffset[block] + laserOffset[laser]) / 1000000); 554 | pc.points.push_back(xyzir); 555 | pc.width++; 556 | } 557 | } 558 | 559 | 560 | int RawData::unpack( 561 | PandarPacket &packet, 562 | PPointCloud &pc, 563 | time_t &gps1, 564 | GPS_STRUCT_T &gps2, 565 | int &lidarRotationStartAngle) 566 | { 567 | currentPacketStart = bufferPacketSize == 0 ? 0 : bufferPacketSize - 1; 568 | parseRawData(&bufferPacket[bufferPacketSize++], &packet.data[0], packet.size); 569 | 570 | int hasAframe = 0; 571 | int currentBlockEnd = 0; 572 | int currentPacketEnd = 0; 573 | if (bufferPacketSize > 1) 574 | { 575 | int lastAzumith = -1; 576 | for (int i = currentPacketStart; i < bufferPacketSize; i++) 577 | { 578 | if (hasAframe) 579 | { 580 | break; 581 | } 582 | 583 | int j = 0; 584 | if (i == currentPacketStart) 585 | { 586 | j = lastBlockEnd; 587 | } 588 | else 589 | { 590 | j = 0; 591 | } 592 | for (; j < bufferPacket[i].blockAmount; ++j) 593 | { 594 | if (lastAzumith == -1) 595 | { 596 | lastAzumith = bufferPacket[i].blocks[j].azimuth; 597 | continue; 598 | } 599 | 600 | int azimuthGap = 0; /* To do */ 601 | if(lastAzumith > bufferPacket[i].blocks[j].azimuth) { 602 | azimuthGap = static_cast(bufferPacket[i].blocks[j].azimuth) + (36000 - static_cast(lastAzumith)); 603 | } else { 604 | azimuthGap = static_cast(bufferPacket[i].blocks[j].azimuth) - static_cast(lastAzumith); 605 | } 606 | 607 | if (lastAzumith != bufferPacket[i].blocks[j].azimuth && 608 | azimuthGap < 600 /* 6 degree*/) { 609 | /* for all the blocks */ 610 | if ((lastAzumith > bufferPacket[i].blocks[j].azimuth && 611 | lidarRotationStartAngle <= bufferPacket[i].blocks[j].azimuth) || 612 | (lastAzumith < lidarRotationStartAngle && 613 | lidarRotationStartAngle <= bufferPacket[i].blocks[j].azimuth)) { 614 | currentBlockEnd = j; 615 | hasAframe = 1; 616 | currentPacketEnd = i; 617 | break; 618 | } 619 | } 620 | 621 | lastAzumith = bufferPacket[i].blocks[j].azimuth; 622 | } 623 | } 624 | } 625 | 626 | if (hasAframe) 627 | { 628 | if (pclDataType == 0) 629 | { 630 | int j = 0; 631 | for (int k = 0; k < (currentPacketEnd + 1); ++k) 632 | { 633 | if (k == 0) 634 | j = lastBlockEnd; 635 | else 636 | j = 0; 637 | 638 | if(deviceType == deviceTypeSingleReturn) 639 | { 640 | // if > 500ms 641 | if (bufferPacket[k].timestamp < 500000 && gps2.used == 0) 642 | { 643 | if (gps1 > gps2.gps) 644 | { 645 | printf("Oops , You give me a wrong gps timestamp I think... ,%ld , %ld\n", gps1, gps2.gps); 646 | } 647 | gps1 = gps2.gps; 648 | gps2.used = 1; 649 | } 650 | else 651 | { 652 | if (bufferPacket[k].timestamp < lastTimestamp) 653 | { 654 | unsigned int gap = lastTimestamp - bufferPacket[k].timestamp; 655 | // avoid the fake jump... wrong udp order 656 | if (gap > (10 * 1000)) // 10ms 657 | { 658 | // Oh , there is a round. But gps2 is not changed , So there is no gps packet!!! 659 | // We need to add the offset. 660 | 661 | gps1 += ((lastTimestamp - 20) / 1000000) + 1; // 20us offset , avoid the timestamp of 1000002... 662 | #ifdef DEBUG 663 | printf("There is a round , But gps packet!!! , Change gps1 by manual!!! %lu %lu %lu\n", gps1, lastTimestamp, bufferPacket[k].timestamp); 664 | #endif 665 | } 666 | } 667 | } 668 | } 669 | else if (deviceType == deviceTypeDualReturn) 670 | { 671 | // if > 500ms 672 | if (((bufferPacket[k].timestamp < 500000) && gps2.usedHour == 0)|| 673 | (gps1 == 0 && gps2.used == 0)) 674 | { 675 | gps1 = mktime(&gps2.t) + 1; 676 | gps2.usedHour = 1; 677 | } 678 | else 679 | { 680 | if (bufferPacket[k].timestamp < lastTimestamp) 681 | { 682 | int gap = lastTimestamp - bufferPacket[k].timestamp; 683 | // avoid the fake jump... wrong udp order 684 | if (gap > (10 * 1000)) // 10ms 685 | { 686 | // Oh , there is a round. But gps2 is not changed , So there is no gps packet!!! 687 | // We need to add the offset. 688 | 689 | gps1 += (60*60); // 20us offset , avoid the timestamp of 1000002... 690 | #ifdef DEBUG 691 | printf("There is a round , But gps packet!!! , Change gps1 by manual!!! %lu %lu %lu\n", gps1, lastTimestamp, bufferPacket[k].timestamp); 692 | #endif 693 | } 694 | } 695 | } 696 | } 697 | for (; j < bufferPacket[k].blockAmount; ++j) 698 | { 699 | /* code */ 700 | if (currentBlockEnd == j && k == (currentPacketEnd)) 701 | { 702 | break; 703 | } 704 | toPointClouds(&bufferPacket[k], j, pc, (double)gps1 + (((double)bufferPacket[k].timestamp) / 1000000)); 705 | } 706 | lastTimestamp = bufferPacket[k].timestamp; 707 | } 708 | } 709 | 710 | else if (pclDataType == 1) 711 | { 712 | int first = 1; 713 | std::vector blockTimestamp; 714 | 715 | for (int i = 0; i < LASER_COUNT; i++) 716 | { 717 | if (pandarEnableList[i] == 1) 718 | { 719 | int j = 0; 720 | for (int k = 0; k < (currentPacketEnd + 1); ++k) 721 | { 722 | if (first) 723 | { 724 | if(deviceType == deviceTypeSingleReturn) 725 | { 726 | // if > 500ms 727 | if (bufferPacket[k].timestamp < 500000 && gps2.used == 0) 728 | { 729 | gps1 = gps2.gps; 730 | gps2.used = 1; 731 | } 732 | else 733 | { 734 | if (bufferPacket[k].timestamp < lastTimestamp) 735 | { 736 | // Oh , there is a round. But gps2 is not changed , So there is no gps packet!!! 737 | // We need to add the offset. 738 | // ROS_ERROR("There is a round , But gps packet!!! , Change gps1 by manual!!!"); 739 | gps1 += ((lastTimestamp - 100) / 1000000) + 1; 740 | } 741 | } 742 | 743 | lastTimestamp = bufferPacket[k].timestamp; 744 | 745 | // build the block stamps 746 | blockTimestamp.push_back((double)gps1 + (((double)bufferPacket[k].timestamp) / 1000000)); 747 | } 748 | else if( deviceType == deviceTypeDualReturn) 749 | { 750 | // if > 500ms 751 | if (((bufferPacket[k].timestamp < 500000) && gps2.usedHour == 0)|| 752 | (gps1 == 0 && gps2.used == 0)) 753 | { 754 | gps1 = mktime(&gps2.t); 755 | gps2.usedHour = 1; 756 | } 757 | else 758 | { 759 | if (bufferPacket[k].timestamp < lastTimestamp) 760 | { 761 | unsigned int gap = lastTimestamp - bufferPacket[k].timestamp; 762 | // avoid the fake jump... wrong udp order 763 | if (gap > (10 * 1000)) // 10ms 764 | { 765 | // Oh , there is a round. But gps2 is not changed , So there is no gps packet!!! 766 | // We need to add the offset. 767 | 768 | gps1 += (60*60); // 20us offset , avoid the timestamp of 1000002... 769 | #ifdef DEBUG 770 | printf("There is a round , But gps packet!!! , Change gps1 by manual!!! %lu %lu %lu\n", gps1, lastTimestamp, bufferPacket[k].timestamp); 771 | #endif 772 | } 773 | } 774 | } 775 | 776 | lastTimestamp = bufferPacket[k].timestamp; 777 | 778 | // build the block stamps 779 | blockTimestamp.push_back((double)gps1 + (((double)bufferPacket[k].timestamp) / 1000000)); 780 | } 781 | } 782 | 783 | if (k == 0) 784 | j = lastBlockEnd; 785 | else 786 | j = 0; 787 | 788 | for (; j < bufferPacket[k].blockAmount; ++j) 789 | { 790 | /* code */ 791 | if (currentBlockEnd == j && k == (currentPacketEnd)) 792 | { 793 | break; 794 | } 795 | toPointClouds(&bufferPacket[k], i, j, pc, blockTimestamp[k]); 796 | } 797 | } 798 | first = 0; 799 | } 800 | } 801 | } 802 | 803 | memcpy(&bufferPacket[0], &bufferPacket[currentPacketEnd], sizeof(RAW_PACKET_T) * (bufferPacketSize - currentPacketEnd)); 804 | bufferPacketSize = bufferPacketSize - currentPacketEnd; 805 | lastBlockEnd = currentBlockEnd; 806 | return 1; 807 | } 808 | else 809 | { 810 | return 0; 811 | } 812 | } 813 | 814 | } // namespace pandar_rawdata 815 | -------------------------------------------------------------------------------- /src/test.cc: -------------------------------------------------------------------------------- 1 | #include "hesaiLidarSDK.h" 2 | 3 | int imageNo = 0; 4 | unsigned long imageNoForSave = 0; 5 | unsigned int modnum = 0; 6 | int lidarNo = 0; 7 | unsigned long lidarNoForSave = 0; 8 | FILE* cameraTimestampFile = fopen("camera-timestamp.txt", "w"); 9 | FILE* lidarTimestampFile = fopen("lidar-timestamp.txt", "w"); 10 | FILE* gpsTimestampFile = fopen("gps-timestamp.txt", "w"); 11 | 12 | double pandoraToSysTimeGap = 0; 13 | int gpsTimestamp = 0; 14 | #ifdef HESAI_WITH_CAMERA 15 | void cameraCallback(boost::shared_ptr matp, double timestamp, int pic_id) 16 | { 17 | // Mat myMat = imread("t.png"); 18 | // imshow("camera", myMat); 19 | // std::cout< 50 && modnum < 4) 21 | // { 22 | // ++modnum; 23 | // cv::imwrite(boost::to_string(++imageNoForSave) + "-" + boost::to_string(pic_id) + ".jpg", *matp); 24 | // if (modnum == 4) 25 | // { 26 | // imageNo = 0; 27 | // modnum = 0; 28 | // } 29 | // } 30 | // if(pic_id == 0) 31 | // { 32 | // cv::imwrite(boost::to_string(++imageNoForSave) + "-" + boost::to_string(pic_id) + ".bmp", *matp); 33 | // } 34 | struct timeval ts; 35 | gettimeofday(&ts, NULL); 36 | // fprintf(cameraTimestampFile, "%d,%f\n", pic_id, ts.tv_sec + (double)ts.tv_usec / 1000000 - pandoraToSysTimeGap - timestamp); 37 | fprintf(cameraTimestampFile, "%d,%lf,%lf\n", pic_id, timestamp, (double)ts.tv_sec + (double)ts.tv_usec / 1000000 - pandoraToSysTimeGap - timestamp); 38 | 39 | printf("cameraid: %d, timestamp: %lf\n", pic_id, timestamp); 40 | } 41 | 42 | void cameraCallbackForDelay(boost::shared_ptr matp, double timestamp, int pic_id) 43 | { 44 | struct timeval ts; 45 | gettimeofday(&ts, NULL); 46 | fprintf(cameraTimestampFile, "%d,%f\n", pic_id, ts.tv_sec + (double)ts.tv_usec / 1000000 - pandoraToSysTimeGap - timestamp); 47 | } 48 | #endif 49 | 50 | 51 | void gpsCallback(int timestamp) 52 | { 53 | struct timeval ts; 54 | gettimeofday(&ts, NULL); 55 | gpsTimestamp = timestamp; 56 | pandoraToSysTimeGap = (double)ts.tv_sec + ( (double)ts.tv_usec / 1000000.0 ) - (double)timestamp; 57 | printf("gps: %d, gap: %f\n", timestamp, pandoraToSysTimeGap); 58 | fprintf(gpsTimestampFile, "%d, %f, %f\n", timestamp, ts.tv_sec + ts.tv_usec / 1000000.0, pandoraToSysTimeGap); 59 | } 60 | 61 | 62 | 63 | void lidarCallback(boost::shared_ptr cld, double timestamp) 64 | { 65 | struct timeval ts; 66 | gettimeofday(&ts, NULL); 67 | fprintf(lidarTimestampFile, "%d, %f,%f\n", gpsTimestamp, timestamp, ts.tv_sec + (double)ts.tv_usec / 1000000 - pandoraToSysTimeGap - timestamp); 68 | printf("lidar: %lf\n", timestamp); 69 | } 70 | 71 | int main(int argc, char **argv) 72 | { 73 | #ifdef HESAI_WITH_CAMERA 74 | HesaiLidarSDK psdk( 75 | std::string("192.168.20.51"), 76 | 9870, 77 | std::string("calibration.yml"), 78 | cameraCallback, 79 | 2368, 10110, 0, 80 | std::string("correction.csv"), 81 | lidarCallback, gpsCallback, 82 | HESAI_LIDAR_RAW_DATA_STRCUT_DUAL_RETURN, 40, HESAI_LIDAR_PCL_DATA_TYPE_REDUCED); 83 | #else 84 | HesaiLidarSDK psdk(2368, 10110, 0, std::string("correction.csv"), lidarCallback, gpsCallback, 85 | HESAI_LIDAR_RAW_DATA_STRCUT_DUAL_RETURN, 40, HESAI_LIDAR_PCL_DATA_TYPE_REDUCED); 86 | #endif 87 | psdk.start(); 88 | while(true) 89 | { 90 | sleep(100); 91 | } 92 | } 93 | -------------------------------------------------------------------------------- /src/utilities.cc: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include "yaml-cpp/yaml.h" 8 | #include 9 | #include "utilities.h" 10 | 11 | 12 | void yuv400ToCvmat(cv::Mat &dst, void *pYUV400, int nWidth, int nHeight, int bitDepth) 13 | { 14 | IplImage *imgHeaderP; 15 | 16 | if (!pYUV400) 17 | { 18 | return; 19 | } 20 | 21 | if (bitDepth == 8) 22 | { 23 | imgHeaderP = cvCreateImageHeader(cvSize(nWidth, nHeight), IPL_DEPTH_8U, 1); 24 | } 25 | else 26 | { 27 | imgHeaderP = cvCreateImageHeader(cvSize(nWidth, nHeight), IPL_DEPTH_16U, 1); 28 | } 29 | 30 | cvSetData(imgHeaderP, (unsigned char *)pYUV400, nWidth); 31 | dst = cv::cvarrToMat(imgHeaderP); 32 | cvReleaseImageHeader(&imgHeaderP); 33 | } 34 | 35 | void yuvToRgb(const int &iY, const int &iU, const int &iV, int &iR, int &iG, int &iB) 36 | { 37 | assert(&iR != NULL && &iG != NULL && &iB != NULL); 38 | 39 | iR = iY + 1.13983 * (iV - 128); 40 | iG = iY - 0.39465 * (iU - 128) - 0.58060 * (iV - 128); 41 | iB = iY + 2.03211 * (iU - 128); 42 | 43 | iR = iR > 255 ? 255 : iR; 44 | iR = iR < 0 ? 0 : iR; 45 | 46 | iG = iG > 255 ? 255 : iG; 47 | iG = iG < 0 ? 0 : iG; 48 | 49 | iB = iB > 255 ? 255 : iB; 50 | iB = iB < 0 ? 0 : iB; 51 | } 52 | 53 | void yuv422ToRgb24(unsigned char *uyvy422, unsigned char *rgb24, int width, int height) 54 | { 55 | int iR, iG, iB; 56 | int iY0, iY1, iU, iV; 57 | int i = 0; 58 | int j = 0; 59 | for (i = 0; i < width * height * 2; i += 4) 60 | { 61 | iU = uyvy422[i + 0]; 62 | iY0 = uyvy422[i + 1]; 63 | iV = uyvy422[i + 2]; 64 | iY1 = uyvy422[i + 3]; 65 | 66 | yuvToRgb(iY0, iU, iV, iR, iG, iB); 67 | // rgb24[j++] = iR; 68 | // rgb24[j++] = iG; 69 | // rgb24[j++] = iB; 70 | rgb24[j++] = iB; 71 | rgb24[j++] = iG; 72 | rgb24[j++] = iR; 73 | yuvToRgb(iY1, iU, iV, iR, iG, iB); 74 | // rgb24[j++] = iR; 75 | // rgb24[j++] = iG; 76 | // rgb24[j++] = iB; 77 | rgb24[j++] = iB; 78 | rgb24[j++] = iG; 79 | rgb24[j++] = iR; 80 | } 81 | } 82 | 83 | void yuv422ToCvmat(cv::Mat &dst, void *pYUV422, int nWidth, int nHeight, int bitDepth) 84 | { 85 | if (!pYUV422) 86 | { 87 | return; 88 | } 89 | unsigned char *rgb24_buffer = new unsigned char[nWidth * nHeight * 3]; 90 | yuv422ToRgb24((unsigned char *)pYUV422, rgb24_buffer, nWidth, nHeight); 91 | dst = cv::Mat(nHeight, nWidth, CV_8UC3, rgb24_buffer).clone(); 92 | delete[] rgb24_buffer; 93 | } 94 | 95 | bool loadCameraIntrinsics(const std::string &filename, std::vector &intrinsicKList, std::vector &intrinsicDList) 96 | { 97 | if ((access(filename.c_str(), 0)) == -1) 98 | { 99 | printf("invalid intrinsicFile\n"); 100 | return false; 101 | } 102 | YAML::Node yn = YAML::LoadFile(filename); 103 | std::string cameraId; 104 | for (int id = 0; id < 5; ++id) 105 | { 106 | // cameraId = std::to_string(i); 107 | cameraId = boost::lexical_cast(id); 108 | cv::Mat intrinsicK, intrinsicD; 109 | 110 | if (yn[cameraId]["K"].IsDefined()) 111 | { 112 | intrinsicK = cv::Mat::zeros(3, 3, CV_64FC1); 113 | for (int i = 0; i < yn[cameraId]["K"].size(); ++i) 114 | { 115 | intrinsicK.at(i) = yn[cameraId]["K"][i].as(); 116 | } 117 | intrinsicKList.push_back(intrinsicK); 118 | } 119 | else 120 | { 121 | printf("invalid intrinsicFile content\n"); 122 | return false; 123 | } 124 | if (yn[cameraId]["D"].IsDefined()) 125 | { 126 | intrinsicD = cv::Mat::zeros(yn[cameraId]["D"].size(), 1, CV_64FC1); 127 | for (int i = 0; i < yn[cameraId]["D"].size(); ++i) 128 | { 129 | intrinsicD.at(i) = yn[cameraId]["D"][i].as(); 130 | } 131 | intrinsicDList.push_back(intrinsicD); 132 | } 133 | else 134 | { 135 | printf("invalid intrinsicFile content\n"); 136 | return false; 137 | } 138 | } 139 | return true; 140 | } 141 | 142 | 143 | void my_output_message (j_common_ptr ptr) 144 | { 145 | return; 146 | } 147 | 148 | void print_mem(unsigned char* mem , unsigned int size) 149 | { 150 | int i =0; 151 | for(i = 0 ; i < size ; i++) 152 | { 153 | printf("%02x " , mem[i]); 154 | } 155 | printf("\n"); 156 | } 157 | 158 | int decompressJpeg(unsigned char *jpgBuffer, unsigned long jpgSize, unsigned char * &bmp, unsigned long& bmpSize) 159 | { 160 | struct jpeg_decompress_struct cinfo; 161 | struct jpeg_error_mgr jerr; 162 | unsigned char *bmpBuffer; 163 | int rowStride, width, height, pixelSize; 164 | 165 | cinfo.err = jpeg_std_error(&jerr); 166 | cinfo.err->output_message = my_output_message; 167 | cinfo.err->error_exit = my_output_message; 168 | 169 | jpeg_create_decompress(&cinfo); 170 | jpeg_mem_src(&cinfo, jpgBuffer, jpgSize); 171 | 172 | int rc = jpeg_read_header(&cinfo, TRUE); 173 | if (rc != 1) 174 | { 175 | return -1; 176 | } 177 | 178 | jpeg_start_decompress(&cinfo); 179 | 180 | width = cinfo.output_width; 181 | height = cinfo.output_height; 182 | pixelSize = cinfo.output_components; 183 | bmpSize = width * height * pixelSize; 184 | bmpBuffer = (unsigned char *)malloc(bmpSize); 185 | rowStride = width * pixelSize; 186 | 187 | while (cinfo.output_scanline < cinfo.output_height) 188 | { 189 | unsigned char *buffer_array[1]; 190 | buffer_array[0] = bmpBuffer + 191 | (cinfo.output_scanline) * rowStride; 192 | 193 | jpeg_read_scanlines(&cinfo, buffer_array, 1); 194 | } 195 | bmp = bmpBuffer; 196 | jpeg_finish_decompress(&cinfo); 197 | jpeg_destroy_decompress(&cinfo); 198 | return 0; 199 | } --------------------------------------------------------------------------------