add lslidar ros2 driver.

src/ros2/src/Lslidar_ROS2_driver/LICENSE

@@ -0,0 +1,13 @@
+Copyright 2022 LeiShen Intelligent
+
+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.

src/ros2/src/Lslidar_ROS2_driver/README_en.md

@@ -0,0 +1,218 @@
+# LSLIDAR_CX_V3.0.4_221228_ROS2
+
+## 1.Introduction
+​		LSLIDAR_CX_V3.0.4_221228_ROS2 is the lidar ros driver in linux environment, which is suitable for c16/c32 lidar(1212bytes ,version 2.6/2.8/3.0). The program has  tested under ubuntu18.04 ros dashing , ubuntu18.04 ros eloquent, ubuntu 20.04 ros foxy , ubuntu 20.04 ros galactic and ubuntu 22.04 ros humble .
+
+## 2.Dependencies
+
+1.ros
+
+To run lidar driver in ROS environment, ROS related libraries need to be installed.
+
+**Ubuntu 18.04**: ros-dashing-desktop-full
+
+**Ubuntu 18.04**: ros-eloquent-desktop-full
+
+**Ubuntu 20.04**: ros-foxy-desktop-full
+
+**Ubuntu 20.04**: ros-galactic-desktop-full
+
+**Ubuntu 22.04**: ros-humble-desktop-full
+
+**Installation**: please refer to [http://wiki.ros.org](http://wiki.ros.org/)
+
+2.ros dependencies
+
+```bash
+# install
+sudo apt-get install ros-$ROS_DISTRO-pcl-ros ros-$ROS_DISTRO-pluginlib  ros-$ROS_DISTRO-pcl-conversions
+```
+
+3.other dependencies
+
+~~~bash
+sudo apt-get install libpcap-dev
+sudo apt-get install libboost${BOOST_VERSION}-dev   #Select the appropriate version
+~~~
+
+## 3.Compile & Run
+
+### 3.1 Compile
+
+~~~bash
+mkdir -p ~/lidar_ws/src
+~~~
+
+Copy the whole lidar ROS driver directory into ROS workspace, i.e "~/lidar_ws/src".
+
+~~~bash
+cd ~/lidar_ws
+colcon build
+source install/setup.bash
+~~~
+
+### 3.2 Run
+
+run with single c32 lidar(It is divided into 1212 bytes lidar and 1206 bytes lidar. Pay attention to modify lslidar_driver/params/lslidar_c32.yaml when start  lidar driver) :
+
+~~~bash
+ros2 launch lslidar_driver lslidar_c32_launch.py
+~~~
+
+run with double c32 lidar(It is divided into 1212 bytes lidar and 1206 bytes lidar. Pay attention to modify lslidar_driver/params/lslidar_c32_1.yaml and  lslidar_c32_2.yaml  when start  lidar  driver) :
+
+~~~bash
+ros2 launch lslidar_driver lslidar_c32_double_launch.py
+~~~
+
+
+
+run with single c16 lidar(It is divided into 1212 bytes lidar and 1206 bytes lidar. Pay attention to modify lslidar_driver/params/lslidar_c16.yaml when start  lidar driver) :
+
+~~~bash
+ros2 launch lslidar_driver lslidar_c16_launch.py
+~~~
+
+run with double c16 lidar(It is divided into 1212 bytes lidar and 1206 bytes lidar. Pay attention to modify lslidar_driver/params/lslidar_c16_1.yaml and  lslidar_c16_2.yaml  when start lidar  driver) :
+
+~~~bash
+ros2 launch lslidar_driver lslidar_c16_double_launch.py
+~~~
+
+
+
+## 4. Introduction to parameters
+
+The content of the lslidar_c32.yaml file is as follows, and the meaning of each parameter is shown in the notes.
+
+~~~bash
+/c32/lslidar_driver_node:
+  ros__parameters:
+    packet_size: 1206                       #lidar data packet length (bytes)，write 1212 or 1206 
+    device_ip: 192.168.1.200                #lidar ip
+    msop_port: 2368                         # Main data Stream Output Protocol packet port
+    difop_port: 2369                        # Device Information Output Protocol packet port
+    frame_id: laser_link                    # lidar point cloud coordinate system name
+    add_multicast: false                    # Whether to add multicast
+    group_ip: 224.1.1.2                     #multicast ip
+    use_gps_ts: true                        # Whether gps time synchronization
+    lidar_type: c32                         #c16:c16 lidar；c32:c32 lidar
+    c16_type: c16_2                         #c16_2:lidar with vertical angular resolution of 2 degrees，c16_1:lidar with vertical angular resolution of 1.33 degrees
+    c32_type: c32_2                         #c32_2:lidar with vertical angular resolution of 1 degrees ，c32_1:lidar with vertical angular resolution of 2 degrees
+    c32_fpga_type: 3                        #3:lidar with fpga version 2.7\2.8\3.0 ，2:lidar with fpga version 2.6
+    min_range: 0.3                          #Unit: m. The minimum value of the lidar blind area, points smaller than this value are filtered
+    max_range: 200.0                        #Unit: m. The maximum value of the lidar blind area, points smaller than this value are filtered
+    distance_unit: 0.4                      #lidar range resolution
+    angle_disable_min: 0                    #lidar clipping angle start value ，unit:0.01°
+    angle_disable_max: 0                    #lidar clipping angle end value ，unit:0.01°
+    scan_num: 16                            #laserscan line number
+    topic_name: lslidar_point_cloud         #point cloud topic name, can be modified
+    publish_scan: false                     #Whether to publish the scan
+    pcl_type: false                         #pointcloud type，false: xyzirt,true:xyzi
+    coordinate_opt: false                   #Default false. The zero degree angle of the lidar corresponds to the direction of the point cloud
+    #pcap: /home/chris/Documents/leishen/1212bytes_c32/gps.pcap                        #Uncomment to read the data from the pcap file, and add the comment to read the data from the lidar
+~~~
+
+### Multicast mode:
+
+- The host computer sets the lidar to enable multicast mode
+
+- Modify the following parameters of the launch file
+
+  ~~~xml
+  add_multicast: true
+  group_ip: 224.1.1.2    //The multicast ip address set by the host computer
+  ~~~
+
+- Run the following command to add the computer to the group (replace enp2s0 in the command with the network card name of the user's computer, you can use ifconfig to view the network card name)
+
+  ~~~shell
+  ifconfig
+  sudo route add -net 224.0.0.0/4 dev enp2s0
+  ~~~
+
+
+
+### Offline pcap mode:
+
+- Copy the recorded pcap file to the lslidar_ros/lslidar_driver/pcap folder
+
+- Modify the following parameters of the launch file
+
+  ~~~xml
+  // uncomment
+  pcap: xxx.pcap  // xxx.pcap is changed to the copied pcap file name
+  ~~~
+
+
+
+###  pcl point cloud type:
+
+- Modify the following parameters of the launch file
+
+  ~~~xml
+  pcl_type: false      # pointcloud type，false: xyzirt,true:xyzi
+  ~~~
+
+- The default false is the custom point cloud type, which references the definition in the file of
+
+  lslidar_driver/include/lslidar_driver.h
+
+  Change it to true, which is the own type of pcl:
+
+  ~~~c++
+  pcl::PointCloud<pcl::PointXYZI>
+  ~~~
+
+## FAQ
+
+Bug Report
+
+version : LSLIDAR_CX_ 1212BYTES_V3.0.1_220719 ROS2
+
+Modify:  original version
+
+Date    : 2022-07-19
+
+-------------------------------------------------------------------------------------------------------------------------
+
+
+
+Updated version: LSLIDAR_CX_V3.0.2_220809_ROS2
+
+Modify:  1.Compatible with 1212 bytes and 1206 bytes C16 / C32 lidar(version 3.0 )
+
+Date    : 2022-08-09
+
+-------------------------
+
+
+
+Updated version : LSLIDAR_CX_V3.0.3_221118_ROS2
+
+Modify:  1.Added support for ros2 humble/ros2 dashing / ros2 eloquent
+
+Date    : 2022-11-18
+
+-------------------------
+
+
+
+Updated version : LSLIDAR_CX_V3.0.4_221228_ROS2
+
+Modify:  1.Fixed the problem of point cloud layering caused by negative C32 lidar compensation angle.
+
+Date    : 2022-11-18
+
+
+
+Updated version : LSLIDAR_CX_V3.0.4_221228_ROS2
+
+Modify:  Reduce cpu usage, boost library to standard library,point time to relative time .
+
+Date    : 2023-03-14
+
+-------------
+
+
+

src/ros2/src/Lslidar_ROS2_driver/lslidar_driver/CMakeLists.txt

@@ -0,0 +1,60 @@
+cmake_minimum_required(VERSION 3.5)
+project(lslidar_driver)
+
+set(CMAKE_BUILD_TYPE Release)    #RelWithDebInfo
+#add_compile_options(-std=c++11)
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+	set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+	add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+set(libpcap_LIBRARIES -lpcap)
+#set(FastRTPS_INCLUDE_DIR /opt/ros/foxy/include)
+#set(FastRTPS_LIBRARY_RELEASE /opt/ros/foxy/lib/libfastrtps.so)
+
+find_package(Boost REQUIRED)
+#find_package(angles REQUIRED)
+find_package(PCL REQUIRED)
+find_package(lslidar_msgs REQUIRED)
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+#find_package(Boost REQUIRED COMPONENTS thread)
+find_package(sensor_msgs REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(pcl_conversions REQUIRED)
+#find_package(diagnostic_updater REQUIRED)
+find_package(PkgConfig REQUIRED)
+
+include_directories(
+		include
+		${PCL_INCLUDE_DIRS}
+#		${Boost_INCLUDE_DIRS}
+)
+
+# c32 Node
+add_executable(lslidar_c32driver_node src/lslidar_c32driver_node.cpp src/lslidar_driver.cpp src/input.cpp)
+target_link_libraries(lslidar_c32driver_node ${rclcpp_LIBRARIES} ${libpcap_LIBRARIES} ${Boost_LIBRARIES})   #${Boost_LIBRARIES}
+ament_target_dependencies(lslidar_c32driver_node rclcpp std_msgs lslidar_msgs sensor_msgs  pcl_conversions)
+
+# c16 Node
+add_executable(lslidar_c16driver_node src/lslidar_c16driver_node.cpp src/lslidar_driver.cpp src/input.cpp)
+target_link_libraries(lslidar_c16driver_node ${rclcpp_LIBRARIES} ${libpcap_LIBRARIES} ${Boost_LIBRARIES})   #${Boost_LIBRARIES}
+ament_target_dependencies(lslidar_c16driver_node rclcpp std_msgs lslidar_msgs sensor_msgs  pcl_conversions)
+
+install(DIRECTORY launch params rviz_cfg
+		DESTINATION share/${PROJECT_NAME})
+
+install(TARGETS
+		lslidar_c32driver_node lslidar_c16driver_node
+		DESTINATION lib/${PROJECT_NAME}
+		)
+
+ament_export_dependencies(rclcpp std_msgs lslidar_msgs sensor_msgs pcl_conversions)
+ament_export_include_directories(include ${PCL_COMMON_INCLUDE_DIRS})
+
+ament_package()

src/ros2/src/Lslidar_ROS2_driver/lslidar_driver/include/lslidar_driver/input.h

@@ -0,0 +1,122 @@
+/******************************************************************************
+ * This file is part of lslidar_cx driver.
+ *
+ * Copyright 2022 LeiShen Intelligent Authors. All Rights Reserved.
+ *
+ * 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.
+ *****************************************************************************/
+
+#ifndef __LSLIDAR_INPUT_H_
+#define __LSLIDAR_INPUT_H_
+
+#include <unistd.h>
+#include <stdio.h>
+#include <pcap.h>
+#include <netinet/in.h>
+#include "rclcpp/rclcpp.hpp"
+#include <lslidar_msgs/msg/lslidar_packet.hpp>
+#include <lslidar_msgs/msg/lslidar_point.hpp>
+#include <lslidar_msgs/msg/lslidar_scan.hpp>
+#include <string>
+#include <sstream>
+#include <sys/socket.h>
+#include <arpa/inet.h>
+#include <poll.h>
+#include <errno.h>
+#include <fcntl.h>
+#include <sys/file.h>
+#include <signal.h>
+#include <sensor_msgs/msg/time_reference.hpp>
+
+namespace lslidar_driver {
+    static uint16_t MSOP_DATA_PORT_NUMBER = 2368;   // lslidar default data port on PC
+    static uint16_t DIFOP_DATA_PORT_NUMBER = 2369;  // lslidar default difop data port on PC
+/**
+ *  从在线的网络数据或离线的网络抓包数据（pcap文件）中提取出lidar的原始数据，即packet数据包
+ * @brief The Input class,
+     *
+     * @param private_nh  一个NodeHandled,用于通过节点传递参数
+     * @param port
+     * @returns 0 if successful,
+     *          -1 if end of file
+     *          >0 if incomplete packet (is this possible?)
+ */
+    class Input {
+    public:
+        Input(rclcpp::Node* private_nh, uint16_t port);
+
+        virtual ~Input() {
+        }
+
+        virtual int getPacket(lslidar_msgs::msg::LslidarPacket::UniquePtr &pkt) = 0;
+
+
+    protected:
+        rclcpp::Node*  private_nh_;
+        uint16_t port_;
+        std::string devip_str_;
+        int cur_rpm_;
+        int return_mode_;
+        bool npkt_update_flag_;
+        bool add_multicast;
+        std::string group_ip;
+        int packet_size_;
+
+    };
+
+/** @brief Live lslidar input from socket. */
+    class InputSocket : public Input {
+    public:
+        InputSocket(rclcpp::Node*  private_nh, uint16_t port = MSOP_DATA_PORT_NUMBER);
+
+        virtual ~InputSocket();
+
+        virtual int getPacket(lslidar_msgs::msg::LslidarPacket::UniquePtr &pkt);
+
+
+    private:
+        int sockfd_;
+        in_addr devip_;
+
+    };
+
+/** @brief lslidar input from PCAP dump file.
+   *
+   * Dump files can be grabbed by libpcap
+   */
+    class InputPCAP : public Input {
+    public:
+        InputPCAP(rclcpp::Node* private_nh, uint16_t port = MSOP_DATA_PORT_NUMBER, double packet_rate = 0.0,
+                  std::string filename = "");
+
+//        InputPCAP(rclcpp::Node* private_nh, uint16_t port = MSOP_DATA_PORT_NUMBER, double packet_rate = 0.0,
+//                  std::string filename = "", bool read_once = false, bool read_fast = false, double repeat_delay = 0.0);
+
+        virtual ~InputPCAP();
+
+        virtual int getPacket(lslidar_msgs::msg::LslidarPacket::UniquePtr &pkt);
+
+    private:
+        rclcpp::Rate packet_rate_;
+        std::string filename_;
+        pcap_t *pcap_;
+        bpf_program pcap_packet_filter_;
+        char errbuf_[PCAP_ERRBUF_SIZE];
+        bool empty_;
+        bool read_once_;
+        bool read_fast_;
+        double repeat_delay_;
+    };
+}
+
+#endif  // __LSLIDAR_INPUT_H

src/ros2/src/Lslidar_ROS2_driver/lslidar_driver/include/lslidar_driver/lslidar_driver.h

@@ -0,0 +1,313 @@
+/******************************************************************************
+ * This file is part of lslidar_cx driver.
+ *
+ * Copyright 2022 LeiShen Intelligent Authors. All Rights Reserved.
+ *
+ * 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.
+ *****************************************************************************/
+
+
+#ifndef _LS_C16_DRIVER_H_
+#define _LS_C16_DRIVER_H_
+
+#define DEG_TO_RAD 0.017453293
+#define RAD_TO_DEG 57.29577951
+
+#include <string>
+#include <rclcpp/rclcpp.hpp>
+#include <pcl/point_types.h>
+//#include <pcl_ros/impl/transforms.hpp>
+#include <pcl_conversions/pcl_conversions.h>
+//#include <boost/shared_ptr.hpp>
+//#include <boost/thread.hpp>
+#include <thread>
+#include <memory>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <sensor_msgs/msg/laser_scan.hpp>
+//#include <sensor_msgs/msg/laser_scan.h>
+#include "input.h"
+//#include <pcl_ros/point_cloud.h>
+//#include <sensor_msgs/msg/point_cloud.h>
+#include <std_msgs/msg/string.h>
+#include <lslidar_msgs/msg/lslidar_packet.hpp>
+#include <lslidar_msgs/msg/lslidar_point.hpp>
+#include <lslidar_msgs/msg/lslidar_scan.hpp>
+#include <lslidar_msgs/srv/lslidarcontrol.hpp>
+
+
+namespace lslidar_driver {
+//raw lslidar packet constants and structures
+    static const int SIZE_BLOCK = 100;
+    static const int RAW_SCAN_SIZE = 3;
+    static const int SCANS_PER_BLOCK = 32;
+    static const int BLOCK_DATA_SIZE = SCANS_PER_BLOCK * RAW_SCAN_SIZE;
+    static const double DISTANCE_RESOLUTION = 0.01; //meters
+    static const uint16_t UPPER_BANK = 0xeeff;
+
+// special defines for lslidarlidar support
+    static const int FIRING_TOFFSET = 32;
+//    static const int PACKET_SIZE = 1206;
+    static const int BLOCKS_PER_PACKET = 12;
+    static const int SCANS_PER_PACKET = SCANS_PER_BLOCK  * BLOCKS_PER_PACKET; //384
+    //unit:meter
+    static const double R1_ = 0.04319;
+    static const double R2_ = 0.0494;
+// Pre-compute the sine and cosine for the altitude angles.
+    //1.33
+    static const double c16_1_vertical_angle[16] = {-10, 0.665, -8.665, 2, -7.33, 3.33, -6, 4.665, -4.665, 6, -3.33,
+                                                    7.33, -2, 8.665, -0.665, 10};
+    //2°
+    static const double c16_2_vertical_angle[16] = {-15, 1, -13, 3, -11, 5, -9, 7, -7, 9, -5, 11, -3, 13, -1, 15};
+
+
+    // 0.33°
+    static const double c32_1_vertical_angle[32] = {-18, -1, -15, -0.66, -12, -0.33, -10, 0, -8, 0.33, -7, 0.66, -6, 1,
+                                                    -5, 1.33, -4, 1.66, -3.33, 2, -3, 3, -2.66, 4, -2.33, 6, -2, 8,
+                                                    -1.66, 11, -1.33, 14};
+    static const double c32_1_vertical_angle_26[32] = {-18, -15, -12, -10, -8, -7, -6, -5, -4, -3.33, -2.66, -3,
+                                                       -2.33, -2, -1.33, -1.66, -1, -0.66, 0, -0.33, 0.33, 0.66,
+                                                       1.33, 1, 1.66, 2, 3, 4, 6, 8, 11, 14};
+    // 1°
+    static const double c32_2_vertical_angle[32] = {-16, 0, -15, 1, -14, 2, -13, 3, -12, 4, -11, 5, -10, 6, -9, 7, -8,
+                                                    8, -7, 9, -6, 10, -5, 11, -4, 12, -3, 13, -2, 14, -1, 15};
+
+    static const double c32_2_vertical_angle_26[32] = {-16, -15, -14, -13, -12, -11, -10, -9, -8, -7, -6, -5, -4,
+                                                       -3, -2, -1, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14,
+                                                       15};
+    static const uint8_t adjust_angle_index[32] = {0, 16, 1, 17, 2, 18, 3, 19, 4, 20, 5, 21, 6, 22, 7, 23, 8, 24, 9,
+                                                   25, 10, 26, 11, 27, 12, 28, 13, 29, 14, 30, 15, 31};
+
+
+    union TwoBytes {
+        uint16_t distance;
+        uint8_t bytes[2];
+    };
+
+    struct RawBlock {
+        uint16_t header;
+        uint16_t rotation;  //0-35999
+        uint8_t data[BLOCK_DATA_SIZE];
+    };
+
+    struct RawPacket {
+        RawBlock blocks[BLOCKS_PER_PACKET];
+        uint32_t time_stamp;
+        uint8_t factory[2];
+    };
+
+    struct Firing {
+        uint16_t firing_azimuth[BLOCKS_PER_PACKET];
+        int azimuth[SCANS_PER_PACKET];
+        double distance[SCANS_PER_PACKET];
+        double intensity[SCANS_PER_PACKET];
+    };
+
+/*    struct FiringC32 {
+        uint16_t firing_azimuth[BLOCKS_PER_PACKET];
+        int azimuth[SCANS_PER_PACKET];
+        double distance[SCANS_PER_PACKET];
+        double intensity[SCANS_PER_PACKET];
+    };*/
+
+    struct PointXYZIRT {
+        PCL_ADD_POINT4D
+        float intensity;
+        std::uint16_t ring;
+        float time;
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW //make sure our new allocators are aligned
+    }EIGEN_ALIGN16; //enforce SSE padding for correct memory alignment
+
+    static std::string lidar_type;
+
+    class lslidarDriver : public rclcpp::Node {
+    public:
+        lslidarDriver();
+
+        lslidarDriver(const rclcpp::NodeOptions &options);
+
+        virtual ~lslidarDriver() {}
+
+        bool checkPacketValidity(const RawPacket *packet);
+
+        //check if a point is in the required range
+        bool isPointInRange(const double &distance);
+
+        bool loadParameters();
+
+        void initTimeStamp();
+
+        bool createRosIO();
+
+        void publishPointcloud();
+
+        void publishScan();
+
+
+        bool lslidarControl(lslidar_msgs::srv::Lslidarcontrol::Request &req,
+                               lslidar_msgs::srv::Lslidarcontrol::Response &res);
+
+        bool SendPacketTolidar(bool power_switch);
+
+        virtual void difopPoll() = 0;
+
+        virtual bool poll() = 0;
+
+        virtual bool initialize() = 0;
+
+        virtual void decodePacket(const RawPacket *packet) = 0;
+
+
+    public:
+        int socket_id;
+        int msop_udp_port;
+        int difop_udp_port;
+        int scan_num;
+        int point_num;
+        int angle_disable_min;
+        int angle_disable_max;
+        uint16_t last_azimuth;
+        uint16_t last_azimuth_tmp;
+        uint64_t packet_time_s;
+        uint64_t packet_time_ns;
+        int return_mode;
+        int c32_fpga_type;
+        int config_vert_num;
+
+        in_addr lidar_ip;
+        std::string lidar_ip_string;
+        std::string group_ip_string;
+        std::string frame_id;
+        std::string dump_file;
+        std::string pointcloud_topic;
+        std::string c32_type;
+        std::string c16_type;
+
+        bool use_gps_ts;
+        bool pcl_type;
+        bool publish_scan;
+        bool coordinate_opt;
+        bool is_first_sweep;
+        bool add_multicast;
+        bool config_vert;
+        double distance_unit;
+        double min_range;
+        double max_range;
+        double sweep_end_time;
+        double horizontal_angle_resolution;
+        double packet_rate;
+        //double packet_start_time;
+        double angle_base;
+        double cos_azimuth_table[36000];
+        double sin_azimuth_table[36000];
+
+
+        std::shared_ptr<Input> msop_input_;
+        std::shared_ptr<Input> difop_input_;
+        std::shared_ptr<std::thread> difop_thread_;
+
+        lslidar_msgs::msg::LslidarScan::UniquePtr sweep_data;
+        //ros::Publisher packet_pub;
+        rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_pub;
+        rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr scan_pub;
+        rclcpp::Service<lslidar_msgs::srv::Lslidarcontrol>::SharedPtr lslidar_control;
+
+        unsigned char difop_data[1212];
+        unsigned char packetTimeStamp[10];
+        struct tm cur_time;
+        rclcpp::Time timeStamp;
+
+        int count;
+        bool is_update_gps_time;
+        double current_packet_time;
+        double last_packet_time;
+        double current_point_time=0.0;
+        double last_point_time=0.0;
+        rclcpp::Time timeStamp_bak;
+        int packet_size;
+
+
+    };
+
+
+    class lslidarC16Driver : public lslidarDriver {
+    public:
+        /**
+       * @brief lslidarDriver
+       * @param node          raw packet output topic
+       * @param private_nh    通过这个节点传参数
+       */
+        lslidarC16Driver();
+
+        virtual ~lslidarC16Driver();
+
+        virtual void difopPoll();
+
+        virtual bool poll(void);
+
+        virtual bool initialize();
+
+        virtual void decodePacket(const RawPacket *packet);
+
+
+    private:
+        Firing firings;
+        double c16_vertical_angle[16];
+        double c16_config_vertical_angle[16];
+        //double c16_scan_altitude[16];
+        double c16_cos_scan_altitude[16];
+        double c16_sin_scan_altitude[16];
+
+    };
+
+
+    class lslidarC32Driver : public lslidarDriver {
+    public:
+        lslidarC32Driver();
+
+        virtual ~lslidarC32Driver();
+
+        virtual void difopPoll();
+
+        virtual bool poll(void);
+
+        virtual bool initialize();
+
+        virtual void decodePacket(const RawPacket *packet);
+
+
+    private:
+        Firing firings;
+        double c32_vertical_angle[32];
+        double c32_config_vertical_angle[32];
+        double c32_config_tmp_angle[32];
+        // double c32_scan_altitude[32];
+        double c32_cos_scan_altitude[32];
+        double c32_sin_scan_altitude[32];
+        int adjust_angle[4];
+        bool get_difop;
+    };
+
+    typedef PointXYZIRT VPoint;
+    typedef pcl::PointCloud<VPoint> VPointcloud;
+
+}  // namespace lslidar_driver
+
+POINT_CLOUD_REGISTER_POINT_STRUCT(lslidar_driver::PointXYZIRT,
+                                  (float, x, x)
+                                          (float, y, y)
+                                          (float, z, z)
+                                          (float, intensity, intensity)
+                                          (std::uint16_t, ring, ring)
+                                          (float, time, time))
+
+#endif

src/ros2/src/Lslidar_ROS2_driver/lslidar_driver/launch/lslidar_c16_double_launch.py

@@ -0,0 +1,85 @@
+#!/usr/bin/python3
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import LifecycleNode
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+
+import lifecycle_msgs.msg
+import os
+import subprocess
+
+def generate_launch_description():
+    driver_dir_left = os.path.join(get_package_share_directory('lslidar_driver'), 'params', 'lslidar_c16_1.yaml')
+    driver_dir_right = os.path.join(get_package_share_directory('lslidar_driver'), 'params', 'lslidar_c16_2.yaml')
+    rviz_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'rviz_cfg', 'lslidar_c16.rviz')
+
+    p = subprocess.Popen("echo $ROS_DISTRO", stdout=subprocess.PIPE, shell=True)
+    driver_node_left = ""
+    driver_node_right = ""
+    rviz_node = ""
+    ros_version = p.communicate()[0]
+    print(ros_version)
+    if ros_version == b'dashing\n' or ros_version == b'eloquent\n':
+        print("ROS VERSION: dashing/eloquent")
+        driver_node_left = LifecycleNode(package='lslidar_driver',
+                                         node_namespace='c16_left',
+                                         node_executable='lslidar_c16driver_node',
+                                         node_name='lslidar_driver_node',
+                                         output='screen',
+                                         parameters=[driver_dir_left],
+                                         )
+
+        driver_node_right = LifecycleNode(package='lslidar_driver',
+                                          node_namespace='c16_right',
+                                          node_executable='lslidar_c16driver_node',
+                                          node_name='lslidar_driver_node',
+                                          output='screen',
+                                          parameters=[driver_dir_right],
+                                          )
+
+        rviz_node = Node(
+            package='rviz2',
+            node_namespace='c16',
+            node_executable='rviz2',
+            node_name='rviz2',
+            arguments=['-d', rviz_dir],
+            output='screen')
+    elif ros_version == b'foxy\n' or ros_version == b'galactic\n' or ros_version == b'humble\n':
+        print("ROS VERSION: foxy/galactic/humble")
+        driver_node_left = LifecycleNode(package='lslidar_driver',
+                                         namespace='c16_left',
+                                         executable='lslidar_c16driver_node',
+                                         name='lslidar_driver_node',
+                                         output='screen',
+                                         emulate_tty=True,
+                                         parameters=[driver_dir_left],
+                                         )
+
+        driver_node_right = LifecycleNode(package='lslidar_driver',
+                                          namespace='c16_right',
+                                          executable='lslidar_c16driver_node',
+                                          name='lslidar_driver_node',
+                                          output='screen',
+                                          emulate_tty=True,
+                                          parameters=[driver_dir_right],
+                                          )
+
+        rviz_node = Node(
+            package='rviz2',
+            namespace='c16',
+            executable='rviz2',
+            name='rviz2',
+            arguments=['-d', rviz_dir],
+            output='screen')
+
+    else:
+        print("Please configure the ros environment")
+        exit()
+
+    return LaunchDescription([
+        driver_node_left,
+        driver_node_right,
+        rviz_node
+    ])

src/ros2/src/Lslidar_ROS2_driver/lslidar_driver/launch/lslidar_c16_launch.py

@@ -0,0 +1,63 @@
+#!/usr/bin/python3
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import LifecycleNode
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+
+import lifecycle_msgs.msg
+import os
+import subprocess
+
+
+def generate_launch_description():
+    driver_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'params', 'lslidar_c16.yaml')
+    rviz_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'rviz_cfg', 'lslidar_c16.rviz')
+
+    p = subprocess.Popen("echo $ROS_DISTRO", stdout=subprocess.PIPE, shell=True)
+    driver_node = ""
+    rviz_node = ""
+    ros_version = p.communicate()[0]
+    print(ros_version)
+    if ros_version == b'dashing\n' or ros_version == b'eloquent\n':
+        print("ROS VERSION: dashing/eloquent")
+        driver_node = LifecycleNode(package='lslidar_driver',
+                                    node_namespace='c16',
+                                    node_executable='lslidar_c16driver_node',
+                                    node_name='lslidar_driver_node',
+                                    output='screen',
+                                    parameters=[driver_dir],
+                                    )
+        rviz_node = Node(
+            package='rviz2',
+            node_namespace='c16',
+            node_executable='rviz2',
+            node_name='rviz2',
+            arguments=['-d', rviz_dir],
+            output='screen')
+    elif ros_version == b'foxy\n' or ros_version == b'galactic\n' or ros_version == b'humble\n':
+        print("ROS VERSION: foxy/galactic/humble")
+        driver_node = LifecycleNode(package='lslidar_driver',
+                                    namespace='c16',
+                                    executable='lslidar_c16driver_node',
+                                    name='lslidar_driver_node',
+                                    output='screen',
+                                    emulate_tty=True,
+                                    parameters=[driver_dir],
+                                    )
+        rviz_node = Node(
+            package='rviz2',
+            namespace='c16',
+            executable='rviz2',
+            name='rviz2',
+            arguments=['-d', rviz_dir],
+            output='screen')
+
+    else:
+        print("Please configure the ros environment")
+        exit()
+
+    return LaunchDescription([
+        driver_node, rviz_node
+    ])

