modularization/src/tool/tool_trace2vectormap/trace2vectormap.cpp

#include "trace2vectormap.h"

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

trace2vectormap::trace2vectormap(iv::trace2vectormap_param xparam)
{
    mparam = xparam;
}

void trace2vectormap::llh2xyy(double flon, double flat, double fhead, double & x, double & y, double & fyaw, double flon0, double flat0)
{
    double x_o,y_o;
    double x_now,y_now;
    GaussProjCal(flon0,flat0,&x_o,&y_o);
    GaussProjCal(flon,flat,&x_now,&y_now);
    x = x_now - x_o;
    y = y_now - y_o;
    fyaw = (90 - fhead)*M_PI/180.0;
}



int trace2vectormap::convert()
{
    iv::trace2vectormap_param xparam = mparam;
    QFile xFile;
    xFile.setFileName(xparam.strtracefilepath.data());
    if(!xFile.open(QIODevice::ReadOnly))
    {
        return -1;
    }
    QByteArray ba = xFile.readAll();
    QList<QByteArray> baline =ba.split('\n');//x.split(QRegExp("\n ")) ;//ba.split('\n');
    int nline = baline.size();
    int i;
    int npid = 1;

    std::vector<iv::vectormap::point> xvectorpoint;
    std::vector<iv::vectormap::node> xvectornode;
    std::vector<iv::vectormap::lane> xvectorlane;
    std::vector<iv::vectormap::line> xvectorline;
    std::vector<iv::vectormap::whiteline> xvectorwhiteline;
    std::vector<iv::vectormap::dtlane> xvectordtlane;
    std::vector<iv::vectormap::area> xvectorarea;
    std::vector<iv::vectormap::wayarea> xvectorwayarea;

    iv::vectormap::point xlastpoint;
    bool bhavelast = false;

    for(i=0;i<nline;i++)
    {
        QString x(baline[i]);
//         QList<QByteArray> badata = baline[i].split('\t');
        QStringList badata = x.split(QRegExp("[\t ,;]"));

        if(badata.size()>=10)
        {
            double flon = QString(badata[1]).toDouble();
            double flat = QString(badata[2]).toDouble();
            double fheading = QString(badata[5]).toDouble();
            int nspeedmode = QString(badata[7]).toInt();
            double fspeed = xparam.fVelLim;
            std::map<int,double>::iterator it = xparam.mapmodevel.find(nspeedmode);
            if(it !=xparam.mapmodevel.end())
            {
                fspeed = it->second;
            }
//            std::cout<<"lat: "<<flat<<" lon:"<<flon<<" heading:"<<fheading<<std::endl;
            double x,y,fyaw;
            llh2xyy(flon,flat,fheading,x,y,fyaw,xparam.fLon0,xparam.fLat0);
            iv::vectormap::point xpoint;
            xpoint.pid = npid;
            xpoint.bx = y;
            xpoint.ly = x;
            xpoint.fyaw = fyaw;
            xpoint.h = 0;
            xpoint.fspeed = fspeed;

            if(bhavelast == false)
            {
                npid++;
                xvectorpoint.push_back(xpoint);
                xlastpoint = xpoint;
                bhavelast = true;
            }
            else
            {
                if(sqrt(pow(xlastpoint.bx - xpoint.bx,2)+pow(xlastpoint.ly - xpoint.ly,2))>=0.5)
                {
                    npid++;
                    xvectorpoint.push_back(xpoint);
                    xlastpoint = xpoint;
                }
            }

        }
    }

    if(xvectorpoint.size()<2)return 0;

    bool bcircle = false;
    iv::vectormap::point xpointstart,xpointend;
    xpointstart = xvectorpoint[0];
    xpointend = xvectorpoint[xvectorpoint.size() -1];
    double fdis = sqrt(pow(xpointstart.bx-xpointend.bx,2)+pow(xpointstart.ly - xpointend.ly,2));
    if(fdis<10.0)
    {
        double fyawdiff = xpointstart.fyaw - xpointend.fyaw;
        while(fyawdiff<0)fyawdiff = fyawdiff + 2.0*M_PI;
        while(fyawdiff>=(2.0*M_PI))fyawdiff = fyawdiff - 2.0*M_PI;
        if((fyawdiff<0.1)||(fyawdiff>(2.0*M_PI -0.1)))
        {
            std::cout<<" circle mode "<<std::endl;
            bcircle = true;
        }
    }

    int npointsize = xvectorpoint.size();
    for(i=0;i<npointsize;i++)
    {
        iv::vectormap::node xnode;
        xnode.nid = i;
        xnode.pid = xvectorpoint[i].pid;
        xnode.fspeed = xvectorpoint[i].fspeed;
        xvectornode.push_back(xnode);
    }

    int nnodesize = xvectornode.size();
    for(i=1;i<nnodesize;i++)
    {
        iv::vectormap::lane xlane;
        xlane.bnid = xvectornode[i-1].nid;
        xlane.fnid = xvectornode[i].nid;
        xlane.lnid = i;
        xlane.did = i;
        xlane.span = xparam.fSpan;
        xlane.lcnt = xparam.Lcnt;
        xlane.lno = xparam.Lno;
        xlane.blid = 0;
        xlane.flid = 0;
        xlane.fspeed = xvectornode[i-1].fspeed;
        xvectorlane.push_back(xlane);
        iv::vectormap::dtlane xdtlane;
        xdtlane.did = i;
        xdtlane.Dist  = xparam.fSpan;
        xdtlane.pid = xvectornode[i-1].pid;
        xdtlane.Dir = 0;
        xdtlane.Apara = 90000000000;
        xdtlane.r = 0.0;
        xdtlane.slope = 0;
        xdtlane.LW = xparam.fDisLeft;
        xdtlane.RW = xparam.fDisRight;
        xvectordtlane.push_back(xdtlane);
    }

    int nlanesize = xvectorlane.size();
    for(i=1;i<nlanesize;i++)
    {
        xvectorlane[i-1].flid = xvectorlane[i].lnid;
        xvectorlane[i].blid = xvectorlane[i-1].lnid;
    }

    bcircle = false;
    if(bcircle)
    {
        iv::vectormap::lane xlane;
        xlane.bnid = xvectornode[nnodesize-1].nid;
        xlane.fnid = xvectornode[0].nid;
        xlane.lnid = nnodesize;
        xlane.did = nnodesize;
        xlane.span = xparam.fSpan;
        xlane.lcnt = xparam.Lcnt;
        xlane.lno = xparam.Lno;
        xlane.blid = xvectorlane[nlanesize-1].lnid;
        xlane.flid = xvectorlane[0].lnid;
        xvectorlane[0].blid = xlane.lnid;
        xvectorlane[nlanesize -1].flid = xlane.lnid;
        xlane.fspeed = xvectornode[nnodesize-1].fspeed;
        xvectorlane.push_back(xlane);
        iv::vectormap::dtlane xdtlane;
        xdtlane.did = nnodesize;
        xdtlane.Dist  = xparam.fSpan;
        xdtlane.pid = xvectornode[nnodesize-1].pid;
        xdtlane.Dir = 0;
        xdtlane.Apara = 90000000000;
        xdtlane.r = 0.0;
        xdtlane.slope = 0;
        xdtlane.LW = xparam.fDisLeft;
        xdtlane.RW = xparam.fDisRight;
        xvectordtlane.push_back(xdtlane);
    }

    for(i=0;i<npointsize;i++)
    {
       iv::vectormap::point xpoint;
       xpoint = xvectorpoint[i];
       xpoint.bx = xpoint.bx + xparam.fDisLeft * sin(xpoint.fyaw + M_PI/2.0);
       xpoint.ly = xpoint.ly + xparam.fDisLeft * cos(xpoint.fyaw + M_PI/2.0);
       xpoint.pid = npid;
       npid++;
       xvectorpoint.push_back(xpoint);
    }

    for(i=0;i<npointsize;i++)
    {
       iv::vectormap::point xpoint;
       xpoint = xvectorpoint[i];
       xpoint.bx = xpoint.bx + xparam.fDisRight * sin(xpoint.fyaw - M_PI/2.0);
       xpoint.ly = xpoint.ly + xparam.fDisRight * cos(xpoint.fyaw - M_PI/2.0);
       xpoint.pid = npid;
       npid++;
       xvectorpoint.push_back(xpoint);
    }


    int nnowwhiteid = 1;
    for(i=1;i<npointsize;i++)
    {
        iv::vectormap::line xline;
        xline.lid = i;
        xline.bpid = xvectorpoint[i-1+npointsize].pid;
        xline.fpid = xvectorpoint[i+npointsize].pid;
        xline.blid = 0;
        xline.flid = 0;
        xvectorline.push_back(xline);
        iv::vectormap::whiteline xwhiteline;
        xwhiteline.id = nnowwhiteid;nnowwhiteid++;
        xwhiteline.fwidth = 0.15;
        xwhiteline.strcorlor = xparam.strColorLeft;
        xwhiteline.lid = xline.lid;
        xvectorwhiteline.push_back(xwhiteline);
    }

    for(i=1;i<(npointsize -1);i++)
    {
        xvectorline[i-1].flid = xvectorline[i].lid;
        xvectorline[i].blid = xvectorline[i-1].lid;
    }

    for(i=1;i<npointsize;i++)
    {
        iv::vectormap::line xline;
        xline.lid = i + (npointsize-1);
        xline.bpid = xvectorpoint[i-1+npointsize*2].pid;
        xline.fpid = xvectorpoint[i+npointsize*2].pid;
        xline.blid = 0;
        xline.flid = 0;
        xvectorline.push_back(xline);
        iv::vectormap::whiteline xwhiteline;
        xwhiteline.id = nnowwhiteid;nnowwhiteid++;
        xwhiteline.lid = xline.lid;
        xwhiteline.fwidth = 0.15;
        xwhiteline.strcorlor = xparam.strColorRight;
        xvectorwhiteline.push_back(xwhiteline);
    }

    for(i=1;i<(npointsize -1);i++)
    {
        xvectorline[i-1 + npointsize-1].flid = xvectorline[i + npointsize -1].lid;
        xvectorline[i + npointsize-1].blid = xvectorline[i-1 + npointsize -1].lid;
    }

    int nlid =  (npointsize-1)*2;
    int naid = 1;
    for(i=1;i<npointsize;i++)
    {

        int p1id,p2id,p3id,p4id;
        p4id =  xvectorpoint[i-1+npointsize*1].pid;
        p3id = xvectorpoint[i+npointsize*1].pid;
        p2id = xvectorpoint[i+npointsize*2].pid;
        p1id = xvectorpoint[i-1+npointsize*2].pid;


            iv::vectormap::area xarea;
            xarea.aid = naid;
        iv::vectormap::line xline;
        xline.lid = nlid;
        xline.bpid = p4id;
        xline.fpid = p3id;
        xline.blid = 0;// nlid+3;
        xline.flid = nlid+1;
        xvectorline.push_back(xline);
        nlid++;
        xarea.slid = xline.lid;
        xline.lid = nlid;
        xline.bpid = p3id;
        xline.fpid = p2id;
        xline.blid = nlid-1;
        xline.flid = nlid+1;
        xvectorline.push_back(xline);
        nlid++;
        xline.lid = nlid;
        xline.bpid = p2id;
        xline.fpid = p1id;
        xline.blid = nlid-1;
        xline.flid = nlid+1;
        xvectorline.push_back(xline);
        nlid++;
        xline.lid = nlid;
        xline.bpid = p1id;
        xline.fpid = p4id;
        xline.blid = nlid-1;
        xline.flid =0;//nlid-3;
        xvectorline.push_back(xline);
        nlid++;
        xarea.elid = xline.lid;
        xvectorarea.push_back(xarea);

        naid++;
        iv::vectormap::wayarea xwayarea;
        xwayarea.waid = i;
        xwayarea.aid = xarea.aid;
        xvectorwayarea.push_back(xwayarea);

    }

    /*
    for(i=1;i<npointsize;i++)
    {
        iv::vectormap::line xline;
        xline.lid = i + (npointsize-1)*2;
//        xline.bpid = xvectorpoint[i+npointsize*2].pid;
//        xline.fpid = xvectorpoint[i-1+npointsize*2].pid;
        xline.bpid = xvectorpoint[i+npointsize*0].pid;
        xline.fpid = xvectorpoint[i-1+npointsize*0].pid;
        xline.blid = 0;
        xline.flid = 0;
        xvectorline.push_back(xline);
    }

    for(i=1;i<(npointsize -1);i++)
    {
        xvectorline[i-1 + (npointsize-1)*2].blid = xvectorline[i + (npointsize -1)*2].lid;
        xvectorline[i + (npointsize-1)*2].flid = xvectorline[i-1 + (npointsize -1)*2].lid;
    }

    for(i=0;i<(npointsize-1);i++)
    {
        iv::vectormap::area xarea;
        xarea.aid = i+1;
        xarea.slid = xvectorline[i+(npointsize-1)*1].lid;
        xarea.elid = xvectorline[i+(npointsize-1)*2].lid;
        xarea.slid = xvectorline[i+(npointsize-1)*1].lid;
        xarea.elid = xvectorline[i+(npointsize-1)*2].lid;
        xvectorarea.push_back(xarea);
        iv::vectormap::wayarea xwayarea;
        xwayarea.waid = i+1;
        xwayarea.aid = xarea.aid;
        xvectorwayarea.push_back(xwayarea);
    }
    */

    if(bcircle)
    {
        iv::vectormap::line xline;
        xline.lid = (npointsize-1)*3+1;
        xline.bpid = xvectorpoint[npointsize-1+npointsize].pid;
        xline.fpid = xvectorpoint[npointsize].pid;
        xline.blid = xvectorline[npointsize-1 -1].lid;
        xvectorline[npointsize-1 -1].flid = xline.lid;
        xline.flid = xvectorline[0].lid;
        xvectorline[0].blid = xline.lid;
        xvectorline.push_back(xline);
        iv::vectormap::whiteline xwhiteline;
        xwhiteline.id = nnowwhiteid;nnowwhiteid++;
        xwhiteline.fwidth = 0.15;
        xwhiteline.strcorlor = xparam.strColorLeft;
        xwhiteline.lid = xline.lid;
        xvectorwhiteline.push_back(xwhiteline);

        xline.lid = (npointsize-1)*3+2;
        xline.bpid = xvectorpoint[npointsize-1+npointsize*2].pid;
        xline.fpid = xvectorpoint[0+npointsize*2].pid;
        xline.blid = xvectorline[npointsize-1-1 + (npointsize-1)].lid;
        xvectorline[npointsize-1-1 + (npointsize-1)].flid = xline.lid;
        xline.flid = xvectorline[npointsize-1].lid;
        xvectorline[npointsize-1].blid = xline.lid;
        xvectorline.push_back(xline);
        xwhiteline.id = nnowwhiteid;nnowwhiteid++;
        xwhiteline.fwidth = 0.15;
        xwhiteline.strcorlor = xparam.strColorLeft;
        xwhiteline.lid = xline.lid;
        xvectorwhiteline.push_back(xwhiteline);

        xline.lid = (npointsize-1)*3+3;
        xline.fpid = xvectorpoint[npointsize-1+npointsize*2].pid;
        xline.bpid = xvectorpoint[0+npointsize*2].pid;
        xline.blid = xvectorline[ (npointsize-1)*2].lid;
        xvectorline[ (npointsize-1)*2].flid = xline.lid;
        xline.flid = xvectorline[(npointsize-1-1) + (npointsize-1)*2].lid;
        xvectorline[(npointsize-1-1) + (npointsize-1)*2].blid = xline.lid;
        xvectorline.push_back(xline);

        iv::vectormap::area xarea;
        xarea.aid = npointsize;
        xarea.slid = xvectorline[0+(npointsize-1)*3].lid;
        xarea.elid = xvectorline[2+(npointsize-1)*3].lid;
        xvectorarea.push_back(xarea);
        iv::vectormap::wayarea xwayarea;
        xwayarea.waid = npointsize;
        xwayarea.aid = xarea.aid;
        xvectorwayarea.push_back(xwayarea);
    }

    QString strfolder = xparam.stroutfolder.data();
    QFile xFilePoints;
    xFilePoints.setFileName(strfolder + "/point.csv");
    if(!xFilePoints.open(QIODevice::ReadWrite))
    {
        qDebug("point file open fail.");
        return -2;
    }
    char strline[1000];
    snprintf(strline,1000,"PID,B,L,H,Bx,Ly,ReF,MCODE1,MCODE2,MCODE3\n");
    xFilePoints.write(strline);
    for(i=0;i<(int)xvectorpoint.size();i++)
    {
        snprintf(strline,1000,"%d,%d,%d,%f,%f,%f,%d,%d,%d,%d\n",
                 xvectorpoint[i].pid,0,0,0.0,xvectorpoint[i].bx,xvectorpoint[i].ly,7,0,0,0);
        xFilePoints.write(strline);
    }
    xFilePoints.close();

    QFile xFileLine;
    xFileLine.setFileName(strfolder + "/line.csv");
    if(!xFileLine.open(QIODevice::ReadWrite))
    {
        qDebug("line file open fail.");
        return -3;
    }
    snprintf(strline,1000,"LID,BPID,FPID,BLID,FLID\n");
    xFileLine.write(strline);
    int nlinesize = xvectorline.size();
    for(i=0;i<nlinesize;i++)
    {
        snprintf(strline,1000,"%d,%d,%d,%d,%d\n",
                 xvectorline[i].lid,xvectorline[i].bpid,xvectorline[i].fpid,
                 xvectorline[i].blid,xvectorline[i].flid);
        xFileLine.write(strline);
    }
    xFileLine.close();

    QFile xFileWhiteLine;
    xFileWhiteLine.setFileName(strfolder + "/whiteline.csv");
    if(!xFileWhiteLine.open(QIODevice::ReadWrite))
    {
        qDebug("whiteline file open fail.");
        return -4;
    }

    snprintf(strline,1000,"ID,LID,Width,Color,type,LinkID\n");
    xFileWhiteLine.write(strline);
    int nwhitelinesize = xvectorwhiteline.size();
    for(i=0;i<nwhitelinesize;i++)
    {
        snprintf(strline,1000,"%d,%d,%f,%s,%d,%d\n",
                 xvectorwhiteline[i].id,xvectorwhiteline[i].lid,0.15,xvectorwhiteline[i].strcorlor.data(),0,0);
        xFileWhiteLine.write(strline);
    }
    xFileWhiteLine.close();

    QFile xFileNode;
    xFileNode.setFileName(strfolder + "/node.csv");
    if(!xFileNode.open(QIODevice::ReadWrite))
    {
        qDebug(" node file open fail.");
        return -5;
    }
    snprintf(strline,1000,"NID,PID\n");
    xFileNode.write(strline);
    for(i=0;i<(int)xvectornode.size();i++)
    {
        snprintf(strline,1000,"%d,%d\n",xvectornode[i].nid,xvectornode[i].pid);
        xFileNode.write(strline);
    }
    xFileNode.close();

    QFile xFileLane;
    xFileLane.setFileName(strfolder + "/lane.csv");
    if(!xFileLane.open(QIODevice::ReadWrite))
    {
        qDebug(" lane file open fail.");
        return -6;
    }
    snprintf(strline,1000,"LnID,DID,BLID,FLID,BNID,FNID,JCT,BLID2,BLID3,BLID4,FLID2,FLID3,FLID4,ClossID,Span,LCnt,Lno,LaneType,LimitVel,RefVel,RoadSecID,LaneChgFG\n");
    xFileLane.write(strline);

    for(i=0;i<(int)xvectorlane.size();i++)
    {
//        std::map<int,double>::iterator it = xparam.mapmodevel.find()
        snprintf(strline,1000,"%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%f,%f,%d,%d\n",
                 xvectorlane[i].lnid,xvectorlane[i].did,xvectorlane[i].blid,
                 xvectorlane[i].flid,xvectorlane[i].bnid,xvectorlane[i].fnid,
                 0,0,0,0,
                 0,0,0,0,
                 1,xvectorlane[i].lcnt,xvectorlane[i].lno,0,
                 xvectorlane[i].fspeed/3.6,xvectorlane[i].fspeed/3.6,0,1);
        xFileLane.write(strline);
    }
    xFileLane.close();

    QFile xFiledtLane;
    xFiledtLane.setFileName(strfolder + "/dtlane.csv");
    if(!xFiledtLane.open(QIODevice::ReadWrite))
    {
        qDebug(" dtlane file open fail.");
        return -6;
    }
    snprintf(strline,1000,"DID,Dist,PID,Dir,Apara,r,slope,cant,LW,RW\n");
    xFiledtLane.write(strline);

    for(i=0;i<(int)xvectordtlane.size();i++)
    {
        snprintf(strline,1000,"%d,%f,%d,%f,%f,%f,%f,%f,%f,%f\n",
                 xvectordtlane[i].did,xvectordtlane[i].Dist,xvectordtlane[i].pid,
                 xvectordtlane[i].Dir,xvectordtlane[i].Apara,xvectordtlane[i].r,
                 xvectordtlane[i].slope,xvectordtlane[i].cant,xvectordtlane[i].LW,
                 xvectordtlane[i].RW);
        xFiledtLane.write(strline);
    }
    xFiledtLane.close();

    QFile xFileArea;
    xFileArea.setFileName((strfolder + "/area.csv"));
    if(!xFileArea.open(QIODevice::ReadWrite))
    {
        std::cout<<"  Area File open fail."<<std::endl;
        return -7;
    }
    snprintf(strline,1000,"AID,SLID,ELID\n");
    xFileArea.write(strline);

    for(i=0;i<(int)xvectorarea.size();i++)
    {
        snprintf(strline,1000,"%d,%d,%d\n",xvectorarea[i].aid,xvectorarea[i].slid,xvectorarea[i].elid);
        xFileArea.write(strline);
    }
    xFileArea.close();

    QFile xFileWayArea;
    xFileWayArea.setFileName((strfolder + "/wayarea.csv"));
    if(!xFileWayArea.open(QIODevice::ReadWrite))
    {
        std::cout<<"  WayArea File open fail."<<std::endl;
        return -8;
    }
    snprintf(strline,1000,"WAID,AID\n");
    xFileWayArea.write(strline);

    for(i=0;i<(int)xvectorwayarea.size();i++)
    {
        snprintf(strline,1000,"%d,%d\n",xvectorwayarea[i].waid,xvectorwayarea[i].aid);
        xFileWayArea.write(strline);
    }
    xFileWayArea.close();

    return 0;
}