modify avoid path program

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

@@ -895,6 +895,20 @@ iv::Point2D iv::decition::Compute00::getLidarObsPointXY(std::vector<Point2D> gps
     }
 
     bool isRemove = false;
+
+    ServiceCarStatus.obsTraceLeft.clear();
+    ServiceCarStatus.obsTraceRight.clear();
+
+    for(int j=0;j<gpsTraceLeft.size();j++)
+    {
+        TracePoint obsptleft(gpsTraceLeft[j].x,gpsTraceLeft[j].y);
+        ServiceCarStatus.obsTraceLeft.push_back(obsptleft);
+    }
+    for(int j=0;j<gpsTraceRight.size();j++)
+    {
+        TracePoint obsptright(gpsTraceRight[j].x,gpsTraceRight[j].y);
+        ServiceCarStatus.obsTraceRight.push_back(obsptright);
+    }
     for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
     {
 

src/decition/decition_brain_sf/decition/adc_tools/transfer.cpp

@@ -36,15 +36,26 @@ iv::Point2D iv::decition::Coordinate_Transfer(double x_path, double y_path, iv::
 ///车体转大地
 iv::GPS_INS iv::decition::Coordinate_UnTransfer(double x_path, double y_path, iv::GPS_INS pos)
 {
+//    iv::GPS_INS gps_ins;
+//    double x_vehicle, y_vehicle;
+//    double theta = atan2(x_path, y_path);
+
+//    double distance = sqrt(x_path * x_path + y_path * y_path);
+//    double angle = (pos.ins_heading_angle / 180 * PI + theta);
+
+//    x_vehicle = pos.gps_x + distance * sin(angle);
+//    y_vehicle = pos.gps_y + distance * cos(angle);
+//    gps_ins.gps_x = x_vehicle;
+//    gps_ins.gps_y = y_vehicle;
+
+
     iv::GPS_INS gps_ins;
     double x_vehicle, y_vehicle;
-    double theta = atan2(x_path, y_path);
-
-    double distance = sqrt(x_path * x_path + y_path * y_path);
-    double angle = (pos.ins_heading_angle / 180 * PI + theta);
+    double x_t= x_path;
+    double y_t= y_path;
 
-    x_vehicle = pos.gps_x + distance * sin(angle);
-    y_vehicle = pos.gps_y + distance * cos(angle);
+    x_vehicle = x_t * cos(pos.ins_heading_angle * PI / 180) + y_t * sin(pos.ins_heading_angle * PI / 180)+pos.gps_x;
+    y_vehicle = -x_t * sin(pos.ins_heading_angle * PI / 180) + y_t * cos(pos.ins_heading_angle * PI / 180)+pos.gps_y;
     gps_ins.gps_x = x_vehicle;
     gps_ins.gps_y = y_vehicle;
 

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,gpsTraceNowLeft,gpsTraceNowRight;
+std::vector<iv::Point2D>  gpsTraceAvoid,gpsTraceNow,gpsTraceBack,gpsTracePre,gpsTraceOri,gpsTraceRear,gpsTraceNowLeft,gpsTraceNowRight,gpsTraceAvoidXY;
 
 std::vector<double> esrDisVector(roadSum, -1);
 std::vector<double> lidarDisVector(roadSum, -1);
@@ -908,16 +908,6 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
     }
 
-
-
-
-
-
-
-
-
-
-
     obsDistance = -1;
     esrIndex = -1;
     lidarDistance = -1;
@@ -926,16 +916,6 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     esrIndexAvoid = -1;
     roadPre = -1;
 
-
-
-
-
-
-
-
-
-
-
     gpsTraceOri.clear();
     gpsTraceNow.clear();
     gpsTraceAim.clear();
@@ -946,10 +926,6 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
 
 
-
-
-
-
     if (!isFirstRun)
     {
         //   PathPoint = Compute00().getNearestPointIndex(now_gps_ins, gpsMapLine, DecideGps00::lastGpsIndex, DecideGps00::minDis, DecideGps00::maxAngle);
@@ -1006,10 +982,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
          gpsMapLine[PathPoint]->runMode=0;
     }
 
-
-    // avoidX = (roadOri-roadNow)*roadWidth;
-     avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
-
+    avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
 
     if ( gpsMapLine[PathPoint]->runMode == 0 &&(vehState == avoidObs||vehState ==stopObs||vehState == preAvoid
                                                 ||vehState == avoiding||vehState == backOri||vehState ==preBack||vehState ==waitAvoid ) )
@@ -1018,21 +991,13 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         roadNow = roadOri;
     }
 
+    gpsTraceOri= getGpsTrace(now_gps_ins, gpsMapLine, lastGpsIndex,circleMode);
 
+    if((vehState == avoiding)&&(gpsTraceAvoidXY.size()>0))
+    {
+        gpsTraceOri=getGpsTraceAvoid(now_gps_ins, gpsTraceAvoidXY, lastGpsIndex,circleMode);
+    }
 
-    //    if (PathPoint!=-1&&((now_gps_ins.rtk_status==6)||(now_gps_ins.rtk_status==5))&&GetDistance(now_gps_ins,*gpsMapLine[PathPoint])<30)
-    //    {
-    //        DecideGps00::lastGpsIndex = PathPoint;
-    //        gpsMissCount = 0;
-
-    //    }
-    //    else
-    //    {
-    //        gpsMissCount++;
-    //    }
-
-    gpsTraceOri= getGpsTrace(now_gps_ins, gpsMapLine, lastGpsIndex,circleMode);
-    //    gpsTraceOri = laneChangePlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,0,lidarGridPtr);
     if(gpsTraceOri.empty()){
         gps_decition->wheel_angle = 0;
         gps_decition->speed = dSpeed;
@@ -1097,9 +1062,16 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
             gpsTraceNowRight.assign(traceOriRight.begin(),traceOriRight.end());
         }else
         {
-            gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidX);
-            gpsTraceNowLeft=getGpsTraceOffset(traceOriLeft, avoidX);
-            gpsTraceNowRight=getGpsTraceOffset(traceOriRight, avoidX);
+            if(vehState == avoiding)
+            {
+                gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
+                gpsTraceNowLeft=getGpsTraceOffset(traceOriLeft, avoidX);
+                gpsTraceNowRight=getGpsTraceOffset(traceOriRight, avoidX);
+            }else{
+                gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidX);
+                gpsTraceNowLeft=getGpsTraceOffset(traceOriLeft, avoidX);
+                gpsTraceNowRight=getGpsTraceOffset(traceOriRight, avoidX);
+            }
         }
     }
 
