modularization/src/detection/detection_ndt_matching/main.cpp

#include <QCoreApplication>

#include "modulecomm.h"

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

#include <QTime>
#include <QDir>
#include <QFile>

#include <thread>

#include "ivfault.h"
#include "ivlog.h"
#include "ndt_matching.h"
#include "gpsimu.pb.h"

#include "ivexit.h"
#include "ivbacktrace.h"
#include "xmlparam.h"
#include "ivversion.h"

extern bool gbuseanh;


QTime gTime;    //a Time function. use calculate elapsed.

int gndttime;    //ndt calculate time

void * gpa,* gpb ,* gpc, * gpd;   //Modulecomm pointer

iv::Ivfault *gfault = nullptr;   //fault diag
iv::Ivlog *givlog = nullptr;   //log

std::vector<iv::ndtmaptrace> gvector_trace;  //trace and map origin. use shift map

iv::gpspos glastndtgpspos;    //last ndt pos(convert to gps)
iv::gpspos gcurndtgpspos;     //cur ndt pos(conver to gps)
iv::gpspos gcurgpsgpspos;    //cur gps pos
bool gbGPSFix = false;   //GPS is Fix. If 300 times report rtk 6.
int64_t gnLastGPSTime = 0; //ms

std::thread * gpthread;   //State thread pointer.

std::string gstr_lidarmsgname;   //lidar message name
std::string gstr_gpsmsgname;   // gps message name
std::string gstr_ndtposmsgname;  //ndtpos message name
std::string gstr_ndtgpsposmsgname;   //gpspos message name
std::string gstr_ndtmainsensormsgname;
std::string gstr_arm_x;
std::string gstr_arm_y;

std::string gstr_yawcorrect;   //lidar yaw correct.
std::string gstr_heightcorrect;  //gps height correct.
double gyawcorrect = 0.0;        //lidar yaw correct.
double gheightcorrect = 0.0;      //gps height correct.

std::string gstr_savelastpos; //set save last pos. default true
bool gbSaveLastPos = true;

double garm_x = 0.0;
double garm_y = 0.0;

QFile * gpFileLastPos = 0;//Save Last Positin

/**
 * @brief readtrace read trace
 * @param pFile
 * @return
 */
std::vector<iv::ndttracepoint> readtrace(QFile * pFile)
{
    std::vector<iv::ndttracepoint> ntp;
    QByteArray ba;
    ba = pFile->readLine();

    do
    {
        QString str;
        str = ba;
        QStringList strlist;
        strlist = str.split("\t");
        if(strlist.size() == 6)
        {
            iv::ndttracepoint tp;
            QString strx;
            int j;
            double fv[6];
            for(j=0;j<6;j++)
            {
                strx = strlist.at(j);
                fv[j] = strx.toDouble();
            }
            tp.x = fv[0];
            tp.y = fv[1];
            tp.z = fv[2];
            tp.pitch = fv[3];
            tp.roll = fv[4];
            tp.yaw = fv[5];
            ntp.push_back(tp);
        }
        else
        {
            qDebug("len is %d, %d",ba.length(),strlist.size());
        }
        ba = pFile->readLine();
    }while(ba.length()>0);

    return ntp;
}

/**
 * @brief readndtorigin read .pcd file's origin gps position.
 * @param pFile
 * @param pnori
 * @return
 */
int readndtorigin(QFile * pFile,iv::gpspos * pnori)
{
    int nrtn = -1;
    QByteArray ba;
    ba = pFile->readLine();
    QString str;
    str = ba;
    QStringList strlist;
    strlist = str.split("\t");
    if(strlist.size() == 6)
    {
        QString xstr[6];
        int i;
        for(i=0;i<6;i++)xstr[i] = strlist.at(i);
        pnori->lon = xstr[0].toDouble();
        pnori->lat = xstr[1].toDouble();
        pnori->height = xstr[2].toDouble();
        pnori->heading = xstr[3].toDouble();
        pnori->pitch = xstr[4].toDouble();
        pnori->roll = xstr[5].toDouble();
        nrtn = 0;
    }
    return nrtn;

}

/**
 * @brief LoadLastPos Load last position
 * @param strlastposfilepath
 */
static void LoadLastPos(const char * strlastposfilepath)
{
    QFile * xFile = new QFile();
    xFile->setFileName(strlastposfilepath);
    if(xFile->open(QIODevice::ReadWrite))
    {
        int nrtn = readndtorigin(xFile,&glastndtgpspos);
        if(nrtn < 0)
        {
            givlog->warn("load last pos fail.");
        }
        if(gbSaveLastPos)gpFileLastPos = xFile;
        else
        {
            gpFileLastPos = 0;
            xFile->close();
        }
    }
}

/**
 * @brief LoadTrace
 */
