change odtolanelet. test route fail, road relation need consider.

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

@@ -1409,6 +1409,17 @@ double  Lane::GetWidthValue(double s_check)
 	return retVal;
 }
 
+double  Lane::GetWidthValueGradient(double s_check)
+{
+    double retVal=0;
+    //find the record where s_check belongs
+    int index=CheckWidthInterval(s_check);
+    //If found, return the type
+    if (index>=0)
+        retVal= mLaneWidth.at(index).GetValueGradient(s_check);
+    return retVal;
+}
+
 /**
 *	Evaluate the record and return the height object
 */

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

@@ -526,6 +526,11 @@ public:
 	*/
 	double GetWidthValue(double s_check);
 
+    /**
+    *	Evaluate the record and return the width value
+    */
+    double GetWidthValueGradient(double s_check);
+
 	/**
 	*	Evaluate the record and return the height object
 	*/

src/common/common/xodr/OpenDrive/OtherStructures.cpp

@@ -87,6 +87,15 @@ double ThirdOrderPolynom::GetValue(double s_check)
 	double value = mA+ mB*ds+ mC*ds*ds + mD*ds*ds*ds;
 	return value;
 }
+
+/**
+ * Returns the value at sample S
+ */
+double ThirdOrderPolynom::GetValueGradient(double s_check)
+{
+    double ds = s_check-mS;
+    return mB + 2.0*mC*ds+ 3.0*mD*ds*ds;
+}
 //----------------------------------------------------------------------------------
 
 

src/common/common/xodr/OpenDrive/OtherStructures.h

@@ -58,6 +58,11 @@ public:
 	 */	
 	double GetValue(double s_check);
 
+    /**
+     * Returns gradient value at Sample S
+     */
+    double GetValueGradient(double s_check);
+
 };
 //----------------------------------------------------------------------------------
 

src/map/odtolanelet/main.cpp

@@ -2,13 +2,22 @@
 
 #include "odtolanelet.h"
 
+#include <thread>
+
+
+
 int main(int argc, char *argv[])
 {
     QCoreApplication a(argc, argv);
 
-    odtolanelet * potl = new odtolanelet("/home/yuchuli/testlane.xodr");
+    odtolanelet * potl = new odtolanelet("/home/yuchuli/map/mapxiali.xodr");
 
+    int64_t time1 = std::chrono::system_clock::now().time_since_epoch().count();
     potl->ConvertToLanelet("/home/yuchuli/testlane.osm");
+    int64_t time2 = std::chrono::system_clock::now().time_since_epoch().count();
+
+    std::cout<<"use time: "<<(time2 - time1)<<std::endl;
 
     return a.exec();
 }
+

src/map/odtolanelet/odtolanelet.cpp

@@ -4,15 +4,19 @@
 
 #include "gaussprojector.h"
 
+#ifdef TESTROUTING
 
+#include <lanelet2_routing/Route.h>
+#include <lanelet2_routing/RoutingCost.h>
+#include <lanelet2_routing/RoutingGraph.h>
+#include <lanelet2_routing/RoutingGraphContainer.h>
+#include <lanelet2_traffic_rules/TrafficRulesFactory.h>
+
+#endif
 
 namespace iv
 {
 
-
-
-
-
 }
 
 odtolanelet::odtolanelet(std::string strxodrfile)
@@ -38,6 +42,7 @@ int odtolanelet::ConvertToLanelet(std::string strlanelet2file)
     unsigned int i;
     for(i=0;i<nroadnum;i++)
     {
+
         Road * pRoad = pxodr->GetRoad(i);
         lanelet::Lanelet let;
         if(Road2Lanelet(pRoad,laneletMap) == 0)
@@ -47,6 +52,33 @@ int odtolanelet::ConvertToLanelet(std::string strlanelet2file)
 
     }
 
