navigation_pathfinder

Description

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.

Authors and versions

• v0.0.0 (A17) @Mindstan
  • Package created with a cleaner code than P15/P17
  • The program can only load barriers from an image.
• v0.0.1 (A17/P18) @Mindstan
  • The program is connected with belt_interpreter and lidar_objects

Configuration

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 and temporary edit the safety margin applied on dynamic objects.

Communication

Servers

Server name	Type	Function
/navigation/pathfinder/find_path	Service	Tries to find a path between given start and end positions. If it succeeds, it returns the smooth path. Else success value is false.

Clients and subscribtions

Server name	Type	Function
/processing/belt_interpreter/rects_firtered	Topic	Publishes rectangles representing obstacles seen by the belt.
/processing/lidar_objects/obstacles	Topic	Publishes circles representing obstacles seen by the lidar.

How it works

The "Pathfinder"

The algorithm

The pathfinder uses an (simplified) algorithm of shortest path with a unique origin, also known as BFS (Breadth First Search), as well as A* and Djikstra if we consider that the weight (move cost) is a positive constant.

In our case, the graph is a 2D array, where a case contain information whether it can be crossed. Transitions or arcs are the neighbors cases in a direction (for example N,S,E,W) and because we haven't any specific weight, its value is considered as 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

On sait qu’il existe un chemin entre la position de départ et la position d’arrivée et on a une grille représentant toutes les distances au point de départ de chaque case ; pour trouver un des chemin les plus cours il suffit de partir du point d’arrivée et de revenir au point de départ en passant par les cases ayant une distance strictement inférieure à la précédente. Comme il peut y avoir plusieurs cases ayant la même distance, dans ce cas le choix de la case suivante est totalement arbitraire. Une fois la position de départ atteinte on inverse le chemin et on obtient le chemin « brut » à suivre, qu’on peut éventuellement lisser.

3) Lissage du chemin (optionnel)

Souvent, faire uniquement des angles à 90 degrés n’est pas vraiment optimal et adapté, et certains points de passages ne sont pas requis. On procède alors à un lissage du chemin afin de supprimer les points inutiles et de diminuer la distance réelle à parcourir. Dans notre cas on considère que pour un point donné si en testant chaque point en partant de la fin du chemin s’il est possible de tracer une ligne entre ces deux points sans qu’il y ait un obstacle. On arrête le balayage au premier test positif, on supprime tous les points entre ces deux-là et on passe au point suivant. Le principal avantage de cet algorithme est qu’il fait toutes les éliminations nécéssaires dès la première itération, contrairement à d’autres algorithme de lissage. Cependant il y a des problèmes d’arrondis difficilement évitables lorsqu’il trace les lignes, d’où la nécéssité d’avoir des marges de sécurité plus larges que le robot.