change odtolanelet. Simple convert complete.

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

@@ -2450,6 +2450,8 @@ short int Road::GetLaneCoords(double s_check, double t_check,double &retX, doubl
 
     double fSuperElevationValue = GetSuperElevationValue(s_check);
 
+    retHDG = fRefHDG;
+
     retX = fRefX + t_check*cos(fSuperElevationValue)*cos(retHDG + M_PI/2.0) + fOffValue;
     retY = fRefY + t_check*cos(fSuperElevationValue)*sin(retHDG + M_PI/2.0) + fOffValue;
 

src/map/odtolanelet/gaussprojector.cpp

@@ -0,0 +1,23 @@
+#include "gaussprojector.h"
+
+#include "../../../common/common/math/gnss_coordinate_convert.h"
+
+lanelet::BasicPoint3d GaussProjector::forward(const lanelet::GPSPoint& p) const
+{
+    double x0,y0;
+    double x,y;
+    GaussProjCal(origin().position.lon,origin().position.lat,&x0,&y0);
+    GaussProjCal(p.lon,p.lat,&x,&y);
+    x = x- x0;
+    y = y- y0;
+    return {x,y,p.ele};
+}
+
+lanelet::GPSPoint GaussProjector::reverse(const lanelet::BasicPoint3d& p) const
+{
+    double x0,y0;
+    GaussProjCal(origin().position.lon,origin().position.lat,&x0,&y0);
+    double lon,lat;
+    GaussProjInvCal(x0+p.x(),y0+p.y(),&lon,&lat);
+    return {lat,lon,p.z()};
+}

src/map/odtolanelet/gaussprojector.h

@@ -0,0 +1,19 @@
+#ifndef GAUSSPROJECTOR_H
+#define GAUSSPROJECTOR_H
+
+
+#include "lanelet2_io/Projection.h"
+
+
+class GaussProjector: public lanelet::Projector
+{
+public:
+ using lanelet::Projector::Projector;
+ lanelet::BasicPoint3d forward(const lanelet::GPSPoint& p) const override;
+
+ lanelet::GPSPoint reverse(const lanelet::BasicPoint3d& p) const override;
+};
+
+
+
+#endif // GAUSSPROJECTOR_H

src/map/odtolanelet/main.cpp

@@ -8,7 +8,7 @@ int main(int argc, char *argv[])
 
     odtolanelet * potl = new odtolanelet("/home/yuchuli/testlane.xodr");
 
-    potl->ConvertToLanelet("hello");
+    potl->ConvertToLanelet("/home/yuchuli/testlane.osm");
 
     return a.exec();
 }

src/map/odtolanelet/odtolanelet.cpp

@@ -2,7 +2,9 @@
 
 #include <vector>
 
-#include "roadsample.h"
+#include "gaussprojector.h"
+
+
 
 namespace iv
 {
@@ -31,24 +33,34 @@ int odtolanelet::ConvertToLanelet(std::string strlanelet2file)
         return -1;
     }
 
+    lanelet::LaneletMapPtr laneletMap  = std::make_shared<lanelet::LaneletMap>();
     unsigned int nroadnum = pxodr->GetRoadCount();
     unsigned int i;
     for(i=0;i<nroadnum;i++)
     {
         Road * pRoad = pxodr->GetRoad(i);
         lanelet::Lanelet let;
-        if(Road2Lanelet(pRoad,let) == 0)
+        if(Road2Lanelet(pRoad,laneletMap) == 0)
         {
 
         }
 
     }
 
+    lanelet::Origin origin2({39.1364713, 117.0866293, 0});
+
+    lanelet::write(strlanelet2file,*laneletMap,GaussProjector(origin2));
+
+    std::cout<<"convert complete."<<std::endl;
+//    lanelet::write(strlanelet2file,*laneletMap,origin2);
+
     return 0;
 }
 
