change driver_lidar_leishen_chx. not complete.

src/driver/driver_lidar_leishen_chx/.gitignore

@@ -0,0 +1,74 @@
+# This file is used to ignore files which are generated
+# ----------------------------------------------------------------------------
+
+*~
+*.autosave
+*.a
+*.core
+*.moc
+*.o
+*.obj
+*.orig
+*.rej
+*.so
+*.so.*
+*_pch.h.cpp
+*_resource.rc
+*.qm
+.#*
+*.*#
+core
+!core/
+tags
+.DS_Store
+.directory
+*.debug
+Makefile*
+*.prl
+*.app
+moc_*.cpp
+ui_*.h
+qrc_*.cpp
+Thumbs.db
+*.res
+*.rc
+/.qmake.cache
+/.qmake.stash
+
+# qtcreator generated files
+*.pro.user*
+CMakeLists.txt.user*
+
+# xemacs temporary files
+*.flc
+
+# Vim temporary files
+.*.swp
+
+# Visual Studio generated files
+*.ib_pdb_index
+*.idb
+*.ilk
+*.pdb
+*.sln
+*.suo
+*.vcproj
+*vcproj.*.*.user
+*.ncb
+*.sdf
+*.opensdf
+*.vcxproj
+*vcxproj.*
+
+# MinGW generated files
+*.Debug
+*.Release
+
+# Python byte code
+*.pyc
+
+# Binaries
+# --------
+*.dll
+*.exe
+

src/driver/driver_lidar_leishen_chx/driver_lidar_leishen_chx.pro

@@ -0,0 +1,43 @@
+QT = core
+
+QT       += network
+
+CONFIG += c++1z cmdline
+
+# You can make your code fail to compile if it uses deprecated APIs.
+# In order to do so, uncomment the following line.
+#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
+
+SOURCES += \
+        leishenchx.cpp \
+        main.cpp \
+        ../common/lidar/lidardecode.cpp \
+        ../common/lidar/lidarudp.cpp
+
+# Default rules for deployment.
+qnx: target.path = /tmp/$${TARGET}/bin
+else: unix:!android: target.path = /opt/$${TARGET}/bin
+!isEmpty(target.path): INSTALLS += target
+
+HEADERS += \
+    leishenchx.h \
+    ../common/lidar/lidardecode.h \
+    ../common/lidar/lidarudp.h
+
+!include(../../../include/common.pri ) {
+    error( "Couldn't find the common.pri file!" )
+}
+
+!include(../../../include/ivpcl.pri ) {
+    error( "Couldn't find the ivpcl.pri file!" )
+}
+
+!include(../../../include/ivyaml-cpp.pri ) {
+    error( "Couldn't find the ivyaml-cpp.pri file!" )
+}
+
+!include(../../../include/iveigen.pri ) {
+    error( "Couldn't find the iveigen.pri file!" )
+}
+
+INCLUDEPATH += $$PWD/../common/lidar

src/driver/driver_lidar_leishen_chx/leishenchx.cpp

