change driver/driver_map_xodrload. fix some bug. test ok.

src/common/common/xodr/xodrfunc/xodrdijkstra.cpp

@@ -124,10 +124,13 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
             continue;
         }
 
+
+
 //        qDebug("lane count is %d left %d right %d" ,px->GetLaneSection(0)->GetLaneCount(),px->GetLaneSection(0)->GetLeftLaneCount(),
 //               px->GetLaneSection(0)->GetRightLaneCount());
 
         int j;
+
         for(j=0;j<px->GetLaneSectionCount();j++)
         {
             if(px->GetLaneSection(j)->GetLaneCount()<2)
@@ -136,6 +139,25 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
                 continue;
             }
 
+            bool bHaveLeftDrving = false;
+            bool bHaveRightDriving = false;
+
+            unsigned int k;
+            for(k=0;k<px->GetLaneSection(j)->GetLaneCount();k++)
+            {
+                if(px->GetLaneSection(j)->GetLane(k)->GetType() == "driving")
+                {
+                    if(px->GetLaneSection(j)->GetLane(k)->GetId()>0)
+                    {
+                        bHaveLeftDrving = true;
+                    }
+                    if(px->GetLaneSection(j)->GetLane(k)->GetId()<0)
+                    {
+                        bHaveRightDriving = true;
+                    }
+                }
+            }
+
             if(px->GetLaneSection(j)->GetLeftLaneCount()>0)
             {
 
@@ -154,9 +176,9 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
                 roadedge xroad(atoi(px->GetRoadId().data()),atoi(px->GetRoadJunction().data()),fseclen,1,j,px);
 //                roadedge xroad(atoi(px->GetRoadId().data()),atoi(px->GetRoadJunction().data()),px->GetRoadLength(),1,j,px);
 
-
                 //           xroad.mpx = px;
-                mroadedge.push_back(xroad);
+                if(bHaveLeftDrving)
+                    mroadedge.push_back(xroad);
             }
             if(px->GetLaneSection(j)->GetRightLaneCount()>0)
             {
@@ -180,7 +202,8 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
 //                roadedge xroad(atoi(px->GetRoadId().data()),atoi(px->GetRoadJunction().data()),px->GetRoadLength(),2,j,px);
 
                 //           xroad.mpx = px;
-                mroadedge.push_back(xroad);
+                if(bHaveRightDriving)
+                    mroadedge.push_back(xroad);
             }
         }
 //        qDebug("id = %s  juncton = %s ",px->GetRoadId().data(),px->GetRoadJunction().data());
@@ -401,6 +424,7 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
 
                 mlanenet.push_back(lnu);
                 iv::vertexsame vx(a,b);
+
                 xvertexsame.push_back(vx);
             }
 
@@ -411,10 +435,17 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
 
     givlog->debug("After Junction vertex same is %d ",xvertexsame.size());
 
