modify avoid obstacle

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

@@ -235,7 +235,7 @@ int iv::decition::Compute00::getMapDelta(std::vector<GPSData> MapTrace){
 }
 
 
-int iv::decition::Compute00::getPlanSpeed(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);
@@ -243,25 +243,135 @@ int iv::decition::Compute00::getPlanSpeed(std::vector<GPSData> MapTrace)
                 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++)
+//                for(int i=0;i<doubledata.size();i++)
+//                {
+//                    if((doubledata[i][3]>-0.4)&&(doubledata[i][3]<0.4)){
+//                        MapTrace[i]->roadMode=0;
+//                    }else if(((doubledata[i][3]>0.4)&&(doubledata[i][3]<4))||((doubledata[i][3]>-4)&&(doubledata[i][3]<-0.4))){
+//                        MapTrace[i]->roadMode=5;
+//                    }else if(((doubledata[i][3]>4)&&(doubledata[i][3]<10))||((doubledata[i][3]>-10)&&(doubledata[i][3]<-4))){
+//                        MapTrace[i]->roadMode=18;
+//                    }else if(((doubledata[i][3]>10))||((doubledata[i][3]<-10))){
+//                        MapTrace[i]->roadMode=14;
+//                    }
+//                }
+                for(int i=0;i<doubledata.size();i++)
                 {
-                    if((doubledata[i][3]>-0.4)&&(doubledata[i][3]<0.4)){
-                        MapTrace[i]->roadMode=0;
-                    }else if(((doubledata[i][3]>0.4)&&(doubledata[i][3]<4))||((doubledata[i][3]>-4)&&(doubledata[i][3]<-0.4))){
+                    if((doubledata[i][3]>-0.26)&&(doubledata[i][3]<0.26)){
                         MapTrace[i]->roadMode=5;
-                    }else if(((doubledata[i][3]>4)&&(doubledata[i][3]<10))||((doubledata[i][3]>-10)&&(doubledata[i][3]<-4))){
+                    }else if(((doubledata[i][3]>=0.26)&&(doubledata[i][3]<=1.0))||((doubledata[i][3]>=-1.0)&&(doubledata[i][3]<=-0.26))){
                         MapTrace[i]->roadMode=18;
-                    }else if(((doubledata[i][3]>10))||((doubledata[i][3]<-10))){
+                    }else if(((doubledata[i][3]>1.0))||((doubledata[i][3]<-1.0))){
                         MapTrace[i]->roadMode=14;
                     }
-                }*/
+                }
+                for(int i=0;i<MapTrace.size();i++)
+                {
+                    if(MapTrace[i]->roadMode==0){
+                        doubledata[i][4]=straightSpeed;
+                        mode0to5count++;
+                        //mode0to18count++;
+                        mode18count=0;
+                        mode0to5flash=mode0to5count;
+                    }else if(MapTrace[i]->roadMode==5){
+                        doubledata[i][4]=straightCurveSpeed;
+                        mode0to5count++;
+                        //mode0to18count++;
+                        mode18count=0;
+                        mode0to5flash=mode0to5count;
+                    }else if(MapTrace[i]->roadMode==18){
+                        mode0to5countSum=mode0to5count;
+                        doubledata[i][4]=Curve_SmallSpeed;
+
+                        double brake_distance_double=(pow((straightCurveSpeed/3.6),2)-pow((Curve_SmallSpeed/3.6),2))*10/0.6;
+                        int brake_distance=(int)brake_distance_double;
+                        int step_count=0;
+                        if((i>brake_distance)&&(mode0to5countSum>brake_distance))
+                        {
+                            double step_speed=(straightCurveSpeed-Curve_SmallSpeed)/brake_distance;
+
+                            for(int j=i;j>i-brake_distance;j--){
+                                doubledata[j][4]=min((Curve_SmallSpeed+step_count*step_speed),straightCurveSpeed);
+                                step_count++;
+                            }
+                        }else if(mode0to5countSum>0){
+                            for(int j=i;j>=i-mode0to5countSum;j--){
+                                doubledata[j][4]=Curve_SmallSpeed;
+                                step_count++;
+                            }
+                        }else{
+                            doubledata[i][4]=Curve_SmallSpeed;
+                        }
+                        mode0to5count=0;
+                        //mode0to18count++;
+                        mode18count++;
+                        mode18flash=mode18count;
+                    }else if(MapTrace[i]->roadMode==14){
+                        mode0to18countSum=mode0to5flash+mode18flash;
+                        mode18countSum=mode18count;
+                        double brake_distanceLarge_double=(pow((Curve_SmallSpeed/3.6),2)-pow((Curve_LargeSpeed/3.6),2))*10/0.6;
+                        double brake_distance0to18_double=(pow((straightCurveSpeed/3.6),2)-pow((Curve_LargeSpeed/3.6),2))*10/0.6;
+                        int brake_distanceLarge=(int)brake_distanceLarge_double;
+                        int brake_distance0to18=(int)brake_distance0to18_double;
+                        int step_count=0;
+                        doubledata[i][4]=Curve_LargeSpeed;
+
+                        if(mode18countSum>brake_distanceLarge)
+                        {
+                                double step_speed=(Curve_SmallSpeed-Curve_LargeSpeed)/brake_distanceLarge;
+
+                                for(int j=i;j>i-brake_distanceLarge;j--){
+                                    doubledata[j][4]=min((Curve_LargeSpeed+step_count*step_speed),Curve_SmallSpeed);
+                                    step_count++;
+                                }
+
+                        }else if(mode0to18countSum>brake_distance0to18){
+
+                                double step_speed=(straightCurveSpeed-Curve_LargeSpeed)/brake_distance0to18;
+
+                                for(int j=i;j>i-brake_distance0to18;j--){
+                                    doubledata[j][4]=min((Curve_LargeSpeed+step_count*step_speed),straightCurveSpeed);
+                                    step_count++;
+                                }
+                        }else{
+                                doubledata[i][4]=Curve_LargeSpeed;
+                        }
+
+                        mode0to5count=0;
+                        mode18count=0;
+                        mode0to5flash=0;
+                        mode18flash=0;
+                    }
+                }
+
+                for(int i=0;i<MapTrace.size();i++){
+                    //将数据写入文件开始
+                    ofstream outfile;
+                    outfile.open("ctr0108003.txt", ostream::app);
+                    outfile<<"Delta"<< ","  <<doubledata[i][3]<< ","<<"roadMode"<< ","  <<MapTrace[i]->roadMode<< ","
+                           <<"plan_speed"<< ","  << doubledata[i][4]<< ","<<"num,curve"<< ","  <<i<< ","
+                           <<MapTrace[i]->mfCurvature<<endl;
+                    outfile.close();
+                    //将数据写入文件结束
+                }
+
+}*/
+
+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((doubledata[i][3]>-0.26)&&(doubledata[i][3]<0.26)){
+                    if((MapTrace[i]->mfCurvature>-0.02)&&(MapTrace[i]->mfCurvature<0.02)){
                         MapTrace[i]->roadMode=5;
-                    }else if(((doubledata[i][3]>=0.26)&&(doubledata[i][3]<=1.0))||((doubledata[i][3]>=-1.0)&&(doubledata[i][3]<=-0.26))){
+                    }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(((doubledata[i][3]>1.0))||((doubledata[i][3]<-1.0))){
+                    }else if(((MapTrace[i]->mfCurvature>0.16))||((MapTrace[i]->mfCurvature<-0.16))){
                         MapTrace[i]->roadMode=14;
                     }
                 }

