add speed plan pro

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

@@ -188,59 +188,63 @@ int iv::decition::Compute00::getDesiredSpeed(std::vector<GPSData> gpsMap)
     return 1;
 }
 
-
-int iv::decition::Compute00::getPlanSpeed(std::vector<GPSData> MapTrace)
-{
+int iv::decition::Compute00::getMapDelta(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);
+    //x,y,phi in rad
+    for (int i = 0; i < MapTrace.size(); i++) {   //                                       1225/14:25
                  std::vector<double> temp;
-                 temp.push_back(0);temp.push_back(0);temp.push_back(0);//先push三个空量，然后就可以给量赋值了
+                 temp.push_back(0);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;}
-        }
+                 doubledata[i][0] = MapTrace.at(i)->gps_x;
+                 doubledata[i][1] = MapTrace.at(i)->gps_y;
+                 doubledata[i][2] = (MapTrace.at(i)->ins_heading_angle) / 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///////////////////////////////////////////////////////////////
+    // compute 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];
+    {
+                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变号
+
+                MapTrace[i]->delta=doubledata[i][3];
+    }
+                MapTrace[MapTrace.size()-1]->delta = MapTrace[MapTrace.size()-2]->delta;
+}
+
+
+int iv::decition::Compute00::getPlanSpeed(std::vector<GPSData> MapTrace)
+{
+                getMapDelta(MapTrace);
+                for(int i=0;i<MapTrace.size();i++)
+                {
+                    if((MapTrace[i]->delta>-0.4)&&(MapTrace[i]->delta<0.4)){
+                        MapTrace[i]->roadMode=0;
+                    }else if(((MapTrace[i]->delta>0.4)&&(MapTrace[i]->delta<4))||((MapTrace[i]->delta>-4)&&(MapTrace[i]->delta<-0.4))){
+                        MapTrace[i]->roadMode=5;
+                    }else if(((MapTrace[i]->delta>4)&&(MapTrace[i]->delta<10))||((MapTrace[i]->delta>-10)&&(MapTrace[i]->delta<-4))){
+                        MapTrace[i]->roadMode=18;
+                    }else if(((MapTrace[i]->delta>10))||((MapTrace[i]->delta<-10))){
+                        MapTrace[i]->roadMode=14;
+                    }
+                }
+                for(int i=0;i<MapTrace.size();i++)
+                {
+                    if(MapTrace[i]->roadMode==0){
+                        MapTrace[i]->plan_speed=min(ServiceCarStatus.mroadmode_vel.mfmode0,60.0);
+                    }else if(MapTrace[i]->roadMode==5){
+                        MapTrace[i]->plan_speed=min(ServiceCarStatus.mroadmode_vel.mfmode5,40.0);
+                    }else if(MapTrace[i]->roadMode==18){
+                        MapTrace[i]->plan_speed=min(ServiceCarStatus.mroadmode_vel.mfmode18,20.0);
+                    }else if(MapTrace[i]->roadMode==14){
+                        MapTrace[i]->plan_speed=min(ServiceCarStatus.mroadmode_vel.mfmode14,8.0);
+                    }
+                }
+
 }
 
 

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

@@ -36,6 +36,7 @@ namespace iv {
 
             static int getDesiredSpeed(std::vector<GPSData> gpsMap);
             static int getPlanSpeed(std::vector<GPSData> MapTrace);
+            static int getMapDelta(std::vector<GPSData> MapTrace);
 
             static double getDecideAngle(std::vector<Point2D>  gpsTrace, double realSpeed);
             static double getAveDef(std::vector<Point2D> farTrace);