src/ros2/src/Lslidar_ROS2_driver/lslidar_driver/launch/lslidar_c32_double_launch.py

@@ -0,0 +1,85 @@
+#!/usr/bin/python3
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import LifecycleNode
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+
+import lifecycle_msgs.msg
+import os
+import subprocess
+
+def generate_launch_description():
+    driver_dir_left = os.path.join(get_package_share_directory('lslidar_driver'), 'params', 'lslidar_c32_1.yaml')
+    driver_dir_right = os.path.join(get_package_share_directory('lslidar_driver'), 'params', 'lslidar_c32_2.yaml')
+    rviz_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'rviz_cfg', 'lslidar_c32.rviz')
+
+    p = subprocess.Popen("echo $ROS_DISTRO", stdout=subprocess.PIPE, shell=True)
+    driver_node_left = ""
+    driver_node_right = ""
+    rviz_node = ""
+    ros_version = p.communicate()[0]
+    print(ros_version)
+    if ros_version == b'dashing\n' or ros_version == b'eloquent\n':
+        print("ROS VERSION: dashing/eloquent")
+        driver_node_left = LifecycleNode(package='lslidar_driver',
+                                         node_namespace='c32_left',
+                                         node_executable='lslidar_c32driver_node',
+                                         node_name='lslidar_driver_node',
+                                         output='screen',
+                                         parameters=[driver_dir_left],
+                                         )
+
+        driver_node_right = LifecycleNode(package='lslidar_driver',
+                                          node_namespace='c32_right',
+                                          node_executable='lslidar_c32driver_node',
+                                          node_name='lslidar_driver_node',
+                                          output='screen',
+                                          parameters=[driver_dir_right],
+                                          )
+
+        rviz_node = Node(
+            package='rviz2',
+            node_namespace='c32',
+            node_executable='rviz2',
+            node_name='rviz2',
+            arguments=['-d', rviz_dir],
+            output='screen')
+    elif ros_version == b'foxy\n' or ros_version == b'galactic\n' or ros_version == b'humble\n':
+        print("ROS VERSION: foxy/galactic/humble")
+        driver_node_left = LifecycleNode(package='lslidar_driver',
+                                         namespace='c32_left',
+                                         executable='lslidar_c32driver_node',
+                                         name='lslidar_driver_node',
+                                         output='screen',
+                                         emulate_tty=True,
+                                         parameters=[driver_dir_left],
+                                         )
+
+        driver_node_right = LifecycleNode(package='lslidar_driver',
+                                          namespace='c32_right',
+                                          executable='lslidar_c32driver_node',
+                                          name='lslidar_driver_node',
+                                          output='screen',
+                                          emulate_tty=True,
+                                          parameters=[driver_dir_right],
+                                          )
+
+        rviz_node = Node(
+            package='rviz2',
+            namespace='c32',
+            executable='rviz2',
+            name='rviz2',
+            arguments=['-d', rviz_dir],
+            output='screen')
+
+    else:
+        print("Please configure the ros environment")
+        exit()
+
+    return LaunchDescription([
+        driver_node_left,
+        driver_node_right,
+        rviz_node
+    ])

src/ros2/src/Lslidar_ROS2_driver/lslidar_driver/launch/lslidar_c32_launch.py

@@ -0,0 +1,63 @@
+#!/usr/bin/python3
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import LifecycleNode
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+
+import lifecycle_msgs.msg
+import os
+import subprocess
+
+
+def generate_launch_description():
+    driver_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'params', 'lslidar_c32.yaml')
+    rviz_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'rviz_cfg', 'lslidar_c32.rviz')
+
+    p = subprocess.Popen("echo $ROS_DISTRO", stdout=subprocess.PIPE, shell=True)
+    driver_node = ""
+    rviz_node = ""
+    ros_version = p.communicate()[0]
+    print(ros_version)
+    if ros_version == b'dashing\n' or ros_version == b'eloquent\n':
+        print("ROS VERSION: dashing/eloquent")
+        driver_node = LifecycleNode(package='lslidar_driver',
+                                    node_namespace='c32',
+                                    node_executable='lslidar_c32driver_node',
+                                    node_name='lslidar_driver_node',
+                                    output='screen',
+                                    parameters=[driver_dir],
+                                    )
+        rviz_node = Node(
+            package='rviz2',
+            node_namespace='c32',
+            node_executable='rviz2',
+            node_name='rviz2',
+            arguments=['-d', rviz_dir],
+            output='screen')
+    elif ros_version == b'foxy\n' or ros_version == b'galactic\n' or ros_version == b'humble\n':
+        print("ROS VERSION: foxy/galactic/humble")
+        driver_node = LifecycleNode(package='lslidar_driver',
+                                    namespace='c32',
+                                    executable='lslidar_c32driver_node',
+                                    name='lslidar_driver_node',
+                                    output='screen',
+                                    emulate_tty=True,
+                                    parameters=[driver_dir],
+                                    )
+        rviz_node = Node(
+            package='rviz2',
+            namespace='c32',
+            executable='rviz2',
+            name='rviz2',
+            arguments=['-d', rviz_dir],
+            output='screen')
+
+    else:
+        print("Please configure the ros environment")
+        exit()
+
+    return LaunchDescription([
+        driver_node, rviz_node
+    ])

src/ros2/src/Lslidar_ROS2_driver/lslidar_driver/package.xml

@@ -0,0 +1,42 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>lslidar_driver</name>
+  <version>1.0.0</version>
+
+  <maintainer email="tongsky723@sina.com">tongsky</maintainer>
+  <author>Yutong</author>
+  <license>BSD</license>
+  <description> ROS device driver for C32 lidar </description>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>angles</build_depend>
+  <build_depend>pcl_conversions</build_depend>
+  <build_depend>pluginlib</build_depend>
+  <build_depend>rclcpp</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>rclpy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>rslidar_input</build_depend>
+  <build_depend>libpcl-all-dev</build_depend>
+  <build_depend>lslidar_msgs</build_depend>
+  <build_depend>libpcap</build_depend>
+
+  <exec_depend>angles</exec_depend>
+  <exec_depend>pluginlib</exec_depend>
+  <exec_depend>rclcpp</exec_depend>
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
+  <exec_depend>pcl_conversions</exec_depend>
+  <exec_depend>libpcl-all</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>lslidar_msgs</exec_depend>
+  <exec_depend>libpcap</exec_depend>
+
+
+  <depend>diagnostic_updater</depend>
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/ros2/src/Lslidar_ROS2_driver/lslidar_driver/params/lslidar_c16.yaml

