add speed plan pro

src/decition/common/common/gps_type.h

@@ -68,6 +68,9 @@ namespace iv {
         double gps_x_avoidright = 0;
         double gps_y_avoidright = 0;
 
+        double plan_speed;   //guihuasudu
+        double delta;        //qianlunzhuanjiao
+
 
 
     };

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

@@ -189,7 +189,59 @@ int iv::decition::Compute00::getDesiredSpeed(std::vector<GPSData> gpsMap)
 }
 
 
+int iv::decition::Compute00::getPlanSpeed(std::vector<GPSData> MapTrace)
+{
+    vector<vector<double>> doubledata;//大地坐标系下x,y,phi,delta
+    double ref_X, ref_Y;
+    double inter_x,inter_y,inter_heading;
+    double distance,deltaphi;
+
+    //插值函数
+    for (int i = 0; i < MapTrace.size()-1; i++) {   //                                       1225/14:25
+        for(int k = 0; k < 10; k++){
+               inter_x=(MapTrace.at(i+1)->gps_lng-MapTrace.at(i)->gps_lng)/10;
+               inter_y=(MapTrace.at(i+1)->gps_lat-MapTrace.at(i)->gps_lat)/10;
+
+               inter_heading =(MapTrace.at(i+1)->ins_heading_angle - MapTrace.at(i)->ins_heading_angle);
+                   if(inter_heading>180)
+                     {  inter_heading=inter_heading-360;}
+                   if(inter_heading<-180)
+                     {  inter_heading=inter_heading+360;}
+                inter_heading=inter_heading/10;
+               GaussProjCal(MapTrace.at(i)->gps_lng+inter_x*k, MapTrace.at(i)->gps_lat+inter_y*k, &ref_X, &ref_Y);
+                 std::vector<double> temp;
+                 temp.push_back(0);temp.push_back(0);temp.push_back(0);//先push三个空量，然后就可以给量赋值了
+                 doubledata.push_back(temp);
+                 doubledata[i*10+k][0] = ref_X;
+                 doubledata[i*10+k][1] = ref_Y;
+                 doubledata[i*10+k][2] = (MapTrace.at(i)->ins_heading_angle+inter_heading*k) / 180 * PI;
+                 if((MapTrace.at(i)->ins_heading_angle+inter_heading*k)>360)
+                   { doubledata[i*10+k][2] = (MapTrace.at(i)->ins_heading_angle+inter_heading*k-360) / 180 * PI;}
+                 if((MapTrace.at(i)->ins_heading_angle+inter_heading*k)<0)
+                   { doubledata[i*10+k][2] = (MapTrace.at(i)->ins_heading_angle+inter_heading*k+360) / 180 * PI;}
+        }
+    }
 
+    GaussProjCal(MapTrace.at(MapTrace.size()-1)->gps_lng, MapTrace.at(MapTrace.size()-1)->gps_lat, &ref_X, &ref_Y);
+    std::vector<double> temp;
+    temp.push_back(0);temp.push_back(0);temp.push_back(0);//先push三个空量，然后就可以给量赋值了
+    doubledata.push_back(temp);
+    doubledata[(MapTrace.size()-1)*10][0] = ref_X;
+    doubledata[(MapTrace.size()-1)*10][1] = ref_Y;
+    doubledata[(MapTrace.size()-1)*10][2] = (MapTrace.at(MapTrace.size()-1)->ins_heading_angle) / 180 * PI;
+// 计算delta///////////////////////////////////////////////////////////////
+    for (int i = 0; i < doubledata.size()-1; i++)
+     {
+        deltaphi=doubledata[i+1][2]-doubledata[i][2];
+          if (deltaphi>PI)
+             {deltaphi=deltaphi-2*PI; }
+          if (deltaphi<-PI)
+             {deltaphi=deltaphi+2*PI;}
+        distance=sqrt( pow((doubledata[i+1][0]-doubledata[i][0]),2)+pow((doubledata[i+1][1]-doubledata[i][1]),2));
+        doubledata[i][3]=-atan( 2.5* (deltaphi) / distance  );//车辆坐标系和惯导坐标系方向相反，delta变号
+     }
+    doubledata[doubledata.size()-1][3] = doubledata[doubledata.size()-2][3];
+}
 
 
 //首次找点

src/decition/decition_brain_sf/decition/adc_tools/compute_00.h

@@ -35,6 +35,7 @@ namespace iv {
             static	int getFirstNearestPointIndex(GPS_INS rp, std::vector<GPSData> gpsMap, int lastIndex, double mindis, double maxAngle);
 
             static int getDesiredSpeed(std::vector<GPSData> gpsMap);
+            static int getPlanSpeed(std::vector<GPSData> MapTrace);
 
             static double getDecideAngle(std::vector<Point2D>  gpsTrace, double realSpeed);
             static double getAveDef(std::vector<Point2D> farTrace);