yincheng.zhong
2025-11-24 275b03224aa6170d4dc8c661c1cd949dd88c1fcb
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
o
IÏ#idbã@sndZddlmZddlZddlZddlZddlZddlZddlZddl m
Z
ddl m Z m Z mZmZz ddlmZmZmZWn eyIdZYnwzddlmZWn ey]dZYnwdd   lmZmZmZmZdd
lmZmZmZd Z d Z!d Z"dZ#dZ$dZ%dZ&e
e'ƒ (¡j)dZ*d#dd„Z+Gdd„dƒZ,Gdd„dej-ƒZ.d$dd„Z/d d!„Z0e1d"krµe 2e0ƒ¡dSdS)%u]
HITL ä»¿çœŸå™¨ GUI:左侧地图显示路径/车辆,右侧显示状态与控制信息。
é)Ú annotationsN)ÚPath)ÚDictÚListÚOptionalÚTuple)ÚQtCoreÚQtGuiÚ  QtWidgets)Ú
list_portsé)Ú ControlStatusÚ
PoseStatusÚ StackStatusÚ StateStatus)Ú
HitlConfigÚ HitlSimulatorÚ  RunLoggerZCOM35ZCOM17zX$GNGGA,080112.000,3949.8105069,N,11616.6876082,E,4,44,0.42,48.502,M,-6.684,M,1.0,0409*73)çrrriÂiz
runlog.txtÚ path_filerÚreturnúList[Tuple[float, float]]c  Cs"| ¡s
tt|ƒƒ‚|jdd ¡}|sgSg}|j ¡dks$| d¡rZz)t    |¡}|D]}t
|t t fƒrJt |ƒdkrJ| t|dƒt|dƒf¡q,|WStjyYgYSw| d¡D]/}| ¡}|shq_| d ¡}t |ƒdkrtq_z| t|dƒt|dƒf¡Wq_tyŽYq_w|S)
Nzutf-8)Úencodingz.jsonú[érr ú;ú,)ÚexistsÚFileNotFoundErrorÚstrZ  read_textÚstripÚsuffixÚlowerÚ
startswithÚjsonÚloadsÚ
isinstanceÚlistÚtupleÚlenÚappendÚfloatZJSONDecodeErrorÚsplitÚ
ValueError)rÚtextZpointsÚdataZptZtokenÚparts©r1ú/E:\GIT\Lawnmower_STM32H7\python\hitl\run_sim.pyÚload_path_points's< 
€ÿ
 " ÿr3c@s.eZdZdd„Zd dd„Zdd„Zd d
„Zd S) Ú HitlGuiBridgecCstjdd|_t ¡|_dS)NéÈ)Úmaxsize)ÚqueueZQueueÚ threadingZLockÚ_lock©Úselfr1r1r2Ú__init__FszHitlGuiBridge.__init__ÚkindrcCs:|j| ||f¡WdƒdS1swYdS©N)r9Ú_enqueue)r;r=r/r1r1r2ÚpublishJs"ÿzHitlGuiBridge.publishc Cs„|j5|j ¡s(z|j ¡Wn
tjyYnw|j ¡r   WdƒdSWdƒdSWdƒdS1s;wYdSr>)r9r7ZemptyÚ
get_nowaitÚEmptyr:r1r1r2ÚresetNs
ÿ ýÿÿ"ûzHitlGuiBridge.resetc Csvz |j |¡WdStjy:z|j ¡Wn
tjy"Ynwz
|j |¡WYdStjy9YYdSwwr>)r7Z
put_nowaitZFullrArB)r;Úitemr1r1r2r?VsÿÿùzHitlGuiBridge._enqueueN)r=r)Ú__name__Ú
__module__Ú __qualname__r<r@rCr?r1r1r1r2r4Es
 
 r4cs´eZdZd3‡fd
