modify avoid area

src/decition/decition_brain_sf/decition/adc_tools/compute_00.cpp

@@ -589,7 +589,7 @@ int  iv::decition::Compute00::getSpeedPointIndex(std::vector<Point2D> gpsTrace,
     return index;
 }
 
-iv::Point2D iv::decition::Compute00::getLidarObsPoint(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr) {
+/*iv::Point2D iv::decition::Compute00::getLidarObsPoint(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr) {
 
     iv::Point2D obsPoint(-1, -1);
     vector<Point2D> gpsTraceLeft;
@@ -734,30 +734,240 @@ iv::Point2D iv::decition::Compute00::getLidarObsPoint(std::vector<Point2D> gpsTr
                 }
             }
 
-            /*if (count >= 2)
-            {
-                obsPoint.x = gpsTrace[j].x;
-                obsPoint.y = gpsTrace[j].y;
+//            if (count >= 2)
+//            {
+//                obsPoint.x = gpsTrace[j].x;
+//                obsPoint.y = gpsTrace[j].y;
 
-                int dx = ( obsPoint.x + gridwide*(double)centerx)/gridwide;
-                int dy = (obsPoint.y + gridwide*(double)centery+xiuzheng)/gridwide;
+//                int dx = ( obsPoint.x + gridwide*(double)centerx)/gridwide;
+//                int dy = (obsPoint.y + gridwide*(double)centery+xiuzheng)/gridwide;
 
-                obsPoint.obs_speed_x=lidarGridPtr[dx * (iv::gry) + dy].speed_x;
-                obsPoint.obs_speed_y=lidarGridPtr[dx * (iv::gry) + dy].speed_y;
-                obsPoint.obs_type= lidarGridPtr[dx * (iv::gry) + dy].type;
+//                obsPoint.obs_speed_x=lidarGridPtr[dx * (iv::gry) + dy].speed_x;
+//                obsPoint.obs_speed_y=lidarGridPtr[dx * (iv::gry) + dy].speed_y;
+//                obsPoint.obs_type= lidarGridPtr[dx * (iv::gry) + dy].type;
 
-                isRemove = true;
-                givlog->debug("brain_decition","obsDistance: %f,obsSpeed: %f",
-                              obsPoint.y,lidarGridPtr[dx * (iv::gry) + dy].speed_y);
-                //		DecideGps00().lidarDistance = obsPoint.y;
-                return obsPoint;
-            }*/
+//                isRemove = true;
+//                givlog->debug("brain_decition","obsDistance: %f,obsSpeed: %f",
+//                              obsPoint.y,lidarGridPtr[dx * (iv::gry) + dy].speed_y);
+//                //		DecideGps00().lidarDistance = obsPoint.y;
+//                return obsPoint;
+//            }
         }
     }
     //	DecideGps00().lidarDistance = obsPoint.y;
     return obsPoint;