@@ -0,0 +1,27 @@
+/c16/lslidar_driver_node:
+  ros__parameters:
+    packet_size: 1206                      # 雷达数据包长度(字节)，填1212/1206
+    device_ip: 192.168.1.200
+    msop_port: 2368
+    difop_port: 2369
+    frame_id: laser_link
+    add_multicast: false
+    group_ip: 224.1.1.2
+    use_gps_ts: false
+    lidar_type: c16                         #c16表示机械式16线雷达；c32表示机械式32线雷达
+    c16_type: c16_2                         #c16_2表示16线垂直角度分辨率为2度的雷达，c16_1表示16线垂直角度分辨率为1.33度的雷达
+    c32_type: c32_2                         #c32_2表示32线垂直角度分辨率为1度的雷达，c32_1表示32线垂直角度分辨率为0.33度的雷达
+    c32_fpga_type: 3                        #3表示32线fpga为2.7\2.8\3.0的版本的雷达，2表示32线fpga为2.6的版本的雷达
+    min_range: 0.3
+    max_range: 200.0
+    distance_unit: 0.25
+    angle_disable_min: 0
+    angle_disable_max: 0
+    scan_num: 8
+    horizontal_angle_resolution: 0.18
+    packet_rate: 840.0
+    topic_name: lslidar_point_cloud
+    publish_scan: true
+    pcl_type: false
+    coordinate_opt: false
+#    pcap: /home/ls/xxx.pcap                        #pcap包路径，加载pcap包时打开此注释

src/ros2/src/Lslidar_ROS2_driver/lslidar_driver/params/lslidar_c16_1.yaml

@@ -0,0 +1,27 @@
+/c16_left/lslidar_driver_node:
+  ros__parameters:
+    packet_size: 1206                  # 雷达数据包长度(字节)，填1212/1206
+    device_ip: 192.168.1.200
+    msop_port: 2368
+    difop_port: 2369
+    frame_id: laser_link
+    add_multicast: false
+    group_ip: 224.1.1.2
+    use_gps_ts: false
+    lidar_type: c16                         #c16表示机械式16线雷达；c32表示机械式32线雷达
+    c16_type: c16_2                         #c16_2表示16线垂直角度分辨率为2度的雷达，c16_1表示16线垂直角度分辨率为1.33度的雷达
+    c32_type: c32_2                         #c32_2表示32线垂直角度分辨率为1度的雷达，c32_1表示32线垂直角度分辨率为0.33度的雷达
+    c32_fpga_type: 3                        #3表示32线fpga为2.7\2.8\3.0的版本的雷达，2表示32线fpga为2.6的版本的雷达
+    min_range: 0.3
+    max_range: 200.0
+    distance_unit: 0.25
+    angle_disable_min: 0
+    angle_disable_max: 0
+    scan_num: 15
+    horizontal_angle_resolution: 0.18
+    packet_rate: 840.0
+    topic_name: lslidar_point_cloud
+    publish_scan: false
+    pcl_type: false
+    coordinate_opt: false
+    #pcap: /home/chris/Documents/leishen/1212bytes_c32/C32_220714_1801.pcap                        #pcap包路径，加载pcap包时打开此注释

src/ros2/src/Lslidar_ROS2_driver/lslidar_driver/params/lslidar_c16_2.yaml

@@ -0,0 +1,27 @@
+/c16_right/lslidar_driver_node:
+  ros__parameters:
+    packet_size: 1206                  # 雷达数据包长度(字节)，填1212/1206
+    device_ip: 192.168.1.200
+    msop_port: 2368
+    difop_port: 2369
+    frame_id: laser_link
+    add_multicast: false
+    group_ip: 224.1.1.2
+    use_gps_ts: false
+    lidar_type: c16                         #c16表示机械式16线雷达；c32表示机械式32线雷达
+    c16_type: c16_2                         #c16_2表示16线垂直角度分辨率为2度的雷达，c16_1表示16线垂直角度分辨率为1.33度的雷达
+    c32_type: c32_2                         #c32_2表示32线垂直角度分辨率为1度的雷达，c32_1表示32线垂直角度分辨率为0.33度的雷达
+    c32_fpga_type: 3                        #3表示32线fpga为2.7\2.8\3.0的版本的雷达，2表示32线fpga为2.6的版本的雷达
+    min_range: 0.3
+    max_range: 200.0
+    distance_unit: 0.25
+    angle_disable_min: 0
+    angle_disable_max: 0
+    scan_num: 15
+    horizontal_angle_resolution: 0.18
+    packet_rate: 840.0
+    topic_name: lslidar_point_cloud
+    publish_scan: false
+    pcl_type: false
+    coordinate_opt: false
+    #pcap: /home/chris/Documents/leishen/1212bytes_c32/C32_220714_1801.pcap                        #pcap包路径，加载pcap包时打开此注释

src/ros2/src/Lslidar_ROS2_driver/lslidar_driver/params/lslidar_c32.yaml

@@ -0,0 +1,27 @@
+/c32/lslidar_driver_node:
+  ros__parameters:
+    packet_size: 1206                  # 雷达数据包长度(字节)，填1212/1206
+    device_ip: 192.168.1.200
+    msop_port: 2368
+    difop_port: 2369
+    frame_id: laser_link
+    add_multicast: false
+    group_ip: 224.1.1.2
+    use_gps_ts: false
+    lidar_type: c32                         #c16表示机械式16线雷达；c32表示机械式32线雷达
+    c16_type: c16_2                         #c16_2表示16线垂直角度分辨率为2度的雷达，c16_1表示16线垂直角度分辨率为1.33度的雷达
+    c32_type: c32_2                         #c32_2表示32线垂直角度分辨率为1度的雷达，c32_1表示32线垂直角度分辨率为0.33度的雷达
+    c32_fpga_type: 3                        #3表示32线fpga为2.7\2.8\3.0的版本的雷达，2表示32线fpga为2.6的版本的雷达
+    min_range: 0.3
+    max_range: 200.0
+    distance_unit: 0.4
+    angle_disable_min: 0
+    angle_disable_max: 0
+    scan_num: 15
+    horizontal_angle_resolution: 0.18
+    packet_rate: 1700.0
+    topic_name: lslidar_point_cloud
+    publish_scan: false
+    pcl_type: false
+    coordinate_opt: false
+    #pcap: /home/chris/Documents/leishen/1212bytes_c32/gps.pcap                        #pcap包路径，加载pcap包时打开此注释

src/ros2/src/Lslidar_ROS2_driver/lslidar_driver/params/lslidar_c32_1.yaml

@@ -0,0 +1,27 @@
+/c32_left/lslidar_driver_node:
+  ros__parameters:
+    packet_size: 1206                  # 雷达数据包长度(字节)，填1212/1206
+    device_ip: 192.168.1.200
+    msop_port: 2368
+    difop_port: 2369
+    frame_id: laser_link
+    add_multicast: false
+    group_ip: 224.1.1.2
+    use_gps_ts: false
+    lidar_type: c32                         #c16表示机械式16线雷达；c32表示机械式32线雷达
+    c16_type: c16_2                         #c16_2表示16线垂直角度分辨率为2度的雷达，c16_1表示16线垂直角度分辨率为1.33度的雷达
+    c32_type: c32_2                         #c32_2表示32线垂直角度分辨率为1度的雷达，c32_1表示32线垂直角度分辨率为0.33度的雷达
+    c32_fpga_type: 3                        #3表示32线fpga为2.7\2.8\3.0的版本的雷达，2表示32线fpga为2.6的版本的雷达
+    min_range: 0.3
+    max_range: 200.0
+    distance_unit: 0.4
+    angle_disable_min: 0
+    angle_disable_max: 0
+    scan_num: 15
+    horizontal_angle_resolution: 0.18
+    packet_rate: 1700.0
+    topic_name: lslidar_point_cloud
+    publish_scan: false
+    pcl_type: false
+    coordinate_opt: false
+    #pcap: /home/chris/Documents/leishen/1212bytes_c32/C32_220714_1801.pcap                        #pcap包路径，加载pcap包时打开此注释

src/ros2/src/Lslidar_ROS2_driver/lslidar_driver/params/lslidar_c32_2.yaml

@@ -0,0 +1,27 @@
+/c32_right/lslidar_driver_node:
+  ros__parameters:
+    packet_size: 1206                      # 雷达数据包长度(字节)，填1212/1206
+    device_ip: 192.168.1.200
+    msop_port: 2368
+    difop_port: 2369
+    frame_id: laser_link
+    add_multicast: false
+    group_ip: 224.1.1.2
+    use_gps_ts: false
+    lidar_type: c32                         #c16表示机械式16线雷达；c32表示机械式32线雷达
+    c16_type: c16_2                         #c16_2表示16线垂直角度分辨率为2度的雷达，c16_1表示16线垂直角度分辨率为1.33度的雷达
+    c32_type: c32_2                         #c32_2表示32线垂直角度分辨率为1度的雷达，c32_1表示32线垂直角度分辨率为0.33度的雷达
+    c32_fpga_type: 3                        #3表示32线fpga为2.7\2.8\3.0的版本的雷达，2表示32线fpga为2.6的版本的雷达
+    min_range: 0.3
+    max_range: 200.0
+    distance_unit: 0.4
+    angle_disable_min: 0
+    angle_disable_max: 0
+    scan_num: 15
+    horizontal_angle_resolution: 0.18
+    packet_rate: 1700.0
+    topic_name: lslidar_point_cloud
+    publish_scan: false
+    pcl_type: false
+    coordinate_opt: false
+    #pcap: /home/chris/Documents/leishen/1212bytes_c32/C32_220714_1801.pcap                        #pcap包路径，加载pcap包时打开此注释

src/ros2/src/Lslidar_ROS2_driver/lslidar_driver/rviz_cfg/lslidar_c16.rviz

@@ -0,0 +1,164 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /PointCloud21
+      Splitter Ratio: 0.35121950507164
+    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: 8
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /c16/lslidar_point_cloud
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Class: rviz_default_plugins/Axes
+      Enabled: true
+      Length: 1
+      Name: Axes
+      Radius: 0.10000000149011612
+      Reference Frame: <Fixed Frame>
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: laser_link
+    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
+      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: 12.808707237243652
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.8897970914840698
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 1.8403964042663574
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1016
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000001620000039efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004bb0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1848
+  X: 72
+  Y: 27

src/ros2/src/Lslidar_ROS2_driver/lslidar_driver/rviz_cfg/lslidar_c32.rviz

@@ -0,0 +1,197 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /PointCloud21
+      Splitter Ratio: 0.35121950507164
+    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: 8
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /c32/lslidar_point_cloud
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Class: rviz_default_plugins/Axes
+      Enabled: true
+      Length: 1
+      Name: Axes
+      Radius: 0.10000000149011612
+      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/LaserScan
+      Color: 255; 255; 255
+      Color Transformer: ""
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: LaserScan
+      Position Transformer: ""
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /c32/scan
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: laser_link
+    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
+      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: 12.808707237243652
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.8897970914840698
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 1.8403964042663574
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1016
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000001620000039efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004bb0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1848
+  X: 72
+  Y: 27

src/ros2/src/Lslidar_ROS2_driver/lslidar_driver/src/input.cpp

@@ -0,0 +1,292 @@
+#include "lslidar_driver/input.h"
+#include <cmath>
+
+extern volatile sig_atomic_t flag;
+namespace lslidar_driver {
+//    static const size_t packet_size = sizeof(lslidar_msgs::msg::LslidarPacket().data);
+////////////////////////////////////////////////////////////////////////
+// Input base class implementation
+////////////////////////////////////////////////////////////////////////
+
+/** @brief constructor
+ *
+ *  @param private_nh ROS private handle for calling node.
+ *  @param port UDP port number.
+ */
+    Input::Input(rclcpp::Node *private_nh, uint16_t port) : private_nh_(private_nh), port_(port) {
+        npkt_update_flag_ = false;
+        cur_rpm_ = 0;
+        return_mode_ = 1;
+        packet_size_=1212;
+//        private_nh->declare_parameter<std::string>("device_ip", "");
+//        private_nh->declare_parameter<bool>("add_multicast", false);
+//        private_nh->declare_parameter<std::string>("group_ip", "224.1.1.2");
+//        private_nh->declare_parameter<int>("packet_size", 1206);
+
+        private_nh->get_parameter("device_ip", devip_str_);
+        private_nh->get_parameter("add_multicast", add_multicast);
+        private_nh->get_parameter("group_ip", group_ip);
+        private_nh->get_parameter("packet_size", packet_size_);
+		RCLCPP_INFO(private_nh->get_logger(), "line: %d,packet size: %d ",__LINE__,packet_size_);
+
+        if (!devip_str_.empty())
+            RCLCPP_INFO(private_nh->get_logger(), "[driver][input] accepting packets from IP address: %s  port: %d",
+                        devip_str_.c_str(),port);
+    }
+
+
+////////////////////////////////////////////////////////////////////////
+// InputSocket class implementation
+////////////////////////////////////////////////////////////////////////
+
+/** @brief constructor
+   *
+   *  @param private_nh ROS private handle for calling node.
+   *  @param port UDP port number
+*/
+    InputSocket::InputSocket(rclcpp::Node *private_nh, uint16_t port) : Input(private_nh, port) {
+        sockfd_ = -1;
+
+        if (!devip_str_.empty()) {
+            inet_aton(devip_str_.c_str(), &devip_);
+        }
+
+        RCLCPP_INFO(private_nh_->get_logger(), "[driver][socket] Opening UDP socket: port %d", port);
+        sockfd_ = socket(PF_INET, SOCK_DGRAM, 0);
+        if (sockfd_ == -1) {
+            perror("socket");  // TODO: ROS_ERROR errno
+            return;
+        }
+
+        int opt = 1;
+        if (setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (const void *) &opt, sizeof(opt))) {
+            perror("setsockopt error!\n");
+            return;
+        }
+
+        sockaddr_in my_addr;                   // my address information
+        memset(&my_addr, 0, sizeof(my_addr));  // initialize to zeros
+        my_addr.sin_family = AF_INET;          // host byte order
+        my_addr.sin_port = htons(port);        // port in network byte order
+        my_addr.sin_addr.s_addr = INADDR_ANY;  // automatically fill in my IP
+
+        if (bind(sockfd_, (sockaddr * ) & my_addr, sizeof(sockaddr)) == -1) {
+            perror("bind");  // TODO: ROS_ERROR errno
+            return;
+        }
+
+        if (add_multicast) {
+            struct ip_mreq group;
+            group.imr_multiaddr.s_addr = inet_addr(group_ip.c_str());
+            group.imr_interface.s_addr = htonl(INADDR_ANY);
+            //group.imr_interface.s_addr = inet_addr("192.168.1.102");
+
+            if (setsockopt(sockfd_, IPPROTO_IP, IP_ADD_MEMBERSHIP, (char *) &group, sizeof(group)) < 0) {
+                perror("Adding multicast group error ");
+                close(sockfd_);
+                exit(1);
+            } else
+                printf("Adding multicast group...OK.\n");
+        }
+        if (fcntl(sockfd_, F_SETFL, O_NONBLOCK | FASYNC) < 0) {
+            perror("non-block");
+            return;
+        }
+    }
+
+/** @brief destructor */
+    InputSocket::~InputSocket(void) {
+        (void) close(sockfd_);
+    }
+
+/** @brief Get one lslidar packet. */
+    int InputSocket::getPacket(lslidar_msgs::msg::LslidarPacket::UniquePtr &pkt) {
+        struct pollfd fds[1];
+        fds[0].fd = sockfd_;
+        fds[0].events = POLLIN;
+        static const int POLL_TIMEOUT = 3000;  // one second (in msec)
+
+        sockaddr_in sender_address{};
+        socklen_t sender_address_len = sizeof(sender_address);
+        while (flag == 1)
+            // while (true)
+        {
+            // Receive packets that should now be available from the
+            // socket using a blocking read.
+            // poll() until input available
+            do {
+                int retval = poll(fds, 1, POLL_TIMEOUT);
+                if (retval < 0)  // poll() error?
+                {
+                    if (errno != EINTR)
+                        RCLCPP_ERROR(private_nh_->get_logger(), "[driver][socket] poll() error: %s", strerror(errno));
+                    return 1;
+                }
+                if (retval == 0)  // poll() timeout?
+                {
+                    time_t curTime = time(NULL);
+                    struct tm *curTm = localtime(&curTime);
+                    char bufTime[30] = {0};
+                    sprintf(bufTime, "%d-%d-%d %d:%d:%d", curTm->tm_year + 1900, curTm->tm_mon + 1,
+                            curTm->tm_mday, curTm->tm_hour, curTm->tm_min, curTm->tm_sec);
+
+                    RCLCPP_WARN(private_nh_->get_logger(), "%s  lslidar poll() timeout, port: %d", bufTime,port_);
+                    /*
+                    char buffer_data[8] = "re-con";
+                    memset(&sender_address, 0, sender_address_len);          // initialize to zeros
+                    sender_address.sin_family = AF_INET;                     // host byte order
+                    sender_address.sin_port = htons(MSOP_DATA_PORT_NUMBER);  // port in network byte order, set any value
+                    sender_address.sin_addr.s_addr = devip_.s_addr;          // automatically fill in my IP
+                    sendto(sockfd_, &buffer_data, strlen(buffer_data), 0, (sockaddr*)&sender_address, sender_address_len);
+                    */
+                    return 1;
+                }
+                if ((fds[0].revents & POLLERR) || (fds[0].revents & POLLHUP) ||
+                    (fds[0].revents & POLLNVAL))  // device error?
+                {
+                    RCLCPP_ERROR(private_nh_->get_logger(),"poll() reports lslidar error");
+                    return 1;
+                }
+            } while ((fds[0].revents & POLLIN) == 0);
+            ssize_t nbytes = recvfrom(sockfd_, &pkt->data[0], packet_size_, 0, (sockaddr * ) & sender_address,
+                                      &sender_address_len);
+
+            if (nbytes < 0) {
+                if (errno != EWOULDBLOCK) {
+                    perror("recvfail");
+                    RCLCPP_ERROR(private_nh_->get_logger(),"recvfail");
+                    return 1;
+                }
+            } else if ((size_t) nbytes == packet_size_) {
+                if (devip_str_ != "" && sender_address.sin_addr.s_addr != devip_.s_addr) {
+                    RCLCPP_ERROR(private_nh_->get_logger(), "lidar IP parameter set error,please reset in launch file");
+                    continue;
+                } else
+                    break;  // done
+            }
+            RCLCPP_WARN(private_nh_->get_logger(), "[driver][socket] incomplete lslidar packet read: %d bytes", nbytes);
+        }
+        if (flag == 0) {
+            abort();
+        }
+
+        return 0;
+    }
+
+////////////////////////////////////////////////////////////////////////
+// InputPCAP class implementation
+////////////////////////////////////////////////////////////////////////
+
+/** @brief constructor
+   *
+   *  @param private_nh ROS private handle for calling node.
+   *  @param port UDP port number
+   *  @param packet_rate expected device packet frequency (Hz)
+   *  @param filename PCAP dump file name
+   */
+    InputPCAP::InputPCAP(rclcpp::Node *private_nh, uint16_t port, double packet_rate, std::string filename)
+            : Input(private_nh, port), packet_rate_(packet_rate), filename_(filename) {
+        pcap_ = NULL;
+        empty_ = true;
+        read_once_ = false;
+        read_fast_ = false;
+        repeat_delay_ = 0.0;
+//        private_nh->declare_parameter<bool>("read_once", false);
+//        private_nh->declare_parameter<bool>("read_fast", false);
+//        private_nh->declare_parameter<double>("repeat_delay", 0.0);
+
+        private_nh->get_parameter("read_once", read_once_);
+        private_nh->get_parameter("read_fast", read_fast_);
+        private_nh->get_parameter("repeat_delay", repeat_delay_);
+
+        if (read_once_)
+            RCLCPP_WARN(private_nh_->get_logger(),"Read input file only once.");
+        if (read_fast_)
+            RCLCPP_WARN(private_nh_->get_logger(),"Read input file as quickly as possible.");
+        if (repeat_delay_ > 0.0)
+            RCLCPP_WARN(private_nh_->get_logger(),"Delay %.3f seconds before repeating input file.", repeat_delay_);
+
+        // Open the PCAP dump file
+        // ROS_INFO("Opening PCAP file \"%s\"", filename_.c_str());
+        RCLCPP_WARN(private_nh_->get_logger(),"Opening PCAP file %s",filename_.c_str());
+        if ((pcap_ = pcap_open_offline(filename_.c_str(), errbuf_)) == NULL) {
+            RCLCPP_WARN(private_nh_->get_logger(),"Error opening lslidar socket dump file.");
+            return;
+        }
+
+        std::stringstream filter;
+        if (devip_str_ != "")  // using specific IP?
+        {
+            filter << "src host " << devip_str_ << " && ";
+        }
+        filter << "udp dst port " << port;
+        pcap_compile(pcap_, &pcap_packet_filter_, filter.str().c_str(), 1, PCAP_NETMASK_UNKNOWN);
+    }
+
+/** destructor */
+    InputPCAP::~InputPCAP(void) {
+        pcap_close(pcap_);
+    }
+
+/** @brief Get one lslidar packet. */
+    int InputPCAP::getPacket(lslidar_msgs::msg::LslidarPacket::UniquePtr &pkt) {
+        struct pcap_pkthdr *header;
+        const u_char *pkt_data;
+
+        // while (flag == 1)
+        while (flag == 1) {
+            int res;
+            if ((res = pcap_next_ex(pcap_, &header, &pkt_data)) >= 0) {
+                // Skip packets not for the correct port and from the
+                // selected IP address.
+                if (!devip_str_.empty() && (0 == pcap_offline_filter(&pcap_packet_filter_, header, pkt_data)))
+                    continue;
+
+                // Keep the reader from blowing through the file.
+                if (read_fast_ == false)
+                    packet_rate_.sleep();
+
+
+                memcpy(&pkt->data[0], pkt_data + 42, packet_size_);
+
+                if (pkt->data[0] == 0xA5 && pkt->data[1] == 0xFF && pkt->data[2] == 0x00 &&
+                    pkt->data[3] == 0x5A) {//difop
+                    int rpm = (pkt->data[8] << 8) | pkt->data[9];
+                    RCLCPP_WARN_ONCE(private_nh_->get_logger(),"lidar rpm: %d", rpm);
+                }
+                empty_ = false;
+                return 0;  // success
+            }
+
+            if (empty_)  // no data in file?
+            {
+                RCLCPP_WARN(private_nh_->get_logger(),"Error %d reading lslidar packet: %s", res, pcap_geterr(pcap_));
+                return -1;
+            }
+
+            if (read_once_) {
+                RCLCPP_WARN(private_nh_->get_logger(),"end of file reached -- done reading.");
+                return -1;
+            }
+
+            if (repeat_delay_ > 0.0) {
+                RCLCPP_WARN(private_nh_->get_logger(),"end of file reached -- delaying %.3f seconds.", repeat_delay_);
+                usleep(rint(repeat_delay_ * 1000000.0));
+            }
+
+            RCLCPP_WARN(private_nh_->get_logger(),"replaying lslidar dump file");
+
+            // I can't figure out how to rewind the file, because it
+            // starts with some kind of header.  So, close the file
+            // and reopen it with pcap.
+            pcap_close(pcap_);
+            pcap_ = pcap_open_offline(filename_.c_str(), errbuf_);
+            empty_ = true;  // maybe the file disappeared?
+        }                 // loop back and try again
+
+        if (flag == 0) {
+            abort();
+        }
+        return 0;
+    }
+}