-int odtolanelet::Road2Lanelet(Road * pRoad,lanelet::Lanelet & LaneLet)
+int odtolanelet::Road2Lanelet(Road * pRoad,lanelet::LaneletMapPtr & laneletMap)
 {
+
+
     if(pRoad == NULL)
     {
         std::cout<<"odtolanelet::Road2Lanelet Road is NULL. "<<std::endl;
@@ -64,91 +76,228 @@ int odtolanelet::Road2Lanelet(Road * pRoad,lanelet::Lanelet & LaneLet)
 
     unsigned int i;
 
-    for(i=0;i<nRPSize;i++)
-    {
-        unsigned int j;
-//        pvectorRoadPoint->at(i).mvectorLanePoint
-    }
-
-    return  0;
-
     std::vector<lanelet::LineString3d> xvectorlinestring;
-    std::vector<std::string> xvectortype;
-    std::vector<std::string> xvectorsubtype;
+    std::vector<lanelet::Point3d> xvectorPoint;
+    std::vector<lanelet::Point3d> xvectorLastPoint;
 
-    unsigned int nLSCount = pRoad->GetLaneSectionCount();
+    unsigned int ncenterlanepos = 0;
 
-    unsigned nleftcount = 0;
-    unsigned nrightcount = 0;
+    for(i=0;i<nRPSize;i++)
+    {
+        bool bNewLet = false;
+        if(i == 0)
+        {
+            bNewLet = true;
+        }
+        else
+        {
+            if(pvectorRoadPoint->at(i).nSecNum != pvectorRoadPoint->at(i-1).nSecNum)
+            {
+                //Create LaneLet
 
-    double fS = 0;
-    const double fstep = 0.1;
+                linetolet(laneletMap,xvectorlinestring,ncenterlanepos);
+                bNewLet = true;
+            }
+        }
 
+        if(bNewLet)
+        {
+            xvectorlinestring.clear();
 
-    for(i=0;i<nLSCount;i++)
-    {
-        LaneSection * pLS = pRoad->GetLaneSection(i);
+            unsigned int nlanesize = static_cast<unsigned int>(pvectorRoadPoint->at(i).mvectorLanePoint.size());
 
-        if(i == 0)
-        {
-            nleftcount = pLS->GetLeftLaneCount();
-            nrightcount = pLS->GetRightLaneCount();
-            unsigned int nline = pLS->GetLeftLaneCount() + pLS->GetRightLaneCount() + 1;
             unsigned int j;
-
-            for(j=0;j<nline;j++)
+            for(j=0;j<nlanesize;j++)
             {
                 lanelet::LineString3d xline;
                 xline.setId(mid);mid++;
+                odtypetolettype(pvectorRoadPoint->at(i).mvectorLanePoint[j],xline);
                 xvectorlinestring.push_back(xline);
-                xvectortype.clear();
-                xvectorsubtype.clear();
+                if(pvectorRoadPoint->at(i).mvectorLanePoint[j].nLane == 0)
+                {
+                    ncenterlanepos = j;
+                }
             }
 
-        }
-        else
-        {
-            bool bNeedNewLine = false;
-            if((pLS->GetLeftLaneCount() != pRoad->GetLaneSection(i-1)->GetLeftLaneCount())||(pLS->GetRightLaneCount() != pRoad->GetLaneSection(i-1)->GetRightLaneCount()))
+            if(i== 0)
             {
-                bNeedNewLine = true;
-            }
+                LanePointToPoint3D(pvectorRoadPoint->at(i).mvectorLanePoint,xvectorPoint);
+                unsigned int nlanesize = static_cast<unsigned int>(pvectorRoadPoint->at(i).mvectorLanePoint.size());
 
-            if(bNeedNewLine)
+                unsigned int j;
+                for(j=0;j<nlanesize;j++)xvectorlinestring[j].push_back(xvectorPoint[j]);
+            }
+            else
             {
-                //Create LaneLet
+                LanePointToPoint3DWithLast(pvectorRoadPoint->at(i).mvectorLanePoint,xvectorPoint,xvectorLastPoint);
+                unsigned int nlanesize = static_cast<unsigned int>(pvectorRoadPoint->at(i).mvectorLanePoint.size());
 
-                nleftcount = pLS->GetLeftLaneCount();
-                nrightcount = pLS->GetRightLaneCount();
-                unsigned int nline = pLS->GetLeftLaneCount() + pLS->GetRightLaneCount() + 1;
                 unsigned int j;
-
-                for(j=0;j<nline;j++)
-                {
-                    lanelet::LineString3d xline;
-                    xline.setId(mid);mid++;
-                    xvectorlinestring.push_back(xline);
-                }
+                for(j=0;j<nlanesize;j++)xvectorlinestring[j].push_back(xvectorPoint[j]);
             }
 
         }
+        else
+        {
+            LanePointToPoint3D(pvectorRoadPoint->at(i).mvectorLanePoint,xvectorPoint);
+            unsigned int nlanesize = static_cast<unsigned int>(pvectorRoadPoint->at(i).mvectorLanePoint.size());
 
-        fS = pLS->GetS();
+            unsigned int j;
+            for(j=0;j<nlanesize;j++)xvectorlinestring[j].push_back(xvectorPoint[j]);
+        }
 
-    //    pLS->GetLane(0)->GetRoadMarkValue(s);
+        xvectorLastPoint = xvectorPoint;
+//        pvectorRoadPoint->at(i).mvectorLanePoint
+    }
 
-        while(fS<pLS->GetS2())
-        {
+    if(xvectorlinestring.size()>0)
+    {
+        linetolet(laneletMap,xvectorlinestring,ncenterlanepos);
+    }
 
-            fS = fS + fstep;
-            double x,y,hdg;
-            pRoad->GetGeometryCoords(fS,x,y,hdg);
-        }
+    return  0;
 
+}
 
+void odtolanelet::odtypetolettype(iv::LanePoint & xLanePoint, lanelet::LineString3d & xlinestring)
+{
+    if(xLanePoint.mstrroadmarkcolor.size()>0)
+    {
+        xlinestring.setAttribute("color",lanelet::Attribute(xLanePoint.mstrroadmarkcolor));
+    }
+    if(xLanePoint.mstrroadmarktype == "none")
+    {
+        xlinestring.setAttribute("type",lanelet::Attribute("virtual"));
+        return;
+    }
+    if(xLanePoint.mstrroadmarktype == "solid")
+    {
+        xlinestring.setAttribute("type",lanelet::Attribute("line_thin"));
+        xlinestring.setAttribute("subtype",lanelet::Attribute("solid"));
+        return;
+    }
+    if(xLanePoint.mstrroadmarktype == "broken")
+    {
+        xlinestring.setAttribute("type",lanelet::Attribute("line_thin"));
+        xlinestring.setAttribute("subtype",lanelet::Attribute("solid"));
+        return;
+    }
+    if(xLanePoint.mstrroadmarktype == "broken")
+    {
+        xlinestring.setAttribute("type",lanelet::Attribute("line_thin"));
+        xlinestring.setAttribute("subtype",lanelet::Attribute("dashed"));
+        return;
+    }
+    if(xLanePoint.mstrroadmarktype == "solid solid")
+    {
+        xlinestring.setAttribute("type",lanelet::Attribute("line_thin"));
+        xlinestring.setAttribute("subtype",lanelet::Attribute("solid_solid"));
+        return;
+    }
+    if(xLanePoint.mstrroadmarktype == "solid broken")
+    {
+        xlinestring.setAttribute("type",lanelet::Attribute("line_thin"));
+        xlinestring.setAttribute("subtype",lanelet::Attribute("solid_dashed"));
+        return;
+    }
+    if(xLanePoint.mstrroadmarktype == "broken solid")
+    {
+        xlinestring.setAttribute("type",lanelet::Attribute("line_thin"));
+        xlinestring.setAttribute("subtype",lanelet::Attribute("dashed_solid"));
+        return;
+    }
+    if(xLanePoint.mstrroadmarktype == "curb")
+    {
+        xlinestring.setAttribute("type",lanelet::Attribute("curbstone"));
+        xlinestring.setAttribute("subtype",lanelet::Attribute("high"));
+        return;
+    }
+    if(xLanePoint.mstrroadmarktype == "edge")
+    {
+        xlinestring.setAttribute("type",lanelet::Attribute("road_border"));
+        return;
+    }
+    xlinestring.setAttribute("type",lanelet::Attribute("virtual"));
+    return;
+}
 
+void odtolanelet::LanePointToPoint3D(std::vector<iv::LanePoint> & xvectorLanePoint, std::vector<lanelet::Point3d> & xvectorPoint3D)
+{
+    xvectorPoint3D.clear();
+    unsigned int i;
+    unsigned int nLPsize = static_cast<unsigned int>(xvectorLanePoint.size());
+    for(i=0;i<nLPsize;i++)
+    {
+        lanelet::Point3d xPoint(mid,xvectorLanePoint[i].mx,xvectorLanePoint[i].my,xvectorLanePoint[i].mz);
+        mid++;
+        xvectorPoint3D.push_back(xPoint);
     }
+}
+
+void odtolanelet::LanePointToPoint3DWithLast(std::vector<iv::LanePoint> & xvectorLanePoint, std::vector<lanelet::Point3d> & xvectorPoint3D,
+                                std::vector<lanelet::Point3d> & xvectorPoint3DLast)
+{
+    xvectorPoint3D.clear();
+    unsigned int i;
+    unsigned int nLPsize = static_cast<unsigned int>(xvectorLanePoint.size());
+    for(i=0;i<nLPsize;i++)
+    {
+        unsigned int j;
+        unsigned int nLastSize = static_cast<unsigned int>(xvectorPoint3DLast.size());
+        bool bUseOld = false;
+        for(j=0;j<nLastSize;j++)
+        {
+            if((fabs(xvectorLanePoint[i].mx - xvectorPoint3DLast[j].x())<0.001)&(fabs(xvectorLanePoint[i].my - xvectorPoint3DLast[j].y())<0.001))
+            {
+                bUseOld = true;
+                xvectorPoint3D.push_back(xvectorPoint3DLast[j]);
+                break;
+            }
+        }
+        if(bUseOld == false)
+        {
+            lanelet::Point3d xPoint(mid,xvectorLanePoint[i].mx,xvectorLanePoint[i].my,xvectorLanePoint[i].mz);
+            mid++;
+            xvectorPoint3D.push_back(xPoint);
+        }
+
+    }
+}
 
+void odtolanelet::linetolet(lanelet::LaneletMapPtr & laneletMap,std::vector<lanelet::LineString3d> & xvectorlinestring, unsigned int ncenterlane)
+{
+    unsigned int i;
+    unsigned int nsize = static_cast<unsigned int>(xvectorlinestring.size()) ;
+    for(i=0;i<ncenterlane;i++)
+    {
+        lanelet::LineString3d xleft;
+        lanelet::LineString3d xright ;
+        InvertLine(xleft,xvectorlinestring[i+1]);
+        InvertLine(xright,xvectorlinestring[i]);
+        lanelet::Lanelet xlet(mid,xleft,xright);
+        mid++;
+        laneletMap->add(xlet);
+    }
+    for(i=ncenterlane;i<(nsize-1);i++)
+    {
+        lanelet::Lanelet xlet(mid,xvectorlinestring[i],xvectorlinestring[i+1]);
+        mid++;
+        laneletMap->add(xlet);
+    }
+}
 
+int odtolanelet::InvertLine(lanelet::LineString3d & xinvert,lanelet::LineString3d & xraw)
+{
+    xinvert.setId(mid);mid++;
+    if(xraw.hasAttribute("color"))xinvert.setAttribute("color",xraw.attribute("color"));
+    if(xraw.hasAttribute("subtype"))xinvert.setAttribute("subtype",xraw.attribute("subtype"));
+    if(xraw.hasAttribute("type"))xinvert.setAttribute("type",xraw.attribute("type"));
+    xinvert.clear();
+    unsigned int npointssize = static_cast<unsigned int >(xraw.size());
+    unsigned int i;
+    for(i=0;i<npointssize;i++)
+    {
+        xinvert.push_back(xraw[npointssize-1-i]);
+    }
     return 0;
 }

src/map/odtolanelet/odtolanelet.h

@@ -9,6 +9,8 @@
 #include <string>
 #include <iostream>
 
+#include "roadsample.h"
+
 class odtolanelet
 {
 public:
@@ -22,7 +24,19 @@ private:
     lanelet::Id mid = 1;
 
 private:
-    int Road2Lanelet(Road * pRoad,lanelet::Lanelet & LaneLet);
+    int Road2Lanelet(Road * pRoad,lanelet::LaneletMapPtr & laneletMap);
+
+    void odtypetolettype(iv::LanePoint & xLanePoint, lanelet::LineString3d & xlinestring);
+
+    void LanePointToPoint3D(std::vector<iv::LanePoint> & xvectorLanePoint, std::vector<lanelet::Point3d> & xvectorPoint3D);
+
+    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);
+
+    int InvertLine(lanelet::LineString3d & xinvert,lanelet::LineString3d & xraw);
+
 };
 
 #endif // ODTOLANELET_H

src/map/odtolanelet/odtolanelet.pro

@@ -16,7 +16,9 @@ DEFINES += QT_DEPRECATED_WARNINGS
 
 SOURCES += main.cpp \
     odtolanelet.cpp \
-    roadsample.cpp
+    roadsample.cpp \
+    gaussprojector.cpp \
+    ../../common/common/math/gnss_coordinate_convert.cpp
 
 
 !include(../../common/common/xodr/OpenDrive/OpenDrive.pri ) {
@@ -33,9 +35,12 @@ SOURCES += main.cpp \
 
 HEADERS += \
     odtolanelet.h \
-    roadsample.h
+    roadsample.h \
+    gaussprojector.h \
+    ../../common/common/math/gnss_coordinate_convert.h
 
 INCLUDEPATH += $$PWD/../../common/common/xodr
+INCLUDEPATH += $$PWD/../../common/common
 
 INCLUDEPATH += $$PWD/../../common/common/xodr/odaux
 

src/map/odtolanelet/roadsample.cpp

@@ -164,6 +164,11 @@ int RoadSample::SampleRoad(Road * pRoad)
                 }
             }
 
