navigation_pathfinder

Description

This program determines the shortest path between two positions, avoiding obstacles. He needs to be efficient to not wait too long for its response.

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
• v0.0.1 (P18) @Mindstan
  • The program is conected with objects_classifier and map
  • The safety margin is now a dynamic parameter

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.
PathfinderNodeConfig	DynParam	Use it to enable and specify an output image in order to debug, and to edit the safety margin.

Clients and subscribtions

Server name	Type	Function
/recognition/objects_classifier/objects	Topic	Publishes rectangles, circles and segments representing obstacles seen by all captors.
/memory/map/get_objects	Service	Publishes static or known obstacles.

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)

Often, doing only 90° angles isn't very optimal and adapted, and some waypoints aren't needed. We proceed then at a path smoothing in order to remove useless points and to decrease the real distance to travel. In our case, we consider that for a given point, if by testing each point starting from the end of the path it is posible to draw a line between the 2 points without meeting any obstacles. We stop the analyse at first positive answer, we remove all the points between this two ones and we do the same thing for the next point. The main advantage of this algorithm is the fact that it does all necessary eliminations starting from the first point, in opposite at other smoothing algorithms However there are floor problems hard to avoid when we draw the lines. This is why we need security margins bigger than the size of the robot.