change odtolanelet. juction relation have some problem.

src/map/odtolanelet/odtolanelet.cpp

@@ -52,6 +52,10 @@ int odtolanelet::ConvertToLanelet(std::string strlanelet2file)
 
     }
 
+    MergeSame(pxodr,laneletMap);
+
+
+
 #ifdef TESTROUTING
     const auto traffic_rules = lanelet::traffic_rules::TrafficRulesFactory::create(
       lanelet::Locations::Germany, lanelet::Participants::Vehicle);
@@ -65,8 +69,8 @@ int odtolanelet::ConvertToLanelet(std::string strlanelet2file)
 
     lanelet::Lanelet goal_lanelet;
 
-    GetLetByID(laneletMap,start_lanelet,1237);
-    GetLetByID(laneletMap,goal_lanelet,401);
+    GetLetByID(laneletMap,start_lanelet,1527);
+    GetLetByID(laneletMap,goal_lanelet,314);
 
 
     // get all possible lanes that can be used to reach goal (including all possible lane change)
@@ -115,6 +119,11 @@ int odtolanelet::Road2Lanelet(Road * pRoad,lanelet::LaneletMapPtr & laneletMap)
 
     unsigned int ncenterlanepos = 0;
 
+    double sstart,send;
+
+    sstart = 0;
+    send = 0;
+
     for(i=0;i<nRPSize;i++)
     {
         bool bNewLet = false;
@@ -128,7 +137,7 @@ int odtolanelet::Road2Lanelet(Road * pRoad,lanelet::LaneletMapPtr & laneletMap)
             {
                 //Create LaneLet
 
-                linetolet(laneletMap,xvectorlinestring,ncenterlanepos);
+                linetolet(laneletMap,xvectorlinestring,ncenterlanepos,pRoad->GetRoadId(),sstart,send);
                 bNewLet = true;
             }
         }
@@ -139,6 +148,8 @@ int odtolanelet::Road2Lanelet(Road * pRoad,lanelet::LaneletMapPtr & laneletMap)
 
             unsigned int nlanesize = static_cast<unsigned int>(pvectorRoadPoint->at(i).mvectorLanePoint.size());
 
+            sstart = pvectorRoadPoint->at(i).ms;
+
             unsigned int j;
             for(j=0;j<nlanesize;j++)
             {
@@ -172,6 +183,7 @@ int odtolanelet::Road2Lanelet(Road * pRoad,lanelet::LaneletMapPtr & laneletMap)
         }
         else
         {
+            send = pvectorRoadPoint->at(i).ms;
             LanePointToPoint3D(pvectorRoadPoint->at(i).mvectorLanePoint,xvectorPoint);
             unsigned int nlanesize = static_cast<unsigned int>(pvectorRoadPoint->at(i).mvectorLanePoint.size());
 
@@ -185,7 +197,7 @@ int odtolanelet::Road2Lanelet(Road * pRoad,lanelet::LaneletMapPtr & laneletMap)
 
     if(xvectorlinestring.size()>0)
     {
-        linetolet(laneletMap,xvectorlinestring,ncenterlanepos);
+        linetolet(laneletMap,xvectorlinestring,ncenterlanepos,pRoad->GetRoadId(),sstart,send);
     }
 
     return  0;
@@ -267,6 +279,7 @@ void odtolanelet::LanePointToPoint3D(std::vector<iv::LanePoint> & xvectorLanePoi
     }
 }
 
+
 void odtolanelet::LanePointToPoint3DWithLast(std::vector<iv::LanePoint> & xvectorLanePoint, std::vector<lanelet::Point3d> & xvectorPoint3D,
                                 std::vector<lanelet::Point3d> & xvectorPoint3DLast)
 {
@@ -297,7 +310,8 @@ void odtolanelet::LanePointToPoint3DWithLast(std::vector<iv::LanePoint> & xvecto
     }
 }
 
