add avoid reverse road veh

src/decition/decition_brain_sf/decition/adc_adapter/hapo_adapter.cpp

@@ -136,16 +136,6 @@ iv::decition::Decition iv::decition::HapoAdapter::getAdapterDeciton(GPS_INS now_
             controlSpeed=ServiceCarStatus.torque_st+2;
         }
 
-//        if(controlSpeed==2.0 && ServiceCarStatus.torque_st<2){
-//          controlSpeed=2.4;
-//        }
-        //seizing end
-
-
-
-
-
-
         // 斜坡加大油门   0217 lsn
 
         if(((now_gps_ins.ins_pitch_angle<-2.5 && ServiceCarStatus.daocheMode)||
@@ -168,19 +158,10 @@ iv::decition::Decition iv::decition::HapoAdapter::getAdapterDeciton(GPS_INS now_
     if(controlSpeed>0){
         controlSpeed=max(controlSpeed,4.6f);
     }
-
-
-
-
-
-
-
     //0227 10m nei xianzhi shache
     if(obsDistance<10 &&obsDistance>0){
         controlSpeed=0;
         controlBrake=max(controlBrake,0.6f);//0.8   zj-0.6
-       // controlBrake=min(controlBrake,0.8f);
-
     }
 
     if(DecideGps00::minDecelerate<0){
@@ -192,24 +173,29 @@ iv::decition::Decition iv::decition::HapoAdapter::getAdapterDeciton(GPS_INS now_
         controlBrake =0.4;
     }
 
-
     controlBrake=limitBrake(realSpeed,controlBrake,dSpeed,obsDistance,ttc);
-
     controlSpeed = limitSpeed(realSpeed, controlBrake, dSpeed, controlSpeed);
 
-   // (*decition)->brake = controlBrake*10;
-      (*decition)->brake = controlBrake*9;//9     zj-6
+    double reverse_speed=-(realSpeed+8)/3.6;//avoid veh in the reverse road
+    if((obsDistance>8.0)&&(obsSpeed<reverse_speed)){
+        controlBrake =0;
+        controlSpeed =0;
+    }
 
-    (*decition)->torque=controlSpeed;
-      lastBrake= (*decition)->brake;
 
+    (*decition)->brake = controlBrake*9;//9     zj-6
+    (*decition)->torque=controlSpeed;
+    lastBrake= (*decition)->brake;
+    lastTorque=(*decition)->torque;
 
-      //givlog->debug("brain_decition","brake: %f, torque: %f,obsDistance: %f",
-                    //(*decition)->brake,(*decition)->torque,obsDistance);
 
+//    if((*decition)->brake>0)
+//    {
 
-    lastTorque=(*decition)->torque;
+//    givlog->debug("brain_decition","brake: %f,obsSpeed: %f,reverse_speed: %f",
+//                            (*decition)->brake,obsSpeed,reverse_speed);
 
+//    }
 
 
 

src/decition/decition_brain_sf/decition/decide_gps_00.cpp

@@ -1416,11 +1416,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     if(!ServiceCarStatus.daocheMode){
         computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
 
-
         //givlog->debug("brain_decition","obsDistance: %f,obsSpeed: %f",
                       //obsDistance,obsSpeed);
-
-
         if(obs_filter_flag==0)
         {
             if(obsDistance>0&&obsDistance<60)
@@ -1464,10 +1461,11 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
             }
         }
 
+
+
+
         obs_speed_for_avoid=secSpeed+obsSpeed;
 
-        //givlog->debug("brain_decition","obsDistance: %f,obsSpeedforAvoid: %f",
-                      //obsDistance,obs_speed_for_avoid);
 
     }else{
         gpsTraceRear.clear();
@@ -1480,10 +1478,6 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
         //  obsDistance=-1; //1227
     }
-
-
-
-
     //old_bz--------------------------------------------------------------------------------------------------
 
     if (vehState == avoiding)
@@ -1553,17 +1547,6 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     }
 
     std::cout<<"\n原始RoadOri:%d\n"<<roadOri<<endl;
-
-    cout<<"\n当前RoadNum:%d\n"<<roadNow<<endl;
-
-
-
-
-
-
-
-
-
     //   计算回归原始路线
 
     if (roadNow!=roadOri)
@@ -1607,42 +1590,11 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
     }
 
-
-    static int obstacle_flag=0,obs_times=0;
-    static double  obs_memory=obsDistance;
-    /*if(obstacle_flag==1)
-    {
-              if((obsDistance<0)|| (obsDistance>ServiceCarStatus.aocfDis+15))
-              {
-                  obsDistance=obs_memory;
-                  obs_times++;
-                  if(obs_times>300)
-                  {
-                      obs_times=0;
-                      obstacle_flag=0;
-                  }
-              }
-
-    }*/
     static bool avoid_speed_flag=false;
     if(obsDistance>0&& (obsDistance<ServiceCarStatus.aocfDis+10)&&(abs(realspeed)>0.5)&&
             (vehState==normalRun) &&(ObsTimeStart==-1)//&&(obs_speed_for_avoid<0.6)
             && (gpsMapLine[PathPoint]->runMode==1)&&(gpsMapLine[PathPoint]->mode2!=3000))
     {
-
-        /*if(obsDistance<ServiceCarStatus.aocfDis+5)
-        {
-            dSpeed = min(6.0,dSpeed);
-        }else if(obsDistance<ServiceCarStatus.aocfDis+8){
-            dSpeed = min(8.0 ,dSpeed);
-        }else if(obsDistance<ServiceCarStatus.aocfDis+11){
-            dSpeed = min(10.0 ,dSpeed);
-        }else if(obsDistance<ServiceCarStatus.aocfDis+13){
-            dSpeed = min(12.0 ,dSpeed);
-        }else{
-            dSpeed = min(14.0 ,dSpeed);
-        }*/
-
         minDecelerate=-0.4;
         avoid_speed_flag=true;
     }