Merge branch 'master' of http://111.33.136.149:3000/adc_pilot/modularization

src/driver/driver_map_xodrload/main.cpp

@@ -511,7 +511,7 @@ void SetPlan(xodrobj xo)
     nSize = xPlan.size();
 
     std::vector<iv::GPSData> mapdata;
-
+    //maptrace
     QFile xfile;
     QString strpath;
     strpath = getenv("HOME");
@@ -519,6 +519,14 @@ void SetPlan(xodrobj xo)
     xfile.setFileName(strpath);
     xfile.open(QIODevice::ReadWrite);
 
+    //maptrace_add
+    QFile xfile_1;
+    QString strpath_1;
+    strpath_1 = getenv("HOME");
+    strpath_1 = strpath_1 + "/map/mapadd.txt";
+    xfile_1.setFileName(strpath_1);
+    xfile_1.open(QIODevice::ReadWrite);
+
     for(i=0;i<nSize;i++)
     {
         iv::GPSData data(new iv::GPS_INS);
@@ -568,9 +576,26 @@ void SetPlan(xodrobj xo)
             for(k=(mapdata.size()-1);k>(mapdata.size()-150);k--)
             {
                 if(k<0)break;
+                if((mapdata.at(k)->mode2 != -1)&&(mapdata.at(k)->mode2 !=3000))
+                {
+                 continue;
+                }
                 mapdata.at(k)->mode2 = 3000;
             }
-        }
+         }
+        if(data->mode2 == 1000001)
+        {
+            int k;
+            for(k=(mapdata.size()-1);k>(mapdata.size()-10);k--)
+            {
+                if(k<0)break;
+                if((mapdata.at(k)->mode2 != -1)&&(mapdata.at(k)->mode2 !=1000001))
+                {
+                 continue;
+                }
+                mapdata.at(k)->mode2 = 1000001;
+            }
+         }
 
 #ifdef BOAVOID
     if(isboringroad(xPlan[i].nRoadID))
@@ -617,17 +642,31 @@ void SetPlan(xodrobj xo)
 
         mapdata.push_back(data);
 
+
+//        char strline[255];
+//  //      snprintf(strline,255,"%d\t%11.7f\t%11.7f\t%d\t%11.3f\t%d\t%d\t%d\t%d\t%d\t%d\t%11.7f\n",
+//  //               i,data->gps_lng,data->gps_lat,0,data->ins_heading_angle,0,0,0,data->roadOri,data->roadSum,data->mode2,data->mfCurvature);
+//              snprintf(strline,255,"%d\t%11.7f\t%11.7f\t%d\t%d\t%11.3f\t%d\t%d\t%d\t%d\t%d\n",
+//                      i,data->gps_lng,data->gps_lat,0,0,data->ins_heading_angle,0,0,data->roadSum,data->roadOri,data->mode2);
+//        xfile.write(strline);
+    }
+
+
+    for(int j=0;j<nSize;j++)
+    {
         char strline[255];
-  //      snprintf(strline,255,"%d\t%11.7f\t%11.7f\t%d\t%11.3f\t%d\t%d\t%d\t%d\t%d\t%d\t%11.7f\n",
-  //               i,data->gps_lng,data->gps_lat,0,data->ins_heading_angle,0,0,0,data->roadOri,data->roadSum,data->mode2,data->mfCurvature);
-              snprintf(strline,255,"%d\t%11.7f\t%11.7f\t%d\t%d\t%11.3f\t%d\t%d\t%d\t%d\n",
-                      i,data->gps_lng,data->gps_lat,0,0,data->ins_heading_angle,0,0,data->roadSum,data->roadOri);
+        snprintf(strline,255,"%d\t%11.7f\t%11.7f\t%d\t%d\t%11.3f\t%d\t%d\t%d\t%d\n",
+                      j,mapdata.at(j)->gps_lng,mapdata.at(j)->gps_lat,0,0,mapdata.at(j)->ins_heading_angle,0,0,mapdata.at(j)->roadSum,mapdata.at(j)->roadOri);
         xfile.write(strline);
-
+     //mapttrace1
+        char strline_1[255];
+        snprintf(strline_1,255,"%d\t%11.7f\t%11.7f\t%d\t%d\t%11.3f\t%d\t%d\t%d\t%d\t%d\n",
+                      j,mapdata.at(j)->gps_lng,mapdata.at(j)->gps_lat,0,0,mapdata.at(j)->ins_heading_angle,0,0,mapdata.at(j)->roadSum,mapdata.at(j)->roadOri,mapdata.at(j)->mode2);
+        xfile_1.write(strline_1);
     }
 
     xfile.close();
-
+    xfile_1.close();
     ShareMap(mapdata);