EmbodiedAI
/
Go2Py
mirror of https://github.com/machines-in-motion/Go2Py.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

LiDAR node and sensor launch files added

This commit is contained in:
Rooholla-KhorramBakht 2024-02-04 08:18:52 +08:00
parent 9bb096405d
commit 8a972dd52d
170 changed files with 24172 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
Dockerfile
View File

@ -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='<CycloneDDS><Domain><General><Interfaces> <NetworkInterface name="eth0" priority="default" multicast="default" /> </Interfaces></General></Domain></CycloneDDS>'" >> /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"]

2
Makefile Normal file
View File

@ -0,0 +1,2 @@
docker:
@docker build --no-cache --tag go2py:latest .

48
README.md
View File

@ -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 <host computer IP address>
```
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.

84
deploy/launch/hw.launch.py Normal file
View File

@ -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'])
# ),
])

267
deploy/ros2_ws/src/lidar_node/CMakeLists.txt Normal file
View File

@ -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")

161
deploy/ros2_ws/src/lidar_node/README.md Normal file
View File

@ -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: "<Your PCAP file path>" #The path of pcap file (set during offline playback)
correction_file_path: "<Your correction file path>" #LiDAR angle file, required for offline playback of pcap/packet rosbag
firetimes_path: "<Your firetime file 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: "<The PCAP file path>"
correction_file_path: "<The correction file path>"
firetimes_path: "<Your firetime file 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: "<The PCAP file path>"
correction_file_path: "<The correction file path>"
firetimes_path: "<Your firetime file 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

21
deploy/ros2_ws/src/lidar_node/Version.h.in Normal file
View File

@ -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@

26
deploy/ros2_ws/src/lidar_node/config/config.yaml Normal file
View File

@ -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

21
deploy/ros2_ws/src/lidar_node/launch/dashing_start.py Executable file
View File

@ -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]
)
])

6
deploy/ros2_ws/src/lidar_node/launch/start.launch Executable file
View File

@ -0,0 +1,6 @@
<launch>
<node pkg="hesai_ros_driver" name="hesai_ros_driver_node" type="hesai_ros_driver_node" output="screen">
</node>
<!-- rviz -->
<node pkg="rviz" name="rviz" type="rviz" args="-d '$(find hesai_ros_driver)/rviz/rviz.rviz'" required="true"/>
</launch>

12
deploy/ros2_ws/src/lidar_node/launch/start.py Executable file
View File

@ -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])
])

2
deploy/ros2_ws/src/lidar_node/msg/UdpFrame.msg Normal file
View File

@ -0,0 +1,2 @@
Header header
UdpPacket[] packets

15
deploy/ros2_ws/src/lidar_node/msg/UdpPacket.msg Normal file
View File

@ -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

2
deploy/ros2_ws/src/lidar_node/msg/msg_ros2/UdpFrame.msg Normal file
View File

@ -0,0 +1,2 @@
std_msgs/Header header
UdpPacket[] packets

15
deploy/ros2_ws/src/lidar_node/msg/msg_ros2/UdpPacket.msg Normal file
View File

@ -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

108
deploy/ros2_ws/src/lidar_node/node/hesai_ros_driver_node.cc Normal file
View File

@ -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 <zhangyu@hesaitech.com>
* Description: Hesai sdk node for CPU
* Created on June 12, 2023, 10:46 AM
*/
#include "manager/node_manager.h"
#include <signal.h>
#include <iostream>
#include "Version.h"
#ifdef ROS_FOUND
#include <ros/ros.h>
#include <ros/package.h>
#elif ROS2_FOUND
#include <rclcpp/rclcpp.hpp>
#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<NodeManager> demo_ptr = std::make_shared<NodeManager>();
demo_ptr->Init(config);
demo_ptr->Start();
#ifdef ROS_FOUND
ros::MultiThreadedSpinner spinner(2);
spinner.spin();
#elif ROS2_FOUND
std::unique_lock<std::mutex> lck(g_mtx);
g_cv.wait(lck);
#endif
return 0;
}

108
deploy/ros2_ws/src/lidar_node/node/hesai_ros_driver_node.cu Normal file
View File

@ -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 <zhangyu@hesaitech.com>
* Description: Hesai sdk node for GPU
* Created on June 12, 2023, 10:46 AM
*/
#include "manager/node_manager.h"
#include <signal.h>
#include <iostream>
#include "Version.h"
#ifdef ROS_FOUND
#include <ros/ros.h>
#include <ros/package.h>
#elif ROS2_FOUND
#include <rclcpp/rclcpp.hpp>
#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<NodeManager> demo_ptr = std::make_shared<NodeManager>();
demo_ptr->Init(config);
demo_ptr->Start();
#ifdef ROS_FOUND
ros::MultiThreadedSpinner spinner(2);
spinner.spin();
#elif ROS2_FOUND
std::unique_lock<std::mutex> lck(g_mtx);
g_cv.wait(lck);
#endif
return 0;
}

41
deploy/ros2_ws/src/lidar_node/package.xml Normal file
View File

@ -0,0 +1,41 @@
<?xml version="1.0"?>
<package format="3">
<name>hesai_ros_driver</name>
<version>1.5.0</version>
<description>The hesai_ros_driver_node package</description>
<maintainer email="zhangyu@hesaitech.com">hesaiwuxiaozhou</maintainer>
<license>BSD</license>
<buildtool_depend condition="$ROS_VERSION == 1">catkin</buildtool_depend>
<buildtool_depend condition="$ROS_VERSION == 2">ament_cmake</buildtool_depend>
<buildtool_depend condition="$ROS_VERSION == 2">rosidl_default_generators</buildtool_depend>
<build_depend condition="$ROS_VERSION == 1">roscpp</build_depend>
<build_depend condition="$ROS_VERSION == 2">rclcpp</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>roslib</build_depend>
<build_depend>std_msgs</build_depend>
<exec_depend condition="$ROS_VERSION == 2">rclcpp</exec_depend>
<exec_depend condition="$ROS_VERSION == 2">sensor_msgs</exec_depend>
<exec_depend condition="$ROS_VERSION == 2">std_msgs</exec_depend>
<depend condition="$ROS_VERSION == 2">ament_index_cpp</depend>
<depend condition="$ROS_VERSION == 2">rclcpp_action</depend>
<exec_depend condition="$ROS_VERSION == 1">roscpp</exec_depend>
<exec_depend condition="$ROS_VERSION == 1">sensor_msgs</exec_depend>
<exec_depend condition="$ROS_VERSION == 1">roslib</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
<export>
<build_type condition="$ROS_VERSION == 1">catkin</build_type>
<build_type condition="$ROS_VERSION == 2">ament_cmake</build_type>
</export>
</package>

346
deploy/ros2_ws/src/lidar_node/rviz/rviz.rviz Executable file
View File

@ -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: <Fixed 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: <Fixed 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

159
deploy/ros2_ws/src/lidar_node/rviz/rviz2.rviz Executable file
View File

@ -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: <Fixed 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: <Fixed 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

117
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/CMakeLists.txt Normal file
View File

@ -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})

14
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/LICENSE Normal file
View File

@ -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.

73
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/README.md Normal file
View File

@ -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
```

21
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/Version.h.in Normal file
View File

@ -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@

23
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/change notes.md Normal file
View File

@ -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

BIN
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/AT128E2X_Angle Correction File.dat Normal file
View File

Binary file not shown.

BIN
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/FT120C1X_Angle Correction File.dat Normal file
View File

Binary file not shown.

129
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/OT128_Angle Correction File.csv Normal file
View File

@ -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
1 Channel Elevation Azimuth
2 1 14.985 0.186
3 2 13.283 0.185
4 3 11.758 1.335
5 4 10.483 1.343
6 5 9.836 0.148
7 6 9.171 0.147
8 7 8.496 0.146
9 8 7.812 0.146
10 9 7.462 1.335
11 10 7.115 1.336
12 11 6.767 1.337
13 12 6.416 1.338
14 13 6.064 1.339
15 14 5.71 1.34
16 15 5.355 1.341
17 16 4.998 1.342
18 17 4.643 0.128
19 18 4.282 0.128
20 19 3.921 0.127
21 20 3.558 0.127
22 21 3.194 0.107
23 22 2.829 0.106
24 23 2.463 0.105
25 24 2.095 0.105
26 25 1.974 -3.118
27 26 1.854 1.315
28 27 1.729 4.529
29 28 1.609 -3.121
30 29 1.487 1.316
31 30 1.362 4.532
32 31 1.242 -3.124
33 32 1.12 1.317
34 33 0.995 4.536
35 34 0.875 -3.127
36 35 0.75 1.317
37 36 0.625 4.539
38 37 0.5 -3.13
39 38 0.375 1.318
40 39 0.25 4.542
41 40 0.125 -3.133
42 41 0 0.103
43 42 -0.125 2.935
44 43 -0.25 -1.517
45 44 -0.375 0.103
46 45 -0.5 2.937
47 46 -0.626 -1.519
48 47 -0.751 0.103
49 48 -0.876 2.939
50 49 -1.001 -1.52
51 50 -1.126 0.103
52 51 -1.251 2.941
53 52 -1.377 -1.521
54 53 -1.502 0.102
55 54 -1.627 2.943
56 55 -1.751 -1.523
57 56 -1.876 0.102
58 57 -2.001 2.945
59 58 -2.126 -1.524
60 59 -2.251 0.102
61 60 -2.376 2.946
62 61 -2.501 -1.526
63 62 -2.626 0.102
64 63 -2.751 2.948
65 64 -2.876 -1.526
66 65 -3.001 1.324
67 66 -3.126 4.57
68 67 -3.251 -3.155
69 68 -3.376 1.325
70 69 -3.501 4.573
71 70 -3.626 -3.157
72 71 -3.751 1.326
73 72 -3.876 4.575
74 73 -4.001 -3.159
75 74 -4.126 1.326
76 75 -4.25 4.578
77 76 -4.375 -3.161
78 77 -4.501 1.327
79 78 -4.626 4.581
80 79 -4.751 -3.163
81 80 -4.876 1.328
82 81 -5.001 4.583
83 82 -5.126 -3.165
84 83 -5.252 1.329
85 84 -5.377 4.586
86 85 -5.502 -3.167
87 86 -5.626 1.329
88 87 -5.752 4.588
89 88 -5.877 -3.168
90 89 -6.002 0.102
91 90 -6.378 0.103
92 91 -6.754 0.103
93 92 -7.13 0.103
94 93 -7.507 0.104
95 94 -7.882 0.104
96 95 -8.257 0.104
97 96 -8.632 0.104
98 97 -9.003 1.337
99 98 -9.376 1.337
100 99 -9.749 1.338
101 100 -10.121 1.339
102 101 -10.493 1.34
103 102 -10.864 1.341
104 103 -11.234 1.341
105 104 -11.603 1.342
106 105 -11.975 0.108
107 106 -12.343 0.108
108 107 -12.709 0.109
109 108 -13.075 0.109
110 109 -13.439 0.13
111 110 -13.803 0.131
112 111 -14.164 0.131
113 112 -14.525 0.132
114 113 -14.879 1.384
115 114 -15.237 1.384
116 115 -15.593 1.385
117 116 -15.948 1.385
118 117 -16.299 1.386
119 118 -16.651 1.386
120 119 -17 1.387
121 120 -17.347 1.387
122 121 -17.701 0.151
123 122 -18.386 0.153
124 123 -19.063 0.154
125 124 -19.73 0.156
126 125 -20.376 1.388
127 126 -21.653 1.408
128 127 -23.044 0.196
129 128 -24.765 0.286

129
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/Pandar128E3X_Angle Correction File.csv Normal file
View File

@ -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
1 Channel Elevation Azimuth
2 1 14.436 3.257
3 2 13.535 3.263
4 3 13.081786 1.091414
5 4 12.624 3.268
6 5 12.165246 1.092504
7 6 11.702 3.273
8 7 11.238522 1.093621
9 8 10.771 3.278
10 9 10.305007 1.094766
11 10 9.83 3.283
12 11 9.356123 1.095941
13 12 8.88 3.288
14 13 8.401321 1.097146
15 14 7.921 3.291
16 15 7.43808 1.098384
17 16 6.952581 -1.101114
18 17 6.466905 1.099655
19 18 5.977753 -1.103649
20 19 5.487 -3.306
21 20 4.995801 -1.106118
22 21 4.501 -3.311
23 22 4.007293 -1.108519
24 23 3.509 -3.318
25 24 3.012822 -1.110852
26 25 2.512 -3.324
27 26 2.013 -1.113115
28 27 1.885 7.72
29 28 1.761 5.535
30 29 1.637 3.325
31 30 1.511 -3.33
32 31 1.385875 1.10673
33 32 1.2582 -5.538034933
34 33 1.13 -7.726107248
35 34 1.008459 -1.115309
36 35 0.88 7.731
37 36 0.756 5.543
38 37 0.63 3.329
39 38 0.505 -3.336
40 39 0.378591 1.108227
41 40 0.251 -5.546813133
42 41 0.124 -7.738214933
43 42 -0.00015 -1.117431
44 43 -0.129 7.743
45 44 -0.2541 5.550783
46 45 -0.38 3.335
47 46 -0.5061259 -3.341759
48 47 -0.63235 1.109762
49 48 -0.7597898 -5.555361
50 49 -0.8872418 -7.750039
51 50 -1.012168 -1.119482
52 51 -1.141 7.757
53 52 -1.2662 5.559905
54 53 -1.393 3.34
55 54 -1.519337 -3.347338
56 55 -1.646275 1.111338
57 56 -1.773301 -5.564415
58 57 -1.900587 -7.762486
59 58 -2.026912 -1.12146
60 59 -2.155 7.768
61 60 -2.2815 5.568891
62 61 -2.409 3.345
63 62 -2.534932 -3.352812
64 63 -2.662501 1.112953
65 64 -2.789024 -5.573332
66 65 -2.916055 -7.774765
67 66 -3.043698 -1.123366
68 67 -3.172 7.78
69 68 -3.299 5.577739
70 69 -3.425 3.351
71 70 -3.552222 -3.358181
72 71 -3.680335 1.114609
73 72 -3.806265 -5.582111
74 73 -3.932954 -7.786874
75 74 -4.06183 -1.125199
76 75 -4.19 7.792
77 76 -4.318 5.586449
78 77 -4.444 3.356
79 78 -4.570508 -3.363443
80 79 -4.699079 1.116305
81 80 -4.824327 -5.590751
82 81 -4.950584 -7.798811
83 82 -5.080608 -1.126958
84 83 -5.209 7.804
85 84 -5.336 5.595019
86 85 -5.463 3.36
87 86 -5.589088 -3.368599
88 87 -5.718031 1.118042
89 88 -5.842508 -5.599251
90 89 -5.968246 -7.810576
91 90 -6.099661 -1.128643
92 91 -6.607262 -3.373647
93 92 -7.117295 -1.130255
94 93 -7.624327 -3.378587
95 94 -8.133802 -1.131792
96 95 -8.639587 -3.383419
97 96 -9.149 3.381
98 97 -9.652353 -3.388143
99 98 -10.16 3.386
100 99 -10.665443 1.127077
101 100 -11.17 3.39
102 101 -11.671568 1.129045
103 102 -12.174 3.395
104 103 -12.673194 1.131048
105 104 -13.173 3.401
106 105 -13.669682 1.133088
107 106 -14.166 3.406
108 107 -14.660411 1.135163
109 108 -15.154 3.41
110 109 -15.644783 1.137272
111 110 -16.135 3.416
112 111 -16.622221 1.139412
113 112 -17.106088 -1.142319
114 113 -17.592171 1.141584
115 114 -18.071976 -1.143128
116 115 -18.54765 -3.425708
117 116 -19.029597 -1.143867
118 117 -19.50071 -3.429329
119 118 -19.978461 -1.144538
120 119 -20.44479 -3.432841
121 120 -20.918108 -1.14514
122 121 -21.37943 -3.436242
123 122 -21.848107 -1.145675
124 123 -22.30422 -3.439533
125 124 -22.768055 -1.146145
126 125 -23.21878 -3.442714
127 126 -23.677577 -1.146549
128 127 -24.12274 -3.445786
129 128 -25.01577 -3.448749

41
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/Pandar40M_Angle Correction File.csv Normal file
View File

@ -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
1 Laser id Elevation Azimuth
2 1 14.881 -1.042
3 2 11.031 -1.042
4 3 8.058 -1.042
5 4 5.056 -1.042
6 5 3.039 -1.042
7 6 2.027 -1.042
8 7 1.687 3.125
9 8 1.35 -5.208
10 9 1.012 -1.042
11 10 0.674 3.125
12 11 0.336 -5.208
13 12 -0.001 -1.042
14 13 -0.338 3.125
15 14 -0.676 -5.208
16 15 -1.014 -1.042
17 16 -1.352 3.125
18 17 -1.689 -5.208
19 18 -2.029 -1.042
20 19 -2.366 3.125
21 20 -2.701 -5.208
22 21 -3.041 -1.042
23 22 -3.376 3.125
24 23 -3.713 -5.208
25 24 -4.051 -1.042
26 25 -4.386 3.125
27 26 -4.721 -5.208
28 27 -5.058 -1.042
29 28 -5.392 3.125
30 29 -5.727 -5.208
31 30 -6.062 -1.042
32 31 -7.064 -1.042
33 32 -8.06 -1.042
34 33 -9.061 -1.042
35 34 -10.046 -1.042
36 35 -11.033 -1.042
37 36 -12.007 -1.042
38 37 -12.975 -1.042
39 38 -13.931 -1.042
40 39 -18.89 -1.042
41 40 -24.898 -1.042

41
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/Pandar40P_Angle Correction File.csv Normal file
View File

@ -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
1 Channel Elevation Azimuth
2 1 15.21 -1.042
3 2 11.36 -1.042
4 3 8.387 -1.042
5 4 5.385 -1.042
6 5 3.368 -1.042
7 6 2.356 -1.042
8 7 2.016 3.125
9 8 1.679 -5.208
10 9 1.341 -1.042
11 10 1.003 3.125
12 11 0.665 -5.208
13 12 0.328 -1.042
14 13 -0.009 3.125
15 14 -0.347 -5.208
16 15 -0.685 -1.042
17 16 -1.023 3.125
18 17 -1.36 -5.208
19 18 -1.7 -1.042
20 19 -2.037 3.125
21 20 -2.372 -5.208
22 21 -2.712 -1.042
23 22 -3.047 3.125
24 23 -3.384 -5.208
25 24 -3.722 -1.042
26 25 -4.057 3.125
27 26 -4.392 -5.208
28 27 -4.729 -1.042
29 28 -5.063 3.125
30 29 -5.398 -5.208
31 30 -5.733 -1.042
32 31 -6.735 -1.042
33 32 -7.731 -1.042
34 33 -8.732 -1.042
35 34 -9.557 -1.042
36 35 -10.704 -1.042
37 36 -11.678 -1.042
38 37 -12.646 -1.042
39 38 -13.602 -1.042
40 39 -18.561 -1.042
41 40 -24.569 -1.042

65
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/Pandar64_Angle Correction File.csv Normal file
View File

@ -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
1 Channel Elevation Azimuth
2 1 14.708 -1.042
3 2 10.858 -1.042
4 3 7.885 -1.042
5 4 4.883 -1.042
6 5 2.866 -1.042
7 6 1.854 -1.042
8 7 1.686 1.042
9 8 1.514 3.125
10 9 1.348 5.208
11 10 1.177 -5.208
12 11 1.01 -3.125
13 12 0.839 -1.042
14 13 0.672 1.042
15 14 0.501 3.125
16 15 0.334 5.208
17 16 0.163 -5.208
18 17 -0.005 -3.125
19 18 -0.174 -1.042
20 19 -0.343 1.042
21 20 -0.511 3.125
22 21 -0.682 5.208
23 22 -0.849 -5.208
24 23 -1.019 -3.125
25 24 -1.187 -1.042
26 25 -1.358 1.042
27 26 -1.525 3.125
28 27 -1.696 5.208
29 28 -1.862 -5.208
30 29 -2.034 -3.125
31 30 -2.202 -1.042
32 31 -2.372 1.042
33 32 -2.539 3.125
34 33 -2.71 5.208
35 34 -2.874 -5.208
36 35 -3.047 -3.125
37 36 -3.214 -1.042
38 37 -3.384 1.042
39 38 -3.549 3.125
40 39 -3.722 5.208
41 40 -3.886 -5.208
42 41 -4.058 -3.125
43 42 -4.224 -1.042
44 43 -4.395 1.042
45 44 -4.559 3.125
46 45 -4.732 5.208
47 46 -4.894 -5.208
48 47 -5.066 -3.125
49 48 -5.231 -1.042
50 49 -5.403 1.042
51 50 -5.565 3.125
52 51 -5.739 5.208
53 52 -5.9 -5.208
54 53 -6.072 -3.125
55 54 -6.235 -1.042
56 55 -7.237 -1.042
57 56 -8.233 -1.042
58 57 -9.234 -1.042
59 58 -10.059 -1.042
60 59 -11.206 -1.042
61 60 -12.18 -1.042
62 61 -13.148 -1.042
63 62 -14.104 -1.042
64 63 -19.063 -1.042
65 64 -25.071 -1.042

91
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/Pandar90E3X_Angle Correction File.csv Normal file
View File