+}*/
+
+
+
+iv::Point2D iv::decition::Compute00::getLidarObsPoint(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr) {
+
+    iv::Point2D obsPoint(-1, -1);
+    vector<Point2D> gpsTraceLeft;
+    vector<Point2D> gpsTraceRight;
+    float xiuzheng=0;
+    if(!ServiceCarStatus.useMobileEye){
+        xiuzheng=0-ServiceCarStatus.msysparam.lidarGpsXiuzheng;
+    }
+
+    ServiceCarStatus.obsTraceLeft.clear();
+    ServiceCarStatus.obsTraceRight.clear();
+
+    for (int j = 0; j < gpsTrace.size(); j++)
+    {
+        double sumx1 = 0, sumy1 = 0, count1 = 0;
+        double sumx2 = 0, sumy2 = 0, count2 = 0;
+        for (int k = max(0, j - 4); k <= j; k++)
+        {
+            count1 = count1 + 1;
+            sumx1 += gpsTrace[k].x;
+            sumy1 += gpsTrace[k].y;
+        }
+        for (unsigned int k = j; k <= min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
+        {
+            count2 = count2 + 1;
+            sumx2 += gpsTrace[k].x;
+            sumy2 += gpsTrace[k].y;
+        }
+        sumx1 /= count1; sumy1 /= count1;
+        sumx2 /= count2; sumy2 /= count2;
+
+        double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);
+
+        double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
+        double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);
+
+        Point2D ptLeft(carFrontx + ServiceCarStatus.msysparam.vehWidth / 2 * cos(anglevalue + PI / 2),
+                       carFronty + ServiceCarStatus.msysparam.vehWidth / 2 * sin(anglevalue + PI / 2));
+
+        Point2D ptRight(carFrontx + ServiceCarStatus.msysparam.vehWidth / 2 * cos(anglevalue - PI / 2),
+                        carFronty + ServiceCarStatus.msysparam.vehWidth / 2 * sin(anglevalue - PI / 2));
+
+        gpsTraceLeft.push_back(ptLeft);
+        gpsTraceRight.push_back(ptRight);
+
+
+        TracePoint obsptleft(ptLeft.x,ptLeft.y);
+        ServiceCarStatus.obsTraceLeft.push_back(obsptleft);
+
+        TracePoint obsptright(ptRight.x,ptRight.y);
+        ServiceCarStatus.obsTraceLeft.push_back(obsptright);
+    }
+
+
+    bool isRemove = false;
+
+    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
+    {
+
+        if (!isRemove && gpsTrace[j].y>ServiceCarStatus.msysparam.lidarGpsXiuzheng)
+        {
+            int count = 0;
+
+            for (double length = 0; length <= ServiceCarStatus.msysparam.vehWidth; length += 0.4)
+            {
+                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / ServiceCarStatus.msysparam.vehWidth * length;
+                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / ServiceCarStatus.msysparam.vehWidth * length;
+                int dx = (ptx + gridwide*(double)centerx)/gridwide;
+                int dy = (pty + gridwide*(double)centery+xiuzheng)/gridwide;
+
+                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
+                {
+                    if (lidarGridPtr[dx * (iv::gry) + dy].ob != 0)
+                    {
+                        count++; obsPoint.x = ptx; obsPoint.y = pty;
+                    }
+                    if (count >= 2)
+                    {
+                        obsPoint.x = gpsTrace[j].x;
+                        obsPoint.y = gpsTrace[j].y;
+                        obsPoint.obs_speed_x=lidarGridPtr[dx * (iv::gry) + dy].speed_x;
+                        obsPoint.obs_speed_y=lidarGridPtr[dx * (iv::gry) + dy].speed_y;
+                        obsPoint.obs_type= lidarGridPtr[dx * (iv::gry) + dy].type;
+                        isRemove = true;
+                        return obsPoint;
+                    }
+                }
+            }
+
+            j++;
+
+            for (double length = 0; length <= ServiceCarStatus.msysparam.vehWidth; length += 0.4)
+            {
+                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / ServiceCarStatus.msysparam.vehWidth * length;
+                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / ServiceCarStatus.msysparam.vehWidth * length;
+                int dx = (ptx + gridwide*(double)centerx)/gridwide;
+                int dy = (pty + gridwide*(double)centery+xiuzheng)/gridwide;
+
+                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
+                {
+                    if (lidarGridPtr[dx * (iv::gry) + dy].ob != 0)
+                    {
+                        count++; obsPoint.x = ptx; obsPoint.y = pty;
+                    }
+
+                    if (count >= 2)
+                    {
+                        obsPoint.x = gpsTrace[j].x;
+                        obsPoint.y = gpsTrace[j].y;
+
+                        obsPoint.obs_speed_x=lidarGridPtr[dx * (iv::gry) + dy].speed_x;
+                        obsPoint.obs_speed_y=lidarGridPtr[dx * (iv::gry) + dy].speed_y;
+                        obsPoint.obs_type= lidarGridPtr[dx * (iv::gry) + dy].type;
+
+                        isRemove = true;
+                        return obsPoint;
+                    }
+                }
+            }
+        }
+    }
+    return obsPoint;
+}
+
+
+iv::Point2D iv::decition::Compute00::getLidarObsPointXY(std::vector<Point2D> gpsTrace,std::vector<Point2D> gpsTraceLeft,std::vector<Point2D> gpsTraceRight, iv::LidarGridPtr lidarGridPtr) {
+
+    iv::Point2D obsPoint(-1, -1);
+    float xiuzheng=0;
+    if(!ServiceCarStatus.useMobileEye){
+        xiuzheng=0-ServiceCarStatus.msysparam.lidarGpsXiuzheng;
+    }
+
+    bool isRemove = false;
+    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
+    {
+
+        if (!isRemove && gpsTrace[j].y>ServiceCarStatus.msysparam.lidarGpsXiuzheng)
+        {
+            int count = 0;
+
+            for (double length = 0; length <= ServiceCarStatus.msysparam.vehWidth; length += 0.4)
+            {
+                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / ServiceCarStatus.msysparam.vehWidth * length;
+                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / ServiceCarStatus.msysparam.vehWidth * length;
+                int dx = (ptx + gridwide*(double)centerx)/gridwide;
+                int dy = (pty + gridwide*(double)centery+xiuzheng)/gridwide;
+
+                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
+                {
+                    if (lidarGridPtr[dx * (iv::gry) + dy].ob != 0)
+                    {
+                        count++; obsPoint.x = ptx; obsPoint.y = pty;
+                    }
+                    if (count >= 2)
+                    {
+                        obsPoint.x = gpsTrace[j].x;
+                        obsPoint.y = gpsTrace[j].y;
+                        obsPoint.obs_speed_x=lidarGridPtr[dx * (iv::gry) + dy].speed_x;
+                        obsPoint.obs_speed_y=lidarGridPtr[dx * (iv::gry) + dy].speed_y;
+                        obsPoint.obs_type= lidarGridPtr[dx * (iv::gry) + dy].type;
+                        isRemove = true;
+                        return obsPoint;
+                    }
+                }
+            }
+
+            j++;
+
+            for (double length = 0; length <= ServiceCarStatus.msysparam.vehWidth; length += 0.4)
+            {
+                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / ServiceCarStatus.msysparam.vehWidth * length;
+                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / ServiceCarStatus.msysparam.vehWidth * length;
+                int dx = (ptx + gridwide*(double)centerx)/gridwide;
+                int dy = (pty + gridwide*(double)centery+xiuzheng)/gridwide;
+
+                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
+                {
+                    if (lidarGridPtr[dx * (iv::gry) + dy].ob != 0)
+                    {
+                        count++; obsPoint.x = ptx; obsPoint.y = pty;
+                    }
+
+                    if (count >= 2)
+                    {
+                        obsPoint.x = gpsTrace[j].x;
+                        obsPoint.y = gpsTrace[j].y;
+
+                        obsPoint.obs_speed_x=lidarGridPtr[dx * (iv::gry) + dy].speed_x;
+                        obsPoint.obs_speed_y=lidarGridPtr[dx * (iv::gry) + dy].speed_y;
+                        obsPoint.obs_type= lidarGridPtr[dx * (iv::gry) + dy].type;
+
+                        isRemove = true;
+                        return obsPoint;
+                    }
+                }
+            }
+        }
+    }
+    return obsPoint;
 }
 
