modify road num

src/decition/decition_brain_sf/decition/decide_gps_00.cpp

@@ -190,6 +190,8 @@ std::vector<double> lastDistanceVector(roadSum, -1);
 std::vector<iv::Point2D> traceOriLeft,traceOriRight;
 
 bool qiedianCount = false;
+
+static int obstacle_avoid_flag=0;
 //日常展示
 
 iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_gps_ins,
@@ -274,7 +276,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         readyParkMode=false;
         finishPointNum=-1;
         roadNowStart=true;
-        isFirstRun = false;       
+        isFirstRun = false;
+        obstacle_avoid_flag=0;
     }
 
 
@@ -931,10 +934,10 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
     if (!isFirstRun)
     {
-        //   PathPoint = Compute00().getNearestPointIndex(now_gps_ins, gpsMapLine, DecideGps00::lastGpsIndex, DecideGps00::minDis, DecideGps00::maxAngle);
-        //    if(PathPoint<0){
-        PathPoint = Compute00().getFirstNearestPointIndex(now_gps_ins, gpsMapLine, DecideGps00::lastGpsIndex, DecideGps00::minDis, DecideGps00::maxAngle);
-        //    }
+            PathPoint = Compute00().getNearestPointIndex(now_gps_ins, gpsMapLine, DecideGps00::lastGpsIndex, DecideGps00::minDis, DecideGps00::maxAngle);
+            if(PathPoint<0){
+                PathPoint = Compute00().getFirstNearestPointIndex(now_gps_ins, gpsMapLine, DecideGps00::lastGpsIndex, DecideGps00::minDis, DecideGps00::maxAngle);
+            }
 
     }
 
@@ -1019,7 +1022,9 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
     if(!ServiceCarStatus.inRoadAvoid){
         roadOri = gpsMapLine[PathPoint]->roadOri;
-        roadSum = gpsMapLine[PathPoint]->roadSum;
+        roadSum = gpsMapLine[PathPoint]->roadSum;      
+        givlog->debug("decition_brain","roadOri: %d",
+                        roadOri);
     }else{
         roadOri=gpsMapLine[PathPoint]->roadOri*3+1;
         roadSum = gpsMapLine[PathPoint]->roadSum*3;
@@ -1039,9 +1044,15 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     }else{
          gpsMapLine[PathPoint]->runMode=0;
     }
-
-    avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
-
+    if(obstacle_avoid_flag==1){
+        avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
+    }else{
+        avoidX=0;
+        roadNow = roadOri;
+        if(vehState==backOri){
+            vehState=normalRun;
+        }
+    }
     if ( gpsMapLine[PathPoint]->runMode == 0 &&(vehState == avoidObs||vehState ==stopObs||vehState == preAvoid
                                                 ||vehState == avoiding||vehState == backOri||vehState ==preBack||vehState ==waitAvoid ) )
     {
@@ -1051,7 +1062,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
     gpsTraceOri= getGpsTrace(now_gps_ins, gpsMapLine, lastGpsIndex,circleMode);
 
-    if((vehState == avoiding || vehState == backOri)&&(gpsTraceAvoidXY.size()>0))
+    if((vehState == avoiding || vehState == backOri)&&(gpsTraceAvoidXY.size()>0)&&(obstacle_avoid_flag==1))
     {
         gpsTraceOri=getGpsTraceAvoid(now_gps_ins, gpsTraceAvoidXY, lastGpsIndex,circleMode);
     }
@@ -1520,6 +1531,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         else if (useOldAvoid && backDistance>35 ) {//abs(gpsTraceNow[60].x)<0.05
             // vehState = avoidObs; 0929
             vehState = normalRun;
+            obstacle_avoid_flag=0;
         }
         else if (turnLampFlag>0)
         {
@@ -1536,7 +1548,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     std::cout<<"\n原始RoadOri:%d\n"<<roadOri<<endl;
     //   计算回归原始路线
 
-    if (roadNow!=roadOri)
+    if ((roadNow!=roadOri))
     {
         if(useFrenet){
             if ((GetDistance(now_gps_ins, startAvoidGpsInsVector[roadOri])>max(avoidMinDistanceVector[roadOri],30.0)))
@@ -1552,7 +1564,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
              avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
         }
     }
-    if (roadNow != roadOri && roadPre!=-1)
+    if ((roadNow != roadOri && roadPre!=-1))
     {
 
         roadNow = roadPre;
@@ -1744,6 +1756,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
 
                 vehState = avoiding;
+                obstacle_avoid_flag=1;
 
                 hasCheckedAvoidLidar = false;
 
@@ -3812,8 +3825,8 @@ void iv::decition::DecideGps00::transferGpsMode2( const std::vector<GPSData> gps
         }
     }
 
-    givlog->debug("decition_brain","PATHFORE: %d,Forecast: %d,cross: %d",
-                PathPoint+forecast_point_num,forecast_point_num,cross);
+    //givlog->debug("decition_brain","PATHFORE: %d,Forecast: %d,cross: %d",
+                //PathPoint+forecast_point_num,forecast_point_num,cross);
     if(  gpsMapLine[PathPoint]->mode2==3000){
         if(obsDistance>4){       //7   zj-5
             obsDistance=200;