modularization/src/fusion/fusion_gpsndt/main.cpp

#include <QCoreApplication>
#include <QMutex>

#include <QUdpSocket>
#include <QNetworkDatagram>

#include <thread>
#include <iostream>
#include <memory>

#include <math.h>

#include "modulecomm.h"
#include "xmlparam.h"
#include "gpsimu.pb.h"
#include "ndtgpspos.pb.h"
#include "fusiongpslidar.pb.h"
#include "chassis.pb.h"

#include "ivbacktrace.h"
#include "ivfault.h"
#include "ivlog.h"
#include "ivexit.h"
#include "ivversion.h"

iv::Ivfault *gfault = nullptr;
iv::Ivlog *givlog = nullptr;

bool gbinit = false;
bool gbGPSFix = false;

bool gbfusionrun = true;

bool gbGPSNewData = false;

bool gbNDTNewData = false;

bool gbUseLidarOnly = false;

bool gbShareUDP = true;

bool gbShareGPSIMU = true;

std::thread * gfusionthread;

iv::gps::gpsimu ggpsimu;
iv::lidar::ndtgpspos gndtgpspos;
iv::fusion::fusiongpslidar gfusiongpslidar;
QMutex mMutexgps;
QMutex mMutexndt;


void * gpa,* gpb, * gpc, * gpd,* gpe;

std::string gstrgpsmsg;
std::string gstrndtmsg;
std::string gstrfusionmsg;
std::string gstrlidaronly;
std::string gstrfusiongpsimumsg;
std::string gstrshareudp;
std::string gstrsharegpsimu;

QUdpSocket * gudpSocketGPSIMU;



static int64_t glastchassitime = 0;
static double gfChassisVelSpeed;


void UpdateChassis(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
{
    (void)index;
    (void)dt;
    (void)strmemname;
    iv::chassis xchassis;
    //    static int ncount = 0;
    if(!xchassis.ParseFromArray(strdata,nSize))
    {
        std::cout<<"iv::decition::BrainDecition::UpdateChassis ParseFrom Array Error."<<std::endl;
        return;
    }


    if(xchassis.has_vel())
    {
        gfChassisVelSpeed = xchassis.vel();
        glastchassitime = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
  //      std::cout<<" gf Veh speed : "<<gfVehSpd<<std::endl;
    }

}

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

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

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

    if(gbinit == false)
    {
        if(nFixCount > 0)
        {
            gbGPSFix = true;
            nFixCount = 300;
        }
        gbinit = true;
    }
    if(nFixCount < 300)gbGPSFix = false;
    else gbGPSFix = true;

    mMutexgps.lock();
    ggpsimu.CopyFrom(xgpsimu);
    mMutexgps.unlock();

    gbGPSNewData = true;


}