@@ -0,0 +1,937 @@
+#include "leishenchx.h"
+
+leishenchx::leishenchx(char * strmemname,double froll,double finclinationang_xaxis ,
+                       double finclinationang_yaxis , int nDevMode ,
+                       unsigned short nDataPort,unsigned short nDevPort ):
+    socket_id(-1),
+    point_cloud_xyzi_(new pcl::PointCloud<pcl::PointXYZI>),
+    point_cloud_xyzi_bak_(new pcl::PointCloud<pcl::PointXYZI>),
+    gain_prism_angle(true)
+{
+    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
+        new pcl::PointCloud<pcl::PointXYZI>());
+
+    point_cloud->header.frame_id = "velodyne";
+    point_cloud->height = 1;
+    point_cloud->header.stamp = static_cast<uint64_t>(std::chrono::system_clock::now().time_since_epoch().count()/1000);
+    point_cloud->width = 0;
+    point_cloud->header.seq =0;
+    mpoint_cloud_temp = point_cloud;
+
+
+    RegisterSend(strmemname);
+    mnLidarMode = nDevMode;
+    mfrollang = froll;
+    mfinclinationang_xaxis = finclinationang_xaxis;
+    mfinclinationang_yaxis = finclinationang_yaxis;
+
+    prism_offset = 0.0;
+    last_packet_timestamp = 0.0;
+    is_update_difop_packet = false;
+    for (int j = 0; j < 36000; ++j) {
+        sin_list[j] = sin(j * 0.01 * DEG_TO_RAD);
+        cos_list[j] = cos(j * 0.01 * DEG_TO_RAD);
+    }
+
+    initialize();
+
+    mplidarudp = new lidarudp(nDataPort);
+    mplidardevudp = new lidarudp(nDevPort);
+    mpthreaddev = new std::thread(&leishenchx::threaddevdecode,this);
+    mpthread = new std::thread(&leishenchx::threaddecode,this);
+}
+
+leishenchx::~leishenchx()
+{
+
+}
+
+bool leishenchx::initialize()
+{
+    for (double &j : prism_angle) {
+        j = 0.0f;
+    }
+    //ch256
+    for (int k1 = 0; k1 < 256; ++k1) {
+        ch256_sin_theta_1[k1] = sin((-20.0 + floor(k1 / 4) * 0.625) * DEG_TO_RAD);
+        ch256_cos_theta_1[k1] = cos((-20.0 + floor(k1 / 4) * 0.625) * DEG_TO_RAD);
+        ch256_sin_theta_2[k1] = sin((k1 % 4) * 0.11 * DEG_TO_RAD);
+        ch256_cos_theta_2[k1] = cos((k1 % 4) * 0.11 * DEG_TO_RAD);
+    }
+    // ch64w
+    for (int m = 0; m < 128; ++m) {
+        //右边
+        if (m / 4 % 2 == 0) {
+            ch64w_sin_theta_1[m] = sin((-25 + floor(m / 8) * 2.5) * DEG_TO_RAD);
+            ch64w_sin_theta_2[m] = sin((m % 4) * 0.35 * DEG_TO_RAD);
+            ch64w_cos_theta_1[m] = cos((-25 + floor(m / 8) * 2.5) * DEG_TO_RAD);
+            ch64w_cos_theta_2[m] = cos((m % 4) * 0.35 * DEG_TO_RAD);
+        } else { //左边
+            ch64w_sin_theta_1[m] = sin((-24 + prism_offset + floor(m / 8) * 2.5) * DEG_TO_RAD);
+            ch64w_sin_theta_2[m] = sin((m % 4) * 0.35 * DEG_TO_RAD);
+            ch64w_cos_theta_1[m] = cos((-24 + prism_offset + floor(m / 8) * 2.5) * DEG_TO_RAD);
+            ch64w_cos_theta_2[m] = cos((m % 4) * 0.35 * DEG_TO_RAD);
+        }
+    }
+
+    //CB64S1_A
+    for (int m = 0; m < 64; ++m) {
+        //右边
+        cb64s1_A_sin_theta_1[m] = sin((-25 + floor(m / 4) * 2.5) * DEG_TO_RAD);
+        cb64s1_A_sin_theta_2[m] = sin((m % 4) * 0.35 * DEG_TO_RAD);
+        cb64s1_A_cos_theta_1[m] = cos((-25 + floor(m / 4) * 2.5) * DEG_TO_RAD);
+        cb64s1_A_cos_theta_2[m] = cos((m % 4) * 0.35 * DEG_TO_RAD);
+    }
+
+
+    for (int i = 0; i < 128; i++) {
+        // 左边
+        if (i / 4 % 2 == 0) {
+            sin_theta_1[i] = sin((big_angle[i / 4] + prism_offset) * DEG_TO_RAD);
+            cos_theta_1[i] = cos((big_angle[i / 4] + prism_offset) * DEG_TO_RAD);
+
+        } else {
+            sin_theta_1[i] = sin(big_angle[i / 4] * DEG_TO_RAD);
+            cos_theta_1[i] = cos(big_angle[i / 4] * DEG_TO_RAD);
+        }
+
+        if (lidar_type == "ch128s1" || lidar_type == "cx128s2") {
+            sin_theta_1[i] = sin(big_angle_ch128s1[i / 4] * DEG_TO_RAD);
+            cos_theta_1[i] = cos(big_angle_ch128s1[i / 4] * DEG_TO_RAD);
+        }
+        sin_theta_2[i] = sin((i % 4) * (-0.17) * DEG_TO_RAD);
+        cos_theta_2[i] = cos((i % 4) * (-0.17) * DEG_TO_RAD);
+    }
+    for (int l = 0; l < 126; ++l) {
+        cx126s3_sin_theta_1[l] = sin(big_angle_cx126s3[l / 3] * DEG_TO_RAD);
+        cx126s3_cos_theta_1[l] = cos(big_angle_cx126s3[l / 3] * DEG_TO_RAD);
+
+        cx126s3_sin_theta_2[l] = sin((l % 3) * (-0.14) * DEG_TO_RAD);
+        cx126s3_cos_theta_2[l] = cos((l % 3) * (-0.14) * DEG_TO_RAD);
+    }
+
+    for (int i1 = 0; i1 < 6; ++i1) {
+        cx6s3_sin_theta_1[i1] = sin(big_angle_cx6s3[i1 / 3] * DEG_TO_RAD);
+        cx6s3_cos_theta_1[i1] = cos(big_angle_cx6s3[i1 / 3] * DEG_TO_RAD);
+
+        cx6s3_sin_theta_2[i1] = sin((i1 % 3) * (-0.14) * DEG_TO_RAD);
+        cx6s3_cos_theta_2[i1] = cos((i1 % 3) * (-0.14) * DEG_TO_RAD);
+    }
+
+    for (int k = 0; k < 16; ++k) {
+        // 左边  prism_offset = 0.0
+        if (k / 4 % 2 == 0) {
+            ch16x1_sin_theta_1[k] = sin((big_angle_ch16x1[k / 4] + prism_offset) * DEG_TO_RAD);
+            ch16x1_cos_theta_1[k] = cos((big_angle_ch16x1[k / 4] + prism_offset) * DEG_TO_RAD);
+        } else {
+            ch16x1_sin_theta_1[k] = sin(big_angle_ch16x1[k / 4] * DEG_TO_RAD);
+            ch16x1_cos_theta_1[k] = cos(big_angle_ch16x1[k / 4] * DEG_TO_RAD);
+        }
+        ch16x1_sin_theta_2[k] = sin((k % 4) * (-0.17) * DEG_TO_RAD);
+        ch16x1_cos_theta_2[k] = cos((k % 4) * (-0.17) * DEG_TO_RAD);
+    }
+
+    point_cloud_xyzirt_->header.frame_id = frame_id;
+    point_cloud_xyzirt_->height = 1;
+
+    point_cloud_xyzi_->header.frame_id = frame_id;
+    point_cloud_xyzi_->height = 1;
+
+
+
+    return true;
+}
+
+void leishenchx::threaddevdecode()
+{
+    std::vector<iv::lidarudppac> xvectorlidarudppac;
+    while(mbrun)
+    {
+        xvectorlidarudppac = mplidardevudp->ComsumeRecv(100);
+        if(xvectorlidarudppac.size()>0)
+        {
+            unsigned int i;
+            for(i=0;i<xvectorlidarudppac.size();i++)
+            {
+                DecodeDevinfo(xvectorlidarudppac[i]);
+            }
+        }
+    }
+}
+
+void leishenchx::threaddecode()
+{
+    std::vector<iv::lidarudppac> xvectorlidarudppac;
+    while(mbrun)
+    {
+        xvectorlidarudppac = mplidarudp->ComsumeRecv(10);
+        if(xvectorlidarudppac.size()>0)
+        {
+            unsigned int i;
+            for(i=0;i<xvectorlidarudppac.size();i++)
+            {
+                DecodeUDPPac(xvectorlidarudppac[i]);
+            }
+        }
+    }
+}
+
+int leishenchx::DecodeDevinfo(iv::lidarudppac & xpac)
+{
+    unsigned char * pdata =  (unsigned char *)xpac.mdata_ptr.get();
+    if (pdata[0] == 0xa5 && pdata[1] == 0xff &&
+        pdata[2] == 0x00 &&
+        pdata[3] == 0x5a) {
+        is_update_difop_packet = true;
+        if (pdata[44] == 0x00) {
+            //gps授时
+            time_service_mode = "gps";
+        } else if (pdata[44] == 0x01) {
+            //ptp授时
+            time_service_mode = "gptp";
+        }
+        if (use_time_service) {
+            std::cout<<"time service mode: "<<time_service_mode.c_str()<<std::endl;
+//            ROS_INFO("time service mode: %s", time_service_mode.c_str());
+        }
+
+        for (int j = 0; j < 1206; ++j) {
+            difop_data[j] = pdata[j];
+        }
+
+        if (gain_prism_angle) {
+            // 240 241   左边 增加角度
+            int prism_offset_difop = pdata[240] * 256 + pdata[241];
+
+            prism_offset_difop =
+                prism_offset_difop > 32767 ? prism_offset_difop - 65536 : prism_offset_difop;
+            this->prism_offset = prism_offset_difop * 0.01;
+            //ROS_INFO("prism=%f",prism_offset);
+
+            int angle0 = pdata[242] * 256 + pdata[243];
+            angle0 = angle0 > 32767 ? (angle0 - 65536) : angle0;
+            this->prism_angle[0] = angle0 * 0.01;
+            //ROS_INFO("0aa=%f",prism_angle[0]);
+
+            int angle1 = pdata[244] * 256 + pdata[245];
+            angle1 = angle1 > 32767 ? (angle1 - 65536) : angle1;
+            this->prism_angle[1] = angle1 * 0.01;
+            //ROS_INFO("1aa=%f",prism_angle[1]);
+
+            int angle2 = pdata[246] * 256 + pdata[247];
+            angle2 = angle2 > 32767 ? (angle2 - 65536) : angle2;
+            this->prism_angle[2] = angle2 * 0.01;
+            //ROS_INFO("2aa=%f",prism_angle[2]);
+
+            int angle3 = pdata[248] * 256 + pdata[249];
+            angle3 = angle3 > 32767 ? (angle3 - 65536) : angle3;
+            this->prism_angle[3] = angle3 * 0.01;
+            //ROS_INFO("3aa=%f",prism_angle[3]);
+            if (lidar_type == "ch128x1" || lidar_type == "ch128s1" || lidar_type == "cx128s2") {
+                for (int i = 0; i < 128; i++) {
+                    // sinTheta_2[i] = sin((i % 4) * (-0.17) * M_PI / 180);
+                    if (fabs(this->prism_angle[0]) < 1e-6 && fabs(this->prism_angle[1]) < 1e-6 &&
+                        fabs(this->prism_angle[2]) < 1e-6 &&
+                        fabs(this->prism_angle[3]) < 1e-6) {
+
+                        sin_theta_2[i] = sin((i % 4) * (-0.17) * DEG_TO_RAD);
+                        cos_theta_2[i] = cos((i % 4) * (-0.17) * DEG_TO_RAD);
+                    } else {
+                        sin_theta_2[i] = sin(this->prism_angle[i % 4] * DEG_TO_RAD);
+                        cos_theta_2[i] = cos(this->prism_angle[i % 4] * DEG_TO_RAD);
+                    }
+
+                    // 左边
+                    if (i / 4 % 2 == 0) {
+                        sin_theta_1[i] = sin((big_angle[i / 4] + prism_offset) * DEG_TO_RAD);
+                        cos_theta_1[i] = cos((big_angle[i / 4] + prism_offset) * DEG_TO_RAD);
+
+                    } else {
+                        sin_theta_1[i] = sin(big_angle[i / 4] * DEG_TO_RAD);
+                        cos_theta_1[i] = cos(big_angle[i / 4] * DEG_TO_RAD);
+                    }
+
+                    if (lidar_type == "ch128s1" || lidar_type == "cx128s2") {
+                        // 左边
+                        if (i / 4 % 2 == 0) {
+                            sin_theta_1[i] = sin(
+                                (big_angle_ch128s1[i / 4] + prism_offset) * DEG_TO_RAD);
+                            cos_theta_1[i] = cos(
+                                (big_angle_ch128s1[i / 4] + prism_offset) * DEG_TO_RAD);
+                        } else {
+                            sin_theta_1[i] = sin(big_angle_ch128s1[i / 4] * DEG_TO_RAD);
+                            cos_theta_1[i] = cos(big_angle_ch128s1[i / 4] * DEG_TO_RAD);
+                        }
+
+                    }
+                }
+
+            } else if (lidar_type == "ch256") {
+                for (int i = 0; i < 256; i++) {
+                    // sinTheta_2[i] = sin((i % 4) * (-0.17) * M_PI / 180);
+                    if (fabs(this->prism_angle[0]) < 1e-6 && fabs(this->prism_angle[1]) < 1e-6 &&
+                        fabs(this->prism_angle[2]) < 1e-6 &&
+                        fabs(this->prism_angle[3]) < 1e-6) {
+                        ch256_sin_theta_2[i] = sin((i % 4) * 0.11 * DEG_TO_RAD);
+                        ch256_cos_theta_2[i] = cos((i % 4) * 0.11 * DEG_TO_RAD);
+                    } else {
+                        ch256_sin_theta_2[i] = sin(this->prism_angle[i % 4] * DEG_TO_RAD);
+                        ch256_cos_theta_2[i] = cos(this->prism_angle[i % 4] * DEG_TO_RAD);
+                    }
+                    // 左边
+                    if (i / 4 % 2 == 0) {
+                        ch256_sin_theta_1[i] = sin(
+                            (-20.0 + floor(i / 4) * 0.625 + prism_offset) * DEG_TO_RAD);
+                        ch256_cos_theta_1[i] = cos(
+                            (-20.0 + floor(i / 4) * 0.625 + prism_offset) * DEG_TO_RAD);
+                    } else {
+                        ch256_sin_theta_1[i] = sin((-20.0 + floor(i / 4) * 0.625) * DEG_TO_RAD);
+                        ch256_cos_theta_1[i] = cos((-20.0 + floor(i / 4) * 0.625) * DEG_TO_RAD);
+                    }
+                }
+            } else if (lidar_type == "cx126s3") {
+                for (int i = 0; i < 126; i++) {
+                    // sinTheta_2[i] = sin((i % 4) * (-0.17) * M_PI / 180);
+                    if (fabs(this->prism_angle[0]) < 1e-6 && fabs(this->prism_angle[1]) < 1e-6 &&
+                        fabs(this->prism_angle[2]) < 1e-6) {
+                        cx126s3_sin_theta_2[i] = sin((i % 3) * (-0.14) * DEG_TO_RAD);
+                        cx126s3_cos_theta_2[i] = cos((i % 3) * (-0.14) * DEG_TO_RAD);
+                    } else {
+                        cx126s3_sin_theta_2[i] = sin(this->prism_angle[i % 3] * DEG_TO_RAD);
+                        cx126s3_cos_theta_2[i] = cos(this->prism_angle[i % 3] * DEG_TO_RAD);
+                    }
+
+                    // 左边
+                    if (i / 3 % 2 == 0) {
+                        sin_theta_1[i] = sin((big_angle_cx126s3[i / 3] + prism_offset) * DEG_TO_RAD);
+                        cos_theta_1[i] = cos((big_angle_cx126s3[i / 3] + prism_offset) * DEG_TO_RAD);
+
+                    } else {
+                        sin_theta_1[i] = sin(big_angle_cx126s3[i / 3] * DEG_TO_RAD);
+                        cos_theta_1[i] = cos(big_angle_cx126s3[i / 3] * DEG_TO_RAD);
+                    }
+                }
+            } else if (lidar_type == "cx6s3") {
+                for (int i = 0; i < 6; i++) {
+                    // sinTheta_2[i] = sin((i % 4) * (-0.17) * M_PI / 180);
+                    if (fabs(this->prism_angle[0]) < 1e-6 && fabs(this->prism_angle[1]) < 1e-6 &&
+                        fabs(this->prism_angle[2]) < 1e-6) {
+                        cx6s3_sin_theta_2[i] = sin((i % 3) * (-0.14) * DEG_TO_RAD);
+                        cx6s3_cos_theta_2[i] = cos((i % 3) * (-0.14) * DEG_TO_RAD);
+                    } else {
+                        cx6s3_sin_theta_2[i] = sin(this->prism_angle[i % 3] * DEG_TO_RAD);
+                        cx6s3_cos_theta_2[i] = cos(this->prism_angle[i % 3] * DEG_TO_RAD);
+                    }
+
+                    // 左边
+                    if (i / 3 % 2 == 0) {
+                        sin_theta_1[i] = sin((big_angle_cx6s3[i / 3] + prism_offset) * DEG_TO_RAD);
+                        cos_theta_1[i] = cos((big_angle_cx6s3[i / 3] + prism_offset) * DEG_TO_RAD);
+
+                    } else {
+                        sin_theta_1[i] = sin(big_angle_cx6s3[i / 3] * DEG_TO_RAD);
+                        cos_theta_1[i] = cos(big_angle_cx6s3[i / 3] * DEG_TO_RAD);
+                    }
+                }
+            } else if (lidar_type == "ch16x1") {
+                for (int k = 0; k < 16; ++k) {
+                    if (fabs(this->prism_angle[0]) < 1e-6 && fabs(this->prism_angle[1]) < 1e-6 &&
+                        fabs(this->prism_angle[2]) < 1e-6 &&
+                        fabs(this->prism_angle[3]) < 1e-6) {
+                        ch16x1_sin_theta_2[k] = sin((k % 4) * (-0.17) * M_PI / 180);
+                        ch16x1_cos_theta_2[k] = cos((k % 4) * (-0.17) * M_PI / 180);
+                    } else {
+                        ch16x1_sin_theta_2[k] = sin(this->prism_angle[k % 4] * M_PI / 180);
+                        ch16x1_cos_theta_2[k] = cos(this->prism_angle[k % 4] * M_PI / 180);
+                    }
+                    // 左边
+                    if (k / 4 % 2 == 0) {
+                        ch16x1_sin_theta_1[k] = sin(
+                            (big_angle_ch16x1[k / 4] + prism_offset) * DEG_TO_RAD);
+                        ch16x1_cos_theta_1[k] = cos(
+                            (big_angle_ch16x1[k / 4] + prism_offset) * DEG_TO_RAD);
+                    } else {
+                        ch16x1_sin_theta_1[k] = sin(big_angle_ch16x1[k / 4] * DEG_TO_RAD);
+                        ch16x1_cos_theta_1[k] = cos(big_angle_ch16x1[k / 4] * DEG_TO_RAD);
+                    }
+                    ch16x1_sin_theta_2[k] = sin((k % 4) * (-0.17) * DEG_TO_RAD);
+                    ch16x1_cos_theta_2[k] = cos((k % 4) * (-0.17) * DEG_TO_RAD);
+                }
+
+            } else if (lidar_type == "ch64w") {
+                for (int m = 0; m < 128; ++m) {
+                    if (fabs(this->prism_angle[0]) < 1e-6 && fabs(this->prism_angle[1]) < 1e-6 &&
+                        fabs(this->prism_angle[2]) < 1e-6 &&
+                        fabs(this->prism_angle[3]) < 1e-6) {
+                        //右边
+                        if (m / 4 % 2 == 0) {
+                            ch64w_sin_theta_1[m] = sin((-25 + floor(m / 8) * 2.5) * DEG_TO_RAD);
+                            ch64w_sin_theta_2[m] = sin((m % 4) * 0.35 * DEG_TO_RAD);
+                            ch64w_cos_theta_1[m] = cos((-25 + floor(m / 8) * 2.5) * DEG_TO_RAD);
+                            ch64w_cos_theta_2[m] = cos((m % 4) * 0.35 * DEG_TO_RAD);
+                        } else { //左边
+                            ch64w_sin_theta_1[m] = sin(
+                                (-24 + prism_offset + floor(m / 8) * 2.5) * DEG_TO_RAD);
+                            ch64w_sin_theta_2[m] = sin((m % 4) * 0.35 * DEG_TO_RAD);
+                            ch64w_cos_theta_1[m] = cos(
+                                (-24 + prism_offset + floor(m / 8) * 2.5) * DEG_TO_RAD);
+                            ch64w_cos_theta_2[m] = cos((m % 4) * 0.35 * DEG_TO_RAD);
+                        }
+
+                    } else {
+                        //右边
+                        if (m / 4 % 2 == 0) {
+                            ch64w_sin_theta_1[m] = sin((-25 + floor(m / 8) * 2.5) * DEG_TO_RAD);
+                            ch64w_sin_theta_2[m] = sin(prism_angle[m % 4] * DEG_TO_RAD);
+                            ch64w_cos_theta_1[m] = cos((-25 + floor(m / 8) * 2.5) * DEG_TO_RAD);
+                            ch64w_cos_theta_2[m] = cos(prism_angle[m % 4] * DEG_TO_RAD);
+                        } else { //左边
+                            ch64w_sin_theta_1[m] = sin(
+                                (-24 + prism_offset + floor(m / 8) * 2.5) * DEG_TO_RAD);
+                            ch64w_sin_theta_2[m] = sin(prism_angle[m % 4] * DEG_TO_RAD);
+                            ch64w_cos_theta_1[m] = cos(
+                                (-24 + prism_offset + floor(m / 8) * 2.5) * DEG_TO_RAD);
+                            ch64w_cos_theta_2[m] = cos(prism_angle[m % 4] * DEG_TO_RAD);
+                        }
+
+                    }
+
+                }
+
+            } else if (lidar_type == "cb64s1_a") {
+                for (int m = 0; m < 64; ++m) {
+                    if (fabs(this->prism_angle[0]) < 1e-6 && fabs(this->prism_angle[1]) < 1e-6 &&
+                        fabs(this->prism_angle[2]) < 1e-6 &&
+                        fabs(this->prism_angle[3]) < 1e-6) {
+                        //右边
+                        // if (m / 4 % 2 == 0)
+                        {
+                            cb64s1_A_sin_theta_1[m] = sin((-25 + floor(m / 4) * 2.5) * DEG_TO_RAD);
+                            cb64s1_A_sin_theta_2[m] = sin((m % 4) * 0.35 * DEG_TO_RAD);
+                            cb64s1_A_cos_theta_1[m] = cos((-25 + floor(m / 4) * 2.5) * DEG_TO_RAD);
+                            cb64s1_A_cos_theta_2[m] = cos((m % 4) * 0.35 * DEG_TO_RAD);
+                        }
+
+                    } else {
+                        //右边
+                        //if (m / 4 % 2 == 0)
+                        {
+                            cb64s1_A_sin_theta_1[m] = sin((-25 + floor(m / 4) * 2.5) * DEG_TO_RAD);
+                            cb64s1_A_sin_theta_2[m] = sin(prism_angle[m % 4] * DEG_TO_RAD);
+                            cb64s1_A_cos_theta_1[m] = cos((-25 + floor(m / 4) * 2.5) * DEG_TO_RAD);
+                            cb64s1_A_cos_theta_2[m] = cos(prism_angle[m % 4] * DEG_TO_RAD);
+                        }
+
+                    }
+
+                }
+
+            }
+
+            gain_prism_angle = false;
+        }
+        if ("ch128x1" == lidar_type || "ch64w" == lidar_type || "cb64s1_a" == lidar_type) {
+            if (pdata[176] == 0x00) {
+                this->packetTimeStamp[7] = pdata[54];
+                this->packetTimeStamp[8] = pdata[53];
+                this->packetTimeStamp[9] = pdata[52];
+            } else if (pdata[176] == 0x01) {
+                this->packetTimeStamp[5] = pdata[56];
+                this->packetTimeStamp[6] = pdata[55];
+                this->packetTimeStamp[7] = pdata[54];
+                this->packetTimeStamp[8] = pdata[53];
+                this->packetTimeStamp[9] = pdata[52];
+            }
+
+        }
+
+    }
+    return 0;
+}
+
+int leishenchx::DecodeUDPPac(iv::lidarudppac & xpac)
+{
+    unsigned char * pdata =  (unsigned char *)xpac.mdata_ptr.get();
+
+    if (use_time_service) {
+        if ("ch128x1" == lidar_type || "ch64w" == lidar_type || "cb64s1_a" == lidar_type) {
+            if (pdata[1205] == 0x01) {
+                this->packetTimeStamp[4] = pdata[1199];
+                this->packetTimeStamp[5] = pdata[1198];
+                this->packetTimeStamp[6] = pdata[1197];
+            } else if (pdata[1205] == 0x02) {
+                this->packetTimeStamp[4] = pdata[1199];
+            }
+
+            //struct tm cur_time{};
+            memset(&cur_time, 0, sizeof(cur_time));
+            cur_time.tm_sec = this->packetTimeStamp[4];
+            cur_time.tm_min = this->packetTimeStamp[5];
+            cur_time.tm_hour = this->packetTimeStamp[6];
+            cur_time.tm_mday = this->packetTimeStamp[7];
+            cur_time.tm_mon = this->packetTimeStamp[8] - 1;
+            cur_time.tm_year = this->packetTimeStamp[9] + 2000 - 1900;
+
+            packet_timestamp_s = timegm(&cur_time);
+            if (time_service_mode == "gps") {
+                packet_timestamp_ns = (pdata[1203] +
+                                       (pdata[1202] << 8) +
+                                       (pdata[1201] << 16) +
+                                       (pdata[1200] << 24)) * 1e3; //ns
+            } else {
+                packet_timestamp_ns = pdata[1203] +
+                                      (pdata[1202] << 8) +
+                                      (pdata[1201] << 16) +
+                                      (pdata[1200] << 24); //ns
+            }
+            packet_timestamp = packet_timestamp_s + packet_timestamp_ns * 1e-9;
+            // ch128x1_packet->timestamp = packet_timestamp;
+        } else {
+            if (pdata[1200] == 0xff) {
+                packet_timestamp_s = pdata[1205] +
+                                     (pdata[1204] << 8) +
+                                     (pdata[1203] << 16) +
+                                     (pdata[1202] << 24);
+
+            } else {
+                // struct tm cur_time{};
+                memset(&cur_time, 0, sizeof(cur_time));
+                cur_time.tm_sec = pdata[1205];
+                cur_time.tm_min = pdata[1204];
+                cur_time.tm_hour = pdata[1203];
+                cur_time.tm_mday = pdata[1202];
+                cur_time.tm_mon = pdata[1201] - 1;
+                cur_time.tm_year = pdata[1200] + 2000 - 1900;
+                packet_timestamp_s = timegm(&cur_time);
+            }
+            packet_timestamp_ns = pdata[1209] +
+                                  (pdata[1208] << 8) +
+                                  (pdata[1207] << 16) +
+                                  (pdata[1206] << 24); //ns
+            packet_timestamp = packet_timestamp_s + packet_timestamp_ns * 1e-9;
+            // packet->timestamp = packet_timestamp;
+        }
+        // it is already the msop msg
+
+    } else {
+        int64_t timenow = std::chrono::system_clock::now().time_since_epoch().count();
+        double ftime = timenow;
+        ftime = ftime/1000000000.0;
+        packet_timestamp = ftime; //ros::Time::now().toSec();
+    }
+
+    struct Firing lidardata{};
+
+
+    packet_interval_time = packet_timestamp - last_packet_timestamp;
+    last_packet_timestamp = packet_timestamp;
+
+    bool packetType = false;
+    // Decode the packet
+    if ("ch128x1" == lidar_type || "ch64w" == lidar_type || "cb64s1_a" == lidar_type) {
+        if (pdata[1205] == 0x01) {
+            for (size_t point_idx = 0; point_idx < 1197; point_idx += 7) {
+                if ((pdata[point_idx] == 0xff) && (pdata[point_idx + 1] == 0xaa) &&
+                    (pdata[point_idx + 2] == 0xbb)) {
+                    packetType = true;
+                    point_cloud_timestamp =
+                        packet_timestamp - packet_interval_time + point_idx * packet_interval_time / 1197.0;
+                }
+
+
+                if (pdata[point_idx] < 128) {
+                    memset(&lidardata, 0, sizeof(lidardata));
+                    lidardata.distance =
+                        ((pdata[point_idx + 3] << 16) + (pdata[point_idx + 4] << 8) +
+                         pdata[point_idx + 5]) * DISTANCE_RESOLUTION;
+                    lidardata.azimuth = (pdata[point_idx + 1] << 8) + pdata[point_idx + 2];
+                    if (!isPointInRange(lidardata.distance) || lidardata.azimuth >18000) {continue; }
+                    // if (!isPointInRange(lidardata.distance)) { continue; }
+                    // Compute the time of the point
+                    point_time = packet_timestamp - packet_interval_time +
+                                 ((int) point_idx - 7) * packet_interval_time / (1197 * 1.0) -
+                                 point_cloud_timestamp;
+
+                    lidardata.vertical_line = pdata[point_idx];
+                    lidardata.intensity = pdata[point_idx + 6];
+                    lidardata.time = point_time;
+                    convertCoordinate(lidardata);
+                }
+
+            }
+        } else if (pdata[1205] == 0x02) {
+            std::cout<<"lidar is double echo model"<<std::endl;
+//            ROS_INFO_ONCE(
+//                "lidar is double echo model,and the selected echo is: %d [0 mean double echo; 1 mean first echo; 2 mean second echo]",
+//                echo_num);
+            for (size_t point_idx = 0; point_idx < 1199; point_idx += 11) {
+                if ((pdata[point_idx] == 0xff) && (pdata[point_idx + 1] == 0xaa) &&
+                    (pdata[point_idx + 2] == 0xbb)) {
+                    packetType = true;
+                    point_cloud_timestamp =
+                        packet_timestamp - packet_interval_time + point_idx * packet_interval_time / 1199.0;
+
+                }
+
+
+                if (pdata[point_idx] < 128) {
+                    // Compute the time of the point
+
+                    point_time = packet_timestamp - packet_interval_time +
+                                 ((int) point_idx - 11) * packet_interval_time / (1199 * 1.0) -
+                                 point_cloud_timestamp;
+                    if (echo_num == 0) {
+                        memset(&lidardata, 0, sizeof(lidardata));
+                        lidardata.azimuth =
+                            (pdata[point_idx + 1] << 8) + pdata[point_idx + 2];
+
+                        lidardata.distance =
+                            ((pdata[point_idx + 3] << 16) + (pdata[point_idx + 4] << 8) +
+                             pdata[point_idx + 5]) * DISTANCE_RESOLUTION;
+                        if (!isPointInRange(lidardata.distance) || lidardata.azimuth >18000) {continue; }
+                        lidardata.vertical_line = pdata[point_idx];
+
+                        lidardata.intensity = pdata[point_idx + 6];
+                        lidardata.time = point_time;
+                        convertCoordinate(lidardata);
+
+                        memset(&lidardata, 0, sizeof(lidardata));
+                        lidardata.vertical_line = pdata[point_idx];
+                        lidardata.azimuth =
+                            (pdata[point_idx + 1] << 8) + pdata[point_idx + 2];
+                        lidardata.distance =
+                            ((pdata[point_idx + 7] << 16) + (pdata[point_idx + 8] << 8) +
+                             pdata[point_idx + 9]) * DISTANCE_RESOLUTION;
+                        lidardata.intensity = pdata[point_idx + 10];
+                        lidardata.time = point_time;
+                        convertCoordinate(lidardata);
+                    } else if (echo_num == 1) {
+                        memset(&lidardata, 0, sizeof(lidardata));
+                        lidardata.azimuth =
+                            (pdata[point_idx + 1] << 8) + pdata[point_idx + 2];
+                        lidardata.distance =
+                            ((pdata[point_idx + 3] << 16) + (pdata[point_idx + 4] << 8) +
+                             pdata[point_idx + 5]) * DISTANCE_RESOLUTION;
+                        if (!isPointInRange(lidardata.distance) || lidardata.azimuth >18000) {continue; }
+                        lidardata.vertical_line = pdata[point_idx];
+
+                        lidardata.intensity = pdata[point_idx + 6];
+                        lidardata.time = point_time;
+                        convertCoordinate(lidardata);
+
+                    } else if (echo_num == 2) {
+                        memset(&lidardata, 0, sizeof(lidardata));
+                        lidardata.azimuth =
+                            (pdata[point_idx + 1] << 8) + pdata[point_idx + 2];
+                        lidardata.distance =
+                            ((pdata[point_idx + 7] << 16) + (pdata[point_idx + 8] << 8) +
+                             pdata[point_idx + 9]) * DISTANCE_RESOLUTION;
+                        if (!isPointInRange(lidardata.distance) || lidardata.azimuth >18000) {continue; }
+                        lidardata.vertical_line = pdata[point_idx];
+                        lidardata.azimuth =
+                            (pdata[point_idx + 1] << 8) + pdata[point_idx + 2];
+                        lidardata.distance =
+                            ((pdata[point_idx + 7] << 16) + (pdata[point_idx + 8] << 8) +
+                             pdata[point_idx + 9]) * DISTANCE_RESOLUTION;
+                        lidardata.intensity = pdata[point_idx + 10];
+                        lidardata.time = point_time;
+                        convertCoordinate(lidardata);
+                    }
+
+                }
+
+            }
+        }
+
+    } else {
+        if (pdata[1211] == 0x01) {
+            for (size_t point_idx = 0; point_idx < 1197; point_idx += 7) {
+                if ((pdata[point_idx] == 0xff) && (pdata[point_idx + 1] == 0xaa) &&
+                    (pdata[point_idx + 2] == 0xbb)) {
+                    packetType = true;
+                    point_cloud_timestamp =
+                        packet_timestamp - packet_interval_time + point_idx * packet_interval_time / 1197.0;
+
+                } else {
+                    memset(&lidardata, 0, sizeof(lidardata));
+                    lidardata.azimuth = (pdata[point_idx + 1] << 8) + pdata[point_idx + 2];
+                    lidardata.distance =
+                        ((pdata[point_idx + 3] << 16) + (pdata[point_idx + 4] << 8) +
+                         pdata[point_idx + 5]) * DISTANCE_RESOLUTION;
+                    if (!isPointInRange(lidardata.distance) || lidardata.azimuth >18000) {continue; }
+                    // Compute the time of the point
+                    point_time = packet_timestamp - packet_interval_time +
+                                 ((int) point_idx - 7) * packet_interval_time / (1197 * 1.0) -
+                                 point_cloud_timestamp;
+
+                    lidardata.vertical_line = pdata[point_idx];
+
+                    lidardata.intensity = pdata[point_idx + 6];
+                    lidardata.time = point_time;
+                    convertCoordinate(lidardata);
+                }
+
+            }
+        } else if (pdata[1211] == 0x02) {
+            for (size_t point_idx = 0; point_idx < 1199; point_idx += 11) {
+                if ((pdata[point_idx] == 0xff) && (pdata[point_idx + 1] == 0xaa) &&
+                    (pdata[point_idx + 2] == 0xbb)) {
+                    packetType = true;
+                    point_cloud_timestamp =
+                        packet_timestamp - packet_interval_time + point_idx * packet_interval_time / 1199.0;
+
+                } else {
+                    // Compute the time of the point
+                    point_time = packet_timestamp - packet_interval_time +
+                                 ((int) point_idx - 11) * packet_interval_time / (1199 * 1.0) -
+                                 point_cloud_timestamp;
+
+                    if (echo_num == 0) {
+                        memset(&lidardata, 0, sizeof(lidardata));
+                        lidardata.azimuth =
+                            (pdata[point_idx + 1] << 8) + pdata[point_idx + 2];
+
+                        lidardata.distance =
+                            ((pdata[point_idx + 3] << 16) + (pdata[point_idx + 4] << 8) +
+                             pdata[point_idx + 5]) * DISTANCE_RESOLUTION;
+                        if (!isPointInRange(lidardata.distance) || lidardata.azimuth >18000) {continue; }
+                        lidardata.vertical_line = pdata[point_idx];
+
+                        lidardata.intensity = pdata[point_idx + 6];
+                        lidardata.time = point_time;
+                        convertCoordinate(lidardata);
+
+                        memset(&lidardata, 0, sizeof(lidardata));
+                        lidardata.vertical_line = pdata[point_idx];
+                        lidardata.azimuth =
+                            (pdata[point_idx + 1] << 8) + pdata[point_idx + 2];
+                        lidardata.distance =
+                            ((pdata[point_idx + 7] << 16) + (pdata[point_idx + 8] << 8) +
+                             pdata[point_idx + 9]) * DISTANCE_RESOLUTION;
+                        lidardata.intensity = pdata[point_idx + 10];
+                        lidardata.time = point_time;
+                        convertCoordinate(lidardata);
+                    } else if (echo_num == 1) {
+                        memset(&lidardata, 0, sizeof(lidardata));
+                        lidardata.azimuth =
+                            (pdata[point_idx + 1] << 8) + pdata[point_idx + 2];
+
+                        lidardata.distance =
+                            ((pdata[point_idx + 3] << 16) + (pdata[point_idx + 4] << 8) +
+                             pdata[point_idx + 5]) * DISTANCE_RESOLUTION;
+                        if (!isPointInRange(lidardata.distance) || lidardata.azimuth >18000) {continue; }
+                        lidardata.vertical_line = pdata[point_idx];
+
+                        lidardata.intensity = pdata[point_idx + 6];
+                        lidardata.time = point_time;
+                        convertCoordinate(lidardata);
+
+                    } else if (echo_num == 2) {
+                        memset(&lidardata, 0, sizeof(lidardata));
+                        lidardata.azimuth =
+                            (pdata[point_idx + 1] << 8) + pdata[point_idx + 2];
+                        lidardata.distance =
+                            ((pdata[point_idx + 7] << 16) + (pdata[point_idx + 8] << 8) +
+                             pdata[point_idx + 9]) * DISTANCE_RESOLUTION;
+                        if (!isPointInRange(lidardata.distance) || lidardata.azimuth >18000) {continue; }
+                        lidardata.vertical_line = pdata[point_idx];
+                        lidardata.intensity = pdata[point_idx + 10];
+                        lidardata.time = point_time;
+                        convertCoordinate(lidardata);
+                    }
+
+                    //---------------
+
+                }
+
+            }
+        }
+
+    }
+
+    return 0;
+}
+
+int leishenchx::convertCoordinate(struct Firing &lidardata) {
+    /*        if (!isPointInRange(lidardata.distance) || lidardata.azimuth > 18000) {
+            return -1;
+        }*/
+    if ((lidardata.azimuth > angle_disable_min) && (lidardata.azimuth < angle_disable_max)) {
+        return -1;
+    }
+
+    double x = 0.0, y = 0.0, z = 0.0;
+    double sin_theat = 0.0;
+    double cos_theat = 0.0;
+    double add_distance = 0.0;
+    double _R_ = 0.0;
+    //ch64w
+    double cos_xita;
+    double sin_xita;
+    double cos_H_xita;
+    double sin_H_xita;
+    double cos_xita_F;
+    double sin_xita_F;
+    if (lidar_type == "ch128x1" || lidar_type == "ch128s1" || lidar_type == "cx128s2") {
+        _R_ = cos_theta_2[lidardata.vertical_line] * cos_theta_1[lidardata.vertical_line] *
+                  cos_list[int(lidardata.azimuth * 0.5)] -
+              sin_theta_2[lidardata.vertical_line] * sin_theta_1[lidardata.vertical_line];
+
+        sin_theat = sin_theta_1[lidardata.vertical_line] + 2 * _R_ * sin_theta_2[lidardata.vertical_line];
+        cos_theat = sqrt(1 - pow(sin_theat, 2));
+        x = lidardata.distance * cos_theat * cos_list[lidardata.azimuth];
+        y = lidardata.distance * cos_theat * sin_list[lidardata.azimuth];
+        z = lidardata.distance * sin_theat;
+    } else if (lidar_type == "cx126s3") {
+        _R_ = cx126s3_cos_theta_2[lidardata.vertical_line] * cx126s3_cos_theta_1[lidardata.vertical_line] *
+                  cos_list[int(lidardata.azimuth * 0.5)] -
+              cx126s3_sin_theta_2[lidardata.vertical_line] * cx126s3_sin_theta_1[lidardata.vertical_line];
+
+        sin_theat = cx126s3_sin_theta_1[lidardata.vertical_line] +
+                    2 * _R_ * cx126s3_sin_theta_2[lidardata.vertical_line];
+        cos_theat = sqrt(1 - pow(sin_theat, 2));
+        x = lidardata.distance * cos_theat * cos_list[lidardata.azimuth];
+        y = lidardata.distance * cos_theat * sin_list[lidardata.azimuth];
+        z = lidardata.distance * sin_theat;
+    } else if (lidar_type == "cx6s3") {
+        _R_ = cx6s3_cos_theta_2[lidardata.vertical_line] * cx6s3_cos_theta_1[lidardata.vertical_line] *
+                  cos_list[int(lidardata.azimuth * 0.5)] -
+              cx6s3_sin_theta_2[lidardata.vertical_line] * cx6s3_sin_theta_1[lidardata.vertical_line];
+
+        sin_theat = cx6s3_sin_theta_1[lidardata.vertical_line] +
+                    2 * _R_ * cx6s3_sin_theta_2[lidardata.vertical_line];
+        cos_theat = sqrt(1 - pow(sin_theat, 2));
+        x = lidardata.distance * cos_theat * cos_list[lidardata.azimuth];
+        y = lidardata.distance * cos_theat * sin_list[lidardata.azimuth];
+        z = lidardata.distance * sin_theat;
+    }else if(lidar_type =="cx1s3"){
+        x = lidardata.distance  * cos_list[lidardata.azimuth];
+        y = lidardata.distance  * sin_list[lidardata.azimuth];
+        z = 0;
+    }else if (lidar_type == "ch16x1") {
+        //中间变量
+        _R_ = ch16x1_cos_theta_2[lidardata.vertical_line] * ch16x1_cos_theta_1[lidardata.vertical_line] *
+                  cos_list[int(lidardata.azimuth * 0.5)] -
+              ch16x1_sin_theta_2[lidardata.vertical_line] * ch16x1_sin_theta_1[lidardata.vertical_line];
+
+        sin_theat =
+            ch16x1_sin_theta_1[lidardata.vertical_line] + 2 * _R_ * ch16x1_sin_theta_2[lidardata.vertical_line];
+        cos_theat = sqrt(1 - pow(sin_theat, 2));
+        x = lidardata.distance * cos_theat * cos_list[lidardata.azimuth];
+        y = lidardata.distance * cos_theat * sin_list[lidardata.azimuth];
+        z = lidardata.distance * sin_theat;
+    } else if (lidar_type == "ch64w") {
+        int line_num = lidardata.vertical_line;
+        if (line_num / 4 % 2 == 0) {
+            cos_xita = cos_list[int(lidardata.azimuth * 0.5 + 2250)];
+            sin_xita = sin_list[int(lidardata.azimuth * 0.5 + 2250)];
+        } else {
+            int angle_tmp =
+                int(11250 - lidardata.azimuth * 0.5) < 0 ? int(11250 - lidardata.azimuth * 0.5) + 36000 : int(
+                    11250 - lidardata.azimuth * 0.5);
+            cos_xita = cos_list[angle_tmp];
+            sin_xita = sin_list[angle_tmp];
+        }
+        _R_ = ch64w_cos_theta_2[line_num] * ch64w_cos_theta_1[line_num] * cos_xita -
+              ch64w_sin_theta_2[line_num] * ch64w_sin_theta_1[line_num];
+
+        sin_theat = ch64w_sin_theta_1[line_num] + 2 * _R_ * ch64w_sin_theta_2[line_num];
+        cos_theat = sqrt(1 - pow(sin_theat, 2));
+
+        cos_H_xita = (2 * _R_ * ch64w_cos_theta_2[line_num] * cos_xita - ch64w_cos_theta_1[line_num]) / cos_theat;
+        sin_H_xita = (2 * _R_ * ch64w_cos_theta_2[line_num] * sin_xita) / cos_theat;
+
+        if (line_num / 4 % 2 == 0) {
+            cos_xita_F = (cos_H_xita + sin_H_xita) * sqrt_0_5;
+            //sin_xita_F = sqrt(1 - pow(cos_xita_F, 2));
+            // sin_xita_F = (sin_H_xita - cos_H_xita) * sqrt(0.5);
+            if (cos_xita_F > 1.0) {
+                cos_xita_F = 1.0;
+            }
+            double xita_hangle = acos(cos_xita_F) * RAD_TO_DEG;
+            double xita_hangle_new = pow1 * pow(xita_hangle, 3)
+                                     + pow2 * pow(xita_hangle, 2)
+                                     + 0.9885 * pow(xita_hangle, 1)
+                                     + 0.5894;
+            while (xita_hangle_new < 0.0) {
+                xita_hangle_new += 360.0;
+            }
+            int xita_hangle_new_index = int(xita_hangle_new * 100) % 36000;
+            cos_xita_F = cos_list[xita_hangle_new_index];
+            sin_xita_F = sin_list[xita_hangle_new_index];
+            // cos_xita_F = cos(xita_hangle_new * DEG_TO_RAD);
+            // sin_xita_F = sin(xita_hangle_new * DEG_TO_RAD);
+            add_distance = 0.017;
+        } else {
+            cos_xita_F = (cos_H_xita + sin_H_xita) * (-sqrt_0_5);
+            //sin_xita_F = sqrt(1 - pow(cos_xita_F, 2));
+            // sin_xita_F = (sin_H_xita - cos_H_xita) * sqrt(0.5);
+            if (cos_xita_F < -1.0) {
+                cos_xita_F = -1.0;
+            }
+            double xita_hangle = acos(cos_xita_F) * RAD_TO_DEG;
+            double xita_hangle_new = pow1 * pow(xita_hangle, 3)
+                                     + pow3 * pow(xita_hangle, 2)
+                                     + 0.9719 * pow(xita_hangle, 1)
+                                     + 1.9003;
+            while (xita_hangle_new < 0.0) {
+                xita_hangle_new += 360.0;
+            }
+            int xita_hangle_new_index = int(xita_hangle_new * 100) % 36000;
+            cos_xita_F = cos_list[xita_hangle_new_index];
+            sin_xita_F = sin_list[xita_hangle_new_index];
+            //  cos_xita_F = cos(xita_hangle_new * DEG_TO_RAD);
+            //  sin_xita_F = sin(xita_hangle_new * DEG_TO_RAD);
+            add_distance = -0.017;
+        }
+
+        x = lidardata.distance * cos_theat * cos_xita_F + add_distance;
+        y = lidardata.distance * cos_theat * sin_xita_F;
+        z = lidardata.distance * sin_theat;
+    } else if (lidar_type == "cb64s1_a") {
+        int line_num = lidardata.vertical_line;
+
+        {
+            cos_xita = cos_list[int(lidardata.azimuth * 0.5)];
+            sin_xita = sin_list[int(lidardata.azimuth * 0.5)];
+        }
+
+        _R_ = cb64s1_A_cos_theta_2[line_num] * cb64s1_A_cos_theta_1[line_num] * cos_xita -
+              cb64s1_A_sin_theta_2[line_num] * cb64s1_A_sin_theta_1[line_num];
+
+        sin_theat = cb64s1_A_sin_theta_1[line_num] + 2 * _R_ * cb64s1_A_sin_theta_2[line_num];
+        cos_theat = sqrt(1 - pow(sin_theat, 2));
+
+        x = lidardata.distance * cos_theat * cos_list[int(lidardata.azimuth)];
+        y = lidardata.distance * cos_theat * sin_list[int(lidardata.azimuth)];
+        z = lidardata.distance * sin_theat;
+    } else if (lidar_type == "ch256") {
+        _R_ = ch256_cos_theta_2[lidardata.vertical_line] * ch256_cos_theta_1[lidardata.vertical_line] *
+                  cos_list[int(lidardata.azimuth * 0.5)] -
+              ch256_sin_theta_2[lidardata.vertical_line] * ch256_sin_theta_1[lidardata.vertical_line];
+
+        sin_theat = ch256_sin_theta_1[lidardata.vertical_line] +
+                    2 * _R_ * ch256_sin_theta_2[lidardata.vertical_line];
+        cos_theat = sqrt(1 - pow(sin_theat, 2));
+        x = lidardata.distance * cos_theat * cos_list[lidardata.azimuth];
+        y = lidardata.distance * cos_theat * sin_list[lidardata.azimuth];
+        z = lidardata.distance * sin_theat;
+    } else {
+        std::cout<<"lidar type error,please reset param in xml file "<<std::endl;
+//        ROS_WARN_ONCE("lidar type error,please reset param in launch file");
+    }
+    //add point
+    if (pcl_type) {
+        pcl::PointXYZI point_xyzi;
+        point_xyzi.x = x;
+        point_xyzi.y = y;
+        point_xyzi.z = z;
+        point_xyzi.intensity = lidardata.intensity;
+        point_cloud_xyzi_->points.push_back(point_xyzi);
+        ++point_cloud_xyzi_->width;
+    } else {
+
+    }
+
+    return 0;
+}

