From 1421d8743ee3036c4d1ebb8b3074ef6e74014fbe Mon Sep 17 00:00:00 2001 From: pmusau17 Date: Tue, 27 Apr 2021 13:51:44 -0500 Subject: [PATCH] rrt_star for occupancy grid and one with obstacles --- .gitignore | 3 + Dockerfile | 1 + rrt_star/__pycache__/env.cpython-37.pyc | Bin 1267 -> 0 bytes rrt_star/__pycache__/plotting.cpython-37.pyc | Bin 2980 -> 0 bytes rrt_star/__pycache__/queue.cpython-37.pyc | Bin 145 -> 0 bytes rrt_star/__pycache__/rrt_star.cpython-37.pyc | Bin 6365 -> 0 bytes rrt_star/__pycache__/utils.cpython-37.pyc | Bin 3577 -> 0 bytes rrt_star/rrt_star.py | 18 +- {rrt_star => rrt_star_ros}/learning_rrt.py | 13 +- rrt_star_ros/plotting.py | 131 +++++++ rrt_star_ros/porto_grid.npy.zip | Bin 0 -> 199790 bytes rrt_star_ros/rrt_star.py | 361 +++++++++++++++++++ rrt_star_ros/utils.py | 20 + 13 files changed, 529 insertions(+), 18 deletions(-) create mode 100644 .gitignore delete mode 100644 rrt_star/__pycache__/env.cpython-37.pyc delete mode 100644 rrt_star/__pycache__/plotting.cpython-37.pyc delete mode 100644 rrt_star/__pycache__/queue.cpython-37.pyc delete mode 100644 rrt_star/__pycache__/rrt_star.cpython-37.pyc delete mode 100644 rrt_star/__pycache__/utils.cpython-37.pyc rename {rrt_star => rrt_star_ros}/learning_rrt.py (74%) create mode 100644 rrt_star_ros/plotting.py create mode 100644 rrt_star_ros/porto_grid.npy.zip create mode 100644 rrt_star_ros/rrt_star.py create mode 100644 rrt_star_ros/utils.py diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..2d2dcbf --- /dev/null +++ b/.gitignore @@ -0,0 +1,3 @@ +# ignore compiled byte code +*.pyc +*.npy \ No newline at end of file diff --git a/Dockerfile b/Dockerfile index 8ca8471..9ff5d32 100644 --- a/Dockerfile +++ b/Dockerfile @@ -38,4 +38,5 @@ WORKDIR .. RUN mkdir -p ~/catkin_ws/src ENV OsqpEigen_DIR=/osqp-eigen +WORKDIR catkin_ws diff --git a/rrt_star/__pycache__/env.cpython-37.pyc b/rrt_star/__pycache__/env.cpython-37.pyc deleted file mode 100644 index 55b3d667498593b6d4bca458994bd54f4fc7fe15..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 1267 zcmah|%Wl&^6dgYjyN(k|#Y19&%mRrlNDwQ8Dpc_hy9mK9o5A(CiD_boaa@`xED#kd zeuSUl1H5JBUtq;K&O=pIiLvJ1J2Us*Ip@yx%XZrn7(aji`1q|M#2+rU!-8>yncsm> zVkq7UWhnDPDD%5HG?ev0bnWHPdy}3eI!mWY`@Fmly`7DfL z?0&B4oweRvB3ReXewV> z%Dym$mTIWxg&5k(Q7&{t?I{nssainUE-iqpo+7fk4-qjL2epa$3NwEMu`*Udij@gz zDA=FG32n2sjoP-9KuU8^!5sxC&2wFv3tc+rQitg%E}g~N=vt+n$CDH7z)`ypzl3JF z|32%FvuWI~`Oo^Vv#4P4^8Wick3$`e`|mc@>uWui`8?G9I6dpl7Nsj?k|uL0nI~sX zVKnji#?lAH-u5^$c!WWAyuW662m?=uM)}nGp}3D^o2!1J@_XG zC3L zwTVv0(H!qPiLdfv^aIQdFVEoDF_$;DympcQBQ^px5`#t?*Ln|>EUeMa)T74cC0YZ0 zmoWD!ZbMZ3FDlko*X(X^$+xKf|W5r49elkFigh4GeLE5ygtE90jCx-|^Y}CNQ>2|rZqE=e& z(z8pMWT}${^yEYT1FE-NivBG<&aLQ4$6SLR`g^k?s|W(xE-^E2=FP{O_kQm;{A7JS zVEFF;@=t$wZ=JD!QfK*D5I#n!c2G$cGRdXLdB{yGhE{0vX3QD7p?e{>!j{o{q4$tU zOWLPQ+Al=tOYw;HoLSV}%gegXib3~rS>{!Dzf|2v@o9wQh@56%gAE8Q|3+t%0QSPHuk5SF}TnIL25fxsGg*CV4 z_L)Er_LSxaC~K0zY`)uTYccr@<)OCH;#gb1Mb$3* zRn#xXg^bly`>UP~NK7)7>M?FV%u!(lyu2^ZG ztc(UKle%rh%=}G zK=FmBEptzl2`*N?`_wXL5^S3jTktKN z=;E|ym1EAEKOxWaLl3nto!iv&rF)xEwo3mc2xz4bF7$!X2TOguwrI~e?FMOGt~JtO zN4DkqON^RWc6LKVGER;j_pH!Qur^ihQdp8sjt$|7C$5KWJcY>5KO?ObhP71Rw4Bc%ICe70hJg@2&OpN7HDOk7!1$TCGYD@UrBl!# zGQ3Ko^cYN)5CrI>mQw8zO7&e-xJ_fxMve4&!3!bTfP;svUG1h-5|2`|drxQ!+iE+V zWHr%=+4oAQw0AF$hkY4u|DN_YIn8HpuhQLk|AJyR?}3lvyxNA)tT`v#4gU4j*MI-K zzxPou(C#Q!Fy25>y$z-26V0avRG~FU_Cq&`i{rQo*YGGuNm&$trgld0xJtov#9QhI zR1pt>^YDCtzKF=K7_PpD3BNC^FPG0=yY?gAdXpjAHYr>SqwIxZs=M9sJkRzW0U$76qk(=chouE8CQA}&ESu>{`&AAv(t~3+~sO3 zPmBKm#y~}VNZY?l%d7?y^dw#TUVR^m45fLm5c+ojgRAsGF4%?D6l_J_?};6-=T6V3 zhn}9rDAGX`4NE!BNp43GG3j{~P7|uXYkpY(iLm^Yg`kg2uyb2_X70h=2h`Aj1KOi&=m~3PUi1CZpd7&-72cWMC6^RM$+9fVj+-!lZkVKY>K09tIH{33Nq<#0P7y#&x}iBMiZcH+ zyG&$)LV(JEou1lbdZK_H+IxHGr9kgJ^-!P-^wOSm>ZPZg`h9Onid2*yN@D(YXJ%*K z``-87%i`-n|UdN&EPG^xW>Eh81F!D7SQrd4y_zoMUywdeP?qaDw)DvZ9CDzfukIwZ#&iE z;8yY4&5E^2B~EI#c%#-&w!8M_%B@;roAtYuANJ!~zj&t>N2b#4RFdte(rouyQ9J4+ zwWNth-L1-Yzu9hf8kNCzw_iNMPMnQ8^T|)6asw$|MHV^Oos;bht8$Htb$F?_(KS(> zJ^5qF^lha0QDj3%DU^7q+5Ky3BSM*NFen){-b)%;18CY4+nZXeN;j+qJ!P zxz&u5u)7t07}=y*kK)wp)spShjXEEu?iJMtWqu5Jj9ja1tEzg?#U7@F=B~DyT`Bzl-{sIL;K?mxl^ZuXqp{5bkvg01AeUu|8 zWXB)sL%k2>j&e{j>mE&e4Ri!*`azO6F2JCbGxY2)(LWd!(9b*6Fu2zq$-`N~G+)yg~7)zlY#W%6HbM4gBr zq3%q#9d=+E3pCC?iwO%jkm~^qe-R;Ivm{DJvv(ZOQ8>K5_pbCP=?@G~LVr%sTeUoi_ z0pGHhWF56mvn|}7x^ZHw`3!aHE>?&=$bQCEt|JZPDHhL-Wg7xL8bQ5}O0;(Zdc=N4 zuiwG&7AOc9Q$s&241Hh#VbaXZh6Mz zneg$m_%&1}!%yGD3z$zf@yxz@h=pHp9_qWwdJq)0^iZMHd1WIjWG7C^kmyf?n6CPVG`h9oW?|ZDtFhS(=Xnu+m zv*mbY_X%&+tN4RQ|F?vv_DlO<5^#P46#{EZ!ruGAP>;c+6DLvdCGaE%kmkPk5D*!9 zkT$}DaR4}{AR3#Xj1(#gT7!br$iYA$jRWVgrPC4(oYn)gP3Bahnoy3IqwHI3QZ`Qw z`~hhKtLkLab0mBknyNk(odC2wP`7Q6MCb%Emhflv@=ZKVJ+3J&GY`>qRyVUNlSN&@ zfZuabl!2pG;l~H(PF>iX64+Zvlpox?M$y0jEFJZZGju6(5UV0Zq6DN-(|0|aV=Z)d zb7OZ2s`Rpryn|`%kC<`h>d7tHTX>o-j$Oyt4W3{-9G09vBgMp~L@3^x8k8q{$z0o1 z?L^QNNkcS*wm@8@@b+#eaDoA7Q;~EVOyOs3Vrq)W|Rh^WN@0jAF4zs zlXOW$-gYn-zJm9;b;b?(jp@SXSdaE1czkM(^*|HCdmedj$2-Y@@5PtdOwi4wH) z3%2j#(P>NlC!UX)S1WOIaPDHlmpS75%zn&_L5aPMtm?`A#Q#?7oRwh~KDp z#NQJ9kB^r9PIIek+qQyN_HkxJ-53|y0q#9MJ;8%t#xtC3#y(aBx~!g1m-XP%L?EXf zHRi~DiU7GvapogrA$A2~ch8f#K;hcH!exjxjEVaOA8ayv9a*ZvlZzzPFL9ClmZO$u z^fl{PrxuN_$NHklK%(?84A#McMkR7wlp~oo`q0OX6_@93%%8e)a%i#FFhgd3a*+8N zo9B_uapKf-r$wH=LqvbI);bqWj&{^wWg^kz*HeITU-T~mFq$R8R9W;of@svabcMQ` z=n+n~h*bb3S_{u2TT?6HG?{@gE)z;m{Tfac*I*|%_=r+Ywb?_ucT&pK%VS(HovH&G zD5>L6>+}=KRN}M{4wDT~i$iB0XMlaZ_jPWChA%-DLo2iyf1?Dcqb?rl#cZP(wOb?= zKpT3afr;b?7th&GdMb(xH*z`g^ga7ic9GcN1jza9Iqpo3Hri934%Abss0XEqQplB4 zbx>opckU{#ARFRb!}ULKa9R5GvZ<3t`P2L`7~+I0fg=@xiszVqYVuk6;xkylOZ=i3q+U^j^)LD+ zEq}ln8tMyuU*c*;C9oTN>E$o$`zjI3=$Q znw>YPGI90_Ho$*wP2ujhBSUeAj6PvlT}<;~Xu5S=CcE*ycziPM^xM6AS;TRX-=iSM zPHDFv^&{~^*(y0-rs!isoT?%Xu4i}EuTi)$Zxv~d_~$ELxm^Ajw-A?ur9k}`F|SIv diff --git a/rrt_star/__pycache__/utils.cpython-37.pyc b/rrt_star/__pycache__/utils.cpython-37.pyc deleted file mode 100644 index 5b6515a66abd0d21ef3d29ff0887f00a94eba7b0..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 3577 zcmai1TW=f36`q;B@+w7EWVvn=Hcf&s5Ukco-M|RipiNb$4?+>UFhDrjP&+GfC32UZ zT`D56`(y(x3KZy1kbpk4eQ<$3`KJ`~6rfM;Pv}#=GfPpI2(?OlbT13SScRiVx2Ub6 zFz+V=beq)OOQ(tKtC@CWG|Ky3OWRpA+SlH3@O3Qnq4s7~~mZHO(%^1j-h5(0N7GwlaKoWyw$+`vwm(KxGZ zH93CvylsHGb2cR;Z0Ms}pctGs&;Uk+523^h;1LLe0N?Fe%E!2(ja3b#YpW(I?JYpi zwdIq0h6gart17ZebL-QI?B`K{nQx(=xgdG1tB3sAK^@}8nl|guz(A>#R@o%TMW2Z zQkWhn;e$8Am0TEiuZ;VJO`Hbsqj^|SGq&UxwNp{ni@M~8LLEr!A#ChGoLCsO3m>S7 zQ(n|>v$A34do=%C7Fr$^;AP_C|#)6h0 z=u~=RJgEcedXS;Fkl~lof+YW8B={hQ9U9Go6;W*@Xx10R!9Gg~`j8rQ1}m&*99m)* zutM6&$c?OSA-5fM8+U)yb=3D!s~-?Cwn&0fKLqL4Dnopm81E2y7esTVg}iI#YM!e* zG(aBrf^WKU3B1ayQxMD6K^TB`4fI<8e~WJt=(nx`y;`@7LoF;N?2R@48x6b^D4z0c z-LzmMYE$xu#9zdxCxSc~`iM`ZbDS3r{M!ir%`LssFKZY9vhdIr;RHK#Rd2DwIo~UM z@YQ$tWVNpP(|7o_!Ug{x@t@v%%#OF96E@C$>xos?%SPG6sjx_#ST7pzmd#UJy)d(1 zJZ2{0uy!BqAUL)Ridig;lh+>Ej_S|U_waT_Uag8EAE4E3RidIupngPTDYTx6td%QN z@6k|~$on8YarB|)$C}U7kBN6hicOrqOmq@VFoA|y>)f_iE#F?_Xk942AdWBQYfE7+ zmxrWP^ic#_TF<-WQz-(866y)b_ZK{Kpy2 zCTXtjfunv#glxs67>j^P?pQ|Ey7O5^@S8OC7Lm&;pgsie&nOw`9O5S=+<}DG`TO`) z^%hC|#!}*Oa;C6LkyB!1p%}y6LW4w3;zj@@L7Bc0G7_c(lWLR@RAKEek_2)7Dr8Da z9oKfM;sFw0B-U5cBlT09^EZ?#FA`0-nR%ui{B=^`9u2F$Zy$OP)o=U92Uv6kVs=qC7O| zkE0;a^&l9hayp`RGYFne`=e?^y+-rNuU_yA20)) + item2 = (np.where(self.grid==0)) + # just plot the boundaries + self.x_obs = (item[0] * self.res) + self.origin[0] + self.y_obs = (item[1] * self.res) + self.origin[1] + self.x_free = (item2[0] * self.res) + self.origin[0] + self.y_free = (item2[1] * self.res) + self.origin[1] + + + # get the start and goal + self.xI, self.xG = x_start, x_goal + # create object that defines the planning workspace + + def animation(self,nodelist,path,name, animation=False,block=False): + # plot the grid + self.ax.clear() + self.plot_grid(name) + # plot visited nodes + self.plot_visited(nodelist, animation) + # plot the final path + self.plot_path(path,block=block) + + + + """ + I'm a visual learner so this method just shows me how points are sampled + """ + def random_sample(self): + x = np.random.choice(self.x_free) + y = np.random.choice(self.y_free) + x_index = int(round((x - self.origin[0])/(self.res))) + y_index = int(round((y - self.origin[1])/(self.res))) + print(x,y,x_index,y_index) + print("Sanity Check") + print((x_index*self.res)+self.origin[0],(y_index*self.res)+self.origin[1],) + print(x_index,y_index,self.grid[x_index][y_index]) + self.ax.plot([x], [y], "ks", linewidth=3,label="random_point") + plt.legend() + plt.show(block=True) + + + + # plot the 2d grid + def plot_grid(self, name,block=False): + + + + self.ax.plot(self.x_obs,self.y_obs,'k.',label="boundaries") + self.ax.plot(self.x_free,self.y_free,'y.',label="freespace") + + self.ax.plot(self.xI[0], self.xI[1], "bs", linewidth=3,label="init") + self.ax.plot(self.xG[0], self.xG[1], "gs", linewidth=3,label="goal") + + # set equal aspect ratio for figures + plt.xlabel("x(m)") + plt.ylabel("y(m)") + plt.title(name) + plt.axis("equal") + plt.legend() + plt.show(block=block) + + + # Go through the list of nodes and connect each node to it's parent + # Pausing if you want to animate + #@staticmethod + def plot_visited(self,nodelist, animation): + if animation: + count = 0 + for node in nodelist: + count += 1 + if node.parent: + self.ax.plot([node.parent.x, node.x], [node.parent.y, node.y], "-g") + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: + [exit(0) if event.key == 'escape' else None]) + if count % 10 == 0: + plt.pause(0.001) + else: + for node in nodelist: + if node.parent: + plt.plot([node.parent.x, node.x], [node.parent.y, node.y], "-g") + + # plotting a path via list comprehensions + #@staticmethod + def plot_path(self,path,block=False): + if len(path) != 0: + self.ax.plot([x[0] for x in path], [x[1] for x in path], '-r', linewidth=2) + plt.pause(0.01) + plt.show(block=block) + + +if __name__ == "__main__": + x_start = (0,0) + x_goal = (1.422220,1.244794) + grid = 'porto_grid.npy' + pl = Plotting(grid,x_start,x_goal) + pl.plot_grid("Porto Occupancy Grid",block=False) + pl.random_sample() diff --git a/rrt_star_ros/porto_grid.npy.zip b/rrt_star_ros/porto_grid.npy.zip new file mode 100644 index 0000000000000000000000000000000000000000..612a079c8f9ef654dd307da1f223d97e9455d130 GIT binary patch literal 199790 zcmeI*c~q0f-Z=0Hptw}fTD2}ftyQklimkPv5Wrfct+%2Th*UOPh1LxUO(TRXUaQu6 zm7=A%K(NvZB@vL+LclC`xu76%3(IXt(klptn4~}m5E9;bV(+i#_m9J$ukZOjo`al2 zzR%2jW}ewHgpKQ6-JU@RO+fyvuioAHFq&~?vMb`F|4jBk^H6F^+9xRq$!Q;LU%EFn zBPwzta$WKYFLCX{&l8#ZCL@|#w<|)g_DX^~HSai9Po00JNY0w4eaAOHd& z00JNY0w4eaAOHd&00JNY0w4eaAOHd&00JNY0w4eatp{9b)M0?MQ{23$dqVyTl%M)iw52kd*McVDrJO++14gAKgD{%g4vd71FJ$UKLnXHyt?%wPq z3u_WW>bfeEm-W#HnLRetAgnU)OgqHlcrFmiQxHl_(8?I?s~;QV{9#3*=BJ{Gygq~V zlU$X_6`?_yR^us5wUR>>U6z^N?+ku6!lB#Zj*GYv2r2r?hDRQ-gf@3_IJdmY{NYd6 z1$IGtv^*T4YOzpYjGtnhOAZ|@*ZdUq(=~67`%0nKAEEpYKh3V$>T~ZnjTo!jkU5z# zapc}D)=BZO)De`eCe5sk`rW``vf|CPDi4%=NXo0oAGF%*4z zd+Vu%W)8#jO0s1h4e=u~fAs0|GR`1JMpKa$WkPFEgIA3M8unokk=ab_`L|hCmBOwS2q{lnR_*h( z;$}d&6mv2sFQ5&^2=W*)bS(`{Fvc&bo=ynDKi=7o(=!KU2=0pnxCdZ$8h4B{k~=QG zsr#jWy6L37`2{;HAqWNL*5=KSOeUunQdG6FZq`VQk96E208YH6v5+VXv3q=ylb z{c3M`tDjAX!hJ-cGw#Q>5oL_D7$FUgyVjpUj-wiyf_`J*r+U3~95OGl`m@f}kzrUW z9EU;Zx zQ`SYB)*xM$F=5bOI*%9^7?rZ^1(dZ$6S`7pU4T-(t^Ol( zv`=p7$DxtnN>qjG6@!E`BK#h!^umiM?T?)C2? zB!0@C#*(a}CZ{1jrK;XH1gXv5c5O}(D;6uP3_ZE`KYQ%tVD zD5{ky3K|EtSp?$|S1FxbSR6`jEkZr6b>wI}s!X!PyCcW%nuDfWN5_yH)k~oCjFgZw zDw{Vl)=MXAi`~iTsi;ng-_^-EW8LX6scb&T_)9wu9VHJ+k3w5I?U}hM4ni9`GzT*? z@yJiO%zLP+P;_-j*|{n@4SjlFbCB6BAw=PMOcmAfBJD}5`9-141sQM=dv2zXX;P>M z3F$lSxMT9BA-{CfPnrGi5!2Icb<$WmIP?a6JdQi{@U29z%omA)=8v2O9PV{A#kBjQ zb0>mkH5bdto`tu!K8qh9UsGR9wABL*9%}P;Yc47ubD+P;iUX&jK{wV?LwE#v_N2i> z;L|9Op|OcYMqgx}gyYuxljGu@J({{s}ewx&tXbyY|u>AwdLFCr#q7HNKZ z6Y-NfLvqA5CY0)Lj1A1@lII1%rBpwtt4gxJznwWs)9p5H4a|!lQyi`YU3;3gKBoOJ z3Orz&*;E`(E?%~?c!NKvL)S%HpQq+$2W>Zf$WOy*&FdQwKjpL~M=x+w#H%l-hGs4x zsw}%h4PqO2G)Lgnd{Og%$`0CYPajKa6lPS1;Dop)oK`WO^^!V|JAs(oc4}}y!ZVh< z9F=_%e~$6JYJU!S$yJ{@c_r>pUhY}0qEF_VQk5MwC~1lnQJ%h1__tqCiRe~o>8+f1E>;a>)n#!yn*P0O3kacodgsyH&5(j1Nnd zyJX+KM+m};5}ETsognc&CNieri7I!T;JZ3vCKYz)&}C*xLU9}u6{pKx-E7UQ@uetR_)w`0t4 zEh1Kwbv0ilVeDt!sUwEzvYeHW6>UsdAY{*1l&UW(Y9DMP#-+-r{tL+#SS#{X^l9hL zuxAWDBPGxKn_8%b8FozFfJ?WQpORkeQh0$px+;d8nkBim7~HE*Fn?r>)ZOVP50Zfi zREtKtu5tEkXJ)(Y3$QLWzcfr=C;0Dg5$Xi9kFkP^-5)6y2H90EIbln%V&y-vUB{$6sCraQNW5xoZ%{T1+(s88^ z))P}p6*#M|>4XhWHoYh5yvId<<4aC#rL)%y{7vK@ZQZQns*4m$`kyBz78vfV0P6{x zu3j&gl&_>u60P$mjywEx=hK=RA(?ewWW1=zz{!>NKSxZi^-+C~LhiaUc4c`?K(!wi+M?HMvI7u-s-7N)A;pmGCPYg0p+j0nf`by7b z6=$wdL7%8TCLmTDvQc|Fi0{=IA5>YB^79dOjw0>>xusRes7W6Ryecw9TT@$42R&UK zBqcuTXy6te5>go%9`}a^yGIk~G-J|hoE^rKoLXsE^Pm znS(OPgd!mxcM=yVj|xSCl>E*%lj)XQ_4~??U$ITotr#X(xHJAa<-wR}rT^fyBCRRP zW&6PMzL%`6N8;N8~T5(y&AZ!xr^FVe=_PU3Qix~rsy%@dV3-HDWm zarik@NP5{mG4#WU3&pJ74_&P3D`u5;%^BI2aOs5V{smc`U#UESe5W$(5g5+ZE)aHk z7iYSruwyUmg+M`ZWy)X>dhD7JGC;nN_?lM)K?AmxP_@p*{9zg znBo`6aXes;d_2q|?_LE_tEhooZMr3k=J1o>iE-&VEw$<;zr+}3M5kFZy6BpQb~%e! zt}UBzUtWNU5Ru&{wQjC*%t*1 z6kUx2KS-z8YBO6@eTSv3lSPVo?BnsQnc7fdc0|Sa$=edaEmrlp4hq_2Kh|yIF-($7 zN5!JP!rW6^%l{B4bza2VC^GRVx}Vu$jiCqdnT-S0b`P5*Q?BYewBnRW^h@0hFOE|Ys)#tJ$K`@swQPibxk5K=|) zS9Gz8^I1!Gx#-Q%n6t7=uUW{o=`0b~za@_8g91<3zP8hvKaMO^*$>F?gvaDKF1g!3 z*rO*eyPKz7oQvPOE2+{CyAMWcd<@enj>l#X9KYOsR`fUdQeh9ymhY~2$+Lut;~%!a zsbi%{qgp~0F=;$8Bdj2X{_izAyR!{pyw;Pl%0(9PVD+d}zAamscGvuutSi~$@2)Hj zx7DXSIA911Z9VI#T*M?dQRiKn@_IxyMq1ekTOr3)->AP-6QRzP;qH1r`(|?Ux3V!- z)KFSMR1009O7r=Fr?4b3lOt9hcKmC@T^qeSVAm_;N#42mAC~&23StvfOwWo*>0#eK+aaa#kJwa4~fgI#Yh@ zaBJJv-(CKm+%1;Oni9DFTTxjGzqZ~!V^A8)!W*BS^7Ez&aq-;;cOFFS+I*Op zmyP~&QonvPCQgg+#)^nOo|_WQVzsf4JC?Ryi_@Rr-6r*2rka zrPesA^?L1K@&aG6Nw%e&~83EVnW?ZKN+P(vaA7U(nT-!m*= zZ&DI(u+jIufT#CqOA7Uw<8=YOvh67@x>>#X-wSx6U%e}RndCRinA9j`JvRJ(Su!fy z#&gmAJ)Z~8SSd?yDAuG-Yu4Fq?flIpyTwtiQ%~MhOEH#cbv9m^S2o^ z#qth^iQ{Uj2vr{#CQi~H>Gh;J^>#I+|Lho-nxgSHPqF<(?}M6yeT9nI26yW@A7Z{A$ulKRd1=SW_kCPj&syE{ug2q94)K{of2pw>(#P@X`w|qV z4K(X{a(_-euC>qaV0yWxdS=K_#Uhh?;K?Cg#2D75IMJ+~cO$4!|D5~(zt3sPTuFp2R zAMZ#IE~MWKu@-cbC$h4yYQ_-V&}}yyv0vhdmRsQ{LtR4 zN-Hg5MbBXs=S=c4-WbFG>YqoBNj>+?VXw+_xIET!s74(f3DPjZ3Tn zwv{s)V;ncf9PJ!h(ddb;uQ^j&RieTuYmDGpY^SH4Jk!>%#r;}ggC!ErSWcgXvFi3D51|C$9N zQ+;})U_#_s%XfP2MBb@9e9na2dLGTJPxc6FG&G%8#__gU`-P3e5zK_oMqlw6n#;lK zHNxdwaOC^8Fql$TMS$U$qP)ZLJ}+s<{rRKkvx)8J(Gslh*l=kX3kCkXoA*P6i|M-k z7fs{!q;kffzx^pf@(4Z3fPYI2v)xrOyEjW5;S!$2`B~6-y)j7hP5BtJpuJ{^jvGa( zi`Wal?35+?B>9S4UK%+XLL4Db&yn%5JMM0!Qulx33`^115t2B%1Bd&M9F>hB{EP9i z3-Ra{ibh@+uoRrm%qml5QCrWf_;1J?`HC89ZlG;oGTPmdZkFM6YN({_;bpu9wCWJ6 z`vB3dqKq2dR}~pcZFph+t|s7NK}BJV7sEPh@H5FTr~B6u@319;S`HP&@>Coe;$Cx1 zw4G83rMU&C*NptbL4JsOjx=|=mF4wR)Py3%48u1S$J=acY>R`^t-49W3yG6Wej9_{9MUi;sk}kFyydFlWxPL->>h4^me>ez6WsekMY*2qf^rR) zN7i#Wh7&60IC*M+&EXjj$TRecLMjiq0gppVI_=GZ+6!%?cb3N3R~`0x&|qjJ&v5yi zwXVjbyG$mJ#-EgP{~L~#F6PNq=>ww)L+^c7%dXn6E&M*YP7Qu|rO=vC)}s$`MKgMX z)f{bsonD*l7(GKD-~7i;&d*K9@6RN1RLH1jnwwU@uzI1A)B(-eLf_EQy(?|3GaT-~ zv(<$R+XVFy>n-w)!b{;57kPvamz>PCI?4sau@Wphq$p`JA0OR6GaHsLv>x_ zoU8GtDRO(~jl7oJXZbDPu9-2Kq|k&s++}`2<4c@LD>!es3YIR4X)i>HC*)FRQ5rY8&_%DLNu+Ge~7w1pM?)n;nVKgD>X{)8O5qA3B=K!Z##;6jliwO<>mKl zIU?y02cIP0^J6Hth9^6taD;G1$z7&l0hRNe5>KzrO`&J$qp3Tx$06OQ+$7qdlV~`F zW4Hs=%oA5gBD{7OQ_4e38{1CO(2^Si->aV1Z5CUziN#mgsTHMRk@XU*Pt{dOFEidl z3P+OdS{1uVHL;>bT~%r$&-KkgxDTW@o7ClL1AT||Tu&4+^JGSrO0cm2Mc_&BjOFCA zR7c}Bh2o6a$;pnD;yP8j3vzt^YToht|K2>Ef4L(hZsmGnXUyM4O(e|)Y6hc0J?fV{ zRdGDY{;KJY%I?@Z5fz@2`jFR6MyfsW=SX})U+Rm+y@keV6tMb(!PUaxUO)km*%MQYoL)$3exQ?IHxnWZh2k&i4P3+`el=0x8iSrjLQ*qJ<ASXD3dkEhTOwYilGu{)_sU@z zac>LZ$xp!3R6z6z6JvqUw3GV&v6pkY=#*MG#9dK2TsDS8sHc^>Y4n$-=#^tiWcbhO zIQOoMaZDsDB0d4gIhnv|@Fxvz$Q2JCCuhsq4N=)z(h(HZqz@v0j)ilSry{D~l||re zogG8AzQEljj^O)s+V0ltYsYXTpd1em&O~09t?wLLB2=GDC6aL6T7ui+D^&PnFC8_c zQn|7(uS%%L8%E(9Gk#z=sBZ&%0zHTkX`63UnI@y2o_0*8@ zEAjABay6TS{I6GeJ?pz1m31i<{@9B@|IbIuu}GVf7Of>8)<=i_ ztj58!S>d=OT5?J1&ol9fE!ZbIA!e&!V2ah%dXgSxhITtLlV2W&0q@6&aUmGu8>1 zV>rj&x)v|XQL0pYpKa0NS>oj+Ye7!(!_(xnYpFLpP}%acN|hXMAF}IN1%4@SljDN^ zd@d8;89Kfkk6+X?`uMBj%!hFssFz@u*Hl(#Kk(QHe%u>9X;ec?z1UWewB6!0hH98a zRRg|aKlrrpd)Zuab5XDTvrCfan3myDbXj}`w$RDj9y;TnkLA7_ z2H6Smum@So9QkKSJ`vmA!u_5nIbU%_59z^EJ$x?Zfsn0Kk zJYhu@XFqI@k<^T^z3GnrjN>w6_q}p1w`COPg5RERcVv96Vv)xvw6F5R_Dw%tyT=H1 z2(Re7>d#=Sz%+~PE4}p(av>C5=b7(b@58gyz-Va|L+(Rf=b*Cb7thkU7F(Ia(Ps6s z(}#MJG6qb%{wsx!P?51)YgJ`8?7}mE_)XF(?l9MboC_3)cS1wIZo%Cc&nlGm7`iU1 zJIPNR;It}F=Q5@km(oMmcDzJMAu%+cxq#NVe~ad7x@=V zagvIq=j{pi{-`R<>&@Nmo}De*%M#1OP(gPoT_;oLD zkJ}Sp8Yln=fB*=900@8p2!H?xfB*=900@8p2!H?xfB*=900@8p2!H?xfB*=900@A< zi~(Jk^MelRM=wu{1P>4Z0T2KI5C8!X009sH0T2KI5C8!X009sH0T2KI5C8!X009sH z0T2KI5CDNEUZC}WtMhN+JU801ZsQYQ8Yln=fB*=900@8p2!H?xfB*=900@8p2!H?x zfB*=900@8p2!H?xfB*=900@AVpCJBMEa?0|y9z00@8p2!H?xfB*=900@8p z2!H?xfB*=900@8p2!H?xfB*=900@8p2!Oy7FL00^IMw+_0(yCS+@AQ-KmkAi1V8`; zKmY_l00ck)1V8`;KmY_l00ck)1V8`;KmY_l00ck)1V8`;KmY`04Cum~X94O*FHegE z4-fzW5C8!X009sH0T2KI5C8!X009sH0T2KI5C8!X009sH0T2KI5C8!X0D&i7p!I+& zjXDhQ+-S?XjZb`Opa38M0w4eaAOHd&00JNY0w4eaAOHd&00JNY0w4eaAOHd&00JNY z0w4eaAOHf}JU!f~vjF^>@DM0|Y<-1V8`;KmY_l00ck)1V8`;KmY_l00ck)1V8`; zKmY_l00ck)1V8`;K;VfNI7kniN}UBjy}UhcPkd>h03ZMYAOHd&00JNY0w4eaAOHd& z00JNY0w4eaAOHd&00JNY0w4eaAOHd&00OxKIzIKm0AV*rTkI2NIFALu7Z3me5C8!X z009sH0T2KI5C8!X009sH0T2KI5C8$9K;XoU>uE0DuFv3CPC)*wuioAHFq&~?vMb`F z|4jBk&;GwV{IH+;HZ>*flaz$yw2!tg-J6;b6*&&ME_sEQ$lNy>(cHRS5psUnxPIJt R>eV#-FB1Rn_7wgJ`X3M2lk@-p literal 0 HcmV?d00001 diff --git a/rrt_star_ros/rrt_star.py b/rrt_star_ros/rrt_star.py new file mode 100644 index 0000000..5f36e39 --- /dev/null +++ b/rrt_star_ros/rrt_star.py @@ -0,0 +1,361 @@ +import numpy as np +import math + +import plotting + +# class def for tree nodes +# It's up to you if you want to use this +class Node(object): + def __init__(self,point,cost=0): + self.x = point[0] + self.y = point[1] + self.parent = None + self.cost = cost # only used in RRT* + self.is_root = False + + + + +### RRT Star Class for occupancy grid +# The inputs to the rrt start class are the goal, and start +# step_len: +# goal_sample_rate +# search_radius +# max number of iterations +class RrtStar: + def __init__(self, x_start, x_goal, step_len,goal_sample_rate, search_radius, iter_max,grid): + # The goal and the start node are nodes within our empty graph + self.s_start = Node(x_start) + self.s_goal = Node(x_goal) + + # Define the parameters for rrt + self.step_len = step_len + self.goal_sample_rate = goal_sample_rate + self.search_radius = search_radius + self.iter_max = iter_max + + # Initialize the list of vertices with the start point + self.list_of_vertices = [self.s_start] + + # The path begins as an empty list + self.path = [] + + # create the plotting utility, this also has information about the obstcles + # and free space + self.plotting = plotting.Plotting(grid,x_start,x_goal) + + + # get the obstacle boundaries and the free space + self.x_obs = self.plotting.x_obs + self.y_obs = self.plotting.y_obs + self.x_free = self.plotting.x_free + self.y_free = self.plotting.y_free + + # get the origin and resolution of the occupancy grid + self.origin= self.plotting.origin + self.res = self.plotting.res + + + # This is the occupancy map + self.grid = self.plotting.grid + + def planning(self): + # Iter-Max looks the number of samples described in sertac karaman's paper + for k in range(self.iter_max): + + # need to generate a random sample in the workspace + node_rand = self.generate_random_node(self.goal_sample_rate) + + # find the nearest neighbor in the tree (probably euclidean) + node_near = self.nearest_neighbor(self.list_of_vertices, node_rand) + + # This is like a steering function, we take a step in the direction and + # angle of a newly sampled point (so for f1tenth it might be using the ackermann dynamics) + # who knows + node_new = self.new_state(node_near, node_rand) + + # print the sample number + if k % 10 == 0: + print("Sample Number: ",k,"Number of vertices",len(self.list_of_vertices)) + + # get a new node and if the ray connecting the new node with near node + # is collision free then we can add the vertex to our list of nodes + if node_new and not self.is_collision(node_near, node_new): + + # find the closest neighbors this is a list + neighbor_indices = self.find_nearest_neighbors(node_new) + + # append the new node + self.list_of_vertices.append(node_new) + + # Go through the list of neighbors + if neighbor_indices: + # choose the parent node from the list of neighbors, the parent will be the node + # with the lowest cost + self.choose_parent(node_new, neighbor_indices) + + # rewire the tree, this ensure's the optimality of the search + self.rewire(node_new, neighbor_indices) + + + # get the index of the minimum cost vertex within a step length of the goal + index = self.search_goal_parent() + self.path = self.extract_path(self.list_of_vertices[index]) + + # Plotting + self.plotting.animation(self.list_of_vertices, self.path, "rrt*, N = " + str(self.iter_max),True) + + + + + + """ starting from the vertex that has been identified as closest the goal work backwards. + """ + def extract_path(self, node_end): + + # last node in the path is the goal node + path = [[self.s_goal.x, self.s_goal.y]] + node = node_end + + # from this node ask for parents + while node.parent is not None: + path.append([node.x, node.y]) + node = node.parent + path.append([node.x, node.y]) + + return path + + + """function that checks for collisions in the newly sampled point""" + def is_collision(self,start_node,end_node): + + + # get the steps in the right direction + x_len = end_node.x - start_node.x + y_len = end_node.y - start_node.y + + # discretize the path and check if any of the grid points are occupied + for i in range(0,101): + + x = start_node.x + (0.01) * x_len + y = start_node.y + (0.01) * y_len + x_index = int(round((x - self.origin[0])/(self.res))) + y_index = int(round((y - self.origin[1])/(self.res))) + + # in the occupancy grid a value of 100 means occupied, -1 is unknown + # better to assume unknown is collision + if (self.grid[x_index][y_index]==100 or self.grid[x_index][y_index]==-1): + return True + return False + + + + """ + Return the node with the minimum cost (in this case it's a euclidean distance based) + cost, this function doesn't return anything other than connect the graph + """ + def choose_parent(self, node_new, neighbor_indices): + cost = [self.get_new_cost(self.list_of_vertices[i], node_new) for i in neighbor_indices] + + # get the parent with the minimum cost + cost_min_index = neighbor_indices[int(np.argmin(cost))] + + # the new node's parent is the node with the minimum cost + node_new.parent = self.list_of_vertices[cost_min_index] + + + """ + check if path is feasible + """ + def path_found(self): + if self.s_goal in self.path and self.s_start in self.path: + print("Path Found") + else: + print("Still Searching") + + + + """ + This is another function that can benefit from a vectorized implementation + It computes the distance to each vertex within the tree and either returns the vertex + that is within a step radius of the goal and has a minimum cost or the index of the + newest node. + + """ + def search_goal_parent(self): + + # create a list of distances to the goal + dist_list = [math.hypot(n.x - self.s_goal.x, n.y - self.s_goal.y) for n in self.list_of_vertices] + + # get all the nodes that are within a step length of the goal + # return all the indices that satisify the criteria + node_index = [i for i in range(len(dist_list)) if dist_list[i] <= self.step_len] + + # with this list of nodes that are within a step length of the goal, compute the distances to each of the nodes, + # and also ensure that connecting this node with the goal doesn't result in a collision + if len(node_index) > 0: + cost_list = [dist_list[i] + self.cost(self.list_of_vertices[i]) for i in node_index + if not self.is_collision(self.list_of_vertices[i], self.s_goal)] + if(len(cost_list)>0): + return node_index[int(np.argmin(cost_list))] + + + ## TBD: need to grok this a little more + return len(self.list_of_vertices) - 1 + + + + """ + function that generates a new node based on a random sample + + Node start is the nearest node in the tree + Node goal is the random node that we just randomly sampled + + """ + def new_state(self, node_start, node_goal): + + # get the distance between these two and the angle + + dist, theta = self.get_distance_and_angle(node_start, node_goal) + + # instead of using this new point we take a step in that direction with a max distance + # step_len + dist = min(self.step_len, dist) + + # create the new node + node_new = Node((node_start.x + dist * math.cos(theta), + node_start.y + dist * math.sin(theta))) + + # make the parent of this node the nearest node + node_new.parent = node_start + + return node_new + + + """ + Rewire the tree. Performing this operation is what gives rrt_star asymptotic optimality. Basically if adding this node to it's neighbors + decreases the cost of those paths then we should do so by rewiring the tree + """ + def rewire(self, node_new, neighbor_indices): + + # iterate through the list of neighbor nodes + for i in neighbor_indices: + node_neighbor = self.list_of_vertices[i] + + # if the cost at the current vertex is greater when adding the new node + # then we should rewire the tree by making this new node it's parent, thereby decreasing this path's length + if self.cost(node_neighbor) > self.get_new_cost(node_new, node_neighbor): + node_neighbor.parent = node_new + + + + """ + This function returns the nearest neighbors of new node. The search radius is the parameter that ensures + asymptotic optimality, it has a minimum distance between the step len and this other thing that I'm still wondering + about tbh. This function can be sped up (vectorized implementation) + + """ + def find_nearest_neighbors(self, node_new): + + + # n is the number of vertices + n = len(self.list_of_vertices) + 1 + + # this is the optimality criterion + r = min(self.search_radius * math.sqrt((math.log(n) / n)), self.step_len) + + # yeah this really should be vectorized compute the distance to every node from this one + # this is an ordered list of each vertex which is a node + dist_table = [math.hypot(nd.x - node_new.x, nd.y - node_new.y) for nd in self.list_of_vertices] + + # return only those nodes that are within the search radius and that don't result in an intersection with + # an obstacle + dist_table_index = [ind for ind in range(len(dist_table)) if dist_table[ind] <= r and + not self.is_collision(node_new, self.list_of_vertices[ind])] + + # return the list of obstacles + + return dist_table_index + + + """ + This method should randomly sample the free space, and returns a viable point + + Args: + Returns: + (x, y) (float float): a tuple representing the sampled point + + """ + def generate_random_node(self,goal_sample_rate): + + if np.random.random() > goal_sample_rate: + x = np.random.choice(self.x_free) + y = np.random.choice(self.y_free) + return Node((x,y)) + return self.s_goal + + + """ + Function that returns new between two nodes, the cost of the starting node and the end node + """ + def get_new_cost(self, node_start, node_end): + dist, _ = self.get_distance_and_angle(node_start, node_end) + + return self.cost(node_start) + dist + + """ + nearest node in terms of euclidean distance + """ + @staticmethod + def nearest_neighbor(node_list, n): + return node_list[int(np.argmin([np.linalg.norm([nd.x - n.x, nd.y - n.y]) + for nd in node_list]))] + + """ + Function that returns the euclidean distance and angle and angle betwen two nodes + """ + @staticmethod + def get_distance_and_angle(node_start, node_end): + dx = node_end.x - node_start.x + dy = node_end.y - node_start.y + return math.hypot(dx, dy), math.atan2(dy, dx) + + + """ + computes the euclidean distance of the path of a node + This can be sped up by storing the cost at each node + + """ + @staticmethod + def cost(node_p): + node = node_p + cost = 0.0 + + while node.parent: + cost += math.hypot(node.x - node.parent.x, node.y - node.parent.y) + node = node.parent + + return cost + + + """ + Function that plots final solution obtained + """ + def plot_final(self): + self.plotting.animation(self.list_of_vertices, self.path, "Porto Final Solution, N={}".format(self.iter_max),True,True) + + + + +if __name__ == "__main__": + x_start = (0,0) + x_goal = (1.422220,1.244794) + grid = 'porto_grid.npy' + step_length = 0.10 + goal_sample_rate = 0.10 + search_radius = 1.00 + n_samples = 100 + + rrt_star = RrtStar(x_start, x_goal, step_length,goal_sample_rate, search_radius, n_samples,grid) + rrt_star.planning() + rrt_star.plot_final() diff --git a/rrt_star_ros/utils.py b/rrt_star_ros/utils.py new file mode 100644 index 0000000..95a7a4d --- /dev/null +++ b/rrt_star_ros/utils.py @@ -0,0 +1,20 @@ +""" +Utilities for collision checking in rrt_star +@author: patrick musau +Inspired by huiming zhou and Hongrui Zheng + +Overall forom this paper: https://arxiv.org/pdf/1105.1186.pdf +""" + + + +import math +import numpy as np +import os +import sys + +# I need this file to get the occupancy grid +import plotting + +# import node from rrt_star (x,y,parent) +from rrt_star import Node \ No newline at end of file