+
+
     for(i=0;i<nroadsize;i++)
     {
         Road * px = mpxodr->GetRoad(i);
 
+        if(px->GetRoadId() == "10034")
+        {
+            int ccc = 1;
+        }
+
         if(px->GetLaneSectionCount() == 1)
         {
             if(px->GetPredecessor() != 0)
@@ -446,6 +477,7 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
                                 if(npre < 0)
                                 {
                                     int nedgepre = getroadedge(nroadpre,2,GetRoadByID(nroadpre)->GetLaneSectionCount()-1);
+                                    if(nedgepre <0)continue;
                                     lnu.mnFromSection = GetRoadByID(nroadpre)->GetLaneSectionCount()-1;
                                     a = mroadedge[nedgepre].mvertexend;
                                     lnu.mpFromRoad = GetRoadByID(nroadpre);
@@ -456,6 +488,7 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
                                 else
                                 {
                                     int nedgepre = getroadedge(nroadpre,1,GetRoadByID(nroadpre)->GetLaneSectionCount()-1);
+                                    if(nedgepre <0)continue;
                                     lnu.mnToSection = GetRoadByID(nroadpre)->GetLaneSectionCount()-1;
                                     a = mroadedge[nedgepre].mvertexstart;
                                     lnu.mpFromRoad = GetRoadByID(nroadcur);
@@ -470,6 +503,7 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
                                 if(npre < 0)
                                 {
                                     int nedgepre = getroadedge(nroadpre,2);
+                                    if(nedgepre <0)continue;
                                     a = mroadedge[nedgepre].mvertexstart;
                                     lnu.mpFromRoad = GetRoadByID(nroadcur);
                                     lnu.mpToRoad = GetRoadByID(nroadpre);
@@ -479,6 +513,7 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
                                 else
                                 {
                                     int nedgepre = getroadedge(nroadpre,1);
+                                    if(nedgepre <0)continue;
                                     a = mroadedge[nedgepre].mvertexend;
                                     lnu.mpFromRoad = GetRoadByID(nroadpre);
                                     lnu.mpToRoad = GetRoadByID(nroadcur);
@@ -489,13 +524,22 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
                             if(nid < 0)
                             {
                                 int nedgecur = getroadedge(nroadcur,2);
+                                if(nedgecur< 0)
+                                {
+                                    continue;
+                                }
                                 b = mroadedge[nedgecur].mvertexstart;
                             }
                             else
                             {
                                 int nedgecur = getroadedge(nroadcur,1);
+                                if(nedgecur< 0)
+                                {
+                                    continue;
+                                }
                                 b = mroadedge[nedgecur].mvertexend;
                             }
+
                             mlanenet.push_back(lnu);
                             iv::vertexsame vx(a,b);
                             xvertexsame.push_back(vx);
@@ -542,6 +586,10 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
                                 if(nnext < 0)
                                 {
                                     int nedgenext = getroadedge(nroadnext,2,GetRoadByID(nroadnext)->GetLaneSectionCount()-1);
+                                    if(nedgenext < 0)
+                                    {
+                                        continue;
+                                    }
                                     b = mroadedge[nedgenext].mvertexend;
                                     lnu.mnFromSection = GetRoadByID(nroadnext)->GetLaneSectionCount()-1;
                                     lnu.mpFromRoad = GetRoadByID(nroadnext);
@@ -553,6 +601,10 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
                                 else
                                 {
                                     int nedgenext = getroadedge(nroadnext,1,GetRoadByID(nroadnext)->GetLaneSectionCount()-1);
+                                    if(nedgenext < 0)
+                                    {
+                                        continue;
+                                    }
                                     b = mroadedge[nedgenext].mvertexstart;
                                     lnu.mpFromRoad = GetRoadByID(nroadcur);
                                     lnu.mnToSection = GetRoadByID(nroadnext)->GetLaneSectionCount()-1;
@@ -567,6 +619,10 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
                                 if(nnext < 0)
                                 {
                                     int nedgenext = getroadedge(nroadnext,2);
+                                    if(nedgenext < 0)
+                                    {
+                                        continue;
+                                    }
                                     b = mroadedge[nedgenext].mvertexstart;
 
                                     lnu.mpFromRoad = GetRoadByID(nroadcur);
@@ -577,6 +633,10 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
                                 else
                                 {
                                     int nedgenext = getroadedge(nroadnext,1);
+                                    if(nedgenext < 0)
+                                    {
+                                        continue;
+                                    }
                                     b = mroadedge[nedgenext].mvertexend;
 
                                     lnu.mpFromRoad = GetRoadByID(nroadnext);
@@ -588,11 +648,19 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
                             if(nid < 0)
                             {
                                 int nedgecur = getroadedge(nroadcur,2,px->GetLaneSectionCount()-1);
+                                if(nedgecur < 0)
+                                {
+                                    continue;
+                                }
                                 a = mroadedge[nedgecur].mvertexend;
                             }
                             else
                             {
                                 int nedgecur = getroadedge(nroadcur,1,px->GetLaneSectionCount() -1);
+                                if(nedgecur < 0)
+                                {
+                                    continue;
+                                }
                                 a = mroadedge[nedgecur].mvertexstart;
                             }
                             iv::vertexsame vx(a,b);
@@ -649,6 +717,11 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
                                 if(npre < 0)
                                 {
                                     int nedgepre = getroadedge(nroadpre,2,pRoad_Pre->GetLaneSectionCount()-1);
+                                    if(nedgepre < 0)
+                                    {
+                                            continue;
+
+                                    }
                                     a = mroadedge[nedgepre].mvertexend;
                                     lnu.mpFromRoad = GetRoadByID(nroadpre);
                                     lnu.mpToRoad = GetRoadByID(nroadcur);
@@ -660,6 +733,11 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
                                 else
                                 {
                                     int nedgepre = getroadedge(nroadpre,1,pRoad_Pre->GetLaneSectionCount()-1);
+                                    if(nedgepre < 0)
+                                    {
+                                            continue;
+
+                                    }
                                     a = mroadedge[nedgepre].mvertexstart;
                                     lnu.mpFromRoad = GetRoadByID(nroadcur);
                                     lnu.mpToRoad = GetRoadByID(nroadpre);
@@ -675,6 +753,11 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
                                 if(npre < 0)
                                 {
                                     int nedgepre = getroadedge(nroadpre,2);
+                                    if(nedgepre < 0)
+                                    {
+                                            continue;
+
+                                    }
                                     a = mroadedge[nedgepre].mvertexstart;
                                     lnu.mpFromRoad = GetRoadByID(nroadcur);
                                     lnu.mpToRoad = GetRoadByID(nroadpre);
@@ -686,6 +769,11 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
                                 else
                                 {
                                     int nedgepre = getroadedge(nroadpre,1);
+                                    if(nedgepre < 0)
+                                    {
+                                            continue;
+
+                                    }
                                     a = mroadedge[nedgepre].mvertexend;
                                     lnu.mpFromRoad = GetRoadByID(nroadpre);
                                     lnu.mpToRoad = GetRoadByID(nroadcur);
@@ -698,17 +786,26 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
                             if(nid < 0)
                             {
                                 int nedgecur = getroadedge(nroadcur,2);
+                                if(nedgecur < 0)
+                                {
+                                    continue;
+                                }
                                 b = mroadedge[nedgecur].mvertexstart;
                             }
                             else
                             {
                                 int nedgecur = getroadedge(nroadcur,1);
+                                if(nedgecur < 0)
+                                {
+                                    continue;
+                                }
                                 b = mroadedge[nedgecur].mvertexend;
                             }
                             iv::vertexsame vx(a,b);
                             xvertexsame.push_back(vx);
                             mlanenet.push_back(lnu);
 
+
                         }
                         if(plane->IsSuccessorSet())
                         {
@@ -807,6 +904,10 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
                                 if(nnext < 0)
                                 {
                                     int nedgenext = getroadedge(nroadnext,2,GetRoadByID(nroadnext)->GetLaneSectionCount()-1);
+                                    if(nedgenext < 0)
+                                    {
+                                        continue;
+                                    }
                                     b = mroadedge[nedgenext].mvertexend;
 
                                     lnu.mpFromRoad = GetRoadByID(nroadnext);
@@ -819,6 +920,10 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
                                 else
                                 {
                                     int nedgenext = getroadedge(nroadnext,1,GetRoadByID(nroadnext)->GetLaneSectionCount()-1);
+                                    if(nedgenext < 0)
+                                    {
+                                        continue;
+                                    }
                                     b = mroadedge[nedgenext].mvertexstart;
 
                                     lnu.mpFromRoad = GetRoadByID(nroadcur);
@@ -835,6 +940,10 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
                                 if(nnext < 0)
                                 {
                                     int nedgenext = getroadedge(nroadnext,2);
+                                    if(nedgenext < 0)
+                                    {
+                                        continue;
+                                    }
                                     b = mroadedge[nedgenext].mvertexstart;
 
                                     lnu.mpFromRoad = GetRoadByID(nroadcur);
@@ -847,6 +956,10 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
                                 else
                                 {
                                     int nedgenext = getroadedge(nroadnext,1);
+                                    if(nedgenext < 0)
+                                    {
+                                        continue;
+                                    }
                                     b = mroadedge[nedgenext].mvertexend;
 
                                     lnu.mpFromRoad = GetRoadByID(nroadnext);
@@ -860,14 +973,24 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
                             if(nid < 0)
                             {
                                 int nedgecur = getroadedge(nroadcur,2,px->GetLaneSectionCount()-1);
+                                if(nedgecur < 0)
+                                {
+                                    continue;
+                                }
                                 a = mroadedge[nedgecur].mvertexend;
                             }
                             else
                             {
                                 int nedgecur = getroadedge(nroadcur,1,px->GetLaneSectionCount()-1);
+                                if(nedgecur <0)
+                                {
+                                    continue;
+                                }
+
                                 a = mroadedge[nedgecur].mvertexstart;
                             }
                             iv::vertexsame vx(a,b);
+
                             xvertexsame.push_back(vx);
                             mlanenet.push_back(lnu);
 
@@ -880,6 +1003,15 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
 
         }
 
+        int k;
+        for(k=0;k<xvertexsame.size();k++)
+        {
+            if((xvertexsame[k].ma == 0) &&(xvertexsame[k].mb == 127))
+            {
+                int bbb =1;
+            }
+        }
+
 
 
     }
@@ -1160,7 +1292,7 @@ inline double xodrdijkstra::getedgedis(int vs, int vd)
     int i;
 
     std::vector<roadedge > * proadedge = &mroadedge;
-    double dis = std::numeric_limits<double>::max();
+    double dis = 1000000;//std::numeric_limits<double>::max();
 
     for(i=0;i<mvectorvertexedge[vs].mvectorroadedge.size();i++)
     {
@@ -1297,6 +1429,7 @@ std::vector<int> xodrdijkstra::getpath(int srcroadid, int nsrclr, int dstroadid,
 
         if(k == -1)
         {
+            qDebug("i = %d not found k",i);
             givlog->debug("i = %d not found k",i);
             break;
         }
@@ -1309,7 +1442,8 @@ std::vector<int> xodrdijkstra::getpath(int srcroadid, int nsrclr, int dstroadid,
         {
             double tmp;
             tmp = getedgedis(k,j);
-            if(tmp == std::numeric_limits<double>::max())
+ //           if(tmp == std::numeric_limits<double>::max())
+            if(tmp>500000)
             {
 
             }
@@ -1330,7 +1464,10 @@ std::vector<int> xodrdijkstra::getpath(int srcroadid, int nsrclr, int dstroadid,
 //        qDebug("  shortest(%d, %d)=%f\n", vs, i, dist[i]);
 
     for (i = 0; i < nvertexnum; i++)
-        givlog->debug("  %d =%d\n", i, prev[i]);
+    {
+  //      qDebug("  %d =%d\n", i, prev[i]);
+ //       givlog->debug("  %d =%d\n", i, prev[i]);
+    }
 
 
     if(flag[dstvertex] == 1)
@@ -1342,6 +1479,7 @@ std::vector<int> xodrdijkstra::getpath(int srcroadid, int nsrclr, int dstroadid,
         while(nend != vs)
         {
             nstart = prev[nend];
+            qDebug("vertex:%d",nstart);
             int nedge = getroadedgefromvertex(nstart,nend);
             if(nedge<0)
             {
@@ -1833,10 +1971,14 @@ std::vector<pathsection> xodrdijkstra::getgpspoint(int srcroadid, int nsrclr, in
                 }
                 else
                 {
-                    if(pLane2->IsPredecessorSet())
+                    if(pLane2->IsSuccessorSet())
                     {
-                        xpathsection[i].secondsel = pLane2->GetPredecessor();
+                        xpathsection[i].secondsel = pLane2->GetSuccessor();
                     }
+//                    if(pLane2->IsPredecessorSet())
+//                    {
+//                        xpathsection[i].secondsel = pLane2->GetPredecessor();
+//                    }
                     else
                     {
                         int k;

src/driver/driver_map_xodrload/main.cpp

@@ -482,6 +482,7 @@ void SetPlan(xodrobj xo)
     int nSize = xPlan.size();
 
     if(nSize<1)
+    {
         qDebug("plan fail.");
         return;
     }
@@ -497,6 +498,7 @@ void SetPlan(xodrobj xo)
         if(gbExtendMap)
         {
             xPlan.push_back(pp);
+        }
 
     }