change driver_lidar_leishen32. code complete, but not test.

src/common/modulecomm/shm/procsm_if.cpp

@@ -198,6 +198,7 @@ void procsm_if_readthread::run()
 }
 
 
+mmutex.unlock();
 #ifdef USE_GROUPUDP
     void procsm_if_readthread::WakeRead()
     {
@@ -285,6 +286,7 @@ int procsm_if::writemsg(const char *str, const unsigned int nSize)
 #ifdef USELCM
     int nres = mlcm.publish(mstrsmname,str,nSize);
     qDebug("publish message. res = %d",nres);
+    mmutex.unlock();
     return 0;
 #endif
     if(mnType == procsm::ModeRead)return -1; //this is listen.

src/common/ndt_cpu/ndt_cpu.pro

@@ -24,6 +24,8 @@ unix:INCLUDEPATH += /usr/include/eigen3
 unix:INCLUDEPATH += /usr/include/pcl-1.8
 unix:INCLUDEPATH += /usr/include/pcl-1.7
 
+unix:INCLUDEPATH += /usr/include/pcl-1.12
+
 
 #PKGCONFIG +=pcl
 

src/driver/driver_lidar_leishen32/driver_lidar_leishen32.pro

@@ -19,11 +19,13 @@ DEFINES += QT_DEPRECATED_WARNINGS
 
 SOURCES += main.cpp \
     lidarudp.cpp \
-    leishen32.cpp
+    leishen32.cpp \
+    lidardecode.cpp
 
 HEADERS += \
     lidarudp.h \
-    leishen32.h
+    leishen32.h \
+    lidardecode.h
 
 !include(../../../include/common.pri ) {
     error( "Couldn't find the common.pri file!" )
@@ -32,3 +34,7 @@ HEADERS += \
 !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!" )
+}

src/driver/driver_lidar_leishen32/leishen32.cpp

@@ -2,30 +2,74 @@
 
 #include "ivstdcolorout.h"
 
-static double leishen32_vertical_1[32]={-16,0,-15,1,-14,2,-13,3,
-                                -12,4,-11,5,-10,6,-9,7,
-                                0,1,2,3,4,5,6,7,
-                                8,9,10,11,12,13,14,15};
 
-static double leishen32_vertical_033[32]={-18,-1,-15,-0.66,-12,-0.33,-10,0,
-                                          -8,0.33,-7,0.66,-6,1,-5,1.33,
-                                          -4,1.66,-3.33,2,-3,3,-2.66,4,
-                                          -2.33,6,-2,8,-1.66,11,-1.33,14};
-
-leishen32::leishen32()
+leishen32::leishen32(char * strmemname,double froll,double finclinationang_xaxis ,
+                     double finclinationang_yaxis , int nDevMode ,
+                     unsigned short nDataPort,unsigned short nDevPort )
 {
-    mpleishen32_vertical = &leishen32_vertical_1[0];
+    RegisterSend(strmemname);
+    mnLidarMode = nDevMode;
+    mfrollang = froll;
+    mfinclinationang_xaxis = finclinationang_xaxis;
+    mfinclinationang_yaxis = finclinationang_yaxis;
+    if(mnLidarMode == 0)
+        mpleishen32_vertical = &leishen32_vertical_1[0];
+    else
+        mpleishen32_vertical = &leishen32_vertical_033[0];
     int i;
     for(i=0;i<32;i++)
     {
         mthetacos[i] = cos(mpleishen32_vertical[i]*M_PI/180.0);
         mthetasin[i] = sin(mpleishen32_vertical[i]*M_PI/180.0);
     }
+
+    mplidarudp = new lidarudp(nDataPort);
+    mplidardevudp = new lidarudp(nDevPort);
+    mpthreaddev = new std::thread(&leishen32::threaddevdecode,this);
+    mpthread = new std::thread(&leishen32::threaddecode,this);
 }
 
-void leishen32::threaddecode()
+leishen32::~leishen32()
 {
+    mbrun = false;
+    mpthreaddev->join();
+    mpthread->join();
+    delete mplidarudp;
+    delete mplidardevudp;
+}
 
+void leishen32::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 leishen32::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 leishen32::DecodeUDPPac(iv::lidarudppac & xpac)
@@ -46,19 +90,19 @@ int leishen32::DecodeUDPPac(iv::lidarudppac & xpac)
     }
     nNotDecodeNum = 0;
 
