modularization/src/driver/driver_lidar_hesaipandarqt64/lidar_driver_hesaipandarqt64.cpp

#include <thread>
#include <iostream>
#include <QUdpSocket>
//#include <QNetworkDatagram>
#include <iostream>
#include <QMutex>
#include <QDateTime>

#include <QFile>


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

#include "modulecomm.h"

#include "lidar_driver_hesaipandarqt64.h"
#include "lidar_hesaipandarqt64_rawdata.h"

#include "ivexit.h"

#include "ivfault.h"
#include "ivlog.h"


#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


static std::thread * g_phesaipandar64Thread;
static std::thread * g_phesaipandar64ProcThread;



//float gV_theta[16] =  {-15,1,-13,3,-11,5,-9,7,-7,9,-5,11,-3,13,-1,15};   //16 Angles
//static float gV_theta[64] =  {14.7,10.85,7.877,4.875,2.858,1.846,1.678,1.506,1.34,1.169,1.002,0.831,0.664,0.493,0.326,0.155,-0.013,-0.182,-0.351,-0.519,-0.69,-0.857,-1.027,-1.195,-1.366,-1.533,-1.704,-1.87,-2.042,-2.21,-2.38,-2.547,-2.718,-2.882,-3.055,-3.222,-3.392,-3.557,-3.73,-3.894,-4.066,-4.232,-4.403,-4.567,-4.74,-4.902,-5.074,-5.239,-5.411,-5.573,-5.747,-5.908,-6.08,-6.243,-7.245,-8.241,-9.242,-10.227,-11.214,-12.188,-13.156,-14.112,-19.071,-25.079};   //64 Angles
static float gV_theta[64] = {-52.121,-49.785,-47.577,-45.477,-43.465,-41.528,-39.653,-37.831,
                             -36.055,-34.32,-32.619,-30.95,-29.308,-27.69,-26.094,-24.517,
                             -22.964,-21.42,-19.889,-18.372,-16.865,-15.368,-13.88,-12.399,
                             -10.925,-9.457,-7.994,-6.535,-5.079,-3.626,-2.175,-0.725,
                             0.725,2.175,3.626,5.079,6.534,7.993,9.456,10.923,
                             12.397,13.877,15.365,16.861,18.368,19.885,21.415,22.959,
                             24.524,26.101,27.697,29.315,30.957,32.627,34.328,36.064,
                             37.84,39.662,41.537,43.475,45.487,47.587,49.795,52.133};
static float gH_theta[64] = {8.736,8.314,7.964,7.669,7.417,7.198,7.007,6.838,
                             6.688,6.554,6.434,6.326,6.228,6.14,6.059,5.987,
                             -5.27,-5.216,-5.167,-5.123,-5.083,-5.047,-5.016,-4.988,
                             -4.963,-4.942,-4.924,-4.91,-4.898,-4.889,-4.884,-4.881,
                             5.493,5.496,5.502,5.512,5.525,5.541,5.561,5.584,
                             5.611,5.642,5.676,5.716,5.759,5.808,5.862,5.921,
                             -5.33,-5.396,-5.469,-5.55,-5.64,-5.74,-5.85,-5.974,
                             -6.113,-6.269,-6.447,-6.651,-6.887,-7.163,-7.493,-7.892};
//static float gH_theta[64] = {-1.042,-1.042,-1.042,-1.042,-1.042,-1.042,1.042,3.125,5.208,-5.208,-3.125,-1.042,1.042,3.125,5.208,-5.208,-3.125,-1.042,1.042,3.125,5.208,-5.208,-3.125,-1.042,1.042,3.125,5.208,-5.208,-3.125,-1.042,1.042,3.125,5.208,-5.208,-3.125,-1.042,1.042,3.125,5.208,-5.208,-3.125,-1.042,1.042,3.125,5.208,-5.208,-3.125,-1.042,1.042,3.125,5.208,-5.208,-3.125,-1.042,-1.042,-1.042,-1.042,-1.042,-1.042,-1.042,-1.042,-1.042,-1.042,-1.042};

static float gV_theta_cos[64],gV_theta_sin[64];

//static void * g_hesaipandar64_raw;

static void * g_lidar_pc;


static bool g_bhesaipandar64_run = false;
static bool g_bhesaipandar64_running = false;
static bool g_bhesaipandar64_Proc_running = false;

