modify avoid back path

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

@@ -770,8 +770,8 @@ iv::Point2D iv::decition::Compute00::getLidarObsPoint(std::vector<Point2D> gpsTr
         xiuzheng=0-ServiceCarStatus.msysparam.lidarGpsXiuzheng;
     }
 
-    ServiceCarStatus.obsTraceLeft.clear();
-    ServiceCarStatus.obsTraceRight.clear();
+//    ServiceCarStatus.obsTraceLeft.clear();
+//    ServiceCarStatus.obsTraceRight.clear();
 
     for (int j = 0; j < gpsTrace.size(); j++)
     {
@@ -807,11 +807,11 @@ iv::Point2D iv::decition::Compute00::getLidarObsPoint(std::vector<Point2D> gpsTr
         gpsTraceRight.push_back(ptRight);
 
 
-        TracePoint obsptleft(ptLeft.x,ptLeft.y);
-        ServiceCarStatus.obsTraceLeft.push_back(obsptleft);
+//        TracePoint obsptleft(ptLeft.x,ptLeft.y);
+//        ServiceCarStatus.obsTraceLeft.push_back(obsptleft);
 
-        TracePoint obsptright(ptRight.x,ptRight.y);
-        ServiceCarStatus.obsTraceRight.push_back(obsptright);
+//        TracePoint obsptright(ptRight.x,ptRight.y);
+//        ServiceCarStatus.obsTraceRight.push_back(obsptright);
     }
 
 

src/decition/decition_brain_sf/decition/decide_gps_00.cpp

@@ -972,8 +972,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         roadSum = gpsMapLine[PathPoint]->roadSum*3;
     }
 
-//    roadOri =0;
-//    roadSum =2;
+    roadOri =0;
+    roadSum =2;
 
     if(roadNowStart){
         roadNow=roadOri;
@@ -1062,20 +1062,29 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     if(gpsTraceNow.size()==0){
         if (roadNow==roadOri)
         {
+            if(vehState == backOri)
+            {
+                gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
+                getGpsTraceNowLeftRight(gpsTraceNow);
+            }
+            else{
             gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
             gpsTraceNowLeft.assign(traceOriLeft.begin(),traceOriLeft.end());
             gpsTraceNowRight.assign(traceOriRight.begin(),traceOriRight.end());
+            }
         }else
         {
             if((vehState == avoiding)||(vehState == backOri))
             {
                 gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
-                gpsTraceNowLeft=getGpsTraceOffset(traceOriLeft, avoidX);
-                gpsTraceNowRight=getGpsTraceOffset(traceOriRight, avoidX);
+//                gpsTraceNowLeft=getGpsTraceOffset(traceOriLeft, avoidX);
+//                gpsTraceNowRight=getGpsTraceOffset(traceOriRight, avoidX);
+                getGpsTraceNowLeftRight(gpsTraceNow);
             }else{
                 gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidX);
-                gpsTraceNowLeft=getGpsTraceOffset(traceOriLeft, avoidX);
-                gpsTraceNowRight=getGpsTraceOffset(traceOriRight, avoidX);
+//                gpsTraceNowLeft=getGpsTraceOffset(traceOriLeft, avoidX);
+//                gpsTraceNowRight=getGpsTraceOffset(traceOriRight, avoidX);
+                getGpsTraceNowLeftRight(gpsTraceNow);
             }
         }
     }
@@ -1107,7 +1116,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     if(ServiceCarStatus.msysparam.mvehtype !="zhongche"){
 
         if(vehState==avoiding){
-            ServiceCarStatus.msysparam.vehWidth=2.1;
+            ServiceCarStatus.msysparam.vehWidth=2.4;
             controlAng=max(-150.0,controlAng);//35  zj-150
             controlAng=min(150.0,controlAng);//35   zj-150
         }
@@ -1298,7 +1307,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
     }
 
-    if((vehState==avoiding)||(vehState==backOri)||(roadNow != roadOri))
+    if((vehState==avoiding)||(vehState==backOri)||(roadNow != roadOri)||(vehState==preAvoid))
     {
         dSpeed = min(8.0,dSpeed);
     }
