add function that aavoid in road

src/decition/common/common/car_status.h

@@ -186,6 +186,7 @@ namespace iv {
         uint32_t ultraDistance[13];
         bool useObsPredict = false;
         bool useLidarPerPredict = false;
+        bool inRoadAvoid = false;
         //hapo station 2021/02/05
         iv::StationCmd   stationCmd;
     };

src/decition/decition_brain/decition/brain.cpp

@@ -400,6 +400,15 @@ void iv::decition::BrainDecition::run() {
     {
         ServiceCarStatus.useObsPredict = true;
     }
+
+    std::string inRoadAvoid = xp.GetParam("inRoadAvoid","true");  //If Use MPC set true
+    if(inRoadAvoid == "true")
+    {
+        ServiceCarStatus.inRoadAvoid = true;
+    }else{
+        ServiceCarStatus.inRoadAvoid = false;
+    }
+
     //map
 
     mstrmemmap_index = xp.GetParam("msg_mapindex","map_index");

src/decition/decition_brain/decition/decide_gps_00.cpp

@@ -215,9 +215,9 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
 
 
-//    GPS_INS gps= Coordinate_UnTransfer(0,1.5,now_gps_ins);
-//    now_gps_ins.gps_x=gps.gps_x;
-//    now_gps_ins.gps_y=gps.gps_y;
+    //    GPS_INS gps= Coordinate_UnTransfer(0,1.5,now_gps_ins);
+    //    now_gps_ins.gps_x=gps.gps_x;
+    //    now_gps_ins.gps_y=gps.gps_y;
 
     //    GaussProjInvCal(now_gps_ins.gps_x, now_gps_ins.gps_y,&now_gps_ins.gps_lng, &now_gps_ins.gps_lat);
 
@@ -709,9 +709,9 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
             Point2D ptt = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
             double xxx=ptt.x;
             double yyy =ptt.y;
-//            if(xxx<-1.5||xx>1){
-//                int a=0;
-//            }
+            //            if(xxx<-1.5||xx>1){
+            //                int a=0;
+            //            }
             vehState = dRever3;
             qiedianCount = true;
             cout<<"到达远切点+++++++++++++++++++++++++++++++++"<<endl;
@@ -921,7 +921,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
 
 
-    avoidX = (roadOri-roadNow)*roadWidth;
+
 
     gpsTraceOri.clear();
     gpsTraceNow.clear();
@@ -955,6 +955,14 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     }
 
     DecideGps00::lastGpsIndex = PathPoint;