static int g_seq = 0;

static unsigned short glidar_port=2368;

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
//char gstr_hostip[256];
extern char gstr_port[256];
extern char gstr_yaml[256];
extern char gstr_calibfile[256];


extern iv::Ivfault * gIvfault;
extern iv::Ivlog * gIvlog;

/**
 * @brief The hesaipandar64_Buf class
 *  Use for Lidar UDP DATA Save
 */
class hesaipandar64_Buf
{
private:
    char * mstrdata;
    int mnSize;  //Data SizeUse
    QMutex mMutex;
    int mIndex;
public:
    hesaipandar64_Buf()
    {
        mstrdata = new char[BK32_DATA_BUFSIZE];
        mIndex = 0;
        mnSize = 0;
    }
    ~hesaipandar64_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_hesaipandarQT64 hesaipandarQT64_Buf ReadData data nRead = "<<nRead<<" is small"<<std::endl;
        }
        memcpy(strdata,mstrdata,nRtn);
        mnSize = 0;
        if(pnIndex != 0)*pnIndex = mIndex;
        mMutex.unlock();
        return nRtn;
    }
};



static hesaipandar64_Buf  * g_hesaipandar64_Buf;
static char * g_RawData_Buf;  //Buffer UDP Data
static int gnRawPos = 0;
static float gAngle_Old = 0;
static float gAngle_Total = 0;

static unsigned short gold = 0;

static int gnPac = 0;

#include <QTime>

static QTime gTime;

/**
 * @brief processhesaipandar64_Data
 * @param ba UDP Buffer
 * 1.UDP ByteArray Length is npacsize.
 * 2.if Angle is More than 360. Tell Another thread process.
 */
static void processhesaipandar64_Data(QByteArray ba)
{

    gnPac++;
    unsigned short * pAng;
    float fAng;

    char * pdata;
    pdata = ba.data();
//    const int npacsize = 1194;
    const int npacsize = 1072;
    if(ba.length() >= npacsize)
    {
//        pAng = (unsigned short *)(pdata+8);
        pAng = (unsigned short *)(pdata+6+6);
        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_hesaipandar64_Buf->WriteData(g_RawData_Buf,gnRawPos);
                lidar_hesaipandar64_raw * p = new lidar_hesaipandar64_raw();
                p->mnLen = gnRawPos;
                memcpy(p->mstrdata,g_RawData_Buf,gnRawPos);
//                iv::modulecomm::ModuleSendMsg(g_hesaipandar64_raw,(char *)p,sizeof(lidar_hesaipandar64_raw));
                delete p;
                memcpy(g_RawData_Buf,pdata,npacsize);
                gnRawPos = npacsize;
  //                  std::cout<<"index = "<<gnPac<<" time ="<<gTime.elapsed()<<" a cycle"<<std::endl;
                    gAngle_Total = 0;
            }
            else
            {
                if((gnRawPos+npacsize)<= BK32_DATA_BUFSIZE)
                {
                    memcpy(g_RawData_Buf+gnRawPos,pdata,npacsize);
                    gnRawPos= gnRawPos+npacsize;
                }
                else
                {
                    std::cout<<"lidar_hesaipandar64 processhesaipandar64_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,npacsize);
            gnRawPos = gnRawPos+npacsize;
        }
    }
    else
    {
        std::cout<<"lidar_hesaipandar64 processhesaipandar64_Data receive data packet len is not npacsize "<<std::endl;
    }
}




/**
 * @brief hesaipandar64_Func thread for receive udp socket
 * @param n
 */
