change driver/driver_map_xodrload. fix some bug,need test.

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

@@ -120,7 +120,7 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
 
         if(px->GetLaneSectionCount()<1)
         {
-            qDebug("no lanesection");
+            qDebug("road %s no lanesection",px->GetRoadId().data());
             continue;
         }
 
@@ -132,7 +132,7 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
         {
             if(px->GetLaneSection(j)->GetLaneCount()<2)
             {
-                givlog->info("no lane");
+                givlog->info("road %s lanesection %d: no lane",px->GetRoadId().data(),j);
                 continue;
             }
 
@@ -204,6 +204,7 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
         mroadedge[i].mbvertex = true;
         mroadedge[i].mvertexstart = 2*i;
         mroadedge[i].mvertexend = 2*i + 1;
+        qDebug("road:%d vertex %d %d ",mroadedge[i].mroadid,mroadedge[i].mvertexstart,mroadedge[i].mvertexend);
 //        std::cout<<"road id "<<mroadedge[i].mroadid<<" lenght "<<mroadedge[i].mfedgelen<<std::endl;
     }
 
@@ -216,12 +217,13 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
     Junction * pj;
     for(i=0;i<pxodr->GetJunctionCount();i++)
     {
+
         pj = pxodr->GetJunction(i);
         int j;
-//        if(atoi(pj->GetId().data()) == 810009)
-//        {
-//            int ggg = 11;
-//        }
+        if(atoi(pj->GetId().data()) == 800012)
+        {
+            int aaa = 11;
+        }
         for(j=0;j<pj->GetJunctionConnectionCount();j++)
         {
             JunctionConnection * pjc = pj->GetJunctionConnection(j);
@@ -229,11 +231,6 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
             int road_from = atoi(pjc->GetIncomingRoad().data());
             int road_to = atoi(pjc->GetConnectingRoad().data());
 
-            if((road_from == 117)||(road_to == 117))
-            {
-                int bbb = 1;
-                bbb++;
-            }
 
             Road * pRoad_From = GetRoadByID(road_from);
             Road * pRoad_To = GetRoadByID(road_to);
@@ -272,36 +269,65 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
 
                 if(strncmp(pjc->GetContactPoint().data(),"end",255) == 0)
                 {
-//                    if(road_from == 30011)
-//                    {
-//                        int xx = 1;
-//                    }
-//                    if(lr_from == 2)
+
                     if(lr_to == 1)
                     {
-                        fromedge = getroadedge(road_from,lr_from,pRoad_From->GetLaneSectionCount() -1);
-                        toedge = getroadedge(road_to,lr_to);
-                        a = mroadedge[fromedge].mvertexend;
-                        b = mroadedge[toedge].mvertexstart;
-                        lnu.mpFromRoad = GetRoadByID(road_from);
-                        lnu.mpToRoad = GetRoadByID(road_to);
-                        lnu.mnFromSection = lnu.mpFromRoad->GetLaneSectionCount() - 1;
-                        lnu.mnToSection = 0;
-                        lnu.mnFromLane = lane_from;
-                        lnu.mnToLane = lane_to;
+                        if(lr_from == 2 )
+                        {
+                            fromedge = getroadedge(road_from,lr_from,pRoad_From->GetLaneSectionCount() -1);
+                            toedge = getroadedge(road_to,lr_to,pRoad_To->GetLaneSectionCount() -1);
+                            a = mroadedge[fromedge].mvertexend;
+                            b = mroadedge[toedge].mvertexstart;
+                            lnu.mpFromRoad = GetRoadByID(road_from);
+                            lnu.mpToRoad = GetRoadByID(road_to);
+                            lnu.mnFromSection = lnu.mpFromRoad->GetLaneSectionCount() - 1;
+                            lnu.mnToSection = pRoad_To->GetLaneSectionCount() -1;
+                            lnu.mnFromLane = lane_from;
+                            lnu.mnToLane = lane_to;
+                        }
+                        else
+                        {
+                            fromedge = getroadedge(road_from,lr_from);
+                            toedge = getroadedge(road_to,lr_to,pRoad_To->GetLaneSectionCount() -1);
+                            a = mroadedge[fromedge].mvertexend;
+                            b = mroadedge[toedge].mvertexstart;
+                            lnu.mpFromRoad = GetRoadByID(road_from);
+                            lnu.mpToRoad = GetRoadByID(road_to);
+                            lnu.mnFromSection = 0;
+                            lnu.mnToSection = pRoad_To->GetLaneSectionCount() -1;
+                            lnu.mnFromLane = lane_from;
+                            lnu.mnToLane = lane_to;
+
+                        }
                     }
                     else
                     {
-                        fromedge = getroadedge(road_from,lr_from);
-                        toedge = getroadedge(road_to,lr_to,pRoad_To->GetLaneSectionCount() -1);
-                        a = mroadedge[fromedge].mvertexstart;
-                        b = mroadedge[toedge].mvertexend;
-                        lnu.mpFromRoad = GetRoadByID(road_to);
-                        lnu.mpToRoad = GetRoadByID(road_from);
-                        lnu.mnFromSection = 0;
-                        lnu.mnToSection = lnu.mpToRoad->GetLaneSectionCount()-1;
-                        lnu.mnFromLane = lane_to;
-                        lnu.mnToLane = lane_from;
+                        if(lr_from == 2 )
+                        {
+                            fromedge = getroadedge(road_from,lr_from);
+                            toedge = getroadedge(road_to,lr_to,pRoad_To->GetLaneSectionCount() -1);
+                            a = mroadedge[fromedge].mvertexstart;
+                            b = mroadedge[toedge].mvertexend;
+                            lnu.mpFromRoad = GetRoadByID(road_to);
+                            lnu.mpToRoad = GetRoadByID(road_from);
+                            lnu.mnFromSection = 0;
+                            lnu.mnToSection = pRoad_To->GetLaneSectionCount() -1;
+                            lnu.mnFromLane = lane_to;
+                            lnu.mnToLane = lane_from;
+                        }
+                        else
+                        {
+                            fromedge = getroadedge(road_from,lr_from,pRoad_From->GetLaneSectionCount() -1);
+                            toedge = getroadedge(road_to,lr_to,pRoad_To->GetLaneSectionCount() -1);
+                            a = mroadedge[fromedge].mvertexstart;
+                            b = mroadedge[toedge].mvertexend;
+                            lnu.mpFromRoad = GetRoadByID(road_to);
+                            lnu.mpToRoad = GetRoadByID(road_from);
+                            lnu.mnFromSection = lnu.mpFromRoad->GetLaneSectionCount() - 1;
+                            lnu.mnToSection = pRoad_To->GetLaneSectionCount() -1;
+                            lnu.mnFromLane = lane_to;
+                            lnu.mnToLane = lane_from;
+                        }
                     }
 
                 }
@@ -311,37 +337,71 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
  //                   if(lr_from == 1)
                     if(lr_to == 2)
                     {
-                        fromedge = getroadedge(road_from,lr_from);
-                        toedge = getroadedge(road_to,lr_to);
-                        a = mroadedge[fromedge].mvertexend;
-                        b = mroadedge[toedge].mvertexstart;
-                        lnu.mpFromRoad = GetRoadByID(road_from);
-                        lnu.mpToRoad = GetRoadByID(road_to);
-                        lnu.mnFromSection = lnu.mpFromRoad->GetLaneSectionCount() - 1;
-                        lnu.mnToSection = 0;
-                        lnu.mnFromLane = lane_from;
-                        lnu.mnToLane = lane_to;
+                        if(lr_from ==2 )
+                        {
+                            fromedge = getroadedge(road_from,lr_from,pRoad_From->GetLaneSectionCount() -1);
+                            toedge = getroadedge(road_to,lr_to);
+                            a = mroadedge[fromedge].mvertexend;
+                            b = mroadedge[toedge].mvertexstart;
+                            lnu.mpFromRoad = GetRoadByID(road_from);
+                            lnu.mpToRoad = GetRoadByID(road_to);
+                            lnu.mnFromSection = lnu.mpFromRoad->GetLaneSectionCount() - 1;
+                            lnu.mnToSection = 0;
+                            lnu.mnFromLane = lane_from;
+                            lnu.mnToLane = lane_to;
+                        }
+                        else
+                        {
+                            fromedge = getroadedge(road_from,lr_from);
+                            toedge = getroadedge(road_to,lr_to);
+                            a = mroadedge[fromedge].mvertexend;
+                            b = mroadedge[toedge].mvertexstart;
+                            lnu.mpFromRoad = GetRoadByID(road_from);
+                            lnu.mpToRoad = GetRoadByID(road_to);
+                            lnu.mnFromSection = 0;
+                            lnu.mnToSection = 0;
+                            lnu.mnFromLane = lane_from;
+                            lnu.mnToLane = lane_to;
+                        }
                     }
                     else
                     {
-                        fromedge = getroadedge(road_from,lr_from);
-                        toedge = getroadedge(road_to,lr_to,pRoad_To->GetLaneSectionCount() -1);
-                        a = mroadedge[fromedge].mvertexstart;
-                        b = mroadedge[toedge].mvertexend;
-                        lnu.mpFromRoad = GetRoadByID(road_to);
-                        lnu.mpToRoad = GetRoadByID(road_from);
-                        lnu.mnFromSection = 0;
-                        lnu.mnToSection = lnu.mpToRoad->GetLaneSectionCount()-1;
-                        lnu.mnFromLane = lane_to;
-                        lnu.mnToLane = lane_from;
+                        if(lr_from ==2 )
+                        {
+                            fromedge = getroadedge(road_from,lr_from);
+                            toedge = getroadedge(road_to,lr_to);
+                            a = mroadedge[fromedge].mvertexstart;
+                            b = mroadedge[toedge].mvertexend;
+                            lnu.mpFromRoad = GetRoadByID(road_to);
+                            lnu.mpToRoad = GetRoadByID(road_from);
+                            lnu.mnFromSection = 0;
+                            lnu.mnToSection = 0;
+                            lnu.mnFromLane = lane_to;
+                            lnu.mnToLane = lane_from;
+                        }
+                        else
+                        {
+                            fromedge = getroadedge(road_from,lr_from,pRoad_From->GetLaneSectionCount() -1);
+                            toedge = getroadedge(road_to,lr_to);
+                            a = mroadedge[fromedge].mvertexstart;
+                            b = mroadedge[toedge].mvertexend;
+                            lnu.mpFromRoad = GetRoadByID(road_to);
+                            lnu.mpToRoad = GetRoadByID(road_from);
+                            lnu.mnFromSection = pRoad_From->GetLaneSectionCount() -1;
+                            lnu.mnToSection = 0;
+                            lnu.mnFromLane = lane_to;
+                            lnu.mnToLane = lane_from;
+                        }
                     }
 
 
-                    mlanenet.push_back(lnu);
+
+                }
+
+
+                mlanenet.push_back(lnu);
                 iv::vertexsame vx(a,b);
                 xvertexsame.push_back(vx);
-
-            }
             }
 
 
