o
|
: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 ó g | ]}| ¡ qS r8 ©r$ ©Ú.0Úpr8 r8 r9 Ú
|
<listcomp>m ó 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 rO )rQ Útr8 r8 r9 rS | s z/build_gga_from_short_origin.<locals>.<listcomp>r; r< r rW u) åç¹æ ¼å¼åºä¸º 'lat,N,lon,E[,alt]'ãr r r rV rX )ÚNÚS)ÚEÚWu æ¹åå¿
é¡»æ¯ N/S/E/WãrD z
|
éþÿÿÿrM é z$GNGGArY )
|
r= r0 r- r1 ÚupperÚDEFAULT_ORIGIN_GGAÚendswithr. rH r ) r2 r_ r> Zlat_dmr[ Zlon_dmr\ r] ÚbaseÚnewlinerZ ZfieldsZchecksumr8 r8 r9 Úbuild_gga_from_short_origin{ s<
|
|
|
ÿ
|
rl 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)rr Ú_enqueue)rt rv 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 rw )rr rp ZemptyÚ
|
get_nowaitÚEmptyrs 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 rw )rp Z
|
put_nowaitZFullrz r{ )rt Úitemr8 r8 r9 rx s ÿÿùzHitlGuiBridge._enqueueN)rv r" )Ú__name__Ú
|
__module__Ú __qualname__ru ry r| rx r8 r8 r8 r9 rm s
|
|
rm 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)Úsuperru Z setRenderHintr ZQPainterZ AntialiasingZsetTransformationAnchorr
|
Ú QGraphicsViewZAnchorUnderMouseZsetResizeAnchorZ setDragModeZNoDragÚ setCursorr ÚQtÚOpenHandCursorÚ _zoom_factorÚ_panningZQPointÚ _pan_last_posÚ on_user_pan)rt ÚargsÚkwargs©Ú __class__r8 r9 ru ¿ s
|
|
z ZoomableGraphicsView.__init__ÚeventúQtGui.QWheelEventc C s4 | ¡ ¡ }|dkr | jnd| j }| ||¡ d S )Nr ç ð?)Z
|
angleDeltar@ r Zscale)rt 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©rt r r r8 r9 r Ð s
|
z$ZoomableGraphicsView.mousePressEventc s | j r2| | ¡ ¡}| | j¡}|| }| ¡ | _| | ¡ | ¡ ¡ | jr,| ¡ | ¡ d S | j j
|
| | ¡ ¡ t |¡ d S rw )r Ú
|
mapToScener r Ú translater? r@ r r ÚsceneMouseMovedZemitÚ _scene_coordsr ÚmouseMoveEvent)rt r Ú scene_posZlast_scene_posr r r8 r9 r Ù s
|
z#ZoomableGraphicsView.mouseMoveEventÚview_posú QtCore.QPointr úTuple[float, float]c C s | |¡}| ¡ | ¡ fS rw )r r? r@ )rt 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 ru 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 rw )r r§ r4 Ú_load)rt r§ r8 r8 r9 ru õ 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# Ú Exceptionrs 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© rs 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ÚposerW c s s | ]}t |V qd S rw )r/ )rQ Úvr8 r8 r9 Ú <genexpr> s z/DashboardStateStore.get_pose.<locals>.<genexpr>)r4 Úgetr* r+ r- r, Ú TypeErrorr1 ©rt 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¬ ©rt 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" )rt r¿ r8 r8 r9 Ú
|
get_origin s
|
z DashboardStateStore.get_originÚ
|
origin_strr" c C s || j d< | ¡ d S r¾ rº )rt 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 ru r¨ r¬ r´ r¼ rÀ r r8 r8 r8 r9 r¦ ô s
|
|
|
|
r¦ c s e Zd ZdJ fd
|
d ZdKd d Zdd Zdd ZdLddZdd ZdMdd Z dNdKd d Z
|
dOd!d"Z d#d$ Z dPd&d'Z dQd*d+Zd,d- Zd.d/ Zd0d1 Zd2d3 Zd4d5 Zd6d7 Z fd8d9Zd:d; Zd<d= Zd>d? ZdQd@dAZdBdC ZdDdE ZdFdG Z dHdI Z Z S )RÚ HitlDashboardÚ simulatorr Úbridgerm Ú 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 ru 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)rt rÄ rÅ rÆ rÇ rÈ r r8 r9 ru " s0
|
|
|
zHitlDashboard.__init__c C sx t ¡ }t |¡}| |¡ t | ¡| _t ¡ }t |¡}| dddd¡ | d¡ t | j| _
|
| j | j
|
_ | j
|
j | j¡ |j| j
|
dd t ¡ }t |¡}| dddd¡ t d¡| _| j d¡ | jj | j¡ | | j¡ t d¡| _| jj | j¡ | | j¡ | d¡ t d¡| _ | j d ¡ | | j ¡ | |¡ |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 rN| 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 ¡| _;| t d ¡dddd ¡ | | j9dd dd ¡ | | j: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< A|¡ qñt d$¡| _B| | jBd
|
ddd¡ |
|
|¡ t d%¡}t |¡}t d&¡| _C| jC DtEjFjGtEjFjHB ¡ | jC d'¡ t I¡ }| Jd¡ | K| jC¡ | Ld(¡ | |¡ |
|
j|dd t d)¡}t |¡}t Mdd
|
¡| _N| jN Og d*¢¡ | jN P¡ Qd¡ | | jN¡ |
|
|¡ t d+¡} t | ¡} t *¡ | _R| jR Sd¡ | | jR¡ |
|
j| dd | j Tt. U¡ t. Vt. Wd,¡d¡¡| _X| j Tt. U¡ t. Vt. Wd-¡d¡¡| _Yd.| _Z| j [| jZ | jZ | jZd | jZd t. Vt. Wd/¡d¡t. \t. Wd0¡¡¡| _]t. Vt. Wd1¡d¡} | ^tEjFj_¡ | j Tt. U¡ | t. \t. Wd1¡¡¡| _`d2| _a| j [| ja | ja | jad | jad t. Vt. Wd3¡¡t. \t. Wd3¡¡¡| _b| j1j | jc¡ | j2j | jd¡ | j:j | je¡ | j;j | jf¡ | jBj | jg¡ | j%j | jh¡ | j&j | ji¡ | h||¡ | jd¡ | k¡ d S )4Nr rW r )Zstretchu èªå¨è·é车è¾Tu æ¸
é¤è½¨è¿¹zE: 0.00 N: 0.00zQcolor:#222; background:rgba(255,255,255,190); padding:2px 6px; border-radius:4px;rV é 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 å¤å¶å®å®ä¹u åæ (lat,N,lon,E):rX )zE (m)zN (m)zU (m)z Heading (deg)g @Àg @@g vÀg v@u éç½®ä½ç½®u 车è¾ç¶æõ çå¾
æ°æ®...z6font-family: Consolas, 'Courier New'; font-size: 14px;éô u å æ çæµ)u ä»»å¡u å æ ä½é(word)u å©ä½å (byte)u 串å£æ¥å¿ZgrayZblueç Ð?z#d11d29z#ff4b5cz#ff8c00g{®Gáz´?Zgreen)lr
|
ZQWidgetZ QHBoxLayoutZsetCentralWidgetZQGraphicsSceneÚsceneZ QVBoxLayoutZsetContentsMarginsZ
|
setSpacingr ÚviewÚ_handle_user_panr r rÜ Ú_update_mouse_posZ addWidgetZ QCheckBoxÚfollow_checkboxÚ
|
setCheckedZtoggledÚ_on_follow_toggledZ QPushButtonZclear_trail_btnZclickedÚ _clear_trailZ
|
addStretchZQLabelÚmouse_pos_labelÚ setStyleSheetr Ú_port_refresh_supportedZ QGroupBoxZ QGridLayoutZ QComboBoxÚ port2_comboÚ port5_comboÚ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_btnZcopy_origin_btnÚpos_spinrF ZQDoubleSpinBoxZsetRangeZ setDecimalsr. Úreset_pose_btnÚ
|
info_labelZ setAlignmentr r
ZAlignTopZ AlignLeftZ QScrollAreaZsetWidgetResizableZ setWidgetZsetMinimumHeightZ QTableWidgetÚ stack_tableZsetHorizontalHeaderLabelsZhorizontalHeaderZsetStretchLastSectionÚlog_viewÚ setReadOnlyZaddPathÚ QPainterPathZQPenZQColorÚ path_itemÚ
|
trail_itemÚ
|
robot_sizeZ
|
addEllipseZQBrushÚ
|
robot_itemZ setJoinStyleZ RoundJoinÚ heading_itemÚ target_radiusÚ target_itemÚ_load_path_from_textÚ_export_path_to_mcuÚ_update_originÚ_copy_origin_macroÚ_reset_positionÚ_refresh_serial_portsÚ_toggle_serialÚ_set_controls_enabledÚ_update_serial_ui) rt rÇ rÈ ZcentralZlayoutZ
|
left_panelZ left_layoutZ
|
bottom_barZ bottom_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 s2
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
$"
|
|
|
|
|
|
|
|
|
|
|
|
""úýú
|
zHitlDashboard._build_uic
|
C s | j ¡ ¡ }|stj | dd¡ d S dd | d¡D }t|dk r-tj | dd¡ d S zu|d }tt |d }t |d }||d
|
}|d
|
¡ d krQ| }|d }tt |d }t |d } || d
|
}
|
|d
|
¡ dkrt|
|
}
|
d} t|dkr|d } d|dd|
|
dd| d} tj ¡ } | | ¡ tj | dd¡ W d S tyÀ } ztj | dd| ¡ W Y d }~d S d }~ww )Nu å¤å¶u åç¹è¾å
¥æ¡ä¸ºç©ºc S rN r8 rO rP r8 r8 r9 rS rT z4HitlDashboard._copy_origin_macro.<locals>.<listcomp>r rW u- åç¹æ ¼å¼é误ï¼éè³å°å
å«ç»çº¬åº¦r éd g N@r rb r rV rd z0.0rX z*#define MC_CFG_ORIGIN_LAT_DEG (z.6fz,)
|
#define MC_CFG_ORIGIN_LON_DEG (z,)
|
#define MC_CFG_ORIGIN_ALT_M (ú)u! å·²å¤å¶å®å®ä¹å°åªè´´æ¿ï¼u æ°å¼è§£æé误: )rù r2 r$ r
|
Ú QMessageBoxÚwarningr0 r- rC r/ rg Ú QApplicationÚ clipboardÚsetTextÚ informationr1 )rt r2 r7 Zlat_rawZ lat_deg_intZlat_minZlat_ddZlon_rawZ lon_deg_intZlon_minZlon_ddr] Zmacror Úer8 r8 r9 r sJ
|
ÿþÿ
|
|
$ÿz HitlDashboard._copy_origin_macroc C s8 | j r| j d¡ | j d¡ | j d¡ d| _ d S d S )NTF)rÏ rê Ú blockSignalsrë rs r8 r8 r9 rè 1 s
|
üz HitlDashboard._handle_user_panÚcheckedÚboolc C s4 || _ |r| jr| j | jj| jj ¡ d S d S d S rw )rÏ rÌ rç ÚcenterOnrµ r¶ )rt r r8 r8 r9 rì 8 s
|
ÿz HitlDashboard._on_follow_toggledc C s g | _ | ¡ dS )u æ¸
é¤å°å¾ä¸ç轨迹ç¹N)rÊ Z _update_sceners r8 r8 r9 rí = s zHitlDashboard._clear_trailrµ r/ Úscene_yc C s& | }| j d|dd|d¡ d S )NzE: z .2fú N: )rî r )rt rµ r r¶ r8 r8 r9 ré C 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 )ZdevicerP r8 r8 r9 rS K 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 Ú_fillQ s
|
|
|
|
|
z2HitlDashboard._refresh_serial_ports.<locals>._fill)r! r" r# r" )
|
r Zcomportsr© rÄ r÷ Ú
|
uart2_portÚ
|
uart5_portrñ rò rÑ )rt rÇ rÈ Z saved_uart2Z saved_uart5r( r8 r' r9 r G s ÿ
|
z#HitlDashboard._refresh_serial_portsÚenabledc C s( | j |¡ | j d¡ | j d¡ d S )NFT)rü rö rù r rú )rt r+ r8 r8 r9 r d 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Æ ©rt r2 r3 r8 r8 r9 rÕ i 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 )rQ r? r@ r8 r8 r9 r° x s $ z>HitlDashboard._update_path_text_from_points.<locals>.<genexpr>)rÖ r% rH r- )rt r3 r2 r8 r8 r9 rØ t 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
|
r r )rt r2 Úexcr8 r8 r9 Ú_save_path_text{ 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 rV )rÓ r´ rû ÚsetValuer³ r8 r8 r9 rÙ s
|
üz HitlDashboard._load_saved_posec C sx | j ¡ ¡ }t|}|stj | dd¡ d S || _| j ¡ |
|
|¡ | ¡ | ¡ tj | ddt| d¡ d S )Nr0 u9 æªè§£æå°ææç轨迹ç¹ï¼è¯·æ£æ¥è¾å
¥æ ¼å¼ãu
|
å·²å è½½ u 个路å¾ç¹ã)rÖ r× r$ rA r
|
r r rÆ rÊ r% r2 rÚ Ú _refresh_viewr r- r. r8 r8 r9 r 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 r r r ÚPATH_EXPORT_FILErª r© r r" )rt r2 r3 Z formattedr r1 r8 r8 r9 r
|
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=r z color: green;rá râ )rô r rÐ rÄ Úuart2ÚportÚlog_uartrõ rï rñ rö Z
|
isEditablerò rð ró )rt r6 Ú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* r6 r7 r8 rÞ r© ÚstopZcriticalrÅ r| r r )rt r6 Z
|
uart5_textr9 r1 r8 r8 r9 r µ 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Ä r; rÅ r| rÊ r% rË rÌ rÍ rÎ rÿ rý r rþ Ú setRowCountrÐ r r r4 rs r8 r8 r9 r: Î s
|
|
|
|
|
|
zHitlDashboard._close_serialc s | j r| ¡ t |¡ d S rw )rÐ r: r Ú
|
closeEventr r r8 r9 r= Þ 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ø rh rl r1 Z update_originr r^ rÓ r r )rt r2 r_ rL r1 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 rV u ä½ç½®u å·²é置仿çç¶æã) rû ÚvaluerÄ Z reset_staterÓ r¼ r
|
r r r» r8 r8 r9 r ô s z HitlDashboard._reset_positionc C s6 | 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 |¡ | j |t jj¡ d S )
|
Niÿÿÿrä TFg $@g è?r r g @)rÆ ræ Z setSceneRectr r r. ÚmoveToÚlineTor ÚsetPathÚminÚmaxr ZQRectFrç Z fitInViewr
ZKeepAspectRatio)rt 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 rw )rÿ ZappendPlainTextZverticalScrollBarr3 Zmaximum)rt 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Å rp rz r{ rË rÌ rÊ r. rµ r¶ r- ÚpoprÍ rÎ Ú task_namerG r4 )rt Úupdatedrv 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. rM Ústack_high_waterÚheap_free_bytesrý r rH Ú_update_scene_itemsÚ_refresh_stack_table)rt Z
|
info_linesr® ZctrlÚstr} r8 r8 r9 r4 ; 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þ r< r- rÎ rF rW ZsetItemr
|
ZQTableWidgetItemrM r" rX rY )rt ZrowÚstatusr8 r8 r9 r[ j 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 r? r@ r rA rÌ rµ r¶ r r ZsetRectÚmathZradiansrP ZcosZsinZ closeSubpathr r r rQ rR rÏ rç r )rt ÚtrailrD 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 rZ q s "
|
|
|
þþþþþþþþþê
|
ü²Mz!HitlDashboard._update_scene_items)
|
rÄ r rÅ rm 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 ru rÔ r rè rì rí ré r r rÕ rØ r2 rÙ r r
|
r r r: r= r r rÚ rG rÝ r4 r[ rZ r¥ r8 r8 r r9 rà ! s:
|
$ ;1
|
|
|
|
|
|
"/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))Útyperc rd 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))rd z--no-guiZ
|
store_trueu" ä¸å¯å¨ Qt GUIï¼ä»
æå°æ°æ®)Úactionrd ) ÚargparseZArgumentParserZ add_argumentÚDEFAULT_UART2_PORTÚDEFAULT_UART5_PORTrh r/ Ú DEFAULT_ENUÚDEFAULT_HEADING_DEGrC ÚDEFAULT_UART2_BAUDÚDEFAULT_UART5_BAUDÚ
|
parse_args)Zparserr8 r8 r9 rn Î s rn 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 rV )r) r* rø Ú initial_enuZinitial_heading_degZ gps_baudrateZ log_baudrate)Ú
|
run_loggerc s | |¡ d S rw ©ry )rv r4 )rÅ r8 r9 ry ü s zmain.<locals>.publishc ó
|
d| S )NrH r8 ©r] rq r8 r9 Ú<lambda>ÿ ó
|
zmain.<locals>.<lambda>c rr r¹ r8 rs rq r8 r9 rt ru c rr )NrI r8 rs rq r8 r9 rt ru c rr )NrJ r8 rs rq r8 r9 rt ru c rr )NrK r8 )Úlinerq r8 r9 rt ru u [WARN] è·¯å¾å 载失败: uF [INFO] HITL 仿çå¨å·²å¯å¨ (æ GUI 模å¼)ãæ Ctrl+C éåºãTr )rÛ rK rH z [CTRL] stage=z F=z+.2fz m/s T=z rad/s freq=r/ rO u
|
[INFO] ç¨æ·ä¸æãu [INFO] 仿çå¨å·²åæ¢ã)3rn r¦ rÒ rÀ r´ r¿ rµ r¶ r· r¸ r r6 r9 Zgps_baudZlog_baudr Ú RUN_LOG_PATHr rm Zon_control_statusZon_pose_statusZon_state_statusZon_stack_statusZon_logr§ r: r r© Úprintr
|
Zno_guirÞ rp r± r{ rV rS rT rU ÚKeyboardInterruptr; Úcloser Úinstancerà Z path_liner ZshowZexec_rÐ r: )r rÓ Z saved_originZ
|
saved_poserø ro Zinitial_headingZcfgrp ZsimrÆ r1 rv r4 ZappZ dashboardr8 )rÅ ry r9 ÚmainÞ s®
|
ù
|
ÿÿ
|
|
ÿÿþÿø ÿý
|
|
ü
|
r| Ú__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 rb )@Ú__doc__Z
|
__future__r rg r( r^ rp Úsysrq Zpathlibr Útypingr r r r ZPyQt5r r r
|
r© Z serial.toolsr Z protocolsr r r r r rÄ r r r rh ri rh rj rk rl rm Ú__file__ZresolveÚparentZBASE_DIRrw r, Z with_suffixr5 rÒ r: rA rK r^ rl rm r r r¦ Z QMainWindowrà rn r| r~ Úexitr8 r8 r8 r9 Ú<module> sj ÿ ÿ
|
|
|
|
|
! 9-
|
2Vÿ
|