+
+
+
+
+
+
+
+
     //2020-01-03, kkcai
     //if(!circleMode && PathPoint>gpsMapLine.size()-200){
     if(!circleMode && PathPoint>gpsMapLine.size()-100){
@@ -964,10 +972,19 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
 
 
+    if(!ServiceCarStatus.inRoadAvoid){
+        roadOri = gpsMapLine[PathPoint]->roadOri;
+        roadSum = gpsMapLine[PathPoint]->roadSum;
+    }else{
+        roadOri=gpsMapLine[PathPoint]->roadOri*3+1;
+        roadSum = gpsMapLine[PathPoint]->roadSum*3;
+    }
+
+
 
-    roadOri = gpsMapLine[PathPoint]->roadOri;
 
-    roadSum = gpsMapLine[PathPoint]->roadSum;
+ //   avoidX = (roadOri-roadNow)*roadWidth;
+    avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
 
 
     if ( gpsMapLine[PathPoint]->runMode == 0 &&(vehState == avoidObs||vehState ==stopObs||vehState == preAvoid
@@ -1033,7 +1050,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     if(useFrenet){
         //如果车辆在变道中，启用frenet规划局部路径
         if(vehState == avoiding || vehState == backOri){
-            avoidX = (roadOri - roadNow)*roadWidth;
+            //avoidX = (roadOri - roadNow)*roadWidth;
+             avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
             gpsTraceNow = frenetPlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
             //gpsTraceAim = laneChangePlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
             gpsTraceAim = getGpsTraceOffset(gpsTraceOri, avoidX);
@@ -1064,7 +1082,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     //  dSpeed = getSpeed(gpsTraceNow);
     dSpeed = 80;
 
- //   planTrace = gpsTraceNow; //Add By YuChuli, 2020.11.26
+    //   planTrace = gpsTraceNow; //Add By YuChuli, 2020.11.26
 
     planTrace.clear();
     for(int i=0;i<gpsTraceNow.size();i++){
@@ -1305,7 +1323,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         gps_decition->leftlamp = false;
         gps_decition->rightlamp = false;
 
-}
+    }
 
 
 
@@ -1467,14 +1485,16 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         }
         else if(useOldAvoid){
             roadPre = chooseBackRoad(lidarGridPtr, now_gps_ins, gpsMapLine,lidar_per);
-            avoidX = (roadOri - roadNow)*roadWidth; //1012
+          //  avoidX = (roadOri - roadNow)*roadWidth; //1012
+             avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
         }
     }
     if (roadNow != roadOri && roadPre!=-1)
     {
 
         roadNow = roadPre;
-        avoidX = (roadOri - roadNow)*roadWidth;
+     //   avoidX = (roadOri - roadNow)*roadWidth;
+         avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
         gpsTraceNow.clear();
         if(useOldAvoid){
             gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidX);
@@ -1570,7 +1590,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
             //roadPre = chooseAvoidRoad(lidarGridPtr, now_gps_ins,gpsMapLine);
             if(useOldAvoid){
                 roadPre = chooseAvoidRoad(lidarGridPtr, now_gps_ins,gpsMapLine,lidar_per);
-                avoidX = (roadOri - roadNow)*roadWidth;  //1012
+              //  avoidX = (roadOri - roadNow)*roadWidth;  //1012
+                 avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
             }
             else if(useFrenet){
                 double offsetL = -(roadSum - 1)*roadWidth;
@@ -1587,7 +1608,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
             {
                 if(useOldAvoid){
                     roadNow = roadPre;
-                    avoidX = (roadOri - roadNow)*roadWidth;
+                //    avoidX = (roadOri - roadNow)*roadWidth;
+                     avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
                     gpsTraceNow.clear();
                     gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidX);
                 }
@@ -1597,7 +1619,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                         startAvoidGpsInsVector[roadNow] = now_gps_ins;
                     }
                     roadNow = roadPre;
-                    avoidX = (roadOri - roadNow)*roadWidth;
+                 //   avoidX = (roadOri - roadNow)*roadWidth;
+                     avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
                     gpsTraceNow = frenetPlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
                 }
 
@@ -1805,10 +1828,10 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         {
             if(ServiceCarStatus.stationCmd.carMode==0x00)
             {
-               ServiceCarStatus.stationCmd.mode_manual_drive=true;
+                ServiceCarStatus.stationCmd.mode_manual_drive=true;
             }else
             {
-               ServiceCarStatus.stationCmd.mode_manual_drive=false;
+                ServiceCarStatus.stationCmd.mode_manual_drive=false;
             }
         }
 
@@ -1873,7 +1896,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
         if((pt.y<5)&&(abs(realspeed)<0.1)&&(ServiceCarStatus.station_control_door==0))
         {
-                ServiceCarStatus.station_control_door=1;   //open door
+            ServiceCarStatus.station_control_door=1;   //open door
         }
 
         //站点停车log
@@ -1929,8 +1952,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
     if((ServiceCarStatus.msysparam.mvehtype=="hapo")&&(abs(realspeed)<18)){
         if(obsDistance>0 && obsDistance<6){
-                dSpeed=0;
-            }
+            dSpeed=0;
+        }
     }else if(obsDistance>0 && obsDistance<10){
         dSpeed=0;
     }
@@ -1947,7 +1970,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     std::cout<<"dSpeed= "<<dSpeed<<std::endl;
 
     // givlog->debug("SPEED","dSpeed is %f",dSpeed);
-     gps_decition->wheel_angle = 0.0 - controlAng;
+    gps_decition->wheel_angle = 0.0 - controlAng;
     phaseSpeedDecition(gps_decition, secSpeed, obsDistance, obsSpeed,now_gps_ins);
 
 
@@ -1977,7 +2000,7 @@ iv::Station iv::decition::DecideGps00::getNearestStation(iv::GPS_INS now_gps_ins
             }
         }
         givlog->debug("brain_v2x","stationi: %d, map_index: %d",
-                       i,station_n[i].map_index);
+                      i,station_n[i].map_index);
 
     }
     int minDistance=10;