@ -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
1 Channel Elevation Azimuth
2 1 14.436 3.257
3 2 13.535 3.263
4 3 11.702 3.273
5 4 9.83 3.283
6 5 8.401 1.097
7 6 6.953 -1.101
8 7 5.487 -3.306
9 8 4.007 -1.109
10 9 3.013 -1.111
11 10 2.512 -3.324
12 11 2.013 -1.113
13 12 1.637 3.325
14 13 1.258 -5.538
15 14 1.008 -1.115
16 15 0.756 5.543
17 16 0.63 3.329
18 17 0.505 -3.336
19 18 0.379 1.108
20 19 0.251 -5.547
21 20 0.124 -7.738
22 21 0 -1.117
23 22 -0.129 7.743
24 23 -0.254 5.551
25 24 -0.38 3.335
26 25 -0.506 -3.342
27 26 -0.632 1.11
28 27 -0.76 -5.555
29 28 -0.887 -7.75
30 29 -1.012 -1.119
31 30 -1.141 7.757
32 31 -1.266 5.56
33 32 -1.393 3.34
34 33 -1.519 -3.347
35 34 -1.646 1.111
36 35 -1.773 -5.564
37 36 -1.901 -7.762
38 37 -2.027 -1.121
39 38 -2.155 7.768
40 39 -2.282 5.569
41 40 -2.409 3.345
42 41 -2.535 -3.353
43 42 -2.663 1.113
44 43 -2.789 -5.573
45 44 -2.916 -7.775
46 45 -3.044 -1.123
47 46 -3.172 7.78
48 47 -3.299 5.578
49 48 -3.425 3.351
50 49 -3.552 -3.358
51 50 -3.68 1.115
52 51 -3.806 -5.582
53 52 -3.933 -7.787
54 53 -4.062 -1.125
55 54 -4.19 7.792
56 55 -4.318 5.586
57 56 -4.444 3.356
58 57 -4.571 -3.363
59 58 -4.699 1.116
60 59 -4.824 -5.591
61 60 -5.081 -1.127
62 61 -5.336 5.595
63 62 -5.718 1.118
64 63 -6.1 -1.129
65 64 -6.607 -3.374
66 65 -7.117 -1.13
67 66 -7.624 -3.379
68 67 -8.134 -1.132
69 68 -8.64 -3.383
70 69 -9.149 3.381
71 70 -9.652 -3.388
72 71 -10.16 3.386
73 72 -10.665 1.127
74 73 -11.17 3.39
75 74 -11.672 1.129
76 75 -12.174 3.395
77 76 -12.673 1.131
78 77 -13.173 3.401
79 78 -13.67 1.133
80 79 -14.166 3.406
81 80 -14.66 1.135
82 81 -15.154 3.41
83 82 -15.645 1.137
84 83 -16.622 1.139
85 84 -17.592 1.142
86 85 -18.548 -3.426
87 86 -19.978 -1.145
88 87 -21.379 -3.436
89 88 -22.768 -1.146
90 89 -24.123 -3.446
91 90 -25.016 -3.449

65
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/PandarQT_Angle Correction File.csv Normal file
View File

@ -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
1 Channel Elevation Azimuth
2 1 -52.121 8.736
3 2 -49.785 8.314
4 3 -47.577 7.964
5 4 -45.477 7.669
6 5 -43.465 7.417
7 6 -41.528 7.198
8 7 -39.653 7.007
9 8 -37.831 6.838
10 9 -36.055 6.688
11 10 -34.32 6.554
12 11 -32.619 6.434
13 12 -30.95 6.326
14 13 -29.308 6.228
15 14 -27.69 6.14
16 15 -26.094 6.059
17 16 -24.517 5.987
18 17 -22.964 -5.27
19 18 -21.42 -5.216
20 19 -19.889 -5.167
21 20 -18.372 -5.123
22 21 -16.865 -5.083
23 22 -15.368 -5.047
24 23 -13.88 -5.016
25 24 -12.399 -4.988
26 25 -10.925 -4.963
27 26 -9.457 -4.942
28 27 -7.994 -4.924
29 28 -6.535 -4.91
30 29 -5.079 -4.898
31 30 -3.626 -4.889
32 31 -2.175 -4.884
33 32 -0.725 -4.881
34 33 0.725 5.493
35 34 2.175 5.496
36 35 3.626 5.502
37 36 5.079 5.512
38 37 6.534 5.525
39 38 7.993 5.541
40 39 9.456 5.561
41 40 10.923 5.584
42 41 12.397 5.611
43 42 13.877 5.642
44 43 15.365 5.676
45 44 16.861 5.716
46 45 18.368 5.759
47 46 19.885 5.808
48 47 21.415 5.862
49 48 22.959 5.921
50 49 24.524 -5.33
51 50 26.101 -5.396
52 51 27.697 -5.469
53 52 29.315 -5.55
54 53 30.957 -5.64
55 54 32.627 -5.74
56 55 34.328 -5.85
57 56 36.064 -5.974
58 57 37.84 -6.113
59 58 39.662 -6.269
60 59 41.537 -6.447
61 60 43.475 -6.651
62 61 45.487 -6.887
63 62 47.587 -7.163
64 63 49.795 -7.493
65 64 52.133 -7.892

17
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/PandarXT-16_Angle Correction File.csv Normal file
View File

@ -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
1 Channel Elevation Azimuth
2 1 15 0
3 2 13 0
4 3 11 0
5 4 9 0
6 5 7 0
7 6 5 0
8 7 3 0
9 8 1 0
10 9 -1 0
11 10 -3 0
12 11 -5 0
13 12 -7 0
14 13 -9 0
15 14 -11 0
16 15 -13 0
17 16 -15 0

33
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/PandarXT_Angle Correction File.csv Normal file
View File

@ -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
1 Channel Elevation Azimuth
2 1 15 0
3 2 14 0
4 3 13 0
5 4 12 0
6 5 11 0
7 6 10 0
8 7 9 0
9 8 8 0
10 9 7 0
11 10 6 0
12 11 5 0
13 12 4 0
14 13 3 0
15 14 2 0
16 15 1 0
17 16 0 0
18 17 -1 0
19 18 -2 0
20 19 -3 0
21 20 -4 0
22 21 -5 0
23 22 -6 0
24 23 -7 0
25 24 -8 0
26 25 -9 0
27 26 -10 0
28 27 -11 0
29 28 -12 0
30 29 -13 0
31 30 -14 0
32 31 -15 0
33 32 -16 0

129
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/QT128C2X_Angle Correction File.csv Normal file
View File

@ -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
1 Laser id Elevation Azimuth
2 1 -52.62676282 10.10830596
3 2 -51.0280939 9.719503673
4 3 -49.51495392 9.384265827
5 4 -48.07394795 9.091433335
6 5 -46.69466297 8.832899657
7 6 -45.36882812 8.602605729
8 7 -44.08974439 8.395920495
9 8 -42.85190128 8.209210679
10 9 -41.65069769 8.03961095
11 10 -40.48225401 7.8848012
12 11 -39.34326358 7.742887867
13 12 -38.23088092 7.612311759
14 13 -37.14263654 7.491771683
15 14 -36.07637316 7.380164213
16 15 -35.03019446 7.276562397
17 16 -34.00242585 7.180171603
18 17 -32.99157758 7.090302002
19 18 -31.99631045 7.006369095
20 19 -31.01543179 6.927848973
21 20 -30.04786472 6.854295551
22 21 -29.09262707 6.785306938
23 22 -28.14883195 6.720536921
24 23 -27.21566989 6.659666871
25 24 -26.29240356 6.602428464
26 25 -25.37835081 6.548569874
27 26 -24.47288318 6.497872808
28 27 -23.57542315 6.450129948
29 28 -22.68544071 6.405178951
30 29 -21.80243688 6.362840261
31 30 -20.92594329 6.32298504
32 31 -20.05553602 6.285467298
33 32 -19.19081372 6.25017485
34 33 -18.33139172 -6.21699538
35 34 -17.47691901 -6.18583344
36 35 -16.62705442 -6.156599148
37 36 -15.78149317 -6.129208192
38 37 -14.93993032 -6.103581848
39 38 -14.10208035 -6.079663981
40 39 -13.26767188 -6.057381402
41 40 -12.43645574 -6.036683527
42 41 -11.60818201 -6.017519726
43 42 -10.78261371 -5.999833659
44 43 -9.959523504 -5.983597281
45 44 -9.138697037 -5.968771181
46 45 -8.319929599 -5.955310246
47 46 -7.503005408 -5.943192006
48 47 -6.687807162 -5.926719867
49 48 -5.873926875 -5.922876604
50 49 -5.061393513 -5.914628713
51 50 -4.249948322 -5.907639071
52 51 -3.43941536 -5.901885097
53 52 -2.629617595 -5.897355529
54 53 -1.820378325 -5.894039091
55 54 -1.011522452 -5.891935825
56 55 -0.202883525 -5.89103442
57 56 0.605717878 -5.89132922
58 57 1.414444374 -5.892831558
59 58 2.223469054 -5.895541414
60 59 3.032962884 -5.899458754
61 60 3.8431104 -5.904600534
62 61 4.654074511 -5.910966684
63 62 5.466034723 -5.918579795
64 63 6.279180222 -5.927451101
65 64 7.093696848 -5.937608821
66 65 7.909760474 5.949064148
67 66 8.727579858 5.961845258
68 67 9.54735011 5.97598597
69 68 10.36927364 5.99150874
70 69 11.19356846 6.008464331
71 70 12.0204614 6.026875139
72 71 12.85016831 6.046797524
73 72 13.68294669 6.068282136
74 73 14.51903314 6.091373918
75 74 15.35869922 6.116134758
76 75 16.20222125 6.142626487
77 76 17.04988688 6.170927865
78 77 17.90200196 6.201111915
79 78 18.75890049 6.233268566
80 79 19.62091911 6.267487655
81 80 20.48842178 6.303870236
82 81 21.36180725 6.342539884
83 82 22.24149217 6.383614369
84 83 23.12791172 6.427233938
85 84 24.02155359 6.473555624
86 85 24.92293181 6.52274755
87 86 25.83260224 6.574994555
88 87 26.75116558 6.63050945
89 88 27.67927338 6.689521655
90 89 28.61763387 6.752299732
91 90 29.56702472 6.819128697
92 91 30.52828971 6.890343815
93 92 31.50236238 6.966319159
94 93 32.49026442 7.0474731
95 94 33.49313666 7.134285032
96 95 34.51223682 7.227334588
97 96 35.54897914 7.327222327
98 97 19.19081372 -6.25017485
99 98 20.05553602 -6.285467298
100 99 20.92594329 -6.32298504
101 100 21.80243688 -6.362840261
102 101 22.68544071 -6.405178951
103 102 23.57542315 -6.450129948
104 103 24.47288318 -6.497872808
105 104 25.37835081 -6.548569874
106 105 26.29240356 -6.602428464
107 106 27.21566989 -6.659666871
108 107 28.14883195 -6.720536921
109 108 29.09262707 -6.785306938
110 109 30.04786472 -6.854295551
111 110 31.01543179 -6.927848973
112 111 31.99631045 -7.006369095
113 112 32.99157758 -7.090302002
114 113 34.00242585 -7.180171603
115 114 35.03019446 -7.276562397
116 115 36.07637316 -7.380164213
117 116 37.14263654 -7.491771683
118 117 38.23088092 -7.612311759
119 118 39.34326358 -7.742887867
120 119 40.48225401 -7.8848012
121 120 41.65069769 -8.03961095
122 121 42.85190128 -8.209210679
123 122 44.08974439 -8.395920495
124 123 45.36882812 -8.602605729
125 124 46.69466297 -8.832899657
126 125 48.07394795 -9.091433335
127 126 49.51495392 -9.384265827
128 127 51.0280939 -9.719503673
129 128 52.62676282 -10.10830596

84
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/QT128C2X_Channel_Cofig.csv Normal file
View File

@ -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,,,
1 EEFF,1,1
2 LaserNum,80,BlockNum,2
3 Loop1,Loop2
4 49,17
5 50,18
6 51,19
7 52,20
8 53,21
9 54,22
10 55,23
11 56,24
12 57,25
13 58,26
14 59,27
15 60,28
16 61,29
17 62,30
18 63,31
19 64,32
20 65,65
21 66,66
22 67,67
23 68,68
24 69,69
25 70,70
26 71,71
27 72,72
28 73,73
29 74,74
30 75,75
31 76,76
32 77,77
33 78,78
34 79,79
35 80,80
36 81,81
37 82,82
38 83,83
39 84,84
40 85,85
41 86,86
42 87,87
43 88,88
44 89,89
45 90,90
46 91,91
47 92,92
48 93,93
49 94,94
50 95,95
51 96,96
52 97,97
53 98,98
54 99,99
55 100,100
56 101,101
57 102,102
58 103,103
59 104,104
60 105,105
61 106,106
62 107,107
63 108,108
64 109,109
65 110,110
66 111,111
67 112,112
68 113,113
69 114,114
70 115,115
71 116,116
72 117,117
73 118,118
74 119,119
75 120,120
76 121,121
77 122,122
78 123,123
79 124,124
80 125,125
81 126,126
82 127,127
83 128,128
84 3142432,,,

33
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/angle_correction/XT32M2X_Angle Correction File.csv Normal file
View File

@ -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
1 Channel Elevation Azimuth
2 1 19.433708 0.175281
3 2 18.129961 0.175729
4 3 16.821214 0.173645
5 4 15.527467 0.171487
6 5 14.227470 0.169308
7 6 12.924973 0.167102
8 7 11.663726 0.166029
9 8 10.363728 0.161264
10 9 9.062481 0.160229
11 10 7.759984 0.161676
12 11 6.461237 0.159349
13 12 5.162489 0.158259
14 13 3.864992 0.155905
15 14 2.566245 0.154796
16 15 1.273747 0.153668
17 16 -0.032500 0.153816
18 17 -1.339997 0.153966
19 18 -2.637495 0.151599
20 19 -3.937492 0.151743
21 20 -5.233739 0.149387
22 21 -6.529987 0.152042
23 22 -7.827484 0.153463
24 23 -9.126231 0.147404
25 24 -10.426228 0.140117
26 25 -11.719976 0.137839
27 26 -12.984973 0.134276
28 27 -14.274970 0.150794
29 28 -15.567467 0.151100
30 29 -16.859964 0.147691
31 30 -18.157461 0.146833
32 31 -19.451208 0.144759
33 32 -20.747455 0.143988

129
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/firetime_correction/AT128E2X_Firetime Correction File.csv Normal file
View File

@ -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
1 Channel fire time(us)
2 1 0
3 2 0
4 3 8.24
5 4 4.112
6 5 4.114
7 6 8.24
8 7 0
9 8 0
10 9 12.424
11 10 4.144
12 11 4.112
13 12 8.264
14 13 12.376
15 14 12.376
16 15 8.264
17 16 12.424
18 17 0
19 18 0
20 19 4.112
21 20 8.24
22 21 4.144
23 22 0
24 23 0
25 24 4.144
26 25 12.424
27 26 8.264
28 27 4.112
29 28 12.376
30 29 12.376
31 30 12.424
32 31 8.264
33 32 0.848
34 33 2.504
35 34 4.976
36 35 6.616
37 36 6.616
38 37 9.112
39 38 2.504
40 39 0.848
41 40 10.768
42 41 13.28
43 42 13.28
44 43 4.976
45 44 9.112
46 45 14.928
47 46 14.928
48 47 10.768
49 48 2.504
50 49 0.848
51 50 6.616
52 51 4.976
53 52 9.112
54 53 6.616
55 54 0.848
56 55 2.504
57 56 13.28
58 57 10.768
59 58 4.976
60 59 13.28
61 60 13.28
62 61 9.112
63 62 10.768
64 63 14.928
65 64 13.28
66 65 0.848
67 66 9.112
68 67 13.28
69 68 2.504
70 69 4.976
71 70 0.848
72 71 2.504
73 72 14.928
74 73 10.768
75 74 10.768
76 75 14.928
77 76 4.976
78 77 6.616
79 78 6.616
80 79 9.112
81 80 10.768
82 81 13.28
83 82 13.28
84 83 9.112
85 84 4.976
86 85 2.504
87 86 2.504
88 87 0.848
89 88 10.768
90 89 14.928
91 90 14.928
92 91 10.768
93 92 6.616
94 93 4.976
95 94 9.112
96 95 6.616
97 96 4.112
98 97 12.424
99 98 0
100 99 4.144
101 100 0
102 101 0
103 102 12.424
104 103 0
105 104 8.264
106 105 12.424
107 106 12.424
108 107 8.24
109 108 8.24
110 109 8.264
111 110 12.376
112 111 12.376
113 112 12.424
114 113 4.112
115 114 4.144
116 115 0
117 116 0
118 117 0
119 118 0
120 119 0
121 120 12.424
122 121 8.264
123 122 8.24
124 123 4.144
125 124 8.264
126 125 8.24
127 126 12.376
128 127 12.376
129 128 8.264

131
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/firetime_correction/Pandar128E3X_Firetime Correction File.csv Normal file
View File

