From 6e7d74a4b721e3fc03bf713aeec74b5a9100e448 Mon Sep 17 00:00:00 2001 From: Hari Date: Wed, 22 Nov 2023 15:47:14 +0100 Subject: [PATCH] added example with visualization --- examples/03_view_map.py | 71 ++++++++++++++++++++++++++++++++++++++++ examples/sample_map.npy | Bin 0 -> 296480 bytes 2 files changed, 71 insertions(+) create mode 100644 examples/03_view_map.py create mode 100644 examples/sample_map.npy diff --git a/examples/03_view_map.py b/examples/03_view_map.py new file mode 100644 index 0000000..7f91782 --- /dev/null +++ b/examples/03_view_map.py @@ -0,0 +1,71 @@ +# import the necessary packages +import os + +import numpy as np + +from pathfinding3d.core.diagonal_movement import DiagonalMovement +from pathfinding3d.core.grid import Grid, GridNode +from pathfinding3d.finder.a_star import AStarFinder + +USE_OPEN3D = True +try: + # for visualization + # only py 3.8, 3.9 and 3.10 + import open3d as o3d +except ImportError: + USE_OPEN3D = False + print("Open3D is not installed. Please install it using 'pip install open3d'") + +# load map +sample_map_path = os.path.join(os.path.dirname(__file__), "sample_map.npy") +matrix = np.load(sample_map_path) + +# define start and end points +start_pt = [21, 21, 21] +end_pt = [5, 38, 33] + +# create grid representation and start and end nodes +grid = Grid(matrix=matrix) +start = grid.node(*start_pt) +end = grid.node(*end_pt) + +# initialize A* finder +finder = AStarFinder(diagonal_movement=DiagonalMovement.only_when_no_obstacle) +path_, runs = finder.find_path(start, end, grid) +path_cost = end.g +print(f"path cost: {path_cost:.4f}, path length: {len(path_)}, runs: {runs}") + +path = [] +for node in path_: + if isinstance(node, GridNode): + path.append([node.x, node.y, node.z]) + elif isinstance(node, tuple): + path.append([node[0], node[1], node[2]]) +print(f"path: {path}") + + +# visualize path in open3d +if USE_OPEN3D: + # Find the obstacles and represent in blue + obstacle_indices = np.where(matrix == 0) + xyz_pt = np.stack(obstacle_indices, axis=-1).astype(float) + colors = np.zeros((xyz_pt.shape[0], 3)) + colors[:, 2] = obstacle_indices[2] / np.max(obstacle_indices[2]) + + # Prepare start and end colors + start_color = np.array([[1.0, 0, 0]]) # Red + end_color = np.array([[0, 1.0, 0]]) # Green + path_colors = np.full((len(path) - 2, 3), [0.7, 0.7, 0.7]) # Grey for the path + + # Combine points and colors + xyz_pt = np.concatenate((xyz_pt, [start_pt], [end_pt], path[1:-1])) + colors = np.concatenate((colors, start_color, end_color, path_colors)) + + # Create and visualize the point cloud + pcd = o3d.geometry.PointCloud() + pcd.points = o3d.utility.Vector3dVector(xyz_pt) + pcd.colors = o3d.utility.Vector3dVector(colors) + + voxel_grid = o3d.geometry.VoxelGrid.create_from_point_cloud(pcd, voxel_size=1.0) + axes = o3d.geometry.TriangleMesh.create_coordinate_frame(size=15.0, origin=np.array([-3.0, -3.0, -3.0])) + o3d.visualization.draw_geometries([axes, voxel_grid], window_name="Voxel Env", width=1024, height=768) diff --git a/examples/sample_map.npy b/examples/sample_map.npy new file mode 100644 index 0000000000000000000000000000000000000000..735f1fd24248265499726f9bf3bc99efef551458 GIT binary patch literal 296480 zcmeI3!LGI2aaC`+JcWA(DJ_&1qD>D(hXyTV5hIWgK@OsWcnV$^Um@$%*=vrkYOPf@ z|KFF-Qbg@hwdNdWpX1m#$^ZE`|L$M^+kgKzzx>Z%{@3sS{*V9oAOH0GfAPED|I7dK zcfbGJ-~IkS{llOB$DjW2-~Pit{ORxi_$&WcfB3Ke_{Tr!1DY@Ba7i4*u~^e)+-w_?tiXSN`iqXjZ&FZ(N?Ay}O@Z&+p&B?)Ue3|2^&b z)8|{~=V!C;Tc_7&*LRcWPoHm{pP$XXZ=GJBUEfWfKYhM+ettIlzIA$ic6~Q_{`C3Q z`T5!G`_}38+4bGz`P1iH=jUg$?^~zWXV-U==TDz+ou8l0zHgmgpIzTgo>^UKvTXup7D0dhjL^0&P;hz{AYHH zKlAI^`_H@g`+UF8>~A3c^UpnJZgKB+i+$0~f1eRkwH--@E46U!7fU)blfY zR^8$q{hX6L_E+agAIh$}#XHKHqDMAQ^j9-8x45rnPTWP0Y@X<^W@c`2U(KAjiyqlL(O=EX+~U5P zIdK;~vU#Gvnwh!9eKm99E_!71M1M6ibBp_G=EPm}$mWUuYG&pZ_tngayXdhqPq;i~ z?zqL{7CU!8aQs+kjaujh~Z*UgFFXEpQvy}!Tr>~Em@w={8=xyAiNe>F3DWOFrh;x2QG z`-%Q)X7tGBYUadU<`(x8{ngCqkbb8 zC+;%0xS!~+W=4-}u4Yc$Wp1>eaCyokkBmQMPV6!t+E2JVWs-;DPdU>&zRE+d;9nM-21+=>-YQ2zM;MS#P?avoVeTh=_mTD znXl*jKR?R;2C9Ec6L*GPk&&=&xo*k8G}HPTXZ~aX-;t&5Rz| zT+N)g%iQ9AqQ9CMJ+ir)IdPY{#r;HoH8Xl-b2W3~E^~|fiT-M4^vLFF=EPm*7WWhV z)y(LT&DG3_yUdOD6E07g7DW+Htq71Pfm(I&qga zcC$P0bGxsL&feXuFN=KI&F;L!P!FH|xtHpLVl5?{mAai_YHNtS^gv+Rg5~&+Wc0I(v7szASz_)9!cBdB5{5cJ}UO z?Rdu9-JaX~*u68~_U^v_ncd>g@_N4id%xM=K>T}g&zW1?yWL`6^vLG;&OK*tH~Y?? z<;J7qZS`mA=(@>xJ#W6f_3it8uHVTGdB4w_Z*P73Gkl*_hsxdh6EdmfP2I57x|wpQ zt@QKlDpb4spL)F zu+_Sma;W64HcTpcQ#Wk2Zl)Y6xvLG6O5W5BTdkWZhf3~h!=#cob;DNAn<*d4eYW8( z9SO>XQ%co)c%bV~b>&6GnWceP?vv!&d8N z%Au0G+Ayi)P2I57x|wpQ?vv!&d8N%Au0G+Ayi)P2I57 zx|wpQnsTGpXcF-LTcVnR2M)t~N|6c~duR zwQi;yD!Ho-lSt@QKlDpb4spL)Fu+_Sma;W64HcTpc zQ#Wk2Zl)Y6xvLG6O5W5BTdkWZhf3~h!=#cob;DNcX3C+GyV@|R?vv!&c9mDIdyxw&5-1O>s6^H&YIk z+|`CjC2#76t=7$yLnU{$VN%JPx?!tzGv!dpU2T|D@}_RsYTZmZRB~6FK2qtg-t<|m zNmYl+-J0GQK2`)B8xJzk1VWwI)>^DtBvoAF1?LZ~Cm(q^d*ZZcXna zmHz5YpVgXFb*S8}=}anlQ#Wk2Zl)Y6xvLG6O5W5BTdkWZhf3~h!=#cob;DNcX3C+G zyV@|Rt@QKlDpb4spL)Fu+_Sma;W64HcTpcQ#Wk2Zl)Y6 zxvLG6O5W5BTdkWZhf3~h!=#cob;DNcX3C+GyV@|R-ozspWngG_xYo4e|0|n$LG&G{rUA=|Hf{<^Y{CFzt5e&ft$azlear=@%E(a=1uJ0 znQuBz-tM@?+mo)FH?ezXzUe%9yWrt{?Oj$6Dv>AHCnyLaZB&Xc!4 zvs?Umc0B1e=1tAiy~C&6V(!o86W!wcdj7b7{~3P|?)m+_zrXkFZ@~RG8T&G~xR3AL zbM(k&w_EJX+~PjIbI;LZXMR2Pe?R+~zr*-D+wuHSx4-)SHT+}OGyWdj^Un9_c8h&G zp3d9o`0M<${^Q2;^R4sq&iCo``t15GdhYy8owvV!x2OO3{Jl(nem(WS?>YV6-|zGN zK6m~GPXEqM-|e`?+ta?&cd>hCKJ7bwx8oLXPy0^a#qOQ?wD0uYj$6Dv?K^!JyLaZ( zzSDO*Zt?cC@AO^l-kDGPPT&2^Zt-V&J%8N44~f4A_x%3e-`{)oH{kx8jD49~+{btB zIeKKX+b#BGZgC&qx##GS&2G2Ym$}7#eCM8{M>f0NVqfM)`v(#3;kT$w_gC4|OOXVwB1p%Mi~xHPKrBxs6+V>qYMWpC&e8;)S-NcQHFz)lj05^>QFw!D8s?Y zNpXh{btoTVl;Pmyq`1R}I+PDF%5ZRUQrzJ~9mpt84gZPiaUI$L-`P+3p?ru@hJ%xn;tn6`P(H*c!@ zmg*%&84gZPiaUI$L-`P+3p?ru@hJ%xn;tn6`P(H*c!@8JcaB@=I;X@tDhZtozI5{cq@SzUnLyR&UoSYPQ_)v%PAx0SvPELwDe5gbD5TgtS zCnv=nKGdOnh*5@vlat~OAL>v(#3;kT$w_gC4|OP?T}IiReAh|ccluD4>Lo@Q4o*&r zJA9}^`4FQF2PY@R9X`~de27togOiiu4j<}JKEx=)!O2N+hYxiqA7Yf@;N+yZ!-qPQ z4>8JcaB@=I;X@tDhZtozI5{cq@SzUnLyR&UoSYPQ_)v%PAx0SvPELwDe5gbD5TgtS zCnv=nKGdOnh*5@vlat~OAL>v(#3;kT$w_gC4|OOXVwB1p%Mi~xHPKrBxs6+V>qYMWpC&e8;)S-Nc zQHFz)lj05^>QFw!D8s?YNpXh{btoTVl;Pmyq`1R}I+PDF%5ZRUQrzJ~9mpt84gZPiaUI$L-`P+3p?ru@hJ%xn;tn6` zP(H*c!@8JcaB@=I;X@tDhZtozI5{cq@SzUnLyR&UoSYPQ_)v%P zAx0SvPELwDe5gbD5TgtSCnv=nKGdOnh*5@vlat~OAL>v(#3;kT$w_gC4|OOXVwB1p%Mi~xHPKrBx zs6+V>qYMWpC&e8;)S-NcQHFz)lj05^>QFw!D8s?YNpXh{btoTVl;Pmyq`1R}I+PDF z%5ZRUQrzJ~9mpt84gZPiaUI$L-`P+3p?ru@hJ%xn;tn6`P(H*c!@8JcaB@=I;X@tDhZtozI5{cq z@SzUnLyR&UoSYPQ_)v%PAx0SvPELwDe5gbD5TgtSCnv=nKGdOnh*5@vlat~OAL>v( z#3;kT$w_gC4|OOXVwB1p%Mi~xHPKrBxs6+V>qYMWpC&e8;)S-NcQHFz)lj05^>QFw!D8s?YNpXh{ zbtoTVl;Pmyq`1R}I+PDF%5ZRUQrzJ~9mpt z84gZPiaUI$L-`P+3p?ru@hJ%xn;tn6`P(H*c!@8Jc zaB@=I;X@tDhZtozI5{cq@SzUnLyR&UoSYPQ_)v%PAx0SvPELwDe5gbD5TgtSCnv=n zKGdOnh*5@vlat~OAL>v(#3;kT$w_gC4|OOXVwB1p%Mi~xHPKrBxs6+V>qYMWpC&e8;)S-NcQHFz) zlj05^>QFw!D8s?YNpXh{btoTVl;Pmyq`1R}I+PDF%5ZRUQrzJ~9mpt84gZPiaUI$L-`P+3p?ru@hJ%xn;tn6`P(H*c z!@8JcaB@=I;X@tDhZtozI5{cq@SzUnLyR&UoSYPQ_)v%PAx0Sv zPELwDe5gbD5TgtSCnv=nKGdOnh*5@vlat~OAL>v(#3;kT$w_gC4|OOXVwB1p%Mi~xHPKrBxs6+V> zqYMWpC&e8;)S-NcQHFz)lj05^>QFw!D8s?YNpXh{btoTVl;Pmyq`1R}I+PDF%5ZRU zQrzJ~9mpt84gZPiaUI$L-`P+3 zp?ru@hJ%xn;tn6`P(H*c!@8JcaB@=I;X@tDhZtozI5{cq@SzUn zLyR&UoSYPQ_)v%PAx0SvPELwDe5gbD5TgtSCnv=nKGdOnh*5@vlat~OAL>v(#3;kT z$w_gC4|OOXVwB1p%Mi~xHPKrBxs6+V>qYMWpC&e8;)S-NcQHFz)lj05^>QFw!D8s?YNpXh{btoTV zl;Pmyq`1R}I+PDF%5ZRUQrzJ~9mpt84gZP ziaUI$L-`P+3p?ru@hJ%xn;tn6`P(H*c!@8JcaB@=I z;X@tDhZtozI5{cq@SzUnLyR&UoSYPQ_)v%PAx0SvPELwDe5gbD5TgtSCnv=nKGdOn zh*5@vlat~OAL>v(#3;kT$w_gC4|OOXVwB1p%Mi~xHPKrBxs6+V>qYMWpC&e8;)S-NcQHFz)lj05^ z>QFw!D8s?YNpXh{btoTVl;Pmyq`1R}I+PDF%5ZRUQrzJ~9mv#6~-JE=Wzt8vk%>D*W{;s;-WNvZa)p?Q=J+k>Ezw1rr7WZA9 zCppn0n@{q)-ehiZ-_?1N6Fsu|B){uT<`(x|ohLcbBb!h1yWV7Oao^Q>k`q0$`6R#V zP39K&U7aU6(IcBr^1I$-ZgJn$d6E-7viT&x>rLhs_g$SQIng7VPx8CoWNvZa)p?Q= zJ+k>Ezw1rr7WZA9Cppn0n@{q)-ehiZ-_?1N6Fsu|B){uT<`(x|ohLcbBb!h1yWV7O zao^Q>k`q0$`6R#VP39K&U7aU6(IcBr^1I$-ZgJn$d6E-7viT&x>rLhs_g$SQIng7V zPx8CoWNvZa)p?Q=Jx=G7-rYB+Zt>l2=gEES%jT2(t~Z%m+;?@JEyx0zeq-*(-+j~>~4({uYabBp`i zuABGKBb#q}Zr^5Zaev!&^FDfH^G(m~+srNQZ@X^ZM~`g2>A8KIxyAi$*UkIru`}Ov zp1j?0i?=6TH*aEhHsAEzzRld?{9@%`;bNe=Pi~HNI zoA=Qpn{Rq<-)3%cf7^BQK6+&HP0#Jy%q{M3yKde`k8Hl_xqX|t(f+n~_dR+1R`~As zaeIIIz1{ZizJEP`+<#sdzx&b8-_P#*)9-M%=l1@$=z05{+`K>iKHqou z-@wi9?e^`C{cYFH``7cw{pA8IyJx=Fahsyg?xA?Bob9)o}cIMmO-S<0g@piZ8_C9v+ z%(uO}?|0nd?QYNQeeB+uZ+my&@3_U=-JaX~*u68~_U^vlaf`RRJ-7Fceurl%!l?d!!eVG;tn6`P(H*c z!@H&Z^8s~m_?hJ%xn;tn6`P(H*c!@|2`exVeE*%znwW|IOZLDW8QI#w+^@15xaNhnBkazX1DnBzn)+Jnis#9)A{{A-|sW~ z8#w*j>UfvA#eGNLX-4$O=F^UjcbQw5?Y>n$#Jan8^2kZ?o!stQa6yM41zEwWNy1RGs$Vu^?-0oZDL#(@dCy$&I-^uO1RX)VJyLa-)N%5WB?px(U zth;+BkDL_W$?d*XKE%4ack;+d@txf6Tjf(>-5yz9#Yx>{x!rr^K&-oaCy$(dH2?e= zezXtu-*|q$b$+IwM{%duXP5FJ_W9QNnVb}NdVO{&A7Y-qJ6BND%t)A{{A z|Nr?u9lz_?vGX?>-?``4^Xs1j9pC-?eZJpk_BYV+cXpbQxyAiyN5{M9kPOJ^JDkv{FyoN`Db>EKkskdGyZ(>=boBl-|0N%7IRPC;=7nPHOIcw zdCD#3p1Q?%F>h*)eLM4%59R*MZqfPmeE;W1+226?dvVX1Tim2Ql3mG4X4#0L4e@DZ{~)4>5-ib&!+dl;PmYhnT~MI>9VoH86-`4DsXPzO0FP8klae26)GsDqporwj*IKExb8)Im-ioH|c< zhYw|(_|O~b=-||O!aIB@?Y<- z&6z`GKjjv4D;_&HF}ri#aj4vOy2Y$YcXku=rsm9{vY&E`xfKs}6EhsMav9V zoH86-`4DsXPzO0FP8klae26)GsDqporwj*IKExb8)Im;)Q-*^pA7Tz4>L4e@DZ{~) z4>5-ib&!+dl;PmYhnT~MI>-ppU`#gR3yT8Med+OfdC%wCGc6{h|xAWwlx=;CuL*>n>5522&p4?F14nOJL zeY4|3x4WGu_tc&7ldhXLnNQr`^xVFU9@%``b@M)Ri~F0N+qcmpn{T^r-e+!cf75gO zHhN_9ZP(5F%q{M3dT!rFk8Hl}x_O_u#r;js?c3;)&9_}Q?=!c!zv;Ps8$Gi5w(I77 z<`(xiJ-2V8M>gMf-Mr7-;{K-R_HFdY=G(5D_nBMV-}KzRjUL&2+ja9kbEExD=gC|0 z$oNUu&6~_8?r(Z--$svYzU{hspSi{TP0#Jy=#kC0T{rJDx46IQxqTZwviY{_=6&WC z_cuMaZ==WL{Cf6&yYKh;{C(bjKfCXrZFsxeb9>Jw&zsxc-S^Knyxr}&y=Rm9=GLL| zzHY;pm7d!hZm(}{9V+kZHhfv>xxL}``sUW5^1g2K{j!qXdGo#f>{F)?Wu2Mz{_j+f zTjZ~2?{~iQea7E|dwxCN-+T5q;QpJ8eVJR_$9L{IdStWPE%s$@aUb8g=jf5mZnxN% zxy5~a=bocSHoM(oU*;C~@tu2)9@*@6i+!0}+{btBIeKKX+b#BGZnTdXj+s0%?r@78 znNQruckVfQWV72X_GNByAK$s>=#kBCx7e4t#eIC|o}))LyWL`6<`(zyoqLWRlk@G* zHR%?!U(fh$+|NJi`@HSleSi8*?e^T>zn(wtKL?}l{`PlTc~9LtywY=fv*QzQZ+my& z$L^hZx99eL$1UF8_U^uq-8=Jc&+Yw=8{OVIRNhl}#w*#KH<=IZI~^)nPGnxn{ue+l9S?`qda7@whuwa+{}&V4;;R4v literal 0 HcmV?d00001