From 6a637809f1e89e4578854ea2b8d2b4924aed1362 Mon Sep 17 00:00:00 2001 From: zhm-real Date: Wed, 24 Jun 2020 19:41:29 -0700 Subject: [PATCH] update RRT --- .../rrt_2D/__pycache__/env.cpython-37.pyc | Bin 1343 -> 1303 bytes .../__pycache__/plotting.cpython-37.pyc | Bin 2816 -> 2831 bytes .../rrt_2D/__pycache__/rrt.cpython-37.pyc | Bin 3673 -> 3513 bytes .../rrt_2D/__pycache__/utils.cpython-37.pyc | Bin 0 -> 2978 bytes Sampling-based Planning/rrt_2D/env.py | 37 +++--- Sampling-based Planning/rrt_2D/plotting.py | 4 +- Sampling-based Planning/rrt_2D/rrt.py | 49 ++++---- Sampling-based Planning/rrt_2D/rrt_star.py | 66 ++++++----- Sampling-based Planning/rrt_2D/utils.py | 106 ++++++++++++++++++ 9 files changed, 181 insertions(+), 81 deletions(-) create mode 100644 Sampling-based Planning/rrt_2D/__pycache__/utils.cpython-37.pyc create mode 100644 Sampling-based Planning/rrt_2D/utils.py diff --git a/Sampling-based Planning/rrt_2D/__pycache__/env.cpython-37.pyc b/Sampling-based Planning/rrt_2D/__pycache__/env.cpython-37.pyc index c96512a2191fe380bb3eea583e72bce3b85230d0..3bd1dd89f90c5062092803d24f393a86faa86e0d 100644 GIT binary patch delta 460 zcmZ9IOG*SW5QfvKq^CP3-M$A`zHp)7;{bxI<;wq4RW+x+qS1^Lt&=YtZ$=-!a z|4NGs6Y|xcs_xXEe3)0`uADOhe1$1J1%*gnc?l(<2gSNn zgL)KmaVrk>G)nxJDbfEAw5lW)NeQ!7lPTqhV}{HLzy6%jg+L>5lftTadF35H$EL zo^^VmoyGrW;cF8v57&sbhhM~&oguPU7bxgIe{luJ#$^V(ien&PVc5&OhmiSMVljHa fTmB*+b8bV3bgoW&i;bYB8m3Ki^+d+@&-&sA65~(B delta 510 zcmYjNJx{|h5Or+Fg)}ZLN?|~V&kjLi=np_3*t@`tI#|l?4lJOE9f`pMe*=C2e}lhp zGYcd4?3Cchm*;oy?w2XJViX2o-A<*eirlV)07_|JgqrWwC=Fq96-PdAcDpe8z+qdC=y}M?>2T)`nc( zfFNS+5eTq8YCHB!@CAIVNOZVRC)0hF ztx66;C97;!h6i=nqGL3RS-n|j8q?VwENYcT;DOV0Y5Qb;wcJCwPP7ygp#6oNYAqIjrR(>L)B7%{CBDxZYkdVs>$8kZ66c&QT zmZIc3D?uy-Ykx&rD{K7$&YeHNh4VGVl zMV*w98VKAWQ)rldO9>^F*@Ki2c#7qN!VAi0*vd{3VY}^Bp}7!M@~}A$%R=J+L)vi_ z6zs#SvlAe%9gMT-C-Vk(b=fM*FPYV+*4m~BMzeRHZ6MEDm(6e>nk{u6c-eJsLm7>* zt+P54Y(N497SKi$r?JRX1pR5fVM*WHQ+TM~?U1?Tj4^LG^H|oAGs9o};5_26UMhV2 E0&>Vy+W-In delta 317 zcmeAdYY^jg;^pOH0D_adKF29+xtez~x@k#BSRrqI76OUg$dYP9jLiUC5+mH3@%Ix zXGmcPX3%8wo4k~*gVA@g9=qY>6n0l`<{}ZGbBn})#N^%V@^5+e_g zp8Sb@JEOtmRUC4RVUtgClmOX^T*9oTKy_i0!#E8XjV8BqsxXF3Uc)($k#Dj$*AW1% CBT4cA diff --git a/Sampling-based Planning/rrt_2D/__pycache__/rrt.cpython-37.pyc b/Sampling-based Planning/rrt_2D/__pycache__/rrt.cpython-37.pyc index b1a9d01d7d45bbb750377dd44da07e69cd385ec4..0b0996c6db79fb1100de3802884066d9532fe3c7 100644 GIT binary patch literal 3513 zcmZ`*&u<&Y72cWs$<>mk6xCMKHdTz)2m@G39Uv(R*Kt!f?himvEf)cZ#ETVYMJ`S5 z(z7FnBFGaBH0Y^4Hiv>d=0E7)GS{4Z?Z~6h!b6nuU#NuDwgeM=z}n2TyltD-X*;HM+ivRiy@j7Tr~^~i(s0R^ zOxU-qC1W(-8Eg%sc4TB1a=l$QvIn`*ZZPYyC43RQvP3ZF?HlOTM2KF9UQ=v|=#|yp zPIpB8IhS9h+j9%E`H9tPOx_BgJb4=5dlWuQ&Q-6FzuNDeXTvP-?oWEfdH5POSVxne z9v&Rf|0EfX26%b*G$~WDe>_O?9Nohf({`G_L=%h#g;E%U?4GM^P`VhEmTuD@`u*cib>)|!Yb%~BpI|` zQ`NFH8*6r#+@)yC9;^(W{BAl`*Uvl(nYHJah@h#~3{W``_AT=PNK)L}5 z1yU0d3Z!jF+Xe~jsI_C9qWSm_D35hes&o_&(p)#YMKXv>f^!Fbs%}w5Fs&*Rb%Mi>P|6>ps(|G&`h`4X z4O0`?0JjKX+MumN`9KfR6CuaM>+cZ8e?y!QjX8T{eLkHbCJXq#Z!g&l?E*3DyA+cd zVz}@SNs^tZBO~(nBEX2VjVxi$9r;W-rca*s3`g=?r0E%!M=AY-;Ss{m#P$_>x6?wB|8`o86zDJ|i(EKxu{Tih- z=P3(#gWYC#_@qhLy&Coo`GB6aFQ_eVqrz@2#S~xIf3?ocA^R$r0?Tt8p>v!R#*z*E zDXGbohB)7W(8U6u@6SM13pQna{-xE2t038sY;xK^_zUNx+Buz0LqI~iy^B$yB&{R^ zR5)4q!F%wFXfe4(qsCCm*l)x8z@O?A!_kAs=o`BB*yJ5D@j8qT?hlGiGAJKFSWjv> z6}aszSMq(V03gdQ&ghkQS5PGV%TKSjVL0RGnDhyKnv%j|yKJ)ieIMWWm?FNx+Y@w< zvv3JzKC;=KISMS%2dmG~Q#PFCOl{>-UN70M1=O*=Z$^Uk2qA0cVhuO0P2DQXL)pUG z+J-3==@BngKK;PF=oF>#uC`04)G)Rt&CsM=#hX?7A+hBl&2m*Q1)mk9Soa?5I`wm zQ1A$XU^CpGi8H2fps)pQQF9j#p4Y^m-S^G&%}3p@{z=c*A7%drr#ziBj*9(bU}FChxRt!!Tsvm{D6XF?nDkpEa}JhPzYR+-S8WK3hDv- EFZV?YWdHyG literal 3673 zcmai1O>Y~=8Qz)w;F2OG+EHD_O;e>GX;{=&ng(rw3bLD|4vGeXQWrHqI9bq~6+cAo z(z7d@B9=W=M%zO#y%=au)TzjS=#S{NbM47RZ@uNz=b5D_Sqac3X6Bu*cV^!Ad7l}+ z*J#une0Tr$-%tP3aGZa!a{jpx?jmKMB2$hMBWEnyLQs~FWm{U=9l31}ZSKf#`yV>W zQ~onY`KPiSpcbeKY8BKfDpb{HPCJaNs&*uGI}T4BXmIa4t+f|e!)XcQ#>o>D;bfHN z`5@`-ciD0NNeFk5vQ1>M(-z8UOC^+qTGuM_jJKbvxXY3JSTenjl)a9u5K6r26i!4* zS_xWEm&(CL+IJiwYsA!u6BA52I!^MIG+q{u4vk1YbMzWUeE#uHKOM(AvwnKK zbD-0n?u>UPoxFc*GU_A=_J6B0>ZN*+_s7}J{mytYV%wum7OU-p<&Yh%^XQ!qSlphR zm@tY4$smuSDi+8BWR9reH*4@9-Ja_~%r>F9MRZk}O=*-NHle6+UvL zq3Cf`kXIJr8U0wvtBYD83V9|`t71fDxCRLcQXLW!qy{7;Nb8W0AZQ|`?4_Mil+mAYq&spY%n8rHZ!Po8L+ z(u4;~Oq!mOjd&?m%}(v1w4&aW}^hRO_rhN{4y6Bg8vfVY0qxzkX1USf#4I` z#59u_5uD{w5)XR)N2%6dWpg!&pO(YSMzhUz!oo*g6@LQGAL1F;yh*$boEk$m^gtiq68;@Fzh^{z`+*fkP z8(1QvI~~y!b+In$5=mUA&)~JvTjzscVmx}xdg~(yIXtxxhz)V%>3g}fb$Djzl}v~d zJa~4|?l0T@vi%Uzcc1? z!1+z|{{Sf?8#y@IP4T*zH3`Mbp6oJ+__5>xS!SG!tvR{K<-+}ga|n9Jbr2SS=BYRX zL|-jweT8O5C8u*+*;~kZQ1Hq296YlSg&4{|IzzbyYYkmU?iMWbXZhH>U&a$y*Bisn z#_yj@(p;0bG}Ym9khkB4K~zgpvt@Hc(V5s^jO9X#Oy5|@hUQJ?~c-L zXO!(hIG@z=O1!#*B-d|41JJ2^<3ZALOSF|xTkWQqaWk|LH0P{Xv$0hYV;N_NVEuiJ z(6>0t&-*c0^9ugHjjo>}W$VZoYE4-eTlfWX)+nvCWQkRb(Tc0v>OE8lK^&R6Jd{21 zAdED9vv7y5a*nEcmlDh-!HzCQx*SLDd%X1k_gi^%zhspartIPLo6rvgkoWZ+WX2=W zw3kGZKD~M#V)`CZM$bFqy6|wGK-`dJOkHNH)q#j$E^Gb$ZwS+Oc^J_9_a3@)5c%B8 z1xOx2QIx@fV=y!d4?Lf#(rO+yaaj?%@hl7pGXA%=jH#Xzu`4dr-SPjKX2m z$<;G3{34|CDGDgn#q1_`{pA_ZUe<%| zIL`a2vJro28#Z1oja|c7>dmM(6yde783v&ju7$O5L$kMwWIx(&g=mJT5_9}-A-*{U0it#L9v^UTfT*}hZH?hVGSyBpz*y76Mol^aaBTmISD;|tOH0;rJihr3=`e-Ji8@jphw8K3|F diff --git a/Sampling-based Planning/rrt_2D/__pycache__/utils.cpython-37.pyc b/Sampling-based Planning/rrt_2D/__pycache__/utils.cpython-37.pyc new file mode 100644 index 0000000000000000000000000000000000000000..032c16243ddc2db3754228a887e09e0b1df4b3ec GIT binary patch literal 2978 zcmai0OK%)S5bo}IdiG)CSc$`PB_7g3Vw1ojLJ>p>kQ|VSFeuU}FdA=M zz097HAHWq(UWp4A&N*}AFZ2y@3KuS%IPq1_dJ|hl%x+azzpJb2tM1#4M!@jhX#V}p zUp2=5pvuYTLAV9V55Ob~ndDLoc*spDLd%qPXq(ar9aFlYo47mPv7b2L3x)?tZOW%y zT9??Ab6Uq9)Q63*VPt2}94>~7oZV&8mEL0}y+H@Fq=7rjh<(CY2?hyAqM8uzmO;3?E)8{^m3*VgF284pJTbY9(x^F+4p z58^aMb-m5Cm81tK?2Sw&9h!CWSrBf4^3T8$h9k33(D5EK9NUsMN?SV8Md?US`Y2tw zAOn((>KW2}3xB$5!n??@gA`Y~qov-LT9gdahqf&tGfbyGQ1v}CGFYArwtr@-f%X5{T<>MWWSvaE zeqUu>6%W@(anZXvnpyg4Jm_YsU-X9ge>y{{BD((Zx^eZ|=t%of)KB|G6s?e1>R^lq z!sUVajhn`XYbS%|#`H0c!qynGgg}Ox@GhH(!vSReG5dP!p*^uC_QaXErB(7(R@%h7 z0yXCnD>>-A7Psm%A@&tez6{2=%Wbj3n|zsH#IGsFE0u9h zd$8lsV0=p;(#e?UWkeq$2!}4kR6G>MD8TV$Rs!9Z8I0pdVQS!HBpeiYf3Dv?)9;rq zwbU3&jKhqDg2Oa^xiA%zpbRA66>2Q4Nl@?|>)1xGgI;ecro0SZVw0K~@6mWuua`B< zalYqj6^N+Y%woG-{VK&Sr?%AQEd8Qzp(*KAtM*-Ca95=}wLl|k*%puZq=6HAIB^5| zG-onSKUplBwdTL%iZ6{k3ahI)3&cYvagDdv`9zVl3Af$ zWEq6Iw$n@vwbM~qo-4{E+NZG+UT!ZL-)ko?VpUN7k($SvZ)u)sF10wg2D%P<4{P=h zv?$O}To3KMmldH;YY;4FQl50r1czSf=Ri)8s2s?SFkqOZ#oJIKZ-FtsBwD=1mqlIp zh?`Z^-{4nx9lrqe@rzaLoVQ4ajzS*+Y!0$RRH_J?@_fZe@FRI@lO2XL$lZSqatyJ6 zXam^nshtCqv$|b6r|M4Wo~i?^Q(LuY-M!WWW^M;-1Fo=i!>F70WkR-5xW|}kgSEX1 zX#^JaDzVpzy+Q0vuuvSlf7X6=tTn%8Ze!34Wh(|{OT7B|V!S-J)O>h{@-2M?_c_Mi zG8jV|5{EZt_E>vpRVkK{>uK_jNx# z3A4GgZA>L-+yUiRz+gHej?_mkGIir#T<1IHrkTJ+rL{e~BZQi3+qTs_ShbByvy10N zAD8r5%`ak~E!vR6jEOp5<>SV)ypOyz*N<#T$=~SrG6Z^HKreb&Aao$n!vX;~o#|t% zs~Dx$h&|IxJk#u~GPGz9l$-b$C%5xS(%Wd)=sr*5VG>0;h@xR8_Xi}`qiAnG9#lPQ zkv62u&45?ELlSvN*O4uYerK5Au_qNBUD1}$sX-4BWiChZ2KhUun$P_Ozu~WfF4SE` zeeD;t7ezAbM3FhBcGCUf=tx~cUuZ*6xP^0V goal_sample_rate: - return Node((np.random.uniform(self.x_range[0], self.x_range[1]), - np.random.uniform(self.y_range[0], self.y_range[1]))) + return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta), + np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta))) + return self.xG def nearest_neighbor(self, node_list, n): @@ -67,12 +72,11 @@ class Rrt: for nd in node_list]))] def new_state(self, node_start, node_end): - node_new = Node((node_start.x, node_start.y)) - dist, theta = self.get_distance_and_angle(node_new, node_end) + dist, theta = self.get_distance_and_angle(node_start, node_end) - dist = min(self.expand_len, dist) - node_new.x += dist * math.cos(theta) - node_new.y += dist * math.sin(theta) + dist = min(self.step_len, dist) + node_new = Node((node_start.x + dist * math.cos(theta), + node_start.y + dist * math.sin(theta))) node_new.parent = node_start return node_new @@ -87,21 +91,6 @@ class Rrt: return path - def check_collision(self, node_end): - for (ox, oy, r) in self.obs_circle: - if math.hypot(node_end.x - ox, node_end.y - oy) <= r: - return True - - for (ox, oy, w, h) in self.obs_rectangle: - if 0 <= (node_end.x - ox) <= w and 0 <= (node_end.y - oy) <= h: - return True - - for (ox, oy, w, h) in self.obs_boundary: - if 0 <= (node_end.x - ox) <= w and 0 <= (node_end.y - oy) <= h: - return True - - return False - @staticmethod def get_distance_and_angle(node_start, node_end): dx = node_end.x - node_start.x @@ -113,11 +102,11 @@ def main(): x_start = (2, 2) # Starting node x_goal = (49, 28) # Goal node - rrt = Rrt(x_start, x_goal, 0.4, 0.05, 2000) + rrt = Rrt(x_start, x_goal, 0.6, 0.05, 3000) path = rrt.planning() if path: - rrt.plotting.animation(rrt.vertex, path) + rrt.plotting.animation(rrt.vertex, path, True) else: print("No Path Found!") diff --git a/Sampling-based Planning/rrt_2D/rrt_star.py b/Sampling-based Planning/rrt_2D/rrt_star.py index 58f7976..3653faf 100644 --- a/Sampling-based Planning/rrt_2D/rrt_star.py +++ b/Sampling-based Planning/rrt_2D/rrt_star.py @@ -13,6 +13,7 @@ sys.path.append(os.path.dirname(os.path.abspath(__file__)) + from rrt_2D import env from rrt_2D import plotting +from rrt_2D import utils class Node: @@ -24,18 +25,19 @@ class Node: class RrtStar: - def __init__(self, x_start, x_goal, expand_len, - goal_sample_rate, search_radius, iter_limit): + def __init__(self, x_start, x_goal, step_len, + goal_sample_rate, search_radius, iter_max): self.xI = Node(x_start) self.xG = Node(x_goal) - self.expand_len = expand_len + self.step_len = step_len self.goal_sample_rate = goal_sample_rate self.search_radius = search_radius - self.iter_limit = iter_limit + self.iter_max = iter_max self.vertex = [self.xI] self.env = env.Env() self.plotting = plotting.Plotting(x_start, x_goal) + self.utils = utils.Utils() self.x_range = self.env.x_range self.y_range = self.env.y_range @@ -44,12 +46,12 @@ class RrtStar: self.obs_boundary = self.env.obs_boundary def planning(self): - for k in range(self.iter_limit): + for k in range(self.iter_max): node_rand = self.random_state(self.goal_sample_rate) node_near = self.nearest_neighbor(self.vertex, node_rand) node_new = self.new_state(node_near, node_rand) - if node_new and not self.check_collision(node_new): + if node_new and not self.utils.is_collision(node_near, node_new): neighbor_index = self.find_near_neighbor(node_new) if neighbor_index: node_new = self.choose_parent(node_new, neighbor_index) @@ -59,10 +61,28 @@ class RrtStar: index = self.search_goal_parent() return self.extract_path(self.vertex[index]) + def check_collision(self, node_end): + for (ox, oy, r) in self.obs_circle: + if math.hypot(node_end.x - ox, node_end.y - oy) <= r: + return True + + for (ox, oy, w, h) in self.obs_rectangle: + if 0 <= (node_end.x - ox) <= w and 0 <= (node_end.y - oy) <= h: + return True + + for (ox, oy, w, h) in self.obs_boundary: + if 0 <= (node_end.x - ox) <= w and 0 <= (node_end.y - oy) <= h: + return True + + return False + def random_state(self, goal_sample_rate): + delta = self.utils.delta + if np.random.random() > goal_sample_rate: - return Node((np.random.uniform(self.x_range[0], self.x_range[1]), - np.random.uniform(self.y_range[0], self.y_range[1]))) + return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta), + np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta))) + return self.xG def nearest_neighbor(self, node_list, n): @@ -70,19 +90,18 @@ class RrtStar: for nd in node_list]))] def new_state(self, node_start, node_goal): - node_new = Node((node_start.x, node_start.y)) - dist, theta = self.get_distance_and_angle(node_new, node_goal) - dist = min(self.expand_len, dist) + dist, theta = self.get_distance_and_angle(node_start, node_goal) - node_new.x += dist * math.cos(theta) - node_new.y += dist * math.sin(theta) + dist = min(self.step_len, dist) + node_new = Node((node_start.x + dist * math.cos(theta), + node_start.y + dist * math.sin(theta))) node_new.parent = node_start return node_new def find_near_neighbor(self, node_new): n = len(self.vertex) + 1 - r = min(self.search_radius * math.sqrt((math.log(n) / n)), self.expand_len) + r = min(self.search_radius * math.sqrt((math.log(n) / n)), self.step_len) dist_table = [math.hypot(nd.x - node_new.x, nd.y - node_new.y) for nd in self.vertex] @@ -103,7 +122,7 @@ class RrtStar: def search_goal_parent(self): dist_list = [math.hypot(n.x - self.xG.x, n.y - self.xG.y) for n in self.vertex] - node_index = [dist_list.index(i) for i in dist_list if i <= self.expand_len] + node_index = [dist_list.index(i) for i in dist_list if i <= self.step_len] if node_index: cost_list = [dist_list[i] + self.vertex[i].cost for i in node_index] @@ -140,21 +159,6 @@ class RrtStar: return path - def check_collision(self, node_end): - for (ox, oy, r) in self.obs_circle: - if math.hypot(node_end.x - ox, node_end.y - oy) <= r: - return True - - for (ox, oy, w, h) in self.obs_rectangle: - if 0 <= (node_end.x - ox) <= w and 0 <= (node_end.y - oy) <= h: - return True - - for (ox, oy, w, h) in self.obs_boundary: - if 0 <= (node_end.x - ox) <= w and 0 <= (node_end.y - oy) <= h: - return True - - return False - @staticmethod def get_distance_and_angle(node_start, node_end): dx = node_end.x - node_start.x @@ -166,7 +170,7 @@ def main(): x_start = (2, 2) # Starting node x_goal = (49, 28) # Goal node - rrt_star = RrtStar(x_start, x_goal, 1, 0.1, 10, 5000) + rrt_star = RrtStar(x_start, x_goal, 1, 0.1, 10, 20000) path = rrt_star.planning() if path: diff --git a/Sampling-based Planning/rrt_2D/utils.py b/Sampling-based Planning/rrt_2D/utils.py new file mode 100644 index 0000000..b8a4dc4 --- /dev/null +++ b/Sampling-based Planning/rrt_2D/utils.py @@ -0,0 +1,106 @@ +""" +utils for collision check +@author: huiming zhou +""" + +import math +import numpy as np +import pyrr +import os +import sys + +sys.path.append(os.path.dirname(os.path.abspath(__file__)) + + "/../../Sampling-based Planning/") + +from rrt_2D import env +from rrt_2D.rrt import Node + + +class Utils: + def __init__(self): + self.env = env.Env() + + self.delta = 0.2 + self.obs_circle = self.env.obs_circle + self.obs_rectangle = self.env.obs_rectangle + self.obs_boundary = self.env.obs_boundary + self.obs_vertex = self.get_obs_vertex() + + def get_obs_vertex(self): + delta = self.delta + obs_list = [] + + for (ox, oy, w, h) in self.obs_rectangle: + vertex_list = [[ox - delta, oy - delta], + [ox + w + delta, oy - delta], + [ox + w + delta, oy + h + delta], + [ox - delta, oy + h + delta]] + obs_list.append(vertex_list) + + return obs_list + + def is_intersect_segment(self, start, end, a, b): + o, d = self.get_ray(start, end) + + v1 = [o[0] - a[0], o[1] - a[1]] + v2 = [b[0] - a[0], b[1] - a[1]] + v3 = [-d[1], d[0]] + + div = np.dot(v2, v3) + + if div == 0: + div = 0.01 + + t1 = np.linalg.norm(np.cross(v2, v1)) / div + t2 = np.dot(v1, v3) / div + + if t1 >= 0 and 0 <= t2 <= 1: + shot = Node((o[0] + t1 * d[0], o[1] + t1 * d[1])) + dist_obs = self.get_dist(start, shot) + dist_seg = self.get_dist(start, end) + if dist_obs <= dist_seg: + return True + + return False + + def is_collision(self, start, end): + if self.is_inside_obs(start) or self.is_inside_obs(end): + return True + + for (v1, v2, v3, v4) in self.obs_vertex: + if self.is_intersect_segment(start, end, v1, v2) \ + or self.is_intersect_segment(start, end, v2, v3) \ + or self.is_intersect_segment(start, end, v3, v4) \ + or self.is_intersect_segment(start, end, v4, v1): + return True + + return False + + def is_inside_obs(self, node): + delta = self.delta + + for (x, y, r) in self.obs_circle: + if math.hypot(node.x - x, node.y - y) <= r + delta: + return True + + for (x, y, w, h) in self.obs_rectangle: + if 0 <= node.x - (x - delta) <= w + 2 * delta \ + and 0 <= node.y - (y - delta) <= h + 2 * delta: + return True + + for (x, y, w, h) in self.obs_boundary: + if 0 <= node.x - (x - delta) <= w + 2 * delta \ + and 0 <= node.y - (y - delta) <= h + 2 * delta: + return True + + return False + + @staticmethod + def get_ray(start, end): + orig = [start.x, start.y] + direc = [end.x - start.x, end.y - start.y] + return orig, direc + + @staticmethod + def get_dist(start, end): + return math.hypot(end.x - start.x, end.y - start.y)