static void hesaipandar64_Func(int n)
{

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

    QUdpSocket * udpSocket = new QUdpSocket( );
    udpSocket->bind(QHostAddress::Any, glidar_port);

    unsigned int ndatamisstime = 0;

    int nstate = 0;
    int nlaststate = 0;
    while(g_bhesaipandar64_run)
    {
        if(udpSocket->hasPendingDatagrams())
        {
            ndatamisstime = 0;
  //          std::cout<<"have data."<<std::endl;
            QByteArray datagram;
            datagram.resize(udpSocket->pendingDatagramSize());
            QHostAddress sender;
            quint16 senderPort;

            udpSocket->readDatagram(datagram.data(), datagram.size(),
                                            &sender, &senderPort);

//                    processTheDatagram(datagram);
  //          std::cout<<"have data."<<std::endl;
//            QNetworkDatagram datagram = udpSocket->receiveDatagram();
            processhesaipandar64_Data(datagram);
            datagram.clear();

        }
        else
        {
//            std::cout<<"running."<<std::endl;
            std::this_thread::sleep_for(std::chrono::milliseconds(1));
            if(ndatamisstime <  1000000) ndatamisstime++;
        }

        if(ndatamisstime > 1000)
        {
            nstate = 1;
        }
        if(ndatamisstime > 60000)
        {
            nstate = 2;
        }
        if(ndatamisstime < 100)
        {
            nstate = 0;
        }
        if(nlaststate != nstate)
        {
            nlaststate = nstate;
            switch (nstate) {
            case 0:
                gIvfault->SetFaultState(0,0,"OK");
                gIvlog->info("received udp data.device is ok.");
                break;
            case 1:
                gIvfault->SetFaultState(1,1,"No data");
                gIvlog->warn("more than 1 second no data. warning.");
                break;
            case 2:
                gIvfault->SetFaultState(2,2,"No data,Please Check Device or setting.");
                gIvlog->error("more than 60 seconds no data. error. Please check device or setting.");
                break;
            default:
                break;
            }
        }

    }
    udpSocket->close();
    delete udpSocket;
    g_bhesaipandar64_running = false;
    std::cout<<"hesaipandar64_Func Exit."<<std::endl;
}


/**
 * @brief process_hesaipandar64obs  Make PointCloud And Share
 * @param strdata  pointer to data
 * @param nLen  data length
 */
