add dongli xunluoche park logic for 3 files

src/decition/common/common/car_status.h

@@ -212,6 +212,22 @@ namespace iv {
         QMutex mMutexgroupgrpc;
         qint64 mgroupgrpcupdatetime = 0;
         int curID=0;
+        //巡逻车增加停车点位.begin.20211203,cxw
+        double mfParkLat0;
+        double mfParkLng0;
+        double mfParkLat1;
+        double mfParkLng1;
+        double mfParkLat2;
+        double mfParkLng2;
+        double mfParkLat3;
+        double mfParkLng3;
+        double mfParkLat4;
+        double mfParkLng4;
+        int mnparknum=0;  //停车点个数
+        bool mbparkEN =false; //停车功能是否打开
+        int mnparktime = 0;  //停车时间，单位分
+        std::vector<iv::GPS_INS> xlcParkPoint;//存放停车点经纬的vetor
+         //巡逻车增加停车点位.end
     };
     typedef boost::serialization::singleton<iv::CarStatus> CarStatusSingleton;//非线程安全，注意多线程加锁，单例模式
 }

src/decition/decition_brain_sf_midcar_xunluoche/decition/brain.cpp

@@ -458,6 +458,75 @@ void iv::decition::BrainDecition::run() {
         ServiceCarStatus.avoidObs = false;
     }
 
+    //巡逻车增加停车点,20211203,cxw
+    if(ServiceCarStatus.msysparam.mvehtype=="zhongche")
+    {
+        iv::GPS_INS parkpoint;
+        ServiceCarStatus.xlcParkPoint.clear();
+        std::string strxlcparklat = xp.GetParam("parklat0","0");
+        std::string strxlcparklng = xp.GetParam("parklng0","0");
+        ServiceCarStatus.mfParkLat0 = atof(strxlcparklat.data());
+        ServiceCarStatus.mfParkLng0 = atof(strxlcparklng.data());
+        parkpoint.gps_lat = ServiceCarStatus.mfParkLat0;
+        parkpoint.gps_lng = ServiceCarStatus.mfParkLng0;
+        ServiceCarStatus.xlcParkPoint.push_back(parkpoint);
+
+        strxlcparklat = xp.GetParam("parklat1","0");
+        strxlcparklng = xp.GetParam("parklng1","0");
+        ServiceCarStatus.mfParkLat1 = atof(strxlcparklat.data());
+        ServiceCarStatus.mfParkLng1 = atof(strxlcparklng.data());
+        parkpoint.gps_lat = ServiceCarStatus.mfParkLat1;
+        parkpoint.gps_lng = ServiceCarStatus.mfParkLng1;
+        ServiceCarStatus.xlcParkPoint.push_back(parkpoint);
+
+        strxlcparklat = xp.GetParam("parklat2","0");
+        strxlcparklng = xp.GetParam("parklng2","0");
+        ServiceCarStatus.mfParkLat2 = atof(strxlcparklat.data());
+        ServiceCarStatus.mfParkLng2 = atof(strxlcparklng.data());
+        parkpoint.gps_lat = ServiceCarStatus.mfParkLat2;
+        parkpoint.gps_lng = ServiceCarStatus.mfParkLng2;
+        ServiceCarStatus.xlcParkPoint.push_back(parkpoint);
+
+        strxlcparklat = xp.GetParam("parklat3","0");
+        strxlcparklng = xp.GetParam("parklng3","0");
+        ServiceCarStatus.mfParkLat3 = atof(strxlcparklat.data());
+        ServiceCarStatus.mfParkLng3 = atof(strxlcparklng.data());
+        parkpoint.gps_lat = ServiceCarStatus.mfParkLat3;
+        parkpoint.gps_lng = ServiceCarStatus.mfParkLng3;
+        ServiceCarStatus.xlcParkPoint.push_back(parkpoint);
+
+        strxlcparklat = xp.GetParam("parklat4","0");
+        strxlcparklng = xp.GetParam("parklng4","0");
+        ServiceCarStatus.mfParkLat4 = atof(strxlcparklat.data());
+        ServiceCarStatus.mfParkLng4 = atof(strxlcparklng.data());
+        parkpoint.gps_lat = ServiceCarStatus.mfParkLat4;
+        parkpoint.gps_lng = ServiceCarStatus.mfParkLng4;
+        ServiceCarStatus.xlcParkPoint.push_back(parkpoint);
+
+    //    strxlcparklat = xp.GetParam("parklat5","0");
+    //    strxlcparklng = xp.GetParam("parklng5","0");
+    //    ServiceCarStatus.mfParkLat5 = atof(strxlcparklat.data());
+    //    ServiceCarStatus.mfParkLng5 = atof(strxlcparklng.data());
+
+        std::string parknum=xp.GetParam("parknum","0");
+        ServiceCarStatus.mnparknum=atoi(parknum.data());
+
+         std::string parken =xp.GetParam("parken","false");
+        if(parken == "true")
+        {
+            ServiceCarStatus.mbparkEN = true;
+        }
+        else
+        {
+            ServiceCarStatus.mbparkEN = false;
+        }
+
+        std::string parktime = xp.GetParam("parktime","0");
+        ServiceCarStatus.mnparktime =atoi(parktime.data());
+    }
+
+//    //巡逻车临时停车点，end,20211203,cxw
+
     mstrmemmap_index = xp.GetParam("msg_mapindex","map_index");
 
 