static void LoadTrace()
{
    std::string strpath = getenv("HOME");
    strpath = strpath + "/map/";

    std::string strlastposfile = strpath;
    strlastposfile = strlastposfile + "lastpos.txt";
    LoadLastPos(strlastposfile.data());

    QString strpathdir = strpath.data();
    QDir dir(strpath.data());
    QStringList xfilter;
    xfilter<<"*.pcd";
    foreach(QString strfilename,dir.entryList(xfilter,QDir::Files))
    {
        qDebug(strfilename.toLatin1().data());
        QString stroripath;
        QString strtracepath;
        QString strpcdpath;
        stroripath = strfilename;
        stroripath = stroripath.left(stroripath.length() - 4);
        stroripath = strpathdir +  stroripath + ".ori";
        strtracepath = strfilename;
        strtracepath = strtracepath.left(strtracepath.length() - 4);
        strtracepath = strpathdir +  strtracepath + ".txt";
        strpcdpath = strfilename;
        strpcdpath = strpathdir + strpcdpath;
        QFile xFileori,xFile,xFilepcd;

        xFileori.setFileName(stroripath);
        xFilepcd.setFileName(strpcdpath);
        xFile.setFileName(strtracepath);
        if(xFileori.exists() && xFile.exists())
        {
            qDebug("file name ok.");
            if(xFile.open(QIODevice::ReadOnly) && xFileori.open(QIODevice::ReadOnly))
            {
                std::vector<iv::ndttracepoint> xv =  readtrace(&xFile);
                iv::gpspos xnori;
                int nrtn = readndtorigin(&xFileori,&xnori);
                if((xv.size() > 0)&&(nrtn == 0))
                {
                    iv::ndtmaptrace nmt;
                    nmt.mvector_trace = xv;
                    nmt.mstrpcdpath = strpcdpath.toLatin1().data();
                    nmt.mndtorigin = xnori;
                    gvector_trace.push_back(nmt);
                    qDebug("this is ok.");
                }

            }
            xFile.close();
            xFileori.close();

        }
        else
        {
            qDebug(" file not ok.");
        }
    }
    return;
}

int gnothavedatatime = 0;
/**
 * @brief ListenPointCloud
 * @param strdata
 * @param nSize
 * @param index
 * @param dt
 * @param strmemname
 */
