complete driver_lidar_bk.

include/ivvtk.pri

@@ -0,0 +1,21 @@
+
+
+unix:infile( /etc/lsb-release, DISTRIB_RELEASE, 18.04 ){
+LIBS += -lvtkCommonExecutionModel-6.3 -lvtkCommonCore-6.3 -lvtkRenderingLOD-6.3 -lvtkRenderingCore-6.3 \
+        -lvtkFiltersSources-6.3
+}
+
+unix:infile( /etc/lsb-release, DISTRIB_RELEASE, 22.04 ){
+LIBS += -lvtkCommonExecutionModel-9.1 -lvtkCommonCore-9.1 -lvtkRenderingLOD-9.1 -lvtkRenderingCore-9.1 \
+        -lvtkFiltersSources-9.1
+}
+
+unix:infile( /etc/lsb-release, DISTRIB_RELEASE, 24.04 ){
+LIBS += -lvtkCommonExecutionModel-9.1 -lvtkCommonCore-9.1 -lvtkRenderingLOD-9.1 -lvtkRenderingCore-9.1 \
+        -lvtkFiltersSources-9.1
+}
+
+unix:infile( /etc/lsb-release, DISTRIB_RELEASE, 20.04 ){
+LIBS += -lvtkCommonExecutionModel-7.1 -lvtkCommonCore-7.1 -lvtkRenderingLOD-7.1 -lvtkRenderingCore-7.1 \
+        -lvtkFiltersSources-7.1
+}

src/driver/driver_lidar_bk_16z/driver_lidar_bk_16z.pro

@@ -9,8 +9,8 @@ CONFIG += c++17 cmdline
 SOURCES += \
         common/ioapi.cpp \
         common/ssFrameLib.cpp \
-        lidar_16z.cpp \
         lidar_bkdriver.cpp \
+        lidar_bkz.cpp \
         main.cpp
 
 # Default rules for deployment.
@@ -23,8 +23,8 @@ HEADERS += \
     common/ioapi.h \
     common/ssFrameLib.h \
     common/utility.h \
-    lidar_16z.h \
-    lidar_bkdriver.h
+    lidar_bkdriver.h \
+    lidar_bkz.h
 
 
 INCLUDEPATH += $$PWD/common

src/driver/driver_lidar_bk_16z/lidar_16z.cpp

@@ -1,128 +0,0 @@
-#include "lidar_16z.h"
-
-
-static float V_theta[16] = {-15,-13,-11,-9,-7,-5,-3,-1,1,3,5,7,9,11,13,15};
-static float H_Beta[16] = {-2.5,-2.5 ,-2.5 ,-2.5,-2.5,-2.5 ,-2.5 ,-2.5,
-                    2.5,-2.5 ,-2.5 ,-2.5, -2.5,-2.5 ,-2.5 ,-2.5};
-
-lidar_16z::lidar_16z() {
-
-    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;
-
-    mpdriver = new lidar_bkdriver();
-    mpTheadDecode = new  std::thread(&lidar_16z::ThreadDecode,this);
-}
-
-lidar_16z::~lidar_16z(){
-    mbRun = false;
-    mpTheadDecode->join();
-    delete mpdriver;
-}
-
-void lidar_16z::ThreadDecode()
-{
-    static double azutotal = 0.0;
-    static int g_seq = 0;
-    while(mbRun)
-    {
-        std::shared_ptr<unsigned char> pdata_ptr;
-        int64_t nrecvtime;
-        int ndatasize = 0;
-        int nread = mpdriver->GetRecvData(pdata_ptr,nrecvtime,ndatasize);
-        if(nread == 1)
-        {
-            std::cout<<"read "<<ndatasize<<std::endl;
-            if(ndatasize % 1184 != 0)
-            {
-                std::cout<<" data not complete. datasize: "<<ndatasize<<std::endl;
-            }
-            else
-            {
-                unsigned char * p = pdata_ptr.get();
-                unsigned short * pang_start = (unsigned short * )(p+12);
-                unsigned short * pang_end = (unsigned short *)(p+14);
-                (void)pang_end;
-                double ang_space = 0.2;
-                int i;
-                int nheadlen = 32;
-                double fAzuAng = static_cast<double>(*pang_start) * 0.01;
-                for(i=0;i<12;i++)
-                {
-                    int j;
-                    for(j=0;j<16;j++)
-                    {
-                        unsigned char * pecho = (unsigned char *)(p +nheadlen +  i*96 + j*6);
-                        unsigned short * prange;
-                        unsigned char * pchch;
-                        double frange;
-                        unsigned char  nch;
-                        unsigned char intensity;
-                        pchch = pecho;
-                        prange = (unsigned short *)(pecho +3);
-                        intensity = *(pecho + 5);
-                        nch = * pchch;
-                        frange = static_cast<double>(*prange);
-                        frange = frange * 0.004;
-                        if(nch<=16){
-                            double fAng = (-fAzuAng - H_Beta[nch] - mfRollAng) * M_PI/180.0;
-                            double vtheta = V_theta[nch] * M_PI/180.0;
-                            double x = frange * cos(fAng)* cos(vtheta);
-                            double y = frange * sin(fAng)* cos(vtheta);
-                            double z = frange * sin(vtheta);
-
-                            pcl::PointXYZI point;
-                            point.x = x;
-                            point.y = y;
-                            point.z = z;
-                            point.intensity = intensity;
-
-                            mpoint_cloud_temp->points.push_back(point);
-                            ++mpoint_cloud_temp->width;
-                        }
-                        else
-                        {
-                            std::cout<<" channel error."<<std::endl;
-                        }
-                    }
-                    fAzuAng = fAzuAng + ang_space;
-
-                    azutotal = azutotal + ang_space;
-                    if(azutotal>=360.0)
-                    {
-                        //share pointcloud
-                        mmutexpc.lock();
-                        mpoint_cloud = mpoint_cloud_temp;
-                        mbpcupdate = true;
-                        mmutexpc.unlock();
-                        mcv.notify_all();
-                        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 =g_seq;
-                        g_seq++;
-                        mpoint_cloud_temp = point_cloud;
-                        azutotal = 0;
-                    }
-                }
-            }
-        }
-    }
-}
-
-int lidar_16z::GetPointCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr & point_cloud)
-{
-    if(mbpcupdate == false)return 0;
-    return 1;
-}

