Merge pull request #1 from leggedrobotics/new/driver

Upgarde the driver to v2.0.7 and add replay functionality

.gitignore

@@ -0,0 +1,2 @@
+build
+.vscode

CMakeLists.txt

@@ -7,7 +7,7 @@ project(hesai_ros_driver)
 #=======================================
 set(VERSION_MAJOR 2)
 set(VERSION_MINOR 0)
-set(VERSION_TINY  5)
+set(VERSION_TINY  7)
 configure_file(
   "${CMAKE_CURRENT_SOURCE_DIR}/Version.h.in"
   "${CMAKE_CURRENT_BINARY_DIR}/Version.h"
@@ -55,8 +55,9 @@ add_compile_options(-Wall)
 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})
+  find_package(rosbag REQUIRED)
+  include_directories(${roscpp_INCLUDE_DIRS} ${roslib_INCLUDE_DIRS} ${rosbag_INCLUDE_DIRS})
+  set(ROS_LIBS ${roscpp_LIBRARIES} ${roslib_LIBRARIES} ${rosbag_LIBRARIES})
   add_definitions(-DROS_FOUND)
   add_definitions(-DRUN_IN_ROS_WORKSPACE)
 
@@ -65,12 +66,16 @@ if($ENV{ROS_VERSION} MATCHES "1")
     sensor_msgs
     std_msgs
     message_generation
-    roslib)
+    roslib
+    rosbag)
 
   add_message_files(
     FILES
     "UdpPacket.msg"
     "UdpFrame.msg"
+    "LossPacket.msg"
+    "Ptp.msg"
+    "Firetime.msg"
   )
 
   generate_messages(
@@ -111,6 +116,9 @@ if($ENV{ROS_VERSION} MATCHES "2")
     rosidl_generate_interfaces(${PROJECT_NAME}
       "msg/msg_ros2/UdpPacket.msg"
       "msg/msg_ros2/UdpFrame.msg"
+      "msg/msg_ros2/Firetime.msg"
+      "msg/msg_ros2/Ptp.msg"
+      "msg/msg_ros2/LossPacket.msg"
       DEPENDENCIES builtin_interfaces std_msgs
     )
     ament_export_dependencies(rosidl_default_runtime)                 
@@ -142,39 +150,45 @@ add_subdirectory(${PROJECT_SOURCE_DIR}/src/driver/hesai_lidar_sdk)
 #                node/hesai_ros_driver_node.cpp
 #                src/manager/node_manager.cpp
 #                )
+find_package(Boost REQUIRED COMPONENTS thread)
+include_directories(${Boost_INCLUDE_DIRS})
+link_directories(${Boost_LIBRARY_DIRS})
 
+# if install different cuda version, set the cuda path, like cuda-11.4
+# set(CUDA_TOOLKIT_ROOT_DIR /usr/local/cuda-11.4/)
+# find_package(CUDA REQUIRED)
+# find_package(CUDA )
 
-find_package(CUDA)
-if(CUDA_FOUND)
+# 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/hesai_lidar_sdk/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 Found. CUDA Support is turned On.")
+#   message(=============================================================)
+
+#   link_directories($ENV{CUDA_PATH}/lib/x64)
+#   set(CUDA_NVCC_FLAGS "-arch=sm_75;-O2")#根据具体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.")
@@ -194,7 +208,7 @@ else(CUDA_FOUND)
               # libhesai
   )            
 