@@ -527,12 +587,12 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
                             }
                             if(nid < 0)
                             {
-                                int nedgecur = getroadedge(nroadcur,2);
+                                int nedgecur = getroadedge(nroadcur,2,px->GetLaneSectionCount()-1);
                                 a = mroadedge[nedgecur].mvertexend;
                             }
                             else
                             {
-                                int nedgecur = getroadedge(nroadcur,1);
+                                int nedgecur = getroadedge(nroadcur,1,px->GetLaneSectionCount() -1);
                                 a = mroadedge[nedgecur].mvertexstart;
                             }
                             iv::vertexsame vx(a,b);
@@ -731,15 +791,10 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
                     int nroadnext = atoi(px->GetSuccessor()->GetElementId().data());
                     int nroadcur = atoi(px->GetRoadId().data());
 
-                    if(nroadcur == 116)
-                    {
-                        int xxxxx = 1;
-                    }
-
                     for(k=0;k<px->GetLaneSection(px->GetLaneSectionCount()-1)->GetLaneCount();k++)
                     {
                         Lane * plane = px->GetLaneSection(px->GetLaneSectionCount()-1)->GetLane(k);
-                        if(plane->GetType() != "driving")continue;
+   //                     if(plane->GetType() != "driving")continue;
                         if(plane->GetId() == 0)continue;
                         if(plane->IsSuccessorSet())
                         {
@@ -833,6 +888,7 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
 
     for(i=0;i<xvertexsame.size();i++)
     {
+        qDebug("%d: %d %d",i,xvertexsame[i].ma,xvertexsame[i].mb);
         givlog->debug("%d: %d %d",i,xvertexsame[i].ma,xvertexsame[i].mb);
     }
 
@@ -889,10 +945,42 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
         }
     }
 
