1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063106410651066106710681069107010711072107310741075107610771078107910801081108210831084108510861087108810891090109110921093109410951096109710981099110011011102110311041105110611071108110911101111111211131114111511161117111811191120112111221123112411251126112711281129113011311132113311341135113611371138113911401141114211431144114511461147114811491150115111521153115411551156115711581159116011611162116311641165116611671168116911701171117211731174117511761177117811791180118111821183118411851186118711881189119011911192119311941195119611971198119912001201120212031204120512061207120812091210121112121213121412151216121712181219122012211222122312241225122612271228122912301231123212331234123512361237123812391240124112421243124412451246124712481249125012511252125312541255125612571258125912601261126212631264126512661267126812691270127112721273127412751276127712781279128012811282128312841285128612871288128912901291129212931294129512961297129812991300130113021303130413051306130713081309131013111312131313141315131613171318131913201321132213231324132513261327132813291330133113321333133413351336133713381339134013411342134313441345134613471348134913501351135213531354135513561357135813591360136113621363136413651366136713681369137013711372137313741375137613771378137913801381138213831384138513861387138813891390139113921393139413951396139713981399140014011402140314041405140614071408140914101411141214131414141514161417141814191420142114221423142414251426142714281429143014311432143314341435143614371438143914401441144214431444144514461447144814491450145114521453145414551456145714581459146014611462146314641465146614671468146914701471147214731474147514761477147814791480148114821483148414851486148714881489149014911492149314941495149614971498149915001501150215031504150515061507150815091510151115121513151415151516151715181519152015211522152315241525152615271528152915301531153215331534153515361537153815391540154115421543154415451546154715481549155015511552155315541555155615571558155915601561156215631564156515661567156815691570157115721573157415751576157715781579158015811582158315841585158615871588158915901591159215931594159515961597159815991600160116021603160416051606160716081609161016111612161316141615161616171618161916201621162216231624162516261627162816291630163116321633163416351636163716381639164016411642164316441645164616471648164916501651165216531654165516561657165816591660166116621663166416651666166716681669167016711672167316741675167616771678167916801681168216831684168516861687168816891690169116921693169416951696169716981699170017011702170317041705170617071708170917101711171217131714171517161717171817191720172117221723172417251726172717281729173017311732173317341735173617371738173917401741174217431744174517461747174817491750175117521753175417551756175717581759176017611762176317641765176617671768176917701771177217731774177517761777177817791780178117821783178417851786178717881789179017911792179317941795179617971798179918001801180218031804180518061807180818091810181118121813181418151816181718181819182018211822182318241825182618271828182918301831183218331834183518361837183818391840184118421843184418451846184718481849185018511852185318541855185618571858185918601861186218631864186518661867186818691870187118721873187418751876187718781879188018811882188318841885188618871888188918901891189218931894189518961897189818991900190119021903190419051906190719081909191019111912191319141915191619171918191919201921192219231924192519261927192819291930193119321933193419351936193719381939194019411942194319441945194619471948194919501951195219531954195519561957195819591960196119621963196419651966196719681969197019711972197319741975197619771978197919801981198219831984198519861987198819891990199119921993199419951996199719981999200020012002200320042005200620072008200920102011201220132014201520162017201820192020202120222023202420252026202720282029203020312032203320342035203620372038203920402041204220432044204520462047204820492050205120522053205420552056205720582059206020612062206320642065206620672068206920702071207220732074207520762077207820792080208120822083208420852086208720882089209020912092209320942095209620972098209921002101210221032104210521062107210821092110211121122113211421152116211721182119212021212122212321242125212621272128212921302131213221332134213521362137213821392140214121422143214421452146214721482149215021512152 |
- #include "ADCIntelligentVehicle.h"
- #include "chassis.pb.h"
- #include "xmlparam.h"
- #define qtcout qDebug() << "[ " << __FILE__ << ":" << __LINE__<< " ]";
- extern std::string gstrmemgps;
- /**
- 常量 值 描述
- QHostAddress::Null 0 空地址对象,相当于QHostAddress()。
- QHostAddress::LocalHost 2 IPv4本地主机地址,相当于QHostAddress(“127.0.0.1”)。
- QHostAddress::LocalHostIPv6 3 IPv6本地主机地址,相当于 QHostAddress(“::1”)。
- QHostAddress::Broadcast 1 Pv4广播地址,相当于QHostAddress(“255.255.255.255”)。
- QHostAddress::AnyIPv4 6 IPv4 any-address,相当于QHostAddress(“0.0.0.0”)。与该地址绑定的socket将只监听IPv4接口。
- QHostAddress::AnyIPv6 5 IPv6 any-address,相当于QHostAddress(“::”)。与该地址绑定的socket将只监听IPv4接口。
- QHostAddress::Any 4 双any-address栈,与该地址绑定的socket将侦听IPv4和IPv6接口。
- */
- #define VIEW_CENTER viewport()->rect().center()
- const double PI = 3.1415926535898;
- extern iv::Ivlog * gIvlog;
- /**
- * @brief MyView::MyView
- * @param parent
- */
- MyView::MyView(QWidget *parent) :
- QGraphicsView(parent),
- beishu(1.00000)
- {
- setDragMode(QGraphicsView::ScrollHandDrag); //设置视图的拖拽模式 (光标变为指向手,然后拖动鼠标将滚动滚动条,此模式在交互和非交互模式下均有效)
- }
- /**
- * @brief MyView::mousePressEvent
- * @param event
- */
- void MyView::mousePressEvent(QMouseEvent *event)
- {
- bottonstatus = true;
- QGraphicsView::mousePressEvent(event);
- }
- /**
- * @brief MyView::mouseMoveEvent
- * @param event
- */
- void MyView::mouseMoveEvent(QMouseEvent *event)
- {
- QGraphicsView::mouseMoveEvent(event);
- }
- void MyView::keyPressEvent(QKeyEvent *event)
- {
- //按键按下,key值放入容器,如果是长按触发的repeat就不判断
- if(!event->isAutoRepeat())
- mPressKeys.insert(event->key());
- if(event->key() == Qt::Key_J)
- {
- emit CtrlManual(true);
- }
- if(event->key() == Qt::Key_K)
- {
- emit CtrlManual(false);
- }
- // qDebug("key count is %d",mPressKeys.count());
- // QDateTime dt;
- // qDebug("key press %ld",QDateTime::currentMSecsSinceEpoch());
- // qDebug(" key is %d",event->key());
- }
- void MyView::keyReleaseEvent(QKeyEvent *event)
- {
- if(!event->isAutoRepeat())mPressKeys.remove(event->key());
- qDebug("key count is %d",mPressKeys.count());
- // QDateTime dt;
- // qDebug("key release %ld",QDateTime::currentMSecsSinceEpoch());
- }
- /**
- * @brief MyView::mouseReleaseEvent
- * @param event
- */
- void MyView::mouseReleaseEvent(QMouseEvent *event)
- {
- bottonstatus = false;
- QGraphicsView::mouseReleaseEvent(event);
- }
- ADCIntelligentVehicle * gAV;
- //void ListenTraceMap(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
- //{
- // gAV->UpdateMap(strdata,nSize);
- //}
- // void Listengpsimu(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
- // {
- // gAV->UpdateGPSIMU(strdata,nSize);
- // }
- // void ListenRadar(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
- // {
- // gAV->UpdateRADAR(strdata,nSize);
- // }
- /**
- * @brief ListenCANState
- * @param strdata
- * @param nSize
- * @param index
- * @param dt
- * @param strmemname
- */
- void ListenCANState(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
- {
- // gIvlog->debug("ListenCanState");
- gAV->UpdateCANState(strdata,nSize);
- }
- //void ListenDecition(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
- //{
- // gAV->UpdateDecition(strdata,nSize);
- //}
- //void ListenVehicleState(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
- //{
- // gAV->UpdateVehicleState(strdata,nSize);
- //}
- /**
- * @brief ListenOBS
- * @param strdata
- * @param nSize
- * @param index
- * @param dt
- * @param strmemname
- */
- void ListenOBS(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
- {
- // qDebug("size is %d",nSize);
- std::shared_ptr<std::vector<iv::ObstacleBasic>> lidar_obs(new std::vector<iv::ObstacleBasic>);
- iv::ObstacleBasic * pdata = (iv::ObstacleBasic *)strdata;
- int nCount = nSize/sizeof(iv::ObstacleBasic);
- int i;
- for(i=0;i<nCount;i++)
- {
- iv::ObstacleBasic temp;
- memcpy(&temp,pdata,sizeof(iv::ObstacleBasic));
- lidar_obs->push_back(temp);
- pdata++;
- }
- gAV->UpdateOBS(lidar_obs);
- // gw->UpdateOBS(lidar_obs);
- }
- //v2x使能状态请求
- void ListenV2xStReq(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
- {
- // gIvlog->info("hmi", "v2x enable request %d %d",nSize,sizeof(iv::v2x::v2xStReq));
- // if(nSize < sizeof(iv::v2x::v2xStReq))
- // return;
- iv::v2x::v2xStReq xv2xStReqMsg;
- if(!xv2xStReqMsg.ParseFromArray(strdata,nSize))
- {
- gIvlog->error("hmi", "iv::v2x::xv2xStReqMsg::ListenV2xStReq parse error");
- return;
- }
- if(xv2xStReqMsg.v2xstreq())
- {
- gAV->UpdateV2xStEn(gAV->mv2xStEn);
- gIvlog->info("hmi", "v2x enable request %d",gAV->mv2xStEn);
- }
- }
- /**
- * @brief ADCIntelligentVehicle::ADCIntelligentVehicle
- * @param parent
- */
- ADCIntelligentVehicle::ADCIntelligentVehicle(QWidget *parent)
- : QMainWindow(parent),
- ui(new Ui::ADCIntelligentVehicle),
- m_translateButton(Qt::LeftButton),
- m_translateSpeed(1.0),
- m_zoomDelta(0.1),
- m_bMouseTranslate(false),
- m_scale(1.0),
- horizontalOffset(0),
- verticalOffset(0),
- scaleFactor(1),
- currentStepScaleFactor(1)
- {
- ui->setupUi(this);
- gAV = this;
- gIvlog->info("ADCIntelligentVehicle Start Initialize");
- mTime.start();
- /*
- QString strpath = QCoreApplication::applicationDirPath();
- strpath = strpath + "/ADS_decision.xml";
- iv::xmlparam::Xmlparam xp(strpath.toStdString());
- ServiceCarStatus.mvehtype= xp.GetParam("vehType","ge3");
- if(ServiceCarStatus.mvehtype=="ge3"){
- ;
- }else if( ServiceCarStatus.mvehtype=="qingyuan"){
- ;
- }else if( ServiceCarStatus.mvehtype=="vv7"){
- ;
- }else if( ServiceCarStatus.mvehtype=="zhongche"){
- ;
- }
- */
- ui->listWidget->setIconSize(QSize(40,40));
- ui->stackedWidget->setCurrentIndex(0);
- // QObject::connect(ui->listWidget,SIGNAL(currentRowChanged(int)),ui->stackedWidget,SLOT(on_listWidget_clicked()));//信号与槽
- QObject::connect(ui->listWidget,SIGNAL(currentRowChanged(int)),this,SLOT(on_listWidget_clicked()));//信号与槽
- ui->listWidget->setCurrentRow(1); //apollo_fu debug 20200409
- myview = new MyView(ui->stackedWidget->widget(1));
- myview->setObjectName(QStringLiteral("graphicsView"));
- myview->setGeometry(QRect(0, 100, 900, 900)); //从屏幕上(0,100)位置开始(即为最左上角的点),显示一个900*900的界面(宽900,高900)
- myview_small = new MyView(ui->stackedWidget->widget(1));
- myview_small->setObjectName(QStringLiteral("graphicsView_small"));
- myview_small->setGeometry(QRect(1000,100,450,450));
- image = new QImage(900 * 2, 900 * 2, QImage::Format_RGB32);//画布的初始化大小设为600*500,使用32位颜色
- //QImage的32、24、8位图。 图像格式:QImage::Format_RGB32 ,QImage::Format_RGB888,QImage::Format_Indexed8。
- //QImage myImage2 = QImage(width, height, QImage::Format_…); 根据图像宽高来构造一幅图像,程序会自动根据图像格式对齐图像数据。
- image_small = new QImage(900 * 2,900 * 2,QImage::Format_RGB32);
- myview->setCacheMode(myview->CacheBackground);
- myview_small->setCacheMode(myview_small->CacheBackground);
- painter = new QPainter(image); //创建QPainter对象
- painter_small = new QPainter(image_small); //创建QPainter对象
- scene = new QGraphicsScene;
- scene_small = new QGraphicsScene;
- look_decition = new Look_decition;
- mobileye_info = new Mobileye_info;
- timer = new QTimer(this);
- connect(timer, SIGNAL(timeout()), this, SLOT(timeoutslot()));//timeoutslot()为自定义槽
- timer->start(100);
- Lidar_obsread = boost::shared_ptr<std::vector<iv::ObstacleBasic>>(new std::vector<iv::ObstacleBasic>);
- Lidar_read = boost::shared_ptr<std::vector<iv::ObstacleBasic>>(new std::vector<iv::ObstacleBasic>);
- mDataToUI.mHead.type = 1;
- mDataToUI.mInfo.accelerate = 0;
- mDataToUI.mInfo.brake = 0;
- mDataToUI.mInfo.gps_lat = 0;
- mDataToUI.mInfo.gps_lng = 0;
- mDataToUI.mInfo.is_mapLoad = 0;
- mDataToUI.mInfo.is_initSuccess = 0;
- mDataToUI.mInfo.is_run = 0;
- mDataToUI.mInfo.speed = 0;
- msockrecv.bind(QHostAddress::Any,9998);//绑定端口
- connect(&msockrecv,SIGNAL(readyRead()),this,SLOT(onRecvUDP())); //接收来自PAD的UDP数据报 readyRead():socket读取缓冲区有数据时发送此信号,在此信号的槽函数里读取缓冲区的数据。
- ServiceCarStatus.speed = 0.0;
- mTimerState.setTimerType(Qt::PreciseTimer); //设置计时器的精确程度 Qt::PreciseTimer值为0,含义:精确定时器试图保持毫秒精度
- connect(&mTimerState,SIGNAL(timeout()),this,SLOT(onStateTimer()));//广播发送UDP数据报
- mTimerState.start(100); //定时100ms
- is_show_enable = true;
- #ifdef USE_PAD_CTRL
- AutoStart();
- #endif
- ModuleFun funplantrace = std::bind(&ADCIntelligentVehicle::UpdatePlanTrace,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
- mpaplantrace = iv::modulecomm::RegisterRecvPlus("plantrace",funplantrace);
- ModuleFun funplantrace_left = std::bind(&ADCIntelligentVehicle::UpdatePlanTrace_left,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
- mpaplantrace_left = iv::modulecomm::RegisterRecvPlus("obstraceleft",funplantrace_left);
- ModuleFun funplantrace_right = std::bind(&ADCIntelligentVehicle::UpdatePlanTrace_right,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
- mpaplantrace_right = iv::modulecomm::RegisterRecvPlus("obstraceright",funplantrace_right);
- ModuleFun funmap = std::bind(&ADCIntelligentVehicle::UpdateMap,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
- mpa = iv::modulecomm::RegisterRecvPlus("tracemap",funmap);
- // mpa = iv::modulecomm::RegisterRecv("tracemap",ListenTraceMap);
- ModuleFun fungpsimu =std::bind(&ADCIntelligentVehicle::UpdateGPSIMU,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
- mpagpsimu = iv::modulecomm::RegisterRecvPlus(gstrmemgps.data(),fungpsimu);
- // mpagpsimu = iv::modulecomm::RegisterRecv("gpsimu",Listengpsimu);
- ModuleFun funradar =std::bind(&ADCIntelligentVehicle::UpdateRADAR,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
- mparadar = iv::modulecomm::RegisterRecvPlus("radar",funradar);
- ModuleFun funchassis =std::bind(&ADCIntelligentVehicle::UpdateChassis,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
- mpachassis = iv::modulecomm::RegisterRecvPlus("chassis",funchassis);
- mpacanstate = iv::modulecomm::RegisterRecv("canstate",ListenCANState);
- gIvlog->debug("canState ptr: %x",mpacanstate);
- ModuleFun funbrainstate =std::bind(&ADCIntelligentVehicle::UpdateVehicleState,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
- // mpaVechicleState = iv::modulecomm::RegisterRecvPlus("vehiclestate",funbrainstate);
- mpaVechicleState = iv::modulecomm::RegisterRecvPlus("brainstate",funbrainstate);
- ModuleFun fundecition =std::bind(&ADCIntelligentVehicle::UpdateDecition,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
- // mpaDecition = iv::modulecomm::RegisterRecvPlus("decition",fundecition);
- mpaDecition = iv::modulecomm::RegisterRecvPlus("deciton",fundecition);
- mpaLidar = iv::modulecomm::RegisterRecv("lidar_obs",ListenOBS);
- mpaHMI = iv::modulecomm::RegisterSend("hmi",10*sizeof(iv::hmi::HMIBasic),10);
- iv::modulecomm::RegisterRecv("v2xStReq", ListenV2xStReq);
- mp_v2xStSend = iv::modulecomm::RegisterSend("v2xStEn",1000,1);
- mTimeState.start();
- mpManualCtrl = iv::modulecomm::RegisterSend("ivmanual",1000,1);
- mTimerManual = new QTimer(this);
- connect(mTimerManual,SIGNAL(timeout()),this,SLOT(onTimerManual()));
- connect(myview,SIGNAL(CtrlManual(bool)),this,SLOT(onCtrlManual(bool)));
- mnCtrlValue[0] = 0; mnCtrlValue[1] = 0; mnCtrlValue[2] = 0;
- // timermanualctrl->start(10);
- //connect(ui->lineEdit_6, SIGNAL(textEdited(const QString &)), this, SLOT(savestabuyEditinfo(const QString &)));
- mpamapreq = iv::modulecomm::RegisterSend("mapreq",1000,1);
- shareMapReqMsg();
- ModuleFun funvbox = std::bind(&ADCIntelligentVehicle::UpdateVbox,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
- mpaplantrace = iv::modulecomm::RegisterRecvPlus("vbox",funvbox);
- #if 1
- ui->lcdNumber->setDigitCount(3); //设置显示几个数字
- ui->lcdNumber->setMode(QLCDNumber::Dec);
- ui->lcdNumber->setSegmentStyle(QLCDNumber::Flat);//设置数字字体:Outline,轮廓突出,颜色为背景色;Filled,内部填充型,颜色为黑色;Flat,平面型
- // 设置数字颜色时要注意: 函数setSegmentStyle(QLCDNumber::Flat)中选择Flat属性,否则在setColor()设置数字颜色时不生效
- // 设置背景色直接用setStyleSheet()函数设定就可以了。
- //调色板
- QPalette lcdpat = ui->lcdNumber->palette();
- /*设置颜色,整体背景颜色 颜色蓝色,此函数的第一个参数可以设置多种。如文本、按钮按钮文字、多种*/
- lcdpat.setColor(QPalette::Normal,QPalette::WindowText,Qt::red);
- //设置当前窗口的调色板
- ui->lcdNumber->setPalette(lcdpat);
- //设置背景色
- //ui->lcdNumber->setStyleSheet("background-color: black");
- //ui->lcdNumber->display(QDateTime::currentDateTime().toString("yyyy-MM-dd hh:mm:ss"));
- ui->lcdNumber->display(0);
- ui->lcdNumber_2->setDigitCount(3);
- ui->lcdNumber_2->setMode(QLCDNumber::Dec);
- ui->lcdNumber_2->setSegmentStyle(QLCDNumber::Flat);
- ui->lcdNumber_2->setPalette(lcdpat);
- //ui->lcdNumber_2->setStyleSheet("background-color: black");
- ui->lcdNumber_2->display(0);
- ui->lcdNumber_3->setDigitCount(3);
- ui->lcdNumber_3->setMode(QLCDNumber::Dec);
- ui->lcdNumber_3->setSegmentStyle(QLCDNumber::Flat);
- ui->lcdNumber_3->setPalette(lcdpat);
- //ui->lcdNumber_3->setStyleSheet("background-color: black");
- ui->lcdNumber_3->display(0);
- ui->lcdNumber_4->setDigitCount(3);
- ui->lcdNumber_4->setMode(QLCDNumber::Dec);
- ui->lcdNumber_4->setSegmentStyle(QLCDNumber::Flat);
- ui->lcdNumber_4->setPalette(lcdpat);
- //ui->lcdNumber_4->setStyleSheet("background-color: black");
- ui->lcdNumber_4->display(0);
- #endif
- // 新建qimage
- // QImage img,img2,img3,img4;
- // 加载图片
- img.load(":/light_image/diaotou-black.png");
- img = img.scaled(66,66,Qt::IgnoreAspectRatio);
- ui->label_38->setPixmap(QPixmap::fromImage(img));
- img2.load(":/light_image/zuoguai-black.png");
- img2 = img2.scaled(66,66,Qt::IgnoreAspectRatio);
- ui->label_33->setPixmap(QPixmap::fromImage(img2));
- img3.load(":/light_image/zhixing-black.png");
- img3 = img3.scaled(66,66,Qt::IgnoreAspectRatio);
- ui->label_35->setPixmap(QPixmap::fromImage(img3));
- img4.load(":/light_image/youguai-black.png");
- img4 = img4.scaled(66,66,Qt::IgnoreAspectRatio);
- ui->label_32->setPixmap(QPixmap::fromImage(img4));
- #if 1
- mTimerState1.setTimerType(Qt::PreciseTimer); //设置计时器的精确程度 Qt::PreciseTimer值为0,含义:精确定时器试图保持毫秒精度
- connect(&mTimerState1,SIGNAL(timeout()),this,SLOT(onStateTimer1()));//广播发送UDP数据报
- mTimerState1.start(100); //定时100ms
- #endif
- }
- /**
- * @brief ADCIntelligentVehicle::savestabuyEditinfo
- * @param txt
- */
- void ADCIntelligentVehicle::savestabuyEditinfo(const QString &txt)
- {
- //ServiceControlStatus.set_accelerate((float)txt.toDouble());
- }
- /**
- * @brief ADCIntelligentVehicle::timeoutslot
- */
- void ADCIntelligentVehicle::timeoutslot()
- {
- update();
- /////////////////////////////////
- // //
- // 实时状态信息显示 //
- ////////////////////////////////
- QString is_ok;
- ui->lineEdit->setText(QString::number(ServiceCarStatus.location->rtk_status));//rtk状态
- if(ServiceCarStatus.location->rtk_status==6)
- ui->pushButton_10->setStyleSheet("background-color: green");
- else
- ui->pushButton_10->setStyleSheet("background-color: red");
- ui->lineEdit_2->setText(QString::number(ServiceCarStatus.location->ins_status));//ins状态
- if(ServiceCarStatus.location->ins_status==4)
- ui->pushButton_11->setStyleSheet("background-color: green");
- else
- ui->pushButton_11->setStyleSheet("background-color: red");
- is_ok = (ServiceCarStatus.mRadarS>0)?QStringLiteral("ok"):QStringLiteral("error");
- ui->lineEdit_3->setText(is_ok);//毫米波雷达状态
- if(ServiceCarStatus.mRadarS>0)
- ui->pushButton_12->setStyleSheet("background-color: green");
- else
- ui->pushButton_12->setStyleSheet("background-color:red");
- is_ok = (ServiceCarStatus.mLidarS>0)?QStringLiteral("ok"):QStringLiteral("error");
- ui->lineEdit_4->setText(is_ok);//激光雷达状态
- if(ServiceCarStatus.mLidarS>0)
- ui->pushButton_13->setStyleSheet("background-color: green");
- else
- ui->pushButton_13->setStyleSheet("background-color:red");
- ui->lineEdit_5->setText(QString::number(mdecition_period)+QStringLiteral("ms"));//决策周期
- if(mdecition_period>0)
- ui->pushButton_14->setStyleSheet("background-color: green");
- if((mTimeState.elapsed()-mnTimeUpdateGPS) > 1000)
- {
- ui->pushButton_10->setStyleSheet("background-color: red");
- ui->pushButton_11->setStyleSheet("background-color: red");
- }
- if((mTimeState.elapsed() - mnTimeUpdateDecition)>1000)
- {
- ui->pushButton_14->setStyleSheet("background-color:red");
- ui->label_decition->setText("No decition");
- mbBrainRunning = false;
- }
- else
- {
- QString strdec = "Acc:";
- strdec = strdec + QString::number(ServiceCarStatus.mfAcc,'f',1);
- // strdec = strdec + " Brake:";
- // strdec = strdec + QString::number(ServiceCarStatus.mfBrake,'f',1);
- strdec = strdec + " EPS:";
- strdec = strdec + QString::number(ServiceCarStatus.mfWheel,'f',1);
- ui->label_decition->setText(strdec);
- }
- if(mbManual)
- {
- char strmanual[256];
- snprintf(strmanual,256,"Manual %d %d %d",mnCtrlValue[0],mnCtrlValue[1],mnCtrlValue[2]);
- ui->label_decition->setText(strmanual);
- }
- #if 0
- ui->lineEdit->setText(QString::number(6));//rtk状态
- ui->pushButton_10->setStyleSheet("background-color: green");
- ui->lineEdit_2->setText(QString::number(4));
- ui->pushButton_11->setStyleSheet("background-color: green");
- ui->lineEdit_3->setText("ok");
- ui->pushButton_12->setStyleSheet("background-color: green");
- ui->lineEdit_4->setText("ok");
- ui->pushButton_13->setStyleSheet("background-color: green");
- ui->lineEdit_5->setText(QString::number(20)+QStringLiteral("ms"));//决策周期
- ui->pushButton_14->setStyleSheet("background-color: green");
- #endif
- /////////////////////////////////
- // //
- // 实时显示障碍物状态 //
- /////////////////////////////////
- // ServiceCarStatus.mObs = 10;
- // ServiceCarStatus.mLidarObs = 9.123;
- // ServiceCarStatus.mRadarObs = 9.222;
- // ServiceCarStatus.mfttc = 0.5;
- // ServiceCarStatus.mfBrake = 20;
- if(ServiceCarStatus.mObs > 0)
- {
- ui->label_13->setText("Obs:"+QString::number(ServiceCarStatus.mObs,'f',1)+" Lidar:"
- +QString::number(ServiceCarStatus.mLidarObs,'f',1) + " Radar:"
- +QString::number(ServiceCarStatus.mRadarObs,'f',1));
- }
- else
- {
- ui->label_13->setText(QString("前方没有障碍物"));
- }
- ui->label_26->setText("ttc is "+QString::number(ServiceCarStatus.mfttc,'f',1) + " brake is " + QString::number(ServiceCarStatus.mfBrake,'f',2)); //add 20200518
- QString strstate = "Map:";
- if(mDataToUI.mInfo.is_mapLoad == 1)
- {
- strstate = strstate+"Yes ";
- }
- else
- {
- strstate = strstate+"No ";
- }
- strstate = strstate+" CANCARD:";
- if(mDataToUI.mInfo.is_initSuccess == 1)
- {
- strstate = strstate+"OK ";
- }
- else
- {
- strstate = strstate+"Fail ";
- }
- strstate = strstate + " Run State:";
- if(mbBrainRunning)
- {
- strstate = strstate+"RUN";
- }
- else
- {
- strstate = strstate+"STOP";
- }
- ui->label_State->setText(strstate);
- /////////////////////////////////
- // //
- // 模式选择标志置位 //
- // //
- ////////////////////////////////
- // if(ui->checkBox->isChecked()==true)ServiceControlStatus.change_line = 1;//是否换道标志
- // else ServiceControlStatus.change_line = 0;
- // if(ui->checkBox_2->isChecked()==true)ServiceControlStatus.stop_obstacle = 1;//是否停障标志
- // else ServiceControlStatus.stop_obstacle = 0;
- // if(ui->checkBox_3->isChecked()==true)ServiceControlStatus.elude_obstacle = 1;//是否避障标志
- // else ServiceControlStatus.elude_obstacle = 0;
- // if(ui->checkBox_4->isChecked()==true)ServiceControlStatus.special_signle = 1;//是否特殊信号输入标志
- // else ServiceControlStatus.special_signle = 0;
- // if(ui->checkBox_5->isChecked()==true)ServiceControlStatus.car_pullover = 1;//是否靠边停车标志位
- // else ServiceControlStatus.car_pullover = 0;
- ui->lb_current_speed_big->setText(QString::number(ServiceCarStatus.speed,'f',1));
- // QString is_ok;
- // is_ok = (ServiceLidar.did_lidar_ok() == true)?QStringLiteral("ok"):QStringLiteral("error");
- // ui->lineEdit_4->setText(is_ok);
- //can state
- mDataToUI.mInfo.is_initSuccess = 0;
- }
- /**
- * @brief ADCIntelligentVehicle::on_pb_load_navigation_data_clicked
- */
- //void ADCIntelligentVehicle::on_pb_load_navigation_data_clicked() //加载地图
- //{
- //}
- /**
- * @brief ADCIntelligentVehicle::onRecvUDP
- */
- void ADCIntelligentVehicle::onRecvUDP()
- {
- while (msockrecv.hasPendingDatagrams()) //hasPendingDatagrams()表示是否有待读取的传入数据报
- {
- qDebug("recv cmd");
- QByteArray ba;
- ba.resize(msockrecv.pendingDatagramSize());//pendingDatagramSize()返回待读取数据报的字节数
- msockrecv.readDatagram(ba.data(), ba.size()); //readDatagram()函数用于读取数据报内容
- int * p;
- iv::hmi::HMIBasic hmi;
- p = (int *)ba.data();
- // qDebug("cmd is ",*p);
- // std::cout<<"data len is "<<ba.size()<<" value is "<<*p<<std::endl;
- if(*p == 1)
- {
- std::cout<<ServiceCarStatus.mRunTime.elapsed()<<":"<<"recv start."<<std::endl;
- ServiceCarStatus.mbRunPause = false;
- hmi.mbBocheMode = false;
- hmi.mbPause = false;
- ShareHMIMsg(hmi);
- }
- if(*p == 0)
- {
- ServiceCarStatus.mbRunPause = true;
- hmi.mbBocheMode = false;
- hmi.mbPause = true;
- ShareHMIMsg(hmi);
- }
- if(*p == 2)
- {
- ServiceCarStatus.bocheMode = true;
- hmi.mbBocheMode = true;
- hmi.mbPause = false;
- ShareHMIMsg(hmi);
- }
- if(*p == 3)
- {
- hmi.mbbusmode = true;
- hmi.mbPause = ServiceCarStatus.mbRunPause;
- ShareHMIMsg(hmi);
- }
- if(*p == 4)
- {
- hmi.mbbusmode = false;
- hmi.mbPause = ServiceCarStatus.mbRunPause;
- ShareHMIMsg(hmi);
- }
- }
- }
- /**
- * @brief ADCIntelligentVehicle::onStateTimer
- */
- void ADCIntelligentVehicle::onStateTimer()
- {
- if(ServiceCarStatus.mbBrainCtring)
- mDataToUI.mInfo.is_run = 1;
- else
- mDataToUI.mInfo.is_run = 0;
- if(ServiceCarStatus.mLidarS<0)
- mDataToUI.mInfo.lidarStatus = 0;
- else
- mDataToUI.mInfo.lidarStatus = 1;
- if(ServiceCarStatus.mRadarS<0)
- mDataToUI.mInfo.radarStatus = 0;
- else
- mDataToUI.mInfo.radarStatus = 1;
- ServiceCarStatus.mLidarS--; //如果从10减到0,表示没有接收到数据。
- ServiceCarStatus.mRadarS--;
- mDataToUI.mInfo.RTKstatus = ServiceCarStatus.mRTKStatus;
- ServiceCarStatus.mRTKStatus = 0;
- mDataToUI.mInfo.radarobs = ServiceCarStatus.mRadarObs;
- mDataToUI.mInfo.lidarobs = ServiceCarStatus.mLidarObs;
- mDataToUI.mInfo.obs = ServiceCarStatus.mObs;
- mDataToUI.mInfo.gps_lat = ServiceCarStatus.location->gps_lat;
- mDataToUI.mInfo.gps_lng = ServiceCarStatus.location->gps_lng;
- mDataToUI.mInfo.accelerate = ServiceCarStatus.mfAcc;
- mDataToUI.mInfo.speed = ServiceCarStatus.speed;
- mDataToUI.mInfo.brake = ServiceCarStatus.mfBrake;
- mDataToUI.mInfo.swerve = ServiceCarStatus.mfWheel;
- mDataToUI.mInfo.boche_status = ServiceCarStatus.bocheEnable;
- if(fabs(mDataToUI.mInfo.gps_lat)<1 )
- {
- // qDebug("gps lat is error.");
- // std::cout<<"no gps."<<std::endl;
- }
- // std::cout<<"send data."<<std::endl;
- // qDebug("speed is %d",mDataToUI.mInfo.speed);
- // mDataToUI.mInfo.is_initSuccess = 1;
- // mDataToUI.mInfo.speed = 36.1;
- msocksend.writeDatagram((char *)&mDataToUI,sizeof(DataToUI),QHostAddress::Broadcast,9999); //QHostAddress::Broadcast 值为1 Pv4广播地址,相当于QHostAddress(“255.255.255.255”)。
- // _udp->writeDatagram(str.toUtf8(), QHostAddress::Broadcast, 10002);
- // multicast, 224.0.0.1~224.0.0.255 is multicast address of LAN
- // _udp->writeDatagram(str.toUtf8(), QHostAddress("224.0.0.131"), 10002);
- }
- /**
- * @brief ADCIntelligentVehicle::AutoStart
- */
- void ADCIntelligentVehicle::AutoStart()
- {
- // is_brain_running_flag_ = true;
- // mDataToUI.mInfo.is_initSuccess = 1;
- // std::cout<<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<<"Start Run SuccessFully."<<std::endl;
- }
- /**
- * @brief ADCIntelligentVehicle::on_pb_start_all_clicked
- */
- void ADCIntelligentVehicle::on_pb_start_all_clicked() {
- #ifdef USE_PAD_CTRL
- ServiceCarStatus.mbRunPause = false;
- iv::hmi::HMIBasic hmi;
- hmi.mbBocheMode = false;
- hmi.mbPause = false;
- ShareHMIMsg(hmi);
- return;
- #endif
- }
- /**
- * @brief ADCIntelligentVehicle::on_pb_open_can_card_clicked
- */
- //void ADCIntelligentVehicle::on_pb_open_can_card_clicked() {
- //}
- /**
- * @brief ADCIntelligentVehicle::on_timer_car_status_time_out
- */
- void ADCIntelligentVehicle::on_timer_car_status_time_out() {
- ui->lb_current_speed_big->setText(QString::number(ServiceCarStatus.speed));
- //ui->sb_accelerate_percent->setValue(ServiceControlStatus.accelerate_percent);
- //ui->sb_wheel_percent->setValue(ServiceControlStatus.wheel_angle);
- }
- /**
- * @brief ADCIntelligentVehicle::on_timer_mobileye_info_time_out
- */
- void ADCIntelligentVehicle::on_timer_mobileye_info_time_out(){
- mobileye_info->aws_display.dusk_time = ServiceCarStatus.aws_display.dusk_time;
- mobileye_info->Lane.curvature = ServiceCarStatus.Lane.curvature;
- mobileye_info->Lane.heading = ServiceCarStatus.Lane.heading;
- mobileye_info->Lane.pitch = ServiceCarStatus.Lane.pitch;
- mobileye_info->Lane.yaw = ServiceCarStatus.Lane.yaw;
- mobileye_info->obstacleStatus.num_obstacles = ServiceCarStatus.obstacleStatus.num_obstacles;
- mobileye_info->obstacleStatus.timestamp = ServiceCarStatus.obstacleStatus.timestamp;
- mobileye_info->aftermarketLane.dist_to_lane_l = ServiceCarStatus.aftermarketLane.dist_to_lane_l;
- mobileye_info->aftermarketLane.dist_to_lane_r = ServiceCarStatus.aftermarketLane.dist_to_lane_r;
- mobileye_info->aftermarketLane.lane_type_left = ServiceCarStatus.aftermarketLane.lane_type_left;
- mobileye_info->aftermarketLane.lane_type_right = ServiceCarStatus.aftermarketLane.lane_type_right;
- for(int i = 0; i < 15; i++)
- {
- mobileye_info->obstacleStatusA[i].obstacle_ID = ServiceCarStatus.obstacleStatusA[i].obstacle_ID;
- mobileye_info->obstacleStatusA[i].obstacle_pos_x = ServiceCarStatus.obstacleStatusA[i].obstacle_pos_x;
- mobileye_info->obstacleStatusA[i].obstacle_pos_y = ServiceCarStatus.obstacleStatusA[i].obstacle_pos_y;
- mobileye_info->obstacleStatusA[i].obstacle_rel_vel_x = ServiceCarStatus.obstacleStatusA[i].obstacle_rel_vel_x;
- mobileye_info->obstacleStatusA[i].obstacle_status = ServiceCarStatus.obstacleStatusA[i].obstacle_status;
- mobileye_info->obstacleStatusA[i].obstacle_type = ServiceCarStatus.obstacleStatusA[i].obstacle_type;
- mobileye_info->obstacleStatusA[i].is_obstacle_brake_lights = ServiceCarStatus.obstacleStatusA[i].is_obstacle_brake_lights;
- }
- for(int i = 0; i < 15; i++)
- {
- mobileye_info->obstacleStatusB[i].obstacle_length = ServiceCarStatus.obstacleStatusB[i].obstacle_length;
- mobileye_info->obstacleStatusB[i].obstacle_width = ServiceCarStatus.obstacleStatusB[i].obstacle_width;
- mobileye_info->obstacleStatusB[i].obstacle_age = ServiceCarStatus.obstacleStatusB[i].obstacle_age;
- }
- for(int i = 0; i < 15; i++)
- {
- mobileye_info->obstacleStatusC[i].obstacle_angle_rate = ServiceCarStatus.obstacleStatusC[i].obstacle_angle_rate;
- mobileye_info->obstacleStatusC[i].obstacle_angle = ServiceCarStatus.obstacleStatusC[i].obstacle_angle;
- mobileye_info->obstacleStatusC[i].object_accel_x = ServiceCarStatus.obstacleStatusC[i].object_accel_x;
- }
- }
- /**
- * @brief ADCIntelligentVehicle::keyPressEvent 加/减键进行缩放
- * @param event
- */
- void ADCIntelligentVehicle::keyPressEvent(QKeyEvent *event)
- {
- switch (event->key()) {
- case Qt::Key_Plus: // 放大
- myview->zoomIn();
- break;
- case Qt::Key_Minus: // 缩小
- myview->zoomOut();
- break;
- default:
- QMainWindow::keyPressEvent(event);
- }
- }
- /**
- * @brief ADCIntelligentVehicle::closeEvent
- * @param event
- */
- void ADCIntelligentVehicle::closeEvent(QCloseEvent *event)
- {
- QMessageBox::StandardButton button;
- button=QMessageBox::question(this,tr("退出程序"),QString(tr("确认退出程序")),QMessageBox::Yes|QMessageBox::No);
- if(button==QMessageBox::No)
- {
- event->ignore(); // 忽略退出信号,程序继续进行
- }
- else if(button==QMessageBox::Yes)
- {
- event->accept(); // 接受退出信号,程序退出
- }
- }
- /**
- * @brief MyView::wheelEvent 放大/缩小
- * @param event
- */
- void MyView::wheelEvent(QWheelEvent *event)
- {
- // 滚轮的滚动量
- QPoint scrollAmount = event->angleDelta();
- // 正值表示滚轮远离使用者(放大),负值表示朝向使用者(缩小)
- scrollAmount.y() > 0 ? zoomIn() : zoomOut();
- }
- /**
- * @brief MyView::zoomIn 放大
- */
- void MyView::zoomIn()
- {
- scale(1.1, 1.1);
- beishu *= 1.1;
- centerOn(450, 700 - (200 / beishu));
- }
- /**
- * @brief MyView::zoomOut 缩小
- */
- void MyView::zoomOut()
- {
- scale(1 / 1.1, 1 / 1.1);
- beishu /= 1.1;
- centerOn(450, 700 - (200 / beishu));
- }
- /**
- * @brief ADCIntelligentVehicle::paintEvent 刷新
- */
- void ADCIntelligentVehicle::paintEvent(QPaintEvent *)
- {
- if (is_show_enable)
- {
- // qDebug("enter paint. time1 is %d",mTime.elapsed());
- if(fabs(mTime.elapsed() - mnTimeLastUpdatePaint)<100)
- {
- return;
- }
- mnTimeLastUpdatePaint = mTime.elapsed();
- // qDebug("enter paint. time is %d",mTime.elapsed());
- painter->begin(image);
- painter_small->begin(image_small);
- // image->fill(QColor(60, 60, 60));//对画布进行填充
- image->fill(QColor(220, 220, 220));//对画布进行填充
- image_small->fill(QColor(220,220,220));
- std::vector<iv::GPSData> navigation_data;
- mMutexNavi.lock();
- navigation_data = m_navigation_data;
- mMutexNavi.unlock();
- std::vector<iv::Point2D> myplan,myplan_left,myplan_right;
- mMutexPlan.lock();
- myplan = m_plan;
- mMutexPlan.unlock();
- mMutexPlan_left.lock();
- myplan_left = m_plan_left;
- mMutexPlan_left.unlock();
- mMutexPlan_right.lock();
- myplan_right = m_plan_right;
- mMutexPlan_right.unlock();
- // std::cout<<"plan size is "<<myplan.size()<<std::endl;
- // std::vector<iv::GPSData> navigation_data = brain->navigation_data;
- painter->setRenderHint(QPainter::Antialiasing, true);//设置反锯齿模式,好看一点
- painter_small->setRenderHint(QPainter::Antialiasing, true);//设置反锯齿模式,好看一点
- int pointx = 450, pointy = 700;//确定坐标轴起点坐标,这里定义(35,280)
- int pointx_small = 450, pointy_small = 700;
- // double x0[22000], y0[22000], lng[22000], x1[22000], y1[22000], x2[22000], y2[22000];
- double * x0, * y0, * lng, * x1, * y1, * x2, * y2;
- std::shared_ptr<double> ptrx0,ptry0,ptrlng,ptrx1,ptry1,ptrx2,ptry2;
- int nmapsize = navigation_data.size();
- x0 = new double[nmapsize];
- y0 = new double[nmapsize];
- lng = new double[nmapsize];
- x1 = new double[nmapsize];
- y1 = new double[nmapsize];
- x2 = new double[nmapsize];
- y2 = new double[nmapsize];
- ptrx0.reset(x0);
- ptry0.reset(y0);
- ptrlng.reset(lng);
- ptrx1.reset(x1);
- ptry1.reset(y1);
- ptrx2.reset(x2);
- ptry2.reset(y2);
- double xx = 0, yy = 0;
- double sumx = 0, sumy = 0;//, ave_x = 0, ave_y = 0;
- int sizeN = navigation_data.size();
- //int max_x_pos = 0, min_x_pos = 0, max_y_pos = 0, min_y_pos = 0;
- int x_max = 0, y_max = 0;//数组里的最大值
- int x_min = 0x3f3f3f3f, y_min = 0x3f3f3f3f;//inf为 #define inf 0x3f3f3f3f
- double k1, k2;
- QPen pen, penPoint;
- //先绘制车位置点及框图
- static const QPointF points1[2] = { QPointF(300, 700), QPointF(600, 700) };
- static const QPointF points2[2] = { QPointF(450, 0), QPointF(450, 900) };
- static const QPointF points1_small[2] = { QPointF(300, 700), QPointF(600, 700) };
- static const QPointF points2_small[2] = { QPointF(450, 0), QPointF(450, 900) };
- penPoint.setColor(Qt::red);
- penPoint.setWidth(2);
- painter->setPen(penPoint);
- painter_small->setPen(penPoint);
- painter->drawPoint(pointx, pointy);
- painter_small->drawPoint(pointx_small,pointy_small);
- painter->drawPolyline(points1, 2);
- painter->drawPolyline(points2, 2);
- painter_small->drawPolyline(points1_small, 2);
- painter_small->drawPolyline(points2_small, 2);
- //路径点的预处理
- for (int i = 0; i < sizeN; i++)
- {
- x0[i] = navigation_data[i]->gps_x;
- y0[i] = navigation_data[i]->gps_y;
- lng[i] = navigation_data[i]->ins_heading_angle;
- sumx = sumx + navigation_data[i]->gps_x;
- sumy = sumy + navigation_data[i]->gps_y;
- if (x0[i] > x_max) {
- x_max = x0[i];
- //max_x_pos = i;
- }
- if (x0[i] < x_min) {
- x_min = x0[i];
- //min_x_pos = i;
- }
- if (y0[i] > y_max) {
- y_max = y0[i];
- //max_y_pos = i;
- }
- if (y0[i] < y_min) {
- y_min = y0[i];
- //min_y_pos = i;
- }
- }
- //ave_x = sumx / sizeN;
- //ave_y = sumy / sizeN;
- //获取到实时 GPS信息,并做路径点的显示更新
- if (ServiceCarStatus.location->gps_x == 0)
- {
- painter->drawText(rect(), Qt::AlignLeft, QString::fromLocal8Bit("等待车辆实时GPS位置信息"));
- painter_small->drawText(rect(), Qt::AlignLeft, QString::fromLocal8Bit("等待车辆实时GPS位置信息"));
- }
- else
- {
- x0[0] = ServiceCarStatus.location->gps_x;
- y0[0] = ServiceCarStatus.location->gps_y;
- // std::cout<<"x = "<<x0[0]<<" y= "<<y0[0]<<std::endl;
- lng[0] = ServiceCarStatus.location->ins_heading_angle;
- }
- //根据标定原点的选取,对路径点进行转化
- for (int i = 1; i < sizeN; i++)
- {
- // if(i == 1)std::cout<<" x1 = "<<x0[i]<<std::endl;
- x0[i] = x0[i] - x0[0];
- y0[i] = y0[i] - y0[0];
- xx = x0[i];
- yy = y0[i];
- x0[i] = xx * cos(lng[0] * PI / 180) - yy * sin(lng[0] * PI / 180);
- y0[i] = xx * sin(lng[0] * PI / 180) + yy * cos(lng[0] * PI / 180);
- k1 = sin((90 + (lng[i] - lng[0])) * PI / 180);
- k2 = cos((90 + (lng[i] - lng[0])) * PI / 180);
- #if 0
- x1[i] = x0[i] + k1 * 5;
- y1[i] = y0[i] + k2 * 5;
- x2[i] = x0[i] - k1 * 5;
- y2[i] = y0[i] - k2 * 5;
- #else
- x1[i] = x0[i] + k1 * 1.75;
- y1[i] = y0[i] + k2 * 1.75;
- x2[i] = x0[i] - k1 * 1.75;
- y2[i] = y0[i] - k2 * 1.75;
- #endif
- }
- double kx_small = (double)(600) / (abs(y_max - y_min));//x轴的系数
- double ky_small = (double)(600) / (abs(y_max - y_min));//y方向的比例系数
- double kx = 10;
- double ky = 10;
- //距离车正前方10m处画一条线
- static const QPointF points3[2] = { QPointF(0, 700 - 10 * ky), QPointF(900, 700 - 10 * ky) };
- painter->drawPolyline(points3, 2);
- //绘制路径点
- penPoint.setColor(Qt::black);
- penPoint.setWidth(1);
- for (int i = 1; i < sizeN - 1; i++)
- {
- painter->setPen(penPoint);//蓝色的笔,用于标记各个点
- painter_small->setPen(penPoint);//蓝色的笔,用于标记各个点
- painter->drawPoint(pointx + x0[i] * kx, pointy - y0[i] * ky);
- painter_small->drawPoint(pointx_small + x0[i] * kx_small, pointy_small - y0[i] * ky_small);
- painter->drawPoint(pointx + x1[i] * kx, pointy - y1[i] * ky);
- painter_small->drawPoint(pointx_small + x1[i] * kx_small, pointy_small - y1[i] * ky_small);
- painter->drawPoint(pointx + x2[i] * kx, pointy - y2[i] * ky);
- painter_small->drawPoint(pointx_small + x2[i] * kx_small, pointy_small - y2[i] * ky_small);
- }
- painter->drawPoint(pointx + x0[sizeN - 1] * kx, pointy - y0[sizeN - 1] * ky);//绘制最后一个点
- painter_small->drawPoint(pointx_small + x0[sizeN - 1] * kx_small, pointy_small - y0[sizeN - 1] * ky_small);//绘制最后一个点
- penPoint.setColor(Qt::red);
- penPoint.setWidth(2);
- painter->drawPoint(pointx + x0[0] * kx, pointy - y0[0] * ky);
- painter_small->drawPoint(pointx_small + x0[0] * kx_small, pointy_small - y0[0] * ky_small);
- // draw plan trace
- penPoint.setColor(Qt::green);
- penPoint.setWidth(2);
- painter->setPen(penPoint);
- QPointF tracePoints[myplan.size()];
- if(myplan.size()>2)
- {
- for(int i=0;i<(myplan.size()-1);i++)
- {
- tracePoints[i].setX((float)(450+myplan[i].x*kx));
- tracePoints[i].setY((float)(700-myplan[i].y*ky));
- painter->drawLine((float)(450+myplan[i].x*kx),(float)(700-myplan[i].y*ky),
- (float)(450+myplan[i+1].x*kx),(float)(700-myplan[i+1].y*ky));
- }
- }
- penPoint.setColor(Qt::blue);
- penPoint.setWidth(2);
- painter->setPen(penPoint);
- QPointF tracePoints_left[myplan_left.size()];
- if(myplan_left.size()>2)
- {
- for(int i=0;i<(myplan_left.size()-1);i++)
- {
- tracePoints_left[i].setX((float)(450+myplan_left[i].x*kx));
- tracePoints_left[i].setY((float)(700-myplan_left[i].y*ky));
- painter->drawLine((float)(450+myplan_left[i].x*kx),(float)(700-myplan_left[i].y*ky),
- (float)(450+myplan_left[i+1].x*kx),(float)(700-myplan_left[i+1].y*ky));
- }
- }
- penPoint.setColor(Qt::blue);
- penPoint.setWidth(2);
- painter->setPen(penPoint);
- QPointF tracePoints_right[myplan_right.size()];
- if(myplan_right.size()>2)
- {
- for(int i=0;i<(myplan_right.size()-1);i++)
- {
- tracePoints_right[i].setX((float)(450+myplan_right[i].x*kx));
- tracePoints_right[i].setY((float)(700-myplan_right[i].y*ky));
- painter->drawLine((float)(450+myplan_right[i].x*kx),(float)(700-myplan_right[i].y*ky),
- (float)(450+myplan_right[i+1].x*kx),(float)(700-myplan_right[i+1].y*ky));
- }
- }
- // painter->drawPolyline(tracePoints,myplan.size());
- // draw plan trace end
- /////////////////////////////////////apollo add car icon 20200409
- QPixmap pix;
- //pix.load("car.png");
- pix.load(":/ADCIntelligentVehicle/car1.png");
- painter->drawPixmap(435,667,30,67,pix);
- painter_small->drawPixmap(442,683,16,34,pix);
- ///////////////////////////////////////////////////////////////////
- penPoint.setColor(Qt::blue);
- penPoint.setWidth(3);
- painter->setPen(penPoint);
- QFont font;
- font.setFamily("Microsoft YaHei");
- font.setPointSize(50);
- font.setItalic(true);
- painter->setFont(font);
- //////////////////////////////////////
- // //
- // 显示毫米波雷达和激光雷达的信息 //
- // //
- //////////////////////////////////////
- painter->setPen(QPen(Qt::black, 2));
- painter->setBrush(Qt::blue);
- QFont esr_font("Microsoft YaHei", 10, 75); //第一个属性是字体(微软雅黑),第二个是大小,第三个是加粗(权重是75)
- painter->setFont(esr_font);
- char coordinate_ear[100];
- iv::radar::radarobjectarray xradararray;
- if(mbradarUpdate)
- {
- mMutexRadar.lock();
- xradararray.CopyFrom(mradararray);
- mMutexRadar.unlock();
- for (int a = 0; a < xradararray.obj_size(); a++)
- {
- if (xradararray.obj(a).bvalid())
- {
- painter->drawEllipse(xradararray.obj(a).x() * 10 + 450, -(xradararray.obj(a).y() * 10) + 700, 10, 10);
- sprintf(coordinate_ear, "(%d, %d)", (int)xradararray.obj(a).x(), (int)xradararray.obj(a).y());
- painter->drawText(xradararray.obj(a).x() * 10 + 450, -(xradararray.obj(a).y() * 10) + 700, QString(coordinate_ear));
- }
- }
- // mbradarUpdate = false;
- }
- ServiceLidar.copylidarto(Lidar_read); //激光雷达障碍物
- ServiceLidar.copylidarobsto(Lidar_obsread);
- painter->setPen(QColor(255, 0, 0));
- for (unsigned int x = 0; x < Lidar_read->size(); x++)
- {
- //painter->drawPoint(((*Lidar_read)[x].nomal_x) * 10 + 450, -(*Lidar_read)[x].nomal_y * 10 + 700);
- }
- painter->setPen(QColor(255,0,0));
- for (unsigned int x = 0; x < Lidar_obsread->size(); x++)
- {
- painter->drawPoint(((*Lidar_obsread)[x].nomal_x) * 10 + 450, -(*Lidar_obsread)[x].nomal_y * 10 + 700);
- }
- painter->end();
- painter_small->end();
- scene->clear();
- scene_small->clear();
- scene->addPixmap(QPixmap::fromImage(*image));
- scene_small->addPixmap(QPixmap::fromImage(*image_small));
- myview->setScene(scene);
- myview_small->setScene(scene_small);
- myview->show();
- myview_small->show();
- navigation_data.clear();
- if(mfSOC != 0.0)
- {
- ui->label_11->setText(QString::number(mfSOC,'f',1));
- }
- }
- }
- /**
- * @brief ADCIntelligentVehicle::on_pb_auto_drive_mode_clicked
- */
- void ADCIntelligentVehicle::on_pb_auto_drive_mode_clicked() {
- is_auto_drive_mode_enable_ = !is_auto_drive_mode_enable_;
- // ServiceCanUtil.startsend(is_auto_drive_mode_enable_);
- ui->pb_auto_drive_mode->setText(is_auto_drive_mode_enable_ ? QStringLiteral("决策控制使能:开") : QStringLiteral("决策控制使能:关"));
- }
- /**
- * @brief ADCIntelligentVehicle::on_pb_auto_braking_mode_clicked
- */
- void ADCIntelligentVehicle::on_pb_auto_braking_mode_clicked() {
- is_auto_braking_mode_enable_ = !is_auto_braking_mode_enable_;
- // ServiceControlStatus.set_turnsignals_control(is_auto_braking_mode_enable_,0);
- ui->pb_auto_braking_mode->setText(is_auto_braking_mode_enable_ ? QStringLiteral("左转向灯:开") : QStringLiteral("左转向灯:关"));
- }
- /**
- * @brief ADCIntelligentVehicle::on_pb_auto_accelerate_mode_clicked
- * QStringLiteral 是Qt5中新引入的一个用来从“字符串常量”创建QString对象的宏
- */
- void ADCIntelligentVehicle::on_pb_auto_accelerate_mode_clicked() {
- is_auto_accelerate_mode_enable_ = !is_auto_accelerate_mode_enable_;
- // ServiceControlStatus.set_turnsignals_control(0,is_auto_accelerate_mode_enable_);
- ui->pb_auto_accelerate_mode->setText(is_auto_accelerate_mode_enable_ ? QStringLiteral("右转向灯:开") : QStringLiteral("右转向灯:关"));
- }
- /**
- * @brief ADCIntelligentVehicle::on_pb_auto_wheel_mode_clicked
- */
- void ADCIntelligentVehicle::on_pb_auto_wheel_mode_clicked() {
- is_auto_wheel_mode_enable_ = !is_auto_wheel_mode_enable_;
- // ServiceControlStatus.set_speaker(is_auto_wheel_mode_enable_);
- ui->pb_auto_wheel_mode->setText(is_auto_wheel_mode_enable_ ? QStringLiteral("喇叭") : QStringLiteral("喇叭"));
- }
- /**
- * @brief ADCIntelligentVehicle::on_pushButton_clicked
- */
- void ADCIntelligentVehicle::on_pushButton_clicked(){
- is_3 = !is_3;
- // ServiceControlStatus.set_light(is_3);
- ui->pushButton->setText(is_3 ? QStringLiteral("近光灯:开") : QStringLiteral("近光灯:关"));
- }
- /**
- * @brief ADCIntelligentVehicle::on_pb_speed_control_mode_clicked
- */
- void ADCIntelligentVehicle::on_pb_speed_control_mode_clicked() {
- is_speed_control_mode_enable_ = !is_speed_control_mode_enable_;
- // if(is_speed_control_mode_enable_==true)
- // ServiceControlStatus.set_engine(0x03);
- // else
- // ServiceControlStatus.set_engine(0x02);
- ui->pb_speed_control_mode->setText(is_speed_control_mode_enable_ ? QStringLiteral("点火:开") : QStringLiteral("点火:关"));
- }
- /**
- * @brief ADCIntelligentVehicle::on_hs_braking_percent_valueChanged
- * @param value
- */
- void ADCIntelligentVehicle::on_hs_braking_percent_valueChanged(int value) {
- //ServiceControlStatus.set_braking((float)value);
- }
- /**
- * @brief ADCIntelligentVehicle::on_hs_accelerate_percent_valueChanged
- * @param value
- */
- void ADCIntelligentVehicle::on_hs_accelerate_percent_valueChanged(int value) {
- //ServiceControlStatus.set_accelerate((float)value);
- }
- /**
- * @brief ADCIntelligentVehicle::on_hs_wheel_angle_valueChanged
- * @param value
- */
- void ADCIntelligentVehicle::on_hs_wheel_angle_valueChanged(int value) {
- //ServiceControlStatus.set_wheel_angle((float)value);
- }
- /**
- * @brief ADCIntelligentVehicle::on_pushButton_3_clicked
- */
- void ADCIntelligentVehicle::on_pushButton_3_clicked(){
- look_decition->show();
- }
- /**
- * @brief ADCIntelligentVehicle::on_pushButton_4_clicked
- */
- void ADCIntelligentVehicle::on_pushButton_4_clicked(){
- mobileye_info->show();
- }
- /**
- * @brief ADCIntelligentVehicle::on_listWidget_clicked
- */
- void ADCIntelligentVehicle::on_listWidget_clicked()
- {
- int currentRow=ui->listWidget->currentRow(); //currentRow()获取当前行号
- if(currentRow==0) //点击第一切换到第一页
- {
- ui->stackedWidget->setCurrentIndex(0);//为堆栈窗体切换到当前索引
- }
- else if(currentRow==1)
- {
- ui->stackedWidget->setCurrentIndex(1);
- }
- else if(currentRow==2)
- {
- ui->stackedWidget->setCurrentIndex(2);
- }
- else
- {
- ui->stackedWidget->setCurrentIndex(0);
- }
- }
- /**
- * @brief ADCIntelligentVehicle::on_pushButton_5_clicked
- */
- void ADCIntelligentVehicle::on_pushButton_5_clicked()//常规速度
- {
- QString normal_speed_ = ui->textEdit->toPlainText();
- // if(normal_speed_!=NULL)
- // ServiceControlStatus.normal_speed = normal_speed_.toInt();
- }
- /**
- * @brief ADCIntelligentVehicle::on_pushButton_6_clicked
- */
- void ADCIntelligentVehicle::on_pushButton_6_clicked()//转弯速度
- {
- QString swerve_speed_ = ui->textEdit_2->toPlainText();
- // if(swerve_speed_!=NULL)
- // ServiceControlStatus.swerve_speed = swerve_speed_.toInt();
- }
- /**
- * @brief ADCIntelligentVehicle::on_pushButton_7_clicked
- */
- void ADCIntelligentVehicle::on_pushButton_7_clicked()//快速
- {
- QString high_speed_ = ui->textEdit_3->toPlainText();
- // if(high_speed_!=NULL)
- // ServiceControlStatus.high_speed = high_speed_.toInt();
- }
- /**
- * @brief ADCIntelligentVehicle::on_pushButton_8_clicked
- */
- void ADCIntelligentVehicle::on_pushButton_8_clicked()//中速
- {
- QString mid_speed_ = ui->textEdit_4->toPlainText();
- // if(mid_speed_!=NULL)
- // ServiceControlStatus.mid_speed = mid_speed_.toInt();
- }
- /**
- * @brief ADCIntelligentVehicle::on_pushButton_9_clicked
- */
- void ADCIntelligentVehicle::on_pushButton_9_clicked()//慢速
- {
- QString low_speed_ = ui->textEdit_5->toPlainText();
- // if(low_speed_!=NULL)
- // ServiceControlStatus.low_speed = low_speed_.toInt();
- }
- /**
- * @brief ADCIntelligentVehicle::UpdatePlanTrace
- * @param strdata
- * @param nSize
- * @param index
- * @param dt
- * @param strmemname
- */
- void ADCIntelligentVehicle::UpdatePlanTrace(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
- {
- mMutexPlan.lock();
- m_plan.clear();
- int nplansize = nSize/sizeof(iv::Point2D);
- int npsize = sizeof(iv::Point2D);
- int i;
- for(i=0;i<nplansize;i++)
- {
- iv::Point2D x;
- memcpy(&x,strdata + i*npsize,npsize);
- m_plan.push_back(x);
- }
- mMutexPlan.unlock();
- }
- void ADCIntelligentVehicle::UpdatePlanTrace_left(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
- {
- mMutexPlan_left.lock();
- m_plan_left.clear();
- int nplansize = nSize/sizeof(iv::Point2D);
- int npsize = sizeof(iv::Point2D);
- int i;
- for(i=0;i<nplansize;i++)
- {
- iv::Point2D x;
- memcpy(&x,strdata + i*npsize,npsize);
- m_plan_left.push_back(x);
- }
- mMutexPlan_left.unlock();
- }
- void ADCIntelligentVehicle::UpdatePlanTrace_right(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
- {
- mMutexPlan_right.lock();
- m_plan_right.clear();
- int nplansize = nSize/sizeof(iv::Point2D);
- int npsize = sizeof(iv::Point2D);
- int i;
- for(i=0;i<nplansize;i++)
- {
- iv::Point2D x;
- memcpy(&x,strdata + i*npsize,npsize);
- m_plan_right.push_back(x);
- }
- mMutexPlan_right.unlock();
- }
- /**
- * @brief ADCIntelligentVehicle::UpdateMap
- * @param strdata
- * @param nSize
- * @param index
- * @param dt
- * @param strmemname
- */
- void ADCIntelligentVehicle::UpdateMap(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
- {
- // std::cout<<"update map "<<std::endl;
- int gpsunitsize = sizeof(iv::GPS_INS);
- int nMapSize = nSize/gpsunitsize;
- // std::cout<<"map size is "<<nMapSize<<std::endl;
- if(nMapSize < 1)return;
- bool bUpdate = false;
- if(nMapSize != m_navigation_data.size())
- {
- bUpdate = true;
- }
- else
- {
- iv::GPS_INS * p = (iv::GPS_INS *)strdata;
- if((p->gps_lat == m_navigation_data.at(0)->gps_lat)&&(p->ins_heading_angle == m_navigation_data.at(0)->ins_heading_angle))
- {
- // qDebug("same map");
- bUpdate = false;
- }
- else
- {
- bUpdate = true;
- }
- }
- if(bUpdate)
- {
- int i;
- mMutexNavi.lock();
- m_navigation_data.clear();
- for(i=0;i<nMapSize;i++)
- {
- iv::GPS_INS x;
- memcpy(&x,strdata + i*gpsunitsize,gpsunitsize);
- iv::GPSData data(new iv::GPS_INS);
- *data = x;
- m_navigation_data.push_back(data);
- }
- mMutexNavi.unlock();
- if(m_navigation_data.size()>0)
- {
- mDataToUI.mInfo.is_mapLoad = 1;
- }
- else
- {
- mDataToUI.mInfo.is_mapLoad = 0;
- }
- }
- else
- {
- // qDebug("not need update");
- }
- }
- //=======================zhaobo0904
- #define PI 3.14159265358979
- /**
- * @brief GaussProjCal
- * @param lon
- * @param lat
- * @param X
- * @param Y
- */
- /*
- void GaussProjCal(double lon, double lat, double *X, double *Y)
- {
- // 1975 年国际椭球体长半轴 a, 第一离心率 e2, 第二离心率 ep2
- double a = 6378140.0;
- double e2 = 0.006694384999588;
- double ep2 = e2/(1-e2);
- // 原点所在经度
- double lon_origin = 6.0*int(lon/6) + 3.0;
- lon_origin *= PI / 180.0;
- double k0 = 0.9996;
- // 角度转弧度
- double lat1 = lat * PI / 180.0;
- double lon1 = lon * PI / 180.0;
- // 经线在该点处的曲率半径,
- double N = a / sqrt(1 - e2*sin(lat1)*sin(lat1));
- // 赤道到该点的经线长度近似值 M, 使用泰勒展开逐项积分然后取前四项.
- // 这个近似值是将 N 作为纬度 \phi 的函数展开为泰勒计数, 然后在区间 [0, lat1] 上积分得到的.
- // 首先计算前四项的系数 a1~a4.
- double a1 = 1 - e2/4 - (3*e2*e2)/64 - (5*e2*e2*e2)/256;
- double a2 = (3*e2)/8 + (3*e2*e2)/32 + (45*e2*e2*e2)/1024;
- double a3 = (15*e2*e2)/256 + (45*e2*e2*e2)/1024;
- double a4 = (35*e2*e2*e2)/3072;
- double M = a * (a1*lat1 - a2*sin(2*lat1) + a3*sin(4*lat1) - a4*sin(6*lat1));
- // 辅助量
- double T = tan(lat1)*tan(lat1);
- double C = ep2*cos(lat1)*cos(lat1);
- double A = (lon1 - lon_origin)*cos(lat1);
- *X = 500000.0 + k0 * N * (A + (1 - T + C)*(A*A*A)/6 + (5 - 18*T + T*T + 72*C - 58*ep2)*(A*A*A*A*A)/120);
- *Y = M + N * tan(lat1) * ((A*A)/2 +
- (5 - T + 9*C + 4*C*C)*(A*A*A*A)/24 +
- (61 - 58*T + T*T + 600*C - 330*ep2)*(A*A*A*A*A*A)/720);
- *Y *= k0;
- return;
- }
- */
- //高斯投影由经纬度(Unit:DD)反算大地坐标(含带号,Unit:Metres)
- void GaussProjCal(double longitude, double latitude, double *X, double *Y)
- {
- int ProjNo = 0; int ZoneWide; ////带宽
- double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
- double a, f, e2, ee, NN, T, C, A, M, iPI;
- iPI = 0.0174532925199433; ////3.1415926535898/180.0;
- ZoneWide = 6; ////6度带宽
- a = 6378245.0; f = 1.0 / 298.3; //54年北京坐标系参数
- ////a=6378140.0; f=1/298.257; //80年西安坐标系参数
- ProjNo = (int)(longitude / ZoneWide);
- longitude0 = ProjNo * ZoneWide + ZoneWide / 2;
- longitude0 = longitude0 * iPI;
- latitude0 = 0;
- longitude1 = longitude * iPI; //经度转换为弧度
- latitude1 = latitude * iPI; //纬度转换为弧度
- e2 = 2 * f - f * f;
- ee = e2 * (1.0 - e2);
- NN = a / sqrt(1.0 - e2 * sin(latitude1)*sin(latitude1));
- T = tan(latitude1)*tan(latitude1);
- C = ee * cos(latitude1)*cos(latitude1);
- A = (longitude1 - longitude0)*cos(latitude1);
- M = a * ((1 - e2 / 4 - 3 * e2*e2 / 64 - 5 * e2*e2*e2 / 256)*latitude1 - (3 * e2 / 8 + 3 * e2*e2 / 32 + 45 * e2*e2
- *e2 / 1024)*sin(2 * latitude1)
- + (15 * e2*e2 / 256 + 45 * e2*e2*e2 / 1024)*sin(4 * latitude1) - (35 * e2*e2*e2 / 3072)*sin(6 * latitude1));
- xval = NN * (A + (1 - T + C)*A*A*A / 6 + (5 - 18 * T + T * T + 72 * C - 58 * ee)*A*A*A*A*A / 120);
- yval = M + NN * tan(latitude1)*(A*A / 2 + (5 - T + 9 * C + 4 * C*C)*A*A*A*A / 24
- + (61 - 58 * T + T * T + 600 * C - 330 * ee)*A*A*A*A*A*A / 720);
- X0 = 1000000L * (ProjNo + 1) + 500000L;
- Y0 = 0;
- xval = xval + X0; yval = yval + Y0;
- *X = xval;
- *Y = yval;
- }
- //高斯投影由大地坐标(Unit:Metres)反算经纬度(Unit:DD)
- void GaussProjInvCal(double X, double Y, double *longitude, double *latitude)
- {
- int ProjNo; int ZoneWide; ////带宽
- double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
- double e1, e2, f, a, ee, NN, T, C, M, D, R, u, fai, iPI;
- iPI = 0.0174532925199433; ////3.1415926535898/180.0;
- a = 6378245.0; f = 1.0 / 298.3; //54年北京坐标系参数
- ////a=6378140.0; f=1/298.257; //80年西安坐标系参数
- ZoneWide = 6; ////6度带宽
- ProjNo = (int)(X / 1000000L); //查找带号
- longitude0 = (ProjNo - 1) * ZoneWide + ZoneWide / 2;
- longitude0 = longitude0 * iPI; //中央经线
- X0 = ProjNo * 1000000L + 500000L;
- Y0 = 0;
- xval = X - X0; yval = Y - Y0; //带内大地坐标
- e2 = 2 * f - f * f;
- e1 = (1.0 - sqrt(1 - e2)) / (1.0 + sqrt(1 - e2));
- ee = e2 / (1 - e2);
- M = yval;
- u = M / (a*(1 - e2 / 4 - 3 * e2*e2 / 64 - 5 * e2*e2*e2 / 256));
- fai = u + (3 * e1 / 2 - 27 * e1*e1*e1 / 32)*sin(2 * u) + (21 * e1*e1 / 16 - 55 * e1*e1*e1*e1 / 32)*sin(
- 4 * u)
- + (151 * e1*e1*e1 / 96)*sin(6 * u) + (1097 * e1*e1*e1*e1 / 512)*sin(8 * u);
- C = ee * cos(fai)*cos(fai);
- T = tan(fai)*tan(fai);
- NN = a / sqrt(1.0 - e2 * sin(fai)*sin(fai));
- R = a * (1 - e2) / sqrt((1 - e2 * sin(fai)*sin(fai))*(1 - e2 * sin(fai)*sin(fai))*(1 - e2 * sin
- (fai)*sin(fai)));
- D = xval / NN;
- //计算经度(Longitude) 纬度(Latitude)
- longitude1 = longitude0 + (D - (1 + 2 * T + C)*D*D*D / 6 + (5 - 2 * C + 28 * T - 3 * C*C + 8 * ee + 24 * T*T)*D
- *D*D*D*D / 120) / cos(fai);
- latitude1 = fai - (NN*tan(fai) / R)*(D*D / 2 - (5 + 3 * T + 10 * C - 4 * C*C - 9 * ee)*D*D*D*D / 24
- + (61 + 90 * T + 298 * C + 45 * T*T - 256 * ee - 3 * C*C)*D*D*D*D*D*D / 720);
- //转换为度 DD
- *longitude = longitude1 / iPI;
- *latitude = latitude1 / iPI;
- }
- //==========================================================
- /**
- * @brief ADCIntelligentVehicle::UpdateGPSIMU
- * @param strdata
- * @param nSize
- * @param index
- * @param dt
- * @param strmemname
- */
- void ADCIntelligentVehicle::UpdateGPSIMU(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
- {
- iv::gps::gpsimu xgpsimu;
- if(!xgpsimu.ParseFromArray(strdata,nSize))
- {
- gIvlog->warn("ADCIntelligentVehicle::UpdateGPSIMU parse error. nSize is %d",nSize);
- return;
- }
- iv::GPSData data(new iv::GPS_INS);
- data->gps_lat = xgpsimu.lat();
- data->gps_lng = xgpsimu.lon();
- data->ins_heading_angle = xgpsimu.heading();
- data->rtk_status = xgpsimu.rtk_state();
- data->ins_status = xgpsimu.ins_state();
- data->vel_D = xgpsimu.vd(); //地向速度,单位(米/秒)
- data->vel_E = xgpsimu.ve(); //东向速度,单位(米/秒)
- data->vel_N = xgpsimu.vn(); //北向速度,单位(米/秒)
- GaussProjCal(data->gps_lng,data->gps_lat,&data->gps_x,&data->gps_y);
- ServiceCarStatus.mRTKStatus = data->rtk_status;
- ServiceCarStatus.speed = data->speed;
- ServiceCarStatus.location->gps_lat = data->gps_lat;
- ServiceCarStatus.location->gps_lng = data->gps_lng;
- ServiceCarStatus.location->ins_heading_angle = data->ins_heading_angle;
- ServiceCarStatus.location->gps_x = data->gps_x;
- ServiceCarStatus.location->gps_y = data->gps_y;
- ServiceCarStatus.location->rtk_status = data->rtk_status;
- ServiceCarStatus.location->ins_status = data->ins_status;
- ServiceCarStatus.speed = sqrt(pow(data->vel_E,2)+pow(data->vel_N,2)) * 3.6; //double pow(double x, double y) 返回 x 的 y 次幂,即 xy。
- mnTimeUpdateGPS = mTimeState.elapsed();
- }
- /**
- * @brief ADCIntelligentVehicle::UpdateRADAR
- * @param strdata
- * @param nSize
- * @param index
- * @param dt
- * @param strmemname
- */
- void ADCIntelligentVehicle::UpdateRADAR(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
- {
- static qint64 oldrecvtime;
- iv::radar::radarobjectarray xradararray;
- if(!xradararray.ParseFromArray(strdata,nSize))
- {
- gIvlog->warn("ADCIntelligentVehicle::UpdateRADAR Parse Error.");
- return;
- }
- // gIvlog->verbose("radar time is %ld",QDateTime::currentMSecsSinceEpoch());
- if((QDateTime::currentMSecsSinceEpoch() - oldrecvtime)>100)
- {
- gIvlog->warn("radar interval is more than 100ms. value is %ld",QDateTime::currentMSecsSinceEpoch() - oldrecvtime);
- }
- oldrecvtime = QDateTime::currentMSecsSinceEpoch();
- mMutexRadar.lock();
- mradararray.CopyFrom(xradararray);
- mbradarUpdate = true;
- mMutexRadar.unlock();
- // int i;
- // for(i=0;i<64;i++)
- // {
- // iv::ObstacleBasic x;
- // memcpy(&x,pdata + i*sizeof(iv::ObstacleBasic),sizeof(iv::ObstacleBasic));
- // ServiceCarStatus.obs_radar[i] = x;
- // }
- ServiceCarStatus.mRadarS = 10;
- // mnTimeUpdateRadar = mTimeState.elapsed();
- // mnTimeUpdateCANState = mTimeState.elapsed();
- }
- /**
- * @brief ADCIntelligentVehicle::UpdateCANState
- * @param pdata
- * @param ndatasize
- */
- void ADCIntelligentVehicle::UpdateCANState(const char *pdata, const int ndatasize)
- {
- iv::canstate::canstate recCanstate;
- if(!recCanstate.ParseFromArray(pdata, ndatasize))
- {
- gIvlog->warn("ADCIntelligentVehicle::updateCanstate error");
- return;
- }
- // if(ndatasize<sizeof(iv::can_state))return;
- // char ccanstate[ndatasize];
- // memcpy(ccanstate,pdata,4);
- #if 1
- if(recCanstate.b_canstate())
- {
- // gIvlog->debug("UpdateCANState<success>");
- mDataToUI.mInfo.is_initSuccess = 1;
- }
- else
- {
- // gIvlog->debug("UpdateCANState<failed>size %d %s ");
- mDataToUI.mInfo.is_initSuccess = 0;
- }
- #endif
- // iv::can_state x;
- // memcpy(&x,pdata,sizeof(iv::can_state));
- // if(x.mbCANOpen)
- // {
- // mDataToUI.mInfo.is_initSuccess = 1;
- // }
- // else
- // {
- // mDataToUI.mInfo.is_initSuccess = 0;
- // }
- }
- /**
- * @brief ADCIntelligentVehicle::UpdateDecition
- * @param strdata
- * @param nSize
- * @param index
- * @param dt
- * @param strmemname
- */
- void ADCIntelligentVehicle::UpdateDecition(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
- {
- iv::brain::decition xdecition;
- if(!xdecition.ParseFromArray(strdata,nSize))
- {
- gIvlog->warn("ADCIntelligentVehicle::UpdateDecition parse errror. nSize is %d",nSize);
- return;
- }
- //ServiceCarStatus.mfAcc = xdecition.accelerator(); //
- ServiceCarStatus.mfAcc = xdecition.torque(); //
- ServiceCarStatus.mfBrake = xdecition.brake();
- ServiceCarStatus.mfWheel = xdecition.wheelangle();
- ServiceCarStatus.mfttc = xdecition.ttc();
- mnTimeUpdateDecition = mTimeState.elapsed();
- }
- /**
- * @brief ADCIntelligentVehicle::UpdateVehicleState
- * @param strdata
- * @param nSize
- * @param index
- * @param dt
- * @param strmemname
- */
- void ADCIntelligentVehicle::UpdateVehicleState(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
- {
- iv::brain::brainstate xbrainstate;
- if(!xbrainstate.ParseFromArray(strdata,nSize))
- {
- gIvlog->warn("ADCIntelligentVehicle::UpdateVehicleState parse error,nSize is %d ",nSize);
- return;
- }
- ServiceCarStatus.bocheEnable = xbrainstate.mbbocheenable();
- ServiceCarStatus.mObs = xbrainstate.mfobs();
- ServiceCarStatus.mLidarObs = xbrainstate.mflidarobs();
- ServiceCarStatus.mRadarObs = xbrainstate.mfradarobs();
- mbBrainRunning = xbrainstate.mbbrainrunning();
- ServiceCarStatus.mbBrainCtring = xbrainstate.mbbrainrunning();
- mdecition_period = xbrainstate.decision_period();
- mnTimeUpdateVS = mTimeState.elapsed();
- }
- /**
- * @brief ADCIntelligentVehicle::UpdateOBS
- * @param lidar_obs
- */
- void ADCIntelligentVehicle::UpdateOBS(std::shared_ptr<std::vector<iv::ObstacleBasic> > lidar_obs)
- {
- iv::ObsLiDAR _obs_grid_ = boost::shared_ptr<std::vector<iv::ObstacleBasic>>(new std::vector<iv::ObstacleBasic>);
- int i;
- int nSize = lidar_obs->size();
- for(i=0;i<nSize;i++)
- _obs_grid_->push_back(lidar_obs->at(i));
- ServiceLidar.copyfromlidarobs(_obs_grid_);
- ServiceCarStatus.mLidarS = 10;
- }
- void ADCIntelligentVehicle::UpdateV2xStEn(unsigned int enable)
- {
- iv::v2x::v2xStEn x;
- x.set_v2xsten(enable);
- int nsize = x.ByteSize();
- char * str = new char[nsize];
- if(x.SerializeToArray(str,nsize))
- {
- iv::modulecomm::ModuleSendMsg(mp_v2xStSend,str,nsize);
- }
- else
- {
- gIvlog->error("hmi","send enable st error");
- }
- delete str;
- }
- /**
- * @brief ADCIntelligentVehicle::ShareHMIMsg
- * @param xhmi
- */
- void ADCIntelligentVehicle::ShareHMIMsg(iv::hmi::HMIBasic xhmi)
- {
- iv::hmi::hmimsg xhmimsg;
- xhmimsg.set_mbpause(xhmi.mbPause);
- xhmimsg.set_mbbusmode(xhmi.mbbusmode);
- xhmimsg.set_mbbochemode(xhmi.mbBocheMode);
- int nsize = xhmimsg.ByteSize();
- char * str = new char[nsize];
- std::shared_ptr<char> pstr;
- pstr.reset(str); //将就对象的引用计数减1(当然,如果发现引用计数为0时,则析构旧对象),然后将新对象的指针交给智能指针保管。这样做可以省略了delete的过程。
- if(!xhmimsg.SerializeToArray(str,nsize))
- {
- std::cout<<"ADCIntelligentVehicle::ShareHMIMsg serialize error."<<std::endl;
- mpivlog->error("ADCIntelligentVehicle::ShareHMIMsg serialize error.");
- return;
- }
- iv::modulecomm::ModuleSendMsg(mpaHMI,str,nsize);
- }
- void ADCIntelligentVehicle::UpdateChassis(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
- {
- iv::chassis xchassis;
- static int ncount = 0;
- if(!xchassis.ParseFromArray(strdata,nSize))
- {
- std::cout<<"iv::decition::BrainDecition::UpdateChassis ParseFrom Array Error."<<std::endl;
- return;
- }
- if((xchassis.has_epsmode())&&(xchassis.epsmode() == 0))
- {
- ServiceCarStatus.mbRunPause = true;
- }
- if((xchassis.has_soc()))
- {
- mfSOC = xchassis.soc();
- }
- }
- void ADCIntelligentVehicle::onTimerManual()
- {
- // if(myview->mPressKeys.count() == 0)return;
- if(mbManual == false)
- {
- if(myview->mPressKeys.contains(Qt::Key_J))
- {
- mbManual = true;
- }
- }
- if(mbManual == true)
- {
- if(myview->mPressKeys.contains(Qt::Key_K))
- {
- mbManual = false;
- }
- }
- if(mbManual == false)return;
- int timediff = mManualTime.elapsed() - mnLastManualTime;
- mnLastManualTime = mManualTime.elapsed();
- if(myview->mPressKeys.contains(Qt::Key_W))
- {
- if(mnCtrlValue[0]<1000)mnCtrlValue[0] = mnCtrlValue[0] + 5 * timediff /20;
- }
- if(myview->mPressKeys.contains(Qt::Key_S))
- {
- mnCtrlValue[0] = 0;
- if(mnCtrlValue[1]<990) mnCtrlValue[1] = mnCtrlValue[1] + 10* timediff /20;
- }
- else
- {
- mnCtrlValue[1] = 0;
- }
- if(myview->mPressKeys.contains(Qt::Key_A))
- {
- if(mnCtrlValue[2]<500)mnCtrlValue[2] = mnCtrlValue[2] + 3* timediff /20;
- }
- if(myview->mPressKeys.contains(Qt::Key_D))
- {
- if(mnCtrlValue[2]>-500)mnCtrlValue[2] = mnCtrlValue[2] - 3* timediff /20;
- }
- iv::modulecomm::ModuleSendMsg(mpManualCtrl,(char *)mnCtrlValue,3*sizeof(int));
- }
- void ADCIntelligentVehicle::onCtrlManual(bool bCtrl)
- {
- mbManual = bCtrl;
- if(bCtrl)
- {
- mTimerManual->start(10);
- mManualTime.start();
- mnLastManualTime = mManualTime.elapsed();
- }
- else
- {
- mTimerManual->stop();
- mnCtrlValue[0] = 0; mnCtrlValue[1] = 0; mnCtrlValue[2] = 0;
- }
- }
- void ADCIntelligentVehicle::on_pb_v2xEn_clicked()
- {
- qDebug()<<mv2xStEn;
- if(mv2xStEn){
- ui->pb_v2xEn->setText("云平台控制:关");
- mv2xStEn = 0;
- }
- else{
- ui->pb_v2xEn->setText("云平台控制:开");
- mv2xStEn = 1;
- }
- gIvlog->info("hmi","v2x enable:%d", mv2xStEn);
- UpdateV2xStEn(mv2xStEn);
- }
- void ADCIntelligentVehicle::shareMapReqMsg()
- {
- iv::map::mapreq x;
- x.set_maptype(1);
- int nsize = x.ByteSize();
- char * str = new char[nsize];
- if(x.SerializeToArray(str,nsize))
- {
- iv::modulecomm::ModuleSendMsg(mpamapreq,str,nsize);
- }
- else
- {
- std::cout<<"iv::map::mapreq serialize error."<<std::endl;
- }
- std::cout<<"iv::map::mapreq serialize OK."<<std::endl;
- delete str;
- }
- void ADCIntelligentVehicle::UpdateVbox(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
- {
- iv::vbox::vbox xvbox;
- if(!xvbox.ParseFromArray(strdata,nSize))
- {
- gIvlog->warn("ADCIntelligentVehicle::UpdateVbox parse errror. nSize is %d",nSize);
- return;
- }
- ServiceCarStatus.st_straight = xvbox.st_straight();
- ServiceCarStatus.st_left = xvbox.st_left();
- ServiceCarStatus.st_right = xvbox.st_right();
- ServiceCarStatus.st_turn = xvbox.st_turn();
- ServiceCarStatus.time_straight = xvbox.time_straight();
- ServiceCarStatus.time_left = xvbox.time_left();
- ServiceCarStatus.time_right = xvbox.time_right();
- ServiceCarStatus.time_turn = xvbox.time_turn();
- }
- void ADCIntelligentVehicle::onStateTimer1()
- {
- ui->lcdNumber->display((int)ServiceCarStatus.time_turn);
- ui->lcdNumber_2->display((int)ServiceCarStatus.time_left);
- ui->lcdNumber_3->display((int)ServiceCarStatus.time_straight);
- ui->lcdNumber_4->display((int)ServiceCarStatus.time_right);
- switch (ServiceCarStatus.st_turn) {
- case 0:
- img.load(":/light_image/diaotou-black.png");
- img = img.scaled(66,66,Qt::IgnoreAspectRatio);
- ui->label_38->setPixmap(QPixmap::fromImage(img));
- break;
- case 1:
- img.load(":/light_image/diaotou-green.png");
- img = img.scaled(66,66,Qt::IgnoreAspectRatio);
- ui->label_38->setPixmap(QPixmap::fromImage(img));
- break;
- case 2:
- img.load(":/light_image/diaotou-red.png");
- img = img.scaled(66,66,Qt::IgnoreAspectRatio);
- ui->label_38->setPixmap(QPixmap::fromImage(img));
- break;
- case 3:
- img.load(":/light_image/diaotou-yellow.png");
- img = img.scaled(66,66,Qt::IgnoreAspectRatio);
- ui->label_38->setPixmap(QPixmap::fromImage(img));
- break;
- default:
- img.load(":/light_image/diaotou-black.png");
- img = img.scaled(66,66,Qt::IgnoreAspectRatio);
- ui->label_38->setPixmap(QPixmap::fromImage(img));
- break;
- }
- switch (ServiceCarStatus.st_left) {
- case 0:
- img2.load(":/light_image/zuoguai-black.png");
- img2 = img2.scaled(66,66,Qt::IgnoreAspectRatio);
- ui->label_33->setPixmap(QPixmap::fromImage(img2));
- break;
- case 1:
- img2.load(":/light_image/zuoguai-green.png");
- img2 = img2.scaled(66,66,Qt::IgnoreAspectRatio);
- ui->label_33->setPixmap(QPixmap::fromImage(img2));
- break;
- case 2:
- img2.load(":/light_image/zuoguai-red.png");
- img2 = img2.scaled(66,66,Qt::IgnoreAspectRatio);
- ui->label_33->setPixmap(QPixmap::fromImage(img2));
- break;
- case 3:
- img2.load(":/light_image/zuoguai-yellow.png");
- img2 = img2.scaled(66,66,Qt::IgnoreAspectRatio);
- ui->label_33->setPixmap(QPixmap::fromImage(img2));
- break;
- default:
- img2.load(":/light_image/zuoguai-black.png");
- img2 = img2.scaled(66,66,Qt::IgnoreAspectRatio);
- ui->label_33->setPixmap(QPixmap::fromImage(img2));
- break;
- }
- switch (ServiceCarStatus.st_straight) {
- case 0:
- img3.load(":/light_image/zhixing-black.png");
- img3 = img3.scaled(66,66,Qt::IgnoreAspectRatio);
- ui->label_35->setPixmap(QPixmap::fromImage(img3));
- break;
- case 1:
- img3.load(":/light_image/zhixing-green.png");
- img3 = img3.scaled(66,66,Qt::IgnoreAspectRatio);
- ui->label_35->setPixmap(QPixmap::fromImage(img3));
- break;
- case 2:
- img3.load(":/light_image/zhixing-red.png");
- img3 = img3.scaled(66,66,Qt::IgnoreAspectRatio);
- ui->label_35->setPixmap(QPixmap::fromImage(img3));
- break;
- case 3:
- img3.load(":/light_image/zhixing-yellow.png");
- img3 = img3.scaled(66,66,Qt::IgnoreAspectRatio);
- ui->label_35->setPixmap(QPixmap::fromImage(img3));
- break;
- default:
- img3.load(":/light_image/zhixing-black.png");
- img3 = img3.scaled(66,66,Qt::IgnoreAspectRatio);
- ui->label_35->setPixmap(QPixmap::fromImage(img3));
- break;
- }
- switch (ServiceCarStatus.st_right) {
- case 0:
- img4.load(":/light_image/youguai-black.png");
- img4 = img4.scaled(66,66,Qt::IgnoreAspectRatio);
- ui->label_32->setPixmap(QPixmap::fromImage(img4));
- break;
- case 1:
- img4.load(":/light_image/youguai-green.png");
- img4 = img4.scaled(66,66,Qt::IgnoreAspectRatio);
- ui->label_32->setPixmap(QPixmap::fromImage(img4));
- break;
- case 2:
- img4.load(":/light_image/youguai-red.png");
- img4 = img4.scaled(66,66,Qt::IgnoreAspectRatio);
- ui->label_32->setPixmap(QPixmap::fromImage(img4));
- break;
- case 3:
- img4.load(":/light_image/youguai-yellow.png");
- img4 = img4.scaled(66,66,Qt::IgnoreAspectRatio);
- ui->label_32->setPixmap(QPixmap::fromImage(img4));
- break;
- default:
- img4.load(":/light_image/youguai-black.png");
- img4 = img4.scaled(66,66,Qt::IgnoreAspectRatio);
- ui->label_32->setPixmap(QPixmap::fromImage(img4));
- break;
- }
- }
|