change odtolanelet.

src/common/common/xodr/OpenDrive/Road.h

@@ -487,6 +487,8 @@ public:
 
     short int GetLaneCoords(double s_check, double t_check,double &retX, double &retY, double &retZ,double &retHDG);
 
+
+
 	
 	//-------------------------------------------------
 

src/map/odtolanelet/main.cpp

@@ -1,9 +1,14 @@
 #include <QCoreApplication>
 
+#include "odtolanelet.h"
+
 int main(int argc, char *argv[])
 {
     QCoreApplication a(argc, argv);
 
+    odtolanelet * potl = new odtolanelet("/home/yuchuli/testlane.xodr");
+
+    potl->ConvertToLanelet("hello");
 
     return a.exec();
 }

src/map/odtolanelet/odtolanelet.cpp

@@ -2,42 +2,14 @@
 
 #include <vector>
 
+#include "roadsample.h"
+
 namespace iv
 {
 
-struct LanePoint
-{
-public:
-    int nLane; // 1 left 1   0 center lane   -1 right lane1
-    double mx;
-    double my;
-    double mz;
-    double mhdg;
-    std::string mstrroadmark;
-    double mfmarkwidth;
-    std::string mstrLaneType;  //if nlane = 0 center lane, lanetype is not valid.
-};
-
-struct RoadPoint
-{
-public:
-    double ms;
-    std::vector<LanePoint> mvectorLanePoint;  //From Left to Right
-    double mfRefX;
-    double mfRefY;
-    double mfRefZ;
-    double mfLaneOffValue;
-};
-
-class RoadDigit
-{
-public:
-    std::string mstrroadid;
-    std::string mstrroadname;
-    double mfRoadLen;
-    std::vector<RoadPoint> mvectorRoadPoint;
 
-};
+
+
 
 }
 
@@ -53,7 +25,7 @@ int odtolanelet::ConvertToLanelet(std::string strlanelet2file)
 
     OpenDrive * pxodr = new OpenDrive();  //because add to xodr,so don't delete
     OpenDriveXmlParser x(pxodr);
-    if(!x.ReadFile(strlanelet2file))
+    if(!x.ReadFile(mstrxodrfile))
     {
         std::cout<<"Can't  load xodr file."<<std::endl;
         return -1;
@@ -83,6 +55,8 @@ int odtolanelet::Road2Lanelet(Road * pRoad,lanelet::Lanelet & LaneLet)
         return -1;
     }
 
+    iv::RoadSample * pSample = new iv::RoadSample(pRoad);
+
     std::vector<lanelet::LineString3d> xvectorlinestring;
     std::vector<std::string> xvectortype;
     std::vector<std::string> xvectorsubtype;
@@ -96,6 +70,7 @@ int odtolanelet::Road2Lanelet(Road * pRoad,lanelet::Lanelet & LaneLet)
     double fS = 0;
     const double fstep = 0.1;
 