src/ros2/src/Lslidar_ROS2_driver/lslidar_driver/src/lslidar_c16driver_node.cpp

@@ -0,0 +1,42 @@
+#include "lslidar_driver/lslidar_driver.h"
+#include "std_msgs/msg/string.h"
+
+using namespace lslidar_driver;
+volatile sig_atomic_t flag = 1;
+
+static void my_handler(int sig) {
+    flag = 0;
+}
+
+
+int main(int argc, char **argv) {
+    rclcpp::init(argc, argv);
+    signal(SIGINT, my_handler);
+    lidar_type = "c16";
+    printf("lslidar type: %s", lidar_type.c_str());
+
+    // start the driver
+    if ("c16" == lidar_type) {
+        auto node = std::make_shared<lslidar_driver::lslidarC16Driver>();
+        if (!node->initialize()) {
+            RCLCPP_ERROR(node->get_logger(), "cannot initialize lslidar driver.");
+            return 0;
+        }
+        while (rclcpp::ok()) {
+            node->poll();
+        }
+        rclcpp::shutdown();
+
+    } else {
+        auto node = std::make_shared<lslidar_driver::lslidarC32Driver>();
+        if (!node->initialize()) {
+            RCLCPP_ERROR(node->get_logger(), "cannot initialize lslidar driver.");
+            return 0;
+        }
+        while (rclcpp::ok()) {
+            node->poll();
+        }
+        rclcpp::shutdown();
+    }
+    return 0;
+}

src/ros2/src/Lslidar_ROS2_driver/lslidar_driver/src/lslidar_c32driver_node.cpp

@@ -0,0 +1,40 @@
+#include "lslidar_driver/lslidar_driver.h"
+#include "std_msgs/msg/string.h"
+
+using namespace lslidar_driver;
+volatile sig_atomic_t flag = 1;
+
+static void my_handler(int sig) {
+    flag = 0;
+}
+
+int main(int argc, char **argv) {
+    rclcpp::init(argc, argv);
+    signal(SIGINT, my_handler);
+    lidar_type = "c32";
+
+    // start the driver
+    if ("c16" == lidar_type) {
+        auto node = std::make_shared<lslidar_driver::lslidarC16Driver>();
+        if (!node->initialize()) {
+            RCLCPP_ERROR(node->get_logger(), "cannot initialize lslidar driver.");
+            return 0;
+        }
+        while (rclcpp::ok()) {
+            node->poll();
+        }
+        rclcpp::shutdown();
+
+    } else {
+        auto node = std::make_shared<lslidar_driver::lslidarC32Driver>();
+        if (!node->initialize()) {
+            RCLCPP_ERROR(node->get_logger(), "cannot initialize lslidar driver.");
+            return 0;
+        }
+        while (rclcpp::ok()) {
+            node->poll();
+        }
+        rclcpp::shutdown();
+    }
+    return 0;
+}

src/ros2/src/Lslidar_ROS2_driver/lslidar_driver/src/lslidar_driver.cpp