+#ifdef TESTROUTING
+    const auto traffic_rules = lanelet::traffic_rules::TrafficRulesFactory::create(
+      lanelet::Locations::Germany, lanelet::Participants::Vehicle);
+    const auto pedestrian_rules = lanelet::traffic_rules::TrafficRulesFactory::create(
+      lanelet::Locations::Germany, lanelet::Participants::Pedestrian);
+    const lanelet::routing::RoutingGraphConstPtr vehicle_graph =
+      lanelet::routing::RoutingGraph::build(*laneletMap, *traffic_rules);
+
+
+    lanelet::Lanelet start_lanelet;
+
+    lanelet::Lanelet goal_lanelet;
+
+    GetLetByID(laneletMap,start_lanelet,1237);
+    GetLetByID(laneletMap,goal_lanelet,401);
+
+
+    // get all possible lanes that can be used to reach goal (including all possible lane change)
+    const lanelet::Optional<lanelet::routing::Route> optional_route =
+      vehicle_graph->getRoute(start_lanelet, goal_lanelet, 0);
+    if (!optional_route) {
+        std::cout<<"route fail."<<std::endl;
+    }
+
+#endif
+
+
     lanelet::Origin origin2({39.1364713, 117.0866293, 0});
 
     lanelet::write(strlanelet2file,*laneletMap,GaussProjector(origin2));
@@ -68,6 +100,7 @@ int odtolanelet::Road2Lanelet(Road * pRoad,lanelet::LaneletMapPtr & laneletMap)
     }
 
     std::shared_ptr<iv::RoadSample> pSample_ptr = std::shared_ptr<iv::RoadSample>(new iv::RoadSample(pRoad));
+
  //   iv::RoadSample * pSample = new iv::RoadSample(pRoad);
 
     std::vector<iv::RoadPoint> * pvectorRoadPoint = pSample_ptr->GetRoadPointVector();
@@ -301,3 +334,9 @@ int odtolanelet::InvertLine(lanelet::LineString3d & xinvert,lanelet::LineString3
     }
     return 0;
 }
+
+int odtolanelet::GetLetByID(lanelet::LaneletMapPtr & laneletMap,lanelet::Lanelet &let,lanelet::Id id)
+{
+    let = laneletMap->laneletLayer.get(id);
+    return 0;
+}

src/map/odtolanelet/odtolanelet.h

@@ -37,6 +37,8 @@ private:
 
     int InvertLine(lanelet::LineString3d & xinvert,lanelet::LineString3d & xraw);
 
+    int GetLetByID(lanelet::LaneletMapPtr & laneletMap,lanelet::Lanelet &let,lanelet::Id id);
+
 };
 
 #endif // ODTOLANELET_H

src/map/odtolanelet/odtolanelet.pro

@@ -9,6 +9,8 @@ CONFIG -= app_bundle
 # deprecated API in order to know how to port your code away from it.
 DEFINES += QT_DEPRECATED_WARNINGS
 
+#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.
 # You can also select to disable deprecated APIs only up to a certain version of Qt.
@@ -48,6 +50,11 @@ INCLUDEPATH += $$PWD/../../map/lanelet2/include
 
 LIBS += -L$$PWD/../../map/lanelet2/bin -llanelet2_core -llanelet2_io
 
+
+if(contains(DEFINES,TESTROUTING)){
+LIBS +=  -llanelet2_routing -llanelet2_traffic_rules
+}
+
 LIBS += -lboost_system -lboost_serialization  -lboost_filesystem -lpugixml
 
 unix:INCLUDEPATH += /usr/include/eigen3

src/map/odtolanelet/roadsample.cpp