@@ -1992,21 +2015,21 @@ iv::Station iv::decition::DecideGps00::getNearestStation(iv::GPS_INS now_gps_ins
     }
 
     givlog->debug("brain_v2x","now_index: %d",
-                   now_index);
+                  now_index);
 
     int min_index=gpsMap.size()-1;
     int station_index=0;
     for(int i=0;i<station_size;i++)
     {
-          if(station_n[i].map_index>now_index)
-          {
-                front_index=station_n[i].map_index;
-                if(front_index<min_index)
-                {
-                    min_index=front_index;
-                    station_index=i;
-                }
-          }
+        if(station_n[i].map_index>now_index)
+        {
+            front_index=station_n[i].map_index;
+            if(front_index<min_index)
+            {
+                min_index=front_index;
+                station_index=i;
+            }
+        }
     }
 
     qDebug("station_index:%d",station_index);
@@ -2602,18 +2625,18 @@ void iv::decition::DecideGps00::computeObsOnRoad(iv::LidarGridPtr lidarGridPtr,
     }
 
 
-   // obsPredict start
+    // obsPredict start
     if(ServiceCarStatus.useObsPredict){
         float preObsDis=200;
         if(!lidar_per.empty()){
-                 preObsDis=PredictObsDistance(  secSpeed,  gpsTrace, lidar_per);
-                lastPreObsDistance=preObsDis;
-            }else{
-                preObsDis=lastPreObsDistance;
-            }
-            if(preObsDis<lidarDistance || lidarDistance==-1){
-                lidarDistance=preObsDis;
-            }
+            preObsDis=PredictObsDistance(  secSpeed,  gpsTrace, lidar_per);
+            lastPreObsDistance=preObsDis;
+        }else{
+            preObsDis=lastPreObsDistance;
+        }
+        if(preObsDis<lidarDistance || lidarDistance==-1){
+            lidarDistance=preObsDis;
+        }
     }
     // obsPredict end
 
