change map_collectfromveh, complete.

src/common/common/xodr/OpenDrive/RoadGeometry.cpp

@@ -169,7 +169,7 @@ double RoadGeometry::GetLength()
 bool RoadGeometry::CheckInterval (double s_check)
 {
 //    if ((s_check >= mS) && (s_check<=mS2))
-    if ((s_check >= mS) && (s_check<=(mS2+0.00001)))   //Solve End Problem.
+    if ((s_check >= (mS-0.000001)) && (s_check<=(mS2+0.01)))   //Solve End Problem.
 		return true;
 	else
 		return false;

src/tool/map_collectfromveh/collectconvert.cpp

@@ -3,6 +3,8 @@
 #include "gnss_coordinate_convert.h"
 #include "geofit.h"
 
+#include "autoconnect.h"
+
 int groadid = 10001;
 CollectConvert::CollectConvert(iv::map::collectveh & xcollectveh,OpenDrive * pxodr)
 {
@@ -26,6 +28,13 @@ void CollectConvert::threadconvert()
         ConvertRoad(proad,pxodr);
 
     }
+    for(i=0;i<mcollectveh.mconnects_size();i++)
+    {
+        iv::map::roadconnect * pconnect = mcollectveh.mutable_mconnects(i);
+        convertconnect(pconnect,pxodr);
+    }
+    AutoConnect pAC(pxodr);
+    pAC.Connect();
 }
 
 void CollectConvert::ConvertRoad(iv::map::collectvehroad *proad, OpenDrive *pxodr)
@@ -103,7 +112,7 @@ void CollectConvert::ConvertRoad(iv::map::collectvehroad *proad, OpenDrive *pxod
             if(dismax < LINE_ERROR)
             {
                 bArcOk = true;
-                std::cout<<" a arc is ok "<<"pos is "<<ncurpos<<" range is "<<nrange<<" error is "<<dismax<<std::endl;
+  //              std::cout<<" a arc is ok "<<"pos is "<<ncurpos<<" range is "<<nrange<<" error is "<<dismax<<std::endl;
             }
         }
 
