modularization/src/driver/driver_lidar_rs16/lidar_driver_rs16.cpp

#include <thread>
#include <iostream>
#include <QUdpSocket>
#include <QNetworkDatagram>
#include <iostream>
#include <QMutex>
#include <QDateTime>
#include "math.h"
#include "Eigen/Dense"

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>


//#include <pcl/conversions.h>
//"the format of " << limitPath << " is not correct, use default data"

#include "modulecomm.h"

#include "lidar_driver_rs16.h"
#include "lidar_rs16_rawdata.h"


Eigen::Matrix3d rotation_matrix;
Eigen::Vector3d trans_matrix;


#ifdef VV7_1

int vv7;
#endif



#define Lidar_Pi 3.1415926535897932384626433832795
#define Lidar32 (unsigned long)3405883584//192.168.1.203
#define Lidar_roll_ang (90)*Lidar_Pi/180.0


std::thread * g_prs16Thread;
std::thread * g_prs16ProcThread;

void * g_rs16_raw;

void * g_lidar_pc;


bool g_brs16_run = false;
bool g_brs16_running = false;
bool g_brs16_Proc_running = false;

int g_seq = 0;

extern char gstr_memname[256];
extern char gstr_rollang[256];
extern char gstr_inclinationang_yaxis[256];  //from y axis
extern char gstr_inclinationang_xaxis[256];  //from x axis
extern char gstr_hostip[256];
extern char gstr_port[256];


class rs16_Buf
{
private:
    char * mstrdata;
    int mnSize;  //Data SizeUse
    QMutex mMutex;
    int mIndex;
public:
    rs16_Buf()
    {
        mstrdata = new char[BK32_DATA_BUFSIZE];
        mIndex = 0;
        mnSize = 0;
    }
    ~rs16_Buf()
    {
        delete mstrdata;
    }
    void WriteData(const char * strdata,const int nSize)
    {
        mMutex.lock();
        memcpy(mstrdata,strdata,nSize);
        mnSize = nSize;
        mIndex++;
        mMutex.unlock();
    }
    int ReadData(char * strdata,const int nRead,int * pnIndex)
    {
        int nRtn = 0;
        if(mnSize <= 0)return 0;
        mMutex.lock();
        nRtn = mnSize;
        if(nRtn >nRead)
        {
            nRtn = nRead;
            std::cout<<"lidar_rs16 rs16_Buf ReadData data nRead = "<<nRead<<" is small"<<std::endl;
        }
        memcpy(strdata,mstrdata,nRtn);
        mnSize = 0;
        if(pnIndex != 0)*pnIndex = mIndex;
        mMutex.unlock();
        return nRtn;
    }
};



rs16_Buf  * g_rs16_Buf;
char * g_RawData_Buf;
int gnRawPos = 0;
float gAngle_Old = 0;
float gAngle_Total = 0;

unsigned short gold = 0;

int gnPac = 0;

#include <QTime>

QTime gTime;

void processrs16_Data(QByteArray ba)
{
    gnPac++;
    unsigned short * pAng;
    float fAng;

    char * pdata;
    float fa,fb;
    pdata = ba.data();
    if(ba.length() == 1248)
    {
        std::cout<<"receive pac"<<std::endl;
      //  pAng = (unsigned short *)(pdata+2+42);
        fa = (float)(*((unsigned char *)(pdata+2+42)));
        fb = (float)(*((unsigned char *)(pdata+2+43)));
        fAng = fa*256+fb;
       // fAng = *pAng;
        fAng = fAng*0.01;
        if(fabs(fAng-gAngle_Old)>300)
        {
            gAngle_Total = gAngle_Total + fabs(fabs(fAng-gAngle_Old)-360);
        }
        else
        {
            gAngle_Total = gAngle_Total + fabs(fabs(fAng-gAngle_Old));
        }
            gAngle_Old = fAng;
            if(gAngle_Total > 360)
            {
                g_rs16_Buf->WriteData(g_RawData_Buf,gnRawPos);
//                lidar_rs16_raw * p = new lidar_rs16_raw();
//                p->mnLen = gnRawPos;
//                memcpy(p->mstrdata,g_RawData_Buf,gnRawPos);
//                g_rs16_raw->writemsg((char *)p,sizeof(lidar_rs16_raw));
//                delete p;
                memcpy(g_RawData_Buf,pdata+42,1206);
                gnRawPos = 1206;
  //                  std::cout<<"index = "<<gnPac<<" time ="<<gTime.elapsed()<<" a cycle"<<std::endl;
                    gAngle_Total = 0;
            }
            else
            {
                if((gnRawPos+1206)<= BK32_DATA_BUFSIZE)
                {
                    memcpy(g_RawData_Buf+gnRawPos,pdata+42,1206);
                    gnRawPos= gnRawPos+1206;
                }
                else
                {
                    std::cout<<"lidar_rs16 processrs16_Data data is very big gnRawPos = "<<gnRawPos<<std::endl;
                }
            }

 //       std::cout<<*pAng<<std::endl;

   //     gold = *pAng;
        if(gnRawPos == 0)
        {
            gAngle_Total = 0;
            gAngle_Old = *pAng;
            gAngle_Old = gAngle_Old*0.01;
            memcpy(g_RawData_Buf,pdata,1206);
            gnRawPos = gnRawPos+1206;
        }
    }
    else
    {
        std::cout<<"lidar_rs16 processrs16_Data receive data packet len is not 1206 "<<std::endl;
    }
}

