ó
|
M»iq ã ó S r SSKJrJrJrJrJrJr SSKrS r " S S5 r
|
\ S:X aÜ SSK r SS/SS/S S//r \
|
" \ 5 r\R S
|
S 5 \" S 5 H¦ r\R% 5 r\" \\5 \S S- \R* - r\R. \S S- \R0 - S- - r\R4 S \\" \5 - S- - r\R4 S \\" \5 - S- - r \R \\ 4\5 M¨ gg)u
|
MowerController
|
|
æ§å¶å¨å设ä¸å¥çº¦ï¼
|
- åæ ç³»ï¼XY å¹³é¢åä½ä¸ºç±³ï¼RTK æä¾ï¼ï¼Z 忽ç¥ç¨äºå¹³é¢è·è¸ªã
|
- èªåï¼headingï¼ï¼å¼§åº¦ï¼èå´ä»»æï¼æ§å¶å¨ä¼å¤çç¯ç»ã
|
- GPS é¢çï¼ä½ç½®/é度/heading/attitudeï¼çº¦ 10Hzï¼IMUï¼å é度ãè§é度ï¼100Hzã
|
- æ§å¶è¾åºé¢çï¼74Hzï¼è¿å两个信å·ï¼forwardï¼-100..100ï¼å¯¹åº -1..+1 m/sï¼å turnï¼-100..100ï¼å¯¹åº -pi/2..+pi/2 rad/sï¼æ£ä¸ºå·¦è½¬ï¼ã
|
- è·¯å¾æ件ï¼JSON å表 [[x,y],[x,y],...]
|
|
设计说æï¼
|
- 使ç¨çº¯è¿½è¸ªï¼pure pursuitï¼æ¹æ³éååè§ç¹ï¼å¹¶åºäºè§åº¦è¯¯å·®ä½¿ç¨ P æ§å¶è®¡ç®ææåèªéçï¼rad/sï¼ã
|
- åè¿é度ä¸åèªè¯¯å·®è¦åï¼åèªè¯¯å·®å¤§æ¶éä½åè¿é度以ä¿è¯å¯è½¬å¼¯è½åã
|
|
æ¥å£ï¼
|
- controller = MowerController(path_points, params)
|
- controller.update_gps(pos_xy, heading, vel_xy, timestamp)
|
- controller.update_imu(accel3, gyro3, timestamp)
|
- controller.compute_control(timestamp) -> {'forward': int, 'turn': int, 'info': {...}}
|
|
é )Úatan2ÚhypotÚsinÚcosÚpiÚacosNc ó U [ :¼ a U S[ - - n U [ :¼ a M U [ * : a U S[ - - n U [ * : a M U $ )Né )r )Úas Ú=d:\Users\zhyin\PycharmProjects\Lowanlower\mower_controller.pyÚ
|
wrap_angler sE à
|
r'Ø QV ð r'à
|
rc'Ø QV ð rc'à Hó c óN \ rS rSrS S jrS S jrS S jrS rS rS r S S jr
|
S
|
r g) ÚMowerControlleré# Nc ó U Vs/ sH n[ U5 PM nn[ U5 S:X a [ S5 eU=( d 0 nUR SS5 n/ n[ [ U5 S-
|
5 H nXH u pXHS- u p¼UR X45 X¹-
|
n XÊ-
|
n[ XÞ5 nXö: d M> US: d MF [ Xö- 5 n[ SUS- 5 H( nUUS- - nUR XU- - X®U- - 45 M* M UR US 5 Xpl [ U R 5 U l / U l
|
S/U l [ U R S-
|
5 H nU R U u nnU R US- u nn[ UU-
|
UU-
|
5 nU R R U5 U R R U R S U- 5 M U R ( a U R S OSU l SU l U=( d 0 nUR S S5 U l UR S
|
S 5 U l UR S [ S - 5 U l UR SS 5 U l SU l SU l UR SS5 U l UR SS5 U l UR SS5 U l UR SS 5 U l UR SS5 U l UR SS5 U l UR SS5 U l UR S S 5 U l UR S S 5 U l UR S S 5 U l UR S!S"5 U l UR S#S$5 U l UR S%S$5 U l! UR S&S5 U l" UR S'S 5 U l# SU l$ UR S(S)5 U l% SU l& S U l' SU l( SU l) SU l* SU l+ UR S*S+5 U l, UR S,S+5 U l- SU l. UR S-S5 U l/ S.U l0 S U l1 S U l2 S/U l3 S U l4 S U l5 S0U l6 S1U l7 U Rl U l8 UR S2S35 U l9 UR S4S+5 U l: UR S5S 5 U l; UR S6S5 U l< UR S7S5 U l= UR S8[ S9- 5 U l> UR S:S;5 U l? S<U l@ UR S=S5 U lA UR S>S"5 U lB UR S?S@5 U lC SU lD SU lE S<U lF SAU lG [ SBSC5 U lI U R R SD5 g s snf )ENr zpath_points cannot be emptyÚmax_segment_lengthç333333ã?é çíµ ÷Æ°>éÿÿÿÿç Úmax_forward_mpsÚmax_reverse_mpsçÉ?Ú max_yawrateé ÚaccelÚ k_headinggÍÌÌÌÌÌü?Ú k_heading_dg Ð?Úk_xteç333333ó?Úk_curvÚ
|
base_speedç à?Úlookahead_timeg @Ú lookahead_mingffffffÖ?Ú lookahead_maxç ø?Úgoal_toleranceç333333Ó?Ú slow_zoneÚfinal_approach_distçé?Úmax_xteç ð?Úxte_gain_scaleÚcurv_gain_scaleÚheading_err_gain_scaleÚ yawrate_alphagÍÌÌÌÌÌì?Ú k_heading_ig{®Gáz?Úk_xte_iÚlarge_err_thresholdé
|
)r r Ú
|
goto_startÚ follow_pathÚstart_toleranceg¸
ëQ¸¾?Ústart_heading_toleranceÚstart_approach_distÚstart_slow_speedÚstart_min_speedÚstart_turn_first_headingé2 Úmin_follow_speedg{®Gáz´?FÚ recovery_distÚrecovery_exit_distÚrecovery_speedgá?Úrotatezcontroller_log.csvÚwzgt,x,y,heading,target_x,target_y,target_dist,target_idx,xte,heading_err,forward,turn,speed,stage,status
|
)KÚtupleÚlenÚ
|
ValueErrorÚgetÚrangeÚappendr ÚintÚpathÚnÚsegment_lengthsÚ cum_lengthsÚ total_lengthÚ last_progressr r r r r Úcurrent_speed_targetÚ current_speedr r r! r# r$ r& r' r( r* r, r- r/ r1 r2 r3 Úyawrate_filterr4 Úlast_heading_errÚ last_timeÚ last_curvÚheading_err_sumÚxte_sumÚmax_sumr5 r6 Úconsecutive_large_errr7 Úmax_large_err_countÚposÚheadingÚvelÚ imu_accelÚimu_gyroÚSTAGE_GOTO_STARTÚSTAGE_FOLLOW_PATHÚstager; r< r= r> r? r@ rB Ú in_recoveryrC rD rE Úcurrent_target_idxÚnearest_path_idxÚfinishedÚ goto_substageÚopenÚlog_fileÚwrite)ÚselfÚ path_pointsÚparamsÚpÚ original_pathÚp_paramsÚ max_seg_lenÚ densifiedÚiÚx1Úy1Úx2Úy2ÚdxÚdyÚseg_lenÚstepsÚkÚratioÚaxÚayÚbxÚbyÚLs r Ú__init__ÚMowerController.__init__$ s ñ ,7Ó7©; aq©; Ð7Ü }Ó Ó "Ü Ð :Ó;Ð ;à<RØ llÐ#7¸Ó= Ø Üs=Ó)¨AÑ-Ö.AØ"Ñ%FBØ" q¡5Ñ)FBØ × Ñ bXÔ &ØBØBÜBmGØÕ$¨°4 äGÑ 2Ó3ä q %¨!¡)Ö,AØ ¨¡OEØ ×$Ñ$ b°©:¡o°rÀ¹J±Ð%GÖHó -ñ /ð ×Ñ rÑ*Ô+Ø ÜTYY à !Ô Ø 5ÔÜtvv zÖ"AØYYq\FBØYYq s^FBÜbeR UÓ#AØ × Ñ × 'Ñ '¨Ô *Ø × Ñ × #Ñ # D×$4Ñ$4°RÑ$8¸1Ñ$<Ö =ñ #ð 59×4D×4DD× ,Ñ ,¨RÒ 0È#Ôà Ôð LbØ uuÐ%6¸Ó <Ô Ø uuÐ%6¸Ó <Ô Ø 55 ´°1±Ó5Ôð UU7 CÓ(
|
Ø$'Ô!Ø Ôð {¨CÓ0Ø 55 °Ó5ÔØUU7 CÓ(
|
ØeeH cÓ* Ø%% ¨cÓ2ð eeÐ$4°cÓ :Ôà UU ?°DÓ 9ÔØ UU ?°CÓ 8ÔØ eeÐ$4°cÓ :ÔØ{¨CÓ0Ø#$§5¡5Ð)>ÀÓ#DÔ ð uuY¨Ó, Ø eeÐ$4°cÓ :Ôð !uuÐ%6¸Ó <Ô Ø&'§e¡eÐ,DÀcÓ&JÔ#ð "ÔØ UU ?°DÓ 9ÔØ #Ô Ø Ø ð #Ô Ø Ø Ø 55 °Ó5ÔØuuY¨Ó- ð &'Ô"Ø#$§5¡5Ð)>ÀÓ#DÔ Ø#%Ô ð
|
Ø Ø Ø Ø ð !-Ô Ø!.Ô Ø×*Ñ*
|
ð !uuÐ%6¸Ó =Ô Ø'(§u¡uÐ-FÈÓ'MÔ$Ø#$§5¡5Ð)>ÀÓ#DÔ Ø !§¡Ð&8¸$Ó ?Ô Ø uuÐ%6¸Ó =Ô à()¯©Ð.HÌ"ÈRÉ%Ó(PÔ%à !§¡Ð&8¸$Ó ?Ô à ÔØ UU ?°CÓ 8ÔØ"#§%¡%Ð(<¸cÓ"BÔ Ø eeÐ$4°dÓ ;Ôð #$Ô Ø !Ô Ø à %Ôä Ð 1°3Ó7 à ×Ñð Gõ HùòG 8s
W;c ól [ U5 U l [ U5 U l Ub [ U5 U l gg)zUpdate GPS-like measurements at ~10Hz.
|
|
pos_xy: (x,y) in meters
|
heading: radians
|
vel_xy: (vx, vy) in m/s in global frame (optional)
|
N)rH r` Úfloatra rb )rp Úpos_xyra Úvel_xyÚ timestamps r Ú
|
update_gpsÚMowerController.update_gps¬ s0 ô =Ü W~ Ø Ñ Ü V}DHð r c óD [ U5 U l [ U5 U l g)z:Update IMU (100Hz). accel3 and gyro3 are 3-element tuples.N)rH rc rd )rp Úaccel3Úgyro3r s r Ú
|
update_imuÚMowerController.update_imu¸ s ä v Ü e r c óX Uu pEUu pgUu pX-
|
n
|
X-
|
n Xª- X»- - n U S:X a [ XF-
|
XW-
|
5 $ XF-
|
U
|
- XW-
|
U - - U - n U S: a [ XF-
|
XW-
|
5 $ U S: a [ XH-
|
XY-
|
5 $ XmU
|
- - nX}U - - n[ XN-
|
X_-
|
5 nXN-
|
U - X_-
|
U
|
- -
|
S: a SOSnUU- $ )zDCalculate cross track error (perpendicular distance to line segment)r r r )r )rp Úp1Úp2r` ÚxÚyry rz r{ r| r} r~ Úl2ÚtÚpxÚpyÚxteÚsigns r Ú _calc_xteÚMowerController._calc_xte½ sè àØØð WØ WØ URU]à 7Ü ¡Ó(Ð (ð fb[AF B;Ñ &¨"Ñ ,à q5Ü ¡Ó(Ð (Ø UÜ ¡Ó(Ð (àdBØdBÜ ¡Ó'Cà q¡v¨r¡kÑ1°AÓ51¸2DØ#:Ð r c óP U R c U R S SSS4$ U R u p#U R U R u pE[ XB-
|
XS-
|
5 nUS: a Sn[ U R 5 nOE[ SU R S-
|
5 n[ [ U R 5 U R S- 5 nU R n [ S5 n
|
[ Xx5 H/ n U R U u pE[ XB-
|
XS-
|
5 n XÊ: d M+ U n
|
U n M1 Xl U n [ U [ U R 5 5 H- n U R U u pE[ XB-
|
XS-
|
5 n XÁ:¼ d M+ U n O [ U R 5 S-
|
n SnSU s=:: a [ U R 5 S-
|
: aï O OìU R U S-
|
nU R U nU R U S- nUS US -
|
US US -
|
4nUS US -
|
US US -
|
4n[ US US 5 n[ US US 5 nUS: ac US: a] US US - US US - - n[ S [ SUUU- - 5 5 n[ [ [ U5 -
|
5 nU[ US5 - nU R U XíU 4$ )
|
uÐ
|
ç®åçPure Pursuitåè§ç¹æ¥æ¾ - åªä½¿ç¨ç¹ç´¢å¼
|
|
ä»nearest_path_idxå¼å§ååæç´¢ï¼æ¾å°ç¬¬ä¸ä¸ªè·ç¦»>=lookahead_distçç¹
|
|
Returns: (lookahead_xy, curvature, lookahead_idx, nearest_idx)
|
r r r0 é é Úinfr r ç ð¿) r` rO rj r rI ÚmaxÚminr rL Úabsr r )rp Úlookahead_distr r r r Úcurrent_nearest_distÚ search_startÚ
|
search_endÚ nearest_idxÚ nearest_distrx ÚdÚ lookahead_idxÚ curvaturer r Úp3Úv1Úv2Úl1r ÚdotÚ cos_angleÚ angle_changes r Ú_find_lookahead_pointÚ%MowerController._find_lookahead_pointÚ sª ð 88Ñ Ø99Q< a¨Ð*Ð *àxxð 4×0Ñ0Ñ1Ü $ R¡V¨R©VÓ 4Ð ð #Ó %Ø LÜ TYY Jô q $×"7Ñ"7¸!Ñ";Ó<LÜ S §¡^¨T×-BÑ-BÀRÑ-GÓHJà×+Ñ+ Ü U| ä|Ö0AØYYq\FBÜbfbfÓ%AØÕ Ø Ø ñ 1ð !,Ô ð $ Ü{¤C¨¯ © £NÖ3AØYYq\FBÜbfbfÓ%AØÕ"Ø ! Ùñ 4ô § ¡ N¨QÑ .Mð Ø Õ 0c $§)¡)n¨qÑ 0Ö 0Ø;¨?Ñ+BØ;Ñ'BØ;¨?Ñ+BàQ%"Q%- A¡¨¨A©¡ Ð/BØQ%"Q%- A¡¨¨A©¡ Ð/Bär!ub eÓ$BÜr!ub eÓ$BàDyR $YØeBqEk B q¡E¨"¨Q©%¡KÑ/Ü ¤c¨#¨s°b¸2±g©Ó&?Ó @ Ü "¤2¬¨Y«Ñ#7Ó 8 Ø (¬3¨r°4«=Ñ 8 àyy Ñ'¨À;ÐNÐNr c óÀ Uu p#U R nU R U u pVU[ U R 5 S-
|
: a U R US- u pxXu-
|
n X-
|
n
|
X- Xª- - n U S: ac X%-
|
U - X6-
|
U
|
- - U - n [ S[ SU 5 5 n X\U - - n XlU
|
- - nX--
|
nX>-
|
n[ UU5 nXú- UU - -
|
nUS: a UOU* nUXÞ4$ [ X%-
|
X6-
|
5 nUXV4$ )uO
|
ç®åç横åè¯¯å·®è®¡ç® - åºäºæè¿ç¹
|
|
Returns: (xte, proj_x, proj_y)
|
r gÖ&è .>r r0 r )rj rO rI r¨ r© r )rp r` r r r¯ r r ÚnxÚnyr} r~ Úseg_len2r Úproj_xÚproj_yÚ xte_vec_xÚ xte_vec_yÚxte_magÚcrossr s r Ú_compute_xte_simpleÚ#MowerController._compute_xte_simple" s ð Ø×+Ñ+ ð ;Ñ'ð TYY ¨!Ñ+Ó +ØYY{¨QÑ/FBð BØBØuru}Hà$àfb[ A¡F¨B¡;Ñ.°(Ñ:ÜS a[Ó)à "fØ "fð J Ø J Ü ¨9Ó5ð "¨°R© Ñ7Ø!&¨£g°°àFÐ*Ð*ô AFAFÓ#ØB{Ðr c óä) U R ( a SSSS0S.$ U R b U R c SSSS0S.$ Uc [ R " 5 nU R c SnOU( a XR -
|
OSnU( a UO!U R ( a U R U- OSU l U R u p4U R
|
U R :X Ga¤ U R S nUS U-
|
nUS U-
|
n[ Xg5 n[ Xv5 n [ XR -
|
5 n
|
[ U S
|
5 ( a U R c S U l U R S :X Ga [ U
|
5 U R :: a S U l OìU R U
|
- S - n [! [# X°R$ - S- 5 5 n [' S[) SU 5 5 n Sn Uu pï[ Xã-
|
Xô-
|
5 nSnU R* R- US SUS SUS SU R S SUS SUS SUS SU SU
|
S SU SU SU R
|
S35 U R* R/ 5 XÜSU R
|
U
|
US.S.$ U R S :X Ga U n[ UU R -
|
5 nXR0 : a SU l GOäXR2 : a@ XR2 - nSSU- - nU R4 U R6 U R4 -
|
U- - nOSn[) U R6 S- S5 n[ U5 [8 - nSSSU-
|
- - n[' U R: S - UU- U- 5 n[' UU R: S - 5 nU R U- n [! [# UU R< - S- 5 5 n [' S[) SU 5 5 n [! [# X°R$ - S- 5 5 n [' S[) SU 5 5 n Uu pï[ Xã-
|
Xô-
|
5 nSnU R* R- US SUS SUS SU R S SUS SUS SUS SU SUS SU SU SUS SU R
|
S 35 U R* R/ 5 XÜS U R
|
UUS .S.$ U R S:X GaÝ [? U R 5 S : aW U R S S U R S S -
|
nU R S S U R S S -
|
n[ UU5 n OSn [ U U R -
|
5 n XR0 : Ga9 [ U 5 U R@ : Ga U RB U l SU l SU l" U R: U l# [! [# U R: U R< - S- 5 5 n U R S u pï[ Xã-
|
Xô-
|
5 nSnU R* R- US SUS SUS SU R S SUS SUS SUS SU SU S SU S!U R: S SU R
|
S"35 U R* R/ 5 U SS#U R
|
U RD S$.S.$ U S:¼ a S OS%n U [) U R$ S- [' S&[ U 5 S- 5 5 - n [! [# X°R$ - S- 5 5 n [' S[) SU 5 5 n Uu pï[ Xã-
|
Xô-
|
5 nSnU R* R- US SUS SUS SU R S SUS SUS SUS SU SU S S!U SSS SU R
|
S'35 U R* R/ 5 SU S(U S).S.$ U R c SnOU( a XR -
|
OSnU( a UO!U R ( a U R U- OSU l U R u p4U RI U R 5 u n n!n"[ U R S% S U-
|
U R S% S U-
|
5 n#[ U RF 5 n$[ U 5 n%U RJ U RL U$- - S*[) U%S5 - -
|
n&[' U RJ [) U RN U&5 5 n'[ U 5 n(U RP ( d U(U RR : a S+U l( U R* R- US SUS SUS SU R S SU!S SU"S SU(S SU RT SU S S,U R
|
S-35 U R* R/ 5 U RP ( a U(U RV : a S.U l( U R* R- US SUS SUS SU R S SU!S SU"S SU(S SU RT SU S S,U R
|
S/35 U R* R/ 5 U RY U'5 u u n)n*n+n,n-U,U l" [ U*U-
|
U)U-
|
5 n.[' S0[) SU U RZ - 5 5 n/U R\ * U/- U R^ - n0[ U.U0- 5 n[ UU R -
|
5 nUS: a UU R` -
|
U- OSn1UU l0 U =Rb UU- - sl1 U =Rd U U- - sl2 [' U Rf * [) U Rf U Rb 5 5 U l1 [' U Rf * [) U Rf U Rd 5 5 U l2 U R U- U Rh U1- - U Rj U Rb - - U Rl U Rd - - n SS[ U+5 U Rn - - - n2[ U5 n3SSU3U Rp - - - n[ U 5 n4SSU4U R^ - - - n5U2U- U5- S1- n6[) U2UU55 n7SU6- S2U7- - n8U R6 U8- nU Rr U - S U Rr -
|
U Rt - - U l: U R$ n9[' U9* [) U9U Rt 5 5 n U RP ( a [ U"U-
|
U!U-
|
5 n.[' S0[) SU U RZ - 5 5 n/U R\ * U/- U R^ - n0[ U.U0- 5 n[ UU R -
|
5 nU R U- n U Rv nU#U Rx : n:U#U Rz : n;[ U 5 U R| S - : d [ U5 [8 S3- : aE U =R~ S - sl? U R~ U R : a SU l? SU l1 SU l2 OSU l? U RP ( d U:( aK [' S*[) SU#U Rx - 5 5 n<U;( a U<[' SU#U Rz - 5 - n<UU<- nU;( a/ US- n[ U 5 S4: d [ U5 [8 S5- : a US6- nU R
|
U RB :X aÞ U RP ( dÍ U:( a U R OU R S - n=[ U5 S7: a UU=: a U)U*pþ[ Xã-
|
Xô-
|
5 nU R* R- US SUS SUS SU R S SUS SUS SUS SU RD SU S SUS S8U=S SU R
|
S935 U R* R/ 5 U=nU R U:( a SOS- n>UU RF -
|
n?U>U- n@U =RF [' U@* [) U@U?5 5 - sl# U R
|
U R :X a% [ U5 S:: a U R * S- U l# O[[ U5 S:: a& U RP ( d U R * S- U l# U RF S: a [ U5 S3: a SU l# U#U R : a S+U l SSSS0S.$ U#U R S - : aJ U RD U R S;-
|
:¼ a- [ U5 S*: a [ U 5 S2: a S+U l SSSS0S.$ Ub UO[ R " 5 nAU RF nBU)U*pþ[ Xã-
|
Xô-
|
5 nU R< S:w a$ [! [# WBU R< - S- 5 5 OSnC[' S[) SUC5 5 nCU R$ S:w a# [! [# X°R$ - S- 5 5 OSnD[' S[) SUD5 5 nDU R* R- WAS SUS SUS SU R S SUS SUS SUS SU RD SU S SUS SWC SUD SWBS SU R
|
S<3 5 U R* R/ 5 U R< S:w a. [! [# U RF U R< - S- 5 5 OSn [' S[) SU 5 5 n U S:X Ga@ U R ( Gd. [ U5 S7: Ga U R< S:w a. [! [# U R U R< - S- 5 5 OSnEUES:w aØ U)U*pþ[ Xã-
|
Xô-
|
5 nU R* R- WAS SUS SUS SU R S SUS SUS SUS SU RD SU S SUS SWE S[! [# X°R$ - S- 5 5 SU R S SU R
|
S=3 5 U R* R/ 5 UEn U R U l# U R$ S:w a# [! [# X°R$ - S- 5 5 OSn [' S[) SU 5 5 n U R ( a SOS>nFUFX44U R UUU RF U U)U*4U+U#S?.
|
nGWAnHU)U*pþ[ Xã-
|
Xô-
|
5 nU R* R- UHS SUS SUS SU R S SUS SUS SUS SU RD SU S SUS SU SU SU RF S SU R
|
SUF S@3 5 U R* R/ 5 XÜUGS.$ )AztCompute forward and turn outputs mapped to [-100,100].
|
|
Returns dict with keys: forward(int), turn(int), info(dict)
|
r Ústatusrk )ÚforwardÚturnÚinfoNÚno_stategyé&1¬?r r rl rF ÚmovegÍÌÌÌÌÌô?éd iÿÿÿz.3fÚ,z,0.000,z,rotate_to_start
|
Úrotate_to_start)rÊ rg Ú heading_errÚ dist_to_startÚaligngÙ?r r0 r. r% r) r" z,moving_to_start
|
Úmoving_to_start)rÊ rg rÔ rÓ z,0,z,reached_start
|
Ú reached_start)rÊ rg Ú advanced_idxr g©?z,align_at_start
|
Úalign_at_start)rÊ rÓ r+ Tz,0.000,0,0,0.000,z,enter_recovery
|
Fz,exit_recovery
|
r§ gUUUUUUÕ?r g @g333333Ã?r8 gffffffæ?g @z,0,0,z,enforce_min_speed
|
g333333@r
|
z ,pre_map
|
z,force_min_forward
|
Úrunning)
|
rÊ r` ra Údesired_headingrÓ rV Ú yawrate_cmdÚ target_xyÚpath_curvatureÚ
|
final_distÚ
|
)Frk r` ra ÚtimerY rg re rO r r r Úhasattrrl rª r@ r rN Úroundr r¨ r© rn ro Úflushr; r= r> r$ r r? r rI r< rf ri rV rÇ r' r& r( rh rC rj rD r» r/ r! r1 rX r[ r\ r] r r5 r6 r2 r3 r4 rW rE r, r- r7 r^ r_ rB r r r* rP )Irp r Údtr r Ú start_pointr} r~ rÔ Údesired_heading_to_startÚheading_err_to_startrÜ Ú turn_signalÚforward_signalÚtarget_xÚtarget_yÚ target_distÚ
|
target_idxrÛ rÓ Ú
|
dist_ratioÚ speed_scaleÚbase_target_speedÚ heading_ratioÚ err_scaleÚ target_speedÚpath_dxÚpath_dyÚdesired_start_headingÚheading_err_finalÚf_sigr r rÁ r rß Ú actual_speedrÅ Úbase_lookaheadÚ lookaheadÚ dist_to_pathÚlxÚlyrÞ r² r¯ Ú path_headingÚxte_normalizedÚxte_correctionÚ heading_err_dÚ
|
curv_scaleÚheading_err_absÚxte_absÚ xte_scaleÚ
|
geom_scaleÚ min_scaleÚ blend_scaler Ú in_slow_zoneÚin_final_approachÚ
|
dist_scaleÚ dynamic_minr Ú speed_changeÚ
|
max_changeÚlog_tÚ pre_map_speedÚpre_map_forwardÚ pre_map_turnÚ forced_signalrÊ rÍ r sI r Úcompute_controlÚ MowerController.compute_controlN së ð
|
==Ø ¨!°hÀ
|
Ð5KÑLÐ Là 88Ñ t||Ñ 3Ø ¨!°hÀ
|
Ð5KÑLÐ Lð Ñ Ü Ið >>Ñ !ØBæ/8^^Ò+¸eBÞ&/ÈTÏ^Ï^°d·n±nÀrÒ6IÐadàxxð ::×.Ñ.Ô .Ø )) A,KØ Q !Ñ#BØ Q !Ñ#BÜ ! "MMô (-¨R£}Ð $Ü#-Ð.FÏ É Ñ.UÓ#VÐ ô 4 ×1Ñ1°T×5GÑ5GÑ5OØ%-Ô"ð ×!Ñ! XÔ-äÐ+Ó,°×0MÑ0MÓMØ)/DÕ&à"&§.¡.Ð3GÑ"GÈ#Ñ"MKÜ"%¤e¨[×;KÑ;KÑ-KÈsÑ,RÓ&SÓ"TKÜ"% d¬C°°[Ó,AÓ"BKØ%&Nà)4Ñ&HÜ"'¨© °h±lÓ"CKØ!"JØMM×'Ñ'¨9°S¨/¸¸1¸S¸'ÀÀ1ÀSÀ'ÈÈ4Ï<É<ÐX[ÐJ\Ð\]Ø+3°C¨.¸¸(À3¸ ÀqÈÐUXÐHYÐYZÐ[eÐZfð g1Ø1EÀcÐ0JÈ!ÈNÐK[Ð[\Ð]hÐ\iÐipÐqu×q{Ñq{Ðp|ð }Oð)Pô Qð MM×'Ñ'Ô)Ø'5Ø"3¸d¿j¹jÐYmØ)6ñUñ ð ð
|
×!Ñ! VÔ+à":Ü (¨ ¸4¿<¹<Ñ)GÓ H ð !×#7Ñ#7Ó7Ø)0DÖ&ð %×'?Ñ'?Ó?Ø%2×5MÑ5MÑ%M
|
Ø&)¨C°*Ñ,<Ñ&< Ø,0×,AÑ,AÀTÇ_Á_ÐW[×WlÑWlÑElÐpzÑDzÑ,zÑ)à&) Ü,/°·±À#Ñ0EÀsÓ,KÐ)ô %(¨ Ó$4´rÑ$9MØ # c¨S°=Ñ-@Ñ&AÑ AIÜ#& t×';Ñ';¸cÑ'AÐCTÐWbÑCbÐenÑCnÓ#oLÜ#& |°T×5IÑ5IÈCÑ5OÓ#PLà"&§.¡.°;Ñ">KÜ%(¬° ¸t×?SÑ?SÑ0SÐWZÑ/ZÓ)[Ó%\NÜ%(¨¬s°3¸Ó/GÓ%HNÜ"%¤e¨[×;KÑ;KÑ-KÈsÑ,RÓ&SÓ"TKÜ"% d¬C°°[Ó,AÓ"BKà)4Ñ&HÜ"'¨© °h±lÓ"CKØ!"JØMM×'Ñ'¨9°S¨/¸¸1¸S¸'ÀÀ1ÀSÀ'ÈÈ4Ï<É<ÐX[ÐJ\Ð\]Ø)1°#¨°a¸À°~ÀQÀ{ÐSVÐFWÐWXÐYcÐXdð e/Ø/:¸3Ð.?¸qÀ Ð@PÐPQÐR]ÐQ^Ð^_Ð`lÐmpÐ_qÐqrÐsw×s}Ñs}Ðr~ð Qð)Rô Sð MM×'Ñ'Ô)à'5Ø"3¸d¿j¹jÐ[hØ'2ñUñ ð ð ×!Ñ! WÔ,Ütyy> AÓ%Ø "ii¨l¨1o°· ± ¸!± ¸Q±Ñ ?GØ "ii¨l¨1o°· ± ¸!± ¸Q±Ñ ?GÜ,1°'¸7Ó,CÑ)à,/Ð)Ü$.Ð/DÀtÇ|Á|Ñ/SÓ$TÐ!ð "×$8Ñ$8Ô8¼sÐCTÓ?UÐX\×XtÑXtÔ?tØ!%×!7Ñ!7DJØ)-DÔ&à./DÔ+à)-×)=Ñ)=DÔ&ä ¤ t×';Ñ';¸d×>RÑ>RÑ'RÐVYÑ&YÓ ZÓ [EØ)-¯©°1© Ñ&HÜ"'¨© °h±lÓ"CKØ!"JØMM×'Ñ'¨9°S¨/¸¸1¸S¸'ÀÀ1ÀSÀ'ÈÈ4Ï<É<ÐX[ÐJ\Ð\]Ø+3°C¨.¸¸(À3¸ ÀqÈÐUXÐHYÐYZÐ[eÐZfð g1Ø1BÀ3Ð0GÀqÈÈÈsÐSW×SgÑSgÐhkÐRlÐlmÐnr×nxÑnxÐmyð zJð)Kô Lð MM×'Ñ'Ô)Ø',°aÈOÐfj×fpÑfpð CG÷ CZñ CZñ B[ñ \ð \ð !2°QÓ 61¸BDØ"&¬¨T×-=Ñ-=ÀÑ-CÄSÈÌsÐSdÓOeÐhkÑOkÓElÓ)mÑ"mKÜ"%¤e¨[×;KÑ;KÑ-KÈsÑ,RÓ&SÓ"TKÜ"% d¬C°°[Ó,AÓ"BKØ)4Ñ&HÜ"'¨© °h±lÓ"CKØ!"JØMM×'Ñ'¨9°S¨/¸¸1¸S¸'ÀÀ1ÀSÀ'ÈÈ4Ï<É<ÐX[ÐJ\Ð\]Ø+3°C¨.¸¸(À3¸ ÀqÈÐUXÐHYÐYZÐ[eÐZfð g1Ø1BÀ3Ð0GÀsÈ;È-ÐWXÐY^Ð_bÐXcÐcdÐei×eoÑeoÐdpð qBð)Cô Dð MM×'Ñ'Ô)Ø'(°+ÐRbð tEñ HFñ Gð Gð >>Ñ !ØBæ/8^^Ò+¸eBÞ&/ÈTÏ^Ï^°d·n±nÀrÒ6IÐadð xxØ "× 6Ñ 6°t·x±xÓ @ÑVVô 499 R=¨Ñ+¨aÑ/°·±¸2± ¸qÑ1AÀAÑ1EÓF
|
ô 4×-Ñ-Ó. Üc(ð ×+Ñ+¨d×.AÑ.AÀLÑ.PÑPÐSVÔY\Ð]dÐfiÓYjÑSjÑjÜ×*Ñ*¬C°×0BÑ0BÀNÓ,SÓT ô 3x Ø× × L°4×3EÑ3EÓ$EØ #DÔ Ø MM× Ñ 9¨S /°°1°S°'¸¸1¸S¸'ÀÀ4Ç<Á<ÐPSÐBTÐTUØ#)¨# ,¨a°°s¨|¸1¸\È#Ð<NÈaÐPT×PeÑPeÐOfÐfgØ#& s )Ð+<¸T¿Z¹Z¸LÐHYð![ô \ð MM× Ñ Ô !à × × ¨t×/FÑ/FÓ FØ $DÔ Ø MM× Ñ 9¨S /°°1°S°'¸¸1¸S¸'ÀÀ4Ç<Á<ÐPSÐBTÐTUØ#)¨# ,¨a°°s¨|¸1¸\È#Ð<NÈaÐPT×PeÑPeÐOfÐfgØ#& s )Ð+<¸T¿Z¹Z¸LÐHXð!Zô [ð MM× Ñ Ô !ð @D×?YÑ?YÐZcÓ?dÑ<R. -°Ø"/Ô ô R !V R¨!¡VÓ, ô T¤3 s¨C°$·,±,Ñ,>Ó#?Ó@Ø ** ~Ñ5¸×8KÑ8KÑKô % \°NÑ%BÓCÜ °4·<±<Ñ!?Ó@ ð GIÈ1Ãf t×'<Ñ'<Ñ<ÀÒBÐRS Ø +Ô ð × Ò ¨bÑ 0Ñ0Õ Ø bÑ Ü " D§L¡L =´#°d·l±lÀD×DXÑDXÓ2YÓ ZÔ ÜDLL=¬#¨d¯l©l¸D¿L¹LÓ*IÓJ ð ~~¨ Ñ3Ø×&Ñ&¨ Ñ6ñ7à×&Ñ&¨×)=Ñ)=Ñ=ñ>ð ll T§\¡\Ñ1ñ2 ð C¤# nÓ"5¸×8LÑ8LÑ"LÑ LÑM
|
ô kÓ*Ø3 °4×3NÑ3NÑ!NÑNÑO ô c(Ø3 ¨4×+>Ñ+>Ñ!>Ñ>Ñ? ð ! 9Ñ,¨yÑ8¸gÑF
|
Ü
|
I¨yÓ9 ØJÑ&¨¨y© Ñ8 à¨Ñ4 ð $× 1Ñ 1°KÑ ?Ø ×!3Ñ!3Ñ 3°t×7JÑ7JÑ Jñ KÔà×&Ñ& Ü;,¬¨K¸×9LÑ9LÓ(MÓN ð × × ä ¨!¡¨V°a©ZÓ8LÜ ¤s¨3°°d·l±lÑ0BÓ'CÓ DNØ "jj[¨>Ñ 9¸D×<OÑ<OÑ ONÜ (¨ ¸Ñ)FÓ GOÜ$ _°t·|±|Ñ%CÓDKØ ..¨;Ñ6KØ ×.Ñ.Là! D§N¡NÑ2 Ø &¨×)AÑ)AÑ AÐô s8d×.Ñ.°Ñ4Ó 4¼¸KÓ8HÌ2ÈcÉ6Ó8QØ × &Ò &¨!Ñ +Õ &Ø×)Ñ)¨D×,DÑ,DÓDà-.Ô*Ø'(Ô$Ø øà)*DÔ &ð × × ÞÜ ¤c¨#¨z¸D¿N¹NÑ/JÓ&KÓ L
|
Þ$Ø ¤# c¨:¸×8PÑ8PÑ+PÓ"QÑQJØ
|
Ñ* ö !Ø Ñ# Üs8d?¤c¨+Ó&6¼¸B¹Ó&>Ø CÑ'Lð ::×/Ñ/Ó /¸×8H×8HÞ3?$×/Ò/ÀT×EZÑEZÐ]`ÑE`KÜ;Ó #Ó%¨,¸Ó*DØ%'¨(Ü # H¡L°(±,Ó ? Ø ×#Ñ# y° o°Q°q¸°g¸Q¸qÀ¸gÀQÀtÇ|Á|ÐTWÐFXÐXYØ'/° n°A°h¸s°^À1À[ÐQTÐDUÐUVÐW[×WnÑWnÐVoÐopØ'*¨3 i¨q°¸SÐ0AÀØ'2°3Ð&7°q¸¿¹¸ ÐDXð%Zô [ð ×#Ñ#Ô%Ø * ð
|
|
¦\c°sÑ;Ø# d×&8Ñ&8Ñ8 ØRZ
|
Ø ×Òc : +¬s°:¸|Ó/LÓ MÑMÕð ::×.Ñ.Ó .Ü;Ó #Ó%Ø&*×&:Ñ&:Ð%:¸SÑ%@Ô"øä;Ó #Ó%¨d×.>×.>Ø&*×&:Ñ&:Ð%:¸SÑ%@Ô"Ø×!Ñ! AÓ%¬#¨kÓ*:¸SÓ*@Ø%(Ô"ð ×+Ñ+Ó +Ø DMØ ¨!°hÀ
|
Ð5KÑLÐ Lð ×,Ñ,¨sÑ2Ó 2Ø × #Ñ # t§v¡v°¡zÓ 1Ü Ó sÓ "¤s¨3£x°#£~Ø DMØ ¨!°hÀ
|
Ð5KÑLÐ Lð 'Ñ 2 ¼¿ º » Ø ×*Ñ* à (ÜHL¨(©,Ó7 ØVZ×VjÑVjÐnoÓVo#e ]°T×5IÑ5IÑ%IÈSÑ$PÓ QÔRÐuvÜ d¤C¨¨_Ó$=Ó>ØMQ×M]ÑM]ÐabÓMbs5 +×0@Ñ0@Ñ"@ÀCÑ!GÓHÔIÐhi Ü4¤ S¨,Ó!7Ó8 Ø ×Ñu Sk¨¨1¨S¨'°°1°S°'¸¸4¿<¹<ÈÐ:LÈAØ '¨n¨A¨h°s¨^¸1¸[ÈÐ<MÈQÈt×OfÑOfÐNgÐghØ " 3i q¨°SÐ(9¸¸?Ð:KÈ1È\ÈNÐZ[Ð\iÐjmÐ[nÐnoØ #zzl¨*ð 6ô 7ð ×ÑÔ àZ^×ZnÑZnÐrsÓZsU D×$6Ñ$6¸×9MÑ9MÑ$MÐQTÑ#TÓ UÔVÐyzÜ T¤3 s¨NÓ#;Ó<ð QÔ t§}§} }¼¸[Ó9IÈCÔ9OØ`d×`tÑ`tÐxyÓ`yC¤ t×'<Ñ'<¸t×?SÑ?SÑ'SÐWZÑ&ZÓ [Ô \ð @AMØ Ó!Ø%'¨(Ü # H¡L°(±,Ó ? Ø ×#Ñ# u¨S k°°1°S°'¸¸1¸S¸'ÀÀ4Ç<Á<ÐPSÐBTÐTUØ'/° n°A°h¸s°^À1À[ÐQTÐDUÐUVÐW[×WnÑWnÐVoÐopØ'*¨3 i¨q°¸SÐ0AÀÀ=À/ÐQRÔSVÔW\Ð^i×l|Ñl|Ñ^|ð ADñ ^Dó XEó TFð SGð GHØ'+×'<Ñ'<¸SÐ&AÀÀ4Ç:Á:À,ÐNbð%dô eð ×#Ñ#Ô%Ø!.Ø%)×%:Ñ%:Ô"àLP×L\ÑL\Ð`aÓLac% ×/?Ñ/?Ñ!?À3Ñ FÓGÔHÐgh Ü$¤ C¨Ó 5Ó6 à #}}°)à Ø6Ø||Ø .Ø&Ø !× /Ñ /Ø&Ø bØ ,Ø$ñ
|
ð Ø (ÜHL¨(©,Ó7 Ø ×Ñq g Q q¨ g¨Q¨q°¨g°Q°t·|±|ÀCÐ6HÈØ '¨n¨A¨h°s¨^¸1¸[ÈÐ<MÈQÈt×OfÑOfÐNgÐghØ " 3i q¨°SÐ(9¸¸>Ð:JÈ!ÈKÈ=ÐXYÐZ^×ZlÑZlÐmpÐYqÐqrØ #zzl¨!¨F¨8°2ð 7ô 8ð ×ÑÔ à)ÈÑMÐMr )@rf re r r$ r^ rR rV rU ri r2 r- rk r* rl ra r3 r[ rc rd rh r# r r r5 r! r6 r7 rZ rX rT rY rn r( r' r& r r_ r r] r/ r rB rP rj rO r` rC rD rE rQ r, rg r= r< r? r> r; r@ rS rb r1 r\ r4 rW )N)NN) Ú__name__Ú
|
__module__Ú __qualname__Ú__firstlineno__r r r r¡ r» rÇ r Ú__static_attributes__© r r r r # s. ôFHôP
|
%ô%ò
|
ò:FOòP*÷X@Nr r Ú__main__r¤ r8 )r r§ r é rË g Y@rÌ g¹?r ) Ú__doc__Úmathr r r r r r rá r r r ÚjsonrO Úcr rL rx r ÚoutÚprintr Úfra r Úhdr` r r r r r Ú<module>r( s ðñ÷* 2× 1Û ò ÷k
|
Nñ k
|
Nð\ zÓãØ qE1Q%AÐ DÙÓ AØLLCÔ Ù 2YØ× Ñ Ó!Ù a à NUÑ " Q×%6Ñ%6Ñ 6Ø YY#f+¨Ñ-°· ± Ñ>ÀÑDÑ DØ UU1XC G cÑ)Ñ )Ø UU1XC G cÑ)Ñ )Ø bWbÖ!ò ð r
|
