navigation_pathfinder
C'est un programme qui détermine le plus court chemin entre deux postions, en évitant les obstacles. Il doit être efficace pour ne pas attendre trop longtemps sa réponse.
-
v0.0.1 (A17) @Mindstan
- Package created with a cleaner code than P15/P17
This program loads the static obstacles (in our case the table shape) from an image with the path ros_ws/src/navigation_pathfinder/def/map.bmp. We're using the C++ SFML library in order to load and read it. In option, by using dynamic_reconfigure (gui or command line) you can enable the generation of a debug image every time the program finds a path.
| Server name | Type | Function |
|---|---|---|
/navigation/navigator/find_path |
navigation_pathfinder::FindPath |
Tries to find a path between given start and end positions. If it succeeds, it returns the smooth path. Else success value is false. |
Le "Pathfinder"
L'algorithme
Le pathfinder ou chercheur de chemin utilise un algorithme (simplifié) de plus court chemin à origine unique, connu aussi sous les noms de parcours en largeur, BFS (Breadth First Search), ainsi que A* et Djikstra si on considère que la pondération (le coût de déplacement) est constante et positive.
Dans notre cas, le graphe est un tableau de 2 dimensions où une case contient l'information si elle peut être traversée. Les transitions ou arcs sont les cases voisines dans une direction (par exemple N,S,E,W) et comme nous n'avons pas de pondération on considère que celle-ci vaut 1.
Dans un autre tableau de même taille on indique si une case a déjà été visitée et sa distance par rapport au point de départ; celui-ci est donc initialisé avec aucune case visitée.
L'algorithme en lui-même est composé de 2 partie : on cherche un chemin vers notre cible en notant pour chaque case la distance déjà parcourue, puis si la position cible est atteinte, on par de la cible et on revient au point de départ en choisissant comme arcs ceux qui mennent à une position ayant une plus faible distance. On obtient alors le plus court chemin entre la position de départ et la position cible.
1) Le marquage des cases
Dans une approche simplifiée on considère qu'on a 2 listes de positions, une des positions précédemments vues et une de celles qui viennes d'être visitées. Une étape consiste, pour chaque positions précédentes, à marquer commes vues à l'aide de sa distance toutes ses voisines valides et non-vues, et à les ajouter dans la liste des positions suivantes. Si on tombe sur notre position cible alors on quitte et on passe à la deuxième partie.
A chaque fin d'étape, on vide copie la liste des nouvelles positions dans celle des précédentes (préalablement vidée) et on vide celle des suivantes. On augmente de 1 la distance déjà parcourrue. Si on se retrouve avec une liste des positions précédentes vide alors on a exploré tout le graphe et on n'a pas trouvé la position cible. Dans ce cas on quitte.
2) La récupération du chemin