+    for(i=0;i<(xvertexsamearray.size()-1);i++)
+    {
+        int j;
+        for(j=(i+1);j<(xvertexsamearray.size());j++)
+        {
+            int k;
+            bool bsame = false;
+            for(k=0;k<xvertexsamearray[j].mvertexarray.size();k++)
+            {
+                if(xvertexsamearray[i].isinarray(xvertexsamearray[j].mvertexarray[k]))
+                {
+                    bsame = true;
+                    break;
+                }
+            }
+            if(bsame)
+            {
+                for(k=0;k<xvertexsamearray[j].mvertexarray.size();k++)
+                {
+                    if(!xvertexsamearray[i].isinarray(xvertexsamearray[j].mvertexarray[k]))
+                    {
+                        xvertexsamearray[i].mvertexarray.push_back(xvertexsamearray[j].mvertexarray[k]);
+                    }
+                }
+                xvertexsamearray.erase(xvertexsamearray.begin() +j);
+                j = j-1;
+
+            }
+        }
+    }
+
     for(i=0;i<xvertexsamearray.size();i++)
     {
         xvertexsamearray[i].sort();
- //       xvertexsamearray[i].print();
+
+        xvertexsamearray[i].print();
     }
 
     for(i=0;i<mroadedge.size();i++)
@@ -913,6 +1001,8 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
 
     for(i=0;i<mroadedge.size();i++)
     {
+        qDebug("%d %d %d: %d %d",mroadedge[i].mroadid,mroadedge[i].mnleftright,mroadedge[i].mnsectionid, mroadedge[i].mvertexstart,mroadedge[i].mvertexend);
+
         givlog->debug("%d %d %d: %d %d",mroadedge[i].mroadid,mroadedge[i].mnleftright,mroadedge[i].mnsectionid,  mroadedge[i].mvertexstart,mroadedge[i].mvertexend);
     }
 
@@ -994,6 +1084,7 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
 
     for(i=0;i<mroadedge.size();i++)
     {
+        qDebug("%d %d %d: %d %d",mroadedge[i].mroadid,mroadedge[i].mnleftright,mroadedge[i].mnsectionid, mroadedge[i].mvertexstart,mroadedge[i].mvertexend);
         givlog->debug("%d %d %d: %d %d",mroadedge[i].mroadid,mroadedge[i].mnleftright,mroadedge[i].mnsectionid, mroadedge[i].mvertexstart,mroadedge[i].mvertexend);
     }
 
@@ -1031,6 +1122,7 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
 
 
 
+
 }