add avoid path plan

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

@@ -811,7 +811,7 @@ iv::Point2D iv::decition::Compute00::getLidarObsPoint(std::vector<Point2D> gpsTr
         ServiceCarStatus.obsTraceLeft.push_back(obsptleft);
 
         TracePoint obsptright(ptRight.x,ptRight.y);
-        ServiceCarStatus.obsTraceLeft.push_back(obsptright);
+        ServiceCarStatus.obsTraceRight.push_back(obsptright);
     }
 
 

src/decition/decition_brain_sf/decition/decide_gps_00.cpp

@@ -1678,7 +1678,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                   //  avoidX = (roadOri - roadNow)*roadWidth;
                      avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
                     gpsTraceNow.clear();
-                    gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidX);
+                    //gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidX);
+                    gpsTraceNow = getGpsTraceOffsetAvoid(gpsTraceOri, avoidX);
                 }
                 else if(useFrenet){
                     if(roadPre != roadNow){
@@ -2132,7 +2133,7 @@ std::vector<iv::Point2D> iv::decition::DecideGps00::getGpsTrace(iv::GPS_INS now_
             ServiceCarStatus.obsTraceLeft.push_back(obsptleft);
 
             TracePoint obsptright(ptRight.x,ptRight.y);
-            ServiceCarStatus.obsTraceLeft.push_back(obsptright);
+            ServiceCarStatus.obsTraceRight.push_back(obsptright);
         }
     }
     return trace;
@@ -2192,9 +2193,87 @@ std::vector<iv::Point2D>  iv::decition::DecideGps00::getGpsTraceOffset(std::vect
             trace.push_back(ptRight);
         }
     }
+
     return trace;
 }
 
+std::vector<iv::Point2D>  iv::decition::DecideGps00::getGpsTraceOffsetAvoid(std::vector<Point2D> gpsTrace, double offset) {
+
+    if (offset==0)
+    {
+        return gpsTrace;
+    }
+
+    std::vector<iv::Point2D> trace;
+    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 = false;
+    if  (use_new_method)
+    {
+        const int val = 100;
+        if(trace.size()>val)
+        {
+            double V = trace[100].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;
+
+                    trace[j].x=ox*trace[j].x;
+                    trace[j].y=oy;
+            }
+        }
+    }
+    return trace;
+}
 
 double iv::decition::DecideGps00::getAngle(std::vector<Point2D> gpsTrace,GPS_INS gpsIns,iv::decition::Decition decition) {
     double angle=0;

src/decition/decition_brain_sf/decition/decide_gps_00.h

@@ -191,6 +191,7 @@ public:
     void init();
 
     std::vector<Point2D>  getGpsTraceOffset(std::vector<Point2D> gpsTrace, double offset);
+    std::vector<Point2D>  getGpsTraceOffsetAvoid(std::vector<Point2D> gpsTrace, double offset);
 
     std::vector<iv::Point2D> getGpsTraceBackExa(iv::GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine, int pathpo);