@@ -1400,13 +1409,14 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
     if (vehState == avoiding)
     {
+        double avoidDistance=GetDistance(startAvoidGpsIns,now_gps_ins);
         //若车辆到达变道后的路径，改变车辆状态，关闭frenet规划
         if (useFrenet && abs(gpsTraceAim[0].x)<1.0) {
             vehState = normalRun;
             //            useFrenet = false;
             //            useOldAvoid = true;
         }
-        else if (useOldAvoid && abs(gpsTraceNow[60].x)<0.05) {    //zj 2021.06.21
+        else if (useOldAvoid && avoidDistance>35) {    //zj 2021.06.21   gpsTraceNow[60].x)<0.02
             // vehState = avoidObs; 0929
             vehState = normalRun;
         }
@@ -1431,13 +1441,14 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
     if (vehState==backOri)
     {
+        double backDistance=GetDistance(startBackGpsIns,now_gps_ins);
         //若车辆到达变道后的路径，改变车辆状态，关闭frenet规划
         if (useFrenet && abs(gpsTraceAim[0].x)<1.0) {
             vehState = normalRun;
             //            useFrenet = false;
             //            useOldAvoid = true;
         }
-        else if (useOldAvoid && abs(gpsTraceNow[0].x)<0.5) {
+        else if (useOldAvoid && backDistance>35 ) {//abs(gpsTraceNow[60].x)<0.05
             // vehState = avoidObs; 0929
             vehState = normalRun;
         }
@@ -1482,6 +1493,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
             //gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidX);
             gpsTraceAvoidXY.clear();
             gpsTraceAvoidXY = getGpsTraceOffsetBackOri(gpsTraceOri, avoidX,now_gps_ins,gpsTraceNow);
+            startBackGpsIns = now_gps_ins;
         }
         else if(useFrenet){
             gpsTraceNow = frenetPlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
@@ -1615,6 +1627,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                     //gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidX);
                     gpsTraceAvoidXY.clear();
                     gpsTraceAvoidXY = getGpsTraceOffsetAvoid(gpsTraceOri, avoidX,now_gps_ins);
+                    startAvoidGpsIns = now_gps_ins;
                 }
                 else if(useFrenet){
                     if(roadPre != roadNow){
@@ -1770,12 +1783,6 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
     }
 
-
-
-
-
-
-
     ///kkcai, 2020-01-03
     //if (ServiceCarStatus.carState == 2 && busMode)
     if (ServiceCarStatus.carState == 2)
@@ -1964,12 +1971,12 @@ std::vector<iv::Point2D> iv::decition::DecideGps00::getGpsTrace(iv::GPS_INS now_
     traceOriLeft.clear();
     traceOriRight.clear();
 
-    if (gpsMapLine.size() > 600 && PathPoint >= 0) {
+    if (gpsMapLine.size() > 800 && PathPoint >= 0) {
         int aimIndex;
         if(circleMode){
-            aimIndex=PathPoint+600;
+            aimIndex=PathPoint+800;
         }else{
-            aimIndex=min((PathPoint+600),(int)gpsMapLine.size());
+            aimIndex=min((PathPoint+800),(int)gpsMapLine.size());
         }
 
         for (int i = PathPoint; i < aimIndex; i++)
@@ -2143,6 +2150,46 @@ std::vector<iv::Point2D>  iv::decition::DecideGps00::getGpsTraceOffset(std::vect
     return trace;
 }
 
+void  iv::decition::DecideGps00::getGpsTraceNowLeftRight(std::vector<Point2D> gpsTrace) {
+    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));
+
+        gpsTraceNowLeft.push_back(ptLeft);
+        gpsTraceNowRight.push_back(ptRight);
+    }
+
+}
+
+
+
+
 std::vector<iv::Point2D>  iv::decition::DecideGps00::getGpsTraceOffsetAvoid(std::vector<Point2D> gpsTrace, double offset,GPS_INS nowgps) {
 
     if (offset==0)
@@ -2203,10 +2250,10 @@ std::vector<iv::Point2D>  iv::decition::DecideGps00::getGpsTraceOffsetAvoid(std:
     bool use_new_method = true;
     if  (use_new_method)
     {
-        const int val = 200;
+        const int val = 300;
         if(trace.size()>val)
         {
-            double V = trace[200].y;
+            double V = trace[300].y;
             for (int j = 0; j < val; j++)
             {
                     double t = (double)j / val;
@@ -2300,10 +2347,10 @@ std::vector<iv::Point2D>  iv::decition::DecideGps00::getGpsTraceOffsetBackOri(st
     bool use_new_method = true;
     if  (use_new_method)
     {
-        const int val = 200;
+        const int val = 300;
         if(trace.size()>val)
         {
-            double V = trace[200].y;
+            double V = trace[300].y;
             for (int j = 0; j < val; j++)
             {
                     double t = (double)j / val;
@@ -3081,7 +3128,7 @@ int iv::decition::DecideGps00::chooseBackRoad(iv::LidarGridPtr lidarGridPtr, GPS
         return -1;
     }
 
-    if ((GetDistance(now_gps_ins, startAvoidGpsInsVector[roadOri])>max(avoidMinDistanceVector[roadOri],28.0)) &&
+    if ((GetDistance(now_gps_ins, startAvoidGpsInsVector[roadOri])>max(avoidMinDistanceVector[roadOri],40.0)) &&
             (checkReturnEnable(avoidX, lidarGridPtr, roadOri)))
     {
         roadPre = roadOri;

src/decition/decition_brain_sf/decition/decide_gps_00.h

@@ -74,6 +74,7 @@ public:
     std::vector<iv::GPS_INS> startAvoidGpsInsVector;
     std::vector<double> avoidMinDistanceVector;
     std::vector<int> v2xTrafficVector;
+    iv::GPS_INS startAvoidGpsIns,startBackGpsIns;
 
     ///kkcai, 2020-01-03
     //bool isFirstRun = true;
@@ -194,7 +195,7 @@ public:
     std::vector<Point2D>  getGpsTraceOffset(std::vector<Point2D> gpsTrace, double offset);
     std::vector<Point2D>  getGpsTraceOffsetAvoid(std::vector<Point2D> gpsTrace, double offset,GPS_INS nowgps);
     std::vector<Point2D>  getGpsTraceOffsetBackOri(std::vector<Point2D> gpsTrace, double offset,GPS_INS nowgps,std::vector<Point2D> gpsTraceNowNow);
-
+    void getGpsTraceNowLeftRight(std::vector<Point2D> gpsTrace);
     std::vector<iv::Point2D> getGpsTraceBackExa(iv::GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine, int pathpo);
 
     float ComputeTrafficLightSpeed(int traffic_light_color, int traffic_light_time,  const std::vector<GPSData> gpsMapLine,int traffic_light_pathpoint,