changing odtolanelet. not complete.

src/common/common/xodr/OpenDrive/Lane.cpp

@@ -467,7 +467,7 @@ bool LaneSection::FillLaneSectionSample(double s_check, LaneSectionSample& laneS
 //Examp i= 1 return lane id = 1 lane
 Lane * LaneSection::GetLeftLaneAt(unsigned int index)
 {
-    int nlaneid = index;
+    int nlaneid = static_cast<int>(index) ;
     Lane * pRtn = NULL;
 
     unsigned int i;
@@ -483,10 +483,27 @@ Lane * LaneSection::GetLeftLaneAt(unsigned int index)
 
 }
 
+
+Lane * LaneSection::GetCenterLane()
+{
+    Lane * pRtn = NULL;
+
+    unsigned int i;
+    for(i=0;i<mLaneVector.size();i++)
+    {
+        if(mLaneVector.at(i).GetId() == 0)
+        {
+            pRtn = &mLaneVector.at(i);
+            break;
+        }
+    }
+    return pRtn;
+}
+
 //Examp i= 1 return lane id = -1 lane
 Lane * LaneSection::GetRightLaneAt(unsigned int index)
 {
-    int nlaneid = index;
+    int nlaneid = static_cast<int>(index) ;
     nlaneid = nlaneid *(-1);
     Lane * pRtn = NULL;
 

src/common/common/xodr/OpenDrive/Lane.h

@@ -1,7 +1,7 @@
 #ifndef LANE_H
 #define LANE_H
 
-#include "Road.h"
+//#include "Road.h"
 #include "OtherStructures.h"
 #include <vector>
 #include <string>
@@ -200,6 +200,7 @@ public:
 
     Lane * GetLeftLaneAt(unsigned int index);
     Lane * GetRightLaneAt(unsigned int index);
+    Lane * GetCenterLane();
 
 
 

src/common/common/xodr/OpenDrive/Road.cpp

@@ -1908,7 +1908,7 @@ int Road::GetLeftDrivingLaneCount(double s_check)
         pLS = GetLaneSection(i);
     }
 
-    if(pLS == NULL)return 0;
+    if(pLS == NULL)return -1;
 
     int nrtn;
 
@@ -1950,11 +1950,11 @@ int Road::GetRightDrivingLaneCount(double s_check)
         pLS = GetLaneSection(i);
     }
 
-    if(pLS == NULL)return 0;
+    if(pLS == NULL)return -1;
 
     int nrtn;
 
-    unsigned int nlanecount = pLS->GetLeftLaneCount();
+    unsigned int nlanecount = pLS->GetRightLaneCount();
     for(i=0;i<nlanecount;i++)
     {
         Lane * pLane = pLS->GetRightLaneAt(i+1);
@@ -1977,6 +1977,153 @@ int Road::GetRightDrivingLaneCount(double s_check)
     return nrtn;
 }
 
