add avoid decide

src/decition/decition_brain_sf/decition/decide_gps_00.cpp

@@ -167,6 +167,7 @@ double traceDevi;
 bool startingFlag = true;  //起步标志，在起步时需要进行frenet规划。
 bool useFrenet = false;    //标志是否启用frenet算法避障
 bool useOldAvoid = true;   //标志是否用老方法避障
+bool front_car_decide_avoid=true;  //前后车距大于30米，后车允许根据自己障碍物情况避障，否则只能听从前车
 
 std::vector<std::vector<double>> doubledata;//大地坐标系下x,y,phi,delta，速度规划用变量
 
@@ -221,10 +222,6 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         ServiceCarStatus.mMutexgroupgrpc.unlock();
         bgroupgrpc = true;
     }
-
-
-    xgroupgrpcinfo.mvehinfo_size();
-    xgroupgrpcinfo.mutable_mvehinfo(0)->intragroupid();
     if((bgroupgrpc==true)&&(ServiceCarStatus.curID>1)){
         for(int i=0;i<xgroupgrpcinfo.mvehinfo_size();i++){
             if(xgroupgrpcinfo.mutable_mvehinfo(i)->intragroupid()+1==ServiceCarStatus.curID){
@@ -242,6 +239,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
           front_car.front_car_ins.ins_heading_angle=xgroupgrpcinfo.mutable_mvehinfo(front_car_id)->mgpsimu().heading();
           GaussProjCal(front_car.front_car_ins.gps_lng,front_car.front_car_ins.gps_lat, &front_car.front_car_ins.gps_x, &front_car.front_car_ins.gps_y);
           front_car.front_car_dis=GetDistance(now_gps_ins,front_car.front_car_ins);
+       }else{
+          front_car.front_car_dis=500;
        }
        if(xgroupgrpcinfo.mutable_mvehinfo(front_car_id)->has_mcarstate()){
            front_car.vehState=xgroupgrpcinfo.mutable_mvehinfo(front_car_id)->mcarstate().mstate();
@@ -253,6 +252,18 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                        front_car_id,front_car.vehState,front_car.avoidX,front_car.front_car_dis,front_car.obs_distance);
     }
 
+    if(front_car_id>0){
+        if(front_car.front_car_dis>30){
+            front_car_decide_avoid=true;
+        }else if((front_car.front_car_dis<=30)&&(front_car.avoidX==0)){
+            front_car_decide_avoid=false;
+        }else if((front_car.front_car_dis<=30)&&(front_car.avoidX!=0)){
+            front_car_decide_avoid=true;
+        }
+    }else{
+        front_car_decide_avoid=true;
+    }
+
 
 
 
@@ -1810,7 +1821,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         cout<<"\n====================preAvoid time count finish==================\n"<<endl;
     }
 
-    if((obsDistance<0 ||obsDistance>ServiceCarStatus.aocfDis)&&(gpsMapLine[PathPoint]->mbnoavoid==true)){
+    if((obsDistance<0 ||obsDistance>ServiceCarStatus.aocfDis)||(gpsMapLine[PathPoint]->mbnoavoid==true)||(front_car_decide_avoid==false)){
         ObsTimeStart=-1;
         ObsTimeEnd=-1;
         ObsTimeSpan=-1;