mirror of
https://github.com/zhm-real/PathPlanning.git
synced 2026-08-30 00:50:46 +08:00
87 lines
5.4 KiB
Plaintext
87 lines
5.4 KiB
Plaintext
|
U
|
|||
|
|
´_… ã @ s| d Z ddlZddlZddlZddlZej ej ej e ¡¡d ¡ ddl
|
|||
|
|
m Z m Z G dd„ dƒZ
|
|||
|
|
dd„ Zed krxeƒ dS )
|
|||
|
|
z!
|
|||
|
|
A_star 2D
|
|||
|
|
@author: huiming zhou
|
|||
|
|
é Nz /../../Search_based_Planning/)ÚplottingÚenvc @ s` e Zd ZdZdd„ Zdd„ Zdd„ Zdd „ Zd
|
|||
|
|
d „ Zd d
|
|||
|
|
„ Z dd„ Z
|
|||
|
|
dd„ Z dd„ Z dd„ Z
|
|||
|
|
dS )ÚAStarz4AStar set the cost + heuristics as the priority
|
|||
|
|
c C sP || _ || _|| _t ¡ | _| jj| _| jj| _g | _g | _ t
|
|||
|
|
ƒ | _ t
|
|||
|
|
ƒ | _ d S )N)
|
|||
|
|
Ús_startÚs_goalÚheuristic_typer ÚEnvÚmotionsÚu_setÚobsÚOPENÚCLOSEDÚdictÚPARENTÚg)Úselfr r r © r úm/home/zhou/GitHub/PathPlanning/Search_based_Planning/Search_2D/../../Search_based_Planning/Search_2D/Astar.pyÚ__init__ s
|
|||
|
|
|
|||
|
|
|
|||
|
|
zAStar.__init__c C sü | j | j| j < d| j| j < tj| j| j< t | j| | j ¡| j f¡ | jrêt
|
|||
|
|
| j¡\}}| j |¡ || jkrrqê|
|
|||
|
|
|¡D ]j}| j| | ||¡ }|| jkr¬tj| j|< || j| k r||| j|< || j|< t | j| |¡|f¡ q|qD| | j¡| j fS )zH
|
|||
|
|
A_star Searching.
|
|||
|
|
:return: path, visited order
|
|||
|
|
r )r r r ÚmathÚinfr ÚheapqÚheappushr Úf_valueÚheappopr
|
|||
|
|
ÚappendÚ get_neighborÚcostÚ extract_path)r Ú_ÚsÚs_nÚnew_costr r r Ú searching# s( ÿ
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
zAStar.searchingc C sN g g }}|dkrF| | j| j|¡\}}| |¡ | |¡ |d8 }q
|
|||
|
|
||fS )ze
|
|||
|
|
repeated A*.
|
|||
|
|
:param e: weight of A*
|
|||
|
|
:return: path and visited order
|
|||
|
|
é g à?)Úrepeated_searchingr r r )r ÚeÚpathÚvisitedÚp_kZv_kr r r Úsearching_repeated_astarC s
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
z AStar.searching_repeated_astarc C sì |d|t dƒi}||i}g }g }t ||| || |¡ |f¡ |rÞt |¡\}} | | ¡ | |krhqÞ| | ¡D ]h}
|
|||
|
|
|| | | |
|
|||
|
|
¡ } |
|
|||
|
|
|krœtj ||
|
|||
|
|
< | ||
|
|||
|
|
k rr| ||
|
|||
|
|
< | ||
|
|||
|
|
< t |||
|
|||
|
|
|| |
|
|||
|
|
¡ |
|
|||
|
|
f¡ qrqB|
|
|||
|
|
|¡|fS )z¸
|
|||
|
|
run A* with weight e.
|
|||
|
|
:param s_start: starting state
|
|||
|
|
:param s_goal: goal state
|
|||
|
|
:param e: weight of a*
|
|||
|
|
:return: path and visited order.
|
|||
|
|
r r ) Úfloatr r Ú heuristicr r r r r r r ) r r r r& r r r r
|
|||
|
|
r r r! r" r r r r% T s* ÿ
|
|||
|
|
|
|||
|
|
&zAStar.repeated_searchingc s ‡ fdd„| j D ƒS )zu
|
|||
|
|
find neighbors of state s that not in obstacles.
|
|||
|
|
:param s: state
|
|||
|
|
:return: neighbors
|
|||
|
|
c s, g | ]$}ˆ d |d ˆ d |d f‘qS )r r$ r )Ú.0Úu©r r r Ú
|
|||
|
|
<listcomp> s z&AStar.get_neighbor.<locals>.<listcomp>)r
|
|||
|
|
©r r r r/ r r x s zAStar.get_neighborc C s6 | ||¡rtjS t |d |d |d |d ¡S )zÔ
|
|||
|
|
Calculate Cost for this motion
|
|||
|
|
:param s_start: starting node
|
|||
|
|
:param s_goal: end node
|
|||
|
|
:return: Cost for this motion
|
|||
|
|
:note: Cost function could be more complicate!
|
|||
|
|
r r$ )Ú is_collisionr r Úhypot)r r r r r r r � s z
|
|||
|
|
AStar.costc C s || j ks|| j krdS |d |d k�r
|
|||
|
|
|d |d k�r
|
|||
|
|
|d |d |d |d kr¦t|d |d ƒt|d |d ƒf}t|d |d ƒt|d |d ƒf}nHt|d |d ƒt|d |d ƒf}t|d |d ƒt|d |d ƒf}|| j k�s|| j k�r
|
|||
|
|
dS dS )zÇ
|
|||
|
|
check if the line segment (s_start, s_end) is collision.
|
|||
|
|
:param s_start: start node
|
|||
|
|
:param s_end: end node
|
|||
|
|
:return: True: is collision / False: not collision
|
|||
|
|
Tr r$ F)r ÚminÚmax)r r Zs_endÚs1Ús2r r r r2 � s $ $&$$zAStar.is_collisionc C s | j | | |¡ S )zu
|
|||
|
|
f = g + h. (g: Cost to come, h: heuristic value)
|
|||
|
|
:param s: current state
|
|||
|
|
:return: f
|
|||
|
|
)r r, r1 r r r r § s z
|
|||
|
|
AStar.f_valuec C s6 | j g}| j }|| }| |¡ || jkrq.qt|ƒS )z^
|
|||
|
|
Extract the path based on the PARENT set.
|
|||
|
|
:return: The planning path
|
|||
|
|
)r r r Úlist)r r r' r r r r r ° s
|