modify avoid back path

src/decition/decition_brain_sf/decition/decide_gps_00.cpp

@@ -946,8 +946,6 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     }
 
     DecideGps00::lastGpsIndex = PathPoint;
-    //2020-01-03, kkcai
-    //if(!circleMode && PathPoint>gpsMapLine.size()-200){
 
     double brake_distance=200;
     brake_distance=max(250,(int)(dSpeed*dSpeed+150));
@@ -956,8 +954,14 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
             std::cout<<"map finish brake"<<std::endl;
     }
 
-
-
+    if(!circleMode && PathPoint+500<gpsMapLine.size()){
+            if(gpsMapLine[PathPoint+500]->mode2==23 && realspeed>20)
+                                        minDecelerate=-0.5;
+            else if(gpsMapLine[PathPoint+300]->mode2==23 && realspeed>15)
+                                        minDecelerate=-0.5;
+            else if(gpsMapLine[PathPoint+100]->mode2==23)
+                                        minDecelerate=-0.5;
+    }
 
     if(!ServiceCarStatus.inRoadAvoid){
         roadOri = gpsMapLine[PathPoint]->roadOri;
@@ -993,7 +997,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
     gpsTraceOri= getGpsTrace(now_gps_ins, gpsMapLine, lastGpsIndex,circleMode);
 
-    if((vehState == avoiding)&&(gpsTraceAvoidXY.size()>0))
+    if((vehState == avoiding || vehState == backOri)&&(gpsTraceAvoidXY.size()>0))
     {
         gpsTraceOri=getGpsTraceAvoid(now_gps_ins, gpsTraceAvoidXY, lastGpsIndex,circleMode);
     }
@@ -1105,12 +1109,12 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
         if(vehState==avoiding){
             ServiceCarStatus.msysparam.vehWidth=2.1;
-            controlAng=max(-35.0,controlAng);//150  zj-80
-            controlAng=min(35.0,controlAng);//150   zj-80
+            controlAng=max(-150.0,controlAng);//35  zj-80
+            controlAng=min(150.0,controlAng);//35   zj-80
         }
         if(vehState==backOri){
-            controlAng=max(-35.0,controlAng);//120   zj-80
-            controlAng=min(35.0,controlAng);//120     zj-80
+            controlAng=max(-150.0,controlAng);//35   zj-80
+            controlAng=min(150.0,controlAng);//35     zj-80
         }
     }
 
@@ -1441,7 +1445,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
             //            useFrenet = false;
             //            useOldAvoid = true;
         }