-    double finclinationang_xaxis = 0.0;// atof(gstr_inclinationang_xaxis)*M_PI/180.0;  //Inclination from x axis
-    double finclinationang_yaxis = 0.0;// atof(gstr_inclinationang_yaxis)*M_PI/180.0;   //Inclination from y axis
+    double finclinationang_xaxis = mfinclinationang_xaxis * M_PI/180.0;// atof(gstr_inclinationang_xaxis)*M_PI/180.0;  //Inclination from x axis
+    double finclinationang_yaxis = mfinclinationang_yaxis * M_PI/180.0;// atof(gstr_inclinationang_yaxis)*M_PI/180.0;   //Inclination from y axis
     bool binclix = false;
     bool bincliy = false;
-    if(finclinationang_xaxis != 0.0)binclix = true;
-    if(finclinationang_yaxis != 0.0)bincliy = true;
+    if(fabs(finclinationang_xaxis)>0.00000001)binclix = true;
+    if(fabs(finclinationang_yaxis)>0.00000001)bincliy = true;
 
     float cos_finclinationang_xaxis = static_cast<float>(cos(finclinationang_xaxis)) ;
     float sin_finclinationang_xaxis = static_cast<float>(sin(finclinationang_xaxis));
     float cos_finclinationang_yaxis = static_cast<float>(cos(finclinationang_yaxis));
     float sin_finclinationang_yaxis = static_cast<float>(sin(finclinationang_yaxis));
 
-    double frollang = 0;
+    double frollang = mfrollang;
 
     char * pstr =  static_cast<char * >(xpac.mdata_ptr.get()) ;
 
@@ -98,8 +142,11 @@ int leishen32::DecodeUDPPac(iv::lidarudppac & xpac)
         }
         gAngleOld = fazu;
 
-        if(gAngleTotal>=360.0)
+        if(gAngleTotal>360.0)
         {
+            gAngleTotal = 0.0;
+            std::cout<< std::chrono::system_clock::now().time_since_epoch().count()/1000000 <<" share point cloud."<<std::endl;
+            ProduceCloud(mpoint_cloud_temp);
             //share point cloud.
 
             pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
@@ -191,6 +238,30 @@ int leishen32::DecodeDevinfo(iv::lidarudppac & xpac)
     mdevinfo.A3 = A[2];
     mdevinfo.A4 = A[3];
 
+    if(mnLidarMode == 0)
+    {
+        mpleishen32_A[0] = mdevinfo.A2;mpleishen32_A[1] = mdevinfo.A2;mpleishen32_A[2] = mdevinfo.A1;mpleishen32_A[3] = mdevinfo.A1;
+        mpleishen32_A[4] = mdevinfo.A2;mpleishen32_A[5] = mdevinfo.A2;mpleishen32_A[6] = mdevinfo.A1;mpleishen32_A[7] = mdevinfo.A1;
+        mpleishen32_A[8] = mdevinfo.A2;mpleishen32_A[9] = mdevinfo.A2;mpleishen32_A[10] = mdevinfo.A1;mpleishen32_A[11] = mdevinfo.A1;
+        mpleishen32_A[12] = mdevinfo.A2;mpleishen32_A[13] = mdevinfo.A2;mpleishen32_A[14] = mdevinfo.A1;mpleishen32_A[15] = mdevinfo.A1;
+        mpleishen32_A[16] = mdevinfo.A2;mpleishen32_A[17] = mdevinfo.A2;mpleishen32_A[18] = mdevinfo.A1;mpleishen32_A[19] = mdevinfo.A1;
+        mpleishen32_A[20] = mdevinfo.A2;mpleishen32_A[21] = mdevinfo.A2;mpleishen32_A[22] = mdevinfo.A1;mpleishen32_A[23] = mdevinfo.A1;
+        mpleishen32_A[24] = mdevinfo.A2;mpleishen32_A[25] = mdevinfo.A2;mpleishen32_A[26] = mdevinfo.A1;mpleishen32_A[27] = mdevinfo.A1;
+        mpleishen32_A[28] = mdevinfo.A2;mpleishen32_A[29] = mdevinfo.A2;mpleishen32_A[30] = mdevinfo.A1;mpleishen32_A[31] = mdevinfo.A1;
+
+    }
+    else
+    {
+        mpleishen32_A[0] = mdevinfo.A2;mpleishen32_A[1] = mdevinfo.A2;mpleishen32_A[2] = mdevinfo.A1;mpleishen32_A[3] = mdevinfo.A1;
+        mpleishen32_A[4] = mdevinfo.A2;mpleishen32_A[5] = mdevinfo.A4;mpleishen32_A[6] = mdevinfo.A1;mpleishen32_A[7] = mdevinfo.A3;
+        mpleishen32_A[8] = mdevinfo.A2;mpleishen32_A[9] = mdevinfo.A2;mpleishen32_A[10] = mdevinfo.A1;mpleishen32_A[11] = mdevinfo.A1;
+        mpleishen32_A[12] = mdevinfo.A2;mpleishen32_A[13] = mdevinfo.A4;mpleishen32_A[14] = mdevinfo.A1;mpleishen32_A[15] = mdevinfo.A3;
+        mpleishen32_A[16] = mdevinfo.A2;mpleishen32_A[17] = mdevinfo.A2;mpleishen32_A[18] = mdevinfo.A1;mpleishen32_A[19] = mdevinfo.A1;
+        mpleishen32_A[20] = mdevinfo.A4;mpleishen32_A[21] = mdevinfo.A2;mpleishen32_A[22] = mdevinfo.A3;mpleishen32_A[23] = mdevinfo.A1;
+        mpleishen32_A[24] = mdevinfo.A2;mpleishen32_A[25] = mdevinfo.A2;mpleishen32_A[26] = mdevinfo.A1;mpleishen32_A[27] = mdevinfo.A1;
+        mpleishen32_A[28] = mdevinfo.A4;mpleishen32_A[29] = mdevinfo.A2;mpleishen32_A[30] = mdevinfo.A3;mpleishen32_A[31] = mdevinfo.A1;
+    }
+
     mbdevinfo = true;
     return 0;
 }

