change driver_lidar_leishen32. test ok.

src/driver/driver_lidar_leishen32/driver_lidar_leishen32.pro

@@ -3,7 +3,7 @@ QT -= gui
 
 QT       += network
 
-CONFIG += c++14 console
+CONFIG += c++14 #console
 CONFIG -= app_bundle
 
 # The following define makes your compiler emit warnings if you use

src/driver/driver_lidar_leishen32/leishen32.cpp

@@ -7,6 +7,17 @@ leishen32::leishen32(char * strmemname,double froll,double finclinationang_xaxis
                      double finclinationang_yaxis , int nDevMode ,
                      unsigned short nDataPort,unsigned short nDevPort )
 {
+    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;
@@ -21,6 +32,7 @@ leishen32::leishen32(char * strmemname,double froll,double finclinationang_xaxis
     {
         mthetacos[i] = cos(mpleishen32_vertical[i]*M_PI/180.0);
         mthetasin[i] = sin(mpleishen32_vertical[i]*M_PI/180.0);
+        std::cout<<"sin "<<i<<" "<<mthetasin[i]<<std::endl;
     }
 
     mplidarudp = new lidarudp(nDataPort);
@@ -102,24 +114,15 @@ int leishen32::DecodeUDPPac(iv::lidarudppac & xpac)
     float cos_finclinationang_yaxis = static_cast<float>(cos(finclinationang_yaxis));
     float sin_finclinationang_yaxis = static_cast<float>(sin(finclinationang_yaxis));
 
-    double frollang = mfrollang;
+    double frollang = mfrollang *M_PI/180.0;
 
     char * pstr =  static_cast<char * >(xpac.mdata_ptr.get()) ;
 
     static double gAngleTotal = 0;
     static double gAngleOld = 0;
-    static unsigned int g_seq = 0;
+    static unsigned int g_seq = 1;
 
-    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;
 
     int i;
     for(i=0;i<12;i++)
@@ -152,6 +155,7 @@ int leishen32::DecodeUDPPac(iv::lidarudppac & xpac)
             gAngleTotal = 0.0;
             std::cout<< std::chrono::system_clock::now().time_since_epoch().count()/1000000 <<" share point cloud."<<std::endl;
             ProduceCloud(mpoint_cloud_temp);
+            std::cout<<" produce. "<<std::endl;
             //share point cloud.
 
             pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
@@ -169,14 +173,16 @@ int leishen32::DecodeUDPPac(iv::lidarudppac & xpac)
         int j;
         for(j=0;j<32;j++)
         {
-            double Range = ((pstrpac[4+j*3] << 8) + pstrpac[4+j*3+1]);
+            double Range = ((pstrpac[4+j*3+1] << 8) + pstrpac[4+j*3+0]);
             unsigned char intensity = static_cast<unsigned char>(pstr[4+j*3+2]) ;
-            Range=Range* 0.0025;
+            Range=Range* 0.004;
 
 
             pcl::PointXYZI point;
-            point.x  = static_cast<float>(Range* mthetacos[j]*cos(fazu + mpleishen32_A[j] + frollang)) ;
-            point.y  = static_cast<float>(Range* mthetacos[j] *sin(fazu + mpleishen32_A[j]  + frollang));
+ //           std::cout<<" fazu: "<<fazu<<" cos theta : "<<mthetacos[j] <<" range."<<Range<< std::endl;
+
+            point.x  = static_cast<float>(Range* mthetacos[j]*cos(frollang -fazu - mpleishen32_A[j] *M_PI/180.0  )) ;
+            point.y  = static_cast<float>(Range* mthetacos[j] *sin(frollang -fazu - mpleishen32_A[j]*M_PI/180.0 ));
             point.z  = static_cast<float>(Range* mthetasin[j]);
             point.intensity = intensity;
 
@@ -194,6 +200,7 @@ int leishen32::DecodeUDPPac(iv::lidarudppac & xpac)
                 point.z = z*cos_finclinationang_yaxis + x*sin_finclinationang_yaxis;
                 point.x = x*cos_finclinationang_yaxis - z*sin_finclinationang_yaxis;
             }
+  //          std::cout<<" x: "<<point.x<<" y: "<<point.y<<std::endl;
 
             mpoint_cloud_temp->points.push_back(point);
 
@@ -238,13 +245,13 @@ int leishen32::DecodeDevinfo(iv::lidarudppac & xpac)
     {
         unsigned short * pA = reinterpret_cast<unsigned short *>(pstr+186+i*2);
         unsigned short Avalue = *pA;
-        A[i] = Avalue;
+        A[i] =((pstr[186+i*2+0] << 8) + pstr[186+i*2+1]);   //Avalue;
         A[i] = A[i]*0.01;
     }
 
     mdevinfo.A1 = A[0];
-    mdevinfo.A2 = A[1];
-    mdevinfo.A3 = A[2];
+    mdevinfo.A2 = A[2];
+    mdevinfo.A3 = A[1];
     mdevinfo.A4 = A[3];
 
     if(mnLidarMode == 0)

src/driver/driver_lidar_leishen32/leishen32.h

@@ -66,8 +66,8 @@ private:
 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};
+                                    -8,8,-7,9,-6,10,-5,11,
+                                    -4,12,-3,13,-2,14,-1,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,

src/driver/driver_lidar_leishen32/lidardecode.cpp

@@ -42,6 +42,7 @@ void lidardecode::sharepointcloud(void * pa,pcl::PointCloud<pcl::PointXYZI>::Ptr
     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<<"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));
 

src/driver/driver_lidar_leishen32/main.cpp

@@ -49,7 +49,7 @@ int  GetOptLong(int argc, char *argv[]) {
     // 第几个元素的描述，即是long_opts的下标值
     int option_index = 0;
     // 设置短参数类型及是否需要参数
-    const char *optstring = "n:m:r:x:y:p:s:h";
+    const char *optstring = "n:m:r:x:y:p:d:f:s:h";
 
     // 设置长参数类型及其简写，比如 --reqarg <==>-r
     /*
@@ -204,7 +204,7 @@ int main(int argc, char *argv[])
     QCoreApplication a(argc, argv);
 
     snprintf(gstr_memname,255,"lidar_pc");
-    snprintf(gstr_rollang,255,"0.0");//这个角度要根据安装情况进行校正！！！！！！！！
+    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");