@@ -0,0 +1,1508 @@
+/******************************************************************************
+ * This file is part of lslidar_cx driver.
+ *
+ * Copyright 2022 LeiShen Intelligent Authors. All Rights Reserved.
+ *
+ * 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.
+ *****************************************************************************/
+
+#include "lslidar_driver/lslidar_driver.h"
+
+namespace lslidar_driver {
+    lslidarDriver::lslidarDriver() : lslidarDriver(rclcpp::NodeOptions()) {
+        config_vert_num = 0;
+        return;
+    }
+
+    lslidarDriver::lslidarDriver(const rclcpp::NodeOptions &options) : Node("lslidar_node", options) {
+        config_vert_num = 0;
+        config_vert = true;
+        socket_id = -1;
+        last_azimuth = 0;
+        sweep_end_time = 0.0;
+        is_first_sweep = true;
+        return_mode = 1;
+        current_packet_time = 0.0;
+        last_packet_time = 0.0;
+        packet_size = 1212;
+        is_update_gps_time = false;
+        sweep_data.reset(new lslidar_msgs::msg::LslidarScan());
+
+        return;
+    }
+
+    bool lslidarDriver::checkPacketValidity(const lslidar_driver::RawPacket *packet) {
+        for (size_t blk_idx = 0; blk_idx < BLOCKS_PER_PACKET; ++blk_idx) {
+            if (packet->blocks[blk_idx].header != UPPER_BANK) {
+                return false;
+            }
+        }
+        return true;
+    }
+
+    bool lslidarDriver::isPointInRange(const double &distance) {
+        return (distance >= min_range && distance < max_range);
+    }
+
+    bool lslidarDriver::loadParameters() {
+        this->declare_parameter<std::string>("pcap", "");
+        this->declare_parameter<std::string>("frame_id", "laser_link");
+        this->declare_parameter<std::string>("lidar_type", "c16");
+        this->declare_parameter<std::string>("c16_type", "c16_2");
+        this->declare_parameter<std::string>("c32_type", "c32_2");
+        this->declare_parameter<int>("c32_fpga_type", 3);
+        this->declare_parameter<bool>("add_multicast", false);
+        this->declare_parameter<bool>("config_vert", true);
+        this->declare_parameter<std::string>("group_ip", "234.2.3.2");
+        this->declare_parameter<std::string>("device_ip", "192.168.1.200");
+        this->declare_parameter<int>("msop_port", 2368);
+        this->declare_parameter<int>("difop_port", 2369);
+        this->declare_parameter<int>("point_num", 2000);
+        this->declare_parameter<int>("scan_num", 8);
+        this->declare_parameter<double>("min_range", 0.3);
+        this->declare_parameter<double>("max_range", 150.0);
+        this->declare_parameter<double>("distance_unit", 0.25);
+        this->declare_parameter<double>("packet_rate", 840.0);
+        this->declare_parameter<double>("horizontal_angle_resolution", 0.18);
+        this->declare_parameter<int>("angle_disable_min", 0);
+        this->declare_parameter<int>("angle_disable_max", 0);
+        this->declare_parameter<int>("packet_size", 1212);
+        this->declare_parameter<bool>("use_gps_ts", false);
+        this->declare_parameter<bool>("pcl_type", false);
+        this->declare_parameter<bool>("publish_scan", false);
+        this->declare_parameter<bool>("coordinate_opt", false);
+        this->declare_parameter<std::string>("topic_name", "lslidar_point_cloud");
+
+        msop_udp_port = 0;
+        difop_udp_port = 0;
+        this->get_parameter("packet_size", packet_size);
+        this->get_parameter("pcap", dump_file);
+        this->get_parameter("frame_id", frame_id);
+        this->get_parameter("lidar_type", lidar_type);
+        this->get_parameter("c16_type", c16_type);
+        this->get_parameter("c32_type", c32_type);
+        this->get_parameter("c32_fpga_type", c32_fpga_type);
+        this->get_parameter("add_multicast", add_multicast);
+        this->get_parameter("config_vert", config_vert);
+        this->get_parameter("group_ip", group_ip_string);
+        this->get_parameter("device_ip", lidar_ip_string);
+        this->get_parameter("msop_port", msop_udp_port);
+        this->get_parameter("difop_port", difop_udp_port);
+        this->get_parameter("point_num", point_num);
+        this->get_parameter("scan_num", scan_num);
+        this->get_parameter("min_range", min_range);
+        this->get_parameter("max_range", max_range);
+        this->get_parameter("distance_unit", distance_unit);
+        this->get_parameter("packet_rate", packet_rate);
+        this->get_parameter("horizontal_angle_resolution", horizontal_angle_resolution);
+        this->get_parameter("angle_disable_min", angle_disable_min);
+        this->get_parameter("angle_disable_max", angle_disable_max);
+        this->get_parameter("use_gps_ts", use_gps_ts);
+        this->get_parameter("pcl_type", pcl_type);
+        this->get_parameter("publish_scan", publish_scan);
+        this->get_parameter("coordinate_opt", coordinate_opt);
+        this->get_parameter("topic_name", pointcloud_topic);
+
+        RCLCPP_INFO(this->get_logger(), "dump_file: %s", dump_file.c_str());
+        RCLCPP_INFO(this->get_logger(), "frame_id: %s", frame_id.c_str());
+        RCLCPP_INFO(this->get_logger(), "lidar_type: %s", lidar_type.c_str());
+        RCLCPP_INFO(this->get_logger(), "c16_type: %s", c16_type.c_str());
+        RCLCPP_INFO(this->get_logger(), "c32_type: %s", c32_type.c_str());
+        RCLCPP_INFO(this->get_logger(), "device_ip: %s", lidar_ip_string.c_str());
+        RCLCPP_INFO(this->get_logger(), "c32_fpag_type: %d", c32_fpga_type);
+        RCLCPP_INFO(this->get_logger(), "config_vert: %d", config_vert);
+        RCLCPP_INFO(this->get_logger(), "msop_port: %d", msop_udp_port);
+        RCLCPP_INFO(this->get_logger(), "use_gps_ts: %d", use_gps_ts);
+        RCLCPP_INFO(this->get_logger(), "pcl_type: %d", pcl_type);
+        RCLCPP_INFO(this->get_logger(), "distance_unit: %f", distance_unit);
+        RCLCPP_INFO(this->get_logger(), "horizontal_angle_resolution: %f", horizontal_angle_resolution);
+        RCLCPP_INFO(this->get_logger(), "packet_rate: %f", packet_rate);
+        RCLCPP_INFO(this->get_logger(), "difop_udp_port: %d", difop_udp_port);
+        RCLCPP_INFO(this->get_logger(), "coordinate_opt: %d", coordinate_opt);
+        RCLCPP_INFO(this->get_logger(), "topic_name: %s", pointcloud_topic.c_str());
+        RCLCPP_INFO(this->get_logger(), "packet_size: %d", packet_size);
+
+        if (lidar_type == "c16") {
+            if (scan_num > 15) { scan_num = 15; }
+            else if (scan_num < 0) { scan_num = 0; }
+        } else {
+            if (scan_num > 31) { scan_num = 31; }
+            else if (scan_num < 0) { scan_num = 0; }
+        }
+
+        if (packet_size != 1206 && packet_size != 1212) {
+            packet_size = 1212;
+            RCLCPP_WARN(this->get_logger(), "packet_size input error, reassign to: %d", packet_size);
+        }
+
+
+        inet_aton(lidar_ip_string.c_str(), &lidar_ip);
+        if (add_multicast)
+            RCLCPP_INFO(this->get_logger(), "opening UDP socket: group_address %s", group_ip_string.c_str());
+        return true;
+    }
+
+    void lslidarDriver::initTimeStamp() {
+        for (int i = 0; i < 10; i++) {
+            this->packetTimeStamp[i] = 0;
+        }
+        this->packet_time_s = 0;
+        this->packet_time_ns = 0;
+        this->timeStamp = rclcpp::Time(0.0);
+        this->timeStamp_bak = rclcpp::Time(0.0);
+    }
+
+    bool lslidarDriver::createRosIO() {
+        pointcloud_pub = this->create_publisher<sensor_msgs::msg::PointCloud2>(pointcloud_topic, 10);
+        scan_pub = this->create_publisher<sensor_msgs::msg::LaserScan>("scan", 10);
+//        lslidar_control =
+//                this->create_service<lslidar_msgs::srv::Lslidarcontrol>(
+//                        "lslidarcontrol",
+//                        std::bind(&lslidarC16Driver::lslidarControl, this));
+
+        if (dump_file != "") {
+            msop_input_.reset(new lslidar_driver::InputPCAP(this, msop_udp_port, packet_rate, dump_file));
+            difop_input_.reset(new lslidar_driver::InputPCAP(this, difop_udp_port, 1, dump_file));
+        } else {
+            msop_input_.reset(new lslidar_driver::InputSocket(this, msop_udp_port));
+            difop_input_.reset(new lslidar_driver::InputSocket(this, difop_udp_port));
+        }
+        difop_thread_ = std::shared_ptr<std::thread>(
+                new std::thread(std::bind(&lslidarDriver::difopPoll, this)));
+        return true;
+    }
+
+    void lslidarDriver::publishPointcloud() {
+        if (sweep_data->points.size() < 65 || !is_update_gps_time) {
+            return;
+        }
+/*        if (pointcloud_pub.getNumSubscribers() == 0) {
+            return;
+        }*/
+        if (pcl_type) {
+
+            pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(new pcl::PointCloud<pcl::PointXYZI>);
+            // pcl_conversions::toPCL(sweep_data->header).stamp;
+            point_cloud->header.frame_id = frame_id;
+            point_cloud->height = 1;
+            point_cloud->header.stamp = static_cast<uint64_t>(sweep_end_time * 1e6);
+            size_t j;
+            pcl::PointXYZI point;
+            if (sweep_data->points.size() > 0) {
+                for (j = 0; j < sweep_data->points.size(); ++j) {
+                    if ((sweep_data->points[j].azimuth > angle_disable_min) &&
+                        (sweep_data->points[j].azimuth < angle_disable_max)) {
+                        continue;
+                    }
+                    point.x = sweep_data->points[j].x;
+                    point.y = sweep_data->points[j].y;
+                    point.z = sweep_data->points[j].z;
+                    point.intensity = sweep_data->points[j].intensity;
+                    point_cloud->points.push_back(point);
+                    ++point_cloud->width;
+                }
+            }
+            sensor_msgs::msg::PointCloud2 pc_msg;
+            pcl::toROSMsg(*point_cloud, pc_msg);
+            pc_msg.header.stamp =  rclcpp::Time(sweep_end_time * 1e9);
+            pointcloud_pub->publish(pc_msg);
+        } else {
+            VPointcloud::Ptr point_cloud(new VPointcloud());
+            //pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(new pcl::PointCloud<pcl::PointXYZI>);
+            // pcl_conversions::toPCL(sweep_data->header).stamp;
+            point_cloud->header.frame_id = frame_id;
+            point_cloud->height = 1;
+            point_cloud->header.stamp = static_cast<uint64_t>(sweep_end_time * 1e6);
+            size_t j;
+            VPoint point;
+            if (sweep_data->points.size() > 0) {
+                for (j = 0; j < sweep_data->points.size(); ++j) {
+                    if ((sweep_data->points[j].azimuth > angle_disable_min) &&
+                        (sweep_data->points[j].azimuth < angle_disable_max)) {
+                        continue;
+                    }
+                    point.x = sweep_data->points[j].x;
+                    point.y = sweep_data->points[j].y;
+                    point.z = sweep_data->points[j].z;
+                    point.intensity = sweep_data->points[j].intensity;
+                    point.ring = sweep_data->points[j].ring;
+                    point.time = sweep_data->points[j].time;
+
+                    point_cloud->points.push_back(point);
+                    ++point_cloud->width;
+                    current_point_time = point.time;
+                    //if(current_point_time - last_point_time<0.0)
+                    //    RCLCPP_WARN(this->get_logger(),"timestamp is rolled back! current point time: %.12f  last point time: %.12f",current_point_time,last_point_time);
+                    //last_point_time = current_point_time;
+                }
+            }
+            sensor_msgs::msg::PointCloud2 pc_msg;
+            pcl::toROSMsg(*point_cloud, pc_msg);
+            pc_msg.header.stamp =  rclcpp::Time(sweep_end_time * 1e9);
+            pointcloud_pub->publish(pc_msg);
+        }
+        return;
+    }
+
+    void lslidarDriver::publishScan() {
+        if (sweep_data->points.size() < 65 || !is_update_gps_time) {
+            return;
+        }
+        sensor_msgs::msg::LaserScan::UniquePtr scan_msg(new sensor_msgs::msg::LaserScan());
+        scan_msg->header.frame_id = frame_id;
+        int layer_num_local = scan_num;
+        RCLCPP_INFO_ONCE(this->get_logger(), "default channel is %d", layer_num_local);
+        scan_msg->header.stamp = rclcpp::Time(sweep_end_time * 1e9);
+
+        scan_msg->angle_min = 0.0;
+        scan_msg->angle_max = 2.0 * M_PI;
+        scan_msg->angle_increment = horizontal_angle_resolution * DEG_TO_RAD;
+        scan_msg->range_min = min_range;
+        scan_msg->range_max = max_range;
+
+        uint point_size = ceil((scan_msg->angle_max - scan_msg->angle_min) / scan_msg->angle_increment);
+        scan_msg->ranges.assign(point_size, std::numeric_limits<float>::quiet_NaN());
+        scan_msg->intensities.assign(point_size, std::numeric_limits<float>::quiet_NaN());
+        if (coordinate_opt) {
+            for (size_t j = 0; j < sweep_data->points.size(); ++j) {
+                if (layer_num_local == sweep_data->points[j].ring) {
+                    float horizontal_angle = sweep_data->points[j].azimuth * 0.01 * DEG_TO_RAD;
+                    uint point_index = (int) ((horizontal_angle - scan_msg->angle_min) / scan_msg->angle_increment);
+                    point_index = (point_index <= point_size) ? point_index : (point_index % point_size);
+                    scan_msg->ranges[point_size - point_index - 1] = sweep_data->points[j].distance;
+                    scan_msg->intensities[point_size - point_index - 1] = sweep_data->points[j].intensity;
+                }
+            }
+        } else {
+            for (size_t j = 0; j < sweep_data->points.size(); ++j) {
+                if (layer_num_local == sweep_data->points[j].ring) {
+/*                    float h_angle =
+                            (sweep_data->points[j].azimuth + 9000.0) < 36000.0 ? sweep_data->points[j].azimuth + 9000.0
+                                                                               : sweep_data->points[j].azimuth - 27000.0;
+                    if(h_angle < 18000.0){
+                        h_angle = 18000.0 - h_angle;
+                    }else{
+                        h_angle = 36000.0 - (h_angle - 18000.0);
+                    }*/
+                    float h_angle = (45000.0 - sweep_data->points[j].azimuth) < 36000.0 ? 45000.0 -
+                                                                                          sweep_data->points[j].azimuth
+                                                                                        :
+                                    9000.0 - sweep_data->points[j].azimuth;
+                    // ROS_INFO("a=%f,b=%f",sweep_data->points[j].azimuth,h_angle);
+                    float horizontal_angle = h_angle * 0.01 * DEG_TO_RAD;
+                    uint point_index = (int) ((horizontal_angle - scan_msg->angle_min) / scan_msg->angle_increment);
+                    point_index = (point_index <= point_size) ? point_index : (point_index % point_size);
+                    scan_msg->ranges[point_index] = sweep_data->points[j].distance;
+                    scan_msg->intensities[point_index] = sweep_data->points[j].intensity;
+                }
+            }
+        }
+        scan_pub->publish(std::move(scan_msg));
+    }
+
+    bool lslidarDriver::lslidarControl(lslidar_msgs::srv::Lslidarcontrol::Request &req,
+                                       lslidar_msgs::srv::Lslidarcontrol::Response &res) {
+        RCLCPP_INFO(this->get_logger(), "--------------------------");
+        // sleep(1);
+        lslidar_msgs::msg::LslidarPacket::UniquePtr packet0(new lslidar_msgs::msg::LslidarPacket());
+        packet0->data[0] = 0x00;
+        packet0->data[1] = 0x00;
+        int rc_msop = -1;
+
+        if (req.lasercontrol == 1) {
+
+            if ((rc_msop = msop_input_->getPacket(packet0)) == 0) {
+                res.status = 1;
+                RCLCPP_WARN(this->get_logger(), "receive cmd: %d,already power on status", req.lasercontrol);
+                return true;
+            }
+            RCLCPP_WARN(this->get_logger(), "receive cmd: %d,power on", req.lasercontrol);
+            SendPacketTolidar(true);
+            double time1 = get_clock()->now().seconds();
+
+            do {
+                rc_msop = msop_input_->getPacket(packet0);
+                double time2 = get_clock()->now().seconds();
+                if ((time2 - time1) > 20) {
+                    res.status = 0;
+                    RCLCPP_WARN(this->get_logger(), "lidar connect error");
+                    return true;
+                }
+            } while ((rc_msop != 0) && (packet0->data[0] != 0xff) && (packet0->data[1] != 0xee));
+            sleep(5);
+            res.status = 1;
+        } else if (req.lasercontrol == 0) {
+            RCLCPP_WARN(this->get_logger(), "receive cmd: %d,power off", req.lasercontrol);
+            SendPacketTolidar(false);
+            res.status = 1;
+        } else {
+            RCLCPP_WARN(this->get_logger(), "cmd error");
+            res.status = 0;
+        }
+
+        return true;
+    }
+
+    bool lslidarDriver::SendPacketTolidar(bool power_switch) {
+        int socketid;
+        unsigned char config_data[1206];
+        //int data_port = difop_data[24] * 256 + difop_data[25];
+        mempcpy(config_data, difop_data, 1206);
+        config_data[0] = 0xAA;
+        config_data[1] = 0x00;
+        config_data[2] = 0xFF;
+        config_data[3] = 0x11;
+        config_data[4] = 0x22;
+        config_data[5] = 0x22;
+        config_data[6] = 0xAA;
+        config_data[7] = 0xAA;
+        config_data[8] = 0x02;
+        config_data[9] = 0x58;
+        if (power_switch) {
+            config_data[45] = 0x00;
+        } else {
+            config_data[45] = 0x01;
+        }
+
+        sockaddr_in addrSrv;
+        socketid = socket(2, 2, 0);
+        addrSrv.sin_addr.s_addr = inet_addr(lidar_ip_string.c_str());
+        addrSrv.sin_family = AF_INET;
+        addrSrv.sin_port = htons(2368);
+        sendto(socketid, (const char *) config_data, 1206, 0, (struct sockaddr *) &addrSrv, sizeof(addrSrv));
+        return 0;
+
+    }
+
+
+    lslidarC16Driver::lslidarC16Driver() : lslidarDriver() {
+//        this->initialize();
+        return;
+    }
+
+    lslidarC16Driver::~lslidarC16Driver() {
+        if (difop_thread_ != nullptr) {
+//            difop_thread_->interrupt();
+            difop_thread_->join();
+        }
+    }
+
+    bool lslidarC16Driver::initialize() {
+        RCLCPP_INFO(this->get_logger(), "lslidarC16Driver::initialize()");
+        this->initTimeStamp();
+        if (!loadParameters()) {
+            RCLCPP_ERROR(this->get_logger(), "cannot load all required ROS parameters.");
+            return false;
+        }
+        if (c16_type == "c16_2") {
+            for (int j = 0; j < 16; ++j) {
+                c16_vertical_angle[j] = c16_2_vertical_angle[j];
+                c16_config_vertical_angle[j] = c16_2_vertical_angle[j];
+                c16_sin_scan_altitude[j] = sin(c16_vertical_angle[j] * DEG_TO_RAD);
+                c16_cos_scan_altitude[j] = cos(c16_vertical_angle[j] * DEG_TO_RAD);
+            }
+        } else {
+            for (int j = 0; j < 16; ++j) {
+                c16_vertical_angle[j] = c16_1_vertical_angle[j];
+                c16_config_vertical_angle[j] = c16_1_vertical_angle[j];
+                c16_sin_scan_altitude[j] = sin(c16_vertical_angle[j] * DEG_TO_RAD);
+                c16_cos_scan_altitude[j] = cos(c16_vertical_angle[j] * DEG_TO_RAD);
+            }
+        }
+
+
+        if (!createRosIO()) {
+            RCLCPP_ERROR(this->get_logger(), "cannot create all ROS IO.");
+            return false;
+        }
+
+        // create the sin and cos table for different azimuth values
+        for (int j = 0; j < 36000; ++j) {
+            sin_azimuth_table[j] = sin(j * 0.01 * DEG_TO_RAD);
+            cos_azimuth_table[j] = cos(j * 0.01 * DEG_TO_RAD);
+        }
+       // angle_base = M_PI * 2 / point_num;
+        return true;
+    }
+
+/*    bool lslidarC16Driver::checkPacketValidity(const RawPacket *packet) {
+        for (size_t blk_idx = 0; blk_idx < BLOCKS_PER_PACKET; ++blk_idx) {
+            if (packet->blocks[blk_idx].header != UPPER_BANK) {
+                return false;
+            }
+        }
+        return true;
+    }*/
+
+
+    void lslidarC16Driver::difopPoll() {
+        // reading and publishing scans as fast as possible.
+        lslidar_msgs::msg::LslidarPacket::UniquePtr difop_packet_ptr(new lslidar_msgs::msg::LslidarPacket);
+        while (rclcpp::ok()) {
+            // keep reading
+            int rc = difop_input_->getPacket(difop_packet_ptr);
+            if (rc == 0) {
+                if (difop_packet_ptr->data[0] != 0xa5 || difop_packet_ptr->data[1] != 0xff ||
+                    difop_packet_ptr->data[2] != 0x00 || difop_packet_ptr->data[3] != 0x5a) {
+                    return;
+                }
+                count++;
+                if (count == 2) { is_update_gps_time = true; }
+                for (int i = 0; i < packet_size; i++) {
+                    difop_data[i] = difop_packet_ptr->data[i];
+                }
+
+                int version_data = difop_packet_ptr->data[1202];
+                if (config_vert) {
+                    if (2 == version_data) {
+                        for (int j = 0; j < 16; ++j) {
+                            uint8_t data1 = difop_packet_ptr->data[234 + 2 * j];
+                            uint8_t data2 = difop_packet_ptr->data[234 + 2 * j + 1];
+                            int vert_angle = data1 * 256 + data2;
+                            vert_angle = vert_angle > 32767 ? (vert_angle - 65536) : vert_angle;
+                            c16_config_vertical_angle[j] = (double) vert_angle * 0.01f;
+                            if (fabs(c16_vertical_angle[j] - c16_config_vertical_angle[j]) > 1.5) {
+                                //c16_config_vertical_angle[j] = c16_vertical_angle[j];
+                                config_vert_num++;
+                            }
+
+                        }
+                        if (config_vert_num == 0) {
+                            for (int k = 0; k < 16; ++k) {
+                                c16_sin_scan_altitude[k] = sin(c16_config_vertical_angle[k] * DEG_TO_RAD);
+                                c16_cos_scan_altitude[k] = cos(c16_config_vertical_angle[k] * DEG_TO_RAD);
+                            }
+                        }
+
+                    } else {
+                        for (int j = 0; j < 16; ++j) {
+                            uint8_t data1 = difop_packet_ptr->data[245 + 2 * j];
+                            uint8_t data2 = difop_packet_ptr->data[245 + 2 * j + 1];
+                            int vert_angle = data1 * 256 + data2;
+                            vert_angle = vert_angle > 32767 ? (vert_angle - 65536) : vert_angle;
+                            c16_config_vertical_angle[j] = (double) vert_angle * 0.01f;
+                            if (fabs(c16_vertical_angle[j] - c16_config_vertical_angle[j]) > 1.5) {
+                                config_vert_num++;
+                            }
+                        }
+                        if (config_vert_num == 0) {
+                            for (int k = 0; k < 16; ++k) {
+                                c16_sin_scan_altitude[k] = sin(c16_config_vertical_angle[k] * DEG_TO_RAD);
+                                c16_cos_scan_altitude[k] = cos(c16_config_vertical_angle[k] * DEG_TO_RAD);
+                            }
+                        }
+                    }
+                    config_vert = false;
+                }
+                if (packet_size == 1206) {
+                    if (2 == version_data) {
+                        this->packetTimeStamp[4] = difop_packet_ptr->data[41];
+                        this->packetTimeStamp[5] = difop_packet_ptr->data[40];
+                        this->packetTimeStamp[6] = difop_packet_ptr->data[39];
+                        this->packetTimeStamp[7] = difop_packet_ptr->data[38];
+                        this->packetTimeStamp[8] = difop_packet_ptr->data[37];
+                        this->packetTimeStamp[9] = difop_packet_ptr->data[36];
+                    } else {
+                        this->packetTimeStamp[4] = difop_packet_ptr->data[57];
+                        this->packetTimeStamp[5] = difop_packet_ptr->data[56];
+                        this->packetTimeStamp[6] = difop_packet_ptr->data[55];
+                        this->packetTimeStamp[7] = difop_packet_ptr->data[54];
+                        this->packetTimeStamp[8] = difop_packet_ptr->data[53];
+                        this->packetTimeStamp[9] = difop_packet_ptr->data[52];
+                    }
+                }
+
+            } else if (rc < 0) {
+                return;
+            }
+//            rclcpp::spinOnce();
+
+        }
+    }
+
+
+    void lslidarC16Driver::decodePacket(const RawPacket *packet) {
+        //couputer azimuth angle for each firing
+        //even numbers
+        for (size_t b_idx = 0; b_idx < BLOCKS_PER_PACKET; ++b_idx) {
+            firings.firing_azimuth[b_idx] = packet->blocks[b_idx].rotation % 36000; //* 0.01 * DEG_TO_RAD;
+        }
+
+        // computer distance ,intensity
+        //12 blocks
+        for (size_t blk_idx = 0; blk_idx < BLOCKS_PER_PACKET; ++blk_idx) {
+            const RawBlock &raw_block = packet->blocks[blk_idx];
+
+            int32_t azimuth_diff = 0;
+            if (1 == return_mode) {
+                if (blk_idx < BLOCKS_PER_PACKET - 1) {
+                    azimuth_diff = firings.firing_azimuth[blk_idx + 1] - firings.firing_azimuth[blk_idx];
+                    azimuth_diff = azimuth_diff < 0 ? azimuth_diff + 36000 : azimuth_diff;
+                } else {
+                    azimuth_diff = firings.firing_azimuth[blk_idx] - firings.firing_azimuth[blk_idx - 1];
+                    azimuth_diff = azimuth_diff < 0 ? azimuth_diff + 36000 : azimuth_diff;
+                }
+            } else {
+                if (blk_idx < BLOCKS_PER_PACKET - 2) {
+                    azimuth_diff = firings.firing_azimuth[blk_idx + 2] - firings.firing_azimuth[blk_idx];
+                    azimuth_diff = azimuth_diff < 0 ? azimuth_diff + 36000 : azimuth_diff;
+                } else {
+                    azimuth_diff = firings.firing_azimuth[blk_idx] - firings.firing_azimuth[blk_idx - 2];
+                    azimuth_diff = azimuth_diff < 0 ? azimuth_diff + 36000 : azimuth_diff;
+                }
+
+            }
+            for (size_t scan_idx = 0; scan_idx < SCANS_PER_BLOCK; ++scan_idx) {
+                size_t byte_idx = RAW_SCAN_SIZE * scan_idx;
+                //azimuth
+                firings.azimuth[blk_idx * 32 + scan_idx] =
+                        firings.firing_azimuth[blk_idx] + scan_idx * azimuth_diff / FIRING_TOFFSET;
+                firings.azimuth[blk_idx * 32 + scan_idx] = firings.azimuth[blk_idx * 32 + scan_idx] % 36000;
+                // distance
+                firings.distance[blk_idx * 32 + scan_idx] =
+                        static_cast<double>(raw_block.data[byte_idx] + raw_block.data[byte_idx + 1] * 256) *
+                        DISTANCE_RESOLUTION * distance_unit;
+
+                //intensity
+                firings.intensity[blk_idx * 32 + scan_idx] = static_cast<double>(
+                        raw_block.data[byte_idx + 2]);
+            }
+
+        }
+        return;
+    }
+
+/** poll the device
+ *  @returns true unless end of file reached
+ */
+    bool lslidarC16Driver::poll(void) {  // Allocate a new shared pointer for zero-copy sharing with other nodelets.
+        lslidar_msgs::msg::LslidarPacket::UniquePtr packet(new lslidar_msgs::msg::LslidarPacket());
+
+        // Since the rslidar delivers data at a very high rate, keep
+        // reading and publishing scans as fast as possible.
+        while (true) {
+            int rc = msop_input_->getPacket(packet);
+            if (rc == 0) break;
+            if (rc < 0) {
+                return false;
+            }
+        }
+        const RawPacket *raw_packet = (const RawPacket *) (&(packet->data[0]));
+
+        //check if the packet is valid
+        if (!checkPacketValidity(raw_packet)) {
+            return false;
+        }
+
+        // packet timestamp
+        if (use_gps_ts) {
+            if (packet_size == 1212) {
+                lslidar_msgs::msg::LslidarPacket pkt = *packet;
+                memset(&cur_time, 0, sizeof(cur_time));
+                cur_time.tm_year = pkt.data[1200] + 2000 - 1900;
+                cur_time.tm_mon = pkt.data[1201] - 1;
+                cur_time.tm_mday = pkt.data[1202];
+                cur_time.tm_hour = pkt.data[1203];
+                cur_time.tm_min = pkt.data[1204];
+                cur_time.tm_sec = pkt.data[1205];
+                cur_time.tm_year = cur_time.tm_year >= 200 ? 100 : cur_time.tm_year;
+                packet_time_s = static_cast<uint64_t>(timegm(&cur_time)); //s
+                packet_time_ns = (pkt.data[1206] +
+                                  pkt.data[1207] * pow(2, 8) +
+                                  pkt.data[1208] * pow(2, 16) +
+                                  pkt.data[1209] * pow(2, 24)) * 1e3; //ns
+                timeStamp = rclcpp::Time(packet_time_s, packet_time_ns);
+                packet->stamp = timeStamp;
+                current_packet_time = timeStamp.seconds();
+
+                if (packet->data[1210] == 0x39) {
+                    return_mode = 2;
+                }
+            } else {
+                lslidar_msgs::msg::LslidarPacket pkt = *packet;
+                memset(&cur_time, 0, sizeof(cur_time));
+                cur_time.tm_year = this->packetTimeStamp[9] + 2000 - 1900;
+                cur_time.tm_mon = this->packetTimeStamp[8] - 1;
+                cur_time.tm_mday = this->packetTimeStamp[7];
+                cur_time.tm_hour = this->packetTimeStamp[6];
+                cur_time.tm_min = this->packetTimeStamp[5];
+                cur_time.tm_sec = this->packetTimeStamp[4];
+                cur_time.tm_year = cur_time.tm_year >= 200 ? 100 : cur_time.tm_year;
+                packet_time_s = static_cast<uint64_t>(timegm(&cur_time)); //s
+                packet_time_ns = (pkt.data[1200] +
+                                  pkt.data[1201] * pow(2, 8) +
+                                  pkt.data[1202] * pow(2, 16) +
+                                  pkt.data[1203] * pow(2, 24)) * 1e3; //ns
+                timeStamp = rclcpp::Time(packet_time_s, packet_time_ns);
+                //使用gps授时
+                if ((timeStamp.seconds() - timeStamp_bak.seconds()) < 1e-9 &&
+                    (timeStamp.seconds() - timeStamp_bak.seconds()) > -1.0
+                    && is_update_gps_time && packet_time_ns < 100000000) {
+                    timeStamp = rclcpp::Time(packet_time_s + 1, packet_time_ns);
+                } else if ((timeStamp - timeStamp_bak).seconds() > 1.0 && (timeStamp - timeStamp_bak).seconds() < 1.2 &&
+                           is_update_gps_time
+                           && packet_time_ns > 900000000) {
+                    timeStamp = rclcpp::Time(packet_time_s - 1, packet_time_ns);
+                }
+                timeStamp_bak = timeStamp;
+                packet->stamp = timeStamp;
+                current_packet_time = timeStamp.seconds();
+
+                if (packet->data[1204] == 0x39) {
+                    return_mode = 2;
+                }
+            }
+
+        } else {
+            packet->stamp = get_clock()->now();
+            current_packet_time = rclcpp::Time(packet->stamp).seconds();
+        }
+
+        RCLCPP_INFO_ONCE(this->get_logger(), "return mode: %d", return_mode);
+
+
+        //decode the packet
+        decodePacket(raw_packet);
+        // find the start of a new revolution
+        // if there is one, new_sweep_start will be the index of the start firing,
+        // otherwise, new_sweep_start will be FIRINGS_PER_PACKET.
+        size_t new_sweep_start = 0;
+        do {
+            if (last_azimuth - firings.azimuth[new_sweep_start] > 35000) {
+                break;
+            } else {
+                last_azimuth = firings.azimuth[new_sweep_start];
+                ++new_sweep_start;
+            }
+        } while (new_sweep_start < SCANS_PER_PACKET);
+
+        // The first sweep may not be complete. So, the firings with
+        // the first sweep will be discarded. We will wait for the
+        // second sweep in order to find the 0 azimuth angle.
+        size_t start_fir_idx = 0;
+        size_t end_fir_idx = new_sweep_start;
+        if (is_first_sweep && new_sweep_start == SCANS_PER_PACKET) {
+            return true;
+        } else {
+            if (is_first_sweep) {
+                is_first_sweep = false;
+                start_fir_idx = new_sweep_start;
+                end_fir_idx = SCANS_PER_PACKET;
+                //sweep_start_time = packet->stamp.toSec() - (end_fir_idx - start_fir_idx) * 3.125 * 1e-6;
+            }
+        }
+        for (size_t fir_idx = start_fir_idx; fir_idx < end_fir_idx; ++fir_idx) {
+            //check if the point is valid
+            if (!isPointInRange(firings.distance[fir_idx]))continue;
+            //if (firings.azimuth[fir_idx]>=36000) { firings.azimuth[fir_idx]-=36000;}  //todo
+            //convert the point to xyz coordinate
+            size_t table_idx = firings.azimuth[fir_idx];
+            double cos_azimuth = cos_azimuth_table[table_idx];
+            double sin_azimuth = sin_azimuth_table[table_idx];
+            double x_coord, y_coord, z_coord;
+            if (coordinate_opt) {
+                int tmp_idx = 1468 - firings.azimuth[fir_idx] < 0 ? 1468 - firings.azimuth[fir_idx] + 36000 : 1468 - firings.azimuth[fir_idx];
+                x_coord = firings.distance[fir_idx] * c16_cos_scan_altitude[fir_idx % 16] * cos_azimuth +
+                          R1_ * cos_azimuth_table[tmp_idx];
+                y_coord = -firings.distance[fir_idx] * c16_cos_scan_altitude[fir_idx % 16] * sin_azimuth +
+                          R1_ * sin_azimuth_table[tmp_idx];
+                z_coord = firings.distance[fir_idx] * c16_sin_scan_altitude[fir_idx % 16];
+
+            } else {
+                //Y-axis correspondence 0 degree
+                int tmp_idx = firings.azimuth[fir_idx] - 1468 < 0 ? firings.azimuth[fir_idx] -1468 + 36000 : firings.azimuth[fir_idx] -1468;
+                x_coord = firings.distance[fir_idx] * c16_cos_scan_altitude[fir_idx % 16] * sin_azimuth +
+                          R1_ * sin_azimuth_table[tmp_idx];
+                y_coord = firings.distance[fir_idx] * c16_cos_scan_altitude[fir_idx % 16] * cos_azimuth +
+                          R1_ * cos_azimuth_table[tmp_idx];
+                z_coord = firings.distance[fir_idx] * c16_sin_scan_altitude[fir_idx % 16];
+            }
+            // computer the time of the point
+            double time = current_packet_time  -
+                          (SCANS_PER_PACKET - fir_idx - 1) * (current_packet_time - last_packet_time) /
+                          (SCANS_PER_PACKET * 1.0) - sweep_end_time;
+
+
+            int remapped_scan_idx = (fir_idx % 16) % 2 == 0 ? (fir_idx % 16) / 2 : (fir_idx % 16) / 2 + 8;
+
+            sweep_data->points.push_back(lslidar_msgs::msg::LslidarPoint());
+            lslidar_msgs::msg::LslidarPoint &new_point = sweep_data->points[
+                    sweep_data->points.size() - 1];
+            //pack the data into point msg
+            new_point.time = time;
+            new_point.x = x_coord;
+            new_point.y = y_coord;
+            new_point.z = z_coord;
+            new_point.intensity = firings.intensity[fir_idx];
+            new_point.ring = remapped_scan_idx;
+            new_point.azimuth = firings.azimuth[fir_idx];
+            new_point.distance = firings.distance[fir_idx];
+        }
+        // a new sweep begins ----------------------------------------------------
+
+        if (end_fir_idx != SCANS_PER_PACKET) {
+            sweep_end_time = current_packet_time - (SCANS_PER_PACKET - end_fir_idx - 1) *
+                                                   (current_packet_time - last_packet_time) / (SCANS_PER_PACKET*1.0);
+            sweep_end_time = sweep_end_time > 0 ? sweep_end_time : 0;
+            publishPointcloud();
+            if (publish_scan) publishScan();
+
+            sweep_data = lslidar_msgs::msg::LslidarScan::UniquePtr(new lslidar_msgs::msg::LslidarScan());
+            //prepare the next frame scan
+            //sweep_start_time = packet->stamp.toSec() - (end_fir_idx - start_fir_idx) * 3.125 * 1e-6;
+            last_azimuth = firings.azimuth[SCANS_PER_PACKET - 1];
+            start_fir_idx = end_fir_idx;
+            end_fir_idx = SCANS_PER_PACKET;
+            for (size_t fir_idx = start_fir_idx; fir_idx < end_fir_idx; ++fir_idx) {
+
+                //check if the point is valid
+                if (!isPointInRange(firings.distance[fir_idx]))continue;
+                //if (firings.azimuth[fir_idx]>=36000) { firings.azimuth[fir_idx]-=36000;} todo
+                //convert the point to xyz coordinate
+                size_t table_idx = firings.azimuth[fir_idx];
+                double cos_azimuth = cos_azimuth_table[table_idx];
+                double sin_azimuth = sin_azimuth_table[table_idx];
+                double x_coord, y_coord, z_coord;
+                if (coordinate_opt) {
+                    int tmp_idx = 1468 - firings.azimuth[fir_idx] < 0 ? 1468 - firings.azimuth[fir_idx] + 36000 : 1468 - firings.azimuth[fir_idx];
+                    x_coord = firings.distance[fir_idx] * c16_cos_scan_altitude[fir_idx % 16] * cos_azimuth +
+                              R1_ * cos_azimuth_table[tmp_idx];
+                    y_coord = -firings.distance[fir_idx] * c16_cos_scan_altitude[fir_idx % 16] * sin_azimuth +
+                              R1_ * sin_azimuth_table[tmp_idx];
+                    z_coord = firings.distance[fir_idx] * c16_sin_scan_altitude[fir_idx % 16];
+
+                } else {
+                    //Y-axis correspondence 0 degree
+                    int tmp_idx = firings.azimuth[fir_idx] - 1468 < 0 ? firings.azimuth[fir_idx] -1468 + 36000 : firings.azimuth[fir_idx] -1468;
+                    x_coord = firings.distance[fir_idx] * c16_cos_scan_altitude[fir_idx % 16] * sin_azimuth +
+                              R1_ * sin_azimuth_table[tmp_idx];
+                    y_coord = firings.distance[fir_idx] * c16_cos_scan_altitude[fir_idx % 16] * cos_azimuth +
+                              R1_ * cos_azimuth_table[tmp_idx];
+                    z_coord = firings.distance[fir_idx] * c16_sin_scan_altitude[fir_idx % 16];
+                }
+
+                // computer the time of the point
+                double time = current_packet_time  -
+                       (SCANS_PER_PACKET - fir_idx -1) * (current_packet_time - last_packet_time) /
+                              (SCANS_PER_PACKET * 1.0) - sweep_end_time;
+
+                int remapped_scan_idx = (fir_idx % 16) % 2 == 0 ? (fir_idx % 16) / 2 : (fir_idx % 16) / 2 + 8;
+                sweep_data->points.push_back(lslidar_msgs::msg::LslidarPoint());
+                lslidar_msgs::msg::LslidarPoint &new_point = sweep_data->points[
+                        sweep_data->points.size() - 1];
+                //pack the data into point msg
+                new_point.time = time;
+                new_point.x = x_coord;
+                new_point.y = y_coord;
+                new_point.z = z_coord;
+                new_point.intensity = firings.intensity[fir_idx];
+                new_point.ring = remapped_scan_idx;
+                new_point.azimuth = firings.azimuth[fir_idx];
+                new_point.distance = firings.distance[fir_idx];
+            }
+
+        }
+        last_packet_time = current_packet_time;
+        //packet_pub.publish(*packet);
+        return true;
+    }
+
+
+    lslidarC32Driver::lslidarC32Driver() : lslidarDriver() {
+//        this->initialize();
+        get_difop = false;
+        return;
+    }
+
+    lslidarC32Driver::~lslidarC32Driver() {
+        if (difop_thread_ != nullptr) {
+//            difop_thread_->interrupt();
+            difop_thread_->join();
+        }
+    }
+
+    bool lslidarC32Driver::initialize() {
+        RCLCPP_INFO(this->get_logger(), "lslidarC32Driver::initialize()");
+        this->initTimeStamp();
+        if (!loadParameters()) {
+            RCLCPP_ERROR(this->get_logger(), "cannot load all required ROS parameters.");
+            return false;
+        }
+        if (c32_type == "c32_2") {
+            if (c32_fpga_type == 2) {
+                for (int j = 0; j < 32; ++j) {
+                    c32_vertical_angle[j] = c32_2_vertical_angle_26[j];
+                    c32_config_vertical_angle[j] = c32_2_vertical_angle_26[j];
+                    c32_sin_scan_altitude[j] = sin(c32_vertical_angle[j] * DEG_TO_RAD);
+                    c32_cos_scan_altitude[j] = cos(c32_vertical_angle[j] * DEG_TO_RAD);
+                }
+            } else {
+                for (int j = 0; j < 32; ++j) {
+                    c32_vertical_angle[j] = c32_2_vertical_angle[j];
+                    c32_config_vertical_angle[j] = c32_2_vertical_angle[j];
+                    c32_sin_scan_altitude[j] = sin(c32_vertical_angle[j] * DEG_TO_RAD);
+                    c32_cos_scan_altitude[j] = cos(c32_vertical_angle[j] * DEG_TO_RAD);
+                }
+            }
+        } else {
+            if (c32_fpga_type == 2) {
+                for (int j = 0; j < 32; ++j) {
+                    c32_vertical_angle[j] = c32_1_vertical_angle_26[j];
+                    c32_config_vertical_angle[j] = c32_1_vertical_angle_26[j];
+                    c32_sin_scan_altitude[j] = sin(c32_vertical_angle[j] * DEG_TO_RAD);
+                    c32_cos_scan_altitude[j] = cos(c32_vertical_angle[j] * DEG_TO_RAD);
+                }
+            } else {
+                for (int j = 0; j < 32; ++j) {
+                    c32_vertical_angle[j] = c32_1_vertical_angle[j];
+                    c32_config_vertical_angle[j] = c32_1_vertical_angle[j];
+                    c32_sin_scan_altitude[j] = sin(c32_vertical_angle[j] * DEG_TO_RAD);
+                    c32_cos_scan_altitude[j] = cos(c32_vertical_angle[j] * DEG_TO_RAD);
+                }
+            }
+        }
+
+        if (!createRosIO()) {
+            RCLCPP_ERROR(this->get_logger(), "cannot create all ROS IO.");
+            return false;
+        }
+
+        for (int i = 0; i < 4; ++i) {
+            adjust_angle[i] = 0.0;
+        }
+
+        for (int j = 0; j < 36000; ++j) {
+            sin_azimuth_table[j] = sin(j * 0.01 * DEG_TO_RAD);
+            cos_azimuth_table[j] = cos(j * 0.01 * DEG_TO_RAD);
+        }
+
+        return true;
+    }
+
+    void lslidarC32Driver::difopPoll() {
+        // reading and publishing scans as fast as possible.
+        lslidar_msgs::msg::LslidarPacket::UniquePtr difop_packet_ptr(new lslidar_msgs::msg::LslidarPacket());
+        while (rclcpp::ok()) {
+            //RCLCPP_INFO(this->get_logger(), "423542354354356563");
+            // keep reading
+            int rc = difop_input_->getPacket(difop_packet_ptr);
+            if (rc == 0) {
+                if (difop_packet_ptr->data[0] != 0xa5 || difop_packet_ptr->data[1] != 0xff ||
+                    difop_packet_ptr->data[2] != 0x00 || difop_packet_ptr->data[3] != 0x5a) {
+                    return;
+                }
+                count++;
+                if (count == 2) { is_update_gps_time = true; }
+                for (int i = 0; i < packet_size; i++) {
+                    difop_data[i] = difop_packet_ptr->data[i];
+                }
+                RCLCPP_INFO_ONCE(this->get_logger(), "c32 vertical angle resolution type: %s; c32 fpga type: %0.1f",
+                                 c32_type.c_str(),
+                                 difop_data[1202] + int(difop_data[1203] / 16) * 0.1);
+                //int version_data = difop_packet_ptr->data[1202];
+                if (config_vert) {
+                    if (3 == c32_fpga_type) {
+                        for (int i = 0; i < 32; ++i) {
+                            uint8_t data1 = difop_packet_ptr->data[245 + 2 * i];
+                            uint8_t data2 = difop_packet_ptr->data[245 + 2 * i + 1];
+                            int vert_angle = data1 * 256 + data2;
+                            vert_angle = vert_angle > 32767 ? (vert_angle - 65536) : vert_angle;
+                            c32_config_tmp_angle[i] = (double) vert_angle * 0.01f;
+
+                        }
+                        for (int j = 0; j < 32; ++j) {
+                            c32_config_vertical_angle[j] = c32_config_tmp_angle[adjust_angle_index[j]];
+
+                            if (fabs(c32_vertical_angle[j] - c32_config_vertical_angle[j]) > 3) {
+                                config_vert_num++;
+                            }
+                        }
+                        if (config_vert_num == 0) {
+                            for (int k = 0; k < 32; ++k) {
+                                c32_sin_scan_altitude[k] = sin(c32_config_vertical_angle[k] * DEG_TO_RAD);
+                                c32_cos_scan_altitude[k] = cos(c32_config_vertical_angle[k] * DEG_TO_RAD);
+                            }
+                        }
+                        // horizontal correction angle
+                        int angle_a0 = difop_packet_ptr->data[186] * 256 + difop_packet_ptr->data[187];
+                        adjust_angle[0] = angle_a0 > 32767 ? 32767 - angle_a0 : angle_a0;
+
+                        int angle_a1 = difop_packet_ptr->data[190] * 256 + difop_packet_ptr->data[191];
+                        adjust_angle[1] = angle_a1 > 32767 ? 32767 - angle_a1 : angle_a1;
+
+                        int angle_a2 = difop_packet_ptr->data[188] * 256 + difop_packet_ptr->data[189];
+                        adjust_angle[2] = angle_a2 > 32767 ? 32767 - angle_a2 : angle_a2;
+
+                        int angle_a3 = difop_packet_ptr->data[192] * 256 + difop_packet_ptr->data[193];
+                        adjust_angle[3] = angle_a3 > 32767 ? 32767 - angle_a3 : angle_a3;
+                    } else {
+                        for (int j = 0; j < 32; ++j) {
+                            uint8_t data1 = difop_packet_ptr->data[882 + 2 * j];
+                            uint8_t data2 = difop_packet_ptr->data[882 + 2 * j + 1];
+                            int vert_angle = data1 * 256 + data2;
+                            vert_angle = vert_angle > 32767 ? (vert_angle - 65536) : vert_angle;
+                            c32_config_vertical_angle[j] = (double) vert_angle * 0.01f;
+
+                            if (fabs(c32_vertical_angle[j] - c32_config_vertical_angle[j]) > 3.0) {
+                                config_vert_num++;
+                            }
+                        }
+                        if (config_vert_num == 0) {
+                            for (int k = 0; k < 32; ++k) {
+                                c32_sin_scan_altitude[k] = sin(c32_config_vertical_angle[k] * DEG_TO_RAD);
+                                c32_cos_scan_altitude[k] = cos(c32_config_vertical_angle[k] * DEG_TO_RAD);
+                            }
+                        }
+                        // horizontal correction angle
+                        int angle_a0 = difop_packet_ptr->data[34] * 256 + difop_packet_ptr->data[35];
+                        adjust_angle[0] = angle_a0 > 32767 ? 32767 - angle_a0 : angle_a0;
+
+                        int angle_a1 = difop_packet_ptr->data[42] * 256 + difop_packet_ptr->data[43];
+                        adjust_angle[1] = angle_a1 > 32767 ? 32767 - angle_a1 : angle_a1;
+
+                        int angle_a2 = difop_packet_ptr->data[66] * 256 + difop_packet_ptr->data[67];
+                        adjust_angle[2] = angle_a2 > 32767 ? 32767 - angle_a2 : angle_a2;
+
+                        int angle_a3 = difop_packet_ptr->data[68] * 256 + difop_packet_ptr->data[69];
+                        adjust_angle[3] = angle_a3 > 32767 ? 32767 - angle_a3 : angle_a3;
+                    }
+                    get_difop = true;
+                    config_vert = false;
+                }
+                if (packet_size == 1206) {
+                    if (2 == c32_fpga_type) {
+                        this->packetTimeStamp[4] = difop_packet_ptr->data[41];
+                        this->packetTimeStamp[5] = difop_packet_ptr->data[40];
+                        this->packetTimeStamp[6] = difop_packet_ptr->data[39];
+                        this->packetTimeStamp[7] = difop_packet_ptr->data[38];
+                        this->packetTimeStamp[8] = difop_packet_ptr->data[37];
+                        this->packetTimeStamp[9] = difop_packet_ptr->data[36];
+                    } else {
+                        this->packetTimeStamp[4] = difop_packet_ptr->data[57];
+                        this->packetTimeStamp[5] = difop_packet_ptr->data[56];
+                        this->packetTimeStamp[6] = difop_packet_ptr->data[55];
+                        this->packetTimeStamp[7] = difop_packet_ptr->data[54];
+                        this->packetTimeStamp[8] = difop_packet_ptr->data[53];
+                        this->packetTimeStamp[9] = difop_packet_ptr->data[52];
+                    }
+                }
+
+            } else if (rc < 0) {
+                return;
+            }
+//            rclcpp::spinOnce();
+        }
+    }
+
+
+    void lslidarC32Driver::decodePacket(const RawPacket *packet) {
+        //couputer azimuth angle for each firing, 12
+
+        for (size_t fir_idx = 0; fir_idx < BLOCKS_PER_PACKET; ++fir_idx) {
+            firings.firing_azimuth[fir_idx] = packet->blocks[fir_idx].rotation % 36000; //* 0.01 * DEG_TO_RAD;
+            // ROS_INFO("azi==%d",packet->blocks[fir_idx].rotation);
+        }
+        // ROS_WARN("-----------------");
+        // computer distance ,intensity
+        //12 blocks
+        for (size_t blk_idx = 0; blk_idx < BLOCKS_PER_PACKET; ++blk_idx) {
+            const RawBlock &raw_block = packet->blocks[blk_idx];
+
+            int32_t azimuth_diff = 0;
+            if (1 == return_mode) {
+                if (blk_idx < BLOCKS_PER_PACKET - 1) {
+                    azimuth_diff = firings.firing_azimuth[blk_idx + 1] - firings.firing_azimuth[blk_idx];
+                } else {
+                    azimuth_diff = firings.firing_azimuth[blk_idx] - firings.firing_azimuth[blk_idx - 1];
+                }
+                azimuth_diff = azimuth_diff < 0 ? azimuth_diff + 36000 : azimuth_diff;
+            } else {
+                if (blk_idx < BLOCKS_PER_PACKET - 2) {
+                    azimuth_diff = firings.firing_azimuth[blk_idx + 2] - firings.firing_azimuth[blk_idx];
+
+                } else {
+                    azimuth_diff = firings.firing_azimuth[blk_idx] - firings.firing_azimuth[blk_idx - 2];
+                }
+                azimuth_diff = azimuth_diff < 0 ? azimuth_diff + 36000 : azimuth_diff;
+            }
+            // 32 scan
+            for (size_t scan_idx = 0; scan_idx < SCANS_PER_BLOCK; ++scan_idx) {
+                size_t byte_idx = RAW_SCAN_SIZE * scan_idx;
+                //azimuth
+                firings.azimuth[blk_idx * 32 + scan_idx] = firings.firing_azimuth[blk_idx] +
+                                                           scan_idx * azimuth_diff / FIRING_TOFFSET;
+
+                firings.azimuth[blk_idx * 32 + scan_idx] = firings.azimuth[blk_idx * 32 + scan_idx] % 36000;
+
+                /*
+                // calibration azimuth ，1°
+              if ("c32_2" == c32_type) {
+                    // -----结果是否是正数 ？
+                    int adjust_diff = adjust_angle[1] - adjust_angle[0];
+                    if (adjust_diff > 300 && adjust_diff < 460) {
+                        // fpga :v 2.7
+                        if (3 == c32_fpga_type) {
+                            if ( 1 >= scan_fir_idx % 4 ) {
+                                firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[1];
+                            } else {
+                                firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[0];
+                            }
+                           // ROS_INFO("id: %d--azi: %d",blk_idx * 32 + scan_fir_idx,firings.azimuth[blk_idx * 32 + scan_fir_idx]);
+                        } else {
+                            if (0 == scan_fir_idx % 2) {
+                                firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[1];
+                            } else {
+                                firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[0];
+                            }
+                        }
+                    } else {
+                        // fpga: v2.6
+                        if (0 == scan_fir_idx % 2) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[0];
+                        } else {
+                            int tmp_azimuth = firings.azimuth[blk_idx * 32 + scan_fir_idx] - adjust_angle[0];
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] =
+                                    tmp_azimuth < 0 ? tmp_azimuth + 36000 : tmp_azimuth;
+                        }
+                    }
+                }
+                //  calibration azimuth ,0.33°
+                if ("c32_1" == c32_type) {
+                    int adjust_diff_one = adjust_angle[1] - adjust_angle[0];
+                    int adjust_diff_two = adjust_angle[3] - adjust_angle[2];
+                    if (3 == c32_fpga_type) {
+                        // fpga v3.0
+                        if (0 == scan_fir_idx || 1 == scan_fir_idx || 4 == scan_fir_idx || 8 == scan_fir_idx ||
+                            9 == scan_fir_idx || 12 == scan_fir_idx
+                            || 16 == scan_fir_idx || 17 == scan_fir_idx || 21 == scan_fir_idx || 24 == scan_fir_idx ||
+                            25 == scan_fir_idx || 29 == scan_fir_idx) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[3];
+                        }
+                        if (2 == scan_fir_idx || 3 == scan_fir_idx || 6 == scan_fir_idx || 10 == scan_fir_idx ||
+                            11 == scan_fir_idx || 14 == scan_fir_idx
+                            || 18 == scan_fir_idx || 19 == scan_fir_idx || 23 == scan_fir_idx || 26 == scan_fir_idx ||
+                            27 == scan_fir_idx || 31 == scan_fir_idx) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[2];
+                        }
+                        if (5 == scan_fir_idx || 13 == scan_fir_idx || 20 == scan_fir_idx || 28 == scan_fir_idx) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[1];
+                        }
+                        if (7 == scan_fir_idx || 15 == scan_fir_idx || 22 == scan_fir_idx || 30 == scan_fir_idx) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[0];
+                        }
+                    } else if (adjust_diff_one > 500 && adjust_diff_one < 660 && adjust_diff_two > 150 &&
+                               adjust_diff_two < 350) {
+                        //fpga v2.7
+                        if (10 == scan_fir_idx || 14 == scan_fir_idx || 18 == scan_fir_idx || 22 == scan_fir_idx) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[3];
+                        }
+                        if (11 == scan_fir_idx || 15 == scan_fir_idx || 19 == scan_fir_idx || 23 == scan_fir_idx) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[2];
+                        }
+                        if (0 == scan_fir_idx || 2 == scan_fir_idx || 4 == scan_fir_idx || 6 == scan_fir_idx ||
+                            8 == scan_fir_idx || 12 == scan_fir_idx
+                            || 16 == scan_fir_idx || 20 == scan_fir_idx || 24 == scan_fir_idx || 26 == scan_fir_idx ||
+                            28 == scan_fir_idx || 30 == scan_fir_idx) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[1];
+                        }
+                        if (1 == scan_fir_idx || 3 == scan_fir_idx || 5 == scan_fir_idx || 7 == scan_fir_idx ||
+                            9 == scan_fir_idx || 13 == scan_fir_idx
+                            || 17 == scan_fir_idx || 21 == scan_fir_idx || 25 == scan_fir_idx || 27 == scan_fir_idx ||
+                            29 == scan_fir_idx || 31 == scan_fir_idx) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[0];
+                        }
+                    } else {
+                        // fpga v2.6
+                        if (10 == scan_fir_idx || 14 == scan_fir_idx || 18 == scan_fir_idx || 22 == scan_fir_idx) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[0];
+                        }
+                        if (11 == scan_fir_idx || 15 == scan_fir_idx || 19 == scan_fir_idx || 23 == scan_fir_idx) {
+                            int tmp_azimuth = firings.azimuth[blk_idx * 32 + scan_fir_idx] - adjust_angle[0];
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] =
+                                    tmp_azimuth < 0 ? tmp_azimuth + 36000 : tmp_azimuth;
+                        }
+                        if (0 == scan_fir_idx || 2 == scan_fir_idx || 4 == scan_fir_idx || 6 == scan_fir_idx ||
+                            8 == scan_fir_idx || 12 == scan_fir_idx
+                            || 16 == scan_fir_idx || 20 == scan_fir_idx || 24 == scan_fir_idx || 26 == scan_fir_idx ||
+                            28 == scan_fir_idx || 30 == scan_fir_idx) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[1];
+                        }
+                        if (1 == scan_fir_idx || 3 == scan_fir_idx || 5 == scan_fir_idx || 7 == scan_fir_idx ||
+                            9 == scan_fir_idx || 13 == scan_fir_idx
+                            || 17 == scan_fir_idx || 21 == scan_fir_idx || 25 == scan_fir_idx || 27 == scan_fir_idx ||
+                            29 == scan_fir_idx || 31 == scan_fir_idx) {
+                            int tmp_azimuth = firings.azimuth[blk_idx * 32 + scan_fir_idx] - adjust_angle[1];
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] =
+                                    tmp_azimuth < 0 ? tmp_azimuth + 36000 : tmp_azimuth;
+                        }
+
+                    }
+
+                }
+
+
+                firings.azimuth[blk_idx * 32 + scan_fir_idx] = firings.azimuth[blk_idx * 32 + scan_fir_idx] % 36000;
+                 */
+
+                // distance
+
+                firings.distance[blk_idx * 32 + scan_idx] =
+                        static_cast<double>(raw_block.data[byte_idx] + raw_block.data[byte_idx + 1] * 256) *
+                        DISTANCE_RESOLUTION * distance_unit;
+
+                //intensity
+                firings.intensity[blk_idx * 32 + scan_idx] = static_cast<double>(
+                        raw_block.data[byte_idx + 2]);
+            }
+
+        }
+        return;
+    }
+
+/** poll the device
+ *  @returns true unless end of file reached
+ */
+    bool lslidarC32Driver::poll(void) {
+        lslidar_msgs::msg::LslidarPacket::UniquePtr packet(new lslidar_msgs::msg::LslidarPacket());
+
+        // Since the rslidar delivers data at a very high rate, keep
+        // reading and publishing scans as fast as possible.
+        while (true) {
+            int rc = msop_input_->getPacket(packet);
+            if (rc == 0) break;
+            if (rc < 0) return false;
+        }
+        const RawPacket *raw_packet = (const RawPacket *) (&(packet->data[0]));
+        //check if the packet is valid
+        if (!checkPacketValidity(raw_packet)) return false;
+        // packet timestamp
+        if (use_gps_ts) {
+            if (packet_size == 1212) {
+                lslidar_msgs::msg::LslidarPacket pkt = *packet;
+                memset(&cur_time, 0, sizeof(cur_time));
+                cur_time.tm_year = pkt.data[1200] + 2000 - 1900;
+                cur_time.tm_mon = pkt.data[1201] - 1;
+                cur_time.tm_mday = pkt.data[1202];
+                cur_time.tm_hour = pkt.data[1203];
+                cur_time.tm_min = pkt.data[1204];
+                cur_time.tm_sec = pkt.data[1205];
+                cur_time.tm_year = cur_time.tm_year >= 200 ? 100 : cur_time.tm_year;
+                packet_time_s = static_cast<uint64_t>(timegm(&cur_time)); //s
+                packet_time_ns = (pkt.data[1206] +
+                                  pkt.data[1207] * pow(2, 8) +
+                                  pkt.data[1208] * pow(2, 16) +
+                                  pkt.data[1209] * pow(2, 24)) * 1e3; //ns
+                timeStamp = rclcpp::Time(packet_time_s, packet_time_ns);
+                packet->stamp = timeStamp;
+                current_packet_time = timeStamp.seconds();
+
+                if (packet->data[1210] == 0x39) {
+                    return_mode = 2;
+                }
+            } else {
+                lslidar_msgs::msg::LslidarPacket pkt = *packet;
+                memset(&cur_time, 0, sizeof(cur_time));
+                cur_time.tm_year = this->packetTimeStamp[9] + 2000 - 1900;
+                cur_time.tm_mon = this->packetTimeStamp[8] - 1;
+                cur_time.tm_mday = this->packetTimeStamp[7];
+                cur_time.tm_hour = this->packetTimeStamp[6];
+                cur_time.tm_min = this->packetTimeStamp[5];
+                cur_time.tm_sec = this->packetTimeStamp[4];
+                cur_time.tm_year = cur_time.tm_year >= 200 ? 100 : cur_time.tm_year;
+                packet_time_s = static_cast<uint64_t>(timegm(&cur_time)); //s
+                packet_time_ns = (pkt.data[1200] +
+                                  pkt.data[1201] * pow(2, 8) +
+                                  pkt.data[1202] * pow(2, 16) +
+                                  pkt.data[1203] * pow(2, 24)) * 1e3; //ns
+                timeStamp = rclcpp::Time(packet_time_s, packet_time_ns);
+                RCLCPP_INFO_ONCE(this->get_logger(), "timeStamp: %f", timeStamp.seconds());
+                RCLCPP_INFO_ONCE(this->get_logger(), "timeStamp_bak: %f", timeStamp_bak.seconds());
+                //使用gps授时
+                if ((timeStamp.seconds() - timeStamp_bak.seconds()) < 1e-9 &&
+                    (timeStamp.seconds() - timeStamp_bak.seconds()) > -1.0
+                    && is_update_gps_time && packet_time_ns < 100000000) {
+                    timeStamp = rclcpp::Time(packet_time_s + 1, packet_time_ns);
+                } else if ((timeStamp - timeStamp_bak).seconds() > 1.0 && (timeStamp - timeStamp_bak).seconds() < 1.2 &&
+                           is_update_gps_time
+                           && packet_time_ns > 900000000) {
+                    timeStamp = rclcpp::Time(packet_time_s - 1, packet_time_ns);
+                }
+                timeStamp_bak = timeStamp;
+                packet->stamp = timeStamp;
+                current_packet_time = timeStamp.seconds();
+                if (packet->data[1204] == 0x39) {
+                    return_mode = 2;
+                }
+            }
+        } else {
+            packet->stamp = get_clock()->now();;
+            current_packet_time = rclcpp::Time(packet->stamp).seconds();
+        }
+
+        RCLCPP_INFO_ONCE(this->get_logger(), "return mode: %d", return_mode);
+
+        //decode the packet
+        decodePacket(raw_packet);
+        // find the start of a new revolution
+        // if there is one, new_sweep_start will be the index of the start firing,
+        // otherwise, new_sweep_start will be FIRINGS_PER_PACKET.
+        size_t new_sweep_start = 0;
+        do {
+            if (last_azimuth - firings.azimuth[new_sweep_start] > 35000) {
+                break;
+            } else {
+                last_azimuth = firings.azimuth[new_sweep_start];
+                ++new_sweep_start;
+            }
+        } while (new_sweep_start < SCANS_PER_PACKET);
+
+        // The first sweep may not be complete. So, the firings with
+        // the first sweep will be discarded. We will wait for the
+        // second sweep in order to find the 0 azimuth angle.
+        size_t start_fir_idx = 0;
+        size_t end_fir_idx = new_sweep_start;
+        if (is_first_sweep && new_sweep_start == SCANS_PER_PACKET) {
+            return true;
+        } else {
+            if (is_first_sweep) {
+                is_first_sweep = false;
+                start_fir_idx = new_sweep_start;
+                end_fir_idx = SCANS_PER_PACKET;
+                //sweep_start_time = packet->stamp.toSec() - (end_fir_idx - start_fir_idx) * 3.125 * 1e-6;
+            }
+        }
+        last_azimuth_tmp = firings.azimuth[SCANS_PER_PACKET - 1];
+
+        for (int blk_idx = 0; blk_idx < BLOCKS_PER_PACKET; ++blk_idx) {
+            for (int scan_fir_idx = 0; scan_fir_idx < SCANS_PER_BLOCK; ++scan_fir_idx) {
+
+                // calibration azimuth ，1°
+                if ("c32_2" == c32_type) {
+
+//                    int adjust_diff = adjust_angle[1] - adjust_angle[0];
+//                    if (adjust_diff > 300 && adjust_diff < 460) {
+                    // fpga :v 2.8 3.0
+                    if (3 == c32_fpga_type) {
+                        if (1 >= scan_fir_idx % 4) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[1];
+                        } else {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[0];
+                        }
+                        // ROS_INFO("id: %d--azi: %d",blk_idx * 32 + scan_fir_idx,firings.azimuth[blk_idx * 32 + scan_fir_idx]);
+                    }
+//                    }
+                    else {
+                        // fpga: v2.6
+                        if (0 == scan_fir_idx % 2) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[0];
+                        } else {
+                            int tmp_azimuth = firings.azimuth[blk_idx * 32 + scan_fir_idx] - adjust_angle[0];
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] =
+                                    tmp_azimuth < 0 ? tmp_azimuth + 36000 : tmp_azimuth;
+                        }
+                    }
+                }
+                //  calibration azimuth ,0.33°
+                if ("c32_1" == c32_type) {
+                  //  int adjust_diff_one = adjust_angle[1] - adjust_angle[0];
+                  //  int adjust_diff_two = adjust_angle[3] - adjust_angle[2];
+                    if (3 == c32_fpga_type) {
+                        // fpga v 2.8 3.0
+                        if (0 == scan_fir_idx || 1 == scan_fir_idx || 4 == scan_fir_idx || 8 == scan_fir_idx ||
+                            9 == scan_fir_idx || 12 == scan_fir_idx
+                            || 16 == scan_fir_idx || 17 == scan_fir_idx || 21 == scan_fir_idx || 24 == scan_fir_idx ||
+                            25 == scan_fir_idx || 29 == scan_fir_idx) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[3];
+                        }
+                        if (2 == scan_fir_idx || 3 == scan_fir_idx || 6 == scan_fir_idx || 10 == scan_fir_idx ||
+                            11 == scan_fir_idx || 14 == scan_fir_idx
+                            || 18 == scan_fir_idx || 19 == scan_fir_idx || 23 == scan_fir_idx || 26 == scan_fir_idx ||
+                            27 == scan_fir_idx || 31 == scan_fir_idx) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[2];
+                        }
+                        if (5 == scan_fir_idx || 13 == scan_fir_idx || 20 == scan_fir_idx || 28 == scan_fir_idx) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[1];
+                        }
+                        if (7 == scan_fir_idx || 15 == scan_fir_idx || 22 == scan_fir_idx || 30 == scan_fir_idx) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[0];
+                        }
+                    } else {
+                        // fpga v2.6
+                        if (10 == scan_fir_idx || 14 == scan_fir_idx || 18 == scan_fir_idx || 22 == scan_fir_idx) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[0];
+                        }
+                        if (11 == scan_fir_idx || 15 == scan_fir_idx || 19 == scan_fir_idx || 23 == scan_fir_idx) {
+                            int tmp_azimuth = firings.azimuth[blk_idx * 32 + scan_fir_idx] - adjust_angle[0];
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] =
+                                    tmp_azimuth < 0 ? tmp_azimuth + 36000 : tmp_azimuth;
+                        }
+                        if (0 == scan_fir_idx || 2 == scan_fir_idx || 4 == scan_fir_idx || 6 == scan_fir_idx ||
+                            8 == scan_fir_idx || 12 == scan_fir_idx
+                            || 16 == scan_fir_idx || 20 == scan_fir_idx || 24 == scan_fir_idx || 26 == scan_fir_idx ||
+                            28 == scan_fir_idx || 30 == scan_fir_idx) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[1];
+                        }
+                        if (1 == scan_fir_idx || 3 == scan_fir_idx || 5 == scan_fir_idx || 7 == scan_fir_idx ||
+                            9 == scan_fir_idx || 13 == scan_fir_idx
+                            || 17 == scan_fir_idx || 21 == scan_fir_idx || 25 == scan_fir_idx || 27 == scan_fir_idx ||
+                            29 == scan_fir_idx || 31 == scan_fir_idx) {
+                            int tmp_azimuth = firings.azimuth[blk_idx * 32 + scan_fir_idx] - adjust_angle[1];
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] =
+                                    tmp_azimuth < 0 ? tmp_azimuth + 36000 : tmp_azimuth;
+                        }
+
+                    }
+
+                }
+                if (firings.azimuth[blk_idx * 32 + scan_fir_idx] < 0)
+                    firings.azimuth[blk_idx * 32 + scan_fir_idx] += 36000;
+                if (firings.azimuth[blk_idx * 32 + scan_fir_idx] > 36000)
+                    firings.azimuth[blk_idx * 32 + scan_fir_idx] -= 36000;
+            }
+
+        }
+
+
+        for (size_t fir_idx = start_fir_idx; fir_idx < end_fir_idx; ++fir_idx) {
+            //check if the point is valid
+            if (!isPointInRange(firings.distance[fir_idx]))continue;
+            //if (firings.azimuth[fir_idx]>=36000) { firings.azimuth[fir_idx]-=36000;}  // todo
+            //convert the point to xyz coordinate
+            firings.azimuth[fir_idx] =
+                    firings.azimuth[fir_idx] >= 36000 ? firings.azimuth[fir_idx] - 36000 : firings.azimuth[fir_idx];
+            size_t table_idx = firings.azimuth[fir_idx];
+            double cos_azimuth = cos_azimuth_table[table_idx];
+            double sin_azimuth = sin_azimuth_table[table_idx];
+            double x_coord, y_coord, z_coord;
+            if (coordinate_opt) {
+                //X-axis correspondence 0 degree
+                int tmp_idx = 1298 - firings.azimuth[fir_idx] < 0 ? 1298 - firings.azimuth[fir_idx] + 36000 : 1298 - firings.azimuth[fir_idx];
+                x_coord = firings.distance[fir_idx] * c32_cos_scan_altitude[fir_idx % 32] * cos_azimuth +
+                          R2_ * cos_azimuth_table[tmp_idx];
+                y_coord = -firings.distance[fir_idx] * c32_cos_scan_altitude[fir_idx % 32] * sin_azimuth +
+                          R2_ * sin_azimuth_table[tmp_idx];
+                z_coord = firings.distance[fir_idx] * c32_sin_scan_altitude[fir_idx % 32];
+
+            } else {
+                //Y-axis correspondence 0 degree
+                int tmp_idx = firings.azimuth[fir_idx] - 1298 < 0 ? firings.azimuth[fir_idx] -1298 + 36000 : firings.azimuth[fir_idx] -1298;
+                x_coord = firings.distance[fir_idx] * c32_cos_scan_altitude[fir_idx % 32] * sin_azimuth +
+                          R2_ * sin_azimuth_table[tmp_idx];
+                y_coord = firings.distance[fir_idx] * c32_cos_scan_altitude[fir_idx % 32] * cos_azimuth +
+                          R2_ * cos_azimuth_table[tmp_idx];
+                z_coord = firings.distance[fir_idx] * c32_sin_scan_altitude[fir_idx % 32];
+            }
+            // computer the time of the point
+            double time = current_packet_time  -
+                          (SCANS_PER_PACKET - fir_idx - 1) * (current_packet_time - last_packet_time) /
+                          (SCANS_PER_PACKET * 1.0) - sweep_end_time;
+            int remapped_scan_idx;
+            if (c32_fpga_type == 2) {
+                remapped_scan_idx = fir_idx % 32;
+            } else {
+                remapped_scan_idx = (fir_idx % 32) % 2 == 0 ? (fir_idx % 32) / 2 : (fir_idx % 32) / 2 + 16;
+            }
+            sweep_data->points.push_back(lslidar_msgs::msg::LslidarPoint());
+            lslidar_msgs::msg::LslidarPoint &new_point = sweep_data->points[
+                    sweep_data->points.size() - 1];
+            //pack the data into point msg
+            new_point.time = time;
+            new_point.x = x_coord;
+            new_point.y = y_coord;
+            new_point.z = z_coord;
+            new_point.intensity = firings.intensity[fir_idx];
+            new_point.ring = remapped_scan_idx;
+            new_point.azimuth = firings.azimuth[fir_idx];
+            new_point.distance = firings.distance[fir_idx];
+        }
+        // a new sweep begins ----------------------------------------------------
+
+        if (end_fir_idx != SCANS_PER_PACKET) {
+            //publish Last frame scan
+
+//            sweep_end_time = rclcpp::Time(packet->stamp).seconds() -
+//                             static_cast<double>(SCANS_PER_PACKET - end_fir_idx) * 3.125 * 1e-6;
+            sweep_end_time = current_packet_time - (SCANS_PER_PACKET - end_fir_idx - 1) *
+                                                   (current_packet_time - last_packet_time) / (SCANS_PER_PACKET *1.0);
+            sweep_end_time = sweep_end_time > 0 ? sweep_end_time : 0;
+            if (get_difop) publishPointcloud();
+            if (publish_scan) publishScan();
+
+            sweep_data = lslidar_msgs::msg::LslidarScan::UniquePtr(new lslidar_msgs::msg::LslidarScan());
+            //prepare the next frame scan
+            //sweep_start_time = packet->stamp.toSec() - (end_fir_idx - start_fir_idx) * 3.125 * 1e-6;
+            last_azimuth = last_azimuth_tmp;
+            start_fir_idx = end_fir_idx;
+            end_fir_idx = SCANS_PER_PACKET;
+            for (size_t fir_idx = start_fir_idx; fir_idx < end_fir_idx; ++fir_idx) {
+                //check if the point is valid
+                if (!isPointInRange(firings.distance[fir_idx]))continue;
+                //if (firings.azimuth[fir_idx]>=36000) { firings.azimuth[fir_idx]-=36000;}  // todo
+                firings.azimuth[fir_idx] =
+                        firings.azimuth[fir_idx] >= 36000 ? firings.azimuth[fir_idx] - 36000 : firings.azimuth[fir_idx];
+                //convert the point to xyz coordinate
+                size_t table_idx = firings.azimuth[fir_idx];
+                double cos_azimuth = cos_azimuth_table[table_idx];
+                double sin_azimuth = sin_azimuth_table[table_idx];
+                double x_coord, y_coord, z_coord;
+                if (coordinate_opt) {
+                    //X-axis correspondence 0 degree
+                    int tmp_idx = 1298 - firings.azimuth[fir_idx] < 0 ? 1298 - firings.azimuth[fir_idx] + 36000 : 1298 - firings.azimuth[fir_idx];
+                    x_coord = firings.distance[fir_idx] * c32_cos_scan_altitude[fir_idx % 32] * cos_azimuth +
+                              R2_ * cos_azimuth_table[tmp_idx];
+                    y_coord = -firings.distance[fir_idx] * c32_cos_scan_altitude[fir_idx % 32] * sin_azimuth +
+                              R2_ * sin_azimuth_table[tmp_idx];
+                    z_coord = firings.distance[fir_idx] * c32_sin_scan_altitude[fir_idx % 32];
+
+                } else {
+                    //Y-axis correspondence 0 degree
+                    int tmp_idx = firings.azimuth[fir_idx] - 1298 < 0 ? firings.azimuth[fir_idx] -1298 + 36000 : firings.azimuth[fir_idx] -1298;
+                    x_coord = firings.distance[fir_idx] * c32_cos_scan_altitude[fir_idx % 32] * sin_azimuth +
+                              R2_ * sin_azimuth_table[tmp_idx];
+                    y_coord = firings.distance[fir_idx] * c32_cos_scan_altitude[fir_idx % 32] * cos_azimuth +
+                              R2_ * cos_azimuth_table[tmp_idx];
+                    z_coord = firings.distance[fir_idx] * c32_sin_scan_altitude[fir_idx % 32];
+                }
+                // computer the time of the point
+//                double time = rclcpp::Time(packet->stamp).seconds() - (SCANS_PER_PACKET - fir_idx) * 3.125 * 1e-6;
+                double time = current_packet_time  -
+                              (SCANS_PER_PACKET - fir_idx - 1) * (current_packet_time - last_packet_time) /
+                              (SCANS_PER_PACKET * 1.0) - sweep_end_time;
+                int remapped_scan_idx;
+                if (c32_fpga_type == 2) {
+                    remapped_scan_idx = fir_idx % 32;
+                } else {
+                    remapped_scan_idx = (fir_idx % 32) % 2 == 0 ? (fir_idx % 32) / 2 : (fir_idx % 32) / 2 + 16;
+                }
+                sweep_data->points.push_back(lslidar_msgs::msg::LslidarPoint());
+                lslidar_msgs::msg::LslidarPoint &new_point = sweep_data->points[
+                        sweep_data->points.size() - 1];
+                //pack the data into point msg
+                new_point.time = time;
+                new_point.x = x_coord;
+                new_point.y = y_coord;
+                new_point.z = z_coord;
+                new_point.intensity = firings.intensity[fir_idx];
+                new_point.ring = remapped_scan_idx;
+                new_point.azimuth = firings.azimuth[fir_idx];
+                new_point.distance = firings.distance[fir_idx];
+            }
+
+        }
+        last_packet_time = current_packet_time;
+        //packet_pub.publish(*packet);
+        return true;
+    }
+
+
+}  // namespace lslidar_driver

