From 0d858e5fc652ac11778a9dbaa80e2ff7b0001483 Mon Sep 17 00:00:00 2001 From: sidtalia Date: Tue, 15 Jul 2025 22:33:01 -0700 Subject: [PATCH 1/8] ROS example initial commit, works on hound docker, need to figure out other deps --- Content/ROS/ros_example.png | Bin 0 -> 35091 bytes examples/ROS/Configs/ros_example.yml | 39 +++ examples/ROS/README.md | 70 +++++ examples/ROS/example.py | 144 ++++++++++ examples/ROS/requirements.txt | 4 + examples/ROS/utils.py | 386 +++++++++++++++++++++++++++ 6 files changed, 643 insertions(+) create mode 100644 Content/ROS/ros_example.png create mode 100644 examples/ROS/Configs/ros_example.yml create mode 100644 examples/ROS/README.md create mode 100644 examples/ROS/example.py create mode 100644 examples/ROS/requirements.txt create mode 100644 examples/ROS/utils.py diff --git a/Content/ROS/ros_example.png b/Content/ROS/ros_example.png new file mode 100644 index 0000000000000000000000000000000000000000..8e7c4a47c9f79a060a9c3f6b80cf0fcb35542888 GIT binary patch literal 35091 zcmXt92Q*ww*cK&Gf~Y~1sL@%`mf%wpU6w>|yV1gGYl#*mdXF9@O4MMLu&mw}t3_w^ z&gwM?`ThSn|2=2UJu_$SlsoTz-+7*SZls>hYjRR1Qan67a`iVVAUwPWXZPun_~Ctt zwo(Px{f!W&q;5cbfB6xAh`7%)dZ-$C=)2f@cv-vK;MqC5Ky8Fz5O*6JXPCW<$00$R zJRaV2JarXCgHKs|Esj+-=yzMF3sgO8y@gM*0e|U-7D4O&R6!C5r1Rh_(dO^i4o}gQ zFnb2<_-}D40-E16FSP;=MEo)%(|ICzn3O4R9}o{(k(u^d{l#y+!DmCCw z;ke5Fcj9~WH0Q6lmiyC!|0~mYRn~TD`B}t^kv~P_zM4vn50R$+7`}c>vM3jHqYR)+~!O2gP##FemaMYqJHxbaEb6GRf5m+ z)V_GJZ_95iGx+0DG`J<+jS8f6mL!F06oT_fuZc^9`DDiOjSkfdLFz;Hge@jQJMB@E zN7B>Y2ncs~HK}tU=poz*ouoC2_RyEErPx#Sh6;;#a@^*~qYl_rymVQXV~U@)qls7i?k7 zg)RsbXmAvy$;b-wynneIJllx`x-1j)zr|#=uYNo)VzVQ;V$Bfolzmayw$=(N?RzV9 zrXkArxga#rP|Lfd%`;3cwMi>Qqen~PEQH8kkgj|~rZ)KpmwOaPeV<`ar2)TiRNZJ< z!;n6XAUwHgfpFt{f#6fa?J&jI>+|h=h&Y$5fgYLR7PPyH2v`~$?G|4nkfF^6q};mcGgKH zHmX9YJ?kr&S_OJNNhWse(6^9U1A24HMRR6yOcy9hCQ&z8@p~0sF|Lm(JHEa*s8>Cv z+nA%%Tp$BBSL8ekxG_^b=}?%MWWy9+%|&+4j2?ZFFbXO*8e|IoUGN=#KkGFfRPOWx`{s4zT>-~}1Yv8TPCe+i z#^e*(A629J`6xgk(|lo1>H3n?)WzqU^fiGL4dV|~9wcuk1ZzuqD=0uJHN_tFhCvku zANZg#n&cXznMI{^-y%##PS@#MCJrukGwyrIGa_oCIlfMxXpJ+B>6n<$AS2x{*B=>(AV53AjJuj zj9pKvPf~5F?BrO)6z;H#>dpHMS3~}k4IW-zozp-B$|>bI|2*83KV_^GBJ+Kj*9)P6 zkaDEjhv(lu9V&5f{7ker6VO`+a?A4dmc|4AOsEUA?fzOyzzZ_`hR(7R>Cao((C)fdc9(K&5(mYwnf! z!1zk}+DXHhK37Xjm7v*_ySkDI$XFmn{#?~|rHa_6;%^jXxomUo)-U}gECBr;P-&1# zBT1I6JfZ2^+oJPftc#8~P5JES=zD(W-E^i$Wf=3!62k!wl_RA9vh2CxpA3flbo(X7 z%9Y8AKhtvbraMXzE~R$esE{_D=lmEoiOp5(ryknxeRiBLZS0rx=sgFI6TuEmcBK|# z`Y-TPsExy_g^4DWL>Nq8BdCLsx(FYrpiQ9vE$+x@{F3&KJmUQe`V^bIxRK_F#(CUOl-*-^NzXeGD%Bi*a_uVv ze&!f!r}40BiL;M|B2S@M+v@hhlajZGDFYmEtyD#`QCzG5WxrH*;TuDfzo0f2#J%$r z<*FWqsJ7^k?pfh@Y*@mYH|qp)iY%$eYa}g8V@B)EUpky zKGvikdI4=MH8BS4X=ukhNAG@3=wt7%cXDn^vU7D2Vm%8M(nXIXV1e+$~M>fm;HW@IVA@C)gQbmrDZZs0(-HFaVaxK53lSkBaJDZWvRY3=5m? zO?_^K%HI{B6N-U99(*7{5j3fE_>`ju2-r%|K;VS5OWf63G_x#AliO=z(TQic#FHC( z4n?=Zi0heWR_fEQNg|>ZL1vmi4>dxQ<}R+PTJk0@b+tyJp^4Hj!af48B1<{3B4wz! z!e;ikM0qLZub0#}{3#O#fGy4rYHaK~kutdwJj z6w|J+Cg$+gkIAc;4uW*It7xLk+8R;?;%To8f)G}tqH!@q2aQ>em6y1`A{!-)b}(YR z8qG^A?Exa7DVMnRI;S z7OdQ{LKrP1n;j#Bz<4t#3OA?UG@HcTV--JmcH_Ij2h2q>Nw4Fj03U?P0o)WFRq82SXJSl3(`F<;qJV6CIBKEou~q5R zhsoD6VY-a=zG-+VHZ;MFsph|Xje1vYx&^lVJ4U&7C(oTr>^6ME7^c-LBNmgyQfQ>Sb3{&cB(95Y-z{A~^74Fym zh%i!dcvWsz*R7)?^b#CO|S}n{)UTPP`rzuo)>i%kK zljGv8qB_n@0Mhgwpx2Z2G;XGBM!cpKlZ~xFR0O%BAFL}~Z?Kyw3B4Uo76Jdx@)VrA z-mK9-d?aFmU;3*JBUfis{IQSvOIg`o)-?AOej>AXF|EuniB2JhA|1@!b&>L`7s}D+ z6gv|^2mi?8j(AHM|B^+Q&oa(1ADn*%IVH~1!RKe#Wsq@JvAoTIR4^yGM3IDiKr@;a zi0!J}4p%!Nrl+B>=}_ zh6K(&33zn(Dm$ zxj(GZp}o+eI85qi%1N{5!QcB$#k;|c1koqej+>J8jby30(riEr;k7xo8eNzxBc9M` z0%@_S$@uA(B=3g$IY-t!9Q0eA4hH1<{<1j2`^~q+mTQUi4KW7IR})NL*_ky-dOR`+ zD`It}liczguN}&NV6&?^9s&jpUWlr~eOG<#o6HcIIi;-{JY`n5=;|pTn=y<<;M|=y zPUo4aGoPWfoq1944dV^1_hxQXtYF>ejW_I@h{M*u=4%f$M#I0kMKo4gO{}5JSLe+4 zB(FCpS{_Ehhp5gMkwSdB6rfjgJYVe!?^h>o9?Y7Vc4>al+H=jIG=G$Lqdb3zq5co8 z9VM}CVjo{4JR7)pqD^%s{6%yiFOEF2F_y{&J0}C=)d8ml%PEA`4eu-x2Qex9$8A`QmJj3E zNu4^Q&R{6Z$}qI~YXF!gd2d(ce9=mA9S0z}<+oGrFo{YR=Y0S%i_diaS9Q>;y zTYu^|H(TM@g>?1@S8PqR8y%a)NB@CNXg7#pQ48Oh2EH`IaWP|3F7Vy`bKe}WM`DC( zqFv}GvnC7Rjmk|iMa=j>jb(t1+JSuQ<8^9SX6U{^(#W~+9kUO=HKn;MWwyAU3JYY2 zJe~w~HKP{9K84@Y{HE7)i1|CK)~p}48uus5@$b7qfp>l5?R=X>zVu?Om+1fY8ohJ4 zT%rgfyzC(6X!Zk-abVFJ-vKVpNK+Dyrr;B)zs&pamdt@OMdLM{dyA_Uo>P8297C`d z8(*}RNaFK4<~&qx`o&_A|HQfHxcr`#bIlRBg#j?~U9<9$BXRngmNNgX43&}2&xqpC zrdB{O%jxV!3rphOWF-$+HAdE@+)?4-0HR>tS_Sj0b(eMIhX631ooAMSvmh$fkGW8Yv0Uw)mD|Q022k|my z14i59&SUKZGsC}sIeq9v zu|nVcAiXepA}PaQ%f2XzHqJ!XVWr#!Q8>y#wjk+TazD{3JQ=P3vK$n6jD|0+j-p)x zzf_uq^0P@*T6i8Jaw%QTJ~)yK`|a*`I4Jo3C3E%sr{A>1hk2BYo>amose9Yg99$UX z%@`use_eiR1h`}=HW|HX2$5O;6cd<+~KLP2-h+G|~hkS1Fod`E0{aItc+g3Qxwb;xLTUyT0+c^av@ zvO67K5fqxr+4E~VZ{24e0TO=KcgntEi*A^?$>E;gZ@uj?*_Pz}#fWuFZ+{e3DYu0z22tL3coMzkkr2$*Z?RF$}u^J6F$Uxh*xS^M}Y~hi3sbWIla8R(TYS2sn-DA{4tB>&|BMyPV)wXHI_FUBOjY<;*N! zle}o{$=rM=w9}LFXTtiAsTK;b1ebW>n zHdrEQV-ba-8@K+5HfHtoIE()JuOY0834UwlzQDBhwq1!YGo;V)m_kst@!B^uNzbdH zApPb97E9Jo*ix(WR?FMh`8bc8>1H6aHPqRcm4$P+YlHzje{O^^hb7BQLiDtJgq3#=SVI5%)`Ifi{1D@pjnU zLMSQ9#_sXjlu@LG4($_M`nfv-nW}OR{+}>mXoLF~?qieFp#s6k;@R(o!`rq|$`3eX z{plfe$pqMx@W^JN(UND}Ztz%Msi!h0oaX_#kmjzuWNIq}vNvrgHm~T~wQoe0A@FA? zjnYQz&8GV_*Y4iNsGD)L<9rfyM&^^x#cUN)?(eI=Qdp8E^q=l@{_ED0n~3UEi6(5_ zk3u$0(M`u^1T!~ZsNB{&p@Idk$n zF90;$gxhpEy@cxSFQTbUQ)#EgY&)Kv*2M+^BCcH7CcljSv*WmGnuk&3LiVd}aip=E zo{Ux*OnON-rG(h&6=0S|= zqJoH;6BPZn;#LD?&tk~m9MVvKpPk<`(98l2ugLGsB%6;u_@FA+6JovgXKl(q9d1Lj zKt6xj`3{dl(b9jP+UP#5wN`v7UKuQ0%F-MOKmIMDjN;l&iHGiSUR zjzi3xA;`8?{~15Od%VGM00QT5^ae|4AD%c!aw0U zirKkd_SEN3z=A)3LDsy{y6t{RyqQw$Q-wm+--q}%XI_cxgN#wen~oxMG3ianbJa=u zDtgXMYO00Z29UJi;f3j_Cuea0a>-luD1ih)VH#K~a=9VBS)u`3 zeRXy6hN>3r=Jc;&-lANCn=ai-dKZufX8mO={2Lub?SUunXpYI9)Olv)y}z0*(q-=& zXe)b1KTGJxyUD=rknyl+oR7cKI)hpfo_BLY!?$$IXMSl`O#p&6 z#}EZ9q66-d|Q7y!@@o89Vg!Ymp~ zOewhp>&B*(LzPfddoJ_T7Qe6nKCpa<<{)$@BaW=@K*0A6sXg6R6s5-Bx7#KSJIq30 zR+*iHfXi5~`^Zi~@l@vh4B%>=xVy%9`@UhLy;f+~NYUX0tOPDI6^U1GD!Q4a(W9be zdO{+MRteJI{n|W`PstbW16%V^sqNG+dUH-kS4F3|pV5^Xu!XmBQ-5h{$c?Qp(=h;iH zgT_3EX=i{#EqwCzLhHqNl!&B4ZFFYCW}m{!V+*Oy;`FQ!O?Qe$rh zcsFmMr~Su~z;IK!DVsyh*RmkQg$8$Wdah>F@?6L~oMO`q>z(NAxmzuqsxvCvFn`=t zTN^}mwqhC8WImu2f6MCg*pn?R;WRs`tHX^Meb2*o6JH(qM!bQH-#bQ`FyGn96vM}w z_k>FoLCJ)6N|^GX@-(Kp)0w|=M_MP^6f_7~wuf*d$2GjWNl}gpQ|}ILbVz>wNR~Zs z1cvc%2U&HuKuzBq^GLfez6t=~W?Ij*wr+SH%)msqPy;!0Vp|%V$^4rsw5}1}5twZ7 zf_}yAXl}z3t}#S#TtVCWI22VAK99MmAtT<4?ew-8|LPoID;8;ek)m^tw@E<}K#7Hf zWuPUaa@^4Dqq1Dz+{W3bJsfSHLL%y20!l&;2I3Q0H~j`LM%N0)iSweoRi7~!N~k6C zIm7nW18bb)XAiJX-I-JPw;yVC22~FOf|Ho08>Kg2zMJr0f|wr`9U`IW{>cz={hsE; zU?YK4{!IXW9K0gF^w2<&vYqQ_CQ!tAnAL~bLiU8Xc9ewszb(3%@8wAqCoA$lQ)1y| zkl_wLufqK%O0&z-v;OvkCzt}U&!9*k_r4J(%O~a{aFT!faRZ^MPq{+C5%AMar_063 zuSZRxjf>&fk^@gIlXUUg2$bXlO2+3u?G3piybA_X)V6;(CA;g5lLB=c1Kuf0J^5wZ z%In$rC7>xyq(>-DV-C{N6!)vJi*GaiuhNAGjpQ^B-1vu>4%hvtE)&q>ybitj5>F5# z+e8cE#BK(6u5t_f`Ku5s$GTp=)K4Xuaw_HiH_AdV9riTpO6x}8!{8OGNIH1ts9~?V zFaW*IzG2M|xPP9PC|jl~{Qa5U#Erw@9l6Fgy@&eWi2-b>Pn$#xY60F>t=Lh)&Ge0M zvzLEc(;@is(4?7xh6t)sBPmb&|ClAscTscGLpL!xqsr!(JRF_{$QyvY~U^(?mb)w(~eOa zg;Dap7XW_6_zUYQdxzaXZ6vcHSX<3$Ppv^OtmO2?D6Ao2_zT9J;f}zlc}d=@X{TAk z+nx%RlM=^A`KZ-Hg0S><8>5|9{vwGXv+r!N zKw7D${a53k_q;Srs<&K&`c0!%t*Fn~l_`Vwul??w|CK9IQBQ<-%qYxwD-rB$I&l8B zHg}62mifopY|-eu!m(G?=OYKlYuZbh^Ge)HNs)2+X-P!gK{9wag{NnK7|7_lfi=gD zDiss?lF2-6&0H%}DQ;JbGq30>WCh!V zN1${g%pGQi(X42PZ!nIO*Ove3uQonqh^HhP_>jw?Tb>YJ?xsIdSg^xk)0WtWd9l|+ zDuWJuV!9%)iG*V9(_{&wykkmkkl$L-QGoDt$O{WVNRf4ue2k4?qzhODC6ZDTasIG} zu+VxuwmK2Q!_PF01EA^owt+qK677ttQgcdZZDunYaw;#a#tE?=qumMdk{iIAMqIVy zG|1zs!0cy9f$A6wOH1lkWXlT9szRb(NMRpjZpZYf$ei(}4f0YUk}GWd2}+*oO~iMv8b9O?}3! z>wL=j|5^Y7JkNw*Bp?0mPQ4qu=id(-y_MRaa?f&UQUj#XyH5j)>@7)@dRp??4z{5m z^`33Wrj=#mvPM1J5xA$E+gYiK_TSSF$u>UlBJbnNRd~acegyDbJ}knU?>&%PD6+)U zw6V0i*<`2YO*Cra{;=^|i8oq@%l**~9p`pp`a}AG%hu?Yw)n0M`51SpjRlb}JJJ+y zi}$+xE}>iU;Yvr*B^DsSC^G5(@_ob@|36I_G!03q}S8500?A*+9>Ytw|6)gYs z>9z+MB5FVM!Nx`NV}Nm3CCV$-MmgIYGIDx0S<47irvc5=yqUaRGze=7ob*}moOS7{@8K78tmLS44NJ+JYY0zG#l9i z9iwOg!mqVh>5RF4pdf5?YZl>`UrpCxqWp@7G?0FM6$JE0j`-y&tO+uI0!T%by-Gh! z_u%enCS?D8q$Q{}@nGAAKkrWSMO|iglU6cJc|4`@q0khtJ%UJ%_GkK`6P4hI#rJ!d zy(DRvk+KuTQTff{FHyuL7lOL4)?KmQ{Y(8>)>-rR3VMW zGX?>PmUMzFY5zw~y2FeZ0=%P>4wtTI$4hTay%YF*)Xd2`tG(HK8FZs}C>x)l(KsJ{Gneb43GTv!ixIC&{%8@aNWGUatRAkH*)u_>Zp z0uf-h!JN5>KkM`rNc!IWe{yU|kO$CnzMnAPDI~i5XtZbNC`>H%ibrloM$=o@ed(X| z9l(El%O_sD4Mfi_Y`3#K{KPB38I)!R#(wp!TIEq2yp4SHR3Jmx@u$Rr7L9diu`?UQ zF&^*wA)5}Ry6RI}oY}59an{$v^fT00g!W7BvI(7^3mRKShlPz5zbIh)I*bhEC+Sdj zY$>|n&5sX$)A2j?t#=C}%BRwLv&vnv6b=3FJon*}ySWYN64n`ic3fM+ETVB{OKy2s zK6F9GlNgr056n35v^=V^faW!w&;oy;*Us5%3`oXigc0_<12Hq2!m1{Wt*P)#p9&SNtM$VuP(H2%2`U z)?ml=CLexNUewc5N#pFRsChjuzV0(7ZG~!o+*iu;XUyAGccNh>qEn}ubzes9ddRKN zbW1q?OQpFQiO-uCMT8rA@p&dY3}hD56_P6y5in>{Z7c9m_mkffN;~l#9_#ey6Yv7_ z8Rf4?as4)FH7u9dWC%_BykwH`_Jl^H@z%NV4y~ad2}?#}tra$G_BGJfm{&0Y*d8VG zpO1iER5j_%6CoO1zGE$_Sz$Tv4rVsmjqmsxqdJ!DkZwPQV{@e!2Ty-{$syPL(E9QH zilzA`CMZ5#-I&O+1AWp0p-)TIGm@!d7SE-A2gj8flt$$lEg&H>K{fs&MCBMc+m@^* z9dT3*2;#$BLJNv&ERq>BpnqoedmlKz&y3z;(g&GtNyfND30Sv8f#I2V!~V|C0(YF8 z#~+VmBXTiZ9ksLgfu*p32n@@wcz3PAV@$}yx68Z)o-ME5RhjW?#BK3@8|_yj#hFE? z6Btd6LdmC6-Rz5d$}K|>UivZ%bC~JQWAtZge?JY@=4H;Kx!okZ{{qCd#kRFMGag|9 zktU=c{R`O*7z!(Pn5X!fqVvZeSNyj0=DQ{&U`aiQZ_Hp;kkkZ>cabs1 z%8~wj-j$pcH1#!AaZ0rms_E;|RJ2iVVDq&huH-2OFTHDWE#L{#usLj2d{b9wPaY1JgA4n$sydWyB;XsMzzkASS_!eh=iMglA^ z!meDZ=z2?h9RQSa@%-)Fk&R&;1kZGm5>p8kaM&0JU;2gxII2U5QvJ}|_q_T6um8-{TVnI(?9dt=UuJ9yk0b4)yLh0nQzCrJJq z{Nacyhd~h+6v>!{T2E&f>hcn%I!>VgDfjqD+x(tsw8fScwwO>I(Z~ac4^kck?gnE% zYHY0V6OA(n{r2QL@KndWQZP^!Fv1{pwR&{p31omxP%Pan~M;^2J`YkHuY4B7Jch4(sBX-FikD0iY)&o7NamkLpmBb0rQSdTz zy|@w1ELG@rYxRv%9GTqfA6uoAJ3V5EH8Xr2mga(oGEguDo+uQ#Qh2+hR(bWmm?f0C z_`z<$==7pHWzpO+wfit$VrWNgTeIbw8G*UJOgdt#eamjQI|V_{#+4gj$UKfc8Lu6Vpmf0_V4OIXtgfkDl{P+3pWsuaHPcf()NGo$nM zN@1@aGn37kHGo5SYIKI@#V~{0h<>#9ZexCw0Ge9YUS_O2 znJ-iVQ9TT5-#8yI4q&dP1RVUCxv-P9WyIEvZ?G~6Il=0~RVKm%>ufJu?P$-l%S)%c zcX=Jm(Vyz3JScWS?D8Kd4X^j3gsUg(uuN4-S%^;&h36Q@uw38y_*yF)GvF13U9vG7 zvoJjQS;KLQHrPTC(rH_L$+-eK>1-&_&M$qCnNAtsQJ_3K910EY&E|PJQLBtlZF;AB z?(v4gy%43YNqagG1ZHBmIS*a_>qehh$4WgP=6%MaGNG~UCx1_Nx!lApydddupIkg{ z==rwhqkJe(|IQ*RLLbbognM68xnY7%Jn)SAktLq^N)wuho`>q$p7}TPhT1Px{Xhm z)-oe6*o9g#Q&bY#8pW!^6NcM_P@n3#o74jI1}X~uKwyNmXcAb;|yVa&sTE=zAyHREQRhhTNgh8Q3J3J4voLEATp|jD`bDo#LQzSIc z!SKzF{;czq^_&wW>FmDjVV2V${72~Lt9Xq}{`?I&6w3_Tsm6WxwyBkXKwc+ity`av z$)*0#!qC19t{FA=;E4B5er#pY9=`RKAmJJ-{BNo)$d<_;sQAjK`*k%E1Y)bvI+o`nTkH_lF4Pt_pDW{f}u}9~giC3>2>aWZ8Wa(Bv5d5*Z!le6(;L83>$qeZOFt&Ock272;2iQ-Ox)Jc=!HPVCF!+0}uzP@Y7nAQvnU42H1~z zjS-9i3bBdUcYP`Xk~2fK*}KqjE(*y4R*~o-)6R1X^L^hc5+Ab7xD_TpCptCrvQ%!- zHzS;z+?3sXE1xGqF(%Vvv-+Rd3J?;nGBCRMm?B&3&XGsAqpbbLtlCpVEZ7e?r|+*{-A*6@p|Y*zkd07nI% zjQPv5ES8Tl*~9R$m94uaiYQ*09>A*Dl%Zu{8pW$uX~T;mKV6<-tZLf%5LY&wX=VP# zyvJW(QD8E#lR85BQOKv6@u_!IJ}gL>M&?m*1LbKj>&xdJN;AX%lKI;gt!Q3u*H!?d zzC)hK)?Yt<<$dxI#_SC5IRKNes z*s@?7UmJtt0{pWLm9VBoCRT77VhiYWt{LXOmUw#41Uz&nHb%V0g2%t|79#mCoH;G>Jz3OxUw2>3g#dwPdbm{qURM^`FXo#a~(q*Y~JoR4>(!At*>UIL6c zd&WBMDA$Vks~h7c{OXEv;Ht3UH6NyjU2g34af=M9bIH09nysB=cHNbStc7Mz3$obw zhoRU`*ofRCQ&Usdzkl3j+5!}==kF{_OPd!#3Azv4 zd#Xn>NAox;Y%$bWje&cZo8I@*xoK8MK&M4%JxzVgI-n5Rb2G=SQhZNQy%P=iKXd4Q zeZWoqWh*nGQD%9^)%odgAZY1?kvmT1i0t)0n!bnB5HBTQtsMcm3k6t!ZoG%-B-B%hF<+ zCH|knf9Gk-H5g$6yukf^XmZ-Xv=kKbu!q06fJ$Hyai;Xby7a{%`>Mkn+Mz=8Ip7-` z&PdJ;uC+9XMUL^R`3)KlHEaWy>+b<4q8~HPIN`_8IW0+m9tF#1DVYVAz#4kNW4Q+~?N+7J z>Xk;X{=@w;JlQtjdV$CuUxk&<%4T}Vdb@aDRV0^}X-~cbM?PPC)+Zs4X}fW+y;cTM z`bLLL&4OyMP79AtysFOI_7wL`uZOq-k-;KuW-5;DMk`t zy!)%y3Xg$Og(<^Cg(7-4ruGWBX-*qZr`%h{Uu7*2YjPGmw&&m)Pk$U3u~z-hZ-dcf zn1mGwPci z-2f(5pKW%$_H&Fp`>jU=?o(9yO$<3g-i#E$Dll9t5d+6O-^7bUPle@}-+<@G*#8K6 z+^guQT}>PD&fsq9?nXNxXVw_bO}93*=pdP|i}Jz0Y6$uQu||ehh5vF|7B5TA zmtImz>bHZN5h8Dz@mB>`16aERt)6|TA>!$-*dI-E4+gECcswhWA%@$!+_3K29v%^C zTMw*p8f585$b>)9+_J`QQ8WK!C?t*{qdMok|9#AVx@l3>2c;!g+Vg5kuF{%?>6h(y z0S1a^c<8tBOsWPnB@$=sGi!cZVavaU%>(?mDDzA<7vY*1povPhBXUH7aoMC2xzczz zMt~xlZi#K&^1^H1da%rlXGV!Y_!9l;85$@l~O&RIjJW@s94>B*weJ z6K**JNqcN3Og>Bbc$LR#%kiV*+&Pn2!#9*mRc~xTXBPb}zSje2^vQ`FZ@RRgn;FOG z>Q5O-;K~kFMQhM6&cYop!CnE^E6 z=`|sP=`f;w`|TO#sZ`7f9Zmi?-qGCf5~nOy@F|CzfyTk><{6M=cp?As{u6?f&2Rm9R5>bVXsK`+uo+!EizsL{ym zX}Qnzw%|MdLBVr+J(2W&)sl4mk!P6Y_}SODtQnR2(vgAk{k5P1941_hHCM2qq2~KG~eYE29JgpEyK4WU0Q=84+7IXTi0^l zM+eIsZ?V4i+uE=<1z=~H{RuZ{bfs#@Sx6h_p`>Gq(Ij7H_+nD z8m#zPVw&#E??dKA)6t`g%PWesTs=^9!zRmnzMnYjw z5>X3`kCzjHXBdCt95mZYd(i#>&))NnR0-Nf%Ne}Zyi$#tQim0HrMT>#yZ4K8k8kv$ zJt%gz^dU0ZeJJK^@%2x9;?Xki29@!_((x(M3hy$i3G2f#*0y;$LElm+@Tx{We-Z?I z2QIml@~ko!R5e4sEH%vUPe8=c`1bU$XJx`26jY~oDtJu?!$PC%iuM^_>ETi7Ffx>_ zW?=|2Xk+C(-1=rlvONXw=rLY1@yoZCN&O-P@nNK|3}5LIy^aZl#Fc_s<0dy)?{z9` zB`op!4Zz4)R!U%w!1Y3Mk&CHQ#=Q5N;uk5l{94d;L`9R;>F&@h=7Y`<()@Ur&eh)i zjrbC?J>R3yCuEEQ5!D^;GcXCVS~V_2%2|>09>Bj0yVba5a+6PO@(ndLj_S8baeR6@ z_K4Q8sgd5+aWmZNqPIaG&+B_Djqp;#EAtg**UD_iFwOsz$D?CCZ*cZ&MD z8pE8gow2rEo1eGn&kS5m(I~|INne_kYW({_Sql92cX&s%f`3;tJ}o{#hn>aG$lH*{ zXtF#Q;rjz{TuVRmM_UPgDs>Oht4Sw#F66NbP$pyH+yt;x%cgY>+7Bu5+To%wlqC^fe3ui8Imrxe{S$}UGxpnmnPd2fs&ae=qO;_se5!I?Y&qyNkn3Z z*7@9wMRM!E6-R>P{><|RI=pnec6Gc@tOg`NdJ5dDx|{h&hV2Y$S&%g9k_WEb-dhD& zm7kJZw{EFM0t-lTO$TRj@&_)GN<`ma`!2bwBbzl5qJ3PbFW`L6b7|?D6N_enG%4q*emIK8_(ki zKI4||ruMp0^tvf)TrY^GjUY{jO!FtS0`#22SxKRzHKAHo?n~iDptPkwV8*0$eY0)$ z;Fdw~Vh12Wy%DYXIR7}W`Y!_}Yu&T$r|c6vMBpsMG4it{{EhuhQSX9Pyt>V8&-%EC zU6gdH(Y4KXnw;&BMex|TRQ0lZeI&4lAARtHnKQ^BJdWIB?QvofY2$BE*KW{>#MvWt z7hi{mIlB0NSO48ayY7C|2XDM&|21}dUC$}_{T(ci$G6sOC?SL~KodB&Exx(}< zwJ*LxsWWwD)V8c1E(h3~18b+`E9MlQ$0-;$%lXHRq9dTxUa*XF!v*tMM| zO^}Qcwo zZy1!?D8i>;UKLxHIpJ-8ui89{UNV0&pYkqw(OxfZ=bI5Gkh214MJ9aQeyv?+Bji{2fJPs8Cx7IGS^kvdSP(XQ@vkZoY3*-gzvxZOm#q{%D3+4%X)D9{aYF{S$;Z= znV6Q)U^2gTkdDgBvlRH8I9o%xP^~m7@?eF#h{fy$w}kWt_KIk0)luPE%ri(?8`#Xw zftmcFU*g;?_8<7max#OBb6R49Fwn_p&f{uS@cUxYJ-kst2s0X&CG+>g6xHmrDwP=b z!|TyZT+KgD+$E>m%FXAcqlzM<>$R6(9c8`0Z8_13tUcVKy*Ia}BdqAQ1w#?>rs}(h zU_`2;eBTNwI;QpSRJb3oQ=~Y?x>qBR9JZ*H4j9;#MY&~9uF;$Nu=PMbgaKfdrLbup zcxVpqf}D1yIz*?c&TmB~n{6*x@+Dt6$l0xy98IPJ4A=*ZqDq?vE z)>7-j*le`k6syBU`+h(q)ND}E_gEAcI3BIQ((Mq4QXc1F*)wY=k@yNvLnP ztU^vP3in3Mt|33m=^g#K3B3FcB$NG2_O@7mfutJ>C?iHPwbbuY zL~JObb#x-6ZU)=X;<2=ZRqnzsx_?X&D$9(zEK${uB&UoSkovQ$QF&T`HgbS4A)E|g zKon9$u#7D_3WpkF+w4<%7aNeH^sSaO7vq5T4|`+WUCb_dO?f3)<%EKlUVRDG!O3;h zz?BDQG&W5o?ixl`zrvd1M>sZVCUtrH2YD3(%F!P4gZC3P$vw7?lv%pYPcvM;m4bAnn+4tktKXYGu-JX3f!aMPg(wb47K)}}R77!cc z^v`fV&qdy&zML-EJ(fP}@kv&i&CVLY_DVNChN3d4d>l?4A=N9()jWHgob|t~mu3hMfs`}HDCrlxJ|{kjK9VWGcGx`T-BY0kZoYb&BaJ%3!bD65}ajNiN8 zIXsVGDMb$IcuU!FztjKz0swyqXaWL|1bXad;pVu(%Gz};s@Q@Iu*|Y`xd(X`g{q|& zv`F5N_QizCjuG;(Prz`PfDmMy*YsHwEoq-WJ0FPnDYQwi0C7ZA0^se>YNvSs|EK`I zLw}`G?&6`fX#(Zr0Bz&m08DeXIA%erFi^+7AT7t!0%7ZvWA>HtC5lE^KXZ$fc7UO! zAwB2&I7@VDyVaup$6;9HQouNTc;ik>Mn;@qMc})=bk0{P>tU4#CVi`$n{}ejYUIBF@c8 z3@<)=f4~@W+?)2YolmIoeO&5?S`VQ*${e8x$8zwdOhPcJ{KZwmNFhGy-1Jre02+z* z(=XAKvkz)C;|qcnstyl>?s~k=riEL{=7pomL8eNBTqYvf;AAV4Fa3`0vt*1ZWZ%mV z+d?Z|tBC6I$hP%*dk!YP-@#Zo)(~&adV{200W1H(wI#{fTUM0^hW^qTm#t7vb#;*F zh0QtIYvpd_oRP);Q_<@8h->HjBD-$zBODZ(9MS$Xzu0v^*28eBFUFtfl)|KJWFd)vX zp-!}tvsZT;?p2za>VMwUS8zG1i~c;ejG0f3x&05&28bc-R33ag7Pys0*wr$2D=|z} z-ir4qnX4PtwHTIIz=WSv_%t=dJUG8=Sd~+gDj98G7W%Ho&Ol+ROf*J(zR=v)Mx#}t zcRhbb3_V2XPF+xLw~O4A@{Iv5b#+|&QbN)J6!;}gB+Xz^ z3>-7Ln8nK`nJ`|#B}YP0QDk4`+6b5u7@#gkYma>iX%~22J?uJy%M8S^&J3M?S_# zfz+Vxk8?G!LZ0_PLtupeXpQcy;;*`dfKQQdxQUD8jx51trG|0qAlH5hpaCJl9*JF% z4~YR-;70pl1ZlTTHJiU}^-UL$`Wp64uh^0zfJu>XpNx`>3gukK6xN7WTldg zLiYVwZYj$!{~M<;*m}EhVT1N=y^h)LXZn|A2G=J0LX1-So~^y>_xBPJE41c(-{+S= zOZg*X1sH{NWsFUCu}s-tZG40Q^Lf$fmAlx zuA!?nqZ+}H*^zz--}XTKtKb(nH<`N1P4-ujTC}c%HR9&$yyferWtw(DG01A4xkBhh zY*2OV8;qSxomhN|*C6YU@vLcsJ<|uz^$gYJ=0I|g`p>yDn~?e zRI^dFV@a1yE|6#~Vyk!?40I~vIW?Cjhj)kOopgF*>|r~e zEsk**1q{(@1dcn2EJNys&Cb$c7)GSA=I!KPT6nMc@V9l*hrZo%u;g1o1Kg&0oJ31zfE5)YUQ_@2^*DQG%aE>tX75pquLE z0DOuigWyV5p1u9Ynp7`ecLL_N6F-(Pefu8=(vSSwv9*vZWNvZ}8DBtbk|e+uGqn+) zN&v>dV%IgsA8#sK^<_$b`?=^{BPdaqJjMyPb_b8vHm{rhI<)dlDA8*vQBMh+vs*`x zpmC!aMA8~j1(Z}fmLe^S1Pl}(PC{71@=HxiQ-EDf_p9wwCJm7deM+sT!1K0uX2l1Q zL#JYN=-j?#=pno!3H7YN@q{@=9`)1|Reu$~gyOOLA$iZILGD6+G);2W*YKAw`1IeM zZwKgzYf%Z zxgA4)G%lE*>K*)(;w*dLO<}X^_u)EH}{hUi*;a#a=b#^v~Cl$OT8NMUL<` zmE>zV#>B8v$Eb1ftNqSDy%Ht8|5c2oVWOlNT3*X9Dh_JWzu+{#AoS>EQ?c-BM(DH@ zgLhvK`DQ?{@s%oP4IjKY-%68Eu#*GG5WG#Gp3{&dSr z=s;Dbb|iwst`$v;>UX1p9*10>X@tg4z!ulu@7_!(WG|p2=yxYyn}#k@J*8)Uys7pm zKUj3>*RSphF84}P!gy22VSK7)QR}beTPm{X(}UWm>wmtnF-wiO6(pD2R4r~ffJ!Ij z9<&1Ywn$-~-qU$4I^y|Rb;RVoX0OKyGokrj9kJ7@na?5tD|B!mw?YWo6uVTM--cv3 z^(*u=IU`z#2*wh)Hvijjjj52x+)`6JWqPu6wc+4cC|ZKc&rRmqo~fMH)J-f9@=Fr+ zd1+rGxA60RMUJ8qp1kJ91kgMTlqUvgJu6BG+4C>iNdYOoh!Ydv`|h791oJEYDX$JO z*!g}Bi)sm07nqtl38;VS@bL3jaw8fB0d-8``se>QSOVnPgLl+yoSZ;;Q&UTCPIID5Xwap^@TLjU`~#zN*>g9j`+qF&Sf z+OZ{Lw&fd=BgJqh(qimfQ=_M+Y6(&S15b4s%qz#}|BlmcC#>+o-HCh{3j?k5Xs4A! z$dqPJc?w_Y&>IjX)>;bA8Vn@-34t;Jgu+v^oe7_47j?_!L>O%pI^VXO%LQ>0aE*`n z1mOb660E62V3CcnQeo4@U;cv(dwrFQjOS>i2%7Jc^?}Fjbim%biu5+EG447N11<>Q ziU3JMz!(wyuIZ%U(I%*Mp^Wcx*t0_P2SCc$!hE1<>Bw@2Ku6$RWvjt23Y_u{E+%`{O+t~!MzUvRQ@>spvv3eZAGIp<<5GQj_fx@H22=L zoMYe%o)@QuRHChKoN=d%=VuLCg*CNJvij%5&mGXt+O+KvLajY%E|FN>uhX$@Q=3NO~wY{sVQ0(5-sg=pUnE1kd_sSTGwO% z-7~CPXMQnwNFupN&I*o#o822ue9*<_d_r`1@5#I z$kSFIu<7$~@lf34!Er`c6!%b~Rl%4r(5^#223i&@nCm?9sRX!J+%jihOzR0q?c(i& zSDKm>C!i=ve~-&TGesk+4Wt!4t(N5iI)FJ#k(h6>_HOyFegFkMqiUL7K~qutDGM2I zei55GSbWuhEm&H~q+C!lONR+ru9z1RcpL_unHh`Th*|v2FhaT^CU?+kh4Ga&CXall&XTe6C zu$ocR&!$YXtu8Pb_MQM5+WV1~>Rm`CgL|g_OU94K`PY3*L?tK0Lx8IE_v_2t+9ix> zIUdX3xX}9WML18?N63>nC6He?*pq(&5&afCqmE+H>5~P5nR`7}qE`;try2vD5LT0k zA(JFZeT!U+d_-*FX6AToRBn%LMzmBEKos+i12^`$&|cO+xZnm^DaDXuHIS=SKm} zY&8chuf~CvfTm*O(*4o-vR@;|J0irvmWMYwuYx3p(Q1ui(?kn*6oe zVUexP4(btS&(Hm67v+k+nJjfwDZ#UZlF_(OP4^@^5iFbH9a9};Q}cv(Ws^xXG5|G< zqoQ|L#}3e|Yz-zkQng>Ov(#mu<*VzdACRrJ=cy}6!Rsif)<#2fB4w||E;Msi-+cV1 zqJWBi9aw@mlc$L(#*8Jn_n=0TzF773;cIK$PjfNNPXy$UGoZhrn7+XGgY;^A7<;@u ziy{+(6jJ4rs7^qV>8XY>lB!nx$NN|r_-B1q^BS?DQ#70kwG;6WTmA-xb{;5gQ*H3;{uvxb5 zc=u-P(Ob(%Vvl!&hJ}c+6}tGr(>6r}fkW2Nv}U ztB3KU-%o6koirS(Ce%-ejV@bZ-DJIq#HPDwtUwa;19tvcku}xg*Odf< z2gEV2{EFlhUDIqZ=C}aff*D~_MeRc_qmYiMfjO6;_<;O+YF*+1E}vu0EM-!7jBp=e zkvd`QYn}#;U9f${>9g6<_CQH>R}N?k0=UV3j?oiuA&#*dDn;-parb>q+0)L*QNQ@)uHdKnG+r>`Um9}+XK$9kLuVrr!L)a~JUD({ z=jbX_12%NW=^%cR#Ol{#Gug3S(_}ZOl&Z4m^Z#B_dpnggYLv_u8G( zL1fF$u_MY}Jw2-xN4!nmm_jscye%Lg(7Ge8K{T~9EwIc1^>uo@{1$Hs|3U))III%Q zXWcJrqm6)P&3bO3`QaTSP`Bfq7{-%dvTx(*17e$0?+w^zr06_7`F1;*(YkM36*Ryp z10fHyt8j|?Dg}VAp6Yi4<*=3^Hd_;ND0K5MM!v)2N`Licf+y2&y#jq-X^6EhLNo4J z0{(6;?j3U=A^s-9)C8#J;;HMF2gEG1-Ow>;!1mB3>PTc1`;v8m?B63Ara|)sg0O_fkO@~GmLYN{Ub);^`xjr?7WM;pKJg+NyRaSv z`!6x6n``;Uqw#9Y=S5MCws3iF3WZ&Sp^gM{E+>tD_qiRkP)6dh74@?hk=&=~ge*C7 zSN}_mOVJQvkEPklCSo(1SbBN7v)snezkb^V>cUTHD>6(EbkwqE=i;r%;yuQ`+y9p_ z78HN5ssA@u^Ig+|ye&JqVGwzoNn^p3GlqE3QuVmFz_oSY@ezd|#&6ptK`=(V%qzKx z72=0EBp_vOQxu`w^Qi1LS3)w-~XYr;z$ES}~ z+Nmq*PrqFUoJ8}iMhRp;%9a?0wJ^BIjM=WDPP#SkKe?7fN+-BWU`&tjBC~g`K(o%{ z+TO%Mjt1{pb074fZC>Qs_mA3&TTYyd$w9j!HN1*iGphpL{g3~$aYf;0eq$w0JaDhy zl{iyo7|lHjh#^X}5AkSTbw=*~d(m%&ICD3KE+i8xiI~R3X-?bXe$@(vnjDL!V6zih zhkO9xWd2-PbC+Qv92}c`Q#+E^yV^|`)sBW!`DN+4<)?S@_7buU6@15}OdLnSSx{l~ ziH{_m{?Tjvt`#{hs?oHbWvI&Tn3njAl*MHH)11O&fx@6*cT4PsmN~W0-9cv8&cc5W zj#qtP?~7=>*rx*~e|q_5@$tG``l#m+o4IXo|Br;$xu^%VLsLtQ>Gl7;#|Cc~B7HVG z+Cx@AWHHM^&~JRi+}mc)+_cpYu(F(fM5WoI2P%_6ZCSAg`IK`mEbh(?07$EoJ|yML zzk=qQ_WV(5f70YVdaN?(`~BjCo^KoAbs$SjCvxP?uL?SH8A-YE51X>e~;6q2-%+{x}Q=K$qUbSPHuz}{HuI+CIC+=O`+pI5uqh7T- zGWU5>2xjV*U^eOlW2{m{Thy7xbHPiPim>3TS=@6;sD2jIYV8@5%DDCVkUEcVyLh1ygCC2ER z93G<*YAq&vn+Oq)SZO@HR3E;SihoEUPa)`aXYRYjxRPajB*MyT_9Oh{JrWVQ1)Lqm zsu*vXV4MxSv-j8lIU}rem=oYFl~ZMVZ&rocN-K1)vXvca?t?oNH}~&wW>S7)qxyaF zuIUx)8dcv90fTXk%#XBShJfsk@HYTHm$+V1YMlEzXglaAxZhwq_duySidPcCX_Oq%q4cS@55?kXM85vzQRTlzt*bM{fCFq{3g zno&pa?Hc0Vd76{sN;N3uryQ+;5l`NgyHO4`o{(45AU9NMUnBJ9aNrY z!{TmPs+wDNWcxi6o>&Si&Sc+nTmI(3rm`C}M4dmCllC~kVUf~^iOe_WD0Ec*sb zU^~R)G#Wfbz92K5P3QW#Gs1-{?vNQnb)PezOHj4#+z9vJmtjtgMbipO!4g_M+?<5z zxh_Ckk4=dgCfe0j+j~?DApEgPs@&TpOTpl7aj=)6HCu+u3e77yo5>VTb37op+cxOt z#lL@4K;eC3nMRVg=2B_5;auE9{!PF=hohp;R zk{N~lAB=*eWf~7QNnIiuacigv`}~Zrsk?(uEDFbN*Q$dfQTf#~)@Y~`f6SggBDzFA z5R;bHQRxg0JG5F_>%dCy6IGv0?G*{Q z5T8J5Q0MM(_yJ>^2LWra$RVj=ezoC1p_h*SzYhv>>fbVU0xQJ#XduS&;snMdfrN4_%iUGBo_M5Zcfi4`6@^}guyyOn5mcq*sutHE!~>1s<1 z`!+PbQ!RctUszTD%eZLqpR=AOxTCohlZ3;t>Oak74>={Bu6=$cH10e7~;&U@>(kk+vV-mkpB|GySs z&AGylvig{{;A|wGNZ6DtNiiR*R1%rAAO3-POCPZ9m*Jf?Vz(u3bh;oD{AhbKZ*$Hl z-~lp0fySx$i%VYqLmh5o7Hh^c6RNq!iU_F=I`tPHf0?(9;gl;vwzkd7sTE`!bN{J|eFqgO4$mx0jrwV5=`O zvx(7y2L<)xT)!Ua8WUywuu!%8JY=o1ah}its_@%SD;BkKk8=2W{Ihzq2-^qJRElz@m&kE9uw0%v#8Fd_$m{!E%A;{@GS+bJLv|3oHbbLAqUlzj|-ett@fKp1}!Zz4o?47I(PCp>q|F0 zzh3eE05Mp8CoKtz@mf-#!Zh+yw095HP(Br@XkeA~Kgj#HkUQ(aNEA0Vc-*{QefU0m zX322VK&?}N$R-D7@GD8B@wpZ_b`&-L)~T@>ImGut(Keq>q9eHow=Mz3dztB_H$s?(fl!bOBDxu<|BZEXk#1XpXz)qCS#xhjDO$Tts*g+>MsE@= zHWEkTKB=VYb$k&V*g7cDJO3)ZMbDY$M1HBEJ}#-saqRF6?Y-rcXzPDKDP=$@I0pURViNa~w` zL))oK_nFPsmGktY2Ll0%DPhyVLA=0v$^Gmbw49n90F!c*te*& z!gfCKCwemyWTlPEVu>mvMEE|AY;=r@Of0=f$5+7dnj{%B}3c?{pJT<6xpUGzYb4@?*51 zg43w?Cym^nRM)q`?1iIEeJa=2QY-Ov8kw&5osTLlL_E%>#>LCP>I0d9m(w0 z;a~QA<2DUI19Xqo{Ea7P^0)CENnK`If zh}N&22rQ%*p9Co6^K9Sh*QTvBe93RJzeVFOZJr+n_@9^l+rDe zCqBySefKR3;C7=#YaKxOaro-Lk|Bkg0);PAj9FQ7>I>Z!zV8LkK2!@$HyGBWY`si( z*Le&b`!*)s=)Cr3dSe+!GZi9#h1&TSCNvmFG{!$<1$17}dL+EsX zP6HNl8*KLkY_}9KhXS3qHZFARAL}~}rQ{fFHbC%{W??-FHkL=Q$Cq}7%wi3ik|d3* zK)o-C{y8&`Up-d0&>Hr+>)1P5CZ1LBXvcimmGf*DHjVzFxmg6?IbRmjY(1dTdrE@-UkvS!gv6m!DnjGR9G$yDz!!7Xr z;|xEmtNwCy$SLXQ+oCD+$F&DOc#{%o!15f?UurF_?$sUDy}Oakj}wezb~bB&blol{ zT_5I+0NZY)wG}143wUHI4?~|eEtZfuY`e*q%B(*SAM~=^b&_nNYP2s#P#=aopt9qO zq*p#(4#cIePHqW#W!u>6|M$c;{u|J?GB?(qS_@`G2UL% z=eUa+H97%VHG)F0$NN56IrF;-;hxQ1Zn*fcT#NS*v=JV)rI|BDmYJ`E^-+OR3wk5GQgESBXGrOA_H-V0==MN+)8P~JJT({O&JYv^<{k{L{t zo}>|?k#aSx_sBQrMbsxW^K(H=Z(9;TV31?)yrGk;rc7tq(jf-$ZN0w#gOX>Lci=m` z7E?BI`DmZZr=q~3f)N{BneS;OH_U!5d}sy z=!qoMk2ddhqy!j>Se^Wd-NUu2?<5REw+7e|EcIq6{Y?m~WXG1QdC!B-IY|3SCUJ=bK3dU#L zPRhE&eG4HZi11KWh2kO<{%`sDPYv;o85QhBCNwmzKS=!3TC z?TcoV2rV)7-VJ<{t18$LaXRi9Oi3Gtx1=nt=V!ue9$=L!7t1Ql?~$7MEiH3?lEF0C zGvj%aiN$qAZ#9(zES6e2<@^_#e{?WU- zwEtp7{?eC%)LpQQl9%lOin-c&6d>>a=bOT=Tr&XQHzUhb9Mjx;7j#c6jK_BKCnUc zsh+RMR$tpxxEh4vUVG2x^V!i9;?%VTC8P)>uYfgUcnP6;bVRrE3nmnLn~u$!yr8nU z?c^3=FQgqZ13ev}Cv9NhyG41U`2#2s>cdQfe47J#i+r`|A|cJW)!ijCVtQ#Qj=W)# z0y4oeFzme*qTnk6$@AfoC1s`TuW$oOZp))0_J!o?}uUUZO+W8}ovDP#>GGNN%57 zt|V{K^q~VdPEpaJ>Nb3xsWDuQapm0{nuA@>ImFqPnJmlCxW;FIDZB<$4w$c>g-s_I zYUeqLQPUaCjzd3)jQ!ys2I*7SjdEKy8vaeWv#NDb8Wf{D+i2kKnUEZ{?-skFtR8j7 zRw-p0{gGinj!Ownefz33C7)e|3XRAQL%&m0Tj8`ev@Y59w^Vp31VFzGx8Ex=!@5iQ zT_C#2Lvrl(Z%E_5F$}N8xl(p7*3PEcu0Tld#=;@j!RU_LVEFfonvldj8#Iw?I272y zC7pTy;hf}^sie=rER}r$Fki}|lJd?5cq0UoCKenG> z>ptft;L;il8Wb_Q7j48KxKXix{1$heu#&BHTk9)d2veEad^A>t*DxziN*}45%L>SN z?M6T7>QA7A6$K^{p^lfcwKND2El}RMhY+%E{lstjnAXe}Z%5(CT|H0f9|-eZ=r_c9 zBJJ-DO;97YlA)rCgKb9h^@cFp9FkD^m$g*cziKM)53B5VL)!7xC#CFH9}SiU_k1cb z9Bi!6Kc7-{!Cd^$-Koa`jA5~{uck$UR z^!eXRYX$QU9>kZUUeCJH`|t%O5cGHG!>`)C#k8xGgfza{-p+nZBF#ukH8>#Zf2BB)^~=oB5ECN=O=sag^bEqSsRM11O)yDdUc*&7MA}SRi_Ag zU^@c*4#FPgMs}<}z8B3Dt2)hRts%I{{kypJDm>wq@aAd^CzYY#CNC|h-s-zlsz8Np zOnA5#=v=z^HXT`c$>78e4RSH=l-CY^f!e263x-TZS@Z4T71jVq+ot`s>6KWq|*`Yn*B`8*O zzp1_yT60y7-T7Hjs<4&ddF*M*o$xnIijCjxic! z%Y6Y724}iY%%IqU14si8O|!!wj#~^D2xR%rjpH(LmG{IlMh$(>?JT<3iJ|Gn8`}Y{ zB$5p3S;pvR-Sa&WkiBxC_kyW_?=3QfeHo}V+PbNOJH|os3kH?gr`B`np6o7)<`n*E z{Iq;A)7S+I+E=3py<1^n|FR|wicHeVjvZZNJMtynM>PU9Gi2_~-ZH_vDBQO){m55# z0=iGyP7VN$2*yTp&!v5Hg@hiNQox=tf$Uvh)y>&+$D;}+&h$QzW3z+?o{CSyTc-6f z6tpY9l?8Q7oq$-D>bt;hQ+8ux=A`{r;(%}}bTfy_I8PE;+wFlX`Zfr`<^#XG?aB5* ztzAU|%>1m}17gqdU3B29YMYIwfkg1|*}UtX2c`Grr=ou==XnWlu-uJL6-_fmK2N7gunR5H++Uwhi|TKGNCbvv{$O zdAmYtPMYG9HidlZ`{7tT)0mHFkO?6L`b{3qvRPm`4m(rU&FoY`afd3c27 zpJPb@ZtET)>_*v8vw5J~-_7I(FE6;vl?>?Jc;$b>HqIap2WDj{>KlMBye0^}A<`=5d zkdK>+25~Va^7^1ZUx9V{T}WPr!mYjnRf74sq-^uW`r?JHmMle)urw88<}!tbg7-d? ziTAwIBYF3~_u8;aN6Sl%_zTel`DHKT5d|oQF^B6+5s+kjQcl;h<$K^uVBXglW+3=()m+J zlf}jqyv#nSP?2!NT9gAZ)~Tq^NomtHXlC`hTwG+G4Jm0fsIJdMxh~?>Wbc-B7}^`& zYDx|96T{wflC#mFnDhV<7r+_62QQ+#G z_uTe0)={4wY5L?+1IKvKL;wJ$bLUP<8@FKGK?0M2;Zwa+$iIGxjt9%f@$LFenF_(6 ztl&Gp{~-8kdvq@&NUgmy}F^QufMN+@h-#Wv{Db_LzOfj)4N3BHqUI~f%Cx1OZc zjZnCAU$5V8XP57|2_CuYIFPUrE=)apQ^kS@If&NUU21fo>sNEDDfs)!msb~DY2A%G zxEe_XzeM{tvWM7an9Mwz^N7+^=}2@rGf|nQyrgdeo6_zyPIX;y1q<11Max03x-9AFuIXZBXccr!TZ| z%Uhd7n|06X#JW4zo8;c6mR|!@s-{FpxQF z-)nbUffHPoVs7Yf21hyx*&~2NaHq^bFqjDByFzd|U4$3%O<%LUHNQsSWU*LIT+juw zd8R(K?@n2Pbi51@h&u(K(gCPr1uKM7ak$I7u*bUC>3Ao3* zJg<55*tyo!*6!9S%#GK2K+%rnTSvsq%tzm&05k>exDeOW>KqWja)(b)2;{NzV{d*a zYb`=k2Jcj4Y z*4DW8h&#oDkBKcF;U!E52Jdj1Csq&3u!shBwQYEr_H<+~-Z{jw zX;k94nx@z&@}y|jYFuJ`Z(IAP)r0ctaFA!URUo_WP(i$%Jjm&~ z6I}X>X<>lj=5(Y}qLE4_`S9??8JDJZ(XR^)_Y%{6saH!g1L`mQrh@caDsGLw)RDKQ z0)zgAxs8a;?gw>Z4_jAD@h-9sLi%nAoZH1wA%@7L@f3+F#YN7mIdoh_`VAo;{B>!} zBnrV$AgCM!hqj3WQZ!(k<{-DR_%_B z0xx^mBLphjJTO7aUbCidXedVT=C`hiAp)&ehrv>IoA(6XE2D@0fSTrL2~rBwCaCUEe)B zf8i~EvkL4f(Jiugp6WG}5PY-dhP%b|AoYA#;!|&EVRTNe0NLw@A+ET-ee5T%3pLk9 z+?e0c%;)-X4Tjb+W(jACTZz=zdh-RIW2T`r4^Tf%k-=GEFo2WZliG5H2F8@zjR&`e zl2Ax7W~Nk@l>q>7g#3kw@4qfmOUTqot(A6c(a7bT5$WX}qm7Vn$h^_DwJS-}ExK?3 zjj4MVTa20%i;|9b&5=FK4dY}6Zyr;WxpwZPj1R#7F(p}G|D!pxe(0-AOj~P4Z_KT} zkhqZ(GNRxkrM$v>`hOn^ADgy%Z*n6+p>b5Fd9RLdU8SyF5u1kiY$oW!!61Z9yED~r zn(AXT)E}|_lLZ7`a5>$!OhEYFOD`08oV{8C6Sq-4vzGSla)@pglf8gMydr&T({ zUV(?hK0Ur*bQ+@ls~GXG3mA02qGk8Dlmu28D0gpbjG}|ey3X;Jx40Rx?_V_l5bAUR z-2lL?8~@?E4l(?6X5Ep;Gy)Y`xdABypUa>22j_V;pSHi{>h(e&j4{(SK1E8A@Ewz# zlUpQo8Bymx)ny@TN8%#_egVn@W=$X?hYkPxVEc1cO8oUM=H!v-sG18c?)& zQ)*>gGQoRq78j3UL3GsR+2rc;H*}w!DbM6w7-*ht$-ttcdGTi%Dnk|>B^Mb|Y#={L zg}=~FP`C#^Q2sAe$zUc^S`S+&{udvo)BNlYjA&VGZIY!$YuO=&`0VBC05Cpkrk+F* z`z$>kk}0z&F+8RhpZnH=1-aTc6ItfiIwd$i`>)-58{6u-z><6$`oh&}Y>cO(!u`b) zrJ$4w*Qeg+x7rlaFCT`ne^hUlB{IR^@WQSyGcT}U&bmdsX3A|!L*gI^*UKYMfLZck z=Syq<;$8kTPTy9ccHw`Iky=KW(I)6q!XOeQviV&!y55%7KfSa=E!f3AmtdkQ)-(Yy zUOARj^YK;nUk7zDrD+Ys`|GPO%&+wyr_Ea9ftLs`4C=wfgIF3edMbsl+sVw2e8&OZg}KqmVx$v zWwJ(ekc^Y7Ymco0c7A)d8F|Mvil5y_!PFMd37S@I2`W0k7yH*eKH)bh zYBzFKOKLC~&?#e*EAGgx_2BZu!6+U_weETvLYc60(x1PIidTK{Bb*oWBTRXaK)xvD z(x$&+4Se!f1xVqIY%XJ!^QSv(TBG(IGF~PG0H${6I?f7&tMlcOqjabnkT~2$#g;_$ zI(aY{JZ4oD#b#v?X<$`N-1VK4f`j&@2)WT$x~Lw|H;)0X;J2F1>pyhqHj-%{kP4Zs zB&t2mHxSXu|4)WNpgQ88fIy_!o>(pKq*f=}#U9o~j5Chk**75L4xn2(xI?&%fuE&F=NT#V-F3oh-NPij{=DxZB) z(-q3O(056Z;eL*|2vAe~0=+i7bLRa00N zcjZWYw5Lvn(3kfdY=!+|z~?>>g_1Pl)sQ?WYGNTyN-!9llAV3DFTosFag>TuTFxD> z$!VqypVzwkBxUL~36Y@=k(k zL^pwF@4cebkld?0N1}>;R;WL+F!yTkUn}CzpZ_#7rDx_{MVSrE%r1Bz_&euH(kuwB zTv=fnrxmhdYz{PaHYjcd!+SQ*VhE4aj3Q z`idJE#W`GN4{j4T=KV@cRo*pJeCRW)$Rf;F+STaCX_BC!yF1IKCUdC6@J^+2UlM<$ z5HjyroDaqvvK^v3kIFh~VkjEFaKKhp zziQETqE!tIn`Ir*!?kNeZ@)K!)=;|ekk;4Zb1V1#(s=k6o=t5{-hHY+tV!}J{xkYx z+S;pNKuNW60>pig_HMf(?p5(}IleQ@(QiL zGwZilj)d#eLFgZiKeCQyuqTec)K10BjFC;>Gf9aPxa&HjdURu=pQs{fhYEBrk2{ z4e3hLkJ*P0*L^oyJW&&ISv;N=o!zahj1rmf&DtRP$u>{@dtYj<0D%9zR9`86IAJ7d2ilssu~2frS-j*fk9Y%6fjl}H*W zn;N-WZhO37`d#|GX_xibKkU)jW7$XI-w<9p?4PC}B^#R)QsngK{%p$1%9*!}!e%R@ znKG#XYy4}U-s?m_0*HdKf>rIaq{`6zfa=uyUTMq^YR}bMKoT(Lz3yFm>|b$flT6N1 zp^>`4k5a#DRM>3?z&B6ys-DLaUuW?=a@76UQhM!AR$E)!7`t;BV2KO3<>!>Fy8 z2NN!@$-^-wU(9_)zPibh#Iv}*YcIyUMH{4HwQ z;>%X^{{lw`xcKrY2<~fZX*a59@ZFQwwP8iRJW$#yUJ^6LlP6Db`}S>@VzIG0_DAWY z#agX~?|yw1({UQ7DEXeVMccdFG`ox?*d-N!xXdX`}|F;CizcTRb>p{tXk zBf2m=XKX#o__;3pZhKt4K2Q&v(32<B{Fwj3x z96|?*LF%q#lqsr>GF_nv_o~++H&{N2G~Hmv2t~hqbUajb>PVd$?F^+|bx?WPtyYUg z|tk>qwUyb~g>V+aPsrBz8X!)QU&wp&?YF3d>OMp@`&8 zZOegy0SpdC?>4Qhtl-|gdoIPYhAzY4SCSz0_Te9ZufB-3>-9Pg90+GFu`LodeI9MR z>dGmDI`>0H+|;l;mDE+T$z;&wqaM?h8KaPd9ts(RB8N#RCj`S;G-B65Y|locfvqk1 zt6{#OH46UKq6;B}-FMystzCaIm^`alCdF~e$O}4RlzXfD&*OyMDdXszF?32=T7HTi z6q3*Z|L=B(r0hD#lsyD84-MQl5!-Wqejew}opU6e8_4y-zix!Cp9>2MID7W2Z!tRH z60)tabYC`*;$4er6k=9+|GIsbq;6ub3rpy}P!uAkq9S`Uh(>W_WCX**!vKKQ)m1!r z@W7X7J%c=Tb>$AQ@geL!Iy#C&hYsadcNTCb#U;!It8%krevuS?UVa}mz$j=&Kznahxcq-5~96phC#H z+-}oWUQ^{0kHt19xjLM9T(*i_d=5oMnJm=nFhQX`bRe7$o`epWSI+T}ZF%>hc;zu{ z$TB-Si|OfU3(`~o)MJ_ajhYtPFThH@V`W9by{oth?=6OyN1ctWqB zhl<2iX&i-8h4FQNAt6m3s=OyooWS__IDlyF_D7E%WtOd1k)@;P8V?^X;nk~G0J`sa zS>R&P85<6OZeMp!n(Y&}b!oD)Ida+}^)%aB5xNE;^+q|Mc;h;v4UKqRz|}Rre*GFx zpB7-P&;XIVCESi>9(ertbG&);CYQQIWLX@><#`;q=-_iaXjNVcajTB$>d#8*Chb)U zLbpY{WmwgWqcplay+G=&P*mOVjUu#K)(L7hk#((h&T_CJ>9h;mL?LtA9S~ibDXK|v zB>`0_WQr@BBqG}{3rspV73^SV_yu-{{tF-`@DI9G1DYSb1(5D_CDZf1Pi4^y?&@qV zzfqC3>|`17wpea59gu%kRads^S5?2w>*Js_gl?<%R8;axDkuf2yhXi!+u*jR?eoiu z?IUa0`p!z%`_akY0M@n9LbcSY>w9fdzD};9+dA&CeR^hCSLm+{IuE%QRcU$^q5&x5 z{j+&ZlarG;dh{rOX#D);%a=~-af$BGYq&C}P8Q0cH?X$0))_~in3%xvlBc z47qbEBWv#BII4~srM!@y^HPkVSBSt_0+PF2WSKxUPL#Hoo14SQlP3X0E5t1?FSE<= z1?kwRYjI;ooG?B~4YzmlwcWS%($W&1J$nWKn3C>kHgngeyn93RgsjEC@75U1x zIYds)la@|Bo)s0L3kX4Jv%sdp_@40Ns{XvMD@0?jl*zUF6UsNA0qs_J>f|xo_q*se zDGRcA#=0;&honv6`ppCxDnc(IvM2~;LXvHWZx@I?XrBhsR^1_zx^Hl15r;B4jyr=q zE3#f!uXQfc#>#CIIp^FwY(Pcm5`?a_6qqUxXB;Z4N5VO`43US5)R8*QO%1*K=y;mI z&=4Cro;yriPMB>=n87F47kDQ01kR6CzxU zS=;EamjyP--IllRtDQ?h=oQr9Dr(kz+}Wi*z%~A5hz^7X)3pFXChk08GHhg1_wu+8 zrZd%5zq0d8+Qim5E^I+X=oPxq3kI_y)e03Vlo1u7SLi|>Waqon;Jf|`6)M<4^(=Rz zBJ>JMl;yRtyz2LTT~?6OC6uDAP))G(4N7?3MLnV7|L*eUj2)2>mH+?%07*qoM6N<$ Ef~CQ6NdN!< literal 0 HcmV?d00001 diff --git a/examples/ROS/Configs/ros_example.yml b/examples/ROS/Configs/ros_example.yml new file mode 100644 index 0000000..34d605a --- /dev/null +++ b/examples/ROS/Configs/ros_example.yml @@ -0,0 +1,39 @@ +Planner_config: + experiment_info_default: + resolution: 2.0 + epsilon: [2.0, 2.0, 1.57, 5.0] # ate, cte, heading, vel + tolerance: 0.125 + max_level: 5 + division_factor: 2 # Example division factor + max_expansions: 500 + hysteresis: 100 + node_info: + node_type: "kinodynamic" + length: 0.3 + width: 0.25 + map_res: 0.25 + dt: 0.05 + timesteps: 5 + step_size: 1.0 + steering_list: [-15.0, -5.0, 0.0, 5.0, 15.0] # in degrees. + throttle_list: [-4.0, -2.0, 0, 2.0, 4.0] + del_theta: 90.0 # in degrees + del_vel: 1.0 + RI: 0.5 + max_vert_acc: 5.0 + min_vel: 0.0 # don't go in reverse. + max_vel: 2.0 + max_theta: 30.0 # in degrees + gear_switch_time: 0.0 +topics: + elevation_map: "/elevation_mapping/elevation_map_cropped_cv2" + odom: "/mavros/local_position/odom" + waypoints: "/mavros/mission/waypoints" + path: "/planner_test/path" + diagnostics: "/planner_test/diagnostic" + goal_marker: "/planner_test/goal_marker" + global_position: "/mavros/global_position/global" + +blur_kernel: 0 +lethal_slope: 60 +wp_radius: 2.0 \ No newline at end of file diff --git a/examples/ROS/README.md b/examples/ROS/README.md new file mode 100644 index 0000000..d1f7b5f --- /dev/null +++ b/examples/ROS/README.md @@ -0,0 +1,70 @@ +# IGHAStar ROS Example + +This folder contains a ROS-based example for the IGHA* path planner. The code here is really an example script to demonstrate how one would use the planner in the loop on their robot. +It demonstrates how to integrate the IGHA* planner with ROS topics for maps, odometry, waypoints, and visualization. +For simplicity (not needing to fiddle with rviz), there is a built in visualization in the example. + +## TODO: does one need to install all grid-map stuff to get grid-map visuals? Might be useful to get rviz visualizations here. + + +## Prerequisites +- ROS Noetic +- Python 3 +- [catkin_tools](https://catkin-tools.readthedocs.io/en/latest/) +- [PyTorch](https://pytorch.org/) +- OpenCV (`cv2`) +- numpy, yaml +- IGHAStar C++/CUDA requirements (requirements.txt) +- ROS messages: `nav_msgs`, `geometry_msgs`, `mavros_msgs`, `grid_map_msgs`, `visualization_msgs`, `diagnostic_msgs` + +## Setup (confirm these instructions) +1. **Clone this repository** into your catkin workspace: + ```bash + cd ~/catkin_ws/src + git clone + cd ~/catkin_ws + catkin_make + source devel/setup.bash + ``` +2. **Install Python dependencies:** + ```bash + pip install torch opencv-python numpy pyyaml + ``` +3. **Check ROS dependencies:** + ```bash + rosdep install --from-paths src --ignore-src -r -y + ``` + +## Configuration +- Edit `Configs/ros_example.yml` to set planner parameters, topic names, and costmap settings. + +## Running the Example + + +
+ ROS Example Visualization +
Fig. 1: Example ROS planner visualization. The window shows the costmap (pink/white), elevation map (grayscale), the planned path (green), the goal (white circle), and the robot state (black rectangle).
+
+ +We provide rosbags that you can use to run the example script. Place them in the `rosbags` folder. Download here: [rosbag 1](https://drive.google.com/file/d/1zV0f3NbPuyewwbHUlrcuboj-PMrPixwG/view?usp=sharing), [rosbag 2](https://drive.google.com/file/d/1BPsypv83_W5EtcodyV2W75BSK3rVE3mX/view?usp=sharing) + +1. **Start ROS core:** + ```bash + roscore + ``` +2. **Run the planner node:** + ```bash + cd examples/ROS + python3 example.py + ``` + Or launch with rosrun/roslaunch if you have a package setup. +3. **Play a rosbag or run your robot simulation** to publish the required topics. + ``` + cd examples/ROS/rosbags/ + rosbag play hound_95.bag + ``` + +## Visualization +- The planner will open an OpenCV window showing the costmap, elevation map, path, goal, and robot state. +- Path and goal are also published as ROS topics for use in RViz. + diff --git a/examples/ROS/example.py b/examples/ROS/example.py new file mode 100644 index 0000000..480cb58 --- /dev/null +++ b/examples/ROS/example.py @@ -0,0 +1,144 @@ +#!/usr/bin/env python3 +""" +ROS wrapper for IGHA* planner (single-class version). +- Can be used live (with ROS subscribers) or offline (by calling callbacks with rosbag messages). +- Maintains internal state and publishes planned paths. +- Clean, modern, and easy to adapt for both online and offline use. +""" +import rospy +import numpy as np +import torch +import yaml +from nav_msgs.msg import Path, Odometry +from mavros_msgs.msg import WaypointList +from grid_map_msgs.msg import GridMap +from visualization_msgs.msg import MarkerArray +from utils import * +import cv2 +import time +import os + +class PlannerNode: + """ + ROS node wrapper for IGHAStar planner. + Handles all ROS communication, map/state/waypoint management, and planning logic. + Can be used live or with rosbag replay by calling the callbacks directly. + """ + def __init__(self, config): + # --- Planner config --- + planner_cfg = config["Planner_config"] + self.map_res = planner_cfg["experiment_info_default"]["node_info"]["map_res"] + self.expansion_limit = planner_cfg["experiment_info_default"]["max_expansions"] + self.hysteresis = planner_cfg["experiment_info_default"]["hysteresis"] + self.planner = create_planner(planner_cfg) + # --- Costmap parameters --- + self.blur_kernel = config["blur_kernel"] + self.costmap_cosine_thresh = np.cos(np.radians(config["lethal_slope"])) + self.wp_radius = config["wp_radius"] + # --- IO variables --- + self.state = np.zeros(9) + self.local_waypoints = None + self.global_pos = None + self.map_init = False + self.height_index = None + self.bitmap = None + self.offset = None + self.map_center = None + self.map_size_px = None + # --- ROS publishers --- + topics = config["topics"] + self.path_pub = rospy.Publisher(topics["path"], Path, queue_size=1, latch=True) + self.marker_pub = rospy.Publisher(topics["goal_marker"], MarkerArray, queue_size=1) + self.diagnostics_pub = rospy.Publisher(topics["diagnostics"], DiagnosticArray, queue_size=1) + # --- ROS subscribers --- + rospy.Subscriber(topics["elevation_map"], GridMap, self.map_callback, queue_size=1) + rospy.Subscriber(topics["odom"], Odometry, self.odom_callback, queue_size=1) + rospy.Subscriber(topics["waypoints"], WaypointList, self.waypoint_callback, queue_size=1) + rospy.Subscriber(topics["global_position"], NavSatFix, self.global_pos_callback, queue_size=1) + self.plan_loop() + + def map_callback(self, grid_map): + self.grid_map = grid_map + if self.height_index is None or self.layers is None: + self.layers = self.grid_map.layers + self.height_index = self.layers.index("elevation") + cent = self.grid_map.info.pose.position + self.map_center = np.array([cent.x, cent.y, cent.z]) + matrix = self.grid_map.data[self.height_index] + self.heightmap = map_from_gridmap(matrix) + if np.any(np.isnan(self.heightmap)): + self.map_init = False + else: + self.bitmap, self.offset = process_grid_map(self.heightmap, lethalmap=None, map_res=self.map_res, blur_kernel=self.blur_kernel, costmap_cosine_thresh=self.costmap_cosine_thresh) + self.map_init = True + + def plan(self, start_state, goal_, stop=False): + if self.bitmap is None: + return None, False, 0, 0.0, goal_ + bitmap = torch.clone(self.bitmap) + start, goal = start_goal_logic(bitmap, self.map_res, start_state, goal_, self.map_center, self.offset, stop=stop) + now = time.perf_counter() + success = self.planner.search(start, goal, bitmap, self.expansion_limit, self.hysteresis, True) + time_taken = time.perf_counter() - now + avg_successor_time, avg_goal_check_time, avg_overhead_time, avg_g_update_time, switches, max_level_profile, Q_v_size, expansion_counter, expansion_list= self.planner.get_profiler_info() + output_goal = goal[:2] - (self.offset - self.map_center[:2]) + if success: + path = self.planner.get_best_path().numpy() + path = np.flip(path, axis=0) + path[..., :2] -= self.offset + path[..., :2] += self.map_center[:2] + return path, True, expansion_counter, time_taken, output_goal + else: + return None, False, expansion_counter, time_taken, output_goal + + def odom_callback(self, msg): + """Update local position from Odometry message.""" + self.state = obtain_state(msg, self.state) + + def global_pos_callback(self, msg): + self.global_pos = msg + + def waypoint_callback(self, msg): + """Update local waypoints from WaypointList message.""" + print("Got waypoints") + self.waypoint_list = msg.waypoints + while self.global_pos is None or self.state is None: + time.sleep(1) + gps_origin = calcposLLH(self.global_pos.latitude, self.global_pos.longitude, -self.state[0], -self.state[1]) + self.local_waypoints = get_local_frame_waypoints(self.waypoint_list, gps_origin) + + def plan_loop(self): + rate = rospy.Rate(20) + while not rospy.is_shutdown(): + rate.sleep() + if self.local_waypoints is not None and self.state is not None and self.bitmap is not None and len(self.local_waypoints): + self.local_waypoints = np.array(self.local_waypoints) + plan_state = np.array([self.state[0], self.state[1], self.state[5], np.linalg.norm(self.state[6:8])]) + goal = self.local_waypoints[0] + path, success, expansions, time_taken, output_goal = self.plan(plan_state, goal, stop=len(self.local_waypoints) == 0) + expansions_per_second = max(expansions/time_taken, 1000) + self.expansion_limit = int(expansions_per_second * 0.5) + if success: + publish_path(path, self.path_pub) + publish_goal(output_goal, self.marker_pub) + # Visualize map with path + + diagnostic_publisher(success, expansions, time_taken, self.hysteresis, self.diagnostics_pub) + display_map = visualize_map_with_path(self.bitmap[..., 0].numpy(), self.bitmap[..., 1].numpy(), path, output_goal, plan_state, self.wp_radius, self.map_center, 480, 7.5/self.map_res) + cv2.imshow("planner_vis", display_map) + cv2.waitKey(1) + # reduce length of local waypoints: + dist = np.linalg.norm(self.local_waypoints[0][:2] - plan_state[:2]) + if dist < self.wp_radius: + if len(self.local_waypoints) > 1: + self.local_waypoints = self.local_waypoints[1:] + + +if __name__ == "__main__": + rospy.init_node("ighastar_planner_node") + config_name = "ros_example.yml" + config_path = os.path.join(os.path.dirname(__file__), "Configs", config_name) + with open(config_path) as f: + Config = yaml.safe_load(f) + node = PlannerNode(Config) + rospy.spin() \ No newline at end of file diff --git a/examples/ROS/requirements.txt b/examples/ROS/requirements.txt new file mode 100644 index 0000000..d3860a6 --- /dev/null +++ b/examples/ROS/requirements.txt @@ -0,0 +1,4 @@ +torch +opencv-python +numpy +pyyaml \ No newline at end of file diff --git a/examples/ROS/utils.py b/examples/ROS/utils.py new file mode 100644 index 0000000..7248b03 --- /dev/null +++ b/examples/ROS/utils.py @@ -0,0 +1,386 @@ +import numpy as np +import cv2 +import torch +from tf.transformations import euler_from_quaternion, quaternion_from_euler +from torch.utils.cpp_extension import load +from nav_msgs.msg import Path +import rospy +from geometry_msgs.msg import PoseStamped +from visualization_msgs.msg import Marker, MarkerArray +from grid_map_msgs.msg import GridMap +from geometry_msgs.msg import Quaternion +from sensor_msgs.msg import NavSatFix +from mavros_msgs.msg import WaypointList +from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue +from nav_msgs.msg import Odometry +import os +import time +import threading + +def find_ighastar_base_dir(): + # Start from the current file's directory and walk up until 'src/ighastar.cpp' is found + cur_dir = os.path.abspath(os.path.dirname(__file__)) + while True: + candidate = os.path.join(cur_dir, 'src', 'ighastar.cpp') + if os.path.isfile(candidate): + return cur_dir + parent = os.path.dirname(cur_dir) + if parent == cur_dir: + raise RuntimeError("Could not find ighastar base directory (src/ighastar.cpp not found)") + cur_dir = parent + +def create_planner(configs, Debug=False): + env_name = configs["experiment_info_default"]["node_info"]["node_type"] + BASE_DIR = find_ighastar_base_dir() + cuda_available = torch.cuda.is_available() + + if not cuda_available: + print("CUDA not available, using CPU versions") + env_macro = { + 'simple': '-DUSE_SIMPLE_ENV', + 'kinematic': '-DUSE_KINEMATIC_CPU_ENV', + 'kinodynamic': '-DUSE_KINODYNAMIC_CPU_ENV', + }[env_name] + else: + print("CUDA available, using GPU versions") + env_macro = { + 'simple': '-DUSE_SIMPLE_ENV', + 'kinematic': '-DUSE_KINEMATIC_ENV', + 'kinodynamic': '-DUSE_KINODYNAMIC_ENV', + }[env_name] + + cpp_path = os.path.join(BASE_DIR, 'src/ighastar.cpp') + header_path = os.path.join(BASE_DIR, 'src/Environments') + + if env_name != "simple": + if cuda_available: + cuda_path = os.path.join(BASE_DIR, f'src/Environments/{env_name}.cu') + kernel = load( + name="ighastar", + sources=[cpp_path, cuda_path], + extra_include_paths=[header_path], + extra_cflags=['-std=c++17', '-O3', env_macro], + extra_cuda_cflags=['-O3'], + verbose=True, + ) + else: + cpu_cpp_path = os.path.join(BASE_DIR, f'src/Environments/{env_name}_cpu.cpp') + kernel = load( + name="ighastar", + sources=[cpp_path, cpu_cpp_path], + extra_include_paths=[header_path], + extra_cflags=['-std=c++17', '-O3', env_macro], + verbose=True, + ) + else: + kernel = load( + name="ighastar", + sources=[cpp_path], + extra_include_paths=[header_path], + extra_cflags=['-std=c++17', '-O3', env_macro], + verbose=True, + ) + planner = kernel.IGHAStar(configs, Debug) + print("planner loaded") + return planner + +def generate_normal( elev, k=3): + dzdx = -cv2.Sobel(elev, cv2.CV_32F, 1, 0, ksize=k) + dzdy = -cv2.Sobel(elev, cv2.CV_32F, 0, 1, ksize=k) + dzdz = np.ones_like(elev) + normal = np.stack((dzdx, dzdy, dzdz), axis=2) + norm = np.linalg.norm(normal, axis=2, keepdims=True) + normal = normal / norm + return normal + +def map_from_gridmap(matrix): + return np.float32( + cv2.flip( + np.reshape( + matrix.data, + (matrix.layout.dim[1].size, matrix.layout.dim[0].size), + order="F", + ).T, + -1, + ) + ) + + +def generate_costmap_from_BEVmap( + BEV_lethal, BEV_normal, costmap_cosine_thresh=np.cos(np.radians(60)) +): + """Generate a costmap from BEV lethal and normal maps.""" + dot_product = BEV_normal[:, :, 2] + angle_cost = np.where(dot_product >= costmap_cosine_thresh, 255, 0).astype( + np.float32 + ) + costmap = 255.0 * (1 - BEV_lethal[..., 0]) + costmap = np.minimum(costmap, angle_cost) + costmap[costmap < 255.0] = 0.0 + return costmap + + +def process_grid_map(heightmap, lethalmap=None, map_res=0.1, blur_kernel=0, costmap_cosine_thresh=np.cos(np.radians(60))): + if lethalmap is None: + lethalmap = np.zeros_like(heightmap, dtype=np.float32) + normal = generate_normal(heightmap) + costmap = generate_costmap_from_BEVmap(lethalmap, normal, costmap_cosine_thresh) + if blur_kernel != 0: + costmap = cv2.GaussianBlur(costmap, (blur_kernel, blur_kernel), 0) + costmap[costmap < 255.0] = 0.0 + bitmap = torch.ones((heightmap.shape[0], heightmap.shape[1], 2), dtype=torch.float32) + bitmap[..., 0] = torch.from_numpy(costmap) + bitmap[..., 1] = torch.from_numpy(heightmap) + offset = map_res * np.array(bitmap.shape[:2]) * 0.5 + return bitmap, offset + +def clip_goal_to_map(bitmap, map_res=0.25, start=None, goal=None, num_samples=200): + H, W = bitmap.shape[:2] + H *= map_res + W *= map_res + + x0, y0 = start[:2] + x1, y1 = goal[:2] + + # Generate t in [0, 1] + t_vals = torch.linspace(0, 1, num_samples, device=bitmap.device) + + # Line points: start + t * (goal - start) + dx = x1 - x0 + dy = y1 - y0 + xs = x0 + t_vals * dx + ys = y0 + t_vals * dy + + # Check which are inside map bounds + valid = (xs >= 0) & (xs < W) & (ys >= 0) & (ys < H) + if torch.any(valid): + last_valid_idx = torch.where(valid)[0][-1] + goal[0] = xs[last_valid_idx] + goal[1] = ys[last_valid_idx] + else: + # All points are out of bounds; snap to start + goal[0] = x0 + goal[1] = y0 + return goal + +def obtain_state(odom, state): + ## obtain the state from the odometry and imu messages: + quaternion = ( + odom.pose.pose.orientation.x, + odom.pose.pose.orientation.y, + odom.pose.pose.orientation.z, + odom.pose.pose.orientation.w, + ) + rpy = euler_from_quaternion(quaternion) + state[0] = odom.pose.pose.position.x + state[1] = odom.pose.pose.position.y + state[2] = odom.pose.pose.position.z + state[3] = rpy[0] + state[4] = rpy[1] + state[5] = rpy[2] + state[6] = odom.twist.twist.linear.x + state[7] = odom.twist.twist.linear.y + state[8] = odom.twist.twist.linear.z + return state + +def publish_goal(goal, marker_pub): + marker_array = MarkerArray() + marker = Marker() + marker.header.frame_id = "odom" + marker.id = 0 + marker.type = marker.SPHERE + marker.action = marker.ADD + marker.scale.x = 4.0 + marker.scale.y = 4.0 + marker.scale.z = 0.1 + marker.color.a = 1.0 + marker.color.r = 1.0 + marker.color.g = 0.0 + marker.color.b = 0.0 + marker.pose.orientation.w = 1.0 + marker.pose.position.x = goal[0] + marker.pose.position.y = goal[1] + marker.pose.position.z = 1 + marker_array.markers.append(marker) + marker_pub.publish(marker_array) + +def publish_path(path, path_publisher): + ros_path = Path() + ros_path.header.stamp = rospy.Time.now() + ros_path.header.frame_id = "map" # or whatever your fixed frame is + + for waypoint in path: + pose = PoseStamped() + pose.header.stamp = rospy.Time.now() + pose.header.frame_id = "map" + + pose.pose.position.x = waypoint[0] + pose.pose.position.y = waypoint[1] + pose.pose.position.z = 1 # would be nice to get the extended state here... + + # Convert theta to quaternion + quat = quaternion_from_euler(0, 0, waypoint[2]) + pose.pose.orientation = Quaternion(*quat) + + ros_path.poses.append(pose) + + path_publisher.publish(ros_path) + +def diagnostic_publisher(status, expansion_counter, time_taken, hysteresis, diagnostics_pub): + diagnostics_array = DiagnosticArray() + diagnostics_status = DiagnosticStatus() + diagnostics_status.name = "planner" + diagnostics_status.level = status + diagnostics_status.values.append(KeyValue(key="Expansion Count", value=str(expansion_counter))) + diagnostics_status.values.append(KeyValue(key="Time Taken (s)", value=f"{time_taken:.4f}")) + diagnostics_status.values.append(KeyValue(key="Expansions/second", value=f"{expansion_counter/time_taken:.4f}")) + diagnostics_status.values.append(KeyValue(key="Hysteresis", value=f"{hysteresis}")) + + diagnostics_array.status.append(diagnostics_status) + diagnostics_pub.publish(diagnostics_array) + +def set_headings(local_waypoints): + target_Vhat = np.zeros_like(local_waypoints) # 0 out everything + for i in range(1, len(local_waypoints) - 1): # all points except first and last + V_prev = local_waypoints[i] - local_waypoints[i - 1] + V_next = local_waypoints[i + 1] - local_waypoints[i] + target_Vhat[i] = (V_next + V_prev) / np.linalg.norm(V_next + V_prev) + target_Vhat[0] = local_waypoints[1] - local_waypoints[0] + target_Vhat[0] /= np.linalg.norm(target_Vhat[0]) + target_Vhat[-1] = local_waypoints[-1] - local_waypoints[-2] + target_Vhat[-1] /= np.linalg.norm(target_Vhat[-1]) + + waypoints = np.zeros((len(local_waypoints), 4)) + waypoints[:, :2] = local_waypoints[:,:2] + waypoints[:, 2] = np.arctan2(target_Vhat[:, 1], target_Vhat[:, 0]) + waypoints[:, 3] = 2.0 + return waypoints + +earthRadius = 6378145.0 +DEG_TO_RAD = 0.01745329252 +RAD_TO_DEG = 57.2957795131 + +def calcposLLH( lat, lon, dX, dY): + lat += dY / (earthRadius * DEG_TO_RAD) + lon += dX / (earthRadius * np.cos(lat * DEG_TO_RAD) * DEG_TO_RAD) + return lat, lon + +def calcposNED( lat, lon, latReference, lonReference): + Y = earthRadius * (lat - latReference) * DEG_TO_RAD + X = ( + earthRadius + * np.cos(latReference * DEG_TO_RAD) + * (lon - lonReference) + * DEG_TO_RAD + ) + return X, Y + +def start_goal_logic(bitmap, map_res, start_state, goal, map_center, offset, stop=False): + start = np.zeros(4, dtype=np.float32) + goal_ = np.zeros(4, dtype=np.float32) + start[:2] = start_state[:2] + offset - map_center[:2] + goal_[:2] = goal[:2] + offset - map_center[:2] + goal_ = clip_goal_to_map(bitmap, map_res=map_res, start=start, goal=goal_) + start[2] = start_state[2] + start[3] = start_state[3] + goal_[2] = goal[2] + goal_[3] = 0.0 if stop else 2.0 + return torch.from_numpy(start).to(dtype=torch.float32), torch.from_numpy(goal_).to(dtype=torch.float32) + +def get_local_frame_waypoints(waypoint_list, gps_origin): + local_waypoints = [] + for waypoint in waypoint_list: + if waypoint.frame != 3: + continue + lat = waypoint.x_lat + lon = waypoint.y_long + # generate X,Y locations using calcposNED and append to path + X, Y = calcposNED(lat, lon, gps_origin[0], gps_origin[1]) + local_waypoints.append(np.array([X, Y, 0])) + local_waypoints = set_headings(np.array(local_waypoints)) + return local_waypoints + +def visualize_map_with_path(costmap, elevation_map, path, goal, state, wp_radius, map_center, map_size, resolution_inv): + """ + Visualize the costmap and elevation map with the path, goal, and optionally the current state. + Args: + costmap: 2D numpy array (uint8) + elevation_map: 2D numpy array (float) + path: Nx4 or Nx2 numpy array (path points) + goal: (2,) or (4,) array-like (goal position) + state: (N,) array-like, current state (x, y, theta, ...) + wp_radius: float, waypoint radius (for goal circle) + map_center: (2,) or (3,) array-like, map center offset + map_size: int, output image size in pixels + resolution_inv: float, 1.0 / map resolution + Returns: + costmap_img: RGB image (np.uint8) for display with OpenCV + """ + # Resize maps to output size + costmap = cv2.resize(costmap, (map_size, map_size)) + elevation_map = cv2.resize(elevation_map, (map_size, map_size)) + + # Colorize costmap: white for 255, pink for others + costmap_color = np.clip(costmap, 0, 255).astype(np.uint8) + pink = np.array([255, 105, 180], dtype=np.uint8) + white = np.array([255, 255, 255], dtype=np.uint8) + color_map = np.zeros((map_size, map_size, 3), dtype=np.uint8) + mask_white = costmap_color == 255 + mask_pink = ~mask_white + color_map[mask_white] = white + color_map[mask_pink] = pink + + # Colorize elevation map and blend with costmap (where costmap is white) + elev_norm = np.clip((elevation_map + 4) / 8, 0, 1) + elev_uint8 = (elev_norm * 255).astype(np.uint8) + elev_color = np.stack([elev_uint8] * 3, axis=-1) + display_img = color_map.copy() + display_img[mask_white] = elev_color[mask_white] + + # Draw goal (convert to local map frame) + goal_disp = np.array(goal[:2]) - np.array(map_center[:2]) + goal_x = int(np.clip((goal_disp[0] * resolution_inv) + map_size // 2, 0, map_size - 1)) + goal_y = int(np.clip((goal_disp[1] * resolution_inv) + map_size // 2, 0, map_size - 1)) + radius = max(2, int(wp_radius * resolution_inv)) + cv2.circle(display_img, (goal_x, goal_y), radius, (255, 255, 255), 2) + + # Draw path + if path is not None and len(path) > 0: + path_disp = np.copy(path) + path_disp[..., :2] -= np.array(map_center[:2]) + path_X = np.clip((path_disp[..., 0] * resolution_inv) + map_size // 2, 0, map_size - 1).astype(int) + path_Y = np.clip((path_disp[..., 1] * resolution_inv) + map_size // 2, 0, map_size - 1).astype(int) + car_width_px = max(1, int(0.15 * resolution_inv)) + if path.shape[1] > 3: + velocity = path[..., 3] + velocity_norm = (velocity - np.min(velocity)) / (np.max(velocity) - np.min(velocity) + 1e-6) + velocity_color = np.clip((velocity_norm * 255), 0, 255).astype(int) + else: + velocity_color = np.full(len(path_X), 128, dtype=int) + for i in range(len(path_X) - 1): + cv2.line(display_img, (path_X[i], path_Y[i]), (path_X[i + 1], path_Y[i + 1]), (0, int(velocity_color[i]), 0), car_width_px) + + # Draw current state as a rectangle (if provided) + if state is not None and len(state) >= 3: + x = state[0] - map_center[0] + y = state[1] - map_center[1] + theta = np.pi - state[2] + x_px = int(x * resolution_inv + map_size // 2) + y_px = int(y * resolution_inv + map_size // 2) + car_width_px = max(2, int(0.29 * resolution_inv)) + car_height_px = max(2, int(0.15 * resolution_inv)) + half_width = car_width_px // 2 + half_height = car_height_px // 2 + corners = np.array([ + [x_px - half_width, y_px - half_height], + [x_px + half_width, y_px - half_height], + [x_px + half_width, y_px + half_height], + [x_px - half_width, y_px + half_height] + ], dtype=np.int32) + rotation_matrix = cv2.getRotationMatrix2D((x_px, y_px), np.degrees(theta), 1.0) + rotated_corners = cv2.transform(np.array([corners]), rotation_matrix)[0] + cv2.polylines(display_img, [rotated_corners], isClosed=True, color=(0, 0, 0), thickness=2) + + # Flip for visualization + display_img = cv2.flip(display_img, 0) + return display_img \ No newline at end of file From 804f8dd489250b28e706c29fc2fba15955593084 Mon Sep 17 00:00:00 2001 From: sidtalia Date: Wed, 16 Jul 2025 20:50:06 -0700 Subject: [PATCH 2/8] rebased from main --- examples/ROS/example.py | 1 + examples/ROS/utils.py | 57 ----------------------------------------- 2 files changed, 1 insertion(+), 57 deletions(-) diff --git a/examples/ROS/example.py b/examples/ROS/example.py index 480cb58..b2ae106 100644 --- a/examples/ROS/example.py +++ b/examples/ROS/example.py @@ -17,6 +17,7 @@ import cv2 import time import os +from ighastar.scripts.common_utils import create_planner class PlannerNode: """ diff --git a/examples/ROS/utils.py b/examples/ROS/utils.py index 7248b03..ad3ec77 100644 --- a/examples/ROS/utils.py +++ b/examples/ROS/utils.py @@ -14,8 +14,6 @@ from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue from nav_msgs.msg import Odometry import os -import time -import threading def find_ighastar_base_dir(): # Start from the current file's directory and walk up until 'src/ighastar.cpp' is found @@ -28,61 +26,6 @@ def find_ighastar_base_dir(): if parent == cur_dir: raise RuntimeError("Could not find ighastar base directory (src/ighastar.cpp not found)") cur_dir = parent - -def create_planner(configs, Debug=False): - env_name = configs["experiment_info_default"]["node_info"]["node_type"] - BASE_DIR = find_ighastar_base_dir() - cuda_available = torch.cuda.is_available() - - if not cuda_available: - print("CUDA not available, using CPU versions") - env_macro = { - 'simple': '-DUSE_SIMPLE_ENV', - 'kinematic': '-DUSE_KINEMATIC_CPU_ENV', - 'kinodynamic': '-DUSE_KINODYNAMIC_CPU_ENV', - }[env_name] - else: - print("CUDA available, using GPU versions") - env_macro = { - 'simple': '-DUSE_SIMPLE_ENV', - 'kinematic': '-DUSE_KINEMATIC_ENV', - 'kinodynamic': '-DUSE_KINODYNAMIC_ENV', - }[env_name] - - cpp_path = os.path.join(BASE_DIR, 'src/ighastar.cpp') - header_path = os.path.join(BASE_DIR, 'src/Environments') - - if env_name != "simple": - if cuda_available: - cuda_path = os.path.join(BASE_DIR, f'src/Environments/{env_name}.cu') - kernel = load( - name="ighastar", - sources=[cpp_path, cuda_path], - extra_include_paths=[header_path], - extra_cflags=['-std=c++17', '-O3', env_macro], - extra_cuda_cflags=['-O3'], - verbose=True, - ) - else: - cpu_cpp_path = os.path.join(BASE_DIR, f'src/Environments/{env_name}_cpu.cpp') - kernel = load( - name="ighastar", - sources=[cpp_path, cpu_cpp_path], - extra_include_paths=[header_path], - extra_cflags=['-std=c++17', '-O3', env_macro], - verbose=True, - ) - else: - kernel = load( - name="ighastar", - sources=[cpp_path], - extra_include_paths=[header_path], - extra_cflags=['-std=c++17', '-O3', env_macro], - verbose=True, - ) - planner = kernel.IGHAStar(configs, Debug) - print("planner loaded") - return planner def generate_normal( elev, k=3): dzdx = -cv2.Sobel(elev, cv2.CV_32F, 1, 0, ksize=k) From c20e17410989f86f8bce96d66f02cfabdfdb267d Mon Sep 17 00:00:00 2001 From: sidtalia Date: Wed, 16 Jul 2025 20:54:52 -0700 Subject: [PATCH 3/8] removed unnecessary function --- examples/ROS/utils.py | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/examples/ROS/utils.py b/examples/ROS/utils.py index ad3ec77..d2b4e11 100644 --- a/examples/ROS/utils.py +++ b/examples/ROS/utils.py @@ -15,18 +15,6 @@ from nav_msgs.msg import Odometry import os -def find_ighastar_base_dir(): - # Start from the current file's directory and walk up until 'src/ighastar.cpp' is found - cur_dir = os.path.abspath(os.path.dirname(__file__)) - while True: - candidate = os.path.join(cur_dir, 'src', 'ighastar.cpp') - if os.path.isfile(candidate): - return cur_dir - parent = os.path.dirname(cur_dir) - if parent == cur_dir: - raise RuntimeError("Could not find ighastar base directory (src/ighastar.cpp not found)") - cur_dir = parent - def generate_normal( elev, k=3): dzdx = -cv2.Sobel(elev, cv2.CV_32F, 1, 0, ksize=k) dzdy = -cv2.Sobel(elev, cv2.CV_32F, 0, 1, ksize=k) From 71456af2ef17c942fbe3a06a3647efaf719035a2 Mon Sep 17 00:00:00 2001 From: sidtalia Date: Thu, 17 Jul 2025 16:39:53 -0700 Subject: [PATCH 4/8] updated gitignore --- .gitignore | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.gitignore b/.gitignore index f90a9cf..ddb36bd 100644 --- a/.gitignore +++ b/.gitignore @@ -13,6 +13,8 @@ ighastar/scripts/test_case_generation.py examples/ros_kinodynamic_example.py examples/ROS/ examples/BeamNG/ +ighastar/src/IGHAStarClass.py +ighastar/src/Nodes.py # Python __pycache__/ *.py[cod] From fd0bd0d2943a551838f079e659199669ebf4dffb Mon Sep 17 00:00:00 2001 From: sidtalia Date: Thu, 17 Jul 2025 16:51:29 -0700 Subject: [PATCH 5/8] rebase main, updated typing --- examples/ROS/example.py | 128 ++++++++++++++++++++++++------- examples/ROS/utils.py | 166 ++++++++++++++++++++++++++++++---------- 2 files changed, 229 insertions(+), 65 deletions(-) diff --git a/examples/ROS/example.py b/examples/ROS/example.py index b2ae106..8589b5e 100644 --- a/examples/ROS/example.py +++ b/examples/ROS/example.py @@ -18,6 +18,8 @@ import time import os from ighastar.scripts.common_utils import create_planner +from typing import Any, Optional, Tuple + class PlannerNode: """ @@ -25,7 +27,8 @@ class PlannerNode: Handles all ROS communication, map/state/waypoint management, and planning logic. Can be used live or with rosbag replay by calling the callbacks directly. """ - def __init__(self, config): + + def __init__(self, config: dict) -> None: # --- Planner config --- planner_cfg = config["Planner_config"] self.map_res = planner_cfg["experiment_info_default"]["node_info"]["map_res"] @@ -49,16 +52,26 @@ def __init__(self, config): # --- ROS publishers --- topics = config["topics"] self.path_pub = rospy.Publisher(topics["path"], Path, queue_size=1, latch=True) - self.marker_pub = rospy.Publisher(topics["goal_marker"], MarkerArray, queue_size=1) - self.diagnostics_pub = rospy.Publisher(topics["diagnostics"], DiagnosticArray, queue_size=1) + self.marker_pub = rospy.Publisher( + topics["goal_marker"], MarkerArray, queue_size=1 + ) + self.diagnostics_pub = rospy.Publisher( + topics["diagnostics"], DiagnosticArray, queue_size=1 + ) # --- ROS subscribers --- - rospy.Subscriber(topics["elevation_map"], GridMap, self.map_callback, queue_size=1) + rospy.Subscriber( + topics["elevation_map"], GridMap, self.map_callback, queue_size=1 + ) rospy.Subscriber(topics["odom"], Odometry, self.odom_callback, queue_size=1) - rospy.Subscriber(topics["waypoints"], WaypointList, self.waypoint_callback, queue_size=1) - rospy.Subscriber(topics["global_position"], NavSatFix, self.global_pos_callback, queue_size=1) + rospy.Subscriber( + topics["waypoints"], WaypointList, self.waypoint_callback, queue_size=1 + ) + rospy.Subscriber( + topics["global_position"], NavSatFix, self.global_pos_callback, queue_size=1 + ) self.plan_loop() - def map_callback(self, grid_map): + def map_callback(self, grid_map: GridMap) -> None: self.grid_map = grid_map if self.height_index is None or self.layers is None: self.layers = self.grid_map.layers @@ -70,18 +83,46 @@ def map_callback(self, grid_map): if np.any(np.isnan(self.heightmap)): self.map_init = False else: - self.bitmap, self.offset = process_grid_map(self.heightmap, lethalmap=None, map_res=self.map_res, blur_kernel=self.blur_kernel, costmap_cosine_thresh=self.costmap_cosine_thresh) + self.bitmap, self.offset = process_grid_map( + self.heightmap, + lethalmap=None, + map_res=self.map_res, + blur_kernel=self.blur_kernel, + costmap_cosine_thresh=self.costmap_cosine_thresh, + ) self.map_init = True - def plan(self, start_state, goal_, stop=False): + def plan( + self, start_state: np.ndarray, goal_: np.ndarray, stop: bool = False + ) -> Tuple[Optional[np.ndarray], bool, int, float, np.ndarray]: if self.bitmap is None: return None, False, 0, 0.0, goal_ bitmap = torch.clone(self.bitmap) - start, goal = start_goal_logic(bitmap, self.map_res, start_state, goal_, self.map_center, self.offset, stop=stop) + start, goal = start_goal_logic( + bitmap, + self.map_res, + start_state, + goal_, + self.map_center, + self.offset, + stop=stop, + ) now = time.perf_counter() - success = self.planner.search(start, goal, bitmap, self.expansion_limit, self.hysteresis, True) + success = self.planner.search( + start, goal, bitmap, self.expansion_limit, self.hysteresis, True + ) time_taken = time.perf_counter() - now - avg_successor_time, avg_goal_check_time, avg_overhead_time, avg_g_update_time, switches, max_level_profile, Q_v_size, expansion_counter, expansion_list= self.planner.get_profiler_info() + ( + avg_successor_time, + avg_goal_check_time, + avg_overhead_time, + avg_g_update_time, + switches, + max_level_profile, + Q_v_size, + expansion_counter, + expansion_list, + ) = self.planner.get_profiler_info() output_goal = goal[:2] - (self.offset - self.map_center[:2]) if success: path = self.planner.get_best_path().numpy() @@ -92,40 +133,75 @@ def plan(self, start_state, goal_, stop=False): else: return None, False, expansion_counter, time_taken, output_goal - def odom_callback(self, msg): + def odom_callback(self, msg: Odometry) -> None: """Update local position from Odometry message.""" self.state = obtain_state(msg, self.state) - - def global_pos_callback(self, msg): + + def global_pos_callback(self, msg: Any) -> None: self.global_pos = msg - def waypoint_callback(self, msg): + def waypoint_callback(self, msg: WaypointList) -> None: """Update local waypoints from WaypointList message.""" print("Got waypoints") self.waypoint_list = msg.waypoints while self.global_pos is None or self.state is None: time.sleep(1) - gps_origin = calcposLLH(self.global_pos.latitude, self.global_pos.longitude, -self.state[0], -self.state[1]) + gps_origin = calcposLLH( + self.global_pos.latitude, + self.global_pos.longitude, + -self.state[0], + -self.state[1], + ) self.local_waypoints = get_local_frame_waypoints(self.waypoint_list, gps_origin) - def plan_loop(self): + def plan_loop(self) -> None: rate = rospy.Rate(20) while not rospy.is_shutdown(): rate.sleep() - if self.local_waypoints is not None and self.state is not None and self.bitmap is not None and len(self.local_waypoints): + if ( + self.local_waypoints is not None + and self.state is not None + and self.bitmap is not None + and len(self.local_waypoints) + ): self.local_waypoints = np.array(self.local_waypoints) - plan_state = np.array([self.state[0], self.state[1], self.state[5], np.linalg.norm(self.state[6:8])]) + plan_state = np.array( + [ + self.state[0], + self.state[1], + self.state[5], + np.linalg.norm(self.state[6:8]), + ] + ) goal = self.local_waypoints[0] - path, success, expansions, time_taken, output_goal = self.plan(plan_state, goal, stop=len(self.local_waypoints) == 0) - expansions_per_second = max(expansions/time_taken, 1000) + path, success, expansions, time_taken, output_goal = self.plan( + plan_state, goal, stop=len(self.local_waypoints) == 0 + ) + expansions_per_second = max(expansions / time_taken, 1000) self.expansion_limit = int(expansions_per_second * 0.5) if success: publish_path(path, self.path_pub) publish_goal(output_goal, self.marker_pub) # Visualize map with path - diagnostic_publisher(success, expansions, time_taken, self.hysteresis, self.diagnostics_pub) - display_map = visualize_map_with_path(self.bitmap[..., 0].numpy(), self.bitmap[..., 1].numpy(), path, output_goal, plan_state, self.wp_radius, self.map_center, 480, 7.5/self.map_res) + diagnostic_publisher( + success, + expansions, + time_taken, + self.hysteresis, + self.diagnostics_pub, + ) + display_map = visualize_map_with_path( + self.bitmap[..., 0].numpy(), + self.bitmap[..., 1].numpy(), + path, + output_goal, + plan_state, + self.wp_radius, + self.map_center, + 480, + 7.5 / self.map_res, + ) cv2.imshow("planner_vis", display_map) cv2.waitKey(1) # reduce length of local waypoints: @@ -133,7 +209,7 @@ def plan_loop(self): if dist < self.wp_radius: if len(self.local_waypoints) > 1: self.local_waypoints = self.local_waypoints[1:] - + if __name__ == "__main__": rospy.init_node("ighastar_planner_node") @@ -142,4 +218,4 @@ def plan_loop(self): with open(config_path) as f: Config = yaml.safe_load(f) node = PlannerNode(Config) - rospy.spin() \ No newline at end of file + rospy.spin() diff --git a/examples/ROS/utils.py b/examples/ROS/utils.py index d2b4e11..4a3c33e 100644 --- a/examples/ROS/utils.py +++ b/examples/ROS/utils.py @@ -14,8 +14,10 @@ from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue from nav_msgs.msg import Odometry import os +from typing import Any, Optional, List, Tuple -def generate_normal( elev, k=3): + +def generate_normal(elev: np.ndarray, k: int = 3) -> np.ndarray: dzdx = -cv2.Sobel(elev, cv2.CV_32F, 1, 0, ksize=k) dzdy = -cv2.Sobel(elev, cv2.CV_32F, 0, 1, ksize=k) dzdz = np.ones_like(elev) @@ -24,7 +26,8 @@ def generate_normal( elev, k=3): normal = normal / norm return normal -def map_from_gridmap(matrix): + +def map_from_gridmap(matrix: Any) -> np.ndarray: return np.float32( cv2.flip( np.reshape( @@ -38,8 +41,10 @@ def map_from_gridmap(matrix): def generate_costmap_from_BEVmap( - BEV_lethal, BEV_normal, costmap_cosine_thresh=np.cos(np.radians(60)) -): + BEV_lethal: np.ndarray, + BEV_normal: np.ndarray, + costmap_cosine_thresh: float = np.cos(np.radians(60)), +) -> np.ndarray: """Generate a costmap from BEV lethal and normal maps.""" dot_product = BEV_normal[:, :, 2] angle_cost = np.where(dot_product >= costmap_cosine_thresh, 255, 0).astype( @@ -51,7 +56,13 @@ def generate_costmap_from_BEVmap( return costmap -def process_grid_map(heightmap, lethalmap=None, map_res=0.1, blur_kernel=0, costmap_cosine_thresh=np.cos(np.radians(60))): +def process_grid_map( + heightmap: np.ndarray, + lethalmap: Optional[np.ndarray] = None, + map_res: float = 0.1, + blur_kernel: int = 0, + costmap_cosine_thresh: float = np.cos(np.radians(60)), +) -> Tuple[torch.Tensor, np.ndarray]: if lethalmap is None: lethalmap = np.zeros_like(heightmap, dtype=np.float32) normal = generate_normal(heightmap) @@ -59,17 +70,26 @@ def process_grid_map(heightmap, lethalmap=None, map_res=0.1, blur_kernel=0, cost if blur_kernel != 0: costmap = cv2.GaussianBlur(costmap, (blur_kernel, blur_kernel), 0) costmap[costmap < 255.0] = 0.0 - bitmap = torch.ones((heightmap.shape[0], heightmap.shape[1], 2), dtype=torch.float32) + bitmap = torch.ones( + (heightmap.shape[0], heightmap.shape[1], 2), dtype=torch.float32 + ) bitmap[..., 0] = torch.from_numpy(costmap) bitmap[..., 1] = torch.from_numpy(heightmap) offset = map_res * np.array(bitmap.shape[:2]) * 0.5 return bitmap, offset -def clip_goal_to_map(bitmap, map_res=0.25, start=None, goal=None, num_samples=200): + +def clip_goal_to_map( + bitmap: torch.Tensor, + map_res: float = 0.25, + start: Optional[np.ndarray] = None, + goal: Optional[np.ndarray] = None, + num_samples: int = 200, +) -> np.ndarray: H, W = bitmap.shape[:2] H *= map_res W *= map_res - + x0, y0 = start[:2] x1, y1 = goal[:2] @@ -93,8 +113,9 @@ def clip_goal_to_map(bitmap, map_res=0.25, start=None, goal=None, num_samples=20 goal[0] = x0 goal[1] = y0 return goal - -def obtain_state(odom, state): + + +def obtain_state(odom: Odometry, state: np.ndarray) -> np.ndarray: ## obtain the state from the odometry and imu messages: quaternion = ( odom.pose.pose.orientation.x, @@ -114,7 +135,8 @@ def obtain_state(odom, state): state[8] = odom.twist.twist.linear.z return state -def publish_goal(goal, marker_pub): + +def publish_goal(goal: np.ndarray, marker_pub: Any) -> None: marker_array = MarkerArray() marker = Marker() marker.header.frame_id = "odom" @@ -135,7 +157,8 @@ def publish_goal(goal, marker_pub): marker_array.markers.append(marker) marker_pub.publish(marker_array) -def publish_path(path, path_publisher): + +def publish_path(path: np.ndarray, path_publisher: Any) -> None: ros_path = Path() ros_path.header.stamp = rospy.Time.now() ros_path.header.frame_id = "map" # or whatever your fixed frame is @@ -147,7 +170,7 @@ def publish_path(path, path_publisher): pose.pose.position.x = waypoint[0] pose.pose.position.y = waypoint[1] - pose.pose.position.z = 1 # would be nice to get the extended state here... + pose.pose.position.z = 1 # would be nice to get the extended state here... # Convert theta to quaternion quat = quaternion_from_euler(0, 0, waypoint[2]) @@ -157,20 +180,34 @@ def publish_path(path, path_publisher): path_publisher.publish(ros_path) -def diagnostic_publisher(status, expansion_counter, time_taken, hysteresis, diagnostics_pub): + +def diagnostic_publisher( + status: Any, + expansion_counter: int, + time_taken: float, + hysteresis: float, + diagnostics_pub: Any, +) -> None: diagnostics_array = DiagnosticArray() diagnostics_status = DiagnosticStatus() diagnostics_status.name = "planner" diagnostics_status.level = status - diagnostics_status.values.append(KeyValue(key="Expansion Count", value=str(expansion_counter))) - diagnostics_status.values.append(KeyValue(key="Time Taken (s)", value=f"{time_taken:.4f}")) - diagnostics_status.values.append(KeyValue(key="Expansions/second", value=f"{expansion_counter/time_taken:.4f}")) + diagnostics_status.values.append( + KeyValue(key="Expansion Count", value=str(expansion_counter)) + ) + diagnostics_status.values.append( + KeyValue(key="Time Taken (s)", value=f"{time_taken:.4f}") + ) + diagnostics_status.values.append( + KeyValue(key="Expansions/second", value=f"{expansion_counter/time_taken:.4f}") + ) diagnostics_status.values.append(KeyValue(key="Hysteresis", value=f"{hysteresis}")) diagnostics_array.status.append(diagnostics_status) diagnostics_pub.publish(diagnostics_array) -def set_headings(local_waypoints): + +def set_headings(local_waypoints: np.ndarray) -> np.ndarray: target_Vhat = np.zeros_like(local_waypoints) # 0 out everything for i in range(1, len(local_waypoints) - 1): # all points except first and last V_prev = local_waypoints[i] - local_waypoints[i - 1] @@ -182,21 +219,26 @@ def set_headings(local_waypoints): target_Vhat[-1] /= np.linalg.norm(target_Vhat[-1]) waypoints = np.zeros((len(local_waypoints), 4)) - waypoints[:, :2] = local_waypoints[:,:2] + waypoints[:, :2] = local_waypoints[:, :2] waypoints[:, 2] = np.arctan2(target_Vhat[:, 1], target_Vhat[:, 0]) waypoints[:, 3] = 2.0 return waypoints + earthRadius = 6378145.0 DEG_TO_RAD = 0.01745329252 RAD_TO_DEG = 57.2957795131 -def calcposLLH( lat, lon, dX, dY): + +def calcposLLH(lat: float, lon: float, dX: float, dY: float) -> Tuple[float, float]: lat += dY / (earthRadius * DEG_TO_RAD) lon += dX / (earthRadius * np.cos(lat * DEG_TO_RAD) * DEG_TO_RAD) return lat, lon -def calcposNED( lat, lon, latReference, lonReference): + +def calcposNED( + lat: float, lon: float, latReference: float, lonReference: float +) -> Tuple[float, float]: Y = earthRadius * (lat - latReference) * DEG_TO_RAD X = ( earthRadius @@ -206,7 +248,16 @@ def calcposNED( lat, lon, latReference, lonReference): ) return X, Y -def start_goal_logic(bitmap, map_res, start_state, goal, map_center, offset, stop=False): + +def start_goal_logic( + bitmap: torch.Tensor, + map_res: float, + start_state: np.ndarray, + goal: np.ndarray, + map_center: np.ndarray, + offset: np.ndarray, + stop: bool = False, +) -> Tuple[torch.Tensor, torch.Tensor]: start = np.zeros(4, dtype=np.float32) goal_ = np.zeros(4, dtype=np.float32) start[:2] = start_state[:2] + offset - map_center[:2] @@ -216,9 +267,14 @@ def start_goal_logic(bitmap, map_res, start_state, goal, map_center, offset, sto start[3] = start_state[3] goal_[2] = goal[2] goal_[3] = 0.0 if stop else 2.0 - return torch.from_numpy(start).to(dtype=torch.float32), torch.from_numpy(goal_).to(dtype=torch.float32) + return torch.from_numpy(start).to(dtype=torch.float32), torch.from_numpy(goal_).to( + dtype=torch.float32 + ) -def get_local_frame_waypoints(waypoint_list, gps_origin): + +def get_local_frame_waypoints( + waypoint_list: Any, gps_origin: Tuple[float, float] +) -> np.ndarray: local_waypoints = [] for waypoint in waypoint_list: if waypoint.frame != 3: @@ -231,7 +287,18 @@ def get_local_frame_waypoints(waypoint_list, gps_origin): local_waypoints = set_headings(np.array(local_waypoints)) return local_waypoints -def visualize_map_with_path(costmap, elevation_map, path, goal, state, wp_radius, map_center, map_size, resolution_inv): + +def visualize_map_with_path( + costmap: np.ndarray, + elevation_map: np.ndarray, + path: Optional[np.ndarray], + goal: np.ndarray, + state: np.ndarray, + wp_radius: float, + map_center: np.ndarray, + map_size: int, + resolution_inv: float, +) -> np.ndarray: """ Visualize the costmap and elevation map with the path, goal, and optionally the current state. Args: @@ -270,8 +337,12 @@ def visualize_map_with_path(costmap, elevation_map, path, goal, state, wp_radius # Draw goal (convert to local map frame) goal_disp = np.array(goal[:2]) - np.array(map_center[:2]) - goal_x = int(np.clip((goal_disp[0] * resolution_inv) + map_size // 2, 0, map_size - 1)) - goal_y = int(np.clip((goal_disp[1] * resolution_inv) + map_size // 2, 0, map_size - 1)) + goal_x = int( + np.clip((goal_disp[0] * resolution_inv) + map_size // 2, 0, map_size - 1) + ) + goal_y = int( + np.clip((goal_disp[1] * resolution_inv) + map_size // 2, 0, map_size - 1) + ) radius = max(2, int(wp_radius * resolution_inv)) cv2.circle(display_img, (goal_x, goal_y), radius, (255, 255, 255), 2) @@ -279,17 +350,29 @@ def visualize_map_with_path(costmap, elevation_map, path, goal, state, wp_radius if path is not None and len(path) > 0: path_disp = np.copy(path) path_disp[..., :2] -= np.array(map_center[:2]) - path_X = np.clip((path_disp[..., 0] * resolution_inv) + map_size // 2, 0, map_size - 1).astype(int) - path_Y = np.clip((path_disp[..., 1] * resolution_inv) + map_size // 2, 0, map_size - 1).astype(int) + path_X = np.clip( + (path_disp[..., 0] * resolution_inv) + map_size // 2, 0, map_size - 1 + ).astype(int) + path_Y = np.clip( + (path_disp[..., 1] * resolution_inv) + map_size // 2, 0, map_size - 1 + ).astype(int) car_width_px = max(1, int(0.15 * resolution_inv)) if path.shape[1] > 3: velocity = path[..., 3] - velocity_norm = (velocity - np.min(velocity)) / (np.max(velocity) - np.min(velocity) + 1e-6) + velocity_norm = (velocity - np.min(velocity)) / ( + np.max(velocity) - np.min(velocity) + 1e-6 + ) velocity_color = np.clip((velocity_norm * 255), 0, 255).astype(int) else: velocity_color = np.full(len(path_X), 128, dtype=int) for i in range(len(path_X) - 1): - cv2.line(display_img, (path_X[i], path_Y[i]), (path_X[i + 1], path_Y[i + 1]), (0, int(velocity_color[i]), 0), car_width_px) + cv2.line( + display_img, + (path_X[i], path_Y[i]), + (path_X[i + 1], path_Y[i + 1]), + (0, int(velocity_color[i]), 0), + car_width_px, + ) # Draw current state as a rectangle (if provided) if state is not None and len(state) >= 3: @@ -302,16 +385,21 @@ def visualize_map_with_path(costmap, elevation_map, path, goal, state, wp_radius car_height_px = max(2, int(0.15 * resolution_inv)) half_width = car_width_px // 2 half_height = car_height_px // 2 - corners = np.array([ - [x_px - half_width, y_px - half_height], - [x_px + half_width, y_px - half_height], - [x_px + half_width, y_px + half_height], - [x_px - half_width, y_px + half_height] - ], dtype=np.int32) + corners = np.array( + [ + [x_px - half_width, y_px - half_height], + [x_px + half_width, y_px - half_height], + [x_px + half_width, y_px + half_height], + [x_px - half_width, y_px + half_height], + ], + dtype=np.int32, + ) rotation_matrix = cv2.getRotationMatrix2D((x_px, y_px), np.degrees(theta), 1.0) rotated_corners = cv2.transform(np.array([corners]), rotation_matrix)[0] - cv2.polylines(display_img, [rotated_corners], isClosed=True, color=(0, 0, 0), thickness=2) + cv2.polylines( + display_img, [rotated_corners], isClosed=True, color=(0, 0, 0), thickness=2 + ) # Flip for visualization display_img = cv2.flip(display_img, 0) - return display_img \ No newline at end of file + return display_img From 4ac0d9eddf8e87f104f8b091a32e8245dee60134 Mon Sep 17 00:00:00 2001 From: sidtalia Date: Mon, 28 Jul 2025 14:03:52 -0700 Subject: [PATCH 6/8] updated README to address missing deps --- examples/ROS/README.md | 53 ++++++++++++++++++++++++------------------ 1 file changed, 31 insertions(+), 22 deletions(-) diff --git a/examples/ROS/README.md b/examples/ROS/README.md index d1f7b5f..a6a2265 100644 --- a/examples/ROS/README.md +++ b/examples/ROS/README.md @@ -10,29 +10,41 @@ For simplicity (not needing to fiddle with rviz), there is a built in visualizat ## Prerequisites - ROS Noetic - Python 3 -- [catkin_tools](https://catkin-tools.readthedocs.io/en/latest/) -- [PyTorch](https://pytorch.org/) -- OpenCV (`cv2`) -- numpy, yaml -- IGHAStar C++/CUDA requirements (requirements.txt) -- ROS messages: `nav_msgs`, `geometry_msgs`, `mavros_msgs`, `grid_map_msgs`, `visualization_msgs`, `diagnostic_msgs` +- IGHAStar C++/CUDA requirements (met with pip install -e .) ## Setup (confirm these instructions) -1. **Clone this repository** into your catkin workspace: +1. **Install ROS dependencies:** + Assuming IGHA* has already been installed, ```bash - cd ~/catkin_ws/src - git clone - cd ~/catkin_ws - catkin_make - source devel/setup.bash - ``` -2. **Install Python dependencies:** - ```bash - pip install torch opencv-python numpy pyyaml + # Install ROS packages for message types + sudo apt-get update + sudo apt-get install ros-noetic-grid-map-msgs \ + ros-noetic-mavros-msgs \ + ros-noetic-nav-msgs \ + ros-noetic-geometry-msgs \ + ros-noetic-sensor-msgs \ + ros-noetic-visualization-msgs \ + ros-noetic-diagnostic-msgs \ + ros-noetic-tf + + # Check for any missing dependencies + rosdep install --from-paths src --ignore-src -r -y ``` -3. **Check ROS dependencies:** + +2. **Verify installation:** ```bash - rosdep install --from-paths src --ignore-src -r -y + # Test that all message types are available + python3 -c " + import rospy + from nav_msgs.msg import Path, Odometry + from mavros_msgs.msg import WaypointList + from grid_map_msgs.msg import GridMap + from visualization_msgs.msg import MarkerArray + from sensor_msgs.msg import NavSatFix + from diagnostic_msgs.msg import DiagnosticArray + from geometry_msgs.msg import PoseStamped, Quaternion + print('All ROS message types imported successfully!') + " ``` ## Configuration @@ -54,10 +66,8 @@ We provide rosbags that you can use to run the example script. Place them in the ``` 2. **Run the planner node:** ```bash - cd examples/ROS - python3 example.py + python3 examples/ROS/example.py ``` - Or launch with rosrun/roslaunch if you have a package setup. 3. **Play a rosbag or run your robot simulation** to publish the required topics. ``` cd examples/ROS/rosbags/ @@ -67,4 +77,3 @@ We provide rosbags that you can use to run the example script. Place them in the ## Visualization - The planner will open an OpenCV window showing the costmap, elevation map, path, goal, and robot state. - Path and goal are also published as ROS topics for use in RViz. - From 731a9830937ecd7f8f9f95b0a6f2a81cd1793e92 Mon Sep 17 00:00:00 2001 From: sidtalia Date: Mon, 28 Jul 2025 14:45:18 -0700 Subject: [PATCH 7/8] minor change in README --- examples/ROS/README.md | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/examples/ROS/README.md b/examples/ROS/README.md index a6a2265..25e53f5 100644 --- a/examples/ROS/README.md +++ b/examples/ROS/README.md @@ -3,8 +3,7 @@ This folder contains a ROS-based example for the IGHA* path planner. The code here is really an example script to demonstrate how one would use the planner in the loop on their robot. It demonstrates how to integrate the IGHA* planner with ROS topics for maps, odometry, waypoints, and visualization. For simplicity (not needing to fiddle with rviz), there is a built in visualization in the example. - -## TODO: does one need to install all grid-map stuff to get grid-map visuals? Might be useful to get rviz visualizations here. +It is however not a "ros-package"; one can use the example shown here as a stencil to incorporate IGHA* into their navigation stack. ## Prerequisites From ff2442b976b5606105d8bb2d5f424a5f58689250 Mon Sep 17 00:00:00 2001 From: sidtalia Date: Mon, 11 Aug 2025 16:42:49 -0700 Subject: [PATCH 8/8] added useful comments to ROS example.py --- examples/ROS/example.py | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/examples/ROS/example.py b/examples/ROS/example.py index 8589b5e..4b1658d 100644 --- a/examples/ROS/example.py +++ b/examples/ROS/example.py @@ -72,6 +72,7 @@ def __init__(self, config: dict) -> None: self.plan_loop() def map_callback(self, grid_map: GridMap) -> None: + """Update the map from the GridMap message.""" self.grid_map = grid_map if self.height_index is None or self.layers is None: self.layers = self.grid_map.layers @@ -83,6 +84,8 @@ def map_callback(self, grid_map: GridMap) -> None: if np.any(np.isnan(self.heightmap)): self.map_init = False else: + # this function creates the bitmap (costmap, heightmap) + # and gets the offset of the map self.bitmap, self.offset = process_grid_map( self.heightmap, lethalmap=None, @@ -95,9 +98,12 @@ def map_callback(self, grid_map: GridMap) -> None: def plan( self, start_state: np.ndarray, goal_: np.ndarray, stop: bool = False ) -> Tuple[Optional[np.ndarray], bool, int, float, np.ndarray]: + """Plan a path from the current state to the goal.""" if self.bitmap is None: return None, False, 0, 0.0, goal_ bitmap = torch.clone(self.bitmap) + # this function effectively projects the start and goal to the + # bitmap if it is not already inside the bitmap bounds start, goal = start_goal_logic( bitmap, self.map_res, @@ -108,10 +114,18 @@ def plan( stop=stop, ) now = time.perf_counter() + # this function does the actual planning + # it takes in the start, goal, bitmap, expansion limit, hysteresis, and ignore goal validity + # it returns a boolean indicating if the goal was reached, the number of expansions, the time taken, and the output goal + # the output goal is the goal projected to the bitmap bounds + # Map convention: x is east, y is north. 0,0 is the bottom left corner of the map. + # if the map was shown using cv2.imshow, the origin would be the top left corner. + # Start and goal are relative to the bottom left corner of the map. success = self.planner.search( start, goal, bitmap, self.expansion_limit, self.hysteresis, True ) time_taken = time.perf_counter() - now + # this function gets the profiler info from the planner ( avg_successor_time, avg_goal_check_time, @@ -138,6 +152,7 @@ def odom_callback(self, msg: Odometry) -> None: self.state = obtain_state(msg, self.state) def global_pos_callback(self, msg: Any) -> None: + """Update global position from NavSatFix message.""" self.global_pos = msg def waypoint_callback(self, msg: WaypointList) -> None: @@ -155,6 +170,7 @@ def waypoint_callback(self, msg: WaypointList) -> None: self.local_waypoints = get_local_frame_waypoints(self.waypoint_list, gps_origin) def plan_loop(self) -> None: + """Main loop for planning.""" rate = rospy.Rate(20) while not rospy.is_shutdown(): rate.sleep()