change map_lanetoxodr.

src/tool/map_lanetoxodr/function/cdaproc.cpp

@@ -1,12 +1,273 @@
 #include "cdaproc.h"
 
 #include <iostream>
+#include <math.h>
 
 CDAProc::CDAProc()
 {
 
 }
 
+
+int CDAProc::ProcIntersectionRoad(OpenDrive * pxodr,  std::string strtype,std::string strradius,std::string strroadlen,std::string strlanewidth,
+    std::string strlannecount,std::string strlanetype,std::string strlanemarkcolor,std::string strlanemarktype)
+{
+    double fRoadLen =  atof(strroadlen.data());
+    double flanewidth = atof(strlanewidth.data());
+    int nlanecount = atoi(strlannecount.data());
+    if(flanewidth<=0)flanewidth = 3.5;
+    if(nlanecount<=0)nlanecount = 1;
+    double fdefradius = 6.0;
+    double finsectlen = fdefradius*2.0 + 2.0*(flanewidth*(nlanecount-1));
+    if(fRoadLen < (finsectlen+10.0))
+    {
+        fRoadLen = finsectlen + 10.0;
+    }
+    double flinelen = (fRoadLen -finsectlen)/2.0;
+    double flinex[4],fliney[4],flinehdg[4];
+    flinex[0] = 0;fliney[0]=0;flinehdg[0] = 0;
+    flinex[1] = fRoadLen*(0.5);fliney[1] = fRoadLen*(-0.5);  flinehdg[1] = M_PI/2.0;
+    flinex[2] = fRoadLen*0.5 + finsectlen*0.5;fliney[2] = 0;  flinehdg[2] = 0;
+    flinex[3] = fRoadLen*(0.5);fliney[3] = finsectlen*0.5;  flinehdg[3] = M_PI/2.0;
+
+    int i;
+    for(i=0;i<4;i++)
+    {
+        char strroadid[100];
+        snprintf(strroadid,100,"%d",i+1);
+        pxodr->AddRoad("zl",flinelen,strroadid,"-1");
+        Road * pRoad = pxodr->GetLastAddedRoad();
+        pRoad->AddGeometryBlock();
+        GeometryBlock * pgeob = pRoad->GetLastAddedGeometryBlock();
+        pgeob->AddGeometryLine(0,flinex[i],fliney[i],flinehdg[i],flinelen);
+        pRoad->AddLaneSection(0);
+        LaneSection * pLS = pRoad->GetLaneSection(0);
+        pLS->AddLane(0,0,"none",false);
+        Lane * pcenterlane = pLS->GetLastAddedLane();
+        pcenterlane->AddRoadMarkRecord(0,"solid solid","standard","yellow",0.15,"none");
+
+        int j;
+        for(j=0;j<nlanecount;j++)
+        {
+            pLS->AddLane(-1,(j+1)*(-1),"driving",false,true);
+            Lane * pnewlane = pLS->GetLastAddedLane();
+            pnewlane->AddWidthRecord(0,flanewidth,0,0,0);
+            if(j == (nlanecount -1))
+            {
+                pnewlane->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"none");
+            }
+            else
+            {
+                std::string type = "broken";
+                if(strlanemarktype == "实线")type = "solid";
+                std::string color = "standard";
+                if(strlanemarkcolor == "黄色")color = "yellow";
+                pnewlane->AddRoadMarkRecord(0,type,"standard",color,0.15,"none");
+  //              pnewlane->AddRoadMarkRecord(0,"broken","standard","standard",0.15,"none");
+            }
+            pLS->AddLane(1,(j+1)*(1),"driving",false,true);
+            pnewlane = pLS->GetLastAddedLane();
+            pnewlane->AddWidthRecord(0,flanewidth,0,0,0);
+            if(j == (nlanecount -1))
+            {
+                pnewlane->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"none");
+            }
+            else
+            {
+                std::string type = "broken";
+                if(strlanemarktype == "实线")type = "solid";
+                std::string color = "standard";
+                if(strlanemarkcolor == "黄色")color = "yellow";
+                pnewlane->AddRoadMarkRecord(0,type,"standard",color,0.15,"none");
+  //              pnewlane->AddRoadMarkRecord(0,"broken","standard","standard",0.15,"none");
+            }
+        }
+    }
+
+    double frightlen = fdefradius * M_PI/2.0;
+    double frightx[4],frighty[4],frighthdg[4];
+    frightx[0] = flinelen;frighty[0] = flanewidth*(nlanecount-1)*(-1);frighthdg[0] = 0;
+    frightx[1] = fRoadLen*0.5 + flanewidth*(nlanecount-1) ;frighty[1] = finsectlen*(-0.5);frighthdg[1] = M_PI/2.0;
+    frightx[2] = fRoadLen*0.5 + finsectlen*0.5;frighty[2] = flanewidth*(nlanecount-1)*(1);frighthdg[2] = M_PI;
+    frightx[3] = fRoadLen*0.5 - flanewidth*(nlanecount-1);frighty[3] = finsectlen*0.5;frighthdg[3] = 3.0*M_PI/2.0;
+
+    for(i=0;i<4;i++)
+    {
+        char strroadid[100];
+        snprintf(strroadid,100,"%d",i+5);
+        pxodr->AddRoad("zl",flinelen,strroadid,"-1");
+        Road * pRoad = pxodr->GetLastAddedRoad();
+        pRoad->AddGeometryBlock();
+        GeometryBlock * pgeob = pRoad->GetLastAddedGeometryBlock();
+        pgeob->AddGeometryArc(0,frightx[i],frighty[i],frighthdg[i],frightlen,-1.0/fdefradius);
+        pRoad->AddLaneSection(0);
+        LaneSection * pLS = pRoad->GetLaneSection(0);
+        pLS->AddLane(0,0,"none",false);
+//        Lane * pcenterlane = pLS->GetLastAddedLane();
+ //       pcenterlane->AddRoadMarkRecord(0,"solid","standard","yellow",0.15,"none");
+
+        pLS->AddLane(-1,-1,"driving",false,true);
+        Lane * pnewlane = pLS->GetLastAddedLane();
+        pnewlane->AddWidthRecord(0,flanewidth,0,0,0);
+    }
+
+    double fleftx[4],flefty[4],flefthdg[4];
+    fleftx[0] = flinelen;flefty[0] = 0;flefthdg[0] = 0;
+    fleftx[1] = fRoadLen*0.5;flefty[1] = finsectlen*(-0.5);flefthdg[1] = M_PI/2.0;
+    fleftx[2] = flinelen+finsectlen;flefty[2] = 0;flefthdg[2] = M_PI;
+    fleftx[3] = fRoadLen*0.5;flefty[3] = finsectlen*0.5;flefthdg[3] = 3.0*M_PI/2.0;
+    for(i=0;i<4;i++)
+    {
+        double fleftlinelen = 0.5*finsectlen - fdefradius;
+        double fleftarcx = fleftx[i] + fleftlinelen * cos(flefthdg[i]);
+        double fleftarcy = flefty[i] + fleftlinelen * sin(flefthdg[i]);
+        double fleftarchdg = flefthdg[i];
+        char strroadid[100];
+        snprintf(strroadid,100,"%d",i+9);
+        pxodr->AddRoad("zl",fleftlinelen*2.0+fdefradius*M_PI/2.0,strroadid,"-1");
+        Road * pRoad = pxodr->GetLastAddedRoad();
+
+        GeometryBlock * pgeob;
+        if(fleftlinelen >0.00000000001)
+        {
+            pRoad->AddGeometryBlock();
+            GeometryBlock * pgeob = pRoad->GetLastAddedGeometryBlock();
+            pgeob->AddGeometryLine(0,fleftx[i],flefty[i],flefthdg[i],fleftlinelen);
+        }
+        pRoad->AddGeometryBlock();
+        pgeob= pRoad->GetLastAddedGeometryBlock();
+        pgeob->AddGeometryArc(fleftlinelen,fleftarcx,fleftarcy,fleftarchdg,fdefradius*M_PI/2.0,1.0/fdefradius);
+        if(fleftlinelen >0.00000000001)
+        {
+            pRoad->AddGeometryBlock();
+            GeometryBlock * pgeob = pRoad->GetLastAddedGeometryBlock();
+            pgeob->AddGeometryLine(fleftlinelen + fdefradius*M_PI/2.0,fleftarcx + fdefradius*cos(flefthdg[i]) + fdefradius*cos(flefthdg[i] + M_PI/2.0) ,
+                                   fleftarcy + fdefradius*sin(flefthdg[i]) + fdefradius*sin(flefthdg[i] + M_PI/2.0),flefthdg[i] +M_PI/2.0,fleftlinelen);
+        }
+        pRoad->AddLaneSection(0);
+        LaneSection * pLS = pRoad->GetLaneSection(0);
+        pLS->AddLane(0,0,"none",false);
+//        Lane * pcenterlane = pLS->GetLastAddedLane();
+ //       pcenterlane->AddRoadMarkRecord(0,"solid","standard","yellow",0.15,"none");
+
+        pLS->AddLane(-1,-1,"driving",false,true);
+        Lane * pnewlane = pLS->GetLastAddedLane();
+        pnewlane->AddWidthRecord(0,flanewidth,0,0,0);
+    }
+
+    double finlinex[4],finliney[4],finlinehdg[4];
+    finlinex[0] = flinelen;finliney[0] = 0;finlinehdg[0] = 0;
+    finlinex[1] = fRoadLen*0.5;finliney[1] = finsectlen*(-0.5);finlinehdg[1] = M_PI/2.0;
+    finlinex[2] = flinelen+finsectlen;finliney[2] = 0;finlinehdg[2] = M_PI;
+    finlinex[3] = fRoadLen*0.5;finliney[3] = finsectlen*0.5;finlinehdg[3] = 3.0*M_PI/2.0;
+
+    for(i=0;i<4;i++)
+    {
+        char strroadid[100];
+        snprintf(strroadid,100,"%d",i+13);
+        pxodr->AddRoad("zl",flinelen,strroadid,"-1");
+        Road * pRoad = pxodr->GetLastAddedRoad();
+        pRoad->AddGeometryBlock();
+        GeometryBlock * pgeob = pRoad->GetLastAddedGeometryBlock();
+        pgeob->AddGeometryLine(0,finlinex[i],finliney[i],finlinehdg[i],finsectlen);
+        pRoad->AddLaneSection(0);
+        LaneSection * pLS = pRoad->GetLaneSection(0);
+        pLS->AddLane(0,0,"none",false);
+//        Lane * pcenterlane = pLS->GetLastAddedLane();
+//        pcenterlane->AddRoadMarkRecord(0,"solid solid","standard","yellow",0.15,"none");
+
+        int j;
+        for(j=0;j<nlanecount;j++)
+        {
+            pLS->AddLane(-1,(j+1)*(-1),"driving",false,true);
+            Lane * pnewlane = pLS->GetLastAddedLane();
+            pnewlane->AddWidthRecord(0,flanewidth,0,0,0);
+//            if(j == (nlanecount -1))
+//            {
+//                pnewlane->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"none");
+//            }
+//            else
+//                pnewlane->AddRoadMarkRecord(0,"broken","standard","standard",0.15,"none");
+            pLS->AddLane(1,(j+1)*(1),"driving",false,true);
+            pnewlane = pLS->GetLastAddedLane();
+            pnewlane->AddWidthRecord(0,flanewidth,0,0,0);
+//            if(j == (nlanecount -1))
+//            {
+//                pnewlane->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"none");
+//            }
+//            else
+//                pnewlane->AddRoadMarkRecord(0,"broken","standard","standard",0.15,"none");
+        }
+    }
+
+
+    return 0;
+}
+
+int CDAProc::ProcArcRoad(OpenDrive * pxodr,  std::string strtype,std::string strradius,std::string strroadlen,std::string strlanewidth,
+    std::string strlannecount,std::string strlanetype,std::string strlanemarkcolor,std::string strlanemarktype)
+{
+    double fRoadLen =  atof(strroadlen.data());
+    double fRadius = atof(strradius.data());
+    if(fabs(fRadius) < 0.000000000000001)return -1;
+    pxodr->AddRoad("wd",fRoadLen,"1","-1");
+    Road * pRoad = pxodr->GetLastAddedRoad();
+
+
+    double fArcLen = fabs(fRadius) * M_PI/2.0;
+    bool bHaveLine = false;
+    double fxline,fyline,fhdgline,flinelen;
+    if(fArcLen>=fRoadLen)
+    {
+        fArcLen = fRoadLen;
+    }
+    else
+    {
+        bHaveLine = true;
+        flinelen = fRoadLen - fArcLen;
+        fxline = fabs(fRadius);
+        fyline = fRadius;
+        fhdgline = (fyline/fxline)*(M_PI/2.0);
+
+    }
+
+    pRoad->AddGeometryBlock();
+    GeometryBlock * pgeob = pRoad->GetLastAddedGeometryBlock();
+    pgeob->AddGeometryArc(0,0,0,0,fArcLen,1.0/fRadius);
+    pRoad->AddGeometryBlock();
+    pgeob = pRoad->GetLastAddedGeometryBlock();
+    pgeob->AddGeometryLine(fArcLen,fxline,fyline,fhdgline,flinelen);
+    pRoad->AddLaneSection(0);
+    LaneSection * pLS = pRoad->GetLaneSection(0);
+    pLS->AddLane(0,0,"none",false);
+    Lane * pcenterlane = pLS->GetLastAddedLane();
+    pcenterlane->AddRoadMarkRecord(0,"solid","standard","yellow",0.15,"none");
+
+    int nlanecount = atoi(strlannecount.data());
+    double flanewidth = atof(strlanewidth.data());
+    int i;
+    for(i=0;i<nlanecount;i++)
+    {
+        pLS->AddLane(-1,(i+1)*(-1),"driving",false,true);
+        Lane * pnewlane = pLS->GetLastAddedLane();
+        pnewlane->AddWidthRecord(0,flanewidth,0,0,0);
+        if(i == (nlanecount -1))
+        {
+            pnewlane->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"none");
+        }
+        else
+        {
+            std::string type = "broken";
+            if(strlanemarktype == "实线")type = "solid";
+            std::string color = "standard";
+            if(strlanemarkcolor == "黄色")color = "yellow";
+            pnewlane->AddRoadMarkRecord(0,type,"standard",color,0.15,"none");
+//              pnewlane->AddRoadMarkRecord(0,"broken","standard","standard",0.15,"none");
+        }
+    }
+    return 0;
+}
+
 int CDAProc::ProcLineRoad(OpenDrive * pxodr,  std::string strtype,std::string strradius,std::string strroadlen,std::string strlanewidth,
     std::string strlannecount,std::string strlanetype,std::string strlanemarkcolor,std::string strlanemarktype)
 {
@@ -20,7 +281,7 @@ int CDAProc::ProcLineRoad(OpenDrive * pxodr,  std::string strtype,std::string st
     LaneSection * pLS = pRoad->GetLaneSection(0);
     pLS->AddLane(0,0,"none",false);
     Lane * pcenterlane = pLS->GetLastAddedLane();
-    pcenterlane->AddRoadMarkRecord(0,"solid solid","standard","yellow",0.15,"none");
+    pcenterlane->AddRoadMarkRecord(0,"solid","standard","yellow",0.15,"none");
 
     int nlanecount = atoi(strlannecount.data());
     double flanewidth = atof(strlanewidth.data());
@@ -35,7 +296,14 @@ int CDAProc::ProcLineRoad(OpenDrive * pxodr,  std::string strtype,std::string st
             pnewlane->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"none");
         }
         else
-            pnewlane->AddRoadMarkRecord(0,"broken","standard","standard",0.15,"none");
+        {
+            std::string type = "broken";
+            if(strlanemarktype == "实线")type = "solid";
+            std::string color = "standard";
+            if(strlanemarkcolor == "黄色")color = "yellow";
+            pnewlane->AddRoadMarkRecord(0,type,"standard",color,0.15,"none");
+//              pnewlane->AddRoadMarkRecord(0,"broken","standard","standard",0.15,"none");
+        }
     }
     return 0;
 }
