Browse Source

change driver_map_xodrlad.

yuchuli 2 years ago
parent
commit
a2e4dd3d77

+ 2 - 2
src/driver/driver_map_xodrload/globalplan.cpp

@@ -2160,7 +2160,7 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP,
             pp.x = pp.x + off1 *cos(pp.hdg + M_PI/2.0);
             pp.y = pp.y + off1 *sin(pp.hdg + M_PI/2.0);
 
-            pp.nlrchange = 1;
+            pp.nlrchange = 0;
 
             if(xps.mainsel != xps.secondsel)
             {
@@ -2208,7 +2208,7 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP,
             pp.x = pp.x + off1 *cos(pp.hdg + M_PI/2.0);
             pp.y = pp.y + off1 *sin(pp.hdg + M_PI/2.0);
 
-            pp.nlrchange = 1;
+            pp.nlrchange = 0;
 
             if(xps.mainsel != xps.secondsel)
             {

+ 31 - 0
src/driver/driver_map_xodrload/main.cpp

@@ -898,6 +898,37 @@ void SetPlan(xodrobj xo)
 
         iv::GPSData data(new iv::GPS_INS);
         data->roadMode = 0;
+
+ //       std::cout<<"i:"<<i<<" lr:"<<xPlan[i].nlrchange<<std::endl;
+        if(xPlan[i].nlrchange == 1)
+        {
+            data->roadMode = 14;
+        }
+        if(xPlan[i].nlrchange == 2)
+        {
+            data->roadMode = 15;
+        }
+
+        if(i>50)
+        {
+            if((xPlan[i-1].nlrchange == 0)&&(xPlan[i].nlrchange != 0))
+            {
+                int j;
+                for(j=(i-1);j>=(i-50);j--)
+                {
+                    if(xPlan[i].nlrchange == 1)
+                    {
+                        mapdata[j]->roadMode = 14;
+                    }
+                    if(xPlan[i].nlrchange == 2)
+                    {
+                        mapdata[j]->roadMode = 15;
+                    }
+                    if(j <=0)break;
+                }
+            }
+        }
+
         data->gps_lat = gps_lat;
         data->gps_lat_avoidleft = gps_lat_avoidleft;
         data->gps_lat_avoidright = gps_lat_avoidright;

+ 1 - 1
src/driver/driver_map_xodrload/planpoint.h

@@ -35,7 +35,7 @@ public:
 
     double mfSecx;
     double mfSecy;
-    int nlrchange; //1 left 2 right
+    int nlrchange = 0; //1 left 2 right
     bool mbBoringRoad = false;
     bool mbNoavoid = false;
     double mfCurvature = 0.0;

+ 8 - 1
src/map/odtolanelet/odtolanelet.cpp

@@ -1,6 +1,13 @@
 #include "odtolanelet.h"
 
-odtolanelet::odtolanelet()
+odtolanelet::odtolanelet(std::string strxodrfile)
+{
+    mstrxodrfile = strxodrfile;
+}
+
+
+int odtolanelet::ConvertToLanelet(std::string strlanelet2file)
 {
 
+    return 0;
 }

+ 10 - 2
src/map/odtolanelet/odtolanelet.h

@@ -1,11 +1,19 @@
 #ifndef ODTOLANELET_H
 #define ODTOLANELET_H
 
+#include "OpenDrive/OpenDrive.h"
+
+#include <string>
 
 class odtolanelet
 {
 public:
-    odtolanelet();
+    odtolanelet(std::string strxodrfile);
+
+    int ConvertToLanelet(std::string strlanelet2file);
+
+private:
+    std::string mstrxodrfile;
 };
 
-#endif // ODTOLANELET_H
+#endif // ODTOLANELET_H

+ 12 - 0
src/map/odtolanelet/odtolanelet.pro

@@ -22,5 +22,17 @@ SOURCES += main.cpp \
     error( "Couldn't find the OpenDrive.pri file!" )
 }
 
+!include(../../common/common/xodr/TinyXML/TinyXML.pri ) {
+    error( "Couldn't find the TinyXML.pri file!" )
+}
+
+!include(../../common/common/xodr/odaux/odaux.pri ) {
+    error( "Couldn't find the odaux.pri file!" )
+}
+
 HEADERS += \
     odtolanelet.h
+
+INCLUDEPATH += $$PWD/../../common/common/xodr
+
+INCLUDEPATH += $$PWD/../../common/common/xodr/odaux

+ 1 - 0
src/test/testlanelet2/main.cpp

@@ -43,6 +43,7 @@ int main(int argc, char *argv[])
 
 
     lanelet::Origin origin2({39, 117, 0});
+
     lanelet::write("test2.osm",*laneletMap2,origin2);
  //   lanelet::write("test2.osm",*laneletMap2,origin);