+            if((i == (nLSCount -1))&&(fabs(fS - send_Section)<0.001))
+            {
+                bInserPoint = true;
+            }
+
 
             if(bInserPoint)
             {
@@ -180,6 +185,7 @@ int RoadSample::SampleRoad(Road * pRoad)
                 bHaveLast = true;
             }
 
+
             fS = fS + fStep;
         }
     }
@@ -242,12 +248,12 @@ int RoadSample::SampleLanePoint(Road * pRoad,LaneSection * pLS, double fS,std::v
     for(i=0;i<nLeftLaneCount;i++)
     {      
         pRoad->GetLaneCoords(fS,xvectorLeftLaneT[i],xLanePoint.mx,xLanePoint.my,xLanePoint.mz,xLanePoint.mhdg);
-        LaneRoadMark xRoadMark = pRoad->GetLaneRoadMarkValue(fS,static_cast<int>(i));
+        LaneRoadMark xRoadMark = pRoad->GetLaneRoadMarkValue(fS,static_cast<int>(i+1));
         xLanePoint.mfmarkwidth = xRoadMark.GetWidth();
         xLanePoint.mstrroadmarkcolor = xRoadMark.GetColor();
         xLanePoint.mstrroadmarktype = xRoadMark.GetType();
-        xLanePoint.nLane = static_cast<int>(i);
-        xLanePoint.mstrLaneType = pLS->GetLeftLaneAt(i)->GetType();
+        xLanePoint.nLane = static_cast<int>(i+1);
+        xLanePoint.mstrLaneType = pLS->GetLeftLaneAt(i+1)->GetType();
         xLanePoint.mfLaneWidth = xLeftWidth[nLeftLaneCount-1 -i];
         xvectorLanePoint.push_back(xLanePoint);
     }
@@ -257,21 +263,21 @@ int RoadSample::SampleLanePoint(Road * pRoad,LaneSection * pLS, double fS,std::v
     xLanePoint.mfmarkwidth = xRoadMark.GetWidth();
     xLanePoint.mstrroadmarkcolor = xRoadMark.GetColor();
     xLanePoint.mstrroadmarktype = xRoadMark.GetType();
-    xLanePoint.nLane = static_cast<int>(i);
-    xLanePoint.mstrLaneType = pLS->GetLeftLaneAt(i)->GetType();
+    xLanePoint.nLane = static_cast<int>(0);
+    xLanePoint.mstrLaneType = pLS->GetCenterLane()->GetType();
     xLanePoint.mfLaneWidth = 0.0;
     xvectorLanePoint.push_back(xLanePoint);
 
     for(i=0;i<nRightLaneCount;i++)
     {
         pRoad->GetLaneCoords(fS,xvectorRightLaneT[i],xLanePoint.mx,xLanePoint.my,xLanePoint.mz,xLanePoint.mhdg);
-        LaneRoadMark xRoadMark = pRoad->GetLaneRoadMarkValue(fS,static_cast<int>(i)*(-1));
+        LaneRoadMark xRoadMark = pRoad->GetLaneRoadMarkValue(fS,static_cast<int>(i+1)*(-1));
         xLanePoint.mfmarkwidth = xRoadMark.GetWidth();
         xLanePoint.mstrroadmarkcolor = xRoadMark.GetColor();
         xLanePoint.mstrroadmarktype = xRoadMark.GetType();
         xLanePoint.mfLaneWidth = xRightWidth[i];
-        xLanePoint.nLane = static_cast<int>(i);
-        xLanePoint.mstrLaneType = pLS->GetLeftLaneAt(i)->GetType();
+        xLanePoint.nLane = static_cast<int>(i+1);
+        xLanePoint.mstrLaneType = pLS->GetRightLaneAt(i+1)->GetType();
         xvectorLanePoint.push_back(xLanePoint);
     }
 
@@ -382,6 +388,8 @@ bool RoadSample::IsCrossFeaturePoint(std::vector<double> & xvectorFeature,double
     return false;
 }
 
+
+
 }