@ -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
operation mode,0,0,0,0,0,0,0,0,2,2,2,2,3,3,3,3
angle state,0,0,1,1,2,2,3,3,0,0,1,1,0,0,1,1
1,4436,5201,0,0,4436,4436,0,0,4436,5201,4436,4436,4436,5201,4436,4436
2,0,0,776,776,0,0,776,776,28554,28554,28554,28554,28554,28554,28554,28554
3,776,1541,0,0,776,776,0,0,776,1541,776,776,776,1541,776,776
4,2431,2431,0,0,2781,2781,0,0,2431,2431,2781,2781,2431,2431,2781,2781
5,4436,4436,0,0,4436,4436,0,0,4436,4436,4436,4436,4436,4436,4436,4436
6,0,0,2781,4026,0,0,2431,2431,30559,31804,30209,30209,30559,31804,30209,30209
7,6441,6441,0,0,6091,6091,0,0,6441,6441,6091,6091,6441,6441,6091,6091
8,0,0,4786,4786,0,0,4086,4086,32564,32564,31864,31864,32564,32564,31864,31864
9,0,0,6441,7206,0,0,6091,6091,34219,34984,33869,33869,34219,34984,33869,33869
10,776,776,0,0,776,776,0,0,776,776,776,776,776,776,776,776
11,2431,2431,0,0,2781,2781,0,0,2431,2431,2781,2781,2431,2431,2781,2781
12,6441,6441,0,0,6091,7336,0,0,6441,6441,6091,7336,6441,6441,6091,7336
13,0,0,776,776,0,0,776,776,28554,28554,28554,28554,28554,28554,28554,28554
14,0,0,6441,6441,0,0,6091,6091,34219,34219,33869,33869,34219,34219,33869,33869
15,0,0,2781,3546,0,0,2431,2431,30559,31324,30209,30209,30559,31324,30209,30209
16,0,0,776,776,0,0,776,776,28554,28554,28554,28554,28554,28554,28554,28554
17,0,0,4786,4786,0,0,4086,4086,32564,32564,31864,31864,32564,32564,31864,31864
18,6441,7206,0,0,6091,6091,0,0,6441,7206,6091,6091,6441,7206,6091,6091
19,0,0,4786,4786,0,0,4086,4086,32564,32564,31864,31864,32564,32564,31864,31864
20,776,776,0,0,776,776,0,0,776,776,776,776,776,776,776,776
21,2431,3196,0,0,2781,2781,0,0,2431,3196,2781,2781,2431,3196,2781,2781
22,0,0,2781,2781,0,0,2431,2431,30559,30559,30209,30209,30559,30559,30209,30209
23,0,0,6441,6441,0,0,6091,6091,34219,34219,33869,33869,34219,34219,33869,33869
24,0,0,4786,4786,0,0,4086,4851,32564,32564,31864,32629,32564,32564,31864,32629
25,4436,4436,0,0,4436,4436,0,0,4436,4436,4436,4436,4436,4436,4436,4436
26,10381,10381,10731,12126,10381,10381,10031,10031,38509,39904,37809,37809,38509,39904,37809,37809
27,14951,14951,15301,15301,14951,14951,14601,14601,43079,43079,42379,42379,43079,43079,42379,42379
28,12666,12666,13016,13016,12666,12666,12316,12316,12666,12666,12666,12666,12666,12666,12666,12666
29,14951,14951,15301,15301,14951,14951,14601,14601,43079,43079,42379,42379,43079,43079,42379,42379
30,19521,19521,19871,19871,19521,19521,19171,19171,19521,19521,19521,19521,19521,19521,19521,19521
31,19521,19521,19871,19871,19521,19521,19171,19171,19521,19521,19521,19521,19521,19521,19521,19521
32,8096,8096,8446,8446,8096,8096,7746,7746,36224,36224,35524,35524,36224,36224,35524,35524
33,12666,12666,13016,13016,12666,14061,12316,12316,12666,12666,12666,14061,12666,12666,12666,14061
34,12666,12666,13016,13016,12666,12666,12316,12316,12666,12666,12666,12666,12666,12666,12666,12666
35,10381,10381,10731,10731,10381,10381,10031,10031,38509,38509,37809,37809,38509,38509,37809,37809
36,24091,24091,24441,24441,24091,24091,23741,23741,52219,52219,51519,51519,52219,52219,51519,51519
37,17236,17236,17586,17586,17236,17236,16886,16886,17236,17236,17236,17236,17236,17236,17236,17236
38,24091,24091,24441,24441,24091,24091,23741,23741,52219,52219,51519,51519,52219,52219,51519,51519
39,14951,14951,15301,15301,14951,14951,14601,14601,43079,43079,42379,42379,43079,43079,42379,42379
40,14951,27056,15301,15301,14951,14951,14601,14601,43079,27056,42379,42379,43079,27056,42379,42379
41,19521,19521,19871,19871,19521,19521,19171,19171,19521,19521,19521,19521,19521,19521,19521,19521
42,17236,17236,17586,17586,17236,17236,16886,16886,17236,17236,17236,17236,17236,17236,17236,17236
43,12666,12666,13016,13016,12666,12666,12316,12316,12666,12666,12666,12666,12666,12666,12666,12666
44,21806,21806,22156,22156,21806,21806,21456,21456,21806,21806,21806,21806,21806,21806,21806,21806
45,8096,8096,8446,8446,8096,8096,7746,7746,36224,36224,35524,35524,36224,36224,35524,35524
46,21806,21806,22156,22156,21806,21806,21456,21456,21806,21806,21806,21806,21806,21806,21806,21806
47,10381,10381,10731,27406,10381,10381,10031,10031,38509,55184,37809,37809,38509,55184,37809,37809
48,10381,10381,10731,10731,10381,10381,10031,10031,38509,38509,37809,37809,38509,38509,37809,37809
49,21806,21806,22156,22156,21806,21806,21456,21456,21806,21806,21806,21806,21806,21806,21806,21806
50,8096,8096,8446,8446,8096,8096,7746,7746,36224,36224,35524,35524,36224,36224,35524,35524
51,8096,8096,8446,8446,8096,8096,7746,7746,36224,36224,35524,35524,36224,36224,35524,35524
52,19521,19521,19871,19871,19521,19521,19171,19171,19521,19521,19521,19521,19521,19521,19521,19521
53,12666,12666,13016,13016,12666,12666,12316,12316,12666,12666,12666,12666,12666,12666,12666,12666
54,12666,12666,13016,13016,12666,27056,12316,12316,12666,12666,12666,27056,12666,12666,12666,27056
55,24091,24091,24441,24441,24091,24091,23741,23741,52219,52219,51519,51519,52219,52219,51519,51519
56,24091,24091,24441,24441,24091,24091,23741,23741,52219,52219,51519,51519,52219,52219,51519,51519
57,17236,17236,17586,17586,17236,17236,16886,16886,17236,17236,17236,17236,17236,17236,17236,17236
58,21806,21806,22156,22156,21806,21806,21456,21456,21806,21806,21806,21806,21806,21806,21806,21806
59,17236,17236,17586,17586,17236,17236,16886,16886,17236,17236,17236,17236,17236,17236,17236,17236
60,14951,14951,15301,15301,14951,14951,14601,14601,43079,43079,42379,42379,43079,43079,42379,42379
61,10381,10381,10731,10731,10381,10381,10031,26706,38509,38509,37809,54484,38509,38509,37809,54484
62,14951,14951,15301,15301,14951,14951,14601,14601,43079,43079,42379,42379,43079,43079,42379,42379
63,17236,17236,17586,17586,17236,17236,16886,16886,17236,17236,17236,17236,17236,17236,17236,17236
64,17236,17236,17586,17586,17236,17236,16886,16886,17236,17236,17236,17236,17236,17236,17236,17236
65,8096,8096,8446,8446,8096,8096,7746,7746,36224,36224,35524,35524,36224,36224,35524,35524
66,19521,19521,19871,19871,19521,19521,19171,19171,19521,19521,19521,19521,19521,19521,19521,19521
67,19521,19521,19871,19871,19521,19521,19171,19171,19521,19521,19521,19521,19521,19521,19521,19521
68,10381,10381,10731,10731,10381,10381,10031,11426,38509,38509,37809,39204,38509,38509,37809,39204
69,24091,24091,24441,24441,24091,24091,23741,23741,52219,52219,51519,51519,52219,52219,51519,51519
70,10381,10381,10731,10731,10381,10381,10031,10031,38509,38509,37809,37809,38509,38509,37809,37809
71,21806,21806,22156,22156,21806,21806,21456,21456,21806,21806,21806,21806,21806,21806,21806,21806
72,12666,12666,13016,13016,12666,12666,12316,12316,12666,12666,12666,12666,12666,12666,12666,12666
73,10381,10381,10731,10731,10381,10381,10031,10031,38509,38509,37809,37809,38509,38509,37809,37809
74,14951,14951,15301,15301,14951,14951,14601,14601,43079,43079,42379,42379,43079,43079,42379,42379
75,21806,23201,22156,22156,21806,21806,21456,21456,21806,23201,21806,21806,21806,23201,21806,21806
76,8096,8096,8446,8446,8096,8096,7746,7746,36224,36224,35524,35524,36224,36224,35524,35524
77,19521,19521,19871,19871,19521,19521,19171,19171,19521,19521,19521,19521,19521,19521,19521,19521
78,17236,17236,17586,17586,17236,17236,16886,16886,17236,17236,17236,17236,17236,17236,17236,17236
79,8096,8096,8446,8446,8096,8096,7746,7746,36224,36224,35524,35524,36224,36224,35524,35524
80,19521,19521,19871,19871,19521,19521,19171,19171,19521,19521,19521,19521,19521,19521,19521,19521
81,24091,24091,24441,24441,24091,24091,23741,23741,52219,52219,51519,51519,52219,52219,51519,51519
82,24091,24091,24441,24441,24091,24091,23741,25136,52219,52219,51519,52914,52219,52219,51519,52914
83,24091,24091,24441,24441,24091,24091,23741,23741,52219,52219,51519,51519,52219,52219,51519,51519
84,17236,17236,17586,17586,17236,17236,16886,16886,17236,17236,17236,17236,17236,17236,17236,17236
85,21806,21806,22156,22156,21806,21806,21456,21456,21806,21806,21806,21806,21806,21806,21806,21806
86,8096,8096,8446,8446,8096,8096,7746,7746,36224,36224,35524,35524,36224,36224,35524,35524
87,12666,12666,13016,13016,12666,12666,12316,12316,12666,12666,12666,12666,12666,12666,12666,12666
88,21806,21806,22156,22156,21806,21806,21456,21456,21806,21806,21806,21806,21806,21806,21806,21806
89,14951,14951,15301,15301,14951,14951,14601,14601,43079,43079,42379,42379,43079,43079,42379,42379
90,2431,3676,0,0,2781,2781,0,0,2431,3676,2781,2781,2431,3676,2781,2781
91,776,776,0,0,776,776,0,0,776,776,776,776,776,776,776,776
92,4436,4436,0,0,4436,4436,0,0,4436,4436,4436,4436,4436,4436,4436,4436
93,6441,6441,0,0,6091,6856,0,0,6441,6441,6091,6856,6441,6441,6091,6856
94,0,0,6441,6441,0,0,6091,6091,34219,34219,33869,33869,34219,34219,33869,33869
95,0,0,2781,2781,0,0,2431,2431,30559,30559,30209,30209,30559,30559,30209,30209
96,776,776,0,0,776,2021,0,0,776,776,776,2021,776,776,776,2021
97,0,0,776,776,0,0,776,776,28554,28554,28554,28554,28554,28554,28554,28554
98,2431,2431,0,0,2781,2781,0,0,2431,2431,2781,2781,2431,2431,2781,2781
99,2431,2431,0,0,2781,3546,0,0,2431,2431,2781,3546,2431,2431,2781,3546
100,4436,4436,0,0,4436,4436,0,0,4436,4436,4436,4436,4436,4436,4436,4436
101,0,0,4786,4786,0,0,4086,4086,32564,32564,31864,31864,32564,32564,31864,31864
102,0,0,776,2021,0,0,776,776,28554,29799,28554,28554,28554,29799,28554,28554
103,0,0,2781,2781,0,0,2431,2431,30559,30559,30209,30209,30559,30559,30209,30209
104,6441,6441,0,0,6091,6091,0,0,6441,6441,6091,6091,6441,6441,6091,6091
105,4436,5681,0,0,4436,4436,0,0,4436,5681,4436,4436,4436,5681,4436,4436
106,0,0,2781,2781,0,0,2431,2431,30559,30559,30209,30209,30559,30559,30209,30209
107,0,0,776,776,0,0,776,776,28554,28554,28554,28554,28554,28554,28554,28554
108,0,0,4786,4786,0,0,4086,5331,32564,32564,31864,33109,32564,32564,31864,33109
109,6441,6441,0,0,6091,6091,0,0,6441,6441,6091,6091,6441,6441,6091,6091
110,0,0,6441,6441,0,0,6091,6091,34219,34219,33869,33869,34219,34219,33869,33869
111,0,0,6441,7686,0,0,6091,6091,34219,35464,33869,33869,34219,35464,33869,33869
112,0,0,4786,4786,0,0,4086,4086,32564,32564,31864,31864,32564,32564,31864,31864
113,776,776,0,0,776,776,0,0,776,776,776,776,776,776,776,776
114,4436,4436,0,0,4436,5201,0,0,4436,4436,4436,5201,4436,4436,4436,5201
115,0,0,4786,4786,0,0,4086,4086,32564,32564,31864,31864,32564,32564,31864,31864
116,2431,2431,0,0,2781,2781,0,0,2431,2431,2781,2781,2431,2431,2781,2781
117,0,0,2781,2781,0,0,2431,3196,30559,30559,30209,30974,30559,30559,30209,30974
118,0,0,6441,6441,0,0,6091,6091,34219,34219,33869,33869,34219,34219,33869,33869
119,776,776,0,0,776,776,0,0,776,776,776,776,776,776,776,776
120,0,0,776,1541,0,0,776,776,28554,29319,28554,28554,28554,29319,28554,28554
121,4436,4436,0,0,4436,4436,0,0,4436,4436,4436,4436,4436,4436,4436,4436
122,6441,6441,0,0,6091,6091,0,0,6441,6441,6091,6091,6441,6441,6091,6091
123,0,0,6441,6441,0,0,6091,6856,34219,34219,33869,34634,34219,34219,33869,34634
124,0,0,2781,2781,0,0,2431,2431,30559,30559,30209,30209,30559,30559,30209,30209
125,2431,2431,0,0,2781,2781,0,0,2431,2431,2781,2781,2431,2431,2781,2781
126,776,776,0,0,776,1541,0,0,776,776,776,1541,776,776,776,1541
127,6441,6441,0,0,6091,6091,0,0,6441,6441,6091,6091,6441,6441,6091,6091
128,0,0,776,776,0,0,776,1541,28554,28554,28554,29319,28554,28554,28554,29319
1 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
2 operation mode 0 0 0 0 0 0 0 0 2 2 2 2 3 3 3 3
3 angle state 0 0 1 1 2 2 3 3 0 0 1 1 0 0 1 1
4 1 4436 5201 0 0 4436 4436 0 0 4436 5201 4436 4436 4436 5201 4436 4436
5 2 0 0 776 776 0 0 776 776 28554 28554 28554 28554 28554 28554 28554 28554
6 3 776 1541 0 0 776 776 0 0 776 1541 776 776 776 1541 776 776
7 4 2431 2431 0 0 2781 2781 0 0 2431 2431 2781 2781 2431 2431 2781 2781
8 5 4436 4436 0 0 4436 4436 0 0 4436 4436 4436 4436 4436 4436 4436 4436
9 6 0 0 2781 4026 0 0 2431 2431 30559 31804 30209 30209 30559 31804 30209 30209
10 7 6441 6441 0 0 6091 6091 0 0 6441 6441 6091 6091 6441 6441 6091 6091
11 8 0 0 4786 4786 0 0 4086 4086 32564 32564 31864 31864 32564 32564 31864 31864
12 9 0 0 6441 7206 0 0 6091 6091 34219 34984 33869 33869 34219 34984 33869 33869
13 10 776 776 0 0 776 776 0 0 776 776 776 776 776 776 776 776
14 11 2431 2431 0 0 2781 2781 0 0 2431 2431 2781 2781 2431 2431 2781 2781
15 12 6441 6441 0 0 6091 7336 0 0 6441 6441 6091 7336 6441 6441 6091 7336
16 13 0 0 776 776 0 0 776 776 28554 28554 28554 28554 28554 28554 28554 28554
17 14 0 0 6441 6441 0 0 6091 6091 34219 34219 33869 33869 34219 34219 33869 33869
18 15 0 0 2781 3546 0 0 2431 2431 30559 31324 30209 30209 30559 31324 30209 30209
19 16 0 0 776 776 0 0 776 776 28554 28554 28554 28554 28554 28554 28554 28554
20 17 0 0 4786 4786 0 0 4086 4086 32564 32564 31864 31864 32564 32564 31864 31864
21 18 6441 7206 0 0 6091 6091 0 0 6441 7206 6091 6091 6441 7206 6091 6091
22 19 0 0 4786 4786 0 0 4086 4086 32564 32564 31864 31864 32564 32564 31864 31864
23 20 776 776 0 0 776 776 0 0 776 776 776 776 776 776 776 776
24 21 2431 3196 0 0 2781 2781 0 0 2431 3196 2781 2781 2431 3196 2781 2781
25 22 0 0 2781 2781 0 0 2431 2431 30559 30559 30209 30209 30559 30559 30209 30209
26 23 0 0 6441 6441 0 0 6091 6091 34219 34219 33869 33869 34219 34219 33869 33869
27 24 0 0 4786 4786 0 0 4086 4851 32564 32564 31864 32629 32564 32564 31864 32629
28 25 4436 4436 0 0 4436 4436 0 0 4436 4436 4436 4436 4436 4436 4436 4436
29 26 10381 10381 10731 12126 10381 10381 10031 10031 38509 39904 37809 37809 38509 39904 37809 37809
30 27 14951 14951 15301 15301 14951 14951 14601 14601 43079 43079 42379 42379 43079 43079 42379 42379
31 28 12666 12666 13016 13016 12666 12666 12316 12316 12666 12666 12666 12666 12666 12666 12666 12666
32 29 14951 14951 15301 15301 14951 14951 14601 14601 43079 43079 42379 42379 43079 43079 42379 42379
33 30 19521 19521 19871 19871 19521 19521 19171 19171 19521 19521 19521 19521 19521 19521 19521 19521
34 31 19521 19521 19871 19871 19521 19521 19171 19171 19521 19521 19521 19521 19521 19521 19521 19521
35 32 8096 8096 8446 8446 8096 8096 7746 7746 36224 36224 35524 35524 36224 36224 35524 35524
36 33 12666 12666 13016 13016 12666 14061 12316 12316 12666 12666 12666 14061 12666 12666 12666 14061
37 34 12666 12666 13016 13016 12666 12666 12316 12316 12666 12666 12666 12666 12666 12666 12666 12666
38 35 10381 10381 10731 10731 10381 10381 10031 10031 38509 38509 37809 37809 38509 38509 37809 37809
39 36 24091 24091 24441 24441 24091 24091 23741 23741 52219 52219 51519 51519 52219 52219 51519 51519
40 37 17236 17236 17586 17586 17236 17236 16886 16886 17236 17236 17236 17236 17236 17236 17236 17236
41 38 24091 24091 24441 24441 24091 24091 23741 23741 52219 52219 51519 51519 52219 52219 51519 51519
42 39 14951 14951 15301 15301 14951 14951 14601 14601 43079 43079 42379 42379 43079 43079 42379 42379
43 40 14951 27056 15301 15301 14951 14951 14601 14601 43079 27056 42379 42379 43079 27056 42379 42379
44 41 19521 19521 19871 19871 19521 19521 19171 19171 19521 19521 19521 19521 19521 19521 19521 19521
45 42 17236 17236 17586 17586 17236 17236 16886 16886 17236 17236 17236 17236 17236 17236 17236 17236
46 43 12666 12666 13016 13016 12666 12666 12316 12316 12666 12666 12666 12666 12666 12666 12666 12666
47 44 21806 21806 22156 22156 21806 21806 21456 21456 21806 21806 21806 21806 21806 21806 21806 21806
48 45 8096 8096 8446 8446 8096 8096 7746 7746 36224 36224 35524 35524 36224 36224 35524 35524
49 46 21806 21806 22156 22156 21806 21806 21456 21456 21806 21806 21806 21806 21806 21806 21806 21806
50 47 10381 10381 10731 27406 10381 10381 10031 10031 38509 55184 37809 37809 38509 55184 37809 37809
51 48 10381 10381 10731 10731 10381 10381 10031 10031 38509 38509 37809 37809 38509 38509 37809 37809
52 49 21806 21806 22156 22156 21806 21806 21456 21456 21806 21806 21806 21806 21806 21806 21806 21806
53 50 8096 8096 8446 8446 8096 8096 7746 7746 36224 36224 35524 35524 36224 36224 35524 35524
54 51 8096 8096 8446 8446 8096 8096 7746 7746 36224 36224 35524 35524 36224 36224 35524 35524
55 52 19521 19521 19871 19871 19521 19521 19171 19171 19521 19521 19521 19521 19521 19521 19521 19521
56 53 12666 12666 13016 13016 12666 12666 12316 12316 12666 12666 12666 12666 12666 12666 12666 12666
57 54 12666 12666 13016 13016 12666 27056 12316 12316 12666 12666 12666 27056 12666 12666 12666 27056
58 55 24091 24091 24441 24441 24091 24091 23741 23741 52219 52219 51519 51519 52219 52219 51519 51519
59 56 24091 24091 24441 24441 24091 24091 23741 23741 52219 52219 51519 51519 52219 52219 51519 51519
60 57 17236 17236 17586 17586 17236 17236 16886 16886 17236 17236 17236 17236 17236 17236 17236 17236
61 58 21806 21806 22156 22156 21806 21806 21456 21456 21806 21806 21806 21806 21806 21806 21806 21806
62 59 17236 17236 17586 17586 17236 17236 16886 16886 17236 17236 17236 17236 17236 17236 17236 17236
63 60 14951 14951 15301 15301 14951 14951 14601 14601 43079 43079 42379 42379 43079 43079 42379 42379
64 61 10381 10381 10731 10731 10381 10381 10031 26706 38509 38509 37809 54484 38509 38509 37809 54484
65 62 14951 14951 15301 15301 14951 14951 14601 14601 43079 43079 42379 42379 43079 43079 42379 42379
66 63 17236 17236 17586 17586 17236 17236 16886 16886 17236 17236 17236 17236 17236 17236 17236 17236
67 64 17236 17236 17586 17586 17236 17236 16886 16886 17236 17236 17236 17236 17236 17236 17236 17236
68 65 8096 8096 8446 8446 8096 8096 7746 7746 36224 36224 35524 35524 36224 36224 35524 35524
69 66 19521 19521 19871 19871 19521 19521 19171 19171 19521 19521 19521 19521 19521 19521 19521 19521
70 67 19521 19521 19871 19871 19521 19521 19171 19171 19521 19521 19521 19521 19521 19521 19521 19521
71 68 10381 10381 10731 10731 10381 10381 10031 11426 38509 38509 37809 39204 38509 38509 37809 39204
72 69 24091 24091 24441 24441 24091 24091 23741 23741 52219 52219 51519 51519 52219 52219 51519 51519
73 70 10381 10381 10731 10731 10381 10381 10031 10031 38509 38509 37809 37809 38509 38509 37809 37809
74 71 21806 21806 22156 22156 21806 21806 21456 21456 21806 21806 21806 21806 21806 21806 21806 21806
75 72 12666 12666 13016 13016 12666 12666 12316 12316 12666 12666 12666 12666 12666 12666 12666 12666
76 73 10381 10381 10731 10731 10381 10381 10031 10031 38509 38509 37809 37809 38509 38509 37809 37809
77 74 14951 14951 15301 15301 14951 14951 14601 14601 43079 43079 42379 42379 43079 43079 42379 42379
78 75 21806 23201 22156 22156 21806 21806 21456 21456 21806 23201 21806 21806 21806 23201 21806 21806
79 76 8096 8096 8446 8446 8096 8096 7746 7746 36224 36224 35524 35524 36224 36224 35524 35524
80 77 19521 19521 19871 19871 19521 19521 19171 19171 19521 19521 19521 19521 19521 19521 19521 19521
81 78 17236 17236 17586 17586 17236 17236 16886 16886 17236 17236 17236 17236 17236 17236 17236 17236
82 79 8096 8096 8446 8446 8096 8096 7746 7746 36224 36224 35524 35524 36224 36224 35524 35524
83 80 19521 19521 19871 19871 19521 19521 19171 19171 19521 19521 19521 19521 19521 19521 19521 19521
84 81 24091 24091 24441 24441 24091 24091 23741 23741 52219 52219 51519 51519 52219 52219 51519 51519
85 82 24091 24091 24441 24441 24091 24091 23741 25136 52219 52219 51519 52914 52219 52219 51519 52914
86 83 24091 24091 24441 24441 24091 24091 23741 23741 52219 52219 51519 51519 52219 52219 51519 51519
87 84 17236 17236 17586 17586 17236 17236 16886 16886 17236 17236 17236 17236 17236 17236 17236 17236
88 85 21806 21806 22156 22156 21806 21806 21456 21456 21806 21806 21806 21806 21806 21806 21806 21806
89 86 8096 8096 8446 8446 8096 8096 7746 7746 36224 36224 35524 35524 36224 36224 35524 35524
90 87 12666 12666 13016 13016 12666 12666 12316 12316 12666 12666 12666 12666 12666 12666 12666 12666
91 88 21806 21806 22156 22156 21806 21806 21456 21456 21806 21806 21806 21806 21806 21806 21806 21806
92 89 14951 14951 15301 15301 14951 14951 14601 14601 43079 43079 42379 42379 43079 43079 42379 42379
93 90 2431 3676 0 0 2781 2781 0 0 2431 3676 2781 2781 2431 3676 2781 2781
94 91 776 776 0 0 776 776 0 0 776 776 776 776 776 776 776 776
95 92 4436 4436 0 0 4436 4436 0 0 4436 4436 4436 4436 4436 4436 4436 4436
96 93 6441 6441 0 0 6091 6856 0 0 6441 6441 6091 6856 6441 6441 6091 6856
97 94 0 0 6441 6441 0 0 6091 6091 34219 34219 33869 33869 34219 34219 33869 33869
98 95 0 0 2781 2781 0 0 2431 2431 30559 30559 30209 30209 30559 30559 30209 30209
99 96 776 776 0 0 776 2021 0 0 776 776 776 2021 776 776 776 2021
100 97 0 0 776 776 0 0 776 776 28554 28554 28554 28554 28554 28554 28554 28554
101 98 2431 2431 0 0 2781 2781 0 0 2431 2431 2781 2781 2431 2431 2781 2781
102 99 2431 2431 0 0 2781 3546 0 0 2431 2431 2781 3546 2431 2431 2781 3546
103 100 4436 4436 0 0 4436 4436 0 0 4436 4436 4436 4436 4436 4436 4436 4436
104 101 0 0 4786 4786 0 0 4086 4086 32564 32564 31864 31864 32564 32564 31864 31864
105 102 0 0 776 2021 0 0 776 776 28554 29799 28554 28554 28554 29799 28554 28554
106 103 0 0 2781 2781 0 0 2431 2431 30559 30559 30209 30209 30559 30559 30209 30209
107 104 6441 6441 0 0 6091 6091 0 0 6441 6441 6091 6091 6441 6441 6091 6091
108 105 4436 5681 0 0 4436 4436 0 0 4436 5681 4436 4436 4436 5681 4436 4436
109 106 0 0 2781 2781 0 0 2431 2431 30559 30559 30209 30209 30559 30559 30209 30209
110 107 0 0 776 776 0 0 776 776 28554 28554 28554 28554 28554 28554 28554 28554
111 108 0 0 4786 4786 0 0 4086 5331 32564 32564 31864 33109 32564 32564 31864 33109
112 109 6441 6441 0 0 6091 6091 0 0 6441 6441 6091 6091 6441 6441 6091 6091
113 110 0 0 6441 6441 0 0 6091 6091 34219 34219 33869 33869 34219 34219 33869 33869
114 111 0 0 6441 7686 0 0 6091 6091 34219 35464 33869 33869 34219 35464 33869 33869
115 112 0 0 4786 4786 0 0 4086 4086 32564 32564 31864 31864 32564 32564 31864 31864
116 113 776 776 0 0 776 776 0 0 776 776 776 776 776 776 776 776
117 114 4436 4436 0 0 4436 5201 0 0 4436 4436 4436 5201 4436 4436 4436 5201
118 115 0 0 4786 4786 0 0 4086 4086 32564 32564 31864 31864 32564 32564 31864 31864
119 116 2431 2431 0 0 2781 2781 0 0 2431 2431 2781 2781 2431 2431 2781 2781
120 117 0 0 2781 2781 0 0 2431 3196 30559 30559 30209 30974 30559 30559 30209 30974
121 118 0 0 6441 6441 0 0 6091 6091 34219 34219 33869 33869 34219 34219 33869 33869
122 119 776 776 0 0 776 776 0 0 776 776 776 776 776 776 776 776
123 120 0 0 776 1541 0 0 776 776 28554 29319 28554 28554 28554 29319 28554 28554
124 121 4436 4436 0 0 4436 4436 0 0 4436 4436 4436 4436 4436 4436 4436 4436
125 122 6441 6441 0 0 6091 6091 0 0 6441 6441 6091 6091 6441 6441 6091 6091
126 123 0 0 6441 6441 0 0 6091 6856 34219 34219 33869 34634 34219 34219 33869 34634
127 124 0 0 2781 2781 0 0 2431 2431 30559 30559 30209 30209 30559 30559 30209 30209
128 125 2431 2431 0 0 2781 2781 0 0 2431 2431 2781 2781 2431 2431 2781 2781
129 126 776 776 0 0 776 1541 0 0 776 776 776 1541 776 776 776 1541
130 127 6441 6441 0 0 6091 6091 0 0 6441 6441 6091 6091 6441 6441 6091 6091
131 128 0 0 776 776 0 0 776 1541 28554 28554 28554 29319 28554 28554 28554 29319

