o
|
¯$i`v ã @ 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 lmZmZmZmZ dd
|
l m Z m Z m Z d Z d Z!d Z"dZ#dZ$dZ%dZ&e
|
e' (¡ j)d Z*d%ddZ+G dd dZ,G dd d ej-Z.G d d d ej/Z0d&d d!Z1d"d# Z2e3d$kr¾e 4e2 ¡ dS dS )'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)ç r r r i  i z
|
runlog.txtÚ 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 )
|
Nzutf-8)Úencodingz.jsonú[é r r ú;ú,)ÚexistsÚFileNotFoundErrorÚstrZ read_textÚstripÚsuffixÚlowerÚ
|
startswithÚjsonÚloadsÚ
|
isinstanceÚlistÚtupleÚlenÚappendÚfloatZJSONDecodeErrorÚsplitÚ
|
ValueError)r ÚtextÚpointsÚdataÚptZtokenÚparts© r3 ú/E:\GIT\Lawnmower_STM32H7\python\hitl\run_sim.pyÚload_path_points' s<
|
ÿ
|
" ÿr5 c @ s. e Zd Zdd Zd ddZdd Zd d
|
Zd S ) Ú HitlGuiBridgec C s t jdd| _ t ¡ | _d S )NéÈ )Úmaxsize)ÚqueueZQueueÚ threadingZLockÚ_lock©Úselfr3 r3 r4 Ú__init__F s zHitlGuiBridge.__init__Úkindr c C s: | j | ||f¡ W d d S 1 sw Y d S ©N)r; Ú_enqueue)r= r? r0 r3 r3 r4 ÚpublishJ 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 r@ )r; r9 ZemptyÚ
|
get_nowaitÚEmptyr< r3 r3 r4 ÚresetN 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 r@ )r9 Z
|
put_nowaitZFullrC rD )r= Úitemr3 r3 r4 rA V s ÿÿùzHitlGuiBridge._enqueueN)r? r )Ú__name__Ú
|
__module__Ú __qualname__r> rB rE rA r3 r3 r3 r4 r6 E s
|
|
r6 c sT e Zd ZdZ fddZdddZd fd d
|
Zd fd d Zd fd d Z Z S )ÚZoomableGraphicsViewu6 æ¯æé¼ æ æ»è½®ç¼©æ¾ + æä½å·¦é®å¹³ç§»çè§å¾c sz t j|i |¤ | tjjd¡ | tjj ¡ |
|
tjj ¡ | tjj ¡ | tjj¡ d| _d| _t ¡ | _d | _d S )NTgffffffò?F)Úsuperr> Z setRenderHintr ZQPainterZ AntialiasingZsetTransformationAnchorr
|
Ú QGraphicsViewZAnchorUnderMouseZsetResizeAnchorZ setDragModeZNoDragÚ setCursorr ÚQtÚOpenHandCursorÚ _zoom_factorÚ_panningZQPointÚ _pan_last_posÚ on_user_pan)r= ÚargsÚkwargs©Ú __class__r3 r4 r> g s
|
|
z ZoomableGraphicsView.__init__ÚeventúQtGui.QWheelEventc C s4 | ¡ ¡ }|dkr | jnd| j }| ||¡ d S )Nr ç ð?)Z
|
angleDeltaÚyrP Zscale)r= rX ÚdeltaZ scale_factorr3 r3 r4 Ú
|
wheelEvents s z ZoomableGraphicsView.wheelEventúQtGui.QMouseEventc sJ | ¡ tjjkr d| _| ¡ | _| tjj¡ | ¡ d S t
|
|¡ d S )NT) Úbuttonr rN Ú
|
LeftButtonrQ ÚposrR rM ZClosedHandCursorÚacceptrK ÚmousePressEvent©r= rX rV r3 r4 rc x s
|
z$ZoomableGraphicsView.mousePressEventc st | j r2| | ¡ ¡}| | j¡}|| }| ¡ | _| | ¡ | ¡ ¡ | jr,| ¡ | ¡ d S t
|
|¡ d S r@ ) rQ Z
|
mapToScenera rR Ú translateÚxr[ rS rb rK ÚmouseMoveEvent)r= rX Z scene_posZlast_scene_posr\ rV r3 r4 rg s
|
z#ZoomableGraphicsView.mouseMoveEventc sF | ¡ tjjkr| jrd| _| tjj¡ | ¡ d S t |¡ d S )NF)
|
r_ r rN r` rQ rM rO rb rK ÚmouseReleaseEventrd rV r3 r4 rh s z&ZoomableGraphicsView.mouseReleaseEvent)rX rY )rX r^ )
|
rG rH rI Ú__doc__r> r] rc rg rh Ú __classcell__r3 r3 rV r4 rJ d s
|
rJ c sÆ e Zd Zd8 fd
|
d Zd9d d Zdd Zd:ddZd;d9ddZd<ddZdd Z d d Z
|
d d Z fd d!Z d"d# Z d$d% Zd&d' Zd(d) Zd*d+ Zd=d.d/Zd0d1 Zd2d3 Zd4d5 Zd6d7 Z ZS )>Ú HitlDashboardÚ simulatorr Úbridger6 Ú path_pointsr Ú initial_uart2ú Optional[str]Ú initial_uart5c sª t ¡ || _|| _t|pg | _g | _d | _d | _d | _ i | _
|
d| _ | d¡ | dd¡ d| _g | _| ||¡ | ¡ t | ¡| _| jj | j¡ | j d¡ d S )NTu HITL 仿çç¶æé¢æ¿i iÐ FéP )rK r> rl rm r' rn Ú trail_pointsÚcontrol_statusÚ pose_statusÚ state_statusÚ stack_statusÚ _auto_followZsetWindowTitleZresizeÚ _serial_openÚ _last_portsÚ _build_uiÚ _init_scener ZQTimerZtimerÚtimeoutÚconnectÚ _drain_queueÚstart)r= rl rm rn ro rq rV r3 r4 r> s&
|
|
zHitlDashboard.__init__c C s8 t ¡ }t |¡}| |¡ t | ¡| _t ¡ }t |¡}| dddd¡ | d¡ t | j| _
|
| j | j
|
_ |j | j
|
dd t d¡| _| j d¡ | jj | j¡ |j | jtjjd |j |dd td 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"dddd¡ | | ¡ td u r | 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+dddd
|
¡ | | j,dd¡ g d¢}g | _-t.|D ]N\}}t /¡ }|dk r| 0dd¡ | 1d¡ n | 0d d ¡ | 1d
|
¡ | t !|¡d|d
|
|d
|
d
|
¡ | |d|d
|
|d
|
d
|
d ¡ | j- 2|¡ qxt d ¡| _3| | j3dddd¡ | | ¡ t d ¡}t |¡}t !d ¡| _4| j4 5tjj6tjjB ¡ | j4 #d!¡ t 7¡ }| 8d¡ | 9| j4¡ | |¡ |j |dd t d"¡}t |¡}t :dd¡| _;| j; <g d#¢¡ | j; =¡ >d¡ | | j;¡ | |¡ t d$¡}t |¡}t ?¡ | _@| j@ Ad¡ | | j@¡ |j |dd | j BtC D¡ tC EtC Fd%¡d&¡¡| _G| j BtC D¡ tC EtC Fd'¡d(¡¡| _H| j Id)d)d*d*tC EtC Fd+¡d(¡tC JtC Fd,¡¡¡| _KtC EtC Fd-¡d¡}| LtjjM¡ | j BtC D¡ |tC JtC Fd-¡¡¡| _N| j Id.d.d/d/tC EtC Fd0¡¡tC JtC Fd0¡¡¡| _O| j(jP | jQ¡ | j)jP | jR¡ | j,jP | jS¡ | j3jP | jT¡ | j jP | jU¡ | j jP | jV¡ | U||¡ | Wd¡ | X¡ d S )1Nr é r )Zstretchu èªå¨è·é车è¾T)Z alignmenté é r u 串å£è®¾ç½®u å·æ°õ æå¼ä¸²å£õ ç¶æ: æªè¿æ¥ú color: gray;zUART2: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é?ZbluerZ g333333Ó¿ç333333ã?z#d11d29z#ff4b5cz#ff8c00gÉ¿çÙ?Zgreen)Yr
|
ZQWidgetZ QHBoxLayoutZsetCentralWidgetZQGraphicsSceneÚsceneZ QVBoxLayoutZsetContentsMarginsZ
|
setSpacingrJ ÚviewÚ_handle_user_panrS Z addWidgetZ QCheckBoxÚfollow_checkboxÚ
|
setCheckedZtoggledr~ Ú_on_follow_toggledr rN Z AlignLeftr Ú_port_refresh_supportedZ 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 setAlignmentZAlignTopZ QScrollAreaZsetWidgetResizableZ setWidgetZ QTableWidgetÚ stack_tableZsetHorizontalHeaderLabelsZhorizontalHeaderZsetStretchLastSectionZQPlainTextEditÚlog_viewÚ setReadOnlyZaddPathr Ú QPainterPathZQPenZQColorÚ path_itemÚ
|
trail_itemZ
|
addEllipseZQBrushÚ
|
robot_itemZ setJoinStyleZ RoundJoinÚ heading_itemÚ target_itemZclickedÚ _browse_pathÚ
|
_load_pathÚ_update_originÚ_reset_positionÚ_refresh_serial_portsÚ_toggle_serialÚ_set_controls_enabledÚ_update_serial_ui)r= ro rq ZcentralZlayoutZ
|
left_panelZ left_layoutZ 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_layoutZ arrow_penr3 r3 r4 r{ ¶ sô
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
$"
|
|
|
|
|
|
|
|
|
|
|
""úý0
|
zHitlDashboard._build_uic C s8 | j r| j d¡ | j d¡ | j d¡ d| _ d S d S )NTF)rx r Ú blockSignalsr r< r3 r3 r4 r K s
|
üz HitlDashboard._handle_user_panÚcheckedÚboolc C s4 || _ |r| jr| j | jj| jj ¡ d S d S d S r@ )rx ru r ÚcenterOnÚeastÚnorth)r= rµ r3 r3 r4 r R s
|
ÿz HitlDashboard._on_follow_toggledNc 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 r3 )Zdevice)Ú.0Úpr3 r3 r4 Ú
|
<listcomp>[ s z7HitlDashboard._refresh_serial_ports.<locals>.<listcomp>Ú ÚcomboúQtWidgets.QComboBoxÚsavedr c st | ¡ }| d¡ | ¡ r| ¡ |r | vr | |¡ |p"|}|r3| |¡}|dkr3| |¡ | d¡ d S )NTr F)Ú currentTextr´ ÚclearZaddItemsZaddItemZfindTextZsetCurrentIndex)r¾ rÀ ZcurrentÚtargetr³ ©Zportsr3 r4 Ú_filla s
|
|
|
|
|
z2HitlDashboard._refresh_serial_ports.<locals>._fill)r¾ r¿ rÀ r )
|
r ZcomportsÚ Exceptionrl ÚconfigÚ
|
uart2_portÚ
|
uart5_portr r rz )r= ro rq Z saved_uart2Z saved_uart5rÅ r3 rÄ r4 r¯ W s ÿ
|
z#HitlDashboard._refresh_serial_portsÚenabledc C sL | j | j| j| j| j| jfD ]}| |¡ q| j | ¡ | j | ¡ d S r@ )r r r r r r r r¤ )r= rÊ Zwidgetr3 r3 r4 r± t s ú z#HitlDashboard._set_controls_enabledc 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 ÚsetTextry rl Úuart2ÚportÚlog_uartr r r r Z
|
isEditabler r r )r= rÍ Úuart5Z enable_portsr3 r3 r4 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)ry Ú _close_serialr rÁ r r
|
Ú QMessageBoxÚwarningr rl rÇ rÈ rÉ rÍ rÎ rÏ r rÆ ÚstopZcriticalrm rE r± r² )r= rÍ Z
|
uart5_textrÐ Úexcr3 r3 r4 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)rl rÔ rm rE rs r rt ru rv rw r£ r¡ rÌ r¢ Ú setRowCountry r± r² Ú _refresh_viewr< r3 r3 r4 rÑ ª s
|
|
|
|
|
|
zHitlDashboard._close_serialc s | j r| ¡ t |¡ d S r@ )ry rÑ rK Ú
|
closeEventrd rV r3 r4 rØ º s zHitlDashboard.closeEventc C s6 t j | dtt ¡ d¡\}}|r| j |¡ d S d S )Nu éæ©è·¯å¾æ件u- è·¯å¾æ件 (*.txt *.json);;æææ件 (*))r
|
Z QFileDialogZgetOpenFileNamer r Úcwdr rÌ )r= Ú file_nameÚ_r3 r3 r4 r« ¿ s ÿzHitlDashboard._browse_pathc
|
C sº | j ¡ ¡ }|stj | dd¡ d S ztt|}W n ty9 } ztj | dd| ¡ W Y d }~d S d }~ww |sFtj | dd¡ d S || _ |
|
¡ tj | ddt | d¡ d S )Nu è·¯å¾u 请å
éæ©è·¯å¾æ件ãu å 载失败: u æªè§£æå°ææè·¯å¾ç¹ãu
|
å·²å è½½ u 个路å¾ç¹ã) r r. r r
|
rÒ rÓ r5 r rÆ rn r| Ú informationr) )r= Z file_textZ
|
new_pointsrÕ r3 r3 r4 r¬ Ä s" þ zHitlDashboard._load_pathc C sZ | j ¡ ¡ }|stj | dd¡ d S | j |¡r#tj | dd¡ d S tj | dd¡ d S )Nu åç¹u 请è¾å
¥ GGA å符串ãu åç¹å·²æ´æ°ãu' åç¹æ´æ°å¤±è´¥ï¼è¯·æ£æ¥æ ¼å¼ã) r r. r r
|
rÒ rÓ rl Z update_originrÜ )r= Zggar3 r3 r4 r Õ s z HitlDashboard._update_originc C s^ | j d ¡ }| j d ¡ }| j d ¡ }| j d ¡ }| j ||||¡ tj | dd¡ d S )Nr r r r u ä½ç½®u å·²é置仿çç¶æã)r Úvaluerl Z reset_stater
|
rÒ rÜ )r= r¸ r¹ ÚupÚheadingr3 r3 r4 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 rZ g @)rn r Z setSceneRectr r¥ r* ÚmoveToÚlineTor¦ ÚsetPathÚminÚmaxr ZQRectF)r= ÚpathÚfirstZxsZysZpxÚpyZmin_xZmax_xZmin_yZmax_yÚwidthZheightZpad_xZpad_yZ
|
scene_rectr3 r3 r4 r| ç s<
|
üzHitlDashboard._init_scener. r c C s* | j |¡ | j ¡ | j ¡ ¡ ¡ d S r@ )r£ ZappendPlainTextZverticalScrollBarZsetValueZmaximum)r= r. r3 r3 r4 Ú _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ÚcontrolÚposeiÐ r ÚstateÚstackÚlog)rm r9 rC rD rt ru rs r* r¸ r¹ r) Úpoprv rw Ú task_nameré r× )r= Úupdatedr? r0 r3 r3 r4 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.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 Br Ú
|
) ru r¸ r¹ rÞ Ú heading_degZ pitch_degZroll_degÚ target_eastÚ target_northrt Ú forward_mpsÚ turn_rateZ steering_pwmZ throttle_pwmÚfreq_hzÚstagerv Zcross_track_errorZheading_error_degrw Úvaluesr* rð Ústack_high_waterÚheap_free_bytesr¡ rÌ ÚjoinÚ_update_scene_itemsÚ_refresh_stack_table)r= Z
|
info_linesrë ZctrlÚstrF r3 r3 r4 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¢ rÖ r) rw r rû ZsetItemr
|
ZQTableWidgetItemrð r rü rý )r= ZrowÚstatusr3 r3 r4 r S s ýz"HitlDashboard._refresh_stack_tablec
|
C st | j r/t ¡ }d}| j dd
D ]\}}| }|r"| ||¡ d}q| ||¡ q| j |¡ | jr6| j}|j}|j }| j
|
|d |d dd¡ 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 |jd |j d dd¡ | jr8| j ||¡ d S d S d S )NTi$úÿÿFg333333Ó?r g V@g Ð?g333333ë?gìQ¸
ë±?gÍÌÌÌÌÌÜ?gìQ¸
ëÑ?r r gÉ?r )rs r r¥ rà rá r§ râ ru r¸ r¹ r¨ ZsetRectÚmathZradiansrô ZcosZsinZ closeSubpathr© rª rõ rö rx r r· )r= Útrailræ rf r[ Zscene_yrë Z heading_radZdir_xZdir_yZperp_xZperp_yÚoffsetZ shaft_lengthZshaft_half_widthZ head_lengthZhead_half_widthr Z shaft_endZtipr/ Z
|
arrow_pathr1 r3 r3 r4 rÿ Z s
|
|
|
þþþþþþþþþê ¹Fz!HitlDashboard._update_scene_items)
|
rl r rm r6 rn r ro rp rq rp )ro rp rq rp )rµ r¶ )NN)rÊ r¶ )r. r )rG rH rI r> r{ r r r¯ r± r² r° rÑ rØ r« r¬ r r® r| ré r r× r rÿ rj r3 r3 rV r4 rk s,
|
|
|
|
|
!/rk ú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_PORTr r+ Ú DEFAULT_ENUÚDEFAULT_HEADING_DEGÚintÚDEFAULT_UART2_BAUDÚDEFAULT_UART5_BAUDÚ
|
parse_args)Zparserr3 r3 r4 r ° s r c
|
|
s t } t| j| j| j| j| j| jf| j| j | j
|
d}t t }t ||d}t fddfdd|_fdd|_fdd|_fd d|_fd
|
d|_g }| jrxz tt| j}W n tyw } z td | W Y d }~nd }~ww td u s| jrïzbzE| ¡ td z j j dd\}}W n
|
t j y Y qw |dkr©t| n |dkrÅtd|j d|j dd|j!dd|j"dd q t#yÒ td Y nw W | $¡ | %¡ td d S | $¡ | %¡ td w tj& '¡ pøt &g ¡}t(| || j| j} | jr| j) *| j¡ | +¡ z| ,¡ W | j-r | .¡ | $¡ | %¡ td d S | j-r6| .¡ | $¡ | %¡ td w )N)rÈ rÉ Z
|
origin_ggaZ initial_enuZinitial_heading_degZ gps_baudrateZ log_baudrate)Ú
|
run_loggerc s | |¡ d S r@ ©rB )r? r0 )rm r3 r4 rB Ð s zmain.<locals>.publishc ó
|
d| S )Nrê r3 ©r r r3 r4 Ú<lambda>Ó ó
|
zmain.<locals>.<lambda>c r )Nrë r3 r r r3 r4 r Ô r c r )Nrì r3 r r r3 r4 r Õ r c r )Nrí r3 r r r3 r4 r Ö r c r )Nrî r3 )Úliner r3 r4 r × r u [WARN] è·¯å¾å 载失败: uF [INFO] HITL 仿çå¨å·²å¯å¨ (æ GUI 模å¼)ãæ Ctrl+C éåºãTrZ )r} rî rê z [CTRL] stage=z F=z+.2fz m/s T=z rad/s freq=z.2frò u
|
[INFO] ç¨æ·ä¸æãu [INFO] 仿çå¨å·²åæ¢ã)/r r rÍ rÐ Úoriginr¸ r¹ rÞ rß Zgps_baudZlog_baudr Ú RUN_LOG_PATHr r6 Zon_control_statusZon_pose_statusZon_state_statusZon_stack_statusZon_logrå r5 r rÆ Úprintr
|
Zno_guir r9 ÚgetrD rú r÷ rø rù ÚKeyboardInterruptrÔ ÚcloseZ QApplicationÚinstancerk r rÌ ZshowZexec_ry rÑ )
|
rT Zcfgr Zsimrn rÕ r? r0 ZappZ dashboardr3 )rm rB r4 ÚmainÀ s ù
|
ÿÿ
|
|
ÿÿþÿø ÿý
|
|
ü
|
r" Ú__main__)r r r r )r r )5ri Z
|
__future__r r r$ r r9 Úsysr: Zpathlibr Útypingr r r r ZPyQt5r r r
|
rÆ Z serial.toolsr Z protocolsr r r r rl r r r r r r r r r r Ú__file__ZresolveÚparentr r5 r6 rL rJ Z QMainWindowrk r r" rG Úexitr3 r3 r3 r4 Ú<module> sV ÿ ÿ
|
3
|
Hÿ
|