src/driver/driver_lidar_bk_16z/lidar_bkdriver.cpp

@@ -36,8 +36,13 @@ static uint32_t s_CrcTab32[] = {
 #define IS_CRC_START_32 0xFFFFFFFF
 #define PACKAGELEN 60000
 
-lidar_bkdriver::lidar_bkdriver() {
-    setparam();
+lidar_bkdriver::lidar_bkdriver(std::string strlidarip,int ndataport,int nctrlport,int ndevport,int nMotorHZ) {
+    mstrlidarip = strlidarip;
+    mndataport = ndataport;
+    mnctrlport = nctrlport;
+    mndevport = ndevport;
+    mnMotorHZ = nMotorHZ;
+    setparam(mstrlidarip,mndataport,mnctrlport,mndevport,mnMotorHZ);
     socketInit();
     configDeviceParams();
 
@@ -59,20 +64,20 @@ void lidar_bkdriver::socketInit()
     m_heart_socket = new rfans_driver::IOSocketAPI(m_input_para.device_ip, m_input_para.heart_port, m_input_para.heart_port);
 }
 
-void lidar_bkdriver::setparam()
+void lidar_bkdriver::setparam(std::string strlidarip,int ndataport,int nctrlport,int ndevport,int nMotorHZ)
 {
     m_input_para.device_name ="R-Fans-16";
     m_input_para.display_mode = "overlay";
 
-    m_input_para.device_ip = "192.168.0.10";
-    m_input_para.dataport = 2014;
-    m_input_para.ctrlport = 2013;
-    m_input_para.heart_port = 2030;
+    m_input_para.device_ip = strlidarip;//"192.168.0.10";
+    m_input_para.dataport = ndataport ;//2014;
+    m_input_para.ctrlport = nctrlport;//2013;
+    m_input_para.heart_port = ndevport;//2030;
 
     m_input_para.frame_id =  "world";
 
     m_input_para.save_xyz = false;
-    m_input_para.scnSpeed = 10;
+    m_input_para.scnSpeed = nMotorHZ;//10;
     m_input_para.simu_filepath = "";
     m_input_para.cfg_path = "";
     m_input_para.dual_echo = false;
@@ -344,8 +349,8 @@ int lidar_bkdriver::RecvRawThread()
         mmutexrawdata.unlock();
         mcv.notify_all();
 
-        std::cout<<std::chrono::system_clock::now().time_since_epoch().count()<<" recv "<<ret<<std::endl;
-        std::cout<<" mod: "<<ret%1248<<std::endl;
+ //       std::cout<<std::chrono::system_clock::now().time_since_epoch().count()<<" recv "<<ret<<std::endl;
+ //       std::cout<<" mod: "<<ret%1248<<std::endl;
 
     }
 
@@ -366,12 +371,16 @@ int lidar_bkdriver::HeartBeatThread()
             //     m_heartbeat_vec.push_back(m_heart);
             // }
         }
+        else
+        {
+            continue;
+        }
 
         static int num = 0;
         if (0 == num++)
         {
-            std::cout<<"temperature = "<<temper<<"motoer speed: "<<m_heart.motorspd<<std::endl;
-            std::cout<<"DEVICE_id = "<<m_heart.device_id<<std::endl;
+            std::cout<<"temperature = "<<temper<<"motor speed: "<<(int)m_heart.motorspd<<std::endl;
+            std::cout<<"DEVICE_id = "<<(int)m_heart.device_id<<std::endl;
             // ROS_WARN("temperature=%f", temper);
             // ROS_INFO("DEVICE_id=%d", m_heart.device_id);
         }

src/driver/driver_lidar_bk_16z/lidar_bkdriver.h

@@ -26,7 +26,7 @@ struct lidarraw
 class lidar_bkdriver
 {
 public:
-    lidar_bkdriver();
+    lidar_bkdriver(std::string strlidarip = "192.168.0.10",int ndataport = 2014,int nctrlport = 2013,int ndevport = 2030,int nMotorHZ = 10);
     ~lidar_bkdriver();
 
 public:
@@ -37,9 +37,15 @@ private:
     rfans_driver::IOAPI *m_data_socket;
     rfans_driver::IOAPI *m_heart_socket;
 
+    std::string mstrlidarip;
+    int mndataport;
+    int mnctrlport;
+    int mndevport;
+    int mnMotorHZ;
+
 private:
     void socketInit();
-    void setparam();
+    void setparam(std::string strlidarip,int ndataport,int nctrlport,int ndevport,int nMotorHZ);
     void configDeviceParams();
     int progSet(lidarAPi::DEB_PROGRM_S &program);
 

src/driver/driver_lidar_bk_16z/lidar_bkz.cpp

@@ -0,0 +1,369 @@
+#include "lidar_bkz.h"
+
+
+static float V_theta[16] = {-15,-13,-11,-9,-7,-5,-3,-1,1,3,5,7,9,11,13,15};
+static float H_Beta[16] = {-2.5,-2.5 ,-2.5 ,-2.5,-2.5,-2.5 ,-2.5 ,-2.5,
+                    2.5,-2.5 ,-2.5 ,-2.5, -2.5,-2.5 ,-2.5 ,-2.5};
+
+float V_theta_32[32] =
+    { -20,-19,-18,-17,-16,-15,-14,-13,-12,-11,-10,-9,-8,-7,-6,-5,-4,-3,-2,-1,0,1,2,3,4,5,6,7,8,
+     9,10,11};
+float H_Beta_32[32]= { 2.5, -2.5, 2.5, -2.5, 2.5, -2.5, 2.5, -2.5,
+                    2.5, -2.5, 2.5, -2.5, 2.5, -2.5, 2.5, -2.5,
+                    2.5, -2.5, 2.5, -2.5, 2.5, -2.5, 2.5, -2.5,
+                    2.5, -2.5, 2.5, -2.5, 2.5, -2.5, 2.5, -2.5};
+
+lidar_bkz::lidar_bkz(std::string strlidartype,std::string strlidarip,int ndataport ,int nctrlport ,int ndevport ,int nMotorHZ,double finclinationang_xaxis,
+                     double finclinationang_yaxis ) {
+
+    mfinclinationang_xaxis = finclinationang_xaxis;
+    mfinclinationang_yaxis = finclinationang_yaxis;
+    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;
+
+    mpdriver = new lidar_bkdriver(strlidarip,ndataport,nctrlport,ndevport,nMotorHZ);
+    if(strlidartype == "16z"){
+        mntype = 0;
+    }
+    else{
+        mntype = 1;
+    }
+    mpTheadDecode = new  std::thread(&lidar_bkz::ThreadDecode,this);
+}
+
+lidar_bkz::~lidar_bkz(){
+    mbRun = false;
+    mpTheadDecode->join();
+    delete mpdriver;
+}
+
+void lidar_bkz::ThreadDecode()
+{
+    static double azutotal = 0.0;
+    static int g_seq = 0;
+
+    bool  bLastAng = false;
+    unsigned  short nlastend;
+
+    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(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));
+
+    while(mbRun)
+    {
+        std::shared_ptr<unsigned char> pdata_ptr;
+        int64_t nrecvtime;
+        int ndatasize = 0;
+        int nread = mpdriver->GetRecvData(pdata_ptr,nrecvtime,ndatasize);
+        if(nread == 1)
+        {
+ //           std::cout<<"read "<<ndatasize<<std::endl;
+            if(ndatasize % 1184 != 0)
+            {
+                std::cout<<" data not complete. datasize: "<<ndatasize<<std::endl;
+            }
+            else
+            {
+                unsigned char * p = pdata_ptr.get();
+                int nPac = ndatasize/1184;
+                int k;
+                for(k=0;k<nPac;k++)
+                {
+                unsigned short * pang_start = (unsigned short * )(p+12);
+                unsigned short * pang_end = (unsigned short *)(p+14);
+ //               std::cout<<"start: "<<*pang_start<<" end: "<<*pang_end<<std::endl;
+                (void)pang_end;
+                double ang_space = static_cast<double>(*pang_end - *pang_start) * 0.01/12.0;
+                if(mntype == 1)
+                {
+                    ang_space = static_cast<double>(*pang_end - *pang_start) * 0.01/6.0;
+                }
+                if(bLastAng == true){
+                    ang_space = static_cast<double>(*pang_end - nlastend) * 0.01/12.0;
+                    if(mntype == 1)ang_space = static_cast<double>(*pang_end - nlastend) * 0.01/6.0;
+                    nlastend = *pang_end;
+                }
+                else {
+                    bLastAng = true;
+                    nlastend = *pang_end;
+                }
+ //               std::cout<<" ang space: "<<ang_space<<std::endl;
+                if(ang_space  < 0)
+                {
+                    if(mntype != 1)
+                    ang_space = ang_space + 360.0/12.0;
+                    else
+                        ang_space = ang_space + 360.0/6.0;
+                    std::cout<<" ang space in round: "<<ang_space<<std::endl;
+                    if((ang_space < 0) || (ang_space > 1.0))
+                    {
+                        std::cout<<" fix ang space fail. "<<" ang space: "<<ang_space<<std::endl;
+                        ang_space = 0.18;
+                    }
+                }
+                int i;
+                int nheadlen = 32;
+                double fAzuAng = static_cast<double>(*pang_start) * 0.01;
+ //               std::cout<<" ang :  "<<fAzuAng<<std::endl;
+                int nchcount = 16;
+                if(mntype == 1)
+                {
+                    nchcount = 32;
+                }
+                int nblocksize = nchcount * 6;
+                int nblockcount = 1184/nblocksize;
+
+                for(i=0;i<nblockcount;i++)
+                {
+                    int j;
+                    for(j=0;j<nchcount;j++)
+                    {
+                        unsigned char * pecho = (unsigned char *)(p +nheadlen +  i*nblocksize + j*6);
+                        unsigned short * prange;
+                        unsigned char * pchch;
+                        double frange;
+                        unsigned char  nch;
+                        unsigned char intensity;
+                        pchch = pecho;
+                        prange = (unsigned short *)(pecho +3);
+                        intensity = *(pecho + 5);
+                        nch = *pchch;
+                        frange = static_cast<double>(*prange);
+                        frange = frange * 0.004;
+                        if(nch<=nchcount){
+                            double fAng = (-fAzuAng - H_Beta[nch] - mfRollAng) * M_PI/180.0;
+                            double vtheta = V_theta[nch] * M_PI/180.0;
+                            if(mntype == 1)
+                            {
+                                fAng = (-fAzuAng - H_Beta_32[nch] - mfRollAng) * M_PI/180.0;
+                                vtheta = V_theta_32[nch] * M_PI/180.0;
+                            }
+
+                            double x = frange * cos(fAng)* cos(vtheta);
+                            double y = frange * sin(fAng)* cos(vtheta);
+                            double z = frange * sin(vtheta);
+
+                            pcl::PointXYZI point;
+                            point.x = x;
+                            point.y = y;
+                            point.z = z;
+                            point.intensity = intensity;
+
+                            if(binclix)
+                            {
+                                float y,z;
+                                y = point.y;z = point.z;
+                                point.y = y*cos_finclinationang_xaxis +z*sin_finclinationang_xaxis;
+                                point.z = z*cos_finclinationang_xaxis - y*sin_finclinationang_xaxis;
+                            }
+                            if(bincliy)
+                            {
+                                float z,x;
+                                z = point.z;x = point.x;
+                                point.z = z*cos_finclinationang_yaxis + x*sin_finclinationang_yaxis;
+                                point.x = x*cos_finclinationang_yaxis - z*sin_finclinationang_yaxis;
+                            }
+
+                            mpoint_cloud_temp->points.push_back(point);
+                            ++mpoint_cloud_temp->width;
+                        }
+                        else
+                        {
+                            std::cout<<" channel error."<<std::endl;
+                        }
+                    }
+                    fAzuAng = fAzuAng + ang_space;
+
+                    azutotal = azutotal + ang_space;
+                    if(azutotal>=360.0)
+                    {
+                        //share pointcloud
+                        mmutexpc.lock();
+                        mpoint_cloud = mpoint_cloud_temp;
+                        mbpcupdate = true;
+                        mmutexpc.unlock();
+                        mcv.notify_all();
+                        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 =g_seq;
+                        g_seq++;
+                        mpoint_cloud_temp = point_cloud;
+                        azutotal = 0;
+                    }
+                }
+                p= p+  1184;
+                }
+            }
+        }
+    }
+}
+
+void lidar_bkz::ThreadDecode32()
+{
+    static double azutotal = 0.0;
+    static int g_seq = 0;
+
+    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(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));
+
+    while(mbRun)
+    {
+        std::shared_ptr<unsigned char> pdata_ptr;
+        int64_t nrecvtime;
+        int ndatasize = 0;
+        int nread = mpdriver->GetRecvData(pdata_ptr,nrecvtime,ndatasize);
+        if(nread == 1)
+        {
+            std::cout<<"read "<<ndatasize<<std::endl;
+            if(ndatasize % 1184 != 0)
+            {
+                std::cout<<" data not complete. datasize: "<<ndatasize<<std::endl;
+            }
+            else
+            {
+                unsigned char * p = pdata_ptr.get();
+                unsigned short * pang_start = (unsigned short * )(p+12);
+                unsigned short * pang_end = (unsigned short *)(p+14);
+                (void)pang_end;
+                double ang_space = 0.2;
+                int i;
+                int nheadlen = 32;
+                double fAzuAng = static_cast<double>(*pang_start) * 0.01;
+                for(i=0;i<6;i++)
+                {
+                    int j;
+                    for(j=0;j<32;j++)
+                    {
+                        unsigned char * pecho = (unsigned char *)(p +nheadlen +  i*192 + j*6);
+                        unsigned short * prange;
+                        unsigned char * pchch;
+                        double frange;
+                        unsigned char  nch;
+                        unsigned char intensity;
+                        pchch = pecho;
+                        prange = (unsigned short *)(pecho +3);
+                        intensity = *(pecho + 5);
+                        nch = * pchch;
+                        frange = static_cast<double>(*prange);
+                        frange = frange * 0.004;
+                        if(nch<=16){
+                            double fAng = (-fAzuAng - H_Beta_32[nch] - mfRollAng) * M_PI/180.0;
+                            double vtheta = V_theta_32[nch] * M_PI/180.0;
+                            double x = frange * cos(fAng)* cos(vtheta);
+                            double y = frange * sin(fAng)* cos(vtheta);
+                            double z = frange * sin(vtheta);
+
+                            pcl::PointXYZI point;
+                            point.x = x;
+                            point.y = y;
+                            point.z = z;
+                            point.intensity = intensity;
+
+                            if(binclix)
+                            {
+                                float y,z;
+                                y = point.y;z = point.z;
+                                point.y = y*cos_finclinationang_xaxis +z*sin_finclinationang_xaxis;
+                                point.z = z*cos_finclinationang_xaxis - y*sin_finclinationang_xaxis;
+                            }
+                            if(bincliy)
+                            {
+                                float z,x;
+                                z = point.z;x = point.x;
+                                point.z = z*cos_finclinationang_yaxis + x*sin_finclinationang_yaxis;
+                                point.x = x*cos_finclinationang_yaxis - z*sin_finclinationang_yaxis;
+                            }
+
+                            mpoint_cloud_temp->points.push_back(point);
+                            ++mpoint_cloud_temp->width;
+                        }
+                        else
+                        {
+                            std::cout<<" channel error."<<std::endl;
+                        }
+                    }
+                    fAzuAng = fAzuAng + ang_space;
+
+                    azutotal = azutotal + ang_space;
+                    if(azutotal>=360.0)
+                    {
+                        //share pointcloud
+                        mmutexpc.lock();
+                        mpoint_cloud = mpoint_cloud_temp;
+                        mbpcupdate = true;
+                        mmutexpc.unlock();
+                        mcv.notify_all();
+                        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 =g_seq;
+                        g_seq++;
+                        mpoint_cloud_temp = point_cloud;
+                        azutotal = 0;
+                    }
+                }
+            }
+        }
+    }
+}
+
+int lidar_bkz::GetPointCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr & point_cloud)
+{
+    if(mbpcupdate == true)
+    {
+        mmutexpc.lock();
+        point_cloud = mpoint_cloud;
+        mbpcupdate = false;
+        mmutexpc.unlock();
+        return 1;
+    }
+    std::unique_lock<std::mutex> lk(mmutexcv);
+    if(mcv.wait_for(lk,std::chrono::milliseconds(10)) == std::cv_status::timeout)
+    {
+        return 0;
+    }
+//    std::cout<<" wait a pc."<<std::endl;
+    if(mbpcupdate == true)
+    {
+        mmutexpc.lock();
+        point_cloud = mpoint_cloud;
+        mbpcupdate = false;
+        mmutexpc.unlock();
+        return 1;
+    }
+
+    return 0;
+}