+
     for(i=0;i<nLSCount;i++)
     {
         LaneSection * pLS = pRoad->GetLaneSection(i);

src/map/odtolanelet/roadsample.cpp

@@ -10,6 +10,10 @@ namespace iv {
 RoadSample::RoadSample(Road * pRoad)
 {
     SampleRoad(pRoad);
+
+    std::vector<RoadPoint> xvectorRoadPoint = mvectorRoadPoint;
+
+    int n = 1;
 }
 
 int RoadSample::SampleRoad(Road * pRoad)
@@ -19,10 +23,11 @@ int RoadSample::SampleRoad(Road * pRoad)
     mstrroadname = pRoad->GetRoadName();
 
     double fS = 0;
-    const double fStep = 0;
+    const double fStep = 0.1;
     LaneSection * pLS;
     unsigned int nLSCount = pRoad->GetLaneSectionCount();
     unsigned int i;
+    int nSec = 0;
 
     for(i=0;i<nLSCount;i++)
     {
@@ -35,14 +40,41 @@ int RoadSample::SampleRoad(Road * pRoad)
 
         double fLastRefX,fLastRefY,fLastRefZ,fLastRefHdg,fLastOffsetValue,fLastS;
         bool bHaveLast = false;
+        std::vector<LanePoint> xvectorLastLanePoint;
 
-        while(fS<pLS->GetS2())
+        double send_Section = pLS->GetS();
+        if(i == (nLSCount -1))
+        {
+            send_Section = pRoad->GetRoadLength();
+        }
+        else
+        {
+            LaneSection * pLSNext = pRoad->GetLaneSection(i+1);
+            if(pLSNext != NULL)
+            {
+                send_Section = pLSNext->GetS();
+            }
+        }
+        nSec++;
+        while(fS<send_Section)
         {
             double fRefX,fRefY,fRefZ,fRefHdg,fOffsetValue;
             pRoad->GetGeometryCoords(fS,fRefX,fRefY,fRefHdg);
             fRefZ = pRoad->GetElevationValue(fS);
             fOffsetValue = pRoad->GetLaneOffsetValue(fS);
 
+            if(fabs(send_Section - fS)<0.1)
+            {
+                if(i == (nLSCount - 1))
+                {
+                    fS = send_Section;
+                }
+                else
+                {
+                    break; //If Not Last Section, use Next Section Start Value.
+                }
+            }
+
             RoadPoint xRP;
             xRP.ms = fS;
             xRP.mfRefX = fRefX;
@@ -50,10 +82,12 @@ int RoadSample::SampleRoad(Road * pRoad)
             xRP.mfRefZ = fRefZ;
             xRP.mfHdg = fRefHdg;
             xRP.mfLaneOffValue = fOffsetValue;
+            xRP.nSecNum = nSec;
 
             std::vector<LanePoint> xvectorLanePoint;
             SampleLanePoint(pRoad,pLS,fS,xvectorLanePoint);
 
+
             bool bInserPoint = false;
 
             if(bHaveLast == false)
@@ -68,21 +102,36 @@ int RoadSample::SampleRoad(Road * pRoad)
                 }
                 else
                 {
-                    if((fabs((fRefHdg - fLastRefHdg)/(fS - fLastS))>0.01) ||((fS - fLastS)>10))
+                    if((fabs((fRefHdg - fLastRefHdg)/(fS - fLastS))>0.01) ||((fS - fLastS)>=10.0))
                     {
                         bInserPoint = true;
                     }
                 }
             }
 
+            if((bInserPoint == false)&&(bHaveLast))
+            {
+                if(IsMarkChange(xvectorLanePoint,xvectorLastLanePoint))
+                {
+                    bInserPoint = true;
+                    nSec++;
+                    xRP.nSecNum = nSec;
+                }
+            }
+
+
             if(bInserPoint)
             {
+                xRP.mvectorLanePoint = xvectorLanePoint;
+                mvectorRoadPoint.push_back(xRP);
+
                 fLastS = fS;
                 fLastRefX = fRefX;
                 fLastRefY = fRefY;
                 fLastRefZ = fRefZ;
                 fLastRefHdg = fRefHdg;
                 fLastOffsetValue = fOffsetValue;
+                xvectorLastLanePoint = xvectorLanePoint;
                 bHaveLast = true;
             }
 
@@ -185,6 +234,39 @@ int RoadSample::SampleLanePoint(Road * pRoad,LaneSection * pLS, double fS,std::v
     return 0;
 }
 
+bool RoadSample::IsMarkChange(std::vector<LanePoint> & xvectorLastLanePoint,std::vector<LanePoint> & xvectorLanePoint)
+{
+    if(xvectorLanePoint.size() != xvectorLastLanePoint.size())
+    {
+        return true;
+    }
+
+    unsigned int i;
+    unsigned int nSize = static_cast<unsigned int >(xvectorLanePoint.size());
+
+    for(i=0;i<nSize;i++)
+    {
+        if(xvectorLanePoint[i].mstrroadmarkcolor != xvectorLastLanePoint[i].mstrroadmarkcolor)
+        {
+            return true;
+        }
+        if(xvectorLanePoint[i].mstrroadmarktype != xvectorLastLanePoint[i].mstrroadmarktype)
+        {
+            return true;
+        }
+    }
+
+    return false;
+
+
 }
 
+std::vector<RoadPoint> * RoadSample::GetRoadPointVector()
+{
+    return  &mvectorRoadPoint;
+}
+
+}
+
+
 

src/map/odtolanelet/roadsample.h

@@ -41,6 +41,9 @@ public:
     double mfRefZ;
     double mfHdg;
     double mfLaneOffValue;
+
+    int nSecNum;  //If lane count change or lane roadmarkchange is new section.
+
 };
 
 
@@ -50,6 +53,8 @@ class RoadSample
 public:
     RoadSample(Road * pRoad);
 
+    std::vector<RoadPoint> * GetRoadPointVector();
+
 private:
     std::string mstrroadid;
     std::string mstrroadname;
@@ -66,6 +71,8 @@ private:
 
 
     int SampleLanePoint(Road * pRoad,LaneSection * pLS, double fS,std::vector<LanePoint> & xvectorLanePoint);
+
+    bool IsMarkChange(std::vector<LanePoint> & xvectorLastLanePoint,std::vector<LanePoint> & xvectorLanePoint);
 };
 
 }