-endif(CUDA_FOUND)
+# endif(CUDA_FOUND)
 
 
   target_include_directories(hesai_ros_driver_node PRIVATE

change notes.md

@@ -0,0 +1,46 @@
+# HesaiLidar_ROS_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
+
+################################################################################
+Monday, January 15th, 2024 21:30
+## version
+V2.0.6
+
+## modify
+1. Add point cloud timestamp usage type configuration
+2. Support P128 and XT32 to set the speed and standby through the configuration file
+3. Add support for AT128 to intercept FOV display
+
+################################################################################
+Saturday April 13th, 2024 20:10:00 CST
+## version
+V2.0.7
+
+## modify
+1. Fix gpu compile problem
+2. Resolve compile warnings, inluding cpu compile and gpu compile
+3. ROS add topics, support angle correction data, ptp data, loss bag count data

config/PandarXT-32.csv

@@ -0,0 +1,33 @@
+Channel,Elevation,Azimuth
+1,14.966,0.737
+2,13.95,0.741
+3,12.939,0.744
+4,11.94,0.748
+5,10.939,0.752
+6,9.941,0.754
+7,8.942,0.756
+8,7.95,0.759
+9,6.956,0.759
+10,5.961,0.761
+11,4.966,0.761
+12,3.975,0.759
+13,2.983,0.757
+14,1.991,0.756
+15,1.003,0.754
+16,0.013,0.753
+17,-0.983,0.751
+18,-1.976,0.748
+19,-2.97,0.746
+20,-3.964,0.743
+21,-4.959,0.742
+22,-5.955,0.739
+23,-6.949,0.735
+24,-7.947,0.731
+25,-8.939,0.725
+26,-9.94,0.719
+27,-10.939,0.712
+28,-11.94,0.705
+29,-12.945,0.699
+30,-13.951,0.691
+31,-14.965,0.684
+32,-15.985,0.679

config/PandarXT-32_firetime_correction.csv

@@ -0,0 +1,33 @@
+Channel,Firetime(us)
+1,3.56
+2,5.072
+3,6.584
+4,8.096
+5,9.608
+6,11.12
+7,12.632
+8,14.144
+9,15.656
+10,17.168
+11,16.68
+12,20.192
+13,21.704
+14,23.216
+15,24.728
+16,26.24
+17,27.752
+18,29.264
+19,30.776
+20,32.288
+21,33.8
+22,35.312
+23,36.824
+24,38.336
+25,39.848
+26,41.36
+27,42.872
+28,44.384
+29,45.896
+30,47.408
+31,48.92
+32,50.432

config/config.yaml

@@ -1,26 +1,43 @@
 lidar:
   - driver:
-      pcap_play_synchronization: true  # pcap play rate synchronize with the host time
+      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 recv = 2368
+      ptc_port: 9347                   #PTC port of lidar
       device_ip_address: 192.168.2.201 #host_ip_address
+      group_address: 255.255.255.255
       pcap_path: "Your pcap file path" #The path of pcap file
-      correction_file_path: "/home/ttuna/hesai_ws/src/grand_tour_box/box_bringup/bringup_hesai/config/PandarXT-32.csv" #The path of correction file
-      firetimes_path: "/home/ttuna/hesai_ws/src/grand_tour_box/box_bringup/bringup_hesai/config/PandarXT-32_firetime_correction.csv" #The path of firetimes file
+      correction_file_path: "/home/rsl/git/grand_tour_box/box_bringup/bringup_hesai/config/PandarXT-32.csv" #The path of correction file
+      firetimes_path: "/home/rsl/git/grand_tour_box/box_bringup/bringup_hesai/config/PandarXT-32_firetime_correction.csv" #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: 200               #Frame azimuth for Pandar128, range from 1 to 359, set it less than 0 if you 
-                                       #do not want to use it.
+      frame_start_azimuth: 200         #Frame azimuth for Pandar128, range from 1 to 359, set it less than 0 if you do not want to use it
+      standby_mode: -1                 #The standby mode: [-1] is invalit [0] in operation [1] standby
+      speed: 600                        #The speed: [-1] invalit, you must make sure your set has been supported by the lidar you are using
       #transform param
       x: 0
       y: 0
       z: 0
       roll: 0
       pitch: 0
       yaw: 0
+      #decoder param
+      use_timestamp_type: 0           #0 use point cloud timestamp; 1 use receive timestamp
+      #fov config, [fov_start, fov_end] range [1, 359], [-1, -1]means use default
+      fov_start: -1
+      fov_end:  -1
+      enable_packet_loss_tool: true     # enable the udp packet loss detection tool
     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 rosbag
-      ros_send_packet_topic: /lidar_packets         #Topic used to send lidar raw 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    
+      ros_frame_id: hesaiXT32                     #Frame id of packet message and point cloud message
+      save_replayed_topics_to_rosbag: false
+      output_rosbag_directory: /tmp/  #if left empty, it will save to /data in this driver.
+      # Topic used to monitor packets loss condition through ROS
+      ros_send_packet_loss_topic: /gt_box/hesai/lidar_packets_loss
+      ros_recv_packet_topic: /gt_box/hesai/packets         #Topic used to receive lidar packets from rosbag
+      ros_send_packet_topic: /gt_box/hesai/packets         #Topic used to send lidar raw packets through ROS
+      ros_send_point_cloud_topic: /gt_box/hesai/points     #Topic used to send point cloud through ROS
+      # ros_recv_correction_topic: /lidar_corrections #Topic used to receive corrections file from rosbag
+      # ros_send_correction_topic: /gt_box/lidar_corrections #Topic used to send correction through ROS
+      # ros_send_firetime_topic: /gt_box/lidar_firetime
+      # ros_send_ptp_topic: /gt_box/hesai/lidar_ptp
+      # Topic used to send PTP lock status, offset through ROS
+      send_packet_ros: true                         #true: Send packets through ROS 
+      send_point_cloud_ros: true                    #true: Send point cloud through ROS    

config/packet_replay.yaml

@@ -0,0 +1,43 @@
+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.2.201 #host_ip_address
+      group_address: 255.255.255.255
+      pcap_path: "Your pcap file path" #The path of pcap file
+      correction_file_path: "/home/rsl/git/grand_tour_box/box_bringup/bringup_hesai/config/PandarXT-32.csv" #The path of correction file
+      firetimes_path: "/home/rsl/git/grand_tour_box/box_bringup/bringup_hesai/config/PandarXT-32_firetime_correction.csv" #The path of firetimes file
+      source_type: 3                   #The type of data source, 1: real-time lidar connection, 2: pcap, 3: packet rosbag    
+      frame_start_azimuth: 200         #Frame azimuth for Pandar128, range from 1 to 359, set it less than 0 if you do not want to use it
+      standby_mode: -1                 #The standby mode: [-1] is invalit [0] in operation [1] standby
+      speed: 600                       #The speed: [-1] invalit, you must make sure your set has been supported by the lidar you are using
+      #transform param
+      x: 0
+      y: 0
+      z: 0
+      roll: 0
+      pitch: 0
+      yaw: 0
+      #decoder param
+      use_timestamp_type: 0           #0 use point cloud timestamp; 1 use receive timestamp
+      #fov config, [fov_start, fov_end] range [1, 359], [-1, -1]means use default
+      fov_start: -1
+      fov_end:  -1
+      enable_packet_loss_tool: true     # enable the udp packet loss detection tool
+    ros:
+      ros_frame_id: hesaiXT32                     #Frame id of packet message and point cloud message
+      save_replayed_topics_to_rosbag: true
+      output_rosbag_directory: /tmp/  #if left empty, it will save to /data in this driver.
+      # Topic used to monitor packets loss condition through ROS
+      ros_send_packet_loss_topic: /gt_box/hesai/lidar_packets_loss
+      ros_recv_packet_topic: /gt_box/hesai/packets         #Topic used to receive lidar packets from rosbag
+      ros_send_packet_topic: /gt_box/hesai/packets         #Topic used to send lidar raw packets through ROS
+      ros_send_point_cloud_topic: /gt_box/hesai/points     #Topic used to send point cloud through ROS
+      # ros_recv_correction_topic: /lidar_corrections #Topic used to receive corrections file from rosbag
+      # ros_send_correction_topic: /gt_box/lidar_corrections #Topic used to send correction through ROS
+      # ros_send_firetime_topic: /gt_box/lidar_firetime
+      # ros_send_ptp_topic: /gt_box/hesai/lidar_ptp
+      # Topic used to send PTP lock status, offset through ROS
+      send_packet_ros: false                         #true: Send packets through ROS 
+      send_point_cloud_ros: false                    #true: Send point cloud through ROS    

launch/replay_packets.launch

@@ -0,0 +1,24 @@
+<launch>
+
+
+  <arg name="input_rosbag_path" default="/home/tutuna/Downloads/stein_am_rhein_box/bags/2024-07-10-15-52-12_nuc_hesai_0.bag"/>
+
+  <param name="/use_sim_time" value="true"/>
+
+  <node pkg="rosbag" type="play" name="player" output="screen" launch-prefix="xterm -bg lightblue -fg darkslategrey -bd plum -bw 10 -geometry 80x40-50+250 -fn -sony-fixed-medium-r-normal--24-170-100-100-c-120-iso8859-1 -e " args="--clock --wait-for-subscribers -r 10 $(arg input_rosbag_path)"/>
+
+  <node pkg="hesai_ros_driver" name="hesai_ros_driver_node" type="hesai_ros_driver_node" output="screen" > <!-- launch-prefix="gdb -ex run args"> -->
+    <param name="config_path" type="string" value= "$(find hesai_ros_driver)/config/packet_replay.yaml"/>
+    <param name="/use_sim_time" value="true"/>
+  </node>
+  <!-- rviz -->
+  <node pkg="rviz" name="rviz" type="rviz" args="-d '$(find hesai_ros_driver)/rviz/rviz.rviz'" required="true"/>
+
+  <!-- Needs xterm -->
+  <!-- sudo apt install xterm -->
+
+  <!-- rosbag -->
+  <!-- <node pkg="rosbag" type="play" name="player" output="screen" launch-prefix="xterm -bg lightblue -fg darkslategrey -bd plum -bw 10 -geometry 80x40-50+250 -e " args="clock -r 10 $(arg input_rosbag_path)"/> -->
+
+</launch>
+

launch/start.launch

@@ -1,6 +1,6 @@
 <launch>
   <node pkg="hesai_ros_driver" name="hesai_ros_driver_node" type="hesai_ros_driver_node" output="screen">
-    <param name="config_path" type="string" value= "$(find bringup_hesai)/config/hesaiXT32_config.yaml"/>
+    <param name="config_path" type="string" value= "$(find hesai_ros_driver)/config/config.yaml"/>
   </node>
   <!-- rviz -->
   <node pkg="rviz" name="rviz" type="rviz" args="-d '$(find hesai_ros_driver)/rviz/rviz.rviz'" required="true"/>

launch/start.py

@@ -3,9 +3,7 @@
 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])

msg/Firetime.msg

@@ -0,0 +1 @@
+float64[512] data