43
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/firetime_correction/Pandar40E3X_Firetime Correction File.csv Normal file
View File

@ -0,0 +1,43 @@
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
operation mode,0,0,0,0,0,0,0,0,2,2,2,2,3,3,3,3
angle state,0,0,1,1,2,2,3,3,0,0,1,1,0,0,1,1
1,51951,52716,51951,51951,51951,52716,51951,51951,51951,52716,51951,51951,51951,52716,51951,51951
2,48871,48871,48871,49636,48871,48871,48871,49636,48871,48871,48871,49636,48871,48871,48871,49636
3,38091,38091,38091,38856,38091,38091,38091,38856,38091,38091,38091,38856,38091,38091,38091,38856
4,45791,45791,45791,46556,45791,45791,45791,46556,45791,45791,45791,46556,45791,45791,45791,46556
5,44251,45016,44251,44251,44251,45016,44251,44251,44251,45016,44251,44251,44251,45016,44251,44251
6,39631,40396,39631,40396,39631,40396,39631,40396,39631,40396,39631,40396,39631,40396,39631,40396
7,42711,42711,42711,42711,42711,42711,42711,42711,42711,42711,42711,42711,42711,42711,42711,42711
8,13946,13946,13946,13946,13946,13946,13946,13946,13946,13946,13946,13946,13946,13946,13946,13946
9,776,776,776,776,776,776,776,776,776,776,776,776,776,776,776,776
10,31506,31506,31506,31506,31506,31506,31506,31506,31506,31506,31506,31506,31506,31506,31506,31506
11,18336,18336,18336,18336,18336,18336,18336,18336,18336,18336,18336,18336,18336,18336,18336,18336
12,2971,2971,2971,2971,2971,2971,2971,2971,2971,2971,2971,2971,2971,2971,2971,2971
13,20531,20531,20531,20531,20531,20531,20531,20531,20531,20531,20531,20531,20531,20531,20531,20531
14,33701,33701,33701,33701,33701,33701,33701,33701,33701,33701,33701,33701,33701,33701,33701,33701
15,5166,5166,5166,5166,5166,5166,5166,5166,5166,5166,5166,5166,5166,5166,5166,5166
16,24921,24921,24921,24921,24921,24921,24921,24921,24921,24921,24921,24921,24921,24921,24921,24921
17,35896,35896,35896,35896,35896,35896,35896,35896,35896,35896,35896,35896,35896,35896,35896,35896
18,7361,7361,7361,7361,7361,7361,7361,7361,7361,7361,7361,7361,7361,7361,7361,7361
19,22726,22726,22726,22726,22726,22726,22726,22726,22726,22726,22726,22726,22726,22726,22726,22726
20,16141,16141,16141,16141,16141,16141,16141,16141,16141,16141,16141,16141,16141,16141,16141,16141
21,9556,9556,9556,9556,9556,9556,9556,9556,9556,9556,9556,9556,9556,9556,9556,9556
22,29311,29311,29311,29311,29311,29311,29311,29311,29311,29311,29311,29311,29311,29311,29311,29311
23,11751,11751,11751,11751,11751,11751,11751,11751,11751,11751,11751,11751,11751,11751,11751,11751
24,41171,41936,41171,41936,41171,41936,41171,41936,41171,41936,41171,41936,41171,41936,41171,41936
25,44251,44251,44251,44251,44251,44251,44251,44251,44251,44251,44251,44251,44251,44251,44251,44251
26,50411,50411,50411,50411,50411,50411,50411,50411,50411,50411,50411,50411,50411,50411,50411,50411
27,42711,43476,42711,43476,42711,43476,42711,43476,42711,43476,42711,43476,42711,43476,42711,43476
28,45791,45791,45791,45791,45791,45791,45791,45791,45791,45791,45791,45791,45791,45791,45791,45791
29,39631,39631,39631,39631,39631,39631,39631,39631,39631,39631,39631,39631,39631,39631,39631,39631
30,45791,46556,45791,45791,45791,46556,45791,45791,45791,46556,45791,45791,45791,46556,45791,45791
31,47331,47331,47331,48096,47331,47331,47331,48096,47331,47331,47331,48096,47331,47331,47331,48096
32,48871,49636,48871,48871,48871,49636,48871,48871,48871,49636,48871,48871,48871,49636,48871,48871
33,50411,50411,50411,51176,50411,50411,50411,51176,50411,50411,50411,51176,50411,50411,50411,51176
34,51951,51951,51951,52716,51951,51951,51951,52716,51951,51951,51951,52716,51951,51951,51951,52716
35,53491,53491,53491,54256,53491,53491,53491,54256,53491,53491,53491,54256,53491,53491,53491,54256
36,44251,44251,44251,45016,44251,44251,44251,45016,44251,44251,44251,45016,44251,44251,44251,45016
37,50411,51176,50411,50411,50411,51176,50411,50411,50411,51176,50411,50411,50411,51176,50411,50411
38,47331,48096,47331,47331,47331,48096,47331,47331,47331,48096,47331,47331,47331,48096,47331,47331
39,38091,38856,38091,38091,38091,38856,38091,38091,38091,38856,38091,38091,38091,38856,38091,38091
40,53491,54256,53491,53491,53491,54256,53491,53491,53491,54256,53491,53491,53491,54256,53491,53491
1 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
2 operation mode 0 0 0 0 0 0 0 0 2 2 2 2 3 3 3 3
3 angle state 0 0 1 1 2 2 3 3 0 0 1 1 0 0 1 1
4 1 51951 52716 51951 51951 51951 52716 51951 51951 51951 52716 51951 51951 51951 52716 51951 51951
5 2 48871 48871 48871 49636 48871 48871 48871 49636 48871 48871 48871 49636 48871 48871 48871 49636
6 3 38091 38091 38091 38856 38091 38091 38091 38856 38091 38091 38091 38856 38091 38091 38091 38856
7 4 45791 45791 45791 46556 45791 45791 45791 46556 45791 45791 45791 46556 45791 45791 45791 46556
8 5 44251 45016 44251 44251 44251 45016 44251 44251 44251 45016 44251 44251 44251 45016 44251 44251
9 6 39631 40396 39631 40396 39631 40396 39631 40396 39631 40396 39631 40396 39631 40396 39631 40396
10 7 42711 42711 42711 42711 42711 42711 42711 42711 42711 42711 42711 42711 42711 42711 42711 42711
11 8 13946 13946 13946 13946 13946 13946 13946 13946 13946 13946 13946 13946 13946 13946 13946 13946
12 9 776 776 776 776 776 776 776 776 776 776 776 776 776 776 776 776
13 10 31506 31506 31506 31506 31506 31506 31506 31506 31506 31506 31506 31506 31506 31506 31506 31506
14 11 18336 18336 18336 18336 18336 18336 18336 18336 18336 18336 18336 18336 18336 18336 18336 18336
15 12 2971 2971 2971 2971 2971 2971 2971 2971 2971 2971 2971 2971 2971 2971 2971 2971
16 13 20531 20531 20531 20531 20531 20531 20531 20531 20531 20531 20531 20531 20531 20531 20531 20531
17 14 33701 33701 33701 33701 33701 33701 33701 33701 33701 33701 33701 33701 33701 33701 33701 33701
18 15 5166 5166 5166 5166 5166 5166 5166 5166 5166 5166 5166 5166 5166 5166 5166 5166
19 16 24921 24921 24921 24921 24921 24921 24921 24921 24921 24921 24921 24921 24921 24921 24921 24921
20 17 35896 35896 35896 35896 35896 35896 35896 35896 35896 35896 35896 35896 35896 35896 35896 35896
21 18 7361 7361 7361 7361 7361 7361 7361 7361 7361 7361 7361 7361 7361 7361 7361 7361
22 19 22726 22726 22726 22726 22726 22726 22726 22726 22726 22726 22726 22726 22726 22726 22726 22726
23 20 16141 16141 16141 16141 16141 16141 16141 16141 16141 16141 16141 16141 16141 16141 16141 16141
24 21 9556 9556 9556 9556 9556 9556 9556 9556 9556 9556 9556 9556 9556 9556 9556 9556
25 22 29311 29311 29311 29311 29311 29311 29311 29311 29311 29311 29311 29311 29311 29311 29311 29311
26 23 11751 11751 11751 11751 11751 11751 11751 11751 11751 11751 11751 11751 11751 11751 11751 11751
27 24 41171 41936 41171 41936 41171 41936 41171 41936 41171 41936 41171 41936 41171 41936 41171 41936
28 25 44251 44251 44251 44251 44251 44251 44251 44251 44251 44251 44251 44251 44251 44251 44251 44251
29 26 50411 50411 50411 50411 50411 50411 50411 50411 50411 50411 50411 50411 50411 50411 50411 50411
30 27 42711 43476 42711 43476 42711 43476 42711 43476 42711 43476 42711 43476 42711 43476 42711 43476
31 28 45791 45791 45791 45791 45791 45791 45791 45791 45791 45791 45791 45791 45791 45791 45791 45791
32 29 39631 39631 39631 39631 39631 39631 39631 39631 39631 39631 39631 39631 39631 39631 39631 39631
33 30 45791 46556 45791 45791 45791 46556 45791 45791 45791 46556 45791 45791 45791 46556 45791 45791
34 31 47331 47331 47331 48096 47331 47331 47331 48096 47331 47331 47331 48096 47331 47331 47331 48096
35 32 48871 49636 48871 48871 48871 49636 48871 48871 48871 49636 48871 48871 48871 49636 48871 48871
36 33 50411 50411 50411 51176 50411 50411 50411 51176 50411 50411 50411 51176 50411 50411 50411 51176
37 34 51951 51951 51951 52716 51951 51951 51951 52716 51951 51951 51951 52716 51951 51951 51951 52716
38 35 53491 53491 53491 54256 53491 53491 53491 54256 53491 53491 53491 54256 53491 53491 53491 54256
39 36 44251 44251 44251 45016 44251 44251 44251 45016 44251 44251 44251 45016 44251 44251 44251 45016
40 37 50411 51176 50411 50411 50411 51176 50411 50411 50411 51176 50411 50411 50411 51176 50411 50411
41 38 47331 48096 47331 47331 47331 48096 47331 47331 47331 48096 47331 47331 47331 48096 47331 47331
42 39 38091 38856 38091 38091 38091 38856 38091 38091 38091 38856 38091 38091 38091 38856 38091 38091
43 40 53491 54256 53491 53491 53491 54256 53491 53491 53491 54256 53491 53491 53491 54256 53491 53491

41
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/firetime_correction/Pandar40P_Firetime Correction File.csv Normal file
View File

@ -0,0 +1,41 @@
Channel,fire time(μs)
4,3.62
40,3.62
36,4.92
28,6.23
12,8.19
16,8.19
32,9.5
24,11.47
29,12.77
17,14.74
3,16.04
39,16.04
35,17.35
25,18.65
9,20.62
13,20.62
31,21.92
21,23.89
26,25.19
14,27.16
2,28.47
38,28.47
34,29.77
6,31.74
22,31.74
10,33.71
30,35.01
18,36.98
23,38.95
11,40.91
1,42.22
37,42.22
33,43.52
5,45.49
19,45.49
7,47.46
27,48.76
15,50.73
20,52.7
8,54.67
1 Channel fire time(μs)
2 4 3.62
3 40 3.62
4 36 4.92
5 28 6.23
6 12 8.19
7 16 8.19
8 32 9.5
9 24 11.47
10 29 12.77
11 17 14.74
12 3 16.04
13 39 16.04
14 35 17.35
15 25 18.65
16 9 20.62
17 13 20.62
18 31 21.92
19 21 23.89
20 26 25.19
21 14 27.16
22 2 28.47
23 38 28.47
24 34 29.77
25 6 31.74
26 22 31.74
27 10 33.71
28 30 35.01
29 18 36.98
30 23 38.95
31 11 40.91
32 1 42.22
33 37 42.22
34 33 43.52
35 5 45.49
36 19 45.49
37 7 47.46
38 27 48.76
39 15 50.73
40 20 52.7
41 8 54.67

67
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/firetime_correction/Pandar64E3X_Firetime Correction File.csv Normal file
View File

@ -0,0 +1,67 @@
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
operation mode,0,0,0,0,0,0,0,0,2,2,2,2,3,3,3,3
angle state,0,0,1,1,2,2,3,3,0,0,1,1,0,0,1,1
1,51951,52716,51951,51951,51951,52716,51951,51951,51951,52716,51951,51951,51951,52716,51951,51951
2,48871,48871,48871,49636,48871,48871,48871,49636,48871,48871,48871,49636,48871,48871,48871,49636
3,38091,38091,38091,38856,38091,38091,38091,38856,38091,38091,38091,38856,38091,38091,38091,38856
4,45791,45791,45791,46556,45791,45791,45791,46556,45791,45791,45791,46556,45791,45791,45791,46556
5,44251,45016,44251,44251,44251,45016,44251,44251,44251,45016,44251,44251,44251,45016,44251,44251
6,39631,40396,39631,40396,39631,40396,39631,40396,39631,40396,39631,40396,39631,40396,39631,40396
7,41171,41171,41171,41171,41171,41171,41171,41171,41171,41171,41171,41171,41171,41171,41171,41171
8,42711,42711,42711,42711,42711,42711,42711,42711,42711,42711,42711,42711,42711,42711,42711,42711
9,42711,42711,42711,42711,42711,42711,42711,42711,42711,42711,42711,42711,42711,42711,42711,42711
10,13946,13946,13946,13946,13946,13946,13946,13946,13946,13946,13946,13946,13946,13946,13946,13946
11,11751,11751,11751,11751,11751,11751,11751,11751,11751,11751,11751,11751,11751,11751,11751,11751
12,776,776,776,776,776,776,776,776,776,776,776,776,776,776,776,776
13,11751,11751,11751,11751,11751,11751,11751,11751,11751,11751,11751,11751,11751,11751,11751,11751
14,31506,31506,31506,31506,31506,31506,31506,31506,31506,31506,31506,31506,31506,31506,31506,31506
15,24921,24921,24921,24921,24921,24921,24921,24921,24921,24921,24921,24921,24921,24921,24921,24921
16,18336,18336,18336,18336,18336,18336,18336,18336,18336,18336,18336,18336,18336,18336,18336,18336
17,27116,27116,27116,27116,27116,27116,27116,27116,27116,27116,27116,27116,27116,27116,27116,27116
18,2971,2971,2971,2971,2971,2971,2971,2971,2971,2971,2971,2971,2971,2971,2971,2971
19,35896,35896,35896,35896,35896,35896,35896,35896,35896,35896,35896,35896,35896,35896,35896,35896
20,20531,20531,20531,20531,20531,20531,20531,20531,20531,20531,20531,20531,20531,20531,20531,20531
21,22726,22726,22726,22726,22726,22726,22726,22726,22726,22726,22726,22726,22726,22726,22726,22726
22,33701,33701,33701,33701,33701,33701,33701,33701,33701,33701,33701,33701,33701,33701,33701,33701
23,20531,20531,20531,20531,20531,20531,20531,20531,20531,20531,20531,20531,20531,20531,20531,20531
24,5166,5166,5166,5166,5166,5166,5166,5166,5166,5166,5166,5166,5166,5166,5166,5166
25,16141,16141,16141,16141,16141,16141,16141,16141,16141,16141,16141,16141,16141,16141,16141,16141
26,24921,24921,24921,24921,24921,24921,24921,24921,24921,24921,24921,24921,24921,24921,24921,24921
27,29311,29311,29311,29311,29311,29311,29311,29311,29311,29311,29311,29311,29311,29311,29311,29311
28,35896,35896,35896,35896,35896,35896,35896,35896,35896,35896,35896,35896,35896,35896,35896,35896
29,18336,18336,18336,18336,18336,18336,18336,18336,18336,18336,18336,18336,18336,18336,18336,18336
30,7361,7361,7361,7361,7361,7361,7361,7361,7361,7361,7361,7361,7361,7361,7361,7361
31,27116,27116,27116,27116,27116,27116,27116,27116,27116,27116,27116,27116,27116,27116,27116,27116
32,22726,22726,22726,22726,22726,22726,22726,22726,22726,22726,22726,22726,22726,22726,22726,22726
33,31506,31506,31506,31506,31506,31506,31506,31506,31506,31506,31506,31506,31506,31506,31506,31506
34,16141,16141,16141,16141,16141,16141,16141,16141,16141,16141,16141,16141,16141,16141,16141,16141
35,13946,13946,13946,13946,13946,13946,13946,13946,13946,13946,13946,13946,13946,13946,13946,13946
36,9556,9556,9556,9556,9556,9556,9556,9556,9556,9556,9556,9556,9556,9556,9556,9556
37,13946,13946,13946,13946,13946,13946,13946,13946,13946,13946,13946,13946,13946,13946,13946,13946
38,29311,29311,29311,29311,29311,29311,29311,29311,29311,29311,29311,29311,29311,29311,29311,29311
39,33701,33701,33701,33701,33701,33701,33701,33701,33701,33701,33701,33701,33701,33701,33701,33701
40,11751,11751,11751,11751,11751,11751,11751,11751,11751,11751,11751,11751,11751,11751,11751,11751
41,39631,39631,39631,39631,39631,39631,39631,39631,39631,39631,39631,39631,39631,39631,39631,39631
42,41171,41936,41171,41936,41171,41936,41171,41936,41171,41936,41171,41936,41171,41936,41171,41936
43,38091,38091,38091,38091,38091,38091,38091,38091,38091,38091,38091,38091,38091,38091,38091,38091
44,44251,44251,44251,44251,44251,44251,44251,44251,44251,44251,44251,44251,44251,44251,44251,44251
45,51951,51951,51951,51951,51951,51951,51951,51951,51951,51951,51951,51951,51951,51951,51951,51951
46,50411,50411,50411,50411,50411,50411,50411,50411,50411,50411,50411,50411,50411,50411,50411,50411
47,48871,48871,48871,48871,48871,48871,48871,48871,48871,48871,48871,48871,48871,48871,48871,48871
48,42711,43476,42711,43476,42711,43476,42711,43476,42711,43476,42711,43476,42711,43476,42711,43476
49,47331,47331,47331,47331,47331,47331,47331,47331,47331,47331,47331,47331,47331,47331,47331,47331
50,45791,45791,45791,45791,45791,45791,45791,45791,45791,45791,45791,45791,45791,45791,45791,45791
51,53491,53491,53491,53491,53491,53491,53491,53491,53491,53491,53491,53491,53491,53491,53491,53491
52,39631,39631,39631,39631,39631,39631,39631,39631,39631,39631,39631,39631,39631,39631,39631,39631
53,41171,41171,41171,41171,41171,41171,41171,41171,41171,41171,41171,41171,41171,41171,41171,41171
54,45791,46556,45791,45791,45791,46556,45791,45791,45791,46556,45791,45791,45791,46556,45791,45791
55,47331,47331,47331,48096,47331,47331,47331,48096,47331,47331,47331,48096,47331,47331,47331,48096
56,48871,49636,48871,48871,48871,49636,48871,48871,48871,49636,48871,48871,48871,49636,48871,48871
57,50411,50411,50411,51176,50411,50411,50411,51176,50411,50411,50411,51176,50411,50411,50411,51176
58,51951,51951,51951,52716,51951,51951,51951,52716,51951,51951,51951,52716,51951,51951,51951,52716
59,53491,53491,53491,54256,53491,53491,53491,54256,53491,53491,53491,54256,53491,53491,53491,54256
60,44251,44251,44251,45016,44251,44251,44251,45016,44251,44251,44251,45016,44251,44251,44251,45016
61,50411,51176,50411,50411,50411,51176,50411,50411,50411,51176,50411,50411,50411,51176,50411,50411
62,47331,48096,47331,47331,47331,48096,47331,47331,47331,48096,47331,47331,47331,48096,47331,47331
63,38091,38856,38091,38091,38091,38856,38091,38091,38091,38856,38091,38091,38091,38856,38091,38091
64,53491,54256,53491,53491,53491,54256,53491,53491,53491,54256,53491,53491,53491,54256,53491,53491
1 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
2 operation mode 0 0 0 0 0 0 0 0 2 2 2 2 3 3 3 3
3 angle state 0 0 1 1 2 2 3 3 0 0 1 1 0 0 1 1
4 1 51951 52716 51951 51951 51951 52716 51951 51951 51951 52716 51951 51951 51951 52716 51951 51951
5 2 48871 48871 48871 49636 48871 48871 48871 49636 48871 48871 48871 49636 48871 48871 48871 49636
6 3 38091 38091 38091 38856 38091 38091 38091 38856 38091 38091 38091 38856 38091 38091 38091 38856
7 4 45791 45791 45791 46556 45791 45791 45791 46556 45791 45791 45791 46556 45791 45791 45791 46556
8 5 44251 45016 44251 44251 44251 45016 44251 44251 44251 45016 44251 44251 44251 45016 44251 44251
9 6 39631 40396 39631 40396 39631 40396 39631 40396 39631 40396 39631 40396 39631 40396 39631 40396
10 7 41171 41171 41171 41171 41171 41171 41171 41171 41171 41171 41171 41171 41171 41171 41171 41171
11 8 42711 42711 42711 42711 42711 42711 42711 42711 42711 42711 42711 42711 42711 42711 42711 42711
12 9 42711 42711 42711 42711 42711 42711 42711 42711 42711 42711 42711 42711 42711 42711 42711 42711
13 10 13946 13946 13946 13946 13946 13946 13946 13946 13946 13946 13946 13946 13946 13946 13946 13946
14 11 11751 11751 11751 11751 11751 11751 11751 11751 11751 11751 11751 11751 11751 11751 11751 11751
15 12 776 776 776 776 776 776 776 776 776 776 776 776 776 776 776 776
16 13 11751 11751 11751 11751 11751 11751 11751 11751 11751 11751 11751 11751 11751 11751 11751 11751
17 14 31506 31506 31506 31506 31506 31506 31506 31506 31506 31506 31506 31506 31506 31506 31506 31506
18 15 24921 24921 24921 24921 24921 24921 24921 24921 24921 24921 24921 24921 24921 24921 24921 24921
19 16 18336 18336 18336 18336 18336 18336 18336 18336 18336 18336 18336 18336 18336 18336 18336 18336
20 17 27116 27116 27116 27116 27116 27116 27116 27116 27116 27116 27116 27116 27116 27116 27116 27116
21 18 2971 2971 2971 2971 2971 2971 2971 2971 2971 2971 2971 2971 2971 2971 2971 2971
22 19 35896 35896 35896 35896 35896 35896 35896 35896 35896 35896 35896 35896 35896 35896 35896 35896
23 20 20531 20531 20531 20531 20531 20531 20531 20531 20531 20531 20531 20531 20531 20531 20531 20531
24 21 22726 22726 22726 22726 22726 22726 22726 22726 22726 22726 22726 22726 22726 22726 22726 22726
25 22 33701 33701 33701 33701 33701 33701 33701 33701 33701 33701 33701 33701 33701 33701 33701 33701
26 23 20531 20531 20531 20531 20531 20531 20531 20531 20531 20531 20531 20531 20531 20531 20531 20531
27 24 5166 5166 5166 5166 5166 5166 5166 5166 5166 5166 5166 5166 5166 5166 5166 5166
28 25 16141 16141 16141 16141 16141 16141 16141 16141 16141 16141 16141 16141 16141 16141 16141 16141
29 26 24921 24921 24921 24921 24921 24921 24921 24921 24921 24921 24921 24921 24921 24921 24921 24921
30 27 29311 29311 29311 29311 29311 29311 29311 29311 29311 29311 29311 29311 29311 29311 29311 29311
31 28 35896 35896 35896 35896 35896 35896 35896 35896 35896 35896 35896 35896 35896 35896 35896 35896
32 29 18336 18336 18336 18336 18336 18336 18336 18336 18336 18336 18336 18336 18336 18336 18336 18336
33 30 7361 7361 7361 7361 7361 7361 7361 7361 7361 7361 7361 7361 7361 7361 7361 7361
34 31 27116 27116 27116 27116 27116 27116 27116 27116 27116 27116 27116 27116 27116 27116 27116 27116
35 32 22726 22726 22726 22726 22726 22726 22726 22726 22726 22726 22726 22726 22726 22726 22726 22726
36 33 31506 31506 31506 31506 31506 31506 31506 31506 31506 31506 31506 31506 31506 31506 31506 31506
37 34 16141 16141 16141 16141 16141 16141 16141 16141 16141 16141 16141 16141 16141 16141 16141 16141
38 35 13946 13946 13946 13946 13946 13946 13946 13946 13946 13946 13946 13946 13946 13946 13946 13946
39 36 9556 9556 9556 9556 9556 9556 9556 9556 9556 9556 9556 9556 9556 9556 9556 9556
40 37 13946 13946 13946 13946 13946 13946 13946 13946 13946 13946 13946 13946 13946 13946 13946 13946
41 38 29311 29311 29311 29311 29311 29311 29311 29311 29311 29311 29311 29311 29311 29311 29311 29311
42 39 33701 33701 33701 33701 33701 33701 33701 33701 33701 33701 33701 33701 33701 33701 33701 33701
43 40 11751 11751 11751 11751 11751 11751 11751 11751 11751 11751 11751 11751 11751 11751 11751 11751
44 41 39631 39631 39631 39631 39631 39631 39631 39631 39631 39631 39631 39631 39631 39631 39631 39631
45 42 41171 41936 41171 41936 41171 41936 41171 41936 41171 41936 41171 41936 41171 41936 41171 41936
46 43 38091 38091 38091 38091 38091 38091 38091 38091 38091 38091 38091 38091 38091 38091 38091 38091
47 44 44251 44251 44251 44251 44251 44251 44251 44251 44251 44251 44251 44251 44251 44251 44251 44251
48 45 51951 51951 51951 51951 51951 51951 51951 51951 51951 51951 51951 51951 51951 51951 51951 51951
49 46 50411 50411 50411 50411 50411 50411 50411 50411 50411 50411 50411 50411 50411 50411 50411 50411
50 47 48871 48871 48871 48871 48871 48871 48871 48871 48871 48871 48871 48871 48871 48871 48871 48871
51 48 42711 43476 42711 43476 42711 43476 42711 43476 42711 43476 42711 43476 42711 43476 42711 43476
52 49 47331 47331 47331 47331 47331 47331 47331 47331 47331 47331 47331 47331 47331 47331 47331 47331
53 50 45791 45791 45791 45791 45791 45791 45791 45791 45791 45791 45791 45791 45791 45791 45791 45791
54 51 53491 53491 53491 53491 53491 53491 53491 53491 53491 53491 53491 53491 53491 53491 53491 53491
55 52 39631 39631 39631 39631 39631 39631 39631 39631 39631 39631 39631 39631 39631 39631 39631 39631
56 53 41171 41171 41171 41171 41171 41171 41171 41171 41171 41171 41171 41171 41171 41171 41171 41171
57 54 45791 46556 45791 45791 45791 46556 45791 45791 45791 46556 45791 45791 45791 46556 45791 45791
58 55 47331 47331 47331 48096 47331 47331 47331 48096 47331 47331 47331 48096 47331 47331 47331 48096
59 56 48871 49636 48871 48871 48871 49636 48871 48871 48871 49636 48871 48871 48871 49636 48871 48871
60 57 50411 50411 50411 51176 50411 50411 50411 51176 50411 50411 50411 51176 50411 50411 50411 51176
61 58 51951 51951 51951 52716 51951 51951 51951 52716 51951 51951 51951 52716 51951 51951 51951 52716
62 59 53491 53491 53491 54256 53491 53491 53491 54256 53491 53491 53491 54256 53491 53491 53491 54256
63 60 44251 44251 44251 45016 44251 44251 44251 45016 44251 44251 44251 45016 44251 44251 44251 45016
64 61 50411 51176 50411 50411 50411 51176 50411 50411 50411 51176 50411 50411 50411 51176 50411 50411
65 62 47331 48096 47331 47331 47331 48096 47331 47331 47331 48096 47331 47331 47331 48096 47331 47331
66 63 38091 38856 38091 38091 38091 38856 38091 38091 38091 38856 38091 38091 38091 38856 38091 38091
67 64 53491 54256 53491 53491 53491 54256 53491 53491 53491 54256 53491 53491 53491 54256 53491 53491

