modularization/src/driver/vtd_pilot_if/vtd_pilot.cpp

#include "vtd_pilot.h"


static void CalcXY(const double lat0,const double lon0,
              const double lat,const double lon,
              double & x,double & y)
{

    double x0,y0;
    GaussProjCal(lon0,lat0,&x0,&y0);
    GaussProjCal(lon,lat,&x,&y);
    x = x - x0;
    y = y-  y0;
}

vtd_pilot::vtd_pilot(std::string strfromvtd,std::string strtovtd)
{
    (void)strfromvtd;
    ModuleFun funvtd =std::bind(&vtd_pilot::UpdateVTD,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
    mpfromvtd = iv::modulecomm::RegisterRecvPlus(strfromvtd.data(),funvtd);

    mptovtd = iv::modulecomm::RegisterSend(strtovtd.data(),10000,1) ;

    ModuleFun fungpsimu =std::bind(&vtd_pilot::UpdateGPSIMU,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
    mpagpsimu = iv::modulecomm::RegisterRecvPlus("hcp2_gpsimu",fungpsimu);

    mpaegostate = iv::modulecomm::RegisterSend("egostate",10000,1);

    mpafusion = iv::modulecomm::RegisterSend("li_ra_fusion",10000000,1);

    mpthread = new std::thread(&vtd_pilot::threadego,this);
    mpthreadfusion = new std::thread(&vtd_pilot::threadobject,this);

    memset( &sOwnObjectState, 0, sizeof( RDB_OBJECT_STATE_t ) );
}

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

    (void)index;
    (void)dt;
    (void)strmemname;
    iv::gps::gpsimu  xgpsimu;
    if(!xgpsimu.ParseFromArray(strdata,nSize))
    {
        std::cout<<"parse gpsimu fail"<<std::endl;
        return;
    }

    mmutex.lock();
    mgpsimu.CopyFrom(xgpsimu);
    mbupdate_gps = true;
    mmutex.unlock();

    mcv.notify_all();


}

void vtd_pilot::ConvertToObjectArray(std::vector<iv::vtd::rdbdata> &xvectorrdbdata, iv::fusion::fusionobjectarray &xfusionarray)
{
    int i;
    int j;
    std::cout<<" obj size: "<<xvectorrdbdata.size()<<std::endl;
    for(i=0;i<(int)xvectorrdbdata.size();i++)
    {
        iv::vtd::rdbdata * prdbdata = &xvectorrdbdata[i];
        for(j=0;j<prdbdata->mrdbitem_size();j++)
        {
            iv::vtd::rdbitem * pitem = prdbdata->mutable_mrdbitem(j);
            if(pitem->pkgid() == RDB_PKG_ID_OBJECT_STATE)
            {
                RDB_OBJECT_STATE_t xobj;
    //            std::cout<<" structure RDB_OBJECT_STATE_t size: "<<sizeof(xobj)<<"data size: "<<xrdbitem.mnpkgdatasize<<std::endl;
                memcpy(&xobj,pitem->pkgdata().data(),sizeof(RDB_OBJECT_STATE_t));
   //             std::cout<<"id:"<<xobj.base.id<<" name:"<<xobj.base.name<< " pos : x "<<xobj.base.pos.x<<" y "<<xobj.base.pos.y<<" z "<<xobj.base.pos.z<<std::endl;
                if(xobj.base.id == 1)
                {
                    mfX = xobj.base.pos.x;
                    mfY = xobj.base.pos.y;
                    mfHeading = xobj.base.pos.h;
                }
                else
                {
                    iv::fusion::fusionobject * pobject = NULL;
                    int k;
                    for(k=0;k<xfusionarray.obj_size();k++)
                    {
                        if(xfusionarray.mutable_obj(k)->id() == xobj.base.id)
                        {
                            pobject = xfusionarray.mutable_obj(k);
                            break;
                        }
                    }
                    if(pobject  == NULL)
                    {
                        pobject = xfusionarray.add_obj();
                    }
                    pobject->set_id(xobj.base.id);
                    double x,y;
                    x = xobj.base.pos.x - mfX;
                    y = (xobj.base.pos.y - mfY)*(1.0);
                    double relx,rely;
                    double beta  = mfHeading*(-1.0);
                    relx = x*cos(beta) - y*sin(beta);
                    rely = x*sin(beta) + y*cos(beta);
                    double vx,vy;
                    vx = xobj.ext.speed.x;
                    vy = xobj.ext.speed.y;
                    vx = vx - mfvx;
                    vy = vy - mfvy;
                    double relvx,relvy;
                    relvx = vx*cos(beta) - vy*sin(beta);
                    relvy = vx*sin(beta) + vy*sin(beta);
                    double fobjheading = xobj.base.pos.h;
                    fobjheading = fobjheading - mfHeading;
                    pobject->set_lifetime(100);
                    pobject->set_prob(1.0);
                    pobject->set_sensor_type(1);
                    pobject->set_yaw(fobjheading);

                    iv::fusion::PointXYZ * ppointxyz = new iv::fusion::PointXYZ;
                    ppointxyz->set_x(rely*(-1.0));
                    ppointxyz->set_y(relx);
                    ppointxyz->set_z(0);
                    iv::fusion::Dimension * pdim = new iv::fusion::Dimension;
                    pdim->set_x(xobj.base.geo.dimX);
                    pdim->set_y(xobj.base.geo.dimY);
                    pdim->set_z(xobj.base.geo.dimZ);
                    pobject->set_allocated_centroid(ppointxyz);
//                    int m,n;
//                    for(m=-30;m<30;m++)
//                    {
//                        for(n=-30;n<30;n++)
//                        {
//                            iv::fusion::NomalXYZ *pnormal = pobject->add_nomal_centroid();
//                            pnormal->set_nomal_x((rely+m*0.1)*(-1.0));
//                            pnormal->set_nomal_y(relx+n*0.1);
//                        }
//                    }

                    double dimx,dimy,dimz;
                    dimx = xobj.base.geo.dimX;
                    dimy = xobj.base.geo.dimY;
                    dimz = xobj.base.geo.dimZ;
  //                  std::cout<<" dimx: "<<dimx<<" dimy: "<<dimy<<std::endl;
                    double xoff,yoff;
                    std::cout<<" obj hedad: "<<xobj.base.pos.h<<" our head: "<<mfHeading<<" rel heading: "<<fobjheading<<std::endl;
                    for(xoff = (dimx*(-0.5));xoff<=(dimx*0.5);xoff=xoff+0.2)
                    {
                        for(yoff = (dimy*(-0.5));yoff<=(dimy*0.5);yoff = yoff+0.2)
                        {
                            double reloffx,reloffy;
                            if(fabs(fobjheading) < 0.1)
                            {
                                for(int m=0;m<1000;m++)
                                std::cout<<" hello;"<<std::endl;

                            }
    //                        fobjheading = 0;
    //                        std::cout<<"xoff: "<<xoff<<" yoff: "<<yoff<<std::endl;
                            reloffx = xoff*cos(fobjheading) - yoff*sin(fobjheading) + relx;
                            reloffy = xoff*sin(fobjheading) + yoff*cos(fobjheading) + rely;
                            iv::fusion::NomalXYZ *pnormal = pobject->add_nomal_centroid();
                            pnormal->set_nomal_x((reloffy)*(-1.0));
                            pnormal->set_nomal_y(reloffx);
                        }
                    }

                    pobject->set_allocated_dimensions(pdim);
                    pobject->set_velocity_linear_x(sqrt(pow(relvx,2)+pow(relvy,2)));

//                    std::cout<<" x: "<<relx<<" y: "<<rely<<std::endl;
                }


            }
        }
    }
}

void vtd_pilot::threadobject()
{
    std::vector<iv::vtd::rdbdata> xvectorrdbdata;
    int64_t nlastsharetime = std::chrono::system_clock::now().time_since_epoch().count();
//    int nmaxobjid = 2;
    int nshareinter = 20;  //100 ms share a data.
    bool bshare = false;
    while(mbRun)
    {
        int64_t now = std::chrono::system_clock::now().time_since_epoch().count();
        int64_t timediff = abs(now-nlastsharetime)/1000000;
        if(timediff >= nshareinter)
        {
            bshare = true;
        }
        if(bshare)
        {
            iv::fusion::fusionobjectarray  xfusionarray;
            ConvertToObjectArray(xvectorrdbdata,xfusionarray);

            if(xfusionarray.obj_size()>=1)
            {

            std::cout<<" now : "<<now/1000000<<std::endl;

            int nbytesize = xfusionarray.ByteSize();

            std::shared_ptr<char> pstr_ptr = std::shared_ptr<char>(new char[nbytesize]);

            if(xfusionarray.SerializeToArray(pstr_ptr.get(),nbytesize))
            {
                iv::modulecomm::ModuleSendMsg(mpafusion,pstr_ptr.get(),nbytesize);
            }

            }
            else
            {
                std::cout<<"no object. "<<std::endl;
            }
            //share object data.
            bshare = false;
            nlastsharetime = now;
            xvectorrdbdata.clear();
        }
        std::unique_lock<std::mutex> lk(mmutexcvrdb);
        if(mcvrdb.wait_for(lk,std::chrono::milliseconds(10)) == std::cv_status::timeout)
        {
            lk.unlock();
            continue;
        }
        else
        {
            lk.unlock();

        }
        mmutexrdb.lock();
        int i;
        for(i=0;i<(int)mvectorrdbdata.size();i++)
        {
            xvectorrdbdata.push_back(mvectorrdbdata[i]);
        }
        mvectorrdbdata.clear();
        mmutexrdb.unlock();




    }
}

void vtd_pilot::UpdateVTD(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
{
    (void)index;
    (void)dt;
    (void)strmemname;

    iv::vtd::rdbdata xrdbdata;
    if(xrdbdata.ParseFromArray(strdata,nSize))
    {
        mmutexrdb.lock();
        mvectorrdbdata.push_back(xrdbdata);
        mmutexrdb.unlock();
        mcvrdb.notify_all();
 //       std::cout<<"notify"<<std::endl;
//        if(xrdbdata.mrdbitem_size()>0)
//        {
//            iv::vtd::rdbitem * pitem = xrdbdata.mutable_mrdbitem(0);
//            msimFrame = pitem->simframe();
//            msimTime = pitem->simtime();
//            if(pitem->pkgid() == RDB_PKG_ID_OBJECT_STATE)
//            {

//            }
//        }
    }
}

void vtd_pilot::setOwnState(double x, double y, double z,double vx,double vy,double fheading)
{
    sOwnObjectState.base.id       = 1;
    sOwnObjectState.base.category = RDB_OBJECT_CATEGORY_PLAYER;
    sOwnObjectState.base.type     = RDB_OBJECT_TYPE_PLAYER_CAR;
    strcpy( sOwnObjectState.base.name, "Ego" );

    // dimensions of own vehicle
    sOwnObjectState.base.geo.dimX = 4.60 ;//* dimFactor;
    sOwnObjectState.base.geo.dimY = 1.86 ;//* dimFactor;
    sOwnObjectState.base.geo.dimZ = 1.60 ;//* dimFactor;


    // offset between reference point and center of geometry
    sOwnObjectState.base.geo.offX = 0.0;//0.80;
    sOwnObjectState.base.geo.offY = 0.00;
    sOwnObjectState.base.geo.offZ = 0.0;//0.30;

    sOwnObjectState.base.pos.x     = x;
    sOwnObjectState.base.pos.y     = y;
    sOwnObjectState.base.pos.z     = z;
    sOwnObjectState.base.pos.h     = fheading;
    sOwnObjectState.base.pos.p     = 0.0;
    sOwnObjectState.base.pos.r     = 0.0;
    sOwnObjectState.base.pos.flags = RDB_COORD_FLAG_POINT_VALID | RDB_COORD_FLAG_ANGLES_VALID;

    sOwnObjectState.ext.speed.x     = vx;
    sOwnObjectState.ext.speed.y     = vy;
    sOwnObjectState.ext.speed.z     = 0;
    sOwnObjectState.ext.speed.h     = 0.0;
    sOwnObjectState.ext.speed.p     = 0.0;
    sOwnObjectState.ext.speed.r     = 0.0;
    sOwnObjectState.ext.speed.flags = RDB_COORD_FLAG_POINT_VALID | RDB_COORD_FLAG_ANGLES_VALID;

    sOwnObjectState.ext.accel.x     = 0.0;
    sOwnObjectState.ext.accel.y     = 0.0;
    sOwnObjectState.ext.accel.z     = 0.0;
    sOwnObjectState.ext.accel.flags = RDB_COORD_FLAG_POINT_VALID;

    sOwnObjectState.base.visMask    =  RDB_OBJECT_VIS_FLAG_TRAFFIC | RDB_OBJECT_VIS_FLAG_RECORDER;
}

void vtd_pilot::threadego()
{
    while(mbRun)
    {
        if(mbupdate_gps == false)
        {
            std::unique_lock<std::mutex> lk(mmutexcv);
            if(mcv.wait_for(lk,std::chrono::milliseconds(100)) == std::cv_status::timeout)
            {
                lk.unlock();
                continue;
            }
            else
            {
                lk.unlock();
                continue;
            }
        }

        iv::gps::gpsimu xgpsimu;
        mmutex.lock();
        xgpsimu.CopyFrom(mgpsimu);
        mbupdate_gps = false;
        mmutex.unlock();

        double x,y,z;
        CalcXY(mflat0,mflon0,xgpsimu.lat(),xgpsimu.lon(),x,y);
        double vx = xgpsimu.ve();
        double vy = xgpsimu.vn();
        z = 0;
        double fheading = (90 - xgpsimu.heading())*M_PI/180.0;
        setOwnState(x,y,z,vx,vy,fheading);

        mfX = x;
        mfY = y;
        mfHeading = fheading;
        mfvx = vx;
        mfvy = vy;

 //       RDB_OBJECT_STATE_t * pr = &sOwnObjectState;

        iv::modulecomm::ModuleSendMsg(mpaegostate,(char *)&sOwnObjectState,sizeof(RDB_OBJECT_STATE_t));



        //convert to x,y,z

        //send data.
    }
}