src/decition/decition_brain_sf_midcar_xunluoche/decition/decide_gps_00.cpp

@@ -336,6 +336,30 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
 
         getV2XTrafficPositionVector(gpsMapLine);
+
+        //巡逻车停车点位逻辑,begin，20211203，cxw
+
+    //        if(ServiceCarStatus.mbparkEN==true &&ServiceCarStatus.mnparknum>0 &&ServiceCarStatus.mnparktime>0)
+    //        {
+
+    //        }
+    //        else
+    //        {
+    //          ServiceCarStatus.xlcParkPoint.clear();
+    //        }
+        if(ServiceCarStatus.mbparkEN==true && ServiceCarStatus.mnparknum!=0)
+        {
+
+            for(unsigned int i=0;i<ServiceCarStatus.xlcParkPoint.size();i++)
+            {
+                if(ServiceCarStatus.xlcParkPoint[i].gps_lng!=0.0 && ServiceCarStatus.xlcParkPoint[i].gps_lat!=0.0)
+                {
+                    GaussProjCal(ServiceCarStatus.xlcParkPoint[i].gps_lng,ServiceCarStatus.xlcParkPoint[i].gps_lat, &ServiceCarStatus.xlcParkPoint[i].gps_x, &ServiceCarStatus.xlcParkPoint[i].gps_y);
+
+                }
+            }
+        }
+        //巡逻车停车点位逻辑，end,20211203，cxw
         //        group_ori_gps=*gpsMapLine[0];
         ServiceCarStatus.bocheMode=false;
         ServiceCarStatus.daocheMode=false;
@@ -362,7 +386,6 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     }
 
 
-
     //1227
     //  ServiceCarStatus.daocheMode=true;
 
@@ -2268,6 +2291,50 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
 
     transferGpsMode2(gpsMapLine);
+    //巡逻车临时停车逻辑，begin，20211203，cxw
+    unsigned int parkpointnum=-1;
+    double min_distoparkpoint =8;
+    bool parktimeEN = false;
+    QTime parktimer;
+     static bool timer_en=true;
+    if((ServiceCarStatus.mbparkEN==true)&&(ServiceCarStatus.mnparknum!=0))
+    {
+        int timecnt =ServiceCarStatus.mnparktime*60*1000;
+        for(unsigned int i=0;i<ServiceCarStatus.xlcParkPoint.size();i++)
+        {
+           double dis= GetDistance(ServiceCarStatus.xlcParkPoint[i],now_gps_ins);
+           if(dis<=min_distoparkpoint)
+           {
+               min_distoparkpoint = dis;
+               parkpointnum = i;
+           }
+
+        }
+        givlog->debug("brain_decition","xlcparkpoingnum %d","min_distoparknum %f",
+                      parkpointnum,min_distoparkpoint);
+
+        if((parkpointnum!=-1)&&(min_distoparkpoint<5) && (parktimeEN ==false))
+        {
+           minDecelerate=-5;
+           if(timer_en==true)
+           {
+              parktimer.start();
+              timer_en = false;
+           }
+           if(parktimer.elapsed()>=timecnt)
+           {
+               minDecelerate=0.0;
+               parktimeEN =true;
+           }
+        }
+        if(min_distoparkpoint>6)
+        {
+            parktimeEN=false;
+            timer_en = true;
+        }
+    }
+
+    //巡逻车临时停车逻辑，end,20211203，cxw
 
     if(ServiceCarStatus.msysparam.mvehtype=="hapo"){
         if(obsDistance>0 && obsDistance<8){