static void process_hesaipandar64obs(char * strdata,int nLen)
{

    double frollang = atof(gstr_rollang) *M_PI/180.0;    //Roll Angle
    double finclinationang_xaxis = atof(gstr_inclinationang_xaxis)*M_PI/180.0;  //Inclination from x axis
    double finclinationang_yaxis = 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;

    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.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;


//    const int npacsize = 1194;
    const int npacsize = 1072;
    int buf1len = nLen/npacsize;

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

//    double angdiff = ang2 - ang1;
//    if(angdiff<0)angdiff = angdiff + 360.0;
//    angdiff = angdiff/2.0;

    for (bag = 0; bag < buf1len; bag++)
    {
//        for (Group = 0; Group < 6; Group++)
        for (Group = 0; Group < 4; Group++)
        {
//            wt1 = ((pstr[bag*npacsize +9 + Group * 194] *256) + pstr[bag*npacsize + 8 + Group * 194]) ;
            wt1 = ((pstr[bag*npacsize +13 + Group * 258] *256) + pstr[bag*npacsize + 12 + Group * 258]) ;
            wt = wt1/ 100.0;

            for (pointi = 0; pointi <64; pointi++)
            {
                Ang = (0 - wt-gH_theta[pointi]) / 180.0 * Lidar_Pi;
//                Range = ((pstr[bag*npacsize + Group * 194 + 11 + 3 * pointi] << 8) + pstr[bag*npacsize+Group * 194 + 10 + 3 * pointi]);
                Range = ((pstr[bag*npacsize + Group * 258 + 15 + 4 * pointi] << 8) + pstr[bag*npacsize+Group * 258 + 14 + 4 * pointi]);
//                unsigned char intensity = pstr[bag*npacsize + Group * 194 + 12 + 3 * pointi];
                unsigned char intensity = pstr[bag*npacsize + Group * 258 + 16 + 4 * pointi];
                Range=Range* 0.004;


                pcl::PointXYZI point;
                point.x  = Range* gV_theta_cos[pointi]*cos(Ang + frollang);
                point.y  = Range* gV_theta_cos[pointi] *sin(Ang + frollang);
                point.z  = Range* gV_theta_sin[pointi];
                point.intensity = intensity;

                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;
                }
                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;
}


/**
 * @brief hesaipandar64_Proc_Func
 *    thread for process data to PointCloud
 * @param n
 */
static void hesaipandar64_Proc_Func(int n)
{
    std::cout<<"Enter hesaipandar64_Proc_Func"<<std::endl;
    char * strdata = new char[BK32_DATA_BUFSIZE];
    int nIndex;
    int nRead;
    while(g_bhesaipandar64_run)
    {
        if((nRead = g_hesaipandar64_Buf->ReadData(strdata,BK32_DATA_BUFSIZE,&nIndex))>0)
        {
            //process data.
            process_hesaipandar64obs(strdata,nRead);
        }
        else
        {
 //           std::cout<<"running."<<std::endl;
            std::this_thread::sleep_for(std::chrono::milliseconds(1));
        }
    }

    g_bhesaipandar64_Proc_running = false;
    std::cout<<"Exit hesaipandar64_Proc_Func"<<std::endl;
}



void exitfunc()
{
    StopLidar_hesaipandar64();

}


static void decodecsv(char * strpath)
{
    QFile xFile;
    xFile.setFileName(strpath);
    if(!xFile.open(QIODevice::ReadOnly))
    {
        std::cout<<"open csv file "<<strpath<<" fail."<<std::endl;
        return;
    }
    QByteArray ba = xFile.readAll();
    QString strdata = ba;
    QStringList strline = strdata.split("\n");
    const int nline= 64;
    if(strline.size()<(nline+1))
    {
        std::cout<<"File not complete. line size is "<<strline.size()<<std::endl;
        xFile.close();
        return;
    }
    int i;
    int nlinesize = strline.size();
    float * pxV,*pxH;
    pxV = new float[nline];
    pxH = new float[nline];
    bool bAllOK = true;
    for(i=1;i<(nline +1);i++)
    {
        QString strx = strline.at(i);
        QStringList strlistvalue = strx.split(",");
        if(strlistvalue.size()>=3)
        {

            pxV[i-1] = QString(strlistvalue.at(1)).toDouble();
            pxH[i-1] = QString(strlistvalue.at(2)).toDouble();
        }
        else
        {
            bAllOK = false;
        }
    }

    std::cout<<"Elevation:"<<std::endl;
    for(i=0;i<nline;i++)
    {
        std::cout<<pxV[i]<<",";
    }
    std::cout<<std::endl;
    std::cout<<"Azimuth:"<<std::endl;
    for(i=0;i<nline;i++)
    {
        std::cout<<pxH[i]<<",";
    }
    std::cout<<std::endl;

    if(bAllOK)
    {
        memcpy(gV_theta,pxV,64*sizeof(float));
        memcpy(gH_theta,pxH,64*sizeof(float));
    }
    else
    {
        std::cout<<"Not All  Data  OK. Please check."<<std::endl;
    }
    delete pxV;
    delete pxH;
    xFile.close();
}

/**
 * @brief StartLidar_hesaipandar64x Start
 * @return
 */
int LIDAR_DRIVER_HESAIPANDARQT64SHARED_EXPORT StartLidar_hesaipandar64()
{

    iv::ivexit::RegIVExitCall(exitfunc);

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

    decodecsv(gstr_calibfile);

    int i;
    for(i=0;i<64;i++)
    {
        gV_theta[i] = gV_theta[i]*M_PI/180.0;
        gV_theta_cos[i] = cos(gV_theta[i]);
        gV_theta_sin[i] = sin(gV_theta[i]);
    }

    g_hesaipandar64_Buf = new hesaipandar64_Buf();
    g_bhesaipandar64_run = true;
    g_bhesaipandar64_running = true;
    g_bhesaipandar64_Proc_running = true;

    glidar_port = atoi(gstr_port);

//    g_hesaipandar64_raw = iv::modulecomm::RegisterSend(strmemnameraw,10*sizeof(lidar_hesaipandar64_raw),10);
    g_lidar_pc = iv::modulecomm::RegisterSend(gstr_memname,20000000,1);
    g_phesaipandar64Thread = new std::thread(hesaipandar64_Func,0);
    g_phesaipandar64ProcThread = new std::thread(hesaipandar64_Proc_Func,0);

    return 0;
}

/**
 * @brief StopLidar_hesaipandar64 Stop
 */
void LIDAR_DRIVER_HESAIPANDARQT64SHARED_EXPORT StopLidar_hesaipandar64()
{
    std::cout<<"Now Close hesaipandar64. "<<std::endl;


    g_bhesaipandar64_run = false;
    g_phesaipandar64Thread->join();
    g_phesaipandar64ProcThread->join();

    iv::modulecomm::Unregister(g_lidar_pc);

    delete g_phesaipandar64ProcThread;
    delete g_phesaipandar64Thread;

    delete g_hesaipandar64_Buf;
    delete g_RawData_Buf;

}