-        else if (useOldAvoid && abs(gpsTraceNow[100].x)<0.2) {    //zj 2021.06.21
+        else if (useOldAvoid && abs(gpsTraceNow[60].x)<0.05) {    //zj 2021.06.21
             // vehState = avoidObs; 0929
             vehState = normalRun;
         }
@@ -1512,10 +1516,11 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
         roadNow = roadPre;
    //     avoidX = (roadOri - roadNow)*roadWidth;
-         avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
-        gpsTraceNow.clear();
+        avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
         if(useOldAvoid){
-            gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidX);
+            //gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidX);
+            gpsTraceAvoidXY.clear();
+            gpsTraceAvoidXY = getGpsTraceOffsetBackOri(gpsTraceOri, avoidX,now_gps_ins,gpsTraceNow);
         }
         else if(useFrenet){
             gpsTraceNow = frenetPlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
@@ -2235,16 +2240,16 @@ std::vector<iv::Point2D>  iv::decition::DecideGps00::getGpsTraceOffsetAvoid(std:
     bool use_new_method = true;
     if  (use_new_method)
     {
-        const int val = 100;
+        const int val = 200;
         if(trace.size()>val)
         {
-            double V = trace[100].y;
+            double V = trace[200].y;
             for (int j = 0; j < val; j++)
             {
                     double t = (double)j / val;
                     double s = t*t*(3.-2.*t);
                     double ox = s;
-                    double oy = t * V;
+                    double oy = t *( V-3.0)+3.0;
 
                     trace[j].x=ox*trace[j].x;
                     trace[j].y=oy;
@@ -2253,6 +2258,12 @@ std::vector<iv::Point2D>  iv::decition::DecideGps00::getGpsTraceOffsetAvoid(std:
     }
 
     traceXY.clear();
+    for(int j=0;j<30;j++)
+    {
+                GPS_INS gpsxy = Coordinate_UnTransfer(gpsTrace[j].x,gpsTrace[j].y,nowgps);
+                Point2D gpsinxy(gpsxy.gps_x,gpsxy.gps_y);
+                traceXY.push_back(gpsinxy);
+    }
     for(int j=0;j<trace.size();j++)
     {
                 GPS_INS gpsxy = Coordinate_UnTransfer(trace[j].x,trace[j].y,nowgps);
@@ -2262,6 +2273,105 @@ std::vector<iv::Point2D>  iv::decition::DecideGps00::getGpsTraceOffsetAvoid(std:
     return traceXY;
 }
 
+std::vector<iv::Point2D>  iv::decition::DecideGps00::getGpsTraceOffsetBackOri(std::vector<Point2D> gpsTrace, double offset,GPS_INS nowgps,std::vector<Point2D> gpsTraceNowNow) {
+
+
+    std::vector<iv::Point2D> trace;
+    std::vector<iv::Point2D> traceXY;
+
+    if (offset==0)
+    {
+        trace.assign(gpsTrace.begin(), gpsTrace.end());
+    }
+    else
+    {
+        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 (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);
+
+            double avoidLenth = abs(offset);
+            if (offset<0)
+            {
+                Point2D ptLeft(carFrontx + avoidLenth * cos(anglevalue + PI / 2),
+                               carFronty + avoidLenth  * sin(anglevalue + PI / 2));
+                ptLeft.speed = gpsTrace[j].speed;
+                ptLeft.v1 = gpsTrace[j].v1;
+                ptLeft.v2 = gpsTrace[j].v2;
+                trace.push_back(ptLeft);
+            }
+            else
+            {
+                Point2D ptRight(carFrontx + avoidLenth  * cos(anglevalue - PI / 2),
+                                carFronty + avoidLenth  * sin(anglevalue - PI / 2));
+                ptRight.speed = gpsTrace[j].speed;
+                ptRight.v1 = gpsTrace[j].v1;
+                ptRight.v2 = gpsTrace[j].v2;
+
+
+                trace.push_back(ptRight);
+            }
+        }
+    }
+
+
+    bool use_new_method = true;
+    if  (use_new_method)
+    {
+        const int val = 200;
+        if(trace.size()>val)
+        {
+            double V = trace[200].y;
+            for (int j = 0; j < val; j++)
+            {
+                    double t = (double)j / val;
+                    double s = t*t*(3.-2.*t);
+                    double ox = s;
+                    double oy = t *( V-3.0)+3.0;
+
+                    trace[j].x=ox*trace[j].x;
+                    trace[j].y=oy;
+            }
+        }
+    }
+
+    traceXY.clear();
+    for(int j=0;j<30;j++)
+    {
+                GPS_INS gpsxy = Coordinate_UnTransfer(gpsTraceNowNow[j].x,gpsTraceNowNow[j].y,nowgps);
+                Point2D gpsinxy(gpsxy.gps_x,gpsxy.gps_y);
+                traceXY.push_back(gpsinxy);
+    }
+    for(int j=0;j<trace.size();j++)
+    {
+                GPS_INS gpsxy = Coordinate_UnTransfer(trace[j].x,trace[j].y,nowgps);
+                Point2D gpsinxy(gpsxy.gps_x,gpsxy.gps_y);
+                traceXY.push_back(gpsinxy);
+    }
+    return traceXY;
+}
+
+
+
 double iv::decition::DecideGps00::getAngle(std::vector<Point2D> gpsTrace,GPS_INS gpsIns,iv::decition::Decition decition) {
     double angle=0;
     if ( abs(iv::decition::DecideGps00().minDis) < 20 && abs(iv::decition::DecideGps00().maxAngle) < 100)

src/decition/decition_brain_sf/decition/decide_gps_00.h

@@ -193,6 +193,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);
 
     std::vector<iv::Point2D> getGpsTraceBackExa(iv::GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine, int pathpo);