o
|
C0i{X ã @ s0 d Z ddlZddlZddlZddlZddlmZ ddlmZm Z m
|
Z
|
ddl Z ddl Z ddlmZ ddlmZ ddlmZ z.ddlZe ¡ ZedkrQg d¢e jd < ned
|
kr]d d ge jd < nd dge jd < de jd< W n Y ej eeej j ¡ G dd dZ G dd dZ dd Z e dkre dS dS )ud
|
车è¾è¿å¨æ¨¡åæ ¡åæ°æ®åæå·¥å
·
|
解æ$CALæ ¼å¼æ¥å¿ï¼æåå·®éè¿å¨æ¨¡ååæ°
|
é N)ÚPath)ÚListÚDictÚTuple)Úoptimize)Ú defaultdictZWindows)zMicrosoft YaHeiZSimHeiZSimSunzfont.sans-serifZDarwinz PingFang SCzHeiti SCzWenQuanYi Micro HeizNoto Sans CJK SCFzaxes.unicode_minusc @ s e Zd ZdZdefddZdS )ÚCalibrationDatau æ ¡åæ°æ®ç¹Úlinec C sN | ¡ d¡}t|dk rtd| t|d | _t|d | _|d | _t|d | _t|d | _ t
|
|d | _ t
|
|d
|
| _ t
|
|d | _ t
|
|d | _t
|
|d | _t
|
|d | _t
|
|d d | _t
|
|d d | _t
|
|d d | _t
|
|d d | _t
|
|d d | _t
|
|d d | _d| _d| _d| _d| _| j | _d S )Nú,é zInvalid CAL line: é é é é é é é é é é
|
é é ç @@é é é é é g ) ÚstripÚsplitÚlenÚ
|
ValueErrorÚintÚseqÚtime_msÚstateÚ throttle_pwmÚ steering_pwmÚfloatÚenu_xÚenu_yZenu_zÚhdgZpitchZrollZgxZgyÚgzÚaxZayÚazÚvelocityÚyaw_rateZlatZlonZalt)Úselfr Úparts© r3 úcalibration_analyzer.pyÚ__init__* s2
|
zCalibrationData.__init__N)Ú__name__Ú
|
__module__Ú __qualname__Ú__doc__Ústrr5 r3 r3 r3 r4 r ( s r c @ s e Zd ZdZdefddZdd Zdd Zd efd
|
d Z d efd d Z
|
d efddZ de fddZ de fddZdededed efddZdS ) ÚCalibrationAnalyzeru æ ¡åæ°æ®åæå¨Úlog_filec C s || _ g | _tt| _d S ©N)r< Údatar ÚlistÚ states_data)r1 r< r3 r3 r4 r5 R s z CalibrationAnalyzer.__init__c C s: t d| j t| jdddd8}|D ]-}| d¡rAz t|}| j |¡ W q ty@ } z t d| W Y d}~qd}~ww qW d n1 sLw Y | jsXtd t d
|
t | j d t d t
|
d d | jD ddt d d | jD dddt
|
dd | jD ddt dd | jD dd dS )u 解ææ¥å¿æ件u 解ææ¥å¿æ件: Úrúutf-8Úignore)ÚencodingÚerrorsz$CALu è¦å: 解æè¡å¤±è´¥: Nu æªæ¾å°ææç$CALæ°æ®u æå解æ u æ¡æ°æ®ç¹u ENUåæ èå´: X=[c s ó | ]}|j V qd S r= ©r) ©Ú.0Údr3 r3 r4 Ú <genexpr>k ó z0CalibrationAnalyzer.parse_log.<locals>.<genexpr>ú.2fz, z], zY=[c s rF r= ©r* rH r3 r3 r4 rK l rL z] (m)) Úprintr< ÚopenÚ
|
startswithr r> ÚappendÚ Exceptionr! r ÚminÚmax)r1 Úfr r> Úer3 r3 r4 Ú parse_logW s*
|
þüÿ ÿ
|
64
|
ÿz CalibrationAnalyzer.parse_logc C s< t d t| jD ]a\}}|dkrj|j| j|d j d }|dkrj|j| j|d j }|j| j|d j }t |d |d ¡| |_|j | j|d j }|dkrZ|d8 }n|dk rb|d7 }t
|
|¡| |_ q | jD ] }| j |j |¡ qnt d t| j d
|
| j ¡ D ]\}}t d | d t| d qdS )u9 计ç®é度ï¼ENUåæ å·²å¨åçæºç«¯è®¡ç®å®æï¼u 计ç®é度åè§é度...r r r r é´ ih iLÿÿÿu æ°æ®åç»å®æï¼å
± u
|
个ç¶æz z: u 个ç¹N)rO Ú enumerater> r$ r) r* ÚnpZsqrtr/ r+ Zdeg2radr0 r@ r% rR r Úitems) r1 Úir> ZdtZdxZdyZdhdgr% Údlistr3 r3 r4 Úcompute_enu_and_velocityn s*
|
|
ÿz,CalibrationAnalyzer.compute_enu_and_velocityÚreturnc sr t d i }g d¢}g }|D ]B}|| jv rP| j| t d d
}|rPt dd |D ¡}t dd |D ¡}| ||f¡ t | d|d d
|
|d d qt|dkr£t d d |D ¡}t dd |D ¡} dd }
|
t |
|
|| ¡\} } | \} }dt | t |d| dd|ddd |D d|d< t d| dd|d g d¢}i }|D ]D}|| jv rï| j| t dkrïdd D } fd d D }t|d krït
|
||d ¡}|d }t |||< t | d |d d! q«||d"< d#| jv r7| jd# t dkr7d$d D } fd%d D }t|d kr7t
|
||d ¡}|d }t ||d&< t d'|d d! |S )(u åæååè¿å¨æ¨¡åu!
|
=== åæååè¿å¨æ¨¡å ===©Z
|
CRUISE_LOWZ
|
CRUISE_MIDZ CRUISE_HIGHr Nc S ó g | ]}|j qS r3 ©r& rH r3 r3 r4 Ú
|
<listcomp> ó z=CalibrationAnalyzer.analyze_forward_model.<locals>.<listcomp>c S rb r3 ©r/ rH r3 r3 r4 rd re z: PWM=ú.0fu , é度=rM z m/sc S ó g | ]}|d qS ©r r3 ©rI Úpr3 r3 r4 rd ¤ ó c S rh ©r r3 rj r3 r3 r4 rd ¥ rl c S s || d | S )NéÜ r3 )ZpwmÚkÚbiasr3 r3 r4 Ú linear_model¨ s z?CalibrationAnalyzer.analyze_forward_model.<locals>.linear_modelÚlinearzv = z.6fz * (pwm - 1500) + c S s g | ] \}}t |t |fqS r3 ©r( )rI rk Úvr3 r3 r4 rd ³ ó )Úmodelro rp ÚformulaÚpwm_velocity_pairsÚpwm_to_velocityu PWM-é度模å: v = )Ú ACCEL_LOWÚ ACCEL_MIDÚ
|
ACCEL_HIGHr c S rb r3 rf rH r3 r3 r4 rd ¿ re c ó g | ] }|j d j d qS ©r r ©r$ rH ©r^ r3 r4 rd À ru r r u : å é度 = ú.3fu m/sÂ²Ú accelerationÚBRAKEc S rb r3 rf rH r3 r3 r4 rd Ï re c r} r~ r rH r r3 r4 rd Ð ru Ú decelerationu å¶å¨åé度: ) rO r@ r r[ ÚmeanrR Úarrayr Z curve_fitr( Zpolyfit)r1 ÚresultsÚ cruise_statesrx r% Ú stable_dataZavg_pwmZavg_velÚpwmsÚvelsrq ZpoptÚ_Z k_forwardrp Ú accel_statesZ accel_resultsÚ
|
velocitiesÚtimesZcoeffsZaccelZdecelr3 r r4 Úanalyze_forward_model sj
|
|
|
û
|
|
|
z)CalibrationAnalyzer.analyze_forward_modelc C s t d i }g d¢}g }|D ]w}|| jv r
| j| }t|d }dt| d }|||
}|r
t dd |D ¡} t dd |D ¡}
|
t dd |D ¡} | d } | | |
|
| f¡ t|
|
d
|
krft| |
|
ntd } t | d | d d| dd|
|
dd| dd| dd qt|dkrÌt dd |D ¡}t dd |D ¡}t || ¡t |d ¡ }dt|d|dd d d |D d |d < t d |dd |S )!u åæ转å模åu
|
=== åæ转å模å ===©ZTURN_LEFT_LIGHTZTURN_RIGHT_LIGHTZTURN_LEFT_HEAVYZTURN_RIGHT_HEAVYr r c S rb r3 ©r' rH r3 r3 r4 rd í re z>CalibrationAnalyzer.analyze_steering_model.<locals>.<listcomp>c S rb r3 ©r0 rH r3 r3 r4 rd î re c S rb r3 rf rH r3 r3 r4 rd ï re rn g{®Gáz?Úinfu : èµæºPWM=rg ú (z+.0fu ), è§é度=r u rad/s, é度=rM u m/s, 转ååå¾=z mr c S rh ri r3 rj r3 r3 r4 rd ý rl c S rh rm r3 rj r3 r3 r4 rd þ rl rr u Ï = ú.8fz * (steering_pwm - 1500)c S s( g | ]\}}}t |t |t |fqS r3 rs )rI ÚoÚyrt r3 r3 r4 rd s ( )rv ro rw r> Úpwm_to_yaw_rateu 转å模å: Ï = )
|
rO r@ r r[ r
rR Úabsr( r Úsum)r1 r Ú turn_statesZpwm_yawrate_pairsr% r^ Ú start_idxÚend_idxr Zavg_steering_pwmZ avg_yaw_rateZ avg_velocityZsteering_offsetZturning_radiusZoffsetsÚ yaw_ratesZ
|
k_steeringr3 r3 r4 Úanalyze_steering_modelÚ sH
|
|
ÿÿ
|
þ
|
üz*CalibrationAnalyzer.analyze_steering_modelc C s t d i }d| jv rDd| jv rD| jd d }| jd d }d}| jd D ] }|j|krC|j|j }t||d< t d| d |S q%|S )
|
u åæååºå»¶è¿u
|
=== åæååºå»¶è¿ ===ZWARMUPrz éÿÿÿÿr ç¹?Úthrottle_response_delay_msu æ²¹é¨ååºå»¶è¿: z ms)rO r@ r/ r$ r( )r1 r Z warmup_lastZ accel_firstZthreshold_velocityrJ Zdelay_msr3 r3 r4 Úanalyze_delays s
|
úz"CalibrationAnalyzer.analyze_delaysÚ
|
output_dirc !
|
sÈ t d |jddd tjdd\}}fddjD }dd jD }d d jD }|j||d
|
d d d |jddd |jd ddd |jddd |j ddd |
|
¡ }|j||ddddd |jddd d |jdd d jd j }|d } t t |jD ]5\}
|
\} } | j |ks|
|
tjd kr¹|j| | d d d |j| | d! t|d" |d#d$d d% | j }| } qtjd&d'd | ¡ tj|d( d)d* t d+|d( t ¡ tjd,d\}}d-d jD } d.d jD }i }d }tj t d dd/¡¡}j ¡ D ]}||d/ ||< |d7 }qj ¡ D ]!\} d0d D }d1d D }|j|||| d!|d2d!d3 q |jd4dd |jd5dd |j d6d'd |j ddd | d7¡ |j d$d8d9 t ¡ tj|d: d)d* t d+|d: t ¡ tjdd!dd\}\}}g d;¢}|D ]0}|jv rÁj| t d! d<
}d=d |D }d>d |D }|j |||d?d@dA q|jddd |jd dd |j dBd'd |j ddd | ¡ g dC¢}|D ]:}|jv r"j| t dD }dEt dD } ||
}dFd |D } dGd |D } |j | | |d?d@dA qé|j!d dHdId?dJ |j"d dHdId?dJ |jdKdd |jdLdd |j dMd'd |j ddd | ¡ t ¡ tj|dN d)d* t d+|dN t ¡ tjdOd\}}g dP¢} | D ])}|jv r¦j| fdQd D } dRd D }|j| |d2dE|d!dS q~|jdTdd |jd dd |j dUd'd |j ddd | ¡ t ¡ tj|dV d)d* t d+|dV t ¡ d<S )Wu çæå¯è§åå¾è¡¨u
|
=== çæå¯è§åå¾è¡¨ ===T)ÚparentsÚexist_ok)r r )Zfigsizec s" g | ] }|j jd j d qS r~ )r$ r> rH )r1 r3 r4 rd + s " z6CalibrationAnalyzer.generate_plots.<locals>.<listcomp>c S rb r3 rf rH r3 r3 r4 rd , re c S rb r3 rc rH r3 r3 r4 rd - re zb-g ø?u é度 (m/s))Ú linewidthÚlabelu æ¶é´ (ç§)r )ÚfontsizeÚb)rª Úcolorr )ÚaxisZ
|
labelcolorg333333Ó?)Úalphazr--r g333333ã?u æ²¹é¨PWM)r¨ r® r© rA r r¢ Zgray)r® r¬ r gÍÌÌÌÌÌì?Úcenterr )Zharª Zrotationu! é度ä¸æ²¹é¨PWMéæ¶é´åår zvelocity_time.pngé )Zdpiu ä¿å: )r r c S rb r3 rG rH r3 r3 r4 rd N re c S rb r3 rN rH r3 r3 r4 rd O re r c S rb r3 rG rH r3 r3 r4 rd [ re c S rb r3 rN rH r3 r3 r4 rd \ re r )r¬ r¨ r© ÚmarkerÚ
|
markersizeu
|
ä¸å (m)u
|
åå (m)u 车è¾è¿å¨è½¨è¿¹ (ENUåæ )ZequalZbest)rª Úlocztrajectory.pngra Nc S rb r3 rc rH r3 r3 r4 rd u re c S rb r3 rf rH r3 r3 r4 rd v re g à?é )r© r® Úsu PWM-é度å
³ç³»r r r c S s g | ]}|j d qS )rn r rH r3 r3 r4 rd s c S rb r3 r rH r3 r3 r4 rd re ro z--)r¬ Z linestyler¨ u èµæºPWMå移 (ç¸å¯¹1500)u è§é度 (rad/s)u èµæºPWM-è§é度å
³ç³»zpwm_relationships.png)r r )rz r{ r| r c r} r~ r rH r r3 r4 rd ru c S rb r3 rf rH r3 r3 r4 rd re )r± r² r© r¨ u ç¸å¯¹æ¶é´ (ç§)u å é/å¶å¨ç¹æ§zacceleration.png)#rO ÚmkdirÚpltZsubplotsr> ZplotZ
|
set_xlabelZ
|
set_ylabelZ tick_paramsZgridZtwinxr% rZ Úzipr ZaxvspanÚtextrU ÚtitleZ tight_layoutZsavefigÚcloseÚcmZtab10r[ Zlinspacer@ Úkeysr\ Z set_titler ZlegendZscatterZaxhlineZaxvline)!r1 r¥ Zfigr- r r Z throttlesZax2Z current_stateZstate_start_timer] ÚtrJ ZxsZysZ state_colorsZ color_idxZ colors_listr% Zxs_stateZys_stateZax1r r r r r r r Zsteering_offsetsr r Z times_relr3 )r^ r1 r4 Úgenerate_plots# sÞ ÿ
|
ÿ
|
|
|
|
z"CalibrationAnalyzer.generate_plotsÚ output_filec C s t d | ¡ }| ¡ }| ¡ }t| jt| j| jd j| jd j d t dd | jD t
|
dd | jD t dd | jD t
|
d d | jD d
|
d d d | j ¡ D d|||d| |||¡d}t|ddd}tj||ddd W d n1 sw Y t d| |S )u çæJSONåæ°æ¥åu
|
=== çæJSONåæ°æ¥å ===r¡ r r c s rF r= rG rH r3 r3 r4 rK ¼ rL z;CalibrationAnalyzer.generate_json_report.<locals>.<genexpr>c s rF r= rG rH r3 r3 r4 rK ½ rL c s rF r= rN rH r3 r3 r4 rK ¾ rL c s rF r= rN rH r3 r3 r4 rK ¿ rL z;ENU coordinates computed onboard, relative to first RTK fix)Zx_minZx_maxZy_minZy_maxZnotec S s i | ] \}}|t |qS r3 )r )rI r% r^ r3 r3 r4 Ú
|
<dictcomp> s z<CalibrationAnalyzer.generate_json_report.<locals>.<dictcomp>)r< Z total_samplesZduration_secondsZ enu_rangeZstates)Ú forward_modelÚsteering_modelÚdelays)ZmetadataZdifferential_drive_modelÚrecommended_configÚwrB )rD r F)ÚindentZ ensure_asciiNu JSONæ¥åå·²ä¿å: )rO r r r¤ r: r< r r> r$ rT rU r@ r\ Ú _generate_recommended_configrP ÚjsonÚdump)r1 rÀ r rà rÄ ÚreportrV r3 r3 r4 Úgenerate_json_report® s4 ûõý í ÿz(CalibrationAnalyzer.generate_json_reportr rà rÄ c C s i }d|v r|d d |d< |d d |d< d|v r"|d d |d< d|v r8|d r2t |d ¡ nd }||d
|
< d |v rDt|d |d < d |v rN|d |d< |S )u çææ¨èçé
ç½®åæ°ry ro ZMC_CFG_FORWARD_Krp ZMC_CFG_FORWARD_BIASr ZMC_CFG_STEERING_Kr g ð?ZMC_CFG_MAX_ACCELERATIONr ZMC_CFG_MAX_DECELERATIONr£ ZMC_CFG_RESPONSE_DELAY_MS)rU Úvaluesr )r1 r rà rÄ ZconfigZ max_accelr3 r3 r4 rÈ Ó s z0CalibrationAnalyzer._generate_recommended_configN)r6 r7 r8 r9 r: r5 rX r_ r r r r¤ r r¿ rÌ rÈ r3 r3 r3 r4 r; O s M3 %r; c C sV t jdd} | jdtdd | jddtdd d
|
| ¡ }t|j}| ¡ s1td | t
|
d ¡ t|j }td td td t t|}| ¡ | ¡ | |¡ |d }| |¡}td td td |d ¡ D ]#\}}t|trtd|dd|dd qptd|dd| d qptd td td| ¡ td d S )Nu* 车è¾è¿å¨æ¨¡åæ ¡åæ°æ®åæå·¥å
·)Ú descriptionr< u& æ ¡åæ¥å¿æä»¶è·¯å¾ (.logæ.txt))ÚtypeÚhelpz-oz--outputZcalibration_resultsu* è¾åºç®å½ (é»è®¤: calibration_results))rÏ ÚdefaultrÐ u é误: æ¥å¿æ件ä¸åå¨: r z<============================================================zcalibration_parameters.jsonz=
|
============================================================u4 æ¨èçé
ç½®åæ° (å¯å¤å¶å°motion_config.h):rÅ z#define z<35r r zf)ú)u åæå®æï¼u è¾åºç®å½: )ÚargparseÚArgumentParserÚ add_argumentr: Ú
|
parse_argsr r< ÚexistsrO ÚsysÚexitÚoutputr; rX r_ r¿ rÌ r\ Ú
|
isinstancer( Zabsolute) ÚparserÚargsr< r¥ ZanalyzerZ json_filerË ÚkeyÚvaluer3 r3 r4 Úmainï s> ÿ
|
|
|
|
|
|
rà Ú__main__)!r9 rÓ rÉ ÚrerØ Zpathlibr Útypingr r r Znumpyr[ Z
|
matplotlibZmatplotlib.pyplotZpyplotr· Zscipyr Ú collectionsr ÚplatformÚsystemZrcParamsÚpathrR r: Ú__file__Úparentr r; rà r6 r3 r3 r3 r4 Ú<module> sB ' #3
|
ÿ
|