-void odtolanelet::linetolet(lanelet::LaneletMapPtr & laneletMap,std::vector<lanelet::LineString3d> & xvectorlinestring, unsigned int ncenterlane)
+void odtolanelet::linetolet(lanelet::LaneletMapPtr & laneletMap,std::vector<lanelet::LineString3d> & xvectorlinestring, unsigned int ncenterlane,
+                            std::string strroadid,double sstart,double send)
 {
     unsigned int i;
     unsigned int nsize = static_cast<unsigned int>(xvectorlinestring.size()) ;
@@ -308,12 +322,20 @@ void odtolanelet::linetolet(lanelet::LaneletMapPtr & laneletMap,std::vector<lane
         InvertLine(xleft,xvectorlinestring[i+1]);
         InvertLine(xright,xvectorlinestring[i]);
         lanelet::Lanelet xlet(mid,xleft,xright);
+        xlet.setAttribute("odr:roadid",lanelet::Attribute(strroadid));
+        xlet.setAttribute("odr:s_start",lanelet::Attribute(send));
+        xlet.setAttribute("odr:s_end",lanelet::Attribute(sstart));
+        xlet.setAttribute("odr:lane",lanelet::Attribute(static_cast<int>(ncenterlane-i) ));
         mid++;
         laneletMap->add(xlet);
     }
     for(i=ncenterlane;i<(nsize-1);i++)
     {
         lanelet::Lanelet xlet(mid,xvectorlinestring[i],xvectorlinestring[i+1]);
+        xlet.setAttribute("odr:roadid",lanelet::Attribute(strroadid));
+        xlet.setAttribute("odr:s_start",lanelet::Attribute(sstart));
+        xlet.setAttribute("odr:s_end",lanelet::Attribute(send));
+        xlet.setAttribute("odr:lane",lanelet::Attribute(static_cast<int>(i-ncenterlane+1) *(-1)));
         mid++;
         laneletMap->add(xlet);
     }
@@ -340,3 +362,168 @@ int odtolanelet::GetLetByID(lanelet::LaneletMapPtr & laneletMap,lanelet::Lanelet
     let = laneletMap->laneletLayer.get(id);
     return 0;
 }
+
+void odtolanelet::MergeSame(OpenDrive * pxodr,lanelet::LaneletMapPtr & laneletMap)
+{
+    lanelet::LaneletLayer::iterator  iter;
+    for(iter = laneletMap->laneletLayer.begin();iter != laneletMap->laneletLayer.end();++iter)
+    {
+        if(iter->hasAttribute("odr:roadid"))
+        {
+            std::string letodrid1 = iter->attribute("odr:roadid").value();
+            lanelet::LaneletLayer::iterator  iterother;
+            for(iterother = iter;iterother != laneletMap->laneletLayer.end();++iterother)
+            {
+                if(iterother == iter)continue;
+                if(iterother->hasAttribute("odr:roadid"))
+                {
+                    std::string letroadid2 = iterother->attribute("odr:roadid").value();
+  //                  std::cout<<" roadid: "<<letodrid1<<" roadid2: "<<letroadid2<<std::endl;
+
+                    if(((letodrid1 == "10010")&&(letroadid2 == "10030")) || ((letodrid1 == "10030")&&(letroadid2 == "10010")))
+                    {
+                         std::cout<<" roadid: "<<letodrid1<<" roadid2: "<<letroadid2<<std::endl;
+                        int a = 1;
+                        a++;
+                    }
+
+                    if(IsRelation(pxodr,letodrid1,letroadid2))
+                    {
+                        int bRtn = MergeLetPoint(iter,iterother);
+                        if(bRtn == 1)
+                        {
+
+                        }
+                    }
+                }
+            }
+        }
+    }
+}
+
+int odtolanelet::MergeLetPoint(lanelet::LaneletLayer::iterator & iter1,lanelet::LaneletLayer::iterator & iter2)
+{
+    unsigned int iter1_leftsize = static_cast<unsigned int>(iter1->leftBound().size());
+    unsigned int iter1_rightsize = static_cast<unsigned int>(iter1->rightBound().size());
+
+    unsigned int iter2_leftsize = static_cast<unsigned int>(iter2->leftBound().size());
+    unsigned int iter2_rightsize = static_cast<unsigned int>(iter2->rightBound().size());
+
+    if((iter1_leftsize == 0) || (iter1_rightsize == 0) || (iter2_leftsize == 0) || (iter2_rightsize == 0))
+    {
+        std::cout<<" odtolanelet::MergeLetPoint "<<"size is zero."<<std::endl;
+        return -1;
+    }
+
+    double fdis1,fdis2;
+    fdis1 = sqrt(pow(iter1->leftBound()[iter1_leftsize-1].x() - iter2->leftBound()[0].x(),2)
+            +pow(iter1->leftBound()[iter1_leftsize-1].y() - iter2->leftBound()[0].y(),2));
+    fdis2 = sqrt(pow(iter1->rightBound()[iter1_rightsize-1].x() - iter2->rightBound()[0].x(),2)
+            +pow(iter1->rightBound()[iter1_rightsize-1].y() - iter2->rightBound()[0].y(),2));
+
+    if((fdis1<0.1) &&(fdis2<1.0))
+    {
+        //Merge
+        std::cout<<" merge1 "<<std::endl;
+
+
+        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;
+    }
+
+    fdis1 = sqrt(pow(iter2->leftBound()[iter2_leftsize-1].x() - iter1->leftBound()[0].x(),2)
+            +pow(iter2->leftBound()[iter2_leftsize-1].y() - iter1->leftBound()[0].y(),2));
+    fdis2 = sqrt(pow(iter2->rightBound()[iter2_rightsize-1].x() - iter1->rightBound()[0].x(),2)
+            +pow(iter2->rightBound()[iter2_rightsize-1].y() - iter1->rightBound()[0].y(),2));
+
+
+    if((fdis1<0.1) &&(fdis2<1.0))
+    {
+        //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]);
+        return 2;
+    }
+
+    return 0;
+
+
+}
+
+bool odtolanelet::IsRelation(OpenDrive * pxodr,std::string strroad1,std::string strroad2)
+{
+    unsigned int nRoadCount = pxodr->GetRoadCount();
+    unsigned int i;
+
+    for(i=0;i<nRoadCount;i++)
+    {
+        Road * pRoad = pxodr->GetRoad(i);
+        if(pRoad->GetRoadId() == strroad1)
+        {
+            RoadLink * pLink = pRoad->GetPredecessor();
+            if(pLink != NULL)
+            {
+                if(pLink->GetElementId() == strroad2)
+                {
+                    return true;
+                }
+            }
+            pLink = pRoad->GetSuccessor();
+            if(pLink != NULL)
+            {
+                if(pLink->GetElementId() == strroad2)
+                {
+                    return true;
+                }
+            }
+        }
+        if(pRoad->GetRoadId() == strroad2)
+        {
+            RoadLink * pLink = pRoad->GetPredecessor();
+            if(pLink != NULL)
+            {
+                if(pLink->GetElementId() == strroad1)
+                {
+                    return true;
+                }
+            }
+            pLink = pRoad->GetSuccessor();
+            if(pLink != NULL)
+            {
+                if(pLink->GetElementId() == strroad1)
+                {
+                    return true;
+                }
+            }
+        }
+    }
+
+    unsigned int nJuctionCount = pxodr->GetJunctionCount();
+
+    for(i=0;i<nJuctionCount;i++)
+    {
+        Junction * pJunc = pxodr->GetJunction(i);
+        unsigned int nJCCount = pJunc->GetJunctionConnectionCount();
+        unsigned int j;
+        for(j=0;j<nJCCount;j++)
+        {
+            JunctionConnection * pJCon = pJunc->GetJunctionConnection(j);
+            if((pJCon->GetConnectingRoad() == strroad1)&&(pJCon->GetIncomingRoad() == strroad2))
+            {
+                return true;
+            }
+            if((pJCon->GetConnectingRoad() == strroad2)&&(pJCon->GetIncomingRoad() == strroad1))
+            {
+                return true;
+            }
+        }
+    }
+
+    return false;
+}

src/map/odtolanelet/odtolanelet.h

@@ -33,12 +33,18 @@ private:
     void LanePointToPoint3DWithLast(std::vector<iv::LanePoint> & xvectorLanePoint, std::vector<lanelet::Point3d> & xvectorPoint3D,
                                     std::vector<lanelet::Point3d> & xvectorPoint3DLast);
 
-    void linetolet(lanelet::LaneletMapPtr & laneletMap,std::vector<lanelet::LineString3d> & xvectorlinestring, unsigned int ncenterlane);
+    void linetolet(lanelet::LaneletMapPtr & laneletMap,std::vector<lanelet::LineString3d> & xvectorlinestring, unsigned int ncenterlane,std::string strroadid,double sstart,double send);
 
     int InvertLine(lanelet::LineString3d & xinvert,lanelet::LineString3d & xraw);
 
     int GetLetByID(lanelet::LaneletMapPtr & laneletMap,lanelet::Lanelet &let,lanelet::Id id);
 
+    void MergeSame(OpenDrive * pxodr,lanelet::LaneletMapPtr & laneletMap);
+
+    bool IsRelation(OpenDrive * pxodr,std::string strroad1,std::string strroad2);
+
+    int MergeLetPoint(lanelet::LaneletLayer::iterator & iter1,lanelet::LaneletLayer::iterator & iter2);
+
 };
 
 #endif // ODTOLANELET_H

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.

src/map/odtolanelet/roadsample.cpp

@@ -473,9 +473,6 @@ double RoadSample::GetMaxWidthRatio(LaneSection * pLS, double fS)
 
 
 
-
-
-
 }
 
 

src/map/odtolanelet/roadsample.h

@@ -80,6 +80,8 @@ private:
     bool IsCrossFeaturePoint(std::vector<double> & xvectorFeature,double & fS,double fSLast);
 
     inline double GetMaxWidthRatio(LaneSection * pLS, double fS);
+
+
 };
 
 }