@@ -53,7 +321,7 @@ int CDAProc::Proc(std::string strxlsxpath,OpenDrive * pxodr)
     std::string strlanemarkcolor;
     std::string strlanemarktype;
     ServiceExcelAPI.getcellvalue(pexcel,119,4,strtype);
-    ServiceExcelAPI.getcellvalue(pexcel,129,4,strradius);
+    ServiceExcelAPI.getcellvalue(pexcel,120,4,strradius);
     ServiceExcelAPI.getcellvalue(pexcel,121,4,strroadlen);
     ServiceExcelAPI.getcellvalue(pexcel,122,4,strlanewidth);
     ServiceExcelAPI.getcellvalue(pexcel,123,4,strlannecount);
@@ -68,5 +336,15 @@ int CDAProc::Proc(std::string strxlsxpath,OpenDrive * pxodr)
         std::cout<<" is zl . "<<std::endl;
         return ProcLineRoad(pxodr,strtype,strradius,strroadlen,strlanewidth,strlannecount,strlanetype,strlanemarkcolor,strlanemarktype);
     }
+    if(strtype == "弯路" )
+    {
+        std::cout<<" is wl . "<<std::endl;
+        return ProcArcRoad(pxodr,strtype,strradius,strroadlen,strlanewidth,strlannecount,strlanetype,strlanemarkcolor,strlanemarktype);
+    }
+    if(strtype == "路口" )
+    {
+        std::cout<<" is lk . "<<std::endl;
+        return ProcIntersectionRoad(pxodr,strtype,strradius,strroadlen,strlanewidth,strlannecount,strlanetype,strlanemarkcolor,strlanemarktype);
+    }
     return 0;
 }

