modfify speed control

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

@@ -192,6 +192,7 @@ int iv::decition::Compute00::getDesiredSpeed(std::vector<GPSData> gpsMap)
 int iv::decition::Compute00::getMapDelta(std::vector<GPSData> MapTrace){
     double distance,deltaphi;
     //x,y,phi in rad
+    doubledata.clear();
     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);temp.push_back(0);temp.push_back(0);//先push四个空量，然后就可以给量赋值了
@@ -199,22 +200,24 @@ int iv::decition::Compute00::getMapDelta(std::vector<GPSData> MapTrace){
                  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;
+                 doubledata[i][3]=0;
+                 doubledata[i][4]=0;
     }
     // compute delta///////////////////////////////////////////////////////////////
-    for (int i = 0; i < doubledata.size()-1; i++)
+    for (int i = 0; i < doubledata.size()-10; i++)
     {
-                deltaphi=doubledata[i+1][2]-doubledata[i][2];
+                deltaphi=doubledata[i+10][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));
+                distance=sqrt( pow((doubledata[i+10][0]-doubledata[i][0]),2)+pow((doubledata[i+10][1]-doubledata[i][1]),2));
                 doubledata[i][3]=-atan( 4.0* (deltaphi) / distance  );//车辆坐标系和惯导坐标系方向相反，delta变号
     }
                 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++)
@@ -222,7 +225,7 @@ int iv::decition::Compute00::getMapDelta(std::vector<GPSData> MapTrace){
             delta_sum+= doubledata[k][3];
         }
         doubledata[j][3]=delta_sum/20;
-    }
+    }*/
 }
 
 
@@ -234,7 +237,7 @@ int iv::decition::Compute00::getPlanSpeed(std::vector<GPSData> MapTrace)
                 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<doubledata.size();i++)
+                /*for(int i=0;i<doubledata.size();i++)
                 {
                     if((doubledata[i][3]>-0.4)&&(doubledata[i][3]<0.4)){
                         MapTrace[i]->roadMode=0;
@@ -245,6 +248,18 @@ int iv::decition::Compute00::getPlanSpeed(std::vector<GPSData> MapTrace)
                     }else if(((doubledata[i][3]>10))||((doubledata[i][3]<-10))){
                         MapTrace[i]->roadMode=14;
                     }
+                }*/
+                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.4))||((doubledata[i][3]>-0.4)&&(doubledata[i][3]<-0.2))){
+                        MapTrace[i]->roadMode=5;
+                    }else if(((doubledata[i][3]>0.4)&&(doubledata[i][3]<1))||((doubledata[i][3]>-1)&&(doubledata[i][3]<-0.4))){
+                        MapTrace[i]->roadMode=18;
+                    }else if(((doubledata[i][3]>1))||((doubledata[i][3]<-1))){
+                        MapTrace[i]->roadMode=14;
+                    }
                 }
                 for(int i=0;i<MapTrace.size();i++)
                 {
@@ -328,10 +343,10 @@ 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.open("ctr0108003.txt", ostream::app);
                     outfile<<"Delta"<< ","  <<doubledata[i][3]<< ","<<"roadMode"<< ","  <<MapTrace[i]->roadMode<< ","
                            <<"plan_speed"<< ","  << doubledata[i][4]<< ","<<endl;
-                    outfile.close();                                 /*关闭文件*/
+                    outfile.close();
                     //将数据写入文件结束
                 }
 

src/decition/decition_brain_sf/decition/decide_gps_00.cpp

@@ -241,7 +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().getDesiredSpeed(gpsMapLine);   //add by zj
+            Compute00().getPlanSpeed(gpsMapLine);
         }
 
         //	ServiceCarStatus.carState = 1;
@@ -967,8 +968,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     DecideGps00::lastGpsIndex = PathPoint;
     //2020-01-03, kkcai
     //if(!circleMode && PathPoint>gpsMapLine.size()-200){
-    if(!circleMode && PathPoint>gpsMapLine.size()-100){
-        minDecelerate=-0.7;
+    if(!circleMode && PathPoint>gpsMapLine.size()-250){
+        minDecelerate=-0.5;
         std::cout<<"map finish brake"<<std::endl;
     }
 
@@ -1109,8 +1110,9 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
 
     controlAng= getAngle(gpsTraceNow,now_gps_ins,gps_decition);
-    if(!circleMode && PathPoint>(gpsMapLine.size()-100)){
-        controlAng=0;
+    if(!circleMode && PathPoint>(gpsMapLine.size()-250)){
+        if(realspeed<0.5)
+            controlAng=0;
     }
 
 
@@ -1265,7 +1267,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
 
 
-    if (gpsMapLine[PathPoint]->roadMode ==0)
+    /*if (gpsMapLine[PathPoint]->roadMode ==0)
     {
         //dSpeed = min(10.0,dSpeed);
 
@@ -1315,16 +1317,16 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     {
         // dSpeed = min(10.0,dSpeed);
         dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode18,dSpeed);
-        /*if(gpsMapLine[PathPoint]->light_left_or_right == 1)
-        {
-            gps_decition->leftlamp = true;
-            gps_decition->rightlamp = false;
-        }
-        else if(gpsMapLine[PathPoint]->light_left_or_right == 2)
-        {
-            gps_decition->leftlamp = false;
-            gps_decition->rightlamp = true;
-        }*/
+//        if(gpsMapLine[PathPoint]->light_left_or_right == 1)
+//        {
+//            gps_decition->leftlamp = true;
+//            gps_decition->rightlamp = false;
+//        }
+//        else if(gpsMapLine[PathPoint]->light_left_or_right == 2)
+//        {
+//            gps_decition->leftlamp = false;
+//            gps_decition->rightlamp = true;
+//        }
     }else if (gpsMapLine[PathPoint]->roadMode == 1)
     {
         dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode1,dSpeed);
@@ -1345,11 +1347,13 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
             gps_decition->leftlamp = false;
             gps_decition->rightlamp = false;
 
-    }
-
+    }*/
 
 
+    dSpeed = min(doubledata[PathPoint][4],dSpeed);
 
+    givlog->debug("brain_decition_speed","dspeed: %f",
+                  dSpeed);
 
     if (gpsMapLine[PathPoint]->speed_mode == 2)
     {
@@ -1616,7 +1620,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     }*/
     static bool avoid_speed_flag=false;
     if(obsDistance>0&& (obsDistance<ServiceCarStatus.aocfDis+10)&&(abs(realspeed)>0.5)&&
-            (vehState==normalRun) &&(ObsTimeStart==-1)&&(obs_speed_for_avoid<0.6)
+            (vehState==normalRun) &&(ObsTimeStart==-1)//&&(obs_speed_for_avoid<0.6)
             && (gpsMapLine[PathPoint]->runMode==1)&&(gpsMapLine[PathPoint]->mode2!=3000))
     {
 
@@ -1639,7 +1643,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
 
     //shiyanbizhang 0929
-    if(obsDistance>0&& (obsDistance<ServiceCarStatus.aocfDis)&&(obs_speed_for_avoid<0.6)&& (avoid_speed_flag==true)        //&&(abs(realspeed)<5.0)
+    if(obsDistance>0&& (obsDistance<ServiceCarStatus.aocfDis)&& (avoid_speed_flag==true)        //&&(obs_speed_for_avoid<0.6)
             &&(vehState==normalRun||vehState==backOri || vehState==avoiding) &&(ObsTimeStart==-1)
             && (gpsMapLine[PathPoint]->runMode==1)&&(gpsMapLine[PathPoint]->mode2!=3000)){
         ObsTimeStart=GetTickCount();