obsAvoid test 0208

src/decition/common/common/car_status.h

@@ -163,7 +163,7 @@ namespace iv {
         iv::roadmode_vel mroadmode_vel;
         int vehGroupId;
         double mfEPSOff = 0.0;
-        float socfDis=15;   //停障触发距离
+        float socfDis=10;   //停障触发距离
         float aocfDis=25;   //避障触发距离//20
         float aocfTimes=3; //避障触发次数
         float aobzDis=5;   //避障保障距离

src/decition/decition_brain_sf_changan_shenlan/decition/adc_adapter/base_adapter.cpp

@@ -173,6 +173,14 @@ bool useOldAvoid = true;   //标志是否用老方法避障
 bool front_car_decide_avoid=true;  //前后车距大于30米，后车允许根据自己障碍物情况避障，否则只能听从前车
 bool road_permit_avoid=false;  //true: 道路允许避障，false:道路不允许避障
 
+//数据存储功能 ,20210903,cxw
+bool file_cnt_add_en =false;  //用于提示是否需要将文件计数值增加
+bool file_cnt_add=false;
+bool map_start_point = true;
+bool first_start_en=true;  //自主巡迹数据存储用
+int  start_tracepoint;
+int  end_tracepoint;
+
 std::vector<std::vector<double>> doubledata;//大地坐标系下x,y,phi,delta，速度规划用变量
 
 enum VehState { normalRun, park, readyPark, stopObs, avoidObs, preAvoid, avoiding, backOri,preBack,
@@ -281,8 +289,10 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         useOldAvoid = true;
     }
 
+    static int file_num;
     if (isFirstRun)
     {
+        file_num=0;
         initMethods();
         vehState = normalRun;
         startAvoid_gps_ins = now_gps_ins;
@@ -1279,19 +1289,19 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     }
 
     //2020-0106
-    if(ServiceCarStatus.msysparam.mvehtype !="zhongche")
-    {
-
-        if(vehState==avoiding){
-            ServiceCarStatus.msysparam.vehWidth=2.4;
-            controlAng=max(-150.0,controlAng);//35  zj-150
-            controlAng=min(150.0,controlAng);//35   zj-150
-        }
-        if(vehState==backOri){
-            controlAng=max(-150.0,controlAng);//35   zj-150
-            controlAng=min(150.0,controlAng);//35     zj-150
-        }
-    }
+//    if(ServiceCarStatus.msysparam.mvehtype !="zhongche")
+//    {
+
+//        if(vehState==avoiding){
+//            ServiceCarStatus.msysparam.vehWidth=2.4;
+//            controlAng=max(-150.0,controlAng);//35  zj-150
+//            controlAng=min(150.0,controlAng);//35   zj-150
+//        }
+//        if(vehState==backOri){
+//            controlAng=max(-150.0,controlAng);//35   zj-150
+//            controlAng=min(150.0,controlAng);//35     zj-150
+//        }
+//    }
 
     //    givlog->debug("brain_decition","vehState: %d,controlAng: %f",vehState,controlAng);
 
@@ -1588,6 +1598,8 @@ 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
@@ -1783,7 +1795,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     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))
+            if((gpsMapLine[k]->mbnoavoid==false))//&&(gpsMapLine[k]->roadSum>1))
                 road_permit_sum++;
         }
         if(road_permit_sum>=400)
@@ -1791,7 +1803,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     }
 
     //shiyanbizhang 0929
-    if(obsDistance>0&& (obsDistance<ServiceCarStatus.aocfDis)&&(obs_speed_for_avoid>-8.0)&&(obs_speed_for_avoid<3.0)&&(realspeed>1)//&& (avoid_speed_flag==true)        //
+    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)
             && (gpsMapLine[PathPoint]->runMode==1)&& (road_permit_avoid==true)&&(gpsMapLine[PathPoint]->mode2!=3000)&&(gpsMapLine[PathPoint+300]->mode2!=3000)&&(gpsMapLine[PathPoint+100]->mode2!=3000)&&(gpsMapLine[PathPoint+300]->mode2!=23)){
         //        ObsTimeStart=GetTickCount();
@@ -1876,6 +1888,7 @@ 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;
@@ -2186,7 +2199,228 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         outfile.close();
         //将数据写入文件结束
     }