src/driver/driver_lidar_bk_16z/lidar_16z.h → src/driver/driver_lidar_bk_16z/lidar_bkz.h

@@ -1,5 +1,5 @@
-#ifndef LIDAR_16Z_H
-#define LIDAR_16Z_H
+#ifndef LIDAR_BKZ_H
+#define LIDAR_BKZ_H
 
 #include <mutex>
 #include <thread>
@@ -11,11 +11,13 @@
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 
-class lidar_16z
+class lidar_bkz
 {
 public:
-    lidar_16z();
-    ~lidar_16z();
+    //strlidartype 16z or 32z
+    lidar_bkz(std::string strlidartype,std::string strlidarip = "192.168.0.10",int ndataport = 2014,int nctrlport = 2013,int ndevport = 2030,int nMotorHZ = 10,double finclinationang_xaxis = 0.0,
+              double finclinationang_yaxis = 0.0);
+    ~lidar_bkz();
 
     int GetPointCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr & point_cloud);
 
@@ -23,6 +25,7 @@ private:
     lidar_bkdriver * mpdriver;
 
     void ThreadDecode();
+    void ThreadDecode32();
 
 private:
     bool mbRun = true;
@@ -37,6 +40,11 @@ private:
     std::mutex mmutexcv;
     std::condition_variable mcv;
     bool mbpcupdate = false;
