o
|
9x$i ã @ sÚ d Z ddlmZ ddlZddlZddlZddlZddlZddlZddl m
|
Z
|
ddl m Z m Z mZmZ z ddlmZmZmZ W n eyI dZY nw zddlmZ W n ey] dZY nw dd lmZmZmZmZm Z dd
|
l m Z m Z m Z d Z!d Z"d Z#dZ$dZ%dZ&dZ'e
|
e( )¡ j*Z+e+d Z,e+d Z-e- .d¡Z/e+d Z0d:ddZ1d;d d Z2d<d=d$d%Z3d>d'd(Z4d?d*d+Z5G d,d- d-Z6G d.d/ d/ej7Z8G d0d1 d1Z9G d2d3 d3ej:Z;d@d5d6Z<d7d8 Z=e>d9krëe ?e= ¡ dS dS )Au]
|
HITL 仿çå¨ GUIï¼å·¦ä¾§å°å¾æ¾ç¤ºè·¯å¾/车è¾ï¼å³ä¾§æ¾ç¤ºç¶æä¸æ§å¶ä¿¡æ¯ã
|
é )Ú annotationsN)ÚPath)ÚDictÚListÚOptionalÚTuple)ÚQtCoreÚQtGuiÚ QtWidgets)Ú
|
list_portsé )Ú ControlStatusÚ
|
PoseStatusÚ StackStatusÚ StateStatusÚ nmea_checksum)Ú
|
HitlConfigÚ HitlSimulatorÚ RunLoggerZCOM11ZCOM17zX$GNGGA,080112.000,3949.8105069,N,11616.6876082,E,4,44,0.42,48.502,M,-6.684,M,1.0,0409*73)ç r r r i  i z
|
runlog.txtzpath_text_input.txtz.czdashboard_state.jsonÚ path_filer ÚreturnúList[Tuple[float, float]]c C s" | ¡ s
|
tt| | jdd ¡ }|sg S g }| j ¡ dks$| d¡rZz)t |¡}|D ] }t
|
|t t frJt |dkrJ| t|d t|d f¡ q,|W S tjyY g Y S w | d¡D ]/}| ¡ }|shq_| d ¡}t |dk rtq_z| t|d t|d f¡ W q_ ty Y q_w |S )
|
Núutf-8©Úencodingz.jsonú[é r r ú;ú,)ÚexistsÚFileNotFoundErrorÚstrÚ read_textÚstripÚsuffixÚlowerÚ
|
startswithÚjsonÚloadsÚ
|
isinstanceÚlistÚtupleÚlenÚappendÚfloatZJSONDecodeErrorÚsplitÚ
|
ValueError)r ÚtextÚpointsÚdataÚptÚtokenÚparts© r8 ú/E:\GIT\Lawnmower_STM32H7\python\hitl\run_sim.pyÚload_path_points+ s<
|
ÿ
|
" ÿr: r2 r" c C s | dd¡ d¡}g }|D ]7}| ¡ }|sq | d¡}t|dk r"q zt|d ¡ }t|d ¡ }| ||f¡ W q tyD Y q w |S )NÚ
|
ú r r r r r )Úreplacer0 r$ r- r/ r. r1 )r2 Útokensr3 r6 r7 ÚxÚyr8 r8 r9 Úparse_path_textI s"
|
ÿrA r r3 ÚdecimalsÚintc C s | sdS d| d}dg}t | D ]&\}\}}|t| d k r!dnd}| d| |¡ d | |¡ d
|
| ¡ q| d ¡ d |¡S ) Nz%const float g_motion_path_xy[] = {};
|
z{:.zf}z"const float g_motion_path_xy[] = {r r Ú z zf, Úfz};r; )Ú enumerater- r. ÚformatÚjoin)r3 rB ZfmtÚlinesÚidxr? r@ Zcommar8 r8 r9 Úformat_path_for_mcu\ s *
|
|
rK Úggac
|
C s¶ | pd ¡ }|s
|
dS | d¡d }dd | d¡D }t|dk r#dS |d }|d }|d
|
}|d }t|d krA|d rA|d nd}| d| d| d| } |rY| d| 7 } | S ) NrD Ú*r c S s g | ]}| ¡ qS r8 ©r$ ©Ú.0Úpr8 r8 r9 Ú
|
<listcomp>m s z'gga_to_short_origin.<locals>.<listcomp>r é r é é é é )r$ r0 r- )
|
rL r2 Úbodyr7 ZlatÚlat_dirZlonÚlon_dirÚaltZshortr8 r8 r9 Úgga_to_short_originh s r\ Útemplatec C sL dd | dd¡ d¡D }t|dk rtd|d }|d ¡ }|d
|
}|d ¡ }t|d kr6|d nd }|d vs@|dvrDtd|pGt}d} | d¡rW|d d
}d} | d¡d }
|
|
|
d¡} t| dkrt| d¡ t| dksi| d pyd| d< || d
|
< || d < || d< || d < |d ur|| d< d | ¡}
|
t |
|
} |
|
d| | S )Nc S s g | ]
|
}| ¡ r| ¡ qS r8 rN )rP Útr8 r8 r9 rR | s z/build_gga_from_short_origin.<locals>.<listcomp>r; r< r rU u) åç¹æ ¼å¼åºä¸º 'lat,N,lon,E[,alt]'ãr r r rT rV )ÚNÚS)ÚEÚWu æ¹åå¿
é¡»æ¯ N/S/E/WãrD z
|
éþÿÿÿrM é z$GNGGArW )
|
r= r0 r- r1 ÚupperÚDEFAULT_ORIGIN_GGAÚendswithr. rH r ) r2 r] r> Zlat_dmrY Zlon_dmrZ r[ ÚbaseÚnewlinerX ZfieldsZchecksumr8 r8 r9 Úbuild_gga_from_short_origin{ s<
|
|
|
ÿ
|
rj c @ s. e Zd Zdd Zd ddZdd Zd d
|
Zd S ) Ú HitlGuiBridgec C s t jdd| _ t ¡ | _d S )NéÈ )Úmaxsize)ÚqueueZQueueÚ threadingZLockÚ_lock©Úselfr8 r8 r9 Ú__init__ s zHitlGuiBridge.__init__Úkindr" c C s: | j | ||f¡ W d d S 1 sw Y d S ©N)rp Ú_enqueue)rr rt r4 r8 r8 r9 Úpublish¡ s "ÿzHitlGuiBridge.publishc C s | j 5 | j ¡ s(z| j ¡ W n
|
tjy Y nw | j ¡ r W d d S W d d S W d d S 1 s;w Y d S ru )rp rn ZemptyÚ
|
get_nowaitÚEmptyrq r8 r8 r9 Úreset¥ s
|
ÿ ýÿÿ"ûzHitlGuiBridge.resetc C sv z | j |¡ W d S t jy: z| j ¡ W n
|
t jy" Y nw z
|
| j |¡ W Y d S t jy9 Y Y d S w w ru )rn Z
|
put_nowaitZFullrx ry )rr Úitemr8 r8 r9 rv s ÿÿùzHitlGuiBridge._enqueueN)rt r" )Ú__name__Ú
|
__module__Ú __qualname__rs rw rz rv r8 r8 r8 r9 rk s
|
|
rk c sh e Zd Ze ee¡Z fddZdddZd fdd Z d fd
|
d Z
|
dddZ d fdd Z Z S )ÚZoomableGraphicsViewc sz t j|i |¤ | tjjd¡ | tjj ¡ |
|
tjj ¡ | tjj ¡ | tjj¡ d| _d| _t ¡ | _d | _d S )NTgffffffò?F)Úsuperrs Z setRenderHintr ZQPainterZ AntialiasingZsetTransformationAnchorr
|
Ú QGraphicsViewZAnchorUnderMouseZsetResizeAnchorZ setDragModeZNoDragÚ setCursorr ÚQtÚOpenHandCursorÚ _zoom_factorÚ_panningZQPointÚ _pan_last_posÚ on_user_pan)rr ÚargsÚkwargs©Ú __class__r8 r9 rs ¿ s
|
|
z ZoomableGraphicsView.__init__ÚeventúQtGui.QWheelEventc C s4 | ¡ ¡ }|dkr | jnd| j }| ||¡ d S )Nr ç ð?)Z
|
angleDeltar@ r
Zscale)rr r ÚdeltaZ scale_factorr8 r8 r9 Ú
|
wheelEventË s z ZoomableGraphicsView.wheelEventúQtGui.QMouseEventc sJ | ¡ tjjkr d| _| ¡ | _| tjj¡ | ¡ d S t
|
|¡ d S )NT) Úbuttonr r Ú
|
LeftButtonr Úposr r ZClosedHandCursorÚacceptr ÚmousePressEvent©rr r r r8 r9 r Ð s
|
z$ZoomableGraphicsView.mousePressEventc s | j r2| | ¡ ¡}| | j¡}|| }| ¡ | _| | ¡ | ¡ ¡ | jr,| ¡ | ¡ d S | j j
|
| | ¡ ¡ t |¡ d S ru )r Ú
|
mapToScener r Ú translater? r@ r r ÚsceneMouseMovedZemitÚ _scene_coordsr ÚmouseMoveEvent)rr r Ú scene_posZlast_scene_posr r r8 r9 r Ù s
|
z#ZoomableGraphicsView.mouseMoveEventÚview_posú QtCore.QPointr úTuple[float, float]c C s | |¡}| ¡ | ¡ fS ru )r r? r@ )rr r r r8 r8 r9 r ç s
|
z"ZoomableGraphicsView._scene_coordsc sF | ¡ tjjkr| jrd| _| tjj¡ | ¡ d S t |¡ d S )NF)
|
r r r r r r r r r ÚmouseReleaseEventr r r8 r9 r¢ ë s z&ZoomableGraphicsView.mouseReleaseEvent)r r )r r )r r r r¡ )r| r} r~ r Z
|
pyqtSignalr/ r rs r r r r r¢ Ú __classcell__r8 r8 r r9 r » s
|
|
r c @ sN e Zd Zd ddZdd Zdd Zd d d Zd ddZd ddZd ddZ dS )!ÚDashboardStateStoreÚpathr c C s t || _i | _| ¡ d S ru )r r¥ r4 Ú_load)rr r¥ r8 r8 r9 rs õ s
|
z DashboardStateStore.__init__c C sL | j ¡ s
|
i | _d S zt | j jdd¡| _W d S ty% i | _Y d S w )Nr r )r¥ r r4 r( r) r# Ú Exceptionrq r8 r8 r9 r¦ ú s
|
ÿzDashboardStateStore._loadc C s: z| j jtj| jddddd W d S ty Y d S w )NFr )Z ensure_asciiZindentr r )r¥ Ú
|
write_textr( Údumpsr4 r§ rq r8 r8 r9 Úsave s
|
$ ÿzDashboardStateStore.saver ú+Optional[Tuple[float, float, float, float]]c C sT | j d¡}t|tr(t|dkr(z
|
tdd |D W S ttfy' Y d S w d S )NÚposerU c s s | ]}t |V qd S ru )r/ )rP Úvr8 r8 r9 Ú <genexpr> s z/DashboardStateStore.get_pose.<locals>.<genexpr>)r4 Úgetr* r+ r- r, Ú TypeErrorr1 ©rr r¬ r8 r8 r9 Úget_pose s ÿz DashboardStateStore.get_poseÚeastr/ ÚnorthÚupÚheadingc C s ||||g| j d< | ¡ d S ©Nr¬ ©r4 rª ©rr r³ r´ rµ r¶ r8 r8 r9 Úset_pose s z DashboardStateStore.set_poseú Optional[str]c C s | j d¡}t|tr |S d S ©NÚorigin)r4 r¯ r* r" )rr r½ r8 r8 r9 Ú
|
get_origin s
|
z DashboardStateStore.get_originÚ
|
origin_strr" c C s || j d< | ¡ d S r¼ r¸ )rr r¿ r8 r8 r9 Ú
|
set_origin s
|
z DashboardStateStore.set_originN)r¥ r )r r« )r³ r/ r´ r/ rµ r/ r¶ r/ )r r» )r¿ r" )
|
r| r} r~ rs r¦ rª r² rº r¾ rÀ r8 r8 r8 r9 r¤ ô s
|
|
|
|
r¤ c sô e Zd ZdF fd
|
d ZdGd d Zdd ZdHddZdIddZdJdGddZdKd d Z d d Z
|
dLd"d#Z dMd&d'Z d(d) Z d*d+ Zd,d- Zd.d/ Zd0d1 Zd2d3 Z fd4d5Zd6d7 Zd8d9 Zd:d; ZdMd<d=Zd>d? Zd@dA ZdBdC ZdDdE Z Z S )NÚ HitlDashboardÚ simulatorr Úbridgerk Ú path_pointsr Ú initial_uart2r» Ú initial_uart5c sä t ¡ || _|| _t|pg | _g | _d | _d | _d | _ i | _
|
d| _ | d¡ | dd¡ d| _g | _tt| _| ||¡ | ¡ | j ¡ ¡ sT| jrT| | j¡ | ¡ | ¡ t | ¡| _ | j j | j ¡ | j !d¡ d S )NTu HITL 仿çç¶æé¢æ¿i iÐ FéP )"r rs r rà r+ rÄ Ú trail_pointsÚcontrol_statusÚ pose_statusÚ state_statusÚ stack_statusÚ _auto_followZsetWindowTitleZresizeÚ _serial_openÚ _last_portsr¤ Ú
|
STATE_FILEÚ state_storeÚ _build_uiÚ_load_saved_path_textÚ path_textÚ toPlainTextr$ Ú _update_path_text_from_pointsÚ_load_saved_poseÚ _init_scener ZQTimerZtimerÚtimeoutÚconnectÚ _drain_queueÚstart)rr r rà rÄ rÅ rÆ r r8 r9 rs " s0
|
|
|
zHitlDashboard.__init__c C sô t ¡ }t |¡}| |¡ t | ¡| _t ¡ }t |¡}| dddd¡ | d¡ t | j| _
|
| j | j
|
_ | j
|
j | j¡ |j| j
|
dd t d¡| _| j d¡ | jj | j¡ |j| jtjjd t d¡| _| j d ¡ |j| jtjj d |j|d
|
d t d u| _ t ¡ }t |¡}| d ddd¡ |j|d d t d ¡} t | ¡}
|
t !¡ | _"t !¡ | _#t $d¡| _%t $d¡| _&t d¡| _'| j' d¡ |
|
t d¡dd¡ |
|
| j"dd¡ |
|
t d¡dd¡ |
|
| j#dd¡ |
|
| j%dd d d¡ |
|
| j&d ddd
|
¡ |
|
| j'd
|
ddd
|
¡ | | ¡ t d u r'| j% (d¡ | j" )d¡ | j# )d¡ t d¡} t | ¡} t *¡ | _+| j+ ,d¡ | j+ -t.j/j0¡ t ¡ } t $d¡| _1t $d¡| _2| | j1¡ | | j2¡ | | j+¡ | 3| ¡ | | ¡ t d¡}t |¡}t4| j5j6j7}t 8|¡| _9| j9 ,d¡ t $d¡| _:| t d ¡dddd ¡ | | j9dd dd ¡ | | j:dd¡ g d ¢}g | _;t<|D ]N\}}t =¡ }|d
|
k rÕ| >d d ¡ | ?d
|
¡ n | >d d!¡ | ?d ¡ | t |¡d|d |d d ¡ | |d|d |d d d ¡ | j; @|¡ q¼t $d"¡| _A| | jAd
|
ddd¡ | |¡ t d#¡}t |¡}t d$¡| _B| jB CtjjDtjjB ¡ | jB d%¡ t E¡ }| Fd¡ | G| jB¡ | |¡ |j|dd t d&¡}t |¡}t Hdd
|
¡| _I| jI Jg d'¢¡ | jI K¡ Ld¡ | | jI¡ | |¡ t d(¡}t |¡}t *¡ | _M| jM Nd¡ | | jM¡ |j|dd | j Ot. P¡ t. Qt. Rd)¡d¡¡| _S| j Ot. P¡ t. Qt. Rd*¡d¡¡| _Td+| _U| j V| jU | jU | jUd | jUd t. Qt. Rd,¡d¡t. Wt. Rd-¡¡¡| _Xt. Qt. Rd.¡d¡} | YtjjZ¡ | j Ot. P¡ | t. Wt. Rd.¡¡¡| _[d/| _\| j V| j\ | j\ | j\d | j\d t. Qt. Rd0¡¡t. Wt. Rd0¡¡¡| _]| j1j^ | j_¡ | j2j^ | j`¡ | j:j^ | ja¡ | jAj^ | jb¡ | j%j^ | jc¡ | j&j^ | jd¡ | c||¡ | ed¡ | f¡ d S )1Nr rU r )Zstretchu èªå¨è·é车è¾T)Z alignmentzE: 0.00 N: 0.00zQcolor:#222; background:rgba(255,255,255,190); padding:2px 6px; border-radius:4px;rT é r u 串å£è®¾ç½®u å·æ°õ æå¼ä¸²å£õ ç¶æ: æªè¿æ¥ú color: gray;zUART2:zUART5:Fu è·¯å¾è½¨è¿¹u) 示ä¾ï¼23.36,-42.93;29.26,-52.99; ... ;u å è½½å°å°å¾u å¯¼åº MCU æ ¼å¼u åç¹/èµ·ç¹z 3949.9120,N,11616.8544,E[,alt]u æ´æ°åç¹u åæ (lat,N,lon,E):)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 串å£æ¥å¿ZgrayZblueç Ð?z#d11d29z#ff4b5cz#ff8c00g{®Gáz´?Zgreen)gr
|
ZQWidgetZ QHBoxLayoutZsetCentralWidgetZQGraphicsSceneÚsceneZ QVBoxLayoutZsetContentsMarginsZ
|
setSpacingr ÚviewÚ_handle_user_panr r rÚ Ú_update_mouse_posZ addWidgetZ QCheckBoxÚfollow_checkboxÚ
|
setCheckedZtoggledÚ_on_follow_toggledr r Z AlignLeftZQLabelÚmouse_pos_labelÚ setStyleSheetZ
|
AlignRightr Ú_port_refresh_supportedZ QGroupBoxZ QGridLayoutZ QComboBoxÚ port2_comboÚ port5_comboZ QPushButtonÚport_refresh_btnÚserial_toggle_btnÚserial_status_labelÚ
|
setEnabledZ setEditableZQPlainTextEditrÔ ZsetPlaceholderTextZsetWordWrapModer Z QTextOptionZNoWrapZ path_load_btnZpath_export_btnZ addLayoutr\ r ÚconfigÚ
|
origin_ggaZ QLineEditÚ origin_textÚ
|
origin_btnÚpos_spinrF ZQDoubleSpinBoxZsetRangeZ setDecimalsr. Úreset_pose_btnÚ
|
info_labelZ setAlignmentZAlignTopZ QScrollAreaZsetWidgetResizableZ setWidgetZ QTableWidgetÚ stack_tableZsetHorizontalHeaderLabelsZhorizontalHeaderZsetStretchLastSectionÚlog_viewÚ setReadOnlyZaddPathÚ QPainterPathZQPenZQColorÚ path_itemÚ
|
trail_itemÚ
|
robot_sizeZ
|
addEllipseZQBrushÚ
|
robot_itemZ setJoinStyleZ RoundJoinÚ heading_itemÚ target_radiusÚ target_itemZclickedÚ_load_path_from_textÚ_export_path_to_mcuÚ_update_originÚ_reset_positionÚ_refresh_serial_portsÚ_toggle_serialÚ_set_controls_enabledÚ_update_serial_ui) rr rÅ rÆ ZcentralZlayoutZ
|
left_panelZ left_layoutZ right_panelZ right_layoutZ serial_groupZ serial_layoutZ
|
path_groupZ path_layoutZ
|
button_rowZ origin_groupZgridZdefault_origin_shortZlabelsrJ ZlabelZspinZ status_groupZ status_layoutZ info_scrollZ stack_groupZ stack_layoutZ log_groupZ
|
log_layoutZ arrow_penr8 r8 r9 rÒ F s
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
$"
|
|
|
|
|
|
|
|
|
|
|
""úýú
|
zHitlDashboard._build_uic C s8 | j r| j d¡ | j d¡ | j d¡ d| _ d S d S )NTF)rÍ rç Ú blockSignalsrè rq r8 r8 r9 rå ï s
|
üz HitlDashboard._handle_user_panÚcheckedÚboolc C s4 || _ |r| jr| j | jj| jj ¡ d S d S d S ru )rÍ rÊ rä ÚcenterOnr³ r´ )rr r r8 r8 r9 ré ö s
|
ÿz HitlDashboard._on_follow_toggledr³ r/ Úscene_yc C s& | }| j d|dd|d¡ d S )NzE: z .2fú N: )rê ÚsetText)rr r³ r r´ r8 r8 r9 ræ û s z HitlDashboard._update_mouse_posNc s g t rz dd t ¡ D W n ty g Y nw |p#| jjjp#d}|p,| jjjp,d}d
|
fdd }|| j| || j| | _ d S ) Nc S s g | ]}|j qS r8 )ZdevicerO r8 r8 r9 rR s z7HitlDashboard._refresh_serial_ports.<locals>.<listcomp>rD ÚcomboúQtWidgets.QComboBoxÚsavedr" c st | ¡ }| d¡ | ¡ r| ¡ |r | vr | |¡ |p"|}|r3| |¡}|dkr3| |¡ | d¡ d S )NTr F)Ú currentTextr ÚclearZaddItemsZaddItemZfindTextZsetCurrentIndex)r r ZcurrentÚtargetrJ ©Zportsr8 r9 Ú_fill s
|
|
|
|
|
z2HitlDashboard._refresh_serial_ports.<locals>._fill)r r r r" )
|
r Zcomportsr§ r ró Ú
|
uart2_portÚ
|
uart5_portrí rî rÏ )rr rÅ rÆ Z saved_uart2Z saved_uart5r r8 r r9 r ÿ s ÿ
|
z#HitlDashboard._refresh_serial_portsÚenabledc C s( | j |¡ | j d¡ | j d¡ d S )NFT)rø rò rõ rü rö )rr r r8 r8 r9 r s z#HitlDashboard._set_controls_enabledc C sZ t ¡ r)zt jdd}W n ty d}Y nw | j |¡ t|}|r+|| _d S d S d S )Nr r rD )ÚPATH_TEXT_FILEr r# r§ rÔ Ú setPlainTextrA rÄ ©rr r2 r3 r8 r8 r9 rÓ ! s ÿ
|
øz#HitlDashboard._load_saved_path_textr3 c C s: |s | j ¡ d S d dd |D ¡d }| j |¡ d S )Nr c s s& | ]\}}|d d|d V qdS )ú.2fr Nr8 )rP r? r@ r8 r8 r9 r® 0 s $ z>HitlDashboard._update_path_text_from_points.<locals>.<genexpr>)rÔ r rH r )rr r3 r2 r8 r8 r9 rÖ , s
|
|
z+HitlDashboard._update_path_text_from_pointsr2 r" c
|
C sV z t j| ¡ dd W d S ty* } ztj | dd| ¡ W Y d }~d S d }~ww )Nr r õ è·¯å¾u ä¿åè·¯å¾ææ¬å¤±è´¥: )r r¨ r$ r§ r
|
Ú QMessageBoxÚwarning)rr r2 Úexcr8 r8 r9 Ú_save_path_text3 s $ÿz HitlDashboard._save_path_textc C sf | j ¡ }|r1| jd |d ¡ | jd |d ¡ | jd |d ¡ | jd |d ¡ d S d S )Nr r r rT )rÑ r² r÷ ÚsetValuer± r8 r8 r9 r× 9 s
|
üz HitlDashboard._load_saved_posec C sx | j ¡ ¡ }t|}|stj | dd¡ d S || _| j ¡ |
|
|¡ | ¡ | ¡ tj | ddt| d¡ d S )Nr# u9 æªè§£æå°ææç轨迹ç¹ï¼è¯·æ£æ¥è¾å
¥æ ¼å¼ãu
|
å·²å è½½ u 个路å¾ç¹ã)rÔ rÕ r$ rA r
|
r$ r% rÄ rÈ r r' rØ Ú _refresh_viewÚ informationr- r! r8 r8 r9 r A s
|
|
z"HitlDashboard._load_path_from_textc
|
C s´ | j ¡ ¡ }t|}|stj | dd¡ d S t|}tj ¡ }|
|
|¡ z t j |dd W n t yK } ztj | dd| ¡ W Y d }~nd }~ww tj | ddtt ¡ d S )Nu 导åºu3 没æå¯å¯¼åºçè·¯å¾ç¹ï¼è¯·å
å 载路å¾ãr r u åå
¥æ件失败: u# å·²å¤å¶å°åªè´´æ¿ï¼å¹¶åå
¥:
|
)rÔ rÕ r$ rA r
|
r$ r% rK Ú QApplicationÚ clipboardr ÚPATH_EXPORT_FILEr¨ r§ r* r" )rr r2 r3 Z formattedr, r& r8 r8 r9 r N s
|
|
"ÿ z!HitlDashboard._export_path_to_mcuc C sÊ | 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 ¡ d S d S ) Nu å
³é串å£rÞ u æªç¥u æ u ç¶æ: å·²è¿æ¥ (UART2=z, UART5=ú)z color: green;rß rà )rð r rÎ r Úuart2ÚportÚlog_uartrñ rë rí rò Z
|
isEditablerî rì rï )rr r/ Úuart5Z enable_portsr8 r8 r9 r ] s ÿz HitlDashboard._update_serial_uic
|
C sò | j r | ¡ d S | j ¡ ¡ }|s tj | dd¡ d S | j ¡ ¡ }|p&d }|| j j
|
_ || j j
|
_ || j j _|| j j_z| j ¡ W n# tye } z| j ¡ tj | dd| ¡ W Y d }~d S d }~ww d| _ | j ¡ | d¡ | ¡ d S )Nu 串å£u 请å
éæ© UART2 端å£ãu æå¼ä¸²å£å¤±è´¥: T)rÎ Ú _close_serialrí r r$ r
|
r$ r% rî r ró r r r/ r0 r1 rÜ r§ ÚstopZcriticalrà rz r r )rr r/ Z
|
uart5_textr2 r& r8 r8 r9 r
|
m s2
|
|
|
|
|
ý
|
|
z HitlDashboard._toggle_serialc C s | j ¡ | j ¡ | j ¡ d | _d | _d | _| j ¡ | j
|
¡ | j d¡ | j d¡ d| _| d¡ | ¡ | ¡ d S )Nrá r F)r r4 rà rz rÈ r rÉ rÊ rË rÌ rû rù r rú Ú setRowCountrÎ r r r) rq r8 r8 r9 r3 s
|
|
|
|
|
|
zHitlDashboard._close_serialc s | j r| ¡ t |¡ d S ru )rÎ r3 r Ú
|
closeEventr r r8 r9 r6 s zHitlDashboard.closeEventc
|
C sÆ | j ¡ ¡ }|stj | dd¡ d S | jjjpt }zt
|
||}W n t y? } ztj | dd| ¡ W Y d }~d S d }~ww | j |¡ || jj_| j t|¡ | j |¡ tj | dd¡ d S )Nu åç¹u, 请è¾å
¥åæ ï¼æ ¼å¼ï¼lat,N,lon,E[,alt]u æ ¼å¼é误: u åç¹å·²æ´æ°ã)rõ r2 r$ r
|
r$ r% r ró rô rf rj r1 Z update_originr r\ rÑ rÀ r* )rr r2 r] rL r& r8 r8 r9 r s" þ
|
z HitlDashboard._update_originc C sp | j d ¡ }| j d ¡ }| j d ¡ }| j d ¡ }| j ||||¡ | j ||||¡ tj | dd¡ d S )Nr r r rT u ä½ç½®u å·²é置仿çç¶æã) r÷ Úvaluer Z reset_staterÑ rº r
|
r$ r* r¹ r8 r8 r9 r ¬ s z HitlDashboard._reset_positionc C s$ | j s| j dddd¡ d S t ¡ }d}g }g }| j D ]"\}}| |¡ | | ¡ |r6| || ¡ d}q| || ¡ q| j |¡ t
|
|}t |}t
|
|} t |}
|
|| } |
|
| } t d| d } t d| d }t || | | | dkrx| nd| d | dkr| nd|d ¡}| j |¡ d S )
|
Niÿÿÿiô TFg $@g è?r r g @)rÄ rã Z setSceneRectr rý r. ÚmoveToÚlineTorþ ÚsetPathÚminÚmaxr ZQRectF)rr r¥ ÚfirstZxsZysZpxÚpyZmin_xZmax_xZmin_yZmax_yÚwidthZheightZpad_xZpad_yZ
|
scene_rectr8 r8 r9 rØ µ s<
|
üzHitlDashboard._init_scenec C s* | j |¡ | j ¡ | j ¡ ¡ ¡ d S ru )rû ZappendPlainTextZverticalScrollBarr( Zmaximum)rr r2 r8 r8 r9 Ú _append_logÖ s zHitlDashboard._append_logc C sÐ d} z
|
| j j ¡ \}}W n
|
tjy Y nGw |dkr || _n;|dkr?|| _| j |j|j f¡ t
|
| jdkr>| j d¡ n |dkrG|| _ n|dkrR|| j |j< n |d kr[| |¡ d}q|rf| ¡ d S d S )
|
NFTÚcontrolr¬ iÐ r ÚstateÚstackÚlog)rà rn rx ry rÉ rÊ rÈ r. r³ r´ r- ÚpoprË rÌ Ú task_namer@ r) )rr Úupdatedrt r4 r8 r8 r9 rÛ Ú s4 ÿ
|
î ÿzHitlDashboard._drain_queuec C s g }| j r?| j }|dd|jdd|jdd|jdddd|jd d
|
|jd d |jd d |jd d|jd dg
|
7 }| j rn| j }|ddd|j
|
ddd|j ddd|j d|j d|jddd|j g7 }| jr| j}|dd |j d |jdd d|jd d g7 }| jr¯|dd g7 }| j ¡ D ]}| d!|j d"|j d#|j d$¡ q|s´d%g}| j d& |¡¡ | ¡ | ¡ d S )'Nu ä½ç½® (ENU, m):z E: z+7.3fr z U: rD 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 Brá r; ) rÊ r³ r´ rµ Ú heading_degZ pitch_degZroll_degÚ target_eastÚ target_northrÉ Ú forward_mpsÚ turn_rateZ steering_pwmZ throttle_pwmÚfreq_hzÚstagerË Zcross_track_errorZheading_error_degrÌ Úvaluesr. rF Ústack_high_waterÚheap_free_bytesrù r rH Ú_update_scene_itemsÚ_refresh_stack_table)rr Z
|
info_linesr¬ ZctrlÚstr{ r8 r8 r9 r) ò sZ ö
|
ù
|
ü ÿ zHitlDashboard._refresh_viewc C s~ | j t| j¡ t| j ¡ D ],\}}| j |dt |j ¡¡ | j |dt t
|
|j ¡¡ | j |dt t
|
|j ¡¡ qd S )Nr r r ) rú r5 r- rÌ rF rP ZsetItemr
|
ZQTableWidgetItemrF r" rQ rR )rr ZrowÚstatusr8 r8 r9 rT ! s ýz"HitlDashboard._refresh_stack_tablec
|
C s | j r/t ¡ }d}| j dd
D ]\}}| }|r"| ||¡ d}q| ||¡ q| j |¡ | jrD| j}|j}|j }| j
|
}| j || || |d |d ¡ t d|j ¡}t |¡} t |¡ }
|
|
|
} | } d} d}d}d }d
|
}|| | ||
|
| f}|d | | |d |
|
| f}|d | | |d |
|
| f}|d | | |d | | f|d | | |d | | f|d | | |d | | f||d | | |d | | f|d | | |d | | f|d | | |d | | fg}t ¡ }|j|d |d d
D ]}|j| q | ¡ | j |¡ | j}| j |j| |j | |d |d ¡ | jrF| j ||¡ d S d S d S ) NTi$úÿÿFr g V@râ g333333ë?gìQ¸
ë±?gÍÌÌÌÌÌÜ?gìQ¸
ëÑ?r r )rÈ r rý r8 r9 rÿ r: rÊ r³ r´ r r ZsetRectÚmathZradiansrI ZcosZsinZ closeSubpathr r r rJ rK rÍ rä r )rr Útrailr= r? r@ r r¬ ÚsizeZ heading_radZdir_xZdir_yZperp_xZperp_yÚoffsetZ shaft_lengthZshaft_half_widthZ head_lengthZhead_half_widthrÜ Z shaft_endZtipr3 Z
|
arrow_pathr5 Z target_sizer8 r8 r9 rS ( s "
|
|
|
þþþþþþþþþê
|
ü²Mz!HitlDashboard._update_scene_items)
|
r r rà rk rÄ r rÅ r» rÆ r» )rÅ r» rÆ r» )r r )r³ r/ r r/ )NN)r r )r3 r )r2 r" ) r| r} r~ rs rÒ rå ré ræ r r rÓ rÖ r' r× r r r r
|
r3 r6 r r rØ r@ rÛ r) rT rS r£ r8 r8 r r9 rÁ ! s6
|
$ *
|
|
|
|
|
|
!/rÁ úargparse.Namespacec C sÚ t jdd} | jdtdd | jdtdd | jdtd d | jd
|
ttd d d | jdttd dd | jdttd dd | jdttdd | jdt t
|
dd | jdt t dd | jddd | jd d d d | ¡ S )!Nu HITL 仿çå¨ GUI)Z descriptionz--uart2u STM32 UART2 串å£å·)ÚdefaultÚhelpz--uart5u STM32 UART5 串å£å·z--originu
|
GGA åç¹z--eastr u åå§ E (m))Útyper\ r] z--northr u åå§ N (m)z--upr u åå§ U (m)z --headingu åå§èªå (deg)z
|
--gps-baudu UART2 æ³¢ç¹çz
|
--log-baudu UART5 æ³¢ç¹çz--pathu åå§è·¯å¾æ件 (.json/.txt))r] z--no-guiZ
|
store_trueu" ä¸å¯å¨ Qt GUIï¼ä»
æå°æ°æ®)Úactionr] ) ÚargparseZArgumentParserZ add_argumentÚDEFAULT_UART2_PORTÚDEFAULT_UART5_PORTrf r/ Ú DEFAULT_ENUÚDEFAULT_HEADING_DEGrC ÚDEFAULT_UART2_BAUDÚDEFAULT_UART5_BAUDÚ
|
parse_args)Zparserr8 r8 r9 rg
s rg c
|
sÚ t } tt}| ¡ }| ¡ }|r|n| j}|r(|d |d |d f}|d }n | j| j| jf}| j }t
|
| j | j |||| j | jd}tt}t||d} t fddfd d
|
| _fd d
|
| _fd d
|
| _fd d
|
| _fdd
|
| _g }
|
| jr z tt| j}
|
W n ty } z t d| W Y d } ~ nd } ~ ww t d u s¨| j rzbzE| ¡ t d z j j!dd\} } W n
|
t j"yÈ Y q²w | dkrÒt | n | dkrît d| j# d| j$dd| j%dd| j&dd q³ t'yû t d Y nw W | (¡ | )¡ t d d S | (¡ | )¡ t d w t j* +¡ p"t *g ¡}t,| |
|
| j | j }| jr8|j- .| j¡ | /¡ z| 0¡ W |j1rJ| 2¡ | (¡ | )¡ t d d S |j1r`| 2¡ | (¡ | )¡ t d w ) Nr r r rT )r r rô Ú initial_enuZinitial_heading_degZ gps_baudrateZ log_baudrate)Ú
|
run_loggerc s | |¡ d S ru ©rw )rt r4 )rà r8 r9 rw ³ s zmain.<locals>.publishc ó
|
d| S )NrA r8 ©rV rj r8 r9 Ú<lambda>¶ ó
|
zmain.<locals>.<lambda>c rk r· r8 rl rj r8 r9 rm · rn c rk )NrB r8 rl rj r8 r9 rm ¸ rn c rk )NrC r8 rl rj r8 r9 rm ¹ rn c rk )NrD r8 )Úlinerj r8 r9 rm º rn u [WARN] è·¯å¾å 载失败: uF [INFO] HITL 仿çå¨å·²å¯å¨ (æ GUI 模å¼)ãæ Ctrl+C éåºãTr )rÙ rD rA z [CTRL] stage=z F=z+.2fz m/s T=z rad/s freq=r" rH u
|
[INFO] ç¨æ·ä¸æãu [INFO] 仿çå¨å·²åæ¢ã)3rg r¤ rÐ r¾ r² r½ r³ r´ rµ r¶ r r/ r2 Zgps_baudZlog_baudr Ú RUN_LOG_PATHr rk Zon_control_statusZon_pose_statusZon_state_statusZon_stack_statusZon_logr¥ r: r r§ Úprintr
|
Zno_guirÜ rn r¯ ry rO rL rM rN ÚKeyboardInterruptr4 Úcloser+ ÚinstancerÁ Z path_liner ZshowZexec_rÎ r3 )r rÑ Z saved_originZ
|
saved_poserô rh Zinitial_headingZcfgri ZsimrÄ r& rt r4 ZappZ dashboardr8 )rà rw r9 Úmain s®
|
ù
|
ÿÿ
|
|
ÿÿþÿø ÿý
|
|
ü
|
ru Ú__main__)r r r r )r2 r" r r )r )r3 r rB rC r r" )rL r" r r" )r2 r" r] r" r r" )r r[ )@Ú__doc__Z
|
__future__r r` r( rW rn Úsysro Zpathlibr Útypingr r r r ZPyQt5r r r
|
r§ Z serial.toolsr Z protocolsr r r r r r r r r ra rb rf rc rd re rf Ú__file__ZresolveÚparentZBASE_DIRrp r Z with_suffixr- rÐ r: rA rK r\ rj rk r r r¤ Z QMainWindowrÁ rg ru r| Úexitr8 r8 r8 r9 Ú<module> sh ÿ ÿ
|
|
|
|
|
! 9-
|
hVÿ
|
