change odtolanelet. change map route ok.

src/map/odtolanelet/odtolanelet.cpp

@@ -69,8 +69,8 @@ int odtolanelet::ConvertToLanelet(std::string strlanelet2file)
 
     lanelet::Lanelet goal_lanelet;
 
-    GetLetByID(laneletMap,start_lanelet,1527);
-    GetLetByID(laneletMap,goal_lanelet,314);
+    GetLetByID(laneletMap,start_lanelet,486);
+    GetLetByID(laneletMap,goal_lanelet,2344);
 
 
     // get all possible lanes that can be used to reach goal (including all possible lane change)
@@ -366,6 +366,7 @@ int odtolanelet::GetLetByID(lanelet::LaneletMapPtr & laneletMap,lanelet::Lanelet
 void odtolanelet::MergeSame(OpenDrive * pxodr,lanelet::LaneletMapPtr & laneletMap)
 {
     lanelet::LaneletLayer::iterator  iter;
+    std::vector<std::vector<iv::LaneletSameID>> xvecotorsameID;
     for(iter = laneletMap->laneletLayer.begin();iter != laneletMap->laneletLayer.end();++iter)
     {
         if(iter->hasAttribute("odr:roadid"))
@@ -389,7 +390,7 @@ void odtolanelet::MergeSame(OpenDrive * pxodr,lanelet::LaneletMapPtr & laneletMa
 
                     if(IsRelation(pxodr,letodrid1,letroadid2))
                     {
-                        int bRtn = MergeLetPoint(iter,iterother);
+                        int bRtn = MergeLetPoint(iter,iterother,xvecotorsameID);
                         if(bRtn == 1)
                         {
 
@@ -399,10 +400,257 @@ void odtolanelet::MergeSame(OpenDrive * pxodr,lanelet::LaneletMapPtr & laneletMa
             }
         }
     }
+
+    unsigned int i,j;
+    for(i=0;i<xvecotorsameID.size();i++)
+    {
+        ExecMerge(laneletMap,xvecotorsameID.at(i));
+        std::cout<<"i: "<<i<<std::endl;
+        for(j=0;j<xvecotorsameID[i].size();j++)
+        {
+            std::cout<<"       "<<xvecotorsameID[i].at(j).mLeftID<<" "<<xvecotorsameID[i].at(j).mRightID<<std::endl;
+        }
+    }
+
+
+
+    for(iter = laneletMap->laneletLayer.begin();iter != laneletMap->laneletLayer.end();++iter)
+    {
+
+   //     ExecMerge(xvecotorsameID,iter);
+//        unsigned int iter_leftsize = static_cast<unsigned int>(iter->leftBound().size());
+//        unsigned int iter_rightsize = static_cast<unsigned int>(iter->rightBound().size());
+
+//        if((iter_leftsize<2)||(iter_rightsize<2))
+//        {
+//            continue;
+//        }
+
+    }
 }
 
-int odtolanelet::MergeLetPoint(lanelet::LaneletLayer::iterator & iter1,lanelet::LaneletLayer::iterator & iter2)
+
+void odtolanelet::ExecMerge(lanelet::LaneletMapPtr & laneletMap,std::vector<iv::LaneletSameID> & xsame)
 {
+    unsigned int nsize = static_cast<unsigned int>(xsame.size());
+    unsigned int i;
+
+    for(i=1;i<nsize;i++)
+    {
+        bool bFind1 = false;
+        bool bFind2 = false;
+        lanelet::LaneletLayer::iterator  iter1,iter2;
+        for(iter1 = laneletMap->laneletLayer.begin();iter1 != laneletMap->laneletLayer.end();++iter1)
+        {
+            if((iter1->leftBound().size()>1) && (iter1->rightBound().size()>1))
+            {
+                if(xsame[0].npos == 1)
+                {
+                    if((iter1->leftBound()[0].id() == xsame[0].mLeftID)&&((iter1->rightBound()[0].id() == xsame[0].mRightID)))
+                    {
+                        bFind1 = true;
+                        break;
+                    }
+                }
+                else
+                {
+                    if((iter1->leftBound()[iter1->leftBound().size() -1].id() == xsame[0].mLeftID)&&((iter1->rightBound()[iter1->rightBound().size() -1].id() == xsame[0].mRightID)))
+                    {
+                        bFind1 = true;
+                        break;
+                    }
+                }
+            }
+
+
+        }
+
+        for(iter2 = laneletMap->laneletLayer.begin();iter2 != laneletMap->laneletLayer.end();++iter2)
+        {
+            if((iter2->leftBound().size()>1) && (iter2->rightBound().size()>1))
+            {
+                if(xsame[i].npos == 1)
+                {
+                    if((iter2->leftBound()[0].id() == xsame[i].mLeftID)&&((iter2->rightBound()[0].id() == xsame[i].mRightID)))
+                    {
+                        bFind2 = true;
+                        break;
+                    }
+                }
+                else
+                {
+                    if((iter2->leftBound()[iter2->leftBound().size() -1].id() == xsame[i].mLeftID)&&((iter2->rightBound()[iter2->rightBound().size() -1].id() == xsame[i].mRightID)))
+                    {
+                        bFind2 = true;
+                        break;
+                    }
+                }
+            }
+        }
+
+
+
+
+        if(bFind1 & bFind2)
+        {
+            std::cout<<" merge.  "<<std::endl;
+
+            if(xsame[i].npos == 1)
+            {
+                iter2->leftBound().erase(iter2->leftBound().begin());
+                iter2->rightBound().erase(iter2->rightBound().begin());
+                if(xsame[0].npos == 1)
+                {
+                    iter2->leftBound().insert(iter2->leftBound().begin(),iter1->leftBound()[0]);
+                    iter2->rightBound().insert(iter2->rightBound().begin(),iter1->rightBound()[0]);
+                }
+                else
+                {
+                    iter2->leftBound().insert(iter2->leftBound().begin(),iter1->leftBound()[iter1->leftBound().size() -1]);
+                    iter2->rightBound().insert(iter2->rightBound().begin(),iter1->rightBound()[iter1->rightBound().size() -1]);
+                }
+            }
+            else
+            {
+                iter2->leftBound().pop_back();
+                iter2->rightBound().pop_back();
+                if(xsame[0].npos == 1)
+                {
+                    iter2->leftBound().push_back(iter1->leftBound()[0]);
+                    iter2->rightBound().push_back(iter1->rightBound()[0]);
+                }
+                else
+                {
+                    iter2->leftBound().push_back(iter1->leftBound()[iter1->leftBound().size() -1]);
+                    iter2->rightBound().push_back(iter1->rightBound()[iter1->rightBound().size() -1]);
+                }
+            }
+        }
+
+    }
+
+
+}
+
+
+void odtolanelet::ExecMerge(std::vector<std::vector<iv::LaneletSameID>> & xvecotorsameID,lanelet::LaneletLayer::iterator & iter)
+{
+    unsigned int nSize = static_cast<unsigned int >(xvecotorsameID.size());
+    unsigned int i;
+
+    unsigned int iter_leftsize = static_cast<unsigned int>(iter->leftBound().size());
+    unsigned int iter_rightsize = static_cast<unsigned int>(iter->rightBound().size());
+
+    if((iter_leftsize<2)||(iter_rightsize<2))
+    {
+        return;
+    }
+
+//    lanelet::Id startleftid = iter->leftBound()[0].id();
+//    lanelet::Id startrightid = iter->rightBound()[0].id();
+
+    for(i=0;i<nSize;i++)
+    {
+        unsigned int nSize2 = static_cast<unsigned int>(xvecotorsameID[i].size()) ;
+        unsigned int j;
+        for(j=1;j<nSize2;j++)
+        {
+
+            if((xvecotorsameID[i].at(j).iter == iter) &&(xvecotorsameID[i].at(j).npos == 1))
+            {
+                iter->leftBound().erase(iter->leftBound().begin());
+                iter->rightBound().erase(iter->rightBound().begin());
+                if(xvecotorsameID[i].at(0).npos == 1)
+                {
+                    iter->leftBound().insert(iter->leftBound().begin(),xvecotorsameID[i].at(0).iter->leftBound()[0]);
+                    iter->rightBound().insert(iter->rightBound().begin(),xvecotorsameID[i].at(0).iter->rightBound()[0]);
+                }
+                else
+                {
+                    iter->leftBound().insert(iter->leftBound().begin(),xvecotorsameID[i].at(0).iter->leftBound()[xvecotorsameID[i].at(0).iter->leftBound().size() -1]);
+                    iter->rightBound().insert(iter->rightBound().begin(),xvecotorsameID[i].at(0).iter->rightBound()[xvecotorsameID[i].at(0).iter->leftBound().size() -1]);
+                }
+                std::cout<<"merge3"<<std::endl;
+                return;
+            }
+            if((xvecotorsameID[i].at(j).iter == iter) &&(xvecotorsameID[i].at(j).npos == 2))
+            {
+                iter->leftBound().pop_back();
+                iter->rightBound().pop_back();
+                if(xvecotorsameID[i].at(0).npos == 1)
+                {
+                    iter->leftBound().push_back(xvecotorsameID[i].at(0).iter->leftBound()[0]);
+                    iter->rightBound().push_back(xvecotorsameID[i].at(0).iter->rightBound()[0]);
+                }
+                else
+                {
+                    iter->leftBound().push_back(xvecotorsameID[i].at(0).iter->leftBound()[xvecotorsameID[i].at(0).iter->leftBound().size() -1]);
+                    iter->rightBound().push_back(xvecotorsameID[i].at(0).iter->rightBound()[xvecotorsameID[i].at(0).iter->rightBound().size() -1]);
+                }
+                return;
+            }
+        }
+    }
+}
+
+void odtolanelet::InsertSameNodeID(std::vector<std::vector<iv::LaneletSameID>> & xvecotorsameID,lanelet::Id xLeftID1,lanelet::Id xRightID1,
+                                   lanelet::Id xLeftID2,lanelet::Id xRightID2,lanelet::LaneletLayer::iterator & iter1,lanelet::LaneletLayer::iterator & iter2,
+                                   int npos1,int npos2)
+{
+    unsigned int nSize = static_cast<unsigned int >(xvecotorsameID.size());
+    unsigned int i;
+
+    for(i=0;i<nSize;i++)
+    {
+        unsigned int j;
+        unsigned int nSize2 = static_cast<unsigned int>(xvecotorsameID.at(i).size()) ;
+        for(j=0;j<nSize2;j++)
+        {
+            if((xvecotorsameID[i].at(j).mLeftID == xLeftID1) && (xvecotorsameID[i].at(j).mRightID == xRightID1))
+            {
+                iv::LaneletSameID xsame;
+                xsame.mLeftID = xLeftID2;
+                xsame.mRightID = xRightID2;
+                xsame.iter = iter2;
+                xsame.npos = npos2;
+
+                xvecotorsameID[i].push_back(xsame);
+                return;
+            }
+        }
+        for(j=0;j<nSize2;j++)
+        {
+            if((xvecotorsameID[i].at(j).mLeftID == xLeftID2) && (xvecotorsameID[i].at(j).mRightID == xRightID2))
+            {
+                iv::LaneletSameID xsame;
+                xsame.mLeftID = xLeftID1;
+                xsame.mRightID = xRightID1;
+                xsame.iter = iter1;
+                xsame.npos = npos1;
+                xvecotorsameID[i].push_back(xsame);
+                return;
+            }
+        }
+    }
+
+    std::vector<iv::LaneletSameID> xvls;
+    iv::LaneletSameID xsame;
+    xsame.mLeftID = xLeftID1;
+    xsame.mRightID = xRightID1;
+    xsame.iter = iter1;
+    xsame.npos = npos1;
+    xvls.push_back(xsame);
+    xsame.mLeftID = xLeftID2;
+    xsame.mRightID = xRightID2;
+    xsame.iter = iter2;
+    xsame.npos = npos2;
+    xvls.push_back(xsame);
+    xvecotorsameID.push_back(xvls);
+}
+
+int odtolanelet::MergeLetPoint(lanelet::LaneletLayer::iterator & iter1,lanelet::LaneletLayer::iterator & iter2,std::vector<std::vector<iv::LaneletSameID>> & xvecotorsameID)
+{
+
     unsigned int iter1_leftsize = static_cast<unsigned int>(iter1->leftBound().size());
     unsigned int iter1_rightsize = static_cast<unsigned int>(iter1->rightBound().size());
 
@@ -426,11 +674,14 @@ int odtolanelet::MergeLetPoint(lanelet::LaneletLayer::iterator & iter1,lanelet::
         //Merge
         std::cout<<" merge1 "<<std::endl;
 
+        InsertSameNodeID(xvecotorsameID,iter1->leftBound()[iter1_leftsize-1].id(),iter1->rightBound()[iter1_rightsize-1].id(),
+                iter2->leftBound()[0].id(),iter2->rightBound()[0].id(),iter1,iter2,2,1);
+
+//        iter2->leftBound().erase(iter2->leftBound().begin());
+//        iter2->rightBound().erase(iter2->rightBound().begin());
+//        iter2->leftBound().insert(iter2->leftBound().begin(),iter1->leftBound()[iter1_leftsize-1]);
+//        iter2->rightBound().insert(iter2->rightBound().begin(),iter1->rightBound()[iter1_rightsize-1]);
 
-        iter2->leftBound().erase(iter2->leftBound().begin());
-        iter2->rightBound().erase(iter2->rightBound().begin());
-        iter2->leftBound().insert(iter2->leftBound().begin(),iter1->leftBound()[iter1_leftsize-1]);
-        iter2->rightBound().insert(iter2->rightBound().begin(),iter1->rightBound()[iter1_rightsize-1]);
         return 1;
     }
 
@@ -444,10 +695,14 @@ int odtolanelet::MergeLetPoint(lanelet::LaneletLayer::iterator & iter1,lanelet::
     {
         //Merge
         std::cout<<" merge2 "<<std::endl;
-        iter2->leftBound().pop_back();
-        iter2->rightBound().pop_back();
-        iter2->leftBound().push_back(iter1->leftBound()[0]);
-        iter2->rightBound().push_back(iter1->rightBound()[0]);
+
+        InsertSameNodeID(xvecotorsameID,iter1->leftBound()[0].id(),iter1->rightBound()[0].id(),
+                iter2->leftBound()[iter2_leftsize-1].id(),iter2->rightBound()[iter2_rightsize-1].id(),iter1,iter2,1,2);
+
+//        iter2->leftBound().pop_back();
+//        iter2->rightBound().pop_back();
+//        iter2->leftBound().push_back(iter1->leftBound()[0]);
+//        iter2->rightBound().push_back(iter1->rightBound()[0]);
         return 2;
     }
 

src/map/odtolanelet/odtolanelet.h

@@ -11,6 +11,17 @@
 
 #include "roadsample.h"
 
+namespace iv {
+struct LaneletSameID
+{
+    lanelet::Id mLeftID;
+    lanelet::Id mRightID;
+
+    lanelet::LaneletLayer::iterator iter;
+    int npos; //1 start  2 end
+};
+}
+
 class odtolanelet
 {
 public:
@@ -43,7 +54,15 @@ private:
 
     bool IsRelation(OpenDrive * pxodr,std::string strroad1,std::string strroad2);
 
-    int MergeLetPoint(lanelet::LaneletLayer::iterator & iter1,lanelet::LaneletLayer::iterator & iter2);
+
+    void ExecMerge(lanelet::LaneletMapPtr & laneletMap,std::vector<iv::LaneletSameID> & xsame);
+
+    void ExecMerge(std::vector<std::vector<iv::LaneletSameID>> & xvecotorsameID,lanelet::LaneletLayer::iterator  & iter1);
+
+    int MergeLetPoint(lanelet::LaneletLayer::iterator & iter1,lanelet::LaneletLayer::iterator & iter2,std::vector<std::vector<iv::LaneletSameID>> & xvecotorsameID);
+
+    void InsertSameNodeID(std::vector<std::vector<iv::LaneletSameID>> & xvecotorsameID,lanelet::Id xLeftID1,lanelet::Id xRightID1,
+                          lanelet::Id xLeftID2,lanelet::Id xRightID2,lanelet::LaneletLayer::iterator & iter1,lanelet::LaneletLayer::iterator & iter2,int npos1,int npos2);
 
 };
 

src/map/odtolanelet/odtolanelet.pro

@@ -9,7 +9,7 @@ CONFIG -= app_bundle
 # deprecated API in order to know how to port your code away from it.
 DEFINES += QT_DEPRECATED_WARNINGS
 
-DEFINES += TESTROUTING
+#DEFINES += TESTROUTING
 
 # You can also make your code fail to compile if you use deprecated APIs.
 # In order to do so, uncomment the following line.