mirror of
https://github.com/zhm-real/PathPlanning.git
synced 2026-08-30 00:50:46 +08:00
41 lines
1.9 KiB
Plaintext
41 lines
1.9 KiB
Plaintext
|
B
|
|||
|
|
�¢ë^ ã @ s4 d Z ddlmZ ddlZdd„ Zdd„ Zdd„ ZdS ) z
|
|||
|
|
@author: huiming zhou
|
|||
|
|
é Nc C sJ | d |d | d |d g}|t jksB||d |d dkrFdS dS )zÔ
|
|||
|
|
Detect if the next state is in obstacles using this input.
|
|||
|
|
|
|||
|
|
:param x: current state
|
|||
|
|
:param u: input
|
|||
|
|
:param obs_map: map of obstacles
|
|||
|
|
:return: in obstacles: True / not in obstacles: False
|
|||
|
|
r é TF)Ú environmentÚmotions)ÚxÚuÚobs_mapÚx_next© r ú<C:\Users\Huiming Zhou\Desktop\Search-based Planning\tools.pyÚ
|
|||
|
|
obs_detect s
|
|||
|
|
|
|||
|
|
|
|||
|
|
r c C s\ |g}|| g}|}x.|| }| |¡ | || ¡ || krP qW tt|ƒƒtt|ƒƒfS )a
|
|||
|
|
Extract the path based on the relationship of nodes.
|
|||
|
|
|
|||
|
|
:param xI: Starting node
|
|||
|
|
:param xG: Goal node
|
|||
|
|
:param parent: Relationship between nodes
|
|||
|
|
:param actions: Action needed for transfer between two nodes
|
|||
|
|
:return: The planning path
|
|||
|
|
)ÚappendÚlistÚreversed)ÚxIÚxGÚparentÚactionsZ path_backZ acts_backÚ x_currentr r r
|
|||
|
|
Ú extract_path s
|
|||
|
|
|
|||
|
|
r c C sä t ¡ }t ¡ \}}x6tt|ƒƒD ]&}dddg||| d || d < q"W x6tt|ƒƒD ]&}dddg||| d || d < qZW dddg|| d | d < dddg||d |d < | |¡ | ¡ tj|dd� t ¡ dS )zÉ
|
|||
|
|
Plot the path.
|
|||
|
|
|
|||
|
|
:param xI: Starting node
|
|||
|
|
:param xG: Goal node
|
|||
|
|
:param path: Planning path
|
|||
|
|
:param visited: Visited nodes
|
|||
|
|
:param name: Name of this figure
|
|||
|
|
:return: A plot
|
|||
|
|
g à?r r g ð?g N)Úfontdict)
|
|||
|
|
r Ú obstaclesÚpltÚsubplotsÚrangeÚlenÚimshowÚ invert_yaxisÚtitleÚshow) r r ÚpathÚvisitedÚnameÚ
|
|||
|
|
backgroundÚfigÚaxÚkr r r
|
|||
|
|
ÚshowPath2 s &&
|
|||
|
|
r& )Ú__doc__Úmatplotlib.pyplotÚpyplotr r r r r& r r r r
|
|||
|
|
Ú<module> s
|
|||
|
|
|