+
+    double mfinclinationang_xaxis = 0.0;
+    double mfinclinationang_yaxis = 0.0;
+
+    int mntype= 0; //0 16  1 32
 };
 
 #endif // LIDAR_16Z_H

src/driver/driver_lidar_bk_16z/main.cpp

@@ -1,13 +1,285 @@
 #include <QCoreApplication>
+#include <thread>
+#include <iostream>
 
+#include "modulecomm.h"
+#include "xmlparam.h"
 
-#include "lidar_bkdriver.h"
+
+#include <getopt.h>
+#include <yaml-cpp/yaml.h>   //if no this,sudo apt-get install libyaml-cpp-dev
+
+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_ctrlport[256];
+static char gstr_devip[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 2014"<<std::endl;
+    std::cout<<" -d --devport $port : port . eq.  -d 2030"<<std::endl;
+    std::cout<<" -c --ctrlport $port : port . eq.  -c 2013"<<std::endl;
+    std::cout<<" -e --devip $devip: devp.  eq. -e 192.168.0.10"<<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:d:c:e: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'},
+        {"ctrlport", required_argument, NULL, 'c'},
+        {"devip", required_argument, NULL, 'e'},
+        {"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 'c':
+            strncpy(gstr_ctrlport,optarg,255);
+            break;
+        case 'e':
+            strncpy(gstr_devip,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["devport"].as<std::string>().data(),255);
+    }
+
+    if(config["ctrlport"])
+    {
+        strncpy(gstr_ctrlport,config["ctrlport"].as<std::string>().data(),255);
+    }
+
+    if(config["devip"])
+    {
+        strncpy(gstr_devip,config["devip"].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;
+}
+
+
+#include "lidar_bkz.h"
+
+bool gbRun = true;
+lidar_bkz * gpdriver;
+void * gpa;
+std::thread * gpthead;
+
+void 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));
+    std::cout<<std::chrono::system_clock::now().time_since_epoch().count()<<" point cloud width: "<<point_cloud->width<<std::endl;
+
+    iv::modulecomm::ModuleSendMsg(pa,strOut,4+sizeof(point_cloud->header.frame_id.size()) + 4+8+point_cloud->width * sizeof(pcl::PointXYZI));
+
+    delete [] strOut;
+}
+
+void ThreadShare()
+{
+    while(gbRun)
+    {
+        pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud;
+        if(gpdriver->GetPointCloud(point_cloud) == 1)
+        {
+            //Share Point Cloud
+            sharepointcloud(gpa,point_cloud);
+        }
+    }
+}
 
 int main(int argc, char *argv[])
 {
     QCoreApplication a(argc, argv);
 
-    lidar_bkdriver * pdriver = new lidar_bkdriver();
+    snprintf(gstr_memname,255,"lidar_pc");
+    snprintf(gstr_rollang,255,"270.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,"2014");//默认端口号
+    snprintf(gstr_devport,255,"2030");//默认端口号
+    snprintf(gstr_ctrlport,255,"2013");//默认端口号
+    snprintf(gstr_devip,255,"192.168.0.10");
+    snprintf(gstr_yaml,255,"y");
+    snprintf(gstr_modulename,255,"driver_lidar_bk_16z");
+
+    int nRtn = GetOptLong(argc,argv);
+    if(nRtn == 1)  //show help,so exit.
+    {
+        return 0;
+    }
+
+    if(strnlen(gstr_yaml,255)>1)
+    {
+        decodeyaml(gstr_yaml);
+    }
+
+    gpdriver = new lidar_bkz("16z",gstr_devip,atoi(gstr_port),atoi(gstr_ctrlport),atoi(gstr_devport),10,atof(gstr_inclinationang_xaxis),atof(gstr_inclinationang_yaxis));
+    gpa = iv::modulecomm::RegisterSend(gstr_memname,10000000,1);
+    gpthead = new std::thread(&ThreadShare);
+
 
     return a.exec();
 }

src/driver/driver_lidar_bk_32z/.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_bk_32z/driver_lidar_bk_32z.pro

@@ -0,0 +1,45 @@
+QT = core
+
+CONFIG += c++17 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 += \
+        ../driver_lidar_bk_16z/common/ioapi.cpp \
+        ../driver_lidar_bk_16z/common/ssFrameLib.cpp \
+        ../driver_lidar_bk_16z/lidar_bkdriver.cpp \
+        ../driver_lidar_bk_16z/lidar_bkz.cpp \
+        main.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 += \
+    ../driver_lidar_bk_16z/ICD_LiDAR_API.h \
+    ../driver_lidar_bk_16z/common/ioapi.h \
+    ../driver_lidar_bk_16z/common/ssFrameLib.h \
+    ../driver_lidar_bk_16z/common/utility.h \
+    ../driver_lidar_bk_16z/lidar_bkdriver.h \
+    ../driver_lidar_bk_16z/lidar_bkz.h
+
+INCLUDEPATH += ../driver_lidar_bk_16z/common
+INCLUDEPATH += ../driver_lidar_bk_16z
+
+INCLUDEPATH += /usr/include/eigen3
+
+!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!" )
+}
+