d „ Zd4d d „Zd5d4dd„Zd6dd„Zdd„Zdd„Zdd„Z   ‡fdd„Z
dd„Z dd „Z d!d"„Z d#d$„Zd%d&„Zd7d)d*„Zd+d,„Zd-d.„Zd/d0„Zd1d2„Z‡ZS)8Ú HitlDashboardÚ    simulatorrÚbridger4Ú path_pointsrÚ initial_uart2ú Optional[str]Ú initial_uart5cs¤tƒ ¡||_||_t|pgƒ|_g|_d|_d|_d|_ i|_
|  d¡|  dd¡d|_ g|_| ||¡| ¡t |¡|_|jj |j¡|j d¡dS)NuHITL ä»¿çœŸçŠ¶æ€é¢æ¿iiÐFéP)Úsuperr<rIrJr'rKÚ trail_pointsÚcontrol_statusÚ pose_statusÚ state_statusÚ stack_statusZsetWindowTitleZresizeÚ _serial_openÚ _last_portsÚ  _build_uiÚ _init_scenerZQTimerZtimerÚtimeoutÚconnectÚ _drain_queueÚstart)r;rIrJrKrLrN©Ú    __class__r1r2r<es$
 
   zHitlDashboard.__init__c CsÊt ¡}t |¡}| |¡t |¡|_t |j¡|_|j t    j
j d¡|j  tjj ¡|j tjj¡|j|jddtdu|_t ¡}t |¡}| dddd¡|j|ddt d¡}t |¡}t ¡|_t ¡|_t d¡|_t d ¡|_t d
¡|_|j d ¡| t d ¡dd¡| |jdd ¡| t d¡d d¡| |jd d ¡| |jdddd ¡| |jddd d¡| |jddd d¡| |¡tdurâ|j  d¡|j !d¡|j !d¡t d¡}   t |   ¡}
t "¡|_#t d¡|_$t d¡|_%|
 |j#¡|
 |j$¡|
 |j%¡| | ¡t d¡} t | ¡} t "t&¡|_'t d¡|_(|  t d¡dd¡|  |j'dd d d¡|  |j(dd¡gd¢} g|_)t*| ƒD]N\}}t +¡}|dkrj| ,dd¡| -d¡n | ,dd¡| -d¡|  t |¡d |d|dd¡|  |d |d|ddd ¡|j) .|¡qQt d¡|_/|  |j/ddd d¡| | ¡t d¡}t |¡}t d¡|_0|j0 1t2j3j4t2j3j5B¡|j0 d¡t 6¡}| 7d¡| 8|j0¡| |¡|j|d dt d ¡}t |¡}t 9dd¡|_:|j: ;gd!¢¡|j: <¡ =d¡| |j:¡| |¡t d"¡}t |¡}t >¡|_?|j? @d¡| |j?¡|j|d d|j At  B¡t    Ct  Dd#¡d$¡¡|_E|j At  B¡t    Ct  Dd%¡d&¡¡|_F|j Gd'd'd(d(t  Ct  Dd)¡¡t    Ht  Dd)¡¡¡|_I|j Jddddt    Ct  Dd)¡d¡¡|_K|j Gd*d*d+d+t  Ct  Dd,¡¡t    Ht  Dd,¡¡¡|_L|j$jM N|jO¡|j%jM N|jP¡|j(jM N|jQ¡|j/jM N|jR¡|jjM N|jS¡|jjM N|jT¡| S||¡| Ud¡| V¡dS)-NTé)Zstretchérru 串口设置u刷新õ 打开串口õ状态: æœªè¿žæŽ¥ú color: gray;zUART2:r zUART5:Fu 路径文件u浏览u加载u 原点/起点u 更新原点zGGA:)zE (m)zN (m)zU (m)z Heading (deg)g@Àg@@g€vÀg€v@u 重置位置éu 车辆状态õ等待数据...z6font-family: Consolas, 'Courier New'; font-size: 12px;u 堆栈监测)u任务u堆栈余量(word)u剩余堆(byte)u 串口日志Zgraygš™™™™™é?Zblueçð?g333333Ó¿ç333333ã?Zredgš™™™™™É¿çš™™™™™Ù?Zgreen)Wr
