Initial commit

2026-07-27 13:51:19 +08:00
commit 7bec56ca51
5408 changed files with 1126933 additions and 0 deletions
@@ -0,0 +1,99 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(lslidar_c16_decoder)
+
+add_compile_options(-std=c++14)
+
+#set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${CMAKE_CURRENT_SOURCE_DIR}/cmake)
+#message(STATUS "CMAKE: ${CMAKE_MODULE_PATH}")
+
+find_package(catkin REQUIRED COMPONENTS
+        angles
+        nodelet
+        pcl_ros
+        roscpp
+        sensor_msgs
+        tf
+        lslidar_c16_driver
+        lslidar_c16_msgs
+        dynamic_reconfigure
+        pcl_conversions)
+
+find_package(Boost COMPONENTS signals)
+find_package(PkgConfig REQUIRED)
+find_package(yaml-cpp REQUIRED)
+
+catkin_package(
+        INCLUDE_DIRS include
+        #  LIBRARIES lslidar_c16_decoder
+        CATKIN_DEPENDS
+        angles
+        nodelet
+        pcl_ros
+        roscpp
+        sensor_msgs
+        tf
+        lslidar_c16_driver
+        lslidar_c16_msgs
+        dynamic_reconfigure
+        pcl_conversions
+        DEPENDS
+        Boost
+)
+
+
+include_directories(
+        include
+        ${catkin_INCLUDE_DIRS}
+        ${Boost_INCLUDE_DIR}
+        ${YAML_CPP_INCLUDE_DIRS}
+
+)
+
+
+add_library(lslidar_data src/rawdata.cc)
+
+target_link_libraries(lslidar_data
+        ${catkin_LIBRARIES}
+        ${libpcap_LIBRARIES}
+        yaml-cpp)
+
+add_library(lslidar_point src/convert.cc)
+target_link_libraries(lslidar_point
+        lslidar_data
+        ${catkin_LIBRARIES})
+
+
+add_library(lslidar_c16_decoder_nodelet src/cloud_nodelet.cc)
+target_link_libraries(lslidar_c16_decoder_nodelet lslidar_point
+        ${catkin_LIBRARIES})
+
+add_executable(lslidar_c16_decoder_node src/cloud_node.cc)
+target_link_libraries(lslidar_c16_decoder_node
+        lslidar_point
+        ${catkin_LIBRARIES}
+        ${libpcap_LIBRARIES})
+add_dependencies(lslidar_c16_decoder_node
+        ${${PROJECT_NAME}_EXPORTED_TARGETS}
+        ${catkin_EXPORTED_TARGETS}
+        )
+
+
+if(catkin_EXPORTED_TARGETS)
+    add_dependencies(lslidar_data ${catkin_EXPORTED_TARGETS})
+endif()
+
+
+install(TARGETS lslidar_data lslidar_point lslidar_c16_decoder_nodelet lslidar_c16_decoder_node
+        ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+        LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+        RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+        )
+
+install(DIRECTORY launch
+        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+        )
+install(FILES
+        nodelets.xml
+        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+        )
+
@@ -0,0 +1,67 @@
+/*
+ * This file is part of lslidar_n301 driver.
+ *
+ * The driver is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * The driver is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with the driver.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef _CONVERT_H_
+#define _CONVERT_H_
+
+#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/LaserScan.h>
+#include <dynamic_reconfigure/server.h>
+#include "rawdata.h"
+#include <sensor_msgs/TimeReference.h>
+
+namespace lslidar_c16_decoder {
+    class Convert {
+    public:
+        Convert(ros::NodeHandle node, ros::NodeHandle private_nh);
+
+        ~Convert() {
+        }
+
+    private:
+
+        void processScan(const lslidar_c16_msgs::LslidarC16ScanUnified::ConstPtr &scanMsg);
+
+        void timeSync(const sensor_msgs::TimeReferenceConstPtr &time_msg);
+        /// Pointer to dynamic reconfigure service srv_
+        void removeNan(lslidar_rawdata::VPointCloud &cloud_in,lslidar_rawdata::VPointCloud &cloud_out,std::vector<int> &index);
+        void publishScan(lslidar_c16_msgs::LslidarC16SweepPtr& sweepData, int scanNum);
+
+        boost::shared_ptr<lslidar_rawdata::RawData> data_;
+        ros::Subscriber packet_sub_;
+        ros::Subscriber sync_sub_;
+        ros::Time global_time;
+        double last_time;
+        lslidar_c16_msgs::LslidarC16ScanUnifiedPtr scan_recv;
+        lslidar_c16_msgs::LslidarC16SweepPtr sweep_data;
+        bool scan_start;
+        bool publish_scan;
+        int scan_num;
+        std::string scan_frame_id;
+        std::string pointcloud_topic;
+        size_t scan_nums;
+        bool time_synchronization_;
+        ros::Publisher output_;
+        ros::Publisher scan_pub;
+        std::vector<int> indices;
+        ros::Time scan_timestamp;
+        int frequency;
+        double  horizontal_angle_resolution;
+    };
+
+}  // namespace lslidar_c16_decoder
+#endif
@@ -0,0 +1,262 @@
+/*
+ * This file is part of lslidar_n301 driver.
+ *
+ * The driver is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * The driver is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with the driver.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef _RAWDATA_H
+#define _RAWDATA_H
+
+#include <ros/ros.h>
+#include <ros/package.h>
+#include <lslidar_c16_msgs/LslidarC16Packet.h>
+#include <lslidar_c16_msgs/LslidarC16ScanUnified.h>
+#include <lslidar_c16_msgs/LslidarC16Sweep.h>
+#include "std_msgs/String.h"
+#include <pcl/point_types.h>
+#include <pcl_ros/point_cloud.h>
+#include <pcl_ros/impl/transforms.hpp>
+#include <pcl_conversions/pcl_conversions.h>
+#include <stdio.h>
+
+#define DEG_TO_RAD 0.017453292
+#define RAD_TO_DEG 57.29577951
+
+namespace lslidar_rawdata {
+
+// static const float  ROTATION_SOLUTION_ = 0.18f;  //水平角分辨率 10hz
+    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);  // 96
+
+    static const float ROTATION_RESOLUTION = 0.01f;   /**< degrees 旋转角分辨率*/
+    static const uint16_t ROTATION_MAX_UNITS = 36000; /**< hundredths of degrees */
+
+    static const unsigned int POINTS_ONE_CHANNEL_PER_SECOND = 20000;
+    static const unsigned int BLOCKS_ONE_CHANNEL_PER_PKT = 12;
+
+    static const float DISTANCE_MAX = 200.0f;            /**< meters */
+    static const float DISTANCE_MIN = 0.2f;              /**< meters */
+    static const float DISTANCE_RESOLUTION = 0.01f;      /**< meters */
+    static const float DISTANCE_RESOLUTION_NEW = 0.005f; /**< meters */
+    static const float DISTANCE_MAX_UNITS = (DISTANCE_MAX / DISTANCE_RESOLUTION + 1.0f);
+/** @todo make this work for both big and little-endian machines */
+    static const uint16_t UPPER_BANK = 0xeeff;  //
+    static const uint16_t LOWER_BANK = 0xddff;
+
+/** Special Defines for LSC16 support **/
+    static const int LSC16_FIRINGS_PER_BLOCK = 2;
+    static const int LSC16_SCANS_PER_FIRING = 16;
+    static const float LSC16_BLOCK_TDURATION = 100.0f;  // [µs]
+    static const float LSC16_DSR_TOFFSET = 3.125f;        // [µs]
+    static const float LSC16_FIRING_TOFFSET = 50.0f;    // [µs]
+
+
+    static const int TEMPERATURE_MIN = 31;
+
+/** \brief Raw LSLIDAR C16 data block.
+ *
+ *  Each block contains data from either the upper or lower laser
+ *  bank.  The device returns three times as many upper bank blocks.
+ *
+ *  use stdint.h types, so things work with both 64 and 32-bit machines
+ */
+// block
+    typedef struct raw_block {
+        uint16_t header;  ///< UPPER_BANK or LOWER_BANK
+        uint8_t rotation_1;
+        uint8_t rotation_2;  /// combine rotation1 and rotation2 together to get 0-35999, divide by 100 to get degrees
+        uint8_t data[BLOCK_DATA_SIZE];  // 96
+    } raw_block_t;
+
+    struct PointXYZIRT {
+        PCL_ADD_POINT4D;
+        PCL_ADD_INTENSITY;
+        uint16_t ring;
+        double time;
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW  // make sure our new allocators are aligned
+    } EIGEN_ALIGN16;
+// enforce SSE padding for correct memory alignment
+/** used for unpacking the first two data bytes in a block
+ *
+ *  They are packed into the actual data stream misaligned.  I doubt
+ *  this works on big endian machines.
+ */
+    union two_bytes {
+        uint16_t uint;
+        uint8_t bytes[2];
+    };
+// Pre-compute the sine and cosine for the altitude angles.
+    static const double scan_altitude_original_2[16] = {
+            -0.2617993877991494, 0.017453292519943295,
+            -0.22689280275926285, 0.05235987755982989,
+            -0.19198621771937624, 0.08726646259971647,
+            -0.15707963267948966, 0.12217304763960307,
+            -0.12217304763960307, 0.15707963267948966,
+            -0.08726646259971647, 0.19198621771937624,
+            -0.05235987755982989, 0.22689280275926285,
+            -0.017453292519943295, 0.2617993877991494
+    };
+    static const double scan_altitude_original_1[16] = {
+            -0.17453292519943295,0.01160643952576229,
+            -0.15123277968530863,0.03490658503988659,
+            -0.12793263417118436,0.05811946409141117,
+            -0.10471975511965978,0.08141960960553547,
+            -0.08141960960553547,0.10471975511965978,
+            -0.05811946409141117,0.12793263417118436,
+            -0.03490658503988659,0.15123277968530863,
+            -0.01160643952576229,0.17453292519943295
+    };
+
+    double scan_altitude[16];
+
+    static const double cos_scan_altitude[16] = {
+            std::cos(scan_altitude[0]), std::cos(scan_altitude[1]),
+            std::cos(scan_altitude[2]), std::cos(scan_altitude[3]),
+            std::cos(scan_altitude[4]), std::cos(scan_altitude[5]),
+            std::cos(scan_altitude[6]), std::cos(scan_altitude[7]),
+            std::cos(scan_altitude[8]), std::cos(scan_altitude[9]),
+            std::cos(scan_altitude[10]), std::cos(scan_altitude[11]),
+            std::cos(scan_altitude[12]), std::cos(scan_altitude[13]),
+            std::cos(scan_altitude[14]), std::cos(scan_altitude[15]),
+    };
+
+    static const double sin_scan_altitude[16] = {
+            std::sin(scan_altitude[0]), std::sin(scan_altitude[1]),
+            std::sin(scan_altitude[2]), std::sin(scan_altitude[3]),
+            std::sin(scan_altitude[4]), std::sin(scan_altitude[5]),
+            std::sin(scan_altitude[6]), std::sin(scan_altitude[7]),
+            std::sin(scan_altitude[8]), std::sin(scan_altitude[9]),
+            std::sin(scan_altitude[10]), std::sin(scan_altitude[11]),
+            std::sin(scan_altitude[12]), std::sin(scan_altitude[13]),
+            std::sin(scan_altitude[14]), std::sin(scan_altitude[15]),
+    };
+    static const int PACKET_SIZE = 1206;
+    static const int BLOCKS_PER_PACKET = 12;
+    static const int PACKET_STATUS_SIZE = 4;
+    static const int SCANS_PER_PACKET = (SCANS_PER_BLOCK * BLOCKS_PER_PACKET);
+
+/** \brief Raw lslidar packet.
+ *
+ *  revolution is described in the device manual as incrementing
+ *    (mod 65536) for each physical turn of the device.  Our device
+ *    seems to alternate between two different values every third
+ *    packet.  One value increases, the other decreases.
+ *
+ *  \todo figure out if revolution is only present for one of the
+ *  two types of status fields
+ *
+ *  status has either a temperature encoding or the microcode level
+ */
+    typedef struct raw_packet {
+        raw_block_t blocks[BLOCKS_PER_PACKET];
+        uint16_t revolution;
+        uint8_t status[PACKET_STATUS_SIZE];
+    } raw_packet_t;
+
+    union vertical_point {
+        uint8_t uint[2];
+        uint16_t value;
+    };
+
+    typedef PointXYZIRT VPoint;
+    typedef pcl::PointCloud<VPoint> VPointCloud;
+
+/** \brief lslidar data conversion class */
+    class RawData {
+    public:
+        RawData();
+
+        ~RawData() {
+        }
+
+        /*load the cablibrated files: angle, distance, intensity*/
+        void loadConfigFile(ros::NodeHandle node, ros::NodeHandle private_nh);
+
+        /*unpack the UDP packet and opuput PCL PointXYZI type*/
+        void unpack(const lslidar_c16_msgs::LslidarC16Packet &pkt, VPointCloud::Ptr pointcloud,int Packet_num,lslidar_c16_msgs::LslidarC16SweepPtr& sweep_data);
+
+        void processDifop(const lslidar_c16_msgs::LslidarC16Packet::ConstPtr &difop_msg);
+
+        ros::Subscriber difop_sub_;
+        bool is_init_curve_;
+        bool is_init_angle_;
+        bool is_init_top_fw_;
+        int block_num = 0;
+        int intensity_mode_;
+        int intensityFactor;
+
+    private:
+        float R1_;
+        float R2_;
+        bool angle_flag_;
+        float start_angle_;
+        float end_angle_;
+        float max_distance_;
+        float min_distance_;
+        float distance_unit_;
+        int dis_resolution_mode_;
+        int return_mode_;
+        int degree_mode_;
+        int rpm_;
+        bool info_print_flag_;
+        std::string calibration_file_;
+        float scan_start_angle_;
+        float scan_end_angle_;
+
+        int packet_end;
+        double point_time;
+        double packet_end_time;
+
+        bool coordinate_opt;
+        bool echo_second_;
+        bool config_vert_;
+        bool config_vert_angle;
+        bool print_vert_;
+        bool config_vert_file_;
+        double vert_angle;
+        double cos_scan_altitude_caliration[LSC16_SCANS_PER_FIRING];
+        double sin_scan_altitude_caliration[LSC16_SCANS_PER_FIRING];
+    };
+
+    float sin_azimuth_table[ROTATION_MAX_UNITS];
+    float cos_azimuth_table[ROTATION_MAX_UNITS];
+
+
+    float VERT_ANGLE[32];
+    float HORI_ANGLE[32];
+    float aIntensityCal[7][32];
+    float aIntensityCal_old[1600][32];
+    bool Curvesis_new = true;
+    int g_ChannelNum[32][51];
+    float CurvesRate[32];
+
+    float temper = 31.0;
+    int tempPacketNum = 0;
+    int numOfLasers = 16;
+    int TEMPERATURE_RANGE = 40;
+
+
+
+}  // namespace lslidar_rawdata
+
+POINT_CLOUD_REGISTER_POINT_STRUCT(lslidar_rawdata::PointXYZIRT,
+                                  (float, x, x)(float, y, y)(float, z, z)
+                                          (float, intensity, intensity)
+                                          (uint16_t, ring, ring)
+                                          (double, time, time)
+                                          )
+
+#endif  // __RAWDATA_H
@@ -0,0 +1,51 @@
+<launch>
+  <arg name="device_ip" default="192.168.10.6" />
+  <arg name="msop_port" default="2368" />
+  <arg name="difop_port" default="2369" />
+  <arg name="return_mode" default="1" />
+  <arg name="time_synchronization" default="false" />
+
+  <node pkg="lslidar_c16_driver" type="lslidar_c16_driver_node" name="lslidar_c16_driver_node" output="screen">
+    <!--param name="pcap" value="$(find lslidar_c16_driver)/pcap/xxx.pcap" /-->
+    <param name="device_ip" value="$(arg device_ip)" />
+    <param name="msop_port" value="$(arg msop_port)" />
+    <param name="difop_port" value="$(arg difop_port)"/>
+    <!-- <param name="frame_id" value="velodyne"/> -->
+    <param name="frame_id" value="lidar"/>
+    <param name="add_multicast" value="false"/>
+    <param name="group_ip" value="224.1.1.2"/>
+    <param name="rpm" value="600"/>
+    <param name="return_mode" value="$(arg return_mode)"/>
+    <param name="time_synchronization" value="$(arg time_synchronization)"/>
+  </node>
+
+  <node pkg="lslidar_c16_decoder" type="lslidar_c16_decoder_node" name="lslidar_c16_decoder_node" output="screen">
+    <param name="scan_frame_id" value="laser_link"/>
+    <param name="min_range" value="0.15"/>
+    <param name="max_range" value="150.0"/>
+    <param name="frequency" value="10"/>
+    <param name="config_vert" value="true"/>
+    <param name="print_vert" value="false"/>
+    <param name="return_mode" value="$(arg return_mode)"/>
+    <param name="degree_mode" value="2"/>
+    <param name="config_vert_file" value="false"/>
+    <param name="distance_unit" value="0.25"/>
+    <param name="time_synchronization" value="$(arg time_synchronization)"/>
+    <param name="scan_start_angle" value="0.0"/>
+    <param name="scan_end_angle" value="36000.0"/>
+    <param name="scan_num" value="8"/>
+    <param name="publish_scan" value="false"/>
+    <remap from="/scan_topic" to="scan"/>
+    <param name="echo_second" value="false"/>
+    <param name="pointcloud_topic" value="lslidar_point_cloud"/>
+    <param name="coordinate_opt" value="true"/>
+  </node>
+
+  <!-- <node pkg="adaptive_clustering" type="adaptive_clustering" name="adaptive_clustering_node" output="screen">
+  </node> -->
+
+  <!-- <node pkg="rviz" type="rviz" name="rviz" args="-d $(find lslidar_c16_decoder)/../../rviz_cfg/EcoCar.rviz" /> -->
+ 
+ <!--node pkg="tf" type="static_transform_publisher" name="laser_link_to_world" args="0 0 0 0 0 0 world laser_link 100" /-->
+  
+</launch>
@@ -0,0 +1,89 @@
+
+<launch>
+	
+  <arg name="device_ip1" default="192.168.1.200" />
+  <arg name="device_ip2" default="192.168.1.201" />
+
+  <arg name="msop_port1" default="2368" />
+  <arg name="difop_port1" default="2369" />
+  <arg name="msop_port2" default="2370" />
+  <arg name="difop_port2" default="2371" />
+  <arg name="return_mode" default="1" />
+  <arg name="time_synchronization" default="false" />
+	
+
+
+  <node pkg="lslidar_c16_driver" type="lslidar_c16_driver_node" name="lslidar_c16_driver_node" output="screen" ns="LeftLidar" >
+    <param name="lidar_ip" value="$(arg device_ip1)" />
+    <param name="msop_port" value="$(arg msop_port1)" />
+    <param name="difop_port" value="$(arg difop_port1)"/>
+    <param name="frame_id" value="laser_link_left"/>
+    <param name="add_multicast" value="false"/>
+    <param name="group_ip" value="224.1.1.2"/>
+    <param name="rpm" value="600"/>
+    <param name="return_mode" value="$(arg return_mode)"/>
+    <param name="time_synchronization" value="$(arg time_synchronization)"/>
+  </node>
+
+  <node pkg="lslidar_c16_decoder" type="lslidar_c16_decoder_node" name="lslidar_c16_decoder_node" output="screen" ns="LeftLidar">
+    <param name="scan_frame_id" value="laser_link"/>
+    <param name="min_range" value="0.15"/>
+    <param name="max_range" value="150.0"/>
+    <param name="frequency" value="10"/>
+    <param name="config_vert" value="true"/>
+    <param name="print_vert" value="false"/>
+    <param name="return_mode" value="$(arg return_mode)"/>
+    <param name="degree_mode" value="2"/>
+    <param name="config_vert_file" value="false"/>
+    <param name="distance_unit" value="0.25"/>
+    <param name="time_synchronization" value="$(arg time_synchronization)"/>
+    <param name="scan_start_angle" value="0.0"/>
+    <param name="scan_end_angle" value="36000.0"/>
+    <param name="scan_num" value="8"/>
+    <param name="publish_scan" value="true"/>
+    <remap from="/scan_topic" to="scan"/>
+    <param name="echo_second" value="false"/>
+    <param name="pointcloud_topic" value="lslidar_point_cloud">
+    <param name="coordinate_opt" value="true">
+  </node>
+
+  <node pkg="lslidar_c16_driver" type="lslidar_c16_driver_node" name="lslidar_c16_driver_node" output="screen" ns="RightLidar" >
+    <param name="lidar_ip" value="$(arg device_ip2)" />
+    <param name="msop_port" value="$(arg msop_port2)" />
+    <param name="difop_port" value="$(arg difop_port2)"/>
+    <param name="frame_id" value="laser_link_right"/>
+    <param name="add_multicast" value="false"/>
+    <param name="group_ip" value="224.1.1.2"/>
+    <param name="rpm" value="600"/>
+    <param name="return_mode" value="$(arg return_mode)"/>
+    <param name="time_synchronization" value="$(arg time_synchronization)"/>
+  </node>
+
+  <node pkg="lslidar_c16_decoder" type="lslidar_c16_decoder_node" name="lslidar_c16_decoder_node" output="screen" ns="RightLidar">
+    <param name="scan_frame_id" value="laser_link"/>
+    <param name="min_range" value="0.15"/>
+    <param name="max_range" value="150.0"/>
+    <param name="frequency" value="10"/>
+    <param name="config_vert" value="true"/>
+    <param name="print_vert" value="false"/>
+    <param name="return_mode" value="$(arg return_mode)"/>
+    <param name="degree_mode" value="2"/>
+    <param name="config_vert_file" value="false"/>
+    <param name="distance_unit" value="0.25"/>
+    <param name="time_synchronization" value="$(arg time_synchronization)"/>
+    <param name="scan_start_angle" value="0.0"/>
+    <param name="scan_end_angle" value="36000.0"/>
+    <param name="scan_num" value="8"/>
+    <param name="publish_scan" value="true"/>
+    <remap from="/scan_topic" to="scan"/>
+    <param name="echo_second" value="false"/>
+    <param name="pointcloud_topic" value="lslidar_point_cloud">
+    <param name="coordinate_opt" value="true">
+  </node>
+    <!--node pkg="tf" type="static_transform_publisher" name="laser_link_right_to_world" args="0 0 0 0 0 0 world laser_link_right 100" /-->
+     <!--node pkg="tf" type="static_transform_publisher" name="laser_link_left_to_world" args="0 0 0 1 0 0 world laser_link_left 100" /-->
+
+
+	
+ 
+</launch>
@@ -0,0 +1,59 @@
+<launch>
+    <arg name="child_frame_id" default="lslidar"/>
+    <arg name="topic_packet" default="/apollo/sensor/lslidar_c16/LslidarPacket"/>
+    <arg name="topic_pointcloud" default="/apollo/sensor/lslidar_c16/PointCloud2"/>
+    <arg name="device_ip" default="192.168.1.200" />
+    <arg name="msop_port" default="2368" />
+    <arg name="difop_port" default="2369" />
+    <arg name="return_mode" default="1" />
+    <arg name="time_synchronization" default="true" />
+    <arg name="manager_name" default="nodelet_manager"/>
+
+
+
+
+  <!--include file="$(find lslidar_c16_driver)/launch/lslidar_c16_driver_nodelet.launch"/-->
+    <!-- nodelet manager -->
+
+     <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager" output="screen" />
+
+      <node pkg="nodelet" type="nodelet"
+        name="lslidar_c16_driver_nodelet"
+        args="load lslidar_c16_driver/DriverNodelet nodelet_manager" respawn="true">
+        <param name="device_ip" value="$(arg device_ip)" />
+        <param name="msop_port" value="$(arg msop_port)" />
+        <param name="difop_port" value="$(arg difop_port)"/>
+        <param name="frame_id" value="laser_link"/>
+        <param name="add_multicast" value="false"/>
+        <param name="group_ip" value="224.1.1.2"/>
+        <param name="rpm" value="600"/>
+        <param name="return_mode" value="$(arg return_mode)"/>
+        <param name="time_synchronization" value="$(arg time_synchronization)"/>
+      </node>
+
+
+  <node pkg="nodelet" type="nodelet"
+    name="LslidarC16DecoderNodelet"
+    args="load lslidar_c16_decoder/LslidarC16DecoderNodelet
+    $(arg manager_name)"
+    output="screen" respawn="true">
+    <param name="scan_frame_id" value="laser_link"/>
+    <param name="min_range" value="0.15"/>
+    <param name="max_range" value="150.0"/>
+    <param name="config_vert" value="true"/>
+    <param name="print_vert" value="false"/>
+    <param name="return_mode" value="$(arg return_mode)"/>
+    <param name="degree_mode" value="2"/>
+    <param name="config_vert_file" value="false"/>
+    <param name="distance_unit" value="0.25"/>
+    <param name="time_synchronization" value="$(arg time_synchronization)"/>
+    <param name="scan_start_angle" value="0.0"/>
+    <param name="scan_end_angle" value="36000.0"/>
+    <param name="scan_num" value="8"/>
+    <param name="publish_scan" value="true"/>
+
+  </node>
+  
+ <!-- <node name="rviz" pkg="rviz" type="rviz" args="-d $(find lslidar_c16_decoder)/launch/lslidar_c16.rviz" output="screen"/>
+-->
+</launch>
@@ -0,0 +1,12 @@
+<library path="lib/liblslidar_c16_decoder_nodelet">
+  <class name="lslidar_c16_decoder/LslidarC16DecoderNodelet"
+         type="lslidar_c16_decoder::LslidarC16DecoderNodelet"
+         base_class_type="nodelet::Nodelet">
+    <description>
+      Aggregates points from multiple packets, publishing PointCloud2.
+    </description>
+  </class>
+</library>
+
+
+
@@ -0,0 +1,48 @@
+<package>
+
+  <name>lslidar_c16_decoder</name>
+  <version>1.0.0</version>
+  <description>
+    Point cloud conversions for Leishen C16 LIDARs.
+  </description>
+  <url>http://www.ros.org/wiki/lslidar_c16</url>
+  <maintainer email="yutong@lslidar.com">Yutong</maintainer>
+  <author>Yutong</author>
+  <license>GNC General Public License V3.0</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>angles</build_depend>
+  <build_depend>nodelet</build_depend>
+  <build_depend>pcl_conversions</build_depend>
+  <build_depend>pcl_ros</build_depend>
+  <build_depend>pluginlib</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>lslidar_c16_driver</build_depend>
+  <build_depend>lslidar_c16_msgs</build_depend>
+  <build_depend>dynamic_reconfigure</build_depend>
+  <build_depend>yaml-cpp</build_depend>
+
+  <!-- these build dependencies are only needed for unit testing -->
+  <build_depend>tf2_ros</build_depend>
+
+  <run_depend>angles</run_depend>
+  <run_depend>nodelet</run_depend>
+  <run_depend>pcl_ros</run_depend>
+  <run_depend>pluginlib</run_depend>
+  <run_depend>pcl_conversions</run_depend>
+  <!-- <run_depend>python-yaml</run_depend> -->
+  <run_depend>roscpp</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>tf</run_depend>
+  <run_depend>lslidar_c16_driver</run_depend>
+  <run_depend>lslidar_c16_msgs</run_depend>
+  <!-- <run_depend>yaml-cpp</run_depend> -->
+  <run_depend>dynamic_reconfigure</run_depend>
+
+  <export>
+    <nodelet plugin="${prefix}/nodelets.xml"/>
+  </export>
+</package>
@@ -0,0 +1,18 @@
+lasers:
+- {vert_correction: -15}    #Channel 0
+- {vert_correction: 1}      #Channel 1
+- {vert_correction: -13}    #Channel 2
+- {vert_correction: 3}      #Channel 3
+- {vert_correction: -11}    #Channel 4
+- {vert_correction: 5}      #Channel 5
+- {vert_correction: -9}     #Channel 6
+- {vert_correction: 7}      #Channel 7
+- {vert_correction: -7}     #Channel 8
+- {vert_correction: 9}      #Channel 9
+- {vert_correction: -5}     #Channel 10
+- {vert_correction: 11}     #Channel 11
+- {vert_correction: -3}     #Channel 12
+- {vert_correction: 13}     #Channel 13
+- {vert_correction: -1}     #Channel 14
+- {vert_correction: 15}     #Channel 15
+
@@ -0,0 +1,152 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+      Splitter Ratio: 0.5
+    Tree Height: 565
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679016
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: PointCloud2
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.0299999993
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 255
+      Min Color: 0; 0; 0
+      Min Intensity: 9
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.00999999978
+      Style: Flat Squares
+      Topic: /lslidar_point_cloud
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Class: rviz/Axes
+      Enabled: true
+      Length: 1
+      Name: Axes
+      Radius: 0.100000001
+      Reference Frame: <Fixed Frame>
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: laser_link
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 21.8094082
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.0599999987
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 1.78493285
+        Y: 1.5556097
+        Z: 0.881267607
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.0500000007
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.00999999978
+      Pitch: 0.509797454
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 3.85353827
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 846
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a000002c4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002c4000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000030000fffffffb0000000800540069006d0065010000000000000450000000000000000000000340000002c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1200
+  X: 60
+  Y: 26
@@ -0,0 +1,34 @@
+/*
+ * This file is part of lslidar_n301 driver.
+ *
+ * The driver is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * The driver is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with the driver.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "lslidar_c16_decoder/convert.h"
+
+/** Main node entry point. */
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "lslidar_decoder_node");
+  ros::NodeHandle node;
+  ros::NodeHandle priv_nh("~");
+
+  // create conversion class, which subscribes to raw data
+  lslidar_c16_decoder::Convert conv(node, priv_nh);
+
+  // handle callbacks until shut down
+  ros::spin();
+
+  return 0;
+}
@@ -0,0 +1,50 @@
+/*
+ * This file is part of lslidar_n301 driver.
+ *
+ * The driver is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * The driver is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with the driver.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <ros/ros.h>
+#include <pluginlib/class_list_macros.h>
+#include <nodelet/nodelet.h>
+
+#include "lslidar_c16_decoder/convert.h"
+
+namespace lslidar_c16_decoder
+{
+class LslidarC16DecoderNodelet : public nodelet::Nodelet
+{
+public:
+  LslidarC16DecoderNodelet()
+  {
+  }
+  ~LslidarC16DecoderNodelet()
+  {
+  }
+
+private:
+  virtual void onInit();
+  boost::shared_ptr<Convert> conv_;
+};
+
+/** @brief Nodelet initialization. */
+void LslidarC16DecoderNodelet::onInit()
+{
+  conv_.reset(new Convert(getNodeHandle(), getPrivateNodeHandle()));
+}
+
+}  // namespace lslidar_c16_decoder
+
+// parameters: class type, base class type
+PLUGINLIB_EXPORT_CLASS(lslidar_c16_decoder::LslidarC16DecoderNodelet, nodelet::Nodelet);
@@ -0,0 +1,201 @@
+/*
+ * This file is part of lslidar_n301 driver.
+ *
+ * The driver is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * The driver is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with the driver.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "lslidar_c16_decoder/convert.h"
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/filters/filter.h>
+
+
+namespace lslidar_c16_decoder {
+    std::string model;
+
+/** @brief Constructor. */
+    Convert::Convert(ros::NodeHandle node, ros::NodeHandle private_nh) : data_(new lslidar_rawdata::RawData()) {
+        scan_nums = 0;
+        scan_start = true;
+
+        data_->loadConfigFile(node, private_nh);  // load lidar parameters
+        private_nh.param("model", model, std::string("LSC16"));
+
+        // advertise output point cloud (before subscribing to input data)
+
+        private_nh.param("pointcloud_topic", pointcloud_topic, std::string("lslidar_point_cloud"));
+        output_ = node.advertise<sensor_msgs::PointCloud2>(pointcloud_topic, 10);
+
+        scan_pub = node.advertise<sensor_msgs::LaserScan>("scan_topic", 100);
+
+        // subscribe to lslidar packets
+        packet_sub_ = node.subscribe("lslidar_packet_c16", 10, &Convert::processScan, (Convert *) this,
+                                     ros::TransportHints().tcpNoDelay(true));
+
+        private_nh.param("time_synchronization", time_synchronization_, false);
+        private_nh.param("scan_num", scan_num, 8);
+        private_nh.param("publish_scan", publish_scan, false);
+        private_nh.param("scan_frame_id", scan_frame_id, std::string("laser_link"));
+        private_nh.param("frequency", frequency, 10);
+        if(publish_scan) {
+            if (scan_num < 0) {
+                scan_num = 0;
+                ROS_WARN("channel_num_ outside of the index, select channel 0 instead!");
+            } else if (scan_num > 15) {
+                scan_num = 15;
+                ROS_WARN("channel_num_ outside of the index, select channel 15 instead!");
+            }
+            ROS_INFO("select scan_num: %d", scan_num);
+        }
+        switch (frequency){
+            case 5:
+                horizontal_angle_resolution = DEG2RAD(0.09);
+                break;
+            case 20:
+                horizontal_angle_resolution = DEG2RAD(0.36);
+                break;
+            default:
+                horizontal_angle_resolution = DEG2RAD(0.18);
+        }
+
+        if (time_synchronization_) {
+            sync_sub_ = node.subscribe("sync_header", 10, &Convert::timeSync, (Convert *) this,
+                                       ros::TransportHints().tcpNoDelay(true));
+        }
+    }
+
+    void Convert::timeSync(const sensor_msgs::TimeReferenceConstPtr &time_msg) {
+        global_time = time_msg->header.stamp;
+    }
+
+    void Convert::removeNan(lslidar_rawdata::VPointCloud &cloud_in, lslidar_rawdata::VPointCloud &cloud_out,
+                            std::vector<int> &index) {
+        if (&cloud_in != &cloud_out) {
+            cloud_out.header = cloud_in.header;
+            cloud_out.resize(cloud_in.size());
+            cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
+            cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
+        }
+        index.resize(cloud_in.size());
+        if (cloud_in.is_dense) {
+            cloud_out = cloud_in;
+            for (std::size_t j = 0; j < cloud_out.size(); ++j)
+                index[j] = j;
+        } else {
+            std::size_t j = 0;
+            for (std::size_t i = 0; i < cloud_in.size(); ++i) {
+                if (!std::isfinite(cloud_in[i].x) ||
+                    !std::isfinite(cloud_in[i].y) ||
+                    !std::isfinite(cloud_in[i].z))
+                    continue;
+                cloud_out[j] = cloud_in[i];
+                index[j] = i;
+                j++;
+            }
+            if(j != cloud_in.size()){
+                cloud_out.resize(j);
+                index.resize(j);
+            }
+            cloud_out.height = 1;
+            cloud_out.width = static_cast<std::uint32_t>(j);
+            cloud_out.is_dense =true;
+        }
+
+    }
+
+    void Convert::publishScan(lslidar_c16_msgs::LslidarC16SweepPtr &sweepData, int scanNum) {
+        sensor_msgs::LaserScan::Ptr scan(new sensor_msgs::LaserScan);
+        if (sweepData->scans[scanNum].points.size() <= 1)
+            return;
+
+        uint16_t point_num = 2 * M_PI / horizontal_angle_resolution;
+
+
+        scan->header.frame_id = scan_frame_id;
+        scan->header.stamp = sweepData->header.stamp;  // timestamp will obtained from sweep data stamp
+        scan->angle_min = 0;
+        scan->angle_max = 2 * M_PI;
+        scan->angle_increment = horizontal_angle_resolution;
+        scan->range_min = 0.15;
+        scan->range_max = 150;
+        scan->ranges.reserve(point_num);
+        scan->ranges.assign(point_num, std::numeric_limits<float>::infinity());
+        scan->intensities.reserve(point_num);
+        scan->intensities.assign(point_num, std::numeric_limits<float>::infinity());
+
+        for (uint16_t i = 0; i < sweepData->scans[scanNum].points.size() - 1; i++) {
+            int point_idx = sweepData->scans[scanNum].points[i].azimuth / scan->angle_increment;
+
+            if (point_idx >= point_num)
+                point_idx = 0;
+            if (point_idx < 0)
+                point_idx = point_num - 1;
+
+            scan->ranges[point_num - 1 - point_idx] = sweepData->scans[scanNum].points[i].distance;
+
+            scan->intensities[point_num - 1 - point_idx] = sweepData->scans[scanNum].points[i].intensity;
+        }
+        scan_pub.publish(scan);
+    }
+
+
+/** @brief Callback for raw scan messages. */
+    void Convert::processScan(const lslidar_c16_msgs::LslidarC16ScanUnified::ConstPtr &scanMsg) {
+        // pcl::PointCloud<pcl::PointXYZI>::Ptr outPoints(new pcl::PointCloud<pcl::PointXYZI>);
+        lslidar_rawdata::VPointCloud::Ptr outPoints(new lslidar_rawdata::VPointCloud);
+
+        sweep_data = lslidar_c16_msgs::LslidarC16SweepPtr(new lslidar_c16_msgs::LslidarC16Sweep());
+
+        if (time_synchronization_) {
+            outPoints->header.stamp = pcl_conversions::toPCL(scanMsg->header).stamp;
+            scan_timestamp = scanMsg->header.stamp;
+        } else {
+            outPoints->header.stamp = ros::Time::now().toNSec() / 1000ull;
+        }
+
+        outPoints->header.frame_id = scanMsg->header.frame_id;
+
+        outPoints->clear();
+        outPoints->height = 16;
+        outPoints->width = 24 * (int) scanMsg->packets.size();
+
+        outPoints->is_dense = false;
+        outPoints->resize(outPoints->height * outPoints->width);
+        int blockNum, dsr;
+        lslidar_rawdata::VPoint point;
+        point.x = NAN;
+        point.y = NAN;
+        point.z = NAN;
+        point.intensity = 0;
+        for (int i = 0; i < outPoints->height * outPoints->width; ++i) {
+            blockNum = i / 16;
+            dsr = i % 16;
+            outPoints->at(blockNum, dsr) = point;
+        }
+
+        // process each packet provided by the driver
+        data_->block_num = 0;
+
+        for (int i = 0; i < scanMsg->packets.size(); ++i) {
+            data_->unpack(scanMsg->packets[i], outPoints, i, sweep_data);
+        }
+
+        removeNan(*outPoints, *outPoints, indices);
+        sensor_msgs::PointCloud2 outMsg;
+        pcl::toROSMsg(*outPoints, outMsg);
+        output_.publish(outMsg);
+
+        if (publish_scan) { publishScan(sweep_data, scan_num); }
+
+    }
+}  // namespace lslidar_c16_decoder
@@ -0,0 +1,398 @@
+/*
+ * This file is part of lslidar_n301 driver.
+ *
+ * The driver is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * The driver is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with the driver.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "lslidar_c16_decoder/rawdata.h"
+#include <angles/angles.h>
+#include <yaml-cpp/yaml.h>
+
+namespace lslidar_rawdata {
+    RawData::RawData() {
+        this->is_init_angle_ = false;
+        this->is_init_curve_ = false;
+        this->is_init_top_fw_ = false;
+    }
+
+
+    void RawData::loadConfigFile(ros::NodeHandle node, ros::NodeHandle private_nh) {
+        std::string model;
+        std::string resolution_param;
+
+        private_nh.param("start_angle", start_angle_, float(0));
+        private_nh.param("end_angle", end_angle_, float(360));
+        private_nh.param("distance_unit", distance_unit_, float(0.25));
+        private_nh.param("calibration_file", calibration_file_, std::string(""));
+        private_nh.param("scan_start_angle", scan_start_angle_, float(0.0));
+        private_nh.param("scan_end_angle", scan_end_angle_, float(36000.0));
+        private_nh.param("rpm", rpm_, 300);
+
+
+        if (start_angle_ < 0 || start_angle_ > 360 || end_angle_ < 0 || end_angle_ > 360) {
+            start_angle_ = 0;
+            end_angle_ = 360;
+
+            ROS_INFO_STREAM("start angle and end angle select feature deactivated.");
+        } else {
+            ROS_INFO_STREAM("start angle and end angle select feature activated.");
+        }
+
+        angle_flag_ = true;
+        if (start_angle_ > end_angle_) {
+            angle_flag_ = false;
+            ROS_INFO_STREAM("Start angle is smaller than end angle, not the normal state!");
+        }
+        ROS_INFO_STREAM(
+                "start_angle: " << start_angle_ << " end_angle: " << end_angle_ << " angle_flag: " << angle_flag_);
+
+        start_angle_ = start_angle_ / 180 * M_PI;
+        end_angle_ = end_angle_ / 180 * M_PI;
+
+        private_nh.param("max_range", max_distance_, 150.0f);
+        private_nh.param("min_range", min_distance_, 0.15f);
+        private_nh.param("return_mode", return_mode_, 1);
+        private_nh.param("config_vert", config_vert_, true);
+        private_nh.param("print_vert", print_vert_, true);
+        private_nh.param("config_vert_file", config_vert_file_, false);
+        private_nh.param("degree_mode", degree_mode_, 1);
+        private_nh.param("echo_second", echo_second_, false);
+        private_nh.param("coordinate_opt", coordinate_opt, true);
+
+        ROS_INFO_STREAM("distance threshlod, max: " << max_distance_ << ", min: " << min_distance_);
+        ROS_INFO_STREAM("return mode : " << return_mode_);
+        if (2 == degree_mode_) {
+            ROS_INFO_STREAM("vertical angle resolution: 2 degree");
+        } else if (1 == degree_mode_) {
+            ROS_INFO_STREAM("vertical angle resolution: 1.33 degree");
+        }
+
+        intensity_mode_ = 1;
+        info_print_flag_ = false;
+        config_vert_angle = false;
+
+        private_nh.param("model", model, std::string("LSC16"));
+        numOfLasers = 16;
+        R1_ = 0.04319;   //calibration
+        R2_ = 0.010875;
+
+        intensityFactor = 51;
+
+        //return mode default
+        //return_mode_ = 1;
+
+
+
+        if (2 == degree_mode_) {
+            //Vertical Angle Calibration for device package
+            for (int i = 0; i < LSC16_SCANS_PER_FIRING; i++) {
+                cos_scan_altitude_caliration[i] = std::cos(scan_altitude_original_2[i]);
+                sin_scan_altitude_caliration[i] = std::sin(scan_altitude_original_2[i]);
+                scan_altitude[i] = scan_altitude_original_2[i];
+            }
+        } else if (1 == degree_mode_) {
+            for (int i = 0; i < LSC16_SCANS_PER_FIRING; i++) {
+                cos_scan_altitude_caliration[i] = std::cos(scan_altitude_original_1[i]);
+                sin_scan_altitude_caliration[i] = std::sin(scan_altitude_original_1[i]);
+                scan_altitude[i] = scan_altitude_original_1[i];
+            }
+
+        }
+
+#if 0
+        //Vertical Angle Calibration for file
+        if(config_vert_file_){
+          YAML::Node calibration_config = YAML::LoadFile(calibration_file_);
+          for(int i = 0; i < LSC16_SCANS_PER_FIRING; i++){
+            vert_angle = calibration_config["lasers"][i]["vert_correction"].as<double>();
+            cos_scan_altitude_caliration[i] = std::cos(vert_angle*DEG_TO_RAD);
+            sin_scan_altitude_caliration[i] = std::sin(vert_angle*DEG_TO_RAD);
+          }
+        }
+#endif
+
+
+        for (uint16_t rot_index = 0; rot_index < ROTATION_MAX_UNITS; ++rot_index) {
+            float rotation = angles::from_degrees(ROTATION_RESOLUTION * rot_index);
+            cos_azimuth_table[rot_index] = cosf(rotation);
+            sin_azimuth_table[rot_index] = sinf(rotation);
+        }
+
+        // receive difop data
+        // subscribe to difop lslidar packets, if not right correct data in difop, it will not revise the correct data in the
+        // VERT_ANGLE, HORI_ANGLE etc.
+        difop_sub_ = node.subscribe("lslidar_packet_difop_c16", 10, &RawData::processDifop, (RawData *) this);
+    }
+
+    void RawData::processDifop(const lslidar_c16_msgs::LslidarC16Packet::ConstPtr &difop_msg) {
+        // std::cout << "Enter difop callback!" << std::endl;
+        const uint8_t *data = &difop_msg->data[0];
+        bool is_support_dual_return = false;
+
+        // check header
+        if (data[0] != 0xa5 || data[1] != 0xff || data[2] != 0x00 || data[3] != 0x5a) {
+            return;
+        }
+
+        int version_data = data[1202];
+        if (config_vert_) {
+            if (2 == version_data) {
+                for (int i = 0; i < 16; i++) {
+                    uint8_t data1 = data[234 + 2 * i];
+                    uint8_t data2 = data[234 + 2 * i + 1];
+                    int vert_angle = data1 * 256 + data2;
+                    if (vert_angle > 32767) {
+                        vert_angle = vert_angle - 65535;
+                    }
+
+                    //ROS_INFO_STREAM("data vert: " << vert_angle);
+                    scan_altitude[i] = ((float) vert_angle / 100.f) * DEG_TO_RAD;
+                    if (2 == degree_mode_) {
+                        if (scan_altitude[i] != 0) {
+                            if (fabs(scan_altitude_original_2[i] - scan_altitude[i]) * RAD_TO_DEG > 1.5) {
+                                scan_altitude[i] = scan_altitude_original_2[i];
+                            }
+                        } else {
+                            scan_altitude[i] = scan_altitude_original_2[i];
+                        }
+                    } else if (1 == degree_mode_) {
+                        if (scan_altitude[i] != 0) {
+                            if (fabs(scan_altitude_original_1[i] - scan_altitude[i]) * RAD_TO_DEG > 1.5) {
+                                scan_altitude[i] = scan_altitude_original_1[i];
+                            }
+                        } else {
+                            scan_altitude[i] = scan_altitude_original_1[i];
+                        }
+
+                    }
+                    config_vert_angle = true;
+                }
+            } else {
+                for (int i = 0; i < 16; i++) {
+                    uint8_t data1 = data[245 + 2 * i];
+                    uint8_t data2 = data[245 + 2 * i + 1];
+                    int vert_angle = data1 * 256 + data2;
+                    if (vert_angle > 32767) {
+                        vert_angle = vert_angle - 65535;
+                    }
+
+                    //ROS_INFO_STREAM("data vert: " << vert_angle);
+                    scan_altitude[i] = ((float) vert_angle / 100.f) * DEG_TO_RAD;
+
+                    if (2 == degree_mode_) {
+                        if (scan_altitude[i] != 0) {
+                            if (fabs(scan_altitude_original_2[i] - scan_altitude[i]) * RAD_TO_DEG > 1.5) {
+                                scan_altitude[i] = scan_altitude_original_2[i];
+                            }
+                        } else {
+                            scan_altitude[i] = scan_altitude_original_2[i];
+                        }
+                    } else if (1 == degree_mode_) {
+                        if (scan_altitude[i] != 0) {
+                            if (fabs(scan_altitude_original_1[i] - scan_altitude[i]) * RAD_TO_DEG > 1.5) {
+                                scan_altitude[i] = scan_altitude_original_1[i];
+                            }
+                        } else {
+                            scan_altitude[i] = scan_altitude_original_1[i];
+                        }
+
+                    }
+                    config_vert_angle = true;
+                }
+            }
+        }
+
+
+        // rpm
+        if ((data[8] == 0x04) && (data[9] == 0xB0)) {
+            rpm_ = 1200;
+        } else if ((data[8] == 0x02) && (data[9] == 0x58)) {
+            rpm_ = 600;
+        } else if ((data[8] == 0x01) && (data[9] == 0x2C)) {
+            rpm_ = 300;
+        } else if ((data[8] == 0x00) && (data[9] == 0x2f)) {
+            rpm_ = 600;
+        } else {
+            //rpm_ = 300;
+            //ROS_WARN("Invalid motor rpm!");
+        }
+        if (print_vert_) {
+            //ROS_INFO("rpm is %d", rpm_);
+        }
+    }
+
+
+/** @brief convert raw packet to point cloud
+ *
+ *  @param pkt raw packet to unpack
+ *  @param pc shared pointer to point cloud (points are appended)
+ */
+    void
+    RawData::unpack(const lslidar_c16_msgs::LslidarC16Packet &pkt, VPointCloud::Ptr pointcloud,
+                    int Packet_num,
+                    lslidar_c16_msgs::LslidarC16SweepPtr &sweep_data) {
+        float azimuth;  // 0.01 dgree
+        float intensity;
+        float azimuth_diff;
+        float azimuth_corrected_f;
+        int azimuth_corrected;
+
+        if (config_vert_angle) {
+            for (int i = 0; i < LSC16_SCANS_PER_FIRING; i++) {
+                cos_scan_altitude_caliration[i] = std::cos(scan_altitude[i]);
+                sin_scan_altitude_caliration[i] = std::sin(scan_altitude[i]);
+
+                if (print_vert_) {
+                    ROS_INFO("Channel %d Data  %f", i, scan_altitude[i] * RAD_TO_DEG);
+                }
+            }
+            config_vert_angle = false;
+        }
+
+        const raw_packet_t *raw = (const raw_packet_t *) &pkt.data[0];
+        packet_end_time = pkt.stamp.toSec();
+        for (int block = 0; block < BLOCKS_PER_PACKET; block++, this->block_num++)  // 1 packet:12 data blocks
+        {
+            if (UPPER_BANK != raw->blocks[block].header) {
+                //ROS_INFO_STREAM_THROTTLE(180, "skipping LSLIDAR DIFOP packet");
+                break;
+            }
+            azimuth = (float) (256 * raw->blocks[block].rotation_2 + raw->blocks[block].rotation_1);
+
+
+            if (2 == return_mode_) {
+
+                if (block < (BLOCKS_PER_PACKET - 2))  // 12
+                {
+                    int azi1, azi2;
+                    azi1 = 256 * raw->blocks[block + 2].rotation_2 + raw->blocks[block + 2].rotation_1;
+                    azi2 = 256 * raw->blocks[block].rotation_2 + raw->blocks[block].rotation_1;
+                    azimuth_diff = (float) ((36000 + azi1 - azi2) % 36000);
+                } else {
+                    int azi1, azi2;
+                    azi1 = 256 * raw->blocks[block].rotation_2 + raw->blocks[block].rotation_1;
+                    azi2 = 256 * raw->blocks[block - 2].rotation_2 + raw->blocks[block - 2].rotation_1;
+                    azimuth_diff = (float) ((36000 + azi1 - azi2) % 36000);
+                }
+
+            } else {
+
+                if (block < (BLOCKS_PER_PACKET - 1))  // 12
+                {
+                    int azi1, azi2;
+                    azi1 = 256 * raw->blocks[block + 1].rotation_2 + raw->blocks[block + 1].rotation_1;
+                    azi2 = 256 * raw->blocks[block].rotation_2 + raw->blocks[block].rotation_1;
+                    azimuth_diff = (float) ((36000 + azi1 - azi2) % 36000);
+                } else {
+                    //block 12
+                    int azi1, azi2;
+                    azi1 = 256 * raw->blocks[block].rotation_2 + raw->blocks[block].rotation_1;
+                    azi2 = 256 * raw->blocks[block - 1].rotation_2 + raw->blocks[block - 1].rotation_1;
+                    azimuth_diff = (float) ((36000 + azi1 - azi2) % 36000);
+                }
+            }
+
+            float cos_azimuth;
+            float sin_azimuth;
+            packet_end = (POINTS_ONE_CHANNEL_PER_SECOND / BLOCKS_ONE_CHANNEL_PER_PKT) / 2 * 60 / rpm_ * return_mode_;
+
+            for (int firing = 0, k = 0; firing < LSC16_FIRINGS_PER_BLOCK; firing++)  // 2
+            {
+                for (int dsr = 0; dsr < LSC16_SCANS_PER_FIRING; dsr++, k += RAW_SCAN_SIZE)  // 16   3
+                {
+                    azimuth_corrected_f = azimuth + azimuth_diff / (LSC16_SCANS_PER_FIRING * 2) *
+                                                    (LSC16_SCANS_PER_FIRING * firing + dsr);
+
+                    azimuth_corrected = ((int) round(azimuth_corrected_f)) % 36000;  // convert to integral value...
+
+                    cos_azimuth = cos_azimuth_table[azimuth_corrected];
+                    sin_azimuth = sin_azimuth_table[azimuth_corrected];
+
+
+                    //distance
+                    union two_bytes tmp;
+                    tmp.bytes[0] = raw->blocks[block].data[k];
+                    tmp.bytes[1] = raw->blocks[block].data[k + 1];
+                    int distance = tmp.uint;
+
+                    // read intensity
+                    intensity = raw->blocks[block].data[k + 2];
+                    float distance2 = (distance * DISTANCE_RESOLUTION) * distance_unit_;
+
+                    //The offset calibration
+                    float arg_horiz = (float) azimuth_corrected_f * ROTATION_RESOLUTION;
+                    arg_horiz = arg_horiz > 360 ? (arg_horiz - 360) : arg_horiz;
+                    float arg_horiz_orginal = (arg_horiz - 14.68) * M_PI / 180;
+
+                    if (2 == return_mode_) {
+                        point_time = packet_end_time - 100 * (5 - block / 2) * 1e-6 -
+                                     3.125 * (31 - firing * 16 - dsr) * 1e-6;
+                    } else {
+                        point_time =
+                                packet_end_time - 100 * (11 - block) - 1e-6 - 3.125 * (31 - firing * 16 - dsr) * 1e-6;
+                    }
+
+                    VPoint point;
+                    if ((distance2 > max_distance_) || (distance2 < min_distance_) ||
+                        (azimuth_corrected_f < scan_start_angle_) || (azimuth_corrected_f > scan_end_angle_) ||
+                        (Packet_num < 10 && azimuth_corrected > 18000) ||
+                        (Packet_num > int(packet_end * 0.9) && azimuth_corrected < 18000) ||
+                        (2 == return_mode_ && echo_second_ && block % 2 == 0)) {
+                        point.x = NAN;
+                        point.y = NAN;
+                        point.z = NAN;
+                        point.intensity = 0;
+                        point.ring = (dsr % 2) * 8 + dsr / 2;
+                        point.time = point_time;
+                        pointcloud->at(2 * this->block_num + firing, point.ring) = point;
+                    } else {
+                        if (coordinate_opt) {
+                            point.x = distance2 * cos_scan_altitude_caliration[dsr] * cos_azimuth +
+                                      R1_ * cos((14.68 - arg_horiz) * M_PI / 180);
+                            point.y = -distance2 * cos_scan_altitude_caliration[dsr] * sin_azimuth -
+                                      R1_ * sin((14.68 - arg_horiz) * M_PI / 180);
+                            point.z = distance2 * sin_scan_altitude_caliration[dsr] + R1_;
+
+                        } else {
+                            point.x = distance2 * cos_scan_altitude_caliration[dsr] * sin_azimuth +
+                                      R1_ * sin(arg_horiz_orginal);
+                            point.y = distance2 * cos_scan_altitude_caliration[dsr] * cos_azimuth +
+                                      R1_ * cos(arg_horiz_orginal);
+                            point.z = distance2 * sin_scan_altitude_caliration[dsr] + R1_;
+                        }
+
+                        point.intensity = intensity;
+                        point.ring = (dsr % 2) * 8 + dsr / 2;
+                        point.time = point_time;
+
+                        pointcloud->at(2 * this->block_num + firing, point.ring) = point;
+
+                        sweep_data->scans[dsr].points.push_back(lslidar_c16_msgs::LslidarC16Point());
+                        lslidar_c16_msgs::LslidarC16Point &new_point = sweep_data->scans[dsr].points[
+                                sweep_data->scans[dsr].points.size() - 1];
+
+                        // Pack the data into point msg
+                        new_point.x = point.x;
+                        new_point.y = point.y;
+                        new_point.z = point.z;
+                        new_point.azimuth = angles::from_degrees(azimuth_corrected * ROTATION_RESOLUTION);
+                        new_point.distance = sqrt(pow(point.x, 2) + pow(point.y, 2) + pow(point.z, 2));
+                        new_point.intensity = point.intensity;
+                    }
+                }
+            }
+        }
+    }
+
+}  // namespace lslidar_c16_decoder