modify speed plan

src/decition/decition_brain_sf/decition/adc_tools/compute_00.cpp

@@ -220,7 +220,7 @@ int iv::decition::Compute00::getMapDelta(std::vector<GPSData> MapTrace){
                 doubledata[doubledata.size()-1][3] =  doubledata[doubledata.size()-2][3];
 
     //delta filter
-    /*for(int j=10;j<doubledata.size()-10;j++)
+    for(int j=10;j<doubledata.size()-10;j++)
     {
         double delta_sum=0;
         for(int k=j-10;k<j+10;k++)
@@ -228,7 +228,7 @@ int iv::decition::Compute00::getMapDelta(std::vector<GPSData> MapTrace){
             delta_sum+= doubledata[k][3];
         }
         doubledata[j][3]=delta_sum/20;
-    }*/
+    }
 }
 
 
@@ -254,11 +254,9 @@ int iv::decition::Compute00::getPlanSpeed(std::vector<GPSData> MapTrace)
                 }*/
                 for(int i=0;i<doubledata.size();i++)
                 {
-                    if((doubledata[i][3]>-0.2)&&(doubledata[i][3]<0.2)){
-                        MapTrace[i]->roadMode=0;
-                    }else if(((doubledata[i][3]>0.2)&&(doubledata[i][3]<0.3))||((doubledata[i][3]>-0.3)&&(doubledata[i][3]<-0.2))){
+                    if((doubledata[i][3]>-0.26)&&(doubledata[i][3]<0.26)){
                         MapTrace[i]->roadMode=5;
-                    }else if(((doubledata[i][3]>0.3)&&(doubledata[i][3]<1.0))||((doubledata[i][3]>-1.0)&&(doubledata[i][3]<-0.3))){
+                    }else if(((doubledata[i][3]>=0.26)&&(doubledata[i][3]<=1.0))||((doubledata[i][3]>=-1.0)&&(doubledata[i][3]<=-0.26))){
                         MapTrace[i]->roadMode=18;
                     }else if(((doubledata[i][3]>1.0))||((doubledata[i][3]<-1.0))){
                         MapTrace[i]->roadMode=14;
@@ -294,7 +292,7 @@ int iv::decition::Compute00::getPlanSpeed(std::vector<GPSData> MapTrace)
                                 step_count++;
                             }
                         }else if(mode0to5countSum>0){
-                            for(int j=i;j>i-mode0to5countSum;j--){
+                            for(int j=i;j>=i-mode0to5countSum;j--){
                                 doubledata[j][4]=Curve_SmallSpeed;
                                 step_count++;
                             }
@@ -343,15 +341,15 @@ int iv::decition::Compute00::getPlanSpeed(std::vector<GPSData> MapTrace)
                     }
                 }
 
-//                for(int i=0;i<MapTrace.size();i++){
-//                    //将数据写入文件开始
-//                    ofstream outfile;
-//                    outfile.open("ctr0108003.txt", ostream::app);
-//                    outfile<<"Delta"<< ","  <<doubledata[i][3]<< ","<<"roadMode"<< ","  <<MapTrace[i]->roadMode<< ","
-//                           <<"plan_speed"<< ","  << doubledata[i][4]<< ","<<endl;
-//                    outfile.close();
-//                    //将数据写入文件结束
-//                }
+                for(int i=0;i<MapTrace.size();i++){
+                    //将数据写入文件开始
+                    ofstream outfile;
+                    outfile.open("ctr0108003.txt", ostream::app);
+                    outfile<<"Delta"<< ","  <<doubledata[i][3]<< ","<<"roadMode"<< ","  <<MapTrace[i]->roadMode<< ","
+                           <<"plan_speed"<< ","  << doubledata[i][4]<< ","<<endl;
+                    outfile.close();
+                    //将数据写入文件结束
+                }
 
 }