ZQWidgetZ QHBoxLayoutZsetCentralWidgetZQGraphicsSceneÚsceneZ QGraphicsViewÚviewZ setRenderHintr ZQPainterZ AntialiasingZ setDragModeZScrollHandDragZsetTransformationAnchorZAnchorUnderMouseZ    addWidgetr Ú_port_refresh_supportedZ QVBoxLayoutZsetContentsMarginsZ   QGroupBoxZ QGridLayoutZ QComboBoxÚ port2_comboÚ port5_comboZ QPushButtonÚport_refresh_btnÚserial_toggle_btnZQLabelÚserial_status_labelÚ setStyleSheetÚ
setEnabledZ setEditableZ    QLineEditÚ  path_lineÚpath_browse_btnÚ path_load_btnÚDEFAULT_ORIGIN_GGAÚ origin_editÚ
origin_btnÚpos_spinÚ   enumerateZQDoubleSpinBoxZsetRangeZ setDecimalsr*Úreset_pose_btnÚ
info_labelZ setAlignmentrZQtZAlignTopZ    AlignLeftZ QScrollAreaZsetWidgetResizableZ setWidgetZ QTableWidgetÚ stack_tableZsetHorizontalHeaderLabelsZhorizontalHeaderZsetStretchLastSectionZQPlainTextEditÚlog_viewÚ setReadOnlyZaddPathÚ QPainterPathZQPenZQColorÚ   path_itemÚ
trail_itemZ
addEllipseZQBrushÚ
robot_itemZaddLineÚ heading_itemÚ target_itemZclickedr[Ú _browse_pathÚ
_load_pathÚ_update_originÚ_reset_positionÚ_refresh_serial_portsÚ_toggle_serialÚ_set_controls_enabledÚ_update_serial_ui)r;rLrNZcentralZlayoutZ right_panelZ right_layoutZ serial_groupZ serial_layoutZ
path_groupZ path_layoutZ origin_groupZgridZlabelsÚidxZlabelZspinZ status_groupZ status_layoutZ info_scrollZ stack_groupZ stack_layoutZ log_groupZ
log_layoutr1r1r2rX‚sÌ
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
$" 
 
 
 
 
 
 
 
 
 
 
  ""0$0 
 zHitlDashboard._build_uiNcsŠg‰trz dd„t ¡Dƒ‰Wn tyg‰Ynw|p#|jjjp#d}|p,|jjjp,d}d
‡fdd „ }||j|ƒ||j|ƒˆ|_   dS) NcSsg|]}|j‘qSr1)Zdevice)Ú.0Úpr1r1r2Ú
<listcomp>sz7HitlDashboard._refresh_serial_ports.<locals>.<listcomp>ÚÚcomboúQtWidgets.QComboBoxÚsavedrcst| ¡}| d¡| ¡ˆr| ˆ¡|r|ˆvr| |¡|p"|}|r3| |¡}|dkr3| |¡| d¡dS)NTrF)Ú currentTextZ blockSignalsÚclearZaddItemsZaddItemZfindTextZsetCurrentIndex)r”r–ZcurrentÚtargetr©Zportsr1r2Ú_fill
s
 
 
 
 
z2HitlDashboard._refresh_serial_ports.<locals>._fill)r”r•r–r)
r ZcomportsÚ    ExceptionrIÚconfigÚ
uart2_portÚ
uart5_portrmrnrW)r;rLrNZ saved_uart2Z saved_uart5r›r1ršr2r‹s ÿ  
z#HitlDashboard._refresh_serial_portsÚenabledÚboolcCsL|j|j|j|j|j|jfD]}| |¡q|j | ¡|j | ¡dSr>)rtrurvrxryr|rsr€)r;r Zwidgetr1r1r2rsú z#HitlDashboard._set_controls_enabledcCsÊ|j |jrdnd¡|jr0|jjjpd}|jjjpd}|j d|›d|›d¡|j d¡n |j d   ¡|j d
¡|j }|j  
|pI|j   ¡¡|j  
|pT|j   ¡¡|j rc|j 
|j ¡dSdS) Nu 关闭串口rbu未知u无u状态: å·²è¿žæŽ¥ (UART2=z, UART5=ú)z color: green;rcrd)rpÚsetTextrVrIÚuart2ÚportÚlog_uartrqrrrmrsZ
isEditablernrlro)r;r¤Úuart5Z enable_portsr1r1r2rŽ*s  ÿzHitlDashboard._update_serial_uic
Csò|jr   | ¡dS|j ¡ ¡}|stj |dd¡dS|j ¡ ¡}|p&d}||j   j
_ ||j j
_ ||j j _||j   j_z|j    ¡Wn#tye}z|j  ¡tj |dd|›¡WYd}~dSd}~wwd|_|j ¡| d¡| ¡dS)Nu串口u请先选择 UART2 ç«¯å£ã€‚u打开串口失败: T)rVÚ _close_serialrmr—r r
Ú QMessageBoxÚwarningrnrIrržrŸr¤r¥r¦r]rœÚstopZcriticalrJrCrrŽ)r;r¤Z
uart5_textr§Úexcr1r1r2rŒ:s2
 
 
 
 
€ý
 
 zHitlDashboard._toggle_serialcCs€|j ¡|j ¡|j ¡d|_d|_d|_|j   ¡|j
 ¡|j   d¡|j  d¡d|_| d¡| ¡| ¡dS)NrfrF)rIr«rJrCrQr˜rRrSrTrUrr}r£r~Ú setRowCountrVrrŽÚ _refresh_viewr:r1r1r2r¨Ss
 
 
 
 
 
 zHitlDashboard._close_serialcs|jr| ¡tƒ |¡dSr>)rVr¨rPÚ
