modify speed control mode

src/decition/decition_brain_sf/decition/decide_gps_00.cpp

@@ -1066,7 +1066,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
             gpsTraceNowRight.assign(traceOriRight.begin(),traceOriRight.end());
         }else
         {
-            if(vehState == avoiding)
+            if((vehState == avoiding)||(vehState == backOri))
             {
                 gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
                 gpsTraceNowLeft=getGpsTraceOffset(traceOriLeft, avoidX);
@@ -1089,8 +1089,6 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         planTrace.push_back(pt);
     }
 
-    dSpeed = limitSpeed(controlAng, dSpeed);
-
     controlAng= getAngle(gpsTraceNow,now_gps_ins,gps_decition);
     if(!circleMode && PathPoint>(gpsMapLine.size()-250)){
         if(realspeed<0.5)
@@ -1109,12 +1107,12 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
         if(vehState==avoiding){
             ServiceCarStatus.msysparam.vehWidth=2.1;
-            controlAng=max(-150.0,controlAng);//35  zj-80
-            controlAng=min(150.0,controlAng);//35   zj-80
+            controlAng=max(-150.0,controlAng);//35  zj-150
+            controlAng=min(150.0,controlAng);//35   zj-150
         }
         if(vehState==backOri){
-            controlAng=max(-150.0,controlAng);//35   zj-80
-            controlAng=min(150.0,controlAng);//35     zj-80
+            controlAng=max(-150.0,controlAng);//35   zj-150
+            controlAng=min(150.0,controlAng);//35     zj-150
         }
     }
 
@@ -1238,92 +1236,72 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         }
     }
 
-
-
-    /*if (gpsMapLine[PathPoint]->roadMode ==0)
-    {
-        //dSpeed = min(10.0,dSpeed);
-
-        dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode0,dSpeed);
-        gps_decition->leftlamp = false;
-        gps_decition->rightlamp = false;
-
-    }else  if (gpsMapLine[PathPoint]->roadMode == 5)
+    if(ServiceCarStatus.speed_control==true){
+            dSpeed = min(doubledata[PathPoint][4],dSpeed);
+    }
+    else
     {
-        //dSpeed = min(8.0,dSpeed);
+            if (gpsMapLine[PathPoint]->roadMode ==0)
+            {
+                dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode0,dSpeed);
+                gps_decition->leftlamp = false;
+                gps_decition->rightlamp = false;
 
-        dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode5,dSpeed);
-        gps_decition->leftlamp = false;
-        gps_decition->rightlamp = false;
+            }else  if (gpsMapLine[PathPoint]->roadMode == 5)
+            {
+                dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode5,dSpeed);
+                gps_decition->leftlamp = false;
+                gps_decition->rightlamp = false;
 
-    }else if (gpsMapLine[PathPoint]->roadMode == 11)
-    {
-        dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode11,dSpeed);
+            }else if (gpsMapLine[PathPoint]->roadMode == 11)
+            {
+                dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode11,dSpeed);
 
-    }else if (gpsMapLine[PathPoint]->roadMode == 14)
-    {
-        //dSpeed = min(8.0,dSpeed);
+            }else if (gpsMapLine[PathPoint]->roadMode == 14)
+            {
+                dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode14,dSpeed);
+                gps_decition->leftlamp = true;
+                gps_decition->rightlamp = false;
+            }else if (gpsMapLine[PathPoint]->roadMode == 15)
+            {
+                dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode15,dSpeed);
+                gps_decition->leftlamp = false;
+                gps_decition->rightlamp = true;
+            }else if (gpsMapLine[PathPoint]->roadMode == 16)
+            {
+                dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode16,dSpeed);
+                gps_decition->leftlamp = true;
+                gps_decition->rightlamp = false;
+            }else if (gpsMapLine[PathPoint]->roadMode == 17)
+            {
+                dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode17,dSpeed);
+                gps_decition->leftlamp = false;
+                gps_decition->rightlamp = true;
+            }else if (gpsMapLine[PathPoint]->roadMode == 18)
+            {
+                dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode18,dSpeed);
+            }else if (gpsMapLine[PathPoint]->roadMode == 1)
+            {
+                dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode1,dSpeed);
+            }else if (gpsMapLine[PathPoint]->roadMode == 2)
+            {
+                dSpeed = min(15.0,dSpeed);
+            }
+            else if (gpsMapLine[PathPoint]->roadMode == 7)
+            {
+                dSpeed = min(15.0,dSpeed);
+                xiuzhengCs=1.5;
+            }else if (gpsMapLine[PathPoint]->roadMode == 4) //1220
+            {
+                dSpeed = min(4.0,dSpeed);
 
-        dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode14,dSpeed);
-        gps_decition->leftlamp = true;
-        gps_decition->rightlamp = false;
-    }else if (gpsMapLine[PathPoint]->roadMode == 15)
-    {
-        //dSpeed = min(8.0,dSpeed);
+            }else{
+                    dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode0,dSpeed);
+                    gps_decition->leftlamp = false;
+                    gps_decition->rightlamp = false;
 
-        dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode15,dSpeed);
-        gps_decition->leftlamp = false;
-        gps_decition->rightlamp = true;
-    }else if (gpsMapLine[PathPoint]->roadMode == 16)
-    {
-        // dSpeed = min(10.0,dSpeed);
-        dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode16,dSpeed);
-        gps_decition->leftlamp = true;
-        gps_decition->rightlamp = false;
-    }else if (gpsMapLine[PathPoint]->roadMode == 17)
-    {
-        //  dSpeed = min(10.0,dSpeed);
-        dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode17,dSpeed);
-        gps_decition->leftlamp = false;
-        gps_decition->rightlamp = true;
-    }else if (gpsMapLine[PathPoint]->roadMode == 18)
-    {
-        // 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;
-//        }
-    }else if (gpsMapLine[PathPoint]->roadMode == 1)
-    {
-        dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode1,dSpeed);
-    }else if (gpsMapLine[PathPoint]->roadMode == 2)
-    {
-        dSpeed = min(15.0,dSpeed);
+            }
     }
-    else if (gpsMapLine[PathPoint]->roadMode == 7)
-    {
-        dSpeed = min(15.0,dSpeed);
-        xiuzhengCs=1.5;
-    }else if (gpsMapLine[PathPoint]->roadMode == 4) //1220
-    {
-        dSpeed = min(4.0,dSpeed);
-
-    }else{
-            dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode0,dSpeed);
-            gps_decition->leftlamp = false;
-            gps_decition->rightlamp = false;
-
-    }*/
-
-
-    dSpeed = min(doubledata[PathPoint][4],dSpeed);
 
     if (gpsMapLine[PathPoint]->speed_mode == 2)
     {
@@ -1342,14 +1320,12 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         if((gpsMapLine[PathPoint]->speed)>4.5)
         {
             dSpeed =gpsMapLine[PathPoint]->speed*3.6;
-            ServiceCarStatus.msysparam.vehWidth=2.1;
         }
-
-
     }
 