+//-------------------------------------------------
+
+std::vector<double> Road::GetLaneWidthVector(double s_check,int nlr) //Lane Width, From Refline to side. nlr 1 left 2 right
+{
+    std::vector<double> xrtn;
+    xrtn.clear();
+
+    LaneSection * pLS = NULL;
+
+    unsigned int nsecCount = GetLaneSectionCount();
+    unsigned int i;
+    if(nsecCount == 0)return  xrtn;
+    for(i=0;i<nsecCount;i++)
+    {
+        if(GetLaneSection(i)->GetS()>s_check)break;
+        pLS = GetLaneSection(i);
+    }
+
+    if(pLS == NULL)return xrtn;
+
+    if(nlr == 1)
+    {
+        unsigned int nlanecount = pLS->GetLeftLaneCount();
+        for(i=0;i<nlanecount;i++)
+        {
+            Lane * pLane = pLS->GetLeftLaneAt(i+1);
+
+            if(pLane == NULL)
+            {
+                std::cout<<" Road::GetLaneWidthVector Fail."<<" s_check: "<<s_check<<" nlr: "<<nlr<<std::endl;
+                return xrtn;
+            }
+           double fwidth = pLane->GetWidthValue(s_check - pLS->GetS());
+
+           xrtn.push_back(fwidth);
+
+
+        }
+    }
+    else
+    {
+        unsigned int nlanecount = pLS->GetRightLaneCount();
+        for(i=0;i<nlanecount;i++)
+        {
+            Lane * pLane = pLS->GetRightLaneAt(i+1);
+            if(pLane == NULL)
+            {
+                std::cout<<" Road::GetLaneWidthVector Fail."<<" s_check: "<<s_check<<" nlr: "<<nlr<<std::endl;
+                return xrtn;
+            }
+
+           double fwidth = pLane->GetWidthValue(s_check - pLS->GetS());
+
+           xrtn.push_back(fwidth);
+
+
+        }
+    }
+
+
+
+    return  xrtn;
+}
+
+//-------------------------------------------------
+
+int Road::GetLeftLaneCount(double s_check)
+{
+    LaneSection * pLS = NULL;
+
+    unsigned int nsecCount = GetLaneSectionCount();
+    unsigned int i;
+    if(nsecCount == 0)return  0;
+    for(i=0;i<nsecCount;i++)
+    {
+        if(GetLaneSection(i)->GetS()>s_check)break;
+        pLS = GetLaneSection(i);
+    }
+
+    if(pLS == NULL)return 0;
+
+    return static_cast<int>(pLS->GetLeftLaneCount());
+}
+
+//-------------------------------------------------
+
+
+
+int Road::GetRightLaneCount(double s_check)
+{
+    LaneSection * pLS = NULL;
+
+    unsigned int nsecCount = GetLaneSectionCount();
+    unsigned int i;
+    if(nsecCount == 0)return  0;
+    for(i=0;i<nsecCount;i++)
+    {
+        if(GetLaneSection(i)->GetS()>s_check)break;
+        pLS = GetLaneSection(i);
+    }
+
+    if(pLS == NULL)return -1;
+
+    return static_cast<int>(pLS->GetRightLaneCount());
+}
+
+
+//-------------------------------------------------
+
+LaneRoadMark  Road::GetLaneRoadMarkValue(double s_check,int nlane)
+{
+    LaneRoadMark xRoadMark;
+    LaneSection * pLS = NULL;
+
+    unsigned int nsecCount = GetLaneSectionCount();
+    unsigned int i;
+    if(nsecCount == 0)return  xRoadMark;
+    for(i=0;i<nsecCount;i++)
+    {
+        if(GetLaneSection(i)->GetS()>s_check)break;
+        pLS = GetLaneSection(i);
+    }
+
+    if(pLS == NULL)return xRoadMark;
+
+    Lane * pLane = NULL;
+
+    if(nlane == 0)pLane = pLS->GetCenterLane();
+    else
+    {
+        if(nlane<0)pLane = pLS->GetRightLaneAt(static_cast<unsigned int>(abs(nlane)));
+        else
+            pLane = pLS->GetLeftLaneAt(static_cast<unsigned int>(nlane));
+    }
+
+    if(pLane == NULL)
+    {
+        std::cout<<"Road::GetLaneRoadMark "<<"Can't Find lane s:"<<s_check<<" lane: "<<nlane<<std::endl;
+        return xRoadMark;
+    }
+
+    return pLane->GetRoadMarkValue(s_check - pLS->GetS());
+
+
+}
+
+
 
 //-------------------------------------------------
 

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

@@ -448,9 +448,21 @@ public:
 
     std::vector<double> GetDrivingLaneWidthVector(double s_check,int nlr); //Lane Width, From Refline to side. nlr 1 left 2 right
 
+
+    /**
+     * @brief GetLeftDrivingLaneCount
+     * @param s_check
+     * @return if No LaneSection return -1
+     */
     int GetLeftDrivingLaneCount(double s_check);
     int GetRightDrivingLaneCount(double s_check);
 
+    std::vector<double> GetLaneWidthVector(double s_check,int nlr); //Lane Width, From Refline to side. nlr 1 left 2 right
+    int GetLeftLaneCount(double s_check);
+    int GetRightLaneCount(double s_check);
+
+    LaneRoadMark GetLaneRoadMarkValue(double s_check,int nlane);  //nlane=0 center lane    nlane<0 left lane     nlane>0 right lane
+
     void SetRoadPriority(int nPriority);
     int GetRoadPriority(int & nPriority);
     void ResetPriority();

src/map/odtolanelet/main.cpp

@@ -4,5 +4,6 @@ int main(int argc, char *argv[])
 {
     QCoreApplication a(argc, argv);
 
+
     return a.exec();
 }

src/map/odtolanelet/odtolanelet.cpp

@@ -1,5 +1,46 @@
 #include "odtolanelet.h"
 