src/ros2/src/Lslidar_ROS2_driver/lslidar_driver/src/lslidar_driver_node.cpp

@@ -0,0 +1,47 @@
+#include "lslidar_driver/lslidar_driver.h"
+#include "std_msgs/msg/string.h"
+
+using namespace lslidar_driver;
+volatile sig_atomic_t flag = 1;
+rclcpp::Node::SharedPtr node_p = nullptr;
+
+static void my_handler(int sig) {
+    flag = 0;
+}
+
+int main(int argc, char **argv) {
+    rclcpp::init(argc, argv);
+    signal(SIGINT, my_handler);
+    lidar_type = "c32";
+    node_p = rclcpp::Node::make_shared("test");
+    // node_p = std::make_shared<rclcpp::Node>();
+    node_p->declare_parameter("lidar_type","c16");
+    node_p->get_parameter("lidar_type", lidar_type);
+    //printf("lslidar type: %s", lidar_type.c_str());
+    RCLCPP_WARN(node_p->get_logger(),"lslidar type-----: %s", lidar_type.c_str());
+    // start the driver
+    if ("c16" == lidar_type) {
+        auto node = std::make_shared<lslidar_driver::lslidarC16Driver>();
+        if (!node->initialize()) {
+            RCLCPP_ERROR(node->get_logger(), "cannot initialize lslidar driver.");
+            return 0;
+        }
+        while (rclcpp::ok()) {
+            node->poll();
+
+        }
+        rclcpp::shutdown();
+
+    } else {
+        auto node = std::make_shared<lslidar_driver::lslidarC32Driver>();
+        if (!node->initialize()) {
+            RCLCPP_ERROR(node->get_logger(), "cannot initialize lslidar driver.");
+            return 0;
+        }
+        while (rclcpp::ok()) {
+            node->poll();
+        }
+        rclcpp::shutdown();
+    }
+    return 0;
+}