-    dSecSpeed = dSpeed / 3.6;
 
+    dSpeed = limitSpeed(controlAng, dSpeed);
+    dSecSpeed = dSpeed / 3.6;
     givlog->debug("brain_decition_speed","dspeed: %f",
                   dSpeed);
 
@@ -1366,10 +1342,6 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         }
     }
 
-    //    qDebug("decide0 time is %d",xTime.elapsed());
-
-    //1220
-
     static int obs_filter=0,obs_filter_flag=0,obs_filter_dis_memory=0;
     static double obs_speed_for_avoid=-1,obs_filter_speed_memory=0;
     if(!ServiceCarStatus.daocheMode){
@@ -2764,22 +2736,14 @@ void iv::decition::DecideGps00::computeObsOnRoad(iv::LidarGridPtr lidarGridPtr,
         lastLidarDis = lidarDistance;
     }
 
-
-
-
-
-
     if(lidarDistance<0){
         lidarDistance=500;
     }
 
-
-
     std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "激光雷达距离:" << lidarDistance << std::endl;
 
     ServiceCarStatus.mLidarObs = lidarDistance;
     obs = lidarDistance;
-
     obsSd= obsPoint.obs_speed_y;
 
     if(ServiceCarStatus.useLidarPerPredict){
@@ -3072,7 +3036,7 @@ int iv::decition::DecideGps00::chooseAvoidRoad(iv::LidarGridPtr lidarGridPtr, GP
                     avoidRunDistance = obsDisVector[roadNow] + ServiceCarStatus.msysparam.vehLenth;
                     startAvoid_gps_ins = now_gps_ins;
                 }	*/
-                avoidMinDistanceVector[roadNow]= obsDisVector[roadNow] + ServiceCarStatus.msysparam.vehLenth;
+                avoidMinDistanceVector[roadNow]= obsDisVector[roadNow] + 2*ServiceCarStatus.msysparam.vehLenth;
                 startAvoidGpsInsVector[roadNow] = now_gps_ins;
                 roadPre = roadNow + i;
                 return roadPre;
@@ -3090,7 +3054,7 @@ int iv::decition::DecideGps00::chooseAvoidRoad(iv::LidarGridPtr lidarGridPtr, GP
                     avoidRunDistance = obsDisVector[roadNow] + ServiceCarStatus.msysparam.vehLenth;
                     startAvoid_gps_ins = now_gps_ins;
                 }*/
-                avoidMinDistanceVector[roadNow] = obsDisVector[roadNow] + ServiceCarStatus.msysparam.vehLenth;
+                avoidMinDistanceVector[roadNow] = obsDisVector[roadNow] + 2*ServiceCarStatus.msysparam.vehLenth;
                 startAvoidGpsInsVector[roadNow] = now_gps_ins;
                 roadPre = roadNow - i;
                 return roadPre;