add back Radar logic with comment 20200418

src/decition/decition_brain_sf_jsrunlegs/decition/adc_tools/compute_00.cpp

@@ -1094,6 +1094,126 @@ iv::Point2D iv::decition::Compute00::getLidarObsPointXY(std::vector<Point2D> gps
     return obsPoint;
 }
 
+iv::Point2D iv::decition::Compute00::getLidarObsPointBackward(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr) {
+
+    iv::Point2D obsPoint(100, 100);
+    vector<Point2D> gpsTraceLeft;
+    vector<Point2D> gpsTraceRight;
+    float xiuzheng=0;
+//    if(!ServiceCarStatus.useMobileEye){
+//        xiuzheng=0-ServiceCarStatus.msysparam.lidarGpsXiuzheng;
+//    }
+    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.obsTraceRight.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;
+}
 
 
 

src/decition/decition_brain_sf_jsrunlegs/decition/adc_tools/compute_00.h

@@ -47,7 +47,7 @@ namespace iv {
             static Point2D getLidarObsPointXY(std::vector<Point2D> gpsTrace,std::vector<Point2D> gpsTraceLeft,std::vector<Point2D> gpsTraceRight, iv::LidarGridPtr lidarGridPtr);
 
 
-
+            static Point2D getLidarObsPointBackward(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr);
             static Point2D getLidarObsPointAvoid(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr);
             static int getEsrIndex(std::vector<Point2D> gpsTrace , int roadNum, int *esrPathpoint);
             static int getRearEsrIndex(std::vector<Point2D> gpsTrace , int roadNum);

src/decition/decition_brain_sf_jsrunlegs/decition/decide_gps_00.cpp

@@ -185,7 +185,7 @@ enum VehState { normalRun, park, readyPark, stopObs, avoidObs, preAvoid, avoidin
                 waitAvoid ,shoushaTest,justRun, changingRoad, chaoche, chaocheBack, reverseCar,reversing, reverseCircle, reverseDirect,reverseArr,
                 dRever,dRever0,dRever1,dRever2,dRever3,dRever4} vehState,lastVehState;
 
-
+std::vector<iv::Point2D>  gpsTraceBackward;//20200418
 std::vector<iv::Point2D>  gpsTraceAim; //记录变道后的目标路径，若车辆经frenet规划行驶至该路径上后，就关闭frenet规划
 std::vector<iv::Point2D>  gpsTraceAvoid,gpsTraceNow,gpsTraceBack,gpsTracePre,gpsTraceOri,gpsTraceRear,gpsTraceNowLeft,gpsTraceNowRight,gpsTraceAvoidXY;
 
@@ -2015,6 +2015,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                 //computeAvoidBoundary(0,2,4.5,2.4,avoidLeft_p,avoidRight_p);
                 //avoidLeft_value=-5;
                 //avoidRight_value=0;
+                computeObsBackward(lidarGridPtr,now_gps_ins, gpsMapLine,lidar_per,avoidLeft_value,avoidRight_value);//2020418
                 avoidXNewPre=computeAvoidXDistance(lidarGridPtr,now_gps_ins, gpsMapLine,lidar_per,avoidLeft_value,avoidRight_value);
                 givlog->debug("decition_brain","vehState: %d,roadOri: %d,roadSum: %d,vehWidth: %f,avoidLeft: %d,avoidRight_value: %d,avoidXNewPre: %d",
                                 vehState,roadOri,roadSum,ServiceCarStatus.msysparam.vehWidth,avoidLeft_value,avoidRight_value,avoidXNewPre);
@@ -4588,6 +4589,73 @@ void iv::decition::DecideGps00::computeAvoidBoundary(int roadOriginal,int roadSu
 
 int iv::decition::DecideGps00::computeAvoidXDistance(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per,int avoidLeft,int avoidRight) {
 
+//    int signed_record_avoidX=0,record_avoidX=20;
+//    double obs_cur_distance=500,record_obstacle_distance;
+
+//    obs_property.clear();
+//    for (int i =  avoidLeft; i <= avoidRight; i++)
+//    {
+//        obsvalue x_value;
+//        obsvalue *x=&x_value;
+//        x_value.avoid_distance=i;
+//        gpsTraceAvoid.clear();
+//        gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, i);
+//        computeObsOnRoadNew(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per,x);
+//        obs_property.push_back(x_value);
+//        if(i==0)
+//            obs_cur_distance=x_value.obs_distance;
+//        givlog->debug("decition_brain","i: %d,obs:%f",x_value.avoid_distance,x_value.obs_distance);
+//    }
+
+//    if (lidarGridPtr!=NULL)
+//    {
+//        hasCheckedAvoidLidar = true;
+//    }
+//    int record_vector=0;
+//    for(int j=0;j<obs_property.size();j++){
+//        if(obs_property[j].obs_distance>100){
+//            if((signed_record_avoidX<0)&&(obs_property[j].avoid_distance>0)){
+//                break;
+//            }
+//                if(abs(obs_property[j].avoid_distance)<record_avoidX){
+//                        record_avoidX=abs(obs_property[j].avoid_distance);
+//                        signed_record_avoidX=obs_property[j].avoid_distance;
+//                        record_vector=j;
+//                }
+//        }
+//    }
+
+//   if((signed_record_avoidX<0)&&(signed_record_avoidX-1>=avoidLeft))
+//   {
+//          int obs_test=record_vector-1;
+
+//          if(obs_property[obs_test].obs_distance>100){
+//                    signed_record_avoidX-=1;
+//                    givlog->debug("decition_brain","singrecord_Y: %d",signed_record_avoidX);
+//          }
+//   }
+//   if(front_car_id>0){
+//           if((obs_cur_distance<50)&&(front_car.front_car_dis<30)&&(front_car_decide_avoid==true)){
+//                    signed_record_avoidX=front_car.avoidX;
+//                    return signed_record_avoidX;
+//           }
+
+//           if((obs_cur_distance<50)&&(front_car.front_car_dis<150)&&(front_car.avoidX!=0||front_car.vehState!=0)){
+//                    signed_record_avoidX=front_car.avoidX;
+//                    return signed_record_avoidX;
+//           }
+//   }
+
+//    if((signed_record_avoidX!=0)&&(obs_cur_distance<50))
+//    {
+
+//                        avoidMinDistance= obs_cur_distance + 2*ServiceCarStatus.msysparam.vehLenth;
+//                        startAvoidGps=now_gps_ins;
+//                        return signed_record_avoidX;
+//    }
+//    return 0;
+
+    //20200418，add,begin
     int signed_record_avoidX=0,record_avoidX=20;
     double obs_cur_distance=500,record_obstacle_distance;
 
@@ -4610,29 +4678,57 @@ int iv::decition::DecideGps00::computeAvoidXDistance(iv::LidarGridPtr lidarGridP
     {
         hasCheckedAvoidLidar = true;
     }
-    int record_vector=0;
+    int record_j=0;
     for(int j=0;j<obs_property.size();j++){
         if(obs_property[j].obs_distance>100){
-            if((signed_record_avoidX<0)&&(obs_property[j].avoid_distance>0)){
-                break;
-            }
                 if(abs(obs_property[j].avoid_distance)<record_avoidX){
                         record_avoidX=abs(obs_property[j].avoid_distance);
                         signed_record_avoidX=obs_property[j].avoid_distance;
-                        record_vector=j;
+                        record_j=j;
                 }
         }
     }
 
-   if((signed_record_avoidX<0)&&(signed_record_avoidX-1>=avoidLeft))
-   {
-          int obs_test=record_vector-1;
-
-          if(obs_property[obs_test].obs_distance>100){
-                    signed_record_avoidX-=1;
-                    givlog->debug("decition_brain","singrecord_Y: %d",signed_record_avoidX);
-          }
+//   if((signed_record_avoidX<0)&&(signed_record_avoidX-1>=avoidLeft))
+//   {
+//          int obs_test=record_j-1;
+//          if(obs_property[obs_test].obs_distance>100){
+//                    signed_record_avoidX-=1;
+//                    givlog->debug("decition_brain","singrecord_Y: %d",signed_record_avoidX);
+//          }
+//   }
+   if(signed_record_avoidX<0){
+           if(signed_record_avoidX-1>=avoidLeft){
+               int obs_test=record_j-1;
+               int signed_record_avoidX_minusone=signed_record_avoidX-1;
+               if(obs_property[obs_test].obs_distance>100){
+                   for(int k=signed_record_avoidX_minusone;k<0;k++){
+                       if(obs_back_property[k].avoid_ok==false){
+                                signed_record_avoidX=0;
+                                break;
+                       }
+                       if(k==-1){
+                                signed_record_avoidX-=1;
+                       }
+                   }
+               }
+           }else{
+               for(int k=signed_record_avoidX;k<0;k++){
+                   if(obs_back_property[k].avoid_ok==false){
+                            signed_record_avoidX=0;
+                            break;
+                   }
+               }
+           }
+   }else if(signed_record_avoidX>0){
+       for(int k=1;k<signed_record_avoidX;k++){
+           if(obs_back_property[k].avoid_ok==false){
+                    signed_record_avoidX=0;
+                    break;
+           }
+       }
    }
+
    if(front_car_id>0){
            if((obs_cur_distance<50)&&(front_car.front_car_dis<30)&&(front_car_decide_avoid==true)){
                     signed_record_avoidX=front_car.avoidX;
@@ -4769,7 +4865,63 @@ int iv::decition::DecideGps00::computeBackDistance(iv::LidarGridPtr lidarGridPtr
 
     return 20;
 }
+void iv::decition::DecideGps00::computeObsOnRoadBack(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,
+                                                 const std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per,backobs* obsva) {
 
+    double obs=100,obsSd=100;
+
+    if (lidarGridPtr == NULL)
+    {
+        lidarDistance = lastLidarDis;
+    }
+    else {
+        obsPoint = Compute00().getLidarObsPointBackward(gpsTrace, lidarGridPtr);
+        float lidarXiuZheng=0;
+        if(!ServiceCarStatus.useMobileEye){
+            lidarXiuZheng=0-ServiceCarStatus.msysparam.frontGpsXiuzheng;
+        }
+        lidarDistance = obsPoint.y;// + lidarXiuZheng;   //激光距离推到车头  gjw20191110 lidar
+
+        lastLidarDis = lidarDistance;
+    }
+
+
+    obs = lidarDistance;
+    obsSd= obsPoint.obs_speed_y;
+    obsva->obs_distance=obs;
+    obsva->obs_speed=obsSd;
+
+//    obs = lidarDistance;
+//    obsSd= obsPoint.obs_speed_y;
+//    if(obs<0){
+//        obsva->obs_distance=500;
+//    }else{
+//        obsva->obs_distance=obs;
+//        obsva->obs_speed=obsSd;
+//    }
+}
+void iv::decition::DecideGps00::computeObsBackward(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per,int avoidLeft,int avoidRight) {
+
+    obs_back_property.clear();
+    for (int i =  avoidLeft; i <= avoidRight; i++)
+    {
+        backobs x_value;
+        backobs *x=&x_value;
+        //x_value.avoid_distance=i;
+        gpsTraceBack.clear();
+        gpsTraceBack = getGpsTraceOffset(gpsTraceBackward, i);
+        computeObsOnRoadBack(lidarGridPtr, gpsTraceBack, i,gpsMapLine,lidar_per,x);
+        if((x_value.obs_distance>-20)&&(x_value.obs_distance<0)&&(x_value.obs_speed>0)){
+               x_value.avoid_ok=false;
+        }else{
+               x_value.avoid_ok=true;
+        }
+        obs_back_property.insert(make_pair(i,x_value));
+        //double test=obs_back_property[i].obs_distance;
+        //double test_f=test.obs_distance;
+        givlog->debug("decition_brain","i: %d,avoid_ok: %d,back_obs:%f,back_obs_speed:%f",i,x_value.avoid_ok,x_value.obs_distance,x_value.obs_speed);
+    }
+}
 
 
 

src/decition/decition_brain_sf_jsrunlegs/decition/decide_gps_00.h

@@ -21,6 +21,7 @@
 #include "ivlog.h"
 #include <memory>
 #include <common/tracepointstation.h>
+#include <unordered_map>  //20200418
 
 namespace iv {
 namespace decition {
@@ -133,6 +134,15 @@ public:
     };
     std::vector<obsvalue> obs_property;
 
+    struct backobs{
+        bool avoid_ok;
+        double obs_distance;
+        double obs_speed;
+    };
+
+    //std::vector<iv::Point2D>  obsSpline,obsSplineFilter;
+    std::unordered_map<int,backobs> obs_back_property;  //20200418
+
     struct front{
         GPS_INS front_car_ins;
         int vehState;
@@ -227,6 +237,7 @@ public:
     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);
+    static std::vector<Point2D> getGpsTraceBackward(iv::GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine, int lastIndex, bool circleMode);//20200418
 
     float ComputeTrafficLightSpeed(int traffic_light_color, int traffic_light_time,  const std::vector<GPSData> gpsMapLine,int traffic_light_pathpoint,
                                    int pathpoint,float secSpeed,float dSpeed);
@@ -248,8 +259,10 @@ public:
                                                      const std::vector<GPSData> gpsMapLine, std::vector<iv::Perception::PerceptionOutput> lidar_per,obsvalue* obsva);
     int computeBackDistance(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per,int avoidLeft,int avoidRight,int avoidXNow);
 
-
-    GPS_INS aim_gps_ins;
+void computeObsBackward(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per,int avoidLeft,int avoidRight);//20200418,houxianghaomibo
+void computeObsOnRoadBack(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,
+                                                 const std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per,backobs* obsva);//20200418
+GPS_INS aim_gps_ins;
     GPS_INS chaoche_start_gps;
     bool is_arrivaled=false;