void rs16_Func(int n)
{

    gTime.start();
    std::cout<<"Enter rs16_Func."<<std::endl;

    QUdpSocket * udpSocket = new QUdpSocket( );
  bool bbind =     udpSocket->bind(QHostAddress(gstr_hostip), atoi(gstr_port));
//    bool bbind = udpSocket->bind(QHostAddress("192.168.50.62"), 6699);
 //   udpSocket->bind(6699);
//     bool bbind = udpSocket->bind(QHostAddress::Any, 6699);

//      bool bbind = udpSocket->bind( 6699);
     int i = 0;
    while(g_brs16_run)
    {
        if(udpSocket->hasPendingDatagrams())
        {
            i++;std::cout<<"have data."<<i<<std::endl;
            QNetworkDatagram datagram = udpSocket->receiveDatagram();
            processrs16_Data(datagram.data());
            datagram.clear();
        }
        else
        {
//            std::cout<<"running."<<std::endl;
            std::this_thread::sleep_for(std::chrono::milliseconds(1));
        }
    }
    udpSocket->close();
    delete udpSocket;
    g_brs16_running = false;
    std::cout<<"rs16_Func Exit."<<std::endl;
}

void process_rs16obs(char * strdata,int nLen)
{

    double frollang = atof(gstr_rollang) *M_PI/180.0;
    double finclinationang_xaxis = atof(gstr_inclinationang_xaxis)*M_PI/180.0;
    double finclinationang_yaxis = atof(gstr_inclinationang_yaxis)*M_PI/180.0;
    bool binclix = false;
    bool bincliy = false;
    if(finclinationang_xaxis != 0.0)binclix = true;
    if(finclinationang_yaxis != 0.0)bincliy = true;

    double cos_finclinationang_xaxis = cos(finclinationang_xaxis);
    double sin_finclinationang_xaxis = sin(finclinationang_xaxis);
    double cos_finclinationang_yaxis = cos(finclinationang_yaxis);
    double sin_finclinationang_yaxis = sin(finclinationang_yaxis);

    QDateTime dt = QDateTime::currentDateTime();
    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
                new pcl::PointCloud<pcl::PointXYZI>());




//    point_cloud->header.stamp =
//            pcl_conversions::toPCL(sweep_data->header).stamp;
    point_cloud->header.frame_id = "velodyne";
    point_cloud->height = 1;
    point_cloud->header.stamp = dt.currentMSecsSinceEpoch();
    point_cloud->width = 0;
    point_cloud->header.seq =g_seq;
    g_seq++;

    unsigned char * pstr = (unsigned char *)strdata;