src/tool/map_lanetoxodr/function/cdaproc.h

@@ -13,6 +13,10 @@ public:
     static int Proc(std::string strxlsxpath,OpenDrive * pxodr);
     static int ProcLineRoad(OpenDrive * pxodr,  std::string strtype,std::string strradius,std::string strroadlen,std::string strlanewidth,
         std::string strlannecount,std::string strlanetype,std::string strlanemarkcolor,std::string strlanemarktype);
+    static int ProcArcRoad(OpenDrive * pxodr,  std::string strtype,std::string strradius,std::string strroadlen,std::string strlanewidth,
+        std::string strlannecount,std::string strlanetype,std::string strlanemarkcolor,std::string strlanemarktype);
+    static int ProcIntersectionRoad(OpenDrive * pxodr,  std::string strtype,std::string strradius,std::string strroadlen,std::string strlanewidth,
+        std::string strlannecount,std::string strlanetype,std::string strlanemarkcolor,std::string strlanemarktype);
 };
 
 #endif // CDAPROC_H

src/tool/map_lanetoxodr/mainwindow.cpp

@@ -5984,16 +5984,30 @@ void MainWindow::on_CreateAfter_triggered()
 
 void MainWindow::on_actionAdd_Road_From_CDA_triggered()
 {
+
+
     if(ServiceExcelAPI.IsLoad() == false)
     {
         QMessageBox::warning(this,tr("Waring"),tr("Can't load ivxlnt plugin."),QMessageBox::YesAll);
         return;
     }
-//    QString str = QFileDialog::getOpenFileName(this,"Load xlsx",".","*.xlsx");
-//    if(str.isEmpty())return;
+    QString str = QFileDialog::getOpenFileName(this,"Load xlsx",".","*.xlsx");
+    if(str.isEmpty())return;
 
-    QString str = "/home/yuchuli/1.xlsx";
+//    QString str = "/home/yuchuli/3.xlsx";
     int nrtn = CDAProc::Proc(str.toStdString(),&mxodr);
 
 
+    AutoConnect pAC(&mxodr);
+    pAC.Connect();
+    updateJunction();
+    updateCBRoad();
+    mbRefresh = true;
+    update();
+    if(mpCBViewMode->currentIndex() == 1)
+    {
+        UpdateScene();
+    }
+
+
 }