src/driver/driver_lidar_leishen32/leishen32.h

@@ -11,6 +11,8 @@
 
 #include "lidarudp.h"
 
+#include "lidardecode.h"
+
 
 
 namespace iv {
@@ -27,10 +29,13 @@ struct leishen32_devinfo
 };
 }
 
-class leishen32
+class leishen32 : public lidardecode
 {
 public:
-    leishen32();
+    leishen32(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);
+    ~leishen32();
 
 private:
     pcl::PointCloud<pcl::PointXYZI>::Ptr mpoint_cloud;
@@ -39,19 +44,44 @@ private:
     iv::leishen32_devinfo mdevinfo;
     bool mbdevinfo = false;
 
-    double * mpleishen32_vertical;
-    double * mpleishen32_A;
+    const double * mpleishen32_vertical;
+    double  mpleishen32_A[32];
 
     double mthetacos[32];
     double mthetasin[32];
 
+    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:
+    const double leishen32_vertical_1[32]={-16,0,-15,1,-14,2,-13,3,
+                                    -12,4,-11,5,-10,6,-9,7,
+                                    0,1,2,3,4,5,6,7,
+                                    8,9,10,11,12,13,14,15};
+
+    const double leishen32_vertical_033[32]={-18,-1,-15,-0.66,-12,-0.33,-10,0,
+                                              -8,0.33,-7,0.66,-6,1,-5,1.33,
+                                              -4,1.66,-3.33,2,-3,3,-2.66,4,
+                                              -2.33,6,-2,8,-1.66,11,-1.33,14};
+
+
 private:
     void threaddecode( );
+    void threaddevdecode();
     int DecodeUDPPac(iv::lidarudppac & xpac);
     int DecodeDevinfo(iv::lidarudppac & xpac);
 
 
-
 };
 
 #endif // LEISHEN32_H

src/driver/driver_lidar_leishen32/lidardecode.cpp

