|
|
@@ -172,6 +172,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,
|
|
|
@@ -301,9 +309,10 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
|
|
|
|
|
|
// GaussProjInvCal(now_gps_ins.gps_x, now_gps_ins.gps_y,&now_gps_ins.gps_lng, &now_gps_ins.gps_lat);
|
|
|
|
|
|
-
|
|
|
+ static int file_num;
|
|
|
if (isFirstRun)
|
|
|
{
|
|
|
+ file_num=0;
|
|
|
initMethods();
|
|
|
vehState = normalRun;
|
|
|
startAvoid_gps_ins = now_gps_ins;
|
|
|
@@ -2411,18 +2420,233 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
|
|
|
givlog->debug("decition_brain","vehState: %d,PathPoint: %d,dSpeed: %f,obsDistance: %f,obsSpeed: %f,realspeed: %f,minDecelerate: %f,torque: %f,brake: %f,wheel_angle: %f,obs_speed_for_avoid: %f,mode2: %d,now_s: %f,now_d: %f",
|
|
|
vehState,PathPoint,dSpeed,obsDistance,obsSpeed,realspeed,minDecelerate,gps_decition->torque,gps_decition->brake,gps_decition->wheel_angle,obs_speed_for_avoid,gpsMapLine[PathPoint]->mode2,now_s_d.s,now_s_d.d);
|
|
|
|
|
|
- if(ServiceCarStatus.txt_log_on==true){
|
|
|
- QDateTime dt = QDateTime::currentDateTime();
|
|
|
- //将数据写入文件开始
|
|
|
- ofstream outfile;
|
|
|
- outfile.open("control_log.txt", ostream::app);
|
|
|
- outfile<<dt.date().year()<<"-"<<dt.date().month()<<"-"<<dt.date().day()<<" "<<dt.time().hour()<<":"<<dt.time().minute()<<":"<<dt.time().second()<<"-"<<dt.time().msec()<<'\t'
|
|
|
- <<vehState<<'\t'<<PathPoint<<'\t'<<setprecision(4)<<dSpeed<<'\t'<<obsDistance<<'\t'<<obsSpeed<<'\t'<<realspeed<<'\t'<<minDecelerate<<'\t'
|
|
|
- <<gps_decition->torque<<'\t'<<gps_decition->brake<<'\t'<<gps_decition->wheel_angle<<'\t'<<now_gps_ins.gps_x<<'\t'<<now_gps_ins.gps_y<<'\t'<<now_s_d.s<<'\t'<<now_s_d.d<<endl;
|
|
|
- outfile.close();
|
|
|
- //将数据写入文件结束
|
|
|
- }
|
|
|
+// if(ServiceCarStatus.txt_log_on==true){
|
|
|
+// QDateTime dt = QDateTime::currentDateTime();
|
|
|
+// //将数据写入文件开始
|
|
|
+// ofstream outfile;
|
|
|
+// outfile.open("control_log.txt", ostream::app);
|
|
|
+// outfile<<dt.date().year()<<"-"<<dt.date().month()<<"-"<<dt.date().day()<<" "<<dt.time().hour()<<":"<<dt.time().minute()<<":"<<dt.time().second()<<"-"<<dt.time().msec()<<'\t'
|
|
|
+// <<vehState<<'\t'<<PathPoint<<'\t'<<setprecision(4)<<dSpeed<<'\t'<<obsDistance<<'\t'<<obsSpeed<<'\t'<<realspeed<<'\t'<<minDecelerate<<'\t'
|
|
|
+// <<gps_decition->torque<<'\t'<<gps_decition->brake<<'\t'<<gps_decition->wheel_angle<<'\t'<<now_gps_ins.gps_x<<'\t'<<now_gps_ins.gps_y<<'\t'<<now_s_d.s<<'\t'<<now_s_d.d<<endl;
|
|
|
+// 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<< ","
|
|
|
+// <<"zuo_ultra"<< ","<<ServiceCarStatus.mbUltraDis[3]<< ","
|
|
|
+// <<"you_ultra"<< ","<<ServiceCarStatus.mbUltraDis[1]<< ","
|
|
|
+ // <<"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<< ","
|
|
|
+// <<"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;
|
chenxiaowei