src/decition/decition_brain_sf/decition/decide_gps_00.cpp

@@ -981,7 +981,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                         double nowspeed = realspeed/3.6;
                         if((distoend<10)||(distoend<(nowspeed*nowspeed)))
                         {
-                            if(distoend == 0.0)distoend = 0.1;
+                            if(distoend == 0.0)distoend = 0.09;
                             acc_end = (-1.0)*nowspeed*nowspeed/(2.0*distoend);
                             if(acc_end<(-3.0))acc_end = -3.0;
                         }
@@ -4228,8 +4228,19 @@ int iv::decition::DecideGps00::computeBackDistance(iv::LidarGridPtr lidarGridPtr
         hasCheckedAvoidLidar = true;
     }
 
-    if((obs_cur_distance>100)&&(GetDistance(now_gps_ins, startAvoidGps)>max(avoidMinDistance,40.0)))
-                        return 0;
+//    if((obs_cur_distance>100)&&(GetDistance(now_gps_ins, startAvoidGps)>max(avoidMinDistance,40.0)))
+//                        return 0;
+    if((obs_cur_distance>100)&&(vehState==normalRun))
+    {
+        if(PathPoint+300<gpsMapLine.size()){
+            for(int k=PathPoint;k<PathPoint+300;k++){
+                    if((gpsMapLine[k]->mfCurvature>0.02))
+                                     return 20;
+            }
+            return 0;
+        }
+    }
+
     return 20;
 }
 

src/fusion/lidar_radar_fusion/fusion.hpp

@@ -36,6 +36,10 @@ void Get_AssociationMat(LidarObjectPtr& lidar_object_vector_ptr,iv::radar::radar
         match_index match;
         match.nlidar = i;
         std::vector<int> fuindex;
+        if(abs(lidar_object_vector_ptr->at(i).nearest_point.x) <10
+                &&abs(lidar_object_vector_ptr->at(i).location.x) <10
+                && lidar_object_vector_ptr->at(i).location.y > 10)
+            continue;
 //        std::cout<<"  size    "<<radar_object_array.obj_size()<<std::endl;
         for(int j =0; j<radar_object_array.obj_size(); j++)
         {
@@ -72,18 +76,24 @@ void Get_AssociationMat(LidarObjectPtr& lidar_object_vector_ptr,iv::radar::radar
         match_idx.push_back(match);
     }
 
+    std::vector<int> index;
+    for(std::vector<match_index>::iterator l = match_idx.begin(); l != match_idx.end(); l++)
+    {
+        index.push_back(l->nradar);
+    }
+
     for(int k = 0; k<radar_object_array.obj_size(); k++)
     {
-        std::vector<int> index;
-        for(std::vector<match_index>::iterator l = match_idx.begin(); l != match_idx.end(); l++)
-        {
-            index.push_back(l->nradar);
-        }
+        if(radar_object_array.obj(k).bvalid() == false) continue;
         if(std::find(index.begin(),index.end(),k) !=index.end())
         {
+            std::cout<<"  radar    only "<<std::endl;
             radar_idx.push_back(k);
+        } else {
+            std::cout<<"   int   it    "<<std::endl;
         }
     }
+    index.clear();
 }
 
 void RLfusion(LidarObjectPtr& lidar_object_vector_ptr,iv::radar::radarobjectarray& radar_object_array, iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array)