closeEvent)r;Úeventr^r1r2r¯cszHitlDashboard.closeEventcCs6tj |dtt ¡ƒd¡\}}|r|j |¡dSdS)Nu选择路径文件u-路径文件 (*.txt *.json);;所有文件 (*))r
Z QFileDialogZgetOpenFileNamerrÚcwdrtr£)r;Ú  file_nameÚ_r1r1r2r‡hsÿzHitlDashboard._browse_pathc
Csº|j ¡ ¡}|stj |dd¡dSztt|ƒƒ}Wnty9}ztj |dd|›¡WYd}~dSd}~ww|sFtj |dd¡dS||_   | 
¡tj  |ddt |ƒ›d¡dS)Nu路径u请先选择路径文件。u加载失败: u未解析到有效路径点。u
已加载 u ä¸ªè·¯å¾„点。) rtr.r r
r©rªr3rrœrKrYÚ informationr))r;Z    file_textZ
new_pointsr¬r1r1r2rˆms"€þ zHitlDashboard._load_pathcCsZ|j ¡ ¡}|stj |dd¡dS|j |¡r#tj |dd¡dStj |dd¡dS)Nu原点u请输入 GGA å­—符串。u原点已更新。u'原点更新失败,请检查格式。)   rxr.r r
r©rªrIZ update_originr´)r;Zggar1r1r2r‰~s zHitlDashboard._update_origincCs^|jd ¡}|jd ¡}|jd ¡}|jd ¡}|j ||||¡tj |dd¡dS)Nrr rr`u位置u已重置仿真状态。)rzÚvaluerIZ reset_stater
r©r´)r;ÚeastÚnorthÚupÚheadingr1r1r2rŠˆs zHitlDashboard._reset_positioncCsØ|js|j dddd¡dSt ¡}d}g}g}|jD]"\}}| |¡| | ¡|r6| || ¡d}q| || ¡q|j    |¡t
  t |ƒdt |ƒdt |ƒt |ƒdt |ƒt |ƒd¡}|j |¡dS)NiöÿÿÿéTFrre)rKrjZ setSceneRectr  rr*ÚmoveToÚlineTor‚ÚsetPathrZQRectFÚminÚmax)r;ÚpathÚfirstZxsZysZpxÚpyZrectr1r1r2rYs"
  @zHitlDashboard._init_scener.rcCs*|j |¡|j ¡ |j ¡ ¡¡dSr>)rZappendPlainTextZverticalScrollBarZsetValueZmaximum)r;r.r1r1r2Ú _append_log¤s zHitlDashboard._append_logcCsÐd} z
|jj ¡\}}Wn
tjyYnGw|dkr ||_n;|dkr?||_|j |j|j f¡t
|jƒdkr>|j  d¡n|dkrG||_ n|dkrR||j |j<n |d kr[| |¡d}q|rf| ¡dSdS)
NFTÚcontrolÚposeiÐrÚstateÚstackÚlog)rJr7rArBrRrSrQr*r¶r·r)ÚpoprTrUÚ    task_namerÃr®)r;Úupdatedr=r/r1r1r2r\¨s4ÿ €
î ÿzHitlDashboard._drain_queuecCsŽg}|jr?|j}|dd|jd›d|jd›d|jd›ddd|jd    ›d
|jd   ›d |jd ›d |jd ›d|jd ›dg
7}|j rn|j   }|ddd|j
d›dd|j d›dd|j ›d|j ›d|jd›dd|j›g7}|jrŒ|j}|dd|j›d|jd›dd|jd ›dg7}|jr¯|dd g7}|j ¡D]}| d!|j›d"|j›d#|j›d$¡qš|s´d%g}|j d& |¡¡| ¡| ¡dS)'Nu位置 (ENU, m):z  E: z+7.3fz  N: z  U: r“u 姿态 (deg):z   Heading: z+7.2fz   Pitch  : z   Roll   : z   Target : (z+6.2fz, r¢u
控制量:z   Forward : z+6.3fz m/sz   Turn    : z rad/sz  PWM     : steer=z
 throttle=z   Freq    : z6.2fú Hzz   Stage   : u算法状态: z   XTE    : z mz degu 堆栈监测:z  z: stack=z  words heap=z BrfÚ
)rSr¶r·r¸Ú heading_degZ    pitch_degZroll_degÚ target_eastÚ target_northrRÚ forward_mpsÚ   turn_rateZ steering_pwmZ throttle_pwmÚfreq_hzÚstagerTZcross_track_errorZheading_error_degrUÚvaluesr*rÊÚstack_high_waterÚheap_free_bytesr}r£ÚjoinÚ_update_scene_itemsÚ_refresh_stack_table)r;Z
info_linesrÅZctrlÚstrDr1r1r2r®ÀsZ      ö 
ù    
ü ÿ zHitlDashboard._refresh_viewc Cs~|j t|jƒ¡t|j ¡ƒD],\}}|j |dt |j   ¡¡|j |dt t
|j ƒ¡¡|j |dt t
|j ƒ¡¡qdS)Nrr r) r~r­r)rUr{rÕZsetItemr
ZQTableWidgetItemrÊrrÖr×)r;ZrowÚstatusr1r1r2rÚïs ýz"HitlDashboard._refresh_stack_tablec
Cs|jr/t ¡}d}|jdd…D]\}}| }|r"| ||¡d}q| ||¡q|j |¡|jr|j}|j}|j  }|j
  |d|ddd¡t   |j¡}t  |¡}t  |¡}   |j |||||| ¡|j  |jd|j ddd¡|j ||¡dSdS)NTi$úÿÿFg333333Ó?rhgš™™™™™É?ri)rQr  rr»r¼rƒr½rSr¶r·r„ZsetRectÚmathZradiansrÎZcosZsinr…ZsetLiner†rÏrÐrkZcenterOn)
r;ÚtrailrÁÚxÚyZscene_yrÅZ heading_radZdxZdyr1r1r2rÙös,   
 
 öz!HitlDashboard._update_scene_items)
rIrrJr4rKrrLrMrNrM)rLrMrNrM)NN)r r¡)r.r)rErFrGr<rXr‹rrŽrŒr¨r¯r‡rˆr‰rŠrYrÃr\r®rÚrÙÚ __classcell__r1r1r^r2rHds&
 ~
 
 
/rHúargparse.NamespacecCsÚtjdd}|jdtdd|jdtdd|jdtd   d|jd
ttd d d |jdttddd |jdttddd |jdttdd |jdt   t
dd |jdt t dd |jddd|jdddd |  ¡S)!NuHITL ä»¿çœŸå™¨ GUI)Z descriptionz--uart2uSTM32 UART2 ä¸²å£å·)ÚdefaultÚhelpz--uart5uSTM32 UART5 ä¸²å£å·z--originu
GGA åŽŸç‚¹z--eastru 初始 E (m))Útyperãräz--northr u 初始 N (m)z--upru 初始 U (m)z   --headingu初始航向 (deg)z
--gps-bauduUART2 æ³¢ç‰¹çއz
--log-bauduUART5 æ³¢ç‰¹çއz--pathu初始路径文件 (.json/.txt))räz--no-guiZ
store_trueu"不启动 Qt GUI,仅打印数据)Úactionrä) ÚargparseZArgumentParserZ add_argumentÚDEFAULT_UART2_PORTÚDEFAULT_UART5_PORTrwr+Ú DEFAULT_ENUÚDEFAULT_HEADING_DEGÚintÚDEFAULT_UART2_BAUDÚDEFAULT_UART5_BAUDÚ
parse_args)Zparserr1r1r2rïs rïc
 
s†tƒ}t|j|j|j|j|j|jf|j|j   |j
d}t t ƒ}t ||d}tƒ‰‡fdd„‰‡fdd„|_‡fdd„|_‡fdd„|_‡fd d„|_‡fd
d„|_g}|jrxz tt|jƒƒ}Wntyw}z td |›ƒWYd}~nd}~wwtdus|jrïzbzE| ¡td ƒ  z ˆjjdd\}}Wn
tjyŸYq‰w|dkr©t|ƒn|dkrÅtd|j›d|j d›d|j!d›d|j"d›d   ƒqŠt#yÒtdƒYnwW| $¡| %¡tdƒdS| $¡| %¡tdƒwtj& '¡pøt &g¡}t(|ˆ||j|jƒ} |jr|   j) *|j¡|    +¡z| ,¡W| j-r |  .¡| $¡| %¡tdƒdS| j-r6|  .¡| $¡| %¡tdƒw)N)ržrŸZ
origin_ggaZ initial_enuZinitial_heading_degZ gps_baudrateZ log_baudrate)Ú
run_loggercsˆ ||¡dSr>©r@)r=r/)rJr1r2r@/szmain.<locals>.publishcó
ˆd|ƒS)NrÄr1©rÜrñr1r2Ú<lambda>2ó
zmain.<locals>.<lambda>crò)NrÅr1rórñr1r2rô3rõcrò)NrÆr1rórñr1r2rô4rõcrò)NrÇr1rórñr1r2rô5rõcrò)NrÈr1)Úlinerñr1r2rô6rõuARN] è·¯å¾„加载失败: uF[INFO] HITL ä»¿çœŸå™¨å·²å¯åЍ (无 GUI æ¨¡å¼)。按 Ctrl+C é€€å‡ºã€‚Trg)rZrÈrÄz [CTRL] stage=z F=z+.2fz m/s T=z  rad/s freq=z.2frÌu
[INFO] ç”¨æˆ·ä¸­æ–­ã€‚u[INFO] ä»¿çœŸå™¨å·²åœæ­¢ã€‚)/rïrr¤r§Úoriginr¶r·r¸r¹Zgps_baudZlog_baudrÚ RUN_LOG_PATHrr4Zon_control_statusZon_pose_statusZon_state_statusZon_stack_statusZon_logrÀr3rrœÚprintr
Zno_guir]r7ÚgetrBrÔrÑrÒrÓÚKeyboardInterruptr«ÚcloseZ QApplicationÚinstancerHrtr£ZshowZexec_rVr¨)
ÚargsZcfgrðZsimrKr¬r=r/ZappZ dashboardr1)rJr@r2Úmainsœù
 €ÿÿ
 
ÿÿþÿø ÿý
 
 ü
rÿÚ__main__)rrrr)rrâ)3Ú__doc__Z
__future__rrçr$rÝr7Úsysr8ZpathlibrÚtypingrrrrZPyQt5rr    r
rœZ serial.toolsr Z    protocolsr rrrrIrrrrèrérwrêrërírîÚ__file__ZresolveÚparentrør3r4Z QMainWindowrHrïrÿrEÚexitr1r1r1r2Ú<module>sR   ÿ ÿ

.Hÿ