修改避障逻辑，添加V2R逻辑，更改某些参数适配深蓝比赛

src/decition/decition_brain_sf_changan_shenlan/decition/adc_planner/spline_planner.cpp

@@ -98,6 +98,84 @@ double end=GetTickCount();
             return gpsTrace;
 
 
+}
+
+std::vector<iv::Point2D> iv::decition::SplinePlanner::chooseRoadBySpline(GPS_INS now_gps_ins, const std::vector<GPSData>& gpsMapLine, int PathPoint,std::vector<iv::Point2D> obsSpline,double avoidX,double nowX){
+
+    double s_obs_current,d_obs_current,s_obs_current_relative;
+    double s_obs_avoid_relative,d_obs_avoid,s_obs_avoid;
+    double s_offset,d_offset;
+    std::vector<iv::Point2D> gpsTrace;
+    for(unsigned int j=0;j<obsSpline.size();j++){
+        if(fabs(obsSpline[j].d - avoidX) <= 1.0e-6){
+                    s_obs_avoid_relative=obsSpline[j].s;
+                    d_obs_avoid=-obsSpline[j].d;
+        }
+        if(fabs(obsSpline[j].d - nowX)<= 1.0e-6){
+                    s_obs_current_relative=obsSpline[j].s;
+                    if(s_obs_current_relative>40)
+                        s_obs_current_relative=40;
+        }
+    }
+    iv::Point2D now_s_d=cartesian_to_frenet1D(gpsMapLine,now_gps_ins.gps_x, now_gps_ins.gps_y);
+//            if(s_obs_avoid_relative>=s_obs_current_relative*2){
+//                            s_obs_avoid=now_s_d.s+s_obs_current_relative*2;
+//            }else{
+//                            s_obs_avoid=now_s_d.s+s_obs_avoid_relative;
+//            }
+//    s_obs_avoid=now_s_d.s+s_obs_current_relative;//避障路线终点s值
+//    s_obs_current=now_s_d.s;
+    s_obs_avoid=s_obs_current_relative;//避障路线终点s值
+    s_obs_current=0;
+    d_obs_current=now_s_d.d;
+    s_offset=s_obs_avoid-s_obs_current;
+    d_offset=d_obs_avoid-d_obs_current;
+    ctrlPoints={QPointF(s_obs_current,d_obs_current),QPointF(s_obs_current+s_offset/4,d_obs_current),QPointF(s_obs_current+s_offset/2,(d_obs_current+(d_offset)/4)),
+                QPointF(s_obs_current+s_offset/2,(d_obs_avoid-(d_offset)/4)),QPointF(s_obs_avoid-s_offset/4,d_obs_avoid),QPointF(s_obs_avoid,d_obs_avoid)};
+
+
+//            now_s_d.s=0;
+//            ctrlPoints.clear();
+//            QPointF point=QPointF(0,0);
+//            ctrlPoints.push_back(point);
+//            point=QPointF(10,0);
+//            ctrlPoints.push_back(point);
+//            point=QPointF(25,-0.5);
+//            ctrlPoints.push_back(point);
+//            point=QPointF(25,-1.5);
+//            ctrlPoints.push_back(point);
+//            point=QPointF(40,-2);
+//            ctrlPoints.push_back(point);
+//            point=QPointF(50,-2);
+//            ctrlPoints.push_back(point);
+//            ctrlPoints={QPointF(now_s_d.s+0,0),QPointF(now_s_d.s+10,0),QPointF(now_s_d.s+25,-0.5),
+//                        QPointF(now_s_d.s+25,-1.5),QPointF(now_s_d.s+40,-2),QPointF(now_s_d.s+50,-2)};
+
+
+            generateCurve();
+
+double start=GetTickCount();
+            gpsTrace.clear();
+            for(int i=0;i<curvePoints.size();i++){
+                double s_transfer=curvePoints[i].x()+now_s_d.s;
+
+                iv::Point2D gpsTracePoint=frenet_to_cartesian1D(gpsMapLine,s_transfer, curvePoints[i].y(),PathPoint);
+
+                gpsTrace.push_back(gpsTracePoint);
+
+            }
+double end=GetTickCount();
+
+            double period=end-start;
+            std::cout<<"===================period========================"<<period<<       std::endl;
+            //std::cout<<"===================curvesize========================"<<curvePoints.size()<<       std::endl;
+            /*for(double s=s_obs_avoid;s<(*gpsMapLine[gpsMapLine.size()-1]).frenet_s;s+=0.1){
+                iv::Point2D gpsTracePoint=frenet_to_cartesian1D(gpsMapLine,s, d_obs_avoid);
+                gpsTrace.push_back(gpsTracePoint);
+            }*/
+            return gpsTrace;
+
+
 }
 void iv::decition::SplinePlanner::generateCurve()
 {

src/decition/decition_brain_sf_changan_shenlan/decition/adc_planner/spline_planner.h

@@ -26,6 +26,7 @@ public:
 
     void generateCurve();
     std::vector<iv::Point2D> chooseRoadBySpline(GPS_INS now_gps_ins, const std::vector<GPSData>& gpsMapLine, int PathPoint,std::vector<iv::Point2D> obsSpline,int avoidX,int nowX);
+    std::vector<iv::Point2D> chooseRoadBySpline(GPS_INS now_gps_ins, const std::vector<GPSData>& gpsMapLine, int PathPoint,std::vector<iv::Point2D> obsSpline,double avoidX,double nowX);
 
 
 

src/decition/decition_brain_sf_changan_shenlan/decition/brain.cpp

@@ -44,7 +44,7 @@ namespace decition {
         void ListenV2r(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
         {
             gbrain->UpdateV2r(strdata,nSize);
-            gbrain->UpdateSate();
+//            gbrain->UpdateSate();
         }
 
         void ListenHMI(const char * strdata,const unsigned int nSize ,const unsigned int index,const QDateTime * dt,const char * strmemname)
@@ -357,6 +357,15 @@ void iv::decition::BrainDecition::run() {
         ServiceCarStatus.avoidObs = false;
     }
 
+    std::string socfDis = xp.GetParam("socfDis","15");
+    std::string aocfDis = xp.GetParam("aocfDis","25");
+    std::string aocfTimes = xp.GetParam("aocfTimes","3");
+
+
+    ServiceCarStatus.socfDis = atof(socfDis.data());
+    ServiceCarStatus.aocfDis = atof(aocfDis.data());
+    ServiceCarStatus.aocfTimes = atof(aocfTimes.data());
+
     mstrmemmap_index = xp.GetParam("msg_mapindex","map_index");
 
 
@@ -387,6 +396,11 @@ void iv::decition::BrainDecition::run() {
         std::shared_ptr<std::vector<iv::Perception::PerceptionOutput>> lidar_per(new std::vector<iv::Perception::PerceptionOutput>);
 
         bool brun =ServiceCarStatus.mbRunPause;
+        if(ServiceCarStatus.mRSUupdateTimer.elapsed()>60*1000)
+         {
+            ServiceCarStatus.mbRunPause=false;//长时间收不到V2R信息认为没有V2R
+         }
+
         if(ServiceCarStatus.mnBocheComplete > 0)
         {
             ServiceCarStatus.mbBrainCtring = false;
@@ -396,11 +410,32 @@ void iv::decition::BrainDecition::run() {
         if((ServiceCarStatus.mbRunPause)||(navigation_data.size()<1)||(ServiceCarStatus.mnBocheComplete>0))
         {
             ServiceCarStatus.mbBrainCtring = false;
-            decition_gps->brake = 6;
-            decition_gps->accelerator = -0.5;
-            decition_gps->wheel_angle = 0;
-            decition_gps->torque = 0;
-            decition_gps->mode = 0;
+//            decition_gps->brake = 6;
+//            decition_gps->accelerator = -0.5;
+//            decition_gps->wheel_angle = 0;
+//            decition_gps->torque = 0;
+//            decition_gps->mode = 0;
+            if(ServiceCarStatus.msysparam.mvehtype=="shenlan")
+            {
+                decition_gps->brake = 6;//保证在进入自动驾驶的时候车辆能够泄压,否则进入自动驾驶车辆无法加速
+                decition_gps->torque=0.0;
+                decition_gps->acc_active = 0;//ACC扭矩请求激活（为1激活，为0不激活），不激活后不执行扭矩请求
+                decition_gps->brake_active = 0;//ACC减速激活（为1制动激活，为0制动不激活）
+                decition_gps->brake_type = 0;//ADC请求类型（制动时为1，其余情况为0）
+                decition_gps->angle_active = 0;//横向控制激活模式
+                decition_gps->angle_mode = 0; //横向控制激活，和上一条同时满足才执行横向请求角度
+                decition_gps->auto_mode = 0; //3为自动控制模式
+                decition_gps->wheel_angle = 0;
+
+            }
+            else
+            {
+                decition_gps->brake = 6;
+                decition_gps->accelerator = -0.5;
+                decition_gps->wheel_angle = 0;
+                decition_gps->torque = 0;
+                decition_gps->mode = 0;
+            }
             ShareDecition(decition_gps);
             ServiceCarStatus.mfAcc = decition_gps->accelerator;
             ServiceCarStatus.mfWheel = decition_gps->wheel_angle;

src/decition/decition_brain_sf_changan_shenlan/decition/decide_gps_00.cpp

@@ -105,7 +105,7 @@ int obsLostTimeAvoid = 0;
 
 // double avoidTime = 0;
 
-int avoidXNewPre=0,avoidXNewPreFilter=0;
+double avoidXNewPre=0,avoidXNewPreFilter=0;//20230213,障碍物检测精度由1米换成0.5米
 //vector<int> avoidXNewPreVector;
 //int avoidXNew=0;
 //int avoidXNewLast=0;
@@ -287,10 +287,10 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
 
     //如果useFrenet、useOldAvoid两者不互为相异，则采用原来的方法。“^”为异或运算符。
-    if(!(useFrenet^useOldAvoid)){
-        useFrenet = false;
-        useOldAvoid = true;
-    }
+//    if(!(useFrenet^useOldAvoid)){
+//        useFrenet = false;
+//        useOldAvoid = true;
+//    }
 
     static int file_num;
     if (isFirstRun)
@@ -352,7 +352,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     minDecelerate=0;
     if(now_gps_ins.gps_lat<0||now_gps_ins.gps_lat>90)
     {
-        gps_decition->wheel_angle = 0;
+        gps_decition->wheel_angle = 0.2;
         gps_decition->speed = dSpeed;
 
         gps_decition->accelerator = -0.5;
@@ -1043,7 +1043,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
             vehState=normalRun;
         }
     }
-    givlog->debug("decition_brain","avoidXNew: %d",avoidXNew);
+    givlog->debug("decition_brain","avoidXNew: %f",avoidXNew);
 #else
     if(obstacle_avoid_flag==1)
     {
@@ -1125,55 +1125,55 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     //    if(GetDistance(now_gps_ins,*gpsMapLine[PathPoint]) < 1){
     //        startingFlag = false;
     //    }
-    startingFlag = false;
-    if(startingFlag)
-    {
-        //        gpsTraceAim = gpsTraceOri;
-        if(abs(gpsTraceOri[0].x)>1)
-        {
-            cout<<"frenetPlanner->getPath:pre"<<endl;
-            gpsTraceNow = frenetPlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,0,lidarGridPtr);
-            cout<<"frenetPlanner->getPath,gpsTraceNow.size: "<<gpsTraceNow.size()<<endl;
-            if(gpsTraceNow.size()==0)
-            {
-                gps_decition->accelerator = 0;
-                gps_decition->brake=10;
-                gps_decition->wheel_angle = lastAngle;
-                gps_decition->speed = 0;
-                return gps_decition;
-            }
-        }
-        else
-        {
-            startingFlag = false;
-        }
-    }
+//    startingFlag = false;
+//    if(startingFlag)
+//    {
+//        //        gpsTraceAim = gpsTraceOri;
+//        if(abs(gpsTraceOri[0].x)>1)
+//        {
+//            cout<<"frenetPlanner->getPath:pre"<<endl;
+//            gpsTraceNow = frenetPlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,0,lidarGridPtr);
+//            cout<<"frenetPlanner->getPath,gpsTraceNow.size: "<<gpsTraceNow.size()<<endl;
+//            if(gpsTraceNow.size()==0)
+//            {
+//                gps_decition->accelerator = 0;
+//                gps_decition->brake=10;
+//                gps_decition->wheel_angle = lastAngle;
+//                gps_decition->speed = 0;
+//                return gps_decition;
+//            }
+//        }
+//        else
+//        {
+//            startingFlag = false;
+//        }
+//    }
 
 
-    if(useFrenet){
-        //如果车辆在变道中，启用frenet规划局部路径
-        if(vehState == avoiding || vehState == backOri)
-        {
-            // avoidX = (roadOri - roadNow)*roadWidth;
-            avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint]);
-            gpsTraceNow = frenetPlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
-            //gpsTraceAim = laneChangePlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
-            gpsTraceAim = getGpsTraceOffset(gpsTraceOri, avoidX);
-            if(gpsTraceNow.size()==0)
-            {
-                gps_decition->accelerator = 0;
-                gps_decition->brake=10;
-                gps_decition->wheel_angle = lastAngle;
-                gps_decition->speed = 0;
-                return gps_decition;
-            }
-        }
-    }
+//    if(useFrenet){
+//        //如果车辆在变道中，启用frenet规划局部路径
+//        if(vehState == avoiding || vehState == backOri)
+//        {
+//            // avoidX = (roadOri - roadNow)*roadWidth;
+//            avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint]);
+//            gpsTraceNow = frenetPlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
+//            //gpsTraceAim = laneChangePlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
+//            gpsTraceAim = getGpsTraceOffset(gpsTraceOri, avoidX);
+//            if(gpsTraceNow.size()==0)
+//            {
+//                gps_decition->accelerator = 0;
+//                gps_decition->brake=10;
+//                gps_decition->wheel_angle = lastAngle;
+//                gps_decition->speed = 0;
+//                return gps_decition;
+//            }
+//        }
+//    }
 
 #ifdef AVOID_NEW
     if(gpsTraceNow.size()==0)
     {
-        if (avoidXNew==0)
+        if (avoidXNew==0.0)
         {
             if((vehState == backOri)||(vehState == avoiding))
             {
@@ -1602,8 +1602,6 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         obs_speed_for_avoid=(secSpeed+obsSpeed)*3.6;
 
         obs_speed_for_avoid=0;//shenlan guosai zhangaiwu sudu zhijiegei 0
-
-
     }
     else
     {
@@ -1723,7 +1721,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     std::cout<<"\n原始RoadOri:%d\n"<<roadOri<<endl;
     //   计算回归原始路线
 #ifdef AVOID_NEW
-    if((avoidXNew == 0)&&(vehState == avoiding))
+    if((avoidXNew == 0.0)&&(vehState == avoiding))
     {
         iv::Point2D now_s_d=cartesian_to_frenet1D(gpsMapLine,now_gps_ins.gps_x, now_gps_ins.gps_y);
         if(fabs(now_s_d.d)<0.05)
@@ -1795,16 +1793,35 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     }*/
 
     road_permit_avoid=false;
-    if(PathPoint+400<gpsMapLine.size()){
-        int road_permit_sum=0;
-        for(int k=PathPoint;k<PathPoint+400;k++){
-            if((gpsMapLine[k]->mbnoavoid==false))//&&(gpsMapLine[k]->roadSum>1))
-                road_permit_sum++;
+
+    if(ServiceCarStatus.msysparam.mvehtype=="shenlan") //比赛总路线可能不会太长
+    {
+        if(PathPoint+200<gpsMapLine.size()){
+            int road_permit_sum=0;
+            for(int k=PathPoint;k<PathPoint+200;k++){
+                if((gpsMapLine[k]->mbnoavoid==false))//&&(gpsMapLine[k]->roadSum>1))
+                    road_permit_sum++;
+            }
+            if(road_permit_sum>=200)
+                road_permit_avoid=true;
+        }
+
+    }
+    else
+    {
+        if(PathPoint+400<gpsMapLine.size()){
+            int road_permit_sum=0;
+            for(int k=PathPoint;k<PathPoint+400;k++){
+                if((gpsMapLine[k]->mbnoavoid==false))//&&(gpsMapLine[k]->roadSum>1))
+                    road_permit_sum++;
+            }
+            if(road_permit_sum>=400)
+                road_permit_avoid=true;
         }
-        if(road_permit_sum>=400)
-            road_permit_avoid=true;
     }
 
+
+
     //shiyanbizhang 0929
     if(obsDistance>0&& (obsDistance<ServiceCarStatus.aocfDis)&&(obs_speed_for_avoid>-8.0)&&(obs_speed_for_avoid<3.0)&&(realspeed>0.01)//&& (avoid_speed_flag==true)        //
             &&(vehState==normalRun||vehState==backOri || vehState==avoiding) //&&(ObsTimeStart==-1)
@@ -1889,7 +1906,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     }
 
 
-    if((vehState == preAvoid)||(avoidXNew!=0))
+    if((vehState == preAvoid)||(avoidXNew!=0.0))
     {
         dSpeed = min(6.0,dSpeed);
 //        int avoidLeft_value=0;
@@ -1928,7 +1945,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
             gpsTraceNow.clear();
             gpsTraceAvoidXY.clear();
-            givlog->debug("decition_brain","avoidXNew: %d,avoidXNewLast: %d",
+            givlog->debug("decition_brain","avoidXNew: %f,avoidXNewLast: %f",
                           avoidXNew,avoidXNewLast);
             //gpsTraceAvoidXY = getGpsTraceOffsetAvoid(gpsTraceOri, avoidXNew,now_gps_ins);
             gpsTraceAvoidXY = splinePlanner->chooseRoadBySpline(now_gps_ins,gpsMapLine,PathPoint,obsSpline,avoidXNew,avoidXNewLast);
@@ -2273,11 +2290,16 @@ else
 
     transferGpsMode2(gpsMapLine);
 
-    if(ServiceCarStatus.msysparam.mvehtype=="hapo"){
-        if(obsDistance>0 && obsDistance<8){
+    if(ServiceCarStatus.msysparam.mvehtype=="hapo")
+    {
+        if(obsDistance>0 && obsDistance<8)
+        {
             dSpeed=0;
         }
-    }else if(obsDistance>0 && obsDistance<15){
+    }
+//    else if(obsDistance>0 && obsDistance<15)
+    else if(obsDistance>0 && obsDistance<  ServiceCarStatus.socfDis)
+    {
         dSpeed=0;
     }
 
@@ -2495,8 +2517,8 @@ else
                                 <<"SecSpeed"<<","<<secSpeed<<","
                                  <<"Vehicle_state"<< ","<<vehState<< ","
                                 <<"avoid_flag"<<","<<obstacle_avoid_flag<<","
-                                 <<"avoidXNew"<<","<<avoidXNew<<","
-                                 <<"avoidXNewPre"<<","<<avoidXNewPre<<","
+                                 <<"avoidXNew"<<","<< setprecision(3)<<avoidXNew<<","
+                                 <<"avoidXNewPre"<<","<< setprecision(3)<<avoidXNewPre<<","
                                  //<<"avoidXPre"<<","<<avoidXPre<<","
                                  <<"ObsTimeEnd"<<","<<ObsTimeEnd<<","
                                  <<"avoidXY_size"<<","<<gpsTraceAvoidXY.size()<<","
@@ -2622,13 +2644,23 @@ std::vector<iv::Point2D> iv::decition::DecideGps00::getGpsTrace(iv::GPS_INS now_
 
     traceOriLeft.clear();
     traceOriRight.clear();
+    int tracesize=800;
+    if(ServiceCarStatus.msysparam.mvehtype=="shenlan")
+    {
+        tracesize=400;
+    }
+    else
+    {
+        tracesize=800;
+    }
 
-    if (gpsMapLine.size() > 800 && PathPoint >= 0) {
+    if (gpsMapLine.size() > tracesize && PathPoint >= 0)
+    {
         int aimIndex;
         if(circleMode){
-            aimIndex=PathPoint+800;
+            aimIndex=PathPoint+tracesize;
         }else{
-            aimIndex=min((PathPoint+800),(int)gpsMapLine.size());
+            aimIndex=min((PathPoint+tracesize),(int)gpsMapLine.size());
         }
 
         for (int i = PathPoint; i < aimIndex; i++)