mainwindow.cpp 18 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606
  1. #include "mainwindow.h"
  2. #include "ui_mainwindow.h"
  3. #include <iostream>
  4. #include <QFileDialog>
  5. #include <QTimer>
  6. #include <functional>
  7. MainWindow::MainWindow(QWidget *parent)
  8. : QMainWindow(parent)
  9. , ui(new Ui::MainWindow)
  10. {
  11. ui->setupUi(this);
  12. ui->actionNoClassification->setChecked(true);
  13. ui->actionNoise->setChecked(true);
  14. ui->actionGround->setChecked(true);
  15. ui->actionTraversableUnder->setChecked(true);
  16. ui->actionObstacle->setChecked(true);
  17. ui->actionInvalid->setChecked(true);
  18. ui->actionCar->setChecked(true);
  19. ui->actionTruck->setChecked(true);
  20. ui->actionMotorcycle->setChecked(true);
  21. ui->actionBicycle->setChecked(true);
  22. ui->actionPedestrian->setChecked(true);
  23. ui->actionAnimal->setChecked(true);
  24. ui->actionHazard->setChecked(true);
  25. ui->actionUnknown->setChecked(true);
  26. vtkSmartPointer<vtkRenderer> renderer = vtkSmartPointer<vtkRenderer>::New();
  27. vtkSmartPointer<vtkGenericOpenGLRenderWindow> renderWindow = vtkSmartPointer<vtkGenericOpenGLRenderWindow>::New();
  28. renderWindow->AddRenderer(renderer);
  29. view.reset(new pcl::visualization::PCLVisualizer(renderer,renderWindow,"viewer",false));
  30. #if VTK890
  31. view->setupInteractor(ui->vtk->interactor(),ui->vtk->renderWindow());
  32. #else
  33. view->setupInteractor(ui->vtk->GetInteractor(),ui->vtk->GetRenderWindow());
  34. #endif
  35. #if VTK890
  36. this->ui->vtk->setRenderWindow(renderWindow);
  37. #else
  38. this->ui->vtk->SetRenderWindow(renderWindow);
  39. #endif
  40. DrawAxis();
  41. // Create a sphere.
  42. int i;
  43. for(i=0;i<NUM_DET_MAX;i++){
  44. detSource[i]->SetBounds(-0.001,-0.001,-0.001,0.001,0.001,0.001);
  45. detSource[i]->Update();
  46. detmapper[i]->SetInputData(detSource[i]->GetOutput());
  47. // Create an actor.
  48. vtkNew<vtkActor> actor;
  49. vtkNew<vtkNamedColors> colors;
  50. actor->GetProperty()->SetColor(colors->GetColor3d("Yellow").GetData());
  51. actor->SetMapper(detmapper[i]);
  52. renderer->AddActor(actor);
  53. }
  54. for(i=0;i<NUM_OBJ_MAX;i++){
  55. sphereSource[i]->SetCenter(0,0,0);
  56. sphereSource[i]->SetRadius(0.001);
  57. sphereSource[i]->Update();
  58. mapper[i]->SetInputData(sphereSource[i]->GetOutput());
  59. // Create an actor.
  60. vtkNew<vtkActor> actor;
  61. vtkNew<vtkNamedColors> colors;
  62. actor->GetProperty()->SetColor(colors->GetColor3d("Red").GetData());
  63. actor->SetMapper(mapper[i]);
  64. renderer->AddActor(actor);
  65. }
  66. QTimer * timer1 = new QTimer();
  67. connect(timer1,SIGNAL(timeout()),this,SLOT(onTimer()));
  68. // timer1->start(100);
  69. connect(this,SIGNAL(detupdate()),this,SLOT(onDetUpdate()));
  70. #ifdef USE_MODULECOMM
  71. ModuleFun fundetect = std::bind(&MainWindow::UpdateDet,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
  72. mpadet = iv::modulecomm::RegisterRecvPlus("radar4ddet",fundetect);
  73. ModuleFun funojbect = std::bind(&MainWindow::UpdateObj,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
  74. mpaobj = iv::modulecomm::RegisterRecvPlus("radar4dobj",funojbect);
  75. #else
  76. mprecv = new ars548recv();
  77. mpthreaddet = new std::thread(&MainWindow::threaddet,this);
  78. mpthreadobj = new std::thread(&MainWindow::threadobj,this);
  79. #endif
  80. setWindowTitle("radar4d 3d Viewer");
  81. }
  82. MainWindow::~MainWindow()
  83. {
  84. mbThreadRun = false;
  85. #ifdef USE_MODULECOMM
  86. iv::modulecomm::Unregister(mpaobj);
  87. iv::modulecomm::Unregister(mpadet);
  88. #else
  89. mpthreadobj->join();
  90. mpthreaddet->join();
  91. delete mprecv;
  92. #endif
  93. delete ui;
  94. }
  95. void MainWindow::DrawAxis()
  96. {
  97. bool bGrid = false;
  98. pcl::PointXYZ p1(-300,0,0);
  99. pcl::PointXYZ p2(300,0,0);
  100. view->addLine(p1,p2,1.0f,1.0f,1.0f,"axis_x");
  101. p1.x = 0;p1.y = -300;
  102. p2.x= 0;p2.y = 300;
  103. view->addLine(p1,p2,1.0f,1.0f,1.0f,"axis_y");
  104. int i;
  105. if(bGrid)
  106. {
  107. for(i=-30;i<=30;i++)
  108. {
  109. if(i==0)continue;
  110. p1.x = -300;p1.y = i*10;
  111. p2.x = 300; p2.y = i*10;
  112. char strname[256];
  113. snprintf(strname,256,"axisgx_%d",i);
  114. view->addLine(p1,p2,1.0f,1.0f,1.0f,strname);
  115. p1.x = i*10;p1.y = -300;
  116. p2.x = i*10; p2.y = 300;
  117. snprintf(strname,256,"axisgy_%d",i);
  118. view->addLine(p1,p2,1.0f,1.0f,1.0f,strname);
  119. }
  120. }
  121. else
  122. {
  123. for(i=-30;i<=30;i++)
  124. {
  125. if(i==0)continue;
  126. p1.x = 0;p1.y = i*10;
  127. p2.x = 1; p2.y = i*10;
  128. char strname[256];
  129. snprintf(strname,256,"axisgx_%d",i);
  130. view->addLine(p1,p2,1.0f,1.0f,1.0f,strname);
  131. p1.x = i*10;p1.y = 0;
  132. p2.x = i*10; p2.y = 1;
  133. snprintf(strname,256,"axisgy_%d",i);
  134. view->addLine(p1,p2,1.0f,1.0f,1.0f,strname);
  135. }
  136. }
  137. for(i=-6;i<=6;i++)
  138. {
  139. char strname[256];
  140. char strtext[256];
  141. double orientation[3];
  142. orientation[0] = 1.0;orientation[1] = 0.0;orientation[2] = 0.0;
  143. p1.x = i*50;p1.y = 1;
  144. snprintf(strname,256,"textx_%d",i);
  145. snprintf(strtext,256,"%d",i*50);
  146. if(i%2 == 0)
  147. view->addText3D(strtext,p1,orientation,1.0,1.0,1.0,1.0,strname);
  148. else
  149. view->addText3D(strtext,p1,orientation,0.5,1.0,1.0,1.0,strname);
  150. if(i== 0)continue;
  151. orientation[0] = 1.0;orientation[1] =0.0;orientation[2] = 0.0;
  152. p1.x = 1;p1.y = i*50+1;
  153. snprintf(strname,256,"texty_%d",i);
  154. snprintf(strtext,256,"%d",i*50);
  155. if(i%2 == 0)
  156. view->addText3D(strtext,p1,orientation,1.0,1.0,1.0,1.0,strname);
  157. else
  158. view->addText3D(strtext,p1,orientation,0.5,1.0,1.0,1.0,strname);
  159. }
  160. // double base_size = 0.2;
  161. // double base_height = 0.2;
  162. // double sigx = 15;
  163. // double sigy = 15;
  164. // double sigz = 1.0;
  165. // view->addCube(sigx-base_size/2.0,sigx + base_size/2.0,sigy-base_size/2.0,sigy+base_size/2.0,sigz-base_height/2.0,sigz + base_height/2.0,1.0,0.0,0.0,"cube1");
  166. view->setCameraPosition(0,0,100,0,0,0,0,1,0);
  167. view->resetCamera(); //视角
  168. // view->addText("300",300,5,10,1.0f,0.0f,0.0f,"txt1");
  169. }
  170. void MainWindow::resizeEvent(QResizeEvent *event)
  171. {
  172. (void)event;
  173. QSize sizemain = ui->centralwidget->size();
  174. ui->vtk->setGeometry(0,0,sizemain.width()-0,sizemain.height()-0);
  175. }
  176. void MainWindow::onTimer()
  177. {
  178. int64_t nstart = std::chrono::system_clock::now().time_since_epoch().count();
  179. static bool bSetCam = false;
  180. if(bSetCam == false)
  181. {
  182. bSetCam = true;
  183. view->setCameraPosition(0,30,130,0,30,0,0,1,0);
  184. // view->resetCamera(); //视角
  185. }
  186. static int nindex = 0;
  187. nindex++;
  188. if(nindex>1000)nindex = 0;
  189. double base_size = 0.5;
  190. double base_height = 0.5;
  191. double sigx = 15;
  192. double sigy = 15 + nindex * 0.01;
  193. double sigz = 1.0;
  194. (void)base_size;
  195. (void)base_height;
  196. (void)sigx;
  197. (void)sigz;
  198. // view->removeShape("cube1");
  199. // view->addCube(sigx-base_size/2.0,sigx + base_size/2.0,sigy-base_size/2.0,sigy+base_size/2.0,sigz-base_height/2.0,sigz + base_height/2.0,1.0,0.0,0.0,"cube1");
  200. int i;
  201. for(i=0;i<100;i++){
  202. sphereSource[i]->SetCenter(i, sigy, 0.0);
  203. sphereSource[i]->Update();
  204. }
  205. // Create a mapper.
  206. // mapper->SetInputData(sphereSource->GetOutput());
  207. #if VTK890
  208. ui->vtk->renderWindow()->Render();
  209. #else
  210. ui->vtk->GetRenderWindow()->Render();
  211. #endif
  212. int64_t nend = std::chrono::system_clock::now().time_since_epoch().count();
  213. std::cout<<" update use : "<<(nend -nstart)/1e6<<std::endl;
  214. }
  215. void MainWindow::onDetUpdate()
  216. {
  217. static int64_t nlastup = 0;
  218. int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count();
  219. int64_t ndiff = (nnow - nlastup)/1e6;
  220. if(abs(ndiff)<10)
  221. {
  222. // nlastup = nnow;
  223. return;
  224. }
  225. bool bdetupdate = false;
  226. bool bobjupdate = false;
  227. int64_t nstart = std::chrono::system_clock::now().time_since_epoch().count();
  228. iv::radar::radar4ddetectarray xdetarray;
  229. mmutexdet.lock();
  230. if(mbdetupdate){
  231. xdetarray.CopyFrom(mdetarray);
  232. mbdetupdate = false;
  233. bdetupdate = true;
  234. }
  235. mmutexdet.unlock();
  236. iv::radar::radar4dobjectarray xobjarray;
  237. mmutexobj.lock();
  238. if(mbobjupdate){
  239. xobjarray.CopyFrom(mobjarray);
  240. mbobjupdate = false;
  241. bobjupdate = true;
  242. }
  243. mmutexobj.unlock();
  244. // static int nskip = 0;
  245. // nskip++;
  246. // if(nskip <2)
  247. // {
  248. // return;
  249. // }
  250. // nskip = 0;
  251. static bool bSetCam = false;
  252. if(bSetCam == false)
  253. {
  254. bSetCam = true;
  255. view->setCameraPosition(0,30,130,0,30,0,0,1,0);
  256. // view->resetCamera(); //视角
  257. }
  258. int i;
  259. double base_size = 0.2;
  260. double obj_size = 0.5;
  261. double sigx = 15;
  262. double sigy = 15 ;
  263. double sigz = 1.0;
  264. int nsize;
  265. if(bdetupdate){
  266. nsize = static_cast<int>(xdetarray.mdet_size());
  267. if(nsize > NUM_DET_MAX){
  268. nsize = NUM_DET_MAX;
  269. }
  270. for(i=0;i<nsize;i++)
  271. {
  272. iv::radar::radar4ddetect * pdet = xdetarray.mutable_mdet(i);
  273. if((pdet->detection_radial_distance()<0.001) || (pdet->detection_classification() != iv::radar::radar4ddetect::Obstacle))
  274. {
  275. detSource[i]->SetBounds(1000.0,1000.1,-0.001,0.001,0.001,0.001);
  276. detSource[i]->Update();
  277. continue;
  278. }
  279. bool bshow = false;
  280. if(ui->actionAll->isChecked())bshow = true;
  281. if((ui->actionNoClassification->isChecked())&&(pdet->detection_classification() == iv::radar::radar4ddetect::NoClassification))bshow = true;
  282. if((ui->actionNoise->isChecked())&&(pdet->detection_classification() == iv::radar::radar4ddetect::Noise))bshow = true;
  283. if((ui->actionGround->isChecked())&&(pdet->detection_classification() == iv::radar::radar4ddetect::Ground))bshow = true;
  284. if((ui->actionTraversableUnder->isChecked())&&(pdet->detection_classification() == iv::radar::radar4ddetect::TraversableUnder))bshow = true;
  285. if((ui->actionObstacle->isChecked())&&(pdet->detection_classification() == iv::radar::radar4ddetect::Obstacle))bshow = true;
  286. if((ui->actionInvalid->isChecked())&&(pdet->detection_classification() == iv::radar::radar4ddetect::Invalid))bshow = true;
  287. if(bshow == true){
  288. sigx = pdet->detection_radial_distance() * cos(pdet->unaligned_detection_elevation_angle()) * cos(pdet->unaligned_detection_azimuth_angle() + M_PI/2.0);
  289. sigy = pdet->detection_radial_distance() * cos(pdet->unaligned_detection_elevation_angle()) * sin(pdet->unaligned_detection_azimuth_angle() + M_PI/2.0);
  290. sigz = pdet->detection_radial_distance() * sin(pdet->unaligned_detection_elevation_angle());
  291. detSource[i]->SetBounds(sigx-base_size/2.0,sigx+base_size/2.0,sigy - base_size/2.0,sigy + base_size/2.0, sigz - base_size/2.0,
  292. sigz + base_size/2.0);
  293. detSource[i]->Update();
  294. }
  295. else {
  296. detSource[i]->SetBounds(1000.0,1000.1,-0.001,0.001,0.001,0.001);
  297. detSource[i]->Update();
  298. }
  299. }
  300. }
  301. if(bobjupdate){
  302. nsize = static_cast<int>(xobjarray.mobj_size());
  303. if(nsize > NUM_OBJ_MAX){
  304. nsize = NUM_OBJ_MAX;
  305. }
  306. for(i=0;i<nsize;i++)
  307. {
  308. iv::radar::radar4dobject * pobj = xobjarray.mutable_mobj(i);
  309. // std::cout<<" i: "<<i<<" unkown: "<<pobj->u_classification_unknown()<<" car: "<<pobj->u_classification_car()
  310. // <<" truck: "<<pobj->u_classification_truck()
  311. // <<" motocycle: "<<pobj->u_classification_motorcycle()
  312. // <<" bycycle: "<<pobj->u_classification_bicycle()
  313. // <<" ped: "<<pobj->u_classification_pedestrian()
  314. // <<" animal: "<<pobj->u_classification_animal()
  315. // <<" hazard: "<<pobj->u_classification_hazard()
  316. // <<"exist: "<<pobj->u_existence_probability()<<std::endl;
  317. sigx = pobj->u_position_y() * (-1);
  318. sigy = pobj->u_position_x();
  319. sigz = pobj->u_position_z();
  320. bool bShow = false;
  321. unsigned int nMinProb = 30;
  322. if(ui->actionObjectAll->isChecked())bShow = true;
  323. if((ui->actionCar->isChecked())&&(pobj->u_classification_car()>=nMinProb))bShow = true;
  324. if((ui->actionTruck->isChecked())&&(pobj->u_classification_truck()>=nMinProb))bShow = true;
  325. if((ui->actionMotorcycle->isChecked())&&(pobj->u_classification_motorcycle()>=nMinProb))bShow = true;
  326. if((ui->actionBicycle->isChecked())&&(pobj->u_classification_bicycle()>=nMinProb))bShow = true;
  327. if((ui->actionPedestrian->isChecked())&&(pobj->u_classification_pedestrian()>=nMinProb))bShow = true;
  328. if((ui->actionAnimal->isChecked())&&(pobj->u_classification_animal()>=nMinProb))bShow = true;
  329. if((ui->actionHazard->isChecked())&&(pobj->u_classification_hazard()>=nMinProb))bShow = true;
  330. if((ui->actionUnknown->isChecked())&&(pobj->u_classification_unknown()>=nMinProb))bShow = true;
  331. if((pobj->u_existence_probability() >0)&&bShow){
  332. sphereSource[i]->SetCenter(sigx,sigy,sigz);
  333. sphereSource[i]->SetRadius(obj_size/2.0);
  334. }
  335. else{
  336. sphereSource[i]->SetCenter(0.0,0.0,0.0);
  337. sphereSource[i]->SetRadius(0.001);
  338. }
  339. sphereSource[i]->Update();
  340. }
  341. }
  342. // std::cout<<"update det."<<std::endl;
  343. #if VTK890
  344. ui->vtk->renderWindow()->Render();
  345. #else
  346. ui->vtk->GetRenderWindow()->Render();
  347. #endif
  348. int64_t nend = std::chrono::system_clock::now().time_since_epoch().count();
  349. int64_t nuse = nend- nstart;
  350. std::cout<<" draw use : "<<nuse/1e6<<std::endl;
  351. nlastup = std::chrono::system_clock::now().time_since_epoch().count();
  352. }
  353. void MainWindow::on_actionReset_Camera_triggered()
  354. {
  355. view->setCameraPosition(0,0,100,0,0,0,0,1,0);
  356. view->resetCamera(); //视角
  357. }
  358. void MainWindow::on_actionFull_Screem_triggered()
  359. {
  360. showFullScreen();
  361. ui->menubar->setVisible(false);
  362. ui->statusbar->setVisible(false);
  363. ui->toolBar->setVisible(false);
  364. mbFull = true;
  365. }
  366. void MainWindow::keyReleaseEvent(QKeyEvent *event)
  367. {
  368. if((event->key() == Qt::Key_Escape)||(event->key() == Qt::Key_F11))
  369. {
  370. if(mbFull)
  371. {
  372. showNormal();
  373. ui->menubar->setVisible(true);
  374. ui->statusbar->setVisible(true);
  375. ui->toolBar->setVisible(true);
  376. mbFull = false;
  377. }
  378. }
  379. }
  380. #ifdef USE_MODULECOMM
  381. void MainWindow::UpdateDet(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
  382. {
  383. (void)index;
  384. (void)dt;
  385. (void)strmemname;
  386. iv::radar::radar4ddetectarray xdetarray;
  387. if(xdetarray.ParseFromArray(strdata,(int)nSize))
  388. {
  389. mmutexdet.lock();
  390. mdetarray.CopyFrom(xdetarray);
  391. mbdetupdate = true;
  392. mmutexdet.unlock();
  393. emit detupdate();
  394. }
  395. else
  396. {
  397. std::cout<<"detection parse fail."<<std::endl;
  398. }
  399. }
  400. void MainWindow::UpdateObj(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
  401. {
  402. (void)index;
  403. (void)dt;
  404. (void)strmemname;
  405. iv::radar::radar4dobjectarray xobjarray;
  406. if(xobjarray.ParseFromArray(strdata,(int)nSize))
  407. {
  408. mmutexobj.lock();
  409. mobjarray.CopyFrom(xobjarray);
  410. mbobjupdate = true;
  411. mmutexobj.unlock();
  412. emit detupdate();
  413. }
  414. else
  415. {
  416. std::cout<<"object parse fail."<<std::endl;
  417. }
  418. }
  419. #else
  420. void MainWindow::threaddet()
  421. {
  422. while(mbThreadRun)
  423. {
  424. iv::radar::radar4ddetectarray xdetarray;
  425. if(mprecv->GetDetect(10,xdetarray) == 1)
  426. {
  427. mmutexdet.lock();
  428. mdetarray.CopyFrom(xdetarray);
  429. mbdetupdate = true;
  430. mmutexdet.unlock();
  431. emit detupdate();
  432. }
  433. }
  434. }
  435. void MainWindow::threadobj()
  436. {
  437. while(mbThreadRun)
  438. {
  439. iv::radar::radar4dobjectarray xobjarray;
  440. if(mprecv->GetObj(10,xobjarray) == 1)
  441. {
  442. mmutexobj.lock();
  443. mobjarray.CopyFrom(xobjarray);
  444. mbobjupdate = true;
  445. mmutexobj.unlock();
  446. emit detupdate();
  447. }
  448. }
  449. }
  450. #endif
  451. void MainWindow::on_actionAll_triggered()
  452. {
  453. if(ui->actionAll->isChecked()){
  454. ui->actionNoClassification->setChecked(true);
  455. ui->actionNoise->setChecked(true);
  456. ui->actionGround->setChecked(true);
  457. ui->actionTraversableUnder->setChecked(true);
  458. ui->actionObstacle->setChecked(true);
  459. ui->actionInvalid->setChecked(true);
  460. }
  461. else
  462. {
  463. ui->actionNoClassification->setChecked(false);
  464. ui->actionNoise->setChecked(false);
  465. ui->actionGround->setChecked(false);
  466. ui->actionTraversableUnder->setChecked(false);
  467. ui->actionObstacle->setChecked(false);
  468. ui->actionInvalid->setChecked(false);
  469. }
  470. }
  471. void MainWindow::on_actionObjectAll_triggered()
  472. {
  473. if(ui->actionObjectAll->isChecked()){
  474. ui->actionCar->setChecked(true);
  475. ui->actionTruck->setChecked(true);
  476. ui->actionMotorcycle->setChecked(true);
  477. ui->actionBicycle->setChecked(true);
  478. ui->actionPedestrian->setChecked(true);
  479. ui->actionAnimal->setChecked(true);
  480. ui->actionHazard->setChecked(true);
  481. ui->actionUnknown->setChecked(true);
  482. }
  483. else{
  484. ui->actionCar->setChecked(false);
  485. ui->actionTruck->setChecked(false);
  486. ui->actionMotorcycle->setChecked(false);
  487. ui->actionBicycle->setChecked(false);
  488. ui->actionPedestrian->setChecked(false);
  489. ui->actionAnimal->setChecked(false);
  490. ui->actionHazard->setChecked(false);
  491. ui->actionUnknown->setChecked(false);
  492. }
  493. }