@@ -0,0 +1,75 @@
+#include "lidardecode.h"
+
+lidardecode::lidardecode()
+{
+    mbRun = true;
+    mbNew = false;
+    mpthread = new std::thread(&lidardecode::threadBroad,this);
+}
+
+lidardecode::~lidardecode()
+{
+    mbRun = false;
+    mpthread->join();
+
+}
+
+void lidardecode::ProduceCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr & point_cloud)
+{
+    mMutex.lock();
+    mpoint_cloud = point_cloud;
+    mbNew = true;
+    mMutex.unlock();
+    mCV.notify_all();
+}
+
+void lidardecode::RegisterSend(char * strmemname)
+{
+    mpa = iv::modulecomm::RegisterSend(strmemname,10000000,1);
+
+}
+
+void lidardecode::sharepointcloud(void * pa,pcl::PointCloud<pcl::PointXYZI>::Ptr & point_cloud)
+{
+    char * strOut = new char[4+sizeof(point_cloud->header.frame_id.size()) + 4+8+point_cloud->width * sizeof(pcl::PointXYZI)];
+
+    int * pHeadSize =  reinterpret_cast<int * >(strOut);
+    *pHeadSize = static_cast<int >(4 + point_cloud->header.frame_id.size()+4+8) ;
+    memcpy(strOut+4,point_cloud->header.frame_id.c_str(),point_cloud->header.frame_id.size());
+    uint32_t * pu32 =  reinterpret_cast<uint32_t *>(strOut+4+sizeof(point_cloud->header.frame_id.size()));
+    *pu32 = point_cloud->header.seq;
+    memcpy(strOut+4+sizeof(point_cloud->header.frame_id.size()) + 4,&point_cloud->header.stamp,8);
+    pcl::PointXYZI * p;
+    p = reinterpret_cast<pcl::PointXYZI *>(strOut +4+sizeof(point_cloud->header.frame_id.size()) + 4+8 );
+    memcpy(p,point_cloud->points.data(),point_cloud->width * sizeof(pcl::PointXYZI));
+
+    iv::modulecomm::ModuleSendMsg(pa,strOut,4+sizeof(point_cloud->header.frame_id.size()) + 4+8+point_cloud->width * sizeof(pcl::PointXYZI));
+
+    delete [] strOut;
+}
+
+void lidardecode::threadBroad()
+{
+    std::cout<<"thread broad run."<<std::endl;
+    while(mbRun)
+    {
+        std::unique_lock<std::mutex> lk(mMutexCV);
+        if(mCV.wait_for(lk,std::chrono::milliseconds(100)) == std::cv_status::timeout)
+        {
+            lk.unlock();
+        }
+        else
+        {
+            lk.unlock();
+        }
+        if(mbNew)
+        {
+            mMutex.lock();
+            pcl::PointCloud<pcl::PointXYZI>::Ptr pointcloud = mpoint_cloud;
+            mbNew = false;
+            mMutex.unlock();
+            sharepointcloud(mpa,pointcloud);
+            //share;
+        }
+    }
+}

src/driver/driver_lidar_leishen32/lidardecode.h

@@ -0,0 +1,47 @@
+#ifndef LIDARDECODE_H
+#define LIDARDECODE_H
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include <vector>
+#include <mutex>
+#include <condition_variable>
+#include <thread>
+
+#include "modulecomm.h"
+
+class lidardecode
+{
+public:
+    lidardecode();
+    ~lidardecode();
+
+private:
+    pcl::PointCloud<pcl::PointXYZI>::Ptr mpoint_cloud;
+    bool mbNew = false;
+    std::mutex mMutex;
+    std::mutex mMutexCV;
+    std::condition_variable mCV;
+    bool mbRun = true;
+    std::thread * mpthread;
+
+    void * mpa = NULL;
+
+public:
+    void ProduceCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr & point_cloud);
+
+    void RegisterSend(char * strmemname);
+
+
+
+
+private:
+    void threadBroad();
+
+    void sharepointcloud(void * pa,pcl::PointCloud<pcl::PointXYZI>::Ptr & point_cloud);
+
+
+};
+
+#endif // LIDARDECODE_H

src/driver/driver_lidar_leishen32/lidarudp.cpp

@@ -18,6 +18,42 @@ lidarudp::~lidarudp()
     mpthread->join();
 }
 
