|
|
@@ -0,0 +1,270 @@
|
|
|
+#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;
|
|
|
+}
|
