Pārlūkot izejas kodu

fix(decition_gps_00):change avoiding logic

sunjiacheng 3 gadi atpakaļ
vecāks
revīzija
1d0708f18e

+ 11 - 3
src/decition/decition_brain_sf_jsrunlegs/decition/decide_gps_00.cpp

@@ -2449,7 +2449,11 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
               }
            }
            double dis1 =  GetDistance(now_gps_ins, *gpsMapLine[start_tracepoint]);
-           double dis2 =  GetDistance(now_gps_ins, *gpsMapLine[end_tracepoint]);
+           double dis2 = 10;// GetDistance(now_gps_ins, *gpsMapLine[end_tracepoint]);
+           if(brake_mode==true)
+           {
+               dis2=2;
+           }
            if(circleMode)//闭环地图
            {
                if(dis1<3&&dis2<3&&circleMode) //距离不能太小,太小了可能会错过,所以适当将距离增大一些
@@ -4565,19 +4569,23 @@ int iv::decition::DecideGps00::computeAvoidXDistance(iv::LidarGridPtr lidarGridP
     {
         hasCheckedAvoidLidar = true;
     }
-
+    int record_vector=0;
     for(int j=0;j<obs_property.size();j++){
         if(obs_property[j].obs_distance>100){
+            if((signed_record_avoidX<0)&&(obs_property[j].avoid_distance>0)){
+                break;
+            }
                 if(abs(obs_property[j].avoid_distance)<record_avoidX){
                         record_avoidX=abs(obs_property[j].avoid_distance);
                         signed_record_avoidX=obs_property[j].avoid_distance;
+                        record_vector=j;
                 }
         }
     }
 
    if((signed_record_avoidX<0)&&(signed_record_avoidX-1>=avoidLeft))
    {
-          int obs_test=abs(signed_record_avoidX)-1;
+          int obs_test=record_vector-1;
 
           if(obs_property[obs_test].obs_distance>100){
                     signed_record_avoidX-=1;