├── pandar ├── CATKIN_IGNORE ├── CHANGELOG.rst ├── CMakeLists.txt └── package.xml ├── pandar_msg ├── CHANGELOG.rst ├── msg │ ├── PandarScan.msg │ ├── PandarGps.msg │ └── PandarPacket.msg ├── mainpage.dox ├── CMakeLists.txt └── package.xml ├── pandar_pointcloud ├── CHANGELOG.rst ├── src │ ├── taskflow │ │ ├── predef │ │ │ ├── os.hpp │ │ │ ├── compiler.hpp │ │ │ ├── architecture.hpp │ │ │ ├── os │ │ │ │ ├── os_detected.hpp │ │ │ │ ├── ios.hpp │ │ │ │ └── macos.hpp │ │ │ ├── compiler │ │ │ │ ├── comp_detected.hpp │ │ │ │ ├── clang.hpp │ │ │ │ ├── gcc.hpp │ │ │ │ └── msvc.hpp │ │ │ ├── stringify.hpp │ │ │ ├── architecture │ │ │ │ ├── ppc.hpp │ │ │ │ ├── arm.hpp │ │ │ │ ├── mips.hpp │ │ │ │ └── x86.hpp │ │ │ └── version_number.hpp │ │ ├── taskflow.hpp │ │ ├── core │ │ │ ├── declarations.hpp │ │ │ ├── topology.hpp │ │ │ ├── observer.hpp │ │ │ ├── taskflow.hpp │ │ │ ├── wsq.hpp │ │ │ └── notifier.hpp │ │ ├── utility │ │ │ ├── object_pool.hpp │ │ │ ├── backoff.hpp │ │ │ ├── traits.hpp │ │ │ ├── passive_vector.hpp │ │ │ ├── singular_allocator.hpp │ │ │ └── generic_allocator.hpp │ │ └── error │ │ │ └── error.hpp │ ├── conversions │ │ ├── cloud_node.cc │ │ ├── transform_node.cc │ │ ├── cloud_nodelet.cc │ │ ├── util.h │ │ ├── transform_nodelet.cc │ │ ├── CMakeLists.txt │ │ ├── laser_ts.h │ │ ├── driver.h │ │ ├── transform.h │ │ ├── transform.cc │ │ ├── util.c │ │ ├── tcp_command_client.h │ │ └── laser_ts.cpp │ └── lib │ │ ├── CMakeLists.txt │ │ ├── platUtil.cc │ │ └── calibration.cc ├── mainpage.dox ├── cfg │ ├── CloudNode.cfg │ └── TransformNode.cfg ├── include │ └── pandar_pointcloud │ │ ├── platUtil.h │ │ ├── calibration.h │ │ ├── point_types.h │ │ ├── rawdata.h │ │ └── input.h ├── params │ ├── QT128C2X_Channel_Cofig.csv │ ├── Pandar90_Correction.csv │ ├── QT128C2X_Firetimes.csv │ ├── Pandar128_Correction.csv │ ├── QT128C2X_Correction.csv │ ├── Pandar90_Firetimes.csv │ └── Pandar128_Firetimes.csv ├── nodelets.xml ├── launch │ ├── transform_nodelet.launch │ ├── cloud_nodelet.launch │ └── PandarSwift_points.launch ├── package.xml └── CMakeLists.txt ├── .gitignore ├── COPYING ├── change notes.md └── README.md /pandar/CATKIN_IGNORE: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /pandar/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | Change history 2 | ============== 3 | 1.0.0 4 | -------------------------------------------------------------------------------- /pandar_msg/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | Change history 2 | ============== 3 | 1.0.0 4 | -------------------------------------------------------------------------------- /pandar_msg/msg/PandarScan.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | PandarPacket[] packets 3 | -------------------------------------------------------------------------------- /pandar_pointcloud/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | Change history 2 | ============== 3 | 1.0.0 4 | -------------------------------------------------------------------------------- /pandar_pointcloud/src/taskflow/predef/os.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "os/macos.hpp" 4 | 5 | -------------------------------------------------------------------------------- /pandar/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(pandar) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /pandar_pointcloud/src/taskflow/predef/compiler.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "compiler/clang.hpp" 4 | #include "compiler/gcc.hpp" 5 | #include "compiler/msvc.hpp" 6 | 7 | -------------------------------------------------------------------------------- /pandar_pointcloud/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | Nodes and nodelets for converting raw Pandar128 3D LIDAR data to point 6 | clouds. 7 | 8 | */ 9 | -------------------------------------------------------------------------------- /pandar_msg/msg/PandarGps.msg: -------------------------------------------------------------------------------- 1 | time stamp 2 | uint32 used 3 | uint16 year 4 | uint16 month 5 | uint16 day 6 | uint16 hour 7 | uint16 minute 8 | uint16 second 9 | uint32 fineTime 10 | uint32 flag -------------------------------------------------------------------------------- /pandar_pointcloud/src/taskflow/predef/architecture.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "architecture/arm.hpp" 4 | #include "architecture/mips.hpp" 5 | #include "architecture/ppc.hpp" 6 | #include "architecture/x86.hpp" 7 | -------------------------------------------------------------------------------- /pandar_msg/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | The @b pandar_msg package collects ROS messages specific to the 6 | Pandar128 LIDAR 7 | 8 | No other programming interfaces or ROS nodes are provided. 9 | 10 | */ 11 | -------------------------------------------------------------------------------- /pandar_pointcloud/src/taskflow/taskflow.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "core/executor.hpp" 4 | 5 | namespace tf { 6 | 7 | 8 | 9 | } // end of namespace tf. --------------------------------------------------- 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /pandar_pointcloud/src/taskflow/predef/os/os_detected.hpp: -------------------------------------------------------------------------------- 1 | // 2019-05-20 created by Chun-Xun Lin 2 | // - modifed from boost/predef/detail/os_detected.h 3 | 4 | #pragma once 5 | 6 | #ifndef TF_PREDEF_DETAIL_OS_DETECTED 7 | #define TF_PREDEF_DETAIL_OS_DETECTED 1 8 | #endif 9 | -------------------------------------------------------------------------------- /pandar_pointcloud/src/taskflow/predef/compiler/comp_detected.hpp: -------------------------------------------------------------------------------- 1 | // 2019-04-11 created by Tsung-Wei Huang 2 | // - modifed from boost/predef/detail/comp_detected.h 3 | 4 | #pragma once 5 | 6 | #ifndef TF_PREDEF_DETAIL_COMP_DETECTED 7 | # define TF_PREDEF_DETAIL_COMP_DETECTED 1 8 | #endif 9 | -------------------------------------------------------------------------------- /pandar_pointcloud/src/taskflow/predef/stringify.hpp: -------------------------------------------------------------------------------- 1 | // 2019/04/15 - created by Tsung-Wei Huang 2 | 3 | #pragma once 4 | 5 | #define TF_STRINGIFY_HELPER(x) #x 6 | #define TF_STRINGIFY(x) TF_STRINGIFY_HELPER(x) 7 | 8 | // usage: 9 | // #pragma message("content of macro: " TF_STRINGIFY(macro)) 10 | -------------------------------------------------------------------------------- /pandar_msg/msg/PandarPacket.msg: -------------------------------------------------------------------------------- 1 | # field size(byte) 2 | # SOB 2 3 | # angle 2 4 | # measure 5 5 | # block SOB + angle + measure * 40 6 | # timestamp 4 7 | # factory 2 8 | # reserve 8 9 | # rpm 2 10 | # tail timestamp + factory + reserve + rpm 11 | # packet block * 6 + tail 12 | 13 | time stamp 14 | uint8[] data 15 | uint32 size 16 | -------------------------------------------------------------------------------- /pandar_pointcloud/src/taskflow/core/declarations.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | namespace tf { 4 | 5 | // Forward declaration 6 | class Node; 7 | class Topology; 8 | class Task; 9 | class TaskView; 10 | class FlowBuilder; 11 | class Subflow; 12 | class Taskflow; 13 | class Executor; 14 | 15 | } // end of namespace tf ----------------------------------------------------- 16 | 17 | 18 | -------------------------------------------------------------------------------- /pandar_msg/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(pandar_msgs) 3 | 4 | find_package(catkin REQUIRED COMPONENTS message_generation std_msgs) 5 | 6 | add_message_files( 7 | DIRECTORY msg 8 | FILES 9 | PandarGps.msg 10 | PandarPacket.msg 11 | PandarScan.msg 12 | ) 13 | generate_messages(DEPENDENCIES std_msgs) 14 | 15 | catkin_package( 16 | CATKIN_DEPENDS message_runtime std_msgs 17 | ) 18 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | sites 2 | *.d 3 | 4 | # Compiled Object files 5 | *.slo 6 | *.lo 7 | *.o 8 | *.obj 9 | 10 | # Precompiled Headers 11 | *.gch 12 | *.pch 13 | 14 | # Compiled Dynamic libraries 15 | *.so 16 | *.dylib 17 | *.dll 18 | 19 | # Fortran module files 20 | *.mod 21 | *.smod 22 | 23 | # Compiled Static libraries 24 | *.lai 25 | *.la 26 | *.a 27 | *.lib 28 | 29 | # Executables 30 | *.exe 31 | *.out 32 | *.app 33 | 34 | build 35 | 36 | *.autosave 37 | *.swp 38 | 39 | .catkin_workspace 40 | devel 41 | -------------------------------------------------------------------------------- /pandar_msg/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | pandar_msgs 4 | 1.0.0 5 | 6 | ROS message definitions for Pandar128 3D LIDARs. 7 | 8 | Lingwen Fang 9 | Lingwen Fang 10 | BSD 11 | 12 | catkin 13 | 14 | message_generation 15 | std_msgs 16 | 17 | message_runtime 18 | std_msgs 19 | 20 | 21 | -------------------------------------------------------------------------------- /pandar_pointcloud/cfg/CloudNode.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "pandar_pointcloud" 3 | 4 | from math import pi 5 | from dynamic_reconfigure.parameter_generator_catkin import * 6 | 7 | gen = ParameterGenerator() 8 | 9 | gen.add("min_range", double_t, 0, "min range to publish", 0.9, 0.1, 10.0) 10 | gen.add("max_range", double_t, 0, "max range to publish", 130, 0.1, 200) 11 | gen.add("view_direction", double_t, 0, "angle defining the center of view", 12 | 0.0, -pi, pi) 13 | gen.add("view_width", double_t, 0, "angle defining the view width", 14 | 2*pi, 0.0, 2*pi) 15 | 16 | exit(gen.generate(PACKAGE, "cloud_node", "CloudNode")) 17 | -------------------------------------------------------------------------------- /pandar/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | pandar 4 | 1.0.0 5 | 6 | Basic ROS support for the Pandar128 LIDARs. 7 | 8 | Lingwen Fang 9 | Lingwen Fang 10 | BSD 11 | 14 | 15 | catkin 16 | 17 | pandar_driver 18 | pandar_msgs 19 | pandar_pointcloud 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /pandar_pointcloud/include/pandar_pointcloud/platUtil.h: -------------------------------------------------------------------------------- 1 | #ifndef _PLAT_UTIL_H_ 2 | #define _PLAT_UTIL_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #define SHED_FIFO_PRIORITY_HIGH 99 12 | #define SHED_FIFO_PRIORITY_MEDIUM 70 13 | #define SHED_FIFO_PRIORITY_LOW 1 14 | 15 | extern void ShowThreadPriorityMaxMin (int policy); 16 | extern void SetThreadPriority (int policy, int priority); 17 | 18 | extern unsigned int GetTickCount(); 19 | 20 | extern unsigned int GetMicroTickCount(); 21 | 22 | extern uint64_t GetMicroTickCountU64(); 23 | 24 | #endif //_PLAT_UTIL_H_ 25 | -------------------------------------------------------------------------------- /pandar_pointcloud/src/taskflow/predef/os/ios.hpp: -------------------------------------------------------------------------------- 1 | // 2019-05-20 created by Chun-Xun Lin 2 | // - modifed from boost/predef/os/ios.h 3 | // - modified the include path of os_detected.hpp 4 | 5 | #pragma once 6 | 7 | #include "../version_number.hpp" 8 | 9 | #define TF_OS_IOS TF_VERSION_NUMBER_NOT_AVAILABLE 10 | 11 | #if !defined(TF_PREDEF_DETAIL_OS_DETECTED) && ( \ 12 | defined(__APPLE__) && defined(__MACH__) && \ 13 | defined(__ENVIRONMENT_IPHONE_OS_VERSION_MIN_REQUIRED__) \ 14 | ) 15 | # undef TF_OS_IOS 16 | # define TF_OS_IOS (__ENVIRONMENT_IPHONE_OS_VERSION_MIN_REQUIRED__*1000) 17 | #endif 18 | 19 | #if TF_OS_IOS 20 | # define TF_OS_IOS_AVAILABLE 21 | # include "os_detected.hpp" 22 | #endif 23 | 24 | #define TF_OS_IOS_NAME "iOS" 25 | 26 | -------------------------------------------------------------------------------- /pandar_pointcloud/src/conversions/cloud_node.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2012 Austin Robot Technology, Jack O'Quin 3 | * Copyright (c) 2017 Hesai Photonics Technology, Yang Sheng 4 | * Copyright (c) 2020 Hesai Photonics Technology, Lingwen Fang 5 | * License: Modified BSD Software License Agreement 6 | * 7 | * $Id$ 8 | */ 9 | 10 | /** \file 11 | 12 | This ROS node converts raw Pandar128 LIDAR packets to PointCloud2. 13 | 14 | */ 15 | #include 16 | #include "convert.h" 17 | 18 | /** Main node entry point. */ 19 | int main(int argc, char **argv) { 20 | ros::init(argc, argv, "cloud_node"); 21 | ros::NodeHandle node; 22 | ros::NodeHandle priv_nh("~"); 23 | 24 | // create conversion class, which subscribes to raw data 25 | pandar_pointcloud::Convert conv(node, priv_nh); 26 | 27 | // handle callbacks until shut down 28 | ros::spin(); 29 | 30 | return 0; 31 | } 32 | -------------------------------------------------------------------------------- /pandar_pointcloud/src/taskflow/predef/compiler/clang.hpp: -------------------------------------------------------------------------------- 1 | // 2019-04-11 created by Tsung-Wei Huang 2 | // - modifed from boost/predef/compiler/clang.h 3 | // - modified the include path of comp_detected.hpp 4 | 5 | #pragma once 6 | 7 | #include "../version_number.hpp" 8 | 9 | #define TF_COMP_CLANG TF_VERSION_NUMBER_NOT_AVAILABLE 10 | 11 | #if defined(__clang__) 12 | # define TF_COMP_CLANG_DETECTION TF_VERSION_NUMBER(__clang_major__,__clang_minor__,__clang_patchlevel__) 13 | #endif 14 | 15 | #ifdef TF_COMP_CLANG_DETECTION 16 | # if defined(TF_PREDEF_DETAIL_COMP_DETECTED) 17 | # define TF_COMP_CLANG_EMULATED TF_COMP_CLANG_DETECTION 18 | # else 19 | # undef TF_COMP_CLANG 20 | # define TF_COMP_CLANG TF_COMP_CLANG_DETECTION 21 | # endif 22 | # define TF_COMP_CLANG_AVAILABLE 23 | # include "comp_detected.hpp" 24 | #endif 25 | 26 | #define TF_COMP_CLANG_NAME "Clang" 27 | 28 | -------------------------------------------------------------------------------- /pandar_pointcloud/src/lib/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(pandar_rawdata rawdata.cc calibration.cc) 2 | target_link_libraries(pandar_rawdata 3 | ${catkin_LIBRARIES} 4 | ${YAML_CPP_LIBRARIES}) 5 | install(TARGETS pandar_rawdata 6 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 7 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 8 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) 9 | set_target_properties(pandar_rawdata PROPERTIES 10 | COMPILE_FLAGS -std=c++11) 11 | 12 | 13 | add_library(pandar_input input.cc platUtil.cc) 14 | target_link_libraries(pandar_input 15 | ${catkin_LIBRARIES} 16 | ${libpcap_LIBRARIES} 17 | ) 18 | if(catkin_EXPORTED_TARGETS) 19 | add_dependencies(pandar_input ${catkin_EXPORTED_TARGETS}) 20 | endif() 21 | 22 | install(TARGETS pandar_input 23 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 24 | ) 25 | -------------------------------------------------------------------------------- /pandar_pointcloud/cfg/TransformNode.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "pandar_pointcloud" 3 | 4 | from math import pi 5 | import dynamic_reconfigure.parameter_generator_catkin as pgc 6 | 7 | gen = pgc.ParameterGenerator() 8 | 9 | gen.add("min_range", 10 | pgc.double_t, 11 | 0, 12 | "min range to publish", 13 | 0.9, 0.1, 10.0) 14 | 15 | gen.add("max_range", 16 | pgc.double_t, 17 | 0, 18 | "max range to publish", 19 | 130, 0.1, 200) 20 | 21 | gen.add("view_direction", 22 | pgc.double_t, 23 | 0, 24 | "angle defining the center of view", 25 | 0.0, -pi, pi) 26 | 27 | gen.add("view_width", 28 | pgc.double_t, 29 | 0, 30 | "angle defining the view width", 31 | 2*pi, 0.0, 2*pi) 32 | 33 | gen.add("frame_id", 34 | pgc.str_t, 35 | 0, 36 | "new frame of reference for point clouds", 37 | "odom") 38 | 39 | exit(gen.generate(PACKAGE, "transform_node", "TransformNode")) 40 | -------------------------------------------------------------------------------- /pandar_pointcloud/src/conversions/transform_node.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2012 Austin Robot Technology, Jack O'Quin 3 | * Copyright (c) 2017 Hesai Photonics Technology, Yang Sheng 4 | * Copyright (c) 2020 Hesai Photonics Technology, Lingwen Fang 5 | * License: Modified BSD Software License Agreement 6 | * 7 | * $Id$ 8 | */ 9 | 10 | /** \file 11 | 12 | This ROS node transforms raw Pandar128 LIDAR packets to PointCloud2 13 | in the /odom frame of reference. 14 | 15 | */ 16 | 17 | #include 18 | #include "transform.h" 19 | 20 | /** Main node entry point. */ 21 | int main(int argc, char **argv) 22 | { 23 | ros::init(argc, argv, "transform_node"); 24 | ros::NodeHandle node; 25 | ros::NodeHandle priv_nh("~"); 26 | 27 | // create conversion class, which subscribes to raw data 28 | pandar_pointcloud::Transform transform(node, priv_nh); 29 | 30 | // handle callbacks until shut down 31 | ros::spin(); 32 | 33 | return 0; 34 | } 35 | -------------------------------------------------------------------------------- /pandar_pointcloud/src/taskflow/predef/os/macos.hpp: -------------------------------------------------------------------------------- 1 | // 2019-05-20 created by Chun-Xun Lin 2 | // - modifed from boost/predef/os/macos.h 3 | // - modified the include path of os_detected.hpp 4 | 5 | #pragma once 6 | 7 | #include "ios.hpp" 8 | #include "../version_number.hpp" 9 | 10 | #define TF_OS_MACOS TF_VERSION_NUMBER_NOT_AVAILABLE 11 | 12 | #if !defined(TF_PREDEF_DETAIL_OS_DETECTED) && ( \ 13 | defined(macintosh) || defined(Macintosh) || \ 14 | (defined(__APPLE__) && defined(__MACH__)) \ 15 | ) 16 | # undef TF_OS_MACOS 17 | # if !defined(TF_OS_MACOS) && defined(__APPLE__) && defined(__MACH__) 18 | # define TF_OS_MACOS TF_VERSION_NUMBER(10,0,0) 19 | # endif 20 | # if !defined(TF_OS_MACOS) 21 | # define TF_OS_MACOS TF_VERSION_NUMBER(9,0,0) 22 | # endif 23 | #endif 24 | 25 | #if TF_OS_MACOS 26 | # define TF_OS_MACOS_AVAILABLE 27 | # include "os_detected.hpp" 28 | #endif 29 | 30 | #define TF_OS_MACOS_NAME "Mac OS" 31 | 32 | -------------------------------------------------------------------------------- /pandar_pointcloud/params/QT128C2X_Channel_Cofig.csv: -------------------------------------------------------------------------------- 1 | EEFF,1,1 2 | LaserNum,80,BlockNum,2 3 | Loop1,Loop2 4 | 49,17 5 | 50,18 6 | 51,19 7 | 52,20 8 | 53,21 9 | 54,22 10 | 55,23 11 | 56,24 12 | 57,25 13 | 58,26 14 | 59,27 15 | 60,28 16 | 61,29 17 | 62,30 18 | 63,31 19 | 64,32 20 | 65,65 21 | 66,66 22 | 67,67 23 | 68,68 24 | 69,69 25 | 70,70 26 | 71,71 27 | 72,72 28 | 73,73 29 | 74,74 30 | 75,75 31 | 76,76 32 | 77,77 33 | 78,78 34 | 79,79 35 | 80,80 36 | 81,81 37 | 82,82 38 | 83,83 39 | 84,84 40 | 85,85 41 | 86,86 42 | 87,87 43 | 88,88 44 | 89,89 45 | 90,90 46 | 91,91 47 | 92,92 48 | 93,93 49 | 94,94 50 | 95,95 51 | 96,96 52 | 97,97 53 | 98,98 54 | 99,99 55 | 100,100 56 | 101,101 57 | 102,102 58 | 103,103 59 | 104,104 60 | 105,105 61 | 106,106 62 | 107,107 63 | 108,108 64 | 109,109 65 | 110,110 66 | 111,111 67 | 112,112 68 | 113,113 69 | 114,114 70 | 115,115 71 | 116,116 72 | 117,117 73 | 118,118 74 | 119,119 75 | 120,120 76 | 121,121 77 | 122,122 78 | 123,123 79 | 124,124 80 | 125,125 81 | 126,126 82 | 127,127 83 | 128,128 84 | 3142432,,, -------------------------------------------------------------------------------- /pandar_pointcloud/nodelets.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | Aggregates points from multiple packets, publishing PointCloud2. 7 | 8 | 9 | 10 | 11 | 12 | 15 | 16 | Converts a Pandar128 PointCloud2 to PointXYZRGB, assigning colors 17 | for visualization of the laser rings. 18 | 19 | 20 | 21 | 22 | 23 | 26 | 27 | Transforms packets into /odom frame, publishing multiple packets 28 | as PointCloud2. 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /pandar_pointcloud/src/taskflow/predef/compiler/gcc.hpp: -------------------------------------------------------------------------------- 1 | // 2019-04-11 created by Tsung-Wei Huang 2 | // - modifed from boost/predef/compiler/clang.h 3 | // - modified the include path of comp_detected.hpp 4 | 5 | #pragma once 6 | 7 | #include "../version_number.hpp" 8 | 9 | #define TF_COMP_GNUC TF_VERSION_NUMBER_NOT_AVAILABLE 10 | 11 | #if defined(__GNUC__) 12 | # if !defined(TF_COMP_GNUC_DETECTION) && defined(__GNUC_PATCHLEVEL__) 13 | # define TF_COMP_GNUC_DETECTION \ 14 | TF_VERSION_NUMBER(__GNUC__,__GNUC_MINOR__,__GNUC_PATCHLEVEL__) 15 | # endif 16 | # if !defined(TF_COMP_GNUC_DETECTION) 17 | # define TF_COMP_GNUC_DETECTION \ 18 | TF_VERSION_NUMBER(__GNUC__,__GNUC_MINOR__,0) 19 | # endif 20 | #endif 21 | 22 | #ifdef TF_COMP_GNUC_DETECTION 23 | # if defined(TF_PREDEF_DETAIL_COMP_DETECTED) 24 | # define TF_COMP_GNUC_EMULATED TF_COMP_GNUC_DETECTION 25 | # else 26 | # undef TF_COMP_GNUC 27 | # define TF_COMP_GNUC TF_COMP_GNUC_DETECTION 28 | # endif 29 | # define TF_COMP_GNUC_AVAILABLE 30 | # include "comp_detected.hpp" 31 | #endif 32 | 33 | #define TF_COMP_GNUC_NAME "Gnu GCC C/C++" 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /pandar_pointcloud/src/taskflow/predef/architecture/ppc.hpp: -------------------------------------------------------------------------------- 1 | // 2019-04-15 created by Tsung-Wei Huang 2 | // - modified from boost/predef/architecture/ppc.hpp 3 | 4 | #pragma once 5 | 6 | #define TF_ARCH_PPC TF_VERSION_NUMBER_NOT_AVAILABLE 7 | 8 | #if defined(__powerpc) || defined(__powerpc__) || \ 9 | defined(__POWERPC__) || defined(__ppc__) || \ 10 | defined(_M_PPC) || defined(_ARCH_PPC) || \ 11 | defined(__PPCGECKO__) || defined(__PPCBROADWAY__) || \ 12 | defined(_XENON) 13 | # undef TF_ARCH_PPC 14 | # if !defined (TF_ARCH_PPC) && (defined(__ppc601__) || defined(_ARCH_601)) 15 | # define TF_ARCH_PPC TF_VERSION_NUMBER(6,1,0) 16 | # endif 17 | # if !defined (TF_ARCH_PPC) && (defined(__ppc603__) || defined(_ARCH_603)) 18 | # define TF_ARCH_PPC TF_VERSION_NUMBER(6,3,0) 19 | # endif 20 | # if !defined (TF_ARCH_PPC) && (defined(__ppc604__) || defined(__ppc604__)) 21 | # define TF_ARCH_PPC TF_VERSION_NUMBER(6,4,0) 22 | # endif 23 | # if !defined (TF_ARCH_PPC) 24 | # define TF_ARCH_PPC TF_VERSION_NUMBER_AVAILABLE 25 | # endif 26 | #endif 27 | 28 | #if TF_ARCH_PPC 29 | # define TF_ARCH_PPC_AVAILABLE 30 | #endif 31 | 32 | #define TF_ARCH_PPC_NAME "PowerPC" 33 | 34 | -------------------------------------------------------------------------------- /pandar_pointcloud/src/taskflow/predef/architecture/arm.hpp: -------------------------------------------------------------------------------- 1 | // 2019-04-15 created by Tsung-Wei Huang 2 | // - modified from boost/predef/architecture/arm.hpp 3 | 4 | #pragma once 5 | 6 | #include "../version_number.hpp" 7 | 8 | #define TF_ARCH_ARM TF_VERSION_NUMBER_NOT_AVAILABLE 9 | 10 | #if defined(__arm__) || defined(__arm64) || defined(__thumb__) || \ 11 | defined(__TARGET_ARCH_ARM) || defined(__TARGET_ARCH_THUMB) || \ 12 | defined(_M_ARM) 13 | # undef TF_ARCH_ARM 14 | # if !defined(TF_ARCH_ARM) && defined(__arm64) 15 | # define TF_ARCH_ARM TF_VERSION_NUMBER(8,0,0) 16 | # endif 17 | # if !defined(TF_ARCH_ARM) && defined(__TARGET_ARCH_ARM) 18 | # define TF_ARCH_ARM TF_VERSION_NUMBER(__TARGET_ARCH_ARM,0,0) 19 | # endif 20 | # if !defined(TF_ARCH_ARM) && defined(__TARGET_ARCH_THUMB) 21 | # define TF_ARCH_ARM TF_VERSION_NUMBER(__TARGET_ARCH_THUMB,0,0) 22 | # endif 23 | # if !defined(TF_ARCH_ARM) && defined(_M_ARM) 24 | # define TF_ARCH_ARM TF_VERSION_NUMBER(_M_ARM,0,0) 25 | # endif 26 | # if !defined(TF_ARCH_ARM) 27 | # define TF_ARCH_ARM TF_VERSION_NUMBER_AVAILABLE 28 | # endif 29 | #endif 30 | 31 | #if TF_ARCH_ARM 32 | # define TF_ARCH_ARM_AVAILABLE 33 | #endif 34 | 35 | #define TF_ARCH_ARM_NAME "ARM" 36 | -------------------------------------------------------------------------------- /pandar_pointcloud/src/conversions/cloud_nodelet.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2012 Austin Robot Technology, Jack O'Quin 3 | * Copyright (c) 2017 Hesai Photonics Technology, Yang Sheng 4 | * Copyright (c) 2020 Hesai Photonics Technology, Lingwen Fang 5 | * License: Modified BSD Software License Agreement 6 | * 7 | * $Id$ 8 | */ 9 | 10 | /** @file 11 | 12 | This ROS nodelet converts raw Pandar128 3D LIDAR packets to a 13 | PointCloud2. 14 | 15 | */ 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | #include "convert.h" 22 | 23 | namespace pandar_pointcloud 24 | { 25 | class CloudNodelet: public nodelet::Nodelet 26 | { 27 | public: 28 | 29 | CloudNodelet() {} 30 | ~CloudNodelet() {} 31 | 32 | private: 33 | 34 | virtual void onInit(); 35 | boost::shared_ptr conv_; 36 | }; 37 | 38 | /** @brief Nodelet initialization. */ 39 | void CloudNodelet::onInit() 40 | { 41 | conv_.reset(new Convert(getNodeHandle(), getPrivateNodeHandle())); 42 | } 43 | 44 | } // namespace pandar_pointcloud 45 | 46 | 47 | // Register this plugin with pluginlib. Names must match nodelet_pandar.xml. 48 | // 49 | // parameters: class type, base class type 50 | PLUGINLIB_EXPORT_CLASS(pandar_pointcloud::CloudNodelet, nodelet::Nodelet); 51 | -------------------------------------------------------------------------------- /pandar_pointcloud/src/conversions/util.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2019 The Hesai Technology Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | #include "openssl/ssl.h" 17 | #ifndef SRC_UTIL_H_ 18 | #define SRC_UTIL_H_ 19 | 20 | #ifdef __cplusplus 21 | extern "C" { 22 | #endif 23 | 24 | int sys_readn(int fd, void* vptr, int n); 25 | int sys_readn_by_ssl(SSL *ssl, void *vptr, int n); 26 | int sys_writen(int fd, const void* vptr, int n); 27 | int tcp_open(const char* ipaddr, int port); 28 | int select_fd(int fd, int timeout, int wait_for); 29 | 30 | enum { WAIT_FOR_READ, WAIT_FOR_WRITE, WAIT_FOR_CONN }; 31 | 32 | #ifdef __cplusplus 33 | } 34 | #endif 35 | 36 | #endif // SRC_UTIL_H_ 37 | -------------------------------------------------------------------------------- /pandar_pointcloud/src/taskflow/predef/architecture/mips.hpp: -------------------------------------------------------------------------------- 1 | // 2019-04-15 created by Tsung-Wei Huang 2 | // - modified from boost/predef/architecture/mips.hpp 3 | 4 | #pragma once 5 | 6 | #define TF_ARCH_MIPS TF_VERSION_NUMBER_NOT_AVAILABLE 7 | 8 | #if defined(__mips__) || defined(__mips) || \ 9 | defined(__MIPS__) 10 | # undef TF_ARCH_MIPS 11 | # if !defined(TF_ARCH_MIPS) && (defined(__mips)) 12 | # define TF_ARCH_MIPS TF_VERSION_NUMBER(__mips,0,0) 13 | # endif 14 | # if !defined(TF_ARCH_MIPS) && (defined(_MIPS_ISA_MIPS1) || defined(_R3000)) 15 | # define TF_ARCH_MIPS TF_VERSION_NUMBER(1,0,0) 16 | # endif 17 | # if !defined(TF_ARCH_MIPS) && (defined(_MIPS_ISA_MIPS2) || defined(__MIPS_ISA2__) || defined(_R4000)) 18 | # define TF_ARCH_MIPS TF_VERSION_NUMBER(2,0,0) 19 | # endif 20 | # if !defined(TF_ARCH_MIPS) && (defined(_MIPS_ISA_MIPS3) || defined(__MIPS_ISA3__)) 21 | # define TF_ARCH_MIPS TF_VERSION_NUMBER(3,0,0) 22 | # endif 23 | # if !defined(TF_ARCH_MIPS) && (defined(_MIPS_ISA_MIPS4) || defined(__MIPS_ISA4__)) 24 | # define TF_ARCH_MIPS TF_VERSION_NUMBER(4,0,0) 25 | # endif 26 | # if !defined(TF_ARCH_MIPS) 27 | # define TF_ARCH_MIPS TF_VERSION_NUMBER_AVAILABLE 28 | # endif 29 | #endif 30 | 31 | #if TF_ARCH_MIPS 32 | # define TF_ARCH_MIPS_AVAILABLE 33 | #endif 34 | 35 | #define TF_ARCH_MIPS_NAME "MIPS" 36 | 37 | -------------------------------------------------------------------------------- /pandar_pointcloud/src/conversions/transform_nodelet.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2012 Austin Robot Technology, Jack O'Quin 3 | * Copyright (c) 2017 Hesai Photonics Technology, Yang Sheng 4 | * Copyright (c) 2020 Hesai Photonics Technology, Lingwen Fang 5 | * License: Modified BSD Software License Agreement 6 | * 7 | * $Id$ 8 | */ 9 | 10 | /** @file 11 | 12 | This ROS nodelet transforms raw Pandar128 3D LIDAR packets to a 13 | PointCloud2 in the /odom frame. 14 | 15 | */ 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | #include "transform.h" 22 | 23 | namespace pandar_pointcloud 24 | { 25 | class TransformNodelet: public nodelet::Nodelet 26 | { 27 | public: 28 | 29 | TransformNodelet() {} 30 | ~TransformNodelet() {} 31 | 32 | private: 33 | 34 | virtual void onInit(); 35 | boost::shared_ptr tf_; 36 | }; 37 | 38 | /** @brief Nodelet initialization. */ 39 | void TransformNodelet::onInit() 40 | { 41 | // ROS_WARN(" TransformNodelet::onInit"); 42 | tf_.reset(new Transform(getNodeHandle(), getPrivateNodeHandle())); 43 | } 44 | 45 | } // namespace pandar_pointcloud 46 | 47 | 48 | // Register this plugin with pluginlib. Names must match nodelet_pandar.xml. 49 | // 50 | // parameters: class type, base class type 51 | PLUGINLIB_EXPORT_CLASS(pandar_pointcloud::TransformNodelet, 52 | nodelet::Nodelet); 53 | -------------------------------------------------------------------------------- /pandar_pointcloud/launch/transform_nodelet.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /pandar_pointcloud/src/taskflow/predef/version_number.hpp: -------------------------------------------------------------------------------- 1 | // 2019/04/15 - created by Tsung-Wei Huang 2 | // - modified from boost/predef/version_number.hpp 3 | 4 | #pragma once 5 | 6 | // TF_VERSION_NUMBER(major,minor,patch) 7 | // 8 | // Defines standard version numbers, with these properties: 9 | // 10 | // Decimal base whole numbers in the range \[0,1000000000). 11 | // The number range is designed to allow for a (2,2,5) triplet. 12 | // Which fits within a 32 bit value. 13 | // 14 | // The `major` number can be in the \[0,99\] range. 15 | // The `minor` number can be in the \[0,99\] range. 16 | // The `patch` number can be in the \[0,99999\] range. 17 | // 18 | // Values can be specified in any base. As the defined value 19 | // is an constant expression. 20 | // 21 | // Value can be directly used in both preprocessor and compiler 22 | // expressions for comparison to other similarly defined values. 23 | // 24 | // The implementation enforces the individual ranges for the 25 | // major, minor, and patch numbers. And values over the ranges 26 | // are truncated (modulo). 27 | 28 | 29 | #define TF_VERSION_NUMBER(major,minor,patch) \ 30 | ( (((major)%100)*10000000) + (((minor)%100)*100000) + ((patch)%100000) ) 31 | 32 | #define TF_VERSION_NUMBER_MAX TF_VERSION_NUMBER(99,99,99999) 33 | 34 | #define TF_VERSION_NUMBER_ZERO TF_VERSION_NUMBER(0,0,0) 35 | 36 | #define TF_VERSION_NUMBER_MIN TF_VERSION_NUMBER(0,0,1) 37 | 38 | #define TF_VERSION_NUMBER_AVAILABLE TF_VERSION_NUMBER_MIN 39 | 40 | #define TF_VERSION_NUMBER_NOT_AVAILABLE TF_VERSION_NUMBER_ZERO 41 | 42 | #define TF_VERSION_NUMBER_MAJOR(N) ( ((N)/10000000)%100 ) 43 | 44 | #define TF_VERSION_NUMBER_MINOR(N) ( ((N)/100000)%100 ) 45 | 46 | #define TF_VERSION_NUMBER_PATCH(N) ( (N)%100000 ) 47 | -------------------------------------------------------------------------------- /pandar_pointcloud/include/pandar_pointcloud/calibration.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file calibration.h 3 | * 4 | * \author Piyush Khandelwal (piyushk@cs.utexas.edu) 5 | * Copyright (C) 2012, Austin Robot Technology, University of Texas at Austin 6 | * Copyright (c) 2017 Hesai Photonics Technology, Yang Sheng 7 | * Copyright (c) 2020 Hesai Photonics Technology, Lingwen Fang 8 | * 9 | * License: Modified BSD License 10 | * 11 | * $ Id: 02/14/2012 11:25:34 AM piyushk $ 12 | */ 13 | 14 | #ifndef __PANDAR_CALIBRATION_H 15 | #define __PANDAR_CALIBRATION_H 16 | 17 | #include 18 | #include 19 | 20 | namespace pandar_pointcloud { 21 | 22 | /** \brief Correction information for a single laser. */ 23 | struct PandarLaserCorrection 24 | { 25 | double azimuthCorrection; 26 | double verticalCorrection; 27 | double distanceCorrection; 28 | double verticalOffsetCorrection; 29 | double horizontalOffsetCorrection; 30 | double sinVertCorrection; 31 | double cosVertCorrection; 32 | double sinVertOffsetCorrection; 33 | double cosVertOffsetCorrection; 34 | }; 35 | 36 | /** \brief Calibration information for the entire device. */ 37 | class Calibration { 38 | 39 | public: 40 | static const int laser_count = 40; 41 | PandarLaserCorrection laser_corrections[laser_count]; 42 | int num_lasers; 43 | bool initialized; 44 | 45 | public: 46 | 47 | Calibration(): initialized(false) 48 | {} 49 | Calibration(const std::string& calibration_file) 50 | { 51 | read(calibration_file); 52 | } 53 | 54 | void read(const std::string& calibration_file); 55 | void write(const std::string& calibration_file); 56 | 57 | private: 58 | void setDefaultCorrections (); 59 | }; 60 | 61 | } /* pandar_pointcloud */ 62 | 63 | 64 | #endif /* end of include guard: __PANDAR_CALIBRATION_H */ 65 | 66 | 67 | -------------------------------------------------------------------------------- /pandar_pointcloud/src/lib/platUtil.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #define gettid() syscall(SYS_gettid) 6 | 7 | void ShowThreadPriorityMaxMin (int policy) 8 | { 9 | int priority = sched_get_priority_max (policy); 10 | printf ("policy %d max_priority = %d\n", policy, priority); 11 | priority = sched_get_priority_min (policy); 12 | printf ("policy %d, min_priority = %d\n", policy, priority); 13 | } 14 | 15 | void SetThreadPriority (int policy, int priority) 16 | { 17 | printf("set thread %lu, tid %ld, policy %d and priority %d\n", pthread_self(), gettid(), policy, priority); 18 | sched_param param; 19 | param.sched_priority = priority; 20 | pthread_setschedparam(pthread_self(), policy, ¶m); 21 | 22 | int ret_policy; 23 | pthread_getschedparam(pthread_self(), &ret_policy, ¶m); 24 | printf("get thead %lu, tid %ld, policy %d and priority %d\n", pthread_self(), gettid(), ret_policy, param.sched_priority); 25 | 26 | } 27 | 28 | 29 | unsigned int GetTickCount() { 30 | unsigned int ret = 0; 31 | timespec time; 32 | memset(&time, 0, sizeof(time)); 33 | if (clock_gettime(CLOCK_MONOTONIC, &time) == 0) { 34 | ret = time.tv_nsec / 1000000 + time.tv_sec * 1000; 35 | } 36 | return ret; 37 | } 38 | 39 | unsigned int GetMicroTickCount() { 40 | unsigned int ret = 0; 41 | timespec time; 42 | memset(&time, 0, sizeof(time)); 43 | if (clock_gettime(CLOCK_MONOTONIC, &time) == 0) { 44 | ret = time.tv_nsec / 1000 + time.tv_sec * 1000000; 45 | } 46 | return ret; 47 | } 48 | 49 | uint64_t GetMicroTickCountU64() { 50 | uint64_t ret = 0; 51 | timespec time; 52 | memset(&time, 0, sizeof(time)); 53 | if (clock_gettime(CLOCK_MONOTONIC, &time) == 0) { 54 | ret = time.tv_nsec / 1000 + time.tv_sec * 1000000; 55 | } 56 | return ret; 57 | } 58 | -------------------------------------------------------------------------------- /COPYING: -------------------------------------------------------------------------------- 1 | Copyright (C) 2020-2021 Hesai Technology, and others 2 | All rights reserved. 3 | 4 | 5 | All code in this repository is released under the terms of the 6 | following Modified BSD License. 7 | 8 | 9 | Modified BSD License: 10 | -------------------- 11 | 12 | Redistribution and use in source and binary forms, with or without 13 | modification, are permitted provided that the following conditions 14 | are met: 15 | 16 | * Redistributions of source code must retain the above copyright 17 | notice, this list of conditions and the following disclaimer. 18 | 19 | * Redistributions in binary form must reproduce the above 20 | copyright notice, this list of conditions and the following 21 | disclaimer in the documentation and/or other materials provided 22 | with the distribution. 23 | 24 | * Neither the names of the University of Texas at Austin, nor 25 | Austin Robot Technology, nor the names of other contributors may 26 | be used to endorse or promote products derived from this 27 | software without specific prior written permission. 28 | 29 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 30 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 31 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 32 | FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 33 | COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 34 | INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 35 | BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 36 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 37 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 38 | LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 39 | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 40 | POSSIBILITY OF SUCH DAMAGE. 41 | -------------------------------------------------------------------------------- /pandar_pointcloud/src/taskflow/predef/compiler/msvc.hpp: -------------------------------------------------------------------------------- 1 | // 2019-04-11 created by Tsung-Wei Huang 2 | // - modifed from boost/predef/compiler/visualc.h 3 | // - modified the include path of comp_detected.hpp 4 | 5 | #pragma once 6 | 7 | #include "../version_number.hpp" 8 | 9 | #define TF_COMP_MSVC TF_VERSION_NUMBER_NOT_AVAILABLE 10 | 11 | #if defined(_MSC_VER) 12 | # if !defined (_MSC_FULL_VER) 13 | # define TF_COMP_MSVC_BUILD 0 14 | # else 15 | # if _MSC_FULL_VER / 10000 == _MSC_VER 16 | # define TF_COMP_MSVC_BUILD (_MSC_FULL_VER % 10000) 17 | # elif _MSC_FULL_VER / 100000 == _MSC_VER 18 | # define TF_COMP_MSVC_BUILD (_MSC_FULL_VER % 100000) 19 | # else 20 | # error "Cannot determine build number from _MSC_FULL_VER" 21 | # endif 22 | # endif 23 | /* 24 | VS2014 was skipped in the release sequence for MS. Which 25 | means that the compiler and VS product versions are no longer 26 | in sync. Hence we need to use different formulas for 27 | mapping from MSC version to VS product version. 28 | */ 29 | # if (_MSC_VER >= 1900) 30 | # define TF_COMP_MSVC_DETECTION TF_VERSION_NUMBER(\ 31 | _MSC_VER/100-5,\ 32 | _MSC_VER%100,\ 33 | TF_COMP_MSVC_BUILD) 34 | # else 35 | # define TF_COMP_MSVC_DETECTION TF_VERSION_NUMBER(\ 36 | _MSC_VER/100-6,\ 37 | _MSC_VER%100,\ 38 | TF_COMP_MSVC_BUILD) 39 | # endif 40 | #endif 41 | 42 | #ifdef TF_COMP_MSVC_DETECTION 43 | # if defined(TF_PREDEF_DETAIL_COMP_DETECTED) 44 | # define TF_COMP_MSVC_EMULATED TF_COMP_MSVC_DETECTION 45 | # else 46 | # undef TF_COMP_MSVC 47 | # define TF_COMP_MSVC TF_COMP_MSVC_DETECTION 48 | # endif 49 | # define TF_COMP_MSVC_AVAILABLE 50 | # include "comp_detected.hpp" 51 | #endif 52 | 53 | #define TF_COMP_MSVC_NAME "Microsoft Visual C/C++" 54 | -------------------------------------------------------------------------------- /pandar_pointcloud/params/Pandar90_Correction.csv: -------------------------------------------------------------------------------- 1 | Channel,Elevation,Azimuth 2 | 1,14.436,3.257 3 | 2,13.535,3.263 4 | 3,11.702,3.273 5 | 4,9.83,3.283 6 | 5,8.401,1.097 7 | 6,6.953,-1.101 8 | 7,5.487,-3.306 9 | 8,4.007,-1.109 10 | 9,3.013,-1.111 11 | 10,2.512,-3.324 12 | 11,2.013,-1.113 13 | 12,1.637,3.325 14 | 13,1.258,-5.538 15 | 14,1.008,-1.115 16 | 15,0.756,5.543 17 | 16,0.63,3.329 18 | 17,0.505,-3.336 19 | 18,0.379,1.108 20 | 19,0.251,-5.547 21 | 20,0.124,-7.738 22 | 21,0,-1.117 23 | 22,-0.129,7.743 24 | 23,-0.254,5.551 25 | 24,-0.38,3.335 26 | 25,-0.506,-3.342 27 | 26,-0.632,1.11 28 | 27,-0.76,-5.555 29 | 28,-0.887,-7.75 30 | 29,-1.012,-1.119 31 | 30,-1.141,7.757 32 | 31,-1.266,5.56 33 | 32,-1.393,3.34 34 | 33,-1.519,-3.347 35 | 34,-1.646,1.111 36 | 35,-1.773,-5.564 37 | 36,-1.901,-7.762 38 | 37,-2.027,-1.121 39 | 38,-2.155,7.768 40 | 39,-2.282,5.569 41 | 40,-2.409,3.345 42 | 41,-2.535,-3.353 43 | 42,-2.663,1.113 44 | 43,-2.789,-5.573 45 | 44,-2.916,-7.775 46 | 45,-3.044,-1.123 47 | 46,-3.172,7.78 48 | 47,-3.299,5.578 49 | 48,-3.425,3.351 50 | 49,-3.552,-3.358 51 | 50,-3.68,1.115 52 | 51,-3.806,-5.582 53 | 52,-3.933,-7.787 54 | 53,-4.062,-1.125 55 | 54,-4.19,7.792 56 | 55,-4.318,5.586 57 | 56,-4.444,3.356 58 | 57,-4.571,-3.363 59 | 58,-4.699,1.116 60 | 59,-4.824,-5.591 61 | 60,-5.081,-1.127 62 | 61,-5.336,5.595 63 | 62,-5.718,1.118 64 | 63,-6.1,-1.129 65 | 64,-6.607,-3.374 66 | 65,-7.117,-1.13 67 | 66,-7.624,-3.379 68 | 67,-8.134,-1.132 69 | 68,-8.64,-3.383 70 | 69,-9.149,3.381 71 | 70,-9.652,-3.388 72 | 71,-10.16,3.386 73 | 72,-10.665,1.127 74 | 73,-11.17,3.39 75 | 74,-11.672,1.129 76 | 75,-12.174,3.395 77 | 76,-12.673,1.131 78 | 77,-13.173,3.401 79 | 78,-13.67,1.133 80 | 79,-14.166,3.406 81 | 80,-14.66,1.135 82 | 81,-15.154,3.41 83 | 82,-15.645,1.137 84 | 83,-16.622,1.139 85 | 84,-17.592,1.142 86 | 85,-18.548,-3.426 87 | 86,-19.978,-1.145 88 | 87,-21.379,-3.436 89 | 88,-22.768,-1.146 90 | 89,-24.123,-3.446 91 | 90,-25.016,-3.449 92 | -------------------------------------------------------------------------------- /pandar_pointcloud/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | pandar_pointcloud 4 | 1.0.0 5 | 6 | Point cloud conversions for Pandar128 3D LIDARs. 7 | 8 | Lingwen Fang 9 | Lingwen Fang 10 | BSD 11 | 12 | catkin 13 | 14 | angles 15 | diagnostic_updater 16 | dynamic_reconfigure 17 | libpcap 18 | nodelet 19 | pandar_msgs 20 | pcl_conversions 21 | pcl_ros 22 | pluginlib 23 | roscpp 24 | roslib 25 | sensor_msgs 26 | tf 27 | yaml-cpp 28 | libpcl-all-dev 29 | pcl_conversions 30 | 31 | 32 | roslaunch 33 | rostest 34 | tf2_ros 35 | 36 | angles 37 | diagnostic_updater 38 | dynamic_reconfigure 39 | libpcap 40 | nodelet 41 | pandar_msgs 42 | pcl_ros 43 | pluginlib 44 | python-yaml 45 | roscpp 46 | roslib 47 | sensor_msgs 48 | tf 49 | yaml-cpp 50 | libpcl-all 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /pandar_pointcloud/src/taskflow/utility/object_pool.hpp: -------------------------------------------------------------------------------- 1 | // 2019/07/10 - modified by Tsung-Wei Huang 2 | // - replace raw pointer with smart pointer 3 | // 4 | // 2019/06/13 - created by Tsung-Wei Huang 5 | // - implemented an object pool class 6 | 7 | #pragma once 8 | 9 | #include 10 | 11 | namespace tf { 12 | 13 | // Class: ObjectPool 14 | template 15 | class ObjectPool { 16 | 17 | public: 18 | 19 | ObjectPool() = default; 20 | ObjectPool(const ObjectPool& other) = delete; 21 | ObjectPool(ObjectPool&& other); 22 | 23 | ~ObjectPool() = default; 24 | 25 | ObjectPool& operator = (const ObjectPool& other) = delete; 26 | ObjectPool& operator = (ObjectPool&& other); 27 | 28 | size_t size() const; 29 | 30 | void release(std::unique_ptr&& obj); 31 | 32 | template 33 | std::unique_ptr acquire(ArgsT&&... args); 34 | 35 | private: 36 | 37 | std::vector> _stack; 38 | }; 39 | 40 | // Move constructor 41 | template 42 | ObjectPool::ObjectPool(ObjectPool&& other) : 43 | _stack {std::move(other._stack)} { 44 | } 45 | 46 | // Move assignment 47 | template 48 | ObjectPool& ObjectPool::operator = (ObjectPool&& other) { 49 | _stack = std::move(other._stack); 50 | return *this; 51 | } 52 | 53 | // Function: size 54 | template 55 | size_t ObjectPool::size() const { 56 | return _stack.size(); 57 | } 58 | 59 | // Function: acquire 60 | template 61 | template 62 | std::unique_ptr ObjectPool::acquire(ArgsT&&... args) { 63 | if(_stack.empty()) { 64 | return std::make_unique(std::forward(args)...); 65 | } 66 | else { 67 | auto ptr = std::move(_stack.back()); 68 | ptr->animate(std::forward(args)...); 69 | _stack.pop_back(); 70 | return ptr; 71 | } 72 | } 73 | 74 | // Procedure: release 75 | template 76 | void ObjectPool::release(std::unique_ptr&& obj) { 77 | obj->recycle(); 78 | _stack.push_back(std::move(obj)); 79 | } 80 | 81 | } // end of namespace tf ----------------------------------------------------- 82 | 83 | 84 | 85 | 86 | 87 | -------------------------------------------------------------------------------- /pandar_pointcloud/src/conversions/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | include_directories(../taskflow) 2 | 3 | find_package(PCL REQUIRED COMPONENTS common) 4 | find_package(OpenSSL REQUIRED) 5 | 6 | message("OpenSSL include dir: ${OPENSSL_INCLUDE_DIR}") 7 | 8 | include_directories( 9 | ${OPENSSL_INCLUDE_DIR} 10 | ) 11 | 12 | add_executable(pandar_cloud_node cloud_node.cc convert.cc driver.cc laser_ts.cpp tcp_command_client.c util.c) 13 | add_dependencies(pandar_cloud_node ${${PROJECT_NAME}_EXPORTED_TARGETS}) 14 | target_link_libraries(pandar_cloud_node pandar_rawdata 15 | pandar_input 16 | pcap 17 | ${PCL_INCLUDE_DIRS} 18 | ${OPENSSL_LIBRARIES} 19 | ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) 20 | 21 | 22 | add_library(pandar_cloud_nodelet cloud_nodelet.cc convert.cc driver.cc laser_ts.cpp tcp_command_client.c util.c) 23 | add_dependencies(pandar_cloud_nodelet ${${PROJECT_NAME}_EXPORTED_TARGETS}) 24 | target_link_libraries(pandar_cloud_nodelet 25 | pandar_rawdata 26 | pandar_input 27 | pcap 28 | ${PCL_INCLUDE_DIRS} 29 | ${OPENSSL_LIBRARIES} 30 | ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) 31 | 32 | add_executable(pandar_transform_node transform_node.cc transform.cc convert.cc driver.cc laser_ts.cpp tcp_command_client.c util.c) 33 | add_dependencies(pandar_transform_node ${${PROJECT_NAME}_EXPORTED_TARGETS}) 34 | target_link_libraries(pandar_transform_node pandar_rawdata 35 | pandar_input 36 | pcap 37 | ${PCL_INCLUDE_DIRS} 38 | ${OPENSSL_LIBRARIES} 39 | ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) 40 | 41 | add_library(pandar_transform_nodelet transform_nodelet.cc transform.cc convert.cc driver.cc laser_ts.cpp tcp_command_client.c util.c) 42 | add_dependencies(pandar_transform_nodelet ${${PROJECT_NAME}_EXPORTED_TARGETS}) 43 | target_link_libraries(pandar_transform_nodelet pandar_rawdata 44 | pandar_input 45 | pcap 46 | ${PCL_INCLUDE_DIRS} 47 | ${OPENSSL_LIBRARIES} 48 | ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) 49 | install(TARGETS 50 | pandar_cloud_node 51 | pandar_cloud_nodelet 52 | pandar_transform_node 53 | pandar_transform_nodelet 54 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 55 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 56 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) 57 | -------------------------------------------------------------------------------- /pandar_pointcloud/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(pandar_pointcloud) 3 | 4 | set(CMAKE_CXX_STANDARD 17) 5 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 6 | 7 | set(${PROJECT_NAME}_CATKIN_DEPS 8 | angles 9 | diagnostic_updater 10 | dynamic_reconfigure 11 | nodelet 12 | pandar_msgs 13 | pcl_ros 14 | roscpp 15 | roslib 16 | sensor_msgs 17 | tf 18 | ) 19 | 20 | find_package(catkin REQUIRED COMPONENTS 21 | ${${PROJECT_NAME}_CATKIN_DEPS} pcl_conversions) 22 | find_package(Boost COMPONENTS signals) 23 | 24 | # Resolve system dependency on yaml-cpp, which apparently does not 25 | # provide a CMake find_package() module. 26 | find_package(PkgConfig REQUIRED) 27 | pkg_check_modules(YAML_CPP REQUIRED yaml-cpp) 28 | find_path(YAML_CPP_INCLUDE_DIR 29 | NAMES yaml_cpp.h 30 | PATHS ${YAML_CPP_INCLUDE_DIRS}) 31 | find_library(YAML_CPP_LIBRARY 32 | NAMES YAML_CPP 33 | PATHS ${YAML_CPP_LIBRARY_DIRS}) 34 | 35 | link_directories(${YAML_CPP_LIBRARY_DIRS}) 36 | 37 | # libpcap provides no pkg-config or find_package module: 38 | set(libpcap_LIBRARIES -lpcap) 39 | 40 | generate_dynamic_reconfigure_options( 41 | cfg/CloudNode.cfg cfg/TransformNode.cfg 42 | ) 43 | 44 | if(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") 45 | add_definitions(-DHAVE_NEW_YAMLCPP) 46 | endif(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") 47 | 48 | include_directories(include ${catkin_INCLUDE_DIRS} 49 | ${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake 50 | ) 51 | 52 | catkin_package( 53 | CATKIN_DEPENDS ${${PROJECT_NAME}_CATKIN_DEPS} 54 | INCLUDE_DIRS include 55 | LIBRARIES pandar_rawdata) 56 | 57 | #add_executable(dynamic_reconfigure_node src/dynamic_reconfigure_node.cpp) 58 | #target_link_libraries(dynamic_reconfigure_node 59 | # ${catkin_LIBRARIES} 60 | # ) 61 | 62 | add_subdirectory(src/lib) 63 | add_subdirectory(src/conversions) 64 | 65 | include_directories(taskflow) 66 | 67 | install(DIRECTORY include/${PROJECT_NAME}/ 68 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) 69 | install(FILES nodelets.xml 70 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 71 | install(DIRECTORY launch/ 72 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) 73 | install(DIRECTORY params/ 74 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/params) 75 | 76 | #if (CATKIN_ENABLE_TESTING) 77 | #add_subdirectory(tests) 78 | #endif() 79 | -------------------------------------------------------------------------------- /pandar_pointcloud/src/taskflow/predef/architecture/x86.hpp: -------------------------------------------------------------------------------- 1 | // 2019-04-15 created by Tsung-Wei Huang 2 | // - modified from boost/predef/architecture/x86.hpp 3 | // - expanded BOOST_PREDEF_MAKE_10_VV00 in x86_32 4 | 5 | #pragma once 6 | 7 | #define TF_ARCH_X86_64 TF_VERSION_NUMBER_NOT_AVAILABLE 8 | 9 | // x86-64 ------------------------------------------------- 10 | #if defined(__x86_64) || defined(__x86_64__) || \ 11 | defined(__amd64__) || defined(__amd64) || \ 12 | defined(_M_X64) 13 | # undef TF_ARCH_X86_64 14 | # define TF_ARCH_X86_64 TF_VERSION_NUMBER_AVAILABLE 15 | #endif 16 | 17 | #if TF_ARCH_X86_64 18 | # define TF_ARCH_X86_64_AVAILABLE 19 | #endif 20 | 21 | #define TF_ARCH_X86_64_NAME "Intel x86-64" 22 | 23 | // x86-32 ------------------------------------------------- 24 | #define TF_ARCH_X86_32 TF_VERSION_NUMBER_NOT_AVAILABLE 25 | 26 | #if defined(i386) || defined(__i386__) || \ 27 | defined(__i486__) || defined(__i586__) || \ 28 | defined(__i686__) || defined(__i386) || \ 29 | defined(_M_IX86) || defined(_X86_) || \ 30 | defined(__THW_INTEL__) || defined(__I86__) || \ 31 | defined(__INTEL__) 32 | # undef TF_ARCH_X86_32 33 | # if !defined(TF_ARCH_X86_32) && defined(__I86__) 34 | # define TF_ARCH_X86_32 TF_VERSION_NUMBER(__I86__,0,0) 35 | # endif 36 | # if !defined(TF_ARCH_X86_32) && defined(_M_IX86) 37 | # define TF_ARCH_X86_32 TF_VERSION_NUMBER(((_M_IX86)/100)%100,0,0) 38 | # endif 39 | # if !defined(TF_ARCH_X86_32) && defined(__i686__) 40 | # define TF_ARCH_X86_32 TF_VERSION_NUMBER(6,0,0) 41 | # endif 42 | # if !defined(TF_ARCH_X86_32) && defined(__i586__) 43 | # define TF_ARCH_X86_32 TF_VERSION_NUMBER(5,0,0) 44 | # endif 45 | # if !defined(TF_ARCH_X86_32) && defined(__i486__) 46 | # define TF_ARCH_X86_32 TF_VERSION_NUMBER(4,0,0) 47 | # endif 48 | # if !defined(TF_ARCH_X86_32) && defined(__i386__) 49 | # define TF_ARCH_X86_32 TF_VERSION_NUMBER(3,0,0) 50 | # endif 51 | # if !defined(TF_ARCH_X86_32) 52 | # define TF_ARCH_X86_32 TF_VERSION_NUMBER_AVAILABLE 53 | # endif 54 | #endif 55 | 56 | #if TF_ARCH_X86_32 57 | # define TF_ARCH_X86_32_AVAILABLE 58 | #endif 59 | 60 | #define TF_ARCH_X86_32_NAME "Intel x86-32" 61 | 62 | // x86 ---------------------------------------------------- 63 | #define TF_ARCH_X86 TF_VERSION_NUMBER_NOT_AVAILABLE 64 | 65 | #if TF_ARCH_X86_32 || TF_ARCH_X86_64 66 | # undef TF_ARCH_X86 67 | # define TF_ARCH_X86 TF_VERSION_NUMBER_AVAILABLE 68 | #endif 69 | 70 | 71 | #if TF_ARCH_X86 72 | # define TF_ARCH_X86_AVAILABLE 73 | #endif 74 | 75 | #define TF_ARCH_X86_NAME "Intel x86" 76 | 77 | 78 | 79 | -------------------------------------------------------------------------------- /pandar_pointcloud/src/taskflow/core/topology.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | //#include "taskflow.hpp" 4 | 5 | namespace tf { 6 | 7 | // ---------------------------------------------------------------------------- 8 | 9 | // class: Topology 10 | class Topology { 11 | 12 | friend class Taskflow; 13 | friend class Executor; 14 | 15 | public: 16 | 17 | template 18 | Topology(Taskflow&, P&&, C&&); 19 | 20 | private: 21 | 22 | Taskflow& _taskflow; 23 | 24 | std::promise _promise; 25 | 26 | PassiveVector _sources; 27 | 28 | std::function _pred; 29 | std::function _call; 30 | 31 | std::atomic _join_counter {0}; 32 | }; 33 | 34 | // Constructor 35 | template 36 | inline Topology::Topology(Taskflow& tf, P&& p, C&& c): 37 | _taskflow(tf), 38 | _pred {std::forward

