Bahnplanungsframework für ein autonomes Fahrzeug - oops ...

56 Suchalgorithmen
1 p u b l i c L i s t AStar ( Node s t a r t , Node g o a l )
{
3 / / C r e a t e two empty s e t s
L i s t c l o s e d S e t = new L i s t ( ) ;
5 L i s t openSet = new L i s t ( ) ;
7 / / Add s t a r t t o open s e t
s t a r t .G = 0 ;
9 s t a r t .H = H e u r i s t i c ( s t a r t , g o a l ) ;
s t a r t . F = s t a r t .G + s t a r t .H;
11 openSet . Add ( s t a r t ) ;
13 w h i l e ( ! openSet . IsEmpty )
{
15 / / Find t h e b e s t s c o r e node
Node c u r r e n t = FindLowest ( openSet ) ;
17 i f ( D i s t a n c e ( c u r r e n t , g o a l ) found a way
19 r e t u r n R e c o n s t r u c t P a t h ( c u r r e n t ) ;
21 openSet . Remove ( c u r r e n t ) ; / / Remove c u r r e n t from open s e t
c l o s e d S e t . Add ( c u r r e n t ) ; / / And add i t t o t h e c l o s e d s e t
23
/ / C r e a t e n e i g h b o r s f o r c u r r e n t and c a l c u l a t e s c o r e s
25 f o r e a c h ( Node n e i g h b o r i n c u r r e n t . Neighbors )
{
27 i f ( GetNode ( c l o s e d S e t , n e i g h b o r ) != n u l l )
/ / Neighbor i s a l r e a d y i n c l o s e d s e t , c o n t i n u e with n e x t
29 c o n t i n u e ;
31 i f ( c h e c k C o l l i s i o n ( n e i g h b o r ) )
{
33 / / Neighbor c o l l i d e s with t h e environment , add i t
/ / d i r e c t l y t o c l o s e d s e t and c o n t i n u e with n e x t
35 c l o s e d S e t . Add ( n e i g h b o r ) ;
c o n t i n u e ;
37 }
39 / / C a l c u l a t e t h e s c o r e s
n e i g h b o r .G = Score ( c u r r e n t , n e i g h b o r ) ;
41 n e i g h b o r .H = H e u r i s t i c ( neighbor , end ) ;
n e i g h b o r . F = n e i g h b o r .G + n e i g h b o r .H;
43
/ / Try t o f i n d o t h e r node , t h a t ’ s n e a r enough t o t h i s node
45 / / and has a l m o s t t h e same o r i e n t a t i o n , so i t can be
/ / h a n d l e d l i k e t h i s n e i g h b o r node