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;

    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();
//            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;
            npid++;
            xvectorpoint.push_back(xpoint);
        }
    }

    int npointsize = xvectorpoint.size();
    for(i=0;i<npointsize;i++)
    {
        iv::vectormap::node xnode;
        xnode.nid = i;
        xnode.pid = xvectorpoint[i].pid;
        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;
        xvectorlane.push_back(xlane);
    }

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

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


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

    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(" node 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++)
    {
        snprintf(strline,1000,"%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%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,
                 20,20,0,0);
        xFileLane.write(strline);
    }
    xFileLane.close();

    return 0;
}