diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..1899660 --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +build +.vscode \ No newline at end of file diff --git a/.gitmodules b/.gitmodules index f82203a..f126a65 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,4 @@ -[submodule "src/driver/HesaiLidar_SDK_2.0"] - path = src/driver/HesaiLidar_SDK_2.0 - url = https://github.com/HesaiTechnology/HesaiLidar_SDK_2.0.git +[submodule "src/driver/hesai_lidar_sdk"] + path = src/driver/hesai_lidar_sdk + url = git@github.com:leggedrobotics/hesai_lidar_sdk.git + branch = master diff --git a/CMakeLists.txt b/CMakeLists.txt index 0b83b3b..bad9668 100644 --- a/CMakeLists.txt +++ b/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) @@ -132,7 +140,7 @@ find_package(yaml-cpp REQUIRED) include_directories(${PROJECT_SOURCE_DIR}/src) #Driver core# -add_subdirectory(src/driver/HesaiLidar_SDK_2.0) +add_subdirectory(${PROJECT_SOURCE_DIR}/src/driver/hesai_lidar_sdk) #======================== # Build Setup @@ -142,39 +150,45 @@ add_subdirectory(src/driver/HesaiLidar_SDK_2.0) # 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/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 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,29 +208,29 @@ else(CUDA_FOUND) # libhesai ) -endif(CUDA_FOUND) +# 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 + ${PROJECT_SOURCE_DIR}/src/driver/hesai_lidar_sdk/ + ${PROJECT_SOURCE_DIR}/src/driver/hesai_lidar_sdk/libhesai + ${PROJECT_SOURCE_DIR}/src/driver/hesai_lidar_sdk/libhesai/Lidar + ${PROJECT_SOURCE_DIR}/src/driver/hesai_lidar_sdk/libhesai/UdpParser + ${PROJECT_SOURCE_DIR}/src/driver/hesai_lidar_sdk/libhesai/UdpParser/include + ${PROJECT_SOURCE_DIR}/src/driver/hesai_lidar_sdk/libhesai/UdpParser/src + ${PROJECT_SOURCE_DIR}/src/driver/hesai_lidar_sdk/libhesai/UdpProtocol + ${PROJECT_SOURCE_DIR}/src/driver/hesai_lidar_sdk/libhesai/Source/include + ${PROJECT_SOURCE_DIR}/src/driver/hesai_lidar_sdk/libhesai/Container/include + ${PROJECT_SOURCE_DIR}/src/driver/hesai_lidar_sdk/libhesai/Container/src + ${PROJECT_SOURCE_DIR}/src/driver/hesai_lidar_sdk/libhesai/UdpParserGpu + ${PROJECT_SOURCE_DIR}/src/driver/hesai_lidar_sdk/libhesai/UdpParserGpu/include + ${PROJECT_SOURCE_DIR}/src/driver/hesai_lidar_sdk/libhesai/UdpParserGpu/src + ${PROJECT_SOURCE_DIR}/src/driver/hesai_lidar_sdk/libhesai/PtcClient/include + ${PROJECT_SOURCE_DIR}/src/driver/hesai_lidar_sdk/libhesai/PtcParser/include + ${PROJECT_SOURCE_DIR}/src/driver/hesai_lidar_sdk/libhesai/PtcParser + ${PROJECT_SOURCE_DIR}/src/driver/hesai_lidar_sdk/libhesai/Logger/include + ${PROJECT_SOURCE_DIR}/src/driver/hesai_lidar_sdk/libhesai/include + ${PROJECT_SOURCE_DIR}/src/driver/hesai_lidar_sdk/driver src/manager src/msg/ros_msg src/msg/rs_msg diff --git a/README.md b/README.md index bcb7b96..b45de57 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # 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 +Developed based on [hesai_lidar_sdk](https://github.com/HesaiTechnology/hesai_lidar_sdk), After launched, the project will monitor UDP packets from Lidar,parse data and publish point cloud frames into ROS topic ## Support Lidar type - Pandar diff --git a/change notes.md b/change notes.md new file mode 100644 index 0000000..3087b44 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/config/PandarXT-32.csv b/config/PandarXT-32.csv new file mode 100755 index 0000000..bd68511 --- /dev/null +++ b/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 diff --git a/config/PandarXT-32_firetime_correction.csv b/config/PandarXT-32_firetime_correction.csv new file mode 100755 index 0000000..63a9b64 --- /dev/null +++ b/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 diff --git a/config/config.yaml b/config/config.yaml index 76dedb6..ae22b1e 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -1,15 +1,17 @@ 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 - device_ip_address: 192.168.1.201 #host_ip_address + 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: "Your correction file path" #The path of correction file - firetimes_path: "Your firetime file path" #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 @@ -17,10 +19,25 @@ lidar: 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 diff --git a/config/packet_replay.yaml b/config/packet_replay.yaml new file mode 100644 index 0000000..da3d8ed --- /dev/null +++ b/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 diff --git a/launch/replay_packets.launch b/launch/replay_packets.launch new file mode 100755 index 0000000..59393e5 --- /dev/null +++ b/launch/replay_packets.launch @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/start.launch b/launch/start.launch index 76e71eb..53185be 100755 --- a/launch/start.launch +++ b/launch/start.launch @@ -1,5 +1,6 @@ + diff --git a/launch/start.py b/launch/start.py index b670762..e21fb3b 100755 --- a/launch/start.py +++ b/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]) diff --git a/msg/Firetime.msg b/msg/Firetime.msg new file mode 100644 index 0000000..6667d04 --- /dev/null +++ b/msg/Firetime.msg @@ -0,0 +1 @@ +float64[512] data \ No newline at end of file diff --git a/msg/LossPacket.msg b/msg/LossPacket.msg new file mode 100644 index 0000000..dfda136 --- /dev/null +++ b/msg/LossPacket.msg @@ -0,0 +1,2 @@ +uint32 total_packet_count +uint32 total_packet_loss_count \ No newline at end of file diff --git a/msg/Ptp.msg b/msg/Ptp.msg new file mode 100644 index 0000000..0af845f --- /dev/null +++ b/msg/Ptp.msg @@ -0,0 +1,2 @@ +uint8 ptp_lock_offset +uint8[16] ptp_status \ No newline at end of file diff --git a/msg/msg_ros2/Firetime.msg b/msg/msg_ros2/Firetime.msg new file mode 100644 index 0000000..6667d04 --- /dev/null +++ b/msg/msg_ros2/Firetime.msg @@ -0,0 +1 @@ +float64[512] data \ No newline at end of file diff --git a/msg/msg_ros2/LossPacket.msg b/msg/msg_ros2/LossPacket.msg new file mode 100644 index 0000000..dfda136 --- /dev/null +++ b/msg/msg_ros2/LossPacket.msg @@ -0,0 +1,2 @@ +uint32 total_packet_count +uint32 total_packet_loss_count \ No newline at end of file diff --git a/msg/msg_ros2/Ptp.msg b/msg/msg_ros2/Ptp.msg new file mode 100644 index 0000000..0af845f --- /dev/null +++ b/msg/msg_ros2/Ptp.msg @@ -0,0 +1,2 @@ +uint8 ptp_lock_offset +uint8[16] ptp_status \ No newline at end of file diff --git a/node/hesai_ros_driver_node.cc b/node/hesai_ros_driver_node.cc index 1078279..1104873 100644 --- a/node/hesai_ros_driver_node.cc +++ b/node/hesai_ros_driver_node.cc @@ -30,10 +30,8 @@ #include "manager/node_manager.h" #include - #include #include "Version.h" - #ifdef ROS_FOUND #include #include @@ -46,7 +44,6 @@ std::mutex g_mtx; std::condition_variable g_cv; #endif - static void sigHandler(int sig) { #ifdef ROS_FOUND @@ -60,7 +57,6 @@ 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 @@ -70,39 +66,41 @@ int main(int argc, char** argv) std::string config_path; #ifdef RUN_IN_ROS_WORKSPACE - config_path = ros::package::getPath("hesai_ros_driver"); + config_path = ros::package::getPath("bringup_hesai"); #else config_path = (std::string)PROJECT_PATH; #endif - config_path += "/config/config.yaml"; + config_path += "/config/hesaiXT32_config.yaml"; #ifdef ROS_FOUND ros::NodeHandle priv_hh("~"); - std::string path; - priv_hh.param("config_path", path, std::string("")); + std::string path = ""; + priv_hh.getParam("config_path", path); if (!path.empty()) { + std::cout << "Path loaded from ROS params." << std::endl; config_path = path; } #endif - + std::cout << "Hesai Lidar ROS config_path: " << config_path << std::endl; 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 + // you can chose [!demo_ptr->IsPlayEnded()] or [1] + // If you chose !demo_ptr->IsPlayEnded(), ROS node will end with the end of the PCAP. + // If you select 1, the ROS node does not end with the end of the PCAP. ros::MultiThreadedSpinner spinner(2); spinner.spin(); -#elif ROS2_FOUND - std::unique_lock lck(g_mtx); - g_cv.wait(lck); -#endif - + // while (!demo_ptr->IsPlayEnded()) + // { + // std::this_thread::sleep_for(std::chrono::microseconds(100)); + // } + + demo_ptr->Stop(); + std::this_thread::sleep_for(std::chrono::microseconds(100)); + ros::shutdown(); return 0; } diff --git a/node/hesai_ros_driver_node.cu b/node/hesai_ros_driver_node.cu index 033532f..d4e1075 100644 --- a/node/hesai_ros_driver_node.cu +++ b/node/hesai_ros_driver_node.cu @@ -70,22 +70,24 @@ int main(int argc, char** argv) std::string config_path; #ifdef RUN_IN_ROS_WORKSPACE - config_path = ros::package::getPath("hesai_ros_driver"); + config_path = ros::package::getPath("bringup_hesai"); #else config_path = (std::string)PROJECT_PATH; #endif - config_path += "/config/config.yaml"; + config_path += "/config/hesaiXT32_config.yaml"; #ifdef ROS_FOUND ros::NodeHandle priv_hh("~"); - std::string path; - priv_hh.param("config_path", path, std::string("")); + std::string path = ""; + priv_hh.getParam("config_path", path); if (!path.empty()) { + std::cout << "Path loaded from ROS params." << std::endl; config_path = path; } #endif + std::cout << "Hesai Lidar ROS config_path: " << config_path << std::endl; YAML::Node config; config = YAML::LoadFile(config_path); diff --git a/package.xml b/package.xml index 3c45e64..baf383e 100644 --- a/package.xml +++ b/package.xml @@ -11,6 +11,7 @@ rosidl_default_generators roscpp + rosbag rclcpp sensor_msgs @@ -26,6 +27,7 @@ roscpp sensor_msgs roslib + rosbag rosidl_interface_packages diff --git a/rviz/rviz.rviz b/rviz/rviz.rviz index 2ca1f27..cd910e9 100755 --- a/rviz/rviz.rviz +++ b/rviz/rviz.rviz @@ -6,8 +6,6 @@ Panels: Expanded: - /Global Options1 - /Status1 - - /PointCloud26 - - /PointCloud28 Splitter Ratio: 0.5 Tree Height: 549 - Class: rviz/Selection @@ -54,146 +52,6 @@ Visualization Manager: 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: @@ -217,7 +75,7 @@ Visualization Manager: Size (Pixels): 3 Size (m): 0.05000000074505806 Style: Flat Squares - Topic: /lidar_points5 + Topic: /gt_box/hesai/points Unreliable: false Use Fixed Frame: true Use rainbow: true @@ -282,7 +140,7 @@ Visualization Manager: Global Options: Background Color: 48; 48; 48 Default Light: true - Fixed Frame: hesai_lidar + Fixed Frame: hesaiXT32 Frame Rate: 30 Name: root Tools: @@ -312,6 +170,7 @@ Visualization Manager: Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false + Field of View: 0.7853981852531433 Focal Point: X: 3.932443618774414 Y: -6.068606853485107 @@ -323,7 +182,6 @@ Visualization Manager: Near Clip Distance: 0.009999999776482582 Pitch: 0.335399329662323 Target Frame: - Value: Orbit (rviz) Yaw: 1.738570213317871 Saved: ~ Window Geometry: diff --git a/rviz/rviz2.rviz b/rviz/rviz2.rviz index af124ae..610e71b 100755 --- a/rviz/rviz2.rviz +++ b/rviz/rviz2.rviz @@ -78,7 +78,7 @@ Visualization Manager: Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: hesai_lidar + Fixed Frame: hesaiXT32 Frame Rate: 30 Name: root Tools: diff --git a/src/driver/HesaiLidar_SDK_2.0 b/src/driver/HesaiLidar_SDK_2.0 deleted file mode 160000 index 7d00ebe..0000000 --- a/src/driver/HesaiLidar_SDK_2.0 +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 7d00ebe715d22eecbd08fc97016b4d7182d5a0e6 diff --git a/src/driver/hesai_lidar_sdk b/src/driver/hesai_lidar_sdk new file mode 160000 index 0000000..810c89f --- /dev/null +++ b/src/driver/hesai_lidar_sdk @@ -0,0 +1 @@ +Subproject commit 810c89f75243012c0ee388b113d483dc4b44a22e diff --git a/src/manager/node_manager.cc b/src/manager/node_manager.cc index db94bb7..4fc2eda 100644 --- a/src/manager/node_manager.cc +++ b/src/manager/node_manager.cc @@ -67,4 +67,48 @@ NodeManager::~NodeManager() { Stop(); } +std::vector NodeManager::GetSourcesDriver() +{ + return sources_driver_; +} + +bool NodeManager::IsPlayEnded() { + int num = GetSourcesDriver().size(); + bool all_pcap_end = true; +#ifdef ROS_FOUND + for (int i = 0; i < num; i++) { + all_pcap_end = GetSourcesDriver()[i]->driver_ptr_->lidar_ptr_->IsPlayEnded(); + if (!all_pcap_end) { + break; + } + } + if (all_pcap_end) { + std::this_thread::sleep_for(std::chrono::seconds(3)); + printf("-----------------%d pcap(s) end, we will close the node(s)!!!-----------------\n", num); + if (system("rosnode kill rviz") == -1) { + printf("Command Execution Error: rosnode kill rviz\n"); + all_pcap_end = false;; + } + } +#elif ROS2_FOUND + for (int i = 0; i < num; i++) { + all_pcap_end = GetSourcesDriver()[i]->driver_ptr_->lidar_ptr_->IsPlayEnded(); + if (!all_pcap_end) { + break; + } + } + if (all_pcap_end) { + std::this_thread::sleep_for(std::chrono::seconds(3)); + printf("-----------------%d pcap(s) end, we will close the node(s)!!!-----------------\n", num); + std::cout.flush(); + if (system("pkill -f rviz2") == -1) { + printf("Command Execution Error: pkill -f rviz2\n"); + all_pcap_end = false; + } + std::this_thread::sleep_for(std::chrono::microseconds(1)); + std::cout.flush(); + } +#endif +return all_pcap_end; +} \ No newline at end of file diff --git a/src/manager/node_manager.h b/src/manager/node_manager.h index 1000980..14b7b27 100644 --- a/src/manager/node_manager.h +++ b/src/manager/node_manager.h @@ -47,10 +47,10 @@ class NodeManager void Start(); // Stop working void Stop(); - + bool IsPlayEnded(); ~NodeManager(); NodeManager() = default; - + std::vector GetSourcesDriver(); private: std::vector sources_driver_; diff --git a/src/manager/source_drive_common.hpp b/src/manager/source_drive_common.hpp new file mode 100644 index 0000000..4960886 --- /dev/null +++ b/src/manager/source_drive_common.hpp @@ -0,0 +1,63 @@ +#pragma once +#include "utility/yaml_reader.hpp" +#ifdef __CUDACC__ + #include "hesai_lidar_sdk_gpu.cuh" +#else + #include "hesai_lidar_sdk.hpp" +#endif +class DriveYamlParam +{ +public: + DriveYamlParam() {}; + ~DriveYamlParam() {}; + + bool GetDriveYamlParam(const YAML::Node& config, DriverParam &driver_param) + { + YAML::Node driver_config = YamlSubNodeAbort(config, "driver"); + int source_type; + + // 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, ""); + YamlRead( driver_config, "standby_mode", driver_param.input_param.standby_mode, -1); + YamlRead( driver_config, "speed", driver_param.input_param.speed, -1); + // decoder related + YamlRead( driver_config, "pcap_play_synchronization", driver_param.decoder_param.pcap_play_synchronization, false); + 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); + YamlRead( driver_config, "use_timestamp_type", driver_param.decoder_param.use_timestamp_type, 0); + YamlRead( driver_config, "fov_start", driver_param.decoder_param.fov_start, -1); + YamlRead( driver_config, "fov_end", driver_param.decoder_param.fov_end, -1); + YamlRead( driver_config, "source_type", source_type, 0); + driver_param.input_param.source_type = SourceType(source_type); + // ROS related + YamlRead( driver_config, "enable_packet_loss_tool", driver_param.decoder_param.enable_packet_loss_tool, false); + YamlRead( config["ros"], "send_packet_ros", driver_param.input_param.send_packet_ros, false); + YamlRead( config["ros"], "send_point_cloud_ros", driver_param.input_param.send_point_cloud_ros, false); + YamlRead( config["ros"], "save_replayed_topics_to_rosbag", driver_param.input_param.save_replayed_topics_to_rosbag, false); + YamlRead(config["ros"], "ros_frame_id", driver_param.input_param.frame_id, "hesai_lidar"); + YamlRead(config["ros"], "output_rosbag_directory", driver_param.input_param.output_rosbag_directory, "/tmp/"); + YamlRead(config["ros"], "ros_send_packet_topic", driver_param.input_param.ros_send_packet_topic, "hesai_packets"); + YamlRead(config["ros"], "ros_send_point_cloud_topic", driver_param.input_param.ros_send_point_topic, "hesai_points"); + YamlRead(config["ros"], "ros_recv_packet_topic", driver_param.input_param.ros_recv_packet_topic, "hesai_packets"); + YamlRead(config["ros"], "ros_send_packet_loss_topic", driver_param.input_param.ros_send_packet_loss_topic, NULL_TOPIC); + YamlRead(config["ros"], "ros_send_ptp_topic", driver_param.input_param.ros_send_ptp_topic, NULL_TOPIC); + YamlRead(config["ros"], "ros_send_correction_topic", driver_param.input_param.ros_send_correction_topic, NULL_TOPIC); + YamlRead(config["ros"], "ros_send_firetime_topic", driver_param.input_param.ros_send_firetime_topic, NULL_TOPIC); + YamlRead(config["ros"], "ros_recv_correction_topic", driver_param.input_param.ros_recv_correction_topic, NULL_TOPIC); + return true; + } + + +}; \ No newline at end of file diff --git a/src/manager/source_driver_ros1.hpp b/src/manager/source_driver_ros1.hpp index dfebd1e..acb5a8a 100644 --- a/src/manager/source_driver_ros1.hpp +++ b/src/manager/source_driver_ros1.hpp @@ -30,15 +30,17 @@ #pragma once #include +#include +#include "std_msgs/UInt8MultiArray.h" #include #include "hesai_ros_driver/UdpFrame.h" #include "hesai_ros_driver/UdpPacket.h" +#include "hesai_ros_driver/LossPacket.h" +#include "hesai_ros_driver/Ptp.h" +#include "hesai_ros_driver/Firetime.h" +#include +#include "source_drive_common.hpp" -#ifdef __CUDACC__ - #include "hesai_lidar_sdk_gpu.cuh" -#else - #include "hesai_lidar_sdk.hpp" -#endif class SourceDriver { public: @@ -55,23 +57,45 @@ class SourceDriver ros::MultiThreadedSpinner spinner(2); spinner.spin(); } + rosbag::Bag outputBag; + ros::Time latestCloudStamp_; + bool save_replayed_topics_to_rosbag_ = false; + #ifdef __CUDACC__ + std::shared_ptr> driver_ptr_; + #else + std::shared_ptr> driver_ptr_; + #endif protected: + // Save Correction file subscribed by "ros_recv_correction_topic" + void RecieveCorrection(const std_msgs::UInt8MultiArray& msg); // 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); + // Build the log file name + std::string buildUpLogFilename(const std::string& typeSuffix, const std::string& extension = ".txt"); + // Used to publish the Correction file through 'ros_send_correction_topic' + void SendCorrection(const u8Array_t& msg); + // Used to publish the Packet loss condition + void SendPacketLoss(const uint32_t& total_packet_count, const uint32_t& total_packet_loss_count); + // Used to publish the Packet loss condition + void SendPTP(const uint8_t& ptp_lock_offset, const u8Array_t& ptp_status); + // Used to publish the firetime correction + void SendFiretime(const double *firetime_correction_); + // Convert ptp lock offset, status into ROS message + hesai_ros_driver::Ptp ToRosMsg(const uint8_t& ptp_lock_offset, const u8Array_t& ptp_status); + // Convert packet loss condition into ROS message + hesai_ros_driver::LossPacket ToRosMsg(const uint32_t& total_packet_count, const uint32_t& total_packet_loss_count); + // Convert correction string into ROS messages + std_msgs::UInt8MultiArray ToRosMsg(const u8Array_t& correction_string); // 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 + // Convert double[512] to float64[512] + hesai_ros_driver::Firetime ToRosMsg(const double *firetime_correction_); // publish point std::shared_ptr nh_; ros::Publisher pub_; @@ -82,67 +106,63 @@ class SourceDriver ros::Subscriber pkt_sub_; //spin thread while recieve data from ROS topic boost::thread* subscription_spin_thread_; + + ros::Publisher crt_pub_; + ros::Publisher firetime_pub_; + ros::Publisher loss_pub_; + ros::Publisher ptp_pub_; + ros::Subscriber crt_sub_; }; 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, ""); + nh_ = std::unique_ptr(new ros::NodeHandle()); - // 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); + DriverParam driver_param; + DriveYamlParam yaml_param; + yaml_param.GetDriveYamlParam(config, driver_param); + frame_id_ = driver_param.input_param.frame_id; + + save_replayed_topics_to_rosbag_ = driver_param.input_param.save_replayed_topics_to_rosbag; + + if (driver_param.input_param.send_point_cloud_ros) { + pub_ = nh_->advertise(driver_param.input_param.ros_send_point_topic, 10); + } - 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"); + if (driver_param.input_param.ros_send_packet_loss_topic != NULL_TOPIC) { + loss_pub_ = nh_->advertise(driver_param.input_param.ros_send_packet_loss_topic, 10); + } - 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 (driver_param.input_param.source_type == DATA_FROM_LIDAR) { + if (driver_param.input_param.ros_send_ptp_topic != NULL_TOPIC) { + ptp_pub_ = nh_->advertise(driver_param.input_param.ros_send_ptp_topic, 10); + } + + if (driver_param.input_param.ros_send_correction_topic != NULL_TOPIC) { + crt_pub_ = nh_->advertise(driver_param.input_param.ros_send_correction_topic, 10); + } + } + if (! driver_param.input_param.firetimes_path.empty() ) { + if (driver_param.input_param.ros_send_firetime_topic != NULL_TOPIC) { + firetime_pub_ = nh_->advertise(driver_param.input_param.ros_send_firetime_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.send_packet_ros && driver_param.input_param.source_type != DATA_FROM_ROS_PACKET) { + pkt_pub_ = nh_->advertise(driver_param.input_param.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); + pkt_sub_ = nh_->subscribe(driver_param.input_param.ros_recv_packet_topic, 10000, &SourceDriver::RecievePacket, this); + + if (driver_param.input_param.ros_recv_correction_topic != NULL_TOPIC) { + crt_sub_ = nh_->subscribe(driver_param.input_param.ros_recv_correction_topic, 10, &SourceDriver::RecieveCorrection, 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; @@ -151,14 +171,53 @@ inline void SourceDriver::Init(const YAML::Node& config) 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){ + if(driver_param.input_param.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_param.input_param.ros_send_packet_loss_topic != NULL_TOPIC) { + driver_ptr_->RegRecvCallback(std::bind(&SourceDriver::SendPacketLoss, this, std::placeholders::_1, std::placeholders::_2)); + } + if (driver_param.input_param.source_type == DATA_FROM_LIDAR) { + if (driver_param.input_param.ros_send_correction_topic != NULL_TOPIC) { + driver_ptr_->RegRecvCallback(std::bind(&SourceDriver::SendCorrection, this, std::placeholders::_1)); + } + if (driver_param.input_param.ros_send_ptp_topic != NULL_TOPIC) { + driver_ptr_->RegRecvCallback(std::bind(&SourceDriver::SendPTP, this, std::placeholders::_1, std::placeholders::_2)); + } } if (!driver_ptr_->Init(driver_param)) { std::cout << "Driver Initialize Error...." << std::endl; exit(-1); } + // We use sim time true so should be okay. + latestCloudStamp_ = ros::Time::now(); + // print ros time now + std::cout << "ros time now:" << latestCloudStamp_ << std::endl; + + if (save_replayed_topics_to_rosbag_) + { + std::string outBagPath_ =""; + // Get the directory of the ros package + // std::string packagePath = ros::package::getPath("hesai_ros_driver"); + + // Generate the log file name + std::string outBagDirectory_ = ros::package::getPath("hesai_ros_driver") + "/data/"; + std::string outBagName_ = "hesaiXT32_" + std::to_string(ros::Time::now().toSec()) + ".bag"; + + if (driver_param.input_param.output_rosbag_directory != "") { + outBagPath_ = driver_param.input_param.output_rosbag_directory + outBagName_; + }else{ + outBagPath_ = outBagDirectory_ + outBagName_; + } + + // Remove the old bag file + std::remove(outBagPath_.c_str()); + + // Open the new bag file + outputBag.open(outBagPath_, rosbag::bagmode::Write); + } + } inline void SourceDriver::Start() @@ -168,11 +227,13 @@ inline void SourceDriver::Start() inline SourceDriver::~SourceDriver() { + std::cout << "Stoping...." << std::endl; Stop(); } inline void SourceDriver::Stop() { + outputBag.close(); driver_ptr_->Stop(); } @@ -186,14 +247,45 @@ inline void SourceDriver::SendPointCloud(const LidarDecodedFrame& frame, const std::string& frame_id) { sensor_msgs::PointCloud2 ros_msg; + + uint32_t frameSize = frame.points_num; + + if (save_replayed_topics_to_rosbag_) + { + if (frameSize > 64000) + { + frameSize = 64000; + } + } + int fields = 6; ros_msg.fields.clear(); ros_msg.fields.reserve(fields); - ros_msg.width = frame.points_num; + ros_msg.width = frameSize; ros_msg.height = 1; int offset = 0; @@ -207,7 +299,7 @@ inline sensor_msgs::PointCloud2 SourceDriver::ToRosMsg(const LidarDecodedFrame iter_x_(ros_msg, "x"); sensor_msgs::PointCloud2Iterator iter_y_(ros_msg, "y"); @@ -215,7 +307,7 @@ inline sensor_msgs::PointCloud2 SourceDriver::ToRosMsg(const LidarDecodedFrame 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++) + for (size_t i = 0; i < frameSize; i++) { LidarPointXYZIRT point = frame.points[i]; *iter_x_ = point.x; @@ -233,14 +325,21 @@ inline sensor_msgs::PointCloud2 SourceDriver::ToRosMsg(const LidarDecodedFramelidar_ptr_->origin_packets_buffer_.emplace_back(&msg.packets[i].data[0], msg.packets[i].size); } -} + // printf("Receiving Buffer Size:%lu\n",driver_ptr_->lidar_ptr_->origin_packets_buffer_.size()); +} +inline void SourceDriver::RecieveCorrection(const std_msgs::UInt8MultiArray& msg) +{ + driver_ptr_->lidar_ptr_->correction_string_.resize(msg.data.size()); + std::copy(msg.data.begin(), msg.data.end(), driver_ptr_->lidar_ptr_->correction_string_.begin()); + while (1) { + if (! driver_ptr_->lidar_ptr_->LoadCorrectionFromROSbag()) { + break; + } + } +} diff --git a/src/manager/source_driver_ros2.hpp b/src/manager/source_driver_ros2.hpp index 43324f9..18072e1 100644 --- a/src/manager/source_driver_ros2.hpp +++ b/src/manager/source_driver_ros2.hpp @@ -31,19 +31,22 @@ #pragma once #include #include +#include #include #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 +#include +#include "source_drive_common.hpp" + class SourceDriver { public: @@ -58,86 +61,99 @@ class SourceDriver SourceDriver(SourceType src_type) {}; void SpinRos2(){rclcpp::spin(this->node_ptr_);} std::shared_ptr node_ptr_; + #ifdef __CUDACC__ + std::shared_ptr> driver_ptr_; + #else + std::shared_ptr> driver_ptr_; + #endif protected: + // Save Correction file subscribed by "ros_recv_correction_topic" + void RecieveCorrection(const std_msgs::msg::UInt8MultiArray::SharedPtr msg); // 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); + + // Used to publish the Correction file through 'ros_send_correction_topic' + void SendCorrection(const u8Array_t& msg); + // Used to publish the Packet loss condition + void SendPacketLoss(const uint32_t& total_packet_count, const uint32_t& total_packet_loss_count); + // Used to publish the Packet loss condition + void SendPTP(const uint8_t& ptp_lock_offset, const u8Array_t& ptp_status); + // Used to publish the firetime correction + void SendFiretime(const double *firetime_correction_); + + // Convert ptp lock offset, status into ROS message + hesai_ros_driver::msg::Ptp ToRosMsg(const uint8_t& ptp_lock_offset, const u8Array_t& ptp_status); + // Convert packet loss condition into ROS message + hesai_ros_driver::msg::LossPacket ToRosMsg(const uint32_t& total_packet_count, const uint32_t& total_packet_loss_count); + // Convert correction string into ROS messages + std_msgs::msg::UInt8MultiArray ToRosMsg(const u8Array_t& correction_string); + // Convert double[512] to float64[512] + hesai_ros_driver::msg::Firetime ToRosMsg(const double *firetime_correction_); // 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 crt_sub_; rclcpp::Subscription::SharedPtr pkt_sub_; rclcpp::Publisher::SharedPtr pkt_pub_; rclcpp::Publisher::SharedPtr pub_; + rclcpp::Publisher::SharedPtr firetime_pub_; + rclcpp::Publisher::SharedPtr crt_pub_; + rclcpp::Publisher::SharedPtr loss_pub_; + rclcpp::Publisher::SharedPtr ptp_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"); + DriveYamlParam yaml_param; + yaml_param.GetDriveYamlParam(config, driver_param); + frame_id_ = driver_param.input_param.frame_id; 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 (driver_param.input_param.send_point_cloud_ros) { + pub_ = node_ptr_->create_publisher(driver_param.input_param.ros_send_point_topic, 100); + } + + + if (driver_param.input_param.ros_send_packet_loss_topic != NULL_TOPIC) { + loss_pub_ = node_ptr_->create_publisher(driver_param.input_param.ros_send_packet_loss_topic, 10); + } + + if (driver_param.input_param.source_type == DATA_FROM_LIDAR) { + if (driver_param.input_param.ros_send_ptp_topic != NULL_TOPIC) { + ptp_pub_ = node_ptr_->create_publisher(driver_param.input_param.ros_send_ptp_topic, 10); + } + + if (driver_param.input_param.ros_send_correction_topic != NULL_TOPIC) { + crt_pub_ = node_ptr_->create_publisher(driver_param.input_param.ros_send_correction_topic, 10); + } + } + if (! driver_param.input_param.firetimes_path.empty() ) { + if (driver_param.input_param.ros_send_firetime_topic != NULL_TOPIC) { + firetime_pub_ = node_ptr_->create_publisher(driver_param.input_param.ros_send_firetime_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_ = node_ptr_->create_publisher(ros_send_packet_topic, 10); + if (driver_param.input_param.send_packet_ros && driver_param.input_param.source_type != DATA_FROM_ROS_PACKET) { + pkt_pub_ = node_ptr_->create_publisher(driver_param.input_param.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)); + pkt_sub_ = node_ptr_->create_subscription(driver_param.input_param.ros_recv_packet_topic, 10, + std::bind(&SourceDriver::RecievePacket, this, std::placeholders::_1)); + if (driver_param.input_param.ros_recv_correction_topic != NULL_TOPIC) { + crt_sub_ = node_ptr_->create_subscription(driver_param.input_param.ros_recv_correction_topic, 10, + std::bind(&SourceDriver::RecieveCorrection, this, std::placeholders::_1)); + } driver_param.decoder_param.enable_udp_thread = false; subscription_spin_thread_ = new boost::thread(boost::bind(&SourceDriver::SpinRos2,this)); } @@ -147,14 +163,25 @@ inline void SourceDriver::Init(const YAML::Node& config) #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){ + if(driver_param.input_param.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_param.input_param.ros_send_packet_loss_topic != NULL_TOPIC) { + driver_ptr_->RegRecvCallback(std::bind(&SourceDriver::SendPacketLoss, this, std::placeholders::_1, std::placeholders::_2)); +} + if (driver_param.input_param.source_type == DATA_FROM_LIDAR) { +if (driver_param.input_param.ros_send_correction_topic != NULL_TOPIC) { + driver_ptr_->RegRecvCallback(std::bind(&SourceDriver::SendCorrection, this, std::placeholders::_1)); +} + if (driver_param.input_param.ros_send_ptp_topic != NULL_TOPIC) { + driver_ptr_->RegRecvCallback(std::bind(&SourceDriver::SendPTP, this, std::placeholders::_1, std::placeholders::_2)); +} + } if (!driver_ptr_->Init(driver_param)) { + std::cout << "Driver Initialize Error...." << std::endl; exit(-1); } } @@ -184,6 +211,26 @@ inline void SourceDriver::SendPointCloud(const LidarDecodedFramepublish(ToRosMsg(msg, frame_id_)); } +inline void SourceDriver::SendCorrection(const u8Array_t& msg) +{ + crt_pub_->publish(ToRosMsg(msg)); +} + +inline void SourceDriver::SendPacketLoss(const uint32_t& total_packet_count, const uint32_t& total_packet_loss_count) +{ + loss_pub_->publish(ToRosMsg(total_packet_count, total_packet_loss_count)); +} + +inline void SourceDriver::SendPTP(const uint8_t& ptp_lock_offset, const u8Array_t& ptp_status) +{ + ptp_pub_->publish(ToRosMsg(ptp_lock_offset, ptp_status)); +} + +inline void SourceDriver::SendFiretime(const double *firetime_correction_) +{ + firetime_pub_->publish(ToRosMsg(firetime_correction_)); +} + inline sensor_msgs::msg::PointCloud2 SourceDriver::ToRosMsg(const LidarDecodedFrame& frame, const std::string& frame_id) { sensor_msgs::msg::PointCloud2 ros_msg; @@ -202,7 +249,6 @@ inline sensor_msgs::msg::PointCloud2 SourceDriver::ToRosMsg(const LidarDecodedFr 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; @@ -214,7 +260,6 @@ inline sensor_msgs::msg::PointCloud2 SourceDriver::ToRosMsg(const LidarDecodedFr 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]; @@ -231,8 +276,9 @@ inline sensor_msgs::msg::PointCloud2 SourceDriver::ToRosMsg(const LidarDecodedFr ++iter_ring_; ++iter_timestamp_; } + // printf("HesaiLidar Runing Status [standby mode:%u] | [speed:%u]\n", frame.work_mode, frame.spin_speed); 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) ; - + std::cout.flush(); 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_; @@ -241,7 +287,7 @@ inline sensor_msgs::msg::PointCloud2 SourceDriver::ToRosMsg(const LidarDecodedFr 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++) { + for (size_t 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); @@ -254,11 +300,49 @@ inline hesai_ros_driver::msg::UdpFrame SourceDriver::ToRosMsg(const UdpFrame_t& return rs_msg; } +inline std_msgs::msg::UInt8MultiArray SourceDriver::ToRosMsg(const u8Array_t& correction_string) { + auto msg = std::make_shared(); + msg->data.resize(correction_string.size()); + std::copy(correction_string.begin(), correction_string.end(), msg->data.begin()); + return *msg; +} + +inline hesai_ros_driver::msg::LossPacket SourceDriver::ToRosMsg(const uint32_t& total_packet_count, const uint32_t& total_packet_loss_count) +{ + hesai_ros_driver::msg::LossPacket msg; + msg.total_packet_count = total_packet_count; + msg.total_packet_loss_count = total_packet_loss_count; + return msg; +} + +inline hesai_ros_driver::msg::Ptp SourceDriver::ToRosMsg(const uint8_t& ptp_lock_offset, const u8Array_t& ptp_status) +{ + hesai_ros_driver::msg::Ptp msg; + msg.ptp_lock_offset = ptp_lock_offset; + std::copy(ptp_status.begin(), ptp_status.begin() + std::min(16ul, ptp_status.size()), msg.ptp_status.begin()); + return msg; +} + +inline hesai_ros_driver::msg::Firetime SourceDriver::ToRosMsg(const double *firetime_correction_) +{ + hesai_ros_driver::msg::Firetime msg; + std::copy(firetime_correction_, firetime_correction_ + 512, msg.data.begin()); + return msg; +} inline void SourceDriver::RecievePacket(const hesai_ros_driver::msg::UdpFrame::SharedPtr msg) { - for (int i = 0; i < msg->packets.size(); i++) { + for (size_t 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); } } - +inline void SourceDriver::RecieveCorrection(const std_msgs::msg::UInt8MultiArray::SharedPtr msg) +{ + driver_ptr_->lidar_ptr_->correction_string_.resize(msg->data.size()); + std::copy(msg->data.begin(), msg->data.end(), driver_ptr_->lidar_ptr_->correction_string_.begin()); + while (1) { + if (! driver_ptr_->lidar_ptr_->LoadCorrectionFromROSbag()) { + break; + } + } +}