modularization/src/fusion/lidar_radar_fusion_cnn/lidarbuffer.cpp

#include "lidarbuffer.h"

#include <iostream>
#include "math/gnss_coordinate_convert.h"

lidarbuffer::lidarbuffer()
{

}

void lidarbuffer::AddGPS(iv::gps::gpsimu & xgpsimu)
{
    const int nMaxGPSFre = 100;
    if(mvectorgps_rec.size() > static_cast<unsigned int>( (mnLookTime * nMaxGPSFre*2/1000) ) )
    {
        std::cout<<" Warning. GPS Buffer Warning."<<std::endl;
        mmutex.lock();
        mvectorgps_rec.clear();;
        mmutex.unlock();
    }

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

    mmutex.lock();
    while(mvectorgps_rec.size()>0)
    {
        if(abs(nnow - mvectorgps_rec[0].nTime)>static_cast<unsigned int>( mnLookTime))
        {
            mvectorgps_rec.erase(mvectorgps_rec.begin());
        }
        else
        {
            break;
        }
    }
    mmutex.unlock();

    mmutex.lock();
    iv::gps_rec xrec;
    xrec.nTime = xgpsimu.msgtime();
    xrec.xgpsimu.CopyFrom(xgpsimu);
    mvectorgps_rec.push_back(xrec);
    mmutex.unlock();
}

void lidarbuffer::AddLidarObj(iv::lidar::objectarray & xobjarray)
{
    const int nMaxLidarFre = 100;
    if(mvectorgps_rec.size() > static_cast<unsigned int>( (mnLookTime * nMaxLidarFre*2/1000) ) )
    {
        std::cout<<" Warning. Lidar Buffer Warning."<<std::endl;
        mmutex.lock();
        mvectorlidarobj_rec.clear();;
        mmutex.unlock();
    }

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

    mmutex.lock();
    while(mvectorlidarobj_rec.size()>0)
    {
        if(abs(nnow - mvectorlidarobj_rec[0].nTime)>static_cast<unsigned int>( mnLookTime))
        {
            mvectorlidarobj_rec.erase(mvectorlidarobj_rec.begin());
        }
        else
        {
            break;
        }
    }
    mmutex.unlock();

    mmutex.lock();
    iv::lidarobj_rec xrec;
    xrec.nTime = xobjarray.timestamp()/1000; //to ms
    ChangePos(&xrec);
    xrec.xobjarray.CopyFrom(xobjarray);
    mvectorlidarobj_rec.push_back(xrec);
    mmutex.unlock();
}

void lidarbuffer::ChangePos(iv::lidarobj_rec * xrec)
{
    if(mvectorgps_rec.size() == 0)
    {
        std::cout<<" warning no gps data. "<<std::endl;
        return;
    }

    unsigned int nnearindex = 0;
    int64_t ntimediff = mnLookTime;

    unsigned int nsize = static_cast<unsigned int>( mvectorgps_rec.size());
    unsigned int i;
    for(i=1;i<nsize;i++)
    {
        if(abs(xrec->nTime - mvectorgps_rec[i].nTime)<abs(ntimediff))
        {
            ntimediff = xrec->nTime - mvectorgps_rec[i].nTime;
            nnearindex = i;
        }
    }

    if(abs(ntimediff) > 100)
    {
        std::cout<<"warning. lidar gps time diff more than 100ms"<<std::endl;
    }

    iv::gps::gpsimu xgpsimu;
    xgpsimu.CopyFrom(mvectorgps_rec[nnearindex].xgpsimu);

    double hdg = (90 - xgpsimu.heading())*M_PI/180.0;

    double fsr = hdg + M_PI/2.0;
    double x_sensor = mfLidarOffX * cos(fsr) - mfLidarOffY * sin(fsr);
    double y_sensor = mfLidarOffX * sin(fsr) + mfLidarOffY * cos(fsr);

    double x_gps,y_gps;
    GaussProjCal(xgpsimu.lon(),xgpsimu.lat(),&x_gps,&y_gps);

    for(i=0;i<nsize;i++)
    {
        iv::lidar::lidarobject * pobj = xrec->xobjarray.mutable_obj(i);
        double x_raw = pobj->position().x();
        double y_raw = pobj->position().y();

        double x_abs = x_gps + x_sensor + x_raw * cos(fsr) - y_raw * sin(fsr);
        double y_abs = y_gps + y_sensor + x_raw * sin(fsr) + y_raw * cos(fsr);

        pobj->mutable_position()->set_x(x_abs);
        pobj->mutable_position()->set_y(y_abs);


    }
}