@@ -9,6 +9,10 @@ namespace iv {
 
 RoadSample::RoadSample(Road * pRoad)
 {
+    mfRefX_max = 0;
+    mfRefY_max = 0;
+    mfRefX_min = 0;
+    mfRefY_min = 0;
     SampleRoad(pRoad);
 
     std::vector<RoadPoint> xvectorRoadPoint = mvectorRoadPoint;
@@ -22,6 +26,8 @@ int RoadSample::SampleRoad(Road * pRoad)
     mstrroadid = pRoad->GetRoadId();
     mstrroadname = pRoad->GetRoadName();
 
+    bool bMaxMinInit = false;
+
     double fS = 0;
     const double fStep = 0.1;
     LaneSection * pLS;
@@ -29,6 +35,8 @@ int RoadSample::SampleRoad(Road * pRoad)
     unsigned int i;
     int nSec = 0;
 
+
+
     for(i=0;i<nLSCount;i++)
     {
         pLS = pRoad->GetLaneSection(i);
@@ -38,6 +46,7 @@ int RoadSample::SampleRoad(Road * pRoad)
             break;
         }
 
+
         std::vector<double> xvectorFeaturePoint = GetFeaturePoint(pLS);
 
         double fLastRefX,fLastRefY,fLastRefZ,fLastRefHdg,fLastOffsetValue,fLastS;
@@ -62,6 +71,7 @@ int RoadSample::SampleRoad(Road * pRoad)
         nSec++;
         while(fS<send_Section)
         {
+
             double fRefX,fRefY,fRefZ,fRefHdg,fOffsetValue;
             pRoad->GetGeometryCoords(fS,fRefX,fRefY,fRefHdg);
             fRefZ = pRoad->GetElevationValue(fS);
@@ -85,6 +95,11 @@ int RoadSample::SampleRoad(Road * pRoad)
             if((fS - sstart_Section)>fStep*0.5)
                 bIsCrossFeature = IsCrossFeaturePoint(xvectorFeaturePoint,fS,fLastS);
 
+            if(bIsCrossFeature)
+            {
+                if(fS> send_Section)fS = send_Section;
+            }
+
             RoadPoint xRP;
             xRP.ms = fS;
             xRP.mfRefX = fRefX;
@@ -94,8 +109,24 @@ int RoadSample::SampleRoad(Road * pRoad)
             xRP.mfLaneOffValue = fOffsetValue;
             xRP.nSecNum = nSec;
 
+            if(bMaxMinInit)
+            {
+                if(mfRefX_max<fRefX)mfRefX_max = fRefX;
+                if(mfRefX_min>fRefX)mfRefX_min = fRefX;
+                if(mfRefY_max<fRefY)mfRefY_max = fRefY;
+                if(mfRefY_min>fRefY)mfRefY_min = fRefY;
+            }
+            else
+            {
+                mfRefX_max = fRefX;
+                mfRefX_min = fRefX;
+                mfRefY_max = fRefY;
+                mfRefY_min = fRefY;
+                bMaxMinInit = true;
+            }
+
             std::vector<LanePoint> xvectorLanePoint;
-            SampleLanePoint(pRoad,pLS,fS,xvectorLanePoint);
+ //           SampleLanePoint(pRoad,pLS,fS,xvectorLanePoint);
 
 
             bool bInserPoint = false;
@@ -112,18 +143,27 @@ int RoadSample::SampleRoad(Road * pRoad)
                 }
                 else
                 {
-                    if((fabs((fRefHdg - fLastRefHdg)/(fS - fLastS))>0.01) ||((fS - fLastS)>=10.0))
+                    double fHdgDiff = fRefHdg - fLastRefHdg;
+                    if(fHdgDiff>M_PI)fHdgDiff = fHdgDiff - 2.0*M_PI;
+                    if(fHdgDiff<-M_PI)fHdgDiff = fHdgDiff + 2.0*M_PI;
+                    if((fabs((fHdgDiff)/(1.0))>0.05) ||((fS - fLastS)>=10.0))
                     {
                         bInserPoint = true;
                     }
                 }
             }
 
+            if(mstrroadid == "10014")
+            {
+                int a = 1;
+            }
+
             if((bInserPoint == false)&&(bHaveLast))
             {
 
                 if(bIsCrossFeature)
                 {
+                    SampleLanePoint(pRoad,pLS,fS,xvectorLanePoint);
                     xRP.mvectorLanePoint = xvectorLanePoint;
                     unsigned int k;
                     unsigned int ksize = static_cast<unsigned int >(xRP.mvectorLanePoint.size());
@@ -151,24 +191,41 @@ int RoadSample::SampleRoad(Road * pRoad)
             if((bInserPoint == false)&&(bHaveLast))
             {
 
-                xRP.mvectorLanePoint = xvectorLanePoint;
-                unsigned int k;
-                unsigned int ksize = static_cast<unsigned int >(xRP.mvectorLanePoint.size());
-                for(k=0;k<ksize;k++)
+                if((fS-fLastS)>1.0)
                 {
-                    if(fabs(xRP.mvectorLanePoint[k].mfLaneWidth - mvectorRoadPoint[mvectorRoadPoint.size()-1].mvectorLanePoint[k].mfLaneWidth )>0.1)
+                    double fGradientMax = GetMaxWidthRatio(pLS,fS);
+                    if(fabs(fGradientMax*(fS-fLastS))>0.1)
                     {
                         bInserPoint = true;
-                        break;
                     }
                 }
+
+
+//                xRP.mvectorLanePoint = xvectorLanePoint;
+//                unsigned int k;
+//                unsigned int ksize = static_cast<unsigned int >(xRP.mvectorLanePoint.size());
+//                for(k=0;k<ksize;k++)
+//                {
+//                    if(fabs(xRP.mvectorLanePoint[k].mfLaneWidth - mvectorRoadPoint[mvectorRoadPoint.size()-1].mvectorLanePoint[k].mfLaneWidth )>0.1)
+//                    {
+//                        bInserPoint = true;
+//                        break;
+//                    }
+//                }
             }
 
+
+
             if((i == (nLSCount -1))&&(fabs(fS - send_Section)<0.001))
             {
                 bInserPoint = true;
             }
 
+            if((bIsCrossFeature== false)&&bInserPoint)
+            {
+                SampleLanePoint(pRoad,pLS,fS,xvectorLanePoint);
+            }
+
 
             if(bInserPoint)
             {
@@ -190,6 +247,8 @@ int RoadSample::SampleRoad(Road * pRoad)
         }
     }
 
+    std::cout<<"Road: "<<mstrroadid<<" max "<<mfRefX_max<<" "<<mfRefY_max<<"  min "<<mfRefX_min<<" "<<mfRefY_min<<std::endl;
+
     return 0;
 }
 
@@ -367,6 +426,7 @@ std::vector<double> RoadSample::GetFeaturePoint(LaneSection * pLS)
                 xvectorFeature.push_back(xvectorS[indexmin]);
             }
         }
