change tool_trace2vectormap, for add area support in vectormap.

src/tool/tool_trace2vectormap/mainwindow.cpp

@@ -14,8 +14,8 @@ MainWindow::MainWindow(QWidget *parent)
 
     ui->lineEdit_Lat0->setText("39.1238777");
     ui->lineEdit_Lon0->setText("117.0273054");
-    ui->lineEdit_Lcnt->setText("5");
-    ui->lineEdit_Lno->setText("3");
+    ui->lineEdit_Lcnt->setText("1");
+    ui->lineEdit_Lno->setText("0");
     ui->lineEdit_LeftDis->setText("2.3");
     ui->lineEdit_RIghtDis->setText("2.3");
     ui->lineEdit_VelLim->setText("15");

src/tool/tool_trace2vectormap/trace2vectormap.cpp

@@ -43,6 +43,11 @@ int trace2vectormap::convert()
     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++)
     {
@@ -72,8 +77,43 @@ int trace2vectormap::convert()
             xpoint.fyaw = fyaw;
             xpoint.h = 0;
             xpoint.fspeed = fspeed;
-            npid++;
-            xvectorpoint.push_back(xpoint);
+
+            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;
         }
     }
 
@@ -122,6 +162,35 @@ int trace2vectormap::convert()
         xvectorlane[i].blid = xvectorlane[i-1].lnid;
     }
 
+    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;
@@ -192,6 +261,88 @@ int trace2vectormap::convert()
         xvectorline[i + npointsize-1].blid = xvectorline[i-1 + npointsize -1].lid;
     }
 
+    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.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)].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");
@@ -310,6 +461,40 @@ int trace2vectormap::convert()
     }
     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;
 }
 

src/tool/tool_trace2vectormap/trace2vectormap.h

@@ -86,6 +86,19 @@ struct dtlane
     double RW;
 };
 
+struct area
+{
+    int aid;
+    int slid;
+    int elid;
+};
+
+struct wayarea
+{
+    int waid;
+    int aid;
+};
+
 }