+#include <vector>
+
+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;
+
+};
+
+}
+
 odtolanelet::odtolanelet(std::string strxodrfile)
 {
     mstrxodrfile = strxodrfile;
@@ -111,6 +152,8 @@ int odtolanelet::Road2Lanelet(Road * pRoad,lanelet::Lanelet & LaneLet)
         {
 
             fS = fS + fstep;
+            double x,y,hdg;
+            pRoad->GetGeometryCoords(fS,x,y,hdg);
         }
 
 

src/map/odtolanelet/odtolanelet.pro

@@ -15,7 +15,8 @@ DEFINES += QT_DEPRECATED_WARNINGS
 #DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
 
 SOURCES += main.cpp \
-    odtolanelet.cpp
+    odtolanelet.cpp \
+    roadsample.cpp
 
 
 !include(../../common/common/xodr/OpenDrive/OpenDrive.pri ) {
@@ -31,7 +32,8 @@ SOURCES += main.cpp \
 }
 
 HEADERS += \
-    odtolanelet.h
+    odtolanelet.h \
+    roadsample.h
 
 INCLUDEPATH += $$PWD/../../common/common/xodr
 

src/map/odtolanelet/roadsample.cpp

@@ -0,0 +1,174 @@
+#include "roadsample.h"
+
+#include <math.h>
+
+#include <iostream>
+
+namespace iv {
+
+
+RoadSample::RoadSample(Road * pRoad)
+{
+    SampleRoad(pRoad);
+}
+
+int RoadSample::SampleRoad(Road * pRoad)
+{
+    mfRoadLen = pRoad->GetRoadLength();
+    mstrroadid = pRoad->GetRoadId();
+    mstrroadname = pRoad->GetRoadName();
+
+    double fS = 0;
+    const double fStep = 0;
+    LaneSection * pLS;
+    unsigned int nLSCount = pRoad->GetLaneSectionCount();
+    unsigned int i;
+
+    for(i=0;i<nLSCount;i++)
+    {
+        pLS = pRoad->GetLaneSection(i);
+
+        if(pLS == NULL)
+        {
+            break;
+        }
+
+        double fLastRefX,fLastRefY,fLastRefZ,fLastRefHdg,fLastOffsetValue,fLastS;
+        bool bHaveLast = false;
+
+        while(fS<pLS->GetS2())
+        {
+            double fRefX,fRefY,fRefZ,fRefHdg,fOffsetValue;
+            pRoad->GetGeometryCoords(fS,fRefX,fRefY,fRefHdg);
+            fRefZ = pRoad->GetElevationValue(fS);
+            fOffsetValue = pRoad->GetLaneOffsetValue(fS);
+
+            RoadPoint xRP;
+            xRP.ms = fS;
+            xRP.mfRefX = fRefX;
+            xRP.mfRefY = fRefY;
+            xRP.mfRefZ = fRefZ;
+            xRP.mfHdg = fRefHdg;
+            xRP.mfLaneOffValue = fOffsetValue;
+
+            bool bInserPoint = false;
+
+            if(bHaveLast == false)
+            {
+                bInserPoint = true;
+            }
+            else
+            {
+                if(fabs((fOffsetValue - fLastOffsetValue)/(fS - fLastS))>0.1)
+                {
+                    bInserPoint = true;
+                }
+                else
+                {
+                    if((fabs((fRefHdg - fLastRefHdg)/(fS - fLastS))>0.01) ||((fS - fLastS)>10))
+                    {
+                        bInserPoint = true;
+                    }
+                }
+            }
+
+            if(bInserPoint)
+            {
+                fLastS = fS;
+                fLastRefX = fRefX;
+                fLastRefY = fRefY;
+                fLastRefZ = fRefZ;
+                fLastRefHdg = fRefHdg;
+                fLastOffsetValue = fOffsetValue;
+                bHaveLast = true;
+            }
+
+            fS = fS + fStep;
+        }
+    }
+
+    return 0;
+}
+
+int RoadSample::SampleLanePoint(Road * pRoad,LaneSection * pLS, double fS,double fRefX,double fRefY,double fRefZ,double fRefHdg,double fLaneOffValue,std::vector<LanePoint> & xvectorLanePoint)
+{
+    Lane * pLane;
+
+    unsigned int nLaneCount = pLS->GetLaneCount();
+    unsigned int nLeftLaneCount = pLS->GetLeftLaneCount();
+    unsigned int nRightLaneCount = pLS->GetRightLaneCount();
+
+
+    std::vector<double> xLeftWidth = pRoad->GetLaneWidthVector(fS,1);
+    std::vector<double> xRightWidth = pRoad->GetLaneWidthVector(fS,2);
+
+    if(static_cast<unsigned int>(xLeftWidth.size()) != nLeftLaneCount )
+    {
+        std::cout<<"RoadSample::SampleLanePoint xLeftWidth not equal nLeftLaneCount. "<<" left width size: "<<xLeftWidth.size()
+                <<" nLeftLaneCount: "<<nLeftLaneCount<<std::endl;
+        return -1;
+    }
+
+    if(static_cast<unsigned int>(xRightWidth.size()) != nRightLaneCount )
+    {
+        std::cout<<"RoadSample::SampleLanePoint xRightWidth not equal nRIghtLaneCount. "<<" right width size: "<<xRightWidth.size()
+                <<" nRightLaneCount: "<<nRightLaneCount<<std::endl;
+        return -2;
+    }
+
+    std::vector<double> xvectorLeftLaneOff;
+    std::vector<double> xvecorRightLaneOff;
+
+    unsigned int i;
+    for(i=0;i<nLeftLaneCount;i++)
+    {
+        unsigned int j;
+        double fValue = fLaneOffValue + xLeftWidth[nLeftLaneCount-i-1];
+        for(j=(nLeftLaneCount-i-1);j>0;j--)
+        {
+            fValue = fValue + xLeftWidth[j-1];
+        }
+        xvectorLeftLaneOff.push_back(fValue);
+    }
+
+    for(i=0;i<nRightLaneCount;i++)
+    {
+        unsigned int j;
+        double fValue = fLaneOffValue;
+        for(j=0;j<=i;j++)
+        {
+            fValue = fValue - xRightWidth[j];
+        }
+        xvecorRightLaneOff.push_back(fValue);
+    }
+
+
+    for(i=0;i<nLeftLaneCount;i++)
+    {
+        iv::LanePoint xLanePoint;
+        xLanePoint.mx = fRefX + xvectorLeftLaneOff[i]*cos(fRefHdg + M_PI/2.0);
+        xLanePoint.my = fRefY + xvectorLeftLaneOff[i]*sin(fRefHdg + M_PI/2.0);
+        xLanePoint.mz = fRefZ;
+        xLanePoint.mhdg = fRefHdg;
+
+        LaneRoadMark xRoadMark = pRoad->GetLaneRoadMarkValue(fS,static_cast<int>(i));
+        xLanePoint.mfmarkwidth = xRoadMark.GetWidth();
+        xLanePoint.mstrroadmarkcolor = xRoadMark.GetColor();
+        xLanePoint.mstrroadmarktype = xRoadMark.GetType();
+
+        xLanePoint.nLane = static_cast<int>(i);
+        xLanePoint.mstrLaneType = pLS->GetLeftLaneAt(i)->GetType();
+
+        xvectorLanePoint.push_back(xLanePoint);
+    }
+
+
+
+
+
+    return 0;
+}
+
+}
+
+

src/map/odtolanelet/roadsample.h

@@ -0,0 +1,69 @@
+#ifndef ROADSAMPLE_H
+#define ROADSAMPLE_H
+
+#include <string>
+#include <vector>
+
+#include "OpenDrive/OpenDrive.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 mstrroadmarktype = "none";
+    std::string mstrroadmarkcolor = "standard";
+    double mfroadmarkwidth;
+
+    double mfLaneWidth;
+    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 mfHdg;
+    double mfLaneOffValue;
+};
+
+
+
+class RoadSample
+{
+public:
+    RoadSample(Road * pRoad);
+
+private:
+    std::string mstrroadid;
+    std::string mstrroadname;
+    double mfRoadLen;
+    std::vector<RoadPoint> mvectorRoadPoint;
+
+private:
+    /**
+     * @brief Samle OpenDrive Road
+     * @param pRoad
+     * @return
+     */
+    int SampleRoad(Road * pRoad);
+
+
+    int SampleLanePoint(Road * pRoad,LaneSection * pLS, double fS,double fRefX,double fRefY,double fRefZ,double fRefHdg,double fLaneOffValue,std::vector<LanePoint> & xvectorLanePoint);
+};
+
+}
+
+#endif // ROADSAMPLE_H