65
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/firetime_correction/Pandar64_Firetime Correction File.csv Normal file
View File

@ -0,0 +1,65 @@
Channel,fire time(μs)
51,3.62
61,3.62
45,4.924
60,4.924
39,6.228
57,6.228
9,7.532
55,7.532
49,8.836
63,8.836
43,10.14
59,10.14
7,11.444
56,11.444
53,12.748
64,12.748
47,14.052
62,14.052
41,15.356
58,15.356
6,16.66
54,16.66
5,17.964
48,17.964
4,19.268
50,19.268
3,20.572
52,20.572
2,21.876
46,21.876
1,23.18
44,23.18
24,25.148
33,25.148
27,27.116
42,27.116
21,29.084
36,29.084
15,31.052
30,31.052
22,33.02
37,33.02
16,34.988
31,34.988
10,36.956
25,36.956
19,38.924
34,38.924
13,40.892
28,40.892
20,42.86
35,42.86
14,44.828
29,44.828
8,46.796
23,46.796
17,48.764
32,48.764
11,50.732
26,50.732
18,52.7
38,52.7
12,54.668
40,54.668
1 Channel fire time(μs)
2 51 3.62
3 61 3.62
4 45 4.924
5 60 4.924
6 39 6.228
7 57 6.228
8 9 7.532
9 55 7.532
10 49 8.836
11 63 8.836
12 43 10.14
13 59 10.14
14 7 11.444
15 56 11.444
16 53 12.748
17 64 12.748
18 47 14.052
19 62 14.052
20 41 15.356
21 58 15.356
22 6 16.66
23 54 16.66
24 5 17.964
25 48 17.964
26 4 19.268
27 50 19.268
28 3 20.572
29 52 20.572
30 2 21.876
31 46 21.876
32 1 23.18
33 44 23.18
34 24 25.148
35 33 25.148
36 27 27.116
37 42 27.116
38 21 29.084
39 36 29.084
40 15 31.052
41 30 31.052
42 22 33.02
43 37 33.02
44 16 34.988
45 31 34.988
46 10 36.956
47 25 36.956
48 19 38.924
49 34 38.924
50 13 40.892
51 28 40.892
52 20 42.86
53 35 42.86
54 14 44.828
55 29 44.828
56 8 46.796
57 23 46.796
58 17 48.764
59 32 48.764
60 11 50.732
61 26 50.732
62 18 52.7
63 38 52.7
64 12 54.668
65 40 54.668

93
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/firetime_correction/Pandar90_Firetime Correction File.csv Normal file
View File

@ -0,0 +1,93 @@
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
operation mode,0,0,0,0,0,0,0,0,2,2,2,2,3,3,3,3
angle state,0,0,1,1,2,2,3,3,0,0,1,1,0,0,1,1
1,4436,5201,0,0,4436,4436,0,0,4436,5201,4436,4436,4436,5201,4436,4436
2,0,0,776,776,0,0,776,776,28554,28554,28554,28554,28554,28554,28554,28554
3,0,0,2781,4026,0,0,2431,2431,30559,31804,30209,30209,30559,31804,30209,30209
4,776,776,0,0,776,776,0,0,776,776,776,776,776,776,776,776
5,0,0,776,776,0,0,776,776,28554,28554,28554,28554,28554,28554,28554,28554
6,0,0,776,776,0,0,776,776,28554,28554,28554,28554,28554,28554,28554,28554
7,0,0,4786,4786,0,0,4086,4086,32564,32564,31864,31864,32564,32564,31864,31864
8,0,0,2781,2781,0,0,2431,2431,30559,30559,30209,30209,30559,30559,30209,30209
9,0,0,4786,4786,0,0,4086,4851,32564,32564,31864,32629,32564,32564,31864,32629
10,4436,4436,0,0,4436,4436,0,0,4436,4436,4436,4436,4436,4436,4436,4436
11,10381,10381,10731,12126,10381,10381,10031,10031,38509,39904,37809,37809,38509,39904,37809,37809
12,14951,14951,15301,15301,14951,14951,14601,14601,43079,43079,42379,42379,43079,43079,42379,42379
13,8096,8096,8446,8446,8096,8096,7746,7746,36224,36224,35524,35524,36224,36224,35524,35524
14,12666,12666,13016,13016,12666,12666,12316,12316,12666,12666,12666,12666,12666,12666,12666,12666
15,24091,24091,24441,24441,24091,24091,23741,23741,52219,52219,51519,51519,52219,52219,51519,51519
16,17236,17236,17586,17586,17236,17236,16886,16886,17236,17236,17236,17236,17236,17236,17236,17236
17,24091,24091,24441,24441,24091,24091,23741,23741,52219,52219,51519,51519,52219,52219,51519,51519
18,14951,14951,15301,15301,14951,14951,14601,14601,43079,43079,42379,42379,43079,43079,42379,42379
19,14951,27056,15301,15301,14951,14951,14601,14601,43079,27056,42379,42379,43079,27056,42379,42379
20,19521,19521,19871,19871,19521,19521,19171,19171,19521,19521,19521,19521,19521,19521,19521,19521
21,17236,17236,17586,17586,17236,17236,16886,16886,17236,17236,17236,17236,17236,17236,17236,17236
22,12666,12666,13016,13016,12666,12666,12316,12316,12666,12666,12666,12666,12666,12666,12666,12666
23,21806,21806,22156,22156,21806,21806,21456,21456,21806,21806,21806,21806,21806,21806,21806,21806
24,8096,8096,8446,8446,8096,8096,7746,7746,36224,36224,35524,35524,36224,36224,35524,35524
25,21806,21806,22156,22156,21806,21806,21456,21456,21806,21806,21806,21806,21806,21806,21806,21806
26,10381,10381,10731,27406,10381,10381,10031,10031,38509,55184,37809,37809,38509,55184,37809,37809
27,10381,10381,10731,10731,10381,10381,10031,10031,38509,38509,37809,37809,38509,38509,37809,37809
28,21806,21806,22156,22156,21806,21806,21456,21456,21806,21806,21806,21806,21806,21806,21806,21806
29,8096,8096,8446,8446,8096,8096,7746,7746,36224,36224,35524,35524,36224,36224,35524,35524
30,8096,8096,8446,8446,8096,8096,7746,7746,36224,36224,35524,35524,36224,36224,35524,35524
31,19521,19521,19871,19871,19521,19521,19171,19171,19521,19521,19521,19521,19521,19521,19521,19521
32,12666,12666,13016,13016,12666,12666,12316,12316,12666,12666,12666,12666,12666,12666,12666,12666
33,12666,12666,13016,13016,12666,27056,12316,12316,12666,12666,12666,27056,12666,12666,12666,27056
34,24091,24091,24441,24441,24091,24091,23741,23741,52219,52219,51519,51519,52219,52219,51519,51519
35,24091,24091,24441,24441,24091,24091,23741,23741,52219,52219,51519,51519,52219,52219,51519,51519
36,17236,17236,17586,17586,17236,17236,16886,16886,17236,17236,17236,17236,17236,17236,17236,17236
37,21806,21806,22156,22156,21806,21806,21456,21456,21806,21806,21806,21806,21806,21806,21806,21806
38,17236,17236,17586,17586,17236,17236,16886,16886,17236,17236,17236,17236,17236,17236,17236,17236
39,14951,14951,15301,15301,14951,14951,14601,14601,43079,43079,42379,42379,43079,43079,42379,42379
40,10381,10381,10731,10731,10381,10381,10031,26706,38509,38509,37809,54484,38509,38509,37809,54484
41,14951,14951,15301,15301,14951,14951,14601,14601,43079,43079,42379,42379,43079,43079,42379,42379
42,17236,17236,17586,17586,17236,17236,16886,16886,17236,17236,17236,17236,17236,17236,17236,17236
43,17236,17236,17586,17586,17236,17236,16886,16886,17236,17236,17236,17236,17236,17236,17236,17236
44,8096,8096,8446,8446,8096,8096,7746,7746,36224,36224,35524,35524,36224,36224,35524,35524
45,19521,19521,19871,19871,19521,19521,19171,19171,19521,19521,19521,19521,19521,19521,19521,19521
46,19521,19521,19871,19871,19521,19521,19171,19171,19521,19521,19521,19521,19521,19521,19521,19521
47,10381,10381,10731,10731,10381,10381,10031,11426,38509,38509,37809,39204,38509,38509,37809,39204
48,24091,24091,24441,24441,24091,24091,23741,23741,52219,52219,51519,51519,52219,52219,51519,51519
49,10381,10381,10731,10731,10381,10381,10031,10031,38509,38509,37809,37809,38509,38509,37809,37809
50,21806,21806,22156,22156,21806,21806,21456,21456,21806,21806,21806,21806,21806,21806,21806,21806
51,12666,12666,13016,13016,12666,12666,12316,12316,12666,12666,12666,12666,12666,12666,12666,12666
52,10381,10381,10731,10731,10381,10381,10031,10031,38509,38509,37809,37809,38509,38509,37809,37809
53,14951,14951,15301,15301,14951,14951,14601,14601,43079,43079,42379,42379,43079,43079,42379,42379
54,21806,23201,22156,22156,21806,21806,21456,21456,21806,23201,21806,21806,21806,23201,21806,21806
55,8096,8096,8446,8446,8096,8096,7746,7746,36224,36224,35524,35524,36224,36224,35524,35524
56,19521,19521,19871,19871,19521,19521,19171,19171,19521,19521,19521,19521,19521,19521,19521,19521
57,17236,17236,17586,17586,17236,17236,16886,16886,17236,17236,17236,17236,17236,17236,17236,17236
58,8096,8096,8446,8446,8096,8096,7746,7746,36224,36224,35524,35524,36224,36224,35524,35524
59,19521,19521,19871,19871,19521,19521,19171,19171,19521,19521,19521,19521,19521,19521,19521,19521
60,24091,24091,24441,24441,24091,24091,23741,25136,52219,52219,51519,52914,52219,52219,51519,52914
61,17236,17236,17586,17586,17236,17236,16886,16886,17236,17236,17236,17236,17236,17236,17236,17236
62,12666,12666,13016,13016,12666,12666,12316,12316,12666,12666,12666,12666,12666,12666,12666,12666
63,2431,3676,0,0,2781,2781,0,0,2431,3676,2781,2781,2431,3676,2781,2781
64,776,776,0,0,776,776,0,0,776,776,776,776,776,776,776,776
65,4436,4436,0,0,4436,4436,0,0,4436,4436,4436,4436,4436,4436,4436,4436
66,6441,6441,0,0,6091,6856,0,0,6441,6441,6091,6856,6441,6441,6091,6856
67,0,0,6441,6441,0,0,6091,6091,34219,34219,33869,33869,34219,34219,33869,33869
68,0,0,2781,2781,0,0,2431,2431,30559,30559,30209,30209,30559,30559,30209,30209
69,776,776,0,0,776,2021,0,0,776,776,776,2021,776,776,776,2021
70,0,0,776,776,0,0,776,776,28554,28554,28554,28554,28554,28554,28554,28554
71,2431,2431,0,0,2781,2781,0,0,2431,2431,2781,2781,2431,2431,2781,2781
72,2431,2431,0,0,2781,3546,0,0,2431,2431,2781,3546,2431,2431,2781,3546
73,4436,4436,0,0,4436,4436,0,0,4436,4436,4436,4436,4436,4436,4436,4436
74,0,0,4786,4786,0,0,4086,4086,32564,32564,31864,31864,32564,32564,31864,31864
75,0,0,776,2021,0,0,776,776,28554,29799,28554,28554,28554,29799,28554,28554
76,0,0,2781,2781,0,0,2431,2431,30559,30559,30209,30209,30559,30559,30209,30209
77,6441,6441,0,0,6091,6091,0,0,6441,6441,6091,6091,6441,6441,6091,6091
78,4436,5681,0,0,4436,4436,0,0,4436,5681,4436,4436,4436,5681,4436,4436
79,0,0,2781,2781,0,0,2431,2431,30559,30559,30209,30209,30559,30559,30209,30209
80,0,0,776,776,0,0,776,776,28554,28554,28554,28554,28554,28554,28554,28554
81,0,0,4786,4786,0,0,4086,5331,32564,32564,31864,33109,32564,32564,31864,33109
82,6441,6441,0,0,6091,6091,0,0,6441,6441,6091,6091,6441,6441,6091,6091
83,0,0,6441,7686,0,0,6091,6091,34219,35464,33869,33869,34219,35464,33869,33869
84,776,776,0,0,776,776,0,0,776,776,776,776,776,776,776,776
85,0,0,4786,4786,0,0,4086,4086,32564,32564,31864,31864,32564,32564,31864,31864
86,0,0,6441,6441,0,0,6091,6091,34219,34219,33869,33869,34219,34219,33869,33869
87,4436,4436,0,0,4436,4436,0,0,4436,4436,4436,4436,4436,4436,4436,4436
88,0,0,2781,2781,0,0,2431,2431,30559,30559,30209,30209,30559,30559,30209,30209
89,6441,6441,0,0,6091,6091,0,0,6441,6441,6091,6091,6441,6441,6091,6091
90,0,0,776,776,0,0,776,1541,28554,28554,28554,29319,28554,28554,28554,29319
1 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
2 operation mode 0 0 0 0 0 0 0 0 2 2 2 2 3 3 3 3
3 angle state 0 0 1 1 2 2 3 3 0 0 1 1 0 0 1 1
4 1 4436 5201 0 0 4436 4436 0 0 4436 5201 4436 4436 4436 5201 4436 4436
5 2 0 0 776 776 0 0 776 776 28554 28554 28554 28554 28554 28554 28554 28554
6 3 0 0 2781 4026 0 0 2431 2431 30559 31804 30209 30209 30559 31804 30209 30209
7 4 776 776 0 0 776 776 0 0 776 776 776 776 776 776 776 776
8 5 0 0 776 776 0 0 776 776 28554 28554 28554 28554 28554 28554 28554 28554
9 6 0 0 776 776 0 0 776 776 28554 28554 28554 28554 28554 28554 28554 28554
10 7 0 0 4786 4786 0 0 4086 4086 32564 32564 31864 31864 32564 32564 31864 31864
11 8 0 0 2781 2781 0 0 2431 2431 30559 30559 30209 30209 30559 30559 30209 30209
12 9 0 0 4786 4786 0 0 4086 4851 32564 32564 31864 32629 32564 32564 31864 32629
13 10 4436 4436 0 0 4436 4436 0 0 4436 4436 4436 4436 4436 4436 4436 4436
14 11 10381 10381 10731 12126 10381 10381 10031 10031 38509 39904 37809 37809 38509 39904 37809 37809
15 12 14951 14951 15301 15301 14951 14951 14601 14601 43079 43079 42379 42379 43079 43079 42379 42379
16 13 8096 8096 8446 8446 8096 8096 7746 7746 36224 36224 35524 35524 36224 36224 35524 35524
17 14 12666 12666 13016 13016 12666 12666 12316 12316 12666 12666 12666 12666 12666 12666 12666 12666
18 15 24091 24091 24441 24441 24091 24091 23741 23741 52219 52219 51519 51519 52219 52219 51519 51519
19 16 17236 17236 17586 17586 17236 17236 16886 16886 17236 17236 17236 17236 17236 17236 17236 17236
20 17 24091 24091 24441 24441 24091 24091 23741 23741 52219 52219 51519 51519 52219 52219 51519 51519
21 18 14951 14951 15301 15301 14951 14951 14601 14601 43079 43079 42379 42379 43079 43079 42379 42379
22 19 14951 27056 15301 15301 14951 14951 14601 14601 43079 27056 42379 42379 43079 27056 42379 42379
23 20 19521 19521 19871 19871 19521 19521 19171 19171 19521 19521 19521 19521 19521 19521 19521 19521
24 21 17236 17236 17586 17586 17236 17236 16886 16886 17236 17236 17236 17236 17236 17236 17236 17236
25 22 12666 12666 13016 13016 12666 12666 12316 12316 12666 12666 12666 12666 12666 12666 12666 12666
26 23 21806 21806 22156 22156 21806 21806 21456 21456 21806 21806 21806 21806 21806 21806 21806 21806
27 24 8096 8096 8446 8446 8096 8096 7746 7746 36224 36224 35524 35524 36224 36224 35524 35524
28 25 21806 21806 22156 22156 21806 21806 21456 21456 21806 21806 21806 21806 21806 21806 21806 21806
29 26 10381 10381 10731 27406 10381 10381 10031 10031 38509 55184 37809 37809 38509 55184 37809 37809
30 27 10381 10381 10731 10731 10381 10381 10031 10031 38509 38509 37809 37809 38509 38509 37809 37809
31 28 21806 21806 22156 22156 21806 21806 21456 21456 21806 21806 21806 21806 21806 21806 21806 21806
32 29 8096 8096 8446 8446 8096 8096 7746 7746 36224 36224 35524 35524 36224 36224 35524 35524
33 30 8096 8096 8446 8446 8096 8096 7746 7746 36224 36224 35524 35524 36224 36224 35524 35524
34 31 19521 19521 19871 19871 19521 19521 19171 19171 19521 19521 19521 19521 19521 19521 19521 19521
35 32 12666 12666 13016 13016 12666 12666 12316 12316 12666 12666 12666 12666 12666 12666 12666 12666
36 33 12666 12666 13016 13016 12666 27056 12316 12316 12666 12666 12666 27056 12666 12666 12666 27056
37 34 24091 24091 24441 24441 24091 24091 23741 23741 52219 52219 51519 51519 52219 52219 51519 51519
38 35 24091 24091 24441 24441 24091 24091 23741 23741 52219 52219 51519 51519 52219 52219 51519 51519
39 36 17236 17236 17586 17586 17236 17236 16886 16886 17236 17236 17236 17236 17236 17236 17236 17236
40 37 21806 21806 22156 22156 21806 21806 21456 21456 21806 21806 21806 21806 21806 21806 21806 21806
41 38 17236 17236 17586 17586 17236 17236 16886 16886 17236 17236 17236 17236 17236 17236 17236 17236
42 39 14951 14951 15301 15301 14951 14951 14601 14601 43079 43079 42379 42379 43079 43079 42379 42379
43 40 10381 10381 10731 10731 10381 10381 10031 26706 38509 38509 37809 54484 38509 38509 37809 54484
44 41 14951 14951 15301 15301 14951 14951 14601 14601 43079 43079 42379 42379 43079 43079 42379 42379
45 42 17236 17236 17586 17586 17236 17236 16886 16886 17236 17236 17236 17236 17236 17236 17236 17236
46 43 17236 17236 17586 17586 17236 17236 16886 16886 17236 17236 17236 17236 17236 17236 17236 17236
47 44 8096 8096 8446 8446 8096 8096 7746 7746 36224 36224 35524 35524 36224 36224 35524 35524
48 45 19521 19521 19871 19871 19521 19521 19171 19171 19521 19521 19521 19521 19521 19521 19521 19521
49 46 19521 19521 19871 19871 19521 19521 19171 19171 19521 19521 19521 19521 19521 19521 19521 19521
50 47 10381 10381 10731 10731 10381 10381 10031 11426 38509 38509 37809 39204 38509 38509 37809 39204
51 48 24091 24091 24441 24441 24091 24091 23741 23741 52219 52219 51519 51519 52219 52219 51519 51519
52 49 10381 10381 10731 10731 10381 10381 10031 10031 38509 38509 37809 37809 38509 38509 37809 37809
53 50 21806 21806 22156 22156 21806 21806 21456 21456 21806 21806 21806 21806 21806 21806 21806 21806
54 51 12666 12666 13016 13016 12666 12666 12316 12316 12666 12666 12666 12666 12666 12666 12666 12666
55 52 10381 10381 10731 10731 10381 10381 10031 10031 38509 38509 37809 37809 38509 38509 37809 37809
56 53 14951 14951 15301 15301 14951 14951 14601 14601 43079 43079 42379 42379 43079 43079 42379 42379
57 54 21806 23201 22156 22156 21806 21806 21456 21456 21806 23201 21806 21806 21806 23201 21806 21806
58 55 8096 8096 8446 8446 8096 8096 7746 7746 36224 36224 35524 35524 36224 36224 35524 35524
59 56 19521 19521 19871 19871 19521 19521 19171 19171 19521 19521 19521 19521 19521 19521 19521 19521
60 57 17236 17236 17586 17586 17236 17236 16886 16886 17236 17236 17236 17236 17236 17236 17236 17236
61 58 8096 8096 8446 8446 8096 8096 7746 7746 36224 36224 35524 35524 36224 36224 35524 35524
62 59 19521 19521 19871 19871 19521 19521 19171 19171 19521 19521 19521 19521 19521 19521 19521 19521
63 60 24091 24091 24441 24441 24091 24091 23741 25136 52219 52219 51519 52914 52219 52219 51519 52914
64 61 17236 17236 17586 17586 17236 17236 16886 16886 17236 17236 17236 17236 17236 17236 17236 17236
65 62 12666 12666 13016 13016 12666 12666 12316 12316 12666 12666 12666 12666 12666 12666 12666 12666
66 63 2431 3676 0 0 2781 2781 0 0 2431 3676 2781 2781 2431 3676 2781 2781
67 64 776 776 0 0 776 776 0 0 776 776 776 776 776 776 776 776
68 65 4436 4436 0 0 4436 4436 0 0 4436 4436 4436 4436 4436 4436 4436 4436
69 66 6441 6441 0 0 6091 6856 0 0 6441 6441 6091 6856 6441 6441 6091 6856
70 67 0 0 6441 6441 0 0 6091 6091 34219 34219 33869 33869 34219 34219 33869 33869
71 68 0 0 2781 2781 0 0 2431 2431 30559 30559 30209 30209 30559 30559 30209 30209
72 69 776 776 0 0 776 2021 0 0 776 776 776 2021 776 776 776 2021
73 70 0 0 776 776 0 0 776 776 28554 28554 28554 28554 28554 28554 28554 28554
74 71 2431 2431 0 0 2781 2781 0 0 2431 2431 2781 2781 2431 2431 2781 2781
75 72 2431 2431 0 0 2781 3546 0 0 2431 2431 2781 3546 2431 2431 2781 3546
76 73 4436 4436 0 0 4436 4436 0 0 4436 4436 4436 4436 4436 4436 4436 4436
77 74 0 0 4786 4786 0 0 4086 4086 32564 32564 31864 31864 32564 32564 31864 31864
78 75 0 0 776 2021 0 0 776 776 28554 29799 28554 28554 28554 29799 28554 28554
79 76 0 0 2781 2781 0 0 2431 2431 30559 30559 30209 30209 30559 30559 30209 30209
80 77 6441 6441 0 0 6091 6091 0 0 6441 6441 6091 6091 6441 6441 6091 6091
81 78 4436 5681 0 0 4436 4436 0 0 4436 5681 4436 4436 4436 5681 4436 4436
82 79 0 0 2781 2781 0 0 2431 2431 30559 30559 30209 30209 30559 30559 30209 30209
83 80 0 0 776 776 0 0 776 776 28554 28554 28554 28554 28554 28554 28554 28554
84 81 0 0 4786 4786 0 0 4086 5331 32564 32564 31864 33109 32564 32564 31864 33109
85 82 6441 6441 0 0 6091 6091 0 0 6441 6441 6091 6091 6441 6441 6091 6091
86 83 0 0 6441 7686 0 0 6091 6091 34219 35464 33869 33869 34219 35464 33869 33869
87 84 776 776 0 0 776 776 0 0 776 776 776 776 776 776 776 776
88 85 0 0 4786 4786 0 0 4086 4086 32564 32564 31864 31864 32564 32564 31864 31864
89 86 0 0 6441 6441 0 0 6091 6091 34219 34219 33869 33869 34219 34219 33869 33869
90 87 4436 4436 0 0 4436 4436 0 0 4436 4436 4436 4436 4436 4436 4436 4436
91 88 0 0 2781 2781 0 0 2431 2431 30559 30559 30209 30209 30559 30559 30209 30209
92 89 6441 6441 0 0 6091 6091 0 0 6441 6441 6091 6091 6441 6441 6091 6091
93 90 0 0 776 776 0 0 776 1541 28554 28554 28554 29319 28554 28554 28554 29319