src/driver/driver_lidar_bk_32z/main.cpp

@@ -0,0 +1,285 @@
+#include <QCoreApplication>
+#include <thread>
+#include <iostream>
+
+#include "modulecomm.h"
+#include "xmlparam.h"
+
+
+#include <getopt.h>
+#include <yaml-cpp/yaml.h>   //if no this,sudo apt-get install libyaml-cpp-dev
+
+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_ctrlport[256];
+static char gstr_devip[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 2014"<<std::endl;
+    std::cout<<" -d --devport $port : port . eq.  -d 2030"<<std::endl;
+    std::cout<<" -c --ctrlport $port : port . eq.  -c 2013"<<std::endl;
+    std::cout<<" -e --devip $devip: devp.  eq. -e 192.168.0.10"<<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:d:c:e: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'},
+        {"ctrlport", required_argument, NULL, 'c'},
+        {"devip", required_argument, NULL, 'e'},
+        {"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 'c':
+            strncpy(gstr_ctrlport,optarg,255);
+            break;
+        case 'e':
+            strncpy(gstr_devip,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["devport"].as<std::string>().data(),255);
+    }
+
+    if(config["ctrlport"])
+    {
+        strncpy(gstr_ctrlport,config["ctrlport"].as<std::string>().data(),255);
+    }
+
+    if(config["devip"])
+    {
+        strncpy(gstr_devip,config["devip"].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;
+}
+
+
+#include "lidar_bkz.h"
+
+bool gbRun = true;
+lidar_bkz * gpdriver;
+void * gpa;
+std::thread * gpthead;
+
+void 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));
+    std::cout<<std::chrono::system_clock::now().time_since_epoch().count()<<" point cloud width: "<<point_cloud->width<<std::endl;
+
+    iv::modulecomm::ModuleSendMsg(pa,strOut,4+sizeof(point_cloud->header.frame_id.size()) + 4+8+point_cloud->width * sizeof(pcl::PointXYZI));
+
+    delete [] strOut;
+}
+
+void ThreadShare()
+{
+    while(gbRun)
+    {
+        pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud;
+        if(gpdriver->GetPointCloud(point_cloud) == 1)
+        {
+            //Share Point Cloud
+            sharepointcloud(gpa,point_cloud);
+        }
+    }
+}
+
+int main(int argc, char *argv[])
+{
+    QCoreApplication a(argc, argv);
+
+    snprintf(gstr_memname,255,"lidar_pc");
+    snprintf(gstr_rollang,255,"270.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,"2014");//默认端口号
+    snprintf(gstr_devport,255,"2030");//默认端口号
+    snprintf(gstr_ctrlport,255,"2013");//默认端口号
+    snprintf(gstr_devip,255,"192.168.0.10");
+    snprintf(gstr_yaml,255,"y");
+    snprintf(gstr_modulename,255,"driver_lidar_bk_16z");
+
+    int nRtn = GetOptLong(argc,argv);
+    if(nRtn == 1)  //show help,so exit.
+    {
+        return 0;
+    }
+
+    if(strnlen(gstr_yaml,255)>1)
+    {
+        decodeyaml(gstr_yaml);
+    }
+
+    gpdriver = new lidar_bkz("32z",gstr_devip,atoi(gstr_port),atoi(gstr_ctrlport),atoi(gstr_devport),10,atof(gstr_inclinationang_xaxis),atof(gstr_inclinationang_yaxis));
+    gpa = iv::modulecomm::RegisterSend(gstr_memname,10000000,1);
+    gpthead = new std::thread(&ThreadShare);
+
+
+    return a.exec();
+}

src/driver/driver_lidar_leishen32/main.cpp

@@ -34,7 +34,7 @@ void print_useage()
 //    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<<" -f --devport $devmode : devmode . 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;
 }