//    std::cout<<"enter obs."<<std::endl;
    float w = 0.0036;
    float Ang = 0;
    float Range = 0;
    int bag = 0;
    int Group = 0;
    int pointi = 0;
    float wt = 0;
    int wt1 = 0;

    Eigen::AngleAxisd r_z ( -0.0418, Eigen::Vector3d ( 0,0,1 ) ); //沿 Z 轴旋转 yaw   +
    Eigen::AngleAxisd r_y ( -0.0242, Eigen::Vector3d ( 0,1,0 ) ); //沿 Y 轴旋转 roll  +
    Eigen::AngleAxisd r_x ( -0.0137, Eigen::Vector3d ( 1,0,0 ) ); //沿 X 轴旋转 pitch -
    Eigen::Quaterniond q_zyx = r_z*r_y*r_x; //ZYX旋转顺序（绕旋转后的轴接着旋转）
    // 四元数-->>旋转矩阵
    rotation_matrix = q_zyx.toRotationMatrix();
    trans_matrix<<0, 0.85, 0.72;
//    int dx;
//    int dy;
//    float lidar_32_xdistance = 0.3;			//32线激光雷达X轴补偿
//    float lidar_32_ydistance = -2.3;			//32线激光雷达Y轴补偿


//    float H_BETA[32] = {
//        1.4,-4.2,1.4,-1.4,1.4,-1.4,4.2,-1.4,
//        1.4,-4.2,1.4,-1.4,4.2,-1.4,4.2,-1.4,
//        1.4,-4.2,1.4,-4.2,4.2,-1.4,1.4,-1.4,
//        1.4,-1.4,1.4,-4.2,4.2,-1.4,1.4,-1.4
//    };

    float V_theta[16] = {-15,-13,-11,-9,-7,-5,-3,-1,15,13,11,9,7,5,3,1};
//    float V_theta[16] =  {-15,1,-13,3,-11,5,-9,7,-7,9,-5,11,-3,13,-1,15};
//    float T[32] = {    0,  3.125,  1.5625,   4.6875,  6.25,  9.375,  7.8125,  10.9375,
//                    12.5,  15.625, 14.0625, 17.1875, 18.75, 21.875,  20.3125, 23.4375,
//                      25,  28.125, 26.5625, 29.6875, 31.25, 34.375, 32.8125,  35.9375,
//                    37.5,  40.625, 39.0625, 42.1875, 43.75, 46.875, 45.3125,  48.4375};


        int buf1len = nLen/1206;

        unsigned short * pAng;
        double ang1,ang2;
   //     pAng = ((pstr[2 + 0 * 100] *256) + pstr[ 3 + 0 * 100]) ;
        ang1 = ((pstr[2 + 0 * 100] *256) + pstr[ 3 + 0 * 100])/100.0;
        pAng = (unsigned short *)(strdata+2+1*100);
        ang2 = ((pstr[2 + 1 * 100] *256) + pstr[ 3 + 1 * 100])/100.0;

        double angdiff = ang2 - ang1;