17
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/firetime_correction/PandarXT-16_Firetime Correction File.csv Normal file
View File

@ -0,0 +1,17 @@
Channel,fire time(us)
1,3.56
2,6.584
3,9.608
4,12.632
5,15.656
6,18.68
7,21.704
8,24.728
9,27.752
10,30.776
11,33.8
12,36.824
13,39.848
14,42.872
15,45.896
16,48.92
1 Channel fire time(us)
2 1 3.56
3 2 6.584
4 3 9.608
5 4 12.632
6 5 15.656
7 6 18.68
8 7 21.704
9 8 24.728
10 9 27.752
11 10 30.776
12 11 33.8
13 12 36.824
14 13 39.848
15 14 42.872
16 15 45.896
17 16 48.92

33
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/firetime_correction/PandarXT_Firetime Correction File.csv Normal file
View File

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

131
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/correction/firetime_correction/QT128C2X_Firetime Correction File.csv Normal file
View File

@ -0,0 +1,131 @@
EEFF,1,1,
Horizontal Resolution Mode,1,LoopNum,2
Loop1,Firetime1,Loop2,Firetime2
99,0.6,65,0.6
65,1.456,99,1.456
35,2.312,1,2.312
102,3.768,72,3.768
72,4.624,102,4.624
38,5.48,8,5.48
107,6.936,73,6.936
73,7.792,107,7.792
43,8.648,9,8.648
110,10.104,80,10.104
80,10.96,110,10.96
46,11.816,16,11.816
115,13.272,81,13.272
81,14.128,115,14.128
51,14.984,17,14.984
118,16.44,88,16.44
88,17.296,118,17.296
54,18.152,24,18.152
123,19.608,89,19.608
89,20.464,123,20.464
59,21.32,25,21.32
126,22.776,96,22.776
96,23.632,126,23.632
62,24.488,32,24.488
97,25.944,67,25.944
67,26.8,97,26.8
33,27.656,3,27.656
104,29.112,70,29.112
70,29.968,104,29.968
40,30.824,6,30.824
105,32.28,75,32.28
75,33.136,105,33.136
41,33.992,11,33.992
112,35.448,78,35.448
78,36.304,112,36.304
48,37.16,14,37.16
113,38.616,83,38.616
83,39.472,113,39.472
49,40.328,19,40.328
120,41.784,86,41.784
86,42.64,120,42.64
56,43.496,22,43.496
121,44.952,91,44.952
91,45.808,121,45.808
57,46.664,27,46.664
128,48.12,94,48.12
94,48.976,128,48.976
64,49.832,30,49.832
98,51.288,68,51.288
68,52.144,98,52.144
34,53,4,53
103,54.456,69,54.456
69,55.312,103,55.312
39,56.168,5,56.168
106,57.624,76,57.624
76,58.48,106,58.48
42,59.336,12,59.336
111,60.792,77,60.792
77,61.648,111,61.648
47,62.504,13,62.504
114,63.96,84,63.96
84,64.816,114,64.816
50,65.672,20,65.672
119,67.128,85,67.128
85,67.984,119,67.984
55,68.84,21,68.84
122,70.296,92,70.296
92,71.152,122,71.152
58,72.008,28,72.008
127,73.464,93,73.464
93,74.32,127,74.32
63,75.176,29,75.176
100,76.632,66,76.632
66,77.488,100,77.488
36,78.344,2,78.344
101,79.8,71,79.8
71,80.656,101,80.656
37,81.512,7,81.512
108,82.968,74,82.968
74,83.824,108,83.824
44,84.68,10,84.68
109,86.136,79,86.136
79,86.992,109,86.992
45,87.848,15,87.848
116,89.304,82,89.304
82,90.16,116,90.16
52,91.016,18,91.016
117,92.472,87,92.472
87,93.328,117,93.328
53,94.184,23,94.184
124,95.64,90,95.64
90,96.496,124,96.496
60,97.352,26,97.352
125,98.808,95,98.808
95,99.664,125,99.664
61,100.52,31,100.52
0,0,0,0
0,0,0,0
0,0,0,0
0,0,0,0
0,0,0,0
0,0,0,0
0,0,0,0
0,0,0,0
0,0,0,0
0,0,0,0
0,0,0,0
0,0,0,0
0,0,0,0
0,0,0,0
0,0,0,0
0,0,0,0
0,0,0,0
0,0,0,0
0,0,0,0
0,0,0,0
0,0,0,0
0,0,0,0
0,0,0,0
0,0,0,0
0,0,0,0
0,0,0,0
0,0,0,0
0,0,0,0
0,0,0,0
0,0,0,0
0,0,0,0
0,0,0,0
1 EEFF 1 1
2 Horizontal Resolution Mode 1 LoopNum 2
3 Loop1 Firetime1 Loop2 Firetime2
4 99 0.6 65 0.6
5 65 1.456 99 1.456
6 35 2.312 1 2.312
7 102 3.768 72 3.768
8 72 4.624 102 4.624
9 38 5.48 8 5.48
10 107 6.936 73 6.936
11 73 7.792 107 7.792
12 43 8.648 9 8.648
13 110 10.104 80 10.104
14 80 10.96 110 10.96
15 46 11.816 16 11.816
16 115 13.272 81 13.272
17 81 14.128 115 14.128
18 51 14.984 17 14.984
19 118 16.44 88 16.44
20 88 17.296 118 17.296
21 54 18.152 24 18.152
22 123 19.608 89 19.608
23 89 20.464 123 20.464
24 59 21.32 25 21.32
25 126 22.776 96 22.776
26 96 23.632 126 23.632
27 62 24.488 32 24.488
28 97 25.944 67 25.944
29 67 26.8 97 26.8
30 33 27.656 3 27.656
31 104 29.112 70 29.112
32 70 29.968 104 29.968
33 40 30.824 6 30.824
34 105 32.28 75 32.28
35 75 33.136 105 33.136
36 41 33.992 11 33.992
37 112 35.448 78 35.448
38 78 36.304 112 36.304
39 48 37.16 14 37.16
40 113 38.616 83 38.616
41 83 39.472 113 39.472
42 49 40.328 19 40.328
43 120 41.784 86 41.784
44 86 42.64 120 42.64
45 56 43.496 22 43.496
46 121 44.952 91 44.952
47 91 45.808 121 45.808
48 57 46.664 27 46.664
49 128 48.12 94 48.12
50 94 48.976 128 48.976
51 64 49.832 30 49.832
52 98 51.288 68 51.288
53 68 52.144 98 52.144
54 34 53 4 53
55 103 54.456 69 54.456
56 69 55.312 103 55.312
57 39 56.168 5 56.168
58 106 57.624 76 57.624
59 76 58.48 106 58.48
60 42 59.336 12 59.336
61 111 60.792 77 60.792
62 77 61.648 111 61.648
63 47 62.504 13 62.504
64 114 63.96 84 63.96
65 84 64.816 114 64.816
66 50 65.672 20 65.672
67 119 67.128 85 67.128
68 85 67.984 119 67.984
69 55 68.84 21 68.84
70 122 70.296 92 70.296
71 92 71.152 122 71.152
72 58 72.008 28 72.008
73 127 73.464 93 73.464
74 93 74.32 127 74.32
75 63 75.176 29 75.176
76 100 76.632 66 76.632
77 66 77.488 100 77.488
78 36 78.344 2 78.344
79 101 79.8 71 79.8
80 71 80.656 101 80.656
81 37 81.512 7 81.512
82 108 82.968 74 82.968
83 74 83.824 108 83.824
84 44 84.68 10 84.68
85 109 86.136 79 86.136
86 79 86.992 109 86.992
87 45 87.848 15 87.848
88 116 89.304 82 89.304
89 82 90.16 116 90.16
90 52 91.016 18 91.016
91 117 92.472 87 92.472
92 87 93.328 117 93.328
93 53 94.184 23 94.184
94 124 95.64 90 95.64
95 90 96.496 124 96.496
96 60 97.352 26 97.352
97 125 98.808 95 98.808
98 95 99.664 125 99.664
99 61 100.52 31 100.52
100 0 0 0 0
101 0 0 0 0
102 0 0 0 0
103 0 0 0 0
104 0 0 0 0
105 0 0 0 0
106 0 0 0 0
107 0 0 0 0
108 0 0 0 0
109 0 0 0 0
110 0 0 0 0
111 0 0 0 0
112 0 0 0 0
113 0 0 0 0
114 0 0 0 0
115 0 0 0 0
116 0 0 0 0
117 0 0 0 0
118 0 0 0 0
119 0 0 0 0
120 0 0 0 0
121 0 0 0 0
122 0 0 0 0
123 0 0 0 0
124 0 0 0 0
125 0 0 0 0
126 0 0 0 0
127 0 0 0 0
128 0 0 0 0
129 0 0 0 0
130 0 0 0 0
131 0 0 0 0

191
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/driver/hesai_lidar_sdk.hpp Normal file
View File

@ -0,0 +1,191 @@
/************************************************************************************************
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 "fault_message.h"
namespace hesai
{
namespace lidar
{
template <typename T_Point>
class HesaiLidarSdk
{
private:
boost::thread *runing_thread_ptr_;
std::function<void(const UdpFrame_t&, double)> pkt_cb_;
std::function<void(const LidarDecodedFrame<T_Point>&)> 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<T_Point> *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<T_Point>;
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<T_Point> 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<void(const LidarDecodedFrame<T_Point>&)>& callback) {
point_cloud_cb_ = callback;
}
// assign callback fuction
void RegRecvCallback(const std::function<void(const UdpFrame_t&, double)>& 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

269
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/driver/hesai_lidar_sdk_gpu.cuh Normal file
View File

@ -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 <typename T_Point>
class HesaiLidarSdkGpu
{
private:
boost::thread *runing_thread_ptr_;
std::function<void(const UdpFrame_t &, double)> pkt_cb_;
std::function<void(const LidarDecodedFrame<T_Point> &)> 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<T_Point> *lidar_ptr_;
UdpParserGpu<T_Point> *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 &param)
{
/*****************************Init decoder******************************************************/
lidar_ptr_ = new Lidar<T_Point>;
gpu_parser_ptr_ = new UdpParserGpu<T_Point>;
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<T_Point> decoded_packet;
LidarDecodedFrame<T_Point> 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<void(const LidarDecodedFrame<T_Point> &)>& callback)
{
point_cloud_cb_ = callback;
}
// assign callback fuction
void RegRecvCallback(const std::function<void(const UdpFrame_t &, double)>& 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

132
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/CMakeLists.txt Executable file
View File

@ -0,0 +1,132 @@
# ----------------------------------------#
# Hesai Tech <http://www.hesaitech.com> #
# ----------------------------------------#
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})

43
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Container/include/blocking_ring.h Normal file
View File