+
+
+
+
+
 //1220
 iv::Point2D iv::decition::Compute00::getLidarRearObsPoint(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr) {
 

src/decition/decition_brain_sf/decition/adc_tools/compute_00.h

@@ -44,6 +44,8 @@ namespace iv {
             static int getSpeedPointIndex(std::vector<Point2D> gpsTrace, double realSpeed);
             static Point2D getLidarObsPoint(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr);
             static Point2D getLidarRearObsPoint(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr);
+            static Point2D getLidarObsPointXY(std::vector<Point2D> gpsTrace,std::vector<Point2D> gpsTraceLeft,std::vector<Point2D> gpsTraceRight, iv::LidarGridPtr lidarGridPtr);
+
 
 
             static Point2D getLidarObsPointAvoid(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr);

src/decition/decition_brain_sf/decition/decide_gps_00.cpp

@@ -173,7 +173,7 @@ enum VehState { normalRun, park, readyPark, stopObs, avoidObs, preAvoid, avoidin
 
 
 std::vector<iv::Point2D>  gpsTraceAim; //记录变道后的目标路径，若车辆经frenet规划行驶至该路径上后，就关闭frenet规划
-std::vector<iv::Point2D>  gpsTraceAvoid,gpsTraceNow,gpsTraceBack,gpsTracePre,gpsTraceOri,gpsTraceRear;
+std::vector<iv::Point2D>  gpsTraceAvoid,gpsTraceNow,gpsTraceBack,gpsTracePre,gpsTraceOri,gpsTraceRear,gpsTraceNowLeft,gpsTraceNowRight;
 
 std::vector<double> esrDisVector(roadSum, -1);
 std::vector<double> lidarDisVector(roadSum, -1);
@@ -185,6 +185,8 @@ std::vector<int> obsLostTimeVector(roadSum, 0);
 std::vector<double> lastLidarDisVector(roadSum, -1);
 std::vector<double> lastDistanceVector(roadSum, -1);
 
+std::vector<iv::Point2D> traceOriLeft,traceOriRight;
+
 bool qiedianCount = false;
 //日常展示
 
@@ -939,6 +941,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     gpsTraceAim.clear();
     gpsTraceAvoid.clear();
     gpsTraceBack.clear();
+    gpsTraceNowLeft.clear();
+    gpsTraceNowRight.clear();
 
 
 
@@ -1089,12 +1093,13 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         if (roadNow==roadOri)
         {
             gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
-            //gpsTraceNow = getGpsTrace(now_gps_ins, gpsMapLine, lastGpsIndex);
+            gpsTraceNowLeft.assign(traceOriLeft.begin(),traceOriLeft.end());
+            gpsTraceNowRight.assign(traceOriRight.begin(),traceOriRight.end());
         }else
         {
-            //	gpsTraceNow = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri-roadNow))], lastGpsIndex);
             gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidX);
-            //gpsTraceNow = laneChangePlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
+            gpsTraceNowLeft=getGpsTraceOffset(traceOriLeft, avoidX);
+            gpsTraceNowRight=getGpsTraceOffset(traceOriRight, avoidX);
         }
     }
 
@@ -1396,7 +1401,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     static int obs_filter=0,obs_filter_flag=0,obs_filter_dis_memory=0;
     static double obs_speed_for_avoid=-1,obs_filter_speed_memory=0;
     if(!ServiceCarStatus.daocheMode){
-        computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
+        //computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
+        computeObsOnRoadXY(lidarGridPtr, gpsTraceNow,gpsTraceNowLeft,gpsTraceNowRight,roadNow,gpsMapLine,lidar_per);
 
 
         if(obs_filter_flag==0)
@@ -2030,9 +2036,12 @@ void iv::decition::DecideGps00::phaseSpeedDecition(iv::decition::Decition  decit
 
 std::vector<iv::Point2D> iv::decition::DecideGps00::getGpsTrace(iv::GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine, int lastIndex, bool circleMode) {
     vector<iv::Point2D> trace;
-    //	int  PathPoint = Compute00().getNearestPointIndex(now_gps_ins, gpsMapLine, DecideGps00::lastGpsIndex, DecideGps00::minDis, DecideGps00::maxAngle);
-    /*if (PathPoint != -1)
-        lastGpsIndex = PathPoint;*/
+
+    traceOriLeft.clear();
+    traceOriRight.clear();
+
+    ServiceCarStatus.obsTraceLeft.clear();
+    ServiceCarStatus.obsTraceRight.clear();
 
     if (gpsMapLine.size() > 600 && PathPoint >= 0) {
         int aimIndex;
@@ -2056,8 +2065,6 @@ std::vector<iv::Point2D> iv::decition::DecideGps00::getGpsTrace(iv::GPS_INS now_
                 DecideGps00::zhuchePointNum = index;
             }
 
-
-
             if (gpsMapLine[index]->roadMode==3 && !hasZhuched)
             {
                 readyZhucheMode = true;
@@ -2070,14 +2077,6 @@ std::vector<iv::Point2D> iv::decition::DecideGps00::getGpsTrace(iv::GPS_INS now_
                 readyParkMode = true;
                 DecideGps00::finishPointNum = index;
             }
-
-            //			if (pt.v1 == 22 || pt.v1 == 23)
-            //			{
-            //				readyParkMode = true;
-            //				DecideGps00::finishPointNum = i;
-            //			}
-
-
             switch (pt.v1)
             {
             case 0:
@@ -2111,6 +2110,29 @@ std::vector<iv::Point2D> iv::decition::DecideGps00::getGpsTrace(iv::GPS_INS now_
                 break;
             }
             trace.push_back(pt);
+
+
+            double hdg=90 - (*gpsMapLine[index]).ins_heading_angle;
+            if(hdg < 0)  hdg = hdg + 360;
+            if(hdg >= 360) hdg = hdg - 360;
+
+            double xyhdg  = hdg/180.0*PI;
+            double ptLeft_x=(*gpsMapLine[index]).gps_x + ServiceCarStatus.msysparam.vehWidth / 2 * cos(xyhdg + PI / 2);
+            double ptLeft_y=(*gpsMapLine[index]).gps_y + ServiceCarStatus.msysparam.vehWidth / 2 * sin(xyhdg + PI / 2);
+            Point2D ptLeft = Coordinate_Transfer(ptLeft_x, ptLeft_y, now_gps_ins);
+
+            double ptRight_x=(*gpsMapLine[index]).gps_x + ServiceCarStatus.msysparam.vehWidth / 2 * cos(xyhdg - PI / 2);
+            double ptRight_y=(*gpsMapLine[index]).gps_y + ServiceCarStatus.msysparam.vehWidth / 2 * sin(xyhdg - PI / 2);
+            Point2D ptRight = Coordinate_Transfer(ptRight_x, ptRight_y, now_gps_ins);
+
+            traceOriLeft.push_back(ptLeft);
+            traceOriRight.push_back(ptRight);
+
+            TracePoint obsptleft(ptLeft.x,ptLeft.y);
+            ServiceCarStatus.obsTraceLeft.push_back(obsptleft);
+
+            TracePoint obsptright(ptRight.x,ptRight.y);
+            ServiceCarStatus.obsTraceLeft.push_back(obsptright);
         }
     }
     return trace;
@@ -2544,6 +2566,7 @@ void iv::decition::DecideGps00::computeObsOnRoad(iv::LidarGridPtr lidarGridPtr,
     //   xTime.start();
     //    qDebug("time 0 is %d ",xTime.elapsed());
     double obs,obsSd;
+
     if (lidarGridPtr == NULL)
     {
         lidarDistance = lastLidarDis;
@@ -2626,8 +2649,97 @@ void iv::decition::DecideGps00::computeObsOnRoad(iv::LidarGridPtr lidarGridPtr,
 
 }
 
+void iv::decition::DecideGps00::computeObsOnRoadXY(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace,std::vector<Point2D> gpsTraceLeft,std::vector<Point2D> gpsTraceRight,int roadNum,
+                                                 const std::vector<GPSData> gpsMapLine, std::vector<iv::Perception::PerceptionOutput> lidar_per) {
+
+    //    QTime xTime;
+    //   xTime.start();
+    //    qDebug("time 0 is %d ",xTime.elapsed());
+    double obs,obsSd;
+
+    if (lidarGridPtr == NULL)
+    {
+        lidarDistance = lastLidarDis;
+        //	lidarDistance = lastlidarDistance;
+
+    }
+    else {
+        //obsPoint = Compute00().getLidarObsPoint(gpsTrace, lidarGridPtr);
+        obsPoint = Compute00().getLidarObsPointXY(gpsTrace,gpsTraceLeft,gpsTraceRight,lidarGridPtr);
+        float lidarXiuZheng=0;
+        if(!ServiceCarStatus.useMobileEye){
+            lidarXiuZheng=0-ServiceCarStatus.msysparam.frontGpsXiuzheng;
+        }
+        lidarDistance = obsPoint.y + lidarXiuZheng;   //激光距离推到车头  gjw20191110 lidar
+        //    lidarDistance=-1;
+        if (lidarDistance<0)
+        {
+            lidarDistance = -1;
+        }
+        lastLidarDis = lidarDistance;
+    }
+
+
+
+
+
+
+    if(lidarDistance<0){
+        lidarDistance=500;
+    }
+
+
+
+    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "激光雷达距离:" << lidarDistance << std::endl;
+
+    ServiceCarStatus.mLidarObs = lidarDistance;
+    obs = lidarDistance;
+
+    obsSd= obsPoint.obs_speed_y;
+
+    if(ServiceCarStatus.useLidarPerPredict){
+       double preDis= predictObsOnRoad(lidar_per, gpsTrace, 1.0);
+       if (preDis<obs){
+           obs = preDis;
+           if(abs(obs-preDis>0.5)){
+            obsSd = 0-realspeed;
+           }
+       }
+    }
+
+
+    if(roadNum==roadNow){
+        obsDistance=obs;
+        obsSpeed=obsSd;
+    }
+
+    if(obsDistance<500&&obsDistance>0){
+     std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "obsDistance<500:" << obsDistance << std::endl;
+    }
+    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "最终得出的obsDistance:" << obsDistance << std::endl;
 
 
+    ServiceCarStatus.mObs = obsDistance;
+    if(ServiceCarStatus.mObs>100){
+        ServiceCarStatus.mObs =-1;
+    }
+
+    if (obsDistance>0)
+    {
+        lastDistance = obsDistance;
+    }
+
+    //lsn
+    if(obs<0){
+        obsDisVector[roadNum]=500;
+    }else{
+        obsDisVector[roadNum]=obs;
+    }
+
+    //   qDebug("time 3 is %d ",xTime.elapsed());
+
+}
+
 
 //1220
 void iv::decition::DecideGps00::computeRearObsOnRoad(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,
@@ -2747,84 +2859,15 @@ double iv::decition::DecideGps00::predictObsOnRoad(std::vector<iv::Perception::P
 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, 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);
-    //            //	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, gpsTraceNow, roadNow,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, gpsTraceNow, roadNow,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=computeAvoidX(i,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
+        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);
+        gpsTraceNowLeft=getGpsTraceOffset(traceOriLeft, avoidX);
+        gpsTraceNowRight=getGpsTraceOffset(traceOriRight, avoidX);
+        //computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per);
+        computeObsOnRoadXY(lidarGridPtr, gpsTraceAvoid,gpsTraceNowLeft,gpsTraceNowRight,i,gpsMapLine,lidar_per);
     }
 
     if (lidarGridPtr!=NULL)
@@ -2888,43 +2931,15 @@ 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);
-
-
-
-    //    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);
-    //        }
-
-    //    }
-
-
     for (int i =  0; i <roadSum; i++)
     {
         gpsTraceBack.clear();
-        //	gpsTraceBack = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri-i))], lastGpsIndex);
-      //  avoidX = (roadWidth)*(roadOri - i);
-         avoidX=computeAvoidX(i,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
+        avoidX=computeAvoidX(i,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
         gpsTraceBack = getGpsTraceOffset(gpsTraceOri, avoidX);
-        computeObsOnRoad(lidarGridPtr, gpsTraceBack, i,gpsMapLine,lidar_per);
+        gpsTraceNowLeft=getGpsTraceOffset(traceOriLeft, avoidX);
+        gpsTraceNowRight=getGpsTraceOffset(traceOriRight, avoidX);
+        //computeObsOnRoad(lidarGridPtr, gpsTraceBack, i,gpsMapLine,lidar_per);
+        computeObsOnRoadXY(lidarGridPtr, gpsTraceBack,gpsTraceNowLeft,gpsTraceNowRight,i,gpsMapLine,lidar_per);
     }
 
 
@@ -2941,14 +2956,6 @@ int iv::decition::DecideGps00::chooseBackRoad(iv::LidarGridPtr lidarGridPtr, GPS
         return -1;
     }
 
-
-
-
-
-
-
-    //if ((GetDistance(now_gps_ins, startAvoid_gps_ins)>avoidRunDistance)&&
-    //(checkReturnEnable(avoidX, lidarGridPtr,roadOri)))
     if ((GetDistance(now_gps_ins, startAvoidGpsInsVector[roadOri])>max(avoidMinDistanceVector[roadOri],28.0)) &&
             (checkReturnEnable(avoidX, lidarGridPtr, roadOri)))
     {

src/decition/decition_brain_sf/decition/decide_gps_00.h

@@ -166,6 +166,7 @@ public:
     bool checkReturnEnable(double avoidX, iv::LidarGridPtr lidarGridPtr, int roadNum);
 
     void computeObsOnRoad(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,const std::vector<GPSData> gpsMapLine, std::vector<iv::Perception::PerceptionOutput> lidar_per);
+    void computeObsOnRoadXY(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace,std::vector<Point2D> gpsTraceLeft,std::vector<Point2D> gpsTraceRight,int roadNum,const std::vector<GPSData> gpsMapLine, std::vector<iv::Perception::PerceptionOutput> lidar_per);
     static void getEsrObsDistanceByFrenet(const std::vector<Point2D>& gpsTrace, iv::decition::FrenetPoint car_now_frenet_point,iv::decition::FrenetPoint &esrObs_F_Point, const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps);
 
     void computeRearObsOnRoad(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,const std::vector<GPSData> gpsMapLine);