+std::vector<iv::lidarudppac> lidarudp::ComsumeRecv(int waitms)
+{
+    std::vector<iv::lidarudppac> xvectorlidarudppac;
+    xvectorlidarudppac.clear();
+    mmutex.lock();
+    if(mvectorba.size()>0)
+    {
+        xvectorlidarudppac = mvectorba;
+    }
+    mmutex.unlock();
+    //If have data, not wait.
+    if(xvectorlidarudppac.size()>0)
+    {
+        return xvectorlidarudppac;
+    }
+
+    std::unique_lock<std::mutex> lk(mmutexcv);
+    if(mcv.wait_for(lk, std::chrono::milliseconds(waitms)) == std::cv_status::timeout)
+    {
+        lk.unlock();
+    }
+    else
+    {
+        lk.unlock();
+    }
+
+    mmutex.lock();
+    if(mvectorba.size()>0)
+    {
+        xvectorlidarudppac = mvectorba;
+    }
+    mmutex.unlock();
+
+    return xvectorlidarudppac;
+}
+
 void lidarudp::threadrecv(int nport)
 {
     QUdpSocket * udpSocket = new QUdpSocket( );

src/driver/driver_lidar_leishen32/lidarudp.h

@@ -25,6 +25,9 @@ public:
     lidarudp(int nport);
     ~lidarudp();
 
+public:
+    std::vector<iv::lidarudppac> ComsumeRecv(int waitms);
+
 private:
     void threadrecv(int nport);
 

src/driver/driver_lidar_leishen32/main.cpp

@@ -1,13 +1,234 @@
 #include <QCoreApplication>
 
 
-#include "lidarudp.h"
+#include "leishen32.h"
+
+#include <getopt.h>
+#include <yaml-cpp/yaml.h>   //if no this,sudo apt-get install libyaml-cpp-dev
+
+#include "ivfault.h"
+#include "ivlog.h"
+
+
+static char gstr_memname[256];
+static char gstr_rollang[256];
+static char gstr_inclinationang_yaxis[256];  //from y axis
+static char gstr_inclinationang_xaxis[256];  //from x axis
+//char gstr_hostip[256];
+static char gstr_port[256];
+static char gstr_devport[256];
+static char gstr_yaml[256];
+static char gstr_modulename[256];
+static char gstr_devmode[256];
+
+/**
+ * @brief print_useage
+ */
+void print_useage()
+{
+    std::cout<<" -n --modulename $modulename : set module name. eq.  -n lidarleft"<<std::endl;
+    std::cout<<" -m --memname $memname : share memory name. eq.  -m lidar_pc"<<std::endl;
+    std::cout<<" -r --rollang $rollang : roll angle. eq.  -r 10.0"<<std::endl;
+    std::cout<<" -x --inclinationang_xaxis $inclinationang_xaxis : inclination angle from x axis. eq.  -x 0.0"<<std::endl;
+    std::cout<<" -y --inclinationang_yaxis $inclinationang_yaxis : inclination angle from y axis. eq.  -y 0.0"<<std::endl;
+//    std::cout<<" -o --hostip $hostip : host ip. eq.  -o 192.168.1.111"<<std::endl;
+    std::cout<<" -p --port $port : port . eq.  -p 2368"<<std::endl;
+    std::cout<<" -d --devport $port : port . eq.  -d 2369"<<std::endl;
+    std::cout<<" -f --devport $port : port . eq.  -f 0"<<std::endl;
+    std::cout<<" -s --setyaml $yaml : port . eq.  -s rs1.yaml"<<std::endl;
+    std::cout<<" -h --help print help"<<std::endl;
+}
+
+int  GetOptLong(int argc, char *argv[]) {
+    int nRtn = 0;
+    int opt; // getopt_long() 的返回值
+    int digit_optind = 0; // 设置短参数类型及是否需要参数
+    (void)digit_optind;
+
+    // 如果option_index非空，它指向的变量将记录当前找到参数符合long_opts里的
+    // 第几个元素的描述，即是long_opts的下标值
+    int option_index = 0;
+    // 设置短参数类型及是否需要参数
+    const char *optstring = "n:m:r:x:y:p:s:h";
+
+    // 设置长参数类型及其简写，比如 --reqarg <==>-r
+    /*
+    struct option {
+             const char * name;  // 参数的名称
+             int has_arg; // 是否带参数值，有三种：no_argument， required_argument，optional_argument
+             int * flag; // 为空时，函数直接将 val 的数值从getopt_long的返回值返回出去，
+                     // 当非空时，val的值会被赋到 flag 指向的整型数中，而函数返回值为0
+             int val; // 用于指定函数找到该选项时的返回值，或者当flag非空时指定flag指向的数据的值
+        };
+    其中：
+        no_argument(即0)，表明这个长参数不带参数（即不带数值，如：--name）
+            required_argument(即1)，表明这个长参数必须带参数（即必须带数值，如：--name Bob）
+            optional_argument(即2)，表明这个长参数后面带的参数是可选的，（即--name和--name Bob均可）
+     */
+    static struct option long_options[] = {
+        {"modulename", required_argument, NULL, 'n'},
+        {"memname", required_argument, NULL, 'm'},
+        {"rollang", required_argument, NULL, 'r'},
+        {"inclinationang_xaxis", required_argument, NULL, 'x'},
+        {"inclinationang_yaxis", required_argument, NULL, 'y'},
+        {"port", required_argument, NULL, 'p'},
+        {"devport", required_argument, NULL, 'd'},
+        {"devmode", required_argument, NULL, 'f'},
+        {"setyaml", required_argument, NULL, 's'},
+        {"help",  no_argument,       NULL, 'h'},
+ //       {"optarg", optional_argument, NULL, 'o'},
+        {0, 0, 0, 0}  // 添加 {0, 0, 0, 0} 是为了防止输入空值
+    };
+
+    while ( (opt = getopt_long(argc,
+                               argv,
+                               optstring,
+                               long_options,
+                               &option_index)) != -1) {
+//        printf("opt = %c\n", opt); // 命令参数，亦即 -a -b -n -r
+//        printf("optarg = %s\n", optarg); // 参数内容
+//        printf("optind = %d\n", optind); // 下一个被处理的下标值
+//        printf("argv[optind - 1] = %s\n",  argv[optind - 1]); // 参数内容
+//        printf("option_index = %d\n", option_index);  // 当前打印参数的下标值
+//        printf("\n");
+        switch(opt)
+        {
+        case 'n':
+            strncpy(gstr_modulename,optarg,255);
+            break;
+        case 'm':
+            strncpy(gstr_memname,optarg,255);
+            break;
+        case 'r':
+            strncpy(gstr_rollang,optarg,255);
+            break;
+        case 'x':
+            strncpy(gstr_inclinationang_xaxis,optarg,255);
+            break;
+        case 'y':
+            strncpy(gstr_inclinationang_yaxis,optarg,255);
+            break;
+//        case 'o':
+//            strncpy(gstr_hostip,optarg,255);
+//            break;
+        case 'p':
+            strncpy(gstr_port,optarg,255);
+            break;
+        case 'd':
+            strncpy(gstr_devport,optarg,255);
+            break;
+        case 'f':
+            strncpy(gstr_devmode,optarg,255);
+            break;
+        case 's':
+            strncpy(gstr_yaml,optarg,255);
+            break;
+        case 'h':
+            print_useage();
+            nRtn = 1; //because use -h
+            break;
+        default:
+            break;
+        }
+
+    }
+
+    return nRtn;
+}
+
+/**
+ * @brief decodeyaml
+ * @param stryaml yaml path
+ */
+void decodeyaml(const char * stryaml)
+{
+    YAML::Node config;
+    try
+    {
+        config = YAML::LoadFile(stryaml);
+    }
+    catch(YAML::BadFile e)
+    {
+        qDebug("load yaml error.");
+        return;
+    }
+
+
+    if(config["memname"])
+    {
+        strncpy(gstr_memname,config["memname"].as<std::string>().data(),255);
+    }
+    if(config["rollang"])
+    {
+        strncpy(gstr_rollang,config["rollang"].as<std::string>().data(),255);
+    }
+    if(config["inclinationang_xaxis"])
+    {
+        strncpy(gstr_inclinationang_xaxis,config["inclinationang_xaxis"].as<std::string>().data(),255);
+    }
+    if(config["inclinationang_yaxis"])
+    {
+        strncpy(gstr_inclinationang_yaxis,config["inclinationang_yaxis"].as<std::string>().data(),255);
+    }
+//    if(config["hostip"])
+//    {
+//        strncpy(gstr_hostip,config["hostip"].as<std::string>().data(),255);
+//    }
+    if(config["port"])
+    {
+        strncpy(gstr_port,config["port"].as<std::string>().data(),255);
+    }
+
+    if(config["devport"])
+    {
+        strncpy(gstr_devport,config["port"].as<std::string>().data(),255);
+    }
+
+    if(config["devmode"])
+    {
+        strncpy(gstr_devmode,config["port"].as<std::string>().data(),255);
+    }
+
+//    std::cout<<gstr_memname<<std::endl;
+//    std::cout<<gstr_rollang<<std::endl;
+//    std::cout<<gstr_inclinationang_xaxis<<std::endl;
+//    std::cout<<gstr_inclinationang_yaxis<<std::endl;
+//    std::cout<<gstr_hostip<<std::endl;
+//    std::cout<<gstr_port<<std::endl;
+}
+
+
 
 int main(int argc, char *argv[])
 {
     QCoreApplication a(argc, argv);
 
-    lidarudp * plu = new lidarudp(2368);
+    snprintf(gstr_memname,255,"lidar_pc");
+    snprintf(gstr_rollang,255,"0.0");//这个角度要根据安装情况进行校正！！！！！！！！
+    snprintf(gstr_inclinationang_xaxis,255,"0.0");
+    snprintf(gstr_inclinationang_yaxis,255,"0");
+//    snprintf(gstr_hostip,255,"192.168.1.102");
+    snprintf(gstr_port,255,"2368");//默认端口号
+    snprintf(gstr_devport,255,"2369");//默认端口号
+    snprintf(gstr_devmode,255,"0");//默认端口号
+    snprintf(gstr_yaml,255,"p");
+    snprintf(gstr_modulename,255,"driver_lidar_leishen32");
+
+    int nRtn = GetOptLong(argc,argv);
+    if(nRtn == 1)  //show help,so exit.
+    {
+        return 0;
+    }
+
+    if(strnlen(gstr_yaml,255)>1)
+    {
+        decodeyaml(gstr_yaml);
+    }
+
+    leishen32 * pleishen32 = new leishen32(gstr_memname,atof(gstr_rollang),atof(gstr_inclinationang_xaxis),
+                                           atof(gstr_inclinationang_yaxis),atoi(gstr_devmode),static_cast<unsigned short>(atoi(gstr_port)),
+                                           static_cast<unsigned short>(atoi(gstr_devport)));
+    (void)pleishen32;
 
     return a.exec();
 }

src/driver/driver_lidar_rs16/lidar_driver_rs16.cpp

@@ -401,6 +401,7 @@ void process_rs16obs(char * strdata,int nLen)
     memcpy(p,point_cloud->points.data(),point_cloud->width * sizeof(pcl::PointXYZI));
 
     iv::modulecomm::ModuleSendMsg(g_lidar_pc,strOut,4+sizeof(point_cloud->header.frame_id.size()) + 4+8+point_cloud->width * sizeof(pcl::PointXYZI));
+
     delete strOut;
 
 //            std::cout<<"point cloud width = "<<point_cloud->width<<"  size = "<<point_cloud->size()<<std::endl;

src/tool/adcndtmultimapping/adcndtmultimapping.pro

@@ -57,6 +57,7 @@ FORMS += \
 INCLUDEPATH += /opt/ros/melodic/include
 INCLUDEPATH += /usr/include/pcl-1.8
 INCLUDEPATH += /usr/include/eigen3
+unix:INCLUDEPATH += /usr/include/pcl-1.12
 
 #INCLUDEPATH += /usr/include/qwt
 

src/tool/adcndtmultimapping/ndt_mapping.cpp

@@ -33,7 +33,10 @@
 #include <tf/transform_broadcaster.h>
 #include <tf/transform_datatypes.h>
 
-#include <pcl/io/io.h>
+#include <numeric>
+
+#include <pcl/common/io.h>
+//#include <pcl/io/io.h>
 #include <pcl/io/pcd_io.h>
 #include <pcl/point_types.h>
 //#include <pcl_conversions/pcl_conversions.h>