Revision history [back]

Navfn uses Dijkstra's algorithm to search through the available working area and find the best path. What this means is that navfn uses breadth-first search to check the costmap cells for the optimal path. It begins at the robot's origin and radiates outward, checking each non-obstacle cell in turn until the goal is reached. You can find a primer on Dijkstra's algorithm here.

Navfn's optimal path is based on a path's "potential". The potential is the relative cost of a path based on the distance from the goal and from the existing path itself. It must be noted that Navfn update's each cell's potential in the potential map, or potarr as it's called in navfn, as it checks that cell. This way, it can step back through the potential array to find the best possible path. The potential is determined by the cost of traversing a cell (traversability factor, hf) and the distance away that the next cell is from the previous cell.

The section of code that you posted occurs during the update of a given cell during traversal. The quadratic approximation is used to overcome the limitation that navfn's potential array is four-connected, i.e. it only checks it's top, right, left, and bottom neighbors and not the diagonals. For example, in the section of code that you posted, tc is defined as the cell with the lowest of either the left cell or the right cell, and ta is the lowest of the top cell or the bottom cell. It then calculates the relative cost between these two cells. Then, the algorithm checks to see if the relative cost of traversing through a cell is less than the cost of going around it. If the cost of going around a cell is less than going through it, the algorithm uses the quadratic estimation to determine an approximate cost.