@@ -3017,83 +3040,84 @@ void iv::decition::DecideGps00::predictObsOnRoad(std::vector<iv::Perception::Per
 int iv::decition::DecideGps00::chooseAvoidRoad(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per) {
     roadPre = -1;
 
-//    if (roadNow<roadOri)
-//    {
-//        for (int i = 0; i < roadNow; i++)
-//        {
-//            gpsTraceAvoid.clear();
-//            //	gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri-i))], lastGpsIndex);
-//            avoidX = (roadWidth)*(roadOri - i);
-//            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
-//            //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
-//            computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per);
-//        }
-
-
-//        for (int i = roadOri+1; i < roadSum; i++)
-//        {
-//            gpsTraceAvoid.clear();
-//            //	gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i - roadOri))], lastGpsIndex);
-//            avoidX = (roadWidth)*(roadOri - i);
-//            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
-//            //	bsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
-//            computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per);
-//        }
-//    }
-//    else if (roadNow>roadOri)
-//    {
-//        for (int i = 0; i < roadOri; i++)
-//        {
-//            gpsTraceAvoid.clear();
-//            //	gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri - i))], lastGpsIndex);
-//            avoidX = (roadWidth)*(roadOri - i);
-//            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
-//            //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
-//           computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per);
-//        }
-//        for (int i = roadNow + 1; i < roadSum; i++)
-//        {
-//            gpsTraceAvoid.clear();
-//            //		gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i - roadOri))], lastGpsIndex);
-//            avoidX = (roadWidth)*(roadOri - i);
-//            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
-//            //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
-//           computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per);
-//        }
-
-//    }
-
-//    else
-//    {
-//        for (int i = 0; i < roadOri; i++)
-//        {
-//            gpsTraceAvoid.clear();
-//            //	gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri - i))], lastGpsIndex);
-//            avoidX = (roadWidth)*(roadOri - i);
-//            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
-//            //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
-//           computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
-//        }
-//        for (int i = roadOri + 1; i < roadSum; i++)
-//        {
-//            gpsTraceAvoid.clear();
-//            //		gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i - roadOri))], lastGpsIndex);
-//            avoidX = (roadWidth)*(roadOri - i);
-//            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
-//            //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
-//           computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
-//        }
-
-//    }
+    //    if (roadNow<roadOri)
+    //    {
+    //        for (int i = 0; i < roadNow; i++)
+    //        {
+    //            gpsTraceAvoid.clear();
+    //            //	gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri-i))], lastGpsIndex);
+    //            avoidX = (roadWidth)*(roadOri - i);
+    //            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
+    //            //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
+    //            computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per);
+    //        }
+
+
+    //        for (int i = roadOri+1; i < roadSum; i++)
+    //        {
+    //            gpsTraceAvoid.clear();
+    //            //	gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i - roadOri))], lastGpsIndex);
+    //            avoidX = (roadWidth)*(roadOri - i);
+    //            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
+    //            //	bsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
+    //            computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per);
+    //        }
+    //    }
+    //    else if (roadNow>roadOri)
+    //    {
+    //        for (int i = 0; i < roadOri; i++)
+    //        {
+    //            gpsTraceAvoid.clear();
+    //            //	gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri - i))], lastGpsIndex);
+    //            avoidX = (roadWidth)*(roadOri - i);
+    //            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
+    //            //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
+    //           computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per);
+    //        }
+    //        for (int i = roadNow + 1; i < roadSum; i++)
+    //        {
+    //            gpsTraceAvoid.clear();
+    //            //		gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i - roadOri))], lastGpsIndex);
+    //            avoidX = (roadWidth)*(roadOri - i);
+    //            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
+    //            //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
+    //           computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per);
+    //        }
+
+    //    }
+
+    //    else
+    //    {
+    //        for (int i = 0; i < roadOri; i++)
+    //        {
+    //            gpsTraceAvoid.clear();
+    //            //	gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri - i))], lastGpsIndex);
+    //            avoidX = (roadWidth)*(roadOri - i);
+    //            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
+    //            //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
+    //           computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
+    //        }
+    //        for (int i = roadOri + 1; i < roadSum; i++)
+    //        {
+    //            gpsTraceAvoid.clear();
+    //            //		gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i - roadOri))], lastGpsIndex);
+    //            avoidX = (roadWidth)*(roadOri - i);
+    //            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
+    //            //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
+    //           computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
+    //        }
+
+    //    }
 
     for (int i =  0; i < roadSum; i++)
     {
         gpsTraceAvoid.clear();
         //		gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i - roadOri))], lastGpsIndex);
-        avoidX = (roadWidth)*(roadOri - i);
+     //   avoidX = (roadWidth)*(roadOri - i);
+         avoidX=computeAvoidX(i,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
         gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
         //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
-       computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per);
+        computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per);
     }
 
 
@@ -3117,7 +3141,8 @@ int iv::decition::DecideGps00::chooseAvoidRoad(iv::LidarGridPtr lidarGridPtr, GP
     for (int i = 1; i < roadSum; i++)
     {
         if (roadNow + i < roadSum) {
-            avoidX = (roadOri-roadNow-i)*roadWidth;
+        //    avoidX = (roadOri-roadNow-i)*roadWidth;
+             avoidX=computeAvoidX(roadNow+i,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
             if (checkAvoidEnable(avoidX, lidarGridPtr, roadNow+i))
             {
                 /*if (roadNow==roadOri)
@@ -3134,7 +3159,8 @@ int iv::decition::DecideGps00::chooseAvoidRoad(iv::LidarGridPtr lidarGridPtr, GP
 
         if (roadNow - i >= 0)
         {
-            avoidX = (roadOri - roadNow+i)*roadWidth;
+         //   avoidX = (roadOri - roadNow+i)*roadWidth;
+             avoidX=computeAvoidX(roadNow-i,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
             if (checkAvoidEnable(avoidX, lidarGridPtr, roadNow - i))
             {
                 /*if (roadNow == roadOri)
@@ -3156,33 +3182,33 @@ int iv::decition::DecideGps00::chooseAvoidRoad(iv::LidarGridPtr lidarGridPtr, GP
 int iv::decition::DecideGps00::chooseBackRoad(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per) {
     roadPre = -1;
 
- //   computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
+    //   computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
 
 
 
-//    if (roadNow>=roadOri+1)
-//    {
-//        for (int i = roadOri+1; i < roadNow; i++)
-//        {
-//            gpsTraceBack.clear();
-//            //	gpsTraceBack = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i-roadOri))], lastGpsIndex);
-//            avoidX = (roadWidth)*(roadOri - i);
-//            gpsTraceBack = getGpsTraceOffset(gpsTraceOri, avoidX);
-//            computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
-//        }
-//    }
-//    else if (roadNow <= roadOri - 1) {
-
-//        for (int i = roadOri - 1; i > roadNow; i--)
-//        {
-//            gpsTraceBack.clear();
-//            //	gpsTraceBack = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri-i))], lastGpsIndex);
-//            avoidX = (roadWidth)*(roadOri - i);
-//            gpsTraceBack = getGpsTraceOffset(gpsTraceOri, avoidX);
-//            computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
-//        }
+    //    if (roadNow>=roadOri+1)
+    //    {
+    //        for (int i = roadOri+1; i < roadNow; i++)
+    //        {
+    //            gpsTraceBack.clear();
+    //            //	gpsTraceBack = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i-roadOri))], lastGpsIndex);
+    //            avoidX = (roadWidth)*(roadOri - i);
+    //            gpsTraceBack = getGpsTraceOffset(gpsTraceOri, avoidX);
+    //            computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
+    //        }
+    //    }
+    //    else if (roadNow <= roadOri - 1) {
+
+    //        for (int i = roadOri - 1; i > roadNow; i--)
+    //        {
+    //            gpsTraceBack.clear();
+    //            //	gpsTraceBack = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri-i))], lastGpsIndex);
+    //            avoidX = (roadWidth)*(roadOri - i);
+    //            gpsTraceBack = getGpsTraceOffset(gpsTraceOri, avoidX);
+    //            computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
+    //        }
 
-//    }
+    //    }
 
 
 
@@ -3190,7 +3216,8 @@ int iv::decition::DecideGps00::chooseBackRoad(iv::LidarGridPtr lidarGridPtr, GPS
     {
         gpsTraceBack.clear();
         //	gpsTraceBack = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri-i))], lastGpsIndex);
-        avoidX = (roadWidth)*(roadOri - i);
+     //   avoidX = (roadWidth)*(roadOri - i);
+         avoidX=computeAvoidX(i,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
         gpsTraceBack = getGpsTraceOffset(gpsTraceOri, avoidX);
         computeObsOnRoad(lidarGridPtr, gpsTraceBack, i,gpsMapLine,lidar_per);
     }
@@ -3777,34 +3804,34 @@ float iv::decition::DecideGps00::computeTrafficSpeedLimt(float trafficDis){
 
 
 bool iv::decition::DecideGps00::adjuseultra(){
-     bool front=false,back=false,left=false,right=false;
-     for(int i=1;i<=13;i++)
-     {
-         if((i==2)||(i==3)||(i==4)||(i==5))   //front
-         {
-             if(ServiceCarStatus.ultraDistance[i]<100)
-             {
-                 front=true;
-             }
-         }else if((i==1)||(i==12)||(i==6)||(i==7))   //left,right
-         {
-             if(ServiceCarStatus.ultraDistance[i]<30)
-             {
-                 left=true;
-             }
-         }else if((i==8)||(i==9)||(i==10)||(i==11))   //back
-         {
-             if(ServiceCarStatus.ultraDistance[i]<10)
-             {
-                 back=true;
-             }
-         }
-     }
-
-     if(front||left||back)
-         return true;
-     else
-         return false;
+    bool front=false,back=false,left=false,right=false;
+    for(int i=1;i<=13;i++)
+    {
+        if((i==2)||(i==3)||(i==4)||(i==5))   //front
+        {
+            if(ServiceCarStatus.ultraDistance[i]<100)
+            {
+                front=true;
+            }
+        }else if((i==1)||(i==12)||(i==6)||(i==7))   //left,right
+        {
+            if(ServiceCarStatus.ultraDistance[i]<30)
+            {
+                left=true;
+            }
+        }else if((i==8)||(i==9)||(i==10)||(i==11))   //back
+        {
+            if(ServiceCarStatus.ultraDistance[i]<10)
+            {
+                back=true;
+            }
+        }
+    }
+
+    if(front||left||back)
+        return true;
+    else
+        return false;
 
 }
 
@@ -3816,3 +3843,46 @@ void iv::decition::DecideGps00::transferGpsMode2( const std::vector<GPSData> gps
         dSpeed=min(dSpeed,5.0);
     }
 }
+float iv::decition::DecideGps00::computeAvoidX(int roadAim,int roadOri,GPSData gps,float vehWidth){
+    if(roadAim==roadOri){
+        return 0;
+    }
+    float x=0;
+    float veh_to_roadSide=(gps->mfRoadWidth- ServiceCarStatus.msysparam.vehWidth)*0.5;
+    float roadSide_to_roadSide=ServiceCarStatus.msysparam.vehWidth;
+    if(!ServiceCarStatus.inRoadAvoid){
+        x= (roadOri-roadAim)*gps->mfRoadWidth;
+    }else{
+        int num=roadOri-roadAim;
+        switch (num%3) {
+        case 0:
+            x=(num/3)*gps->mfRoadWidth;
+            break;
+        case 1:
+            if(num>0){
+                x=(num/3)*gps->mfRoadWidth +veh_to_roadSide;
+            }else{
+                x=(num/3)*gps->mfRoadWidth -veh_to_roadSide;
+            }
+            break;
+        case 2:
+            if(num>0){
+                x=(num/3)*gps->mfRoadWidth +veh_to_roadSide+roadSide_to_roadSide;
+            }else{
+                x=(num/3)*gps->mfRoadWidth -veh_to_roadSide-roadSide_to_roadSide;
+            }
+
+            break;
+        default:
+            break;
+        }
+
+
+
+    }
+
+
+    return x;
+
+
+}

src/decition/decition_brain/decition/decide_gps_00.h

@@ -204,6 +204,8 @@ public:
     bool computeTrafficPass(float trafficDis,int trafficColor,float trafficTime,float realSecSpeed,float dSecSpeed);
     float computeTrafficSpeedLimt(float trafficDis);
 
+    float computeAvoidX(int roadAim,int roadOri,GPSData gps,float vehWidth);
+
     bool adjuseultra();
 
     iv::Station getNearestStation(iv::GPS_INS now_gps_ins,std::vector<Station> station_n,std::vector<GPSData> gpsMap);