@ -0,0 +1,43 @@
#ifndef ___HESAI__CONTAINER__BLOCKING_RING_HH___
#define ___HESAI__CONTAINER__BLOCKING_RING_HH___
#include "ring.h"
#include <mutex>
#include <condition_variable>
#include <algorithm>
#include <chrono>
#include <functional>
namespace hesai
{
namespace lidar
{
template <typename T, size_t N>
class BlockingRing : public Ring<T, N>{
public:
using Super = Ring<T, N>;
using Mutex = std::mutex;
using Condv = std::condition_variable;
using LockS = std::lock_guard<Mutex>; // lock in scope
using LockC = std::unique_lock<Mutex>; // lock by condition
private:
Mutex _mutex;
Condv _condv;
public:
template <typename... Args>
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___

59
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Container/include/ring.h Normal file
View File

@ -0,0 +1,59 @@
#ifndef ___HESAI__CONTAINER__RING_HH___
#define ___HESAI__CONTAINER__RING_HH___
#include <memory>
#include <array>
#include <cassert>
#include <iterator>
namespace hesai
{
namespace lidar
{
template <typename T, size_t N>
class Ring {
public:
static constexpr size_t Size = N;
template <typename Item> class ItemIterator;
using iterator = ItemIterator<T>;
using const_iterator = ItemIterator<const T>;
private:
std::unique_ptr<std::array<T, N>> _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 <typename... Args>
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 <typename... Args>
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___

67
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Container/include/ring_2d_shared.h Normal file
View File

@ -0,0 +1,67 @@
#ifndef ___HESAI__CONTAINER__RING2D_SHARED_HH___
#define ___HESAI__CONTAINER__RING2D_SHARED_HH___
#include <memory>
#include <array>
#include <boost/interprocess/mapped_region.hpp>
namespace hesai
{
namespace lidar
{
template <typename T, size_t N, typename T2, size_t M>
class Ring2D_shared {
public:
template <typename Item> class ItemIterator;
using iterator = ItemIterator<T>;
using const_iterator = ItemIterator<const T>;
private:
std::unique_ptr<std::array<T, N>> _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 <typename... Args>
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 <typename... Args>
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___

64
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Container/include/ring_2dex.h Normal file
View File

@ -0,0 +1,64 @@
#ifndef ___HESAI__CONTAINER__RING2D_EX_HH___
#define ___HESAI__CONTAINER__RING2D_EX_HH___
#include <memory>
#include <array>
namespace hesai
{
namespace lidar
{
template <typename T, size_t N, typename T2, size_t M>
class Ring2D_ex {
public:
template <typename Item> class ItemIterator;
using iterator = ItemIterator<T>;
using const_iterator = ItemIterator<const T>;
private:
std::unique_ptr<std::array<T, N>> _ring;
std::unique_ptr<std::array<T2, N * M>> _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 <typename... Args>
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 <typename... Args>
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___

90
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Container/src/blocking_ring.cc Normal file
View File

@ -0,0 +1,90 @@
#include "blocking_ring.h"
#include <functional>
using namespace hesai::lidar;
template <typename T, size_t N>
void BlockingRing<T, N>::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 <typename T, size_t N>
template <typename... Args>
void BlockingRing<T, N>::emplace_back(Args&&... args) {
LockC lock(_mutex);
_condv.wait(lock, std::bind(&Super::not_full, this));
Super::emplace_back(args...);
_condv.notify_one();
}
template <typename T, size_t N>
void BlockingRing<T, N>::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 <typename T, size_t N>
T BlockingRing<T, N>::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 <typename T, size_t N>
bool BlockingRing<T, N>::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 <typename T, size_t N>
T BlockingRing<T, N>::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 <typename T, size_t N>
bool BlockingRing<T, N>::empty()
{
LockS lock(_mutex);
return Super::empty();
}
template <typename T, size_t N>
bool BlockingRing<T, N>::not_empty()
{
LockS lock(_mutex);
return Super::not_empty();
}
template <typename T, size_t N>
bool BlockingRing<T, N>::full()
{
LockS lock(_mutex);
return Super::full();
}
template <typename T, size_t N>
bool BlockingRing<T, N>::not_full()
{
LockS lock(_mutex);
return Super::not_full();
}

120
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Container/src/ring.cc Normal file
View File

@ -0,0 +1,120 @@
#include "ring.h"
#include <cassert>
#include <iterator>
using namespace hesai::lidar;
template <typename T, size_t N>
template <typename Item>
class Ring<T, N>::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 <typename T, size_t N>
Ring<T, N>::Ring() : _ring(new std::array<T, N>), _begin(0), _size(0) {};
template <typename T, size_t N>
inline constexpr size_t Ring<T, N>::size() const { return _size; }
template <typename T, size_t N>
inline bool Ring<T, N>::empty() const { return _size == 0; }
template <typename T, size_t N>
inline bool Ring<T, N>::not_empty() const { return !empty(); }
template <typename T, size_t N>
inline bool Ring<T, N>::full() const { return _size >= N; }
template <typename T, size_t N>
inline bool Ring<T, N>::not_full() const { return !full(); }
template <typename T, size_t N>
inline void Ring<T, N>::clear() { for (auto& item : *this) { item = T(); } _size = 0; _begin = 0; }
template <typename T, size_t N>
inline void Ring<T, N>::eff_clear() { _size = 0; _begin = 0; }
template <typename T, size_t N>
template <typename... Args>
inline void Ring<T, N>::emplace_back(Args&&... args) { assert(!full()); (*_ring)[(_begin + _size) % N] = T(args...); ++_size;}
template <typename T, size_t N>
inline void Ring<T, N>::push_back(T&& item) { assert(!full()); (*_ring)[(_begin + _size) % N] = item; ++_size; }
template <typename T, size_t N>
inline const T& Ring<T, N>::peek_back() const { assert(!empty()); return (*_ring)[(_begin + _size - 1) % N]; }
template <typename T, size_t N>
inline T Ring<T, N>::pop_back() { assert(!empty()); --_size; T item; std::swap(item, (*_ring)[(_begin + _size) % N]); return item; }
template <typename T, size_t N>
inline void Ring<T, N>::eff_pop_back() { --_size; }
template <typename T, size_t N>
template <typename... Args>
inline void Ring<T, N>::emplace_front(Args&&... args) { assert(!full()); _begin = (_begin + N - 1) % N; (*_ring)[_begin] = T(args...); ++_size; }
template <typename T, size_t N>
inline void Ring<T, N>::push_front(T&& item) { assert(!full()); _begin = (_begin + N - 1) % N; (*_ring)[_begin] = item; ++_size; }
template <typename T, size_t N>
inline const T& Ring<T, N>::peek_front() const { assert(!empty()); return (*_ring)[_begin]; }
template <typename T, size_t N>
inline T Ring<T, N>::pop_front() { assert(!empty()); --_size; T item; std::swap(item, (*_ring)[_begin]); _begin = (_begin + 1) % N; return item; }
template <typename T, size_t N>
inline void Ring<T, N>::eff_pop_front() { --_size; _begin = (_begin + 1) % N; }
template <typename T, size_t N>
inline typename Ring<T, N>::iterator Ring<T, N>::begin() { return iterator((T*)_ring.get(), _begin); }
template <typename T, size_t N>
inline typename Ring<T, N>::iterator Ring<T, N>::end() { return iterator((T*)_ring.get(), _begin + _size); }
template <typename T, size_t N>
inline typename Ring<T, N>::const_iterator Ring<T, N>::cbegin() const { return const_iterator((const T*)_ring.get(), _begin); }
template <typename T, size_t N>
inline typename Ring<T, N>::const_iterator Ring<T, N>::cend() const { return const_iterator((const T*)_ring.get(), _begin + _size); }
template <typename T, size_t N>
inline T& Ring<T, N>::operator[](size_t index) { assert(index < size()); return (*_ring)[(_begin + index) % N]; }
template <typename T, size_t N>
inline const T& Ring<T, N>::operator[](size_t index) const { assert(index < size()); return (*_ring)[(_begin + index) % N]; }
template <typename T, size_t N>
inline T* Ring<T, N>::data() { return _ring->data(); }
template <typename T, size_t N>
inline const T* Ring<T, N>::data() const { return _ring->data(); };

251
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Container/src/ring_2d_shared.cc Normal file
View File

@ -0,0 +1,251 @@
#include "ring_2d_shared.h"
#include <cassert>
#include <iterator>
#include <iostream>
#include <boost/interprocess/shared_memory_object.hpp>
#if _MSC_VER
#include <boost/interprocess/windows_shared_memory.hpp>
# else
#include <stdio.h>
#include <unistd.h>
#include <sys/ipc.h>
#include <sys/shm.h>
#include <stdlib.h>
#include <errno.h>
#endif
using namespace hesai::lidar;
template <typename T, size_t N, typename T2, size_t M>
template <typename Item>
class Ring2D_shared<T, N, T2, M>::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 <typename T, size_t N, typename T2, size_t M>
Ring2D_shared<T, N, T2, M>::Ring2D_shared() : _ring(new std::array<T, N>), _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 <typename T, size_t N, typename T2, size_t M>
Ring2D_shared<T, N, T2, M>::~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 <typename T, size_t N, typename T2, size_t M>
inline constexpr size_t Ring2D_shared<T, N, T2, M>::size() const { return _size; }
template <typename T, size_t N, typename T2, size_t M>
inline bool Ring2D_shared<T, N, T2, M>::empty() const { return _size == 0; }
template <typename T, size_t N, typename T2, size_t M>
inline bool Ring2D_shared<T, N, T2, M>::not_empty() const { return !empty(); }
template <typename T, size_t N, typename T2, size_t M>
inline bool Ring2D_shared<T, N, T2, M>::full() const { return _size >= N; }
template <typename T, size_t N, typename T2, size_t M>
inline bool Ring2D_shared<T, N, T2, M>::not_full() const { return !full(); }
template <typename T, size_t N, typename T2, size_t M>
inline void Ring2D_shared<T, N, T2, M>::clear() { for (auto& item : *this) { item = T(); } _size = 0; _begin = 0; }
template <typename T, size_t N, typename T2, size_t M>
inline void Ring2D_shared<T, N, T2, M>::eff_clear() { _size = 0; _begin = 0; }
template <typename T, size_t N, typename T2, size_t M>
template <typename... Args>
inline void Ring2D_shared<T, N, T2, M>::emplace_back(Args&&... args) { assert(!full()); (*_ring)[(_begin + _size) % N] = T(args...); ++_size;}
template <typename T, size_t N, typename T2, size_t M>
inline void Ring2D_shared<T, N, T2, M>::push_back(T&& item) { assert(!full()); (*_ring)[(_begin + _size) % N] = item; ++_size; }
template <typename T, size_t N, typename T2, size_t M>
inline const T& Ring2D_shared<T, N, T2, M>::peek_back() const { assert(!empty()); return (*_ring)[(_begin + _size - 1) % N]; }
template <typename T, size_t N, typename T2, size_t M>
inline T Ring2D_shared<T, N, T2, M>::pop_back() { assert(!empty()); --_size; T item; std::swap(item, (*_ring)[(_begin + _size) % N]); return item; }
template <typename T, size_t N, typename T2, size_t M>
inline void Ring2D_shared<T, N, T2, M>::eff_pop_back() { --_size; }
template <typename T, size_t N, typename T2, size_t M>
template <typename... Args>
inline void Ring2D_shared<T, N, T2, M>::emplace_front(Args&&... args) { assert(!full()); _begin = (_begin + N - 1) % N; (*_ring)[_begin] = T(args...); ++_size; }
template <typename T, size_t N, typename T2, size_t M>
inline void Ring2D_shared<T, N, T2, M>::push_front(T&& item) { assert(!full()); _begin = (_begin + N - 1) % N; (*_ring)[_begin] = item; ++_size; }
template <typename T, size_t N, typename T2, size_t M>
inline const T& Ring2D_shared<T, N, T2, M>::peek_front() const { assert(!empty()); return (*_ring)[_begin]; }
template <typename T, size_t N, typename T2, size_t M>
inline T Ring2D_shared<T, N, T2, M>::pop_front() { assert(!empty()); --_size; T item; std::swap(item, (*_ring)[_begin]); _begin = (_begin + 1) % N; return item; }
template <typename T, size_t N, typename T2, size_t M>
inline void Ring2D_shared<T, N, T2, M>::eff_pop_front() { --_size; _begin = (_begin + 1) % N; }
template <typename T, size_t N, typename T2, size_t M>
inline typename Ring2D_shared<T, N, T2, M>::iterator Ring2D_shared<T, N, T2, M>::begin() { return iterator((T*)_ring.get(), _begin); }
template <typename T, size_t N, typename T2, size_t M>
inline typename Ring2D_shared<T, N, T2, M>::iterator Ring2D_shared<T, N, T2, M>::end() { return iterator((T*)_ring.get(), _begin + _size); }
template <typename T, size_t N, typename T2, size_t M>
inline typename Ring2D_shared<T, N, T2, M>::const_iterator Ring2D_shared<T, N, T2, M>::cbegin() const { return const_iterator((const T*)_ring.get(), _begin); }
template <typename T, size_t N, typename T2, size_t M>
inline typename Ring2D_shared<T, N, T2, M>::const_iterator Ring2D_shared<T, N, T2, M>::cend() const { return const_iterator((const T*)_ring.get(), _begin + _size); }
template <typename T, size_t N, typename T2, size_t M>
inline T& Ring2D_shared<T, N, T2, M>::operator[](size_t index) { assert(index < size()); return (*_ring)[(_begin + index) % N]; }
template <typename T, size_t N, typename T2, size_t M>
inline const T& Ring2D_shared<T, N, T2, M>::operator[](size_t index) const { assert(index < size()); return (*_ring)[(_begin + index) % N]; }
template <typename T, size_t N, typename T2, size_t M>
inline T* Ring2D_shared<T, N, T2, M>::data() { return _ring->data(); }
template <typename T, size_t N, typename T2, size_t M>
inline const T* Ring2D_shared<T, N, T2, M>::data() const { return _ring->data(); };
template <typename T, size_t N, typename T2, size_t M>
inline void Ring2D_shared<T, N, T2, M>::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 <typename T, size_t N, typename T2, size_t M>
inline void Ring2D_shared<T, N, T2, M>::emplace_back_at(size_t i, T&& item)
{
assert(i < _emplace_size);
(*_ring)[(_emplace_start + i) % N] = item;
};
template <typename T, size_t N, typename T2, size_t M>
inline T& Ring2D_shared<T, N, T2, M>::peek_back_at(size_t i)
{
assert(i < _emplace_size);
return (*_ring)[(_emplace_start + i) % N];
};
template <typename T, size_t N, typename T2, size_t M>
inline void Ring2D_shared<T, N, T2, M>::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 <typename T, size_t N, typename T2, size_t M>
inline T2& Ring2D_shared<T, N, T2, M>::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 <typename T, size_t N, typename T2, size_t M>
inline void Ring2D_shared<T, N, T2, M>::finish_emplace_back()
{
_size += _emplace_size;
};
template <typename T, size_t N, typename T2, size_t M>
inline T2* Ring2D_shared<T, N, T2, M>::data2() { return _ring2; }
template <typename T, size_t N, typename T2, size_t M>
inline const T2* Ring2D_shared<T, N, T2, M>::data2() const { return _ring2; };
template <typename T, size_t N, typename T2, size_t M>
inline T2& Ring2D_shared<T, N, T2, M>::peek_front(size_t j) const {
assert(!empty() && j < _2D_size);
return (_ring2)[_begin * _2D_size + j];
}

177
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Container/src/ring_2dex.cc Normal file
View File

@ -0,0 +1,177 @@
#include "ring_2dex.h"
#include <cassert>
#include <iterator>
using namespace hesai::lidar;
template <typename T, size_t N, typename T2, size_t M>
template <typename Item>
class Ring2D_ex<T, N, T2, M>::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 <typename T, size_t N, typename T2, size_t M>
Ring2D_ex<T, N, T2, M>::Ring2D_ex() : _ring(new std::array<T, N>), _ring2(new std::array<T2, N*M>), _begin(0), _size(0) {};
template <typename T, size_t N, typename T2, size_t M>
inline constexpr size_t Ring2D_ex<T, N, T2, M>::size() const { return _size; }
template <typename T, size_t N, typename T2, size_t M>
inline bool Ring2D_ex<T, N, T2, M>::empty() const { return _size == 0; }
template <typename T, size_t N, typename T2, size_t M>
inline bool Ring2D_ex<T, N, T2, M>::not_empty() const { return !empty(); }
template <typename T, size_t N, typename T2, size_t M>
inline bool Ring2D_ex<T, N, T2, M>::full() const { return _size >= N; }
template <typename T, size_t N, typename T2, size_t M>
inline bool Ring2D_ex<T, N, T2, M>::not_full() const { return !full(); }
template <typename T, size_t N, typename T2, size_t M>
inline void Ring2D_ex<T, N, T2, M>::clear() { for (auto& item : *this) { item = T(); } _size = 0; _begin = 0; }
template <typename T, size_t N, typename T2, size_t M>
inline void Ring2D_ex<T, N, T2, M>::eff_clear() { _size = 0; _begin = 0; }
template <typename T, size_t N, typename T2, size_t M>
template <typename... Args>
inline void Ring2D_ex<T, N, T2, M>::emplace_back(Args&&... args) { assert(!full()); (*_ring)[(_begin + _size) % N] = T(args...); ++_size;}
template <typename T, size_t N, typename T2, size_t M>
inline void Ring2D_ex<T, N, T2, M>::push_back(T&& item) { assert(!full()); (*_ring)[(_begin + _size) % N] = item; ++_size; }
template <typename T, size_t N, typename T2, size_t M>
inline const T& Ring2D_ex<T, N, T2, M>::peek_back() const { assert(!empty()); return (*_ring)[(_begin + _size - 1) % N]; }
template <typename T, size_t N, typename T2, size_t M>
inline T Ring2D_ex<T, N, T2, M>::pop_back() { assert(!empty()); --_size; T item; std::swap(item, (*_ring)[(_begin + _size) % N]); return item; }
template <typename T, size_t N, typename T2, size_t M>
inline void Ring2D_ex<T, N, T2, M>::eff_pop_back() { --_size; }
template <typename T, size_t N, typename T2, size_t M>
template <typename... Args>
inline void Ring2D_ex<T, N, T2, M>::emplace_front(Args&&... args) { assert(!full()); _begin = (_begin + N - 1) % N; (*_ring)[_begin] = T(args...); ++_size; }
template <typename T, size_t N, typename T2, size_t M>
inline void Ring2D_ex<T, N, T2, M>::push_front(T&& item) { assert(!full()); _begin = (_begin + N - 1) % N; (*_ring)[_begin] = item; ++_size; }
template <typename T, size_t N, typename T2, size_t M>
inline const T& Ring2D_ex<T, N, T2, M>::peek_front() const { assert(!empty()); return (*_ring)[_begin]; }
template <typename T, size_t N, typename T2, size_t M>
inline T Ring2D_ex<T, N, T2, M>::pop_front() { assert(!empty()); --_size; T item; std::swap(item, (*_ring)[_begin]); _begin = (_begin + 1) % N; return item; }
template <typename T, size_t N, typename T2, size_t M>
inline void Ring2D_ex<T, N, T2, M>::eff_pop_front() { --_size; _begin = (_begin + 1) % N; }
template <typename T, size_t N, typename T2, size_t M>
inline typename Ring2D_ex<T, N, T2, M>::iterator Ring2D_ex<T, N, T2, M>::begin() { return iterator((T*)_ring.get(), _begin); }
template <typename T, size_t N, typename T2, size_t M>
inline typename Ring2D_ex<T, N, T2, M>::iterator Ring2D_ex<T, N, T2, M>::end() { return iterator((T*)_ring.get(), _begin + _size); }
template <typename T, size_t N, typename T2, size_t M>
inline typename Ring2D_ex<T, N, T2, M>::const_iterator Ring2D_ex<T, N, T2, M>::cbegin() const { return const_iterator((const T*)_ring.get(), _begin); }
template <typename T, size_t N, typename T2, size_t M>
inline typename Ring2D_ex<T, N, T2, M>::const_iterator Ring2D_ex<T, N, T2, M>::cend() const { return const_iterator((const T*)_ring.get(), _begin + _size); }
template <typename T, size_t N, typename T2, size_t M>
inline T& Ring2D_ex<T, N, T2, M>::operator[](size_t index) { assert(index < size()); return (*_ring)[(_begin + index) % N]; }
template <typename T, size_t N, typename T2, size_t M>
inline const T& Ring2D_ex<T, N, T2, M>::operator[](size_t index) const { assert(index < size()); return (*_ring)[(_begin + index) % N]; }
template <typename T, size_t N, typename T2, size_t M>
inline T* Ring2D_ex<T, N, T2, M>::data() { return _ring->data(); }
template <typename T, size_t N, typename T2, size_t M>
inline const T* Ring2D_ex<T, N, T2, M>::data() const { return _ring->data(); };
template <typename T, size_t N, typename T2, size_t M>
inline void Ring2D_ex<T, N, T2, M>::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 <typename T, size_t N, typename T2, size_t M>
inline void Ring2D_ex<T, N, T2, M>::emplace_back_at(size_t i, T&& item)
{
assert(i < _emplace_size);
(*_ring)[(_emplace_start + i) % N] = item;
};
template <typename T, size_t N, typename T2, size_t M>
inline T& Ring2D_ex<T, N, T2, M>::peek_back_at(size_t i)
{
assert(i < _emplace_size);
return (*_ring)[(_emplace_start + i) % N];
};
template <typename T, size_t N, typename T2, size_t M>
inline void Ring2D_ex<T, N, T2, M>::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 <typename T, size_t N, typename T2, size_t M>
inline T2& Ring2D_ex<T, N, T2, M>::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 <typename T, size_t N, typename T2, size_t M>
inline void Ring2D_ex<T, N, T2, M>::finish_emplace_back()
{
_size += _emplace_size;
};
template <typename T, size_t N, typename T2, size_t M>
inline T2* Ring2D_ex<T, N, T2, M>::data2() { return _ring2->data(); }
template <typename T, size_t N, typename T2, size_t M>
inline const T2* Ring2D_ex<T, N, T2, M>::data2() const { return _ring2->data(); };
template <typename T, size_t N, typename T2, size_t M>
inline T2& Ring2D_ex<T, N, T2, M>::peek_front(size_t j) const {
assert(!empty() && j < _2D_size);
return (*_ring2)[_begin * _2D_size + j];
}

543
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Lidar/lidar.cc Normal file
View File

@ -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 <inttypes.h>
#include <stdio.h>
#include "Version.h"
#ifndef _MSC_VER
#include <unistd.h>
#endif
using namespace hesai::lidar;
template <typename T_Point>
Lidar<T_Point>::Lidar() {
udp_parser_ = new UdpParser<T_Point>();
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 <typename T_Point>
Lidar<T_Point>::~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 <typename T_Point>
std::string Lidar<T_Point>::GetLidarType() {
return udp_parser_->GetLidarType();
}
template <typename T_Point>
int Lidar<T_Point>::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<T_Point> 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 <typename T_Point>
int Lidar<T_Point>::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 <typename T_Point>
int Lidar<T_Point>::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 <typename T_Point>
int Lidar<T_Point>::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 <typename T_Point>
int Lidar<T_Point>::GetOnePacket(UdpPacket &packet) {
if (origin_packets_buffer_.try_pop_front(packet)) return 0;
else return -1;
}
template <typename T_Point>
int Lidar<T_Point>::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 <typename T_Point>
int Lidar<T_Point>::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 <typename T_Point>
int Lidar<T_Point>::ComputeXYZI(LidarDecodedPacket<T_Point> &packet) {
int ret = -1;
decoded_packets_buffer_.push_back(std::move(packet));
return 0;
}
template <typename T_Point>
int Lidar<T_Point>::DecodePacket(LidarDecodedPacket<T_Point> &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 <typename T_Point>
int Lidar<T_Point>::DecodePacket(LidarDecodedFrame<T_Point> &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 <typename T_Point>
bool Lidar<T_Point>::ComputeXYZIComplete(int index) {
return frame_.packet_num == index;
}
template <typename T_Point>
void Lidar<T_Point>::LoadCorrectionFile(std::string correction_path) {
if (udp_parser_) {
udp_parser_->LoadCorrectionFile(correction_path);
return ;
} else
std::cout << __func__ << "udp_parser_ nullptr\n";
return ;
}
template <typename T_Point>
int Lidar<T_Point>::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 <typename T_Point>
void Lidar<T_Point>::LoadFiretimesFile(std::string firetimes_path) {
if (udp_parser_) {
udp_parser_->LoadFiretimesFile(firetimes_path);
return ;
} else
std::cout << __func__ << "udp_parser_ nullptr\n";
return ;
}
template <typename T_Point>
int Lidar<T_Point>::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 <typename T_Point>
int Lidar<T_Point>::StopRecordPcap() {
int ret = -1;
if (udp_parser_) {
EnableRecordPcap(false);
udp_parser_->GetPcapSaver()->close();
} else
std::cout << __func__ << "udp_parser_ nullptr\n";
return ret;
}
template <typename T_Point>
int Lidar<T_Point>::GetGeneralParser(GeneralParser<T_Point> **parser) {
int ret = 0;
if (udp_parser_)
ret = udp_parser_->GetGeneralParser(parser);
else
std::cout << __func__ << "udp_parser_ nullptr\n";
return ret;
}
template <typename T_Point>
void Lidar<T_Point>::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 <typename T_Point>
void Lidar<T_Point>::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<T_Point> 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 <typename T_Point>
void Lidar<T_Point>::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<T_Point> 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 <typename T_Point>
void Lidar<T_Point>::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 <typename T_Point>
void Lidar<T_Point>::SetSource(Source **source) {
source_ = *source;
}
template <typename T_Point>
UdpParser<T_Point> *Lidar<T_Point>::GetUdpParser() { return udp_parser_; }
template <typename T_Point>
void Lidar<T_Point>::SetUdpParser(UdpParser<T_Point> *udpParser) {
udp_parser_ = udpParser;
}
template <typename T_Point>
void Lidar<T_Point>::EnableRecordPcap(bool bRecord) {
is_record_pcap_ = bRecord;
}

172
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Lidar/lidar.h Normal file
View File

@ -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 <zhangyu@hesaitech.com>
* Description: Define Lidar class
*/
#ifndef Lidar_H
#define Lidar_H
#include <stdint.h>
#include <time.h>
#include "lidar_types.h"
#include "ptc_client.h"
#include <iostream>
#include <vector>
#include <boost/thread/thread.hpp>
#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 <endian.h>
#include <semaphore.h>
#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 <typename T_Point>
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<T_Point> **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<T_Point> &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<T_Point> &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<T_Point> &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<T_Point> *udp_parser);
UdpParser<T_Point> *GetUdpParser();
void EnableRecordPcap(bool bRecord);
// set the parser thread number
void SetThreadNum(int thread_num);
void SetSource(Source **source);
std::string GetLidarType();
UdpParser<T_Point> *udp_parser_;
Source *source_;
PtcClient *ptc_client_;
LidarDecodedFrame<T_Point> frame_;
BlockingRing<UdpPacket, kPacketBufferSize> 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<LidarDecodedPacket<T_Point>, 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<std::list<LidarDecodedPacket<T_Point>>> handle_thread_packet_buffer_;
std::vector<boost::thread *> 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 */

349
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Lidar/lidar_types.h Normal file
View File

@ -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 <zhangyu@hesaitech.com>
* Description:
*/
#ifndef LIDAR_TYPES_H_
#define LIDAR_TYPES_H_
#include <stdint.h>
#include <vector>
#include <iostream>
#include <string.h>
#include <algorithm>
#include <functional>
#include <memory>
#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 <typename PointT>
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 <typename PointT>
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<uint8_t> u8Array_t;
typedef std::vector<uint16_t> u16Array_t;
typedef std::vector<uint32_t> u32Array_t;
typedef std::vector<double> doubleArray_t;
typedef std::vector<u8Array_t> u8ArrayArray_t;
typedef std::vector<doubleArray_t> doubleArrayArray_t;
typedef std::vector<u16Array_t> u16ArrayArray_t;
typedef std::vector<std::string> stringArray_t;
typedef std::vector<UdpPacket> UdpFrame_t;
typedef std::vector<UdpFrame_t> 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_

102
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Logger/include/logger.h Normal file
View File

@ -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 <string>
#include <memory>
#include <thread>
#include <mutex>
#include <condition_variable>
#include <list>
#include <functional>
#include <algorithm>
//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<void(LOGLEVEL loglevel, const char* pszFile, int lineNo, const char* pszFuncSig, char* pszFmt)> 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<std::thread> spthread_;
std::mutex mutex_;
std::condition_variable cv_; //有新的日志到来的标识
bool exit_{false};
std::list<std::string> queue_;
uint8_t log_level_rule_;
uint8_t log_target_rule_;
std::function<void(LOGLEVEL loglevel, const char* pszFile, int lineNo, const char* pszFuncSig, char* pszFmt)> log_callback_;
bool running_{false};
};
#endif

180
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Logger/src/logger.cc Normal file
View File

@ -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 <time.h>
#include <stdio.h>
#include <memory>
#include <stdarg.h>
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<std::mutex> 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<std::mutex> 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<void(LOGLEVEL loglevel, const char* pszFile, int lineNo, const char* pszFuncSig, char* pszFmt)> log_callback) {
log_callback_ = log_callback;
}

79
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcClient/include/client_base.h Normal file
View File

@ -0,0 +1,79 @@
/*
* Copyright (C) 2019 Hesai Tech<http://www.hesaitech.com>
*
* 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<int_zhangxu@hesaitech.com>
*
* Created on Jul 9, 2023, 20:03 PM
*/
#ifndef CLIENT_BASE_H
#define CLIENT_BASE_H
#include <stdint.h>
#include <string.h>
#include <atomic>
#include <functional>
#include <mutex>
#include <string>
#include <thread>
#include <memory>
#include <vector>
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

115
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcClient/include/lidar_communication_header.h Normal file
View File

@ -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<zouke@hesaitech.com>
* Description:
*/
#ifndef LIDARCOMMUNICATIONHEADER_H
#define LIDARCOMMUNICATIONHEADER_H
#include <endian.h>
#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

100
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcClient/include/ptc_client.h Normal file
View File

@ -0,0 +1,100 @@
/*
* Copyright (C) 2019 Hesai Tech<http://www.hesaitech.com>
*
* 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<zouke@hesaitech.com>
*
* Created on Jun 20, 2019, 10:46 AM
*/
#ifndef PtcClient_H
#define PtcClient_H
// #include <endian.h>
// #include <semaphore.h>
#include <vector>
#include <boost/thread/thread.hpp>
// #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<ClientBase> client_;
std::shared_ptr<PtcParser> ptc_parser_;
};
}
}
#endif /* PtcClient_H */

115
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcClient/include/tcp_client.h Normal file
View File

@ -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<zouke@hesaitech.com>
*
* Created on Sep 5, 2019, 10:46 AM
*/
#ifndef TCPCLIENT_H
#define TCPCLIENT_H
#ifdef _MSC_VER
#include <winsock2.h>
#include <ws2tcpip.h>
#else
typedef unsigned int SOCKET;
#endif
#include "client_base.h"
#include <stdint.h>
#include <string.h>
#include <atomic>
#include <functional>
#include <mutex>
#include <string>
#include <thread>
#include <vector>
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 */

114
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcClient/include/tcp_ssl_client.h Normal file
View File

@ -0,0 +1,114 @@
/*
* Copyright (C) 2019 Hesai Tech<http://www.hesaitech.com>
*
* 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<int_zhangxu@hesaitech.com>
*
* 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 <openssl/err.h>
#include <string.h>
#include <stdint.h>
#include <memory>
#include <iostream>
#include <thread>
#include <chrono>
#include <iostream>
#ifdef _MSC_VER
#include <winsock2.h>
#include <ws2tcpip.h>
#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

340
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcClient/src/ptc_client.cc Normal file
View File

@ -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<zouke@hesaitech.com>
*/
#include "ptc_client.h"
#include <plat_utils.h>
#ifdef _MSC_VER
#ifndef MSG_DONTWAIT
#define MSG_DONTWAIT (0x40)
#endif
#else
#include <sched.h>
#include <sys/socket.h>
#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<TcpClient>();
client_->Open(ip, u16TcpPort, bAutoReceive);
} else if(client_mode == PtcMode::tcp_ssl) {
client_ = std::make_shared<TcpSslClient>();
client_->Open(ip, u16TcpPort, bAutoReceive, cert, private_key, ca);
}
// init ptc parser
ptc_parser_ = std::make_shared<PtcParser>(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<u8Buf.size(); i++) {
printf("%x ", u8Buf[i]);
}
}
} while (len > 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<PTCHeader_1_0 *>(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<PTCHeader_2_0 *>(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;
}

338
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcClient/src/tcp_client.cc Normal file
View File

@ -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<zouke@hesaitech.com>
*/
#include "tcp_client.h"
#ifdef _MSC_VER
#include <winsock2.h>
#include <ws2tcpip.h>
#pragma comment(lib, "ws2_32.lib") // Winsock Library
#include <BaseTsd.h>
typedef int ssize_t;
typedef int socklen_t;
#define MSG_DONTWAIT (0x40)
#else
#include <arpa/inet.h>
#include <errno.h>
#include <netinet/in.h>
#include <netinet/ip.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <unistd.h>
#endif
#include <plat_utils.h>
#include <string.h>
#include <algorithm>
#include <iostream>
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;
}

388
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcClient/src/tcp_ssl_client.cc Normal file
View File

@ -0,0 +1,388 @@
/*
* Copyright (C) 2019 Hesai Tech<http://www.hesaitech.com>
*
* 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<int_zhangxu@hesaitech.com>
*/
#include "tcp_ssl_client.h"
#include <plat_utils.h>
#ifdef _MSC_VER
#include <winsock2.h>
#include <ws2tcpip.h>
#pragma comment(lib, "ws2_32.lib") // Winsock Library
#include <BaseTsd.h>
typedef int ssize_t;
typedef int socklen_t;
#define MSG_DONTWAIT (0x40)
#else
#include <arpa/inet.h>
#include <errno.h>
#include <netinet/in.h>
#include <netinet/ip.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <unistd.h>
#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));
}
}

65
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcParser/include/general_ptc_parser.h Normal file
View File

@ -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 <int_zhangxu@hesaitech.com>
* Description: ptc protocol general parser class.
*/
#ifndef GENERAL_PTC_PARSER_H_
#define GENERAL_PTC_PARSER_H_
#include <iostream>
#include <fstream>
#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<u8Array_t>& 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

110
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcParser/include/ptc_1_0_parser.h Normal file
View File

@ -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 <int_zhangxu@hesaitech.com>
* Description: ptc 1.0 protocol parser class.
*/
#ifndef PTC_1_0_PARSER_H_
#define PTC_1_0_PARSER_H_
#ifdef _MSC_VER
#include <boost/endian/conversion.hpp>
#endif
#include <iostream>
#include <fstream>
#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

123
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcParser/include/ptc_2_0_parser.h Normal file
View File

@ -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 <int_zhangxu@hesaitech.com>
* Description: ptc 2.0 protocol parser class.
*/
#ifndef PTC_2_0_PARSER_H_
#define PTC_2_0_PARSER_H_
#ifdef _MSC_VER
#include <boost/endian/conversion.hpp>
#endif
#include <iostream>
#include <fstream>
#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

63
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcParser/ptc_parser.cc Normal file
View File

@ -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 <int_zhangxu@hesaitech.com>
* 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<Ptc_1_0_parser>(parser_);
parser_ = new Ptc_1_0_parser();
} else if(ptc_version == 2) {
// parser_ = std::dynamic_pointer_cast<Ptc_2_0_parser>(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;
}

71
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcParser/ptc_parser.h Normal file
View File

@ -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 <int_zhangxu@hesaitech.com>
* 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 <memory>
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

60
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcParser/src/general_ptc_parser.cc Normal file
View File

@ -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 <int_zhangxu@hesaitech.com>
*/
#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<u8Array_t>& packages) {
const int FRAME_LENGTH = 1024;
int file_length = file.size();
std::vector<u8Array_t> 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;
}

68
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcParser/src/ptc_1_0_parser.cc Normal file
View File

@ -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 <int_zhangxu@hesaitech.com>
*/
#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<PTCHeader_1_0*>(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;
}

98
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/PtcParser/src/ptc_2_0_parser.cc Normal file
View File

@ -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 <int_zhangxu@hesaitech.com>
*/
#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<PTCHeader_2_0*>(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;
}

104
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Source/include/pcap_saver.h Normal file
View File

@ -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 <functional>
#include <array>
#include <deque>
#include <mutex>
#include <thread>
#include <utility>
#include <string>
#include <string_view>
#include <fstream>
#include <memory>
#include <queue>
#include <cstring>
#include "pcap_source.h"
#include "blocking_ring.h"
#include <chrono>
#include <ctime>
#include <sys/types.h>
#include <sys/stat.h>
#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<PandarPacket, 32*1024>;
std::shared_ptr<Container> 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_

197
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Source/include/pcap_source.h Normal file
View File

@ -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 <functional>
#include <array>
#include <deque>
#include <mutex>
#include <thread>
#include <utility>
#include <string>
#include <string_view>
#include <fstream>
#include <memory>
#include <queue>
#include <cstring>
#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<int(const uint8_t*, uint32_t)>;
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<uint8_t, 1500> 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_

89
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Source/include/socket_source.h Normal file
View File

@ -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<zouke@hesaitech.com>
*
* Created on Sep 5, 2019, 10:46 AM
*/
#ifndef SOCKETSTREAMER_H
#define SOCKETSTREAMER_H
#include <stdint.h>
#include <string>
#include "source.h"
#ifdef _MSC_VER
#include <winsock2.h>
#include <ws2tcpip.h>
#pragma comment(lib, "ws2_32.lib") // Winsock Library
#include <BaseTsd.h>
typedef int ssize_t;
#else
#include <arpa/inet.h>
#include <errno.h>
#include <netinet/ip.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <unistd.h>
#include <fcntl.h>
#include <poll.h>
#include <sys/file.h>
#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 */

77
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Source/include/source.h Normal file
View File

@ -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<zouke@hesaitech.com>
*
* Created on Sep 5, 2019, 10:46 AM
*/
#ifndef STREAMER_H
#define STREAMER_H
#include <stdint.h>
#include <string>
#include <lidar_types.h>
#ifdef _MSC_VER
#include <winsock2.h>
#include <ws2tcpip.h>
#pragma comment(lib, "ws2_32.lib") // Winsock Library
typedef int socklen_t;
#else
#include <arpa/inet.h>
#include <errno.h>
#include <netinet/ip.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <unistd.h>
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 */

224
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Source/src/pcap_saver.cc Normal file
View File

@ -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 <stdexcept>
#include <cassert>
#include <cstring>
#include <boost/iostreams/device/mapped_file.hpp>
#include <iostream>
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<uint8_t, 1500> 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<uint8_t, 1500> 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<uint8_t, 1500> 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;
}