src/tool/pointcloudviewer/main.cpp

@@ -105,6 +105,9 @@ int user_data = 0;
 
 void ListenPointCloud(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
 {
+    (void)index;
+    (void)dt;
+    (void)strmemname;
     if(nSize <=16)return;
     unsigned int * pHeadSize = (unsigned int *)strdata;
     if(*pHeadSize > nSize)
@@ -126,7 +129,7 @@ void ListenPointCloud(const char * strdata,const unsigned int nSize,const unsign
     point_cloud->height = 1;
     point_cloud->width = 0;
     pcl::PointXYZI * p;
-    std::cout<<std::chrono::system_clock::now().time_since_epoch().count()/1000000 <<" size : "<<nPCount<<std::endl;
+    std::cout<<std::chrono::system_clock::now().time_since_epoch().count()/1000000 <<"ms size : "<<nPCount<<std::endl;
     p = (pcl::PointXYZI *)((char *)strdata + *pHeadSize);
     for(i=0;i<nPCount;i++)
     {

src/tool/pointcloudviewer/pointcloudviewer.pro

@@ -30,6 +30,7 @@ INCLUDEPATH += /usr/include/vtk-7.1
 INCLUDEPATH += /usr/include/pcl-1.10
 
 unix:INCLUDEPATH += /usr/include/pcl-1.12
+unix:INCLUDEPATH += /usr/include/pcl-1.14
 
 #unix:LIBS +=  -lpcl_common\
 #        -lpcl_features\
@@ -53,7 +54,7 @@ unix:INCLUDEPATH += /usr/include/pcl-1.12
 
 LIBS += -lpcl_visualization
 
-LIBS += -L/home/yuchuli/git/pcl/build/lib
+#LIBS += -L/home/yuchuli/git/pcl/build/lib
 
 
 
@@ -65,14 +66,22 @@ LIBS += -L$$PWD/../../../bin/ -lxmlparam -lmodulecomm -livlog -livfault
     error( "Couldn't find the common.pri file!" )
 }
 
+!include(../../../include/ivvtk.pri ) {
+    error( "Couldn't find the ivvtk.pri file!" )
+}
+
+
 LIBS += -lboost_system
 
+
+
+
 #LIBS += /home/yuchuli/git/pcl/build/lib/libpcl_visualization.so.1.14
 #LIBS += -lvtkCommonExecutionModel-9.1 -lvtkCommonCore-9.1 -lvtkRenderingLOD-9.1 -lvtkRenderingCore-9.1 \
 #       -lvtkFiltersSources-9.1
 
-LIBS += -lvtkCommonExecutionModel-7.1 -lvtkCommonCore-7.1 -lvtkRenderingLOD-7.1 -lvtkRenderingCore-7.1 \
-        -lvtkFiltersSources-7.1
+# LIBS += -lvtkCommonExecutionModel-7.1 -lvtkCommonCore-7.1 -lvtkRenderingLOD-7.1 -lvtkRenderingCore-7.1 \
+#         -lvtkFiltersSources-7.1
 
 
 #LIBS += -lvtkCommonExecutionModel-6.3 -lvtkCommonCore-6.3 -lvtkRenderingLOD-6.3 -lvtkRenderingCore-6.3 \