-
+    //shuju cunchu gognnenng add,20210624,cxw
+             if(ServiceCarStatus.txt_log_on==true)
+             {
+                if(first_start_en)
+                 {
+                    first_start_en=false;
+                    start_tracepoint = PathPoint;
+                    if(circleMode)
+                    {
+                        if(start_tracepoint==0)
+                        {
+
+                            end_tracepoint =gpsMapLine.size()-1;  //这种计算终点坐标的序号只适合与闭环地图
+                        }
+                        else
+                        {
+                            end_tracepoint=start_tracepoint-1;
+                        }
+                    }
+                    else
+                    {
+                       end_tracepoint =gpsMapLine.size()-1; //20210929,不是闭环地图的时候终点参考坐标就是地图数组的最大序号
+                    }
+                 }
+                 double dis1 =  GetDistance(now_gps_ins, *gpsMapLine[start_tracepoint]);
+                 double dis2 = 10;// GetDistance(now_gps_ins, *gpsMapLine[end_tracepoint]);
+                 if(brake_mode==true)
+                 {
+                     dis2=2;
+                 }
+
+                 //double dis2 =  GetDistance(now_gps_ins, *gpsMapLine[end_tracepoint]);
+                 if(circleMode)//闭环地图
+                 {
+                     if(dis1<3&&dis2<3&&circleMode) //距离不能太小，太小了可能会错过，所以适当将距离增大一些
+                    // if(dis1<1&&dis2<1&&circleMode)
+                     {
+                         file_cnt_add_en=true;  //201200102
+                     }
+                     else
+                     {
+                         file_cnt_add_en=false;
+                     }
+
+                     if(dis1>10)
+                     {
+                        file_cnt_add=true;
+                     }
+
+                     if(file_cnt_add_en&&file_num<256&&file_cnt_add)
+                     {
+                         file_num++;
+                         file_cnt_add=false;
+                         map_start_point=true;
+                     }
+                     if(file_num==256)
+                     {
+                         file_num=1;
+                     }
+                     else
+                     {
+                     }
+                     QDateTime dt=QDateTime::currentDateTime();
+                     QString date =dt.toString("yyyy_MM_dd_hhmm");
+                     QString strsen = "1xdatalog_";
+                     date.append(strsen);
+                     QString s = QString::number(file_num);
+                     date.append(s);
+                     date.append(".txt");
+                     string filename;
+                     filename=date.toStdString();
+                     ofstream outfile;
+                     outfile.open(filename, ostream::app);
+                     if((file_cnt_add==false)&&(map_start_point==true))
+                     {
+                         QDateTime dt=QDateTime::currentDateTime();
+                         outfile<<dt.date().year()<<"-"<<dt.date().month()<<"-"<<dt.date().day()<<" "<<dt.time().hour()<<":"
+                                <<dt.time().minute()<<":"<<dt.time().second()<<"-"<<dt.time().msec()
+                                <<endl;
+                         outfile<<"***********************the vehicle at map start point!*************************"<<endl;
+                         outfile<<"the number of lap  is "<<":    "<<file_num<<""<<endl;
+                         map_start_point=false;
+                     }
+                     QDateTime dt2=QDateTime::currentDateTime();
+                     outfile <<dt2.time().hour()<<":"<<dt2.time().minute()<<":"<<dt2.time().second()<<":"<<dt2.time().msec()<<","
+                            <<"Decide_Dspeed"  << ","  <<setprecision(2)<<dSpeed<< ","
+                            <<"Decide_ACC"  << ","  <<setprecision(2)<<gps_decition->torque<< ","
+                            <<"Decide_Brake"<< ","  <<gps_decition->brake<< ","
+                            <<"Vehicle_RealSpd"<< ","  <<setprecision(2)<<now_gps_ins.speed<< ","
+                            <<"Decide_Angle"<< ","  << setprecision(2)<<gps_decition->wheel_angle<< ","
+                            <<"Vehicle_GPS_heading"<< ","<< setprecision(10)<<now_gps_ins.ins_heading_angle<< ","
+                            <<"Vehicle_GPS_X"<< ","<< setprecision(10)<<now_gps_ins.gps_lat<< ","
+                            <<"Vehicle_GPS_Y"<< ","<< setprecision(10)<<now_gps_ins.gps_lng<< ","
+                            <<"Trace_Point"<< ","<<PathPoint<< ","
+                            <<"OBS_Distance"<< ","<<obsDistance<< ","
+                            <<"OBS_Speed"<< ","<<obsSpeed<< ","
+                            <<endl;
+                     outfile.close();
+                 }
+                 else //fei yuanxing luxian
+                 {
+                     if(dis1<3)
+                     {
+                         file_cnt_add_en=true;  //201200102
+                     }
+                     else
+                     {
+                         file_cnt_add_en=false;
+                     }
+
+                     if(dis1>3)
+                     {
+                        file_cnt_add=true;
+                     }
+
+                     if(file_cnt_add_en&&file_num<256&&file_cnt_add)
+                     {
+                         file_num++;
+                         file_cnt_add=false;
+                         map_start_point=true;
+                     }
+                     if(file_num==256)//20210712
+                     {
+                         file_num=1;
+                     }
+                     else
+                     {
+                     }
+                     QDateTime dt=QDateTime::currentDateTime();
+                     QString date =dt.toString("yyyy_MM_dd_hhmm");
+                     QString strsen = "datalog_";
+                     date.append(strsen);
+                     QString s = QString::number(file_num);
+                     date.append(s);
+                     date.append(".txt");
+                     string filename;
+                     filename=date.toStdString();
+                     ofstream outfile;
+                     outfile.open(filename, ostream::app);
+                     if((file_cnt_add==false)&&(map_start_point==true))
+                     {
+                         QDateTime dt=QDateTime::currentDateTime();
+                         outfile<<dt.date().year()<<"-"<<dt.date().month()<<"-"<<dt.date().day()<<" "<<dt.time().hour()<<":"<<dt.time().minute()<<":"<<dt.time().second()<<"-"<<dt.time().msec()
+                               <<endl;
+                         outfile<<"********the vehicle near map start point!********"<<endl;
+                           outfile<<"the number of lap  is "<<":" <<file_num<<""<<endl;
+                         map_start_point=false;
+                     }
+                     if(dis2<3)
+                     {
+                         outfile<<"********the vehicle near map end point!********" <<endl;
+                         QDateTime dt=QDateTime::currentDateTime();
+                         outfile<<dt.date().year()<<"-"<<dt.date().month()<<"-"<<dt.date().day()<<" "<<dt.time().hour()<<":"<<dt.time().minute()<<":"<<dt.time().second()<<"-"<<dt.time().msec()
+                               <<endl;
+                     }
+                     else
+                     {
+                         float ttc = 0-(obsDistance/obsSpeed);
+                         double obsDistance_log=0;
+                         if(obsDistance>500)
+                             obsDistance_log=100;
+                         else
+                             obsDistance_log=obsDistance;
+                         QDateTime dt2=QDateTime::currentDateTime();
+                         outfile <<dt2.time().hour()<<":"<<dt2.time().minute()<<":"<<dt2.time().second()<<":"<<dt2.time().msec()<<","
+                                 <<"Decide_Dspeed"  << ","  <<setprecision(2)<<dSpeed<< ","
+                                 <<"Decide_ACC"  << ","  <<setprecision(2)<<gps_decition->torque<< ","
+                                 <<"Decide_Brake"<< ","  <<gps_decition->brake<< ","
+                                 <<"Vehicle_RealSpd"<< ","  <<setprecision(2)<<now_gps_ins.speed<< ","
+  //                            <<"readyParkMode"<< ","<<readyParkMode<< ","
+  //                              <<"readyZhucheMode"<< ","<<readyZhucheMode<< ","
+                                <<"OBS_Distance"<< ","<<obsDistance_log<< ","
+                                 <<"OBS_Speed"<< ","<<obs_speed_for_avoid<< ","
+                                <<"obsSpeed_fusion"<<","<<obsSpeed<<","
+                                <<"SecSpeed"<<","<<secSpeed<<","
+                                 <<"Vehicle_state"<< ","<<vehState<< ","
+                                <<"avoid_flag"<<","<<obstacle_avoid_flag<<","
+                                 <<"avoidXNew"<<","<<avoidXNew<<","
+                                 <<"avoidXNewPre"<<","<<avoidXNewPre<<","
+                                 //<<"avoidXPre"<<","<<avoidXPre<<","
+                                 <<"ObsTimeEnd"<<","<<ObsTimeEnd<<","
+                                 <<"avoidXY_size"<<","<<gpsTraceAvoidXY.size()<<","
+                                 <<"Min_Decelation"","<<minDecelerate<< ","
+      //                           <<"Decide_Angle"<< ","  << setprecision(2)<<gps_decition->wheel_angle<< ","
+                                 <<"Vehicle_GPS_heading"<< ","<< setprecision(10)<<now_gps_ins.ins_heading_angle<< ","
+                                 <<"Vehicle_GPS_lat"<< ","<< setprecision(10)<<now_gps_ins.gps_lat<< ","
+                                 <<"Vehicle_GPS_lng"<< ","<< setprecision(10)<<now_gps_ins.gps_lng<< ","
+                                <<"Vehicle_GPS_X"<< ","<< setprecision(10)<<now_gps_ins.gps_x<< ","
+                                <<"Vehicle_GPS_Y"<< ","<< setprecision(10)<<now_gps_ins.gps_y<< ","
+                                <<"MAP_GPS_heading"<< ","<< setprecision(10)<<gpsMapLine[PathPoint]->ins_heading_angle<< ","
+                                <<"MAP_GPS_X"<< ","<< setprecision(10)<<gpsMapLine[PathPoint]->gps_x<< ","
+                                <<"MAP_GPS_Y"<< ","<< setprecision(10)<<gpsMapLine[PathPoint]->gps_y<< ","
+                                 <<"Trace_Point"<< ","<<PathPoint<< ","
+                                 <<"changingDangWei"<< ","<<changingDangWei<< ","
+                                 <<"gpsTraceOri"<< ","<<gpsTraceOri.size()<< ","
+      //                           <<"TTC"<< ","<<ttc<< ","
+  //                               <<"Decide_torque"  << ","  <<setprecision(2)<<gps_decition->torque<< ","
+  //                               <<"Decide_Brake"<< ","  <<gps_decition->brake<< ","
+  //                               <<"Vehicle_RealSpd"<< ","  <<setprecision(2)<<now_gps_ins.speed<< ","
+  //                               <<"OBS_Distance"<< ","<<obsDistance_log<< ","
+  //                               <<"OBS_Speed"<< ","<<obs_speed_for_avoid<< ","
+  //                               <<"Vehicle_state"<< ","<<vehState<< ","
+  //                               <<"avoid_flag"<<","<<obstacle_avoid_flag<<","
+  //                               <<"avoidXNew"<<","<<avoidXNew<<","
+  //                               <<"avoidXNewPre"<<","<<avoidXNewPre<<","
+  ////                               <<"avoidXPre"<<","<<avoidXPre<<","
+  //                               <<"ObsTimeEnd"<<","<<ObsTimeEnd<<","
+                               <<"avoidXY_size"<<","<<gpsTraceAvoidXY.size()<<","
+  //                               <<"Min_Decelation"","<<minDecelerate<< ","
+                                 <<"Decide_Angle"<< ","  << setprecision(2)<<gps_decition->wheel_angle<< ","
+                                // <<"Vehicle_GPS_heading"<< ","<< setprecision(10)<<now_gps_ins.ins_heading_angle<< ","
+                                // <<"Vehicle_GPS_X"<< ","<< setprecision(10)<<now_gps_ins.gps_lat<< ","
+                                // <<"Vehicle_GPS_Y"<< ","<< setprecision(10)<<now_gps_ins.gps_lng<< ","
+                                // <<"Trace_Point"<< ","<<PathPoint<< ","
+                                // <<"OBS_Speed"<< ","<<obsSpeed<< ","
+                                // <<"OBS_Distance"<< ","<<obsDistance<< ","
+                                // <<"TTC"<< ","<<ttc<< ","
+                                 <<endl;
+                         outfile.close();
+                     }
+                  }
+  }
 
     lastAngle=gps_decition->wheel_angle;
     lastVehState=vehState;