src/ros2/src/Lslidar_ROS2_driver/lslidar_msgs/CMakeLists.txt

@@ -0,0 +1,39 @@
+cmake_minimum_required(VERSION 3.5)
+project(lslidar_msgs)
+
+set(CMAKE_BUILD_TYPE Release)    #RelWithDebInfo00
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+    set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+    set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+    add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+if(BUILD_TESTING)
+    find_package(ament_lint_auto REQUIRED)
+    ament_lint_auto_find_test_dependencies()
+endif()
+
+rosidl_generate_interfaces(lslidar_msgs
+        "msg/LslidarPacket.msg"
+        "msg/LslidarPoint.msg"
+        "msg/LslidarScan.msg"
+        "srv/Lslidarcontrol.srv"
+        DEPENDENCIES builtin_interfaces std_msgs sensor_msgs
+        )
+
+ament_package()

src/ros2/src/Lslidar_ROS2_driver/lslidar_msgs/msg/LslidarPacket.msg

@@ -0,0 +1,6 @@
+# Raw Leishen LIDAR packet.
+
+builtin_interfaces/Time stamp              # packet timestamp
+uint8[1212] data        # packet contents
+
+

src/ros2/src/Lslidar_ROS2_driver/lslidar_msgs/msg/LslidarPoint.msg