@@ -1107,18 +1079,14 @@ 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.clear();
+    planTrace.clear();//Add By YuChuli, 2020.11.26
     for(int i=0;i<gpsTraceNow.size();i++){
         TracePoint pt(gpsTraceNow[i].x,gpsTraceNow[i].y);
         planTrace.push_back(pt);
     }
 
-
     dSpeed = limitSpeed(controlAng, dSpeed);
 
-
-
     controlAng= getAngle(gpsTraceNow,now_gps_ins,gps_decition);
     if(!circleMode && PathPoint>(gpsMapLine.size()-250)){
         if(realspeed<0.5)
@@ -1473,7 +1441,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
             //            useFrenet = false;
             //            useOldAvoid = true;
         }
-        else if (useOldAvoid && abs(gpsTraceNow[0].x)<1.0) {
+        else if (useOldAvoid && abs(gpsTraceNow[100].x)<0.2) {    //zj 2021.06.21
             // vehState = avoidObs; 0929
             vehState = normalRun;
         }
@@ -1679,7 +1647,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                      avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
                     gpsTraceNow.clear();
                     //gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidX);
-                    gpsTraceNow = getGpsTraceOffsetAvoid(gpsTraceOri, avoidX);
+                    gpsTraceAvoidXY.clear();
+                    gpsTraceAvoidXY = getGpsTraceOffsetAvoid(gpsTraceOri, avoidX,now_gps_ins);
                 }
                 else if(useFrenet){
                     if(roadPre != roadNow){
@@ -1753,16 +1722,6 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         hasZhuched = false;
     }
 
-
-
-
-
-
-
-
-
-
-
     if ( vehState==changingRoad || vehState==chaocheBack)
     {
         double lastAng = 0.0 - lastAngle;
@@ -1941,10 +1900,6 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
     transferGpsMode2(gpsMapLine);
 
-   // if(obsDistance>6.5){
-     //   obsDistance=500;
-    //}
-
     if(ServiceCarStatus.msysparam.mvehtype=="hapo"){
         if(obsDistance>0 && obsDistance<8){
                 dSpeed=0;
@@ -2041,9 +1996,6 @@ std::vector<iv::Point2D> iv::decition::DecideGps00::getGpsTrace(iv::GPS_INS now_
     traceOriLeft.clear();
     traceOriRight.clear();
 
-    ServiceCarStatus.obsTraceLeft.clear();
-    ServiceCarStatus.obsTraceRight.clear();
-
     if (gpsMapLine.size() > 600 && PathPoint >= 0) {
         int aimIndex;
         if(circleMode){
@@ -2129,16 +2081,42 @@ std::vector<iv::Point2D> iv::decition::DecideGps00::getGpsTrace(iv::GPS_INS now_
             traceOriLeft.push_back(ptLeft);
             traceOriRight.push_back(ptRight);
 
-            TracePoint obsptleft(ptLeft.x,ptLeft.y);
-            ServiceCarStatus.obsTraceLeft.push_back(obsptleft);
+        }
+    }
+    return trace;
+}
+
+
+std::vector<iv::Point2D> iv::decition::DecideGps00::getGpsTraceAvoid(iv::GPS_INS now_gps_ins, const std::vector<Point2D> gpsTrace, int lastIndex, bool circleMode) {
+    vector<iv::Point2D> trace;
+
+    int index = -1;
+    float minDis = 10;
 
-            TracePoint obsptright(ptRight.x,ptRight.y);
-            ServiceCarStatus.obsTraceRight.push_back(obsptright);
+    for (unsigned int i = 0; i < gpsTrace.size(); i++)
+    {
+        double tmpdis = sqrt((now_gps_ins.gps_x - gpsTrace[i].x) * (now_gps_ins.gps_x - gpsTrace[i].x) + (now_gps_ins.gps_y - gpsTrace[i].y) * (now_gps_ins.gps_y - gpsTrace[i].y));
+
+        if (tmpdis < minDis)
+        {
+            index = i;
+            minDis = tmpdis;
+        }
+    }
+
+    trace.clear();
+    if (index >= 0) {
+        for (unsigned int i = index; i < gpsTrace.size(); i++)
+        {
+            Point2D pt = Coordinate_Transfer(gpsTrace[i].x, gpsTrace[i].y, now_gps_ins);
+            trace.push_back(pt);
         }
     }
     return trace;
 }
 
+
+
 std::vector<iv::Point2D>  iv::decition::DecideGps00::getGpsTraceOffset(std::vector<Point2D> gpsTrace, double offset) {
 
     if (offset==0)
@@ -2197,7 +2175,7 @@ std::vector<iv::Point2D>  iv::decition::DecideGps00::getGpsTraceOffset(std::vect
     return trace;
 }
 
-std::vector<iv::Point2D>  iv::decition::DecideGps00::getGpsTraceOffsetAvoid(std::vector<Point2D> gpsTrace, double offset) {
+std::vector<iv::Point2D>  iv::decition::DecideGps00::getGpsTraceOffsetAvoid(std::vector<Point2D> gpsTrace, double offset,GPS_INS nowgps) {
 
     if (offset==0)
     {
@@ -2205,6 +2183,7 @@ std::vector<iv::Point2D>  iv::decition::DecideGps00::getGpsTraceOffsetAvoid(std:
     }
 
     std::vector<iv::Point2D> trace;
+    std::vector<iv::Point2D> traceXY;
     for (int j = 0; j < gpsTrace.size(); j++)
     {
         double sumx1 = 0, sumy1 = 0, count1 = 0;
@@ -2253,7 +2232,7 @@ std::vector<iv::Point2D>  iv::decition::DecideGps00::getGpsTraceOffsetAvoid(std:
     }
 
 
-    bool use_new_method = false;
+    bool use_new_method = true;
     if  (use_new_method)
     {
         const int val = 100;
@@ -2272,7 +2251,15 @@ std::vector<iv::Point2D>  iv::decition::DecideGps00::getGpsTraceOffsetAvoid(std:
             }
         }
     }
-    return trace;
+
+    traceXY.clear();
+    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) {
@@ -2943,10 +2930,7 @@ int iv::decition::DecideGps00::chooseAvoidRoad(iv::LidarGridPtr lidarGridPtr, GP
         gpsTraceAvoid.clear();
         avoidX=computeAvoidX(i,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
         gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
-        gpsTraceNowLeft=getGpsTraceOffset(traceOriLeft, avoidX);
-        gpsTraceNowRight=getGpsTraceOffset(traceOriRight, avoidX);
-        //computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per);
-        computeObsOnRoadXY(lidarGridPtr, gpsTraceAvoid,gpsTraceNowLeft,gpsTraceNowRight,i,gpsMapLine,lidar_per);
+        computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per);
     }
 
     if (lidarGridPtr!=NULL)
@@ -3015,10 +2999,7 @@ int iv::decition::DecideGps00::chooseBackRoad(iv::LidarGridPtr lidarGridPtr, GPS
         gpsTraceBack.clear();
         avoidX=computeAvoidX(i,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
         gpsTraceBack = getGpsTraceOffset(gpsTraceOri, avoidX);
-        gpsTraceNowLeft=getGpsTraceOffset(traceOriLeft, avoidX);
-        gpsTraceNowRight=getGpsTraceOffset(traceOriRight, avoidX);
-        //computeObsOnRoad(lidarGridPtr, gpsTraceBack, i,gpsMapLine,lidar_per);
-        computeObsOnRoadXY(lidarGridPtr, gpsTraceBack,gpsTraceNowLeft,gpsTraceNowRight,i,gpsMapLine,lidar_per);
+        computeObsOnRoad(lidarGridPtr, gpsTraceBack, i,gpsMapLine,lidar_per);
     }
 
 

src/decition/decition_brain_sf/decition/decide_gps_00.h

@@ -146,6 +146,7 @@ public:
 
     void initMethods();
     static std::vector<Point2D> getGpsTrace(GPS_INS rp, const std::vector<GPSData> gpsMap, int lastIndex, bool circleMode);
+    static std::vector<Point2D> getGpsTraceAvoid(GPS_INS rp, const std::vector<Point2D> gpsTrace, int lastIndex, bool circleMode);
 
 
     static double getAngle(std::vector<Point2D> gpsTrace);
@@ -191,7 +192,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<Point2D>  getGpsTraceOffsetAvoid(std::vector<Point2D> gpsTrace, double offset,GPS_INS nowgps);
 
     std::vector<iv::Point2D> getGpsTraceBackExa(iv::GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine, int pathpo);