From d08ff124a4b3a6f31fc0428dd36d139e8b85ccdd Mon Sep 17 00:00:00 2001 From: zhm-real Date: Thu, 30 Jul 2020 12:57:01 -0700 Subject: [PATCH] update --- .idea/.gitignore | 4 +- .../inspectionProfiles/profiles_settings.xml | 6 - .idea/misc.xml | 4 - .idea/modules.xml | 8 - .idea/path-planning-algorithms.iml | 11 - .idea/vcs.xml | 7 - .../__pycache__/draw.cpython-37.pyc | Bin 0 -> 1824 bytes .../__pycache__/dubins_path.cpython-37.pyc | Bin 0 -> 8258 bytes CurvesGenerator/dubins_path.py | 2 +- Sampling_based_Planning/.idea/.gitignore | 3 - .../.idea/Sampling-based Planning.iml | 8 - .../.idea/dictionaries/zhou.xml | 8 - .../inspectionProfiles/profiles_settings.xml | 6 - Sampling_based_Planning/.idea/misc.xml | 4 - Sampling_based_Planning/.idea/modules.xml | 8 - Sampling_based_Planning/.idea/vcs.xml | 6 - .../__pycache__/plotting.cpython-37.pyc | Bin 3778 -> 3765 bytes .../rrt_2D/__pycache__/rrt.cpython-37.pyc | Bin 3609 -> 3572 bytes .../rrt_2D/__pycache__/utils.cpython-37.pyc | Bin 3676 -> 3687 bytes .../rrt_2D/dubins_rrt_star.py | 278 ++++++++++++++++- .../.idea/Search-based Planning.iml | 8 - .../.idea/dictionaries/Huiming_Zhou.xml | 10 - .../inspectionProfiles/profiles_settings.xml | 6 - Search_based_Planning/.idea/misc.xml | 4 - Search_based_Planning/.idea/modules.xml | 8 - Search_based_Planning/.idea/vcs.xml | 6 - Search_based_Planning/.idea/workspace.xml | 293 ------------------ .../__pycache__/plotting.cpython-37.pyc | Bin 5246 -> 5246 bytes .../__pycache__/env.cpython-37.pyc | Bin 1141 -> 0 bytes .../__pycache__/plotting.cpython-35.pyc | Bin 2899 -> 0 bytes .../__pycache__/plotting.cpython-37.pyc | Bin 3402 -> 0 bytes .../__pycache__/queue.cpython-35.pyc | Bin 2667 -> 0 bytes .../__pycache__/queue.cpython-37.pyc | Bin 2652 -> 0 bytes 33 files changed, 274 insertions(+), 424 deletions(-) delete mode 100644 .idea/inspectionProfiles/profiles_settings.xml delete mode 100644 .idea/misc.xml delete mode 100644 .idea/modules.xml delete mode 100644 .idea/path-planning-algorithms.iml delete mode 100644 .idea/vcs.xml create mode 100644 CurvesGenerator/__pycache__/draw.cpython-37.pyc create mode 100644 CurvesGenerator/__pycache__/dubins_path.cpython-37.pyc delete mode 100644 Sampling_based_Planning/.idea/.gitignore delete mode 100644 Sampling_based_Planning/.idea/Sampling-based Planning.iml delete mode 100644 Sampling_based_Planning/.idea/dictionaries/zhou.xml delete mode 100644 Sampling_based_Planning/.idea/inspectionProfiles/profiles_settings.xml delete mode 100644 Sampling_based_Planning/.idea/misc.xml delete mode 100644 Sampling_based_Planning/.idea/modules.xml delete mode 100644 Sampling_based_Planning/.idea/vcs.xml delete mode 100644 Search_based_Planning/.idea/Search-based Planning.iml delete mode 100644 Search_based_Planning/.idea/dictionaries/Huiming_Zhou.xml delete mode 100644 Search_based_Planning/.idea/inspectionProfiles/profiles_settings.xml delete mode 100644 Search_based_Planning/.idea/misc.xml delete mode 100644 Search_based_Planning/.idea/modules.xml delete mode 100644 Search_based_Planning/.idea/vcs.xml delete mode 100644 Search_based_Planning/.idea/workspace.xml delete mode 100644 Search_based_Planning/__pycache__/env.cpython-37.pyc delete mode 100644 Search_based_Planning/__pycache__/plotting.cpython-35.pyc delete mode 100644 Search_based_Planning/__pycache__/plotting.cpython-37.pyc delete mode 100644 Search_based_Planning/__pycache__/queue.cpython-35.pyc delete mode 100644 Search_based_Planning/__pycache__/queue.cpython-37.pyc diff --git a/.idea/.gitignore b/.idea/.gitignore index 26d3352..0e40fe8 100644 --- a/.idea/.gitignore +++ b/.idea/.gitignore @@ -1,3 +1,3 @@ + # Default ignored files -/shelf/ -/workspace.xml +/workspace.xml \ No newline at end of file diff --git a/.idea/inspectionProfiles/profiles_settings.xml b/.idea/inspectionProfiles/profiles_settings.xml deleted file mode 100644 index 105ce2d..0000000 --- a/.idea/inspectionProfiles/profiles_settings.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - - \ No newline at end of file diff --git a/.idea/misc.xml b/.idea/misc.xml deleted file mode 100644 index a2e120d..0000000 --- a/.idea/misc.xml +++ /dev/null @@ -1,4 +0,0 @@ - - - - \ No newline at end of file diff --git a/.idea/modules.xml b/.idea/modules.xml deleted file mode 100644 index e889cbd..0000000 --- a/.idea/modules.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - \ No newline at end of file diff --git a/.idea/path-planning-algorithms.iml b/.idea/path-planning-algorithms.iml deleted file mode 100644 index 95f5d6e..0000000 --- a/.idea/path-planning-algorithms.iml +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - \ No newline at end of file diff --git a/.idea/vcs.xml b/.idea/vcs.xml deleted file mode 100644 index f5eb5fc..0000000 --- a/.idea/vcs.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - \ No newline at end of file diff --git a/CurvesGenerator/__pycache__/draw.cpython-37.pyc b/CurvesGenerator/__pycache__/draw.cpython-37.pyc new file mode 100644 index 0000000000000000000000000000000000000000..2abd05e1205a014f0880bf7048dd746a31909729 GIT binary patch literal 1824 zcmb7EOK;mo5Z(u0lBlO0CynbOXnTkPG>0CGpiS+|ks?i?0!hW3jc6P)a&bP$;-Kf(WMgd%92VlXXKu6fk_t_=*?SeWIKP zWZkT6h-|6Gx~J{hck-+Av+^uY0Oz;<&2f_O?tFw_n(=9aU3}5mR;L1KP?H z)-M(%W~HqVaS$6lQ;}@Jq#yPF;CIjI(KP7X(wW ze9L;uDc~kJL`l^u8w6BWuCkCfIci(cd0UpNiuL~yTi?Vs)oyIXf*6%;a#liaE8j35 zgxpchJ273=x)akY;1+{iP1OYXtu@-P?z{%k(!ZNoQ3eLH> zwV-qo;TB3qw0)gM!<#;n_>rUy(0646ZLG1ZA>gemeS{{05$T?6q1{I40BF977V-ey z9zfuh0y@@g33GKEcpKt;Yh4A_GCV#Htd&8qfdSj_r-nydhNp%^dG4xyOi#~)LvlzCch@UgT1y&5-Z&d4c9hNTO1p`agkl6z#sUp)I>V{v zY%iNL!|J9qTyzHrxN&r_fenz0eK83b0elaF~N{{ry*URdxOEzh0@;91Yk1uxGsg_>`voC%ud>9l$L-(YH_t&D8`G zJY+6YHFtGY8{4L9F>2Gp(zflE70g3tyW&=u_NpcfVLsA?xzF9Iute#R=GKHQ%BbtY z5f#)^qAF^r8=@|zP*017m_|J#X2dM&SuIl^ygD{Yq9tAn@` z2ZMf>Fyo5@xP>QbqVP5Rw&rsBWel$i9RnJf@y5$@r%@y<(L62T zC|RPT41UnGyNnrZg~)G^wEBWr+$K z#Wyrbr2EO6w_jTSLgdS6{k7d-JLvb8-1~#w^*esF6%U5%uLkk`-HlJXuf4ubKmA77 z>G#oT{r0YW%a305`@ZbN1Gz5L#&|>YxjNj-Y|jh&LF{>T%n>ObVODWJtkGXz9rswl zKA?NQv+a9c=6D|dWH%(Z>Ulf6oiJ~aRa!ZXbfT!xEAd$kPeglXGxgqOfa#)|I;cH; z1;FT1qQ%++wq-~_f)U)*KCh+P4!^67F6XTkZA+8akJ~>-?MrvHg!Y`T^e_-zx7r*# zkAKeiFaRf*@qIt;oLOL28V24qZx}4#i()UTY=+nI$XVb{EtaTs%m9-G#@YecfEIfZ zxQp5$7Tr3#7+p5-wVXzgnVaEYAlppN5bAi}Xv;DSdwgved=jwQHI2zRz+KuYx@fN_ z3;9>PV0!T4Axm`yF+fJXBtWKu2xsNEgte$|Nw5SQKdUWZ$w`S8OqiwyUN%}<6Zq6l zY{9YDv4D!etwY@Pp1x_U(VTi>V;mb2pf70jO=?{z4zWRrE(KQw35%F80*e&z7OSyXo`Fvj&flnGhO=KT&xu3fO>** z%WJEDgbCi4a?diQmR~@h^C|Z(T7Ef{!+w)C{6A7Iv7iw_Hxe*t+jHoalnW69%?kK@ zddcT$%O{ehsH9{|QEp7HTOTjIBy?UI^Af!P=aTlM8eCml7U=c9^t!^TtjguDpwB6K zm4HxY^uGaoG4l`}V@b=^pfE0iW-+YPJ`*N9ilne&a8oA|X9&>df!U;0SY*!> zRmKXj1YeF6VnIogz?KF}Ru=k2Sg(m?gD;3)PTp4FiJE0twiB~K`U-lrZYhdRs8;|> zw+i6_e*4g1XSDA*2yRrV8pbd{QV^msj_& zjELqxY3rVcm7C!$_99ujt7to)#NS2B$4ughrTeHPR<`W~k#jeOh@6vnOki?e_F)b_ zb|NePpY-k%M3yfCdrV~cX?p!|CUZjLP(p&?&rk(_21Gt8-$M|DaOh%AbBjD->}n~|u4@pcaSKnzEw*a0AS&q5 zKLDrj1kNgr0Oydy{p4~eCXOvAKrPk&CA1l6`_k#Qibk!9->eGU+@zvJ8}q5Rs&Z>< zY8HiovCMn0Q$6bVi`t2G*8%4%%~$j@xVI4hV%78cS{}olCp>YF`YWY&fPGe2PzLmW zHjKA|up>5lIpj)1Ej*#n3k5J%5ZVzr!$+{(@3o6KLU~gplkKTY5BefwVaB#8;2Cu@ zJ{aNI%Zve{O@u#qL)>Q!>CF)VEHOpIk*kMdM8%$~_Xz5Vy|cVg3Hq@whl3Da@z*e2 zMAlbhHay%5%t4%FvnD-!i9M~vy!r?4Gu+r?s@6 zP9=3U^0!ZZ3odXLiR-Dfsnn4>NE_*NI+Hk}gn-sbrbKDW1b4gusQ}#4&==4mHc8l9 zhH9yRKdW#(X8Q<|a%i*+Z8VGhGLrf}+hXYVr!*#lUS*V4ryYc3>SM_D@PhZ5MSpud%X!$I6PeD+#Hr9O+=2Ni0tBAtBcA zL=;x{z(@D&6lt8lkf8hg-}5v5VKB^XASsYc9}b4?W}Xcw zp<6G$ALWU&-=OAl6#GLj3Lg4yc{spWUVnF6{uWI~p`iS2Dy~y;4MoO8#=}s4mJq+L z5P|$ndifj`pC=3-iZe9H=Fa7be?af(4^U|EgJxKM8}N-t`&F66EpG5D(2xXOLVf^F zFX*@n?mCyRW3-9RB%?n`_xunqxiQZn>L_B$VOTF zI$EK*(%H0`ObczxmS5ZvaV43C7OEsOq_t5)CqZ-1K~K#=(+Z`3W|QWoaXfZ1msWNJ zWV;IOQt#>Md|Zud$vk|k`DBU`Px~f%FQlz>F};v1B&}o-_{ydUiPl$$OE%la3a)lN z&j*?J_lAQ!4Wzsym2y#`y(BxOLJ#GfD(^+*-8ym3SG38yRpHu0O0^;{>y|hCD294Ks)=vVGj#dJPJyPhuUjhmW-QLw zFk?H4Z!rnJk+0Gu*2p7ul39CsZDSD%%31cbYRH;uJfwNOO*z>11~TXc{U2kTh$2M| zHrVD3Wq}*uHnPJ_)Eu6AZmAv4DgE&$==)z^{R&-wf3r(TXSyiSNX$mcHToV3%51|p zfHdUTsJtSM2;Kpk9q>bBH-!G@aEYkgyw)trt-&yess;i16h%VHX_uyZc!O>QuWWly&IdJE3h|R?_|pU9IMl|GaUyT znWK2XM@n91RwwdAcRS<#A@ZGZr@NK0Hx->#*1bKK@d#`l?a3~tl&e&b0hMo3@dYY~ z=Up@O#p;6r(j2K-|xJSvo} zVENQ4&D}WrpDo&iJfjLd2!E1vBxg(fX+U%%BfpKIX!Yu%=(mjLHlFBRd`$;9DwPZ( zBgnUVb_%Z}g)cz2^1*njr^e{rn7kLzOCV?`5A)vNL6;&zftOMN8(ReouZ46eG;7{g zU8K}GB7wvd&H_9()f?Xa-!s{-JTmaC$d&XfpV`YObk+X?%}; zf{;^*G$nQcCwkNev*1*ZRsd*L%oflLo$%4=Ue72nQ^k6lqD2^tJ+Jq z>%SEYLO(9P{?Sc&1A}L#Pse5t7uSOAUBpxf!O$=O$vhz9G|M~vhrH~84b1&b%WvF)d+hL zbi%7KJlJS+Ah#t26OvfUZJs%jy-M3Hzew#m6=YKKY|p0%dX5TWfin(_il&u&nCo>? zhw6u9jiBRAQ;<@#_VxV8_6E%p(U=C@0qh^$M%}R;+py<~d)A({FWcAcr<|sJ&1U}# Dqd2}O literal 0 HcmV?d00001 diff --git a/CurvesGenerator/dubins_path.py b/CurvesGenerator/dubins_path.py index bbc2466..33361fe 100644 --- a/CurvesGenerator/dubins_path.py +++ b/CurvesGenerator/dubins_path.py @@ -6,7 +6,7 @@ import math import numpy as np import matplotlib.pyplot as plt from scipy.spatial.transform import Rotation as Rot -import draw +import CurvesGenerator.draw as draw # class for PATH element diff --git a/Sampling_based_Planning/.idea/.gitignore b/Sampling_based_Planning/.idea/.gitignore deleted file mode 100644 index 26d3352..0000000 --- a/Sampling_based_Planning/.idea/.gitignore +++ /dev/null @@ -1,3 +0,0 @@ -# Default ignored files -/shelf/ -/workspace.xml diff --git a/Sampling_based_Planning/.idea/Sampling-based Planning.iml b/Sampling_based_Planning/.idea/Sampling-based Planning.iml deleted file mode 100644 index c444878..0000000 --- a/Sampling_based_Planning/.idea/Sampling-based Planning.iml +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - \ No newline at end of file diff --git a/Sampling_based_Planning/.idea/dictionaries/zhou.xml b/Sampling_based_Planning/.idea/dictionaries/zhou.xml deleted file mode 100644 index 6638eb0..0000000 --- a/Sampling_based_Planning/.idea/dictionaries/zhou.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - - huiming - zhou - - - \ No newline at end of file diff --git a/Sampling_based_Planning/.idea/inspectionProfiles/profiles_settings.xml b/Sampling_based_Planning/.idea/inspectionProfiles/profiles_settings.xml deleted file mode 100644 index 105ce2d..0000000 --- a/Sampling_based_Planning/.idea/inspectionProfiles/profiles_settings.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - - \ No newline at end of file diff --git a/Sampling_based_Planning/.idea/misc.xml b/Sampling_based_Planning/.idea/misc.xml deleted file mode 100644 index a2e120d..0000000 --- a/Sampling_based_Planning/.idea/misc.xml +++ /dev/null @@ -1,4 +0,0 @@ - - - - \ No newline at end of file diff --git a/Sampling_based_Planning/.idea/modules.xml b/Sampling_based_Planning/.idea/modules.xml deleted file mode 100644 index caf2a07..0000000 --- a/Sampling_based_Planning/.idea/modules.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - \ No newline at end of file diff --git a/Sampling_based_Planning/.idea/vcs.xml b/Sampling_based_Planning/.idea/vcs.xml deleted file mode 100644 index 6c0b863..0000000 --- a/Sampling_based_Planning/.idea/vcs.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - \ No newline at end of file diff --git a/Sampling_based_Planning/rrt_2D/__pycache__/plotting.cpython-37.pyc b/Sampling_based_Planning/rrt_2D/__pycache__/plotting.cpython-37.pyc index 0c955c0b729c9484107de5bdd5248a40bee97b7e..395452ff9abc041faba2932616e0c5532f8ca481 100644 GIT binary patch delta 750 zcmZva&ubGw6vs1@bQ6+nlD28pN?RyI6WYW!_^U001iEehon-Ob0 zc#+bB7oD4U_28u-q5pvYg113XLGkQWJow&5VJ$AqXTJ01{g^j9?!u$}oc5UG_t#{uqu!(wn&l1uGSvU5qmT$XnBalYtpF3)vFaLE_ulUCtnS(tr zNsXz4=zW$|KcW+*VKkJ6Acd3WcNcI}VU(Qmgiv7rVywVfUe%4Mllij!L!-dhv^t85 zR47EvCF(|j^31B|iSqdol4A%x)bJmAQpY^M>4b}rL$3chQDf|qnoBO0FO!8KN*u0` zzDkVR(tz{^TE*j1dvJ{raw<0@@t|Qxi6LMr9 zC(r{3H5()})hDx7q)XHzU0e2CU%H*$O3#*!wj-cO8{8K(3Lt?b@D)%L;(BsyBdoh3J24z0REb4Plj58Qn6xxq_|1D2em2-SW$k_D%EKWR3{*Zlj8R`aM&`kknIpanRD27+aJt4UtQDNh9+>Um=Ahy^OUo zL$plR6Rqedj8!^}+U~epY49nt!`KyRWH+s=l)@0I95#txBhKJD@x)g2c|!)-Ah$(y zwCS&G`7+$5z%~N*f0lR!L8W~++!&&**yC}lIcev9%#(kWD_&cN1|=r;llG)|w)iiI z#qAOwS5a&aLD1j`dg_#00C$mM6Blvvp3JPam8%h+PBcOYb=woPDkRQ#RRXQ?N3Y@s- qN3q{MsP&w<)o}%^(E^Wz(!_~?S!v~0FX(^oB2Z7D{nzt#e)$iLGNg9^ 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 51815730644f0748b22f54c385acc05870799a1e..ab74d2f92c0cb4bf119faee66878975b6763feff 100644 GIT binary patch delta 504 zcmZ8d&ubGw6rMNPWGA~jYZLsD#i}(>OGs&h(b_|4X+#TBr6q!}Lc^3_@E03G6NkmCe9}15e6kTGLf7$2@eJHT+c|U>GTrKlWFy2^&N;Y?ADt&bGaGv4 zxJ4vFU9MSrK9d$uxffs!`)&)S@Pm6DDtO9!3ODh&cgHB7y~b>q(>l?Mnf>1_Q!5sm zDl&x~-v9v%EII;=T5y``Wo+|mIr})xtA}y#v2K`LFXBEQ56-tm-0Pb2{deCf9P%+( b#`nBFS}}HuGB|#;V&&Aqw%AMJAIo>O((`}RDl!Pachx2%uZ^F5wUH%7P{OG0i7ThQkY-_(|RG=dq zP0d3I>#396HF~pg&zy{-DRK*_)5mZZSH&%;;5%_0mhrndger!aYN1SwC>d_~p)#d2 z@u1D1!!%*{@JHsrS&W%hSA|;vFYesUC_(QxwV3|z+>zB$^bu6(dE^*aVDVuP+=2_-G;mB# xW#fyZWbmxhePJpTn;O<-0aExz=AnUy@=AV!V2cYF7{5q*7swJ`+Pg2LfWJk2gx3H7 diff --git a/Sampling_based_Planning/rrt_2D/__pycache__/utils.cpython-37.pyc b/Sampling_based_Planning/rrt_2D/__pycache__/utils.cpython-37.pyc index 1e50eefe9d218bb43d548b9996b76c3401b38a90..137a52c43fae1e51cf7bcebc48abe6dc9762903a 100644 GIT binary patch delta 191 zcmca3^IV44iINIx(PVx;UjP=SIf(!O delta 180 zcmaDZb4P~PiI|FEcMa zrl_bS-pEB?PY;BG6LSl4fTB=Ei*uOR8KXAma$aU+GXff0WIWl8TLH$*;a&w}%J6gv qn1K|Sg9xb7Z9MgiCX>y0QyI-B&*638V}n>#!~|3m#Wh)q&ldpvA1`|V diff --git a/Sampling_based_Planning/rrt_2D/dubins_rrt_star.py b/Sampling_based_Planning/rrt_2D/dubins_rrt_star.py index 08337b7..d5bc3ef 100644 --- a/Sampling_based_Planning/rrt_2D/dubins_rrt_star.py +++ b/Sampling_based_Planning/rrt_2D/dubins_rrt_star.py @@ -17,6 +17,7 @@ sys.path.append(os.path.dirname(os.path.abspath(__file__)) + from Sampling_based_Planning.rrt_2D import env, plotting, utils import CurvesGenerator.dubins_path as dubins +import CurvesGenerator.draw as draw class Node: @@ -24,23 +25,24 @@ class Node: self.x = x self.y = y self.yaw = yaw + self.parent = None + self.cost = 0.0 self.path_x = [] self.path_y = [] self.paty_yaw = [] - self.parent = None - self.cost = 0.0 -class RrtStarSmart: - def __init__(self, sx, sy, syaw, gx, gy, gyaw, step_len, +class DubinsRRTStar: + def __init__(self, sx, sy, syaw, gx, gy, gyaw, vehicle_radius, step_len, goal_sample_rate, search_radius, iter_max): self.s_start = Node(sx, sy, syaw) self.s_goal = Node(gx, gy, gyaw) + self.vr = vehicle_radius self.step_len = step_len self.goal_sample_rate = goal_sample_rate self.search_radius = search_radius self.iter_max = iter_max - self.curv = 1.0 + self.curv = 1 self.env = env.Env() self.utils = utils.Utils() @@ -49,9 +51,271 @@ class RrtStarSmart: self.delta = self.utils.delta self.x_range = self.env.x_range self.y_range = self.env.y_range - self.obs_circle = self.env.obs_circle - self.obs_rectangle = self.env.obs_rectangle + self.obs_circle = self.obs_circle() self.obs_boundary = self.env.obs_boundary + self.utils.update_obs(self.obs_circle, self.obs_boundary, []) self.V = [self.s_start] self.path = None + + def planning(self): + + for i in range(self.iter_max): + print("Iter:", i, ", number of nodes:", len(self.V)) + rnd = self.Sample() + node_nearest = self.Nearest(self.V, rnd) + new_node = self.Steer(node_nearest, rnd) + + if new_node and not self.is_collision(new_node): + near_indexes = self.Near(self.V, new_node) + new_node = self.choose_parent(new_node, near_indexes) + + if new_node: + self.V.append(new_node) + self.rewire(new_node, near_indexes) + + if i % 5 == 0: + self.draw_graph() + + last_index = self.search_best_goal_node() + + path = self.generate_final_course(last_index) + print("get!") + px = [s[0] for s in path] + py = [s[1] for s in path] + plt.plot(px, py, '-r') + plt.pause(0.01) + plt.show() + + def draw_graph(self, rnd=None): + plt.cla() + # for stopping simulation with the esc key. + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) + for node in self.V: + if node.parent: + plt.plot(node.path_x, node.path_y, "-g") + + self.plot_grid("dubins rrt*") + plt.plot(self.s_start.x, self.s_start.y, "xr") + plt.plot(self.s_goal.x, self.s_goal.y, "xr") + plt.grid(True) + self.plot_start_goal_arrow() + plt.pause(0.01) + + def plot_start_goal_arrow(self): + draw.Arrow(self.s_start.x, self.s_start.y, self.s_start.yaw, 2, "darkorange") + draw.Arrow(self.s_goal.x, self.s_goal.y, self.s_goal.yaw, 2, "darkorange") + + def generate_final_course(self, goal_index): + print("final") + path = [[self.s_goal.x, self.s_goal.y]] + node = self.V[goal_index] + while node.parent: + for (ix, iy) in zip(reversed(node.path_x), reversed(node.path_y)): + path.append([ix, iy]) + node = node.parent + path.append([self.s_start.x, self.s_start.y]) + return path + + def calc_dist_to_goal(self, x, y): + dx = x - self.s_goal.x + dy = y - self.s_goal.y + return math.hypot(dx, dy) + + def search_best_goal_node(self): + dist_to_goal_list = [self.calc_dist_to_goal(n.x, n.y) for n in self.V] + goal_inds = [dist_to_goal_list.index(i) for i in dist_to_goal_list if i <= self.step_len] + + safe_goal_inds = [] + for goal_ind in goal_inds: + t_node = self.Steer(self.V[goal_ind], self.s_goal) + if t_node and not self.is_collision(t_node): + safe_goal_inds.append(goal_ind) + + if not safe_goal_inds: + return None + + min_cost = min([self.V[i].cost for i in safe_goal_inds]) + for i in safe_goal_inds: + if self.V[i].cost == min_cost: + return i + + return None + + def rewire(self, new_node, near_inds): + for i in near_inds: + near_node = self.V[i] + edge_node = self.Steer(new_node, near_node) + if not edge_node: + continue + edge_node.cost = self.calc_new_cost(new_node, near_node) + + no_collision = ~self.is_collision(edge_node) + improved_cost = near_node.cost > edge_node.cost + + if no_collision and improved_cost: + self.V[i] = edge_node + self.propagate_cost_to_leaves(new_node) + + def choose_parent(self, new_node, near_inds): + if not near_inds: + return None + + costs = [] + for i in near_inds: + near_node = self.V[i] + t_node = self.Steer(near_node, new_node) + if t_node and not self.is_collision(t_node): + costs.append(self.calc_new_cost(near_node, new_node)) + else: + costs.append(float("inf")) # the cost of collision node + min_cost = min(costs) + + if min_cost == float("inf"): + print("There is no good path.(min_cost is inf)") + return None + + min_ind = near_inds[costs.index(min_cost)] + new_node = self.Steer(self.V[min_ind], new_node) + + return new_node + + def calc_new_cost(self, from_node, to_node): + d, _ = self.get_distance_and_angle(from_node, to_node) + return from_node.cost + d + + def propagate_cost_to_leaves(self, parent_node): + for node in self.V: + if node.parent == parent_node: + node.cost = self.calc_new_cost(parent_node, node) + self.propagate_cost_to_leaves(node) + + @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) + + def Near(self, nodelist, node): + n = len(nodelist) + 1 + r = min(self.search_radius * math.sqrt((math.log(n)) / n), self.step_len) + + dist_table = [(nd.x - node.x) ** 2 + (nd.y - node.y) ** 2 for nd in nodelist] + node_near_ind = [ind for ind in range(len(dist_table)) if dist_table[ind] <= r ** 2] + + return node_near_ind + + def Steer(self, node_start, node_end): + sx, sy, syaw = node_start.x, node_start.y, node_start.yaw + gx, gy, gyaw = node_end.x, node_end.y, node_end.yaw + maxc = self.curv + + path = dubins.calc_dubins_path(sx, sy, syaw, gx, gy, gyaw, maxc) + + if len(path.x) <= 1: + return None + + node_new = Node(path.x[-1], path.y[-1], path.yaw[-1]) + node_new.path_x = path.x + node_new.path_y = path.y + node_new.path_yaw = path.yaw + node_new.cost = node_start.cost + path.L + node_new.parent = node_start + + return node_new + + def Sample(self): + delta = self.utils.delta + + if random.random() > self.goal_sample_rate: + return Node(random.uniform(self.x_range[0] + delta, self.x_range[1] - delta), + random.uniform(self.y_range[0] + delta, self.y_range[1] - delta), + random.uniform(-math.pi, math.pi)) + else: + return self.s_goal + + @staticmethod + def Nearest(nodelist, n): + return nodelist[int(np.argmin([(nd.x - n.x) ** 2 + (nd.y - n.y) ** 2 + for nd in nodelist]))] + + def is_collision(self, node): + for ox, oy, r in self.obs_circle: + dx = [ox - x for x in node.path_x] + dy = [oy - y for y in node.path_y] + dist = np.hypot(dx, dy) + + if min(dist) < r + self.delta: + return True + + return False + + def animation(self): + self.plot_grid("dubins rrt*") + self.plot_arrow() + plt.show() + + def plot_arrow(self): + draw.Arrow(self.s_start.x, self.s_start.y, self.s_start.yaw, 2.5, "darkorange") + draw.Arrow(self.s_goal.x, self.s_goal.y, self.s_goal.yaw, 2.5, "darkorange") + + def plot_grid(self, name): + + for (ox, oy, w, h) in self.obs_boundary: + self.ax.add_patch( + patches.Rectangle( + (ox, oy), w, h, + edgecolor='black', + facecolor='black', + fill=True + ) + ) + + for (ox, oy, r) in self.obs_circle: + self.ax.add_patch( + patches.Circle( + (ox, oy), r, + edgecolor='black', + facecolor='gray', + fill=True + ) + ) + + plt.plot(self.s_start.x, self.s_start.y, "bs", linewidth=3) + plt.plot(self.s_goal.x, self.s_goal.y, "gs", linewidth=3) + + plt.title(name) + plt.axis("equal") + + @staticmethod + def obs_circle(): + obs_cir = [ + [10, 10, 3], + [15, 22, 3], + [22, 8, 2.5], + [26, 16, 2], + [37, 10, 3], + [37, 23, 3], + [45, 15, 2] + ] + + return obs_cir + + +def main(): + sx, sy, syaw = 5, 5, np.deg2rad(90) + gx, gy, gyaw = 45, 25, np.deg2rad(0) + goal_sample_rate = 0.1 + search_radius = 50.0 + step_len = 30.0 + iter_max = 250 + vehicle_radius = 2.0 + + drrtstar = DubinsRRTStar(sx, sy, syaw, gx, gy, gyaw, vehicle_radius, step_len, + goal_sample_rate, search_radius, iter_max) + drrtstar.planning() + + +if __name__ == '__main__': + main() diff --git a/Search_based_Planning/.idea/Search-based Planning.iml b/Search_based_Planning/.idea/Search-based Planning.iml deleted file mode 100644 index c444878..0000000 --- a/Search_based_Planning/.idea/Search-based Planning.iml +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - \ No newline at end of file diff --git a/Search_based_Planning/.idea/dictionaries/Huiming_Zhou.xml b/Search_based_Planning/.idea/dictionaries/Huiming_Zhou.xml deleted file mode 100644 index f550db9..0000000 --- a/Search_based_Planning/.idea/dictionaries/Huiming_Zhou.xml +++ /dev/null @@ -1,10 +0,0 @@ - - - - astar - dijk - huiming - zhou - - - \ No newline at end of file diff --git a/Search_based_Planning/.idea/inspectionProfiles/profiles_settings.xml b/Search_based_Planning/.idea/inspectionProfiles/profiles_settings.xml deleted file mode 100644 index 105ce2d..0000000 --- a/Search_based_Planning/.idea/inspectionProfiles/profiles_settings.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - - \ No newline at end of file diff --git a/Search_based_Planning/.idea/misc.xml b/Search_based_Planning/.idea/misc.xml deleted file mode 100644 index a2e120d..0000000 --- a/Search_based_Planning/.idea/misc.xml +++ /dev/null @@ -1,4 +0,0 @@ - - - - \ No newline at end of file diff --git a/Search_based_Planning/.idea/modules.xml b/Search_based_Planning/.idea/modules.xml deleted file mode 100644 index 1bc9d64..0000000 --- a/Search_based_Planning/.idea/modules.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - \ No newline at end of file diff --git a/Search_based_Planning/.idea/vcs.xml b/Search_based_Planning/.idea/vcs.xml deleted file mode 100644 index 6c0b863..0000000 --- a/Search_based_Planning/.idea/vcs.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - \ No newline at end of file diff --git a/Search_based_Planning/.idea/workspace.xml b/Search_based_Planning/.idea/workspace.xml deleted file mode 100644 index 669fe32..0000000 --- a/Search_based_Planning/.idea/workspace.xml +++ /dev/null @@ -1,293 +0,0 @@ - - - - - - - - - - - - - - - - - - -