@@ -0,0 +1,14 @@
+# Time when the point is captured
+float64 time
+
+# Converted distance in the sensor frame
+float64 x
+float64 y
+float64 z
+
+# Raw measurement from Leishen C16
+float64 azimuth
+float64 distance
+float64 intensity
+uint16 ring
+

src/ros2/src/Lslidar_ROS2_driver/lslidar_msgs/msg/LslidarScan.msg

@@ -0,0 +1,6 @@
+# Altitude of all the points within this scan
+float64 altitude
+
+# The valid points in this scan sorted by azimuth
+# from 0 to 359.99
+LslidarPoint[] points

src/ros2/src/Lslidar_ROS2_driver/lslidar_msgs/package.xml

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>lslidar_msgs</name>
+  <version>3.0.1</version>
+  <description>ROS message definitions for C32 LIDARs.</description>
+  <url>http://www.ros.org/wiki/lidar_c32</url>
+  <maintainer email="yutongyan@lidar.com">Yutong</maintainer>
+  <author>Yutong</author>
+  <license>BSD</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+
+  <build_depend>builtin_interfaces</build_depend>
+
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <exec_depend>builtin_interfaces</exec_depend>
+
+  <depend>std_msgs</depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/ros2/src/Lslidar_ROS2_driver/lslidar_msgs/srv/Lslidarcontrol.srv

@@ -0,0 +1,3 @@
+int32 lasercontrol
+---
+bool status