ADCIntelligentVehicle.cpp 80 KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063106410651066106710681069107010711072107310741075107610771078107910801081108210831084108510861087108810891090109110921093109410951096109710981099110011011102110311041105110611071108110911101111111211131114111511161117111811191120112111221123112411251126112711281129113011311132113311341135113611371138113911401141114211431144114511461147114811491150115111521153115411551156115711581159116011611162116311641165116611671168116911701171117211731174117511761177117811791180118111821183118411851186118711881189119011911192119311941195119611971198119912001201120212031204120512061207120812091210121112121213121412151216121712181219122012211222122312241225122612271228122912301231123212331234123512361237123812391240124112421243124412451246124712481249125012511252125312541255125612571258125912601261126212631264126512661267126812691270127112721273127412751276127712781279128012811282128312841285128612871288128912901291129212931294129512961297129812991300130113021303130413051306130713081309131013111312131313141315131613171318131913201321132213231324132513261327132813291330133113321333133413351336133713381339134013411342134313441345134613471348134913501351135213531354135513561357135813591360136113621363136413651366136713681369137013711372137313741375137613771378137913801381138213831384138513861387138813891390139113921393139413951396139713981399140014011402140314041405140614071408140914101411141214131414141514161417141814191420142114221423142414251426142714281429143014311432143314341435143614371438143914401441144214431444144514461447144814491450145114521453145414551456145714581459146014611462146314641465146614671468146914701471147214731474147514761477147814791480148114821483148414851486148714881489149014911492149314941495149614971498149915001501150215031504150515061507150815091510151115121513151415151516151715181519152015211522152315241525152615271528152915301531153215331534153515361537153815391540154115421543154415451546154715481549155015511552155315541555155615571558155915601561156215631564156515661567156815691570157115721573157415751576157715781579158015811582158315841585158615871588158915901591159215931594159515961597159815991600160116021603160416051606160716081609161016111612161316141615161616171618161916201621162216231624162516261627162816291630163116321633163416351636163716381639164016411642164316441645164616471648164916501651165216531654165516561657165816591660166116621663166416651666166716681669167016711672167316741675167616771678167916801681168216831684168516861687168816891690169116921693169416951696169716981699170017011702170317041705170617071708170917101711171217131714171517161717171817191720172117221723172417251726172717281729173017311732173317341735173617371738173917401741174217431744174517461747174817491750175117521753175417551756175717581759176017611762176317641765176617671768176917701771177217731774177517761777177817791780178117821783178417851786178717881789179017911792179317941795179617971798179918001801180218031804180518061807180818091810181118121813181418151816181718181819182018211822182318241825182618271828182918301831183218331834183518361837183818391840184118421843184418451846184718481849185018511852185318541855185618571858185918601861186218631864186518661867186818691870187118721873187418751876187718781879188018811882188318841885188618871888188918901891189218931894189518961897189818991900190119021903190419051906190719081909191019111912191319141915191619171918191919201921192219231924192519261927192819291930193119321933193419351936193719381939194019411942194319441945194619471948194919501951195219531954195519561957195819591960196119621963196419651966196719681969197019711972197319741975197619771978197919801981198219831984198519861987198819891990199119921993199419951996199719981999200020012002200320042005200620072008200920102011201220132014201520162017201820192020202120222023202420252026202720282029203020312032203320342035203620372038203920402041204220432044204520462047204820492050205120522053205420552056205720582059206020612062206320642065206620672068206920702071207220732074207520762077207820792080208120822083208420852086208720882089209020912092209320942095209620972098209921002101210221032104210521062107210821092110211121122113211421152116211721182119212021212122212321242125212621272128212921302131213221332134213521362137213821392140214121422143214421452146214721482149215021512152215321542155215621572158215921602161216221632164216521662167216821692170217121722173217421752176217721782179218021812182218321842185218621872188218921902191219221932194219521962197219821992200220122022203220422052206220722082209221022112212221322142215221622172218221922202221222222232224222522262227222822292230223122322233223422352236223722382239224022412242224322442245224622472248224922502251225222532254225522562257225822592260226122622263226422652266226722682269227022712272227322742275227622772278227922802281228222832284228522862287228822892290229122922293229422952296229722982299230023012302230323042305230623072308230923102311231223132314231523162317231823192320232123222323232423252326232723282329233023312332233323342335233623372338233923402341234223432344234523462347234823492350235123522353235423552356235723582359236023612362
  1. #include "ADCIntelligentVehicle.h"
  2. #include "chassis.pb.h"
  3. #include "xmlparam.h"
  4. #define qtcout qDebug() << "[ " << __FILE__ << ":" << __LINE__<< " ]";
  5. extern std::string gstrmemgps;
  6. extern std::string gstrvin;
  7. extern std::string gstrLat;
  8. extern std::string gstrLon;
  9. /**
  10. 常量 值 描述
  11. QHostAddress::Null 0 空地址对象,相当于QHostAddress()。
  12. QHostAddress::LocalHost 2 IPv4本地主机地址,相当于QHostAddress(“127.0.0.1”)。
  13. QHostAddress::LocalHostIPv6 3 IPv6本地主机地址,相当于 QHostAddress(“::1”)。
  14. QHostAddress::Broadcast 1 Pv4广播地址,相当于QHostAddress(“255.255.255.255”)。
  15. QHostAddress::AnyIPv4 6 IPv4 any-address,相当于QHostAddress(“0.0.0.0”)。与该地址绑定的socket将只监听IPv4接口。
  16. QHostAddress::AnyIPv6 5 IPv6 any-address,相当于QHostAddress(“::”)。与该地址绑定的socket将只监听IPv4接口。
  17. QHostAddress::Any 4 双any-address栈,与该地址绑定的socket将侦听IPv4和IPv6接口。
  18. */
  19. #define VIEW_CENTER viewport()->rect().center()
  20. const double PI = 3.1415926535898;
  21. extern iv::Ivlog * gIvlog;
  22. /**
  23. * @brief MyView::MyView
  24. * @param parent
  25. */
  26. MyView::MyView(QWidget *parent) :
  27. QGraphicsView(parent),
  28. beishu(1.00000)
  29. {
  30. setDragMode(QGraphicsView::ScrollHandDrag); //设置视图的拖拽模式 (光标变为指向手,然后拖动鼠标将滚动滚动条,此模式在交互和非交互模式下均有效)
  31. }
  32. /**
  33. * @brief MyView::mousePressEvent
  34. * @param event
  35. */
  36. void MyView::mousePressEvent(QMouseEvent *event)
  37. {
  38. bottonstatus = true;
  39. QGraphicsView::mousePressEvent(event);
  40. }
  41. /**
  42. * @brief MyView::mouseMoveEvent
  43. * @param event
  44. */
  45. void MyView::mouseMoveEvent(QMouseEvent *event)
  46. {
  47. QGraphicsView::mouseMoveEvent(event);
  48. }
  49. void MyView::keyPressEvent(QKeyEvent *event)
  50. {
  51. //按键按下,key值放入容器,如果是长按触发的repeat就不判断
  52. if(!event->isAutoRepeat())
  53. mPressKeys.insert(event->key());
  54. if(event->key() == Qt::Key_J)
  55. {
  56. emit CtrlManual(true);
  57. }
  58. if(event->key() == Qt::Key_K)
  59. {
  60. emit CtrlManual(false);
  61. }
  62. // qDebug("key count is %d",mPressKeys.count());
  63. // QDateTime dt;
  64. // qDebug("key press %ld",QDateTime::currentMSecsSinceEpoch());
  65. // qDebug(" key is %d",event->key());
  66. }
  67. void MyView::keyReleaseEvent(QKeyEvent *event)
  68. {
  69. if(!event->isAutoRepeat())mPressKeys.remove(event->key());
  70. qDebug("key count is %d",mPressKeys.count());
  71. // QDateTime dt;
  72. // qDebug("key release %ld",QDateTime::currentMSecsSinceEpoch());
  73. }
  74. /**
  75. * @brief MyView::mouseReleaseEvent
  76. * @param event
  77. */
  78. void MyView::mouseReleaseEvent(QMouseEvent *event)
  79. {
  80. bottonstatus = false;
  81. QGraphicsView::mouseReleaseEvent(event);
  82. }
  83. ADCIntelligentVehicle * gAV;
  84. //void ListenTraceMap(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
  85. //{
  86. // gAV->UpdateMap(strdata,nSize);
  87. //}
  88. // void Listengpsimu(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
  89. // {
  90. // gAV->UpdateGPSIMU(strdata,nSize);
  91. // }
  92. // void ListenRadar(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
  93. // {
  94. // gAV->UpdateRADAR(strdata,nSize);
  95. // }
  96. /**
  97. * @brief ListenCANState
  98. * @param strdata
  99. * @param nSize
  100. * @param index
  101. * @param dt
  102. * @param strmemname
  103. */
  104. void ListenCANState(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
  105. {
  106. // gIvlog->debug("ListenCanState");
  107. gAV->UpdateCANState(strdata,nSize);
  108. }
  109. //void ListenDecition(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
  110. //{
  111. // gAV->UpdateDecition(strdata,nSize);
  112. //}
  113. //void ListenVehicleState(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
  114. //{
  115. // gAV->UpdateVehicleState(strdata,nSize);
  116. //}
  117. /**
  118. * @brief ListenOBS
  119. * @param strdata
  120. * @param nSize
  121. * @param index
  122. * @param dt
  123. * @param strmemname
  124. */
  125. void ListenOBS(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
  126. {
  127. // qDebug("size is %d",nSize);
  128. std::shared_ptr<std::vector<iv::ObstacleBasic>> lidar_obs(new std::vector<iv::ObstacleBasic>);
  129. iv::ObstacleBasic * pdata = (iv::ObstacleBasic *)strdata;
  130. int nCount = nSize/sizeof(iv::ObstacleBasic);
  131. int i;
  132. for(i=0;i<nCount;i++)
  133. {
  134. iv::ObstacleBasic temp;
  135. memcpy(&temp,pdata,sizeof(iv::ObstacleBasic));
  136. lidar_obs->push_back(temp);
  137. pdata++;
  138. }
  139. gAV->UpdateOBS(lidar_obs);
  140. // gw->UpdateOBS(lidar_obs);
  141. }
  142. //v2x使能状态请求
  143. void ListenV2xStReq(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
  144. {
  145. // gIvlog->info("hmi", "v2x enable request %d %d",nSize,sizeof(iv::v2x::v2xStReq));
  146. // if(nSize < sizeof(iv::v2x::v2xStReq))
  147. // return;
  148. iv::v2x::v2xStReq xv2xStReqMsg;
  149. if(!xv2xStReqMsg.ParseFromArray(strdata,nSize))
  150. {
  151. gIvlog->error("hmi", "iv::v2x::xv2xStReqMsg::ListenV2xStReq parse error");
  152. return;
  153. }
  154. if(xv2xStReqMsg.v2xstreq())
  155. {
  156. gAV->UpdateV2xStEn(gAV->mv2xStEn);
  157. gIvlog->info("hmi", "v2x enable request %d",gAV->mv2xStEn);
  158. }
  159. }
  160. //vbu数据处理
  161. void ListenObu(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
  162. {
  163. iv::v2r::v2r_send xv2rSendMsg;
  164. if(!xv2rSendMsg.ParseFromArray(strdata,nSize))
  165. {
  166. gIvlog->error("hmi", "iv::v2r::xv2rSendMsg::ListenObu parse error");
  167. return;
  168. }
  169. gAV->updateV2R(xv2rSendMsg);
  170. }
  171. /**
  172. * @brief ADCIntelligentVehicle::ADCIntelligentVehicle
  173. * @param parent
  174. */
  175. ADCIntelligentVehicle::ADCIntelligentVehicle(QWidget *parent)
  176. : QMainWindow(parent),
  177. ui(new Ui::ADCIntelligentVehicle),
  178. m_translateButton(Qt::LeftButton),
  179. m_translateSpeed(1.0),
  180. m_zoomDelta(0.1),
  181. m_bMouseTranslate(false),
  182. m_scale(1.0),
  183. horizontalOffset(0),
  184. verticalOffset(0),
  185. scaleFactor(1),
  186. currentStepScaleFactor(1)
  187. {
  188. ui->setupUi(this);
  189. gAV = this;
  190. gIvlog->info("ADCIntelligentVehicle Start Initialize");
  191. mTime.start();
  192. /*
  193. QString strpath = QCoreApplication::applicationDirPath();
  194. strpath = strpath + "/ADS_decision.xml";
  195. iv::xmlparam::Xmlparam xp(strpath.toStdString());
  196. ServiceCarStatus.mvehtype= xp.GetParam("vehType","ge3");
  197. if(ServiceCarStatus.mvehtype=="ge3"){
  198. ;
  199. }else if( ServiceCarStatus.mvehtype=="qingyuan"){
  200. ;
  201. }else if( ServiceCarStatus.mvehtype=="vv7"){
  202. ;
  203. }else if( ServiceCarStatus.mvehtype=="zhongche"){
  204. ;
  205. }
  206. */
  207. QString strpath = QCoreApplication::applicationDirPath();
  208. strpath = strpath + "/ui_ads_hmi.xml";
  209. iv::xmlparam::Xmlparam xp(strpath.toStdString());
  210. ServiceCarStatus.esr_y_offset = atof(xp.GetParam("esr_y_offset","2.5").data());
  211. ServiceCarStatus.lidar_y_offset = atof(xp.GetParam("lidar_y_offset","0").data());
  212. ui->stackedWidget->setCurrentIndex(0);
  213. // /********************************** 百度地图显示 ********************************/
  214. // mMapview = new QWebEngineView(ui->stackedWidget->widget(0));
  215. //// qDebug((QDir::currentPath()).toLatin1().data());
  216. // mMapview->load(QUrl(QString("file:///%1/%2").arg(QApplication::applicationDirPath()).arg("BaiDuMap.html")));
  217. // mMapview->setGeometry(750,250,400,400);
  218. // /********************************************************************************/
  219. // ui->listWidget->setCurrentRow(1); //apollo_fu debug 20200409
  220. myview = new MyView(ui->stackedWidget->widget(0));
  221. myview->setObjectName(QStringLiteral("graphicsView"));
  222. myview->setGeometry(QRect(0, 100, 900, 750)); //从屏幕上(0,100)位置开始(即为最左上角的点),显示一个900*900的界面(宽900,高900)
  223. #if 0
  224. myview_small = new MyView(ui->stackedWidget->widget(1));
  225. myview_small->setObjectName(QStringLiteral("graphicsView_small"));
  226. myview_small->setGeometry(QRect(1000,100,500,500));
  227. myview_small->setAlignment(Qt::AlignLeft | Qt::AlignTop);
  228. #endif
  229. // myview_small->scale(0.7,0.7);
  230. //myview_small->centerOn(-200,-200);
  231. image = new QImage(900 , 750 , QImage::Format_RGB32);//画布的初始化大小设为600*500,使用32位颜色
  232. //QImage的32、24、8位图。 图像格式:QImage::Format_RGB32 ,QImage::Format_RGB888,QImage::Format_Indexed8。
  233. //QImage myImage2 = QImage(width, height, QImage::Format_…); 根据图像宽高来构造一幅图像,程序会自动根据图像格式对齐图像数据。
  234. // image_small = new QImage(,1500,QImage::Format_RGB32);
  235. myview->setCacheMode(myview->CacheBackground);
  236. #if 0
  237. myview_small->setCacheMode(myview_small->CacheBackground);
  238. #endif
  239. painter = new QPainter(image); //创建QPainter对象
  240. // painter_small = new QPainter(image_small); //创建QPainter对象
  241. scene = new QGraphicsScene;
  242. scene_small = new QGraphicsScene;
  243. timer = new QTimer(this);
  244. connect(timer, SIGNAL(timeout()), this, SLOT(timeoutslot()));//timeoutslot()为自定义槽
  245. timer->start(100);
  246. Lidar_obsread = boost::shared_ptr<std::vector<iv::ObstacleBasic>>(new std::vector<iv::ObstacleBasic>);
  247. Lidar_read = boost::shared_ptr<std::vector<iv::ObstacleBasic>>(new std::vector<iv::ObstacleBasic>);
  248. mDataToUI.mHead.type = 1;
  249. mDataToUI.mInfo.accelerate = 0;
  250. mDataToUI.mInfo.brake = 0;
  251. mDataToUI.mInfo.gps_lat = 0;
  252. mDataToUI.mInfo.gps_lng = 0;
  253. mDataToUI.mInfo.is_mapLoad = 0;
  254. mDataToUI.mInfo.is_initSuccess = 0;
  255. mDataToUI.mInfo.is_run = 0;
  256. mDataToUI.mInfo.speed = 0;
  257. msockrecv.bind(QHostAddress::Any,9998);//绑定端口
  258. connect(&msockrecv,SIGNAL(readyRead()),this,SLOT(onRecvUDP())); //接收来自PAD的UDP数据报 readyRead():socket读取缓冲区有数据时发送此信号,在此信号的槽函数里读取缓冲区的数据。
  259. ServiceCarStatus.speed = 0.0;
  260. mTimerState.setTimerType(Qt::PreciseTimer); //设置计时器的精确程度 Qt::PreciseTimer值为0,含义:精确定时器试图保持毫秒精度
  261. connect(&mTimerState,SIGNAL(timeout()),this,SLOT(onStateTimer()));//广播发送UDP数据报
  262. mTimerState.start(100); //定时100ms
  263. mTimerStateMap.setTimerType(Qt::PreciseTimer); //
  264. connect(&mTimerStateMap,SIGNAL(timeout()),this,SLOT(onStateTimerMap()));//
  265. mTimerStateMap.start(500); //定时500ms
  266. is_show_enable = true;
  267. #ifdef USE_PAD_CTRL
  268. AutoStart();
  269. #endif
  270. ModuleFun funplantrace = std::bind(&ADCIntelligentVehicle::UpdatePlanTrace,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
  271. mpaplantrace = iv::modulecomm::RegisterRecvPlus("plantrace",funplantrace);
  272. ModuleFun funplantrace_left = std::bind(&ADCIntelligentVehicle::UpdatePlanTrace_left,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
  273. mpaplantrace_left = iv::modulecomm::RegisterRecvPlus("obstraceleft",funplantrace_left);
  274. ModuleFun funplantrace_right = std::bind(&ADCIntelligentVehicle::UpdatePlanTrace_right,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
  275. mpaplantrace_right = iv::modulecomm::RegisterRecvPlus("obstraceright",funplantrace_right);
  276. ModuleFun funfusion = std::bind(&ADCIntelligentVehicle::UpdateFusion,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
  277. mpafusion = iv::modulecomm::RegisterRecvPlus("li_ra_fusion",funfusion);
  278. ModuleFun funmap = std::bind(&ADCIntelligentVehicle::UpdateMap,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
  279. mpa = iv::modulecomm::RegisterRecvPlus("tracemap",funmap);
  280. // mpa = iv::modulecomm::RegisterRecv("tracemap",ListenTraceMap);
  281. ModuleFun fungpsimu =std::bind(&ADCIntelligentVehicle::UpdateGPSIMU,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
  282. mpagpsimu = iv::modulecomm::RegisterRecvPlus(gstrmemgps.data(),fungpsimu);
  283. // mpagpsimu = iv::modulecomm::RegisterRecv("gpsimu",Listengpsimu);
  284. ModuleFun funradar =std::bind(&ADCIntelligentVehicle::UpdateRADAR,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
  285. mparadar = iv::modulecomm::RegisterRecvPlus("radar",funradar);
  286. ModuleFun funchassis =std::bind(&ADCIntelligentVehicle::UpdateChassis,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
  287. mpachassis = iv::modulecomm::RegisterRecvPlus("chassis",funchassis);
  288. mpacanstate = iv::modulecomm::RegisterRecv("canstate",ListenCANState);
  289. gIvlog->debug("canState ptr: %x",mpacanstate);
  290. ModuleFun funbrainstate =std::bind(&ADCIntelligentVehicle::UpdateVehicleState,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
  291. // mpaVechicleState = iv::modulecomm::RegisterRecvPlus("vehiclestate",funbrainstate);
  292. mpaVechicleState = iv::modulecomm::RegisterRecvPlus("brainstate",funbrainstate);
  293. ModuleFun fundecition =std::bind(&ADCIntelligentVehicle::UpdateDecition,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
  294. mpaDecition = iv::modulecomm::RegisterRecvPlus("deciton",fundecition);
  295. mpaLidar = iv::modulecomm::RegisterRecv("lidar_obs",ListenOBS);
  296. mpaObu = iv::modulecomm::RegisterRecv("v2r_send",ListenObu);
  297. mpaHMI = iv::modulecomm::RegisterSend("hmi",10*sizeof(iv::hmi::hmimsg),10);
  298. mpadst = iv::modulecomm::RegisterSend("xodrreq",1000,1);//地图站点gps信息
  299. iv::modulecomm::RegisterRecv("v2xStReq", ListenV2xStReq);
  300. mp_v2xStSend = iv::modulecomm::RegisterSend("v2xStEn",1000,1);
  301. mTimeState.start();
  302. mpManualCtrl = iv::modulecomm::RegisterSend("ivmanual",1000,1);
  303. mTimerManual = new QTimer(this);
  304. connect(mTimerManual,SIGNAL(timeout()),this,SLOT(onTimerManual()));
  305. connect(myview,SIGNAL(CtrlManual(bool)),this,SLOT(onCtrlManual(bool)));
  306. mnCtrlValue[0] = 0; mnCtrlValue[1] = 0; mnCtrlValue[2] = 0;
  307. // timermanualctrl->start(10);
  308. //connect(ui->lineEdit_6, SIGNAL(textEdited(const QString &)), this, SLOT(savestabuyEditinfo(const QString &)));
  309. mpamapreq = iv::modulecomm::RegisterSend("mapreq",1000,1);
  310. shareMapReqMsg();
  311. ModuleFun funvbox = std::bind(&ADCIntelligentVehicle::UpdateVbox,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
  312. mpaplantrace = iv::modulecomm::RegisterRecvPlus("vbox",funvbox);
  313. #if 1 //红绿灯
  314. ui->lcdTurn->setDigitCount(3); //设置显示几个数字
  315. ui->lcdTurn->setMode(QLCDNumber::Dec);
  316. ui->lcdTurn->setSegmentStyle(QLCDNumber::Flat);//设置数字字体:Outline,轮廓突出,颜色为背景色;Filled,内部填充型,颜色为黑色;Flat,平面型
  317. // 设置数字颜色时要注意: 函数setSegmentStyle(QLCDNumber::Flat)中选择Flat属性,否则在setColor()设置数字颜色时不生效
  318. // 设置背景色直接用setStyleSheet()函数设定就可以了。
  319. //调色板
  320. QPalette lcdpat = ui->lcdTurn->palette();
  321. /*设置颜色,整体背景颜色 颜色蓝色,此函数的第一个参数可以设置多种。如文本、按钮按钮文字、多种*/
  322. lcdpat.setColor(QPalette::Normal,QPalette::WindowText,Qt::red);
  323. //设置当前窗口的调色板
  324. ui->lcdTurn->setPalette(lcdpat);
  325. //设置背景色
  326. //ui->lcdNumber->setStyleSheet("background-color: black");
  327. //ui->lcdNumber->display(QDateTime::currentDateTime().toString("yyyy-MM-dd hh:mm:ss"));
  328. ui->lcdTurn->display(0);
  329. ui->lcdLeft->setDigitCount(3);
  330. ui->lcdLeft->setMode(QLCDNumber::Dec);
  331. ui->lcdLeft->setSegmentStyle(QLCDNumber::Flat);
  332. ui->lcdLeft->setPalette(lcdpat);
  333. //ui->lcdLeft->setStyleSheet("background-color: black");
  334. ui->lcdLeft->display(0);
  335. ui->lcdStraight->setDigitCount(3);
  336. ui->lcdStraight->setMode(QLCDNumber::Dec);
  337. ui->lcdStraight->setSegmentStyle(QLCDNumber::Flat);
  338. ui->lcdStraight->setPalette(lcdpat);
  339. //ui->lcdStraight->setStyleSheet("background-color: black");
  340. ui->lcdStraight->display(0);
  341. ui->lcdRight->setDigitCount(3);
  342. ui->lcdRight->setMode(QLCDNumber::Dec);
  343. ui->lcdRight->setSegmentStyle(QLCDNumber::Flat);
  344. ui->lcdRight->setPalette(lcdpat);
  345. //ui->lcdRight->setStyleSheet("background-color: black");
  346. ui->lcdRight->display(0);
  347. // 新建qimage
  348. // QImage img,img2,img3,img4;
  349. // 加载图片
  350. img.load(":/light_image/diaotou-black.png");
  351. img = img.scaled(66,66,Qt::IgnoreAspectRatio);
  352. ui->label_38->setPixmap(QPixmap::fromImage(img));
  353. img2.load(":/light_image/zuoguai-black.png");
  354. img2 = img2.scaled(66,66,Qt::IgnoreAspectRatio);
  355. ui->label_33->setPixmap(QPixmap::fromImage(img2));
  356. img3.load(":/light_image/zhixing-black.png");
  357. img3 = img3.scaled(66,66,Qt::IgnoreAspectRatio);
  358. ui->label_35->setPixmap(QPixmap::fromImage(img3));
  359. img4.load(":/light_image/youguai-black.png");
  360. img4 = img4.scaled(66,66,Qt::IgnoreAspectRatio);
  361. ui->label_32->setPixmap(QPixmap::fromImage(img4));
  362. mTimerState1.setTimerType(Qt::PreciseTimer); //设置计时器的精确程度 Qt::PreciseTimer值为0,含义:精确定时器试图保持毫秒精度
  363. connect(&mTimerState1,SIGNAL(timeout()),this,SLOT(onStateTimerTraffic()));//广播发送UDP数据报
  364. mTimerState1.start(100); //定时100ms
  365. #endif
  366. ui->lineEdit_vin->setText(QString::fromStdString(gstrvin));
  367. mdLon = QString::fromStdString(gstrLon).toDouble();
  368. mdLat = QString::fromStdString(gstrLat).toDouble();
  369. }
  370. /**
  371. * @brief ADCIntelligentVehicle::savestabuyEditinfo
  372. * @param txt
  373. */
  374. void ADCIntelligentVehicle::savestabuyEditinfo(const QString &txt)
  375. {
  376. //ServiceControlStatus.set_accelerate((float)txt.toDouble());
  377. }
  378. /**
  379. * @brief ADCIntelligentVehicle::timeoutslot
  380. */
  381. void ADCIntelligentVehicle::timeoutslot()
  382. {
  383. update();
  384. /////////////////////////////////
  385. // //
  386. // 实时状态信息显示 //
  387. ////////////////////////////////
  388. QString is_ok;
  389. ui->lineEdit_RTK->setText(QString::number(ServiceCarStatus.location->rtk_status));//rtk状态
  390. if(ServiceCarStatus.location->rtk_status==6)
  391. {
  392. ui->button_RTK_st->setStyleSheet("background-color: green");
  393. }
  394. else
  395. {
  396. ui->button_RTK_st->setStyleSheet("background-color: red");
  397. }
  398. ui->lineEdit_INS->setText(QString::number(ServiceCarStatus.location->ins_status));//ins状态
  399. if(ServiceCarStatus.location->ins_status==4)
  400. ui->button_INS_st->setStyleSheet("background-color: green");
  401. else
  402. ui->button_INS_st->setStyleSheet("background-color: red");
  403. ui->lineEdit_lon->setText(QString::number(ServiceCarStatus.location->gps_lng,'f',7));
  404. ui->lineEdit_lat->setText(QString::number(ServiceCarStatus.location->gps_lat,'f',7));
  405. ui->lineEdit_heading->setText(QString::number(ServiceCarStatus.location->ins_heading_angle,'f',1));
  406. is_ok = (ServiceCarStatus.mLidarS>0)?QStringLiteral("ok"):QStringLiteral("error");
  407. ui->lineEdit_lidar->setText(is_ok);//激光雷达状态
  408. if(ServiceCarStatus.mLidarS>0)
  409. {
  410. ui->button_lidar_st->setStyleSheet("background-color: green");
  411. }
  412. else
  413. {
  414. ui->button_lidar_st->setStyleSheet("background-color:red");
  415. }
  416. ui->lineEdit_dec_period->setText(QString::number(mdecition_period)+QStringLiteral("ms"));//决策周期
  417. if(mdecition_period>0)
  418. {
  419. ui->button_decition_st->setStyleSheet("background-color: green");
  420. }
  421. if((mTimeState.elapsed()-mnTimeUpdateGPS) > 1000)
  422. {
  423. ui->button_RTK_st->setStyleSheet("background-color: red");
  424. ui->button_INS_st->setStyleSheet("background-color: red");
  425. }
  426. if((mTimeState.elapsed() - mnTimeUpdateDecition)>1000)
  427. {
  428. ui->button_decition_st->setStyleSheet("background-color:red");
  429. ui->label_decition->setText("No decition");
  430. mbBrainRunning = false;
  431. }
  432. else
  433. {
  434. QString strdec = "Acc:";
  435. strdec = strdec + QString::number(ServiceCarStatus.mfAcc,'f',1);
  436. // strdec = strdec + " Brake:";
  437. // strdec = strdec + QString::number(ServiceCarStatus.mfBrake,'f',1);
  438. strdec = strdec + " EPS:";
  439. strdec = strdec + QString::number(ServiceCarStatus.mfWheel,'f',1);
  440. ui->label_decition->setText(strdec);
  441. }
  442. if(mbManual)
  443. {
  444. char strmanual[256];
  445. snprintf(strmanual,256,"Manual %d %d %d",mnCtrlValue[0],mnCtrlValue[1],mnCtrlValue[2]);
  446. ui->label_decition->setText(strmanual);
  447. }
  448. /////////////////////////////////
  449. // //
  450. // 实时显示障碍物状态 //
  451. /////////////////////////////////
  452. // ServiceCarStatus.mObs = 10;
  453. // ServiceCarStatus.mLidarObs = 9.123;
  454. // ServiceCarStatus.mRadarObs = 9.222;
  455. // ServiceCarStatus.mfttc = 0.5;
  456. // ServiceCarStatus.mfBrake = 20;
  457. if(ServiceCarStatus.mObs > 0)
  458. {
  459. ui->label_13->setText("Obs:"+QString::number(ServiceCarStatus.mObs,'f',1)+" Lidar:"
  460. +QString::number(ServiceCarStatus.mLidarObs,'f',1) + " Radar:"
  461. +QString::number(ServiceCarStatus.mRadarObs,'f',1));
  462. }
  463. else
  464. {
  465. ui->label_13->setText(QString("前方没有障碍物"));
  466. }
  467. ui->label_26->setText("ttc is "+QString::number(ServiceCarStatus.mfttc,'f',1) + " brake is " + QString::number(ServiceCarStatus.mfBrake,'f',2)); //add 20200518
  468. QString strstate = "Map:";
  469. if(mDataToUI.mInfo.is_mapLoad == 1)
  470. {
  471. strstate = strstate+"Yes ";
  472. }
  473. else
  474. {
  475. strstate = strstate+"No ";
  476. }
  477. strstate = strstate+" CANCARD:";
  478. if(mDataToUI.mInfo.is_initSuccess == 1)
  479. {
  480. strstate = strstate+"OK ";
  481. }
  482. else
  483. {
  484. strstate = strstate+"Fail ";
  485. }
  486. strstate = strstate + " Run State:";
  487. if(mbBrainRunning)
  488. {
  489. strstate = strstate+"RUN";
  490. }
  491. else
  492. {
  493. strstate = strstate+"STOP";
  494. }
  495. ui->label_State->setText(strstate);
  496. ui->lb_current_speed_big->setText(QString::number(ServiceCarStatus.speed,'f',1));
  497. // QString is_ok;
  498. // is_ok = (ServiceLidar.did_lidar_ok() == true)?QStringLiteral("ok"):QStringLiteral("error");
  499. // ui->lineEdit_4->setText(is_ok);
  500. //can state
  501. mDataToUI.mInfo.is_initSuccess = 0;
  502. }
  503. /**
  504. * @brief ADCIntelligentVehicle::on_pb_load_navigation_data_clicked
  505. */
  506. //void ADCIntelligentVehicle::on_pb_load_navigation_data_clicked() //加载地图
  507. //{
  508. //}
  509. void ADCIntelligentVehicle::updateV2R(iv::v2r::v2r_send xv2rMsg)
  510. {
  511. if(xv2rMsg.has_radiolighttype())
  512. {
  513. miLightType = xv2rMsg.radiolighttype();
  514. miLightRemain = xv2rMsg.radiolightremain();
  515. ServiceCarStatus.st_straight = miLightType;
  516. ServiceCarStatus.time_straight = miLightRemain;
  517. miobuSt = 0;
  518. }
  519. if(xv2rMsg.has_radiobroadcastgpslat())
  520. {
  521. miBroadcastGpsLat = xv2rMsg.radiobroadcastgpslat();
  522. miBroadcastGpsLon = xv2rMsg.radiobroadcastgpslon();
  523. miobuSt = 0;
  524. }
  525. if(xv2rMsg.has_radiobroadcastrange())
  526. {
  527. miBroadcastRange = xv2rMsg.radiobroadcastrange();
  528. miBroadcastTrafficType = xv2rMsg.radiobroadcasttraffictype();
  529. miobuSt = 0;
  530. }
  531. if(xv2rMsg.has_radiowarningspeedlimit())
  532. {
  533. miBroadcastSpeedLimit = xv2rMsg.radiobroadcastspeedlimit();
  534. miobuSt = 0;
  535. }
  536. if(xv2rMsg.has_radiowarningtype())
  537. {
  538. miWarningType = xv2rMsg.radiowarningtype();
  539. miobuSt = 0;
  540. }
  541. if(xv2rMsg.has_radiowarningspeedlimit())
  542. {
  543. miWarningSpeedLimit = xv2rMsg.radiowarningspeedlimit();
  544. miobuSt = 0;
  545. }
  546. if(xv2rMsg.has_radioidentistart())
  547. {
  548. miIdentiStart = xv2rMsg.radioidentistart();
  549. miobuSt = 0;
  550. }
  551. }
  552. /**
  553. * @brief ADCIntelligentVehicle::onRecvUDP
  554. */
  555. void ADCIntelligentVehicle::onRecvUDP()
  556. {
  557. while (msockrecv.hasPendingDatagrams()) //hasPendingDatagrams()表示是否有待读取的传入数据报
  558. {
  559. qDebug("recv cmd");
  560. QByteArray ba;
  561. ba.resize(msockrecv.pendingDatagramSize());//pendingDatagramSize()返回待读取数据报的字节数
  562. msockrecv.readDatagram(ba.data(), ba.size()); //readDatagram()函数用于读取数据报内容
  563. int * p;
  564. iv::hmi::HMIBasic hmi;
  565. p = (int *)ba.data();
  566. // qDebug("cmd is ",*p);
  567. // std::cout<<"data len is "<<ba.size()<<" value is "<<*p<<std::endl;
  568. if(*p == 1)
  569. {
  570. std::cout<<ServiceCarStatus.mRunTime.elapsed()<<":"<<"recv start."<<std::endl;
  571. ServiceCarStatus.mbRunPause = false;
  572. hmi.mbBocheMode = false;
  573. hmi.mbPause = false;
  574. ShareHMIMsg(hmi);
  575. }
  576. if(*p == 0)
  577. {
  578. ServiceCarStatus.mbRunPause = true;
  579. hmi.mbBocheMode = false;
  580. hmi.mbPause = true;
  581. ShareHMIMsg(hmi);
  582. }
  583. if(*p == 2)
  584. {
  585. ServiceCarStatus.bocheMode = true;
  586. hmi.mbBocheMode = true;
  587. hmi.mbPause = false;
  588. ShareHMIMsg(hmi);
  589. }
  590. if(*p == 3)
  591. {
  592. hmi.mbbusmode = true;
  593. hmi.mbPause = ServiceCarStatus.mbRunPause;
  594. ShareHMIMsg(hmi);
  595. }
  596. if(*p == 4)
  597. {
  598. hmi.mbbusmode = false;
  599. hmi.mbPause = ServiceCarStatus.mbRunPause;
  600. ShareHMIMsg(hmi);
  601. }
  602. }
  603. }
  604. /**
  605. * @brief ADCIntelligentVehicle::onStateTimer
  606. */
  607. void ADCIntelligentVehicle::onStateTimer()
  608. {
  609. if(ServiceCarStatus.mbBrainCtring)
  610. mDataToUI.mInfo.is_run = 1;
  611. else
  612. mDataToUI.mInfo.is_run = 0;
  613. if(ServiceCarStatus.mLidarS<0)
  614. mDataToUI.mInfo.lidarStatus = 0;
  615. else
  616. mDataToUI.mInfo.lidarStatus = 1;
  617. if(ServiceCarStatus.mRadarS<0)
  618. mDataToUI.mInfo.radarStatus = 0;
  619. else
  620. mDataToUI.mInfo.radarStatus = 1;
  621. ServiceCarStatus.mLidarS--; //如果从10减到0,表示没有接收到数据。
  622. ServiceCarStatus.mRadarS--;
  623. mDataToUI.mInfo.RTKstatus = ServiceCarStatus.mRTKStatus;
  624. ServiceCarStatus.mRTKStatus = 0;
  625. mDataToUI.mInfo.radarobs = ServiceCarStatus.mRadarObs;
  626. mDataToUI.mInfo.lidarobs = ServiceCarStatus.mLidarObs;
  627. mDataToUI.mInfo.obs = ServiceCarStatus.mObs;
  628. mDataToUI.mInfo.gps_lat = ServiceCarStatus.location->gps_lat;
  629. mDataToUI.mInfo.gps_lng = ServiceCarStatus.location->gps_lng;
  630. mDataToUI.mInfo.accelerate = ServiceCarStatus.mfAcc;
  631. mDataToUI.mInfo.speed = ServiceCarStatus.speed;
  632. mDataToUI.mInfo.brake = ServiceCarStatus.mfBrake;
  633. mDataToUI.mInfo.swerve = ServiceCarStatus.mfWheel;
  634. mDataToUI.mInfo.boche_status = ServiceCarStatus.bocheEnable;
  635. if(fabs(mDataToUI.mInfo.gps_lat)<1 )
  636. {
  637. // qDebug("gps lat is error.");
  638. // std::cout<<"no gps."<<std::endl;
  639. }
  640. // std::cout<<"send data."<<std::endl;
  641. // qDebug("speed is %d",mDataToUI.mInfo.speed);
  642. // mDataToUI.mInfo.is_initSuccess = 1;
  643. // mDataToUI.mInfo.speed = 36.1;
  644. msocksend.writeDatagram((char *)&mDataToUI,sizeof(DataToUI),QHostAddress::Broadcast,9999); //QHostAddress::Broadcast 值为1 Pv4广播地址,相当于QHostAddress(“255.255.255.255”)。
  645. // _udp->writeDatagram(str.toUtf8(), QHostAddress::Broadcast, 10002);
  646. // multicast, 224.0.0.1~224.0.0.255 is multicast address of LAN
  647. // _udp->writeDatagram(str.toUtf8(), QHostAddress("224.0.0.131"), 10002);
  648. }
  649. void ADCIntelligentVehicle::onStateTimerMap()
  650. {
  651. /*******************************更新百度地图******************************/
  652. double flat = ServiceCarStatus.location->gps_lat;
  653. double flon = ServiceCarStatus.location->gps_lng;
  654. double fang = ServiceCarStatus.location->ins_heading_angle;
  655. char strscript[256];
  656. snprintf(strscript,255,"theLocation(%11.7f,%11.7f,%11.7f);",flon,flat,fang);
  657. /************************************************************************/
  658. }
  659. /**
  660. * @brief ADCIntelligentVehicle::AutoStart
  661. */
  662. void ADCIntelligentVehicle::AutoStart()
  663. {
  664. // is_brain_running_flag_ = true;
  665. // mDataToUI.mInfo.is_initSuccess = 1;
  666. // std::cout<<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<<"Start Run SuccessFully."<<std::endl;
  667. }
  668. void ADCIntelligentVehicle::onStateTimerTraffic()
  669. {
  670. if(miobuSt < 10)
  671. {
  672. miobuSt++;
  673. ui->lcdStraight->display((int)ServiceCarStatus.time_straight);
  674. switch (ServiceCarStatus.st_straight) {
  675. case 0:
  676. img3.load(":/light_image/zhixing-black.png");
  677. img3 = img3.scaled(66,66,Qt::IgnoreAspectRatio);
  678. ui->label_35->setPixmap(QPixmap::fromImage(img3));
  679. break;
  680. case 1:
  681. img3.load(":/light_image/zhixing-green.png");
  682. img3 = img3.scaled(66,66,Qt::IgnoreAspectRatio);
  683. ui->label_35->setPixmap(QPixmap::fromImage(img3));
  684. break;
  685. case 2:
  686. img3.load(":/light_image/zhixing-red.png");
  687. img3 = img3.scaled(66,66,Qt::IgnoreAspectRatio);
  688. ui->label_35->setPixmap(QPixmap::fromImage(img3));
  689. break;
  690. case 3:
  691. img3.load(":/light_image/zhixing-yellow.png");
  692. img3 = img3.scaled(66,66,Qt::IgnoreAspectRatio);
  693. ui->label_35->setPixmap(QPixmap::fromImage(img3));
  694. break;
  695. default:
  696. img3.load(":/light_image/zhixing-black.png");
  697. img3 = img3.scaled(66,66,Qt::IgnoreAspectRatio);
  698. ui->label_35->setPixmap(QPixmap::fromImage(img3));
  699. break;
  700. }
  701. }
  702. else
  703. {
  704. img3.load(":/light_image/zhixing-black.png");
  705. img3 = img3.scaled(66,66,Qt::IgnoreAspectRatio);
  706. ui->label_35->setPixmap(QPixmap::fromImage(img3));
  707. ui->lcdStraight->display(0);
  708. }
  709. if(mobuEn)
  710. {
  711. if(mbTrafficInfoEn)
  712. {
  713. if(miobuSt == 1)
  714. {
  715. QString eventType;
  716. switch(miBroadcastTrafficType){
  717. case 1:
  718. eventType = "塌方";
  719. break;
  720. case 2:
  721. eventType = "道路施工";
  722. break;
  723. case 3:
  724. eventType = "道路结冰";
  725. break;
  726. case 4:
  727. eventType = "前方限速";
  728. break;
  729. default:
  730. eventType = "";
  731. break;
  732. }
  733. ui->button_trafficInfoLight_st->setStyleSheet("background-color: green");
  734. ui->textBr_trafficInfo->append(QTime::currentTime().toString("hh:mm:ss.zzz") + \
  735. "\n\t事件范围[ 经度:" + QString::number(miBroadcastGpsLon,'g',6) +\
  736. " 纬度:" + QString::number(miBroadcastGpsLat,'g',6) + \
  737. " ]\n\t辐射范围: " + QString::number(miBroadcastRange) + "米" + \
  738. "\n\t事件类型: " + eventType);
  739. switch(micarSt){
  740. case 0:
  741. ui->lineEd_trafficInfo->setText("正常行驶");
  742. break;
  743. case 1:
  744. ui->lineEd_trafficInfo->setText("减速至 " + QString::number(micarSpeedDis,'g',1) + "km/h");
  745. break;
  746. case 2:
  747. ui->lineEd_trafficInfo->setText("停车");
  748. break;
  749. default:
  750. break;
  751. }
  752. }
  753. else if(miobuSt == 10)
  754. {
  755. ui->button_trafficInfoLight_st->setStyleSheet("background-color: red");
  756. ui->textBr_trafficInfo->append(QTime::currentTime().toString("hh:mm:ss.zzz") +"连接丢失");
  757. ui->lineEd_trafficInfo->clear();
  758. miobuSt++;
  759. }
  760. }
  761. else if(mbFCWEn)//碰撞预警
  762. {
  763. if(miobuSt == 1)
  764. {
  765. switch(miWarningType){
  766. case 1:
  767. ui->textBr_FCW->append(QTime::currentTime().toString("hh:mm:ss.zzz") + \
  768. "前方碰撞预警,减速至:" + QString::number(miWarningSpeedLimit));
  769. break;
  770. case 2:
  771. ui->textBr_FCW->append(QTime::currentTime().toString("hh:mm:ss.zzz") + \
  772. "前方碰撞预警,停车");
  773. break;
  774. default:
  775. break;
  776. }
  777. ui->button_FWCLight_st->setStyleSheet("background-color: green");
  778. }
  779. else if(miobuSt == 10)
  780. {
  781. ui->textBr_FCW->append(QTime::currentTime().toString("hh:mm:ss.zzz") +"连接丢失");
  782. ui->button_FWCLight_st->setStyleSheet("background-color: red");
  783. miobuSt++;
  784. }
  785. }
  786. else if(mbjamsEn)//交通拥堵
  787. {
  788. if(miobuSt == 1)
  789. {
  790. ui->button_jamsLight_st->setStyleSheet("background-color: green");
  791. ui->lineEdit_jamsMode->setText("开启");
  792. ui->lineEdit_jamsVIN->setText(QString::fromStdString(gstrvin));
  793. }
  794. else if(miobuSt == 10)
  795. {
  796. ui->button_jamsLight_st->setStyleSheet("background-color: red");
  797. ui->lineEdit_jamsMode->clear();
  798. ui->lineEdit_jamsVIN->clear();
  799. miobuSt++;
  800. }
  801. }
  802. else if(mbdriCrimsEn)//危险驾驶
  803. {
  804. if(miobuSt == 1)
  805. {
  806. ui->button_DriCrimsLight_st->setStyleSheet("background-color: green");
  807. ui->textBr_FCW_2->append(QTime::currentTime().toString("hh:mm:ss.zzz")+"进入危险驾驶模式");
  808. }
  809. else if(miobuSt == 10)
  810. {
  811. ui->button_DriCrimsLight_st->setStyleSheet("background-color: red");
  812. ui->textBr_FCW_2->append(QTime::currentTime().toString("hh:mm:ss.zzz") +"连接丢失");
  813. miobuSt++;
  814. }
  815. }
  816. }
  817. }
  818. /**
  819. * @brief ADCIntelligentVehicle::on_pb_open_can_card_clicked
  820. */
  821. //void ADCIntelligentVehicle::on_pb_open_can_card_clicked() {
  822. //}
  823. /**
  824. * @brief ADCIntelligentVehicle::on_timer_car_status_time_out
  825. */
  826. //void ADCIntelligentVehicle::on_timer_car_status_time_out() {
  827. // ui->lb_current_speed_big->setText(QString::number(ServiceCarStatus.speed));
  828. // //ui->sb_accelerate_percent->setValue(ServiceControlStatus.accelerate_percent);
  829. // //ui->sb_wheel_percent->setValue(ServiceControlStatus.wheel_angle);
  830. //}
  831. /**
  832. * @brief ADCIntelligentVehicle::keyPressEvent 加/减键进行缩放
  833. * @param event
  834. */
  835. void ADCIntelligentVehicle::keyPressEvent(QKeyEvent *event)
  836. {
  837. switch (event->key()) {
  838. case Qt::Key_Plus: // 放大
  839. myview->zoomIn();
  840. break;
  841. case Qt::Key_Minus: // 缩小
  842. myview->zoomOut();
  843. break;
  844. default:
  845. QMainWindow::keyPressEvent(event);
  846. }
  847. }
  848. /**
  849. * @brief ADCIntelligentVehicle::closeEvent
  850. * @param event
  851. */
  852. void ADCIntelligentVehicle::closeEvent(QCloseEvent *event)
  853. {
  854. QMessageBox::StandardButton button;
  855. button=QMessageBox::question(this,tr("退出程序"),QString(tr("确认退出程序")),QMessageBox::Yes|QMessageBox::No);
  856. if(button==QMessageBox::No)
  857. {
  858. event->ignore(); // 忽略退出信号,程序继续进行
  859. }
  860. else if(button==QMessageBox::Yes)
  861. {
  862. event->accept(); // 接受退出信号,程序退出
  863. }
  864. }
  865. /**
  866. * @brief MyView::wheelEvent 放大/缩小
  867. * @param event
  868. */
  869. void MyView::wheelEvent(QWheelEvent *event)
  870. {
  871. // 滚轮的滚动量
  872. // QPoint scrollAmount = event->angleDelta();
  873. // // 正值表示滚轮远离使用者(放大),负值表示朝向使用者(缩小)
  874. // scrollAmount.y() > 0 ? zoomIn() : zoomOut();
  875. }
  876. /**
  877. * @brief MyView::zoomIn 放大
  878. */
  879. void MyView::zoomIn()
  880. {
  881. scale(1.1, 1.1);
  882. beishu *= 1.1;
  883. centerOn(450, 700 - (200 / beishu));
  884. }
  885. /**
  886. * @brief MyView::zoomOut 缩小
  887. */
  888. void MyView::zoomOut()
  889. {
  890. scale(1 / 1.1, 1 / 1.1);
  891. beishu /= 1.1;
  892. centerOn(450, 700 - (200 / beishu));
  893. }
  894. /**
  895. * @brief ADCIntelligentVehicle::paintEvent 刷新
  896. */
  897. void ADCIntelligentVehicle::paintEvent(QPaintEvent *)
  898. {
  899. if (is_show_enable)
  900. {
  901. // qDebug("enter paint. time1 is %d",mTime.elapsed());
  902. if(fabs(mTime.elapsed() - mnTimeLastUpdatePaint)<100)
  903. {
  904. return;
  905. }
  906. mnTimeLastUpdatePaint = mTime.elapsed();
  907. // qDebug("enter paint. time is %d",mTime.elapsed());
  908. painter->begin(image);
  909. // painter_small->begin(image_small);
  910. // image->fill(QColor(60, 60, 60));//对画布进行填充
  911. image->fill(QColor(220, 220, 220));//对画布进行填充
  912. // image_small->fill(QColor(220,220,220));
  913. std::vector<iv::GPSData> navigation_data;
  914. mMutexNavi.lock();
  915. navigation_data = m_navigation_data;
  916. mMutexNavi.unlock();
  917. std::vector<iv::TracePoint> myplan,myplan_left,myplan_right;
  918. mMutexPlan.lock();
  919. myplan = m_plan;
  920. mMutexPlan.unlock();
  921. mMutexPlan_left.lock();
  922. myplan_left = m_plan_left;
  923. mMutexPlan_left.unlock();
  924. mMutexPlan_right.lock();
  925. myplan_right = m_plan_right;
  926. mMutexPlan_right.unlock();
  927. // std::cout<<"plan size is "<<myplan.size()<<std::endl;
  928. // std::vector<iv::GPSData> navigation_data = brain->navigation_data;
  929. painter->setRenderHint(QPainter::Antialiasing, true);//设置反锯齿模式,好看一点
  930. // painter_small->setRenderHint(QPainter::Antialiasing, true);//设置反锯齿模式,好看一点
  931. int pointx = 450, pointy = 700;//确定坐标轴起点坐标,这里定义(35,280)
  932. // int pointx_small = 450, pointy_small = 700;
  933. // double x0[22000], y0[22000], lng[22000], x1[22000], y1[22000], x2[22000], y2[22000];
  934. double * x0, * y0, * lng, * x1, * y1, * x2, * y2;
  935. std::shared_ptr<double> ptrx0,ptry0,ptrlng,ptrx1,ptry1,ptrx2,ptry2;
  936. int nmapsize = navigation_data.size();
  937. x0 = new double[nmapsize];
  938. y0 = new double[nmapsize];
  939. lng = new double[nmapsize];
  940. x1 = new double[nmapsize];
  941. y1 = new double[nmapsize];
  942. x2 = new double[nmapsize];
  943. y2 = new double[nmapsize];
  944. ptrx0.reset(x0);
  945. ptry0.reset(y0);
  946. ptrlng.reset(lng);
  947. ptrx1.reset(x1);
  948. ptry1.reset(y1);
  949. ptrx2.reset(x2);
  950. ptry2.reset(y2);
  951. double xx = 0, yy = 0;
  952. double sumx = 0, sumy = 0;//, ave_x = 0, ave_y = 0;
  953. int sizeN = navigation_data.size();
  954. //int max_x_pos = 0, min_x_pos = 0, max_y_pos = 0, min_y_pos = 0;
  955. int x_max = 0, y_max = 0;//数组里的最大值
  956. int x_min = 0x3f3f3f3f, y_min = 0x3f3f3f3f;//inf为 #define inf 0x3f3f3f3f
  957. double k1, k2;
  958. QPen pen, penPoint;
  959. //先绘制车位置点及框图
  960. static const QPointF points1[2] = { QPointF(300, 700), QPointF(600, 700) };
  961. static const QPointF points2[2] = { QPointF(450, 0), QPointF(450, 900) };
  962. // static const QPointF points1_small[2] = { QPointF(300, 700), QPointF(600, 700) };
  963. // static const QPointF points2_small[2] = { QPointF(450, 0), QPointF(450, 900) };
  964. penPoint.setColor(Qt::red);
  965. penPoint.setWidth(2);
  966. painter->setPen(penPoint);
  967. // painter_small->setPen(penPoint);
  968. painter->drawPoint(pointx, pointy);
  969. // painter_small->drawPoint(pointx_small,pointy_small);
  970. painter->drawPolyline(points1, 2);
  971. painter->drawPolyline(points2, 2);
  972. // painter_small->drawPolyline(points1_small, 2);
  973. // painter_small->drawPolyline(points2_small, 2);
  974. //路径点的预处理
  975. for (int i = 0; i < sizeN; i++)
  976. {
  977. x0[i] = navigation_data[i]->gps_x;
  978. y0[i] = navigation_data[i]->gps_y;
  979. lng[i] = navigation_data[i]->ins_heading_angle;
  980. sumx = sumx + navigation_data[i]->gps_x;
  981. sumy = sumy + navigation_data[i]->gps_y;
  982. if (x0[i] > x_max) {
  983. x_max = x0[i];
  984. //max_x_pos = i;
  985. }
  986. if (x0[i] < x_min) {
  987. x_min = x0[i];
  988. //min_x_pos = i;
  989. }
  990. if (y0[i] > y_max) {
  991. y_max = y0[i];
  992. //max_y_pos = i;
  993. }
  994. if (y0[i] < y_min) {
  995. y_min = y0[i];
  996. //min_y_pos = i;
  997. }
  998. }
  999. //ave_x = sumx / sizeN;
  1000. //ave_y = sumy / sizeN;
  1001. //获取到实时 GPS信息,并做路径点的显示更新
  1002. if (ServiceCarStatus.location->gps_x == 0)
  1003. {
  1004. painter->drawText(rect(), Qt::AlignLeft, QString::fromLocal8Bit("等待车辆实时GPS位置信息"));
  1005. // painter_small->drawText(rect(), Qt::AlignLeft, QString::fromLocal8Bit("等待车辆实时GPS位置信息"));
  1006. }
  1007. else
  1008. {
  1009. x0[0] = ServiceCarStatus.location->gps_x;
  1010. y0[0] = ServiceCarStatus.location->gps_y;
  1011. // std::cout<<"x = "<<x0[0]<<" y= "<<y0[0]<<std::endl;
  1012. lng[0] = ServiceCarStatus.location->ins_heading_angle;
  1013. }
  1014. //根据标定原点的选取,对路径点进行转化
  1015. for (int i = 1; i < sizeN; i++)
  1016. {
  1017. // if(i == 1)std::cout<<" x1 = "<<x0[i]<<std::endl;
  1018. x0[i] = x0[i] - x0[0];
  1019. y0[i] = y0[i] - y0[0];
  1020. xx = x0[i];
  1021. yy = y0[i];
  1022. x0[i] = xx * cos(lng[0] * PI / 180) - yy * sin(lng[0] * PI / 180);
  1023. y0[i] = xx * sin(lng[0] * PI / 180) + yy * cos(lng[0] * PI / 180);
  1024. k1 = sin((90 + (lng[i] - lng[0])) * PI / 180);
  1025. k2 = cos((90 + (lng[i] - lng[0])) * PI / 180);
  1026. #if 0
  1027. x1[i] = x0[i] + k1 * 5;
  1028. y1[i] = y0[i] + k2 * 5;
  1029. x2[i] = x0[i] - k1 * 5;
  1030. y2[i] = y0[i] - k2 * 5;
  1031. #else
  1032. x1[i] = x0[i] + k1 * 1.75;
  1033. y1[i] = y0[i] + k2 * 1.75;
  1034. x2[i] = x0[i] - k1 * 1.75;
  1035. y2[i] = y0[i] - k2 * 1.75;
  1036. #endif
  1037. }
  1038. // double kx_small = (double)(1200) / (abs(y_max - y_min));//x轴的系数
  1039. // double ky_small = (double)(1200) / (abs(y_max - y_min));//y方向的比例系数
  1040. double kx = 10;
  1041. double ky = 10;
  1042. //距离车正前方10m处画一条线
  1043. static const QPointF points3[2] = { QPointF(0, 700 - 10 * ky), QPointF(900, 700 - 10 * ky) };
  1044. painter->drawPolyline(points3, 2);
  1045. //绘制路径点
  1046. penPoint.setColor(Qt::black);
  1047. penPoint.setWidth(1);
  1048. for (int i = 1; i < sizeN - 1; i++)
  1049. {
  1050. painter->setPen(penPoint);//蓝色的笔,用于标记各个点
  1051. // painter_small->setPen(penPoint);//蓝色的笔,用于标记各个点
  1052. painter->drawPoint(pointx + x0[i] * kx, pointy - y0[i] * ky);
  1053. // painter_small->drawPoint(pointx_small + x0[i] * kx_small, pointy_small - y0[i] * ky_small);
  1054. painter->drawPoint(pointx + x1[i] * kx, pointy - y1[i] * ky);
  1055. // painter_small->drawPoint(pointx_small + x1[i] * kx_small, pointy_small - y1[i] * ky_small);
  1056. painter->drawPoint(pointx + x2[i] * kx, pointy - y2[i] * ky);
  1057. // painter_small->drawPoint(pointx_small + x2[i] * kx_small, pointy_small - y2[i] * ky_small);
  1058. }
  1059. painter->drawPoint(pointx + x0[sizeN - 1] * kx, pointy - y0[sizeN - 1] * ky);//绘制最后一个点
  1060. // painter_small->drawPoint(pointx_small + x0[sizeN - 1] * kx_small, pointy_small - y0[sizeN - 1] * ky_small);//绘制最后一个点
  1061. penPoint.setColor(Qt::red);
  1062. penPoint.setWidth(2);
  1063. painter->drawPoint(pointx + x0[0] * kx, pointy - y0[0] * ky);
  1064. // painter_small->drawPoint(pointx_small + x0[0] * kx_small, pointy_small - y0[0] * ky_small);
  1065. #if 1
  1066. // draw plan trace
  1067. penPoint.setColor(QColor(187, 255, 255, 100));
  1068. penPoint.setWidth(2);
  1069. painter->setPen(penPoint);
  1070. QPointF tracePoints[myplan.size()];
  1071. if(myplan.size()>2)
  1072. {
  1073. for(int i=0;i<(myplan.size()-1);i++)
  1074. {
  1075. tracePoints[i].setX((float)(450+myplan[i].x*kx));
  1076. tracePoints[i].setY((float)(700-myplan[i].y*ky));
  1077. painter->drawLine((float)(450+myplan[i].x*kx),(float)(700-myplan[i].y*ky),
  1078. (float)(450+myplan[i+1].x*kx),(float)(700-myplan[i+1].y*ky));
  1079. }
  1080. }
  1081. #endif
  1082. #if 1
  1083. // penPoint.setColor(Qt::gray);
  1084. penPoint.setColor(QColor(187, 255, 255, 100));
  1085. penPoint.setWidth(2);
  1086. painter->setPen(penPoint);
  1087. QPointF tracePoints_left[myplan_left.size()];
  1088. if(myplan_left.size()>2)
  1089. {
  1090. for(int i=0;i<(myplan_left.size()-1);i++)
  1091. {
  1092. tracePoints_left[i].setX((float)(450+myplan_left[i].x*kx));
  1093. tracePoints_left[i].setY((float)(700-myplan_left[i].y*ky));
  1094. painter->drawLine((float)(450+myplan_left[i].x*kx),(float)(700-myplan_left[i].y*ky),
  1095. (float)(450+myplan_left[i+1].x*kx),(float)(700-myplan_left[i+1].y*ky));
  1096. }
  1097. }
  1098. // penPoint.setColor(Qt::gray);
  1099. // penPoint.setWidth(2);
  1100. // painter->setPen(penPoint);
  1101. QPointF tracePoints_right[myplan_right.size()];
  1102. if(myplan_right.size()>2)
  1103. {
  1104. for(int i=0;i<(myplan_right.size()-1);i++)
  1105. {
  1106. tracePoints_right[i].setX((float)(450+myplan_right[i].x*kx));
  1107. tracePoints_right[i].setY((float)(700-myplan_right[i].y*ky));
  1108. painter->drawLine((float)(450+myplan_right[i].x*kx),(float)(700-myplan_right[i].y*ky),
  1109. (float)(450+myplan_right[i+1].x*kx),(float)(700-myplan_right[i+1].y*ky));
  1110. }
  1111. }
  1112. #endif
  1113. // painter->drawPolyline(tracePoints,myplan.size());
  1114. // draw plan trace end
  1115. /////////////////////////////////////apollo add car icon 20200409
  1116. QPixmap pix;
  1117. //pix.load("car.png");
  1118. pix.load(":/ADCIntelligentVehicle/car1.png");
  1119. painter->drawPixmap(435,667,30,67,pix);
  1120. // painter_small->drawPixmap(442,683,16,34,pix);
  1121. ///////////////////////////////////////////////////////////////////
  1122. penPoint.setColor(Qt::blue);
  1123. penPoint.setWidth(3);
  1124. painter->setPen(penPoint);
  1125. QFont font;
  1126. font.setFamily("Microsoft YaHei");
  1127. font.setPointSize(50);
  1128. font.setItalic(true);
  1129. painter->setFont(font);
  1130. //////////////////////////////////////
  1131. // //
  1132. // 显示毫米波雷达和激光雷达的信息 //
  1133. // //
  1134. //////////////////////////////////////
  1135. if(ui->checkBox_lidar_show->isChecked())
  1136. {
  1137. ServiceLidar.copylidarto(Lidar_read); //激光雷达障碍物
  1138. ServiceLidar.copylidarobsto(Lidar_obsread);
  1139. painter->setPen(QColor(255, 0, 0));
  1140. for (unsigned int x = 0; x < Lidar_read->size(); x++)
  1141. {
  1142. //painter->drawPoint(((*Lidar_read)[x].nomal_x) * 10 + 450, -(*Lidar_read)[x].nomal_y * 10 + 700);
  1143. }
  1144. painter->setPen(QColor(255,0,0));
  1145. for (unsigned int x = 0; x < Lidar_obsread->size(); x++)
  1146. {
  1147. painter->drawPoint(((*Lidar_obsread)[x].nomal_x) * 10 + 450, -((*Lidar_obsread)[x].nomal_y + ServiceCarStatus.lidar_y_offset) * 10 + 700);
  1148. }
  1149. }
  1150. //////////////////////////////////////
  1151. // //
  1152. // 显示融合的信息 //
  1153. // //
  1154. //////////////////////////////////////
  1155. if(ui->checkBox_fusion_show->isChecked())
  1156. {
  1157. painter->setPen(QColor(255,0,0));
  1158. iv::fusion::fusionobjectarray xfusionarray;
  1159. if(mbfusionUpdate)
  1160. {
  1161. mMutexFusion.lock();
  1162. xfusionarray.CopyFrom(mfusionarray);
  1163. mMutexFusion.unlock();
  1164. for(int a = 0; a < xfusionarray.obj_size(); a++)
  1165. {
  1166. for(int b = 0; b < xfusionarray.obj(a).nomal_centroid_size(); b++)
  1167. {
  1168. painter->drawPoint((xfusionarray.obj(a).nomal_centroid(b).nomal_x())*10 + 450, -(xfusionarray.obj(a).nomal_centroid(b).nomal_y() + ServiceCarStatus.lidar_y_offset)*10 + 700);
  1169. }
  1170. }
  1171. }
  1172. }
  1173. painter->end();
  1174. // painter_small->end();
  1175. scene->clear();
  1176. // scene_small->clear();
  1177. scene->addPixmap(QPixmap::fromImage(*image));
  1178. // scene_small->addPixmap(QPixmap::fromImage(*image_small));
  1179. myview->setScene(scene);
  1180. // myview_small->setScene(scene_small);
  1181. myview->show();
  1182. // myview_small->show();
  1183. navigation_data.clear();
  1184. }
  1185. }
  1186. /**
  1187. * @brief ADCIntelligentVehicle::on_listWidget_clicked
  1188. */
  1189. void ADCIntelligentVehicle::on_listWidget_clicked()
  1190. {
  1191. // int currentRow=ui->listWidget->currentRow(); //currentRow()获取当前行号
  1192. // if(currentRow==0) //点击第一切换到第一页
  1193. // {
  1194. // ui->stackedWidget->setCurrentIndex(0);//为堆栈窗体切换到当前索引
  1195. // }
  1196. // else if(currentRow==1)
  1197. // {
  1198. // ui->stackedWidget->setCurrentIndex(1);
  1199. // }
  1200. // else if(currentRow==2)
  1201. // {
  1202. // ui->stackedWidget->setCurrentIndex(2);
  1203. // }
  1204. // else
  1205. // {
  1206. // ui->stackedWidget->setCurrentIndex(0);
  1207. // }
  1208. }
  1209. /**
  1210. * @brief ADCIntelligentVehicle::UpdatePlanTrace
  1211. * @param strdata
  1212. * @param nSize
  1213. * @param index
  1214. * @param dt
  1215. * @param strmemname
  1216. */
  1217. void ADCIntelligentVehicle::UpdatePlanTrace(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
  1218. {
  1219. mMutexPlan.lock();
  1220. m_plan.clear();
  1221. int nplansize = nSize/sizeof(iv::TracePoint);
  1222. int npsize = sizeof(iv::TracePoint);
  1223. int i;
  1224. for(i=0;i<nplansize;i++)
  1225. {
  1226. iv::TracePoint x;
  1227. memcpy(&x,strdata + i*npsize,npsize);
  1228. m_plan.push_back(x);
  1229. }
  1230. mMutexPlan.unlock();
  1231. }
  1232. void ADCIntelligentVehicle::UpdatePlanTrace_left(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
  1233. {
  1234. mMutexPlan_left.lock();
  1235. m_plan_left.clear();
  1236. int nplansize = nSize/sizeof(iv::TracePoint);
  1237. if(nplansize < 1)
  1238. std::cout<<"UpdatePlanTrace_left size is 0................. "<<std::endl;
  1239. // return;
  1240. int npsize = sizeof(iv::TracePoint);
  1241. int i;
  1242. for(i=0;i<nplansize;i++)
  1243. {
  1244. iv::TracePoint x;
  1245. memcpy(&x,strdata + i*npsize,npsize);
  1246. m_plan_left.push_back(x);
  1247. }
  1248. mMutexPlan_left.unlock();
  1249. }
  1250. void ADCIntelligentVehicle::UpdatePlanTrace_right(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
  1251. {
  1252. mMutexPlan_right.lock();
  1253. m_plan_right.clear();
  1254. int nplansize = nSize/sizeof(iv::TracePoint);
  1255. if(nplansize < 1)
  1256. std::cout<<"UpdatePlanTrace_right size is 0................. "<<std::endl;
  1257. int npsize = sizeof(iv::TracePoint);
  1258. int i;
  1259. for(i=0;i<nplansize;i++)
  1260. {
  1261. iv::TracePoint x;
  1262. memcpy(&x,strdata + i*npsize,npsize);
  1263. m_plan_right.push_back(x);
  1264. }
  1265. mMutexPlan_right.unlock();
  1266. }
  1267. void ADCIntelligentVehicle::UpdateFusion(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
  1268. {
  1269. iv::fusion::fusionobjectarray xfusionarray;
  1270. if(!xfusionarray.ParseFromArray(strdata,nSize))
  1271. {
  1272. gIvlog->warn("ADCIntelligentVehicle::UpdateFusion Parse Error.");
  1273. return;
  1274. }
  1275. mMutexFusion.lock();
  1276. mfusionarray.CopyFrom(xfusionarray);
  1277. mbfusionUpdate = true;
  1278. mMutexFusion.unlock();
  1279. }
  1280. /**
  1281. * @brief ADCIntelligentVehicle::UpdateMap
  1282. * @param strdata
  1283. * @param nSize
  1284. * @param index
  1285. * @param dt
  1286. * @param strmemname
  1287. */
  1288. void ADCIntelligentVehicle::UpdateMap(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
  1289. {
  1290. // std::cout<<"update map "<<std::endl;
  1291. int gpsunitsize = sizeof(iv::GPS_INS);
  1292. int nMapSize = nSize/gpsunitsize;
  1293. // std::cout<<"map size is "<<nMapSize<<std::endl;
  1294. if(nMapSize < 1)return;
  1295. bool bUpdate = false;
  1296. if(nMapSize != m_navigation_data.size())
  1297. {
  1298. bUpdate = true;
  1299. }
  1300. else
  1301. {
  1302. iv::GPS_INS * p = (iv::GPS_INS *)strdata;
  1303. if((p->gps_lat == m_navigation_data.at(0)->gps_lat)&&(p->ins_heading_angle == m_navigation_data.at(0)->ins_heading_angle))
  1304. {
  1305. // qDebug("same map");
  1306. bUpdate = false;
  1307. }
  1308. else
  1309. {
  1310. bUpdate = true;
  1311. }
  1312. }
  1313. if(bUpdate)
  1314. {
  1315. int i;
  1316. mMutexNavi.lock();
  1317. m_navigation_data.clear();
  1318. for(i=0;i<nMapSize;i++)
  1319. {
  1320. iv::GPS_INS x;
  1321. memcpy(&x,strdata + i*gpsunitsize,gpsunitsize);
  1322. iv::GPSData data(new iv::GPS_INS);
  1323. *data = x;
  1324. m_navigation_data.push_back(data);
  1325. }
  1326. mMutexNavi.unlock();
  1327. if(m_navigation_data.size()>0)
  1328. {
  1329. mDataToUI.mInfo.is_mapLoad = 1;
  1330. }
  1331. else
  1332. {
  1333. mDataToUI.mInfo.is_mapLoad = 0;
  1334. }
  1335. }
  1336. else
  1337. {
  1338. // qDebug("not need update");
  1339. }
  1340. }
  1341. //=======================zhaobo0904
  1342. #define PI 3.14159265358979
  1343. /**
  1344. * @brief GaussProjCal
  1345. * @param lon
  1346. * @param lat
  1347. * @param X
  1348. * @param Y
  1349. */
  1350. /*
  1351. void GaussProjCal(double lon, double lat, double *X, double *Y)
  1352. {
  1353. // 1975 年国际椭球体长半轴 a, 第一离心率 e2, 第二离心率 ep2
  1354. double a = 6378140.0;
  1355. double e2 = 0.006694384999588;
  1356. double ep2 = e2/(1-e2);
  1357. // 原点所在经度
  1358. double lon_origin = 6.0*int(lon/6) + 3.0;
  1359. lon_origin *= PI / 180.0;
  1360. double k0 = 0.9996;
  1361. // 角度转弧度
  1362. double lat1 = lat * PI / 180.0;
  1363. double lon1 = lon * PI / 180.0;
  1364. // 经线在该点处的曲率半径,
  1365. double N = a / sqrt(1 - e2*sin(lat1)*sin(lat1));
  1366. // 赤道到该点的经线长度近似值 M, 使用泰勒展开逐项积分然后取前四项.
  1367. // 这个近似值是将 N 作为纬度 \phi 的函数展开为泰勒计数, 然后在区间 [0, lat1] 上积分得到的.
  1368. // 首先计算前四项的系数 a1~a4.
  1369. double a1 = 1 - e2/4 - (3*e2*e2)/64 - (5*e2*e2*e2)/256;
  1370. double a2 = (3*e2)/8 + (3*e2*e2)/32 + (45*e2*e2*e2)/1024;
  1371. double a3 = (15*e2*e2)/256 + (45*e2*e2*e2)/1024;
  1372. double a4 = (35*e2*e2*e2)/3072;
  1373. double M = a * (a1*lat1 - a2*sin(2*lat1) + a3*sin(4*lat1) - a4*sin(6*lat1));
  1374. // 辅助量
  1375. double T = tan(lat1)*tan(lat1);
  1376. double C = ep2*cos(lat1)*cos(lat1);
  1377. double A = (lon1 - lon_origin)*cos(lat1);
  1378. *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);
  1379. *Y = M + N * tan(lat1) * ((A*A)/2 +
  1380. (5 - T + 9*C + 4*C*C)*(A*A*A*A)/24 +
  1381. (61 - 58*T + T*T + 600*C - 330*ep2)*(A*A*A*A*A*A)/720);
  1382. *Y *= k0;
  1383. return;
  1384. }
  1385. */
  1386. //高斯投影由经纬度(Unit:DD)反算大地坐标(含带号,Unit:Metres)
  1387. void GaussProjCal(double longitude, double latitude, double *X, double *Y)
  1388. {
  1389. int ProjNo = 0; int ZoneWide; ////带宽
  1390. double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
  1391. double a, f, e2, ee, NN, T, C, A, M, iPI;
  1392. iPI = 0.0174532925199433; ////3.1415926535898/180.0;
  1393. ZoneWide = 6; ////6度带宽
  1394. a = 6378245.0; f = 1.0 / 298.3; //54年北京坐标系参数
  1395. ////a=6378140.0; f=1/298.257; //80年西安坐标系参数
  1396. ProjNo = (int)(longitude / ZoneWide);
  1397. longitude0 = ProjNo * ZoneWide + ZoneWide / 2;
  1398. longitude0 = longitude0 * iPI;
  1399. latitude0 = 0;
  1400. longitude1 = longitude * iPI; //经度转换为弧度
  1401. latitude1 = latitude * iPI; //纬度转换为弧度
  1402. e2 = 2 * f - f * f;
  1403. ee = e2 * (1.0 - e2);
  1404. NN = a / sqrt(1.0 - e2 * sin(latitude1)*sin(latitude1));
  1405. T = tan(latitude1)*tan(latitude1);
  1406. C = ee * cos(latitude1)*cos(latitude1);
  1407. A = (longitude1 - longitude0)*cos(latitude1);
  1408. 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
  1409. *e2 / 1024)*sin(2 * latitude1)
  1410. + (15 * e2*e2 / 256 + 45 * e2*e2*e2 / 1024)*sin(4 * latitude1) - (35 * e2*e2*e2 / 3072)*sin(6 * latitude1));
  1411. 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);
  1412. yval = M + NN * tan(latitude1)*(A*A / 2 + (5 - T + 9 * C + 4 * C*C)*A*A*A*A / 24
  1413. + (61 - 58 * T + T * T + 600 * C - 330 * ee)*A*A*A*A*A*A / 720);
  1414. X0 = 1000000L * (ProjNo + 1) + 500000L;
  1415. Y0 = 0;
  1416. xval = xval + X0; yval = yval + Y0;
  1417. *X = xval;
  1418. *Y = yval;
  1419. }
  1420. //高斯投影由大地坐标(Unit:Metres)反算经纬度(Unit:DD)
  1421. void GaussProjInvCal(double X, double Y, double *longitude, double *latitude)
  1422. {
  1423. int ProjNo; int ZoneWide; ////带宽
  1424. double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
  1425. double e1, e2, f, a, ee, NN, T, C, M, D, R, u, fai, iPI;
  1426. iPI = 0.0174532925199433; ////3.1415926535898/180.0;
  1427. a = 6378245.0; f = 1.0 / 298.3; //54年北京坐标系参数
  1428. ////a=6378140.0; f=1/298.257; //80年西安坐标系参数
  1429. ZoneWide = 6; ////6度带宽
  1430. ProjNo = (int)(X / 1000000L); //查找带号
  1431. longitude0 = (ProjNo - 1) * ZoneWide + ZoneWide / 2;
  1432. longitude0 = longitude0 * iPI; //中央经线
  1433. X0 = ProjNo * 1000000L + 500000L;
  1434. Y0 = 0;
  1435. xval = X - X0; yval = Y - Y0; //带内大地坐标
  1436. e2 = 2 * f - f * f;
  1437. e1 = (1.0 - sqrt(1 - e2)) / (1.0 + sqrt(1 - e2));
  1438. ee = e2 / (1 - e2);
  1439. M = yval;
  1440. u = M / (a*(1 - e2 / 4 - 3 * e2*e2 / 64 - 5 * e2*e2*e2 / 256));
  1441. fai = u + (3 * e1 / 2 - 27 * e1*e1*e1 / 32)*sin(2 * u) + (21 * e1*e1 / 16 - 55 * e1*e1*e1*e1 / 32)*sin(
  1442. 4 * u)
  1443. + (151 * e1*e1*e1 / 96)*sin(6 * u) + (1097 * e1*e1*e1*e1 / 512)*sin(8 * u);
  1444. C = ee * cos(fai)*cos(fai);
  1445. T = tan(fai)*tan(fai);
  1446. NN = a / sqrt(1.0 - e2 * sin(fai)*sin(fai));
  1447. R = a * (1 - e2) / sqrt((1 - e2 * sin(fai)*sin(fai))*(1 - e2 * sin(fai)*sin(fai))*(1 - e2 * sin
  1448. (fai)*sin(fai)));
  1449. D = xval / NN;
  1450. //计算经度(Longitude) 纬度(Latitude)
  1451. 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
  1452. *D*D*D*D / 120) / cos(fai);
  1453. latitude1 = fai - (NN*tan(fai) / R)*(D*D / 2 - (5 + 3 * T + 10 * C - 4 * C*C - 9 * ee)*D*D*D*D / 24
  1454. + (61 + 90 * T + 298 * C + 45 * T*T - 256 * ee - 3 * C*C)*D*D*D*D*D*D / 720);
  1455. //转换为度 DD
  1456. *longitude = longitude1 / iPI;
  1457. *latitude = latitude1 / iPI;
  1458. }
  1459. //==========================================================
  1460. /**
  1461. * @brief ADCIntelligentVehicle::UpdateGPSIMU
  1462. * @param strdata
  1463. * @param nSize
  1464. * @param index
  1465. * @param dt
  1466. * @param strmemname
  1467. */
  1468. void ADCIntelligentVehicle::UpdateGPSIMU(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
  1469. {
  1470. iv::gps::gpsimu xgpsimu;
  1471. if(!xgpsimu.ParseFromArray(strdata,nSize))
  1472. {
  1473. gIvlog->warn("ADCIntelligentVehicle::UpdateGPSIMU parse error. nSize is %d",nSize);
  1474. return;
  1475. }
  1476. iv::GPSData data(new iv::GPS_INS);
  1477. data->gps_lat = xgpsimu.lat();
  1478. data->gps_lng = xgpsimu.lon();
  1479. data->ins_heading_angle = xgpsimu.heading();
  1480. data->rtk_status = xgpsimu.rtk_state();
  1481. data->ins_status = xgpsimu.ins_state();
  1482. data->vel_D = xgpsimu.vd(); //地向速度,单位(米/秒)
  1483. data->vel_E = xgpsimu.ve(); //东向速度,单位(米/秒)
  1484. data->vel_N = xgpsimu.vn(); //北向速度,单位(米/秒)
  1485. GaussProjCal(data->gps_lng,data->gps_lat,&data->gps_x,&data->gps_y);
  1486. ServiceCarStatus.mRTKStatus = data->rtk_status;
  1487. ServiceCarStatus.speed = data->speed;
  1488. ServiceCarStatus.location->gps_lat = data->gps_lat;
  1489. ServiceCarStatus.location->gps_lng = data->gps_lng;
  1490. ServiceCarStatus.location->ins_heading_angle = data->ins_heading_angle;
  1491. ServiceCarStatus.location->gps_x = data->gps_x;
  1492. ServiceCarStatus.location->gps_y = data->gps_y;
  1493. ServiceCarStatus.location->rtk_status = data->rtk_status;
  1494. ServiceCarStatus.location->ins_status = data->ins_status;
  1495. ServiceCarStatus.speed = sqrt(pow(data->vel_E,2)+pow(data->vel_N,2)) * 3.6; //double pow(double x, double y) 返回 x 的 y 次幂,即 xy。
  1496. mnTimeUpdateGPS = mTimeState.elapsed();
  1497. }
  1498. /**
  1499. * @brief ADCIntelligentVehicle::UpdateRADAR
  1500. * @param strdata
  1501. * @param nSize
  1502. * @param index
  1503. * @param dt
  1504. * @param strmemname
  1505. */
  1506. void ADCIntelligentVehicle::UpdateRADAR(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
  1507. {
  1508. static qint64 oldrecvtime;
  1509. iv::radar::radarobjectarray xradararray;
  1510. if(!xradararray.ParseFromArray(strdata,nSize))
  1511. {
  1512. gIvlog->warn("ADCIntelligentVehicle::UpdateRADAR Parse Error.");
  1513. return;
  1514. }
  1515. // gIvlog->verbose("radar time is %ld",QDateTime::currentMSecsSinceEpoch());
  1516. if((QDateTime::currentMSecsSinceEpoch() - oldrecvtime)>100)
  1517. {
  1518. gIvlog->warn("radar interval is more than 100ms. value is %ld",QDateTime::currentMSecsSinceEpoch() - oldrecvtime);
  1519. }
  1520. oldrecvtime = QDateTime::currentMSecsSinceEpoch();
  1521. mMutexRadar.lock();
  1522. mradararray.CopyFrom(xradararray);
  1523. mbradarUpdate = true;
  1524. mMutexRadar.unlock();
  1525. // int i;
  1526. // for(i=0;i<64;i++)
  1527. // {
  1528. // iv::ObstacleBasic x;
  1529. // memcpy(&x,pdata + i*sizeof(iv::ObstacleBasic),sizeof(iv::ObstacleBasic));
  1530. // ServiceCarStatus.obs_radar[i] = x;
  1531. // }
  1532. ServiceCarStatus.mRadarS = 10;
  1533. // mnTimeUpdateRadar = mTimeState.elapsed();
  1534. // mnTimeUpdateCANState = mTimeState.elapsed();
  1535. }
  1536. /**
  1537. * @brief ADCIntelligentVehicle::UpdateCANState
  1538. * @param pdata
  1539. * @param ndatasize
  1540. */
  1541. void ADCIntelligentVehicle::UpdateCANState(const char *pdata, const int ndatasize)
  1542. {
  1543. iv::canstate::canstate recCanstate;
  1544. if(!recCanstate.ParseFromArray(pdata, ndatasize))
  1545. {
  1546. gIvlog->warn("ADCIntelligentVehicle::updateCanstate error");
  1547. return;
  1548. }
  1549. // if(ndatasize<sizeof(iv::can_state))return;
  1550. // char ccanstate[ndatasize];
  1551. // memcpy(ccanstate,pdata,4);
  1552. #if 1
  1553. if(recCanstate.b_canstate())
  1554. {
  1555. // gIvlog->debug("UpdateCANState<success>");
  1556. mDataToUI.mInfo.is_initSuccess = 1;
  1557. }
  1558. else
  1559. {
  1560. // gIvlog->debug("UpdateCANState<failed>size %d %s ");
  1561. mDataToUI.mInfo.is_initSuccess = 0;
  1562. }
  1563. #endif
  1564. // iv::can_state x;
  1565. // memcpy(&x,pdata,sizeof(iv::can_state));
  1566. // if(x.mbCANOpen)
  1567. // {
  1568. // mDataToUI.mInfo.is_initSuccess = 1;
  1569. // }
  1570. // else
  1571. // {
  1572. // mDataToUI.mInfo.is_initSuccess = 0;
  1573. // }
  1574. }
  1575. /**
  1576. * @brief ADCIntelligentVehicle::UpdateDecition
  1577. * @param strdata
  1578. * @param nSize
  1579. * @param index
  1580. * @param dt
  1581. * @param strmemname
  1582. */
  1583. void ADCIntelligentVehicle::UpdateDecition(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
  1584. {
  1585. iv::brain::decition xdecition;
  1586. if(!xdecition.ParseFromArray(strdata,nSize))
  1587. {
  1588. gIvlog->warn("ADCIntelligentVehicle::UpdateDecition parse errror. nSize is %d",nSize);
  1589. return;
  1590. }
  1591. //ServiceCarStatus.mfAcc = xdecition.accelerator(); //
  1592. ServiceCarStatus.mfAcc = xdecition.torque(); //
  1593. ServiceCarStatus.mfBrake = xdecition.brake();
  1594. ServiceCarStatus.mfWheel = xdecition.wheelangle();
  1595. ServiceCarStatus.mfttc = xdecition.ttc();
  1596. mnTimeUpdateDecition = mTimeState.elapsed();
  1597. }
  1598. /**
  1599. * @brief ADCIntelligentVehicle::UpdateVehicleState
  1600. * @param strdata
  1601. * @param nSize
  1602. * @param index
  1603. * @param dt
  1604. * @param strmemname
  1605. */
  1606. void ADCIntelligentVehicle::UpdateVehicleState(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
  1607. {
  1608. iv::brain::brainstate xbrainstate;
  1609. if(!xbrainstate.ParseFromArray(strdata,nSize))
  1610. {
  1611. gIvlog->warn("ADCIntelligentVehicle::UpdateVehicleState parse error,nSize is %d ",nSize);
  1612. return;
  1613. }
  1614. if(xbrainstate.has_mbbocheenable())
  1615. ServiceCarStatus.bocheEnable = xbrainstate.mbbocheenable();
  1616. if(xbrainstate.has_mfobs())
  1617. ServiceCarStatus.mObs = xbrainstate.mfobs();
  1618. if(xbrainstate.has_mflidarobs())
  1619. ServiceCarStatus.mLidarObs = xbrainstate.mflidarobs();
  1620. if(xbrainstate.has_mfradarobs())
  1621. ServiceCarStatus.mRadarObs = xbrainstate.mfradarobs();
  1622. if(xbrainstate.has_mbbrainrunning())
  1623. mbBrainRunning = xbrainstate.mbbrainrunning();
  1624. if(xbrainstate.has_mbbrainrunning())
  1625. ServiceCarStatus.mbBrainCtring = xbrainstate.mbbrainrunning();
  1626. if(xbrainstate.has_decision_period())
  1627. mdecition_period = xbrainstate.decision_period();
  1628. if(xbrainstate.has_vehicle_state())
  1629. micarSt = xbrainstate.vehicle_state();
  1630. if(xbrainstate.has_vehicle_spd())
  1631. micarSpeedDis = xbrainstate.vehicle_spd();
  1632. mnTimeUpdateVS = mTimeState.elapsed();
  1633. }
  1634. /**
  1635. * @brief ADCIntelligentVehicle::UpdateOBS
  1636. * @param lidar_obs
  1637. */
  1638. void ADCIntelligentVehicle::UpdateOBS(std::shared_ptr<std::vector<iv::ObstacleBasic> > lidar_obs)
  1639. {
  1640. iv::ObsLiDAR _obs_grid_ = boost::shared_ptr<std::vector<iv::ObstacleBasic>>(new std::vector<iv::ObstacleBasic>);
  1641. int i;
  1642. int nSize = lidar_obs->size();
  1643. for(i=0;i<nSize;i++)
  1644. _obs_grid_->push_back(lidar_obs->at(i));
  1645. ServiceLidar.copyfromlidarobs(_obs_grid_);
  1646. ServiceCarStatus.mLidarS = 10;
  1647. }
  1648. void ADCIntelligentVehicle::UpdateV2xStEn(unsigned int enable)
  1649. {
  1650. iv::v2x::v2xStEn x;
  1651. x.set_v2xsten(enable);
  1652. int nsize = x.ByteSize();
  1653. char * str = new char[nsize];
  1654. if(x.SerializeToArray(str,nsize))
  1655. {
  1656. iv::modulecomm::ModuleSendMsg(mp_v2xStSend,str,nsize);
  1657. }
  1658. else
  1659. {
  1660. gIvlog->error("hmi","send enable st error");
  1661. }
  1662. delete str;
  1663. }
  1664. /**
  1665. * @brief ADCIntelligentVehicle::ShareHMIMsg
  1666. * @param xhmi
  1667. */
  1668. void ADCIntelligentVehicle::ShareHMIMsg(iv::hmi::HMIBasic xhmi)
  1669. {
  1670. iv::hmi::hmimsg xhmimsg;
  1671. xhmimsg.set_mbpause(xhmi.mbPause);
  1672. xhmimsg.set_mbbusmode(xhmi.mbbusmode);
  1673. xhmimsg.set_mbbochemode(xhmi.mbBocheMode);
  1674. xhmimsg.set_mbchemen(xhmi.mbchemen);
  1675. xhmimsg.set_mbjinguang(xhmi.mbjinguang);
  1676. xhmimsg.set_mbyuanguang(xhmi.mbyuanguang);
  1677. int nsize = xhmimsg.ByteSize();
  1678. char * str = new char[nsize];
  1679. std::shared_ptr<char> pstr;
  1680. pstr.reset(str); //将就对象的引用计数减1(当然,如果发现引用计数为0时,则析构旧对象),然后将新对象的指针交给智能指针保管。这样做可以省略了delete的过程。
  1681. if(!xhmimsg.SerializeToArray(str,nsize))
  1682. {
  1683. std::cout<<"ADCIntelligentVehicle::ShareHMIMsg serialize error."<<std::endl;
  1684. mpivlog->error("ADCIntelligentVehicle::ShareHMIMsg serialize error.");
  1685. return;
  1686. }
  1687. iv::modulecomm::ModuleSendMsg(mpaHMI,str,nsize);
  1688. }
  1689. /**
  1690. * @brief ADCIntelligentVehicle::ShareHMIMsg
  1691. * @param xhmi
  1692. */
  1693. void ADCIntelligentVehicle::ShareHMIMsgPro(iv::hmi::hmimsg xhmimsg)
  1694. {
  1695. int nsize = xhmimsg.ByteSize();
  1696. char * str = new char[nsize];
  1697. std::shared_ptr<char> pstr;
  1698. pstr.reset(str); //将就对象的引用计数减1(当然,如果发现引用计数为0时,则析构旧对象),然后将新对象的指针交给智能指针保管。这样做可以省略了delete的过程。
  1699. if(!xhmimsg.SerializeToArray(str,nsize))
  1700. {
  1701. std::cout<<"ADCIntelligentVehicle::ShareHMIMsg serialize error."<<std::endl;
  1702. mpivlog->error("ADCIntelligentVehicle::ShareHMIMsg serialize error.");
  1703. return;
  1704. }
  1705. iv::modulecomm::ModuleSendMsg(mpaHMI,str,nsize);
  1706. }
  1707. void ADCIntelligentVehicle::UpdateChassis(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
  1708. {
  1709. iv::chassis xchassis;
  1710. static int ncount = 0;
  1711. if(!xchassis.ParseFromArray(strdata,nSize))
  1712. {
  1713. std::cout<<"iv::decition::BrainDecition::UpdateChassis ParseFrom Array Error."<<std::endl;
  1714. return;
  1715. }
  1716. if((xchassis.has_epsmode())&&(xchassis.epsmode() == 0))
  1717. {
  1718. ServiceCarStatus.mbRunPause = true;
  1719. }
  1720. if((xchassis.has_soc()))
  1721. {
  1722. mfSOC = xchassis.soc();
  1723. ui->lineEdit_soc->setText(QString::number(mfSOC,'f',1));
  1724. }
  1725. unsigned char errSig = 0;
  1726. if((xchassis.has_driver_error()))
  1727. {
  1728. errSig |= xchassis.driver_error();
  1729. }
  1730. if((xchassis.has_swj_error()))
  1731. {
  1732. errSig |= xchassis.swj_error();
  1733. }
  1734. if((xchassis.has_battery_error()))
  1735. {
  1736. errSig |= xchassis.battery_error();
  1737. }
  1738. if((xchassis.has_remote_error()))
  1739. {
  1740. errSig |= xchassis.remote_error();
  1741. }
  1742. if((xchassis.has_steer_motor_error()))
  1743. {
  1744. errSig |= xchassis.steer_motor_error();
  1745. }
  1746. if((xchassis.has_rrmotor_error()))
  1747. {
  1748. errSig |= xchassis.rrmotor_error();
  1749. }
  1750. if((xchassis.has_rlmotor_error()))
  1751. {
  1752. errSig |= xchassis.rlmotor_error();
  1753. }
  1754. if((xchassis.has_fsteer_code_error()))
  1755. {
  1756. errSig |= xchassis.fsteer_code_error();
  1757. }
  1758. ui->lineEdit_errCode->setText(QString::number(errSig,16));
  1759. }
  1760. void ADCIntelligentVehicle::onTimerManual()
  1761. {
  1762. // if(myview->mPressKeys.count() == 0)return;
  1763. if(mbManual == false)
  1764. {
  1765. if(myview->mPressKeys.contains(Qt::Key_J))
  1766. {
  1767. mbManual = true;
  1768. }
  1769. }
  1770. if(mbManual == true)
  1771. {
  1772. if(myview->mPressKeys.contains(Qt::Key_K))
  1773. {
  1774. mbManual = false;
  1775. }
  1776. }
  1777. if(mbManual == false)return;
  1778. int timediff = mManualTime.elapsed() - mnLastManualTime;
  1779. mnLastManualTime = mManualTime.elapsed();
  1780. if(myview->mPressKeys.contains(Qt::Key_W))
  1781. {
  1782. if(mnCtrlValue[0]<1000)mnCtrlValue[0] = mnCtrlValue[0] + 5 * timediff /20;
  1783. }
  1784. if(myview->mPressKeys.contains(Qt::Key_S))
  1785. {
  1786. mnCtrlValue[0] = 0;
  1787. if(mnCtrlValue[1]<990) mnCtrlValue[1] = mnCtrlValue[1] + 10* timediff /20;
  1788. }
  1789. else
  1790. {
  1791. mnCtrlValue[1] = 0;
  1792. }
  1793. if(myview->mPressKeys.contains(Qt::Key_A))
  1794. {
  1795. if(mnCtrlValue[2]<500)mnCtrlValue[2] = mnCtrlValue[2] + 3* timediff /20;
  1796. }
  1797. if(myview->mPressKeys.contains(Qt::Key_D))
  1798. {
  1799. if(mnCtrlValue[2]>-500)mnCtrlValue[2] = mnCtrlValue[2] - 3* timediff /20;
  1800. }
  1801. iv::modulecomm::ModuleSendMsg(mpManualCtrl,(char *)mnCtrlValue,3*sizeof(int));
  1802. }
  1803. void ADCIntelligentVehicle::onCtrlManual(bool bCtrl)
  1804. {
  1805. mbManual = bCtrl;
  1806. if(bCtrl)
  1807. {
  1808. mTimerManual->start(10);
  1809. mManualTime.start();
  1810. mnLastManualTime = mManualTime.elapsed();
  1811. }
  1812. else
  1813. {
  1814. mTimerManual->stop();
  1815. mnCtrlValue[0] = 0; mnCtrlValue[1] = 0; mnCtrlValue[2] = 0;
  1816. }
  1817. }
  1818. void ADCIntelligentVehicle::shareMapReqMsg()
  1819. {
  1820. iv::map::mapreq x;
  1821. x.set_maptype(1);
  1822. int nsize = x.ByteSize();
  1823. char * str = new char[nsize];
  1824. if(x.SerializeToArray(str,nsize))
  1825. {
  1826. iv::modulecomm::ModuleSendMsg(mpamapreq,str,nsize);
  1827. }
  1828. else
  1829. {
  1830. std::cout<<"iv::map::mapreq serialize error."<<std::endl;
  1831. }
  1832. std::cout<<"iv::map::mapreq serialize OK."<<std::endl;
  1833. delete str;
  1834. }
  1835. void ADCIntelligentVehicle::UpdateVbox(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
  1836. {
  1837. iv::vbox::vbox xvbox;
  1838. if(!xvbox.ParseFromArray(strdata,nSize))
  1839. {
  1840. gIvlog->warn("ADCIntelligentVehicle::UpdateVbox parse errror. nSize is %d",nSize);
  1841. return;
  1842. }
  1843. ServiceCarStatus.st_straight = xvbox.st_straight();
  1844. ServiceCarStatus.st_left = xvbox.st_left();
  1845. ServiceCarStatus.st_right = xvbox.st_right();
  1846. ServiceCarStatus.st_turn = xvbox.st_turn();
  1847. ServiceCarStatus.time_straight = xvbox.time_straight();
  1848. ServiceCarStatus.time_left = xvbox.time_left();
  1849. ServiceCarStatus.time_right = xvbox.time_right();
  1850. ServiceCarStatus.time_turn = xvbox.time_turn();
  1851. }
  1852. void ADCIntelligentVehicle::on_button_start_clicked()
  1853. {
  1854. #ifdef USE_PAD_CTRL
  1855. iv::hmi::hmimsg hmi;
  1856. if(mbPause){
  1857. ServiceCarStatus.mbRunPause = false;
  1858. hmi.set_mbpause(false);
  1859. mbPause = false;
  1860. ui->button_start->setText("暂停自动驾驶");
  1861. }else{
  1862. ServiceCarStatus.mbRunPause = true;
  1863. hmi.set_mbpause(true);
  1864. mbPause = true;
  1865. ui->button_start->setText("启动自动驾驶");
  1866. }
  1867. ShareHMIMsgPro(hmi);
  1868. return;
  1869. #endif
  1870. }
  1871. //云平台使能控制
  1872. void ADCIntelligentVehicle::on_button_platform_en_clicked()
  1873. {
  1874. if(mplatformEn){
  1875. ui->button_platform_en->setStyleSheet("background-color: gray");
  1876. mplatformEn = 0;
  1877. }
  1878. else{
  1879. ui->button_platform_en->setStyleSheet("background-color: green");
  1880. mplatformEn = 1;
  1881. }
  1882. gIvlog->info("hmi","platform enable:%d", mplatformEn);
  1883. // UpdateV2xStEn(mv2xStEn);
  1884. iv::hmi::hmimsg hmi;
  1885. hmi.set_platformen(mplatformEn);
  1886. ShareHMIMsgPro(hmi);
  1887. }
  1888. //路测设备使能控制
  1889. void ADCIntelligentVehicle::on_button_obu_en_clicked()
  1890. {
  1891. if(mobuEn){
  1892. ui->button_obu_en->setStyleSheet("background-color: gray");
  1893. mobuEn = 0;
  1894. }
  1895. else{
  1896. ui->button_obu_en->setStyleSheet("background-color: green");
  1897. mobuEn = 1;
  1898. }
  1899. gIvlog->info("hmi","platform enable:%d", mobuEn);
  1900. // UpdateV2xStEn(mv2xStEn);
  1901. iv::hmi::hmimsg hmi;
  1902. hmi.set_obuen(mobuEn);
  1903. ShareHMIMsgPro(hmi);
  1904. }
  1905. void ADCIntelligentVehicle::on_button_map_set_clicked()
  1906. {
  1907. QString fileName = QFileDialog::getOpenFileName(this,
  1908. tr("选择地图文件"),
  1909. "/home/nvidia/map",
  1910. tr("Text Files (*.xodr)"));
  1911. if (fileName.isEmpty())
  1912. {
  1913. QMessageBox msgBox(QMessageBox::Warning, tr("警告"),
  1914. "No Map Selected", 0, this);
  1915. msgBox.addButton(tr("OK"), QMessageBox::AcceptRole);
  1916. // msgBox.addButton(tr("&Continue"), QMessageBox::RejectRole);
  1917. // if (msgBox.exec() == QMessageBox::AcceptRole)
  1918. // qDebug()<<"accept";
  1919. // else
  1920. // qDebug()<<"reject";
  1921. }else{
  1922. //发送地图文件名到driver_map_xodrload
  1923. iv::hmi::hmimsg hmi;
  1924. hmi.set_mapname(fileName.toStdString());
  1925. gIvlog->info("mapLoad","send map name");
  1926. ShareHMIMsgPro(hmi);
  1927. QThread::sleep(1);
  1928. gIvlog->info("mapLoad","send des gps");
  1929. //发送站点gps信息
  1930. xodrobj xo;
  1931. xo.flon = mdLon;
  1932. xo.flat = mdLat;
  1933. xo.lane = 1;
  1934. iv::modulecomm::ModuleSendMsg(mpadst,(char *)&xo,sizeof(xodrobj));
  1935. }
  1936. }
  1937. void ADCIntelligentVehicle::on_button_vin_set_clicked()
  1938. {
  1939. bool ok;
  1940. QString text = QInputDialog::getText(this, tr("输入车辆VIN:"),
  1941. tr("VIN:"), QLineEdit::Normal,
  1942. Q_NULLPTR, &ok);
  1943. if (ok && !text.isEmpty())
  1944. {
  1945. gstrvin = text.toStdString();
  1946. iv::hmi::hmimsg hmi;
  1947. hmi.set_vin(gstrvin);
  1948. ShareHMIMsgPro(hmi);
  1949. ui->lineEdit_vin->setText(text);
  1950. }
  1951. }
  1952. void ADCIntelligentVehicle::on_button_speedLimit_set_clicked()
  1953. {
  1954. bool ok;
  1955. double d = QInputDialog::getDouble(this, tr("QInputDialog::getDouble()"),
  1956. tr("Amount:"), 0.0, 0, 30, 1, &ok);
  1957. if (ok)
  1958. {
  1959. // doubleLabel->setText(QString("$%1").arg(d));
  1960. mdspeedLimit = d;
  1961. iv::hmi::hmimsg hmi;
  1962. hmi.set_speedlimit(mdspeedLimit);
  1963. ShareHMIMsgPro(hmi);
  1964. ui->lineEdit_speedLimit->setText(QString::number(mdspeedLimit,'f',1));
  1965. }
  1966. }
  1967. void ADCIntelligentVehicle::buttonColorReset()
  1968. {
  1969. ui->button_trafficInfo_en->setStyleSheet("background-color: gray");
  1970. ui->button_FCW_en->setStyleSheet("background-color: gray");
  1971. ui->button_jams_en->setStyleSheet("background-color: gray");
  1972. ui->button_DriCrims_en->setStyleSheet("background-color: gray");
  1973. }
  1974. void ADCIntelligentVehicle::on_button_trafficInfo_en_clicked()
  1975. {
  1976. buttonColorReset();
  1977. if(mbTrafficInfoEn){
  1978. ui->button_trafficInfo_en->setStyleSheet("background-color: gray");
  1979. mbTrafficInfoEn = false;
  1980. }
  1981. else{
  1982. ui->button_trafficInfo_en->setStyleSheet("background-color: green");
  1983. mbTrafficInfoEn = true;
  1984. }
  1985. gIvlog->info("hmi","traffic info enable:%d", mbTrafficInfoEn);
  1986. // UpdateV2xStEn(mv2xStEn);
  1987. iv::hmi::hmimsg hmi;
  1988. mbFCWEn = false;
  1989. mbjamsEn = false;
  1990. mbdriCrimsEn = false;
  1991. hmi.set_rodeinfoen(mbTrafficInfoEn);
  1992. hmi.set_rodefcwen(mbFCWEn);
  1993. hmi.set_roadjamsen(mbjamsEn);
  1994. hmi.set_roaddricrimsen(mbdriCrimsEn);
  1995. ShareHMIMsgPro(hmi);
  1996. }
  1997. void ADCIntelligentVehicle::on_button_FCW_en_clicked()
  1998. {
  1999. buttonColorReset();
  2000. if(mbFCWEn){
  2001. ui->button_FCW_en->setStyleSheet("background-color: gray");
  2002. mbFCWEn = false;
  2003. }
  2004. else{
  2005. ui->button_FCW_en->setStyleSheet("background-color: green");
  2006. mbFCWEn = true;
  2007. }
  2008. gIvlog->info("hmi","traffic FCW enable:%d", mbFCWEn);
  2009. // UpdateV2xStEn(mv2xStEn);
  2010. iv::hmi::hmimsg hmi;
  2011. mbTrafficInfoEn = false;
  2012. mbjamsEn = false;
  2013. mbdriCrimsEn = false;
  2014. hmi.set_rodeinfoen(mbTrafficInfoEn);
  2015. hmi.set_rodefcwen(mbFCWEn);
  2016. hmi.set_roadjamsen(mbjamsEn);
  2017. hmi.set_roaddricrimsen(mbdriCrimsEn);
  2018. ShareHMIMsgPro(hmi);
  2019. }
  2020. void ADCIntelligentVehicle::on_button_jams_en_clicked()
  2021. {
  2022. buttonColorReset();
  2023. if(mbjamsEn){
  2024. ui->button_jams_en->setStyleSheet("background-color: gray");
  2025. mbjamsEn = false;
  2026. }
  2027. else{
  2028. ui->button_jams_en->setStyleSheet("background-color: green");
  2029. mbjamsEn = true;
  2030. }
  2031. gIvlog->info("hmi","traffic jams enable:%d", mbjamsEn);
  2032. // UpdateV2xStEn(mv2xStEn);
  2033. iv::hmi::hmimsg hmi;
  2034. mbTrafficInfoEn = false;
  2035. mbFCWEn = false;
  2036. mbdriCrimsEn = false;
  2037. hmi.set_rodeinfoen(mbTrafficInfoEn);
  2038. hmi.set_rodefcwen(mbFCWEn);
  2039. hmi.set_roadjamsen(mbjamsEn);
  2040. hmi.set_roaddricrimsen(mbdriCrimsEn);
  2041. ShareHMIMsgPro(hmi);
  2042. }
  2043. void ADCIntelligentVehicle::on_button_SimCar_en_clicked()
  2044. {
  2045. iv::hmi::hmimsg hmi;
  2046. hmi.set_lonup(ui->lineEdit_jamsLon_up->text().toDouble());
  2047. hmi.set_lonlow(ui->lineEdit_jamsLon_low->text().toDouble());
  2048. hmi.set_latup(ui->lineEdit_jamsLat_Up->text().toDouble());
  2049. hmi.set_latlow(ui->lineEdit_jamsLat_low->text().toDouble());
  2050. hmi.set_headingup(ui->lineEdit_jamsHead_up->text().toDouble());
  2051. hmi.set_headinglow(ui->lineEdit_jamsHead_low->text().toDouble());
  2052. hmi.set_carcount(ui->lineEdit_jamsCarNum->text().toInt());
  2053. hmi.set_speedlow(ui->lineEdit_jamsSpeed_low->text().toDouble());
  2054. hmi.set_speedup(ui->lineEdit_jamsSpeed_up->text().toDouble());
  2055. ShareHMIMsgPro(hmi);
  2056. }
  2057. void ADCIntelligentVehicle::on_button_DriCrims_en_clicked()
  2058. {
  2059. buttonColorReset();
  2060. if(mbdriCrimsEn){
  2061. ui->button_DriCrims_en->setStyleSheet("background-color: gray");
  2062. mbdriCrimsEn = false;
  2063. }
  2064. else{
  2065. ui->button_DriCrims_en->setStyleSheet("background-color: green");
  2066. mbdriCrimsEn = true;
  2067. }
  2068. gIvlog->info("hmi","traffic jams enable:%d", mbdriCrimsEn);
  2069. // UpdateV2xStEn(mv2xStEn);
  2070. iv::hmi::hmimsg hmi;
  2071. mbTrafficInfoEn = false;
  2072. mbFCWEn = false;
  2073. mbjamsEn = false;
  2074. hmi.set_rodeinfoen(mbTrafficInfoEn);
  2075. hmi.set_rodefcwen(mbFCWEn);
  2076. hmi.set_roadjamsen(mbjamsEn);
  2077. hmi.set_roaddricrimsen(mbdriCrimsEn);
  2078. ShareHMIMsgPro(hmi);
  2079. }