@@ -122,14 +131,14 @@ void CollectConvert::ConvertRoad(iv::map::collectvehroad *proad, OpenDrive *pxod
         if(dismax < LINE_ERROR)
         {
             bLineOk = true;
-            std::cout<<" a line is ok "<<"pos is "<<ncurpos<<" range is "<<nrange<<" error is "<<dismax<<std::endl;
+ //           std::cout<<" a line is ok "<<"pos is "<<ncurpos<<" range is "<<nrange<<" error is "<<dismax<<std::endl;
         }
 
         if((nrange <= 2)||bLineOk||bArcOk)
         {
             if((bLineOk)||(nrange<=2))
             {
-                std::cout<<"use line"<<std::endl;
+ //               std::cout<<"use line"<<std::endl;
                 geobase xgeo;
                 xgeo.mnStartPoint = ncurpos;
                 xgeo.mnEndPoint = ncurpos + nrange -1 ;
@@ -219,7 +228,7 @@ void CollectConvert::ConvertRoad(iv::map::collectvehroad *proad, OpenDrive *pxod
         {
             if(ncurpos == (int)(xvectorcollectxy.size()-1))
             {
-                std::cout<<"Last section, 2 Points, use line."<<std::endl;
+ //               std::cout<<"Last section, 2 Points, use line."<<std::endl;
                 VectorXd x_veh1(2);
                 VectorXd y_veh1(2);
                 x_veh1[0] = fLastX;
@@ -247,11 +256,14 @@ void CollectConvert::ConvertRoad(iv::map::collectvehroad *proad, OpenDrive *pxod
     }
 
     double snow = 0;
+    double froadlen = 0;
     for(i=0;i<(int)xvectorgeo.size();i++)
     {
         geobase * pgeo = &xvectorgeo[i];
+        froadlen = froadlen + xvectorgeo[i].mfLen;
 //        std::cout<<"geo len: "<<pgeo->mfLen<<" type"<<pgeo->mnType<<std::endl;
         double tems = 0;
+        xvectorcollectxy[pgeo->mnStartPoint].s = snow;
         for(j=(pgeo->mnStartPoint+1);j<=pgeo->mnEndPoint;j++)
         {
             xvectorcollectxy[j].s = snow + tems+xvectorcollectxy[j].fdistolast;
@@ -321,7 +333,7 @@ void CollectConvert::ConvertRoad(iv::map::collectvehroad *proad, OpenDrive *pxod
         }
     }
 
-    pxodr->AddRoad(proad->strroadname(),xvectorcollectxy[xvectorcollectxy.size()-1].s, QString::number(groadid).toStdString(),"-1");
+    pxodr->AddRoad(proad->strroadname(),froadlen, QString::number(groadid).toStdString(),"-1");
     Road * p = pxodr->GetRoad(pxodr->GetRoadCount() - 1);
     groadid++;
 
@@ -330,7 +342,7 @@ void CollectConvert::ConvertRoad(iv::map::collectvehroad *proad, OpenDrive *pxod
     double s = 0;
     j= 0;
 //        for(j=0;j<4;j++)
-    for(j=0;j<xvectorgeo.size();j++)
+    for(j=0;j<(int)xvectorgeo.size();j++)
     {
 
         p->AddGeometryBlock();
@@ -352,7 +364,7 @@ void CollectConvert::ConvertRoad(iv::map::collectvehroad *proad, OpenDrive *pxod
             break;
         case 2:
             pbez = &xvectorgeo[j];
-            std::cout<<"u0:"<<pbez->mfu[0]<<std::endl;
+//            std::cout<<"u0:"<<pbez->mfu[0]<<std::endl;
             pgb->AddGeometryParamPoly3(s,pbez->mfX,pbez->mfY,
                                        pbez->mfHdg,pbez->mfLen,pbez->mfu[0],
                                        pbez->mfu[1],pbez->mfu[2],pbez->mfu[3],pbez->mfv[0],
@@ -459,6 +471,7 @@ void CollectConvert::convertconnect(iv::map::roadconnect *pconnect, OpenDrive *p
     int i;
     p1 = NULL;
     p2 = NULL;
+    double fwidth1,fwidth2;
     for(i=0;i<(int)pxodr->GetRoadCount();i++)
     {
         if(pxodr->GetRoad(i)->GetRoadName() == strroad1name)
@@ -485,6 +498,54 @@ void CollectConvert::convertconnect(iv::map::roadconnect *pconnect, OpenDrive *p
     p1->GetGeometryCoords(p1->GetRoadLength() - 0.001,startx,starty,starthdg);
     p2->GetGeometryCoords(0,endx,endy,endhdg);
 
+//    std::cout<<"start x : "<<startx<<" start y: "<<starty<<std::endl;
+
+    LaneSection * pLS1 = p1->GetLastLaneSection();
+    if(pLS1 == NULL)
+    {
+        std::cout<<" road p1 no lanesection."<<std::endl;
+        return;
+    }
+    Lane * pLane = NULL;
+    for(i=0;i<(int)pLS1->GetLaneCount();i++)
+    {
+        Lane * pLaneX = pLS1->GetLane(i);
+        if(pLaneX->GetId() == -1)
+        {
+            pLane = pLaneX;
+            break;
+        }
+    }
+    if(pLane == NULL)
+    {
+        std::cout<<"can't get road1 lane."<<std::endl;
+        return;
+    }
+    fwidth1 = pLane->GetWidthValue(p1->GetRoadLength() - 0.001);
+
+     pLS1 = p2->GetLaneSection(0);
+    if(pLS1 == NULL)
+    {
+        std::cout<<" road p2 no lanesection."<<std::endl;
+        return;
+    }
+    pLane = NULL;
+    for(i=0;i<(int)pLS1->GetLaneCount();i++)
+    {
+        Lane * pLaneX = pLS1->GetLane(i);
+        if(pLaneX->GetId() == -1)
+        {
+            pLane = pLaneX;
+            break;
+        }
+    }
+    if(pLane == NULL)
+    {
+        std::cout<<"can't get road2 lane."<<std::endl;
+        return;
+    }
+    fwidth2 = pLane->GetWidthValue(0.001);
+
     //Create Geo
     double R = 6.0;
     std::vector<geobase> xvectorgeo;
@@ -536,9 +597,9 @@ void CollectConvert::convertconnect(iv::map::roadconnect *pconnect, OpenDrive *p
 
     double xroadlen = 0;
 
-    if(pconnect->mmode() == iv::map::roadconnect_ConnectMode_UTURN)
+    if(pconnect->mmode() != iv::map::roadconnect_ConnectMode_UTURN)
     {
-        for(i=0;i<xvectorgeo.size();i++)xroadlen = xroadlen + xvectorgeo[i].mfLen;
+        for(i=0;i<(int)xvectorgeo.size();i++)xroadlen = xroadlen + xvectorgeo[i].mfLen;
 
         pxodr->AddRoad("",xroadlen, QString::number(groadid).toStdString(),"-1");
         groadid++;
@@ -550,7 +611,7 @@ void CollectConvert::convertconnect(iv::map::roadconnect *pconnect, OpenDrive *p
         double s = 0;
         int j;
         j= 0;
-        for(j=0;j<xvectorgeo.size();j++)
+        for(j=0;j<(int)xvectorgeo.size();j++)
         {
             p->AddGeometryBlock();
             GeometryBlock * pgb = p->GetGeometryBlock(j);
@@ -584,7 +645,11 @@ void CollectConvert::convertconnect(iv::map::roadconnect *pconnect, OpenDrive *p
         pLS->SetS(0);
         pLS->AddLane(0,0,"none",false);
 
+        double b = (fwidth2 - fwidth1)/xroadlen;
 
+        pLS->AddLane(-1,-1,"driving",false);
+        pLane = pLS->GetLastAddedLane();
+        pLane->AddWidthRecord(0,fwidth1,b,0,0);
 
 
 
@@ -593,8 +658,8 @@ void CollectConvert::convertconnect(iv::map::roadconnect *pconnect, OpenDrive *p
     {
         double xroadlen1 = 0;
         double xroadlen2 = 0;
-        for(i=0;i<xvectorgeo1.size();i++)xroadlen1 = xroadlen1 + xvectorgeo1[i].mfLen;
-        for(i=0;i<xvectorgeo2.size();i++)xroadlen2 = xroadlen2 + xvectorgeo2[i].mfLen;
+        for(i=0;i<(int)xvectorgeo1.size();i++)xroadlen1 = xroadlen1 + xvectorgeo1[i].mfLen;
+        for(i=0;i<(int)xvectorgeo2.size();i++)xroadlen2 = xroadlen2 + xvectorgeo2[i].mfLen;
 
         int index1 = pxodr->AddRoad("",xroadlen1, QString::number(groadid).toStdString(),"-1");groadid++;
         int index2 = pxodr->AddRoad("",xroadlen2, QString::number(groadid).toStdString(),"-1");groadid++;
@@ -606,7 +671,7 @@ void CollectConvert::convertconnect(iv::map::roadconnect *pconnect, OpenDrive *p
         double s = 0;
         int j;
         j= 0;
-        for(j=0;j<xvectorgeo1.size();j++)
+        for(j=0;j<(int)xvectorgeo1.size();j++)
         {
             proad1->AddGeometryBlock();
             GeometryBlock * pgb = proad1->GetGeometryBlock(j);
@@ -637,7 +702,7 @@ void CollectConvert::convertconnect(iv::map::roadconnect *pconnect, OpenDrive *p
         }
 
         s=0.0;
-        for(j=0;j<xvectorgeo2.size();j++)
+        for(j=0;j<(int)xvectorgeo2.size();j++)
         {
             proad2->AddGeometryBlock();
             GeometryBlock * pgb = proad2->GetGeometryBlock(j);
@@ -677,6 +742,20 @@ void CollectConvert::convertconnect(iv::map::roadconnect *pconnect, OpenDrive *p
         pLS2->SetS(0);
         pLS2->AddLane(0,0,"none",false);
 
+        double b1,b2;
+        double fwidth3;
+        fwidth3 = fwidth1 + (fwidth2 - fwidth1)*(xroadlen1)/(xroadlen1+xroadlen2);
+        b1 = (fwidth3 - fwidth1)/xroadlen1;
+        b2 = (fwidth2 - fwidth3)/xroadlen2;
+
+        pLS1->AddLane(-1,-1,"driving",false);
+        pLane = pLS1->GetLastAddedLane();
+        pLane->AddWidthRecord(0,fwidth1,b1,0,0);
+
+        pLS2->AddLane(-1,-1,"driving",false);
+        pLane = pLS2->GetLastAddedLane();
+        pLane->AddWidthRecord(0,fwidth3,b2,0,0);
+
 
 
     }

src/tool/map_collectfromveh/mainwindow.cpp

@@ -730,12 +730,20 @@ void MainWindow::onEditWinClose()
 #include "OpenDrive/OpenDriveXmlWriter.h"
 void MainWindow::on_actionConvert_triggered()
 {
+    if(mcollectveh.mroads_size() == 0)
+    {
+        QMessageBox::warning(this,"Warning","No Roads",QMessageBox::YesAll);
+        return;
+    }
     OpenDrive xxodr;
     CollectConvert * pco = new CollectConvert(mcollectveh,&xxodr);
     pco->StartConvert();
     delete pco;
 
+    QString str = QFileDialog::getSaveFileName(this,"Save OpenDrive",".","*.xodr");
+    if(str.isEmpty())return;
+    if(str.right(5) != ".xodr")str = str + ".xodr";
     OpenDriveXmlWriter x(&xxodr);
-    x.WriteFile("/home/yuchuli/test0331.xodr");
+    x.WriteFile(str.toStdString());
 
 }

src/tool/map_collectfromveh/map_collectfromveh.pro

@@ -16,6 +16,7 @@ SOURCES += \
     ../map_lanetoxodr/TinyXML/tinyxml.cpp \
     ../map_lanetoxodr/TinyXML/tinyxmlerror.cpp \
     ../map_lanetoxodr/TinyXML/tinyxmlparser.cpp \
+    ../map_lanetoxodr/autoconnect.cpp \
     ../map_lanetoxodr/circlefitting.cpp \
     ../map_lanetoxodr/const.cpp \
     ../map_lanetoxodr/fresnl.cpp \
@@ -36,6 +37,7 @@ HEADERS += \
     ../../include/msgtype/gpsimu.pb.h \
     ../map_lanetoxodr/TinyXML/tinystr.h \
     ../map_lanetoxodr/TinyXML/tinyxml.h \
+    ../map_lanetoxodr/autoconnect.h \
     ../map_lanetoxodr/circlefitting.h \
     ../map_lanetoxodr/geofit.h \
     collectconvert.h \