add front car avoid decide

src/decition/common/common/car_status.h

@@ -211,6 +211,7 @@ namespace iv {
         iv::group::groupinfo mgroupgrpcinfo;
         QMutex mMutexgroupgrpc;
         qint64 mgroupgrpcupdatetime = 0;
+        int curID=0;
     };
     typedef boost::serialization::singleton<iv::CarStatus> CarStatusSingleton;//非线程安全，注意多线程加锁，单例模式
 }

src/decition/decition_brain_sf/decition/decide_gps_00.cpp

@@ -193,7 +193,7 @@ std::vector<iv::Point2D> traceOriLeft,traceOriRight;
 bool qiedianCount = false;
 
 static int obstacle_avoid_flag=0;
-
+static int front_car_id=-1;
 
 //日常展示
 
@@ -223,6 +223,39 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     }
 
 
+    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){
+                        front_car_id=i;
+            }
+        }
+    }else{
+                        front_car_id=-1;
+    }
+
+    if(front_car_id>0){
+       if(xgroupgrpcinfo.mutable_mvehinfo(front_car_id)->has_mgpsimu()){
+          front_car.front_car_ins.gps_lat=xgroupgrpcinfo.mutable_mvehinfo(front_car_id)->mgpsimu().lat();
+          front_car.front_car_ins.gps_lng=xgroupgrpcinfo.mutable_mvehinfo(front_car_id)->mgpsimu().lon();
+          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);
+       }
+       if(xgroupgrpcinfo.mutable_mvehinfo(front_car_id)->has_mcarstate()){
+           front_car.vehState=xgroupgrpcinfo.mutable_mvehinfo(front_car_id)->mcarstate().mstate();
+           front_car.avoidX=xgroupgrpcinfo.mutable_mvehinfo(front_car_id)->mcarstate().mavoidx();
+           front_car.obs_distance=xgroupgrpcinfo.mutable_mvehinfo(front_car_id)->mcarstate().mobsdistance();
+           front_car.obs_speed=xgroupgrpcinfo.mutable_mvehinfo(front_car_id)->mcarstate().mobsspeed();
+       }
+       givlog->debug("decition_brain","front_car_id: %d,FrontState: %d,FrontAvoidX: %d,FrontDis: %f,FrontObs: %f",
+                       front_car_id,front_car.vehState,front_car.avoidX,front_car.front_car_dis,front_car.obs_distance);
+    }
+
+
+
+
     //如果useFrenet、useOldAvoid两者不互为相异，则采用原来的方法。“^”为异或运算符。
     if(!(useFrenet^useOldAvoid)){
         useFrenet = false;
@@ -349,8 +382,6 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     traffic_light_gps.gps_lng=ServiceCarStatus.iTrafficeLightLon;
     GaussProjCal(traffic_light_gps.gps_lng,traffic_light_gps.gps_lat, &traffic_light_gps.gps_x, &traffic_light_gps.gps_y);
 
-    GaussProjCal(front_car_gps.gps_lng,front_car_gps.gps_lat, &front_car_gps.gps_x, &front_car_gps.gps_y);
-    front_car_dis=GetDistance(now_gps_ins, front_car_gps);
 
     //xiesi
     ///kkcai, 2020-01-03

src/decition/decition_brain_sf/decition/decide_gps_00.h

@@ -119,9 +119,9 @@ public:
     static int PathPoint;
     float lastGroupOffsetX=0.0;
 
-    GPS_INS traffic_light_gps,front_car_gps;
+    GPS_INS traffic_light_gps;
     int traffic_light_pathpoint;
-    double front_car_dis;
+
 
     bool changingDangWei=false;
 
@@ -131,6 +131,15 @@ public:
     };
     std::vector<obsvalue> obs_property;
 
+    struct front{
+        GPS_INS front_car_ins;
+        int vehState;
+        int avoidX;
+        double obs_distance;
+        double obs_speed;
+        double front_car_dis;
+    };
+    front front_car;
 
 
     BaseController *pid_Controller;