void ListenNDTGPS(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{

    iv::lidar::ndtgpspos xndtgpspos;
    if(!xndtgpspos.ParseFromArray(strdata,nSize))
    {
        std::cout<<"ListenNDTGPS Parse error."<<std::endl;
        return;
    }

    givlog->verbose("NDT","lat:%11.7f lon:%11.7f height:%6.3f heading:%6.3f trans_prob:%6.3f score:%6.3f",
                        xndtgpspos.lat(),xndtgpspos.lon(),xndtgpspos.height(),
                    xndtgpspos.heading(),xndtgpspos.tras_prob(),xndtgpspos.score());

    mMutexndt.lock();
    gndtgpspos.CopyFrom(xndtgpspos);
    mMutexndt.unlock();
    gbNDTNewData = true;
}


static void BroadCastData(iv::fusion::fusiongpslidar * pfusiongpslidar)
{
    unsigned char strSend[72];
    int i;
    for(i=0;i<72;i++)strSend[i] = 0x0;
    strSend[0] = 0xE7;

    strSend[21] = 4;
    strSend[68] = pfusiongpslidar->state();
    strSend[67] = 0;
    strSend[62] = 62;

    double fV = pfusiongpslidar->lat();
    fV = fV/(180.0/M_PI);
    memcpy(strSend+23,&fV,8);
    fV = pfusiongpslidar->lon();fV = fV/(180.0/M_PI);memcpy(strSend+31,&fV,8);
    float fF =0;memcpy(strSend+39,&fF,4);

    int iV;
    char * piV = (char *)&iV;
    piV++;
    fV = pfusiongpslidar->ve();fV = fV*10000.0;iV = (int)fV;iV = iV*256;memcpy(strSend+43,piV,3);
    fV = pfusiongpslidar->vn();fV = fV*10000.0;iV = (int)fV;iV = iV*256;memcpy(strSend+46,piV,3);
    fV = pfusiongpslidar->vd();fV = fV*10000.0;iV = (int)fV;iV = iV*256;memcpy(strSend+49,piV,3);

    fV = pfusiongpslidar->heading()/(180.0/M_PI);fV = fV*1000000.0;iV = (int)fV;iV = iV*256;memcpy(strSend+52,piV,3);
    fV = pfusiongpslidar->pitch()/(180.0/M_PI);fV = fV*1000000.0;iV = (int)fV;iV = iV*256;memcpy(strSend+55,piV,3);
    fV = pfusiongpslidar->roll()/(180.0/M_PI);fV = fV*1000000.0;iV = (int)fV;iV = iV*256;memcpy(strSend+58,piV,3);

    char c;
    c = strSend[0];
    for(i=1;i<71;i++)c = c+strSend[i];
    strSend[71]= c;

    gudpSocketGPSIMU->writeDatagram((char *)strSend,72,QHostAddress::Broadcast, 3000);

}

static void sharefusiongpsimu(iv::fusion::fusiongpslidar * pfusiongpslidar)
{
    iv::gps::gpsimu gpsimu;
    gpsimu.set_vd(pfusiongpslidar->vd());
    gpsimu.set_ve(pfusiongpslidar->ve());
    gpsimu.set_vn(pfusiongpslidar->vn());
    gpsimu.set_lat(pfusiongpslidar->lat());
    gpsimu.set_lon(pfusiongpslidar->lon());
    gpsimu.set_heading(pfusiongpslidar->heading());
    gpsimu.set_state(4);
    gpsimu.set_msgtime(QDateTime::currentMSecsSinceEpoch());
    gpsimu.set_roll(pfusiongpslidar->roll());
    gpsimu.set_pitch(pfusiongpslidar->pitch());


    gpsimu.set_rtk_state(pfusiongpslidar->state());
    gpsimu.set_ins_state(4);

    int64_t nmsnow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
    if(abs(nmsnow - glastchassitime) < 1000)
    {
        gpsimu.set_speed(gfChassisVelSpeed);
        std::cout<<"use chassis speed."<<std::endl;
        return;
    }

    char * strser;
    bool bser;
    int nbytesize;
    nbytesize = gpsimu.ByteSize();
    strser = new char[nbytesize];
    bser = gpsimu.SerializeToArray(strser,nbytesize);
    if(bser)
        iv::modulecomm::ModuleSendMsg(gpd,strser,nbytesize);
    else
    {
//        givlog->error("gpsimu serialize error.");
//        gfault->SetFaultState(1, 0, "gpsimu serialize err");
    }
    delete strser;
}


static void sharefusionres()
{
//    std::cout<<"state is "<<gfusiongpslidar.state()<<std::endl;
    qDebug("lat %11.7f lon %11.7f heading %6.3f ve %6.3f vn %6.3f state %g",
           gfusiongpslidar.lat(),gfusiongpslidar.lon(),gfusiongpslidar.heading(),
           gfusiongpslidar.ve(),gfusiongpslidar.vn(),gfusiongpslidar.state());

    givlog->debug("FUSION_GPSNDT","lat %11.7f lon %11.7f heading %6.3f ve %6.3f vn %6.3f state %g",
                  gfusiongpslidar.lat(),gfusiongpslidar.lon(),gfusiongpslidar.heading(),
                  gfusiongpslidar.ve(),gfusiongpslidar.vn(),gfusiongpslidar.state());

    int ndatasize = gfusiongpslidar.ByteSize();
    char * strser = new char[ndatasize];
    std::shared_ptr<char> pstrser; pstrser.reset(strser);
    if(gfusiongpslidar.SerializeToArray(strser,ndatasize))
    {
        iv::modulecomm::ModuleSendMsg(gpc,strser,ndatasize);
    }
    else
    {
        std::cout<<"sharefusionres serialize error."<<std::endl;
        givlog->warn("sharefusionres serialize error.");
    }

    if(gbShareUDP)BroadCastData(&gfusiongpslidar);
    if(gbShareGPSIMU)sharefusiongpsimu(&gfusiongpslidar);
}

static void usegps(iv::gps::gpsimu * pgpsimu)
{
    gfusiongpslidar.set_heading(pgpsimu->heading());
    gfusiongpslidar.set_height(pgpsimu->height());
    gfusiongpslidar.set_lat(pgpsimu->lat());
    gfusiongpslidar.set_lon(pgpsimu->lon());
    gfusiongpslidar.set_pitch(pgpsimu->pitch());
    gfusiongpslidar.set_roll(pgpsimu->roll());
    gfusiongpslidar.set_state(pgpsimu->rtk_state());
    gfusiongpslidar.set_timestamp(pgpsimu->time());
    gfusiongpslidar.set_vd(pgpsimu->vd());
    gfusiongpslidar.set_ve(pgpsimu->ve());
    gfusiongpslidar.set_vn(pgpsimu->vn());
    sharefusionres();

}

static void usendt(iv::lidar::ndtgpspos * pndtgpspos)
{
    gfusiongpslidar.set_heading(pndtgpspos->heading());
    gfusiongpslidar.set_height(pndtgpspos->height());
    gfusiongpslidar.set_lat(pndtgpspos->lat());
    gfusiongpslidar.set_lon(pndtgpspos->lon());
    gfusiongpslidar.set_pitch(pndtgpspos->pitch());
    gfusiongpslidar.set_roll(pndtgpspos->roll());
    if(pndtgpspos->tras_prob() > 2.0)gfusiongpslidar.set_state(16);
    else gfusiongpslidar.set_state(15);

//    std::cout<<"state is "<<gfusiongpslidar.state()<<std::endl;
    gfusiongpslidar.set_timestamp(pndtgpspos->lidartime());
    gfusiongpslidar.set_vd(pndtgpspos->vd());
    gfusiongpslidar.set_ve(pndtgpspos->ve());
    gfusiongpslidar.set_vn(pndtgpspos->vn());
    sharefusionres();
}

void fusionthread()
{
    unsigned int nnodatacount = 0;
    unsigned int nnogpsdata = 0;
    unsigned int nnondtdata = 0;
    int nstate = 0;
    int noldstate = 0;
    iv::gps::gpsimu xgpsimu;
    iv::lidar::ndtgpspos xndtgpspos;

    while(gbfusionrun)
    {
        std::this_thread::sleep_for(std::chrono::milliseconds(5));
        if(gbGPSNewData == false)
        {
            if(nnogpsdata < 1000000)nnogpsdata++;
        }
        else
        {
            nnogpsdata = 0;
        }
        if(gbNDTNewData == false)
        {
            if(nnondtdata < 1000000)nnondtdata++;
        }
        else
        {
            nnondtdata = 0;
        }

        if((gbNDTNewData == false)&&(gbGPSNewData == false))
        {
            if(nnodatacount < 1000000)nnodatacount++;
        }
        else
        {
            nnodatacount= 0;
        }

        if(gbUseLidarOnly)
        {
            //         if() // trans ok  use
            //Use  NDT Data,if trans  >0.5
            if(gndtgpspos.tras_prob() >0.5)
            {
                mMutexndt.lock();
                xndtgpspos.CopyFrom(gndtgpspos);
                mMutexndt.unlock();
                usendt(&xndtgpspos);
            }
        }
        else
        {
            if(gbGPSNewData && gbGPSFix)
            {
                //Use GPS Data;
                mMutexgps.lock();
                xgpsimu.CopyFrom(ggpsimu);
                mMutexgps.unlock();
                usegps(&xgpsimu);

            }
            else
            {
                if((nnondtdata > 200)&&(gbGPSNewData))
                {
                    //Use GPS Data;
                    mMutexgps.lock();
                    xgpsimu.CopyFrom(ggpsimu);
                    mMutexgps.unlock();
                    usegps(&xgpsimu);
                }
                else
                {
                    if(gbNDTNewData && ((gbGPSFix == false)||((gbGPSFix == true)&&(nnogpsdata > 100))))
                    {
                        //         if() // trans ok  use
                        //Use  NDT Data,if trans  >0.5
                        if(gndtgpspos.tras_prob() >0.5)
                        {
                            mMutexndt.lock();
                            xndtgpspos.CopyFrom(gndtgpspos);
                            mMutexndt.unlock();
                            usendt(&xndtgpspos);
                        }
                    }
                }
            }

        }
        gbGPSNewData = false;
        gbNDTNewData = false;
        if(nnodatacount > 200)
        {
            nstate = 1;

        }

        if(nnodatacount > 1200)
        {
            nstate = 2;
        }

        if(nnodatacount < 30)
        {
            nstate = 0;
        }

        if(nstate != noldstate)
        {
            noldstate = nstate;
            switch (nstate) {
            case 0:
                gfault->SetFaultState(0,0,"OK");
                givlog->info("fusion gps ndt ok.");

                break;

            case 1:
                gfault->SetFaultState(1,1,"no data more than 1 second.");
                givlog->warn("no data more than 1 second.");
                break;
            case 2:
                gfault->SetFaultState(2,2,"no data more than 60 seconds.");
                givlog->error("no data more than 60 seconds.");
                break;
            default:
                break;
            }
        }
    }
}


void exitfunc()
{
    std::cout<<"enter exitfunc."<<std::endl;
    gbfusionrun = false;
    gfusionthread->join();
    iv::modulecomm::Unregister(gpa);
    iv::modulecomm::Unregister(gpb);
    iv::modulecomm::Unregister(gpc);

    std::cout<<"Complete exitfunc."<<std::endl;
}

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

    gudpSocketGPSIMU = new QUdpSocket();

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

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

    RegisterIVBackTrace();

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

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

    gstrfusionmsg = xparam.GetParam("fusionmsg","fusion_gpslidar");
    gstrgpsmsg = xparam.GetParam("gpsmsg","hcp2_gpsimu");
    gstrndtmsg = xparam.GetParam("ndtgpsposmsg","ndtgpspos");
    gstrlidaronly = xparam.GetParam("LidarOnly","false");
    gstrfusiongpsimumsg = xparam.GetParam("fusiongpsimumsg","fusion_gpsimu");
    gstrshareudp = xparam.GetParam("shareudp","true");
    gstrsharegpsimu = xparam.GetParam("sharegpsimu","true");

    if(gstrlidaronly == "true")
    {
        gbUseLidarOnly = true;
    }

    if(gstrshareudp == "false")
    {
        gbShareUDP = false;
    }

    if(gstrsharegpsimu == "false")
    {
        gbShareGPSIMU = false;
    }

    gpc = iv::modulecomm::RegisterSend(gstrfusionmsg.data(),1000,1);
    gpa = iv::modulecomm::RegisterRecv(gstrgpsmsg.data(),ListenRaw);
    gpb = iv::modulecomm::RegisterRecv(gstrndtmsg.data(),ListenNDTGPS);
    gpd = iv::modulecomm::RegisterSend(gstrfusiongpsimumsg.data(),10000,1);
    gpe = iv::modulecomm::RegisterRecv("chassis",UpdateChassis);


    std::cout<<"start fusion_gpsndt"<<std::endl;

    gfusionthread = new std::thread(fusionthread);

    iv::ivexit::RegIVExitCall(exitfunc);


    return a.exec();
}