避障距离计算改成0.5米

src/decition/decition_brain_sf_changan_shenlan/decition/adc_tools/compute_00.cpp

@@ -356,25 +356,24 @@ int iv::decition::Compute00::getMapDelta(std::vector<GPSData> MapTrace){
                 }
 
 }*/
-
 int iv::decition::Compute00::getPlanSpeed(std::vector<GPSData> MapTrace)
 {
-                int mode0to5count=0,mode0to18count=0,mode0to5countSum=0,mode0to18countSum=0,mode18count=0,mode18countSum=0,mode0to5flash=0,mode18flash=0;
-                double straightSpeed=min(ServiceCarStatus.mroadmode_vel.mfmode0,60.0);
-                double straightCurveSpeed=min(ServiceCarStatus.mroadmode_vel.mfmode5,40.0);
-                double Curve_SmallSpeed=min(ServiceCarStatus.mroadmode_vel.mfmode18,15.0);
-                double Curve_LargeSpeed=min(ServiceCarStatus.mroadmode_vel.mfmode14,8.0);
-                getMapDelta(MapTrace);
-                for(int i=0;i<doubledata.size();i++)
-                {
-                    if((MapTrace[i]->mfCurvature>-0.02)&&(MapTrace[i]->mfCurvature<0.02)){
-                        MapTrace[i]->roadMode=5;
-                    }else if(((MapTrace[i]->mfCurvature>=0.02)&&(MapTrace[i]->mfCurvature<=0.16))||((MapTrace[i]->mfCurvature>=-0.16)&&(MapTrace[i]->mfCurvature<=-0.02))){
-                        MapTrace[i]->roadMode=18;
-                    }else if(((MapTrace[i]->mfCurvature>0.16))||((MapTrace[i]->mfCurvature<-0.16))){
-                        MapTrace[i]->roadMode=14;
-                    }
-                }
+    int mode0to5count=0,mode0to18count=0,mode0to5countSum=0,mode0to18countSum=0,mode18count=0,mode18countSum=0,mode0to5flash=0,mode18flash=0;
+    double straightSpeed=min(ServiceCarStatus.mroadmode_vel.mfmode0,60.0);
+    double straightCurveSpeed=min(ServiceCarStatus.mroadmode_vel.mfmode5,40.0);
+    double Curve_SmallSpeed=min(ServiceCarStatus.mroadmode_vel.mfmode18,15.0);
+    double Curve_LargeSpeed=min(ServiceCarStatus.mroadmode_vel.mfmode14,8.0);
+    getMapDelta(MapTrace);
+    for(int i=0;i<doubledata.size();i++)
+    {
+        if((MapTrace[i]->mfCurvature>-0.02)&&(MapTrace[i]->mfCurvature<0.02)){
+            MapTrace[i]->roadMode=5;
+        }else if(((MapTrace[i]->mfCurvature>=0.02)&&(MapTrace[i]->mfCurvature<=0.16))||((MapTrace[i]->mfCurvature>=-0.16)&&(MapTrace[i]->mfCurvature<=-0.02))){
+            MapTrace[i]->roadMode=18;
+        }else if(((MapTrace[i]->mfCurvature>0.16))||((MapTrace[i]->mfCurvature<-0.16))){
+            MapTrace[i]->roadMode=14;
+        }
+    }
                 for(int i=0;i<MapTrace.size();i++)
                 {
                     if(MapTrace[i]->roadMode==0){
@@ -458,7 +457,7 @@ int iv::decition::Compute00::getPlanSpeed(std::vector<GPSData> MapTrace)
                         mode18flash=0;
                     }
                 }
-
+//
                 /*for(int i=0;i<MapTrace.size();i++){
                     //将数据写入文件开始
                     ofstream outfile;

src/decition/decition_brain_sf_changan_shenlan/decition/decide_gps_00.cpp

@@ -106,9 +106,12 @@ int obsLostTimeAvoid = 0;
 // double avoidTime = 0;
 
 int avoidXNewPre=0,avoidXNewPreFilter=0;
-vector<int> avoidXNewPreVector;
-int avoidXNew=0;
-int avoidXNewLast=0;
+//vector<int> avoidXNewPreVector;
+//int avoidXNew=0;
+//int avoidXNewLast=0;
+vector<double> avoidXNewPreVector;
+double avoidXNew=0;
+double avoidXNewLast=0;  //20230213,障碍物检测精度由1米换成0.5米
 double avoidX =0;
 int    turnLampFlag=0;  //  if <0:left , if >0:right
 float roadWidth = 3.5;
@@ -1889,10 +1892,14 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     if((vehState == preAvoid)||(avoidXNew!=0))
     {
         dSpeed = min(6.0,dSpeed);
-        int avoidLeft_value=0;
-        int avoidRight_value=0;
-        int* avoidLeft_p=&avoidLeft_value;
-        int* avoidRight_p=&avoidRight_value;
+//        int avoidLeft_value=0;
+//        int avoidRight_value=0;
+//        int* avoidLeft_p=&avoidLeft_value;
+//        int* avoidRight_p=&avoidRight_value;
+        double avoidLeft_value=0;
+        double avoidRight_value=0;
+        double* avoidLeft_p=&avoidLeft_value;
+        double* avoidRight_p=&avoidRight_value;
         computeAvoidBoundary(roadOri,roadSum,gpsMapLine[PathPoint]->mfLaneWidth,ServiceCarStatus.msysparam.vehWidth,avoidLeft_p,avoidRight_p);
         avoidXNewPre=computeAvoidXDistance(lidarGridPtr,now_gps_ins, gpsMapLine,avoidLeft_value,avoidRight_value,avoidXNewLast);
         if(avoidXNewPreVector.size()<5){
@@ -4378,16 +4385,24 @@ void iv::decition::DecideGps00::computeAvoidBoundary(int roadOriginal,int roadSu
     *avoidXLeft = (-(int)LeftBoundary);
 }
 
+void iv::decition::DecideGps00::computeAvoidBoundary(int roadOriginal,int roadSumMap,double roadWidthEvery,double vehicleWidth,double* avoidXLeft, double* avoidXRight )
+{
+    double RightBoundary = (((double)roadOriginal + 0.5) * roadWidthEvery - vehicleWidth / 2);
+    *avoidXRight = ((int)RightBoundary);
+    double LeftBoundary = (((double)(roadSumMap -roadOriginal - 1) + 0.5) * roadWidthEvery - vehicleWidth / 2);
+    *avoidXLeft = (-(int)LeftBoundary);
+}
+
 int iv::decition::DecideGps00::computeAvoidXDistance(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine,
                                                      int avoidLeft,int avoidRight,int offsetLast)
 {
     int signed_record_avoidX = offsetLast, record_avoidX = 20;
     double obs_cur_distance =500, record_obstacle_distance;
     iv::Point2D obs_spline_record;
-    double step = 1.5; //lyp 
+    double step = 0.5; //lyp
 
     obs_property.clear();
-    for (int i =  avoidLeft; i <= avoidRight; i=i+step)
+    for (double i =  avoidLeft; i <= avoidRight; i=i+step)  //由原来的1米改成0.5米，将i由int 转换成double
     {
         obsvalue x_value;
         obsvalue *x = &x_value;
@@ -4510,8 +4525,173 @@ int iv::decition::DecideGps00::computeAvoidXDistance(iv::LidarGridPtr lidarGridP
     return offsetLast;
 }
 
-iv::Point2D iv::decition::DecideGps00::computeObsOnRoadSpline(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,
-                                                              const std::vector<GPSData> gpsMapLine,GPS_INS now_gps_ins,obsvalue* obsva)
+double iv::decition::DecideGps00::computeAvoidXDistance(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine,
+                                                     double avoidLeft,double avoidRight,double offsetLast)
+{
+    double signed_record_avoidX = offsetLast, record_avoidX = 20;
+    double obs_cur_distance =500, record_obstacle_distance;
+    iv::Point2D obs_spline_record;
+    double step = 0.5;
+
+    obs_property.clear();
+    for (double i =  avoidLeft; i <= avoidRight; i=i+step)  //由原来的1米改成0.5米，将i由int 转换成double
+    {
+        obsvalue x_value;
+        obsvalue *x = &x_value;
+        x_value.avoid_distance = i;
+        gpsTraceAvoid.clear();
+        gpsTraceAvoid = getGpsTraceOffset(gpsTraceMapOri, i);
+        obs_spline_record=computeObsOnRoadSpline(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,now_gps_ins,x);
+        obs_property.push_back(x_value);
+        if(i==offsetLast)
+        {
+            obs_cur_distance=x_value.obs_distance;
+            record_obstacle_distance=obs_cur_distance;
+        }
+        givlog->debug("decition_brain","i: %d,obs:%f",x_value.avoid_distance,x_value.obs_distance);
+
+        obsSpline.push_back(obs_spline_record);
+    }
+
+    if (lidarGridPtr != NULL)
+    {
+        hasCheckedAvoidLidar = true;
+    }
+    if(offsetLast==0)//上次的避障距离是0或者沿参考线行驶第一次避障
+    {
+        unsigned int vector_num_record=0;
+        for(int j = 0; j < obs_property.size(); j++)
+        {
+            if(obs_property[j].obs_distance > 100) //障碍物距离大于100米认为可以臂章
+            {
+                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;
+                    vector_num_record = j;
+                }
+            }
+        }
+        //优先向左避障
+        if((signed_record_avoidX < 0) && (signed_record_avoidX-2*step>= avoidLeft))  //-0.5  -1  -1.5都可以避障的时候选则-1.5,
+        {
+            //int obs_test=abs(signed_record_avoidX)-1;
+            int obs_test = vector_num_record-1;
+            int obs_testneighbor = obs_test-1;
+            if((obs_property[obs_test].obs_distance > 100)&&(obs_property[obs_testneighbor].obs_distance>100))
+            {
+                signed_record_avoidX =signed_record_avoidX-2*step;
+                givlog->debug("decition_brain","singrecord_Y: %d",signed_record_avoidX);
+            }
+        }
+        else if((signed_record_avoidX < 0) && (signed_record_avoidX-1*step>= avoidLeft))//-0.5  -1 可以避障的时候选则-1,
+        {
+            //int obs_test=abs(signed_record_avoidX)-1;
+            int obs_test = vector_num_record-1;
+            if(obs_property[obs_test].obs_distance > 100)
+            {
+                signed_record_avoidX -= 1;
+                givlog->debug("decition_brain","singrecord_Y: %d",signed_record_avoidX);
+            }
+        }
+        else if((signed_record_avoidX > 0) && (signed_record_avoidX+2*step<= avoidRight))  //0.5  1  1.5都可以避障的时候选则1.5,
+        {
+            int obs_test = vector_num_record+1;
+            int obs_testneighbor = obs_test+1;
+            if((obs_property[obs_test].obs_distance > 100)&&(obs_property[obs_testneighbor].obs_distance>100)
+                    )
+            {
+                signed_record_avoidX =signed_record_avoidX+2*step;
+                givlog->debug("decition_brain","singrecord_Y: %d",signed_record_avoidX);
+            }
+        }
+        else if((signed_record_avoidX>0) && (signed_record_avoidX+1*step<= avoidRight))//0.5  1 可以避障的时候选则1,
+        {
+            int obs_test = vector_num_record+1;
+            if(obs_property[obs_test].obs_distance > 100)
+            {
+                signed_record_avoidX -= 1;
+                givlog->debug("decition_brain","singrecord_Y: %d",signed_record_avoidX);
+            }
+        }
+        else if (signed_record_avoidX < 0)
+        {
+                signed_record_avoidX=signed_record_avoidX; //只有-0.5 0.5 的时候就选则-0.5  0.5,优先选择-0.5
+        }
+        else
+        {
+            ;
+        }
+
+        if(signed_record_avoidX == 0)
+        {
+            for(unsigned int j = 0; j < obs_property.size(); j++)
+            {
+                if(obs_property[j].obs_distance > obs_cur_distance+15)
+                {
+                    if(obs_property[j].obs_distance > record_obstacle_distance)
+                    {
+                        record_avoidX = abs(obs_property[j].avoid_distance);
+                        signed_record_avoidX = obs_property[j].avoid_distance;
+                        record_obstacle_distance = obs_property[j].obs_distance;
+                    }
+                }
+            }
+            givlog->debug("decition_brain","offsetLast==0,and signed_record_avoidX==0");//20230213
+        }
+    }
+    else
+    {
+        for(int j = 0; j < obs_property.size();j++)
+        {
+            if(obs_property[j].obs_distance > 100)
+            {
+                if((abs(obs_property[j].avoid_distance)<record_avoidX)&&(obs_property[j].avoid_distance!=offsetLast))
+                {
+                    record_avoidX=abs(obs_property[j].avoid_distance);
+                    signed_record_avoidX=obs_property[j].avoid_distance;//返回的是距离原始参考路线最近的路线
+                }
+            }
+        }
+        if(signed_record_avoidX==0) //优先返回原参考路线
+        {
+            return 0;
+        }
+        else if(obs_cur_distance>30)//如果当前障碍物距离大于30米，继续当前的避障距离
+        {
+            signed_record_avoidX = offsetLast;
+        }
+        else if((obs_cur_distance<=30) && (signed_record_avoidX==offsetLast))
+        {
+            for(unsigned int j = 0;j<obs_property.size();j++)
+            {
+                if(obs_property[j].obs_distance>obs_cur_distance+15)
+                {
+                    if(obs_property[j].obs_distance>record_obstacle_distance)
+                    {
+                        record_avoidX = abs(obs_property[j].avoid_distance);
+                        signed_record_avoidX = obs_property[j].avoid_distance;
+                        record_obstacle_distance = obs_property[j].obs_distance;
+                    }
+                }
+            }
+            givlog->debug("decition_brain","obs_cur_distance<=30and signed_record_avoidX==offsetLast");//20230213
+        }
+    }
+
+    if((signed_record_avoidX!=offsetLast)&&(obs_cur_distance<50))
+    {
+        avoidMinDistance = obs_cur_distance + 2*ServiceCarStatus.msysparam.vehLenth;
+        startAvoidGps = now_gps_ins;
+        return signed_record_avoidX;
+    }
+    return offsetLast;
+}
+
+//iv::Point2D iv::decition::DecideGps00::computeObsOnRoadSpline(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,
+//                                                              const std::vector<GPSData> gpsMapLine,GPS_INS now_gps_ins,obsvalue* obsva)
+iv::Point2D iv::decition::DecideGps00::computeObsOnRoadSpline(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, double roadNum,
+                                                              const std::vector<GPSData> gpsMapLine,GPS_INS now_gps_ins,obsvalue* obsva) //20230213
 {
     double obs=-1,obsSd=-1;
     double obsCarCoordinateX,obsCarCoordinateY;

src/decition/decition_brain_sf_changan_shenlan/decition/decide_gps_00.h

@@ -249,10 +249,14 @@ public:
     float computeAvoidX(int roadAim,int roadOri,GPSData gps);
     double computeDistToEnd(const std::vector<GPSData> gpsMapLine,int pathpoint);
     void computeAvoidBoundary(int roadOriginal,int roadSumMap,double roadWidthEvery,double vehicleWidth,int* avoidXLeft,int* avoidXRight );
+    void computeAvoidBoundary(int roadOriginal,int roadSumMap,double roadWidthEvery,double vehicleWidth,double* avoidXLeft,double* avoidXRight );
     int computeAvoidXDistance(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine, int avoidLeft, int avoidRight, int offsetLast);
+    double computeAvoidXDistance(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine, double avoidLeft, double avoidRight, double offsetLast);//20230213,修改了障ai五位置检测精度由1米换成0.5米
 
     iv::Point2D computeObsOnRoadSpline(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,
                                        const std::vector<GPSData> gpsMapLine, GPS_INS now_gps_ins,obsvalue* obsva);
+    iv::Point2D computeObsOnRoadSpline(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, double roadNum,
+                                       const std::vector<GPSData> gpsMapLine, GPS_INS now_gps_ins,obsvalue* obsva);  //20230213,修改了障ai五位置检测精度由1米换成0.5米
 
 
     GPS_INS aim_gps_ins;
@@ -291,7 +295,8 @@ extern bool rapidStop;
 extern int gpsMissCount;
 extern bool changeRoad;
 extern double avoidX;
-extern int avoidXNew;
+//extern int avoidXNew;
+extern double avoidXNew; //20230213
 extern bool parkBesideRoad;
 extern double steerSpeed;
 extern bool transferPieriod;