void ListenPointCloud(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{

    QTime xTime;
    xTime.start();

    gnothavedatatime = 0;
    if(nSize <=16)return;
    unsigned int * pHeadSize = (unsigned int *)strdata;
    if(*pHeadSize > nSize)
    {
        std::cout<<"ListenPointCloud data is small headsize ="<<*pHeadSize<<"  data size is"<<nSize<<std::endl;
    }


    pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud(
                new pcl::PointCloud<pcl::PointXYZ>());
    int nNameSize;
    nNameSize = *pHeadSize - 4-4-8;
    char * strName = new char[nNameSize+1];strName[nNameSize] = 0;
    memcpy(strName,(char *)((char *)strdata +4),nNameSize);
    point_cloud->header.frame_id = strName;
    memcpy(&point_cloud->header.seq,(char *)strdata+4+nNameSize,4);
    memcpy(&point_cloud->header.stamp,(char *)strdata+4+nNameSize+4,8);
    int nPCount = (nSize - *pHeadSize)/sizeof(pcl::PointXYZI);
    int i;
    pcl::PointXYZI * p;
    p = (pcl::PointXYZI *)((char *)strdata + *pHeadSize);
    for(i=0;i<nPCount;i++)
    {
        pcl::PointXYZ xp;
        xp.x = p->y;
        xp.y = p->x * (-1.0);
        xp.z = p->z;

        if(gyawcorrect != 0.0)
        {
            double yaw =  gyawcorrect;
            double x1,y1;
            x1 = xp.x;
            y1 = xp.y;
            xp.x = x1*cos(yaw) -y1*sin(yaw);
            xp.y = x1*sin(yaw) + y1*cos(yaw);

        }

 //       xp.intensity = p->intensity;
        point_cloud->push_back(xp);
        p++;
    }

    givlog->verbose("point cloud size is %d",
                    point_cloud->size());



    point_ndt(point_cloud);
    gndttime = xTime.elapsed();
    givlog->verbose("ndt time is %d, gtime is %d", xTime.elapsed(), gTime.elapsed());
 //   return;


 //   gw->UpdatePointCloud(point_cloud);
 //   DBSCAN_PC(point_cloud);

    gfault->SetFaultState(0, 0, "ok");
}

/**
 * @brief ListenMapUpdate
 * @param strdata
 * @param nSize
 * @param index
 * @param dt
 * @param strmemname
 */
void ListenMapUpdate(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    char * strpath = new char[nSize+1];
    memcpy(strpath,strdata,nSize);
    strpath[nSize] = 0;

    pcl::PointCloud<pcl::PointXYZ>::Ptr mapx = boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>>(new pcl::PointCloud<pcl::PointXYZ>);;
    if(0 == pcl::io::loadPCDFile(strpath,*mapx))
    {
        int size = mapx->size();
        givlog->verbose("mapx size = %d", size);
        ndt_match_SetMap(mapx);

    }

    gfault->SetFaultState(0, 0, "ok");
    delete strpath;
}

/**
 * @brief ListenNDTRunstate
 * @param strdata
 * @param nSize
 * @param index
 * @param dt
 * @param strmemname
 */
void ListenNDTRunstate(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    bool bState;
    if(nSize < sizeof(bool))
    {
        gfault->SetFaultState(1, 0, "ListenNDTRunstate data size is small");
        givlog->error("ListenNDTRunstate data size is small");
        return;
    }
    memcpy(&bState,strdata,sizeof(bool));

    SetRunState(bState);
    gfault->SetFaultState(0, 0, "ok");
}

/**
 * @brief pausecomm
 */
void pausecomm()
{
    iv::modulecomm::PauseComm(gpa);
    iv::modulecomm::PauseComm(gpb);
    iv::modulecomm::PauseComm(gpc);
    iv::modulecomm::PauseComm(gpd);
}

/**
 * @brief continuecomm
 */
void continuecomm()
{
    iv::modulecomm::ContintuComm(gpa);
    iv::modulecomm::ContintuComm(gpb);
    iv::modulecomm::ContintuComm(gpc);
    iv::modulecomm::ContintuComm(gpd);
}

/**
 * @brief ListenRaw
 * @param strdata
 * @param nSize
 * @param index
 * @param dt
 * @param strmemname
 */
void ListenRaw(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    static unsigned int nFixCount = 0;
    iv::gps::gpsimu xgpsimu;
    if(!xgpsimu.ParseFromArray(strdata,nSize))
    {
        std::cout<<"ListenRaw Parse error."<<std::endl;
    }

    gcurgpsgpspos.lat = xgpsimu.lat();
    gcurgpsgpspos.lon = xgpsimu.lon();
    gcurgpsgpspos.height = xgpsimu.height() + gheightcorrect;
    gcurgpsgpspos.heading = xgpsimu.heading();
    gcurgpsgpspos.pitch = xgpsimu.pitch();
    gcurgpsgpspos.roll = xgpsimu.roll();
    gcurgpsgpspos.ve = xgpsimu.ve();
    gcurgpsgpspos.vn = xgpsimu.vn();

    givlog->verbose("gps lat:%11.7f lon:%11.7f heading:%11.7f height:%6.3f rtk:%d",
                    xgpsimu.lat(),xgpsimu.lon(),xgpsimu.heading(),
                    xgpsimu.height(),xgpsimu.rtk_state());

    if(xgpsimu.has_acce_x())
    {
        setuseimu(true);
        imu_update(xgpsimu.time(),
                   xgpsimu.roll(),xgpsimu.pitch(),
                   xgpsimu.heading(),xgpsimu.acce_x(),
                   xgpsimu.acce_y(),xgpsimu.acce_z());
    }


    if(xgpsimu.rtk_state() == 6)
    {
        nFixCount++;
    }
    else
    {
        nFixCount = 0;
    }

    if(nFixCount < 300)gbGPSFix = false;
    else gbGPSFix = true;

    gnLastGPSTime = std::chrono::system_clock::now().time_since_epoch().count()/1000000;

}


bool gbstate = true;

/**
 * @brief statethread Monitor ndt state thread.
 */
void statethread()
{
    int nstate = 0;
    int nlaststate = 0;
    while(gbstate)
    {
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
        if(gnothavedatatime < 100000) gnothavedatatime++;

        if(gnothavedatatime < 100)
        {
            nstate = 0;
        }
        if(gnothavedatatime > 1000)
        {
            nstate = 1;
        }
        if(gnothavedatatime > 6000)
        {
            nstate = 2;
        }
        if(nstate != nlaststate)
        {
            nlaststate = nstate;
            switch (nstate) {
            case 0:
                givlog->info("ndt matching  is ok");
                gfault->SetFaultState(0,0,"data is ok.");
                break;
            case 1:
                givlog->warn("more than 10 seconds not have lidar pointcloud.");
                gfault->SetFaultState(1,1,"more than 10 seconds not have lidar pointcloud.");
                break;
            case 2:
                givlog->error("more than 60 seconds not have lidar pointcloud.");
                gfault->SetFaultState(2,2,"more than 60 seconds not have lidar pointcloud.");
                break;
            default:
                break;
            }
        }
    }
}

/**
 * @brief exitfunc when receive exit command.
 */
void exitfunc()
{
    std::cout<<"enter exitfunc."<<std::endl;
    gbstate = false;
    gpthread->join();
    iv::modulecomm::Unregister(gpa);
    iv::modulecomm::Unregister(gpb);
    iv::modulecomm::Unregister(gpc);
    iv::modulecomm::Unregister(gpd);
    if(gpFileLastPos != 0)gpFileLastPos->close();
    std::cout<<"Complete exitfunc."<<std::endl;
}

int main(int argc, char *argv[])
{
    showversion("detection_ndt_matching");
    QCoreApplication a(argc, argv);

    QString strpath = QCoreApplication::applicationDirPath();

   if(argc < 2)
       strpath = strpath + "/detection_ndt_matching.xml";
   else
       strpath = argv[1];
   std::cout<<strpath.toStdString()<<std::endl;

   iv::xmlparam::Xmlparam xparam(strpath.toStdString());

   gstr_lidarmsgname = xparam.GetParam("lidarmsg","lidar_pc");
   gstr_gpsmsgname = xparam.GetParam("gpsmsg","hcp2_gpsimu");
   gstr_ndtposmsgname = xparam.GetParam("ndtposmsg","ndtpos");
   gstr_ndtgpsposmsgname = xparam.GetParam("ndtgpsposmsg","ndtgpspos");
   gstr_ndtmainsensormsgname = xparam.GetParam("mainsensor","gps");

   gstr_yawcorrect = xparam.GetParam("AngleCorrect","0.0");
   gstr_heightcorrect = xparam.GetParam("HeightCorrect","0.0");
   gstr_savelastpos = xparam.GetParam("SaveLastPos","true");

   gstr_arm_x = xparam.GetParam("Arm_LidarGPSBase_x","0.0");
   gstr_arm_y = xparam.GetParam("Arm_LidarGPSBase_y","0.0");

   gbuseanh = xparam.GetParam("useanh",false);

   gyawcorrect = atof(gstr_yawcorrect.data()) * M_PI/180.0;
   gheightcorrect = atof(gstr_heightcorrect.data());

   if(gstr_savelastpos == "true")gbSaveLastPos = true;
   else gbSaveLastPos = false;

   garm_x = atof(gstr_arm_x.data());
   garm_y = atof(gstr_arm_y.data());


   std::cout<<"lidar message is "<<gstr_lidarmsgname<<std::endl;
   std::cout<<"gps message is "<<gstr_gpsmsgname<<std::endl;
   std::cout<<"ndtpos message is "<<gstr_ndtposmsgname<<std::endl;
   std::cout<<"ndtgpspos message is "<<gstr_ndtgpsposmsgname<<std::endl;
   std::cout<<"AngleCorrect  is "<<gstr_yawcorrect<<std::endl;
   std::cout<<"HeightCorrect is "<<gstr_heightcorrect<<std::endl;
   std::cout<<"SaveLastPos is "<<gstr_savelastpos<<std::endl;
   std::cout<<"Arm_LidarGPSBase_x is "<<gstr_arm_x<<std::endl;
   std::cout<<"Arm_LidarGPSBase_x is "<<gstr_arm_y<<std::endl;


    gfault = new iv::Ivfault("detection_ndt_matching_gpu");
    givlog = new iv::Ivlog("detection_ndt_matching_gpu");

    gfault->SetFaultState(0,0,"Initialize.");

    glastndtgpspos.lat = 39;
    glastndtgpspos.lon = 119;
    glastndtgpspos.heading = 0;
    glastndtgpspos.height = 0;
    glastndtgpspos.pitch = 0;
    glastndtgpspos.roll = 0;
    gcurndtgpspos = glastndtgpspos;

    iv::gpspos xpos = glastndtgpspos;
    xpos.lat = 39.1;
    xpos.heading = 90;
    pose xp = CalcPose(glastndtgpspos,xpos);
    iv::gpspos xx = PoseToGPS(glastndtgpspos,xp);
    LoadTrace();
    std::cout<<"trace size  is "<<gvector_trace.size()<<std::endl;

    gpa = iv::modulecomm::RegisterRecv(gstr_lidarmsgname.data(),ListenPointCloud);
    gpb = iv::modulecomm::RegisterRecv(gstr_gpsmsgname.data(),ListenRaw);
    gpc = iv::modulecomm::RegisterSend(gstr_ndtposmsgname.data(),10000,1);
    gpd = iv::modulecomm::RegisterSend(gstr_ndtgpsposmsgname.data(),10000,1);
    ndt_match_Init_nomap();

    iv::ivexit::RegIVExitCall(exitfunc);
    RegisterIVBackTrace();

    gpthread = new std::thread(statethread);
//    gpb = iv::modulecomm::RegisterRecv("ndt_mappath",ListenMapUpdate);
 //   gpc = iv::modulecomm::RegisterRecv("ndt_runstate",ListenNDTRunstate);



    return a.exec();
}