modify avoid pro

src/decition/common/common/car_status.h

@@ -156,7 +156,7 @@ namespace iv {
         int vehGroupId;
         double mfEPSOff = 0.0;
         float socfDis=15;   //停障触发距离
-        float aocfDis=25;   //避障触发距离
+        float aocfDis=20;   //避障触发距离
         float aocfTimes=3; //避障触发次数
         float aobzDis=5;   //避障保障距离
         /// traffice light : 0x90

src/decition/decition_brain_sf/decition/decide_gps_00.cpp

@@ -1360,7 +1360,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     }
 
     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;
+    static double obs_speed_for_avoid=-1,obs_filter_speed_memory=0;   //obs_speed_for_avoid: obstacle actual speed in km/h
     if(!ServiceCarStatus.daocheMode){
         //computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
         computeObsOnRoadXY(lidarGridPtr, gpsTraceNow,gpsTraceNowLeft,gpsTraceNowRight,roadNow,gpsMapLine,lidar_per);
@@ -1410,7 +1410,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         }
 
 
-        obs_speed_for_avoid=secSpeed+obsSpeed;
+        obs_speed_for_avoid=(secSpeed+obsSpeed)*3.6;
 
 
     }else{
@@ -1537,20 +1537,39 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
 
     //shiyanbizhang 0929
-    if(obsDistance>0&& (obsDistance<ServiceCarStatus.aocfDis)//&& (avoid_speed_flag==true)        //&&(obs_speed_for_avoid<0.6)
+    if(obsDistance>0&& (obsDistance<ServiceCarStatus.aocfDis)&&(obs_speed_for_avoid>-8.0)&&(obs_speed_for_avoid<3.0)//&& (avoid_speed_flag==true)        //
             &&(vehState==normalRun||vehState==backOri || vehState==avoiding) &&(ObsTimeStart==-1)
-            && (gpsMapLine[PathPoint]->runMode==1)&&(gpsMapLine[PathPoint]->mode2!=3000)){
+            && (gpsMapLine[PathPoint]->runMode==1)&&(gpsMapLine[PathPoint]->mode2!=3000)&&(gpsMapLine[PathPoint+300]->mode2!=3000)){
         ObsTimeStart=GetTickCount();
         //dSpeed = min(6.0,dSpeed);
         cout<<"\n====================preAvoid time count start==================\n"<<endl;
     }
-    if(ObsTimeStart!=-1){
-        ObsTimeEnd=GetTickCount();
-    }
-    if(ObsTimeEnd!=-1){
-        ObsTimeSpan=ObsTimeEnd-ObsTimeStart;
+//    if(ObsTimeStart!=-1){
+//        ObsTimeEnd=GetTickCount();
+//    }
+//    if(ObsTimeEnd!=-1){
+//        ObsTimeSpan=ObsTimeEnd-ObsTimeStart;
+//    }
+
+
+
+//    if(ObsTimeSpan>ObsTimeWidth &&(obsDistance>1.5||obsDistance<0) && (gpsMapLine[PathPoint]->runMode==1)){ //1026
+//        vehState=avoidObs;
+//        ObsTimeStart=-1;
+//        ObsTimeEnd=-1;
+//        ObsTimeSpan=-1;
+//        avoid_speed_flag=false;
+//        cout<<"\n====================preAvoid time count finish==================\n"<<endl;
+//    }
+
+
+
+
+    if((ObsTimeStart!=-1)&&(obs_speed_for_avoid>-8.0)&&(obs_speed_for_avoid<3.0)){
+            ObsTimeEnd+=1.0;
     }
-    if(ObsTimeSpan>ObsTimeWidth &&(obsDistance>1.5||obsDistance<0) && (gpsMapLine[PathPoint]->runMode==1)){ //1026
+
+    if(ObsTimeEnd>ObsTimeWidth &&(obsDistance>1.5||obsDistance<0) && (gpsMapLine[PathPoint]->runMode==1)){ //1026
         vehState=avoidObs;
         ObsTimeStart=-1;
         ObsTimeEnd=-1;
@@ -1566,10 +1585,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         avoid_speed_flag=false;
     }
 
-
     //避障模式
-
-
     if (vehState == avoidObs   || vehState==waitAvoid ) {
 
         //      if (obsDistance <20 && obsDistance >0)
@@ -1911,8 +1927,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     phaseSpeedDecition(gps_decition, secSpeed, obsDistance, obsSpeed,now_gps_ins);
 
 
-    givlog->debug("decition_brain","vehState: %d,PathPoint: %d,dSpeed: %f,obsDistance: %f,obsSpeed: %f,realspeed: %f,minDecelerate: %f,torque: %f,brake: %f,wheel_angle: %f",
-            vehState,PathPoint,dSpeed,obsDistance,obsSpeed,realspeed,minDecelerate,gps_decition->torque,gps_decition->brake,gps_decition->wheel_angle);
+    givlog->debug("decition_brain","vehState: %d,PathPoint: %d,dSpeed: %f,obsDistance: %f,obsSpeed: %f,realspeed: %f,minDecelerate: %f,torque: %f,brake: %f,wheel_angle: %f,obs_speed_for_avoid: %f",
+            vehState,PathPoint,dSpeed,obsDistance,obsSpeed,realspeed,minDecelerate,gps_decition->torque,gps_decition->brake,gps_decition->wheel_angle,obs_speed_for_avoid);
 
     if(ServiceCarStatus.txt_log_on==true){
         QDateTime dt = QDateTime::currentDateTime();

src/decition/decition_brain_sf/decition/decide_gps_00.h

@@ -229,7 +229,7 @@ public:
     float  ObsTimeSpan=-1;
     double ObsTimeStart=-1;
     double ObsTimeEnd=-1;
-    float ObsTimeWidth=500;
+    float ObsTimeWidth=30.0;   //500   zj-30
     std::vector<iv::TracePoint> planTrace;
 
     bool roadNowStart=true;