(p)}, 39 | _call {std::forward(c)} { 40 | } 41 | 42 | // Procedure: _bind 43 | // Re-builds the source links and the sink number for this topology. 44 | //inline void Topology::_bind(Graph& g) { 45 | // 46 | // _sources.clear(); 47 | // 48 | // //PassiveVector condition_nodes; 49 | // 50 | // // scan each node in the graph and build up the links 51 | // for(auto& node : g.nodes()) { 52 | // 53 | // node->_topology = this; 54 | // node->_clear_state(); 55 | // node->_set_up_join_counter(); 56 | // 57 | // if(node->num_dependents() == 0) { 58 | // _sources.push_back(node.get()); 59 | // } 60 | // 61 | // //int join_counter = 0; 62 | // //for(auto p : node->_dependents) { 63 | // // if(p->_work.index() == Node::CONDITION_WORK) { 64 | // // node->_set_state(Node::BRANCH); 65 | // // } 66 | // // else { 67 | // // join_counter++; 68 | // // } 69 | // //} 70 | // 71 | // //node->_join_counter.store(join_counter, std::memory_order_relaxed); 72 | // 73 | // //// TODO: Merge with the loop below? 74 | // //if(node->_work.index() == Node::CONDITION_WORK) { 75 | // // condition_nodes.push_back(node.get()); 76 | // //} 77 | // 78 | // //// Reset each node's num_dependents 79 | // //node->_join_counter.store(node->_dependents.size(), std::memory_order_relaxed); 80 | // } 81 | // 82 | // // We need to deduct the condition predecessors in impure case nodes 83 | // //for(auto& n: condition_nodes) { 84 | // // for(auto& s: n->_successors) { 85 | // // s->_join_counter.fetch_sub(1, std::memory_order_relaxed); 86 | // // s->set_branch(); 87 | // // } 88 | // //} 89 | //} 90 | 91 | 92 | 93 | } // end of namespace tf. ---------------------------------------------------- 94 | -------------------------------------------------------------------------------- /pandar_pointcloud/src/conversions/laser_ts.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2019 The Hesai Technology Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | 17 | #ifndef LASER_TS_H_ 18 | #define LASER_TS_H_ 19 | 20 | #include 21 | #include 22 | 23 | #ifndef CIRCLE 24 | #define CIRCLE (36000) 25 | #endif 26 | 27 | #ifndef PAI_ANGLE 28 | #define PAI_ANGLE (180000) 29 | #endif 30 | 31 | #ifndef HALF_PAI_ANGLE 32 | #define HALF_PAI_ANGLE (90000) 33 | #endif 34 | #define PANDAR80_LIDAR_NUM (80) 35 | #define PANDAR128_LIDAR_NUM (128) 36 | 37 | class LasersTSOffset { 38 | public: 39 | LasersTSOffset(); 40 | ~LasersTSOffset(); 41 | 42 | void setFilePath(std::string sFile); 43 | int ParserFiretimeData(std::string firetimeContent); 44 | float getTSOffset(int nLaser, int nMode, int nState, float fDistance, int nMajorVersion); 45 | int getBlockTS(int nBlock, int nRetMode, int nMode, int nLaserNum); 46 | float getAngleOffset(float nTSOffset, int speed, int nMajorVersion); 47 | float getAzimuthOffset(std::string type, float azimuth, float originAzimuth, float distance); 48 | float getPitchOffset(std::string type, float pitch, float distance); 49 | 50 | private: 51 | float mFDist; 52 | bool mBInitFlag; 53 | std::vector> mVLasers; 54 | int mNLaserNum; 55 | std::vector mShortOffsetIndex; 56 | std::vector mLongOffsetIndex; 57 | float mCosAllAngle[CIRCLE]; 58 | float mSinAllAngleHB[CIRCLE]; 59 | float mSinAllAngleH[CIRCLE]; 60 | float mArcSin[PAI_ANGLE]; 61 | float m_fAzimuthOffset[PANDAR128_LIDAR_NUM]; 62 | float m_fCDAAzimuthOffset[PANDAR128_LIDAR_NUM]; 63 | float m_fCDBAzimuthOffset[PANDAR128_LIDAR_NUM]; 64 | float m_fArctanHB; 65 | std::array, 4> m_vQT128Firetime; 66 | 67 | void fillVector(char *pContent, int nLen, std::vector &vec); 68 | float atanAngle(float value); 69 | float asinAngle(float value); 70 | }; 71 | 72 | #endif // ASER_TS_H_ 73 | 74 | -------------------------------------------------------------------------------- /pandar_pointcloud/src/taskflow/error/error.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | namespace tf { 9 | 10 | /** 11 | @struct Error 12 | 13 | @brief The error category of taskflow. 14 | */ 15 | struct Error : public std::error_category { 16 | 17 | /** 18 | @enum Code 19 | @brief Error code definition. 20 | */ 21 | enum Code : int { 22 | SUCCESS = 0, 23 | TASKFLOW, 24 | EXECUTOR 25 | }; 26 | 27 | /** 28 | @brief returns the name of the taskflow error category 29 | */ 30 | inline const char* name() const noexcept override final; 31 | 32 | /** 33 | @brief acquires the singleton instance of the taskflow error category 34 | */ 35 | inline static const std::error_category& get(); 36 | 37 | /** 38 | @brief query the human-readable string of each error code 39 | */ 40 | inline std::string message(int) const override final; 41 | }; 42 | 43 | // Function: name 44 | inline const char* Error::name() const noexcept { 45 | return "Taskflow error"; 46 | } 47 | 48 | // Function: get 49 | inline const std::error_category& Error::get() { 50 | static Error instance; 51 | return instance; 52 | } 53 | 54 | // Function: message 55 | inline std::string Error::message(int code) const { 56 | switch(auto ec = static_cast(code); ec) { 57 | case SUCCESS: 58 | return "success"; 59 | break; 60 | 61 | case TASKFLOW: 62 | return "taskflow error"; 63 | break; 64 | 65 | case EXECUTOR: 66 | return "executor error"; 67 | break; 68 | 69 | default: 70 | return "unknown"; 71 | break; 72 | }; 73 | } 74 | 75 | // Function: make_error_code 76 | // Argument dependent lookup. 77 | inline std::error_code make_error_code(Error::Code e) { 78 | return std::error_code(static_cast(e), Error::get()); 79 | } 80 | 81 | } // end of namespace tf ---------------------------------------------------- 82 | 83 | // Register for implicit conversion 84 | namespace std { 85 | template <> 86 | struct is_error_code_enum : true_type {}; 87 | } 88 | 89 | // ---------------------------------------------------------------------------- 90 | 91 | namespace tf { 92 | 93 | // Procedure: throw_se 94 | // Throws the system error under a given error code. 95 | template 96 | void throw_se(const char* fname, const size_t line, Error::Code c, ArgsT&&... args) { 97 | std::ostringstream oss; 98 | oss << "[" << fname << ":" << line << "] "; 99 | (oss << ... << args); 100 | throw std::system_error(c, oss.str()); 101 | } 102 | 103 | } // ------------------------------------------------------------------------ 104 | 105 | #define TF_THROW(...) tf::throw_se(__FILE__, __LINE__, __VA_ARGS__); 106 | 107 | -------------------------------------------------------------------------------- /pandar_pointcloud/launch/cloud_nodelet.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 59 | 60 | -------------------------------------------------------------------------------- /pandar_pointcloud/src/conversions/driver.h: -------------------------------------------------------------------------------- 1 | /* -*- mode: C++ -*- */ 2 | /* 3 | * Copyright (C) 2012 Austin Robot Technology, Jack O'Quin 4 | * Copyright (c) 2017 Hesai Photonics Technology Co., Ltd 5 | * 6 | * License: Modified BSD Software License Agreement 7 | * 8 | * $Id$ 9 | */ 10 | 11 | /** \file 12 | * 13 | * ROS driver interface for the pandar 3D LIDARs 14 | */ 15 | 16 | #ifndef _PANDAR_DRIVER_H_ 17 | #define _PANDAR_DRIVER_H_ 1 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include 26 | #include 27 | 28 | typedef struct PandarGPS_s PandarGPS; 29 | #define PANDAR128_READ_PACKET_SIZE (1800) 30 | #define PANDARQT128_READ_PACKET_SIZE (200) 31 | #define PANDAR80_READ_PACKET_SIZE (500) 32 | #define PANDAR90_READ_PACKET_SIZE (200) 33 | #define PANDAR64S_READ_PACKET_SIZE (400) 34 | #define PANDAR40S_READ_PACKET_SIZE (100) 35 | #define PANDAR_LASER_NUMBER_INDEX (6) 36 | #define PANDAR_MAJOR_VERSION_INDEX (2) 37 | 38 | namespace pandar_pointcloud { 39 | class Convert; 40 | 41 | class PandarDriver { 42 | public: 43 | PandarDriver(ros::NodeHandle node, ros::NodeHandle private_nh, 44 | std::string nodetype, pandar_pointcloud::Convert *convert); 45 | ~PandarDriver() {} 46 | 47 | bool poll(void); 48 | void publishRawData(); 49 | void setUdpVersion(uint8_t major, uint8_t minor); 50 | int getPandarScanArraySize(boost::shared_ptr); 51 | 52 | private: 53 | /// Callback for dynamic reconfigure 54 | void callback(pandar_pointcloud::CloudNodeConfig &config, uint32_t level); 55 | 56 | /// Pointer to dynamic reconfigure service srv_ 57 | boost::shared_ptr< 58 | dynamic_reconfigure::Server > 59 | srv_; 60 | 61 | // configuration parameters 62 | struct { 63 | std::string frame_id; ///< tf frame ID 64 | std::string model; ///< device model name 65 | int npackets; ///< number of packets to collect 66 | double rpm; ///< device rotation rate (RPMs) 67 | double time_offset; ///< time in seconds added to each pandar time stamp 68 | } config_; 69 | 70 | boost::shared_ptr m_spInput; 71 | ros::Publisher output_; 72 | ros::Publisher gpsoutput_; 73 | bool m_bNeedPublish; 74 | std::array pandarScanArray; 75 | int m_iScanPushIndex; 76 | int m_iScanPopIndex; 77 | pthread_mutex_t piclock; 78 | int m_iPandarScanArraySize; 79 | bool m_bGetScanArraySizeFlag; 80 | 81 | /** diagnostics updater */ 82 | diagnostic_updater::Updater diagnostics_; 83 | double diag_min_freq_; 84 | double diag_max_freq_; 85 | boost::shared_ptr diag_topic_; 86 | std::string publishmodel; 87 | std::string nodeType; 88 | 89 | pandar_pointcloud::Convert *convert; 90 | }; 91 | 92 | } // namespace pandar_driver 93 | 94 | #endif // _PANDAR_DRIVER_H_ 95 | -------------------------------------------------------------------------------- /pandar_pointcloud/params/QT128C2X_Firetimes.csv: -------------------------------------------------------------------------------- 1 | EEFF,1,1, 2 | Horizontal Resolution Mode,1,LoopNum,2 3 | Loop1,Firetime1,Loop2,Firetime2 4 | 99,0.6,65,0.6 5 | 65,1.456,99,1.456 6 | 35,2.312,1,2.312 7 | 102,3.768,72,3.768 8 | 72,4.624,102,4.624 9 | 38,5.48,8,5.48 10 | 107,6.936,73,6.936 11 | 73,7.792,107,7.792 12 | 43,8.648,9,8.648 13 | 110,10.104,80,10.104 14 | 80,10.96,110,10.96 15 | 46,11.816,16,11.816 16 | 115,13.272,81,13.272 17 | 81,14.128,115,14.128 18 | 51,14.984,17,14.984 19 | 118,16.44,88,16.44 20 | 88,17.296,118,17.296 21 | 54,18.152,24,18.152 22 | 123,19.608,89,19.608 23 | 89,20.464,123,20.464 24 | 59,21.32,25,21.32 25 | 126,22.776,96,22.776 26 | 96,23.632,126,23.632 27 | 62,24.488,32,24.488 28 | 97,25.944,67,25.944 29 | 67,26.8,97,26.8 30 | 33,27.656,3,27.656 31 | 104,29.112,70,29.112 32 | 70,29.968,104,29.968 33 | 40,30.824,6,30.824 34 | 105,32.28,75,32.28 35 | 75,33.136,105,33.136 36 | 41,33.992,11,33.992 37 | 112,35.448,78,35.448 38 | 78,36.304,112,36.304 39 | 48,37.16,14,37.16 40 | 113,38.616,83,38.616 41 | 83,39.472,113,39.472 42 | 49,40.328,19,40.328 43 | 120,41.784,86,41.784 44 | 86,42.64,120,42.64 45 | 56,43.496,22,43.496 46 | 121,44.952,91,44.952 47 | 91,45.808,121,45.808 48 | 57,46.664,27,46.664 49 | 128,48.12,94,48.12 50 | 94,48.976,128,48.976 51 | 64,49.832,30,49.832 52 | 98,51.288,68,51.288 53 | 68,52.144,98,52.144 54 | 34,53,4,53 55 | 103,54.456,69,54.456 56 | 69,55.312,103,55.312 57 | 39,56.168,5,56.168 58 | 106,57.624,76,57.624 59 | 76,58.48,106,58.48 60 | 42,59.336,12,59.336 61 | 111,60.792,77,60.792 62 | 77,61.648,111,61.648 63 | 47,62.504,13,62.504 64 | 114,63.96,84,63.96 65 | 84,64.816,114,64.816 66 | 50,65.672,20,65.672 67 | 119,67.128,85,67.128 68 | 85,67.984,119,67.984 69 | 55,68.84,21,68.84 70 | 122,70.296,92,70.296 71 | 92,71.152,122,71.152 72 | 58,72.008,28,72.008 73 | 127,73.464,93,73.464 74 | 93,74.32,127,74.32 75 | 63,75.176,29,75.176 76 | 100,76.632,66,76.632 77 | 66,77.488,100,77.488 78 | 36,78.344,2,78.344 79 | 101,79.8,71,79.8 80 | 71,80.656,101,80.656 81 | 37,81.512,7,81.512 82 | 108,82.968,74,82.968 83 | 74,83.824,108,83.824 84 | 44,84.68,10,84.68 85 | 109,86.136,79,86.136 86 | 79,86.992,109,86.992 87 | 45,87.848,15,87.848 88 | 116,89.304,82,89.304 89 | 82,90.16,116,90.16 90 | 52,91.016,18,91.016 91 | 117,92.472,87,92.472 92 | 87,93.328,117,93.328 93 | 53,94.184,23,94.184 94 | 124,95.64,90,95.64 95 | 90,96.496,124,96.496 96 | 60,97.352,26,97.352 97 | 125,98.808,95,98.808 98 | 95,99.664,125,99.664 99 | 61,100.52,31,100.52 100 | 0,0,0,0 101 | 0,0,0,0 102 | 0,0,0,0 103 | 0,0,0,0 104 | 0,0,0,0 105 | 0,0,0,0 106 | 0,0,0,0 107 | 0,0,0,0 108 | 0,0,0,0 109 | 0,0,0,0 110 | 0,0,0,0 111 | 0,0,0,0 112 | 0,0,0,0 113 | 0,0,0,0 114 | 0,0,0,0 115 | 0,0,0,0 116 | 0,0,0,0 117 | 0,0,0,0 118 | 0,0,0,0 119 | 0,0,0,0 120 | 0,0,0,0 121 | 0,0,0,0 122 | 0,0,0,0 123 | 0,0,0,0 124 | 0,0,0,0 125 | 0,0,0,0 126 | 0,0,0,0 127 | 0,0,0,0 128 | 0,0,0,0 129 | 0,0,0,0 130 | 0,0,0,0 131 | 0,0,0,0 132 | -------------------------------------------------------------------------------- /pandar_pointcloud/src/conversions/transform.h: -------------------------------------------------------------------------------- 1 | /* -*- mode: C++ -*- */ 2 | /* 3 | * Copyright (C) 2009, 2010 Austin Robot Technology, Jack O'Quin 4 | * Copyright (C) 2011 Jesse Vera 5 | * Copyright (C) 2012 Austin Robot Technology, Jack O'Quin 6 | * Copyright (c) 2017 Hesai Photonics Technology, Yang Sheng 7 | * Copyright (c) 2020 Hesai Photonics Technology, Lingwen Fang 8 | * License: Modified BSD Software License Agreement 9 | * 10 | * $Id$ 11 | */ 12 | 13 | /** @file 14 | 15 | This class transforms raw Pandar128 3D LIDAR packets to PointCloud2 16 | in the /odom frame of reference. 17 | 18 | */ 19 | 20 | #ifndef _PANDAR_POINTCLOUD_TRANSFORM_H_ 21 | #define _PANDAR_POINTCLOUD_TRANSFORM_H_ 1 22 | 23 | #include 24 | #include "tf/message_filter.h" 25 | #include "message_filters/subscriber.h" 26 | #include 27 | 28 | #include 29 | #include 30 | 31 | #include 32 | #include 33 | 34 | // include template implementations to transform a custom point cloud 35 | #include 36 | #include "convert.h" 37 | 38 | /** types of point and cloud to work with */ 39 | typedef pandar_rawdata::PPoint PPoint; 40 | typedef pandar_rawdata::PPointCloud PPointCloud; 41 | 42 | // instantiate template for transforming a PPointCloud 43 | template bool 44 | pcl_ros::transformPointCloud(const std::string &, 45 | const PPointCloud &, 46 | PPointCloud &, 47 | const tf::TransformListener &); 48 | 49 | namespace pandar_pointcloud 50 | { 51 | class Transform 52 | { 53 | public: 54 | 55 | Transform(ros::NodeHandle node, ros::NodeHandle private_nh); 56 | ~Transform() {} 57 | 58 | private: 59 | 60 | void processScan(const pandar_msgs::PandarScan::ConstPtr &scanMsg); 61 | 62 | ///Pointer to dynamic reconfigure service srv_ 63 | boost::shared_ptr > srv_; 65 | void reconfigure_callback(pandar_pointcloud::TransformNodeConfig &config, 66 | uint32_t level); 67 | 68 | const std::string tf_prefix_; 69 | boost::shared_ptr data_; 70 | boost::shared_ptr m_spConver; 71 | message_filters::Subscriber pandar_scan_; 72 | tf::MessageFilter *tf_filter_; 73 | ros::Publisher output_; 74 | tf::TransformListener listener_; 75 | 76 | /// configuration parameters 77 | typedef struct { 78 | std::string frame_id; ///< target frame ID 79 | } Config; 80 | Config config_; 81 | 82 | // Point cloud buffers for collecting points within a packet. The 83 | // inPc_ and tfPc_ are class members only to avoid reallocation on 84 | // every message. 85 | PPointCloud inPc_; ///< input packet point cloud 86 | PPointCloud tfPc_; ///< transformed packet point cloud 87 | }; 88 | 89 | } // namespace pandar_pointcloud 90 | 91 | #endif // _PANDAR_POINTCLOUD_TRANSFORM_H_ 92 | -------------------------------------------------------------------------------- /pandar_pointcloud/params/Pandar128_Correction.csv: -------------------------------------------------------------------------------- 1 | Laser id,Elevation,Azimuth 2 | 1,14.436,3.257 3 | 2,13.535,3.263 4 | 3,13.081786,1.091414 5 | 4,12.624,3.268 6 | 5,12.165246,1.092504 7 | 6,11.702,3.273 8 | 7,11.238522,1.093621 9 | 8,10.771,3.278 10 | 9,10.305007,1.094766 11 | 10,9.83,3.283 12 | 11,9.356123,1.095941 13 | 12,8.88,3.288 14 | 13,8.401321,1.097146 15 | 14,7.921,3.291 16 | 15,7.43808,1.098384 17 | 16,6.952581,-1.101114 18 | 17,6.466905,1.099655 19 | 18,5.977753,-1.103649 20 | 19,5.487,-3.306 21 | 20,4.995801,-1.106118 22 | 21,4.501,-3.311 23 | 22,4.007293,-1.108519 24 | 23,3.509,-3.318 25 | 24,3.012822,-1.110852 26 | 25,2.512,-3.324 27 | 26,2.013,-1.113115 28 | 27,1.885,7.72 29 | 28,1.761,5.535 30 | 29,1.637,3.325 31 | 30,1.511,-3.33 32 | 31,1.385875,1.10673 33 | 32,1.2582,-5.53803493333333 34 | 33,1.13,-7.72610724761905 35 | 34,1.008459,-1.115309 36 | 35,0.88,7.731 37 | 36,0.756,5.543 38 | 37,0.63,3.329 39 | 38,0.505,-3.336 40 | 39,0.378591,1.108227 41 | 40,0.251,-5.54681313333333 42 | 41,0.124,-7.73821493333333 43 | 42,-0.00015,-1.117431 44 | 43,-0.129,7.743 45 | 44,-0.2541,5.550783 46 | 45,-0.38,3.335 47 | 46,-0.5061259,-3.341759 48 | 47,-0.63235,1.109762 49 | 48,-0.7597898,-5.555361 50 | 49,-0.8872418,-7.750039 51 | 50,-1.012168,-1.119482 52 | 51,-1.141,7.757 53 | 52,-1.2662,5.559905 54 | 53,-1.393,3.34 55 | 54,-1.519337,-3.347338 56 | 55,-1.646275,1.111338 57 | 56,-1.773301,-5.564415 58 | 57,-1.900587,-7.762486 59 | 58,-2.026912,-1.12146 60 | 59,-2.155,7.768 61 | 60,-2.2815,5.568891 62 | 61,-2.409,3.345 63 | 62,-2.534932,-3.352812 64 | 63,-2.662501,1.112953 65 | 64,-2.789024,-5.573332 66 | 65,-2.916055,-7.774765 67 | 66,-3.043698,-1.123366 68 | 67,-3.172,7.78 69 | 68,-3.299,5.577739 70 | 69,-3.425,3.351 71 | 70,-3.552222,-3.358181 72 | 71,-3.680335,1.114609 73 | 72,-3.806265,-5.582111 74 | 73,-3.932954,-7.786874 75 | 74,-4.06183,-1.125199 76 | 75,-4.19,7.792 77 | 76,-4.318,5.586449 78 | 77,-4.444,3.356 79 | 78,-4.570508,-3.363443 80 | 79,-4.699079,1.116305 81 | 80,-4.824327,-5.590751 82 | 81,-4.950584,-7.798811 83 | 82,-5.080608,-1.126958 84 | 83,-5.209,7.804 85 | 84,-5.336,5.595019 86 | 85,-5.463,3.36 87 | 86,-5.589088,-3.368599 88 | 87,-5.718031,1.118042 89 | 88,-5.842508,-5.599251 90 | 89,-5.968246,-7.810576 91 | 90,-6.099661,-1.128643 92 | 91,-6.607262,-3.373647 93 | 92,-7.117295,-1.130255 94 | 93,-7.624327,-3.378587 95 | 94,-8.133802,-1.131792 96 | 95,-8.639587,-3.383419 97 | 96,-9.149,3.381 98 | 97,-9.652353,-3.388143 99 | 98,-10.16,3.386 100 | 99,-10.665443,1.127077 101 | 100,-11.17,3.39 102 | 101,-11.671568,1.129045 103 | 102,-12.174,3.395 104 | 103,-12.673194,1.131048 105 | 104,-13.173,3.401 106 | 105,-13.669682,1.133088 107 | 106,-14.166,3.406 108 | 107,-14.660411,1.135163 109 | 108,-15.154,3.41 110 | 109,-15.644783,1.137272 111 | 110,-16.135,3.416 112 | 111,-16.622221,1.139412 113 | 112,-17.106088,-1.142319 114 | 113,-17.592171,1.141584 115 | 114,-18.071976,-1.143128 116 | 115,-18.54765,-3.425708 117 | 116,-19.029597,-1.143867 118 | 117,-19.50071,-3.429329 119 | 118,-19.978461,-1.144538 120 | 119,-20.44479,-3.432841 121 | 120,-20.918108,-1.14514 122 | 121,-21.37943,-3.436242 123 | 122,-21.848107,-1.145675 124 | 123,-22.30422,-3.439533 125 | 124,-22.768055,-1.146145 126 | 125,-23.21878,-3.442714 127 | 126,-23.677577,-1.146549 128 | 127,-24.12274,-3.445786 129 | 128,-25.01577,-3.448749 -------------------------------------------------------------------------------- /pandar_pointcloud/launch/PandarSwift_points.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 65 | 66 | 67 | -------------------------------------------------------------------------------- /pandar_pointcloud/src/conversions/transform.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2009, 2010 Austin Robot Technology, Jack O'Quin 3 | * Copyright (C) 2011 Jesse Vera 4 | * Copyright (C) 2012 Austin Robot Technology, Jack O'Quin 5 | * Copyright (c) 2017 Hesai Photonics Technology, Yang Sheng 6 | * Copyright (c) 2020 Hesai Photonics Technology, Lingwen Fang 7 | * License: Modified BSD Software License Agreement 8 | * 9 | * $Id$ 10 | */ 11 | 12 | /** @file 13 | 14 | This class transforms raw Pandar128 3D LIDAR packets to PointCloud2 15 | in the /odom frame of reference. 16 | 17 | @author Jack O'Quin 18 | @author Jesse Vera 19 | @author Yang Sheng 20 | 21 | */ 22 | 23 | #include "transform.h" 24 | 25 | #include 26 | 27 | namespace pandar_pointcloud 28 | { 29 | /** @brief Constructor. */ 30 | Transform::Transform(ros::NodeHandle node, ros::NodeHandle private_nh): 31 | tf_prefix_(tf::getPrefixParam(private_nh)), 32 | data_(new pandar_rawdata::RawData()), 33 | m_spConver(new Convert(node, private_nh,"transform")) 34 | { 35 | ROS_WARN(" Transform::Transform"); 36 | private_nh.getParam("frame_id", config_.frame_id); 37 | // Read calibration. 38 | data_->setup(private_nh); 39 | 40 | // advertise output point cloud (before subscribing to input data) 41 | // output_ = 42 | // node.advertise("transform_pandar_points", 10); 43 | 44 | srv_ = boost::make_shared > (private_nh); 46 | dynamic_reconfigure::Server:: 47 | CallbackType f; 48 | f = boost::bind (&Transform::reconfigure_callback, this, _1, _2); 49 | srv_->setCallback (f); 50 | 51 | // subscribe to PandarScan packets using transform filter 52 | 53 | pandar_scan_.subscribe(node, "pandar_packets", 10); 54 | tf_filter_ = 55 | new tf::MessageFilter(pandar_scan_, 56 | listener_, 57 | config_.frame_id, 10); 58 | tf_filter_->registerCallback(boost::bind(&Transform::processScan, this, _1)); 59 | ROS_WARN(" Transform::processScan config[%s]",config_.frame_id.c_str()); 60 | ROS_WARN(" Transform::Transform finisher"); 61 | } 62 | 63 | void Transform::reconfigure_callback( 64 | pandar_pointcloud::TransformNodeConfig &config, uint32_t level) 65 | { 66 | ROS_INFO_STREAM("Reconfigure request."); 67 | data_->setParameters(config.min_range, config.max_range, 68 | config.view_direction, config.view_width); 69 | config_.frame_id = tf::resolve(tf_prefix_, config.frame_id); 70 | ROS_INFO_STREAM("Target frame ID: " << config_.frame_id); 71 | } 72 | 73 | /** @brief Callback for raw scan messages. 74 | * 75 | * @pre TF message filter has already waited until the transform to 76 | * the configured @c frame_id can succeed. 77 | */ 78 | void 79 | Transform::processScan(const pandar_msgs::PandarScan::ConstPtr &scanMsg) 80 | { 81 | // process each packet provided by the driver 82 | // ROS_WARN(" Transform::processScan"); 83 | for (size_t next = 0; next < scanMsg->packets.size(); ++next) { 84 | m_spConver->pushLiDARData(scanMsg->packets[next]); 85 | } 86 | } 87 | } // namespace pandar_pointcloud 88 | -------------------------------------------------------------------------------- /pandar_pointcloud/include/pandar_pointcloud/point_types.h: -------------------------------------------------------------------------------- 1 | /* -*- mode: C++ -*- 2 | * 3 | * Copyright (C) 2011, 2012 Austin Robot Technology 4 | * Copyright (c) 2017 Hesai Photonics Technology, Yang Sheng 5 | * Copyright (c) 2020 Hesai Photonics Technology, Lingwen Fang 6 | * 7 | * License: Modified BSD Software License Agreement 8 | * 9 | * $Id: data_base.h 1554 2011-06-14 22:11:17Z jack.oquin $ 10 | */ 11 | 12 | /** \file 13 | * 14 | * Point Cloud Library point structures for Velodyne data. 15 | * 16 | * @author Jesse Vera 17 | * @author Jack O'Quin 18 | * @author Piyush Khandelwal 19 | * @author Yang Sheng 20 | */ 21 | 22 | #ifndef __PANDAR_POINTCLOUD_POINT_TYPES_H 23 | #define __PANDAR_POINTCLOUD_POINT_TYPES_H 24 | 25 | #include 26 | 27 | namespace pandar_pointcloud 28 | { 29 | /** Euclidean Pandar128 coordinate, including intensity and ring number. */ 30 | struct PointXYZIR 31 | { 32 | PCL_ADD_POINT4D; // quad-word XYZ 33 | float intensity; ///< laser intensity reading 34 | uint16_t ring; ///< laser ring number 35 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // ensure proper alignment 36 | } EIGEN_ALIGN16; 37 | 38 | 39 | struct PointXYZIT { 40 | PCL_ADD_POINT4D 41 | uint8_t intensity; 42 | double timestamp; 43 | uint16_t ring; ///< laser ring number 44 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned 45 | } EIGEN_ALIGN16; 46 | // enforce SSE padding for correct memory alignment 47 | 48 | struct PointXYZITd { 49 | double x; 50 | double y; 51 | double z; 52 | uint8_t intensity; 53 | double timestamp; 54 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned 55 | } EIGEN_ALIGN16; 56 | // enforce SSE padding for correct memory alignment 57 | 58 | struct PointXYZd { 59 | double x; 60 | double y; 61 | double z; 62 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned 63 | } EIGEN_ALIGN16; 64 | 65 | struct PointXYZRGBd { 66 | double x; 67 | double y; 68 | double z; 69 | PCL_ADD_RGB 70 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned 71 | } EIGEN_ALIGN16; 72 | }; // namespace pandar_pointcloud 73 | 74 | 75 | 76 | 77 | 78 | POINT_CLOUD_REGISTER_POINT_STRUCT(pandar_pointcloud::PointXYZIR, 79 | (float, x, x) 80 | (float, y, y) 81 | (float, z, z) 82 | (float, intensity, intensity) 83 | (uint16_t, ring, ring)) 84 | 85 | POINT_CLOUD_REGISTER_POINT_STRUCT(pandar_pointcloud::PointXYZIT, 86 | (float, x, x)(float, y, y)(float, z, z) 87 | (uint8_t, intensity, intensity)(double, timestamp, timestamp)(uint16_t, ring, ring)) 88 | 89 | POINT_CLOUD_REGISTER_POINT_STRUCT(pandar_pointcloud::PointXYZITd, 90 | (double, x, x)(double, y, y)(double, z, z)(uint8_t, intensity, intensity)(double, timestamp, 91 | timestamp)) 92 | 93 | POINT_CLOUD_REGISTER_POINT_STRUCT(pandar_pointcloud::PointXYZd, 94 | (double, x, x)(double, y, y)(double, z, z)) 95 | POINT_CLOUD_REGISTER_POINT_STRUCT(pandar_pointcloud::PointXYZRGBd, 96 | (double, x, x)(double, y, y)(double, z, z)(float, rgb, rgb)) 97 | #endif // __PANDAR_POINTCLOUD_POINT_TYPES_H 98 | 99 | -------------------------------------------------------------------------------- /change notes.md: -------------------------------------------------------------------------------- 1 | # HesaiLidar_Swift_ROS 2 | 3 | 星期二, 13. 十月 2020 01:25下午 4 | ##version 5 | Pandar128_1.2.1 6 | 7 | ##modify 8 | 1. add version 9 | 2. add the function of read angle configuration from lidar 10 | 3. revise correction.csv file 11 | 4. modify judgment of PandarDriver::poll 12 | 5. fix points clould blink when lidar's mode is last return/standard 13 | 14 | 星期一, 07. 十二月 2020 23:25下午 15 | ##version 16 | Pandar128_1.0.2 17 | 18 | ##modify 19 | 1. fix timestamp is zero 20 | 21 | 星期四, 10. 十二月 2020 19:25下午 22 | ##version 23 | Pandar128_1.0.3 24 | 25 | ##modify 26 | 1. Delete unnecessary msg files 27 | 2. Modify the launch file 28 | 3. Fixed lidar speed reading problem 29 | 4. Increase the output log of certificate authentication results 30 | 5. Fix pcap file reading 31 | 32 | 星期五, 18. 十二月 2020 14:05下午 33 | ##version 34 | Pandar128_1.0.4 35 | 36 | ##modify 37 | 1. Fix the problem that the cloud points data is put in wrong place in dual model 38 | 2. Change the size of rosbag 39 | 40 | 41 | 星期三, 23. 十二月 2020 19:30下午 42 | ##version 43 | Pandar128_1.0.6 44 | 45 | ##modify 46 | 1. Fix that all point cloud values have timestamp and ring 47 | 2. Fix that sdk will core dumped when input wrong correction file path 48 | 49 | 星期一, 01. 二月 2021 17:30下午 50 | ##version 51 | Pandar128_1.0.7 52 | 53 | ##modify 54 | 1. Change readme and branch name 55 | 56 | 星期二, 04. 二月 2021 17:30下午 57 | ##version 58 | PandarSwift_1.0.8 59 | 60 | ##modify 61 | 1. Support to parser packet with UDP protocol v1.4 62 | 63 | 星期一, 08. 二月 2021 10:30下午 64 | ##version 65 | PandarSwift_1.0.9 66 | 67 | ##modify 68 | 1. Fix bug in different rpm 69 | 70 | 星期四, 04. 三月 2021 17:30下午 71 | ##version 72 | PandarSwift_1.0.10 73 | 74 | ##modify 75 | 1. Fix bug in parser half packets 76 | 77 | Wednesday, April 14th, 2021 17:30 78 | ##version 79 | PandarSwiftROS_1.0.15 80 | 81 | ##modify 82 | 1. Add namespace parmeter for topic 83 | 2. Optimize calculation efficiency 84 | 3. Support QT128 85 | 4. Fix bug in calculation firetime correction 86 | 87 | Monday, April 26th, 2021 20:30 88 | ##version 89 | PandarSwiftROS_1.0.16 90 | 91 | ##modify 92 | 1. Fix bug in calculate cosAllAngle and sinAllAngle 93 | 94 | Thursday, May 20th, 2021 17:30 95 | ##version 96 | PandarSwiftROS_1.0.17 97 | 98 | ##modify 99 | 1. Support ubuntu 20.04 100 | 2. Update firetime correction of QT128 101 | 102 | Friday, July 23th, 2021 17:30 103 | ##version 104 | PandarSwiftROS_1.0.20 105 | 106 | ##modify 107 | 1. Support coordinate correction of QT128 108 | 109 | Friday, August 24th, 2021 17:30 110 | ##version 111 | PandarSwiftROS_1.0.22 112 | 113 | ##modify 114 | 1. Support to parser UDP1.3 packet when connect to lidar 115 | 2. Fix bug that parser gps packet as point cloud packet 116 | 3. Fix bug in calculate taskflow end 117 | 118 | 119 | Friday, November 12th, 2021 17:30 120 | ##version 121 | PandarSwiftROS_1.0.25 122 | 123 | ##modify 124 | 1. Support BD90 first time 125 | 126 | Sunday, May 22th, 2022 17:30 127 | ##version 128 | PandarSwiftROS_1.0.30 129 | 130 | ##modify 131 | 1. Support self-define QT128 132 | 2. Support to parser weight factor and env light info in Udp1.4 133 | 134 | Wednesday June 1th, 2022 17:30 135 | ##version 136 | PandarSwiftROS_1.0.31 137 | 138 | ##modify 139 | 1. Fix bug in publish points mode 140 | 2. Support 0x3a and 0x3e return mode of QT128 141 | 142 | Monday December 5th, 2022 17:30 143 | ##version 144 | PandarSwiftROS_1.0.36 145 | 146 | ##modify 147 | 1. Support multicast 148 | 149 | Wednesday December 14th, 2022 17:30 150 | ##version 151 | PandarSwiftROS_1.0.37 152 | 153 | ##modify 154 | 1. Support save pcd 155 | 156 | 157 | 158 | -------------------------------------------------------------------------------- /pandar_pointcloud/params/QT128C2X_Correction.csv: -------------------------------------------------------------------------------- 1 | Laser id,Elevation,Azimuth 2 | 1,-52.62676282,10.10830596 3 | 2,-51.0280939,9.719503673 4 | 3,-49.51495392,9.384265827 5 | 4,-48.07394795,9.091433335 6 | 5,-46.69466297,8.832899657 7 | 6,-45.36882812,8.602605729 8 | 7,-44.08974439,8.395920495 9 | 8,-42.85190128,8.209210679 10 | 9,-41.65069769,8.03961095 11 | 10,-40.48225401,7.8848012 12 | 11,-39.34326358,7.742887867 13 | 12,-38.23088092,7.612311759 14 | 13,-37.14263654,7.491771683 15 | 14,-36.07637316,7.380164213 16 | 15,-35.03019446,7.276562397 17 | 16,-34.00242585,7.180171603 18 | 17,-32.99157758,7.090302002 19 | 18,-31.99631045,7.006369095 20 | 19,-31.01543179,6.927848973 21 | 20,-30.04786472,6.854295551 22 | 21,-29.09262707,6.785306938 23 | 22,-28.14883195,6.720536921 24 | 23,-27.21566989,6.659666871 25 | 24,-26.29240356,6.602428464 26 | 25,-25.37835081,6.548569874 27 | 26,-24.47288318,6.497872808 28 | 27,-23.57542315,6.450129948 29 | 28,-22.68544071,6.405178951 30 | 29,-21.80243688,6.362840261 31 | 30,-20.92594329,6.32298504 32 | 31,-20.05553602,6.285467298 33 | 32,-19.19081372,6.25017485 34 | 33,-18.33139172,-6.21699538 35 | 34,-17.47691901,-6.18583344 36 | 35,-16.62705442,-6.156599148 37 | 36,-15.78149317,-6.129208192 38 | 37,-14.93993032,-6.103581848 39 | 38,-14.10208035,-6.079663981 40 | 39,-13.26767188,-6.057381402 41 | 40,-12.43645574,-6.036683527 42 | 41,-11.60818201,-6.017519726 43 | 42,-10.78261371,-5.999833659 44 | 43,-9.959523504,-5.983597281 45 | 44,-9.138697037,-5.968771181 46 | 45,-8.319929599,-5.955310246 47 | 46,-7.503005408,-5.943192006 48 | 47,-6.687807162,-5.926719867 49 | 48,-5.873926875,-5.922876604 50 | 49,-5.061393513,-5.914628713 51 | 50,-4.249948322,-5.907639071 52 | 51,-3.43941536,-5.901885097 53 | 52,-2.629617595,-5.897355529 54 | 53,-1.820378325,-5.894039091 55 | 54,-1.011522452,-5.891935825 56 | 55,-0.202883525,-5.89103442 57 | 56,0.605717878,-5.89132922 58 | 57,1.414444374,-5.892831558 59 | 58,2.223469054,-5.895541414 60 | 59,3.032962884,-5.899458754 61 | 60,3.8431104,-5.904600534 62 | 61,4.654074511,-5.910966684 63 | 62,5.466034723,-5.918579795 64 | 63,6.279180222,-5.927451101 65 | 64,7.093696848,-5.937608821 66 | 65,7.909760474,5.949064148 67 | 66,8.727579858,5.961845258 68 | 67,9.54735011,5.97598597 69 | 68,10.36927364,5.99150874 70 | 69,11.19356846,6.008464331 71 | 70,12.0204614,6.026875139 72 | 71,12.85016831,6.046797524 73 | 72,13.68294669,6.068282136 74 | 73,14.51903314,6.091373918 75 | 74,15.35869922,6.116134758 76 | 75,16.20222125,6.142626487 77 | 76,17.04988688,6.170927865 78 | 77,17.90200196,6.201111915 79 | 78,18.75890049,6.233268566 80 | 79,19.62091911,6.267487655 81 | 80,20.48842178,6.303870236 82 | 81,21.36180725,6.342539884 83 | 82,22.24149217,6.383614369 84 | 83,23.12791172,6.427233938 85 | 84,24.02155359,6.473555624 86 | 85,24.92293181,6.52274755 87 | 86,25.83260224,6.574994555 88 | 87,26.75116558,6.63050945 89 | 88,27.67927338,6.689521655 90 | 89,28.61763387,6.752299732 91 | 90,29.56702472,6.819128697 92 | 91,30.52828971,6.890343815 93 | 92,31.50236238,6.966319159 94 | 93,32.49026442,7.0474731 95 | 94,33.49313666,7.134285032 96 | 95,34.51223682,7.227334588 97 | 96,35.54897914,7.327222327 98 | 97,19.19081372,-6.25017485 99 | 98,20.05553602,-6.285467298 100 | 99,20.92594329,-6.32298504 101 | 100,21.80243688,-6.362840261 102 | 101,22.68544071,-6.405178951 103 | 102,23.57542315,-6.450129948 104 | 103,24.47288318,-6.497872808 105 | 104,25.37835081,-6.548569874 106 | 105,26.29240356,-6.602428464 107 | 106,27.21566989,-6.659666871 108 | 107,28.14883195,-6.720536921 109 | 108,29.09262707,-6.785306938 110 | 109,30.04786472,-6.854295551 111 | 110,31.01543179,-6.927848973 112 | 111,31.99631045,-7.006369095 113 | 112,32.99157758,-7.090302002 114 | 113,34.00242585,-7.180171603 115 | 114,35.03019446,-7.276562397 116 | 115,36.07637316,-7.380164213 117 | 116,37.14263654,-7.491771683 118 | 117,38.23088092,-7.612311759 119 | 118,39.34326358,-7.742887867 120 | 119,40.48225401,-7.8848012 121 | 120,41.65069769,-8.03961095 122 | 121,42.85190128,-8.209210679 123 | 122,44.08974439,-8.395920495 124 | 123,45.36882812,-8.602605729 125 | 124,46.69466297,-8.832899657 126 | 125,48.07394795,-9.091433335 127 | 126,49.51495392,-9.384265827 128 | 127,51.0280939,-9.719503673 129 | 128,52.62676282,-10.10830596 130 | -------------------------------------------------------------------------------- /pandar_pointcloud/src/taskflow/utility/backoff.hpp: -------------------------------------------------------------------------------- 1 | // 2019/04/15 - created by Tsung-Wei Huang 2 | // - modified from boost/fiber/cpu_relax.hpp 3 | 4 | #pragma once 5 | 6 | #include 7 | #include 8 | 9 | #include "../predef/compiler.hpp" 10 | #include "../predef/architecture.hpp" 11 | 12 | #if TF_COMP_MSVC || TF_COMP_MSVC_EMULATED 13 | # define NOMINMAX 14 | # include 15 | #endif 16 | 17 | namespace tf { 18 | 19 | #if TF_ARCH_ARM 20 | # if TF_COMP_MSVC 21 | # define TF_PAUSE() YieldProcessor(); 22 | # elif (defined(__ARM_ARCH_6K__) || \ 23 | defined(__ARM_ARCH_6Z__) || \ 24 | defined(__ARM_ARCH_6ZK__) || \ 25 | defined(__ARM_ARCH_6T2__) || \ 26 | defined(__ARM_ARCH_7__) || \ 27 | defined(__ARM_ARCH_7A__) || \ 28 | defined(__ARM_ARCH_7R__) || \ 29 | defined(__ARM_ARCH_7M__) || \ 30 | defined(__ARM_ARCH_7S__) || \ 31 | defined(__ARM_ARCH_8A__) || \ 32 | defined(__aarch64__)) 33 | // http://groups.google.com/a/chromium.org/forum/#!msg/chromium-dev/YGVrZbxYOlU/Vpgy__zeBQAJ 34 | // mnemonic 'yield' is supported from ARMv6k onwards 35 | # define TF_PAUSE() asm volatile ("yield" ::: "memory"); 36 | # else 37 | # define TF_PAUSE() asm volatile ("nop" ::: "memory"); 38 | # endif 39 | #elif TF_ARCH_MIPS 40 | # define TF_PAUSE() asm volatile ("pause" ::: "memory"); 41 | #elif TF_ARCH_PPC 42 | // http://code.metager.de/source/xref/gnu/glibc/sysdeps/powerpc/sys/platform/ppc.h 43 | // http://stackoverflow.com/questions/5425506/equivalent-of-x86-pause-instruction-for-ppc 44 | // mnemonic 'or' shared resource hints 45 | // or 27, 27, 27 This form of 'or' provides a hint that performance 46 | // will probably be imrpoved if shared resources dedicated 47 | // to the executing processor are released for use by other 48 | // processors 49 | // extended mnemonics (available with POWER7) 50 | // yield == or 27, 27, 27 51 | # define TF_PAUSE() asm volatile ("or 27,27,27" ::: "memory"); 52 | #elif TF_ARCH_X86 53 | # if TF_COMP_MSVC || TF_COMP_MSVC_EMULATED 54 | # define TF_PAUSE() YieldProcessor(); 55 | # else 56 | # define TF_PAUSE() asm volatile ("pause" ::: "memory"); 57 | # endif 58 | #else 59 | # define TF_PAUSE() { \ 60 | static constexpr std::chrono::microseconds us0{ 0 }; \ 61 | std::this_thread::sleep_for(us0); \ 62 | } 63 | #endif 64 | 65 | // ------------------------------------------------------------------ 66 | 67 | // Procedure: relax_cpu 68 | // pause cpu for a few rounds 69 | inline void relax_cpu(int32_t cycles) { 70 | while(cycles > 0) { 71 | TF_PAUSE(); 72 | cycles--; 73 | } 74 | } 75 | 76 | // Procedure: relax_cpu 77 | inline void relax_cpu() { 78 | TF_PAUSE(); 79 | } 80 | 81 | // ------------------------------------------------------------------ 82 | 83 | // Class that implements the exponential backoff 84 | class ExponentialBackoff { 85 | 86 | static constexpr int LOOPS_BEFORE_YIELD = 16; 87 | 88 | public: 89 | 90 | ExponentialBackoff() = default; 91 | 92 | void backoff() { 93 | if(_count <= LOOPS_BEFORE_YIELD) { 94 | relax_cpu(_count); 95 | _count = _count << 1; 96 | } 97 | else { 98 | std::this_thread::yield(); 99 | } 100 | } 101 | 102 | // pause for a few times and return false if saturated 103 | bool bounded_pause() { 104 | relax_cpu(_count); 105 | if(_count < LOOPS_BEFORE_YIELD) { 106 | _count = _count << 1; 107 | return true; 108 | } else { 109 | return false; 110 | } 111 | } 112 | 113 | void reset() { 114 | _count = 1; 115 | } 116 | 117 | private: 118 | 119 | int _count {1}; 120 | 121 | }; 122 | 123 | // Class that implements the exponential backoff 124 | class LinearBackoff { 125 | 126 | public: 127 | 128 | LinearBackoff() = default; 129 | 130 | void backoff() { 131 | if(_count <= 16) { 132 | relax_cpu(_count); 133 | ++_count; 134 | } 135 | else { 136 | std::this_thread::yield(); 137 | } 138 | } 139 | 140 | void reset() { 141 | _count = 1; 142 | } 143 | 144 | private: 145 | 146 | int32_t _count {1}; 147 | 148 | }; 149 | 150 | }; // end of namespace tf. --------------------------------------------------- 151 | 152 | -------------------------------------------------------------------------------- /pandar_pointcloud/src/lib/calibration.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * \file calibration.cc 3 | * \brief 4 | * 5 | * \author Piyush Khandelwal (piyushk@cs.utexas.edu) 6 | * \author Yang Sheng 7 | * Copyright (C) 2012, Austin Robot Technology, 8 | * The University of Texas at Austin 9 | * Copyright (c) 2017 Hesai Photonics Technology, Yang Sheng 10 | * Copyright (c) 2020 Hesai Photonics Technology, Lingwen Fang 11 | * 12 | * License: Modified BSD License 13 | * 14 | * $ Id: 02/14/2012 11:36:36 AM piyushk $ 15 | */ 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | namespace pandar_pointcloud 28 | { 29 | 30 | // elevation angle of each line for HS Line 40 Lidar, Line 1 - Line 40 31 | static const float hesai_elev_angle_map[] = { 32 | 6.96, 5.976, 4.988, 3.996, 33 | 2.999, 2.001, 1.667, 1.333, 34 | 1.001, 0.667, 0.333, 0, 35 | -0.334, -0.667, -1.001, -1.334, 36 | -1.667, -2.001, -2.331, -2.667, 37 | -3, -3.327, -3.663, -3.996, 38 | -4.321, -4.657, -4.986, -5.311, 39 | -5.647, -5.974, -6.957, -7.934, 40 | -8.908, -9.871, -10.826, -11.772, 41 | -12.705, -13.63, -14.543, -15.444 42 | }; 43 | 44 | // Line 40 Lidar azimuth Horizatal offset , Line 1 - Line 40 45 | static const float hesai_horizatal_azimuth_offset_map[] = { 46 | 0.005, 0.006, 0.006, 0.006, 47 | -2.479, -2.479, 2.491, -4.953, 48 | -2.479, 2.492, -4.953, -2.479, 49 | 2.492, -4.953, 0.007, 2.491, 50 | -4.953, 0.006, 4.961, -2.479, 51 | 0.006, 4.96, -2.478, 0.006, 52 | 4.958, -2.478, 2.488, 4.956, 53 | -2.477, 2.487, 2.485, 2.483, 54 | 0.004, 0.004, 0.003, 0.003, 55 | -2.466, -2.463, -2.46, -2.457 56 | }; 57 | 58 | void Calibration::setDefaultCorrections () 59 | { 60 | for (int i = 0; i < laser_count; i++) 61 | { 62 | laser_corrections[i].azimuthCorrection = hesai_horizatal_azimuth_offset_map[i]; 63 | laser_corrections[i].distanceCorrection = 0.0; 64 | laser_corrections[i].horizontalOffsetCorrection = 0.0; 65 | laser_corrections[i].verticalOffsetCorrection = 0.0; 66 | laser_corrections[i].verticalCorrection = hesai_elev_angle_map[i]; 67 | laser_corrections[i].sinVertCorrection = std::sin (hesai_elev_angle_map[i] / 180.0 * M_PI); 68 | laser_corrections[i].cosVertCorrection = std::cos (hesai_elev_angle_map[i] / 180.0 * M_PI); 69 | } 70 | } 71 | 72 | void Calibration::read(const std::string& calibration_file) 73 | { 74 | initialized = true; 75 | num_lasers = laser_count; 76 | setDefaultCorrections (); 77 | if (calibration_file.empty ()) 78 | return; 79 | 80 | boost::filesystem::path file_path(calibration_file); 81 | if (!boost::filesystem::is_regular(file_path)) { 82 | file_path = boost::filesystem::path("Pandar128.csv"); 83 | if (!boost::filesystem::is_regular(file_path)) 84 | return; 85 | } 86 | 87 | std::ifstream ifs(file_path.string()); 88 | if (!ifs.is_open()) 89 | return; 90 | 91 | std::string line; 92 | if (std::getline(ifs, line)) { // first line "Laser id,Elevation,Azimuth" 93 | std::cout << "parsing correction file." << std::endl; 94 | } 95 | 96 | int line_cnt = 0; 97 | while (std::getline(ifs, line)) { 98 | if (line_cnt++ >= 40) 99 | break; 100 | 101 | int line_id = 0; 102 | double elev, azimuth; 103 | 104 | std::stringstream ss(line); 105 | std::string subline; 106 | std::getline(ss, subline, ','); 107 | std::stringstream(subline) >> line_id; 108 | std::getline(ss, subline, ','); 109 | std::stringstream(subline) >> elev; 110 | std::getline(ss, subline, ','); 111 | std::stringstream(subline) >> azimuth; 112 | line_id--; 113 | laser_corrections[line_id].azimuthCorrection = azimuth; 114 | laser_corrections[line_id].distanceCorrection = 0.0; 115 | laser_corrections[line_id].horizontalOffsetCorrection = 0.0; 116 | laser_corrections[line_id].verticalOffsetCorrection = 0.0; 117 | laser_corrections[line_id].verticalCorrection = elev; 118 | laser_corrections[line_id].sinVertCorrection = std::sin (angles::from_degrees(elev)); 119 | laser_corrections[line_id].cosVertCorrection = std::cos (angles::from_degrees(elev)); 120 | } 121 | } 122 | 123 | void Calibration::write(const std::string& calibration_file) { 124 | } 125 | 126 | } /* pandar_pointcloud */ 127 | -------------------------------------------------------------------------------- /pandar_pointcloud/src/conversions/util.c: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2018 The Hesai Technology Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | #include "util.h" 37 | 38 | #define DEFAULT_TIMEOUT 10 /*secondes waitting for read/write*/ 39 | 40 | int sys_readn(int fd, void* vptr, int n) { 41 | // printf("start sys_readn: %d....\n", n); 42 | int nleft, nread; 43 | char* ptr; 44 | 45 | ptr = vptr; 46 | nleft = n; 47 | while (nleft > 0) { 48 | // printf("start read\n"); 49 | if ((nread = read(fd, ptr, nleft)) < 0) { 50 | if (errno == EINTR) 51 | nread = 0; 52 | else 53 | return -1; 54 | } else if (nread == 0) { 55 | break; 56 | } 57 | // printf("end read, read: %d\n", nread); 58 | nleft -= nread; 59 | ptr += nread; 60 | } 61 | // printf("stop sys_readn....\n"); 62 | 63 | return n - nleft; 64 | } 65 | 66 | int sys_readn_by_ssl(SSL *ssl, void *vptr, int n) 67 | { 68 | int nleft, nread; 69 | char *ptr; 70 | 71 | ptr = vptr; 72 | nleft = n; 73 | while (nleft > 0) { 74 | if ((nread = SSL_read(ssl, ptr, nleft)) < 0) { 75 | if (errno == EINTR) 76 | nread = 0; 77 | else 78 | return -1; 79 | } 80 | else if (nread == 0) 81 | break; 82 | 83 | nleft -= nread; 84 | ptr += nread; 85 | } 86 | 87 | return n - nleft; 88 | } 89 | 90 | int sys_writen(int fd, const void* vptr, int n) { 91 | int nleft; 92 | int nwritten; 93 | const char* ptr; 94 | 95 | ptr = vptr; 96 | nleft = n; 97 | while (nleft > 0) { 98 | if ((nwritten = write(fd, ptr, nleft)) <= 0) { 99 | if (nwritten < 0 && errno == EINTR) 100 | nwritten = 0; /* and call write() again */ 101 | else 102 | return (-1); /* error */ 103 | } 104 | 105 | nleft -= nwritten; 106 | ptr += nwritten; 107 | } 108 | 109 | return n; 110 | } 111 | 112 | int tcp_open(const char* ipaddr, int port) { 113 | int sockfd; 114 | struct sockaddr_in servaddr; 115 | printf("ip:%s port:%d\n",ipaddr,port); 116 | 117 | if ((sockfd = socket(AF_INET, SOCK_STREAM, 0)) == -1){ 118 | printf("socket errno:%d, %s\n",errno,strerror(errno)); 119 | return -1; 120 | } 121 | 122 | bzero(&servaddr, sizeof(servaddr)); 123 | servaddr.sin_family = AF_INET; 124 | servaddr.sin_port = htons(port); 125 | if (inet_pton(AF_INET, ipaddr, &servaddr.sin_addr) <= 0) { 126 | printf("inet_pton errno:%d, %s\n",errno,strerror(errno)); 127 | close(sockfd); 128 | return -1; 129 | } 130 | 131 | if (connect(sockfd, (struct sockaddr*)&servaddr, sizeof(servaddr)) == -1) { 132 | printf("connect errno:%d, %s\n",errno,strerror(errno)); 133 | close(sockfd); 134 | return -1; 135 | } 136 | 137 | return sockfd; 138 | } 139 | 140 | /** 141 | *Description:check the socket state 142 | * 143 | * @param 144 | * fd: socket 145 | * timeout:the time out of select 146 | * wait_for:socket state(r,w,conn) 147 | * 148 | * @return 1 if everything was ok, 0 otherwise 149 | */ 150 | int select_fd(int fd, int timeout, int wait_for) { 151 | fd_set fdset; 152 | fd_set *rd = NULL, *wr = NULL; 153 | struct timeval tmo; 154 | int result; 155 | 156 | FD_ZERO(&fdset); 157 | FD_SET(fd, &fdset); 158 | if (wait_for == WAIT_FOR_READ) { 159 | rd = &fdset; 160 | } 161 | if (wait_for == WAIT_FOR_WRITE) { 162 | wr = &fdset; 163 | } 164 | if (wait_for == WAIT_FOR_CONN) { 165 | rd = &fdset; 166 | wr = &fdset; 167 | } 168 | 169 | tmo.tv_sec = timeout; 170 | tmo.tv_usec = 0; 171 | do { 172 | result = select(fd + 1, rd, wr, NULL, &tmo); 173 | } while (result < 0 && errno == EINTR); 174 | 175 | return result; 176 | } -------------------------------------------------------------------------------- /pandar_pointcloud/src/conversions/tcp_command_client.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright 2019 The Hesai Technology Authors. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *****************************************************************************/ 16 | #include "openssl/ssl.h" 17 | #include "openssl/err.h" 18 | 19 | #ifndef SRC_TCP_COMMAND_CLIENT_H_ 20 | #define SRC_TCP_COMMAND_CLIENT_H_ 21 | 22 | #ifdef __cplusplus 23 | extern "C" { 24 | #endif 25 | 26 | typedef enum { 27 | PTC_COMMAND_GET_CALIBRATION = 0, 28 | PTC_COMMAND_SET_CALIBRATION, 29 | PTC_COMMAND_HEARTBEAT, 30 | PTC_COMMAND_RESET_CALIBRATION, 31 | PTC_COMMAND_TEST, 32 | PTC_COMMAND_GET_LIDAR_CALIBRATION, 33 | PTC_COMMAND_GET_LIDAR_CONFIG_INFO = 8, 34 | PTC_COMMAND_GET_LIDAR_STATUS = 9, 35 | PTC_COMMAND_SET_LIDAR_SPIN_RATE = 23, 36 | PTC_COMMAND_SET_LIDAR_OPERATE_MODE = 28, 37 | PTC_COMMAND_SET_LIDAR_RETURN_MODE = 30, 38 | PTC_COMMAND_SET_LIDAR_LENS_HEAT_SWITCH = 147, 39 | PTC_COMMAND_GET_LIDAR_LENS_HEAT_SWITCH = 148, 40 | PTC_COMMAND_GET_LIDAR_CHANNEL_CONFIG = 168, 41 | PTC_COMMAND_GET_LIDAR_FIRETIMES = 169, 42 | } PTC_COMMAND; 43 | 44 | typedef enum { 45 | PTC_ERROR_NO_ERROR = 0, 46 | PTC_ERROR_BAD_PARAMETER, 47 | PTC_ERROR_CONNECT_SERVER_FAILED, 48 | PTC_ERROR_TRANSFER_FAILED, 49 | PTC_ERROR_NO_MEMORY, 50 | } PTC_ErrCode; 51 | 52 | typedef enum { 53 | CERTIFY_MODE_NONE = 0, 54 | CERTIFY_MODE_SINGLE, 55 | CERTIFY_MODE_DUAL, 56 | CERTIFY_MODE_ERROR, 57 | } CERTIFY_MODE; 58 | 59 | typedef struct TcpCommandHeader_s { 60 | unsigned char cmd; 61 | unsigned char ret_code; 62 | unsigned int len; 63 | } TcpCommandHeader; 64 | 65 | typedef struct TC_Command_s { 66 | TcpCommandHeader header; 67 | unsigned char* data; 68 | 69 | unsigned char* ret_data; 70 | unsigned int ret_size; 71 | } TC_Command; 72 | 73 | void* TcpCommandClientNew(const char* ip, const unsigned short port); 74 | void BuildCmd(TC_Command command, PTC_COMMAND cmd, unsigned char* data); 75 | PTC_ErrCode TcpCommandSetCalibration(const void* handle, const char* buffer, 76 | unsigned int len); 77 | PTC_ErrCode TcpCommandGetCalibration(const void* handle, char** buffer, 78 | unsigned int* len); 79 | PTC_ErrCode TcpCommandGetLidarCalibration(const void* handle, char** buffer, 80 | unsigned int* len); 81 | PTC_ErrCode TcpCommandGetLidarChannelConfig(const void* handle, char** buffer, 82 | unsigned int* len); 83 | PTC_ErrCode TcpCommandGetLidarFiretime(const void* handle, char** buffer, 84 | unsigned int* len); 85 | PTC_ErrCode TcpCommandGetLidarLensHeatSwitch(const void* handle, unsigned char** buffer, 86 | unsigned int* len); 87 | PTC_ErrCode TcpCommandGetLidarStatus(const void* handle, unsigned char** buffer, 88 | unsigned int* len); 89 | PTC_ErrCode TcpCommandGetLidarConfigInfo(const void* handle, unsigned char** buffer, 90 | unsigned int* len); 91 | PTC_ErrCode TcpCommandResetCalibration(const void* handle); 92 | PTC_ErrCode TcpCommandSetLidarStandbyMode(const void* handle); 93 | PTC_ErrCode TcpCommandSetLidarNormalMode(const void* handle); 94 | PTC_ErrCode TcpCommandSetLidarLensHeatSwitch(const void* handle, uint8_t heatSwitch); 95 | PTC_ErrCode TcpCommandSetLidarReturnMode(const void* handle, uint8_t mode); 96 | PTC_ErrCode TcpCommandSetLidarSpinRate(const void* handle, uint16_t spinRate); 97 | PTC_ErrCode TcpCommandSet(const void* handle, PTC_COMMAND cmd, unsigned char* data, uint32_t len); 98 | PTC_ErrCode TcpCommandGet(const void* handle, PTC_COMMAND cmd, unsigned char** buffer, unsigned int* len); 99 | void TcpCommandClientDestroy(const void* handle); 100 | SSL_CTX* initial_client_ssl(const char* cert, const char* private_key, const char* ca); 101 | void TcpCommandSetSsl(const char* cert, const char* private_key, const char* ca); 102 | 103 | #ifdef __cplusplus 104 | } 105 | #endif 106 | 107 | #endif // SRC_TCP_COMMAND_CLIENT_H_ 108 | -------------------------------------------------------------------------------- /pandar_pointcloud/src/taskflow/utility/traits.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | namespace tf { 26 | 27 | //----------------------------------------------------------------------------- 28 | // Traits 29 | //----------------------------------------------------------------------------- 30 | 31 | // Macro to check whether a class has a member function 32 | #define define_has_member(member_name) \ 33 | template \ 34 | class has_member_##member_name \ 35 | { \ 36 | typedef char yes_type; \ 37 | typedef long no_type; \ 38 | template static yes_type test(decltype(&U::member_name)); \ 39 | template static no_type test(...); \ 40 | public: \ 41 | static constexpr bool value = sizeof(test(0)) == sizeof(yes_type); \ 42 | } 43 | 44 | #define has_member(class_, member_name) has_member_##member_name::value 45 | 46 | // Struct: dependent_false 47 | template 48 | struct dependent_false { 49 | static constexpr bool value = false; 50 | }; 51 | 52 | template 53 | constexpr auto dependent_false_v = dependent_false::value; 54 | 55 | // Struct: is_iterator 56 | template 57 | struct is_iterator { 58 | static constexpr bool value = false; 59 | }; 60 | 61 | template 62 | struct is_iterator< 63 | T, 64 | std::enable_if_t::value_type, void>> 65 | > { 66 | static constexpr bool value = true; 67 | }; 68 | 69 | template 70 | inline constexpr bool is_iterator_v = is_iterator::value; 71 | 72 | // Struct: is_iterable 73 | template 74 | struct is_iterable : std::false_type { 75 | }; 76 | 77 | template 78 | struct is_iterable().begin()), 79 | decltype(std::declval().end())>> 80 | : std::true_type { 81 | }; 82 | 83 | template 84 | inline constexpr bool is_iterable_v = is_iterable::value; 85 | 86 | // Struct: MoC 87 | // Move-on-copy wrapper. 88 | template 89 | struct MoC { 90 | 91 | MoC(T&& rhs) : object(std::move(rhs)) {} 92 | MoC(const MoC& other) : object(std::move(other.object)) {} 93 | 94 | T& get() { return object; } 95 | 96 | mutable T object; 97 | }; 98 | 99 | //----------------------------------------------------------------------------- 100 | // Functors. 101 | //----------------------------------------------------------------------------- 102 | 103 | // Overloadded. 104 | template 105 | struct Functors : Ts... { 106 | using Ts::operator()... ; 107 | }; 108 | 109 | template 110 | Functors(Ts...) -> Functors; 111 | 112 | 113 | // ---------------------------------------------------------------------------- 114 | // Function Traits 115 | // ---------------------------------------------------------------------------- 116 | 117 | // we specialize for pointers to member function 118 | 119 | template 120 | struct function_traits; 121 | 122 | // function pointer 123 | template 124 | struct function_traits : public function_traits { 125 | }; 126 | 127 | // function reference 128 | template 129 | struct function_traits : public function_traits { 130 | }; 131 | 132 | // for operator () 133 | template 134 | struct function_traits : public function_traits{ 135 | }; 136 | 137 | // immutable operator 138 | template 139 | struct function_traits : 140 | public function_traits { 141 | }; 142 | 143 | // mutable operator 144 | template 145 | struct function_traits : 146 | public function_traits { 147 | }; 148 | 149 | // function_traits 150 | template 151 | struct function_traits { 152 | 153 | using return_type = R; 154 | using argument_tuple_type = std::tuple; 155 | 156 | static constexpr size_t arity = sizeof...(Args); 157 | 158 | template 159 | struct argument { 160 | static_assert(N < arity, "error: invalid parameter index."); 161 | using type = std::tuple_element_t>; 162 | }; 163 | 164 | template 165 | using argument_t = typename argument::type; 166 | }; 167 | 168 | 169 | } // end of namespace tf. --------------------------------------------------- 170 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # HesaiLidar_Swift_ROS 2 | 3 | ## About the project 4 | This repository includes the ROS Driver for Pandar LiDAR sensor manufactured by Hesai Technology. Branches are included for different systems and UDP protocol versions. 5 | 6 | Developed based on [HesaiLidar_Swift_SDK](https://github.com/HesaiTechnology/HesaiLidar_Swift_SDK), After launched, the project will monitor UDP packets from Lidar,parse data and publish point clouds frames into ROS under topic: ```/pandar_points```. It can also be used as an official demo showing how to work with [HesaiLidar_Swift_SDK](https://github.com/HesaiTechnology/HesaiLidar_Swift_SDK). 7 | 8 | ## Branches 9 | ``` 10 | * master: Pandar LiDAR ROS driver for Ubuntu 18.04 and Ubuntu20.04 supports the latest UDP protocol v1.4 v1.3 and v3.2 11 | * UDP1.4_ubuntu16.04: Pandar LiDAR ROS driver for Ubuntu 16.04 supports the latest UDP protocol v1.4 12 | * UDP4.3: Pandar LiDAR ROS driver for Ubuntu 16.04/Ubuntu 18.04/Ubuntu 20.04 supports the latest UDP protocol v4.3 13 | 14 | To get the UDP protocol version number of your device, check the UDP package header field. 15 | ``` 16 | ## Environment and Dependencies 17 | **System environment requirement: Linux + ROS** 18 | ``` 19 | 　Recommanded: 20 | 　Ubuntu 16.04 - with ROS kinetic desktop-full installed or 21 | 　Ubuntu 18.04 - with ROS melodic desktop-full installed or 22 | Ubuntu 20.04 - with ROS noetic desktop-full installed 23 | 　Check resources on http://ros.org for installation guide 24 | ``` 25 | **Compiler version requirement** 26 | ``` 27 | Cmake version requirement: Cmake 3.8.0 or above 28 | G++ version requirement: G++ 7.5.0 or above 29 | ``` 30 | 31 | **Library Dependencies: libpcl-dev + libpcap-dev + libyaml-cpp-dev + libboost-dev** 32 | ``` 33 | $ sudo apt-get update 34 | $ sudo apt install libpcl-dev libpcap-dev libyaml-cpp-dev libboost-dev 35 | ``` 36 | ## Download and Build 37 | 38 | **Install `catkin_tools`** 39 | ``` 40 | $ sudo apt-get update 41 | $ sudo apt-get install python-catkin-tools 42 | ``` 43 | **Download code** 44 | ``` 45 | $ mkdir -p rosworkspace/src 46 | $ cd rosworkspace/src 47 | $ git clone https://github.com/HesaiTechnology/HesaiLidar_Swift_ROS.git --recursive 48 | ``` 49 | **Install required dependencies with the help of `rosdep`** 50 | ``` 51 | $ cd .. 52 | $ rosdep install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO 53 | ``` 54 | **Build** 55 | ``` 56 | $ catkin_make -DCMAKE_BUILD_TYPE=Release 57 | ``` 58 | 59 | 60 | ## Configuration 61 | ``` 62 | $ cd src/HesaiLidar_Swift_ROS/pandar_pointcloud/launch 63 | ``` 64 | open PandarSwift_points.launch to set configuration parameters 65 | 66 | ### Reciving data from connected LiDAR: config LiDAR IP address & UDP port, and leave the pcap_file empty 67 | 68 | |Parameter | Default Value| 69 | |---------|---------------| 70 | |device_ip |192.168.1.201| 71 | |port |2368| 72 | |pcap_file || 73 | 74 | Data source will be read from connected LiDAR when "pcap_file" is set to empty 75 | 76 | ### Reciving data from pcap file: config pcap_file and correction file path 77 | 78 | |Parameter | Value| 79 | |---------|---------------| 80 | |pcap |pcap file path| 81 | |calibration|lidar calibration file path|　 82 | 83 | Data source will be read from pcap file instead of LiDAR once "pcap_file" not empty 84 | 85 | 86 | ## Run 87 | 88 | ### View the point clouds from connected LiDAR 89 | 90 | 1. Make sure current path in the `rosworkspace` directory 91 | ``` 92 | $ source devel/setup.bash 93 | $ roslaunch pandar_pointcloud PandarSwift_points.launch 94 | ``` 95 | 96 | 2. The driver will publish a PointCloud2 message in the topic. 97 | ``` 98 | /pandar_points 99 | ``` 100 | 101 | 3. Open Rviz and add display by topic. 102 | 103 | 4. Change fixed frame to "PandarSwift" to view published point clouds. 104 | 105 | ### View the point clouds from rosbag file 106 | 107 | 1. Make sure current path in the `rosworkspace` directory 108 | ``` 109 | $ source devel/setup.bash 110 | $ roslaunch pandar_pointcloud PandarSwift_points.launch 111 | ``` 112 | 113 | 2. The driver will publish a raw data packet message in the topic. 114 | ``` 115 | /pandar_packets 116 | ``` 117 | 3. record raw data rosbag 118 | ``` 119 | $rosbag record -b 4096 /hesai/pandar_packets 120 | ``` 121 | 122 | 4. stop roslaunch and rosbag record by "Ctrl + C" 123 | 124 | 5. play raw data rosbag 125 | ``` 126 | $rosbag play 127 | ``` 128 | 129 | 6. launch transform_nodelet.launch 130 | ``` 131 | $ roslaunch pandar_pointcloud transform_nodelet.launch data_type:=rosbag 132 | ``` 133 | 7. Open Rviz and add display by topic. 134 | 8. Change fixed frame to "PandarSwift" to view published point clouds. 135 | 136 | ## Details of launch file parameters and utilities 137 | |Parameter | Default Value| 138 | |---------|---------------| 139 | |calibration|Path of correction file, will be used when not able to get correction file from a connected Liar| 140 | |device_ip_ip|The IP address of connected Lidar, will be used to get correction file| 141 | |host_ip|The IP address of host pc, will be used to get udp data from lidar| 142 | |frame_id|frame id of published messages| 143 | |firetime_file|Path of firetime files| 144 | |pcap|Path of the pcap file, once not empty, driver will get data from pcap file instead of a connected Lidar| 145 | |port|The destination port of Lidar, driver will monitor this port to get point clouds packets from Lidar| 146 | |start_angle|Driver will publish one frame point clouds data when azimuth angle step over start_angle, make sure set to within FOV| 147 | |publish_model|default "points":publish point clouds "raw":publish raw UDP packets "both_point_raw":publish point clouds and UDP packets| 148 | |namespace|namesapce of the launching node| 149 | |coordinate_correction_flag|default "false":Disable coordinate correction "true":Enable coordinate correction| 150 | |channel_config_file|Path of channel config file, will be used when not able to get channel config file from a connected Liar| 151 | |cert_file|Path of the user's certificate| 152 | |private_key_file|Path of the user's private key| 153 | |ca_file|Path of the root certificate| 154 | |multicast_ip|The multicast IP address of connected Lidar, will be used to get udp packets from multicast ip address| -------------------------------------------------------------------------------- /pandar_pointcloud/include/pandar_pointcloud/rawdata.h: -------------------------------------------------------------------------------- 1 | /* -*- mode: C++ -*- 2 | * 3 | * Copyright (C) 2007 Austin Robot Technology, Yaxin Liu, Patrick Beeson 4 | * Copyright (C) 2009, 2010, 2012 Austin Robot Technology, Jack O'Quin 5 | * Copyright (c) 2017 Hesai Photonics Technology, Yang Sheng 6 | * Copyright (c) 2020 Hesai Photonics Technology, Lingwen Fang 7 | * 8 | * License: Modified BSD Software License Agreement 9 | * 10 | * $Id$ 11 | */ 12 | 13 | /** @file 14 | * 15 | * @brief Interfaces for interpreting raw packets from the Pandar128 3D LIDAR. 16 | * 17 | * @author Yaxin Liu 18 | * @author Patrick Beeson 19 | * @author Jack O'Quin 20 | * @author Yang Sheng 21 | */ 22 | 23 | #ifndef __PANDAR_RAWDATA_H 24 | #define __PANDAR_RAWDATA_H 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | namespace pandar_rawdata 40 | { 41 | // Shorthand typedefs for point cloud representations 42 | typedef pandar_pointcloud::PointXYZIT PPoint; 43 | typedef pcl::PointCloud PPointCloud; 44 | 45 | static const int SOB_ANGLE_SIZE = 4; 46 | static const int RAW_MEASURE_SIZE = 5; 47 | static const int LASER_COUNT = 40; 48 | static const int BLOCKS_PER_PACKET = 6; 49 | static const int BLOCK_SIZE = RAW_MEASURE_SIZE * LASER_COUNT + SOB_ANGLE_SIZE; 50 | static const int TIMESTAMP_SIZE = 4; 51 | static const int FACTORY_ID_SIZE = 2; 52 | static const int RESERVE_SIZE = 8; 53 | static const int REVOLUTION_SIZE = 2; 54 | static const int INFO_SIZE = TIMESTAMP_SIZE + FACTORY_ID_SIZE 55 | + RESERVE_SIZE + REVOLUTION_SIZE; 56 | static const int PACKET_SIZE = BLOCK_SIZE * BLOCKS_PER_PACKET + INFO_SIZE; 57 | static const int ROTATION_MAX_UNITS = 36001; 58 | static const float ROTATION_RESOLUTION = 0.01; 59 | 60 | typedef struct raw_measure { 61 | uint32_t range; 62 | uint16_t reflectivity; 63 | } raw_measure_t; 64 | 65 | /** \brief Raw Pandar128 data block. 66 | */ 67 | typedef struct raw_block 68 | { 69 | uint16_t sob; 70 | uint16_t azimuth; 71 | raw_measure_t measures[LASER_COUNT]; 72 | } raw_block_t; 73 | 74 | 75 | /** \brief Raw Pandar128 packet. 76 | */ 77 | typedef struct raw_packet 78 | { 79 | raw_block_t blocks[BLOCKS_PER_PACKET]; 80 | uint8_t reserved[RESERVE_SIZE]; 81 | uint16_t revolution; 82 | uint32_t timestamp; 83 | uint8_t factory[2]; 84 | double recv_time; 85 | } raw_packet_t; 86 | 87 | typedef struct gps_struct{ 88 | int used; 89 | time_t gps; 90 | }gps_struct_t; 91 | 92 | /** \brief Pandar128 data conversion class */ 93 | class RawData 94 | { 95 | public: 96 | 97 | RawData(); 98 | ~RawData() {} 99 | 100 | /** \brief Set up for data processing. 101 | * 102 | * Perform initializations needed before data processing can 103 | * begin: 104 | * 105 | * - read device-specific angles calibration 106 | * 107 | * @param private_nh private node handle for ROS parameters 108 | * @returns 0 if successful; 109 | * errno value for failure 110 | */ 111 | int setup(ros::NodeHandle private_nh); 112 | 113 | /** \brief Set up for data processing offline. 114 | * Performs the same initialization as in setup, in the abscence of a ros::NodeHandle. 115 | * this method is useful if unpacking data directly from bag files, without passing 116 | * through a communication overhead. 117 | * 118 | * @param calibration_file path to the calibration file 119 | * @param max_range_ cutoff for maximum range 120 | * @param min_range_ cutoff for minimum range 121 | * @returns 0 if successful; 122 | * errno value for failure 123 | */ 124 | int setupOffline(std::string calibration_file, double max_range_, double min_range_); 125 | 126 | void unpack(const pandar_msgs::PandarPacket &pkt, PPointCloud &pc); 127 | int unpack(const pandar_msgs::PandarScan::ConstPtr &scanMsg, PPointCloud &pc, time_t& gps1 , 128 | gps_struct_t &gps2 , double& firstStamp, int& lidarRotationStartAngle); 129 | 130 | int unpack(pandar_msgs::PandarPacket &packet, PPointCloud &pc, time_t& gps1 , 131 | gps_struct_t &gps2 , double& firstStamp, int& lidarRotationStartAngle); 132 | 133 | void setParameters(double min_range, double max_range, double view_direction, 134 | double view_width); 135 | 136 | private: 137 | 138 | /** configuration parameters */ 139 | typedef struct { 140 | std::string calibrationFile; ///< calibration file name 141 | double max_range; ///< maximum range to publish 142 | double min_range; ///< minimum range to publish 143 | int min_angle; ///< minimum angle to publish 144 | int max_angle; ///< maximum angle to publish 145 | 146 | double tmp_min_angle; 147 | double tmp_max_angle; 148 | } Config; 149 | Config config_; 150 | 151 | /** 152 | * Calibration file 153 | */ 154 | pandar_pointcloud::Calibration calibration_; 155 | float sin_lookup_table_[ROTATION_MAX_UNITS]; 156 | float cos_lookup_table_[ROTATION_MAX_UNITS]; 157 | 158 | /** in-line test whether a point is in range */ 159 | bool pointInRange(float range) 160 | { 161 | return (range >= config_.min_range 162 | && range <= config_.max_range); 163 | } 164 | 165 | int parseRawData(raw_packet* packet, const uint8_t* buf, const int len); 166 | void toPointClouds (raw_packet* packet, PPointCloud& pc); 167 | void toPointClouds (raw_packet_t* packet,int laser , PPointCloud& pc , double stamp , double& firstStamp); 168 | void toPointClouds (raw_packet_t* packet,int laser , int block, PPointCloud& pc); 169 | void computeXYZIR(PPoint& point, int azimuth, 170 | const raw_measure_t& laserReturn, 171 | const pandar_pointcloud::PandarLaserCorrection& correction); 172 | 173 | int lastBlockEnd; 174 | 175 | raw_packet_t *bufferPacket; 176 | int bufferPacketSize; 177 | 178 | int currentPacketStart; 179 | 180 | int lastTimestamp; 181 | 182 | int lastAzumith; 183 | }; 184 | 185 | } // namespace pandar_rawdata 186 | 187 | #endif // __PANDAR_RAWDATA_H 188 | -------------------------------------------------------------------------------- /pandar_pointcloud/src/taskflow/utility/passive_vector.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace tf { 10 | 11 | // Class: PassiveVector 12 | // A vector storing only passive data structure (PDS) or POD data type. 13 | template > 14 | class PassiveVector { 15 | 16 | static_assert(std::is_pod_v, "must be a passive data structure type"); 17 | 18 | public: 19 | 20 | typedef T value_type; 21 | typedef T & reference; 22 | typedef const T & const_reference; 23 | typedef T * pointer; 24 | typedef const T * const_pointer; 25 | typedef T * iterator; 26 | typedef const T * const_iterator; 27 | typedef std::reverse_iterator reverse_iterator; 28 | typedef std::reverse_iterator const_reverse_iterator; 29 | typedef ptrdiff_t difference_type; 30 | typedef size_t size_type; 31 | 32 | PassiveVector() noexcept : 33 | _data {reinterpret_cast(_stack)}, 34 | _num {0}, 35 | _cap {S} { 36 | } 37 | 38 | explicit PassiveVector(size_type n) : _num {n} { 39 | 40 | // need to place on heap 41 | if(n > S) { 42 | _cap = n << 2; 43 | _data = _allocator.allocate(_cap); 44 | } 45 | // stack 46 | else { 47 | _cap = S; 48 | _data = reinterpret_cast(_stack); 49 | } 50 | 51 | } 52 | 53 | PassiveVector(const PassiveVector& rhs) : _num {rhs._num} { 54 | 55 | // heap 56 | if(rhs._num > S) { 57 | _cap = rhs._cap; 58 | _data = _allocator.allocate(rhs._cap); 59 | } 60 | else { 61 | _cap = S; 62 | _data = reinterpret_cast(_stack); 63 | } 64 | 65 | std::memcpy(_data, rhs._data, _num * sizeof(T)); 66 | } 67 | 68 | PassiveVector(PassiveVector&& rhs) : _num {rhs._num} { 69 | 70 | // rhs is in the stack 71 | if(rhs.in_stack()) { 72 | _cap = S; 73 | _data = reinterpret_cast(_stack); 74 | std::memcpy(_stack, rhs._stack, rhs._num*sizeof(T)); 75 | } 76 | // rhs is in the heap 77 | else { 78 | _cap = rhs._cap; 79 | _data = rhs._data; 80 | rhs._data = reinterpret_cast(rhs._stack); 81 | rhs._cap = S; 82 | } 83 | 84 | rhs._num = 0; 85 | } 86 | 87 | ~PassiveVector() { 88 | if(!in_stack()) { 89 | _allocator.deallocate(_data, _cap); 90 | } 91 | } 92 | 93 | iterator begin() noexcept { return _data; } 94 | const_iterator begin() const noexcept { return _data; } 95 | const_iterator cbegin() const noexcept { return _data; } 96 | iterator end() noexcept { return _data + _num; } 97 | const_iterator end() const noexcept { return _data + _num; } 98 | const_iterator cend() const noexcept { return _data + _num; } 99 | 100 | reverse_iterator rbegin() noexcept { return _data + _num; } 101 | const_reverse_iterator crbegin() const noexcept { return _data + _num; } 102 | reverse_iterator rend() noexcept { return _data; } 103 | const_reverse_iterator crend() const noexcept { return _data; } 104 | 105 | reference operator [] (size_type idx) { return _data[idx]; } 106 | const_reference operator [] (size_type idx) const { return _data[idx]; } 107 | 108 | reference at(size_type pos) { 109 | if(pos >= _num) { 110 | throw std::out_of_range("accessed position is out of range"); 111 | } 112 | return this->operator[](pos); 113 | } 114 | 115 | const_reference at(size_type pos) const { 116 | if(pos >= _num) { 117 | throw std::out_of_range("accessed position is out of range"); 118 | } 119 | return this->operator[](pos); 120 | } 121 | 122 | 123 | reference front() { return _data[0]; } 124 | const_reference front() const { return _data[0]; } 125 | reference back() { return _data[_num-1]; } 126 | const_reference back() const { return _data[_num-1]; } 127 | 128 | pointer data() noexcept { return _data; } 129 | const_pointer data() const noexcept { return _data; } 130 | 131 | void push_back(const T& item) { 132 | if(_num == _cap) { 133 | _enlarge(_cap << 1); 134 | } 135 | _data[_num++] = item; 136 | } 137 | 138 | void push_back(T&& item) { 139 | if(_num == _cap) { 140 | _enlarge(_cap << 1); 141 | } 142 | _data[_num++] = item; 143 | } 144 | 145 | void pop_back() { 146 | if(_num > 0) { 147 | --_num; 148 | } 149 | } 150 | 151 | void clear() { 152 | _num = 0; 153 | } 154 | 155 | void resize(size_type N) { 156 | if(N > _cap) { 157 | _enlarge(N<<1); 158 | } 159 | _num = N; 160 | } 161 | 162 | void reserve(size_type C) { 163 | if(C > _cap) { 164 | _enlarge(C); 165 | } 166 | } 167 | 168 | bool empty() const { return _num == 0; } 169 | bool in_stack() const { return _data == reinterpret_cast(_stack); } 170 | 171 | size_type size() const { return _num; } 172 | size_type capacity() const { return _cap; } 173 | size_type max_size() const { return std::numeric_limits::max(); } 174 | 175 | bool operator == (const PassiveVector& rhs) const { 176 | if(_num != rhs._num) { 177 | return false; 178 | } 179 | return std::memcmp(_data, rhs._data, _num * sizeof(T)) == 0; 180 | } 181 | 182 | private: 183 | 184 | std::byte _stack[S*sizeof(T)]; 185 | 186 | T* _data; 187 | 188 | size_type _num; 189 | size_type _cap; 190 | 191 | A _allocator; 192 | 193 | void _enlarge(size_type new_cap) { 194 | 195 | auto new_data = _allocator.allocate(new_cap); 196 | 197 | std::memcpy(new_data, _data, sizeof(T) * _num); 198 | 199 | if(!in_stack()) { 200 | _allocator.deallocate(_data, _cap); 201 | } 202 | 203 | _cap = new_cap; 204 | _data = new_data; 205 | } 206 | }; 207 | 208 | 209 | } // end of namespace tf. ---------------------------------------------------- 210 | 211 | -------------------------------------------------------------------------------- /pandar_pointcloud/src/taskflow/utility/singular_allocator.hpp: -------------------------------------------------------------------------------- 1 | // 2019/02/15 - modified by Tsung-Wei Huang 2 | // - refactored the code 3 | // 4 | // 2019/02/10 - modified by Tsung-Wei Huang 5 | // - fixed the compilation error on MS platform 6 | // 7 | // 2019/02/08 - created by Chun-Xun Lin 8 | // - added a singular memory allocator 9 | // - refactored by Tsung-Wei Huang 10 | 11 | #pragma once 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | namespace tf { 20 | 21 | // Class: SingularMempool 22 | template 23 | struct SingularMempool { 24 | 25 | // FreeList uses Node array to store Node* 26 | struct FreeList { 27 | typedef T* pointer; 28 | typedef pointer* ppointer; 29 | 30 | FreeList() = default; 31 | 32 | void push(pointer ptr) { 33 | *(reinterpret_cast(ptr)) = top; 34 | top = ptr; 35 | } 36 | 37 | // Must use empty() before calling pop 38 | pointer pop() { 39 | assert(!empty()); 40 | pointer retval = top; 41 | top = *(reinterpret_cast(top)); 42 | return retval; 43 | } 44 | 45 | bool empty() const { return top == nullptr; } 46 | 47 | pointer top {nullptr}; 48 | }; 49 | 50 | struct MemBlock{ 51 | T* block; // Block memory. 52 | size_t size; // Size of the block (count). 53 | struct MemBlock* next; // Pointer to the next block. 54 | }; 55 | 56 | // Ctor 57 | SingularMempool() { 58 | head = allocate_memblock(1024); 59 | tail = head; 60 | } 61 | 62 | // Dtor 63 | ~SingularMempool() { 64 | for(auto* prev = head; head!=nullptr;) { 65 | head = head->next; 66 | std::free(prev->block); 67 | std::free(prev); 68 | prev = head; 69 | } 70 | } 71 | 72 | MemBlock* allocate_memblock(size_t n) { 73 | MemBlock* ptr = static_cast(std::malloc(sizeof(MemBlock))); 74 | ptr->block = static_cast(std::malloc(n*sizeof(T))); 75 | ptr->size = n; 76 | ptr->next = nullptr; 77 | return ptr; 78 | } 79 | 80 | T* allocate() { 81 | 82 | if(!free_list.empty()) { 83 | return free_list.pop(); 84 | } 85 | 86 | if(tail->size == used) { 87 | auto next = allocate_memblock(tail->size << 1); 88 | tail->next = next; 89 | tail = next; 90 | used = 0; 91 | } 92 | return &(tail->block[used++]); 93 | } 94 | 95 | void deallocate(T* ptr) { 96 | free_list.push(ptr); 97 | } 98 | 99 | FreeList free_list; 100 | 101 | MemBlock *head {nullptr}; 102 | MemBlock *tail {nullptr}; 103 | size_t used {0}; 104 | }; 105 | 106 | // Class: SingularMempoolManager 107 | template 108 | struct SingularMempoolManager { 109 | 110 | struct Handle { 111 | 112 | Handle(SingularMempoolManager &mgr) : manager {mgr} { 113 | std::scoped_lock lock(mgr.mtx); 114 | if(mgr.pools.empty()) { 115 | mempool = new SingularMempool(); 116 | } 117 | else { 118 | mempool = mgr.pools.back(); 119 | mgr.pools.pop_back(); 120 | } 121 | } 122 | 123 | ~Handle() { 124 | // Return the memory pool to SingularMempoolManager 125 | std::scoped_lock lock(manager.mtx); 126 | manager.pools.emplace_back(mempool); 127 | } 128 | 129 | SingularMempoolManager& manager; 130 | SingularMempool* mempool {nullptr}; 131 | }; 132 | 133 | // Ctor 134 | SingularMempoolManager() { 135 | pools.reserve(std::thread::hardware_concurrency()); 136 | } 137 | 138 | // Dtor 139 | ~SingularMempoolManager() { 140 | std::scoped_lock lock(mtx); 141 | for(auto& p : pools) { 142 | delete p; 143 | } 144 | } 145 | 146 | SingularMempool* get_per_thread_mempool() { 147 | thread_local Handle handle {*this}; 148 | return handle.mempool; 149 | } 150 | 151 | std::mutex mtx; 152 | std::vector*> pools; 153 | }; 154 | 155 | // The singleton allocator 156 | template 157 | auto& get_singular_mempool_manager() { 158 | static SingularMempoolManager manager; 159 | return manager; 160 | } 161 | 162 | // Class: SingularAllocator 163 | template 164 | class SingularAllocator { 165 | 166 | public: 167 | 168 | // Allocator traits 169 | typedef T value_type ; 170 | typedef T* pointer ; 171 | typedef const T* const_pointer ; 172 | typedef T& reference ; 173 | typedef const T& const_reference; 174 | typedef std::size_t size_type ; 175 | typedef std::ptrdiff_t difference_type; 176 | 177 | template 178 | struct rebind { 179 | typedef SingularAllocator other; 180 | }; 181 | 182 | explicit SingularAllocator() {} 183 | ~SingularAllocator() {} 184 | 185 | explicit SingularAllocator(const SingularAllocator&) {} 186 | 187 | template 188 | explicit SingularAllocator(const SingularAllocator&) {} 189 | 190 | T* allocate(size_t n=1) ; 191 | void deallocate(T*, size_t n=1); 192 | 193 | template 194 | void construct(T*, ArgsT&&...); 195 | void destroy(T*); 196 | 197 | //SingularAllocator & operator = (const SingularAllocator &) {} 198 | 199 | bool operator == (const SingularAllocator &) const { return true; } 200 | bool operator != (const SingularAllocator &) const { return false; } 201 | }; 202 | 203 | // Procedure: construct 204 | // Construct an item with placement new. 205 | template 206 | template 207 | void SingularAllocator::construct(T* ptr, ArgsT&&... args) { 208 | new (ptr) T(std::forward(args)...); 209 | } 210 | 211 | // Procedure: destroy 212 | // Destroy an item 213 | template 214 | void SingularAllocator::destroy(T* ptr) { 215 | ptr->~T(); 216 | } 217 | 218 | // Function: allocate 219 | // Allocate a memory piece of type T from the memory pool and return the T* to that memory. 220 | template 221 | T* SingularAllocator::allocate(size_t n) { 222 | if(n == 1) { 223 | return get_singular_mempool_manager().get_per_thread_mempool()->allocate(); 224 | } 225 | else { 226 | throw std::runtime_error("can only allocate one object"); 227 | } 228 | } 229 | 230 | // Function: deallocate 231 | // Deallocate given memory piece of type T. 232 | template 233 | void SingularAllocator::deallocate(T* ptr, size_t n) { 234 | if(n == 1) { 235 | get_singular_mempool_manager().get_per_thread_mempool()->deallocate(ptr); 236 | } 237 | else { 238 | throw std::runtime_error("can only deallocate one object"); 239 | } 240 | } 241 | 242 | } // End of namespace tf. ---------------------------------------------------- 243 | 244 | -------------------------------------------------------------------------------- /pandar_pointcloud/include/pandar_pointcloud/input.h: -------------------------------------------------------------------------------- 1 | /* -*- mode: C++ -*- 2 | * 3 | * Copyright (C) 2007 Austin Robot Technology, Yaxin Liu, Patrick Beeson 4 | * Copyright (C) 2009, 2010 Austin Robot Technology, Jack O'Quin 5 | * Copyright (C) 2015, Jack O'Quin 6 | * Copyright (c) 2017 Hesai Photonics Technology, Yang Sheng 7 | * Copyright (c) 2020 Hesai Photonics Technology, Lingwen Fang 8 | * 9 | * License: Modified BSD Software License Agreement 10 | * 11 | * $Id$ 12 | */ 13 | 14 | /** @file 15 | * 16 | * Pandar128 3D LIDAR data input classes 17 | * 18 | * These classes provide raw Pandar128 LIDAR input packets from 19 | * either a live socket interface or a previously-saved PCAP dump 20 | * file. 21 | * 22 | * Classes: 23 | * 24 | * pandar::Input -- base class for accessing the data 25 | * independently of its source 26 | * 27 | * pandar::InputSocket -- derived class reads live data from the 28 | * device via a UDP socket 29 | * 30 | * pandar::InputPCAP -- derived class provides a similar interface 31 | * from a PCAP dump file 32 | */ 33 | 34 | #ifndef __PANDAR_INPUT_H 35 | #define __PANDAR_INPUT_H 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | #include 43 | #include 44 | #define UDP_VERSION_MAJOR_1 (1) 45 | #define UDP_VERSION_MINOR_3 (3) 46 | #define UDP_VERSION_MINOR_4 (4) 47 | #define UDP_VERSION_MAJOR_3 (3) 48 | #define UDP_VERSION_MINOR_2 (2) 49 | #define UDP_VERSION_1_3 "1.3" 50 | #define UDP_VERSION_1_4 "1.4" 51 | #define UDP_VERSION_3_2 "3.2" 52 | #define PANDAR128_SOB_SIZE (2) 53 | #define PANDAR128_VERSION_MAJOR_SIZE (1) 54 | #define PANDAR128_VERSION_MINOR_SIZE (1) 55 | #define PANDAR128_HEAD_RESERVED1_SIZE (2) 56 | #define PANDAR128_LASER_NUM_SIZE (1) 57 | #define PANDAR128_BLOCK_NUM_SIZE (1) 58 | #define PANDAR128_ECHO_COUNT_SIZE (1) 59 | #define PANDAR128_ECHO_NUM_SIZE (1) 60 | #define PANDAR128_HEAD_RESERVED2_SIZE (2) 61 | #define PANDAR128_HEAD_SIZE \ 62 | (PANDAR128_SOB_SIZE + PANDAR128_VERSION_MAJOR_SIZE + \ 63 | PANDAR128_VERSION_MINOR_SIZE + PANDAR128_HEAD_RESERVED1_SIZE + \ 64 | PANDAR128_LASER_NUM_SIZE + PANDAR128_BLOCK_NUM_SIZE + \ 65 | PANDAR128_ECHO_COUNT_SIZE + PANDAR128_ECHO_NUM_SIZE + \ 66 | PANDAR128_HEAD_RESERVED2_SIZE) 67 | #define PANDAR128_AZIMUTH_SIZE (2) 68 | #define DISTANCE_SIZE (2) 69 | #define INTENSITY_SIZE (1) 70 | #define CONFIDENCE_SIZE (1) 71 | #define PANDAR128_UNIT_WITHOUT_CONFIDENCE_SIZE (DISTANCE_SIZE + INTENSITY_SIZE) 72 | #define PANDAR128_UNIT_WITH_CONFIDENCE_SIZE (DISTANCE_SIZE + INTENSITY_SIZE + CONFIDENCE_SIZE) 73 | #define PANDAR128_TAIL_RESERVED1_SIZE (3) 74 | #define PANDAR128_TAIL_RESERVED2_SIZE (3) 75 | #define PANDAR128_SHUTDOWN_FLAG_SIZE (1) 76 | #define PANDAR128_TAIL_RESERVED3_SIZE (3) 77 | #define PANDAR128_MOTOR_SPEED_SIZE (2) 78 | #define PANDAR128_AZIMUTH_FLAG_SIZE (2) 79 | #define PANDAR128_TS_SIZE (4) 80 | #define PANDAR128_RETURN_MODE_SIZE (1) 81 | #define PANDAR128_FACTORY_INFO (1) 82 | #define PANDAR128_UTC_SIZE (6) 83 | #define PANDAR128_CRC_SIZE (4) 84 | #define PANDAR128_FUNCTION_SAFETY_SIZE (17) 85 | #define PANDAR128_IMU_SIZE (22) 86 | #define PANDAR128_SIGNATURE_SIZE (32) 87 | #define PANDAR128_SEQ_NUM_SIZE (4) 88 | 89 | enum enumIndex{ 90 | TIMESTAMP_INDEX, 91 | UTC_INDEX, 92 | SEQUENCE_NUMBER_INDEX, 93 | PACKET_SIZE, 94 | }; 95 | 96 | static std::map udpVersion13 = { 97 | {TIMESTAMP_INDEX, 796}, 98 | {UTC_INDEX, 802}, 99 | {SEQUENCE_NUMBER_INDEX, 817}, 100 | {PACKET_SIZE, 812}, 101 | }; 102 | 103 | static std::map udpVersion14 = { 104 | {TIMESTAMP_INDEX, 826}, 105 | {UTC_INDEX, 820}, 106 | {SEQUENCE_NUMBER_INDEX, 831}, 107 | {PACKET_SIZE, 893}, 108 | }; 109 | 110 | static std::map udpVersion32 = { 111 | {TIMESTAMP_INDEX, 826}, 112 | {UTC_INDEX, 820}, 113 | {SEQUENCE_NUMBER_INDEX, 831}, 114 | {PACKET_SIZE, 893}, 115 | }; 116 | 117 | typedef struct PandarPacket_s { 118 | ros::Time stamp; 119 | uint8_t data[1500]; 120 | uint32_t size; 121 | } PandarPacket; 122 | 123 | 124 | namespace pandar_pointcloud 125 | { 126 | static uint16_t DATA_PORT_NUMBER = 2368; // default data port 127 | static uint16_t GPS_PORT_NUMBER = 8308; // default position port 128 | 129 | #define PANDAR128_SEQUENCE_NUMBER_OFFSET (831) 130 | /** @brief pandar input base class */ 131 | class Input 132 | { 133 | public: 134 | Input(ros::NodeHandle private_nh, uint16_t port); 135 | virtual ~Input() {} 136 | 137 | /** @brief Read one pandar packet. 138 | * 139 | * @param pkt points to pandarPacket message 140 | * 141 | * @returns 0 if successful, 142 | * -1 if end of file 143 | * > 0 if incomplete packet (is this possible?) 144 | */ 145 | virtual int getPacket(PandarPacket *pkt) = 0; 146 | bool checkPacket(PandarPacket *pkt); 147 | void setUdpVersion(uint8_t major, uint8_t minor); 148 | std::string getUdpVersion(); 149 | 150 | protected: 151 | ros::NodeHandle private_nh_; 152 | uint16_t port_; 153 | std::string devip_str_; 154 | std::string m_sUdpVresion; 155 | bool m_bGetUdpVersion; 156 | int m_iTimestampIndex; 157 | int m_iUtcIindex; 158 | int m_iSequenceNumberIndex; 159 | int m_iPacketSize; 160 | }; 161 | 162 | /** @brief Live pandar input from socket. */ 163 | class InputSocket: public Input 164 | { 165 | public: 166 | InputSocket(ros::NodeHandle private_nh, 167 | std::string host_ip = "", 168 | uint16_t port = DATA_PORT_NUMBER, 169 | uint16_t gpsport = GPS_PORT_NUMBER, std::string multicast_ip = ""); 170 | virtual ~InputSocket(); 171 | 172 | virtual int getPacket(PandarPacket *pkt); 173 | void setDeviceIP( const std::string& ip ); 174 | private: 175 | 176 | private: 177 | int sockfd_; 178 | in_addr devip_; 179 | uint32_t m_u32Sequencenum; 180 | uint8_t seqnub1; 181 | uint8_t seqnub2; 182 | uint8_t seqnub3; 183 | uint8_t seqnub4; 184 | 185 | }; 186 | 187 | 188 | /** @brief pandar input from PCAP dump file. 189 | * 190 | * Dump files can be grabbed by libpcap, pandar's DSR software, 191 | * ethereal, wireshark, tcpdump, or the \ref vdump_command. 192 | */ 193 | class InputPCAP: public Input 194 | { 195 | public: 196 | InputPCAP(ros::NodeHandle private_nh, 197 | uint16_t port = DATA_PORT_NUMBER, 198 | double packet_rate = 0.0, 199 | std::string filename="", 200 | bool read_once=false, 201 | bool read_fast=false, 202 | double repeat_delay=0.0); 203 | virtual ~InputPCAP(); 204 | 205 | virtual int getPacket(PandarPacket *pkt); 206 | void setDeviceIP( const std::string& ip ); 207 | 208 | private: 209 | ros::Rate packet_rate_; 210 | std::string filename_; 211 | pcap_t *pcap_; 212 | bpf_program pcap_packet_filter_; 213 | char errbuf_[PCAP_ERRBUF_SIZE]; 214 | bool empty_; 215 | bool read_once_; 216 | bool read_fast_; 217 | double repeat_delay_; 218 | int gap; 219 | int64_t last_pkt_ts; 220 | int count; 221 | int64_t last_time; 222 | int64_t current_time; 223 | int64_t pkt_ts; 224 | }; 225 | 226 | } // pandar_driver namespace 227 | 228 | #endif // __PANDAR_INPUT_H 229 | -------------------------------------------------------------------------------- /pandar_pointcloud/src/taskflow/core/observer.hpp: -------------------------------------------------------------------------------- 1 | // 2019/07/31 - modified by Tsung-Wei Huang 2 | // - fixed the missing comma in outputing JSON 3 | // 4 | // 2019/06/13 - modified by Tsung-Wei Huang 5 | // - added TaskView interface 6 | // 7 | // 2019/04/17 - created by Tsung-Wei Huang 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include "task.hpp" 27 | 28 | namespace tf { 29 | 30 | /** 31 | @class: ExecutorObserverInterface 32 | 33 | @brief The interface class for creating an executor observer. 34 | 35 | The tf::ExecutorObserver class let users define methods to monitor the behaviors 36 | of an executor. 37 | This is particularly useful when you want to inspect the performance of an executor. 38 | */ 39 | class ExecutorObserverInterface { 40 | 41 | public: 42 | 43 | /** 44 | @brief virtual destructor 45 | */ 46 | virtual ~ExecutorObserverInterface() = default; 47 | 48 | /** 49 | @brief constructor-like method to call when the executor observer is fully created 50 | @param num_workers the number of the worker threads in the executor 51 | */ 52 | virtual void set_up(unsigned num_workers) = 0; 53 | 54 | /** 55 | @brief method to call before a worker thread executes a closure 56 | @param worker_id the id of this worker thread 57 | @param task_view a constant wrapper object to the task 58 | */ 59 | virtual void on_entry(unsigned worker_id, TaskView task_view) = 0; 60 | 61 | /** 62 | @brief method to call after a worker thread executed a closure 63 | @param worker_id the id of this worker thread 64 | @param task_view a constant wrapper object to the task 65 | */ 66 | virtual void on_exit(unsigned worker_id, TaskView task_view) = 0; 67 | }; 68 | 69 | // ------------------------------------------------------------------ 70 | 71 | /** 72 | @class: ExecutorObserver 73 | 74 | @brief Default executor observer to dump the execution timelines 75 | 76 | */ 77 | class ExecutorObserver : public ExecutorObserverInterface { 78 | 79 | friend class Executor; 80 | 81 | // data structure to record each task execution 82 | struct Execution { 83 | 84 | TaskView task_view; 85 | 86 | std::chrono::time_point beg; 87 | std::chrono::time_point end; 88 | 89 | Execution( 90 | TaskView tv, 91 | std::chrono::time_point b 92 | ) : 93 | task_view {tv}, beg {b} { 94 | } 95 | 96 | Execution( 97 | TaskView tv, 98 | std::chrono::time_point b, 99 | std::chrono::time_point e 100 | ) : 101 | task_view {tv}, beg {b}, end {e} { 102 | } 103 | }; 104 | 105 | // data structure to store the entire execution timeline 106 | struct Timeline { 107 | std::chrono::time_point origin; 108 | std::vector> executions; 109 | }; 110 | 111 | public: 112 | 113 | /** 114 | @brief dump the timelines in JSON format to an ostream 115 | @param ostream the target std::ostream to dump 116 | */ 117 | inline void dump(std::ostream& ostream) const; 118 | 119 | /** 120 | @brief dump the timelines in JSON to a std::string 121 | @return a JSON string 122 | */ 123 | inline std::string dump() const; 124 | 125 | /** 126 | @brief clear the timeline data 127 | */ 128 | inline void clear(); 129 | 130 | /** 131 | @brief get the number of total tasks in the observer 132 | @return number of total tasks 133 | */ 134 | inline size_t num_tasks() const; 135 | 136 | private: 137 | 138 | inline void set_up(unsigned num_workers) override final; 139 | inline void on_entry(unsigned worker_id, TaskView task_view) override final; 140 | inline void on_exit(unsigned worker_id, TaskView task_view) override final; 141 | 142 | Timeline _timeline; 143 | }; 144 | 145 | // Procedure: set_up 146 | inline void ExecutorObserver::set_up(unsigned num_workers) { 147 | 148 | _timeline.executions.resize(num_workers); 149 | 150 | for(unsigned w=0; w(tv); // avoid warning from compiler 165 | assert(_timeline.executions[w].size() > 0); 166 | _timeline.executions[w].back().end = std::chrono::steady_clock::now(); 167 | } 168 | 169 | // Function: clear 170 | inline void ExecutorObserver::clear() { 171 | for(size_t w=0; w<_timeline.executions.size(); ++w) { 172 | _timeline.executions[w].clear(); 173 | } 174 | } 175 | 176 | // Procedure: dump 177 | inline void ExecutorObserver::dump(std::ostream& os) const { 178 | 179 | size_t first; 180 | 181 | for(first = 0; first<_timeline.executions.size(); ++first) { 182 | if(_timeline.executions[first].size() > 0) { 183 | break; 184 | } 185 | } 186 | 187 | os << '['; 188 | 189 | for(size_t w=first; w<_timeline.executions.size(); w++) { 190 | 191 | if(w != first && _timeline.executions[w].size() > 0) { 192 | os << ','; 193 | } 194 | 195 | for(size_t i=0; i<_timeline.executions[w].size(); i++) { 196 | 197 | os << '{' 198 | << "\"cat\":\"ExecutorObserver\"," 199 | << "\"name\":\"" << _timeline.executions[w][i].task_view.name() << "\"," 200 | << "\"ph\":\"X\"," 201 | << "\"pid\":1," 202 | << "\"tid\":" << w << ',' 203 | << "\"ts\":" << std::chrono::duration_cast( 204 | _timeline.executions[w][i].beg - _timeline.origin 205 | ).count() << ',' 206 | << "\"dur\":" << std::chrono::duration_cast( 207 | _timeline.executions[w][i].end - _timeline.executions[w][i].beg 208 | ).count(); 209 | 210 | if(i != _timeline.executions[w].size() - 1) { 211 | os << "},"; 212 | } 213 | else { 214 | os << '}'; 215 | } 216 | } 217 | } 218 | os << "]\n"; 219 | } 220 | 221 | // Function: dump 222 | inline std::string ExecutorObserver::dump() const { 223 | std::ostringstream oss; 224 | dump(oss); 225 | return oss.str(); 226 | } 227 | 228 | // Function: num_tasks 229 | inline size_t ExecutorObserver::num_tasks() const { 230 | return std::accumulate( 231 | _timeline.executions.begin(), _timeline.executions.end(), size_t{0}, 232 | [](size_t sum, const auto& exe){ 233 | return sum + exe.size(); 234 | } 235 | ); 236 | } 237 | 238 | 239 | } // end of namespace tf ------------------------------------------- 240 | 241 | 242 | -------------------------------------------------------------------------------- /pandar_pointcloud/src/taskflow/utility/generic_allocator.hpp: -------------------------------------------------------------------------------- 1 | // 2019/02/10 - modified by Tsung-Wei Huang 2 | // - fixed the compilation error on MS platform 3 | // 4 | // 2019/02/08 - created by Chun-Xun Lin 5 | // - added a singular memory allocator 6 | // - refactored by Tsung-Wei Huang 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | namespace tf { 17 | 18 | // Class: Mempool 19 | template 20 | struct Mempool { 21 | 22 | // FreeList uses Node array to store Node* 23 | struct FreeList { 24 | typedef T* pointer; 25 | typedef pointer* ppointer; 26 | 27 | FreeList() = default; 28 | 29 | void push(pointer ptr) { 30 | *(reinterpret_cast(ptr)) = top; 31 | top = ptr; 32 | } 33 | 34 | // Must use empty() before calling pop 35 | pointer pop() { 36 | assert(!empty()); 37 | pointer retval = top; 38 | top = *(reinterpret_cast(top)); 39 | return retval; 40 | } 41 | 42 | bool empty() const { return top == nullptr; } 43 | 44 | pointer top {nullptr}; 45 | }; 46 | 47 | struct MemBlock{ 48 | T* block; // Block memory. 49 | size_t size; // Size of the block (count). 50 | struct MemBlock* next; // Pointer to the next block. 51 | }; 52 | 53 | // Ctor 54 | Mempool() { 55 | head = allocate_memblock(1024); 56 | tail = head; 57 | } 58 | 59 | // Dtor 60 | ~Mempool() { 61 | for(auto* prev = head; head!=nullptr;) { 62 | head = head->next; 63 | std::free(prev->block); 64 | std::free(prev); 65 | prev = head; 66 | } 67 | } 68 | 69 | size_t rank(const uint64_t v) { 70 | assert(v != 0 && (v&(v-1)) == 0); 71 | #ifdef __GNUC__ 72 | return __builtin_ffs(v) - 1; // GCC 73 | #endif 74 | #ifdef _MSC_VER 75 | return CHAR_BIT * sizeof(x)-__lzcnt( x ); // Visual studio 76 | #endif 77 | } 78 | 79 | void deposit() { 80 | auto left = tail->size - used; 81 | size_t pos = rank(left & (~left+1)); // get rightmost set bit 82 | size_t num {0}; 83 | left >>= pos; 84 | for(; left; left>>=1, pos++) { 85 | if(left & 1) { 86 | free_list[pos].push(&tail->block[used+num]); 87 | num += (1 << pos); 88 | } 89 | } 90 | assert(num + used == tail->size); 91 | } 92 | 93 | MemBlock* allocate_memblock(size_t n) { 94 | assert(n && (n & (n-1)) == 0); 95 | MemBlock* ptr = static_cast(std::malloc(sizeof(MemBlock))); 96 | ptr->block = static_cast(std::malloc(n*sizeof(T))); 97 | ptr->size = n; 98 | ptr->next = nullptr; 99 | return ptr; 100 | } 101 | 102 | T* allocate(size_t n) { 103 | // TODO: we only deal with n which is power of 2 104 | assert(n && (n & (n-1)) == 0); 105 | 106 | auto r = rank(n); 107 | 108 | if(!free_list[r].empty()) { 109 | return free_list[r].pop(); 110 | } 111 | 112 | if(tail->size < n + used) { 113 | auto next = allocate_memblock(std::max(n, tail->size) << 1); 114 | if(tail->size > used) deposit(); 115 | tail->next = next; 116 | tail = next; 117 | used = 0; 118 | } 119 | used += n; 120 | return &(tail->block[used-n]); 121 | } 122 | 123 | void deallocate(T* ptr, size_t n) { 124 | // TODO: we only deal with n which is power of 2 125 | assert(n && (n & (n-1)) == 0); 126 | auto pos = rank(n); 127 | free_list[pos].push(ptr); 128 | } 129 | 130 | FreeList free_list [64]; 131 | 132 | MemBlock *head {nullptr}; 133 | MemBlock *tail {nullptr}; 134 | size_t used {0}; 135 | }; 136 | 137 | // Class: MempoolManager 138 | template 139 | struct MempoolManager { 140 | 141 | struct Handle { 142 | 143 | Handle(MempoolManager &mgr) : manager {mgr} { 144 | std::scoped_lock lock(mgr.mtx); 145 | if(mgr.pools.empty()) { 146 | mempool = new Mempool(); 147 | } 148 | else { 149 | mempool = mgr.pools.back(); 150 | mgr.pools.pop_back(); 151 | } 152 | } 153 | 154 | ~Handle() { 155 | // Return the memory pool to MempoolManager 156 | std::scoped_lock lock(manager.mtx); 157 | manager.pools.emplace_back(mempool); 158 | } 159 | 160 | MempoolManager& manager; 161 | Mempool* mempool {nullptr}; 162 | }; 163 | 164 | // Ctor 165 | MempoolManager() { 166 | pools.reserve(std::thread::hardware_concurrency()); 167 | } 168 | 169 | // Dtor 170 | ~MempoolManager() { 171 | std::scoped_lock lock(mtx); 172 | for(auto& p : pools) { 173 | delete p; 174 | } 175 | } 176 | 177 | Mempool* get_per_thread_mempool() { 178 | thread_local Handle handle {*this}; 179 | return handle.mempool; 180 | } 181 | 182 | std::mutex mtx; 183 | std::vector*> pools; 184 | }; 185 | 186 | // The singleton allocator 187 | template 188 | auto& get_mempool_manager() { 189 | static MempoolManager manager; 190 | return manager; 191 | } 192 | 193 | // Class: GenericAllocator 194 | template 195 | class GenericAllocator { 196 | 197 | public: 198 | 199 | // Allocator traits 200 | typedef T value_type ; 201 | typedef T* pointer ; 202 | typedef const T* const_pointer ; 203 | typedef T& reference ; 204 | typedef const T& const_reference; 205 | typedef std::size_t size_type ; 206 | typedef std::ptrdiff_t difference_type; 207 | 208 | template 209 | struct rebind { 210 | typedef GenericAllocator other; 211 | }; 212 | 213 | explicit GenericAllocator() {} 214 | ~GenericAllocator() {} 215 | 216 | explicit GenericAllocator(const GenericAllocator&) {} 217 | 218 | template 219 | explicit GenericAllocator(const GenericAllocator&) {} 220 | 221 | T* allocate(size_t n=1) ; 222 | void deallocate(T*, size_t n=1); 223 | 224 | template 225 | void construct(T*, ArgsT&&...); 226 | void destroy(T*); 227 | 228 | //GenericAllocator & operator = (const GenericAllocator &) {} 229 | 230 | bool operator == (const GenericAllocator &) const { return true; } 231 | bool operator != (const GenericAllocator &) const { return false; } 232 | }; 233 | 234 | // Procedure: construct 235 | // Construct an item with placement new. 236 | template 237 | template 238 | void GenericAllocator::construct(T* ptr, ArgsT&&... args) { 239 | new (ptr) T(std::forward(args)...); 240 | } 241 | 242 | // Procedure: destroy 243 | // Destroy an item 244 | template 245 | void GenericAllocator::destroy(T* ptr) { 246 | ptr->~T(); 247 | } 248 | 249 | // Function: allocate 250 | // Allocate a memory piece of type T from the memory pool and return the T* to that memory. 251 | template 252 | T* GenericAllocator::allocate(size_t n) { 253 | return get_mempool_manager().get_per_thread_mempool()->allocate(n); 254 | } 255 | 256 | // Function: deallocate 257 | // Deallocate given memory piece of type T. 258 | template 259 | void GenericAllocator::deallocate(T* ptr, size_t n) { 260 | get_mempool_manager().get_per_thread_mempool()->deallocate(ptr, n); 261 | } 262 | 263 | } // End of namespace tf. ---------------------------------------------------- 264 | 265 | -------------------------------------------------------------------------------- /pandar_pointcloud/src/taskflow/core/taskflow.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "flow_builder.hpp" 6 | #include "topology.hpp" 7 | 8 | namespace tf { 9 | 10 | // ---------------------------------------------------------------------------- 11 | 12 | /** 13 | @class Taskflow 14 | 15 | @brief the class to create a task dependency graph 16 | 17 | */ 18 | class Taskflow : public FlowBuilder { 19 | 20 | friend class Topology; 21 | friend class Executor; 22 | friend class FlowBuilder; 23 | 24 | public: 25 | 26 | /** 27 | @brief constructs a taskflow with a given name 28 | */ 29 | Taskflow(const std::string& name); 30 | 31 | /** 32 | @brief constructs a taskflow 33 | */ 34 | Taskflow(); 35 | 36 | /** 37 | @brief destroy the taskflow (virtual call) 38 | */ 39 | virtual ~Taskflow(); 40 | 41 | /** 42 | @brief dumps the taskflow to a std::ostream in DOT format 43 | 44 | @param ostream a std::ostream target 45 | */ 46 | void dump(std::ostream& ostream) const; 47 | 48 | /** 49 | @brief dumps the taskflow in DOT format to a std::string 50 | */ 51 | std::string dump() const; 52 | 53 | /** 54 | @brief queries the number of tasks in the taskflow 55 | */ 56 | size_t num_tasks() const; 57 | 58 | /** 59 | @brief queries the emptiness of the taskflow 60 | */ 61 | bool empty() const; 62 | 63 | /** 64 | @brief sets the name of the taskflow 65 | 66 | @return @c *this 67 | */ 68 | tf::Taskflow& name(const std::string&); 69 | 70 | /** 71 | @brief queries the name of the taskflow 72 | */ 73 | const std::string& name() const ; 74 | 75 | /** 76 | @brief clears the associated task dependency graph 77 | */ 78 | void clear(); 79 | 80 | /** 81 | @brief applies an visitor callable to each task in the taskflow 82 | */ 83 | template 84 | void for_each_task(V&& visitor) const; 85 | 86 | private: 87 | 88 | std::string _name; 89 | 90 | Graph _graph; 91 | 92 | std::mutex _mtx; 93 | 94 | std::list _topologies; 95 | 96 | void _dump(std::ostream&, const Taskflow*) const; 97 | 98 | void _dump( 99 | std::ostream&, 100 | const Node*, 101 | std::stack&, 102 | std::unordered_set& 103 | ) const; 104 | 105 | void _dump( 106 | std::ostream&, 107 | const Graph&, 108 | std::stack&, 109 | std::unordered_set& 110 | ) const; 111 | }; 112 | 113 | // Constructor 114 | inline Taskflow::Taskflow(const std::string& name) : 115 | FlowBuilder {_graph}, 116 | _name {name} { 117 | } 118 | 119 | // Constructor 120 | inline Taskflow::Taskflow() : FlowBuilder{_graph} { 121 | } 122 | 123 | // Destructor 124 | inline Taskflow::~Taskflow() { 125 | assert(_topologies.empty()); 126 | } 127 | 128 | // Procedure: 129 | inline void Taskflow::clear() { 130 | _graph.clear(); 131 | } 132 | 133 | // Function: num_noces 134 | inline size_t Taskflow::num_tasks() const { 135 | return _graph.size(); 136 | } 137 | 138 | // Function: empty 139 | inline bool Taskflow::empty() const { 140 | return _graph.empty(); 141 | } 142 | 143 | // Function: name 144 | inline Taskflow& Taskflow::name(const std::string &name) { 145 | _name = name; 146 | return *this; 147 | } 148 | 149 | // Function: name 150 | inline const std::string& Taskflow::name() const { 151 | return _name; 152 | } 153 | 154 | // Function: for_each_task 155 | template 156 | void Taskflow::for_each_task(V&& visitor) const { 157 | for(size_t i=0; i<_graph.nodes().size(); ++i) { 158 | visitor(Task(_graph.nodes()[i].get())); 159 | } 160 | } 161 | 162 | // Procedure: dump 163 | inline std::string Taskflow::dump() const { 164 | std::ostringstream oss; 165 | dump(oss); 166 | return oss.str(); 167 | } 168 | 169 | // Function: dump 170 | inline void Taskflow::dump(std::ostream& os) const { 171 | os << "digraph Taskflow {\n"; 172 | _dump(os, this); 173 | os << "}\n"; 174 | } 175 | 176 | // Procedure: _dump 177 | inline void Taskflow::_dump(std::ostream& os, const Taskflow* top) const { 178 | 179 | std::stack stack; 180 | std::unordered_set visited; 181 | 182 | stack.push(top); 183 | visited.insert(top); 184 | 185 | while(!stack.empty()) { 186 | 187 | auto f = stack.top(); 188 | stack.pop(); 189 | 190 | os << "subgraph cluster_p" << f << " {\nlabel=\"Taskflow: "; 191 | if(f->_name.empty()) os << 'p' << f; 192 | else os << f->_name; 193 | os << "\";\n"; 194 | _dump(os, f->_graph, stack, visited); 195 | os << "}\n"; 196 | } 197 | } 198 | 199 | // Procedure: _dump 200 | inline void Taskflow::_dump( 201 | std::ostream& os, 202 | const Node* node, 203 | std::stack& stack, 204 | std::unordered_set& visited 205 | ) const { 206 | 207 | os << 'p' << node << "[label=\""; 208 | if(node->_name.empty()) os << 'p' << node; 209 | else os << node->_name; 210 | os << "\" "; 211 | 212 | // condition node is colored green 213 | if(node->_work.index() == Node::CONDITION_WORK) { 214 | os << " shape=diamond color=black fillcolor=aquamarine style=filled"; 215 | } 216 | 217 | os << "];\n"; 218 | 219 | for(size_t s=0; s_successors.size(); ++s) { 220 | if(node->_work.index() == Node::CONDITION_WORK) { 221 | // case edge is dashed 222 | os << 'p' << node << " -> p" << node->_successors[s] 223 | << " [style=dashed label=\"" << s << "\"];\n"; 224 | } 225 | else { 226 | os << 'p' << node << " -> p" << node->_successors[s] << ";\n"; 227 | } 228 | } 229 | 230 | // subflow join node 231 | if(node->_parent && node->_successors.size() == 0) { 232 | os << 'p' << node << " -> p" << node->_parent << ";\n"; 233 | } 234 | 235 | if(node->_subgraph && !node->_subgraph->empty()) { 236 | 237 | os << "subgraph cluster_p" << node << " {\nlabel=\"Subflow: "; 238 | if(node->_name.empty()) os << 'p' << node; 239 | else os << node->_name; 240 | 241 | os << "\";\n" << "color=blue\n"; 242 | _dump(os, *(node->_subgraph), stack, visited); 243 | os << "}\n"; 244 | } 245 | } 246 | 247 | // Procedure: _dump 248 | inline void Taskflow::_dump( 249 | std::ostream& os, 250 | const Graph& graph, 251 | std::stack& stack, 252 | std::unordered_set& visited 253 | ) const { 254 | 255 | for(const auto& n : graph.nodes()) { 256 | // regular task 257 | if(auto module = n->_module; !module) { 258 | _dump(os, n.get(), stack, visited); 259 | } 260 | // module task 261 | else { 262 | os << 'p' << n.get() << "[shape=box, color=blue, label=\""; 263 | if(n->_name.empty()) os << n.get(); 264 | else os << n->_name; 265 | os << " [Taskflow: "; 266 | if(module->_name.empty()) os << 'p' << module; 267 | else os << module->_name; 268 | os << "]\"];\n"; 269 | 270 | if(visited.find(module) == visited.end()) { 271 | visited.insert(module); 272 | stack.push(module); 273 | } 274 | 275 | for(const auto s : n->_successors) { 276 | os << 'p' << n.get() << "->" << 'p' << s << ";\n"; 277 | } 278 | } 279 | } 280 | } 281 | 282 | // ---------------------------------------------------------------------------- 283 | // Backward compatibility 284 | // ---------------------------------------------------------------------------- 285 | 286 | using Framework = Taskflow; 287 | 288 | } // end of namespace tf. --------------------------------------------------- 289 | 290 | -------------------------------------------------------------------------------- /pandar_pointcloud/params/Pandar90_Firetimes.csv: -------------------------------------------------------------------------------- 1 | 2.85,distance>=A1,distance=A1,distance=A1,distance=A1,distance=A1,distance=A1,distance=A1,distance=A1,distance 7 | #include 8 | #include 9 | 10 | namespace tf { 11 | 12 | /** 13 | @class: WorkStealingQueue 14 | 15 | @tparam T data type 16 | 17 | @brief Lock-free unbounded single-producer multiple-consumer queue. 18 | 19 | This class implements the work stealing queue described in the paper, 20 | "Correct and Efficient Work-Stealing for Weak Memory Models," 21 | available at https://www.di.ens.fr/~zappa/readings/ppopp13.pdf. 22 | 23 | Only the queue owner can perform pop and push operations, 24 | while others can steal data from the queue. 25 | */ 26 | template 27 | class WorkStealingQueue { 28 | 29 | //constexpr static int64_t cacheline_size = 64; 30 | 31 | //using storage_type = std::aligned_storage_t; 32 | 33 | struct Array { 34 | 35 | int64_t C; 36 | int64_t M; 37 | //storage_type* S; 38 | std::atomic* S; 39 | 40 | //T* S; 41 | 42 | explicit Array(int64_t c) : 43 | C {c}, 44 | M {c-1}, 45 | //S {new storage_type[C]} { 46 | //S {new T[static_cast(C)]} { 47 | S {new std::atomic[static_cast(C)]} { 48 | //for(int64_t i=0; i(std::addressof(S[i]))->~T(); 56 | //} 57 | delete [] S; 58 | } 59 | 60 | int64_t capacity() const noexcept { 61 | return C; 62 | } 63 | 64 | template 65 | void push(int64_t i, O&& o) noexcept { 66 | //T* ptr = reinterpret_cast(std::addressof(S[i & M])); 67 | //*ptr = std::forward(o); 68 | //S[i & M] = std::forward(o); 69 | S[i & M].store(std::forward(o), std::memory_order_relaxed); 70 | } 71 | 72 | T pop(int64_t i) noexcept { 73 | //return *reinterpret_cast(std::addressof(S[i & M])); 74 | //return S[i & M]; 75 | return S[i & M].load(std::memory_order_relaxed); 76 | } 77 | 78 | Array* resize(int64_t b, int64_t t) { 79 | Array* ptr = new Array {2*C}; 80 | for(int64_t i=t; i!=b; ++i) { 81 | ptr->push(i, pop(i)); 82 | } 83 | return ptr; 84 | } 85 | 86 | }; 87 | 88 | std::atomic _top; 89 | std::atomic _bottom; 90 | std::atomic _array; 91 | std::vector _garbage; 92 | //char _padding[cacheline_size]; 93 | 94 | public: 95 | 96 | /** 97 | @brief constructs the queue with a given capacity 98 | 99 | @param capacity the capacity of the queue (must be power of 2) 100 | */ 101 | explicit WorkStealingQueue(int64_t capacity = 1024); 102 | 103 | /** 104 | @brief destructs the queue 105 | */ 106 | ~WorkStealingQueue(); 107 | 108 | /** 109 | @brief queries if the queue is empty at the time of this call 110 | */ 111 | bool empty() const noexcept; 112 | 113 | /** 114 | @brief queries the number of items at the time of this call 115 | */ 116 | size_t size() const noexcept; 117 | 118 | /** 119 | @brief queries the capacity of the queue 120 | */ 121 | int64_t capacity() const noexcept; 122 | 123 | /** 124 | @brief inserts an item to the queue 125 | 126 | Only the owner thread can insert an item to the queue. 127 | The operation can trigger the queue to resize its capacity 128 | if more space is required. 129 | 130 | @tparam O data type 131 | 132 | @param item the item to perfect-forward to the queue 133 | */ 134 | template 135 | void push(O&& item); 136 | 137 | /** 138 | @brief pops out an item from the queue 139 | 140 | Only the owner thread can pop out an item from the queue. 141 | The return can be a @std_nullopt if this operation failed (empty queue). 142 | */ 143 | std::optional pop(); 144 | 145 | /** 146 | @brief steals an item from the queue 147 | 148 | Any threads can try to steal an item from the queue. 149 | The return can be a @std_nullopt if this operation failed (not necessary empty). 150 | */ 151 | std::optional steal(); 152 | }; 153 | 154 | // Constructor 155 | template 156 | WorkStealingQueue::WorkStealingQueue(int64_t c) { 157 | assert(c && (!(c & (c-1)))); 158 | _top.store(0, std::memory_order_relaxed); 159 | _bottom.store(0, std::memory_order_relaxed); 160 | _array.store(new Array{c}, std::memory_order_relaxed); 161 | _garbage.reserve(32); 162 | } 163 | 164 | // Destructor 165 | template 166 | WorkStealingQueue::~WorkStealingQueue() { 167 | for(auto a : _garbage) { 168 | delete a; 169 | } 170 | delete _array.load(); 171 | } 172 | 173 | // Function: empty 174 | template 175 | bool WorkStealingQueue::empty() const noexcept { 176 | int64_t b = _bottom.load(std::memory_order_relaxed); 177 | int64_t t = _top.load(std::memory_order_relaxed); 178 | return b <= t; 179 | } 180 | 181 | // Function: size 182 | template 183 | size_t WorkStealingQueue::size() const noexcept { 184 | int64_t b = _bottom.load(std::memory_order_relaxed); 185 | int64_t t = _top.load(std::memory_order_relaxed); 186 | return static_cast(b >= t ? b - t : 0); 187 | } 188 | 189 | // Function: push 190 | template 191 | template 192 | void WorkStealingQueue::push(O&& o) { 193 | int64_t b = _bottom.load(std::memory_order_relaxed); 194 | int64_t t = _top.load(std::memory_order_acquire); 195 | Array* a = _array.load(std::memory_order_relaxed); 196 | 197 | // queue is full 198 | if(a->capacity() - 1 < (b - t)) { 199 | Array* tmp = a->resize(b, t); 200 | _garbage.push_back(a); 201 | std::swap(a, tmp); 202 | _array.store(a, std::memory_order_relaxed); 203 | } 204 | 205 | a->push(b, std::forward(o)); 206 | std::atomic_thread_fence(std::memory_order_release); 207 | _bottom.store(b + 1, std::memory_order_relaxed); 208 | } 209 | 210 | // Function: pop 211 | template 212 | std::optional WorkStealingQueue::pop() { 213 | int64_t b = _bottom.load(std::memory_order_relaxed) - 1; 214 | Array* a = _array.load(std::memory_order_relaxed); 215 | _bottom.store(b, std::memory_order_relaxed); 216 | std::atomic_thread_fence(std::memory_order_seq_cst); 217 | int64_t t = _top.load(std::memory_order_relaxed); 218 | 219 | std::optional item; 220 | 221 | if(t <= b) { 222 | item = a->pop(b); 223 | if(t == b) { 224 | // the last item just got stolen 225 | if(!_top.compare_exchange_strong(t, t+1, 226 | std::memory_order_seq_cst, 227 | std::memory_order_relaxed)) { 228 | item = std::nullopt; 229 | } 230 | _bottom.store(b + 1, std::memory_order_relaxed); 231 | } 232 | } 233 | else { 234 | _bottom.store(b + 1, std::memory_order_relaxed); 235 | } 236 | 237 | return item; 238 | } 239 | 240 | // Function: steal 241 | template 242 | std::optional WorkStealingQueue::steal() { 243 | int64_t t = _top.load(std::memory_order_acquire); 244 | std::atomic_thread_fence(std::memory_order_seq_cst); 245 | int64_t b = _bottom.load(std::memory_order_acquire); 246 | 247 | std::optional item; 248 | 249 | if(t < b) { 250 | Array* a = _array.load(std::memory_order_consume); 251 | item = a->pop(t); 252 | if(!_top.compare_exchange_strong(t, t+1, 253 | std::memory_order_seq_cst, 254 | std::memory_order_relaxed)) { 255 | return std::nullopt; 256 | } 257 | } 258 | 259 | return item; 260 | } 261 | 262 | // Function: capacity 263 | template 264 | int64_t WorkStealingQueue::capacity() const noexcept { 265 | return _array.load(std::memory_order_relaxed)->capacity(); 266 | } 267 | 268 | } // end of namespace tf ----------------------------------------------------- 269 | -------------------------------------------------------------------------------- /pandar_pointcloud/src/taskflow/core/notifier.hpp: -------------------------------------------------------------------------------- 1 | // 2019/02/09 - created by Tsung-Wei Huang 2 | // - modified the event count from Eigen 3 | 4 | #pragma once 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | // This file is part of Eigen, a lightweight C++ template library 22 | // for linear algebra. 23 | // 24 | // Copyright (C) 2016 Dmitry Vyukov 25 | // 26 | // This Source Code Form is subject to the terms of the Mozilla 27 | // Public License v. 2.0. If a copy of the MPL was not distributed 28 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 29 | 30 | namespace tf { 31 | 32 | // Notifier allows to wait for arbitrary predicates in non-blocking 33 | // algorithms. Think of condition variable, but wait predicate does not need to 34 | // be protected by a mutex. Usage: 35 | // Waiting thread does: 36 | // 37 | // if (predicate) 38 | // return act(); 39 | // Notifier::Waiter& w = waiters[my_index]; 40 | // ec.prepare_wait(&w); 41 | // if (predicate) { 42 | // ec.cancel_wait(&w); 43 | // return act(); 44 | // } 45 | // ec.commit_wait(&w); 46 | // 47 | // Notifying thread does: 48 | // 49 | // predicate = true; 50 | // ec.notify(true); 51 | // 52 | // notify is cheap if there are no waiting threads. prepare_wait/commit_wait are not 53 | // cheap, but they are executed only if the preceeding predicate check has 54 | // failed. 55 | // 56 | // Algorihtm outline: 57 | // There are two main variables: predicate (managed by user) and _state. 58 | // Operation closely resembles Dekker mutual algorithm: 59 | // https://en.wikipedia.org/wiki/Dekker%27s_algorithm 60 | // Waiting thread sets _state then checks predicate, Notifying thread sets 61 | // predicate then checks _state. Due to seq_cst fences in between these 62 | // operations it is guaranteed than either waiter will see predicate change 63 | // and won't block, or notifying thread will see _state change and will unblock 64 | // the waiter, or both. But it can't happen that both threads don't see each 65 | // other changes, which would lead to deadlock. 66 | class Notifier { 67 | 68 | public: 69 | 70 | struct Waiter { 71 | std::atomic next; 72 | std::mutex mu; 73 | std::condition_variable cv; 74 | uint64_t epoch; 75 | unsigned state; 76 | enum { 77 | kNotSignaled, 78 | kWaiting, 79 | kSignaled, 80 | }; 81 | }; 82 | 83 | explicit Notifier(std::vector& waiters) : _waiters{waiters} { 84 | assert(waiters.size() < (1 << kWaiterBits) - 1); 85 | // Initialize epoch to something close to overflow to test overflow. 86 | _state = kStackMask | (kEpochMask - kEpochInc * waiters.size() * 2); 87 | } 88 | 89 | ~Notifier() { 90 | // Ensure there are no waiters. 91 | assert((_state.load() & (kStackMask | kWaiterMask)) == kStackMask); 92 | } 93 | 94 | // prepare_wait prepares for waiting. 95 | // After calling this function the thread must re-check the wait predicate 96 | // and call either cancel_wait or commit_wait passing the same Waiter object. 97 | void prepare_wait(Waiter* w) { 98 | w->epoch = _state.fetch_add(kWaiterInc, std::memory_order_relaxed); 99 | std::atomic_thread_fence(std::memory_order_seq_cst); 100 | } 101 | 102 | // commit_wait commits waiting. 103 | void commit_wait(Waiter* w) { 104 | w->state = Waiter::kNotSignaled; 105 | // Modification epoch of this waiter. 106 | uint64_t epoch = 107 | (w->epoch & kEpochMask) + 108 | (((w->epoch & kWaiterMask) >> kWaiterShift) << kEpochShift); 109 | uint64_t state = _state.load(std::memory_order_seq_cst); 110 | for (;;) { 111 | if (int64_t((state & kEpochMask) - epoch) < 0) { 112 | // The preceeding waiter has not decided on its fate. Wait until it 113 | // calls either cancel_wait or commit_wait, or is notified. 114 | std::this_thread::yield(); 115 | state = _state.load(std::memory_order_seq_cst); 116 | continue; 117 | } 118 | // We've already been notified. 119 | if (int64_t((state & kEpochMask) - epoch) > 0) return; 120 | // Remove this thread from prewait counter and add it to the waiter list. 121 | assert((state & kWaiterMask) != 0); 122 | uint64_t newstate = state - kWaiterInc + kEpochInc; 123 | newstate = (newstate & ~kStackMask) | (w - &_waiters[0]); 124 | if ((state & kStackMask) == kStackMask) 125 | w->next.store(nullptr, std::memory_order_relaxed); 126 | else 127 | w->next.store(&_waiters[state & kStackMask], std::memory_order_relaxed); 128 | if (_state.compare_exchange_weak(state, newstate, 129 | std::memory_order_release)) 130 | break; 131 | } 132 | _park(w); 133 | } 134 | 135 | // cancel_wait cancels effects of the previous prepare_wait call. 136 | void cancel_wait(Waiter* w) { 137 | uint64_t epoch = 138 | (w->epoch & kEpochMask) + 139 | (((w->epoch & kWaiterMask) >> kWaiterShift) << kEpochShift); 140 | uint64_t state = _state.load(std::memory_order_relaxed); 141 | for (;;) { 142 | if (int64_t((state & kEpochMask) - epoch) < 0) { 143 | // The preceeding waiter has not decided on its fate. Wait until it 144 | // calls either cancel_wait or commit_wait, or is notified. 145 | std::this_thread::yield(); 146 | state = _state.load(std::memory_order_relaxed); 147 | continue; 148 | } 149 | // We've already been notified. 150 | if (int64_t((state & kEpochMask) - epoch) > 0) return; 151 | // Remove this thread from prewait counter. 152 | assert((state & kWaiterMask) != 0); 153 | if (_state.compare_exchange_weak(state, state - kWaiterInc + kEpochInc, 154 | std::memory_order_relaxed)) 155 | return; 156 | } 157 | } 158 | 159 | // notify wakes one or all waiting threads. 160 | // Must be called after changing the associated wait predicate. 161 | void notify(bool all) { 162 | std::atomic_thread_fence(std::memory_order_seq_cst); 163 | uint64_t state = _state.load(std::memory_order_acquire); 164 | for (;;) { 165 | // Easy case: no waiters. 166 | if ((state & kStackMask) == kStackMask && (state & kWaiterMask) == 0) 167 | return; 168 | uint64_t waiters = (state & kWaiterMask) >> kWaiterShift; 169 | uint64_t newstate; 170 | if (all) { 171 | // Reset prewait counter and empty wait list. 172 | newstate = (state & kEpochMask) + (kEpochInc * waiters) + kStackMask; 173 | } else if (waiters) { 174 | // There is a thread in pre-wait state, unblock it. 175 | newstate = state + kEpochInc - kWaiterInc; 176 | } else { 177 | // Pop a waiter from list and unpark it. 178 | Waiter* w = &_waiters[state & kStackMask]; 179 | Waiter* wnext = w->next.load(std::memory_order_relaxed); 180 | uint64_t next = kStackMask; 181 | if (wnext != nullptr) next = wnext - &_waiters[0]; 182 | // Note: we don't add kEpochInc here. ABA problem on the lock-free stack 183 | // can't happen because a waiter is re-pushed onto the stack only after 184 | // it was in the pre-wait state which inevitably leads to epoch 185 | // increment. 186 | newstate = (state & kEpochMask) + next; 187 | } 188 | if (_state.compare_exchange_weak(state, newstate, 189 | std::memory_order_acquire)) { 190 | if (!all && waiters) return; // unblocked pre-wait thread 191 | if ((state & kStackMask) == kStackMask) return; 192 | Waiter* w = &_waiters[state & kStackMask]; 193 | if (!all) w->next.store(nullptr, std::memory_order_relaxed); 194 | _unpark(w); 195 | return; 196 | } 197 | } 198 | } 199 | 200 | private: 201 | 202 | // State_ layout: 203 | // - low kStackBits is a stack of waiters committed wait. 204 | // - next kWaiterBits is count of waiters in prewait state. 205 | // - next kEpochBits is modification counter. 206 | static const uint64_t kStackBits = 16; 207 | static const uint64_t kStackMask = (1ull << kStackBits) - 1; 208 | static const uint64_t kWaiterBits = 16; 209 | static const uint64_t kWaiterShift = 16; 210 | static const uint64_t kWaiterMask = ((1ull << kWaiterBits) - 1) 211 | << kWaiterShift; 212 | static const uint64_t kWaiterInc = 1ull << kWaiterBits; 213 | static const uint64_t kEpochBits = 32; 214 | static const uint64_t kEpochShift = 32; 215 | static const uint64_t kEpochMask = ((1ull << kEpochBits) - 1) << kEpochShift; 216 | static const uint64_t kEpochInc = 1ull << kEpochShift; 217 | std::atomic _state; 218 | std::vector& _waiters; 219 | 220 | void _park(Waiter* w) { 221 | std::unique_lock lock(w->mu); 222 | while (w->state != Waiter::kSignaled) { 223 | w->state = Waiter::kWaiting; 224 | w->cv.wait(lock); 225 | } 226 | } 227 | 228 | void _unpark(Waiter* waiters) { 229 | Waiter* next = nullptr; 230 | for (Waiter* w = waiters; w; w = next) { 231 | next = w->next.load(std::memory_order_relaxed); 232 | unsigned state; 233 | { 234 | std::unique_lock lock(w->mu); 235 | state = w->state; 236 | w->state = Waiter::kSignaled; 237 | } 238 | // Avoid notifying if it wasn't waiting. 239 | if (state == Waiter::kWaiting) w->cv.notify_one(); 240 | } 241 | } 242 | 243 | Notifier(const Notifier&) = delete; 244 | void operator=(const Notifier&) = delete; 245 | }; 246 | 247 | 248 | 249 | } // namespace tf ------------------------------------------------------------ 250 | 251 | -------------------------------------------------------------------------------- /pandar_pointcloud/params/Pandar128_Firetimes.csv: -------------------------------------------------------------------------------- 1 | 2.85,distance>=A1,distance=A1,distance=A1,distance=A1,distance=A1,distance=A1,distance=A1,distance=A1,distance 18 | #include 19 | #include 20 | #include 21 | #include "laser_ts.h" 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #define OFFSET1 (3148) 28 | #define OFFSET2 (-27778) 29 | #define PAI (3.14159265358979323846) 30 | #define SEC_TO_NS (1000000000.0) 31 | #define SPEED (600.0 / SEC_TO_NS) 32 | #define A_TO_R (PAI / 180.0) 33 | #define R_TO_A (180.0 / PAI) 34 | 35 | #define PANDAR80_BLOCK_TIMESTAMP (33.33) 36 | #define BLOCK_ID_MAX (3) 37 | #define SEC_TO_US (1000000.0) 38 | #define SPEED_US (3600.0 / SEC_TO_US) 39 | 40 | #define PANDAR128_COORDINATE_CORRECTION_H (0.04) 41 | #define PANDAR128_COORDINATE_CORRECTION_B (0.012) 42 | 43 | LasersTSOffset::LasersTSOffset() { 44 | mBInitFlag = false; 45 | mNLaserNum = 0; 46 | 47 | for (int j = 0; j < CIRCLE; j++) { 48 | float angle = static_cast(j) / 100.0f; 49 | 50 | mSinAllAngleHB[j] = sinf(A_TO_R * angle) * sqrtf(PANDAR128_COORDINATE_CORRECTION_B * PANDAR128_COORDINATE_CORRECTION_B + PANDAR128_COORDINATE_CORRECTION_H * PANDAR128_COORDINATE_CORRECTION_H); 51 | mSinAllAngleH[j] = sinf(A_TO_R * angle) * PANDAR128_COORDINATE_CORRECTION_H; 52 | } 53 | 54 | for (int j = 0; j < PAI_ANGLE; j++) { 55 | mArcSin[j] = asinf(float(j - HALF_PAI_ANGLE) / HALF_PAI_ANGLE) * 180 / M_PI; 56 | } 57 | 58 | mShortOffsetIndex.resize(100); 59 | mLongOffsetIndex.resize(100); 60 | m_fArctanHB = atanf(PANDAR128_COORDINATE_CORRECTION_B / PANDAR128_COORDINATE_CORRECTION_H) + 0.5f; 61 | m_vQT128Firetime[0].fill(0); 62 | m_vQT128Firetime[1].fill(0); 63 | m_vQT128Firetime[2].fill(0); 64 | m_vQT128Firetime[3].fill(0); 65 | } 66 | 67 | LasersTSOffset::~LasersTSOffset() { 68 | 69 | } 70 | 71 | int LasersTSOffset::ParserFiretimeData(std::string firetime_content){ 72 | std::istringstream fin(firetime_content); 73 | std::string line; 74 | if (std::getline(fin, line)) { //first line sequence,chn id,firetime/us 75 | // printf("Parse Lidar firetime now...\n"); 76 | } 77 | std::vector firstLine; 78 | boost::split(firstLine, line, boost::is_any_of(",")); 79 | if(firstLine[0] == "EEFF" || firstLine[0] == "eeff"){ 80 | std::array, 4> firetimes; 81 | firetimes[0].fill(0); 82 | firetimes[1].fill(0); 83 | firetimes[2].fill(0); 84 | firetimes[3].fill(0); 85 | std::getline(fin, line); 86 | std::vector loopNumLine; 87 | boost::split(loopNumLine, line, boost::is_any_of(",")); 88 | int loopNum = atoi(loopNumLine[3].c_str()); 89 | std::getline(fin, line); 90 | for(int i = 0; i < PANDAR128_LIDAR_NUM; i++){ 91 | std::getline(fin, line); 92 | std::vector ChannelLine; 93 | boost::split(ChannelLine, line, boost::is_any_of(",")); 94 | for(int j = 0; j < loopNum; j++){ 95 | if(ChannelLine.size() == loopNum * 2){ 96 | int laserId = atoi(ChannelLine[j* 2].c_str()) - 1; 97 | firetimes[j][laserId] = std::stof(ChannelLine[j* 2 + 1].c_str()); 98 | } 99 | else{ 100 | std::cout << "loop num is not equal to the first channel line" << std::endl; 101 | return -1; 102 | } 103 | } 104 | } 105 | m_vQT128Firetime = firetimes; 106 | 107 | } 108 | else{ 109 | std::cout << "firetime file delimiter is wrong " << firstLine[0] << std::endl; 110 | return -1; 111 | } 112 | return 0; 113 | } 114 | 115 | void LasersTSOffset::setFilePath(std::string file) { 116 | 117 | std::ifstream fin(file); 118 | std::string line; 119 | if (std::getline(fin, line)) { //first line sequence,chn id,firetime/us 120 | printf("Parse Lidar firetime now...\n"); 121 | } 122 | std::vector firstLine; 123 | boost::split(firstLine, line, boost::is_any_of(",")); 124 | if(firstLine[0] == "EEFF" || firstLine[0] == "eeff"){ 125 | return; 126 | } 127 | if (firstLine.size() == 2){ 128 | while (std::getline(fin, line)) { 129 | int sequence = 0; 130 | int chnId = 0; 131 | float firetime; 132 | std::stringstream ss(line); 133 | std::string subline; 134 | // std::getline(ss, subline, ','); 135 | // std::stringstream(subline) >> sequence; 136 | std::getline(ss, subline, ','); 137 | std::stringstream(subline) >> chnId; 138 | std::getline(ss, subline, ','); 139 | std::stringstream(subline) >> firetime; 140 | // printf("seq, chnId, firetime:[%d][%d][%f]\n",sequence, chnId, firetime); 141 | m_fAzimuthOffset[chnId - 1] = firetime; 142 | } 143 | } 144 | else if(firstLine.size() == 5){ 145 | std::getline(fin, line); 146 | std::getline(fin, line); 147 | while (std::getline(fin, line)) { 148 | int sequence = 0; 149 | int chnIdCDB = 0; 150 | int chnIdCDA = 0; 151 | float firetimeCDB; 152 | float firetimeCDA; 153 | std::string blank; 154 | std::stringstream ss(line); 155 | std::string subline; 156 | // std::getline(ss, subline, ','); 157 | // std::stringstream(subline) >> sequence; 158 | std::getline(ss, subline, ','); 159 | std::stringstream(subline) >> chnIdCDB; 160 | std::getline(ss, subline, ','); 161 | std::stringstream(subline) >> firetimeCDB; 162 | // printf("chnIdCDB, firetimeCDB:[%d][%f] ", chnIdCDB, firetimeCDB); 163 | m_fCDBAzimuthOffset[chnIdCDB - 1] = firetimeCDB; 164 | std::getline(ss, subline, ','); 165 | std::stringstream(subline) >> blank; 166 | std::getline(ss, subline, ','); 167 | std::stringstream(subline) >> chnIdCDA; 168 | std::getline(ss, subline, ','); 169 | std::stringstream(subline) >> firetimeCDA; 170 | // printf("chnIdCDA, firetimeCDA:[%d][%f]\n", chnIdCDA, firetimeCDA); 171 | m_fCDAAzimuthOffset[chnIdCDA - 1] = firetimeCDA; 172 | } 173 | } 174 | else{ 175 | fin.close(); 176 | FILE *pFile = fopen(file.c_str(), "r"); 177 | char content[512] = {0}; 178 | char subStr[255] = {0}; 179 | std::vector mode; 180 | std::vector state; 181 | 182 | if (NULL == pFile) { 183 | return; 184 | } 185 | 186 | fgets(content, 512, pFile); 187 | strncpy(subStr, content, strchr(content, ',') - content); 188 | sscanf(subStr, "%f", &mFDist); 189 | memset(subStr, 0, 255); 190 | memset(content, 0, 512); 191 | 192 | fgets(content, 512, pFile); 193 | fillVector(strchr(content, ','), strlen(strchr(content, ',')), mode); 194 | memset(content, 0, 512); 195 | 196 | fgets(content, 512, pFile); 197 | fillVector(strchr(content, ','), strlen(strchr(content, ',')), state); 198 | memset(content, 0, 512); 199 | 200 | if (mode.size() != state.size()) { 201 | printf("file format error\n"); 202 | return; 203 | } 204 | 205 | for (int i = 0; i < mode.size(); i++) { 206 | int index = mode[i] * 10 + state[i]; 207 | if (i % 2 == 0) { 208 | mLongOffsetIndex[index] = i; 209 | } else { 210 | mShortOffsetIndex[index] = i; 211 | } 212 | } 213 | 214 | while (!feof(pFile)) { 215 | fgets(content, 512, pFile); 216 | 217 | std::vector values; 218 | char *pValues = strchr(content, ','); 219 | 220 | if (NULL == pValues) { 221 | break; 222 | } 223 | 224 | strncpy(subStr, content, pValues - content); 225 | sscanf(subStr, "%d", &mNLaserNum); 226 | fillVector(pValues, strlen(pValues), values); 227 | 228 | if (values.size() != mode.size()) { 229 | continue; 230 | } 231 | 232 | mVLasers.push_back(values); 233 | } 234 | 235 | mBInitFlag = true; 236 | 237 | } 238 | } 239 | 240 | void LasersTSOffset::fillVector(char *pContent, int nLen, std::vector &vec) { 241 | char *pNext = strtok(pContent, ","); 242 | 243 | while (pNext != NULL) { 244 | if (pNext != NULL && strlen(pNext) > 0) { 245 | vec.push_back(atoi(pNext)); 246 | } 247 | 248 | pNext = strtok(NULL, ","); 249 | } 250 | } 251 | 252 | float LasersTSOffset::getTSOffset(int nLaser, int nMode, int nState, float fDistance, int nMajorVersion) { 253 | switch (nMajorVersion){ 254 | case 1: 255 | if (nLaser >= mNLaserNum || !mBInitFlag) { 256 | return 0; 257 | } 258 | if (fDistance >= mFDist) { 259 | return mVLasers[nLaser][mLongOffsetIndex[nMode * 10 + nState]]; 260 | } 261 | else { 262 | return mVLasers[nLaser][mShortOffsetIndex[nMode * 10 + nState]]; 263 | } 264 | case 3: 265 | return m_vQT128Firetime[nMode][nLaser]; 266 | default: 267 | return 0; 268 | } 269 | } 270 | 271 | int LasersTSOffset::getBlockTS(int nBlock, int nRetMode, int nMode, int nLaserNum) { 272 | switch (nLaserNum){ 273 | case PANDAR80_LIDAR_NUM: 274 | if (0x39 == nRetMode || 0x3b == nRetMode || 0x3c == nRetMode) { 275 | return (((BLOCK_ID_MAX - nBlock)/2) * PANDAR80_BLOCK_TIMESTAMP); 276 | } 277 | else { 278 | return ((BLOCK_ID_MAX - nBlock) * PANDAR80_BLOCK_TIMESTAMP); 279 | } 280 | default: 281 | int ret = OFFSET1; 282 | if (nRetMode != 0x39) { 283 | if (nBlock % 2 == 0) { 284 | ret += OFFSET2; 285 | } 286 | if (nMode != 0) { 287 | ret += OFFSET2; 288 | } 289 | } 290 | return ret; 291 | } 292 | } 293 | 294 | float LasersTSOffset::getAngleOffset(float nTSOffset, int speed, int nMajorVersion) { 295 | switch (nMajorVersion){ 296 | case 1: 297 | return nTSOffset * speed * 6E-9; 298 | case 3: 299 | return nTSOffset * speed * 6E-6; 300 | default: 301 | return 0; 302 | } 303 | } 304 | 305 | 306 | float LasersTSOffset::getAzimuthOffset(std::string type, float azimuth, \ 307 | float originAzimuth, float distance) { 308 | int angle = static_cast(100 * (m_fArctanHB + azimuth - originAzimuth)); 309 | 310 | if (angle < 0) { 311 | angle += CIRCLE; 312 | } else if (angle >= CIRCLE) { 313 | angle -= CIRCLE; 314 | } 315 | 316 | if (distance < 0.00001) { 317 | return 0; 318 | } 319 | 320 | float value = mSinAllAngleHB[angle] / distance; 321 | if(value < -1 || value > 1) 322 | return 0; 323 | int index = int(value * HALF_PAI_ANGLE) + HALF_PAI_ANGLE; 324 | if(index < 0 || index > PAI_ANGLE -1) 325 | return 0; 326 | return -mArcSin[index]; 327 | } 328 | 329 | float LasersTSOffset::getPitchOffset(std::string type, float pitch, float distance) { 330 | int angle = static_cast(100 * pitch + 0.5f); 331 | 332 | if (angle < 0) { 333 | angle += CIRCLE; 334 | } else if (angle >= CIRCLE) { 335 | angle -= CIRCLE; 336 | } 337 | 338 | float value = mSinAllAngleH[angle] / distance; 339 | if(value < -1.0 || value > 1.0) 340 | return 0; 341 | int index = int(value * HALF_PAI_ANGLE) + HALF_PAI_ANGLE; 342 | if(index < 0 || index > PAI_ANGLE -1) 343 | return 0; 344 | return -mArcSin[index]; 345 | } 346 | 347 | --------------------------------------------------------------------------------