+        xvectorS.erase(xvectorS.begin() + indexmin);
     }
 
     return xvectorFeature;
@@ -388,6 +448,32 @@ bool RoadSample::IsCrossFeaturePoint(std::vector<double> & xvectorFeature,double
     return false;
 }
 
+double RoadSample::GetMaxWidthRatio(LaneSection * pLS, double fS)
+{
+    unsigned int nlanecount = static_cast<unsigned int>(pLS->GetLaneCount());
+    unsigned int i;
+    double fRtn = 0.0;
+    for(i=0;i<nlanecount;i++)
+    {
+        Lane * pLane = pLS->GetLane(i);
+        if(pLane != NULL)
+        {
+            if(pLane->GetId() != 0)
+            {
+               double fGradient =  pLane->GetWidthValueGradient(fS-pLS->GetS());
+               if(fabs(fGradient)>fabs(fRtn))
+               {
+                   fRtn = fGradient;
+               }
+            }
+        }
+    }
+    return fRtn;
+}
+
+
+
+
 
 
 }

src/map/odtolanelet/roadsample.h

@@ -60,6 +60,7 @@ private:
     std::string mstrroadname;
     double mfRoadLen;
     std::vector<RoadPoint> mvectorRoadPoint;
+    double mfRefX_min,mfRefX_max,mfRefY_min,mfRefY_max;
 
 private:
     /**
@@ -77,6 +78,8 @@ private:
     std::vector<double> GetFeaturePoint(LaneSection * pLS);
 
     bool IsCrossFeaturePoint(std::vector<double> & xvectorFeature,double & fS,double fSLast);
+
+    inline double GetMaxWidthRatio(LaneSection * pLS, double fS);
 };
 
 }