ADCIntelligentVehicle.h 8.6 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336
  1. #ifndef ADCINTELLIGENTVEHICLE_H
  2. #define ADCINTELLIGENTVEHICLE_H
  3. #include <QMainWindow>
  4. #include <QtWidgets/QMainWindow>
  5. #include <common/boost.h>
  6. //#include <control/control_status.h>
  7. //#include <control/can.h>
  8. #include "ui_ADCIntelligentVehicle.h"
  9. //#include <decition/brain.h>
  10. //#include <control/can.h>
  11. #include "look.h"
  12. #include <common/car_status.h>
  13. #include <qtimer.h>
  14. #include <qpainter.h>
  15. #include <QGraphicsView>
  16. #include <QWheelEvent>
  17. #include <QKeyEvent>
  18. #include <QPoint>
  19. #include <QPointF>
  20. #include <qfiledialog.h>
  21. #include <qmessagebox.h>
  22. #include <QCloseEvent>
  23. #include <qabstractscrollarea.h>
  24. #include <QGraphicsItem>
  25. #include <QKeyEvent>
  26. #include <qpainter.h>
  27. #include <QDebug>
  28. #include <string>
  29. #include <cassert>
  30. #include <iostream>
  31. #include <sstream>
  32. #include <math.h>
  33. #include <mobileye_info.h>
  34. #include <QUdpSocket>
  35. #include <QHostAddress>
  36. #include <QTime>
  37. #include <QSet>
  38. #include "common/can_type.h"
  39. #include "common/decition_type.h"
  40. #include "common/vehiclestate_type.h"
  41. #include "common/hmi_type.h"
  42. #include "common/lidar.h"
  43. #include "common/obstacle_type.h"
  44. #include "modulecomm.h"
  45. #include <QMutex>
  46. //#include <platform/platform.h>
  47. #include "hmi.pb.h"
  48. #include "gpsimu.pb.h"
  49. #include "radarobjectarray.pb.h"
  50. #include "decition.pb.h"
  51. #include "brainstate.pb.h"
  52. #include "canstate.pb.h"
  53. #include "v2x.pb.h"
  54. #include "ivlog.h"
  55. extern iv::Ivlog * gIvlog;
  56. #define USE_PAD_CTRL
  57. class MyView : public QGraphicsView
  58. {
  59. Q_OBJECT
  60. public:
  61. MyView(QWidget *parent);
  62. qreal x, y, beishu; //倍数
  63. protected:
  64. void wheelEvent(QWheelEvent *event) Q_DECL_OVERRIDE; //Q_DECL_OVERRIDE也就是c++的override 作用是防止写错虚函数:
  65. void mouseMoveEvent(QMouseEvent *event) Q_DECL_OVERRIDE;
  66. void mousePressEvent(QMouseEvent *event) Q_DECL_OVERRIDE;
  67. void mouseReleaseEvent(QMouseEvent *event) Q_DECL_OVERRIDE;
  68. void keyPressEvent(QKeyEvent *event) Q_DECL_OVERRIDE;
  69. void keyReleaseEvent(QKeyEvent *event) Q_DECL_OVERRIDE;
  70. signals:
  71. void CtrlManual(bool bManual);
  72. public:
  73. QSet<int> mPressKeys;
  74. public Q_SLOTS:
  75. void zoomIn(); // 放大
  76. void zoomOut(); // 缩小
  77. private:
  78. bool bottonstatus = false;
  79. QPoint myview_lastMousePos;
  80. };
  81. namespace Ui {
  82. class ADCIntelligentVehicle;
  83. }
  84. struct packageHead
  85. {
  86. unsigned char type;
  87. int size;
  88. };
  89. struct dataInfo //与PAD通信结构体
  90. {
  91. unsigned char is_run = 0;
  92. unsigned char is_mapLoad = 0;
  93. unsigned char is_initSuccess = 0;
  94. double speed;
  95. double accelerate;
  96. double brake;
  97. double swerve;
  98. double gps_lng;
  99. double gps_lat;
  100. double gps_head;
  101. double radarobs;
  102. double lidarobs;
  103. double obs;
  104. unsigned char RTKstatus;
  105. unsigned char radarStatus;
  106. unsigned char lidarStatus;
  107. unsigned char boche_status;
  108. };
  109. class DataToUI
  110. {
  111. public:
  112. struct packageHead mHead;
  113. struct dataInfo mInfo;
  114. };
  115. class ADCIntelligentVehicle : public QMainWindow
  116. {
  117. Q_OBJECT
  118. public:
  119. explicit ADCIntelligentVehicle(QWidget *parent = 0);
  120. //~ADCIntelligentVehicle();
  121. public slots:
  122. void savestabuyEditinfo(const QString &txt);
  123. // void on_pb_load_navigation_data_clicked(); //加载地图 当前没有实现
  124. void on_pb_start_all_clicked(); //启动无人驾驶
  125. // void on_pb_open_can_card_clicked(); //打开can
  126. void on_pb_auto_drive_mode_clicked(); //自动驾驶使能
  127. void on_pb_auto_braking_mode_clicked(); //自动刹车使能
  128. void on_pb_auto_accelerate_mode_clicked(); //自动油门使能
  129. void on_pb_auto_wheel_mode_clicked(); //自动方向盘使能
  130. void on_pushButton_clicked();
  131. void on_pb_speed_control_mode_clicked(); //速度控制模式使能
  132. void on_hs_braking_percent_valueChanged(int value); //响应刹车压力改变
  133. void on_hs_accelerate_percent_valueChanged(int value); //响应油门改变
  134. void on_hs_wheel_angle_valueChanged(int value); //响应方向盘改变
  135. // void on_pushButton_2_clicked(); //打开地图采集器
  136. void on_pushButton_3_clicked();
  137. void on_pushButton_4_clicked();
  138. void on_timer_car_status_time_out(); //获取车辆状态定时器响应
  139. void on_timer_mobileye_info_time_out();
  140. virtual void paintEvent(QPaintEvent *);
  141. void timeoutslot();
  142. void onRecvUDP();
  143. void onStateTimer();
  144. protected:
  145. void keyPressEvent(QKeyEvent *event) Q_DECL_OVERRIDE;// 加/减键进行缩放
  146. void closeEvent(QCloseEvent *event);
  147. private slots:
  148. void on_listWidget_clicked();
  149. void on_pushButton_5_clicked();
  150. void on_pushButton_6_clicked();
  151. void on_pushButton_7_clicked();
  152. void on_pushButton_8_clicked();
  153. void on_pushButton_9_clicked();
  154. void onTimerManual();
  155. void onCtrlManual(bool bCtrl);
  156. void on_pb_v2xEn_clicked();
  157. private:
  158. Ui::ADCIntelligentVehicle *ui;
  159. // boost::shared_ptr<iv::decition::BrainDecition> brain;
  160. bool is_brain_running_flag_ = false;
  161. bool is_map_loaded_flag_ = false;
  162. bool is_can_opened_ = false;
  163. bool is_auto_drive_mode_enable_ = false;
  164. bool is_auto_braking_mode_enable_ = false;
  165. bool is_auto_accelerate_mode_enable_ = false;
  166. bool is_auto_wheel_mode_enable_ = false;
  167. bool is_3 = false;
  168. bool is_speed_control_mode_enable_ = false;
  169. bool is_show_enable = false;
  170. Look_decition *look_decition;
  171. Mobileye_info *mobileye_info;
  172. Qt::MouseButton m_translateButton; // 平移按钮
  173. qreal m_translateSpeed; // 平移速度
  174. qreal m_zoomDelta; // 缩放的增量
  175. bool m_bMouseTranslate; // 平移标识
  176. QPoint m_lastMousePos; // 鼠标最后按下的位置
  177. qreal m_scale; // 缩放值
  178. QTimer * timer_car_status;
  179. QTimer * timer_mobileye_info;
  180. qreal horizontalOffset;
  181. qreal verticalOffset;
  182. qreal scaleFactor;
  183. qreal currentStepScaleFactor;
  184. QImage *image,*image_small;
  185. QPainter *painter,*painter_small;
  186. MyView *myview,*myview_small;
  187. QTimer *timer;
  188. QGraphicsScene *scene,*scene_small;
  189. iv::ObsLiDAR Lidar_read;
  190. iv::ObsLiDAR Lidar_obsread;
  191. iv::ObsRadar RadarDate;
  192. double asd = 0.0;
  193. int load_counts = 0;
  194. void AutoStart();
  195. QUdpSocket msocksend;
  196. QUdpSocket msockrecv;
  197. QTimer mTimerState;
  198. DataToUI mDataToUI;
  199. QTime mTime;
  200. int mnTimeLastUpdatePaint;
  201. void * mpa;
  202. QMutex mMutexNavi;
  203. std::vector<iv::GPSData> m_navigation_data;
  204. bool mbBrainRunning = false;
  205. double mdecition_period = 0;
  206. QTime mTimeState;
  207. int mnTimeUpdateGPS= 0;
  208. int mnTimeUpdateRadar=0;
  209. int mnTimeUpdateDecition=0;
  210. int mnTimeUpdateVS=0;
  211. int mnTimeUpdateCANState=0;
  212. double mfSOC = 0.0;
  213. void * mpManualCtrl;
  214. bool mbManual= false;
  215. QTimer * mTimerManual;
  216. int mnCtrlValue[3]; //Acc Brake Gear Acc 0-1000 Brake 0-1000 Gear -500 --- 500
  217. QTime mManualTime;
  218. int mnLastManualTime;
  219. private:
  220. void UpdateMap(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
  221. private:
  222. void * mpagpsimu;
  223. public:
  224. void UpdateGPSIMU(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
  225. unsigned int mv2xStEn = 0;
  226. private:
  227. void * mparadar;
  228. private:
  229. iv::radar::radarobjectarray mradararray;
  230. bool mbradarUpdate = false;
  231. QMutex mMutexRadar;
  232. void UpdateRADAR(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
  233. void UpdateChassis(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname);
  234. void * mpachassis;
  235. private:
  236. void * mpacanstate;
  237. public:
  238. void UpdateCANState(const char * pdata,const int ndatasize);
  239. private:
  240. void * mpaDecition;
  241. public:
  242. void UpdateDecition(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
  243. private:
  244. void * mpaVechicleState;
  245. public:
  246. void UpdateVehicleState(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
  247. private:
  248. void * mpaHMI;
  249. /***********20200509****************/
  250. public:
  251. void UpdatePlanTrace(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
  252. std::vector<iv::Point2D> m_plan;
  253. QMutex mMutexPlan;
  254. private:
  255. void * mpaplantrace;
  256. /**************************/
  257. private:
  258. void * mpaLidar;
  259. public:
  260. void UpdateOBS(std::shared_ptr<std::vector<iv::ObstacleBasic> > lidar_obs);
  261. void UpdateV2xStEn(unsigned int enable);
  262. private:
  263. void * mp_v2xStSend;
  264. void ShareHMIMsg(iv::hmi::HMIBasic xhmi);
  265. iv::Ivlog * mpivlog;
  266. // iv::platform::Client mplatform_client;
  267. };
  268. #endif