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

6.1 A* 57
47 Node otherWay = GetNode ( openSet , n e i g h b o r ) ;
i f ( otherWay == n u l l )
49 / / No node was found , so i t ’ s new , add i t t o open s e t
openSet . Add ( n e i g h b o r ) ;
51 e l s e i f ( n e i g h b o r .G < otherWay .G)
{
53 / / This n e i g h b o r has b e t t e r s c o r e
/ / o v e r w r i t e otherWay with n e i g h b o r
55 openSet . Remove ( otherWay ) ;
openSet . Add ( n e i g h b o r ) ;
57 }
}
59 }
r e t u r n n u l l ; / / No way found
61 }
63 p u b l i c L i s t R e c o n s t r u c t P a t h ( Node node )
{
65 / / The p a r e n t p a r a m e t e r of t h e node i s s e t w h i l e c r e a t i n g
/ / t h e n e i g h b o r s ( f i r s t time c u r r e n t . Neighbors i s c a l l e d )
67
L i s t r e s = new L i s t ( ) ;
69 / / Go from t h e end t o t h e p a r e n t of each node
/ / u n t i l f i n d t h e r o o t , t h a t has no p a r e n t
71 w h i l e ( node . P a r e n t != n u l l )
{
73 r e s . I n s e r t ( 0 , node ) ;
node = node . P a r e n t ;
75 }
r e t u r n r e s ;
77 }
Algorithmus (6.1): A*-Algorithmus im C# Stil.
Die Methoden float Score(Node from, Node to) und float Heuristic(Node
from, Node to), können nach belieben angepasst werden und haben teilweise große Auswirkungen
auf die Arbeitsweise des Algorithmus und sollen in den nächsten Kapiteln besser erklärt
werden. Im Normalfall wird als Score die Länge der gesamten Bahn vom Startpunkt geliefert und als
Heuristik der euklidische Abstand.