From 8a972dd52d874e96a1b28db63d41e7baf2c806dd Mon Sep 17 00:00:00 2001 From: Rooholla-KhorramBakht Date: Sun, 4 Feb 2024 08:18:52 +0800 Subject: [PATCH] LiDAR node and sensor launch files added --- Dockerfile | 29 +- Makefile | 2 + README.md | 48 ++ deploy/launch/hw.launch.py | 84 +++ deploy/ros2_ws/src/lidar_node/CMakeLists.txt | 267 +++++++++ deploy/ros2_ws/src/lidar_node/README.md | 161 ++++++ deploy/ros2_ws/src/lidar_node/Version.h.in | 21 + .../ros2_ws/src/lidar_node/config/config.yaml | 26 + .../src/lidar_node/launch/dashing_start.py | 21 + .../src/lidar_node/launch/start.launch | 6 + deploy/ros2_ws/src/lidar_node/launch/start.py | 12 + .../ros2_ws/src/lidar_node/msg/UdpFrame.msg | 2 + .../ros2_ws/src/lidar_node/msg/UdpPacket.msg | 15 + .../src/lidar_node/msg/msg_ros2/UdpFrame.msg | 2 + .../src/lidar_node/msg/msg_ros2/UdpPacket.msg | 15 + .../lidar_node/node/hesai_ros_driver_node.cc | 108 ++++ .../lidar_node/node/hesai_ros_driver_node.cu | 108 ++++ deploy/ros2_ws/src/lidar_node/package.xml | 41 ++ deploy/ros2_ws/src/lidar_node/rviz/rviz.rviz | 346 +++++++++++ deploy/ros2_ws/src/lidar_node/rviz/rviz2.rviz | 159 +++++ .../driver/HesaiLidar_SDK_2.0/CMakeLists.txt | 117 ++++ .../src/driver/HesaiLidar_SDK_2.0/LICENSE | 14 + .../src/driver/HesaiLidar_SDK_2.0/README.md | 73 +++ .../driver/HesaiLidar_SDK_2.0/Version.h.in | 21 + .../driver/HesaiLidar_SDK_2.0/change notes.md | 23 + .../AT128E2X_Angle Correction File.dat | Bin 0 -> 72572 bytes .../FT120C1X_Angle Correction File.dat | Bin 0 -> 153641 bytes .../OT128_Angle Correction File.csv | 129 +++++ .../Pandar128E3X_Angle Correction File.csv | 129 +++++ .../Pandar40M_Angle Correction File.csv | 41 ++ .../Pandar40P_Angle Correction File.csv | 41 ++ .../Pandar64_Angle Correction File.csv | 65 +++ .../Pandar90E3X_Angle Correction File.csv | 91 +++ .../PandarQT_Angle Correction File.csv | 65 +++ .../PandarXT-16_Angle Correction File.csv | 17 + .../PandarXT_Angle Correction File.csv | 33 ++ .../QT128C2X_Angle Correction File.csv | 129 +++++ .../QT128C2X_Channel_Cofig.csv | 84 +++ .../XT32M2X_Angle Correction File.csv | 33 ++ .../AT128E2X_Firetime Correction File.csv | 129 +++++ .../Pandar128E3X_Firetime Correction File.csv | 131 +++++ .../Pandar40E3X_Firetime Correction File.csv | 43 ++ .../Pandar40P_Firetime Correction File.csv | 41 ++ .../Pandar64E3X_Firetime Correction File.csv | 67 +++ .../Pandar64_Firetime Correction File.csv | 65 +++ .../Pandar90_Firetime Correction File.csv | 93 +++ .../PandarXT-16_Firetime Correction File.csv | 17 + .../PandarXT_Firetime Correction File.csv | 33 ++ .../QT128C2X_Firetime Correction File.csv | 131 +++++ .../driver/hesai_lidar_sdk.hpp | 191 ++++++ .../driver/hesai_lidar_sdk_gpu.cuh | 269 +++++++++ .../libhesai/CMakeLists.txt | 132 +++++ .../Container/include/blocking_ring.h | 43 ++ .../libhesai/Container/include/ring.h | 59 ++ .../Container/include/ring_2d_shared.h | 67 +++ .../libhesai/Container/include/ring_2dex.h | 64 +++ .../libhesai/Container/src/blocking_ring.cc | 90 +++ .../libhesai/Container/src/ring.cc | 120 ++++ .../libhesai/Container/src/ring_2d_shared.cc | 251 ++++++++ .../libhesai/Container/src/ring_2dex.cc | 177 ++++++ .../libhesai/Lidar/lidar.cc | 543 ++++++++++++++++++ .../HesaiLidar_SDK_2.0/libhesai/Lidar/lidar.h | 172 ++++++ .../libhesai/Lidar/lidar_types.h | 349 +++++++++++ .../libhesai/Logger/include/logger.h | 102 ++++ .../libhesai/Logger/src/logger.cc | 180 ++++++ .../libhesai/PtcClient/include/client_base.h | 79 +++ .../include/lidar_communication_header.h | 115 ++++ .../libhesai/PtcClient/include/ptc_client.h | 100 ++++ .../libhesai/PtcClient/include/tcp_client.h | 115 ++++ .../PtcClient/include/tcp_ssl_client.h | 114 ++++ .../libhesai/PtcClient/src/ptc_client.cc | 340 +++++++++++ .../libhesai/PtcClient/src/tcp_client.cc | 338 +++++++++++ .../libhesai/PtcClient/src/tcp_ssl_client.cc | 388 +++++++++++++ .../PtcParser/include/general_ptc_parser.h | 65 +++ .../PtcParser/include/ptc_1_0_parser.h | 110 ++++ .../PtcParser/include/ptc_2_0_parser.h | 123 ++++ .../libhesai/PtcParser/ptc_parser.cc | 63 ++ .../libhesai/PtcParser/ptc_parser.h | 71 +++ .../PtcParser/src/general_ptc_parser.cc | 60 ++ .../libhesai/PtcParser/src/ptc_1_0_parser.cc | 68 +++ .../libhesai/PtcParser/src/ptc_2_0_parser.cc | 98 ++++ .../libhesai/Source/include/pcap_saver.h | 104 ++++ .../libhesai/Source/include/pcap_source.h | 197 +++++++ .../libhesai/Source/include/socket_source.h | 89 +++ .../libhesai/Source/include/source.h | 77 +++ .../libhesai/Source/src/pcap_saver.cc | 224 ++++++++ .../libhesai/Source/src/pcap_source.cc | 318 ++++++++++ .../libhesai/Source/src/socket_source.cc | 244 ++++++++ .../libhesai/Source/src/source.cc | 45 ++ .../UdpParser/include/general_parser.h | 254 ++++++++ .../UdpParser/include/udp1_4_parser.h | 97 ++++ .../UdpParser/include/udp2_4_parser.h | 82 +++ .../UdpParser/include/udp2_5_parser.h | 109 ++++ .../UdpParser/include/udp3_1_parser.h | 74 +++ .../UdpParser/include/udp3_2_parser.h | 110 ++++ .../UdpParser/include/udp4_3_parser.h | 177 ++++++ .../UdpParser/include/udp6_1_parser.h | 78 +++ .../UdpParser/include/udp7_2_parser.h | 106 ++++ .../UdpParser/include/udp_p40_parser.h | 74 +++ .../UdpParser/include/udp_p64_parser.h | 73 +++ .../libhesai/UdpParser/src/general_parser.cc | 264 +++++++++ .../libhesai/UdpParser/src/udp1_4_parser.cc | 367 ++++++++++++ .../libhesai/UdpParser/src/udp2_4_parser.cc | 337 +++++++++++ .../libhesai/UdpParser/src/udp2_5_parser.cc | 340 +++++++++++ .../libhesai/UdpParser/src/udp3_1_parser.cc | 279 +++++++++ .../libhesai/UdpParser/src/udp3_2_parser.cc | 527 +++++++++++++++++ .../libhesai/UdpParser/src/udp4_3_parser.cc | 496 ++++++++++++++++ .../libhesai/UdpParser/src/udp6_1_parser.cc | 225 ++++++++ .../libhesai/UdpParser/src/udp7_2_parser.cc | 298 ++++++++++ .../libhesai/UdpParser/src/udp_p40_parser.cc | 152 +++++ .../libhesai/UdpParser/src/udp_p64_parser.cc | 156 +++++ .../libhesai/UdpParser/udp_parser.cc | 413 +++++++++++++ .../libhesai/UdpParser/udp_parser.h | 121 ++++ .../UdpParserGpu/include/general_parser_gpu.h | 165 ++++++ .../libhesai/UdpParserGpu/include/nvbuffer.h | 196 +++++++ .../UdpParserGpu/include/return_code.h | 79 +++ .../UdpParserGpu/include/safe_call.cuh | 85 +++ .../UdpParserGpu/include/udp1_4_parser_gpu.h | 77 +++ .../UdpParserGpu/include/udp2_5_parser_gpu.h | 81 +++ .../UdpParserGpu/include/udp3_1_parser_gpu.h | 75 +++ .../UdpParserGpu/include/udp3_2_parser_gpu.h | 73 +++ .../UdpParserGpu/include/udp4_3_parser_gpu.h | 117 ++++ .../UdpParserGpu/include/udp6_1_parser_gpu.h | 74 +++ .../UdpParserGpu/include/udp7_2_parser_gpu.h | 79 +++ .../UdpParserGpu/include/udp_p40_parser_gpu.h | 74 +++ .../UdpParserGpu/include/udp_p64_parser_gpu.h | 75 +++ .../libhesai/UdpParserGpu/src/buffer.cu | 94 +++ .../UdpParserGpu/src/general_parser_gpu.cu | 158 +++++ .../UdpParserGpu/src/udp1_4_parser_gpu.cu | 223 +++++++ .../UdpParserGpu/src/udp2_5_parser_gpu.cu | 288 ++++++++++ .../UdpParserGpu/src/udp3_1_parser_gpu.cu | 205 +++++++ .../UdpParserGpu/src/udp3_2_parser_gpu.cu | 205 +++++++ .../UdpParserGpu/src/udp4_3_parser_gpu.cu | 272 +++++++++ .../UdpParserGpu/src/udp6_1_parser_gpu.cu | 237 ++++++++ .../UdpParserGpu/src/udp7_2_parser_gpu.cu | 292 ++++++++++ .../UdpParserGpu/src/udp_p40_parser_gpu.cu | 206 +++++++ .../UdpParserGpu/src/udp_p64_parser_gpu.cu | 207 +++++++ .../libhesai/UdpParserGpu/udp_parser_gpu.cu | 241 ++++++++ .../libhesai/UdpParserGpu/udp_parser_gpu.h | 73 +++ .../libhesai/UdpProtocol/fault_message.h | 289 ++++++++++ .../UdpProtocol/udp_protocol_header.h | 149 +++++ .../libhesai/UdpProtocol/udp_protocol_p40.h | 134 +++++ .../libhesai/UdpProtocol/udp_protocol_p64.h | 161 ++++++ .../libhesai/UdpProtocol/udp_protocol_v1_4.h | 470 +++++++++++++++ .../libhesai/UdpProtocol/udp_protocol_v2_4.h | 251 ++++++++ .../libhesai/UdpProtocol/udp_protocol_v2_5.h | 267 +++++++++ .../libhesai/UdpProtocol/udp_protocol_v3_1.h | 267 +++++++++ .../libhesai/UdpProtocol/udp_protocol_v3_2.h | 377 ++++++++++++ .../libhesai/UdpProtocol/udp_protocol_v4_3.h | 347 +++++++++++ .../libhesai/UdpProtocol/udp_protocol_v6_1.h | 290 ++++++++++ .../libhesai/UdpProtocol/udp_protocol_v7_2.h | 278 +++++++++ .../libhesai/driver_param.h | 133 +++++ .../libhesai/include/auto_tick_count.h | 94 +++ .../libhesai/include/plat_utils.h | 43 ++ .../libhesai/src/auto_tick_count.cc | 228 ++++++++ .../libhesai/src/plat_utils.cc | 190 ++++++ .../driver/HesaiLidar_SDK_2.0/test/test.cc | 58 ++ .../driver/HesaiLidar_SDK_2.0/test/test.cu | 51 ++ .../HesaiLidar_SDK_2.0/tool/CMakeLists.txt | 125 ++++ .../HesaiLidar_SDK_2.0/tool/Version.h.in | 21 + .../tool/packet_loss_tool.cc | 79 +++ .../HesaiLidar_SDK_2.0/tool/pcl_tool.cc | 130 +++++ .../HesaiLidar_SDK_2.0/tool/pcl_tool.cu | 127 ++++ .../lidar_node/src/manager/node_manager.cc | 70 +++ .../lidar_node/src/manager/node_manager.cu | 70 +++ .../src/lidar_node/src/manager/node_manager.h | 59 ++ .../src/manager/source_driver_ros1.hpp | 262 +++++++++ .../src/manager/source_driver_ros2.hpp | 264 +++++++++ .../lidar_node/src/utility/yaml_reader.hpp | 48 ++ deploy/scripts/hw_start.sh | 1 + 170 files changed, 24172 insertions(+), 3 deletions(-) create mode 100644 Makefile create mode 100644 deploy/launch/hw.launch.py create mode 100644 deploy/ros2_ws/src/lidar_node/CMakeLists.txt create mode 100644 deploy/ros2_ws/src/lidar_node/README.md create mode 100644 deploy/ros2_ws/src/lidar_node/Version.h.in create mode 100644 deploy/ros2_ws/src/lidar_node/config/config.yaml create mode 100755 deploy/ros2_ws/src/lidar_node/launch/dashing_start.py create mode 100755 deploy/ros2_ws/src/lidar_node/launch/start.launch create mode 100755 deploy/ros2_ws/src/lidar_node/launch/start.py create mode 100644 deploy/ros2_ws/src/lidar_node/msg/UdpFrame.msg create mode 100644 deploy/ros2_ws/src/lidar_node/msg/UdpPacket.msg create mode 100644 deploy/ros2_ws/src/lidar_node/msg/msg_ros2/UdpFrame.msg create mode 100644 deploy/ros2_ws/src/lidar_node/msg/msg_ros2/UdpPacket.msg create mode 100644 deploy/ros2_ws/src/lidar_node/node/hesai_ros_driver_node.cc create mode 100644 deploy/ros2_ws/src/lidar_node/node/hesai_ros_driver_node.cu create mode 100644 deploy/ros2_ws/src/lidar_node/package.xml create mode 100755 deploy/ros2_ws/src/lidar_node/rviz/rviz.rviz create mode 100755 deploy/ros2_ws/src/lidar_node/rviz/rviz2.rviz create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/CMakeLists.txt create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/LICENSE create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/README.md create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/Version.h.in create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/change notes.md create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/AT128E2X_Angle Correction File.dat create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/FT120C1X_Angle Correction File.dat create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/OT128_Angle Correction File.csv create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/Pandar128E3X_Angle Correction File.csv create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/Pandar40M_Angle Correction File.csv create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/Pandar40P_Angle Correction File.csv create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/Pandar64_Angle Correction File.csv create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/Pandar90E3X_Angle Correction File.csv create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/PandarQT_Angle Correction File.csv create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/PandarXT-16_Angle Correction File.csv create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/PandarXT_Angle Correction File.csv create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/QT128C2X_Angle Correction File.csv create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/QT128C2X_Channel_Cofig.csv create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/XT32M2X_Angle Correction File.csv create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/firetime_correction/AT128E2X_Firetime Correction File.csv create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/firetime_correction/Pandar128E3X_Firetime Correction File.csv create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/firetime_correction/Pandar40E3X_Firetime Correction File.csv create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/firetime_correction/Pandar40P_Firetime Correction File.csv create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/firetime_correction/Pandar64E3X_Firetime Correction File.csv create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/firetime_correction/Pandar64_Firetime Correction File.csv create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/firetime_correction/Pandar90_Firetime Correction File.csv create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/firetime_correction/PandarXT-16_Firetime Correction File.csv create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/firetime_correction/PandarXT_Firetime Correction File.csv create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/firetime_correction/QT128C2X_Firetime Correction File.csv create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/driver/hesai_lidar_sdk.hpp create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/driver/hesai_lidar_sdk_gpu.cuh create mode 100755 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/CMakeLists.txt create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Container/include/blocking_ring.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Container/include/ring.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Container/include/ring_2d_shared.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Container/include/ring_2dex.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Container/src/blocking_ring.cc create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Container/src/ring.cc create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Container/src/ring_2d_shared.cc create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Container/src/ring_2dex.cc create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Lidar/lidar.cc create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Lidar/lidar.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Lidar/lidar_types.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Logger/include/logger.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Logger/src/logger.cc create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcClient/include/client_base.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcClient/include/lidar_communication_header.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcClient/include/ptc_client.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcClient/include/tcp_client.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcClient/include/tcp_ssl_client.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcClient/src/ptc_client.cc create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcClient/src/tcp_client.cc create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcClient/src/tcp_ssl_client.cc create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcParser/include/general_ptc_parser.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcParser/include/ptc_1_0_parser.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcParser/include/ptc_2_0_parser.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcParser/ptc_parser.cc create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcParser/ptc_parser.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcParser/src/general_ptc_parser.cc create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcParser/src/ptc_1_0_parser.cc create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcParser/src/ptc_2_0_parser.cc create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Source/include/pcap_saver.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Source/include/pcap_source.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Source/include/socket_source.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Source/include/source.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Source/src/pcap_saver.cc create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Source/src/pcap_source.cc create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Source/src/socket_source.cc create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Source/src/source.cc create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/general_parser.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/udp1_4_parser.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/udp2_4_parser.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/udp2_5_parser.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/udp3_1_parser.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/udp3_2_parser.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/udp4_3_parser.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/udp6_1_parser.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/udp7_2_parser.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/udp_p40_parser.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/udp_p64_parser.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/src/general_parser.cc create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/src/udp1_4_parser.cc create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/src/udp2_4_parser.cc create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/src/udp2_5_parser.cc create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/src/udp3_1_parser.cc create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/src/udp3_2_parser.cc create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/src/udp4_3_parser.cc create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/src/udp6_1_parser.cc create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/src/udp7_2_parser.cc create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/src/udp_p40_parser.cc create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/src/udp_p64_parser.cc create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/udp_parser.cc create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/udp_parser.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/general_parser_gpu.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/nvbuffer.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/return_code.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/safe_call.cuh create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/udp1_4_parser_gpu.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/udp2_5_parser_gpu.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/udp3_1_parser_gpu.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/udp3_2_parser_gpu.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/udp4_3_parser_gpu.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/udp6_1_parser_gpu.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/udp7_2_parser_gpu.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/udp_p40_parser_gpu.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/udp_p64_parser_gpu.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/src/buffer.cu create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/src/general_parser_gpu.cu create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/src/udp1_4_parser_gpu.cu create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/src/udp2_5_parser_gpu.cu create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/src/udp3_1_parser_gpu.cu create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/src/udp3_2_parser_gpu.cu create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/src/udp4_3_parser_gpu.cu create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/src/udp6_1_parser_gpu.cu create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/src/udp7_2_parser_gpu.cu create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/src/udp_p40_parser_gpu.cu create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/src/udp_p64_parser_gpu.cu create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/udp_parser_gpu.cu create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/udp_parser_gpu.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpProtocol/fault_message.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpProtocol/udp_protocol_header.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpProtocol/udp_protocol_p40.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpProtocol/udp_protocol_p64.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpProtocol/udp_protocol_v1_4.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpProtocol/udp_protocol_v2_4.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpProtocol/udp_protocol_v2_5.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpProtocol/udp_protocol_v3_1.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpProtocol/udp_protocol_v3_2.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpProtocol/udp_protocol_v4_3.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpProtocol/udp_protocol_v6_1.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpProtocol/udp_protocol_v7_2.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/driver_param.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/include/auto_tick_count.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/include/plat_utils.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/src/auto_tick_count.cc create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/src/plat_utils.cc create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/test/test.cc create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/test/test.cu create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/tool/CMakeLists.txt create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/tool/Version.h.in create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/tool/packet_loss_tool.cc create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/tool/pcl_tool.cc create mode 100644 deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/tool/pcl_tool.cu create mode 100644 deploy/ros2_ws/src/lidar_node/src/manager/node_manager.cc create mode 100644 deploy/ros2_ws/src/lidar_node/src/manager/node_manager.cu create mode 100644 deploy/ros2_ws/src/lidar_node/src/manager/node_manager.h create mode 100644 deploy/ros2_ws/src/lidar_node/src/manager/source_driver_ros1.hpp create mode 100644 deploy/ros2_ws/src/lidar_node/src/manager/source_driver_ros2.hpp create mode 100644 deploy/ros2_ws/src/lidar_node/src/utility/yaml_reader.hpp create mode 100644 deploy/scripts/hw_start.sh diff --git a/Dockerfile b/Dockerfile index e00b446..d8f5ba0 100644 --- a/Dockerfile +++ b/Dockerfile @@ -1,16 +1,39 @@ -FROM ubuntu:20.04 +FROM isaac_ros_dev-aarch64 ENV DEBIAN_FRONTEND=noninteractive # uodate and install dependencies RUN apt-get update && apt-get install -y \ + ros-humble-rmw-cyclonedds-cpp ros-humble-rosidl-generator-dds-idl \ + ros-humble-realsense2-camera \ + ros-humble-pointcloud-to-laserscan \ + ros-humble-isaac-ros-visual-slam \ + ros-humble-isaac-ros-occupancy-grid-localizer\ build-essential \ cmake \ git \ && rm -rf /var/lib/apt/lists/* -# make and install the project +# Cheange the ROS2 RMW to CycloneDDS as instructed by Unitree +RUN cd / && git clone https://github.com/unitreerobotics/unitree_ros2 && cd /unitree_ros2/cyclonedds_ws/src && \ +git clone https://github.com/ros2/rmw_cyclonedds -b humble && git clone https://github.com/eclipse-cyclonedds/cyclonedds -b releases/0.10.x &&\ +cd .. && colcon build --packages-select cyclonedds && source /opt/ros/humble/setup.bash && colcon build + +RUN echo "source /opt/ros/humble/setup.bash" >> /usr/local/bin/scripts/workspace-entrypoint.sh +RUN echo "source /unitree_ros2/cyclonedds_ws/install/setup.bash" >> /usr/local/bin/scripts/workspace-entrypoint.sh +RUN echo "export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp" >> /usr/local/bin/scripts/workspace-entrypoint.sh +RUN echo "export CYCLONEDDS_URI=' '" >> /usr/local/bin/scripts/workspace-entrypoint.sh + +# copy the go2py ros2 nodes +COPY deploy/ros2_ws/src /ros2_ws/src +RUN cd /ros2_ws && source /opt/ros/humble/setup.bash && colcon build --symlink-install + +# Compile the C++ hypervisor bridge COPY cpp_bridge /cpp_bridge WORKDIR /cpp_bridge RUN ./install.sh && mkdir build && cd build && cmake .. && make +# Copy the script to start the nodes +COPY deploy/scripts /root/scripts +COPY deploy/launch /root/launch # set the entrypoint to bash -ENTRYPOINT ["/bin/bash"] +# ENTRYPOINT ["/bin/bash"] +ENTRYPOINT ["/bin/bash", "/root/scripts/hw_start.sh"] diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..a90d119 --- /dev/null +++ b/Makefile @@ -0,0 +1,2 @@ +docker: + @docker build --no-cache --tag go2py:latest . diff --git a/README.md b/README.md index 8b5143d..ec54651 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,50 @@ # Go2Py Python interface, example controllers, and calibration tools for the Unitree Go2 robot. + +## System Setup + +The GO2 EDU comes with an onbard Jetson Orin NX, a Hessai LX-16 LiDAR sensor, and an intel Realsense D435i camera. This setup procedure targets the onboard Jetson Orin with IP `192.168.123.18` and presents the installation proceudures of nodes for reading the sensors and robot states and publisheing them as ROS2 topics plus some basic configurations for setting up the autostart services and sharing internet. The architecture of the GO2 system is illustrated in the following image: +TODO: add image + +### Internet Sharing + +In order to access internet on the Jetson computer, we hook the robot to a host development computer with internet access and configure it to share its connection with the robot. To cofigure the host computer, the follwoing steps should be taken: +#### Host Computer +The following steps configures the host computer to share its intentrent with the robot. +##### Enable IP forwarding: + +```bash +sudo sysctl -w net.ipv4.ip_forward=1 +``` +##### Configure the iptables: + +```bash +sudo iptables -t nat -A POSTROUTING -o wlan0 -j MASQUERADE +sudo iptables -A FORWARD -i wlan0 -o eth0 -m state --state RELATED,ESTABLISHED -j ACCEPT +sudo iptables -A FORWARD -i eth0 -o wlan0 -j ACCEPT +``` +Note that `wlan0` should be replaced with the actual name of the network interface over which the internet is provided to the host computer, and eth0 should be replaced with the name of the Ethernet interface connected to the robot and having a local IP address in robot's network range. + +##### Storing the Settings +Make the iptables rules persistent by installing the `iptables-persistent`: + +```bash +sudo apt-get install iptables-persistent +sudo iptables-save > /etc/iptables/rules.v4 +sudo ip6tables-save > /etc/iptables/rules.v6 +``` +#### Robot +Now tell the computer on the robot to use the internet shared by the host computer. SSH into the robot's computer with IP address `192.168.123.18`, username `unitree` and password `123`. Note that the host computer's IP reange should have already been set to static mode with an IP in `192.168.123.*` range. + +```bash +sudo ip route add default via +``` + +Finally, configure the DNS server by adding the following to the `/etc/resolv.conf` file: +```bash +nameserver 8.8.8.8 +``` +**Note:** Similarly to the host computer, you can make save this configuration using the `iptables-persistent` tool. + +If everything has been successful, you should be able to access the internet on the robot. Run `ping www.google.com` to verify this. + diff --git a/deploy/launch/hw.launch.py b/deploy/launch/hw.launch.py new file mode 100644 index 0000000..940de08 --- /dev/null +++ b/deploy/launch/hw.launch.py @@ -0,0 +1,84 @@ +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import ThisLaunchFileDir +from launch_ros.actions import Node + + +def generate_launch_description(): + return LaunchDescription([ + # launch the pointcloud to laser scan converter + Node( + package='pointcloud_to_laserscan', executable='pointcloud_to_laserscan_node', + remappings=[('cloud_in', '/go2/lidar_points'), + ('scan', '/go2/lidar_scans')], + parameters=[{ + 'target_frame': 'go2/go2/hesai_lidar', + 'transform_tolerance': 0.01, + 'min_height': 0.0, + 'max_height': 1.0, + 'angle_min': -1.5708, # -M_PI/2 + 'angle_max': 1.5708, # M_PI/2 + 'angle_increment': 0.0087, # M_PI/360.0 + 'scan_time': 0.3333, + 'range_min': 0.45, + 'range_max': 100.0, + 'use_inf': True, + 'inf_epsilon': 1.0 + }], + name='pointcloud_to_laserscan' + ), + + Node( + package='hesai_ros_driver', + executable='hesai_ros_driver_node', + name='b1_hesai_ros_driver_node' + ), + + Node( + name='go2_d455_cam', + namespace='go2/d435i_cam', + package='realsense2_camera', + executable='realsense2_camera_node', + parameters=[{ + 'enable_infra1': True, + 'enable_infra2': True, + 'enable_color': False, + 'enable_depth': False, + 'depth_module.emitter_enabled': 0, + 'depth_module.profile': '640x480x60', + 'enable_gyro': True, + 'enable_accel': True, + 'gyro_fps': 400, + 'accel_fps': 200, + 'unite_imu_method': 2, + # 'tf_publish_rate': 0.0 + }] + ), + + # Launch the front looking D455 camera + # IncludeLaunchDescription( + # PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/d455.launch.py']) + # ), + # Run the B1py node + # Node( + # package='b1py_node', + # executable='highlevel', + # name='b1_highlevel_node' + # ), + + # Run the B1py calibration TF broadcaster + # Node( + # package='b1py_calib', + # executable='calib_broadcaster', + # name='b1_calib_broadcaster_node' + # ), + # Launch the LiDAR sensor + # IncludeLaunchDescription( + # PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/rslidar.launch.py']) + # ), + # Launch the LiDAR sensor + # IncludeLaunchDescription( + # PythonLaunchDescriptionSource(['/B1Py/deploy/docker/launch/state_estimation/ekf.launch.py']) + # ), + ]) \ No newline at end of file diff --git a/deploy/ros2_ws/src/lidar_node/CMakeLists.txt b/deploy/ros2_ws/src/lidar_node/CMakeLists.txt new file mode 100644 index 0000000..db19153 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/CMakeLists.txt @@ -0,0 +1,267 @@ +cmake_minimum_required(VERSION 3.5) +cmake_policy(SET CMP0048 NEW) +project(hesai_ros_driver) + +#======================================= +# Version information +#======================================= +set(VERSION_MAJOR 2) +set(VERSION_MINOR 0) +set(VERSION_TINY 5) +configure_file( + "${CMAKE_CURRENT_SOURCE_DIR}/Version.h.in" + "${CMAKE_CURRENT_BINARY_DIR}/Version.h" +) + +#======================================= +# Custom Point Type (XYZI, XYZIRT) +#======================================= +set(POINT_TYPE XYZI) + +#======================================= +# Compile setup (ORIGINAL, CATKIN, COLCON) +#======================================= +message(=============================================================) +message("-- ROS_VERSION is $ENV{ROS_VERSION}") +message(=============================================================) + + +#======================== +# Project details / setup +#======================== +set(PROJECT_NAME hesai_ros_driver) + +add_definitions(-DPROJECT_PATH="${PROJECT_SOURCE_DIR}") + +if (CMAKE_BUILD_TYPE STREQUAL "") + set(CMAKE_BUILD_TYPE Release) + add_definitions(-O3) +endif() + +if($ENV{ROS_DISTRO} STREQUAL "humble") # the ros2 humble requires c++17 +add_definitions(-std=c++17) +else() +add_definitions(-std=c++14) +endif() + +add_compile_options(-Wall) + +#======================== +# Dependencies Setup +#======================== + +#ROS# +#Catkin# +if($ENV{ROS_VERSION} MATCHES "1") + find_package(roscpp 1.12 QUIET) + find_package(roslib QUIET) + include_directories(${roscpp_INCLUDE_DIRS} ${roslib_INCLUDE_DIRS}) + set(ROS_LIBS ${roscpp_LIBRARIES} ${roslib_LIBRARIES}) + add_definitions(-DROS_FOUND) + add_definitions(-DRUN_IN_ROS_WORKSPACE) + + find_package(catkin REQUIRED COMPONENTS + roscpp + sensor_msgs + std_msgs + message_generation + roslib) + + add_message_files( + FILES + "UdpPacket.msg" + "UdpFrame.msg" + ) + + generate_messages( + DEPENDENCIES + std_msgs + ) + + + catkin_package(CATKIN_DEPENDS + sensor_msgs + roslib) + +endif($ENV{ROS_VERSION} MATCHES "1") + +#ROS2# +if($ENV{ROS_VERSION} MATCHES "2") + + find_package(rclcpp QUIET) + if(rclcpp_FOUND) + + message(=============================================================) + message("-- ROS2 Found. ROS2 Support is turned On.") + message(=============================================================) + + add_definitions(-DROS2_FOUND) + include_directories(${rclcpp_INCLUDE_DIRS}) + set(CMAKE_CXX_STANDARD 14) + + find_package(ament_cmake REQUIRED) + find_package(sensor_msgs REQUIRED) + find_package(std_msgs REQUIRED) + find_package(rclcpp_action REQUIRED) + find_package(rosidl_typesupport_c REQUIRED) + find_package(rosidl_default_generators REQUIRED) + find_package(builtin_interfaces REQUIRED) + find_package(std_msgs REQUIRED) + + rosidl_generate_interfaces(${PROJECT_NAME} + "msg/msg_ros2/UdpPacket.msg" + "msg/msg_ros2/UdpFrame.msg" + DEPENDENCIES builtin_interfaces std_msgs + ) + ament_export_dependencies(rosidl_default_runtime) + + else(rclcpp_FOUND) + + message(=============================================================) + message("-- ROS2 Not Found. ROS2 Support is turned Off.") + message(=============================================================) + + endif(rclcpp_FOUND ) +endif($ENV{ROS_VERSION} MATCHES "2") + + +#Others# +find_package(yaml-cpp REQUIRED) + +#Include directory# +include_directories(${PROJECT_SOURCE_DIR}/src) + +#Driver core# +add_subdirectory(src/driver/HesaiLidar_SDK_2.0) + + +#======================== +# Build Setup +#======================== + +# add_executable(hesai_ros_driver_node +# node/hesai_ros_driver_node.cpp +# src/manager/node_manager.cpp +# ) + + +find_package(CUDA) +if(CUDA_FOUND) + + message(=============================================================) + message("-- CUDA Found. CUDA Support is turned On.") + message(=============================================================) + + link_directories($ENV{CUDA_PATH}/lib/x64) + set(CUDA_NVCC_FLAGS "-arch=sm_75;-O2;-std=c++17")#根据具体GPU性能更改算力参数 + set(CMAKE_CUDA_FLAGS "${CMAKE_CUDA_FLAGS} -rdc=true") + list(APPEND CUDA_NVCC_FLAGS -Xcompiler -fPIC) + + CUDA_ADD_EXECUTABLE(hesai_ros_driver_node + node/hesai_ros_driver_node.cu + src/manager/node_manager.cu + ./src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/src/buffer.cu + ) + set(CUDA_LIBS "${CUDA_TOOLKIT_ROOT_DIR}/lib64/libcudart.so") + + target_link_libraries(hesai_ros_driver_node + ${YAML_CPP_LIBRARIES} + ${Boost_LIBRARIES} + source_lib + container_lib + ptcClient_lib + ptcParser_lib + log_lib + ${CUDA_LIBS} + # libhesai + ) +else(CUDA_FOUND) + + message(=============================================================) + message("-- CUDA Not Found. CUDA Support is turned Off.") + message(=============================================================) + add_executable(hesai_ros_driver_node + node/hesai_ros_driver_node.cc + src/manager/node_manager.cc + ) + target_link_libraries(hesai_ros_driver_node + ${YAML_CPP_LIBRARIES} + ${Boost_LIBRARIES} + source_lib + container_lib + ptcClient_lib + ptcParser_lib + log_lib + # libhesai + ) + +endif(CUDA_FOUND) + + + target_include_directories(hesai_ros_driver_node PRIVATE + src/driver/HesaiLidar_SDK_2.0/ + src/driver/HesaiLidar_SDK_2.0/libhesai + src/driver/HesaiLidar_SDK_2.0/libhesai/Lidar + src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser + src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include + src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/src + src/driver/HesaiLidar_SDK_2.0/libhesai/UdpProtocol + src/driver/HesaiLidar_SDK_2.0/libhesai/Source/include + src/driver/HesaiLidar_SDK_2.0/libhesai/Container/include + src/driver/HesaiLidar_SDK_2.0/libhesai/Container/src + src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu + src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include + src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/src + src/driver/HesaiLidar_SDK_2.0/libhesai/PtcClient/include + src/driver/HesaiLidar_SDK_2.0/libhesai/PtcParser/include + src/driver/HesaiLidar_SDK_2.0/libhesai/PtcParser + src/driver/HesaiLidar_SDK_2.0/libhesai/Logger/include + src/driver/HesaiLidar_SDK_2.0/libhesai/include + src/driver/HesaiLidar_SDK_2.0/driver + src/manager + src/msg/ros_msg + src/msg/rs_msg + src/utility + ${CMAKE_CURRENT_BINARY_DIR} +) + +#Ros# +if($ENV{ROS_VERSION} MATCHES "1") + target_link_libraries(hesai_ros_driver_node ${ROS_LIBS}) + install(TARGETS hesai_ros_driver_node + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) +endif($ENV{ROS_VERSION} MATCHES "1") + + +#Ros2# + +if($ENV{ROS_VERSION} MATCHES "2") + find_package(ament_index_cpp REQUIRED) + ament_target_dependencies(hesai_ros_driver_node + "ament_index_cpp" + "rcl_interfaces" + "rclcpp" + "rcutils" + "std_msgs" + "sensor_msgs" + # "tf2_geometry_msgs" + ) + rosidl_target_interfaces(hesai_ros_driver_node ${PROJECT_NAME} "rosidl_typesupport_cpp") + + install(TARGETS + hesai_ros_driver_node + DESTINATION lib/${PROJECT_NAME}) + + install(DIRECTORY + launch + rviz + DESTINATION share/${PROJECT_NAME}) + + + ament_package() + +endif($ENV{ROS_VERSION} MATCHES "2") + + diff --git a/deploy/ros2_ws/src/lidar_node/README.md b/deploy/ros2_ws/src/lidar_node/README.md new file mode 100644 index 0000000..bcb7b96 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/README.md @@ -0,0 +1,161 @@ +# Introduction to HesaiLidar_ROS_2.0 +This repository includes the ROS Driver for Hesai LiDAR sensor manufactured by Hesai Technology. +Developed based on [HesaiLidar_SDK_2.0](https://github.com/HesaiTechnology/HesaiLidar_SDK_2.0), After launched, the project will monitor UDP packets from Lidar,parse data and publish point cloud frames into ROS topic + +## Support Lidar type +- Pandar +- AT128 +- QT +- FT120 +- XT16/XT32 + +### Installation dependencies + +Install ROS related dependency libraries, please refer to: http://wiki.ros.org + +- Ubuntu 16.04 - ROS Kinetic desktop +- Ubuntu 18.04 - ROS Melodic desktop +- Ubuntu 20.04 - ROS Noetic desktop +- Ubuntu 18.04 - ROS2 Dashing desktop +- Ubuntu 20.04 - ROS2 Foxy desktop +- Ubuntu 22.04 - ROS2 Humble desktop + +### Install Boost + + sudo apt-get update + sudo apt-get install libboost-all-dev + +### Install Yaml + + sudo apt-get update + sudo apt-get install -y libyaml-cpp-dev + +### Clone +``` +$ git clone --recurse-submodules https://github.com/HesaiTechnology/HesaiLidar_ROS_2.0.git +``` + +### Compile and run + +- ros1 + + Create an `src` folder, copy the source code of the ros driver into it, and then run the following command: + + catkin_make + source devel/setup.bash + roslaunch hesai_ros_driver start.launch + +- ros2 + + Create an `src` folder, copy the source code of the ros driver into it, and then run the following command: + + colcon build --symlink-install + . install/local_setup.bash + + For ROS2-Dashing + + ros2 launch hesai_ros_driver dashing_start.py + + For other ROS2 version + + ros2 launch hesai_ros_driver start.py + +### Introduction to the configuration file `config.yaml` parameters + + lidar: + - driver: + udp_port: 2368 #UDP port of lidar + ptc_port: 9347 #PTC port of lidar + device_ip_address: 192.168.1.201 #IP address of lidar + pcap_path: "" #The path of pcap file (set during offline playback) + correction_file_path: "" #LiDAR angle file, required for offline playback of pcap/packet rosbag + firetimes_path: "" #The path of firetimes file + source_type: 2 #The type of data source, 1: real-time lidar connection, 2: pcap, 3: packet rosbag + pcap_play_synchronization: true #Pcap play rate synchronize with the host time + x: 0 #Calibration parameter + y: 0 #Calibration parameter + z: 0 #Calibration parameter + roll: 0 #Calibration parameter + pitch: 0 #Calibration parameter + yaw: 0 #Calibration parameter + ros: + ros_frame_id: hesai_lidar #Frame id of packet message and point cloud message + ros_recv_packet_topic: /lidar_packets #Topic used to receive lidar packets from ROS + ros_send_packet_topic: /lidar_packets #Topic used to send lidar packets through ROS + ros_send_point_cloud_topic: /lidar_points #Topic used to send point cloud through ROS + send_packet_ros: true #true: Send packets through ROS + send_point_cloud_ros: true #true: Send point cloud through ROS + +### Real time playback + +Set the `source_type` in the configuration file to `1` and input the correct lidar `udp_port`, `ptc_port` (default 9347, usually unchanged) and `device_ip_address`, then run start.launch. + +### Parsing PCAP file + +Set the `source_type` in the configuration file to `2` and input the correct lidar `pcap_path` , `correction_file_path` and `firetime_file_path`, then run start.launch. + +### Record and playback ROSBAG file + +- Record ： + + When playing or parsing PCAP in real-time, set `send_packet_ros` to `true`, start another terminal and enter the following command to record the data packet ROSBAG. + + rosbag record ros_send_packet_topic + +- Playback ： + + First, replay the recorded rosbag file `test.bag` using the following command. + + rosbag play test.bag + + Set the `source_type` in the configuration file to `3` and input the correct lidar `correction_file_path` , `firetime_file_path` and `ros_recv_packet_topic`(the topic name of rosbag), then run start.launch. + +### Realize multi lidar fusion + +According to the configuration of a single lidar, multiple drivers can be created in `config.yaml`, as shown in the following example + + lidar: + - driver: + udp_port: 2368 + ptc_port: 9347 + device_ip_address: 192.168.1.201 + pcap_path: "" + correction_file_path: "" + firetimes_path: "" + source_type: 2 + pcap_play_synchronization: true + x: 0 + y: 0 + z: 0 + roll: 0 + pitch: 0 + yaw: 0 + ros: + ros_frame_id: hesai_lidar + ros_recv_packet_topic: /lidar_packets + ros_send_packet_topic: /lidar_packets + ros_send_point_cloud_topic: /lidar_points + send_packet_ros: true + send_point_cloud_ros: true + - driver: + udp_port: 2368 + ptc_port: 9347 + device_ip_address: 192.168.1.201 + pcap_path: "" + correction_file_path: "" + firetimes_path: "" + source_type: 2 + pcap_play_synchronization: true + x: 0 + y: 0 + z: 0 + roll: 0 + pitch: 0 + yaw: 0 + ros: + ros_frame_id: hesai_lidar + ros_recv_packet_topic: /lidar_packets2 + ros_send_packet_topic: /lidar_packets2 + ros_send_point_cloud_topic: /lidar_points2 + send_packet_ros: false + send_point_cloud_ros: true diff --git a/deploy/ros2_ws/src/lidar_node/Version.h.in b/deploy/ros2_ws/src/lidar_node/Version.h.in new file mode 100644 index 0000000..77e23c7 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/Version.h.in @@ -0,0 +1,21 @@ +///////////////////////////////////////////////////////////////////////////////////////// +// +// Copyright [2022] [Hesai Technology Co., Ltd] +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License +// +///////////////////////////////////////////////////////////////////////////////////////// + +#define VERSION_MAJOR @VERSION_MAJOR@ +#define VERSION_MINOR @VERSION_MINOR@ +#define VERSION_TINY @VERSION_TINY@ \ No newline at end of file diff --git a/deploy/ros2_ws/src/lidar_node/config/config.yaml b/deploy/ros2_ws/src/lidar_node/config/config.yaml new file mode 100644 index 0000000..9ed5a52 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/config/config.yaml @@ -0,0 +1,26 @@ +lidar: + - driver: + pcap_play_synchronization: false # pcap play rate synchronize with the host time + udp_port: 2368 #UDP port of lidar + ptc_port: 9347 #PTC port of lidar + device_ip_address: 192.168.123.20 #host_ip_address + #pcap_path: "Your pcap file path" #The path of pcap file + #correction_file_path: "Your correction file path" #The path of correction file + #firetimes_path: "Your firetime file path" #The path of firetimes file + source_type: 1 #The type of data source, 1: real-time lidar connection, 2: pcap, 3: packet rosbag + frame_start_azimuth: -1 #Frame azimuth for Pandar128, range from 1 to 359, set it less than 0 if you + #do not want to use it. + #transform param + x: 0 + y: 0 + z: 0 + roll: 0 + pitch: 0 + yaw: 0 + ros: + ros_frame_id: go2/hesai_lidar #Frame id of packet message and point cloud message + ros_recv_packet_topic: go2/lidar_packets #Topic used to receive lidar packets from rosbag + ros_send_packet_topic: go2/lidar_packets #Topic used to send lidar raw packets through ROS + ros_send_point_cloud_topic: go2/lidar_points #Topic used to send point cloud through ROS + send_packet_ros: false #true: Send packets through ROS + send_point_cloud_ros: true #true: Send point cloud through ROS diff --git a/deploy/ros2_ws/src/lidar_node/launch/dashing_start.py b/deploy/ros2_ws/src/lidar_node/launch/dashing_start.py new file mode 100755 index 0000000..84ba352 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/launch/dashing_start.py @@ -0,0 +1,21 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +def generate_launch_description(): + rviz_config=get_package_share_directory('hesai_ros_driver')+'/rviz/rviz2.rviz' + return LaunchDescription([ + Node( + package='hesai_ros_driver', + node_namespace='hesai_ros_driver', + node_name='hesai_ros_driver_node', + node_executable='hesai_ros_driver_node', + output='screen' + ), + Node( + package='rviz2', + node_namespace='rviz2', + node_name='rviz2', + node_executable='rviz2', + arguments=['-d',rviz_config] + ) + ]) diff --git a/deploy/ros2_ws/src/lidar_node/launch/start.launch b/deploy/ros2_ws/src/lidar_node/launch/start.launch new file mode 100755 index 0000000..76e71eb --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/launch/start.launch @@ -0,0 +1,6 @@ + + + + + + diff --git a/deploy/ros2_ws/src/lidar_node/launch/start.py b/deploy/ros2_ws/src/lidar_node/launch/start.py new file mode 100755 index 0000000..b670762 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/launch/start.py @@ -0,0 +1,12 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + + rviz_config=get_package_share_directory('hesai_ros_driver')+'/rviz/rviz2.rviz' + + return LaunchDescription([ + Node(namespace='hesai_ros_driver', package='hesai_ros_driver', executable='hesai_ros_driver_node', output='screen'), + Node(namespace='rviz2', package='rviz2', executable='rviz2', arguments=['-d',rviz_config]) + ]) diff --git a/deploy/ros2_ws/src/lidar_node/msg/UdpFrame.msg b/deploy/ros2_ws/src/lidar_node/msg/UdpFrame.msg new file mode 100644 index 0000000..695f8b8 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/msg/UdpFrame.msg @@ -0,0 +1,2 @@ +Header header +UdpPacket[] packets diff --git a/deploy/ros2_ws/src/lidar_node/msg/UdpPacket.msg b/deploy/ros2_ws/src/lidar_node/msg/UdpPacket.msg new file mode 100644 index 0000000..74bc21e --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/msg/UdpPacket.msg @@ -0,0 +1,15 @@ +# field size(byte) +# SOB 2 +# angle 2 +# measure 5 +# block SOB + angle + measure * 40 +# timestamp 4 +# factory 2 +# reserve 8 +# rpm 2 +# tail timestamp + factory + reserve + rpm +# packet block * 6 + tail + +time stamp +uint8[] data +uint32 size \ No newline at end of file diff --git a/deploy/ros2_ws/src/lidar_node/msg/msg_ros2/UdpFrame.msg b/deploy/ros2_ws/src/lidar_node/msg/msg_ros2/UdpFrame.msg new file mode 100644 index 0000000..02ac45b --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/msg/msg_ros2/UdpFrame.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +UdpPacket[] packets diff --git a/deploy/ros2_ws/src/lidar_node/msg/msg_ros2/UdpPacket.msg b/deploy/ros2_ws/src/lidar_node/msg/msg_ros2/UdpPacket.msg new file mode 100644 index 0000000..1b438d5 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/msg/msg_ros2/UdpPacket.msg @@ -0,0 +1,15 @@ +# field size(byte) +# SOB 2 +# angle 2 +# measure 5 +# block SOB + angle + measure * 40 +# timestamp 4 +# factory 2 +# reserve 8 +# rpm 2 +# tail timestamp + factory + reserve + rpm +# packet block * 6 + tail + +builtin_interfaces/Time stamp +uint8[] data +uint32 size \ No newline at end of file diff --git a/deploy/ros2_ws/src/lidar_node/node/hesai_ros_driver_node.cc b/deploy/ros2_ws/src/lidar_node/node/hesai_ros_driver_node.cc new file mode 100644 index 0000000..1078279 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/node/hesai_ros_driver_node.cc @@ -0,0 +1,108 @@ +/************************************************************************************************ + Copyright(C)2023 Hesai Technology Co., Ltd. + All code in this repository is released under the terms of the following [Modified BSD License.] + Modified BSD License: + Redistribution and use in source and binary forms,with or without modification,are permitted + provided that the following conditions are met: + *Redistributions of source code must retain the above copyright notice,this list of conditions + and the following disclaimer. + *Redistributions in binary form must reproduce the above copyright notice,this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + *Neither the names of the University of Texas at Austin,nor Austin Robot Technology,nor the names of + other contributors maybe used to endorse or promote products derived from this software without + specific prior written permission. + THIS SOFTWARE IS PROVIDED BY THE COPYRIGH THOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED + WARRANTIES,INCLUDING,BUT NOT LIMITED TO,THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A + PARTICULAR PURPOSE ARE DISCLAIMED.IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR + ANY DIRECT,INDIRECT,INCIDENTAL,SPECIAL,EXEMPLARY,OR CONSEQUENTIAL DAMAGES(INCLUDING,BUT NOT LIMITED TO, + PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;LOSS OF USE,DATA,OR PROFITS;OR BUSINESS INTERRUPTION)HOWEVER + CAUSED AND ON ANY THEORY OF LIABILITY,WHETHER IN CONTRACT,STRICT LIABILITY,OR TORT(INCLUDING NEGLIGENCE + OR OTHERWISE)ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,EVEN IF ADVISED OF THE POSSIBILITY OF + SUCHDAMAGE. +************************************************************************************************/ + +/* + * File: hesai_ros_driver_node.cc + * Author: Zhang Yu + * Description: Hesai sdk node for CPU + * Created on June 12, 2023, 10:46 AM + */ + +#include "manager/node_manager.h" +#include + +#include +#include "Version.h" + +#ifdef ROS_FOUND +#include +#include +#elif ROS2_FOUND +#include +#endif + +#ifdef ROS2_FOUND +std::mutex g_mtx; +std::condition_variable g_cv; +#endif + + +static void sigHandler(int sig) +{ +#ifdef ROS_FOUND + ros::shutdown(); +#elif ROS2_FOUND + g_cv.notify_all(); +#endif +} + +int main(int argc, char** argv) +{ + std::cout << "-------- Hesai Lidar ROS V" << VERSION_MAJOR << "." << VERSION_MINOR << "." << VERSION_TINY << " --------" << std::endl; + signal(SIGINT, sigHandler); ///< bind ctrl+c signal with the sigHandler function + +#ifdef ROS_FOUND + ros::init(argc, argv, "hesai_ros_driver_node", ros::init_options::NoSigintHandler); +#elif ROS2_FOUND + rclcpp::init(argc, argv); +#endif + + std::string config_path; + +#ifdef RUN_IN_ROS_WORKSPACE + config_path = ros::package::getPath("hesai_ros_driver"); +#else + config_path = (std::string)PROJECT_PATH; +#endif + + config_path += "/config/config.yaml"; + +#ifdef ROS_FOUND + ros::NodeHandle priv_hh("~"); + std::string path; + priv_hh.param("config_path", path, std::string("")); + if (!path.empty()) + { + config_path = path; + } +#endif + + YAML::Node config; + config = YAML::LoadFile(config_path); + + + std::shared_ptr demo_ptr = std::make_shared(); + demo_ptr->Init(config); + demo_ptr->Start(); + + +#ifdef ROS_FOUND + ros::MultiThreadedSpinner spinner(2); + spinner.spin(); +#elif ROS2_FOUND + std::unique_lock lck(g_mtx); + g_cv.wait(lck); +#endif + + return 0; +} diff --git a/deploy/ros2_ws/src/lidar_node/node/hesai_ros_driver_node.cu b/deploy/ros2_ws/src/lidar_node/node/hesai_ros_driver_node.cu new file mode 100644 index 0000000..033532f --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/node/hesai_ros_driver_node.cu @@ -0,0 +1,108 @@ +/************************************************************************************************ + Copyright(C)2023 Hesai Technology Co., Ltd. + All code in this repository is released under the terms of the following [Modified BSD License.] + Modified BSD License: + Redistribution and use in source and binary forms,with or without modification,are permitted + provided that the following conditions are met: + *Redistributions of source code must retain the above copyright notice,this list of conditions + and the following disclaimer. + *Redistributions in binary form must reproduce the above copyright notice,this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + *Neither the names of the University of Texas at Austin,nor Austin Robot Technology,nor the names of + other contributors maybe used to endorse or promote products derived from this software without + specific prior written permission. + THIS SOFTWARE IS PROVIDED BY THE COPYRIGH THOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED + WARRANTIES,INCLUDING,BUT NOT LIMITED TO,THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A + PARTICULAR PURPOSE ARE DISCLAIMED.IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR + ANY DIRECT,INDIRECT,INCIDENTAL,SPECIAL,EXEMPLARY,OR CONSEQUENTIAL DAMAGES(INCLUDING,BUT NOT LIMITED TO, + PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;LOSS OF USE,DATA,OR PROFITS;OR BUSINESS INTERRUPTION)HOWEVER + CAUSED AND ON ANY THEORY OF LIABILITY,WHETHER IN CONTRACT,STRICT LIABILITY,OR TORT(INCLUDING NEGLIGENCE + OR OTHERWISE)ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,EVEN IF ADVISED OF THE POSSIBILITY OF + SUCHDAMAGE. +************************************************************************************************/ + +/* + * File: hesai_ros_driver_node.cu + * Author: Zhang Yu + * Description: Hesai sdk node for GPU + * Created on June 12, 2023, 10:46 AM + */ + +#include "manager/node_manager.h" +#include + +#include +#include "Version.h" + +#ifdef ROS_FOUND +#include +#include +#elif ROS2_FOUND +#include +#endif + +#ifdef ROS2_FOUND +std::mutex g_mtx; +std::condition_variable g_cv; +#endif + + +static void sigHandler(int sig) +{ +#ifdef ROS_FOUND + ros::shutdown(); +#elif ROS2_FOUND + g_cv.notify_all(); +#endif +} + +int main(int argc, char** argv) +{ + std::cout << "-------- Hesai Lidar ROS V" << VERSION_MAJOR << "." << VERSION_MINOR << "." << VERSION_TINY << " --------" << std::endl; + signal(SIGINT, sigHandler); ///< bind ctrl+c signal with the sigHandler function + +#ifdef ROS_FOUND + ros::init(argc, argv, "hesai_ros_driver_node", ros::init_options::NoSigintHandler); +#elif ROS2_FOUND + rclcpp::init(argc, argv); +#endif + + std::string config_path; + +#ifdef RUN_IN_ROS_WORKSPACE + config_path = ros::package::getPath("hesai_ros_driver"); +#else + config_path = (std::string)PROJECT_PATH; +#endif + + config_path += "/config/config.yaml"; + +#ifdef ROS_FOUND + ros::NodeHandle priv_hh("~"); + std::string path; + priv_hh.param("config_path", path, std::string("")); + if (!path.empty()) + { + config_path = path; + } +#endif + + YAML::Node config; + config = YAML::LoadFile(config_path); + + + std::shared_ptr demo_ptr = std::make_shared(); + demo_ptr->Init(config); + demo_ptr->Start(); + + +#ifdef ROS_FOUND + ros::MultiThreadedSpinner spinner(2); + spinner.spin(); +#elif ROS2_FOUND + std::unique_lock lck(g_mtx); + g_cv.wait(lck); +#endif + + return 0; +} diff --git a/deploy/ros2_ws/src/lidar_node/package.xml b/deploy/ros2_ws/src/lidar_node/package.xml new file mode 100644 index 0000000..3c45e64 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/package.xml @@ -0,0 +1,41 @@ + + + hesai_ros_driver + 1.5.0 + The hesai_ros_driver_node package + hesaiwuxiaozhou + BSD + + catkin + ament_cmake + rosidl_default_generators + + roscpp + rclcpp + + sensor_msgs + roslib + std_msgs + + rclcpp + sensor_msgs + std_msgs + ament_index_cpp + rclcpp_action + + roscpp + sensor_msgs + roslib + + rosidl_interface_packages + + + catkin + ament_cmake + + + + + + + diff --git a/deploy/ros2_ws/src/lidar_node/rviz/rviz.rviz b/deploy/ros2_ws/src/lidar_node/rviz/rviz.rviz new file mode 100755 index 0000000..2ca1f27 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/rviz/rviz.rviz @@ -0,0 +1,346 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /PointCloud26 + - /PointCloud28 + Splitter Ratio: 0.5 + Tree Height: 549 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /lidar_points + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /lidar_points1 + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /lidar_points2 + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /lidar_points3 + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /lidar_points4 + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.05000000074505806 + Style: Flat Squares + Topic: /lidar_points5 + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /lidar_points6 + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /lidar_points6 + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: hesai_lidar + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 35.43638229370117 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 3.932443618774414 + Y: -6.068606853485107 + Z: 3.755636215209961 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.335399329662323 + Target Frame: + Value: Orbit (rviz) + Yaw: 1.738570213317871 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004f30000003efc0100000002fb0000000800540069006d00650100000000000004f3000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000282000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1267 + X: 514 + Y: 27 diff --git a/deploy/ros2_ws/src/lidar_node/rviz/rviz2.rviz b/deploy/ros2_ws/src/lidar_node/rviz/rviz2.rviz new file mode 100755 index 0000000..af124ae --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/rviz/rviz2.rviz @@ -0,0 +1,159 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /PointCloud21 + Splitter Ratio: 0.5 + Tree Height: 787 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 255 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.05000000074505806 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /lidar_points + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: hesai_lidar + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 49.51913070678711 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.3094900846481323 + Y: 6.032918930053711 + Z: -1.6452505588531494 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.7053972482681274 + Target Frame: + Value: Orbit (rviz) + Yaw: 3.2622175216674805 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1016 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001560000039efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004c50000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1846 + X: 74 + Y: 27 diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/CMakeLists.txt b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/CMakeLists.txt new file mode 100644 index 0000000..774542e --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/CMakeLists.txt @@ -0,0 +1,117 @@ +cmake_minimum_required(VERSION 3.8.11) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_STANDARD 14) +PROJECT(Template) +set(VERSION_MAJOR 2) +set(VERSION_MINOR 0) +set(VERSION_TINY 5) +configure_file( + "${CMAKE_CURRENT_SOURCE_DIR}/Version.h.in" + "${CMAKE_CURRENT_BINARY_DIR}/Version.h" +) + +cmake_policy(SET CMP0053 NEW) +set(CMAKE_BUILD_TYPE ON) +set(CMAKE_BUILD_TYPE Release) +add_definitions(-O3) +add_compile_options(-Wall) + +# 设置生成平台为 x64 +if(_MSC_VER) +set(CMAKE_GENERATOR_PLATFORM x64) +set(OPENSSL_ROOT_DIR "C:/Program Files/OpenSSL-Win64") +endif() + + +include_directories("${PROJECT_BINARY_DIR}") +find_package(Boost REQUIRED COMPONENTS system filesystem thread) +find_package(OpenSSL REQUIRED) +add_definitions(-DQT_MESSAGELOGCONTEXT) +add_subdirectory(libhesai) + +# set (Boost_INCLUDE_DIRS "/usr/lib/x86_64-linux-gnu/") + +include_directories( + libhesai + libhesai/Lidar + libhesai/UdpParser + libhesai/UdpParser/include + libhesai/UdpParser/src + libhesai/UdpProtocol + libhesai/Source/include + libhesai/Container/include + libhesai/Container/src + libhesai/UdpParserGpu + libhesai/UdpParserGpu/include + libhesai/UdpParserGpu/src + libhesai/PtcClient/include + libhesai/PtcParser + libhesai/PtcParser/src + libhesai/PtcParser/include + libhesai/Logger/include + libhesai/include + driver + third_party + ${Boost_INCLUDE_DIRS} + ${OPENSSL_INCLUDE_DIR} +) +link_directories(./libhesai/PtcClient/lib) +link_directories(${Boost_LIBRARY_DIRS}) + +add_executable(sample + test/test.cc +) + +target_link_libraries(sample + ${Boost_LIBRARIES} + source_lib + container_lib + ptcClient_lib + ptcParser_lib + log_lib + # ${PCL_LIBRARIES} + ${OPENSSL_LIBRARIES} +) + +# add_library(libhesai +# driver/hesai_lidar_sdk.hpp +# ) +# target_link_libraries(libhesai +# ${Boost_LIBRARIES} +# source_lib +# container_lib +# ptcClient_lib +# log_lib + +# ) + +find_package(CUDA ) +if(${CUDA_FOUND}) + set(CUDA_SOURCE_PROPERTY_FORMAT OBJ) + set(CUDA_SEPARABLE_COMPILATION ON) + include_directories(${CUDA_INCLUDE_DIRS}) + set(CUDA_PROPAGATE_HOST_FLAGS OFF) + set(CUDA_NVCC_FLAGS -arch=sm_61;-O3;-G;-g;-std=c++14)#根据具体GPU性能更改算力参数 + link_directories($ENV{CUDA_PATH}/lib/x64) + file(GLOB_RECURSE CURRENT_HEADERS *.h *.hpp *.cuh) + file(GLOB CURRENT_SOURCES *.cpp *.cu) + file(GLOB CURRENT_SOURCES *.cpp *.cu) + source_group("Include" FILES ${CURRENT_HEADERS}) + source_group("Source" FILES ${CURRENT_SOURCES}) + CUDA_ADD_EXECUTABLE(sample_gpu ${CURRENT_HEADERS} ${CURRENT_SOURCES} + ./test/test.cu + ./libhesai/UdpParserGpu/src/buffer.cu) + target_link_libraries(sample_gpu + ${Boost_LIBRARIES} + source_lib + container_lib + ptcClient_lib + log_lib + ptcParser_lib + ${OPENSSL_LIBRARIES} + # ${PCL_LIBRARIES} +) +else(${CUDA_FOUND}) + MESSAGE(STATUS "cuda not found!") +endif(${CUDA_FOUND}) + diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/LICENSE b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/LICENSE new file mode 100644 index 0000000..e33835d --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/LICENSE @@ -0,0 +1,14 @@ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Hesai Technology, zhangyu +Copyright (C) 2023 Hesai Technology, huangzhongbo +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. \ No newline at end of file diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/README.md b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/README.md new file mode 100644 index 0000000..9c9e290 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/README.md @@ -0,0 +1,73 @@ +# HesaiLidar_SDK_2.0 +## About the project +This repository includes the software development kit for Hesai LiDAR sensor manufactured by Hesai Technology + +## Support Lidar type +- Pandar +- AT128 +- QT +- FT120 +- XT16/XT32 + +## Environment and Dependencies + +**System environment requirement:Linux** +``` +Recommanded +-Ubuntu 16.04 +-Ubuntu 18.04 +-Ubuntu 20.04 +-Ubuntu 22.04 +``` + +**Compiler vresion requirement** +``` +Cmake version requirement:Cmake 3.8.0 or above +G++ version requirement:G++ 7.5 or above +``` +**Library Dependencies: libpcl-dev + libpcap-dev + libyaml-cpp-dev + libboost-dev** +``` +$ sudo apt install libpcl-dev libpcap-dev libyaml-cpp-dev libboost-dev +``` + +## Clone +``` +$ git clone https://github.com/HesaiTechnology/HesaiLidar_SDK_2.0.git +``` + +## Build +``` +1.$ cd HesaiLidar_SDK_2.0 +2.$ mkdir build +3.$ cd build +4.$ cmake .. +5.$ make +``` + +## Run a sample + +Set the parameters in param in main.cc or main.cu +``` +// Reciving data from pcap file +``` + param.input_param.source_type = DATA_FROM_PCAP; + param.input_param.pcap_path = "path/to/pcap"; + param.input_param.correction_file_path = "/path/to/correction.csv"; + param.input_param.firetimes_path = "path/to/firetimes.csv"; +``` +// Reciving data from connected Lidar +``` + param.input_param.source_type = DATA_FROM_LIDAR; + param.input_param.device_ip_address = '192.168.1.201'; // 192.168.1.201 is the lidar ip address + param.input_param.ptc_port = 9347; // 9347 is the lidar ptc port + param.input_param.udp_port = 2368; // 2368 is the lidar udp port + param.input_param.host_ip_address = "192.168.1.100"; // 192.168.1.100 is the pc ip address + param.input_param.multicast_ip_address = "239.0.0.1"; // 239.0.0.1 is the lidar multcast ip address, set this parameter to "" when lidar do not support muticast +``` + +$ make +// run a cpu sample +$ ./sample +// run a gpu sample +$ ./sample_gpu +``` diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/Version.h.in b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/Version.h.in new file mode 100644 index 0000000..77e23c7 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/Version.h.in @@ -0,0 +1,21 @@ +///////////////////////////////////////////////////////////////////////////////////////// +// +// Copyright [2022] [Hesai Technology Co., Ltd] +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License +// +///////////////////////////////////////////////////////////////////////////////////////// + +#define VERSION_MAJOR @VERSION_MAJOR@ +#define VERSION_MINOR @VERSION_MINOR@ +#define VERSION_TINY @VERSION_TINY@ \ No newline at end of file diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/change notes.md b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/change notes.md new file mode 100644 index 0000000..ba23102 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/change notes.md @@ -0,0 +1,23 @@ +# HesaiLidar_SDK_2.0 + +Friday, June 9th, 2023 16:45 +## version +V2.0.1 + +## modify +1. first update +2. fix AT128 frame segmentation bug + +Monday, October 16th, 2023 11:00 +## version +V2.0.4 + +## modify +1. support ET25 + +Wednesday, October 25th, 2023 20:00 +## version +V2.0.5 + +## modify +1. Fix bug in decode OT128 timestamp \ No newline at end of file diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/AT128E2X_Angle Correction File.dat b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/AT128E2X_Angle Correction File.dat new file mode 100644 index 0000000000000000000000000000000000000000..2ca48f7bf7f108353c078181d81c8017b516097a GIT binary patch literal 72572 zcmeIwKS)+_6b9gX@9zo@5)KXy-|#^WMh-#*4nhVFMh+4V4h{}NMG_7M1`ZBF4h{}N z4Msu^4h{|m4iXLyLKF@TLPA194iXLq(*0N8*z&C{&w+P3yvuXW&ngb{A;d`#(&4l(?MmCzES!g< zuopJN>#z`}!+019cS1e1h3n)jIZSqwjbtU6Po|QwWH9MXYDsHy6`#hR;+=RsUXJJD zC-G=J5ckBLaT;GnC(%K)9j!%6(QGssjYR!XchvFs7#G!XwO?&jtJPvPQ%zLERbSOr zwO4t0ULKWuYF)7%X~=TG3iu<)``Q zd?#Pem-D&&X+D|{quL0RjXF5FkK+ z009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBly zK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF z5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk z1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs z0RjXF5FkK+009C72oNAZfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZ zfB*pk1PBlyK!5-N0t5&UAV7cs0RjXF5FkK+009C72oNAZfB*pk1PBly@UOsw=ik2H hc=h9Uc>Cqi@5disyuW{IYWC~1U)9YI@2>8(gg*h;6<7cO literal 0 HcmV?d00001 diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/FT120C1X_Angle Correction File.dat b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/FT120C1X_Angle Correction File.dat new file mode 100644 index 0000000000000000000000000000000000000000..40e521425d40cc47731060383dd7d75f48c7740d GIT binary patch literal 153641 zcmdqmWz?Nj_dCX3 z<6Og`_j5nzoN->97oV~AwfEX<&H0Nt*M^60B82Y|enH>xr{4}2E?lH=;c}oM#$pR@ zAj&)8!sSMN48{^1#eKwhm+zn&x?loU;xK-|pGXinT)12)kEZB@30Q=!IEJfugg1y4 zC0w}F$c5slf(B@V9vFx*n1*>+f-kWNJFpLj@eNMn94_KAzQM2iYM?!w$2@F=`MiPWc$c{+Lk^UL@o53`I1E!TAFE-EjrS>-tJ`>h z7x)WrM(m)JTs^$bzgP?{N$S?`7t_3`mQVNP_r?iO7h6m)z$G?%_Jl!#c1A zHee~t{a6fud2fh{D2R-R2kZDgE?_TKU<&%73Cbc1V!&LV!)DAxZ`4L^*k3=Bk3*P` z-l&4Kcu5{lVm=094X(m|HLuMv0b6hlZ*#4zuvdG)dwqrP@Dhm_i#e@@E*Oi&*ahcd ze@0?%$zg9==gz$ghQfZd7dPV&?7JIyj6Y$Fjdv=Tle{Q~GN^(&XoTiyi%#fL1tGbr?aKaQT)1D(+Utt}UNo9PIVh zsEJ~*$K&D;Vsjhcz#g!-2csFvAtR#TK5;&V6&Q_{D1jt+Mvh!p7h?c|JpV>c&m#iw zlhw*1cDY(vd#YoJ-8tj9) z{1NuzJH#mg>^pN<66UWVjJG*6Pa`oAv+y~V;!AAASJ;JpID{iOiW4}AQz5?gy>H;> zhv9ejU2v>0&u0z)GWZ2{8yBb{6 zQ^Fp2K-|t^Gv;6rK0~KaG1{Z9ORl#AF$31}0bGJ>YdGfbdTJl!hw-sj+QQhG zL*r@wR%0uSzxlX;>$r(G}*s21+0Yk|PT2^B>?ozYU9Ef4eW)10|3FQSgv> zo`HQm4L#u+=`)S%w|(|C7N9R`AQRl9-A{d98i92P@>-bBTVpb;ksFA_wX&f)yvI!V zeB|0|pT=h_=G0p30DE)+9KX*wu3P3a0j%%hs0Dk{T#msUEXOt&J9B6}pTOL`ORVF< zoTNui6hJYQLsis9J$!`bXoa@ufKKR)E@Ak0N3=&9v_um$L|xQCMU+7i7Up9y zR$vV_U<jh_*ZbBm!hCcyRK5_}eLpYVCNIfh{&%&E^6zatJ~d=DSM=M(#L z5?sS}!1{L1zat7UaD6l`#b6#A!+z@v<2e=P&b)1g`8kZ!_zpL47Z30&UgA$gh|ao$ z$cT#QAu%~B_}kB-z;h!a9A5MN7kGmExQ(kghZ8smYrM+Y4#nv&enoc?#>{GpK8LEx1<3!Y}0G2)@8T)Pd`41Y9JK z=6O7J;2sk2zCGU&GvM>D&vxzw@4;ukp0KWbR=5bCi_B?GREBHrFgWf_aKAL4FX4KY z7S7pquMzCWk6|C0M|;m+JBo`iwvS=Vjla1`jMT`4TquB|D1-8-f*PoU577XPLz-|@ z@OL9#`*~f|Mm1DMIg~_UXGqAN0kM9;jQ@E!thtGk(z_r)Cv=Ur*t-%G@3HMIx^c}{Z66UiqT+e!A4CcXH z?uBu&5A7FYW^W~h`7^e~Q3>Y2+_XhE^v6(qf{B=hPca`0u>{Mp605Q1Z+uUFz6@Vr z0p?*AreG{aVlaB66I#KVu!gJyYaj>Gz}&kQxQ>|j?_jOE{x5~=k$v6~_2B-K754XQ zVrDP6XRXC_1i7}aeJ=lt*!w)b1G8bjRfoT~-oj<_wjQI<5Jh3Xn%6^kf~1^R20klI zg}r(OZ{Tx5DKy7WnA2Tw&2hhp#W=H|G#a8S%%{D%39hYI@Cf0FLmZ?<9@sC&%p4lW zz8D4LY|PEqI&8x}9K#u0#5MeYdw7Vac#hxj3V($B#WCP}ukjL|_Y9Bm3+~`1F5^5- z;t+OYGpvh+unxxKWAs34xMoy@dCv-KH41)b&OhNS+-Fu`1_q-Is=;1J4tx6^@$@&G z>$uM>Jy08Y5F7sXvfq}%e@AEl*KVK3{5|8o>K<)gpTb_R0N44s@b`?*zA?B~Zn!^K zPyQD0+2|t7X&lDln&X<=5{}ok$^QHr=JP46Yv<}(XD(}@C3?XgnFf2v92!USXYOvn zn45!eG1yZP2Z@mi8Sx(Sq9BT*1j^zAR6r$ELDdjn`=0!~97@6O6-IvKL{_9n3dBbY zybWu^8nO<)hjn%UTd@-JVBQD8T5SY#UI4E1vGFG{@!7$A@4zCAgFRty7lqGj=K2nC zwy#~o$G|nyf8R-iKgp55JKd+p!1XpCqQZY$GOs=d_kqt#*2FXTOy?fpp6WGM!FqDf za4*dUb877khdsItj@RcBpK;tPUAx>HUDG;aD9ok(XfGUwJ@O--AwnpQNjaLk+%RY6 zsygbUIohEs`eG19U<@W;3Z`Qw=3p*9`y1cWY^f_t?6YF_sq$4@YnU z=Wq#EaTB-k6Yk$rjoID_Ljh+Wu%FR>KXiuGVU4MKOAe{0X& z7lZxp`VtG_@R0f1>#j@oxog!#nD1ur86yvpAUs^lkHdA)T=zsB_`A{l`YAa%1^aFk znxY_FyYG<4omd3_4Zen(QbC$JrlK z;5ubK?all63vrpJ>z%oDT{Ir{VLyBV`)D!N!o^L7>H;R#;ht(fd-h>b)@fwahk zY{-E;cpn8&D5MC-fbaR4JTDjCLl$H}Y9v8?#6Tpx;odJ`o%{gn$=W&q^S=t_elmu_ zyt~%agloV1NODBRbK-Fm-@tY3bBso3)PcR75)pBSIQyJtUynlvRDi#|-h%(O;+{DP z?cll_AAgY7EF9fieO|H_&cSCn*Zy+wTK>MXj(paAjF{ZhoVus@OfeL$KaTSx?9V?C zk9lN<_3Rv5!k!!h*GA*91NO;fn8W7?ABw5DGjHb0Jd{L5)ItL^LmPBN5A?x648d@W z#AuAgxRCLoqwkG@pL?#~8I1nug|29i7WfEtP!(lijaWZfkP>n6F8*NbkKkJHE$sbG zux6*ioOeM(xZmW3d%O8|KlgdgecaxdgkJa%u7~E@zP?5L{oUc(>9daOxBV9j_sE;S zwOn^Q!@Q1w&uBgayO+DK)`Gq2p1L2m;q#GeuYKAIuDkx8*olkq+0${l7ZpTxw80>_ zA3DdcU@pIhJ@6-@6PIK#R_4(7nLlG{e2uknHy2Ye7hhln)?q7lVLuMzI8Nd$&f_A! z3%SBk!QXy%0q1ZUCvX&puov5~39GRL^DzU~h&5p?SO?ZXeN;vXxE7>EeAxH){B5`v z?Z;YJ-=koUH%2+w-|kngbw9whd@WoL&9!@80VIO^^*L;S{pY`_m4g5N=fCS74&~L} zj?8;`(HQ=_k?VlH>NR{uDuTus4EuCDzQc34zhr~sYl1$Q0P|^Yo`Q4x9Z{I8xpW;Y zj_NRvonhaN#Vjnw8tj0vHecrC7VhCGULss9_HaZ;93(_iq(mB|Lk46*=6{fp_tGOR zQXx4KBOYQR3L@f9?)3r>@e{7Wny?nEgUzts=3z3-eJ{AS)I)jXhwHw*?)qeXyJme2 z^F0SXYjl8Xojqc2M}*Jm=Gtc%_v|jH0)Kz`Z*=ybJ?-Au*#PfX_?Tz!qGE zYkn4Zjc)LHcO!f@dIoc9tyMy6xb8T<4X{65pUh_r=3yN>w~A4hygjyI~IP ztvmP?<}NA{z}TCg+$e-nsEF#Qi-u^1)@YB;=!Tx?jXvm$eqs2x?|V*nbU{b7MN2e6 zebhn~ltVG(!+WrHtfg3p1nbTGTZ87_wPY*I`)rIsADDC3C-<6ca1VM5=KBKnU@7d8 zp70sP{mo~!x8UA)2ut7^>NAh)w|(d{?r|)J>u+oLY_<#T;qI&XV6VETdX1y-`6w#a z&WZ|XixK!7_Ne1}1ot5Kqxa!j<^E_L+n3h1xpWm;s4k|G#FA_hIeY@7u5p(_o!@b$9;bD`O@BY?{-}@ z*M*S`KHs=sZ^KO3hn3;)tv|`7Yqvf90IugH;lKU4ultO2 z;xnw{Dh>Oy2h69v>AK~duHrGwrE8yS;`=aJCegS$zFGzvd{No6=q;C?Cr0hVvf{^SA_?U=JC$EBFb<^$!?tzdDsKFf`Vz2ff#pHvmpMq?3n;ym1Y-e#=H;2KpP z)~|hOO`FT*aQ$@tmv9%x#=f)HQX(tz!?+sbdT5Gv=#KsviqV*e>6nfA_yWtY0$*Ys zHee$*VRIP%?Ps2|2CK0gORxa*Fbk704kIuKz0nEQi*-~JAHW*0?$X0rbPf57xjlyK z%^6t3Yw-)ocuL*nHe8+hrhH zOXl2vKl1mCYy4-}51)D6Pcxw++QFP!Tdpatxxc~jWQP6O0OoTPT(6vqd!TEYxqJm{ zJ}Jy&A=o4KjD6M>17Yr_;WL;obFdBja1^I;5!Y}F_wWEu@d7XL8h;^N9M)UBg@_@( zR(QPOoIgY7|HkoGJjQ+egd4bwb2x!R*ad546&Ardn1Eq0|LxHjt|=vv8)@NQ@D^N~ zuHyu@V=1O!Abhr~iu_0cd&d3yG&W%t%(Z=82tJ?rEbDKt^{_8n!e=s{f!(v+SM7Iu z)%7)oVxD%yx_BgJz52AFa-9e&m`_km+=VBB_T4SFsi_Lcfugp7js|^ z?I-hR3@^dFJ%BMcN0AX5iIEB!kR5qY07X#}WlwPFofQ+II{XK@%i;F{n%V4d3gy3UkICkPZ+<(lcYm@s_Y1D(YY)`tDevY-+gOe~O_KE#wyy74^GQ+%?Gh=QZK14IL zMHlqOKn%rbjKyS3!z|3fJj}=ESb&9L__yzW2G9KzGcW}cF$N>>G5VqhI-(^Sqc*Ie zQn0?PHESRtqQZ5-+~39pnD?!)mS(uHW|OGji%)?Y?TC`&(ute3m{Is#*IoCFm9R&Bmay*PFrF-M z9rD?x6I`ce!F_NSPT?k=z_rpovXI-*pD69h}HN4)`j)(2?oO&Xp2U$ zCQHLLz_rG8Co-&M_Y>Esui?67pHDy^w1D}}2cP32;Q?{}7GGf=d{#5p?uls;0XN9k zZp?=LSrz`K@%NYex6etQ^ET&N9~IF7<6us$CD)Mf+%FltzxzvD42Cu7Gs=Ekz|V01 zi^ZJMAP>sGx^IDQ7y^66I4y&*Gj~UE4#wHIo0H!WE-ug0h=F)Wf>cP4%*c+M$cy|a zh{7m>q9_)IfBU}Y6u|q)gB-|;j7WpzNQl^og17JoWBC=iAHcObdQUPXXM}_d`|Tl$G-MCwh&xn{oUgF zZBH+N{cW!%Mj6O~XJ)=q9%TPb19MaEyu6O!gn4(ru%-v0 zBO0JQ^1?MO8eR}f^L-RPTh7K1xPMoK`(JGQN^aa6H)A@wqZ+cpKJ|B(`?k+X?&YrY z?(bLOv%TwsIc*5nj_Gj!^u9j-_Dg z?4=5*jV5Rhb2bR0FbT8pIhJBIHsCAl!hU>><2Z>kIEM?kgzs<}SHkdb-}jvJ_!g(| z4UXa<_F@M%V=Y!-5$0kVteauz2kWdAtbwX9|M`#^$za`@ch@5KfYWfTGUu*euJQIk z6I6ohdP>;ekBGBt;!Z5WDEM4c8RptO`yu)I2ClXKesTS_uZLp=PQvH*_?(*`^Gg|%xgUDJ{xD+~6IbI`tJm!vQh>iG2gycwt)JTsE$b`&)<9iw5IceefDUbvS5f?EL1rhNV z_xv4C@H1|~8aaz2*aPceC9K2ga9tRH&anS$pcLE(tnD}mkB7v`y~usZwQT{$q9=To zD~s%~m)yUv!F|y6b2Qu|{hgT+Z^LJ^U2wnlImtcTXQZm=ipg+YI0I|PH6s&B<0JIJ zWGsjIJddAYea2T_VrLSg==nNBnZ83&e5J9NH)8kQ0Sa4mHpK=F51Shrt+$@tB6$n2#k`i8a`OE!c*g z*n_<|fP**`@^$Fwdw#Ybp0^u2uoauI4y&;Yi!cu}Fd1Vo6#ZZwSyxR^2iBf7@IG8K ztkGC_3$8tPVBYQdudoa=Va~hZBe=%BkCZUq?&X)^GrT?J{@o4khpwYZ;a+(KJ1`6W z-uC&-e_!zT=q*I#JfG#up*2Rp=Rltgd`@_VXxz`|)uJ$;?J)$?u>w0`E^p!~oR@1| zT3GWXQ5E)vy<+~1mARXRg;)h+ybI>#1kT}m+`?Tv#8do^KM_7Y{eyQA710qBu@NVX zcwt`qSuDhW--(P!h=4a-^Ce#33GU-ZT*oDx#!*-s)`T@?z0JiGj6{EQhHHVjxBm;m zzIPvp3j6&o%=sbsTwz~~MK3f#dD!2H5f1j&ajeBO^n$szuM@!Mao66n_zlT;uQ-}v zD9q_zxF@_qg3$FVacl|u(|oSPUYy6zaF2@3n9Zg2Yd^Y%wt{&a0sC(OzQlIePsYmJ z-GQ-ujflk9n46EJNQbP*h5RUrQYeQ?sD@goi~4AY#%L1KjAQcc$5uac<2B9~) zpe?Ks>%#i7)`}oEtO4uNH6kKhXIulW;uP%rb#T41-`#`Exz7{jVSl88J^q|nn{S_2 z*24AN9&3iO$c(6XK;FK=YK(>Jxcm2c_^g|da|*y3ac!7_FX3L{^P+2sYpwY#iF)XW zA()P3FqhV?{rCW`o#xTGW=1~P6V+k^EXN_TeZ_;XE$mI)1=i z{DMb#is$$ZFYy|G{0l#Oh2Pe!+fWJ{p~Z1Yo@<5?Cs%b4cBpV zZD0HELgqU*uk*k@HK(q-?g8$h_YsNfrA8stLI(`PEV#xxrVF?S*H!njl*ow^Fpt)_ za~%M4XwTVe_SQBWz)4(!@iex^+W5bffM+qpMk1s@T4ct1$c=m`grX>c(kP1$PyrQD z>2G|`&pf9b%AgdAp)m3zFLEL)(jyg;AReOOZMAp)_P_{CgMGLjdvOBx-5r>}zhLf+ zmwC>rgMW$eh$feKbXDbVPUb!9Wbb2z-KZn20HuiW!)RPeW#N4EWwG`1y2r{v=Gm z7>vR&3`RfnL>II}3p7FKMw1|s5j=INSn1K;9nd!{;~GPWQz8aPRi{$93FXFNDuW{u_e*9)r)ViHh)f-)AAO zvmLG-_hCMhz}|F?ZGg_OCe5Y$muuExxSp9u*FEPL2jBGYI=Hr8 zz&*Tz<4%gKun(PEJ+wqu*bnB<-ZFQ_$-M1>aXf>|Fy{C01TXL>A||BY5fkx{2+5He z>5vgwkR3Tfa&gRqyno|+ekRZLJ6Vwl>ESg~APEv6HliUC!s8X=`4tafomd;z#Bo>) z+p!MTz#L41>w)V|J2XZ$lz@3pi@1mYp8?Fd>z(`8QcS@>w1)Y1O;3qP_=P#<>R9*LN6864;a0r{lF|yWnHE*3HFA zY=!f-C+rb>=_&qzF*9f8%eWe2<6ZzIP!3g43m>8}nxPHaqYJvD7y6<<24XNi#t;m} zun=F%_x;>+{Z2phMh|pFN3=x?G(iK@K{b>|X%vCAV|`c`$q*OT;Tz^=ZTR7Uco9D5`kTdnD|nap zGoUm)-@U+Rpfzx!lYbNDXvjgQnY7Ur%b z%v)VFg|X}g<81st!6eMUTr9*gtj0QQ#y0H4UL3$79L6zxgA+LUFZ|4Nj^il&-a+ic zZfwU^Y`_|>t;}_y`A|L4A(fj)ibdwHIr^y*nW?!rZsUa4f)1xZXZSWUgaA^TX!| z*VulrFBf71zQzUI#qThWu4frwU7JJe-TAhGF|e=3U>fFO8O)clG`5Fv8W(X5w{Z`T z@fp64V&0>p#gkBMk_7jGjR{@{MU;VB;A z4sPHwzJ+ySZCGoYunN|rbubaufNP8SZ-!bZk3z_X6fp1hyghLpCt%K3U?%LN&hVL~ zG+a01!T$aMKC`=j+uJ^)oAae`54{P`bMMcMN@#%r@Og9@yqEi<`^^iu1|>swSff?Y z2(Fp^F&g%s^R)i$HT%ol85i^R5XQ|I8c%a%yi+0rvLiPNq8Lh{JgT4uYU4vRL}N5V z3$#RQv_-p+_8bGg=VzWH&u@+<@H!1p7d7F%DxwTZpfK_x2QniKtQG6Sx`>F^%*k4? z4y@60a822bjaY^`7!UV}?y&F8dl}dpX%PqJ+f}Ygudto>&CjW&T63&O2N9c4${LKuzp<=T!UN(Tm$TX z>)!QiIcCH4tsmN=9?ZFGynSw8xwhNm?&s!vHip1GyfSj)1Gwf6h4tVwko|cN;kia4 z~AaRuE_UM} zj^Q*e;4*ID2i(I0JjOHphL`vQe<56A<`D80$B6&H_dUmR--N#VisSEijwg7CpYaoJ z;wmoTTb#gQ?7?}73;%# zvnH&=%Q%B0ux4E^tmFBZ3TwPSI-v<_z`oCmbclzDcuKtOb#uN0uJ5jy1JMT6VZKws z-y=S=do=&W@FBWkEZi^cO|SI>>`QAa1#-gs*FsD5z;HO$&*A#G1J36h>^JB70?yyO z#Yb{vgfTOIC18Gxc^x!DGqgoV^gwS6#1M?YCm4@On2KqbiCLHv@)<|X{|CN551u<4 zes2c6j@R_s-eV+&Vleul7rLN5TB0%P!uqjBO2gW-Caj0luufwlBCG-H`A4`GSnGSR z5ldk|jDdOYfCi|D0XV&a0HHJH@0CT zR%00!Vh*NZ9EPJmdcay}3hSUUN+KV!A{AV#-hsJ)fNMC7eOQl$a4#DK*Ll~;a>xzW zP=81G`@-i|^SuIxaSN`st^-+NZ~EOXurFuAT<*dt*pE*Tp0Sum*G|{WA~1)JzXi;n za~lbB=e!qV4b0hI7%Ssu42`Kd`32AL3gMElpCdA2A|4VVDN-N}(jzmnAqR3I5Aq@( z^8XJ!$8-IT-}M^VkrkQX^;5(9B|!qjLNqwG2zbp{U*IvU8EfP+zJ)bmJ#5DYEQd90 z4Sa&ZF#m1Q0M%gb^B^4(z`XxX%mX?QkYy;OS>(m@NZpXg`&dIr%JLhZOjKN=cmzWr%#4vWo(YPAx zq9}z5sDj$4hsJ1*)@X-L=!%}`gT5Gm!T1<%G@nAhffOTsfxL#NTu0w}m{?}q5X2RU}M@P8+n|Iei zdnq}h!d^G$-@^6XKAaAFyfs|o7r}M*3_RE8PuB`-rywfB>vzErn8yWhEW2Ew(44G!=b0DIy*!ME`DzAZX8eq$u^ol+n1W9+7mKhItFaE7@D+AoH}>HGzQ$1; z!-+6X^ZLwx;OD{l-|*V+d#yv*k3H~S-qU-p$Cp?E$M!kqU^*t?6AZ%u^h8Hk6Ae)t z)o*sd{co&Z0ZN?H8_Fr}sMmf~NM=*CiU_Xq)WSBE!vJ%E=JIv1! z7~^xef}8jWzu*a;;}!lwgrwXXkr53s5F7CjABjSea!eMIf}>J~1b+vgdv5T(guM1T zUNaV=BMKtnExch2ukjlk+XFb(AMiac;tZ@4>uv`&;Y%#RTujFp3`HMwLUYtZW!MYu z7wO>|7ZraJbJsxgehRy>8gpQ;oAYL<3i~oGeAe?lKbwe!*aDwnuHq5g17pEF=7jgI zf`%}Mj&B&upJQJNbLYIi#%Wx_E&PmUcnxD^+>E2KHRfrN1v&9P3ZOVj;{#MgRn$No z)JG#UK~uB{Y0a?>+M)gbz;irT!S|c<+G~1k?@PXUxJFn5*1hY2`F9-{0QUxSUmN9+ALczF-o^`ZbQ`Yo=6n@q z!T$91KG>JmhPjLjzwdP_paINde^_(&pgG(K$8`ec&oMuSxpO}Dm2*r3bCwsz!d#gr zV`c2ji?QvDp6G{%bv=1#>?alkhRRq8X~gp39D;co)yerO&I;cx_+$Tw^Xjg5R?rt)WHO1lJjB%^tM= z{y-GQ6CY{eSPP&uoQHX{2b`nx9f(nwh#4?W#>^P5$7bxp0UX8&oW%uP!42HT9o)x5 zJi)JcftMkFa#Xlv|ChIT9r0iIS=e*mguWkK&uc%&Q#^wAb_|ZiFF03Ex%{s9@?!daRCahoUVLz;eHLwn>{izs@ zf#`-7sE>*$4s)Lt@!?u&-d#V>!CqX4`7oFEq@PFPyhO-?!th#NvmN@t9C}~tFv#Bl zUYk3|`vc6Ib1`SmFE*UB^UnlhVtmYvu`{N|ye=A{8Cs$Px}XPoqdx{=C`RHFjKf4s z!c)a6J9c3*FEWZP5aa@gZu# zIevf=ur95QtVoAsupXkqT6jt9f;#w~*Vcgju@mOsHE#yS!rXU;eP-X6fO$`e7;ue! zk7Fs=kFF8s(Yo;(){;GF4$r`~$o0wmITmZw+&RwoU|*Gl_3ymQm$`D@Jz$Q^k8v_) z#?aU<#1gE+T5QI4?8ZJE!V!Ff)A$w_a1obr71wYxFb4dGs}2%F%(aUSk1Phk(ngx7bC%!ksbh6XTq zU0@9l$9Oo8`B;Xv*b3)vj?9noF;>RSSl-8Dyud5`g^0=NeMCbn#6?0RL2{%*8l*=i zWJb1-92|2ZH}W8F82NY|R@l!x*YEgU1=q{UYp?IU(jhfcASoP^V{?p-H8S2tcsQ5e z@D#t`4y>EYI1lUNF!o{_j8#w%#&8lw!aA@w+QJ&Bg%3~&IglFh@ea)W&+vKWIL!MK zm`Cd)EpnqIs=@1ZKyO%c-g6F?VJ&vTyq$uz?fC5x=j8k%gy!u0jfHVBM#jq+8tcL+ zhO(%DDyWHiXn>E<9Ien69nc9~(E~lvC!{~e0T_hAVGQN9hW$Dwe9oYM>qnVHxHPHgyF%;&|@2`e+W&XU!RhT>PZ;#nujx8x1uj4jf z#o)ZmlXG?6#=v-Vfw39@<2VXqFd5S^3!h;jmS6=|V=Xpf6TZTZkUbpthaBSgHICqD z$Tu8y;=kZ|!S_6REd|&2Uc2GF9m5tlrgiudj&&&(VLs+yCY;-NjKVNDck9Mlu~u8+ zBUl^NQ69x$J>-P3O@X+u4#LBA!5XliF2MZnf<5;IW?(ez%Z=CvbLf8a5Z00T^P1LK zHh8~MsEoRBOzq$}?Hk8F29x33%+Yc(`dEayn2re;3D=^Yuojx59$X*Fq7ZVxH8CNg zAsn8PKleV@RCAaHIbq$HKd)h3b;ST!dy`?_9D_OA3TxW2p2kHum!IJLoU6Gp2FAl& z7%O9#3@MQTS&$uhkROFn45d&Olb#5B$sln)hwrGgzD206JjbSid!v=j9xoZxt8^<6``cVH!xUvgX%vX0~WkPRIFN$|Pn2H*1!1;=`}T0mj(-T8(?mE|=WQH}i7_&Mo3IT#u^(UK7*660 z&f^lU;CtM_ZTy5gxQ_>Th$nc8U-3NTcaASZUUU2dfBu&|*Y5=1_gcYqy@&Vmp5EIr zI2Okw$LLtE!*M$Y=W-U#%{d;%0qle^*bHN0Y?i`U&BY8%hIKw11JMhe(Hho79aKeG z6oGw}8P-8OM1eJ6KNjG%x%2!FVcxt}Ukt-oOvgN!t1qz$JK%Va;v~-Fd)$I^HU`Gz zcl?2HDXASqK@7w}0whLqq(WL`49Utd2XY}d@}U3PPf!8tiM=jdFWvvW5F#v&Z7KVxL9 z9>UoD0Ap!Pt^4CJ);qBoYhX>x!wgKoa11~(B8y#7yu^E?LM_gY@lYX|r7p5EIrI2Om`*n)97X2A@3G1hSq9oEL1P)>rHaP2T>Ct)qvf9v6T z6p{0-s|4^`)}Pl8@>HJJj>9pTn-*})j^DXBuOV>G#$XbR)lAI80xZUItioDsz-Da0 zcI?7F9KazQ3HgTOsgScAzr}f62)WEr-{UH-g>i$|ft$SkC&A|)!+zImdClPZ!Tr3q zV{k0NxE$kgIA+Iw5YELp?Zh@XPv`91SHoBslLatFGhxgo!5A9LA?Ocd+y(8?5{*$8 z)leQKV6Ji_3sS*)FsJq5cdRq7GYDRL9K7EgEQI4(gN@jZy>Kqh>lB>xcQ8iA@kiXn zLp;TEyu>T~h486(PQ$y1g6N2eIEaS?NQ5Lvh7?GN)FEj(rbh;3L?&bo$-*&fNH&gP zW#?~&eb3jy`F=OJj@R_s-otx&&(tA~#W6WH$LLrcvtxG-&c!)7H|OYF!^62717l%K zjEyleR>sWO8AD@v9>&%f8*5{3>^EaA%!#?NK4xGd%$+&xjn1&X4#FBVN0;F}yx$`@ z2FK&r-a#}t{)BK|so>m=iE+z?_fZf z13HFu;n)@3&>cO{6TL!ub5x&@z8wFP;Jn~_!F9Z**Y+OX%X@n7U>uIgv9*C?b3&lWUP#tu``CoGBb>ADj4g8h=XV_7vW)UUcg+LGjnGS z&80a_&N=B|{pLmi6h|3UM0J=4bI=6N!8tic=WI;+V-SX7BtF47Ou|&mz%0zcXPA#g zSd67uj#XHVHCTrY*oe*8g00ww?bv~x*oEELgS{d9IPMSQ0I&ZWVZY-wg6ny0@8P|? zr}uUYj>R!KHpl2#9k-l=b6E-J<{X{tLO6G0U@VNubWFiS7&Bux0z)wf{m=`>+L*UR zOPGrWr~`9kuF9Yo%%Qn7r-R}3#$qyNVlEb72^`~EIDY5j9G&|?9L8~+!daZhC0xNZ z+{A7CgnPJ;2Y8I9_!Tek8(!jd$X^`aAbe_m`-i;EF%sUvyNHY^AyGL-3yIEAF~W$+ z>wgpWJYNUj_j+De-pBiSU+?cY98W|9;|s@Y$L!dh$16A|=k^@W@C1)w42;KJ7@J$T z0b^$DjG?hSjgvSEV{M&UZ#!Tv%!##Rj?C2p%*70tOLOWq&4KrO2*=@=9HV1)4$jN@ z#zI^qL=q%NDx^gQWJWe*M=s<+UKBtf6hSeRKuMHFS(HP0R6r$EMwO6i9BZIvNG*=F zQ3rKH>T%SEA@w;18u0qR5uEQaxK40g@8kXCy&Z$&aa`5m7=!UT{)%ue&P!!b3eLGW zilQ(IB0r2#Zsdfq%Zf}$4`XYLlOiz^AP%foYtMSK#;h%KX721AYsmWXe%{~s8dt~b z9GqiAe1zs`g|_H`&gh07=#9P@fI;{eLopm9F&bkq4&yNqlQ1P@I>#B9iCOp*b1)b4 z@L9-wj-O)z7KSY1s4qema||ru_5W7zU5~*vgL?$`^!|=Rj>+*kPRHxG9lxB5^KyRE z;Cv^;xle%c7>iFZ3L`KK#&0kN!npQ9FLXy2bV7TyMoTn7V_3u1p>qw=j;UU|fx_aW>w@{Q&mCeC&kz*@{hAk2P3{iu7hE?u2KNlc5sWJsr(>3La4vFg z&e6Fln7eT>9>&G^7$;+9?9#(H8qd@)zA0e5lOi$9gZVHoaS#jU%eIr{clF|+5g7BT_^1Qd>xF%V=zXK!Pq?p^9kk`%-8uF2N@URqabF+PsY-i z8e3x=#M{`L19M?c%#ArRSLV#zwL(jnQ**m7WIx9PI2gv)yw>56BOG-!PS~BkW|0hG4&k%f! z!5D;r=pWLLqxzx`dWZDl*yC@y^S8Q%bmgcnA)Pq}I`aCTbm09!dtU!tJKp;zZF&EH zs}0`?{BOVO`TsNy-~0QRgZTuLuO<`2Bbq8q(Vw0 zM^Yp~A|ylt#6w)fK`g{X3`9pXL`CE<-sSZ>h=jL8B63uOFv9a%;lg;sJOY35I;=nW z`+p5&oU$XuDToG^#x@^j3?rv|0C=e-q-#iL_%td7~SI|n%@=jI%pt8;el#=uw@6JukHjFmAncE+$hI-(Q0!dM$~ zW8V+vVld2&IU0pA7>`MqiZ^(T7kGk)_!&Rp7Ovq6F5w)`;3SUWD89x4?89#C#CCjz zE!c$hScf%Oja68IhB1rRGefLRO%G!lufuY_|0L`= zz7Bhh;JU$myl2=kI3~yD7#*u)4#w|XoYP`BN9XFCox3sk62`>X7$ak4%#58e+=V^Z zkApZ2{um2mt`5uGo1lRRG-cP~(9gAafY>v^fI_6*C9E`7Xa&FG?Iex=S*i**h4Z?@w z^ft$L5e3l^3vm%2iDA4`A`LPi3$h~@-bW!6Luq_~%GiaiSdUd$f(4k38JLW57>!{V zjDF~i?&ypTXoHq$3TvhzK13bVM0Hd_C0Ik=-#RLVk|+-AsW2R80a#m(J1?xU+{g)Q zP0mf(kQL5XnM16_url)Z--Z3$*THv#Yk6IH5APM++i@6I$K==?ryO%ZI0xrryzLd| zSqA0c+$+NPS49ojU-i%cjbI#Gpf%c|6S|=%`eGo4U?j$3BBsK;&Bqs5fn*t|55&Mb z2#;61|0^Ei9)7@8e1~&5jpI0s{n(9duue8$EmpznFNOD7fY0FlKgA3v>(TFYguH*Ks(W!EkJj z(Xo!eC^!%2hh=b^ejEJyC{^a`B$qRVT$G8vc z#j)JRONI?f9LC^Z5=}a1}T31Ac;WFdmQa6whH^{y?};9N*;_ z4Y6SlCP7N1LnfGyyeNbcD2FPjjRt6j?{FGNup3*k7R#{^b1)6#F$zP_AHC28?a>NN zVQthyEmT7#e1Os@jv^=kYb7@vUp81X8DZU|K`JDNYfBO&f_3B^Vk83(RMMjO-~$hop#u z$Ow;DeEt;o@dK{n0<4E)I0&!t6*giGR$wu_*Bs2kG)%%cjK&BI#UMD=K5*RK;T$@l zJ)Bc(I6rIA8u|#0(IBKgM{CM>)Pc3DS};zk@m~ne^?ULfUeD`#{SOh`)B8IP$K$wM z_pEWp>$p3@d31yGa(>R!`3}Kw7zg7q5mPY}bMQHg-*T+R1{mL6IDn%#h4c6x=H+KR z#!Hwt^JgBDU?#?4DEgoiTB0Fpq5?{x5b_`!(jx^DA{L?|62jp%_jnPy-!B~Rz%g9I zWn93waE!-s1c$I6*2+$7!&YpB^IHdN$N8>=b6*nj1;<5L2y4pt%*SUT^Ek@b{U^bB ze$VfF9k1tgy}tJe?&YvDK@?-n?I=iz+z;UJu+^F4{PFb>A!8g9Wj-G_O4 zj#n_AZ^QT+XX9<$?ZvFf4fA4t%#-;tZ_QyId*d1I;wsMI2zFx=R$&olV+zK=I_Qrc z=!jPM2p_`hSH=e@iNeSa$Ke{08P-N>Bu8Q-KpezGG(?8A@)n%mU;l?vyNtG~?Ei32 zcY~k;Dye{Ucegas-6h@Ky=jo{knT?DP9>EN=}v)tuIm|ljlpsFJpXfEe8$GL=9=@K z_xE?lTubiLnz1%t!+U=Y`$3QJ5Z01)d>4Pg{?e`g9|E88^Pb1^+2@|$>jbXp^=*gk zv0b+B18lGDHj_v&7w^ma`wZT%_wD`L5B7)sV*l7r_Lu!;|JjcfVZYkH_H#?LhyC9V z=3y@8V@~E}Zsuo>=2_u8o>Pz$X^|8O5FKIhj$@wS4zA!FPGCQFViQ)wbI-?2OvX3_ zVKDlm7rLS&+Q7Cpg|$`})}8lK8Qxbpc#kFF{T4+b6hJ=M7s?6yCL6LM3!G;%A;Txq z^Y8!W)8qV{pZ7eT&+~eIuj5$pnqJ>_*q$P=eYVr~+HTwLeRx0Km-pv=dcWRx8+3&I zVZZc;{WKEeFd6ov{b|42znfrx+wUh}4p-pVG#_&^FLV1G=9msEFdGvv1U=CXO;8i& zUmW?672hKn5+W|5!1I3K{IBpBcX0!M!gg3kM`7D`V+U;Sdf4{mSd0aj3-4_jyw3?3 zk1?=bMqn81k3kp+`=>AZpf~KZ9wDrCb^9Lz#{@p(=RJ?-^Sqva0KA^p_4>BM_Sh~n zv7NTpcH4gM!~5~Ryg%>L`}MweU^na!`^EmTpX@LD?Jn#``||_r-)OME?RWd%9L&R9 zN}wWY!Q9Nx9ETxlD$c>*C%A#LIEO^JSh-Vl5E z``-kP@%X@JJ&)(}yq@3dcs;M{^=*gkv0b*$cG_OsZTr0s@5lS{{=84`*ZcPV?FakA zezAY-C;KZ4>_7X_{)Q_7W4lr#Eo`stw*B5met2KrpZ8fF-naK}KiD7ki~Z9D_Lu!;|JjfB=Ugm-{k#=> zaSY~Q9{2GIp+oZeoWJIm5&5wX8?X?QF&w?n1`SaKW?ukVkp@X%{?QQ*?>YaokZaxG z?;rRLwqrkbAy5k|`1c~rgY$y-Fae|BeGS0?^g$0;tDVput!EY{yBi-_#O72W6$hf;63c;7%)Hk-yE``AbvnKG(tP{#t2M7($v%} zLgN|#eFMMaAhuuy=3o-cz7IN~1?t26%c2PKAPc;9awLXz5EGFQ2Da6Ddji{k8`h!s zbPlI+0!QFITd&^#cGwpiVZW?_eY6});rwL3EyM!Mhka@N&;I|w<2>Ha`FYRd`5Zf* z-|MV`*Y*0g!}i!N+h;p%ukE(|-p6@(U*4bhc^^;V{o4=rM`YMP_LKc(zoms^$@#Px z%EA2X@0RF{zA(d|F%K)T4To?Jx9}Xj$@M4H#1F`ebod5w5FW2N?mjN#3_QmUti=+{ zhSwQ|L2&$A13#fLYQs9Pouyzc*!CRAjC4o^@9k?Og!dZ<-g{J7AK_r11c&s`TmHVm zOIYX6VBI{%BRqtCs(b%K;24khbAI0Qcs|c-9$v@mnTyxA9k$1I**@E8du_Mv_ddKI z@5}r1K6AqRE`rji0CTI0Ca}NkH~Vie>`(jE@wF7|u@i^!JFejY-ol*XA_+1eAIjn) z4q^iqViE?U3!1~+J+2_K;5#IOW8QPV=iH9_zhM4naTI%DJ=vyZaQ>f#$ry(buokSt z?(n|ape35ZTD30R8(15aV0}0jStG?^KNW&&hyCXKkqh=?wh&qQ>zpF{_@8{;V-)y| zpZ7eT&+~eIujBQ+uGcRE+he|ZY zI2?mlaTm|=0g>o$GyDpvkPU@U4zZ<1$WQCst!F%zYSo zpe-7qI?TQZa>2Sufy9W1Xb1;uz&1X>Em#K^a0W+l0KdX|*oZaoo|j+&tlycK3hQ}1 z#=`oxzlLKd?7xBN7s5X6gWl-%Klr}KdAy$;0MFz3Jg?{XI-}ury}s?RJ+{mCnUn3c z-D|N4=JhMQKkw7~^}fyR4j$np%+9f5e|-k?`xf?RZWKj%)IbxoM{kUPIWE9j?8Yfv z!&8JI)3NA@7N~~e$cB_ifJk`5vG?&OPGT=MVi{&*0=!l~IPRVA&EK(K38i73*;eO( zw6G3b1HOPY83Wd5SbXGu--g`#Q~o}Hb!=^1$7NiEeRUT0+bNvDaoCqfa0mxO*vF2I zfAaOfF^BoLpY!vc=Oj+U^Lrhy_XoVb?XW$z%l6q$vw8>HZ(T=*_hqer3Gdh3(jhDI zpa@(SE5rV3jMnIi{uqg$;TT(v&De+2xPpiH8&SyVb9{#!D21B1kMr1z)tH4*F!P^K z3uRFN8R1wrchC2c^I9jj@dvD({n(CmSPI)U1!Lj(x6R$r0oH-_SP#~w_vlc!p!QUDZJl~=mGm-7{*~L?5AZ|2j|U$I0MJjUw8rM z*J$*+IXeEbpb#pc9@?TW9J6z=4#mFbyF4UD6ujZJ+xQK8uom+%0fW&Mj&*Y{kHW}- zv`7lGkB+c-M|;fQ_E~G^;Jk0!y$5S?ExfOVn2l+efH81R7y|pk`N+EHj1FiE`>Q$Z zyT)h$`?42NXpaE!+XKI?frpXc@bUdQWsUH2r;ld%vFw$HkC z{<7WHa%Olx)^{d z?J*FOuoOG-V_Nza-ytqO^4fiz$3CouV|*;k+;ce2E5o@xCmieM?sX&LJ?(f3v$w`h z;}CYi{B8dt%)(^2UJQqI;Qe-ibAj_vQ&R!5ke|xls~T&9Ayr1*) zp2zczgy%P-iSWA9VOF+hF;-wLHem;B|6w@x&fzj{;UQk)10v85G2t4R6lswSh2VTy z9p>nG>VuJ(ieIn^X6YFG3(mn&$>}R(KvC2{8$?RScQyRIfZbRQ^X`wfsEyLd3CFlK zVdif*=UrGYN3jczb?0~6F&fraH<-O`t%-`T4$VIYoMTeK8c2wEunr=@HNaYU4eR<5 z?!vyh4*Siu;XHoFY1pU7aTJb?Lpbmc`~UIZzHc5L@8|ry=ka`=*YkUwEAYCvaSyh~ zcD;d_g{8fbVQ!8+*TZB;jZDY|^D70{N7p{rNc*P;2E%!B2F%fM^ebFDFXI8sG7A0g z*h_;vD2saNfFYQMRoIXE6IA+MYFK6u%clL4*Mi8tOxrkJ)9T5LvmOfiD7>_9-J%e zUuz`}V#7T{fc+o%x9`d0JU%`?hv)Hpt~*KLb-bR}&44V(3ENc|B~TXTRs;3W6s^%2 zyu?Bv;u##bUttU8 zU?|$51_~oB%sT>}bNm$?!*(o#H8K*t&>GfHB{(-Y*3CT$K7;iZ3a>)ievQ8ua0=en zE^NkXc+c}O1NOsMI5!PJKiE&*U`@1x{nrxB&tr-IFD8RI zI&Mn9y-@?SMPH1DV{IjN;S{dp6{3)nc{)x@pbk1API{hy_R#-1QF8N^0% zWJf8Or{mLcIuq-06fMcOJhI~}M8G3nKaV|FfvFgb4sbp%hkS61e+4rS=DJUD6KCOE zu^p>mJEvj{2Epvxqd6S+=I?!4vjt!cWQKD<3Vefvu#aNH{&EjsO@xAV@h+rKt&eB0 zk00Yfi2MA#_b+@saE!-4#dCO`H}JgX6q@UVM`U>YIB?7*g7cKQ+1@O0{1rk;lt*<~ z=iYBen4#m^N{vl;_qo3gkydkmceoF zeU8Tn41|5q1#Mx!G={ZM6IDb0L!rfyKo4IE>Ypf_`vp|A+!egU=Bj z)`#=?aqNI&d=|!_A3C53s^JIZM;4@l<2@c+FURLPjI%faYruJF3)W#J7GpkU z!G8N0*2HLx#4rqo_0b=FVSigIJ^l;d^B8%&pY!vMFVAOA&O2u1^}OzEn3?Ta1#{bq z-8h7kI0y6dzMkL>!f?;g5FcM7HJo1^la7y?Xa?8K0WizCaBS|z8QjE6L?nkq$N41DBzk8lMiVO!Q=A)MQT zVD7fN1?r*-%-$N!1?L{~PXfn&Y&ibI<6}twJm;^qa2GdW|6Rg)I3AoEtc_zh0{eP@ zh<*Is`!9UmW2`5S_j7*UjLyJ%bH4l&*KiAtxyNway@Pccfp(f*TqHzNm|te(hWA+( z)_eo}gsw10$HjCkhU0P%%}P8v7Tg~=_sI+b0>Y{ovEf^+OWyoU3m zS$+e@kaK4RG(dX{#AGbPE}X$Vgd(d1NRQ&EhaUJkBU#`U|2FGYn1o(vjIzj%B#4H$ zoXfmV!}G7e42(ce`~)*EheF7LZ{gbDSPzRg+{Z)QfZ3mfdxITV5AWZxKMRvF79%kj z&I3Kr1?|up)aV_jq=Y_?v zj~yqn&#jZ0A*S;;z}I|lHayPb7r@Ui!E$(B*P2b(2CusxN8s2y2ixbmcpvUf%+L8N zD!eZ?KqArc#P2WbzGRIYpLU|A-Z5R7GgWj;xVF- zV+tI_a*RO-m~~zxM^rrL*h|=lm6(b_aNRcVAK>*;;|oNBbNYS!0o(X1*1@@b8b-tW z>5evNf?DvNOT%$*KV(80SOc#0Umy;g2dsr~upZvipVq`n*uRh9{9u1uBX>gF=C2$_ zfqw_yyT@xD@8=w2p2zc<)dzST=guhb`te|UzJb~Kyy4oI2Swn0xHmP!W@r!Rw;>pZ z8CZf1*o#vz%SZSC=gP$R9{Ern=GhsJx0!H^9>O)eK`io0g~F(V?nsh}TEi{=eE`ca z7F|#qMUfWq5zIO6!y4HQ^L8%qde%`>RE6_@cBI0Wu&u5SuKRc3SU-V%aJ;XE*?Z3u z5rl#01#8a9V9_jvtulv5ox|U>vpSQj| zpS9-sOQ9^hZZ*_KLpc6ypY620?q5e?5@y5w=~`^Z0i4EV+`}t`rC(yC1=ZLQ8{6BOHInLw#oNJ2pH3Y+9UgO~PrehAQ$K^0PYt%XF zAWq;suE7kg=?`#TbH8ei)_qpEk1dDVFw1Tjil6ZdHpB60p3b@MlN@JhkRMg>69!;9 z*27HiAp%(@#Stt+5Za;=vcs$$*LOMgBsO6#hNCm;qYQGvyyL;^KcgMjaT<!!Z~IabVrr{xqBJ9BLc!WsgWu|#i9i8xXX68;@ za2}Ui$;PFdrk( z9)7kEQX?Kh;Xda-hdnUwS+E_h0j*FAj&W-*18lqFJTg9TPftVc&-v#h4q!X%gJqZt z^Phkq3_)LXhyB+UEzuP2J?g+du8u0O&#jO0A)E`{cLw<1f%iQ2M^r@()J8p+S2LKI z*K^HrP3#Y|8;waYzXe#14cG~5`3(NVUw97pFz#C&m+o)PvJlFnF503G#$q1K(|wWS z>IuTq?_VGTilHvLVl0+mKOC#UNJO3ou?R!a9Ho&CvGJN?%-V77XFW#`G(uT8wo@QB zg1PnsT!d@)W-P{JIH#L=YdEi0Mls}pxqF`p5gQTkkv_1dAL175pR+gt`^_*L>e&&B)JLS7!AKtg}_{I$L0Fdtp0H!o9~8I1V4< zHO$Z&c04A=cQ8lmycDXUA=<$#M`1eL+it@#Tt*a2Oh4dr za&xR!KpP~^!rI2)OW20#=!=FZf$tF)??TRTn7?Z<9Yf*RwuVZ>yqzm-Pk6kfjkoYS ztU)vP9%jM%91PczHfW5Rr~tFik8DVbl=vFfy!*OnunwI2{|@O>$Ho&pz+KqiH*p=; zUleBa}&B|qoqtu4=IoxQ;axWDka(P4I9A_>gTI?VywT@sbx^G6eyqxC%u z6X8B)1{7n^Vrci`Ik1u~&5T3`s~V>jB6Z#kqx zbUfy@lUR=_=moPbg^ciAVeo)+pM~@Ia!kQsIR7_*?J5B05!d{2_h1t8V_dyq!e=|7ttpUe>IoOY`Q}$^d*uPnj5!S``NR3pmM*fZ2_`Z7>KjYrP z&wC!v=XndFC>(p`;l8yN9D^;;4qed?!!aJy;r?eew!!*71J})mFiZDCjs@qytSAb{ zXH%G`YvW8fo}70t;VB}~|L&Wzqdc5<2Vf?);5=U8GxFSvIp~9WD2QYTkGmXm9BW|K zj_amy?B_yq#DHVlTKWyV(?(sXK9ooVgY5gyJ-DA`V)>C(Q9`hQ6VHgE-n~a%Q z0Nb+~*5_^K`_(JI0MIKRB}s&5@>?YvN8wp_cT^x96F*3a^Wk4fmu7QH)1wiuiK**oX4{u z3C#O_NV_iZ*R{dodv>tiF<{|jIDeUJ0_oiM9?ID}(31+Q}nj=wu_48DN*S)Wl64~gJ- zbl!5kGDqvW795v8v-HOpI0lwuD~{j~JcM(u`ya3~_n+{;ecksx&g1=@pZ7f0m*=(C9D80b71G1`)aT5?D2>W6!zO40=hp!kg(+AF z$K_5O!zJ9s8#v#(FLHc3FS^IA1ILeJ)VXsd_Q8G9GdNDoG%u>bF*yb+a1{3ug*>-n zGCJc&WPxM)HSb@(^ZC29CqI-@W*F^hZ0?Ls=An>$hXv%p<^g z-FkKJXYSVUF099L%*8Z}hxyxIJ>VE5&kQZENc)%-j9A?VN@Y=nluYwdoq}y}3t74fhM?ZY{gs zINsgE**Dhu8JPb*Y=?d5c(4u@!CIIF`*||#bL(O(978e->m#T#R_2JSzj!W^yVbZ}f2hgmw;wm~0^f@`Sba~DqICLA}8 zQRmDoD24jy0>`PD?#3lJ7UPmr29&{17>VVmOTM}BIbQMKzheV_MmJak`H=#Sce6f^ zomdRl>z;5ecWk=`e2*^>5wB?HZMarAzih-}n0XNT!n$n^*K+UQ`8_Y3<9$A}))T?} zqrHa`FZQg^Eroley`(v<8^%|se(Fag4Qs@z8H$}Fh}cn zJ$B(3%<>MbdFQ&gaGi5}IuDv>b2wf`!?|)Jj=_D=J2+0wv)rQk3@I0g!6f6 zxF<04B=8;`=hp9In7jSpUfuC-_V$rAKOfUE3D&@HI4+#$y2Dy%k2bI-oD-U&QHX~8 zb#9dZmhYL7$2r#goS*kRX4Vtt=DO$_<8@sh&2J%A!1nEg4BBWQ1#GJ(%e@IQATaHxP`(C~8oZ}*P!mLN53%uqJ$d06NY+Gx0aSqNYwtE34!M&Pe+{|mE zJc=MEoZFMZ`8_7AbNlE89>DxB<2>xkBRGKFuoj&6?C&*Ljg_!AmcsG$DPQwFd7Q_u z!$x=>GxNOW=J-1UYws$|?-3k}!SETy`N@5VIaA%8M{78?T@&1ko44zP?Y17>+vf;F-vsh>e3XLoZaWNt z>*-n?Mj+ET^uP13W3M6lVg|P2BK}4qa?FQkIEr5|5DiceNf8z|IradSU^F_Q2F&_9 zIIgWD=ZIt2j0G5j-e?K;Xtp~i+><-sxL146{XGe}SLb!>cqh!={+JENyV(yyFLZ?c z=YGx_s0r)9{&Z;$KYP{Hq0_K_Z$;n;d}TDQwHYQ7LE_c=wj@^Y4~j8-0L_^hhnG) z$DCu&{nK&WhjZ|ED2bLBi9$KZ8gKabZ&-t|Xp0KS1jqCnKJPmHD;B_cz^t9?i@oQw8yc12=2wrdjf2~Yqxv7AK`jp=H9FKY#m30xw}vQ3+~^{-oCQ-x555f32R^; zX2AX(568<$426B~80wFH=<_dp-S@2@j~|X8c%F%v3ilCS$Lo1r$Dws-drsgSuHi0T zAQ;xK`x57`bTCV6+&!xEt?QcO(|ON4x5Dw^7IzHk{ z^2v#+=!~DS1%F^H*|tD2Bt7w&c4!<@o(Jcr}L@!?)4KPtm#n%;1nuE2g=!CN@y(xWt*VK5e8 zKW-rsIi@yCY{3MyMOmapG(6xq=kAqoOm{*JIIpKbAZy3IUo&9?Rec{|5M zfX~^taUO@^T;x5^gY_H)bGKhy&l|(+E5m)9`R7A+*r$$v>mV8I>xB3m)`PD#-Y#Cz_C^rJ>Xc~0O#K4h)ZsnQ5D^hJ||h@CjZ`r z>F9y#$bm2MH=nzVUtx`mfb+NWfmu7QW5PDL2R{MVddIf4Hvs1Cy5W6TpYHjRA|A}# zdwvM#r{7`j*0}Tga+tk+HUT4HKlVX4bbxix9QJhs*zYw_4OL+lvPJ>|uX&7nLXY=z z4dHnld!Dx=ypGp%P7T6DI0u>G3T(!1nB#fez!SWObFI&sDd2ot7!~1s*9k)~1Bhf>zVy-P0WTlXxhK<^;z&ZkN0za-po9o_2rsk zb_d{ff5)G=g-38c3Wj6SIqDlYE_0v=%+lI!jh=AtG6T!81E=5^eFgVOUn4WzC)Gz+ zjK(k61J_meOYz7f6P$B@!YC}qNjyV*@;rk1=!=@jfp~bq`)9BY&fQ($n9c{+Y3KLH zT;nWUs~y+2p*tF(9CE|4Z4HKl>$dlC8t&7af84A2y^3>^_wG1v4EN~fUJTA(*1dg} z49;tgd!N&+0oQoP$s1S;jvMRY9&W=NtPNfNMBp`#ai8k(5AhV9$2rCN3d{K&gE3)# z)}Le1XB2DF`KUO|(Ry{=?0~*7OV=#tvJG$z{T;Vpo)PI6$Ae?E04k#ydSe0{r)GK) z&cBYaR45F`YHv8_Zp9`1jYQ;E01fd)F0$tDF)YR))Q9hXjZnD8XZBzrhQV5?j9hSD zFzY8FZLlrNF%_=)?crMS1I*j?!~2N}pVJ=UD$Lw_-wfCBxiI%pa6Rt z7A4_0v7d7y8?1*6NC)d8HJls%$@k=Ot{HyL&*z5c^Sq^Ct$983b6#x;GqffLV>Has zdi9xQ8xFxNt?4HS#XZM_<1;0z>zTXO^krvL~j``oKjDA-y6<~jS6D~xSv`;IqT_EYtTpGf^_ z4_HU8K``e+_zLes-uJl4=nCiVyhwuZaGiFZUk~R8*J{VL`!=7c)8Y$+hikUAcpR?V z&NJ)qDo40@s|p$b@fU|HMILnEhM&&aq(r_UE6lZ%@HGH~{1z{~#L~S@Gt z=E}j(JCAxk&+8a$iT3ci{V)vH;#ACsIc|dcj}vf@YnIls&o8mzUL^y}vjQ5S6NbS# za25990-nO>Rx@?aTLG@C?vLhRJDh`GBR=_LM-_C#H0;C;|4ptPQ3l_^@qCrnxBDKt zqbBmexjPgb(}&?Y?R?)8fvmIgZ`)+sUeZSQX2;-~ZB3fDbGi4`7**l(mwPoc{~GaN zeSe^D%-w#v0>`^^ykp)yyZJlUIrbO8@#4I1EjWJce`{hSM!-z|$@k=O9`EO-z`4Y_ zT7+d-1M}O3gD}H$xQ2T$N9!~yoTrk)EPdYi0q%>MqbtmF8m#}FIE9;VJ$0Xx9F9{n zZ4T$(X;_bwaI8imhvaZgt&jeghkdw>DCC$9x3LqG(E*O8O`^Z11rg642ecTC5{ zTds2%&fm@xX6^HIQ@Gx{UZ;Y4fUvM8ui!YGdzNDs#-KmU+&Rj%r#yBEW&s=&$K}ul!fYD0asVE#vUEscG1rEcxH5{2FLm@Q8KrF-| zJVYEaJ&d{NfgfSc(Qudd&EMk(p$Up36{5p2eHPm=2SeeW%&Z;Pw(WC-!DHI{2VAq= zpDn~Bcz>PXd|n-naWl6sQo=fSJqnM%>91$F3;WPLi1VENx)1iT% zm;&?M2J8PSUcmJU)wuM(bE|7k${SCt7KG$^qcD`_pAA!!O2iI%&cJ2k@!2Q`H z?#H~HgSNwc+H8!4ncEkxLDsqJk-57zIlrfeedw4sfBQ8m?Bh_d4x9(=d)IrP7azbp zWPJn#UUR&8ob}_H;pffG=K;?f9dY6HlE6KMHRv9wFwC(!++#XFb%$A6!}G8PyWrmF z8lE8>{oojNeRJ>Xvx;-CYu^}{se54OU&oUBrIaX$+UN%NOdD|yukbl}*ca=t2-D%*Zh!TG2J65%z`oCptT2m=ur~h5 z*L~mPtRFv@ALdp9W>*R3XT3Fr8CrvVF#;3e_Y028_1J^su!isBEgYZDXXcp;*1coY zId32)!+nzD)Jz}4J#G>>mfR1u#&9^N?!|S4CYx_i5DhUH=6V#*v6Ni)yoi>3r4({b1eC$7Z;mUc)OmPR-Q4 zQh7Lr`eOzhtG~fHH99$@K`FGva4g3Om}>%Z%#D{%gM(8&3l6T0u{enL$n-ooxH;zF z9Adr*4la&9*o3D@{W3VX5oY2%;=T$FE`wp%i(uq_9UR;RYw-l>-f&(l#4UXDHaNHr zX5a!6ybBJlg7G+on12Tcm&HgNLDcub!6h*i`w`(oaBvX}z;1;37#v&(eX$cCk&mYI z!Zz@cV7f6>cWl91_mAHeiqp^M= z2z&7s>7%nx#8e!|W%7+fp2g7%tMM1UBGVrcguQr=>}1*+i*OZRl4)g(#vz0w&;00t z4S0rhWZDvoa1%+$v>vA60uqpEWlX>+#3a+Q7>UD(Os2Rb>8fC^(Eb|Va#7C;~D z#7E>K)85#Q56DZVJ^zt!kC1%d@i!NlcEe`8K@KwQf(>|utYq2|Yw#2q$+Rt2;t{?l z)0S9_J4ivM4KWi}kdRDk;AfmhZ1OCNVc3tbR?3NTx+G5PJ}YObenfb|M)0$h8-C-~;lI zX-{m!d*mV09@vV%L-OsxN4J8*|1*BgcPsuzZZhqGEqI4qWZDf|@CG@_v@15^C9;ue zXROC_WG2)0ScS((OQx-`1b-nJnKs01Tt*@?t&T}JjTq!v8bh!LACZ$xJ7NXyA{lws z#?Lr~DCAiPJ+KCMk%T;}VHEb@9afR;EhHq<3K)u=c!hLi+7!;w=MjfYOJfjr;T^J) zXQlaO^`wGA)FG*oBYCORhb!4SypynRdq(yhTnz z>V_?Ni(F*d4V&>6xyZB|Hsjs@$afprzDFK1?TKx8kGy2s3)}G?dC9aVw&FeVkZE^p z#XICA)2`Tvm&i({9kB*ak)BL{!Y{as6lB^Eb8s06$+IfP<0PVyXK@U`4!lKXGHs1t za19B`voZp6^9TN&g*;nf4lW=@4Auy;J&Ew-nHL?g6gQBFOapWDe*PVbJaeHFR^dKU zl4%3Xz(vF-)5;i)qlikT#W4tb5S~o)qZhW}9deRs4{XG1WFyl~*nk(vLZ%(D4lj_E zOgmx&ULqTrcEU!yLXMDpH}Urkax!MSVaq4uwJPV>Pb|Dz~$g~f3-~;lIX-{m#TjU_q&RB(K$Uvs8u?%;Sl1v+7HvYutxRWLgQX!AB99Op9UwcHtxPl52Nt!YgDa(+*gRr^rC29k3FQk&aB;UWWrC7CwFOt|*OC(nuq!a;-~&%Ef2Rd8-jN}e?^9)}TzY_p>^xh}vZ#3R$v7=Uef zi40`wK6)N5;|nsaj4?QhNMu?B{jdXnBL|sw#yYsi%0Q-VupAHI9=ZjV;xBwhrcJR3 zckmq{G{qwPh409;ITqs{Qj=**EXPBnBhxlmg{R0wrX8>juaJ#QyJGV{^4&qU!6Etf zBir2wOQuCI2>TI{Op9SC4j>Admcj@eK~ysR0V8n)(a5wEM&JmdkY`B@!2!5;E{Xy8 z6`{y8FWg&g#w%nb)3#U!*WMIlS|8JK4spn{42EDA{)Th&PgsD<_>64JpenhJf%~kF z$WEs1umsN0?xU+>9F8F}nHE8RY{y$63hW z(Y59qicAZlKlUO#nHI-T97Gf{ErpRdf*5344x@1jvBua2ILF zv^7@ZDKe97r;vO({eRY9$8R9t!sI$AB;UwnS{gw(f!JhP5fgA0@yWCrrs5(Jk!fwr z!WDc?ru8uwSCN!V1J6d+`FAohZGgGBhOf!AE@t8nBqY=7n26uub8k6}#36(uPoIl= zV>4dB?=)Iq9-NyKkZXAi!{=mL0mHBdAK)C_9?Ri(9Vy7P9;V|G;*)75xCS3bG%_uP zVK{&YWa=JkF#H}X9GMoy0PKZ(=;9cJ{qPy7B!=P;qLS$k2*L@(CeweANlqs-(3hpu0=5f2N9J_%U}#nBQBX%!DO6ALNcw5 zS-6U%Wa@WU3ve6Xl4&z6#9gE&)0Xhr=svzD(^hcrd>?7Zv;`L94!$K*_g3?972lAl z&%IM|9`VSt0(>qygmC1UAAYCdzBw>AHzL~^2uq%M&ZJ4dG^(|VYO^N35PND_egd@{p7>YxP zN~UEn8mADKOsilrE+H|Q*2P@h#J6PH0?Y6S8AI~jz+X=PpYdzHzalJ|7K8a7MszYQ zk8$`73COfMrs5BLL#FjHAGh!=nKs7~JU}`!^}CG~@Yy#LnRbBBM$eIzOgmvc{9ZCE znRdcDJcoO$cJO+u{uC$+{>xCVbqrcJN_w~&lX8^S%-4J0Ge zMwo}2a1Y%Ceh>W@Qq$2bu^f*Ocn01;wy%(rTzg;#LX&A>_#N~?_!~o6jKLYiC)4Vf zjw?t;rj0}LeGrmwdvaa>Nxpp;ySovdOiP61dz``L_En`9>hqk|Ft?nbLtpvZ9 z{0*Oxsoz_hQ*Vac=@hT$k;k!dCTj7#{MOdDVU z?uO*smVDR!WBl%6h?#FGjKV2=MyAy;9aoTyOq*aa{B1BjnRdViyhR=|^}EU42uG&H zF$_l$gG~Kh&}ZM%h)-;ilN%tl~uRgYX};W84FX)R2HYj1q= ztPG!v{4FpVc^1b&?8H0FB3tL^_+(lMe%Emnk;$|ed`{Yf&}8Zw+!s6W5&6lr7k1zy z@{_B-CGA3JGA)dO*pEnLS`s7S?+mfXv?33YdWNNKB^nFdu*YANfWk)6y7?Ge|(DH82y`kb+E`VL6^43z_=c(H49} zL2@041BgPVes>jwz;_!zlB>T9`t17^nbw8RMmLdyOq;^reEnWB6}h&=Qapfrt5#Ts z`$$8kE#PzS9i$}FM)3CppNkTaXLU@%Da44$+(EVn;Xb+`dSffzA{)7O##*=rXC%}1 zSdAyhK&F1ru^P{inOr+!1Kz;zDZ67E{2sahx%!N>50S{UG=gvvamlm_{GI<#_#IXw z_#Nc~q$kt%*noHNH$r~{_BZ}0WLgg6a25&4v@Yi2PDs9O7`to1rT??P>dV;OgGglh z1IB!kZ!L12jT`umOk2U<_Fo_enRdrcgdtPEdme^kh()e`cQp}yx8d);wJ{q$`}(_4 zGc3VFq$AU|SOfRYS;@3BHp1_%a*(NeD}PgXgB)bq1sm}SuDuHsWEezM* zUHFLHWZDfI@B$ghw=ucS#T6tb(^~L1!wZN{rd2QrzvDA9_4mSwIEw^i>KFCM{+W40)Oj&fOO>A4(ss-J_Gm1UIf152qIg*!}^R&{f=@5{0=%Lnfe=pzac$C zHgfHOod`#!B@l$uh)1aj9>FDi*Y!IL}XeI3vd@{$+SH-;4Si#>i`^p zzb%x*c$`B*GOY{0ySj~3WZMdXXWwjO>hDJWKRF*zfJ_I#@0}x(X-SNLzX`@5Q@^(w z4fj^D$+RNI;WT2CX9bLgYp=icmWIzo2N9lZ3!*Z)j=>2;BhymwIq480lBsL(VC+Xk zGWEOYq40Z-C}ip$D+nhLn@s(!a3b78e@Ui(4?P<n-wC_H-+_HFEljRM za0LE;I(|nv8Gpdv7#d><9>L#$yTbp6!~aL87)IbE;*n`}%*2iVBi|@wS`HI%0bh}6 z11!S-!`_>R?Od*Z+oTL7L*{v&N*RhI5oIbFlQPdTMIj22p;G3^JP##PDxyM(gbb08 zIrC5?5utbA*K?g~yVmvIhSqx4`~0?dd;ZwB`~K_qyO+K1b$!p{_#DT15(?wgY|f`} z86Lsw>tOH5-ole$?Rghin>3dbYFGC4t!;b)7vUkC_J*$@Ax=FTy#*T}1x{;$@6OvG zJx=SwIQS8Kw`vFzVJ~FEX(N~je)nd*_ybDe z)f(0+utsSP%beXdu*c8@W`aFJd%*Vi&2U_SNAUUvY=Ao=ekbtrr-iK;& z^8Ksd>Ja>9!)bGv1;2pVIg1ZFt?gtB;j z9o9ws*1_vI*bjN}+V&cLYvA`C*aIDH1j!#>D`Q}cAQ-~`-*(~j@~Shp&M)9`L( zPO>CUtvP=R7vTY%`rW%2{($>&+Xb@Xv_6aj&q*0^S{KHGJ%7)^_6sM#9Sq`+xy7z_I#&sF@c<{9?+tr_vVo1Zyx`Yg9I-q*pJb0U0JhVb2+7_SwfKP1KJ(D zI=CID)}n`i{a||yzGICBdr4Vw+8CyR@6ZKs+8*rnp9RlIJ;AKbEjaZIJP5XcePC-G z_W2LNojC0P)+jH6HE45`=48!5KMCVtFXX|g84fc#=b#i`%|Ne)WH_w_V_*;D#&4TA zo?rdmj?-E&8uXhRr)^+9{0=4Y+8e%x+i+SP-T`}q*>Tzu>@A!Ib9FDna<~bnmB7ri zeSK>ibzvg>1ZJ-68_k0=;MwRUuhTJYk^sutzcH84tVah zCh2!?Z5RoEp5i?guifD@_!F#E_JCzzE&5^T11rIItjb_7X(O1atPR#F?FDDYsdeag z;RF=IYZq7ym!T|PUxRgEjU)8=tzqTHsWr;^a2879wHK@cdq~yc9oPlAar!LGgWtdm zWiMC-NpV^OM!{~#h1=FKe*56|8%T!JCt)n?g*-TI3k$&Pv)ReMU^d6>hQ0kUum^JE zv<=JybCaR=yb`}jaBA)9ZP*TGVw=DWI1ZkDUx0;h5$r4UfmM(Mr&Yjr=gnYGunvrY zonSxTcPr}}W}MB@*$TzuW`h~K5_s(iU%{;rzwgBOosVPwq<_>m`o#Dhhui&-52x+H z*#)!DrSWRE&TLLfoYsK}@Dt?6>2vTgT!1oo?FXwNIZmGhYgcBT?dx0Hu&5zR#un;bS*-5i?8z41K>%wF>49+c>YhMiZ7R*gr zd)@%%uAYHOVC|{^PR+zx+xP>_)b#<+zPI7D8oUjjjWXfXp8hnj=X)nk+k;ujGhp4S z8~AQ@2}sa}4+7{-* zuV63uWv~vN0H+nfGtzfp9=bkEfx}P`uhuvg!6hh%SF`Tx!5XDGENjq*!5%{=u!r;) zl*j8^uo=v7Gy*eNW+?B)>r3zjB*g0zFe2hNN5t%^Z8xFGaUR*!0!RL6Q>>F z6ENGZ-vMCu%509=jcH)-w=hm$1bas};j|K1d;T6W;MCgW3^)#jaB5%wQ?T#*C|(DE zXWvvftpnpBoQZ8Oq|cFRX$jIJK@Z2-ZV#oYsKhuo*tZuiwF? z@aj4E3$PYl9$tsF;5$}zcpJ7uW}G$#-=U8{LA+XrUI@-K*k|Ym&Lkznsb^sOgnJ=3 zPTPQeezVYJ@M;a~8?Z*{ERr>7bCl*}?IC>(_6VI*c>^{=8l2XT_&veT!g%cp%OZZO z;C49dfb2MJ1#$UJfm5H~i4nhd;q(Pq0#~2{UI)Pz$cWR%@Gkre&ZWEz%ON37?JW$4 zPW-8l(PtAKT@%@E$L ztZVFo%s6cblVKmckK2=Q4_-Th{lc?Q0Lf@gsz;4Ki)7CHt z%yO8Q?G9f;LY!8C;jj&?aWsQj5NcTFbgWS(!KpdBcfcM)Zk)D-58ynM!K*!fGaRXK zY6i=U-4QSY?F_@`a5GLHi}>Bn&uqAD8RNGv&+b}C8Sy)bpNHUXoH{%BDa6NV@N2d% zGftbq`*0fU{k{TUfxV*|U~geJICInnLT&OvyuJ#n;dZ=Q+ZYYIAqP%dfqCa&p*T*v z!*WP~+X`UMH@rJPh1aoQ-6}gyn}R(B>l%0Bv>nWY(=Z;lyCFMHo5FNB3U}hwcdP~A z3~33x_Jl9t7Mwl?LtzV~$7uubjC2(4#;bYgC19WMQFs&V1E<4lLzo7~pfFyoQJTZD z2K^Y=V=yOc52+>00q0blL+S%&I8xxW4on0yvIX(l2|k0Ha9TOycUzR-xj5#}`^V>V ztl!DFJp%XOwF`U>32o0Hr9iEeQO(&;2_u+^z8dFn8|Stv@d)EN%3l)ZY0?A_1(D%m}5T%)~z~#?^eG< z37ozRpTlJsh}#X25~tRpM}c(?dkwy0%>e6IcjMLE`yy~Y_+h;E1MASv20PPWpJ6ZL z!K;0w1z?}>A-wj5wU7d*Pk}Y8gJ4d_8s%d63(QeEOSlCx;KS+zoQC3fwZ`!kIB!-RM!_DiM%fOWMRL~6 z9HlwgjgS_n_6TQyJ>dKB+8xa3n87k*HwN}Ye!O-BXUzVB3K73Q@H0!q?;L*e=l%2f zUB~CMe(Og39^+?Wymo`-;A~1&7zt)4&DOPpkKiKM+kXQ#LRy?U(>)!`#hRORrffMl zlVWZ1ZP)?Uu3CcgM%Fe;;I#*=1kb+qjYhx@@NCo^?CJjuMREEf`0i{^upC}rgSB8k z-*>CGVH;$?ZwK6d2*1MvcC1!t3d zhpq{uVK?N$sdH!Zz&?X>qxO+j!|gbAu73hJuaY0H9bgey!!qk`PRAOhHRvYbtl4R> z$IuI$^|nXou7-(VkNk#+>vf|X<;9M~G{2*TKEm(VY=E&TY zwMlb11#xQaY6<)W74SL`HiLaZ&%V>)I26Wd7w~Kp53lCw-UNHTW?`*Mnq%J&d2req z=78^3Me+M2UPr-B@Exl;yazwSy?8ZK`8oIw?GA@QVBRhRPCX;dfRj)ZuP?!Juuu2| zjDVex6Q`}g8rFFzgI8;m&Z?xrX?>Uu_89KPtFyo>!5(2P7z;mv84fc#X0XiIJqE+U zjJq=``uzz0jQD*Ew_6|+ew)E;;LrQV=W{>)xxR(eIIR!*Jqh>WRlh4CDNbv_crY7l zcG7H}bKMDWYIei9vL7J_PFsV$!O%Ohx3B?HUWIQU z)SxHf)|{+4=&rCF+|^JGM!{alhu03U7~w*(x(lzJz&VBlIIRNCnVI3pjo0?D5H5om*}>op zLpGc~3m?EA5x)a)`yFJ&X;bj|e z#5k=EW;gbMb4MM)xrKN*wRdDLb{k~FX-k*~=B`TN)m+XxxC5upz+^ZA_I*3UXAmE! zo_*~beGeINYG1*#kv)Cu&RyX%@ZGsQyagK|HEy4VyYc!wEP_j5p5ry}9Xb_GpN5HG z9oo#hXC(U!e?fWvvFsyx2F`@jrZ5YvaXf(6-mn^yJH!wQrOQr_aKC_#MpCS$F;#ZpCSJuqU_!vf;Kl zWXEX>m<^}le!Th)y#f;Bv?^GKw$~rdNb_-f4(ucKhi@SjPV0d+j^j`iuP=c$EO$0I zFKUf)KNP^LHE3te>@hqB?$g-~x$xQ!>;Yc}d;DfN%;;ppX)E{u+_O?P;&%%__1her z@jeIcNO~PML56GiO&jt1E5PC~U=6xGn4`Q5&WhSYa@N}(p*`RQV2{5XUI&61 z9Wz*F?95P}kN9=ZnSL|jv>D8S-y?os!|g^$e;vP#@EZI+fLC{K=-1g${muYq7mDN6 zZ0t8+w(c320>_~!UhVC>bHkkswO~BhJ1U6N&R}lxW}H?5XHs^8xtz9OZo}G@v)r%4 zCP;_VhG1XN%#~+f`$pzro#D2x;MvGLompq|R5oVEaaz~`V0UY!$lmt97jHiJ3f9-W8qs^5)}9;Z#f8UEkE9a#OZ;kQ4Z%^M&c zZW~AWEs58@;Ou@HoHm4+;Ov6g_E%vIq{3-Em=4Z`n%!`&>}#<1>)e99!J|+ZujXRE zfW$bp_B;m6UFFBAyEZ<7csR9oWncdXFf-W#%+%QzEQwe1&SrAV!#)Y)z_U?)oIVF1 z!v!dV+x~F*7itrD^&NT>q{C?gunujn-!qbZh85tFkN9r@{pTV4a=mFn>(|TZr;}^Ieuf1Un+=0{jFau7(19*KE);?zEpXJE5f58(Axu*PAQvM$)? zw}xdF+IhmSAvsR#fU_#cpeSCu!%9eo(>gE(>=8Q4Zx47C*yA_DF$K)%n8E4^t04tW z>%uf}hQS$?-rx*jDxB7bnQ$uNw-0{5jXb|``Q6B8w0@hyY|w9MyuJ?KfwTL~z}Z3l zK8)7^uo*Jpv;~;0I}Z=z)ohM)gU*%N+n*cpTN$xa?dofA2a>goiEs$+#%X8p?3)0mkHJu|ui)9JCCq{E@tX;!O~GFOuV9~{ z53B+ENOfTf*e7%ksWlE~Mw8;yKEE|Av(We8wJW%jBMDB;VOfJd1kRdu1#`0Ykeme` z4~L-;UhM(f!SR= z$1}{I{m<{;jdA!bgV#4;Gh~YRb$0L%a4(^=3*SQ)oVJ1wz-*n_tAVgJ;@7z{d;89% zJc8GO@I7R~slB85a30FybpTj<&WO`yFdNLYm%?j5uy&Oirww2lSlhS{uie2N44!@M z8;t=o8~O3-*=QlG#cxWSdPbT8$Dk-)Ujq9`x8k%0jD?@T8b@bvzny)4YglHX_dx-? zz5t)Y%{Z+JqhKH0iPuhG4=Eu|tHBs>-`PEQ?E=doF;2~Jn9(@|h4A_ktbp5b`V>q8 zXH@jt6PyuEfz!G$J<9K^QGOfZH7>syqMl!OKkIh@Y=O);Z3+5yw(JqS4ul^d8&2J; z@*!M`_%(ZFHYW#8Lw3X7zP;ZeupM&X)ZFt&a0$xebr6`V%Z5{PSI#V405k0aU^8UG zX)|~q?E6~V=mYB@B~HWH_ZYty#%ott24CVgF-~j1XxIk@@ah?O37EyQPxvkHtVnHdiKjsfSq z@5XCa_yTUlsTp_u9)%)!b%s#C$?@tAj%n~S`26;UwUCZK^;&+L;dL(P_hG!g3EzXe zH(J4hh+p?k4uSIig+Cs@oP4xJuHHFIIRrBU>D@U>2vT2*gL9>*Wuuv zNoS6%JuieS5Nea#@tX~&&w{hQ7vN#MTHA1r+rD5E@a${fs03bn!D?8H-*`B!1Vdpv zcm{3_AA))JP~+H&->f*b&%XeiH+uxH)+o0^HoTg{a;J{7NagW57`B5wBy-RU!CCK0 zcpVPAz#hLD4*e#;=@T#t%-G$D*Un%@HVIB^!gx3g&Y0HsFbsuI+h~Q?58yo5H+mg5!hHOm zhq8Em1HJ?E&`n`BSmUq{Y?g8pSi@=p<~ZCxYK_u)(M^yMr_I1wBzp{H@%kp%L&}2F zRep=DUU2UBg@|9X8`Z$xzH=#e;k65xYfp^R8ZZ_PfVoNM_082K#;Lh0XBK_} zYm=SebFg-0U*CC@-H;Eb);8=5{sk5B>e+W2yo=k@Py(;7!dkckr{>+QakwALEM;%7 zhLswp<~U};DR=;{eZU%Y8k{0@CTH`t3Cc7AUj^$fODem zIx~Z1#?B0-dpJ76r*KQeuX_p)z}G^Z zWxFDNJL2|JxEZHaBYyYs)9h6jFq`AtpxKRaU~j(=UhVy^0CVlN!I@BdM@8`ZGT2)% zS63U%T{*K*2(Q*Azl7UxS`(~Y9fX27bzWf^B*bY|cn9{tB;0xiE`ryW;VW?FtTwoN z#VlnJyjsIr1?D)cQJT{^4#n_l4SF@C#Hl?7_ci6K9pl$K7{0l|=XXBOuFvlyJhy{j z8|1)gTX1$i9!{MNeH)xTayG@;1+(pDpR2=I2!738g=|i3yqeuO3PthiT#CKlWH@~q zroaiXcjO+?)nIR-E=&b$&(18o0;?efPTiw26|7y|i`O1t-`CnkZI}RuU^H&`!<~5T z1lBli!D)3E1N*^wq%L3<+8U)f9d{QVg2H%x3BCe*3{QhSq!Zxo{$8*K+&O9w_+7Ba z{}67?aBKoII{KXt7r;6G!C*!%nw53Bz&w zBjmzsJFpKN52ux3IP8YJc-RAj346gk-_G#6qd~v5V*K{R z@#@I)TmLHGznO3w{Cc;>qj=Tt4#i&Q-UD;(_6D5^-3aM%+7#?9oP~$*>b&oFkO8OWa;#07 zc`l7tGqKjL%uF_c_u)5q6Svklvf#BPd;k~V5xhE&WR4@$C>P@Q3Rr_44!gh}gE`92 zz#dX{7zc;o9=vvgui$o^+T))JW;lxD)r^jFqTZ?EojGRg%uoiuTk&hgy)`(aas`|* zbB1sa6u@hzh~LC9ev9Ju-|<@>uR|h!bL029$n%>3r_Oed0%r%EEpzrrzs@e`*X*;| z*n9D6cJdpz1E*%MW`fzAl6W<{;oMO=oHl{k@H>>j>i}>jG&4@^9nFK#TNsF2=Y6d` zKMM=M+NASj17Ry<#c3;;4;P>>Zr4LPoHhn)SZAR$UI)NtFsIWJtU>7x)5h#c6FY!*LwU=)3}Jz>Hn2-#2h;#yty8pM{U$GE~Cr zaM%s`!$(A=|k8`(Sh6P(w#w_vU=Cr;af zwdcQ}5?-xM?u6VpZ4Zm!FX)Ne)nE;)9+;Ii$6<}KADGi|N3=EQIdC5AF}w-xOtOb$ zPWEH4M_383Z-YHx_hH%NUk2{ts0kC`2skI&16D`;nxS-0gZFKifp(9Mdy?FP)e08C zC8&tkVc?8+tlt`V)$d^_67l;rKhxpW&*vNUI~2D&!QFKo!QDCT-lzu7b{_=&b^~XR zZpZJ_Fco}$qy0L!-w51kb{0zG_6^tqW;dKWS^)O;AH}P^Uvus50k$`|2+YMk2IeMr zfw{Vl@F`e(t^($A_JFyK7vM9vIVD+c+^&G6IDHBx!*M8vSLX@cZD$UvA-oIr7#_mw zYhVw_S;EjGbQZWgUWdX?u*csK+{JMVPOHOMI0S|9>aH_)*_olN3p2nBbVh$Fh^MeuYF+yq{nG9a2D7e@FRE~1lu7OUd?bU1~WSD8yyV?z>HltFhiLPr_X?U zILttsad%IWGb$Ny+8pM?MR*ji&UpU_dGM-VcO*soJ%sDRxV;g-nQ`jpvwpqXO25Nk zSJd;X--I}=0pq~g?!tKO4yzyqPV2%9I0Yr~+7C8D2Annnvy;wso2?rJX0MzZG@G*s z%x*jeBfwpv`SEJ+*Iau-oZ1^43kTpHymo^XU~i!|OoSs~roB6?f@Jt@k6UY$H{rAj zn04O|=CED_=Yem-X>D+~toz-I<2CdM)8N#cyL+L32YdW)!B)tQSLe*k=r~7c2J0O# zW9OW=8Ojx4M%FzXv3}R%H$6_9!92JS@jDo|?r3laM~5iC?x1u>aJ1i5kdjoSSf31>DK7AMV1da|_GCTzf4r_k0A*#o9Ys3C?pr15@Dy6vwNz=Wietep}=A zBe(+Apx*}PMMIBaDSpjS)&g^~N5CFoPxuDx0XKxu<1d3(GaPaGt&CSQcFuXJRjD9vDf2$!J}Uf+g2Pynx;!3?x}R-S}O za2$%^wKq6p<_uvIm;>jb99{=q!|xb8AA~}<{ZIV5+uPl-!LPfoYDN4W<>$S4b+-Fj zew|&&g41Wg?6W&3&BhLgU62p2&x3QJ32|B-%;p@5^1Bkh_V&I1CiH%t34Imp4W_|q zL$G&r1|Gtzy@l@}6MmcEbvB#>dkoG4Zv%Ts?ZKREe4JK+QD6`FZoGB_d;H08`V35i zU*G|}x@*Nb!i+d=0cPy}1T&Pwz>I7@yuJWS!Ck@5FieDFa35ZK!CG+7Xk(ZS=fE8e z17RCPe?Euj_j-Q$pYt#M1wWtTdVYi7PB>l$!S6)e9*gq32FIzd^8D&ID^6R(#}U8o zoHQG|2h2`(0<(3Aaat25z!4~d*B)SYBROvCf^!S*%W+p-KXBhh=nXnkb`IwEAUJcL(!NL#?YcJ>IJB{T@H3Ras~Ifs%`s!w z9K3VmB6w%cP}m81@oL8Xb4Y~Kn&2+?qu`9$E3gJq1`!lQ0p?UfqjVvpK85 zxg)b1?}GQ=*xPsJ*Iau>oVI{@U@o>CUI&4_qa66Hi`N-o59vX?z6SOPGvm}8v_0U< zPzkU0`1gQ2Da~*!hg)%48@xmIB;1cz=S0oexeLn-(-_$|tF+Y`Ql zR5*2p-yIES;s1o+<@mid^87~o?Tur1GdQ~+{GQ|YvUnXB@tXs$?IM2TsnBA-nNCew|xr4IhI0HY(wDIP8La_^plC$#4RS zO2&Xl{jNMVVm*=(@tOe)z8-p3>b2s4E8MDJs z1g|}=;#P3i1 zygTA|1wU`csdM{N;pd3oK2d&~;B^k1kNABHx6bVAw+$?WcsP9wM!;SufZtkpoe0O^ zKD?T9x5pp+&c*Q`@CaT9!w$%e*ADP$l;82VJp#^ozXEH(40NL?zs@iW1ZPy@dVc>M zzin~sj^O|1=W|?si}T#NJ10E9!S8t-m&5BI2!7k+b}=M~_;oMRfr#I3xHa4EzILVw;WyvMf~Q#YdbKzp2Ldf!9D9 zoHhnCI_IG5Rs5QvtPbPAjI4gmK)b6jRm89NG5ih>NBPakXR|YAi@_PfFe7@1-<|Qk z(dYMnvS0U#IvX18w<=zpJ<{(zczr3#?=yIHFN1y`z^mEV4Uhr1-jgy9%+>|JX0LKZ z`Mn9JPelCg=Vw9uR>$i&I1FYu+^6#mILjaF*EvG>6*?yxp5Iu%Mey1q%5Ouw&WiH; z7LI>_>-lv@LtNj#v7gV;eyjYK-mh-t`EAN`>+X%{=QsFu?__v>gJ1Vv1;6giNsiOt z_f(YMjrh$N@jE}__ffnKf#BEd)e>;-$ZXDNI1uCaP{eN!+^&xJZHVJpV8+hpH`=cm z*~Jk2j*UFO!LNH(e13!9vytaF_|0*h=U2a9L+tz2_56hl|y-?*ON|K#&IykCXS=kR_NKA->p_Wc{z`*Zkw zj_dvEdf&f(KKp(ZKA(NRitF<^+Hd%Lj`i#NRqz|v_iuQAj{W}id=$Qa!~3(J&wuCr zIl8_PcIf<_^KbO~)qksgb+zX=uJdp7^BdRs$nW29J__$w;d~U{ucF_d^&8%wulIfx z{6>HO{@wTI*z<3+-*EobZ}j=s^O5z9|JM2Uzg?e<_ItJZRd{~ms&DxGM%OojU+Wv! zJO76Bk>}s5osYuzZ|wWC?^pT_=ij2dTg7$$z0vwcs9%Np#(z@3x>|iQyg&a>s6U72 zH@f~D?brHqs80sJp*|T`{mS}As9#y%uznTl8`1Tv=<{!MeIu^(Z%zD80B0C(q<(d+ z`lR(|XS_pwGP=KTt@?9pe<8O1Z2##0M1R5lZ|D!k_7|f2N6~)mA6>1#VEsAt7ef8n z-5a6)Y<<%HLR|Id;5XEtLwz#%4fV;``jz!bXHX#Xhm2SfkJ{@}IxNBXsY6#5IHe{{9}La0AGTNYb?zE=Op zyG1_$XGHBE{hj{cdY&~J2pBlwLgpA-7Op}%kc zH@d$cSO0esZtefxhhO`@p+9K;pt{KmEXM&~EP`PckpbpFcxM#x_Uzvk<%m(MZ35uMKoe!X|$ zYWa<$_$`Xto)G%`_3<0q-?#r8`uooQ+W#$&*U&UC(e zPl0=&`@<&4#Buqwr%rjb{A9@2{qM?O#ns;r^9vy#8=Idre--kR`i;#` zUeB-jx>jKR>Jpg08U}IYuWI1c{FQeexjUyjn7>Mn+qm*M<~PjeG=q8IY z{5HhvEVx$w${h{vp&SG58NFJ*&byFG;q`Uc3|VmcEPMo4;P2)qLq0aH{A6rCHsr6O z^Rcn{y6gE3`Sxq&pY?10`S0Xo%};&+<|oVJ*8Jp;5SyQ@hS#|AlObO>1Ldwu4@r?|U4Jx-lZnG5H^ z`32`w;+kK0oafiwgnPl=b)8`u#OB+J;?;b+dx695oEi9aujm6jxBXxPq{nX?yemkgi%)@P%Pw}qF9grKZ9bgIEjNfVy=8p>T zy&FA$R1dFV{^&uT+t(n>mw5+Lm_MpOF30(@Fn{EHXk31sFN}SruvGZl_Zg;*c%pbjp-{|=x?>)K#mGJ8Pk-I0IKf2a@ig!7?`|5U_ z)&chdhxvuxxLpg*FLcE161W+s)nF_{&mVowKS)ZPI)5|+PQ!zEz1n=4J4T0t^MiTu z`aFCN&JVf+dOSEkSOmXu%@3ZzbC@68jN>pr_#uwN{Gjupe|LVc9-qt34_?o&^WCxY zzs`p`|NHOGhdMtv3x0zzKe!pkSDPPnzAQI>pM$vO%kILf^JUAy`J-A8J%7{#w_igF z{C2|aGH_4ulQ0q7aTewW*YFRL8mG<=x})$cl)-P956#MR8$BOd8LuN?4-~*_XIKVF zaOypTH#-08{J!(QS@9a?f1U4k{@3~Le|P>j)^A3>ccY)**!lf1-|hT~#Ps23$1!Cuao8fgHT!3=;9R#uS-8bRX`R-A$9}42NE4afwDQ=&F zDR2V*&iuan8Jyq$3!L8{0XH(ge59x8*6x`i@4j#tqoA5oj`#G-p?jP}+8?WvbT@3N@`WTFaJ>dLr zCs+yz@%svH*FY+qHiY-!43y?S^w%NG@3+G10=NVfa61gbeuj=b!*T6rD2!M4Z>)sm zIDH1D!7osP&u8~;Ag9 z_UE{NBka$)+J2S9IIRiZJ$M)jnGq2 z=*Qt5*b8^!wlge)uwP{&&#w1OdIv*K@Qw}ltJH^g!TmW8;wdH7{Yd(a-Cw6)_lt)8ld=2j z{*(P?`uzfK#p#m}_Fom@b2;q4N{-WKU>ckR?}c^$)fz~J+XfKVex!6bZ4B?jSty0y z*I^T6z-cp>3+KT7NIM}9PCLL7aL0F57zNS$ufD+VZ8&`rCc#lCiq{_SHKgR34g1Yb z@q5^B<{d~GaM~2+z&R+3+qYmVWaFJVcE4Hd{<_b2j^o;2*A2H<+g}&$x4{kgb-(D< z_D{yOpRfbp!Al@|KjA)}-#hWz8J0mJ+}40`5WBxFDNe)wx?}ua6t6wtYeY+r;n#bQ8o*3A4emGl3GTwHJKUE;61+YM6Cmub>w()< z;2j&!z%&SVsJx2Xbr8Lu@HhNEgxA;MJMa#(7BC+!g8L^2!*=*P`zLGg?7DyQ5V&9T zKiNOIG3NP=-5;Bk&*kgw7ySgk@p0<@$q}#z^5Zq^7rh0qRbe#z1l~Q^1-zTi{gX9e zJRE{Tcy<4z`w4HuX>FJYN1+H_Uxu$BDNgIaBsdC1@aq1$Rges)bzm|a1AD*DrkLF@ zd*vMrDe(FXOoyMLI9}a9xfa|%*$Cc)-=Gv;`@=@afYaFhq62Ze6|&*9HGBk@p(0*~ z!A=PKV;A!r-^6!s74Q!4|8#%s_4ZHdH$Bg6*xw%Q*Zt2~@O!=ezzgyF7gWOQaM%Uz z2iEUmxCy83j~xkn!Tqt`E$RN)TX0$xMuT_L-G$fAunZF6v>J?opTPa1FTyfNgwyIU z77ju|ymoJ+4cY8;t;Q_olI~ZomoLw;c{2P?Q>+7%yGU2p2%!TvtFkat; z?;$IGTf+kQ6Wk9x1a?3!oVJHW5D%x`5jz6*Kt8;N{lKyNp9}K2{CD?1$F(0guFvNU zF@D|8ANG5@zx`VKy|d%AHH7`%74RDNd+WCyECTm?$L@dbfZN63{%7x&909u_A6`4c z5{PR*a0lFe3JGxfIE;X>Kei)oondeW$sYgP;0)nB+=l(YZ{j!1c02oPZ{OKaXUjr% zG8;}?!$)ulD&lo0?1Wr6Z3of&y+`17H{``@NB9(O#%WdXj_3X09p0T`8N{`}y&H~i zbbovFesA};NALHJe!uF+KSl8C{Tk8tFSwuI`xi3t{CfXFT>JU6;>(|}?_c0<6?+Ri!QC6tyBT8lw>x{}Y>KlBkHZMq1Nm|LJjCwj-;ZDS^LK$|kO-$W zU@RO2_w$GQ7jETq`AP8pg(L8H?_ZeCyK}gI;adCogJ18rioHMOb=+=(tKF}$h5A)y zyoUQV{^0krczp}L2k+Mi_b*)F_vrfukya@P3UKVL2qhY4rUXh49)9z69^rcoHVU5h#M+m%;lr!u=_eaqIml_wwv|f6CY3 z{VC6Y_oti$?@#Fk-k)*@UcEoX`!&2jCH8)ehWuw7eg8tVU+*sq_aBA(alHTNYWL%O zhu@4iZ3c7UcPN8b@5i~`{VADn+8pMcF{;b$m@*Ph@GEO!&8z-hQ&!`*fF;1=3sSP9ATTL&h?F}N46Jz*6j$LZ5B1x`Y^|7bOSQ{dG5alHTNBoxPM zF9`P^)x+xy@P3?d|B-&f{YNu#8-4##^!+%oe%JFE9qy+Ke#8A%8}J*~{Z_Bxb|ZNI zQ4{d~qu=0q_aF7=-D(4*#c5-h1;0W`y!M4}Aq`H${W#8;nUOVPX9g=JPR(%G<39nR zN4OHtNpV^m#zUwFhqQPNy9wpE34EO~~;MM!ByuU0JP8+~XI0X;jwhyd>)HrPb;r=rH z_JeQ1`^y?axWB9<-@UQ-TgBx!Ezhp^O9sDj-A^}*?_lqzdl0YvU_GS8X(MtBTku*1tU>RFJa}yn3t0NWJa8N zf0?`M&Vswm+E`SP!q0!TSe`;k74x4asm?2PVTYD1z6Q!5t0mSn+p4dYa5;5Gq85$9o{3r+GHM_J_lj*-PWK zKdgr|cnv$;-J4SkuhuqJf_E>3+SNgRzZStkv%Q5qkPoN!jy{EWIJNg{Z~sSd zw`kZsd70ncedX>PcW-3JsrSP!fQ#@5Uf%-mhxPv7mM|awfU;2tv-S0e|&t~ucEr!=uU^OJiYaQ_Z-(wJa z|L>D{4fp@vjn@}p86?DMHE@Sc*l`wpKdc#!A+Q~?uh=teLT zeuiRr4K=K!_^k!jI6V87!K-_(+>7MijRr6itX&nu>nmVw@^+j)1ry*f*jw=aUwcQ1 zaO&=Md%y1XcK5T{4YN6BuguoDo6y~L?pAU4#zMFP74bS8wnGk_dcX6B@FzTiSMPV; z3R!X566V7n5x)a)d!zR|N8c|Q?Y9A5XFy!{v)9F|_jjLwd-2)>Rzgyo)&}qIJ`BhdM@$8(LHI88rAE%XJD7cp}J5H@_xEIN}Q13qK4;vs2P8+~Xu=ZRG zuil-q3hW)#277~t!QK2X!g4U1A@+Xu^nCX=g}D5t#%r|S;=DKag0CStPV2xFI0i-V z`Z9b8x8bxFjDv%47vH^|;d8ht;&&uJe}p`Eb%xL#cJ82b$EX=fGgxMHn!#LfN54H_ z?_l^AQscBfOa*%k-rsEv`b!8k%5Vo#9=x^#=NK+PS=_pZGAT}Lfw`;wPynay6?N~V zVI}8M2E$g!g432@ZPL4~tUdRGaQ9JtyiN!2#<>r#J;2_+cQ3e`-`(wp;BMTy8+aKw zTNZZb?B)0TIPC;WAU;kj!*JLMxp3MRK7vb79wy7 zKl!u&(f{qs=kq$yZ+(~sC!rW#dq(^w!>N8J!VxHh*KY6yB*tlV7!CWN0A8PmPar-{ zE5i`j0nQLQqp|?Z$UcnM0T6cdH^%FGa0>3nt35({NZ}5o@whz*)}Wtt1hjljdUG8#@a2KwkcF+QY|SZL&OW--0cW z38(fJX2TgMiPwH$@AnRz)&=j@@a_e7^P8P?x3{wk?iLNR-FM-&Gc1FfaasjN!fwcm z)Aq0ku0RF64vq5rEM7l=KO%ky;P$)g_)Ue=dQpDw!)w=_8geY@$9<}%uPNG);7$w z--AXIdR$s>@8dX@1}bLyt~ZauXkI$2dChEy!L|CkQ}#9!z6I` zq`R-W!g5HA)9Nr9_CbD}J`al_9$qWMFxUY(aM}hIzy&CW*EeA^WW2`n8}0W5o{L8O ze#Ot*a9Rt-!U4DoubtpCNPyGF;BD9mIdR$sK7515+6=TAO7GY(gXJALuYh-4 z-Hy{*V2|(s+=TaZ!a2rl*!8kYocj2`Yd=3e4`Z$b$osbKs?cgK$ z6CTCuTd)N(#o@OP|9orV4xHA7DR4aE_a)q}fLn1|1IEBUD1g`JVF~;NmGC+gwm~+W zwt{)!jJG>voni0}hSWGUV`m1-j80L!z64)DBAix(kzfzWJ4D+;s6m&;t2IjJyqyyb zH7xhIyQklrtoLk~vonV^5H>@4+%^RJg67b0VBpg^{oea^tiu*!#7&Uk)~ z!9+L=_u#b)EQLfktqLPy7vzrkUC7T%arkY9W50j-Q~&Y(8<*c>_$`cA{eA(7aas*V z!d}RS)8}9jT!Hd<9R%M)W}G$ycgVV<-x&t)$T1_E9H(`_jE)%&d;Io*KZTocS{a7G z50C}7&A}S<8L&p_9$DwitYOuF(GY4JZE@>8NS=X9;k7TU2K)S;k;>zBAh;9Qos*u8 z?CT$gLOAv8`#Ic%)5>6N;|DPJ+zj3a=gQps+zY;j+j06NOn?Jm@92404EBEQ?GFX- zmJD~(&B3jATRFSn-5T!pcDGqwm;%S3FkZXC7ZJbJ@H!gyLSCFc2l~AX6?kq3!uOCF zr&sfv5~sDn=l5vDZ&%zdhlDt-8u7b}pSf|`4nBfEp&VWZz;}=yr;Wio7`#Kp473?p zGnBXCv?h#!ePECOIammnz#h^7u*Z-Vr`Dj|b7qawJ>Oly8kYB{n1dby-gA{1r%fT8 zf$zm@SFq3T8OdF;HSy}58qOTKv%NKVHnOi@60f~rHF);*-oeqZ7xLh=9efBEzlG=tKDR-Puh0QMd=EonZ-h zHY$(TH^JOSIQyErGFNA9!(6PnXYVbm1kN2Zev*0w`kJndV6(q&!Q!oL3f;(~gJS>5DIIRf!{QO^R@Hr%i^1Geiv*WZC%!A*d6khv+JKViP#XGLdxSN4C zBWuQP3B<=~B^V6fLuQ;d0eeWNz#c;numbBK>q@BSSE-Yx0fbl!dB z-3w>nLA>^XZy=D+7X<(0`5MHf8e+Kb! z>OEJ^saV6xfLC+S)8QBt!fO{Wr*j1!#jAb(a7KC>uj64K*hlJy*UurmJHLh7jo{3Y zGi9EQoay$?1#=q<;Ubv3^4@N18+YLJX|Q(Xy^?q0)Y_zTq1K)s#p|2!9oRc+2-Csd zZ&AFO-B=E0ubu$2vEKcg1EQ97q$4a*#KN}N6g);RV-UYvRc{s7#iW1rty zV9!X2acZB?SqA$^W}*Az)_u>(aO%7BJFpXS;Isw251x&j`F#nNgJ<8zz}(eVu(r_{ zW`eaV=N7twb7eQ<)Y|hfu(yx}r}mEChhHJ|_RVhGhSQob8umaQoVEjJ2ff?M-Tbe^ zdPt4adN37^K_R@p2utB+oIW1$yPcoecy?RDT;R|B$LI5_=!^C{KH~RIymo|7;0lz( z>szoH(&Mxd%z%^N48L~}dWUF2yqb}H8@59>oSM;@182Y<@GI~Y+=|oc@DA((?=foy z)+n7*DS=mOSmtCC zoVst)cV{!t1##LD7J_$%mcgrc(mB&@Zo{+hXfStWuFl-#JU9#1uAE!&-e2d+YQQM) z-XnVpZQuh4y}`b?wYP6J=P8%~2cQ5>JHjIH?ojWxayS2XkPfE}VFsLlB6#fv%ON37 zt3>?n;Ai$Izx;Xs^t&3z$?)p)I}Y|kew=oIg>Wh2cK~iTLRy^GhpBK3yo1mgGw<*= z<8DUQj9tiJHNoqWl@2Av6~)+n7*IRS<7+66uXYaHca0GQM9 zE*0-Wvd_OC>;pTCG#7pa`$#XrQZP&DeUb+uA5Pmscy}&=Q#0*ffPKMmHrm4P&U7~f z&%W*?EXY6F3-AeCf^v8@*S--_;ncY@=RyyH_m+8Y$|AS~J?M=naq>r_QNN1Z!CN@!B4&ah!)zcI=`*O_0ntA>foLO)t#hIhskPD}+z}&`ZFqiW(gxbbqc(rzAu0118 ztxe7V=R%9%)ZW79;9mYpFc|FpX27Z04YOA#!0e>i*k#~sig$N=cc{Aw_1g?)!6_(? z*B-DU;zqMdA?1j8IZ3heB4=9b-ey|o&;PfeQ$I4HT zAE(d3$6!YGVZ55LTL&p|S{p(SI3G^!A-PA#Jq-`xwGXTY=TvHdHLRVG9jE3f-vf7z zdKZ#s;LpIjVjl(jz@Cxp6V?R#NbdXg9l8mawX+WGyHz}#IxqMdtbwFBH8bfxvu$9e zy%B`7Q9+z~XZJ!dw^0VSeZif;);7#djsj~}IdR$woI7%E(AslPSOMN!X79+};1002 z-yFG}#tpV>u{N}{#vvK(Sihr(K zaauFtcQ-$C%&bv5$6(Ig8rBv_kJI|#uEIm$8Q59=58)h?#A{FZ5^lz+eWbU* zcWC!29n`4^p5uMdv2WC+c&#$7E0o^H>`r&aO&*QC~!A} zyK|m}d2kj=;>jKpFcVHdA-r~mC1B3o8rJKu4wB>4yUg5`;C&kp;?;NP6>tkqz0dni-NFw1x~tB2tI2Q>^5V1&%z^OkY~RRz z&sU%|1lFJn;Iut_2sEK-)OiKpt$u}j@!A!ZKzMg{Uygkv?<=bUo{jALrsE&19!!D*U@pg< zNY*xfhX?Tb3akL<7OH@?XXiq#J=9z`NNy zz(?>0l*VgcSQGJE8?R$vFXYB)n}}cjq<{GB73H@&UPpj_v*WZm%!boY47c6E9njv< z{um5_tzgF8jO=tUgLMyHJAplZd%)ha;XT0ikW%3ENw5aJ3!GDF3Gc%%V2<)d_!Qiw zQx31MfoEX*{LY$rM%o4$aq2$GN$?Z+4sF(M7959yc(qRHJJxypK7dnu`o3FPm-Mcb zzOV|C;Pi1A1ZHCG8<}ahFX-7Q2TohSEb#1Ggnz&;VD8G=hPg>|vDU7V;j|Wvf?bdU zr_TM(2KQdwhf{ll_VyFtv=R&gvvuikS|6ssVQ_ZeySu&H>Q8tWul->iq`;|u$HG3y za~;3cB7S%9Gh4*(EPnn1Mey1UmcmUqtq23b83r@Z^7K_Yxsh5=wt-*>C3Fcda}^Po?I@6PVqFcWL$c?P)8+q02<-;dxNc=qiH z?o_!2r>b$~d<9lQBAlAt7zSpq%uY6j>2MtG!Ku5i zyqogF7_CsF$wuJ@2pY<=#ZxWnVh2gLbvf#8SybC8Ie!XMT9hA;^ zKZ@4@umQ}-nxPyI`ydZaTZ27ddxZ9ox`RE21URh#Z^1?|=WY&q9GIiD#?caH!AbB8 z+zA%J1@MeypYRJX3vE``cj&d?Jg{}>q2SI@vmCx-jRbpwb-r285V(AS>K_1z%nq4^)NV(WS!D?tny$T z%eqD@co(c&<;Q7ww>ru1W^$ef=RwWX`R?q#4f{q{z`meoBm2J2lvRPDum#fM)ZCT1 zI&+hGaM~K?z-h2HY3JO=j!Zah z1ojAzfITGVygz{7pd?QRxv(L^hID6Cs+`Zv$(aJCwHbYvR)`LlKFygmEjNexx?^lU%`gp|e7Jg>H zX#1b< z7qt#;R%ZwJj`a+TgPq_^uz5%w^04cTzo z1g3#~LC;3cq=3mU0Iu~3}zoMRW%q6KR_m&HiD^e`2Xqa&cl4H_dkx?MwUb(OCcoL_bqGI zBAt*mONoleGBma^V;LMab-b>b`~JL_&-b2uSCH+W<=d28C*y16Ce!wa$B*!iQSVq;gcMXJ*LUFzr89QU z=s3f%2M>~|J;L{K82QMw6XxI~oOAas_eD4lYaGsDIj3_So`LQ2Cn61*dPed-tR2Wm zrolV(9=>-Ls~NoW%sQp-Sj}M_YabpYTko~=+3PNfb&V&<)OpEyux{n&{Q7S7F;2mK zBkRr!;Y?jAGVKNXMk(-Y6pl4;r>rKKI=8U}>G_AXw&9&ud*R%3P@6o=_tu^}Vm9m@ zJw>KH@Hw2_s7$62aCXw!XLlFeJ#zOq8=1DiG$f%wDBpQ}P5vMGZsngbj7*=$SR6zy zGHr_)_yL8<^bNQp>JE}K?gOwEH{pzqp95?UIOq}DL$b$^lT58a2Q|uqWNHn|Im**; z4yz~J^>&WKGjM;{2fhYp-3P;2*<0`(IuePv1NV`d;QKsM`9E1z-351#-2Kf#rmf&-hklPjWcnKB zm9IW3UG89+EXm=iKvr_I~Z{ z*CN*_d;w=;oo#pb=m5N%p$(?vSSa7u$@LSQ4dvUH{|oo$yCsxwlTf~2^7UadeHkC( zdw54u7r0~Q4nqktb%t^&oYAR5uJ-s7V2`jK`HsX6Sc7&?#Tun|eOtqFj?y_hYaDsW zv^_in`*|A$$<+HOJtLh!5i)%p&QhL)@6g>bAOD7RNSBm$2^a%$4uX?sMlLQ~O4-@NATuOxwVnGS9y5 z{5qH8+?99cc;}V1s|ENOrOC7pmc!nHy`#a{fK)t7uFmG{g0qv(KD(RZ?x4H-?JyJn zLSZuPhR^WxJ^8M>@8{Qi)01iAP`>;5nw?BrVj7a*9XhXI4%}gQicH^vGguekJ^kTW zjo;uNf6!xa52*p1bKeDPSWPhz-qr9BnYKVIzJYz<*6@sU1on~I!FTBI;4D^Kcqf*# z9C^vK4cz&*j+Ki{?G;YN0eJWKXXNX-w-}kej(@WY zubi!OHrCw*cgymU=_{Cp6DUfiZ-nwa7s|IE*{-@L-(h6C8R^KhQ7GTNe9cCtFCi9( z;2j+9h`NJx0!7HQI~Kqh4txCefb9|5L#j-^?h#spc26a!QEnmMv}Ed>opV?_k&#S2 z1Ka1{i>zeo8EGQEL^d*Y-`iQcFJT?pS)GZnPU$;VQ&`8^g)lO8-fjdo!)NdYvi0)? ztB|SB-sP}oREA9L2`<9Vc$!RoxB3L9@f4Z90cWmG<0&$I6AN$#_65DKZV}GGoxwg> z3g@nztMkq)YggA{Z8E4m-{t#f$<^MG_vU0KQ+xXpaRAQNwZex;!eeCG33G4~#mMxn zP`>9w`S$;deAAL?LyW<0WF^xVF$LaXmzzx8A#?}H8TW!@>WtkyoI-Ij?E&vuv4`Xy za6h<*6x1l4gZ3`Fo2W^y!>|dSf$NfMG`1o*BaI|mXJylosqfGu;Vf2KGId|YI%V*V zRf}AQz@0*O&a89TOA4OB-l5_wtb3rrvp1FRYm?~^tb_N|S+^Pp->s}mRwPqra;!UF zg741GOs>FXcwbI9?EAWNRFzC4unsry44DqYCZr;$OnBhx1MJG@sV8=1a@srUxD z$+Rt;ZFl#m5ShM;xj2R5WZDypaUqm%IQg!=@8>s>pI`HBfEesV7BX#y$vB9I$+R^- zfHTm}P92Zf6T*Ln&-!x=e zAEQJ0W+Kz)F%e(E9q*QKhrt=yN6FL~tT@=?w+C#G(0dwAq9~aLHE3&;&Ouwl`V`LD zl_Jw#aE`+>a2YbS59}Gq`*g~XX)icS8N5TgZ|1Dd8TgL%2AqdJi9%#*uP_eQIdYS! z&tUJi^Bioyw=|i)h57IfGoQV$z`BOJM)m|-!n)O0@ZIVKOn`SGS$BROzB})N`}$2V z7M_jl`#uNHzI%|FT${o>H-g&Li{$EDyPuh2?b+VKGF>Fl$+ z3+|S6#{!&1DKdQrOOS$!WIF(Bz<=`h`qfZ=b~oWJo+aB+*p7^3+63dV7w!PNV>SiO z$U0-!8qRPWL0&TTo)!1_?J*P}Q}}jND}EKG`&Ujhtj^-)Jg48$C>>Eie`TglFHDaPG>vI_D-^ z;{&+2kcUj$VLI#`Jx-?Hdo>$BpfH(sg|o4zP@GKN?OudmP$rb`GQM6yB{Cg&PrfzC zG{`rV?`x6k2y8pK~U+e1~?Q&^oktCOOM-0KQ{2gT3ILuvh3#(r_fe zXK+Pw?E~-swkGKw%6M3Fc2BS_f@iNiqY7mD4i@7ayl2vPEAL`BiNa)R-FX&{qX3zD z7yk?#MSe21?;8hq$~^maKv3Iwj7+UvIrnUB(!G?WH=NCJwr&BO zZFl#`-QUGHk8)(%7t8T0Du?o2$5;NdcY}O4lI@*PzR_g61?kDOA!4u-50I%lD$YPV zBl{qkzJN)v2W*ePggMEyC1MfOuv(Gt`>@9G2${Bmeg3cUFqwKrngaVs50Pne z_zt}nnaI?Al^FPrm4-}x$MW8@wYUQ79QKleXRzmBXJ)-W*jcOBFcZ#7+Ryjg8$5f{ zlc#kJca0L@XINRc3deGIPkR|MweGwKXJKFAO)P--ZFn|vXJH=f>wEU?ilDYpluW(z z%DGr;lTVVVwP*JRPr-W`>AUz} z`9_8EO-rVJ2E!<9M+P!&gcvwO=?vC$u*Yu?I18EDLz;knumRv<_JG64)IIMQ>_8Zq1~p1+Sizj#XtH$|*!#ZgV+6L~E}VrPh6LO|H8LH5mAHhm z-OG_<+ucAH=NB`fy;1r!QDZ3yS>}2YAD||p?s^6>7YN!H-T(#h4LLn zt{ZV1b;vamiEszW8TTkSW0!_Z>meFjU=OJtxkkep^j*{?S8G^_aL&#e$1pg@;TgCF znYyd82A+{Bk*R&8WpLK6G?`k5c3;JN`-_mN?^x~xCm|P^w!kFVOY#|PufchawXhal znrz=dXEOZ=o{O@QXJd@OMqGnE1)sh4j84IOR64`DRT6x+Y6)jH+()tQ?97$#&O2b= z$om+gu^IM#>yoKwU*|TQ%L!^5>&W&xs*z~~tUX^tH8On{-kWm`Rms%Z9B1pU!QF*- z;cmBgBUKOOyOyuNp?WCa^?c>8|KsP^d~f0zG98Kp+=O?GdWViX!0sS9CGg!{( z)FM-R!1f4lp(dFQfi-CN7^;)$AXvllE)M6g+y!=y;}Xh~seRzj;ViVXvTq`IhjtdL zGiD$OxyiI8e8+O0(tC9pz&gizIP+eCOg#tN?{_chF|vIbeaY4PQ+`4L@@$8xu%G_` zc{=MHJbSHclqXYrg3e3Y^DRQAT@a7&kcUkD9J*Ne?(EE@eIsX{ooRo7Jey!NJp1~2 z3)VKGu^G=`m03TNwXz}fb}aJTygygSz4&+BmmHOO=@ z*58xw0CHW68>m61gRvfdMotYf9gOvGN2LatIz#CU)^D)KZx474uE8FIduHyb{ECWX zY7NUdyNhs6rx!c}+vj&z#ru-%6aEPMNS$DxAqlz3)X&{;pJ6{TlW9YYz$Q3vSA|UN zB{{Ei8imQUBdkSxf1$hIo|CLudB4V0crJPi?x9$7&O@G_dwurqg3n&-8bh!Kenv`p zGIiehQ#iBXK8khcczln%WNP2Y`xyMJqa0*v-**x``(`0eYa8CluoKp<8o}D6dnxXf z*;|OlX52*`GL3?>SI$m4+dd2%;oUj4$kfjc-GG~@%^w?zgulqQI++f_I`|nI)yOmg zYv7Jb6*6^((ixpAs6?jy;XMtPV2_~>mf#$`%dRJ!v$MwGoK81*20j7%z#S2XB;+R3 z;2ru)zRyCYO)v_+W8Fq|GWD}J+&TLN);WS_@DaX$m`v^W$H0ED=Op`u{%*Ek2uFk}IHp)k) z_I*EqXWyJ;>RgVs4d?2dn`{bele_T%c{auvY=^yHd;89AY=g6t^$>+bq{6#JBe4;; z@5`6J^7r4*!O!n+{QOoU(+I4=6;vV90a%G&;S97hcFyQrL|HQJjm7u{CCJnov^C1p zC`P8%u!0(g_w{##XW%5{B2&*uQ(zy-J7w)N#9$lTS8l$ApJ9%2S zih=J|sc>c^0xOXM`wBg=5U1eT$eo2*u&?iZz#Z@*4#7KftX)mP0a$x(2KRpVAQPE3 zfwLRV=42qx25@&_E4+Kdy9uL^h&!kq`uW|!SN_WXm+y7*txBc?u?m+_kxcu;9YS{) zoRM|L&KaFwP?Ai0z#fu&qQ%J68niXa6DUZw*0ADWjl*5hmtmj(03IaM=P?%Ud#52! z-=Uqwa#rV8cxQw6j{1&e9m~&VXpd>I&XJi+8)7&-2VX)dGIi!|ChQkFvy(`+e&(zF zB4=^p;khU~c|H$o&d$R6+1x&Ry`yeEPQadETg1Y5t1RT%7$f0K&Mo-vJP<2z5uS~L zzOQ}#B4p~^#w=Ld$WH*)u0F)Ka4)3=-osb0H`omJ_MOdfwyrT^;O?Ni``%p_jm=0! zZ89B(4S$mFHU4?4km&%dL<-82sXN~8n4O0+ls(~$PBQEPcf(xRV<<$X-t}#bG6~L6 zI%nq`mNkxt$g~;8!p{_S*1aAgu^zrdS0vLu_zcOgPU$;V91bA|nKpwvEABMhguMp$ z3f((%rZPX7w!(OPfm;ZEH(RqB1a}0J@dSCcM=bUsc=iq^*Hv&Ar39IFgS&!9;k?^qMYv1VJv(W|e4bHwnZNs^`E{KPQw_H1v#-jTgQd%u(6Y))1( zbvAY!cEa8Lh8T(MNJFM|5Q&ZV<-3M#uc9)UzJukVe9Mw)Z@7cxjQi7M>I~L=*yAru zrd<$^?_mwvJqBx(-yjE>zK98MS2QD;+UIvyWfMFjRVCAKEX6rEi)9^pHjW|>nfi`3 z3C>fdBhOl}&T;=U*jn^K_&M&@Du=+CowM-nl;C%>HLI<-32TyXV=k;Y2j^a&z1v{V z$hwB}bk0lK^Yt^VIwKB;;Y^NqUHR_36ZVbnJsY``Vqf30uX8!>bU#g|-Qis9kMPcC z_l{;D33rTTw_zd6(5J5~)c4TrtL zWE3XTSKu?)d5({v|s4V&SdyI+L}gv1izc@7d=j%t>L+7J8r|8b8jqw z&)$6G*$R_jPcR*M`WYI&Tdlx(JWZz7ooB;$=RD-u3bF8PO4x4sdsw&nK&om-oCxTcsRT9D49AtITZ)t?r&3!!%k!%&-#c$BKRZ!m2de_zJ7+8 zcTg55Q+Gt?!WmgVXVMug?-}(TEPI6ZkS1dvGLdN`jD$6;Td>CAoQ`K;`}`%z)IRVW ze2;u&+8Vw??}M{C^m*mX5y@o_wM;S7G6SLqsDI1wKMDRPh z5P7zN@6G;hc2=V-d3HxUzJ>kXMu>#xUV92plc{&qS=Ts(?Br?PY7`P-UDAC8XL1(7 zcW3uOJHoz^XCpuB=mpr<-+{E`SqDR~7FSS#OszdHg1rTMNA?Eo{T_p}Ic?$W^Ve`U z)Vo#2V>iOcvjL)!2>!?)`K}86{FWorURa1^6eCl22?36-(`Fcht+)+mIr_nOtWzjNrrx^}3+o(V z|(h`a60ytT$WJn2kg5ceDMX2rPj$$%5qB8rGb5Ab9rn zCD(;GiO0#)dC4j8-71Vc>tYC2!+o;g-Pyj8XCr6Y?fbfu;@LMdd0N{TfkfPdwMq9D zmciaaNdmGrXz$m1Ckv2i2RQqD2=0b@x5`B9!2{&k5F?NX{>UHst_x`ZEK-=TDN0^07-2)y4YtYsxorAW9wH(f2Ij0kxfsgZjUNQ~NNcNG! z$kTV|p|B2}f>LDK9lm4P3(iTV&fB@O;!cC}IzEHn!WkceFkK zCS)3k<*=XN?`CUOe~0&LcrGeSrri;bL&!#+K6{;IUyBr!B-3uNZsog`J^f~gfipQb z;k&b+6KdZm8J>+gVLCkfI=Arx#$h|GUDd{5SbM$<_kQgSE`;|II-AoO&c+^ryP@8# z;@y+`kcm7SVI($#Kk{GsmLby~m=Eu;dxA_mAr6P}2$?#=F%k9%!^pEf+%rpnHOdNP z>KvtWcGftGkf~>2`~2RAm4i$@Be~CSAIW`D-=X`$eHCXptW( zaGt}N+4ry;cTts0d%@3X_zv00)!)&BFzd~@6NjETH}iatX%)kAOp4vT=i^)pAKjN_ O*>A7Czp!8P6aN8C9L4hh literal 0 HcmV?d00001 diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/OT128_Angle Correction File.csv b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/OT128_Angle Correction File.csv new file mode 100644 index 0000000..1c264b6 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/OT128_Angle Correction File.csv @@ -0,0 +1,129 @@ +Channel,Elevation,Azimuth +1,14.985,0.186 +2,13.283,0.185 +3,11.758,1.335 +4,10.483,1.343 +5,9.836,0.148 +6,9.171,0.147 +7,8.496,0.146 +8,7.812,0.146 +9,7.462,1.335 +10,7.115,1.336 +11,6.767,1.337 +12,6.416,1.338 +13,6.064,1.339 +14,5.71,1.34 +15,5.355,1.341 +16,4.998,1.342 +17,4.643,0.128 +18,4.282,0.128 +19,3.921,0.127 +20,3.558,0.127 +21,3.194,0.107 +22,2.829,0.106 +23,2.463,0.105 +24,2.095,0.105 +25,1.974,-3.118 +26,1.854,1.315 +27,1.729,4.529 +28,1.609,-3.121 +29,1.487,1.316 +30,1.362,4.532 +31,1.242,-3.124 +32,1.12,1.317 +33,0.995,4.536 +34,0.875,-3.127 +35,0.75,1.317 +36,0.625,4.539 +37,0.5,-3.13 +38,0.375,1.318 +39,0.25,4.542 +40,0.125,-3.133 +41,0,0.103 +42,-0.125,2.935 +43,-0.25,-1.517 +44,-0.375,0.103 +45,-0.5,2.937 +46,-0.626,-1.519 +47,-0.751,0.103 +48,-0.876,2.939 +49,-1.001,-1.52 +50,-1.126,0.103 +51,-1.251,2.941 +52,-1.377,-1.521 +53,-1.502,0.102 +54,-1.627,2.943 +55,-1.751,-1.523 +56,-1.876,0.102 +57,-2.001,2.945 +58,-2.126,-1.524 +59,-2.251,0.102 +60,-2.376,2.946 +61,-2.501,-1.526 +62,-2.626,0.102 +63,-2.751,2.948 +64,-2.876,-1.526 +65,-3.001,1.324 +66,-3.126,4.57 +67,-3.251,-3.155 +68,-3.376,1.325 +69,-3.501,4.573 +70,-3.626,-3.157 +71,-3.751,1.326 +72,-3.876,4.575 +73,-4.001,-3.159 +74,-4.126,1.326 +75,-4.25,4.578 +76,-4.375,-3.161 +77,-4.501,1.327 +78,-4.626,4.581 +79,-4.751,-3.163 +80,-4.876,1.328 +81,-5.001,4.583 +82,-5.126,-3.165 +83,-5.252,1.329 +84,-5.377,4.586 +85,-5.502,-3.167 +86,-5.626,1.329 +87,-5.752,4.588 +88,-5.877,-3.168 +89,-6.002,0.102 +90,-6.378,0.103 +91,-6.754,0.103 +92,-7.13,0.103 +93,-7.507,0.104 +94,-7.882,0.104 +95,-8.257,0.104 +96,-8.632,0.104 +97,-9.003,1.337 +98,-9.376,1.337 +99,-9.749,1.338 +100,-10.121,1.339 +101,-10.493,1.34 +102,-10.864,1.341 +103,-11.234,1.341 +104,-11.603,1.342 +105,-11.975,0.108 +106,-12.343,0.108 +107,-12.709,0.109 +108,-13.075,0.109 +109,-13.439,0.13 +110,-13.803,0.131 +111,-14.164,0.131 +112,-14.525,0.132 +113,-14.879,1.384 +114,-15.237,1.384 +115,-15.593,1.385 +116,-15.948,1.385 +117,-16.299,1.386 +118,-16.651,1.386 +119,-17,1.387 +120,-17.347,1.387 +121,-17.701,0.151 +122,-18.386,0.153 +123,-19.063,0.154 +124,-19.73,0.156 +125,-20.376,1.388 +126,-21.653,1.408 +127,-23.044,0.196 +128,-24.765,0.286 diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/Pandar128E3X_Angle Correction File.csv b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/Pandar128E3X_Angle Correction File.csv new file mode 100644 index 0000000..d07be84 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/Pandar128E3X_Angle Correction File.csv @@ -0,0 +1,129 @@ +Channel,Elevation,Azimuth +1,14.436,3.257 +2,13.535,3.263 +3,13.081786,1.091414 +4,12.624,3.268 +5,12.165246,1.092504 +6,11.702,3.273 +7,11.238522,1.093621 +8,10.771,3.278 +9,10.305007,1.094766 +10,9.83,3.283 +11,9.356123,1.095941 +12,8.88,3.288 +13,8.401321,1.097146 +14,7.921,3.291 +15,7.43808,1.098384 +16,6.952581,-1.101114 +17,6.466905,1.099655 +18,5.977753,-1.103649 +19,5.487,-3.306 +20,4.995801,-1.106118 +21,4.501,-3.311 +22,4.007293,-1.108519 +23,3.509,-3.318 +24,3.012822,-1.110852 +25,2.512,-3.324 +26,2.013,-1.113115 +27,1.885,7.72 +28,1.761,5.535 +29,1.637,3.325 +30,1.511,-3.33 +31,1.385875,1.10673 +32,1.2582,-5.538034933 +33,1.13,-7.726107248 +34,1.008459,-1.115309 +35,0.88,7.731 +36,0.756,5.543 +37,0.63,3.329 +38,0.505,-3.336 +39,0.378591,1.108227 +40,0.251,-5.546813133 +41,0.124,-7.738214933 +42,-0.00015,-1.117431 +43,-0.129,7.743 +44,-0.2541,5.550783 +45,-0.38,3.335 +46,-0.5061259,-3.341759 +47,-0.63235,1.109762 +48,-0.7597898,-5.555361 +49,-0.8872418,-7.750039 +50,-1.012168,-1.119482 +51,-1.141,7.757 +52,-1.2662,5.559905 +53,-1.393,3.34 +54,-1.519337,-3.347338 +55,-1.646275,1.111338 +56,-1.773301,-5.564415 +57,-1.900587,-7.762486 +58,-2.026912,-1.12146 +59,-2.155,7.768 +60,-2.2815,5.568891 +61,-2.409,3.345 +62,-2.534932,-3.352812 +63,-2.662501,1.112953 +64,-2.789024,-5.573332 +65,-2.916055,-7.774765 +66,-3.043698,-1.123366 +67,-3.172,7.78 +68,-3.299,5.577739 +69,-3.425,3.351 +70,-3.552222,-3.358181 +71,-3.680335,1.114609 +72,-3.806265,-5.582111 +73,-3.932954,-7.786874 +74,-4.06183,-1.125199 +75,-4.19,7.792 +76,-4.318,5.586449 +77,-4.444,3.356 +78,-4.570508,-3.363443 +79,-4.699079,1.116305 +80,-4.824327,-5.590751 +81,-4.950584,-7.798811 +82,-5.080608,-1.126958 +83,-5.209,7.804 +84,-5.336,5.595019 +85,-5.463,3.36 +86,-5.589088,-3.368599 +87,-5.718031,1.118042 +88,-5.842508,-5.599251 +89,-5.968246,-7.810576 +90,-6.099661,-1.128643 +91,-6.607262,-3.373647 +92,-7.117295,-1.130255 +93,-7.624327,-3.378587 +94,-8.133802,-1.131792 +95,-8.639587,-3.383419 +96,-9.149,3.381 +97,-9.652353,-3.388143 +98,-10.16,3.386 +99,-10.665443,1.127077 +100,-11.17,3.39 +101,-11.671568,1.129045 +102,-12.174,3.395 +103,-12.673194,1.131048 +104,-13.173,3.401 +105,-13.669682,1.133088 +106,-14.166,3.406 +107,-14.660411,1.135163 +108,-15.154,3.41 +109,-15.644783,1.137272 +110,-16.135,3.416 +111,-16.622221,1.139412 +112,-17.106088,-1.142319 +113,-17.592171,1.141584 +114,-18.071976,-1.143128 +115,-18.54765,-3.425708 +116,-19.029597,-1.143867 +117,-19.50071,-3.429329 +118,-19.978461,-1.144538 +119,-20.44479,-3.432841 +120,-20.918108,-1.14514 +121,-21.37943,-3.436242 +122,-21.848107,-1.145675 +123,-22.30422,-3.439533 +124,-22.768055,-1.146145 +125,-23.21878,-3.442714 +126,-23.677577,-1.146549 +127,-24.12274,-3.445786 +128,-25.01577,-3.448749 diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/Pandar40M_Angle Correction File.csv b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/Pandar40M_Angle Correction File.csv new file mode 100644 index 0000000..594b385 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/Pandar40M_Angle Correction File.csv @@ -0,0 +1,41 @@ +Laser id,Elevation,Azimuth +1,14.881,-1.042 +2,11.031,-1.042 +3,8.058,-1.042 +4,5.056,-1.042 +5,3.039,-1.042 +6,2.027,-1.042 +7,1.687,3.125 +8,1.35,-5.208 +9,1.012,-1.042 +10,0.674,3.125 +11,0.336,-5.208 +12,-0.001,-1.042 +13,-0.338,3.125 +14,-0.676,-5.208 +15,-1.014,-1.042 +16,-1.352,3.125 +17,-1.689,-5.208 +18,-2.029,-1.042 +19,-2.366,3.125 +20,-2.701,-5.208 +21,-3.041,-1.042 +22,-3.376,3.125 +23,-3.713,-5.208 +24,-4.051,-1.042 +25,-4.386,3.125 +26,-4.721,-5.208 +27,-5.058,-1.042 +28,-5.392,3.125 +29,-5.727,-5.208 +30,-6.062,-1.042 +31,-7.064,-1.042 +32,-8.06,-1.042 +33,-9.061,-1.042 +34,-10.046,-1.042 +35,-11.033,-1.042 +36,-12.007,-1.042 +37,-12.975,-1.042 +38,-13.931,-1.042 +39,-18.89,-1.042 +40,-24.898,-1.042 \ No newline at end of file diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/Pandar40P_Angle Correction File.csv b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/Pandar40P_Angle Correction File.csv new file mode 100644 index 0000000..28012e3 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/Pandar40P_Angle Correction File.csv @@ -0,0 +1,41 @@ +Channel,Elevation,Azimuth +1,15.21,-1.042 +2,11.36,-1.042 +3,8.387,-1.042 +4,5.385,-1.042 +5,3.368,-1.042 +6,2.356,-1.042 +7,2.016,3.125 +8,1.679,-5.208 +9,1.341,-1.042 +10,1.003,3.125 +11,0.665,-5.208 +12,0.328,-1.042 +13,-0.009,3.125 +14,-0.347,-5.208 +15,-0.685,-1.042 +16,-1.023,3.125 +17,-1.36,-5.208 +18,-1.7,-1.042 +19,-2.037,3.125 +20,-2.372,-5.208 +21,-2.712,-1.042 +22,-3.047,3.125 +23,-3.384,-5.208 +24,-3.722,-1.042 +25,-4.057,3.125 +26,-4.392,-5.208 +27,-4.729,-1.042 +28,-5.063,3.125 +29,-5.398,-5.208 +30,-5.733,-1.042 +31,-6.735,-1.042 +32,-7.731,-1.042 +33,-8.732,-1.042 +34,-9.557,-1.042 +35,-10.704,-1.042 +36,-11.678,-1.042 +37,-12.646,-1.042 +38,-13.602,-1.042 +39,-18.561,-1.042 +40,-24.569,-1.042 \ No newline at end of file diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/Pandar64_Angle Correction File.csv b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/Pandar64_Angle Correction File.csv new file mode 100644 index 0000000..cf77f4f --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/Pandar64_Angle Correction File.csv @@ -0,0 +1,65 @@ +Channel,Elevation,Azimuth +1,14.708,-1.042 +2,10.858,-1.042 +3,7.885,-1.042 +4,4.883,-1.042 +5,2.866,-1.042 +6,1.854,-1.042 +7,1.686,1.042 +8,1.514,3.125 +9,1.348,5.208 +10,1.177,-5.208 +11,1.01,-3.125 +12,0.839,-1.042 +13,0.672,1.042 +14,0.501,3.125 +15,0.334,5.208 +16,0.163,-5.208 +17,-0.005,-3.125 +18,-0.174,-1.042 +19,-0.343,1.042 +20,-0.511,3.125 +21,-0.682,5.208 +22,-0.849,-5.208 +23,-1.019,-3.125 +24,-1.187,-1.042 +25,-1.358,1.042 +26,-1.525,3.125 +27,-1.696,5.208 +28,-1.862,-5.208 +29,-2.034,-3.125 +30,-2.202,-1.042 +31,-2.372,1.042 +32,-2.539,3.125 +33,-2.71,5.208 +34,-2.874,-5.208 +35,-3.047,-3.125 +36,-3.214,-1.042 +37,-3.384,1.042 +38,-3.549,3.125 +39,-3.722,5.208 +40,-3.886,-5.208 +41,-4.058,-3.125 +42,-4.224,-1.042 +43,-4.395,1.042 +44,-4.559,3.125 +45,-4.732,5.208 +46,-4.894,-5.208 +47,-5.066,-3.125 +48,-5.231,-1.042 +49,-5.403,1.042 +50,-5.565,3.125 +51,-5.739,5.208 +52,-5.9,-5.208 +53,-6.072,-3.125 +54,-6.235,-1.042 +55,-7.237,-1.042 +56,-8.233,-1.042 +57,-9.234,-1.042 +58,-10.059,-1.042 +59,-11.206,-1.042 +60,-12.18,-1.042 +61,-13.148,-1.042 +62,-14.104,-1.042 +63,-19.063,-1.042 +64,-25.071,-1.042 \ No newline at end of file diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/Pandar90E3X_Angle Correction File.csv b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/Pandar90E3X_Angle Correction File.csv new file mode 100644 index 0000000..239e681 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/Pandar90E3X_Angle Correction File.csv @@ -0,0 +1,91 @@ +Channel,Elevation,Azimuth +1,14.436,3.257 +2,13.535,3.263 +3,11.702,3.273 +4,9.83,3.283 +5,8.401,1.097 +6,6.953,-1.101 +7,5.487,-3.306 +8,4.007,-1.109 +9,3.013,-1.111 +10,2.512,-3.324 +11,2.013,-1.113 +12,1.637,3.325 +13,1.258,-5.538 +14,1.008,-1.115 +15,0.756,5.543 +16,0.63,3.329 +17,0.505,-3.336 +18,0.379,1.108 +19,0.251,-5.547 +20,0.124,-7.738 +21,0,-1.117 +22,-0.129,7.743 +23,-0.254,5.551 +24,-0.38,3.335 +25,-0.506,-3.342 +26,-0.632,1.11 +27,-0.76,-5.555 +28,-0.887,-7.75 +29,-1.012,-1.119 +30,-1.141,7.757 +31,-1.266,5.56 +32,-1.393,3.34 +33,-1.519,-3.347 +34,-1.646,1.111 +35,-1.773,-5.564 +36,-1.901,-7.762 +37,-2.027,-1.121 +38,-2.155,7.768 +39,-2.282,5.569 +40,-2.409,3.345 +41,-2.535,-3.353 +42,-2.663,1.113 +43,-2.789,-5.573 +44,-2.916,-7.775 +45,-3.044,-1.123 +46,-3.172,7.78 +47,-3.299,5.578 +48,-3.425,3.351 +49,-3.552,-3.358 +50,-3.68,1.115 +51,-3.806,-5.582 +52,-3.933,-7.787 +53,-4.062,-1.125 +54,-4.19,7.792 +55,-4.318,5.586 +56,-4.444,3.356 +57,-4.571,-3.363 +58,-4.699,1.116 +59,-4.824,-5.591 +60,-5.081,-1.127 +61,-5.336,5.595 +62,-5.718,1.118 +63,-6.1,-1.129 +64,-6.607,-3.374 +65,-7.117,-1.13 +66,-7.624,-3.379 +67,-8.134,-1.132 +68,-8.64,-3.383 +69,-9.149,3.381 +70,-9.652,-3.388 +71,-10.16,3.386 +72,-10.665,1.127 +73,-11.17,3.39 +74,-11.672,1.129 +75,-12.174,3.395 +76,-12.673,1.131 +77,-13.173,3.401 +78,-13.67,1.133 +79,-14.166,3.406 +80,-14.66,1.135 +81,-15.154,3.41 +82,-15.645,1.137 +83,-16.622,1.139 +84,-17.592,1.142 +85,-18.548,-3.426 +86,-19.978,-1.145 +87,-21.379,-3.436 +88,-22.768,-1.146 +89,-24.123,-3.446 +90,-25.016,-3.449 diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/PandarQT_Angle Correction File.csv b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/PandarQT_Angle Correction File.csv new file mode 100644 index 0000000..dff76b8 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/PandarQT_Angle Correction File.csv @@ -0,0 +1,65 @@ +Channel,Elevation,Azimuth +1,-52.121,8.736 +2,-49.785,8.314 +3,-47.577,7.964 +4,-45.477,7.669 +5,-43.465,7.417 +6,-41.528,7.198 +7,-39.653,7.007 +8,-37.831,6.838 +9,-36.055,6.688 +10,-34.32,6.554 +11,-32.619,6.434 +12,-30.95,6.326 +13,-29.308,6.228 +14,-27.69,6.14 +15,-26.094,6.059 +16,-24.517,5.987 +17,-22.964,-5.27 +18,-21.42,-5.216 +19,-19.889,-5.167 +20,-18.372,-5.123 +21,-16.865,-5.083 +22,-15.368,-5.047 +23,-13.88,-5.016 +24,-12.399,-4.988 +25,-10.925,-4.963 +26,-9.457,-4.942 +27,-7.994,-4.924 +28,-6.535,-4.91 +29,-5.079,-4.898 +30,-3.626,-4.889 +31,-2.175,-4.884 +32,-0.725,-4.881 +33,0.725,5.493 +34,2.175,5.496 +35,3.626,5.502 +36,5.079,5.512 +37,6.534,5.525 +38,7.993,5.541 +39,9.456,5.561 +40,10.923,5.584 +41,12.397,5.611 +42,13.877,5.642 +43,15.365,5.676 +44,16.861,5.716 +45,18.368,5.759 +46,19.885,5.808 +47,21.415,5.862 +48,22.959,5.921 +49,24.524,-5.33 +50,26.101,-5.396 +51,27.697,-5.469 +52,29.315,-5.55 +53,30.957,-5.64 +54,32.627,-5.74 +55,34.328,-5.85 +56,36.064,-5.974 +57,37.84,-6.113 +58,39.662,-6.269 +59,41.537,-6.447 +60,43.475,-6.651 +61,45.487,-6.887 +62,47.587,-7.163 +63,49.795,-7.493 +64,52.133,-7.892 diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/PandarXT-16_Angle Correction File.csv b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/PandarXT-16_Angle Correction File.csv new file mode 100644 index 0000000..4453cd2 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/PandarXT-16_Angle Correction File.csv @@ -0,0 +1,17 @@ +Channel,Elevation,Azimuth +1,15,0 +2,13,0 +3,11,0 +4,9,0 +5,7,0 +6,5,0 +7,3,0 +8,1,0 +9,-1,0 +10,-3,0 +11,-5,0 +12,-7,0 +13,-9,0 +14,-11,0 +15,-13,0 +16,-15,0 diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/PandarXT_Angle Correction File.csv b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/PandarXT_Angle Correction File.csv new file mode 100644 index 0000000..87f268a --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/PandarXT_Angle Correction File.csv @@ -0,0 +1,33 @@ +Channel,Elevation,Azimuth +1,15,0 +2,14,0 +3,13,0 +4,12,0 +5,11,0 +6,10,0 +7,9,0 +8,8,0 +9,7,0 +10,6,0 +11,5,0 +12,4,0 +13,3,0 +14,2,0 +15,1,0 +16,0,0 +17,-1,0 +18,-2,0 +19,-3,0 +20,-4,0 +21,-5,0 +22,-6,0 +23,-7,0 +24,-8,0 +25,-9,0 +26,-10,0 +27,-11,0 +28,-12,0 +29,-13,0 +30,-14,0 +31,-15,0 +32,-16,0 diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/QT128C2X_Angle Correction File.csv b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/QT128C2X_Angle Correction File.csv new file mode 100644 index 0000000..9201fc9 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/QT128C2X_Angle Correction File.csv @@ -0,0 +1,129 @@ +Laser id,Elevation,Azimuth +1,-52.62676282,10.10830596 +2,-51.0280939,9.719503673 +3,-49.51495392,9.384265827 +4,-48.07394795,9.091433335 +5,-46.69466297,8.832899657 +6,-45.36882812,8.602605729 +7,-44.08974439,8.395920495 +8,-42.85190128,8.209210679 +9,-41.65069769,8.03961095 +10,-40.48225401,7.8848012 +11,-39.34326358,7.742887867 +12,-38.23088092,7.612311759 +13,-37.14263654,7.491771683 +14,-36.07637316,7.380164213 +15,-35.03019446,7.276562397 +16,-34.00242585,7.180171603 +17,-32.99157758,7.090302002 +18,-31.99631045,7.006369095 +19,-31.01543179,6.927848973 +20,-30.04786472,6.854295551 +21,-29.09262707,6.785306938 +22,-28.14883195,6.720536921 +23,-27.21566989,6.659666871 +24,-26.29240356,6.602428464 +25,-25.37835081,6.548569874 +26,-24.47288318,6.497872808 +27,-23.57542315,6.450129948 +28,-22.68544071,6.405178951 +29,-21.80243688,6.362840261 +30,-20.92594329,6.32298504 +31,-20.05553602,6.285467298 +32,-19.19081372,6.25017485 +33,-18.33139172,-6.21699538 +34,-17.47691901,-6.18583344 +35,-16.62705442,-6.156599148 +36,-15.78149317,-6.129208192 +37,-14.93993032,-6.103581848 +38,-14.10208035,-6.079663981 +39,-13.26767188,-6.057381402 +40,-12.43645574,-6.036683527 +41,-11.60818201,-6.017519726 +42,-10.78261371,-5.999833659 +43,-9.959523504,-5.983597281 +44,-9.138697037,-5.968771181 +45,-8.319929599,-5.955310246 +46,-7.503005408,-5.943192006 +47,-6.687807162,-5.926719867 +48,-5.873926875,-5.922876604 +49,-5.061393513,-5.914628713 +50,-4.249948322,-5.907639071 +51,-3.43941536,-5.901885097 +52,-2.629617595,-5.897355529 +53,-1.820378325,-5.894039091 +54,-1.011522452,-5.891935825 +55,-0.202883525,-5.89103442 +56,0.605717878,-5.89132922 +57,1.414444374,-5.892831558 +58,2.223469054,-5.895541414 +59,3.032962884,-5.899458754 +60,3.8431104,-5.904600534 +61,4.654074511,-5.910966684 +62,5.466034723,-5.918579795 +63,6.279180222,-5.927451101 +64,7.093696848,-5.937608821 +65,7.909760474,5.949064148 +66,8.727579858,5.961845258 +67,9.54735011,5.97598597 +68,10.36927364,5.99150874 +69,11.19356846,6.008464331 +70,12.0204614,6.026875139 +71,12.85016831,6.046797524 +72,13.68294669,6.068282136 +73,14.51903314,6.091373918 +74,15.35869922,6.116134758 +75,16.20222125,6.142626487 +76,17.04988688,6.170927865 +77,17.90200196,6.201111915 +78,18.75890049,6.233268566 +79,19.62091911,6.267487655 +80,20.48842178,6.303870236 +81,21.36180725,6.342539884 +82,22.24149217,6.383614369 +83,23.12791172,6.427233938 +84,24.02155359,6.473555624 +85,24.92293181,6.52274755 +86,25.83260224,6.574994555 +87,26.75116558,6.63050945 +88,27.67927338,6.689521655 +89,28.61763387,6.752299732 +90,29.56702472,6.819128697 +91,30.52828971,6.890343815 +92,31.50236238,6.966319159 +93,32.49026442,7.0474731 +94,33.49313666,7.134285032 +95,34.51223682,7.227334588 +96,35.54897914,7.327222327 +97,19.19081372,-6.25017485 +98,20.05553602,-6.285467298 +99,20.92594329,-6.32298504 +100,21.80243688,-6.362840261 +101,22.68544071,-6.405178951 +102,23.57542315,-6.450129948 +103,24.47288318,-6.497872808 +104,25.37835081,-6.548569874 +105,26.29240356,-6.602428464 +106,27.21566989,-6.659666871 +107,28.14883195,-6.720536921 +108,29.09262707,-6.785306938 +109,30.04786472,-6.854295551 +110,31.01543179,-6.927848973 +111,31.99631045,-7.006369095 +112,32.99157758,-7.090302002 +113,34.00242585,-7.180171603 +114,35.03019446,-7.276562397 +115,36.07637316,-7.380164213 +116,37.14263654,-7.491771683 +117,38.23088092,-7.612311759 +118,39.34326358,-7.742887867 +119,40.48225401,-7.8848012 +120,41.65069769,-8.03961095 +121,42.85190128,-8.209210679 +122,44.08974439,-8.395920495 +123,45.36882812,-8.602605729 +124,46.69466297,-8.832899657 +125,48.07394795,-9.091433335 +126,49.51495392,-9.384265827 +127,51.0280939,-9.719503673 +128,52.62676282,-10.10830596 \ No newline at end of file diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/QT128C2X_Channel_Cofig.csv b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/QT128C2X_Channel_Cofig.csv new file mode 100644 index 0000000..19a6fd6 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/QT128C2X_Channel_Cofig.csv @@ -0,0 +1,84 @@ +EEFF,1,1 +LaserNum,80,BlockNum,2 +Loop1,Loop2 +49,17 +50,18 +51,19 +52,20 +53,21 +54,22 +55,23 +56,24 +57,25 +58,26 +59,27 +60,28 +61,29 +62,30 +63,31 +64,32 +65,65 +66,66 +67,67 +68,68 +69,69 +70,70 +71,71 +72,72 +73,73 +74,74 +75,75 +76,76 +77,77 +78,78 +79,79 +80,80 +81,81 +82,82 +83,83 +84,84 +85,85 +86,86 +87,87 +88,88 +89,89 +90,90 +91,91 +92,92 +93,93 +94,94 +95,95 +96,96 +97,97 +98,98 +99,99 +100,100 +101,101 +102,102 +103,103 +104,104 +105,105 +106,106 +107,107 +108,108 +109,109 +110,110 +111,111 +112,112 +113,113 +114,114 +115,115 +116,116 +117,117 +118,118 +119,119 +120,120 +121,121 +122,122 +123,123 +124,124 +125,125 +126,126 +127,127 +128,128 +3142432,,, \ No newline at end of file diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/XT32M2X_Angle Correction File.csv b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/XT32M2X_Angle Correction File.csv new file mode 100644 index 0000000..7e81b6c --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/XT32M2X_Angle Correction File.csv @@ -0,0 +1,33 @@ +Channel,Elevation,Azimuth +1,19.433708,0.175281 +2,18.129961,0.175729 +3,16.821214,0.173645 +4,15.527467,0.171487 +5,14.227470,0.169308 +6,12.924973,0.167102 +7,11.663726,0.166029 +8,10.363728,0.161264 +9,9.062481,0.160229 +10,7.759984,0.161676 +11,6.461237,0.159349 +12,5.162489,0.158259 +13,3.864992,0.155905 +14,2.566245,0.154796 +15,1.273747,0.153668 +16,-0.032500,0.153816 +17,-1.339997,0.153966 +18,-2.637495,0.151599 +19,-3.937492,0.151743 +20,-5.233739,0.149387 +21,-6.529987,0.152042 +22,-7.827484,0.153463 +23,-9.126231,0.147404 +24,-10.426228,0.140117 +25,-11.719976,0.137839 +26,-12.984973,0.134276 +27,-14.274970,0.150794 +28,-15.567467,0.151100 +29,-16.859964,0.147691 +30,-18.157461,0.146833 +31,-19.451208,0.144759 +32,-20.747455,0.143988 \ No newline at end of file diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/firetime_correction/AT128E2X_Firetime Correction File.csv b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/firetime_correction/AT128E2X_Firetime Correction File.csv new file mode 100644 index 0000000..f096e05 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/firetime_correction/AT128E2X_Firetime Correction File.csv @@ -0,0 +1,129 @@ +Channel,fire time(us) +1,0 +2,0 +3,8.24 +4,4.112 +5,4.114 +6,8.24 +7,0 +8,0 +9,12.424 +10,4.144 +11,4.112 +12,8.264 +13,12.376 +14,12.376 +15,8.264 +16,12.424 +17,0 +18,0 +19,4.112 +20,8.24 +21,4.144 +22,0 +23,0 +24,4.144 +25,12.424 +26,8.264 +27,4.112 +28,12.376 +29,12.376 +30,12.424 +31,8.264 +32,0.848 +33,2.504 +34,4.976 +35,6.616 +36,6.616 +37,9.112 +38,2.504 +39,0.848 +40,10.768 +41,13.28 +42,13.28 +43,4.976 +44,9.112 +45,14.928 +46,14.928 +47,10.768 +48,2.504 +49,0.848 +50,6.616 +51,4.976 +52,9.112 +53,6.616 +54,0.848 +55,2.504 +56,13.28 +57,10.768 +58,4.976 +59,13.28 +60,13.28 +61,9.112 +62,10.768 +63,14.928 +64,13.28 +65,0.848 +66,9.112 +67,13.28 +68,2.504 +69,4.976 +70,0.848 +71,2.504 +72,14.928 +73,10.768 +74,10.768 +75,14.928 +76,4.976 +77,6.616 +78,6.616 +79,9.112 +80,10.768 +81,13.28 +82,13.28 +83,9.112 +84,4.976 +85,2.504 +86,2.504 +87,0.848 +88,10.768 +89,14.928 +90,14.928 +91,10.768 +92,6.616 +93,4.976 +94,9.112 +95,6.616 +96,4.112 +97,12.424 +98,0 +99,4.144 +100,0 +101,0 +102,12.424 +103,0 +104,8.264 +105,12.424 +106,12.424 +107,8.24 +108,8.24 +109,8.264 +110,12.376 +111,12.376 +112,12.424 +113,4.112 +114,4.144 +115,0 +116,0 +117,0 +118,0 +119,0 +120,12.424 +121,8.264 +122,8.24 +123,4.144 +124,8.264 +125,8.24 +126,12.376 +127,12.376 +128,8.264 \ No newline at end of file diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/firetime_correction/Pandar128E3X_Firetime Correction File.csv b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/firetime_correction/Pandar128E3X_Firetime Correction File.csv new file mode 100644 index 0000000..d26e11f --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/firetime_correction/Pandar128E3X_Firetime Correction File.csv @@ -0,0 +1,131 @@ +2.85,distance>=A1,distance=A1,distance=A1,distance=A1,distance=A1,distance=A1,distance=A1,distance=A1,distance=A1,distance=A1,distance=A1,distance=A1,distance=A1,distance=A1,distance=A1,distance=A1,distance=A1,distance=A1,distance=A1,distance=A1,distance=A1,distance=A1,distance=A1,distance=A1,distance=A1,distance=A1,distance=A1,distance=A1,distance=A1,distance=A1,distance=A1,distance=A1,distance +class HesaiLidarSdk +{ +private: + boost::thread *runing_thread_ptr_; + std::function pkt_cb_; + std::function&)> point_cloud_cb_; + bool is_thread_runing_; + bool packet_loss_tool_; +public: + HesaiLidarSdk() { + std::cout << "-------- Hesai Lidar SDK V" << VERSION_MAJOR << "." << VERSION_MINOR << "." << VERSION_TINY << " --------" << std::endl; + runing_thread_ptr_ = nullptr; + lidar_ptr_ = nullptr; + is_thread_runing_ = false; + packet_loss_tool_ = false; + } + + ~HesaiLidarSdk() { + Stop(); + } + Lidar *lidar_ptr_; + + //init lidar with param. init logger, udp parser, source, ptc client, start receive/parser thread + bool Init(const DriverParam& param) + { + /*****************************Init decoder******************************************************/ + lidar_ptr_ = new Lidar; + if (nullptr == lidar_ptr_) { + std::cout << "create Lidar fail" << std::endl; + return false; + } + //init lidar with param + lidar_ptr_->Init(param); + //set packet_loss_tool + packet_loss_tool_ = param.decoder_param.enable_packet_loss_tool; + /***********************************************************************************/ + return true; + } + + // stop process thread + void Stop() { + if (nullptr != runing_thread_ptr_) { + is_thread_runing_ = false; + runing_thread_ptr_->join(); + delete runing_thread_ptr_; + runing_thread_ptr_ = nullptr; + } + + if (nullptr != lidar_ptr_) { + delete lidar_ptr_; + lidar_ptr_ = nullptr; + } + } + // start process thread + void Start() { + runing_thread_ptr_ = new boost::thread(boost::bind(&HesaiLidarSdk::Run, this)); + } + + // process thread + void Run() + { + is_thread_runing_ = true; + UdpFrame_t udp_packet_frame; + LidarDecodedPacket decoded_packet; + int packet_index = 0; + uint32_t start = GetMicroTickCount(); + UdpPacket packet; + FaultMessageInfo fault_message_info; + while (is_thread_runing_) { + + //get one packte from origin_packets_buffer_, which receive data from upd or pcap thread + int ret = lidar_ptr_->GetOnePacket(packet); + if (ret == -1) continue; + + //get fault message + if (packet.packet_len == kFaultMessageLength) { + FaultMessageCallback(packet, fault_message_info); + continue; + } + + //get distance azimuth reflection, etc.and put them into decode_packet + lidar_ptr_->DecodePacket(decoded_packet, packet); + + //do not compute xyzi of points if enable packet_loss_tool_ + if(packet_loss_tool_ == true) continue; + + //one frame is receive completely, split frame + if(decoded_packet.scan_complete) { + //waiting for parser thread compute xyzi of points in the same frame + while(!lidar_ptr_->ComputeXYZIComplete(decoded_packet.packet_index)) std::this_thread::sleep_for(std::chrono::microseconds(100)); + uint32_t end = GetMicroTickCount(); + //log info, display frame message + if (lidar_ptr_->frame_.points_num > kMinPointsOfOneFrame) { + // LogInfo("frame:%d points:%u packet:%d time:%lf %lf\n",lidar_ptr_->frame_.frame_index, lidar_ptr_->frame_.points_num, packet_index, lidar_ptr_->frame_.points[0].timestamp, lidar_ptr_->frame_.points[lidar_ptr_->frame_.points_num - 1].timestamp) ; + + //publish point cloud topic + if(point_cloud_cb_) point_cloud_cb_(lidar_ptr_->frame_); + + //publish upd packet topic + if(pkt_cb_) pkt_cb_(udp_packet_frame, lidar_ptr_->frame_.points[0].timestamp); + } + + //reset frame variable + lidar_ptr_->frame_.Update(); + + //clear udp packet vector + udp_packet_frame.clear(); + packet_index = 0; + + //if the packet which contains split frame msgs is vaild, it will be the first packet of new frame + if(decoded_packet.IsDecodedPacketValid()) { + decoded_packet.packet_index = packet_index; + lidar_ptr_->ComputeXYZI(decoded_packet); + udp_packet_frame.emplace_back(packet); + packet_index++; + } + } + else { + //new decoded packet of one frame, put it into decoded_packets_buffer_ and compute xyzi of points + decoded_packet.packet_index = packet_index; + lidar_ptr_->ComputeXYZI(decoded_packet); + udp_packet_frame.emplace_back(packet); + packet_index++; + + //update status manually if start a new frame failedly + if (packet_index >= kMaxPacketNumPerFrame) { + packet_index = 0; + udp_packet_frame.clear(); + lidar_ptr_->frame_.Update(); + printf("fail to start a new frame\n"); + } + } + } + } + // assign callback fuction + void RegRecvCallback(const std::function&)>& callback) { + point_cloud_cb_ = callback; + } + + // assign callback fuction + void RegRecvCallback(const std::function& callback) { + pkt_cb_ = callback; + } + + //parsar fault message + void FaultMessageCallback(UdpPacket& udp_packet, FaultMessageInfo& fault_message_info) { + FaultMessageVersion3 *fault_message_ptr = + reinterpret_cast< FaultMessageVersion3*> (&(udp_packet.buffer[0])); + fault_message_ptr->ParserFaultMessage(fault_message_info); + return; + } +}; + +} // namespace lidar +} // namespace hesai diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/driver/hesai_lidar_sdk_gpu.cuh b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/driver/hesai_lidar_sdk_gpu.cuh new file mode 100644 index 0000000..8c15f62 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/driver/hesai_lidar_sdk_gpu.cuh @@ -0,0 +1,269 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ +#include "lidar.h" +#include "udp_parser_gpu.h" +#include "fault_message.h" +namespace hesai +{ +namespace lidar +{ +template +class HesaiLidarSdkGpu +{ +private: + boost::thread *runing_thread_ptr_; + std::function pkt_cb_; + std::function &)> point_cloud_cb_; + bool is_thread_runing_; + bool packet_loss_tool_; + +public: + HesaiLidarSdkGpu() + { + std::cout << "-------- Hesai Lidar SDK Gpu V" << VERSION_MAJOR << "." << VERSION_MINOR << "." << VERSION_TINY << " --------" << std::endl; + runing_thread_ptr_ = nullptr; + lidar_ptr_ = nullptr; + is_thread_runing_ = false; + packet_loss_tool_ = false; + } + ~HesaiLidarSdkGpu() { + Stop(); + } + hesai::lidar::Lidar *lidar_ptr_; + UdpParserGpu *gpu_parser_ptr_; + + // stop process thread + void Stop() + { + if (nullptr != runing_thread_ptr_) + { + is_thread_runing_ = false; + runing_thread_ptr_->join(); + delete runing_thread_ptr_; + runing_thread_ptr_ = nullptr; + } + + if (nullptr != lidar_ptr_) + { + delete lidar_ptr_; + lidar_ptr_ = nullptr; + } + } + //init lidar with param. init logger, udp parser, source, ptc client, start receive/parser thread + //init gpu parser class + bool Init(const DriverParam ¶m) + { + /*****************************Init decoder******************************************************/ + lidar_ptr_ = new Lidar; + gpu_parser_ptr_ = new UdpParserGpu; + if (nullptr == lidar_ptr_) + { + std::cout << "create Lidar fail"; + return false; + } + + //init lidar with param + lidar_ptr_->Init(param); + + //set packet_loss_tool + packet_loss_tool_ = param.decoder_param.enable_packet_loss_tool; + + //get lidar type from Lidar class + std::string lidar_type_from_parser = lidar_ptr_->GetLidarType(); + + //init gpu parser with lidar type + gpu_parser_ptr_->SetLidarType(lidar_type_from_parser); + + //set transform param + gpu_parser_ptr_->SetTransformPara(param.decoder_param.transform_param.x, + param.decoder_param.transform_param.y, + param.decoder_param.transform_param.z, + param.decoder_param.transform_param.roll, + param.decoder_param.transform_param.pitch, + param.decoder_param.transform_param.yaw); + + /***********************************************************************************/ + + /***************************Init source****************************************/ + + + //load correction file, depanding on source type + if (param.input_param.source_type == 1) + { + u8Array_t sData; + for (int i = 0; i < 3; i++) + { + int ret = lidar_ptr_->ptc_client_->GetCorrectionInfo(sData); + if (ret != 0 || gpu_parser_ptr_->LoadCorrectionString((char *)sData.data()) != 0) + { + gpu_parser_ptr_->LoadCorrectionFile(param.input_param.correction_file_path); + } + else + { + break; + } + } + } + else + { + gpu_parser_ptr_->LoadCorrectionFile(param.input_param.correction_file_path); + } + /********************************************************************************/ + return true; + } + + // start process thread + void Start() + { + runing_thread_ptr_ = new boost::thread(boost::bind(&HesaiLidarSdkGpu::Run, this)); + } + // process thread + void Run() + { + is_thread_runing_ = true; + UdpFrame_t udp_packet_frame; + LidarDecodedPacket decoded_packet; + LidarDecodedFrame frame; + FaultMessageInfo fault_message_info; + int packet_index = 0; + uint32_t start = GetMicroTickCount(); + while (is_thread_runing_) + { + UdpPacket packet; + + //get one packte from origin_packets_buffer_, which receive data from upd or pcap thread + int ret = lidar_ptr_->GetOnePacket(packet); + if (ret == -1) continue; + + //get fault message + if (packet.packet_len == kFaultMessageLength) { + FaultMessageCallback(packet, fault_message_info); + continue; + } + + //get distance azimuth reflection, etc.and put them into decode_packet + int res = lidar_ptr_->DecodePacket(decoded_packet, packet); + + //do not compute xyzi of points if enable packet_loss_tool_ + if(packet_loss_tool_ == true) continue; + + //one frame is receive completely, split frame + if (decoded_packet.scan_complete) + { + //compute xyzi of points in one frame, using gpu device + int ret = gpu_parser_ptr_->ComputeXYZI(frame); + frame.packet_num = packet_index; + + //log info, display frame message + if (frame.points_num > kMinPointsOfOneFrame) { + // LogInfo("frame:%d points:%u packet:%d time:%lf\n", frame.frame_index, frame.points_num, packet_index, frame.points[0].timestamp); + + //publish point cloud topic + if(point_cloud_cb_) point_cloud_cb_(frame); + + //publish upd packet topic + if(pkt_cb_) pkt_cb_(udp_packet_frame, frame.points[0].timestamp); + } + + //reset frame variable + frame.Update(); + + //clear udp packet vector + udp_packet_frame.clear(); + packet_index = 0; + + //if the packet which contains split frame msgs is vaild, it will be the first packet of new frame + if(decoded_packet.IsDecodedPacketValid()) { + decoded_packet.packet_index = packet_index; + udp_packet_frame.emplace_back(packet); + //copy distances, reflection, azimuth, elevation from decoded packet to frame container + memcpy((frame.distances + packet_index * decoded_packet.block_num * decoded_packet.laser_num), (decoded_packet.distances), decoded_packet.block_num * decoded_packet.laser_num * sizeof(uint16_t)); + memcpy((frame.reflectivities + packet_index * decoded_packet.block_num * decoded_packet.laser_num), (decoded_packet.reflectivities), decoded_packet.block_num * decoded_packet.laser_num * sizeof(uint8_t)); + memcpy((frame.azimuth + packet_index * decoded_packet.block_num * decoded_packet.laser_num), (decoded_packet.azimuth), decoded_packet.block_num * decoded_packet.laser_num * sizeof(float)); + memcpy((frame.elevation + packet_index * decoded_packet.block_num * decoded_packet.laser_num), (decoded_packet.elevation), decoded_packet.block_num * decoded_packet.laser_num * sizeof(float)); + frame.distance_unit = decoded_packet.distance_unit; + frame.sensor_timestamp[packet_index] = decoded_packet.sensor_timestamp; + frame.points_num = frame.points_num + decoded_packet.block_num * decoded_packet.laser_num; + frame.lidar_state = decoded_packet.lidar_state; + frame.work_mode = decoded_packet.work_mode; + packet_index++; + } + + } + else + { + //new decoded packet of one frame, put it into frame container + decoded_packet.packet_index = packet_index; + udp_packet_frame.emplace_back(packet); + //copy distances, reflection, azimuth, elevation from decoded packet to frame container + memcpy((frame.distances + packet_index * decoded_packet.block_num * decoded_packet.laser_num), (decoded_packet.distances), decoded_packet.block_num * decoded_packet.laser_num * sizeof(uint16_t)); + memcpy((frame.reflectivities + packet_index * decoded_packet.block_num * decoded_packet.laser_num), (decoded_packet.reflectivities), decoded_packet.block_num * decoded_packet.laser_num * sizeof(uint8_t)); + memcpy((frame.azimuth + packet_index * decoded_packet.block_num * decoded_packet.laser_num), (decoded_packet.azimuth), decoded_packet.block_num * decoded_packet.laser_num * sizeof(float)); + memcpy((frame.elevation + packet_index * decoded_packet.block_num * decoded_packet.laser_num), (decoded_packet.elevation), decoded_packet.block_num * decoded_packet.laser_num * sizeof(float)); + frame.distance_unit = decoded_packet.distance_unit; + frame.sensor_timestamp[packet_index] = decoded_packet.sensor_timestamp; + frame.points_num = frame.points_num + decoded_packet.block_num * decoded_packet.laser_num; + if (decoded_packet.block_num != 0 && decoded_packet.laser_num != 0) { + frame.laser_num = decoded_packet.laser_num; + frame.block_num = decoded_packet.block_num; + } + packet_index++; + + //update status manually if start a new frame failedly + if (packet_index >= kMaxPacketNumPerFrame) { + packet_index = 0; + udp_packet_frame.clear(); + lidar_ptr_->frame_.Update(); + printf("fail to start a new frame\n"); + } + } + } + } + + // assign callback fuction + void RegRecvCallback(const std::function &)>& callback) + { + point_cloud_cb_ = callback; + } + + // assign callback fuction + void RegRecvCallback(const std::function& callback) + { + pkt_cb_ = callback; + } + + void FaultMessageCallback(UdpPacket& udp_packet, FaultMessageInfo& fault_message_info) { + FaultMessageVersion3 *fault_message_ptr = + reinterpret_cast< FaultMessageVersion3*> (&(udp_packet.buffer[0])); + fault_message_ptr->ParserFaultMessage(fault_message_info); + return; + } +}; + +} // namespace lidar +} // namespace hesai diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/CMakeLists.txt b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/CMakeLists.txt new file mode 100755 index 0000000..1e2f279 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/CMakeLists.txt @@ -0,0 +1,132 @@ +# ----------------------------------------# +# Hesai Tech # +# ----------------------------------------# + +cmake_minimum_required(VERSION 2.8) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_STANDARD 14) +project(CommonModules) +if(POLICY CMP0074) +cmake_policy(SET CMP0074 NEW) +else() +cmake_policy(SET CMP0065 NEW) +endif() + +set(CMAKE_BUILD_TYPE ON) + +# Add each module's entry +set(WITH_LIDARBOARD ON CACHE BOOL "Build lidarboard code") +set(WITH_PLATUTIL ON CACHE BOOL "Build platform utils code") +set(WITH_UDPPARSER ON CACHE BOOL "Build udp parser code") +set(WITH_PTCCLIENT ON CACHE BOOL "Build ptc client code") +set(WITH_PTCPARSER ON CACHE BOOL "Build ptc parser code") +set(WITH_CONTAINER ON CACHE BOOL "Build container code") +set(WITH_LOG ON CACHE BOOL "Build log code") + +find_package(Boost REQUIRED COMPONENTS + system + filesystem + thread + iostreams) + +set(Boost_INCLUDE_DIRS "/usr/lib/x86_64-linux-gnu/") + +add_definitions(-DQT_MESSAGELOGCONTEXT) + + +include_directories( + ./ + include + Lidar + UdpProtocol + Server + UdpParser + UdpParser/include + UdpParser/src + PtcParser + PtcParser/include + PtcParser/src + Source/include + Container/include + Container/src + PtcClient/include + Logger/include + ${OPENSSL_INCLUDE_DIR} + ${Boost_INCLUDE_DIRS} +) + +# Make each module an 'OBJECT' library, can be refered to by get_link_libraries +if(WITH_PTCCLIENT) + add_library(ptcClient_lib + PtcClient/src/ptc_client.cc + PtcClient/src/tcp_client.cc + PtcClient/src/tcp_ssl_client.cc + ) + target_link_libraries(ptcClient_lib + ${OPENSSL_LIBRARIES} + ) + set_target_properties(ptcClient_lib + PROPERTIES CXX_STANDARD_REQUIRED TRUE CXX_STANDARD 14) +endif(WITH_PTCCLIENT) + +if(WITH_PTCPARSER) + add_library(ptcParser_lib + PtcParser/src/general_ptc_parser.cc + PtcParser/src/ptc_1_0_parser.cc + PtcParser/src/ptc_2_0_parser.cc + PtcParser/ptc_parser.cc) + set_target_properties(ptcParser_lib + PROPERTIES CXX_STANDARD_REQUIRED TRUE CXX_STANDARD 14) +endif(WITH_PTCPARSER) + + +if(WITH_CONTAINER) + add_library(container_lib + Container/src/ring_2d_shared.cc + Container/src/ring_2dex.cc) + set_target_properties(container_lib + PROPERTIES CXX_STANDARD_REQUIRED TRUE CXX_STANDARD 14) +endif(WITH_CONTAINER) + +if(WITH_LIDARBOARD) + add_library(source_lib + Source/src/source.cc + Source/src/socket_source.cc + Source/src/pcap_source.cc + Source/src/pcap_saver.cc + ) + target_link_libraries(source_lib + platutils_lib + container_lib + ptcClient_lib + Boost::iostreams + ${Boost_LIBRARIES}) + set_target_properties(source_lib + PROPERTIES CXX_STANDARD_REQUIRED TRUE CXX_STANDARD 14) +endif(WITH_LIDARBOARD) + +if(WITH_PLATUTIL) + add_library(platutils_lib + ./src/plat_utils.cc + ./src/auto_tick_count.cc) + set_target_properties(platutils_lib + PROPERTIES CXX_STANDARD_REQUIRED TRUE CXX_STANDARD 14) +endif(WITH_PLATUTIL) + + + +if(WITH_LOG) + find_package(Threads) + add_library(log_lib EXCLUDE_FROM_ALL + Logger/src/logger.cc) + target_link_libraries(log_lib + ${CMAKE_THREAD_LIBS_INIT}) + set_target_properties(log_lib + PROPERTIES CXX_STANDARD_REQUIRED TRUE CXX_STANDARD 14) +endif(WITH_LOG) + + + + +# Property +include_directories(${PROJECT_SOURCE_DIR}) diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Container/include/blocking_ring.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Container/include/blocking_ring.h new file mode 100644 index 0000000..2c574bf --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Container/include/blocking_ring.h @@ -0,0 +1,43 @@ +#ifndef ___HESAI__CONTAINER__BLOCKING_RING_HH___ +#define ___HESAI__CONTAINER__BLOCKING_RING_HH___ + +#include "ring.h" +#include +#include +#include +#include +#include +namespace hesai +{ +namespace lidar +{ +template +class BlockingRing : public Ring{ +public: + using Super = Ring; + using Mutex = std::mutex; + using Condv = std::condition_variable; + using LockS = std::lock_guard; // lock in scope + using LockC = std::unique_lock; // lock by condition +private: + Mutex _mutex; + Condv _condv; +public: + template + void emplace_back(Args&&... args); + void push_back(T&& item); + T pop_back(); + void push_front(T&& item); + T pop_front(); + bool try_pop_front(T&); + bool empty(); + bool not_empty(); + bool full(); + bool not_full(); +}; +} // namespace lidar +} // namespace hesai + +#include "blocking_ring.cc" + +#endif // !___HESAI__CONTAINER__BLOCKING_RING_HH___ diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Container/include/ring.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Container/include/ring.h new file mode 100644 index 0000000..fc45eb7 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Container/include/ring.h @@ -0,0 +1,59 @@ +#ifndef ___HESAI__CONTAINER__RING_HH___ +#define ___HESAI__CONTAINER__RING_HH___ + +#include +#include +#include +#include +namespace hesai +{ +namespace lidar +{ +template +class Ring { +public: + static constexpr size_t Size = N; + template class ItemIterator; + using iterator = ItemIterator; + using const_iterator = ItemIterator; +private: + std::unique_ptr> _ring; + size_t _begin, _size; +public: + Ring(); + inline constexpr size_t size() const; + inline bool empty() const; + inline bool not_empty() const; + inline bool full() const; + inline bool not_full() const; + inline void clear(); + inline void eff_clear(); + template + inline void emplace_back(Args&&... args); + inline void push_back(T&& item); + inline const T& peek_back() const; + inline const T& back() const { return peek_back(); } + inline T pop_back(); + inline void eff_pop_back(); + template + inline void emplace_front(Args&&... args); + inline void push_front(T&& item); + inline const T& peek_front() const; + inline const T& front() const { return peek_front(); } + inline T pop_front(); + inline void eff_pop_front(); + inline iterator begin(); + inline iterator end(); + inline const_iterator cbegin() const; + inline const_iterator cend() const; + inline T& operator[](size_t index); + inline const T& operator[](size_t index) const; + inline T* data(); + inline const T* data() const; +}; +} // namespace lidar +} // namespace hesai + +#include "ring.cc" + +#endif // !___HESAI__CONTAINER__RING_HH___ diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Container/include/ring_2d_shared.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Container/include/ring_2d_shared.h new file mode 100644 index 0000000..6a46080 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Container/include/ring_2d_shared.h @@ -0,0 +1,67 @@ +#ifndef ___HESAI__CONTAINER__RING2D_SHARED_HH___ +#define ___HESAI__CONTAINER__RING2D_SHARED_HH___ + +#include +#include +#include +namespace hesai +{ +namespace lidar +{ +template +class Ring2D_shared { +public: + template class ItemIterator; + using iterator = ItemIterator; + using const_iterator = ItemIterator; +private: + std::unique_ptr> _ring; + boost::interprocess::mapped_region _mapped_region; + int _shmid; + T2 *_ring2; + size_t _begin, _size; + size_t _emplace_start, _emplace_size; + size_t _2D_size; +public: + Ring2D_shared(); + ~Ring2D_shared(); + inline constexpr size_t size() const; + inline bool empty() const; + inline bool not_empty() const; + inline bool full() const; + inline bool not_full() const; + inline void clear(); + inline void eff_clear(); + template + inline void emplace_back(Args&&... args); + inline void push_back(T&& item); + inline const T& peek_back() const; + inline T pop_back(); + inline void eff_pop_back(); + template + inline void emplace_front(Args&&... args); + inline void push_front(T&& item); + inline const T& peek_front() const; + inline T pop_front(); + inline void eff_pop_front(); + inline iterator begin(); + inline iterator end(); + inline const_iterator cbegin() const; + inline const_iterator cend() const; + inline T& operator[](size_t index); + inline const T& operator[](size_t index) const; + inline T* data(); + inline const T* data() const; + inline void prepare_emplace_back(size_t s, size_t t); + inline void emplace_back_at(size_t i, T&& item); + inline T& peek_back_at(size_t i); + inline void emplace_back_at(size_t i, size_t j, T2&& item); + inline T2& peek_back_at(size_t i, size_t j); + inline void finish_emplace_back(); + inline T2* data2(); + inline const T2* data2() const; + inline T2& peek_front(size_t j) const; +}; +} // namespace lidar +} // namespace hesai +#endif // !___HESAI__CONTAINER__RING2D_SHARED_HH___ diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Container/include/ring_2dex.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Container/include/ring_2dex.h new file mode 100644 index 0000000..8aeca67 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Container/include/ring_2dex.h @@ -0,0 +1,64 @@ +#ifndef ___HESAI__CONTAINER__RING2D_EX_HH___ +#define ___HESAI__CONTAINER__RING2D_EX_HH___ + +#include +#include +namespace hesai +{ +namespace lidar +{ +template +class Ring2D_ex { +public: + template class ItemIterator; + using iterator = ItemIterator; + using const_iterator = ItemIterator; +private: + std::unique_ptr> _ring; + std::unique_ptr> _ring2; + size_t _begin, _size; + size_t _emplace_start, _emplace_size; + size_t _2D_size; +public: + Ring2D_ex(); + inline constexpr size_t size() const; + inline bool empty() const; + inline bool not_empty() const; + inline bool full() const; + inline bool not_full() const; + inline void clear(); + inline void eff_clear(); + template + inline void emplace_back(Args&&... args); + inline void push_back(T&& item); + inline const T& peek_back() const; + inline T pop_back(); + inline void eff_pop_back(); + template + inline void emplace_front(Args&&... args); + inline void push_front(T&& item); + inline const T& peek_front() const; + inline T pop_front(); + inline void eff_pop_front(); + inline iterator begin(); + inline iterator end(); + inline const_iterator cbegin() const; + inline const_iterator cend() const; + inline T& operator[](size_t index); + inline const T& operator[](size_t index) const; + inline T* data(); + inline const T* data() const; + inline void prepare_emplace_back(size_t s, size_t t); + inline void emplace_back_at(size_t i, T&& item); + inline T& peek_back_at(size_t i); + inline void emplace_back_at(size_t i, size_t j, T2&& item); + inline T2& peek_back_at(size_t i, size_t j); + inline void finish_emplace_back(); + inline T2* data2(); + inline const T2* data2() const; + inline T2& peek_front(size_t j) const; +}; +} // namespace lidar +} // namespace hesai + +#endif // !___HESAI__CONTAINER__RING2D_EX_HH___ diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Container/src/blocking_ring.cc b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Container/src/blocking_ring.cc new file mode 100644 index 0000000..bd40f7d --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Container/src/blocking_ring.cc @@ -0,0 +1,90 @@ +#include "blocking_ring.h" +#include +using namespace hesai::lidar; +template +void BlockingRing::push_front(T&& value) { + LockC lock(_mutex); + _condv.wait(lock, std::bind(&Super::not_full, this)); + Super::push_front(std::move(value)); + _condv.notify_one(); +} + +template +template +void BlockingRing::emplace_back(Args&&... args) { + LockC lock(_mutex); + _condv.wait(lock, std::bind(&Super::not_full, this)); + Super::emplace_back(args...); + _condv.notify_one(); +} + +template +void BlockingRing::push_back(T&& value) { + LockC lock(_mutex); + _condv.wait(lock, std::bind(&Super::not_full, this)); + Super::push_back(std::move(value)); + _condv.notify_one(); +} + +template +T BlockingRing::pop_front() { + LockC lock(_mutex); + _condv.wait(lock, std::bind(&Super::not_empty, this)); + T value = Super::pop_front(); + _condv.notify_one(); + return value; +} + +template +bool BlockingRing::try_pop_front(T& value) { + LockC lock(_mutex); + using namespace std::literals::chrono_literals; + bool ret = _condv.wait_for(lock, 100ms, std::bind(&Super::not_empty, this)); + // int ret = 0; + if (ret) { + value = Super::peek_front(); + Super::eff_pop_front(); + } + else{ + // printf("????\n"); + } + _condv.notify_one(); + return ret; +} + +template +T BlockingRing::pop_back() { + LockC lock(_mutex); + _condv.wait(lock, std::bind(&Super::not_empty, this)); + T value = Super::pop_back(); + _condv.notify_one(); + return value; +} + +template +bool BlockingRing::empty() +{ + LockS lock(_mutex); + return Super::empty(); +} + +template +bool BlockingRing::not_empty() +{ + LockS lock(_mutex); + return Super::not_empty(); +} + +template +bool BlockingRing::full() +{ + LockS lock(_mutex); + return Super::full(); +} + +template +bool BlockingRing::not_full() +{ + LockS lock(_mutex); + return Super::not_full(); +} diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Container/src/ring.cc b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Container/src/ring.cc new file mode 100644 index 0000000..b47ea85 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Container/src/ring.cc @@ -0,0 +1,120 @@ +#include "ring.h" +#include +#include +using namespace hesai::lidar; +template +template +class Ring::ItemIterator { +public: + using iterator_category = std::random_access_iterator_tag; + using value_type = T; + using difference_type = ptrdiff_t; + using pointer = T*; + using reference = T&; +private: + pointer _p0; + difference_type _steps; +public: + ItemIterator() : _p0(nullptr), _steps(0) {} + ItemIterator(const ItemIterator&) = default; + ItemIterator& operator=(const ItemIterator&) = default; + ItemIterator(pointer p0, difference_type steps) : _p0(p0), _steps(steps) {} + + inline ItemIterator& operator++() { ++_steps; return *this; } + inline ItemIterator& operator--() { assert(_steps >= 1); --_steps; return *this; } + inline ItemIterator operator++(int) { ItemIterator old(*this); ++(*this); return old; } + inline ItemIterator operator--(int) { assert(_steps >= 1); ItemIterator old(*this); --(*this); return old; } + inline ItemIterator operator+(difference_type n) const { return ItemIterator(_p0, _steps + n); } + inline ItemIterator operator-(difference_type n) const { assert(_steps >= n); return ItemIterator(_p0, _steps - n); } + inline ItemIterator& operator+=(difference_type n) { _steps += n; return *this; } + inline ItemIterator& operator-=(difference_type n) { assert(_steps >= n); _steps -= n; return *this; } + inline reference operator*() const { return _p0[_steps % N]; } + inline operator pointer () const { return _p0 + _steps % N; } + // inline pointer operator->() const { return this->operator pointer (); } + inline pointer p0() const { return _p0; } + inline difference_type steps() const { return _steps; } + inline bool operator==(const ItemIterator& other) const { return this->p0() == other.p0() && this->steps() == other.steps(); } + inline bool operator!=(const ItemIterator& other) const { return !(*this == other); } + inline difference_type operator-(const ItemIterator& other) const { assert(this->p0() == other.p0()); return this->steps() - other.steps(); } +}; + +template +Ring::Ring() : _ring(new std::array), _begin(0), _size(0) {}; + +template +inline constexpr size_t Ring::size() const { return _size; } + +template +inline bool Ring::empty() const { return _size == 0; } + +template +inline bool Ring::not_empty() const { return !empty(); } + +template +inline bool Ring::full() const { return _size >= N; } + +template +inline bool Ring::not_full() const { return !full(); } + +template +inline void Ring::clear() { for (auto& item : *this) { item = T(); } _size = 0; _begin = 0; } + +template +inline void Ring::eff_clear() { _size = 0; _begin = 0; } + +template +template +inline void Ring::emplace_back(Args&&... args) { assert(!full()); (*_ring)[(_begin + _size) % N] = T(args...); ++_size;} + +template +inline void Ring::push_back(T&& item) { assert(!full()); (*_ring)[(_begin + _size) % N] = item; ++_size; } + +template +inline const T& Ring::peek_back() const { assert(!empty()); return (*_ring)[(_begin + _size - 1) % N]; } + +template +inline T Ring::pop_back() { assert(!empty()); --_size; T item; std::swap(item, (*_ring)[(_begin + _size) % N]); return item; } + +template +inline void Ring::eff_pop_back() { --_size; } + +template +template +inline void Ring::emplace_front(Args&&... args) { assert(!full()); _begin = (_begin + N - 1) % N; (*_ring)[_begin] = T(args...); ++_size; } + +template +inline void Ring::push_front(T&& item) { assert(!full()); _begin = (_begin + N - 1) % N; (*_ring)[_begin] = item; ++_size; } + +template +inline const T& Ring::peek_front() const { assert(!empty()); return (*_ring)[_begin]; } + +template +inline T Ring::pop_front() { assert(!empty()); --_size; T item; std::swap(item, (*_ring)[_begin]); _begin = (_begin + 1) % N; return item; } + +template +inline void Ring::eff_pop_front() { --_size; _begin = (_begin + 1) % N; } + +template +inline typename Ring::iterator Ring::begin() { return iterator((T*)_ring.get(), _begin); } + +template +inline typename Ring::iterator Ring::end() { return iterator((T*)_ring.get(), _begin + _size); } + +template +inline typename Ring::const_iterator Ring::cbegin() const { return const_iterator((const T*)_ring.get(), _begin); } + +template +inline typename Ring::const_iterator Ring::cend() const { return const_iterator((const T*)_ring.get(), _begin + _size); } + +template +inline T& Ring::operator[](size_t index) { assert(index < size()); return (*_ring)[(_begin + index) % N]; } + +template +inline const T& Ring::operator[](size_t index) const { assert(index < size()); return (*_ring)[(_begin + index) % N]; } + +template +inline T* Ring::data() { return _ring->data(); } + +template +inline const T* Ring::data() const { return _ring->data(); }; + diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Container/src/ring_2d_shared.cc b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Container/src/ring_2d_shared.cc new file mode 100644 index 0000000..d282243 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Container/src/ring_2d_shared.cc @@ -0,0 +1,251 @@ +#include "ring_2d_shared.h" +#include +#include +#include +#include +#if _MSC_VER +#include +# else +#include +#include +#include +#include +#include +#include +#endif +using namespace hesai::lidar; +template +template +class Ring2D_shared::ItemIterator { +public: + using iterator_category = std::random_access_iterator_tag; + using value_type = T; + using difference_type = ptrdiff_t; + using pointer = T*; + using reference = T&; +private: + pointer _p0; + difference_type _steps; +public: + ItemIterator(const ItemIterator&) = default; + ItemIterator& operator=(const ItemIterator&) = default; + ItemIterator(pointer p0, difference_type steps) : _p0(p0), _steps(steps) {} + + inline ItemIterator& operator++() { ++_steps; return *this; } + inline ItemIterator& operator--() { assert(_steps >= 1); --_steps; return *this; } + inline ItemIterator operator++(int) { ItemIterator old(*this); ++(*this); return old; } + inline ItemIterator operator--(int) { assert(_steps >= 1); ItemIterator old(*this); --(*this); return old; } + inline ItemIterator operator+(difference_type n) const { return ItemIterator(_p0, _steps + n); } + inline ItemIterator operator-(difference_type n) const { assert(_steps >= n); return ItemIterator(_p0, _steps - n); } + inline ItemIterator& operator+=(difference_type n) { _steps += n; return *this; } + inline ItemIterator& operator-=(difference_type n) { assert(_steps >= n); _steps -= n; return *this; } + inline reference operator*() const { return _p0[_steps % N]; } + inline operator pointer () const { return _p0 + _steps % N; } + inline pointer p0() const { return _p0; } + inline difference_type steps() const { return _steps; } + inline bool operator==(const ItemIterator& other) const { return this->p0() == other.p0() && this->steps() == other.steps(); } + inline bool operator!=(const ItemIterator& other) const { return !(*this == other); } + inline difference_type operator-(const ItemIterator& other) const { assert(this->p0() == other.p0()); return this->steps() - other.steps(); } +}; + +template +Ring2D_shared::Ring2D_shared() : _ring(new std::array), _begin(0), _size(0) +{ + using namespace boost::interprocess; + std::cout << "sizeof(T2): " << sizeof(T2) << std::endl; + std::cout << "sizeof(T2) * N * M: " << sizeof(T2) * N * M << std::endl; +#if _MSC_VER + windows_shared_memory shm (open_or_create, "MySharedMemory", read_write, sizeof(T2) * N * M); + _mapped_region = mapped_region{ shm, read_write }; + _ring2 = new(_mapped_region.get_address())T2[N*M]; + std::cout << "region.get_size():" << _mapped_region.get_size() << std::endl; + std::cout << "region.get_address():" << _mapped_region.get_address() << std::endl; +#else + // shared_memory_object shm (open_or_create, "MySharedMemory", read_write); + // shm.truncate(sizeof(T2) * N * M); + + constexpr static int MY_SHM_ID = 67483; + _shmid = shmget(MY_SHM_ID, sizeof(T2) * N * M, 0666|IPC_CREAT); + if (_shmid > 0) + { + printf("Create a shared memory segment %d\n", _shmid); + } + struct shmid_ds shmds; + int ret = shmctl( _shmid, IPC_STAT, &shmds ); + + if (ret == 0 ) + { + printf( "Size of memory segment is %d \n", shmds.shm_segsz ); + printf( "Number of attaches %d \n", (int)shmds.shm_nattch ); + } + else + { + printf( "shmctl () call failed \n"); + } + void* ptr = shmat(_shmid, NULL, 0); + if (ptr == (void*)-1) + { + perror("Share memary can't get pointer\n"); + exit(1); + } + _ring2 = new(ptr)T2[N*M]; +#endif + + +}; + +template +Ring2D_shared::~Ring2D_shared() +{ + using namespace boost::interprocess; +#if _MSC_VER + shared_memory_object::remove("MySharedMemory"); +#else + int ret = shmctl(_shmid, IPC_RMID, 0); + + if (ret == 0) + { + printf("Shared memary removed \n"); + } + else + { + printf("Shared memory remove failed \n"); + } +#endif +} + +template +inline constexpr size_t Ring2D_shared::size() const { return _size; } + +template +inline bool Ring2D_shared::empty() const { return _size == 0; } + +template +inline bool Ring2D_shared::not_empty() const { return !empty(); } + +template +inline bool Ring2D_shared::full() const { return _size >= N; } + +template +inline bool Ring2D_shared::not_full() const { return !full(); } + +template +inline void Ring2D_shared::clear() { for (auto& item : *this) { item = T(); } _size = 0; _begin = 0; } + +template +inline void Ring2D_shared::eff_clear() { _size = 0; _begin = 0; } + +template +template +inline void Ring2D_shared::emplace_back(Args&&... args) { assert(!full()); (*_ring)[(_begin + _size) % N] = T(args...); ++_size;} + +template +inline void Ring2D_shared::push_back(T&& item) { assert(!full()); (*_ring)[(_begin + _size) % N] = item; ++_size; } + +template +inline const T& Ring2D_shared::peek_back() const { assert(!empty()); return (*_ring)[(_begin + _size - 1) % N]; } + +template +inline T Ring2D_shared::pop_back() { assert(!empty()); --_size; T item; std::swap(item, (*_ring)[(_begin + _size) % N]); return item; } + +template +inline void Ring2D_shared::eff_pop_back() { --_size; } + +template +template +inline void Ring2D_shared::emplace_front(Args&&... args) { assert(!full()); _begin = (_begin + N - 1) % N; (*_ring)[_begin] = T(args...); ++_size; } + +template +inline void Ring2D_shared::push_front(T&& item) { assert(!full()); _begin = (_begin + N - 1) % N; (*_ring)[_begin] = item; ++_size; } + +template +inline const T& Ring2D_shared::peek_front() const { assert(!empty()); return (*_ring)[_begin]; } + +template +inline T Ring2D_shared::pop_front() { assert(!empty()); --_size; T item; std::swap(item, (*_ring)[_begin]); _begin = (_begin + 1) % N; return item; } + +template +inline void Ring2D_shared::eff_pop_front() { --_size; _begin = (_begin + 1) % N; } + +template +inline typename Ring2D_shared::iterator Ring2D_shared::begin() { return iterator((T*)_ring.get(), _begin); } + +template +inline typename Ring2D_shared::iterator Ring2D_shared::end() { return iterator((T*)_ring.get(), _begin + _size); } + +template +inline typename Ring2D_shared::const_iterator Ring2D_shared::cbegin() const { return const_iterator((const T*)_ring.get(), _begin); } + +template +inline typename Ring2D_shared::const_iterator Ring2D_shared::cend() const { return const_iterator((const T*)_ring.get(), _begin + _size); } + +template +inline T& Ring2D_shared::operator[](size_t index) { assert(index < size()); return (*_ring)[(_begin + index) % N]; } + +template +inline const T& Ring2D_shared::operator[](size_t index) const { assert(index < size()); return (*_ring)[(_begin + index) % N]; } + +template +inline T* Ring2D_shared::data() { return _ring->data(); } + +template +inline const T* Ring2D_shared::data() const { return _ring->data(); }; + +template +inline void Ring2D_shared::prepare_emplace_back(size_t s, size_t t) +{ + assert(s <= N && t <= M); + while (_size + s > N) + { + eff_pop_front(); + } + _emplace_start = (_begin + _size) % N; + _emplace_size = s; + _2D_size = t; +}; + +template +inline void Ring2D_shared::emplace_back_at(size_t i, T&& item) +{ + assert(i < _emplace_size); + (*_ring)[(_emplace_start + i) % N] = item; +}; + +template +inline T& Ring2D_shared::peek_back_at(size_t i) +{ + assert(i < _emplace_size); + return (*_ring)[(_emplace_start + i) % N]; +}; + +template +inline void Ring2D_shared::emplace_back_at(size_t i, size_t j, T2&& item) +{ + assert((i < _emplace_size) && (j < _2D_size)); + (_ring2)[((_emplace_start + i) % N) * _2D_size + j] = item; +}; + +template +inline T2& Ring2D_shared::peek_back_at(size_t i, size_t j) +{ + assert((i < _emplace_size) && (j < _2D_size)); + return (_ring2)[((_emplace_start + i) % N) * _2D_size + j]; +}; + +template +inline void Ring2D_shared::finish_emplace_back() +{ + _size += _emplace_size; +}; + +template +inline T2* Ring2D_shared::data2() { return _ring2; } + +template +inline const T2* Ring2D_shared::data2() const { return _ring2; }; + +template +inline T2& Ring2D_shared::peek_front(size_t j) const { + assert(!empty() && j < _2D_size); + return (_ring2)[_begin * _2D_size + j]; +} diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Container/src/ring_2dex.cc b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Container/src/ring_2dex.cc new file mode 100644 index 0000000..30a667e --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Container/src/ring_2dex.cc @@ -0,0 +1,177 @@ +#include "ring_2dex.h" +#include +#include +using namespace hesai::lidar; +template +template +class Ring2D_ex::ItemIterator { +public: + using iterator_category = std::random_access_iterator_tag; + using value_type = T; + using difference_type = ptrdiff_t; + using pointer = T*; + using reference = T&; +private: + pointer _p0; + difference_type _steps; +public: + ItemIterator(const ItemIterator&) = default; + ItemIterator& operator=(const ItemIterator&) = default; + ItemIterator(pointer p0, difference_type steps) : _p0(p0), _steps(steps) {} + + inline ItemIterator& operator++() { ++_steps; return *this; } + inline ItemIterator& operator--() { assert(_steps >= 1); --_steps; return *this; } + inline ItemIterator operator++(int) { ItemIterator old(*this); ++(*this); return old; } + inline ItemIterator operator--(int) { assert(_steps >= 1); ItemIterator old(*this); --(*this); return old; } + inline ItemIterator operator+(difference_type n) const { return ItemIterator(_p0, _steps + n); } + inline ItemIterator operator-(difference_type n) const { assert(_steps >= n); return ItemIterator(_p0, _steps - n); } + inline ItemIterator& operator+=(difference_type n) { _steps += n; return *this; } + inline ItemIterator& operator-=(difference_type n) { assert(_steps >= n); _steps -= n; return *this; } + inline reference operator*() const { return _p0[_steps % N]; } + inline operator pointer () const { return _p0 + _steps % N; } + inline pointer p0() const { return _p0; } + inline difference_type steps() const { return _steps; } + inline bool operator==(const ItemIterator& other) const { return this->p0() == other.p0() && this->steps() == other.steps(); } + inline bool operator!=(const ItemIterator& other) const { return !(*this == other); } + inline difference_type operator-(const ItemIterator& other) const { assert(this->p0() == other.p0()); return this->steps() - other.steps(); } +}; + +template +Ring2D_ex::Ring2D_ex() : _ring(new std::array), _ring2(new std::array), _begin(0), _size(0) {}; + +template +inline constexpr size_t Ring2D_ex::size() const { return _size; } + +template +inline bool Ring2D_ex::empty() const { return _size == 0; } + +template +inline bool Ring2D_ex::not_empty() const { return !empty(); } + +template +inline bool Ring2D_ex::full() const { return _size >= N; } + +template +inline bool Ring2D_ex::not_full() const { return !full(); } + +template +inline void Ring2D_ex::clear() { for (auto& item : *this) { item = T(); } _size = 0; _begin = 0; } + +template +inline void Ring2D_ex::eff_clear() { _size = 0; _begin = 0; } + +template +template +inline void Ring2D_ex::emplace_back(Args&&... args) { assert(!full()); (*_ring)[(_begin + _size) % N] = T(args...); ++_size;} + +template +inline void Ring2D_ex::push_back(T&& item) { assert(!full()); (*_ring)[(_begin + _size) % N] = item; ++_size; } + +template +inline const T& Ring2D_ex::peek_back() const { assert(!empty()); return (*_ring)[(_begin + _size - 1) % N]; } + +template +inline T Ring2D_ex::pop_back() { assert(!empty()); --_size; T item; std::swap(item, (*_ring)[(_begin + _size) % N]); return item; } + +template +inline void Ring2D_ex::eff_pop_back() { --_size; } + +template +template +inline void Ring2D_ex::emplace_front(Args&&... args) { assert(!full()); _begin = (_begin + N - 1) % N; (*_ring)[_begin] = T(args...); ++_size; } + +template +inline void Ring2D_ex::push_front(T&& item) { assert(!full()); _begin = (_begin + N - 1) % N; (*_ring)[_begin] = item; ++_size; } + +template +inline const T& Ring2D_ex::peek_front() const { assert(!empty()); return (*_ring)[_begin]; } + +template +inline T Ring2D_ex::pop_front() { assert(!empty()); --_size; T item; std::swap(item, (*_ring)[_begin]); _begin = (_begin + 1) % N; return item; } + +template +inline void Ring2D_ex::eff_pop_front() { --_size; _begin = (_begin + 1) % N; } + +template +inline typename Ring2D_ex::iterator Ring2D_ex::begin() { return iterator((T*)_ring.get(), _begin); } + +template +inline typename Ring2D_ex::iterator Ring2D_ex::end() { return iterator((T*)_ring.get(), _begin + _size); } + +template +inline typename Ring2D_ex::const_iterator Ring2D_ex::cbegin() const { return const_iterator((const T*)_ring.get(), _begin); } + +template +inline typename Ring2D_ex::const_iterator Ring2D_ex::cend() const { return const_iterator((const T*)_ring.get(), _begin + _size); } + +template +inline T& Ring2D_ex::operator[](size_t index) { assert(index < size()); return (*_ring)[(_begin + index) % N]; } + +template +inline const T& Ring2D_ex::operator[](size_t index) const { assert(index < size()); return (*_ring)[(_begin + index) % N]; } + +template +inline T* Ring2D_ex::data() { return _ring->data(); } + +template +inline const T* Ring2D_ex::data() const { return _ring->data(); }; + +template +inline void Ring2D_ex::prepare_emplace_back(size_t s, size_t t) +{ + assert(s <= N && t <= M); + while (_size + s > N) + { + eff_pop_front(); + } + _emplace_start = (_begin + _size) % N; + _emplace_size = s; + _2D_size = t; +}; + +template +inline void Ring2D_ex::emplace_back_at(size_t i, T&& item) +{ + assert(i < _emplace_size); + (*_ring)[(_emplace_start + i) % N] = item; +}; + +template +inline T& Ring2D_ex::peek_back_at(size_t i) +{ + assert(i < _emplace_size); + return (*_ring)[(_emplace_start + i) % N]; +}; + +template +inline void Ring2D_ex::emplace_back_at(size_t i, size_t j, T2&& item) +{ + assert((i < _emplace_size) && (j < _2D_size)); + (*_ring2)[((_emplace_start + i) % N) * _2D_size + j] = item; +}; + +template +inline T2& Ring2D_ex::peek_back_at(size_t i, size_t j) +{ + assert((i < _emplace_size) && (j < _2D_size)); + return (*_ring2)[((_emplace_start + i) % N) * _2D_size + j]; +}; + +template +inline void Ring2D_ex::finish_emplace_back() +{ + _size += _emplace_size; +}; + +template +inline T2* Ring2D_ex::data2() { return _ring2->data(); } + +template +inline const T2* Ring2D_ex::data2() const { return _ring2->data(); }; + +template +inline T2& Ring2D_ex::peek_front(size_t j) const { + assert(!empty() && j < _2D_size); + return (*_ring2)[_begin * _2D_size + j]; +} + diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Lidar/lidar.cc b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Lidar/lidar.cc new file mode 100644 index 0000000..1733cc2 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Lidar/lidar.cc @@ -0,0 +1,543 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ + +#include "lidar_types.h" +#include "lidar.h" +#include +#include +#include "Version.h" +#ifndef _MSC_VER +#include +#endif +using namespace hesai::lidar; + +template +Lidar::Lidar() { + udp_parser_ = new UdpParser(); + is_record_pcap_ = false; + running_ = true; + udp_thread_running_ = true; + parser_thread_running_ = true; + handle_thread_count_ = 0; + recieve_packet_thread_ptr_ = nullptr; + mutex_list_ = new boost::mutex[GetAvailableCPUNum()]; + handle_buffer_size_ = kPacketBufferSize; +} + +template +Lidar::~Lidar() { + running_ = false; + udp_thread_running_ = false; + parser_thread_running_ = false; + if (recieve_packet_thread_ptr_) recieve_packet_thread_ptr_->join(); + delete recieve_packet_thread_ptr_; + recieve_packet_thread_ptr_ = nullptr; + + if (parser_thread_ptr_) parser_thread_ptr_->join(); + delete parser_thread_ptr_; + parser_thread_ptr_ = nullptr; + if (handle_thread_count_ > 1) { + for (int i = 0; i < handle_thread_count_; i++) { + if (handle_thread_vec_[i]) { + handle_thread_vec_[i]->join(); + delete handle_thread_vec_[i]; + handle_thread_vec_[i] = nullptr; + } + } + } + + if (udp_parser_ != nullptr) { + delete udp_parser_; + udp_parser_ = nullptr; + } + + if (ptc_client_ != nullptr) { + delete ptc_client_; + ptc_client_ = nullptr; + } + + if (source_ != nullptr) { + delete source_; + source_ = nullptr; + } + + if (mutex_list_ != nullptr) { + delete[] mutex_list_; + mutex_list_ = nullptr; + } + Logger::GetInstance().Stop(); +} + +template +std::string Lidar::GetLidarType() { + return udp_parser_->GetLidarType(); +} + +template +int Lidar::Init(const DriverParam& param) { + int res = -1; + /*******************************Init log*********************************************/ + Logger::GetInstance().SetFileName(param.log_path.c_str()); + Logger::GetInstance().setLogTargetRule(param.log_Target); + Logger::GetInstance().setLogLevelRule(param.log_level); + // Logger::GetInstance().bindLogCallback(logCallback); + Logger::GetInstance().Start(); + /**********************************************************************************/ + + /***************************Init source****************************************/ + int packet_interval = 10; + udp_port_ = param.input_param.udp_port; + if (param.input_param.source_type == 2) { + source_ = new PcapSource(param.input_param.pcap_path, packet_interval); + source_->Open(); + } + else if(param.input_param.source_type == 1){ + ptc_client_ = new (std::nothrow) PtcClient(param.input_param.device_ip_address + , param.input_param.ptc_port + , false + , param.input_param.ptc_mode + , 1 + , param.input_param.certFile + , param.input_param.privateKeyFile + , param.input_param.caFile); + + // ptc_client_ = new (std::nothrow) PtcClient(param.input_param.device_ip_address, param.input_param.ptc_port); + source_ = new SocketSource(param.input_param.udp_port, param.input_param.multicast_ip_address); + source_->Open(); + } + parser_thread_running_ = param.decoder_param.enable_parser_thread; + udp_thread_running_ = param.decoder_param.enable_udp_thread; + udp_parser_->SetTransformPara(param.decoder_param.transform_param.x, \ + param.decoder_param.transform_param.y, \ + param.decoder_param.transform_param.z, \ + param.decoder_param.transform_param.roll, \ + param.decoder_param.transform_param.pitch, \ + param.decoder_param.transform_param.yaw); + SetThreadNum(param.decoder_param.thread_num); + /********************************************************************************/ + + /***************************Init decoder****************************************/ + clock_t start_time, end_time; + double time_interval = 0; + UdpPacket udp_packet; + LidarDecodedPacket decoded_packet; + start_time = clock(); + while (udp_parser_->GetParser() == nullptr) { + int ret = this->GetOnePacket(udp_packet); + if (ret == -1) continue; + this->DecodePacket(decoded_packet, udp_packet); + end_time = clock(); + time_interval = double(end_time-start_time) / CLOCKS_PER_SEC; + } + if (udp_parser_->GetParser() == nullptr) { + return res; + } + switch (param.input_param.source_type) + { + case 1: + if (LoadCorrectionForUdpParser() == -1) { + std::cout << "---Failed to obtain correction file from lidar!---" << std::endl; + LoadCorrectionFile(param.input_param.correction_file_path); + } + break; + case 2: + LoadCorrectionFile(param.input_param.correction_file_path); + break; + case 3: + LoadCorrectionFile(param.input_param.correction_file_path); + default: + break; + } + LoadFiretimesFile(param.input_param.firetimes_path); + /********************************************************************************/ + udp_parser_->SetPcapPlay(param.decoder_param.pcap_play_synchronization, param.input_param.source_type); + udp_parser_->SetFrameAzimuth(param.decoder_param.frame_start_azimuth); + udp_parser_->GetParser()->EnablePacketLossTool(param.decoder_param.enable_packet_loss_tool); + res = 0; + return res; +} + +template +int Lidar::LoadCorrectionForUdpParser() { + u8Array_t sData; + if (ptc_client_->GetCorrectionInfo(sData) != 0) { + std::cout << __func__ << "get correction info fail\n"; + return -1; + } + if (udp_parser_) { + return udp_parser_->LoadCorrectionString( + (char *)sData.data()); + } else { + std::cout << __func__ << "udp_parser_ nullptr\n"; + return -1; + } + return 0; +} + +template +int Lidar::SaveCorrectionFile(std::string correction_save_path) { + int ret = -1; + u8Array_t raw_data; + if (ptc_client_->GetCorrectionInfo(raw_data) != 0) { + std::cout << __func__ << "get correction info fail\n"; + return ret; + } + std::string correction_content_str = (char*) raw_data.data(); + std::ofstream out_file(correction_save_path, std::ios::out); + if(out_file.is_open()) { + out_file << correction_content_str; + ret = 0; + out_file.close(); + return ret; + } else { + std::cout << __func__ << "create correction file fail\n"; + return ret; + } +} + +template +int Lidar::SetLidarType(std::string lidar_type) { + int ret = -1; + if (udp_parser_) { + udp_parser_->CreatGeneralParser(lidar_type); + ret = 0; + } else + std::cout << __func__ << "udp_parser_ nullptr\n"; + + return ret; +} + + +template +int Lidar::GetOnePacket(UdpPacket &packet) { + if (origin_packets_buffer_.try_pop_front(packet)) return 0; + else return -1; +} + +template +int Lidar::StartRecordPcap(std::string record_path) { + int ret = -1; + if (udp_parser_) { + udp_parser_->GetPcapSaver()->SetPcapPath(record_path); + EnableRecordPcap(true); + udp_parser_->GetPcapSaver()->Save(); + } else + std::cout << __func__ << "udp_parser_ nullptr\n"; + + return ret; +} + +template +int Lidar::SaveUdpPacket(const std::string &record_path, + const UdpFrameArray_t &packets, int port) { + int ret = -1; + if (udp_parser_) { + ret = udp_parser_->GetPcapSaver()->Save(record_path, packets, + port); + } else + std::cout << __func__ << "udp_parser_ nullptr\n"; + + return ret; +} + +template +int Lidar::ComputeXYZI(LidarDecodedPacket &packet) { + int ret = -1; + decoded_packets_buffer_.push_back(std::move(packet)); + return 0; + +} + +template +int Lidar::DecodePacket(LidarDecodedPacket &output, const UdpPacket& udp_packet) { + if (udp_parser_) { + udp_parser_->DecodePacket(output,udp_packet); + return 0; + } else + std::cout << __func__ << "udp_parser_ nullptr\n"; + + return -1; +} + +template +int Lidar::DecodePacket(LidarDecodedFrame &frame, const UdpPacket& udp_packet) { + if (udp_parser_) { + udp_parser_->DecodePacket(frame,udp_packet); + return 0; + } else + std::cout << __func__ << "udp_parser_ nullptr\n"; + + return -1; +} + + +template +bool Lidar::ComputeXYZIComplete(int index) { + return frame_.packet_num == index; +} + +template +void Lidar::LoadCorrectionFile(std::string correction_path) { + if (udp_parser_) { + udp_parser_->LoadCorrectionFile(correction_path); + return ; + } else + std::cout << __func__ << "udp_parser_ nullptr\n"; + + return ; +} + +template +int Lidar::LoadCorrectionString(char *correction_string) { + int ret = -1; + if (udp_parser_) { + return udp_parser_->LoadCorrectionString(correction_string); + + } else + std::cout << __func__ << "udp_parser_ nullptr\n"; + + return ret; +} + +template +void Lidar::LoadFiretimesFile(std::string firetimes_path) { + if (udp_parser_) { + udp_parser_->LoadFiretimesFile(firetimes_path); + return ; + } else + std::cout << __func__ << "udp_parser_ nullptr\n"; + + return ; +} + +template +int Lidar::SaveUdpPacket(const std::string &record_path, + const UdpFrame_t &packets, int port) { + int ret = -1; + if (udp_parser_) { + ret = udp_parser_->GetPcapSaver()->Save(record_path, packets, + port); + } else + std::cout << __func__ << "udp_parser_ nullptr\n"; + + return ret; +} + +template +int Lidar::StopRecordPcap() { + int ret = -1; + if (udp_parser_) { + EnableRecordPcap(false); + udp_parser_->GetPcapSaver()->close(); + } else + std::cout << __func__ << "udp_parser_ nullptr\n"; + + return ret; +} + +template +int Lidar::GetGeneralParser(GeneralParser **parser) { + int ret = 0; + if (udp_parser_) + ret = udp_parser_->GetGeneralParser(parser); + else + std::cout << __func__ << "udp_parser_ nullptr\n"; + + return ret; +} + +template +void Lidar::RecieveUdpThread() { + if(!udp_thread_running_) return; + uint32_t u32StartTime = GetMicroTickCount(); + std::cout << "Lidar::Recieve Udp Thread start to run\n"; +#ifdef _MSC_VER + SetThreadPriorityWin(THREAD_PRIORITY_TIME_CRITICAL); +#else + SetThreadPriority(SCHED_FIFO, SHED_FIFO_PRIORITY_MEDIUM); +#endif + while (running_) { + if (source_ == nullptr) { + std::this_thread::sleep_for(std::chrono::microseconds(1000)); + continue; + } + UdpPacket udp_packet; + int len = source_->Receive(udp_packet, kBufSize); + if (len == -1) { + std::this_thread::sleep_for(std::chrono::microseconds(1000)); + continue; + } + while(origin_packets_buffer_.full() && running_) std::this_thread::sleep_for(std::chrono::microseconds(1000)); + if(running_ == false) break; + + switch (len) { + case 0: + if (is_timeout_ == false) { + udp_packet.packet_len = AT128E2X_PACKET_LEN; + origin_packets_buffer_.emplace_back(udp_packet); + is_timeout_ = true; + } + break; + case kFaultMessageLength: + udp_packet.packet_len = len; + origin_packets_buffer_.emplace_back(udp_packet); + break; + case GPS_PACKET_LEN: + break; + default : + if (len > 0) { + udp_packet.packet_len = len; + origin_packets_buffer_.emplace_back(udp_packet); + is_timeout_ = false; + } + break; + } + + if (udp_packet.packet_len > 0 && is_record_pcap_) { + udp_parser_->GetPcapSaver()->Dump(udp_packet.buffer, udp_packet.packet_len, udp_port_); + } + } + return; +} + +template +void Lidar::ParserThread() { + if(!parser_thread_running_) return; + int nUDPCount = 0; + std::cout << "Lidar::ParserThread start to run\n"; +#ifdef _MSC_VER + SetThreadPriorityWin(THREAD_PRIORITY_TIME_CRITICAL); +#else + SetThreadPriority(SCHED_FIFO, SHED_FIFO_PRIORITY_MEDIUM); +#endif + while (running_) { + LidarDecodedPacket decoded_packet; + decoded_packets_buffer_.try_pop_front(decoded_packet); + if (handle_thread_count_ < 2) { + udp_parser_->ComputeXYZI(frame_, decoded_packet); + continue; + } else { + nUDPCount = nUDPCount % handle_thread_count_; + mutex_list_[nUDPCount].lock(); + handle_thread_packet_buffer_[nUDPCount].push_back(decoded_packet); + + if (handle_thread_packet_buffer_[nUDPCount].size() > handle_buffer_size_) { + handle_thread_packet_buffer_[nUDPCount].pop_front(); + } + mutex_list_[nUDPCount].unlock(); + nUDPCount++; + } + } + return; +} + +template +void Lidar::HandleThread(int nThreadNum) { + struct timespec timeout; +#ifdef _MSC_VER + SetThreadPriorityWin(THREAD_PRIORITY_TIME_CRITICAL); +#else + SetThreadPriority(SCHED_FIFO, SHED_FIFO_PRIORITY_MEDIUM); +#endif + if(!parser_thread_running_) return; + while (running_) { + LidarDecodedPacket decoded_packet; + mutex_list_[nThreadNum].lock(); + if (handle_thread_packet_buffer_[nThreadNum].size() > 0) { + decoded_packet = handle_thread_packet_buffer_[nThreadNum].front(); + handle_thread_packet_buffer_[nThreadNum].pop_front(); + udp_parser_->ComputeXYZI(frame_, decoded_packet); + } + mutex_list_[nThreadNum].unlock(); + } +} + +template +void Lidar::SetThreadNum(int nThreadNum) { + if (nThreadNum > GetAvailableCPUNum() - 2) { + nThreadNum = GetAvailableCPUNum() - 2; + } + + if (handle_thread_count_ == nThreadNum) { + return; + } + + running_ = false; + + if (recieve_packet_thread_ptr_ != nullptr) { + recieve_packet_thread_ptr_->join(); + delete recieve_packet_thread_ptr_; + recieve_packet_thread_ptr_ = nullptr; + } + + for (int i = 0; i < handle_thread_count_; i++) { + if (handle_thread_vec_[i] != nullptr) { + handle_thread_vec_[i]->join(); + delete handle_thread_vec_[i]; + handle_thread_vec_[i] = nullptr; + } + + handle_thread_packet_buffer_[i].clear(); + } + + running_ = true; + if (nThreadNum > 1) { + handle_thread_vec_.resize(nThreadNum); + handle_thread_packet_buffer_.resize(nThreadNum); + + for (int i = 0; i < nThreadNum; i++) { + boost::arg<1> _1; + handle_thread_vec_[i] = new boost::thread( + boost::bind(&Lidar::HandleThread, this, _1), i); + } + } + + handle_thread_count_ = nThreadNum; + recieve_packet_thread_ptr_ = + new boost::thread(boost::bind(&Lidar::RecieveUdpThread, this)); + parser_thread_ptr_ = + new boost::thread(boost::bind(&Lidar::ParserThread, this)); +} +template +void Lidar::SetSource(Source **source) { + source_ = *source; +} + +template +UdpParser *Lidar::GetUdpParser() { return udp_parser_; } + +template +void Lidar::SetUdpParser(UdpParser *udpParser) { + udp_parser_ = udpParser; +} + +template +void Lidar::EnableRecordPcap(bool bRecord) { + is_record_pcap_ = bRecord; +} + diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Lidar/lidar.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Lidar/lidar.h new file mode 100644 index 0000000..90a1cfc --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Lidar/lidar.h @@ -0,0 +1,172 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ + +/* + * File: lidar.h + * Author: Zhang Yu + * Description: Define Lidar class + */ + +#ifndef Lidar_H +#define Lidar_H +#include +#include +#include "lidar_types.h" +#include "ptc_client.h" +#include +#include +#include +#include "tcp_client.h" +#include "udp_parser.h" +#include "pcap_source.h" +#include "socket_source.h" +#include "blocking_ring.h" +#include "ring.h" +#include "ptc_client.h" +#include "driver_param.h" +#ifndef _MSC_VER +#include +#include +#endif +#define PKT_SIZE_40P (1262) +#define PKT_SIZE_AC (1256) +#define PKT_SIZE_64 (1194) +#define PKT_SIZE_20 (1270) +#define AT128E2X_PACKET_LEN (1180) +#define FAULTMESSAGE_PACKET_LEN (99) +#define GPS_PACKET_LEN (512) + + +namespace hesai +{ +namespace lidar +{ + +// class Lidar +// the Lidar class will start a udp data recieve thread and parser thread when init() +// udp packet data will be recived in udp data recieve thread and put into origin_packets_buffer_ +// you can get origin udp packet data from origin_packets_buffer_ when you need +// you can put decoded packet into decoded_packets_buffer_ , parser thread will convert it pointsxyzi info in variable frame_ +// you can control lidar with variable ptc_client_ +template +class Lidar +{ +public: + ~Lidar(); + Lidar(); + Lidar(const Lidar &orig) = delete; + // init lidar with param. init logger, udp parser, source, ptc client, start receive/parser thread + int Init(const DriverParam& param); + int GetGeneralParser(GeneralParser **parser); + // set lidar type for Lidar object and udp parser, udp parser will initialize the corresponding subclass + int SetLidarType(std::string lidar_type); + // get one packet from origin_packets_buffer_, + int GetOnePacket(UdpPacket &packet); + // start to record pcap file with name record_path,the record source is udp data recieve from source + int StartRecordPcap(std::string record_path); + // stop record pcap + int StopRecordPcap(); + // start to record pcap file with name record_path,the record source is the parameter UdpFrame_t + // port is the udp port recorded in pcap + int SaveUdpPacket(const std::string &record_path, const UdpFrame_t &packets, + int port = 2368); + // start to record pcap file with name record_path,the record source is the parameter UdpFrameArray_t + // port is the udp port recorded in pcap + int SaveUdpPacket(const std::string &record_path, + const UdpFrameArray_t &packets, int port = 2368); + /* this founction whill put a LidarDecodedPacket into decoded_packets_buffer_, and then the parser thread will + convert decoded packet dato into pointxyzi info*/ + int ComputeXYZI(LidarDecodedPacket &packet); + // covert a origin udp packet to decoded packet, the decode function is in UdpParser module + // udp_packet is the origin udp packet, output is the decoded packet + int DecodePacket(LidarDecodedPacket &output, const UdpPacket& udp_packet); + // covert a origin udp packet to decoded data, and pass the decoded data to a frame struct to reduce memory copy + int DecodePacket(LidarDecodedFrame &frame, const UdpPacket& udp_packet); + // Determine whether all pointxyzi info is parsed in this frame + bool ComputeXYZIComplete(int index); + // save lidar correction file from ptc + int SaveCorrectionFile(std::string correction_save_path); + // get lidar correction file from ptc,and pass to udp parser + int LoadCorrectionForUdpParser(); + // get lidar correction file from local file,and pass to udp parser + void LoadCorrectionFile(std::string correction_path); + int LoadCorrectionString(char *correction_string); + // get lidar firetime correction file from local file,and pass to udp parser + void LoadFiretimesFile(std::string firetimes_path); + void SetUdpParser(UdpParser *udp_parser); + UdpParser *GetUdpParser(); + void EnableRecordPcap(bool bRecord); + // set the parser thread number + void SetThreadNum(int thread_num); + void SetSource(Source **source); + std::string GetLidarType(); + UdpParser *udp_parser_; + Source *source_; + PtcClient *ptc_client_; + LidarDecodedFrame frame_; + BlockingRing origin_packets_buffer_; + +private: + uint16_t ptc_port_; + uint16_t udp_port_; + std::string device_ip_address_; + std::string host_ip_address_; + // recieve udp data thread, this function will recieve udp data in while(),exit when variable running_ is false + void RecieveUdpThread(); + /* parser decoded packet data thread. when thread num < 2, this function will parser decoded packet data in this thread + when thread num >= 2,decoded packet data will be put in handle_thread_packet_buffer_, and the decoded packet will be parered in handle_thread*/ + void ParserThread(); + /* this function will parser decoded packet data in handle_thread_packet_buffer_ into pointxyzi info */ + void HandleThread(int thread_num); + BlockingRing, kPacketBufferSize> decoded_packets_buffer_; + + // this variable is the exit condition of udp/parser thread + bool running_; + // this variable decide whether recieve_packet_thread will run + bool udp_thread_running_; + // this variable decide whether parser_thread will run + bool parser_thread_running_; + boost::thread *recieve_packet_thread_ptr_; + boost::thread *parser_thread_ptr_; + boost::mutex *mutex_list_; + std::vector>> handle_thread_packet_buffer_; + std::vector handle_thread_vec_; + uint32_t handle_buffer_size_; + int handle_thread_count_; + bool is_record_pcap_; + std::string lidar_type_; + bool is_timeout_ = false; + +}; +} // namespace lidar +} // namespace hesai + +#include "lidar.cc" +#endif /* Lidar_H */ + + diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Lidar/lidar_types.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Lidar/lidar_types.h new file mode 100644 index 0000000..2d34a2d --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Lidar/lidar_types.h @@ -0,0 +1,349 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ + +/* + * File: lidar.h + * Author: Zhang Yu + * Description: + */ + +#ifndef LIDAR_TYPES_H_ +#define LIDAR_TYPES_H_ +#include +#include +#include +#include +#include +#include +#include +#define CHANNEL_NUM 256 +#define PACKET_NUM 3600 +namespace hesai +{ +namespace lidar +{ + +//max point num of one packet, laser_num * block_num <= kMaxPointsNumPerPacket +static constexpr uint16_t kMaxPointsNumPerPacket = 512; +//max packet num of one frame, it means the capacity of frame buffer +static constexpr uint16_t kMaxPacketNumPerFrame = 4000; +//max points num of one frame +static constexpr uint32_t kMaxPointsNumPerFrame = kMaxPointsNumPerPacket * kMaxPacketNumPerFrame; +//half of the max value in degrees, 1 LSB represents 0.01 degree, float type +static constexpr float kHalfCircleFloat = 18000.0f; +//half of the max value in degrees, 1 LSB represents 0.01 degree, int type +static constexpr int kHalfCircleInt = 18000; +//max value in degrees, 1 LSB represents 0.01 degree +static constexpr int kCircle = 36000; +//laser azimuth resolution, 100 units represents 1 degree, int type +static constexpr int kResolutionInt = 100; +//laser azimuth resolution, 100 units represents 1 degree, float type +static constexpr float kResolutionFloat = 100.0f; +//conversion factor between second and micorsecond +static constexpr float kMicrosecondToSecond = 1000000.0f; +static constexpr int kMicrosecondToSecondInt = 1000000; +//the difference between last azimuth and current azimuth must be greater than this angle in split frame function, +//to avoid split frame unsuccessfully +static constexpr uint16_t kSplitFrameMinAngle = 300; +//laser fine azimuth resolution, 1 LSB represents 0.01 / 256 degree, float type +static constexpr float kFineResolutionFloat = 256.0f; +//laser fine azimuth resolution, 1 LSB represents 0.01 / 256 degree, int type +static constexpr int kFineResolutionInt = 256; +//synchronize host time with sensor time per kPcapPlaySynchronizationCount packets +static constexpr int kPcapPlaySynchronizationCount = 100; +//min points of one frame for displaying frame message +static constexpr int kMinPointsOfOneFrame = 1000; +//max time interval between two frame +static constexpr int kMaxTimeInterval = 150000; + +//length of fault message packet +static constexpr int kFaultMessageLength = 99; + +static constexpr int kPacketBufferSize = 36000; +//default udp data max lenth +static const uint16_t kBufSize = 1500; +typedef struct LidarPointXYZI +{ + float x; + float y; + float z; + float intensity; +} LidarPointXYZI; + +typedef struct LidarPointXYZIRT +{ + float x; + float y; + float z; + float intensity; + uint16_t ring; + double timestamp; +} LidarPointXYZIRT; + +typedef struct LidarPointRTHI +{ + int theta; + int phi; + int radius; + int intensity; +} LidarPointRTHI; +template +struct LidarDecodedPacket +{ + uint64_t host_timestamp; + uint64_t sensor_timestamp; + float duration; + double distance_unit; + uint32_t maxPoints; + uint32_t points_num; + uint16_t block_num; + uint16_t laser_num; + int packet_index; + bool scan_complete; // when this packet is the last packet in one frame, this value should be true + uint8_t reflectivities[kMaxPointsNumPerPacket]; + uint16_t distances[kMaxPointsNumPerPacket]; + float azimuth[kMaxPointsNumPerPacket]; + float elevation[kMaxPointsNumPerPacket]; + uint16_t azimuths; + uint16_t spin_speed; + uint8_t lidar_state; + uint8_t work_mode; + bool IsDecodedPacketValid() { + return block_num != 0; + } +}; + +template +class LidarDecodedFrame +{ + public: + LidarDecodedFrame() { + points_num = 0; + packet_index = 0; + distance_unit = 0.0; + points = new PointT[kMaxPacketNumPerFrame * kMaxPointsNumPerPacket]; + sensor_timestamp = new uint64_t[kMaxPacketNumPerFrame]; + azimuths = new uint16_t[kMaxPacketNumPerFrame]; + azimuth = new float[kMaxPacketNumPerFrame * kMaxPointsNumPerPacket]; + elevation = new float[kMaxPacketNumPerFrame * kMaxPointsNumPerPacket]; + distances = new uint16_t[kMaxPacketNumPerFrame * kMaxPointsNumPerPacket]; + reflectivities = new uint8_t[kMaxPacketNumPerFrame * kMaxPointsNumPerPacket]; + host_timestamp = 0; + major_version = 0; + minor_version = 0; + return_mode = 0; + spin_speed = 0; + points_num = 0; + packet_num = 0; + block_num = 0; + laser_num = 0; + packet_index = 0; + scan_complete = false; + distance_unit = 0; + frame_index = 0; + }; + ~LidarDecodedFrame() { + delete points; + points = nullptr; + delete sensor_timestamp; + sensor_timestamp = nullptr; + delete azimuths; + azimuths = nullptr; + delete distances; + distances = nullptr; + delete reflectivities; + reflectivities = nullptr; + delete azimuth; + azimuth = nullptr; + delete elevation; + elevation = nullptr; + } + void Update(){ + host_timestamp = 0; + major_version = 0; + minor_version = 0; + return_mode = 0; + spin_speed = 0; + points_num = 0; + packet_num = -1; + block_num = 0; + laser_num = 0; + packet_index = 0; + scan_complete = false; + distance_unit = 0; + lidar_state = -1; + work_mode = -1; + frame_index++; + } + uint64_t host_timestamp; + uint64_t* sensor_timestamp; + uint8_t major_version; + uint8_t minor_version; + uint16_t return_mode; + uint16_t spin_speed; + uint32_t points_num; + uint32_t packet_num; + PointT* points; + uint16_t* azimuths; + uint8_t* reflectivities; + float* azimuth; + float* elevation; + uint16_t* distances; + uint16_t block_num; + uint16_t laser_num; + uint16_t packet_index; + bool scan_complete; + double distance_unit; + int frame_index; + uint8_t lidar_state; + uint8_t work_mode; +}; + + +struct UdpPacket { + uint8_t buffer[1500]; + int16_t packet_len; + bool is_timeout = false; + UdpPacket(const uint8_t* data = nullptr, uint32_t sz = 0) + : packet_len(sz) + { + memcpy(buffer, data, packet_len); + } +}; + +typedef std::vector u8Array_t; +typedef std::vector u16Array_t; +typedef std::vector u32Array_t; +typedef std::vector doubleArray_t; +typedef std::vector u8ArrayArray_t; +typedef std::vector doubleArrayArray_t; +typedef std::vector u16ArrayArray_t; +typedef std::vector stringArray_t; +typedef std::vector UdpFrame_t; +typedef std::vector UdpFrameArray_t; + +#define PANDAR_AT128_LIDAR_NUM (128) +#define LENS_AZIMUTH_AREA_NUM (12) +#define LENS_ELEVATION_AREA_NUM (8) + +enum LidarOperateState { + kBoot, + kInit, + kFullPerformance, + kHalfPower, + kSleepMode, + kHighTempertureShutdown, + kFaultShutdown, + kUndefineOperateState = -1, +}; + +enum LidarFaultState { + kNormal, + kWarning, + kPrePerformanceDegradation, + kPerformanceDegradation, + kPreShutDown, + kShutDown, + kPreReset, + kReset, + kUndefineFaultState = -1, +}; + +enum FaultCodeType { + kUndefineFaultCode = -1, + kCurrentFaultCode = 1, + kHistoryFaultCode = 2, +}; + +enum DTCState { + kNoFault, + kFault, +}; + +enum TDMDataIndicate { + kInvaild = 0, + kLensDirtyInfo = 1, + kUndefineIndicate = -1, +}; + +enum LensDirtyState { + kUndefineData = -1, + kLensNormal = 0, + kPassable = 1, + kUnPassable = 3, +}; + +enum HeatingState { + kOff = 0, + kHeating = 1, + kHeatingProhibit = 2, + kUndefineHeatingState = -1, +}; + +enum HighTempertureShutdownState { + kPreShutdown = 1, + kShutdownMode1 = 2, + kShutdownMode2 = 6, + kShutdownMode2Fail = 10, + kUndefineShutdownData = -1, +}; + +struct FaultMessageInfo { + uint8_t version; + uint8_t utc_time[6]; + uint32_t timestamp; + double total_time; + LidarOperateState operate_state; + LidarFaultState fault_state; + FaultCodeType faultcode_type; + uint8_t rolling_counter; + uint8_t total_faultcode_num; + uint8_t faultcode_id; + uint32_t faultcode; + int dtc_num; + DTCState dtc_state; + TDMDataIndicate tdm_data_indicate; + double temperature; + LensDirtyState lens_dirty_state[LENS_AZIMUTH_AREA_NUM] + [LENS_ELEVATION_AREA_NUM]; + uint16_t software_id; + uint16_t software_version; + uint16_t hardware_version; + uint16_t bt_version; + HeatingState heating_state; + HighTempertureShutdownState high_temperture_shutdown_state; + uint8_t reversed[3]; + uint32_t crc; + uint8_t cycber_security[32]; +}; + +} // namespace lidar +} // namespace hesai + + +#endif // LIDAR_TYPES_H_ diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Logger/include/logger.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Logger/include/logger.h new file mode 100644 index 0000000..4285902 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Logger/include/logger.h @@ -0,0 +1,102 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ +#ifndef __LOGGER_H__ +#define __LOGGER_H__ + +#include +#include +#include +#include +#include +#include +#include +#include + +//struct FILE; + +enum LOGLEVEL +{ + LOG_DEBUG = 0x01, /*debug*/ + LOG_INFO = 0x02, /*info*/ + LOG_WARNING = 0x04, /*warning*/ + LOG_ERROR = 0x08, /*error*/ + LOG_FATAL = 0x10, /*assert*/ +}; + +enum LOGTARGET +{ + LOG_TARGET_NONE = 0x00, + LOG_TARGET_CONSOLE = 0x01, + LOG_TARGET_FILE = 0x10 +}; + +#define LogDebug(...) Logger::GetInstance().AddToQueue(LOG_DEBUG, __FILE__, __LINE__, __PRETTY_FUNCTION__, __VA_ARGS__) +#define LogInfo(...) Logger::GetInstance().AddToQueue(LOG_INFO, __FILE__, __LINE__, __PRETTY_FUNCTION__, __VA_ARGS__) +#define LogWarning(...) Logger::GetInstance().AddToQueue(LOG_WARNING, __FILE__, __LINE__, __PRETTY_FUNCTION__, __VA_ARGS__) +#define LogError(...) Logger::GetInstance().AddToQueue(LOG_ERROR, __FILE__, __LINE__, __PRETTY_FUNCTION__, __VA_ARGS__) +#define LogFatal(...) Logger::GetInstance().AddToQueue(LOG_FATAL, __FILE__, __LINE__, __PRETTY_FUNCTION__, __VA_ARGS__) + +class Logger +{ +public: + static Logger& GetInstance(); + + void SetFileName(const char* filename); + + bool Start(); + void Stop(); + + void AddToQueue(LOGLEVEL loglevel, const char* pszFile, int lineNo, const char* pszFuncSig, char* pszFmt, ...); + void setLogLevelRule(uint8_t rule); + void setLogTargetRule(uint8_t rule); + void bindLogCallback(std::function log_callback); + +private: + Logger() = default; + Logger(const Logger& rhs) = delete; + Logger& operator =(Logger& rhs) = delete; + + void threadfunc(); + + +private: + std::string filename_; + FILE* fp_{}; + std::shared_ptr spthread_; + std::mutex mutex_; + std::condition_variable cv_; //有新的日志到来的标识 + bool exit_{false}; + std::list queue_; + uint8_t log_level_rule_; + uint8_t log_target_rule_; + std::function log_callback_; + bool running_{false}; + +}; + +#endif \ No newline at end of file diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Logger/src/logger.cc b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Logger/src/logger.cc new file mode 100644 index 0000000..8824a2b --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Logger/src/logger.cc @@ -0,0 +1,180 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ +#include "logger.h" +#include +#include +#include +#include + +Logger& Logger::GetInstance() +{ + static Logger logger; + return logger; +} + +void Logger::SetFileName(const char* filename) +{ + filename_ = filename; +} + +bool Logger::Start() +{ + + if (running_ == true) return true; + if (filename_.empty()) + { + time_t now = time(NULL); + struct tm* t = localtime(&now); + char timestr[64] = { 0 }; + sprintf(timestr, "%04d%02d%02d%02d%02d%02d.imserver.log", t->tm_year + 1900, t->tm_mon + 1, t->tm_mday, t->tm_hour, t->tm_min, t->tm_sec); + filename_ = timestr; + } + fp_ = fopen(filename_.c_str(), "wt+"); + if (fp_ == NULL) + return false; + + spthread_.reset(new std::thread(std::bind(&Logger::threadfunc, this))); + printf("logger start to run\n"); + running_ = true; + + return true; +} + +void Logger::Stop() +{ + exit_ = true; + cv_.notify_one(); + + //等待时间线程结束 + spthread_->join(); +} + +void Logger::AddToQueue(LOGLEVEL loglevel, const char* pszFile, int lineNo, const char* pszFuncSig, char* pszFmt, ...) +{ + + if ((loglevel & log_level_rule_) != loglevel) return; + char msg[256] = { 0 }; + + va_list vArgList; + va_start(vArgList, pszFmt); + vsnprintf(msg, 256, pszFmt, vArgList); + va_end(vArgList); + if (log_callback_ != nullptr) { + log_callback_(loglevel, pszFile, lineNo, pszFuncSig, msg); + return; + } + + time_t now = time(NULL); + struct tm* tmstr = localtime(&now); + char content[512] = { 0 }; + char* logLevel; + if (loglevel == LOG_DEBUG){ + logLevel = "DEBUG"; + } + else if (loglevel == LOG_INFO){ + logLevel = "INFO"; + } + else if (loglevel == LOG_WARNING){ + logLevel = "WARNING"; + } + else if (loglevel == LOG_ERROR){ + logLevel = "ERROR"; + } + else if (loglevel == LOG_FATAL){ + logLevel = "FATAL"; + } + sprintf(content, "[%04d-%02d-%02d %02d:%02d:%02d][%s][0x%04x][%s:%d %s]%s", + tmstr->tm_year + 1900, + tmstr->tm_mon + 1, + tmstr->tm_mday, + tmstr->tm_hour, + tmstr->tm_min, + tmstr->tm_sec, + logLevel, + std::this_thread::get_id(), + pszFile, + lineNo, + pszFuncSig, + msg); + + + if (log_target_rule_ & LOG_TARGET_FILE) + { + { + std::lock_guard guard(mutex_); + queue_.emplace_back(content); + } + + cv_.notify_one(); + } + if (log_target_rule_ & LOG_TARGET_CONSOLE) + { + printf("%s\n", content); + } +} + +void Logger::threadfunc() +{ + if (fp_ == NULL) + return; + + while (!exit_) + { + //写日志 + std::unique_lock guard(mutex_); + while (queue_.empty()) + { + if (exit_) + return; + + cv_.wait(guard); + } + + //写日志 + const std::string& str = queue_.front(); + + fwrite((void*)str.c_str(), str.length(), 1, fp_); + fflush(fp_); + queue_.pop_front(); + } +} + + +void Logger::setLogTargetRule(uint8_t rule) +{ + log_target_rule_ = rule; +} + +void Logger::setLogLevelRule(uint8_t rule) +{ + log_level_rule_ = rule; +} + +void Logger::bindLogCallback(std::function log_callback) { + log_callback_ = log_callback; +} \ No newline at end of file diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcClient/include/client_base.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcClient/include/client_base.h new file mode 100644 index 0000000..b3400b1 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcClient/include/client_base.h @@ -0,0 +1,79 @@ +/* + * Copyright (C) 2019 Hesai Tech + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + */ + +/* + * File: client_base.h + * Author: zhang xu + * + * Created on Jul 9, 2023, 20:03 PM + */ + +#ifndef CLIENT_BASE_H +#define CLIENT_BASE_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +namespace hesai +{ +namespace lidar +{ +class ClientBase { + public: + explicit ClientBase(){} + virtual ~ClientBase(){} + virtual bool Open(std::string IPAddr, uint16_t port, bool bAutoReceive = false, + const char* cert = nullptr, const char* private_key = nullptr, const char* ca = nullptr) = 0; + virtual void Close() = 0; + virtual bool IsOpened() = 0; + virtual int Send(uint8_t *u8Buf, uint16_t u16Len, int flags = 0) = 0; + virtual int Receive(uint8_t *u8Buf, uint32_t u16Len, int flags = 0) = 0; + /** + * @brief 设置接收超时 + * @param u32Timeout 超时时间/ms + * @return + */ + virtual bool SetReceiveTimeout(uint32_t u32Timeout) = 0; + + /** + * @brief 设置收发超时 + * @param u32RecMillisecond 接收超时/ms + * @param u32SendMillisecond 发送超时/ms + * @return + */ + virtual int SetTimeout(uint32_t u32RecMillisecond, uint32_t u32SendMillisecond) = 0; + + /** + * @brief 设置自动接收模式下Buff的大小 + * @param size + */ + virtual void SetReceiveBufferSize(const uint32_t &size) = 0; +}; + +} +} + +#endif \ No newline at end of file diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcClient/include/lidar_communication_header.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcClient/include/lidar_communication_header.h new file mode 100644 index 0000000..3c65b9e --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcClient/include/lidar_communication_header.h @@ -0,0 +1,115 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ + +/* + * File: lidar_communication_header.h + * Author: Felix Zou + * Description: + */ + +#ifndef LIDARCOMMUNICATIONHEADER_H +#define LIDARCOMMUNICATIONHEADER_H + +#include + +#ifdef _MSC_VER +#define PACKED +#pragma pack(push, 1) +#else +#define PACKED __attribute__((packed)) +#endif + +struct LidarCommunicationHeader { + // [5:4] 00:NOP, 2'b01:read, 2'b10:write + static const uint8_t kNOP = 0x00; + static const uint8_t kRead = 0x10; + static const uint8_t kWrite = 0x20; + // [3:2] operation width, 0: 1byte, 1: 2bytes, 2: 4bytes + static const uint8_t kOperationByte1 = 0x00; + static const uint8_t kOperationByte2 = 0x04; + static const uint8_t kOperationByte4 = 0x08; + // [1] 0: auto increament, 1: addr fixed + static const uint8_t kAddrAutoIncrement = 0x00; + static const uint8_t kAddrFixed = 0x02; + // [0] 1: send ack flag + static const uint8_t kAckFlag = 0x01; + + uint8_t m_u8Indicator; + uint8_t m_u8CmdOption; + uint32_t m_u32Addr; + uint16_t m_u16Length; + + void Init(uint8_t u8CmdOption, uint32_t u32Addr, uint16_t u16Len, + uint8_t u8Indicator = 0xA0) { + m_u8Indicator = u8Indicator; + m_u8CmdOption = u8CmdOption; + m_u32Addr = htobe32(u32Addr); + m_u16Length = htobe16(u16Len); + } + + uint32_t GetAddr() const { return be32toh(m_u32Addr); } + + uint16_t GetOpertationLength() const { return be16toh(m_u16Length); } + uint8_t GetOperationByte() const { + return (m_u8CmdOption & kOperationByte4) + ? 4 + : ((m_u8CmdOption & kOperationByte2) ? 2 : 1); + } + + uint16_t GetDataLength() const { + return (m_u8CmdOption & kRead) + ? 0 + : (GetOperationByte() * be16toh(m_u16Length)); + } + uint16_t GetRspDataLength() const { + return (m_u8CmdOption & kWrite) + ? 0 + : (GetOperationByte() * be16toh(m_u16Length)); + } + + uint16_t GetDataOffset() const { return sizeof(LidarCommunicationHeader); } + uint16_t GetCrcOffset() const { + return sizeof(LidarCommunicationHeader) + GetDataLength(); + } + + // size: header + data + crc32 + uint16_t GetPacketSize() const { return GetCrcOffset() + sizeof(uint32_t); } + uint16_t GetRspPacketSize() const { + return (m_u8CmdOption & kWrite) + ? (m_u8CmdOption & kAckFlag + ? sizeof(LidarCommunicationHeader) + sizeof(uint32_t) + 1 + : 0) + : (sizeof(LidarCommunicationHeader) + + GetOperationByte() * be16toh(m_u16Length) + sizeof(uint32_t)); + } + +} PACKED; +#ifdef _MSC_VER +#pragma pack(pop) +#endif +#endif diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcClient/include/ptc_client.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcClient/include/ptc_client.h new file mode 100644 index 0000000..3bc31d4 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcClient/include/ptc_client.h @@ -0,0 +1,100 @@ +/* + * Copyright (C) 2019 Hesai Tech + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + */ + +/* + * File: ptc_client.h + * Author: Felix Zou + * + * Created on Jun 20, 2019, 10:46 AM + */ + +#ifndef PtcClient_H +#define PtcClient_H + +// #include +// #include +#include +#include +// #include "tcp_client_boost.h" +// #include "tcp_ssl_client_boost.h" +#include "tcp_client.h" +#include "tcp_ssl_client.h" +#include "lidar_types.h" +#include "driver_param.h" +#include "ptc_parser.h" + +#define PKT_SIZE_40P (1262) +#define PKT_SIZE_AC (1256) +#define PKT_SIZE_64 (1194) +#define PKT_SIZE_20 (1270) +namespace hesai +{ +namespace lidar +{ + +const uint8_t kPTCGetLidarCalibration = 0x05; +const uint8_t kPTCGetInventoryInfo = 0x07; +const uint8_t kPTCGetLidarFiretimes = 0xA9; +const uint8_t kPTCGetLidarChannelConfig = 0xA8; + +class PtcClient { + public: + PtcClient(std::string IP = kLidarIPAddr + , uint16_t u16TcpPort = kTcpPort + , bool bAutoReceive = false + , PtcMode client_mode = PtcMode::tcp + , uint8_t ptc_version = 1 + , const char* cert = nullptr + , const char* private_key = nullptr + , const char* ca = nullptr); + ~PtcClient() {} + + PtcClient(const PtcClient &orig) = delete; + + bool IsValidRsp(u8Array_t &byteStreamIn); + + void TcpFlushIn(); + int QueryCommand(u8Array_t &byteStreamIn, u8Array_t &byteStreamOut, uint8_t u8Cmd ); + int SendCommand(u8Array_t &byteStreamIn, uint8_t u8Cmd); + bool GetValFromOutput(uint8_t cmd, uint8_t retcode, const u8Array_t &payload, int start_pos, int length, u8Array_t &res); + + u8Array_t GetCorrectionInfo(); + int GetCorrectionInfo(u8Array_t &dataOut); + int GetFiretimesInfo(u8Array_t &dataOut); + int GetChannelConfigInfo(u8Array_t &dataOut); + int SetSocketTimeout(uint32_t u32RecMillisecond, uint32_t u32SendMillisecond); + void CRCInit(); + uint32_t CRCCalc(uint8_t *bytes, int len); + + public: + uint32_t m_CRCTable[256]; + + private: + static const std::string kLidarIPAddr; + static const uint16_t kTcpPort = 9347; + uint16_t m_u16PtcPort; + bool running_; + PtcMode client_mode_; + uint8_t ptc_version_; + std::shared_ptr client_; + std::shared_ptr ptc_parser_; +}; +} +} + +#endif /* PtcClient_H */ \ No newline at end of file diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcClient/include/tcp_client.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcClient/include/tcp_client.h new file mode 100644 index 0000000..c1998e3 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcClient/include/tcp_client.h @@ -0,0 +1,115 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ + +/* + * File: tcp_client.h + * Author: Felix Zou + * + * Created on Sep 5, 2019, 10:46 AM + */ + +#ifndef TCPCLIENT_H +#define TCPCLIENT_H + +#ifdef _MSC_VER +#include +#include +#else +typedef unsigned int SOCKET; +#endif +#include "client_base.h" +#include +#include +#include +#include +#include +#include +#include +#include +namespace hesai +{ +namespace lidar +{ +class TcpClient : public ClientBase{ + public: + explicit TcpClient(); + virtual ~TcpClient(); + + TcpClient(const TcpClient &orig) = delete; + + virtual bool Open(std::string IPAddr, uint16_t port, bool bAutoReceive = false, + const char* cert = nullptr, const char* private_key = nullptr, const char* ca = nullptr); + bool Open(); + virtual void Close(); + virtual bool IsOpened(); + bool IsOpened(bool bExpectation); + virtual int Send(uint8_t *u8Buf, uint16_t u16Len, int flags = 0); + virtual int Receive(uint8_t *u8Buf, uint32_t u16Len, int flags = 0); + + /** + * @brief 设置接收超时 + * @param u32Timeout 超时时间/ms + * @return + */ + virtual bool SetReceiveTimeout(uint32_t u32Timeout); + + /** + * @brief 设置收发超时 + * @param u32RecMillisecond 接收超时/ms + * @param u32SendMillisecond 发送超时/ms + * @return + */ + virtual int SetTimeout(uint32_t u32RecMillisecond, uint32_t u32SendMillisecond); + + /** + * @brief 设置自动接收模式下Buff的大小 + * @param size + */ + virtual void SetReceiveBufferSize(const uint32_t &size); + + private: + /** + * monitor file descriptor and wait for I/O operation + */ + int WaitFor(const int &socketfd, uint32_t timeoutSeconds = 1); + + protected: + static const uint32_t kDefaultTimeout = 500; + + std::string m_sServerIP; + uint16_t ptc_port_; + SOCKET m_tcpSock; + bool m_bLidarConnected; + uint32_t m_u32ReceiveBufferSize; + // 收发超时/ms + uint32_t m_u32RecTimeout = kDefaultTimeout; + uint32_t m_u32SendTimeout = kDefaultTimeout; +}; +} +} +#endif /* TCPCLIENT_H */ diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcClient/include/tcp_ssl_client.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcClient/include/tcp_ssl_client.h new file mode 100644 index 0000000..355015f --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcClient/include/tcp_ssl_client.h @@ -0,0 +1,114 @@ +/* + * Copyright (C) 2019 Hesai Tech + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + */ + +/* + * File: tcp_client_boost.h + * Author: zhang xu + * + * Created on Jul 7, 2023, 13:55 PM + */ + +#ifndef TCPSSLCLIENT_H +#define TCPSSLCLIENT_H + +#include "client_base.h" +// #include "util.h" +#include "openssl/ssl.h" +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef _MSC_VER +#include +#include +#else +typedef unsigned int SOCKET; +#endif + +const int MAX_LENGTH = 1024; + +namespace hesai +{ +namespace lidar +{ +class TcpSslClient : public ClientBase { + public: + explicit TcpSslClient(); + virtual ~TcpSslClient(); + + TcpSslClient(const TcpSslClient &orig) = delete; + + // 设置ssl单双向模式、密钥、证书等 + SSL_CTX* InitSslClient(const char* cert, const char* private_key, const char* ca); + + virtual bool Open(std::string IPAddr, uint16_t port, bool bAutoReceive = false, + const char* cert = nullptr, const char* private_key = nullptr, const char* ca = nullptr); + bool Open(); + virtual void Close(); + virtual bool IsOpened(); + bool IsOpened(bool bExpectation); + virtual int Send(uint8_t *u8Buf, uint16_t u16Len, int flags = 0); + virtual int Receive(uint8_t *u8Buf, uint32_t u32Len, int flags = 0); + + /** + * @brief 设置接收超时 + * @param u32Timeout 超时时间/ms + * @return + */ + virtual bool SetReceiveTimeout(uint32_t u32Timeout); + + /** + * @brief 设置收发超时 + * @param u32RecMillisecond 接收超时/ms + * @param u32SendMillisecond 发送超时/ms + * @return + */ + virtual int SetTimeout(uint32_t u32RecMillisecond, uint32_t u32SendMillisecond); + + /** + * @brief 设置自动接收模式下Buff的大小 + * @param size + */ + virtual void SetReceiveBufferSize(const uint32_t &size); + + private: + // void connect(const tcp::resolver::results_type& endpoints); + void handshake(); + + protected: + static const uint32_t kDefaultTimeout = 500; + + std::string m_sServerIP; + uint16_t ptc_port_; + bool m_bLidarConnected; + uint32_t m_u32ReceiveBufferSize; + // 收发超时/ms + uint32_t m_u32RecTimeout = kDefaultTimeout; + uint32_t m_u32SendTimeout = kDefaultTimeout; + SSL_CTX* ctx_; + SOCKET tcpsock_; + SSL* ssl_; +}; +} +} + +#endif \ No newline at end of file diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcClient/src/ptc_client.cc b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcClient/src/ptc_client.cc new file mode 100644 index 0000000..69e094c --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcClient/src/ptc_client.cc @@ -0,0 +1,340 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ + +/* + * File: ptc_client.cc + * Author: Felix Zou + */ + +#include "ptc_client.h" + +#include +#ifdef _MSC_VER +#ifndef MSG_DONTWAIT +#define MSG_DONTWAIT (0x40) +#endif +#else +#include +#include +#endif + + +// #include "udp_protocol_v1_4.h" +// #include "udp_protocol_p40.h" +// #include "udp_protocol_p64.h" +// #include "udp_protocol_v4_3.h" +// #include "udp_protocol_v6_1.h" +using namespace hesai::lidar; +const std::string PtcClient::kLidarIPAddr("192.168.1.201"); + +PtcClient::PtcClient(std::string ip + , uint16_t u16TcpPort + , bool bAutoReceive + , PtcMode client_mode + , uint8_t ptc_version + , const char* cert + , const char* private_key + , const char* ca) + : client_mode_(client_mode) + , ptc_version_(ptc_version) { + std::cout << "PtcClient::PtcClient()" << ip.c_str() + << u16TcpPort << std::endl; + // TcpClient::Open(ip, u16TcpPort); + if(client_mode == PtcMode::tcp) { + client_ = std::make_shared(); + client_->Open(ip, u16TcpPort, bAutoReceive); + } else if(client_mode == PtcMode::tcp_ssl) { + client_ = std::make_shared(); + client_->Open(ip, u16TcpPort, bAutoReceive, cert, private_key, ca); + } + + // init ptc parser + ptc_parser_ = std::make_shared(ptc_version); + + CRCInit(); +} + +bool PtcClient::IsValidRsp(u8Array_t &byteStreamIn) { + bool ret = true; + // 根据ptc version来检查输入数据的前两位是否是对应版本的标记位 + while (byteStreamIn.size() >= 2 && + (byteStreamIn.at(0) != ptc_parser_->GetHeaderIdentifier0() || + byteStreamIn.at(1) != ptc_parser_->GetHeaderIdentifier1())) { + byteStreamIn.erase(byteStreamIn.begin()); + } + //输入数据长度小于头长度 没有数据 + if (byteStreamIn.size() < ptc_parser_->GetPtcParserHeaderSize()) ret = false; + + if (ret) { + if(ptc_version_ == 1) { + PTCHeader_1_0 *pHeader = (PTCHeader_1_0 *)byteStreamIn.data(); + if (!pHeader->IsValidReturnCode()) ret = false; + } else { + PTCHeader_2_0 *pHeader = (PTCHeader_2_0 *)byteStreamIn.data(); + if (!pHeader->IsValidReturnCode()) ret = false; + } + } + return ret; +} + +//接收数据 非阻塞模式 +void PtcClient::TcpFlushIn() { + u8Array_t u8Buf(1500, 0); + int len = 0; + do { + len = client_->Receive(u8Buf.data(), u8Buf.size(), MSG_DONTWAIT); + if (len > 0) { + std::cout << "TcpFlushIn, len" << len << std::endl; + for(int i = 0; i 0); +} + +int PtcClient::QueryCommand(u8Array_t &byteStreamIn, + u8Array_t &byteStreamOut, + uint8_t u8Cmd) { + int ret = 0; + u8Array_t encoded; + uint32_t tick = GetMicroTickCount(); + ptc_parser_->PtcStreamEncode(byteStreamIn, encoded, u8Cmd); + // TcpFlushIn(); + + int nLen = client_->Send(encoded.data(), encoded.size()); + if (nLen != encoded.size()) { + // qDebug("%s: send failure, %d.", __func__, nLen); + ret = -1; + } + + //开始接收数据 + int nOnceRecvLen = 0; + u8Array_t u8RecvHeaderBuf(ptc_parser_->GetPtcParserHeaderSize()); + u8Array_t::pointer pRecvHeaderBuf = u8RecvHeaderBuf.data(); + u8Array_t u8HeaderBuf(ptc_parser_->GetPtcParserHeaderSize()); + u8Array_t::pointer pHeaderBuf = u8HeaderBuf.data(); + //还要接收的数据长度 + int nLeft = ptc_parser_->GetPtcParserHeaderSize(); + //是否已经找到连续的0x47和0x74 + bool bHeaderFound = false; + int nValidDataLen = 0; + int nPayLoadLen = 0; + + if (ret == 0) { + while (nLeft > 0) { + nOnceRecvLen = client_->Receive(pRecvHeaderBuf, nLeft); + if (nOnceRecvLen <= 0) break; + + if (!bHeaderFound) { + for (int i = 0; i < nOnceRecvLen - 1; i++) { + if (pRecvHeaderBuf[i] == ptc_parser_->GetHeaderIdentifier0() && + pRecvHeaderBuf[i + 1] == ptc_parser_->GetHeaderIdentifier1()) { + nValidDataLen = nOnceRecvLen - i; + bHeaderFound = true; + std::memcpy(pHeaderBuf, pRecvHeaderBuf + i, nValidDataLen); + //剩下还需接收的长度 + nLeft -= nValidDataLen; + break; + } + } + } else { + //已经收到PTC正确的头部 只是还没有达到8位 + std::memcpy(pHeaderBuf + nValidDataLen, pRecvHeaderBuf, nOnceRecvLen); + nValidDataLen += nOnceRecvLen; + nLeft -= nOnceRecvLen; + } + } + //因超时退出而导致数据未接收完全 + if (nLeft > 0) { + // std::cout << "PtcClient::QueryCommand, invalid Header, nLeft:" + // << nLeft << ", u8HeaderBuf:" << std::hex << u8HeaderBuf; + ret = -1; + } else { + //开始接收body + if(ptc_version_ == 1) { + PTCHeader_1_0 *pPTCHeader = reinterpret_cast(u8HeaderBuf.data()); + if (!pPTCHeader->IsValidReturnCode() || pPTCHeader->cmd_ != u8Cmd) { + std::cout << "PtcClient::QueryCommand,RspCode invalid:" << pPTCHeader->return_code_ << ", u8HeaderBuf: " << std::endl; + ret = -1; + } else { + nPayLoadLen = pPTCHeader->GetPayloadLen(); + } + } else { + PTCHeader_2_0 *pPTCHeader = reinterpret_cast(u8HeaderBuf.data()); + if (!pPTCHeader->IsValidReturnCode() || pPTCHeader->cmd_ != u8Cmd) { + std::cout << "PtcClient::QueryCommand,RspCode invalid:" << pPTCHeader->return_code_ << ", u8HeaderBuf: " << std::endl; + ret = -1; + } else { + nPayLoadLen = pPTCHeader->GetPayloadLen(); + } + } + } + } + // std::cout << "nPayLoadLen = " << nPayLoadLen << std::endl; + if (ret == 0 && nPayLoadLen > 0) { + //对payLoad进行一个判断，避免负载过长申请过大的空间 目前指定为10K + const int kMaxPayloadBuffSize = 10240; + if (nPayLoadLen > kMaxPayloadBuffSize) + std::cout << "PtcClient::QueryCommand, warning, nPayLoadLen too large:" << nPayLoadLen << std::endl; + + // tmp code to allow LiDAR bug + // const int kExtraBufLen = 4; + // u8Array_t u8BodyBuf(nPayLoadLen + kExtraBufLen); + u8Array_t u8BodyBuf(nPayLoadLen); + u8Array_t::pointer pBodyBuf = u8BodyBuf.data(); + + nLeft = nPayLoadLen; + while (nLeft > 0) { + nOnceRecvLen = client_->Receive(pBodyBuf, nLeft); + if (nOnceRecvLen <= 0) { + break; + } + + nLeft -= nOnceRecvLen; + pBodyBuf += nOnceRecvLen; + } + + if (nLeft > 0) { + // std::cout << "PtcClient::QueryCommand,Body " + // <<"incomplete:nPayLoadLen:" + // << nPayLoadLen << " nLeft:" << nLeft << std::endl; + ret = -1; + } else { + //将收到的bodyBuf拷贝到最终要传出的buf + byteStreamOut.resize(nPayLoadLen); + pBodyBuf = u8BodyBuf.data(); + std::memcpy(byteStreamOut.data(), pBodyBuf, nPayLoadLen); + } + } + + // std::cout << "PtcClient::QueryCommand,rsp, header:" << std::hex + // << u8HeaderBuf << "byteStreamOut:" << byteStreamOut; + + printf("exec time: %fms\n", (GetMicroTickCount() - tick) / 1000.f); + + return ret; +} + +int PtcClient::SendCommand(u8Array_t &byteStreamIn, uint8_t u8Cmd) { + // std::cout << "PtcClient::SendCommand, cmd:" << u8Cmd + // << "data:" << std::hex << byteStreamIn; + + u8Array_t byteStreamOut; + + return QueryCommand(byteStreamIn, byteStreamOut, u8Cmd); +} + +bool PtcClient::GetValFromOutput(uint8_t cmd, uint8_t retcode, const u8Array_t &payload, int start_pos, int length, u8Array_t& res) { + return ptc_parser_->PtcStreamDecode(cmd, retcode, payload, start_pos, length, res); +} + +u8Array_t PtcClient::GetCorrectionInfo() { + u8Array_t dataIn; + u8Array_t dataOut; + + int ret = -1; + + ret = this->QueryCommand(dataIn, dataOut, + kPTCGetLidarCalibration); + + // ret = this->QueryCommand(dataIn, dataOut, + // PtcClient::kPTCGetInventoryInfo); + + if (ret == 0 && !dataOut.empty()) { + std::cout << "Read correction file from lidar success" << std::endl; + } else { + std::cout << "Read correction file from lidar fail" << std::endl; + } + + return dataOut; +} + +int PtcClient::GetCorrectionInfo(u8Array_t &dataOut) { + u8Array_t dataIn; + int ret = -1; + ret = this->QueryCommand(dataIn, dataOut, + kPTCGetLidarCalibration); + + if (ret == 0 && !dataOut.empty()) { + return 0; + } else { + return -1; + } +} + +int PtcClient::GetFiretimesInfo(u8Array_t &dataOut) { + u8Array_t dataIn; + + int ret = -1; + ret = this->QueryCommand(dataIn, dataOut, + kPTCGetLidarFiretimes); + if (ret == 0 && !dataOut.empty()) { + return 0; + } else { + return -1; + } +} + +int PtcClient::GetChannelConfigInfo(u8Array_t &dataOut) { + u8Array_t dataIn; + + int ret = -1; + ret = this->QueryCommand(dataIn, dataOut, + kPTCGetLidarChannelConfig); + if (ret == 0 && !dataOut.empty()) { + return 0; + } else { + return -1; + } +} + +int PtcClient::SetSocketTimeout(uint32_t u32RecMillisecond, + uint32_t u32SendMillisecond) { + return client_->SetTimeout(u32RecMillisecond, u32SendMillisecond); +} + + +void PtcClient::CRCInit() { + uint32_t i, j, k; + + for (i = 0; i < 256; i++) { + k = 0; + for (j = (i << 24) | 0x800000; j != 0x80000000; j <<= 1) + k = (k << 1) ^ (((k ^ j) & 0x80000000) ? 0x04c11db7 : 0); + + m_CRCTable[i] = k; + } +} + +uint32_t PtcClient::CRCCalc(uint8_t *bytes, int len) { + uint32_t i_crc = 0xffffffff; + int i = 0; + for (i = 0; (size_t)i < len; i++) + i_crc = (i_crc << 8) ^ m_CRCTable[((i_crc >> 24) ^ bytes[i]) & 0xff]; + return i_crc; +} diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcClient/src/tcp_client.cc b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcClient/src/tcp_client.cc new file mode 100644 index 0000000..022ffcb --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcClient/src/tcp_client.cc @@ -0,0 +1,338 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ + +/* + * File: tcp_client.cc + * Author: Felix Zou + */ + +#include "tcp_client.h" +#ifdef _MSC_VER +#include +#include +#pragma comment(lib, "ws2_32.lib") // Winsock Library +#include +typedef int ssize_t; +typedef int socklen_t; +#define MSG_DONTWAIT (0x40) +#else +#include +#include +#include +#include +#include +#include +#include +#endif +#include +#include +#include +#include +using namespace hesai::lidar; +TcpClient::TcpClient() { + m_sServerIP.clear(); + ptc_port_ = 0; + m_tcpSock = -1; + m_bLidarConnected = false; + m_u32ReceiveBufferSize = 4096; +} + +TcpClient::~TcpClient() { + Close(); +} + +void TcpClient::Close() { + printf("TcpClient::Close()\n"); + + m_sServerIP.clear(); + ptc_port_ = 0; + m_bLidarConnected = false; + + if (m_tcpSock > 0) { +#ifdef _MSC_VER + closesocket(m_tcpSock); + WSACleanup(); +#else + close(m_tcpSock); +#endif + m_tcpSock = -1; + } +} + +bool TcpClient::Open(std::string IPAddr, uint16_t u16Port, bool bAutoReceive, + const char* cert, const char* private_key, const char* ca) { + if (IsOpened(true) && m_sServerIP == IPAddr && u16Port == ptc_port_) { + return true; + } +#ifdef _MSC_VER + std::cout << __FUNCTION__ << "IP" << IPAddr.c_str() << "port" + << u16Port << std::endl; +#else + std::cout << __PRETTY_FUNCTION__ << "IP" << IPAddr.c_str() << "port" + << u16Port << std::endl; +#endif + Close(); + + m_sServerIP = IPAddr; + ptc_port_ = u16Port; + return Open(); +} + +bool TcpClient::Open() { +#ifdef _MSC_VER + WSADATA wsaData; + WORD version = MAKEWORD(2, 2); + int res = WSAStartup(version, &wsaData); // win sock start up + if (res) { + std::cerr << "Initilize winsock error !" << std::endl; + return false; + } +#endif + struct sockaddr_in serverAddr; + + m_tcpSock = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP); + + if (m_tcpSock == -1) return false; + + memset(&serverAddr, 0, sizeof(serverAddr)); + + serverAddr.sin_family = AF_INET; + serverAddr.sin_port = htons(ptc_port_); + if (inet_pton(AF_INET, m_sServerIP.c_str(), &serverAddr.sin_addr) <= 0) { +#ifdef _MSC_VER + closesocket(m_tcpSock); +#else + close(m_tcpSock); +#endif + m_tcpSock = -1; + std::cout << __FUNCTION__ << "inet_pton error:" << m_sServerIP.c_str() << std::endl; + + return false; + } + + // int retVal = SetTimeout(m_u32RecTimeout, m_u32SendTimeout); + int retVal = 0; + + if (retVal == 0) { + if (::connect(m_tcpSock, (sockaddr *)&serverAddr, sizeof(serverAddr)) < 0) { + if (EINPROGRESS != errno && EWOULDBLOCK != errno) { +#ifdef _MSC_VER + closesocket(m_tcpSock); +#else + close(m_tcpSock); +#endif + m_tcpSock = -1; + std::cout << __FUNCTION__ << "connect failed" << errno << std::endl; + + return false; + } else if (EINPROGRESS == errno) { + std::cout << "connect lidar time out\n"; + + return false; + } + + std::cout << "connect lidar fail errno" << errno << std::endl; + + return false; + } + } else { + std::cout << __FUNCTION__ << "setsockopt failed, errno" << errno << std::endl; + + return false; + } + + std::cout << __FUNCTION__ << " succeed, IP" << m_sServerIP.c_str() << "port" + << ptc_port_ << std::endl; + + m_bLidarConnected = true; + + return true; +} + +bool TcpClient::IsOpened() { + // std::cout << "is opened" << m_bLidarConnected; + + return m_bLidarConnected; +} + +bool TcpClient::IsOpened(bool bExpectation) { + return m_bLidarConnected; +} + +int TcpClient::Send(uint8_t *u8Buf, uint16_t u16Len, int flags) { + ssize_t len = -1; + bool ret = true; + + if (!IsOpened()) ret = Open(); + + if (ret) { + len = send(m_tcpSock, (char*)u8Buf, u16Len, flags); + if (len != u16Len && errno != EAGAIN && errno != EWOULDBLOCK && + errno != EINTR) { + std::cout << __FUNCTION__ << "errno" << errno << std::endl; +#ifdef _MSC_VER + closesocket(m_tcpSock); +#else + close(m_tcpSock); +#endif + m_tcpSock = -1; + m_bLidarConnected = false; + } + } + return len; +} + +int TcpClient::Receive(uint8_t *u8Buf, uint32_t u32Len, int flags) { + ssize_t len = -1; + bool ret = true; + + if (!IsOpened()) ret = Open(); + + int tick = GetMicroTickCount(); + if (ret) { +#ifdef _MSC_VER + if (flags == MSG_DONTWAIT) { + unsigned long nonBlockingMode = 1; + ioctlsocket(m_tcpSock, FIONBIO, &nonBlockingMode); + } +#endif + len = recv(m_tcpSock, (char*)u8Buf, u32Len, flags); + if (len == 0 || (len == -1 && errno != EINTR && errno != EAGAIN && + errno != EWOULDBLOCK)) { + std::cout << __FUNCTION__ << ", len: " << len << " errno: " << errno << std::endl; +#ifdef _MSC_VER + closesocket(m_tcpSock); +#else + close(m_tcpSock); +#endif + m_tcpSock = -1; + m_bLidarConnected = false; + } + } + + int delta = GetMicroTickCount() - tick; + + if (delta >= 1000000) { + std::cout << __FUNCTION__ << "execu:" << delta << "us" << std::endl; + } + + return len; +} + + +bool TcpClient::SetReceiveTimeout(uint32_t u32Timeout) { + if (m_tcpSock < 0) { + printf("TcpClient not open\n"); + return false; + } +#ifdef _MSC_VER + int timeout_ms = u32Timeout; + int retVal = setsockopt(m_tcpSock, SOL_SOCKET, SO_RCVTIMEO, (char*)&timeout_ms, + sizeof(timeout_ms)); +#else + uint32_t sec = u32Timeout / 1000; + uint32_t msec = u32Timeout % 1000; + struct timeval timeout; + timeout.tv_sec = sec; + timeout.tv_usec = msec * 1000; + int retVal = setsockopt(m_tcpSock, SOL_SOCKET, SO_RCVTIMEO, + (const void *)&timeout, sizeof(timeval)); +#endif + return retVal == 0; +} + +int TcpClient::SetTimeout(uint32_t u32RecMillisecond, + uint32_t u32SendMillisecond) { + if (m_tcpSock < 0) { + printf("TcpClient not open\n"); + return -1; + } + m_u32RecTimeout = u32RecMillisecond; + m_u32SendTimeout = u32SendMillisecond; +#ifdef _MSC_VER + int timeout_ms = u32RecMillisecond; + int retVal = setsockopt(m_tcpSock, SOL_SOCKET, SO_RCVTIMEO, + (char*)&timeout_ms, sizeof(timeout_ms)); +#else + uint32_t sec = u32RecMillisecond / 1000; + uint32_t msec = u32RecMillisecond % 1000; + + struct timeval timeout; + timeout.tv_sec = sec; + timeout.tv_usec = msec * 1000; + int retVal = setsockopt(m_tcpSock, SOL_SOCKET, SO_RCVTIMEO, + (const void *)&timeout, sizeof(timeval)); +#endif + if (retVal == 0) { +#ifdef _MSC_VER + int timeout_ms = u32SendMillisecond; + retVal = setsockopt(m_tcpSock, SOL_SOCKET, SO_SNDTIMEO, + (char*)&timeout_ms, sizeof(timeout_ms)); +#else + uint32_t sec = u32SendMillisecond / 1000; + uint32_t msec = u32SendMillisecond % 1000; + + struct timeval timeout; + timeout.tv_sec = sec; + timeout.tv_usec = msec * 1000; + retVal = setsockopt(m_tcpSock, SOL_SOCKET, SO_SNDTIMEO, + (const void *)&timeout, sizeof(timeval)); +#endif + } + return retVal; +} + +void TcpClient::SetReceiveBufferSize(const uint32_t &size) { + if (m_tcpSock < 0) { + printf("TcpClient not open\n"); + return; + } + + m_u32ReceiveBufferSize = size; + int recbuffSize; + socklen_t optlen = sizeof(recbuffSize); + int ret = getsockopt(m_tcpSock, SOL_SOCKET, SO_RCVBUF, (char*)&recbuffSize, &optlen); + if (ret == 0 && recbuffSize < size) { + setsockopt(m_tcpSock, SOL_SOCKET, SO_RCVBUF, (char*)&size, sizeof(size)); + } +} + + +int TcpClient::WaitFor(const int &socketfd, uint32_t timeoutSeconds) { + struct timeval tv; + tv.tv_sec = timeoutSeconds; + tv.tv_usec = 0; + fd_set fds; + + FD_ZERO(&fds); + FD_SET(socketfd, &fds); + const int selectRet = select(socketfd + 1, &fds, nullptr, nullptr, &tv); + + return selectRet; +} + diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcClient/src/tcp_ssl_client.cc b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcClient/src/tcp_ssl_client.cc new file mode 100644 index 0000000..975ece4 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcClient/src/tcp_ssl_client.cc @@ -0,0 +1,388 @@ +/* + * Copyright (C) 2019 Hesai Tech + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + */ + +/* + * File: tcp_ssl_client_boost.cc + * Author: zhang xu + */ +#include "tcp_ssl_client.h" + +#include + +#ifdef _MSC_VER +#include +#include +#pragma comment(lib, "ws2_32.lib") // Winsock Library +#include +typedef int ssize_t; +typedef int socklen_t; +#define MSG_DONTWAIT (0x40) +#else +#include +#include +#include +#include +#include +#include +#include +#endif +using namespace hesai::lidar; +using std::placeholders::_1; +using std::placeholders::_2; +int tcp_open(const char* ipaddr, int port) { +#ifdef _MSC_VER + WSADATA wsaData; + WORD version = MAKEWORD(2, 2); + int res = WSAStartup(version, &wsaData); // win sock start up + if (res) { + std::cerr << "Initilize winsock error !" << std::endl; + return false; + } +#endif + int sockfd; + struct sockaddr_in servaddr; + printf("ip:%s port:%d\n",ipaddr,port); + + if ((sockfd = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP)) == -1){ + printf("socket errno:%d, %s\n",errno,strerror(errno)); + return -1; + } + memset(&servaddr, 0, sizeof(servaddr)); + servaddr.sin_family = AF_INET; + servaddr.sin_port = htons(port); + if (inet_pton(AF_INET, ipaddr, &servaddr.sin_addr) <= 0) { +#ifdef _MSC_VER + closesocket(sockfd); +#else + close(sockfd); +#endif + sockfd = -1; + std::cout << __FUNCTION__ << "inet_pton error:" << ipaddr << std::endl; + return sockfd; + } + + if (connect(sockfd, (struct sockaddr*)&servaddr, sizeof(servaddr)) == -1) { + printf("connect errno:%d, %s\n",errno,strerror(errno)); +#ifdef _MSC_VER + closesocket(sockfd); +#else + close(sockfd); +#endif + sockfd = -1; + return sockfd; + } + + return sockfd; +} + +int sys_readn_by_ssl(SSL *ssl, void *vptr, int n) +{ + int nleft, nread; + char *ptr; + + ptr = (char *)(vptr); + nleft = n; + while (nleft > 0) { + nread = SSL_read(ssl, ptr, nleft); + if (nread < 0) { + if (errno == EINTR) + nread = 0; + else + return -1; + } + else if (nread == 0) + break; + + nleft -= nread; + ptr += nread; + } + return n - nleft; +} + +/* + * + * 调用顺序：TcpSslClient() -> InitSslClient() -> Open() + * + */ +TcpSslClient::TcpSslClient() { + m_sServerIP.clear(); + ptc_port_ = 0; + tcpsock_ = -1; + m_bLidarConnected = false; + m_u32ReceiveBufferSize = 4096; + ctx_ = nullptr; + ssl_ = nullptr; +} + +TcpSslClient::~TcpSslClient() { + Close(); +} + +void TcpSslClient::Close() { + printf("TcpSslClient::Close()\n"); + + m_sServerIP.clear(); + ptc_port_ = 0; + m_bLidarConnected = false; + if(tcpsock_ > 0) { +#ifdef _MSC_VER + closesocket(tcpsock_); + WSACleanup(); +#else + close(tcpsock_); +#endif + } + if (ctx_ != nullptr) { + SSL_CTX_free(ctx_); + } + if (ssl_ != nullptr) SSL_shutdown(ssl_); +} + +bool TcpSslClient::IsOpened() { + // std::cout << "is opened" << m_bLidarConnected; + + return m_bLidarConnected; +} + +bool TcpSslClient::IsOpened(bool bExpectation) { + return m_bLidarConnected; +} + +bool TcpSslClient::Open(std::string IPAddr, + uint16_t u16Port, + bool bAutoReceive, + const char* cert, + const char* private_key, + const char* ca) { + if (IsOpened(true) && m_sServerIP == IPAddr && u16Port == ptc_port_) { + return true; + } +#ifdef _MSC_VER + std::cout << __FUNCTION__ << "IP" << IPAddr.c_str() << "port" + << u16Port << std::endl; +#else + std::cout << __PRETTY_FUNCTION__ << "IP" << IPAddr.c_str() << "port" + << u16Port << std::endl; +#endif + Close(); + m_sServerIP = IPAddr; + ptc_port_ = u16Port; + + ctx_ = InitSslClient(cert, private_key, ca); + if(ctx_ == NULL) { + printf("%s:%d, create SSL_CTX failed\n", __func__, __LINE__); + // ERR_print_errors_fp(stderr); + return false; + } + return Open(); +} + +bool TcpSslClient::Open() { + tcpsock_ = tcp_open(m_sServerIP.c_str(), ptc_port_); + if(tcpsock_ < 0) { + printf("Connect to Server Failed!~!~\n"); + Close(); + return false; + } + ssl_ = SSL_new(ctx_); + if (ssl_ == NULL) { + printf("%s:%d, create ssl failed\n", __func__, __LINE__); + Close(); + return false; + } + + SSL_set_fd(ssl_, tcpsock_); + if(SSL_connect(ssl_) == 0) { + printf("%s:%d, connect ssl failed\n", __func__, __LINE__); + Close(); + return false; + } + + if(SSL_get_verify_result(ssl_) != X509_V_OK) { + printf("%s:%d, verify ssl failed\n", __func__, __LINE__); + Close(); + return false; + } + + printf("TcpSslClient::Open() success!\n"); + m_bLidarConnected = true; + return true; +} + +SSL_CTX* TcpSslClient::InitSslClient(const char* cert, const char* private_key, const char* ca) { + SSL_library_init(); + SSL_load_error_strings(); + OpenSSL_add_all_algorithms(); + SSL_CTX *ctx = SSL_CTX_new(SSLv23_client_method()); + + if(ctx == NULL) { + printf("%s:%d, create SSL_CTX failed\n", __func__, __LINE__); + return NULL; + } + + if (ca) { + printf("ca path: %s\n",ca); + if( SSL_CTX_load_verify_locations(ctx, ca, NULL) == 0) { + // ERR_print_errors_fp(stderr); + printf("%s:%d, load ca failed,please check ca file path\n", __func__, __LINE__); + return NULL; + } + } + + if (cert && private_key){ + printf("cert path: %s,\nprivate_key path: %s\n",cert, private_key); + SSL_CTX_set_verify(ctx, SSL_VERIFY_PEER | SSL_VERIFY_FAIL_IF_NO_PEER_CERT, NULL); + if(SSL_CTX_use_certificate_file(ctx, cert, SSL_FILETYPE_PEM) == 0) { + printf("%s:%d, load cert file failed,please check cert file path\n", __func__, __LINE__); + return NULL; + } + if(SSL_CTX_use_PrivateKey_file(ctx, private_key, SSL_FILETYPE_PEM) == 0) { + printf("%s:%d, load private key file failed,please check private key file path\n", __func__, __LINE__); + return NULL; + } + if(SSL_CTX_check_private_key(ctx) == 0) { + printf("%s:%d, check private key failed\n", __func__, __LINE__); + return NULL; + } + } + return ctx; +} + +int TcpSslClient::Send(uint8_t *u8Buf, uint16_t u16Len, int flags) { + ssize_t len = -1; + bool ret = true; + + if(!IsOpened()) ret = Open(); + if(ret) { + len = SSL_write(ssl_, u8Buf, u16Len); + if (len != u16Len && errno != EAGAIN && errno != EWOULDBLOCK && + errno != EINTR) { + std::cout << __FUNCTION__ << "errno" << errno << std::endl; + ERR_print_errors_fp(stderr); + Close(); + return -1; + } + } + + return len; +} + +int TcpSslClient::Receive(uint8_t *u8Buf, uint32_t u32Len, int flags) { + ssize_t len = -1; + bool ret = true; + + if(!IsOpened()) ret = Open(); + + int tick = GetMicroTickCount(); + if(ret) { + len = sys_readn_by_ssl(ssl_, u8Buf , u32Len); + if(len != u32Len) { + printf("ssl receive failed\n"); + Close(); + return -1; + } + } + + int delta = GetMicroTickCount() - tick; + + if (delta >= 1000000) { + std::cout << __FUNCTION__ << "execu:" << delta << "us" << std::endl; + } + + return len; +} + + +bool TcpSslClient::SetReceiveTimeout(uint32_t u32Timeout) { + if (tcpsock_ < 0) { + printf("TcpClient not open\n"); + return false; + } +#ifdef _MSC_VER + int timeout_ms = u32Timeout; + int retVal = setsockopt(tcpsock_, SOL_SOCKET, SO_RCVTIMEO, (char*)&timeout_ms, + sizeof(timeout_ms)); +#else + uint32_t sec = u32Timeout / 1000; + uint32_t msec = u32Timeout % 1000; + + struct timeval timeout; + timeout.tv_sec = sec; + timeout.tv_usec = msec * 1000; + int retVal = setsockopt(tcpsock_, SOL_SOCKET, SO_RCVTIMEO, + (const void *)&timeout, sizeof(timeval)); +#endif + return retVal == 0; +} + + +int TcpSslClient::SetTimeout(uint32_t u32RecMillisecond, + uint32_t u32SendMillisecond) { + if (tcpsock_ < 0) { + printf("TcpClient not open\n"); + return -1; + } + m_u32RecTimeout = u32RecMillisecond; + m_u32SendTimeout = u32SendMillisecond; +#ifdef _MSC_VER + int timeout_ms = u32RecMillisecond; + int retVal = setsockopt(tcpsock_, SOL_SOCKET, SO_RCVTIMEO, + (char*)&timeout_ms, sizeof(timeout_ms)); +#else + uint32_t sec = u32RecMillisecond / 1000; + uint32_t msec = u32RecMillisecond % 1000; + + struct timeval timeout; + timeout.tv_sec = sec; + timeout.tv_usec = msec * 1000; + int retVal = setsockopt(tcpsock_, SOL_SOCKET, SO_RCVTIMEO, + (const void *)&timeout, sizeof(timeval)); +#endif + if (retVal == 0) { +#ifdef _MSC_VER + int timeout_ms = u32SendMillisecond; + retVal = setsockopt(tcpsock_, SOL_SOCKET, SO_SNDTIMEO, + (char*)&timeout_ms, sizeof(timeout_ms)); +#else + uint32_t sec = u32SendMillisecond / 1000; + uint32_t msec = u32SendMillisecond % 1000; + + struct timeval timeout; + timeout.tv_sec = sec; + timeout.tv_usec = msec * 1000; + retVal = setsockopt(tcpsock_, SOL_SOCKET, SO_SNDTIMEO, + (const void *)&timeout, sizeof(timeval)); +#endif + } + return retVal; +} + +void TcpSslClient::SetReceiveBufferSize(const uint32_t &size) { + if (tcpsock_ < 0) { + printf("TcpClient not open\n"); + return; + } + + m_u32ReceiveBufferSize = size; + int recbuffSize; + socklen_t optlen = sizeof(recbuffSize); + int ret = getsockopt(tcpsock_, SOL_SOCKET, SO_RCVBUF, (char*)&recbuffSize, &optlen); + if (ret == 0 && recbuffSize < size) { + setsockopt(tcpsock_, SOL_SOCKET, SO_RCVBUF, (char*)&size, sizeof(size)); + } +} \ No newline at end of file diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcParser/include/general_ptc_parser.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcParser/include/general_ptc_parser.h new file mode 100644 index 0000000..197eb5c --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcParser/include/general_ptc_parser.h @@ -0,0 +1,65 @@ +/************************************************************************************************ + Copyright(C)2023 Hesai Technology Co., Ltd. + All code in this repository is released under the terms of the following [Modified BSD License.] + Modified BSD License: + Redistribution and use in source and binary forms,with or without modification,are permitted + provided that the following conditions are met: + *Redistributions of source code must retain the above copyright notice,this list of conditions + and the following disclaimer. + *Redistributions in binary form must reproduce the above copyright notice,this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + *Neither the names of the University of Texas at Austin,nor Austin Robot Technology,nor the names of + other contributors maybe used to endorse or promote products derived from this software without + specific prior written permission. + THIS SOFTWARE IS PROVIDED BY THE COPYRIGH THOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED + WARRANTIES,INCLUDING,BUT NOT LIMITED TO,THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A + PARTICULAR PURPOSE ARE DISCLAIMED.IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR + ANY DIRECT,INDIRECT,INCIDENTAL,SPECIAL,EXEMPLARY,OR CONSEQUENTIAL DAMAGES(INCLUDING,BUT NOT LIMITED TO, + PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;LOSS OF USE,DATA,OR PROFITS;OR BUSINESS INTERRUPTION)HOWEVER + CAUSED AND ON ANY THEORY OF LIABILITY,WHETHER IN CONTRACT,STRICT LIABILITY,OR TORT(INCLUDING NEGLIGENCE + OR OTHERWISE)ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,EVEN IF ADVISED OF THE POSSIBILITY OF + SUCHDAMAGE. +************************************************************************************************/ + +/* + * File: general_parser.h + * Author: Zhang Xu + * Description: ptc protocol general parser class. + */ + +#ifndef GENERAL_PTC_PARSER_H_ +#define GENERAL_PTC_PARSER_H_ +#include +#include +#include "lidar_types.h" +#include "client_base.h" + +namespace hesai +{ +namespace lidar +{ +class GeneralPtcParser { +public: + GeneralPtcParser(){}; + virtual ~GeneralPtcParser(); + + // 字节流的打包。 + // 因为要将header和payload进行组装，通用类中无法实现。 + virtual bool PtcStreamEncode(const u8Array_t &payload, u8Array_t &byteStreamOut, uint8_t u8Cmd) = 0; + + // 字节流的解析（拆包） + virtual bool PtcStreamDecode(uint8_t cmd, uint8_t retcode, const u8Array_t &payload, int start_pos, int length, u8Array_t &res) = 0; + + // 对文件数据划分成包，并且对每一个包进行数据帧的封装 + virtual bool SplitFileFrames(const u8Array_t &file, uint8_t u8Cmd, std::vector& packages); + + virtual uint8_t GetHeaderIdentifier0() = 0; + virtual uint8_t GetHeaderIdentifier1() = 0; + virtual int GetHeaderSize() = 0; + virtual uint8_t GetHeaderReturnCode() const = 0; + virtual uint8_t GetHeaderCmd() const = 0; + virtual uint32_t GetHeaderPayloadLen() const = 0; +}; +} +} +#endif \ No newline at end of file diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcParser/include/ptc_1_0_parser.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcParser/include/ptc_1_0_parser.h new file mode 100644 index 0000000..056a852 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcParser/include/ptc_1_0_parser.h @@ -0,0 +1,110 @@ +/************************************************************************************************ + Copyright(C)2023 Hesai Technology Co., Ltd. + All code in this repository is released under the terms of the following [Modified BSD License.] + Modified BSD License: + Redistribution and use in source and binary forms,with or without modification,are permitted + provided that the following conditions are met: + *Redistributions of source code must retain the above copyright notice,this list of conditions + and the following disclaimer. + *Redistributions in binary form must reproduce the above copyright notice,this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + *Neither the names of the University of Texas at Austin,nor Austin Robot Technology,nor the names of + other contributors maybe used to endorse or promote products derived from this software without + specific prior written permission. + THIS SOFTWARE IS PROVIDED BY THE COPYRIGH THOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED + WARRANTIES,INCLUDING,BUT NOT LIMITED TO,THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A + PARTICULAR PURPOSE ARE DISCLAIMED.IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR + ANY DIRECT,INDIRECT,INCIDENTAL,SPECIAL,EXEMPLARY,OR CONSEQUENTIAL DAMAGES(INCLUDING,BUT NOT LIMITED TO, + PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;LOSS OF USE,DATA,OR PROFITS;OR BUSINESS INTERRUPTION)HOWEVER + CAUSED AND ON ANY THEORY OF LIABILITY,WHETHER IN CONTRACT,STRICT LIABILITY,OR TORT(INCLUDING NEGLIGENCE + OR OTHERWISE)ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,EVEN IF ADVISED OF THE POSSIBILITY OF + SUCHDAMAGE. +************************************************************************************************/ + +/* + * File: ptc_1_0_parser.h + * Author: Zhang Xu + * Description: ptc 1.0 protocol parser class. + */ + +#ifndef PTC_1_0_PARSER_H_ +#define PTC_1_0_PARSER_H_ +#ifdef _MSC_VER +#include +#endif +#include +#include +#include "general_ptc_parser.h" +#ifdef _MSC_VER +#define PACKED +#pragma pack(push, 1) +#else +#define PACKED __attribute__((packed)) +#endif +namespace hesai +{ +namespace lidar +{ +struct PTCHeader_1_0 { + uint8_t identifier0_; // protocol identifier + uint8_t identifier1_; // protocol identifier + uint8_t cmd_; // command code + uint8_t return_code_; // return code + uint32_t payload_len_; // length of payload for the command + + PTCHeader_1_0() { + identifier0_ = 0x47; + identifier1_ = 0x74; + return_code_ = 0; + payload_len_ = 0; + } + void init(uint8_t cmd) { + identifier0_ = 0x47; + identifier1_ = 0x74; + cmd_ = cmd; + return_code_ = 0; + payload_len_ = 0; + } + + bool IsValidIdentifier() const { return identifier0_ == 0x47 && identifier1_ == 0x74; } + bool IsValidReturnCode() const { return IsValidIdentifier() && return_code_ == 0; } + uint8_t GetIdentifier0() { return identifier0_; } + uint8_t GetIdentifier1() { return identifier1_; } + uint8_t GetReturnCode() const { return return_code_; } + uint8_t GetCmd() const {return cmd_; } +#ifdef _MSC_VER + uint32_t GetPayloadLen() const { return boost::endian::big_to_native(payload_len_); } + void SetPayloadLen(uint32_t u32Len) { payload_len_ = boost::endian::native_to_big(u32Len); } +#else + uint32_t GetPayloadLen() const { return be32toh(payload_len_); } + void SetPayloadLen(uint32_t u32Len) { payload_len_ = htobe32(u32Len); } +#endif + +} PACKED; +#ifdef _MSC_VER +#pragma pack(pop) +#endif + +class Ptc_1_0_parser : public GeneralPtcParser { +public: + ~Ptc_1_0_parser(){}; + // 字节流的打包。 + // 因为要将header和payload进行组装 + bool PtcStreamEncode(const u8Array_t &payload, u8Array_t &byteStreamOut, uint8_t u8Cmd); + + // 字节流的解析（拆包） + bool PtcStreamDecode(uint8_t cmd, uint8_t retcode, const u8Array_t &payload, int start_pos, int length, u8Array_t &res); + + uint8_t GetHeaderIdentifier0() { return header_.identifier0_; } + uint8_t GetHeaderIdentifier1() { return header_.identifier1_; } + int GetHeaderSize() { return sizeof(PTCHeader_1_0); } + uint8_t GetHeaderReturnCode() const { return header_.return_code_; } + uint8_t GetHeaderCmd() const {return header_.cmd_; } + uint32_t GetHeaderPayloadLen() const { return header_.GetPayloadLen();} + +private: + PTCHeader_1_0 header_; +}; +} +} +#endif \ No newline at end of file diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcParser/include/ptc_2_0_parser.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcParser/include/ptc_2_0_parser.h new file mode 100644 index 0000000..0e22e64 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcParser/include/ptc_2_0_parser.h @@ -0,0 +1,123 @@ +/************************************************************************************************ + Copyright(C)2023 Hesai Technology Co., Ltd. + All code in this repository is released under the terms of the following [Modified BSD License.] + Modified BSD License: + Redistribution and use in source and binary forms,with or without modification,are permitted + provided that the following conditions are met: + *Redistributions of source code must retain the above copyright notice,this list of conditions + and the following disclaimer. + *Redistributions in binary form must reproduce the above copyright notice,this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + *Neither the names of the University of Texas at Austin,nor Austin Robot Technology,nor the names of + other contributors maybe used to endorse or promote products derived from this software without + specific prior written permission. + THIS SOFTWARE IS PROVIDED BY THE COPYRIGH THOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED + WARRANTIES,INCLUDING,BUT NOT LIMITED TO,THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A + PARTICULAR PURPOSE ARE DISCLAIMED.IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR + ANY DIRECT,INDIRECT,INCIDENTAL,SPECIAL,EXEMPLARY,OR CONSEQUENTIAL DAMAGES(INCLUDING,BUT NOT LIMITED TO, + PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;LOSS OF USE,DATA,OR PROFITS;OR BUSINESS INTERRUPTION)HOWEVER + CAUSED AND ON ANY THEORY OF LIABILITY,WHETHER IN CONTRACT,STRICT LIABILITY,OR TORT(INCLUDING NEGLIGENCE + OR OTHERWISE)ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,EVEN IF ADVISED OF THE POSSIBILITY OF + SUCHDAMAGE. +************************************************************************************************/ + +/* + * File: ptc_2_0_parser.h + * Author: Zhang Xu + * Description: ptc 2.0 protocol parser class. + */ +#ifndef PTC_2_0_PARSER_H_ +#define PTC_2_0_PARSER_H_ +#ifdef _MSC_VER +#include +#endif +#include +#include +#include "lidar_types.h" +#include "general_ptc_parser.h" +#ifdef _MSC_VER +#define PACKED +#pragma pack(push, 1) +#else +#define PACKED __attribute__((packed)) +#endif + +namespace hesai +{ +namespace lidar +{ +struct PTCHeader_2_0 { + uint8_t identifier0_; // protocol identifier + uint8_t identifier1_; // protocol identifier + uint8_t ptc_version_; // ptc版本号 + uint8_t cmd_; // command code + uint8_t return_code_; // return code + uint8_t reserved_; // 预留1字节 + uint8_t integrity_check_type_; // 校验类型 + uint8_t integrity_data_length_; // 校验数据长度 + uint32_t payload_len_; // length of payload for the command + + PTCHeader_2_0() { + identifier0_ = 0x57; + identifier1_ = 0x75; + ptc_version_ = 1; + integrity_check_type_ = 0; + return_code_ = 0; + reserved_ = 0; + integrity_data_length_ = 0; + payload_len_ = 0; + } + void init(uint8_t cmd) { + identifier0_ = 0x57; + identifier1_ = 0x75; + ptc_version_ = 1; + cmd_ = cmd; + integrity_check_type_ = 0; + return_code_ = 0; + reserved_ = 0; + integrity_data_length_ = 0; + payload_len_ = 0; + } + + bool IsValidIdentifier() const { return identifier0_ == 0x57 && identifier1_ == 0x75; } + bool IsValidReturnCode() const { return IsValidIdentifier() && return_code_ == 0; } + uint8_t GetIdentifier0() { return identifier0_; } + uint8_t GetIdentifier1() { return identifier1_; } + uint8_t GetReturnCode() const { return return_code_; } + uint8_t GetCmd() const {return cmd_; } +#ifdef _MSC_VER + uint32_t GetPayloadLen() const { return boost::endian::big_to_native(payload_len_); } + void SetPayloadLen(uint32_t u32Len) { payload_len_ = boost::endian::native_to_big(u32Len); } +#else + uint32_t GetPayloadLen() const { return be32toh(payload_len_); } + void SetPayloadLen(uint32_t u32Len) { payload_len_ = htobe32(u32Len); } +#endif + +} PACKED; +#ifdef _MSC_VER +#pragma pack(pop) +#endif + +class Ptc_2_0_parser : public GeneralPtcParser { +public: + ~Ptc_2_0_parser(){}; + // 字节流的打包。 + // 因为要将header和payload进行组装 + bool PtcStreamEncode(const u8Array_t &payload, u8Array_t &byteStreamOut, uint8_t u8Cmd); + + // 字节流的解析（拆包） + bool PtcStreamDecode(uint8_t cmd, uint8_t retcode, const u8Array_t &payload, int start_pos, int length, u8Array_t &res); + + uint8_t GetHeaderIdentifier0() { return header_.identifier0_; } + uint8_t GetHeaderIdentifier1() { return header_.identifier1_; } + int GetHeaderSize() { return sizeof(PTCHeader_2_0); } + uint8_t GetHeaderReturnCode() const { return header_.return_code_; } + uint8_t GetHeaderCmd() const {return header_.cmd_; } + uint32_t GetHeaderPayloadLen() const { return header_.GetPayloadLen();} + +private: + PTCHeader_2_0 header_; +}; +} +} +#endif \ No newline at end of file diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcParser/ptc_parser.cc b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcParser/ptc_parser.cc new file mode 100644 index 0000000..9d45409 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcParser/ptc_parser.cc @@ -0,0 +1,63 @@ +/************************************************************************************************ + Copyright(C)2023 Hesai Technology Co., Ltd. + All code in this repository is released under the terms of the following [Modified BSD License.] + Modified BSD License: + Redistribution and use in source and binary forms,with or without modification,are permitted + provided that the following conditions are met: + *Redistributions of source code must retain the above copyright notice,this list of conditions + and the following disclaimer. + *Redistributions in binary form must reproduce the above copyright notice,this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + *Neither the names of the University of Texas at Austin,nor Austin Robot Technology,nor the names of + other contributors maybe used to endorse or promote products derived from this software without + specific prior written permission. + THIS SOFTWARE IS PROVIDED BY THE COPYRIGH THOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED + WARRANTIES,INCLUDING,BUT NOT LIMITED TO,THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A + PARTICULAR PURPOSE ARE DISCLAIMED.IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR + ANY DIRECT,INDIRECT,INCIDENTAL,SPECIAL,EXEMPLARY,OR CONSEQUENTIAL DAMAGES(INCLUDING,BUT NOT LIMITED TO, + PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;LOSS OF USE,DATA,OR PROFITS;OR BUSINESS INTERRUPTION)HOWEVER + CAUSED AND ON ANY THEORY OF LIABILITY,WHETHER IN CONTRACT,STRICT LIABILITY,OR TORT(INCLUDING NEGLIGENCE + OR OTHERWISE)ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,EVEN IF ADVISED OF THE POSSIBILITY OF + SUCHDAMAGE. +************************************************************************************************/ + +/* + * File: ptc_parser.cc + * Author: Zhang Xu + * Description: ptc parser class is an interface layer. + */ +#include "ptc_parser.h" + +using namespace hesai::lidar; + +PtcParser::~PtcParser() { + delete parser_; + parser_ = nullptr; +}; + +PtcParser::PtcParser(uint8_t ptc_version) : ptc_version_(ptc_version) { + if(ptc_version == 1) { + // parser_ = std::dynamic_pointer_cast(parser_); + parser_ = new Ptc_1_0_parser(); + } else if(ptc_version == 2) { + // parser_ = std::dynamic_pointer_cast(parser_); + parser_ = new Ptc_2_0_parser(); + } +} + +// 字节流的打包。 +// 因为要将header和payload进行组装 +bool PtcParser::PtcStreamEncode(const u8Array_t &payload, u8Array_t &byteStreamOut, uint8_t u8Cmd) { + if(parser_ != nullptr) { + return parser_->PtcStreamEncode(payload, byteStreamOut, u8Cmd); + } + return false; +} + +// 字节流的解析（拆包） +bool PtcParser::PtcStreamDecode(uint8_t cmd, uint8_t retcode, const u8Array_t &payload, int start_pos, int length, u8Array_t &res) { + if(parser_ != nullptr) { + return parser_->PtcStreamDecode(cmd, retcode, payload, start_pos, length, res); + } + return false; +} \ No newline at end of file diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcParser/ptc_parser.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcParser/ptc_parser.h new file mode 100644 index 0000000..ba22720 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcParser/ptc_parser.h @@ -0,0 +1,71 @@ +/************************************************************************************************ + Copyright(C)2023 Hesai Technology Co., Ltd. + All code in this repository is released under the terms of the following [Modified BSD License.] + Modified BSD License: + Redistribution and use in source and binary forms,with or without modification,are permitted + provided that the following conditions are met: + *Redistributions of source code must retain the above copyright notice,this list of conditions + and the following disclaimer. + *Redistributions in binary form must reproduce the above copyright notice,this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + *Neither the names of the University of Texas at Austin,nor Austin Robot Technology,nor the names of + other contributors maybe used to endorse or promote products derived from this software without + specific prior written permission. + THIS SOFTWARE IS PROVIDED BY THE COPYRIGH THOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED + WARRANTIES,INCLUDING,BUT NOT LIMITED TO,THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A + PARTICULAR PURPOSE ARE DISCLAIMED.IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR + ANY DIRECT,INDIRECT,INCIDENTAL,SPECIAL,EXEMPLARY,OR CONSEQUENTIAL DAMAGES(INCLUDING,BUT NOT LIMITED TO, + PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;LOSS OF USE,DATA,OR PROFITS;OR BUSINESS INTERRUPTION)HOWEVER + CAUSED AND ON ANY THEORY OF LIABILITY,WHETHER IN CONTRACT,STRICT LIABILITY,OR TORT(INCLUDING NEGLIGENCE + OR OTHERWISE)ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,EVEN IF ADVISED OF THE POSSIBILITY OF + SUCHDAMAGE. +************************************************************************************************/ + +/* + * File: ptc_parser.h + * Author: Zhang Xu + * Description: ptc parser class is an interface layer. + */ +#ifndef PTC_PARSER_H_ +#define PTC_PARSER_H_ +#include "general_ptc_parser.h" +#include "ptc_1_0_parser.h" +#include "ptc_2_0_parser.h" +#include "lidar_types.h" +#include + +namespace hesai +{ +namespace lidar +{ +// class PtcParser +// the PtcParser class is an interface layer. It instantiates a specific ptc parser class, +// which is determined by the version of ptc's protocol, 1.0 or 2.0. +// PtcParser mainly parsers ptc packets and get values the request want. +class PtcParser { +public: + PtcParser(uint8_t ptc_version); + PtcParser(){}; + virtual ~PtcParser(); + + // 字节流的打包。 + // 因为要将header和payload进行组装 + bool PtcStreamEncode(const u8Array_t &payload, u8Array_t &byteStreamOut, uint8_t u8Cmd); + + // 字节流的解析（拆包） + bool PtcStreamDecode(uint8_t cmd, uint8_t retcode, const u8Array_t &payload, int start_pos, int length, u8Array_t &res); + + uint8_t GetHeaderIdentifier0() { return parser_->GetHeaderIdentifier0(); } + uint8_t GetHeaderIdentifier1() { return parser_->GetHeaderIdentifier1(); } + int GetPtcParserHeaderSize() { return parser_->GetHeaderSize(); } + uint8_t GetHeaderReturnCode() { return parser_->GetHeaderReturnCode(); } + uint8_t GetHeaderCmd() { return parser_->GetHeaderCmd(); } + uint32_t GetHeaderPayloadLen() { return parser_->GetHeaderPayloadLen(); } + +private: + GeneralPtcParser* parser_; + uint8_t ptc_version_; +}; +} +} +#endif \ No newline at end of file diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcParser/src/general_ptc_parser.cc b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcParser/src/general_ptc_parser.cc new file mode 100644 index 0000000..ac721b0 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcParser/src/general_ptc_parser.cc @@ -0,0 +1,60 @@ +/************************************************************************************************ + Copyright(C)2023 Hesai Technology Co., Ltd. + All code in this repository is released under the terms of the following [Modified BSD License.] + Modified BSD License: + Redistribution and use in source and binary forms,with or without modification,are permitted + provided that the following conditions are met: + *Redistributions of source code must retain the above copyright notice,this list of conditions + and the following disclaimer. + *Redistributions in binary form must reproduce the above copyright notice,this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + *Neither the names of the University of Texas at Austin,nor Austin Robot Technology,nor the names of + other contributors maybe used to endorse or promote products derived from this software without + specific prior written permission. + THIS SOFTWARE IS PROVIDED BY THE COPYRIGH THOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED + WARRANTIES,INCLUDING,BUT NOT LIMITED TO,THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A + PARTICULAR PURPOSE ARE DISCLAIMED.IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR + ANY DIRECT,INDIRECT,INCIDENTAL,SPECIAL,EXEMPLARY,OR CONSEQUENTIAL DAMAGES(INCLUDING,BUT NOT LIMITED TO, + PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;LOSS OF USE,DATA,OR PROFITS;OR BUSINESS INTERRUPTION)HOWEVER + CAUSED AND ON ANY THEORY OF LIABILITY,WHETHER IN CONTRACT,STRICT LIABILITY,OR TORT(INCLUDING NEGLIGENCE + OR OTHERWISE)ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,EVEN IF ADVISED OF THE POSSIBILITY OF + SUCHDAMAGE. +************************************************************************************************/ + +/* + * File: general_parser.cc + * Author: Zhang Xu + */ + +#include "general_ptc_parser.h" +#include "ptc_client.h" +using namespace hesai::lidar; + +GeneralPtcParser::~GeneralPtcParser(){} + +// 对文件数据划分成包，并且对每一个包进行数据帧的封装 +// 对需要分成多个包的payload进行split和封装成包 +bool GeneralPtcParser::SplitFileFrames(const u8Array_t &file, uint8_t u8Cmd, std::vector& packages) { + const int FRAME_LENGTH = 1024; + int file_length = file.size(); + std::vector frames; + // split file -> frames + int pos = 0; + while(file_length > 0) { + // u8Array_t tmp = u8Array_t(pos, pos + min(FRAME_LENGTH, file_length)); + // frames.push_back(tmp); + // pos += FRAME_LENGTH; + // file_length -= FRAME_LENGTH; + } + // 对frame进行封装成包 + for(auto &frame : frames) { + u8Array_t cur; + bool f = PtcStreamEncode(frame, cur, u8Cmd); + if(!f) { + std::cout << "GeneralPtcParser::PtcFilestreamSend pack frame failed!" << std::endl; + return false; + } + packages.push_back(cur); + } + return true; +} \ No newline at end of file diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcParser/src/ptc_1_0_parser.cc b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcParser/src/ptc_1_0_parser.cc new file mode 100644 index 0000000..6f86f1f --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcParser/src/ptc_1_0_parser.cc @@ -0,0 +1,68 @@ +/************************************************************************************************ + Copyright(C)2023 Hesai Technology Co., Ltd. + All code in this repository is released under the terms of the following [Modified BSD License.] + Modified BSD License: + Redistribution and use in source and binary forms,with or without modification,are permitted + provided that the following conditions are met: + *Redistributions of source code must retain the above copyright notice,this list of conditions + and the following disclaimer. + *Redistributions in binary form must reproduce the above copyright notice,this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + *Neither the names of the University of Texas at Austin,nor Austin Robot Technology,nor the names of + other contributors maybe used to endorse or promote products derived from this software without + specific prior written permission. + THIS SOFTWARE IS PROVIDED BY THE COPYRIGH THOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED + WARRANTIES,INCLUDING,BUT NOT LIMITED TO,THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A + PARTICULAR PURPOSE ARE DISCLAIMED.IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR + ANY DIRECT,INDIRECT,INCIDENTAL,SPECIAL,EXEMPLARY,OR CONSEQUENTIAL DAMAGES(INCLUDING,BUT NOT LIMITED TO, + PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;LOSS OF USE,DATA,OR PROFITS;OR BUSINESS INTERRUPTION)HOWEVER + CAUSED AND ON ANY THEORY OF LIABILITY,WHETHER IN CONTRACT,STRICT LIABILITY,OR TORT(INCLUDING NEGLIGENCE + OR OTHERWISE)ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,EVEN IF ADVISED OF THE POSSIBILITY OF + SUCHDAMAGE. +************************************************************************************************/ + +/* + * File: ptc_1_0_parser.cc + * Author: Zhang Xu + */ +#include "ptc_1_0_parser.h" + +using namespace hesai::lidar; + +// 字节流的打包 +// 对只需要一个包的payload进行封装成包 +bool Ptc_1_0_parser::PtcStreamEncode(const u8Array_t &payload, u8Array_t &byteStreamOut, uint8_t u8Cmd) { + byteStreamOut.resize(sizeof(PTCHeader_1_0) + payload.size()); //设置byteStreamOut长度 + // std::cout << sizeof(PTCHeader_1_0) << std::endl; + PTCHeader_1_0 *pHeader = (PTCHeader_1_0 *)byteStreamOut.data(); + // std::cout << "pHeader's identifier0 : " << pHeader->GetIdentifier0() << std::endl; + // std::cout << "pHeader's identifier1 : " << pHeader->GetIdentifier1() << std::endl; + // std::cout << "pHeader's payload length : " << pHeader->GetPayloadLen() << std::endl; + // std::cout << "-------Ptc_1_0_parser::PtcStreamEncode()" << std::endl; + pHeader->init(u8Cmd); + pHeader->SetPayloadLen(payload.size()); + pHeader++; + + memcpy((void *)pHeader, payload.data(), payload.size()); + return true; +} + +// 字节流的解析（拆包） +// 根据协议解析字节流，并且取出对应的结果 +// 给定所要取的值在payload中的起始位置和长度，将值取出赋给res +bool Ptc_1_0_parser::PtcStreamDecode(uint8_t cmd, uint8_t retcode, const u8Array_t &payload, int start_pos, int length, u8Array_t &res) { + // PTCHeader_1_0 *header = dynamic_cast(base_header); + // if(header == nullptr) { + // std::cout << "Ptc_1_0_parser::PtcStreamDecode failed! header is not a 1.0 version!" << std::endl; + // return false; + // } + std::cout << length << " " << payload.size() << std::endl; + res = u8Array_t(payload.begin() + start_pos, payload.begin() + start_pos + length); + + // for(int i = 0; i < res.size(); i++) { + // printf("%c", char(res[i])); + // } + + std::cout << std::endl << "Ptc_1_0_parser::PtcStreamDecode success!" << std::endl; + return true; +} \ No newline at end of file diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcParser/src/ptc_2_0_parser.cc b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcParser/src/ptc_2_0_parser.cc new file mode 100644 index 0000000..a12c41a --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcParser/src/ptc_2_0_parser.cc @@ -0,0 +1,98 @@ +/************************************************************************************************ + Copyright(C)2023 Hesai Technology Co., Ltd. + All code in this repository is released under the terms of the following [Modified BSD License.] + Modified BSD License: + Redistribution and use in source and binary forms,with or without modification,are permitted + provided that the following conditions are met: + *Redistributions of source code must retain the above copyright notice,this list of conditions + and the following disclaimer. + *Redistributions in binary form must reproduce the above copyright notice,this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + *Neither the names of the University of Texas at Austin,nor Austin Robot Technology,nor the names of + other contributors maybe used to endorse or promote products derived from this software without + specific prior written permission. + THIS SOFTWARE IS PROVIDED BY THE COPYRIGH THOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED + WARRANTIES,INCLUDING,BUT NOT LIMITED TO,THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A + PARTICULAR PURPOSE ARE DISCLAIMED.IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR + ANY DIRECT,INDIRECT,INCIDENTAL,SPECIAL,EXEMPLARY,OR CONSEQUENTIAL DAMAGES(INCLUDING,BUT NOT LIMITED TO, + PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;LOSS OF USE,DATA,OR PROFITS;OR BUSINESS INTERRUPTION)HOWEVER + CAUSED AND ON ANY THEORY OF LIABILITY,WHETHER IN CONTRACT,STRICT LIABILITY,OR TORT(INCLUDING NEGLIGENCE + OR OTHERWISE)ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,EVEN IF ADVISED OF THE POSSIBILITY OF + SUCHDAMAGE. +************************************************************************************************/ + +/* + * File: ptc_2_0_parser.cc + * Author: Zhang Xu + */ + +#include "ptc_2_0_parser.h" +using namespace hesai::lidar; + +// 字节流的打包。 +// 因为要将header和payload进行组装 +bool Ptc_2_0_parser::PtcStreamEncode(const u8Array_t &payload, u8Array_t &byteStreamOut, uint8_t u8Cmd) { + byteStreamOut.resize(sizeof(PTCHeader_2_0) + payload.size()); //设置byteStreamOut长度 + PTCHeader_2_0 *pHeader = (PTCHeader_2_0 *)byteStreamOut.data(); + pHeader->init(u8Cmd); + pHeader->SetPayloadLen(payload.size()); + pHeader++; + + memcpy((void *)pHeader, payload.data(), payload.size()); + return true; +} + +// 字节流的解析（拆包） +bool Ptc_2_0_parser::PtcStreamDecode(uint8_t cmd, uint8_t retcode, const u8Array_t &payload, int start_pos, int length, u8Array_t &res) { + // PTCHeader_2_0 *header = dynamic_cast(base_header); + // if(header == nullptr) { + // std::cout << "Ptc_2_0_parser::PtcStreamDecode failed! header is not a 2.0 version!" << std::endl; + // return false; + // } + // uint8_t cmd = header->GetCmd(); + // uint8_t retcode = header->GetReturnCode(); + + if(cmd == 0x01) { // 读变量 + res = payload; + } else if(cmd == 0x02) { // 写变量 + if(payload.size() == 0) { + return true; + } + res = payload; // 写值不成功时返回对应的变量ID + return false; + } else if(cmd == 0x03) { // 读变量组 + res = payload; + } else if(cmd == 0x04) { // 写变量组 + if(payload.size() == 0) { + return true; + } + res = payload; // 写值不成功时返回对应的变量ID + return false; + } else if(cmd == 0x05) { // 读设备地址 + res = payload; + } else if(cmd == 0x06) { // 写设备地址 + if(payload.size() == 0) { + return true; + } + res = payload; // 写值不成功时返回对应的变量ID + return false; + } else if(cmd == 0x07) { // 读设备连续地址 + res = payload; + } else if(cmd == 0x08) { // 写设备连续地址 + if(payload.size() == 0) { + return true; + } + res = payload; // 写值不成功时返回对应的变量ID + return false; + } else if(cmd == 0x09) { // 读文件 + res = payload; + } else if(cmd == 0x0A) { // 写文件 + if(payload.size() == 0 && retcode == 0) { + return true; + } + res.push_back(retcode); // 写文件失败时用res记录return code的值 + return false; + } + std::cout << "Ptc_2_0_parser::PtcStreamDecode success!" << std::endl; + return true; +} \ No newline at end of file diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Source/include/pcap_saver.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Source/include/pcap_saver.h new file mode 100644 index 0000000..8b836f8 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Source/include/pcap_saver.h @@ -0,0 +1,104 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ +#ifndef PCPASAVER_H_ +#define PCPASAVER_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "pcap_source.h" +#include "blocking_ring.h" +#include +#include +#include +#include +#include "pcap_source.h" +#include "lidar_types.h" +namespace hesai +{ +namespace lidar +{ +using Clock = std::chrono::system_clock; +using Time = Clock::time_point; +using Period = Clock::period; +using Duration = Clock::duration; + +struct PandarPacket { + uint32_t size; + uint16_t port; + uint8_t buffer[1500]; + PandarPacket(const uint8_t *data = nullptr, uint32_t sz = 0, uint16_t prt = 0) + : size(sz), port(prt) { + memcpy(buffer, data, size); + } +}; + +class PcapSaver { +public: + bool tcp_dumped_; +private: + std::ofstream ofs_; + std::string pcap_path_; + + using Container = BlockingRing; + std::shared_ptr packets_cache_; + bool dumping_; + bool dumping_blocked_; + // std::mutex _mutex; + std::thread dumping_thread_; +public: + PcapSaver(); + PcapSaver(const PcapSaver&) = delete; + PcapSaver& operator=(const PcapSaver&) = delete; + ~PcapSaver(); + + std::string pcap_path() const; + void SetPcapPath(std::string pcap_path); + int Save(); + int Save(const std::string &recordPath, const UdpFrame_t &packets, + int port = 2368); + int Save(const std::string &recordPath, const UdpFrameArray_t &packets, + int port = 2368); + void Dump(const uint8_t*, uint32_t, uint16_t port = 2368); + void TcpDump(const uint8_t*, uint32_t, uint32_t max_pkt_len = 1500 - sizeof(PcapTCPHeader), uint16_t port = 2368); + void close(); +}; +} // namespace lidar +} // namespace hesai + + +#endif // PCPASTREAMER_H_ diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Source/include/pcap_source.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Source/include/pcap_source.h new file mode 100644 index 0000000..3c00f3e --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Source/include/pcap_source.h @@ -0,0 +1,197 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ +#ifndef PCPASTREAMER_H_ +#define PCPASTREAMER_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "source.h" +namespace hesai +{ +namespace lidar +{ +#pragma pack(push, 1) +struct PcapHeader { + uint32_t magic_number; + uint16_t version_major; + uint16_t version_minor; + int32_t thiszone; + uint32_t sigfigs; + uint32_t snaplen; + uint32_t network; +}; +static_assert(sizeof(PcapHeader) == 24); + +struct PcapRecord { + uint32_t ts_sec; + uint32_t ts_usec; + uint32_t incl_len; + uint32_t orig_len; +}; +static_assert(sizeof(PcapRecord) == 16); +struct Ethernet { + uint8_t destination_MAC_address[6]; + uint8_t source_MAC_address[6]; + uint16_t ether_type; +}; +static_assert(sizeof(Ethernet) == 14); +struct PcapIPHeader { + Ethernet ether; + struct IP { + uint8_t version : 4; /* version */ + uint8_t length : 4; /* header length */ + uint8_t type_of_service;/* type of service */ + uint16_t total_length; /* total length */ + uint16_t identification; /* identification */ + uint16_t fragment_offset;/* fragment offset field */ +#define IP_DF 0x4000 /* dont fragment flag */ +#define IP_MF 0x2000 /* more fragments flag */ + uint8_t time_to_live; /* time to live */ + uint8_t protocol; /* protocol */ + uint16_t checksum; /* checksum */ + uint32_t srce_addr; /* source address*/ + uint32_t dist_addr; /* destination address */ + } ip; + PcapIPHeader(uint8_t protocol, uint16_t pkt_len); +}; +static_assert(sizeof(PcapIPHeader::IP) == 20); +struct PcapIPv6Header { + Ethernet ether; + struct IPv6 { + uint32_t version : 4; /* version */ + uint32_t flow_level : 8; + uint32_t flow_label : 20; + uint16_t payload_length; + uint8_t next_header; /* protocol */ + uint8_t hop_limit; + uint8_t srce_addr[16]; /* source address*/ + uint8_t dist_addr[16]; /* destination address */ + } ipv6; + PcapIPv6Header(uint8_t protocol, uint16_t pkt_len); +}; +static_assert(sizeof(PcapIPv6Header::IPv6) == 40); +struct UDP { + uint16_t source_port; + uint16_t distination_port; + uint16_t length; + uint16_t check_sum; +}; +static_assert(sizeof(UDP) == 8); +struct PcapUDPHeader : public PcapIPHeader { + UDP udp; + PcapUDPHeader(uint16_t pkt_len, uint16_t port = 2368); +}; +static_assert(sizeof(PcapUDPHeader) == 42); +struct PcapUDPv6Header : public PcapIPv6Header { + UDP udp; + PcapUDPv6Header(uint16_t pkt_len, uint16_t port = 2368); +}; +static_assert(sizeof(PcapUDPv6Header) == 62); +struct TCP { + uint16_t source_port; + uint16_t distination_port; + uint32_t seq; + uint32_t ack; + uint8_t lenres : 4; + uint8_t reserved1 : 4; + uint8_t reserved2 : 2; + uint8_t flag : 6; + uint16_t win; + uint16_t check_sum; + uint16_t urp; +}; +static_assert(sizeof(TCP) == 20); +struct PcapTCPHeader : public PcapIPHeader { + TCP tcp; + PcapTCPHeader(uint16_t pkt_len, uint16_t port = 2368); +}; +static_assert(sizeof(PcapTCPHeader) == 54); +struct PcapTCPv6Header : public PcapIPv6Header { + TCP tcp; + PcapTCPv6Header(uint16_t pkt_len, uint16_t port = 2368); +}; +static_assert(sizeof(PcapTCPv6Header) == 74); +#pragma pack(pop) + +class PcapSource : public Source{ +public: + using Callback = std::function; +public: + struct Private; +private: + Private* _p; + std::string pcap_path_; + Callback udp_callback_; + Callback tcp_callback_; + PcapHeader pcap_header_; + PcapRecord pcap_record_; + UDP pcap_udp_header_; + TCP pcap_tcp_header_; + std::array payload_; + int packet_interval_; +public: + PcapSource(std::string path, int packet_interval); + PcapSource(const PcapSource&) = delete; + PcapSource& operator=(const PcapSource&) = delete; + virtual ~PcapSource(); + void setPcapPath(std::string path); + + Callback callback() const; + void callback(Callback); + Callback tcp_callback() const; + void tcp_callback(Callback); + size_t fpos() const; + void fpos(size_t); + std::string pcap_path() const; + int next(UdpPacket& udpPacket, uint16_t u16Len,int flags = 0, + int timeout = 1000); + virtual bool Open(); + virtual void Close(); + + virtual bool IsOpened(); + virtual int Send(uint8_t* u8Buf, uint16_t u16Len, int flags = 0); + virtual int Receive(UdpPacket& udpPacket, uint16_t u16Len, int flags = 0, + int timeout = 20000); + int distinationPort(); + void setPacketInterval(int microsecond); + virtual void SetSocketBufferSize(uint32_t u32BufSize) {}; +}; +#pragma pack(pop) +} // namespace lidar +} // namespace hesai +#endif // PCPASTREAMER_H_ diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Source/include/socket_source.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Source/include/socket_source.h new file mode 100644 index 0000000..9d7f68a --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Source/include/socket_source.h @@ -0,0 +1,89 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ + +/* + * File: socket_source.h + * Author: Felix Zou + * + * Created on Sep 5, 2019, 10:46 AM + */ + +#ifndef SOCKETSTREAMER_H +#define SOCKETSTREAMER_H + +#include +#include +#include "source.h" + +#ifdef _MSC_VER +#include +#include +#pragma comment(lib, "ws2_32.lib") // Winsock Library +#include +typedef int ssize_t; +#else +#include +#include +#include +#include +#include +#include +#include +#include +#include +#endif + +namespace hesai +{ +namespace lidar +{ + +class SocketSource : public Source{ + public: + SocketSource(uint16_t port = kUdpPort, std::string multicastIp = ""); + virtual ~SocketSource(); + + virtual bool Open(); + virtual void Close(); + virtual bool IsOpened(); + virtual int Send(uint8_t* u8Buf, uint16_t u16Len, int flags = 0); + virtual int Receive(UdpPacket& udpPacket, uint16_t u16Len, int flags = 0, + int timeout = 1000); + virtual void SetSocketBufferSize(uint32_t u32BufSize); +private: + std::string multicast_ip_; + std::string client_ip_; + uint16_t udp_port_; + SOCKET udp_sock_; + bool is_select_; + static const int32_t kUDPBufferSize = 26214400; // udp buffer size + static const uint16_t kUdpPort = 2368; +}; +} // namespace lidar +} // namespace hesai +#endif /* SOCKETSTREAMER_H */ diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Source/include/source.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Source/include/source.h new file mode 100644 index 0000000..8add7e7 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Source/include/source.h @@ -0,0 +1,77 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ + +/* + * File: source.h + * Author: Felix Zou + * + * Created on Sep 5, 2019, 10:46 AM + */ + +#ifndef STREAMER_H +#define STREAMER_H + +#include +#include +#include + +#ifdef _MSC_VER +#include +#include +#pragma comment(lib, "ws2_32.lib") // Winsock Library +typedef int socklen_t; +#else +#include +#include +#include +#include +#include +#include +typedef unsigned int SOCKET; +#define INVALID_SOCKET -1 +#define SOCKET_ERROR -1 +#endif +namespace hesai +{ +namespace lidar +{ +class Source { + public: + Source(); + virtual ~Source(); + virtual bool Open() = 0; + virtual void Close(); + virtual bool IsOpened() = 0; + virtual int Send(uint8_t* u8Buf, uint16_t u16Len, int flags = 0) = 0; + virtual int Receive(UdpPacket& udpPacket, uint16_t u16Len, int flags = 0, + int timeout = 1000) = 0; + virtual void SetSocketBufferSize(uint32_t u32BufSize) = 0; +}; +} // namespace lidar +} // namespace hesai +#endif /* STREAMER_H */ diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Source/src/pcap_saver.cc b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Source/src/pcap_saver.cc new file mode 100644 index 0000000..e732dae --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Source/src/pcap_saver.cc @@ -0,0 +1,224 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ +#include "pcap_saver.h" + +#include +#include +#include +#include +#include +using namespace hesai::lidar; +static constexpr uint32_t pcap_magic_number = 0xa1b2c3d4; + + +PcapSaver::PcapSaver() + : ofs_() + , pcap_path_("") + , dumping_(false) + , dumping_blocked_(false) + , tcp_dumped_(false) + , packets_cache_(new Container) +{} + +PcapSaver::~PcapSaver() +{ + PcapSaver::close(); +} + +std::string PcapSaver::pcap_path() const { + return pcap_path_; +} +void PcapSaver::SetPcapPath(std::string pcap_path) { + pcap_path_ = pcap_path; +} + + +int PcapSaver::Save() { + try + { + + PcapSaver::close(); + ofs_.open(pcap_path_, std::ios::binary); + if(!ofs_.good()) { + throw std::runtime_error(std::string("Fail to open .pcap file: \"") + (pcap_path_)+"\""); + } + PcapHeader pcap_header{ + pcap_magic_number, + 2, + 4, + 0, + 0, + 0xffff, + 1, + }; + ofs_.write((char*)&pcap_header, sizeof(pcap_header)); + // pcap_path_ = pcap_path; + { + dumping_ = true; + dumping_thread_ = std::thread([this]() { + using namespace std::chrono_literals; + while (dumping_ || packets_cache_->not_empty()) { + while (!dumping_blocked_ && packets_cache_->not_empty()) { + auto pkt = packets_cache_->pop_front(); + auto& len = pkt.size; + std::array data_with_fake_header; + *(PcapUDPHeader*)data_with_fake_header.data() = PcapUDPHeader(len, pkt.port); + std::memcpy(data_with_fake_header.data() + sizeof(PcapUDPHeader), pkt.buffer, len); + PcapRecord pcap_record; + Duration since_begin = Clock::now() - Time(); + Duration time_of_day = since_begin - int64_t(since_begin / 24h) * 24h; + pcap_record.ts_sec = time_of_day / 1s; + pcap_record.ts_usec = (time_of_day - pcap_record.ts_sec * 1s) / 1us; + pcap_record.orig_len = len + sizeof(PcapUDPHeader); + pcap_record.incl_len = len + sizeof(PcapUDPHeader); + ofs_.write((char*)&pcap_record, sizeof(pcap_record)); + ofs_.write((char*)&data_with_fake_header, pcap_record.incl_len); + } + std::this_thread::sleep_for(1ms); + } + }); + } + return 0; + } + catch(const std::exception& e) + { + std::cerr << e.what() << std::endl; + return -1; + } + +} + +void PcapSaver::Dump(const uint8_t* data, uint32_t len, uint16_t port) { + packets_cache_->push_back(PandarPacket(data, len, port)); +} +void PcapSaver::TcpDump(const uint8_t* data, uint32_t data_len, uint32_t max_pkt_len, uint16_t port) { + dumping_blocked_ = true; + using namespace std::chrono_literals; + std::this_thread::sleep_for(100ms); // delay to make sure Dump successful + + using namespace std::chrono_literals; + int remain_len = data_len, pkt_len = max_pkt_len; + int i = 0; + while ( remain_len > 0 ) { + pkt_len = (remain_len > max_pkt_len) ? max_pkt_len : remain_len; + + std::array data_with_fake_header; + *(PcapTCPHeader*)data_with_fake_header.data() = PcapTCPHeader(pkt_len, port); + std::memcpy(data_with_fake_header.data() + sizeof(PcapTCPHeader), data + i * max_pkt_len, pkt_len); + PcapRecord pcap_record; + Duration since_begin = Clock::now() - Time(); + Duration time_of_day = since_begin - int64_t(since_begin / 24h) * 24h; + pcap_record.ts_sec = time_of_day / 1s; + pcap_record.ts_usec = (time_of_day - pcap_record.ts_sec * 1s) / 1us; + pcap_record.orig_len = pkt_len + sizeof(PcapTCPHeader); + pcap_record.incl_len = pkt_len + sizeof(PcapTCPHeader); + ofs_.write((char*)&pcap_record, sizeof(pcap_record)); + ofs_.write((char*)&data_with_fake_header, pcap_record.incl_len); + + remain_len -= pkt_len; + i++; + } + + dumping_blocked_ = false; +} + +void PcapSaver::close() { + dumping_ = false; + if (dumping_thread_.joinable()) { + dumping_thread_.join(); + } + if (ofs_.is_open()) { + ofs_.close(); + } + // pcap_path_ = ""; + tcp_dumped_ = false; +} + +int PcapSaver::Save(const std::string& recordPath, const UdpFrame_t& packets, + int port) { + try { + std::ofstream ofs; + struct stat buf; + if (stat(recordPath.c_str(), &buf) != 0) { + ofs.open(recordPath, std::ios::binary | std::ios_base::app); + if (!ofs.good()) { + throw std::runtime_error(std::string("Fail to open .pcap file: \"") + + (pcap_path_) + "\""); + } + PcapHeader pcap_header{ + pcap_magic_number, 2, 4, 0, 0, 0xffff, 1, + }; + ofs.write((char*)&pcap_header, sizeof(pcap_header)); + } else { + ofs.open(recordPath, std::ios::binary | std::ios_base::app); + if (!ofs.good()) { + throw std::runtime_error(std::string("Fail to open .pcap file: \"") + + (pcap_path_) + "\""); + } + } + // static unsigned int time_begin = GetMicroTickCount(); + for (int i = 0; i < packets.size(); i++) { + auto pkt = PandarPacket(packets[i].buffer, packets[i].packet_len, port); + auto& len = packets[i].packet_len; + std::array data_with_fake_header; + *(PcapUDPHeader*)data_with_fake_header.data() = + PcapUDPHeader(len, pkt.port); + std::memcpy(data_with_fake_header.data() + sizeof(PcapUDPHeader), + pkt.buffer, len); + PcapRecord pcap_record; + // unsigned int time_now = GetMicroTickCount(); + // pcap_record.ts_sec = (time_now - time_begin) / 1000000; + // pcap_record.ts_usec = (time_now - time_begin - pcap_record.ts_sec); + pcap_record.orig_len = len + sizeof(PcapUDPHeader); + pcap_record.incl_len = len + sizeof(PcapUDPHeader); + ofs.write((char*)&pcap_record, sizeof(pcap_record)); + ofs.write((char*)&data_with_fake_header, pcap_record.incl_len); + } + ofs.close(); + return 0; + } catch (const std::exception& e) { + std::cerr << e.what() << std::endl; + return -1; + } +} + +int PcapSaver::Save(const std::string& recordPath, + const UdpFrameArray_t& packets, int port) { + int ret = 0; + for (int i = 0; i < packets.size(); i++) { + ret = Save(recordPath, packets[i], port); + if (ret != 0) { + break; + } + } + + return ret; +} + + + diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Source/src/pcap_source.cc b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Source/src/pcap_source.cc new file mode 100644 index 0000000..a8f2b6f --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Source/src/pcap_source.cc @@ -0,0 +1,318 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ +#include "pcap_source.h" + +#include +#include +#include +// #include "hesai/time/time_basic.hh" +#include +#include + +using namespace hesai::lidar; + +static constexpr uint32_t pcap_magic_number = 0xa1b2c3d4; + +PcapIPHeader::PcapIPHeader(uint8_t protocol, uint16_t pkt_len) + : ether{ {0xff,0xff,0xff,0xff,0xff,0xff}, {0x00, 0x0a, 0x35, 0x00, 0x1e, 0x53}, 0x0008 } + , ip{ 0x5, 0x4, 0x00, uint16_t((((pkt_len + 20) & 0xff) << 8) | ((pkt_len + 20) >> 8)), 0xf17a, 0x0040, 0x40, protocol, 0x80f8, 0xc901a8c0, 0xffffffff } +{} + +PcapIPv6Header::PcapIPv6Header(uint8_t protocol, uint16_t pkt_len) + : ether{ {0xff,0xff,0xff,0xff,0xff,0xff}, {0x00, 0x0a, 0x35, 0x00, 0x1e, 0x53}, 0xdd86 } + , ipv6 { 0x6, 0, 0, 0, uint16_t((((pkt_len + 40) & 0xff) << 8) | ((pkt_len + 40) >> 8)), protocol, } +{} + +PcapUDPHeader::PcapUDPHeader(uint16_t pkt_len, uint16_t port) + : PcapIPHeader::PcapIPHeader(0x11, pkt_len + 8) + , udp{ 0x1027, uint16_t(((port & 0xff) << 8) | (port >> 8)), uint16_t((((pkt_len + 8) & 0xff) << 8) | ((pkt_len + 8) >> 8)), 0x0000 } +{} + +PcapUDPv6Header::PcapUDPv6Header(uint16_t pkt_len, uint16_t port) + : PcapIPv6Header::PcapIPv6Header(0x11, pkt_len + 8) + , udp{ 0x1027, uint16_t(((port & 0xff) << 8) | (port >> 8)), uint16_t((((pkt_len + 8) & 0xff) << 8) | ((pkt_len + 8) >> 8)), 0x0000 } +{} + +PcapTCPHeader::PcapTCPHeader(uint16_t pkt_len, uint16_t port) + : PcapIPHeader::PcapIPHeader(0x06, pkt_len + 20) + , tcp{ 0x1027, uint16_t(((port & 0xff) << 8) | (port >> 8)), 0x000000000, 0x00000000, 0x4, 0x0, 0x0, 0x00, 0x0000, 0x0000, 0x0000 } +{} + +PcapTCPv6Header::PcapTCPv6Header(uint16_t pkt_len, uint16_t port) + : PcapIPv6Header::PcapIPv6Header(0x06, pkt_len + 20) + , tcp{ 0x1027, uint16_t(((port & 0xff) << 8) | (port >> 8)), 0x000000000, 0x00000000, 0x4, 0x0, 0x0, 0x00, 0x0000, 0x0000, 0x0000 } +{} + +class PcapSource::Private { + boost::iostreams::mapped_file_source _fpcap; + size_t _fpos; +public: + Private() : _fpos(0) {} + ~Private() { close(); } + void open(std::string pcap_path) { + close(); + _fpcap.open(pcap_path.data()); + }; + bool is_open() const { return _fpcap.is_open(); } + void close() { + if (is_open()) { + _fpcap.close(); + } + _fpos = 0; + } + inline bool eof() const { return _fpos == _fpcap.size(); } + inline size_t fpos() const { return _fpos; } + inline void fpos(size_t new_fpos) { _fpos = new_fpos; } + template + bool read(T& value) { + if (_fpos + sizeof(T) > _fpcap.size()) return false; + value = *(T*)(_fpcap.data() + _fpos); + _fpos += sizeof(T); + return true; + } + bool read(void* dst, size_t length) { + if (_fpos + length > _fpcap.size()) return false; + std::memcpy(dst, _fpcap.data() + _fpos, length); + _fpos += length; + return true; + } + bool move(size_t length) { + if (_fpos + length > _fpcap.size()) return false; + _fpos += length; + return true; + } +}; + +PcapSource::PcapSource(std::string path, int packet_interval) + : _p(new Private) + , pcap_udp_header_{0, 0, 0, 0} +{ + pcap_path_ = path; + packet_interval_ = packet_interval; +} + +PcapSource::~PcapSource() { + PcapSource::Close(); + delete _p; +} + +PcapSource::Callback PcapSource::callback() const { + return udp_callback_; +} + +void PcapSource::callback(Callback callback) { + udp_callback_ = callback; +} +PcapSource::Callback PcapSource::tcp_callback() const { + return tcp_callback_; +} + +void PcapSource::tcp_callback(Callback callback) { + tcp_callback_ = callback; +} + +size_t PcapSource::fpos() const +{ + return _p->fpos(); +} + +void PcapSource::fpos(size_t new_fpos) +{ + _p->fpos(new_fpos); +} + +bool PcapSource::Open() { + + try + { + PcapSource::Close(); + _p->open(pcap_path_); + if (!_p->is_open()) { + throw std::runtime_error(std::string("Fail to open .pcap file: \"") + (pcap_path_)+"\""); + } + _p->read(pcap_header_); + if (pcap_header_.magic_number != pcap_magic_number) { + throw std::runtime_error(std::string("Not a valid .pcap file: \"") + (pcap_path_)+"\""); + } + return true; + } + catch (const std::exception& e) + { + std::cerr << e.what() << std::endl; + return false; + } + +} + +std::string PcapSource::pcap_path() const { + return pcap_path_; +} + +int PcapSource::next(UdpPacket& udpPacket, uint16_t u16Len, int flags, + int timeout) { + //std::this_thread::sleep_for(std::chrono::microseconds(packet_interval_)); + if(_p->eof()) { + return -1; + } + bool ret = _p->read(pcap_record_); + if (!ret) return -1; + if (pcap_record_.incl_len <= 1500) { + ret = _p->read(payload_.data(), pcap_record_.incl_len); + if (!ret) return -1; + switch (((Ethernet*)payload_.data())->ether_type) + { + case 0x0008: // ipv4 + switch (((PcapIPHeader*)payload_.data())->ip.protocol) + { + case 17: + pcap_udp_header_ = *(const UDP*)(payload_.data() + sizeof(PcapIPHeader)); + u16Len = pcap_record_.incl_len - sizeof(PcapUDPHeader); + memcpy(udpPacket.buffer, payload_.data() + sizeof(PcapUDPHeader), pcap_record_.incl_len - sizeof(PcapUDPHeader)); + return pcap_record_.incl_len - sizeof(PcapUDPHeader); + break; + case 6: + pcap_tcp_header_ = *(const TCP*)(payload_.data() + sizeof(PcapIPHeader)); + if (tcp_callback_) + return pcap_record_.incl_len - sizeof(PcapTCPHeader); + else + return 2; + break; + default: + return 2; + break; + } + break; + case 0xdd86: // ipv6 + switch (((PcapIPv6Header*)payload_.data())->ipv6.next_header) + { + case 17: + pcap_udp_header_ = *(const UDP*)(payload_.data() + sizeof(PcapIPv6Header)); + memcpy(udpPacket.buffer, payload_.data() + sizeof(PcapUDPv6Header), pcap_record_.incl_len - sizeof(PcapUDPv6Header)); + return pcap_record_.incl_len - sizeof(PcapUDPv6Header); + break; + case 6: + pcap_tcp_header_ = *(const TCP*)(payload_.data() + sizeof(PcapIPv6Header)); + if (tcp_callback_) + return pcap_record_.incl_len - sizeof(PcapTCPv6Header); + else + return 2; + break; + default: + return 2; + break; + } + break; + case 0x0081: // vlan tag + switch (*((uint16_t*)(payload_.data() + sizeof(Ethernet) + 2))) + { + case 0x0008: + switch (*((uint8_t*)(payload_.data() + sizeof(Ethernet) + 13))) + { + case 17: + pcap_udp_header_ = *(const UDP*)(payload_.data() + sizeof(PcapIPHeader)); + memcpy(udpPacket.buffer, payload_.data() + sizeof(PcapUDPHeader) + 4, pcap_record_.incl_len - sizeof(PcapUDPHeader) - 4); + return pcap_record_.incl_len - sizeof(PcapUDPHeader) - 4; + break; + case 6: + pcap_tcp_header_ = *(const TCP*)(payload_.data() + sizeof(PcapIPHeader)); + if (tcp_callback_) + return pcap_record_.incl_len - sizeof(PcapTCPHeader) - 4; + else + return 2; + break; + default: + return 2; + break; + } + break; + case 0xdd86: + switch (*((uint8_t*)(payload_.data() + sizeof(Ethernet) + 13))) + { + case 17: + pcap_udp_header_ = *(const UDP*)(payload_.data() + sizeof(PcapIPv6Header)); + return pcap_record_.incl_len - sizeof(PcapUDPv6Header) - 4; + break; + case 6: + pcap_tcp_header_ = *(const TCP*)(payload_.data() + sizeof(PcapIPv6Header)); + if (tcp_callback_) + return pcap_record_.incl_len - sizeof(PcapTCPv6Header) - 4; + else + return 2; + break; + default: + return 2; + break; + } + default: + return 2; + break; + } + break; + default: + printf("can not parser Ethernet data, type = %x \n", ((Ethernet*)payload_.data())->ether_type); + break; + } + + } + else { + ret = _p->move(pcap_record_.incl_len); + if (!ret) return -1; + return 1; + } + return 1; +} +int PcapSource::distinationPort() { + uint16_t port = pcap_udp_header_.distination_port; + return ((port & 0xff) << 8) | ((port >> 8) & 0xff); +} + +void PcapSource::Close() { + _p->close(); +} + + +bool PcapSource::IsOpened() { + return _p->is_open(); +} + +int PcapSource::Receive(UdpPacket& udpPacket, uint16_t u16Len, int flags, + int iTimeout) { + return next(udpPacket, u16Len, flags, iTimeout); +} + +int PcapSource::Send(uint8_t* u8Buf, uint16_t u16Len, int flags) { + return u16Len; +} + +void PcapSource::setPcapPath(std::string path) { + pcap_path_ = path; +} + +void PcapSource::setPacketInterval(int microsecond) { + packet_interval_ = microsecond; +} \ No newline at end of file diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Source/src/socket_source.cc b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Source/src/socket_source.cc new file mode 100644 index 0000000..77d5570 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Source/src/socket_source.cc @@ -0,0 +1,244 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ + +/* + * File: socket_source.cc + * Author: Felix Zou + */ + +#include "socket_source.h" +#include +#include +using namespace hesai::lidar; +SocketSource::SocketSource(uint16_t port, std::string multicastIp) { + client_ip_.clear(); + udp_port_ = port; + udp_sock_ = -1; + multicast_ip_ = multicastIp; + is_select_ = false; +} + +SocketSource::~SocketSource() { Close(); } + +void SocketSource::Close() { + printf("SocketSource::Close()\n"); + + client_ip_.clear(); + udp_port_ = 0; + + if (udp_sock_ > 0) { +#ifdef _MSC_VER + closesocket(udp_sock_); + WSACleanup(); +#else + close(udp_sock_); +#endif + udp_sock_ = -1; + } +} + + +bool SocketSource::Open() { +#ifdef _MSC_VER + WSADATA wsaData; + WORD version = MAKEWORD(2, 2); + int res = WSAStartup(version, &wsaData); // win sock start up + if (res) { + std::cerr << "Initilize winsock error !" << std::endl; + return false; + } +#endif + int retVal = 0; + struct sockaddr_in serverAddr; + + udp_sock_ = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); + + if (udp_sock_ == -1) return false; + + memset(&serverAddr, 0, sizeof(serverAddr)); + + serverAddr.sin_family = AF_INET; + serverAddr.sin_addr.s_addr = htonl(INADDR_ANY); + serverAddr.sin_port = htons(udp_port_); + + int reuseaddr = 1; + retVal = setsockopt(udp_sock_, SOL_SOCKET, SO_REUSEADDR, (char*)&reuseaddr, + sizeof(reuseaddr)); + int nRecvBuf = 400000000; + setsockopt(udp_sock_, SOL_SOCKET, SO_RCVBUF, (const char *)&nRecvBuf, + sizeof(int)); + int curRcvBufSize = -1; + socklen_t optlen = sizeof(curRcvBufSize); + if (getsockopt(udp_sock_, SOL_SOCKET, SO_RCVBUF, (char*)&curRcvBufSize, &optlen) < + 0) { + printf("getsockopt error=%d(%s)!!!\n", errno, strerror(errno)); + } + printf("OS current udp socket recv buff size is: %d\n", curRcvBufSize); + + if (retVal == 0) { +#ifdef _MSC_VER + int timeout_ms = 20; + retVal = setsockopt(udp_sock_, SOL_SOCKET, SO_RCVTIMEO, (char*)&timeout_ms, + sizeof(timeout_ms)); + // SetThreadPriority(GetCurrentThread(), THREAD_PRIORITY_TIME_CRITICAL); +#else + struct timeval timeout; + timeout.tv_sec = 0; + timeout.tv_usec = 20000; + + retVal = setsockopt(udp_sock_, SOL_SOCKET, SO_RCVTIMEO, (char*)&timeout, + sizeof(struct timeval)); +#endif + if (retVal == 0) { + if (bind(udp_sock_, (sockaddr*)&serverAddr, sizeof(sockaddr)) == -1) { + if (EINPROGRESS != errno && EWOULDBLOCK != errno) { +#ifdef _MSC_VER + closesocket(udp_sock_); +#else + close(udp_sock_); +#endif + udp_sock_ = -1; + printf("SocketSource::Open(), bind failed, errno: %d\n", errno); + return false; + } + } else { + printf("SocketSource::Open succeed, sock:%d\n", udp_sock_); + } + } else { + printf("setsockopt SO_RCVTIMEO failed, errno:%d\n", errno); + } + } else { + printf("setsockopt SO_REUSEADDR failed, errno:%d\n", errno); + } +#ifdef _MSC_VER + unsigned long nonBlockingMode = 1; + if (ioctlsocket(udp_sock_, FIONBIO, &nonBlockingMode) != 0) { + perror("non-block"); + } +#else + if (fcntl(udp_sock_, F_SETFL, O_NONBLOCK | FASYNC) < 0) { + perror("non-block"); + } +#endif + + int32_t rcvBufSize = kUDPBufferSize; + setsockopt(udp_sock_, SOL_SOCKET, SO_RCVBUF, (char*)&rcvBufSize, + sizeof(rcvBufSize)); + + if(multicast_ip_ != ""){ + struct ip_mreq mreq; + mreq.imr_multiaddr.s_addr=inet_addr(multicast_ip_.c_str()); + mreq.imr_interface.s_addr = htonl(INADDR_ANY); + int ret = setsockopt(udp_sock_, IPPROTO_IP, IP_ADD_MEMBERSHIP, (const char *)&mreq, sizeof(mreq)); + if (ret < 0) { + perror("Multicast IP error,set correct multicast ip address or keep it empty\n"); + } + else { + printf("Recive data from multicast ip address %s\n", multicast_ip_.c_str()); + } + } + return retVal == 0; +} + +bool SocketSource::IsOpened() { + bool ret = true; + + if (udp_port_ == 0 || udp_sock_ < 0) { + ret = false; + printf("SocketSource::IsOpened(), port %d, sock %d\n", udp_port_, + udp_sock_); + } + + return ret; +} + +int SocketSource::Send(uint8_t* u8Buf, uint16_t u16Len, int flags) { + ssize_t len = -1; + bool ret = true; + if (!IsOpened()) ret = false; + + if (ret) { + sockaddr addr; + int val = inet_pton(AF_INET, client_ip_.c_str(), &addr); + if (val == 1) { + len = sendto(udp_sock_, (char*)u8Buf, u16Len, flags, &addr, sizeof(addr)); + if (len == -1) printf("SocketSource::Send, errno:%d\n", errno); + } else { + printf("SocketSource::Send(), invalid IP %s\n", client_ip_.c_str()); + } + } + return len; +} + +bool isTimeout = false; + +int SocketSource::Receive(UdpPacket& udpPacket, uint16_t u16Len, int flags, + int iTimeout) { + ssize_t len = -1; + bool ret = true; + if (!IsOpened()) ret = Open(); + + if (ret) { + fd_set rfd; + timeval timeout; + + timeout.tv_sec = 0; + timeout.tv_usec = iTimeout; + FD_ZERO(&rfd); + + FD_SET(udp_sock_, &rfd); + if (!is_select_) { + sockaddr_in clientAddr; + socklen_t addrLen = sizeof(sockaddr); + len = recvfrom(udp_sock_, (char*)udpPacket.buffer, u16Len, flags, + (sockaddr*)&clientAddr, &addrLen); + if(len == -1) {is_select_ = true;} + } else { + int cnt = select(udp_sock_ + 1, &rfd, NULL, NULL, &timeout); + if (cnt > 0) { + is_select_ = false; + sockaddr_in clientAddr; + socklen_t addrLen = sizeof(sockaddr); + len = recvfrom(udp_sock_, (char*)udpPacket.buffer, u16Len, flags, + (sockaddr*)&clientAddr, &addrLen); + } else if (cnt == 0) { + len = 0; + udpPacket.is_timeout = true; + } else { + std::cout << "Select timeout error" << std::endl; + } + } + } + + return len; +} + +void SocketSource::SetSocketBufferSize(uint32_t u32BufSize) { + setsockopt(udp_sock_, SOL_SOCKET, SO_RCVBUF, (char*)&u32BufSize, + sizeof(u32BufSize)); +} diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Source/src/source.cc b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Source/src/source.cc new file mode 100644 index 0000000..5cea366 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Source/src/source.cc @@ -0,0 +1,45 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ + +/* + * File: source.cc + * Author: Felix Zou + */ + +#include "source.h" +#include +using namespace hesai::lidar; +Source::Source() {} + +Source::~Source() { Close(); } + +void Source::Close() { + printf("Source::Close()\n"); +} + + diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/general_parser.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/general_parser.h new file mode 100644 index 0000000..57ea2e4 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/general_parser.h @@ -0,0 +1,254 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ + +/* + * File: general_parser.h + * Author: Zhang Yu + * Description: Declare GeneralParser class +*/ + +#ifndef GENERAL_PARSER_H_ +#define GENERAL_PARSER_H_ +#define CIRCLE (36000) +#define MAX_LASER_NUM (512) +#ifndef M_PI +#define M_PI (3.14159265358979323846) +#endif + +// #include "udp_parser.h" +#ifndef _MSC_VER +#include +#endif +#include +#include +#include +#include +#include +#include +#include "lidar_types.h" +namespace hesai +{ +namespace lidar +{ + +#define DEFINE_MEMBER_CHECKER(member) \ + template \ + struct has_##member : std::false_type \ + { \ + }; \ + template \ + struct has_##member< \ + T, typename std::enable_if().member), void>::value, bool>::type> \ + : std::true_type \ + { \ + }; +#define PANDAR_HAS_MEMBER(C, member) has_##member::value +DEFINE_MEMBER_CHECKER(x) +DEFINE_MEMBER_CHECKER(y) +DEFINE_MEMBER_CHECKER(z) +DEFINE_MEMBER_CHECKER(intensity) +DEFINE_MEMBER_CHECKER(ring) +DEFINE_MEMBER_CHECKER(timestamp) + +template +inline typename std::enable_if::type setX(T_Point& point, const float& value) +{ +} + +template +inline typename std::enable_if::type setX(T_Point& point, const float& value) +{ + point.x = value; +} + +template +inline typename std::enable_if::type setY(T_Point& point, const float& value) +{ +} + +template +inline typename std::enable_if::type setY(T_Point& point, const float& value) +{ + point.y = value; +} + +template +inline typename std::enable_if::type setZ(T_Point& point, const float& value) +{ +} + +template +inline typename std::enable_if::type setZ(T_Point& point, const float& value) +{ + point.z = value; +} + +template +inline typename std::enable_if::type setIntensity(T_Point& point, + const uint8_t& value) +{ +} + +template +inline typename std::enable_if::type setIntensity(T_Point& point, + const uint8_t& value) +{ + point.intensity = value; +} + +template +inline typename std::enable_if::type setRing(T_Point& point, const uint16_t& value) +{ +} + +template +inline typename std::enable_if::type setRing(T_Point& point, const uint16_t& value) +{ + point.ring = value; +} + +template +inline typename std::enable_if::type setTimestamp(T_Point& point, + const double& value) +{ +} + +template +inline typename std::enable_if::type setTimestamp(T_Point& point, + const double& value) +{ + point.timestamp = value; +} + +inline float deg2Rad(float deg) +{ + return deg * 0.01745329251994329575; +} + +inline float rad2Deg(float rad) +{ + return rad * 57.29577951308232087721; +} + +struct Transform { + float x; + float y; + float z; + float roll; + float pitch; + float yaw; +}; + +// class GeneralParser +// the GenneralParser class is a base class for parsering packets and computing points +// you can parser the upd or pcap packets using the DocodePacket fuction +// you can compute xyzi of points using the ComputeXYZI fuction, which uses cpu to compute +template +class GeneralParser { + public: + GeneralParser(); + virtual ~GeneralParser(); + + // get lidar correction file from local file,and pass to udp parser + virtual void LoadCorrectionFile(std::string correction_path); + virtual int LoadCorrectionString(char *correction_string); + + // get lidar firetime correction file from local file,and pass to udp parser + virtual void LoadFiretimesFile(std::string firetimes_path); + virtual int LoadFiretimesString(char *firetimes_string); + + // compute lidar firetime correciton + virtual double GetFiretimesCorrection(int laserId, double speed); + + // compute lidar distance correction + virtual void GetDistanceCorrection(double &azimuth, double &elevation, double &distance); + void SetEnableFireTimeCorrection(bool enable); + void SetEnableDistanceCorrection(bool enable); + // covert a origin udp packet to decoded packet, the decode function is in UdpParser module + // udp_packet is the origin udp packet, output is the decoded packet + virtual int DecodePacket(LidarDecodedPacket &output, const UdpPacket& udpPacket); + + // covert a origin udp packet to decoded data, and pass the decoded data to a frame struct to reduce memory copy + virtual int DecodePacket(LidarDecodedFrame &frame, const UdpPacket& udpPacket); + + // compute xyzi of points from decoded packet + // param packet is the decoded packet; xyzi of points after computed is puted in frame + virtual int ComputeXYZI(LidarDecodedFrame &frame, LidarDecodedPacket &packet); + + //set frame azimuth + virtual void SetFrameAzimuth(float frame_start_azimuth); + + //set enable_packet_loss_tool_ + virtual void EnablePacketLossTool(bool enable); + + void TransformPoint(float& x, float& y, float& z); + void SetTransformPara(float x, float y, float z, float roll, float pitch, float yaw); + void EnableUpdateMonitorInfo(); + void DisableUpdateMonitorInfo(); + uint16_t *GetMonitorInfo1(); + uint16_t *GetMonitorInfo2(); + uint16_t *GetMonitorInfo3(); + std::vector elevation_correction_; + std::vector azimuth_collection_; + uint32_t total_start_seqnum_; + uint32_t total_loss_count_; + uint32_t current_seqnum_; + uint32_t total_packet_count_; + + protected: + uint16_t monitor_info1_[256]; + uint16_t monitor_info2_[256]; + uint16_t monitor_info3_[256]; + float cos_all_angle_[CIRCLE]; + float sin_all_angle_[CIRCLE]; + bool enable_update_monitor_info_; + int return_mode_; + int motor_speed_; + bool get_correction_file_; + bool get_firetime_file_; + bool is_dual_return_; + uint16_t spin_speed_; + static const uint16_t kAzimuthTolerance = 1000; + bool use_angle_ = true; + uint16_t last_azimuth_; + uint32_t start_seqnum_; + uint32_t last_seqnum_; + uint32_t loss_count_; + uint32_t start_time_; + double firetime_correction_[512]; + bool enable_firetime_correction_; + bool enable_distance_correction_; + bool enable_packet_loss_tool_; + Transform transform_; + float frame_start_azimuth_; +}; +} +} +#include "general_parser.cc" + +#endif // GENERAL_PARSER_H_ diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/udp1_4_parser.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/udp1_4_parser.h new file mode 100644 index 0000000..b71bd8b --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/udp1_4_parser.h @@ -0,0 +1,97 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ + +/* + * File: udp1_4_parser.h + * Author: Zhang Yu + * Description: Declare Udp1_4Parser class +*/ + +#ifndef UDP1_4_PARSER_H_ +#define UDP1_4_PARSER_H_ + +#include "general_parser.h" +namespace hesai +{ +namespace lidar +{ + +struct FiretimeSectionValues { + struct SectionValue { + std::array firetime; + }; + std::array section_values; +}; +// class Udp1_4Parser +// parsers packets and computes points for Pandar128 +// you can parser the upd or pcap packets using the DocodePacket fuction +// you can compute xyzi of points using the ComputeXYZI fuction, which uses cpu to compute +template +class Udp1_4Parser : public GeneralParser { + public: + Udp1_4Parser(); + virtual ~Udp1_4Parser(); + + // covert a origin udp packet to decoded packet, the decode function is in UdpParser module + // udp_packet is the origin udp packet, output is the decoded packet + virtual int DecodePacket(LidarDecodedPacket &output, const UdpPacket& udpPacket); + + // compute xyzi of points from decoded packet + // param packet is the decoded packet; xyzi of points after computed is puted in frame + virtual int ComputeXYZI(LidarDecodedFrame &frame, LidarDecodedPacket &packet); + + // get lidar firetime correction file from local file,and pass to udp parser + virtual void LoadFiretimesFile(std::string firetimes_path); + + // compute lidar firetime correciton + double GetFiretimesCorrection(int laserId, double speed, uint8_t optMode, uint8_t angleState,uint16_t dist); + + // determine whether frame splitting is needed + bool IsNeedFrameSplit(uint16_t azimuth); + + // compute lidar distance correction + void GetDistanceCorrection(int laser_id, float distance, int& azimuth, int& elevation); +// virtual int ComputeXYZI(LidarDecodedFrame &frame, LidarDecodedPacket &packet); + + private: + static const int kLaserNum = 128; + double section_distance; + std::array firetime_section_values; + float distance_correction_para_a_; + float distance_correction_para_b_; + float distance_correction_para_h_; + float distance_correction_para_c_; + float distance_correction_para_d_; + bool use_frame_start_azimuth_ = true; +}; +} // namespace lidar +} // namespace hesai + +#include "udp1_4_parser.cc" + +#endif // UDP1_4_PARSER_H_ diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/udp2_4_parser.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/udp2_4_parser.h new file mode 100644 index 0000000..6c0e043 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/udp2_4_parser.h @@ -0,0 +1,82 @@ +/* + This file encapsulates the data types for ET radar correction files and defines a class for a parser(UDP2.4 protocol). +*/ +#ifndef UDP2_4_PARSER_H +#define UDP2_4_PARSER_H + +#include +#include +#include "general_parser.h" +#include "lidar_types.h" +#include +#include +#include +#include +namespace hesai +{ + namespace lidar + { + static constexpr int ET_MAX_CHANNEL_NUM_24 = 512; + struct ETCorrections_V4 { + uint8_t delimiter[2]; + uint8_t major_version; + uint8_t min_version; + uint8_t reserved1; + uint8_t reserved2; + uint8_t channel_number; + uint8_t mirror_nummber_reserved3; + uint16_t angle_division; + int16_t apha; + int16_t beta; + int16_t gamma; + std::array azimuths, elevations; + int16_t raw_azimuths[ET_MAX_CHANNEL_NUM_24]; + int16_t raw_elevations[ET_MAX_CHANNEL_NUM_24]; + uint8_t SHA_value[32]; + }; + + struct ETCorrections_v4_Header { + uint8_t delimiter[2]; + uint8_t major_version; + uint8_t min_version; + uint8_t reserved1; + uint8_t reserved2; + uint8_t channel_number; + uint8_t mirror_nummber_reserved3; + uint16_t angle_division; + int16_t apha; + int16_t beta; + int16_t gamma; + }; + + // class Udp2_4Parser + // parsers packets and computes points for ET25 + // you can parser the upd or pcap packets using the DocodePacket fuction + // you can compute xyzi of points using the ComputeXYZI fuction, which uses cpu to compute + template + class Udp2_4Parser : public GeneralParser { + public: + Udp2_4Parser(); + virtual ~Udp2_4Parser(); + int16_t GetVecticalAngle(int channel); + // determine whether frame splitting is needed + bool IsNeedFrameSplit(uint16_t nowid); + // get lidar correction file from local file,and pass to udp parser + virtual void LoadCorrectionFile(std::string correction_path); + virtual int LoadCorrectionString(char *correction_string); + int LoadCorrectionString_csv(std::string lidar_correction_file); + // covert a origin udp packet to decoded packet, the decode function is in UdpParser module + // udp_packet is the origin udp packet, output is the decoded packet + virtual int DecodePacket(LidarDecodedPacket &output, const UdpPacket& udpPacket); + // compute xyzi of points from decoded packet + // param packet is the decoded packet; xyzi of points after computed is puted in frame + virtual int ComputeXYZI(LidarDecodedFrame &frame, LidarDecodedPacket &packet); + ETCorrections_V4 m_ET_corrections; + protected: + bool get_correction_file_; + int last_frameid_ = 0; + }; + } // namespace lidar +} // namespace hesai +#include "udp2_4_parser.cc" +#endif // UDP2_4_PARSER_H \ No newline at end of file diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/udp2_5_parser.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/udp2_5_parser.h new file mode 100644 index 0000000..c0a1725 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/udp2_5_parser.h @@ -0,0 +1,109 @@ +/* + This file encapsulates the data types for ET radar correction files and defines a class for an ET parser. +*/ + +#ifndef UDP2_5_PARSER_H +#define UDP2_5_PARSER_H + +#include +#include +#include "general_parser.h" +#include "lidar_types.h" +#include +#include +#include +#include +namespace hesai +{ +namespace lidar +{ + + static constexpr int ET_MAX_CHANNEL_NUM = 512; + + struct ETCorrections { + uint8_t delimiter[2]; + uint8_t major_version; + uint8_t min_version; + uint8_t reserved1; + uint8_t reserved2; + uint8_t channel_number; + uint8_t mirror_nummber_reserved3; + uint16_t angle_division; + int16_t apha; + int16_t beta; + int16_t gamma; + float azimuths[ET_MAX_CHANNEL_NUM]; + float elevations[ET_MAX_CHANNEL_NUM]; + int16_t raw_azimuths[ET_MAX_CHANNEL_NUM]; + int16_t raw_elevations[ET_MAX_CHANNEL_NUM]; + // SHA-256_value + uint8_t SHA_value[32]; + + }; + + struct ETCorrectionsHeader { + uint8_t delimiter[2]; + uint8_t major_version; + uint8_t min_version; + uint8_t reserved1; + uint8_t reserved2; + uint8_t channel_number; + uint8_t mirror_nummber_reserved3; + uint16_t angle_division; + int16_t apha; + int16_t beta; + int16_t gamma; + }; + + + // class Udp2_5Parser + // parsers packets and computes points for ET25 + // you can parser the upd or pcap packets using the DocodePacket fuction + // you can compute xyzi of points using the ComputeXYZI fuction, which uses cpu to compute + template + class Udp2_5Parser : public GeneralParser { + public: + Udp2_5Parser(); + virtual ~Udp2_5Parser(); + + // + int16_t GetVecticalAngle(int channel); + + // determine whether frame splitting is needed + bool IsNeedFrameSplit(uint16_t frame_id); + + + // get lidar correction file from local file,and pass to udp parser + virtual void LoadCorrectionFile(std::string correction_path); + virtual int LoadCorrectionString(char *correction_string); + int LoadCorrectionDatData(char *correction_string); + int LoadCorrectionCsvData(char *correction_string); + + // covert a origin udp packet to decoded packet, the decode function is in UdpParser module + // udp_packet is the origin udp packet, output is the decoded packet + virtual int DecodePacket(LidarDecodedPacket &output, const UdpPacket& udpPacket); + + // compute xyzi of points from decoded packet + // param packet is the decoded packet; xyzi of points after computed is puted in frame + virtual int ComputeXYZI(LidarDecodedFrame &frame, LidarDecodedPacket &packet); + ETCorrections corrections_; + + + // 类维护的成员变量 + protected: + // int view_mode_; + // std::string pcap_file_; + // std::string lidar_correction_file_; + bool get_correction_file_; + int last_frameid_ = 0; + }; + + + +} // namespace lidar +} // namespace hesai + +#include "udp2_5_parser.cc" + + +#endif // UDP2_5_PARSER_H \ No newline at end of file diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/udp3_1_parser.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/udp3_1_parser.h new file mode 100644 index 0000000..697f1c9 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/udp3_1_parser.h @@ -0,0 +1,74 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ + +/* + * File: udp3_1_parser.h + * Author: Zhang Yu + * Description: Declare Udp3_1Parser class +*/ + +#ifndef UDP3_1_PARSER_H_ +#define UDP3_1_PARSER_H_ + +#include "general_parser.h" +#define HS_LIDAR_QT_COORDINATE_CORRECTION_ODOG (0.0298) +#define HS_LIDAR_QT_COORDINATE_CORRECTION_OGOT (0.0072) +namespace hesai +{ +namespace lidar +{ +// class Udp3_1Parser +// parsers packets and computes points for PandarQT64 +// you can parser the upd or pcap packets using the DocodePacket fuction +// you can compute xyzi of points using the ComputeXYZI fuction, which uses cpu to compute +template +class Udp3_1Parser : public GeneralParser { + public: + Udp3_1Parser(); + virtual ~Udp3_1Parser(); + + // covert a origin udp packet to decoded packet, the decode function is in UdpParser module + // udp_packet is the origin udp packet, output is the decoded packet + virtual int DecodePacket(LidarDecodedPacket &output, const UdpPacket& udpPacket); + + // compute xyzi of points from decoded packet + // param packet is the decoded packet; xyzi of points after computed is puted in frame + virtual int ComputeXYZI(LidarDecodedFrame &frame, LidarDecodedPacket &packet); + + // determine whether frame splitting is needed + bool IsNeedFrameSplit(uint16_t azimuth); + + // compute lidar distance correction + void GetDistanceCorrection(double &azimuth, double &elevation, double &distance); + private: +}; +} // namespace lidar +} // namespace hesai +#include "udp3_1_parser.cc" + +#endif // UDP3_1_PARSER_H_ diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/udp3_2_parser.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/udp3_2_parser.h new file mode 100644 index 0000000..918deb4 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/udp3_2_parser.h @@ -0,0 +1,110 @@ + +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ + +/* + * File: udp3_2_parser.h + * Author: Zhang Yu + * Description: Declare Udp3_2Parser class +*/ + +#ifndef UDP3_2_PARSER_H_ +#define UDP3_2_PARSER_H_ +#define HS_LIDAR_QT128_LASER_NUM (128) +#define HS_LIDAR_QT128_LOOP_NUM (4) +#define HS_LIDAR_QT128_COORDINATE_CORRECTION_ODOG (0.0354) +#define HS_LIDAR_QT128_COORDINATE_CORRECTION_OGOT (-0.0072) + +#include +#include +#include "general_parser.h" +#include "udp_protocol_v3_2.h" +namespace hesai +{ +namespace lidar +{ + +struct PandarQTChannelConfig { + public: + uint16_t sob; + uint8_t major_version; + uint8_t min_version; + uint8_t laser_num; + uint8_t m_u8BlockNum; + std::vector> m_vChannelConfigTable; + std::string m_sHashValue; + bool m_bIsChannelConfigObtained; +}; +// class Udp3_2Parser +// parsers packets and computes points for PandarQT128 +// you can parser the upd or pcap packets using the DocodePacket fuction +// you can compute xyzi of points using the ComputeXYZI fuction, which uses cpu to compute +template +class Udp3_2Parser : public GeneralParser { + public: + Udp3_2Parser(); + virtual ~Udp3_2Parser(); + + // get lidar firetime correction file from local file,and pass to udp parser + virtual int LoadFiretimesString(char *firetimes_string); + virtual void LoadFiretimesFile(std::string firetimes_path); + + // compute lidar distance correction + virtual double GetFiretimesCorrection(int laserId, double speed, int loopIndex); + + // compute lidar distance correction + void GetDistanceCorrection(double &azimuth, double &elevation, double &distance); + + // get lidar correction file from local file,and pass to udp parser + int LoadChannelConfigString(char *channel_config_content); + void LoadChannelConfigFile(std::string channel_config_path); + // virtual int ComputeXYZI(LidarDecodedFrame &frame, LidarDecodedPacket &packet); + + // covert a origin udp packet to decoded packet, the decode function is in UdpParser module + // udp_packet is the origin udp packet, output is the decoded packet + virtual int DecodePacket(LidarDecodedPacket &output, const UdpPacket& udpPacket); + + // compute xyzi of points from decoded packet + // param packet is the decoded packet; xyzi of points after computed is puted in frame + virtual int ComputeXYZI(LidarDecodedFrame &frame, LidarDecodedPacket &packet); + + // determine whether frame splitting is needed + bool IsNeedFrameSplit(uint16_t azimuth); + + private: + std::array, + HS_LIDAR_QT128_LOOP_NUM> + qt128_firetime_; + PandarQTChannelConfig pandarQT_channel_config_; +}; +} // namespace lidar +} // namespace hesai + +#include "udp3_2_parser.cc" + +#endif // UDP3_2_PARSER_H_ diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/udp4_3_parser.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/udp4_3_parser.h new file mode 100644 index 0000000..e3da69c --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/udp4_3_parser.h @@ -0,0 +1,177 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ + +/* + * File: udp4_3_parser.h + * Author: Zhang Yu + * Description: Declare Udp4_3Parser class +*/ + +#ifndef UDP4_3_PARSER_H_ +#define UDP4_3_PARSER_H_ + +#define MAX_AZI_LEN (36000 * 256) +#define CIRCLE_ANGLE (36000) +#define M_PI (3.14159265358979323846) +#define CORRECTION_AZIMUTH_STEP (200) +#define CORRECTION_AZIMUTH_NUM (180) +#define FINE_AZIMUTH_UNIT (256) +#define AZIMUTH_UNIT (25600.0f) +#define AT128_LASER_NUM (128) +#define FAULTMESSAGE_LENTH (99) +#define ANGULAR_RESOLUTION (256) +#define MARGINAL_ANGLE (7625) +#define ACCEPTANCE_ANGLE (200) +#define RESOLUTION (256 * 100) + + +#include +#include +#include "general_parser.h" +#include "lidar_types.h" +namespace hesai +{ +namespace lidar +{ + +struct PandarATCorrectionsHeader { + uint8_t delimiter[2]; + uint8_t version[2]; + uint8_t channel_number; + uint8_t mirror_number; + uint8_t frame_number; + uint8_t frame_config[8]; + uint8_t resolution; +}; +static_assert(sizeof(PandarATCorrectionsHeader) == 16, ""); + +struct PandarATFrameInfo { + uint32_t start_frame[8]; + uint32_t end_frame[8]; + int32_t azimuth[AT128_LASER_NUM]; + int32_t elevation[AT128_LASER_NUM]; + std::array sin_map; + std::array cos_map; +}; + +struct PandarATCorrections { + public: + PandarATCorrectionsHeader header; + uint16_t start_frame[8]; + uint16_t end_frame[8]; + int16_t azimuth[AT128_LASER_NUM]; + int16_t elevation[AT128_LASER_NUM]; + int8_t azimuth_offset[CIRCLE_ANGLE]; + int8_t elevation_offset[CIRCLE_ANGLE]; + uint8_t SHA256[32]; + PandarATFrameInfo l; // V1.5 + std::array sin_map; + std::array cos_map; + PandarATCorrections() { + for (int i = 0; i < MAX_AZI_LEN; ++i) { + sin_map[i] = std::sin(2 * i * M_PI / MAX_AZI_LEN); + cos_map[i] = std::cos(2 * i * M_PI / MAX_AZI_LEN); + } + } + static const int STEP = CORRECTION_AZIMUTH_STEP; + int8_t GetAzimuthAdjust(uint8_t ch, uint16_t azi) const { + unsigned int i = std::floor(1.f * azi / STEP); + unsigned int l = azi - i * STEP; + float k = 1.f * l / STEP; + return round((1 - k) * azimuth_offset[ch * CORRECTION_AZIMUTH_NUM + i] + + k * azimuth_offset[ch * CORRECTION_AZIMUTH_NUM + i + 1]); + } + int8_t GetElevationAdjust(uint8_t ch, uint16_t azi) const { + unsigned int i = std::floor(1.f * azi / STEP); + unsigned int l = azi - i * STEP; + float k = 1.f * l / STEP; + return round((1 - k) * elevation_offset[ch * CORRECTION_AZIMUTH_NUM + i] + + k * elevation_offset[ch * CORRECTION_AZIMUTH_NUM + i + 1]); + } + static const int STEP3 = CORRECTION_AZIMUTH_STEP * FINE_AZIMUTH_UNIT; + int8_t GetAzimuthAdjustV3(uint8_t ch, uint32_t azi) const { + unsigned int i = std::floor(1.f * azi / STEP3); + unsigned int l = azi - i * STEP3; + float k = 1.f * l / STEP3; + return round((1 - k) * azimuth_offset[ch * CORRECTION_AZIMUTH_NUM + i] + + k * azimuth_offset[ch * CORRECTION_AZIMUTH_NUM + i + 1]); + } + int8_t GetElevationAdjustV3(uint8_t ch, uint32_t azi) const { + unsigned int i = std::floor(1.f * azi / STEP3); + unsigned int l = azi - i * STEP3; + float k = 1.f * l / STEP3; + return round((1 - k) * elevation_offset[ch * CORRECTION_AZIMUTH_NUM + i] + + k * elevation_offset[ch * CORRECTION_AZIMUTH_NUM + i + 1]); + } +}; +// class Udp4_3Parser +// parsers packets and computes points for PandarAT128 +// you can parser the upd or pcap packets using the DocodePacket fuction +// you can compute xyzi of points using the ComputeXYZI fuction, which uses cpu to compute +template +class Udp4_3Parser : public GeneralParser { + public: + Udp4_3Parser(); + virtual ~Udp4_3Parser(); + // 从PandarATCorrections中获取 + int16_t GetVecticalAngle(int channel); + + // determine whether frame splitting is needed + bool IsNeedFrameSplit(uint16_t azimuth, int field); + + // get lidar correction file from local file,and pass to udp parser + virtual void LoadCorrectionFile(std::string correction_path); + virtual int LoadCorrectionString(char *correction_string); + + virtual void HandlePacketData(uint8_t *pu8Buf, uint16_t u16Len); + + // covert a origin udp packet to decoded packet, the decode function is in UdpParser module + // udp_packet is the origin udp packet, output is the decoded packet + virtual int DecodePacket(LidarDecodedPacket &output, const UdpPacket& udpPacket); + + // covert a origin udp packet to decoded data, and pass the decoded data to a frame struct to reduce memory copy + virtual int DecodePacket(LidarDecodedFrame &frame, const UdpPacket& udpPacket); + + // compute xyzi of points from decoded packet + // param packet is the decoded packet; xyzi of points after computed is puted in frame + virtual int ComputeXYZI(LidarDecodedFrame &frame, LidarDecodedPacket &packet); + PandarATCorrections m_PandarAT_corrections; + + protected: + int view_mode_; + std::string pcap_file_; + std::string lidar_correction_file_; + bool get_correction_file_; + // void *m_pTcpCommandClient; +}; +} // namespace lidar +} // namespace hesai + +#include "udp4_3_parser.cc" + +#endif // UDP4_3_PARSER_H_ diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/udp6_1_parser.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/udp6_1_parser.h new file mode 100644 index 0000000..71eff2b --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/udp6_1_parser.h @@ -0,0 +1,78 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ + +/* + * File: udp6_1_parser.h + * Author: Zhang Yu + * Description: Declare Udp6_1Parser class +*/ + +#ifndef UDP6_1_PARSER_H_ +#define UDP6_1_PARSER_H_ + +#include "general_parser.h" +#include "lidar_types.h" +namespace hesai +{ +namespace lidar +{ +// class Udp6_1Parser +// parsers packets and computes points for PandarXT PandarXT6 PandarXT32 PandarXTM +// you can parser the upd or pcap packets using the DocodePacket fuction +// you can compute xyzi of points using the ComputeXYZI fuction, which uses cpu to compute +template +class Udp6_1Parser : public GeneralParser { + public: + Udp6_1Parser(); + virtual ~Udp6_1Parser(); + + // covert a origin udp packet to decoded packet, the decode function is in UdpParser module + // udp_packet is the origin udp packet, output is the decoded packet + virtual int DecodePacket(LidarDecodedPacket &output, const UdpPacket& udpPacket); + + // compute xyzi of points from decoded packet + // param packet is the decoded packet; xyzi of points after computed is puted in frame + virtual int ComputeXYZI(LidarDecodedFrame &frame, LidarDecodedPacket &packet); + + // determine whether frame splitting is needed + bool IsNeedFrameSplit(uint16_t azimuth); + + // compute lidar distance correction + void GetDistanceCorrection(int const& aziOrigin, int const& aziDelt, int const& elevation, + float const& distance, float& x, float& y, float& z); + private: + float distance_correction_b_; + float distance_correction_h_; + int block_num_; +}; +} // namespace lidar +} // namespace hesai + +#include "udp6_1_parser.cc" + +#endif // UDP6_1_PARSER_H_ diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/udp7_2_parser.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/udp7_2_parser.h new file mode 100644 index 0000000..83a8a0e --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/udp7_2_parser.h @@ -0,0 +1,106 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ + +/* + * File: udp7_2_parser.h + * Author: Zhang Yu + * Description: Declare Udp7_2Parser class +*/ + +#ifndef UDP7_2_PARSER_H_ +#define UDP7_2_PARSER_H_ + +#include "general_parser.h" +#include "lidar_types.h" +namespace hesai +{ +namespace lidar +{ + +constexpr int CHANNEL_MAX = 256; +constexpr int COLUMN_MAX = 384; +constexpr int HASH_BYTES_LENGTH = 64; + +struct PandarFTCorrectionsHeader { + uint8_t pilot[2]; + uint8_t version[2]; + uint8_t reversed[2]; + uint8_t column_number; + uint8_t channel_number; + uint8_t resolution; + PandarFTCorrectionsHeader() + : resolution(1) + {} +}; +struct PandarFTCorrections { +public: + using ColumnFloatArray = std::array; + using CorrectionMatrix = std::array; +public: + std::array elevations, azimuths; + uint8_t major_version; + uint8_t min_version; + std::string hash_value; +}; +// class Udp7_2Parser +// parsers packets and computes points for PandarFT120 +// you can parser the upd or pcap packets using the DocodePacket fuction +// you can compute xyzi of points using the ComputeXYZI fuction, which uses cpu to compute +template +class Udp7_2Parser : public GeneralParser { + public: + Udp7_2Parser(); + virtual ~Udp7_2Parser(); + + // get lidar correction file from local file,and pass to udp parser + virtual void LoadCorrectionFile(std::string correction_path); + virtual int LoadCorrectionString(char *correction_string); + int LoadCorrectionDatData(char *correction_string); + int LoadCorrectionCsvData(char *correction_string); + + // covert a origin udp packet to decoded packet, the decode function is in UdpParser module + // udp_packet is the origin udp packet, output is the decoded packet + virtual int DecodePacket(LidarDecodedPacket &output, const UdpPacket& udpPacket); + + // compute xyzi of points from decoded packet + // param packet is the decoded packet; xyzi of points after computed is puted in frame + virtual int ComputeXYZI(LidarDecodedFrame &frame, LidarDecodedPacket &packet); + + // determine whether frame splitting is needed + bool IsNeedFrameSplit(uint16_t column_id, uint16_t total_column); + + private: + int last_cloumn_id_; + PandarFTCorrections corrections_; +}; +} // namespace lidar +} // namespace hesai + +#include "udp7_2_parser.cc" + +#endif // UDP7_2_PARSER_H_ diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/udp_p40_parser.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/udp_p40_parser.h new file mode 100644 index 0000000..7cb1878 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/udp_p40_parser.h @@ -0,0 +1,74 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ + +/* + * File: udp_p40_parser.h + * Author: Zhang Yu + * Description: Declare UdpP40Parser class +*/ + +#ifndef UDP_P40_PARSER_H_ +#define UDP_P40_PARSER_H_ + +#include "general_parser.h" +#define BLOCKNUM 10 +#define LASERNUM 40 +namespace hesai +{ +namespace lidar +{ +// class UdpP40Parser +// parsers packets and computes points for Pandar40 +// you can parser the upd or pcap packets using the DocodePacket fuction +// you can compute xyzi of points using the ComputeXYZI fuction, which uses cpu to compute +template +class UdpP40Parser : public GeneralParser { + public: + UdpP40Parser(); + virtual ~UdpP40Parser(); + + // covert a origin udp packet to decoded packet, the decode function is in UdpParser module + // udp_packet is the origin udp packet, output is the decoded packet + virtual int DecodePacket(LidarDecodedPacket &output, const UdpPacket& udpPacket); + + // compute xyzi of points from decoded packet + // param packet is the decoded packet; xyzi of points after computed is puted in frame + virtual int ComputeXYZI(LidarDecodedFrame &frame, LidarDecodedPacket &packet); + + // virtual int ComputeXYZI(LidarDecodedFrame &frame, LidarDecodedPacket &packet); + + // determine whether frame splitting is needed + bool IsNeedFrameSplit(uint16_t azimuth); + private: +}; +} // namespace lidar +} // namespace hesai + +#include "udp_p40_parser.cc" + +#endif // UDP_P40_PARSER_H_ diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/udp_p64_parser.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/udp_p64_parser.h new file mode 100644 index 0000000..13a2e6c --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/udp_p64_parser.h @@ -0,0 +1,73 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ + +/* + * File: udp_p64_parser.h + * Author: Zhang Yu + * Description: Declare UdpP64Parser class +*/ + +#ifndef UDP_P64_PARSER_H_ +#define UDP_P64_PARSER_H_ + +#include "general_parser.h" +namespace hesai +{ +namespace lidar +{ +// class UdpP64Parser +// parsers packets and computes points for Pandar64 +// you can parser the upd or pcap packets using the DocodePacket fuction +// you can compute xyzi of points using the ComputeXYZI fuction, which uses cpu to compute +template +class UdpP64Parser : public GeneralParser { + public: + UdpP64Parser(); + virtual ~UdpP64Parser(); + + // covert a origin udp packet to decoded packet, the decode function is in UdpParser module + // udp_packet is the origin udp packet, output is the decoded packet + virtual int DecodePacket(LidarDecodedPacket &output, const UdpPacket& udpPacket); + + // compute xyzi of points from decoded packet + // param packet is the decoded packet; xyzi of points after computed is puted in frame + virtual int ComputeXYZI(LidarDecodedFrame &frame, LidarDecodedPacket &packet); + +// virtual int ComputeXYZI(LidarDecodedFrame &frame, LidarDecodedPacket &packet); + + // determine whether frame splitting is needed + bool IsNeedFrameSplit(uint16_t azimuth); + + private: +}; +} // namespace lidar +} // namespace hesai + +#include "udp_p64_parser.cc" + +#endif // UDP_P64_PARSER_H_ diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/src/general_parser.cc b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/src/general_parser.cc new file mode 100644 index 0000000..782096c --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/src/general_parser.cc @@ -0,0 +1,264 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ + +/* + * File: general_parser.cc + * Author: Zhang Yu + * Description: Implemente GeneralParser class +*/ + +#include "general_parser.h" +// #include +// #include +using namespace hesai::lidar; +template +GeneralParser::GeneralParser() { + this->motor_speed_ = 0; + this->return_mode_ = 0; + this->enable_update_monitor_info_ = false; + this->start_seqnum_ = 0; + this->last_seqnum_ = 0; + this->loss_count_ = 0; + this->start_time_ = 0; + this->last_azimuth_ = 0; + this->total_packet_count_ = 0; + this->enable_packet_loss_tool_ = false; + for (int i = 0; i < CIRCLE; ++i) { + this->sin_all_angle_[i] = std::sin(i * 2 * M_PI / CIRCLE); + this->cos_all_angle_[i] = std::cos(i * 2 * M_PI / CIRCLE); + } +} + +template +int GeneralParser::ComputeXYZI(LidarDecodedFrame &frame, LidarDecodedPacket &packet) { + return 0; +} +template +int GeneralParser::DecodePacket(LidarDecodedPacket &output, const UdpPacket& udpPacket) { + return 0; +} +template +int GeneralParser::DecodePacket(LidarDecodedFrame &frame, const UdpPacket& udpPacket) { + return 0; +} + +template +GeneralParser::~GeneralParser() { printf("release general Parser\n"); } + +template +void GeneralParser::SetFrameAzimuth(float frame_start_azimuth) { + this->frame_start_azimuth_ = frame_start_azimuth; +} +template +void GeneralParser::EnableUpdateMonitorInfo() { + this->enable_update_monitor_info_ = true; +} +template +void GeneralParser::DisableUpdateMonitorInfo() { + this->enable_update_monitor_info_ = false; +} +template +uint16_t *GeneralParser::GetMonitorInfo1() { return this->monitor_info1_; } +template +uint16_t *GeneralParser::GetMonitorInfo2() { return this->monitor_info2_; } +template +uint16_t *GeneralParser::GetMonitorInfo3() { return this->monitor_info3_; } +template +void GeneralParser::LoadCorrectionFile(std::string correction_path) { + int ret = 0; + std::ifstream fin(correction_path); + if (fin.is_open()) { + int length = 0; + fin.seekg(0, std::ios::end); + length = fin.tellg(); + fin.seekg(0, std::ios::beg); + char *buffer = new char[length]; + fin.read(buffer, length); + fin.close(); + ret = LoadCorrectionString(buffer); + if (ret != 0) { + std::cout << "Parse local correction file Error\n"; + } else { + std::cout << "Parser correction file success!" << std::endl; + } + } else { + std::cout << "Open correction file failed\n"; + return; + } +} +template +int GeneralParser::LoadCorrectionString(char *correction_content) { + std::string correction_content_str = correction_content; + std::istringstream ifs(correction_content_str); + std::string line; + + // skip first line "Laser id,Elevation,Azimuth" or "eeff" + std::getline(ifs, line); + float elevation_list[MAX_LASER_NUM], azimuth_list[MAX_LASER_NUM]; + std::vector vfirstLine; + boost::split(vfirstLine, line, boost::is_any_of(",")); + if (vfirstLine[0] == "EEFF" || vfirstLine[0] == "eeff") { + // skip second line + std::getline(ifs, line); + } + + int lineCount = 0; + while (std::getline(ifs, line)) { + std::vector vLineSplit; + boost::split(vLineSplit, line, boost::is_any_of(",")); + // skip error line or hash value line + if (vLineSplit.size() < 3) { + continue; + } else { + lineCount++; + } + float elevation, azimuth; + int laserId = 0; + + std::stringstream ss(line); + std::string subline; + std::getline(ss, subline, ','); + std::stringstream(subline) >> laserId; + std::getline(ss, subline, ','); + std::stringstream(subline) >> elevation; + std::getline(ss, subline, ','); + std::stringstream(subline) >> azimuth; + + if (laserId != lineCount || laserId >= MAX_LASER_NUM) { + std::cout << "laser id is wrong in correction file. laser Id:" + << laserId << ", line" << lineCount << std::endl; + // return -1; + } + elevation_list[laserId - 1] = elevation; + azimuth_list[laserId - 1] = azimuth; + } + this->elevation_correction_.resize(lineCount); + this->azimuth_collection_.resize(lineCount); + + for (int i = 0; i < lineCount; ++i) { + this->elevation_correction_[i] = elevation_list[i]; + this->azimuth_collection_[i] = azimuth_list[i]; + } + this->get_correction_file_ = true; + return 0; +} + +template +void GeneralParser::LoadFiretimesFile(std::string firetimes_path) { + std::ifstream inFile(firetimes_path, std::ios::in); + if (inFile.is_open()) { + std::string lineStr; + //skip first line + std::getline(inFile, lineStr); + while (getline(inFile, lineStr)) { + std::stringstream ss(lineStr); + std::string index, deltTime; + std::getline(ss, index, ','); + std::getline(ss, deltTime, ','); + } + this->get_firetime_file_ = true; + std::cout << "Open firetime file success!" << std::endl; + inFile.close(); + return; + } else { + std::cout << "Open firetime file failed" << std::endl; + this->get_firetime_file_ = false; + return; + } +} + +template +void GeneralParser::EnablePacketLossTool(bool enable) { + this->enable_packet_loss_tool_ = enable; +} + +template +void GeneralParser::SetEnableFireTimeCorrection(bool enable) { + this->enable_firetime_correction_ = enable; +} +template +void GeneralParser::SetEnableDistanceCorrection(bool enable) { + this->enable_distance_correction_ = enable; +} +template +int GeneralParser::LoadFiretimesString(char *correction_string) { + printf("load firetimes string\n"); + return 0; +} + +template +double GeneralParser::GetFiretimesCorrection(int laserId, double speed) { + return this->firetime_correction_[laserId] * speed * 6E-6; +} +template +void GeneralParser::GetDistanceCorrection(double &azimuth, double &elevation, + double &distance) { + printf("get distance correction\n"); +} + +template +void GeneralParser::TransformPoint(float& x, float& y, float& z) +{ + // Eigen::AngleAxisd current_rotation_x(transform_.roll, Eigen::Vector3d::UnitX()); + // Eigen::AngleAxisd current_rotation_y(transform_.pitch, Eigen::Vector3d::UnitY()); + // Eigen::AngleAxisd current_rotation_z(transform_.yaw, Eigen::Vector3d::UnitZ()); + // Eigen::Translation3d current_translation(transform_.x, transform_.y, + // transform_.z); + // Eigen::Matrix4d trans = (current_translation * current_rotation_z * current_rotation_y * current_rotation_x).matrix(); + // Eigen::Vector4d target_ori(x, y, z, 1); + // Eigen::Vector4d target_rotate = trans * target_ori; + // x = target_rotate(0); + // y = target_rotate(1); + // z = target_rotate(2); + + float cosa = std::cos(transform_.roll); + float sina = std::sin(transform_.roll); + float cosb = std::cos(transform_.pitch); + float sinb = std::sin(transform_.pitch); + float cosc = std::cos(transform_.yaw); + float sinc = std::sin(transform_.yaw); + + float x_ = cosb * cosc * x + (sina * sinb * cosc - cosa * sinc) * y + + (sina * sinc + cosa * sinb * cosc) * z + transform_.x; + float y_ = cosb * sinc * x + (cosa * cosc + sina * sinb * sinc) * y + + (cosa * sinb * sinc - sina * cosc) * z + transform_.y; + float z_ = -sinb * x + sina * cosb * y + cosa * cosb * z + transform_.z; + x = x_; + y = y_; + z = z_; +} + +template +void GeneralParser::SetTransformPara(float x, float y, float z, float roll, float pitch, float yaw) { + transform_.x = x; + transform_.y = y; + transform_.z = z; + transform_.roll = roll; + transform_.yaw = yaw; + transform_.pitch = pitch; +} diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/src/udp1_4_parser.cc b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/src/udp1_4_parser.cc new file mode 100644 index 0000000..b42bb3c --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/src/udp1_4_parser.cc @@ -0,0 +1,367 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ + +/* + * File: udp1_4_parser.cc + * Author: Zhang Yu + * Description: Implemente Udp1_4Parser class +*/ + +#include "udp1_4_parser.h" +#include "udp_protocol_v1_4.h" +using namespace hesai::lidar; +template +Udp1_4Parser::Udp1_4Parser() { + this->motor_speed_ = 0; + this->return_mode_ = 0; + distance_correction_para_a_ = 1; + distance_correction_para_b_ = 0.012; + distance_correction_para_h_ = 0.04; + distance_correction_para_c_ = std::sqrt(distance_correction_para_b_ * distance_correction_para_b_ + distance_correction_para_h_ * distance_correction_para_h_); + distance_correction_para_d_ = std::atan(distance_correction_para_b_ / distance_correction_para_h_); + use_frame_start_azimuth_ = true; +} + +template +Udp1_4Parser::~Udp1_4Parser() { printf("release general parser\n"); } + +template +void Udp1_4Parser::LoadFiretimesFile(std::string firetimes_path) { + std::ifstream inFile(firetimes_path, std::ios::in); + if (inFile.is_open()) { + int uselessLine = 0; + int count = 0; + std::string lineStr; + while (std::getline(inFile, lineStr)) { + std::stringstream ss(lineStr); + std::string str; + std::vector strList; + strList.reserve(20); + while(std::getline(ss, str, ',')){ + strList.push_back(str); + } + if(uselessLine == 0) { + std::string dist; + for (auto e : strList[0]) { + if (e == '.' || (e >= '0' && e <= '9')) { + dist.push_back(e); + } + } + section_distance = std::stod(dist); + } + if(uselessLine < 3) { + uselessLine++; + continue; + } + if (lineStr[lineStr.size() - 1] == '\n'){ + lineStr = lineStr.substr(lineStr.size() - 1); + } + if (strList.size() < 17) { + std::cout << "nvalid input file!" << std::endl; + this->get_firetime_file_ = false; + return; + } + int idx = std::stoi(strList[0]) - 1; + for (int i = 1; i <= 15; i += 2) { + int a = std::stoi(strList[i]); + int b = std::stoi(strList[i + 1]); + firetime_section_values[idx].section_values[i / 2].firetime[0] = a; + firetime_section_values[idx].section_values[i / 2].firetime[1] = b; + } + count++; + if (count > 128) { + std::cout << "Invalid input file!" << std::endl; + this->get_firetime_file_ = false; + return; + } + } + } else { + std::cout << "Open firetime file failed" << std::endl; + this->get_firetime_file_ = false; + return; + } + this->get_firetime_file_ = true; + std::cout << "Open firetime file success!" << std::endl; + return; +} + +template +double Udp1_4Parser::GetFiretimesCorrection(int laserId, double speed, uint8_t optMode, uint8_t angleState,uint16_t dist){ + int idx = 0; + switch (optMode) + { + case 0: + idx = angleState; + break; + case 2: + idx = 4 + angleState; + break; + case 3: + idx = 6 + angleState; + break; + + default: + return 0.0; + break; + } + int k = (dist >= section_distance)? 0 : 1; + return firetime_section_values[laserId].section_values[idx].firetime[k] * speed * 6E-9; +} + +template +int Udp1_4Parser::ComputeXYZI(LidarDecodedFrame &frame, LidarDecodedPacket &packet) { + frame.lidar_state = packet.lidar_state; + frame.work_mode = packet.work_mode; + for (int blockid = 0; blockid < packet.block_num; blockid++) { + T_Point point; + int Azimuth = packet.azimuth[blockid * packet.laser_num]; + int elevation = 0; + auto azimuth = Azimuth; + + for (int i = 0; i < packet.laser_num; i++) { + int point_index = packet.packet_index * packet.points_num + blockid * packet.laser_num + i; + float distance = packet.distances[blockid * packet.laser_num + i] * packet.distance_unit; + if (this->get_correction_file_) { + elevation = this->elevation_correction_[i] * kResolutionInt; + elevation = (CIRCLE + elevation) % CIRCLE; + azimuth = Azimuth + this->azimuth_collection_[i] * kResolutionInt; + azimuth = (CIRCLE + azimuth) % CIRCLE; + } + if (this->enable_distance_correction_) { + GetDistanceCorrection(i, distance, azimuth, elevation); + } + float xyDistance = distance * this->cos_all_angle_[(elevation)]; + float x = xyDistance * this->sin_all_angle_[(azimuth)]; + float y = xyDistance * this->cos_all_angle_[(azimuth)]; + float z = distance * this->sin_all_angle_[(elevation)]; + + this->TransformPoint(x, y, z); + setX(frame.points[point_index], x); + setY(frame.points[point_index], y); + setZ(frame.points[point_index], z); + setIntensity(frame.points[point_index], packet.reflectivities[blockid * packet.laser_num + i]); + setTimestamp(frame.points[point_index], double(packet.sensor_timestamp) / kMicrosecondToSecond); + setRing(frame.points[point_index], i); + } + } + frame.points_num += packet.points_num; + frame.packet_num = packet.packet_index; + return 0; +} + +template +int Udp1_4Parser::DecodePacket(LidarDecodedPacket &output, const UdpPacket& udpPacket) { + if (udpPacket.buffer[0] != 0xEE || udpPacket.buffer[1] != 0xFF) return -1; + const HS_LIDAR_HEADER_ME_V4 *pHeader = + reinterpret_cast( + udpPacket.buffer + sizeof(HS_LIDAR_PRE_HEADER)); + + // point to azimuth of udp start block + const HS_LIDAR_BODY_AZIMUTH_ME_V4 *pAzimuth = + reinterpret_cast( + (const unsigned char *)pHeader + sizeof(HS_LIDAR_HEADER_ME_V4)); + if(pHeader->HasFuncSafety()) { + const auto *function_savety_ptr = reinterpret_cast( + (const unsigned char *)pHeader + sizeof(HS_LIDAR_HEADER_ME_V4) + + (sizeof(HS_LIDAR_BODY_AZIMUTH_ME_V4) + + (pHeader->HasConfidenceLevel() + ? sizeof(HS_LIDAR_BODY_CHN_UNIT_ME_V4) + : sizeof(HS_LIDAR_BODY_CHN_UNIT_NO_CONF_ME_V4)) * + pHeader->GetLaserNum()) * + pHeader->GetBlockNum() + + sizeof(HS_LIDAR_BODY_CRC_ME_V4) + ); + output.lidar_state = function_savety_ptr->GetLidarState(); + } else { + output.lidar_state = -1; + } + + const auto *pTail = reinterpret_cast( + (const unsigned char *)pHeader + sizeof(HS_LIDAR_HEADER_ME_V4) + + (sizeof(HS_LIDAR_BODY_AZIMUTH_ME_V4) + + (pHeader->HasConfidenceLevel() + ? sizeof(HS_LIDAR_BODY_CHN_UNIT_ME_V4) + : sizeof(HS_LIDAR_BODY_CHN_UNIT_NO_CONF_ME_V4)) * + pHeader->GetLaserNum()) * + pHeader->GetBlockNum() + + sizeof(HS_LIDAR_BODY_CRC_ME_V4) + + (pHeader->HasFuncSafety() ? sizeof(HS_LIDAR_FUNC_SAFETY_ME_V4) : 0)); + if (pHeader->HasSeqNum()) { + const HS_LIDAR_TAIL_SEQ_NUM_ME_V4 *pTailSeqNum = + reinterpret_cast( + (const unsigned char *)pHeader + sizeof(HS_LIDAR_HEADER_ME_V4) + + (sizeof(HS_LIDAR_BODY_AZIMUTH_ME_V4) + + (pHeader->HasConfidenceLevel() + ? sizeof(HS_LIDAR_BODY_CHN_UNIT_ME_V4) + : sizeof(HS_LIDAR_BODY_CHN_UNIT_NO_CONF_ME_V4)) * + pHeader->GetLaserNum()) * + pHeader->GetBlockNum() + + sizeof(HS_LIDAR_BODY_CRC_ME_V4) + + (pHeader->HasFuncSafety() ? sizeof(HS_LIDAR_FUNC_SAFETY_ME_V4) + : 0) + + sizeof(HS_LIDAR_TAIL_ME_V4)); + // printf("%u\n",pTailSeqNum->GetSeqNum()); + + //skip decode packet if enable packet_loss_tool + if (this->enable_packet_loss_tool_ == true) { + this->current_seqnum_ = pTailSeqNum->m_u32SeqNum; + if (this->current_seqnum_ > this->last_seqnum_ && this->last_seqnum_ != 0) { + this->total_packet_count_ += this->current_seqnum_ - this->last_seqnum_; + } + pTailSeqNum->CalPktLoss(this->start_seqnum_, this->last_seqnum_, this->loss_count_, this->start_time_, this->total_loss_count_, this->total_start_seqnum_); + } + } + output.sensor_timestamp = pTail->GetMicroLidarTimeU64(); + output.host_timestamp = GetMicroTickCountU64(); + + if(this->enable_packet_loss_tool_ == true) return 0 ; + this->spin_speed_ = pTail->m_u16MotorSpeed; + this->is_dual_return_= pTail->IsDualReturn(); + output.spin_speed = pTail->m_u16MotorSpeed; + output.work_mode = pTail->getOperationMode(); + // 如下三条：max min这样的参数一点用都没有 + output.maxPoints = pHeader->GetBlockNum() * pHeader->GetLaserNum(); + // 不填直接崩调，=0界面一个点也没有 + output.points_num = pHeader->GetBlockNum() * pHeader->GetLaserNum(); + // 不填则仅显示很小一部分点云 + output.scan_complete = false; + output.distance_unit = pHeader->GetDistUnit(); + int index = 0; + float minAzimuth = 0; + float maxAzimuth = 0; + output.block_num = pHeader->GetBlockNum(); + output.laser_num = pHeader->GetLaserNum(); + + if (this->enable_update_monitor_info_) { + this->monitor_info1_[pTail->m_reservedInfo1.m_u8ID] = + pTail->m_reservedInfo1.m_u16Sts; + this->monitor_info2_[pTail->m_reservedInfo2.m_u8ID] = + pTail->m_reservedInfo2.m_u16Sts; + this->monitor_info3_[pTail->m_reservedInfo3.m_u8ID] = pTail->m_reservedInfo3.m_u16Sts; + } + uint8_t optMode = pTail->getOperationMode(); + for (int i = 0; i < pHeader->GetBlockNum(); i++) { + uint8_t angleState = pTail->getAngleState(i); + uint16_t u16Azimuth = pAzimuth->GetAzimuth(); + output.azimuths = u16Azimuth; + // point to channel unit addr + if (pHeader->HasConfidenceLevel()) { + const HS_LIDAR_BODY_CHN_UNIT_ME_V4 *pChnUnit = + reinterpret_cast( + (const unsigned char *)pAzimuth + + sizeof(HS_LIDAR_BODY_AZIMUTH_ME_V4)); + + // point to next block azimuth addr + pAzimuth = reinterpret_cast( + (const unsigned char *)pAzimuth + + sizeof(HS_LIDAR_BODY_AZIMUTH_ME_V4) + + sizeof(HS_LIDAR_BODY_CHN_UNIT_ME_V4) * pHeader->GetLaserNum()); + auto elevation = 0; + for (int j = 0; j < pHeader->GetLaserNum(); ++j) { + if (this->get_firetime_file_) { + float fireTimeCollection = GetFiretimesCorrection(i, this->spin_speed_, optMode, angleState, pChnUnit->GetDistance()); + output.azimuth[index] = u16Azimuth + fireTimeCollection * kResolutionInt; + }else { + output.azimuth[index] = u16Azimuth; + } + output.reflectivities[index] = pChnUnit->GetReflectivity(); + output.distances[index] = pChnUnit->GetDistance(); + output.elevation[index] = elevation; + pChnUnit = pChnUnit + 1; + index++; + } + } else { + const HS_LIDAR_BODY_CHN_UNIT_NO_CONF_ME_V4 *pChnUnitNoConf = + reinterpret_cast( + (const unsigned char *)pAzimuth + + sizeof(HS_LIDAR_BODY_AZIMUTH_ME_V4)); + + // point to next block azimuth addr + pAzimuth = reinterpret_cast( + (const unsigned char *)pAzimuth + + sizeof(HS_LIDAR_BODY_AZIMUTH_ME_V4) + + sizeof(HS_LIDAR_BODY_CHN_UNIT_NO_CONF_ME_V4) * + pHeader->GetLaserNum()); + auto elevation = 0; + for (int j = 0; j < pHeader->GetLaserNum(); ++j) { + if (this->get_firetime_file_) { + float fireTimeCollection = GetFiretimesCorrection(i, this->spin_speed_, optMode, angleState, pChnUnitNoConf->GetDistance()); + output.azimuth[index] = u16Azimuth + fireTimeCollection * kResolutionInt; + }else { + output.azimuth[index] = u16Azimuth; + } + output.reflectivities[index] = pChnUnitNoConf->GetReflectivity(); + output.distances[index] = pChnUnitNoConf->GetDistance(); + output.elevation[index] = elevation; + index++; + pChnUnitNoConf += 1; + } + } + if (IsNeedFrameSplit(u16Azimuth)) { + output.scan_complete = true; + } + this->last_azimuth_ = u16Azimuth; + } + + return 0; +} + +template +bool Udp1_4Parser::IsNeedFrameSplit(uint16_t azimuth) { + if (this->last_azimuth_ > azimuth && (this->last_azimuth_- azimuth > kSplitFrameMinAngle)) { + use_frame_start_azimuth_ = true; + } + //do not use frame_start_azimuth, split frame near 360 degree + if (this->frame_start_azimuth_ < 1 || this->frame_start_azimuth_ > 359) { + if (this->last_azimuth_ > azimuth && (this->last_azimuth_- azimuth > kSplitFrameMinAngle)) { + return true; + } + return false; + } else { //use frame_start_azimuth + if ((azimuth >= uint16_t(this->frame_start_azimuth_ * kResolutionInt)) && (use_frame_start_azimuth_ == true)) { + use_frame_start_azimuth_ = false; + return true; + } + return false; + } +} + +template +void Udp1_4Parser::GetDistanceCorrection(int laser_id, float distance, int& azimuth, int& elevation) { + double sin_delt_elevation = distance_correction_para_c_ / distance * this->sin_all_angle_[elevation]; + if (sin_delt_elevation >= -1 && sin_delt_elevation <= 1) { + elevation -= distance_correction_para_a_ * std::asin(sin_delt_elevation) * kHalfCircleFloat / M_PI; + elevation = elevation % CIRCLE; + } + double sin_delt_azimuth = distance_correction_para_c_ / distance / this->cos_all_angle_[elevation] * \ + this->sin_all_angle_[int((distance_correction_para_d_ + this->azimuth_collection_[laser_id]) * kResolutionInt) % CIRCLE]; + if (sin_delt_azimuth >= -1 && sin_delt_azimuth <= 1) { + azimuth -= distance_correction_para_a_ * std::asin(sin_delt_azimuth) * kHalfCircleFloat / M_PI; + azimuth = azimuth % CIRCLE; + } +} \ No newline at end of file diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/src/udp2_4_parser.cc b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/src/udp2_4_parser.cc new file mode 100644 index 0000000..01d45c4 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/src/udp2_4_parser.cc @@ -0,0 +1,337 @@ +#include "udp2_4_parser.h" +#include "udp_protocol_v2_4.h" +#include "udp_protocol_header.h" +#include "udp_parser.h" + +using namespace hesai::lidar; + +template +Udp2_4Parser::Udp2_4Parser() { + this->get_correction_file_ = false; + for (int i = 0; i < CIRCLE; ++i) { + this->sin_all_angle_[i] = std::sin(i * 2 * M_PI / CIRCLE); + this->cos_all_angle_[i] = std::cos(i * 2 * M_PI / CIRCLE); + } +} + +template +Udp2_4Parser::~Udp2_4Parser() { printf("release Udp2_4Parser\n"); } + +// The main purpose of this function is to open, read, and parse a lidar calibration file, and print appropriate messages based on the parsing results. +// This function can read both .bin and .csv files. +template +void Udp2_4Parser::LoadCorrectionFile(std::string lidar_correction_file) { + int type = 0; + int length = lidar_correction_file.length(); + if (length >= 4) { + std::string extension = lidar_correction_file.substr(length - 4); + if (extension == ".bin") { + type = 1; // .bin + printf(".bin\n"); + } else if (extension == ".csv") { + type = 2; // .csv + printf(".csv\n"); + } else { + type = 0; // wrong + return; + } + } + if (type == 1) { + // print information + int ret = 0; + printf("load correction file from local correction.bin now!\n"); + std::ifstream fin(lidar_correction_file); + if (fin.is_open()) { + printf("Open correction file success!\n"); + int length = 0; + std::string str_lidar_calibration; + fin.seekg(0, std::ios::end); + length = fin.tellg(); + // return the begin of file + fin.seekg(0, std::ios::beg); + char *buffer = new char[length]; + // file --> buffer + fin.read(buffer, length); + fin.close(); + ret = LoadCorrectionString(buffer); + if (ret != 0) { + printf("Parse local Correction file Error!\n"); + } else { + printf("Parse local Correction file Success!!!\n"); + this->get_correction_file_ = true; + } + } else { // open failed + printf("Open correction file failed\n"); + return; + } + } + + if (type == 2) { + // print information + int ret = 0; + printf("load correction file from local correction.csv now!\n"); + ret = LoadCorrectionString_csv(lidar_correction_file); + if (1 == ret) { + printf("Parse local Correction file Success!\n"); + this->get_correction_file_ = true; + } else { + printf("Parse local Correction file faild!\n"); + } + } +} + +// csv ----> correction +template +int Udp2_4Parser::LoadCorrectionString_csv(std::string lidar_correction_file) +{ + std::ifstream file(lidar_correction_file); + if (!file.is_open()) { + printf("open the .csv faild\n"); + return 0; + } + std::vector column2; + std::vector column3; + std::string line; + std::getline(file, line); + while (std::getline(file, line)) { + std::stringstream lineStream(line); + std::string cell; + std::vector row; + while (std::getline(lineStream, cell, ',')) { + row.push_back(cell); + } + if (row.size() >= 3) { + column2.push_back(std::stof(row[1])); + column3.push_back(std::stof(row[2])); + } + } + file.close(); + m_ET_corrections.delimiter[0] = 0xee; + m_ET_corrections.delimiter[1] = 0xff; + m_ET_corrections.major_version = 0x03; + m_ET_corrections.min_version = 0x01; + m_ET_corrections.channel_number = 0x40; + m_ET_corrections.angle_division = 0x01; + + for (int i = 0; i < column3.size(); i++) { + m_ET_corrections.elevations[i] = column2[i]; + m_ET_corrections.azimuths[i] = column3[i]; + } + return 1; +} + +// buffer(.bin) ---> correction +template +int Udp2_4Parser::LoadCorrectionString(char *data) { + try { + char *p = data; + struct ETCorrections_v4_Header ETheader = *((struct ETCorrections_v4_Header* )p); + if (0xee == ETheader.delimiter[0] && 0xff == ETheader.delimiter[1]) { + switch (ETheader.min_version) { + case 1: { + memcpy((void *)&m_ET_corrections, p, sizeof(struct ETCorrections_v4_Header)); + p += sizeof(ETCorrections_v4_Header); + auto channel_num = m_ET_corrections.channel_number; + uint16_t division = m_ET_corrections.angle_division; + memcpy((void *)&m_ET_corrections.raw_azimuths, p, + sizeof(int16_t) * channel_num); + p += sizeof(int16_t) * channel_num; + memcpy((void *)&m_ET_corrections.raw_elevations, p, + sizeof(int16_t) * channel_num); + p += sizeof(uint32_t) * channel_num; + m_ET_corrections.elevations[0] = ((float)(m_ET_corrections.apha)) / division; + m_ET_corrections.elevations[1] = ((float)(m_ET_corrections.beta)) / division; + m_ET_corrections.elevations[2] = ((float)(m_ET_corrections.gamma)) / division; + printf("apha:%f, beta:%f, gamma:%f\n", m_ET_corrections.elevations[0], m_ET_corrections.elevations[1], m_ET_corrections.elevations[2]); + for (int i = 0; i < channel_num; i++) { + m_ET_corrections.azimuths[i + 3] = ((float)(m_ET_corrections.raw_azimuths[i])) / division; + m_ET_corrections.elevations[i + 3] = ((float)(m_ET_corrections.raw_elevations[i])) / division; + printf("%d %f %f \n",i, m_ET_corrections.azimuths[i + 3], m_ET_corrections.elevations[i + 3]); + } + memcpy((void*)&m_ET_corrections.SHA_value, p, 32); + // successed + this->get_correction_file_ = true; + return 0; + } break; + default: + printf("min_version is wrong!\n"); + break; + } + } else { + return -1; + } + } catch (const std::exception &e) { + std::cerr << e.what() << '\n'; + return -1; + } + return -1; +} + +// decode, udpPacket---->DecodePacket +template +int Udp2_4Parser::DecodePacket(LidarDecodedPacket &output, const UdpPacket& udpPacket) { + // verify buffer[0] and buffer[1] + if (udpPacket.buffer[0] != 0xEE || udpPacket.buffer[1] != 0xFF ) { + return -1; + } + + // point to header skip pre_header + const HS_LIDAR_HEADER_ET_V4* pHeader = + reinterpret_cast( + &(udpPacket.buffer[0]) + sizeof(HS_LIDAR_PRE_HEADER)); + // point to tails + const HS_LIDAR_TAIL_ET_V4 *pTail = + reinterpret_cast( + &(udpPacket.buffer[0]) + pHeader->GetPacketSize() - + sizeof(HS_LIDAR_TAIL_CRC_ET_V4) - + sizeof(HS_LIDAR_TAIL_SEQ_NUM_ET_V4) - + sizeof(HS_LIDAR_TAIL_ET_V4) + ); + if (pHeader->HasSeqNum()){ + const HS_LIDAR_TAIL_SEQ_NUM_ET_V4 *pTailSeqNum = + reinterpret_cast( + &(udpPacket.buffer[0]) + pHeader->GetPacketSize() - + sizeof(HS_LIDAR_TAIL_CRC_ET_V4) - + sizeof(HS_LIDAR_TAIL_SEQ_NUM_ET_V4) + ); + } + const HS_LIDAR_BODY_SEQ2_ET_V4* pSeq2 = + reinterpret_cast( + (const unsigned char *)pHeader + + sizeof(HS_LIDAR_HEADER_ET_V4) + 2 + ); + const HS_LIDAR_BODY_LASTER_UNIT_ET_V4* pUnit = + reinterpret_cast( + (const unsigned char *)pSeq2 + + sizeof(HS_LIDAR_BODY_SEQ2_ET_V4) + ); + // write the value to output + output.host_timestamp = GetMicroTickCountU64(); + output.distance_unit = pHeader->GetDistUnit(); + output.sensor_timestamp = pTail->GetMicroLidarTimeU64(); + output.host_timestamp = GetMicroTickCountU64(); + output.points_num = pHeader->GetBlockNum() * pHeader->GetLaserNum(); + output.scan_complete = false; + output.block_num = pHeader->GetBlockNum(); + output.laser_num = pHeader->GetLaserNum(); + + int index_unit = 0; + int index_seq = 0; + for (int blockId = 0; blockId < pHeader->GetBlockNum(); blockId++) { + for (int seqId = 0; seqId < pHeader->GetSeqNum(); seqId++) { + int16_t horizontalAngle = pSeq2->GetHorizontalAngle(); + int16_t verticalAngle = pSeq2->GetVerticalAngle(); + for (int unitId = 0; unitId < (pHeader->GetLaserNum()/pHeader->GetSeqNum()); unitId++){ + int16_t distance = pUnit->GetDistance(); + int8_t reflectivity = pUnit->GetReflectivity(); + int8_t confidence = pUnit->GetConfidence(); + output.reflectivities[index_unit] = reflectivity; + output.distances[index_unit] = distance; + // get the degrees + output.azimuth[index_unit] = horizontalAngle/512.0f; + output.elevation[index_unit] = verticalAngle/512.0f; + index_unit++; + pUnit += 1; + } + // pSeq2 next + pSeq2 = reinterpret_cast( + (const unsigned char *)pSeq2 + + sizeof(HS_LIDAR_BODY_SEQ2_ET_V4) + + sizeof(HS_LIDAR_BODY_LASTER_UNIT_ET_V4)*(pHeader->GetLaserNum()/pHeader->GetSeqNum()) + ); + + // pUnit next + pUnit = reinterpret_cast( + (const unsigned char *)pSeq2 + + sizeof(HS_LIDAR_BODY_SEQ2_ET_V4) + ); + } + if (blockId < (pHeader->GetBlockNum())) { + // skip point id + pSeq2 = reinterpret_cast( + (const unsigned char *)pSeq2 + 2 + ); + pUnit = reinterpret_cast( + (const unsigned char *)pUnit + 2 + ); + } // end if (blockId < (pHeader->GetBlockNum())) + } + //printf("%d\n",index_unit); + uint16_t nowid = pTail->GetFrameID(); + //printf("%d\n", nowid); + if (this->use_angle_ && IsNeedFrameSplit(nowid)) { + output.scan_complete = true; + } + this->last_frameid_ = pTail->GetFrameID(); + return 0; +} + +// Framing +template +bool Udp2_4Parser::IsNeedFrameSplit(uint16_t nowid) { + if ( nowid != this->last_frameid_ ) { + return true; + } + return false; +} + +// get elevations[i] +template +int16_t Udp2_4Parser::GetVecticalAngle(int channel) { + if (this->get_correction_file_ == false) { + printf ("GetVecticalAngle: no correction file get, Error\n"); + return -1; + } + return m_ET_corrections.elevations[channel]; +} + + +template +int Udp2_4Parser::ComputeXYZI(LidarDecodedFrame &frame, LidarDecodedPacket &packet) { + // get configer information from m_ET_corrections, 1 block 1 m_ET_corrections + float division = (float)m_ET_corrections.angle_division; + float apha = m_ET_corrections.elevations[0]; + float beta = m_ET_corrections.elevations[1]; + float gamma = m_ET_corrections.elevations[2]; + // get the laser_num + uint16_t lasernum = packet.laser_num; + for (int blockId = 0; blockId < packet.block_num; blockId++) { + T_Point point; + float delte_apha = 0; + float delte_theta = 0; + for (int i = 0; i < lasernum; i++) { + int point_index = packet.packet_index * packet.points_num + blockId * packet.laser_num + i; + // get phi and psi and distance + float raw_azimuth = packet.azimuth[i + blockId*lasernum]; + float raw_elevation = packet.elevation[i + blockId*lasernum]; + float distance = packet.distances[i + blockId*lasernum] * packet.distance_unit; + float phi = m_ET_corrections.azimuths[i + 3]; + float theta = m_ET_corrections.elevations[i + 3]; + float an = apha + phi; + float theta_n = (raw_elevation + theta / std::cos(an * M_PI / 180)); + float elv_v = raw_elevation * M_PI / 180 + theta * M_PI / 180 - std::tan(raw_elevation * M_PI / 180) * (1 - std::cos(an * M_PI / 180)) ; + float delt_azi_v = std::sin(an * M_PI / 180) * std::cos(an * M_PI / 180) * theta_n * theta_n / 2 * 1.016 * M_PI / 180 * M_PI / 180; + float eta = phi + delt_azi_v * 180 / M_PI + beta + raw_azimuth / 2; + float delt_azi_h = std::sin(eta * M_PI / 180) * std::tan(2 * gamma * M_PI / 180) * std::tan(elv_v ) + std::sin(2 * eta * M_PI / 180) * gamma * gamma * M_PI / 180 * M_PI / 180; + float elv_h = elv_v * 180 / M_PI + std::cos(eta * M_PI / 180) * 2 * gamma ; + float azi_h = 90 + raw_azimuth + delt_azi_h * 180 / M_PI + delt_azi_v * 180 / M_PI + phi; + + + int azimuth = (int)(azi_h * 100 + CIRCLE) % CIRCLE; + int elevation = (int)(elv_h * 100 + CIRCLE) % CIRCLE; + float xyDistance = distance * this->cos_all_angle_[elevation]; + float x = xyDistance * this->sin_all_angle_[azimuth]; + float y = xyDistance * this->cos_all_angle_[azimuth]; + float z = distance * this->sin_all_angle_[elevation]; + setX(frame.points[point_index], x); + setY(frame.points[point_index], y); + setZ(frame.points[point_index], z); + setIntensity(frame.points[point_index], packet.reflectivities[blockId * packet.laser_num + i]); + setTimestamp(frame.points[point_index], double(packet.sensor_timestamp) / kMicrosecondToSecond); + setRing(frame.points[point_index], i); + } + } + frame.points_num += packet.points_num; + frame.packet_num = packet.packet_index; + return 0; +} \ No newline at end of file diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/src/udp2_5_parser.cc b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/src/udp2_5_parser.cc new file mode 100644 index 0000000..1dd4211 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/src/udp2_5_parser.cc @@ -0,0 +1,340 @@ + +#include "udp2_5_parser.h" +#include "udp_protocol_v2_5.h" +#include "udp_protocol_header.h" +#include "udp_parser.h" + +using namespace hesai::lidar; + +template +Udp2_5Parser::Udp2_5Parser() { + this->get_correction_file_ = false; + for (int i = 0; i < CIRCLE; ++i) { + this->sin_all_angle_[i] = std::sin(i * 2 * M_PI / CIRCLE); + this->cos_all_angle_[i] = std::cos(i * 2 * M_PI / CIRCLE); + } +} + +template +Udp2_5Parser::~Udp2_5Parser() { printf("release Udp2_5Parser\n"); } + + +// The main purpose of this function is to open, read, and parse a lidar calibration file, and print appropriate messages based on the parsing results. +// This function can read both .bin and .csv files. +template +void Udp2_5Parser::LoadCorrectionFile(std::string correction_path) { + int ret = 0; + printf("load correction file from local correction file now!\n"); + std::ifstream fin(correction_path); + if (fin.is_open()) { + printf("Open correction file success\n"); + int length = 0; + std::string str_lidar_calibration; + fin.seekg(0, std::ios::end); + length = fin.tellg(); + fin.seekg(0, std::ios::beg); + char *buffer = new char[length]; + fin.read(buffer, length); + fin.close(); + str_lidar_calibration = buffer; + ret = LoadCorrectionString(buffer); + if (ret != 0) { + printf("Parse local Correction file Error\n"); + } else { + + printf("Parse local Correction file Success!!!\n"); + } + } else { + printf("Open correction file failed\n"); + return; + } +} + +template +int Udp2_5Parser::LoadCorrectionString(char *data) { + if (LoadCorrectionDatData(data) == 0) { + return 0; + } + return LoadCorrectionCsvData(data); +} + +// csv ----> correction +template +int Udp2_5Parser::LoadCorrectionCsvData(char *correction_string) +{ + std::string correction_content_str = correction_string; + std::istringstream ifs(correction_content_str); + std::string line; + + // skip first line "Laser id,Elevation,Azimuth" or "eeff" + std::getline(ifs, line); + float elevation_list[MAX_LASER_NUM], azimuth_list[MAX_LASER_NUM]; + std::vector vfirstLine; + boost::split(vfirstLine, line, boost::is_any_of(",")); + if (vfirstLine[0] == "EEFF" || vfirstLine[0] == "eeff") { + // skip second line + std::getline(ifs, line); + } + + int lineCount = 0; + while (std::getline(ifs, line)) { + std::vector vLineSplit; + boost::split(vLineSplit, line, boost::is_any_of(",")); + // skip error line or hash value line + if (vLineSplit.size() < 3) { + continue; + } else { + lineCount++; + } + float elevation, azimuth; + int laserId = 0; + + std::stringstream ss(line); + std::string subline; + std::getline(ss, subline, ','); + std::stringstream(subline) >> laserId; + std::getline(ss, subline, ','); + std::stringstream(subline) >> elevation; + std::getline(ss, subline, ','); + std::stringstream(subline) >> azimuth; + + if (laserId != lineCount || laserId >= MAX_LASER_NUM) { + std::cout << "laser id is wrong in correction file. laser Id:" + << laserId << ", line" << lineCount << std::endl; + // return -1; + } + elevation_list[laserId - 1] = elevation; + azimuth_list[laserId - 1] = azimuth; + } + + for (int i = 0; i < lineCount; ++i) { + corrections_.azimuths[i] = azimuth_list[i]; + corrections_.elevations[i] = elevation_list[i]; + printf("%d %f %f \n",i, corrections_.azimuths[i], corrections_.elevations[i]); + } + this->get_correction_file_ = true; + return 0; +} + + +// buffer(.bin) ---> correction +template +int Udp2_5Parser::LoadCorrectionDatData(char *data) { + try { + char *p = data; + struct ETCorrectionsHeader ETheader = *((struct ETCorrectionsHeader* )p); + if (0xee == ETheader.delimiter[0] && 0xff == ETheader.delimiter[1]) { + switch (ETheader.min_version) { + case 1: { + memcpy((void *)&corrections_, p, sizeof(struct ETCorrectionsHeader)); + p += sizeof(ETCorrectionsHeader); + auto channel_num = corrections_.channel_number; + uint16_t division = corrections_.angle_division; + memcpy((void *)&corrections_.raw_azimuths, p, + sizeof(int16_t) * channel_num); + p += sizeof(int16_t) * channel_num; + memcpy((void *)&corrections_.raw_elevations, p, + sizeof(int16_t) * channel_num); + p += sizeof(uint32_t) * channel_num; + corrections_.elevations[0] = ((float)(corrections_.apha)) / division; + corrections_.elevations[1] = ((float)(corrections_.beta)) / division; + corrections_.elevations[2] = ((float)(corrections_.gamma)) / division; + printf("apha:%f, beta:%f, gamma:%f\n", corrections_.elevations[0], corrections_.elevations[1], corrections_.elevations[2]); + for (int i = 0; i < channel_num; i++) { + corrections_.azimuths[i + 3] = ((float)(corrections_.raw_azimuths[i])) / division; + corrections_.elevations[i + 3] = ((float)(corrections_.raw_elevations[i])) / division; + printf("%d %f %f \n",i, corrections_.azimuths[i + 3], corrections_.elevations[i + 3]); + } + + memcpy((void*)&corrections_.SHA_value, p, 32); + // successed + this->get_correction_file_ = true; + return 0; + } break; + default: + printf("min_version is wrong!\n"); + break; + } + } else { + return -1; + } + } catch (const std::exception &e) { + std::cerr << e.what() << '\n'; + return -1; + } + return -1; +} + + + + +// decode, udpPacket---->DecodePacket +template +int Udp2_5Parser::DecodePacket(LidarDecodedPacket &output, const UdpPacket& udpPacket) { + // verify buffer[0] and buffer[1] + if (udpPacket.buffer[0] != 0xEE || udpPacket.buffer[1] != 0xFF ) { + return -1; + } + + // point to header skip pre_header + const HS_LIDAR_HEADER_ET_V5* pHeader = + reinterpret_cast( + &(udpPacket.buffer[0]) + sizeof(HS_LIDAR_PRE_HEADER)); + + + // point to tails + const HS_LIDAR_TAIL_ET_V5 *pTail = + reinterpret_cast( + &(udpPacket.buffer[0]) + pHeader->GetPacketSize() - + sizeof(HS_LIDAR_CYBER_SECURITY_ET_V5) - + sizeof(HS_LIDAR_TAIL_CRC_ET_V5) - + sizeof(HS_LIDAR_TAIL_SEQ_NUM_ET_V5) - + sizeof(HS_LIDAR_TAIL_ET_V5) + ); + if (pHeader->HasSeqNum()){ + const HS_LIDAR_TAIL_SEQ_NUM_ET_V5 *pTailSeqNum = + reinterpret_cast( + &(udpPacket.buffer[0]) + pHeader->GetPacketSize() - + sizeof(HS_LIDAR_CYBER_SECURITY_ET_V5) - + sizeof(HS_LIDAR_TAIL_CRC_ET_V5) - + sizeof(HS_LIDAR_TAIL_SEQ_NUM_ET_V5) + ); + } + const HS_LIDAR_BODY_SEQ3_ET_V5* pSeq3 = + reinterpret_cast( + (const unsigned char *)pHeader + + sizeof(HS_LIDAR_HEADER_ET_V5) + 2 + ); + const HS_LIDAR_BODY_LASTER_UNIT_ET_V5* pUnit = + reinterpret_cast( + (const unsigned char *)pSeq3 + + sizeof(HS_LIDAR_BODY_SEQ3_ET_V5) + ); + + // write the value to output + output.host_timestamp = GetMicroTickCountU64(); + output.distance_unit = pHeader->GetDistUnit(); + output.sensor_timestamp = pTail->GetMicroLidarTimeU64(); + output.host_timestamp = GetMicroTickCountU64(); + output.points_num = pHeader->GetBlockNum() * pHeader->GetLaserNum(); + output.scan_complete = false; + output.block_num = pHeader->GetBlockNum(); + output.laser_num = pHeader->GetLaserNum(); + + int index_unit = 0; + int index_seq = 0; + for (int blockId = 0; blockId < pHeader->GetBlockNum(); blockId++) { + for (int seqId = 0; seqId < pHeader->GetSeqNum(); seqId++) { + int16_t horizontalAngle = pSeq3->GetHorizontalAngle(); + int16_t verticalAngle = pSeq3->GetVerticalAngle(); + uint8_t Confidence = pSeq3->GetConfidence(); + for (int unitId = 0; unitId < (pHeader->GetLaserNum()/pHeader->GetSeqNum()); unitId++){ + int16_t distance = pUnit->GetDistance(); + int8_t reflectivity = pUnit->GetReflectivity(); + output.reflectivities[index_unit] = reflectivity; + output.distances[index_unit] = distance; + output.azimuth[index_unit] = horizontalAngle/512.0f; + output.elevation[index_unit] = verticalAngle/512.0f; + index_unit++; + pUnit += 1; + } + // pSeq3 next + pSeq3 = reinterpret_cast( + (const unsigned char *)pSeq3 + + sizeof(HS_LIDAR_BODY_SEQ3_ET_V5) + + sizeof(HS_LIDAR_BODY_LASTER_UNIT_ET_V5)*(pHeader->GetLaserNum()/pHeader->GetSeqNum()) + ); + + // pUnit next + pUnit = reinterpret_cast( + (const unsigned char *)pSeq3 + + sizeof(HS_LIDAR_BODY_SEQ3_ET_V5) + ); + } + if (blockId < (pHeader->GetBlockNum())) { + // skip point id + pSeq3 = reinterpret_cast( + (const unsigned char *)pSeq3 + 2 + ); + pUnit = reinterpret_cast( + (const unsigned char *)pUnit + 2 + ); + } // end if (blockId < (pHeader->GetBlockNum())) + } + uint16_t frame_id = pTail->GetFrameID(); + if (this->use_angle_ && IsNeedFrameSplit(frame_id)) { + output.scan_complete = true; + } + this->last_frameid_ = pTail->GetFrameID(); + return 0; +} + +// Framing +template +bool Udp2_5Parser::IsNeedFrameSplit(uint16_t frame_id) { + if ( frame_id != this->last_frameid_ ) { + return true; + } + return false; +} + +// get elevations[i] +template +int16_t Udp2_5Parser::GetVecticalAngle(int channel) { + if (this->get_correction_file_ == false) { + printf ("GetVecticalAngle: no correction file get, Error\n"); + return -1; + } + return corrections_.elevations[channel]; +} + + +template +int Udp2_5Parser::ComputeXYZI(LidarDecodedFrame &frame, LidarDecodedPacket &packet) { + // get configer information from corrections_, 1 block 1 corrections_ + float division = (float)corrections_.angle_division; + float apha = corrections_.elevations[0]; + float beta = corrections_.elevations[1]; + float gamma = corrections_.elevations[2]; + // get the laser_num + uint16_t lasernum = packet.laser_num; + for (int blockId = 0; blockId < packet.block_num; blockId++) { + T_Point point; + float delte_apha = 0; + float delte_theta = 0; + for (int i = 0; i < lasernum; i++) { + int point_index = packet.packet_index * packet.points_num + blockId * packet.laser_num + i; + // get phi and psi and distance + float raw_azimuth = packet.azimuth[i + blockId*lasernum]; + float raw_elevation = packet.elevation[i + blockId*lasernum]; + float distance = packet.distances[i + blockId*lasernum] * packet.distance_unit; + float phi = corrections_.azimuths[i + 3]; + float theta = corrections_.elevations[i + 3]; + float an = apha + phi; + float theta_n = (raw_elevation + theta / std::cos(an * M_PI / 180)); + float elv_v = raw_elevation * M_PI / 180 + theta * M_PI / 180 - std::tan(raw_elevation * M_PI / 180) * (1 - std::cos(an * M_PI / 180)) ; + float delt_azi_v = std::sin(an * M_PI / 180) * std::cos(an * M_PI / 180) * theta_n * theta_n / 2 * 1.016 * M_PI / 180 * M_PI / 180; + float eta = phi + delt_azi_v * 180 / M_PI + beta + raw_azimuth / 2; + float delt_azi_h = std::sin(eta * M_PI / 180) * std::tan(2 * gamma * M_PI / 180) * std::tan(elv_v ) + std::sin(2 * eta * M_PI / 180) * gamma * gamma * M_PI / 180 * M_PI / 180; + float elv_h = elv_v * 180 / M_PI + std::cos(eta * M_PI / 180) * 2 * gamma ; + float azi_h = 90 + raw_azimuth + delt_azi_h * 180 / M_PI + delt_azi_v * 180 / M_PI + phi; + + + int azimuth = (int)(azi_h * 100 + CIRCLE) % CIRCLE; + int elevation = (int)(elv_h * 100 + CIRCLE) % CIRCLE; + float xyDistance = distance * this->cos_all_angle_[elevation]; + float x = xyDistance * this->sin_all_angle_[azimuth]; + float y = xyDistance * this->cos_all_angle_[azimuth]; + float z = distance * this->sin_all_angle_[elevation]; + setX(frame.points[point_index], x); + setY(frame.points[point_index], y); + setZ(frame.points[point_index], z); + setIntensity(frame.points[point_index], packet.reflectivities[blockId * packet.laser_num + i]); + setTimestamp(frame.points[point_index], double(packet.sensor_timestamp) / kMicrosecondToSecond); + setRing(frame.points[point_index], i); + } + } + frame.points_num += packet.points_num; + frame.packet_num = packet.packet_index; + return 0; +} \ No newline at end of file diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/src/udp3_1_parser.cc b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/src/udp3_1_parser.cc new file mode 100644 index 0000000..53f87e3 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/src/udp3_1_parser.cc @@ -0,0 +1,279 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ + +/* + * File: udp3_1_parser.cc + * Author: Zhang Yu + * Description: Implemente Udp3_1Parser class +*/ + +#include "udp3_1_parser.h" +#include "udp_protocol_v3_1.h" +using namespace hesai::lidar; +template +Udp3_1Parser::Udp3_1Parser() { + this->motor_speed_ = 0; + this->return_mode_ = 0; +} + +template +Udp3_1Parser::~Udp3_1Parser() { printf("release general parser\n"); } + +template +int Udp3_1Parser::ComputeXYZI(LidarDecodedFrame &frame, LidarDecodedPacket &packet) { + for (int blockid = 0; blockid < packet.block_num; blockid++) { + T_Point point; + int elevation = 0; + int azimuth = 0; + + for (int i = 0; i < packet.laser_num; i++) { + int point_index = packet.packet_index * packet.points_num + blockid * packet.laser_num + i; + float distance = packet.distances[blockid * packet.laser_num + i] * packet.distance_unit; + int Azimuth = packet.azimuth[blockid * packet.laser_num + i]; + if (this->get_correction_file_) { + elevation = packet.elevation[blockid * packet.laser_num + i]; + elevation = (CIRCLE + elevation) % CIRCLE; + azimuth = Azimuth; + azimuth = (CIRCLE + azimuth) % CIRCLE; + } + float xyDistance = distance * this->cos_all_angle_[(elevation)]; + float x = xyDistance * this->sin_all_angle_[(azimuth)]; + float y = xyDistance * this->cos_all_angle_[(azimuth)]; + float z = distance * this->sin_all_angle_[(elevation)]; + this->TransformPoint(x, y, z); + setX(frame.points[point_index], x); + setY(frame.points[point_index], y); + setZ(frame.points[point_index], z); + setIntensity(frame.points[point_index], packet.reflectivities[blockid * packet.laser_num + i]); + setTimestamp(frame.points[point_index], double(packet.sensor_timestamp) / kMicrosecondToSecond); + setRing(frame.points[point_index], i); + } + } + frame.points_num += packet.points_num; + frame.packet_num = packet.packet_index; + return 0; +} + +template +int Udp3_1Parser::DecodePacket(LidarDecodedPacket &output, const UdpPacket& udpPacket) { + if (udpPacket.buffer[0] != 0xEE || udpPacket.buffer[1] != 0xFF ) { + return -1; + } + const HS_LIDAR_HEADER_QT_V1 *pHeader = + reinterpret_cast( + &(udpPacket.buffer[0]) + sizeof(HS_LIDAR_PRE_HEADER)); + + const HS_LIDAR_TAIL_QT_V1 *pTail = + reinterpret_cast( + (const unsigned char *)pHeader + sizeof(HS_LIDAR_HEADER_QT_V1) + + (sizeof(HS_LIDAR_BODY_AZIMUTH_QT_V1) + + sizeof(HS_LIDAR_BODY_CHN_NNIT_QT_V1) * pHeader->GetLaserNum()) * + pHeader->GetBlockNum()); + if(this->enable_packet_loss_tool_ == true) { + this->current_seqnum_ = pTail->m_u32SeqNum; + if (this->current_seqnum_ > this->last_seqnum_ && this->last_seqnum_ != 0) { + this->total_packet_count_ += this->current_seqnum_ - this->last_seqnum_; + } + pTail->CalPktLoss(this->start_seqnum_, this->last_seqnum_, this->loss_count_, this->start_time_, this->total_loss_count_, this->total_start_seqnum_); + } + // pTail->CalPktLoss(this->start_seqnum_, this->last_seqnum_, this->loss_count_, this->start_time_); + output.sensor_timestamp = pTail->GetMicroLidarTimeU64(); + output.host_timestamp = GetMicroTickCountU64(); + if(this->enable_packet_loss_tool_ == true) return 0; + this->spin_speed_ = pTail->m_u16MotorSpeed; + this->is_dual_return_= pTail->IsDualReturn(); + output.spin_speed = pTail->m_u16MotorSpeed; + output.distance_unit = pHeader->GetDistUnit(); + + // 如下三条：max min这样的参数一点用都没有 + output.maxPoints = pHeader->GetBlockNum() * pHeader->GetLaserNum(); + // 不填直接崩调，=0界面一个点也没有 + output.points_num = pHeader->GetBlockNum() * pHeader->GetLaserNum(); + // 不填则仅显示很小一部分点云 + output.scan_complete = false; + + int index = 0; + float minAzimuth = 0; + float maxAzimuth = 0; + output.block_num = pHeader->GetBlockNum(); + output.laser_num = pHeader->GetLaserNum(); + + const HS_LIDAR_BODY_AZIMUTH_QT_V1 *pAzimuth = + reinterpret_cast( + (const unsigned char *)pHeader + sizeof(HS_LIDAR_HEADER_QT_V1)); + + const HS_LIDAR_BODY_CHN_NNIT_QT_V1 *pChnUnit = + reinterpret_cast( + (const unsigned char *)pAzimuth + + sizeof(HS_LIDAR_BODY_AZIMUTH_QT_V1)); + for (int blockid = 0; blockid < pHeader->GetBlockNum(); blockid++) { + uint16_t u16Azimuth = pAzimuth->GetAzimuth(); + output.azimuths = u16Azimuth; + pChnUnit = reinterpret_cast( + (const unsigned char *)pAzimuth + sizeof(HS_LIDAR_BODY_AZIMUTH_QT_V1)); + // point to next block azimuth addr + pAzimuth = reinterpret_cast( + (const unsigned char *)pAzimuth + sizeof(HS_LIDAR_BODY_AZIMUTH_QT_V1) + + sizeof(HS_LIDAR_BODY_CHN_NNIT_QT_V1) * pHeader->GetLaserNum()); + // point to next block fine azimuth addr + auto elevation = 0; + for (int i = 0; i < pHeader->GetLaserNum(); i++) { + double elevationCorr = this->elevation_correction_[i]; + double azimuthCorr = u16Azimuth / kResolutionFloat + this->azimuth_collection_[i]; + double distance = static_cast(pChnUnit->GetDistance()) * pHeader->GetDistUnit(); + if (this->enable_distance_correction_) { + GetDistanceCorrection(azimuthCorr, elevationCorr, distance); + } + if (this->enable_firetime_correction_ && this->get_firetime_file_) { + azimuthCorr += this->GetFiretimesCorrection(i, this->spin_speed_) * kResolutionInt; + } + output.reflectivities[index] = pChnUnit->GetReflectivity(); + output.distances[index] = pChnUnit->GetDistance(); + output.azimuth[index] = azimuthCorr * kResolutionFloat; + output.elevation[index] = elevationCorr * kResolutionFloat; + pChnUnit = pChnUnit + 1; + index++; + } + if (IsNeedFrameSplit(u16Azimuth)) { + output.scan_complete = true; + } + this->last_azimuth_ = u16Azimuth; + } + return 0; +} + +template +bool Udp3_1Parser::IsNeedFrameSplit(uint16_t azimuth) { + if (this->last_azimuth_ > azimuth && (this->last_azimuth_- azimuth > kSplitFrameMinAngle)) { + return true; + } + return false; +} + +template +void Udp3_1Parser::GetDistanceCorrection(double &azi, double &ele, + double &distance) { + int aziIndex = int(azi * kResolutionFloat); + int eleIndex = int(ele * kResolutionFloat); + if (aziIndex >= CIRCLE) aziIndex -= CIRCLE; + if (aziIndex < 0) aziIndex += CIRCLE; + if (eleIndex >= CIRCLE) eleIndex -= CIRCLE; + if (eleIndex < 0) eleIndex += CIRCLE; + float point_x, point_y, point_z; + if (distance > 0.1) { + if (this->sin_all_angle_[eleIndex] != 0) { + // printf("%d %f\n", eleIndex, this->sin_all_angle_[eleIndex]); + float c = (HS_LIDAR_QT_COORDINATE_CORRECTION_ODOG * + HS_LIDAR_QT_COORDINATE_CORRECTION_ODOG + + HS_LIDAR_QT_COORDINATE_CORRECTION_OGOT * + HS_LIDAR_QT_COORDINATE_CORRECTION_OGOT - + distance * distance) * + this->sin_all_angle_[eleIndex] * this->sin_all_angle_[eleIndex]; + float b = 2 * this->sin_all_angle_[eleIndex] * this->cos_all_angle_[eleIndex] * + (HS_LIDAR_QT_COORDINATE_CORRECTION_ODOG * + this->cos_all_angle_[aziIndex] - + HS_LIDAR_QT_COORDINATE_CORRECTION_OGOT * + this->sin_all_angle_[aziIndex]); + point_z = (-b + sqrt(b * b - 4 * c)) / 2; + point_x = point_z * this->sin_all_angle_[aziIndex] * this->cos_all_angle_[eleIndex] / + this->sin_all_angle_[eleIndex] - + HS_LIDAR_QT_COORDINATE_CORRECTION_OGOT; + point_y = point_z * this->cos_all_angle_[aziIndex] * this->cos_all_angle_[eleIndex] / + this->sin_all_angle_[eleIndex] + + HS_LIDAR_QT_COORDINATE_CORRECTION_ODOG; + if (((point_x + HS_LIDAR_QT_COORDINATE_CORRECTION_OGOT) * + this->cos_all_angle_[eleIndex] * this->sin_all_angle_[aziIndex] + + (point_y - HS_LIDAR_QT_COORDINATE_CORRECTION_ODOG) * + this->cos_all_angle_[eleIndex] * this->cos_all_angle_[aziIndex] + + point_z * this->sin_all_angle_[eleIndex]) <= 0) { + point_z = (-b - sqrt(b * b - 4 * c)) / 2; + point_x = point_z * this->sin_all_angle_[aziIndex] * + this->cos_all_angle_[eleIndex] / this->sin_all_angle_[eleIndex] - + HS_LIDAR_QT_COORDINATE_CORRECTION_OGOT; + point_y = point_z * this->cos_all_angle_[aziIndex] * + this->cos_all_angle_[eleIndex] / this->sin_all_angle_[eleIndex] + + HS_LIDAR_QT_COORDINATE_CORRECTION_ODOG; + } + } else if (this->cos_all_angle_[aziIndex] != 0) { + float tan_azimuth = this->sin_all_angle_[aziIndex] / this->cos_all_angle_[aziIndex]; + float c = (HS_LIDAR_QT_COORDINATE_CORRECTION_ODOG * tan_azimuth + + HS_LIDAR_QT_COORDINATE_CORRECTION_OGOT) * + (HS_LIDAR_QT_COORDINATE_CORRECTION_ODOG * tan_azimuth + + HS_LIDAR_QT_COORDINATE_CORRECTION_OGOT) - + distance * distance; + float a = 1 + tan_azimuth * tan_azimuth; + float b = -2 * tan_azimuth * + (HS_LIDAR_QT_COORDINATE_CORRECTION_ODOG * tan_azimuth + + HS_LIDAR_QT_COORDINATE_CORRECTION_OGOT); + point_z = 0; + point_y = (-b + sqrt(b * b - 4 * a * c)) / (2 * a); + point_x = + (point_y - HS_LIDAR_QT_COORDINATE_CORRECTION_ODOG) * tan_azimuth - + HS_LIDAR_QT_COORDINATE_CORRECTION_OGOT; + if (((point_x + HS_LIDAR_QT_COORDINATE_CORRECTION_OGOT) * + this->cos_all_angle_[eleIndex] * this->sin_all_angle_[aziIndex] + + (point_y - HS_LIDAR_QT_COORDINATE_CORRECTION_ODOG) * + this->cos_all_angle_[eleIndex] * this->cos_all_angle_[aziIndex] + + point_z * this->sin_all_angle_[eleIndex]) <= 0) { + point_z = 0; + point_y = (-b - sqrt(b * b - 4 * a * c)) / (2 * a); + point_x = (point_y - HS_LIDAR_QT_COORDINATE_CORRECTION_ODOG) * + tan_azimuth - + HS_LIDAR_QT_COORDINATE_CORRECTION_OGOT; + } + } else { + point_x = sqrt(distance * distance - + HS_LIDAR_QT_COORDINATE_CORRECTION_ODOG * + HS_LIDAR_QT_COORDINATE_CORRECTION_ODOG); + point_y = HS_LIDAR_QT_COORDINATE_CORRECTION_ODOG; + point_z = 0; + if (((point_x + HS_LIDAR_QT_COORDINATE_CORRECTION_OGOT) * + this->cos_all_angle_[eleIndex] * this->sin_all_angle_[aziIndex] + + (point_y - HS_LIDAR_QT_COORDINATE_CORRECTION_ODOG) * + this->cos_all_angle_[eleIndex] * this->cos_all_angle_[aziIndex] + + point_z * this->sin_all_angle_[eleIndex]) <= 0) { + point_x = -sqrt(distance * distance - + HS_LIDAR_QT_COORDINATE_CORRECTION_ODOG * + HS_LIDAR_QT_COORDINATE_CORRECTION_ODOG); + point_y = HS_LIDAR_QT_COORDINATE_CORRECTION_ODOG; + point_z = 0; + } + } + azi = atan2(point_x, point_y) / M_PI * (kHalfCircleInt / kResolutionInt); + azi = azi < 0 ? azi + (kCircle / kResolutionFloat) : azi; + ele = atan2(point_z, sqrt(point_x * point_x + point_y * point_y)) / M_PI * + 180; + ele = ele < 0 ? ele + (kCircle / kResolutionFloat) : ele; + distance = sqrt(point_x * point_x + point_y * point_y + point_z * point_z); + } +} + + + + + diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/src/udp3_2_parser.cc b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/src/udp3_2_parser.cc new file mode 100644 index 0000000..2c0b6e4 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/src/udp3_2_parser.cc @@ -0,0 +1,527 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ + +/* + * File: udp3_2_parser.cc + * Author: Zhang Yu + * Description: Implemente Udp3_2Parser class +*/ + +#include "udp3_2_parser.h" +using namespace hesai::lidar; +template +Udp3_2Parser::Udp3_2Parser() { + this->motor_speed_ = 0; + this->return_mode_ = 0; +} + +template +Udp3_2Parser::~Udp3_2Parser() { printf("release general parser\n"); } + +template +int Udp3_2Parser::LoadFiretimesString(char *firetimes_string) { + // printf("%s\n",firetimes_string); + std::string firetimes_content_str = firetimes_string; + std::istringstream fin(firetimes_content_str); + std::string line; + // first line sequence,chn id,firetime/us + if (std::getline(fin, line)) { + printf("Parse Lidar firetime now...\n"); + } + std::vector firstLine; + boost::split(firstLine, line, boost::is_any_of(",")); + if (firstLine[0] == "EEFF" || firstLine[0] == "eeff") { + std::array, + HS_LIDAR_QT128_LOOP_NUM> + firetimes; + firetimes[0].fill(0); + firetimes[1].fill(0); + firetimes[2].fill(0); + firetimes[3].fill(0); + std::getline(fin, line); + std::vector loopNumLine; + boost::split(loopNumLine, line, boost::is_any_of(",")); + int loopNum = atoi(loopNumLine[3].c_str()); + std::getline(fin, line); + for (int i = 0; i < HS_LIDAR_QT128_LASER_NUM; i++) { + std::getline(fin, line); + std::vector ChannelLine; + boost::split(ChannelLine, line, boost::is_any_of(",")); + for (int j = 0; j < loopNum; j++) { + if (ChannelLine.size() == loopNum * 2) { + int laserId = atoi(ChannelLine[j * 2].c_str()) - 1; + if (laserId >= 0) + firetimes[j][laserId] = std::stof(ChannelLine[j * 2 + 1].c_str()); + } else { + std::cout << "loop num is not equal to the first channel line\n"; + return -1; + } + } + } + qt128_firetime_ = firetimes; + } else { + std::cout << "firetime file delimiter is wrong \n"; + return -1; + } + return 0; +} +template +void Udp3_2Parser::LoadFiretimesFile(std::string firetimes_path) { + int ret = 0; + std::ifstream fin(firetimes_path); + if (fin.is_open()) { + int length = 0; + fin.seekg(0, std::ios::end); + length = fin.tellg(); + fin.seekg(0, std::ios::beg); + char *buffer = new char[length]; + fin.read(buffer, length); + fin.close(); + ret = LoadFiretimesString(buffer); + if (ret != 0) { + std::cout << "Parse local firetimes file Error\n"; + } + } else { + std::cout << "Open firetimes file failed\n"; + return; + } +} +template +int Udp3_2Parser::LoadChannelConfigString(char *channel_config_content) { + std::string channel_config_content_str = channel_config_content; + pandarQT_channel_config_.m_bIsChannelConfigObtained = false; + std::istringstream ifs(channel_config_content_str); + std::string line; + + std::getline(ifs, line); + std::vector versionLine; + boost::split(versionLine, line, boost::is_any_of(",")); + if (versionLine[0] == "EEFF" || versionLine[0] == "eeff") { + pandarQT_channel_config_.major_version = + std::stoi(versionLine[1].c_str()); + pandarQT_channel_config_.min_version = std::stoi(versionLine[2].c_str()); + } else { + std::cout << "channel config file delimiter is wrong\n"; + return -1; + } + std::getline(ifs, line); + std::vector channelNumLine; + boost::split(channelNumLine, line, boost::is_any_of(",")); + pandarQT_channel_config_.laser_num = std::stoi(channelNumLine[1].c_str()); + pandarQT_channel_config_.m_u8BlockNum = std::stoi(channelNumLine[3].c_str()); + if (pandarQT_channel_config_.laser_num <= 0 || + pandarQT_channel_config_.m_u8BlockNum <= 0) { + std::cout << "LaserNum:" << pandarQT_channel_config_.laser_num + << " BlockNum:" << pandarQT_channel_config_.m_u8BlockNum << std::endl; + return -1; + } + std::getline(ifs, line); + std::vector firstChannelLine; + boost::split(firstChannelLine, line, boost::is_any_of(",")); + int loop_num = firstChannelLine.size(); + pandarQT_channel_config_.m_vChannelConfigTable.resize(loop_num); + + for (int i = 0; i < loop_num; i++) { + pandarQT_channel_config_.m_vChannelConfigTable[i].resize( + pandarQT_channel_config_.laser_num); + } + for (int i = 0; i < pandarQT_channel_config_.laser_num; i++) { + std::getline(ifs, line); + std::vector ChannelLine; + boost::split(ChannelLine, line, boost::is_any_of(",")); + for (int j = 0; j < loop_num; j++) { + if (ChannelLine.size() == loop_num) { + pandarQT_channel_config_.m_vChannelConfigTable[j][i] = + std::stoi(ChannelLine[j].c_str()); + } else { + std::cout << "loop num is not equal to the first channel line" << std::endl; + return -1; + } + } + } + std::getline(ifs, line); + pandarQT_channel_config_.m_sHashValue = line; + pandarQT_channel_config_.m_bIsChannelConfigObtained = true; + return 0; +} +template +void Udp3_2Parser::LoadChannelConfigFile(std::string channel_config_path) { + int ret = 0; + std::ifstream fin(channel_config_path); + if (fin.is_open()) { + int length = 0; + fin.seekg(0, std::ios::end); + length = fin.tellg(); + fin.seekg(0, std::ios::beg); + char *buffer = new char[length]; + fin.read(buffer, length); + fin.close(); + ret = LoadChannelConfigString(buffer); + if (ret != 0) { + std::cout << "Parse local channel congfig file Error" << std::endl; + } + } else { + std::cout << "Open channel congfig file failed" << std::endl; + return; + } +} +template +double Udp3_2Parser::GetFiretimesCorrection(int laserId, double speed, + int loopIndex) { + return qt128_firetime_[loopIndex][laserId] * speed * 6E-6; +} +template +void Udp3_2Parser::GetDistanceCorrection(double &azi, double &ele, + double &distance) { + int aziIndex = int(azi * kResolutionFloat); + int eleIndex = int(ele * kResolutionFloat); + if (aziIndex >= CIRCLE) aziIndex -= CIRCLE; + if (aziIndex < 0) aziIndex += CIRCLE; + if (eleIndex >= CIRCLE) eleIndex -= CIRCLE; + if (eleIndex < 0) eleIndex += CIRCLE; + float point_x, point_y, point_z; + if (distance > 0.1) { + if (0) { + float c = (HS_LIDAR_QT128_COORDINATE_CORRECTION_ODOG * + HS_LIDAR_QT128_COORDINATE_CORRECTION_ODOG + + HS_LIDAR_QT128_COORDINATE_CORRECTION_OGOT * + HS_LIDAR_QT128_COORDINATE_CORRECTION_OGOT - + distance * distance) * + this->sin_all_angle_[eleIndex] * this->sin_all_angle_[eleIndex]; + float b = 2 * this->sin_all_angle_[eleIndex] * this->cos_all_angle_[eleIndex] * + (HS_LIDAR_QT128_COORDINATE_CORRECTION_ODOG * + this->cos_all_angle_[aziIndex] - + HS_LIDAR_QT128_COORDINATE_CORRECTION_OGOT * + this->sin_all_angle_[aziIndex]); + point_z = (-b + sqrt(b * b - 4 * c)) / 2; + point_x = point_z * this->sin_all_angle_[aziIndex] * this->cos_all_angle_[eleIndex] / + this->sin_all_angle_[eleIndex] - + HS_LIDAR_QT128_COORDINATE_CORRECTION_OGOT; + point_y = point_z * this->cos_all_angle_[aziIndex] * this->cos_all_angle_[eleIndex] / + this->sin_all_angle_[eleIndex] + + HS_LIDAR_QT128_COORDINATE_CORRECTION_ODOG; + if (((point_x + HS_LIDAR_QT128_COORDINATE_CORRECTION_OGOT) * + this->cos_all_angle_[eleIndex] * this->sin_all_angle_[aziIndex] + + (point_y - HS_LIDAR_QT128_COORDINATE_CORRECTION_ODOG) * + this->cos_all_angle_[eleIndex] * this->cos_all_angle_[aziIndex] + + point_z * this->sin_all_angle_[eleIndex]) <= 0) { + point_z = (-b - sqrt(b * b - 4 * c)) / 2; + point_x = point_z * this->sin_all_angle_[aziIndex] * + this->cos_all_angle_[eleIndex] / this->sin_all_angle_[eleIndex] - + HS_LIDAR_QT128_COORDINATE_CORRECTION_OGOT; + point_y = point_z * this->cos_all_angle_[aziIndex] * + this->cos_all_angle_[eleIndex] / this->sin_all_angle_[eleIndex] + + HS_LIDAR_QT128_COORDINATE_CORRECTION_ODOG; + } + } else if (0) { + float tan_azimuth = this->sin_all_angle_[aziIndex] / this->cos_all_angle_[aziIndex]; + float c = (HS_LIDAR_QT128_COORDINATE_CORRECTION_ODOG * tan_azimuth + + HS_LIDAR_QT128_COORDINATE_CORRECTION_OGOT) * + (HS_LIDAR_QT128_COORDINATE_CORRECTION_ODOG * tan_azimuth + + HS_LIDAR_QT128_COORDINATE_CORRECTION_OGOT) - + distance * distance; + float a = 1 + tan_azimuth * tan_azimuth; + float b = -2 * tan_azimuth * + (HS_LIDAR_QT128_COORDINATE_CORRECTION_ODOG * tan_azimuth + + HS_LIDAR_QT128_COORDINATE_CORRECTION_OGOT); + point_z = 0; + point_y = (-b + sqrt(b * b - 4 * a * c)) / (2 * a); + point_x = + (point_y - HS_LIDAR_QT128_COORDINATE_CORRECTION_ODOG) * tan_azimuth - + HS_LIDAR_QT128_COORDINATE_CORRECTION_OGOT; + if (((point_x + HS_LIDAR_QT128_COORDINATE_CORRECTION_OGOT) * + this->cos_all_angle_[eleIndex] * this->sin_all_angle_[aziIndex] + + (point_y - HS_LIDAR_QT128_COORDINATE_CORRECTION_ODOG) * + this->cos_all_angle_[eleIndex] * this->cos_all_angle_[aziIndex] + + point_z * this->sin_all_angle_[eleIndex]) <= 0) { + point_z = 0; + point_y = (-b - sqrt(b * b - 4 * a * c)) / (2 * a); + point_x = (point_y - HS_LIDAR_QT128_COORDINATE_CORRECTION_ODOG) * + tan_azimuth - + HS_LIDAR_QT128_COORDINATE_CORRECTION_OGOT; + } + } else { + point_x = sqrt(distance * distance - + HS_LIDAR_QT128_COORDINATE_CORRECTION_ODOG * + HS_LIDAR_QT128_COORDINATE_CORRECTION_ODOG); + point_y = HS_LIDAR_QT128_COORDINATE_CORRECTION_ODOG; + point_z = 0; + if (((point_x + HS_LIDAR_QT128_COORDINATE_CORRECTION_OGOT) * + this->cos_all_angle_[eleIndex] * this->sin_all_angle_[aziIndex] + + (point_y - HS_LIDAR_QT128_COORDINATE_CORRECTION_ODOG) * + this->cos_all_angle_[eleIndex] * this->cos_all_angle_[aziIndex] + + point_z * this->sin_all_angle_[eleIndex]) <= 0) { + + point_x = -sqrt(distance * distance - + HS_LIDAR_QT128_COORDINATE_CORRECTION_ODOG * + HS_LIDAR_QT128_COORDINATE_CORRECTION_ODOG); + point_y = HS_LIDAR_QT128_COORDINATE_CORRECTION_ODOG; + point_z = 0; + } + } + azi = atan2(point_x, point_y) / M_PI * (kHalfCircleInt / kResolutionInt); + azi = azi < 0 ? azi + (kCircle / kResolutionFloat) : azi; + ele = atan2(point_z, sqrt(point_x * point_x + point_y * point_y)) / M_PI * + (kHalfCircleInt / kResolutionInt); + ele = ele < 0 ? ele + (kCircle / kResolutionFloat) : ele; + distance = sqrt(point_x * point_x + point_y * point_y + point_z * point_z); + } +} +template +int Udp3_2Parser::ComputeXYZI(LidarDecodedFrame &frame, LidarDecodedPacket &packet) { + for (int blockid = 0; blockid < packet.block_num; blockid++) { + T_Point point; + int elevation = 0; + int azimuth = 0; + + for (int i = 0; i < packet.laser_num; i++) { + int point_index = packet.packet_index * packet.points_num + blockid * packet.laser_num + i; + float distance = packet.distances[blockid * packet.laser_num + i] * packet.distance_unit; + if (this->get_correction_file_) { + elevation = packet.elevation[blockid * packet.laser_num + i]; + elevation = (CIRCLE + elevation) % CIRCLE; + azimuth = packet.azimuth[blockid * packet.laser_num + i]; + azimuth = (CIRCLE + azimuth) % CIRCLE; + } + float xyDistance = distance * this->cos_all_angle_[(elevation)]; + float x = xyDistance * this->sin_all_angle_[(azimuth)]; + float y = xyDistance * this->cos_all_angle_[(azimuth)]; + float z = distance * this->sin_all_angle_[(elevation)]; + this->TransformPoint(x, y, z); + setX(frame.points[point_index], x); + setY(frame.points[point_index], y); + setZ(frame.points[point_index], z); + setIntensity(frame.points[point_index], packet.reflectivities[blockid * packet.laser_num + i]); + setTimestamp(frame.points[point_index], double(packet.sensor_timestamp) / kMicrosecondToSecond); + setRing(frame.points[point_index], i); + } + } + frame.points_num += packet.points_num; + frame.packet_num = packet.packet_index; + return 0; +} +template +int Udp3_2Parser::DecodePacket(LidarDecodedPacket &output, const UdpPacket& udpPacket) { + if (udpPacket.buffer[0] != 0xEE || udpPacket.buffer[1] != 0xFF) return -1; + const HS_LIDAR_HEADER_QT_V2 *pHeader = + reinterpret_cast( + &(udpPacket.buffer[0]) + sizeof(HS_LIDAR_PRE_HEADER)); + + output.host_timestamp = GetMicroTickCountU64(); + // 如下三条：max min这样的参数一点用都没有 + output.maxPoints = pHeader->GetBlockNum() * pHeader->GetLaserNum(); + // 不填直接崩调，=0界面一个点也没有 + output.points_num = pHeader->GetBlockNum() * pHeader->GetLaserNum(); + // 不填则仅显示很小一部分点云 + output.scan_complete = false; + // 不填可以播放，只是显示的时间戳不对 + int index = 0; + float minAzimuth = 0; + float maxAzimuth = 0; + output.block_num = pHeader->GetBlockNum(); + output.laser_num = pHeader->GetLaserNum(); + output.distance_unit = pHeader->GetDistUnit(); + + // point to azimuth of udp start block + if (pHeader->HasConfidenceLevel()) { + const HS_LIDAR_BODY_AZIMUTH_QT_V2 *pAzimuth = + reinterpret_cast( + (const unsigned char *)pHeader + sizeof(HS_LIDAR_HEADER_QT_V2)); + + const HS_LIDAR_BODY_CHN_UNIT_QT_V2 *pChnUnit = + reinterpret_cast( + (const unsigned char *)pAzimuth + + sizeof(HS_LIDAR_BODY_AZIMUTH_QT_V2)); + + const HS_LIDAR_TAIL_QT_V2 *pTail = + reinterpret_cast( + (const unsigned char *)pHeader + sizeof(HS_LIDAR_HEADER_QT_V2) + + (sizeof(HS_LIDAR_BODY_AZIMUTH_QT_V2) + + sizeof(HS_LIDAR_BODY_CHN_UNIT_QT_V2) * pHeader->GetLaserNum()) * + pHeader->GetBlockNum() + + sizeof(HS_LIDAR_BODY_CRC_QT_V2) + + (pHeader->HasFunctionSafety() ? sizeof(HS_LIDAR_FUNCTION_SAFETY) + : 0)); + if (pHeader->HasSeqNum()) { + const HS_LIDAR_TAIL_SEQ_NUM_QT_V2 *pTailSeqNum = + reinterpret_cast( + (const unsigned char *)pHeader + sizeof(HS_LIDAR_HEADER_QT_V2) + + (sizeof(HS_LIDAR_BODY_AZIMUTH_QT_V2) + + sizeof(HS_LIDAR_BODY_CHN_UNIT_QT_V2) * pHeader->GetLaserNum()) * + pHeader->GetBlockNum() + + sizeof(HS_LIDAR_BODY_CRC_QT_V2) + + (pHeader->HasFunctionSafety() ? sizeof(HS_LIDAR_FUNCTION_SAFETY) + : 0) + + sizeof(HS_LIDAR_TAIL_QT_V2)); + if(this->enable_packet_loss_tool_ == true) { + this->current_seqnum_ = pTailSeqNum->m_u32SeqNum; + if (this->current_seqnum_ > this->last_seqnum_ && this->last_seqnum_ != 0) { + this->total_packet_count_ += this->current_seqnum_ - this->last_seqnum_; + } + pTailSeqNum->CalPktLoss(this->start_seqnum_, this->last_seqnum_, this->loss_count_, this->start_time_, this->total_loss_count_, this->total_start_seqnum_); + } + } + output.sensor_timestamp = pTail->GetMicroLidarTimeU64(); + output.host_timestamp = GetMicroTickCountU64(); + if(this->enable_packet_loss_tool_ == true) return 0; + + this->spin_speed_ = pTail->m_u16MotorSpeed; + for (int i = 0; i < pHeader->GetBlockNum(); i++) { + // point to channel unit addr + uint16_t u16Azimuth = pAzimuth->GetAzimuth(); + auto elevation = 0; + pChnUnit = reinterpret_cast( + (const unsigned char *)pAzimuth + + sizeof(HS_LIDAR_BODY_AZIMUTH_QT_V2)); + + // point to next block azimuth addr + pAzimuth = reinterpret_cast( + (const unsigned char *)pAzimuth + + sizeof(HS_LIDAR_BODY_AZIMUTH_QT_V2) + + sizeof(HS_LIDAR_BODY_CHN_UNIT_QT_V2) * pHeader->GetLaserNum()); + int loopIndex = (pTail->GetModeFlag() + + (i / ((pTail->GetReturnMode() < 0x39) ? 1 : 2)) + 1) % 2; + for (int j = 0; j < pHeader->GetLaserNum(); j++) { + int laserId = + (pHeader->HasSelfDefine() && + pandarQT_channel_config_.m_bIsChannelConfigObtained && + i < pandarQT_channel_config_.m_vChannelConfigTable[loopIndex] + .size()) + ? pandarQT_channel_config_ + .m_vChannelConfigTable[loopIndex][j] - + 1 + : j; + double elevationCorr = this->elevation_correction_[laserId]; + double azimuthCorr = u16Azimuth / 100.0f + this->azimuth_collection_[laserId]; + uint16_t u16Distance = pChnUnit->GetDistance(); + uint8_t u8Intensity = pChnUnit->GetReflectivity(); + uint8_t u8Confidence = 0; + double distance = + static_cast(pChnUnit->GetDistance()) * pHeader->GetDistUnit(); + if (this->enable_distance_correction_) { + this->GetDistanceCorrection(azimuthCorr, elevationCorr, distance); + } + if (this->enable_firetime_correction_) { + azimuthCorr += GetFiretimesCorrection( + laserId, pTail->GetMotorSpeed(), loopIndex); + } + output.distances[index] = pChnUnit->GetDistance(); + output.reflectivities[index] = pChnUnit->GetReflectivity(); + output.azimuth[index] = azimuthCorr * 100; + output.elevation[index] = elevationCorr * 100; + pChnUnit = pChnUnit + 1; + index++; + } + if (IsNeedFrameSplit(u16Azimuth)) { + output.scan_complete = true; + } + this->last_azimuth_ = u16Azimuth; + } + + } else { + const HS_LIDAR_BODY_AZIMUTH_QT_V2 *pAzimuth = + reinterpret_cast( + (const unsigned char *)pHeader + sizeof(HS_LIDAR_HEADER_QT_V2)); + + const HS_LIDAR_BODY_CHN_UNIT_NO_CONF_QT_V2 *pChnUnit = + reinterpret_cast( + (const unsigned char *)pAzimuth + + sizeof(HS_LIDAR_BODY_AZIMUTH_QT_V2)); + + const HS_LIDAR_TAIL_QT_V2 *pTail = + reinterpret_cast( + (const unsigned char *)pHeader + sizeof(HS_LIDAR_HEADER_QT_V2) + + (sizeof(HS_LIDAR_BODY_AZIMUTH_QT_V2) + + sizeof(HS_LIDAR_BODY_CHN_UNIT_NO_CONF_QT_V2) * + pHeader->GetLaserNum()) * + pHeader->GetBlockNum() + + sizeof(HS_LIDAR_BODY_CRC_QT_V2) + + pHeader->HasFunctionSafety() + ? sizeof(HS_LIDAR_FUNCTION_SAFETY) + : 0); + output.sensor_timestamp = pTail->GetMicroLidarTimeU64(); + this->spin_speed_ = pTail->m_u16MotorSpeed; + for (int i = 0; i < pHeader->GetBlockNum(); i++) { + // point to channel unit addr + pChnUnit = reinterpret_cast( + (const unsigned char *)pAzimuth + + sizeof(HS_LIDAR_BODY_AZIMUTH_QT_V2)); + uint16_t u16Azimuth = pAzimuth->GetAzimuth(); + // point to next block azimuth addr + pAzimuth = reinterpret_cast( + (const unsigned char *)pAzimuth + + sizeof(HS_LIDAR_BODY_AZIMUTH_QT_V2) + + sizeof(HS_LIDAR_BODY_CHN_UNIT_NO_CONF_QT_V2) * + pHeader->GetLaserNum()); + int loopIndex = (pTail->GetModeFlag() + + (i / ((pTail->GetReturnMode() < 0x39) ? 1 : 2)) + 1) % 2; + for (int j = 0; j < pHeader->GetLaserNum(); j++) { + int laserId = + (pHeader->HasSelfDefine() && + pandarQT_channel_config_.m_bIsChannelConfigObtained && + i < pandarQT_channel_config_.m_vChannelConfigTable[loopIndex] + .size()) + ? pandarQT_channel_config_ + .m_vChannelConfigTable[loopIndex][j] - + 1 + : j; + double elevationCorr = this->elevation_correction_[laserId]; + double azimuthCorr = u16Azimuth / kResolutionFloat + this->azimuth_collection_[laserId]; + double distance = + static_cast(pChnUnit->GetDistance()) * pHeader->GetDistUnit(); + if (this->enable_distance_correction_) { + GetDistanceCorrection(azimuthCorr, elevationCorr, distance); + } + if (this->enable_firetime_correction_) { + azimuthCorr += GetFiretimesCorrection( + laserId, pTail->GetMotorSpeed(), loopIndex); + } + output.distances[index] = pChnUnit->GetDistance(); + output.reflectivities[index] = pChnUnit->GetReflectivity(); + output.azimuth[index] = azimuthCorr * kResolutionInt; + output.elevation[index] = elevationCorr * kResolutionInt; + pChnUnit = pChnUnit + 1; + index++; + } + if (IsNeedFrameSplit(u16Azimuth)) { + output.scan_complete = true; + } + this->last_azimuth_ = u16Azimuth; + } + + } + return 0; +} + +template +bool Udp3_2Parser::IsNeedFrameSplit(uint16_t azimuth) { + if (this->last_azimuth_ > azimuth && (this->last_azimuth_- azimuth > kSplitFrameMinAngle)) { + return true; + } + return false; +} \ No newline at end of file diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/src/udp4_3_parser.cc b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/src/udp4_3_parser.cc new file mode 100644 index 0000000..9d1e84e --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/src/udp4_3_parser.cc @@ -0,0 +1,496 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ + +/* + * File: udp4_3_parser.cc + * Author: Zhang Yu + * Description: Implemente Udp4_3Parser class +*/ + +#include "udp4_3_parser.h" +#include "udp_protocol_v4_3.h" +#include "udp_protocol_header.h" +#include "udp_parser.h" + +using namespace hesai::lidar; +template +Udp4_3Parser::Udp4_3Parser() { + view_mode_ = 1; + this->get_correction_file_ = false; +} + +template +void Udp4_3Parser::HandlePacketData(uint8_t *u8Buf, uint16_t u16Len) { +} + +template +void Udp4_3Parser::LoadCorrectionFile(std::string lidar_correction_file) { + int ret = 0; + printf("load correction file from local correction.csv now!\n"); + std::ifstream fin(lidar_correction_file); + if (fin.is_open()) { + printf("Open correction file success\n"); + int length = 0; + std::string str_lidar_calibration; + fin.seekg(0, std::ios::end); + length = fin.tellg(); + fin.seekg(0, std::ios::beg); + char *buffer = new char[length]; + fin.read(buffer, length); + fin.close(); + str_lidar_calibration = buffer; + ret = LoadCorrectionString(buffer); + if (ret != 0) { + printf("Parse local Correction file Error\n"); + } else { + printf("Parse local Correction file Success!!!\n"); + } + } else { + printf("Open correction file failed\n"); + return; + } +} + +template +int Udp4_3Parser::LoadCorrectionString(char *data) { + try { + char *p = data; + PandarATCorrectionsHeader header = *(PandarATCorrectionsHeader *)p; + if (0xee == header.delimiter[0] && 0xff == header.delimiter[1]) { + switch (header.version[1]) { + case 3: { + m_PandarAT_corrections.header = header; + auto frame_num = m_PandarAT_corrections.header.frame_number; + auto channel_num = m_PandarAT_corrections.header.channel_number; + p += sizeof(PandarATCorrectionsHeader); + memcpy((void *)&m_PandarAT_corrections.start_frame, p, + sizeof(uint16_t) * frame_num); + p += sizeof(uint16_t) * frame_num; + memcpy((void *)&m_PandarAT_corrections.end_frame, p, + sizeof(uint16_t) * frame_num); + p += sizeof(uint16_t) * frame_num; + // printf("frame_num: %d\n", frame_num); + // printf("start_frame, end_frame: \n"); + for (int i = 0; i < frame_num; ++i) + // printf("%lf, %lf\n", + // m_PandarAT_corrections.start_frame[i] / kResolutionFloat, + // m_PandarAT_corrections.end_frame[i] / kResolutionFloat); + memcpy((void *)&m_PandarAT_corrections.azimuth, p, + sizeof(int16_t) * channel_num); + p += sizeof(int16_t) * channel_num; + memcpy((void *)&m_PandarAT_corrections.elevation, p, + sizeof(int16_t) * channel_num); + p += sizeof(int16_t) * channel_num; + memcpy((void *)&m_PandarAT_corrections.azimuth_offset, p, + sizeof(int8_t) * CIRCLE_ANGLE); + p += sizeof(int8_t) * CIRCLE_ANGLE; + memcpy((void *)&m_PandarAT_corrections.elevation_offset, p, + sizeof(int8_t) * CIRCLE_ANGLE); + p += sizeof(int8_t) * CIRCLE_ANGLE; + memcpy((void *)&m_PandarAT_corrections.SHA256, p, + sizeof(uint8_t) * 32); + p += sizeof(uint8_t) * 32; + this->get_correction_file_ = true; + return 0; + } break; + case 5: { + m_PandarAT_corrections.header = header; + auto frame_num = m_PandarAT_corrections.header.frame_number; + auto channel_num = m_PandarAT_corrections.header.channel_number; + p += sizeof(PandarATCorrectionsHeader); + memcpy((void *)&m_PandarAT_corrections.l.start_frame, p, + sizeof(uint32_t) * frame_num); + p += sizeof(uint32_t) * frame_num; + memcpy((void *)&m_PandarAT_corrections.l.end_frame, p, + sizeof(uint32_t) * frame_num); + p += sizeof(uint32_t) * frame_num; + memcpy((void *)&m_PandarAT_corrections.l.azimuth, p, + sizeof(int32_t) * channel_num); + p += sizeof(int32_t) * channel_num; + memcpy((void *)&m_PandarAT_corrections.l.elevation, p, + sizeof(int32_t) * channel_num); + p += sizeof(int32_t) * channel_num; + auto adjust_length = channel_num * CORRECTION_AZIMUTH_NUM; + memcpy((void *)&m_PandarAT_corrections.azimuth_offset, p, + sizeof(int8_t) * adjust_length); + p += sizeof(int8_t) * adjust_length; + memcpy((void *)&m_PandarAT_corrections.elevation_offset, p, + sizeof(int8_t) * adjust_length); + p += sizeof(int8_t) * adjust_length; + memcpy((void *)&m_PandarAT_corrections.SHA256, p, + sizeof(uint8_t) * 32); + p += sizeof(uint8_t) * 32; + // printf("frame_num: %d\n", frame_num); + // printf("start_frame, end_frame: \n"); + for (int i = 0; i < frame_num; ++i) { + m_PandarAT_corrections.l.start_frame[i] = + m_PandarAT_corrections.l.start_frame[i] * + m_PandarAT_corrections.header.resolution; + m_PandarAT_corrections.l.end_frame[i] = + m_PandarAT_corrections.l.end_frame[i] * + m_PandarAT_corrections.header.resolution; + // printf("%lf, %lf\n", + // m_PandarAT_corrections.l.start_frame[i] / AZIMUTH_UNIT, + // m_PandarAT_corrections.l.end_frame[i] / AZIMUTH_UNIT); + } + for (int i = 0; i < AT128_LASER_NUM; i++) { + m_PandarAT_corrections.l.azimuth[i] = + m_PandarAT_corrections.l.azimuth[i] * + m_PandarAT_corrections.header.resolution; + m_PandarAT_corrections.l.elevation[i] = + m_PandarAT_corrections.l.elevation[i] * + m_PandarAT_corrections.header.resolution; + } + for (int i = 0; i < adjust_length; i++) { + m_PandarAT_corrections.azimuth_offset[i] = + m_PandarAT_corrections.azimuth_offset[i] * + m_PandarAT_corrections.header.resolution; + m_PandarAT_corrections.elevation_offset[i] = + m_PandarAT_corrections.elevation_offset[i] * + m_PandarAT_corrections.header.resolution; + } + + this->get_correction_file_ = true; + return 0; + } break; + default: + break; + } + } + + return -1; + } catch (const std::exception &e) { + std::cerr << e.what() << '\n'; + return -1; + } + + return -1; +} + +template +Udp4_3Parser::~Udp4_3Parser() { printf("release Udp4_3Parser\n"); } + +template +int Udp4_3Parser::DecodePacket(LidarDecodedPacket &output, const UdpPacket& udpPacket){ + if(udpPacket.is_timeout == true) { + for(int i = 0; i < 3; i++) { + int angle = m_PandarAT_corrections.l.start_frame[i] / ANGULAR_RESOLUTION + MARGINAL_ANGLE - this->last_azimuth_; + if (abs(angle) < ACCEPTANCE_ANGLE) { + this->use_angle_ = false; + output.scan_complete = true; + output.points_num = 0; + output.block_num = 0; + output.laser_num = 0; + return 0; + } + } + } + + if (udpPacket.buffer[0] != 0xEE || udpPacket.buffer[1] != 0xFF ) { + output.scan_complete = false; + output.points_num = 0; + output.block_num = 0; + output.laser_num = 0; + return -1; + } + const HS_LIDAR_HEADER_ST_V3 *pHeader = + reinterpret_cast( + &(udpPacket.buffer[0]) + sizeof(HS_LIDAR_PRE_HEADER)); + + const HS_LIDAR_TAIL_ST_V3 *pTail = + reinterpret_cast( + (const unsigned char *)pHeader + sizeof(HS_LIDAR_HEADER_ST_V3) + + (sizeof(HS_LIDAR_BODY_AZIMUTH_ST_V3) + + sizeof(HS_LIDAR_BODY_FINE_AZIMUTH_ST_V3) + + sizeof(HS_LIDAR_BODY_CHN_NNIT_ST_V3) * pHeader->GetLaserNum()) * + pHeader->GetBlockNum() + + sizeof(HS_LIDAR_BODY_CRC_ST_V3)); + if (pHeader->HasSeqNum()) { + const HS_LIDAR_TAIL_SEQ_NUM_ST_V3 *pTailSeqNum = + reinterpret_cast( + (const unsigned char *)pHeader + sizeof(HS_LIDAR_HEADER_ST_V3) + + (sizeof(HS_LIDAR_BODY_AZIMUTH_ST_V3) + + sizeof(HS_LIDAR_BODY_FINE_AZIMUTH_ST_V3) + + sizeof(HS_LIDAR_BODY_CHN_NNIT_ST_V3) * pHeader->GetLaserNum()) * + pHeader->GetBlockNum() + + sizeof(HS_LIDAR_BODY_CRC_ST_V3) + sizeof(HS_LIDAR_TAIL_ST_V3)); + if(this->enable_packet_loss_tool_ == true) { + this->current_seqnum_ = pTailSeqNum->m_u32SeqNum; + if (this->current_seqnum_ > this->last_seqnum_ && this->last_seqnum_ != 0) { + this->total_packet_count_ += this->current_seqnum_ - this->last_seqnum_; + } + pTailSeqNum->CalPktLoss(this->start_seqnum_, this->last_seqnum_, this->loss_count_, + this->start_time_, this->total_loss_count_, this->total_start_seqnum_); + } + } + output.host_timestamp = GetMicroTickCountU64(); + output.sensor_timestamp = pTail->GetMicroLidarTimeU64(); + if(this->enable_packet_loss_tool_ == true) return 0; + + this->spin_speed_ = pTail->GetMotorSpeed(); + this->is_dual_return_= pTail->IsDualReturn(); + output.spin_speed = pTail->m_i16MotorSpeed; + output.points_num = pHeader->GetBlockNum() * pHeader->GetLaserNum(); + output.scan_complete = false; + + output.distance_unit = pHeader->GetDistUnit(); + int index = 0; + output.block_num = pHeader->GetBlockNum(); + output.laser_num = pHeader->GetLaserNum(); + const HS_LIDAR_BODY_AZIMUTH_ST_V3 *pAzimuth = + reinterpret_cast( + (const unsigned char *)pHeader + sizeof(HS_LIDAR_HEADER_ST_V3)); + const HS_LIDAR_BODY_FINE_AZIMUTH_ST_V3 *pFineAzimuth = + reinterpret_cast( + (const unsigned char *)pAzimuth + + sizeof(HS_LIDAR_BODY_AZIMUTH_ST_V3)); + + const HS_LIDAR_BODY_CHN_NNIT_ST_V3 *pChnUnit = + reinterpret_cast( + (const unsigned char *)pAzimuth + + sizeof(HS_LIDAR_BODY_FINE_AZIMUTH_ST_V3) + + sizeof(HS_LIDAR_BODY_AZIMUTH_ST_V3)); + for (int blockid = 0; blockid < pHeader->GetBlockNum(); blockid++) { + uint16_t u16Azimuth = pAzimuth->GetAzimuth(); + uint8_t u8FineAzimuth = pFineAzimuth->GetFineAzimuth(); + pChnUnit = reinterpret_cast( + (const unsigned char *)pAzimuth + sizeof(HS_LIDAR_BODY_AZIMUTH_ST_V3) + + sizeof(HS_LIDAR_BODY_FINE_AZIMUTH_ST_V3)); + + // point to next block azimuth addr + pAzimuth = reinterpret_cast( + (const unsigned char *)pAzimuth + sizeof(HS_LIDAR_BODY_AZIMUTH_ST_V3) + + sizeof(HS_LIDAR_BODY_FINE_AZIMUTH_ST_V3) + + sizeof(HS_LIDAR_BODY_CHN_NNIT_ST_V3) * pHeader->GetLaserNum()); + output.azimuths = u16Azimuth; + // point to next block fine azimuth addr + pFineAzimuth = reinterpret_cast( + (const unsigned char *)pAzimuth + sizeof(HS_LIDAR_BODY_AZIMUTH_ST_V3)); + + int azimuth = u16Azimuth * ANGULAR_RESOLUTION + u8FineAzimuth; + auto elevation = 0; + int field = 0; + for (int i = 0; i < pHeader->GetLaserNum(); i++) { + if (this->get_firetime_file_) { + output.azimuth[index] = azimuth + this->GetFiretimesCorrection(i, this->spin_speed_) * RESOLUTION * 2; + }else { + output.azimuth[index] = azimuth; + } + output.reflectivities[index] = pChnUnit->GetReflectivity(); + output.distances[index] = pChnUnit->GetDistance(); + output.elevation[index] = elevation; + pChnUnit = pChnUnit + 1; + index++; + } + + if (this->use_angle_ && IsNeedFrameSplit(u16Azimuth, field)) { + // 未置true会一直显示loading，几秒后超时退出崩溃 + output.scan_complete = true; + } + this->last_azimuth_ = u16Azimuth; + } + return 0; +} + +template +int Udp4_3Parser::DecodePacket(LidarDecodedFrame &frame, const UdpPacket& udpPacket){ + if(udpPacket.is_timeout == true) { + for(int i = 0; i < 3; i++) { + int angle = m_PandarAT_corrections.l.start_frame[i] / ANGULAR_RESOLUTION + MARGINAL_ANGLE - this->last_azimuth_; + if (abs(angle) < ACCEPTANCE_ANGLE) { + this->use_angle_ = false; + frame.scan_complete = true; + frame.laser_num = 0; + frame.block_num = 0; + return 0; + } + } + } + + if (udpPacket.buffer[0] != 0xEE || udpPacket.buffer[1] != 0xFF ) { + printf("Udp4_3Parser: DecodePacket, invalid packet %x %x\n", udpPacket.buffer[0], udpPacket.buffer[1]); + return -1; + } + const HS_LIDAR_HEADER_ST_V3 *pHeader = + reinterpret_cast( + &(udpPacket.buffer[0]) + sizeof(HS_LIDAR_PRE_HEADER)); + + const HS_LIDAR_TAIL_ST_V3 *pTail = + reinterpret_cast( + (const unsigned char *)pHeader + sizeof(HS_LIDAR_HEADER_ST_V3) + + (sizeof(HS_LIDAR_BODY_AZIMUTH_ST_V3) + + sizeof(HS_LIDAR_BODY_FINE_AZIMUTH_ST_V3) + + sizeof(HS_LIDAR_BODY_CHN_NNIT_ST_V3) * pHeader->GetLaserNum()) * + pHeader->GetBlockNum() + + sizeof(HS_LIDAR_BODY_CRC_ST_V3)); + if (pHeader->HasSeqNum()) { + const HS_LIDAR_TAIL_SEQ_NUM_ST_V3 *pTailSeqNum = + reinterpret_cast( + (const unsigned char *)pHeader + sizeof(HS_LIDAR_HEADER_ST_V3) + + (sizeof(HS_LIDAR_BODY_AZIMUTH_ST_V3) + + sizeof(HS_LIDAR_BODY_FINE_AZIMUTH_ST_V3) + + sizeof(HS_LIDAR_BODY_CHN_NNIT_ST_V3) * pHeader->GetLaserNum()) * + pHeader->GetBlockNum() + + sizeof(HS_LIDAR_BODY_CRC_ST_V3) + sizeof(HS_LIDAR_TAIL_ST_V3)); + // pTailSeqNum->CalPktLoss(this->start_seqnum_, this->last_seqnum_, this->loss_count_, this->start_time_); + } + this->spin_speed_ = pTail->m_i16MotorSpeed; + this->is_dual_return_= pTail->IsDualReturn(); + frame.laser_num = pHeader->GetBlockNum(); + frame.block_num = pHeader->GetLaserNum(); + frame.points_num += pHeader->GetBlockNum() * pHeader->GetLaserNum(); + // 不填则仅显示很小一部分点云 + frame.scan_complete = false; + + int index = frame.packet_index * pHeader->GetBlockNum() * pHeader->GetLaserNum(); + float minAzimuth = 0; + float maxAzimuth = 0; + const HS_LIDAR_BODY_AZIMUTH_ST_V3 *pAzimuth = + reinterpret_cast( + (const unsigned char *)pHeader + sizeof(HS_LIDAR_HEADER_ST_V3)); + const HS_LIDAR_BODY_FINE_AZIMUTH_ST_V3 *pFineAzimuth = + reinterpret_cast( + (const unsigned char *)pAzimuth + + sizeof(HS_LIDAR_BODY_AZIMUTH_ST_V3)); + + const HS_LIDAR_BODY_CHN_NNIT_ST_V3 *pChnUnit = + reinterpret_cast( + (const unsigned char *)pAzimuth + + sizeof(HS_LIDAR_BODY_FINE_AZIMUTH_ST_V3) + + sizeof(HS_LIDAR_BODY_AZIMUTH_ST_V3)); + for (int blockid = 0; blockid < pHeader->GetBlockNum(); blockid++) { + uint16_t u16Azimuth = pAzimuth->GetAzimuth(); + uint8_t u8FineAzimuth = pFineAzimuth->GetFineAzimuth(); + pChnUnit = reinterpret_cast( + (const unsigned char *)pAzimuth + sizeof(HS_LIDAR_BODY_AZIMUTH_ST_V3) + + sizeof(HS_LIDAR_BODY_FINE_AZIMUTH_ST_V3)); + + // point to next block azimuth addr + pAzimuth = reinterpret_cast( + (const unsigned char *)pAzimuth + sizeof(HS_LIDAR_BODY_AZIMUTH_ST_V3) + + sizeof(HS_LIDAR_BODY_FINE_AZIMUTH_ST_V3) + + sizeof(HS_LIDAR_BODY_CHN_NNIT_ST_V3) * pHeader->GetLaserNum()); + // point to next block fine azimuth addr + pFineAzimuth = reinterpret_cast( + (const unsigned char *)pAzimuth + sizeof(HS_LIDAR_BODY_AZIMUTH_ST_V3)); + + int azimuth = u16Azimuth * ANGULAR_RESOLUTION + u8FineAzimuth; + auto elevation = 0; + int field = 0; + for (int i = 0; i < pHeader->GetLaserNum(); i++) { + frame.distances[index] = pChnUnit->GetDistance(); + frame.reflectivities[index] = pChnUnit->GetReflectivity(); + frame.azimuth[index] = azimuth; + frame.elevation[index] = elevation; + frame.azimuths[frame.packet_index] = u16Azimuth; + pChnUnit = pChnUnit + 1; + index++; + } + if (this->use_angle_ && IsNeedFrameSplit(u16Azimuth, field)) { + frame.scan_complete = true; + } + this->last_azimuth_ = u16Azimuth; + if(blockid == 0 ) minAzimuth = azimuth; + else maxAzimuth = azimuth; + + } + frame.packet_index++; + return 0; +} + +template +int Udp4_3Parser::ComputeXYZI(LidarDecodedFrame &frame, LidarDecodedPacket &packet){ + for (int blockid = 0; blockid < packet.block_num; blockid++) { + T_Point point; + int Azimuth = packet.azimuth[blockid * packet.laser_num]; + int count = 0, field = 0; + if ( this->get_correction_file_) { + while (count < m_PandarAT_corrections.header.frame_number && + (((Azimuth + MAX_AZI_LEN - m_PandarAT_corrections.l.start_frame[field]) % MAX_AZI_LEN + + (m_PandarAT_corrections.l.end_frame[field] + MAX_AZI_LEN - Azimuth) % MAX_AZI_LEN) != + (m_PandarAT_corrections.l.end_frame[field] + MAX_AZI_LEN - + m_PandarAT_corrections.l.start_frame[field]) % MAX_AZI_LEN)) { + field = (field + 1) % m_PandarAT_corrections.header.frame_number; + count++; + } + if (count >= m_PandarAT_corrections.header.frame_number) continue; + } + + auto elevation = 0; + int azimuth = 0; + + for (int i = 0; i < packet.laser_num; i++) { + int point_index = packet.packet_index * packet.points_num + blockid * packet.laser_num + i; + float distance = packet.distances[blockid * packet.laser_num + i] * packet.distance_unit; + Azimuth = packet.azimuth[blockid * packet.laser_num + i]; + if (this->get_correction_file_) { + elevation = (m_PandarAT_corrections.l.elevation[i] + + m_PandarAT_corrections.GetElevationAdjustV3(i, Azimuth) * + FINE_AZIMUTH_UNIT ); + elevation = (MAX_AZI_LEN + elevation) % MAX_AZI_LEN; + azimuth = ((Azimuth + MAX_AZI_LEN - m_PandarAT_corrections.l.start_frame[field]) * 2 - + m_PandarAT_corrections.l.azimuth[i] + + m_PandarAT_corrections.GetAzimuthAdjustV3(i, Azimuth) * FINE_AZIMUTH_UNIT); + azimuth = (MAX_AZI_LEN + azimuth) % MAX_AZI_LEN; + } + float xyDistance = distance * m_PandarAT_corrections.cos_map[(elevation)]; + float x = xyDistance * m_PandarAT_corrections.sin_map[(azimuth)]; + float y = xyDistance * m_PandarAT_corrections.cos_map[(azimuth)]; + float z = distance * m_PandarAT_corrections.sin_map[(elevation)]; + this->TransformPoint(x, y, z); + setX(frame.points[point_index], x); + setY(frame.points[point_index], y); + setZ(frame.points[point_index], z); + setIntensity(frame.points[point_index], packet.reflectivities[blockid * packet.laser_num + i]); + setTimestamp(frame.points[point_index], double(packet.sensor_timestamp) / kMicrosecondToSecond); + setRing(frame.points[point_index], i); + } + } + frame.points_num += packet.points_num; + frame.packet_num = packet.packet_index; + return 0; +} + +template +int16_t Udp4_3Parser::GetVecticalAngle(int channel) { + if (this->get_correction_file_ == false) { + printf ("GetVecticalAngle: no correction file get, Error"); + return -1; + } + + return m_PandarAT_corrections.elevation[channel]; +} + +template +bool Udp4_3Parser::IsNeedFrameSplit(uint16_t azimuth, int field) { + if (abs(azimuth - this->last_azimuth_) > GeneralParser::kAzimuthTolerance && + this->last_azimuth_ != 0 ) { + return true; + } + return false; +} diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/src/udp6_1_parser.cc b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/src/udp6_1_parser.cc new file mode 100644 index 0000000..5445842 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/src/udp6_1_parser.cc @@ -0,0 +1,225 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ + +/* + * File: udp6_1_parser.cc + * Author: Zhang Yu + * Description: Implemente Udp6_1Parser class +*/ + +#include "udp6_1_parser.h" +#include "udp_protocol_v6_1.h" +#include "udp_protocol_header.h" +using namespace hesai::lidar; +template +Udp6_1Parser::Udp6_1Parser() { + this->motor_speed_ = 0; + this->return_mode_ = 0; + block_num_ = 6; +} +template +Udp6_1Parser::~Udp6_1Parser() { printf("release general parser\n"); } + +template +int Udp6_1Parser::ComputeXYZI(LidarDecodedFrame &frame, LidarDecodedPacket &packet) { + for (int blockid = 0; blockid < packet.block_num; blockid++) { + T_Point point; + int elevation = 0; + int azimuth = 0; + + for (int i = 0; i < packet.laser_num; i++) { + int point_index = packet.packet_index * packet.points_num + blockid * packet.laser_num + i; + float distance = packet.distances[blockid * packet.laser_num + i] * packet.distance_unit; + int Azimuth = packet.azimuth[blockid * packet.laser_num + i]; + if (this->get_correction_file_) { + elevation = this->elevation_correction_[i] * kResolutionInt; + elevation = (CIRCLE + elevation) % CIRCLE; + azimuth = Azimuth + this->azimuth_collection_[i] * kResolutionInt; + azimuth = (CIRCLE + azimuth) % CIRCLE; + } + float x = 0; + float y = 0; + float z = 0; + + if (this->enable_distance_correction_) { + GetDistanceCorrection(Azimuth, this->azimuth_collection_[i] * kResolutionInt, elevation , distance, x, y, z); + } + else { + float xyDistance = distance * this->cos_all_angle_[(elevation)]; + x = xyDistance * this->sin_all_angle_[(azimuth)]; + y = xyDistance * this->cos_all_angle_[(azimuth)]; + z = distance * this->sin_all_angle_[(elevation)]; + } + + this->TransformPoint(x, y, z); + setX(frame.points[point_index], x); + setY(frame.points[point_index], y); + setZ(frame.points[point_index], z); + setIntensity(frame.points[point_index], packet.reflectivities[blockid * packet.laser_num + i]); + setTimestamp(frame.points[point_index], double(packet.sensor_timestamp) / kMicrosecondToSecond); + setRing(frame.points[point_index], i); + } + } + frame.points_num += packet.points_num; + frame.packet_num = packet.packet_index; + return 0; +} + +template +int Udp6_1Parser::DecodePacket(LidarDecodedPacket &output, const UdpPacket& udpPacket) { + if (udpPacket.buffer[0] != 0xEE || udpPacket.buffer[1] != 0xFF ) { + return -1; + } + const HsLidarXTV1Header *pHeader = + reinterpret_cast( + &(udpPacket.buffer[0]) + sizeof(HS_LIDAR_PRE_HEADER)); + + const HsLidarXTV1Tail *pTail = + reinterpret_cast( + (const unsigned char *)pHeader + sizeof(HsLidarXTV1Header) + + (sizeof(HsLidarXTV1BodyAzimuth) + + sizeof(HsLidarXTV1BodyChannelData) * pHeader->GetLaserNum()) * + pHeader->GetBlockNum()); + + output.sensor_timestamp = pTail->GetMicroLidarTimeU64(); + output.host_timestamp = GetMicroTickCountU64(); + + if (this->enable_packet_loss_tool_ == true) { + this->current_seqnum_ = pTail->m_u32SeqNum; + if (this->current_seqnum_ > this->last_seqnum_ && this->last_seqnum_ != 0) { + this->total_packet_count_ += this->current_seqnum_ - this->last_seqnum_; + } + pTail->CalPktLoss(this->start_seqnum_, this->last_seqnum_, this->loss_count_, + this->start_time_, this->total_loss_count_, this->total_start_seqnum_); + return 0; + } + + this->spin_speed_ = pTail->m_u16MotorSpeed; + this->is_dual_return_= pTail->IsDualReturn(); + output.spin_speed = pTail->m_u16MotorSpeed; + + // 如下三条：max min这样的参数一点用都没有 + output.maxPoints = pHeader->GetBlockNum() * pHeader->GetLaserNum(); + // 不填直接崩调，=0界面一个点也没有 + output.points_num = pHeader->GetBlockNum() * pHeader->GetLaserNum(); + // 不填则仅显示很小一部分点云 + output.scan_complete = false; + output.distance_unit = pHeader->GetDistUnit(); + int index = 0; + float minAzimuth = 0; + float maxAzimuth = 0; + output.block_num = pHeader->GetBlockNum(); + output.laser_num = pHeader->GetLaserNum(); + + const HsLidarXTV1BodyAzimuth *pAzimuth = + reinterpret_cast( + (const unsigned char *)pHeader + sizeof(HsLidarXTV1Header)); + + const HsLidarXTV1BodyChannelData *pChnUnit = + reinterpret_cast( + (const unsigned char *)pAzimuth + + sizeof(HsLidarXTV1BodyAzimuth)); + for (int blockid = 0; blockid < pHeader->GetBlockNum(); blockid++) { + uint16_t u16Azimuth = pAzimuth->GetAzimuth(); + pChnUnit = reinterpret_cast( + (const unsigned char *)pAzimuth + sizeof(HsLidarXTV1BodyAzimuth)); + // point to next block azimuth addr + pAzimuth = reinterpret_cast( + (const unsigned char *)pAzimuth + sizeof(HsLidarXTV1BodyAzimuth) + + sizeof(HsLidarXTV1BodyChannelData) * pHeader->GetLaserNum()); + // point to next block fine azimuth addr + auto elevation = 0; + for (int i = 0; i < pHeader->GetLaserNum(); i++) { + if (this->get_firetime_file_) { + output.azimuth[index] = u16Azimuth + this->GetFiretimesCorrection(i, this->spin_speed_) * kResolutionInt; + }else { + output.azimuth[index] = u16Azimuth; + } + output.distances[index] = pChnUnit->GetDistance() ; + output.reflectivities[index] = pChnUnit->GetReflectivity(); + output.elevation[index] = elevation; + pChnUnit = pChnUnit + 1; + index++; + } + if (IsNeedFrameSplit(u16Azimuth)) { + output.scan_complete = true; + } + this->last_azimuth_ = u16Azimuth; + + } + return 0; +} + +template +bool Udp6_1Parser::IsNeedFrameSplit(uint16_t azimuth) { + if (this->last_azimuth_ > azimuth && (this->last_azimuth_- azimuth > kSplitFrameMinAngle)) { + return true; + } + return false; +} + +template +void Udp6_1Parser::GetDistanceCorrection(int const& aziOrigin, + int const& aziDelt, + int const& elevation, + float const& distance, + float& x, + float& y, + float& z + ) { + int aziCal = (aziOrigin + aziDelt) % CIRCLE; + if(distance <= 0.1) { + float xyDistance = distance * this->cos_all_angle_[(elevation)]; + float x = xyDistance * this->sin_all_angle_[(aziCal)]; + float y = xyDistance * this->cos_all_angle_[(aziCal)]; + float z = distance * this->sin_all_angle_[(elevation)]; + return; + } + switch (block_num_) { + case 6: // XTM + distance_correction_b_ = 0.0130; + distance_correction_h_ = 0.0305; + break; + case 8: // XT + distance_correction_b_ = 0.0130; + distance_correction_h_ = 0.0315; + break; + default: + std::cout << __func__ << "default: never occur" << block_num_ << std::endl; + break; + } + int aziCorrection = aziDelt % CIRCLE; + float calDistance = distance + - this->cos_all_angle_[elevation] * (distance_correction_h_ * this->cos_all_angle_[aziCorrection] - distance_correction_b_ * this->sin_all_angle_[aziCorrection]); + x = calDistance * this->cos_all_angle_[elevation] * this->sin_all_angle_[aziCal] + - distance_correction_b_ * this->cos_all_angle_[aziOrigin] + distance_correction_h_ * this->sin_all_angle_[aziOrigin]; + y = calDistance * this->cos_all_angle_[elevation] * this->cos_all_angle_[aziCal] + + distance_correction_b_ * this->sin_all_angle_[aziOrigin] + distance_correction_h_ * this->cos_all_angle_[aziOrigin]; + z = calDistance * this->sin_all_angle_[elevation]; +} + diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/src/udp7_2_parser.cc b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/src/udp7_2_parser.cc new file mode 100644 index 0000000..c09602d --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/src/udp7_2_parser.cc @@ -0,0 +1,298 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ + +/* + * File: udp7_2_parser.cc + * Author: Zhang Yu + * Description: Implemente Udp7_2Parser class +*/ + +#include "udp7_2_parser.h" +#include "udp_protocol_v7_2.h" +#include "udp_protocol_header.h" +using namespace hesai::lidar; +template +Udp7_2Parser::Udp7_2Parser() { + this->motor_speed_ = 0; + this->return_mode_ = 0; + this->last_cloumn_id_ = -1; +} +template +Udp7_2Parser::~Udp7_2Parser() { printf("release Udp7_2Parser \n"); } + + +template +void Udp7_2Parser::LoadCorrectionFile(std::string correction_path) { + int ret = 0; + printf("load correction file from local correction.csv now!\n"); + std::ifstream fin(correction_path); + if (fin.is_open()) { + printf("Open correction file success\n"); + int length = 0; + std::string str_lidar_calibration; + fin.seekg(0, std::ios::end); + length = fin.tellg(); + fin.seekg(0, std::ios::beg); + char *buffer = new char[length]; + fin.read(buffer, length); + fin.close(); + str_lidar_calibration = buffer; + ret = LoadCorrectionString(buffer); + if (ret != 0) { + printf("Parse local Correction file Error\n"); + } else { + + printf("Parse local Correction file Success!!!\n"); + } + } else { + printf("Open correction file failed\n"); + return; + } +} + +template +int Udp7_2Parser::LoadCorrectionString(char *data) { + if (LoadCorrectionDatData(data) == 0) { + return 0; + } + return LoadCorrectionCsvData(data); +} + +template +int Udp7_2Parser::LoadCorrectionCsvData(char *correction_string) { + std::istringstream ifs(correction_string); + std::string line; + // first line "Laser id,Elevation,Azimuth" + if(std::getline(ifs, line)) { + printf("Parse Lidar Correction...\n"); + } + int lineCounter = 0; + std::vector firstLine; + boost::split(firstLine, line, boost::is_any_of(",")); + while (std::getline(ifs, line)) { + if(line.length() < strlen("1,1,1,1")) { + return -1; + } + else { + lineCounter++; + } + float elev, azimuth; + int lineId = 0; + int cloumnId = 0; + std::stringstream ss(line); + std::string subline; + std::getline(ss, subline, ','); + std::stringstream(subline) >> lineId; + std::getline(ss, subline, ','); + std::stringstream(subline) >> cloumnId; + std::getline(ss, subline, ','); + std::stringstream(subline) >> elev; + std::getline(ss, subline, ','); + std::stringstream(subline) >> azimuth; + corrections_.elevations[lineId - 1][cloumnId - 1] = elev * kResolutionInt; + corrections_.azimuths[lineId - 1][cloumnId - 1] = azimuth * kResolutionInt; + } + this->get_correction_file_ = true; + return 0; +} + +template +int Udp7_2Parser::LoadCorrectionDatData(char *correction_string) { + + try { + char *p = correction_string; + PandarFTCorrectionsHeader header = *(PandarFTCorrectionsHeader *)p; + if (0xee == header.pilot[0] && 0xff == header.pilot[1]) { + switch (header.version[1]) { + case 0: { + int column_num = header.column_number; + int channel_num = header.channel_number; + int resolution = header.resolution; + float fResolution = float(resolution); + int angleNum = column_num * channel_num; + int doubleAngleNum = angleNum * 2; + int16_t* angles = new int16_t[doubleAngleNum]{0}; + int readLen = sizeof(int16_t) * doubleAngleNum; + memcpy((void*)angles, correction_string, readLen); + int hashLen = 32; + uint8_t* hashValue = new uint8_t[hashLen]; + memcpy((void*)hashValue, correction_string + readLen, hashLen); + for (int row = 0; row < column_num; row++) { + for (int col = 0; col < channel_num; col++) { + int idx = row * channel_num + col; + corrections_.azimuths[col][row] = angles[idx] * fResolution; + } + } + + for (int row = 0; row < column_num; row++) { + for (int col = 0; col < channel_num; col++) { + int idx = angleNum + row * channel_num + col; + corrections_.elevations[col][row] = angles[idx] * fResolution; + } + } + this->get_correction_file_ = true; + return 0; + } break; + case 1: { + int column_num = header.column_number; + int channel_num = header.channel_number; + int resolution = header.resolution; + float fResolution = float(resolution); + int angleNum = column_num * channel_num; + int doubleAngleNum = angleNum * 2; + int32_t* angles = new int32_t[doubleAngleNum]{0}; + int readLen = sizeof(int32_t) * doubleAngleNum; + memcpy((void*)angles, correction_string + sizeof(PandarFTCorrectionsHeader), readLen); + int hashLen = 32; + uint8_t* hashValue = new uint8_t[hashLen]; + memcpy((void*)hashValue, correction_string + readLen + sizeof(PandarFTCorrectionsHeader), hashLen); + for (int row = 0; row < column_num; row++) { + for (int col = 0; col < channel_num; col++) { + int idx = row * channel_num + col; + corrections_.azimuths[col][row] = angles[idx] * fResolution; + } + } + + for (int row = 0; row < column_num; row++) { + for (int col = 0; col < channel_num; col++) { + int idx = angleNum + row * channel_num + col; + corrections_.elevations[col][row] = angles[idx] * fResolution; + } + } + this->get_correction_file_ = true; + return 0; + } break; + default: + break; + } + } + + return -1; + } catch (const std::exception &e) { + std::cerr << e.what() << '\n'; + return -1; + } + + return -1; + +} + +template +int Udp7_2Parser::ComputeXYZI(LidarDecodedFrame &frame, LidarDecodedPacket &packet) { + T_Point point; + for (int i = 0; i < packet.laser_num; i++) { + int point_index = packet.packet_index * packet.points_num + i; + float distance = packet.distances[i] * packet.distance_unit; + int azimuth = 0; + int elevation = 0; + if (this->get_correction_file_) { + azimuth = packet.azimuth[i]; + elevation = packet.elevation[i]; + elevation = (CIRCLE + elevation) % CIRCLE; + azimuth = (CIRCLE + azimuth) % CIRCLE; + } + float xyDistance = distance * this->cos_all_angle_[(elevation)]; + float x = xyDistance * this->sin_all_angle_[(azimuth)]; + float y = xyDistance * this->cos_all_angle_[(azimuth)]; + float z = distance * this->sin_all_angle_[(elevation)]; + this->TransformPoint(x, y, z); + setX(frame.points[point_index], x); + setY(frame.points[point_index], y); + setZ(frame.points[point_index], z); + setIntensity(frame.points[point_index], packet.reflectivities[i]); + setTimestamp(frame.points[point_index], double(packet.sensor_timestamp) / kMicrosecondToSecond); + setRing(frame.points[point_index], i); + } + frame.points_num += packet.points_num; + frame.packet_num = packet.packet_index; + return 0; +} + +template +int Udp7_2Parser::DecodePacket(LidarDecodedPacket &output, const UdpPacket& udpPacket) { + if (udpPacket.buffer[0] != 0xEE || udpPacket.buffer[1] != 0xFF ) { + return -1; + } + const HS_LIDAR_HEADER_FT_V2 *pHeader = + reinterpret_cast( + &(udpPacket.buffer[0]) + sizeof(HS_LIDAR_PRE_HEADER)); + + const HS_LIDAR_TAIL_FT_V2 *pTail = + reinterpret_cast( + (const unsigned char *)pHeader + sizeof(HS_LIDAR_HEADER_FT_V2) + + (sizeof(HS_LIDAR_BODY_CHN_UNIT_FT_V2) * pHeader->GetChannelNum())); + output.sensor_timestamp = pTail->GetMicroLidarTimeU64(); + output.host_timestamp = GetMicroTickCountU64(); + + if (this->enable_packet_loss_tool_ == true) { + this->current_seqnum_ = pTail->sequence_num; + if (this->current_seqnum_ > this->last_seqnum_ && this->last_seqnum_ != 0) { + this->total_packet_count_ += this->current_seqnum_ - this->last_seqnum_; + } + pTail->CalPktLoss(this->start_seqnum_, this->last_seqnum_, this->loss_count_, + this->start_time_, this->total_loss_count_, this->total_start_seqnum_); + return 0; + } + + output.maxPoints = pHeader->GetChannelNum(); + output.points_num = pHeader->GetChannelNum(); + output.scan_complete = false; + output.distance_unit = pHeader->GetDistUnit(); + int index = 0; + float minAzimuth = 0; + float maxAzimuth = 0; + output.block_num = 1; + output.laser_num = pHeader->GetChannelNum(); + + const HS_LIDAR_BODY_CHN_UNIT_FT_V2 *pChnUnit = + reinterpret_cast( + (const unsigned char *)pHeader + sizeof(HS_LIDAR_HEADER_FT_V2)); + for (int blockid = 0; blockid < 1; blockid++) { + for (int i = 0; i < pHeader->GetChannelNum(); i++) { + output.azimuth[index] = corrections_.azimuths[i][pTail->column_id]; + output.elevation[index] = corrections_.elevations[i][pTail->column_id]; + output.reflectivities[index] = pChnUnit->GetReflectivity(); + output.distances[index] = pChnUnit->GetDistance(); + pChnUnit = pChnUnit + 1; + index++; + } + } + if (IsNeedFrameSplit(pTail->column_id, pHeader->total_column)) { + output.scan_complete = true; + } + this->last_cloumn_id_ = pTail->column_id; + return 0; +} + +template +bool Udp7_2Parser::IsNeedFrameSplit(uint16_t column_id, uint16_t total_column) { + if (column_id < this->last_cloumn_id_) { + return true; + } + return false; +} diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/src/udp_p40_parser.cc b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/src/udp_p40_parser.cc new file mode 100644 index 0000000..030efaa --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/src/udp_p40_parser.cc @@ -0,0 +1,152 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ + +/* + * File: udp_p40_parser.cc + * Author: Zhang Yu + * Description: Implemente UdpP40Parser class +*/ + +#include "udp_p40_parser.h" +#include "general_parser.h" +#include "udp_protocol_p40.h" +using namespace hesai::lidar; +#define DISTANCEUNIT 0.004 +template +UdpP40Parser::UdpP40Parser() { + this->motor_speed_ = 0; + this->return_mode_ = 0; +} + +template +UdpP40Parser::~UdpP40Parser() { printf("release general parser\n"); } + +template +int UdpP40Parser::ComputeXYZI(LidarDecodedFrame &frame, LidarDecodedPacket &packet) { + for (int blockid = 0; blockid < packet.block_num; blockid++) { + T_Point point; + int elevation = 0; + int azimuth = 0; + + for (int i = 0; i < packet.laser_num; i++) { + int point_index = packet.packet_index * packet.points_num + blockid * packet.laser_num + i; + float distance = packet.distances[blockid * packet.laser_num + i] * DISTANCEUNIT; + int Azimuth = packet.azimuth[blockid * packet.laser_num + i]; + if (this->get_correction_file_) { + elevation = this->elevation_correction_[i] * kResolutionInt; + elevation = (CIRCLE + elevation) % CIRCLE; + azimuth = Azimuth + this->azimuth_collection_[i] * kResolutionInt; + azimuth = (CIRCLE + azimuth) % CIRCLE; + } + float xyDistance = distance * this->cos_all_angle_[(elevation)]; + float x = xyDistance * this->sin_all_angle_[(azimuth)]; + float y = xyDistance * this->cos_all_angle_[(azimuth)]; + float z = distance * this->sin_all_angle_[(elevation)]; + this->TransformPoint(x, y, z); + setX(frame.points[point_index], x); + setY(frame.points[point_index], y); + setZ(frame.points[point_index], z); + setIntensity(frame.points[point_index], packet.reflectivities[blockid * packet.laser_num + i]); + setTimestamp(frame.points[point_index], double(packet.sensor_timestamp) / kMicrosecondToSecond); + setRing(frame.points[point_index], i); + } + } + frame.points_num += packet.points_num; + frame.packet_num = packet.packet_index; + return 0; +} + +template +int UdpP40Parser::DecodePacket(LidarDecodedPacket &output, const UdpPacket& udpPacket) { + if (udpPacket.buffer[0] != 0xFF || udpPacket.buffer[1] != 0xEE ) { + return -1; + } + const HS_LIDAR_BODY_AZIMUTH_L40 *pAzimuth = + reinterpret_cast (&(udpPacket.buffer[0])); + + const HS_LIDAR_BODY_CHN_UNIT_L40 *pChnUnit = + reinterpret_cast ((const unsigned char *)pAzimuth + + sizeof(HS_LIDAR_BODY_AZIMUTH_L40)); + + const HS_LIDAR_TAIL_L40 *pTail = + reinterpret_cast ( + &(udpPacket.buffer[0]) +(sizeof(HS_LIDAR_BODY_AZIMUTH_L40) + + sizeof(HS_LIDAR_BODY_CHN_UNIT_L40) * LASERNUM) * BLOCKNUM); + + this->spin_speed_ = pTail->GetMotorSpeed(); + output.spin_speed = pTail->m_u16MotorSpeed; + + output.points_num = BLOCKNUM * LASERNUM; + output.scan_complete = false; + output.block_num = BLOCKNUM; + output.laser_num = LASERNUM; + output.distance_unit = DISTANCEUNIT; + + output.host_timestamp = GetMicroTickCountU64(); + output.sensor_timestamp = pTail->GetMicroLidarTimeU64(); + int index = 0; + float minAzimuth = 0; + float maxAzimuth = 0; + + for (int j = 0; j < BLOCKNUM; j++) { + uint16_t u16Azimuth = pAzimuth->GetAzimuth(); + pChnUnit = reinterpret_cast((const unsigned char *)pAzimuth + sizeof(HS_LIDAR_BODY_AZIMUTH_L40)); + + pAzimuth = reinterpret_cast( + (const unsigned char *)pAzimuth + sizeof(HS_LIDAR_BODY_AZIMUTH_L40) + + sizeof(HS_LIDAR_BODY_CHN_UNIT_L40) * LASERNUM + ); + auto elevation = 0; + for (int i = 0; i < LASERNUM; i++) { + if (this->get_firetime_file_) { + output.azimuth[index] = u16Azimuth + this->GetFiretimesCorrection(i, this->spin_speed_) * kResolutionInt; + }else { + output.azimuth[index] = u16Azimuth; + } + output.distances[index] = pChnUnit->GetDistance(); + output.reflectivities[index] = pChnUnit->GetReflectivity(); + output.elevation[index] = elevation; + pChnUnit = pChnUnit + 1; + index = index + 1; + } + + if (IsNeedFrameSplit(u16Azimuth)) { + output.scan_complete = true; + } + this->last_azimuth_ = u16Azimuth; + } + return 0; +} + +template +bool UdpP40Parser::IsNeedFrameSplit(uint16_t azimuth) { + if (this->last_azimuth_ > azimuth && (this->last_azimuth_- azimuth > kSplitFrameMinAngle)) { + return true; + } + return false; +} diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/src/udp_p64_parser.cc b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/src/udp_p64_parser.cc new file mode 100644 index 0000000..4a0f2be --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/src/udp_p64_parser.cc @@ -0,0 +1,156 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ + +/* + * File: udp_p64_parser.cc + * Author: Zhang Yu + * Description: Implemente UdpP64Parser class +*/ + +#include "udp_p64_parser.h" +#include "general_parser.h" +#include "udp_protocol_p64.h" +using namespace hesai::lidar; +template +UdpP64Parser::UdpP64Parser() { + this->motor_speed_ = 0; + this->return_mode_ = 0; +} + +template +UdpP64Parser::~UdpP64Parser() { printf("release general parser\n"); } + +template +int UdpP64Parser::ComputeXYZI(LidarDecodedFrame &frame, LidarDecodedPacket &packet) { + for (int blockid = 0; blockid < packet.block_num; blockid++) { + T_Point point; + int elevation = 0; + int azimuth = 0; + + for (int i = 0; i < packet.laser_num; i++) { + int point_index = packet.packet_index * packet.points_num + blockid * packet.laser_num + i; + float distance = packet.distances[blockid * packet.laser_num + i] * packet.distance_unit; + int Azimuth = packet.azimuth[blockid * packet.laser_num + i]; + if (this->get_correction_file_) { + elevation = this->elevation_correction_[i] * kResolutionInt; + elevation = (CIRCLE + elevation) % CIRCLE; + azimuth = Azimuth + this->azimuth_collection_[i] * kResolutionInt; + azimuth = (CIRCLE + azimuth) % CIRCLE; + } + float xyDistance = distance * this->cos_all_angle_[(elevation)]; + float x = xyDistance * this->sin_all_angle_[(azimuth)]; + float y = xyDistance * this->cos_all_angle_[(azimuth)]; + float z = distance * this->sin_all_angle_[(elevation)]; + this->TransformPoint(x, y, z); + setX(frame.points[point_index], x); + setY(frame.points[point_index], y); + setZ(frame.points[point_index], z); + setIntensity(frame.points[point_index], packet.reflectivities[blockid * packet.laser_num + i]); + setTimestamp(frame.points[point_index], double(packet.sensor_timestamp) / kMicrosecondToSecond); + setRing(frame.points[point_index], i); + } + } + frame.points_num += packet.points_num; + frame.packet_num = packet.packet_index; + return 0; +} + +template +int UdpP64Parser::DecodePacket(LidarDecodedPacket &output, const UdpPacket& udpPacket) { + if (udpPacket.buffer[0] != 0xEE || udpPacket.buffer[1] != 0xFF ) { + return -1; + } + const HS_LIDAR_L64_Header *pHeader = + reinterpret_cast(&(udpPacket.buffer[0])); + + const HS_LIDAR_BODY_AZIMUTH_L64 *pAzimuth = + reinterpret_cast( + (const unsigned char *)pHeader + sizeof(HS_LIDAR_L64_Header)); + + const HS_LIDAR_BODY_CHN_UNIT_L64 *pChnUnit = + reinterpret_cast( + (const unsigned char *)pAzimuth + sizeof(HS_LIDAR_BODY_AZIMUTH_L64)); + + const HS_LIDAR_TAIL_L64 *pTail = + reinterpret_cast( + (const unsigned char *)pHeader + sizeof(HS_LIDAR_L64_Header) + + (sizeof(HS_LIDAR_BODY_AZIMUTH_L64) + + sizeof(HS_LIDAR_BODY_CHN_UNIT_L64) * pHeader->GetLaserNum()) * + pHeader->GetBlockNum()); + this->spin_speed_ = pTail->GetMotorSpeed(); + output.spin_speed = pTail->m_u16MotorSpeed; + output.host_timestamp = GetMicroTickCountU64(); + output.points_num = pHeader->GetBlockNum() * pHeader->GetLaserNum(); + output.scan_complete = false; + output.sensor_timestamp = pTail->GetMicroLidarTimeU64(); + output.distance_unit = pHeader->GetDistUnit(); + output.block_num = pHeader->GetBlockNum(); + output.laser_num = pHeader->GetLaserNum(); + + int index = 0; + float minAzimuth = 0; + float maxAzimuth = 0; + + for (int blockid = 0; blockid < pHeader->GetBlockNum(); blockid++) { + uint16_t u16Azimuth = pAzimuth->GetAzimuth(); + pChnUnit = reinterpret_cast( + (const unsigned char *)pAzimuth + sizeof(HS_LIDAR_BODY_AZIMUTH_L64)); + + pAzimuth = reinterpret_cast( + (const unsigned char *)pAzimuth + sizeof(HS_LIDAR_BODY_AZIMUTH_L64) + + sizeof(HS_LIDAR_BODY_CHN_UNIT_L64) * pHeader->GetLaserNum()); + + auto elevation = 0; + for (int i = 0; i < pHeader->GetLaserNum(); i++) { + if(this->get_firetime_file_) { + output.azimuth[index] = u16Azimuth + this->GetFiretimesCorrection(i, this->spin_speed_) * kResolutionInt; + }else { + output.azimuth[index] = u16Azimuth; + } + output.distances[index] = pChnUnit->GetDistance(); + output.reflectivities[index] = pChnUnit->GetReflectivity(); + output.elevation[index] = elevation; + pChnUnit = pChnUnit + 1; + index++; + } + if (IsNeedFrameSplit(u16Azimuth)) { + output.scan_complete = true; + } + this->last_azimuth_ = u16Azimuth; + } + return 0; +} + +template +bool UdpP64Parser::IsNeedFrameSplit(uint16_t azimuth) { + if (this->last_azimuth_ > azimuth && (this->last_azimuth_- azimuth > kSplitFrameMinAngle)) { + return true; + } + return false; +} + diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/udp_parser.cc b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/udp_parser.cc new file mode 100644 index 0000000..3c342d5 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/udp_parser.cc @@ -0,0 +1,413 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ +#include "udp_parser.h" +#include "general_parser.h" +using namespace hesai::lidar; + + +template +UdpParser::UdpParser(uint8_t major, uint8_t minor) { + parser_ = nullptr; + pcap_saver_ = nullptr; + fisrt_packet_ = true; + pcap_time_synchronization_ = true; + this->CreatGeneralParser(major, minor); +} + +template +UdpParser::UdpParser(UdpPacket& packet) { + parser_ = nullptr; + pcap_saver_ = nullptr; + fisrt_packet_ = true; + pcap_time_synchronization_ = true; + this->CreatGeneralParser(packet); +} + +template +UdpParser::UdpParser(std::string lidar_type) { + parser_ = nullptr; + pcap_saver_ = nullptr; + fisrt_packet_ = true; + pcap_time_synchronization_ = true; + this->CreatGeneralParser(lidar_type); +} + +template +UdpParser::UdpParser() { + parser_ = nullptr; + pcap_saver_ = nullptr; + fisrt_packet_ = true; + pcap_time_synchronization_ = true; +} + +template +UdpParser::~UdpParser() { + if (parser_ != nullptr) { + delete parser_; + parser_ = nullptr; + } + + if (pcap_saver_ != nullptr) { + delete pcap_saver_; + pcap_saver_ = nullptr; + } +} + +template +void UdpParser::LoadCorrectionFile(std::string correction_path) { + if (parser_ != nullptr) { + parser_->LoadCorrectionFile(correction_path); + } + return; +} + +template +int UdpParser::LoadCorrectionString(char *correction_string) { + if (parser_ != nullptr) { + return parser_->LoadCorrectionString(correction_string); + } + return -1; +} + +template +void UdpParser::LoadFiretimesFile(std::string firetimes_path) { + if (parser_ != nullptr) { + parser_->LoadFiretimesFile(firetimes_path); + } + return; +} + +template +void UdpParser::CreatGeneralParser(uint8_t major, uint8_t minor) { + if (parser_ != nullptr) { + return; + } + switch (major) { + case 1: // Pandar128 + { + switch (minor) { + case 4: + parser_ = new Udp1_4Parser(); + lidar_type_decoded_ = "Pandar128"; + break; + + default: + break; + } + } break; + case 2: // ET + { + switch (minor) { + case 4: + parser_ = new Udp2_4Parser(); + lidar_type_decoded_ = "ET25-E1X"; + case 5: + parser_ = new Udp2_5Parser(); // ET25 + lidar_type_decoded_ = "ET25-E2X"; + break; + default: + break; + } + + } break; + case 3: // QT + { + switch (minor) { + case 1: + parser_ = new Udp3_1Parser(); // QT32 QT64 + lidar_type_decoded_ = "PandarQT"; + break; + case 2: + parser_ = new Udp3_2Parser(); // QT128 + lidar_type_decoded_ = "PandarQT128"; + break; + default: + break; + } + + } break; + case 4: // AT128 + { + switch (minor) { + case 1: + case 3: + parser_ = new Udp4_3Parser(); + lidar_type_decoded_ = "AT128"; + break; + default: + break; + } + + } break; + case 6: // PandarXT + { + switch (minor) { + case 1: + parser_ = new Udp6_1Parser(); + lidar_type_decoded_ = "PandarXT"; + break; + default: + break; + } + + } break; + case 7: // PandarFT + { + switch (minor) { + case 2: + parser_ = new Udp7_2Parser(); + lidar_type_decoded_ = "PandarFT120"; + break; + default: + break; + } + + } break; + default: + break; + } + return ; +} + +template +void UdpParser::CreatGeneralParser(const UdpPacket& packet) { + if (parser_ != nullptr) { + return; + } + if (PKT_SIZE_40P == packet.packet_len || + PKT_SIZE_40P + 4 == packet.packet_len || PKT_SIZE_AC == packet.packet_len || + PKT_SIZE_AC + 4 == packet.packet_len) { + + // Pandar40 + parser_ = new UdpP40Parser(); + lidar_type_decoded_ = "Pandar40"; + return; + } + if (PKT_SIZE_64 == packet.packet_len || PKT_SIZE_64 + 4 == packet.packet_len || + PKT_SIZE_20 == packet.packet_len || PKT_SIZE_20 + 4 == packet.packet_len) { + + // Pandar64 + parser_ = new UdpP64Parser(); + lidar_type_decoded_ = "Pandar64"; + return; + } + if (packet.buffer[0] != 0xEE && packet.buffer[1] != 0xFF) { + printf("Packet with invaild delimiter\n"); + return; + } + uint8_t UdpMajorVersion = packet.buffer[2]; + uint8_t UdpMinorVersion = packet.buffer[3]; + this->CreatGeneralParser(UdpMajorVersion, UdpMinorVersion); + return; +} +template +void UdpParser::CreatGeneralParser(std::string lidar_type) { + if (parser_ != nullptr) { + return; + } + + if (lidar_type == "AT128" || lidar_type == "AT128E2X" || lidar_type == "AT128E3X") { + parser_ = new Udp4_3Parser(); + } else if (lidar_type == "Pandar128E3X" || lidar_type == "Pandar128") { + parser_ = new Udp1_4Parser(); + } else if (lidar_type == "Pandar40S" || lidar_type == "Pandar40E3X") { + parser_ = new Udp1_4Parser(); + } else if (lidar_type == "Pandar60S" || lidar_type == "Pandar64E3X") { + parser_ = new Udp1_4Parser(); + } else if (lidar_type == "Pandar90" || lidar_type == "Pandar90E3X") { + parser_ = new Udp1_4Parser(); + } else if (lidar_type == "PandarXT") { + parser_ = new Udp6_1Parser(); + } else if (lidar_type == "PandarXT16" || lidar_type == "PandarXT-16") { + parser_ = new Udp6_1Parser(); + } else if (lidar_type == "PandarXT32" || lidar_type == "PandarXT-32") { + parser_ = new Udp6_1Parser(); + } else if (lidar_type == "PandarXTM" || lidar_type == "XT32M2X") { + parser_ = new Udp6_1Parser(); + } else if (lidar_type == "PandarQT") { + parser_ = new Udp3_1Parser(); + } else if (lidar_type == "PandarQT128" || lidar_type == "QT128C2X") { + parser_ = new Udp3_2Parser(); + } else if (lidar_type == "Pandar64") { + parser_ = new UdpP64Parser(); + } else if (lidar_type == "Pandar40" || lidar_type == "Pandar40P") { + parser_ = new UdpP40Parser(); + } else if (lidar_type == "PandarFT120" || lidar_type == "FT120C1X") { + parser_ = new Udp7_2Parser(); + } else if ( lidar_type == "ET25-E1X" ) { + parser_ = new Udp2_4Parser(); + } else if (lidar_type == "ET25-E2X" || lidar_type == "ET25" || lidar_type == "ET") { + parser_ = new Udp2_5Parser(); + } +} +template +PcapSaver *UdpParser::GetPcapSaver() { + if (pcap_saver_ == nullptr) { + pcap_saver_ = new PcapSaver; + } + return pcap_saver_; +} +template +GeneralParser *UdpParser::GetGeneralParser() { return parser_; } + +template +void UdpParser::SetGeneralParser(GeneralParser *parser) { parser_ = parser; } + +template +void UdpParser::EnableUpdateMonitorInfo() { + if (parser_ != nullptr) { + parser_->EnableUpdateMonitorInfo(); + } + return; +} + +template +void UdpParser::DisableUpdateMonitorInfo() { + if (parser_ != nullptr) { + parser_->DisableUpdateMonitorInfo(); + } + return; +} + +template +uint16_t *UdpParser::GetMonitorInfo1() { + if (parser_ != nullptr) { + return parser_->GetMonitorInfo1(); + } +} + +template +uint16_t *UdpParser::GetMonitorInfo2() { + if (parser_ != nullptr) { + return parser_->GetMonitorInfo2(); + } +} + +template +uint16_t *UdpParser::GetMonitorInfo3() { + if (parser_ != nullptr) { + return parser_->GetMonitorInfo3(); + } +} + +template +int UdpParser::ComputeXYZI(LidarDecodedFrame &frame, LidarDecodedPacket &packet) { + if (parser_ == nullptr) { + return -1; + } + if (parser_ != nullptr) { + return parser_->ComputeXYZI(frame, packet); + } +} + +template +int UdpParser::DecodePacket(LidarDecodedPacket &output, const UdpPacket& udpPacket) { + if(pcap_saver_ == nullptr){ + pcap_saver_ = new PcapSaver; + } + if (parser_ == nullptr) { + // Udp raw_udp_packet + this->CreatGeneralParser(udpPacket); + return 0; + } else { + int res = parser_->DecodePacket(output, udpPacket); + //data from pcap and play rate synchronize with the host time + if (source_type_ == 2 && pcap_time_synchronization_ == true) { + if(fisrt_packet_ == true) { + last_host_timestamp_ = output.host_timestamp; + last_sensor_timestamp_ = output.sensor_timestamp; + packet_count_ = 1; + fisrt_packet_ = false; + } else { + packet_count_ += 1; + if (packet_count_ >= kPcapPlaySynchronizationCount) { + int reset_time = static_cast((output.sensor_timestamp - last_sensor_timestamp_) - (output.host_timestamp - last_host_timestamp_)); + last_host_timestamp_ = output.host_timestamp; + last_sensor_timestamp_ = output.sensor_timestamp; + packet_count_ = 0; + if (reset_time > 0) { + std::this_thread::sleep_for(std::chrono::microseconds(reset_time)); + } + } + } + } + return res; + } +} + +template +int UdpParser::DecodePacket(LidarDecodedFrame &frame, const UdpPacket& udpPacket) { + if (parser_ == nullptr) { + uint8_t UdpMajorVersion = udpPacket.buffer[2]; + uint8_t UdpMinorVersion = udpPacket.buffer[3]; + this->CreatGeneralParser(UdpMajorVersion, UdpMinorVersion); + } + if(pcap_saver_ == nullptr){ + pcap_saver_ = new PcapSaver; + } + if (parser_ != nullptr) { + return parser_->DecodePacket(frame, udpPacket); + } + +} + + +template +int UdpParser::GetGeneralParser(GeneralParser **parser) { + if (parser_ != nullptr) { + *parser = parser_; + return 1; + } else { + return -1; + } +} + +template +int UdpParser::SetTransformPara(float x, float y, float z, float roll, float pitch, float yaw) { + if (parser_ != nullptr) { + parser_->SetTransformPara(x, y, z, roll, pitch, yaw); + return 0; + } + return -1; +} + +template +void UdpParser::SetPcapPlay(bool pcap_time_synchronization, int source_type) { + pcap_time_synchronization_ = pcap_time_synchronization; + source_type_ = source_type; +} + +template +void UdpParser::SetFrameAzimuth(float frame_start_azimuth) { + if (parser_ != nullptr) { + parser_->SetFrameAzimuth(frame_start_azimuth); + } else { + printf("parser is nullptr\n"); + } + return; +} diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/udp_parser.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/udp_parser.h new file mode 100644 index 0000000..2d995bc --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/udp_parser.h @@ -0,0 +1,121 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ +#ifndef UDP_PARSER_H_ +#define UDP_PARSER_H_ + +#include +#include "general_parser.h" +#include "pcap_saver.h" +#include "lidar_types.h" +#include "udp_p40_parser.h" +#include "udp_p64_parser.h" +#include "udp1_4_parser.h" +#include "udp2_4_parser.h" +#include "udp2_5_parser.h" +#include "udp3_1_parser.h" +#include "udp3_2_parser.h" +#include "udp4_3_parser.h" +#include "udp6_1_parser.h" +#include "udp7_2_parser.h" +#include "udp_p40_parser.h" +#include "udp_p64_parser.h" +#define PKT_SIZE_40P (1262) +#define PKT_SIZE_AC (1256) +#define PKT_SIZE_64 (1194) +#define PKT_SIZE_20 (1270) +namespace hesai +{ +namespace lidar +{ +// class UdpParser +// the UdpParser class is an interface layer.it instantiates a specific udp parser class, +// which is determined by lidar type. +// UdpParser mainly parsers udp or pcap packets and computes xyzi of points +// you can parser the upd or pcap packets using the DocodePacket fuction +// you can compute xyzi of points using the ComputeXYZI fuction, which uses cpu to compute +template +class UdpParser { + public: + UdpParser(uint8_t major, uint8_t minor); + UdpParser(std::string lidar_type); + UdpParser(UdpPacket &packet); + UdpParser(); + virtual ~UdpParser(); + void CreatGeneralParser(uint8_t major, uint8_t minor); + void CreatGeneralParser(std::string lidar_type); + void CreatGeneralParser(const UdpPacket& packet); + GeneralParser *GetGeneralParser(); + void SetGeneralParser(GeneralParser *Parser); + PcapSaver *GetPcapSaver(); + + // get lidar correction file from local file,and pass to udp parser + void LoadCorrectionFile(std::string correction_path); //从本地文件获取 + int LoadCorrectionString(char *correction_string); //从角度文件char*数据获取 + + // get lidar firetime correction file from local file,and pass to udp parser + void LoadFiretimesFile(std::string firetimes_path); + void EnableUpdateMonitorInfo(); + void DisableUpdateMonitorInfo(); + uint16_t *GetMonitorInfo1(); + uint16_t *GetMonitorInfo2(); + uint16_t *GetMonitorInfo3(); + + // covert a origin udp packet to decoded packet, the decode function is in UdpParser module + // udp_packet is the origin udp packet, output is the decoded packet + int DecodePacket(LidarDecodedPacket &output, const UdpPacket& udpPacket); + + // covert a origin udp packet to decoded data, and pass the decoded data to a frame struct to reduce memory copy + int DecodePacket(LidarDecodedFrame &frame, const UdpPacket& udpPacket); + + // compute xyzi of points from decoded packet + // param packet is the decoded packet; xyzi of points after computed is puted in frame + int ComputeXYZI(LidarDecodedFrame &frame, LidarDecodedPacket &packet); + + int GetGeneralParser(GeneralParser **parser); + int SetTransformPara(float x, float y, float z, float roll, float pitch, float yaw); + GeneralParser* GetParser() {return parser_;} + std::string GetLidarType() {return lidar_type_decoded_;} + void SetPcapPlay(bool pcap_time_synchronization, int source_type); + void SetFrameAzimuth(float frame_start_azimuth); + private: + GeneralParser *parser_; + PcapSaver *pcap_saver_; + std::string lidar_type_decoded_; + bool fisrt_packet_; + uint64_t last_host_timestamp_; + uint64_t last_sensor_timestamp_; + uint8_t packet_count_; + bool pcap_time_synchronization_; + int source_type_; +}; +} // namespace lidar +} // namespace hesai + +#include "udp_parser.cc" + +#endif // UDP_PARSER_H_ diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/general_parser_gpu.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/general_parser_gpu.h new file mode 100644 index 0000000..fb398b0 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/general_parser_gpu.h @@ -0,0 +1,165 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ +#ifndef GENERAL_PARSER_GPU_H_ +#define GENERAL_PARSER_GPU_H_ +#define MAX_LASER_NUM (256) +#include +#include +#include "nvbuffer.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "lidar_types.h" +#ifndef M_PI +#define M_PI 3.1415926535898 +#endif + +#ifndef CIRCLE +#define CIRCLE 36000 +#endif + +#ifndef HALF_CIRCLE +#define HALF_CIRCLE 18000 +#endif + +#define PANDAR_HAS_MEMBER(C, member) has_##member::value +namespace hesai +{ +namespace lidar +{ + +namespace gpu +{ + template + __device__ inline typename std::enable_if::type setX(T_Point& point, const float& value) + { + } + + template + __device__ inline typename std::enable_if::type setX(T_Point& point, const float& value) + { + point.x = value; + } + + template + __device__ inline typename std::enable_if::type setY(T_Point& point, const float& value) + { + } + + template + __device__ inline typename std::enable_if::type setY(T_Point& point, const float& value) + { + point.y = value; + } + + template + __device__ inline typename std::enable_if::type setZ(T_Point& point, const float& value) + { + } + + template + __device__ inline typename std::enable_if::type setZ(T_Point& point, const float& value) + { + point.z = value; + } + + template + __device__ inline typename std::enable_if::type setIntensity(T_Point& point, + const uint8_t& value) + { + } + + template + __device__ inline typename std::enable_if::type setIntensity(T_Point& point, + const uint8_t& value) + { + point.intensity = value; + } + + template + __device__ inline typename std::enable_if::type setTimestamp(T_Point& point, + const double& value) + { + } + + template + __device__ inline typename std::enable_if::type setTimestamp(T_Point& point, + const double& value) + { + point.timestamp = value; + } + +} // namespace gpu + +template +struct PointCloudStruct { + uint32_t frame_id = 0; + int seq = 0; + uint64_t sensor_timestamp[kMaxPacketNumPerFrame]; + float azimuths[kMaxPacketNumPerFrame * kMaxPointsNumPerPacket]; + uint16_t distances[kMaxPacketNumPerFrame * kMaxPointsNumPerPacket]; + uint8_t reflectivities[kMaxPacketNumPerFrame * kMaxPointsNumPerPacket]; + uint16_t spin_speed[kMaxPacketNumPerFrame]; + float firetimes[kMaxPointsNumPerPacket]; + PointT points[kMaxPacketNumPerFrame * kMaxPointsNumPerPacket]; +}; +// class GeneralParserGpu +// the GeneralParserGpu class is a base class for computing points with gpu +// you can compute xyzi of points using the ComputeXYZI fuction, which uses gpu to compute +template +class GeneralParserGpu { + public: + GeneralParserGpu(); + ~GeneralParserGpu(); + virtual int LoadCorrectionFile(std::string correction_path); + virtual int LoadCorrectionString(char *correction_string); + virtual void LoadFiretimesFile(std::string firetimes_path); + virtual int LoadFiretimesString(char *firetimes_string); + + // compute xyzi of points from decoded packet， use gpu device + // param packet is the decoded packet; xyzi of points after computed is puted in frame + virtual int ComputeXYZI(LidarDecodedFrame &frame); + void SetTransformPara(float x, float y, float z, float roll, float pitch, float yaw); + Transform transform_; + bool corrections_loaded_ = false; + protected: + double firetime_correction_[kMaxPointsNumPerPacket]; + MemBufferClass> frame_; +}; +} +} +#include "general_parser_gpu.cu" + +#endif // GENERAL_PARSER_GPU_H_ diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/nvbuffer.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/nvbuffer.h new file mode 100644 index 0000000..9a0588c --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/nvbuffer.h @@ -0,0 +1,196 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ +#pragma once +#include +#include +#include +namespace hesai +{ +namespace lidar +{ +class MemBuffer { + protected: + uint64_t size_; + // typedef __device_builtin__ struct CUstream_st *cudaStream_t; + // void* = cudaStream_t + void* stream_; + bool enable_cache_; + + public: + static std::shared_ptr New(); + virtual ~MemBuffer() {} + virtual void *GpuPtr() { return NULL; } + virtual void *CpuPtr() { return NULL; } + inline bool Init(uint64_t size, void *stream = NULL, bool enable_cache = true) { + size_ = size; + stream_ = stream; + enable_cache_ = enable_cache; + int res = OnInit(); + return res; + } + virtual bool OnInit() { return true; } + virtual void HostToDevice(int start, int size) {} + virtual void DeviceToHost(int start, int size) {} + virtual void HostToDevice() { HostToDevice(0, size_); } + virtual void DeviceToHost() { DeviceToHost(0, size_); } + // Wrap CPU || GPU operations between Wait & Signal + // E.g.: 1.Wait 2.kernal_function 3.cudaDeviceSynchronize 4.Signal + virtual void Signal() {} + virtual void Wait() {} + virtual uint64_t Size() const { return size_; } + virtual void *stream() { return stream_; } + virtual void flush(){ return; } +}; +typedef std::shared_ptr MemBufferPtr; + +class MemBufferSub : public MemBuffer { + private: + MemBufferPtr ptr_; + int start_, sub_size_; + + public: + MemBufferSub(const MemBufferSub &) = delete; + void operator=(const MemBufferSub &) = delete; + + public: + MemBufferSub(MemBufferPtr ptr, int start, int size) + : ptr_(ptr), start_(start_), sub_size_(size) {} + virtual void *GpuPtr() { return (char*)ptr_->GpuPtr() + start_; } + virtual void *CpuPtr() { return (char*)ptr_->CpuPtr() + start_; } + void HostToDevice(int start, int size) override { + ptr_->HostToDevice(start, size); + } + void DeviceToHost(int start, int size) override { + ptr_->DeviceToHost(start, size); + } + void HostToDevice() override { + ptr_->HostToDevice(start_, sub_size_); + } + void DeviceToHost() override { + ptr_->DeviceToHost(start_, sub_size_); + } + void Signal() override { + ptr_->Signal(); + } + void Wait() override { + ptr_->Wait(); + } + uint64_t Size() const override { return sub_size_; } + void *stream() override { return ptr_->stream(); } +}; + +// Normal GPU + CPU memory +// user cudaMemcpy to transfer +class MemBufferGPU : public MemBuffer { + private: + void *cpuPtr; + void *gpuPtr; + MemBufferGPU(const MemBufferGPU &) = delete; + void operator=(const MemBufferGPU &) = delete; + + public: + MemBufferGPU() {} + ~MemBufferGPU(); + bool OnInit() override; + void *CpuPtr() override; + void *GpuPtr() override; + void HostToDevice(int start, int size) override; + void DeviceToHost(int start, int size) override; + void HostToDevice() override { HostToDevice(0, size_); } + void DeviceToHost() override { DeviceToHost(0, size_); } +}; + +// QNX ZeroCopy Buffer +class MemBufferQNX : public MemBuffer { + private: + struct Private; + Private *p_; + MemBufferQNX(const MemBufferQNX &) = delete; + void operator=(const MemBufferQNX &) = delete; + + public: + MemBufferQNX(); + ~MemBufferQNX(); + bool OnInit() override; + void *CpuPtr() override; + void *GpuPtr() override; + void Signal() override; + void Wait() override; + void flush() override; +}; + +template +struct MemBufferArray { + MemBufferPtr buf; + MemBufferArray() {} + MemBufferArray(uint64_t size, void *stream = NULL, bool force_gpu = false) { + if (force_gpu) { + buf.reset(new MemBufferGPU); + } else { + buf = MemBuffer::New(); + } + if (!buf->Init(sizeof(T) * size, stream)) { + printf("Failed to Init MemBufferArray\n "); + abort(); + } + } + T *cpu() { return (T *)buf->CpuPtr(); } + T *gpu() { return (T *)buf->GpuPtr(); } + void Signal() { buf->Signal(); } + void Wait() { buf->Wait(); } + void HostToDevice() { buf->HostToDevice(); } + void DeviceToHost() { buf->DeviceToHost(); } + uint64_t Size() const { return buf->Size(); } + void flush(){buf->flush();} +}; + +template +struct MemBufferClass { + MemBufferPtr buf; + MemBufferClass(void *stream = NULL, bool force_gpu=false) { + if (force_gpu) { + buf.reset(new MemBufferGPU); + } else { + buf = MemBuffer::New(); + } + if (!buf->Init(sizeof(T), stream)) { + printf("Failed to Init MemBufferClass\n "); + abort(); + } + } + T *cpu() { return (T *)buf->CpuPtr(); } + T *gpu() { return (T *)buf->GpuPtr(); } + void Signal() { buf->Signal(); } + void Wait() { buf->Wait(); } + void HostToDevice() { buf->HostToDevice(); } + void DeviceToHost() { buf->DeviceToHost(); } + uint64_t Size() const { return buf->Size(); } + void flush(){buf->flush();} +}; +} +} diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/return_code.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/return_code.h new file mode 100644 index 0000000..6bdbcae --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/return_code.h @@ -0,0 +1,79 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ + +// DO NOT change this file !!!!! +#ifndef __RETURE_CODE__ +#define __RETURE_CODE__ + +#include +#include +#include +namespace hesai +{ +namespace lidar +{ +enum class ReturnCode : int { + Default = 0, + CorrectionsUnloaded = 1, + ConfigJsonUnloaded = 2, + LiDAR2SpeedometerCalibrationUnloaded = 3, + LiDAR2GroundCalibrationUnloaded = 4, + CudaMemcpyHostToDeviceError = 5, + CudaMemcpyDeviceToHostError = 6, + LiDARPacketInvalid = 7, + LiDARPacketExpired = 8, + LiDARPacketNewFrame = 9, + LiDARPacketPreFrame = 10, + LiDARPacketPostFrame = 11, + PerceptionHistoryReset = 12, + WaitForNextFrame = 13, + LiDARRawFrameNotReady = 14, + FrameScansTooFew = 15, + CudaXYZComputingError = 16, + CudaMotionCompensationError = 17, + GravityDirectionHistoryEmpty = 18, + GravityDirectionHistoryExpired = 19, + GravityExpired = 20, + RPYRateHistoryEmpty = 21, + VelocityHistoryEmpty = 22, + RPYRateHistoryExpired = 23, + VelocityHistoryExpired = 24, + SpeedometerExpired = 25, + RPYRateStuck = 26, + VelocityStuck = 27, + SpeedometerStuck = 28, + CudaPointsClusteringError = 29, + CudaProposeObstacleCuboidsError = 30, + CudaComputeCuboidTimestampError = 31, + DetectionInferenceError = 32, + SegmentationInferenceError = 33, + Total +}; +} +} +#endif // !__AT128Perception_RETURE_CODE__ diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/safe_call.cuh b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/safe_call.cuh new file mode 100644 index 0000000..bc50fdd --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/safe_call.cuh @@ -0,0 +1,85 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ +#pragma once +#include +#include "cuda_runtime_api.h" + +#define cudaSafeCall(expr, return_code) \ + { \ + auto err = expr; \ + if (cudaSuccess != err) { \ + printf("Error: %s\tfile(%s):%d\n", cudaGetErrorString(err), __FILE__, \ + __LINE__); \ + return int(return_code); \ + } \ + } +#define cudaSafeMalloc(gpu, size, ...) \ + if (cudaMalloc(&gpu, size) != cudaError::cudaSuccess) { \ + printf("%s cudaMalloc failed", #gpu); \ + return __VA_ARGS__; \ + } + +#define cudaSafeFree(gpu) if (gpu) cudaFree(gpu); + +#define CUDACheck(func) \ + { \ + cudaError_t err = func; \ + if (err != cudaSuccess) { \ + printf("[%s:%d] CudaCheck Failed, error code (%s)!\n", __FILE__, \ + __LINE__, cudaGetErrorString(err)); \ + exit(EXIT_FAILURE); \ + } \ + } + +#define CUDAFreeWithLog(ptr) \ + { \ + cudaError_t err = cudaFree(ptr); \ + if (err != cudaSuccess) { \ + printf("[%s:%d]%s CudaFree Failed, error code (%s)!\n", __FILE__, \ + __LINE__, #ptr, cudaGetErrorString(err)); \ + exit(EXIT_FAILURE); \ + } \ + } + +namespace hesai { +inline void CudaInit() { + // check the compute capability of the device + int num_devices = 0; + CUDACheck(cudaGetDeviceCount(&num_devices)); + printf("%d CUDA Device found\n", num_devices); +} + +inline void CudaSyncWithCheck(const char* msg) { + { + cudaError_t err = cudaDeviceSynchronize(); + if (err != cudaSuccess) { + printf("%s %s \n", msg, cudaGetErrorString(err)); + } + } +} +} // namespace hesai diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/udp1_4_parser_gpu.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/udp1_4_parser_gpu.h new file mode 100644 index 0000000..1ee4670 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/udp1_4_parser_gpu.h @@ -0,0 +1,77 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ +#ifndef Udp1_4_PARSER_GPU_H_ +#define Udp1_4_PARSER_GPU_H_ +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include + +#include "general_parser_gpu.h" + +#ifndef M_PI +#define M_PI 3.1415926535898 +#endif + +namespace hesai +{ +namespace lidar +{ +// class Udp1_4ParserGpu +// computes points for Pandar128 +// you can compute xyzi of points using the ComputeXYZI fuction, which uses gpu to compute +template +class Udp1_4ParserGpu: public GeneralParserGpu{ + private: + float* channel_azimuths_cu_; + float* channel_elevations_cu_; + float* raw_azimuths_cu_; + uint16_t* raw_distances_cu_; + uint8_t* raw_reflectivities_cu_; + uint64_t* raw_sensor_timestamp_cu_; + public: + Udp1_4ParserGpu(); + ~Udp1_4ParserGpu(); + + // compute xyzi of points from decoded packet， use gpu device + // param packet is the decoded packet; xyzi of points after computed is puted in frame + virtual int ComputeXYZI(LidarDecodedFrame &frame); + virtual int LoadCorrectionFile(std::string correction_path); + virtual int LoadCorrectionString(char *correction_string); + bool corrections_loaded_; + +}; +} +} +#include "udp1_4_parser_gpu.cu" +#endif // Udp1_4_PARSER_GPU_H_ diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/udp2_5_parser_gpu.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/udp2_5_parser_gpu.h new file mode 100644 index 0000000..60cb7c0 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/udp2_5_parser_gpu.h @@ -0,0 +1,81 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ +#ifndef Udp2_5_PARSER_GPU_H_ +#define Udp2_5_PARSER_GPU_H_ +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include + +#include "general_parser_gpu.h" + +#ifndef M_PI +#define M_PI 3.1415926535898 +#endif + +namespace hesai +{ +namespace lidar +{ +// class Udp2_5ParserGpu +// computes points for ET25 +// you can compute xyzi of points using the ComputeXYZI fuction, which uses gpu to compute +template +class Udp2_5ParserGpu: public GeneralParserGpu{ + private: + float* channel_azimuths_cu_; + float* channel_elevations_cu_; + float* raw_azimuths_cu_; + float* raw_elevations_cu_; + uint16_t* raw_distances_cu_; + uint8_t* raw_reflectivities_cu_; + uint64_t* raw_sensor_timestamp_cu_; + public: + Udp2_5ParserGpu(); + ~Udp2_5ParserGpu(); + + // compute xyzi of points from decoded packet， use gpu device + // param packet is the decoded packet; xyzi of points after computed is puted in frame + virtual int ComputeXYZI(LidarDecodedFrame &frame); + virtual int LoadCorrectionFile(std::string correction_path); + virtual int LoadCorrectionString(char *correction_string); + int LoadCorrectionDatData(char *correction_string); + int LoadCorrectionCsvData(char *correction_string); + ETCorrections corrections_; + bool corrections_loaded_; + +}; +} +} +#include "udp2_5_parser_gpu.cu" +#endif // Udp2_5_PARSER_GPU_H_ diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/udp3_1_parser_gpu.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/udp3_1_parser_gpu.h new file mode 100644 index 0000000..7413a2b --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/udp3_1_parser_gpu.h @@ -0,0 +1,75 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ +#ifndef Udp3_1_PARSER_GPU_H_ +#define Udp3_1_PARSER_GPU_H_ +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include "general_parser_gpu.h" +#ifndef M_PI +#define M_PI 3.1415926535898 +#endif + +namespace hesai +{ +namespace lidar +{ +// class Udp3_1ParserGpu +// computes points for PandarQT64 +// you can compute xyzi of points using the ComputeXYZI fuction, which uses gpu to compute +template +class Udp3_1ParserGpu: public GeneralParserGpu{ + private: + float* channel_azimuths_cu_; + float* channel_elevations_cu_; + float* raw_azimuths_cu_; + uint16_t* raw_distances_cu_; + uint8_t* raw_reflectivities_cu_; + uint64_t* raw_sensor_timestamp_cu_; + public: + Udp3_1ParserGpu(); + ~Udp3_1ParserGpu(); + + // compute xyzi of points from decoded packet， use gpu device + // param packet is the decoded packet; xyzi of points after computed is puted in frame + virtual int ComputeXYZI(LidarDecodedFrame &frame); + virtual int LoadCorrectionFile(std::string correction_path); + virtual int LoadCorrectionString(char *correction_string); + bool corrections_loaded_; +}; +} +} + +#include "udp3_1_parser_gpu.cu" +#endif // Udp3_1_PARSER_GPU_H_ diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/udp3_2_parser_gpu.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/udp3_2_parser_gpu.h new file mode 100644 index 0000000..a80f14e --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/udp3_2_parser_gpu.h @@ -0,0 +1,73 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ +#ifndef Udp3_2_PARSER_GPU_H_ +#define Udp3_2_PARSER_GPU_H_ +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include "general_parser_gpu.h" +#ifndef M_PI +#define M_PI 3.1415926535898 +#endif +namespace hesai +{ +namespace lidar +{ +// class Udp3_2ParserGpu +// computes points for PandarQT128 +// you can compute xyzi of points using the ComputeXYZI fuction, which uses gpu to compute +template +class Udp3_2ParserGpu: public GeneralParserGpu{ + private: + float* channel_azimuths_cu_; + float* channel_elevations_cu_; + float* raw_azimuths_cu_; + uint16_t* raw_distances_cu_; + uint8_t* raw_reflectivities_cu_; + uint64_t* raw_sensor_timestamp_cu_; + public: + Udp3_2ParserGpu(); + ~Udp3_2ParserGpu(); + + // compute xyzi of points from decoded packet， use gpu device + // param packet is the decoded packet; xyzi of points after computed is puted in frame + virtual int ComputeXYZI(LidarDecodedFrame &frame); + virtual int LoadCorrectionFile(std::string correction_path); + virtual int LoadCorrectionString(char *correction_string); + bool corrections_loaded_; +}; +} +} +#include "udp3_2_parser_gpu.cu" +#endif // Udp3_2_PARSER_GPU_H_ diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/udp4_3_parser_gpu.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/udp4_3_parser_gpu.h new file mode 100644 index 0000000..61a8064 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/udp4_3_parser_gpu.h @@ -0,0 +1,117 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ +#ifndef UDP4_3_PARSER_GPU_H_ +#define UDP4_3_PARSER_GPU_H_ +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include "general_parser_gpu.h" + +namespace hesai +{ +namespace lidar +{ +struct Corrections { + struct Header { + uint8_t delimiter[2]; + uint8_t version[2]; + uint8_t max_channel_num; + uint8_t nmirrors; + uint8_t nframes; + uint8_t frame_config[8]; + uint8_t resolution; + } header; +}; + +struct CorrectionsV1_3 : Corrections { + uint16_t mirror_azi_begins[3]; + uint16_t mirror_azi_ends[3]; + int16_t channel_azimuths[kMaxPointsNumPerPacket]; + int16_t channel_elevations[kMaxPointsNumPerPacket]; + int8_t dazis[36000]; + int8_t deles[36000]; + uint8_t SHA256[32]; +}; + +struct CorrectionsV1_5 : Corrections { + uint32_t mirror_azi_begins[3]; + uint32_t mirror_azi_ends[3]; + int32_t channel_azimuths[128]; + int32_t channel_elevations[128]; + int8_t dazis[128 * 180]; + int8_t deles[128 * 180]; + uint8_t SHA256[32]; +}; + +// class Udp4_3ParserGpu +// computes points for PandarAT128 +// you can compute xyzi of points using the ComputeXYZI fuction, which uses gpu to compute +template +class Udp4_3ParserGpu: public GeneralParserGpu{ + + private: + // corrections + bool corrections_loaded_; + Corrections::Header corrections_header; + float raw_azimuth_begin; + float mirror_azi_begins[3]; + float mirror_azi_ends[3]; + int32_t* channel_azimuths_cu_; + int32_t* channel_elevations_cu_; + int8_t* dazis_cu; + int8_t* deles_cu; + uint8_t* raw_fine_azimuths_cu; + float* raw_azimuths_cu_; + uint16_t* raw_distances_cu_; + uint8_t* raw_reflectivities_cu_; + uint32_t* mirror_azi_begins_cu; + uint32_t* mirror_azi_ends_cu; + uint64_t* raw_sensor_timestamp_cu_; + + public: + Udp4_3ParserGpu(); + ~Udp4_3ParserGpu(); + + // compute xyzi of points from decoded packet， use gpu device + // param packet is the decoded packet; xyzi of points after computed is puted in frame + virtual int ComputeXYZI(LidarDecodedFrame &frame); + virtual int LoadCorrectionFile(std::string correction_path); + virtual int LoadCorrectionString(char *correction_string); + +}; +} +} + +#include "udp4_3_parser_gpu.cu" +#endif // UDP4_3_PARSER_GPU_H_ diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/udp6_1_parser_gpu.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/udp6_1_parser_gpu.h new file mode 100644 index 0000000..65006d7 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/udp6_1_parser_gpu.h @@ -0,0 +1,74 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ +#ifndef Udp6_1_PARSER_GPU_H_ +#define Udp6_1_PARSER_GPU_H_ +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include "general_parser_gpu.h" +#ifndef M_PI +#define M_PI 3.1415926535898 +#endif +namespace hesai +{ +namespace lidar +{ +// class Udp6_1ParserGpu +// computes points for PandarXT PandarXT6 PandarXT32 PandarXTM +// you can compute xyzi of points using the ComputeXYZI fuction, which uses gpu to compute +template +class Udp6_1ParserGpu: public GeneralParserGpu{ + private: + float* channel_azimuths_cu_; + float* channel_elevations_cu_; + float* raw_azimuths_cu_; + uint16_t* raw_distances_cu_; + uint8_t* raw_reflectivities_cu_; + uint64_t* raw_sensor_timestamp_cu_; + public: + Udp6_1ParserGpu(); + ~Udp6_1ParserGpu(); + + // compute xyzi of points from decoded packet， use gpu device + // param packet is the decoded packet; xyzi of points after computed is puted in frame + virtual int ComputeXYZI(LidarDecodedFrame &frame); + virtual int LoadCorrectionFile(std::string correction_path); + virtual int LoadCorrectionString(char *correction_string); + bool corrections_loaded_; +}; +} +} + +#include "udp6_1_parser_gpu.cu" +#endif // Udp6_1_PARSER_GPU_H_ diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/udp7_2_parser_gpu.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/udp7_2_parser_gpu.h new file mode 100644 index 0000000..7d33f1f --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/udp7_2_parser_gpu.h @@ -0,0 +1,79 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ +#ifndef Udp7_2_PARSER_GPU_H_ +#define Udp7_2_PARSER_GPU_H_ +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include "general_parser_gpu.h" +#ifndef M_PI +#define M_PI 3.1415926535898 +#endif +namespace hesai +{ +namespace lidar +{ +// class Udp7_2ParserGpu +// computes points for PandarFT120 +// you can compute xyzi of points using the ComputeXYZI fuction, which uses gpu to compute +template +class Udp7_2ParserGpu: public GeneralParserGpu{ + private: + float* channel_azimuths_cu_; + float* channel_elevations_cu_; + float* raw_azimuths_cu_; + float* raw_elevations_cu; + uint16_t* raw_distances_cu_; + uint8_t* raw_reflectivities_cu_; + uint64_t* raw_sensor_timestamp_cu_; + + public: + Udp7_2ParserGpu(); + ~Udp7_2ParserGpu(); + + // compute xyzi of points from decoded packet， use gpu device + // param packet is the decoded packet; xyzi of points after computed is puted in frame + virtual int ComputeXYZI(LidarDecodedFrame &frame); + virtual int LoadCorrectionFile(std::string correction_path); + virtual int LoadCorrectionString(char *correction_string); + int LoadCorrectionDatData(char *correction_string); + int LoadCorrectionCsvData(char *correction_string); + PandarFTCorrections corrections_; + bool corrections_loaded_; +}; +} +} + +#include "udp7_2_parser_gpu.cu" +#endif // Udp7_2_PARSER_GPU_H_ diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/udp_p40_parser_gpu.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/udp_p40_parser_gpu.h new file mode 100644 index 0000000..60b0446 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/udp_p40_parser_gpu.h @@ -0,0 +1,74 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ +#ifndef Udp_P40_PARSER_GPU_H_ +#define Udp_P40_PARSER_GPU_H_ +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include "general_parser_gpu.h" +#ifndef M_PI +#define M_PI 3.1415926535898 +#endif +namespace hesai +{ +namespace lidar +{ +// class UdpP40Parser +// computes points for Pandar40 +// you can compute xyzi of points using the ComputeXYZI fuction, which uses gpu to compute +template +class UdpP40ParserGpu: public GeneralParserGpu{ + private: + float* channel_azimuths_cu_; + float* channel_elevations_cu_; + float* raw_azimuths_cu_; + uint16_t* raw_distances_cu_; + uint8_t* raw_reflectivities_cu_; + uint64_t* raw_sensor_timestamp_cu_; + public: + UdpP40ParserGpu(); + ~UdpP40ParserGpu(); + + // compute xyzi of points from decoded packet， use gpu device + // param packet is the decoded packet; xyzi of points after computed is puted in frame + virtual int ComputeXYZI(LidarDecodedFrame &frame); + virtual int LoadCorrectionFile(std::string correction_path); + virtual int LoadCorrectionString(char *correction_string); + bool corrections_loaded_; + +}; +} +} +#include "udp_p40_parser_gpu.cu" +#endif // Udp6_1_PARSER_GPU_H_ diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/udp_p64_parser_gpu.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/udp_p64_parser_gpu.h new file mode 100644 index 0000000..a97ec99 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/include/udp_p64_parser_gpu.h @@ -0,0 +1,75 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ +#ifndef Udp_P64_PARSER_GPU_H_ +#define Udp_P64_PARSER_GPU_H_ +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include "general_parser_gpu.h" + +#ifndef M_PI +#define M_PI 3.1415926535898 +#endif +namespace hesai +{ +namespace lidar +{ +// class UdpP64ParserGpu +// computes points for Pandar64 +// you can compute xyzi of points using the ComputeXYZI fuction, which uses gpu to compute +template +class UdpP64ParserGpu: public GeneralParserGpu{ + private: + float* channel_azimuths_cu_; + float* channel_elevations_cu_; + float* raw_azimuths_cu_; + uint16_t* raw_distances_cu_; + uint8_t* raw_reflectivities_cu_; + uint64_t* raw_sensor_timestamp_cu_; + public: + UdpP64ParserGpu(); + ~UdpP64ParserGpu(); + + // compute xyzi of points from decoded packet， use gpu device + // param packet is the decoded packet; xyzi of points after computed is puted in frame + virtual int ComputeXYZI(LidarDecodedFrame &frame); + virtual int LoadCorrectionFile(std::string correction_path); + virtual int LoadCorrectionString(char *correction_string); + bool corrections_loaded_; + +}; +} +} +#include "udp_p64_parser_gpu.cu" +#endif // Udp6_1_PARSER_GPU_H_ diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/src/buffer.cu b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/src/buffer.cu new file mode 100644 index 0000000..ad04d52 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/src/buffer.cu @@ -0,0 +1,94 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ +#include + +#include "nvbuffer.h" +#include "cuda_runtime_api.h" + +#define CUDACheck(func) \ + { \ + cudaError_t err = func; \ + if (err != cudaSuccess) { \ + printf("[%s:%d] CudaCheck Failed, error code (%s)!\n", __FILE__, \ + __LINE__, cudaGetErrorString(err)); \ + exit(EXIT_FAILURE); \ + } \ + } +namespace hesai +{ +namespace lidar +{ +std::shared_ptr MemBuffer::New() { +#ifdef __QNX__ + std::shared_ptr ptr(new MemBufferQNX); +#else + std::shared_ptr ptr(new MemBufferGPU); +#endif + return ptr; +} + +bool MemBufferGPU::OnInit() { + if (cudaMalloc(&gpuPtr, size_) != cudaError::cudaSuccess) { + printf("gpuPtr cudaMalloc failed L MemBufferGPU::OnInit\n"); + return false; + } + cpuPtr = malloc(size_); + return true; +} + +void* MemBufferGPU::CpuPtr() { return cpuPtr; } +void* MemBufferGPU::GpuPtr() { return gpuPtr; } +void MemBufferGPU::HostToDevice(int start, int size) { + if (stream_) { + CUDACheck(cudaMemcpyAsync((void*)((char*)gpuPtr + start), + (void*)((char*)cpuPtr + start), size, + cudaMemcpyHostToDevice, (cudaStream_t)stream_)); + } else { + CUDACheck(cudaMemcpy((void*)((char*)gpuPtr + start), + (void*)((char*)cpuPtr + start), size, + cudaMemcpyHostToDevice)); + } +} +void MemBufferGPU::DeviceToHost(int start, int size) { + if (stream_) { + cudaMemcpyAsync((void*)((char*)cpuPtr + start), + (void*)((char*)gpuPtr + start), size, + cudaMemcpyDeviceToHost); + } else { + cudaMemcpy((void*)((char*)cpuPtr + start), + (void*)((char*)gpuPtr + start), size, + cudaMemcpyDeviceToHost); + } +} + +MemBufferGPU::~MemBufferGPU() { + if (gpuPtr) cudaFree(gpuPtr); + if (cpuPtr) free(cpuPtr); +} +} +} diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/src/general_parser_gpu.cu b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/src/general_parser_gpu.cu new file mode 100644 index 0000000..e077a4b --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/src/general_parser_gpu.cu @@ -0,0 +1,158 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ +#include "general_parser_gpu.h" +#include +#include +using namespace hesai::lidar; +template +GeneralParserGpu::GeneralParserGpu() {} +template +GeneralParserGpu::~GeneralParserGpu() { + if (corrections_loaded_) { + corrections_loaded_ = false; + } +} +template +int GeneralParserGpu::LoadCorrectionFile(std::string correction_path) { + int ret = 0; + std::ifstream fin(correction_path); + if (fin.is_open()) { + int length = 0; + fin.seekg(0, std::ios::end); + length = fin.tellg(); + fin.seekg(0, std::ios::beg); + char *buffer = new char[length]; + fin.read(buffer, length); + fin.close(); + ret = LoadCorrectionString(buffer); + if (ret != 0) { + std::cout << "Parse local correction file Error" << std::endl; + return -1; + } + } else { + std::cout << "Open correction file failed" << std::endl; + return -1; + } + return 0; +} +template +int GeneralParserGpu::LoadCorrectionString(char *correction_content) { + std::string correction_content_str = correction_content; + std::istringstream ifs(correction_content_str); + std::string line; + + std::getline(ifs, line); // skip first line "Laser id,Elevation,Azimuth" or "eeff" + + float elevation_list[MAX_LASER_NUM], azimuth_list[MAX_LASER_NUM]; + + std::vector vfirstLine; + boost::split(vfirstLine, line, boost::is_any_of(",")); + if (vfirstLine[0] == "EEFF" || vfirstLine[0] == "eeff") { + std::getline(ifs, line); // skip second line + } + + int lineCount = 0; + while (std::getline(ifs, line)) { + std::vector vLineSplit; + boost::split(vLineSplit, line, boost::is_any_of(",")); + if (vLineSplit.size() < 3) { // skip error line or hash value line + continue; + } else { + lineCount++; + } + float elevation, azimuth; + int laserId = 0; + + std::stringstream ss(line); + std::string subline; + std::getline(ss, subline, ','); + std::stringstream(subline) >> laserId; + std::getline(ss, subline, ','); + std::stringstream(subline) >> elevation; + std::getline(ss, subline, ','); + std::stringstream(subline) >> azimuth; + + if (laserId != lineCount || laserId >= MAX_LASER_NUM) { + std::cout << "laser id is wrong in correction file. laser Id:" + << laserId << ", line" << lineCount << std::endl; + return -1; + } + elevation_list[laserId - 1] = elevation; + azimuth_list[laserId - 1] = azimuth; + } + // this->elevation_correction_.resize(lineCount); + // this->azimuth_collection_.resize(lineCount); + + // for (int i = 0; i < lineCount; ++i) { + // this->elevation_correction_[i] = elevation_list[i]; + // this->azimuth_collection_[i] = azimuth_list[i]; + // } + return 0; +} + +template +void GeneralParserGpu::LoadFiretimesFile(std::string firetimes_path) { + std::ifstream inFile(firetimes_path, std::ios::in); + if (inFile.is_open()) { + std::string lineStr; + //skip first line + std::getline(inFile, lineStr); + while (getline(inFile, lineStr)) { + std::stringstream ss(lineStr); + std::string index, deltTime; + std::getline(ss, index, ','); + std::getline(ss, deltTime, ','); + this->firetime_correction_[std::stoi(index) - 1] = std::stod(deltTime); + } + } else { + std::cout << "Open correction file failed" << std::endl; + } + return; +} + +template +int GeneralParserGpu::LoadFiretimesString(char *correction_string) { + printf("load firetimes string\n"); + return 0; +} + +template +int GeneralParserGpu::ComputeXYZI(LidarDecodedFrame &frame) { + return 0; +} + +template +void GeneralParserGpu::SetTransformPara(float x, float y, float z, float roll, float pitch, float yaw) { + transform_.x = x; + transform_.y = y; + transform_.z = z; + transform_.roll = roll; + transform_.yaw = yaw; + transform_.pitch = pitch; +} + diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/src/udp1_4_parser_gpu.cu b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/src/udp1_4_parser_gpu.cu new file mode 100644 index 0000000..d05baa5 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/src/udp1_4_parser_gpu.cu @@ -0,0 +1,223 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ + +#include +#include +#include + +#include "udp1_4_parser_gpu.h" +#include "safe_call.cuh" +#include "return_code.h" + +using namespace hesai::lidar; +template +Udp1_4ParserGpu::Udp1_4ParserGpu() { + corrections_loaded_ = false; + cudaSafeMalloc(raw_azimuths_cu_, sizeof(PointCloudStruct::azimuths)); + cudaSafeMalloc(raw_distances_cu_, sizeof(PointCloudStruct::distances)); + cudaSafeMalloc(raw_reflectivities_cu_, sizeof(PointCloudStruct::reflectivities)); + cudaSafeMalloc(raw_sensor_timestamp_cu_, sizeof(PointCloudStruct::sensor_timestamp)); +} +template +Udp1_4ParserGpu::~Udp1_4ParserGpu() { + cudaSafeFree(raw_azimuths_cu_); + cudaSafeFree(raw_distances_cu_); + cudaSafeFree(raw_reflectivities_cu_); + if (corrections_loaded_) { + cudaSafeFree(channel_elevations_cu_); + cudaSafeFree(channel_azimuths_cu_); + corrections_loaded_ = false; + } +} +template +__global__ void compute_xyzs_1_4_impl(T_Point *xyzs, const float* channel_azimuths, const float* channel_elevations, + const float* raw_azimuths, const uint16_t *raw_distances, const uint8_t *raw_reflectivities, + const uint64_t *raw_sensor_timestamp, const double raw_distance_unit, Transform transform, const uint16_t blocknum, uint16_t lasernum) { + auto iscan = blockIdx.x; + auto ichannel = threadIdx.x; + + float azimuth = raw_azimuths[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))]; + auto theta = ((azimuth + channel_azimuths[(ichannel % lasernum)] * kResolutionFloat)) / kHalfCircleFloat * M_PI; + float phi = (channel_elevations[(ichannel % lasernum)] * kResolutionFloat) / HALF_CIRCLE * M_PI; + auto rho = raw_distances[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))] * raw_distance_unit; + + float distance_correction_para_a = 1; + float distance_correction_para_b = 0.012; + float distance_correction_para_h = 0.04; + float distance_correction_para_c = sqrtf(distance_correction_para_b * distance_correction_para_b + distance_correction_para_h * distance_correction_para_h); + float distance_correction_para_d = atanf(distance_correction_para_b / distance_correction_para_h); + + double sin_delt_elevation = distance_correction_para_c / rho * sin(phi); + if (sin_delt_elevation >= -1 && sin_delt_elevation <= 1) { + phi -= distance_correction_para_a * asin(sin_delt_elevation); + phi = (int(phi * kHalfCircleInt / M_PI) % kCircle) / kHalfCircleFloat * M_PI; + } + double sin_delt_azimuth = distance_correction_para_c / rho / cos(phi) * \ + sin((distance_correction_para_d + int(channel_azimuths[(ichannel % lasernum)]) * kResolutionInt % kCircle) / kHalfCircleFloat * M_PI); + + if (sin_delt_azimuth >= -1 && sin_delt_azimuth <= 1) { + theta -= distance_correction_para_a * asin(sin_delt_azimuth); + theta = (int(theta * kHalfCircleInt / M_PI) % kCircle) / kHalfCircleFloat * M_PI; + } + float z = rho * sin(phi); + auto r = rho * cosf(phi); + float x = r * sin(theta); + float y = r * cos(theta); + + float cosa = std::cos(transform.roll); + float sina = std::sin(transform.roll); + float cosb = std::cos(transform.pitch); + float sinb = std::sin(transform.pitch); + float cosc = std::cos(transform.yaw); + float sinc = std::sin(transform.yaw); + + float x_ = cosb * cosc * x + (sina * sinb * cosc - cosa * sinc) * y + + (sina * sinc + cosa * sinb * cosc) * z + transform.x; + float y_ = cosb * sinc * x + (cosa * cosc + sina * sinb * sinc) * y + + (cosa * sinb * sinc - sina * cosc) * z + transform.y; + float z_ = -sinb * x + sina * cosb * y + cosa * cosb * z + transform.z; + gpu::setX(xyzs[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))], x_); + gpu::setY(xyzs[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))], y_); + gpu::setZ(xyzs[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))], z_); + gpu::setIntensity(xyzs[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))], raw_reflectivities[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))]); + gpu::setTimestamp(xyzs[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))], double(raw_sensor_timestamp[iscan]) / kMicrosecondToSecond); +} +template +int Udp1_4ParserGpu::ComputeXYZI(LidarDecodedFrame &frame) { + if (!corrections_loaded_) return int(ReturnCode::CorrectionsUnloaded); + cudaSafeCall(cudaMemcpy(raw_azimuths_cu_, frame.azimuth, + kMaxPacketNumPerFrame * kMaxPointsNumPerPacket * sizeof(float), cudaMemcpyHostToDevice), + ReturnCode::CudaMemcpyHostToDeviceError); + cudaSafeCall(cudaMemcpy(raw_distances_cu_, frame.distances, + kMaxPacketNumPerFrame * kMaxPointsNumPerPacket * sizeof(uint16_t), + cudaMemcpyHostToDevice), + ReturnCode::CudaMemcpyHostToDeviceError); + cudaSafeCall(cudaMemcpy(raw_reflectivities_cu_, frame.reflectivities, + kMaxPacketNumPerFrame * kMaxPointsNumPerPacket * sizeof(uint8_t), + cudaMemcpyHostToDevice), + ReturnCode::CudaMemcpyHostToDeviceError); + cudaSafeCall(cudaMemcpy(raw_sensor_timestamp_cu_, frame.sensor_timestamp, + kMaxPacketNumPerFrame * sizeof(uint64_t), + cudaMemcpyHostToDevice), + ReturnCode::CudaMemcpyHostToDeviceError); + +compute_xyzs_1_4_impl<<>>(this->frame_.gpu()->points, channel_azimuths_cu_, channel_elevations_cu_, + raw_azimuths_cu_, raw_distances_cu_, raw_reflectivities_cu_, raw_sensor_timestamp_cu_, frame.distance_unit, + this->transform_, frame.block_num, frame.laser_num); + cudaSafeCall(cudaGetLastError(), ReturnCode::CudaXYZComputingError); + this->frame_.DeviceToHost(); + std::memcpy(frame.points, this->frame_.cpu()->points, sizeof(T_Point) * kMaxPacketNumPerFrame * kMaxPointsNumPerPacket); + return 0; +} +template +int Udp1_4ParserGpu::LoadCorrectionString(char *correction_content) { + if (corrections_loaded_) { + return 0; + if (channel_elevations_cu_) cudaFree(channel_elevations_cu_); + if (channel_azimuths_cu_) cudaFree(channel_azimuths_cu_); + corrections_loaded_ = false; + } + std::string correction_content_str = correction_content; + std::istringstream ifs(correction_content_str); + std::string line; + // skip first line "Laser id,Elevation,Azimuth" or "eeff" + std::getline(ifs, line); + + float elevation_list[MAX_LASER_NUM], azimuth_list[MAX_LASER_NUM]; + + std::vector vfirstLine; + boost::split(vfirstLine, line, boost::is_any_of(",")); + if (vfirstLine[0] == "EEFF" || vfirstLine[0] == "eeff") { + // skip second line + std::getline(ifs, line); + } + + int lineCount = 0; + while (std::getline(ifs, line)) { + std::vector vLineSplit; + boost::split(vLineSplit, line, boost::is_any_of(",")); + // skip error line or hash value line + if (vLineSplit.size() < 3) { + continue; + } else { + lineCount++; + } + float elevation, azimuth; + int laserId = 0; + + std::stringstream ss(line); + std::string subline; + std::getline(ss, subline, ','); + std::stringstream(subline) >> laserId; + std::getline(ss, subline, ','); + std::stringstream(subline) >> elevation; + std::getline(ss, subline, ','); + std::stringstream(subline) >> azimuth; + if (laserId != lineCount || laserId >= MAX_LASER_NUM) { + std::cout << "laser id is wrong in correction file. laser Id:" + << laserId << ", line" << lineCount << std::endl; + return -1; + } + elevation_list[laserId - 1] = elevation; + azimuth_list[laserId - 1] = azimuth; +} + CUDACheck(cudaMalloc(&channel_azimuths_cu_, sizeof(float) * MAX_LASER_NUM)); + CUDACheck(cudaMalloc(&channel_elevations_cu_, sizeof(float) * MAX_LASER_NUM)); + CUDACheck(cudaMemcpy(channel_azimuths_cu_, azimuth_list, sizeof(float) * MAX_LASER_NUM, cudaMemcpyHostToDevice)); + CUDACheck(cudaMemcpy(channel_elevations_cu_, elevation_list, sizeof(float) * MAX_LASER_NUM, cudaMemcpyHostToDevice)); + corrections_loaded_ = true; + return 0; +} +template +int Udp1_4ParserGpu::LoadCorrectionFile(std::string lidar_correction_file) { + int ret = 0; + printf("load correction file from local correction.csv now!\n"); + std::ifstream fin(lidar_correction_file); + if (fin.is_open()) { + int length = 0; + std::string str_lidar_calibration; + fin.seekg(0, std::ios::end); + length = fin.tellg(); + fin.seekg(0, std::ios::beg); + char *buffer = new char[length]; + fin.read(buffer, length); + fin.close(); + str_lidar_calibration = buffer; + ret = LoadCorrectionString(buffer); + if (ret != 0) { + printf("Parse local Correction file Error\n"); + } else { + printf("Parse local Correction file Success!!!\n"); + return 0; + } + } else { + printf("Open correction file failed\n"); + return -1; + } + return -1; +} diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/src/udp2_5_parser_gpu.cu b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/src/udp2_5_parser_gpu.cu new file mode 100644 index 0000000..43881d9 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/src/udp2_5_parser_gpu.cu @@ -0,0 +1,288 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ + +#include +#include +#include + +#include "udp2_5_parser_gpu.h" +#include "safe_call.cuh" +#include "return_code.h" + +using namespace hesai::lidar; +template +Udp2_5ParserGpu::Udp2_5ParserGpu() { + corrections_loaded_ = false; + cudaSafeMalloc(raw_azimuths_cu_, kMaxPacketNumPerFrame * kMaxPointsNumPerPacket * sizeof(float)); + cudaSafeMalloc(raw_elevations_cu_, kMaxPacketNumPerFrame * kMaxPointsNumPerPacket * sizeof(float)); + cudaSafeMalloc(raw_distances_cu_, sizeof(PointCloudStruct::distances)); + cudaSafeMalloc(raw_reflectivities_cu_, sizeof(PointCloudStruct::reflectivities)); + cudaSafeMalloc(raw_sensor_timestamp_cu_, sizeof(PointCloudStruct::sensor_timestamp)); +} +template +Udp2_5ParserGpu::~Udp2_5ParserGpu() { + cudaSafeFree(raw_azimuths_cu_); + cudaSafeFree(raw_elevations_cu_); + cudaSafeFree(raw_distances_cu_); + cudaSafeFree(raw_reflectivities_cu_); + if (corrections_loaded_) { + cudaSafeFree(channel_elevations_cu_); + cudaSafeFree(channel_azimuths_cu_); + corrections_loaded_ = false; + } +} +template +__global__ void compute_xyzs_2_5_impl(T_Point *xyzs, const float* channel_azimuths, const float* channel_elevations, + const float* raw_azimuths, const float* raw_elevations, const uint16_t *raw_distances, const uint8_t *raw_reflectivities, + const uint64_t *raw_sensor_timestamp, const double raw_distance_unit, Transform transform, const uint16_t blocknum, uint16_t lasernum) { + auto iscan = blockIdx.x; + auto ichannel = threadIdx.x; + if (blocknum == 0 || lasernum == 0) return; + float apha = channel_elevations[0]; + float beta = channel_elevations[1]; + float gamma = channel_elevations[2]; + float raw_azimuth = raw_azimuths[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))]; + float raw_elevation = raw_elevations[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))]; + float phi = channel_azimuths[(ichannel % lasernum) + 3]; + float theta = channel_elevations[(ichannel % lasernum) + 3]; + float an = apha + phi; + float theta_n = (raw_elevation + theta / std::cos(an * M_PI / 180)); + float elv_v = raw_elevation * M_PI / 180 + theta * M_PI / 180 - std::tan(raw_elevation * M_PI / 180) * (1 - std::cos(an * M_PI / 180)) ; + float delt_azi_v = std::sin(an * M_PI / 180) * std::cos(an * M_PI / 180) * theta_n * theta_n / 2 * 1.016 * M_PI / 180 * M_PI / 180; + float eta = phi + delt_azi_v * 180 / M_PI + beta + raw_azimuth / 2; + float delt_azi_h = std::sin(eta * M_PI / 180) * std::tan(2 * gamma * M_PI / 180) * std::tan(elv_v ) + std::sin(2 * eta * M_PI / 180) * gamma * gamma * M_PI / 180 * M_PI / 180; + float elv_h = elv_v * 180 / M_PI + std::cos(eta * M_PI / 180) * 2 * gamma ; + float azi_h = 90 + raw_azimuth + delt_azi_h * 180 / M_PI + delt_azi_v * 180 / M_PI + phi; + + auto rho = raw_distances[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))] * raw_distance_unit; + float z = rho * std::sin(elv_h * M_PI / 180); + auto r = rho * std::cos(elv_h * M_PI / 180) ; + float x = r * std::sin(azi_h * M_PI / 180); + float y = r * std::cos(azi_h * M_PI / 180); + + float cosa = std::cos(transform.roll); + float sina = std::sin(transform.roll); + float cosb = std::cos(transform.pitch); + float sinb = std::sin(transform.pitch); + float cosc = std::cos(transform.yaw); + float sinc = std::sin(transform.yaw); + + float x_ = cosb * cosc * x + (sina * sinb * cosc - cosa * sinc) * y + + (sina * sinc + cosa * sinb * cosc) * z + transform.x; + float y_ = cosb * sinc * x + (cosa * cosc + sina * sinb * sinc) * y + + (cosa * sinb * sinc - sina * cosc) * z + transform.y; + float z_ = -sinb * x + sina * cosb * y + cosa * cosb * z + transform.z; + gpu::setX(xyzs[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))], x_); + gpu::setY(xyzs[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))], y_); + gpu::setZ(xyzs[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))], z_); + gpu::setIntensity(xyzs[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))], raw_reflectivities[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))]); + gpu::setTimestamp(xyzs[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))], double(raw_sensor_timestamp[iscan]) / kMicrosecondToSecond); +} +template +int Udp2_5ParserGpu::ComputeXYZI(LidarDecodedFrame &frame) { + if (!corrections_loaded_) return int(ReturnCode::CorrectionsUnloaded); + cudaSafeCall(cudaMemcpy(raw_azimuths_cu_, frame.azimuth, + kMaxPacketNumPerFrame * kMaxPointsNumPerPacket * sizeof(float), cudaMemcpyHostToDevice), + ReturnCode::CudaMemcpyHostToDeviceError); + cudaSafeCall(cudaMemcpy(raw_elevations_cu_, frame.elevation, + kMaxPacketNumPerFrame * kMaxPointsNumPerPacket * sizeof(float), cudaMemcpyHostToDevice), + ReturnCode::CudaMemcpyHostToDeviceError); + cudaSafeCall(cudaMemcpy(raw_distances_cu_, frame.distances, + kMaxPacketNumPerFrame * kMaxPointsNumPerPacket * sizeof(uint16_t), + cudaMemcpyHostToDevice), + ReturnCode::CudaMemcpyHostToDeviceError); + cudaSafeCall(cudaMemcpy(raw_reflectivities_cu_, frame.reflectivities, + kMaxPacketNumPerFrame * kMaxPointsNumPerPacket * sizeof(uint8_t), + cudaMemcpyHostToDevice), + ReturnCode::CudaMemcpyHostToDeviceError); + cudaSafeCall(cudaMemcpy(raw_sensor_timestamp_cu_, frame.sensor_timestamp, + kMaxPacketNumPerFrame * sizeof(uint64_t), + cudaMemcpyHostToDevice), + ReturnCode::CudaMemcpyHostToDeviceError); + +compute_xyzs_2_5_impl<<>>(this->frame_.gpu()->points, channel_azimuths_cu_, channel_elevations_cu_, + raw_azimuths_cu_, raw_elevations_cu_, raw_distances_cu_, raw_reflectivities_cu_, raw_sensor_timestamp_cu_, frame.distance_unit, + this->transform_, frame.block_num, frame.laser_num); + cudaSafeCall(cudaGetLastError(), ReturnCode::CudaXYZComputingError); + this->frame_.DeviceToHost(); + std::memcpy(frame.points, this->frame_.cpu()->points, sizeof(T_Point) * kMaxPacketNumPerFrame * kMaxPointsNumPerPacket); + return 0; +} +template +int Udp2_5ParserGpu::LoadCorrectionString(char *data) { + if (LoadCorrectionDatData(data)) { + return 0; + } + return LoadCorrectionCsvData(data); +} + + +// csv ----> correction +template +int Udp2_5ParserGpu::LoadCorrectionCsvData(char *correction_string) +{ + std::string correction_content_str = correction_string; + std::istringstream ifs(correction_content_str); + std::string line; + + // skip first line "Laser id,Elevation,Azimuth" or "eeff" + std::getline(ifs, line); + float elevation_list[MAX_LASER_NUM], azimuth_list[MAX_LASER_NUM]; + std::vector vfirstLine; + boost::split(vfirstLine, line, boost::is_any_of(",")); + if (vfirstLine[0] == "EEFF" || vfirstLine[0] == "eeff") { + // skip second line + std::getline(ifs, line); + } + + int lineCount = 0; + while (std::getline(ifs, line)) { + std::vector vLineSplit; + boost::split(vLineSplit, line, boost::is_any_of(",")); + // skip error line or hash value line + if (vLineSplit.size() < 3) { + continue; + } else { + lineCount++; + } + float elevation, azimuth; + int laserId = 0; + + std::stringstream ss(line); + std::string subline; + std::getline(ss, subline, ','); + std::stringstream(subline) >> laserId; + std::getline(ss, subline, ','); + std::stringstream(subline) >> elevation; + std::getline(ss, subline, ','); + std::stringstream(subline) >> azimuth; + + if (laserId != lineCount || laserId >= MAX_LASER_NUM) { + std::cout << "laser id is wrong in correction file. laser Id:" + << laserId << ", line" << lineCount << std::endl; + // return -1; + } + elevation_list[laserId - 1] = elevation; + azimuth_list[laserId - 1] = azimuth; + } + + for (int i = 0; i < lineCount; ++i) { + corrections_.azimuths[i] = azimuth_list[i]; + corrections_.elevations[i] = elevation_list[i]; + printf("%d %f %f \n",i, corrections_.azimuths[i], corrections_.elevations[i]); + } + CUDACheck(cudaMalloc(&channel_azimuths_cu_, sizeof(corrections_.azimuths))); + CUDACheck(cudaMalloc(&channel_elevations_cu_, sizeof(corrections_.azimuths))); + CUDACheck(cudaMemcpy(channel_azimuths_cu_, corrections_.azimuths, sizeof(corrections_.azimuths), cudaMemcpyHostToDevice)); + CUDACheck(cudaMemcpy(channel_elevations_cu_, corrections_.elevations, sizeof(corrections_.azimuths), cudaMemcpyHostToDevice)); + corrections_loaded_ = true; + return 0; +} + + +// buffer(.bin) ---> correction +template +int Udp2_5ParserGpu::LoadCorrectionDatData(char *data) { + try { + char *p = data; + struct ETCorrectionsHeader ETheader = *((struct ETCorrectionsHeader* )p); + if (0xee == ETheader.delimiter[0] && 0xff == ETheader.delimiter[1]) { + switch (ETheader.min_version) { + case 1: { + memcpy((void *)&corrections_, p, sizeof(struct ETCorrectionsHeader)); + p += sizeof(ETCorrectionsHeader); + auto channel_num = corrections_.channel_number; + uint16_t division = corrections_.angle_division; + memcpy((void *)&corrections_.raw_azimuths, p, + sizeof(int16_t) * channel_num); + p += sizeof(int16_t) * channel_num; + memcpy((void *)&corrections_.raw_elevations, p, + sizeof(int16_t) * channel_num); + p += sizeof(uint32_t) * channel_num; + corrections_.elevations[0] = ((float)(corrections_.apha)) / division; + corrections_.elevations[1] = ((float)(corrections_.beta)) / division; + corrections_.elevations[2] = ((float)(corrections_.gamma)) / division; + printf("apha:%f, beta:%f, gamma:%f\n", corrections_.elevations[0], corrections_.elevations[1], corrections_.elevations[2]); + for (int i = 0; i < channel_num; i++) { + corrections_.azimuths[i + 3] = ((float)(corrections_.raw_azimuths[i])) / division; + corrections_.elevations[i + 3] = ((float)(corrections_.raw_elevations[i])) / division; + printf("%d %f %f \n",i, corrections_.azimuths[i + 3], corrections_.elevations[i + 3]); + } + + memcpy((void*)&corrections_.SHA_value, p, 32); + // successed + CUDACheck(cudaMalloc(&channel_azimuths_cu_, sizeof(corrections_.azimuths))); + CUDACheck(cudaMalloc(&channel_elevations_cu_, sizeof(corrections_.azimuths))); + CUDACheck(cudaMemcpy(channel_azimuths_cu_, corrections_.azimuths, sizeof(corrections_.azimuths), cudaMemcpyHostToDevice)); + CUDACheck(cudaMemcpy(channel_elevations_cu_, corrections_.elevations, sizeof(corrections_.azimuths), cudaMemcpyHostToDevice)); + corrections_loaded_ = true; + return 0; + } break; + default: + printf("min_version is wrong!\n"); + break; + } + } else { + return -1; + } + } catch (const std::exception &e) { + std::cerr << e.what() << '\n'; + return -1; + } + return -1; +} +template +int Udp2_5ParserGpu::LoadCorrectionFile(std::string lidar_correction_file) { + int ret = 0; + printf("load correction file from local correction.csv now!\n"); + std::ifstream fin(lidar_correction_file); + if (fin.is_open()) { + printf("Open correction file success\n"); + int length = 0; + std::string str_lidar_calibration; + fin.seekg(0, std::ios::end); + length = fin.tellg(); + fin.seekg(0, std::ios::beg); + char *buffer = new char[length]; + fin.read(buffer, length); + fin.close(); + str_lidar_calibration = buffer; + ret = LoadCorrectionString(buffer); + if (ret != 0) { + printf("Parse local Correction file Error\n"); + } else { + printf("Parse local Correction file Success!!!\n"); + return 0; + } + } else { + printf("Open correction file failed\n"); + return -1; + } + return -1; +} + diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/src/udp3_1_parser_gpu.cu b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/src/udp3_1_parser_gpu.cu new file mode 100644 index 0000000..be05514 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/src/udp3_1_parser_gpu.cu @@ -0,0 +1,205 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ + +#include +#include +#include + +#include "udp3_1_parser_gpu.h" +#include "safe_call.cuh" +#include "return_code.h" + +using namespace hesai::lidar; +template +Udp3_1ParserGpu::Udp3_1ParserGpu() { + corrections_loaded_ = false; + cudaSafeMalloc(raw_azimuths_cu_, sizeof(PointCloudStruct::azimuths)); + cudaSafeMalloc(raw_distances_cu_, sizeof(PointCloudStruct::distances)); + cudaSafeMalloc(raw_reflectivities_cu_, sizeof(PointCloudStruct::reflectivities)); + cudaSafeMalloc(raw_sensor_timestamp_cu_, sizeof(PointCloudStruct::sensor_timestamp)); +} +template +Udp3_1ParserGpu::~Udp3_1ParserGpu() { + cudaSafeFree(raw_azimuths_cu_); + cudaSafeFree(raw_distances_cu_); + cudaSafeFree(raw_reflectivities_cu_); + if (corrections_loaded_) { + cudaSafeFree(channel_elevations_cu_); + cudaSafeFree(channel_azimuths_cu_); + corrections_loaded_ = false; + } +} +template +__global__ void compute_xyzs_3_1_impl(T_Point *xyzs, const float* channel_azimuths, const float* channel_elevations, + const float* raw_azimuths, const uint16_t *raw_distances, const uint8_t *raw_reflectivities, + const uint64_t *raw_sensor_timestamp, const double raw_distance_unit, Transform transform, + int blocknum, int lasernum) { + auto iscan = blockIdx.x; + auto ichannel = threadIdx.x; + float azimuth = raw_azimuths[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))]; + auto theta = (azimuth) / HALF_CIRCLE * M_PI; + auto phi = (channel_elevations[(ichannel % lasernum)] * kResolutionFloat) / HALF_CIRCLE * M_PI; + + auto rho = raw_distances[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))] * raw_distance_unit; + float z = rho * sin(phi); + auto r = rho * cosf(phi); + float x = r * sin(theta); + float y = r * cos(theta); + + float cosa = std::cos(transform.roll); + float sina = std::sin(transform.roll); + float cosb = std::cos(transform.pitch); + float sinb = std::sin(transform.pitch); + float cosc = std::cos(transform.yaw); + float sinc = std::sin(transform.yaw); + + float x_ = cosb * cosc * x + (sina * sinb * cosc - cosa * sinc) * y + + (sina * sinc + cosa * sinb * cosc) * z + transform.x; + float y_ = cosb * sinc * x + (cosa * cosc + sina * sinb * sinc) * y + + (cosa * sinb * sinc - sina * cosc) * z + transform.y; + float z_ = -sinb * x + sina * cosb * y + cosa * cosb * z + transform.z; + gpu::setX(xyzs[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))], x_); + gpu::setY(xyzs[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))], y_); + gpu::setZ(xyzs[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))], z_); + gpu::setIntensity(xyzs[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))], raw_reflectivities[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))]); + gpu::setTimestamp(xyzs[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))], double(raw_sensor_timestamp[iscan]) / kMicrosecondToSecond); +} +template +int Udp3_1ParserGpu::ComputeXYZI(LidarDecodedFrame &frame) { + if (!corrections_loaded_) return int(ReturnCode::CorrectionsUnloaded); + + cudaSafeCall(cudaMemcpy(raw_azimuths_cu_, frame.azimuth, + kMaxPacketNumPerFrame * kMaxPointsNumPerPacket * sizeof(float), cudaMemcpyHostToDevice), + ReturnCode::CudaMemcpyHostToDeviceError); + cudaSafeCall(cudaMemcpy(raw_distances_cu_, frame.distances, + kMaxPacketNumPerFrame * kMaxPointsNumPerPacket * sizeof(uint16_t), + cudaMemcpyHostToDevice), + ReturnCode::CudaMemcpyHostToDeviceError); + cudaSafeCall(cudaMemcpy(raw_reflectivities_cu_, frame.reflectivities, + kMaxPacketNumPerFrame * kMaxPointsNumPerPacket * sizeof(uint8_t), + cudaMemcpyHostToDevice), + ReturnCode::CudaMemcpyHostToDeviceError); + cudaSafeCall(cudaMemcpy(raw_sensor_timestamp_cu_, frame.sensor_timestamp, + kMaxPacketNumPerFrame * sizeof(uint64_t), + cudaMemcpyHostToDevice), + ReturnCode::CudaMemcpyHostToDeviceError); +compute_xyzs_3_1_impl<<>>(this->frame_.gpu()->points, channel_azimuths_cu_, channel_elevations_cu_, + raw_azimuths_cu_, raw_distances_cu_, raw_reflectivities_cu_, raw_sensor_timestamp_cu_, frame.distance_unit, this->transform_, frame.block_num, frame.laser_num); + cudaSafeCall(cudaGetLastError(), ReturnCode::CudaXYZComputingError); + this->frame_.DeviceToHost(); + std::memcpy(frame.points, this->frame_.cpu()->points, sizeof(T_Point) * kMaxPacketNumPerFrame * kMaxPointsNumPerPacket); + return 0; +} +template +int Udp3_1ParserGpu::LoadCorrectionString(char *correction_content) { + if (corrections_loaded_) { + return 0; + if (channel_elevations_cu_) cudaFree(channel_elevations_cu_); + if (channel_azimuths_cu_) cudaFree(channel_azimuths_cu_); + corrections_loaded_ = false; + } + std::string correction_content_str = correction_content; + std::istringstream ifs(correction_content_str); + std::string line; + // skip first line "Laser id,Elevation,Azimuth" or "eeff" + std::getline(ifs, line); + + float elevation_list[MAX_LASER_NUM], azimuth_list[MAX_LASER_NUM]; + + std::vector vfirstLine; + boost::split(vfirstLine, line, boost::is_any_of(",")); + if (vfirstLine[0] == "EEFF" || vfirstLine[0] == "eeff") { + // skip second line + std::getline(ifs, line); + } + + int lineCount = 0; + while (std::getline(ifs, line)) { + std::vector vLineSplit; + boost::split(vLineSplit, line, boost::is_any_of(",")); + // skip error line or hash value line + if (vLineSplit.size() < 3) { + continue; + } else { + lineCount++; + } + float elevation, azimuth; + int laserId = 0; + + std::stringstream ss(line); + std::string subline; + std::getline(ss, subline, ','); + std::stringstream(subline) >> laserId; + std::getline(ss, subline, ','); + std::stringstream(subline) >> elevation; + std::getline(ss, subline, ','); + std::stringstream(subline) >> azimuth; + + if (laserId != lineCount || laserId >= MAX_LASER_NUM) { + std::cout << "laser id is wrong in correction file. laser Id:" + << laserId << ", line" << lineCount << std::endl; + return -1; + } + elevation_list[laserId - 1] = elevation; + azimuth_list[laserId - 1] = azimuth; +} + CUDACheck(cudaMalloc(&channel_azimuths_cu_, sizeof(float) * MAX_LASER_NUM)); + CUDACheck(cudaMalloc(&channel_elevations_cu_, sizeof(float) * MAX_LASER_NUM)); + CUDACheck(cudaMemcpy(channel_azimuths_cu_, azimuth_list, sizeof(float) * MAX_LASER_NUM, cudaMemcpyHostToDevice)); + CUDACheck(cudaMemcpy(channel_elevations_cu_, elevation_list, sizeof(float) * MAX_LASER_NUM, cudaMemcpyHostToDevice)); + corrections_loaded_ = true; + return 0; +} +template +int Udp3_1ParserGpu::LoadCorrectionFile(std::string lidar_correction_file) { + int ret = 0; + printf("load correction file from local correction.csv now!\n"); + std::ifstream fin(lidar_correction_file); + if (fin.is_open()) { + int length = 0; + std::string str_lidar_calibration; + fin.seekg(0, std::ios::end); + length = fin.tellg(); + fin.seekg(0, std::ios::beg); + char *buffer = new char[length]; + fin.read(buffer, length); + fin.close(); + str_lidar_calibration = buffer; + ret = LoadCorrectionString(buffer); + if (ret != 0) { + printf("Parse local Correction file Error\n"); + } else { + printf("Parse local Correction file Success!!!\n"); + return 0; + } + } else { + printf("Open correction file failed\n"); + return -1; + } + return -1; +} diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/src/udp3_2_parser_gpu.cu b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/src/udp3_2_parser_gpu.cu new file mode 100644 index 0000000..959f39f --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/src/udp3_2_parser_gpu.cu @@ -0,0 +1,205 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ + +#include +#include +#include + +#include "udp3_2_parser_gpu.h" +#include "safe_call.cuh" +#include "return_code.h" + +using namespace hesai::lidar; +template +Udp3_2ParserGpu::Udp3_2ParserGpu() { + corrections_loaded_ = false; + cudaSafeMalloc(raw_azimuths_cu_, sizeof(PointCloudStruct::azimuths)); + cudaSafeMalloc(raw_distances_cu_, sizeof(PointCloudStruct::distances)); + cudaSafeMalloc(raw_reflectivities_cu_, sizeof(PointCloudStruct::reflectivities)); + cudaSafeMalloc(raw_sensor_timestamp_cu_, sizeof(PointCloudStruct::sensor_timestamp)); +} +template +Udp3_2ParserGpu::~Udp3_2ParserGpu() { + cudaSafeFree(raw_azimuths_cu_); + cudaSafeFree(raw_distances_cu_); + cudaSafeFree(raw_reflectivities_cu_); + if (corrections_loaded_) { + cudaSafeFree(channel_elevations_cu_); + cudaSafeFree(channel_azimuths_cu_); + corrections_loaded_ = false; + } +} +template +__global__ void compute_xyzs_3_2_impl(T_Point *xyzs, const float* channel_azimuths, const float* channel_elevations, + const float* raw_azimuths, const uint16_t *raw_distances, const uint8_t *raw_reflectivities, + const uint64_t *raw_sensor_timestamp, const double raw_distance_unit, Transform transform, + int blocknum, int lasernum) { + auto iscan = blockIdx.x; + auto ichannel = threadIdx.x; + float azimuth = raw_azimuths[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))]; + auto theta = (azimuth) / HALF_CIRCLE * M_PI; + auto phi = (channel_elevations[(ichannel % lasernum)] * 100.0f) / HALF_CIRCLE * M_PI; + + auto rho = raw_distances[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))] * raw_distance_unit; + float z = rho * sin(phi); + auto r = rho * cosf(phi); + float x = r * sin(theta); + float y = r * cos(theta); + + float cosa = std::cos(transform.roll); + float sina = std::sin(transform.roll); + float cosb = std::cos(transform.pitch); + float sinb = std::sin(transform.pitch); + float cosc = std::cos(transform.yaw); + float sinc = std::sin(transform.yaw); + + float x_ = cosb * cosc * x + (sina * sinb * cosc - cosa * sinc) * y + + (sina * sinc + cosa * sinb * cosc) * z + transform.x; + float y_ = cosb * sinc * x + (cosa * cosc + sina * sinb * sinc) * y + + (cosa * sinb * sinc - sina * cosc) * z + transform.y; + float z_ = -sinb * x + sina * cosb * y + cosa * cosb * z + transform.z; + gpu::setX(xyzs[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))], x_); + gpu::setY(xyzs[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))], y_); + gpu::setZ(xyzs[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))], z_); + gpu::setIntensity(xyzs[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))], raw_reflectivities[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))]); + gpu::setTimestamp(xyzs[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))], double(raw_sensor_timestamp[iscan]) / kMicrosecondToSecond); +} +template +int Udp3_2ParserGpu::ComputeXYZI(LidarDecodedFrame &frame) { + if (!corrections_loaded_) return int(ReturnCode::CorrectionsUnloaded); + + cudaSafeCall(cudaMemcpy(raw_azimuths_cu_, frame.azimuth, + kMaxPacketNumPerFrame * kMaxPointsNumPerPacket * sizeof(float), cudaMemcpyHostToDevice), + ReturnCode::CudaMemcpyHostToDeviceError); + cudaSafeCall(cudaMemcpy(raw_distances_cu_, frame.distances, + kMaxPacketNumPerFrame * kMaxPointsNumPerPacket * sizeof(uint16_t), + cudaMemcpyHostToDevice), + ReturnCode::CudaMemcpyHostToDeviceError); + cudaSafeCall(cudaMemcpy(raw_reflectivities_cu_, frame.reflectivities, + kMaxPacketNumPerFrame * kMaxPointsNumPerPacket * sizeof(uint8_t), + cudaMemcpyHostToDevice), + ReturnCode::CudaMemcpyHostToDeviceError); + cudaSafeCall(cudaMemcpy(raw_sensor_timestamp_cu_, frame.sensor_timestamp, + kMaxPacketNumPerFrame * sizeof(uint64_t), + cudaMemcpyHostToDevice), + ReturnCode::CudaMemcpyHostToDeviceError); +compute_xyzs_3_2_impl<<>>(this->frame_.gpu()->points, channel_azimuths_cu_, channel_elevations_cu_, + raw_azimuths_cu_, raw_distances_cu_, raw_reflectivities_cu_, raw_sensor_timestamp_cu_, frame.distance_unit, this->transform_, frame.block_num, frame.laser_num); + cudaSafeCall(cudaGetLastError(), ReturnCode::CudaXYZComputingError); + this->frame_.DeviceToHost(); + std::memcpy(frame.points, this->frame_.cpu()->points, sizeof(T_Point) * kMaxPacketNumPerFrame * kMaxPointsNumPerPacket); + return 0; +} +template +int Udp3_2ParserGpu::LoadCorrectionString(char *correction_content) { + // corrections_loaded_ = false; + if (corrections_loaded_) { + return 0; + if (channel_elevations_cu_) cudaFree(channel_elevations_cu_); + if (channel_azimuths_cu_) cudaFree(channel_azimuths_cu_); + corrections_loaded_ = false; + } + std::string correction_content_str = correction_content; + std::istringstream ifs(correction_content_str); + std::string line; + // skip first line "Laser id,Elevation,Azimuth" or "eeff" + std::getline(ifs, line); + + float elevation_list[MAX_LASER_NUM], azimuth_list[MAX_LASER_NUM]; + + std::vector vfirstLine; + boost::split(vfirstLine, line, boost::is_any_of(",")); + if (vfirstLine[0] == "EEFF" || vfirstLine[0] == "eeff") { + // skip second line + std::getline(ifs, line); + } + + int lineCount = 0; + while (std::getline(ifs, line)) { + std::vector vLineSplit; + boost::split(vLineSplit, line, boost::is_any_of(",")); + // skip error line or hash value line + if (vLineSplit.size() < 3) { + continue; + } else { + lineCount++; + } + float elevation, azimuth; + int laserId = 0; + + std::stringstream ss(line); + std::string subline; + std::getline(ss, subline, ','); + std::stringstream(subline) >> laserId; + std::getline(ss, subline, ','); + std::stringstream(subline) >> elevation; + std::getline(ss, subline, ','); + std::stringstream(subline) >> azimuth; + if (laserId != lineCount || laserId >= MAX_LASER_NUM) { + std::cout << "laser id is wrong in correction file. laser Id:" + << laserId << ", line" << lineCount << std::endl; + return -1; + } + elevation_list[laserId - 1] = elevation; + azimuth_list[laserId - 1] = azimuth; +} + CUDACheck(cudaMalloc(&channel_azimuths_cu_, sizeof(float) * MAX_LASER_NUM)); + CUDACheck(cudaMalloc(&channel_elevations_cu_, sizeof(float) * MAX_LASER_NUM)); + CUDACheck(cudaMemcpy(channel_azimuths_cu_, azimuth_list, sizeof(float) * MAX_LASER_NUM, cudaMemcpyHostToDevice)); + CUDACheck(cudaMemcpy(channel_elevations_cu_, elevation_list, sizeof(float) * MAX_LASER_NUM, cudaMemcpyHostToDevice)); + corrections_loaded_ = true; + return 0; +} +template +int Udp3_2ParserGpu::LoadCorrectionFile(std::string lidar_correction_file) { + int ret = 0; + printf("load correction file from local correction.csv now!\n"); + std::ifstream fin(lidar_correction_file); + if (fin.is_open()) { + int length = 0; + std::string str_lidar_calibration; + fin.seekg(0, std::ios::end); + length = fin.tellg(); + fin.seekg(0, std::ios::beg); + char *buffer = new char[length]; + fin.read(buffer, length); + fin.close(); + str_lidar_calibration = buffer; + ret = LoadCorrectionString(buffer); + if (ret != 0) { + printf("Parse local Correction file Error\n"); + } else { + printf("Parse local Correction file Success!!!\n"); + return 0; + } + } else { + printf("Open correction file failed\n"); + return -1; + } + return -1; +} diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/src/udp4_3_parser_gpu.cu b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/src/udp4_3_parser_gpu.cu new file mode 100644 index 0000000..78a1d0b --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/src/udp4_3_parser_gpu.cu @@ -0,0 +1,272 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ + +#include +#include +#include + +#include "udp4_3_parser_gpu.h" +#include "safe_call.cuh" +#include "return_code.h" + +using namespace hesai::lidar; +template +Udp4_3ParserGpu::Udp4_3ParserGpu() { + corrections_loaded_ = false; + cudaSafeMalloc(raw_azimuths_cu_, sizeof(PointCloudStruct::azimuths)); + cudaSafeMalloc(raw_distances_cu_, sizeof(PointCloudStruct::distances)); + cudaSafeMalloc(raw_reflectivities_cu_, sizeof(PointCloudStruct::reflectivities)); + cudaSafeMalloc(raw_sensor_timestamp_cu_, sizeof(PointCloudStruct::sensor_timestamp)); +} +template +Udp4_3ParserGpu::~Udp4_3ParserGpu() { + cudaSafeFree(raw_azimuths_cu_); + cudaSafeFree(raw_distances_cu_); + cudaSafeFree(raw_reflectivities_cu_); + if (corrections_loaded_) { + cudaSafeFree(deles_cu); + cudaSafeFree(dazis_cu); + cudaSafeFree(channel_elevations_cu_); + cudaSafeFree(channel_azimuths_cu_); + cudaSafeFree(mirror_azi_begins_cu); + cudaSafeFree(mirror_azi_ends_cu); + corrections_loaded_ = false; + } +} +template +__global__ void compute_xyzs_v4_3_impl( + T_Point *xyzs, const int32_t* channel_azimuths, + const int32_t* channel_elevations, const int8_t* dazis, const int8_t* deles, + const uint32_t* raw_azimuth_begin, const uint32_t* raw_azimuth_end, const uint8_t raw_correction_resolution, + const float* raw_azimuths, const uint16_t *raw_distances, const uint8_t *raw_reflectivities, + const uint64_t *raw_sensor_timestamp, const double raw_distance_unit, Transform transform, const uint16_t blocknum, const uint16_t lasernum) { + auto iscan = blockIdx.x; + auto ichannel = threadIdx.x; + if (ichannel >= blocknum * lasernum) return; + float azimuth = raw_azimuths[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))] / kFineResolutionInt; + int Azimuth = raw_azimuths[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))]; + int count = 0, field = 0; + while (count < 3 && + (((Azimuth + (kFineResolutionInt * kCircle) - raw_azimuth_begin[field]) % (kFineResolutionInt * kCircle) + + (raw_azimuth_end[field] + kFineResolutionInt * kCircle - Azimuth) % (kFineResolutionInt * kCircle)) != + (raw_azimuth_end[field] + kFineResolutionInt * kCircle - + raw_azimuth_begin[field]) % (kFineResolutionInt * kCircle))) { + field = (field + 1) % 3; + count++; + } + if (count >= 3) return; + float m = azimuth / 200.f; + int i = m; + int j = i + 1; + float alpha = m - i; // k + float beta = 1 - alpha; // 1-k + // getAziAdjustV3 + auto dazi = + beta * dazis[(ichannel % lasernum) * (kHalfCircleInt / kResolutionInt) + i] + alpha * dazis[(ichannel % lasernum) * (kHalfCircleInt / kResolutionInt) + j]; + auto theta = ((azimuth + kCircle - raw_azimuth_begin[field] / kFineResolutionFloat) * 2 - + channel_azimuths[(ichannel % lasernum)] * raw_correction_resolution / kFineResolutionFloat + dazi * raw_correction_resolution) / + kHalfCircleFloat * M_PI; + // getEleAdjustV3 + auto dele = + beta * deles[(ichannel % lasernum) * (kHalfCircleInt / kResolutionInt) + i] + alpha * deles[(ichannel % lasernum) * (kHalfCircleInt / kResolutionInt) + j]; + auto phi = (channel_elevations[(ichannel % lasernum)] / kFineResolutionFloat + dele) * raw_correction_resolution / kHalfCircleFloat * M_PI; + + auto rho = raw_distances[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))] * raw_distance_unit; + float z = rho * sin(phi); + auto r = rho * cosf(phi); + float x = r * sin(theta); + float y = r * cos(theta); + + float cosa = std::cos(transform.roll); + float sina = std::sin(transform.roll); + float cosb = std::cos(transform.pitch); + float sinb = std::sin(transform.pitch); + float cosc = std::cos(transform.yaw); + float sinc = std::sin(transform.yaw); + + float x_ = cosb * cosc * x + (sina * sinb * cosc - cosa * sinc) * y + + (sina * sinc + cosa * sinb * cosc) * z + transform.x; + float y_ = cosb * sinc * x + (cosa * cosc + sina * sinb * sinc) * y + + (cosa * sinb * sinc - sina * cosc) * z + transform.y; + float z_ = -sinb * x + sina * cosb * y + cosa * cosb * z + transform.z; + gpu::setX(xyzs[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))], x_); + gpu::setY(xyzs[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))], y_); + gpu::setZ(xyzs[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))], z_); + gpu::setIntensity(xyzs[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))], raw_reflectivities[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))]); + gpu::setTimestamp(xyzs[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))], double(raw_sensor_timestamp[iscan]) / kMicrosecondToSecond); +} +template +int Udp4_3ParserGpu::ComputeXYZI(LidarDecodedFrame &frame) { + if (!corrections_loaded_) return int(ReturnCode::CorrectionsUnloaded); + cudaSafeCall(cudaMemcpy(raw_azimuths_cu_, frame.azimuth, + kMaxPacketNumPerFrame * kMaxPointsNumPerPacket * sizeof(float), cudaMemcpyHostToDevice), + ReturnCode::CudaMemcpyHostToDeviceError); + cudaSafeCall(cudaMemcpy(raw_distances_cu_, frame.distances, + kMaxPacketNumPerFrame * kMaxPointsNumPerPacket * sizeof(uint16_t), + cudaMemcpyHostToDevice), + ReturnCode::CudaMemcpyHostToDeviceError); + cudaSafeCall(cudaMemcpy(raw_reflectivities_cu_, frame.reflectivities, + kMaxPacketNumPerFrame * kMaxPointsNumPerPacket * sizeof(uint8_t), + cudaMemcpyHostToDevice), + ReturnCode::CudaMemcpyHostToDeviceError); + cudaSafeCall(cudaMemcpy(raw_sensor_timestamp_cu_, frame.sensor_timestamp, + kMaxPacketNumPerFrame * sizeof(uint64_t), + cudaMemcpyHostToDevice), + ReturnCode::CudaMemcpyHostToDeviceError); +compute_xyzs_v4_3_impl<<>>( + this->frame_.gpu()->points, (const int32_t*)channel_azimuths_cu_, + (const int32_t*)channel_elevations_cu_, (const int8_t*)dazis_cu, deles_cu, + mirror_azi_begins_cu, mirror_azi_ends_cu, corrections_header.resolution, + raw_azimuths_cu_, raw_distances_cu_, raw_reflectivities_cu_, raw_sensor_timestamp_cu_, frame.distance_unit, + this->transform_, frame.block_num, frame.laser_num); + cudaSafeCall(cudaGetLastError(), ReturnCode::CudaXYZComputingError); + this->frame_.DeviceToHost(); + std::memcpy(frame.points, this->frame_.cpu()->points, sizeof(T_Point) * kMaxPacketNumPerFrame * kMaxPointsNumPerPacket); + return 0; +} +template +int Udp4_3ParserGpu::LoadCorrectionString(char *p) { + corrections_header = *(Corrections::Header *)p; + if ( 0xee != corrections_header.delimiter[0] || 0xff != corrections_header.delimiter[1]) { + std::cerr << "Correction Header delimiter not right" << std::endl; + return -1; + } + // printf("mirror_num: %d,\t channel_num=%d,\tversion=%d\n", + // corrections_header.nframes, corrections_header.max_channel_num, + // corrections_header.version[1]); + // printf("resolution: %d\n", + // corrections_header.resolution); + if (corrections_loaded_) { + return 0; + if (deles_cu) cudaFree(deles_cu); + if (dazis_cu) cudaFree(dazis_cu); + if (channel_elevations_cu_) cudaFree(channel_elevations_cu_); + if (channel_azimuths_cu_) cudaFree(channel_azimuths_cu_); + if (mirror_azi_begins_cu) cudaFree(mirror_azi_begins_cu); + if (mirror_azi_ends_cu) cudaFree(mirror_azi_ends_cu); + corrections_loaded_ = false; + } + float channel_azimuths[kMaxPointsNumPerPacket]; + switch (corrections_header.version[1]) { + case 3: { + // HCHECK_GE(size, sizeof(CorrectionsV1_3)); + auto& corrections = *(CorrectionsV1_3*)p; + mirror_azi_begins[0] = corrections.mirror_azi_begins[0]; + mirror_azi_begins[1] = corrections.mirror_azi_begins[1]; + mirror_azi_begins[2] = corrections.mirror_azi_begins[2]; + mirror_azi_ends[0] = corrections.mirror_azi_ends[0]; + mirror_azi_ends[1] = corrections.mirror_azi_ends[1]; + mirror_azi_ends[2] = corrections.mirror_azi_ends[2]; + CUDACheck(cudaMalloc(&channel_azimuths_cu_, sizeof(CorrectionsV1_3::channel_azimuths))); + CUDACheck(cudaMalloc(&channel_elevations_cu_, sizeof(CorrectionsV1_3::channel_elevations))); + CUDACheck(cudaMalloc(&dazis_cu, sizeof(CorrectionsV1_3::dazis))); + CUDACheck(cudaMalloc(&deles_cu, sizeof(CorrectionsV1_3::deles))); + + CUDACheck(cudaMemcpy(channel_azimuths_cu_, corrections.channel_azimuths, sizeof(corrections.channel_azimuths), cudaMemcpyHostToDevice)); + CUDACheck(cudaMemcpy(channel_elevations_cu_, corrections.channel_elevations, sizeof(corrections.channel_elevations), cudaMemcpyHostToDevice)); + CUDACheck(cudaMemcpy(dazis_cu, corrections.dazis, sizeof(corrections.dazis), cudaMemcpyHostToDevice)); + CUDACheck(cudaMemcpy(deles_cu, corrections.deles, sizeof(corrections.deles), cudaMemcpyHostToDevice)); + for (auto i = 0; i < kMaxPointsNumPerPacket; ++i) { + channel_azimuths[i] = corrections.channel_azimuths[i]; + } + break; + } + case 5: { + // HCHECK_GE(size, sizeof(CorrectionsV1_5)); + auto& corrections = *(CorrectionsV1_5*)(p); + mirror_azi_begins[0] = corrections.mirror_azi_begins[0] / kFineResolutionFloat * corrections.header.resolution; + mirror_azi_begins[1] = corrections.mirror_azi_begins[1] / kFineResolutionFloat * corrections.header.resolution; + mirror_azi_begins[2] = corrections.mirror_azi_begins[2] / kFineResolutionFloat * corrections.header.resolution; + mirror_azi_ends[0] = corrections.mirror_azi_ends[0] / kFineResolutionFloat * corrections.header.resolution; + mirror_azi_ends[1] = corrections.mirror_azi_ends[1] / kFineResolutionFloat * corrections.header.resolution; + mirror_azi_ends[2] = corrections.mirror_azi_ends[2] / kFineResolutionFloat * corrections.header.resolution; + for (int i = 0; i < corrections_header.nframes; ++i) { + corrections.mirror_azi_begins[i] = + corrections.mirror_azi_begins[i] * + corrections.header.resolution; + corrections.mirror_azi_ends[i] = + corrections.mirror_azi_ends[i] * + corrections.header.resolution; + // printf("%lf, %lf\n", + // corrections.mirror_azi_begins[i] / (kFineResolutionFloat * kResolutionFloat), + // corrections.mirror_azi_ends[i] / (kFineResolutionFloat * kResolutionFloat)); + } + CUDACheck(cudaMalloc(&channel_azimuths_cu_, sizeof(CorrectionsV1_5::channel_azimuths))); + CUDACheck(cudaMalloc(&channel_elevations_cu_, sizeof(CorrectionsV1_5::channel_elevations))); + CUDACheck(cudaMalloc(&dazis_cu, sizeof(CorrectionsV1_5::dazis))); + CUDACheck(cudaMalloc(&deles_cu, sizeof(CorrectionsV1_5::deles))); + CUDACheck(cudaMalloc(&mirror_azi_begins_cu, sizeof(CorrectionsV1_5::mirror_azi_begins))); + CUDACheck(cudaMalloc(&mirror_azi_ends_cu, sizeof(CorrectionsV1_5::mirror_azi_ends))); + CUDACheck(cudaMemcpy(channel_azimuths_cu_, corrections.channel_azimuths, sizeof(corrections.channel_azimuths), cudaMemcpyHostToDevice)); + CUDACheck(cudaMemcpy(channel_elevations_cu_, corrections.channel_elevations, sizeof(corrections.channel_elevations), cudaMemcpyHostToDevice)); + CUDACheck(cudaMemcpy(dazis_cu, corrections.dazis, sizeof(corrections.dazis), cudaMemcpyHostToDevice)); + CUDACheck(cudaMemcpy(deles_cu, corrections.deles, sizeof(corrections.deles), cudaMemcpyHostToDevice)); + CUDACheck(cudaMemcpy(mirror_azi_begins_cu, corrections.mirror_azi_begins, sizeof(corrections.mirror_azi_begins), cudaMemcpyHostToDevice)); + CUDACheck(cudaMemcpy(mirror_azi_ends_cu, corrections.mirror_azi_ends, sizeof(corrections.mirror_azi_ends), cudaMemcpyHostToDevice)); + for (auto i = 0; i < 128; ++i) { + channel_azimuths[i] = corrections.channel_azimuths[i] * corrections.header.resolution / kFineResolutionFloat; + // std::cout << corrections.channel_azimuths[i] * corrections.header.resolution / kFineResolutionFloat << std::endl; + } + break; + } + default: + std::cout << "Unknown Corrections Version !!!" << std::endl; + return -1; + } + corrections_loaded_ = true; + return 0; +} +template +int Udp4_3ParserGpu::LoadCorrectionFile(std::string lidar_correction_file) { + int ret = 0; + printf("load correction file from local correction.csv now!\n"); + std::ifstream fin(lidar_correction_file); + if (fin.is_open()) { + printf("Open correction file success\n"); + int length = 0; + std::string str_lidar_calibration; + fin.seekg(0, std::ios::end); + length = fin.tellg(); + fin.seekg(0, std::ios::beg); + char *buffer = new char[length]; + fin.read(buffer, length); + fin.close(); + str_lidar_calibration = buffer; + ret = LoadCorrectionString(buffer); + if (ret != 0) { + printf("Parse local Correction file Error\n"); + } else { + printf("Parse local Correction file Success!!!\n"); + return 0; + } + } else { + printf("Open correction file failed\n"); + return -1; + } + return -1; +} diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/src/udp6_1_parser_gpu.cu b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/src/udp6_1_parser_gpu.cu new file mode 100644 index 0000000..4b0d059 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/src/udp6_1_parser_gpu.cu @@ -0,0 +1,237 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ + +#include +#include +#include +#include "udp6_1_parser_gpu.h" +#include "safe_call.cuh" +#include "return_code.h" + +using namespace hesai::lidar; +template +Udp6_1ParserGpu::Udp6_1ParserGpu() { + corrections_loaded_ = false; + cudaSafeMalloc(raw_azimuths_cu_, sizeof(PointCloudStruct::azimuths)); + cudaSafeMalloc(raw_distances_cu_, sizeof(PointCloudStruct::distances)); + cudaSafeMalloc(raw_reflectivities_cu_, sizeof(PointCloudStruct::reflectivities)); + cudaSafeMalloc(raw_sensor_timestamp_cu_, sizeof(PointCloudStruct::sensor_timestamp)); +} +template +Udp6_1ParserGpu::~Udp6_1ParserGpu() { + cudaSafeFree(raw_azimuths_cu_); + cudaSafeFree(raw_distances_cu_); + cudaSafeFree(raw_reflectivities_cu_); + if (corrections_loaded_) { + cudaSafeFree(channel_elevations_cu_); + cudaSafeFree(channel_azimuths_cu_); + corrections_loaded_ = false; + } +} +template +__global__ void compute_xyzs_6_1_impl(T_Point *xyzs, const float* channel_azimuths, const float* channel_elevations, + const float* raw_azimuths, const uint16_t *raw_distances, const uint8_t *raw_reflectivities, + const uint64_t *raw_sensor_timestamp, const double raw_distance_unit, Transform transform, + int blocknum, int lasernum) { + auto iscan = blockIdx.x; + auto ichannel = threadIdx.x; + float azimuth = raw_azimuths[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))]; + auto theta = ((azimuth + channel_azimuths[(ichannel % lasernum)] * kResolutionInt)) / HALF_CIRCLE * M_PI; + float aziDelt = channel_azimuths[(ichannel % lasernum)] * kResolutionInt / HALF_CIRCLE * M_PI; + auto phi = (channel_elevations[(ichannel % lasernum)] * kResolutionInt) / HALF_CIRCLE * M_PI; + auto rho = raw_distances[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))] * raw_distance_unit; + + float x = 0.0f, y = 0.0f, z = 0.0f, r = 0.0f; + float b_, h_; + float aziCal = (int((azimuth + aziDelt) * HALF_CIRCLE / M_PI) % CIRCLE) / HALF_CIRCLE * M_PI; + if (rho <= 0.1) { + r = rho * cos(phi); + x = r * sin(aziCal); + y = r * cos(aziCal); + z = rho * sin(phi); + } else { + switch (blocknum) + { + //XTM + case 6: + b_ = 0.0130; + h_ = 0.0305; + break; + //XT + case 8: + b_ = 0.0130; + h_ = 0.0315; + break; + + default: + printf("default: never occur%d\n", blocknum); + break; + } + float aziCorrection = (int(aziDelt * HALF_CIRCLE / M_PI) % CIRCLE) / HALF_CIRCLE * M_PI; + float calDistance = rho - cos(phi) * (h_ * cos(aziCorrection) - b_ * sin(aziCorrection)); + x = calDistance * cos(phi) * sin(aziCal) - b_ * cos(azimuth) + h_ * sin(azimuth); + y = calDistance * cos(phi) * cos(aziCal) + b_ * sin(azimuth) + h_ * cos(azimuth); + z = calDistance * sin(phi); + } + + z = rho * sin(phi); + r = rho * cosf(phi); + x = r * sin(theta); + y = r * cos(theta); + + float cosa = std::cos(transform.roll); + float sina = std::sin(transform.roll); + float cosb = std::cos(transform.pitch); + float sinb = std::sin(transform.pitch); + float cosc = std::cos(transform.yaw); + float sinc = std::sin(transform.yaw); + + float x_ = cosb * cosc * x + (sina * sinb * cosc - cosa * sinc) * y + + (sina * sinc + cosa * sinb * cosc) * z + transform.x; + float y_ = cosb * sinc * x + (cosa * cosc + sina * sinb * sinc) * y + + (cosa * sinb * sinc - sina * cosc) * z + transform.y; + float z_ = -sinb * x + sina * cosb * y + cosa * cosb * z + transform.z; + gpu::setX(xyzs[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))], x_); + gpu::setY(xyzs[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))], y_); + gpu::setZ(xyzs[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))], z_); + gpu::setIntensity(xyzs[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))], raw_reflectivities[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))]); + gpu::setTimestamp(xyzs[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))], double(raw_sensor_timestamp[iscan]) / kMicrosecondToSecond); +} +template +int Udp6_1ParserGpu::ComputeXYZI(LidarDecodedFrame &frame) { + cudaSafeCall(cudaMemcpy(raw_azimuths_cu_, frame.azimuth, + kMaxPacketNumPerFrame * kMaxPointsNumPerPacket * sizeof(float), cudaMemcpyHostToDevice), + ReturnCode::CudaMemcpyHostToDeviceError); + cudaSafeCall(cudaMemcpy(raw_distances_cu_, frame.distances, + kMaxPacketNumPerFrame * kMaxPointsNumPerPacket * sizeof(uint16_t), + cudaMemcpyHostToDevice), + ReturnCode::CudaMemcpyHostToDeviceError); + cudaSafeCall(cudaMemcpy(raw_reflectivities_cu_, frame.reflectivities, + kMaxPacketNumPerFrame * kMaxPointsNumPerPacket * sizeof(uint8_t), + cudaMemcpyHostToDevice), + ReturnCode::CudaMemcpyHostToDeviceError); + cudaSafeCall(cudaMemcpy(raw_sensor_timestamp_cu_, frame.sensor_timestamp, + kMaxPacketNumPerFrame * sizeof(uint64_t), + cudaMemcpyHostToDevice), + ReturnCode::CudaMemcpyHostToDeviceError); +compute_xyzs_6_1_impl<<>>(this->frame_.gpu()->points, channel_azimuths_cu_, channel_elevations_cu_, + raw_azimuths_cu_, raw_distances_cu_, raw_reflectivities_cu_, raw_sensor_timestamp_cu_, frame.distance_unit, this->transform_, frame.block_num, frame.laser_num); + cudaSafeCall(cudaGetLastError(), ReturnCode::CudaXYZComputingError); + this->frame_.DeviceToHost(); + std::memcpy(frame.points, this->frame_.cpu()->points, sizeof(T_Point) * kMaxPacketNumPerFrame * kMaxPointsNumPerPacket); + return 0; +} +template +int Udp6_1ParserGpu::LoadCorrectionString(char *correction_content) { + if (corrections_loaded_) { + return 0; + if (channel_elevations_cu_) cudaFree(channel_elevations_cu_); + if (channel_azimuths_cu_) cudaFree(channel_azimuths_cu_); + corrections_loaded_ = false; + } + std::string correction_content_str = correction_content; + std::istringstream ifs(correction_content_str); + std::string line; + // skip first line "Laser id,Elevation,Azimuth" or "eeff" + std::getline(ifs, line); + + float elevation_list[MAX_LASER_NUM], azimuth_list[MAX_LASER_NUM]; + + std::vector vfirstLine; + boost::split(vfirstLine, line, boost::is_any_of(",")); + if (vfirstLine[0] == "EEFF" || vfirstLine[0] == "eeff") { + // skip second line + std::getline(ifs, line); + } + + int lineCount = 0; + while (std::getline(ifs, line)) { + std::vector vLineSplit; + boost::split(vLineSplit, line, boost::is_any_of(",")); + // skip error line or hash value line + if (vLineSplit.size() < 3) { + continue; + } else { + lineCount++; + } + float elevation, azimuth; + int laserId = 0; + + std::stringstream ss(line); + std::string subline; + std::getline(ss, subline, ','); + std::stringstream(subline) >> laserId; + std::getline(ss, subline, ','); + std::stringstream(subline) >> elevation; + std::getline(ss, subline, ','); + std::stringstream(subline) >> azimuth; + + if (laserId != lineCount || laserId >= MAX_LASER_NUM) { + std::cout << "laser id is wrong in correction file. laser Id:" + << laserId << ", line" << lineCount << std::endl; + return -1; + } + elevation_list[laserId - 1] = elevation; + azimuth_list[laserId - 1] = azimuth; +} + CUDACheck(cudaMalloc(&channel_azimuths_cu_, sizeof(float) * MAX_LASER_NUM)); + CUDACheck(cudaMalloc(&channel_elevations_cu_, sizeof(float) * MAX_LASER_NUM)); + CUDACheck(cudaMemcpy(channel_azimuths_cu_, azimuth_list, sizeof(float) * MAX_LASER_NUM, cudaMemcpyHostToDevice)); + CUDACheck(cudaMemcpy(channel_elevations_cu_, elevation_list, sizeof(float) * MAX_LASER_NUM, cudaMemcpyHostToDevice)); + corrections_loaded_ = true; + return 0; +} +template +int Udp6_1ParserGpu::LoadCorrectionFile(std::string lidar_correction_file) { + int ret = 0; + printf("load correction file from local correction.csv now!\n"); + std::ifstream fin(lidar_correction_file); + if (fin.is_open()) { + printf("Open correction file success\n"); + int length = 0; + std::string str_lidar_calibration; + fin.seekg(0, std::ios::end); + length = fin.tellg(); + fin.seekg(0, std::ios::beg); + char *buffer = new char[length]; + fin.read(buffer, length); + fin.close(); + str_lidar_calibration = buffer; + ret = LoadCorrectionString(buffer); + if (ret != 0) { + printf("Parse local Correction file Error\n"); + } else { + printf("Parse local Correction file Success!!!\n"); + return 0; + } + } else { + printf("Open correction file failed\n"); + return -1; + } + return -1; +} diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/src/udp7_2_parser_gpu.cu b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/src/udp7_2_parser_gpu.cu new file mode 100644 index 0000000..5acb8a9 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/src/udp7_2_parser_gpu.cu @@ -0,0 +1,292 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ + +#include +#include +#include +#include "udp7_2_parser_gpu.h" +#include "safe_call.cuh" +#include "return_code.h" + +using namespace hesai::lidar; +template +Udp7_2ParserGpu::Udp7_2ParserGpu() { + corrections_loaded_ = false; + cudaSafeMalloc(raw_azimuths_cu_, kMaxPacketNumPerFrame * kMaxPointsNumPerPacket * sizeof(float)); + cudaSafeMalloc(raw_elevations_cu, kMaxPacketNumPerFrame * kMaxPointsNumPerPacket * sizeof(float)); + cudaSafeMalloc(raw_distances_cu_, sizeof(PointCloudStruct::distances)); + cudaSafeMalloc(raw_reflectivities_cu_, sizeof(PointCloudStruct::reflectivities)); + cudaSafeMalloc(raw_sensor_timestamp_cu_, sizeof(PointCloudStruct::sensor_timestamp)); +} +template +Udp7_2ParserGpu::~Udp7_2ParserGpu() { + cudaSafeFree(raw_azimuths_cu_); + cudaSafeFree(raw_elevations_cu); + cudaSafeFree(raw_distances_cu_); + cudaSafeFree(raw_reflectivities_cu_); + if (corrections_loaded_) { + cudaSafeFree(channel_elevations_cu_); + cudaSafeFree(channel_azimuths_cu_); + corrections_loaded_ = false; + } +} +template +__global__ void compute_xyzs_7_2_impl(T_Point *xyzs, const float* channel_azimuths, const float* channel_elevations, + float* raw_azimuths, float* raw_elevations, const uint16_t *raw_distances, const uint8_t *raw_reflectivities, + const uint64_t *raw_sensor_timestamp, const double raw_distance_unit, Transform transform, + int blocknum, int lasernum) { + auto iscan = blockIdx.x; + auto ichannel = threadIdx.x; + float azimuth = raw_azimuths[iscan * blocknum * lasernum + ichannel] / HALF_CIRCLE * M_PI; + float elevation = raw_elevations[iscan * blocknum * lasernum + ichannel] / HALF_CIRCLE * M_PI; + + auto rho = raw_distances[iscan * blocknum * lasernum + ichannel] * raw_distance_unit; + float z = rho * sin(elevation); + auto r = rho * cosf(elevation); + float x = r * sin(azimuth); + float y = r * cos(azimuth); + + float cosa = std::cos(transform.roll); + float sina = std::sin(transform.roll); + float cosb = std::cos(transform.pitch); + float sinb = std::sin(transform.pitch); + float cosc = std::cos(transform.yaw); + float sinc = std::sin(transform.yaw); + + float x_ = cosb * cosc * x + (sina * sinb * cosc - cosa * sinc) * y + + (sina * sinc + cosa * sinb * cosc) * z + transform.x; + float y_ = cosb * sinc * x + (cosa * cosc + sina * sinb * sinc) * y + + (cosa * sinb * sinc - sina * cosc) * z + transform.y; + float z_ = -sinb * x + sina * cosb * y + cosa * cosb * z + transform.z; + gpu::setX(xyzs[iscan * blocknum * lasernum + ichannel], x_); + gpu::setY(xyzs[iscan * blocknum * lasernum + ichannel], y_); + gpu::setZ(xyzs[iscan * blocknum * lasernum + ichannel], z_); + gpu::setIntensity(xyzs[iscan * blocknum * lasernum + ichannel], raw_reflectivities[iscan * blocknum * lasernum + ichannel]); + gpu::setTimestamp(xyzs[iscan * blocknum * lasernum + ichannel], double(raw_sensor_timestamp[iscan]) / kMicrosecondToSecond); +} + +template +int Udp7_2ParserGpu::ComputeXYZI(LidarDecodedFrame &frame) { + if (!corrections_loaded_) return int(ReturnCode::CorrectionsUnloaded); + cudaSafeCall(cudaMemcpy(raw_azimuths_cu_, frame.azimuth, + kMaxPacketNumPerFrame * kMaxPointsNumPerPacket * sizeof(float), cudaMemcpyHostToDevice), + ReturnCode::CudaMemcpyHostToDeviceError); + cudaSafeCall(cudaMemcpy(raw_elevations_cu, frame.elevation, + kMaxPacketNumPerFrame * kMaxPointsNumPerPacket * sizeof(float), cudaMemcpyHostToDevice), + ReturnCode::CudaMemcpyHostToDeviceError); + cudaSafeCall(cudaMemcpy(raw_distances_cu_, frame.distances, + kMaxPacketNumPerFrame * kMaxPointsNumPerPacket * sizeof(uint16_t), + cudaMemcpyHostToDevice), + ReturnCode::CudaMemcpyHostToDeviceError); + cudaSafeCall(cudaMemcpy(raw_reflectivities_cu_, frame.reflectivities, + kMaxPacketNumPerFrame * kMaxPointsNumPerPacket, + cudaMemcpyHostToDevice), + ReturnCode::CudaMemcpyHostToDeviceError); + cudaSafeCall(cudaMemcpy(raw_sensor_timestamp_cu_, frame.sensor_timestamp, + kMaxPacketNumPerFrame * sizeof(uint64_t), + cudaMemcpyHostToDevice), + ReturnCode::CudaMemcpyHostToDeviceError); + + +compute_xyzs_7_2_impl<<>>(this->frame_.gpu()->points, channel_azimuths_cu_, channel_elevations_cu_, + raw_azimuths_cu_, raw_elevations_cu, raw_distances_cu_, raw_reflectivities_cu_, raw_sensor_timestamp_cu_, frame.distance_unit, this->transform_, frame.block_num, frame.laser_num); + cudaSafeCall(cudaGetLastError(), ReturnCode::CudaXYZComputingError); + this->frame_.DeviceToHost(); + std::memcpy(frame.points, this->frame_.cpu()->points, sizeof(T_Point) * kMaxPacketNumPerFrame * kMaxPointsNumPerPacket); + return 0; +} +template +int Udp7_2ParserGpu::LoadCorrectionString(char *data) { + if (LoadCorrectionDatData(data)) { + return 0; + } + return LoadCorrectionCsvData(data); +} + +template +int Udp7_2ParserGpu::LoadCorrectionCsvData(char *correction_string) { + std::istringstream ifs(correction_string); + std::string line; + // first line "Laser id,Elevation,Azimuth" + if(std::getline(ifs, line)) { + printf("Parse Lidar Correction...\n"); + } + int lineCounter = 0; + std::vector firstLine; + boost::split(firstLine, line, boost::is_any_of(",")); + float elevations[CHANNEL_MAX][COLUMN_MAX]; + float azimuths[CHANNEL_MAX][COLUMN_MAX]; + while (std::getline(ifs, line)) { + if(line.length() < strlen("1,1,1,1")) { + return -1; + } + else { + lineCounter++; + } + float elev, azimuth; + int lineId = 0; + int cloumnId = 0; + std::stringstream ss(line); + std::string subline; + std::getline(ss, subline, ','); + std::stringstream(subline) >> lineId; + std::getline(ss, subline, ','); + std::stringstream(subline) >> cloumnId; + std::getline(ss, subline, ','); + std::stringstream(subline) >> elev; + std::getline(ss, subline, ','); + std::stringstream(subline) >> azimuth; + elevations[lineId - 1][cloumnId - 1] = elev * 100; + azimuths[lineId - 1][cloumnId - 1] = azimuth * 100; + } + CUDACheck(cudaMalloc(&channel_azimuths_cu_, sizeof(azimuths))); + CUDACheck(cudaMalloc(&channel_elevations_cu_, sizeof(elevations))); + CUDACheck(cudaMemcpy(channel_azimuths_cu_, azimuths, sizeof(azimuths), cudaMemcpyHostToDevice)); + CUDACheck(cudaMemcpy(channel_elevations_cu_, elevations, sizeof(elevations), cudaMemcpyHostToDevice)); + corrections_loaded_ = true; + return 0; +} + +template +int Udp7_2ParserGpu::LoadCorrectionDatData(char *correction_string) { + float elevations[CHANNEL_MAX][COLUMN_MAX]; + float azimuths[CHANNEL_MAX][COLUMN_MAX]; + try { + char *p = correction_string; + PandarFTCorrectionsHeader header = *(PandarFTCorrectionsHeader *)p; + if (0xee == header.pilot[0] && 0xff == header.pilot[1]) { + switch (header.version[1]) { + case 0: { + int column_num = header.column_number; + int channel_num = header.channel_number; + int resolution = header.resolution; + float fResolution = float(resolution); + int angleNum = column_num * channel_num; + int doubleAngleNum = angleNum * 2; + int16_t* angles = new int16_t[doubleAngleNum]{0}; + int readLen = sizeof(int16_t) * doubleAngleNum; + memcpy((void*)angles, correction_string, readLen); + int hashLen = 32; + uint8_t* hashValue = new uint8_t[hashLen]; + memcpy((void*)hashValue, correction_string + readLen, hashLen); + for (int row = 0; row < column_num; row++) { + for (int col = 0; col < channel_num; col++) { + int idx = row * channel_num + col; + azimuths[col][row] = angles[idx] * fResolution; + } + } + + for (int row = 0; row < column_num; row++) { + for (int col = 0; col < channel_num; col++) { + int idx = angleNum + row * channel_num + col; + elevations[col][row] = angles[idx] * fResolution; + } + } + CUDACheck(cudaMalloc(&channel_azimuths_cu_, sizeof(azimuths))); + CUDACheck(cudaMalloc(&channel_elevations_cu_, sizeof(elevations))); + CUDACheck(cudaMemcpy(channel_azimuths_cu_, azimuths, sizeof(azimuths), cudaMemcpyHostToDevice)); + CUDACheck(cudaMemcpy(channel_elevations_cu_, elevations, sizeof(elevations), cudaMemcpyHostToDevice)); + corrections_loaded_ = true; + return 0; + } break; + case 1: { + int column_num = header.column_number; + int channel_num = header.channel_number; + int resolution = header.resolution; + float fResolution = float(resolution); + int angleNum = column_num * channel_num; + int doubleAngleNum = angleNum * 2; + int32_t* angles = new int32_t[doubleAngleNum]{0}; + int readLen = sizeof(int32_t) * doubleAngleNum; + memcpy((void*)angles, correction_string + sizeof(PandarFTCorrectionsHeader), readLen); + int hashLen = 32; + uint8_t* hashValue = new uint8_t[hashLen]; + memcpy((void*)hashValue, correction_string + readLen + sizeof(PandarFTCorrectionsHeader), hashLen); + for (int row = 0; row < column_num; row++) { + for (int col = 0; col < channel_num; col++) { + int idx = row * channel_num + col; + azimuths[col][row] = angles[idx] * fResolution; + } + } + + for (int row = 0; row < column_num; row++) { + for (int col = 0; col < channel_num; col++) { + int idx = angleNum + row * channel_num + col; + elevations[col][row] = angles[idx] * fResolution; + } + } + CUDACheck(cudaMalloc(&channel_azimuths_cu_, sizeof(azimuths))); + CUDACheck(cudaMalloc(&channel_elevations_cu_, sizeof(elevations))); + CUDACheck(cudaMemcpy(channel_azimuths_cu_, azimuths, sizeof(azimuths), cudaMemcpyHostToDevice)); + CUDACheck(cudaMemcpy(channel_elevations_cu_, elevations, sizeof(elevations), cudaMemcpyHostToDevice)); + corrections_loaded_ = true; + return 0; + } break; + default: + break; + } + } + + return -1; + } catch (const std::exception &e) { + std::cerr << e.what() << '\n'; + return -1; + } + + return -1; + +} +template +int Udp7_2ParserGpu::LoadCorrectionFile(std::string lidar_correction_file) { + int ret = 0; + printf("load correction file from local correction.csv now!\n"); + std::ifstream fin(lidar_correction_file); + if (fin.is_open()) { + printf("Open correction file success\n"); + int length = 0; + std::string str_lidar_calibration; + fin.seekg(0, std::ios::end); + length = fin.tellg(); + fin.seekg(0, std::ios::beg); + char *buffer = new char[length]; + fin.read(buffer, length); + fin.close(); + str_lidar_calibration = buffer; + ret = LoadCorrectionString(buffer); + if (ret != 0) { + printf("Parse local Correction file Error\n"); + } else { + printf("Parse local Correction file Success!!!\n"); + return 0; + } + } else { + printf("Open correction file failed\n"); + return -1; + } + return -1; +} diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/src/udp_p40_parser_gpu.cu b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/src/udp_p40_parser_gpu.cu new file mode 100644 index 0000000..84d8b18 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/src/udp_p40_parser_gpu.cu @@ -0,0 +1,206 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ + +#include +#include +#include + +#include "udp_p40_parser_gpu.h" +#include "safe_call.cuh" +#include "return_code.h" + +using namespace hesai::lidar; +template +UdpP40ParserGpu::UdpP40ParserGpu() { + corrections_loaded_ = false; + cudaSafeMalloc(raw_azimuths_cu_, sizeof(PointCloudStruct::azimuths)); + cudaSafeMalloc(raw_distances_cu_, sizeof(PointCloudStruct::distances)); + cudaSafeMalloc(raw_reflectivities_cu_, sizeof(PointCloudStruct::reflectivities)); + cudaSafeMalloc(raw_sensor_timestamp_cu_, sizeof(PointCloudStruct::sensor_timestamp)); +} +template +UdpP40ParserGpu::~UdpP40ParserGpu() { + cudaSafeFree(raw_azimuths_cu_); + cudaSafeFree(raw_distances_cu_); + cudaSafeFree(raw_reflectivities_cu_); + cudaSafeFree(raw_sensor_timestamp_cu_); + if (corrections_loaded_) { + cudaSafeFree(channel_elevations_cu_); + cudaSafeFree(channel_azimuths_cu_); + corrections_loaded_ = false; + } +} +template +__global__ void compute_xyzs_p40_impl(T_Point *xyzs, const float* channel_azimuths, const float* channel_elevations, + const float* raw_azimuths, const uint16_t *raw_distances, const uint8_t *raw_reflectivities, + const uint64_t *raw_sensor_timestamp, const double raw_distance_unit, Transform transform, const uint16_t blocknum, uint16_t lasernum) { + auto iscan = blockIdx.x; + auto ichannel = threadIdx.x; + float azimuth = raw_azimuths[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))]; + auto theta = ((azimuth + channel_azimuths[(ichannel % lasernum)] * kResolutionInt)) / + HALF_CIRCLE * M_PI; + auto phi = (channel_elevations[(ichannel % lasernum)] * kResolutionInt) / HALF_CIRCLE * M_PI; + + auto rho = raw_distances[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))] * raw_distance_unit; + float z = rho * sin(phi); + auto r = rho * cosf(phi); + float x = r * sin(theta); + float y = r * cos(theta); + + float cosa = std::cos(transform.roll); + float sina = std::sin(transform.roll); + float cosb = std::cos(transform.pitch); + float sinb = std::sin(transform.pitch); + float cosc = std::cos(transform.yaw); + float sinc = std::sin(transform.yaw); + + float x_ = cosb * cosc * x + (sina * sinb * cosc - cosa * sinc) * y + + (sina * sinc + cosa * sinb * cosc) * z + transform.x; + float y_ = cosb * sinc * x + (cosa * cosc + sina * sinb * sinc) * y + + (cosa * sinb * sinc - sina * cosc) * z + transform.y; + float z_ = -sinb * x + sina * cosb * y + cosa * cosb * z + transform.z; + gpu::setX(xyzs[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))], x_); + gpu::setY(xyzs[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))], y_); + gpu::setZ(xyzs[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))], z_); + gpu::setIntensity(xyzs[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))], raw_reflectivities[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))]); + gpu::setTimestamp(xyzs[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))], double(raw_sensor_timestamp[iscan]) / kMicrosecondToSecond); +} +template +int UdpP40ParserGpu::ComputeXYZI(LidarDecodedFrame &frame) { + if (!corrections_loaded_) return int(ReturnCode::CorrectionsUnloaded); + cudaSafeCall(cudaMemcpy(raw_azimuths_cu_, frame.azimuth, + kMaxPacketNumPerFrame * kMaxPointsNumPerPacket * sizeof(float), cudaMemcpyHostToDevice), + ReturnCode::CudaMemcpyHostToDeviceError); + cudaSafeCall(cudaMemcpy(raw_distances_cu_, frame.distances, + kMaxPacketNumPerFrame * kMaxPointsNumPerPacket * sizeof(uint16_t), + cudaMemcpyHostToDevice), + ReturnCode::CudaMemcpyHostToDeviceError); + cudaSafeCall(cudaMemcpy(raw_reflectivities_cu_, frame.reflectivities, + kMaxPacketNumPerFrame * kMaxPointsNumPerPacket, + cudaMemcpyHostToDevice), + ReturnCode::CudaMemcpyHostToDeviceError); + cudaSafeCall(cudaMemcpy(raw_sensor_timestamp_cu_, frame.sensor_timestamp, + kMaxPacketNumPerFrame * sizeof(uint64_t), + cudaMemcpyHostToDevice), + ReturnCode::CudaMemcpyHostToDeviceError); +compute_xyzs_p40_impl<<>>(this->frame_.gpu()->points, channel_azimuths_cu_, channel_elevations_cu_, + raw_azimuths_cu_, raw_distances_cu_, raw_reflectivities_cu_, raw_sensor_timestamp_cu_, frame.distance_unit, + this->transform_, frame.block_num, frame.laser_num); + + cudaSafeCall(cudaGetLastError(), ReturnCode::CudaXYZComputingError); + this->frame_.DeviceToHost(); + std::memcpy(frame.points, this->frame_.cpu()->points, sizeof(T_Point) * kMaxPacketNumPerFrame * kMaxPointsNumPerPacket); + return 0; +} +template +int UdpP40ParserGpu::LoadCorrectionString(char *correction_content) { + if (corrections_loaded_) { + return 0; + if (channel_elevations_cu_) cudaFree(channel_elevations_cu_); + if (channel_azimuths_cu_) cudaFree(channel_azimuths_cu_); + corrections_loaded_ = false; + } + std::string correction_content_str = correction_content; + std::istringstream ifs(correction_content_str); + std::string line; + // skip first line "Laser id,Elevation,Azimuth" or "eeff" + std::getline(ifs, line); + + float elevation_list[MAX_LASER_NUM], azimuth_list[MAX_LASER_NUM]; + + std::vector vfirstLine; + boost::split(vfirstLine, line, boost::is_any_of(",")); + if (vfirstLine[0] == "EEFF" || vfirstLine[0] == "eeff") { + // skip second line + std::getline(ifs, line); + } + + int lineCount = 0; + while (std::getline(ifs, line)) { + std::vector vLineSplit; + boost::split(vLineSplit, line, boost::is_any_of(",")); + // skip error line or hash value line + if (vLineSplit.size() < 3) { + continue; + } else { + lineCount++; + } + float elevation, azimuth; + int laserId = 0; + std::stringstream ss(line); + std::string subline; + std::getline(ss, subline, ','); + std::stringstream(subline) >> laserId; + std::getline(ss, subline, ','); + std::stringstream(subline) >> elevation; + std::getline(ss, subline, ','); + std::stringstream(subline) >> azimuth; + if (laserId != lineCount || laserId >= MAX_LASER_NUM) { + std::cout << "laser id is wrong in correction file. laser Id:" + << laserId << ", line" << lineCount << std::endl; + return -1; + } + elevation_list[laserId - 1] = elevation; + azimuth_list[laserId - 1] = azimuth; + } + + CUDACheck(cudaMalloc(&channel_azimuths_cu_, sizeof(float) * MAX_LASER_NUM)); + CUDACheck(cudaMalloc(&channel_elevations_cu_, sizeof(float) * MAX_LASER_NUM)); + CUDACheck(cudaMemcpy(channel_azimuths_cu_, azimuth_list, sizeof(float) * MAX_LASER_NUM, cudaMemcpyHostToDevice)); + CUDACheck(cudaMemcpy(channel_elevations_cu_, elevation_list, sizeof(float) * MAX_LASER_NUM, cudaMemcpyHostToDevice)); + corrections_loaded_ = true; + return 0; +} +template +int UdpP40ParserGpu::LoadCorrectionFile(std::string lidar_correction_file) { + int ret = 0; + printf("load correction file from local correction.csv now!\n"); + std::ifstream fin(lidar_correction_file); + if (fin.is_open()) { + int length = 0; + std::string str_lidar_calibration; + fin.seekg(0, std::ios::end); + length = fin.tellg(); + fin.seekg(0, std::ios::beg); + char *buffer = new char[length]; + fin.read(buffer, length); + fin.close(); + str_lidar_calibration = buffer; + ret = LoadCorrectionString(buffer); + if (ret != 0) { + printf("Parse local Correction file Error\n"); + } else { + printf("Parse local Correction file Success!!!\n"); + return 0; + } + } else { + printf("Open correction file failed\n"); + return -1; + } + return -1; +} diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/src/udp_p64_parser_gpu.cu b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/src/udp_p64_parser_gpu.cu new file mode 100644 index 0000000..0f3ff7f --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/src/udp_p64_parser_gpu.cu @@ -0,0 +1,207 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ + +#include +#include +#include +#include "udp_p64_parser_gpu.h" +#include "safe_call.cuh" +#include "return_code.h" + +using namespace hesai::lidar; +template +UdpP64ParserGpu::UdpP64ParserGpu() { + corrections_loaded_ = false; + cudaSafeMalloc(raw_azimuths_cu_, sizeof(PointCloudStruct::azimuths)); + cudaSafeMalloc(raw_distances_cu_, sizeof(PointCloudStruct::distances)); + cudaSafeMalloc(raw_reflectivities_cu_, sizeof(PointCloudStruct::reflectivities)); + cudaSafeMalloc(raw_sensor_timestamp_cu_, sizeof(PointCloudStruct::sensor_timestamp)); +} +template +UdpP64ParserGpu::~UdpP64ParserGpu() { + cudaSafeFree(raw_azimuths_cu_); + cudaSafeFree(raw_distances_cu_); + cudaSafeFree(raw_reflectivities_cu_); + cudaSafeFree(raw_sensor_timestamp_cu_); + if (corrections_loaded_) { + cudaSafeFree(channel_elevations_cu_); + cudaSafeFree(channel_azimuths_cu_); + corrections_loaded_ = false; + } +} +template +__global__ void compute_xyzs_p64_impl(T_Point *xyzs, const float* channel_azimuths, const float* channel_elevations, + const float* raw_azimuths, const uint16_t *raw_distances, const uint8_t *raw_reflectivities, + const uint64_t *raw_sensor_timestamp, const double raw_distance_unit, Transform transform, const uint16_t blocknum, uint16_t lasernum) { + auto iscan = blockIdx.x; + auto ichannel = threadIdx.x; + float azimuth = raw_azimuths[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))]; + auto theta = ((azimuth + channel_azimuths[(ichannel % lasernum)] * kResolutionInt)) / + HALF_CIRCLE * M_PI; + auto phi = (channel_elevations[(ichannel % lasernum)] * kResolutionInt) / HALF_CIRCLE * M_PI; + + auto rho = raw_distances[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))] * raw_distance_unit; + float z = rho * sin(phi); + auto r = rho * cosf(phi); + float x = r * sin(theta); + float y = r * cos(theta); + + float cosa = std::cos(transform.roll); + float sina = std::sin(transform.roll); + float cosb = std::cos(transform.pitch); + float sinb = std::sin(transform.pitch); + float cosc = std::cos(transform.yaw); + float sinc = std::sin(transform.yaw); + + float x_ = cosb * cosc * x + (sina * sinb * cosc - cosa * sinc) * y + + (sina * sinc + cosa * sinb * cosc) * z + transform.x; + float y_ = cosb * sinc * x + (cosa * cosc + sina * sinb * sinc) * y + + (cosa * sinb * sinc - sina * cosc) * z + transform.y; + float z_ = -sinb * x + sina * cosb * y + cosa * cosb * z + transform.z; + gpu::setX(xyzs[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))], x_); + gpu::setY(xyzs[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))], y_); + gpu::setZ(xyzs[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))], z_); + gpu::setIntensity(xyzs[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))], raw_reflectivities[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))]); + gpu::setTimestamp(xyzs[iscan * blocknum * lasernum + (ichannel % (lasernum * blocknum))], double(raw_sensor_timestamp[iscan]) / kMicrosecondToSecond); +} +template +int UdpP64ParserGpu::ComputeXYZI(LidarDecodedFrame &frame) { + if (!corrections_loaded_) return int(ReturnCode::CorrectionsUnloaded); + cudaSafeCall(cudaMemcpy(raw_azimuths_cu_, frame.azimuth, + kMaxPacketNumPerFrame * kMaxPointsNumPerPacket * sizeof(float), cudaMemcpyHostToDevice), + ReturnCode::CudaMemcpyHostToDeviceError); + cudaSafeCall(cudaMemcpy(raw_distances_cu_, frame.distances, + kMaxPacketNumPerFrame * kMaxPointsNumPerPacket * sizeof(uint16_t), + cudaMemcpyHostToDevice), + ReturnCode::CudaMemcpyHostToDeviceError); + cudaSafeCall(cudaMemcpy(raw_reflectivities_cu_, frame.reflectivities, + kMaxPacketNumPerFrame * kMaxPointsNumPerPacket, + cudaMemcpyHostToDevice), + ReturnCode::CudaMemcpyHostToDeviceError); + cudaSafeCall(cudaMemcpy(raw_sensor_timestamp_cu_, frame.sensor_timestamp, + kMaxPacketNumPerFrame * sizeof(uint64_t), + cudaMemcpyHostToDevice), + ReturnCode::CudaMemcpyHostToDeviceError); +compute_xyzs_p64_impl<<>>(this->frame_.gpu()->points, channel_azimuths_cu_, channel_elevations_cu_, + raw_azimuths_cu_, raw_distances_cu_, raw_reflectivities_cu_, raw_sensor_timestamp_cu_, frame.distance_unit, + this->transform_, frame.block_num, frame.laser_num); + + cudaSafeCall(cudaGetLastError(), ReturnCode::CudaXYZComputingError); + this->frame_.DeviceToHost(); + std::memcpy(frame.points, this->frame_.cpu()->points, sizeof(T_Point) * kMaxPacketNumPerFrame * kMaxPointsNumPerPacket); + return 0; +} +template +int UdpP64ParserGpu::LoadCorrectionString(char *correction_content) { + if (corrections_loaded_) { + return 0; + if (channel_elevations_cu_) cudaFree(channel_elevations_cu_); + if (channel_azimuths_cu_) cudaFree(channel_azimuths_cu_); + corrections_loaded_ = false; + } + std::string correction_content_str = correction_content; + std::istringstream ifs(correction_content_str); + std::string line; + // skip first line "Laser id,Elevation,Azimuth" or "eeff" + std::getline(ifs, line); + + float elevation_list[MAX_LASER_NUM], azimuth_list[MAX_LASER_NUM]; + + std::vector vfirstLine; + boost::split(vfirstLine, line, boost::is_any_of(",")); + if (vfirstLine[0] == "EEFF" || vfirstLine[0] == "eeff") { + // skip second line + std::getline(ifs, line); + } + + int lineCount = 0; + while (std::getline(ifs, line)) { + std::vector vLineSplit; + boost::split(vLineSplit, line, boost::is_any_of(",")); + // skip error line or hash value line + if (vLineSplit.size() < 3) { + continue; + } else { + lineCount++; + } + float elevation, azimuth; + int laserId = 0; + + std::stringstream ss(line); + std::string subline; + std::getline(ss, subline, ','); + std::stringstream(subline) >> laserId; + std::getline(ss, subline, ','); + std::stringstream(subline) >> elevation; + std::getline(ss, subline, ','); + std::stringstream(subline) >> azimuth; + + if (laserId != lineCount || laserId >= MAX_LASER_NUM) { + std::cout << "laser id is wrong in correction file. laser Id:" + << laserId << ", line" << lineCount << std::endl; + return -1; + } + elevation_list[laserId - 1] = elevation; + azimuth_list[laserId - 1] = azimuth; +} + CUDACheck(cudaMalloc(&channel_azimuths_cu_, sizeof(float) * MAX_LASER_NUM)); + CUDACheck(cudaMalloc(&channel_elevations_cu_, sizeof(float) * MAX_LASER_NUM)); + CUDACheck(cudaMemcpy(channel_azimuths_cu_, azimuth_list, sizeof(float) * MAX_LASER_NUM, cudaMemcpyHostToDevice)); + CUDACheck(cudaMemcpy(channel_elevations_cu_, elevation_list, sizeof(float) * MAX_LASER_NUM, cudaMemcpyHostToDevice)); + corrections_loaded_ = true; + return 0; +} +template +int UdpP64ParserGpu::LoadCorrectionFile(std::string lidar_correction_file) { + int ret = 0; + printf("load correction file from local correction.csv now!\n"); + std::ifstream fin(lidar_correction_file); + if (fin.is_open()) { + printf("Open correction file success\n"); + int length = 0; + std::string str_lidar_calibration; + fin.seekg(0, std::ios::end); + length = fin.tellg(); + fin.seekg(0, std::ios::beg); + char *buffer = new char[length]; + fin.read(buffer, length); + fin.close(); + str_lidar_calibration = buffer; + ret = LoadCorrectionString(buffer); + if (ret != 0) { + printf("Parse local Correction file Error\n"); + } else { + printf("Parse local Correction file Success!!!\n"); + return 0; + } + } else { + printf("Open correction file failed\n"); + return -1; + } + return -1; +} diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/udp_parser_gpu.cu b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/udp_parser_gpu.cu new file mode 100644 index 0000000..ae5ca6c --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/udp_parser_gpu.cu @@ -0,0 +1,241 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ +#include "udp_parser_gpu.h" +#include "udp_p40_parser_gpu.h" +#include "udp_p64_parser_gpu.h" +#include "udp4_3_parser_gpu.h" +#include "udp1_4_parser_gpu.h" +#include "udp3_1_parser_gpu.h" +#include "udp3_2_parser_gpu.h" +#include "udp6_1_parser_gpu.h" +#include "udp7_2_parser_gpu.h" +#include "udp2_5_parser_gpu.h" + +using namespace hesai::lidar; +template +int UdpParserGpu::LoadCorrectionString(char *correction_content) { + if (m_generalParserGpu != nullptr) { + return m_generalParserGpu->LoadCorrectionString(correction_content); + } + else { + printf("m_generalParserGpu is nullptr\n"); + return -1; + } + return 0; +} +template +int UdpParserGpu::LoadCorrectionFile(std::string correction_path) { + if (m_generalParserGpu != nullptr) { + return m_generalParserGpu->LoadCorrectionFile(correction_path); + } + else { + printf("m_generalParserGpu is nullptr\n"); + return -1; + } + return 0; +} + + +template +int UdpParserGpu::LoadFiretimesString(char *correction_string) { + if (m_generalParserGpu != nullptr) { + m_generalParserGpu->LoadFiretimesString(correction_string); + } + else { + printf("m_generalParserGpu is nullptr\n"); + return -1; + } + return 0; +} +template +void UdpParserGpu::LoadFiretimesFile(std::string firetimes_path) { + if (m_generalParserGpu != nullptr) { + m_generalParserGpu->LoadFiretimesFile(firetimes_path); + } + else { + printf("m_generalParserGpu is nullptr\n"); + } +} +template +void UdpParserGpu::CreatGeneralParser(uint8_t major, uint8_t minor) { + // QMutexLocker locker(&this->m_mutex); + if (m_generalParserGpu != nullptr) { + return; + } + + switch (major) { + // AT128 + case 4: + { + switch (minor) { + case 1: + case 3: + m_generalParserGpu = new Udp4_3ParserGpu(); + break; + default: + break; + } + + } break; + // Pandar128E3X + case 1: + { + switch (minor) { + case 1: + case 4: + m_generalParserGpu = new Udp1_4ParserGpu(); + break; + default: + break; + } + + } break; + // ET + case 2: + { + switch (minor) { + case 5: + m_generalParserGpu = new Udp2_5ParserGpu(); + break; + default: + break; + } + + } break; + // PandarQt + case 3: + { + switch (minor) { + case 1: + m_generalParserGpu = new Udp3_1ParserGpu(); + case 2: + m_generalParserGpu = new Udp3_2ParserGpu(); + break; + default: + break; + } + + } break; + // PandarXt + case 6: + { + switch (minor) { + case 1: + m_generalParserGpu = new Udp6_1ParserGpu(); + break; + default: + break; + } + + } break; + // PandarFt + case 7: + { + switch (minor) { + case 2: + m_generalParserGpu = new Udp7_2ParserGpu(); + break; + default: + break; + } + + } break; + default: + break; + } +} +template +int UdpParserGpu::ComputeXYZI(LidarDecodedFrame &frame) { + if (m_generalParserGpu == nullptr) { + this->CreatGeneralParser(frame.major_version, frame.minor_version); + } + m_generalParserGpu->ComputeXYZI(frame); + return 0; +} +template +void UdpParserGpu::SetLidarType(std::string lidar_type) { + if (m_generalParserGpu != nullptr) { + return; + } + if (lidar_type == "Pandar40" || lidar_type == "Pandar40P") { + m_generalParserGpu = new UdpP40ParserGpu(); + } + if (lidar_type == "Pandar64") { + m_generalParserGpu = new UdpP64ParserGpu(); + } + if (lidar_type == "AT128" || lidar_type == "AT128E2X" || lidar_type == "AT128E3X") { + m_generalParserGpu = new Udp4_3ParserGpu(); + } + if (lidar_type == "Pandar128E3X" || lidar_type == "Pandar128") { + m_generalParserGpu = new Udp1_4ParserGpu(); + } + if (lidar_type == "Pandar90" || lidar_type == "Pandar90E3X") { + m_generalParserGpu = new Udp1_4ParserGpu(); + } + if (lidar_type == "Pandar64S" || lidar_type == "Pandar64E3X") { + m_generalParserGpu = new Udp1_4ParserGpu(); + } + if (lidar_type == "Pandar40S" || lidar_type == "Pandar40E3X") { + m_generalParserGpu = new Udp1_4ParserGpu(); + } + if (lidar_type == "PandarQT") { + m_generalParserGpu = new Udp3_1ParserGpu(); + } + if (lidar_type == "PandarQT128" || lidar_type == "QT128C2X") { + m_generalParserGpu = new Udp3_2ParserGpu(); + } + if (lidar_type == "PandarXT") { + m_generalParserGpu = new Udp6_1ParserGpu(); + } + if (lidar_type == "PandarXT16" || lidar_type == "PandarXT-16") { + m_generalParserGpu = new Udp6_1ParserGpu(); + } + if (lidar_type == "PandarXT32" || lidar_type == "PandarXT-32") { + m_generalParserGpu = new Udp6_1ParserGpu(); + } + if (lidar_type == "PandarXTM" || lidar_type == "XT32M2X") { + m_generalParserGpu = new Udp6_1ParserGpu(); + } + if (lidar_type == "PandarFT120" || lidar_type == "FT120C1X") { + m_generalParserGpu = new Udp7_2ParserGpu(); + } + if (lidar_type == "ET" || lidar_type == "ET25" || lidar_type == "ET25-E2X") { + m_generalParserGpu = new Udp2_5ParserGpu(); + } + +} + +template +int UdpParserGpu::SetTransformPara(float x, float y, float z, float roll, float pitch, float yaw) { + if (m_generalParserGpu != nullptr) { + m_generalParserGpu->SetTransformPara(x, y, z, roll, pitch, yaw); + return 0; + } + return -1; +} + diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/udp_parser_gpu.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/udp_parser_gpu.h new file mode 100644 index 0000000..572b669 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParserGpu/udp_parser_gpu.h @@ -0,0 +1,73 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ +#ifndef UDP_PARSER_GPU_H_ +#define UDP_PARSER_GPU_H_ +#include +#include +#include "lidar_types.h" +#include "general_parser_gpu.h" + +namespace hesai +{ +namespace lidar +{ +// class UdpParserGpu +// the UdpParserGpu class is an interface layer.it instantiates a specific udp parser class, +// which is determined by lidar type. +// UdpParserGpu mainly computes xyzi of points with gpu +// you can compute xyzi of points using the ComputeXYZI fuction, which uses gpu to compute +template +class UdpParserGpu { + public: + UdpParserGpu() { + m_generalParserGpu = nullptr; + }; + ~UdpParserGpu(){ + if (m_generalParserGpu != nullptr) { + delete m_generalParserGpu; + m_generalParserGpu = nullptr; + } + }; + int LoadCorrectionFile(std::string correction_path); + int LoadCorrectionString(char *correction_string); + void LoadFiretimesFile(std::string firetimes_path); + int LoadFiretimesString(char *firetimes_string); + void CreatGeneralParser(uint8_t major, uint8_t minor); + int ComputeXYZI(LidarDecodedFrame &frame); + void SetLidarType(std::string lidar_type); + int SetTransformPara(float x, float y, float z, float roll, float pitch, float yaw); + private: + GeneralParserGpu * m_generalParserGpu; + +}; +} +} +#include "udp_parser_gpu.cu" + +#endif // UDP_PARSER_GPU_H_ + diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpProtocol/fault_message.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpProtocol/fault_message.h new file mode 100644 index 0000000..55f971f --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpProtocol/fault_message.h @@ -0,0 +1,289 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ +#ifndef FAULT_MESSAGE_H +#define FAULT_MESSAGE_H +#include "lidar_types.h" + +#pragma pack(1) +struct FaultMessageVersion3 { + public: + uint16_t sob; + uint8_t version_info; + uint8_t utc_time[6]; + uint32_t time_stamp; + uint8_t operate_state; + uint8_t fault_state; + uint8_t fault_code_type; + uint8_t rolling_counter; + uint8_t total_fault_code_num; + uint8_t fault_code_id; + uint32_t fault_code; + uint8_t time_division_multiplexing[27]; + uint8_t software_version[8]; + uint8_t heating_state; + uint8_t lidar_high_temp_state; + uint8_t reversed[3]; + uint32_t crc; + uint8_t cycber_security[32]; + DTCState ParserDTCState() { + switch (fault_code & 0x01) { + case 1: { + return kFault; + break; + } + case 0: { + return kNoFault; + break; + } + + default: + break; + } + return kNoFault; + } + LidarOperateState ParserOperateState() { + switch (operate_state) { + case 0: + return kBoot; + break; + case 1: + return kInit; + break; + case 2: + return kFullPerformance; + break; + case 3: + return kHalfPower; + break; + case 4: + return kSleepMode; + break; + case 5: + return kHighTempertureShutdown; + break; + case 6: + return kFaultShutdown; + break; + + default: + return kUndefineOperateState; + break; + } + } + LidarFaultState ParserFaultState() { + switch (fault_state) { + case 0: + return kNormal; + break; + case 1: + return kWarning; + break; + case 2: + return kPrePerformanceDegradation; + break; + case 3: + return kPerformanceDegradation; + break; + case 4: + return kPreShutDown; + break; + case 5: + return kShutDown; + break; + case 6: + return kPreReset; + break; + case 7: + return kReset; + break; + + default: + return kUndefineFaultState; + break; + } + } + FaultCodeType ParserFaultCodeType() { + switch (fault_code_type) { + case 1: + return kCurrentFaultCode; + break; + case 2: + return kHistoryFaultCode; + break; + + default: + return kUndefineFaultCode; + break; + } + } + TDMDataIndicate ParserTDMDataIndicate() { + switch (time_division_multiplexing[0]) { + case 0: + return kInvaild; + break; + case 1: + return kLensDirtyInfo; + break; + + default: + return kUndefineIndicate; + break; + } + } + void ParserLensDirtyState( + LensDirtyState lens_dirty_state[LENS_AZIMUTH_AREA_NUM] + [LENS_ELEVATION_AREA_NUM]) { + for (int i = 0; i < LENS_AZIMUTH_AREA_NUM; i++) { + uint16_t rawdata = + (*((uint16_t *)(&time_division_multiplexing[3 + i * 2]))); + for (int j = 0; j < LENS_ELEVATION_AREA_NUM; j++) { + uint16_t lens_dirty_state_temp = + (rawdata << ((LENS_ELEVATION_AREA_NUM - j - 1) * 2)); + uint16_t lens_dirty_state_temp1 = + (lens_dirty_state_temp >> ((LENS_ELEVATION_AREA_NUM - 1) * 2)); + if (time_division_multiplexing[0] == 1) { + switch (lens_dirty_state_temp1) { + case 0: { + lens_dirty_state[i][j] = kLensNormal; + break; + } + case 1: { + lens_dirty_state[i][j] = kPassable; + break; + } + case 3: { + lens_dirty_state[i][j] = kUnPassable; + break; + } + default: + lens_dirty_state[i][j] = kUndefineData; + break; + } + + } else + lens_dirty_state[i][j] = kUndefineData; + } + } + } + HeatingState ParserHeatingState() { + switch (heating_state) { + case 0: + return kOff; + break; + case 1: + return kHeating; + break; + case 2: + return kHeatingProhibit; + break; + + default: + break; + } + return kUndefineHeatingState; + } + HighTempertureShutdownState ParserHighTempertureShutdownState() { + switch (lidar_high_temp_state) { + case 1: + return kPreShutdown; + break; + case 2: + return kShutdownMode1; + break; + case 6: + return kShutdownMode2; + break; + case 10: + return kShutdownMode2Fail; + break; + default: + break; + } + return kUndefineShutdownData; + } + void ParserFaultMessage(FaultMessageInfo &fault_message_info) { + fault_message_info.version = version_info; + memcpy(fault_message_info.utc_time, utc_time, sizeof(utc_time)); + double unix_second = 0; + if (utc_time[0] != 0) { + struct tm t = {0}; + t.tm_year = utc_time[0]; + if (t.tm_year >= 200) { + t.tm_year -= 100; + } + t.tm_mon = utc_time[1] - 1; + t.tm_mday = utc_time[2]; + t.tm_hour = utc_time[3]; + t.tm_min = utc_time[4]; + t.tm_sec = utc_time[5]; + t.tm_isdst = 0; + + unix_second = static_cast(mktime(&t)); + } else { + uint32_t utc_time_big = *(uint32_t *)(&utc_time[0] + 2); + unix_second = ((utc_time_big >> 24) & 0xff) | + ((utc_time_big >> 8) & 0xff00) | + ((utc_time_big << 8) & 0xff0000) | ((utc_time_big << 24)); + } + fault_message_info.timestamp = time_stamp; + fault_message_info.total_time = + unix_second + (static_cast(time_stamp)) / 1000000.0; + fault_message_info.operate_state = ParserOperateState(); + fault_message_info.fault_state = ParserFaultState(); + fault_message_info.faultcode_type = ParserFaultCodeType(); + fault_message_info.rolling_counter = rolling_counter; + fault_message_info.total_faultcode_num = total_fault_code_num; + fault_message_info.faultcode_id = fault_code_id; + fault_message_info.faultcode = fault_code; + fault_message_info.dtc_num = (fault_code & 0x0000ffc0) >> 6; + fault_message_info.dtc_state = ParserDTCState(); + fault_message_info.tdm_data_indicate = ParserTDMDataIndicate(); + fault_message_info.temperature = ParserTemperature(); + ParserLensDirtyState(fault_message_info.lens_dirty_state); + fault_message_info.software_id = *((uint16_t *)(&software_version[0])); + fault_message_info.software_version = + *((uint16_t *)(&software_version[2])); + fault_message_info.hardware_version = + *((uint16_t *)(&software_version[4])); + fault_message_info.bt_version = *((uint16_t *)(&software_version[6])); + fault_message_info.heating_state = ParserHeatingState(); + fault_message_info.high_temperture_shutdown_state = + ParserHighTempertureShutdownState(); + memcpy(fault_message_info.reversed, reversed, sizeof(reversed)); + fault_message_info.crc = crc; + memcpy(fault_message_info.cycber_security, cycber_security, + sizeof(cycber_security)); + } + double ParserTemperature() { + double temp = + ((double)(*((uint16_t *)(&time_division_multiplexing[1])))) * 0.1f; + return temp; +} +}; +#pragma pack() +#endif \ No newline at end of file diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpProtocol/udp_protocol_header.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpProtocol/udp_protocol_header.h new file mode 100644 index 0000000..16c8d18 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpProtocol/udp_protocol_header.h @@ -0,0 +1,149 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ +#include "plat_utils.h" +#ifndef LIDAR_PROTOCOL_HEADER_H +#define LIDAR_PROTOCOL_HEADER_H +#ifdef _MSC_VER +#include +#include +#endif +namespace hesai +{ +namespace lidar +{ +#ifdef _MSC_VER +#define PACKED +#pragma pack(push, 1) +#else +#define PACKED __attribute__((packed)) +#endif + +static bool IsLittleEndian() { + const int a = 1; + const unsigned char *p = reinterpret_cast(&a); + + return *p == 1 ? true : false; +} + +template +T little_to_native(T data) { + T out = 0; + if (IsLittleEndian()) { + out = data; + } else { + unsigned char *pSrc = reinterpret_cast(&data + + sizeof(data) - 1), + *pDst = reinterpret_cast(&out); + for (int i = 0; i < sizeof(data); i++) { + *pDst++ = *pSrc--; + } + } + return out; +} + +struct HS_LIDAR_PRE_HEADER { + static const uint16_t kDelimiter = 0xffee; + // major version + static const uint8_t kME = 0x01; // mechanical lidar + static const uint8_t kQT = 0x03; + static const uint8_t kST = 0x04; + static const uint8_t kFT = 0x07; + static const uint8_t kXT = 0x06; + + // minor version + static const uint8_t kV1 = 0x01; // reserved + static const uint8_t kV2 = 0x02; // used by P128 series / POROS + static const uint8_t kV3 = 0x03; // used by P128 series + static const uint8_t kV4 = 0x04; // used by P128 series + + // status info version + static const uint8_t kStatusInfoV0 = 0; + + uint16_t m_u16Delimiter; + uint8_t m_u8VersionMajor; + uint8_t m_u8VersionMinor; + uint8_t m_u8StatusInfoVersion; + uint8_t m_u8Reserved1; + + bool IsValidDelimiter() const { + return little_to_native(m_u16Delimiter) == kDelimiter; + } + uint8_t GetDelimiter() const { return little_to_native(m_u16Delimiter); } + uint8_t GetVersionMajor() const { return m_u8VersionMajor; } + uint8_t GetVersionMinor() const { return m_u8VersionMinor; } + uint8_t GetStatusInfoVersion() const { return m_u8StatusInfoVersion; } + + void Init(uint8_t u8VerMajor, uint8_t u8VerMinor, + uint8_t u8StatusInfoVer = kStatusInfoV0) { + m_u16Delimiter = 0xffee; + m_u8VersionMajor = u8VerMajor; + m_u8VersionMinor = u8VerMinor; + m_u8StatusInfoVersion = u8StatusInfoVer; + } + + void Print() const { + printf("HS_LIDAR_PRE_HEADER:\n"); + printf("Delimiter:%02x, valid:%d, Ver Major: %02x, minor: %02x, " + "StatusInfoVer:%02x\n", + GetDelimiter(), IsValidDelimiter(), GetVersionMajor(), + GetVersionMinor(), GetStatusInfoVersion()); + } +} PACKED; + +struct ReservedInfo1 { + uint16_t m_u16Sts; + uint8_t m_u8ID; + + uint8_t GetID() const { return m_u8ID; } + uint16_t GetData() const { return little_to_native(m_u16Sts); } +} PACKED; + +struct ReservedInfo2 { + uint16_t m_u16Sts; + uint8_t m_u8ID; + uint8_t GetID() const { return m_u8ID; } + uint16_t GetData() const { return little_to_native(m_u16Sts); } + + void Print() const { + printf("lowerBoard ID:%u, STS:0x%02x\n", m_u8ID, m_u16Sts); + } +} PACKED; + +struct ReservedInfo3 { + uint16_t m_u16Sts; + uint8_t m_u8ID; + uint8_t GetID() const { return m_u8ID; } + uint16_t GetData() const { return little_to_native(m_u16Sts); } +} PACKED; + +#ifdef _MSC_VER +#pragma pack(pop) +#endif +} // namespace lidar +} // namespace hesai +#endif diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpProtocol/udp_protocol_p40.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpProtocol/udp_protocol_p40.h new file mode 100644 index 0000000..3ba886d --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpProtocol/udp_protocol_p40.h @@ -0,0 +1,134 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ + +#ifndef HSLIDARP40_H +#define HSLIDARP40_H +#include +#include +#include +namespace hesai +{ +namespace lidar +{ + +#ifdef _MSC_VER +#define PACKED +#pragma pack(push, 1) +#else +#define PACKED __attribute__((packed)) +#endif + +struct HS_LIDAR_BODY_CHN_UNIT_L40 { + uint16_t m_u16Distance; + uint8_t m_u8Reflectivity; + uint16_t GetDistance() const { return little_to_native(m_u16Distance); } + uint8_t GetReflectivity() const { return m_u8Reflectivity; } +} PACKED; + +struct HS_LIDAR_BODY_AZIMUTH_L40 { + // 0xFFEE 2bytes + uint16_t m_u16Sob; + uint16_t m_u16Azimuth; + + uint16_t GetAzimuth() const { return little_to_native(m_u16Azimuth); } + + void Print() const { + printf("HS_LIDAR_BODY_AZIMUTH_L64: azimuth:%u\n", GetAzimuth()); + } +} PACKED; + +struct HS_LIDAR_TAIL_SEQ_NUM_L40 { + uint32_t m_u32SeqNum; + + uint32_t GetSeqNum() const { return little_to_native(m_u32SeqNum); } + static uint32_t GetSeqNumSize() { return sizeof(m_u32SeqNum); } + + void Print() const { + printf("HS_LIDAR_TAIL_SEQ_NUM_L40:\n"); + printf("seqNum: %u\n", GetSeqNum()); + } +} PACKED; + +struct HS_LIDAR_TAIL_L40 { + ReservedInfo1 m_reservedInfo1; + uint8_t m_u8Reserved0[2]; + uint8_t m_u8Shutdown; + uint16_t m_u16ErrorCode; + uint16_t m_u16MotorSpeed; + uint32_t m_u32Timestamp; + uint8_t m_u8ReturnMode; + uint8_t m_u8FactoryInfo; + uint8_t m_u8UTC[6]; + + uint16_t GetMotorSpeed() const { return little_to_native(m_u16MotorSpeed); } + uint8_t GetStatusID() const { return m_reservedInfo1.GetID(); } + uint16_t GetStatusData() const { return m_reservedInfo1.GetData(); } + uint16_t GetErrorCode() const { return little_to_native(m_u16ErrorCode); } + uint32_t GetTimestamp() const { return little_to_native(m_u32Timestamp); } + + uint8_t GetUTCData(uint8_t index) const { + return m_u8UTC[index < sizeof(m_u8UTC) ? index : 0]; + } + + int64_t GetMicroLidarTimeU64() const { + if (m_u8UTC[0] != 0) { + struct tm t = {0}; + t.tm_year = m_u8UTC[0] + 100; + if (t.tm_year >= 200) { + t.tm_year -= 100; + } + t.tm_mon = m_u8UTC[1] - 1; + t.tm_mday = m_u8UTC[2]; + t.tm_hour = m_u8UTC[3]; + t.tm_min = m_u8UTC[4]; + t.tm_sec = m_u8UTC[5]; + t.tm_isdst = 0; +#ifdef _MSC_VER + TIME_ZONE_INFORMATION tzi; + GetTimeZoneInformation(&tzi); + long int timezone = tzi.Bias * 60; +#endif + return (mktime(&t) - timezone) * 1000000 + GetTimestamp(); + } + else { + uint32_t utc_time_big = *(uint32_t*)(&m_u8UTC[0] + 2); + int64_t unix_second = ((utc_time_big >> 24) & 0xff) | + ((utc_time_big >> 8) & 0xff00) | + ((utc_time_big << 8) & 0xff0000) | + ((utc_time_big << 24)); + return unix_second * 1000000 + GetTimestamp(); + } + } + +} PACKED; +#ifdef _MSC_VER +#pragma pack(pop) +#endif +} // namespace lidar +} // namespace hesai +#endif // INTENSITYRETEST_HSLIDARP64_H diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpProtocol/udp_protocol_p64.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpProtocol/udp_protocol_p64.h new file mode 100644 index 0000000..f9680a0 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpProtocol/udp_protocol_p64.h @@ -0,0 +1,161 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ + +#ifndef HSLIDARP64_H +#define HSLIDARP64_H +#include +#include +#include +namespace hesai +{ +namespace lidar +{ + +#ifdef _MSC_VER +#define PACKED +#pragma pack(push, 1) +#else +#define PACKED __attribute__((packed)) +#endif + +struct HS_LIDAR_L64_Header { + // 0xFFEE 2bytes + uint16_t m_u16Sob; + uint8_t m_u8LaserNum; + uint8_t m_u8BlockNum; + // return mode 1 byte when dual return 0-Single Return , + // 1-The first block is the 1 st return. 2-The first block + // is the 2 nd return + uint8_t m_u8ReturnType; + uint8_t m_u8DistUnit; + uint8_t m_u8Reserved0; + uint8_t m_u8Reserved1; + + HS_LIDAR_L64_Header() + : m_u16Sob(0), + m_u8LaserNum(0), + m_u8BlockNum(0), + m_u8ReturnType(0), + m_u8DistUnit(0), + m_u8Reserved0(0), + m_u8Reserved1(0) {} + + uint8_t GetLaserNum() const { return m_u8LaserNum; } + uint8_t GetBlockNum() const { return m_u8BlockNum; } + double GetDistUnit() const { return m_u8DistUnit / 1000.f; } + uint8_t GetReturnType() const { return m_u8ReturnType; } +} PACKED; + +struct HS_LIDAR_BODY_CHN_UNIT_L64 { + uint16_t m_u16Distance; + uint8_t m_u8Reflectivity; + uint16_t GetDistance() const { return little_to_native(m_u16Distance); } + uint8_t GetReflectivity() const { return m_u8Reflectivity; } +} PACKED; + +struct HS_LIDAR_BODY_AZIMUTH_L64 { + uint16_t m_u16Azimuth; + + uint16_t GetAzimuth() const { return little_to_native(m_u16Azimuth); } + + void Print() const { + printf("HS_LIDAR_BODY_AZIMUTH_L64: azimuth:%u\n", GetAzimuth()); + } +} PACKED; + +struct HS_LIDAR_TAIL_SEQ_NUM_L64 { + uint32_t m_u32SeqNum; + + uint32_t GetSeqNum() const { return little_to_native(m_u32SeqNum); } + static uint32_t GetSeqNumSize() { return sizeof(m_u32SeqNum); } + + void Print() const { + printf("HS_LIDAR_TAIL_SEQ_NUM_L64:\n"); + printf("seqNum: %u\n", GetSeqNum()); + } +} PACKED; + +struct HS_LIDAR_TAIL_L64 { + ReservedInfo1 m_reservedInfo1; + uint8_t m_u8Reserved0[2]; + uint8_t m_u8Shutdown; + uint16_t m_u16ErrorCode; + uint16_t m_u16MotorSpeed; + uint32_t m_u32Timestamp; + uint8_t m_u8ReturnMode; + uint8_t m_u8FactoryInfo; + uint8_t m_u8UTC[6]; + + uint16_t GetMotorSpeed() const { return little_to_native(m_u16MotorSpeed); } + uint8_t GetStatusID() const { return m_reservedInfo1.GetID(); } + uint16_t GetStatusData() const { return m_reservedInfo1.GetData(); } + uint16_t GetErrorCode() const { return little_to_native(m_u16ErrorCode); } + + uint32_t GetTimestamp() const { return little_to_native(m_u32Timestamp); } + + uint8_t GetUTCData(uint8_t index) const { + return m_u8UTC[index < sizeof(m_u8UTC) ? index : 0]; + } + + int64_t GetMicroLidarTimeU64() const { + if (m_u8UTC[0] != 0) { + struct tm t = {0}; + t.tm_year = m_u8UTC[0] + 100; + if (t.tm_year >= 200) { + t.tm_year -= 100; + } + t.tm_mon = m_u8UTC[1] - 1; + t.tm_mday = m_u8UTC[2]; + t.tm_hour = m_u8UTC[3]; + t.tm_min = m_u8UTC[4]; + t.tm_sec = m_u8UTC[5]; + t.tm_isdst = 0; +#ifdef _MSC_VER + TIME_ZONE_INFORMATION tzi; + GetTimeZoneInformation(&tzi); + long int timezone = tzi.Bias * 60; +#endif + return (mktime(&t) - timezone) * 1000000 + GetTimestamp(); + } + else { + uint32_t utc_time_big = *(uint32_t*)(&m_u8UTC[0] + 2); + int64_t unix_second = ((utc_time_big >> 24) & 0xff) | + ((utc_time_big >> 8) & 0xff00) | + ((utc_time_big << 8) & 0xff0000) | + ((utc_time_big << 24)); + return unix_second * 1000000 + GetTimestamp(); + } + } + +} PACKED; +#ifdef _MSC_VER +#pragma pack(pop) +#endif +} // namespace lidar +} // namespace hesai +#endif // INTENSITYRETEST_HSLIDARP64_H diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpProtocol/udp_protocol_v1_4.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpProtocol/udp_protocol_v1_4.h new file mode 100644 index 0000000..80080a6 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpProtocol/udp_protocol_v1_4.h @@ -0,0 +1,470 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ +#ifndef HS_LIDAR_ME_V4_H +#define HS_LIDAR_ME_V4_H + +#include +#include +namespace hesai +{ +namespace lidar +{ + +#ifdef _MSC_VER +#define PACKED +#pragma pack(push, 1) +#else +#define PACKED __attribute__((packed)) +#endif + +struct HS_LIDAR_BODY_AZIMUTH_ME_V4 { + uint16_t m_u16Azimuth; + + uint16_t GetAzimuth() const { return little_to_native(m_u16Azimuth); } + + void Print() const { + printf("HS_LIDAR_BODY_AZIMUTH_ME_V4: azimuth:%u\n", GetAzimuth()); + } +} PACKED; + +struct HS_LIDAR_BODY_CHN_UNIT_NO_CONF_ME_V4 { + uint16_t m_u16Distance; + uint8_t m_u8Reflectivity; + + uint16_t GetDistance() const { return little_to_native(m_u16Distance); } + uint8_t GetReflectivity() const { return m_u8Reflectivity; } + void Print() const { + printf("HS_LIDAR_BODY_CHN_UNIT_NO_CONF_ME_V4:\n"); + printf("Dist:%u, Reflectivity: %u\n", GetDistance(), GetReflectivity()); + } + void PrintMixData() const { + printf("HS_LIDAR_BODY_CHN_UNIT_NO_CONF_ME_V4:\n"); + } +} PACKED; + +struct HS_LIDAR_BODY_CHN_UNIT_ME_V4 { + uint16_t m_u16Distance; + uint8_t m_u8Reflectivity; + uint8_t m_u8Confidence; + + uint16_t GetDistance() const { return little_to_native(m_u16Distance); } + uint8_t GetReflectivity() const { return m_u8Reflectivity; } + uint8_t GetConfidenceLevel() const { return m_u8Confidence; } + + + void Print() const { + printf("HS_LIDAR_BODY_CHN_UNIT_ME_V4:\n"); + printf("Dist:%u, Reflectivity: %u, confidenceLevel:%d\n", GetDistance(), + GetReflectivity(), GetConfidenceLevel()); + } + void PrintMixData() const { + printf("HS_LIDAR_BODY_CHN_UNIT_ME_V4:\n"); + } +} PACKED; + +struct HS_LIDAR_BODY_CRC_ME_V4 { + uint32_t m_u32Crc; + + uint32_t GetCrc() const { return little_to_native(m_u32Crc); } + + void Print() const { + printf("HS_LIDAR_BODY_CRC_ME_V4:\n"); + printf("crc:0x%08x\n", GetCrc()); + } +} PACKED; + +struct HS_LIDAR_CYBER_SECURITY_ME_V4 { + uint8_t m_u8Signature[32]; + + uint8_t GetSignatureData(uint8_t index) const { + return m_u8Signature[index < sizeof(m_u8Signature) ? index : 0]; + } + + void Print() const { + printf("HS_LIDAR_CYBER_SECURITY_ME_V4:\n"); + for (uint8_t i = 0; i < sizeof(m_u8Signature); i++) + printf("Signature%d:%d, ", i, GetSignatureData(i)); + printf("\n"); + } +} PACKED; + +struct HS_LIDAR_FUNC_SAFETY_ME_V4 { + static const uint8_t kLidarStateShiftBits = 5; + static const uint8_t kLidarStateMask = 0x07; + static const uint8_t kRollingCntShiftBits = 0; + static const uint8_t kRollingCntMask = 0x07; + static const uint8_t kFaultTypeCurrent = 0x08; + static const uint8_t kFaultTypeHistory = 0x10; + static const uint8_t kFaultNumShiftBits = 4; + static const uint8_t kFaultNumMask = 0x0F; + static const uint8_t kFaultIDShiftBits = 0; + static const uint8_t kFaultIDMask = 0x0F; + + uint8_t m_u8Version; + uint8_t m_u8Status; + uint8_t m_u8FaultInfo; + uint16_t m_u16FaultCode; + uint8_t m_u8Reserved[8]; + uint32_t m_u32Crc; + + uint8_t GetVersion() const { return m_u8Version; } + + uint8_t GetLidarState() const { + return (m_u8Status >> kLidarStateShiftBits) & kLidarStateMask; + } + bool IsHistoryFault() const { return m_u8Status & kFaultTypeHistory; } + bool IsCurrentFault() const { return m_u8Status & kFaultTypeCurrent; } + + uint8_t GetRollingCnt() const { + return (m_u8Status >> kRollingCntShiftBits) & kRollingCntMask; + } + + uint8_t GetFaultNum() const { + return (m_u8FaultInfo >> kFaultNumShiftBits) & kFaultNumMask; + } + uint8_t GetFaultID() const { + return (m_u8FaultInfo >> kFaultIDShiftBits) & kFaultIDMask; + } + + uint16_t GetFaultCode() const { return little_to_native(m_u16FaultCode); } + + uint32_t GetCrc() const { return little_to_native(m_u32Crc); } + + void Print() const { + printf("HS_LIDAR_FUNC_SAFETY_ME_V4:\n"); + printf("ver:%u, lidarState:%u, rollingCnt:%u, isCurrFault:%u, " + "faultNum:%u, faultID:%u, faultCode:0x%04x, crc:0x%08x\n", + GetVersion(), GetLidarState(), GetRollingCnt(), IsCurrentFault(), + GetFaultNum(), GetFaultID(), GetFaultCode(), GetCrc()); + } +} PACKED; + +struct HS_LIDAR_TAIL_ME_V4 { + // shutdown flag, bit 0 + static const uint8_t kHighPerformanceMode = 0x00; + static const uint8_t kShutdown = 0x01; + static const uint8_t kStandardMode = 0x02; + static const uint8_t kEnergySavingMode = 0x03; + + // return mode + static const uint8_t kFirstReturn = 0x33; + static const uint8_t kSecondReturn = 0x34; + static const uint8_t kThirdReturn = 0x35; + static const uint8_t kFourthReturn = 0x36; + static const uint8_t kStrongestReturn = 0x37; + static const uint8_t kLastReturn = 0x38; + static const uint8_t kDualReturn = 0x39; + static const uint8_t kFirstSecondReturn = 0x3a; + static const uint8_t kStongestFirstReturn = 0x3b; + + ReservedInfo1 m_reservedInfo1; + ReservedInfo2 m_reservedInfo2; + ReservedInfo3 m_reservedInfo3; + uint16_t m_u16AzimuthFlag; + uint8_t m_u8RunningMode; + uint8_t m_u8ReturnMode; + uint16_t m_u16MotorSpeed; + uint8_t m_u8UTC[6]; + uint32_t m_u32Timestamp; + uint8_t m_u8FactoryInfo; + + uint8_t GetStsID0() const { return m_reservedInfo1.GetID(); } + uint16_t GetData0() const { return m_reservedInfo1.GetData(); } + uint8_t GetStsID1() const { return m_reservedInfo2.GetID(); } + uint16_t GetData1() const { return m_reservedInfo2.GetData(); } + uint8_t GetStsID2() const { return m_reservedInfo3.GetID(); } + uint16_t GetData2() const { return m_reservedInfo3.GetData(); } + + uint8_t HasShutdown() const { return m_u8RunningMode == kShutdown; } + uint8_t GetReturnMode() const { return m_u8ReturnMode; } + uint16_t GetMotorSpeed() const { return little_to_native(m_u16MotorSpeed); } + uint32_t GetTimestamp() const { return little_to_native(m_u32Timestamp); } + uint8_t getOperationMode() const { return m_u8RunningMode & 0x0f; } + uint8_t getAngleState(int blockIndex) const { + return (m_u16AzimuthFlag >> (2 * (7 - blockIndex))) & 0x03; + } + + bool IsFirstReturn() const { return m_u8ReturnMode == kFirstReturn; } + bool IsSecondReturn() const { return m_u8ReturnMode == kSecondReturn; } + bool IsThirdReturn() const { return m_u8ReturnMode == kThirdReturn; } + bool IsFourthReturn() const { return m_u8ReturnMode == kFourthReturn; } + bool IsLastReturn() const { return m_u8ReturnMode == kLastReturn; } + bool IsStrongestReturn() const { return m_u8ReturnMode == kStrongestReturn; } + bool IsDualReturn() const { return m_u8ReturnMode == kDualReturn; } + bool IsFirstSecondReturn() const { + return m_u8ReturnMode == kFirstSecondReturn; + } + bool IsStongestFirstReturn() const { + return m_u8ReturnMode == kStongestFirstReturn; + } + int64_t GetMicroLidarTimeU64() const { + if (m_u8UTC[0] != 0) { + struct tm t = {0}; + t.tm_year = m_u8UTC[0]; + if (t.tm_year < 70) { + return 0; + } + t.tm_mon = m_u8UTC[1] - 1; + t.tm_mday = m_u8UTC[2] + 1; + t.tm_hour = m_u8UTC[3]; + t.tm_min = m_u8UTC[4]; + t.tm_sec = m_u8UTC[5]; + t.tm_isdst = 0; +#ifdef _MSC_VER + TIME_ZONE_INFORMATION tzi; + GetTimeZoneInformation(&tzi); + long int timezone = tzi.Bias * 60; +#endif + return (mktime(&t) - timezone - 86400) * 1000000 + GetTimestamp() ; + } + else { + uint32_t utc_time_big = *(uint32_t*)(&m_u8UTC[0] + 2); + int64_t unix_second = ((utc_time_big >> 24) & 0xff) | + ((utc_time_big >> 8) & 0xff00) | + ((utc_time_big << 8) & 0xff0000) | + ((utc_time_big << 24)); + return unix_second * 1000000 + GetTimestamp(); + } + } + + uint8_t GetFactoryInfo() const { return m_u8FactoryInfo; } + uint8_t GetUTCData(uint8_t index) const { + return m_u8UTC[index < sizeof(m_u8UTC) ? index : 0]; + } + + void Print() const { + printf("HS_LIDAR_TAIL_ME_V4:\n"); + printf("sts0:%d, data0:%d, sts1:%d, data1:%d, sts2:%d, data2:%d, " + "shutDown:%d, motorSpeed:%u, timestamp:%u, return_mode:0x%02x, " + "factoryInfo:0x%02x, utc:%u %u %u %u %u %u\n", + GetStsID0(), GetData0(), GetStsID1(), GetData1(), GetStsID2(), + GetData2(), HasShutdown(), GetMotorSpeed(), GetTimestamp(), + GetReturnMode(), GetFactoryInfo(), GetUTCData(0), GetUTCData(1), + GetUTCData(2), GetUTCData(3), GetUTCData(4), GetUTCData(5)); + } +} PACKED; + +struct HS_LIDAR_TAIL_SEQ_NUM_ME_V4 { + uint32_t m_u32SeqNum; + + uint32_t GetSeqNum() const { return little_to_native(m_u32SeqNum); } + static uint32_t GetSeqNumSize() { return sizeof(m_u32SeqNum); } + + void CalPktLoss(uint32_t &u32StartSeqNum, uint32_t &u32LastSeqNum, uint32_t &u32LossCount, + uint32_t &u32StartTime, uint32_t &u32TotalLossCount, uint32_t &u32TotalStartSeqNum) const { + bool print = false; + if (u32StartSeqNum == 0) { + u32LossCount = 0; + u32TotalLossCount = 0; + u32StartTime = GetMicroTickCount(); + u32StartSeqNum = m_u32SeqNum; + u32LastSeqNum = m_u32SeqNum; + u32TotalStartSeqNum = m_u32SeqNum; + return; + } + if (m_u32SeqNum - u32LastSeqNum > 1) { + u32LossCount += (m_u32SeqNum - u32LastSeqNum - 1); + u32TotalLossCount += (m_u32SeqNum - u32LastSeqNum - 1); + print = true; + // if (m_u32SeqNum - u32LastSeqNum - 1 > 1000) + // printf("%d, %u, %u\n", m_u32SeqNum - u32LastSeqNum - 1, u32LastSeqNum, + // m_u32SeqNum); + } + + // print log every 1s + if (GetMicroTickCount() - u32StartTime >= 1 * 1000 * 1000) { + printf("pkt loss freq: %u/%u\n", u32LossCount, + m_u32SeqNum - u32StartSeqNum); + u32LossCount = 0; + u32StartTime = GetMicroTickCount(); + u32StartSeqNum = m_u32SeqNum; + } + + u32LastSeqNum = m_u32SeqNum; + } + + void CalPktLoss(uint32_t &u32StartSeqNum, uint32_t &u32LastSeqNum, uint32_t &u32LossCount, uint32_t &u32StartTime) const { + bool print = false; + if (m_u32SeqNum - u32LastSeqNum > 1) { + u32LossCount += (m_u32SeqNum - u32LastSeqNum - 1); + print = true; + // if (m_u32SeqNum - u32LastSeqNum - 1 > 1000) + // printf("%d, %u, %u\n", m_u32SeqNum - u32LastSeqNum - 1, u32LastSeqNum, + // m_u32SeqNum); + } + + // print log every 1s + if (GetMicroTickCount() - u32StartTime >= 1 * 1000 * 1000) { + printf("pkt loss freq: %u/%u\n", u32LossCount, + m_u32SeqNum - u32StartSeqNum); + u32LossCount = 0; + u32StartTime = GetMicroTickCount(); + u32StartSeqNum = m_u32SeqNum; + } + + u32LastSeqNum = m_u32SeqNum; + } + + void Print() const { + printf("HS_LIDAR_TAIL_SEQ_NUM_ME_V4:\n"); + printf("seqNum: %u\n", GetSeqNum()); + } +} PACKED; + +struct HS_LIDAR_TAIL_IMU_ME_V4 { + int16_t m_i16IMUTemperature; + uint16_t m_u16IMUAccelUnit; + uint16_t m_u16IMUAngVelUnit; + uint32_t m_u32IMUTimeStamp; + int16_t m_i16IMUXAccel; + int16_t m_i16IMUYAccel; + int16_t m_i16IMUZAccel; + int16_t m_i16IMUXAngVel; + int16_t m_i16IMUYAngVel; + int16_t m_i16IMUZAngVel; + + int16_t GetIMUTemperature() const { + return little_to_native(m_i16IMUTemperature); + } + double GetIMUAccelUnit() const { + return little_to_native(m_u16IMUAccelUnit) / 1000.f; + } + double GetIMUAngVelUnit() const { + return little_to_native(m_u16IMUAngVelUnit) / 1000.f; + } + uint32_t GetIMUTimestamp() const { + return little_to_native(m_u32IMUTimeStamp); + } + double GetIMUXAccel() const { + return little_to_native(m_i16IMUXAccel) * GetIMUAccelUnit(); + } + double GetIMUYAccel() const { + return little_to_native(m_i16IMUYAccel) * GetIMUAccelUnit(); + } + double GetIMUZAccel() const { + return little_to_native(m_i16IMUZAccel) * GetIMUAccelUnit(); + } + double GetIMUXAngVel() const { + return little_to_native(m_i16IMUXAngVel) * GetIMUAngVelUnit(); + } + double GetIMUYAngVel() const { + return little_to_native(m_i16IMUYAngVel) * GetIMUAngVelUnit(); + } + double GetIMUZAngVel() const { + return little_to_native(m_i16IMUZAngVel) * GetIMUAngVelUnit(); + } + + void Print() const { + printf("HS_LIDAR_TAIL_IMU_ME_V4:\n"); + printf("Temp:%u, AccelUnit:%f, AngVelUnit:%f, TimeStamp:%u, XAccel:%f, " + "YAccel:%f, ZAccel:%f, XAngVel:%f, YAngVel:%f, ZAngVel:%f\n", + GetIMUTemperature(), GetIMUAccelUnit(), GetIMUAngVelUnit(), + GetIMUTimestamp(), GetIMUXAccel(), GetIMUYAccel(), GetIMUZAccel(), + GetIMUXAngVel(), GetIMUYAngVel(), GetIMUZAngVel()); + } +} PACKED; + +struct HS_LIDAR_TAIL_CRC_ME_V4 { + uint32_t m_u32Crc; + + uint32_t GetCrc() const { return little_to_native(m_u32Crc); } + + void Print() const { + printf("HS_LIDAR_TAIL_CRC_ME_V4:\n"); + printf("crc:0x%08x\n", GetCrc()); + } +} PACKED; + +struct HS_LIDAR_HEADER_ME_V4 { + static const uint8_t kSequenceNum = 0x01; + static const uint8_t kIMU = 0x02; + static const uint8_t kFunctionSafety = 0x04; + static const uint8_t kCyberSecurity = 0x08; + static const uint8_t kConfidenceLevel = 0x10; + static const uint8_t kDistUnit = 0x04; + + static const uint8_t kSingleReturn = 0x00; + static const uint8_t kFirstBlockLastReturn = 0x01; + static const uint8_t kFirstBlockStrongestReturn = 0x02; + + uint8_t m_u8LaserNum; + uint8_t m_u8BlockNum; + uint8_t m_u8EchoCount; + uint8_t m_u8DistUnit; + uint8_t m_u8EchoNum; + uint8_t m_u8Status; + + uint8_t GetLaserNum() const { return m_u8LaserNum; } + uint8_t GetBlockNum() const { return m_u8BlockNum; } + double GetDistUnit() const { return m_u8DistUnit / 1000.f; } + uint8_t GetEchoCount() const { return m_u8EchoCount; } + uint8_t GetEchoNum() const { return m_u8EchoNum; } + bool HasSeqNum() const { return m_u8Status & kSequenceNum; } + bool HasIMU() const { return m_u8Status & kIMU; } + bool HasFuncSafety() const { return m_u8Status & kFunctionSafety; } + bool HasCyberSecurity() const { return m_u8Status & kCyberSecurity; } + bool HasConfidenceLevel() const { return m_u8Status & kConfidenceLevel; } + bool IsFirstBlockLastReturn() const { + return m_u8EchoCount == kFirstBlockLastReturn; + } + bool IsFirstBlockStrongestReturn() const { + return m_u8EchoCount == kFirstBlockStrongestReturn; + } + + uint16_t GetPacketSize() const { + return sizeof(HS_LIDAR_PRE_HEADER) + sizeof(HS_LIDAR_HEADER_ME_V4) + + (sizeof(HS_LIDAR_BODY_AZIMUTH_ME_V4) + + (HasConfidenceLevel() + ? sizeof(HS_LIDAR_BODY_CHN_UNIT_ME_V4) + : sizeof(HS_LIDAR_BODY_CHN_UNIT_NO_CONF_ME_V4)) * + GetLaserNum()) * + GetBlockNum() + + sizeof(HS_LIDAR_BODY_CRC_ME_V4) + + (HasFuncSafety() ? sizeof(HS_LIDAR_FUNC_SAFETY_ME_V4) : 0) + + (HasCyberSecurity() ? sizeof(HS_LIDAR_CYBER_SECURITY_ME_V4) : 0) + + sizeof(HS_LIDAR_TAIL_ME_V4) + sizeof(HS_LIDAR_TAIL_CRC_ME_V4) + + (HasSeqNum() ? sizeof(HS_LIDAR_TAIL_SEQ_NUM_ME_V4) : 0) + + (HasIMU() ? sizeof(HS_LIDAR_TAIL_IMU_ME_V4) : 0); + } + + void Print() const { + printf("HS_LIDAR_HEADER_ME_V4:\n"); + printf("laserNum:%02u, block_num:%02u, DistUnit:%g, EchoCnt:%02u, " + "EchoNum:%02u, HasSeqNum:%d, HasIMU:%d, " + "HasFuncSafety:%d, HasCyberSecurity:%d, HasConfidence:%d\n", + GetLaserNum(), GetBlockNum(), GetDistUnit(), GetEchoCount(), + GetEchoNum(), HasSeqNum(), HasIMU(), HasFuncSafety(), + HasCyberSecurity(), HasConfidenceLevel()); + } +} PACKED; +#ifdef _MSC_VER +#pragma pack(pop) +#endif +} // namespace lidar +} // namespace hesai +#endif diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpProtocol/udp_protocol_v2_4.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpProtocol/udp_protocol_v2_4.h new file mode 100644 index 0000000..58564af --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpProtocol/udp_protocol_v2_4.h @@ -0,0 +1,251 @@ +/* +This document is an encapsulation of the UDP2.4 protocol, +defining protocol-related data structures and some operational functions. +*/ + +#ifndef HS_LIDAR_ET_V4_H +#define HS_LIDAR_ET_V4_H + +#include "plat_utils.h" +#include +namespace hesai +{ + namespace lidar + { + #ifdef _MSC_VER + #define PACKED + #pragma pack(push, 1) + #else + #define PACKED __attribute__((packed)) + #endif + + // BODY + // BODY_unit + struct HS_LIDAR_BODY_LASTER_UNIT_ET_V4{ + uint16_t distance; + uint8_t reflectivity; + uint8_t confidence; + uint16_t GetDistance() const {return little_to_native(distance); } + uint8_t GetReflectivity() const {return reflectivity; } + uint8_t GetConfidence() const {return confidence; } + void Print() const { + printf("HS_LIDAR_BODY_CHN_UNIT_ET_V4:\n"); + printf("Distance:%u, Reflectivity: %u, Confidence: %u\n", GetDistance(), GetReflectivity(), GetConfidence() ); + } + } PACKED; + + // BODY + // the first two fields of seq + struct HS_LIDAR_BODY_SEQ2_ET_V4 { + int16_t horizontalAngle; + int16_t verticalAngle; + int16_t GetHorizontalAngle() const { return little_to_native(horizontalAngle); } + int16_t GetVerticalAngle() const { return little_to_native(verticalAngle); } + } PACKED; + + // BODY + // Body_crc + struct HS_LIDAR_BODY_CRC_ET_V4 { + uint32_t m_u32Crc; + uint32_t GetCrc() const { return little_to_native(m_u32Crc); } + void Print() const { + printf("HS_LIDAR_BODY_CRC_ET_V4:\n"); + printf("crc:0x%08x\n", GetCrc()); + } + } PACKED; + + // Tail + // the first nine fields of tail + struct HS_LIDAR_TAIL_ET_V4 { + static const uint8_t kShutDown = 0x01; + static const uint8_t kFirstReturn = 0x33; + static const uint8_t kStrongestReturn = 0x37; + static const uint8_t kLastReturn = 0x38; + static const uint8_t kDualReturn = 0x39; + static const uint8_t kFactoryInfo = 0x42; + ReservedInfo1 m_reservedInfo1; + ReservedInfo2 m_reservedInfo2; + ReservedInfo3 m_reservedInfo3; + uint8_t m_u8FrameID; + uint8_t m_u8ShutDown; + uint8_t m_u8EchoMode; + uint8_t m_u8UTC[6]; + uint32_t m_u32Timestamp; + uint8_t m_u8FactoryInfo; + + + uint8_t GetStsID0() const { return m_reservedInfo1.GetID(); } + uint16_t GetData0() const { return m_reservedInfo1.GetData(); } + + uint8_t GetStsID1() const { return m_reservedInfo2.GetID(); } + uint16_t GetData1() const { return m_reservedInfo2.GetData(); } + + uint8_t GetStsID2() const { return m_reservedInfo3.GetID(); } + uint16_t GetData2() const { return m_reservedInfo3.GetData(); } + + uint8_t GetFrameID() const { return m_u8FrameID; } + + uint8_t HasShutdown() const { return m_u8ShutDown & kShutDown; } + + uint8_t GetReturnMode() const { return m_u8EchoMode; } + + bool IsLastReturn() const { return m_u8EchoMode == kLastReturn; } + bool IsStrongestReturn() const { return m_u8EchoMode == kStrongestReturn; } + bool IsDualReturn() const { return m_u8EchoMode == kDualReturn; } + bool IsFirstReturn() const { return m_u8EchoMode == kFirstReturn; } + + uint32_t GetTimestamp() const { return little_to_native(m_u32Timestamp); } + uint8_t GetFactoryInfo() const { return m_u8FactoryInfo; } + + uint8_t GetUTCData(uint8_t index) const { + return m_u8UTC[index < sizeof(m_u8UTC) ? index : 0]; + } + + // get us time + int64_t GetMicroLidarTimeU64() const { + if (m_u8UTC[0] != 0) { + struct tm t = {0}; + t.tm_year = m_u8UTC[0]; + if (t.tm_year >= 200) { + t.tm_year -= 100; + } + t.tm_mon = m_u8UTC[1] - 1; + t.tm_mday = m_u8UTC[2]; + t.tm_hour = m_u8UTC[3]; + t.tm_min = m_u8UTC[4]; + t.tm_sec = m_u8UTC[5]; + t.tm_isdst = 0; +#ifdef _MSC_VER + TIME_ZONE_INFORMATION tzi; + GetTimeZoneInformation(&tzi); + long int timezone = tzi.Bias * 60; +#endif + return (mktime(&t) - timezone) * 1000000 + GetTimestamp(); + } + else { + uint32_t utc_time_big = *(uint32_t*)(&m_u8UTC[0] + 2); + int64_t unix_second = ((utc_time_big >> 24) & 0xff) | + ((utc_time_big >> 8) & 0xff00) | + ((utc_time_big << 8) & 0xff0000) | + ((utc_time_big << 24)); + return unix_second * 1000000 + GetTimestamp(); + } + } + void Print() const { + printf("HS_LIDAR_TAIL_ET_V4:\n"); + printf( + "sts0:%d, data0:%d, sts1:%d, data1:%d, shutDown:%d" + "timestamp:%u, return_mode:0x%02x, factoryInfo:0x%02x, utc:%u %u " + "%u %u %u %u\n", + GetStsID0(), GetData0(), GetStsID1(), GetData1(), HasShutdown(), + GetTimestamp(), GetReturnMode(), GetFactoryInfo(), + GetUTCData(0), GetUTCData(1), GetUTCData(2), GetUTCData(3), + GetUTCData(4), GetUTCData(5)); + } + } PACKED; + + // Tail + // seqnum + struct HS_LIDAR_TAIL_SEQ_NUM_ET_V4 { + uint32_t m_u32SeqNum; + uint32_t GetSeqNum() const { return little_to_native(m_u32SeqNum); } + static uint32_t GetSeqNumSize() { return sizeof(m_u32SeqNum); } + void CalPktLoss(uint32_t &u32StartSeqNum, uint32_t &u32LastSeqNum, uint32_t &u32LossCount, uint32_t &u32StartTime) const { + bool print = false; + if (m_u32SeqNum - u32LastSeqNum > 1) { + u32LossCount += (m_u32SeqNum - u32LastSeqNum - 1); + print = true; + } + if (GetMicroTickCount() - u32StartTime >= 1 * 1000 * 1000) { + printf("pkt loss freq: %u/%u\n", u32LossCount, m_u32SeqNum - u32StartSeqNum); + u32LossCount = 0; + u32StartTime = GetMicroTickCount(); + u32StartSeqNum = m_u32SeqNum; + } + u32LastSeqNum = m_u32SeqNum; + } + void Print() const { + printf("HS_LIDAR_TAIL_SEQ_NUM_ET_V4:\n"); + printf("seqNum: %u\n", GetSeqNum()); + } + } PACKED; + + // Tail + // crc + struct HS_LIDAR_TAIL_CRC_ET_V4 { + uint32_t m_u32Crc; + uint32_t GetCrc() const { return little_to_native(m_u32Crc); } + void Print() const { + printf("HS_LIDAR_TAIL_CRC_ET_V4:\n"); + printf("crc:0x%08x\n", GetCrc()); + } + } PACKED; + + // Cyber Security (optional) none + + // Header + struct HS_LIDAR_HEADER_ET_V4 { + static const uint8_t kLaserNum = 0x40; + static const uint8_t kBlockNum = 0x04; + static const uint8_t kFirstBlockLastReturn = 0x01; + static const uint8_t kFirstBlockStrongestReturn = 0x02; + static const uint8_t kDistUnit = 0x05; + static const uint8_t kSeqNum = 0x08; + static const uint8_t kSequenceNum = 0x01; // bit0 + static const uint8_t kIMU = 0x02; // bit1 + static const uint8_t kFunctionSafety = 0x04; // bit2 + static const uint8_t kCyberSecurity = 0x08; // bit3 + static const uint8_t kConfidenceLevel = 0x10; // bit4 + static const uint8_t kWeightFactor = 0x20; // bit5 + + uint8_t m_u8LaserNum; + uint8_t m_u8BlockNum; + uint8_t m_u8EchoCount; + uint8_t m_u8DistUnit; + uint8_t m_u8EchoNum; + uint8_t m_u8SeqNum; + uint8_t m_u8Reserved2; + + + uint8_t GetLaserNum() const { return m_u8LaserNum; } + uint8_t GetBlockNum() const { return m_u8BlockNum; } + double GetDistUnit() const { return m_u8DistUnit / 1000.f; } + uint8_t GetEchoCount() const { return m_u8EchoCount; } + uint8_t GetSeqNum() const { return m_u8SeqNum; } + + bool IsFirstBlockLastReturn() const { + return m_u8EchoCount == kFirstBlockLastReturn; + } + bool IsFirstBlockStrongestReturn() const { + return m_u8EchoCount == kFirstBlockStrongestReturn; + } + uint8_t GetEchoNum() const { return m_u8EchoNum; } + bool HasSeqNum() const { return m_u8Reserved2 & kSequenceNum; } + bool HasIMU() const { return m_u8Reserved2 & kIMU; } + bool HasFuncSafety() const { return m_u8Reserved2& kFunctionSafety; } + bool HasCyberSecurity() const { return m_u8Reserved2 & kCyberSecurity; } + bool HasConfidenceLevel() const { return m_u8Reserved2 & kConfidenceLevel; } + bool HasWeightFactor() const { return m_u8Reserved2 & kWeightFactor ; } + + uint16_t GetPacketSize() const { + return (sizeof(struct HS_LIDAR_PRE_HEADER) + sizeof(struct HS_LIDAR_HEADER_ET_V4) + + (sizeof(uint16_t) + (sizeof(struct HS_LIDAR_BODY_LASTER_UNIT_ET_V4) * (m_u8LaserNum / m_u8SeqNum) + 4) * m_u8SeqNum) * m_u8BlockNum + + sizeof(struct HS_LIDAR_BODY_CRC_ET_V4) + + sizeof(struct HS_LIDAR_TAIL_ET_V4) + sizeof(struct HS_LIDAR_TAIL_SEQ_NUM_ET_V4) + sizeof(struct HS_LIDAR_TAIL_CRC_ET_V4) + ); + } + void Print() const { + printf("HS_LIDAR_HEADER_ET_V4:\n"); + printf( + "laserNum:%02u, block_num:%02u, DistUnit:%g, EchoCnt:%02u, " + "EchoNum:%02u, HasSeqNum:%d, HasIMU:%d, " + "HasFuncSafety:%d, HasCyberSecurity:%d, HasConfidence:%d\n", + GetLaserNum(), GetBlockNum(), GetDistUnit(), GetEchoCount(), + GetEchoNum(), HasSeqNum(), HasIMU(), HasFuncSafety(), + HasCyberSecurity(), HasConfidenceLevel()); + } + } PACKED; + } // namespace lidar +} // namespace hesai + +#endif \ No newline at end of file diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpProtocol/udp_protocol_v2_5.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpProtocol/udp_protocol_v2_5.h new file mode 100644 index 0000000..947e22f --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpProtocol/udp_protocol_v2_5.h @@ -0,0 +1,267 @@ +#ifndef HS_LIDAR_ET_V5_H +#define HS_LIDAR_ET_V5_H + +#include "plat_utils.h" +#include +namespace hesai +{ + namespace lidar + { + #ifdef _MSC_VER + #define PACKED + #pragma pack(push, 1) + #else + #define PACKED __attribute__((packed)) + #endif + + // Body + // body_unit + struct HS_LIDAR_BODY_LASTER_UNIT_ET_V5 { + uint16_t distance; + uint8_t reflectivity; + + uint16_t GetDistance() const { return little_to_native(distance); } + uint8_t GetReflectivity() const { return reflectivity; } + void Print() const { + printf("HS_LIDAR_BODY_CHN_UNIT_FT_V2:\n"); + printf("Distance:%u, Reflectivity: %u\n", GetDistance(), GetReflectivity() ); + } + } PACKED; + + //Body + // The first three fields of seq + struct HS_LIDAR_BODY_SEQ3_ET_V5 { + int16_t horizontalAngle; + int16_t verticalAngle; + uint8_t confidence; + + int16_t GetHorizontalAngle() const { return little_to_native(horizontalAngle); } + int16_t GetVerticalAngle() const { return little_to_native(verticalAngle); } + uint8_t GetConfidence() const { return confidence; } + } PACKED; + + // Body + // crc + struct HS_LIDAR_BODY_CRC_ET_V5 { + uint32_t m_u32Crc; + + uint32_t GetCrc() const { return little_to_native(m_u32Crc); } + + void Print() const { + printf("HS_LIDAR_BODY_CRC_ET_V5:\n"); + printf("crc:0x%08x\n", GetCrc()); + } + } PACKED; + + // Tail + // the first nine fields of tail + struct HS_LIDAR_TAIL_ET_V5{ + + static const uint8_t kShutDown = 0x01; + static const uint8_t kFirstReturn = 0x33; + static const uint8_t kStrongestReturn = 0x37; + static const uint8_t kLastReturn = 0x38; + static const uint8_t kDualReturn = 0x39; + static const uint8_t kFactoryInfo = 0x42; + ReservedInfo1 m_reservedInfo1; + ReservedInfo2 m_reservedInfo2; + ReservedInfo3 m_reservedInfo3; + uint8_t m_u8FrameID; + uint8_t m_u8ShutDown; + uint8_t m_u8EchoMode; + uint8_t m_u8UTC[6]; + uint32_t m_u32Timestamp; + uint8_t m_u8FactoryInfo; + + + uint8_t GetStsID0() const { return m_reservedInfo1.GetID(); } + uint16_t GetData0() const { return m_reservedInfo1.GetData(); } + + uint8_t GetStsID1() const { return m_reservedInfo2.GetID(); } + uint16_t GetData1() const { return m_reservedInfo2.GetData(); } + + uint8_t GetStsID2() const { return m_reservedInfo3.GetID(); } + uint16_t GetData2() const { return m_reservedInfo3.GetData(); } + + uint8_t GetFrameID() const { return m_u8FrameID; } + + uint8_t HasShutdown() const { return m_u8ShutDown & kShutDown; } + + uint8_t GetReturnMode() const { return m_u8EchoMode; } + + bool IsLastReturn() const { return m_u8EchoMode == kLastReturn; } + bool IsStrongestReturn() const { return m_u8EchoMode == kStrongestReturn; } + bool IsDualReturn() const { return m_u8EchoMode == kDualReturn; } + bool IsFirstReturn() const { return m_u8EchoMode == kFirstReturn; } + + uint32_t GetTimestamp() const { return little_to_native(m_u32Timestamp); } + uint8_t GetFactoryInfo() const { return m_u8FactoryInfo; } + + uint8_t GetUTCData(uint8_t index) const { + return m_u8UTC[index < sizeof(m_u8UTC) ? index : 0]; + } + + // get us time + int64_t GetMicroLidarTimeU64() const { + if (m_u8UTC[0] != 0) { + struct tm t = {0}; + t.tm_year = m_u8UTC[0]; + if (t.tm_year >= 200) { + t.tm_year -= 100; + } + t.tm_mon = m_u8UTC[1] - 1; + t.tm_mday = m_u8UTC[2]; + t.tm_hour = m_u8UTC[3]; + t.tm_min = m_u8UTC[4]; + t.tm_sec = m_u8UTC[5]; + t.tm_isdst = 0; +#ifdef _MSC_VER + TIME_ZONE_INFORMATION tzi; + GetTimeZoneInformation(&tzi); + long int timezone = tzi.Bias * 60; +#endif + return (mktime(&t) - timezone) * 1000000 + GetTimestamp(); + } + else { + uint32_t utc_time_big = *(uint32_t*)(&m_u8UTC[0] + 2); + int64_t unix_second = ((utc_time_big >> 24) & 0xff) | + ((utc_time_big >> 8) & 0xff00) | + ((utc_time_big << 8) & 0xff0000) | + ((utc_time_big << 24)); + return unix_second * 1000000 + GetTimestamp(); + } + } + void Print() const { + printf("HS_LIDAR_TAIL_ET_V5:\n"); + printf( + "sts0:%d, data0:%d, sts1:%d, data1:%d, shutDown:%d" + "timestamp:%u, return_mode:0x%02x, factoryInfo:0x%02x, utc:%u %u " + "%u %u %u %u\n", + GetStsID0(), GetData0(), GetStsID1(), GetData1(), HasShutdown(), + GetTimestamp(), GetReturnMode(), GetFactoryInfo(), + GetUTCData(0), GetUTCData(1), GetUTCData(2), GetUTCData(3), + GetUTCData(4), GetUTCData(5)); + } + } PACKED; + + // Tail + // seqnum + struct HS_LIDAR_TAIL_SEQ_NUM_ET_V5 { + uint32_t m_u32SeqNum; + uint32_t GetSeqNum() const { return little_to_native(m_u32SeqNum); } + static uint32_t GetSeqNumSize() { return sizeof(m_u32SeqNum); } + + void CalPktLoss(uint32_t &u32StartSeqNum, uint32_t &u32LastSeqNum, uint32_t &u32LossCount, uint32_t &u32StartTime) const { + bool print = false; + if (m_u32SeqNum - u32LastSeqNum > 1) { + u32LossCount += (m_u32SeqNum - u32LastSeqNum - 1); + print = true; + } + + if (GetMicroTickCount() - u32StartTime >= 1 * 1000 * 1000) { + printf("pkt loss freq: %u/%u\n", u32LossCount, m_u32SeqNum - u32StartSeqNum); + u32LossCount = 0; + u32StartTime = GetMicroTickCount(); + u32StartSeqNum = m_u32SeqNum; + } + u32LastSeqNum = m_u32SeqNum; + } + + void Print() const { + printf("HS_LIDAR_TAIL_SEQ_NUM_ET_V5:\n"); + printf("seqNum: %u\n", GetSeqNum()); + } + } PACKED; + + // Tail + // crc + struct HS_LIDAR_TAIL_CRC_ET_V5 { + uint32_t m_u32Crc; + uint32_t GetCrc() const { return little_to_native(m_u32Crc); } + void Print() const { + printf("HS_LIDAR_TAIL_CRC_ET_V5:\n"); + printf("crc:0x%08x\n", GetCrc()); + } + } PACKED; + + // Cyber Security (optional) + struct HS_LIDAR_CYBER_SECURITY_ET_V5 { + uint8_t m_u8Signature[32]; + uint8_t GetSignatureData(uint8_t index) const { + return m_u8Signature[index < sizeof(m_u8Signature) ? index : 0]; + } + void Print() const { + printf("HS_LIDAR_CYBER_SECURITY_ET_V5:\n"); + for (uint8_t i = 0; i < sizeof(m_u8Signature); i++) + printf("Signature%d:%d, ", i, GetSignatureData(i)); + printf("\n"); + } + } PACKED; + + // Header + struct HS_LIDAR_HEADER_ET_V5 { + static const uint8_t kLaserNum = 0x40; + static const uint8_t kBlockNum = 0x04; + static const uint8_t kFirstBlockLastReturn = 0x01; + static const uint8_t kFirstBlockStrongestReturn = 0x02; + static const uint8_t kDistUnit = 0x05; + static const uint8_t kSeqNum = 0x08; + static const uint8_t kSequenceNum = 0x01; // bit0 + static const uint8_t kIMU = 0x02; // bit1 + static const uint8_t kFunctionSafety = 0x04; // bit2 + static const uint8_t kCyberSecurity = 0x08; // bit3 + static const uint8_t kConfidenceLevel = 0x10; // bit4 + static const uint8_t kWeightFactor = 0x20; // bit5 + + uint8_t m_u8LaserNum; + uint8_t m_u8BlockNum; + uint8_t m_u8EchoCount; + uint8_t m_u8DistUnit; + uint8_t m_u8EchoNum; + uint8_t m_u8SeqNum; + uint8_t m_u8Reserved2; + + + uint8_t GetLaserNum() const { return m_u8LaserNum; } + uint8_t GetBlockNum() const { return m_u8BlockNum; } + double GetDistUnit() const { return m_u8DistUnit / 1000.f; } + uint8_t GetEchoCount() const { return m_u8EchoCount; } + uint8_t GetSeqNum() const { return m_u8SeqNum; } + + bool IsFirstBlockLastReturn() const { + return m_u8EchoCount == kFirstBlockLastReturn; + } + bool IsFirstBlockStrongestReturn() const { + return m_u8EchoCount == kFirstBlockStrongestReturn; + } + uint8_t GetEchoNum() const { return m_u8EchoNum; } + bool HasSeqNum() const { return m_u8Reserved2 & kSequenceNum; } + bool HasIMU() const { return m_u8Reserved2 & kIMU; } + bool HasFuncSafety() const { return m_u8Reserved2& kFunctionSafety; } + bool HasCyberSecurity() const { return m_u8Reserved2 & kCyberSecurity; } + bool HasConfidenceLevel() const { return m_u8Reserved2 & kConfidenceLevel; } + bool HasWeightFactor() const { return m_u8Reserved2 & kWeightFactor ; } + + uint16_t GetPacketSize() const { + return (sizeof(struct HS_LIDAR_PRE_HEADER) + sizeof(struct HS_LIDAR_HEADER_ET_V5) + + (sizeof(uint16_t) + (sizeof(struct HS_LIDAR_BODY_LASTER_UNIT_ET_V5) * (m_u8LaserNum / m_u8SeqNum) + 5) * m_u8SeqNum) * m_u8BlockNum + + sizeof(struct HS_LIDAR_BODY_CRC_ET_V5) + + sizeof(struct HS_LIDAR_TAIL_ET_V5) + sizeof(struct HS_LIDAR_TAIL_SEQ_NUM_ET_V5) + sizeof(struct HS_LIDAR_TAIL_CRC_ET_V5) + + sizeof(struct HS_LIDAR_CYBER_SECURITY_ET_V5) + ); + } + void Print() const { + printf("HS_LIDAR_HEADER_ET_V5:\n"); + printf( + "laserNum:%02u, block_num:%02u, DistUnit:%g, EchoCnt:%02u, " + "EchoNum:%02u, HasSeqNum:%d, HasIMU:%d, " + "HasFuncSafety:%d, HasCyberSecurity:%d, HasConfidence:%d\n", + GetLaserNum(), GetBlockNum(), GetDistUnit(), GetEchoCount(), + GetEchoNum(), HasSeqNum(), HasIMU(), HasFuncSafety(), + HasCyberSecurity(), HasConfidenceLevel()); + } + } PACKED; + } // namespace lidar +} // namespace hesai + +#endif \ No newline at end of file diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpProtocol/udp_protocol_v3_1.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpProtocol/udp_protocol_v3_1.h new file mode 100644 index 0000000..28cdbbb --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpProtocol/udp_protocol_v3_1.h @@ -0,0 +1,267 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ +#ifndef HS_LIDAR_QT_V1_H +#define HS_LIDAR_QT_V1_H + +#include +namespace hesai +{ +namespace lidar +{ + +#ifdef _MSC_VER +#define PACKED +#pragma pack(push, 1) +#else +#define PACKED __attribute__((packed)) +#endif + +struct HS_LIDAR_BODY_AZIMUTH_QT_V1 { + uint16_t m_u16Azimuth; + + uint16_t GetAzimuth() const { return little_to_native(m_u16Azimuth); } + + void Print() const { + printf("HS_LIDAR_BODY_AZIMUTH_QT_V1: azimuth:%u\n", GetAzimuth()); + } +} PACKED; + +struct HS_LIDAR_BODY_CHN_NNIT_QT_V1 { + uint16_t m_u16Distance; + uint8_t m_u8Reflectivity; + uint8_t m_u8Confidence; + + uint16_t GetDistance() const { return little_to_native(m_u16Distance); } + uint8_t GetReflectivity() const { return m_u8Reflectivity; } + uint8_t GetConfidenceLevel() const { return m_u8Confidence; } + + void Print() const { + printf("HS_LIDAR_BODY_CHN_NNIT_QT_V1:\n"); + printf("Dist:%u, Reflectivity: %u, confidenceLevel:%d\n", GetDistance(), + GetReflectivity(), GetConfidenceLevel()); + } +} PACKED; + +struct HS_LIDAR_TAIL_QT_V1 { + // shutdown flag, bit 0 + static const uint8_t kShutdown = 0x01; + + // return mode + static const uint8_t kStrongestReturn = 0x37; + static const uint8_t kLastReturn = 0x38; + static const uint8_t kDualReturn = 0x39; + + ReservedInfo1 m_reservedInfo1; + ReservedInfo2 m_reservedInfo2; + uint8_t m_u8Shutdown; + ReservedInfo3 m_reservedInfo3; + uint16_t m_u16MotorSpeed; + uint32_t m_u32Timestamp; + uint8_t m_u8ReturnMode; + uint8_t m_u8FactoryInfo; + uint8_t m_u8UTC[6]; + uint32_t m_u32SeqNum; + + uint8_t GetStsID0() const { return m_reservedInfo1.GetID(); } + uint16_t GetData0() const { return m_reservedInfo1.GetData(); } + + uint8_t GetStsID1() const { return m_reservedInfo2.GetID(); } + uint16_t GetData1() const { return m_reservedInfo2.GetData(); } + + uint8_t HasShutdown() const { return m_u8Shutdown & kShutdown; } + + uint8_t GetStsID2() const { return m_reservedInfo3.GetID(); } + uint16_t GetData2() const { return m_reservedInfo3.GetData(); } + + uint16_t GetMotorSpeed() const { return little_to_native(m_u16MotorSpeed); } + + uint32_t GetTimestamp() const { return little_to_native(m_u32Timestamp); } + + uint8_t GetReturnMode() const { return m_u8ReturnMode; } + bool IsLastReturn() const { return m_u8ReturnMode == kLastReturn; } + bool IsStrongestReturn() const { return m_u8ReturnMode == kStrongestReturn; } + bool IsDualReturn() const { return m_u8ReturnMode == kDualReturn; } + + uint8_t GetFactoryInfo() const { return m_u8FactoryInfo; } + + uint8_t GetUTCData(uint8_t index) const { + return m_u8UTC[index < sizeof(m_u8UTC) ? index : 0]; + } + + uint32_t GetSeqNum() const { return little_to_native(m_u32SeqNum); } + + void CalPktLoss(uint32_t &u32StartSeqNum, uint32_t &u32LastSeqNum, uint32_t &u32LossCount, + uint32_t &u32StartTime, uint32_t &u32TotalLossCount, uint32_t &u32TotalStartSeqNum) const { + bool print = false; + if (u32StartSeqNum == 0) { + u32LossCount = 0; + u32TotalLossCount = 0; + u32StartTime = GetMicroTickCount(); + u32StartSeqNum = m_u32SeqNum; + u32LastSeqNum = m_u32SeqNum; + u32TotalStartSeqNum = m_u32SeqNum; + return; + } + if (m_u32SeqNum - u32LastSeqNum > 1) { + u32LossCount += (m_u32SeqNum - u32LastSeqNum - 1); + u32TotalLossCount += (m_u32SeqNum - u32LastSeqNum - 1); + print = true; + // if (m_u32SeqNum - u32LastSeqNum - 1 > 1000) + // printf("%d, %u, %u\n", m_u32SeqNum - u32LastSeqNum - 1, u32LastSeqNum, + // m_u32SeqNum); + } + + // print log every 1s + if (GetMicroTickCount() - u32StartTime >= 1 * 1000 * 1000) { + printf("pkt loss freq: %u/%u\n", u32LossCount, + m_u32SeqNum - u32StartSeqNum); + u32LossCount = 0; + u32StartTime = GetMicroTickCount(); + u32StartSeqNum = m_u32SeqNum; + } + + u32LastSeqNum = m_u32SeqNum; + } + + void CalPktLoss(uint32_t &u32StartSeqNum, uint32_t &u32LastSeqNum, uint32_t &u32LossCount, uint32_t &u32StartTime) const { + bool print = false; + if (m_u32SeqNum - u32LastSeqNum > 1) { + u32LossCount += (m_u32SeqNum - u32LastSeqNum - 1); + print = true; + } + if (GetMicroTickCount() - u32StartTime >= 1 * 1000 * 1000) { + printf("pkt loss freq: %u/%u\n", u32LossCount, + m_u32SeqNum - u32StartSeqNum); + u32LossCount = 0; + u32StartTime = GetMicroTickCount(); + u32StartSeqNum = m_u32SeqNum; + } + + u32LastSeqNum = m_u32SeqNum; + } + static uint32_t GetSeqNumSize() { return sizeof(m_u32SeqNum); } + int64_t GetMicroLidarTimeU64() const { + if (m_u8UTC[0] != 0) { + struct tm t = {0}; + t.tm_year = m_u8UTC[0] + 100; + if (t.tm_year >= 200) { + t.tm_year -= 100; + } + t.tm_mon = m_u8UTC[1] - 1; + t.tm_mday = m_u8UTC[2]; + t.tm_hour = m_u8UTC[3]; + t.tm_min = m_u8UTC[4]; + t.tm_sec = m_u8UTC[5]; + t.tm_isdst = 0; +#ifdef _MSC_VER + TIME_ZONE_INFORMATION tzi; + GetTimeZoneInformation(&tzi); + long int timezone = tzi.Bias * 60; +#endif + return (mktime(&t) - timezone) * 1000000 + GetTimestamp(); + } + else { + uint32_t utc_time_big = *(uint32_t*)(&m_u8UTC[0] + 2); + int64_t unix_second = ((utc_time_big >> 24) & 0xff) | + ((utc_time_big >> 8) & 0xff00) | + ((utc_time_big << 8) & 0xff0000) | + ((utc_time_big << 24)); + return unix_second * 1000000 + GetTimestamp(); + } + + } + + void Print() const { + printf("HS_LIDAR_TAIL_QT_V1:\n"); + printf( + "sts0:%d, data0:%d, sts1:%d, data1:%d, shutDown:%d, motorSpeed:%u, " + "timestamp:%u, return_mode:0x%02x, factoryInfo:0x%02x, utc:%u %u " + "%u %u %u %u, seqNum: %u\n", + GetStsID0(), GetData0(), GetStsID1(), GetData1(), HasShutdown(), + GetMotorSpeed(), GetTimestamp(), GetReturnMode(), GetFactoryInfo(), + GetUTCData(0), GetUTCData(1), GetUTCData(2), GetUTCData(3), + GetUTCData(4), GetUTCData(5), GetSeqNum()); + } + +} PACKED; + +struct HS_LIDAR_HEADER_QT_V1 { + static const uint8_t kSequenceNum = 0x01; + static const uint8_t kIMU = 0x02; + static const uint8_t kDistUnit = 0x04; + static const uint8_t kFirstBlockLastReturn = 0x01; + static const uint8_t kFirstBlockStrongestReturn = 0x02; + + uint8_t m_u8LaserNum; + uint8_t m_u8BlockNum; + uint8_t m_u8EchoCount; + uint8_t m_u8DistUnit; + uint8_t m_u8EchoNum; + uint8_t m_u8Status; + + uint8_t GetLaserNum() const { return m_u8LaserNum; } + uint8_t GetBlockNum() const { return m_u8BlockNum; } + double GetDistUnit() const { return m_u8DistUnit / 1000.f; } + uint8_t GetEchoCount() const { return m_u8EchoCount; } + bool IsFirstBlockLastReturn() const { + return m_u8EchoCount == kFirstBlockLastReturn; + } + bool IsFirstBlockStrongestReturn() const { + return m_u8EchoCount == kFirstBlockStrongestReturn; + } + uint8_t GetEchoNum() const { return m_u8EchoNum; } + bool HasSeqNum() const { return m_u8Status & kSequenceNum; } + bool HasIMU() const { return m_u8Status & kIMU; } + + uint16_t GetPacketSize() const { + uint16_t u16TailDelta = + (HasSeqNum() ? 0 : HS_LIDAR_TAIL_QT_V1::GetSeqNumSize()); + + return sizeof(HS_LIDAR_PRE_HEADER) + sizeof(HS_LIDAR_HEADER_QT_V1) + + sizeof(HS_LIDAR_TAIL_QT_V1) + + (sizeof(HS_LIDAR_BODY_AZIMUTH_QT_V1) + + sizeof(HS_LIDAR_BODY_CHN_NNIT_QT_V1) * GetLaserNum()) * + GetBlockNum() - + u16TailDelta; + } + + void Print() const { + printf("HS_LIDAR_HEADER_QT_V1:\n"); + printf( + "laserNum:%02u, block_num:%02u, DistUnit:%g, EchoCnt:%02u, " + "EchoNum:%02u, HasSeqNum:%d\n", + GetLaserNum(), GetBlockNum(), GetDistUnit(), GetEchoCount(), + GetEchoNum(), HasSeqNum()); + } +} PACKED; +#ifdef _MSC_VER +#pragma pack(pop) +#endif +} // namespace lidar +} // namespace hesai +#endif diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpProtocol/udp_protocol_v3_2.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpProtocol/udp_protocol_v3_2.h new file mode 100644 index 0000000..c9e23fe --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpProtocol/udp_protocol_v3_2.h @@ -0,0 +1,377 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ +#ifndef HS_LIDAR_QT_V2_H +#define HS_LIDAR_QT_V2_H + +#include +namespace hesai +{ +namespace lidar +{ + +#ifdef _MSC_VER +#define PACKED +#pragma pack(push, 1) +#else +#define PACKED __attribute__((packed)) +#endif + + +struct HS_LIDAR_BODY_AZIMUTH_QT_V2 { + uint16_t m_u16Azimuth; + + uint16_t GetAzimuth() const { return little_to_native(m_u16Azimuth); } + + void Print() const { + printf("HS_LIDAR_BODY_AZIMUTH_QT_V2: azimuth:%u\n", GetAzimuth()); + } +} PACKED; + +struct HS_LIDAR_BODY_CHN_UNIT_QT_V2 { + uint16_t m_u16Distance; + uint8_t m_u8Reflectivity; + uint8_t m_u8Confidence; + + uint16_t GetDistance() const { return little_to_native(m_u16Distance); } + uint8_t GetReflectivity() const { return m_u8Reflectivity; } + uint8_t GetConfidenceLevel() const { return m_u8Confidence; } + + void Print() const { + printf("HS_LIDAR_BODY_CHN_UNIT_QT_V2:\n"); + printf("Dist:%u, Reflectivity: %u, confidenceLevel:%u\n", GetDistance(), + GetReflectivity(), GetConfidenceLevel()); + } +} PACKED; + +struct HS_LIDAR_BODY_CHN_UNIT_NO_CONF_QT_V2 { + uint16_t m_u16Distance; + uint8_t m_u8Reflectivity; + + uint16_t GetDistance() const { return little_to_native(m_u16Distance); } + uint8_t GetReflectivity() const { return m_u8Reflectivity; } + + void Print() const { + printf("HS_LIDAR_BODY_CHN_UNIT_QT_V2:\n"); + printf("Dist:%u, Reflectivity: %u, n", GetDistance(), GetReflectivity()); + } +} PACKED; + +struct HS_LIDAR_BODY_CRC_QT_V2 { + uint32_t m_u32Crc; + + uint32_t GetCrc() const { return little_to_native(m_u32Crc); } + + void Print() const { + printf("HS_LIDAR_BODY_CRC_QT_V2:\n"); + printf("crc:0x%08x\n", GetCrc()); + } +} PACKED; + +struct HS_LIDAR_FUNCTION_SAFETY { + uint8_t m_u8FSVersion; + uint8_t m_u8State; + uint8_t m_u8Code; + uint16_t m_u16OutCode; + uint8_t m_u8Reserved[8]; + HS_LIDAR_BODY_CRC_QT_V2 m_crc; +} PACKED; + +struct HS_LIDAR_TAIL_QT_V2 { + // shutdown flag, bit 0 + static const uint8_t kShutdown = 0x01; + + // return mode + static const uint8_t kFirstReturn = 0x33; + static const uint8_t kStrongestReturn = 0x37; + static const uint8_t kLastReturn = 0x38; + static const uint8_t kLastAndStrongestReturn = 0x39; + static const uint8_t kFirstAndLastReturn = 0x3b; + static const uint8_t kFirstAndStrongestReturn = 0x3c; + static const uint8_t kStrongestAndSecondReturn = 0x3e; + static const uint8_t kSecondReturn = 0x34; + static const uint8_t kFirstAndSecondReturn = 0x3a; + + ReservedInfo1 m_reservedInfo1; + uint16_t m_reservedInfo2; + uint8_t m_u8ModeFlag; + ReservedInfo3 m_reservedInfo3; + uint16_t m_u16AzimuthFlag; + uint8_t m_u8WorkingMode; + uint8_t m_u8ReturnMode; + uint16_t m_u16MotorSpeed; + uint8_t m_u8UTC[6]; + uint32_t m_u32Timestamp; + uint8_t m_u8FactoryInfo; + + uint8_t GetStsID1() const { return m_reservedInfo1.GetID(); } + uint16_t GetData1() const { return m_reservedInfo1.GetData(); } + uint8_t GetStsID3() const { return m_reservedInfo3.GetID(); } + uint16_t GetData3() const { return m_reservedInfo3.GetData(); } + uint8_t GetModeFlag() const { return m_u8ModeFlag; } + uint16_t GetMotorSpeed() const { return little_to_native(m_u16MotorSpeed); } + uint32_t GetTimestamp() const { return little_to_native(m_u32Timestamp); } + uint8_t GetReturnMode() const { return m_u8ReturnMode; } + bool IsLastReturn() const { return m_u8ReturnMode == kLastReturn; } + bool IsStrongestReturn() const { return m_u8ReturnMode == kStrongestReturn; } + bool IsLastAndStrongestReturn() const { + return m_u8ReturnMode == kLastAndStrongestReturn; + } + uint8_t GetFactoryInfo() const { return m_u8FactoryInfo; } + uint8_t GetUTCData(uint8_t index) const { + return m_u8UTC[index < sizeof(m_u8UTC) ? index : 0]; + } + + int64_t GetMicroLidarTimeU64() const { + if (m_u8UTC[0] != 0) { + struct tm t = {0}; + t.tm_year = m_u8UTC[0] + 100; + if (t.tm_year >= 200) { + t.tm_year -= 100; + } + t.tm_mon = m_u8UTC[1] - 1; + t.tm_mday = m_u8UTC[2]; + t.tm_hour = m_u8UTC[3]; + t.tm_min = m_u8UTC[4]; + t.tm_sec = m_u8UTC[5]; + t.tm_isdst = 0; +#ifdef _MSC_VER + TIME_ZONE_INFORMATION tzi; + GetTimeZoneInformation(&tzi); + long int timezone = tzi.Bias * 60; +#endif + return (mktime(&t) - timezone) * 1000000 + GetTimestamp(); + } + else { + uint32_t utc_time_big = *(uint32_t*)(&m_u8UTC[0] + 2); + int64_t unix_second = ((utc_time_big >> 24) & 0xff) | + ((utc_time_big >> 8) & 0xff00) | + ((utc_time_big << 8) & 0xff0000) | + ((utc_time_big << 24)); + return unix_second * 1000000 + GetTimestamp(); + } + } + + void Print() const { + printf("HS_LIDAR_TAIL_QT_V2:\n"); + printf( + "sts1:%d, data1:%d, motorSpeed:%u, timestamp:%u, return_mode:0x%02x, " + "factoryInfo:0x%02x, utc:%u %u %u %u %u %u,\n", + GetStsID1(), GetData1(), GetMotorSpeed(), GetTimestamp(), + GetReturnMode(), GetFactoryInfo(), GetUTCData(0), GetUTCData(1), + GetUTCData(2), GetUTCData(3), GetUTCData(4), GetUTCData(5)); + } + +} PACKED; + +struct HS_LIDAR_TAIL_SEQ_NUM_QT_V2 { + uint32_t m_u32SeqNum; + + uint32_t GetSeqNum() const { return little_to_native(m_u32SeqNum); } + static uint32_t GetSeqNumSize() { return sizeof(m_u32SeqNum); } + + void CalPktLoss(uint32_t &u32StartSeqNum, uint32_t &u32LastSeqNum, uint32_t &u32LossCount, + uint32_t &u32StartTime, uint32_t &u32TotalLossCount, uint32_t &u32TotalStartSeqNum) const { + bool print = false; + if (u32StartSeqNum == 0) { + u32LossCount = 0; + u32TotalLossCount = 0; + u32StartTime = GetMicroTickCount(); + u32StartSeqNum = m_u32SeqNum; + u32LastSeqNum = m_u32SeqNum; + u32TotalStartSeqNum = m_u32SeqNum; + return; + } + if (m_u32SeqNum - u32LastSeqNum > 1) { + u32LossCount += (m_u32SeqNum - u32LastSeqNum - 1); + u32TotalLossCount += (m_u32SeqNum - u32LastSeqNum - 1); + print = true; + // if (m_u32SeqNum - u32LastSeqNum - 1 > 1000) + // printf("%d, %u, %u\n", m_u32SeqNum - u32LastSeqNum - 1, u32LastSeqNum, + // m_u32SeqNum); + } + + // print log every 1s + if (GetMicroTickCount() - u32StartTime >= 1 * 1000 * 1000) { + printf("pkt loss freq: %u/%u\n", u32LossCount, + m_u32SeqNum - u32StartSeqNum); + u32LossCount = 0; + u32StartTime = GetMicroTickCount(); + u32StartSeqNum = m_u32SeqNum; + } + + u32LastSeqNum = m_u32SeqNum; + } + + void CalPktLoss(uint32_t &u32StartSeqNum, uint32_t &u32LastSeqNum, uint32_t &u32LossCount, uint32_t &u32StartTime) const { + bool print = false; + if (m_u32SeqNum - u32LastSeqNum > 1) { + u32LossCount += (m_u32SeqNum - u32LastSeqNum - 1); + print = true; + } + if (GetMicroTickCount() - u32StartTime >= 1 * 1000 * 1000) { + printf("pkt loss freq: %u/%u\n", u32LossCount, + m_u32SeqNum - u32StartSeqNum); + u32LossCount = 0; + u32StartTime = GetMicroTickCount(); + u32StartSeqNum = m_u32SeqNum; + } + + u32LastSeqNum = m_u32SeqNum; + } + + void CalPktLoss() const { + static uint32_t u32StartSeqNum = m_u32SeqNum; + static uint32_t u32LastSeqNum = m_u32SeqNum; + static int32_t u32LossCount = 0; + static uint32_t u32StartTime = GetMicroTickCount(); + bool print = false; + if (m_u32SeqNum - u32LastSeqNum > 1) { + u32LossCount += (m_u32SeqNum - u32LastSeqNum - 1); + print = true; + // if (m_u32SeqNum - u32LastSeqNum - 1 > 1000) + // printf("%d, %u, %u\n", m_u32SeqNum - u32LastSeqNum - 1, u32LastSeqNum, + // m_u32SeqNum); + } + + // print log every 10s + if (GetMicroTickCount() - u32StartTime >= 10 * 1000 * 1000) { + printf("pkt loss freq: %u/%u\n", u32LossCount, + m_u32SeqNum - u32StartSeqNum); + u32LossCount = 0; + u32StartTime = GetMicroTickCount(); + u32StartSeqNum = m_u32SeqNum; + } + + u32LastSeqNum = m_u32SeqNum; + } + + void Print() const { + printf("HS_LIDAR_TAIL_SEQ_NUM_QT_V2:\n"); + printf("seqNum: %u\n", GetSeqNum()); + } +} PACKED; + +struct HS_LIDAR_TAIL_CRC_QT_V2 { + uint32_t m_u32Crc; + + uint32_t GetCrc() const { return little_to_native(m_u32Crc); } + + void Print() const { + printf("HS_LIDAR_TAIL_CRC_QT_V2:\n"); + printf("crc:0x%08x\n", GetCrc()); + } +} PACKED; + +struct HS_LIDAR_CYBER_SECURITY_QT_V2 { + uint8_t m_u8Signature[32]; + + uint8_t GetSignatureData(uint8_t index) const { + return m_u8Signature[index < sizeof(m_u8Signature) ? index : 0]; + } + + void Print() const { + printf("HS_LIDAR_CYBER_SECURITY_QT_V2:\n"); + for (uint8_t i = 0; i < sizeof(m_u8Signature); i++) + printf("Signature%d:%d, ", i, GetSignatureData(i)); + printf("\n"); + } +} PACKED; + +struct HS_LIDAR_CYBER_SECURITYQ_QT_V2 { + uint8_t m_u8Signature[32]; +} PACKED; + +struct HS_LIDAR_HEADER_QT_V2 { + static const uint8_t kSequenceNum = 0x01; + static const uint8_t kIMU = 0x02; + static const uint8_t kFunctionSafety = 0x04; + static const uint8_t kCyberSecurity = 0x08; + static const uint8_t kConfidenceLevel = 0x10; + static const uint8_t kSlope = 0x20; + static const uint8_t kSelfDefine = 0x40; + + static const uint8_t kDistUnit = 0x04; + static const uint8_t kFirstBlockLastReturn = 0x01; + static const uint8_t kFirstBlockStrongestReturn = 0x02; + + uint8_t m_u8LaserNum; + uint8_t m_u8BlockNum; + uint8_t m_u8EchoCount; + uint8_t m_u8DistUnit; + uint8_t m_u8EchoNum; + uint8_t m_u8Status; + + uint8_t GetLaserNum() const { return m_u8LaserNum; } + uint8_t GetBlockNum() const { return m_u8BlockNum; } + double GetDistUnit() const { return m_u8DistUnit / 1000.f; } + uint8_t GetEchoCount() const { return m_u8EchoCount; } + bool IsFirstBlockLastReturn() const { + return m_u8EchoCount == kFirstBlockLastReturn; + } + bool IsFirstBlockStrongestReturn() const { + return m_u8EchoCount == kFirstBlockStrongestReturn; + } + uint8_t GetEchoNum() const { return m_u8EchoNum; } + bool HasSeqNum() const { return m_u8Status & kSequenceNum; } + bool HasFunctionSafety() const { return m_u8Status & kFunctionSafety; } + bool HasCyberSecurity() const { return m_u8Status & kCyberSecurity; } + bool HasConfidenceLevel() const { return m_u8Status & kConfidenceLevel; } + bool HasSlope() const { return m_u8Status & kSlope; } + bool HasSelfDefine() const { return m_u8Status & kSelfDefine; } + + uint16_t GetPacketSize() const { + return sizeof(HS_LIDAR_PRE_HEADER) + sizeof(HS_LIDAR_HEADER_QT_V2) + + (sizeof(HS_LIDAR_BODY_AZIMUTH_QT_V2) + + (HasConfidenceLevel() + ? sizeof(HS_LIDAR_BODY_CHN_UNIT_QT_V2) + : sizeof(HS_LIDAR_BODY_CHN_UNIT_NO_CONF_QT_V2)) * + GetLaserNum()) * + GetBlockNum() + + sizeof(HS_LIDAR_BODY_CRC_QT_V2) + + (HasFunctionSafety() ? sizeof(HS_LIDAR_FUNCTION_SAFETY) : 0) + + sizeof(HS_LIDAR_TAIL_QT_V2) + + (HasSeqNum() ? sizeof(HS_LIDAR_TAIL_SEQ_NUM_QT_V2) : 0) + + sizeof(HS_LIDAR_TAIL_CRC_QT_V2) + + (HasCyberSecurity() ? sizeof(HS_LIDAR_CYBER_SECURITY_QT_V2) : 0); + } + + void Print() const { + printf("HS_LIDAR_HEADER_QT_V2:\n"); + printf( + "laserNum:%02u, block_num:%02u, DistUnit:%g, EchoCnt:%02u, " + "EchoNum:%02u, HasSeqNum:%d\n", + GetLaserNum(), GetBlockNum(), GetDistUnit(), GetEchoCount(), + GetEchoNum(), HasSeqNum()); + } +} PACKED; +#ifdef _MSC_VER +#pragma pack(pop) +#endif +} // namespace lidar +} // namespace hesai +#endif diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpProtocol/udp_protocol_v4_3.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpProtocol/udp_protocol_v4_3.h new file mode 100644 index 0000000..b3340c2 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpProtocol/udp_protocol_v4_3.h @@ -0,0 +1,347 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ +#ifndef HS_LIDAR_ST_V3_H +#define HS_LIDAR_ST_V3_H + +#include +#include "plat_utils.h" +namespace hesai +{ +namespace lidar +{ +#ifdef _MSC_VER +#define PACKED +#pragma pack(push, 1) +#else +#define PACKED __attribute__((packed)) +#endif + +struct HS_LIDAR_BODY_AZIMUTH_ST_V3 { + uint16_t m_u16Azimuth; + + uint16_t GetAzimuth() const { return little_to_native(m_u16Azimuth); } + + void Print() const { + printf("HS_LIDAR_BODY_AZIMUTH_ST_V3: azimuth:%u\n", GetAzimuth()); + } +} PACKED; + +struct HS_LIDAR_BODY_FINE_AZIMUTH_ST_V3 { + uint8_t m_u8FineAzimuth; + + uint8_t GetFineAzimuth() const { return little_to_native(m_u8FineAzimuth); } + + void Print() const { + printf("HS_LIDAR_BODY_FINE_AZIMUTH_ST_V3: FineAzimuth:%u\n", + GetFineAzimuth()); + } +} PACKED; + +struct HS_LIDAR_BODY_CHN_NNIT_ST_V3 { + uint16_t m_u16Distance; + uint8_t m_u8Reflectivity; + uint8_t m_u8Confidence; + + uint16_t GetDistance() const { return little_to_native(m_u16Distance); } + uint8_t GetReflectivity() const { return m_u8Reflectivity; } + uint8_t GetConfidenceLevel() const { return m_u8Confidence; } + + void Print() const { + printf("HS_LIDAR_BODY_CHN_NNIT_ST_V3:\n"); + printf("Dist:%u, Reflectivity: %u, confidenceLevel:%d\n", GetDistance(), + GetReflectivity(), GetConfidenceLevel()); + } +} PACKED; + + +struct HS_LIDAR_BODY_CRC_ST_V3 { + uint32_t m_u32Crc; + + uint32_t GetCrc() const { return little_to_native(m_u32Crc); } + + void Print() const { + printf("HS_LIDAR_BODY_CRC_ST_V3:\n"); + printf("crc:0x%08x\n", GetCrc()); + } +} PACKED; + +struct HS_LIDAR_TAIL_ST_V3 { + // shutdown flag, bit 0 + static const uint8_t kShutdown = 0x01; + + // return mode + static const uint8_t kStrongestReturn = 0x37; + static const uint8_t kLastReturn = 0x38; + static const uint8_t kDualReturn = 0x39; + + ReservedInfo1 m_reservedInfo1; + ReservedInfo2 m_reservedInfo2; + uint8_t m_u8Shutdown; + ReservedInfo3 m_reservedInfo3; + uint8_t m_u8UReserved[8]; + int16_t m_i16MotorSpeed; + uint32_t m_u32Timestamp; + uint8_t m_u8ReturnMode; + uint8_t m_u8FactoryInfo; + uint8_t m_u8UTC[6]; + // uint32_t m_u32SeqNum; + + uint8_t GetStsID0() const { return m_reservedInfo1.GetID(); } + uint16_t GetData0() const { return m_reservedInfo1.GetData(); } + + uint8_t GetStsID1() const { return m_reservedInfo2.GetID(); } + uint16_t GetData1() const { return m_reservedInfo2.GetData(); } + + uint8_t HasShutdown() const { return m_u8Shutdown & kShutdown; } + + uint8_t GetStsID2() const { return m_reservedInfo3.GetID(); } + uint16_t GetData2() const { return m_reservedInfo3.GetData(); } + + int16_t GetMotorSpeed() const { return little_to_native(m_i16MotorSpeed); } + + uint32_t GetTimestamp() const { return little_to_native(m_u32Timestamp); } + + uint8_t GetReturnMode() const { return m_u8ReturnMode; } + bool IsLastReturn() const { return m_u8ReturnMode == kLastReturn; } + bool IsStrongestReturn() const { return m_u8ReturnMode == kStrongestReturn; } + bool IsDualReturn() const { return m_u8ReturnMode == kDualReturn; } + + uint8_t GetFactoryInfo() const { return m_u8FactoryInfo; } + + uint8_t GetUTCData(uint8_t index) const { + return m_u8UTC[index < sizeof(m_u8UTC) ? index : 0]; + } + int64_t GetMicroLidarTimeU64() const { + if (m_u8UTC[0] != 0) { + struct tm t = {0}; + t.tm_year = m_u8UTC[0]; + if (t.tm_year >= 200) { + t.tm_year -= 100; + } + t.tm_mon = m_u8UTC[1] - 1; + t.tm_mday = m_u8UTC[2] + 1; + t.tm_hour = m_u8UTC[3]; + t.tm_min = m_u8UTC[4]; + t.tm_sec = m_u8UTC[5]; + t.tm_isdst = 0; +#ifdef _MSC_VER + TIME_ZONE_INFORMATION tzi; + GetTimeZoneInformation(&tzi); + long int timezone = tzi.Bias * 60; +#endif + return (mktime(&t) - timezone - 86400) * 1000000 + GetTimestamp() ; + } + else { + uint32_t utc_time_big = *(uint32_t*)(&m_u8UTC[0] + 2); + int64_t unix_second = ((utc_time_big >> 24) & 0xff) | + ((utc_time_big >> 8) & 0xff00) | + ((utc_time_big << 8) & 0xff0000) | + ((utc_time_big << 24)); + return unix_second * 1000000 + GetTimestamp(); + } + + } + + // uint32_t GetSeqNum() const { return little_to_native(m_u32SeqNum); } + // static uint32_t GetSeqNumSize() { return sizeof(m_u32SeqNum); } + + void Print() const { + printf("HS_LIDAR_TAIL_ST_V3:\n"); + printf( + "sts0:%d, data0:%d, sts1:%d, data1:%d, shutDown:%d, motorSpeed:%d, " + "timestamp:%u, return_mode:0x%02x, factoryInfo:0x%02x, utc:%u %u " + "%u %u %u %u\n", + GetStsID0(), GetData0(), GetStsID1(), GetData1(), HasShutdown(), + GetMotorSpeed(), GetTimestamp(), GetReturnMode(), GetFactoryInfo(), + GetUTCData(0), GetUTCData(1), GetUTCData(2), GetUTCData(3), + GetUTCData(4), GetUTCData(5)); + } + +} PACKED; + +struct HS_LIDAR_TAIL_SEQ_NUM_ST_V3 { + uint32_t m_u32SeqNum; + + uint32_t GetSeqNum() const { return little_to_native(m_u32SeqNum); } + static uint32_t GetSeqNumSize() { return sizeof(m_u32SeqNum); } + + void CalPktLoss(uint32_t &u32StartSeqNum, uint32_t &u32LastSeqNum, uint32_t &u32LossCount, + uint32_t &u32StartTime, uint32_t &u32TotalLossCount, uint32_t &u32TotalStartSeqNum) const { + bool print = false; + if (u32StartSeqNum == 0) { + u32LossCount = 0; + u32TotalLossCount = 0; + u32StartTime = GetMicroTickCount(); + u32StartSeqNum = m_u32SeqNum; + u32LastSeqNum = m_u32SeqNum; + u32TotalStartSeqNum = m_u32SeqNum; + return; + } + if (m_u32SeqNum - u32LastSeqNum > 1) { + u32LossCount += (m_u32SeqNum - u32LastSeqNum - 1); + u32TotalLossCount += (m_u32SeqNum - u32LastSeqNum - 1); + print = true; + // if (m_u32SeqNum - u32LastSeqNum - 1 > 1000) + // printf("%d, %u, %u\n", m_u32SeqNum - u32LastSeqNum - 1, u32LastSeqNum, + // m_u32SeqNum); + } + + // print log every 1s + if (GetMicroTickCount() - u32StartTime >= 1 * 1000 * 1000) { + printf("pkt loss freq: %u/%u\n", u32LossCount, + m_u32SeqNum - u32StartSeqNum); + u32LossCount = 0; + u32StartTime = GetMicroTickCount(); + u32StartSeqNum = m_u32SeqNum; + } + u32LastSeqNum = m_u32SeqNum; + } + + void CalPktLoss(uint32_t &u32StartSeqNum, uint32_t &u32LastSeqNum, uint32_t &u32LossCount, uint32_t &u32StartTime) const { + // static uint32_t u32StartSeqNum = m_u32SeqNum; + // static uint32_t u32LastSeqNum = m_u32SeqNum; + // u32StartTime = GetMicroTickCount(); + bool print = false; + if (m_u32SeqNum - u32LastSeqNum > 1) { + u32LossCount += (m_u32SeqNum - u32LastSeqNum - 1); + print = true; + // if (m_u32SeqNum - u32LastSeqNum - 1 > 1000) + // printf("%d, %u, %u\n", m_u32SeqNum - u32LastSeqNum - 1, u32LastSeqNum, + // m_u32SeqNum); + } + + // print log every 10s + if (GetMicroTickCount() - u32StartTime >= 1 * 1000 * 1000) { + printf("pkt loss freq: %u/%u\n", u32LossCount, + m_u32SeqNum - u32StartSeqNum); + u32LossCount = 0; + u32StartTime = GetMicroTickCount(); + u32StartSeqNum = m_u32SeqNum; + } + + u32LastSeqNum = m_u32SeqNum; + } + + void Print() const { + printf("HS_LIDAR_TAIL_SEQ_NUM_ST_V3:\n"); + printf("seqNum: %u\n", GetSeqNum()); + } +} PACKED; + +struct HS_LIDAR_TAIL_CRC_ST_V3 { + uint32_t m_u32Crc; + + uint32_t GetCrc() const { return little_to_native(m_u32Crc); } + + void Print() const { + printf("HS_LIDAR_TAIL_CRC_ST_V3:\n"); + printf("crc:0x%08x\n", GetCrc()); + } +} PACKED; + +struct HS_LIDAR_CYBER_SECURITY_ST_V3 { + uint8_t m_u8Signature[32]; + + uint8_t GetSignatureData(uint8_t index) const { + return m_u8Signature[index < sizeof(m_u8Signature) ? index : 0]; + } + + void Print() const { + printf("HS_LIDAR_CYBER_SECURITY_ST_V3:\n"); + for (uint8_t i = 0; i < sizeof(m_u8Signature); i++) + printf("Signature%d:%d, ", i, GetSignatureData(i)); + printf("\n"); + } +} PACKED; + +struct HS_LIDAR_HEADER_ST_V3 { + static const uint8_t kSequenceNum = 0x01; + static const uint8_t kIMU = 0x02; + static const uint8_t kFunctionSafety = 0x04; + static const uint8_t kCyberSecurity = 0x08; + static const uint8_t kConfidenceLevel = 0x10; + + static const uint8_t kDistUnit = 0x04; + static const uint8_t kFirstBlockLastReturn = 0x01; + static const uint8_t kFirstBlockStrongestReturn = 0x02; + + uint8_t m_u8LaserNum; + uint8_t m_u8BlockNum; + uint8_t m_u8EchoCount; + uint8_t m_u8DistUnit; + uint8_t m_u8EchoNum; + uint8_t m_u8Status; + + uint8_t GetLaserNum() const { return m_u8LaserNum; } + uint8_t GetBlockNum() const { return m_u8BlockNum; } + double GetDistUnit() const { return m_u8DistUnit / 1000.f; } + uint8_t GetEchoCount() const { return m_u8EchoCount; } + + bool IsFirstBlockLastReturn() const { + return m_u8EchoCount == kFirstBlockLastReturn; + } + bool IsFirstBlockStrongestReturn() const { + return m_u8EchoCount == kFirstBlockStrongestReturn; + } + uint8_t GetEchoNum() const { return m_u8EchoNum; } + + bool HasSeqNum() const { return m_u8Status & kSequenceNum; } + bool HasIMU() const { return m_u8Status & kIMU; } + bool HasFuncSafety() const { return m_u8Status & kFunctionSafety; } + bool HasCyberSecurity() const { return m_u8Status & kCyberSecurity; } + bool HasConfidenceLevel() const { return m_u8Status & kConfidenceLevel; } + + uint16_t GetPacketSize() const { + return sizeof(HS_LIDAR_PRE_HEADER) + sizeof(HS_LIDAR_HEADER_ST_V3) + + (sizeof(HS_LIDAR_BODY_AZIMUTH_ST_V3) + + sizeof(HS_LIDAR_BODY_FINE_AZIMUTH_ST_V3) + + sizeof(HS_LIDAR_BODY_CHN_NNIT_ST_V3) * GetLaserNum()) * + GetBlockNum() + + sizeof(HS_LIDAR_BODY_CRC_ST_V3) + sizeof(HS_LIDAR_TAIL_ST_V3) + + (HasSeqNum() ? sizeof(HS_LIDAR_TAIL_SEQ_NUM_ST_V3) : 0) + + sizeof(HS_LIDAR_TAIL_CRC_ST_V3) + + (HasCyberSecurity() ? sizeof(HS_LIDAR_CYBER_SECURITY_ST_V3) : 0); + } + + void Print() const { + printf("HS_LIDAR_HEADER_ST_V3:\n"); + printf( + "laserNum:%02u, block_num:%02u, DistUnit:%g, EchoCnt:%02u, " + "EchoNum:%02u, HasSeqNum:%d, HasIMU:%d, " + "HasFuncSafety:%d, HasCyberSecurity:%d, HasConfidence:%d\n", + GetLaserNum(), GetBlockNum(), GetDistUnit(), GetEchoCount(), + GetEchoNum(), HasSeqNum(), HasIMU(), HasFuncSafety(), + HasCyberSecurity(), HasConfidenceLevel()); + } +} PACKED; +#ifdef _MSC_VER +#pragma pack(pop) +#endif +} // namespace lidar +} // namespace hesai +#endif diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpProtocol/udp_protocol_v6_1.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpProtocol/udp_protocol_v6_1.h new file mode 100644 index 0000000..f087abf --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpProtocol/udp_protocol_v6_1.h @@ -0,0 +1,290 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ + +#ifndef HSLIDARXTV1_H +#define HSLIDARXTV1_H + +#include +namespace hesai +{ +namespace lidar +{ +#ifdef _MSC_VER +#define PACKED +#pragma pack(push, 1) +#else +#define PACKED __attribute__((packed)) +#endif + +struct HsLidarXTV1BodyAzimuth { + uint16_t m_u16Azimuth; + + uint16_t GetAzimuth() const { return little_to_native(m_u16Azimuth); } + + void Print() const { + printf("HsLidarXTV1BodyAzimuth: azimuth:%u\n", GetAzimuth()); + } +} PACKED; + +struct HsLidarXTV1BodyChannelData { + uint16_t m_u16Distance; + uint8_t m_u8Reflectivity; + uint8_t m_u8Confidence; + + uint16_t GetDistance() const { return little_to_native(m_u16Distance); } + uint8_t GetReflectivity() const { return m_u8Reflectivity; } + uint8_t GetConfidenceLevel() const { return m_u8Confidence; } + + void Print() const { + printf("HsLidarXTV1BodyChannelData:\n"); + printf("Dist:%u, Reflectivity: %u, confidenceLevel:%d\n", GetDistance(), + GetReflectivity(), GetConfidenceLevel()); + } +} PACKED; + +struct HsLidarXTV1Tail { + // shutdown flag, bit 0 + static const uint8_t kShutdown = 0x01; + + // return mode + static const uint8_t kStrongestReturn = 0x37; + static const uint8_t kLastReturn = 0x38; + static const uint8_t kDualReturn = 0x39; + + ReservedInfo1 m_reservedInfo1; + ReservedInfo2 m_reservedInfo2; + ReservedInfo3 m_reservedInfo3; + uint8_t m_u8Shutdown; + uint8_t m_u8ReturnMode; + uint16_t m_u16MotorSpeed; + uint8_t m_u8UTC[6]; + uint32_t m_u32Timestamp; + uint8_t m_u8FactoryInfo; + uint32_t m_u32SeqNum; + + uint8_t GetStsID0() const { return m_reservedInfo1.GetID(); } + uint16_t GetData0() const { return m_reservedInfo1.GetData(); } + + uint8_t GetStsID1() const { return m_reservedInfo2.GetID(); } + uint16_t GetData1() const { return m_reservedInfo2.GetData(); } + + uint8_t HasShutdown() const { return m_u8Shutdown & kShutdown; } + + uint8_t GetStsID2() const { return m_reservedInfo3.GetID(); } + uint16_t GetData2() const { return m_reservedInfo3.GetData(); } + + uint16_t GetMotorSpeed() const { return little_to_native(m_u16MotorSpeed); } + + uint32_t GetTimestamp() const { return little_to_native(m_u32Timestamp); } + + int64_t GetMicroLidarTimeU64() const { + if (m_u8UTC[0] != 0) { + struct tm t = {0}; + t.tm_year = m_u8UTC[0] + 100; + if (t.tm_year >= 200) { + t.tm_year -= 100; + } + t.tm_mon = m_u8UTC[1] - 1; + t.tm_mday = m_u8UTC[2]; + t.tm_hour = m_u8UTC[3]; + t.tm_min = m_u8UTC[4]; + t.tm_sec = m_u8UTC[5]; + t.tm_isdst = 0; +#ifdef _MSC_VER + TIME_ZONE_INFORMATION tzi; + GetTimeZoneInformation(&tzi); + long int timezone = tzi.Bias * 60; +#endif + return (mktime(&t) - timezone) * 1000000 + GetTimestamp(); + } + else { + uint32_t utc_time_big = *(uint32_t*)(&m_u8UTC[0] + 2); + int64_t unix_second = ((utc_time_big >> 24) & 0xff) | + ((utc_time_big >> 8) & 0xff00) | + ((utc_time_big << 8) & 0xff0000) | + ((utc_time_big << 24)); + return unix_second * 1000000 + GetTimestamp(); + } + } + + uint8_t GetReturnMode() const { return m_u8ReturnMode; } + bool IsLastReturn() const { return m_u8ReturnMode == kLastReturn; } + bool IsStrongestReturn() const { return m_u8ReturnMode == kStrongestReturn; } + bool IsDualReturn() const { return m_u8ReturnMode == kDualReturn; } + + uint8_t GetFactoryInfo() const { return m_u8FactoryInfo; } + + uint8_t GetUTCData(uint8_t index) const { + return m_u8UTC[index < sizeof(m_u8UTC) ? index : 0]; + } + + uint32_t GetSeqNum() const { return little_to_native(m_u32SeqNum); } + + void CalPktLoss(uint32_t &u32StartSeqNum, uint32_t &u32LastSeqNum, uint32_t &u32LossCount, + uint32_t &u32StartTime, uint32_t &u32TotalLossCount, uint32_t &u32TotalStartSeqNum) const { + bool print = false; + if (u32StartSeqNum == 0) { + u32LossCount = 0; + u32TotalLossCount = 0; + u32StartTime = GetMicroTickCount(); + u32StartSeqNum = m_u32SeqNum; + u32LastSeqNum = m_u32SeqNum; + u32TotalStartSeqNum = m_u32SeqNum; + return; + } + if (m_u32SeqNum - u32LastSeqNum > 1) { + u32LossCount += (m_u32SeqNum - u32LastSeqNum - 1); + u32TotalLossCount += (m_u32SeqNum - u32LastSeqNum - 1); + print = true; + // if (m_u32SeqNum - u32LastSeqNum - 1 > 1000) + // printf("%d, %u, %u\n", m_u32SeqNum - u32LastSeqNum - 1, u32LastSeqNum, + // m_u32SeqNum); + } + + // print log every 1s + if (GetMicroTickCount() - u32StartTime >= 1 * 1000 * 1000) { + printf("pkt loss freq: %u/%u\n", u32LossCount, + m_u32SeqNum - u32StartSeqNum); + u32LossCount = 0; + u32StartTime = GetMicroTickCount(); + u32StartSeqNum = m_u32SeqNum; + } + + u32LastSeqNum = m_u32SeqNum; + } + + void CalPktLoss(uint32_t &u32StartSeqNum, uint32_t &u32LastSeqNum, uint32_t &u32LossCount, uint32_t &u32StartTime) const { + // static uint32_t u32StartSeqNum = m_u32SeqNum; + // static uint32_t u32LastSeqNum = m_u32SeqNum; + // u32StartTime = GetMicroTickCount(); + bool print = false; + if (m_u32SeqNum - u32LastSeqNum > 1) { + u32LossCount += (m_u32SeqNum - u32LastSeqNum - 1); + print = true; + // if (m_u32SeqNum - u32LastSeqNum - 1 > 1000) + // printf("%d, %u, %u\n", m_u32SeqNum - u32LastSeqNum - 1, u32LastSeqNum, + // m_u32SeqNum); + } + + // print log every 10s + if (GetMicroTickCount() - u32StartTime >= 1 * 1000 * 1000) { + printf("pkt loss freq: %u/%u\n", u32LossCount, + m_u32SeqNum - u32StartSeqNum); + u32LossCount = 0; + u32StartTime = GetMicroTickCount(); + u32StartSeqNum = m_u32SeqNum; + } + + u32LastSeqNum = m_u32SeqNum; + } + + void CalPktLoss() const { + static uint32_t u32StartSeqNum = GetSeqNum(); + static uint32_t u32LastSeqNum = GetSeqNum(); + static uint32_t u32LossCount = 0; + static uint32_t u32StartTime = GetMicroTickCount(); + + if (GetSeqNum() - u32LastSeqNum - 1 > 0) { + u32LossCount += (GetSeqNum() - u32LastSeqNum - 1); + } + + // print log every 10s + if (GetMicroTickCount() - u32StartTime >= 10 * 1000 * 1000) { + printf("pkt loss freq: %u/%u\n", u32LossCount, + GetSeqNum() - u32StartSeqNum); + u32LossCount = 0; + u32StartTime = GetMicroTickCount(); + u32StartSeqNum = GetSeqNum(); + } + + u32LastSeqNum = GetSeqNum(); + } + + void Print() const { + printf("HsLidarXTV1Tail:\n"); + printf( + "sts0:%d, data0:%d, sts1:%d, data1:%d, sts2:%d, data2:%d, shutDown:%d, motorSpeed:%u, " + "timestamp:%u, return_mode:0x%02x, factoryInfo:0x%02x, utc:%u %u " + "%u %u %u %u, seqNum: %u\n", + GetStsID0(), GetData0(), GetStsID1(), GetData1(), GetStsID2(), GetData2(), HasShutdown(), + GetMotorSpeed(), GetTimestamp(), GetReturnMode(), GetFactoryInfo(), + GetUTCData(0), GetUTCData(1), GetUTCData(2), GetUTCData(3), + GetUTCData(4), GetUTCData(5), GetSeqNum()); + } + +} PACKED; + +struct HsLidarXTV1Header { + static const uint8_t kSequenceNum = 0x01; + static const uint8_t kDistUnit = 0x04; + static const uint8_t kFirstBlockLastReturn = 0x01; + static const uint8_t kFirstBlockStrongestReturn = 0x02; + + uint8_t m_u8LaserNum; + uint8_t m_u8BlockNum; + uint8_t m_u8EchoCount; + uint8_t m_u8DistUnit; + uint8_t m_u8EchoNum; + uint8_t m_u8Status; + + uint8_t GetLaserNum() const { return m_u8LaserNum; } + uint8_t GetBlockNum() const { return m_u8BlockNum; } + double GetDistUnit() const { return m_u8DistUnit / 1000.f; } + uint8_t GetEchoCount() const { return m_u8EchoCount; } + bool IsFirstBlockLastReturn() const { + return m_u8EchoCount == kFirstBlockLastReturn; + } + bool IsFirstBlockStrongestReturn() const { + return m_u8EchoCount == kFirstBlockStrongestReturn; + } + uint8_t GetEchoNum() const { return m_u8EchoNum; } + bool HasSeqNum() const { return m_u8Status & kSequenceNum; } + + uint16_t GetPacketSize() const { + return sizeof(HS_LIDAR_PRE_HEADER) + sizeof(HsLidarXTV1Header) + + sizeof(HsLidarXTV1Tail) + + (sizeof(HsLidarXTV1BodyAzimuth) + + sizeof(HsLidarXTV1BodyChannelData) * GetLaserNum()) * + GetBlockNum(); + } + + void Print() const { + printf("HsLidarXTV1Header:\n"); + printf( + "laserNum:%02u, block_num:%02u, DistUnit:%g, EchoCnt:%02u, " + "EchoNum:%02u, HasSeqNum:%d\n", + GetLaserNum(), GetBlockNum(), GetDistUnit(), GetEchoCount(), + GetEchoNum(), HasSeqNum()); + } +} PACKED; +#ifdef _MSC_VER +#pragma pack(pop) +#endif +} // namespace lidar +} // namespace hesai +#endif diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpProtocol/udp_protocol_v7_2.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpProtocol/udp_protocol_v7_2.h new file mode 100644 index 0000000..d12f538 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpProtocol/udp_protocol_v7_2.h @@ -0,0 +1,278 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ +#ifndef HS_LIDAR_FT_V2_H +#define HS_LIDAR_FT_V2_H + +#include +namespace hesai +{ +namespace lidar +{ + +#ifdef _MSC_VER +#define PACKED +#pragma pack(push, 1) +#else +#define PACKED __attribute__((packed)) +#endif + +struct HS_LIDAR_BODY_CHN_UNIT_FT_V2 { + uint16_t distance; + uint8_t reflectivity; + uint8_t background; + uint8_t confidence; + + uint16_t GetDistance() const { return little_to_native(distance); } + uint8_t GetReflectivity() const { return reflectivity; } + uint8_t GetConfidenceLevel() const { return confidence; } + + void Print() const { + printf("HS_LIDAR_BODY_CHN_UNIT_FT_V2:\n"); + printf("Dist:%u, Reflectivity: %u, confidenceLevel:%d\n", GetDistance(), + GetReflectivity(), GetConfidenceLevel()); + } +} PACKED; + +struct HS_LIDAR_TAIL_FT_V2 { + // shutdown flag, bit 0 + static const uint8_t kShutdown = 0x01; + + // return mode + static const uint8_t kStrongestReturn = 0x37; + static const uint8_t kLastReturn = 0x38; + static const uint8_t kDualReturn = 0x39; + + ReservedInfo1 m_reservedInfo1; + ReservedInfo2 m_reservedInfo2; + uint8_t status_mode; + uint16_t column_id; + uint8_t frame_flag; + uint8_t high_temperature_shutdown_flag; + uint8_t return_mode; + uint16_t frame_duration; + uint8_t utc[6]; + uint32_t timestamp; + uint8_t factory; + uint32_t sequence_num; + + uint8_t GetStsID0() const { return m_reservedInfo1.GetID(); } + uint16_t GetData0() const { return m_reservedInfo1.GetData(); } + + uint8_t GetStsID1() const { return m_reservedInfo2.GetID(); } + uint16_t GetData1() const { return m_reservedInfo2.GetData(); } + + + uint16_t GetMotorSpeed() const { return little_to_native(frame_duration); } + + uint32_t GetTimestamp() const { return little_to_native(timestamp); } + + uint8_t GetReturnMode() const { return return_mode; } + + uint8_t GetUTCData(uint8_t index) const { + return utc[index < sizeof(utc) ? index : 0]; + } + + uint32_t GetSeqNum() const { return little_to_native(sequence_num); } + + void CalPktLoss(uint32_t &u32StartSeqNum, uint32_t &u32LastSeqNum, uint32_t &u32LossCount, + uint32_t &u32StartTime, uint32_t &u32TotalLossCount, uint32_t &u32TotalStartSeqNum) const { + bool print = false; + if (u32StartSeqNum == 0) { + u32LossCount = 0; + u32TotalLossCount = 0; + u32StartTime = GetMicroTickCount(); + u32StartSeqNum = sequence_num; + u32LastSeqNum = sequence_num; + u32TotalStartSeqNum = sequence_num; + return; + } + if (sequence_num - u32LastSeqNum > 1) { + u32LossCount += (sequence_num - u32LastSeqNum - 1); + u32TotalLossCount += (sequence_num - u32LastSeqNum - 1); + print = true; + // if (sequence_num - u32LastSeqNum - 1 > 1000) + // printf("%d, %u, %u\n", sequence_num - u32LastSeqNum - 1, u32LastSeqNum, + // sequence_num); + } + + // print log every 1s + if (GetMicroTickCount() - u32StartTime >= 1 * 1000 * 1000) { + printf("pkt loss freq: %u/%u\n", u32LossCount, + sequence_num - u32StartSeqNum); + u32LossCount = 0; + u32StartTime = GetMicroTickCount(); + u32StartSeqNum = sequence_num; + } + + u32LastSeqNum = sequence_num; + } + + void CalPktLoss(uint32_t &u32StartSeqNum, uint32_t &u32LastSeqNum, uint32_t &u32LossCount, uint32_t &u32StartTime) const { + bool print = false; + if (sequence_num - u32LastSeqNum > 1) { + u32LossCount += (sequence_num - u32LastSeqNum - 1); + print = true; + // if (sequence_num - u32LastSeqNum - 1 > 1000) + // printf("%d, %u, %u\n", sequence_num - u32LastSeqNum - 1, u32LastSeqNum, + // sequence_num); + } + + // print log every 1s + if (GetMicroTickCount() - u32StartTime >= 1 * 1000 * 1000) { + printf("pkt loss freq: %u/%u\n", u32LossCount, + sequence_num - u32StartSeqNum); + u32LossCount = 0; + u32StartTime = GetMicroTickCount(); + u32StartSeqNum = sequence_num; + } + + u32LastSeqNum = sequence_num; + } + static uint32_t GetSeqNumSize() { return sizeof(sequence_num); } + + + void CalPktLoss() const { + static uint32_t u32StartSeqNum = GetSeqNum(); + static uint32_t u32LastSeqNum = GetSeqNum(); + static uint32_t u32LossCount = 0; + static uint32_t u32StartTime = GetMicroTickCount(); + + if (GetSeqNum() - u32LastSeqNum - 1 > 0) { + u32LossCount += (GetSeqNum() - u32LastSeqNum - 1); + } + + // print log every 10s + if (GetMicroTickCount() - u32StartTime >= 10 * 1000 * 1000) { + printf("pkt loss freq: %u/%u\n", u32LossCount, + GetSeqNum() - u32StartSeqNum); + u32LossCount = 0; + u32StartTime = GetMicroTickCount(); + u32StartSeqNum = GetSeqNum(); + } + + u32LastSeqNum = GetSeqNum(); + } + int64_t GetMicroLidarTimeU64() const { + if (utc[0] != 0) { + struct tm t = {0}; + t.tm_year = utc[0] + 100; + if (t.tm_year >= 200) { + t.tm_year -= 100; + } + t.tm_mon = utc[1] - 1; + t.tm_mday = utc[2]; + t.tm_hour = utc[3]; + t.tm_min = utc[4]; + t.tm_sec = utc[5]; + t.tm_isdst = 0; +#ifdef _MSC_VER + TIME_ZONE_INFORMATION tzi; + GetTimeZoneInformation(&tzi); + long int timezone = tzi.Bias * 60; +#endif + return (mktime(&t) - timezone) * 1000000 + GetTimestamp(); + } + else { + uint32_t utc_time_big = *(uint32_t*)(&utc[0] + 2); + int64_t unix_second = ((utc_time_big >> 24) & 0xff) | + ((utc_time_big >> 8) & 0xff00) | + ((utc_time_big << 8) & 0xff0000) | + ((utc_time_big << 24)); + return unix_second * 1000000 + GetTimestamp(); + } + + } + + void Print() const { + printf("HS_LIDAR_TAIL_FT_V2:\n"); + printf( + "sts0:%d, data0:%d, sts1:%d, data1:%d, motorSpeed:%u, " + "timestamp:%u, return_mode:0x%02x, utc:%u %u " + "%u %u %u %u, seqNum: %u\n", + GetStsID0(), GetData0(), GetStsID1(), GetData1(), + GetMotorSpeed(), GetTimestamp(), GetReturnMode(), + GetUTCData(0), GetUTCData(1), GetUTCData(2), GetUTCData(3), + GetUTCData(4), GetUTCData(5), GetSeqNum()); + } + +} PACKED; + +struct HS_LIDAR_HEADER_FT_V2 { + static const uint8_t kSequenceNum = 0x01; + static const uint8_t kIMU = 0x02; + static const uint8_t kDistUnit = 0x04; + static const uint8_t kFirstBlockLastReturn = 0x01; + static const uint8_t kFirstBlockStrongestReturn = 0x02; + uint16_t total_column; + uint16_t total_row; + uint8_t column_resolution; + uint8_t row_resolution; + uint8_t echo; + uint8_t dist_unit; + uint8_t block_index; + uint16_t channel_num; + uint8_t reserved[8]; + + uint8_t GetChannelNum() const { return channel_num; } + double GetDistUnit() const { return dist_unit / 1000.f; } + uint8_t GetEchoCount() const { return echo; } + bool IsFirstBlockLastReturn() const { + return echo == kFirstBlockLastReturn; + } + bool IsFirstBlockStrongestReturn() const { + return echo == kFirstBlockStrongestReturn; + } + + // uint16_t GetPacketSize() const { + // uint16_t u16TailDelta = + // (HasSeqNum() ? 0 : HS_LIDAR_TAIL_FT_V2::GetSeqNumSize()) + + // (HasIMU() ? 0 : HS_LIDAR_TAIL_FT_V2::GetIMUSize()); + + // return sizeof(HS_LIDAR_PRE_HEADER) + sizeof(HS_LIDAR_HEADER_FT_V2) + + // sizeof(HS_LIDAR_TAIL_FT_V2) + + // (sizeof(HS_LIDAR_BODY_AZIMUTH_FT_V2) + + // sizeof(HS_LIDAR_BODY_CHN_UNIT_FT_V2) * GetColumnNum()) * + // GetBlockNum() - + // u16TailDelta; + // } + + // void Print() const { + // printf("HS_LIDAR_HEADER_FT_V2:\n"); + // printf( + // "laserNum:%02u, block_num:%02u, DistUnit:%g, EchoCnt:%02u, " + // "EchoNum:%02u, HasSeqNum:%d, HasIMU:%d\n", + // GetColumnNum(), GetRowNum(), GetDistUnit(), GetEchoCount(), + // GetEchoNum(), HasSeqNum(), HasIMU()); + // } +} PACKED; +#ifdef _MSC_VER +#pragma pack(pop) +#endif +} // namespace lidar +} // namespace hesai +#endif diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/driver_param.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/driver_param.h new file mode 100644 index 0000000..43c126e --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/driver_param.h @@ -0,0 +1,133 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ + +#pragma once +#include +#include "logger.h" +namespace hesai +{ +namespace lidar +{ +enum SourceType +{ + DATA_FROM_LIDAR = 1, + DATA_FROM_PCAP = 2, + DATA_FROM_ROS_PACKET = 3, +}; + +enum PtcMode +{ + tcp = 0, + tcp_ssl +}; + +///< The Point transform parameter +typedef struct TransformParam +{ + ///< unit, m + float x = 0.0f; + ///< unit, m + float y = 0.0f; + ///< unit, m + float z = 0.0f; + ///< unit, radian + float roll = 0.0f; + ///< unit, radian + float pitch = 0.0f; + ///< unit, radian + float yaw = 0.0f; +} TransformParam; + +///< LiDAR decoder parameter +typedef struct DecoderParam +{ + // float max_distance = 200.0f; ///< Max distance of point cloud range + // float min_distance = 0.2f; ///< Minimum distance of point cloud range + // float start_angle = 0.0f; ///< Start angle of point cloud + // float end_angle = 360.0f; ///< End angle of point cloud + + ///< Used to transform points + TransformParam transform_param; + int thread_num = 1; + bool enable_udp_thread = true; + bool enable_parser_thread = true; + bool pcap_play_synchronization = true; + //start a new frame when lidar azimuth greater than frame_start_azimuth + //range:1-359, set frame_start_azimuth less than 0 if you do want to use it + float frame_start_azimuth = 359; + // enable the udp packet loss detection tool + // it forbiddens parser udp packet while trun on this tool + bool enable_packet_loss_tool = false; +} DecoderParam; + +///< The LiDAR input parameter +typedef struct InputParam +{ + // PTC mode + PtcMode ptc_mode = PtcMode::tcp; + SourceType source_type = DATA_FROM_PCAP; + ///< Ip of LiDAR + std::string device_ip_address = "Your lidar ip"; + ///< Address of multicast + std::string multicast_ip_address = ""; + ///< Address of host + std::string host_ip_address = "Your host ip"; + ///< udp packet port number + uint16_t udp_port = 2368; + ///< ptc packet port number + uint16_t ptc_port = 9347; + bool read_pcap = true; ///< true: The driver will process the pcap through pcap_path. false: The driver will + ///< Get data from online LiDAR + std::string pcap_path = "Your pcap file path"; ///< Absolute path of pcap file + std::string correction_file_path = "Your correction file path"; ///< Path of angle calibration files(angle.csv).Only used for internal debugging. + std::string firetimes_path = "Your firetime file path"; ///< Path of firetime files(angle.csv). + /// certFile Represents the path of the user's certificate + char* certFile = nullptr; + /// privateKeyFile Represents the path of the user's private key + char* privateKeyFile = nullptr; + /// caFile Represents the path of the root certificate + char* caFile = nullptr; +} InputParam; + +///< The LiDAR driver parameter +typedef struct DriverParam +{ + ///< Input parameter + InputParam input_param; + ///< Decoder parameter + DecoderParam decoder_param; + ///< The frame id of LiDAR message + std::string frame_id = "hesai"; + ///< Lidar type + std::string lidar_type = "AT128"; + int log_level = LOG_DEBUG | LOG_INFO; // + int log_Target = LOG_TARGET_CONSOLE | LOG_TARGET_FILE; + std::string log_path = "./log.log"; +} DriverParam; +} // namespace lidar +} // namespace hesai \ No newline at end of file diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/include/auto_tick_count.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/include/auto_tick_count.h new file mode 100644 index 0000000..784e5cd --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/include/auto_tick_count.h @@ -0,0 +1,94 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ + +#ifndef AUTOTIMER_H_ +#define AUTOTIMER_H_ + +#include +#include +#include +#include +#include +namespace hesai +{ +namespace lidar +{ +class AutoTickCount; + +class TickCount { + public: + TickCount(); + ~TickCount(); + + int Start(); + int Pause(); + std::string GetTimeCost(uint64_t &u64TimeCost); + std::string GetTimeCost(); + int ShowTimeSlice(std::string sLogFile = "", bool bSaveSysTime = false); + std::map> GetTimeSlice() { + return m_timeSlice; + } + uint64_t GetTimeSlice(std::string sKey, int nTime = -1); + // 多次使用相同关键字调用，只有第一次的生效 + int Begin(std::string sKey); + int End(std::string sKey, bool bShow = true); + int SetName(std::string sName) { + m_sName = sName; + + return 0; + } + + private: + static const uint32_t kMicroToSec = 1000000; + static const uint8_t kClockUnit = 60; + uint64_t m_u64StartTime; + uint64_t m_u64EndTime; + std::map> m_timeSlice; + std::map m_startTime; + std::string m_sName; + + friend class AutoTickCount; + + int AppentTimeSlice(std::string sKey, uint64_t u64Time); +}; + +class AutoTickCount { + public: + AutoTickCount(TickCount &t, std::string sKey = "", bool bShow = true); + ~AutoTickCount(); + + private: + TickCount *m_pT; + uint64_t m_u64StartTime; + uint64_t m_u64EndTime; + bool m_bShow; + std::string m_sKey; +}; +} // namespace lidar +} // namespace hesai +#endif // AUTOTIMER_H_ diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/include/plat_utils.h b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/include/plat_utils.h new file mode 100644 index 0000000..f7e993a --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/include/plat_utils.h @@ -0,0 +1,43 @@ +#ifndef _PLAT_UTILS_H_ +#define _PLAT_UTILS_H_ + +#ifdef _MSC_VER +#else +#include +#include +#endif +#include +#include +#include +#include +#include +#include +#include +#include + +#define SHED_FIFO_PRIORITY_HIGH 99 +#define SHED_FIFO_PRIORITY_MEDIUM 70 +#define SHED_FIFO_PRIORITY_LOW 1 +#define ISO_8601_FORMAT 1 + +#ifdef _MSC_VER +extern void SetThreadPriorityWin(int priority); +#else +extern void SetThreadPriority(int policy, int priority); +#endif +#ifndef _MSC_VER +extern unsigned int GetTickCount(); +#endif + +extern unsigned int GetMicroTickCount(); + +extern uint64_t GetMicroTickCountU64(); + +extern int GetAvailableCPUNum(); + +// extern int GetAnglesFromFile( +// const std::string& sFile, +// std::map>& mapAngleMetaData); + +extern int GetCurrentTimeStamp(std::string &sTime, int nFormat = ISO_8601_FORMAT); +#endif //_PLAT_UTILS_H_ diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/src/auto_tick_count.cc b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/src/auto_tick_count.cc new file mode 100644 index 0000000..4e1f3cb --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/src/auto_tick_count.cc @@ -0,0 +1,228 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ + +#include "auto_tick_count.h" + + + +#include "plat_utils.h" +using namespace hesai::lidar; +TickCount::TickCount() { + m_u64StartTime = m_u64EndTime = GetMicroTickCountU64(); +} + +TickCount::~TickCount() { + m_u64EndTime = GetMicroTickCountU64(); + std::cout << "cost" << GetTimeCost().c_str() << std::endl; + m_timeSlice.clear(); +} + +int TickCount::Start() { + m_u64StartTime = m_u64EndTime = GetMicroTickCountU64(); + return 0; +} + +int TickCount::Pause() { + m_u64EndTime = GetMicroTickCountU64(); + return 0; +} + +std::string TickCount::GetTimeCost(uint64_t &u64TimeCost) { + u64TimeCost = m_u64EndTime - m_u64StartTime; + uint32_t u32Sec = static_cast(u64TimeCost / kMicroToSec); + uint32_t u32Min = u32Sec / kClockUnit; + uint32_t u32Hour = u32Min / kClockUnit; + + u32Sec = u32Sec % kClockUnit; + u32Min = u32Min % kClockUnit; + + return std::to_string(u32Hour) + std::string("h ") + std::to_string(u32Min) + + std::string("m ") + std::to_string(u32Sec) + std::string("s ") + + std::to_string(u64TimeCost % kMicroToSec) + std::string("us"); +} + +std::string TickCount::GetTimeCost() { + uint64_t u64Time; + return GetTimeCost(u64Time); +} + +int TickCount::AppentTimeSlice(std::string sKey, uint64_t u64Time) { + if (m_timeSlice.find(sKey) == m_timeSlice.end()) { + std::vector vTimes; + m_timeSlice[sKey] = vTimes; + } + + m_timeSlice[sKey].push_back(u64Time); + + return 0; +} + +int TickCount::ShowTimeSlice(std::string sLogFile, bool bSaveSysTime) { + std::map>::iterator iter = + m_timeSlice.begin(); + FILE *pFileSel = fopen(sLogFile.c_str(), "a"); + FILE *pFile = fopen("timeConsumption.txt", "a"); + + // 获取系统时间写入文件 + time_t time_t_now; + struct tm *tm_now; + time(&time_t_now); + tm_now = localtime(&time_t_now); + + if (bSaveSysTime) { + if (pFile != NULL) + fprintf(pFile, "recording time: %s-------------\n", asctime(tm_now)); + if (pFileSel != NULL) { + fprintf(pFileSel, "recording time: %s-------------\n", asctime(tm_now)); + } + } + + if (pFile != NULL) fprintf(pFile, "object %s:\n", m_sName.c_str()); + + if (pFileSel != NULL) fprintf(pFileSel, "object %s:\n", m_sName.c_str()); + + while (iter != m_timeSlice.end()) { + std::cout << iter->first.c_str() << std::endl; + + if (pFile != NULL) fprintf(pFile, "%s:\n", iter->first.c_str()); + + if (pFileSel != NULL) fprintf(pFileSel, "%s:\n", iter->first.c_str()); + + for (int i = 0; i < iter->second.size(); i++) { + std::cout << " " << static_cast(iter->second.at(i)) / kMicroToSec + << "s\n"; + + if (pFile != NULL) + fprintf(pFile, " %lf\n", + static_cast(iter->second.at(i)) / kMicroToSec); + + if (pFileSel != NULL) + fprintf(pFileSel, " %lf\n", + static_cast(iter->second.at(i)) / kMicroToSec); + } + + iter++; + } + m_u64EndTime = GetMicroTickCountU64(); + + std::cout << "total cost:" << GetTimeCost().c_str() << std::endl; + + if (pFile != NULL) fprintf(pFile, "total cost:%s\n", GetTimeCost().c_str()); + + if (pFileSel != NULL) + fprintf(pFileSel, "total cost:%s\n", GetTimeCost().c_str()); + + fclose(pFile); + pFile = NULL; + fclose(pFileSel); + pFileSel = NULL; + + return 0; +} + +uint64_t TickCount::GetTimeSlice(std::string sKey, int nTime) { + if (m_timeSlice.find(sKey) == m_timeSlice.end()) { + return 0; + } + + if (m_timeSlice[sKey].empty()) { + return 0; + } + + int nCount = m_timeSlice[sKey].size(); + + if (nTime < 0) { + nTime = nCount + nTime; + } + + if (nTime <= 0) { + return m_timeSlice[sKey].at(0); + } + + if (nTime >= nCount) { + return m_timeSlice[sKey].at(nCount - 1); + } + + return m_timeSlice[sKey].at(nTime); +} + +int TickCount::Begin(std::string sKey) { + if (m_startTime.find(sKey) == m_startTime.end()) { + m_startTime[sKey] = GetMicroTickCountU64(); + return 0; + } + return -1; +} + +int TickCount::End(std::string sKey, bool bShow) { + if (m_startTime.find(sKey) == m_startTime.end()) { + return -1; + } + + uint64_t u64Time = GetMicroTickCountU64(); + + uint64_t u64TimeCost = u64Time - m_startTime[sKey]; + for (std::map::iterator it = m_startTime.begin(); + it != m_startTime.end(); it++) { + if (it->first == sKey) { + m_startTime.erase(it); + break; + } + } + + AppentTimeSlice(sKey, u64TimeCost); + + if (bShow) { + std::cout << sKey.c_str() << "cost" + << static_cast(u64TimeCost) / kMicroToSec << "s\n"; + } + + return 0; +} + +AutoTickCount::AutoTickCount(TickCount &t, std::string sKey, bool bShow) { + m_pT = &t; + m_u64StartTime = m_u64EndTime = GetMicroTickCountU64(); + m_sKey = sKey; + m_bShow = bShow; +} + +AutoTickCount::~AutoTickCount() { + m_u64EndTime = GetMicroTickCountU64(); + + if (m_sKey.length() > 0) { + m_pT->AppentTimeSlice(m_sKey, m_u64EndTime - m_u64StartTime); + } + + if (m_bShow) { + std::cout << m_sKey.c_str() << "cost" + << static_cast(m_u64EndTime - m_u64StartTime) / + TickCount::kMicroToSec + << "s\n"; + } +} diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/src/plat_utils.cc b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/src/plat_utils.cc new file mode 100644 index 0000000..b3f962e --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/src/plat_utils.cc @@ -0,0 +1,190 @@ +/************************************************************************************************ +Copyright (C) 2023 Hesai Technology Co., Ltd. +Copyright (C) 2023 Original Authors +All rights reserved. + +All code in this repository is released under the terms of the following Modified BSD License. +Redistribution and use in source and binary forms, with or without modification, are permitted +provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and + the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or + promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +************************************************************************************************/ +#include +static const int kTimeStrLen = 1000; +#ifdef _MSC_VER +#define EPOCHFILETIME (116444736000000000UL) +#include +#else +#include +#include +#include +#define gettid() syscall(SYS_gettid) +#endif + +#ifdef _MSC_VER +void SetThreadPriorityWin(int priority) { + auto handle = GetCurrentThread(); + printf("set thread %lu, priority %d\n",std::this_thread::get_id(), + priority); + SetThreadPriority(handle, priority); + int prior = GetThreadPriority(handle); + printf("get thead %lu, priority %d\n", std::this_thread::get_id(), + prior); +} +#else +void SetThreadPriority(int policy, int priority) { + printf("set thread %lu, tid %ld, policy %d and priority %d\n", pthread_self(), + gettid(), policy, priority); + sched_param param; + param.sched_priority = priority; + pthread_setschedparam(pthread_self(), policy, ¶m); + + int ret_policy; + pthread_getschedparam(pthread_self(), &ret_policy, ¶m); + printf("get thead %lu, tid %ld, policy %d and priority %d\n", pthread_self(), + gettid(), ret_policy, param.sched_priority); +} +#endif + +#ifndef _MSC_VER +unsigned int GetTickCount() { + unsigned int ret = 0; +#ifdef _MSC_VER + FILETIME time; + LARGE_INTEGER larger_int; + GetSystemTimeAsFileTime(&time); + larger_int.LowPart = time.dwLowDateTime; + larger_int.HighPart = time.dwHighDateTime; + ret = (larger_int.QuadPart - EPOCHFILETIME) / 10000; +#else + timespec time; + memset(&time, 0, sizeof(time)); + if (clock_gettime(CLOCK_MONOTONIC, &time) == 0) { + ret = time.tv_nsec / 1000000 + time.tv_sec * 1000; + } +#endif + return ret; +} +#endif + +unsigned int GetMicroTickCount() { + unsigned int ret = 0; +#ifdef _MSC_VER + FILETIME time; + LARGE_INTEGER larger_int; + GetSystemTimeAsFileTime(&time); + larger_int.LowPart = time.dwLowDateTime; + larger_int.HighPart = time.dwHighDateTime; + ret = (larger_int.QuadPart - EPOCHFILETIME) / 10; +#else + timespec time; + memset(&time, 0, sizeof(time)); + if (clock_gettime(CLOCK_MONOTONIC, &time) == 0) { + ret = time.tv_nsec / 1000 + time.tv_sec * 1000000; + } +#endif + return ret; +} + +uint64_t GetMicroTickCountU64() { + uint64_t ret = 0; +#ifdef _MSC_VER + FILETIME time; + LARGE_INTEGER larger_int; + GetSystemTimeAsFileTime(&time); + larger_int.LowPart = time.dwLowDateTime; + larger_int.HighPart = time.dwHighDateTime; + ret = (larger_int.QuadPart - EPOCHFILETIME) / 10; +#else + timespec time; + memset(&time, 0, sizeof(time)); + if (clock_gettime(CLOCK_MONOTONIC, &time) == 0) { + ret = time.tv_nsec / 1000 + time.tv_sec * 1000000; + } +#endif + return ret; +} + +int GetAvailableCPUNum() { +#ifdef _MSC_VER + SYSTEM_INFO sysInfo; + GetSystemInfo(&sysInfo); + int numProcessors = sysInfo.dwNumberOfProcessors; + return numProcessors; + return 1; +#else + return sysconf(_SC_NPROCESSORS_ONLN); +#endif +} + +int GetAnglesFromFile( + const std::string& sFile, + std::map>& mapAngleMetaData) { + FILE* pFile = fopen(sFile.c_str(), "r"); + + if (NULL == pFile) { + printf("cannot open the angle file, please check: %s\n", sFile.c_str()); + return 1; + } + + char sContent[255] = {0}; + fgets(sContent, 255, pFile); // skip first line + + while (!feof(pFile)) { + memset(sContent, 0, 255); + fgets(sContent, 255, pFile); + + if (strlen(sContent) < strlen(" , , ")) { + break; + } + + int iChannelId; + float fPitch; + float fAzimuth; + + sscanf(sContent, "%d,%f,%f", &iChannelId, &fPitch, &fAzimuth); + + std::pair pairAngleData = std::make_pair(fPitch, fAzimuth); + mapAngleMetaData.insert(std::map>::value_type( + iChannelId, pairAngleData)); + } + + fclose(pFile); + pFile = NULL; + return 0; +} + +// 2004-05-03T17:30:08+08:00 +int GetCurrentTimeStamp(std::string &sTime, int nFormat) { + time_t currentTime = time(NULL); + struct tm *pLocalTime = localtime(¤tTime); + char sFormattedTime[kTimeStrLen]; + + if (ISO_8601_FORMAT == nFormat) { + strftime(sFormattedTime, kTimeStrLen, "%FT%T%z", pLocalTime); + + sTime = std::string(sFormattedTime); + // either ISO 8601 or C language is stupid, so change 0800 to 08:00 + sTime = sTime.insert(sTime.length() - 2, ":"); + + return 0; + } else { + return -1; + } +} diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/test/test.cc b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/test/test.cc new file mode 100644 index 0000000..0c68ee8 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/test/test.cc @@ -0,0 +1,58 @@ +#include "hesai_lidar_sdk.hpp" + +uint32_t last_frame_time = 0; +uint32_t cur_frame_time = 0; + +//log info, display frame message +void lidarCallback(const LidarDecodedFrame &frame) { + cur_frame_time = GetMicroTickCount(); + if (last_frame_time == 0) last_frame_time = GetMicroTickCount(); + if (cur_frame_time - last_frame_time > kMaxTimeInterval) { + printf("Time between last frame and cur frame is: %d us\n", (cur_frame_time - last_frame_time)); + } + last_frame_time = cur_frame_time; + printf("frame:%d points:%u packet:%d start time:%lf end time:%lf\n",frame.frame_index, frame.points_num, frame.packet_num, frame.points[0].timestamp, frame.points[frame.points_num - 1].timestamp) ; +} + +int main(int argc, char *argv[]) +{ +#ifndef _MSC_VER + system("sudo sh -c \"echo 562144000 > /proc/sys/net/core/rmem_max\""); +#endif + HesaiLidarSdk sample; + DriverParam param; + + // assign param + // param.input_param.source_type = DATA_FROM_PCAP; + // param.decoder_param.enable_packet_loss_tool = true; + param.input_param.source_type = DATA_FROM_LIDAR; + param.input_param.pcap_path = "Your pcap file path"; + param.input_param.correction_file_path = "Your correction file path"; + param.input_param.firetimes_path = "Your firetime file path"; + + + param.input_param.device_ip_address = "192.168.1.201"; + param.input_param.ptc_port = 9347; + param.input_param.udp_port = 2368; + param.input_param.host_ip_address = "192.168.1.100"; + param.input_param.multicast_ip_address = ""; + + //init lidar with param + sample.Init(param); + float socket_buffer = 262144000; + sample.lidar_ptr_->source_->SetSocketBufferSize(socket_buffer); + + //assign callback fuction + sample.RegRecvCallback(lidarCallback); + + + + sample.Start(); + + uint64_t start = GetMicroTickCountU64(); + while (1) + { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + +} \ No newline at end of file diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/test/test.cu b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/test/test.cu new file mode 100644 index 0000000..1698d8b --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/test/test.cu @@ -0,0 +1,51 @@ +#include "hesai_lidar_sdk_gpu.cuh" +uint32_t last_frame_time; +uint32_t cur_frame_time;std::mutex mex_viewer; + + +//log info, display frame message +void lidarCallback(const LidarDecodedFrame &frame) { + cur_frame_time = GetMicroTickCount(); + if (cur_frame_time - last_frame_time > kMaxTimeInterval) { + printf("Time between last frame and cur frame is: %d us\n", (cur_frame_time - last_frame_time)); + } + last_frame_time = cur_frame_time; + printf("frame:%d points:%u packet:%d start time:%lf end time:%lf\n",frame.frame_index, frame.points_num, frame.packet_num, frame.points[0].timestamp, frame.points[frame.points_num - 1].timestamp) ; +} + + +int main(int argc, char *argv[]) +{ + HesaiLidarSdkGpu sample; + DriverParam param; + // assign param + param.decoder_param.enable_parser_thread = false; + param.input_param.source_type = DATA_FROM_LIDAR; + param.input_param.pcap_path = "Your pcap file path"; + param.input_param.correction_file_path = "Your correction file path"; + param.input_param.firetimes_path = "Your firetime file path"; + + param.input_param.device_ip_address = "192.168.1.201"; + param.input_param.ptc_port = 9347; + param.input_param.udp_port = 2368; + param.input_param.host_ip_address = "192.168.1.100"; + param.input_param.multicast_ip_address = ""; + + + //init lidar with param + sample.Init(param); + float socket_buffer = 262144000; + sample.lidar_ptr_->source_->SetSocketBufferSize(socket_buffer); + + //assign callback fuction + sample.RegRecvCallback(lidarCallback); + + //star process thread + sample.Start(); + while (1) + { + + std::this_thread::sleep_for(std::chrono::milliseconds(40)); + + } +} \ No newline at end of file diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/tool/CMakeLists.txt b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/tool/CMakeLists.txt new file mode 100644 index 0000000..9519468 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/tool/CMakeLists.txt @@ -0,0 +1,125 @@ +cmake_minimum_required(VERSION 3.8.11) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_STANDARD 14) +PROJECT(SDKTool) +set(VERSION_MAJOR 2) +set(VERSION_MINOR 0) +set(VERSION_TINY 5) +configure_file( + "${CMAKE_CURRENT_SOURCE_DIR}/Version.h.in" + "${CMAKE_CURRENT_BINARY_DIR}/Version.h" +) + +cmake_policy(SET CMP0053 NEW) +set(CMAKE_BUILD_TYPE ON) +set(CMAKE_BUILD_TYPE Release) +add_definitions(-O3) +add_compile_options(-Wall) + +# 设置生成平台为 x64 +if(_MSC_VER) +set(CMAKE_GENERATOR_PLATFORM x64) +set(OPENSSL_ROOT_DIR "C:/Program Files/OpenSSL-Win64") +endif() + + +include_directories("${PROJECT_BINARY_DIR}") +find_package(Boost REQUIRED COMPONENTS system filesystem thread) +find_package(OpenSSL REQUIRED) +add_definitions(-DQT_MESSAGELOGCONTEXT) +add_subdirectory(../libhesai lbhesai.out) + +# set (Boost_INCLUDE_DIRS "/usr/lib/x86_64-linux-gnu/") + +include_directories( + ../libhesai + ../libhesai/Lidar + ../libhesai/UdpParser + ../libhesai/UdpParser/include + ../libhesai/UdpParser/src + ../libhesai/UdpProtocol + ../libhesai/Source/include + ../libhesai/Container/include + ../libhesai/Container/src + ../libhesai/UdpParserGpu + ../libhesai/UdpParserGpu/include + ../libhesai/UdpParserGpu/src + ../libhesai/PtcClient/include + ../libhesai/PtcParser + ../libhesai/PtcParser/src + ../libhesai/PtcParser/include + ../libhesai/Logger/include + ../libhesai/include + ../driver + ../third_party + ${Boost_INCLUDE_DIRS} + ${OPENSSL_INCLUDE_DIR} +) +link_directories(../libhesai/PtcClient/lib) +link_directories(${Boost_LIBRARY_DIRS}) + + +add_executable(packet_loss_tool + ./packet_loss_tool.cc +) + +target_link_libraries(packet_loss_tool + ${Boost_LIBRARIES} + source_lib + container_lib + ptcClient_lib + log_lib + ptcParser_lib + ${OPENSSL_LIBRARIES} +) + +find_package(PCL COMPONENTS common visualization io QUIET REQUIRED) +include_directories(${PCL_INCLUDE_DIRS}) +link_directories(${PCL_LIBRARY_DIRS}) + +add_executable(pcl_tool + ./pcl_tool.cc +) + +target_link_libraries(pcl_tool + ${Boost_LIBRARIES} + source_lib + container_lib + ptcClient_lib + ptcParser_lib + log_lib + ${PCL_LIBRARIES} + ${OPENSSL_LIBRARIES} +) + + +find_package(CUDA ) +if(${CUDA_FOUND}) + set(CUDA_SOURCE_PROPERTY_FORMAT OBJ) + set(CUDA_SEPARABLE_COMPILATION ON) + include_directories(${CUDA_INCLUDE_DIRS}) + set(CUDA_PROPAGATE_HOST_FLAGS OFF) + set(CUDA_NVCC_FLAGS -arch=sm_61;-O3;-G;-g;-std=c++14)#根据具体GPU性能更改算力参数 + link_directories($ENV{CUDA_PATH}/lib/x64) + file(GLOB_RECURSE CURRENT_HEADERS ../*.h ../*.hpp ../*.cuh) + file(GLOB CURRENT_SOURCES ../*.cpp ../*.cu) + source_group("Include" FILES ${CURRENT_HEADERS}) + source_group("Source" FILES ${CURRENT_SOURCES}) + CUDA_ADD_EXECUTABLE(pcl_tool_gpu ${CURRENT_HEADERS} ${CURRENT_SOURCES} + ./pcl_tool.cu + ../libhesai/UdpParserGpu/src/buffer.cu + ) + target_link_libraries(pcl_tool_gpu + ${Boost_LIBRARIES} + source_lib + container_lib + ptcClient_lib + log_lib + ptcParser_lib + ${OPENSSL_LIBRARIES} + ${PCL_LIBRARIES} +) +else(${CUDA_FOUND}) + MESSAGE(STATUS "cuda not found!") +endif(${CUDA_FOUND}) + diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/tool/Version.h.in b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/tool/Version.h.in new file mode 100644 index 0000000..77e23c7 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/tool/Version.h.in @@ -0,0 +1,21 @@ +///////////////////////////////////////////////////////////////////////////////////////// +// +// Copyright [2022] [Hesai Technology Co., Ltd] +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License +// +///////////////////////////////////////////////////////////////////////////////////////// + +#define VERSION_MAJOR @VERSION_MAJOR@ +#define VERSION_MINOR @VERSION_MINOR@ +#define VERSION_TINY @VERSION_TINY@ \ No newline at end of file diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/tool/packet_loss_tool.cc b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/tool/packet_loss_tool.cc new file mode 100644 index 0000000..3dc53a1 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/tool/packet_loss_tool.cc @@ -0,0 +1,79 @@ +#include "hesai_lidar_sdk.hpp" + +uint32_t last_frame_time; +uint32_t cur_frame_time; + +//log info, display frame message +void lidarCallback(const LidarDecodedFrame &frame) { + cur_frame_time = GetMicroTickCount(); + if (cur_frame_time - last_frame_time > kMaxTimeInterval) { + printf("Time between last frame and cur frame is: %d\n us", (cur_frame_time - last_frame_time)); + } +} + +int main(int argc, char *argv[]) +{ +#ifndef _MSC_VER + system("sudo sh -c \"echo 562144000 > /proc/sys/net/core/rmem_max\""); +#endif + HesaiLidarSdk sample; + DriverParam param; + + // assign param + param.input_param.source_type = DATA_FROM_LIDAR; + param.decoder_param.enable_packet_loss_tool = true; + param.input_param.pcap_path = "Your pcap file path"; + param.input_param.correction_file_path = "Your correction file path"; + param.input_param.firetimes_path = "Your firetime file path"; + + param.input_param.device_ip_address = "192.168.1.201"; + param.input_param.ptc_port = 9347; + param.input_param.udp_port = 2368; + param.input_param.host_ip_address = "192.168.1.100"; + param.input_param.multicast_ip_address = ""; + + //init lidar with param + sample.Init(param); + float socket_buffer = 262144000; + if (argc > 2) socket_buffer = atof(argv[2]); + sample.lidar_ptr_->source_->SetSocketBufferSize(socket_buffer); + + //assign callback fuction + sample.RegRecvCallback(lidarCallback); + + //star process thread + last_frame_time = GetMicroTickCount(); + float run_time = 15; + if (argc > 1 ) run_time = atof(argv[1]); + + + sample.Start(); + + uint64_t start = GetMicroTickCountU64(); + while (1) + { + uint64_t now = GetMicroTickCountU64(); + + if (now <= start) { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + continue; + } + //packet loss tool run time + if (int(now -start) < int(run_time * kMicrosecondToSecondInt)) std::this_thread::sleep_for(std::chrono::microseconds(1000)); + else { + break; + } + } + + uint64_t end = GetMicroTickCountU64(); + uint32_t total_packet_count = sample.lidar_ptr_->udp_parser_->GetGeneralParser()->total_packet_count_; + uint32_t total_packet_loss_count = sample.lidar_ptr_->udp_parser_->GetGeneralParser()->total_loss_count_; + printf("total recevice packet time: %ums\n",(end -start) / 1000); + printf("total receviced packet count: %u\n",total_packet_count); + printf("total loss packet count: %u\n",total_packet_loss_count); + if (float(total_packet_loss_count) / float(total_packet_count) > 0.01) { + printf("Error: Packet loss rate exceeds 1%\n"); + } + sample.Stop(); + +} \ No newline at end of file diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/tool/pcl_tool.cc b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/tool/pcl_tool.cc new file mode 100644 index 0000000..e783ee5 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/tool/pcl_tool.cc @@ -0,0 +1,130 @@ +#include "hesai_lidar_sdk.hpp" +#define PCL_NO_PRECOMPILE +#include +#include +#include +#include + +// #define SAVE_PCD_FILE +// #define SAVE_PLY_FILE +// #define ENABLE_VIEWER + + +struct PointXYZIT { + //添加pcl里xyz + PCL_ADD_POINT4D + float intensity; + double timestamp; + uint16_t ring; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +} EIGEN_ALIGN16; + +POINT_CLOUD_REGISTER_POINT_STRUCT( + PointXYZIT, + (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)( + double, timestamp, timestamp)(uint16_t, ring, ring)) + + +using namespace pcl::visualization; +std::shared_ptr pcl_viewer; +std::mutex mex_viewer; +uint32_t last_frame_time; +uint32_t cur_frame_time; +//log info, display frame message +void lidarCallback(const LidarDecodedFrame &frame) { + cur_frame_time = GetMicroTickCount(); + if (cur_frame_time - last_frame_time > kMaxTimeInterval) { + printf("Time between last frame and cur frame is: %d us\n", (cur_frame_time - last_frame_time)); + } + last_frame_time = cur_frame_time; + printf("frame:%d points:%u packet:%d start time:%lf end time:%lf\n",frame.frame_index, frame.points_num, frame.packet_num, frame.points[0].timestamp, frame.points[frame.points_num - 1].timestamp) ; + pcl::PointCloud::Ptr pcl_pointcloud(new pcl::PointCloud); + mex_viewer.lock(); + if (frame.points_num == 0) return; + pcl_pointcloud->clear(); + pcl_pointcloud->resize(frame.points_num); + pcl_pointcloud->points.assign(frame.points, frame.points + frame.points_num); + pcl_pointcloud->height = 1; + pcl_pointcloud->width = frame.points_num; + pcl_pointcloud->is_dense = false; + + std::string file_name = "./PointCloudFrame" + std::to_string(frame.frame_index) + "_" + std::to_string(frame.points[0].timestamp)+ ".ply"; + +//save point cloud with pcd file if define SAVE_PCD_FILE +#ifdef SAVE_PCD_FILE + pcl::PCDWriter writer; + writer.writeASCII(file_name, *pcl_pointcloud); +#endif +#ifdef SAVE_PLY_FILE + pcl::PLYWriter writer1; + writer1.write(file_name, *pcl_pointcloud, true); +#endif + +//display point cloud with pcl if define ENABLE_VIEWER +#ifdef ENABLE_VIEWER + PointCloudColorHandlerGenericField point_color_handle(pcl_pointcloud, "intensity"); + pcl_viewer->updatePointCloud(pcl_pointcloud, point_color_handle, "pandar"); +#endif +mex_viewer.unlock(); +} + +//display point cloud with pcl if define ENABLE_VIEWER +void PclViewerInit(std::shared_ptr& pcl_viewer) { + pcl_viewer = std::make_shared("HesaiPointCloudViewer"); + pcl_viewer->setBackgroundColor(0.0, 0.0, 0.0); + pcl_viewer->addCoordinateSystem(1.0); + pcl::PointCloud::Ptr pcl_pointcloud(new pcl::PointCloud); + pcl_viewer->addPointCloud(pcl_pointcloud, "pandar"); + pcl_viewer->setPointCloudRenderingProperties(PCL_VISUALIZER_POINT_SIZE, 2, "pandar"); + return; +} + +int main(int argc, char *argv[]) +{ +#ifdef ENABLE_VIEWER + PclViewerInit(pcl_viewer); +#endif + + HesaiLidarSdk sample; + DriverParam param; + + // assign param + param.input_param.source_type = DATA_FROM_LIDAR; + param.input_param.pcap_path = "Your pcap file path"; + param.input_param.correction_file_path = "Your correction file path"; + param.input_param.firetimes_path = "Your firetime file path"; + + // param.input_param.ptc_mode = PtcMode::tcp_ssl; + param.input_param.certFile = "Your cert file"; + param.input_param.privateKeyFile = "Your privateKey file"; + param.input_param.caFile = "Your ca file"; + param.input_param.device_ip_address = "192.168.1.201"; + param.input_param.ptc_port = 9347; + param.input_param.udp_port = 2368; + param.input_param.host_ip_address = "192.168.1.100"; + param.input_param.multicast_ip_address = ""; + + //init lidar with param + sample.Init(param); + + //assign callback fuction + sample.RegRecvCallback(lidarCallback); + + //star process thread + last_frame_time = GetMicroTickCount(); + sample.Start(); + + + while (1) + { + +#ifdef ENABLE_VIEWER + mex_viewer.lock(); + if(pcl_viewer->wasStopped()) break; + pcl_viewer->spinOnce(); + mex_viewer.unlock(); +#endif + std::this_thread::sleep_for(std::chrono::milliseconds(40)); + + } +} \ No newline at end of file diff --git a/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/tool/pcl_tool.cu b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/tool/pcl_tool.cu new file mode 100644 index 0000000..f2db7ed --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/tool/pcl_tool.cu @@ -0,0 +1,127 @@ +#include "hesai_lidar_sdk_gpu.cuh" +#define PCL_NO_PRECOMPILE +#include +#include +#include + +// #define SAVE_PCD_FILE +// #define SAVE_PLY_FILE +// #define ENABLE_VIEWER + +// using namespace hesai::lidar; +struct PointXYZIT { + //添加pcl里xyz + PCL_ADD_POINT4D + float intensity; + double timestamp; + uint16_t ring; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +} EIGEN_ALIGN16; + +POINT_CLOUD_REGISTER_POINT_STRUCT( + PointXYZIT, + (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)( + double, timestamp, timestamp)(uint16_t, ring, ring)) + + +using namespace pcl::visualization; +std::shared_ptr pcl_viewer; +uint32_t last_frame_time; +uint32_t cur_frame_time;std::mutex mex_viewer; + + +//log info, display frame message +void lidarCallback(const LidarDecodedFrame &frame) { + cur_frame_time = GetMicroTickCount(); + if (cur_frame_time - last_frame_time > kMaxTimeInterval) { + printf("Time between last frame and cur frame is: %d us\n", (cur_frame_time - last_frame_time)); + } + last_frame_time = cur_frame_time; + printf("frame:%d points:%u packet:%d start time:%lf end time:%lf\n",frame.frame_index, frame.points_num, frame.packet_num, frame.points[0].timestamp, frame.points[frame.points_num - 1].timestamp) ; + pcl::PointCloud::Ptr pcl_pointcloud(new pcl::PointCloud); + mex_viewer.lock(); + if (frame.points_num == 0) return; + pcl_pointcloud->clear(); + pcl_pointcloud->resize(frame.points_num); + pcl_pointcloud->points.assign(frame.points, frame.points + frame.points_num); + pcl_pointcloud->height = 1; + pcl_pointcloud->width = frame.points_num; + pcl_pointcloud->is_dense = false; + + std::string file_name = "./PointCloudFrame" + std::to_string(frame.frame_index) + ".pcd"; + +//save point cloud with pcd file if define SAVE_PCD_FILE +#ifdef SAVE_PCD_FILE + pcl::PCDWriter writer; + writer.writeASCII(file_name, *pcl_pointcloud); +#endif +#ifdef SAVE_PLY_FILE + pcl::PLYWriter writer1; + writer1.write(file_name, *pcl_pointcloud, true); +#endif + +//display point cloud with pcl if define ENABLE_VIEWER +#ifdef ENABLE_VIEWER + PointCloudColorHandlerGenericField point_color_handle(pcl_pointcloud, "intensity"); + pcl_viewer->updatePointCloud(pcl_pointcloud, point_color_handle, "pandar"); +#endif +mex_viewer.unlock(); +} + +//display point cloud with pcl if define ENABLE_VIEWER +void PclViewerInit(std::shared_ptr& pcl_viewer) { + pcl_viewer = std::make_shared("HesaiPointCloudViewer"); + pcl_viewer->setBackgroundColor(0.0, 0.0, 0.0); + pcl_viewer->addCoordinateSystem(1.0); + pcl::PointCloud::Ptr pcl_pointcloud(new pcl::PointCloud); + pcl_viewer->addPointCloud(pcl_pointcloud, "pandar"); + pcl_viewer->setPointCloudRenderingProperties(PCL_VISUALIZER_POINT_SIZE, 2, "pandar"); + return; +} + +int main(int argc, char *argv[]) +{ + #ifdef ENABLE_VIEWER + PclViewerInit(pcl_viewer); + #endif + HesaiLidarSdkGpu sample; + DriverParam param; + // assign param + param.decoder_param.enable_parser_thread = false; + param.input_param.source_type = DATA_FROM_LIDAR; + param.input_param.pcap_path = "Your pcap file path"; + param.input_param.correction_file_path = "Your correction file path"; + param.input_param.firetimes_path = "Your firetime file path"; + + param.input_param.device_ip_address = "192.168.1.201"; + param.input_param.ptc_port = 9347; + param.input_param.udp_port = 2368; + param.input_param.host_ip_address = "192.168.1.100"; + param.input_param.multicast_ip_address = ""; + + param.input_param.ptc_mode = PtcMode::tcp_ssl; + param.input_param.certFile = "Your cert file"; + param.input_param.privateKeyFile = "Your privateKey file"; + param.input_param.caFile = "Your ca file"; + + //init lidar with param + sample.Init(param); + + //assign callback fuction + sample.RegRecvCallback(lidarCallback); + + //star process thread + sample.Start(); + while (1) + { + +#ifdef ENABLE_VIEWER + mex_viewer.lock(); + if(pcl_viewer->wasStopped()) break; + pcl_viewer->spinOnce(); + mex_viewer.unlock(); +#endif + std::this_thread::sleep_for(std::chrono::milliseconds(40)); + + } +} \ No newline at end of file diff --git a/deploy/ros2_ws/src/lidar_node/src/manager/node_manager.cc b/deploy/ros2_ws/src/lidar_node/src/manager/node_manager.cc new file mode 100644 index 0000000..db94bb7 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/manager/node_manager.cc @@ -0,0 +1,70 @@ +/************************************************************************************************ + Copyright(C)2023 Hesai Technology Co., Ltd. + All code in this repository is released under the terms of the following [Modified BSD License.] + Modified BSD License: + Redistribution and use in source and binary forms,with or without modification,are permitted + provided that the following conditions are met: + *Redistributions of source code must retain the above copyright notice,this list of conditions + and the following disclaimer. + *Redistributions in binary form must reproduce the above copyright notice,this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + *Neither the names of the University of Texas at Austin,nor Austin Robot Technology,nor the names of + other contributors maybe used to endorse or promote products derived from this software without + specific prior written permission. + THIS SOFTWARE IS PROVIDED BY THE COPYRIGH THOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED + WARRANTIES,INCLUDING,BUT NOT LIMITED TO,THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A + PARTICULAR PURPOSE ARE DISCLAIMED.IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR + ANY DIRECT,INDIRECT,INCIDENTAL,SPECIAL,EXEMPLARY,OR CONSEQUENTIAL DAMAGES(INCLUDING,BUT NOT LIMITED TO, + PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;LOSS OF USE,DATA,OR PROFITS;OR BUSINESS INTERRUPTION)HOWEVER + CAUSED AND ON ANY THEORY OF LIABILITY,WHETHER IN CONTRACT,STRICT LIABILITY,OR TORT(INCLUDING NEGLIGENCE + OR OTHERWISE)ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,EVEN IF ADVISED OF THE POSSIBILITY OF + SUCHDAMAGE. +************************************************************************************************/ + +/* + * File: node_manager.cc + * Author: Zhang Yu + * Description: ROS Node Manager + * Created on June 12, 2023, 10:46 AM + */ + +#include "manager/node_manager.h" +void NodeManager::Init(const YAML::Node& config) +{ + YAML::Node lidar_config = YamlSubNodeAbort(config, "lidar"); + for (uint8_t i = 0; i < lidar_config.size(); ++i) + { + std::shared_ptr source; + source = std::make_shared(SourceType::DATA_FROM_LIDAR); + source->Init(lidar_config[i]); + sources_driver_.emplace_back(source); + } +} + +void NodeManager::Start() +{ + for (auto& iter : sources_driver_) + { + if (iter != nullptr) + { + iter->Start(); + } + } +} + +void NodeManager::Stop() +{ + for (auto& iter : sources_driver_) + { + if (iter != nullptr) + { + iter->Stop(); + } + } +} + +NodeManager::~NodeManager() +{ + Stop(); +} + diff --git a/deploy/ros2_ws/src/lidar_node/src/manager/node_manager.cu b/deploy/ros2_ws/src/lidar_node/src/manager/node_manager.cu new file mode 100644 index 0000000..db94bb7 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/manager/node_manager.cu @@ -0,0 +1,70 @@ +/************************************************************************************************ + Copyright(C)2023 Hesai Technology Co., Ltd. + All code in this repository is released under the terms of the following [Modified BSD License.] + Modified BSD License: + Redistribution and use in source and binary forms,with or without modification,are permitted + provided that the following conditions are met: + *Redistributions of source code must retain the above copyright notice,this list of conditions + and the following disclaimer. + *Redistributions in binary form must reproduce the above copyright notice,this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + *Neither the names of the University of Texas at Austin,nor Austin Robot Technology,nor the names of + other contributors maybe used to endorse or promote products derived from this software without + specific prior written permission. + THIS SOFTWARE IS PROVIDED BY THE COPYRIGH THOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED + WARRANTIES,INCLUDING,BUT NOT LIMITED TO,THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A + PARTICULAR PURPOSE ARE DISCLAIMED.IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR + ANY DIRECT,INDIRECT,INCIDENTAL,SPECIAL,EXEMPLARY,OR CONSEQUENTIAL DAMAGES(INCLUDING,BUT NOT LIMITED TO, + PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;LOSS OF USE,DATA,OR PROFITS;OR BUSINESS INTERRUPTION)HOWEVER + CAUSED AND ON ANY THEORY OF LIABILITY,WHETHER IN CONTRACT,STRICT LIABILITY,OR TORT(INCLUDING NEGLIGENCE + OR OTHERWISE)ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,EVEN IF ADVISED OF THE POSSIBILITY OF + SUCHDAMAGE. +************************************************************************************************/ + +/* + * File: node_manager.cc + * Author: Zhang Yu + * Description: ROS Node Manager + * Created on June 12, 2023, 10:46 AM + */ + +#include "manager/node_manager.h" +void NodeManager::Init(const YAML::Node& config) +{ + YAML::Node lidar_config = YamlSubNodeAbort(config, "lidar"); + for (uint8_t i = 0; i < lidar_config.size(); ++i) + { + std::shared_ptr source; + source = std::make_shared(SourceType::DATA_FROM_LIDAR); + source->Init(lidar_config[i]); + sources_driver_.emplace_back(source); + } +} + +void NodeManager::Start() +{ + for (auto& iter : sources_driver_) + { + if (iter != nullptr) + { + iter->Start(); + } + } +} + +void NodeManager::Stop() +{ + for (auto& iter : sources_driver_) + { + if (iter != nullptr) + { + iter->Stop(); + } + } +} + +NodeManager::~NodeManager() +{ + Stop(); +} + diff --git a/deploy/ros2_ws/src/lidar_node/src/manager/node_manager.h b/deploy/ros2_ws/src/lidar_node/src/manager/node_manager.h new file mode 100644 index 0000000..1000980 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/manager/node_manager.h @@ -0,0 +1,59 @@ +/************************************************************************************************ + Copyright(C)2023 Hesai Technology Co., Ltd. + All code in this repository is released under the terms of the following [Modified BSD License.] + Modified BSD License: + Redistribution and use in source and binary forms,with or without modification,are permitted + provided that the following conditions are met: + *Redistributions of source code must retain the above copyright notice,this list of conditions + and the following disclaimer. + *Redistributions in binary form must reproduce the above copyright notice,this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + *Neither the names of the University of Texas at Austin,nor Austin Robot Technology,nor the names of + other contributors maybe used to endorse or promote products derived from this software without + specific prior written permission. + THIS SOFTWARE IS PROVIDED BY THE COPYRIGH THOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED + WARRANTIES,INCLUDING,BUT NOT LIMITED TO,THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A + PARTICULAR PURPOSE ARE DISCLAIMED.IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR + ANY DIRECT,INDIRECT,INCIDENTAL,SPECIAL,EXEMPLARY,OR CONSEQUENTIAL DAMAGES(INCLUDING,BUT NOT LIMITED TO, + PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;LOSS OF USE,DATA,OR PROFITS;OR BUSINESS INTERRUPTION)HOWEVER + CAUSED AND ON ANY THEORY OF LIABILITY,WHETHER IN CONTRACT,STRICT LIABILITY,OR TORT(INCLUDING NEGLIGENCE + OR OTHERWISE)ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,EVEN IF ADVISED OF THE POSSIBILITY OF + SUCHDAMAGE. +************************************************************************************************/ + +/* + * File: node_manager.h + * Author: Zhang Yu + * Description: ROS Node Manager + * Created on June 12, 2023, 10:46 AM + */ + +#pragma once + +#include "utility/yaml_reader.hpp" +#ifdef ROS_FOUND +#include "source_driver_ros1.hpp" +#elif ROS2_FOUND +#include "source_driver_ros2.hpp" +#endif + +class NodeManager +{ +public: + + // Initialize ROS nodes based on configuration files + void Init(const YAML::Node& config); + // Start working + void Start(); + // Stop working + void Stop(); + + ~NodeManager(); + NodeManager() = default; + +private: + + std::vector sources_driver_; +}; + + diff --git a/deploy/ros2_ws/src/lidar_node/src/manager/source_driver_ros1.hpp b/deploy/ros2_ws/src/lidar_node/src/manager/source_driver_ros1.hpp new file mode 100644 index 0000000..dfebd1e --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/manager/source_driver_ros1.hpp @@ -0,0 +1,262 @@ +/************************************************************************************************ + Copyright(C)2023 Hesai Technology Co., Ltd. + All code in this repository is released under the terms of the following [Modified BSD License.] + Modified BSD License: + Redistribution and use in source and binary forms,with or without modification,are permitted + provided that the following conditions are met: + *Redistributions of source code must retain the above copyright notice,this list of conditions + and the following disclaimer. + *Redistributions in binary form must reproduce the above copyright notice,this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + *Neither the names of the University of Texas at Austin,nor Austin Robot Technology,nor the names of + other contributors maybe used to endorse or promote products derived from this software without + specific prior written permission. + THIS SOFTWARE IS PROVIDED BY THE COPYRIGH THOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED + WARRANTIES,INCLUDING,BUT NOT LIMITED TO,THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A + PARTICULAR PURPOSE ARE DISCLAIMED.IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR + ANY DIRECT,INDIRECT,INCIDENTAL,SPECIAL,EXEMPLARY,OR CONSEQUENTIAL DAMAGES(INCLUDING,BUT NOT LIMITED TO, + PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;LOSS OF USE,DATA,OR PROFITS;OR BUSINESS INTERRUPTION)HOWEVER + CAUSED AND ON ANY THEORY OF LIABILITY,WHETHER IN CONTRACT,STRICT LIABILITY,OR TORT(INCLUDING NEGLIGENCE + OR OTHERWISE)ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,EVEN IF ADVISED OF THE POSSIBILITY OF + SUCHDAMAGE. +************************************************************************************************/ + +/* + * File: source_driver_ros1.hpp + * Author: Zhang Yu + * Description: Source Driver for ROS1 + * Created on June 12, 2023, 10:46 AM + */ + +#pragma once +#include +#include +#include "hesai_ros_driver/UdpFrame.h" +#include "hesai_ros_driver/UdpPacket.h" + +#ifdef __CUDACC__ + #include "hesai_lidar_sdk_gpu.cuh" +#else + #include "hesai_lidar_sdk.hpp" +#endif +class SourceDriver +{ +public: + typedef std::shared_ptr Ptr; + // Initialize some necessary configuration parameters, create ROS nodes, and register callback functions + virtual void Init(const YAML::Node& config); + // Start working + virtual void Start(); + // Stop working + virtual void Stop(); + virtual ~SourceDriver(); + SourceDriver(SourceType src_type) {}; + void SpinRos1() { + ros::MultiThreadedSpinner spinner(2); + spinner.spin(); + } +protected: + // Save packets subscribed by 'ros_recv_packet_topic' + void RecievePacket(const hesai_ros_driver::UdpFrame& msg); + // Used to publish point clouds through 'ros_send_point_cloud_topic' + void SendPointCloud(const LidarDecodedFrame& msg); + // Used to publish the original pcake through 'ros_send_packet_topic' + void SendPacket(const UdpFrame_t& ros_msg, double); + // Convert point clouds into ROS messages + sensor_msgs::PointCloud2 ToRosMsg(const LidarDecodedFrame& frame, const std::string& frame_id); + // Convert packets into ROS messages + hesai_ros_driver::UdpFrame ToRosMsg(const UdpFrame_t& ros_msg, double timestamp); + + #ifdef __CUDACC__ + std::shared_ptr> driver_ptr_; + #else + std::shared_ptr> driver_ptr_; + #endif + // publish point + std::shared_ptr nh_; + ros::Publisher pub_; + std::string frame_id_; + // publish packet + ros::Publisher pkt_pub_; + // packet sub + ros::Subscriber pkt_sub_; + //spin thread while recieve data from ROS topic + boost::thread* subscription_spin_thread_; +}; + + +inline void SourceDriver::Init(const YAML::Node& config) +{ + YAML::Node driver_config = YamlSubNodeAbort(config, "driver"); + DriverParam driver_param; + // input related + YamlRead(driver_config, "udp_port", driver_param.input_param.udp_port, 2368); + YamlRead(driver_config, "ptc_port", driver_param.input_param.ptc_port, 9347); + YamlRead(driver_config, "host_ip_address", driver_param.input_param.host_ip_address, "192.168.1.100"); + YamlRead(driver_config, "group_address", driver_param.input_param.multicast_ip_address, ""); + YamlRead(driver_config, "pcap_path", driver_param.input_param.pcap_path, ""); + YamlRead(driver_config, "firetimes_path",driver_param.input_param.firetimes_path, ""); + YamlRead(driver_config, "correction_file_path", driver_param.input_param.correction_file_path, ""); + + // decoder related + YamlRead(driver_config, "pcap_play_synchronization", driver_param.decoder_param.pcap_play_synchronization, true); + YamlRead(driver_config, "x", driver_param.decoder_param.transform_param.x, 0); + YamlRead(driver_config, "y", driver_param.decoder_param.transform_param.y, 0); + YamlRead(driver_config, "z", driver_param.decoder_param.transform_param.z, 0); + YamlRead(driver_config, "roll", driver_param.decoder_param.transform_param.roll, 0); + YamlRead(driver_config, "pitch", driver_param.decoder_param.transform_param.pitch, 0); + YamlRead(driver_config, "yaw", driver_param.decoder_param.transform_param.yaw, 0); + YamlRead(driver_config, "device_ip_address", driver_param.input_param.device_ip_address, "192.168.1.201"); + YamlRead(driver_config, "frame_start_azimuth", driver_param.decoder_param.frame_start_azimuth, -1); + + int source_type = 0; + YamlRead(driver_config, "source_type", source_type, 0); + driver_param.input_param.source_type = SourceType(source_type); + bool send_packet_ros; + YamlRead(config["ros"], "send_packet_ros", send_packet_ros, false); + bool send_point_cloud_ros; + YamlRead(config["ros"], "send_point_cloud_ros", send_point_cloud_ros, false); + YamlRead(config["ros"], "ros_frame_id", frame_id_, "hesai_lidar"); + std::string ros_send_packet_topic; + YamlRead(config["ros"], "ros_send_packet_topic", ros_send_packet_topic, "hesai_packets"); + std::string ros_send_point_topic; + YamlRead(config["ros"], "ros_send_point_cloud_topic", ros_send_point_topic, "hesai_points"); + + nh_ = std::unique_ptr(new ros::NodeHandle()); + if (send_point_cloud_ros) { + std::string ros_send_point_topic; + YamlRead(config["ros"], "ros_send_point_cloud_topic", ros_send_point_topic, "hesai_points"); + pub_ = nh_->advertise(ros_send_point_topic, 10); + } + + if (send_packet_ros && driver_param.input_param.source_type != DATA_FROM_ROS_PACKET) { + std::string ros_send_packet_topic; + YamlRead(config["ros"], "ros_send_packet_topic", ros_send_packet_topic, "hesai_packets"); + pkt_pub_ = nh_->advertise(ros_send_packet_topic, 10); + } + + if (driver_param.input_param.source_type == DATA_FROM_ROS_PACKET) { + std::string ros_recv_packet_topic; + YamlRead(config["ros"], "ros_recv_packet_topic", ros_recv_packet_topic, "hesai_packets"); + pkt_sub_ = nh_->subscribe(ros_recv_packet_topic, 100, &SourceDriver::RecievePacket, this); + driver_param.decoder_param.enable_udp_thread = false; + subscription_spin_thread_ = new boost::thread(boost::bind(&SourceDriver::SpinRos1,this)); + } + + #ifdef __CUDACC__ + driver_ptr_.reset(new HesaiLidarSdkGpu()); + driver_param.decoder_param.enable_parser_thread = false; + #else + driver_ptr_.reset(new HesaiLidarSdk()); + driver_param.decoder_param.enable_parser_thread = true; + #endif + driver_ptr_->RegRecvCallback(std::bind(&SourceDriver::SendPointCloud, this, std::placeholders::_1)); + if(send_packet_ros && driver_param.input_param.source_type != DATA_FROM_ROS_PACKET){ + driver_ptr_->RegRecvCallback(std::bind(&SourceDriver::SendPacket, this, std::placeholders::_1, std::placeholders::_2)) ; + } + if (!driver_ptr_->Init(driver_param)) + { + std::cout << "Driver Initialize Error...." << std::endl; + exit(-1); + } +} + +inline void SourceDriver::Start() +{ + driver_ptr_->Start(); +} + +inline SourceDriver::~SourceDriver() +{ + Stop(); +} + +inline void SourceDriver::Stop() +{ + driver_ptr_->Stop(); +} + +inline void SourceDriver::SendPacket(const UdpFrame_t& msg, double timestamp) +{ + pkt_pub_.publish(ToRosMsg(msg, timestamp)); +} + +inline void SourceDriver::SendPointCloud(const LidarDecodedFrame& msg) +{ + pub_.publish(ToRosMsg(msg, frame_id_)); +} + +inline sensor_msgs::PointCloud2 SourceDriver::ToRosMsg(const LidarDecodedFrame& frame, const std::string& frame_id) +{ + sensor_msgs::PointCloud2 ros_msg; + + int fields = 6; + ros_msg.fields.clear(); + ros_msg.fields.reserve(fields); + ros_msg.width = frame.points_num; + ros_msg.height = 1; + + int offset = 0; + offset = addPointField(ros_msg, "x", 1, sensor_msgs::PointField::FLOAT32, offset); + offset = addPointField(ros_msg, "y", 1, sensor_msgs::PointField::FLOAT32, offset); + offset = addPointField(ros_msg, "z", 1, sensor_msgs::PointField::FLOAT32, offset); + offset = addPointField(ros_msg, "intensity", 1, sensor_msgs::PointField::FLOAT32, offset); + offset = addPointField(ros_msg, "ring", 1, sensor_msgs::PointField::UINT16, offset); + offset = addPointField(ros_msg, "timestamp", 1, sensor_msgs::PointField::FLOAT64, offset); + + ros_msg.point_step = offset; + ros_msg.row_step = ros_msg.width * ros_msg.point_step; + ros_msg.is_dense = false; + ros_msg.data.resize(frame.points_num * ros_msg.point_step); + + sensor_msgs::PointCloud2Iterator iter_x_(ros_msg, "x"); + sensor_msgs::PointCloud2Iterator iter_y_(ros_msg, "y"); + sensor_msgs::PointCloud2Iterator iter_z_(ros_msg, "z"); + sensor_msgs::PointCloud2Iterator iter_intensity_(ros_msg, "intensity"); + sensor_msgs::PointCloud2Iterator iter_ring_(ros_msg, "ring"); + sensor_msgs::PointCloud2Iterator iter_timestamp_(ros_msg, "timestamp"); + for (size_t i = 0; i < frame.points_num; i++) + { + LidarPointXYZIRT point = frame.points[i]; + *iter_x_ = point.x; + *iter_y_ = point.y; + *iter_z_ = point.z; + *iter_intensity_ = point.intensity; + *iter_ring_ = point.ring; + *iter_timestamp_ = point.timestamp; + ++iter_x_; + ++iter_y_; + ++iter_z_; + ++iter_intensity_; + ++iter_ring_; + ++iter_timestamp_; + } + printf("frame:%d points:%u packet:%d start time:%lf end time:%lf\n",frame.frame_index, frame.points_num, frame.packet_num, frame.points[0].timestamp, frame.points[frame.points_num - 1].timestamp) ; + // ros_msg.header.seq = s; + ros_msg.header.stamp = ros::Time().fromSec(frame.points[0].timestamp); + ros_msg.header.frame_id = frame_id_; + return ros_msg; +} + +inline hesai_ros_driver::UdpFrame SourceDriver::ToRosMsg(const UdpFrame_t& ros_msg, double timestamp) { + hesai_ros_driver::UdpFrame rs_msg; + for (int i = 0 ; i < ros_msg.size(); i++) { + hesai_ros_driver::UdpPacket rawpacket; + rawpacket.size = ros_msg[i].packet_len; + rawpacket.data.resize(ros_msg[i].packet_len); + memcpy(&rawpacket.data[0], &ros_msg[i].buffer[0], ros_msg[i].packet_len); + rs_msg.packets.push_back(rawpacket); + } + rs_msg.header.stamp = ros::Time().fromSec(timestamp); + rs_msg.header.frame_id = frame_id_; + return rs_msg; +} + +inline void SourceDriver::RecievePacket(const hesai_ros_driver::UdpFrame& msg) +{ + for (int i = 0; i < msg.packets.size(); i++) { + driver_ptr_->lidar_ptr_->origin_packets_buffer_.emplace_back(&msg.packets[i].data[0], msg.packets[i].size); + } +} + + diff --git a/deploy/ros2_ws/src/lidar_node/src/manager/source_driver_ros2.hpp b/deploy/ros2_ws/src/lidar_node/src/manager/source_driver_ros2.hpp new file mode 100644 index 0000000..43324f9 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/manager/source_driver_ros2.hpp @@ -0,0 +1,264 @@ +/************************************************************************************************ + Copyright(C)2023 Hesai Technology Co., Ltd. + All code in this repository is released under the terms of the following [Modified BSD License.] + Modified BSD License: + Redistribution and use in source and binary forms,with or without modification,are permitted + provided that the following conditions are met: + *Redistributions of source code must retain the above copyright notice,this list of conditions + and the following disclaimer. + *Redistributions in binary form must reproduce the above copyright notice,this list of conditions and + the following disclaimer in the documentation and/or other materials provided with the distribution. + *Neither the names of the University of Texas at Austin,nor Austin Robot Technology,nor the names of + other contributors maybe used to endorse or promote products derived from this software without + specific prior written permission. + THIS SOFTWARE IS PROVIDED BY THE COPYRIGH THOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED + WARRANTIES,INCLUDING,BUT NOT LIMITED TO,THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A + PARTICULAR PURPOSE ARE DISCLAIMED.IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR + ANY DIRECT,INDIRECT,INCIDENTAL,SPECIAL,EXEMPLARY,OR CONSEQUENTIAL DAMAGES(INCLUDING,BUT NOT LIMITED TO, + PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;LOSS OF USE,DATA,OR PROFITS;OR BUSINESS INTERRUPTION)HOWEVER + CAUSED AND ON ANY THEORY OF LIABILITY,WHETHER IN CONTRACT,STRICT LIABILITY,OR TORT(INCLUDING NEGLIGENCE + OR OTHERWISE)ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,EVEN IF ADVISED OF THE POSSIBILITY OF + SUCHDAMAGE. +************************************************************************************************/ + +/* + * File: source_driver_ros2.hpp + * Author: Zhang Yu + * Description: Source Driver for ROS2 + * Created on June 12, 2023, 10:46 AM + */ + +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef __CUDACC__ + #include "hesai_lidar_sdk_gpu.cuh" +#else + #include "hesai_lidar_sdk.hpp" +#endif +class SourceDriver +{ +public: + typedef std::shared_ptr Ptr; + // Initialize some necessary configuration parameters, create ROS nodes, and register callback functions + virtual void Init(const YAML::Node& config); + // Start working + virtual void Start(); + // Stop working + virtual void Stop(); + virtual ~SourceDriver(); + SourceDriver(SourceType src_type) {}; + void SpinRos2(){rclcpp::spin(this->node_ptr_);} + std::shared_ptr node_ptr_; +protected: + // Save packets subscribed by 'ros_recv_packet_topic' + void RecievePacket(const hesai_ros_driver::msg::UdpFrame::SharedPtr msg); + // Used to publish point clouds through 'ros_send_point_cloud_topic' + void SendPointCloud(const LidarDecodedFrame& msg); + // Used to publish the original pcake through 'ros_send_packet_topic' + void SendPacket(const UdpFrame_t& ros_msg, double timestamp); + // Convert point clouds into ROS messages + sensor_msgs::msg::PointCloud2 ToRosMsg(const LidarDecodedFrame& frame, const std::string& frame_id); + // Convert packets into ROS messages + hesai_ros_driver::msg::UdpFrame ToRosMsg(const UdpFrame_t& ros_msg, double timestamp); + #ifdef __CUDACC__ + std::shared_ptr> driver_ptr_; + #else + std::shared_ptr> driver_ptr_; + #endif + std::string frame_id_; + rclcpp::Subscription::SharedPtr pkt_sub_; + rclcpp::Publisher::SharedPtr pkt_pub_; + rclcpp::Publisher::SharedPtr pub_; + //spin thread while recieve data from ROS topic + boost::thread* subscription_spin_thread_; +}; + + +inline void SourceDriver::Init(const YAML::Node& config) +{ + YAML::Node driver_config = YamlSubNodeAbort(config, "driver"); + DriverParam driver_param; + // input related + YamlRead(driver_config, "udp_port", driver_param.input_param.udp_port, 2368); + YamlRead(driver_config, "ptc_port", driver_param.input_param.ptc_port, 9347); + YamlRead(driver_config, "host_ip_address", driver_param.input_param.host_ip_address, "192.168.1.100"); + YamlRead(driver_config, "group_address", driver_param.input_param.multicast_ip_address, ""); + YamlRead(driver_config, "pcap_path", driver_param.input_param.pcap_path, ""); + YamlRead(driver_config, "firetimes_path",driver_param.input_param.firetimes_path, ""); + YamlRead(driver_config, "correction_file_path", driver_param.input_param.correction_file_path, ""); + + // decoder related + YamlRead(driver_config, "pcap_play_synchronization", driver_param.decoder_param.pcap_play_synchronization, true); + YamlRead(driver_config, "x", driver_param.decoder_param.transform_param.x, 0); + YamlRead(driver_config, "y", driver_param.decoder_param.transform_param.y, 0); + YamlRead(driver_config, "z", driver_param.decoder_param.transform_param.z, 0); + YamlRead(driver_config, "roll", driver_param.decoder_param.transform_param.roll, 0); + YamlRead(driver_config, "pitch", driver_param.decoder_param.transform_param.pitch, 0); + YamlRead(driver_config, "yaw", driver_param.decoder_param.transform_param.yaw, 0); + YamlRead(driver_config, "device_ip_address", driver_param.input_param.device_ip_address, "192.168.1.201"); + YamlRead(driver_config, "frame_start_azimuth", driver_param.decoder_param.frame_start_azimuth, -1); + + int source_type = 0; + YamlRead(driver_config, "source_type", source_type, 0); + driver_param.input_param.source_type = SourceType(source_type); + bool send_packet_ros; + YamlRead(config["ros"], "send_packet_ros", send_packet_ros, false); + bool send_point_cloud_ros; + YamlRead(config["ros"], "send_point_cloud_ros", send_point_cloud_ros, false); + YamlRead(config["ros"], "ros_frame_id", frame_id_, "hesai_lidar"); + std::string ros_send_packet_topic; + YamlRead(config["ros"], "ros_send_packet_topic", ros_send_packet_topic, "hesai_packets"); + std::string ros_send_point_topic; + YamlRead(config["ros"], "ros_send_point_cloud_topic", ros_send_point_topic, "hesai_points"); + + node_ptr_.reset(new rclcpp::Node("hesai_ros_driver_node")); + if (send_point_cloud_ros) { + std::string ros_send_point_topic; + YamlRead(config["ros"], "ros_send_point_cloud_topic", ros_send_point_topic, "hesai_points"); + pub_ = node_ptr_->create_publisher(ros_send_point_topic, 100); + } + + if (send_packet_ros && driver_param.input_param.source_type != DATA_FROM_ROS_PACKET) { + std::string ros_send_packet_topic; + YamlRead(config["ros"], "ros_send_packet_topic", ros_send_packet_topic, "hesai_packets"); + pkt_pub_ = node_ptr_->create_publisher(ros_send_packet_topic, 10); + } + + if (driver_param.input_param.source_type == DATA_FROM_ROS_PACKET) { + std::string ros_recv_packet_topic; + YamlRead(config["ros"], "ros_recv_packet_topic", ros_recv_packet_topic, "hesai_packets"); + pkt_sub_ = node_ptr_->create_subscription(ros_recv_packet_topic, 10, + std::bind(&SourceDriver::RecievePacket, this, std::placeholders::_1)); + driver_param.decoder_param.enable_udp_thread = false; + subscription_spin_thread_ = new boost::thread(boost::bind(&SourceDriver::SpinRos2,this)); + } + #ifdef __CUDACC__ + driver_ptr_.reset(new HesaiLidarSdkGpu()); + driver_param.decoder_param.enable_parser_thread = false; + #else + driver_ptr_.reset(new HesaiLidarSdk()); + driver_param.decoder_param.enable_parser_thread = true; + + #endif + driver_ptr_->RegRecvCallback(std::bind(&SourceDriver::SendPointCloud, this, std::placeholders::_1)); + if(send_packet_ros && driver_param.input_param.source_type != DATA_FROM_ROS_PACKET){ + driver_ptr_->RegRecvCallback(std::bind(&SourceDriver::SendPacket, this, std::placeholders::_1, std::placeholders::_2)) ; + } + if (!driver_ptr_->Init(driver_param)) + { + exit(-1); + } +} + +inline void SourceDriver::Start() +{ + driver_ptr_->Start(); +} + +inline SourceDriver::~SourceDriver() +{ + Stop(); +} + +inline void SourceDriver::Stop() +{ + driver_ptr_->Stop(); +} + +inline void SourceDriver::SendPacket(const UdpFrame_t& msg, double timestamp) +{ + pkt_pub_->publish(ToRosMsg(msg, timestamp)); +} + +inline void SourceDriver::SendPointCloud(const LidarDecodedFrame& msg) +{ + pub_->publish(ToRosMsg(msg, frame_id_)); +} + +inline sensor_msgs::msg::PointCloud2 SourceDriver::ToRosMsg(const LidarDecodedFrame& frame, const std::string& frame_id) +{ + sensor_msgs::msg::PointCloud2 ros_msg; + + int fields = 6; + ros_msg.fields.clear(); + ros_msg.fields.reserve(fields); + ros_msg.width = frame.points_num; + ros_msg.height = 1; + + int offset = 0; + offset = addPointField(ros_msg, "x", 1, sensor_msgs::msg::PointField::FLOAT32, offset); + offset = addPointField(ros_msg, "y", 1, sensor_msgs::msg::PointField::FLOAT32, offset); + offset = addPointField(ros_msg, "z", 1, sensor_msgs::msg::PointField::FLOAT32, offset); + offset = addPointField(ros_msg, "intensity", 1, sensor_msgs::msg::PointField::FLOAT32, offset); + offset = addPointField(ros_msg, "ring", 1, sensor_msgs::msg::PointField::UINT16, offset); + offset = addPointField(ros_msg, "timestamp", 1, sensor_msgs::msg::PointField::FLOAT64, offset); + + + ros_msg.point_step = offset; + ros_msg.row_step = ros_msg.width * ros_msg.point_step; + ros_msg.is_dense = false; + ros_msg.data.resize(frame.points_num * ros_msg.point_step); + + sensor_msgs::PointCloud2Iterator iter_x_(ros_msg, "x"); + sensor_msgs::PointCloud2Iterator iter_y_(ros_msg, "y"); + sensor_msgs::PointCloud2Iterator iter_z_(ros_msg, "z"); + sensor_msgs::PointCloud2Iterator iter_intensity_(ros_msg, "intensity"); + sensor_msgs::PointCloud2Iterator iter_ring_(ros_msg, "ring"); + sensor_msgs::PointCloud2Iterator iter_timestamp_(ros_msg, "timestamp"); + + for (size_t i = 0; i < frame.points_num; i++) + { + LidarPointXYZIRT point = frame.points[i]; + *iter_x_ = point.x; + *iter_y_ = point.y; + *iter_z_ = point.z; + *iter_intensity_ = point.intensity; + *iter_ring_ = point.ring; + *iter_timestamp_ = point.timestamp; + ++iter_x_; + ++iter_y_; + ++iter_z_; + ++iter_intensity_; + ++iter_ring_; + ++iter_timestamp_; + } + printf("frame:%d points:%u packet:%d start time:%lf end time:%lf\n",frame.frame_index, frame.points_num, frame.packet_num, frame.points[0].timestamp, frame.points[frame.points_num - 1].timestamp) ; + + ros_msg.header.stamp.sec = (uint32_t)floor(frame.points[0].timestamp); + ros_msg.header.stamp.nanosec = (uint32_t)round((frame.points[0].timestamp - ros_msg.header.stamp.sec) * 1e9); + ros_msg.header.frame_id = frame_id_; + return ros_msg; +} + +inline hesai_ros_driver::msg::UdpFrame SourceDriver::ToRosMsg(const UdpFrame_t& ros_msg, double timestamp) { + hesai_ros_driver::msg::UdpFrame rs_msg; + for (int i = 0 ; i < ros_msg.size(); i++) { + hesai_ros_driver::msg::UdpPacket rawpacket; + rawpacket.size = ros_msg[i].packet_len; + rawpacket.data.resize(ros_msg[i].packet_len); + memcpy(&rawpacket.data[0], &ros_msg[i].buffer[0], ros_msg[i].packet_len); + rs_msg.packets.push_back(rawpacket); + } + rs_msg.header.stamp.sec = (uint32_t)floor(timestamp); + rs_msg.header.stamp.nanosec = (uint32_t)round((timestamp - rs_msg.header.stamp.sec) * 1e9); + rs_msg.header.frame_id = frame_id_; + return rs_msg; +} + +inline void SourceDriver::RecievePacket(const hesai_ros_driver::msg::UdpFrame::SharedPtr msg) +{ + for (int i = 0; i < msg->packets.size(); i++) { + driver_ptr_->lidar_ptr_->origin_packets_buffer_.emplace_back(&msg->packets[i].data[0], msg->packets[i].size); + } +} + + diff --git a/deploy/ros2_ws/src/lidar_node/src/utility/yaml_reader.hpp b/deploy/ros2_ws/src/lidar_node/src/utility/yaml_reader.hpp new file mode 100644 index 0000000..219b1e6 --- /dev/null +++ b/deploy/ros2_ws/src/lidar_node/src/utility/yaml_reader.hpp @@ -0,0 +1,48 @@ +#pragma once + +#include +#include + + + +template +inline void YamlReadAbort(const YAML::Node& yaml, const std::string& key, T& out_val) +{ + if (!yaml[key] || yaml[key].Type() == YAML::NodeType::Null) + { + std::cout << " : Not set " << key << std::endl; + std::cout << " value, Aborting!!!" << std::endl; + exit(-1); + } + else + { + out_val = yaml[key].as(); + } +} + +template +inline bool YamlRead(const YAML::Node& yaml, const std::string& key, T& out_val, const T& default_val) +{ + if (!yaml[key] || yaml[key].Type() == YAML::NodeType::Null) + { + out_val = default_val; + return false; + } + else + { + out_val = yaml[key].as(); + return true; + } +} + +inline YAML::Node YamlSubNodeAbort(const YAML::Node& yaml, const std::string& node) +{ + YAML::Node ret = yaml[node.c_str()]; + if (!ret) + { + std::cout << " : Cannot find subnode " << node << ". Aborting!!!" << std::endl; + exit(-1); + } + return ret; +} + diff --git a/deploy/scripts/hw_start.sh b/deploy/scripts/hw_start.sh new file mode 100644 index 0000000..86737b3 --- /dev/null +++ b/deploy/scripts/hw_start.sh @@ -0,0 +1 @@ +source /ros2_ws/install/setup.bash && ros2 launch /root/launch/hw.launch.py \ No newline at end of file