//        std::cout<<"diff is "<<angdiff<<std::endl;
        //     if(angdiff<0)angdiff = angdiff + 360.0;
        angdiff = angdiff/2.0;

            for (bag = 0; bag < buf1len; bag++)
            {
                for (Group = 0; Group <= 11; Group++)
                {
                    wt1 = ((pstr[bag*1206 +2 + Group * 100] *256) + pstr[bag*1206 + 3 + Group * 100]) ;
                    wt = wt1/ 100.0;

                    for (pointi = 0; pointi <16; pointi++)
                    {
//                        Ang = (0 - wt - w * T[pointi] - H_BETA[pointi]+213) / 180.0 * Lidar_Pi;
                        Ang = (0 - wt) / 180.0 * Lidar_Pi;
                        Range = ((pstr[bag*1206 + Group * 100 + 4 + 3 * pointi] << 8) + pstr[bag*1206+Group * 100 + 5 + 3 * pointi]);
                        unsigned char intensity = pstr[bag*1206 + Group * 100 + 6 + 3 * pointi];
                        Range=Range* 5.0/1000.0;

                        if(Range<150)
                        {
                            pcl::PointXYZI point;
                            point.x  = Range*cos(V_theta[pointi] / 180 * Lidar_Pi)*cos(Ang + frollang);
                            point.y  = Range*cos(V_theta[pointi] / 180 * Lidar_Pi)*sin(Ang + frollang);
                            point.z  = Range*sin(V_theta[pointi] / 180 * Lidar_Pi);
                            if(point.z > 1.0) continue;
                            if(binclix)
                            {
                                double 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)
                            {
                                double 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;
                            }

                            Eigen::Vector3d new_point, old_point;
                            old_point<< point.x, point.y,point.z;
                            new_point = rotation_matrix * (old_point) + trans_matrix;
                            point.x = new_point[0];
                            point.y = new_point[1];
                            point.z = new_point[2];
                            point.intensity = intensity;
                            point_cloud->points.push_back(point);
                            ++point_cloud->width;
                        }
                    }


                    wt = wt + 0.18;



                    for (pointi = 0; pointi < 16; pointi++)
                    {
                        Ang = (0 - wt) / 180.0 * Lidar_Pi;
               //         Ang = Ang+angdiff;
                        Range = ((pstr[bag*1206 + Group * 100 + 52 + 3 * pointi] << 8) + pstr[bag*1206+Group * 100 + 53 + 3 * pointi]);
                        unsigned char intensity = pstr[bag*1206 + Group * 100 + 54 + 3 * pointi];
                        Range=Range* 5.0/1000.0;

                        if(Range<150)
                        {
                            pcl::PointXYZI point;
                            point.x  = Range*cos(V_theta[pointi] / 180 * Lidar_Pi)*cos(Ang + frollang);
                            point.y  = Range*cos(V_theta[pointi] / 180 * Lidar_Pi)*sin(Ang + frollang);
                            point.z  = Range*sin(V_theta[pointi] / 180 * Lidar_Pi);
                            if(binclix)
                            {
                                double 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)
                            {
                                double 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;
                            }

                            Eigen::Vector3d new_point, old_point;
                            old_point<< point.x, point.y,point.z;
                            new_point = rotation_matrix * (old_point) + trans_matrix;
                            point.x = new_point[0];
                            point.y = new_point[1];
                            point.z = new_point[2];

                            point.intensity = intensity;
                            point_cloud->points.push_back(point);


                            ++point_cloud->width;
                        }
                    }
                }
            }


            char * strOut = new char[4+sizeof(point_cloud->header.frame_id.size()) + 4+8+point_cloud->width * sizeof(pcl::PointXYZI)];

            int * pHeadSize = (int *)strOut;
            *pHeadSize = 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());
            pcl::uint32_t * pu32 = (pcl::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 = (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(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;
}


void  rs16_Proc_Func(int n)
{
    std::cout<<"Enter rs16_Proc_Func"<<std::endl;
    char * strdata = new char[BK32_DATA_BUFSIZE];
    int nIndex;
    int nRead;
    while(g_brs16_run)
    {
        if((nRead = g_rs16_Buf->ReadData(strdata,BK32_DATA_BUFSIZE,&nIndex))>0)
        {
            //process data.
            process_rs16obs(strdata,nRead);
        }
        else
        {
//            std::cout<<"running."<<std::endl;
            std::this_thread::sleep_for(std::chrono::milliseconds(1));
        }
    }

    g_brs16_Proc_running = false;
    std::cout<<"Exit rs16_Proc_Func"<<std::endl;
}


int LIDAR_DRIVER_RS16SHARED_EXPORT  StartLidar_rs16()
{

    std::cout<<"Now Start rs16 Listen."<<std::endl;
    g_RawData_Buf = new char[BK32_DATA_BUFSIZE];

    g_rs16_Buf = new rs16_Buf();
    g_brs16_run = true;
    g_brs16_running = true;
    g_brs16_Proc_running = true;

    g_rs16_raw = iv::modulecomm::RegisterSend("rs16_Raw",10*sizeof(lidar_rs16_raw),10);
    g_lidar_pc = iv::modulecomm::RegisterSend(gstr_memname,20000000,1);

    g_prs16Thread = new std::thread(rs16_Func,0);
    g_prs16ProcThread = new std::thread(rs16_Proc_Func,0);

    return 0;
}

void  LIDAR_DRIVER_RS16SHARED_EXPORT StopLidar_rs16()
{
    std::cout<<"Now Close rs16. "<<std::endl;
    g_brs16_run = false;
    g_prs16Thread->join();
    g_prs16ProcThread->join();

    delete g_prs16ProcThread;
    delete g_prs16Thread;

    delete g_rs16_Buf;
    delete g_RawData_Buf;

}