318
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Source/src/pcap_source.cc Normal file
View File

@ -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 <stdexcept>
#include <cassert>
#include <cstring>
// #include "hesai/time/time_basic.hh"
#include <boost/iostreams/device/mapped_file.hpp>
#include <iostream>
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<typename T>
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;
}

244
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Source/src/socket_source.cc Normal file
View File

@ -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<zouke@hesaitech.com>
*/
#include "socket_source.h"
#include <stdio.h>
#include <iostream>
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));
}

45
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/Source/src/source.cc Normal file
View File

@ -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<zouke@hesaitech.com>
*/
#include "source.h"
#include <string.h>
using namespace hesai::lidar;
Source::Source() {}
Source::~Source() { Close(); }
void Source::Close() {
printf("Source::Close()\n");
}

254
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/general_parser.h Normal file
View File

@ -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 <zhangyu@hesaitech.com>
* 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 <semaphore.h>
#endif
#include <list>
#include <vector>
#include <boost/algorithm/string.hpp>
#include <boost/thread/thread.hpp>
#include <iostream>
#include <fstream>
#include "lidar_types.h"
namespace hesai
{
namespace lidar
{
#define DEFINE_MEMBER_CHECKER(member) \
template <typename T, typename V = bool> \
struct has_##member : std::false_type \
{ \
}; \
template <typename T> \
struct has_##member< \
T, typename std::enable_if<!std::is_same<decltype(std::declval<T>().member), void>::value, bool>::type> \
: std::true_type \
{ \
};
#define PANDAR_HAS_MEMBER(C, member) has_##member<C>::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 <typename T_Point>
inline typename std::enable_if<!PANDAR_HAS_MEMBER(T_Point, x)>::type setX(T_Point& point, const float& value)
{
}
template <typename T_Point>
inline typename std::enable_if<PANDAR_HAS_MEMBER(T_Point, x)>::type setX(T_Point& point, const float& value)
{
point.x = value;
}
template <typename T_Point>
inline typename std::enable_if<!PANDAR_HAS_MEMBER(T_Point, y)>::type setY(T_Point& point, const float& value)
{
}
template <typename T_Point>
inline typename std::enable_if<PANDAR_HAS_MEMBER(T_Point, y)>::type setY(T_Point& point, const float& value)
{
point.y = value;
}
template <typename T_Point>
inline typename std::enable_if<!PANDAR_HAS_MEMBER(T_Point, z)>::type setZ(T_Point& point, const float& value)
{
}
template <typename T_Point>
inline typename std::enable_if<PANDAR_HAS_MEMBER(T_Point, z)>::type setZ(T_Point& point, const float& value)
{
point.z = value;
}
template <typename T_Point>
inline typename std::enable_if<!PANDAR_HAS_MEMBER(T_Point, intensity)>::type setIntensity(T_Point& point,
const uint8_t& value)
{
}
template <typename T_Point>
inline typename std::enable_if<PANDAR_HAS_MEMBER(T_Point, intensity)>::type setIntensity(T_Point& point,
const uint8_t& value)
{
point.intensity = value;
}
template <typename T_Point>
inline typename std::enable_if<!PANDAR_HAS_MEMBER(T_Point, ring)>::type setRing(T_Point& point, const uint16_t& value)
{
}
template <typename T_Point>
inline typename std::enable_if<PANDAR_HAS_MEMBER(T_Point, ring)>::type setRing(T_Point& point, const uint16_t& value)
{
point.ring = value;
}
template <typename T_Point>
inline typename std::enable_if<!PANDAR_HAS_MEMBER(T_Point, timestamp)>::type setTimestamp(T_Point& point,
const double& value)
{
}
template <typename T_Point>
inline typename std::enable_if<PANDAR_HAS_MEMBER(T_Point, timestamp)>::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 <typename T_Point>
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<T_Point> &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<T_Point> &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<T_Point> &frame, LidarDecodedPacket<T_Point> &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<double> elevation_correction_;
std::vector<double> 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_

97
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/udp1_4_parser.h Normal file
View File

@ -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 <zhangyu@hesaitech.com>
* 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<int, 2> firetime;
};
std::array<SectionValue, 8> 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<typename T_Point>
class Udp1_4Parser : public GeneralParser<T_Point> {
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<T_Point> &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<T_Point> &frame, LidarDecodedPacket<T_Point> &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<FiretimeSectionValues, kLaserNum> 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_

82
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/udp2_4_parser.h Normal file
View File

@ -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 <boost/atomic.hpp>
#include <boost/lockfree/queue.hpp>
#include "general_parser.h"
#include "lidar_types.h"
#include <vector>
#include <sstream>
#include <fstream>
#include <string>
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<float, ET_MAX_CHANNEL_NUM_24> 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<typename T_Point>
class Udp2_4Parser : public GeneralParser<T_Point> {
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<T_Point> &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<T_Point> &frame, LidarDecodedPacket<T_Point> &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

109
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/udp2_5_parser.h Normal file
View File

@ -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 <boost/atomic.hpp>
#include <boost/lockfree/queue.hpp>
#include "general_parser.h"
#include "lidar_types.h"
#include <vector>
#include <sstream>
#include <fstream>
#include <string>
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<typename T_Point>
class Udp2_5Parser : public GeneralParser<T_Point> {
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<T_Point> &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<T_Point> &frame, LidarDecodedPacket<T_Point> &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

74
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/udp3_1_parser.h Normal file
View File

@ -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 <zhangyu@hesaitech.com>
* 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<typename T_Point>
class Udp3_1Parser : public GeneralParser<T_Point> {
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<T_Point> &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<T_Point> &frame, LidarDecodedPacket<T_Point> &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_

110
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/udp3_2_parser.h Normal file
View File

@ -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 <zhangyu@hesaitech.com>
* 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 <boost/algorithm/string.hpp>
#include <fstream>
#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<std::vector<int>> 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<typename T_Point>
class Udp3_2Parser : public GeneralParser<T_Point> {
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<T_Point> &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<T_Point> &frame, LidarDecodedPacket<T_Point> &packet);
// determine whether frame splitting is needed
bool IsNeedFrameSplit(uint16_t azimuth);
private:
std::array<std::array<float, HS_LIDAR_QT128_LASER_NUM>,
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_

177
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/udp4_3_parser.h Normal file
View File

@ -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 <zhangyu@hesaitech.com>
* 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 <boost/atomic.hpp>
#include <boost/lockfree/queue.hpp>
#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<float, MAX_AZI_LEN> sin_map;
std::array<float, MAX_AZI_LEN> 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<float, MAX_AZI_LEN> sin_map;
std::array<float, MAX_AZI_LEN> 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<typename T_Point>
class Udp4_3Parser : public GeneralParser<T_Point> {
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<T_Point> &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<T_Point> &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<T_Point> &frame, LidarDecodedPacket<T_Point> &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_

78
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/udp6_1_parser.h Normal file
View File

@ -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 <zhangyu@hesaitech.com>
* 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<typename T_Point>
class Udp6_1Parser : public GeneralParser<T_Point> {
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<T_Point> &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<T_Point> &frame, LidarDecodedPacket<T_Point> &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_

106
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/udp7_2_parser.h Normal file
View File

@ -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 <zhangyu@hesaitech.com>
* 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<float, COLUMN_MAX>;
using CorrectionMatrix = std::array<ColumnFloatArray, CHANNEL_MAX>;
public:
std::array<ColumnFloatArray, CHANNEL_MAX> 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<typename T_Point>
class Udp7_2Parser : public GeneralParser<T_Point> {
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<T_Point> &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<T_Point> &frame, LidarDecodedPacket<T_Point> &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_

74
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/udp_p40_parser.h Normal file
View File

@ -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 <zhangyu@hesaitech.com>
* 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 <typename T_Point>
class UdpP40Parser : public GeneralParser<T_Point> {
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<T_Point> &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<T_Point> &frame, LidarDecodedPacket<T_Point> &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_

73
deploy/ros2_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/libhesai/UdpParser/include/udp_p64_parser.h Normal file
View File

@ -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 <zhangyu@hesaitech.com>
* 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 <typename T_Point>
class UdpP64Parser : public GeneralParser<T_Point> {
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<T_Point> &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<T_Point> &frame, LidarDecodedPacket<T_Point> &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_

Some files were not shown because too many files have changed in this diff Show More