add speed plan pro

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

@@ -8,8 +8,11 @@
 #include <iostream>
 #include <fstream>
 #include <control/can.h>
+#include <sstream>
+#include <cstdlib>
+#include <string>
+#include <vector>
 using namespace std;
-
 #define PI (3.1415926535897932384626433832795)
 
 iv::decition::Compute00::Compute00() {
@@ -214,11 +217,27 @@ int iv::decition::Compute00::getMapDelta(std::vector<GPSData> MapTrace){
                 MapTrace[i]->delta=doubledata[i][3];
     }
                 MapTrace[MapTrace.size()-1]->delta = MapTrace[MapTrace.size()-2]->delta;
+
+    //delta filter
+    for(int j=10;j<MapTrace.size()-10;j++)
+    {
+        double delta_sum=0;
+        for(int k=j-10;k<j+10;k++)
+        {
+            delta_sum+= MapTrace[k]->delta;
+        }
+        MapTrace[j]->delta=delta_sum/20;
+    }
 }
 
 
 int iv::decition::Compute00::getPlanSpeed(std::vector<GPSData> MapTrace)
 {
+                int mode0to5count=0,mode0to18count=0,mode0to5countSum=0,mode0to18countSum=0,mode18count=0,mode18countSum=0,mode0to5flash=0,mode18flash=0;
+                double straightSpeed=min(ServiceCarStatus.mroadmode_vel.mfmode0,60.0);
+                double straightCurveSpeed=min(ServiceCarStatus.mroadmode_vel.mfmode5,40.0);
+                double Curve_SmallSpeed=min(ServiceCarStatus.mroadmode_vel.mfmode18,15.0);
+                double Curve_LargeSpeed=min(ServiceCarStatus.mroadmode_vel.mfmode14,8.0);
                 getMapDelta(MapTrace);
                 for(int i=0;i<MapTrace.size();i++)
                 {
@@ -235,14 +254,90 @@ int iv::decition::Compute00::getPlanSpeed(std::vector<GPSData> MapTrace)
                 for(int i=0;i<MapTrace.size();i++)
                 {
                     if(MapTrace[i]->roadMode==0){
-                        MapTrace[i]->plan_speed=min(ServiceCarStatus.mroadmode_vel.mfmode0,60.0);
+                        MapTrace[i]->plan_speed=straightSpeed;
+                        mode0to5count++;
+                        //mode0to18count++;
+                        mode18count=0;
+                        mode0to5flash=mode0to5count;
                     }else if(MapTrace[i]->roadMode==5){
-                        MapTrace[i]->plan_speed=min(ServiceCarStatus.mroadmode_vel.mfmode5,40.0);
+                        MapTrace[i]->plan_speed=straightCurveSpeed;
+                        mode0to5count++;
+                        //mode0to18count++;
+                        mode18count=0;
+                        mode0to5flash=mode0to5count;
                     }else if(MapTrace[i]->roadMode==18){
-                        MapTrace[i]->plan_speed=min(ServiceCarStatus.mroadmode_vel.mfmode18,20.0);
+                        mode0to5countSum=mode0to5count;
+                        MapTrace[i]->plan_speed=Curve_SmallSpeed;
+
+                        double brake_distance_double=(pow((straightCurveSpeed/3.6),2)-pow((Curve_SmallSpeed/3.6),2))*10/0.6;
+                        int brake_distance=(int)brake_distance_double;
+                        int step_count=0;
+                        if((i>brake_distance)&&(mode0to5countSum>brake_distance))
+                        {
+                            double step_speed=(straightCurveSpeed-Curve_SmallSpeed)/brake_distance;
+
+                            for(int j=i;j>i-brake_distance;j--){
+                                MapTrace[j]->plan_speed=min((Curve_SmallSpeed+step_count*step_speed),straightCurveSpeed);
+                                step_count++;
+                            }
+                        }else if(mode0to5countSum>0){
+                            for(int j=i;j>i-mode0to5countSum;j--){
+                                MapTrace[j]->plan_speed=Curve_SmallSpeed;
+                                step_count++;
+                            }
+                        }else{
+                            MapTrace[i]->plan_speed=Curve_SmallSpeed;
+                        }
+                        mode0to5count=0;
+                        //mode0to18count++;
+                        mode18count++;
+                        mode18flash=mode18count;
                     }else if(MapTrace[i]->roadMode==14){
-                        MapTrace[i]->plan_speed=min(ServiceCarStatus.mroadmode_vel.mfmode14,8.0);
+                        mode0to18countSum=mode0to5flash+mode18flash;
+                        mode18countSum=mode18count;
+                        mode0to5count=0;
+                        mode18count=0;
+                        double brake_distanceLarge_double=(pow((Curve_SmallSpeed/3.6),2)-pow((Curve_LargeSpeed/3.6),2))*10/0.6;
+                        double brake_distance0to18_double=(pow((straightCurveSpeed/3.6),2)-pow((Curve_LargeSpeed/3.6),2))*10/0.6;
+                        int brake_distanceLarge=(int)brake_distanceLarge_double;
+                        int brake_distance0to18=(int)brake_distance0to18_double;
+                        int step_count=0;
+                        MapTrace[i]->plan_speed=Curve_LargeSpeed;
+
+                        if(mode18countSum>brake_distanceLarge)
+                        {
+                                double step_speed=(Curve_SmallSpeed-Curve_LargeSpeed)/brake_distanceLarge;
+
+                                for(int j=i;j>i-brake_distanceLarge;j--){
+                                    MapTrace[j]->plan_speed=min((Curve_LargeSpeed+step_count*step_speed),Curve_SmallSpeed);
+                                    step_count++;
+                                }
+
+                        }else if(mode0to18countSum>brake_distance0to18){
+
+                                double step_speed=(straightCurveSpeed-Curve_LargeSpeed)/brake_distanceLarge;
+
+                                for(int j=i;j>i-brake_distance0to18;j--){
+                                    MapTrace[j]->plan_speed=min((Curve_LargeSpeed+step_count*step_speed),straightCurveSpeed);
+                                    step_count++;
+                                }
+                        }else{
+                                MapTrace[i]->plan_speed=Curve_LargeSpeed;
+                        }
+
+
+
+
+                        mode0to5flash=0;
+                        mode18flash=0;
                     }
+                    //将数据写入文件开始
+                    ofstream outfile;
+                    outfile.open("ctr0108003.txt", ostream::app);        /*以添加模式打开文件*/
+                    outfile<<"MODE"<< ","  <<MapTrace[i]->plan_speed<< "," <<endl;
+                    outfile.close();                                 /*关闭文件*/
+                    //将数据写入文件结束
+
                 }
 
 }

src/decition/decition_brain_sf/decition/decide_gps_00.cpp

@@ -241,6 +241,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         if(ServiceCarStatus.speed_control==true){
             Compute00().getDesiredSpeed(gpsMapLine);   //add by zj
         }
+        Compute00().getPlanSpeed(gpsMapLine);
+
 
         //	ServiceCarStatus.carState = 1;
         //        roadOri = gpsMapLine[PathPoint]->roadOri;