modify avoid pro back road

src/decition/decition_brain_sf/decition/decide_gps_00.cpp

@@ -174,7 +174,7 @@ std::vector<std::vector<double>> doubledata;//大地坐标系下x,y,phi,delta，
 
 enum VehState { normalRun, park, readyPark, stopObs, avoidObs, preAvoid, avoiding, backOri,preBack,
                 waitAvoid ,shoushaTest,justRun, changingRoad, chaoche, chaocheBack, reverseCar,reversing, reverseCircle, reverseDirect,reverseArr,
-                dRever,dRever0,dRever1,dRever2,dRever3,dRever4} vehState;
+                dRever,dRever0,dRever1,dRever2,dRever3,dRever4} vehState,lastVehState;
 
 
 std::vector<iv::Point2D>  gpsTraceAim; //记录变道后的目标路径，若车辆经frenet规划行驶至该路径上后，就关闭frenet规划
@@ -2298,6 +2298,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
 
     lastAngle=gps_decition->wheel_angle;
+    lastVehState=vehState;
 
     //   qDebug("decide1 time is %d",xTime.elapsed());
     return gps_decition;
@@ -4324,13 +4325,14 @@ void iv::decition::DecideGps00::computeObsOnRoadNew(iv::LidarGridPtr lidarGridPt
         obsva->obs_distance=500;
     }else{
         obsva->obs_distance=obs;
+        obsva->obs_speed=obsSd;
     }
 }
 
 int iv::decition::DecideGps00::computeBackDistance(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per,int avoidLeft,int avoidRight,int avoidXNow) {
 
     obs_property.clear();
-    double record_avoidX=20,signed_record_avoidX=0,obs_cur_distance=500;
+    double record_avoidX=20,signed_record_avoidX=0,obs_cur_distance=500,obs_cur_speed=0;
     for (int i =  avoidLeft; i <= avoidRight; i++)
     {
         obsvalue x_value;
@@ -4340,8 +4342,10 @@ int iv::decition::DecideGps00::computeBackDistance(iv::LidarGridPtr lidarGridPtr
         gpsTraceBack = getGpsTraceOffset(gpsTraceOri, i);
         computeObsOnRoadNew(lidarGridPtr, gpsTraceBack, i,gpsMapLine,lidar_per,x);
         obs_property.push_back(x_value);
-        if(i==0)
+        if(i==0){
             obs_cur_distance=x_value.obs_distance;
+            obs_cur_speed=x_value.obs_speed;
+        }
         givlog->debug("decition_brain","i: %d,obs:%f",x_value.avoid_distance,x_value.obs_distance);
 
     }
@@ -4366,7 +4370,17 @@ int iv::decition::DecideGps00::computeBackDistance(iv::LidarGridPtr lidarGridPtr
             }
         }
     }
-    if((obs_cur_distance>100)&&(GetDistance(now_gps_ins, startAvoidGps)>max(avoidMinDistance,40.0)))
+    /*if((obs_cur_distance>100)&&(GetDistance(now_gps_ins, startAvoidGps)>max(avoidMinDistance,40.0)))
+    {
+        if(PathPoint+300<gpsMapLine.size()){
+            for(int k=PathPoint;k<PathPoint+300;k++){
+                    if((gpsMapLine[k]->mfCurvature>0.02))
+                                     return 20;
+            }
+            return 0;
+        }
+    }*/
+    if((obs_cur_distance>100||obs_cur_speed>1)&&(lastVehState==normalRun))
     {
         if(PathPoint+300<gpsMapLine.size()){
             for(int k=PathPoint;k<PathPoint+300;k++){

src/decition/decition_brain_sf/decition/decide_gps_00.h

@@ -128,6 +128,7 @@ public:
     struct obsvalue{
         int avoid_distance;
         double obs_distance;
+        double obs_speed;
     };
     std::vector<obsvalue> obs_property;