From dc744f46d6e7b8027383320921044984c1d6d37c Mon Sep 17 00:00:00 2001
From: Paul-Winpenny <92634321+Paul-Winpenny@users.noreply.github.com>
Date: Wed, 18 Dec 2024 12:36:32 +0000
Subject: [PATCH] Graph maker is now inside of the ros2 package to make it
easier to save the graph.
---
...realtime_location_cli_only.cpython-312.pyc | Bin 0 -> 13269 bytes
.../robobin/robobin/helpers/graph_maker.py | 112 ++++++++
.../helpers/realtime_location_cli_only.py | 263 ++++++++++++++++++
3 files changed, 375 insertions(+)
create mode 100644 ros2/src/robobin/robobin/helpers/__pycache__/realtime_location_cli_only.cpython-312.pyc
create mode 100644 ros2/src/robobin/robobin/helpers/graph_maker.py
create mode 100644 ros2/src/robobin/robobin/helpers/realtime_location_cli_only.py
diff --git a/ros2/src/robobin/robobin/helpers/__pycache__/realtime_location_cli_only.cpython-312.pyc b/ros2/src/robobin/robobin/helpers/__pycache__/realtime_location_cli_only.cpython-312.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..c9190690f3276b2c8cc4075a9a6b33e305014630
GIT binary patch
literal 13269
zcmX@j%ge>Uz`$^Je^UAv4F-nCAPx+(KpCG47#SF*Go&!2Fy=5sL1@M(CNQ5liaCWL
zg(-(6mo<u&ks+NSg?SNU6k9q&3d<tKDE4%Q6xKzIQ5;MR?hGkxEet8_tC=9CMsb40
zI9eD|IN@SksoW`C*%}NCMfFS!saz@CDLiX<;j%oO3``8EyeWLE86jq5$wFvkay68b
zB@bmFQ82kEfmDGMey}|@sXSTANJ^kgm`X;FjY25`FdG>eI2lrTveco<kSG)#3|SgT
zQcz}!Ai|U$<y3(bA+QTLnHW-qQiKt5m5e<lm5iDqFG2eJG#PL4=A<STm&6wrmL?XZ
z7HhI)>M=4fWS$2Rav;J0L@WXk9*hhOnk+BH7#J8{W`l@t3=9mJ>JZTs1_p*Ab`&BR
zWCaXEBZlF#5d$c?QyHQdQy8L{QW#qpqL@=SQkYs8qgYayTNt8PQ&?IUqS#VcTNt9)
zLD3q;QOT~!d5b4FwJ0+&$Eh?eEwv~aW**3VRt5$JW(Ed^&vsz5OP~?Rz>tNk3Z#;O
zfuV*mg)xO`4fAS{4iKLq9<HjAL6gNVli?5}!%L7EnvA#DOEPm)^Gi!KnQyTbgYCb?
z264wN=Hk>MO{QBc#i==Iw^$1Di%N<?1}P}~3P`bvanCHtC{2nf%1_Eq%FK%?$}cvG
zDK1Jz5`ai#q~;W)78S=7r6%Tp?26CHPfje!%+HHY&dH3=&&#RQE2zB15g(tKmst`Y
zUnL5$9TEz9Fkv1B28Lo##56G65tf{8JJEK5$bzyBg;#_vJ6L)-K7;&@5@aCHgYaii
zY)@xMXQ*XD1X~RgEWOk)W-~D`6t&b~4azK79tIf?#YJ8?)Pm9mR1J)RrRpqBm<WP~
z%W)&bU@VwiEprWXk$MeN7B5T&LDw+D{J_YNCtAV}lgSc*u@H0$BSI~nP+;t7sbLCc
z&}8yUuNGP7{;;aWUX$|{b53er5y<#kT$y?C<%yXknR)5AIEqpeQ*tu%Qg5-Pq$cO5
zq~2mJE-A_^xW$%OP>`CJa*G8N*|%7Wb5c_aG+By3uDZpWT2YXiT#}mNlbM%Ve2W#N
zzxWmlm;wd0f`WoVkq`p|!!16L@oq)=xh|<?naQbD(kQ6{RTAX3;t~c1h6aWk0zw_E
zPxu5UsCH+~u)V~maf4sDzoN6^I=|8-ex)1Y($gy@R$Ld?xG1i1SzPP7xZy={!^`5v
z*TpR_id$Y5x1PXqQ&{|lh~#vii9QQNu8OEV5fGWqHIZwE$rS;GFT4!=LJ*TL^J#qG
zV-OJS;3*PeU|>iFWhPi~bHFm|0&r%nVaP(3g{nf1ks5|9P)dc$z^D{>8ckuAWME*Z
zWh?;|17PVAcv>p~*#;J?Va&pkl2TY;F%PQ;VEkGpa9p!iSqG&irW7lbWTYzOrY07b
z7Nw>rq+}MCB<3Zj7At5t>bUB-=s4-P={RdDq!s1of>ow~qebr)b5cQW5y;*mP|@&<
zQ9+aK7H@e`W=U#DJ|yaj6hZOHl9O3na*H`PvEUYKT26jq$t~96f}G3}O=d{SU`qm*
zdbilh5_3vZi;F=40#6LG>8T}7sfo$?c>(#wnV_;GGcVl*=EN#3l(d0Ytu&|@aRDcl
z2OK=tIixOfNX;<4!l8IWQ1rT>%0)qy1%a0Z)mJcH6x8ZqyCWz%C3%kO0<q;X3uRU$
zUsg1_EM|OJ(4>Ry2ES+rFJ{z(8UrAgf9?ZU`IJXJa-`QFWnxfqR>PD9PX^!=047S{
zMNJ7PS|DPe(i6<8VT2^66qXcLDTXXi&H;-fh!i$h0zpqOFmXmu4Z{LTk}2%j<_ru)
z_GG#QVg_X{VG3rzZhHzVl6ygg8b_5OBB3MFH#|j`B&HWDl;nd`cVb?0Mt)JT-Yw>$
zy!4l#lnqYa;Dqj1qykFNjE0&VMXI3Y062ZK78GUXl@x)}SCJY>6x7Bk0ySfbG#MBe
zpvk-l)T+A03AY_JnG2_<mINi{rK2Qn1@y#?u0jcvq&H(r(gGsa1(YueC@*llETGoG
zdWVCzBejR;hKN`P=K~JG>l_jnIV9$Y%t>CNx<YhC;Yx`OiW`(x$XwU7zNl%v!}+qN
z!(}<g%aTr4IGi7F3rrB~w3txbWq*T5^g55sMIM<sAxo5(q^=0vV7M}3L*j;{6*1R!
z?Jw%uA7H+$>vCDi^|Gwn6(08wYzzXD7dRw7Ff;IqK{U<?xx%A#gJ0qThXf=Cfx;J-
z0710?I0wbR3Qw2}5{<361hsmq7#J9`;DsbY1FSklq=-sTCHISwOOqK=v$2AUtXpiU
zd7!FHlL=BNu|f(Xko%zZ5;(@Iq)_4ntQA(;8G>R16w3_^H#m6uIXXG|`8xS#ur6Sk
z$#Y#?=c2gIWpTZ$9QqKSBUcO{UxV;xKd{dc)ihdF0dft(YhV^WKQl6b{aVH2oReRi
znU}5rYR73Z7l9lK^?PzoesQWMBiPTyASXfVVD|X<l+>K~_$m<;|3XDgKt7cKr#n!}
z%DciLi{#_0(8%@EWCZ6LP0m{!@$q?yxvBB-x47ctbMsS5b3kmK`1r!o#2koBkuWIh
zfHG8(4v1w8BEXr<nt_2~B?CCrzzu{VP%#H4K&4!<0s{j>3&STSMpmoOY+S6OpCnjW
z#Xj-zvPyoEV`Me>%*Mnj3DFGFmkbUyC;<!4&mbG8Gr-&J>?w>bj8V)fOf3x9+U=EW
znjE)y9Ki)eNMgFPk7qI{_MnC`fKnld|M?rFCSa;zh=<8Blz?QwJVcpZgVah-VN7RC
zVX9(fV5nh;hpSBiwd+Bcc@9erLp&&_fXzS?2iXi3&jjga12Y*I7*arb;PpifLl#T|
zi7r}NA_(J_fSO@o4lLKWFvN;5FvKu1Fw`>FvedBDFl50^2P*^<HOw{)3=BPDEDR;0
zU;%IsBn#e5fr&9NWQil#FcvJQmVgo#SP)hVA=%CfwH=F@HLO@|2i419(+~t++ofRY
z@Ys&UOoZ)~pj^l5_Z()<OHfFyWVyxUXn2ds(dZVFqwy^!M-w+qzFX`isl}icH8?Tf
z;)V3V;-SqUAy|tn9$shM;w?!`N8<98B&Hi7@!6r3*DVn!BR)AlzbK_RK07bJJnt4~
zYF=q>YEfcI>MijQ*Wi$NUsuQA&>&Zrco)y$5Jx{}*Wg>sRhb31Sh7<qi*E^o)r2^@
z8zRZW1&t7bnrz_mqX?9MioC!zWacdvkgIR8K^+1L15gVeR+fS?dzC117J_&8tU!g!
zZ*bvqLtL(drHAK^tmX=p%d+|%ELS+BK5#Ios4q}m!MIQz#FziT#-OUPqIie$f#8d>
zo**Hq4{Qu-nj4sR7#~Q!DCY$dl71jA2{P%1iYADw_<)<g-=@pviL}ZURhw(lwiDSW
zFitSJp=&sS<+_0SMFI5{jF$woJ}@)L*nMGRP*z)za$U{hqMF5ZHQS47wwKlHuQ-NW
zQwp8PH$iNM2vnQWMFFJ+W)N-4VT^EnmKW75udCTzRI|IR=5WO+^qNu_RG*#^$U1N}
z?RH5(`y(@haya7$K?WXZ<p?T|VW|j|<Uqy2XFVp+5CbOzqLRQm5Ws+~{|d{_wTvl@
zwM;dPppFy70&LkCThFzIiJPH@3DLYn6k=<bV1o{zs<@UJGM-h-lEPBU3K`3)VNGXD
zVFMLyX-u_jDeNr_wd^JE-fazg4NDDk8e<Jx3db7G)zG4>mZOFvix1*PWU`hsg{y`$
zg&WiqO=GIzOyL5tYB*DPKr~o{7sRUJOyL93X-p~nDFWca6JB)GFl2#xrpU&E8WEtR
zi6#asnp_zAx%#=2nKK!ZnPOF-1tM21cMTU(fyWJTOA37S4Mh&@K7?Ev(`2R|H5P_s
z=34Gru3&~_CPpZ`oI#nP1T^Xac8dx_kwh&|3CJs8eu`iX&uoUd@O~mALnK2ALoH7o
zPbHKtXDnx^V2)%cXVet(D{=-kL>X_f6lYeYYO=np09CG_8sX(M&{)?A5CIyy$}}it
zV9;cRw0J?aBO2=^C|Xd)T0n+?LhSPeNM*+m!^*%=%LJ+>YFTU9Kut4{qifk~7*m+Q
zEDl&lq?WUWF$LTasO73*OkoAfa@R1Xuz^`T1X4It3S$))149aP6$b-D4RZ|}f(J^0
z@U+fLz#NtuCWOu!)*5C655*ilLgqkS!U~~L^zzrR)d~=+AE!?Q37EhHvAKpBLZi4q
zsD`(O9ptkbz8Wqt&0oU<rUhy^!L(ov2bdPB;Rey^Y{3kg?0%300BUFz>4Q=Us0?}u
zYJU_ZfRYr`EvCGJTP($eMJ1ZTw>ZlZi!u|Fa#D+LalmS?Tg(;lPPdpVK}1EoGl&Hy
z9B*+Y#={kHr6RFYkl0B`>@*~HGJ?&P7@wS&lYEOU2}&nJ=@clP3Z>J)bTKHoLh2$E
z65O`D#g|%ClwTB|R+^U#8f>VtL8+rsQcF^cax?Q%<017Lw0_mI0S#>C6x->c>P`ez
zWh#u|(J3wCivsEsm?s3!P@Yk|!1;l!>Vm|X(G$3Ckiz)F#vmm#Uwo$c0%cGIxWaLv
z%m(2N#TzAe1a9QJsAzXt(tZNl9Z6}Z66S@PD-svVZ7|*;ywP$;;YPiSiVl}09T7@|
z7bGqeTcNz7aHZNp`OA`q6WDG@%AiXdZxCK-xuI~S-bF==%aWGp(#X0$urY{A&xpDx
zqJBd}iYWR6FS~@$2L^UQA#fZ07JG4iT1kA4p(gV!mc*j;;#+J<`K5U&ppnA-;$lc@
z1hwJ8JqvJ>EJ^|=M2Hl)%yMO5V5pLE1x2JndTJi1#RKa7K%%VJN<ovi2sEO9i#4&R
zD6z802h?I=D@x2u$<MvTUYeJgmS2=x1S)!qK&kx}V?~iaNC|6ZNosEKE!HBCgKlxU
zRwSnufYJ!KKc&eFX#;@`<Ic;^EKZHjNi9pwxg`Nh<cNk^JV?Is7B6VnJ2NpSJ{>gd
zb&E4MvBC*tc5zV+sC=#jS;|wCT3nh_0@WdwUr>^nn^~2b0!?{PamMUhjAgeNQ;R{>
zDV9_%jw9h#sSuOA!$8S65S)xZFqw0j+)-3sF0oKzh01j;$BSBymld5Z@W|belI`%m
zp>DayX}{-A&+9h67j1k`RG-MX5R-l-ukZr{LouT((`N>Td`35>3pT!=85nHcnLaWw
zXt*<7;8FR+#vp6j;d?_uw!`z5nCc471=Sk@7uH`8HSciy#3IQn^u>ZfM)QV%+zkQQ
z8)AAFL=A3;%ia)``@+p8!`I>SfsaAWXnEYixC=Z=AJ`ZqBrixAY!JLCWqd)*<c7HJ
zio}cJ1{aJxP6%E!^1dMEgH1?G<|8Mo3}1&!kIx4l23JN;#^XK*eXhHNU33Y%>=JQZ
zIC7`U4#ypZ6CCHT&PZIqI5TB|<7~b)tSb^XFs@A5;JBLag1Grb5%bH!kr#L(e_j@j
z{Beh0;u8ZSr^y{2!3!dKD+*U6Ul207&SQL$$N2XLW=2kvU&UGs3=B<9cFx8OhZ!`T
zb=eMUGK1K<Y#`E@(?x{&unnIJKl2fORu>UwjG7meK0mKxVqj<&Y!{l&fTL?Cfz;{5
z+b&Fj%!n{nGNm(g38pi23Dq!S8)!tdhhjZJwLP|h#~Q{OaD5M}hf|nWgEA**G@H3o
zh|sJ?ryy1pSY3~54<kd5Hw#iXyAwWli11y9V24l|QwmE9M=eXIpbJAkYcf+hs7=St
zz);It!wMNFoygR~$-+>}R?CW24Y<7mRl{D(ihmZS20pY(Kpi_QywaI!IZ8lP6}Y{J
z(4PWs=hSi{!W=~o6uw+Y`rzh4^>HKVLzV;S<Ei1S;ZA3&;i}<DXG&wL<wa__vea<Y
z@TRb@;aCl-{6VVuYWT81g(TR02m(Avz+Wp+!(Sti#sqEAGS#w{L)x}rmL{hkxYS<B
z<OgYBpiH8G`idZrf8N5tz%Z2wl&ryqGS)JsGt@F88bO^Lo$v{87lv2|NP`@#mIc)K
zL?nA`ouL}W8kQ<h1CkNkfMlv+MetCXPV6<z*fK#4JEWloi!erp9w!!t8fNe$8n|ZC
zWGM;;7384GrYIG}0*w)Ca@}GnODrlbY6O+<jFq=Ii&Bd-Q%Vzaif=I`Ror4qsw~O}
zNpXP+`P_oilGK!2f{A5`nK_`QReTb7Hl+BLFhU5_)do-56oa~nu=2m50o<f10@Wi`
zb|?issJ&F0lUR}(4;oMa4-#bNC6=TX<7&yw162Vz3=9lE8W?T}i+8$oICd1?kdW%|
zyrZFgLC5xjeb6<H;EMt(6FDXbO$eHybw^fye)!Dr1&J$^7N)HzTo}E<aR<{zw;h2S
ztuLzDU6!?<z;#1F^16V+MFE8cQY*49Dq36>u>8Qvpc%sWfsH{#_5zO#xJ3Y|=t13#
zB2cVoGC?ZGT2Q6WQxpISRZuDfSJ6cwpm1Z(O{{<nzk`AsQq{BMrY7dyV$RLX1J~)`
zu-6oZRN1~@P1&iHw-{5vfpv>9=@us>nByyo;mpe7qIQr{){6Mt%)DD5DzV}gYbAtN
z84t1+RB_*8gVfm|6(x!3AQdHv=|wX@PMrw~M^NI1MG~BLZ~({FElHelQ>8{+98Lh)
z5zffK@Uwy82EXuievM1~8f%<4ux#hq$a7i8_5kB09mgBOvJYhBE^tWRkW;w8A$>zw
z;kvNuMPb!z!s;E)cLao|xX#dB!F*9l=dy_Iio(kRh8>PKc;v71s9oeyTTpm~NAm`c
z>~$WMi##d|LYBuYjM<=kQPu1!kNF371`&-fTns$?7x)z}b1N=zyu__~Ls<4J3md2G
z2LT2Z(`zE8GZH5RPAHrlazVtjgY$xj=?!HQETXV>tmI`$?KMH`!&ioHh`g+8e?h{b
zgZGJm@C47vd<%px3aE6j-XWTILs(*l>IA(7rYlqz=v@#r>|lE$C^w^SMeTvG3qjFW
z1Y<hbZivf*W{Nv`I#@eOZ}5vuNbRV(z#;wflO%(v+0R=drp2HswL^+LuDmRVjCovz
zSTIU2aJ8HV?GR%fZDlWkR)(O--8`uptQ8x!QU_M`)-oa$<nY-+7lv3}NGS;Ff};%7
zFl2$8jVPzVELhnES_o0h*dt%VT!U3z4HI~5Ri6da#K1Ch2zM1r4NH-A3S$jJJbai1
zQ5hkowHX;|ShC<_#9*g@379!946&k6f3ViFAxx-Yoz0NKT*Ee-VJ^}Vh#qkk2JomW
zi(g2U93+z{AQfr~@YZjNRTYz!f+jb3l(HxYlq0=C1Ze!fC=kQ~mnEP^d{G~W3oc*4
z^CeB70*4)3V%!pd77p-ow<sJ`FmS;5w<HiYqV^PvB0&n-z>X^}29;{i0sxZvIg%4|
za^jN;a?!>{VZxvqv3LVyY!ottc}L#tB9F`idFAW!h8N`xugm*il=r_ZA9#UB=83e*
z1y$2)(q^6B9gZCpH~0mn7*8m<F0OY`T<@~5{$+ms8@i^T5lF;<Co6-DIcR`Y<GQl_
zMP>WT%8nN}6mD`W-_X$MaJj;x3Tnv;8C~HqzM*7wUCH61lEXD6r-^(M*k-WZ5fGgr
zHiKgU(*m;v8W+U0uL|gZ1~i=+Kd>|KsDO)dlzajjQUK?ZMJ%8JP3%qAV#Z2Pii77B
za3TQ{MZ6`@h6n>g38=*Y7ENcWWd!FYc<KWSg9&U)Bw*>SmJv2k)#!~hB*~b<2pWol
z<q{W$SSx6%WU6IGG~`GhNQ9XPX}Z?3fb$)e!N?jGaK5u)0nNLEo6~6S1B<~-!{a{C
zN)>P@Ac!nbHH6@TS+MYcnTX;(EM_3N59^!`_K<;@4sNM|1_>Fmu$Y|%3VN`s5CqJ9
zE)4zb&?Y+<14Au)4SNb>whO3_*~3}GK9Q-1hlQb*qn5n{)B*r&ui=2%n!*e!?TX||
z;7cG=Sg@&^&5**1T*{=df!)hl%atcrgUh#A`~kBUrW?s#cmte~p@y|lyoRk&6iJq&
zhBbwAHcJXu4I7x|o{O}uA&(nLB@fIFMur;BM)4Xh6mxiMIKle4YPi5OZgb$vL&5H;
ztzm1duHkH~tl?@bui=EWk3oaokalk^PYGzU8yvb>ph;LT3qjOyr|^SY);tgz)Ov%t
zvz8Z0FK875LK~QcBnw(`0Tx3LH9R%ku+AjhM7|on6iD}#wT3T6po*7)p_UIRj09`A
z!M+!&;Q`aaHN0S2q=pYni_YbQnFaEF4I5JDg}t0XQw-8mfiy=!Lrzs1o_WQkX=$0s
znW=dt3T25onaGQ$((;QGLK4#zjI0!@xD*PC^2;()Qd9JbK&`VXK9CG-K~u3+6$glK
zXr-W0#jIzPR>h>F@QYEiN(?IMmtUfgnwMXio}rMESds`)Z3I#cR$*kNfTjZ6`^d~-
zWMHULaLFu61rKE^fS0?3B&HjJmb!!_rW;u)q*T~evFa$KRN7YY>ws2%733r)r{<>S
zmDpBs=qM!SrRSvDegzf3hc;*mLt5LQ3BICGP~`|37b*hRpWw-QaQz8h00U}KgWKXo
zpal&@D?y62K?JyV1y`r=KG7}ql+^U1)YRfz?1@FmC5d@Pn*5MjR<H=NAO<mR2Cim{
z)__cc)T1Sd>4xzYx7fgRWl;pEauor`1gK+!JT(eZVgyxc6kmCZDW&2TQ%dD6o|MdD
zxTkM%f_)sHlA2x&YUM!dW*7yjqm@ewQb0W?SY=-vpO^<3X@>WximMDDtq{oS06ijf
z90fJn=P`kYieYW`8$3eLiOM^QCRfZnFDQ6j<dJ=#pmJTo=%Rwrbp^MJ3T~GbJTCCa
zeqdwZk)08Ekz4)(xAq-*jSHGqI}9&s+Fp~lo5($Zae~DiN!b}G*X0Z@${AdjGrcHh
zdRfl=vZTch!HbgC6WE{)0mKw0D}#dl2QK2wG`uKhcwNryqMX@fInV&{4#kU-HWS!x
z=$U~U7x3xL4^T6u<gQDaUz9Z8!E)Wg>!O9%Wl8T&z7DntY&Q%|uN%5tG<3Oc=zY=9
z`?8_$HTkF&A{PQfFGNIM4vbvHazQ@o0*A~6`KT{!;0}iJWp0%Pg_pQBK<yF{n?1o7
z978YLhF!1;zYv~yK`7}8Px1{}^-jMIw~1~KWEHQ=T3?j4-r;iH#{Z&?|7F<#kjw<P
zCmg&N_~fo|$lu@=yudBHK<$DOXvl7Y$BeKA&NF;gFs(4#pu9q7L*fM?i>o}APk00`
z2x(p6(SAUha!?cr!OY(wcEQs7LO|GsfTSCO;xp7H%6B-y8F!>*=bO(oUtqMLaG~i6
zqZNfKO&8i;mNxA0xg#MppKB)95~Jl-3$0cpZBV+bY<gM3ti$sLzr+l~uG$*{k~4xY
z3MfqA1c!*phM*nH8$5Qn9#A?Ed_dxa{DsKW3u&1bBC;-I6<sK<`M|(X$LPoe9-MUo
zQBF)QAWGPk=?en`uN%`1e$feK9d#EtWPg6LW>AP`1owT8NOO7eupCii@#JCg(`5Q3
zrk|2pre9K0saIN@l$)QDnrmocXkh53$x#Gqz7*MlGAwBMdJ(9}0clu+%Y^kHiTxnL
z9z=+M2+-P-B1aGlL|B5d1*oEKVff4>#cKS?!h+T0QwgIItIQ`4Mt)Y^&q^k&a^Th_
zSQBWaQEp;pUVMDf0R{#JH%%tQs)Q;_*SwO{B8BAq+}y;x6a@|Sq=H;^P+?e{TA~1*
z9new8FH%r1EX^zd%cNFhmS|cj6oEE6RJjG0XO<*qfaVfP@)eTv^GY)FO7lyL6`%!`
z0wkXoD`*sF=7I_XkV^HUymWPi+|=UY#Prl+O+7t5a20Zk1!PB+fNKSKO*N>3NH0pv
z)zcI$k^z-sta+um1(o2ziz3jV{w==Z<jjIfJ=g>QWEcb7gF%$Vpao9gU;u~vE#}06
zf?KSac?G2<MWAIZw^(!X%TtSPag`RQ7R5uv!BGG%k-_l*3hE-%$oR!!16i?US9Fqr
zfdRBAq4+!l1H%VqMn=Zl4BWRFgl;qN-eu7E#Ks_Pd4WOvmYC)R2H{&`S{E3Ezq4pF
zN`7R}W|aIM!|1@wDEE;8M1F8#V_@OwsJg@~d6P@*f{e+I)GJ&r4bC^X_-=|yU62mD
zEE;}YH1?us>}ApT>!PU_MN=<}rgw1N;1-=BdWl=^1`mI~Pp40h-%U_vjJm=TeM3m>
zhJf&O0rQIj=2rwPZwN?DU;(e*Hk`uyft5j6`~w>UpTrEaOT3C;Jt|jtRB!MIeh_8g
N=KsLTz#;`sLjY+FmDK<M
literal 0
HcmV?d00001
diff --git a/ros2/src/robobin/robobin/helpers/graph_maker.py b/ros2/src/robobin/robobin/helpers/graph_maker.py
new file mode 100644
index 00000000..c197c10b
--- /dev/null
+++ b/ros2/src/robobin/robobin/helpers/graph_maker.py
@@ -0,0 +1,112 @@
+import json
+import time
+import os
+from realtime_location_cli_only import AnchorTagCLI
+
+class GraphMaker:
+ def __init__(self, port="COM11", testing=False):
+ self.app = AnchorTagCLI(port=port, testing=testing)
+ self.graph = {
+ "name": "Lab 1 Extended",
+ "nodes": [],
+ "connections": []
+ }
+ self.previous_node_index = None
+
+ def call_bpm_and_store_anchors(self):
+ self.app.call_bpm()
+ for name, (x, y) in self.app.anchors.items():
+ self.graph["nodes"].append({
+ "name": name,
+ "x": x,
+ "y": y
+ })
+
+ # Initialize connections matrix with False
+ num_nodes = len(self.graph["nodes"])
+ self.graph["connections"] = [[False] * num_nodes for _ in range(num_nodes)]
+
+ print("Anchor coordinates stored successfully.")
+
+ def start_session(self):
+ print("Waiting for 'start' command...")
+ while True:
+ user_input = input("Enter 'start' to begin adding nodes or 'quit' to exit: ").strip().lower()
+ if user_input == "start":
+ print("Session started. Type 'save' to add nodes, 'finish' to save the graph, or 'quit' to exit.")
+ break
+ elif user_input == "quit":
+ print("Exiting...")
+ exit()
+
+ def add_node_and_connect(self):
+ print(self.app.tag_distances)
+ distances = self.app.serial_buffer.getRangingDistances()
+
+ tag1_x, tag1_y = self.app.update_distances_and_calculate_tags()
+ print(f"{tag1_x}, {tag1_y}")
+ if tag1_x is None or tag1_y is None:
+ print("Failed to determine tag position. Skipping node creation.")
+ return
+
+ new_node_index = len(self.graph["nodes"])
+
+ # Add new node
+ self.graph["nodes"].append({
+ "name": f"Node{new_node_index+1}",
+ "x": tag1_x,
+ "y": tag1_y
+ })
+
+ # Dynamically resize the connections matrix
+ for row in self.graph["connections"]:
+ row.append(False) # Add a column for the new node
+ self.graph["connections"].append([False] * (new_node_index + 1)) # Add a new row
+
+ # Update connections
+ if self.previous_node_index is not None:
+ self.graph["connections"][self.previous_node_index][new_node_index] = True
+ self.graph["connections"][new_node_index][self.previous_node_index] = True
+
+ self.previous_node_index = new_node_index
+
+ print(f"Node {new_node_index+1} added at ({tag1_x:.2f}, {tag1_y:.2f}).")
+
+ def save_graph(self):
+ directory = r"D:\Github\robobin\ros2\src\robobin\robobin\graphs" #Replace with whereever your graphs folder is in the ros2 workspace
+ os.makedirs(directory, exist_ok=True) # Create the directory if it doesn't exist
+ file_path = os.path.join(directory, "graph.json")
+ with open(file_path, "w") as f:
+ json.dump(self.graph, f, indent=2)
+ print(f"Graph saved to '{file_path}'.")
+
+
+ def run(self):
+ while True:
+ user_input = input("Enter command ('bpm', 'start', 'save', 'finish', 'quit'): ").strip().lower()
+ if user_input == "bpm":
+ self.call_bpm_and_store_anchors()
+ elif user_input == "start":
+ self.start_session()
+ while True:
+ user_input = input("Enter 'save' to add a node, 'finish' to save and exit, or 'quit' to exit: ").strip().lower()
+ if user_input == "save":
+ self.add_node_and_connect()
+ elif user_input == "finish":
+ self.save_graph()
+ print("Session finished.")
+ exit()
+ elif user_input == "quit":
+ print("Exiting without saving.")
+ exit()
+ else:
+ print("Invalid command. Try 'save', 'finish', or 'quit'.")
+ elif user_input == "quit":
+ print("Exiting...")
+ break
+ else:
+ print("Invalid command. Try 'bpm', 'start', 'save', 'finish', or 'quit'.")
+
+if __name__ == "__main__":
+ graph_maker = GraphMaker(port="/dev/tty.usbmodem14101", testing=False)
+ graph_maker.run()
diff --git a/ros2/src/robobin/robobin/helpers/realtime_location_cli_only.py b/ros2/src/robobin/robobin/helpers/realtime_location_cli_only.py
new file mode 100644
index 00000000..a314cec3
--- /dev/null
+++ b/ros2/src/robobin/robobin/helpers/realtime_location_cli_only.py
@@ -0,0 +1,263 @@
+import time
+import numpy as np
+import serial
+from scipy.optimize import least_squares
+
+# Define test values at the top of the file for easy modification
+TEST_MEASURED_DISTANCES = [302, 463, 286, 304, 418, 328]
+TEST_TAG1_DISTANCES = [22, 107, 246, 295]
+TEST_TAG2_DISTANCES = [100, 100, 100, 100]
+
+class SerialBuffer:
+ def __init__(self, port):
+ self.ser = serial.Serial(port, 115200, timeout=1)
+
+ def readFromDevice(self, expectedLines=1):
+ lines = []
+ # Attempt to read expected lines; if device is slow, you may need retries
+ while len(lines) < expectedLines:
+ if self.ser.in_waiting:
+ line = self.ser.readline().decode().strip()
+ if line:
+ lines.append(line)
+ else:
+ time.sleep(0.01)
+ return lines
+
+ def getBeaconPositioningDistances(self):
+ """Reads the measured distances (A,E,D,B,F,C) from the device."""
+ self.writeToDevice("bpm")
+ buffer = self.readFromDevice(1)[0]
+ values = list(map(float, buffer.split(" ")))
+ return values
+
+ def getRangingDistances(self):
+ """Reads the distances from the tags to the anchors."""
+ self.writeToDevice("rng")
+ lines = self.readFromDevice(2)
+ print(lines)
+ distances = []
+ # First line: Tag 1 distances
+ distances.append(list(map(float, lines[0][1:].split(" "))))
+ # Second line: Tag 2 distances or "0"
+ if lines[1] != "1":
+ distances.append(list(map(float, lines[1][1:].split(" "))))
+ else:
+ distances.append(None)
+ return distances
+
+ def writeToDevice(self, value):
+ self.ser.write((value + "\n").encode())
+
+ def __del__(self):
+ print("Closing port")
+ self.ser.close()
+
+
+class AnchorTagCLI:
+ def __init__(self, port="/dev/tty.usbmodem14101", testing=False):
+ self.testing = testing
+ self.serial_buffer = SerialBuffer(port)
+
+ # Distances between anchors (A,E,D,B,F,C)
+ # Corresponding to the measured_distances in original code.
+ self.measured_distances = [0.0]*6
+
+ # Distances from Tag 1 to anchors: A1, A2, A3, A4
+ self.tag_distances = {"A1": 0.0, "A2": 0.0, "A3": 0.0, "A4": 0.0}
+
+ # Distances from Tag 2 to anchors: A1, A2, A3, A4
+ self.tag2_distances = {"A1": 0.0, "A2": 0.0, "A3": 0.0, "A4": 0.0}
+
+ self.anchors = {}
+ self.anchors_coords_known = False
+
+ if self.testing:
+ # Set predefined test distances
+ for i, dist in enumerate(TEST_MEASURED_DISTANCES):
+ self.measured_distances[i] = dist
+
+ for (anchor, dist) in zip(self.tag_distances.keys(), TEST_TAG1_DISTANCES):
+ self.tag_distances[anchor] = dist
+
+ for (anchor, dist) in zip(self.tag2_distances.keys(), TEST_TAG2_DISTANCES):
+ self.tag2_distances[anchor] = dist
+
+ def determine_anchor_coords(self):
+ try:
+ measured_distances = np.array(self.measured_distances)
+ noise_level = 0.0
+ measured_distances_noisy = measured_distances + np.random.uniform(-noise_level, noise_level, size=len(measured_distances))
+
+ # Variables: x_B, y_B, x_C, y_C, y_A
+ initial_guess = [120, 100, 150, 200, 50] # [x_B, y_B, x_C, y_C, y_A]
+ maxBounds = 30000
+ bounds = ([0, 0, 0, 0, 0], [maxBounds] * 5)
+
+ def error_function(variables, measured):
+ x_B, y_B, x_C, y_C, y_A = variables
+ # measured: [A, E, D, B, F, C]
+ a_measured = measured[0]
+ e_measured = measured[1]
+ d_measured = measured[2]
+ b_measured = measured[3]
+ f_measured = measured[4]
+ c_measured = measured[5]
+
+ # A=(0,y_A), B=(x_B,y_B), C=(x_C,y_C), D=(0,0)
+ a_calc = np.sqrt((x_B - 0)**2 + (y_B - y_A)**2) # A-B
+ b_calc = np.sqrt((x_C - x_B)**2 + (y_C - y_B)**2) # B-C
+ c_calc = np.sqrt(x_C**2 + y_C**2) # C-D
+ d_calc = y_A # A-D
+ e_calc = np.sqrt(x_C**2 + (y_C - y_A)**2) # A-C
+ f_calc = np.sqrt(x_B**2 + y_B**2) # B-D
+
+ return [
+ a_calc - a_measured,
+ b_calc - b_measured,
+ c_calc - c_measured,
+ d_calc - d_measured,
+ e_calc - e_measured,
+ f_calc - f_measured
+ ]
+
+ # Run least squares optimization
+ result_noisy = least_squares(
+ error_function,
+ initial_guess,
+ args=(measured_distances_noisy,),
+ bounds=bounds,
+ loss='soft_l1'
+ )
+ optimized_coords_noisy = result_noisy.x
+
+ self.anchors = {
+ "A1": (0, optimized_coords_noisy[4]),
+ "A2": (optimized_coords_noisy[0], optimized_coords_noisy[1]),
+ "A3": (optimized_coords_noisy[2], optimized_coords_noisy[3]),
+ "A4": (0, 0),
+ }
+ return {k: (round(v[0], 2), round(v[1], 2)) for k, v in self.anchors.items()}
+ except Exception as e:
+ print(f"Error generating anchors: {e}")
+
+ def calculate_tag_coordinates(self, tag_distances):
+ if not self.anchors_coords_known or len(self.anchors) < 3:
+ return None, None
+
+ available_beacons = []
+ available_distances = []
+ for key in tag_distances.keys():
+ d = max(float(tag_distances[key]), 0)
+ available_distances.append(d)
+ available_beacons.append(self.anchors[key])
+
+ if len(available_beacons) < 3:
+ return None, None
+
+ def error_function(vars):
+ x, y = vars
+ residuals = []
+ for (bx, by), d_measured in zip(available_beacons, available_distances):
+ d_computed = np.sqrt((x - bx)**2 + (y - by)**2)
+ residuals.append(d_computed - d_measured)
+ return residuals
+
+ beacon_xs = [b[0] for b in available_beacons]
+ beacon_ys = [b[1] for b in available_beacons]
+ initial_guess = [np.mean(beacon_xs), np.mean(beacon_ys)]
+
+ x_min = min(beacon_xs) - 100
+ x_max = max(beacon_xs) + 100
+ y_min = min(beacon_ys) - 100
+ y_max = max(beacon_ys) + 100
+ bounds = ([x_min, y_min], [x_max, y_max])
+
+ result = least_squares(error_function, initial_guess, bounds=bounds, loss='soft_l1')
+ x_tag, y_tag = result.x
+ return x_tag, y_tag
+
+ def call_bpm(self):
+ if self.testing:
+ beacon_distances = TEST_MEASURED_DISTANCES
+ else:
+ beacon_distances = self.serial_buffer.getBeaconPositioningDistances()
+
+ for i, distance in enumerate(beacon_distances):
+ if i < len(self.measured_distances):
+ self.measured_distances[i] = distance
+
+ determined_anchor_coords = self.determine_anchor_coords()
+ if determined_anchor_coords:
+ self.anchors_coords_known = True
+ print("Anchor coordinates determined:")
+ for anchor, coords in determined_anchor_coords.items():
+ print(f"{anchor}: {coords}")
+
+ def update_distances_and_calculate_tags(self):
+ if not self.anchors_coords_known:
+ return
+
+ if self.testing:
+ ranging_distances = [TEST_TAG1_DISTANCES, TEST_TAG2_DISTANCES]
+ else:
+ ranging_distances = self.serial_buffer.getRangingDistances()
+
+ # Update tag 1 distances
+ if ranging_distances[0] is not None:
+ for i, distance in enumerate(ranging_distances[0]):
+ anchor = list(self.tag_distances.keys())[i]
+ self.tag_distances[anchor] = distance
+
+ # Update tag 2 distances
+ if ranging_distances[1] is not None:
+ for i, distance in enumerate(ranging_distances[1]):
+ anchor = list(self.tag2_distances.keys())[i]
+ self.tag2_distances[anchor] = distance
+
+ # Now calculate both tags
+ tag1_x, tag1_y = self.calculate_tag_coordinates(self.tag_distances)
+ valid_tag2_distances = [dist for dist in self.tag2_distances.values() if dist > 0]
+
+ # Check if there are enough valid distances for Tag 2
+ if len(valid_tag2_distances) < 3:
+ print(f"Insufficient valid distances for Tag 2: {len(valid_tag2_distances)} provided.")
+ tag2_x, tag2_y = None, None
+ else:
+ tag2_x, tag2_y = self.calculate_tag_coordinates(self.tag2_distances)
+
+ print("Tag Positions:")
+ if tag1_x is not None and tag1_y is not None:
+ print(f"Tag 1: ({tag1_x:.2f}, {tag1_y:.2f})")
+ else:
+ print("Tag 1: Not enough data")
+
+ if tag2_x is not None and tag2_y is not None:
+ print(f"Tag 2: ({tag2_x:.2f}, {tag2_y:.2f})")
+ else:
+ print("Tag 2: Not enough data")
+
+
+ if tag1_x is not None and tag1_y is not None and tag2_x is not None and tag2_y is not None:
+ dx = tag2_x - tag1_x
+ dy = tag2_y - tag1_y
+ displacement = np.sqrt(dx**2 + dy**2)
+ angle_deg = np.degrees(np.arctan2(dy, dx))
+ if angle_deg < 0:
+ angle_deg += 360
+ print(f"Direction from Tag1 to Tag2: dx={dx:.2f}, dy={dy:.2f}, displacement={displacement:.2f}, angle={angle_deg:.2f}°")
+ return tag1_x, tag1_y
+
+if __name__ == "__main__":
+ app = AnchorTagCLI(port="/dev/tty.usbmodem14101", testing=False)
+ while True:
+ user_input = input("Enter command ('bpm' to set anchors, or 'quit' to exit): ").strip().lower()
+ if user_input == "bpm":
+ app.call_bpm()
+ print("Switching to continuous ranging updates (simulating 'rng' messages)...")
+ while True:
+ app.update_distances_and_calculate_tags()
+ time.sleep(1)
+ elif user_input == "quit":
+ print("Exiting program.")
+ break
\ No newline at end of file
--
GitLab