src/driver/driver_lidar_leishen_chx/leishenchx.h

@@ -0,0 +1,245 @@
+#ifndef LEISHENCHX_H
+#define LEISHENCHX_H
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include <vector>
+#include <mutex>
+#include <condition_variable>
+#include <thread>
+
+#include "lidarudp.h"
+
+#include "lidardecode.h"
+
+#define DEG_TO_RAD 0.017453292
+#define RAD_TO_DEG 57.29577951
+
+static const double DISTANCE_RESOLUTION = 0.0000390625; /**< meters */
+static const double sqrt_0_5 = sqrt(0.5);
+static const double pow1 = -3.6636 * pow(10, -7);
+static const double pow2 = 5.2766 * pow(10, -5);
+static const double pow3 = 1.4507 * pow(10, -4);
+static const double big_angle[32]={-17,-16,-15,-14,-13,-12,-11,-10,
+    -9,-8,-7,-6,-5,-4.125,-4,-3.125,
+    -3,-2.125,-2,-1.125,-1,-0.125,0,0.875,
+    1,1.875,2,3,4,5,6,7
+};
+static const double big_angle_ch128s1[32] = {
+    -12, -11, -10, -9, -8, -7, -6, -5,
+    -4.125, -4, -3.125, -3, -2.125, -2, -1.125, -1,
+    -0.125, 0, 0.875, 1, 1.875, 2, 3, 4,
+    5, 6, 7, 8, 9, 10, 11, 12
+};
+static const double big_angle_cx126s3[42] = {
+    -12.0,-11.4,-10.8,-10.2,-9.6,-9.0,-8.4,-7.8,-7.2,-6.6,-6.0,-5.4,-4.8,-4.2,
+    -3.6,-3.0,-2.4,-1.8,-1.2,-0.6,0.0,0.6,1.2,1.8,2.4,3.0,3.6,4.2,4.8,5.4,6.0,
+    6.6,7.2,7.8,8.4,9.0,9.6,10.2,10.8,11.4,12.0,12.6
+};
+static const double big_angle_cx6s3[2] = {-0.6,0.0};
+
+//static const double big_angle_ch16x1[16] = {-1.0,-0.75,-0.50,-0.25,0.0,0.25,0.50,0.75,1.0,1.25,1.50,1.75,2.0,2.25,2.50,2.75};
+static const double big_angle_ch16x1[4] = {-1.0,0.0,1.0,2.0};
+
+
+/*    static const double scan_mirror_altitude[4] = {
+            -0.0,
+            0.005759586531581287,
+            0.011693705988362009,
+            0.017453292519943295,
+    };
+
+    static const double sin_scan_mirror_altitude[4] = {
+            std::sin(scan_mirror_altitude[0]), std::sin(scan_mirror_altitude[1]),
+            std::sin(scan_mirror_altitude[2]), std::sin(scan_mirror_altitude[3]),
+    };*/
+struct PointXYZIRT {
+    PCL_ADD_POINT4D
+        float intensity;
+    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
+
+
+class leishenchx : public lidardecode
+{
+public:
+    leishenchx(char * strmemname, double froll = 0.0,double finclinationang_xaxis = 0.0,
+               double finclinationang_yaxis = 0.0, int nDevMode = 0,
+               unsigned short nDataPort = 2368,unsigned short nDevPort = 2369);
+
+    ~leishenchx();
+
+private:
+    struct Firing {
+        //double vertical_angle;
+        int vertical_line;
+        int azimuth;
+        double distance;
+        float intensity;
+        float time;
+    };
+
+private:
+    bool initialize();
+
+    int convertCoordinate(struct Firing &lidardata);
+
+    bool isPointInRange(const double& distance) {
+        return (distance >= min_range && distance <= max_range);
+    }
+
+private:
+    pcl::PointCloud<pcl::PointXYZI>::Ptr mpoint_cloud;
+    pcl::PointCloud<pcl::PointXYZI>::Ptr mpoint_cloud_temp;
+
+ //   iv::leishenc16_devinfo mdevinfo;
+    bool mbdevinfo = false;
+
+    const double * mpleishenc16_vertical;
+    double  mpleishenc16_A[16];
+
+    double mthetacos[16];
+    double mthetasin[16];
+
+    std::thread * mpthread;
+    std::thread * mpthreaddev;
+    lidarudp * mplidarudp;
+    lidarudp * mplidardevudp;
+
+    bool mbrun = true;
+
+    int mnLidarMode = 0; //0 1degree   1 0.33degree
+
+    double mfinclinationang_xaxis = 0.0;
+    double mfinclinationang_yaxis = 0.0;
+    double mfrollang = 0.0;
+
+private:
+    void threaddecode( );
+    void threaddevdecode();
+    int DecodeUDPPac(iv::lidarudppac & xpac);
+    int DecodeDevinfo(iv::lidarudppac & xpac);
+
+private:
+private:
+
+
+    bool loadParameters();
+
+    bool createRosIO();
+
+    //socket Parameters
+    int msop_udp_port;
+    int difop_udp_port;
+
+    // Converter convtor_
+    std::shared_ptr<std::thread> difop_thread_;
+
+    // Ethernet relate variables
+    std::string lidar_ip_string;
+    std::string group_ip_string;
+    std::string frame_id;
+    std::string pointcloud_topic;
+    std::string lidar_type;
+
+
+    int socket_id;
+
+    bool add_multicast;
+    bool pcl_type;
+    std::string dump_file;
+    double prism_angle[4];
+    double sin_list[36000]{};
+    double cos_list[36000]{};
+    pcl::PointCloud<PointXYZIRT>::Ptr point_cloud_xyzirt_;
+    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud_xyzi_;
+    pcl::PointCloud<PointXYZIRT>::Ptr point_cloud_xyzirt_bak_;
+    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud_xyzi_bak_;
+    uint point_size;
+    double prism_offset;
+    double min_range;
+    double max_range;
+    double angle_disable_min;
+    double angle_disable_max;
+    int channel_num;
+    int channel_num1;
+    int channel_num2;
+    int echo_num;
+
+    double horizontal_angle_resolution;
+
+
+
+    // lslidar_msgs::LslidarChScanPtr sweep_data;
+    // lslidar_msgs::LslidarChScanPtr sweep_data_bac;
+
+
+    unsigned char difop_data[1206] = {0};
+    // add for time synchronization
+    bool use_time_service;
+    std::string time_service_mode;
+
+    bool publish_laserscan;
+    bool gain_prism_angle;
+    bool is_update_difop_packet;
+
+
+    uint32_t packet_timestamp_s;
+    uint32_t packet_timestamp_ns;
+    double packet_timestamp;
+    double last_packet_timestamp;
+    double point_cloud_timestamp;
+    double point_time;
+    double packet_rate;
+
+    std::mutex pointcloud_lock;
+
+    unsigned char packetTimeStamp[10];
+    struct tm cur_time;
+
+    //   ros::Time timeStamp;
+
+    //   ros::Time packet_timeStamp;
+
+    double packet_interval_time;
+    //   Firing firings[171];
+
+    double sin_theta_1[128];
+    double sin_theta_2[128];
+    double cos_theta_1[128];
+    double cos_theta_2[128];
+    double ch256_sin_theta_1[256];
+    double ch256_sin_theta_2[256];
+    double ch256_cos_theta_1[256];
+    double ch256_cos_theta_2[256];
+
+    double ch64w_sin_theta_1[128];
+    double ch64w_sin_theta_2[128];
+    double ch64w_cos_theta_1[128];
+    double ch64w_cos_theta_2[128];
+
+    double cb64s1_A_sin_theta_1[128];
+    double cb64s1_A_sin_theta_2[128];
+    double cb64s1_A_cos_theta_1[128];
+    double cb64s1_A_cos_theta_2[128];
+
+    double ch16x1_sin_theta_1[16];
+    double ch16x1_sin_theta_2[16];
+    double ch16x1_cos_theta_1[16];
+    double ch16x1_cos_theta_2[16];
+
+    double cx126s3_sin_theta_1[126];
+    double cx126s3_sin_theta_2[126];
+    double cx126s3_cos_theta_1[126];
+    double cx126s3_cos_theta_2[126];
+    double cx6s3_sin_theta_1[6];
+    double cx6s3_sin_theta_2[6];
+    double cx6s3_cos_theta_1[6];
+    double cx6s3_cos_theta_2[6];
+};
+
+#endif // LEISHENCHX_H

src/driver/driver_lidar_leishen_chx/main.cpp

@@ -0,0 +1,8 @@
+#include <QCoreApplication>
+
+int main(int argc, char *argv[])
+{
+    QCoreApplication a(argc, argv);
+
+    return a.exec();
+}