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 contains 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.

In another array with the same size, we indicate whether a case has been already visited and its distance compared to the starting position; so this one is initialized with not visited cases.

The algorithm in it-self is composed with 2 parts : first, we search a way to our target, marking for each cases crossed the distance already stepped, then if we found the target position, we go from the target to the starting point by choosing the arc that go to a position, which has one of the shortest distance. Finaly we obtain the shortest path between the starting position and the targeted position.

1) Case marking

In a simplified approach we consider that we have 2 position lists, one containing last treated postions and the other one the positions visited in that step. A step consists, for each previous position, to mark as visited by the distance all its valid and not visited neighbors, and to add them to the new position list. If we find our target position then we quit and execute the second part.

At each end step, we clear the previous position list and copy inside it the new ones; then the new position list is also cleared. We increase by 1 the distance already traveled. If we the previous position list is empty, it means we explored the entire connected graph and we didn't find the target position. In that case, we quit and answer that no valid paths exist.

2) The path recovery

We know there is at least one path between the stating position and the target position, and we have a grid representing all distances to the starting position for each cases.; to find one of the shortest path you travel from the target position to the starting position by crossing the cases having their distance strictly lower than that of the previous one. As it can be a few cases with the same distance, the choice of the next case is totally arbitrary. When we arrive on the starting position, we invert the path and we obtain a "raw" way to follow, which can be smoothed.

3) Path smoothing (optional)

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.