@@ -4150,9 +4384,10 @@ int iv::decition::DecideGps00::computeAvoidXDistance(iv::LidarGridPtr lidarGridP
     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 
 
     obs_property.clear();
-    for (int i =  avoidLeft; i <= avoidRight; i++)
+    for (int i =  avoidLeft; i <= avoidRight; i=i+step)
     {
         obsvalue x_value;
         obsvalue *x = &x_value;
@@ -4200,6 +4435,18 @@ int iv::decition::DecideGps00::computeAvoidXDistance(iv::LidarGridPtr lidarGridP
                 givlog->debug("decition_brain","singrecord_Y: %d",signed_record_avoidX);
             }
         }
+        //added by lyp
+        // if((signed_record_avoidX < 0) && (signed_record_avoidX-1 >= avoidLeft))
+        // {
+        //     //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);
+        //     }
+        // }
+
         if(signed_record_avoidX == 0)
         {
             for(int j = 0; j < obs_property.size(); j++)

src/decition/decition_brain_sf_changan_shenlan/decition/decide_gps_00.h

@@ -272,7 +272,7 @@ public:
     float  ObsTimeSpan=-1;
     double ObsTimeStart=-1;
     double ObsTimeEnd=-1;
-    float ObsTimeWidth=70.0;   //500   zj-30
+    float ObsTimeWidth=10;//70.0;   //500   zj-30
     std::vector<iv::TracePoint> planTrace;
 
     bool roadNowStart=true;